Fast and adaptive fractal tree-based path planning for programmable bevel tip steerable needles

© 2016 IEEE. Steerable needles are a promising technology for minimally invasive surgery, as they can provide access to difficult to reach locations while avoiding delicate anatomical regions. However, due to the unpredictable tissue deformation associated with needle insertion and the complexity of many surgical scenarios, a real-time path planning algorithm with high update frequency would be advantageous. Real-time path planning for nonholonomic systems is commonly used in a broad variety of fields, ranging from aerospace to submarine navigation. In this letter, we propose to take advantage of the architecture of graphics processing units (GPUs) to apply fractal theory and thus parallelize real-time path planning computation. This novel approach, termed adaptive fractal trees (AFT), allows for the creation of a database of paths covering the entire domain, which are dense, invariant, procedurally produced, adaptable in size, and present a recursive structure. The generated cache of paths can in turn be analyzed in parallel to determine the most suitable path in a fraction of a second. The ability to cope with nonholonomic constraints, as well as constraints in the space of states of any complexity or number, is intrinsic to the AFT approach, rendering it highly versatile. Three-dimensional (3-D) simulations applied to needle steering in neurosurgery show that our approach can successfully compute paths in real-time, enabling complex brain navigation

Obstacle Avoidance Problem for Second Degree Nonholonomic Systems

In this paper, we propose a new control design scheme for solving the obstacle avoidance problem for nonlinear driftless control-affine systems. The class of systems under consideration satisfies controllability conditions with iterated Lie brackets up to the second order. The time-varying control strategy is defined explicitly in terms of the gradient of a potential function. It is shown that the limit behavior of the closed-loop system is characterized by the set of critical points of the potential function. The proposed control design method can be used under rather general assumptions on potential functions, and particular applications with navigation functions are illustrated by numerical examples.Comment: This is the author's accepted version of the paper to appear in: 2018 IEEE Conference on Decision and Control (CDC), (c) IEE

Nonholonomic motion planning: steering using sinusoids

Methods for steering systems with nonholonomic constraints between arbitrary configurations are investigated. Suboptimal trajectories are derived for systems that are not in canonical form. Systems in which it takes more than one level of bracketing to achieve controllability are considered. The trajectories use sinusoids at integrally related frequencies to achieve motion at a given bracketing level. A class of systems that can be steered using sinusoids (claimed systems) is defined. Conditions under which a class of two-input systems can be converted into this form are given

A Discrete Geometric Optimal Control Framework for Systems with Symmetries

This paper studies the optimal motion control of mechanical systems through a discrete geometric approach. At the core of our formulation is a discrete Lagrange-d’Alembert- Pontryagin variational principle, from which are derived discrete equations of motion that serve as constraints in our optimization framework. We apply this discrete mechanical approach to holonomic systems with symmetries and, as a result, geometric structure and motion invariants are preserved. We illustrate our method by computing optimal trajectories for a simple model of an air vehicle flying through a digital terrain elevation map, and point out some of the numerical benefits that ensue

Trajectory generation for the N-trailer problem using Goursat normal form

Develops the machinery of exterior differential forms, more particularly the Goursat normal form for a Pfaffian system, for solving nonholonomic motion planning problems, i.e., motion planning for systems with nonintegrable velocity constraints. The authors use this technique to solve the problem of steering a mobile robot with n trailers. The authors present an algorithm for finding a family of transformations which will convert the system of rolling constraints on the wheels of the robot with n trailers into the Goursat canonical form. Two of these transformations are studied in detail. The Goursat normal form for exterior differential systems is dual to the so-called chained-form for vector fields that has been studied previously. Consequently, the authors are able to give the state feedback law and change of coordinates to convert the N-trailer system into chained-form. Three methods for planning trajectories for chained-form systems using sinusoids, piecewise constants, and polynomials as inputs are presented. The motion planning strategy is therefore to first convert the N-trailer system into Goursat form, use this to find the chained-form coordinates, plan a path for the corresponding chained-form system, and then transform the resulting trajectory back into the original coordinates. Simulations and frames of movie animations of the N-trailer system for parallel parking and backing into a loading dock using this strategy are included

Path planning for simple wheeled robots : sub-Riemannian and elastic curves on SE(2)

This paper presents a motion planning method for a simple wheeled robot in two cases: (i) where translational and rotational speeds are arbitrary and (ii) where the robot is constrained to move forwards at unit speed. The motions are generated by formulating a constrained optimal control problem on the Special Euclidean group SE(2). An application of Pontryagin’s maximum principle for arbitrary speeds yields an optimal Hamiltonian which is completely integrable in terms of Jacobi elliptic functions. In the unit speed case, the rotational velocity is described in terms of elliptic integrals and the expression for the position reduced to quadratures. Reachable sets are defined in the arbitrary speed case and a numerical plot of the time-limited reachable sets presented for the unit speed case. The resulting analytical functions for the position and orientation of the robot can be parametrically optimised to match prescribed target states within the reachable sets. The method is shown to be easily adapted to obstacle avoidance for static obstacles in a known environment

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