Occlusion-Aware Contingency Safety-Critical Planning for Autonomous Driving

Ensuring safe driving while maintaining travel efficiency for autonomous vehicles in dynamic and occluded environments is a critical challenge, which this paper addresses by proposing an occlusion-aware contingency safety-critical planning approach. The gist: This work introduces an occlusion-aware contingency planner that couples online reachability analysis for risk quantification with a consensus ADMM formulation to decompose the high-dimensional planning problem into low-dimensional convex subproblems, enabling real-time generation of exploration and fallback trajectories in dynamic occluded environments.

Problem Formulation and Reachability Analysis

The study addresses the challenge of safe and efficient navigation for an autonomous vehicle (EV) in areas where visibility is hindered by static obstacles and dynamic surrounding vehicles (SVs). The dynamics of the EV are modeled using a Dubin’s car model, defined by its state vector, control input vector, and subsequent state vector. To manage potential hazards arising from occlusions—defined as potential events that cannot be predicted with certainty—the paper introduces Forward Reachable Set (FRS) and Backward Reachable Set (BRS). The FRS is defined as the set of all states reachable within a time horizon T under admissible controls, while the BRS identifies initial states from which a target set T can be reached. A general contingency motion planning problem is formulated to minimize control effort and goal-tracking errors subject to constraints that ensure both exploration and safety fallback trajectories share an initial segment, denoted by the consistency constraint (2h).

Risk Quantification via SRQ and Dynamic Velocity Boundaries

To assess risks arising from occlusions, the paper employs the Simplified Reachability Quantification (SRQ) method to derive a risk-based speed boundary. The SRQ method quantifies risk within a dynamic Phantom Vehicle Set (PVS), which represents the occluded zone where phantom vehicles might exist, based on assumptions that PVs drive along lane centerlines and their initial positions are uniformly distributed. This leads to the calculation of longitudinal risk, rlon(s), and lateral risk, rlat(d). The total risk is computed as r(s, d) = rlon(s) · rlat(d). Based on this quantified risk, dynamic velocity sets are established using the formula: vocc,s = (vocc,min if rtotal > cth,max ∆v(rtotal − cth,min) + vocc,max otherwise), which yields two distinct maximum velocity boundaries: v0occ,s for the exploration trajectory and v1occ,s for the fallback trajectory.

Biconvex NLP Formulation and Spatiotemporal Barriers

The contingency motion planning problem is reformulated into a biconvex optimization problem that explicitly incorporates these risk considerations. This formulation includes spatiotemporal control constraints (4) to enforce safety, which are enforced using a spatiotemporal barrier constraint (4). This constraint ensures that the EV’s reachable sets do not overlap with the obstacles’ occupancy sets, defined by the function h(xk, o(i)k) > 0. Furthermore, high-order m-dimensional Bezier curves are used to parameterize the EV’s trajectory, where control points are optimized for each of the two trajectories (exploration and fallback). The objective function L minimizes control effort and goal-tracking errors across both trajectories.

Consensus ADMM for Real-Time Optimization

To facilitate real-time computation and coordination between the dual trajectories, the consensus Alternating Direction Method of Multipliers (ADMM) is employed. This method decomposes the biconvex NLP problem into low-dimensional convex subproblems by iteratively solving them in an alternating fashion. The updates involve minimizing primal control point variables (Cθ, Cx, Cy), updating risk parameters ω and ξ based on safety conditions, updating global consistency variables (Zx, Zy, Zθ) to ensure trajectory alignment over a common segment of length Ns, and updating dual variables. This iterative process allows for rapid computation of exploration and fallback trajectories within a receding horizon planning framework.

Validation through Simulation and Real-World Experiments

The effectiveness of the proposed approach is validated through simulations using a 1:10 scale Ackermann mobile robot platform in occluded intersections. Experimental results demonstrate enhanced safety and improved travel efficiency, achieving a reduction in intersection-traversal time to 12.50 s compared to baselines. Furthermore, computational efficiency analysis shows that the algorithm requires only 23.83 ms on average per optimization, demonstrating its feasibility for real-time application in dynamic and traffic-heavy environments with varying obstacle conditions, outperforming existing methods significantly in solving time. The method also maintains a minimum longitudinal velocity of at least 1.64 m/s during maneuvers, indicating superior safety compared to baseline methods that adopt overly conservative braking.

Limitations and Future Work

A primary limitation noted is the assumption that all PVs remain within their designated driving lanes, which restricts applicability to unstructured environments like those involving pedestrians where risk estimation via SRQ becomes challenging.

Here are specific improvements to an existing AI system based on the concepts presented in this scientific paper, along with what those improved systems could achieve:

1. Improve real-time trajectory generation in dynamic, occluded environments by implementing a novel occlusion-aware contingency planner.

2. Enhance safety and efficiency by simultaneously optimizing two trajectories: one for exploration (maximizing situational awareness) and one for safety fallback (prioritizing hazard avoidance based on reachability analysis).

3. Implement the Consensus Alternating Direction Method of Multipliers (ADMM) to decompose the high-dimensional, non-convex trajectory optimization problem into low-dimensional convex subproblems, enabling real-time computation.

4. Leverage Forward Reachable Sets (FRS) and Backward Reachable Sets (BRS), quantified via simplified reachability quantification (SRQ), to derive risk-aware dynamic velocity boundaries that adapt in real time based on the density and potential movement of phantom vehicles.

5. Integrate spatiotemporal barrier constraints into the biconvex NLP formulation to formally enforce safety by ensuring the ego vehicle's reachable sets do not overlap with obstacle occupancy sets, while maintaining trajectory consistency via shared segments.

6. Enable adaptive risk management by dynamically adjusting maximum velocity boundaries for both exploration and fallback trajectories based on aggregated longitudinal and lateral risk metrics derived from SRQ, allowing the system to balance efficiency (exploration) against safety (fallback).

7. Develop a consistent driving policy by enforcing shared initial segments between the exploration and fallback trajectories, ensuring smooth transitions and preventing abrupt control changes during trajectory switching.

8. Achieve superior performance in dense traffic scenarios by reducing intersection traversal time (e.g., 30% reduction compared to baselines) and improving average cruise velocity (e.g., 32% improvement), moving beyond overly conservative braking strategies of traditional methods like ST-RHC or Control-Tree MPC.

9. Ensure computational scalability by demonstrating that the ADMM framework maintains linear complexity with respect to the number of obstacles, allowing for real-time replanning frequency exceeding 20 Hz, even as the number of dynamic vehicles increases in occluded intersections.

The improved AI system can perform:

1. Navigate complex urban intersections safely and efficiently in environments where sensor visibility is frequently obstructed by buildings or other vehicles (occlusions).

2. Perform rapid, real-time trajectory replanning when unforeseen events or sudden movements from phantom vehicles occur, ensuring a safe fallback path is always immediately available.

3. Maintain superior driving performance by achieving a better trade-off between aggressive maneuvering for efficiency and conservative braking for safety compared to existing state-of-the-art models.

4. Operate effectively in high-density traffic scenarios (e.g., heavy vehicle interaction) where computational constraints are critical, as the system scales efficiently with the number of surrounding obstacles.