Non-Holonomic Motion Planning
description
Transcript of 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)