Dual-loop Iterative Anchoring Path Smoothing

• The paper presents a dual-loop algorithm that decouples collision detection and curvature constraints to enable efficient, collision-precise path smoothing for autonomous vehicles.
• It employs Sequential Convex Programming with adaptive trust-region and penalty loops to handle nonconvex constraints while ensuring strict convergence criteria.
• Empirical evaluations in simulations and on-road tests demonstrate that DL-IAPS achieves smooth vehicle maneuvers with low lateral and heading errors and superior computational efficiency.

Dual-loop Iterative Anchoring Path Smoothing (DL-IAPS) is a trajectory optimization algorithm designed for autonomous vehicle free-space planning. It achieves real-time, curvature-feasible, and collision-precise path smoothing by decoupling the collision-free trajectory planning into a two-loop structure. DL-IAPS operates as the path smoothing component within a broader decoupled trajectory optimization framework that also incorporates Piece-wise Jerk Speed Optimization (PJSO). Its effectiveness and robustness are substantiated by extensive simulation and on-road experiments, demonstrating significant improvements in driving comfort, control feasibility, and computational efficiency compared to prior approaches (Zhou et al., 2020).

1. Mathematical Formulation

DL-IAPS starts from a reference path $P^{ref} = \{P_k^{ref}\in\mathbb{R}^2\}_{k=0}^{n-1}$, typically generated by the Hybrid A* algorithm. The variables $P_k = [x_k, y_k]$ for $k=0...n-1$ are iteratively optimized by the smoother. The velocity and acceleration increments are defined as:

$$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$

A curvature constraint enforces

$$\|A_k\| \leq \frac{\|P_k-P_{k-1}\|^2}{R_{min}} \qquad (k=1..n-2)$$

where $R_{min}$ is the minimal admissible turning radius.

The nonconvex formulation is: \begin{subequations} \begin{align} \min_{P_0..P_{n-1}} \;\; & f(P) = \sum_{k=1}^{n-2}|2P_k - P_{k-1} - P_{k+1}|^2 \ \text{s.t.}\;\; & P_0 = P^{ref}_0\,,\;\; P_{n-1} = P^{ref}_{n-1} \ & P_1 = P_0 + |P_1 - P_0| \hat u_{\phi_0}\,,\;\; P_{n-2}=P_{n-1}-|P_{n-1}-P_{n-2}|\,\hat u_{\phi_{n-1}} \ & P_k \in \mathcal B_k \quad (k=2..n-3) \ & g_k(P) = |2P_k -P_{k-1} - P_{k+1}|^2 - \frac{|P_k-P_{k-1}|^4}{R_{min}^2} \leq 0 \;\; (k=1..n-2) \end{align} \end{subequations} where $\mathcal B_k$ represent axis-aligned (possibly rectangular) “bubbles” centered on $P_k^{ref}$ to limit feasible deviations.

To address nonconvexity, DL-IAPS applies Sequential Convex Programming (SCP). Each $g_k$ is linearized at the current iterate $P^{pre}$, bubbles $P_k = [x_k, y_k]$0 are replaced by box constraints, slack variables $P_k = [x_k, y_k]$1 are added with penalty $P_k = [x_k, y_k]$2, and a trust-region $P_k = [x_k, y_k]$3 is introduced: \begin{subequations} \begin{align} \min_{P,\,s} \;\; & \sum_{k=1}^{n-2}|2P_k-P_{k-1}-P_{k+1}|^2 + \mu\sum_{k=1}^{n-2}s_k \ \text{s.t.}\;\; & P_0,P_{n-1}\text{ fixed}; \;\; Lx_k\leq x_k\leq Ux_k,\; Ly_k\leq y_k\leq Uy_k\ (k=2..n-3) \ & P_k^{pre}-t\leq P_k\leq P_k^{pre}+t\ (k=2..n-3) \ & \hat g_k(P^{pre},P) - s_k \leq 0 \ (k=1..n-2),\;\; s_k \geq 0 \end{align} \end{subequations} $P_k = [x_k, y_k]$4 denotes the first-order Taylor approximation.

2. Dual-Loop Algorithmic Structure

DL-IAPS employs a hierarchical dual-loop architecture:

• Outer Anchoring/Collision Loop: Inputs are the current bubble set $P_k = [x_k, y_k]$5. The algorithm calls the inner loop for local path smoothing, then performs a precise polygonal collision check between smoothed path segments and environmental obstacles. If collisions are detected, the corresponding bubbles for each colliding index $P_k = [x_k, y_k]$6 are shrunk ($P_k = [x_k, y_k]$7 with $P_k = [x_k, y_k]$8), and the process restarts. Termination is achieved when no collisions are present.
• Inner Smoothing/Curvature Loop: Consists of a penalty loop (increasing $P_k = [x_k, y_k]$9 with $k=0...n-1$0 until all linearized curvature constraints $k=0...n-1$1 are satisfied) and a trust-region loop (adaptively increasing/decreasing $k=0...n-1$2 based on the ratio TrueImprove/ModelImprove compared to threshold $k=0...n-1$3, using update factors $k=0...n-1$4). SCP convex subproblems are solved at each step until the trust-region or penalty convergence criteria are met.

