Part I · 第 6 章优化 · 6.7

约束优化：拉格朗日与 KKT

现实中的问题几乎都带"不许越界"的规矩：关节不能超限，力矩不能超过电机能力，末端不能穿过障碍物。有了约束，最优点常常被推到边界上，那里梯度不再为零。拉格朗日乘子给出了新的最优性条件：在最优点，代价的梯度被约束的"墙"恰好抵消。乘子本身还有一个物理意义：放松这堵墙一点点，能换来多少好处，也就是约束的影子价格。

约 30 分钟前置：6.1 什么是优化问题 · 6.2 凸性 · 3.2 梯度 · 1.9 投影 5 个交互

1为什么机器人需要它

没有约束的机器人问题几乎不存在。机械臂 IK 要满足关节限位，腿式机器人的脚底力要落在摩擦锥内，无人机的推力有上下限，轨迹优化要避开障碍并满足动力学。把这些规矩全部"罚"进代价里往往不可靠（6.1 节误区 3），要认真处理，就需要约束优化的理论。

这一节建立两件事：最优点处长什么样（KKT 条件），以及最优点处的乘子意味着什么（哪些约束在起作用，它们各"出了多少力"）。这是后面 6.8 节二次规划、整个模型预测控制和全身控制（第 IV 部分）的共同语言：这些求解器输出的不只是最优解，还有每个约束的乘子，工程师用它来诊断"到底是哪条限制拖了后腿"。

2直觉：山路上的栏杆

你沿着一条山路（约束）走，想尽量下到最低处。在路上的每一点，山坡想把你往下拉（$-\nabla f$），但你只能沿着路走，路的"法线"方向上的拉力被路本身抵消了。如果某一点上，山坡的拉力恰好完全垂直于路，也就是和路的法向平行，那你沿着路往任何方向走都不再下降：这就是路上的最低点（或最高点，或鞍）。

这就是 Lagrange 条件：$\nabla f=-\nu\,\nabla h$，代价的梯度与约束的梯度平行，比例系数 $\nu$ 就是乘子。如果约束是"栏杆"（不等式，只能待在一侧），那么栏杆只能往里顶，不能往外拉：乘子必须非负；而没有碰到栏杆时，栏杆不出力，乘子为零。这两条加起来，就是 KKT 条件。

3动手玩：沿着约束曲线走

代价 $f$ 的等高线是青色椭圆，中心 $\mathbf c$ 可拖动（$f=\tfrac12[(x-c_1)^2+3(y-c_2)^2]$）。约束是紫色曲线 $h(x,y)=0$，可行点只能在曲线上。拖动曲线上的黄点：黄色箭头是 $\hl{gradf}{\nabla f}$，紫色箭头是 $\hl{gradh}{\nabla h}$（悬停在符号上，对应箭头会加粗）。右图画出 $f$ 沿曲线的变化。你会发现，只在两个箭头共线的地方，右图的曲线才有极值。

FIG 1等式约束：梯度平行

圆：$x^2+y^2=1.4^2$；直线：$0.8x+1.2y=1.2$；抛物线：$y=0.35x^2-0.6$。乘子 $\nu$ 取使 $\nabla f+\nu\nabla h$ 最小的值（最小二乘投影）$\nu=-\nabla f\cdot\nabla h/\|\nabla h\|^2$，残差 $\|\nabla f+\nu\nabla h\|$ 为 0 才是驻点。右图的 ● 是沿约束的局部最小，▲ 是局部最大，两者都满足 Lagrange 条件。

试一试

把黄点拖到两个箭头平行的位置（读数里的"夹角"接近 0° 或 180°）：此处右图的曲线是平的。
找到一个 ▲（沿约束的最大点）并停在上面：Lagrange 条件同样成立。条件只说"驻点"，不区分最小和最大。
在"圆"上，把 $\mathbf c$ 拖到圆外，再拖到圆内，观察最小点处乘子 $\nu$ 的符号怎样变化。

核心洞见

约束优化的最优性条件不是"$\nabla f=0$"，而是"$\nabla f$ 与约束的法向平行"，也就是$\nabla f$ 在约束曲线的切线方向上的分量为零。梯度的"切向分量为零，法向分量由约束承担"，这个法向分量的大小就是乘子 $\nu$。

4不等式约束：KKT 的四条

