1 Introduction↩︎

Ensuring the safety of autonomous vehicles in dynamic, uncertain environments is paramount. This requires strict safety guarantees without unnecessarily compromising driving efficiency [1], [2]. A core challenge lies in accurately modeling and adapting to the uncertain, time-varying behaviors of surrounding human-driven vehicles (HVs), compounded by perception inaccuracies [3], [4]. Unanticipated contingency events, such as abrupt lane changes or sudden acceleration/deceleration, can force disruptive maneuvers and undermine planning feasibility, posing significant risks [5]–[7]. Like expert drivers who adapt their caution based on perceived intent (e.g., giving more space to aggressive vehicles), autonomous vehicles must interpret and respond to evolving uncertainties online to adaptively maintain safety and efficiency. Yet achieving such a flexible balance remains elusive in practice: planners that ignore uncertainty can underestimate risks, whereas worst-case formulations become so conservative that efficiency suffers [8]–[10]. Consequently, ensuring the persistent existence of safe trajectories while maintaining high driving efficiency under uncertainty remains an unresolved problem, particularly when adaptive responsiveness to uncertainties is essential.

Stochastic optimization methods, such as chance-constrained approaches, address this problem by permitting small, predefined collision probabilities [11], [12]. However, exact chance constraint evaluation is computationally intractable [13], leading to common approximations with Gaussian uncertainty models [6], [14]. Recent advances, such as chance-constrained nonlinear model predictive control (NMPC) for multi-modal obstacle behaviors [15], ensure recursive feasibility and safety across the entire planning horizon. Nevertheless, these methods still face fundamental limitations: Gaussian approximations struggle to capture the inherently non-Gaussian nature of real-world driving behaviors [16], and even minor constraint violations can lead to hazardous outcomes in unexpected situations. Scenario optimization offers a sampling-based alternative [17], but demands large sample sets to address low-probability, high-risk events, which hinder its practical application in the real world.

To overcome these limitations, robust optimization enforces strict safety by requiring constraint satisfaction under all possible uncertainty realizations within bounded sets [18], [19]. This is typically achieved using forward reachability analysis in motion planning [20], [21]. For example, prior works [8], [22] compute the forward reachable sets (FRSs) for traffic participants while accounting for road network constraints and worst-case control capabilities. Similarly, a robust NMPC framework is designed using HV FRS to guarantee safety under motion uncertainties of the HV [23]. To ensure recursive feasibility, methods such as [24], [25] maintain fail-safe trajectories over infinite horizons, allowing the ego vehicle (EV) to fall back to emergency maneuvers when needed[26]. In [27], a safe backup trajectory is computed using FRS and enforced if the primary stochastic NMPC optimization becomes infeasible. However, sudden transitions to backup trajectories may degrade passenger comfort.

While reachability-based methods offer robust safety guarantees, control barrier function (CBF) provides an alternative approach through forward invariance [28]. The forward invariance of safety sets is closely related to the recursive feasibility for safe trajectories [29], [30], which can be naturally encoded via barrier function constraints [31]. Recent advances integrate reachability analysis with CBFs to certify forward-invariant safety despite uncertainty [32], [33]. For instance, FRS predictions are employed to construct CBFs to handle uncertainties in HV behaviors [34]. While these CBF-based methods provide strong safety guarantees, they often remain overly conservative, especially in dense traffic scenarios, where worst-case assumptions lead to infeasible or inefficient motion plans.

To reduce the conservatism without sacrificing safety, recent research explores learning-based approaches to refine uncertainty estimates. Uncertainty sets are constructed as subsets of worst-case forward reachable space to enable safer yet less conservative planning [35], [36]. When sufficient prior data is available, FRS predictions can be refined through online updates [37], [38]. Learning techniques such as self-supervised neural networks[39], Gaussian processes [40], [41], and active learning [42] show promise, yet often rely on extensive offline training or face challenges in real-time adaptation for multiple HVs. For online learning efficiency, polytopic control-intent sets are learned to predict FRSs of dynamic obstacles [43], while ellipsoidal representations are employed to accelerate computation [44]. However, most of these works do not directly address the recursive feasibility problem under uncertainty. Furthermore, planning solely based on the FRSs of HVs can still be conservative, as it requires a single trajectory to perform well under all possible behaviors.

Complementing these approaches, contingency planning offers a balanced approach by optimizing nominal behavior while maintaining flexibility through backup plans [45], [46]. These methods jointly optimize over a primary horizon for performance and a parallel horizon to handle contingencies [4], [47]. For example, contingency planning is applied to account for model uncertainties and reduce the conservation of worst-case FRS prediction in [48]. Nevertheless, the non-convex nature of safety requirements in multiple horizons and inherent non-holonomic kinematic constraints poses significant computational challenges for real-time implementation [49]

Recent advances in trajectory optimization for autonomous driving have employed distributed optimization techniques to address the computational bottlenecks. Specifically, the alternating direction method of multipliers (ADMM) is employed to maintain feasibility under constraints while achieving real-time performance[49], [50]. Particularly effective for separable biconvex optimization problems, ADMM-based approaches leverage parallel computing capabilities to facilitate high computational efficiency, enabling rapid response to changing environments [2], [51].

In this study, we propose a real-time contingency trajectory optimization framework that enforces safety through FRS-based barrier functions, with dynamic refinement of HV control-intent sets using event-triggered online learning. This approach ensures feasible, safe trajectories while adaptively managing uncertainty to avoid excessive conservatism. The main contributions of this work are summarized as follows:

• We propose a novel safety-preserving framework for autonomous vehicles, which integrates FRS with discrete barrier functions to ensure robust forward invariance of the safety set. Our method dynamically updates safety constraints through event-triggered online learning of HV control-intent sets to refine the FRS incrementally. This formulation addresses HV behavioral uncertainty appropriately and ensures collision-free maneuvers without reliance on accurate trajectory predictions.

• We develop a contingency trajectory optimization scheme that ensures safe trajectories with recursive feasibility. By co-optimizing a nominal performance-driven trajectory and a contingency trajectory with FRS-based barrier constraints, it ensures planning outcomes toward regions where safe maneuvers exist. This approach enables defensive driving while maintaining efficiency.

• We leverage the bi-convex structure of the barrier constraints to decompose the non-convex contingency planning problem into tractable QP subproblems using consensus ADMM. This enables efficient computation of feasible trajectories, facilitating rapid response to uncertain driving conditions.

• We validate the proposed approach on synthetic and real-world traffic datasets across different driving scenarios. The results demonstrate its ability to balance safety and efficiency, with improved passenger comfort under uncertain HV behaviors. Real-time performance is further verified through hardware experiments, which underscore its deployability in dynamic and uncertain traffic environments.

The remainder of this paper is structured as follows: Section 2 introduces the problem statement. Section 3 discusses event-triggered online learning of HV control-intent sets and FRS prediction, followed by FRS-based safety barrier design. Section 4 describes the contingency planning framework and optimization scheme with consensus ADMM, with supporting theoretical analysis. The proposed trajectory planning approach is evaluated in Section 5, and a conclusion is drawn in Section 6.

2 Problem Statement↩︎

2.1 System Modeling↩︎

We model the motion of the \(h\)-th HV’s geometric center with a discrete-time integrator for \(h\in\mathcal{I}_0^{M-1}\), where \(M\) is the total number of HVs in the considered scenario. This formulation facilitates computational tractability while maintaining sufficient accuracy for reachability analysis. With sampling time \(\delta T\), the dynamics model of the \(h\)-th HV at time instant \(t_i\) is given by: \[\label{eq:hv95model} {z}^h_{i+1}={A}{z}^h_{i}+{B}{u}_i^h,\tag{1}\] where \(A\) and \(B\) are the state and control matrices, respectively; the state vector \({z}^h_i = [{{p}^h_i}^T~ {{v}^h_i}^T]^T \in \mathbb{R}^4\) is composed of position \({p}^h_i = [p^h_{x,i}~p^h_{y,i}]^T\) and velocity \({v}^h_i = [v^h_{x,i}~ v^h_{y,i}]^T\), with control input vector \({u}^h_i = [a^h_{x,i}~ a^h_{y,i}]^T \in \mathcal{U}^h\) comprising longitudinal and lateral accelerations.

To account for perception inaccuracy, we model the HV state measurement at time \(t_i\) as \[\tilde{z}_i^h = z_i^h + \omega_i^h, \quad \|\omega^h_i\|_{{\Sigma_{\omega}^{h}}^{-1}} \leq 1, \label{eq:measurement95noise}\tag{2}\] where the noise \(\omega_i^h\) is bounded with known, positive-definite covariance \(\Sigma_{\omega}^h\).

For the \(h\)-th HV, the control capability is defined by two distinct sets: admissible control set \(\mathcal{U}^h\), which encompasses all physically feasible control inputs under worst-case constraints, and control-intent set \(\hat{\mathcal{U}}^h \subseteq \mathcal{U}^h\), encompassing the control inputs the HV is likely to execute within a finite horizon. The admissible set \(\mathcal{U}^h\) is assumed known, while the intended set \(\hat{\mathcal{U}}^h\) remains unknown a priori and should be estimated online through iterative adjustments based on observed behaviors.

