In the real world, people and objects will move around the environment, the world will change, and a robot cannot stay stationary and wait, scratching its head to think of what to do. It is essential for critical robotics algorithms that guarantee safety and liveness (i.e., will be able to reach the goal) to be able to react immediately to changing circumstances. Autonomous cars must make decisions that determine the safety of their passengers and the world around them, mobile manipulators working in a factory or a warehouse may need to dodge unseen people and obstacles, and surgical robots must maintain patient safety at all times.
Algorithms that provide real-time performance can come through theoretical properties (guaranteeing a solution in a fixed amount of computation) or software/hardware engineering (using implementation or platform-specific features to accelerate algorithms).
Partially Observable Markov Decision Processes (POMDPs) are a general and principled framework for motion planning under uncertainty. Despite tremendous improvement in the scalability of POMDP solvers, long-horizon POMDPs (e.g., ≥15 steps) remain difficult to solve. This paper proposes a new approximate online POMDP solver, called Reference-Based Online POMDP Planning via Rapid State Space Sampling (ROP-RaS3). ROP-RaS3 uses novel extremely fast sampling-based motion planning techniques to sample the state space and generate a diverse set of macro actions online which are then used to bias belief-space sampling and infer high-quality policies without requiring exhaustive enumeration of the action space—a fundamental constraint for modern online POMDP solvers. ROP-RaS3 is evaluated on various long-horizon POMDPs, including on a problem with a planning horizon of more than 100 steps and a problem with a 15-dimensional state space that requires more than 20 look ahead steps. In all of these problems, ROP-RaS3 substantially outperforms other state-of-the-art methods by up to multiple folds.
@inproceedings{liang2024ropras,title={Scaling Long-Horizon Online {POMDP} Planning via Rapid State Space Sampling},author={Liang, Yuanchu and Kim, Edward and Thomason, Wil and Kingston, Zachary and Kurniawati, Hanna and Kavraki, Lydia E.},booktitle={International Symposium of Robotics Research},year={2024},note={To Be Presented},}
arXiv
Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees (FCIT*)
Improving the performance of motion planning algorithms for high-degree-of-freedom robots usually requires reducing the cost or frequency of computationally expensive operations. Traditionally, and especially for asymptotically optimal sampling-based motion planners, the most expensive operations are local motion validation and querying the nearest neighbours of a configuration. Recent advances have significantly reduced the cost of motion validation by using single instruction/multiple data (SIMD) parallelism to improve solution times for satisficing motion planning problems. These advances have not yet been applied to asymptotically optimal motion planning. This paper presents Fully Connected Informed Trees (FCIT*), the first fully connected, informed, anytime almost-surely asymptotically optimal (ASAO) algorithm. FCIT* exploits the radically reduced cost of edge evaluation via SIMD parallelism to build and search fully connected graphs. This removes the need for nearest-neighbours structures, which are a dominant cost for many sampling-based motion planners, and allows it to find initial solutions faster than state-of-the-art ASAO (VAMP, OMPL) and satisficing (OMPL) algorithms on the MotionBenchMaker dataset while converging towards optimal plans in an anytime manner.
@misc{wilson2024fcit,title={Nearest-Neighbourless Asymptotically Optimal Motion Planning with Fully Connected Informed Trees ({FCIT}*)},author={Wilson, Tyler S. and Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E. and Gammell, Jonathan D.},year={2024},eprint={2411.17902},archiveprefix={arXiv},primaryclass={cs.RO},note={Under Review},}
Motion planning against sensor data is often a critical bottleneck in real-time robot control. For sampling-based motion planners, which are effective for high-dimensional systems such as manipulators, the most time-intensive component is collision checking. We present a novel spatial data structure, the collision-affording point tree (CAPT): an exact representation of point clouds that accelerates collision-checking queries between robots and point clouds by an order of magnitude, with an average query time of less than 10 nanoseconds on 3D scenes comprising thousands of points. With the CAPT, sampling-based planners can generate valid, high-quality paths in under a millisecond, with total end-to-end computation time faster than 60 FPS, on a single thread of a consumer-grade CPU. We also present a point cloud filtering algorithm, based on space-filling curves, which reduces the number of points in a point cloud while preserving structure. Our approach enables robots to plan at real-time speeds in sensed environments, opening up potential uses of planning for high-dimensional systems in dynamic, changing, and unmodeled environments.
@inproceedings{ramsey2024,author={Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},title={Collision-Affording Point Trees: SIMD-Amenable Nearest Neighbors for Fast Collision Checking},booktitle={Robotics: Science and Systems},year={2024},doi={10.15607/RSS.2024.XX.038},}
Modern sampling-based motion planning algorithms typically take between hundreds of milliseconds to dozens of seconds to find collision-free motions for high degree-of-freedom problems. This paper presents performance improvements of more than 500x over the state-of-the-art, bringing planning times into the range of microseconds and solution rates into the range of kilohertz, without specialized hardware. Our key insight is how to exploit fine-grained parallelism within sampling-based planners, providing generality-preserving algorithmic improvements to any such planner and significantly accelerating critical subroutines, such as forward kinematics and collision checking. We demonstrate our approach over a diverse set of challenging, realistic problems for complex robots ranging from 7 to 14 degrees-of-freedom. Moreover, we show that our approach does not require high-power hardware by also evaluating on a low-power single-board computer. The planning speeds demonstrated are fast enough to reside in the range of control frequencies and open up new avenues of motion planning research.
@inproceedings{thomason2024vamp,author={Thomason, Wil and Kingston, Zachary and Kavraki, Lydia E.},title={Motions in Microseconds via Vectorized Sampling-Based Planning},year={2024},booktitle={IEEE International Conference on Robotics and Automation},pages={8749--8756},doi={10.1109/ICRA57147.2024.10611190},}
Workshop
Dynamic Motion Planning from Perception via Accelerated Point Cloud Collision Checking
@inproceedings{Ramsey2024wksp,author={Ramsey, Clayton W. and Kingston, Zachary and Thomason, Wil and Kavraki, Lydia E.},title={Dynamic Motion Planning from Perception via Accelerated Point Cloud Collision Checking},booktitle={IEEE ICRA 2024 Workshop---Agile Robotics: From Perception to Dynamic Action},year={2024},}
Over the years, many motion planning algorithms have been proposed. It is often unclear which algorithm might be best suited for a particular class of problems. The problem is compounded by the fact that algorithm performance can be highly dependent on parameter settings. This paper shows that hyperparameter optimization is an effective tool in both algorithm selection and parameter tuning over a given set of motion planning problems. We present different loss functions for optimization that capture different notions of optimality. The approach is evaluated on a broad range of scenes using two different manipulators, a Fetch and a Baxter. We show that optimized planning algorithm performance significantly improves upon baseline performance and generalizes broadly in the sense that performance improvements carry over to problems that are very different from the ones considered during optimization.
@inproceedings{moll2021hyper,author={Moll, Mark and Chamzas, Constantinos and Kingston, Zachary and Kavraki, Lydia E.},title={HyperPlan: A Framework for Motion Planning Algorithm Selection and Parameter Optimization},booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems},year={2021},pages={2511-2518},doi={10.1109/IROS51168.2021.9636651},}