This paper presents a novel approach for realtime 3D navigation in large-scale complex environments by introducing a hierarchical 3D visibility graph (V-graph) and an efficient path search method. The proposed algorithm addresses the computational challenges of V-graph construction and shortest path search on the graph simultaneously. By introducing hierarchical 3D V-graph construction with heuristic visibility update, the 3D V-graph is constructed in$O\left(K \cdot n^{2} \log n\right)$time, which guarantees real-time performance. The proposed iterative divide-and-conquer path search method can achieve near-optimal path solutions within the constraints of realtime operations. The algorithm ensures efficient 3D V-graph construction and path search. Extensive simulated and realworld environments validated that our algorithm reduces the travel time by 42%, achieves up to 24.8% higher trajectory efficiency, and runs faster than most benchmarks by orders of magnitude in complex environments. The code and developed simulator have been open-sourced to facilitate future research.

This paper investigates Path planning Among Movable Obstacles (PAMO), which seeks a minimum cost collision-free path among static obstacles from start to goal while allowing the robot to push away movable obstacles (i.e., objects) along its path when needed. To develop planners that are complete and optimal for PAMO, the planner has to search a giant state space involving both the location of the robot as well as the locations of the objects, which grows exponentially with respect to the number of objects. This paper leverages a simple yet under-explored idea that, only a small fraction of this giant state space needs to be searched during planning as guided by a heuristic, and most of the objects far away from the robot are intact, which thus leads to runtime efficient algorithms. Based on this idea, this paper introduces two PAMO formulations, i.e., bi-objective and resource constrained problems in an occupancy grid, and develops PAMO*, a planning method with completeness and solution optimality guarantees, to solve the two problems. We then further extend PAMO* to hybrid-state PAMO* to plan in continuous spaces with high-fidelity interaction between the robot and the objects. Our results show that, PAMO* can often find optimal solutions within a second in cluttered maps with up to 400 objects.

With the recent rise of large language models, vision-language models, and other general foundation models, there is growing potential for multimodal, multi-task robotics that can operate in diverse environments given natural language input. One such application is indoor navigation using natural language instructions. However, despite recent progress, this problem remains challenging due to the 3D spatial reasoning and semantic understanding required. Additionally, the language used may be imperfect or misaligned with the scene, further complicating the task. To address this challenge, we curate a benchmark dataset, IRef-VLA, for Interactive Referential Vision and Language-guided Action in 3D Scenes with imperfect references. IRef-VLA is the largest real-world dataset for the referential grounding task, consisting of over 11.5 K scanned 3D rooms from existing datasets, 7.6 M heuristically generated semantic relations, and 4.7 M referential statements. Our dataset also contains semantic object and room annotations, scene graphs, navigable free space annotations, and is augmented with statements where the language has imperfections or ambiguities. We verify the generalizability of our dataset by evaluating with state-of-the-art models to obtain a performance baseline and also develop a graphsearch baseline to demonstrate the performance bound and generation of alternatives using scene-graph knowledge. With this benchmark, we aim to provide a resource for 3D scene understanding that aids the development of robust, interactive navigation systems. The dataset and all source code is publicly released11https://github.com/HaochenZ11/IRef-VLA.

Interpreting object-referential language and grounding objects in 3D with spatial relations and attributes is essential for robots operating alongside humans. However, this task is often challenging due to the diversity of scenes, large number of fine-grained objects, and complex free-form nature of language references. Furthermore, in the 3D domain, obtaining large amounts of natural language training data is difficult. Thus, it is important for methods to learn from little data and zero-shot generalize to new environments. To address these challenges, we propose SORT3D, an approach that utilizes rich object attributes from 2D data and merges a heuristics-based spatial reasoning toolbox with the ability of large language models (LLMs) to perform sequential reasoning. Importantly, our method does not require text-to-3D data for training and can be applied zero-shot to unseen environments. We show that SORT3D achieves state-of-the-art zero-shot performance on complex view-dependent grounding tasks on two benchmarks. We also implement the pipeline to run real-time on two autonomous vehicles and demonstrate that our approach can be used for object-goal navigation on previously unseen real-world environments. All source code for the system pipeline is publicly released.1

