Nonholonomic Multibody Mobile Robots: Controllability and Motion Planning in the Presence of Obstacles (1991)

1
Nonholonomic Multibody Mobile Robots
Controllability and Motion Planning in the
Presence of Obstacles (1991)

• Jerome Barraquand
• Jean-Claude Latombe

2
Controllability

• A robot is controllable if for any q1 and q2
• ? a free path between from q1 to q2 ?
• ? a feasible free path from q1 to q2
• A multi-body robot is controllable if it can take
  on at least 2 different steering angles, j1 and
  j2, on the interval -p, p), and can move both
  forward and backward

3
Motivation

• How do we actually generate a feasible path?
• Other algorithms first generate a free path
  ignoring nonholonomic constraints, then convert
  to a topologically equivalent feasible path.
• We would like to take our constraints into
  account as we plan our path

4
Algorithm Overview

• Series of hinged bodies with wheels on flat
  ground, no slipping
• Front wheels have 2 different steering angles,
  jmin and jmax, in the range of -p, p)

• Arbitrary obstacles
• Searches for a path from qinit to any
  configuration within a neighborhood of qgoal
• Asymptotically complete
• Optimal in number of reversals
• Exponential in number of bodies of the robot

5
Summary of How It Works

• Searches a tree where each node is a feasible
  configuration

• At each step, considers only the option of
  setting the steering angle to jmin or jmax,
  backing up or going forward
• Successors of a node represent configurations
  where the car can be dt0 time later

6
Summary of How It Works

• Expand the tree in an order that minimizes
  reversals
• Tricks to prune the tree

• Limit your search depth (H)
• Avoid visiting identical configurations (R)

7
Generating Successors

• Consider only a set of discrete values for v and
  j, specifically v-1 or 1, j jmin or jmax.
• Integrate velocity equations defined by
  nonholonomic systems over dt0 to compute each new
  position

dx/dt v cos(j) cos(q) dy/dt v cos(j) sin(q) L
dq/dt v sin(j)
8
Growing the Tree

• Maintain fringe nodes in heap
• Select the fringe node along a path with the
  least number of reversals, tie goes to shortest
  path
• Terminate this node if it collides with an
  obstacle or is in a previously visited
  configuration
• Mark nodes configuration as visited
• Test if nodes configuration is in the
  neighborhood of the goal
• Add nodes successors to fringe
• Bound maximum search depth (H)

9
Node Deletion

• A node is deleted (i.e. we dont need to expand
  it) if

• The path between this node and this nodes parent
  collides with an obstacle ? use a bitmap to
  represent workspace
• Current configuration is in the neighborhood of a
  previously visited configuration

• Store an array A composed of 2R(p2)
  parallelepipeds which maps entire configuration
  space
• R is user provided tuning parameter, p is the
  number of bodies in the robot
• Need two separate arrays for going forward and in
  reverse

10
Completeness and Optimality

• Algorithm achieves controllability if dt0 is set
  small enough, and R and H are set large enough
• Since nodes along paths with minimum number of
  reversals are expanded first, algorithm is
  optimal in number of reversals (if dt0, R, and H
  are fine enough)

11
Picking Tuning Parameters

• Three tuning parameters dt0, R, and H
• If algorithm returns failure, you dont know if
  there is no solution or if dt0, R, and H were not
  set properly
• Small dt0 might be necessary results in much
  larger tree
• Values of dt0, R, and H are dependent on each
  other setting the parameters in the order dt0,
  H, and R works well
• Guess dt0 using prior knowledge of the workspace
• Can use faster algorithm ignoring nonholonomic
  constraints to test if solution is possible, then
  keep refining dt0 until solution is found

12
Performance

• Exponential in the size of the configuration
  space (number of bodies of the robot)
• Exponential in R, so in practice should be
  exponential in 1/dt0
• Not very practical for robots with more than 2
  bodies, but can solve some difficult problems in
  reasonable amounts of time

13
Performance (in 1991)
Car That Can Only Turn Left
Random Obstacles
jmax45o, jmin22.5o, R9, 20 sec.
jmax45o, R9, 2 min.
14
Performance (in 1991)
jmax45o, R7, 20 min.
15
Algorithm Evaluation

• General/Extendable With modest changes, could
  work for other types of robots with different
  nonholonomic constraints or in a 3D environment
  room for speed optimizations dealing with the
  tree search
• Complete and Optimal In the asymptotic case will
  always find a path (if one exists) with the
  minimum number of reversals
• Approximate Final configuration is in the
  neighborhood of goal
• Discrete Steering Angles Does not take advantage
  of possibility of steering angles on continuous
  interval

• Use more discrete steering angles than 2
  (increases branching factor)
• Use some kind of smoothing algorithm

16
Algorithm Evaluation (Cont)

• Minimizes Reversals Better than most algorithms
  but still not necessarily what you want perhaps
  could try a different evaluation metric