Large Language Models (LLMs) excel at generating contextually relevant text but lack logical reasoning abilities. They rely on statistical patterns rather than logical inference, making them unreliable for structured decision-making. Integrating LLMs with task planning can address this limitation by combining their natural language understanding with the precise, goal-oriented reasoning of planners. This paper introduces ViPlan, a hybrid system that leverages Vision Language Models (VLMs) to extract high-level semantic information from visual and textual inputs while integrating classical planners for logical reasoning. ViPlan utilizes VLMs to generate syntactically correct and semantically meaningful PDDL problem files from images and natural language instructions, which are then processed by a task planner to generate an executable plan. The entire process is embedded within a behavior tree framework, enhancing efficiency, reactivity, replanning, modularity, and flexibility. The generation and planning capabilities of ViPlan are empirically evaluated with simulated and real-world experiments.

Simultaneous Localization and Mapping (SLAM) allows mobile robots to navigate without external positioning systems or pre-existing maps. Radar is emerging as a valuable sensing tool, especially in vision-obstructed environments, as it is less affected by particles than lidars or cameras. Modern 4D imaging radars provide three-dimensional geometric information and relative velocity measurements, but they bring challenges, such as a small field of view and sparse, noisy point clouds. Detecting loop closures in SLAM is critical for reducing trajectory drift and maintaining map accuracy. However, the directional nature of 4D radar data makes identifying loop closures, especially from reverse viewpoints, difficult due to limited scan overlap. This article explores using 4D radar for loop closure in SLAM, focusing on similar and opposing viewpoints. We generate submaps for a denser environment representation and use introspective measures to reject false detections in feature-degenerate environments. Our experiments show accurate loop closure detection in geometrically diverse settings for both similar and opposing viewpoints, improving trajectory estimation with up to 82% improvement in ATE and rejecting false positives in self-similar environments.

Object detection is crucial in diverse autonomous systems like surveillance, autonomous driving, and driver assistance, ensuring safety by recognizing pedestrians, vehicles, traffic lights, and signs. However, adverse weather conditions such as snow, fog, and rain pose a challenge, affecting detection accuracy and risking accidents and damage. This clearly demonstrates the need for robust object detection solutions that work in all weather conditions. We employed three strategies to enhance deep learning-based object detection in adverse weather: training on real-world all-weather images, training on images with synthetic augmented weather noise, and integrating object detection with adverse weather image denoising. The synthetic weather noise is generated using analytical methods, GAN networks, and style-transfer networks. We compared the performance of these strategies by training object detection models using real-world all-weather images from the BDD100K dataset and for assessment employed unseen real-world adverse weather images. Adverse weather denoising methods were evaluated by denoising real-world adverse weather images and the results of object detection on denoised and original noisy images were compared. We found that the model trained using all-weather real-world images performed best, while the strategy of doing object detection on denoised images performed worst.

This article studies group-wise point set registration and makes the following contributions: "FuzzyGReg", which is a new fuzzy cluster-based method to register multiple point sets jointly, and "FuzzyQA", which is the associated quality assessment to check registration accuracy automatically. Given a group of point sets, FuzzyGReg creates a model of fuzzy clusters and equally treats all the point sets as the elements of the fuzzy clusters. Then, the group-wise registration is turned into a fuzzy clustering problem. To resolve this problem, FuzzyGReg applies a fuzzy clustering algorithm to identify the parameters of the fuzzy clusters while jointly transforming all the point sets to achieve an alignment. Next, based on the identified fuzzy clusters, FuzzyQA calculates the spatial properties of the transformed point sets and then checks the alignment accuracy by comparing the similarity degrees of the spatial properties of the point sets. When a local misalignment is detected, a local re-alignment is performed to improve accuracy. The proposed method is cost-efficient and convenient to be implemented. In addition, it provides reliable quality assessments in the absence of ground truth and user intervention. In the experiments, different point sets are used to test the proposed method and make comparisons with state-of-the-art registration techniques. The experimental results demonstrate the effectiveness of our method. The code is available at https://gitsvn-nt.oru.se/qianfang.liao/FuzzyGRegWithQA.

