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 f k (q)u k Vector fieldsControls 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 = v dv/dt = f / M

Example: 1D Point Mass x v dx/dt = dv dv/dt = f / M Solution: v(t) = v(0)+t f /M x(t) = x(0)+t v(0) + ½ t 2 f / M x v f=0

Example: 1D Point Mass x v x v f>0 dx/dt = dv dv/dt = f / M Solution: v(t) = v(0)+t f /M x(t) = x(0)+t v(0) + ½ t 2 f / M

Example: 1D Point Mass x v x v f<0 dx/dt = dv dv/dt = f / M Solution: v(t) = v(0)+t f /M x(t) = x(0)+t v(0) + ½ t 2 f / M

Example: 1D Point Mass x v x v |f|<=f max dx/dt = dv dv/dt = f / M Solution: v(t) = v(0)+t f /M x(t) = x(0)+t v(0) + ½ t 2 f / M

Example: Car-Like Robot y x Configuration 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 cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0

Example: Car-Like Robot q = (x,y,q) q’= dq/dt = (dx/dt,dy/dt,d q /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 cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0 y x L    

Example: Car-Like Robot dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  dx sin  – dy cos  = 0 y x L     Lower-bounded turning radius

How Can This Work? Tangent Space/Velocity Space x y  (x,y,  )  (dx,dy,d  ) (dx,dy) y x L     dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | < 

x y  (x,y,  )  (dx,dy,d  ) (dx,dy) dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  y x 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 x1x1 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 Deformation Holonomic 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 SLTC q 

Lie Bracket Maneuver made of 4 motions X (  t) Y -X -Y

Lie Bracket Maneuver made of 4 motions For example: dx/dt = v cos  dy/dt = v sin  d  dt = (v/L) tan    | <  X : Going straight Y : Turning, angle  T T

Maneuver made of 4 motions For example: X (  t) Y -X -Y [X,Y] (  t 2 ) Lie bracket Lie Bracket X : Going straight Y : Turning, angle  T T

[X,Y] = dY.X – dX.Y  X 1 /  x  X 1 /  y  X 1 /  dX =  X 2 /  x  X 2 /  y  X 1 /   X 2 /  x  X 2 /  y  X 2 /  X (  t) Y -X -Y [X,Y] (  t 2 ) 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    (x,y) q CYL(x,y, ,  )  = 2   tan  d = 2  (1/ cos  1) > 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

CC|C 0 CC|CC|CS 0 C|C Given any two configurations, the shortest RS paths between them 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 m 2. Pick v, , and  t 3. Integrate motion from m  new milestone m’ dx/dt = v cos  dy/dt = v sin  d  dt = (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 = 45 o,  min = 22.5 o Car That Can Only Turn Left  max = 45 o Tractor-trailer

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