Spatio-Temporal Joint Trajectory Planning for Autonomous Vehicles Based on Improved Constrained Iterative LQR

. 2025 Jan 17;25(2):512. doi: 10.3390/s25020512

Algorithm 1. Improved CILQR Algorithm

1:
Output: Trajectory vector and control sequence $x, u$
2:
//enable hybrid A* method; using the PI controller, get the speed profile
3:
InitRefPath: $x_{0}, u_{0}$ , tolerance
4:
// calculate the matrix in advance based on the initial trajectory to prevent oscillations
5:
Warmstart: $δ Q (x_{0}, u_{0})$ ← $x_{0}, u_{0}$
6:
While | $Δ J$ | < tolerance || stepcounter > threshold || feedforward gain ≈ 0
7:
do
8:
$b_{k_3}^{x}, b_{k_3}^{u}$ ← $g_{k} (x_{k}, u_{k})$ // convert inequality constraints into cost functions
9:
$J^{-}$ ← $J$
10:
$K, d, Δ V$ ← BACKWARDPASS( $x, u$ )
11:
AdaptiveWeight: $w_{a x}, w_{\dot{κ}}$ // adaptive weight tuning function
12:
$X, U, J$ ← FORWARDPASS( $x, u, K, d, Δ V, J^{-}$ )
13:
$α = ρ α$
14:
return $x, u, J$