Simultaneous localization and mapping (SLAM) is a fundamental task for numerous applications such as autonomous navigation and exploration. Despite many SLAM datasets have been released, current SLAM solutions still struggle to have sustained and resilient performance. One major issue is the absence of high-quality datasets including diverse all-weather conditions and a reliable metric for assessing robustness. This limitation significantly restricts the scalability and generalizability of SLAM technologies, impacting their development, validation, and deployment. To address this problem, we present SubT-MRS, an ex-tremely challenging real-world dataset designed to push SLAM towards all-weather environments to pursue the most robust SLAM performance. It contains multi-degraded en-vironments including over 30 diverse scenes such as structureless corridors, varying lighting conditions, and perceptual obscurants like smoke and dust; multimodal sensors such as LiDAR, fisheye camera, IMU, and thermal camera; and multiple locomotions like aerial, legged, and wheeled robots. We developed accuracy and robustness evaluation tracks for SLAM and introduced novel robustness metrics. Comprehensive studies are performed, revealing new obser-vations, challenges, and opportunities for future research.

This paper introduces a real-time algorithm for navigating complex unknown environments cluttered with movable obstacles. Our algorithm achieves fast, adaptable routing by actively attempting to manipulate obstacles during path planning and adjusting the global plan from sensor feedback. The main contributions include an improved dynamic Directed Visibility Graph (DV-graph) for rapid global path searching, a real-time interaction planning method that adapts online from new sensory perceptions, and a comprehensive framework designed for interactive navigation in complex unknown or partially known environments. Our algorithm is capable of replanning the global path in several milliseconds. It can also attempt to move obstacles, update their affordances, and adapt strategies accordingly. Extensive experiments validate that our algorithm reduces the travel time by 33%, achieves up to 49% higher path efficiency, and runs faster than traditional methods by orders of magnitude in complex environments. It has been demonstrated to be the most efficient solution in terms of speed and efficiency for interactive navigation in environments of such complexity. We also open-source our code in the docker demo1to facilitate future research.

The Orienteering Problem (OP) seeks a path on a graph to maximize total rewards collected subject to a path length budget. Typically, a reward is achieved by visiting a vertex in the graph, and such a reward is constant for all time. This paper considers a variant of OP where the reward of each vertex is an arbitrary time-dependent function, and hence the name time-varying reward OP (TR-OP). To solve this problem, we develop a novel heuristic search algorithm called Reward Maximization A* (RMA*), which is guaranteed to find an optimal solution to TR-OP. We also develop a fast method to compute an admissible heuristic for RMA* that can effectively direct the search to save computational effort. Furthermore, we introduce a hyper-parameter in RMA* that trades off between solution quality and runtime efficiency for RMA*. We benchmark RMA* against a recent dynamic programming (DP) approach, which runs fast in practice, but has no guarantee of the solution optimality. In our tests, RMA* reduces the runtime by up to 70% compared to DP. By adjusting the hyper-parameter, RMA* is able to find solutions with up to 30% more rewards than those found by DP.

The camera is an attractive device for use in beyond visual line of sight drone operation since cameras are low in size, weight, power, and cost. However, state-of-the-art visual localization algorithms have trouble matching visual data that have significantly different appearances due to changes in illumination or viewpoint. This article presents iSimLoc, a learning-based global relocalization approach that is robust to appearance and viewpoint differences. The features learned by iSimLoc's place recognition network can be utilized to match query images to reference images of a different stylistic domain and viewpoint. In addition, our hierarchical global relocalization module searches in a coarse-to-fine manner, allowing iSimLoc to perform fast and accurate pose estimation. We evaluate our method on a dataset with appearance variations and a dataset that focuses on demonstrating large-scale matching over a long flight over complex terrain. iSimLoc achieves 88.7% and 83.8% successful retrieval rates on our two datasets, with 1.5 s inference time, compared to 45.8% and 39.7% using the next best method. These results demonstrate robust localization in a range of environments and conditions.

In the era of advancing autonomous driving and increasing reliance on geospatial information, high-precision mapping not only demands accuracy but also flexible construction. Current approaches mainly rely on expensive mapping devices, which are time consuming for city-scale map construction and vulnerable to erroneous data associations without accurate GPS assistance. In this article, we present AutoMerge, a novel framework for merging large-scale maps that surpasses these limitations, which: 1) provides robust place recognition performance despite differences in both translation and viewpoint; 2) is capable of identifying and discarding incorrect loop closures caused by perceptual aliasing; and 3) effectively associates and optimizes large-scale and numerous map segments in the real-world scenario. AutoMerge utilizes multiperspective fusion and adaptive loop closure detection for accurate data associations, and it uses incremental merging to assemble large maps from individual trajectory segments given in random order and with no initial estimations. Furthermore, AutoMerge performs pose graph optimization after assembling the segments to smooth the merged map globally. We demonstrate AutoMerge on both city-scale merging (120 km) and campus-scale repeated merging (4.5 km × 8). The experiments show that AutoMerge: 1) surpasses the second- and third-best methods by 0.9% and 6.5% recall in segment retrieval; 2) achieves comparable 3-D mapping accuracy for 120-km large-scale map assembly; and 3) and is robust to temporally spaced revisits. To our knowledge, AutoMerge is the first mapping approach to merge hundreds of kilometers of individual segments without using GPS.

