A Projection Operator Approach to Solving Optimal Control Problems on Lie Groups

Transcription

1 A Projection Operator Approach to Solving Optimal Control Problems on Lie Groups Alessandro Saccon LARSyS, Instituto Superior Técnico (IST), Technical University of Lisbon (UTL) Multiple Vehicle Motion Planning, Navigation, and Control - Theory and Practice IFAC 18th World Congress, Milan, Italy, 28 August, 2011

2 Introduction

3 Minimization of Trajectory Functionals Let G be a Lie group (think to, e.g., the set SO(3) of rotational matrices). We are interested in finding numerical algorithms for minimizing a functional h(g( ),u( )) := T over the set T of trajectories of the nonlinear system with g(0) = g 0 G. 0 l(g(τ),u(τ),τ) dτ +m(g(t)) ġ(t) = f(g(t),u(t)) 3 / 25

4 Minimization of Trajectory Functionals To solve this type of problems, the Lie group projection operator approach was introduced in [1]. The method is the extension to Lie groups of the projection operator approach for optimization of trajectory functionals developed in [2]. State and input constraints can be handled with a barrier functional approach [3]. [1] Saccon, A., Hauser, J., and Aguiar, A., Optimal control on non-compact Lie Groups: A Projection Operator Approach, CDC 2010, Atlanta, Georgia [2] Hauser, J., A projection operator approach to the optimization of trajectory functionals, 15th IFAC World Congress 2002, Barcelona, Spain. [3] Hauser, J., Saccon, A., A Barrier Function Method for the Optimization of Trajectory Functionals with Constraints CDC 2006, San Diego, CA, USA 4 / 25

5 Minimization of Trajectory Functionals The strategy is being developed for minimum-energy trajectory planning of multiple autonomous underwater vehicles with obstacle and collision avoidance. 5 / 25

6 Lie groups: a short review

7 Lie groups A Lie group is a smooth manifold endowed with a group structure. The group operation must be smooth. A generic Lie group is denoted by G. Typical examples are the groups SO(3), SE(2), SE(3), and U(n)......but also TSO(3), TSE(2), TSE(3) are Lie groups! The so called tangent groups. Therefore, the theory we have developed may be applied to the optimal control of mechanical systems on Lie groups 7 / 25

8 Lie groups (cont d) Left and right translations of x G (a group element) by the group element g G that is L g x and R g x, will be denoted using the shorthand notation gx, xg, gv x, v x g which stands for L g x, R g x, T x L g (v x ), T x R g (v x ) The Lie algebra g of G is the tangent space T e G endowed with the Lie bracket operation [, ] : g g g, defined by [,ς] := [X,X ς ](e), where the later bracket is the Jacobi-Lie bracket evaluated at the group identity and X is the left-invariant vector field generated by g. For matrix Lie groups, [A,B] = AB BA. 8 / 25

9 The Projection Operator Approach

10 The projection operator approach Key facts The set of trajectories of a nonlinear control system is an infinite dimensional Banach manifold T The minimization of the trajectory functional h over the set T of trajectories of the nonlinear system f, i.e., subject to min u( ) T 0 l(g(τ),u(τ),τ) dτ +m(g(t)) ġ(t) = f(g(t),u(t)), g(0) = g 0 G, can be thought as a constrained optimization problem min h(ξ) ξ T the state-control curve ξ(t) = (α(t),µ(t)) G R m, t 0 being constrained to stay on T. At each trajectory ξ T, it is attached a tangent space T ξ T. A tangent vectors ξζ T ξ T is a trajectory of the linearization of the nonlinear control system about ξ. 10 / 25

11 The projection operator approach the (Lie group) Projection Operator P maps state-control curves into trajectories P : ξ η where ξ(t) = (α(t),µ(t)) G R m and η(t) = (g(t),u(t)) G R m, t 0. P is defined as the trajectory tracking control law ġ = f(g,k(g,ξ,t)) u = k(g,ξ,t) = µ+k(t)[log(g 1 α)], g(0) = α(0) For G = (R n,+), we get the classical trajectory tracking since log(g 1 α) = α g. P is a (nonlinear) projection: it satisfies P 2 = P. 11 / 25

