Fast Collision Checking: From Single Robots to Multi-Robot Teams

We examine three different algorithms that enable the collision certificate method from [Bialkowski, et al.] to handle the case of a centralized multi-robot team. By taking advantage of symmetries in the configuration space of multi-robot teams, our methods can significantly reduce the number of collision checks vs. both [Bialkowski, et al.] and standard collision checking implementations

Online Sampling in the Parameter Space of a Neural Network for GPU-accelerated Motion Planning of Autonomous Vehicles

This paper proposes online sampling in the parameter space of a neural network for GPU-accelerated motion planning of autonomous vehicles. Neural networks are used as controller parametrization since they can handle nonlinear non-convex systems and their complexity does not scale with prediction horizon length. Network parametrizations are sampled at each sampling time and then held constant throughout the prediction horizon. Controls still vary over the prediction horizon due to varying feature vectors fed to the network. Full-dimensional vehicles are modeled by polytopes. Under the assumption of obstacle point data, and their extrapolation over a prediction horizon under constant velocity assumption, collision avoidance reduces to linear inequality checks. Steering and longitudinal acceleration controls are determined simultaneously. The proposed method is designed for parallelization and therefore well-suited to benefit from continuing advancements in hardware such as GPUs. Characteristics of proposed method are illustrated in 5 numerical simulation experiments including dynamic obstacle avoidance, waypoint tracking requiring alternating forward and reverse driving with maximal steering, and a reverse parking scenario.Comment: 8 pages, 8 figures, 3 tables, conference pape

Efficient high-quality motion planning by fast all-pairs r-nearest-neighbors

Sampling-based motion-planning algorithms typically rely on nearest-neighbor (NN) queries when constructing a roadmap. Recent results suggest that in various settings NN queries may be the computational bottleneck of such algorithms. Moreover, in several asymptotically-optimal algorithms these NN queries are of a specific form: Given a set of points and a radius r report all pairs of points whose distance is at most r. This calls for an application-specific NN data structure tailored to efficiently answering this type of queries. Randomly transformed grids (RTG) were recently proposed by Aiger et al. as a tool to answer such queries and have been shown to outperform common implementations of NN data structures in this context. In this work we employ RTG for sampling-based motion-planning algorithms and describe an efficient implementation of the approach. We show that for motion-planning, RTG allow for faster convergence to high-quality solutions when compared with existing NN data structures. Additionally, RTG enable significantly shorter construction times for batched-PRM variants; specifically, we demonstrate a speedup by a factor of two to three for some scenarios

Generalized Lazy Search for Robot Motion Planning: Interleaving Search and Edge Evaluation via Event-based Toggles

Lazy search algorithms can efficiently solve problems where edge evaluation is the bottleneck in computation, as is the case for robotic motion planning. The optimal algorithm in this class, LazySP, lazily restricts edge evaluation to only the shortest path. Doing so comes at the expense of search effort, i.e., LazySP must recompute the search tree every time an edge is found to be invalid. This becomes prohibitively expensive when dealing with large graphs or highly cluttered environments. Our key insight is the need to balance both edge evaluation and search effort to minimize the total planning time. Our contribution is two-fold. First, we propose a framework, Generalized Lazy Search (GLS), that seamlessly toggles between search and evaluation to prevent wasted efforts. We show that for a choice of toggle, GLS is provably more efficient than LazySP. Second, we leverage prior experience of edge probabilities to derive GLS policies that minimize expected planning time. We show that GLS equipped with such priors significantly outperforms competitive baselines for many simulated environments in R2, SE(2) and 7-DoF manipulation.Comment: Accepted at International Conference on Automated Planning and Scheduling (ICAPS) 201

Computationally Efficient Obstacle Avoidance Trajectory Planner for UAVs Based on Heuristic Angular Search Method

For accomplishing a variety of missions in challenging environments, the capability of navigating with full autonomy while avoiding unexpected obstacles is the most crucial requirement for UAVs in real applications. In this paper, we proposed such a computationally efficient obstacle avoidance trajectory planner that can be used in cluttered unknown environments. Because of the narrow view field of single depth camera on a UAV, the information of obstacles around is quite limited thus the shortest entire path is difficult to achieve. Therefore we focus on the time cost of the trajectory planner and safety rather than other factors. This planner is mainly composed of a point cloud processor, a waypoint publisher with Heuristic Angular Search(HAS) method and a motion planner with minimum acceleration optimization. Furthermore, we propose several techniques to enhance safety by making the possibility of finding a feasible trajectory as big as possible. The proposed approach is implemented to run onboard in real-time and is tested extensively in simulation and the average control output calculating time of iteration steps is less than 18 ms

Trajectory Generation with Fast Lidar-based 3D Collision Avoidance for Agile MAVs

