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
fsolvecombined 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