This article presents an accurate, highly efficient, and learning-free method for large-scale odometry estimation using spinning radar, empirically found to generalize well across very diverse environments—outdoors, from urban to woodland, and indoors in warehouses and mines—without changing parameters. Our method integrates motion compensation within a sweep with one-to-many scan registration that minimizes distances between nearby oriented surface points and mitigates outliers with a robust loss function. Extending our previous approach conservative filtering for efficient and accurate radar odometry (CFEAR), we present an in-depth investigation on a wider range of datasets, quantifying the importance of filtering, resolution, registration cost and loss functions, keyframe history, and motion compensation. We present a new solving strategy and configuration that overcomes previous issues with sparsity and bias, and improves our state-of-the-art by 38%, thus, surprisingly, outperforming radar simultaneous localization and mapping (SLAM) and approaching lidar SLAM. The most accurate configuration achieves 1.09% error at 5 Hz on the Oxford benchmark, and the fastest achieves 1.79% error at 160 Hz.

This article studies the fuzzy cluster-based point set registration (FuzzyPSReg). First, we propose a new metric based on Gustafson–Kessel (GK) fuzzy clustering to measure the alignment of two point clouds. Unlike the metric based on fuzzy c-means (FCM) clustering in our previous work, the GK-based metric includes orientation properties of the point clouds, thereby providing more information for registration. We then develop the registration quality assessment of the GK-based metric, which is more sensitive to small misalignments than that of the FCM-based metric. Next, by effectively combining the two metrics, we design two FuzzyPSReg strategies with global optimization. 1)FuzzyPSReg-SS, which extends our previous work and aligns two similar-sized point clouds with greatly improved efficiency. 2)FuzzyPSReg-O2S, which aligns two point clouds with a relatively large difference in size and can be used to estimate the pose of an object in a scene. In the experiment, we use different point clouds to test and compare the proposed method with state-of-the-art registration approaches. The results demonstrate the advantages and effectiveness of our method.

This paper presents an accurate, highly efficient and learning free method for large-scale radar odometry estimation. By using a simple filtering technique that keeps the strongest returns, we produce a clean radar data representation and reconstruct surface normals for efficient and accurate scan matching. Registration is carried out by minimizing a point-to-line metric and robustness to outliers is achieved using a Huber loss. Drift is additionally reduced by jointly registering the latest scan to a history of keyframes. We found that our odometry pipeline generalize well to different sensor models and datasets without changing a single parameter. We evaluate our method in three widely different environments and demonstrate an improvement over spatially cross validated state-of-the-art with an overall translation error of 1.76% in a public urban radar odometry benchmark, running merely on a single laptop CPU thread at 55 Hz.

This study presents a new point set registration method to align 3D range scans. In our method, fuzzy clusters are utilized to represent a scan, and the registration of two given scans is realized by minimizing a fuzzy weighted sum of the distances between their fuzzy cluster centers. This fuzzy cluster-based metric has a broad basin of convergence and is robust to noise. Moreover, this metric provides analytic gradients, allowing standard gradient-based algorithms to be applied for optimization. Based on this metric, the outlier issues are addressed. In addition, for the first time in rigid point set registration, a registration quality assessment in the absence of ground truth is provided. Furthermore, given specified rotation and translation spaces, we derive the upper and lower bounds of the fuzzy cluster-based metric and develop a branch-and-bound (BnB)-based optimization scheme, which can globally minimize the metric regardless of the initialization. This optimization scheme is performed in an efficient coarse-to-fine fashion: First, fuzzy clustering is applied to describe each of the two given scans by a small number of fuzzy clusters. Then, a global search, which integrates BnB and gradient-based algorithms, is implemented to achieve a coarse alignment for the two scans. During the global search, the registration quality assessment offers a beneficial stop criterion to detect whether a good result is obtained. Afterwards, a relatively large number of points of the two scans are directly taken as the fuzzy cluster centers, and then, the coarse solution is refined to be an exact alignment using the gradient-based local convergence. Compared to existing counterparts, this optimization scheme makes a large improvement in terms of robustness and efficiency by virtue of the fuzzy cluster-based metric and the registration quality assessment. In the experiments, the registration results of several 3D range scan pairs demonstrate the accuracy and effectiveness of the proposed method, as well as its superiority to state-of-the-art registration approaches.