现在改成不等式：$\min\ f=\tfrac12\|\x-\mathbf c\|^2$，约束 $g_1=\|\x\|^2-r^2\le0$（圆盘）和 $g_2=\mathbf n^\top\x-b\le0$（半平面）。$f$ 取最小的地方就是离 $\mathbf c$ 最近的可行点，也就是 $\mathbf c$ 到可行区域的投影（1.9 节）。程序用 Dykstra 交替投影算法求这个点（每个约束的投影都有闭式），再从最优点反求乘子，逐条检查 KKT：

$$ \begin{aligned} \text{stationarity:}&\ \ \nabla f(\x^\star)+\hl{lam}{\textstyle\sum_i\lambda_i}\nabla g_i(\x^\star)=\mathbf 0 &\qquad \text{primal feasibility:}&\ \ g_i(\x^\star)\le0\\ \text{dual feasibility:}&\ \ \lambda_i\ge0 &\qquad \text{complementary slackness:}&\ \ \lambda_i\,g_i(\x^\star)=0\end{aligned} $$

FIG 2KKT 检查表与"力的平衡"

拖动目标 $\mathbf c$（青色十字）。红色区域不可行。琥珀色箭头是 $-\nabla f(\x^\star)$，即代价想把你拉去的方向；紫色箭头是各个起作用约束分担的部分 $\lambda_i\nabla g_i$。两者平衡（紫色之和等于琥珀色），就是平稳性。滑块改变圆盘半径、半平面的偏移和方向；按钮开关每个约束。

试一试

把 $\mathbf c$ 放进可行区域：没有约束起作用，所有 $\lambda_i=0$，$\x^\star=\mathbf c$，就是无约束问题。
把 $\mathbf c$ 放到只有一个约束起作用的位置：那个约束的 $\lambda>0$，另一个 $\lambda=0$（互补松弛）。
把 $\mathbf c$ 放到圆盘和半平面的交角外侧：两个约束同时起作用，两个乘子都为正，紫色箭头合成琥珀色箭头。

5乘子的意义：影子价格

把约束写成 $g(\x)\le s$，$s$ 是"预算"。最优值 $f^\star(s)$ 是预算的函数。乘子 $\lambda^\star$ 就是它的斜率的相反数：

$$ \frac{\mathrm d f^\star}{\mathrm d s}=\hl{shadow}{-\lambda^\star}\qquad\Longrightarrow\qquad f^\star(s+\delta)\approx f^\star(s)-\lambda^\star\,\delta $$

于是 $\lambda$ 是约束的影子价格：多给一单位预算，最优值下降 $\lambda$。$\lambda$ 大，说明这堵墙正顶得很紧，放松它收益大；$\lambda=0$，说明墙根本没用上，放松它一点好处也没有。下图用圆盘约束 $\|\x\|^2\le s$（$s$ 是半径平方）：滑动 $s$，右图是用同一求解器逐点算出的 $f^\star(s)$ 曲线，紫色切线的斜率就是 $-\lambda$；程序同时用有限差分验证。

FIG 3最优值对预算的灵敏度

把 $s$ 调到 0.5 和 2 两个值：$s$ 越小，约束越紧，$\lambda$（曲线的陡度）越大。有限差分和 $-\lambda$ 是否吻合？
把 $s$ 调大到 $\|\mathbf c\|^2$ 以上：$\lambda$ 变成 0。多出来的预算一文不值。

6预告：把约束变成"软"的：罚函数与障碍函数

怎样用 6.3–6.6 节的无约束求解器去解约束问题？一个自然的想法：把约束放进代价里。

二次罚函数：$f(\x)+\tfrac\mu2\max(0,g(\x))^2$。越界要付代价，$\mu$ 越大罚得越重。最小点在可行域外面，随 $\mu\to\infty$ 从外侧逼近真解。
对数障碍函数：$f(\x)-\tfrac1t\log(-g(\x))$。越靠近边界，代价趋向无穷，像一堵无形的墙把你关在里面。最小点在可行域内部，随 $t\to\infty$ 从内侧逼近真解（内点法）。

两条路径（对每个 $\mu$ 或 $t$ 求得的最小点）合起来像一道括号，夹着真正的约束最优点。更妙的是，乘子可以从它们读出来：罚函数里 $\lambda\approx\mu\max(0,g)$，障碍函数里 $\lambda\approx-1/(t\,g)$，两者都收敛到 KKT 乘子 $\lambda^\star$。拖动 $\mu$（障碍函数用 $t=\mu$），每个点都是用阻尼 Newton 法（6.6 节）真实求出的无约束最小点。

