Confidence-aware motion prediction for real-time collision ...prediction for real-time collision...
Transcript of 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