Successfully reported this slideshow.
We use your LinkedIn profile and activity data to personalize ads and to show you more relevant ads. You can change your ad preferences anytime.

Cooperative Collision Avoidance via Proximal Message Passing


Published on

We propose a distributed model predictive controller to cooperatively solve the collision avoidance problem for a set of vehicles. The method, called proximal message passing, is completely decentralized and needs no global coordination other than synchronizing iterations, allowing us to solve the avoidance problem extremely efficiently and in parallel.

Published in: Engineering
  • Be the first to comment

Cooperative Collision Avoidance via Proximal Message Passing

  1. 1. Cooperative Collision Avoidance via Proximal Message Passing Hao Yi Ong and J. Christian Gerdes Stanford University July 3, 2015
  2. 2. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Introduction 2
  3. 3. Cooperative collision avoidance (CCA) multivehicle problem – stage-cost function (fuel, trajectory deviation, etc.) – linearized dynamics – shared collision avoidance constraints Introduction 3
  4. 4. Complications practically – vehicles has limited network information, sensing, actuation, and computation capabilities – dynamic network: changing number of vehicles and obstacles mathematically – CCA as mathematical program is nonconvex – large problem size (>100 vehicles) – efficient algorithm needed for online implementation Introduction 4
  5. 5. Goals application-agnostic, robust method for arbitrary-scale optimization – dynamic control of large-scale network decentralized optimization – vehicles/devices coordinate to solve large problem by passing relatively small messages model predictive control (MPC) – fast enough for real-time use on embedded systems Introduction 5
  6. 6. Previous approaches mixed-integer programming [M+ 12a] distributed algorithm – satellite swarms [M+ 12b] – cooperative robots [B+ 13] former works well for problem with few vehicles, while latter capitalizes on application-specific problem structures Introduction 6
  7. 7. Proximal message passing builds on network energy management by Kraning et al. [K+ 13] – convex objective and constraints only – finite horizon problem – offline solution extensions handle – nonconvexity – infinite horizon problem – real-time application Introduction 7
  8. 8. Proximal message passing 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 can handle enormous problems Introduction 8
  9. 9. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Mathematical formulation 9
  10. 10. Network model network consists of vehicles V, communication links E, and nets W 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: Mathematical formulation 10
  11. 11. Vehicles models vehicles with linearizable dynamics variables are state-control pairs (xv, uv) ∈ RnT × RmT objective function fv (xv, uv) : RnT × RmT → R ∪ {+∞} – fuel consumption, trajectory tracking, smooth trajectory,... – +∞ used to encode vehicle constraints Mathematical formulation 11
  12. 12. Nets models collision warning and guidance software with transponder objective function µw (xVw ) : Rn|Vw|T → R ∪ {+∞} 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 Mathematical formulation 12
  13. 13. Cooperative collision avoidance problem cooperative collision avoidance problem (CCA): minimize v∈V fv (xv, uv) + w∈W µw (xVw ) variables are vehicle state-control pairs (x, u) ∈ R|V|×nT × R|V|×mT linear dynamics and other constraints are contained in vehicle objectives fv nonconvex collision avoidance constraints are contained in net objectives µw Mathematical formulation 13
  14. 14. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Algorithm 14
  15. 15. Convex-concave procedure (CCP) 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 iterative procedure: convexify nonconvex parts at kth iteration by replacing g(x) with ˆg(x) = g x(k) + g x(k) T x − x(k) Algorithm 15
  16. 16. Convex-concave procedure collision avoidance constraint function is a “negative ball” D − pi,τ − pj,τ 2 ≤ 0 first term is constant (convex); second term is concave linearize second term in objective to get halfspace D − gT (pi,τ − pj,τ ) ≤ 0, where g = pk i,τ − pk j,τ pk i,τ − pk j,τ 2 Algorithm 16
  17. 17. Convex-concave procedure intuition: ball for forbidden region is approximated by a halfspace tangent to the ball in each iteration, vehicle position is optimized within halfspace constraint and halfspace approximation is updated thereafter Obstacle Allowable region k = 1 Obstacle Allowable region k = 2 Obstacle Allowable region k = 3 Algorithm 17
  18. 18. Problem decomposition convexified CCA: minimize v∈V fv (xv, uv) + w∈W gk w (xVw ) issues: problem dimension is still large decomposition needs to allow vehicle and obstacle plug-and-play Algorithm 18
  19. 19. 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 fv (xv, uv) + w∈W gk w (˜xVw ) subject to x = ˜x fv and gk w encode vehicle objectives and convexified net constraints in the kth iteration of CCP x = ˜x are consensus constraints that couple solutions to independent fv and gk w minimizations Algorithm 19
  20. 20. Proximal message passing algorithm define “price” term to be running sum of inconsistencies: y := y + (x − ˜x) repeat until convergence: 1. prox controller update: in parallel, minimize each vehicle’s fv plus vehicle’s price component yv 2. avoidance projection update: in parallel, minimize each net’s gk w plus net’s price component yw 3. price update: in parallel, update price with the difference between prox controller and avoidance projection solutions Algorithm 20
  21. 21. Proximal message passing algorithm repeat until convergence: 1. prox controller update xκ+1 v , uκ+1 v := argmin (xv,uv) fv (xv, uv) + (ρ/2) xv − (˜xκ v − yκ v ) 2 2 in parallel, for each vehicle (ρ > 0; RHS is proximal operator of fv at ˜xκ v − yκ v ) 2. avoidance projection update ˜xκ+1 Vw := argmin ˜xVw gκ w (˜xVw ) + (ρ/2) ˜xVw − xκ Vw + yκ Vw 2 2 in parallel, for each net (ρ > 0; RHS is projection operator onto convexified constraints at xκ Vw + yκ Vw ) 3. scaled price update yκ+1 Vw := yκ Vw + xκ Vw − ˜xκ Vw in parallel, for each net Algorithm 21
  22. 22. Convergence for proximal message passing objective functions just has to be convex closed proper (can be nondifferentiable, take on +∞ values, . . . ) residual convergence: x → ˜x (trajectories deconflicted) objective convergence: f (xκ , uκ ) + g (˜xκ ) → f0 (controls are locally optimal) price convergence: yκ → y (optimal prices are found) Algorithm 22
  23. 23. Example vehicles compute new tentative trajectories V: v1 v2 v3 v4 v5 v6 v7 v8 N: w1 w2 w3 E: Algorithm 23
  24. 24. Example vehicles send tentative trajectories to neighboring nets V: v1 v2 v3 v4 v5 v6 v7 v8 N: w1 w2 w3 E: Algorithm 24
  25. 25. Example nets deconflict trajectories; update prices V: v1 v2 v3 v4 v5 v6 v7 v8 N: w1 w2 w3 E: Algorithm 25
  26. 26. 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
  27. 27. Example repeat V: v1 v2 v3 v4 v5 v6 v7 v8 N: w1 w2 w3 E: Algorithm 27
  28. 28. Proximal message passing algorithm each vehicle and net only has knowledge of its own objective function for each vehicle type, need to implement own minimizer – can have different types of vehicles with different dynamics, constraints (motorcycle, cars,...) all message passing is local, between vehicles and adjacent nets no global coordination other than iteration synchronization Algorithm 28
  29. 29. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Numerical experiments 29
  30. 30. Vehicle model simple kinematic bicycle model of the car ˙px = ν cos (θ) , ˙py = ν sin (θ) , ˙θ = ν L tan (θ) δ ( px , py ) θ L x y Numerical experiments 30
  31. 31. Model predictive control consider MPC algorithm minimize v∈V(t) λ T −2 τ=0 (δv,τ+1 − 2δv,τ + δv,τ−1) 2 + T τ=1 pveh τ − ¯pveh τ 2 2 + w∈W(t) µw (xVw ) subject to xv,0 = xinit v , xv,τ+1 = Av,τ xv,τ + Bv,τ δv,τ + cv,τ , ∀v ∈ V (t) , τ = 0, . . . , T − 1, solves CCA only for the next T discrete time steps locally optimizes for smooth steering and reference tracking repeatedly executes first input of control sequence Numerical experiments 31
  32. 32. Model predictive control transform into familiar convexified CCA in consensus form minimize v∈V fv (xv, uv) + w∈W gk w (xVw ) fv locally optimizes for smooth steering and reference tracking and encodes vehicle dynamics constraints gk w encodes convexified constraints at the kth CCP iteration Numerical experiments 32
  33. 33. Network topology and simulation number of vehicles ranges from 5 to 500 number of nets ranges from 0 to 200 MPC: 4 sec time horizon, 0.1 sec intervals network topology (initial positions, reference trajectories, obstacle locations) chosen randomly – vehicles linked to nets if in close proximity Numerical experiments 33
  34. 34. Example: Proximal message passing Numerical experiments 34
  35. 35. Example: 10 vehicles −200 −100 0 100 200 −250 −200 −150 −100 −50 0 50 100 150 200 250 x−position (m) y−position(m) car start obstacle Numerical experiments 35
  36. 36. Serial single-thread implementation use CVXGEN to generate custom C optimization code examples run on a 2.7 Ghz Intel i7 with 8 GB of RAM objective values at convergence of message passing algorithm always <1% suboptimal when compared with CVX for small network sizes for each (centralized) CCP iteration Numerical experiments 36
  37. 37. Solve time scaling (serial) 10 0 10 1 10 2 10 3 10 −2 10 −1 10 0 10 1 10 2 10 3 network size (number of cars) solvetimepersecondofsimulation fit exponent is 1.145; solve time scales as O |V| 1.145 with fully decentralized computation, average of 35 ms per MPC iteration for any network size Numerical experiments 37
  38. 38. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Conclusion and future work 38
  39. 39. Summary developed a completely decentralized local optimization method for guidance and control of multivehicle networks decentralized computation allows for millisecond solve times independent of network size when combined with MPC, can be used for real-time network operation envision a robust plug-and-play system for any vehicle types Conclusion and future work 39
  40. 40. Future work robust, asynchronous message passing algorithm – work with unreliable communication or network protocols more sophisticated vehicle models shared constraints beyond collision avoidance Conclusion and future work 40
  41. 41. References S. Boyd et al. Distributed optimization and statistical learning via the alternating direction method of multipliers. Foundations and Trends in Optimization, 1(3):1–122, 2010. J. Bento et al. A message-passing algorithm for multi-agent trajectory planning. In Advances in Neural Information Processing Systems, pages 521–529, 2013. M. Kraning et al. Dynamic network energy management via proximal message passing. Foundations and Trends in Optimization, 1(2):70–122, 2013. 41
  42. 42. References D. Mellinger et al. Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams. In IEEE Conf. on Robotics and Automation, pages 477–483, May 2012. D. Morgan et al. Model predictive control of swarms of spacecraft using sequential convex programming. In AIAA/AAS Astrodynamics Specialist Conf., Aug. 2012. 42