Sampling-based Motion Planning Research Articles

Motion Planning and Control of Autonomous Aggressive Vehicle Maneuvers

Aggressive vehicle maneuvers such as those performed by professional racing drivers achieve high agility motion at the edge of handling limits. These aggressive maneuvers can be used to design human-inspired active safety features for next-generation “accident-free” vehicles. We present a motion planning and control design for autonomous aggressive vehicle maneuvers. The motion planner takes advantages of the sparse stable trees and the enhanced rapidly exploring random tree (RRT*) algorithms. The use of the sparsity property helps to reduce the computational cost of the RRT* method by removing non-useful nodes in each iteration and therefore to rapidly converge to the optimal solution. The proposed motion control design allows the vehicle to operate outside the stability region to accomplish a safe, agile maneuver. A safety region is computed to augment the stability region and the motion control is built on a modified nonlinear model predictive control method. We implement the proposed planner and controller and demonstrate the autonomous aggressive maneuvers on a 1/7-scale racing vehicle platform. Comparison with human expert driver and other existing methods is also presented to demonstrate the performance and robustness. <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">Note to Practitioners</i> —Motion planning and control of human driver-inspired aggressive vehicle maneuvers is a challenging task because of high-agility, unstable fast vehicle motions. This paper is motivated by addressing this challenge in autonomous driving technologies. Instead of restricting vehicle motions within a stability region that is taken by existing methods, we augment the conservative stability region to a safety region with guaranteed performance. To improve the computational efficiency of sampling-based motion planners, we take advantage of sparsity and also integration of a nonlinear predictive control method to compute feasible vehicle motion in searching space. The stability of the vehicle motion controller and sub-optimality of the motion planner are analyzed and guaranteed. Using a scaled vehicle testbed, we validate and compare the proposed motion planning and control design with other existing methods and human expert driver. The experimental results demonstrate the superior performance than the other methods and comparable with human expert driving skills.

Sampling-Based Motion Planning: A Comparative Review

Sampling-based motion planning is one of the fundamental paradigms to generate robot motions, and a cornerstone of robotics research. This comparative review provides an up-to-date guide and reference manual for the use of sampling-based motion planning algorithms. It includes a history of motion planning, an overview of the most successful planners, and a discussion of their properties. It also shows how planners can handle special cases and how extensions of motion planning can be accommodated. To put sampling-based motion planning into a larger context, a discussion of alternative motion generation frameworks highlights their respective differences from sampling-based motion planning. Finally, a set of sampling-based motion planners are compared on 24 challenging planning problems in order to provide insights into which planners perform well in which situations and where future research would be required. This comparative review thereby provides not only a useful reference manual for researchers in the field but also a guide for practitioners to make informed algorithmic decisions. Expected final online publication date for the Annual Review of Control, Robotics, and Autonomous Systems, Volume 7 is May 2024. Please see http://www.annualreviews.org/page/journal/pubdates for revised estimates.

Astrodynamics-Informed Kinodynamic Sampling-Based Motion Planning for Relative Spacecraft Motion

This paper presents a sampling-based spacecraft relative motion planning algorithm to reconfigure a spacecraft from a given initial state to a desired final state, subject to kinodynamic (simultaneous kinematic and dynamics) constraints. We leverage theoretical advancement in the field of sampling-based motion planning in robotics and extend their application to achieve motion planning in astrodynamics. We present an astrodynamics-informed kinodynamic motion planning (AIKMP) algorithm that takes advantage of a spacecraft’s natural motion to compute a safe, fuel-efficient motion plan for spacecraft reconfiguration in a cluttered environment. This algorithm is developed in the relative orbital element space of the deputy spacecraft around a chief spacecraft that provides very useful geometric insight to relative spacecraft motion, as well as helps in generating an overall smooth final transfer trajectory without demanding additional postprocessing of the transfer solution. Through several example scenarios, we demonstrate that the AIKMP algorithm is regularly able to compute safe and fuel-efficient solutions to relative transfer problems. We also show that this algorithm iteratively improves on its computed solutions and thus holds the capability of finding near-optimal transfer solutions given a sufficient number of algorithm iterations—without requiring an initial guess of the solution.

