A Descent Algorithm for the Optimal Control of Constrained ... fileNeeds to handle arbitrary...

Motivation Problem Formulation Solving P1 Solving P2 Examples Conclusions

A Descent Algorithm for the Optimal Controlof Constrained Nonlinear Switched Dynamical

Systems

Humberto Gonzalez, Ram Vasudevan, Maryam Kamgarpour,Shankar Sastry, Ruzena Bajcsy, and Claire Tomlin

Frameworks and Tools for the High Confidence Design of AdaptiveDistributed Embedded Control Systems

AFOSR MURI Review, December 2nd 2009

UC Berkeley

Optimal Control of Hybrid Systems


1 Motivating Example

2 Problem Formulation

3 Solving P1

4 Solving P2

5 Examples

6 Conclusions

UC Berkeley



Scenario

Autonomous vehiclefollowing waypoints comesto intersection where car isnot moving.

One waypoint sits understopped car.

What would a human do?

What should anautonomous vehicle do?

UC Berkeley



What options do we have?

Logic based trajectoryplanner.

Fast Marching PotentialFunction algorithm.

Optimal Control?

UC Berkeley



Optimal Control

Needs to work in real-time (i.e. order of hunderds of [ms]).

Needs to handle arbitrary constraints (i.e. non-convex).

Needs to work on nonlinear systems.

In this project we developed an algorithm to solve an optimalcontrol problem for switched hybrid systems.

UC Berkeley





3 Solving P1

4 Solving P2

5 Examples

6 Conclusions

UC Berkeley



Definitions

Definition (Hybrid System)

A Hybrid System H is a collection (Q,X ,Σ0,V, f,Dom,R)defined as:

Q ={q1, q2, . . . , qNq

}the set of discrete states.

X = RN the set of continuous states.

Σ0 ⊂ X the set of initial states.

V = RNi the space of continuous control input variables.

f : Q×X × V → RN is the flow of the system.

Dom : Q → 2X×V is the domain as a function of thediscrete state.

R : Q×X × V → 2Q×X is the reset map relation.

UC Berkeley



Definitions

Definition (Jump Sequence)

A jump sequence, σ = (σ(k))Mk=1, is a finite subset of 2Q.

Definition (Jump Time Sequence)

A jump time sequence, s = (τ(k))Mk=0 ⊂ R+, is a finite setsatisfying τ(k) ≤ τ(k + 1) for all k = 0, . . . ,M − 1, τ(0) = 0,τ(M) <∞. The set of all jump time sequences will be denotedby S.

Input Function Space

Given a > 0 large enough, we assume input functions

u ∈ U ={f ∈ L2

(R+,RNi

)| ‖f(t)‖ ≤ a ∀t ≥ 0

}.

UC Berkeley



Problem Statement

Switched Hybrid System Optimal Control Problem

Given x0 ∈ Σ0, we want to solve:

minσ∈2Q

mins∈S,(uk)

Mk=1⊂U

∫ τ(M)

0

L(x(t), u(t)

)dt+

M∑k=1

φk(x(τ(k))

)subject to:

u(t) = uk(t− τ(k − 1)

), ∀t ∈

[τ(k − 1), τ(k)

), k = 1 . . .M

x(t) = f(σ(k), x(t), u(t)

), x(0) = x0, ∀t ∈

[τ(k − 1), τ(k)

), k = 1 . . .M

gj(x(t)

)≤ 0, ∀t ∈

[τ(0), τ(M)

], ∀j ∈ J

Time Free Formulation

UC Berkeley



Solving the Optimal Control Problem

Divide the problem into two nested optimization subproblems:

Problem 1 (P1)

Given an initial sequence σ ∈ 2Q, calculate the optimal s∗ ∈ Sand the optimal (u∗k)

Mk=1 ⊂ U that (locally) minimize the cost.

Note that at this point the cost can only be decreased by theinsertion of a new jump.

Problem 2 (P2)

Find a new sequence σ which is the result of the insertion of anew jump in the original sequence σ.

UC Berkeley



Solving the Optimal Control Problem

Iterate between the nested optimization problems P1 andP2 until we reach a local minimum, i.e. until we reach:

A sequence σ such that every insertion of a new mode(locally) increases the cost.A pair (s, u) that solves Problem P1.

