1. NUS CS5247 Randomized Kinodynamic Motion Planning with Moving Obstacles - D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. Int. J. Robotics Research, 21(3):233-255, 2002. Wai Kok Hoong

2. NUS CS52472 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

3. NUS CS52473 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

4. NUS CS52474 Introduction  Kinodynamic Planning Solve a robot motion problem subject to Non-Holonomic Constraints  Constraints between robot configuration and velocity Dynamics Constraints  Constraints among configuration, velocity, and acceleration / force  Both non-holonomic and dynamic constraints can be mapped into motion constraint equations in a control system

5. NUS CS52475 Introduction  Extends existing PRM framework  State × time space formulation a state typically encodes both the configuration and the velocity of the robot  Represents kinodynamic constraints by a control system set of differential equations describing all possible local motions of a robot  Generalization of expansiveness to state × time space  Analysis of the planner’s convergence rate  Experiment on real robot

6. NUS CS52476 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

7. NUS CS52477 Planning Framework – State-Space Formulation  Motion constraint equation ś = f(s, u)(1) s is in S: robot state ś is derivative of s relative to time u is in Ω: control input S: state space, bounded of dimension n. Ω: control space, bounded of dimension m (m<=n).  Under appropriate conditions, (1) is equivalent to k independent equations F i (s, ś) = 0, i =1, 2, … k and k = n-m

8. NUS CS52478 Planning Framework – State-Space Formulation (Examples)  Car-like Robot Configuration space representation  (x, y, θ) Motion constraints x’= v cos θ y’ = v sin θ θ’ = ( v/ L ) tan  x y m  Point-mass Robot Configuration space representation  s = (x, y, v x, v y ) Motion constraints  x’ = v x v ' x = u x / m  y ’ = v x v ’ y = u y / m

9. NUS CS52479 Complete Problem Formulation  Configuration space representation ST denotes the state × time space S × [0, +∞) Obstacles are mapped as forbidden regions Free space F belongs to ST is the set of all collision-free points (s, t). A collision-free trajectory τ: t in [t 1, t 2 ]-> τ(t)=(s(t), t) in F is admissible if it is induced by a function u:[t 1,b 2 ] through motion constraint equation.  Problem Given an initial (s b, t b ) and a goal (s g, t g ) Find a function u:[t b, t g ]->Ω which induces a collision-free trajectory τ:t in [t b, t g ] -> τ(t) = (s(t), t) in F and s(t b ) = s b, s(t g ) = s g. Returns no path existence if failure

10. NUS CS524710 Planning Framework - The Planning Algorithm

11. NUS CS524711 The Planning Algorithm – Milestone Selection  Each milestone is assigned a weight ω(m) = number of other milestones lying the neighborhood of m.  Randomly pick an existing m with probability π(m) ~ 1/ ω(m) and sample new point around m

12. NUS CS524712 The Planning Algorithm – Control Selection  Let U l be the set of all piecewise-constant control functions with at most l constant pieces. u in U l, for t 0 < t 1 <…<t l, u(t) is a constant c i in Ω in (t i-1,t i ), i=1,2,…,l  Picks a control u in U l for pre-specified l and δ max, by sampling each constant piece of u independently. For each piece, c i and δ i =t i -t i-1 are selected uniform-randomly from Ω and [0,δ max ]

13. NUS CS524713 The Planning Algorithm – Endgame Connection  Check if m is in a ball of small radius centered at the goal. Limitation: relative volume of the ball - > 0 as the dimensionality increases.  Check whether a canonical control function generates a collision-free trajectory from m to (s g, t g )  Build a secondary tree T ’ of milestones from the goal with motion constraints equation backwards in time.  Endgame region is the union of the neighborhood of milestones in T ’

14. NUS CS524714 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

15. NUS CS524715 Analysis of the Planner - Concepts  Expansiveness Extend visibility to reachability  β-LOOKOUT(S)

16. NUS CS524716 Analysis of the Planner - Concepts  (α,β) - expansiveness

17. NUS CS524717 Analysis of the Planner – Ideal Sampling  Algorithm 2 is the same as Algorithm 1, except that the use of IDEAL-SAMPLE replaces lines 3-5 in Algorithm 1.

