Discrete-Time Differential Dynamic Programming on SO(3) With Pose Constraints

The motion of many robotics systems, such as the rotation of unmanned aerial vehicles, can be modeled by SO(3). However, the difficulties in the parameterization of the SO(3) makes it hard to implement the state-of-the-art collision avoidance algorithm. In this paper, we present a new method for control on SO(3) via the combination of the nonlinear state constraints and geometric control technique. We first define the nonlinear constraints for trajectory optimization on SO(3). Then we solve the trajectory optimization problem by constrained Differential Dynamic Programming (DDP) in a Riemannian geometry framework. The first and second-order expansion of the cost function and dynamics are derived in a coordinate-free manner. The safety condition represented by nonlinear constraints is incorporated into the DDP via an active set method, where we find the active set in the backward path to obtain the optimal control policy. We validated our methods on motion planning of rigid model on SO(3) manifold where pose constraints are imposed. Our methods outperform the baseline methods in terms of convergence speed and numerical robustness to disturbance. The numerical robustness is essential when the system is initialized far from the local optimum.