The convergence of this algorithm is proved in the papersubmitted to HSCC 2010, Stockholm for the case of onlyone final cost function, i.e. φk ≡ 0 for eachk = 1, . . . ,M − 1.

UC Berkeley





3 Solving P1

4 Solving P2

5 Examples

6 Conclusions

UC Berkeley



Methodology

1 Assume the jump sequence σ ∈ 2Q is known.

2 Transform the optimization over jump times and inputs,into an optimization over initial conditions and inputs on afamily of M continuous systems.

3 Use any numerical method (with minimum guaranteesabout convergence) for optimal control.

For example, discretize the problem in time (e.g. ForwardEuler integration) and then use a nonlinear programmingsolver (e.g. SNOPT, NPSOL).

UC Berkeley



Time Free Formulation

Switched Hybrid System Optimal Control Problem

Given x0 ∈ Σ0 and σ ∈ 2Q, we want to solve:

mins∈S,(uk)

Mk=1⊂U

M∑k=1

(Jk(x(1)) + φk

(x(1)

))subject to:

xk(t) = rkf(σ(k), xk(t), uk(t)

), ∀t ∈ [0, 1], k = 1 . . .M

xk(0) = xk−1(1), k = 2, . . . ,M, x1(0) = x0

rk = τ(k)− τ(k − 1)

Jk(t) = rkL(xk(t), uk(t)), Jk(0) = 0, ∀t ∈ [0, 1], k = 1 . . .M

gj(xk(t)

)≤ 0, ∀t ∈ [0, 1], k = 1, . . . ,M, ∀j ∈ J

Original Formulation

UC Berkeley





3 Solving P1

4 Solving P2

5 Examples

6 Conclusions

UC Berkeley



Methodology

1 Assume the jump sequence σ ∈ 2Q, the jump times s∗ ∈ S,and the input u∗ ∈ U are known.

2 Cost can only be reduced by the insertion of a new jump.

3 Consider variations of the cost w.r.t. the insertion of a newmode α ∈ Q, at time t ∈ [0, τ(M)], with input u ∈ RNi , foran infinitesimal time.

4 Find the minimum value of the variation of the cost overall posible 3-tuples (α, t, u), and form new elements σ, s,and u by making an instantaneous insertion of (α, t, u) into(σ, s∗, u∗).

UC Berkeley



Optimal Mode Scheduling

Let p : R+ → Rn, p =∑Mk=1 pk, be the costate of the dynamical

system, where

pk(t) = −∂f∂x

(σ(k), x(t), u(t)

)pk(t)− ∂L

∂x

(x(t), u(t)

)pk(τ(k)) =

∂φk∂x

(x(τ(k)

)), k = 1, . . . ,M

then, given a 3-tuple η = (α, t, u), if we denote the mode at time t byβ, the variational of the cost is:

δJ (η) = pT (t)[f(α, x(t), u

)− f

(β, x(t), u(t)

)]+

+[u− u(t)

]T ∂L∂u

(x(t), u(t)

)and, if Φ is the STM of the costate, the variational of the constraintsis:

δg(η)j (t) =

∂gj∂x

(x(ξ)(t)

)Φ(t, t)

[f(α, x(t), u

)− f

(β, x(t), u(t)

)]UC Berkeley



Optimal Mode Scheduling

Hence, we will choose the 3-tuple to be inserted (α, t, u) as

(α, t, u) = arg minη=(α,t,u)

{δJ (η), ψ + δψ(η)

}where

ψ = maxt∈R+,j∈J

gj(x(t)

)and δψ(η) = max

{(t,j):gj(t)=ψ}δh

(η)j (t)

UC Berkeley





3 Solving P1

4 Solving P2

5 Examples

6 Conclusions

UC Berkeley



Simple Example, General Car Kinematics

Car Dynamics

x(t) = v(t) cos(ψ(t) + δ(t))

y(t) = v(t) sin(ψ(t) + δ(t))

ψ(t) =1

bv(t) sin(δ(t))

v(t) = a(t)

Cost Function

φk(x, y) =

∥∥∥∥(xy)− waypointk

∥∥∥∥2

2

Following two waypoints.

Only one mode.

No obstacles.

Just solve P1.

UC Berkeley



Simple Example, General Car Kinematics: P1

(τ(1), τ(2)) = (11.25, 22.08)

UC Berkeley



Simple Example, Forward/Turn Car Kinematics

Forward Car Dynamics: σ = 1

