Covariance Steering as a Tool for Planning and Control in the - PowerPoint PPT Presentation

Covariance Steering as a Tool for Planning and Control in the Presence of Uncertainty Panagiotis Tsiotras School of Aerospace Engineering Institute for Robotics and Intelligent Machines Georgia Institute of Technology Uncertainty Synthesis Workshop, CDC 2019 December 10, 2019

Moving Densities

Gaussian Case: Steering the Covariance

Example: Powered Descend Guidance

Problem Formulation Given the discrete-time stochastic linear system x k +1 = A k x k + B k u k + D k w k Initial and final states to be distributed according to x 0 ∼ N ( µ 0 , Σ 0 ) , x N ∼ N ( µ f , Σ f ) with µ 0 , Σ 0 , µ f , Σ f given. Minimize the cost function � N − 1 � � u ⊤ J ( u 0 , . . . , u N − 1 ) = E k u k → min k =0

The system state at step k + 1 is given by x k +1 = A k x 0 + B k U k + D k W k . where     u 0 w 0     u 1 w 1     U k =  , W k =  .   .  . .    . . u k w k and where A k � A k, 0 , B k � B k, 0 , D k � D k, 0 � � B k 1 ,k 0 � B k 1 ,k 0 B k 1 ,k 0 +1 · · · B k 1 ,k 1 , � � D k 1 ,k 0 � D k 1 ,k 0 D k 1 ,k 0 +1 · · · D k 1 ,k 1 , A k 1 ,k 0 = A k 1 A k 1 − 1 · · · A k 0 , B k 1 ,k 0 = A k 1 ,k 0 +1 B k 0 , D k 1 ,k 0 = A k 1 ,k 0 +1 D k 0

Let A = A N − 1 , B = B N − 1 , D = D N − 1 and     u 0 w 0     u 1 w 1     U = U N − 1 =    , W = W N − 1 =   . . . .    . . u N − 1 w N − 1 then x N = A x 0 + B U + D W

The mean of the state µ k = E [ x k ] obeys the expression µ k +1 = A k µ 0 + B k U k where U k = E [ U k ] . Let � U k � U k − U k , x k � x k − µ k , � It follows that x 0 + B k � � x k +1 = A k � U k + D k W k . � � � � U ⊤ �� U ⊤ U = U ⊤ U U � � J ( U ) = E + tr . E ���� � �� � J µ J Σ

Steering the Mean Main Result The optimal control U ⋆ that minimizes the cost N − 1 � ⊤ U = E [ u k ] ⊤ E [ u k ] J µ = U k =0 subject to the constraint A µ 0 + B U = µ f is given by U ⋆ = B ⊤ ( BB ⊤ ) − 1 ( µ f − A µ 0 )

Diffusionless Case ( D k = 0 ) Theorem (Goldshtein and Tsiotras, 2017) Let V 0 S 0 V ⊤ V F S F V ⊤ 0 = Σ 0 , F = Σ f , and 1 1 − 1 A V 0 S 0 U Ω S Ω V ⊤ 2 V ⊤ F ( BB ⊤ ) Ω � S F 2 . Then the optimal control gain L that minimizes J Σ subject to a constraint Σ N = Σ f , is given by 1 − 1 − 1 ( V F S F L ⋆ = B ⊤ ( BB ⊤ ) 2 U Ω V ⊤ 2 V ⊤ Ω S 0 0 − A ) Control is of the form � U = L � x 0 ,

General Case ( D k � = 0 ) Key Observation The system x k +1 = A k x k + B k u k + D k w k at time step N can be viewed as a sum of N uncorrelated � ⊤ � x ( i ) k x ( j ) = 0 , k, m, i, j ∈ { 0 , . . . , N } , i � = j, E m diffusion-less sub-systems N − 1 � x ( i ) x N = N + Dw N − 1 , i =0 � x 0 , for i = 0 , x ( i ) k +1 = A k x ( i ) k + B k u ( i ) x ( i ) k , = i D i − 1 w i − 1 , otherwise .

