Motion planning around obstacles with convex optimization

From quadrotors delivering packages in urban areas to robot arms moving in confined warehouses, motion planning around obstacles is a core challenge in modern robotics. Planners based on optimization can design trajectories in high-dimensional spaces while satisfying the robot dynamics. However, in the presence of obstacles, these optimization problems become nonconvex and very hard to solve, even just locally. Thus, when facing cluttered environments, roboticists typically fall back to sampling-based planners that do not scale equally well to high dimensions and struggle with continuous differential constraints. Here, we present a framework that enables convex optimization to efficiently and reliably plan trajectories around obstacles. Specifically, we focus on collision-free motion planning with costs and constraints on the shape, the duration, and the velocity of the trajectory. Using recent techniques for finding shortest paths in Graphs of Convex Sets (GCS), we design a practical convex relaxation of the planning problem. We show that this relaxation is typically very tight, to the point that a cheap postprocessing of its solution is almost always sufficient to identify a collision-free trajectory that is globally optimal (within the parameterized class of curves). Through numerical and hardware experiments, we demonstrate that our planner, which we name GCS, can find better trajectories in less time than widely used sampling-based algorithms and can reliably design trajectories in high-dimensional complex environments.

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.

Parallel Sensor-Space Lattice Planner for Real-Time Obstacle Avoidance

This paper presents a parallel motion planner for mobile robots and autonomous vehicles based on lattices created in the sensor space of planar range finders. The planner is able to compute paths in a few milliseconds, thus allowing obstacle avoidance in real time. The proposed sensor-space lattice (SSLAT) motion planner uses a lattice to tessellate the area covered by the sensor and to rapidly compute collision-free paths in the robot surroundings by optimizing a cost function. The cost function guides the vehicle to follow a vector field, which encodes the desired vehicle path. We evaluated our method in challenging cluttered static environments, such as warehouses and forests, and in the presence of moving obstacles, both in simulations and real experiments. In these experiments, we show that our algorithm performs collision checking and path planning faster than baseline methods. Since the method can have sequential or parallel implementations, we also compare the two versions of SSLAT and show that the run time for its parallel implementation, which is independent of the number and shape of the obstacles found in the environment, provides a speedup greater than 25.

Sampling-based unmanned aerial vehicle air traffic integration, path planning, and collision avoidance

Unmanned aircraft or drones as they are sometimes called are continuing to become part of more real-life applications. The integration of unmanned aerial vehicles in public airspace is becoming an important issue that should be addressed. As the number of unmanned aerial vehicles and their applications are largely increasing, air traffic with more unmanned aircraft has to be given more attention to prevent collisions and maintain safe skies. Unmanned aerial vehicle air traffic integration and regulation has become a priority to different regulatory agencies and has become of greater interest for many researchers all around the world. In this research, a sampling-based air traffic integration, path planning, and collision avoidance approach is presented. The proposed algorithm expands an existing 2D sampling-based approach. The original 2D approach deals with only two unmanned aircraft. Each of the two aircraft shares location information with a ground-based path planner computer, which would send back the avoidance waypoints after performing the 2D sampling. The algorithm proposed in this article can handle any number of drones in the 3D space by performing either 2D or 3D sampling. The proposed work shows a 10-fold enhancement in terms of the number of unmanned aerial vehicle collisions. The presented results also contribute to enabling a better understanding of what is expected of integrating more drones in dense skies.

HyP-DESPOT: A hybrid parallel algorithm for online planning under uncertainty

Robust planning under uncertainty is critical for robots in uncertain, dynamic environments, but incurs high computational cost. State-of-the-art online search algorithms, such as DESPOT, have vastly improved the computational efficiency of planning under uncertainty and made it a valuable tool for robotics in practice. This work takes one step further by leveraging both CPU and GPU parallelization in order to achieve real-time online planning performance for complex tasks with large state, action, and observation spaces. Specifically, Hybrid Parallel DESPOT (HyP-DESPOT) is a massively parallel online planning algorithm that integrates CPU and GPU parallelism in a multi-level scheme. It performs parallel DESPOT tree search by simultaneously traversing multiple independent paths using multi-core CPUs; it performs parallel Monte Carlo simulations at the leaf nodes of the search tree using GPUs. HyP-DESPOT provably converges in finite time under moderate conditions and guarantees near-optimality of the solution. Experimental results show that HyP-DESPOT speeds up online planning by up to a factor of several hundred in several challenging robotic tasks in simulation, compared with the original DESPOT algorithm. It also exhibits real-time performance on a robot vehicle navigating among many pedestrians.