18. NUS CS524718 Analysis of the Planner – Bounding the number of milestones  Lemma 1 If a sequence of milestones M contains k lookout points, then μ(R l (M)) >= 1 – e -βk  Lemma 2 A sequence of τ milestones contains k lookout points with probability at least 1 – e -αr/k  Theorem 1 Let g > 0 be the volume of endgame region E in χ and γ be a constant in (0,1]. If r >= (k/α) ln(2k/ γ) + (2/g) ln(2/ γ) and k = (1/β)ln(2/g) then a sequence M of r milestones contains a milestone in E with probability at lease 1 - γ

19. NUS CS524719 Analysis of the Planner – Approximating IDEAL-SAMPLE  Candidates Rejection sampling. (No) Weighted sampling. (Yes)  Concerns New milestone tends to be generated in l-reachability sets of existing milestones overlapping area  Those existing milestones are likely to be close

20. NUS CS524720 Analysis of the Planner – Choice of Suitable Control Functions  l must be large enough so that for any p in R(m b ), R l (p) has the same dimension as R(m b )  Theoretically, it is sufficient to set l=n-2, n is the dimension of state space.  The larger l and δ max yield the greater α and β, fewer milestones. But too large of them will make poor IDEAL-SAMPLE.

21. NUS CS524721 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

22. NUS CS524722 Experiments on Non-Holonomic Robots  Cooperative Mobile Manipulators  Two wheeled non-holonomic robots keeping visual contact and a distance range

23. NUS CS524723 Planner for Non-Holonomic Robots  Configuration Space Representation Project the cart/obstacle geometry onto horizontal plane. 6-D state space without time: s = (x 1, y 1, θ 1 x 2, y 2, θ 2 ) Coordination and orientation of the two carts.  Motion Constraint Equations  Implementation Weights computing PROPAGATE Endgame region

24. NUS CS524724 Experimental Results – Computed Examples for the Non-Holonomic Carl-Like Robot  Computed path for 3 different configurations  Planner was ran for several different queries in each workspace.  For every query, planner was ran 30 times independently with different random seeds.

25. NUS CS524725 Experimental Results – Computed Examples for the Non-Holonomic Carl-Like Robot  Planner Performance SGI Indigo workstation with a 195 Mhz R10000 processor Nclear –number of collision checks Nmil – number of milestones sampled Npro – number of calls to PROPAGATE

26. NUS CS524726 Experimental Results – Computed Examples for the Non-Holonomic Carl-Like Robot  Histogram of planning times for more than 100 runs on a particular query. The average time if 1.4 sec, and the four quartiles are 0.6, 1.1, 1.9 and 4.9 seconds. Due to a few runs taking 4 times the mean run time.

27. NUS CS524727 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

28. NUS CS524728 Planner for Air-Cushioned Robot  Configuration space representation 5-D Robot state × time space:  (x, y, x’, y’, t), coordination and velocity Constraint /motion equation:  x’’ = u cos θ / m, y’’ = u sin θ / m  Implementation Weight computing PROPAGATE Endgame region

29. NUS CS524729 Experimental Results – Computed Examples for the Air-Cushioned Robot Narrow passage

30. NUS CS524730 Experimental Results – Computed Examples for the Air-Cushioned Robot  Planner performance Pentium-III 550 MHz 128 MB memory  Narrow passage in configuration × time space

31. NUS CS524731 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

32. NUS CS524732 Experiments with the Real Robot  Integration Challenges Time Delay Sensing Errors Trajectory Tracking Trajectory Optimization  Sample additional milestones in the rest of the 0.4 second time slot.  Use a cost function to compare trajectories Safe-Mode Planning  If failing to find a path, compute an escape trajectory  Any acceleration-bounded, collision-free motion within a small time duration in the workspace  Escape path simultaneously computed with normal path

33. NUS CS524733 Snapshots of Robot Executing a Trajectory

34. NUS CS524734 On-the-fly Re-Planning (Simulation)

35. NUS CS524735 On-the-fly Re-Planning (Real) 123 456 789

36. NUS CS524736 Contents  Introduction  Planning Framework  Analysis of the Planner  Experiments Non-Holonomic Robots Air-Cushioned Robot Real Robot  Summary

37. NUS CS524737 Summary  What was presented in this paper: Generalization of expansiveness to state × time space Analysis of the planner convergence rate Experiment on real robot  Future Work: Apply the planner to environments with more complex geometry and robots with high DOFs  Hierarchical algorithms for collision checking Reducing standard deviation of running time  Thin and long tail in histogram Further develop tools to analyze the efficiency of randomized motion planners ~ The End ~