Optimal Controller � L ( i ) x ( i ) i , i = 1 , . . . , N − 1 , U ( i ) i,N − 1 = L (0) x 0 + E [ U ] , i = 0 . where   u ( i ) k 1   u ( i )   U ( i ) k 1 +1   k 1 ,k 2 � , 0 ≤ k 1 ≤ k 2 ≤ N − 1 . .   .  .  u ( i ) k 2

Optimal Controller Assume Σ 0 � 0 and Σ f � 0 , let y 0 = x 0 − µ 0 , and define y k = D k − 1 w k − 1 = x k − ( A k − 1 x k − 1 + B k − 1 u k − 1 ) − 1 A N,k , with Λ = Λ ⊤ be the solution of the matrix equation Let Φ k = ( I + B N,k B ⊤ N,k Λ) N − 1 � Φ k D k − 1 D ⊤ k − 1 Φ ⊤ k + Φ 0 Σ 0 Φ ⊤ 0 = Σ f − D N − 1 D ⊤ N − 1 � 0 k =1 The optimal linear control law is given by k � L ( i ) u ⋆ k = B ⊤ N,k ( BB ⊤ ) − 1 ( µ f − A µ 0 ) + k y i i =0 where, L ( i ) k = − B ⊤ N,k ΛΦ i .

Relation with LQG Theorem (Goldshtein and Tsiotras, 2017; Chen et al, 2016) Let initial and final state covariance matrices Σ 0 and Σ f and symmetric matrix Q f . Assume that the LQG controller that minimizes the cost function � N − 1 � � u ⊤ k u k + x ⊤ J ( u 0 , . . . , u N − 1 ) = E N Q f x N , k =0 results in the final state covariance being equal to Σ f . Then, this controller coincides is the same as the covariance steering controller with boundary constraints x 0 ∼ N (0 , Σ 0 ) , x N ∼ N (0 , Σ f ) , with Λ = Q f .

6000 6000 4000 4000 11 12 2000 σ 2 x σ 2 x 2000 0 0 -2000 0 50 100 0 50 100 6000 6000 CC 4000 LQG 4000 21 22 2000 σ 2 x σ 2 x 2000 0 -2000 0 0 50 100 0 50 100 Steps

General Cost Consider discrete-time stochastic linear system x k +1 = A k x k + B k u k + D k w k We wish the initial and final states to be distributed according to x 0 ∼ N ( µ 0 , Σ 0 ) , x N ∼ N ( µ N , Σ N ) where µ 0 , Σ 0 , µ N , Σ N given, while minimizing the cost function � N − 1 � � x ⊤ k Q k x k + u ⊤ J ( x, u ) = E k R k u k k =0 where Q k � 0 and R k ≻ 0 for all k = 0 , 1 , . . . , N − 1 . Assume that Σ 0 � 0 and Σ N ≻ 0 ,