Collision-Free Path Planning and Delivery Sequence Optimization in Noncoplanar Radiation Therapy

Radiation therapy is among the top three cancer treatments in current medical services. The novel noncoplanar radiation therapy which claimed the best characteristics in almost all dosimetric properties encountered the challenges of the potential collision and the long time delivering. In this paper, we proposed a brand new scheme which uses a combined method of the collision avoidance path planning based on an improved probability roadmap method (PRM) and the delivery sequence optimization based on a modified genetic algorithm (GA) to solve the problems in noncoplanar radiation therapy. A uniform sampling strategy, an improved connection strategy, and an efficient local planner are introduced to optimize the roadmap result and accelerate the roadmap construction. The GA is improved by the elitist selection, the local search strategy, and the similar substitution strategy to achieve a better performance both in convergence rate and optimal solution. Experiments are carried out on the simulation platform with typical therapy system models. The results show that our proposed methods work well with the radiation therapy system in a compact working area. Collision is avoided and time consumption is reduced. We believe that our proposed algorithms could solve the problems in current radiation therapy and promote their clinic applications.

Maintaining and Enhancing Diversity of Sampled Protein Conformations in Robotics-Inspired Methods

The ability to efficiently sample structurally diverse protein conformations allows one to gain a high-level view of a protein's energy landscape. Algorithms from robot motion planning have been used for conformational sampling, and several of these algorithms promote diversity by keeping track of "coverage" in conformational space based on the local sampling density. However, large proteins present special challenges. In particular, larger systems require running many concurrent instances of these algorithms, but these algorithms can quickly become memory intensive because they typically keep previously sampled conformations in memory to maintain coverage estimates. In addition, robotics-inspired algorithms depend on defining useful perturbation strategies for exploring the conformational space, which is a difficult task for large proteins because such systems are typically more constrained and exhibit complex motions. In this article, we introduce two methodologies for maintaining and enhancing diversity in robotics-inspired conformational sampling. The first method addresses algorithms based on coverage estimates and leverages the use of a low-dimensional projection to define a global coverage grid that maintains coverage across concurrent runs of sampling. The second method is an automatic definition of a perturbation strategy through readily available flexibility information derived from B-factors, secondary structure, and rigidity analysis. Our results show a significant increase in the diversity of the conformations sampled for proteins consisting of up to 500 residues when applied to a specific robotics-inspired algorithm for conformational sampling. The methodologies presented in this article may be vital components for the scalability of robotics-inspired approaches.

Deterministic sampling-based motion planning: Optimality, complexity, and performance

Probabilistic sampling-based algorithms, such as the probabilistic roadmap (PRM) and the rapidly exploring random tree (RRT) algorithms, represent one of the most successful approaches to robotic motion planning, due to their strong theoretical properties (in terms of probabilistic completeness or even asymptotic optimality) and remarkable practical performance. Such algorithms are probabilistic in that they compute a path by connecting independently and identically distributed (i.i.d.) random points in the configuration space. Their randomization aspect, however, makes several tasks challenging, including certification for safety-critical applications and use of offline computation to improve real-time execution. Hence, an important open question is whether similar (or better) theoretical guarantees and practical performance could be obtained by considering deterministic, as opposed to random, sampling sequences. The objective of this paper is to provide a rigorous answer to this question. Specifically, we first show that PRM, for a certain selection of tuning parameters and deterministic low-dispersion sampling sequences, is deterministically asymptotically optimal, in other words, it returns a path whose cost converges deterministically to the optimal one as the number of points goes to infinity. Second, we characterize the convergence rate, and we find that the factor of sub-optimality can be very explicitly upper-bounded in terms of the[Formula: see text] -dispersion of the sampling sequence and the connection radius of PRM. Third, we show that an asymptotically optimal version of PRM exists with computational and space complexity arbitrarily close to [Formula: see text] (the theoretical lower bound), where n is the number of points in the sequence. This is in contrast to the [Formula: see text] complexity results for existing asymptotically optimal probabilistic planners. Fourth, we discuss extending our theoretical results and insights to other batch-processing algorithms such as FMT*, to non-uniform sampling strategies, to k-nearest-neighbor implementations, and to differentially constrained problems. Importantly, our main theoretical tool is the [Formula: see text]-dispersion, an interesting consequence of which is that all our theoretical results also hold for low-[Formula: see text]-dispersion random sampling (which i.i.d. sampling does not satisfy). In other words, achieving deterministic guarantees is really a matter of i.i.d. sampling versus non-i.i.d. low-dispersion sampling (with deterministic sampling as a prominent case), as opposed to random versus deterministic. Finally, through numerical experiments, we show that planning with deterministic (or random) low-dispersion sampling generally provides superior performance in terms of path cost and success rate.

