Geometric structure-preserving optimal control of the rigid body

Anthony M. Bloch, Islam I. Hussein, Melvin Leok, Amit K. Sanyal

Abstract

This paper presents a discrete variational optimal control framework for rigid body dynamics. By minimizing external torque through a discretization of the Lagrange–d’Alembert principle rather than the equations of motion, the authors develop a method that preserves the geometric properties of the rotation group $SO(3)$. The approach utilizes a Lie-algebraic formulation to ensure flow remains on the manifold, providing superior long-term stability and accuracy compared to traditional numerical methods.

Introduction

In fields such as aerospace engineering and underwater robotics, the ability to precisely control the orientation of a rigid body is paramount. Whether a satellite is performing a multi-month imaging mission or a drone is navigating complex environments, the underlying mathematical space—the Special Orthogonal group $SO(3)$—imposes strict geometric constraints.

Traditional numerical methods often struggle to maintain these constraints over long durations. Standard solvers typically "drift" off the $SO(3)$ manifold, leading to non-physical results where rotation matrices lose their orthogonality. Furthermore, these methods often fail to conserve fundamental physical quantities like energy and momentum, which is critical for high-fidelity mission planning and long-term stability.

The Limitations of Traditional Discretization

Standard approaches to optimal control typically involve taking the continuous equations of motion (Euler-Lagrange equations) and applying general-purpose numerical solvers like Runge-Kutta or collocation methods. While effective for short-term simulations, these methods do not "know" about the underlying geometry of the state space. Over time, numerical errors accumulate, causing the simulation to lose its physical validity.

A Discrete Variational Approach

The authors propose a shift in perspective: Discrete Variational Optimal Control. The core innovation lies in discretizing the variational principle (the Lagrange–d’Alembert principle) itself, rather than the resulting differential equations. Within a discrete-time setting, these two approaches are not generally equivalent, and discretizing the principle yields superior results.

Key Methodological Pillars

  • Discrete Lagrange–d’Alembert Principle: By starting with the variational principle, the resulting discrete equations of motion naturally inherit the conservation properties of the physical system.
  • Lie Group Variational Integrators: The kinematics are handled via a Lie-algebraic formulation. By using the exponential map to update orientations, the algorithm ensures that every computed state remains exactly on $SO(3)$ without the need for ad-hoc re-normalization.
  • Symplectic Euler and Effective Order: The researchers employ a first-order Lie symplectic Euler scheme. A significant finding is that this method is "symplectically conjugate" to the second-order Störmer–Verlet method. This allows the solver to achieve second-order accuracy while maintaining the low computational overhead of a first-order approach.
  • High-Precision Optimization: To solve the necessary conditions for optimality, the team utilized MATLAB’s fsolve combined with complex-step differentiation. This technique provides Jacobians with near-analytic precision, avoiding the truncation errors inherent in standard finite-difference methods.

Numerical Experiments and Results

The framework was validated through two primary scenarios:

SO(2) Verification

To confirm the theoretical convergence rates, the authors first tested a simple planar rigid body. The results demonstrated that numerical errors in angular position and velocity decreased predictably as the time-step size was reduced, confirming the "effective order" of the integrator and the reliability of the discrete-time necessary conditions.

3D Asymmetric Rigid Body Maneuvers

The method was then applied to a complex 3D "rest-to-rest" maneuver for a spacecraft with a non-symmetric inertia tensor. In these simulations, the structure-preserving integrator successfully captured the intricate coupling between axes. For instance, a commanded rotation about the x-axis correctly generated the necessary torques in the y and z axes to counteract centrifugal effects—nuances that are often lost or distorted in non-geometric simulations.

Discussion and Insights

The primary takeaway is that the principle matters more than the equation. Discretizing the variational principle preserves the "physical DNA" of the system. Additionally, because the method operates directly on rotation matrices and Lie algebras, it is $SO(3)$-equivariant. This coordinate independence ensures that the solution remains consistent regardless of the chosen reference frame, eliminating common issues associated with Euler angles (like gimbal lock) or quaternions (like double-covering).

Future Directions

While the current work focuses on $SO(3)$, the authors identify several avenues for expansion:

  • Time-Reversal Symmetry: Future iterations aim to implement a fully reversible Lie Störmer–Verlet algorithm to further improve long-term stability.
  • Expansion to SE(3): Generalizing the framework to the Special Euclidean group $SE(3)$ would allow for the simultaneous control of rotation and translation, essential for docking and landing maneuvers.
  • Non-Compact Groups: Exploring the stability of these methods on non-compact Lie groups remains an open mathematical challenge for future research.

Summary updated

Abstract

In this paper we study a discrete variational optimal control problem for the rigid body. The cost to be minimized is the external torque applied to move the rigid body from an initial condition to a pre-specified terminal condition. Instead of discretizing the equations of motion, we use the discrete equations obtained from the discrete Lagrange--d'Alembert principle, a process that better approximates the equations of motion. Within the discrete-time setting, these two approaches are not equivalent in general. The kinematics are discretized using a natural Lie-algebraic formulation that guarantees that the flow remains on the Lie group SO(3) and its algebra so(3). We use Lagrange's method for constrained problems in the calculus of variations to derive the discrete-time necessary conditions. We give a numerical example for a three-dimensional rigid body maneuver.
BibTeX
@article{bloch2009geometric,
  author = {Anthony M. Bloch and Islam I. Hussein and Melvin Leok and Amit K. Sanyal},
  title = {{Geometric structure-preserving optimal control of the rigid body}},
  journal = {Journal of Dynamical and Control Systems},
  year = {2009},
  doi = {10.1007/s10883-009-9071-2},
}
Figure