Micro aerial vehicles (MAVs), are frequently used for exploration, examination, and surveillance during search and rescue missions. Manually piloting these robots under stressful conditions provokes pilot errors and can result in crashes with disastrous consequences. Also, during fully autonomous flight, planned high-level trajectories can be erroneous and steer the robot into obstacles. In this work, we propose an approach to efficiently compute smooth, time-optimal trajectories MAVs that avoid obstacles. Our method first computes a trajectory from the start to an arbitrary target state, including position, velocity, and acceleration. It respects input- and state-constraints and is thus dynamically feasible. Afterward, we efficiently check the trajectory for collisions in the 3D-point cloud, recorded with the onboard lidar. We exploit the piecewise polynomial formulation of our trajectories to analytically compute axis-aligned bounding boxes (AABB) to speed up the collision checking process. If collisions occur, we generate a set of alternative trajectories in real-time. Alternative trajectories bring the MAV in a safe state, while still pursuing the original goal. Subsequently, we choose and execute the best collision-free alternative trajectory based on a distance metric. The evaluation in simulation and during a real firefighting exercise shows the capability of our method.Comment: Accepted for IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), Abu Dhabi, UAE, 202

Fast Medial Axis Sampling for Use in Motion Planning

Motion planning is a difficult but important problem in robotics. Research has tended toward approximations and randomized algorithms, like sampling-based planning. Probabilistic RoadMaps (PRMs) are one common sampling-based planning approach, but they lack safety guarantees. One main approach, Medial Axis PRM (MAPRM) addressed this deficiency by generating robot configurations as far away from the obstacles as possible, but it introduced an extensive computational burden. We present two techniques, Medial Axis Bridge and Medial Axis Spherical Step, to reduce the computational cost of sampling in MAPRM and additionally propose recycling previously computed clearance information to reduce the cost of connection in MAPRM. We provide experimental results that demonstrate the effectiveness of our proposed methods by: (1) showing that Medial Axis Bridge and Medial Axis Spherical Step both reduce the sampling time of MAPRM by nearly 50% while guaranteeing the same degree of safety, and (2) showing a nearly 50% decrease in connection time in MAPRM

Asymptotically-Optimal Motion Planning using Lower Bounds on Cost

Many path-finding algorithms on graphs such as A* are sped up by using a heuristic function that gives lower bounds on the cost to reach the goal. Aiming to apply similar techniques to speed up sampling-based motion-planning algorithms, we use effective lower bounds on the cost between configurations to tightly estimate the cost-to-go. We then use these estimates in an anytime asymptotically-optimal algorithm which we call Motion Planning using Lower Bounds (MPLB). MPLB is based on the Fast Marching Trees (FMT*) algorithm recently presented by Janson and Pavone. An advantage of our approach is that in many cases (especially as the number of samples grows) the weight of collision detection in the computation is almost negligible with respect to nearest-neighbor calls. We prove that MPLB performs no more collision-detection calls than an anytime version of FMT*. Additionally, we demonstrate in simulations that for certain scenarios, the algorithmic tools presented here enable efficiently producing low-cost paths while spending only a small fraction of the running time on collision detection

Collision detection or nearest-neighbor search? On the computational bottleneck in sampling-based motion planning

The complexity of nearest-neighbor search dominates the asymptotic running time of many sampling-based motion-planning algorithms. However, collision detection is often considered to be the computational bottleneck in practice. Examining various asymptotically optimal planning algorithms, we characterize settings, which we call NN-sensitive, in which the practical computational role of nearest-neighbor search is far from being negligible, i.e., the portion of running time taken up by nearest-neighbor search is comparable, or sometimes even greater than the portion of time taken up by collision detection. This reinforces and substantiates the claim that motion-planning algorithms could significantly benefit from efficient and possibly specifically-tailored nearest-neighbor data structures. The asymptotic (near) optimality of these algorithms relies on a prescribed connection radius, defining a ball around a configuration $q$, such that $q$ needs to be connected to all other configurations in that ball. To facilitate our study, we show how to adapt this radius to non-Euclidean spaces, which are prevalent in motion planning. This technical result is of independent interest, as it enables to compare the radial-connection approach with the common alternative, namely, connecting each configuration to its $k$ nearest neighbors ($k$-NN). Indeed, as we demonstrate, there are scenarios where using the radial connection scheme, a solution path of a specific cost is produced ten-fold (and more) faster than with $k$-NN

Predicting Sample Collision with Neural Networks

Many state-of-art robotics applications require fast and efficient motion planning algorithms. Existing motion planning methods become less effective as the dimensionality of the robot and its workspace increases, especially the computational cost of collision detection routines. In this work, we present a framework to address the cost of expensive primitive operations in sampling-based motion planning. This framework determines the validity of a sample robot configuration through a novel combination of a Contractive AutoEncoder (CAE), which captures a occupancy grids representation of the robot's workspace, and a Multilayer Perceptron, which efficiently predicts the collision state of the robot from the CAE and the robot's configuration. We evaluate our framework on multiple planning problems with a variety of robots in 2D and 3D workspaces. The results show that (1) the framework is computationally efficient in all investigated problems, and (2) the framework generalizes well to new workspaces.Comment: 7 pages, 7 figure