Randomized Kinodynamic Motion Planning with Moving Obstacles

1. 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 NUS CS5247

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

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

4. 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 NUS CS5247

5. 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 NUS CS5247

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

7. 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 • Fi (s, ś) = 0, i =1, 2, … k and k = n-m NUS CS5247

8. Planning Framework –State-Space Formulation (Examples) • Car-like Robot • Configuration space representation • (x, y, θ) • Motion constraints x’= v cos θ y’ = v sin θ θ’ = ( v/ L ) tanf • Point-mass Robot • Configuration space representation • s = (x, y, vx, vy) • Motion constraints • x’ = vx v'x = ux / m • y’ = vx v’y = uy / m y m x NUS CS5247

9. 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 [t1, t2]-> τ(t)=(s(t), t) in F is admissible if it is induced by a function u:[t1,b2] through motion constraint equation. • Problem • Given an initial (sb, tb) and a goal (sg, tg) • Find a function u:[tb, tg]->Ω which induces a collision-free trajectory τ:t in [tb, tg] -> τ(t) = (s(t), t) in F and s(tb) = sb, s(tg) = sg. • Returns no path existence if failure NUS CS5247

10. Planning Framework -The Planning Algorithm NUS CS5247

11. 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 NUS CS5247

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

13. 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 (sg, tg) • 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’ NUS CS5247

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

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

16. Analysis of the Planner - Concepts • (α,β) - expansiveness NUS CS5247

17. 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. NUS CS5247

18. Analysis of the Planner –Bounding the number of milestones • Lemma 1 • If a sequence of milestones M contains k lookout points, then μ(Rl(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 aconstant 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 - γ NUS CS5247

19. 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 NUS CS5247

20. Analysis of the Planner –Choice of Suitable Control Functions • l must be large enough so that for any p in R(mb),Rl(p) has the same dimension as R(mb) • 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. NUS CS5247

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

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

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

24. 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. NUS CS5247

25. 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 NUS CS5247

26. 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. NUS CS5247

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

28. 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 NUS CS5247

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

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

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

32. 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 NUS CS5247

33. Snapshots of Robot Executing a Trajectory NUS CS5247

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

35. On-the-fly Re-Planning (Real) 1 2 3 4 5 6 7 8 9 NUS CS5247

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

37. 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 ~ NUS CS5247