---
title: Dual-loop Iterative Anchoring Path Smoothing
url: https://www.emergentmind.com/topics/dual-loop-iterative-anchoring-path-smoothing-dl-iaps
type: topic
---

# Dual-loop Iterative Anchoring Path Smoothing

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 [2009.11135].

## 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 $\mathcal B_k$ are replaced by box constraints, slack variables $s_k\geq 0$ are added with penalty $\mu$, and a trust-region $t$ 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}
$\hat g_k$ 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 $\{\mathcal B_k\}$. 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 $k$ are shrunk ($\mathcal B_k \leftarrow \beta\,\mathcal B_k$ with $\beta < 1$), and the process restarts. Termination is achieved when no collisions are present.

- **Inner Smoothing/Curvature Loop:** Consists of a penalty loop (increasing $\mu \leftarrow \alpha\mu$ with $\alpha>1$ until all linearized curvature constraints $\hat g_k \leq c_{tol}$ are satisfied) and a trust-region loop (adaptively increasing/decreasing $t$ based on the ratio TrueImprove/ModelImprove compared to threshold $\rho\in(0,1)$, using update factors $\gamma^+>1>\gamma^-$). 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 $t < x_{tol}$ or $\|P-P^{pre}\|_{\infty} < x_{tol}$ or $|f(P) - f(P^{pre})| < f_{tol}$.
- **Inner Penalty Loop**: Concludes when $\max_k \hat g_k(P) \leq c_{tol}$.
- **Outer Anchoring Loop**: Stops once no point $P_k$ 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 $\mu_0$, and trust-region size $t_0$.
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 $\mu$ until all linearized curvature constraints are met.
4. **Inner Trust-Region Loop**:
    - Linearize constraints, solve the convex QP, and adapt the trust region $t$ 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 $P$ is returned.

## 5. Computational Complexity and Runtime

In each inner iteration, DL-IAPS solves a convex QP of dimension $2(n-2)$ 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 $1/R_{min}$, 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 $1 \times 10^{-3}$ s.
- **On-Road Testing**: U.S. “Apollo” pull-over and Beijing T4 parallel parking trials yielded lateral errors $<0.09$ m and heading errors $<5^\circ$, with mean values around $0.06$ m and $1.3^\circ$ for T4, and $0.045$ m, $1.96^\circ$ 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 [2009.11135].

## 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 [2009.11135].

Source: https://www.emergentmind.com/topics/dual-loop-iterative-anchoring-path-smoothing-dl-iaps