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!