Feedback synthesis for underactuated systems using sequential second-order needle variations

This paper derives nonlinear feedback control synthesis for general control affine systems using second-order actions, the second-order needle variations of optimal control, as the basis for choosing each control response to the current state. A second result of this paper is that the method provably exploits the nonlinear controllability of a system by virtue of an explicit dependence of the second-order needle variation on the Lie bracket between vector fields. As a result, each control decision necessarily decreases the objective when the system is nonlinearly controllable using first-order Lie brackets. Simulation results using a differential drive cart, an underactuated kinematic vehicle in three dimensions, and an underactuated dynamic model of an underwater vehicle demonstrate that the method finds control solutions when the first-order analysis is singular. Finally, the underactuated dynamic underwater vehicle model demonstrates convergence even in the presence of a velocity field.