![Page 1: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/1.jpg)
16.412/6.834J Cognitive Robotics February 7th, 2005
Probabilistic Methods for
Kinodynamic Path Planning
Based on Past Student Lectures by: Lecturer: Paul Elliott, Aisha Walcott,
Nathan Ickes and Stanislav Funiak Prof. Brian C. Williams
![Page 2: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/2.jpg)
How do we maneuver or manipulate?
courtesy NASA JSC
courtesy NASA Ames
![Page 3: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/3.jpg)
Outline
� Roadmap path planning
� Probabilistic roadmaps
� Planning in the real world
� Planning amidst moving obstacles
� RRT-based planners
� Conclusions
![Page 4: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/4.jpg)
Outline
• Roadmap path planning
• Probabilistic roadmaps
• Planning in the real world
• Planning amidst moving obstacles
• RRT-based planners
• Conclusions
Brian Williams, Fall 03
![Page 5: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/5.jpg)
Path Planning through Obstacles
Start
position
Goal Brian Williams, Fall 03 position
![Page 6: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/6.jpg)
1. Create Configuration SpaceAssume: Vehicle
Start
position
translates, but no rotation
Idea: Transform to equivalent Goal
positionproblem of navigating a point. Brian Williams, Fall 03
![Page 7: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/7.jpg)
2. Map From Continuous Problem to a
Roadmap: Create Visibility GraphStart
position
Goal Brian Williams, Fall 03 position
![Page 8: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/8.jpg)
2. Map From Continuous Problem to a
Roadmap: Create Visibility GraphStart
position
Goal Brian Williams, Fall 03 position
![Page 9: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/9.jpg)
3. Plan Shortest Path
Start
position
Goal Brian Williams, Fall 03 position
![Page 10: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/10.jpg)
Resulting Solution
Start
position
Goal Brian Williams, Fall 03 position
![Page 11: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/11.jpg)
A Visibility Graph is One Kind of Roadmap
Start
position
What are some other types of roadmaps?
Goal Brian Williams, Fall 03 position
![Page 12: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/12.jpg)
Roadmaps: Voronoi Diagrams
Brian Williams, Fall 03
Lines equidistant from CSpace obstacles
![Page 13: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/13.jpg)
Roadmaps: Approximate Fixed Cell
Brian Williams, Fall 03
![Page 14: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/14.jpg)
Roadmaps: Approximate Fixed Cell
Brian Williams, Fall 03
![Page 15: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/15.jpg)
Roadmaps: Exact Cell Decomposition
Brian Williams, Fall 03
![Page 16: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/16.jpg)
Khatib 1986Potential Functions Latombe 1991
Koditschek 1998
Attractive Potential Repulsive Potential Combined Potential for goals for obstacles Field
Move along force: F(x) = �Uatt(x)-�Urep(x)
Brian Williams, Fall 03
![Page 17: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/17.jpg)
Exploring Roadmaps
• Shortest path
– Dijkstra’s algorithm
– Bellman-Ford algorithm
– Floyd-Warshall algorithm
– Johnson’s algorithm
• Informed search
– Uniform cost search
– Greedy search
– A* search
– Beam search
– Hill climbing
Brian Williams, Fall 03
![Page 18: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/18.jpg)
Robonaut Teamwork: Tele-robotic
•High dimensional state space
•Controllability and dynamics
•Safety and complianceBrian Williams, Fall 03
![Page 19: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/19.jpg)
Outline
� Roadmap path planning
� Probabilistic roadmaps
� Planning in the real world
� Planning amidst moving obstacles
� RRT-based planners
� Conclusions
![Page 20: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/20.jpg)
Applicability of Lazy
Probabilistic Road Maps
to Portable Satellite Assistant
By Paul Elliott
![Page 21: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/21.jpg)
courtesy NASA Ames
![Page 22: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/22.jpg)
Zvezda Service Module
Idea: Probabilistic Roadmaps
• Search randomly generated roadmap
• Probabilistically complete
• Trim infeasible edges and nodes lazily
![Page 23: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/23.jpg)
Place Start and Goal
Goal
Start
![Page 24: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/24.jpg)
Place Nodes Randomly
![Page 25: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/25.jpg)
Select a Set of Neighbors
![Page 26: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/26.jpg)
A* Search
16
18
18
2
1
2
![Page 27: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/27.jpg)
A* Search
![Page 28: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/28.jpg)
A* Search
![Page 29: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/29.jpg)
Check Feasible Nodes
![Page 30: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/30.jpg)
Check Feasible Nodes
![Page 31: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/31.jpg)
Check Feasible Nodes
![Page 32: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/32.jpg)
A* Search
![Page 33: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/33.jpg)
Check Feasible Edges
![Page 34: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/34.jpg)
A* Search
![Page 35: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/35.jpg)
Lazy PRM Algorithm
�
�
�
�
Build Roadmap
Start and Goal Nodes
Uniform Dist Nodes
Nearest Neighbors
qinit, qgoal
Build Roadmap
Shortest
Path (A*)
Node
Enhancement
Remove Colliding
node/edge No path found
Check Nodes Collision
Check EdgesCollision
![Page 36: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/36.jpg)
Lazy PRM Algorithm
�
�
the goal
�
Shortest Path (A*)
Heuristic = distance to
Path length = distance
between nodes
qinit, qgoal
Build Roadmap
Shortest
Path (A*)
Node
Enhancement
Remove Colliding
node/edge
Check Nodes
Check Edges
Collision
Collision
No path found
![Page 37: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/37.jpg)
Lazy PRM Algorithm
� Check Nodes & Edges
� Search from Start and
End for collisions
� First check Nodes
then Edges
qinit, qgoal
Build Roadmap
Shortest
Path (A*)
Node
Enhancement
Check Nodes
Check Edges
Remove Colliding
node/edge
Collision
Collision
No path found
![Page 38: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/38.jpg)
Lazy PRM Algorithm
�
�
edges
�
Remove Node/Edge
For Nodes, remove all
For Edges, just
remove the edge
qinit, qgoal
Build Roadmap
Shortest
Path (A*)
Node
Enhancement
Check Nodes
Check Edges
Remove Colliding
node/edge
Collision
Collision
No path found
![Page 39: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/39.jpg)
Lazy PRM Algorithm
�
�
�
edges
Node Enhancement
Add ½ uniformly
Add ½ clustered around
midpoints of removed
qinit, qgoal
Build Roadmap
Shortest
Path (A*)
Node
Enhancement
Remove Colliding
node/edge No path found
Check Nodes Collision
Check EdgesCollision
![Page 40: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/40.jpg)
PRMs Fall Short For Dynamical Systems
� Using PRM
1. Construct roadmap
2. A* finds path in
roadmap
3. Must derive control
inputs from path
� Cannot always find
inputs for an arbitrary
path
![Page 41: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/41.jpg)
Outline
� Roadmap path planning
� Probabilistic roadmaps
� Planning in the real world
� Planning amidst moving obstacles
� RRT-based planners
� Conclusions
![Page 42: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/42.jpg)
Path Planning in the Real World
Real World Robots
� Have inertia
� Have limited controllability
� Have limited sensors� Have limited sensors
� Face a dynamic environment
� Face an unreliable environment� Face an unreliable environment
Static planners (e.g. PRM) are not sufficientStatic planners (e.g. PRM) are not sufficient
![Page 43: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/43.jpg)
Two Approaches to Path Planning
Kinematic: only concerned with motion, without regard to the forces that cause it
� Works well: when position controlled directly.
� Works poorly: for systems with significant inertia.
Kinodynamic: incorporates dynamic constraints
� Plans velocity as well as position
![Page 44: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/44.jpg)
Representing Static State
� Configuration space represents the position
and orientation of a robot
� Sufficient for static planners like PRM
TExample: Steerable car
yConfiguration space
(x, y, T)
x
![Page 45: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/45.jpg)
Representing Dynamic State
� State space incorporates robot dynamic state
� Allows expression of dynamic constraints
� Doubles dimensionality
Example: Steerable car
yState space . . .
X = (x, y, T, x, y, T)Constraints
•max velocity, min turn x
T
•car dynamics
![Page 46: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/46.jpg)
Incorporating Dynamic Constraints
� For some states, collision is unavoidable
� Robot actuators can apply limited force
Obstacle
Imminent
collision
region
� Path planner should avoid these states
![Page 47: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/47.jpg)
Regions in State Space
� Collision regions: Xcoll
� Clearly illegal
� Region of Imminent Collision: Xric
� Where robot’s actuators cannot prevent a collision
� Free Space: Xfree = X – (Xcoll + Xric)
Xfree XcollXric
� Collision-free planning involves finding paths that lie entirely in Xfree
![Page 48: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/48.jpg)
Constraints on Maneuvering
� Nonholonomic: Fewer controllable degrees of freedom then total degrees of freedom
� Example: steerable car
� 3 dof (x, y, T), but only
� 1 controllable dof (steering angle)
. .� Equation of Motion: G( s, s ) = 0
� Constraint is a function of state and time derivative of state
![Page 49: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/49.jpg)
Outline
� Roadmap path planning
� Probabilistic roadmaps
� Planning in the real world
� Planning amidst moving obstacles
� RRT-based planners
� Conclusions
![Page 50: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/50.jpg)
Problem
� Kinodynamic motion planning amidst
moving obstacles with known trajectories
� Example: Asteroid avoidance problem
� Moving Obstacle Planner (MOP)
� Extension to PRM
![Page 51: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/51.jpg)
MOP Overview
Similar to PRM, except
� Does not pre-compute the roadmap
� Incrementally constructs the roadmap by extending it from existing nodes
� Roadmap is a directed tree rooted at initial state u time point and oriented along time axis
![Page 52: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/52.jpg)
Building the Roadmap
1. Randomly choose an
existing node
2.
input u
3.
4.
motion
Nodes (state, time)
Randomly choose
existing node
Select control
input u
Randomly select control
Randomly select integration
time interval G �[0, G max ]
Integrate equations of
Collision- free?
at random
Integrate equations of motion from an existing
node with respect to u for some time interval G
![Page 53: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/53.jpg)
Building the Roadmap (cont.)
5. If edge is collision-free then
6. Store control input with new edge
7. Add new node to roadmap
Collision-free trajectory
Store control input u
with new edge
u Add new node
Result: Any trajectory along tree satisfies
motion constraints and is collision-free!
Nodes (state, time)
![Page 54: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/54.jpg)
Solution Trajectory
1. If goal is reached then
2. Proceed backwards from
the goal to the start Goal state and time
(sstart, tstart)
Start state and time
(sgoal, tgoal)
![Page 55: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/55.jpg)
MOP details: Inputs and Outputs
Planning Query:
� t
� Let (sstart , tstart ) denote the robot’s start point in the state u time space, and (sgoal , tgoal ) denote the goal
goal �Igoal , where I is some time interval in whichgoal
the goal should be reached
Solution Trajectory:
� Finite sequence of fixed control inputs applied over a specified duration of time
� Avoids moving obstacles by indexing each state with the time when it is attained
� Obeys the dynamic constraints
![Page 56: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/56.jpg)
MOP details: Roadmap Construction
� Objective: obtain new node (s’, t’)
� s’ = the new state in the robot’s state space
� t’ = t + G, current time plus the integration time
Each iteration:
1. Select an existing node (s, t) in the roadmap at
random
2. Select control input u at random
3. Select integration time G at random from [0, G max]
![Page 57: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/57.jpg)
MOP details: Roadmap Construction
3. Integrate control inputs over time interval
4. Edge between (s, t) and (s’, t’) is checked
for collision with static obstacles and
moving obstacles
5. If collision-free, store control input u with
the new edge
6. (s’, t’) is accepted as new node
![Page 58: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/58.jpg)
MOP details: Uniform Distribution
Modify to Ensure
Uniform Distribution
of Space:
� Why? If existing roadmap
nodes were selected
uniformly, the planner
would pick a node in an
already densely sampled
region
� Avoid oversampling of
any region by dividing the
stateutime space into bins
![Page 59: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/59.jpg)
Achieving Uniform Node Distribution
Space 1. Equally divide space
2. Denote each section
as a bin; number each
bin
*bins store roadmap nodes
that lie in their region
bin 1 bin 2 bin 3 bin 4 bin 5 bin 6 bin 7
bin 8 bin 9 bin 10 bin 11 bin 12 bin 13 bin 14
. . . . . .
.
.
.
![Page 60: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/60.jpg)
Achieving Uniform Node Distribution
Array
Equal-sized bins
bin 1 bin 2 bin 3 ….Existing nodes
3. Create an array of
bins
![Page 61: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/61.jpg)
Achieving Uniform Node Distribution
Planner randomly picks a
bin with at least one node
At that bin, the planner
picks a node at random
bin 1 bin 2 bin 3 ….
![Page 62: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/62.jpg)
Achieving Uniform Node Distribution
into its
corresponding bin
Insert new node
![Page 63: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/63.jpg)
Demonstration of MOP
•Point–mass robot moving in a planex x
•State s = (x, y, x, y )
(sstart , tstart)
(sgoal , tgoal)
Moving obstacles
Static obstacle
Point-mass robot
![Page 64: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/64.jpg)
Demonstration of MOP
![Page 65: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/65.jpg)
Summary
� MOP algorithm incrementally builds a roadmap in the stateutime space
� The roadmap is a directed tree oriented along the time axis
� By including time the planner is able to generate a solution trajectory that
� avoids moving and static obstacles
� obeys the dynamic constraints
� Bin technique to ensure that the space is explored somewhat uniformly
![Page 66: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/66.jpg)
Outline
� Roadmap path planning
� Probabilistic roadmaps
� Planning in the real world
� Planning amidst moving obstacles
� RRT-based planners
� Conclusions
![Page 67: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/67.jpg)
Planning with RRTs
� RRTs: Rapidly-exploring Random Trees
� Similar to MOP
� Incrementally builds the roadmap tree
� Integrates the control inputs to ensure that the
kinodynamic constraints are satisfied
� Informed exploration strategy from MOP
� Extends to more advanced planning
techniques
![Page 68: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/68.jpg)
How it Works
� Build RRT in state space (X), starting at sstart
� Stop when tree gets sufficiently close to sgoal
Goal
Start
![Page 69: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/69.jpg)
Building an RRT
� To extend an RRT:
� Pick a random point a
in X
� Find b, the node of the
tree closest to a
� Find control inputs u to
steer the robot from b
to a
b u
a
![Page 70: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/70.jpg)
Building an RRT
� To extend an RRT (cont.)
� Apply control inputs
u for time G, so robot
reaches c
� If no collisions occur
in getting from a to c,
add c to RRT and
record u with new
edge
b u
c a
![Page 71: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/71.jpg)
Executing the Path
Once the RRT reaches sgoal � Backtrack along tree to identify edges that
lead from s to sstart goal
� Drive robot using control inputs stored along
edges in the tree
![Page 72: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/72.jpg)
Principle Advantage
� RRT quickly
explores the state
space:
� Nodes most likely
to be expanded are
those with largest
Voronoi regions
![Page 73: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/73.jpg)
Advanced RRT Algorithms
1. Single RRT biased towards the goal
2. Bidirectional planners
3. RRT planning in dynamic environments
![Page 74: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/74.jpg)
Example: Simple RRT Planner
� Problem: ordinary RRT explores X uniformly
ĺ slow convergence
� Solution: bias distribution towards the goal
![Page 75: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/75.jpg)
Goal-biased RRT
BIASED_RANDOM_STATE()
1 toss ĸ COIN_TOSS()
2 if toss = heads then
3 Return sgoal
4 else
5 Return RANDOM_STATE()
![Page 76: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/76.jpg)
Goal-biased RRT
![Page 77: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/77.jpg)
The world is full of…local minima
� If too much bias, the
planner may get trapped in
a local minimum
A different strategy:
� Pick RRT point near sgoal
� Based on distance from
goal to the nearest v in G
� Gradual bias towards sgoal
Rather slow convergence
![Page 78: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/78.jpg)
Bidirectional Planners
� Build two RRTs, from start and goal state
� Complication: need to connect two RRTs
� local planner will not work (dynamic constraints)
� bias the distribution, so that the trees meet
![Page 79: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/79.jpg)
Bidirectional Planner Algorithm
![Page 80: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/80.jpg)
Bidirectional Planner Example
![Page 81: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/81.jpg)
Bidirectional Planner Example
![Page 82: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/82.jpg)
Conclusions
� Path planners for real-world robots must account for dynamic constraints
� Building the roadmap tree incrementally
� ensures that the kinodynamic constraints are satisfied
� avoids the need to reconstruct control inputs from the path
� allows extensions to moving obstacles problem
![Page 83: Kinodynamic Path Planning - MIT OpenCourseWare · PDF fileOutline Roadmap path planning Probabilistic roadmaps Planning in the real world Planning amidst moving obstacles RRT-based](https://reader031.vdocuments.net/reader031/viewer/2022021510/5ab3663a7f8b9aea528e3ace/html5/thumbnails/83.jpg)
Conclusions
� MOP and RRT planners are similar
� Well-suited for single-query problems
� RRTs benefit from the ability to steer a
robot toward a point
� RRTs explore the state more uniformly
� RRTs can be biased towards a goal or to grow
into another RRT