CS 326 A: Motion PlanningCS 326 A: Motion Planninghttp://robotics.stanford.edu/~latombe/cs326/2004

Kinodynamic PlanningKinodynamic Planning

Underactuated RobotsUnderactuated 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 RobotExample: Car-Like Robot

yy

xx

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

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

Ldx/dt = v cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

Example: Car-Like RobotExample: Car-Like Robot

yy

xx

L

q = (x,y,)q’= dq/dt = (dx/dt,dy/dt,d/dt)dx sin – dy cos = 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 cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

Example: Car-Like RobotExample: Car-Like Robot

yy

xx

L

Lower-bounded turning radius

dx/dt = v cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

How Can This Work?How Can This Work?Tangent Space/Velocity Tangent Space/Velocity

SpaceSpace

x

y

(x,y,)

yy

xx

L

dx/dt = v cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

(dx,dy,d)

(dx,dy)

How Can This Work?How Can This Work?Tangent Space/Velocity Tangent Space/Velocity

SpaceSpace

x

y

(x,y,)

yy

xx

L

dx/dt = v cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

(dx,dy,d)

(dx,dy)

Nonholonomic Path Nonholonomic Path Planning ApproachesPlanning 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 TransformPath TransformHolonomic path

Nonholonomic path

Coverage of a Path by Coverage of a Path by CylindersCylinders

x

y

+q

q’

Type 1 ManeuverType 1 Maneuver

Allows sidewise motion

dqdq

(x1, y1, 0+)

(x3, y3, 0)

(x2, y2, 0+)

(x0, y0, 0)

d

CYL(x,y,,)

= 2/cosd = 2(1/cos - 1)

Type 2 ManeuverType 2 Maneuver

Allows pure rotation

CombinationCombination

Coverage of a Path by Coverage of a Path by CylindersCylinders

x

y

+q

q’

CombinationCombination

Path ExamplesPath 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 AlgebraControl Lie AlgebraLie Bracket [X,Y] = Basic maneuver based on 4 motions

X (t)

Y

-X

-Y

Control Lie AlgebraControl Lie AlgebraLie Bracket [X,Y] = Basic maneuver based on 4 motionsFor example: X: Going straight

Y: Turning, angle 0,sin,cos X

tan,sin,cos

LY

dx/dt = v cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

Control Lie AlgebraControl Lie AlgebraLie Bracket [X,Y] = Basic maneuver based on 4 motionsFor example:

X (t)

Y

-X

-Y

[X,Y] (t2 )

X: Going straight

Y: Turning, angle 0,sin,cos X

tan,sin,cos

LY

Control-Based SamplingControl-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

mb

mg

Control-Based SamplingControl-Based Samplingendgame region

ExampleExample

dx/dt = v cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

1. Select a milestone m2. Pick v, , and t3. Integrate motion from mnew configuration

Computed PathsComputed Paths

max=45o, min=22.5o

Car That Can Only Turn Left

max=45o

Tractor-trailer

Another ExampleAnother Example

Cooperative load carryingCooperative load carrying(Grasp Lab - U. Penn)(Grasp Lab - U. Penn)

Nonholonomic vs. Nonholonomic vs. Dynamic ConstraintsDynamic Constraints

Nonholonomic constraint: q’ = f(q,u)

where u is the control input (function of time), with dim(u) < dim(q)

dx/dt = v cos dy/dt = v sin ddt = (v/L) tan

| <

dx sin – dy cos = 0

u = (v, )

Nonholonomic vs. Nonholonomic vs. Dynamic ConstraintsDynamic 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 EquationGeneral Dynamic EquationFor 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. Nonholonomic vs. Dynamic ConstraintsDynamic 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 Space” Robot (ARL Lab)Lab)

air bearing

gas tankair thrusters

obstacles

robot

Modeling of RobotModeling of Robot

xx

y

f q = (x,y)s = (q,q’)u = (f,)x” = (f/m) cosy” = (f/m) sinf fmax

s’ = F(s,u)

mb

mg

PRM in State (x Time) PRM in State (x Time) SpaceSpace

The roadmap is a tree oriented along the time axis

endgame region

Computed PathComputed Path

Mean planning time: .002 s Mean number of milestones: 22

Another PathAnother Path

Mean planning time: .27s Mean number of milestones: 1946

Example with ReplanningExample with Replanning

11 22 33

44 55 66

77

Another Example with Another Example with ReplanningReplanning

Total duration : 40 sec

mb

R(mb)

Endgame region

Expansive SpaceExpansive Space

m

Rw(M)RRww(m)(m)

lookout pointlookout

Expansive SpaceExpansive SpaceVisibility Reachability

S

Visibility Reachability

Optimality of a TrajectoryOptimality of a Trajectory

Often one seeks a trajectory that optimizes a given criterion, e.g.:– smallest number of backup maneuvers, – minimal execution time, – minimal energy consumption

Bobrow’s paper+ variational techniques

Variational Path/Trajectory Variational Path/Trajectory OptimizationOptimization

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 re-compute the optimal control. If better value of criterion, vary further.

No performance guarantee regarding optimality of computed trajectory