Non-Holonomic Motion Planning
description
Transcript of Non-Holonomic Motion Planning
![Page 1: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/1.jpg)
Non-Holonomic Motion Planning
![Page 2: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/2.jpg)
Probabilistic Roadmaps
What if omnidirectional motion in C-space is not permitted?
![Page 3: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/3.jpg)
Paths for a Car-Like Robot
![Page 4: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/4.jpg)
Agenda
Differential constraints Dynamics, nonholonomic systems
dq/dt = k fk(q)uk
Vector fields Controls
We’ll consider fewer control dimensions than state dimensions
![Page 5: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/5.jpg)
Example: 1D Point Mass
Mass M
2D configuration space (state space) Controlled force f Equations of motion:
x
v
dx/dt = vdv/dt = f / M
![Page 6: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/6.jpg)
Example: 1D Point Massx
v
dx/dt = dvdv/dt = f / M
Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M
x
v
f=0
![Page 7: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/7.jpg)
Example: 1D Point Massx
v
x
v
f>0
dx/dt = dvdv/dt = f / M
Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M
![Page 8: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/8.jpg)
Example: 1D Point Massx
v
x
v
f<0
dx/dt = dvdv/dt = f / M
Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M
![Page 9: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/9.jpg)
Example: 1D Point Massx
v
x
v
|f|<=fmax
dx/dt = dvdv/dt = f / M
Solution:v(t) = v(0)+t f /Mx(t) = x(0)+t v(0) + ½ t2 f / M
![Page 10: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/10.jpg)
Example: Car-Like Robot
yy
xxConfiguration 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 cosdy/dt = v sin
ddt = (v/L) tan
| <
dx sin – dy cos = 0
![Page 11: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/11.jpg)
Example: Car-Like Robot
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 non-integrable equation of the form f(q,q’) = 0
dx/dt = v cosdy/dt = v sin
ddt = (v/L) tan
| <
dx sin – dy cos = 0
yy
xx
L
![Page 12: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/12.jpg)
Example: Car-Like Robot
dx/dt = v cosdy/dt = v sin
ddt = (v/L) tan
| <
dx sin – dy cos = 0
yy
xx
L
Lower-bounded turning radius
![Page 13: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/13.jpg)
How Can This Work?Tangent Space/Velocity Space
x
y
(x,y,)
(dx,dy,d)
(dx,dy)
yy
xx
L
dx/dt = v cosdy/dt = v sin
ddt = (v/L) tan
| <
![Page 14: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/14.jpg)
x
y
(x,y,)
(dx,dy,d)
(dx,dy)dx/dt = v cosdy/dt = v sin
ddt = (v/L) tan
| <
yy
xx
L
How Can This Work?Tangent Space/Velocity Space
![Page 15: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/15.jpg)
Kinematic Reduction of 1D Point Mass If:
Constraints are time-invariant Starts and stops at rest
Then we only need to consider position
x1 x1+1. Speed up
2. Slow down
![Page 16: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/16.jpg)
Dynamic Execution of Kinematic Paths For fully actuated N-D robots, a collision-free
path can be executed “slowly enough”
x
v
![Page 17: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/17.jpg)
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
![Page 18: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/18.jpg)
Path DeformationHolonomic path
Nonholonomic path
![Page 19: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/19.jpg)
Small-Time Local Controllability
A system is SLTC if at a given configuration q, it can reach an open neighborhood of q
Not SLTC Not SLTCSLTC
q
![Page 20: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/20.jpg)
Lie BracketManeuver made of 4 motions
X (t)
Y
-X
-Y
![Page 21: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/21.jpg)
Lie BracketManeuver made of 4 motions
For example:
dx/dt = v cosdy/dt = v sin
ddt = (v/L) tan
| <
X: Going straight
Y: Turning, angle
0,sin,cos X
tan,sin,cos
LY
T
T
![Page 22: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/22.jpg)
Maneuver made of 4 motions
For example:
X (t)
Y
-X
-Y
[X,Y] (t2 )
Lie bracket
Lie Bracket
X: Going straight
Y: Turning, angle
0,sin,cos X
tan,sin,cos
LY
T
T
![Page 23: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/23.jpg)
[X,Y] = dY.X – dX.Y
X1/x X1/y X1/
dX = X2/x X2/y X1/
X2/x X2/y X2/
X (t)
Y
-X
-Y
[X,Y] (t2 )
Lie bracket
Lie Bracket
[X,Y] Lin(X,Y) the motion constraint is nonholonomic
![Page 24: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/24.jpg)
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)
![Page 25: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/25.jpg)
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
![Page 26: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/26.jpg)
Type 1 Maneuver
Allows sideways motion
dq
dq
(x1, y1, 0+)
(x3, y3, 0)
(x2, y2, 0+)
(x0, y0, 0)
d
(x,y)
q
CYL(x,y,,)
= 2tand = 2(1/cos1) > 0
(x,y,)
When 0, so does d and the cylinder becomes arbitrarily small
![Page 27: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/27.jpg)
Type 2 Maneuver
Allows pure rotation
![Page 28: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/28.jpg)
Combination
![Page 29: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/29.jpg)
Coverage of a Path by Cylinders
x
y
+q q’
![Page 30: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/30.jpg)
Path Examples
![Page 31: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/31.jpg)
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)
![Page 32: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/32.jpg)
Reeds and Shepp Paths
![Page 33: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/33.jpg)
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
![Page 34: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/34.jpg)
Example of Generated Path
Holonomic
Nonholonomic
![Page 35: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/35.jpg)
Path Optimization
![Page 36: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/36.jpg)
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
![Page 37: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/37.jpg)
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
![Page 38: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/38.jpg)
Example
1. Select a milestone m2. Pick v, , and t3. Integrate motion from m new milestone m’
dx/dt = v cosdy/dt = v sin
ddt = (v/L) tan
| <
![Page 39: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/39.jpg)
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.
![Page 40: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/40.jpg)
Computed Paths
max=45o, min=22.5o
Car That Can Only Turn Left
max=45o
Tractor-trailer
![Page 41: Non-Holonomic Motion Planning](https://reader034.fdocuments.us/reader034/viewer/2022052414/56815a91550346895dc8058f/html5/thumbnails/41.jpg)
Readings
P.R.M. Chapter 7.2 LaValle and Kuffner (2001) Hsu et al (2001)