Probabilistic Roadmaps with Higher Expressive Power

Sampling-based motion planning had an enormous impact on robot motion planning because of its efficiency and scalability. Many sampling-based motion planners construct a probabilistic roadmap (PRM) that captures the connectivity of the robot's free configuration space. A valid node of a PRM contains a collision-free robot configuration (also known as a sample) and a valid edge of a PRM connects two valid nodes with a collision-free path. Nodes connected by an edge are usually also required to satisfy additional requirements based on the distance between them. PRM planners use PRMs. Increasing the expressive power will allow PRMs to be used for a wider set of motion planning problems. In this paper we report on increasing the expressive power of PRMs by including the following five features in PRMs-nodes with multiple samples that need not be organized as a graph, temporal intervals of validity of nodes and edges, nodes with samples of multiple robots, special edges for the state transitions performed by humans sharing a workspace with robots, and conditional validity of samples and edges. We report on motion planning problems solvable using these new features.

Feedback-motion-planning with simulation-based LQR-trees

The paper presents the simulation-based variant of the LQR-tree feedback-motion-planning approach. The algorithm generates a control policy that stabilizes a nonlinear dynamic system from a bounded set of initial conditions to a goal. This policy is represented by a tree of feedback-stabilized trajectories. The algorithm explores the bounded set with random state samples and, where needed, adds new trajectories to the tree using motion planning. Simultaneously, the algorithm approximates the funnel of a trajectory, which is the set of states that can be stabilized to the goal by the trajectory’s feedback policy. Generating a control policy that stabilizes the bounded set to the goal is equivalent to adding trajectories to the tree until their funnels cover the set. In previous work, funnels are approximated with sums-of-squares verification. Here, funnels are approximated by sampling and falsification by simulation, which allows the application to a broader range of systems and a straightforward enforcement of input and state constraints. A theoretical analysis shows that, in the long run, the algorithm tends to improve the coverage of the bounded set as well as the funnel approximations. Focusing on the practical application of the method, a detailed example implementation is given that is used to generate policies for two example systems. Simulation results support the theoretical findings, while experiments demonstrate the algorithm’s state-constraints capability, and applicability to highly-dynamic systems.

An incremental sampling-based approach to inspection planning: the rapidly exploring random tree of trees

A new algorithm, called rapidly exploring random tree of trees (RRTOT) is proposed, that aims to address the challenge of planning for autonomous structural inspection. Given a representation of a structure, a visibility model of an onboard sensor, an initial robot configuration and constraints, RRTOT computes inspection paths that provide full coverage. Sampling based techniques and a meta-tree structure consisting of multiple RRT* trees are employed to find admissible paths with decreasing cost. Using this approach, RRTOT does not suffer from the limitations of strategies that separate the inspection path planning problem into that of finding the minimum set of observation points and only afterwards compute the best possible path among them. Analysis is provided on the capability of RRTOT to find admissible solutions that, in the limit case, approach the optimal one. The algorithm is evaluated in both simulation and experimental studies. An unmanned rotorcraft equipped with a vision sensor was utilized as the experimental platform and validation of the achieved inspection properties was performed using3Dreconstruction techniques.

Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions

