Slideshare uses cookies to improve functionality and performance, and to provide you with relevant advertising. If you continue browsing the site, you agree to the use of cookies on this website. See our User Agreement and Privacy Policy.

Slideshare uses cookies to improve functionality and performance, and to provide you with relevant advertising. If you continue browsing the site, you agree to the use of cookies on this website. See our Privacy Policy and User Agreement for details.

Successfully reported this slideshow.

Like this presentation? Why not share!

- Value Function Approximation via Lo... by Lyft 235 views
- Ijetr042739 by International Jou... 29 views
- Secure Routing in Mobile Ad-hoc Net... by warse 223 views
- Ijeas0310005 by International Jou... 24 views
- Ijetr042703 by International Jou... 34 views
- Enhancing Traffic Intersection Cont... by Rudi Ball 1482 views

525 views

Published on

Published in:
Engineering

No Downloads

Total views

525

On SlideShare

0

From Embeds

0

Number of Embeds

6

Shares

0

Downloads

12

Comments

0

Likes

1

No embeds

No notes for slide

- 1. Cooperative Collision Avoidance via Proximal Message Passing Hao Yi Ong and J. Christian Gerdes Stanford University July 3, 2015
- 2. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Introduction 2
- 3. Cooperative collision avoidance (CCA) multivehicle problem – stage-cost function (fuel, trajectory deviation, etc.) – linearized dynamics – shared collision avoidance constraints Introduction 3
- 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) – eﬃcient algorithm needed for online implementation Introduction 4
- 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. 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-speciﬁc problem structures Introduction 6
- 7. Proximal message passing builds on network energy management by Kraning et al. [K+ 13] – convex objective and constraints only – ﬁnite horizon problem – oﬄine solution extensions handle – nonconvexity – inﬁnite horizon problem – real-time application Introduction 7
- 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. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Mathematical formulation 9
- 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. 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. 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. 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. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Algorithm 14
- 15. Convex-concave procedure (CCP) express problem with ‘diﬀerence 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. Convex-concave procedure collision avoidance constraint function is a “negative ball” D − pi,τ − pj,τ 2 ≤ 0 ﬁrst 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. 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. Problem decomposition convexiﬁed 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. Consensus optimization idea: decompose each iteration of convexiﬁed CCA into small, manageable subproblems using ADMM [B+ 10] convexiﬁed 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 convexiﬁed 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. Proximal message passing algorithm deﬁne “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 diﬀerence between prox controller and avoidance projection solutions Algorithm 20
- 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 convexiﬁed 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. Convergence for proximal message passing objective functions just has to be convex closed proper (can be nondiﬀerentiable, take on +∞ values, . . . ) residual convergence: x → ˜x (trajectories deconﬂicted) objective convergence: f (xκ , uκ ) + g (˜xκ ) → f0 (controls are locally optimal) price convergence: yκ → y (optimal prices are found) Algorithm 22
- 23. Example vehicles compute new tentative trajectories V: v1 v2 v3 v4 v5 v6 v7 v8 N: w1 w2 w3 E: Algorithm 23
- 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. Example nets deconﬂict trajectories; update prices V: v1 v2 v3 v4 v5 v6 v7 v8 N: w1 w2 w3 E: Algorithm 25
- 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. Example repeat V: v1 v2 v3 v4 v5 v6 v7 v8 N: w1 w2 w3 E: Algorithm 27
- 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 diﬀerent types of vehicles with diﬀerent dynamics, constraints (motorcycle, cars,...) all message passing is local, between vehicles and adjacent nets no global coordination other than iteration synchronization Algorithm 28
- 29. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Numerical experiments 29
- 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. 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 ﬁrst input of control sequence Numerical experiments 31
- 32. Model predictive control transform into familiar convexiﬁed 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 convexiﬁed constraints at the kth CCP iteration Numerical experiments 32
- 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. Example: Proximal message passing Numerical experiments 34
- 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. 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. 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 ﬁt 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. Outline Introduction Mathematical formulation Algorithm Numerical experiments Conclusion and future work Conclusion and future work 38
- 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. 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. 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. 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

No public clipboards found for this slide

Be the first to comment