Upload
johnda
View
40
Download
0
Tags:
Embed Size (px)
DESCRIPTION
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 - PowerPoint PPT Presentation
Citation preview
NUS CS5247
Randomized Randomized Kinodynamic Motion Kinodynamic Motion
Planning with Moving Planning with Moving ObstaclesObstacles
- D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. - D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. Int. J. Robotics Research, 21(3):233-255, 2002.Int. J. Robotics Research, 21(3):233-255, 2002.
Wai Kok HoongWai 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 ) tan
x
ym
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
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 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 - γ
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, θ1 x2, 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 ~