A Heuristic-Guided Dynamical Multi-Rover Motion Planning Framework for Planetary Surface Missions

We present a heuristic-guided multi-robot motion planning framework that solves the problem of <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">n</i> dynamical agents visiting <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">m</i> unlabeled targets in a partially known environment for planetary surface missions without solving the two-point boundary value problem (BVP). The framework design is motivated by typical planetary surface mission constraints of limited power, limited computation, and limited communication. The framework maintains a centralized, dynamically updated probabilistic roadmap (PRM) that incorporates new obstacle updates as the agents move in the environment. The dynamic roadmap captures the changing obstacle topology and provides updated cost-to-go heuristics to accelerate each agent's independent single-query motion-planning process. The agents use a feasible sampling-based motion planner without computing the BVP while leveraging the roadmap heuristics to quickly plan and visit their assigned target. The agents handle robot-robot and robot-obstacle collision avoidance in a decentralized fashion. We conduct multiple simulation experiments using robots with non-linear dynamics to show our planner performs better in overall planning time and mission time than approaches not using the roadmap heuristic. We also field our algorithm on prototype rovers and demonstrate the viability of implementing our algorithm on real-world hardware platforms.

Motion Planning of Manipulator by Points-Guided Sampling Network

This paper proposes a network called points-guided sampling net (PGSN) to guide the sampling process in sampling-based motion planner by utilizing the geometric information of obstacles. The geometric information is extracted from the point cloud of obstacles. By analyzing the properties of the point cloud, we propose a VAE feature extraction net that incorporates the variational autoencoder (VAE) framework with unique architectures designed for point clouds. Furthermore, we design a multi-modal sampling net to model the probability distribution of the states based on training trajectories taken from different environments. Based on PGSN, we propose a sampling-based motion planning algorithm called the point-guided rapidly-exploring random tree (PG-RRT). Three experiments are conducted to verify the proposed PGSN: Exp I shows the proposed VAE feature extraction net can successfully extract geometric features from the inputted point cloud; Exp II verifies the multi-modal sampling net successfully chooses corresponding mode with respect to extracted features; Exp III demonstrates the efficacy of our PG-RRT algorithm by showing PG-RRT outperforms other algorithms. Moreover, we provide theoretical analysis and insights towards understanding our model. Note to Practitioners—Obstacles cause lots of the sampling space invalid, thus the traditional sampling-based motion planning (SBMP) algorithm is usually unable to generate a trajectory within a reasonable short period of time. To improve the success rate and efficiency of SBMP, this paper proposes a novel deep neural network called points-guided sampling net (PGSN). PGSN is designed to exploit: (1) environmental point clouds and (2) training trajectories from multiple environments with different obstacles. In the first step, the point clouds include important geometric information. To utilize this information, we adopt a variational autoencoder approach which combines an encoder and a decoder together to extract geometric features more accurately from point clouds. In the second step, trajectories from multiple environments have a multi-modal property which can be represented by a truncated multivariate Gaussian mixture model. We propose a multi-modal sampling net to learn optimal parameters of this model from the training trajectories, and to select corresponding mode based on the extracted features. Experiments demonstrate that the proposed algorithm is feasible and can achieve higher success rate than the state-of-the-art methods. Our method uses a single frame of point cloud to improve efficiency, therefore multiple point clouds from different perspective maybe needed when objects occlude with each other.

A sampling and learning framework to prove motion planning infeasibility

