Cooperative Collision Avoidance via Proximal Message Passing

Cooperative Collision Avoidance viaProximal Message Passing

Hao Yi Ong and J. Christian Gerdes

Stanford University

July 3, 2015

Outline

Introduction

Mathematical formulation

Algorithm

Numerical experiments

Conclusion and future work

Introduction 2

Cooperative collision avoidance (CCA)

I multivehicle problem

– stage-cost function (fuel, trajectory deviation, etc.)– linearized dynamics– shared collision avoidance constraints

Introduction 3

Complications

I practically

– vehicles has limited network information, sensing, actuation, andcomputation capabilities

– dynamic network: changing number of vehicles and obstacles

I mathematically

– CCA as mathematical program is nonconvex– large problem size (>100 vehicles)– efficient algorithm needed for online implementation

Introduction 4

Goals

application-agnostic, robust method for

I arbitrary-scale optimization

– dynamic control of large-scale network

I decentralized optimization

– vehicles/devices coordinate to solve large problem by passingrelatively small messages

I model predictive control (MPC)

– fast enough for real-time use on embedded systems

Introduction 5

Previous approaches

I mixed-integer programming [M+12a]

I distributed algorithm

– satellite swarms [M+12b]– cooperative robots [B+13]

I former works well for problem with few vehicles, while lattercapitalizes on application-specific problem structures

Introduction 6

Proximal message passing

I builds on network energy management by Kraning et al. [K+13]

– convex objective and constraints only– finite horizon problem– offline solution

I extensions handle

– nonconvexity– infinite horizon problem– real-time application

Introduction 7

Proximal message passing

I decentralized method to solve CCA for real-time MPC applications

– each vehicle plans its own state trajectory and control inputs– vehicles coordinate via simple positional message exchanges with

neighbors– “plug-and-play” concept for more/less vehicles and obstacles

I can handle enormous problems

Introduction 8

Outline

Introduction


Algorithm



Mathematical formulation 9

Network model

I network consists of vehicles V, communication links E , and nets W

I nets represent communication relays or transponders

Link Net node

Vehicle

Obstacle

V: v1 v2 v3 v4 v5 v6 v7 v8

N : w1 w2 w3

E :


Vehicles

I models vehicles with linearizable dynamics

I variables are state-control pairs (xv, uv) ∈ RnT × RmT

I objective function fv (xv, uv) : RnT × RmT → R ∪ {+∞}

– fuel consumption, trajectory tracking, smooth trajectory,...– +∞ used to encode vehicle constraints


Nets

I models collision warning and guidance software with transponder

I objective function µw (xVw) : Rn|Vw|T → R ∪ {+∞}

I nonconvex constraints of the form

‖pi,τ − pj,τ‖2 ≥ D

– multiple balls to cover forbidden region around vehicle– can also use minimum covering ellipsoid or any other shape


Cooperative collision avoidance problem

cooperative collision avoidance problem (CCA):

minimize∑v∈V

fv (xv, uv) +∑w∈W

µw (xVw)

I variables are vehicle state-control pairs (x, u) ∈ R|V|×nT × R|V|×mT

I linear dynamics and other constraints are contained in vehicleobjectives fv

I nonconvex collision avoidance constraints are contained in netobjectives µw


Outline

Introduction


Algorithm



Algorithm 14

Convex-concave procedure (CCP)

I express problem with ‘difference of convex’ functions

minimize f0(x)− g0(x)subject to fi(x)− gi(x) ≤ 0, i = 1, . . . ,m

where fi and gi are convex

I iterative procedure: convexify nonconvex parts at kth iteration byreplacing g(x) with

g(x) = g(x(k)

)+∇g

(x(k)

)T (x− x(k)

)

Algorithm 15

Convex-concave procedure

I collision avoidance constraint function is a “negative ball”

D − ‖pi,τ − pj,τ‖2 ≤ 0

I first term is constant (convex); second term is concave

I linearize second term in objective to get halfspace

D − gT (pi,τ − pj,τ ) ≤ 0,

where

g =pki,τ − pkj,τ∥∥pki,τ − pkj,τ

∥∥2

Algorithm 16

Convex-concave procedure

intuition: ball for forbidden region is approximated by a halfspacetangent to the ball

I in each iteration, vehicle position is optimized within halfspaceconstraint and halfspace approximation is updated thereafter

ObstacleAllowable

region

k = 1

Obstacle

Allowable region

k = 2

Obstacle

Allowable region

k = 3

Algorithm 17

Problem decomposition

convexified CCA:

minimize∑v∈V


gkw (xVw)

issues:

I problem dimension is still large

I decomposition needs to allow vehicle and obstacle plug-and-play

Algorithm 18

Consensus optimization

idea: decompose each iteration of convexified CCA into small,manageable subproblems using ADMM [B+10]

convexified CCA in consensus form:

minimize∑v∈V


gkw (xVw)

subject to x = x

I fv and gkw encode vehicle objectives and convexified net constraintsin the kth iteration of CCP

I x = x are consensus constraints that couple solutions toindependent fv and gkw minimizations

Algorithm 19

Proximal message passing algorithm

I define “price” term to be running sum of inconsistencies:

y := y + (x− x)

I repeat until convergence:

1. prox controller update: in parallel, minimize each vehicle’s fv plusvehicle’s price component yv

2. avoidance projection update: in parallel, minimize each net’s gkwplus net’s price component yw

3. price update: in parallel, update price with the difference betweenprox controller and avoidance projection solutions

Algorithm 20


repeat until convergence:

1. prox controller update(xκ+1v , uκ+1

v

):= argmin

(xv,uv)

(fv (xv , uv) + (ρ/2) ‖xv − (xκ

v − yκv )‖22

)in parallel, for each vehicle(ρ > 0; RHS is proximal operator of fv at xκ

v − yκv )