This paper proposes a novel approach for global localisation of mobile robots in large-scale environments. Our method leverages learning-based localisation and filtering-based localisation, to localise the robot efficiently and precisely through seeding Monte Carlo Localisation (MCL) with a deeplearned distribution. In particular, a fast localisation system rapidly estimates the 6-DOF pose through a deep-probabilistic model (Gaussian Process Regression with a deep kernel), then a precise recursive estimator refines the estimated robot pose according to the geometric alignment. More importantly, the Gaussian method (i.e. deep probabilistic localisation) and nonGaussian method (i.e. MCL) can be integrated naturally via importance sampling. Consequently, the two systems can be integrated seamlessly and mutually benefit from each other. To verify the proposed framework, we provide a case study in large-scale localisation with a 3D lidar sensor. Our experiments on the Michigan NCLT long-term dataset show that the proposed method is able to localise the robot in 1.94 s on average (median of 0.8 s) with precision 0.75 m in a largescale environment of approximately 0.5 km2.

This paper presents LOGOS, a method of spatial verification for visual localization that is robust in the presence of a high proportion of outliers. LOGOS uses scale and orientation information from local neighbourhoods of features to determine which points are likely to be inliers. The inlier points can be used for secondary localization verification and pose estimation. LOGOS is demonstrated on a number of benchmark localization datasets and outperforms RANSAC as a method of outlier removal and localization verification in scenarios that require robustness to many outliers.

Place categorisation; i.e., learning to group perception data into categories based on appearance; typically uses supervised learning and either visual or 2D range data. This paper shows place categorisation from 3D data without any training phase. We show that, by leveraging the NDT histogram descriptor to compactly encode 3D point cloud appearance, in combination with standard clustering techniques, it is possible to classify public indoor data sets with accuracy comparable to, and sometimes better than, previous supervised training methods. We also demonstrate the effectiveness of this approach to outdoor data, with an added benefit of being able to hierarchically categorise places into sub-categories based on a user-selected threshold. This technique relieves users of providing relevant training data, and only requires them to adjust the sensitivity to the number of place categories, and provide a semantic label to each category after the process is completed.

Local scan registration approaches commonly only utilize ego-motion estimates (e.g. odometry) as an initial pose guess in an iterative alignment procedure. This paper describes a new method to incorporate ego-motion estimates, including uncertainty, into the objective function of a registration algorithm. The proposed approach is particularly suited for feature-poor and self-similar environments, which typically present challenges to current state of the art registration algorithms. Experimental evaluation shows significant improvements in accuracy when using data acquired by Automatic Guided Vehicles (AGVs) in industrial production and warehouse environments.

The highly percipient nature of human mind in avoiding sensory overload is a crucial factor which gives human vision an advantage over machine vision, the latter has otherwise powerful computational resources at its disposal given today's technology. This stresses the need to focus on methods which extract a concise representation of the environment inorder to approach a complex problem such as visual mapping. This article is an attempt of creating a mapping system, which proposes an architecture that combines task-specific and saliency driven approaches. The proposed method is implemented on a warehouse robot. The proposed solution provide a priority framework which enables an industrial robot to build a concise visual representation of the environment. The method is evaluated on data collected by a RGBD sensor mounted on a fork-lift robot and shows promise for addressing visual mapping problems in industrial environments.

Autonomous navigation in real-world industrial environments is a challenging task in many respects. One of the key open challenges is fast planning and execution of trajectories to reach arbitrary target positions and orientations with high accuracy and precision, while taking into account non-holonomic vehicle constraints. In recent years, lattice-based motion planners have been successfully used to generate kinematically and kinodynamically feasible motions for non-holonomic vehicles. However, the discretized nature of these algorithms induces discontinuities in both state and control space of the obtained trajectories, resulting in a mismatch between the achieved and the target end pose of the vehicle. As endpose accuracy is critical for the successful loading and unloading of cargo in typical industrial applications, automatically planned paths have not be widely adopted in commercial AGV systems. The main contribution of this paper addresses this shortcoming by introducing a path smoothing approach, which builds on the output of a lattice-based motion planner to generate smooth drivable trajectories for non-holonomic industrial vehicles. In real world tests presented in this paper we demonstrate that the proposed approach is fast enough for online use (it computes trajectories faster than they can be driven) and highly accurate. In 100 repetitions we achieve mean end-point pose errors below 0.01 meters in translation and 0.002 radians in orientation. Even the maximum errors are very small: only 0.02 meters in translation and 0.008 radians in orientation.

