perpetual collision-free receding horizon control of ...esteager/talk.pdf · equation, but better...

Introduction Formulation Collision Avoidance Example Conclusion

Perpetual Collision-Free Receding HorizonControl of Fleets of Vehicles

Humberto Gonzalez, Elijah Polak, and Shankar Sastry

SWARMSUniversity of California, Berkeley

Feb. 23th, 2010

SWARMS, UC Berkeley

Perpetual Collision-Free RHC


IntroductionMotivationRHC as centralized control

Problem FormulationDynamical Model of the VehiclesReceding Horizon Control

Perpetual Collision AvoidanceMain IdeaDefinitionsFinding rImplementation

ExampleDrones under Centralized Control

Conclusion

SWARMS, UC Berkeley



Introduction

• There are many applications where a centralized controlscheme is important:

Commercial planes flying to known destinations.

SWARMS, UC Berkeley



Introduction

• There are many applications where a centralized controlscheme is important:

Drones confined to fly in a bounded space for indefinite time.

SWARMS, UC Berkeley



Introduction

• Receding Horizon Control (RHC) hasbecome a very promising technique forthe centralized control of unmannedvehicles.

• Constraints and objectives are explicitlydefined, hence the design is transparent.

• Real-time deployment is not feasible ingeneral, but computers are faster andmore efficient (GPUs, parallel linearalgebra packages).

SWARMS, UC Berkeley



Introduction

• A key feature in centralized control systems of unmannedvehicles is to guarantee collision avoidance.

• However, RHC does not guarantee collision avoidanceautomatically.

• Adding constraints to avoid collisions between vehicles isnot enough, collision are avoided within the horizon ofcomputation.

We propose a simple condition to guarantee collision avoidancein centralized RHC.

SWARMS, UC Berkeley







Conclusion

SWARMS, UC Berkeley



Dynamical Model of the Vehicles

• Suppose we have Nv vehicles, each satisfying the followingdifferential equation:

xi(t) = hi(xi(t), ui(t)

), xi(0) = ζi

where hi : Rni × Rmi → Rni , ζi ∈ Rni , andui ∈ Ui =

{u ∈ L2(R+,Rmi) | ‖u(t)‖ ≤M

}.

• We will denote the Cartesian position of vehicle i byxiP : R→ Rd, where d = 2, 3.

• We assume Lipschitz continuity of the functions hi andtheir partial derivatives ∂hi

∂x and ∂hi∂u , for each i = 1, . . . , Nv.

SWARMS, UC Berkeley



Dynamical Model of the Vehicles

• We concatenate all the states and their dynamics into asingle dynamical system with state x, initial condition ζ,and input u, i.e.

n =Nv∑i=1

ni, m =Nv∑i=1

mi,

x(t) =

x1(t)...

xNv(t)

, u(t) =

u1(t)...

uNv(t)

, ζ =

ζ1...ζNv

,

h(x(t), u(t)

)=

h1

(x1(t), u1(t)

)...

hNv

(xNv(t), uNv(t)

)SWARMS, UC Berkeley



Receding Horizon Control

• Given an initial condition ζ ∈ Rn, define the OptimalControl Problem (OCPζ) by

(OCPζ) minu∈U

f0(ζ, u)

subject to: f j(ζ, u) ≤ 0, j = 1, . . . , q

• For example, we can choose:

f0(ζ, u) =∫ T

0

L(x(ζ,u)(t), u(t)

)dt+ φ

(x(ζ,u)(T )

)where x(ζ,u) : R+ → Rn is the unique solution of:

x(t) = h(x(t), u(t)

), x(0) = ζ

SWARMS, UC Berkeley




• The RHC law is defined as follows:1. Set tk = k∆.2. Measure the vehicle states x(tk), set ζ = x(tk).3. Solve the Optimal Control Problem (OCPζ) for the optimal

input u, and set the input of the vehicles u(t) on theinterval [tk, tk + ∆] as

u(t) = u(t− tk), t ∈ [tk, tk + ∆].

4. Apply the input during ∆ seconds. Then go back to step 1.

tk-1=(k-1)Δ tk=kΔ tk+1=(k+1)Δ Time

Apply uk-1 Apply uk

Solve OCP at t=tk Solve OCP at t=tk-1

SWARMS, UC Berkeley




• Note that adding constraints of the form:

‖xiP (t)− xjP (t)‖2 ≥ ρ2min, ∀i 6= j, ∀t ∈ [0,∆]

is not enough to guarantee the collision avoidance underRHC, because the problem (OCPζ) might not be feasiblein one of the iterations.

• If we can guarantee the feasibility of (OCPζ) for any initialcondition ζ in our domain of interest, then we will haveperpetual collision avoidance under our RHC law.

SWARMS, UC Berkeley







Conclusion

SWARMS, UC Berkeley



Main Idea

• If each pair of vehicles is far enough at time tk then theywill not collide for the next ∆ seconds. Call the minimumstarting distance that guarantees collision avoidance byr > 0.

• Make sure that the distance between pairs of vehicles is rat each tk = k∆, k ∈ N. Then feasibility is inductivelyguaranteed.

• This argument is known as recursive feasibility in theliterature.

SWARMS, UC Berkeley



Definitions

