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 input S 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 navigation Let (x,y) be the position of midpoint R between rear wheels  is orientation of rear wheels with respect to x- axis Car’s state (x,y, )  R^3 Control 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 m b = (s b,t b ) where (s b,t b ) 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’   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 (s g, t g ) 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  U l over time interval (t i-1, t i ), with t i – t i-1 ≤ δ max, where δ max is a constant  Endgame connection: This planner’s control driven sampling does not reach the goal (s g, t g ) 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.