We address the problem of human detection from heavy mobile machinery and robotic equipment operating at industrial working sites. Exploiting the fact that workers are typically obliged to wear high-visibility clothing with reflective markers, we propose a new recognition algorithm that specifically incorporates the highly discriminative features of the safety garments in the detection process. Termed Multi-band Hough Forest, our detector fuses the input from active near-infrared (NIR) and RGB color vision to learn a human appearance model that not only allows us to detect and localize industrial workers, but also to estimate their body orientation. We further propose an efficient pipeline for automated generation of training data with high-quality body part annotations that are used in training to increase detector performance. We report a thorough experimental evaluation on challenging image sequences from a real-world production environment, where persons appear in a variety of upright and non-upright body positions.

Industrial environments are rarely static and often their configuration is continuously changing due to the material transfer flow. This is a major challenge for infrastructure free localization systems. In this paper we address this challenge by introducing a localization approach that uses a dual-timescale approach. The proposed approach - Dual-Timescale Normal Distributions Transform Monte Carlo Localization (DT-NDT-MCL) - is a particle filter based localization method, which simultaneously keeps track of the pose using an apriori known static map and a short-term map. The short-term map is continuously updated and uses Normal Distributions Transform Occupancy maps to maintain the current state of the environment. A key novelty of this approach is that it does not have to select an entire timescale map but rather use the best timescale locally. The approach has real-time performance and is evaluated using three datasets with increasing levels of dynamics. We compare our approach against previously proposed NDT-MCL and commonly used SLAM algorithms and show that DT-NDT-MCL outperforms competing algorithms with regards to accuracy in all three test cases.

We report on a novel vision-based method for reliable human detection from vehicles operating in industrial environments in the vicinity of workers. By exploiting the fact that reflective vests represent a standard safety equipment on most industrial worksites, we use a single camera system and active IR illumination to detect humans by identifying the reflective vest markers. Adopting a sparse feature based approach, we classify vest markers against other reflective material and perform supervised learning of the object distance based on local image descriptors. The integration of the resulting per-feature 3D position estimates in a particle filter finally allows to perform human tracking in conditions ranging from broad daylight to complete darkness.

Autonomous vehicles operating in real-world industrial environments have to overcome numerous challenges, chief among which is the creation and maintenance of consistent 3D world models. This paper proposes to address the challenges of online real-world mapping by building upon previous work on compact spatial representation and formulating a novel 3D mapping approach - the Normal Distributions Transform Occupancy Map (NDT-OM). The presented algorithm enables accurate real-time 3D mapping in large-scale dynamic environments employing a recursive update strategy. In addition, the proposed approach can seamlessly provide maps at multiple resolutions allowing for fast utilization in high-level functions such as localization or path planning. Compared to previous approaches that use the NDT representation, the proposed NDT-OM formulates an exact and efficient recursive update formulation and models the full occupancy of the map.

We propose and evaluate a system for detecting and tracking multiple humans wearing high-visibility clothing from vehicles operating in industrial work environments. We use a customized stereo camera setup equipped with IR flash and IR filter to detect the reflective material on the worker's garments and estimate their trajectories in 3D space. An evaluation in two distinct industrial environments with different degrees of complexity demonstrates the approach to be robust and accurate for tracking workers in arbitrary body poses, under occlusion, and under a wide range of different illumination settings.

Industrial applications often impose hard requirements on the precision of autonomous vehicle systems. As a consequence industrial Automatically Guided Vehicle (AGV) systems still use high-cost infrastructure based positioning solutions. In this paper we propose a map based localization method that fulfills the requirements on precision and repeatability, typical for industrial application scenarios. The proposed method - Normal Distributions Transform Monte Carlo Localization (NDT-MCL) is based on a well established probabilistic framework. In a novel contribution, we formulate the MCL localization approach using the Normal Distributions Transform (NDT) as an underlying representation for both map and sensor data. By relaxing the hard discretization assumption imposed by grid-map models and utilizing the piece-wise continuous NDT representation the proposed algorithm achieves substantially improved accuracy and repeatability. The proposed NDT-MCL algorithm is evaluated using offline data sets from both a laboratory and a real-world industrial environments. Additionally, we report a comparison of the proposed algorithm to grid-based MCL and to a commercial localization system when used in a closed-loop with the control system of an AGV platform. In all tests the proposed algorithm is demonstrated to provide performance superior to that of standard grid-based MCL and comparable to the performance of the commercial infrastructure based positioning system.