Autonomous Exploration Development Environment is an open-source repository released to facilitate development of high-level planning algorithms and integration of com-plete autonomous navigation systems. The repository contains representative simulation environment models, fundamental navigation modules, e.g., local planner, terrain traversability analysis, waypoint following, and visualization tools. Together with two of our high-level planner releases - TARE planner for exploration and FAR planner for route planning, we detail usage of the three open-source repositories and share experiences in integration of autonomous navigation systems. We use DARPA Subterranean Challenge as a use case where the repositories together form the main navigation system of the CMU-OSU Team. In the end, we discuss a few potential use cases in extended applications.

Path planning in unknown environments remains a challenging problem, as the environment is gradually observed during the navigation, the underlying planner has to update the environment representation and replan, promptly and constantly, to account for the new observations. In this paper, we present a visibility graph-based planning framework capable of dealing with navigation tasks in both known and unknown environments. The planner employs a polygonal representation of the environment and constructs the representation by extracting edge points around obstacles to form enclosed polygons. With that, the method dynamically updates a global visibility graph using a two-layered data structure, expanding the visibility edges along with the navigation, and removing edges that become occluded by newly observed obstacles. When navigating in unknown environments, the method is attemptable in discovering a way to the goal by picking up the environment layout on the fly, updating the visibility graph, and fast replanning corresponding to the newly observed environment. We evaluate the method in simulated and real-world settings. The method shows the capability to attempt and navigate through unknown environments, reducing travel time by up to 12-47% from search-based methods: A*, D* Lite, and more than 24-35% from sampling-based methods: RRT*, BIT*, and SPARS.

Under shared autonomy, wheelchair users expect vehicles to provide safe and comfortable rides while following users' high-level navigation plans. To find such a path, vehicles negotiate with different terrains and assess their traversal difficulty. Most prior works model surroundings either through geometric representations or semantic classifications, which do not reflect perceived motion intensity and ride comfort in downstream navigation tasks. We propose to model ride comfort explicitly in traversability analysis using proprioceptive sensing. We develop a self-supervised learning framework to predict traversability costmap from first-person-view images by leveraging vehicle states as training signals. Our approach estimates how the vehicle would “feel” if traversing over based on terrain appearances. We then show our navigation system provides human-preferred ride comfort through robot experiments together with a human evaluation study. The project could be found at https://sites.google.com/view/rca-navigation.

This paper describes a novel framework for autonomous exploration in large and complex environments. We show that the framework is efficient as a result of its hierarchical structure, where at one level it maintains a sparse representation of the environment and at another level, a dense representation is used within a local planning horizon around the robot. The exploration path is computed at the two levels, coarsely at the global scale and finely around the robot. Such a framework produces detailed paths in the vicinity of the robot, while trades off data resolution far away from the robot for computational efficiency. In experiments, we evaluate our method with a real robot exploring large and complex indoor and outdoor environments. Results show that our method is twice as efficient in covering spaces while using less than one-fifth of processing in comparison to state-of-the-art methods.

We present a method for efficiently exploring highly convoluted environments. The method incorporates two planning stages - an exploration stage for extending the boundary of the map, and a relocation stage for explicitly transiting the robot to different sub-areas in the environment. The exploration stage develops a local Rapidly-exploring Random Tree (RRT) in the free space of the environment, and the relocation stage maintains a global graph through the mapped environment, both are dynamically expanded over replanning steps. The method is compared to existing state-of-the-art methods in various challenging simulation and real environments. Experiment comparisons show that our method is twice as efficient in exploring spaces using less processing than the existing methods. Further, we release a benchmark environment to evaluate exploration algorithms as well as facilitate development of autonomous navigation systems. The benchmark environment and our method are open-sourced.

