non-holonomic motion planning

Non-Holonomic Motion Planning

Probabilistic Roadmaps

What if omnidirectional motion in C-space is not permitted?

Paths for a Car-Like Robot

Agenda

Differential constraints Dynamics, nonholonomic systems

dq/dt = k fk(q)uk

Vector fields Controls

We’ll consider fewer control dimensions than state dimensions

Example: 1D Point Mass

Mass M

2D configuration space (state space) Controlled force f Equations of motion:

x

v

dx/dt = vdv/dt = f / M

Example: 1D Point Massx

v

dx/dt = dvdv/dt = f / M

Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M

x

v

f=0

Example: 1D Point Massx

v

x

v

f>0

dx/dt = dvdv/dt = f / M

Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M

Example: 1D Point Massx

v

x

v

f<0

dx/dt = dvdv/dt = f / M

Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M

Example: 1D Point Massx

v

x

v

|f|<=fmax

dx/dt = dvdv/dt = f / M

Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M

Example: Car-Like Robot

yy

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

But control space is 2-dimensional: (v, ) with |v| = sqrt[(dx/dt)2+(dy/dt)2]

L

dx/dt = v cosdy/dt = v sin

ddt = (v/L) tan

| <

dx sin – dy cos = 0

Example: Car-Like Robot

q = (x,y,q)q’= dq/dt = (dx/dt,dy/dt,dq/dt)dx sinq – dy cosq = 0 is a particular form of f(q,q’)=0

A robot is nonholonomic if its motion is constrained by a non-integrable equation of the form f(q,q’) = 0

dx/dt = v cosdy/dt = v sin

ddt = (v/L) tan

| <

dx sin – dy cos = 0

yy

xx

L

Example: Car-Like Robot

dx/dt = v cosdy/dt = v sin

ddt = (v/L) tan

| <

dx sin – dy cos = 0

yy

xx

L

Lower-bounded turning radius

How Can This Work?Tangent Space/Velocity Space

x

y

(x,y,)

(dx,dy,d)

(dx,dy)

yy

xx

L

dx/dt = v cosdy/dt = v sin

ddt = (v/L) tan

| <

x

y

(x,y,)

(dx,dy,d)

(dx,dy)dx/dt = v cosdy/dt = v sin

ddt = (v/L) tan

| <

yy

xx

L

How Can This Work?Tangent Space/Velocity Space

Kinematic Reduction of 1D Point Mass If:

Constraints are time-invariant Starts and stops at rest

Then we only need to consider position

x1 x1+1. Speed up

2. Slow down

Dynamic Execution of Kinematic Paths For fully actuated N-D robots, a collision-free

path can be executed “slowly enough”

x

v

Nonholonomic Path Planning Approaches

Two-phase planning (path deformation):• Compute collision-free path ignoring nonholonomic

constraints• Transform this path into a nonholonomic one• Efficient, but possible only if robot is “controllable”• Need for a “good” set of maneuvers

Direct planning (control-based sampling):• Use “control-based” sampling to generate a tree of

milestones until one is close enough to the goal (deterministic or randomized)

• Robot need not be controllable• Applicable to high-dimensional c-spaces

Path DeformationHolonomic path

Nonholonomic path

Small-Time Local Controllability

A system is SLTC if at a given configuration q, it can reach an open neighborhood of q

Not SLTC Not SLTCSLTC

q

Lie BracketManeuver made of 4 motions

X (t)

Y

-X

-Y

Lie BracketManeuver made of 4 motions

For example:

dx/dt = v cosdy/dt = v sin

ddt = (v/L) tan

| <

X: Going straight

Y: Turning, angle

0,sin,cos X

tan,sin,cos

LY

T

T

Maneuver made of 4 motions

For example:

X (t)

Y

-X

-Y

[X,Y] (t2 )

Lie bracket

Lie Bracket

X: Going straight

Y: Turning, angle

0,sin,cos X

tan,sin,cos

LY

T

T

[X,Y] = dY.X – dX.Y

X1/x X1/y X1/

dX = X2/x X2/y X1/

X2/x X2/y X2/

X (t)

Y

-X

-Y

[X,Y] (t2 )

Lie bracket

Lie Bracket

[X,Y] Lin(X,Y) the motion constraint is nonholonomic

Tractor-Trailer Example

4-D configuration space 2-D control/velocity space two independent

velocity vectors X and Y U = [X,Y] Lin(X,Y) V = [X,U] Lin(X,Y,U)

Lie Bracket Maneuvers

Problem: To move distance along vector field U=[X,Y] using Lie bracket, may need to move much farther in C-space

Solution: Use better maneuvers

Type 1 Maneuver

Allows sideways motion

dq

dq

(x1, y1, 0+)

(x3, y3, 0)

(x2, y2, 0+)

(x0, y0, 0)

d

(x,y)

q

CYL(x,y,,)

= 2tand = 2(1/cos1) > 0

(x,y,)

When 0, so does d and the cylinder becomes arbitrarily small

Type 2 Maneuver

Allows pure rotation

Combination

Coverage of a Path by Cylinders

x

y

+q q’

Path Examples

Drawbacks of Two-phase Planning Final path can be far from optimal

Not applicable to robots that are not locally controllable (e.g., car that can only move forward)

Reeds and Shepp Paths

Reeds and Shepp Paths

CC|C0 CC|C C|CS0C|C

Given any two configurations,the shortest RS paths betweenthem is also the shortest path

Example of Generated Path

Holonomic

Nonholonomic

Path Optimization

Nonholonomic Path Planning Approaches

Two-phase planning (path deformation):• Compute collision-free path ignoring nonholonomic

constraints• Transform this path into a nonholonomic one• Efficient, but possible only if robot is “controllable”• Need for a “good” set of maneuvers

Direct planning (control-based sampling):• Use “control-based” sampling to generate a tree of

milestones until one is close enough to the goal (deterministic or randomized)

• Robot need not be controllable• Applicable to high-dimensional c-spaces

Control-Based Sampling

PRM sampling technique: Pick each milestone in some region

Control-based sampling:1. Pick control vector (at random or not)2. Integrate equation of motion over short duration (picked

at random or not)3. If the motion is collision-free, then the endpoint is the new

milestone

Tree-structured roadmaps Need for endgame regions

Example

1. Select a milestone m2. Pick v, , and t3. Integrate motion from m new milestone m’

dx/dt = v cosdy/dt = v sin

ddt = (v/L) tan

| <

Example Indexing array: A 3-D grid is placed over the configuration space. Each milestone falls into one cell of the grid. A maximum number of milestones is allowed in each cell (e.g., 2 or 3).

Asymptotic completeness: If a path exists, the planner is guaranteed to find one if the resolution of the grid is fine enough.

Computed Paths

max=45o, min=22.5o

Car That Can Only Turn Left

max=45o

Tractor-trailer

Readings

P.R.M. Chapter 7.2 LaValle and Kuffner (2001) Hsu et al (2001)