Combining Homotopy Methods and Numerical Optimal Control to Solve Motion Planning Problems

This paper presents a systematic approach for computing local solutions to motion planning problems in non-convex environments using numerical optimal control techniques. It extends the range of use of state-of-the-art numerical optimal control tools to problem classes where these tools have previously not been applicable. Today these problems are typically solved using motion planners based on randomized or graph search. The general principle is to define a homotopy that perturbs, or preferably relaxes, the original problem to an easily solved problem. By combining a Sequential Quadratic Programming (SQP) method with a homotopy approach that gradually transforms the problem from a relaxed one to the original one, practically relevant locally optimal solutions to the motion planning problem can be computed. The approach is demonstrated in motion planning problems in challenging 2D and 3D environments, where the presented method significantly outperforms a state-of-the-art open-source optimizing sampled-based planner commonly used as benchmark

Path Planning for Concentric Tube Robots: a Toolchain with Application to Stereotactic Neurosurgery

We present a toolchain for solving path planning problems for concentric tube robots through obstacle fields. First, ellipsoidal sets representing the target area and obstacles are constructed from labelled point clouds. Then, the nonlinear and highly nonconvex optimal control problem is solved by introducing a homotopy on the obstacle positions where at one extreme of the parameter the obstacles are removed from the operating space, and at the other extreme they are located at their intended positions. We present a detailed example (with more than a thousand obstacles) from stereotactic neurosurgery with real-world data obtained from labelled MPRI scans.Comment: 8 pages, 7 figures. Paper under revie

Batch Informed Trees (BIT*): Informed Asymptotically Optimal Anytime Search

Path planning in robotics often requires finding high-quality solutions to continuously valued and/or high-dimensional problems. These problems are challenging and most planning algorithms instead solve simplified approximations. Popular approximations include graphs and random samples, as respectively used by informed graph-based searches and anytime sampling-based planners. Informed graph-based searches, such as A*, traditionally use heuristics to search a priori graphs in order of potential solution quality. This makes their search efficient but leaves their performance dependent on the chosen approximation. If its resolution is too low then they may not find a (suitable) solution but if it is too high then they may take a prohibitively long time to do so. Anytime sampling-based planners, such as RRT*, traditionally use random sampling to approximate the problem domain incrementally. This allows them to increase resolution until a suitable solution is found but makes their search dependent on the order of approximation. Arbitrary sequences of random samples approximate the problem domain in every direction simultaneously and but may be prohibitively inefficient at containing a solution. This paper unifies and extends these two approaches to develop Batch Informed Trees (BIT*), an informed, anytime sampling-based planner. BIT* solves continuous path planning problems efficiently by using sampling and heuristics to alternately approximate and search the problem domain. Its search is ordered by potential solution quality, as in A*, and its approximation improves indefinitely with additional computational time, as in RRT*. It is shown analytically to be almost-surely asymptotically optimal and experimentally to outperform existing sampling-based planners, especially on high-dimensional planning problems.Comment: International Journal of Robotics Research (IJRR). 32 Pages. 16 Figure

A New Mixed Iterative Algorithm to Solve the Fuel-Optimal Linear Impulsive Rendezvous Problem

International audienceThe optimal fuel impulsive time-fixed rendezvous problem is reviewed. In a linear setting, it may be reformulated as a non convex polynomial optimization problem for a pre-specified fixed number of velocity increments. Relying on variational results previously published in the literature, an improved mixed iterative algorithm is defined to address the issue of optimization over the number of impulses. Revisiting the primer vector theory, it combines variational tests with sophisticated numerical tools from algebraic geometry to solve polynomial necessary and sufficient conditions of optimality. Numerical examples under circular and elliptic assumptions show that this algorithm is efficient and can be integrated into a rendezvous planning tool