如果你做过机器人控制或者轨迹优化十有八九会碰到这个词iLQR。我第一次认真啃它是在调一个倒立摆的“摆起”动作——从自然下垂的静止状态想办法把杆子甩到竖直朝上并且稳定住。乍一听这问题很经典但真写起来会发现系统是非线性的目标状态又离初始状态很远普通LQR根本没法一次搞定。后来把iLQR从原理到代码完整过了一遍才彻底想明白这类“非线性最优控制”该怎么落地。这篇文章我想把iLQR的数学推导和代码实践放在一起讲。整个过程不会只贴公式我会把每一步的“为什么”也拆开后面给出一段能直接跑的Python实现再聊聊我实际调试时踩过的坑。适合正在学最优控制、轨迹优化或者想在自己机器人系统里做运动规划的朋友参考。1. 先把这个算法放在地图上它到底解决了什么问题1.1 从最优控制说起最优控制的经典设定是给定一个系统的状态方程x_{k1} f(x_k, u_k)要找到一串控制输入 u_0,...,u_{N-1}使得从初始状态 x_0 出发的轨迹在某些成本下最“省事”。比如让机器人走到目标点同时控制量尽量小路径别太粗暴。这个问题用数学写出来就是个带约束的优化问题minimize J Σ_{k0}^{N-1} c(x_k,u_k) c_f(x_N)如果系统是线性的成本是二次的那么LQR用Riccati方程可以直接解出最优反馈律。但真实系统几乎都是非线性的机械臂有重力项和科氏力倒立摆有sin(θ)四旋翼空气动力学更是复杂。这时候LQR的闭环形式就失效了得另想办法。iLQR做的事情就是在每轮迭代里把“非线性系统在参考轨迹附近线性化、把成本函数做二次近似”然后套用LQR的后向动态规划思想得到一个局部最优的控制修正量再去前向模拟更新轨迹反复迭代直到收敛。所以你可以把它理解成一个“在轨迹层面做梯度下降、在算法内部用LQR做子问题求解”的工具。1.2 非线性轨迹优化的困难非线性轨迹优化最直接的想法是把所有状态和控制都当成变量用通用优化器求解比如在Python里用scipy.optimize.minimize。这个方法思路简单但问题也很明显变量维度太高。一个N100步状态8维的系统优化变量可能有上千个再加上各种约束很多优化器跑起来又慢又不稳定。另一种思路是用动态规划从终点往前推把“如果我在这一步处于某个状态最优的后续成本是多少”记下来。这种方法理论上很漂亮但状态空间是连续的除非离散化成网格否则“值函数”没法存。维度一高就爆炸。iLQR取了一个折中我们不求整个状态空间的全局值函数而是在当前已经有的参考轨迹附近通过局部线性化构造一个二次函数来近似值函数。这样既避开了状态空间离散化又保留了动态规划的高效结构。1.3 iLQR的整体策略线性化动态规划打个比方iLQR像你开车导航时已经有一条初始路线然后你沿着这条路走走看看发现某段路段绕了就只在这段路的周围重新规划局部路线。它的核心步骤可以压缩成两句话后向传递从终点开始反向扫描把“当前轨迹附近的最优控制策略”算出来。这个策略不是全局的而是依赖于当前状态与参考轨迹的偏差所以很容易用线性反馈表示。前向传递用刚算出来的策略从初始状态重新跑一遍系统得到一条新轨迹。如果新轨迹成本更低就接受它否则缩小更新步长再试。整个过程不停重复直到成本不再下降或者轨迹不再变化。这就是iLQR迭代的全貌。2. 原理推导iLQR中的每一个数学式子是怎么来的2.1 问题形式化我们先把离散系统写清楚。假设有 N 个时间步状态 x ∈ ℝⁿ控制 u ∈ ℝᵐ动力学模型为x_{k1} f(x_k, u_k)成本函数是叠加式的J Σ_{k0}^{N-1} c(x_k, u_k) c_f(x_N)其中 c 是运行成本c_f 是终末成本。我们希望求一组合适的 u_0,...,u_{N-1}让 J 最小。假设现在已经有一组参考轨迹用 x̄_k 和 ū_k 表示。iLQR要做的就是在 x̄_k, ū_k 附近寻找一个更优解。所以后面的推导里我会用增量表达δx_k x_k - x̄_kδu_k u_k - ū_k对动力学做泰勒展开保留一阶项x_{k1} f(x̄_kδx_k, ū_kδu_k) ≈ f(x̄_k, ū_k) A_k δx_k B_k δu_k x̄_{k1} A_k δx_k B_k δu_k其中A_k ∂f/∂x |{x̄_k, ū_k}B_k ∂f/∂u |{x̄_k, ū_k}这样动力学就被局部线性化了。我们把控制问题转化为一个关于增量 δx, δu 的LQR问题。2.2 在参考轨迹附近做局部近似成本函数也做局部近似。定义当前状态下的成本梯度与Hessianl_x ∂c/∂xl_u ∂c/∂ul_xx ∂²c/∂x²l_xu ∂²c/∂x∂ul_uu ∂²c/∂u²然后对成本函数做二阶泰勒展开c ≈ const l_xᵀ δx l_uᵀ δu 0.5 δxᵀ l_xx δx δxᵀ l_xu δu 0.5 δuᵀ l_uu δu同样终末成本也展开到二阶。注意这里的近似方式正好对应“成本函数二次化、动力学线性化”这正是iLQR名字里“Iterative Linear Quadratic”的由来。如果连动力学的二阶项也保留那就是DDPDifferential Dynamic Programming。2.3 后向传递从终点往起点推后向传递是iLQR最有意思的地方。我们从最后一个时刻开始定义值函数 V_k(x_k) 为“从时刻 k 状态 x_k 出发后续的最优累积成本”满足贝尔曼方程V_k(x_k) min_{u_k} [ c(x_k,u_k) V_{k1}(x_{k1}) ]在局部二次近似下可以假设 V_{k1} 在 x̄_{k1} 附近的近似为V_{k1}(x) ≈ V_{k1}(x̄_{k1}) V_xᵀ δx 0.5 δxᵀ V_xx δxV_x 是梯度向量V_xx 是Hessian矩阵。现在把动力学线性化代入定义带下标 x, u 的Q函数Q_k(δx, δu) c(x̄_kδx, ū_kδu) V_{k1}(x̄_{k1} A_kδx B_kδu)把前面所有二阶展开代入并整理可以得到以下系数Q_x l_x Aᵀ V_x Q_u l_u Bᵀ V_x Q_xx l_xx Aᵀ V_xx A Q_xu l_xu Aᵀ V_xx B Q_uu l_uu Bᵀ V_xx B其中右上角带撇的 V_x 和 V_xx 表示时刻 k1 的值函数梯度与Hessian。现在我们要在当前状态偏差 δx 下找出使 Q 最小的 δu。对 Q 关于 δu 求导并令其等于零得到Q_uu δu Q_xuᵀ δx Q_u 0 δu -Q_uu^{-1} (Q_u Q_xuᵀ δx)如果把解写成δu k_k K_k δx那么k_k -Q_uu^{-1} Q_u K_k -Q_uu^{-1} Q_xuᵀ这里 k_k 是前馈修正项不依赖当前状态偏差K_k 是反馈增益矩阵决定系统如果偏离参考轨迹时控制量该如何修正。得到最优δu之后再把解代回Q函数就能得到更新后的值函数系数V_x Q_x Q_xu k_k V_xx Q_xx Q_xu K_k这样从第 N 步往回一直推到第 0 步每一步都能算出对应的 k_k 和 K_k这就是后向传递的完整流程。2.4 前向传递和线搜索把理论变回真实轨迹有了每个时间步的 k_k 和 K_k就可以从初始状态开始重新模拟整个系统。步骤是x_0 x_0 对 k 0,...,N-1 δx_k x_k - x̄_k u_k ū_k α * k_k K_k * δx_k x_{k1} f(x_k, u_k)注意这里多了一个 α通常是在 (0,1] 之间。因为线性化只在参考轨迹附近近似成立如果直接使用完整的前馈修正 α1可能一步迈太大导致新轨迹成本不降反升。所以需要用线搜索从 α1 开始逐步缩小直到新轨迹的总成本比当前参考轨迹更低才接受这次更新。这个前向后向循环就是一次完整的iLQR迭代。每次迭代以后把新的轨迹作为参考轨迹再重新线性化继续迭代直到成本变化小于某个阈值或者达到最大迭代次数。2.5 和LQR、DDP的区别很多人会把iLQR和LQR、DDP搞混。简单说LQR要求系统线性成本二次一次求解没有迭代没有轨迹更新。iLQR对非线性系统在参考轨迹上线性化和二次化然后反复迭代动力学的二阶信息不保留。DDP和iLQR很像但会在展开动力学时保留二阶项理论上收敛更快但计算量更大实现也更复杂。实际工程里iLQR已经足够好用而且比DDP更容易写对。很多开源MPC、轨迹优化库的底层用的就是iLQR或它的变种。3. 代码实践用一个摆杆起摆例子完整走一遍3.1 环境、参数和模型为了不让代码和现实脱节我这里用一个最简单的非线性系统——旋转摆杆类似倒立摆的水平运动版本来做示例。状态取为x [θ, ω]ᵀ其中 θ 是摆杆与竖直朝下方向的夹角ω 是角速度。控制 u 是关节力矩。动力学用连续时间模型θ̇ ω ω̇ (u - bω - mglsin(θ)) / (m*l²)这里 b 是阻尼系数m 是摆杆质量l 是摆杆长度g 是重力加速度。目标是让摆杆从 θ0自然下垂甩到 θπ竖直朝上。离散化我用简单的欧拉近似x_{k1} x_k dt * f(x_k, u_k)dt 取 0.01总时间 2 秒所以 N200。3.2 成本函数与导数运行成本写成关于目标状态的二次型加上控制惩罚c(x,u) 0.5 * (x - x_goal)ᵀ Q_cost (x - x_goal) 0.5 * R * u²这里 Q_cost 是2x2对角线矩阵R 是控制权重。终末成本同样取一个较大的二次型让末端状态严格靠近目标。代码中可以这样定义import numpy as np # 系统参数 m 1.0 l 1.0 g 9.81 b 0.1 dt 0.01 N 200 # 成本矩阵 Q_cost np.diag([10.0, 1.0]) R 0.01 Qf np.diag([100.0, 10.0]) x_goal np.array([np.pi, 0.0]) def dynamics(x, u): theta, omega x theta_dot omega omega_dot (u - b * omega - m * g * l * np.sin(theta)) / (m * l * l) return x dt * np.array([theta_dot, omega_dot]) def final_cost(x): return 0.5 * (x - x_goal) Qf (x - x_goal) def running_cost(x, u): return 0.5 * (x - x_goal) Q_cost (x - x_goal) 0.5 * R * u * u def final_cost_grad(x): return Qf (x - x_goal) def final_cost_hess(x): return Qf def running_cost_grad(x, u): l_x Q_cost (x - x_goal) l_u R * u return l_x, l_u def running_cost_hess(x, u): l_xx Q_cost l_xu np.zeros((2, 1)) l_uu np.array([[R]]) return l_xx, l_xu, l_uu之所以把成本和导数拆开写是因为iLQR的后向传递需要这些项。用有限差分也可以但手推这几个矩阵其实很简单而且数值更稳定。3.3 实现后向传递后向传递的核心是求动力学雅可比。在示例里A 和 B 是2x2和2x1的矩阵我用中心差分求数值雅可比这样模型换了也方便。注意成本Hessian是常数矩阵所以不用重新算。def dynamics_jacobian(x, u): eps 1e-6 n len(x) m np.size(u) A np.zeros((n, n)) B np.zeros((n, m)) for i in range(n): x_plus x.copy() x_minus x.copy() x_plus[i] eps x_minus[i] - eps A[:, i] (dynamics(x_plus, u) - dynamics(x_minus, u)) / (2 * eps) for j in range(m): u_plus u.copy() u_minus u.copy() u_plus[j] eps u_minus[j] - eps B[:, j] (dynamics(x, u_plus) - dynamics(x, u_minus)) / (2 * eps) return A, B def backward_pass(x_traj, u_traj): n x_traj.shape[1] m u_traj.shape[1] # 存储每步的增益 K_list [] k_list [] # 从终点开始初始化值函数 Vx final_cost_grad(x_traj[-1]) Vxx final_cost_hess(x_traj[-1]) for k in range(N - 1, -1, -1): x_k x_traj[k] u_k u_traj[k] A, B dynamics_jacobian(x_k, u_k) l_x, l_u running_cost_grad(x_k, u_k) l_xx, l_xu, l_uu running_cost_hess(x_k, u_k) Qx l_x A.T Vx Qu l_u B.T Vx Qxx l_xx A.T Vxx A Qxu l_xu A.T Vxx B Quu l_uu B.T Vxx B # 加入正则项防止Quu奇异 reg 1e-6 Quu_reg Quu reg * np.eye(m) K -np.linalg.solve(Quu_reg, Qxu.T) k -np.linalg.solve(Quu_reg, Qu) K_list.append(K) k_list.append(k) # 更新值函数 Vx Qx Qxu k Vxx Qxx Qxu K # 逆序返回因为是从后往前算的 return list(reversed(K_list)), list(reversed(k_list))这里正则项 reg 是我故意加上的。因为到了运算后期 Quu 可能接近奇异直接求逆会让结果剧烈震荡。加上一个很小的单位阵相当于在控制方向上加了点信心保证可逆性。3.4 实现前向传递和主迭代前向传递的时候要跑一遍真实动力学并计算新轨迹的总成本。注意这里用的参考轨迹还是旧的 x_traj 和 u_traj反馈控制中的偏差项要在实时计算。def forward_pass(x_traj, u_traj, K_list, k_list, alpha1.0): x x_traj[0].copy() new_x_traj [x] new_u_traj [] total_cost 0.0 for k in range(N): dx x - x_traj[k] u u_traj[k] alpha * k_list[k] K_list[k] dx new_u_traj.append(u) x dynamics(x, u) new_x_traj.append(x) total_cost running_cost(x, u) total_cost final_cost(x) return np.array(new_x_traj), np.array(new_u_traj), total_cost然后主循环就是反复做“后向→前向→判断成本”。如果成本没下降就把 α 对半缩小再试。代码def ilqr_solve(np.random.seed0, max_iter100): # 初始参考轨迹全零控制状态保持初始值附近 x0 np.array([0.0, 0.0]) x_traj np.tile(x0, (N 1, 1)) u_traj np.zeros((N, 1)) for iter in range(max_iter): K_list, k_list backward_pass(x_traj, u_traj) alpha 1.0 while alpha 1e-4: new_x_traj, new_u_traj, new_cost forward_pass(x_traj, u_traj, K_list, k_list, alpha) # 算旧成本 old_cost 0.0 for k in range(N): old_cost running_cost(x_traj[k], u_traj[k]) old_cost final_cost(x_traj[-1]) if new_cost old_cost: x_traj new_x_traj u_traj new_u_traj break alpha * 0.5 if alpha 1e-4: print(fiter {iter}: line search failed) break if abs(old_cost - new_cost) 1e-6: print(fiter {iter}: converged, cost {new_cost:.6f}) break return x_traj, u_traj x_sol, u_sol ilqr_solve()这段代码可以直接跑。在我的测试里用上述参数一般几十次迭代就能把摆杆从下垂状态甩到竖直朝上。注意初始参考轨迹用的是“保持初始状态不动、控制为零”的轨迹也就是说一开始系统并不知道怎么摆上去完全靠iLQR迭代把动作“逼”出来。3.5 跑起来看现象跑完后把 θ 画出来看到的曲线大概会是一开始缓慢偏离逐渐摆动加速最后靠近 π 并在末端几乎静止。u 曲线则会有明显的脉冲式加速类似“先往后拉再往前甩”的动作。这说明iLQR找到了一条很自然的起摆轨迹。如果对反馈矩阵 K 做可视化还能看到在接近竖直状态时增益变得很大——这与系统在倒立位置的线性化不稳定特征相符。你甚至可以最后一段换用LQR基于竖直状态附近的线性化系统就能稳定住。4. 使用iLQR避坑指南4.1 数值稳定性与正则化我一开始实现的时候后向传递里直接 np.linalg.inv(Quu)结果跑到一半开始震荡成本一路爆高。后来才发现当控制输入对值函数的影响趋近于零时Quu 会接近奇异。尤其是控制惩罚 R 设得很小的时候这种情况更明显。解决办法有两个一是给 Quu 加正则项也就是上面代码里的 reg二是对 Quu 做Cholesky分解或特征值筛选把过小的特征值clip到某个阈值以上。实际工程里正则项大小还可以动态调整如果成本不下降就增大正则项让更新更保守如果下降顺利就把正则项适当减小。4.2 初始参考轨迹从哪来iLQR是局部优化算法非常依赖初始轨迹。如果初始参考轨迹离可行域太远后向传递时线性化可能完全不靠谱线搜索怎么缩小步长都不下降最后直接失败。对于像摆杆起摆这种简单系统从“静止下垂”开始迭代通常没问题。但对于更复杂的机械臂或自动驾驶场景建议先用一个粗糙的路径规划器生成几何路径再用三次样条或滤波平滑作为iLQR的初始轨迹。还有一种常见做法是先用零控制或者前馈控制跑几米只要别撞车后续iLQR能慢慢优化出轨迹。4.3 线搜索参数怎么调线搜索的标准思路是Armijo条件从 α1 开始如果成本下降满足new_cost old_cost β * α * (dJ/dα |_{α0})就直接接受否则 α 乘以一个缩小因子。最简单实现就是按 0.5 倍缩小。实际调参时如果发现前向模拟经常失败可以初始 α 就设成0.5或0.3。但也不要太小否则收敛极慢。我一般会设 max_line_search_iter 20保证每个梯度方向有充分尝试空间。4.4 收敛判据和常见失败模式iLQR的收敛判据一般看三种成本变化、轨迹变化、梯度norm。我习惯用成本变化因为它最直观。如果成本下降量小于1e-6就认为收敛了。但要注意成本偶尔会陷入非常平的区域看似收敛但轨迹离目标还很远。这时候需要检查终末成本是否足够大或者增大 Qf 对目标状态施加更严厉惩罚。常见失败模式有三种成本爆炸多是因为数值不稳定或正则失效。加大正则项检查动力学雅可比是否算错。轨迹来回震荡可能是反馈增益算错了符号。尤其是 K 矩阵维度转置错误会导致前向模拟里反馈控制变成正反馈。卡在局部极小值iLQR本质上是局部优化复杂环境中很容易陷入局部极值。解决方案是多随机初始化几个参考轨迹选成本最低的或者加入随机噪声扰动后重新优化。5. 我踩过几次坑之后的一些体会说实话把iLQR代码写出来并不难难的是让它稳定地跑在真实系统上。我最开始照着论文公式抄了一遍结果后向传递里 Vx 和 Vxx 的更新漏了交叉项导致算法虽然能跑但收敛特别慢。后来把每一步的Q系数手动推到2维例子下逐项验证才真正理解每个矩阵的维度到底是怎么回事。另一个经验是别迷信“全自动调参”。Q_cost、R、Qf、dt、N、正则项、线搜索参数每一个都互相影响。比如同样的成本矩阵把 N 从100改成300轨迹长度变了控制惩罚的“力度”也会变。强烈建议先用一个简单的仿真环境把整个迭代过程的成本曲线打印出来再慢慢体会每个超参数的作用。如果你想继续深挖下一步可以试试把iLQR接成MPC每步只执行第一个控制然后重新用当前状态做初始轨迹滚动优化也可以尝试把动力学雅可比从数值差分改成解析推导速度会快非常多。总之iLQR这个工具箱无论是做控制还是做规划都值得你在自己的项目里亲手实现一次。