We present a learning-based approach to prove infeasibility of kinematic motion planning problems. Sampling-based motion planners are effective in high-dimensional spaces but are only probabilistically complete. Consequently, these planners cannot provide a definite answer if no plan exists, which is important for high-level scenarios, such as task-motion planning. We apply data generated during multi-directional sampling-based planning (such as PRM) to a machine learning approach to construct an infeasibility proof. An infeasibility proof is a closed manifold in the obstacle region of the configuration space that separates the start and goal into disconnected components of the free configuration space. We train the manifold using common machine learning techniques and then triangulate the manifold into a polytope to prove containment in the obstacle region. Under assumptions about the hyper-parameters and robustness of configuration space optimization, the output is either an infeasibility proof or a motion plan in the limit. We demonstrate proof construction for up to 4-DOF configuration spaces. A large part of the algorithm is parallelizable, which offers potential to address higher dimensional configuration spaces.

HDM-RRT: A Fast HD-Map-Guided Motion Planning Algorithm for Autonomous Driving in the Campus Environment

On campus, the complexity of the environment and the lack of regulatory constraints make it difficult to model the environment, resulting in less efficient motion planning algorithms. To solve this problem, HD-Map-guided sampling-based motion planning is a feasible research direction. We proposed a motion planning algorithm for autonomous vehicles on campus, called HD-Map-guided rapidly-exploring random tree (HDM-RRT). In our algorithm, A collision risk map (CR-Map) that quantifies the collision risk coefficient on the road is combined with the Gaussian distribution for sampling to improve the efficiency of algorithm. Then, the node optimization strategy of the algorithm is deeply optimized through the prior information of the CR-Map to improve the convergence rate and solve the problem of poor stability in campus environments. Three experiments were designed to verify the efficiency and stability of our approach. The results show that the sampling efficiency of our algorithm is four times higher than that of the Gaussian distribution method. The average convergence rate of the proposed algorithm outperforms the RRT* algorithm and DT-RRT* algorithm. In terms of algorithm efficiency, the average computation time of the proposed algorithm is only 15.98 ms, which is much better than that of the three compared algorithms.

A Reachability-Based Spatio-Temporal Sampling Strategy for Kinodynamic Motion Planning

By limiting the planning domain to “ <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">L</i> <sub xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2</sub> Informed Set”, some sampling-based motion planners (SBMP) (e.g., Informed RRT <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> , BIT <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> ) can solve the geometric motion planning problems efficiently. However, the construction of informed set (IS) will be very challenging, when further differential constraints are considered. For the time-optimal kinodynamic motion planning problem, this paper defines a modified time informed set (MTIS) to limit the planning domain. Due to drawing inspiration from Hamilton-Jacobi-Bellman (HJB) reachability analysis, MTIS, compared with the original TIS, can not only help save the running time of SBMP, but also extend the applicable scope from linear systems to polynomial nonlinear systems with control constrains. On this basis, a spatio-temporal sampling strategy adapted to MTIS is proposed. Firstly, MTIS is used to estimate the optimal cost and the valid tree structure is reused, so that we do not need to provide a solution trajectory in advance. Secondly, this strategy is generic, allowing it to be combined with common SBMP (such as SST, etc.) to accelerate convergence and reduce the memory requirement. Several simulation experiments also demonstrate the effectiveness of proposed method.

Fast Trajectory Generation on a Path using Feedback Linearization*

This paper describes a method for quickly generating feasible trajectories for fully actuated autonomous surface vehicles. It is assumed that a path consisting of a list of positions with associated heading angles is given. The goal is to generate a feasible trajectory that follows a given path and takes into account the nonlinear dynamical model. The trajectory contains the state as well as force-level input variables and can be used as feed-forward control by a trajectory controller. The generation of the trajectory is based on a closed-loop simulation of the nonlinear model in combination with a nonlinear controller and guidance law. Feedback linearization is applied to linearize the motion model of the surface vehicle at the velocity level. A state feedback controller is used to control the pose along the path. The presented method allows for simple and fast trajectory generation and can therefore be applied in sampling-based motion planners where many trajectories have to be generated.

On Free Velocity Cones of Narrow Passages of High-DoF Kinematic Chains