Autonomous vehicles operating in real-world industrial environments have to overcome numerous challenges, chief among which is the creation and maintenance of consistent 3D world models. This paper focuses on a particularly important challenge: mapping in dynamic environments. We introduce several improvements to the recently proposed Normal Distributions Transform Occupancy Map (NDT-OM) aimed for efficient mapping in dynamic environments. A careful consistency analysis is given based on convergence and similarity metrics specifically designed for evaluation of NDT maps in dynamic environments. We show that in the context of mapping with known poses the proposed method results in improved consistency and in superior runtime performance, when compared against 3D occupancy grids at the same size and resolution. Additionally, we demonstrate that NDT-OM features real-time performance in a highly dynamic 3D mapping and tracking scenario with centimeter accuracy over a 1.5km trajectory.

Autonomous vehicles operating in real-world industrial environments have to overcome numerous challenges, chief among which are the creation of consistent 3D world models and the simultaneous tracking of the vehicle pose with respect to the created maps. In this paper we integrate two recently proposed algorithms in an online, near-realtime mapping and tracking system. Using the Normal Distributions Transform (NDT), a sparse Gaussian Mixture Model, for representation of 3D range scan data, we propose a frame-to-model registration and data fusion algorithm - NDT Fusion. The proposed approach uses a submap indexing system to achieve operation in arbitrarily-sized environments. The approach is evaluated on a publicly available city-block sized data set, achieving accuracy and runtime performance significantly better than current state of the art. In addition, the system is evaluated on a data set covering ten hours of operation and a trajectory of 7.2km in a real-world industrial environment, achieving centimeter accuracy at update rates of 5-10 Hz.

In this paper we propose a new grid based approach to model a dynamic environment. Each grid cell is assumed to be an independent Markov chain (iMac) with two states. The state transition parameters are learned online and modeled as two Poisson processes. As a result, our representation not only encodes the expected occupancy of the cell, but also models the expected dynamics within the cell. The paper also presents a strategy based on recency weighting to learn the model parameters from observations that is able to deal with non-stationary cell dynamics. Moreover, an interpretation of the model parameters with discussion about the convergence rates of the cells is presented. The proposed model is experimentally validated using offline data recorded with a Laser Guided Vehicle (LGV) system running in production use.

Planning feasible paths in fully three-dimensional environments is a challenging problem. Application of existing algorithms typically requires the use of limited 3D representations that discard potentially useful information. This article proposes a novel approach to path planning that utilizes a full 3D representation directly: the Three-Dimensional Normal Distributions Transform (3D-NDT). The well known wavefront planner is modified to use 3D-NDT as a basis for map representation and evaluated using both indoor and outdoor data sets. The use of 3D-NDT for path planning is thus demonstrated to be a viable choice with good expressive capabilities.

This paper presents an overview of an autonomous robotic system for material handling. The system is being developed by extending the functionalities of traditional AGVs to be able to operate reliably and safely in highly dynamic environments. Traditionally, the reliable functioning of AGVs relies on the availability of adequate infrastructure to support navigation. In the target environments of our system, such infrastructure is difficult to setup in an efficient way. Additionally, the location of objects to handle are unknown, which requires runtime object detection and tracking. Another requirement to be fulfilled by the system is the ability to generate trajectories dynamically, which is uncommon in industrial AGV systems.

We propose a new approach to appearance based loop detection from metric 3D maps, exploiting the NDT surface representation. Locations are described with feature histograms based on surface orientation and smoothness, and loop closure can be detected by matching feature histograms. We also present a quantitative performance evaluation using two real-world data sets, showing that the proposed method works well in different environments.