12 Projection operator Newton method Key idea: the optimization problems min ξ T h(ξ) and min ξ h(p(ξ)) are essentially equivalent. The advantage of using the cost functional h := h P is that the second problem is unconstrained. The projection operator approach constructs and minimize a second order approximation of the h around the current trajectory ξ i at each iteration. The minimization is performed on the tangent space. given initial trajectory ξ 0 T for i = 0, 1, 2,... descent direction ζ i = arg min Dh(ξ i) ξ i ζ + 1 ξ i ζ T ξi T 2 D2 h(ξi ) (ξ i ζ,ξ i ζ) (LQ) end line search γ i = arg min γ (0,1] h(p(ξ iexp(γζ i ))) update ξ i+1 = P(ξ i exp(γ i ζ i )) 12 / 25

13 A graphical representation of the projection operator approach Using a finite dimensional analogy, the three main algorithmic steps that form the projection operator approach can be represented as follows. Trajectory manifold Descent direction Line search Update 13 / 25

14 Left-trivialized linearization Consider ġ(t) = g(t)λ(g(t),u(t)) and perturb the input as u(t)+εv(t) we get g ǫ (t) = g(t)exp(εz(t)+o(ε)) where z(t) g is with ż = A(η(t))z +B(η(t))v A(η(t)) = D 1 λ(g(t),u(t)) TL g(t) ad λ(g(t),u(t)) B(η(t)) = D 2 λ(g(t),u(t)) Example: Then λ(r,ω) = ω and A(R,ω) = ω = B(R,ω) = I Ṙ(t) = R(t)ω (t) 0 ω z ω y ω z 0 ω x ω y ω x 0 14 / 25

15 Derivatives The Lie group Projection Operator requires second order geometry on smooth manifolds Indeed, one needs to expand h = h P about a trajectory ξ as h(ξexp(εζ)) = h(p(ξ))+ εd h(ξ) ξζ +1/2ε 2 D 2 h(ξ) (ξζ,ξζ) +o(ε 2 ) where D represent covariant differentiation. For ξ T and ξζ i T ξ T, one shows that D h(ξ) ξζ = Dh(ξ) ξζ D 2 h(ξ) (ξζ1,ξζ 2 ) = D 2 h(ξ) (ξζ 1,ξζ 2 )+Dh(ξ) D 2 P(ξ) (ξζ 1,ξζ 2 ) How can we compute D 2 P(ξ) (ξζ 1,ξζ 2 )? Recall that P is a operator that maps state-control curves intro state-control trajectories. Note that D 2 P(ξ) is a tangent vector at ξ! 15 / 25

16 Linearization of the Projection Operator Vector Space Lie Group Curve ξ = (α,µ) R n R m G R m Perturbation ζ = (β,ν) R n R m g R m Trajectory η = (g,u) R n R m G R m Traj. perturbation γ = (z,v) R n R m g R m Vector space R n P(ξ +εζ) = η +εγ +o(ε). We obtain ż = A(η(t))z +B(η(t))v, z(0) = 0 v = ν +K(t)(β z) Lie group G P(ξexp(εζ)) = P(ξ)exp(εγ +o(ε)). We obtain, recall P(ξ) = η, ż = A(η(t))z +B(η(t))v, z(0) = 0 v = ν +K(t)dlog log(g 1 α)(ad g 1 αβ z) 16 / 25

17 Linearization of the Projection Operator Vector Space Lie Group Curve ξ = (α,µ) R n R m G R m Perturbation ζ = (β,ν) R n R m g R m Trajectory η = (g,u) R n R m G R m Traj. perturbation γ = (z,v) R n R m g R m Vector space R n P(ξ +εζ) = η +εγ +o(ε). We obtain ż = A(η(t))z +B(η(t))v, z(0) = 0 v = ν +K(t)(β z) Lie group G P(ξexp(εζ)) = P(ξ)exp(εγ +o(ε)). We obtain, recall P(ξ) = η, ż = A(η(t))z +B(η(t))v, z(0) = 0 v = ν +K(t)dlog log(g 1 α)(ad g 1 αβ z) When ξ = P(ξ) = η, dlog log(g 1 α) = id and Ad g 1 α = id! 16 / 25

