1 / 23

Real-Time Motion Planning for Agile Autonomous Vehicles

Emilio Frazzoli, Munther A. Dahleh and Eric Feron. Real-Time Motion Planning for Agile Autonomous Vehicles. Jingru Luo. Background. Kinodynamic motion planning A new randomized incremental algorithm System’s dynamics An environment with moving obstacles Efficiency Safety guarantee

rahim-freeman
Download Presentation

Real-Time Motion Planning for Agile Autonomous Vehicles

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Emilio Frazzoli, Munther A. Dahleh and Eric Feron Real-Time Motion Planning for Agile Autonomous Vehicles Jingru Luo

  2. Background • Kinodynamic motion planning • A new randomized incremental algorithm • System’s dynamics • An environment with moving obstacles • Efficiency • Safety guarantee • Optimal solution • Probabilistic complete • Guaranteed performance

  3. Framework • System Dynamics • Ordinary Differential Equations(ODEs) • Equilibrium points (zero velocity) (1) (2)

  4. Framework • Obstacle-Free Guidance System • Optimal cost-to-go function for all • Compute the optimal control policy (5) (6) x

  5. Framework • Notation • Feasible set F • for all (state, time) pairs satisfying all constraints • Reachable set Given , is reachable if find x

  6. Framework • Problem Formulation • Given an initial state at time , and a goal , find that can steer the system from to • Optimal trajectory to minimize the cost

  7. Motion Planning Target • Basic idea Random equilibrium point Initial condition

  8. Data Structure • Node • (state, time) couple • Total cost • Accumulated cost by bookkeeping • Cost-to-go • Lower bound: optimal cost function • Upper bound: infinity • Edge • The randomly generated equilibrium point • Cost along the edge

  9. Initialization • Root node: some point in the future • Contains the initial condition • As an example: , moving at , and we devote s to the computation, the root node must be set at • Total cost: • Accumulated cost: set to zero • Cost-to-go • Lower bound: optimal cost function • Upper bound: infinity B A

  10. Tree Expansion Step • Two points: • Which node to expand • In which direction to explore • Samples a random configuration , and try to expand all nodes in order of increasing cost • Apply the optimal control policy until the system gets to endgame region of

  11. Target Random Equilibrium point 3 1 2 Initial state

  12. Real-Time Consideration • Safety Check • Moving obstacles • Collision check at time point is not enough • Check safety over a time • safety • A milestone (x, t) is τ-safe if (x, t) is feasible for all t∈[t,t+τ]

  13. Real-Time Consideration • Improving performance • Trajectories from equilibrium to equilibrium provide unsatisfactory performance • Primary milestone • Secondary milestone, split the new edge into n segments and add the breakpoints Primary milestone Secondary milestone

  14. Update Cost Target • Look for the optimal trajectory • When a solution is found • Upper bound on the cost-to-go are updated • Update backward to the root • Parent.U.B.> node.U.B.+ edge_cost • Parent.U.B  node.U.B.+ edge_cost Xnew Initial state

  15. Tree Pruning • Upper bound and lower bound • Lower bound: optimal cost-to-go in free environment • Upper bound: cost of the best trajectory from the milestone to the destination or +∞ • Every time a solution is found, prune subtree while updating the node • lower bound + edge cost > upper bound current node

  16. Target 10 L: 10 U: 10 Random point x 10 L: 15 U: 20 L: 15 U: +∞ 3 L: 25 U: +∞ 1 5 2 10 L: 35 U: +∞ 5 L: 20 U: +∞ L: 20 U: 25 5 4 5 6 L: 22 U: +∞ L: 22 U: 30 L: 25 U: +∞ L: 25 U: 35 Initial state

  17. Complete Algorithm Target Initialization Loop if trajectory(root, target) collison free then return success loop newTrajectory=Expand-Tree if newTrajectory != failure and safe then get Primary and Second M.S. for all new M.S. do Update-Cost and Prune-Tree until Time is up if feasible solution found then root  best child else if root has children then root  random child else generate another root Random point Initial state

  18. Application Example • Four planners • A: one node, chosen randomly • B: one node, chosen as the closest one • C: all node sorted in random order • D: all node sorted in the order of increasing distance

  19. Application Example • Ground robot moving among fixed spheres • Helicopter moving among fixed spheres • The planners handle easily

  20. Application Example • Ground robot moving through sliding doors • C, D perform better Algorithm A Algorithm D

  21. Application Example • Helicopter moving through sliding doors • C, D perform better while A failed Algorithm B Algorithm D

  22. Conclusion • Deal with system dynamics efficiently • Decouple between the high-level motion plan and low-level control tasks • Real time issues • Finite computation time • Safety check • Exploration strategy • Future work • Motion plan in the uncertain environment

  23. Thank You!

More Related