This paper presents a vision-based approach to simultaneous localization and mapping (SLAM) in indoor/outdoor environments with minimalistic sensing and computational requirements. The approach is based on a graph representation of robot poses, using a relaxation algorithm to obtain a globally consistent map. Each link corresponds to a relative measurement of the spatial relation between the two nodes it connects. The links describe the likelihood distribution of the relative pose as a Gaussian distribution. To estimate the covariance matrix for links obtained from an omnidirectional vision sensor, a novel method is introduced based on the relative similarity of neighboring images. This new method does not require the determination of distances to image features using multiple-view geometry, for example. Combined indoor and outdoor experiments demonstrate that the approach can handle different environments (without modification of the parameters), and it can cope with violations of the ldquoflat floor assumptionrdquo to some degree and scales well with increasing size of the environment, producing topologically correct and geometrically accurate maps at low computational cost. Further experiments demonstrate that the approach is also suitable for combining multiple overlapping maps, e.g., for solving the multirobot SLAM problem with unknown initial poses.

This paper presents a vision-based approach to SLAM in large-scale environments with minimal sensing and computational requirements. The approach is based on a graphical representation of robot poses and links between the poses. Links between the robot poses are established based on odomety and image similarity, then a relaxation algorithm is used to generate a globally consistent map. To estimate the covariance matrix for links obtained from the vision sensor, a novel method is introduced based on the relative similarity of neighbouring images, without requiring distances to image features or multiple view geometry. Indoor and outdoor experiments demonstrate that the approach scales well to large-scale environments, producing topologically correct and geometrically accurate maps at minimal computational cost. Mini-SLAM was found to produce consistent maps in an unstructured, large-scale environment (the total path length was 1.4 km) containing indoor and outdoor passages.

This paper presents a system for autonomous change detection with a security patrol robot. In an initial step a reference model of the environment is created and changes are then detected with respect to the reference model as differences in coloured 3D point clouds, which are obtained from a 3D laser range scanner and a CCD camera. The suggested approach introduces several novel aspects, including a registration method that utilizes local visual features to determine point correspondences (thus essentially working without an initial pose estimate) and the 3D-NDT representation with adaptive cell size to efficiently represent both the spatial and colour aspects of the reference model. Apart from a detailed description of the individual parts of the difference detection system, a qualitative experimental evaluation in an indoor lab environment is presented, which demonstrates that the suggested system is able register and detect changes in spatial 3D data and also to detect changes that occur in colour space and are not observable using range values only.

In this paper we present a vision-based approach to self-localization that uses a novel scheme to integrate feature-based matching of panoramic images with Monte Carlo localization. A specially modified version of Lowe’s SIFT algorithm is used to match features extracted from local interest points in the image, rather than using global features calculated from the whole image. Experiments conducted in a large, populated indoor environment (up to 5 persons visible) over a period of several months demonstrate the robustness of the approach, including kidnapping and occlusion of up to 90% of the robot’s field of view.

For a mobile robot it is a natural task to build a 3D model of its environment. Such a model is not only useful for planning robot actions but also to provide a remote human surveillant a realistic visualization of the robot’s state with respect to the environment. Acquiring 3D models of environments is also an important task on its own with many possible applications like creating virtual interactive walkthroughs or as basis for 3D-TV. In this paper we present our method to acquire a 3D model using a mobile robot that is equipped with a laser scanner and a panoramic camera. The method is based on calculating dense depth maps for panoramic images using pairs of panoramic images taken from different positions using stereo matching. Traditional 2D-SLAM using laser-scan-matching is used to determine the needed camera poses. To receive high-quality results we use a high-quality stereo matching algorithm – the graph cut method. We describe the necessary modifications to handle panoramic images and specialized post-processing methods.

The problem of extracting three-dimensional structures from data acquired with mobile robots has received considerable attention over the past years. Robots that are able to perceive their three-dimensional environment are envisioned to more robustly perform tasks like navigation, rescue, and manipulation. In this paper we present an approach that simultaneously uses color and range information to cluster 3D points into planar structures. Our current system also is able to calibrate the camera and the laser based on the remission values provided by the range scanner and the brightness of the pixels in the image. It has been implemented on a mobile robot equipped with a manipulator that carries a range scanner and a camera for acquiring colored range scans. Several experiments carried out on real data and in simulations demonstrate that our approach yields highly accurate results also in comparison with previous approaches.

We present a method to acquire a realistic, visually convincing 3D model of indoor office environments based on a mobile robot that is equipped with a laser range scanner and a panoramic camera. The data of the 2D laser scans are used to solve the SLAM problem and to extract walls. Textures for walls and floor are built from the images of a calibrated panoramic camera. Multiresolution blending is used to hide seams in the generated textures.