Narrow passages in free space pose great challenges to many sampling-based motion planners. In problems with high-dimensional configuration spaces (C-spaces), narrow passages in free space (C-free) can be hard to find and navigate and, sometimes, even counterintuitive. In this article, we present an algorithm to construct cones of available instantaneous velocities (“free velocity cones” or, briefly, “free cones”) on the boundary of C-free to facilitate finding and navigating narrow passages. This is accomplished by first developing free cones and associated measures of local C-space narrowness for a single rigid link. These results are then extended to kinematic chains (open and closed) of arbitrary degrees of freedom. It turns out that the degeneracy of the free cones dictates the existence of C-space narrow passages, and the locations and orientations of the links in the workspace relate intimately to the corresponding connected component of C-free. This observation leads us to a modified probabilistic roadmap (M-PRM) algorithm and a modified rapidly exploring random vine (M-RRV) algorithm of combining the enumeration of topological components and random sampling of the free cones. Experimental results from applying our algorithms to several challenging examples show that our new algorithms are more efficient than some variants of the PRM algorithms, and several variants of the rapidly exploring random tree (RRT) algorithms, such as RRT-CONNECT and RRV.

Adaptively Exploits Local Structure With Generalised Multi-Trees Motion Planning

Sampling-based motion planners perform exceptionallywell in robotic applications that operate in high-dimensional spaces. However, most previous works often constrain the planning workspace rooted at some fixed locations, do not adaptively reason on strategies for narrow passages, and ignore valuable local structure information. In this letter, we propose Rapidly-exploring Random Forest ( <sc xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">rrf</small> <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> )—a generalised multi-trees motion planner that combines the rapid exploring property of tree-based methods and adaptively learns to deploys a Bayesian local sampling strategy in regions that are deemed to be bottlenecks. Local sampling exploits the local-connectivity of spaces via Markov Chain random sampling, which is updated sequentially with a Bayesian proposal distribution to learn the local structure from past observations. The trees selection problem is formulated as a multi-armed bandit problem, which efficiently allocates resources to the most promising tree accelerating planning runtime. <sc xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">rrf</small> <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> learns the region that is difficult to perform tree extensions and adaptively deploys local sampling in those regions to maximise the benefit of exploiting local structure. We provide rigorous proofs of completeness and almost-surely asymptotic optimal convergence, and experimentally demonstrate that the effectiveness of <sc xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">rrf</small> <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> ’s adaptive multi-trees approach allows it to perform well in a wide range of problems.

Custom distribution for sampling-based motion planning

Sampling-based motion planning algorithms are widely used in robotics because they are very effective in high-dimensional spaces. However, the success rate and quality of the solutions are determined by an adequate selection of their parameters such as the distance between states, the local planner, and the sampling distribution. For robots with large configuration spaces or dynamic restrictions, selecting these parameters is a challenging task. This paper proposes a method for improving the performance to a set of the most popular sampling-based algorithms, the Rapidly-exploring Random Trees (RRTs) by adjusting the sampling method. The idea is to replace the uniform probability density function (U-PDF) with a custom distribution (C-PDF) learned from previously successful queries in similar tasks. With a few samples, our method builds a custom distribution that allows the RRT to grow to promising states that will lead to a solution. We tested our method in several autonomous driving tasks such as parking maneuvers, obstacle clearance and under narrow passages scenarios. The results show that the proposed method outperforms the original RRT and several improved versions in terms of success rate, tree density and computation time. In addition, the proposed method requires a relatively small set of examples, unlike current deep learning techniques that require a vast amount of examples.

Integrating Algorithmic Sampling-Based Motion Planning with Learning in Autonomous Driving

Sampling-based motion planning (SBMP) is a major algorithmic trajectory planning approach in autonomous driving given its high efficiency and outstanding performance in practice. However, driving safety still calls for further refinement of SBMP. In this article we organically integrate algorithmic motion planning with learning models to improve SBMP in highway traffic scenarios from the following two perspectives. First, given the number of points to be sampled, we develop a new model to sample “important” points for SBMP by predicting the intention of surrounding vehicles and learning the distribution of human drivers’ trajectory. Second, we empirically study the relationship between the number of sample points and the environment, which is largely ignored in conventional SBMP. Then, we provide a guideline to select the appropriate number of points to be sampled under different scenarios to guarantee efficiency. The simulation experiments are conducted based on the vehicle trajectory dataset NGSIM. The results show that the proposed sampling strategy outperforms existing sampling strategies in terms of the computing time, traveling time, and smoothness of the trajectory.

A Survey on the Integration of Machine Learning with Sampling-based Motion Planning

Sampling-based methods are widely adopted solutions for robot motion planning. The methods are straightforward to implement, effective in practice for many robotic systems. It is often possible to prove that they have desirable properties, such as probabilistic completeness and asymptotic optimality. Nevertheless, they still face challenges as the complexity of the underlying planning problem increases, especially under tight computation time constraints, which impact the quality of returned solutions or given inaccurate models. This has motivated machine learning to improve the computational efficiency and applicability of Sampling-Based Motion Planners (SBMPs). This survey reviews such integrative efforts and aims to provide a classification of the alternative directions that have been explored in the literature. It first discusses how learning has been used to enhance key components of SBMPs, such as node sampling, collision detection, distance or nearest neighbor computation, local planning, and termination conditions. Then, it highlights planners that use learning to adaptively select between different implementations of such primitives in response to the underlying problem's features. It also covers emerging methods, which build complete machine learning pipelines that reflect the traditional structure of SBMPs. It also discusses how machine learning has been used to provide data-driven models of robots, which can then be used by a SBMP. Finally, it provides a comparative discussion of the advantages and disadvantages of the approaches covered, and insights on possible future directions of research. An online version of this survey can be found at: https://prx-kinodynamic.github.io/

MP-RRT#: a Model Predictive Sampling-based Motion Planning Algorithm for Unmanned Aircraft Systems

This paper introduces a kinodynamic motion planning algorithm for Unmanned Aircraft Systems (UAS), called MP-RRT#. MP-RRT# joins the potentialities of RRT# with a strategy based on Model Predictive Control to efficiently solve motion planning problems under differential constraints. Similar to other RRT-based algorithms, MP-RRT# explores the map constructing an asymptotically optimal graph. In each iteration the graph is extended with a new vertex in the reference state of the UAS. Then, a forward simulation is performed using a Model Predictive Control strategy to evaluate the motion between two adjacent vertices, and a trajectory in the state space is computed. As a result, the MP-RRT# algorithm eventually generates a feasible trajectory for the UAS satisfying dynamic constraints. Simulation results obtained with a simulated drone controlled with the PX4 autopilot corroborate the validity of the MP-RRT# approach.

Sbp-env: A Python Package for Sampling-based Motion Planner and Samplers

Sampling-based motion planners' testing environment (sbp-env) is a full feature framework to quickly test different sampling-based algorithms for motion planning. sbp-env focuses on the flexibility of tinkering with different aspects of the framework, and had divided the main planning components into two categories (i) samplers and (ii) planners. The focus of motion planning research had been mainly on (i) improving the sampling efficiency (with methods such as heuristic or learned distribution) and (ii) the algorithmic aspect of the planner using different routines to build a connected graph. Therefore, by separating the two components one can quickly swap out different components to test novel ideas.

Urban Metric Maps for Small Unmanned Aircraft Systems Motion Planning

Low-altitude urban flight planning for small unmanned aircraft systems (UASs) accurate vehicle kinodynamics, environment maps, and risk models to assure that flight plans consider the urban landscape as well as airspace constraints. This paper presents a suite of motion planning metrics designed for small UAS urban flight and defines map-based and path-based metrics to holistically characterize motion plan quality. Proposed metrics are examined in the context of representative geometric, graph-based, and sampling-based motion planners applied to a multicopter small UAS. A novel multi-objective heuristic is proposed and applied for graph-based and sampling motion planners at four urban UAS flight altitude layers. Monte Carlo case studies in a New York City urban environment illustrate metric map properties and planner performance. Motion plans are evaluated as a function of planning algorithm, location, range, and flight altitude.

Creating Better Collision-Free Trajectory for Robot Motion Planning by Linearly Constrained Quadratic Programming.

Many algorithms in probabilistic sampling-based motion planning have been proposed to create a path for a robot in an environment with obstacles. Due to the randomness of sampling, they can efficiently compute the collision-free paths made of segments lying in the configuration space with probabilistic completeness. However, this property also makes the trajectories have some unnecessary redundant or jerky motions, which need to be optimized. For most robotics applications, the trajectories should be short, smooth and keep away from obstacles. This paper proposes a new trajectory optimization technique which transforms a polygon collision-free path into a smooth path, and can deal with trajectories which contain various task constraints. The technique removes redundant motions by quadratic programming in the parameter space of trajectory, and converts collision avoidance conditions to linear constraints to ensure absolute safety of trajectories. Furthermore, the technique uses a projection operator to realize the optimization of trajectories which are subject to some hard kinematic constraints, like keeping a glass of water upright or coordinating operation with dual robots. The experimental results proved the feasibility and effectiveness of the proposed method, when it is compared with other trajectory optimization methods.

Autonomous Data Collection With Timed Communication Constraints for Unmanned Underwater Vehicles

This letter studies the problem of data collection where an autonomous underwater vehicle (AUV) is required to inspect several goals while staying within the communication range of an unmanned surface vehicle (USV) moving along a predefined path. A reward is associated with each inspected goal, and, so the overall objective is to increase the sum of the rewards. The proposed planner takes the AUV dynamics and the obstacles into account in order to generate collision-free and dynamically-feasible trajectories that enable the AUV to inspect multiple goals while maintaining communication with the USV. When not all the goals can be inspected, the planner generates trajectories that increase the sum of the rewards. The proposed planner constructs a roadmap equipped with time information in order to facilitate navigation and maintain communication in complex 3D environments. A crucial aspect is an effective coupling of discrete search with sampling-based motion planning. Specifically, a multi-goal discrete search is proposed to effectively compute roadmap paths that increase the sum of the rewards. These paths then guide the sampling-based expansion of a motion tree. Experiments using complex 3D environments and an AUV model defined by nonlinear second-order differential equations indicate the efficiency of the planner to inspect an increasing number of goals.

Planning Under Non-Rational Perception of Uncertain Spatial Costs

This work investigates the design of risk-perception-aware motion-planning strategies that incorporate non-rational risk associated with uncertain spatial costs. Our proposed method employs the Cumulative Prospect Theory (CPT) to generate a perceived risk map over a given environment. CPT-like perceived risks and path-length metrics are then combined to define a cost function that is compliant with the requirements of asymptotic optimality of sampling-based motion planners (RRT*). The modeling power of CPT is illustrated in theory and in simulation, along with a comparison to other risk perception models like Conditional Value at Risk (CVaR). Theoretically, we define a notion of expressiveness for a risk perception model and show that CPT's is higher than that of CVaR and expected risk. We then show that this expressiveness translates to our path planning setting, where we observe that a planner equipped with CPT together with a simultaneous perturbation stochastic approximation (SPSA) method can better approximate arbitrary paths in an environment. Additionally, we show in simulation that our planner captures a rich set of meaningful paths, representative of different risk perceptions in a custom environment. We then compare the performance of our planner with T-RRT* (a planner for continuous cost spaces) and Risk-RRT* (a risk-aware planner for dynamic human obstacles) through simulations in cluttered and dynamic environments respectively, showing the advantage of our proposed planner.