FIG 4罚函数与障碍函数的"路径"

问题：$\min\tfrac12\|\x-\mathbf c\|^2$ s.t. $\|\x\|^2\le1$，$\mathbf c$ 可拖动（在圆外）。琥珀色：罚函数路径与当前 $\mu$ 的最小点；青色：障碍函数路径与当前 $t$ 的最小点；标着"x⋆ (KKT)"的中性色圆点是 KKT 解。右图：两种方法估出的乘子随 $\mu$（$t$）的变化，虚线是真实的 $\lambda^\star$。

把 $\mu$ 调到约 10：罚函数的最小点在圆外（$g>0$），障碍函数的在圆内（$g<0$）。
把 $\mu$ 拉到 $10^3$ 以上：两种方法估出的乘子都与 $\lambda^\star$ 相差不到 1%。

7再看数学

提示：悬停在带底色的符号上，FIG 2 和 FIG 3 中对应的元素（乘子、影子价格）会被强调。

拉格朗日函数与等式约束

对 $\min f(\x)$ s.t. $h_j(\x)=0,\ g_i(\x)\le0$，定义拉格朗日函数

$$ \mathcal L(\x,\boldsymbol\lambda,\boldsymbol\nu)=f(\x)+\sum_i\lambda_i\,g_i(\x)+\sum_j\nu_j\,h_j(\x) $$

只有等式约束时。设 $\x^\star$ 是局部最优且各 $\nabla h_j$ 线性无关（LICQ）。沿约束的任一切向 $\mathbf d$（$\nabla h_j^\top\mathbf d=0\ \forall j$）走，$f$ 一阶不变，即 $\nabla f^\top\mathbf d=0$。所有与 $\nabla h_j$ 垂直的 $\mathbf d$ 都与 $\nabla f$ 垂直，意味着 $\nabla f$ 属于 $\nabla h_j$ 张成的空间（1.9 节的正交补）：

$$ \nabla f(\x^\star)+\sum_j\nu_j\nabla h_j(\x^\star)=\mathbf 0,\qquad h_j(\x^\star)=0\qquad\Longleftrightarrow\qquad\nabla_{\x}\mathcal L=\mathbf 0,\ \ \nabla_{\boldsymbol\nu}\mathcal L=\mathbf 0 $$

这是 $n+p$ 个方程、$n+p$ 个未知数 $(\x,\boldsymbol\nu)$ 的方程组。要点：它是驻点条件，所以最大点和鞍点也满足（FIG 1 的 ▲）；区分它们需要二阶条件：拉格朗日函数的 Hessian 在约束切空间上正定。例：$\min x^2+y^2$ s.t. $x+y=2$：$2x+\nu=0,\ 2y+\nu=0\Rightarrow x=y$，代入约束得 $x=y=1$，$\nu=-2$，$f^\star=2$。

KKT 条件

$$ \begin{cases}\nabla f(\x^\star)+\sum_i\lambda_i\nabla g_i(\x^\star)+\sum_j\nu_j\nabla h_j(\x^\star)=\mathbf 0 & \text{(stationarity)}\\ g_i(\x^\star)\le0,\quad h_j(\x^\star)=0 & \text{(primal feasibility)}\\ \lambda_i\ge0 & \text{(dual feasibility)}\\ \lambda_i\,g_i(\x^\star)=0 & \text{(complementary slackness)}\end{cases} $$

为什么 $\lambda_i\ge0$：在起作用的不等式约束处，可行方向是 $\nabla g_i^\top\mathbf d\le0$（朝内或沿边界）。最优时没有可行的下降方向，即不存在 $\mathbf d$ 使 $\nabla f^\top\mathbf d<0$ 同时 $\nabla g_i^\top\mathbf d\le0$。由 Farkas 引理，这等价于 $-\nabla f$ 是 $\nabla g_i$ 的非负组合，系数就是 $\lambda_i\ge0$。几何上：墙只能往里顶，不能往外拉。
互补松弛："要么约束起作用（$g_i=0$），要么乘子为零"。没碰到的墙不出力。起作用的那些下标集合叫有效集。
什么时候充分：KKT 在 LICQ 等约束规格下是局部最优的必要条件；若问题是凸的（$f,g_i$ 凸，$h_j$ 仿射，6.2 节），KKT 点就是全局最优。FIG 2 的问题是凸的，所以检查表全绿就等于证明了最优。

