做控制、做机器人规划、甚至做视觉SLAM后端优化的朋友对ilqr这个名字应该都不陌生。我最早接触ilqr是在研究机械臂轨迹优化时当时刚被非线性规划那套SQP方法折磨得够呛换成ilqr之后整个流程清爽了不止一个量级。所谓ilqr全称Iterative Linear Quadratic Regulator迭代线性二次调节器本质上是先把一个非线性最优控制问题在当前轨迹附近用线性动力学加二阶代价函数近似成一个LQR子问题然后反复迭代求解逐步逼近原问题的最优解。它既能处理状态约束和输入约束又不需要像直接配点法那样去维护一个巨大的稀疏矩阵天然适合实时性要求高的场景。这篇文章我打算从最基础的轨迹优化建模讲起把ilqr的数学推导一步一步拆开再给出一份可以直接复现的Python代码最后把我在实际调试过程中踩过的坑、见过的发散案例、以及怎么通过正则化和Line Search把算法救回来全部整理出来。如果你刚接触最优控制和轨迹优化这篇文章能帮你建立完整的理论框架如果你已经在用ilqr做工程后面调试经验部分应该能帮到你。1. 从一个控制问题说起ilqr到底在解什么1.1 轨迹优化问题的标准形式任何轨迹优化问题都能写成一套标准形式。考虑一个离散时间系统状态转移方程为[ x_{t1} f(x_t, u_t) ]其中 (x_t) 是系统在时刻 (t) 的状态(u_t) 是时刻 (t) 的控制输入(f) 一般是非线性函数。比如机械臂的关节角度和角速度是状态关节力矩是控制四旋翼的位置、速度、姿态是状态螺旋桨推力是控制。我们的目标是找一组控制序列 (u_0, u_1, \ldots, u_{T-1})让系统从给定的初始状态 (x_0) 出发跑完整个时间窗口后既能让状态轨迹尽可能贴近期望路径又不会消耗过大的控制能量。这个目标用数学语言表达就是最小化下面的代价函数[ J(x_0, u_{0:T-1}) \sum_{t0}^{T-1} l(x_t, u_t) \phi(x_T) ]其中 (l(x_t, u_t)) 是运行代价描述每个时刻状态和控制的“好坏程度”(\phi(x_T)) 是终端代价约束最终状态。注意 (J) 里面的状态序列并不是独立变量它们由动力学方程 (x_{t1}f(x_t,u_t)) 递推决定所以真正自由的变量其实就是控制序列 (u_{0:T-1})状态轨迹是被动跟着控制走的。我在最初学这部分时有个误区总觉得轨迹优化就是要同时优化一堆状态变量和控制变量像配点法那样把所有变量都堆进一个优化器里。但ilqr走的是另一条路它把状态看成控制的“影子”通过对动力学做递推把优化问题化约成只关于控制的局部搜索问题。这个思路的好处是变量维度低、结构清晰缺点是每一步都要串行递推难以并行。不过对大部分实时控制场景来说这个代价完全可以接受。1.2 为什么是“迭代”“线性二次”“迭代”两个字是ilqr的精髓也是它区别于普通LQR的地方。经典LQR只处理线性系统 (x_{t1} A x_t B u_t) 和二次代价函数这类问题存在解析解可以直接用Riccati方程求解。但现实系统几乎没有线性的所以ilqr的做法是把非线性动力学在每条候选轨迹附近做一阶泰勒展开把代价函数在候选轨迹附近做二阶泰勒展开然后在这个局部近似模型上解一个LQR问题得到一个控制修正量。修正完之后得到一条新轨迹再在新轨迹附近重新线性化重复这个过程。这个思路和一个经典比喻很像你要在山谷里找最低点但是视野只能看到脚下这一小块地。LQR就是基于脚下这一小块地形算出一个最陡下降方向走一小步ilqr就是不断地停一下、重新看脚下、再走一步。每一步都在更新你对地形的认识最终逼近山谷底部。那么问题来了如果每次只往前走一小步效率会不会很低实践下来并不会因为与传统基于梯度的优化方法相比ilqr利用了动力学和代价函数的二阶结构信息收敛速度接近Newton类方法在问题规模适中时通常十几步到几十步就能收敛。这背后具体的数学机制下一节详细拆解。2. ilqr核心思想把最优化问题拆成两个传播过程2.1 局部近似让非线性问题“局部线性化”假设算法进行到第 (i) 轮迭代我们手里已经有一条名义轨迹记作 (\bar{x}{0:T}) 和 (\bar{u}{0:T-1})。这条轨迹可以是随便猜的比如直接把控制全部设成0然后从初始状态正推得到也可以是上一轮迭代的结果。之后我们把状态量写成名义值加偏差的形式[ x_t \bar{x}_t \delta x_t, \quad u_t \bar{u}_t \delta u_t ]然后对动力学方程做一阶Taylor展开[ x_{t1} \approx \bar{x}_{t1} A_t \delta x_t B_t \delta u_t ]其中 (A_t) 和 (B_t) 是 (f) 在 ((\bar{x}_t, \bar{u}_t)) 处对状态和控制的Jacobian矩阵。如果你不熟悉Jacobian可以简单理解成(A_t) 描述状态偏差如何影响下一时刻状态(B_t) 描述控制偏差如何影响下一时刻状态。对于实际系统这两个矩阵一般可以通过数值差分或自动微分得到不需要手动推导复杂的导数公式。代价函数同样做二阶近似。把运行代价在每个时间步展开到二阶终端代价在最终状态处展开到二阶。展开完以后整个问题变成了一个标准的有限时域LQR问题目标是最小化一个关于 (\delta x_t) 和 (\delta u_t) 的二次函数同时满足线性递推关系。这个子问题有解析解不需要调用通用的非线性优化求解器。我在实际写代码时最常犯的错误是把 (A_t, B_t) 的维度搞错。假设状态维度是 (n_x)控制维度是 (n_u)那么 (A_t) 的形状是 (n_x \times n_x)(B_t) 的形状是 (n_x \times n_u)。如果系统模型是自写的建议一上来就用随机数测试一遍维度别等跑起来报错了再回头查。2.2 反向传播从终端时刻倒推价值函数局部近似做完之后关键的一步是逆向递推。动态规划思想告诉我们一个最优控制问题的最优代价函数可以用Bellman方程表达。定义时刻 (t) 的最优代价函数 (V_t(x_t)) 为从状态 (x_t) 出发在当前时刻 (t) 到最后时刻 (T) 的最优累计代价。Bellman方程写作[ V_t(x_t) \min_{u_t} \left[ l(x_t, u_t) V_{t1}(x_{t1}) \right] ]ilqr的做法是假设 (V_{t1}(x_{t1})) 在名义轨迹附近可以近似为一个二次函数。拿着这个假设把动力学线性近似代入Bellman方程就可以把 (l V_{t1}) 合并成一个关于 (\delta x_t) 和 (\delta u_t) 的二次函数记作 (Q_t(\delta x_t, \delta u_t))[ Q_t \begin{bmatrix} 1 \ \delta x_t \ \delta u_t \end{bmatrix}^T \begin{bmatrix} 0 Q_x^T Q_u^T \ Q_x Q_{xx} Q_{xu} \ Q_u Q_{ux} Q_{uu} \end{bmatrix} \begin{bmatrix} 1 \ \delta x_t \ \delta u_t \end{bmatrix} ]这个二次函数里的各项是由当前代价函数的梯度、Hessian和下一时刻价值函数的梯度、Hessian以及动力学Jacobian乘出来的。具体的乘法关系我会在代码部分展示这里先抓重点。对每个时刻 (t)我们要算出最优的控制修正。固定的 (\delta x_t) 下(Q_t) 对 (\delta u_t) 求导并令其等于0[ \frac{\partial Q_t}{\partial \delta u_t} Q_u Q_{uu} \delta u_t Q_{ux} \delta x_t 0 ]于是得到局部最优控制律[ \delta u_t^* -Q_{uu}^{-1} \left( Q_u Q_{ux} \delta x_t \right) ]注意这里有个维度细节(Q_{ux}) 是 (n_u \times n_x) 矩阵它刻画控制偏差对状态偏差的一阶反馈增益。传统LQR中这个矩阵对应反馈增益矩阵Kilqr里把它写成 (K_t)[ k_t -Q_{uu}^{-1} Q_u, \quad K_t -Q_{uu}^{-1} Q_{ux} ]所以 (\delta u_t k_t K_t \delta x_t)。其中 (k_t) 是前馈控制修正代表当前名义控制应该往哪个方向调整(K_t) 是反馈增益代表如果实际状态跟名义轨迹有偏差控制应该如何响应。这就是ilqr输出里最有价值的部分它不仅是开环控制序列还附带一个时变的线性反馈控制器。得到最优控制律后把 (\delta u_t) 代回 (Q_t)就得到更新后的 (V_t)。这一步在数学上叫“极小化后消元”实际效果是把价值函数的二次近似参数从下一时刻传递到当前时刻[ V_x Q_x - Q_{xu} Q_{uu}^{-1} Q_u ][ V_{xx} Q_{xx} - Q_{xu} Q_{uu}^{-1} Q_{ux} ]其中 (Q_{xu}) 和 (Q_{ux}) 互为转置。从终端时刻 (T) 的 (\phi(x_T)) 开始一直倒推到 (t0)这就是“反向传播”的含义。这个递推过程和LQR里的Riccati方程在数学上完全等价但形式上更适合写成循环代码。2.3 正向传播用新控制律更新状态轨迹反向传播算完我们就得到了整条轨迹上每个时刻的 (k_t) 和 (K_t)。接下来要做的是从初始状态 (x_0) 开始把新控制律作用到非线性动力学上正推出一条新轨迹。[ u_t \bar{u}_t \alpha k_t K_t (x_t - \bar{x}_t) ][ x_{t1} f(x_t, u_t) ]这里 (\alpha) 是一个步长参数它的作用我们在下一节详细讲。正推结束之后如果新轨迹的代价比原来低就接受它把它作为下一轮迭代的名义轨迹如果代价反而升高说明步长太大了需要缩小 (\alpha) 重试。需要特别注意的是正推过程的动力学必须用真实非线性模型 (f(x_t, u_t))而不是线性近似 (A_t, B_t)。很多新手在这一步想当然地用线性模型去推结果算法很快就发散或者收敛到一个和真实系统完全无关的解。我做这个东西的时候第一条调试经验就是正推时如果看到轨迹异常平滑、跟目标物距离曲线过于规律十有八九是用了线性模型在推。至此一个完整的迭代循环就闭环了反向传播计算控制增益正向传播更新轨迹然后重新线性化继续迭代直到代价函数不再显著下降或满足收敛条件。3. 藏在公式背后的工程细节正则化与Line Search3.1 为什么Qxx可能不正定以及正则化的作用理论上如果一切顺利反向传播算出来的 (Q_{uu}) 应该是对称正定矩阵这样才能保证 (\delta u_t^*) 是极小值而不是极大值或鞍点。但实际中由于数值误差、非线性模型的二阶项被忽略、或者当前轨迹离最优点太远(Q_{uu}) 很可能变成半正定甚至奇异。如果此时直接求逆结果会爆炸算法一轮发散。解决办法是加正则化项把求逆的对象替换成[ Q_{uu}^{\text{reg}} Q_{uu} \rho I ]其中 (\rho 0) 是正则化系数(I) 是单位矩阵。这个操作在数学上等于在控制变化量上施加一个二次惩罚物理意义是当你对模型不确定时别让控制变化太猛烈。(\rho) 越大每一步的控制更新越保守(\rho) 越小算法越激进但越容易发散。一个常用的策略是自适应正则化如果某次迭代后代价下降就减小 (\rho)让算法加快收敛如果代价反而上升就增大 (\rho)让算法更保守重新尝试。这个思路和LM算法Levenberg-Marquardt里的阻尼系数调节如出一辙。我习惯把 (\rho) 的初始值设为1.0每次代价上升就乘以10代价下降就乘以0.5效果很稳。当然具体数值跟模型尺度有关如果你发现算法对 (\rho) 特别敏感先检查一下代价函数各分量是否量纲统一。3.2 Line Search让更新步长可控前面提到了 (\alpha)它是控制更新幅度的标量参数。反向传播得到的 (k_t) 和 (K_t) 其实是在局部二次近似下算出的最优方向但是因为真实动力学和代价函数是非线性的这个方向的“最优性”只有在一个很小的邻域内成立。如果一上来就用 (\alpha 1) 完整应用全部修正量很可能一步迈过头轨迹直接飞出可行域。Line Search的思路是从 (\alpha 1) 开始尝试用 (\alpha) 更新轨迹并计算实际代价如果实际代价比名义代价高就把 (\alpha) 减半继续尝试直到代价下降或者 (\alpha) 小到某个阈值为止。这样既能保证每次迭代稳定下降又不会因为步子太小导致收敛过慢。实际计算时还会用到“期望代价下降量”这个量可以在反向传播过程中顺手算出来[ \Delta J_{\text{exp}} \sum_{t0}^{T-1} \left[ -k_t^T Q_{uu,t} k_t \right] ]每个时刻的期望下降都是一个负数累加起来就是整轮迭代的期望代价下降。如果实际代价下降和期望下降的比值十分接近1说明二次近似非常准确可以放心大步更新如果比值远小于1说明近似程度差需要减半步长。我把这个比值打印出来算是判断当前轨迹附近线性化质量的一个直观指标。3.3 终止条件与常见参数设置ilqr的终止条件一般看三个指标迭代轮数、代价变化量、梯度范数。我一般设置最大迭代次数100轮代价变化量小于1e-6就认为收敛梯度范数小于一个自定义阈值也可以提前退出。你需要理解的是不同类型的系统收敛速度差异很大如果50轮后代价还在持续下降但速度极慢多半不是没收敛而是当前轨迹附近存在一个较平坦的区域这通常是代价函数权重没调好导致的。参数设置方面有几个默认值可以先用起来再根据系统调整正则化初始值 (\rho 1.0)上下限建议设在1e-6到1e10之间Line Search的衰减系数设为0.5最多尝试30次张力系数控制更新的激进程度习惯上设0.5到1.0之间最大迭代次数100到200视计算资源而定这些参数在不同项目中不一定通用我的经验是先把算法跑通再逐步调节不要一开始就追求最优参数。4. 从公式到代码一个最小可运行实现4.1 代码整体结构下面这份代码我尽量精简但保留了ilqr的全部核心环节。为了不引入第三方依赖我直接用NumPy实现Jacobian和Hessian的数值差分所以任何具备NumPy的环境都能跑。整个实现分三部分第一部分是系统模型由你自己定义运动学和代价函数第二部分是数值导数工具函数第三部分是ilqr主循环完成反向传播、Line Search、正向传播。import numpy as np def finite_diff_jacobian(f, x, u, e1e-6): n_x len(x) n_u len(u) fx np.zeros((n_x, n_x n_u)) for i in range(n_x n_u): xp x.copy() xm x.copy() up u.copy() um u.copy() if i n_x: xp[i] e xm[i] - e else: up[i - n_x] e um[i - n_x] - e fp f(xp, up) fm f(xm, um) fx[:, i] (fp - fm) / (2 * e) A fx[:, :n_x] B fx[:, n_x:] return A, B def finite_diff_hessian(l, x, u, e1e-6): n_x len(x) n_u len(u) n n_x n_u H np.zeros((n, n)) z np.concatenate([x, u]) for i in range(n): for j in range(i, n): zp z.copy() zm z.copy() zpp z.copy() zmm z.copy() zp[i] e zp[j] e zm[i] - e zm[j] - e zpp[i] e zpp[j] - e zmm[i] - e zmm[j] e H[i, j] (l(zp) - l(zmm) - l(zpp) l(zm)) / (4 * e * e) H[j, i] H[i, j] lx H[:n_x, :n_x] lu H[:n_x, n_x:] lxx H[:n_x, :n_x] luu H[n_x:, n_x:] lxu H[:n_x, n_x:] return lxx, lxu, luu数值差分的写法比较笨拙但好处是模型函数随便怎么定义都能跑。如果你用的是PyTorch、JAX这类带自动微分的框架把这段换成自动微分会更准更快。数值差分在实际工程里最大的问题是步长 (e) 不好选选太大则近似误差大选太小则浮点误差占主导。我对一般量级的模型取1e-6如果模型状态量级特别大或特别小需要按比例缩放。ilqr主循环的核心代码如下。为了可读性我把一次迭代里的反向传播和正向传播分成两个函数。class ILQR: def __init__(self, f, l, phi, x0, us, n_iter100, tol1e-6): self.f f self.l l self.phi phi self.x0 np.array(x0, dtypefloat) self.us [np.array(u, dtypefloat) for u in us] self.T len(self.us) self.n_iter n_iter self.tol tol def forward(self, us, alpha1.0, KsNone, ksNone, xs_nomNone, reg0.0): N self.T n_x len(self.x0) n_u len(self.us[0]) xs [self.x0.copy()] cost 0.0 for t in range(N): if Ks is not None: du alpha * ks[t] Ks[t] (xs[-1] - xs_nom[t]) else: du np.zeros(n_u) u us[t] du cost self.l(xs[-1], u) xs.append(self.f(xs[-1], u)) cost self.phi(xs[-1]) return xs, cost def backward(self, xs, us, reg1.0): N self.T n_x len(self.x0) n_u len(self.us[0]) Vx self.phi_x(xs[-1]) Vxx self.phi_xx(xs[-1]) ks [] Ks [] for t in range(N - 1, -1, -1): A, B finite_diff_jacobian(self.f, xs[t], us[t]) lxx, lxu, luu finite_diff_hessian(self.l, xs[t], us[t]) lx, lu self.l_grad(xs[t], us[t]) Qx lx A.T Vx Qu lu B.T Vx Qxx lxx A.T Vxx A Qxu lxu A.T Vxx B Quu luu B.T Vxx B Quu_reg Quu reg * np.eye(n_u) Quu_inv np.linalg.inv(Quu_reg) k -Quu_inv Qu K -Quu_inv Qxu.T Vx Qx - Qxu Quu_inv Qu Vxx Qxx - Qxu Quu_inv Qxu.T ks.append(k) Ks.append(K) ks.reverse() Ks.reverse() return ks, Ks这里有几个地方要特别提醒。第一(V_{xx}) 的更新公式里最后一项右侧是 (Q_{xu}^T) 而不是 (Q_{xu})写错会导致整个递推失去对称性。第二(k) 和 (K) 的定义要跟正向传播里的用法一致我在代码里统一用 (\delta u \alpha k K \delta x)。第三梯度 (l_x) 和 (l_u) 我直接用数值差分计算还有一个更省事的方法是用有限差分差分代价函数关于每个分量的偏导什至可以借助前面的Hessian计算函数额外返回对角线以外的东西但为了准确我是单独写的 (l_grad)def l_grad(self, x, u): n_x len(x) n_u len(u) e 1e-6 lx np.zeros(n_x) for i in range(n_x): xp x.copy() xm x.copy() xp[i] e xm[i] - e lx[i] (self.l(xp, u) - self.l(xm, u)) / (2 * e) lu np.zeros(n_u) for i in range(n_u): up u.copy() um u.copy() up[i] e um[i] - e lu[i] (self.l(x, up) - self.l(x, um)) / (2 * e) return lx, lu数值差分梯度虽然看起来代码长但好处是任何自定义的 (l(x,u)) 都能直接适配。如果你做的是项目验证这种“免求导”的实现方式能让你把注意力放在算法本身而不是繁琐的导数推导上。等算法验证通过再去考虑用解析导数替换数值差分来提速。4.2 主循环与Line Search有了正向传播和反向传播主循环就水到渠成了。每一步迭代先反向传播得到 (k_t, K_t)然后从 (\alpha1) 开始做Line Search找到让代价下降的步长更新控制序列检查终止条件。def run(self): N self.T n_x len(self.x0) n_u len(self.us[0]) xs_nom, cost_nom self.forward(self.us) reg 1.0 for it in range(self.n_iter): ks, Ks self.backward(xs_nom, self.us, regreg) alpha 1.0 accepted False for _ in range(30): xs_new, cost_new self.forward(self.us, alphaalpha, KsKs, ksks, xs_nomxs_nom) if cost_new cost_nom: accepted True break alpha * 0.5 reg * 10.0 if not accepted: print(Line search failed, increase reg) reg * 10.0 continue delta abs(cost_nom - cost_new) self.us [] for t in range(N): du alpha * ks[t] Ks[t] (xs_new[t] - xs_nom[t]) self.us.append(self.us_prev[t] du) # 注意这里要保存上一轮控制 xs_nom, cost_nom self.forward(self.us) reg max(reg * 0.5, 1e-6) print(fIter {it1}, cost{cost_nom:.6e}, alpha{alpha:.3f}, reg{reg:.3e}) if delta self.tol: break return xs_nom, self.us, ks, Ks要提醒的是上面主循环里我留了一个变量 (self.us_prev)实际使用时需要在每次迭代前把 (self.us) 的副本保存下来否则更新控制序列时加减会乱套。更好的写法是直接基于 (xs_nom) 和 (us_nom)通过Line Search得到 (u_t \bar{u}_t \alpha k_t K_t(x_t - \bar{x}_t)) 后生成新的控制序列。我自己的实现里通常用数组索引来操作避免这种绕来绕去的引用问题。Line Search失败的处理方式要灵活。如果连续多轮迭代代价都不下降先别急着把正则化疯狂放大先检查是不是代价函数本身存在约束冲突或者Bellman近似的泰勒展开出了界。正则化放大只是治标真正的问题是当前轨迹附近模型不准确。4.3 一个简单实例二维点质量轨迹规划理论讲得再多不如跑一个例子。假设一个二维平面上的点质量模型状态 (x [p_x, p_y, v_x, v_y]^T)控制 (u [a_x, a_y]^T)动力学为[ x_{t1} x_t dt \cdot [v_x, v_y, a_x, a_y]^T ]这个模型虽然简单但包含了速度和加速度的积分关系很适合验证轨迹优化算法。我设定起点为原点目标位置为终点运行代价里加一个对目标位置的二次惩罚控制代价也设为二次形式。用全零控制作为初始猜测运行ilqr效果非常直观算法在迭代中逐渐“学会”先加速再减速最终形成一条平滑的轨迹。模型函数和代价函数写成dt 0.05 def f(x, u): x_next x.copy() x_next[0] x[2] * dt x_next[1] x[3] * dt x_next[2] u[0] * dt x_next[3] u[1] * dt return x_next def l(x, u): goal np.array([2.0, 2.0, 0.0, 0.0]) state_cost 0.5 * (x - goal) np.diag([10.0, 10.0, 1.0, 1.0]) (x - goal) control_cost 0.5 * u u return state_cost control_cost def phi(x): goal np.array([2.0, 2.0, 0.0, 0.0]) return 0.5 * (x - goal) np.diag([100.0, 100.0, 10.0, 10.0]) (x - goal)运行这个例子后你会看到每轮迭代的代价都在下降。大约迭代10到20轮轨迹已经能稳定地到达目标点附近控制量前后段呈“加速-减速”的模式。这就是ilqr在轨迹规划里最常见的使用方式先离线把参考轨迹算出来再在线用反馈增益 (K_t) 做跟踪控制。5. 代码实践中的常见问题与调试实录5.1 发散与振荡怎么看日志定位问题ilqr调试中最常见的问题是发散。你满怀期待地运行代码结果不到几轮迭代代价飞到了1e20开外控制量直接爆炸。碰到这种情况我的第一反应永远是看Line Search里的 (\alpha) 和正则化系数 (\rho)。如果 (\rho) 已经涨到1e8以上但代价还在涨说明问题不在数值稳定性而是当前轨迹离真实解太远局部二次模型已经完全不可信。另一个常见的现象是振荡。代数轮迭代后代价下降然后反弹再下降再反弹。这个模式通常意味着正则化调整策略过于激烈或者步长衰减速度不合理。我用过的有效做法是如果连续3轮迭代代价都在同一个数量级内来回震荡强制把当前轨迹重新初始化一下比如在现有轨迹上加一点随机扰动再重新开始迭代。有时候这是跳出局部极小的一种有效手段。发散现场一定要看日志。把每轮迭代的代价、最大控制范数、最大状态偏差、(\alpha)、(\rho) 全部打印出来问题往往一目了然。我见过太多人只print一个最终cost出了问题完全不知道从哪查起。5.2 数值稳定性逆矩阵、正则化、浮点误差数值稳定性是ilqr的永恒话题。反向传播中需要对 (Q_{uu}) 求逆如果矩阵接近奇异逆矩阵的数值误差会被放大。除了加正则化之外还可以对 (Q_{uu}) 做特征值裁剪把过小的特征值直接替换成一个下限值再重组矩阵求逆。这个方法比单纯加对角正则化更精细但实现稍复杂。浮点误差方面数值差分步长的选择直接影响Jacobian和Hessian的精度。我个人的经验是如果模型函数里有量级很大的常量比如物理常数、大范围的坐标值先做归一化处理把状态和控制都缩放到接近1的量级数值差分会稳定得多。这一条对所有基于梯度的优化算法都适用不只是ilqr。还有一个容易忽略的点代价函数的Hessian必须是对称的。数值差分计算时如果只计算上三角然后手工复制到对称位置像前面代码里写的那样能避免因为浮点误差造成微小不对称。某些线性代数库对非对称矩阵求逆也能出结果但结果会很怪找bug时极难察觉。5.3 边界约束与复杂系统的实用建议ilqr的经典形式并不直接支持约束比如关节角度限制、控制输入饱和、避障约束。但这些在工程里又逃不掉。几种常见的处理方法第一种是把约束转化为代价函数的惩罚项比如对超出限位的部分施加高次罚函数第二种是做控制量裁剪在正向传播时把控制限幅但这会破坏线性化的自洽性可能导致算法不稳定第三种是引入Barrier函数或Augmented Lagrangian把约束问题转化成一个带惩罚系数的无约束问题迭代中逐步增大惩罚系数。我练过不少带避障约束的项目最实用的组合是把障碍物的距离函数作为惩罚项加入代价然后配上正则化和Line Search。虽然理论上的收敛性不如专门处理不等约束的算法那么漂亮但在工程上能解决90%的问题。如果约束特别严格我建议你还是考虑用SQP或内点法不要硬拿ilqr去撞。还有一点是想做实时控制的朋友最关心的计算效率。ilqr每轮迭代需要对每个时间步计算Jacobian和Hessian这是最耗时的部分。优化手段无非三种减少时间离散点数、用解析导数替代数值差分、把整个反向传播用矩阵批量运算实现。我用矩阵批量运算重写之后在500步时间离散的机械臂问题上单轮迭代从几十毫秒降到了几毫秒提升非常可观。6. 最后分享一点个人体会ilqr最让我着迷的地方是它把复杂的非线性最优控制问题拆解成一次次“线性化反向递推正向滚动”的循环结构清晰到几乎每一步都能在物理上找到解释。我做过不下几十个轨迹规划项目从简单的点到点运动到带避障的机械臂抓取ilqr都是第一个给答案的算法哪怕答案不完美它也提供了极有价值的初始轨迹给后续的精细调整省了大量时间。踩过这么多坑之后我最大的体会是不要一上来就把所有技巧全上。先跑通一个最简单的版本确认动力学模型、代价函数、维度都没问题再慢慢加正则化、Line Search这些工程细节。ilqr的数学推导并不复杂真正的难点在于把公式变成代码后还要让它在真实系统上稳如老狗。希望这篇文章能帮你少走我当初走过的弯路。