To enable smooth trajectory optimization, the state vector \(x_i\in \mathcal{X}\) is augmented to include control inputs and their derivatives, defined as: \[\notag x_i = \left[p_{x,i}\quad p_{y,i}\quad\theta_{i}\quad\dot{\theta}_i\quad v_i\quad a_{x,i}\quad a_{y,i}\quad j_{x,i}\quad j_{y,i} \right]^T.\] Here, \(p_{x,i}\) and \(p_{y,i}\) denote the longitudinal and lateral positions in the global frame, respectively; \(v_i\) is the speed; and \(\theta_i\) is the heading angle. Additionally, \(a_{x,i}\) and \(a_{y,i}\) are the longitudinal and lateral accelerations, while \(j_{x,i}\) and \(j_{y,i}\) represent the longitudinal and lateral jerks. Using the yaw rate \(\dot{\theta}_i\) and acceleration \(a_i\) as control input variables, the EV is modeled as Dubins car with nonholonomic constraints: \[\begin{align} \begin{aligned} \dot{p}_{x,i} -& v_i \cos (\theta_i) = 0,\\ \dot{p}_{y,i}- &v_i\sin (\theta_i) = 0. \end{aligned} \label{eq:nonholonomic95cons} \end{align}\tag{3}\] Based on these constraints, we can further derive the following constraint of \(\theta_i\) and closed-form solution of \(v_i\) [52]: \[ \theta_i - \arctan (\dot{p}_{y,i}/{\dot{p}_{x,i}}) = 0, \label{eq:polar95nonholonomic95cons}\tag{4}\] \[ v_i = \sqrt{(\dot{p}_{x,i} )^2 + (\dot{p}_{y,i} )^2}. \label{eq:polar95vel} \tag{5}\]

2.2 Problem Statement↩︎

This study investigates trajectory optimization for autonomous vehicles in safety-critical environments where \(M\) HVs exhibit uncertain behaviors compounded by perception inaccuracies. Complex driving patterns influenced by diverse driving styles and traffic conditions make their trajectories difficult to predict. To robustly handle the uncertainties, we aim to generate collision-free trajectories by avoiding the reachable regions of HVs, characterized via FRSs over a given time horizon.

Conventional FRSs derived from admissible control sets \(\mathcal{U}^{h}\) tend to be overly conservative, as they account for worst-case actuator limitations. Conversely, using fixed bounds of control inputs may lead to either excessive caution or dangerous underestimation. In reality, the near-future behaviors of an HV are better characterized by the empirical bounds of historical trajectory data, which captures both driving style and traffic conditions [43].

Accordingly, we aim to develop a computationally efficient trajectory optimization framework that enables the EV to accommodate HV uncertainties while ensuring the existence of safe trajectories. Given the EV and HV models in Section 2.1, the problem is decomposed into three key subproblems:

(i) efficiently updating the FRS of HVs 1 by online learning the unknown control-intent sets \(\hat{\mathcal{U}}\);

(ii) formulating barrier constraints using the FRS to enable safe and computationally efficient trajectory optimization;

(iii) ensuring the feasibility of safe trajectories while balancing safety, efficiency, and passenger comfort.

3 FRS-Based Safety Barrier↩︎

This section introduces the FRS-based discrete barrier function method, addressing the subproblems (i) and (ii) in Section 2.2. Section 3.1 designs the event trigger mechanism for online learning of HV control-intent sets and FRS propagation. Section 3.2 then formulates the FRS-based safety barrier constraint.

3.1 FRS Prediction↩︎

The FRS rigorously characterizes potential future states for an HV, defined as follows:

Following [44], we employ ellipsoidal representations for both the control-intent set and the associated FRS. This ellipsoidal representation supports efficient online learning while simplifying FRS propagation. It also facilitates the construction of safety barrier constraints, improving the computational efficiency for trajectory optimization, as elaborated in Section 4.4.

3.1.1 Event-Triggered Online Learning of Control-Intent Set↩︎

The control-intent set is initialized according to the EV’s prior knowledge or assumptions on the HV, and subsequently refined through online learning.

Specifically, at each time step \(t_i\), we obtain the control input vector \(u_{i-1}^h\) from the observed state vectors. Then, a control input dataset \(\mathcal{C}_i^h=\mathcal{C}_{i-1}^h \cup \{u_{i-1}^h\}\) is maintained for the \(h\)-th HV, initialized with \(\mathcal{C}_0^h\), a user-specified estimate of potential HV control inputs.

From an estimation perspective, the elements in \(\mathcal{C}_i^h\) can be viewed as the samples drawn from an unknown distribution of \(\hat{\mathcal{U}}_i^h\). It is over-approximated using a minimum-volume enclosing ellipsoid of \(\mathcal{C}_i^h\), denoted as: \[\hat{\mathcal{U}}_i^h = \{ u \in \mathbb{R}^2 \mid (u - \mu_{u,i}^h)^T (\Sigma_{u,i}^h)^{-1} (u - \mu_{u,i}^h) \leq 1 \}, \label{eq:ellipsoid95set}\tag{6}\] with an equivalent form \(\|P_{u,i}^h u + q_{u,i}^h\|_2 \leq 1\). The parameters \(P_{u,i}^h = (\Sigma_{u,i}^h)^{-1/2}\) and \(q_{u,i}^h = -P_{u,i}^h \mu_{u,i}^h\) are obtained by solving the following semi-definite programming (SDP) problem [53]: \[\label{eq:full95sdp} \begin{align} \{P_{u,i}^h, q_{u,i}^h\} &= \underset{\{P,\,q\}}{\text{argmin}} \quad \log \det(P^{-1}) \\ \text{s.t.} \quad & \|Pu + q\|_2^2 \leq 1, \quad \forall u \in \mathcal{C}_i^h, \\ & P \in \mathbb{S}_{++}^2. \end{align}\tag{7}\]

For large datasets, Khachiyan’s algorithm [54] provides efficient computation via first-order approximation. Nevertheless, as the size of \(\mathcal{C}_i^h\) increases with the accumulation of control input samples, it introduces computational challenges that hinder real-time implementation. To mitigate these issues, we design the following condition to trigger the updates.

Triggering Condition:The updates occur when \[t_{m^\prime} = \inf_{i\delta T > t_m} i\delta T \label{eq:event95trigger95time}\tag{8}\] such that \[\|P^h_{u, m}u_{i-1}^h + q_{u, m}^h\|_2^2 \geq 1, \label{eq:trigger95condition}\tag{9}\] where \(m,\, m^\prime \in \mathbb{N}\) denote successive update time instants. Intuitively, when a new control input sample \(u_{i-1}^h\) falls outside the current estimated set \(\hat{\mathcal{U}}_m^h\), trigger an incremental update such that \(\mathcal{C}_i^h \subseteq \hat{\mathcal{U}}_{m}^h\cup \{u_{i-1}\}\subseteq \hat{\mathcal{U}}_{m^\prime}^h\).

Once the triggering condition 9 is satisfied, the parameters \(P_{u,m}^h\) and \(q_{u,m}^h\) can be incrementally obtained by solving an SDP problem ?? with fixed computational complexity, as formalized in the following lemma:

3.1.2 FRS Propagation↩︎

Using the learned control-intent set \(\hat{\mathcal{U}}_{m}^h\), we propagate the FRSs over the prediction horizon \(N\), starting at time step \(t_i\geq t_m\).

Considering perception noise, the initial FRS is given by: \[\begin{align} \mathcal{R}_{0|i}^{h,z} = \{ z \in \mathbb{R}^4 \mid (z - \tilde{z}_i^h)^\top (\Sigma_{z,0|i}^h)^{-1} (z - \tilde{z}_i^h) \leq 1 \}, \label{eq:init95reachable95set} \end{align}\tag{10}\] where \(\Sigma_{z,0|i}^h=\Sigma_{\omega}^h\).

Then, the FRS \(\mathcal{R}_{k|i}^{h,z}\) at predicted time step \(k\) starting from \(t_{i}\) is computed recursively: \[\begin{align} \mathcal{R}_{k|i}^{h,z} &= A \mathcal{R}_{k-1|i}^{h,z} \oplus B \hat{\mathcal{U}}_m^h, \quad k \in \mathcal{I}_1^N. \label{eq:reachable95recursion} \end{align}\tag{11}\]

Although \(\mathcal{R}_{0|i}^{h,z}\) and \(\hat{\mathcal{U}}_m^h\) are ellipsoids, their Minkowski sum \(\mathcal{R}_{k|i}^{h,z}\) is typically not an ellipsoid [55]. We therefore construct an efficient ellipsoidal over-approximation: \[\hat{\mathcal{R}}_{k|i}^{h,z} = \left\{ z \in \mathbb{R}^4 \, | \, (z - \mu_{z,k|i}^{h})^{T}(\hat{\Sigma}_{z,k|i}^{h})^{-1}(z - \mu_{z,k|i}^{h}) \leq 1 \right\},\] with parameters \((\mu_{z,k|i}^{h}, \hat{\Sigma}_{z,k|i}^{h})\) given by: \[\begin{align} \mu_{z,k|i}^{h} &= A \mu_{z,k-1|i}^{h} + B \mu_{u,m}^h, \tag{12} \\ \hat{\Sigma}_{z,k|i}^{h} &= \kappa ( {\tilde{\Sigma}_{z,k-1|i}^{h}}/{\rho_{z,k-1|i}} + {\tilde{\Sigma}_{u,m}^h}/{\rho_{u,m}} ), \tag{13} \end{align}\] where \(\tilde{\Sigma}_{z,k-1|i}^{h} = A \hat{\Sigma}_{z,k-1|i}^{h} A^T\); \(\tilde{\Sigma}_{u,m}^h = B \Sigma_{u,m}^h B^T + \epsilon_2 I_4\); and directional scaling factors \(\rho_{z,k-1|i} = \sqrt{l^T \hat{\Sigma}_{z,k-1|i}^{h} l}\), \(\rho_{u,m} = \sqrt{l^T \tilde{\Sigma}_{u,m}^h l}\), \(\kappa = \rho_{z,k-1|i} + \rho_{u,m}\) are computed along direction \(l\).