• Let S ⊂ Rn, a compact connected set, be the region ofinterest for the vehicles (for example, airspace around anairport).

• Given r > 0, let I(r) ⊂ Rn be the set of admissible initialconditions, defined by:

I(r) = {ζ = (ζ1, . . . , ζNv) ∈ S | ‖ζiP − ζjP ‖ ≥ r ∀i 6= j}

SWARMS, UC Berkeley



Definitions

• Consider the following constraints:

‖xiP (t)− xjP (t)‖2 ≥ ρ2min, ∀i 6= j, ∀t ∈ [0,∆],

‖xiP (∆)− xjP (∆)‖2 ≥ r2, ∀i 6= j,

(xi(∆))Nv

i=1 ∈ S.

(1)

Proposition

Suppose there exists r > ρmin such that for each ζ ∈ I(r) thereexists a feasible control u satisfying (1). Then, whenever theinitial state is in I(r), the RHC law will generate a control thatsatisfies (1) for all t ≥ 0.

SWARMS, UC Berkeley



Finding r

• In practice, we are interested in a method to compute avalue of r that satisfies the assumption in the previousproposition.

• We transcribed the assumption into a min-max-min-minproblem where we look for:

• The closest possible distance among planes,• over all initial conditions,• using feasible controls,• for all t ∈ [0,∆]• and all pairs of vehicles:

SWARMS, UC Berkeley



Finding r

• Let ψ : R+ → R+ be the closest possible distance amongvehicles, then

ψ(r) = minζ∈I(r)

maxu∈U∗(r)

mint∈[0,∆]

mini 6=j‖xiP (t)− xjP (t)‖2

where

U∗(r) ={u ∈ U | ‖xiP (∆)− xjP (∆)‖2 ≥ r2 ∀i 6= j,

(xi(∆))Nvi=1 ∈ S

}Proposition

If there exists r ≥ ρmin such that ψ(r) ≥ ρ2min then for each

ζ ∈ I(r) there exists a control u ∈ U that satisfies theconstraints (1).

SWARMS, UC Berkeley



Finding r

tk=kΔ tk+1=(k+1)Δ

r ψ(r) r

Time

Trajectory Airplane i

Trajectory Airplane j

SWARMS, UC Berkeley



Implementation

• We split ψ in two parts:• Solve the following max-min problem:

ψ(r, ζ) = maxu∈U

mint∈[0,∆]

mini6=j‖xiP (t)− xjP (t)‖

subject to: ‖xiP (∆)− xjP (∆)‖2 ≥ r2

(xi(∆))Nv

i=1 ∈ S

• Find ψ by minimizing ψ over ζ, i.e.

ψ(r) = minζ∈I(r)

ψ(r, ζ)

SWARMS, UC Berkeley



Implementation

• Given r > 0 and ζ ∈ I(r), ψ(r, ζ) can be computed using amin-max algorithm as Polak-He.

• We used a first order approximation for the differentialequation, but better discretization techniques can beapplied too.

• ψ has to be computed using a derivative-free algorithm,since it is not differentiable in general.

• We used Hooke-Jeeves algorithm, but once again betteroptions can be used.

SWARMS, UC Berkeley







Conclusion

SWARMS, UC Berkeley



Drones under Centralized Control

• Consider 4 drones flying in a disc of radius ρ, with speedlimited in the interval [13, 16] [m/s].

• We use a kinematic model for the drones:

xi(t) =

pxi(t)pyi(t)vi(t)θi(t)

, ui(t) =(ai(t)δi(t)

),

hi(xi(t), ui(t)

)=

vi(t) cos(θi(t))vi(t) sin(θi(t))

ai(t)δi(t)

• Hence

S ={

[px, py, v, θ]T ∈ R4 | ‖(px, py)‖ ≤ ρ, v ∈ [13, 16]}

SWARMS, UC Berkeley




• Parameters of the problem: ∆ = 7[s], ρ = 150[m],ρmin = 4[m].

• The general step in Hooke-Jeeves was chosen randomlywith uniform distribution.

• We used several optimizations, including parallelcomputation of the internal step in Hooke-Jeeves andε-active sets in the calculation of ψ.

• For each value of r, each computation took 7 hoursapproimately.

SWARMS, UC Berkeley




• Results for arbitrary values of r.r[m] ψ(r)[m]23 3.630625 4.365427 4.6679

SWARMS, UC Berkeley




• Worst-case scenario for r = 23[m].

SWARMS, UC Berkeley




• As a test for the RHC law with guaranteed collisionavoidance whenever the drones are 25 meters apart every 7seconds, we considered the following example.

SWARMS, UC Berkeley







Conclusion

SWARMS, UC Berkeley



Conclusion

• We proposed a method to guarantee collision avoidanceunder a centralized RHC law. The method is flexibleenough to not disturbe the design of the controller, sincethe objective function is left untouched and aditionalconstraints can be added.

• Most of the computations are done offline (i.e. thecalculation of ψ(r)).

• The journal paper was accepted in the Journal ofOptimization Theory and Applications.

SWARMS, UC Berkeley



Questions?

SWARMS, UC Berkeley


perpetual collision-free receding horizon control of ...esteager/talk.pdf · equation, but better...

Documents