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.)
The deployment of autonomous vehicles in urban environments introduces significant safety challenges, particularly in scenarios with occlusions, where critical traffic participants may be hidden from view. Recent accidents involving driverless vehicles highlight the importance of motion planners that explicitly addresses the risks posed by occlusions. In this work, we propose a formal, occlusion-aware trajectory planning framework that guarantees collision avoidance even when there are possible hidden traffic participants. Building on our previous methods that apply reachability analysis to sequentially determine the possible states of hidden traffic participants, we integrate a tree-based motion planner capable of reasoning over future observations and the absence thereof. This approach reduces conservativeness while maintaining safety guarantees. We demonstrate the effectiveness of our framework in a challenging simulated occluded scenario, showing that it pro-actively and efficiently guarantees collision-avoidance.
Truls Nyberg, Anna Gautier, Jana Tumova
KTH Royal Institute of Technology · Traton AB · Chalmers University of Technology +1
Safely handling occlusions is a fundamental challenge for autonomous mobile robots operating in dynamic environments. This issue is especially prominent in autonomous valet parking (AVP), where traffic rules are lax, occlusions are frequent and cluttered, and overly conservative behavior can leave vehicles stuck. However, existing methods either lack formal safety guarantees, assume agents follow road structures, or introduce conservatism, leaving occlusion-aware planning for AVP an open challenge. In this paper, we propose APRO (AH-Polyhedron Reachability for Occlusions), an exact and efficient occlusion-aware planning framework based on game-theoretic active perception and AH-polyhedron reachability analysis with AVP as our canonical use case. Our key insight is to reformulate set-based safety conditions in prior work as unions of AH-polyhedrons, enabling exact safety verification through linear programming (LP) without any additional conservatism in set computations or assumptions on road topology. We further show how the resulting safety conditions can be integrated into optimization-based planners or a bisection search scheme for real-time applications. We validate our method in simulation and hardware experiments, including data replay on a real-world parking lot dataset. Experimental results demonstrate that our method consistently achieved a 100% safety rate across all evaluated scenarios while maintaining real-time performance, resulting in safer and more optimal decisions than existing methods with formal safety guarantees.
Long Kiu Chung, David Isele, Toktam Mohammadnejad +4
Honda Research Institute (HRI), Mountain View, CA. · Georgia Institute of Technology, Atlanta, GA.
Occlusion-aware prediction remains a critical challenge in autonomous driving due to the inherent uncertainty of unobserved regions. Existing approaches either overestimate risk based on reachable states or struggle to predict accurate trajectories under high occlusion uncertainty. To address these limitations, we propose a unified risk map modeling and learning framework for partially observable environments. Our method integrates traffic flow risk and collision risk through spatiotemporal modeling, enabling fine-grained assessment of occlusion-induced hazards. To address the scarcity of scenarios involving occluded interactions, we introduce a diffusion-based scenario generation framework that produces realistic yet adversarial scenarios. We integrate the modeling and learning of a unified risk map into a framework that supports risk-aware planning under partial observability. Experiments on the Waymo Open Motion Dataset show that our method significantly outperforms the state-of-the-art occlusion-aware baseline, improving minimum time-to-collision by 0.78 times and average time-to-collision by 1.67 times. The proposed framework offers a comprehensive and practical solution for risk-aware planning in partially observable environments.
Jie Jia, Yaofeng Su, Zeyu Bao +4
Fudan University, China · Tongji University, China