An Efficient Motion Planning Algorithm for Stochastic Dynamic - - PowerPoint PPT Presentation

▶
an efficient motion planning algorithm for stochastic
SMART_READER_LITE
LIVE PREVIEW

An Efficient Motion Planning Algorithm for Stochastic Dynamic - - PowerPoint PPT Presentation

Apr 06, 2023 6 likes •460 views

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.

slide-1
SLIDE 1

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 16th 2008

Masahiro Ono and Brian C. Williams

Massachusetts Institute of Technology

slide-2
SLIDE 2

2

Robust Planning for Autonomous Vehicles

slide-3
SLIDE 3

3

The world is uncertain.

slide-4
SLIDE 4

4

Approach

Formulate robust planning problem as an optimization with a chance constraint

slide-5
SLIDE 5

5

Key Concept

Risk Allocation

slide-6
SLIDE 6

6

Solution

Iterative Risk Allocation (IRA) algorithm

slide-7
SLIDE 7

7

Chance constraint Risk allocation Iterative risk allocation algorithm

slide-8
SLIDE 8

8

Kinodynamic Path Planning

  • Use continuous state space
  • Explicitly consider vehicle dynamics model

max 1 1

u u Bu Ax x

t t t t

≤ + =

+ +

Vehicle dynamics model

Equation of motion: Control limit (maneuverability): Obstacle Non-kinodynamic path planning Kinodynamic path planning Sharp turn is impossible in reality

slide-9
SLIDE 9

9

Uncertainty in Mobile Robotics

99.9% 99% 90% 80% 99.9% 99% 90% 80%

t =1

  • Outer disturbance
  • Control error
  • State estimation error

Distribution is

  • Continuous
  • Unbounded

x P(x) t =2 Nominal Path

slide-10
SLIDE 10

10

Example: Race Car Path Planning

  • A race car driver wants

to go from the start to the goal as fast as possible

  • Crashing into the wall

may kill the driver

  • Actual path may differ

from the planned path due to uncertainty

Start Goal Walls

Planned Path Actual Path

slide-11
SLIDE 11

11

Example: Race Car Path Planning

  • Good strategy: set a

safety margin

  • What is the necessary

width of the safety margin?

Start Goal Walls

Planned Path Actual Path

Safety Margin Safety Margin

slide-12
SLIDE 12

12

Example: Race Car Path Planning

Start Goal Walls

Planned Path

Safety Margin Safety Margin

  • 1 m margin

– 1 % risk of crash – 100 m distance to goal

slide-13
SLIDE 13

13

Example: Race Car Path Planning

Start Goal Walls

Planned Path

Safety Margin Safety Margin

  • 2 m margin

– 0.1 % risk – 120 m distance to goal

slide-14
SLIDE 14

14

Example: Race Car Path Planning

Start Goal Walls

Planned Path

Safety Margin Safety Margin

  • 3 m margin

– 0.01 % risk – 150 m distance to goal

  • Risk can only be

reduced by sacrificing performance

  • Cannot guarantee

100% success

slide-15
SLIDE 15

15

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?

slide-16
SLIDE 16

16

Robust Path Planner

  • A robust planner provides probabilistic guarantee

– eg. Probability of failure is less than 0.1%

slide-17
SLIDE 17

17

Problem Statement: Robust Planning

  • Maximize expected performance while constraining

that the probability of failure is below an upper bound (chance constraint).

Δ ≤

Fail

P

Chance constraint:

PFail: Probability of failure in a mission (from start to goal)

slide-18
SLIDE 18

18

Race Car Example Again

  • Find the shortest path
  • Chance constraint:

Probability of crashing into the wall is limited up to 0.1%

Start Goal Walls

001 . ≤

Fail

P

slide-19
SLIDE 19

19

How to set safety margin?

Start Start Goal

Safety margin Walls

Goal

Walls Safety margin (a) Uniform width safety margin (b) Uneven width safety margin

(b) results in better path

slide-20
SLIDE 20

20

Key Idea - Risk Allocation

(b) Uneven width safety margin

Corner Narrow safety margin = higher risk Straight line Wide safety margin = lower risk

  • Taking a risk at the

corner results in a greater time saving than taking the same risk at the straight line

  • Risk Allocation

– Take risk when it leads to a great reward, while saving it when the reward is small

slide-21
SLIDE 21

21

Risk allocation: More precise definition

  • pFail(t): Probability of failure

at time step t

  • Setting safety margin =

setting pFail(t)

– wide margin = small pFail(t)

  • By using Boole’s inequality,
  • Risk allocation: assign

values to pFail(t) so that

∑

≤

t Fail Fail