2. avoidance projection update

xκ+1Vw

:= argminxVw

(gκw (xVw ) + (ρ/2)

∥∥xVw −(xκVw

+ yκVw

)∥∥22

)in parallel, for each net(ρ > 0; RHS is projection operator onto convexified constraints at xκ

Vw+ yκVw

)

3. scaled price updateyκ+1Vw

:= yκVw+

(xκVw

− xκVw

)in parallel, for each net

Algorithm 21

Convergence

for proximal message passing objective functions just has to be convexclosed proper (can be nondifferentiable, take on +∞ values, . . . )

I residual convergence: x → x (trajectories deconflicted)

I objective convergence: f (xκ, uκ) + g (xκ) → f?0

(controls are locally optimal)

I price convergence: yκ → y? (optimal prices are found)

Algorithm 22

Example

vehicles compute new tentative trajectories

V: v1 v2 v3 v4 v5 v6 v7 v8

N : w1 w2 w3

E :

Algorithm 23

Example

vehicles send tentative trajectories to neighboring nets

V: v1 v2 v3 v4 v5 v6 v7 v8

N : w1 w2 w3

E :

Algorithm 24

Example

nets deconflict trajectories; update prices

V: v1 v2 v3 v4 v5 v6 v7 v8

N : w1 w2 w3

E :

Algorithm 25

Example

nets send updated prices, trajectories to neighboring vehicles

V: v1 v2 v3 v4 v5 v6 v7 v8

N : w1 w2 w3

E :

Algorithm 26

Example

repeat

V: v1 v2 v3 v4 v5 v6 v7 v8

N : w1 w2 w3

E :

Algorithm 27


I each vehicle and net only has knowledge of its own objective function

I for each vehicle type, need to implement own minimizer

– can have different types of vehicles with different dynamics,constraints (motorcycle, cars,...)

I all message passing is local, between vehicles and adjacent nets

I no global coordination other than iteration synchronization

Algorithm 28

Outline

Introduction


Algorithm



Numerical experiments 29

Vehicle model

simple kinematic bicycle model of the car

px = ν cos (θ) , py = ν sin (θ) , θ =ν

Ltan (θ)

δ

( px , py ) θ

L

x

y


Model predictive control

consider MPC algorithm

minimize∑

v∈V(t)

(λ

T−2∑τ=0

(δv,τ+1 − 2δv,τ + δv,τ−1)2

+

T∑τ=1

∥∥pvehτ − pvehτ

∥∥22

)+

∑w∈W(t)

µw (xVw)

subject to xv,0 = xinitv ,

xv,τ+1 = Av,τxv,τ +Bv,τδv,τ + cv,τ ,∀v ∈ V (t) , τ = 0, . . . , T − 1,

I solves CCA only for the next T discrete time steps

I locally optimizes for smooth steering and reference tracking

I repeatedly executes first input of control sequence


Model predictive control

transform into familiar convexified CCA in consensus form

minimize∑v∈V


gkw (xVw)

I fv locally optimizes for smooth steering and reference tracking andencodes vehicle dynamics constraints

I gkw encodes convexified constraints at the kth CCP iteration


Network topology and simulation

I number of vehicles ranges from 5 to 500

I number of nets ranges from 0 to 200

I MPC: 4 sec time horizon, 0.1 sec intervals

I network topology (initial positions, reference trajectories, obstaclelocations) chosen randomly

– vehicles linked to nets if in close proximity


Example: Proximal message passing


Example: 10 vehicles

−200 −100 0 100 200−250

−200

−150

−100

−50

0

50

100

150

200

250

x−position (m)

y−

po

siti

on

(m

)

car

start

obstacle


Serial single-thread implementation

I use CVXGEN to generate custom C optimization code

I examples run on a 2.7 Ghz Intel i7 with 8 GB of RAM

I objective values at convergence of message passing algorithm always<1% suboptimal when compared with CVX for small network sizesfor each (centralized) CCP iteration


Solve time scaling (serial)

100

101

102

103

10−2

10−1

100

101

102

103

network size (number of cars)

solv

e ti

me

per

sec

ond o

f si

mu

lati

on

I fit exponent is 1.145; solve time scales as O(|V|1.145

)I with fully decentralized computation, average of 35 ms per MPC

iteration for any network size


Outline

Introduction


Algorithm



Conclusion and future work 38

Summary

I developed a completely decentralized local optimization method forguidance and control of multivehicle networks

I decentralized computation allows for millisecond solve timesindependent of network size

I when combined with MPC, can be used for real-time networkoperation

I envision a robust plug-and-play system for any vehicle types


Future work

I robust, asynchronous message passing algorithm

– work with unreliable communication or network protocols

I more sophisticated vehicle models

I shared constraints beyond collision avoidance


References

I S. Boyd et al.Distributed optimization and statistical learning via the alternatingdirection method of multipliers.Foundations and Trends in Optimization, 1(3):1–122, 2010.

I J. Bento et al.A message-passing algorithm for multi-agent trajectory planning.In Advances in Neural Information Processing Systems, pages521–529, 2013.

I M. Kraning et al.Dynamic network energy management via proximal message passing.Foundations and Trends in Optimization, 1(2):70–122, 2013.

41

References

I D. Mellinger et al.Mixed-integer quadratic program trajectory generation forheterogeneous quadrotor teams.In IEEE Conf. on Robotics and Automation, pages 477–483, May 2012.

I D. Morgan et al.Model predictive control of swarms of spacecraft using sequentialconvex programming.In AIAA/AAS Astrodynamics Specialist Conf., Aug. 2012.

42

Cooperative Collision Avoidance via Proximal Message Passing

Engineering

Transcript of Cooperative Collision Avoidance via Proximal Message Passing