3.1.3 Occupancy Prediction↩︎

Based on the FRS prediction, the positional occupancy \(\mathcal{R}_{k|i}^{h} = \text{Proj}_{\text{position}}(\hat{\mathcal{R}}_{k|i}^{h,z})\) is obtained via projection: \[{\mathcal{R}}_{k|i}^{h} = \left\{ p \in \mathbb{R}^2 \, | \, (p - \mu_{p,k|i}^{h})^{T}(\hat{\Sigma}_{p,k|i}^{h})^{-1}(p - \mu_{p,k|i}^{h}) \leq 1 \right\}. \label{eq:occupancy95projection}\tag{14}\] However, considering only the HV center while neglecting the geometry is insufficient for safety assurance. Therefore, we employ an ellipsoidal geometry set to explicitly account for the full shape characteristics of both the HV and EV, which is represented by \(\mathcal{D}_{safe} = \left\{ p \in \mathbb{R}^2 \, |\, p^{T}(\Sigma^h_{safe})^{-1}p \leq 1 \right\}\). Its lengths of major and minor axes are denoted as \(l^h_{x}\) and \(l^h_{y}\), respectively.

Thus, the complete HV FRS occupancy can be derived as an external ellipsoidal approximation of \(\mathcal{R}_{k|i}^{h}\oplus\mathcal{D}_{safe}\), denoted by: \[\hat{\mathcal{R}}_{k|i}^{h} = \left\{ p \in \mathbb{R}^2 \,|\, (p - \mu_{p,k|i}^{h})^{T}(\hat{\Sigma}_{p,k|i}^{h})^{-1}(p - \mu_{p,k|i}^{h}) \leq 1 \right\}, \label{eq:actual95occupancy95projection}\tag{15}\] where \(\mu_{p,k|i}^{h} = [\mu_{x,k|i}^{h}~\mu_{y,k|i}^{h}]^T\) denotes the center of the positional occupancy \(\hat{\mathcal{R}}_{k|i}^{h}\). The ellipsoidal approximation can be computed similarly to 12 and 13 . We denote the lengths of major and minor axes of the ellipsoid \(\hat{\mathcal{R}}_{k|i}^{h}\) as \(l^h_{x,k|i}\) and \(l^h_{y,k|i}\), respectively.

Let \(\hat{\mathcal{O}}^h_{k|i} = z^h_{p,{k|i}} \oplus \mathcal{D}_{\text{safe}}\) denote the predicted positional occupancy at predicted time step \(k\) from \(t_i\), with \(z^{h}_{p,{k|i}} = [z^{h}_{x,{k|i}}\, z^{h}_{y,{k|i}}]^T\) denoting the position vector, while \({\mathcal{O}}^h_{k|i}\) represents the actual positional occupancy at time instant \(t_{i+k}\).

3.2 FRS-Based Safety Barrier↩︎

The safe set can be characterized by a sublevel set of a continuously differentiable function \(\mathscr{B}\): \[\mathcal{F}(\mathcal{O}^{h}) =\{x_i\in\mathcal{X} \mid \mathscr{B}(x_i, \mathcal{O}^{h})\geq 0\} \;\label{eq:safe95set}\tag{16}\]

