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.
Download
Report
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