Occlusion-Aware Contingency Safety-Critical Planning for Autonomous Driving
Authors: Lei Zheng, Rui Yang, Minzhe Zheng, Zengqi Peng, Michael Yu Wang, Jun Ma
Organizations: Robotics and Autonomous Systems Thrust, The Hong Kong University of Science and Technology (Guangzhou), Guangzhou 511453, China · School of Engineering, Great Bay University, Dongguan 523000, China · Cheng Kar-Shun Robotics Institute, The Hong Kong University of Science and Technology, Hong Kong SAR, China
Ensuring safe driving while maintaining travel efficiency for autonomous vehicles in dynamic and occluded environments is a critical challenge. This paper proposes an occlusion-aware contingency safety-critical planning approach for real-time autonomous driving. Leveraging reachability analysis for risk assessment, forward reachable sets of phantom vehicles are used to derive risk-aware dynamic velocity boundaries. These velocity boundaries are incorporated into a biconvex nonlinear programming (NLP) formulation that formally enforces safety using spatiotemporal barrier constraints, while simultaneously optimizing exploration and fallback trajectories within a receding horizon planning framework. To enable real-time computation and coordination between trajectories, we employ the consensus alternating direction method of multipliers (ADMM) to decompose the biconvex NLP problem into low-dimensional convex subproblems. The effectiveness of the proposed approach is validated through simulations and real-world experiments in occluded intersections. Experimental results demonstrate enhanced safety and improved travel efficiency, enabling real-time safe trajectory generation in dynamic occluded intersections under varying obstacle conditions. The project page is available at https://zack4417.github.io/oacp-website/.
Figures & tables
Fig. 1: The EV (red) navigates an occluded four-way intersection, generating two trajectories: a blue exploration trajectory and a red fallback trajectory with an arrow. Both trajectories share an initial common segment to enable smooth transitions between trajectories. Visibility is obstructed by static obstacles (buildings) and dynamic vehicles (orange), highlighting the challenges of safe and efficient driving in occluded environments.
Fig. 2: Spatial distribution of risk around the EV in the presence of occlusion. The EV is positioned at [ 0m , −2m ], with the occluded area extending from [ −8m , 0m ] to [ −2m , 0m ]. The quantitative color scale (right), labeled ‘Risk Value’, illustrates the risk level, where warmer colors (e.g., red) represent higher risk and cooler colors (e.g., blue) indicate lower risk.
Description
Value
Front axle distance to center of mass
lf=1.06m
Rear axle distance to center of mass
lr=1.85m
Maximum anticipated number of obstacles
M=4
Maximum velocity of PVs
vpv,max=10m/s
Longitudinal position range
px∈[−50m,50m]
Velocity range of SVs
[0m/s,10m/s]
TABLE I: Vehicle Parameters
Fig. 3: A top-down view of the EV navigating through a dynamic and occluded intersection under dense traffic conditions. The blue vehicles (SV3, SV4) represent non-interacting vehicles whose future trajectories do not intersect with the EV. The lighter orange vehicles (SV5, SV6) are occluded, while the solid orange vehicles (SV1, SV2) are fully visible. The EV plans two potential trajectories to manage the occlusion risks: the black exploration trajectory prioritizes travel efficiency, while the conservative fallback trajectory accounts for higher occlusion risks.
Algorithm
Collision
Task Duration ( s )
Longitudinal Velocity ( m/s )
Solving Time ( ms )
MEAN
MAX
MIN
MEAN
MAX
MIN
Occlusion-Ignorant
Yes
–
–
–
–
–
–
–
ST-RHC
No
17.90
3.11
6.37
0.00
43.14
227.06
5.21
Control-Tree
No
16.40
3.53
7.00
2.51 ×10−3
260.86
270.75
240.25
Occlusion-Aware Contingency Planner
No
12.50
4.68
7.41
1.64
23.83
44.75
0.56
TABLE II: Quantitative Results Comparison Among Different Algorithms
Fig. 4: Third-person view snapshots of the red EV navigating through a dense and occluded intersection at different time instants. The proposed occlusion-aware contingency planner enables the EV to first decelerate to avoid potential hazards and then accelerate to safely pass through the occluded intersection under dense traffic conditions.
Fig. 5: The generated velocity profiles of two trajectories at time instant 7s . Two trajectories show the shared initial segment and the separate process for safe navigation.
Fig. 6: Evolution of optimization of the proposed occlusion-aware contingency planning approach with the prediction step N=40 .
Fig. 7: Snapshots of a real-world experimental driving task at an occluded four-lane unsignalized intersection. The TianRacer EV (outlined with a green dashed box) navigates through the intersection, with two surrounding dynamic PVs (outlined with orange and yellow dashed boxes) maintaining their respective lanes, posing potential risks. As the EV approaches the intersection, it adjusts its speed dynamically to ensure safe navigation.
Fig. 8: Optimal longitudinal velocity and acceleration profiles of the selected trajectory at two different time instants. (a) The EV decelerates as it approaches the occluded intersection; (b) The EV accelerates to pass through the intersection.
Fig. 9: Evolution of the optimization time with a prediction step of N=40 while interacting with three SVs. The average and maximum optimization time are 27.33ms and 44.65ms , respectively.
Number of Obstacles
Average Time
Maximum Time
Minimum Time
2
23.17 (0.18)
40.92 (1.75)
0.37 (0.03)
3
25.12 (0.25)
39.74 (1.69)
0.41 (0.02)
4
27.18 (0.26)
43.97 (3.46)
0.64 (0.11)
5
27.67 (0.37)
46.66 (2.56)
0.95 (0.12)
6
28.51 (0.38)
44.49 (3.41)
0.93 (0.15)
TABLE III: Statistical Results for Optimization Time (ms) vs. Number of Obstacles Over 10 Trials, Reported as Mean (Std. Dev.)
Ns
Collision
Task Duration
Min. Long. Vel
Mean Solve Time
( s )
( m/s )
( ms )
3
No
12.5 (0)
1.45 (0.03)
23.24 (0.30)
5
No
12.5 (0)
1.48 (0.03)
23.41 (0.25)
8
No
12.5 (0)
1.48 (0.01)
23.44 (0.28)
10
No
12.5 (0)
1.48 (0.01)
23.66 (0.28)
15
No
12.5 (0)
1.47 (0.03)
23.70 (0.41)
TABLE IV: Statistical Results for Ablation Study on Consensus Steps ( Ns ) Over 10 Trials, Reported as Mean (Std. Dev.)