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

Abstract

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.

Trajectory Optimization on Matrix Lie Groups with Differential Dynamic Programming and Nonlinear Constraints

“…Both of these approaches [13], [18] formulate the trajectory optimization on matrix Lie groups in an unconstrained framework. In order to address this limitation, Liu et al [14] extended the work [13] by imposing SO(3) pose constraints. However, this method is not generalizable to nonlinear constraints for generic matrix Lie groups.…”

“…1) Development of an augmented Lagrangian based constrained DDP algorithm for trajectory optimization on matrix Lie groups. 2) A principled approach for nonlinear constraint handling for generic matrix Lie groups unlike [14], which only addressed constraint handling for SO(3) pose constraints. 3) Evaluating the effectiveness of the proposed DDP method in handling external disturbances through its application in a numerical simulation as a Lie-algebraic feedback control policy on SE(3).…”