State-of-the-art coverage planning methods perform well in simple environments but take an ineffectively long time to converge to an optimal solution in complex three-dimensional (3D) environments. As more structures are present in the same volume of workspace, these methods slow down as they spend more time searching for all of the nooks and crannies concealed in three-dimensional spaces. This work presents a method for coverage planning that employs a multi-resolution hierarchical framework to solve the problem at two different levels, producing much higher efficiency than the state-of-the-art. First, a high-level algorithm separates the environment into multiple subspaces at different resolutions and computes an order of the subspaces for traversal. Second, a low-level sampling-based algorithm solves for paths within the subspaces for detailed coverage. In experiments, we evaluate our method using real-world datasets from complex three-dimensional scenes. Our method finds paths that are constantly shorter and converges at least ten times faster than the state-of-the-art. Further, we show results of a physical experiment where a lightweight UAV follows the paths to realize the coverage.

Human beings and animals are capable of recognizing places from a previous journey when viewing them under different environmental conditions (e.g., illuminations and weathers). This paper seeks to provide robots with a human-like place recognition ability using a new point cloud feature learning method. This is a challenging problem due to the difficulty of extracting invariant local descriptors from the same place under various orientation differences and dynamic obstacles. In this paper, we propose a novel lightweight 3D place recognition method, SeqSphereVLAD, which is capable of recognizing places from a previous trajectory regardless of the viewpoint and the temporary observation differences. The major contributions of our method lie in two modules: (1) the spherical convolution feature extraction module, which produces orientation-invariant local place descriptors, and (2) the coarse-to-fine sequence matching module, which ensures both accurate loop-closure detection and real-time performance. Despite the apparent simplicity, our proposed approach outperform the state-of-the-arts for place recognition under datasets that combine orientation and context differences. Compared with the arts, our method can achieve above 95% average recall for the best match with only 18% inference time of PointNet-based place recognition methods.

Light-weight camera localization in existing maps is essential for vision-based navigation. Currently, visual and visual-inertial odometry (VO&VIO) techniques are well-developed for state estimation but with inevitable accumulated drifts and pose jumps upon loop closure. To overcome these problems, we propose an efficient monocular camera localization method in prior LiDAR maps using direct 2D-3D line correspondences. To handle the appearance differences and modality gaps between LiDAR point clouds and images, geometric 3D lines are extracted offline from LiDAR maps while robust 2D lines are extracted online from video sequences. With the pose prediction from VIO, we can efficiently obtain coarse 2D-3D line correspondences. Then the camera poses and 2D-3D correspondences are iteratively optimized by minimizing the projection error of correspondences and rejecting outliers. Experimental results on the EurocMav dataset and our collected dataset demonstrate that the proposed method can efficiently estimate camera poses without accumulated drifts or pose jumps in structured environments.

We propose a planning method to enable fast autonomous flight in cluttered environments. Typically, autonomous navigation through a complex environment requires a continuous search on a graph generated by a k-connected grid or a probabilistic scheme. As the vehicle travels, updating the graph with data from onboard sensors is expensive as is the search on the graph especially if the paths must be kinodynamically feasible. We propose to avoid the online search to reduce the computational complexity. Our method models the environment differently in two separate regions. Obstacles are considered to be deterministically known within the sensor range and probabilistically known beyond the sensor range. Instead of searching for the path with the lowest cost (typically the shortest path), the method maximizes the likelihood to reach the goal in determining the immediate next step for navigation. With such a problem formulation, the online method realized by a trajectory library can determine a path within 0.2-0.3ms using a single CPU thread on a modem embedded computer. In experiments, it enables a lightweight UAV to fly at 10m/s in a cluttered forest environment (see Fig. 1 as an example).

We propose a novel method to enable fast autonomous flight in cluttered environments. Typically, autonomous navigation through a complex environment requires a continuous heuristic search on a graph generated by a k-connected grid or a probabilistic scheme. As the vehicle progresses, modification of the graph with data from onboard sensors is expensive as is search on the graph, especially if the paths must be kino-dynamically feasible. We suggest that computation needed to find safe paths during fast flight can be greatly reduced if we precompute and carefully arrange a dense set of alternative paths before the flight. Any prior map information can be used to prune the alternative paths to come up with a data structure that enables very fast online computation to deal with obstacles that are not on the map but only detected by onboard sensors. To test this idea, we have conducted a large number of flight experiments in structured (large industrial facilities) and unstructured (forests-like) environments. We show that even in the most unstructured environments, this method enables flight at a speed up to 10m/s while avoiding obstacles detected from onboard sensors.

