Lie Algebraic Cost Function Design for Control on Lie Groups

Abstract

This paper reports a novel result: with proper robot models on matrix Lie groups, one can formulate the kinodynamic motion planning problem for rigid body systems as exact polynomial optimization problems that can be relaxed as semidefinite programming (SDP). Due to the nonlinear rigid body dynamics, the motion planning problem for rigid body systems is nonconvex. Existing global optimization-based methods do not properly deal with the configuration space of the 3D rigid body; thus, they do not scale well to long-horizon planning problems. We use Lie groups as the configuration space in our formulation and apply the variational integrator to formulate the forced rigid body systems as quadratic polynomials. Then we leverage Lasserre's hierarchy to obtain the globally optimal solution via SDP. By constructing the motion planning problem in a sparse manner, the results show that the proposed algorithm has linear complexity with respect to the planning horizon. This paper demonstrates the proposed method can provide rank-one optimal solutions at relaxation order two for most of the testing cases of 1) 3D drone landing using the full dynamics model and 2) inverse kinematics for serial manipulators.

Convex Geometric Motion Planning on Lie Groups via Moment Relaxation

“…Such property is an improvement compared to other global optimization methods, such as combinatorial optimization with exponential complexity. To improve the scalability for realtime deployment, combining the global convergence property of SDP [76] and fast local search on Lie groups [77][78][79][80][81], should be considered in the future.…”

An Adaptive MPC Trajectory Tracking Algorithm For Autonomous Vehicles

“…Since we now have a linear model for the error dynamics, we proceed with the linearization of the hydrodynamics described in (16). The linearization is performed around the operating point ξ:…”

“…Another promising approach to achieving computational efficiency is geometric control, based on the Lie group framework, to exploit existing symmetry in the problem [16], [17]. Unlike methods that rely on approximations of the hydrodynamics model, geometric control leverages the intrinsic geom-etry of the system and represents the dynamics in an invariant and symmetric manner.…”