Following the methodology in [2], we employ polar coordinate transformations to formulate the nominal deterministic safety barrier constraints for collision avoidance as: \[\begin{align} \left\{ \begin{aligned} p_{x,{k|i}}& = z^{h}_{x,{k|i}} + l^{h}_{x} d^{h}_{k|i} \cos(\omega^{h}_{k|i}), \\ p_{y,{k|i}}& = z^{h}_{y,{k|i}} + l^{h}_{y} d^{h}_{k|i} \sin(\omega^{h}_{k|i}), \\ \Delta \mathscr{B}&{(x_{k|i}, \hat{\mathcal{O}}^{h}_{k|i}) + \alpha \mathscr{B}(x_{k|i}, \hat{\mathcal{O}}^{h}_{k|i}) \geq 0},\, k\in\mathcal{I}_0^{N-1}. \end{aligned} \right. \label{eq:polar95prediction95safety} \end{align}\tag{17}\] Here, the parameters \(l^{h}_{x}\) and \(l^{h}_{y}\) represent the semi-axes of the ellipsoidal safety boundary, and barrier function \(\mathscr{B}\) is defined as \(\mathscr{B}(x_{k|i}, \hat{\mathcal{O}}_{k|i}^{h})=d_{k|i}^{h}(x_{k|i}, \hat{\mathcal{O}}_{k|i}^{h})-1\), where \(d_{k|i}^{h}\) is the Mahalanobis distance from \(x_{k|i}\) to the ellipsoid \(\hat{\mathcal{O}}_{k|i}^{h}\). Intuitively, when a trajectory point \([p_{x,{k|i}}\, p_{y,{k|i}}]^T\) of the EV penetrates the obstacle region centered at \([z^{h}_{x,{k|i}}\, z^{h}_{y,{k|i}}]^T\), it is pushed outward along an optimal repulsion direction \(\omega^{h}_{k|i}\): \[\label{eq:obtain95omega} \omega^{h}_{k|i} = \arctan\left(\frac{l^{h}_{x}(p_{y,{k|i}} - z^{h}_{y,{k|i}})}{l^{h}_{y}(p_{x,{k|i}} - z^{h}_{x,{k|i}})}\right)\tag{18}\] Note that the constraint 17 characterizes the collision avoidance by maintaining a minimum safety margin throughout the planning horizon, with an equivalent polar coordinate formulation: \[\label{eq:barrier95cons95polar} d^{h}_{k+1|i} -1 - (1- \alpha) ( d^{h}_{k|i} -1) \geq 0. \tag{19}\]

However, since accurate trajectory prediction of HVs are typically unavailable, we utilize the obtained ellipsoid FRS occupancy \(\hat{\mathcal{R}}_{k|i}^{h}\) to construct the FRS-based safety barrier constraints as shown in Fig. 2, which is derived as: \[\begin{align} \left\{ \begin{aligned} p_{x,{k|i}}& = \mu^{h}_{x,{k|i}} + l^{h}_{x,{k|i}} d^{h}_{k|i} \cos(\omega^{h}_{k|i}), \\ p_{y,{k|i}}& = \mu^{h}_{y,{k|i}} + l^{h}_{y,{k|i}} d^{h}_{k|i} \sin(\omega^{h}_{k|i}), \\ \Delta \mathscr{B}&{(x_{k|i}, \hat{\mathcal{R}}^{h}_{k|i}) + \alpha \mathscr{B}(x_{k|i}, \hat{\mathcal{R}}^{h}_{k|i}) \geq 0},\, k\in\mathcal{I}_0^{N-1}. \end{aligned} \right. \label{eq:polar95reachable95safety} \end{align}\tag{20}\]

The barrier function introduces a fundamental duality principle that a larger obstacle region induces a smaller safe set.

It can be shown that the \(\mathcal{F}(\hat{\mathcal{R}}_{k|i}^{h})\) is a robust safe set of the EV with respect to the \(h\)-th HV.

4 Safe Contingency Trajectory Optimization↩︎

In this section, we first introduce the trajectory representation based on Bézier curves for smoothness and computational efficiency. Next, we formulate a contingency optimization problem with FRS-based safety barrier constraints to enforce safety. Finally, leveraging the biconvex structure of the optimization problem, we derive an equivalent reformulation that enables efficient alternating optimization using ADMM.

4.1 Trajectory Parameterization↩︎

We employ Bézier curves to parameterize EV trajectories over a finite duration of \(T\). This representation exploits the hodograph property to directly constrain higher-order derivatives, ensuring dynamical feasibility [57]. A degree-\(n\) Bézier curve is constructed using \((n+1)\) control points over a normalized interval \(\nu \in [0, 1]\): \[b(\nu) = \sum_{j=0}^{n} c_{j} b_{j,n}(\nu)=c^T\mathcal{B}_v(\nu), \notag\] where \(c = [c_{0}\, \dots\, c_{n}]^T \in \mathbb{R}^{n+1}\) are control points, \(\mathcal{B}_v(\nu) = [b_{0,n}(\nu)\,\dots\,b_{n,n}(\nu)]^T\in \mathbb{R}^{n+1}\) consists of Bernstein polynomial basis \(b_{j,n}(\nu)= \binom{n}{j} \nu^j (1 - \nu)^{n-j}\), and \(\nu = (t - t_i)/T\in[0,1]\) is the normalized time. Discrete-time points are obtained as: \[b_{k} = c^T\mathcal{B}_{k},\] where \(\mathcal{B}_{k}=\mathcal{B}_v(k\delta T)\), and \(\delta T=T/N\) is the fixed time step.

The state vector of EV can then be parameterized as: \[x_{k|i}=\mathfrak{p}(c_x, c_y, c_\theta).\] Specifically, three distinct Bézier curves are used to represent the longitudinal displacement, lateral displacement and heading angle, which are denoted by \(p_{x,k|i}=c_x^T \mathcal{B}_{k}\), \(p_{y,{k|i}}=c_y^T\mathcal{B}_{k}\), and \(p_{\theta,{k|i}}=c_\theta^T\mathcal{B}_{k}\), respectively, with corresponding control points \(c_x\), \(c_y\), and \(c_\theta\). Higher-order derivatives follow from the hodograph property: \(\dot{\theta}_{k|i}={c_\theta}^T\dot{\mathcal{B}}_{k}\), \(v_{k|i}=({c_x}^T\dot{\mathcal{B}}_{k} + {c_y}^T\dot{\mathcal{B}}_{k})^{\frac{1}{2}}\), \(a_{x,{k|i}}={c_x}^T\ddot{\mathcal{B}}_{k}\), \(a_{y,{k|i}}={c_y}^T\ddot{\mathcal{B}}_{k}\), \(j_{x,{k|i}}={c_x}^T\dddot{\mathcal{B}}_{k}\), and \(j_{y,{k|i}}={c_y}^T\dddot{\mathcal{B}}_{k}\).

4.2 Safe Contingency Trajectory Optimization↩︎

We formulate a trajectory optimization problem through simultaneous optimization of a nominal trajectory and a contingency trajectory constrained by FRS-based safety barriers. For clarity, the following formulations are presented for an arbitrary HV indexed by \(h \in \mathcal{I}_0^{M-1}\), with all \(h\)-indexed quantities defined per-HV and naturally extendable to all \(M\) HVs.

While more accurate prediction methods exist [58], we intentionally employ a constant-velocity model to explicitly demonstrate the robustness to prediction errors. Notably, our framework works with any HV prediction method as long as the predicted states meet this basic containment condition:

This assumption is enforceable because the HV prediction method can be systematically designed to ensure containment through proper control constraint characterization. It is trivially satisfied under constant-velocity prediction due to the conservative nature of FRS propagation.

The optimization problem at \(t_i\) is formulated as: \[\tag{21}\begin{align} &\displaystyle\operatorname*{min}_{\substack{ \{c^r_x,c^r_y,c^r_\theta, c^s_x,c^s_y,c^s_\theta\} }}~~ \mathcal{J}\tag{22}\\ \quad\text{s.t.}\quad &x^{r}_{0|i}=x(t_i), \quad x^{s}_{0|i}=x(t_i), \tag{23}\\ & x^{r}_{k|i} \,\in \mathcal{X}, \quad \,\, \,\,\,\, \, x^{s}_{k|i} \,\in \mathcal{X}\tag{24}\\ & x^{r}_{N|i} \in \mathcal{X}_f, \quad \,\, \, \, x^{s}_{N|i} \in \mathcal{X}_f\tag{25}\\ & x^{r}_{k+1|i} = f(x^{r}_{k|i}), \tag{26}\\ & x^{s}_{k+1|i} = f(x^{s}_{k|i}), \tag{27}\\ & x^{r}_{k|i}=\mathfrak{p}(c^r_x, c^r_y, c^r_{\theta}), \tag{28}\\ & x^{s}_{k|i}=\mathfrak{p}(c^s_x, c^s_y, c^s_{\theta}), \tag{29}\\ & x^{r}_{k|i} \text{ satisfies \eqref{eq:polar95prediction95safety} }, \tag{30}\\ & x^{s}_{k|i} \text{ satisfies \eqref{eq:polar95reachable95safety} }, \tag{31}\\ & \varphi( x^{r}_{l|i}) = \varphi( x^{s}_{l|i}), \quad l\in\mathcal{I}_0^{N_s-1}, \tag{32}\\ & \forall k\in\mathcal{I}_0^{N-1}. \nonumber\end{align}\] Here, \(x^r_k\) and \(x^s_k\) represent the state vectors for the nominal and safe contingency trajectories, respectively, with corresponding control points \(c^r_x\), \(c^r_y\), \(c^r_\theta\), and \(c^s_x\), \(c^s_y\), \(c^s_\theta\). \(x(t_i)\) denotes current state vector of the EV at \(t_i\). The function \(f(\cdot)\) implements the kinematic constraint 3 .

The objective function \(\mathcal{J}\) combines the desired lateral position \(p_{y,d}\) and longitudinal speed \(v_{x,d}\) with trajectory smoothness, expressed as: \[\begin{align} \small \mathcal{J} = &(1-p_s) \sum_{k=0}^{N-1} w^r_x({c^r_x}^T{c^r_x})^2 + w^r_y({c^r_y}^T{c^r_y})^2 + w^r_\theta({c^r_\theta}^T{c^r_\theta})^2 \\ & + w^r_{v,d}({c^r_x}^T\dot{\mathcal{B}}_k-v_{x,d})^2+ w^r_{y,d}({c^r_y}^T\mathcal{B}_k-p_{y,d})^2\\ &+ p_s \sum_{k=0}^{N-1} w^s_x({c^s_x}^T{c^s_x})^2 + w^s_y({c^s_y}^T{c^s_y})^2 + w^s_\theta({c^s_\theta}^T{c^s_\theta})^2 \\ & + w^s_{v,d}({c^s_x}^T\dot{\mathcal{B}}_k-v_{x,d})^2+ w^s_{y,d}({c^s_y}^T\mathcal{B}_k-p_{y,d})^2, \label{eq:obj95func} \end{align}\tag{33}\] where the weights \(p_s\), \(w^r_x\), \(w^r_y\), \(w^r_\theta\), \(w^r_{v,d}\), \(w^r_{y,d}\), \(w^s_x\), \(w^s_y\), \(w^s_\theta\), \(w^s_{v,d}\), and \(w^s_{y,d}\) are all positive constants.

The two parallel trajectories share the same dynamics inherent in the hodograph property of the Bézier curve 28 29 , initial conditions 23 , terminal state constraints 25 with terminal state set \(\mathcal{X}_f\), and kinematic constraints as stated in 26 27 . We tie them together by requiring the initial segments of length \(N_s\), \(N_s\leq N\), to be consistent 32 . For safety assurance, the state vectors \(x^r_k\) over the nominal horizon are required to remain within the safe state set with respect to the HV predictions \(\hat{\mathcal{O}}^{h}_k\), as stated in 30 . To handle behavioral uncertainty, we impose the FRS-based safety barrier 31 as safeguards against possible HV behaviors in the contingency horizon.

The weighting parameter \(p_s\) balances nominal planning and contingency handling via FRS-based barrier constraints and can be adjusted according to the desired level of conservatism. The contingency planning formulation in 21 minimizes the expected cost over both trajectories. As prediction confidence increases, smaller values of \(p_s\) can be used. This approach generates a nominal trajectory based on prediction \(\hat{\mathcal{O}}_k(\cdot)\) while consistently maintaining a safe contingency plan.

4.3 Planning-Horizon Configurations with Formal Guarantees↩︎

We analyze two standard planning-horizon configurations used by the proposed planning framework: a shrinking horizon and a receding horizon. The former captures short, goal-directed maneuvers that must complete within a known mission duration (e.g., mandated lane change to reach an exit, controlled stop into a refuge bay). It also serves as a safety mode should perception temporarily fail. The latter reflects the normal navigation regime in which fresh predictions and FRS updates arrive as time advances.

Shrinking-Horizon Configuration

Let \(N_{\mathrm{SH}}\) denote the total number of mission steps remaining at \(t_0\). At generic time step index \(i\) with current time \(t_i\), the optimization horizon in 22 is chosen to span the remaining mission duration: \[\label{eq:shrinking95horizon95n} N = N_{\mathrm{SH}} - i.\tag{34}\] This configuration is appropriate when the EV must guarantee completion of a finite-time maneuver under bounded uncertainty without relying on future FRS updates.

We next establish that, under mild conditions, feasibility and safety propagate forward as the horizon shrinks.

Receding-Horizon Configuration

For routine navigation in dynamic environments, we employ a fixed horizon: \[ \label{eq:receding95horizon95n} N = N_{\mathrm{RH}} \tag{35}\] which shifts forward one step at each planning instant. This allows continuous incorporation of updated HV intent and FRS information and is the standard operating mode of our framework.

4.4 Contingency Planning with Consensus ADMM↩︎

4.4.1 Reformulation of Trajectory Optimization↩︎

Building upon the optimization problem 21 , we present a compact and equivalent reformulation as follows: \[\tag{36} \begin{align} \displaystyle\operatorname*{min}_{\substack{\{c_\theta, c_x,c_y\} \\ \{{\omega}, {d}\}}}~~ \;&f(c_\theta) + f_x(c_x) + f_y(c_y) \tag{37}\\ \operatorname*{s.t.}\quad\quad & F_0 [c_{x}\;c_{y}]-[E_{x,0}\;E_{y,0}]=0,\tag{38}\\ & F_{\theta,0} c_{\theta}=E_{\theta,0}, \, F_{\theta,f} c_{\theta}=E_{\theta,f},\tag{39}\\ & {\omega} \in \mathcal{C}_{\omega}, {d} \in \mathcal{C}_d, \tag{40}\\ \mathcal{G}_{k,x} \triangleq\,& \dot{\mathcal{B}}^T c_{x} - \mathbf{v} \cdot \cos{ \mathcal{B}^Tc_{\theta}} = 0, \tag{41}\\ \mathcal{G}_{k,y} \triangleq\,& \dot{\mathcal{B}}^T c_{y} - \mathbf{v} \cdot \sin{ \mathcal{B}^Tc_{\theta}} = 0, \tag{42}\\ \mathcal{G}_{bo,x} \triangleq\,& \mathcal{B}^T c^r_{x} - \mathcal{O}_{x} - {l}^r_x \cdot {d}^r \cdot \cos{{\omega}^r} = 0, \tag{43}\\ \mathcal{G}_{bo,y} \triangleq\,&\mathcal{B}^T c^r_{y} - \mathcal{O}_{y} - {l}^r_y \cdot {d}^r \cdot \sin{{\omega}^r} = 0, \tag{44}\\ \mathcal{G}_{br,x} \triangleq\,& \mathcal{B}^T c^s_{x} - \mathcal{R}_{x} - {l}^s_x \cdot {d}^s \cdot \cos{{\omega}^s} = 0, \tag{45}\\ \mathcal{G}_{br,y} \triangleq\,&\mathcal{B}^T c^s_{y} - \mathcal{R}_{y} - {l}^s_y \cdot {d}^s \cdot \sin{{\omega}^s} = 0, \tag{46}\\ \mathcal{G}_{cs,x} \triangleq\,& {A}^T_{c,x} c_x - {Y}_{x}=0, ,\tag{47} \\ \mathcal{G}_{cs,y} \triangleq\,& {A}^T_{c,y} c_y - {Y}_{y} =0 ,\tag{48} \\ \mathcal{G}_{cs,\theta} \triangleq\,& {A}^T_{c,\theta} c_{\theta} - {Y}_{\theta}=0 ,\tag{49} \\ \mathcal{G}_{c,x} \triangleq\,&{G} c_x -{h}_x \leq 0, \tag{50} \\ \mathcal{G}_{c,y} \triangleq\,&{G} c_y - {h}_y \leq 0. \tag{51} \end{align}\] Here, \(\triangleq\) denotes the definition operator for convenience. The optimization variables are denoted as \(c_x=[c^r_x \;c_x^s]\in\mathbb{R}^{(n+1)\times 2},c_\theta=[c^r_\theta \;c_\theta^s]\in\mathbb{R}^{(n+1)\times 2}\), and \(c_y=[c^r_y \;c_y^s]\in\mathbb{R}^{(n+1)\times 2}\). The variables \({\omega} = [{\omega}^r\;{\omega}^s] \in \mathbb{R}^{(N \times M)\times 2}\) correspond to the matrix stacking relative angle \(\omega^{h}_k\), for \(M\) HVs within the planning horizon \(N\), while \({d} = [{d}^r\;{d}^s] \in \mathbb{R}^{(N \times M) \times 2}\) follows an analogous structure. The cost function 37 comprises three components: \[\small \begin{align} &f(c_\theta) = \frac{1}{2}c_\theta^T Q_\theta c_\theta \tag{52}, \\ &f(c_x) = \frac{1}{2}c_x^T Q_x c_x + \frac{1}{2}c_x^T \dot{\mathcal{B}} Q_{x,v} \dot{\mathcal{B}}^T c_x - v_{x,d}^T Q_{x,d} \dot{\mathcal{B}}^T c_x, \tag{53} \\ &f(c_y) = \frac{1}{2}c_y^T Q_{y} c_y + \frac{1}{2}c_y^T \dot{\mathcal{B}} Q_{y,v} \dot{\mathcal{B}}^T c_y - v_{y,d}^T Q_{y,d} \dot{\mathcal{B}}^T c_y, \tag{54} \end{align}\] where \(Q_\theta\), \(Q_x\), \(Q_y\), \(Q_{x,v}\), \(Q_{x,d}\), \(Q_{y,v}\), and \(Q_{y,d}\) denote symmetric positive semi-definite weighting matrices.

The constraint matrices for the initial and terminal state constraints 38 39 are given by \(F_0=[\mathcal{B}_0\, \dot{\mathcal{B}}_0\, \ddot{\mathcal{B}}_0]^T\in\mathbb{R}^{3\times(n+1)}\), \(F_{\theta,0}=[\mathcal{B}_0\, \dot{\mathcal{B}}_0]^T\in\mathbb{R}^{2\times(n+1)}\), and \(F_{\theta,f}=[\mathcal{B}_N\, \dot{\mathcal{B}}_N]^T\in\mathbb{R}^{2\times(n+1)}\), respectively. The vectors \({E}_{x,0}, {E}_{y,0} \in \mathbb{R}^{3\times2}\) encode the initial position, velocity and acceleration constraints along longitudinal and lateral components for both nominal and contingency trajectories, while \({E}_{\theta,0}, \, {E}_{\theta,f} \in \mathbb{R}^{2\times2}\) encode the initial and terminal state vectors for orientation angles and angular velocities.

The kinematic constraints 41 and 42 derived from 26 and 27 , respectively, are enforced through the composite speed matrix \(\mathbf{v} = [\mathbf{v}^r\; \mathbf{v}^s] \in \mathbb{R}^{N\times 2}\), where \(\mathbf{v}^r\) and \(\mathbf{v}^s\) represent the speed profiles for the nominal and contingency trajectories over the \(N\)-step horizon.

The collision avoidance constraints 43 44 and 45 46 correspond to the safety conditions 30 and 31 , respectively. The predicted longitudinal and lateral positions of HVs for the nominal trajectory are denoted by \(\mathcal{O}_{x} ,\mathcal{O}_{y} \in \mathbb{R}^{N \times M}\) while \(\mathcal{R}_{x} ,\mathcal{R}_{y} \in \mathbb{R}^{N \times M}\) represent the corresponding FRS occupancy centers for the contingency trajectory. The safety ellipsoid dimension \({l}^r_x\in \mathbb{R}^{N\times M}\) is constructed by stacking \(l^{h}_x\) along the nominal trajectory, while \({l}^r_y, {l}^s_x, {l}^s_y\) are constructed analogously.

For the state constraints 50 and 51 implementing 24 , the constraint matrix \({G} = [\mathcal{B}^T\;-\mathcal{B}^T\;\ddot{\mathcal{B}}^T\;-\ddot{\mathcal{B}}^T\;\dddot{\mathcal{B}}^T\;-\dddot{\mathcal{B}}^T]^T\in \mathbb{R}^{6N\times (n+1)}\) is constructed from \(\mathcal{B}=[\mathcal{B}_1\, \cdots \,\mathcal{B}_N]\in\mathbb{R}^{(n+1)\times N}\). The matrices \({h}_x,{h}_y \in \mathbb{R}^{6N\times 2}\) contain the corresponding state bounds for longitudinal and lateral motions, respectively.

The constraints 47 49 enforce trajectory consistency for position, velocity, and acceleration at the initial segments between two trajectories. The corresponding matrices are \({A}_{c,x} ={A}_{c,y} = [\mathcal{B}_{[1:N_s]}^T\;\dot{\mathcal{B}}_{[1:N_s]}^T\;\ddot{\mathcal{B}}_{[1:N_s]}^T]^T\in \mathbb{R}^{((n+1) \times 3) \times N_s}\) and \({A}_{c,\theta} = \mathcal{B}_{[1:N_s]} \in \mathbb{R}^{(n+1) \times N_s}\). For the consensus variables, each column of \({Y}_{x} \in \mathbb{R}^{3N_s \times N_c}\), \({Y}_{y} \in \mathbb{R}^{3N_s \times N_c}\), and \({Y}_{\theta} \in \mathbb{R}^{N_s \times N_c}\) maintains consistency across candidate trajectories during ADMM iterations.

4.4.2 ADMM Iterations↩︎

The barrier constraints 43 –46 exhibit biconvexity in \([c_{x}\, c_{y}\, {d}]^T\) and \([\cos\omega\, \sin\omega]^T\). This biconvex structure allows decomposition of the joint constraints 43 46 into two alternating optimizations: (i) Position optimization of \([c_x\, c_y]^T\) and \({d}\) with fixed \({\omega}\); (ii) Angular optimization of \({\omega}\) with fixed \([c_x\, c_y]^T\) and \({d}\). Similarly, the kinematic constraints 41 42 admit an analogous decomposition, alternating between position variables \([c_x\, c_y]^T\) (fixed \(c_\theta\)) and heading angle \(c_\theta\) (fixed \([c_x\, c_y]^T\)). Given that the quadratic objective 37 preserves convexity, this constraint structure enables efficient ADMM-based decomposition of 36 into quadratic programming (QP) subproblems.

We derive an augmented Lagrangian of 36 for ADMM iterations, as follows: \[\begin{align}&\mathcal{L}({ c_x, c_y, c_{\theta} }, {{Z}_x, {Z}_y}, {{\omega}, {d}}, {Y}_x,{Y}_y,{Y}_{\theta} \nonumber\\ & \quad \quad {\lambda}_{x}, {\lambda}_{y}, {{\lambda}_{\theta}, {\lambda}_{\text{obs},x}, {\lambda}_{\text{obs},y}, {\lambda}_{c,x}, {\lambda}_{c,y}, {\lambda}_{c,\theta}} ) \nonumber\\ &= f_x(c_x) + f_y(c_y) +f(c_\theta) + \mathcal{I}_{+}({Z}_x) + \mathcal{I}_{+}({Z}_y) \nonumber\\ &\quad+ {\lambda}^T_{x}c_x + {\lambda}^T_{y}c_y + \rho_x \left\| \mathcal{G}_{c,x} + {Z}_x \right\|_{2}^{2} + \rho_y \left\| \mathcal{G}_{c,y} + {Z}_y \right\|_{2}^{2}\nonumber \\ &\quad+ \rho_{\theta} \left\| \mathcal{G}_{k,x} + \frac{{\lambda}_{\theta} }{\rho_{\theta}} \right\|_{2}^{2} + \rho_{\theta} \left\| \mathcal{G}_{k,y} +\frac{{\lambda}_{\theta} }{\rho_{\theta}} \right\|_{2}^{2} \nonumber \\ &\quad+ \rho^r_{\text{obs}} \left\| \mathcal{G}_{bo,x} + \frac{ {\lambda}^r_{\text{obs},x}}{\rho^r_{\text{obs}}} \right\|_{2}^{2} + \rho^r_{\text{obs}} \left\| \mathcal{G}_{bo,y} + \frac{{\lambda}^r_{\text{obs},y}}{\rho^r_{\text{obs}}} \right\|_{2}^{2} \nonumber\\ &\quad+ \rho^s_{\text{obs}} \left\| \mathcal{G}_{br,x} + \frac{ {\lambda}^s_{\text{obs},x}}{\rho^s_{\text{obs}}} \right\|_{2}^{2} + \rho^s_{\text{obs}} \left\| \mathcal{G}_{br,y} + \frac{{\lambda}^s_{\text{obs},y}}{\rho^s_{\text{obs}}} \right\|_{2}^{2} \nonumber\\ &\quad+ \rho_{c,x } \left\| \mathcal{G}_{cs,x} + \frac{ {\lambda}_{c,x }}{\rho_{c,x}} \right\|_{2}^{2} + \rho_{c, y } \left\| \mathcal{G}_{cs,y} + \frac{ {\lambda}_{c,y }}{\rho_{c,y}} \right\|_{2}^{2} \nonumber\\ &\quad+ \rho_{c,\theta } \left\| \mathcal{G}_{cs,\theta} + \frac{ {\lambda}_{c,\theta }}{\rho_{c,\theta}} \right\|_{2}^{2}. \label{lang95dual95problem}\end{align}\tag{55}\]

In this formulation, the dual variables \({\lambda}_{x}, {\lambda}_{y} \in \mathbb{R}^{(n+1)\times 2}\) correspond to inequality constraints 50 51 for iteration stability, following [59]; \(\rho_{x}\), \(\rho_{y}\) \(\rho_{\theta}\), \(\rho^r_{\text{obs}}\), and \(\rho^s_{\text{obs}}\) denote the associated \(l_2\) penalty parameters. The consensus dual variables \({\lambda}_{c,x} = [{\lambda}^r_{c,x}\, {\lambda}^s_{c,x}]\in \mathbb{R}^{3N_s \times 2}\), \({\lambda}_{c,y} = [{\lambda}^r_{c,y}\, {\lambda}^s_{c,y}]\in \mathbb{R}^{3N_s \times 2}\) and \({\lambda}_{c,\theta} = [{\lambda}^r_{c,\theta}\, {\lambda}^s_{c,\theta}] \in \mathbb{R}^{N_s\times 2}\) enforce the constraints 47 49 ; \(\rho_{c,x}\), \(\rho_{c,y}\), \(\rho_{c,\theta}\) are the corresponding \(l_2\) penalty parameters, driving local variables toward their consensus global values \({Y}_x\), \({Y}_y\), and \({Y}_{\theta}\). The remaining dual variables \({\lambda}_{\theta}\in \mathbb{R}^{N\times 2}\), \({\lambda}_{\text{obs},x} = [{\lambda}^r_{\text{obs},x}\, {\lambda}^s_{\text{obs},x}] \in \mathbb{R}^{(N\times M)\times 2}\), and \({\lambda}_{\text{obs},y} = [{\lambda}^r_{\text{obs},y}\, {\lambda}^s_{\text{obs},y}] \in \mathbb{R}^{(N\times M)\times 2}\) are assigned to the constraints 43 46 . We introduce slack variables \(Z_x, Z_y\) to handle potential constraint violations in 50 and 51 . The indicator function \(\mathcal{I}_{+}(\cdot)\) enforces exact constraint satisfaction: \(\mathcal{I}_{+}(Z) = 0 \text{ if } Z \geq 0, \text{and}\;\infty \text{ otherwise}\).

Following the ADMM framework [2], the augmented Lagrangian function 55 naturally partitions the primal variables into five distinct groups for alternate optimization: heading angle \(c_\theta\), longitudinal variables \(\{{c}_x, {Z}_x\}\), lateral variables \(\{{c}_y, {Z}_y\}\), angular variables \({\omega}\), and distance variables \({d}\).

Primal Update. The primal variables are updated sequentially by solving the following QP subproblems: \[\begin{align} {2} c^{\iota+1}_{\theta} &= \displaystyle\operatorname*{\text{argmin}}_{c_{\theta}}~~ \mathcal{L} \Big(c_\theta, \{\cdot\}^\iota \Big) \;\text{s.t.} \; [F_{\theta,0}\,F_{\theta,f}] c_{\theta} = [E_{\theta,0}\,E_{\theta,f}], \nonumber \end{align}\] \[\begin{align} {2} c^{\iota+1}_{x} &= \displaystyle\operatorname*{ \text{argmin}}_{c_{x}}~~ \mathcal{L} \Big(c_x, c^{\iota+1}_{\theta}, \{\cdot\}^\iota \Big) \;\text{s.t.}\; F_0 c_{x} = E_{x,0}, \nonumber \end{align}\] \[\begin{align} {2} c^{\iota+1}_{y} &= \displaystyle\operatorname*{ \text{argmin}}_{c_{y}}~~ \mathcal{L} \Big( c_y, c^{\iota+1}_{x}, c^{\iota+1}_{\theta}, \{\cdot\}^{\iota} \Big) \;\text{s.t.}\; F_0 c_{y} = E_{y,0}, \nonumber \end{align}\] where \(\{\cdot\}^\iota\) compactly represents all other variables of \(\mathcal{L}\) fixed at iteration \(\iota\).

To handle the slack variables \(Z_x, Z_y\) for the inequality constraints 50 51 , the iteration follows the form \[Z^{\iota+1}_\bullet = \max(0, F_\bullet - G c^{\iota+1}_\bullet), \quad \bullet \in \{x,y\} \label{eq:z95x95relaxedadmm95update} \nonumber\tag{56}\] to strictly enforce the inequality constraints [60].

By leveraging the conditions in 18 and 19 , we derive analytical solutions for \(\omega\) and \(d\): \[\small \begin{align} \omega^{r,\iota+1} &= \arctan\left(\frac{l^r_x \cdot (\mathbf{v}^r c^{\iota+1}_y - \mathcal{O}_y)}{l^r_y \cdot (\mathbf{v}^r c^{\iota+1}_x - \mathcal{O}_x)}\right), \nonumber \\ \omega^{s,\iota+1} &= \arctan\left(\frac{l^s_x \cdot (\mathbf{v}^s c^{\iota+1}_y - \mathcal{R}_y)}{l^s_y \cdot (\mathbf{v}^s c^{\iota+1}_x - \mathcal{R}_x)}\right), \nonumber \\ d^{\iota+1} &= \max\Big(1, 1 + (1-\alpha)(d^\iota -1)\Big). \nonumber \end{align}\]

Consensus Variables Update. As established in [61], the dual variables for consensus constraints maintain the zero-mean property: \[\lambda^{r,\iota}_{c,\bullet} + \lambda^{s,\iota}_{c,\bullet} = 0, \quad \bullet \in \{x,y,\theta\}.\nonumber\] This structural property persists through ADMM iterations. The global variables update according to: \[\small \begin{align} Y^{\iota+1}_\bullet[:,0] = Y^{\iota+1}_\bullet[:,1] = \frac{1}{2}A^T_{c,\bullet}\Big(c^{\iota+1}_\bullet[:,0] + c^{\iota+1}_\bullet[:,1]\Big), \\ \quad \bullet \in \{x,y,\theta\}.\nonumber \end{align} \label{eq:consensus95x95admm95update}\tag{57}\]

Dual Variables Update. The dual variables are updated through: \[\small \begin{align} &\lambda^{\iota+1}_\theta = \lambda^\iota_\theta + \rho_\theta \Big(\mathcal{B}^Tc^{\iota+1}_\theta - \arctan\Big(\frac{\dot{\mathcal{B}}^T c^{\iota+1}_y}{\dot{\mathcal{B}}^T c^{\iota+1}_x}\Big)\Big), \notag\\ &\lambda^{\iota+1}_\bullet = \lambda^\iota_\bullet + \rho_\bullet \Big((1-\alpha_\bullet)(Z^{\iota+1}_\bullet - Z^\iota_\bullet) \notag\\& \quad\quad \quad \quad \quad + \alpha_\bullet (G c^{\iota+1}_\bullet - F_\bullet + Z^{\iota+1}_\bullet)\Big), \notag\\ &\lambda^{r,\iota+1}_{\text{obs},\bullet} = \lambda^{r,\iota}_{\text{obs},\bullet} + \rho^r_{\text{obs}} \Big(\mathbf{v}^r c^{r,\iota+1}_\bullet - \mathcal{O}_\bullet - l^r_\bullet d^{r,\iota+1} \cos(\omega^{r,\iota+1})\Big), \notag\\ &\lambda^{s,\iota+1}_{\text{obs},\bullet} = \lambda^{s,\iota}_{\text{obs},\bullet} + \rho^s_{\text{obs}} \Big(\mathbf{v}^s c^{s,\iota+1}_\bullet - \mathcal{R}_\bullet - l^s_\bullet d^{s,\iota+1} \cos(\omega^{s,\iota+1})\Big), \notag\\ &\lambda^{\iota+1}_{c,\theta} = \lambda^\iota_{c,\theta} + \rho_{\theta} (A^T_{c,\theta} c^{\iota+1}_\theta - Y^{\iota+1}_\theta), \notag\\ &\lambda^{\iota+1}_{c,\bullet} = \lambda^\iota_{c,\bullet} + \rho_{c,\bullet} (A^T_{c,\bullet} c^{\iota+1}_\bullet - Y^{\iota+1}_\bullet), \quad\quad \quad \quad \quad \bullet \in \{x,y\}. \notag \end{align} \label{eq:dual95updates}\tag{58}\]

The iteration terminates when either the primal residual falls below \(\epsilon^{\text{pri}}=0.5\) or the maximum iteration count \(\iota_{\text{max}}=200\) is reached, ensuring balanced computational efficiency and solution accuracy.

5 Results↩︎

This section presents a comprehensive evaluation of the proposed approach through both high-fidelity simulations and real-world experiments. The simulation study constructs an urban intersection scenario and utilizes safety-critical highway scenarios from the Next Generation Simulation (NGSIM) dataset⁴, while physical validation is conducted using a 1:10-scale Ackermann-steering platform operating with HVs exhibiting diverse driving behaviors.

5.1 Simulation↩︎

5.1.1 Simulation Setup↩︎

The simulations are implemented in C++ running on an Ubuntu 24.04 LTS system with an Intel Ultra 9 285H CPU with 16 cores and 32 threads. The visualization uses RViz in ROS Noetic with a 100 Hz communication rate, while the planning loop operates with a time step \(\delta T = 0.08\,\text{s}\) and a planning horizon \(N = 50\) for both nominal and safe contingency trajectories.

The diagonal elements of the weighting matrices in 37 are set to \(50\) for \({Q}_x,{Q}_y,{Q}_\theta,{Q}_{xv},{Q}_{yv}\), and \(100\) for \({Q}_{xd},{Q}_{yd}\). All \(L_2\) penalty parameters in 55 are set to \(5\). The barrier coefficient parameter \(\alpha\) is set to \(0.8\). The HV geometric ellipsoid axes \(l_x^h\) and \(l_y^h\) are set to \(6\,\text{m}\) and \(4\,\text{m}\) in the intersection scenario, and set according to their actual shapes in the highway dataset. The Bézier curve order \(n\) is set to \(10\). Acceleration bounds of \(\pm5\, \mathrm{m/s^2}\) are enforced in both longitudinal and lateral directions. The terminal angular velocity of the orientation is set to zero. The considered HV number is set as \(M=4\). The consensus step \(N_s\) is set to \(5\). The control-intent set is initialized with \(\pm0.2\, \mathrm{m/s^2}\) for longitudinal acceleration bounds and \(\pm0.1\, \mathrm{m/s^2}\) for lateral acceleration bounds.

We evaluate five approaches:

• Proposed Safe Contingency Planner: Our complete framework integrating online-updated FRS-based barrier constraints with contingency planning;

• Deterministic Barrier Planner: A variant of our planner replacing the FRS-based barrier constraint 31 with nominal barrier constraint 30 in both nominal and contingency branches;

• Worst-Case Barrier Planner: A conservative version using worst-case FRS predictions (derived from a prior control-intent set with \(\pm3\, \mathrm{m/s^2}\) acceleration bounds) instead of online-updated FRS in 31 ;

• ST-RHC [7]: A baseline NMPC method using constant-velocity prediction for collision-avoidance constraints, solved efficiently through ACADO toolkit [62] with multiple shooting and sequential quadratic programming;

• Uncertain-Aware Planner [44]: A baseline uncertain-aware planner embeds FRSs directly in NMPC solved with ACADO in C++, maintaining identical FRS update parameters for fair comparison.

The comprehensive evaluation incorporates four key performance metrics:

• Safety Performance: Collision rate \(P_\text{c}\) and the minimum distance to the nearest HV \(d_{\text{min}}\);

• Comfort: Peak longitudinal/lateral jerk \(J_{\text{x,max}}\), \(J_{\text{y,max}}\);

• Driving Efficiency: Mean speed \(v_{\text{mean}}\) and travel distance \(s_{\text{mean}}\);

• Computation: Mean computation time \(t_{\text{mean}}\).

3pt

1) Highway Cut-in Scenario on NGSIM.

The validation leverages challenging dense traffic driving scenarios from the NGSIM dataset to rigorously assess the performance. In the tested scenario, the EV navigates a multi-lane highway environment. As shown in Fig. 3, the HV-A in the leftmost lane initiates a rightward staged lane change, briefly pauses in the second leftmost lane, and then abruptly resumes its rightward maneuver just as the EV approaches. Meanwhile, the HV-B and HV-C maintain steady driving. This scenario challenges the EV’s ability to dynamically identify and adapt to uncertain HV intentions while proactively responding to potential cut-ins.

The simulation configuration establishes an initial EV speed of \(20\, \text{m}/\text{s}\), with time headways to HV-A systematically varied from \(4.5\,\text{s}\) to \(5.5\,\text{s}\) in \(0.1\,\text{s}\) increments to ensure statistical reliability, with each parameter tested three times. Perception noise is modeled as a zero-mean Gaussian distribution [63]. The standard deviations are modeled as decreasing functions of the EV-HV distance. For the noise in longitudinal position, the standard deviation is \(\sigma_{p_x}^{h} = \overline{\sigma}_{p_x}/\max\left(\frac{10}{s^h + 0.1}, 1\right)\), where \(s^h\) denotes the EV-HV distance and \(\overline{\sigma}_{p_x}=0.2\, \text{m}\) is a noise parameter. The same principle applies to the lateral position, longitudinal and lateral velocity components, with the corresponding noise parameters set as \(\overline{\sigma}_{p_y}=0.2\, \text{m}\), \(\overline{\sigma}_{v_x}=0.1\, \text{m/s}\) and \(\overline{\sigma}_{v_y}=0.1\, \text{m/s}\), repsectively.

The performance evaluation presented in Table 1 demonstrates that the proposed method achieves collision-free with a safe minimum EV-HV distance of \(1.48 \,\text{m}\), while maintaining comfort requirements and competitive efficiency metrics. It represents significant improvements over both the Deterministic Barrier Planner with \(27.27\%\) collision rate and ST-RHC with \(18.18\%\) collision rate. This safety assurance stems from our rigorous FRS-based barrier constraints.

Trajectory analysis in Fig. 4 and Fig. 5 reveals distinct behavioral patterns among the compared methods. The proposed approach exhibits defensive characteristics through gradual deceleration initiated at approximately \(40\,\text{s}\) to anticipate potential cut-in maneuvers by HV-A. This proactive strategy enables smooth speed adjustment combined with moderate lateral avoidance when HV-A executes its sudden lane change, ensuring safe passage. In contrast, the Worst-Case Barrier method exhibits overly conservative behaviors, implementing premature, exaggerated deceleration and lateral displacement, substantially compromising driving efficiency. Meanwhile, the ST-RHC, relying exclusively on deterministic predictions, resorts to last-minute aggressive avoidance maneuvers when confronted with actual cut-in situations. Notably, our method demonstrates significant comfort improvements, reducing peak longitudinal and lateral jerk by 31.0% and 18.3% respectively compared to ST-RHC.

Moreover, comparative analysis with the Uncertain-Aware Planner [44] shows superior performance of our method in both driving efficiency and comfort. While this baseline approach directly employs FRS for obstacle avoidance, its lack of contingency planning capability results in abrupt lateral maneuvers and degraded driving efficiency. The discontinuous nature of FRS updates eads to sudden trajectory adjustments, manifesting as high lateral jerk. Our method addresses these limitations through integrating FRS-based barrier constraint in contingency planning, achieving a 64.4% reduction in lateral jerk while maintaining smoother acceleration profiles within constraints.

From a computational perspective, the proposed approach achieves real-time performance with \(0.034 \,\text{s}\) per iteration. Biconvex structure exploitation with ADMM enables reliable real-time optimization for autonomous driving.

2) Urban Intersection Scenario. To further rigorously evaluate the capability of handling emergent traffic conflicts under uncertainty, we conduct a high-risk unsignalized intersection scenario test. The EV navigates northbound at \(10 \,\text{m/s}\) through dense east-west traffic (\(6–10 \,\text{m/s}\)) along \(y\) axis. The critical challenge arises when an oncoming southbound HV along the \(x\) axis initially exhibits normal driving behavior before abruptly executing a right turn across the intended path of the EV. This scenario specifically tests two key capabilities: (1) proactive defensive maneuvers to account for trajectory prediction inaccuracies, and (2) simultaneous management of multi-vehicle motion uncertainties while ensuring safety.

