Randomized Kinodynamics Motion Planning with Moving Obstacles

David Hsu, Robert Kindel, Jean-Claude Latombe, Stephen Rock

Contents

Introduction Planning Algorithm Expansiveness Analysis Experiments

Introduction

Problem statement:- Motion planning with kinodynamic constraints and moving obstacles adding to the uncertainty of the environment

Roadmap is build in the collision free subset of state x time space where state = configuration x velocity

Tree shaped roadmap rooted at the initial state x time point and oriented along time axis

To sample milestones, a control input is selected at random and integrated with the control system for a short duration of time, from a previous milestone

Key Contributions

Proof of convergence:- If the space satisfies a geometric expansiveness property, then the planner is probabilistically complete. Convergence is provably fast.

Results of integrating the planner into a hardware robot testbed. Demonstrating that a fast planner can reliably handle dynamic environments, even with uncertainty in the future motions of obstacles.

Planning framework

The algorithm builds a probabilistic roadmap in the collision free subset F of the state x time space of the robot.

The roadmap is computed in the connected component of F that contains the robot’s initial state x time point.

State-space formulation

Robot motion equation to represent nonholonomic and dynamic constraints:s’ = f(s,u) where s S is robot’s state, s’ is its derivative relative to time,u is control inputS and are robot’s state space and control space and are bounded manifold’s of dimensions n and m with m ≤ n. S and are subsets of R^n and R^m

State-space formulation

Example – Nonholonomic car navigationLet (x,y) be the position of midpoint R

between rear wheels is orientation of rear wheels with

respect to x- axisCar’s state (x,y, ) R^3Control input is vector(v, ) where v and

are car’s speed and steering angle.

The nonholonomic constraints:x’ = v.cos

y’ = v.sin ’ = (v/L)tan

S and are subsets of R^3 and R^2.

Planning algorithm

Iteratively builds tree shaped roadmap T rooted at mb = (sb,tb) where (sb,tb) is the initial state x time point

Planning algorithm

At each iteration it randomly picks a milestone (s,t) from T, a time t’ with t’ <= tg and a control function u:[t, t’] ->

It then computes the trajectory induced by u by integrating the equation from (s,t).

Planning algorithm

If this trajectory lies in free space F its endpoint (s’, t’) is added to T as new milestone. A directed edge is created from (s,t) to (s’, t’) and u is stored with this edge

The planner exits with success when the newly generated milestone falls in an endgame region that contains (sg, tg) ie. goal state x time

Planning algorithm

Milestone selection: weight w(m) is attached to each miletstone m in T. The weight of m is the number of other milestones lying it’s neighborhood. To avoid over sampling, the planner picks an existing milestone at random with probability t(m) inversely proportional to w(m).

Control selection: is done uniformly at random from by selecting a piecewise constant control function u Ul over time interval (ti-1, ti), with ti – ti-1 ≤ δmax, where δmax is a constant

Endgame connection: This planner’s control driven sampling does not reach the goal (sg, tg) exactly but rather reaches an endgame region

Algorithm

Expansiveness Analysis

Expansive state x time space: Expansiveness tries to characterize how difficult it is to capture the connectivity of free space

A free space F is said to be expansive if every subset S F has large lookout. It’s been proved that for expansive space, a classic PRM planner converges at exponential rate as number of sampled milestones increase

Expansiveness Analysis

• With kinodynamic constraints notion of visibility is generalized to that of reachability

• Lookout of a set S is the subset of all points in S whose l-reachability sets overlap significantly with the reachability set of S outside S

Experiments

Nonholonomic robots Air-cushioned robots Real robots

Thank you.