We present a data processing pipeline to online estimate ego-motion and build a map of the traversed environment, leveraging data from a 3D laser, a camera, and an IMU. Different from traditional methods that use a Kalman filter or factor-graph optimization, the proposed method employs a sequential, multi-layer processing pipeline, solving for motion from coarse to fine. The resulting system enables high-frequency, low-latency ego-motion estimation, along with dense, accurate 3D map registration. Further, the system is capable of handling sensor degradation by automatic reconfiguration bypassing failure modules. Therefore, it can operate in the presence of highly dynamic motion as well as in dark, texture-less, and structure-less environments. During experiments, the system demonstrates 0.22% of relative position drift over 9.3km of navigation and robustness w.r.t aggressive motion such as highway speed driving (up to 33m/s).

Positioning and mapping can be conducted accurately by state-of-the-art state estimation methods. However, reliability of these methods is largely based on avoiding degeneracy that can arise from cases such as scarcity of texture features for vision sensors and lack of geometrical structures for range sensors. Since the problems are inevitably solved in uncontrived environments where sensors cannot function with their highest quality, it is important for the estimation methods to be robust to degeneracy. This paper proposes an online method to mitigate for degeneracy in optimization-based problems, through analysis of geometric structure of the problem constraints. The method determines and separates degenerate directions in the state space, and only partially solves the problem in well-conditioned directions. We demonstrate utility of this method with data from a camera and lidar sensor pack to estimate 6-DOF ego-motion. Experimental results show that the system is able to improve estimation in environmentally degenerate cases, resulting in enhanced robustness for online positioning and mapping.

Here, we present a general framework for combining visual odometry and lidar odometry in a fundamental and first principle method. The method shows improvements in performance over the state of the art, particularly in robustness to aggressive motion and temporary lack of visual features. The proposed on-line method starts with visual odometry to estimate the ego-motion and to register point clouds from a scanning lidar at a high frequency but low fidelity. Then, scan matching based lidar odometry refines the motion estimation and point cloud registration simultaneously.We show results with datasets collected in our own experiments as well as using the KITTI odometry benchmark. Our proposed method is ranked #1 on the benchmark in terms of average translation and rotation errors, with a 0.75% of relative position drift. In addition to comparison of the motion estimation accuracy, we evaluate robustness of the method when the sensor suite moves at a high speed and is subject to significant ambient lighting changes.

Visual odometry can be augmented by depth information such as provided by RGB-D cameras, or from lidars associated with cameras. However, such depth information can be limited by the sensors, leaving large areas in the visual images where depth is unavailable. Here, we propose a method to utilize the depth, even if sparsely available, in recovery of camera motion. In addition, the method utilizes depth by triangulation from the previously estimated motion, and salient visual features for which depth is unavailable. The core of our method is a bundle adjustment that refines the motion estimates in parallel by processing a sequence of images, in a batch optimization. We have evaluated our method in three sensor setups, one using an RGB-D camera, and two using combinations of a camera and a 3D lidar. Our method is rated #2 on the KITTI odometry benchmark irrespective of sensing modality, and is rated #1 among visual odometry methods.

Rows of trees such as in orchards, planted in straight parallel lines can provide navigation cues for autonomous machines that operate in between them. When the tree canopies are well managed, tree rows appear similar to corridor walls and a simple 2D sensing scheme suffices. However, when the tree canopies are three dimensional, or ground vegetation occludes tree trunks, it is necessary to use a three dimensional sensing mode. An additional complication in prolific canopies is that GPS is not reliable and hence is not suitable to register data from sensors onboard a traversing vehicle. Here, we present a method to register 3D data from a lidar sensor onboard a vehicle that must accurately determine its pose relative to the rows. We first register point cloud into a common reference frame and then determine the position of tree rows and trunks in the vicinity to determine the vehicle pose. Our method is tested online and with data from commercial orchards. Experimental results show that the accuracy is sufficient to enable accurate traversal between tree rows even when tree canopies do not approximate planar walls.

We present a monocular visual navigation methodology for autonomous orchard vehicles. Modern orchards are usually planted with straight and parallel tree rows that form a corridor-like environment. Our task consists of driving a vehicle autonomously along the tree rows. The original contributions of this paper are: 1) a method to recover vehicle rotation independently of translation by modeling the vehicle as a car-like robot driving on a 3D ground surface-the rotation is estimated from monocular images while the translation is measured by a wheel encoder; and 2) a method to fit the 3D points corresponding to the trees into straight lines via an optimization algorithm that minimizes the error variance on the robot lookahead point. Additionally, we use a simple vanishing point detection approach to find the ends of the tree rows. The vanishing point detection is integrated into the system via an extended Kalman filter. The methodology's robustness to environmental changes is validated in more than fifty experiments in research and commercial orchards, six of which are presented and discussed in detail.