confidence-aware motion prediction for real-time collision ...prediction for real-time collision...

Confidence-aware motion prediction for real-time collision avoidance

Andrea Bajcsy

Long-term Human Motion Prediction Workshop

ICRA 2019

Work with Sylvia Herbert, Jaime Fisac, David Fridovich-Keil, Steven Wang, Sampada Deglurkar, Claire Tomlin and Anca Dragan

When robots observe behavior that is not well explained by their predictive models, how do

they produce safe but efficient motions?

Connections to reachability analysis

Confidence-aware prediction & planning

Scaling up to multi-robot, multi-human scenarios

Confidence-aware prediction & planning

Scaling up to multi-robot, multi-human scenarios

Connections to reachability analysis

Low probabilityHigh probability

𝑃 𝑢𝐻 𝑥𝐻; 𝜃, 𝛽) ∝ 𝑒𝛽𝑄(𝑥𝐻,𝑢𝐻;𝜃)

Noisily-Rational Human Motion Prediction

ሶ𝑥𝐻 = 𝑓𝐻(𝑥𝐻 , 𝑢𝐻)

[Luce, 1959]

[Baker et al., 2007][Ziebart et al., AAAI 2008]

[Ramachandran et al., IJCAI 2007]

[Finn et al., ICML 2016]

[Pfeiffer et al., IROS 2016]

[Herman et al., ICRA 2015]

[Schultz et al., ICRA 2017]

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

ሶ𝑥𝑅 = 𝑓𝑅(𝑥𝑅 , 𝑢𝑅)

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

ሶ𝑥𝑅 = 𝑓𝑅(𝑥𝑅 , 𝑢𝑅)

ሶ𝑠𝑅 = ሚ𝑓𝑅(𝑠𝑅 , 𝑎𝑅)

vs.

[Herbert, 2017]

[Mitchell, 2005]

[Lygeros, 2005]

Hamilton-Jacobi Reachability Analysis

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

ሶ𝑥𝑅 = 𝑓𝑅(𝑥𝑅 , 𝑢𝑅)

ሶ𝑠𝑅 = ሚ𝑓𝑅(𝑠𝑅 , 𝑎𝑅)

vs.

[Herbert, 2017]

𝑉 𝑟, 𝑇 = sup𝑎[𝑢](∙)

inf𝑢(∙)

{ sup𝑡∈[0,𝑇]

𝑐𝑜𝑠𝑡(𝜉𝑟𝑢,𝑎(𝑡))}

ሶ𝑟 = 𝑓𝑅 𝑥𝑅 , 𝑢𝑅 − 𝜙( ሚ𝑓𝑅 𝑠𝑅 , 𝑎𝑅 )

[Mitchell, 2005]

[Lygeros, 2005]

[Herbert*, Chen*, Han, Bansal, Fisac, Tomlin. "FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning." CDC, 2017.]

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

Error bound𝑉 𝑟, 𝑇 = sup

𝑎[𝑢](∙)inf𝑢(∙)

{ sup𝑡∈[0,𝑇]

𝑐𝑜𝑠𝑡(𝜉𝑟𝑢,𝑎(𝑡))}

ሶ𝑟 = 𝑓𝑅 𝑥𝑅 , 𝑢𝑅 − 𝜙( ሚ𝑓𝑅 𝑠𝑅 , 𝑎𝑅 )

[Mitchell, 2005]

[Lygeros, 2005]

Hamilton-Jacobi Reachability Analysis

[Herbert*, Chen*, Han, Bansal, Fisac, Tomlin. "FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning." CDC, 2017.]

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

[Herbert, 2017]

[Fisac, 2018]

𝑉 𝑟, 𝑇 = sup𝑎[𝑢](∙)

inf𝑢(∙)

{ sup𝑡∈[0,𝑇]

𝑐𝑜𝑠𝑡(𝜉𝑟𝑢,𝑎(𝑡))}

ሶ𝑟 = 𝑓𝑅 𝑥𝑅 , 𝑢𝑅 − 𝜙( ሚ𝑓𝑅 𝑠𝑅 , 𝑎𝑅 )

[Mitchell, 2005]

[Lygeros, 2005]

Hamilton-Jacobi Reachability Analysis

𝑃 Crash > 𝑐𝑜𝑙𝑙𝑖𝑠𝑖𝑜𝑛𝑡ℎ𝑟𝑒𝑠ℎ

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

[Herbert, 2017]

[Mitchell, 2005]

[Fisac, 2018]

[Lygeros, 2005]

Robust Robot Planning with Human Predictions

[Mitchell, 2008]

𝑥𝑅𝑔

[Herbert, 2017]

[Fisac, 2018]

[Lygeros, 2005]

𝑃 Crash > 𝑐𝑜𝑙𝑙𝑖𝑠𝑖𝑜𝑛𝑡ℎ𝑟𝑒𝑠ℎ

What if the predictive model is wrong?

Modeled human goal

UnmodeledObstacle

Robot goal

High Confidence Low Confidence

UnmodeledObstacle

UnmodeledGoal

human goal

human goal 1

human goal 2

robot goal

robot goal 1

robot goal 2

𝑃 𝑢𝐻 𝑥𝐻; 𝜃, 𝛽) ∝ 𝑒𝛽𝑄(𝑥𝐻,𝑢𝐻;𝜃)

Bayesian Model Confidence

ሶ𝑥𝐻 = 𝑓𝐻(𝑥𝐻 , 𝑢𝐻)