The trajectory analysis in Figs. 6–7 shows safe navigation of our method through anticipatory deceleration and controlled rightward avoidance maneuvers, while maintaining minimum \(0.55 \,\text{m}\) clearance from adjacent vehicles, as shown in Fig. 8. This defensive strategy achieves the longest travel distance (\(122.68 \,\text{m}\)) among all methods, optimally balancing safety and efficiency.

Comparative analysis reveals distinct limitations of baseline methods. The ST-RHC approach, relying on deterministic constant-velocity predictions, fails to anticipate the right-turning HV’s maneuver, resulting in a collision due to rapidly shrinking feasible space between adjacent vehicles, ultimately leading to planner infeasibility. Parallel deficiencies emerge in the Deterministic Barrier Planner, which fails to adequately handle uncertain HV behaviors without FRS-based barrier constraints.

While the Worst-Case Barrier Planner ensures safety through excessive early deceleration and complete yielding, this approach severely compromises efficiency by requiring the EV to wait for complete intersection clearance. This result highlights the necessity of our online update mechanism for FRS predictions, adapting to observed behaviors rather than assuming worst-case behaviors. Similarly, the Uncertainty-Aware Planner with direct FRS constraint incorporation results in overly cautious behaviors with substantial speed reduction and the shortest travel distance. This limitation originates from directly embedding FRS constraints in the ST-RHC framework while lacking contingency optimization.

