1. Emilio Frazzoli, Munther A. Dahleh and Eric Feron 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  Basic idea Initial condition Random equilibrium point Target

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 A B

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. Initial state Random Equilibrium point Target 1 2 3

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  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 Initial state Target Xnew

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. Initial state Random point Target x 3 4 5 6 L: 10 U: 10 L: 15 U: +∞ 2 L: 25 U: +∞ 1 L: 35 U: +∞ 10 L: 20 U: +∞ L: 22 U: +∞ L: 25 U: +∞ L: 15 U: 20 L: 20 U: 25 L: 22 U: 30 L: 25 U: 35 10 5 5 5

17. Initial state Random point Target Complete Algorithm Initialization Loop if trajectory(root, target) collison free thenreturn 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

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 AAlgorithm D

21. Application Example  Helicopter moving through sliding doors C, D perform better while A failed Algorithm BAlgorithm 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