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

42
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

Transcript of Confidence-aware motion prediction for real-time collision ...prediction for real-time collision...

Page 1: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Page 2: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

they produce safe but efficient motions?

Page 3: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Connections to reachability analysis

Confidence-aware prediction & planning

Scaling up to multi-robot, multi-human scenarios

Page 4: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Confidence-aware prediction & planning

Scaling up to multi-robot, multi-human scenarios

Connections to reachability analysis

Page 5: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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]

Page 6: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

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

Page 7: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

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

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

vs.

[Herbert, 2017]

[Mitchell, 2005]

[Lygeros, 2005]

Page 8: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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.]

Page 9: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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.]

Page 10: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

[Herbert, 2017]

[Fisac, 2018]

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

inf𝑢(∙)

{ sup𝑡∈[0,𝑇]

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

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

[Mitchell, 2005]

[Lygeros, 2005]

Hamilton-Jacobi Reachability Analysis

Page 11: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Robust Robot Planning with Human Predictions

𝑥𝑅𝑔

[Herbert, 2017]

[Mitchell, 2005]

[Fisac, 2018]

[Lygeros, 2005]

Page 12: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Robust Robot Planning with Human Predictions

[Mitchell, 2008]

𝑥𝑅𝑔

[Herbert, 2017]

[Fisac, 2018]

[Lygeros, 2005]

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

Page 13: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

What if the predictive model is wrong?

Modeled human goal

UnmodeledObstacle

Robot goal

Page 14: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

High Confidence Low Confidence

UnmodeledObstacle

UnmodeledGoal

human goal

human goal 1

human goal 2

robot goal

robot goal 1

robot goal 2

Page 15: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Bayesian Model Confidence

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

Page 16: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Bayesian Model Confidence

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

𝛽

Page 17: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Bayesian Model Confidence

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

Page 18: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Bayesian Model Confidence

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

Page 19: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Bayesian Model Confidence

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

𝑏𝑡 𝛽 ∝ 𝑃 𝑢𝐻𝑡 𝑥𝐻

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

Page 20: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Bayesian Model Confidence

𝑏𝑡 𝛽 ∝ 𝑃 𝑢𝐻𝑡 𝑥𝐻

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

Page 21: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Fixed confidence Bayesian confidence𝛽

Page 22: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Less confident!

More confident!

Page 23: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Connections to reachability analysis

Robust motion planning

Page 24: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Connections to reachability analysis

Robust motion planning

Page 25: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

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

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

𝑥𝐻(0)

Page 26: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

Δ𝑡 ∗ 𝑣𝐻

Page 27: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

Δ𝑡 ∗ 𝑣𝐻

Page 28: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

Page 29: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

𝑃𝑐𝑜𝑙𝑙 ≫ 0

Page 30: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

𝑃𝑐𝑜𝑙𝑙 > 0

Page 31: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

𝟏 𝒔𝒆𝒄 ∗ 𝑣𝐻

𝑃𝑐𝑜𝑙𝑙 → 0

Page 32: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

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

Page 33: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Forward Reachable Set

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

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

modeled goal

Page 34: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Page 35: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Page 36: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

1000

1000 x 1000 = 1M

Page 37: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

1000

1000

[Bajcsy, 2019]

Page 38: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Hardware Demonstration

Page 39: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

Hardware Demonstration

Page 40: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Page 41: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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

Page 42: Confidence-aware motion prediction for real-time collision ...prediction for real-time collision avoidance Andrea Bajcsy Long-term Human Motion Prediction Workshop ICRA 2019 Work with

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