18 Second order approximation of the Projection Operator The second order approximation of the Projection Operator is P(ξexp(εζ)) = P(ξ)exp(εγ +1/2ε 2 ω +o(ε 2 )) with γ the left-trivialized linearization of P shown in the previous slide. In [1], we show that DP 2 (ξ) (ξζ 1,ξζ 2 ) = P(ξ)ω where ω is a curve in the Lie algebra g R m which, for ξ T, may be computed as ẏ = A(η)y +B(η)w y(0) = 0, +1/2 ( ad z1 Dλ(ξ) ξγ 2 +ad z2 Dλ(ξ) ξγ 1 ) +D 2 λ(η) (ηγ 1,ηγ 2 ), w = K(t) [ y +1/2 ( [z 1,β 2 ]+[z 2,β 1 ] )] with P(ξ)γ i = DP(ξ) ξζ i and γ i = (z i,v i ), ζ i = (β i,ν i ). [1] Saccon, A., Hauser, J., and Aguiar, A., Optimal control on non-compact Lie Groups: A Projection Operator Approach, CDC 2010, Atlanta, Georgia 17 / 25

19 Implementation details In [4], we show that the minimization of D h(ξ) ξζ +1/2D 2 h(ξ) (ξζ,ξζ) over ξζ T ξ T amounts in solving the linear quadratic optimal control problem T min (z,v)( ) 0 a(τ) T z(τ)+b(τ) T v(τ)+ 1 [ ] T [ ] z(τ) z(τ) W(τ) dτ +a T 2 v(τ) v(τ) 1z(T)+ 1 2 z(t)t P 1 z(t), subject to the dynamic constraint ż(t) = A(ξ(t))z(t)+B(ξ(t))v(t), z(0) = 0. The vectors a(τ), b(τ), and a 1 (τ) are obtained by direct differentiation of the incremental cost l and terminal cost m. The matrix W(τ) depends on second covariant derivative of l and a term depending on D 2 P. No time for details... [4] Optimal Control on Lie Groups: Implementation Details of the Projection Operator Approach, Proceedings of the 18th IFAC World Congress, Milan, Italy, / 25

20 Second covariant derivative of a mapping M 1 and M 2 smooth manifolds endowed with affine connections 1 and 2 1 P and 2 P the parallel displacements associated to 1 and 2 f : M 1 M 2 a smooth mapping The second covariant derivative of a mapping allows to apply the classical (Leibniz s) product rule to the covariant derivative of the product Df(γ 1 (t)) V 1 (t), with γ 1 a curve and V 1 a vector field along γ 1 in M 1. We get D t (Df(γ 1 (t)) V 1 (t)) = D 2 f(γ 1 (t)) (V 1 (t), γ 1 (t))+df(γ 1 (t)) D t V 1 (t) One shows that D 2 f(γ 1 (t)) (V 1 (t), γ 1 (t)) equals lim ε 0 1 ( 2 P t t+ε γ ε 2 ) Df(γ 1 (t+ε)) 1P t+ε t γ 1 V 1 (t) Df(γ 1 (t)) V 1 (t) We have specialized these concept to Lie groups, using the symmetric (0)-Cartan-Shouten connection. One needs to understand covariant derivative of two-point tensors! 19 / 25

21 A numerical example

22 Numerical example We have tested [5] the algorithm on different problems on SO(3) and TSO(3) to verify its effectiveness and second order convergence rate. min u( ) 1 2 T 0 I R T d(τ)r 2 Q R + ω d (τ) ω 2 Q ω + u d(τ) u 2 Rdτ I RT f R 2 P R f ω f ω 2 P ω f. subject to Ṙ = Rˆω R(0) = g 0 SO(3) Iω = (Iω) ω +Cu, ω(0) = w 0 R 3 with M 2 P := tr(m T PM), the Frobenius weighted matrix norm. Note that, for R SO(3), I R P has a local minimum at R = I. [5] A. Saccon, A.P. Aguiar, J. Hauser, Lie group Projection Operator Approach: Optimal Control on TSO(3), in the Proceeding of the 50th Conference on Decision and Control (CDC) and European Control Conference (ECC), Orlando, Florida, / 25

23 Numerical Example (cont d) Attitude attitude Angular velocity angular velocity Torque log 10 Dh(ξ k ) ξ k ζ k torque k (iteration number) / 25

24 Conclusions

25 Conclusions We have outlined the main ideas that defines the projection operator based trajectory optimization approach for nonlinear systems that evolve on Lie groups. This requires the understanding of the covariant derivative for the repeated differentiation of a mapping between two Lie groups, endowed with affine connections. With this tool, chain rule like formulas where used to develop expressions for the basic objects needed for trajectory optimization. The algorithm has been already coded and tested on SO(3) and TSO(3). Much more to do... Possible practical applications: constrained path planning aerial, ground, and underwater vehicles, robotics, quantum mechanics, / 25