Efficient kinematic planning for mobile manipulators with non-holonomic constraints using optimal control

This work addresses the problem of kinematic trajectory planning for mobile manipulators with non-holonomic constraints, and holonomic operational-space tracking constraints. We obtain whole-body trajectories and time-varying kinematic feedback controllers by solving a Constrained Sequential Linear Quadratic Optimal Control problem. The employed algorithm features high efficiency through a continuous-time formulation that benefits from adaptive step-size integrators and through linear complexity in the number of integration steps. In a first application example, we solve kinematic trajectory planning problems for a 26 DoF wheeled robot. In a second example, we apply Constrained SLQ to a real-world mobile manipulator in a receding-horizon optimal control fashion, where we obtain optimal controllers and plans at rates up to 100 Hz.