影子价格的来由：把约束扰动为 $g_i(\x)\le s_i$，最优值 $f^\star(\mathbf s)$ 满足 $\partial f^\star/\partial s_i=-\lambda_i^\star$。由包络定理：最优点处拉格朗日函数对参数的变化率，等于直接对参数求偏导，而 $\partial\mathcal L/\partial s_i=-\lambda_i$。

求解思路概览。(1) 有效集法：猜一个有效集，把其中的约束当等式解 Lagrange 方程，检查乘子符号与可行性，再增减约束（6.8 节的单纯形与二次规划）。(2) 投影/可行方向法：约束简单（盒子、球）时，每步梯度下降后投影回可行域，FIG 5 用的就是这个。(3) 罚函数与增广拉格朗日：FIG 4；罚参数要趋于无穷，Hessian 条件数随之爆炸，增广拉格朗日法同时更新乘子，$\mu$ 不必太大。(4) 内点法：沿障碍函数的中心路径走，是现代 QP/SOCP 求解器的主流。

8机器人中的它：带关节限位的 IK

两连杆臂的 IK：$\min\tfrac12\|\mathrm{FK}(\mathbf q)-\mathbf t\|^2$，约束是关节限位 $\mathbf q_{\min}\le\mathbf q\le\mathbf q_{\max}$。限位是简单的"盒子"约束，所以用投影梯度法：每步 $\mathbf q\leftarrow\mathrm{clamp}\big(\mathbf q-\alpha\nabla f\big)$，其中 $\alpha$ 由 Armijo 回溯确定（6.5 节）。收敛后读出每条限位的 KKT 乘子：若某个关节被"顶"在限位上，对应乘子就是 $\partial f/\partial q_i$ 的大小（下限处为 $+\partial_if$，上限处为 $-\partial_if$）。

目标远在工作空间外缘附近时，手臂需要几乎伸直，所需的肘角 $q_2$ 小于允许的下限。优化器无法做到，只能停在 $q_2=q_{2,\min}$ 的边界上，剩下的误差是约束的"代价"。$q_2$ 下限的乘子告诉你：把下限放松 1 rad，误差平方的一半能降低多少。下面拖动目标，调节 $q_2$ 的下限，对照乘子预测的下降量与真正重新求解的结果。

FIG 5关节限位的 KKT

左：工作空间（拖动目标）；虚线紫色手臂是无约束的两个解析解。右：关节空间，带框线、未涂红的矩形是可行盒子（限位），盒子外涂成红色，路径是投影梯度法的迭代。读数：每条限位的状态与乘子，以及"放松 $q_2$ 下限 0.05 rad"的预测下降（$\lambda\times0.05$）和重新求解得到的真实下降。

让某个限位起作用（乘子 $>0$，路径撞上了盒子的边）。哪个关节被顶住？
在有一个限位起作用时，把 $q_2$ 下限调低一点：误差下降的量与"预测"一致吗？（乘子是一阶近似，放松量小时很准。）

真实系统里

机器人栈里约束处处都是：MoveIt 与 TRAC-IK 的关节限位（盒约束，用投影或"夹紧+重启"）；全身控制里的摩擦锥、力矩上限、接触不穿透（一个 QP，每毫秒用有效集或内点法求解，6.8 节与第 IV 部分）；模型预测控制里的状态与输入约束；轨迹优化里的避障（SQP 把非线性约束逐步线性化）。实践中还会读乘子：全身控制里"哪个力矩限位被顶住"、MPC 里"哪条约束在限制速度"，乘子就是答案，也是做约束松弛、对偶分解和灵敏度分析的出发点。

9常见误区

误区 1：乘子的正负号是随便的

对不等式约束，符号是有意义的：按 $g_i\le0$、$\mathcal L=f+\sum\lambda_ig_i$ 的惯例，必须 $\lambda_i\ge0$，一旦算出负的乘子，说明这条约束其实不该在有效集里（放松它能让 $f$ 更低）。换成 $g_i\ge0$ 或 $\mathcal L=f-\sum\lambda_ig_i$ 的写法，符号约定会翻转，读手册时先确认惯例。对等式约束，$\nu$ 的符号则取决于你把 $h$ 写成 $a-b$ 还是 $b-a$，没有内在含义。

误区 2：Lagrange 条件找到的就是最小点

它只给出驻点（FIG 1：最小和最大都满足）。非凸问题里 KKT 点可能是局部最小、鞍点甚至最大；只有凸问题里，KKT 点才一定是全局最优。找到之后要用二阶条件或比较函数值来区分。

