Optimization-based Robot Control and State Estimation on Matrix Lie Groups

Abstract

Motion planning, feedback control, and state estimation of robots often require optimizations subject to the constraints induced by rigid body motions. As the rigid body motion is nonlinear and evolves on non-Euclidean space, these constraints are usually nonconvex, which makes it challenging for numerical optimization. Though general-purpose nonlinear programming solver can be applied to solve these problems, three challenges are not properly addressed: 1) lack of guarantees on feasibility and global optimality for motion planning with full rigid body dynamics; 2) absence of fast gradient-based solver that respects the geometric and topological structure of the configurations space of rigid bodies; 3) Kalman Filter type state estimation still relies on Gaussian assumption and cannot asymptotically represent arbitrary belief. To address all these problems, we first leverage the Lie Group Variational Integrator (LGVI) to formulate the dynamics of rigid bodies on matrix Lie groups. Then, we leverage the algebraic structure of the matrix Lie group for global optimization and the geometric structure for fast gradient-based optimization.

To address the first challenge, we propose to bridge the polynomial structure of the special orthogonal group for 3D rotation and convex optimization. The key finding is that the special orthogonal group is quadratically constrained. Thus, the motion planning problem of rigid bodies modeled by LGVI can be relaxed as the Semi-Definite Programming via the moment relaxation technique. We show that the second-order moment relaxation is sufficient to solve a wide range of motion planning problems with global optimality or non-trivial lower bound, including inverse kinematics of serial manipulators, motion planning of 3D drones, and cart-poles. We also extend the algorithm to a rigid body modeled on quaternion, which provides tight first-order relaxation for the motion planning of 3D drones in cluttered environments.

To address the second challenge, we propose to apply the second-order Riemannian nonlinear optimizations on the matrix Lie group for direct trajectory optimizations of rigid bodies. The crux of the proposed method is to preserve the manifold structure of discrete time rigid body motions by LGVI. Then, we derive the analytical Riemannian gradients of the dynamics leveraging the second-order retraction map induced by the invariant metric on matrix Lie groups. Finally, we apply a customized filter line-search Riemannian Interior Point Method to synthesize motions for rigid bodies. We showcase the proposed algorithm as being more efficient than the methods in the ambient space and correct-by-construction to respect the geometric and topological structure of the configuration space of rigid bodies.

To address the third challenge, we propose the Generalized Moment Kalman Filter (GMKF) that iteratively updates and propagates the belief of polynomial systems represented by Moment Constrained Max Entropy Distribution (MCMED). By increasing the number of moment constraints, the MCMED can asymptotically approximate any moment-determinant distribution. In the prediction step, we leverage the polynomial dynamics to predict the moments and recover the distribution via convex optimization, which avoids the marginalization step that is generally intractable. In the update steps, we solve the Maximum A Posterior estimation with belief represented by MCMED. We showcase the power of GMKF in challenging robotics tasks, such as robot localization with unknown data association.

Together, this thesis provides a comprehensive set of tools for certifiable, efficient, and geometrically consistent motion planning and state estimation in rigid body robotic systems.