These findings collectively underscore the capability of our method to preserve safe contingency trajectories with feasibility in uncertain traffic conflicts.

3) Discussion. The effectiveness of the proposed method is affected by two critical parameters: the consensus steps parameter \(N_s\) and the weight \(p_s\). These parameters collectively determine how the vehicle balances immediate responsiveness with long-term performance in uncertain traffic scenarios.

\(N_s\) controls the consensus horizon shared by both trajectories. Fig. 9 shows smaller values (\(N_s \leq 10\)) enable more aggressive maneuvers by permitting earlier commitment to specific trajectory, with fixed \(p_s=0.5\). This can improve efficiency in most cases, but potentially reduce safety margins when HVs exhibit unexpected behaviors. Conversely, larger values (\(N_s \geq 20\)) enforce conservative behaviors by maintaining both deterministic and FRS-based constraints longer, often causing preemptive deceleration until intentions clarify. As demonstrated in Fig. 9, when \(N_s\) exceeds a certain threshold, the planner conservatively decelerates until the HV’s intention becomes clear.

\(p_s\) regulates contingency branch weighting in optimization. Here we fix \(N_s=5\). As shown in Fig. 10, larger \(p_s\) values prioritize FRS-based safety barrier constraints in consensus segments, producing earlier and more pronounced deceleration. In contrast, smaller \(p_s\) values approximate deterministic planning that assumes perfect HV trajectory prediction, yielding reduced yet adequate safe margins.

