Optimal Path Planning on Matrix Lie Groups Mechanical Engineering and Applied Mechanics Sung K. Koh U n i v e r s i t y o f P e n n s y l v a n i a

Motivation Providing trajectories for airplanes near airports. Optimal path between given initial position, orientation, and final position, orientation to be made with a final time T. Optimal path is obtained by minimizing the cost which is the sum square of the inputs. Control tower problem : The airplane from some initial position and orientation is assigned a final position and orientation plus a final time.

U n i v e r s i t y o f P e n n s y l v a n i a Landing Tower Problem

U n i v e r s i t y o f P e n n s y l v a n i a Maximization of Hamiltonian

U n i v e r s i t y o f P e n n s y l v a n i a Co-state and Invariant Evolution of costate p Invariant

U n i v e r s i t y o f P e n n s y l v a n i a Example 1 : Path Planning on SE(2) Given that the car always dives forward at a fixed velocity, finding the steering controls so that the robot, starting from an initial position and orientation, arrives at some final goal position and orientation at a fixed time. Dynamics

U n i v e r s i t y o f P e n n s y l v a n i a Example 1 : Path Planning on SE(2)

U n i v e r s i t y o f P e n n s y l v a n i a Input dynamics

U n i v e r s i t y o f P e n n s y l v a n i a Example 2 : Path Planning on SO(3)

U n i v e r s i t y o f P e n n s y l v a n i a Example 2 : Path Planning on SO(3)

U n i v e r s i t y o f P e n n s y l v a n i a Example 2 : Path Planning on SO(3)

U n i v e r s i t y o f P e n n s y l v a n i a Example 2 : Path Planning on SO(3)

U n i v e r s i t y o f P e n n s y l v a n i a Conclusions The problems formulated as an optimal control problem of a left invariant control system on the Lie group are considered. Through the use of Pontryagin’s maximum principle and the techniques of numerical optimization, the solutions of problems on SE(2) and on SO(3) are presented.