real-time motion planning for agile autonomous vehicles
DESCRIPTION
Emilio Frazzoli, Munther A. Dahleh and Eric Feron. Real-Time Motion Planning for Agile Autonomous Vehicles. Jingru Luo. Background. Kinodynamic motion planning A new randomized incremental algorithm System’s dynamics An environment with moving obstacles Efficiency Safety guarantee - PowerPoint PPT PresentationTRANSCRIPT
Real-Time Motion Planning for Agile Autonomous
VehiclesEmilio Frazzoli, Munther A. Dahleh and Eric Feron
Jingru Luo
Background
Kinodynamic motion planning A new randomized incremental algorithm
System’s dynamicsAn environment with moving obstaclesEfficiencySafety guaranteeOptimal solutionProbabilistic completeGuaranteed performance
Framework
System DynamicsOrdinary Differential Equations(ODEs)
Equilibrium points (zero velocity)
),( uxfdt
dx
eqx0),( eqeq uxf
(1)
(2)
Framework
Obstacle-Free Guidance SystemOptimal cost-to-go function for all
Compute the optimal control policy ft
t eqeq dtuxxxxJ0
),,(),(* (5)
UXX : (6)
x
eqx
x),( eqxx
),( 1 eqxx
1x
Framework
Notation ○ Feasible set F
for all (state, time) pairs satisfying all constraints
○ Reachable set
Given , is reachable if find
RXF
),( 00 tx ),( ff tx Uttu f ],[: 0
x
)( eqxT
),( 00 tx
),()( 11 eqxxtu
),()( 10 eqxxtu
),( 11 tx
),( ff tx
),( 00 txR
Framework
Problem FormulationGiven an initial state at time , and a goal ,
find that can steer the system from to
Optimal trajectory to minimize the cost
0t fx0xUttu f ],[: 0
)( fxT0x
Motion Planning
Basic idea
Initial condition
Random equilibrium point
Target
Data Structure
Node (state, time) coupleTotal cost
○ Accumulated cost by bookkeeping○ Cost-to-go
Lower bound: optimal cost functionUpper bound: infinity
Edge The randomly generated equilibrium pointCost along the edge
rx
),(* eqxxJ
Initialization
Root node: some point in the futureContains the initial conditionAs an example: , moving at , and
we devote s to the computation, the root node must be set at
Total cost:○ Accumulated cost: set to zero○ Cost-to-go
Lower bound: optimal cost functionUpper bound: infinity
),( 00 tx 0v
),( 0000 vtvx
),( 00 tx
),( 0000 vtvx
0v
A
B
Tree Expansion Step
Two points:Which node to expandIn which direction to explore
○ Samples a random configuration , and try to expand all nodes in order of increasing cost
○ Apply the optimal control policy until the system gets to endgame region of
rx
),( rxrx
Initial state
Random Equilibrium point
Target
1
2
3
Real-Time Consideration
Safety CheckMoving obstaclesCollision check at time point is not enoughCheck safety over a time
○ safetyA milestone (x, t) is τ-safe if (x, t) is feasible for all
t [t,t+∈ τ]
Real-Time Consideration
Improving performanceTrajectories from equilibrium to equilibrium
provide unsatisfactory performancePrimary milestoneSecondary milestone, split the new edge
into n segments and add the breakpoints
Primary milestone
Secondary milestone
Update Cost
Look for the optimal trajectoryWhen a solution is foundUpper bound on the cost-to-go are updatedUpdate backward to the root
○ Parent.U.B.> node.U.B.+ edge_cost○ Parent.U.B node.U.B.+ edge_cost
Initial state
Target
Xnew
Tree Pruning
Upper bound and lower boundLower bound: optimal cost-to-go in free
environmentUpper bound: cost of the best trajectory from the
milestone to the destination or +∞Every time a solution is found, prune subtree while
updating the node○ lower bound + edge cost > upper bound current node
Initial state
Random point
Target
x
3
4
56
L: 10U: 10
L: 15U: +∞
2
L: 25U: +∞
1
L: 35U: +∞
10L: 20U: +∞
L: 22U: +∞ L: 25
U: +∞
L: 15U: 20
L: 20U: 25
L: 22U: 30 L: 25
U: 35
10
5
5 5
10
Initial state
Random point
Target
Complete Algorithm
InitializationLoop if trajectory(root, target) collison free then return success loop
newTrajectory=Expand-Treeif newTrajectory != failure and safethen get Primary and Second M.S.for all new M.S. doUpdate-Cost and Prune-Tree
until Time is up if feasible solution found then root best child else if root has children then root random child else generate another root
Application Example
Four plannersA: one node, chosen randomlyB: one node, chosen as the closest oneC: all node sorted in random orderD: all node sorted in the order of increasing
distance
Application Example
Ground robot moving among fixed spheres Helicopter moving among fixed spheres
The planners handle easily
Application Example
Ground robot moving through sliding doorsC, D perform better
Algorithm A Algorithm D
Application Example
Helicopter moving through sliding doorsC, D perform better while A failed
Algorithm B Algorithm D
Conclusion
Deal with system dynamics efficiently Decouple between the high-level motion
plan and low-level control tasks Real time issues
Finite computation timeSafety checkExploration strategy
Future workMotion plan in the uncertain environment
Thank You!