For practical deployment, we recommend tailoring parameter configurations to specific operational requirements and desired conservatism levels. Although the method does not incorporate explicit risk quantification, it ensures safety when HV behaviors stay within the updated uncertainty bounds. Our analysis confirms that \(N_s\) and \(p_s\) effectively regulate the critical tradeoff between conservation and flexibility in uncertain traffic scenarios, allowing methodical adjustment of planning behavior across the safety-efficiency spectrum.

5.2 Experiment Results↩︎

To validate the practical efficacy of the proposed method, we conduct real-world experiments using 1:10-scale Ackermann-steering robotic platforms. The experimental setup consists of two robotic vehicles operating within an Optitrack motion capture system, with one serving as the EV and the other emulating the HV. The planning system, implemented on a laptop with computational specifications matching our simulation platform, receives state feedback through ROS Noetic via wireless communication and generates appropriate control commands. Trajectory tracking is achieved through a linear feedback controller that demonstrates sufficient tracking accuracy for our experimental purposes.

We design safety-critical overtaking scenarios to rigorously evaluate the adaptive responsiveness to evolving environmental uncertainties. These scenarios specifically assess its ability to generate safe yet non-conservative trajectories while maintaining robustness against unpredictable lane intrusions. In the experimental configuration, the EV navigates along the reference path at \(p_y = 0\,\text{m}\) with a desired speed of \(3\,\text{m}/\text{s}\) while attempting to overtake an HV moving from \(p_y = -0.3\) m with varying degrees of aggressiveness. The experimental parameters are aligned with those used in the simulation.

