1 / 37

Randomized Kinodynamic Motion Planning with Moving Obstacles

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. Contents. Introduction Planning Framework Analysis of the Planner Experiments Non-Holonomic Robots

ellard
Download Presentation

Randomized Kinodynamic Motion Planning with Moving Obstacles

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. 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

More Related