Lie Algebraic Cost Function Design for Control on Lie Groups

TL;DR

This paper introduces a novel control framework on Lie groups by designing cost functions in the Lie algebra, resulting in globally exponential convergence and improved trajectory optimization efficiency.

Contribution

It proposes a Lie algebra-based cost function with a left-invariant metric that guarantees exponential convergence and enhances control and trajectory optimization on Lie groups.

Findings

Quadratic Lyapunov function ensures global exponential convergence.

Controller maintains exponential rate even near $ ext{error} o ext{pi}$ in SO(3).

Faster convergence of iLQR over DDP with the proposed cost function.

Abstract

This paper presents a control framework on Lie groups by designing the control objective in its Lie algebra. Control on Lie groups is challenging due to its nonlinear nature and difficulties in system parameterization. Existing methods to design the control objective on a Lie group and then derive the gradient for controller design are non-trivial and can result in slow convergence in tracking control. We show that with a proper left-invariant metric, setting the gradient of the cost function as the tracking error in the Lie algebra leads to a quadratic Lyapunov function that enables globally exponential convergence. In the PD control case, we show that our controller can maintain an exponential convergence rate even when the initial error is approaching $\pi$ in SO(3). We also show the merit of this proposed framework in trajectory optimization. The proposed cost function enables the iterative Linear Quadratic Regulator (iLQR) to converge much faster than the Differential Dynamic Programming (DDP) with a well-adopted cost function when the initial trajectory is poorly initialized on SO(3).