In this paper we present a novel probabilistic sampling-based motion planning algorithm called the Fast Marching Tree algorithm (FMT*). The algorithm is specifically aimed at solving complex motion planning problems in high-dimensional configuration spaces. This algorithm is proven to be asymptotically optimal and is shown to converge to an optimal solution faster than its state-of-the-art counterparts, chiefly PRM* and RRT*. The FMT* algorithm performs a "lazy" dynamic programming recursion on a predetermined number of probabilistically-drawn samples to grow a tree of paths, which moves steadily outward in cost-to-arrive space. As such, this algorithm combines features of both single-query algorithms (chiefly RRT) and multiple-query algorithms (chiefly PRM), and is reminiscent of the Fast Marching Method for the solution of Eikonal equations. As a departure from previous analysis approaches that are based on the notion of almost sure convergence, the FMT* algorithm is analyzed under the notion of convergence in probability: the extra mathematical flexibility of this approach allows for convergence rate bounds-the first in the field of optimal sampling-based motion planning. Specifically, for a certain selection of tuning parameters and configuration spaces, we obtain a convergence rate bound of order O(n^-1/d+ρ), where n is the number of sampled points, d is the dimension of the configuration space, and ρ is an arbitrarily small constant. We go on to demonstrate asymptotic optimality for a number of variations on FMT*, namely when the configuration space is sampled non-uniformly, when the cost is not arc length, and when connections are made based on the number of nearest neighbors instead of a fixed connection radius. Numerical experiments over a range of dimensions and obstacle configurations confirm our the-oretical and heuristic arguments by showing that FMT*, for a given execution time, returns substantially better solutions than either PRM* or RRT*, especially in high-dimensional configuration spaces and in scenarios where collision-checking is expensive.

A real-time motion planning algorithm for a hyper-redundant set of mechanisms

We introduce a novel probabilistic algorithm (CPRM) for real-time motion planning in the configuration space${\EuScript C}$. Our algorithm differs from a probabilistic road map (PRM) algorithm in the motion between a pair of anchoring points (local planner) which takes place on the boundary of the obstacle subspace${\EuScript O}$. We define a varying potential fieldfon ∂${\EuScript O}$as a Morse function and follow$\vec{\nabla} f$. We then exemplify our algorithm on a redundant worm climbing robot withndegrees of freedom and compare our algorithm running results with those of the PRM.

Sampling-based algorithms for optimal motion planning

During the last decade, sampling-based path planning algorithms, such as probabilistic roadmaps (PRM) and rapidly exploring random trees (RRT), have been shown to work well in practice and possess theoretical guarantees such as probabilistic completeness. However, little effort has been devoted to the formal analysis of the quality of the solution returned by such algorithms, e.g. as a function of the number of samples. The purpose of this paper is to fill this gap, by rigorously analyzing the asymptotic behavior of the cost of the solution returned by stochastic sampling-based algorithms as the number of samples increases. A number of negative results are provided, characterizing existing algorithms, e.g. showing that, under mild technical conditions, the cost of the solution returned by broadly used sampling-based algorithms converges almost surely to a non-optimal value. The main contribution of the paper is the introduction of new algorithms, namely, PRM* and RRT* , which are provably asymptotically optimal, i.e. such that the cost of the returned solution converges almost surely to the optimum. Moreover, it is shown that the computational complexity of the new algorithms is within a constant factor of that of their probabilistically complete (but not asymptotically optimal) counterparts. The analysis in this paper hinges on novel connections between stochastic sampling-based path planning algorithms and the theory of random geometric graphs.

Distributed Computation of the knn Graph for Large High-Dimensional Point Sets

High-dimensional problems arising from robot motion planning, biology, data mining, and geographic information systems often require the computation of k nearest neighbor (knn) graphs. The knn graph of a data set is obtained by connecting each point to its k closest points. As the research in the above-mentioned fields progressively addresses problems of unprecedented complexity, the demand for computing knn graphs based on arbitrary distance metrics and large high-dimensional data sets increases, exceeding resources available to a single machine. In this work we efficiently distribute the computation of knn graphs for clusters of processors with message passing. Extensions to our distributed framework include the computation of graphs based on other proximity queries, such as approximate knn or range queries. Our experiments show nearly linear speedup with over one hundred processors and indicate that similar speedup can be obtained with several hundred processors.