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

2
Contents

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

3
Contents

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

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

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 planners convergence rate
• Experiment on real robot

6
Contents

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

7
Planning Framework State-Space Formulation

• Motion constraint equation
• s f(s, u) (1)
• s is in S robot state
• s is derivative of s relative to time
• u is in O control input
• S state space, bounded of dimension n.
• O control space, bounded of dimension m (mltn).
• Under appropriate conditions, (1) is equivalent
  to k independent equations
• Fi (s, s) 0, i 1, 2, k and k n-m

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 f

• Point-mass Robot
• Configuration space representation
• s (x, y, vx, vy)
• Motion constraints
• x vx v'x ux / m
• y vx vy uy / m

y
m
x
9
Complete Problem Formulation

• Configuration space representation
• ST denotes the state time space S 0, 8)
• 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 t in t1, t2-gt
  t(t)(s(t), t) in F is admissible if it is
  induced by a function ut1,b2 through motion
  constraint equation.
• Problem
• Given an initial (sb, tb) and a goal (sg, tg)
• Find a function utb, tg-gtO which induces a
  collision-free trajectory tt in tb, tg -gt t(t)
  (s(t), t) in F and s(tb) sb, s(tg) sg.
• Returns no path existence if failure

10
Planning Framework -The Planning Algorithm
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 p(m)
  1/ ?(m) and sample new point around m

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 lt t1 ltlttl,
• u(t) is a constant ci in O in (ti-1,ti),
  i1,2,,l
• Picks a control u in Ul for pre-specified l and
  dmax, by sampling each constant piece of u
  independently. For each piece, ci and diti-ti-1
  are selected uniform-randomly from O and 0,dmax

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

14
Contents

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

15
Analysis of the Planner - Concepts

• Expansiveness
• Extend visibility to reachability
• ß-LOOKOUT(S)

16
Analysis of the Planner - Concepts

• (a,ß) - expansiveness

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.

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)) gt 1 e -ßk
• Lemma 2
• A sequence of t milestones contains k lookout
  points with probability at least 1 e -ar/k
• Theorem 1
• Let g gt 0 be the volume of endgame region E in ?
  and ? be a constant in (0,1. If r gt(k/a)
  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
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
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 ln-2, n
  is the dimension of state space.
• The larger l and dmax yield the greater a and ß,
  fewer milestones. But too large of them will make
  poor IDEAL-SAMPLE.

21
Contents

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

22
Experiments on Non-Holonomic Robots

• Cooperative Mobile Manipulators

• Two wheeled non-holonomic robots keeping visual
  contact and a distance range

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

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.

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

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

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

29
Experimental Results Computed Examples for the
Air-Cushioned Robot
Narrow passage
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

31
Contents

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

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

33
Snapshots of Robot Executing a Trajectory
34
On-the-fly Re-Planning (Simulation)
35
On-the-fly Re-Planning (Real)
1
2
3
4
5
6
7
8
9
36
Contents

• Introduction
• Planning Framework
• Analysis of the Planner
• Experiments
• Non-Holonomic Robots
• Air-Cushioned Robot
• Real Robot
• Summary

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