Kinodynamic Planning
Using Probabalistic Road MapsSteven M. LaValle
James J. Kuffner, Jr.
Presented by Petter Frykman
What is the difference?
Dynamic equations of motion and physical constraints
What is the difference?
Dynamic equations of motion and physical constraints
Higher dimensional state space
qbkq uqm
q
qx
f(x,u)x
ux
m
-b
m
-k010
k
bu
m
v
v
x
x
goal start
v
How it works... We are now controling u instead of q Numerical integration of x = f(x,u)
By knowing:
We want to calculate:
Which could be done by a standars form of Runge-Kutta
asuming that u is constant
tttttu ´|´)()(tx
)( ttx
)'''''2'2)),(((6
)()( xxxutxft
txttx
Obstacles in the state space
Regular obstacles As we have seen them before
Region of Inevitable Collision Where no input we can give the robot can prevent a
collision
obst
ricobstric ,
v
Rapidly-exploring trees
Nothing else than we have seen beforeSelect a point in state space at randomSelect the point in the tree that is nearestTry to expand towards the new point
Find new state Given two points x, and x´ Try to find u that takes the robot from x
towards x´Reached
Found u that takes the robot all the way
Advanced Found u that takes the robot closer
Trapped Can’t find any u that is any good...
Metrics
Time and energy is two of the possible metrics used. The problem is that an ideal metric is often as difficult to find as solving the original problem.The performance and the solution depends very much on the choice of the metric ρ.
Nearest neighbor
This is also a bottleneck. The implementation used in the paper searches all states in the trees for the nearest one. There are other techniques that are better at doing this, at least approximatly. They often require some addisional data structure to represent the state space. This representation must be compatible with the problem in hand.
Bidirectional search
Good for faster algorithms Bad when time is explicitly needed
Experimental results
5 different experiments, 4 – 12 dimensions Control inputs were defined for each setup
Planar translating body 4DPlanar body with rotation 6DTranslating 3D body 6D3D satellite 12D3D spacecraft 12D
Planar translating body
1
0
0
,
1
0
0
,
0
0
1
,
0
0
1
fU
400 – 2500 nodes explored
Approximatly 5 seconds
Planar body with rotation
0
01.0
0
,
0
01.0
0
,
0
0
0
0
0
0
,
0
0
0
,
1
0
0
UU f
~13600 nodes explored
5 minutes
Translating 3D body
1
0
0
,
1
0
0
,
0
1
0
,
0
1
0
,
0
0
1
,
0
0
1
fU
~16300 nodes explored
1 minute
3D body with rotation, satellite
0
1
0
,
0
1
0
,
0
0
0
,
0
0
0
,
0
0
0
,
0
0
0
,
0
0
0
,
0
0
0
fU
0
0
0
,
0
0
0
,
01.0
0
0
,
01.0
0
0
,
0
01.0
0
,
0
01.0
0
,
0
0
01.0
,
0
0
01.0
U
~23800 nodes explored
6 minutes
3D body with rotation, space craft
0
0
0
,
0
0
0
,
0
25.0
0
,
0
25.0
0
,
5.0
0
0
fU
01.0
0
0
,
01.0
0
0
,
0
0
0
,
0
0
0
,
0
0
0
U
? nodes explored
11 minutes
Further development
More efficient metrics Efficient nearest neighbor Collision detection
David Hsu