N ] ⊤ to write Introduce augmented state X = [ x ⊤ 0 , x ⊤ 1 , . . . , x ⊤ X = A x 0 + B U + D W cost � � X ⊤ ¯ QX + U ⊤ ¯ J ( X, U ) = E RU boundary conditions � E [ XX ⊤ ] − E [ X ] E [ X ] ⊤ � E ⊤ µ 0 = E 0 E [ X ] , Σ 0 = E 0 0 � E [ XX ⊤ ] − E [ X ] E [ X ] ⊤ � E ⊤ µ N = E N E [ X ] , Σ N = E N N where E k is a matrix such that x k = E k X, k = 0 , 1 , . . . , N A ← E N A , B ← E N B , D ← E N D Note:

Let the control sequence u k = v k + K k y k where y k is given by y k +1 = A k y k + D k w k y 0 = x 0 − µ 0 and let the control law U = V + KY

Theorem (Okamoto & PT, 2018) The cost function takes the form � � (( I + B K ) ⊤ ¯ Q ( I + B K ) + K ⊤ ¯ RK )( A Σ 0 A ⊤ + DD ⊤ ) J ( V , K ) = tr + ( A µ 0 + B V ) ⊤ ¯ Q ( A µ 0 + B V ) + V ⊤ ¯ RV In addition, the terminal state constraints can be written as µ N = E N ( A µ 0 + B V ) , Σ N = E N ( I + B K )( A Σ 0 A ⊤ + DD ⊤ )( I + B K ) ⊤ E ⊤ N Note that V steers the mean and K steers the covariance, respectively. Letting Σ N � E N ( I + B K )( A Σ 0 A ⊤ + DD ⊤ )( I + B K ) ⊤ E ⊤ N yields a convex problem .

Can handle convex chance constraints of the form ∈ χ ) ≤ P fail , k = 0 , . . . , N − 1 Pr ( x k / where M � { x : α ⊤ χ = j x ≤ β j } j =1 using the standard trick   M  β j − a ⊤ � j x  ≥ 1 − p j , Pr ( α ⊤ j x ≤ β j ) = Φ � p j ≤ P fail α ⊤ j Σ x α j j =1 or � α ⊤ j Σ x α j Φ − 1 (1 − p j ) ≤ 0 α ⊤ j x − β j + where Φ is the cumulative distribution function of the standard normal distribution.

Assuming M � Pr ( α ⊤ j X > β j ) ≤ p j, fail p j, fail ≤ P fail j =1 the chance constraint can be formulated as j ( A µ 0 + B V ) + � ( A Σ 0 A ⊤ + DD ⊤ ) 1 / 2 ( I + B K ) ⊤ α j � Φ − 1 (1 − p j, fail ) − β j ≤ 0 α ⊤ Second order cone (convex) constraint in K and V .

Example 09

Non-Convex Constraints For non-convex polytopic constraints, write N R − 1 M r − 1 � � { x : α ⊤ χ = r,q x ≤ β r,q } r =0 q =0 � �� � R r and enforce Pr ( x k / ∈ R r ) < ǫ and Pr ( x k +1 / ∈ R r ) < ǫ Lemma Given R r , the condition ∈ R r ) < ǫ ∈ R r ) < ǫ, Pr ( x k / and Pr ( x k +1 / is a second-order cone constraint in V and K .

Example

Example

MPC ( Model Predictive Control: Classical, Robust and Stochastic , B. Kouvaritakis and M. Cannon )

Stochastic MPC

Stochastic MPC � k + N − 1 � � x ⊤ t | k Qx t | k + u ⊤ u k | k ,...,u k + N − 1 | k J N ( µ k , Σ k ; u k | k , . . . , u k + N − 1 | k ) = E k min t | k Ru t | k + J f ( x k + N | k ) t = k subject to x t +1 | k = Ax t | k + Bu t | k + Dw t , x k | k = x k ∼ N ( µ k , Σ k ) � � α ⊤ Pr k x,i x t | k ≤ β x,i ≥ 1 − p x,i , i = 0 , . . . , N s − 1 � � α ⊤ Pr k u,j u t | k ≤ β u,j ≥ 1 − p u,j , j = 0 , . . . , N c − 1

Stochastic MPC u k | k ,u k +1 | k ,...,u k + N − 1 | k J N ( x k ; u k | k , u k +1 | k , . . . , u k + N − 1 | k ) = min � k + N − 1 � � x ⊤ t | k Qx t | k + u ⊤ + E k [ x k + N | k ] ⊤ P mean E k [ x k + N | k ] t | k Ru t | k E k t = k subject to x t +1 | k = Ax t | k + Bu t | k + Dw t , x k | k = x k ∼ N ( µ k , Σ k ) � � α ⊤ Pr k x,i x t | k ≤ β x,i ≥ 1 − p x,i , i = 0 , . . . , N s − 1 � � α ⊤ u,j u t | k ≤ β u,j ≥ 1 − p u,j , j = 0 , . . . , N c − 1 Pr k � � ∈ X µ x k + N | k E k f � ( x k + N | k − E [ x k + N | k ])( x k + N | k − E [ x k + N | k ]) ⊤ � � Σ f E k