CS 326A: Motion Planning

1. CS 326A: Motion Planning Non-Holonomic Motion Planning

2. 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 …

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

4. 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

5. 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)?

6. How can m controls make it possible to span a C-space with n > mdimensions? 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 pushingWhy is it useful?- Fewer actuators (less weight)- Design simplicity • - Convenience (think about driving a car with 3 controls!)

7. f dx/dt = v cosq dy/dt = v sinq dx sinq – dy cosq = 0 q dq/dt = (v/L) tan f f |f| <F Example: Car-Like Robot L q y 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]

8. f dx/dt = v cosq dy/dt = v sinq dx sinq – dy cosq = 0 L q dq/dt = (v/L) tan f q f y |f| <F x Example: Car-Like Robot q = (x,y,q) q’= dq/dt = (dx/dt,dy/dt,dq/dt)dx sinq – dy cosq = 0is a particular form off(q,q’)=0 A robot is nonholonomic if its motion is constrained by a non-integrable equation of the form f(q,q’) = 0

9. f dx/dt = v cosq dy/dt = v sinq dx sinq – dy cosq = 0 q dq/dt = (v/L) tan f f |f| <F Example: Car-Like Robot L q y x Lower-bounded turning radius

10. f q (x,y,q) L (dx,dy,dq) q q f y x y dx/dt = v cosq dy/dt = v sinq (dx,dy) x q dq/dt = (v/L) tan f |f|<F How Can This Work?Tangent Space/Velocity Space

11. f q (x,y,q) L (dx,dy,dq) q q f y x y dx/dt = v cosq dy/dt = v sinq (dx,dy) x q dq/dt = (v/L) tan f |f|<F How Can This Work?Tangent Space/Velocity Space

12. Lie Bracket Maneuver made of 4 motions -X -Y Y X (dt)

13. X: Going straight Y: Turning, angle f T T dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f|<F Lie Bracket Maneuver made of 4 motions For example:

14. X: Going straight Y: Turning, angle f T -X -Y Y T X (dt) [X,Y] (dt2 ) Lie Bracket Maneuver made of 4 motions For example: Lie bracket

15. -X -Y Y X (dt) [X,Y] (dt2 ) Lie Bracket [X,Y] = dY.X – dX.Y X1/x X1/y X1/q dX = X2/x X2/y X1/q X2/x X2/y X2/q [X,Y] Lin(X,Y)  the motion constraint is nonholonomic Lie bracket

16. Tractor-Trailer Example • 4-D configuration space • 2-D control/velocity space •  two independentvelocity vectors X and Y • U = [X,Y]  Lin(X,Y) • V = [X,U]  Lin(X,Y,U)

17. 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

18. Holonomic path Nonholonomic path Path Deformation

19. CYL(x,y,dq,h) h h dq (x,y,q) • = 2rtandq d = 2r(1/cosdq - 1) > 0 (x,y) Type 1 Maneuver q r dq dq  Allows sidewise motion r When dq 0, so does d and the cylinder becomes arbitrarily small

20. Type 2 Maneuver  Allows pure rotation

21. Combination

22. q q’ + Coverage of a Path by Cylinders q y x

23. Path Examples

24. Drawbacks ofTwo-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)

25. Reeds and Shepp Paths

26. Reeds and Shepp Paths CC|C0 CC|C C|CS0C|C Given any two configurations,the shortest RS paths betweenthem is also the shortest path

27. Example of Generated Path Holonomic Nonholonomic

28. Path Optimization

29. 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

30. Control-Based Sampling • Previous sampling technique: Pick each milestone in some region • Control-based sampling: • Pick control vector (at random or not) • Integrate equation of motion over short duration (picked at random or not) • If the motion is collision-free, then the endpoint is the new milestone • Tree-structured roadmaps • Need for endgame regions

31. dx/dt = v cosq dy/dt = v sinq dq/dt = (v/L) tan f |f|<F Example 1. Select a milestonem 2. Pickv, f, anddt 3. Integrate motion from m  new milestone m’

32. 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.

33. Computed Paths

34. Computed Paths Tractor-trailer Car That Can Only Turn Left jmax=45o, jmin=22.5o jmax=45o

35. Application

36. 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)