t p P ) (

t = 1 t = 2 t = 3 t = 4 t = 5

Δ ≤

∑

t Fail t

p ) (

Chance constraint:

01 . ≤

Fail

P

Risk allocation: pFail(1) = 0.001,

pFail(2) = 0.002, pFail(3) = 0.004, pFail(4) = 0.002, pFail(5) = 0.001

Example

slide-22
SLIDE 22

22

Iterative Risk Allocation (IRA) Algorithm

Optimal risk allocation Suboptimal risk allocation

Iteration

  • Starts from a suboptimal risk allocation
  • Improves it by iteration
slide-23
SLIDE 23

23

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin

slide-24
SLIDE 24

24

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin No gap = Constraint is active Gap = constraint is inactive

Best available path given the safety margin

slide-25
SLIDE 25

25

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin

slide-26
SLIDE 26

26

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin

slide-27
SLIDE 27

27

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin

Active Inactive Inactive

slide-28
SLIDE 28

28

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin

slide-29
SLIDE 29

29

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin

slide-30
SLIDE 30

30

Iterative Risk Allocation Algorithm

Algorithm IRA 1 Initialize with arbitral risk allocation 2 Loop 3 Compute the best available path given the current risk allocation 4 Decrease the risk where the constraint is inactive 5 Increase the risk where the constraint is active 6 End loop Start Goal

Safety margin

Deterministic optimization problem

slide-31
SLIDE 31

31

IRA is a Two-stage Optimization

Any existing deterministic path planner can be used By putting IRA on top of your deterministic planner, it provides robustness (probabilistic guarantee).

IRA

Risk allocation

Deterministic Path Planner

Path

slide-32
SLIDE 32

32

Simulation: AUV Depth Planning

  • Dorado-class AUV
  • perated by MBARI

(Monterey Bay Aquarium Research Institute)

  • Bathymetric data of the

Monterey Bay

slide-33
SLIDE 33

33

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

Δ

slide-34
SLIDE 34

34

Simulation Result

Sea floor level

Risk allocation ( )

% 5 = Δ

slide-35
SLIDE 35

35

Result: Comparison with Prior Arts

0.297 0.023 <10-5 PFail 56.2 64.1 88.6 Objective Function

(minimizing problem)

0.093 sec Ellipsoid relaxation[1] 915.6 sec 1.36 sec Computation time Particle Control[2]* IRA Algorithm

Average result of 50 runs on different segments of the Monterey Bay sea floor

[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

05 . ≤

Fail

P

(Chance constraint)

slide-36
SLIDE 36

36

Applications

  • Application is not limited to path planning

– Motion planning – Chemical process control – Financial engineering?

  • Can be expanded to discrete/hybrid state space

Temperature Pressure

Explode Freeze

slide-37
SLIDE 37

37

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

slide-38
SLIDE 38

38

Conclusion (2)

  • IRA provides probabilistic guarantee to the existing

deterministic planners

  • Validated the new robust planning algorithm in

simulation by using Dorado-class AUV model and the actual bathymetric data of the Monterey Bay

slide-39
SLIDE 39

39

¿Questions?

For more questions and discussions,

  • Poster session: in “Doctoral Consortium Abstracts”

section, TODAY 6:00 - 9:30

For further update,

  • M. Ono and B. C. Williams. Two-stage Optimization

Approach to Robust Model Predictive Control with a Joint Chance Constraint, accepted to IEEE CDC 08

Special Thanks to:

  • The Boeing Company for funding this research
  • MBARI for wonderful collaboration
slide-40
SLIDE 40

40

  • Good news

– IRA algorithm monotonically increases the objective function by iteration – Refer to the paper for proof

  • Bad news

– There is no proof of global optimality

  • Another good news

– Risk allocation is a convex optimization problem when the feasible region is convex and

slide-41
SLIDE 41

41

Resource Allocation and Risk Allocation

  • Resource allocation
  • Risk allocation

∑

=

i i

r R

Total amount of the resource is the sum of the resource allocated to all subsections

∑

≠

t Fail Fail

t p P ) (

PFail: Probability of failure in a mission (from start to goal) pFail(t): Probability of failure at t th time step

slide-42
SLIDE 42

42

Trick: Boole’s Inequality

  • By definition,
  • Using Boole’s inequality:
  • PFail is upper bounded:

) ( ) ( ) ( B P A P B A P + ≤ ∪

] ) ( [ ) ( )] ) ( ( ) ) 2 ( ( ) ) 1 ( [( R t x P t p R T x R x R x P P

Fail Fail

∉ ≡ ∉ ∨ ∉ ∨ ∉ ≡ L

∑

≤

t Fail Fail

t p P ) (

Δ ≤

Δ ≤ ⇒

Fail

P

slide-43
SLIDE 43

43

Suboptimality of Boole’s Inequality

  • Suboptimality:
  • Observation:

in most applications

– (No one wants to die in a race for 50% probability)

  • Let’s assume

– – 10 time steps – Uniform risk allocation – All probabilities are independent

  • In general, the suboptimality is small when is small and

the number of time steps is large. ) ( ≥ −

∑

Fail t Fail

P t p

1 << Δ

) 01 . ( % 1 ≤ = Δ

Fail

P 001 . ) ( = ∀ t p t

Fail

L 0099551 . ) 001 . 1 ( 1

10 =

− − =

Fail

P

L 0000448 . ) ( = −

∑

Fail t Fail

P t p

Suboptimality:

Δ

slide-44
SLIDE 44

44

Suboptimality: Graphical Interpretation

) ( ) ( ) ( B P A P B A P + ≤ ∪

) (A P ) (B P

1 1

) ( ) ( ) ( B A P B P A P ∪ − +

(Suboptimality)

slide-45
SLIDE 45

45

Risk Allocation Problem

  • Original chance constraint

is implied by

  • Risk allocation problem: assign values to

Δ ≤

Fail

P . ) (

t

Δ ≤ ∨ ≤ ∨

∑

t t t Fail t

p δ δ

t

δ