iLQR学习
LQR基础 状态转移：
x k + 1 = A k x t + B k u t x_{k+1} = A_k x_t + B_ku_t x k + 1 = A k x t + B k u t 需要使如下目标函数最优
J ( x 0 , U ) = 1 2 Σ k = 1 N − 1 ( x k T Q k x k + u k T R k u k ) + 1 2 x N T Q f x N J(x_0,U) = \frac{1}{2} \Sigma_{k=1}^ {N-1} (x_k^TQ_kx_k + u_k^TR_ku_k) + \frac{1}{2}x_N^TQ_fx_N J ( x 0 , U ) = 2 1 Σ k = 1 N − 1 ( x k T Q k x k + u k T R k u k ) + 2 1 x N T Q f x N 其中三部分为状态成本，控制成本和终点状态成本。
在终端时刻 N N N
V N ( x N ) = 1 2 x N T Q f x N V_N(x_N) = \frac{1}{2} x_N^T Q_f x_N V N ( x N ) = 2 1 x N T Q f x N 为便于递推，我们定义：
S N = Q f \boldsymbol{S_N = Q_f} S N = Q f 所以：
V N ( x N ) = 1 2 x N T S N x N V_N(x_N) = \frac{1}{2} x_N^T S_N x_N V N ( x N ) = 2 1 x N T S N x N k = N − 1 k=N-1 k = N − 1 u N − 1 ∗ u^*_{N-1} u N − 1 ∗ V N ( x N ) = 1 2 x N T Q f x N = 1 2 x N T S N x N V_N(x_N) = \frac{1}{2} x_N^TQ_fx_N=\frac{1}{2}x_N^TS_Nx_N V N ( x N ) = 2 1 x N T Q f x N = 2 1 x N T S N x N 根据贝尔曼最优性原理
V i ( x i ) = min u i V i + 1 ( x i + 1 ) + 1 2 u i T R i u i + 1 2 x i T Q i x i = min u i 1 2 u i T R i u i + 1 2 x i T Q i x i + 1 2 ( A i x i + B i u i ) T S i + 1 ( A i x i + B i u i ) = min u i 1 2 u i T R i u i + 1 2 x i T Q i x i + 1 2 x i T A T S i + 1 A x i + 1 2 u i T B i T S i + 1 B i u i + x i T A i T S i + 1 B i u i = min u i 1 2 u i T ( R i + B i T S i + 1 B i ) u i + 1 2 x i T ( Q i + A i T S i + 1 A i ) x i + u i T ( B i T S i + 1 A i ) x i \begin{aligned}V_{i}(x_{i}) &=
\min_{u_{i}}
V_{i+1}(x_{i+1})+ \frac{1}{2}u_{i}^TR_{i}u_{i} + \frac{1}{2}x_{i}^TQ_{i}x_{i}
\\&=\min_{u_{i}} \frac{1}{2}u_{i}^TR_{i}u_{i} + \frac{1}{2}x_{i}^TQ_{i}x_{i} + \frac{1}{2}(A_{i}x_{i} + B_{i}u_{i})^TS_{i+1}(A_{i}x_{i} + B_{i}u_{i})\\
&=\min_{u_{i}} \frac{1}{2}u_{i}^TR_{i}u_{i} + \frac{1}{2}x_{i}^TQ_{i}x_{i} +\frac{1}{2} x_i^TA^TS_{i+1} Ax_i +
\frac{1}{2} u_i^TB_i^TS_{i+1}B_iu_i + x_i^TA_i^TS_{i+1}B_iu_i \\
&=\min_{u_{i}} \frac{1}{2} u_i^T(R_i + B_i^TS_{i+1}B_i)u_i + \frac{1}{2} x_i^T(Q_{i} + A_i^TS_{i+1}A_i)x_i + u_i^T(B_i^TS_{i+1}A_i)x_i \\
\end{aligned} V i ( x i ) = u i min V i + 1 ( x i + 1 ) + 2 1 u i T R i u i + 2 1 x i T Q i x i = u i min 2 1 u i T R i u i + 2 1 x i T Q i x i + 2 1 ( A i x i + B i u i ) T S i + 1 ( A i x i + B i u i ) = u i min 2 1 u i T R i u i + 2 1 x i T Q i x i + 2 1 x i T A T S i + 1 A x i + 2 1 u i T B i T S i + 1 B i u i + x i T A i T S i + 1 B i u i = u i min 2 1 u i T ( R i + B i T S i + 1 B i ) u i + 2 1 x i T ( Q i + A i T S i + 1 A i ) x i + u i T ( B i T S i + 1 A i ) x i 接下来求导
∂ V ∂ u = ( R i + B i T S i + 1 B i ) u i + ( B i T S i + 1 A i ) x i = 0
\begin{aligned}
\frac{\partial V}{\partial u} &=
(R_i + B_i^TS_{i+1}B_i)u_i + (B_i^TS_{i+1}A_i)x_i
&= 0
\end{aligned}
∂ u ∂ V = ( R i + B i T S i + 1 B i ) u i + ( B i T S i + 1 A i ) x i = 0 u i = − ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) x i u_i = - (R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i)x_i u i = − ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) x i u i = − K i x i , K i = ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) u_i = -K_ix_i, \ \ \ \ \ K_i = (R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i) u i = − K i x i , K i = ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) Riccati 代入最优控制
V i ( x i ) = 1 2 u i ∗ T ( R i + B i T S i + 1 B i ) u i ∗ + 1 2 x i T ( Q i + A i T S i + 1 A i ) x i + u i ∗ T ( B i T S i + 1 A i ) x i = 1 2 x i T K i T ( R i + B i T S i + 1 B i ) K i x i + 1 2 x i T ( Q i + A i T S i + 1 A i ) x i − x i T K i T ( B i T S i + 1 A i ) x i = 1 2 x i T ( K i T ( R i + B i T S i + 1 B i ) K i + Q i + A i T S i + 1 A i − 2 K i T ( B i T S i + 1 A i ) ) x i \begin{aligned}V_i(x_i) = &\frac{1}{2} u_i^{*T}(R_i + B_i^TS_{i+1}B_i)u_i^* + \frac{1}{2} x_i^T(Q_{i} + A_i^TS_{i+1}A_i)x_i + u_i^{*T}(B_i^TS_{i+1}A_i)x_i \\
= & \frac{1}{2} x_i^TK_i^T(R_i + B_i^TS_{i+1}B_i)K_ix_i +
\frac{1}{2} x_i^T(Q_{i} + A_i^TS_{i+1}A_i)x_i - x_i^TK_i^T(B_i^TS_{i+1}A_i)x_i \\
= & \frac{1}{2} x_i^T (K_i^T(R_i + B_i^TS_{i+1}B_i)K_i + Q_{i} + A_i^TS_{i+1}A_i - 2K_i^T(B_i^TS_{i+1}A_i)) x_i
\end{aligned}
V i ( x i ) = = = 2 1 u i ∗ T ( R i + B i T S i + 1 B i ) u i ∗ + 2 1 x i T ( Q i + A i T S i + 1 A i ) x i + u i ∗ T ( B i T S i + 1 A i ) x i 2 1 x i T K i T ( R i + B i T S i + 1 B i ) K i x i + 2 1 x i T ( Q i + A i T S i + 1 A i ) x i − x i T K i T ( B i T S i + 1 A i ) x i 2 1 x i T ( K i T ( R i + B i T S i + 1 B i ) K i + Q i + A i T S i + 1 A i − 2 K i T ( B i T S i + 1 A i )) x i 完整形式Riccati方程 S i = Q i + A i T S i + 1 A i − 2 K T ( B i T S i + 1 A i ) + K i T R i K i + K i T ( B i T S i + 1 B i ) K i = Q i + K i T R i K i + ( A i − B i K i ) T S i + 1 ( A i − B i K i ) \begin{aligned}S_i = & Q_i + A_i^TS_{i+1}A_i - 2K^T(B_i^TS_{i+1}A_i) + K_i^TR_iK_i + K_i^T(B_i^TS_{i+1}B_i)K_i \\
= & Q_i + K_i^TR_iK_i + (A_i - B_iK_i)^TS_{i+1}(A_i - B_iK_i)
\end{aligned} S i = = Q i + A i T S i + 1 A i − 2 K T ( B i T S i + 1 A i ) + K i T R i K i + K i T ( B i T S i + 1 B i ) K i Q i + K i T R i K i + ( A i − B i K i ) T S i + 1 ( A i − B i K i ) 标准的 离散时间 Riccati 矩阵方程 (DARE)： 将Ki代入
S i = Q i + K i T R i K i + ( A i − B i K i ) T S i + 1 ( A i − B i K i ) = Q i + ( ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) ) T R i ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) + ( A i − B i ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) ) T S i + 1 ( A i − B i ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) ) = Q i + A i T S i + 1 A i − A i T S i + 1 B i ( R i + B i T S i + 1 B i ) − 1 B i T S i + 1 A i
\begin{aligned}
S_i =& Q_i + K_i^TR_iK_i + (A_i - B_iK_i)^TS_{i+1}(A_i - B_iK_i) \\
= & Q_i + ((R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i)) ^T R_i (R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i) + (A_i - B_i(R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i))^TS_{i+1}(A_i - B_i(R_i + B_i^TS_{i+1}B_i)^{-1}(B_i^TS_{i+1}A_i)) \\
= & Q_i + A_i^T S_{i+1} A_i - A_i^T S_{i+1} B_i (R_i + B_i^T S_{i+1} B_i)^{-1} B_i^T S_{i+1} A_i
\end{aligned}
S i = = = Q i + K i T R i K i + ( A i − B i K i ) T S i + 1 ( A i − B i K i ) Q i + (( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) ) T R i ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) + ( A i − B i ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i ) ) T S i + 1 ( A i − B i ( R i + B i T S i + 1 B i ) − 1 ( B i T S i + 1 A i )) Q i + A i T S i + 1 A i − A i T S i + 1 B i ( R i + B i T S i + 1 B i ) − 1 B i T S i + 1 A i iLQR 状态转移方程变成非线性，即
x k + 1 = f ( x k , u k ) x_{k+1} = f(x_k, u _k) x k + 1 = f ( x k , u k ) 目标函数为
J ( x 0 , U ) = h ( x N ) + Σ k = 0 N − 1 l ( x k , u k ) J(x_0,U) = h(x_N) + \Sigma_{k = 0} ^ {N-1} l (x_k,u_k) J ( x 0 , U ) = h ( x N ) + Σ k = 0 N − 1 l ( x k , u k ) 如果我们用iLQR去优化轨迹 假设我们有一个粗节轨迹( x ˉ , u ˉ ) \bold{(\bar x, \bar u)} ( x ˉ , u ˉ ) δ x , δ u \delta x, \delta u δ x , δ u
扰动为
δ x k = x k − x ˉ k δ u k = u k − u ˉ k
\begin{aligned}
\delta x_k = x_k - \bar x_k\\
\delta u_k = u_k - \bar u_k
\end{aligned}
δ x k = x k − x ˉ k δ u k = u k − u ˉ k 我们需要将非线性泰勒展开，得到线性的局部目标函数及状态转移方程。
对状态转移方程进行展开
x k + 1 ≈ f ( x ˉ k , u ˉ k ) + ∂ f ∂ x ∣ k ˉ ( x k − x ˉ k ) + ∂ f ∂ u ∣ k ˉ ( u k − u ˉ k ) A k = ∂ f ∂ x ∣ k ˉ B k = ∂ f ∂ u ∣ k ˉ c k = f ( x ˉ k , u ˉ k ) − x ˉ k + 1
\begin{aligned}
& x_{k+1} \approx f(\bar x_k, \bar u_k) + \frac{\partial f}{\partial x} |_{\bar k} (x_k -\bar x_k) + \frac{\partial f}{\partial u} | _{\bar k} (u_k - \bar u_k)
\\
& A_k = \frac{\partial f}{\partial x} |_{\bar k}
\\
& B_k = \frac{\partial f}{\partial u} |_{\bar k}
\\
& c_k = f(\bar x_k, \bar u_k) - \bar x_{k+1}
\end{aligned}
x k + 1 ≈ f ( x ˉ k , u ˉ k ) + ∂ x ∂ f ∣ k ˉ ( x k − x ˉ k ) + ∂ u ∂ f ∣ k ˉ ( u k − u ˉ k ) A k = ∂ x ∂ f ∣ k ˉ B k = ∂ u ∂ f ∣ k ˉ c k = f ( x ˉ k , u ˉ k ) − x ˉ k + 1 δ x k + 1 = A k δ x k + B k δ u k + c k \delta x_{k+1} = A_k\delta x_k + B_k \delta u_k + c_k δ x k + 1 = A k δ x k + B k δ u k + c k 目标函数的二次化（局部成本） δ J = δ h ( x N ) + Σ k = 0 N − 1 δ l ( x k , u k ) \delta J = \delta h(x_N) + \Sigma_{k=0}^{N-1} \delta l (x_k, u_k) δ J = δ h ( x N ) + Σ k = 0 N − 1 δ l ( x k , u k ) 运行成本 对l进行泰勒展开（在粗解的point上）
l ( x k , u k ) ≈ l ( x ˉ k , u ˉ k ) + l x T δ x k + l u T δ u k + 1 2 δ x k T l x x δ x k + 1 2 δ u k T l u u δ u k + δ x k T l x u δ u k , l ( x ˉ k , u ˉ k ) 为常数
l(x_k, u_k) \approx l(\bar x_k, \bar u_k) + l_x^T\delta x_k + l_u^T\delta u_k + \frac{1}{2} \delta x_k^T l_{xx} \delta x_k + \frac{1}{2} \delta u_k^T l_{uu} \delta u_k + \delta x_k^Tl_{xu}\delta u_k, \ \ \ \ \ \ \ l(\bar x_k, \bar u_k) 为常数
l ( x k , u k ) ≈ l ( x ˉ k , u ˉ k ) + l x T δ x k + l u T δ u k + 2 1 δ x k T l xx δ x k + 2 1 δ u k T l uu δ u k + δ x k T l xu δ u k , l ( x ˉ k , u ˉ k ) 为常数 终端成本 h ( x N ) ≈ h ( x ˉ N ) + h x T δ x N + 1 2 δ x N T h x x δ x N , h ( x ˉ N ) 为常数 h(x_N) \approx h(\bar x_N) + h^T_x\delta x_N + \frac{1}{2} \delta x_N^T h_{xx}\delta x_N, \ \ \ \ \ \ \ \ h(\bar x_N) 为常数 h ( x N ) ≈ h ( x ˉ N ) + h x T δ x N + 2 1 δ x N T h xx δ x N , h ( x ˉ N ) 为常数 此时可以发现，与LQR相比，iLQR除了二次型外，还有线性项
iLQR 与 LQR 的线性项差异 🔹 LQR (Linear Quadratic Regulator)：纯二次型 核心目标： 将状态驱动到原点 x = 0 \boldsymbol{x}=\mathbf{0} x = 0
A. 零参考和零梯度 (Zero Reference & Zero Gradient) LQR 的局部目标函数是纯二次型 ，且假设最优轨迹是 x = 0 , u = 0 \boldsymbol{x}=\mathbf{0}, \boldsymbol{u}=\mathbf{0} x = 0 , u = 0 x = 0 \boldsymbol{x}=\mathbf{0} x = 0 x \boldsymbol{x} x 梯度 （一阶导数）总是 零 ：∂ l ∂ x ∣ x = 0 , u = 0 = 0 \frac{\partial l}{\partial \boldsymbol{x}}\bigg|_{\boldsymbol{x}=\mathbf{0}, \boldsymbol{u}=\mathbf{0}} = \mathbf{0} ∂ x ∂ l x = 0 , u = 0 = 0
B. 贝尔曼方程的性质：线性项消失 由于成本函数在原点没有梯度，所以值函数 V k V_k V k x = 0 \boldsymbol{x}=\mathbf{0} x = 0 V k V_k V k V k ( x k ) ≈ V k ( 0 ) + v k T ⏟ 梯度 x k + 1 2 x k T V x x x k V_k(\boldsymbol{x}_k) \approx V_k(\mathbf{0}) + \underbrace{\boldsymbol{v}_k^T}_{\text{梯度}} \boldsymbol{x}_k + \frac{1}{2} \boldsymbol{x}_k^T \boldsymbol{V}_{\boldsymbol{xx}} \boldsymbol{x}_k V k ( x k ) ≈ V k ( 0 ) + 梯度 v k T x k + 2 1 x k T V xx x k V k ( 0 ) = 0 V_k(\mathbf{0})=0 V k ( 0 ) = 0 v k = ∂ V k ∂ x ∣ x = 0 = 0 \boldsymbol{v}_k = \frac{\partial V_k}{\partial \boldsymbol{x}}|_{\boldsymbol{x}=\mathbf{0}} = \mathbf{0} v k = ∂ x ∂ V k ∣ x = 0 = 0
LQR 结论： 值函数是纯二次型 ：
V k ( x k ) = 1 2 x k T S k x k V_k(\boldsymbol{x}_k) = \frac{1}{2} \boldsymbol{x}_k^T \boldsymbol{S}_k \boldsymbol{x}_k V k ( x k ) = 2 1 x k T S k x k
🔹 iLQR (Iterative LQR)：仿射二次型 (Affine Quadratic) 核心目标： 在一条任意名义轨迹 ( x ˉ , u ˉ ) (\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}}) ( x ˉ , u ˉ )
A. 轨迹偏离原点，存在非零梯度 (Non-Zero Gradient) 名义轨迹 ( x ˉ , u ˉ ) (\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}}) ( x ˉ , u ˉ ) 通常不经过原点 ，且在优化过程中通常不是最优的 。x ˉ k \bar{\boldsymbol{x}}_k x ˉ k δ l \boldsymbol{\delta l} δl x ˉ k \bar{\boldsymbol{x}}_k x ˉ k l ( x , u ) l(\boldsymbol{x}, \boldsymbol{u}) l ( x , u ) x ˉ k \bar{\boldsymbol{x}}_k x ˉ k x \boldsymbol{x} x u \boldsymbol{u} u 不为零 ：l x = ∂ l ∂ x ∣ k ˉ ≠ 0 \boldsymbol{l}_{\boldsymbol{x}} = \frac{\partial l}{\partial \boldsymbol{x}}\bigg|_{\bar{k}} \neq \mathbf{0} l x = ∂ x ∂ l k ˉ = 0 l u = ∂ l ∂ u ∣ k ˉ ≠ 0 \boldsymbol{l}_{\boldsymbol{u}} = \frac{\partial l}{\partial \boldsymbol{u}}\bigg|_{\bar{k}} \neq \mathbf{0} l u = ∂ u ∂ l k ˉ = 0
B. 线性项的引入：驱动优化过程 这些非零的梯度 l x \boldsymbol{l}_{\boldsymbol{x}} l x l u \boldsymbol{l}_{\boldsymbol{u}} l u Q k \mathcal{Q}_k Q k 线性分量 q \boldsymbol{q} q r \boldsymbol{r} r Q k ≈ ⋯ + q T δ x k ⏟ 非零 + r T δ u k ⏟ 非零 + 1 2 δ x k T Q δ x k + … \mathcal{Q}_k \approx \dots + \underbrace{\boldsymbol{q}^T \boldsymbol{\delta x}_k}_{\text{非零}} + \underbrace{\boldsymbol{r}^T \boldsymbol{\delta u}_k}_{\text{非零}} + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{Q} \boldsymbol{\delta x}_k + \dots Q k ≈ ⋯ + 非零 q T δx k + 非零 r T δu k + 2 1 δx k T Q δx k + … q \boldsymbol{q} q r \boldsymbol{r} r V k V_k V k v k \boldsymbol{v}_k v k 也不为零 ：v k = q − M R − 1 r ≠ 0 \boldsymbol{v}_k = \boldsymbol{q} - \boldsymbol{M} \boldsymbol{R}^{-1} \boldsymbol{r} \neq \mathbf{0} v k = q − M R − 1 r = 0
iLQR 结论： 值函数是仿射二次型 （包含线性项）：
V k ( δ x k ) ≈ const + v k T δ x k ⏟ 非零线性项 + 1 2 δ x k T V x x δ x k V_k(\boldsymbol{\delta x}_k) \approx \text{const} + \underbrace{\boldsymbol{v}_k^T \boldsymbol{\delta x}_k}_{\text{非零线性项}} + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{V}_{\boldsymbol{xx}} \boldsymbol{\delta x}_k V k ( δx k ) ≈ const + 非零线性项 v k T δx k + 2 1 δx k T V xx δx k
🎯 总结：线性项的作用 iLQR 中的非零线性项 v k T δ x k \boldsymbol{v}_k^T \boldsymbol{\delta x}_k v k T δx k 非零的斜率 。
这个斜率就是优化过程的驱动力 ：它告诉我们沿着哪个方向（即最优反馈控制 δ u k ∗ \boldsymbol{\delta u}_k^* δu k ∗ δ x \boldsymbol{\delta x} δx 最大的成本下降 ，从而不断地将名义轨迹推向局部最优。
iLQR的最优目标函数 终端：
V N ( x N ) = h ( x N ) V_N(x_N) = h (x_N) V N ( x N ) = h ( x N ) 将其泰勒展开，去掉常数项，可以得到
V N ( δ x N ) ≈ h x T δ x N + 1 2 δ x N T h x x δ x N V_N(\delta x_N) \approx h^T_x\delta x_N + \frac{1}{2} \delta x^T _N h_{xx} \delta x_N V N ( δ x N ) ≈ h x T δ x N + 2 1 δ x N T h xx δ x N 一般：
V k ( δ x k ) = C k + v k T δ x k + 1 2 δ x k T V x x , k δ x k
V_k(\delta x_k) = C_k + v_k^T\delta x_k + \frac{1}{2}\delta x^T_kV_{xx,k} \delta x_k
V k ( δ x k ) = C k + v k T δ x k + 2 1 δ x k T V xx , k δ x k 其中，在终端的时候
v N = h x , V x x , N = h x x v_N = h_x,\ \ \ V_{xx,N} = h_{xx} v N = h x , V xx , N = h xx 递推过程k + 1 ----> k： Q k ( δ x k , δ u k ) = l ( x k , u k ) + V k + 1 ( x k + 1 ) Q_k(\delta x_k, \delta u_k) = l (x_k,u_k) + V_{k+1}(x_{k+1}) Q k ( δ x k , δ u k ) = l ( x k , u k ) + V k + 1 ( x k + 1 ) 我们的目标是将 Q k \mathcal{Q}_k Q k ( x ˉ k , u ˉ k ) (\bar{\boldsymbol{x}}_k, \bar{\boldsymbol{u}}_k) ( x ˉ k , u ˉ k ) ( δ x k , δ u k ) (\boldsymbol{\delta x}_k, \boldsymbol{\delta u}_k) ( δx k , δu k ) 二次近似 ：
Q k ≈ 常数 + q T δ x k + r T δ u k + 1 2 δ x k T Q δ x k + 1 2 δ u k T R δ u k + δ x k T M δ u k \mathcal{Q}_k \approx \text{常数} + \boldsymbol{q}^T \boldsymbol{\delta x}_k + \boldsymbol{r}^T \boldsymbol{\delta u}_k + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{Q} \boldsymbol{\delta x}_k + \frac{1}{2} \boldsymbol{\delta u}_k^T \boldsymbol{R} \boldsymbol{\delta u}_k + \boldsymbol{\delta x}_k^T \boldsymbol{M} \boldsymbol{\delta u}_k Q k ≈ 常数 + q T δx k + r T δu k + 2 1 δx k T Q δx k + 2 1 δu k T R δu k + δx k T M δu k 分别展开 l l l V k + 1 V_{k+1} V k + 1 。
2. 已知信息 (输入) 在 k k k 已知 以下信息：
瞬时成本 l l l
l x , l u \boldsymbol{l}_{\boldsymbol{x}}, \boldsymbol{l}_{\boldsymbol{u}} l x , l u l x x , l u u , l x u \boldsymbol{l}_{\boldsymbol{xx}}, \boldsymbol{l}_{\boldsymbol{uu}}, \boldsymbol{l}_{\boldsymbol{xu}} l xx , l uu , l xu
未来成本 V k + 1 V_{k+1} V k + 1
v k + 1 \boldsymbol{v}_{k+1} v k + 1 V k + 1 V_{k+1} V k + 1 δ x k + 1 \boldsymbol{\delta x}_{k+1} δx k + 1 V x x , k + 1 \boldsymbol{V}_{\boldsymbol{xx}, k+1} V xx , k + 1 V k + 1 V_{k+1} V k + 1 δ x k + 1 \boldsymbol{\delta x}_{k+1} δx k + 1
线性化动力学：
δ x k + 1 = A k δ x k + B k δ u k + c k \boldsymbol{\delta x}_{k+1} = \boldsymbol{A}_k \boldsymbol{\delta x}_k + \boldsymbol{B}_k \boldsymbol{\delta u}_k + \boldsymbol{c}_k δx k + 1 = A k δx k + B k δu k + c k
瞬时成本 l l l 我们首先对 l ( x k , u k ) l(\boldsymbol{x}_k, \boldsymbol{u}_k) l ( x k , u k ) ( x ˉ k , u ˉ k ) (\bar{\boldsymbol{x}}_k, \bar{\boldsymbol{u}}_k) ( x ˉ k , u ˉ k )
l ( x k , u k ) ≈ l ( x ˉ k , u ˉ k ) + l x T δ x k + l u T δ u k + 1 2 δ x k T l x x δ x k + 1 2 δ u k T l u u δ u k + δ x k T l x u δ u k l(\boldsymbol{x}_k, \boldsymbol{u}_k) \approx l(\bar{\boldsymbol{x}}_k, \bar{\boldsymbol{u}}_k) + \boldsymbol{l}_{\boldsymbol{x}}^T \boldsymbol{\delta x}_k + \boldsymbol{l}_{\boldsymbol{u}}^T \boldsymbol{\delta u}_k + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{l}_{\boldsymbol{xx}} \boldsymbol{\delta x}_k + \frac{1}{2} \boldsymbol{\delta u}_k^T \boldsymbol{l}_{\boldsymbol{uu}} \boldsymbol{\delta u}_k + \boldsymbol{\delta x}_k^T \boldsymbol{l}_{\boldsymbol{xu}} \boldsymbol{\delta u}_k l ( x k , u k ) ≈ l ( x ˉ k , u ˉ k ) + l x T δx k + l u T δu k + 2 1 δx k T l xx δx k + 2 1 δu k T l uu δu k + δx k T l xu δu k 未来成本 V k + 1 V_{k+1} V k + 1 我们必须将 V k + 1 V_{k+1} V k + 1 δ x k + 1 \boldsymbol{\delta x}_{k+1} δx k + 1 ( δ x k , δ u k ) (\boldsymbol{\delta x}_k, \boldsymbol{\delta u}_k) ( δx k , δu k )
我们从 V k + 1 V_{k+1} V k + 1
V k + 1 ( δ x k + 1 ) ≈ 常数 + v k + 1 T δ x k + 1 + 1 2 δ x k + 1 T V x x , k + 1 δ x k + 1 V_{k+1}(\boldsymbol{\delta x}_{k+1}) \approx \text{常数} + \boldsymbol{v}_{k+1}^T \boldsymbol{\delta x}_{k+1} + \frac{1}{2} \boldsymbol{\delta x}_{k+1}^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{\delta x}_{k+1} V k + 1 ( δx k + 1 ) ≈ 常数 + v k + 1 T δx k + 1 + 2 1 δx k + 1 T V xx , k + 1 δx k + 1 现在，我们将线性化动力学 δ x k + 1 = A k δ x k + B k δ u k + c k \boldsymbol{\delta x}_{k+1} = \boldsymbol{A}_k \boldsymbol{\delta x}_k + \boldsymbol{B}_k \boldsymbol{\delta u}_k + \boldsymbol{c}_k δx k + 1 = A k δx k + B k δu k + c k
V k + 1 V_{k+1} V k + 1 将 δ x k + 1 \boldsymbol{\delta x}_{k+1} δx k + 1 v k + 1 T δ x k + 1 \boldsymbol{v}_{k+1}^T \boldsymbol{\delta x}_{k+1} v k + 1 T δx k + 1
v k + 1 T δ x k + 1 = v k + 1 T ( A k δ x k + B k δ u k + c k ) \boldsymbol{v}_{k+1}^T \boldsymbol{\delta x}_{k+1} = \boldsymbol{v}_{k+1}^T (\boldsymbol{A}_k \boldsymbol{\delta x}_k + \boldsymbol{B}_k \boldsymbol{\delta u}_k + \boldsymbol{c}_k) v k + 1 T δx k + 1 = v k + 1 T ( A k δx k + B k δu k + c k ) v k + 1 T δ x k + 1 = ( A k T v k + 1 ) T δ x k ⏟ 对 δ x k 线性 + ( B k T v k + 1 ) T δ u k ⏟ 对 δ u k 线性 + v k + 1 T c k ⏟ 常数 \boldsymbol{v}_{k+1}^T \boldsymbol{\delta x}_{k+1} = \underbrace{(\boldsymbol{A}_k^T \boldsymbol{v}_{k+1})^T \boldsymbol{\delta x}_k}_{\text{对 } \boldsymbol{\delta x}_k \text{ 线性}} + \underbrace{(\boldsymbol{B}_k^T \boldsymbol{v}_{k+1})^T \boldsymbol{\delta u}_k}_{\text{对 } \boldsymbol{\delta u}_k \text{ 线性}} + \underbrace{\boldsymbol{v}_{k+1}^T \boldsymbol{c}_k}_{\text{常数}} v k + 1 T δx k + 1 = 对 δx k 线性 ( A k T v k + 1 ) T δx k + 对 δu k 线性 ( B k T v k + 1 ) T δu k + 常数 v k + 1 T c k V k + 1 V_{k+1} V k + 1 将 δ x k + 1 \boldsymbol{\delta x}_{k+1} δx k + 1 1 2 δ x k + 1 T V x x , k + 1 δ x k + 1 \frac{1}{2} \boldsymbol{\delta x}_{k+1}^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{\delta x}_{k+1} 2 1 δx k + 1 T V xx , k + 1 δx k + 1
1 2 ( A k δ x k + B k δ u k + c k ) T V x x , k + 1 ( A k δ x k + B k δ u k + c k ) \frac{1}{2} (\boldsymbol{A}_k \boldsymbol{\delta x}_k + \boldsymbol{B}_k \boldsymbol{\delta u}_k + \boldsymbol{c}_k)^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} (\boldsymbol{A}_k \boldsymbol{\delta x}_k + \boldsymbol{B}_k \boldsymbol{\delta u}_k + \boldsymbol{c}_k) 2 1 ( A k δx k + B k δu k + c k ) T V xx , k + 1 ( A k δx k + B k δu k + c k ) 展开这个二次型（我们只保留到二阶，忽略 c k \boldsymbol{c}_k c k
δ x k \boldsymbol{\delta x}_k δx k 1 2 ( A k δ x k ) T V x x , k + 1 ( A k δ x k ) = 1 2 δ x k T ( A k T V x x , k + 1 A k ) δ x k \frac{1}{2} (\boldsymbol{A}_k \boldsymbol{\delta x}_k)^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} (\boldsymbol{A}_k \boldsymbol{\delta x}_k) = \frac{1}{2} \boldsymbol{\delta x}_k^T (\boldsymbol{A}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{A}_k) \boldsymbol{\delta x}_k 2 1 ( A k δx k ) T V xx , k + 1 ( A k δx k ) = 2 1 δx k T ( A k T V xx , k + 1 A k ) δx k δ u k \boldsymbol{\delta u}_k δu k 1 2 ( B k δ u k ) T V x x , k + 1 ( B k δ u k ) = 1 2 δ u k T ( B k T V x x , k + 1 B k ) δ u k \frac{1}{2} (\boldsymbol{B}_k \boldsymbol{\delta u}_k)^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} (\boldsymbol{B}_k \boldsymbol{\delta u}_k) = \frac{1}{2} \boldsymbol{\delta u}_k^T (\boldsymbol{B}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{B}_k) \boldsymbol{\delta u}_k 2 1 ( B k δu k ) T V xx , k + 1 ( B k δu k ) = 2 1 δu k T ( B k T V xx , k + 1 B k ) δu k 交叉项 (δ x k , δ u k \boldsymbol{\delta x}_k, \boldsymbol{\delta u}_k δx k , δu k δ x k T ( A k T V x x , k + 1 B k ) δ u k \boldsymbol{\delta x}_k^T (\boldsymbol{A}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{B}_k) \boldsymbol{\delta u}_k δx k T ( A k T V xx , k + 1 B k ) δu k 线性项 (来自 c k \boldsymbol{c}_k c k δ x k T ( A k T V x x , k + 1 c k ) + δ u k T ( B k T V x x , k + 1 c k ) \boldsymbol{\delta x}_k^T (\boldsymbol{A}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{c}_k) + \boldsymbol{\delta u}_k^T (\boldsymbol{B}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{c}_k) δx k T ( A k T V xx , k + 1 c k ) + δu k T ( B k T V xx , k + 1 c k )
(注：iLQR 简化忽略了 f f f f x x , f u u \boldsymbol{f}_{\boldsymbol{xx}}, \boldsymbol{f}_{\boldsymbol{uu}} f xx , f uu V k + 1 V_{k+1} V k + 1
合成 Q 函数系数 现在我们将第 1 部分 (l l l 第 2 部分 (V k + 1 V_{k+1} V k + 1 Q k \mathcal{Q}_k Q k
梯度 q \boldsymbol{q} q δ x k \boldsymbol{\delta x}_k δx k q = ∂ Q k ∂ δ x k = l x ⏟ 来自 l + A k T v k + 1 ⏟ 来自 V k + 1 线性项 + A k T V x x , k + 1 c k ⏟ 来自 V k + 1 二次项与 c k 交叉 \boldsymbol{q} = \frac{\partial \mathcal{Q}_k}{\partial \boldsymbol{\delta x}_k} = \underbrace{\boldsymbol{l}_{\boldsymbol{x}}}_{\text{来自 } l} + \underbrace{\boldsymbol{A}_k^T \boldsymbol{v}_{k+1}}_{\text{来自 } V_{k+1} \text{ 线性项}} + \underbrace{\boldsymbol{A}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{c}_k}_{\text{来自 } V_{k+1} \text{ 二次项与 } \boldsymbol{c}_k \text{ 交叉}} q = ∂ δx k ∂ Q k = 来自 l l x + 来自 V k + 1 线性项 A k T v k + 1 + 来自 V k + 1 二次项与 c k 交叉 A k T V xx , k + 1 c k 梯度 r \boldsymbol{r} r δ u k \boldsymbol{\delta u}_k δu k r = ∂ Q k ∂ δ u k = l u ⏟ 来自 l + B k T v k + 1 ⏟ 来自 V k + 1 线性项 + B k T V x x , k + 1 c k ⏟ 来自 V k + 1 二次项与 c k 交叉 \boldsymbol{r} = \frac{\partial \mathcal{Q}_k}{\partial \boldsymbol{\delta u}_k} = \underbrace{\boldsymbol{l}_{\boldsymbol{u}}}_{\text{来自 } l} + \underbrace{\boldsymbol{B}_k^T \boldsymbol{v}_{k+1}}_{\text{来自 } V_{k+1} \text{ 线性项}} + \underbrace{\boldsymbol{B}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{c}_k}_{\text{来自 } V_{k+1} \text{ 二次项与 } \boldsymbol{c}_k \text{ 交叉}} r = ∂ δu k ∂ Q k = 来自 l l u + 来自 V k + 1 线性项 B k T v k + 1 + 来自 V k + 1 二次项与 c k 交叉 B k T V xx , k + 1 c k Hessian Q \boldsymbol{Q} Q δ x k , δ x k \boldsymbol{\delta x}_k, \boldsymbol{\delta x}_k δx k , δx k Q = ∂ 2 Q k ∂ δ x k 2 = l x x ⏟ 来自 l + A k T V x x , k + 1 A k ⏟ 来自 V k + 1 二次项 \boldsymbol{Q} = \frac{\partial^2 \mathcal{Q}_k}{\partial \boldsymbol{\delta x}_k^2} = \underbrace{\boldsymbol{l}_{\boldsymbol{xx}}}_{\text{来自 } l} + \underbrace{\boldsymbol{A}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{A}_k}_{\text{来自 } V_{k+1} \text{ 二次项}} Q = ∂ δx k 2 ∂ 2 Q k = 来自 l l xx + 来自 V k + 1 二次项 A k T V xx , k + 1 A k Hessian R \boldsymbol{R} R δ u k , δ u k \boldsymbol{\delta u}_k, \boldsymbol{\delta u}_k δu k , δu k R = ∂ 2 Q k ∂ δ u k 2 = l u u ⏟ 来自 l + B k T V x x , k + 1 B k ⏟ 来自 V k + 1 二次项 \boldsymbol{R} = \frac{\partial^2 \mathcal{Q}_k}{\partial \boldsymbol{\delta u}_k^2} = \underbrace{\boldsymbol{l}_{\boldsymbol{uu}}}_{\text{来自 } l} + \underbrace{\boldsymbol{B}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{B}_k}_{\text{来自 } V_{k+1} \text{ 二次项}} R = ∂ δu k 2 ∂ 2 Q k = 来自 l l uu + 来自 V k + 1 二次项 B k T V xx , k + 1 B k Hessian M \boldsymbol{M} M δ x k , δ u k \boldsymbol{\delta x}_k, \boldsymbol{\delta u}_k δx k , δu k M = ∂ 2 Q k ∂ δ x k ∂ δ u k = l x u ⏟ 来自 l + A k T V x x , k + 1 B k ⏟ 来自 V k + 1 二次项 \boldsymbol{M} = \frac{\partial^2 \mathcal{Q}_k}{\partial \boldsymbol{\delta x}_k \partial \boldsymbol{\delta u}_k} = \underbrace{\boldsymbol{l}_{\boldsymbol{xu}}}_{\text{来自 } l} + \underbrace{\boldsymbol{A}_k^T \boldsymbol{V}_{\boldsymbol{xx}, k+1} \boldsymbol{B}_k}_{\text{来自 } V_{k+1} \text{ 二次项}} M = ∂ δx k ∂ δu k ∂ 2 Q k = 来自 l l xu + 来自 V k + 1 二次项 A k T V xx , k + 1 B k 求解最优控制扰动 求导，就是求导！
Q k ≈ 常数 + q T δ x k + r T δ u k + 1 2 δ x k T Q δ x k + 1 2 δ u k T R δ u k + δ x k T M δ u k \mathcal{Q}_k \approx \text{常数} + \boldsymbol{q}^T \boldsymbol{\delta x}_k + \boldsymbol{r}^T \boldsymbol{\delta u}_k + \frac{1}{2} \boldsymbol{\delta x}_k^T \boldsymbol{Q} \boldsymbol{\delta x}_k + \frac{1}{2} \boldsymbol{\delta u}_k^T \boldsymbol{R} \boldsymbol{\delta u}_k + \boldsymbol{\delta x}_k^T \boldsymbol{M} \boldsymbol{\delta u}_k Q k ≈ 常数 + q T δx k + r T δu k + 2 1 δx k T Q δx k + 2 1 δu k T R δu k + δx k T M δu k ∂ Q k ∂ δ u k = r + R δ u k + M T δ x k \frac{\partial Q_k}{\partial \delta u_k} = r + R\delta u_k + M^T\delta x_k ∂ δ u k ∂ Q k = r + R δ u k + M T δ x k δ u k ∗ = − R − 1 r − R − 1 M T δ x k = k k + K k δ x k \delta u_k^* = - R^{-1}r - R^{-1}M^T\delta x_k = k_k + K_k\delta x_k δ u k ∗ = − R − 1 r − R − 1 M T δ x k = k k + K k δ x k δ u k = u k − u ˉ k \delta u_k = u_k - \bar u_k δ u k = u k − u ˉ k 前馈：k k = − R − 1 r k_k = - R^{-1}r k k = − R − 1 r r \boldsymbol{r} r
反馈：K k = − R − 1 M T K_k = - R^{-1}M^T K k = − R − 1 M T δ x k \boldsymbol{\delta x}_k δx k
递推过程 k->k-1
计算q k q_{k} q k r k r_{k} r k Q k Q_{k} Q k R k R_{k} R k M k M_{k} M k
计算前馈：k k k_k k k K k K_k K k
更新hessianV x x , k V_{xx,k} V xx , k v k v_k v k
v k = q k + K k T r k + K k T R k k k + M k k k
\boldsymbol{v}_k = \boldsymbol{q}_k + \boldsymbol{K}_k^T \boldsymbol{r}_k + \boldsymbol{K}_k^T \boldsymbol{R}_k \boldsymbol{k}_k + \boldsymbol{M}_k \boldsymbol{k}_k
v k = q k + K k T r k + K k T R k k k + M k k k V x x , k = Q k + K k T R k K k + M k K k + K k T M k T \boldsymbol{V}_{\boldsymbol{xx}, k} = \boldsymbol{Q}_k + \boldsymbol{K}_k^T \boldsymbol{R}_k \boldsymbol{K}_k + \boldsymbol{M}_k \boldsymbol{K}_k + \boldsymbol{K}_k^T \boldsymbol{M}_k^T V xx , k = Q k + K k T R k K k + M k K k + K k T M k T
计算q k − 1 q_{k-1} q k − 1 r k − 1 r_{k-1} r k − 1 Q k − 1 Q_{k-1} Q k − 1 R k − 1 R_{k-1} R k − 1 M k − 1 M_{k-1} M k − 1
计算前馈：k k − 1 k_{k-1} k k − 1 K k − 1 K_{k-1} K k − 1
前向更新 目的： 利用刚刚计算出的 ( k , K ) (\boldsymbol{k}, \boldsymbol{K}) ( k , K ) 新的、成本更低 的轨迹 ( x new , u new ) (\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}}) ( x new , u new )
A. 步长与线性搜索 (Line Search) 我们不能 简单地将 δ u k ∗ = k k + K k δ x k \boldsymbol{\delta u}_k^* = \boldsymbol{k}_k + \boldsymbol{K}_k \boldsymbol{\delta x}_k δu k ∗ = k k + K k δx k
为什么？ 因为我们的 ( k , K ) (\boldsymbol{k}, \boldsymbol{K}) ( k , K ) 局部线性近似 f f f f f f 完整 的 δ u k ∗ \boldsymbol{\delta u}_k^* δu k ∗ α = 1 \alpha=1 α = 1 x new \boldsymbol{x}_{\text{new}} x new 总成本 J J J 。
为了确保收敛，我们引入一个步长 α \alpha α (Alpha)，α ∈ ( 0 , 1 ] \alpha \in (0, 1] α ∈ ( 0 , 1 ]
B. 前向更新的执行步骤
初始化：
设置新轨迹的起点：x new , 0 = x 0 \boldsymbol{x}_{\text{new}, 0} = \boldsymbol{x}_0 x new , 0 = x 0
尝试一个初始步长，例如 α = 1.0 \alpha = 1.0 α = 1.0
模拟（Rollout）： k = 0 k=0 k = 0 N − 1 N-1 N − 1
计算状态偏差： δ x k = x new , k − x ˉ k \boldsymbol{\delta x}_k = \boldsymbol{x}_{\text{new}, k} - \bar{\boldsymbol{x}}_k δx k = x new , k − x ˉ k 新 状态 x new , k \boldsymbol{x}_{\text{new}, k} x new , k 旧 名义状态 x ˉ k \bar{\boldsymbol{x}}_k x ˉ k
计算新的控制 u new , k \boldsymbol{u}_{\text{new}, k} u new , k u new , k = u ˉ k + α ⋅ k k + K k δ x k \boldsymbol{u}_{\text{new}, k} = \bar{\boldsymbol{u}}_k + \alpha \cdot \boldsymbol{k}_k + \boldsymbol{K}_k \boldsymbol{\delta x}_k u new , k = u ˉ k + α ⋅ k k + K k δx k 前馈项 k k \boldsymbol{k}_k k k α \alpha α ，而反馈项 K k \boldsymbol{K}_k K k α \alpha α
积分（使用真实的非线性动力学 f f f x new , k + 1 = f ( x new , k , u new , k ) \boldsymbol{x}_{\text{new}, k+1} = f(\boldsymbol{x}_{\text{new}, k}, \boldsymbol{u}_{\text{new}, k}) x new , k + 1 = f ( x new , k , u new , k )
评估成本： ( x new , u new ) (\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}}) ( x new , u new ) 真实总成本 J new J_{\text{new}} J new 原始非线性成本函数 l l l h h h
接受或拒绝 (Line Search)： 如果 J new < J old J_{\text{new}} < J_{\text{old}} J new < J old 接受！ 新轨迹 ( x new , u new ) (\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}}) ( x new , u new ) 如果 J new ≥ J old J_{\text{new}} \ge J_{\text{old}} J new ≥ J old 拒绝！ 步子迈得太大了。减小 α \alpha α (例如 α = α / 2 \alpha = \alpha / 2 α = α /2 返回步骤 2 ，用更小的 α \alpha α
迭代与收敛 前向更新 成功找到一条成本更低的轨迹 ( x new , u new ) (\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}}) ( x new , u new )
更新名义轨迹： ( x new , u new ) (\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}}) ( x new , u new ) 下一次迭代 的名义轨迹 ( x ˉ , u ˉ ) (\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}}) ( x ˉ , u ˉ )
重复： 逆向递推 (Backward Pass) 阶段，在新的 ( x ˉ , u ˉ ) (\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}}) ( x ˉ , u ˉ ) A k , B k , l x , l x x … \boldsymbol{A}_k, \boldsymbol{B}_k, l_{\boldsymbol{x}}, l_{\boldsymbol{xx}} \dots A k , B k , l x , l xx … ( k , K ) (\boldsymbol{k}, \boldsymbol{K}) ( k , K )
收敛： J J J ϵ \epsilon ϵ ( x ˉ , u ˉ ) (\bar{\boldsymbol{x}}, \bar{\boldsymbol{u}}) ( x ˉ , u ˉ ) 局部最优轨迹 。
一些疑惑与解答 什么时候计算ck呢？ c k \boldsymbol{c}_k c k
阶段
动作
c k \boldsymbol{c}_k c k 解释
（1）初始化 首次粗略猜测 u bar \boldsymbol{u}_{\text{bar}} u bar x bar \boldsymbol{x}_{\text{bar}} x bar
c k ≠ 0 \boldsymbol{c}_k \neq 0 c k = 0 x bar \boldsymbol{x}_{\text{bar}} x bar 如果初始轨迹是用直线插值等方式创建的，那么它不满足 f ( x ˉ k , u ˉ k ) = x ˉ k + 1 f(\bar{\boldsymbol{x}}_k, \bar{\boldsymbol{u}}_k) = \bar{\boldsymbol{x}}_{k+1} f ( x ˉ k , u ˉ k ) = x ˉ k + 1 c k ≠ 0 \boldsymbol{c}_k \neq 0 c k = 0
（2）Backward Pass (首次迭代) 使用 c k ≠ 0 \boldsymbol{c}_k \neq 0 c k = 0 Q k \mathcal{Q}_k Q k q k , r k \boldsymbol{q}_k, \boldsymbol{r}_k q k , r k
使用 c k \boldsymbol{c}_k c k 只有在第一次 迭代中，当名义轨迹不可行时，我们才需要在 q k \boldsymbol{q}_k q k r k \boldsymbol{r}_k r k c k \boldsymbol{c}_k c k
（3）Forward Pass 使用 ( k , K ) (\boldsymbol{k}, \boldsymbol{K}) ( k , K ) 非线性动力学 f f f 得到新的轨迹 ( x new , u new ) (\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}}) ( x new , u new )
c k \boldsymbol{c}_k c k 新轨迹 ( x new , u new ) (\boldsymbol{x}_{\text{new}}, \boldsymbol{u}_{\text{new}}) ( x new , u new ) x ˉ , u ˉ \bar{\boldsymbol{x}}, \bar{\boldsymbol{u}} x ˉ , u ˉ f f f f ( x ˉ k , u ˉ k ) = x ˉ k + 1 f(\bar{\boldsymbol{x}}_k, \bar{\boldsymbol{u}}_k) = \bar{\boldsymbol{x}}_{k+1} f ( x ˉ k , u ˉ k ) = x ˉ k + 1
（4）后续 Backward Pass (第 2 轮及以后) 执行逆向递推。
c k = 0 \boldsymbol{c}_k = 0 c k = 0 后续所有迭代中，名义轨迹都是动态可行的，c k \boldsymbol{c}_k c k
给出一条轨迹没有动力学模型怎么办？ 必须有，没有你就蒙一个，比如"Longitudinal Kinematic Point Model" (纵向运动学点模型)
LQR与iLQR
特性
LQR
LQR (用于跟踪)
iLQR (迭代 LQR)
系统 线性 x k + 1 = A x k + B u k \boldsymbol{x}_{k+1} = \boldsymbol{A}\boldsymbol{x}_k + \boldsymbol{B}\boldsymbol{u}_k x k + 1 = A x k + B u k
线性 (或线性化)
非线性 x k + 1 = f ( x k , u k ) \boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k) x k + 1 = f ( x k , u k )
优化目标 驱动状态到原点 x = 0 \boldsymbol{x}=0 x = 0
最小化 误差 e k \boldsymbol{e}_k e k
最小化总成本 J ( x , u ) J(\boldsymbol{x}, \boldsymbol{u}) J ( x , u )
输出 纯反馈控制 u ∗ = − K x \boldsymbol{u}^* = -\boldsymbol{K}\boldsymbol{x} u ∗ = − K x
前馈+反馈 u ∗ = u r − K e \boldsymbol{u}^* = \boldsymbol{u}_r - \boldsymbol{K}\boldsymbol{e} u ∗ = u r − K e
仿射控制 u ∗ = u ˉ + k + K δ x \boldsymbol{u}^* = \bar{\boldsymbol{u}} + \boldsymbol{k} + \boldsymbol{K}\delta \boldsymbol{x} u ∗ = u ˉ + k + K δ x
用途 镇定系统，调节器
跟踪 已知的参考轨迹优化生成 最优轨迹
iLQR, ALiLQR, CILQR 算法特征对比 这三者都是用于轨迹优化的高效算法，它们的核心都基于 iLQR（迭代线性二次调节器）。它们最大的区别在于如何处理优化问题中的“约束” 。
核心特征对比表
特征
iLQR (基础版)ALiLQR (增广拉格朗日版)CILQR (内点法/障碍函数版)
核心目标 无约束 轨迹优化有约束 轨迹优化有约束 轨迹优化
约束处理能力 几乎没有（或很弱）
强（等式 & 不等式约束）
强（主要用于不等式约束）
核心数学方法 迭代LQR求解
增广拉格朗日法 (Augmented Lagrangian) 内点法 / 障碍函数法 (Interior Point / Barrier Function)
如何处理约束 -
将约束转为“代价惩罚项”和“乘子项”
将约束边界变为“无穷高”的“障碍墙”
算法结构 单循环（前向-反向迭代）
双层循环 (外循环更新约束，内循环跑iLQR)单循环（但在代价函数中加入了障碍项）
对初始轨迹要求 低
低 （可以从不可行 的轨迹开始）高 （必须 从严格可行 的轨迹开始）
迭代过程特点 轨迹逐步逼近最优
轨迹可穿过 约束边界，再被“拉”回
轨迹始终保持 在约束边界内部
1. iLQR (Iterative Linear Quadratic Regulator)
全称 ：迭代线性二次调节器核心思想 ：
在一个名义轨迹（Nominal Trajectory）周围，将非线性 的系统动力学 线性化 。
将非二次 的代价函数 二次化 。
这样问题就变成了一个局部的 LQR 问题，可以通过动态规划（DP）高效求解，得到一个更优的控制策略。
不断重复这个“线性化-二次化-求解”的过程，直到收敛。
约束处理 ：
标准 iLQR 不直接支持 硬约束（如关节限位、避障）。
在实践中，人们有时会用一些技巧（Hacks）来模拟约束，比如：
将约束作为软约束 （Soft Constraint）添加到代价函数中（例如：cost += w * (q - q_limit)^2）。
在控制输入上进行简单的“裁剪”（Clipping）。
这些方法并不严谨，且效果通常不好。
比喻 ：短跑运动员 。他只管在没有障碍的直跑道上用最快的速度冲刺（最小化代价）。
2. ALiLQR (Augmented Lagrangian iLQR)
全称 ：增广拉格朗日 iLQR核心思想 ：
采用增广拉格朗日方法 ，将一个有约束 问题转化为一系列无约束 问题来求解。
它在原代价函数 J 的基础上，增加了“拉格朗日乘子项 λ”和“二次惩罚项 μ”。
新的代价变为：J_aug = J + λ * c(x) + μ * c(x)^2（简化示意）。
算法结构（双循环） ：
内循环 ：固定当前的 λ 和 μ，调用 iLQR 来最小化这个 J_aug（这步是无约束的）。外循环 ：在 iLQR 收敛后，检查当前轨迹对约束 c(x) 的违反程度。
如果违反严重，就增大 惩罚系数 μ（加大罚款力度）。
根据违反情况，更新 拉格朗日乘子 λ（调整罚款基准）。
重复1和2，直到约束被满足且代价最优。
比喻 ：罚款制障碍赛 。运动员可以“撞到”栏架（暂时违反约束），但每次都会被“罚款”（惩罚项）。他会通过调整策略（更新 λ 和 μ），最终找到一条既跑得快、又不再撞栏（满足约束）的路线。
3. CILQR (Constrained iLQR)
全称 ：约束 iLQR (通常特指基于内点法的实现)核心思想 ：
采用**内点法（或称障碍函数法）**来处理不等式约束（例如 g(x) <= 0）。
它在代价函数 J 中加入一个障碍项 (Barrier Term) ，最常见的是对数障碍函数：J_barrier = J - μ * log(-g(x))。
工作机制 ：
当轨迹 x 远离 约束边界时（g(x) 是一个较大的负数），log(-g(x)) 变化很小，对优化影响不大。
当轨迹 x 接近 约束边界时（g(x) 趋近于0），log(-g(x)) 会迅速趋向负无穷 ，导致 -μ * log(-g(x)) 变成一个无穷大的代价 。
这个“无穷大”的代价墙会阻止 iLQR 优化器在求解时越过约束边界。
关键要求 ：
这种方法要求初始轨迹必须严格可行 （即 g(x) < 0）。如果初始轨迹就在边界外，log 函数的输入为正数，算法会直接崩溃。
ALiLQR在项目中的实际应用 车辆动力学模型 1.定义状态变量和控制变量 x = [ x 0 x 1 x 2 x 3 x 4 x 5 ] = [ pos_x ( X坐标 ) pos_y ( Y坐标 ) θ ( 航向角 Yaw ) v ( 纵向速度 ) a ( 纵向加速度 ) ω ( 角速度 YawRate ) ] x = \begin{bmatrix}
x_0 \\ x_1 \\ x_2 \\ x_3 \\ x_4 \\ x_5
\end{bmatrix} = \begin{bmatrix}
\text{pos\_x} & (\text{X坐标}) \\
\text{pos\_y} & (\text{Y坐标}) \\
\theta & (\text{航向角 Yaw}) \\
v & (\text{纵向速度}) \\
a & (\text{纵向加速度}) \\
\omega & (\text{角速度 YawRate})
\end{bmatrix} x = x 0 x 1 x 2 x 3 x 4 x 5 = pos_x pos_y θ v a ω ( X 坐标 ) ( Y 坐标 ) ( 航向角 Yaw ) ( 纵向速度 ) ( 纵向加速度 ) ( 角速度 YawRate ) u = [ u 0 u 1 ] = [ Jerk ( 加加速度/变加速率 ) ω ˙ ( 角加速度 ) ] u = \begin{bmatrix}
u_0 \\ u_1
\end{bmatrix} = \begin{bmatrix}
\text{Jerk} & (\text{加加速度/变加速率}) \\
\dot{\omega} & (\text{角加速度})
\end{bmatrix} u = [ u 0 u 1 ] = [ Jerk ω ˙ ( 加加速度 / 变加速率 ) ( 角加速度 ) ] 2.实现运动方程与积分离散 运动学模型的前向递推-IDM与自行车
使用龙格库塔积分
1 2 void LKPointModel::forward_calculation (const State& state, const Control& ctrl, State& state_output, double step) const
我们需要得到
A t = ∂ f R K 4 ( x t , u t ) ∂ x ( 6 × 6 矩阵 ) A_t = \frac{\partial f_{RK4}(x_t, u_t)}{\partial x} \quad (6 \times 6 \text{ 矩阵}) A t = ∂ x ∂ f R K 4 ( x t , u t ) ( 6 × 6 矩阵 ) B t = ∂ f R K 4 ( x t , u t ) ∂ u ( 6 × 2 矩阵 ) B_t = \frac{\partial f_{RK4}(x_t, u_t)}{\partial u} \quad (6 \times 2 \text{ 矩阵}) B t = ∂ u ∂ f R K 4 ( x t , u t ) ( 6 × 2 矩阵 ) 一个使用python自动推导并得到C++代码的方式
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 import sympy as spdef generate_jacobian_code ():'x y theta v a omega' )'jerk d_omega' )'h' )0.5 * h * k1list (zip (state, state_k2)) 0.5 * h * k2list (zip (state, state_k3))list (zip (state, state_k4))6.0 ) * (k1 + 2 *k2 + 2 *k3 + k4)print ("// --- 自动生成的 C++ 代码开始 ---" )for var, expr in sub_exprs:str (expr).replace("sin" , "std::sin" ).replace("cos" , "std::cos" )print (f"double {var} = {c_expr} ;" )print ("\n// 填充 A 矩阵" )for r in range (rows):for c in range (cols):if val != 0 : print (f"A_output({r} , {c} ) = {val} ;" )print ("\n// 填充 B 矩阵" )for r in range (rows):for c in range (cols):if val != 0 :print (f"B_output({r} , {c} ) = {val} ;" )if __name__ == "__main__" :
反向传播，求解最优控制 我们优化的目标函数 J J J x x x u u u J ( x , u ) = StateCost ( x ) + ControlCost ( u ) J(x, u) = \text{StateCost}(x) + \text{ControlCost}(u) J ( x , u ) = StateCost ( x ) + ControlCost ( u ) z = [ x u ] ( 维度 6 + 2 = 8 ) \mathbf{z} = \begin{bmatrix}
x \\
u
\end{bmatrix} \quad (\text{维度 } 6 + 2 = 8) z = [ x u ] ( 维度 6 + 2 = 8 )
hessians_[i] (矩阵 H)：一个 8 × 8 8 \times 8 8 × 8
H = [ w x 0 0 0 0 0 0 0 0 w y 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 w a 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 w j 0 0 0 0 0 0 0 0 w ω ˙ ] \mathbf{H} = \begin{bmatrix}
\color{blue}{w_{x}} & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & \color{blue}{w_{y}} & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & \color{gray}{0} & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & \color{gray}{0} & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & \color{blue}{w_{a}} & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & \color{gray}{0} & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & \color{red}{w_{j}} & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & \color{red}{w_{\dot{\omega}}}
\end{bmatrix} H = w x 0 0 0 0 0 0 0 0 w y 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 w a 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 w j 0 0 0 0 0 0 0 0 w ω ˙
gradiants_[i] (向量 g)：一个 8 × 1 8 \times 1 8 × 1
g = [ − 2 ⋅ w x ⋅ ref_x [ i ] − 2 ⋅ w y ⋅ ref_y [ i ] 0 0 − 2 ⋅ w a ⋅ ref_a [ i ] 0 0 0 ] \mathbf{g} = \begin{bmatrix}
-2 \cdot w_{x} \cdot \text{ref\_x}[i] \\
-2 \cdot w_{y} \cdot \text{ref\_y}[i] \\
\color{gray}{0} \\
\color{gray}{0} \\
-2 \cdot w_{a} \cdot \text{ref\_a}[i] \\
\color{gray}{0} \\
\color{green}{0} \\
\color{green}{0}
\end{bmatrix} g = − 2 ⋅ w x ⋅ ref_x [ i ] − 2 ⋅ w y ⋅ ref_y [ i ] 0 0 − 2 ⋅ w a ⋅ ref_a [ i ] 0 0 0 如何获取这两个量？： 我们要追踪一个参考点 x r e f x_{ref} x re f
J ( x ) = w ⋅ ( x − x r e f ) 2 = w ⏟ H ⋅ x 2 + ( − 2 w ⋅ x r e f ) ⏟ g ⋅ x + const J(x) = w \cdot (x - x_{ref})^2 = \underbrace{w}_{H} \cdot x^2 + \underbrace{(-2w \cdot x_{ref})}_{g} \cdot x + \text{const} J ( x ) = w ⋅ ( x − x re f ) 2 = H w ⋅ x 2 + g ( − 2 w ⋅ x re f ) ⋅ x + const l x = ∂ J ∂ x = 2 ⋅ w ⋅ ( x − x r e f ) = 2 w x − 2 w x r e f l_x = \frac{\partial J}{\partial x} = 2 \cdot w \cdot (x - x_{ref}) = 2wx - 2wx_{ref} l x = ∂ x ∂ J = 2 ⋅ w ⋅ ( x − x re f ) = 2 w x − 2 w x re f l x x = ∂ 2 J ∂ x 2 = 2 w l_{xx} = \frac{\partial^2 J}{\partial x^2} = 2w l xx = ∂ x 2 ∂ 2 J = 2 w 那么代入可以有
l x = g + 2 H x l_x = g + 2Hx l x = g + 2 H x l x x = 2 H l_{xx} = 2H l xx = 2 H 我们先预计算出g和H，待进行反向传播时候，代入x，得到真正的梯度。
l x , l u l_x, l_u l x , l u l x x , l u u , l x u l_{xx}, l_{uu}, l_{xu} l xx , l uu , l xu 除此之外，我们也可以得到在某个点处的状态转移方程（来自车辆动力学）
A k = ∂ f ∂ x A_k = \frac{\partial f}{\partial x} A k = ∂ x ∂ f B k = ∂ f ∂ u B_k = \frac{\partial f}{\partial u} B k = ∂ u ∂ f 然后就是由未来时刻传递来的未来梯度、hessian
V x ′ V_x' V x ′ V x x ′ V_{xx}' V xx ′ 接下来只需要依据公式组装对应的q,r,Q,R,M即可
1 2 3 4 5 6 7 8 9 10 noalias () += cur_A.transpose () * Vdx;noalias () += cur_B.transpose () * Vdx;noalias () += cur_A.transpose () * Vddx * cur_A;noalias () += cur_B.transpose () * Vddx * cur_B;noalias () += cur_A.transpose () * Vddx * cur_B;
然后求解最优控制量
δ u = − R − 1 r ⏟ 前馈 k + − R − 1 M T ⏟ 反馈 K δ x \delta u = \underbrace{-\mathbf{R}^{-1} \mathbf{r}}_{\text{前馈 } k} + \underbrace{-\mathbf{R}^{-1} \mathbf{M}^T}_{\text{反馈 } K} \delta x δ u = 前馈 k − R − 1 r + 反馈 K − R − 1 M T δ x 我们要计算两个量：前馈向量 k k k → \rightarrow →
反馈矩阵 K K K → \rightarrow →
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 Eigen::LLT<Eigen::MatrixXd> lltOfA (Puu_regular) ; if (lltOfA.info () == Eigen::NumericalIssue) {return false ; inverse ();noalias () = -Puu_inv * Pu;noalias () = -Pxu_regular * Puu_inv;
经过反向传播，从末端到起点遍历参考路径上的每一个点之后，我们得到了每一个点的前馈和反馈系数。
前向递推Forward Pass （初始化假设jerk和dyawrate都为0）输入： i − 1 i-1 i − 1 x n e w , i x_{new, i} x n e w , i
上一轮迭代留下的旧状态 (x o l d , i x_{old, i} x o l d , i u o l d , i u_{old, i} u o l d , i
这一步的策略 K i K_i K i k i k_i k i
计算最优控制：
δ x = x n e w , i − x o l d , i \delta x = x_{new, i} - x_{old, i} δ x = x n e w , i − x o l d , i u n e w , i = u o l d , i + α ⋅ k i + K i ⋅ δ x u_{new, i} = u_{old, i} + \alpha \cdot k_i + K_i \cdot \delta x u n e w , i = u o l d , i + α ⋅ k i + K i ⋅ δ x 更新状态：
x n e w , i + 1 = f ( x n e w , i , u n e w , i ) x_{new, i+1} = f(x_{new, i}, u_{new, i}) x n e w , i + 1 = f ( x n e w , i , u n e w , i ) 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 for (int step = 0 ; step < horizon_; step++) {transpose () * state_error +
轨迹cost计算 遍历整条新轨迹，把 ActionCost, ObstacleCost 等所有代价加起来。
1 new_cost = get_traj_value (new_states_ptr_, new_inputs_ptr_);
验收：线搜索 (Line Search Check)
情况 A： 新轨迹更好了 (J n e w < J o l d J_{new} < J_{old} J n e w < J o l d
情况 B： 新轨迹更烂了 (J n e w ≥ J o l d J_{new} \ge J_{old} J n e w ≥ J o l d α = 1.0 \alpha=1.0 α = 1.0 α = α × 0.5 \alpha = \alpha \times 0.5 α = α × 0.5 α \alpha α
iLQR迭代过程
graph TD
A[开始迭代] --> B[Backward Pass: 算 K, k]
B --> C{设置 alpha = 1.0}
C --> D[Forward Pass: 刚才写的代码, 算出 x_new, u_new]
D --> E[计算新代价 J_new]
E --> F{J_new < J_old ?}
F -- Yes (更好) --> G[接受: 更新旧轨迹, 迭代结束]
F -- No (更烂) --> H[拒绝: alpha 减半]
H --> I{alpha < 阈值?}
I -- No --> D
I -- Yes (实在没救了) --> J[终止: 增加正则化 mu, 重做 Backward]
Levenberg-Marquardt 正则化： Q u u Q_{uu} Q uu Q u u Q_{uu} Q uu μ \mu μ Q u u n e w = Q u u + μ ⋅ I Q_{uu}^{new} = Q_{uu} + \mu \cdot \mathbf{I} Q uu n e w = Q uu + μ ⋅ I μ \mu μ
ALiLQR的外层循环 AL-iLQR 的目标函数（Lagrangian）实际上由 三部分 组成：L ( x ) = J ( x ) ⏟ 原始代价 + λ ⋅ C ( x ) ⏟ 拉格朗日项 + 1 2 μ ⋅ C ( x ) 2 ⏟ 二次惩罚项 L(x) = \underbrace{J(x)}_{\text{原始代价}} + \underbrace{\lambda \cdot C(x)}_{\text{拉格朗日项}} + \underbrace{\frac{1}{2} \mu \cdot C(x)^2}_{\text{二次惩罚项}} L ( x ) = 原始代价 J ( x ) + 拉格朗日项 λ ⋅ C ( x ) + 二次惩罚项 2 1 μ ⋅ C ( x ) 2
前文所说的iLQR过程其实没加这些约束，现在考虑加上
L ( x , u ) = J a c t i o n ( x , u ) ⏟ ActionCostFunction + ∑ ( λ C ( x ) + 1 2 μ C ( x ) 2 ) ⏟ ConstrainCost (Linear Obstacle) L(x, u) = \underbrace{J_{action}(x, u)}_{\text{ActionCostFunction}} + \underbrace{\sum \left( \lambda C(x) + \frac{1}{2}\mu C(x)^2 \right)}_{\text{ConstrainCost (Linear Obstacle)}} L ( x , u ) = ActionCostFunction J a c t i o n ( x , u ) + ConstrainCost (Linear Obstacle) ∑ ( λ C ( x ) + 2 1 μ C ( x ) 2 ) 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 noalias () = state_cost_func_->gradient_lx (index_step);noalias () = state_cost_func_->hessian_lxx (index_step);for (auto & iter : linear_constrain_cost_list) {noalias () += iter->gradient_lx (index_step); noalias () += iter->hessian_lxx (index_step); for (auto & iter : obstacle_constrain_cost_list) {noalias () += iter->gradient_lx (index_step); noalias () += iter->hessian_lxx (index_step);
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 for (int idx = 0 ; idx < solver_config_.max_iteration; idx++) {if (mu_ > solver_config_.regular_mu_max) break ;update_traj_of_costfunc_list (optimal_states_ptr_, optimal_inputs_ptr_);if (!backward_pass ()) {"backward pass failed, increase mu to " << mu_ << std::endl;continue ; bool forward_succ_flag = false ;if (!forward_pass(optimal_cost)) {"forward pass failed, increase mu" << std::endl;else {true ;max (solver_config_.regular_mu_min, mu_ / solver_config_.mu_regular);get_traj_value (optimal_states_ptr_, optimal_inputs_ptr_);if (!forward_succ_flag) continue ;bool convergence = true ; bool linear_convergence = true ;for (auto & iter : linear_constrain_cost_list) {update_constrain_value ()) && linear_convergence;if (!linear_convergence) {bool obstacle_convergence = true ;for (auto & iter : obstacle_constrain_cost_list) {update_constrain_value ()) && obstacle_convergence;if (!obstacle_convergence) break ; for (auto & iter : linear_constrain_cost_list) {update_lamda_penalty (solver_config_.penatly_growth_factor);for (auto & iter : obstacle_constrain_cost_list) {update_lamda_penalty (solver_config_.penatly_growth_factor);if ((std::abs (optimal_cost - last_cost) < solver_config_.convergence_threshold) && return SolverStatus::SUCCESS;else {
在每次AL Iteration中，我们先调用内层的iLQR更新代价，然后接茬线性约束和障碍物约束。λ \lambda λ μ \mu μ x x x u u u 更新拉格朗日乘子 (λ \lambda λ λ n e w = λ o l d + μ ⋅ C ( x ) \lambda_{new} = \lambda_{old} + \mu \cdot C(x) λ n e w = λ o l d + μ ⋅ C ( x ) C ( x ) C(x) C ( x ) λ \lambda λ λ \lambda λ
更新惩罚因子 (μ \mu μ μ n e w = μ o l d × growth_factor \mu_{new} = \mu_{old} \times \text{growth\_factor} μ n e w = μ o l d × growth_factor
ALiLQR的ConstrainCost构建
车辆模型：覆盖圆分解 (Circle Decomposition)x = [ p o s _ x , p o s _ y , θ , … ] x = [pos\_x, pos\_y, \theta, \dots] x = [ p os _ x , p os _ y , θ , … ] j j j
{ P c x = p o s x + b i a s j ⋅ cos ( θ ) P c y = p o s y + b i a s j ⋅ sin ( θ ) \begin{cases}P_{cx} = pos_x + bias_j \cdot \cos(\theta) \\ P_{cy} = pos_y + bias_j \cdot \sin(\theta)\end{cases} { P c x = p o s x + bia s j ⋅ cos ( θ ) P cy = p o s y + bia s j ⋅ sin ( θ )
障碍物模型：简单的圆/点假设障碍物位于 ( x o b s , y o b s ) (x_{obs}, y_{obs}) ( x o b s , y o b s )
约束函数 C ( x ) C(x) C ( x ) R s a f e 2 R_{safe}^2 R s a f e 2 C ( x ) = R s a f e 2 − ( ( P c x − x o b s ) 2 + ( P c y − y o b s ) 2 ) C(x) = R_{safe}^2 - \left( (P_{cx} - x_{obs})^2 + (P_{cy} - y_{obs})^2 \right) C ( x ) = R s a f e 2 − ( ( P c x − x o b s ) 2 + ( P cy − y o b s ) 2 ) C ( x ) > 0 C(x) > 0 C ( x ) > 0 → \rightarrow → C ( x ) ≤ 0 C(x) \le 0 C ( x ) ≤ 0 → \rightarrow →
对 p o s _ x pos\_x p os _ x
∂ C ∂ p o s _ x = − 2 Δ x ⋅ ∂ P c x ∂ p o s _ x = − 2 Δ x ⋅ 1 = − 2 Δ x \frac{\partial C}{\partial pos\_x} = -2\Delta x \cdot \frac{\partial P_{cx}}{\partial pos\_x} = -2\Delta x \cdot 1 = -2\Delta x ∂ p os _ x ∂ C = − 2Δ x ⋅ ∂ p os _ x ∂ P c x = − 2Δ x ⋅ 1 = − 2Δ x
对 p o s _ y pos\_y p os _ y
∂ C ∂ p o s _ y = − 2 Δ y ⋅ 1 = − 2 Δ y \frac{\partial C}{\partial pos\_y} = -2\Delta y \cdot 1 = -2\Delta y ∂ p os _ y ∂ C = − 2Δ y ⋅ 1 = − 2Δ y
对 θ \theta θ ∂ C ∂ θ = − 2 Δ x ⋅ ∂ P c x ∂ θ − 2 Δ y ⋅ ∂ P c y ∂ θ \frac{\partial C}{\partial \theta} = -2\Delta x \cdot \frac{\partial P_{cx}}{\partial \theta} - 2\Delta y \cdot \frac{\partial P_{cy}}{\partial \theta} ∂ θ ∂ C = − 2Δ x ⋅ ∂ θ ∂ P c x − 2Δ y ⋅ ∂ θ ∂ P cy
∂ P c x ∂ θ = − b i a s j ⋅ sin ( θ ) \frac{\partial P_{cx}}{\partial \theta} = -bias_j \cdot \sin(\theta) ∂ θ ∂ P c x = − bia s j ⋅ sin ( θ ) ∂ P c y ∂ θ = b i a s j ⋅ cos ( θ ) \frac{\partial P_{cy}}{\partial \theta} = bias_j \cdot \cos(\theta) ∂ θ ∂ P cy = bia s j ⋅ cos ( θ ) 组装： 梯度 l x l_x l x
l x = ∂ L ∂ x = ( λ + μ ⋅ C ( x ) ) ⋅ C ′ ( x ) l_x = \frac{\partial L}{\partial x} = (\lambda + \mu \cdot C(x)) \cdot C'(x) l x = ∂ x ∂ L = ( λ + μ ⋅ C ( x )) ⋅ C ′ ( x ) Hessian l x x l_{xx} l xx
l x x = μ ⋅ C ′ ( x ) T ⋅ C ′ ( x ) + ( λ + μ C ) ⋅ C ′ ′ ( x ) l_{xx} = \mu \cdot C'(x)^T \cdot C'(x) + (\lambda + \mu C) \cdot C''(x) l xx = μ ⋅ C ′ ( x ) T ⋅ C ′ ( x ) + ( λ + μ C ) ⋅ C ′′ ( x ) 为什么 ConstraintCost 制造了非对角项？
H ≈ μ ⋅ J J T H \approx \mu \cdot \mathbf{J} \mathbf{J}^T H ≈ μ ⋅ J J T 这里的 J \mathbf{J} J C ( x ) C(x) C ( x ) 3 × 1 3 \times 1 3 × 1
J = [ ∂ C ∂ x ∂ C ∂ y ∂ C ∂ θ ] \mathbf{J} = \begin{bmatrix}
\frac{\partial C}{\partial x} \\
\frac{\partial C}{\partial y} \\
\frac{\partial C}{\partial \theta}
\end{bmatrix} J = ∂ x ∂ C ∂ y ∂ C ∂ θ ∂ C 当我们做外积（Outer Product） J J T \mathbf{J} \mathbf{J}^T J J T
J J T = [ ( ∂ C ∂ x ) 2 ∂ C ∂ x ∂ C ∂ y ∂ C ∂ x ∂ C ∂ θ ∂ C ∂ y ∂ C ∂ x ( ∂ C ∂ y ) 2 ∂ C ∂ y ∂ C ∂ θ ∂ C ∂ θ ∂ C ∂ x ∂ C ∂ θ ∂ C ∂ y ( ∂ C ∂ θ ) 2 ] \mathbf{J} \mathbf{J}^T = \begin{bmatrix}
(\frac{\partial C}{\partial x})^2 & \color{red}{\frac{\partial C}{\partial x}\frac{\partial C}{\partial y}} & \color{red}{\frac{\partial C}{\partial x}\frac{\partial C}{\partial \theta}} \\
\color{red}{\frac{\partial C}{\partial y}\frac{\partial C}{\partial x}} & (\frac{\partial C}{\partial y})^2 & \color{red}{\frac{\partial C}{\partial y}\frac{\partial C}{\partial \theta}} \\
\color{red}{\frac{\partial C}{\partial \theta}\frac{\partial C}{\partial x}} & \color{red}{\frac{\partial C}{\partial \theta}\frac{\partial C}{\partial y}} & (\frac{\partial C}{\partial \theta})^2
\end{bmatrix} J J T = ( ∂ x ∂ C ) 2 ∂ y ∂ C ∂ x ∂ C ∂ θ ∂ C ∂ x ∂ C ∂ x ∂ C ∂ y ∂ C ( ∂ y ∂ C ) 2 ∂ θ ∂ C ∂ y ∂ C ∂ x ∂ C ∂ θ ∂ C ∂ y ∂ C ∂ θ ∂ C ( ∂ θ ∂ C ) 2
