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 ~