Fig. 11 shows the snapshots in two experimental scenarios that highlight the algorithm performance under different HV uncertainties. In Scenario I, the HV exhibits aggressive, erratic behavior analogous to impaired driving, abruptly cutting into the EV’s path at approximately \(p_x = -2.8\, \text{m}\). Scenario II presents the contrasting case where the HV maintains relatively steady lane intrusion behavior. These scenarios encapsulate the fundamental safety challenges addressed by our method, particularly the EV’s need to continuously estimate potential hazardous maneuvers without prior knowledge, dynamically update the HV’s reachable sets for real-time safety assurance, and maintain viable contingency plans against possible lane intrusions.

The planned and executed trajectories are depicted in Fig. 12. In Scenario I, the HV’s erratic motions, caused by intentional hardware modifications to induce unpredictable behavior, creates significant uncertainty that prevents accurate trajectory prediction. Through online learning of the HV’s control-intent set and subsequent FRS occupancy prediction, the EV successfully maintains both progress and safety. The resulting overtaking trajectory shows properly adaptive safety margin while effectively avoiding the HV’s sudden lane intrusion towards \(p_y = 0\, \text{m}\). Scenario II confirms the inherent adaptability of the proposed approach, where the relatively steady HV behavior results in smaller predicted reachable sets, enabling the planner to naturally generate more efficient overtaking trajectories with appropriately reduced conservatism.

Fig. 13 provides insight into the online adaptation mechanism governing the response to varying HV behaviors. The event \(\|P_{u,m}^h u + q_{u,m}^h\|_2 \geq 1.0\) triggers the online learning of the control-intent sets given a newly collected control input vector \(u\), leading to progressive expansion of set volume, computed by \(\text{Vol}(\hat{\mathcal{U}}_m^h)=\pi\sqrt{\det(\Sigma_{u,m}^h)}\) [53]. Scenario I demonstrates rapid growth of set volume corresponding to the HV’s aggressive maneuvers, while Scenario II maintains smaller sets with relatively steady behaviors. This adaptive learning mechanism inherent in the FRS-based safety barrier enables the planner to automatically adjust safety margins according to the perceived driving uncertainty, providing a principled approach to balancing safety and efficiency.

The physical experimental results demonstrate reliable and timely online update of varying FRS prediction through the event-triggered mechanism, which facilitates flexible adjustment of safety margins to match the perceived uncertain environment. Most importantly, the experimental outcomes verify that the planner generates non-conservative yet safe trajectories while maintaining defensive behaviors against sudden dangerous maneuvers, validating the practical applicability of our approach in safety-critical scenarios.

6 Conclusions↩︎

In this study, we present a real-time trajectory optimization framework that generates safe and non-conservative trajectories through FRS-based barrier constraints with event-triggered online adaptive refinement. The method maintains feasible, safe trajectories under evolving uncertainties and avoids excessive conservatism or dangerous underestimation through incremental FRS updates. By decomposing the contingency optimization via consensus ADMM, the framework achieves computational efficiency for real-time implementation. Comprehensive validation demonstrates that the approach successfully achieves collision-free navigation with reduced jerks and maintains an average speed 14-20% higher than other baselines, preserving safety with balanced driving efficiency in dense traffic scenarios. Future research will extend this framework by incorporating game-theoretic interaction models, which would lead to more natural cooperative behaviors in dense traffic.

References↩︎

---

1. Rui Yang, Lei Zheng, and Jun Ma are with The Hong Kong University of Science and Technology, China (e-mail: ryang253@connect.hkust-gz.edu.cn; lzheng135@connect.hkust-gz.edu.cn; jun.ma@ust.hk).↩︎

2. Shuzhi Sam Ge is with the Department of Electrical and Computer Engineering, National University of Singapore, Singapore 117576 (e-mail: samge@nus.edu.sg).↩︎

3. This work has been submitted to the IEEE for possible publication. Copyright may be transferred without notice, after which this version may no longer be accessible.↩︎

4. https://data.transportation.gov/Automobiles/Next-Generation-Simulation-NGSIM-Vehicle-Trajector/8ect-6jqj↩︎