误区 3：约束写成等价形式，问题就等价

$\|\x\|\le1$ 与 $\|\x\|^2\le1$ 在可行集上完全相同，但前者在 $\x=\mathbf 0$ 处不可微，后者光滑；$h(\x)=0$ 与 $h(\x)^2=0$ 的可行集相同，但后者在可行点处梯度为零，LICQ 失效，KKT 条件不再适用。写约束时要选光滑、梯度不退化的形式。乘子的数值也随写法缩放（$2\lambda$ 对 $\lambda$）。

误区 4：罚参数设得越大越好

罚函数的最小点只有在 $\mu\to\infty$ 时才满足约束，但 $\mu$ 大了，代价函数的 Hessian 条件数正比于 $\mu$，6.3 节的梯度下降会走"之"字，甚至数值上做不下去。实践中从小的 $\mu$ 开始，逐步增大，每次用上一次的解热启动（FIG 4 就是这个路径）；或者直接用增广拉格朗日法和内点法。

10小练习

1. 求 $\min x^2+y^2$ s.t. $x+y=2$ 的最优解（用 Lagrange 条件）。

$\nabla f=(2x,2y)$，$\nabla h=(1,1)$。平行条件 $2x=2y$，所以 $x=y$，代入 $x+y=2$ 得 $(1,1)$，$f^\star=2$。几何：以原点为中心的圆（等高线）与直线相切的点就是离原点最近的点。$(0,0)$ 不可行；$(2,0)$ 与 $(0,2)$ 可行但 $f=4$，更大。

2. $\min(x-3)^2$ s.t. $x\le1$（即 $g=x-1\le0$）。最优点处的 KKT 乘子 $\lambda$ 是多少？

最优点是 $x^\star=1$（约束起作用）。平稳性 $f'(x^\star)+\lambda g'(x^\star)=2(1-3)+\lambda\cdot1=0\Rightarrow\lambda=4$。$\lambda\ge0$ 也满足。这与 6.1 节小练习的结果一致：$f'=-4\ne0$，被约束的"力" $\lambda\nabla g=4$ 抵消。$\lambda=-4$ 违反对偶可行性。

3. 接上题：把约束放松为 $x\le1.1$，最优值大约改变多少？

影子价格：$f^\star(s+\delta)\approx f^\star(s)-\lambda\delta=4-0.4=3.6$。精确值：新的最优点 $x^\star=1.1$，$f^\star=(1.1-3)^2=3.61$，下降 0.39，吻合。放松约束，可行域变大，最优值不会变差，所以必然是下降或不变。$x^\star$ 会跟着墙移动到 1.1，不是不动。

4. 在最优点处，某个不等式约束 $g_i(\x^\star)=-0.5<0$（没有碰到边界）。KKT 的互补松弛说明什么？

$\lambda_i g_i=0$ 且 $g_i\ne0$，所以 $\lambda_i=0$。该约束对平稳性方程没有任何贡献，等价于把它删掉（FIG 2 中 $\mathbf c$ 在可行域内时所有 $\lambda=0$）。

5. 二次罚函数法与对数障碍法，当参数趋于极限时，各自的最小点相对可行域在哪一侧？

罚函数只在越界时才有惩罚，所以最小点愿意稍微越界，用一点违反换代价下降；$\mu$ 越大越贴近边界。障碍函数在边界处趋于无穷，最小点永远在内部。因此内点法的每一步都严格可行，罚函数法的中间结果可能略微违反约束，这在安全攸关的场合要注意（FIG 4）。

11延伸阅读

S. Boyd & L. Vandenberghe, Convex Optimization, Ch. 5 (Lagrange duality, KKT, sensitivity analysis) & Ch. 11 (interior-point methods) — KKT、影子价格与障碍法的权威讲解，有大量几何图示
J. Nocedal & S. Wright, Numerical Optimization, 2nd ed., Ch. 12 (theory of constrained optimization), Ch. 17 (penalty, augmented Lagrangian) & Ch. 19 (interior-point) — 非凸情形的一阶与二阶最优性条件、约束规格（LICQ）与算法
D. P. Bertsekas, Nonlinear Programming, 3rd ed., Athena Scientific — 拉格朗日乘子与增广拉格朗日法的经典专著
K. Lynch & F. Park, Modern Robotics, Ch. 6 & Appendix — 带限位的数值逆运动学与约束优化的机器人视角