Transcript CS 326 A: Motion Planning http://robotics.stanford.edu/~latombe/cs326/2004 Kinodynamic Planning Underactuated Robots  Fewer controls than dimensions in configuration space  What is a degree of.

CS 326 A: Motion Planning
http://robotics.stanford.edu/~latombe/cs326/2004
Kinodynamic Planning
Underactuated Robots
 Fewer controls than dimensions in configuration space
 What is a degree of freedom: number of dimensions of
C-space (global) or number of controls (local)?
How can m controls generate span a C-space with n>m
dimensions?
By exploiting mechanics properties:
- Rolling-with-no-sliding contact (friction), e.g.,: car,
bicycle, roller skate
- Conservation of angular momentum: satellite robot,
under-actuated robot, cat
- Others: submarine, plane, object pushing
Why is it useful?
- Fewer actuators (less weight)
- Design simplicity
- Convenience (think about driving a car with 3 controls!)
Example: Car-Like Robot
dx sinq – dy cosq = 0
f
y
q
f
L
dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
x
Configuration space is 3-dimensional: q = (x, y, q)
But control space is 2-dimensional: (v, f) with
|v| = sqrt[(dx/dt)2+(dy/dt)2]
Example: Car-Like Robot
dx sinq – dy cosq = 0
f
q
f
L
y
dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
x
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 nonintegrable equation of the form f(q,q’) = 0
Example: Car-Like Robot
dx sinq – dy cosq = 0
f
q
f
L
y
dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
x
Lower-bounded turning radius
How Can This Work?
Tangent Space/Velocity Space
f
q
q
(x,y,q)
f
L
y
(dx,dy,dq)
x
dx sinq – dy cosq = 0
dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
(dx,dy)
x
q
y
How Can This Work?
Tangent Space/Velocity Space
f
q
q
(x,y,q)
f
L
y
(dx,dy,dq)
x
dx sinq – dy cosq = 0
dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
(dx,dy)
x
q
y
Nonholonomic Path Planning
Approaches
 Two-phase planning:
(first paper)
 Compute collision-free path ignoring nonholonomic
constraints
 Transform this path into a nonholonomic one
 Efficient, but possible only if robot is “controllable”
 Plus need to have “good” set of maneuvers
 Direct planning: (second paper)
 Build a tree of milestones until one is close enough
to the goal (deterministic or randomized)
 Robot need not be controllable
 Works in high-dimensional c-spaces
Path Transform
Holonomic path
Nonholonomic path
Coverage of a Path by Cylinders
q
q
+ q’
y
x
Type 1 Maneuver
CYL(x,y,dq,h)
h
h= 2r/cosdq
d = 2r(1/cosdq - 1)
dq
dq
r
 Allows sidewise motion
Type 2 Maneuver
 Allows pure rotation
Combination
Coverage of a Path by Cylinders
q
q
+ q’
y
x
Combination
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)
Control Lie Algebra
Lie Bracket [X,Y] = Basic maneuver based on 4
motions
-X
-Y
Y
X (dt)
Control Lie Algebra
Lie Bracket [X,Y] = Basic maneuver based on 4
motions
For example:
dx sinq – dy cosq = 0
X: Going straight
X   cosq , sin q ,0
Y: Turning, angle f



Y   cosq , sin q , tanf 
L


dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
Control Lie Algebra
Lie Bracket [X,Y] = Basic maneuver based on 4
motions
For example:
-X
X: Going straight
X   cosq , sin q ,0
Y: Turning, angle f



Y   cosq , sin q , tanf 
L


-Y
[X,Y] (dt2 )
Y
X (dt)
Control-Based Sampling


Previous 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. The endpoint is the new milestone
 Tree-structured roadmaps
 Need for endgame regions
Control-Based Sampling
endgame region
mg
mb
Example
dx sinq – dy cosq = 0
dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
1. Select a milestone m
2. Pick v, f, and dt
3. Integrate motion from m
new configuration
Computed Paths
Car That Can Only Turn Left
Tractor-trailer
jmax=45o, jmin=22.5o
jmax=45o
Another Example
Cooperative load carrying
(Grasp Lab - U. Penn)
Nonholonomic vs.
Dynamic Constraints
 Nonholonomic constraint:
q’ = f(q,u)
where u is the control input (function of
time), with dim(u) < dim(q)
dx sinq – dy cosq = 0
dx/dt = v cos q
dy/dt = v sin q
dq/dt = (v/L) tan f
|f| < F
u = (v, f)
Nonholonomic vs.
Dynamic Constraints
 Nonholonomic constraint:
q’ = f(q,u)
where u is the control input (function of
time), with dim(u) < dim(q)
 Dynamic constraint:
 s = (q,q’), the state of the system
 s’ = f(s,u) where u is the control input
General Dynamic Equation
For an arbitrary mechanical linkage:
u = M(q)q” + C(q,q’) + G(q) + F(q,q’)
where:
- M is the inertia matrix
- C is the vector of centrifugal and
Coriolis terms
- G is the vector of gravity terms
- F is the vector of friction terms
+ constraints on u
Nonholonomic vs.
Dynamic Constraints
 Nonholonomic constraint:
q’ = f(q,u)
where u is the control input (function of
time), with dim(u) < dim(q)
 Dynamic constraint:
 s = (q,q’), the state of the system
 s’ = f(s,u) where u is the control input
 Similar techniques to handle nonholonomic and
dynamic constraints (kinodynamic planning)
“Space” Robot (ARL Lab)
robot
obstacles
air thrusters
gas tank
air bearing
Modeling of Robot
f
y
x
a
q = (x,y)
s = (q,q’)
u = (f,a)
x” = (f/m) cosa
y” = (f/m) sina
f  fmax
s’ = F(s,u)
PRM in State (x Time) Space
endgame region
mg
mb
The roadmap is a tree oriented along the time axis
Computed Path
Mean planning time: .002 s
Mean number of milestones: 22
Another Path
Mean planning time: .27s
Mean number of milestones: 1946
Example with Replanning
1
2
3
4
5
6
7
Another Example with Replanning
Total duration : 40 sec
Expansive Space
R(mb)
mb
Rw(m)
Rw(M)
m
lookout
Endgame region
lookout point
Expansive Space
Visibility  Reachability
Visibility
S
Reachability
Optimality of a Trajectory
Often one seeks a trajectory that
optimizes a given criterion, e.g.:
– smallest number of backup maneuvers,
– minimal execution time,
Bobrow’s paper
– minimal energy consumption
+ variational
techniques
Variational Path/Trajectory
Optimization
 Steepest descent technique.
 Parameterize the geometry of a trajectory,
e.g., by defining control points through which
cubic spines are fitted.
 Vary the parameters. For the new values recompute the optimal control. If better value
of criterion, vary further.

No performance guarantee regarding
optimality of computed trajectory