MultiLevel Path Planning for Nonholonomic Robots using SemiHolonomic Subsystems

1
Multi-Level Path Planning for Nonholonomic Robots
using Semi-Holonomic Subsystems

• Paper By S. Sekhavat, P. Svestka, J.-P.
  Laumond, M.H. Overmars
• Presentation By Chad Helm

2
Classic Approach

• Solve problem in two steps
• Find a collision-free path without taking into
  account the nonholonomic constraints.
• Transform the geometric path into one that
  respects the nonholonomic constraints.

3
Overview of Algorithm

• Obtain initial path P0 and smooth (car
  constraints only)
• Transform path with tube-Probabilistic Path
  Planner (car and one trailer)
• Smooth path with probabilistic path shortening
  (car and one trailer)

4
Overview of Algorithm(cont)

• Transform path to a feasible path for a car with
  two trailers with geometric Nonholonomic-approxima
  tion.
• Transform path with the Pick and Link method (car
  with two trailers).
• Smooth path with probabilistic path shortening.

5
Semi-holonomic subsystem

• Car-like robot
• Configuration of car Pulling n trailers n3
  parameters
• Non-holonomic constraints represented as n1
  equations
• Or

6
Kinematic Model of Car and Two Trailers
7
System Definition
y
x
8
Multi-level Planning Definition

• Sequence of transformation steps
• Find a path Po for system So, then in n steps
  transform it to a path feasible for real system
  S. At step i, path Pi is feasible for Si, is
  transformed to a path Pi1 feasible for Si1.
• Semi-holonomic subsystem
• Local planner

9
Multi-level Planning Scheme

• Compute a collision-free path P0, respecting the
  nonholonomic constraints of system So.
• For i0 to n-1
• Transform path Pi to a collision free path Pi1
  respecting the nonholonomic constraints of system
  Si1

10
1. Tube-Probabilistic Path PlannerPath
Transformation

• For a given path Pi-1 a subset of CS is created
  with distance ? of Pi-1.
• is the CS-tube around Pi-1.
• Transformation is performed by executing Li on

Local Planner
11
Tube-Probabilistic Path Planner
CS
12
2. Probabilistic path shortening

• Loop until
• Let Q be a random path segment of Pi with start
  and end configuration of s and e
• Let OL be Li(s,e)
• If QL is collision-free and length(QL) length(Q)
• Then replace Q by QL in Pi
• If a random path segment is shorter than the
  existing path, replace (must be collision free)

13
Nonholonomic Violation Metric

• The i-violation of (c1,c2) is given by Vli(c1,c2)
  defined as
• where D is the Euclidean distance in R2
• The i-violation of denoted by

14
The i-violations of the Configuration Pair
15
3. Geometric Nonholonomic-approximation(Make
path feasible)

• Let Pi-1 be the path for which the i-th
  nonholonomic constraint is to be approximated.
• Loop until
• Let Q be a random path segment of Pi-1
• Let be an i-alteration of Q
• If is collisionfree and
• Then replace Q by in Pi-1

16
i-alterations

• Let be the path
  (segment) to be i-altered.
• Let be a (experimentally) chosen constant.
• Let
• Replace by

17
5. Pick and Link (PL) methodPath Transformation

• Transforming path Pi-1 to Pi, by joining the
  start Pi-1(0) and goal Pi-1(1) configurations, by
  using the local planner Li.
• If collision-free problem is solved.
• If not, use a intermediate configuration.
• Pi-1(1/2), and apply the local planner both
  portions.
• Repeat.

18
Pick and Link Method
Local Planner gives new path respecting new NH
constraints
19
RTR PlannerLocal Planner

• Given two configurations the planner constructs
  the shortest path consisting of a curve, a
  straight path, and another curve.
• Curves are of constant curvature.
• Construction and collision checking of local
  paths can by done very efficiently.

20
Sinusoidal plannerLocal Planner

• Sinusoidal local planner is to transform the
  state coordinates into the chained form and to
  apply sinusoidal inputs to the transformed system.

21
Chained Form

• From nonlinear controls systems
• By satisfying a set of sufficient conditions
  there exists a transformation of the system to
  chained form. (car and n trailers)
• Local feedback transformation
• Such that the transformed system is in chained
  form

22
How to change the coordinates

• Start with state equation
• Define the map as
• Where

23
Kinematic Model of a Car

• Equations of motion
• Change of coordinates and inputs

24
Chained form for car(no trailers)
25
Steering Chained formed systems

• Steer z1 and z2 to their desired values.
• For each zk2, steer zk to its final
  value using ,
• where a and b satisfy
• By steering the states step by step using
  integrally related frequencies, the previous
  states no not change.

26
Steering by SinusoidsA Mathematical Introduction
to Robotic Manipulation Murray, Li, Sastry
27
Obtaining the initial paths for So

• Free configurations are generated using PPP and
  interconnected using the RTR local planner.
• Start and goal are connected to this roadmap.
• If fail then there is no solution or the roadmap
  needs to be extended.

28
Final Multi-Level Algorithm

• Obtain initial path P0.
• Retrieve and smooth P0 from the S0 roadmap.
• Transform P0 to a S1-path with tube-PPP
• Transform to a smooth path with
  probabilistic path shortening.

29
Final Multi-Level Algorithm (cont)

• Transform to a path respecting the
  second trailers nonholonomic constraint with
  geometric NH-approximation.
• Transform to a S2 path with the PL
  method.
• Transform to a smooth path P2, with
  probabilistic path shortening.

30
Results

• Whole multi-level algorithm performs better with
  respect to computation times and path qualities,
  than just two-level planning.
• Computation time and path length shorter
• Method is much faster then going from a car
  path to a car with two trailers path.

31
Open Questions and Future Research

• Not clear in general what should be the order in
  which nonholonomic constraints are to be
  introduced.
• Running time depends on choice of the initial
  path.
• Heuristics to stop smoothing algorithms.
• What about more then 2 trailers?

32
References

• Multi-Level Path Planning for Nonholonomic Robots
  using Semi-Holonomic Subsystems S. Sekhavat, P.
  Svestka, J.-P. Laumond, M.H. Overmars (Journal
  of Robotics Research, Vol.17, No. 8, pp.840-857,
  August 1998)
• Nonholonomic Motion Planning Steering Using
  Sinusoids Murray an Sastry (IEEE Trans. On
  Automatic Control, Vol 38, No. 5 1993)
• Steering Car-Like Systems with Trailers Using
  Sinusoids D. Tilbury, J-P. Laumond, R. Murray, S.
  Sastry, G. Walsh (ICRA 92)
• Steering nonholonomic systems in chained form R.
  Murray, S. Sastry (IEEE Conference on Decision
  and Control 91)
• Nonlinear Control Systems (3rd ed.)- Alberto
  Isidori
• A Mathematical Introduction to Robotic
  Manipulation Murray, Li, Sastry