nonholonomic multibody mobile robots controllability and motion planning in the presence of obstacles

Nonholonomic Multibody Mobile Robots: Controllability and Motion Planning in the Presence of Obstacles Authors: Jerome Barraquand and Jean-Claude Latombe Published: 1991 Presented by:

Nonholonomic Multibody

Mobile Robots: Controllability and Motion Planning in the Presence of

Obstacles

Authors: Jerome Barraquand

and Jean-Claude Latombe

Published: 1991 Presented by: Jason Haas

Main Contributions

 Application of Controllability Rank Condition

Theorem resulting in a general result on the

controllability of nonholonomic robots

 Application to multibody mobile robots –

controllability results, even with inequality

kinematic constraints

 Implementation of planner for one- and two-body mobile robots

Approach – Main Idea

 Divide up the path into

 Compute the control or

next step repeatedly

Controllability Generalization

 Piecewise constant control inputs

 Nonlinear control concepts

U  subset of

2

q

) ( q1

) (

) (

)

WAU  U  U  U

Controllability – Controllable

 A system is controllable if and only if ∀ q0 ∈

 Any state can reach any other state

 System is locally controllable if and only if

∀ q ∈ is a neighborhood of q

 Neighborhood is an open subset

 Local controllability implies controllability via patching

Controllability – Locally

Controllable

Controllability – Weak

Controllability

 A system is weakly controllable at q0 if and only if

Not a neighborhood, not an open subset

 “A system is locally weakly controllable at q0 if for every neighborhood U of q0, is also a

neighborhood of q0 ∀ q0 ∈ ”

 Weak controllability implies controllability via

patching

C q

WA C ( 0) 

)(q0

WA U

Controllability – Symmetry

 Definition symmetric: accessibility relation

(U-accessibility or weak U-(U-accessibility) is symmetric (i.e applies q0 → q1 and q1 → q0)

 Local controllability implies controllability

 Local weak controllability implies weak

controllability if symmetric system

Control Lie Algebra (1)

Example: Car-Like Robot*

y

x

Configuration space is 3-dimensional: q = (x, y,  )

But control space is 2-dimensional: (v, ) with

Maneuver made of 4 motions

For example:

X (t)

Y

-X -Y

T

T

* Slide obtained from J.-C Latombe – Stanford CS 326 slides

Control Lie Algebra (2)

 Recursively compute Lie brackets to find maximal distribution

 Find hidden degrees of freedom

 External product (e.g cross product)

 Defines tangent space (where lives)q

 Theorem: two conditions equivalent

 Controllability Rank Condition satisfied 

(Chow, 1939)

 CLA(F) = distribution

 Vector ↔ basis, vector field ↔ distribution

 Locally weakly controllable (controllable)

System Classification –

Questions

 1 Are constraints of the above form nonintegrable/

nonholonomic? (integrability)

 2 Do constraints of the above form “restrict the set

of configurations reachable from any given

(q q 



System Classification –

Constraints

 Set of k < n independent kinematic constraints

 Definition –

 Subset of tangent space defined by

 Chart defined by Implicit Function Theorem

(independent) – mapping free from system model –

G q

G q

) 0 , ,

0 (

1





q G

System Classification –

Equivalence

 System equivalent to nonlinear control system

 Kinematic inequalities on velocities map to

inequalities on controls

 Inequalities do not reduce dimension of control, only determine shape of control space

System Classification – Results

 Two cases for (Frobenius)

 > n-k  nonintegrable  nonholonomic

 = n-k  integrable  holonomic

 Two propositions answer integrability question

 1 Proplerly nonlinear kinematic constraints are nonholonomic

Planner – Claims

 Applicable to multi-body mobile robots

 Cars – 1 body

 Tractor-trailer – 2+ bodies

 Asymptotic completeness: if a solution path exists,

it will be found given a fine enough grained search

 Asymptotic optimality: if a solution path exists, the planner generates the solution with the minimal

number of reversals (changes of sign of linear

velocity)

 Practical only for 1-2 bodies (1991)

Planner – System Model

 No slipping

 Car / tractor –

 Trailer –

Planner – Input

 Start and goal configuration

 System model (equations of motion, constraints)

 Steering angle limits

Planner – Discretization

 Controls

 Duration –

 Values – extremal values only

Linear car / tractor velocity –

Steering angle –

 Configuration space

 Divide each dimension R ways

 Configuration space divided into grid of hyperparallelepipeds (cells)

 Bit-vector – constant access time

min, max

0

dt

 1 , 1

Planner – Operation

 Begin at start configuration

 Generate tree of configurations (neighbors)

 Expand current node’s neighbors

 Add acceptable neighbors to OPEN list

 Search tree simultaneously

 Use Dijkstra’s algorithm (shortest path)

 Metric – number of reversals

 Search depth – H

 Remove current node from OPEN list

 Pick next node in OPEN list with fewest reversals

Planner – Neighbors

 Generate all controls –

 Find neighbor configuration

 Solve car/tractor equations analytically

 Solve more bodies numerically (Runge-Kutta)

 Check neighbor configuration cell

 Visitation – use grid(s)

 Collision (more expensive)

  1 ,  1  min, max

Planner – Output

 Success or failure

 Roadmap from start to goal configuration

 Controls used to generate roadmap

Planner – Completeness

 Assumption: Start and goal configurations live in same connected component of free configuration space

Planner – Optimality (1)

 Assumption: some path exists with a finite number

of reversals for a single-body mobile robot

, ,

min max

min min

i i

Motion Planner – Complexity

 Overall –

Maximal size of search tree –

Node selection and insertion –

Collision checking –

 Space

 Configuration space grid (A) –

 Workspace collision checking grid –

Planner – Experimental Results

A



A

Planner – Hacks

 Configuration space grid halved

 Search straight paths when allowed by constraints

 Search node ties resolved with minimum length of curve of P1













 Planner results in unnecessarily long paths

 Could use different search algorithm/criteria

 Changing search criteria affects optimality result

 Exponential in time and space complexity: only practical for 1-2 bodies

 Use potential field techniques to guide search

 > 100x increase in memory size

 More efficient search

 Reduce the number of neighbors expanded

 Optimality results not applicable

Fruit That Was Rather Low

 Rapidly expanding Random Trees – Prof LeValle

 Adding dynamics to model

Questions?