CS 326A: Motion Planning Non-Holonomic Motion Planning

Coordination for Multiple Robots (Notes for HW#2)  n robots R1, …, Rn, with configuration spaces C1, …, Cn, sharing the same workspace  Problem: Plan coordinated motion so that each robot achieves its own goal configuration.  Centralized planning: Plan the coordinated motion in C1xC2x…xCn (but very high dimensional space)  Decoupled planning: Plan the motion of each robot ignoring the other robots; then coordinate their motions so that no two robots collide  Prioritized planning: Plan the motion of one robot ignoring the other robots; then plan the trajectory of a second robot in its configurationxtime space treating the first robot as a moving obstacle; then plan the trajectory of a third robot …

Coordination Space  2 robots R1 and R2  2 paths:  i : si  [0,1]  Ci (i=1,2)  2-D coordination space  Generalize to n robots  n-D coordination space s1 s2

Variants of Decoupled Planning  #1: Coordinate the n paths in n-D coordination space  #2: Coordinate paths of R1 and R2 in a 2-D coordination diagram (  path of “R1-R2”), then coordinate paths of R1-R2 and R3 in 2-D coordination diagram, etc… s1

Under-Actuated 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 make it possible to 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 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’= 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  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

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)

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

Type 1 Maneuver  Allows sidewise 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  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.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

Application

Summary Two planning approaches:  Path deformation: Fast but paths can be far from optimal. Restricted to “controllable” robots.  Control-based sampling: Can generate better paths, but slower. Can be scaled to higher dimensional space using probabilistic sampling techniques (next lecture)