Massachusetts Institute of Technology An Efficient Motion Planning Algorithm for Stochastic Dynamic Systems with Constraints on Probability of Failure AAAI 08 Physically Grounded AI Special Track July 16 th 2008 Masahiro Ono and Brian C. Williams Massachusetts Institute of Technology
Robust Planning for Autonomous Vehicles 2
The world is uncertain. 3
Approach Formulate robust planning problem as an optimization with a chance constraint 4
Key Concept Risk Allocation 5
Solution Iterative Risk Allocation (IRA) algorithm 6
Iterative risk allocation algorithm Chance constraint Risk allocation 7
Kinodynamic Path Planning • Use continuous state space • Explicitly consider vehicle dynamics model Vehicle dynamics model = + x Ax Bu Equation of motion: + t 1 t t ≤ u u Control limit (maneuverability): + t 1 max Sharp turn is impossible in reality Kinodynamic path planning Non-kinodynamic path planning Obstacle 8
Uncertainty in Mobile Robotics •Outer disturbance Distribution is •Continuous •Control error •Unbounded •State estimation error 99.9% Nominal Path 99.9% 99% 99% 90% 90% 80% 80% t =1 P( x ) t =2 x 9
Example: Race Car Path Planning • A race car driver wants to go from the start to Planned Path the goal as fast as Actual Path possible Goal • Crashing into the wall Walls may kill the driver • Actual path may differ from the planned path Start due to uncertainty 10
Example: Race Car Path Planning • Good strategy: set a safety margin Planned Path Actual Path Safety Margin • What is the necessary Goal Safety Margin width of the safety Walls margin? Start 11
Example: Race Car Path Planning • 1 m margin – 1 % risk of crash Planned Path – 100 m distance to goal Safety Margin Goal Safety Margin Walls Start 12
Example: Race Car Path Planning • 2 m margin – 0.1 % risk Planned Path – 120 m distance to goal Safety Margin Goal Safety Margin Walls Start 13
Example: Race Car Path Planning • 3 m margin – 0.01 % risk Planned Path – 150 m distance to goal Safety Margin Goal Safety Margin • Risk can only be Walls reduced by sacrificing performance • Cannot guarantee Start 100% success 14
What Robust Planner Can Provide? • Robust plan: a plan that has a certain guarantee of success in uncertain environment • What guarantee can a robust planner offer in uncertain world? 15
Robust Path Planner • A robust planner provides probabilistic guarantee – eg. Probability of failure is less than 0.1% 16
Problem Statement: Robust Planning • Maximize expected performance while constraining that the probability of failure is below an upper bound ( chance constraint ). ≤ Δ P Chance constraint: Fail P Fail : Probability of failure in a mission (from start to goal) 17
Race Car Example Again • Find the shortest path • Chance constraint: Probability of crashing into the wall is limited up Goal to 0.1% Walls Start ≤ P 0 . 001 Fail 18
How to set safety margin? (b) Uneven width safety margin (a) Uniform width safety margin Goal Goal Walls Walls Safety margin Safety margin Start Start (b) results in better path 19
Key Idea - Risk Allocation • Taking a risk at the (b) Uneven width safety margin corner results in a greater time saving than taking the same risk at the straight line Corner • Risk Allocation Narrow safety margin – Take risk when it leads to = higher risk a great reward, while saving it when the reward is small Straight line Wide safety margin = lower risk 20
Risk allocation: More precise definition • p Fail ( t ): Probability of failure at time step t t = 5 t = 4 • Setting safety margin = t = 3 setting p Fail ( t ) t = 2 – wide margin = small p Fail ( t ) • By using Boole’s inequality, t = 1 ∑ ≤ P p ( t ) Fail Fail t Example ≤ • Risk allocation : assign P 0 . 01 Chance constraint: Fail values to p Fail ( t ) so that Risk allocation: p Fail (1) = 0.001, ∑ p Fail (2) = 0.002, p Fail (3) = 0.004, ≤ Δ p Fail t ( ) p Fail (4) = 0.002, p Fail (5) = 0.001 t 21
Iterative Risk Allocation (IRA) Algorithm •Starts from a suboptimal risk allocation • Improves it by iteration Iteration Suboptimal risk allocation Optimal risk allocation 22
Iterative Risk Allocation Algorithm Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop 23
Iterative Risk Allocation Algorithm Algorithm IRA No gap = Constraint is active 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation Best available path 4 Decrease the risk where given the safety margin the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop Gap = constraint is inactive 24
Iterative Risk Allocation Algorithm Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop 25
Iterative Risk Allocation Algorithm Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop 26
Iterative Risk Allocation Algorithm Algorithm IRA Active Inactive 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation 4 Decrease the risk where Inactive the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop 27
Iterative Risk Allocation Algorithm Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop 28
Iterative Risk Allocation Algorithm Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop 29
Iterative Risk Allocation Algorithm Algorithm IRA Deterministic optimization problem 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best Goal available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where Safety margin Start the constraint is active 6 End loop 30
IRA is a Two-stage Optimization IRA Any existing deterministic path planner can be used By putting IRA on top of Risk your deterministic planner, allocation Deterministic it provides robustness Path Planner (probabilistic guarantee). Path 31
Simulation: AUV Depth Planning • Dorado-class AUV operated by MBARI (Monterey Bay Aquarium Research Institute) • Bathymetric data of the Monterey Bay 32
Simulation: AUV Depth Planning • Objective: Minimize the average altitude during a mission • Chance constraint: Probability of crashing into the seafloor is Δ limited up to Planned trajectory Altitude 33
Simulation Result Sea floor level Δ = 5 % Risk allocation ( ) 34
Result: Comparison with Prior Arts Average result of 50 runs on different segments of the Monterey Bay sea floor Algorithm Ellipsoid IRA Particle relaxation [1] Control [2]* <10 -5 0.023 0.297 P Fail Objective Function 88.6 64.1 56.2 (minimizing problem) Computation time 0.093 sec 1.36 sec 915.6 sec ≤ P 0 . 05 Fail (Chance constraint) [1] van Hessem, D. H. 2004. Stochastic Inequality Constrained Closed-loop Model Predictive Control with Application to Chemical Process Operation . Ph.D. Dissertation, Delft University of Technology. [2] Blackmore, L. 2006. A Probabilistic Particle Control Approach to Optimal, Robust Predictive Control. In Proceedings of the AIAA Guidance, Navigation and Control Conference . *20 particles are used for the computation 35
Applications • Application is not limited to path planning – Motion planning – Chemical process control Pressure Explode Freeze Temperature – Financial engineering? • Can be expanded to discrete/hybrid state space 36
Conclusion (1) • Formulated the robust planning problem with unbounded uncertainty as an optimization with chance constraint • Introduced a novel concept risk allocation • Developed Iterative Risk Allocation (IRA) algorithm 37
Recommend
More recommend
