Local planning using switching time optimization

Switching-time optimization has applications in local motion planning using the geometry of the nonlinear vector fields that govern the control system. In this paper, we present an algorithm for computing the second derivative of a switching-time cost function that enables second-order numerical optimization techniques that often converge quickly compared to first-order only algorithms. The resulting algorithms (for both first and second derivatives) each require only a single integration along the time horizon, yielding excellent computational performance. We present an example that uses this method to do local motion planning for a parallel parking maneuver for a kinematic car using the infinitesimal Lie bracket expansion that is used to demonstrate controllability. This same expansion allows one to construct a sequence of motions and approximate switching times that can then be used in the switching time optimization for a finite (non-infinitesimal) motion.