3. Convergence Criteria and Termination

DL-IAPS defines explicit convergence thresholds for both inner and outer loops:

• Inner Trust-Region Loop: Terminates if $k=0...n-1$5 or $k=0...n-1$6 or $k=0...n-1$7.
• Inner Penalty Loop: Concludes when $k=0...n-1$8.
• Outer Anchoring Loop: Stops once no point $k=0...n-1$9 intersects any obstacle polygon, as verified by an exact collision check.

4. Annotated Pseudocode

The core workflow can be summarized as follows:

1. Initialize: Set initial bubbles, penalty $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$0, and trust-region size $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$1.
2. Outer Loop:
   • While collision remains, invoke the inner SCP-based smoothing.
   • After smoothing, conduct collision check.
   • For colliding indices, shrink bubbles and repeat.
3. Inner Penalty Loop:
   • Increase penalty parameter $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$2 until all linearized curvature constraints are met.
4. Inner Trust-Region Loop:
   • Linearize constraints, solve the convex QP, and adapt the trust region $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$3 based on the achieved vs. predicted improvement.
   • Progress until trust-region or objective decrease criteria are satisfied.
5. Final Output: When outer loop terminates without collisions, the path $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$4 is returned.

5. Computational Complexity and Runtime

In each inner iteration, DL-IAPS solves a convex QP of dimension $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$5 augmented with slack variables. Empirical measurements using OSQP on a 2.6 GHz i7 CPU over 80 parallel-parking test cases yielded:

• Path smoothing: mean 35 ms (range: 2–82 ms)
• PJSO speed optimization: mean 35 ms (21–70 ms)
• Total path & speed: average 70 ms (min 23 ms, max 152 ms)

By comparison, a one-shot nonlinear MPC approach (H-OBCA) required an average of 1.247 s (range: 0.313–4.019 s). In 208 complex obstacle/boundary scenarios, total path smoothing and speed optimization consistently remained under 0.21 s.

6. Empirical Performance and Evaluation

Performance of DL-IAPS, validated through large-scale simulation and on-road testing, is characterized by the following empirical findings:

• Collision and Curvature Feasibility: Across 80 parallel-parking and pull-over simulations, DL-IAPS+PJSO maintained smooth curvature profiles always below $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$6, while reference methods (CES) occasionally exceeded curvature under extreme steering.
• Maneuver Quality: Balanced speed, acceleration, and jerk profiles delivered comfortable minimum-time vehicle maneuvers.
• Robustness: In 208 free-space cases with up to 5 obstacles, the algorithm achieved aggregate runtimes of 0.18–0.21 s and a per-point computation of approximately $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$7 s.
• On-Road Testing: U.S. “Apollo” pull-over and Beijing T4 parallel parking trials yielded lateral errors $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$8 m and heading errors $$V_k = P_k - P_{k-1} \,, \qquad A_k = V_{k+1} - V_k\,.$$9, with mean values around $$\|A_k\| \leq \frac{\|P_k-P_{k-1}\|^2}{R_{min}} \qquad (k=1..n-2)$$0 m and $$\|A_k\| \leq \frac{\|P_k-P_{k-1}\|^2}{R_{min}} \qquad (k=1..n-2)$$1 for T4, and $$\|A_k\| \leq \frac{\|P_k-P_{k-1}\|^2}{R_{min}} \qquad (k=1..n-2)$$2 m, $$\|A_k\| \leq \frac{\|P_k-P_{k-1}\|^2}{R_{min}} \qquad (k=1..n-2)$$3 for U.S. sites. Over 400 hours of field experiments, DL-IAPS demonstrated consistent robustness, collision-avoidance precision, strict observance of curvature limits, and high driving comfort (Zhou et al., 2020).

7. Significance and Context in Trajectory Planning

DL-IAPS addresses critical challenges in free-space path smoothing by ensuring both real-time performance and stringent geometric and collision constraints. Its explicit dual-loop structure separates curvature-constrained smoothing from collision handling, improving computational tractability over tightly coupled optimal control approaches such as H-OBCA. The resulting method is practically viable for deployment in production autonomous driving stacks, as evidenced by real-world integration under realistic traffic scenarios. The path/speed decoupled design, in conjunction with PJSO, yields a composable optimizer with empirically validated high performance in complex environments (Zhou et al., 2020).