perpetual collision-free receding horizon control of ...esteager/talk.pdf · equation, but better...
TRANSCRIPT
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
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Introduction
• There are many applications where a centralized controlscheme is important:
Commercial planes flying to known destinations.
SWARMS, UC Berkeley
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Receding Horizon Control
• 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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Receding Horizon Control
• 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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Finding r
tk=kΔ tk+1=(k+1)Δ
r ψ(r) r
Time
Trajectory Airplane i
Trajectory Airplane j
SWARMS, UC Berkeley
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Drones under Centralized Control
• 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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Drones under Centralized Control
• Results for arbitrary values of r.r[m] ψ(r)[m]23 3.630625 4.365427 4.6679
SWARMS, UC Berkeley
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Drones under Centralized Control
• Worst-case scenario for r = 23[m].
SWARMS, UC Berkeley
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Drones under Centralized Control
• 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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
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
Perpetual Collision-Free RHC
Introduction Formulation Collision Avoidance Example Conclusion
Questions?
SWARMS, UC Berkeley
Perpetual Collision-Free RHC