x(t) = v(t) cos(ψ(t))

y(t) = v(t) sin(ψ(t))

ψ(t) = 0

v(t) = a(t)

Cost Function

φk(x, y) =


∥∥∥∥2

2

Turn Car Dynamics: σ = 2

x(t) = v(t) cos(ψ(t) + δ(t))

y(t) = v(t) sin(ψ(t) + δ(t))

ψ(t) =1

bv(t) sin(δ(t))

v(t) = 0

Following two waypoints.

Two modes.

Solve hybrid optimalcontrol with forwardand turn dynamics.

UC Berkeley



Simple Example, Forward/Turn Car Kinematics: P1

(σ(1), σ(2)) = (1, 1)

(τ(1), τ(2)) = (10.868, 19.76)

UC Berkeley



Simple Example, Forward/Turn Car Kinematics:P1, P2, P1

(σ(1), σ(2), σ(3), σ(4)) = (1, 2, 1, 1)

(τ(1), τ(2), τ(3), τ(4)) = (6.008, 13.2, 13.21, 23.94)

UC Berkeley



Obstacles Example, General Car Kinematics

Car Dynamics

x(t) = v(t) cos(ψ(t) + δ(t))

y(t) = v(t) sin(ψ(t) + δ(t))

ψ(t) =1

bv(t) sin(δ(t))

v(t) = a(t)

Cost Function

φk(x, y) =


∥∥∥∥2

2

Following three waypoints.

Only one mode.

Street obstacles.

Just solve P1.

UC Berkeley



Obstacles Example, General Car Kinematics: P1

(τ(1), τ(2), τ(3)) = (25.01, 29.81, 39.74)

UC Berkeley



Obstacles Example, Forward/Turn Car Kinematics

Forward Car Dynamics: σ = 1

x(t) = v(t) cos(ψ(t))

y(t) = v(t) sin(ψ(t))

ψ(t) = 0

v(t) = a(t)

Cost Function

φk(x, y) =


∥∥∥∥2

2

Turn Car Dynamics: σ = 2

x(t) = v(t) cos(ψ(t) + δ(t))

y(t) = v(t) sin(ψ(t) + δ(t))

ψ(t) =1

bv(t) sin(δ(t))

v(t) = 0

Following three waypoints.

Two modes.

Street obstacles.

UC Berkeley



Simple Example, Forward/Turn Car Kinematics: P1

(σ(1), σ(2), σ(3)) = (1, 2, 1)

(τ(1), τ(2), τ(3)) = (25, 30, 40)

UC Berkeley



Simple Example, Forward/Turn Car Kinematics:P1, P2, P1

(σ(1), σ(2), σ(3), σ(4), σ(5)) = (1, 2, 1, 2, 1)

(τ(1), τ(2), τ(3), τ(4), τ(5)) =(12.46, 15.64, 25.77, 31.45, 40.8)

UC Berkeley



STARMAC Example

STARMAC Dynamics

x(t) =1

Msin(θ(t))

3∑k=1

Tk

z(t) =1

Mcos(θ(t))

3∑k=1

Tk − g

θ(t) =L

Iy(T1 − T3)

Cost Function

φk(x, z, θ) =1

2

((x− wx,k)2 + (z − wz,k)2 − 100 cos(θ − wθ,k)

)L(T1, T2, T3) =

1

200

3∑k=1

T 2k

UC Berkeley



STARMAC Example

UC Berkeley



STARMAC Example

Implementation Issue

As discussed in Claire Tomlin’s talk for practical reasons theinputs need to be constrained to be zero during the time thatSTARMAC is upside down. This needs to be worked into theformulation.

UC Berkeley





3 Solving P1

4 Solving P2

5 Examples

6 Conclusions

UC Berkeley



Conclusions

Future work:

Extension to robust optimal control using a game theoretic(min-max) formulation to account for disturbances.

Extension of proof to general hybrid systems, i.e. systemswith discrete inputs and discrete states.

Acknowledgements:

Thanks to the HCDDES MURI group and especiallyHaomiao Huang, Jeremy Gillula, and Michael Vitus forSTARMAC models.

Elijah Polak for the time free formulation.

UC Berkeley


A Descent Algorithm for the Optimal Control of Constrained ... fileNeeds to handle arbitrary...

Documents

Transcript of A Descent Algorithm for the Optimal Control of Constrained ... fileNeeds to handle arbitrary...