𝑃 𝑢𝐻 𝑥𝐻; 𝜃, 𝛽) ∝ 𝑒𝛽𝑄(𝑥𝐻,𝑢𝐻;𝜃)

Bayesian Model Confidence

ሶ𝑥𝐻 = 𝑓𝐻(𝑥𝐻 , 𝑢𝐻)

𝛽

𝑃 𝑢𝐻 𝑥𝐻; 𝜃, 𝛽) ∝ 𝑒𝛽𝑄(𝑥𝐻,𝑢𝐻;𝜃)

Bayesian Model Confidence

ሶ𝑥𝐻 = 𝑓𝐻(𝑥𝐻 , 𝑢𝐻)

𝑃 𝑢𝐻 𝑥𝐻; 𝜃, 𝛽) ∝ 𝑒𝛽𝑄(𝑥𝐻,𝑢𝐻;𝜃)

Bayesian Model Confidence

ሶ𝑥𝐻 = 𝑓𝐻(𝑥𝐻 , 𝑢𝐻)

Bayesian Model Confidence

ሶ𝑥𝐻 = 𝑓𝐻(𝑥𝐻 , 𝑢𝐻)

𝑏𝑡 𝛽 ∝ 𝑃 𝑢𝐻𝑡 𝑥𝐻

𝑡 ; 𝜃, 𝛽)𝑏𝑡−1(𝛽)

Bayesian Model Confidence

𝑏𝑡 𝛽 ∝ 𝑃 𝑢𝐻𝑡 𝑥𝐻

𝑡 ; 𝜃, 𝛽)𝑏𝑡−1(𝛽)

Fixed confidence Bayesian confidence𝛽

Less confident!

More confident!

Confidence-aware prediction Scaling up to multi-robot, multi-human scenarios

Connections to reachability analysis

Robust motion planning

Confidence-aware prediction Scaling up to multi-robot, multi-human scenarios

Connections to reachability analysis

Robust motion planning

Forward Reachable Set

ሶ𝑥𝐻 = 𝑓𝐻(𝑥𝐻 , 𝑢𝐻)

𝐹𝑅𝑆 𝑥𝐻 , 𝑡 ≔ {𝑥′: ∃𝑢𝐻(∙), 𝑥′ = 𝜉(𝑥𝐻 , 𝑡, 𝑢𝐻(∙))}

𝜉(𝑥𝐻(0), 𝑡, 𝑢𝐻(∙))

𝑥𝐻(0)

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

Δ𝑡 ∗ 𝑣𝐻

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

Δ𝑡 ∗ 𝑣𝐻

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

𝑃𝑐𝑜𝑙𝑙 ≫ 0

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

𝑃𝑐𝑜𝑙𝑙 > 0

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

𝑃𝑐𝑜𝑙𝑙 → 0

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

2 𝒔 ∗ 𝑣𝐻 3 𝒔 ∗ 𝑣𝐻𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

Forward Reachable Set

ሶℎ𝑥 = 𝑣𝐻cos(𝑢𝐻)ሶℎ𝑦 = 𝑣𝐻sin(𝑢𝐻)

2 𝒔 ∗ 𝑣𝐻 3 𝒔 ∗ 𝑣𝐻𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

modeled goal

Scaling up to multi-robot, multi-human scenarios

Connections between predictions and FRS

Confidence-aware prediction

Robust motion planning

Confidently determining subsets of the FRS to avoid

Scaling up to multi-robot, multi-human scenarios

Connections between predictions and FRS

Confidence-aware prediction

Robust motion planning

Determining subsets of the FRS to avoid

1000 x 1000 x 1000 = 1B 1000 x 1000 x 1000 x 1000 = 1T

1000

1000 x 1000 = 1M

1000

1000

[Bajcsy, 2019]

Hardware Demonstration

Hardware Demonstration

Confidence-aware predictions offer promising directions for scaling

Connections between predictions and FRS

Confidence-aware prediction

Robust motion planning

Determining subsets of the FRS to avoid

1000

1000

1000 x 1000 = 1M

1000 x 1000 x 1000 = 1B

1000 x 1000 x 1000 x 1000 = 1T

1000

Confidence-aware predictions offer promising directions for scaling

Connections between predictions and FRS

Confidence-aware prediction

Robust motion planning

Determining subsets of the FRS to avoid

1000

1000

1000 x 1000 = 1M

1000 x 1000 x 1000 = 1B

1000 x 1000 x 1000 x 1000 = 1T

1000

Papers

Herbert*, Chen*, Han, Bansal, Fisac, Tomlin. "FaSTrack: a Modular Framework for Fast and Guaranteed Safe Motion Planning." CDC, 2017.

Fisac*, Bajcsy*, Herbert, Fridovich-Keil, Wang, Tomlin, and Dragan. "Probabilistically Safe Robot Planning with Confidence-Based Human Predictions." RSS, 2018.

Fridovich-Keil*, Bajcsy*, Fisac, Herbert, Wang, Dragan, and Tomlin. “Confidence-Aware Motion Prediction for Real-Time Collision Avoidance.” IJRR, 2019

Bajcsy*, Herbert*, Fridovich-Keil, Fisac, Deglurkar, Dragan, and Tomlin, “A Scalable Framework for Real-Time Multi-Robot, Multi-Human Collision Avoidance.” ICRA, 2019.

Multi-robot, multi-human planning: https://github.com/HJReachability/faSTPeople

Code

Fast and safe robot tracking: https://github.com/HJReachability/fastrack

Pedestrian prediction: https://github.com/shwang/pedestrian_prediction

ROS wrapper for pedestrian prediction: https://github.com/abajcsy/crazyflie_human