Part I · 第 6 章 优化 · 6.1

什么是优化问题

机器人几乎所有的"智能"，最后都能写成同一句话：在所有可选的方案里，挑一个"最好"的，而且不许违反规矩。"方案"叫决策变量，"最好"由代价函数衡量，"规矩"是约束。把这三样东西说清楚，问题就有了形状：一片起伏的地形，而你要找最低的那个谷底。

约 20 分钟 前置：1.1 向量 · 2.1 导数 · 3.1 偏导 4 个交互

    1为什么机器人需要它

    想一想机器人每天要回答的问题：

    • 手爪要碰到杯子，每个关节该转到多少度？（逆运动学）
    • 相机装歪了一点点，外参到底是多少，才能让点云和地图对齐？（标定）
    • 从 A 到 B 有无数条路径，走哪一条最短、最平滑、又不会撞到障碍物？（轨迹规划）
    • 神经网络有上亿个参数，取什么值，才能让机器人在示范数据上的动作误差最小？（学习）

    这四个问题长得完全不同，骨架却一样：有一组可以调的数，有一个衡量好坏的数，还有一些不能越界的规矩。一旦写成这个形式，你就可以用同一套工具去解它们，这正是本章（第 6 章）要教的全部内容。这一节先把"问题长什么样"讲清楚；怎么解，从 6.3 节开始。

    2直觉：地形与小球

    把决策变量 \(\x\) 想成你在地图上的位置，把代价 \(f(\x)\) 想成这个位置的海拔。优化就是找海拔最低的地方。\(\x\) 有几个分量，地图就有几个维度：两个关节角是一张二维地图；上亿个神经网络参数是一张上亿维的地图，画不出来，但"起伏的地形"这个比喻仍然成立。

    地形好不好走，取决于它有几个谷底。只有一个谷底的地形，把小球扔在哪里，它都滚到同一处。但机器人的代价函数常常有好几个谷：滚到哪个，取决于你从哪里出发。下面这张地形有四个谷，其中只有一个是全局最低（全局最小值），其余的只是"附近最低"（局部最小值）。

    3动手玩：把球放在哪里？

    点击地图（或拖动空心圆）放下小球，它会沿着最陡的下坡方向滚（这就是 6.3 节要讲的梯度下降，这里先当黑盒用）。打开"盆地图"，每个颜色格子表示：从这一格出发，最终会滚进哪个谷。

    FIG 1四个谷的地形
    各个谷底（按海拔排序）
    底色和等高线表示海拔 \(f\)：青色越浓，地势越低。四个圆点是用梯度下降从各个谷心附近精确求出的谷底。小球的每一步都是真实的梯度下降迭代 \(\x\leftarrow\x-0.2\,\nabla f(\x)\)，不是预先画好的路径。盆地图是对 80×55 个起点逐个做梯度下降的结果。
    试一试
    • 让小球滚进全局最低的谷（右侧列表第 1 项）。
    • 让小球掉进一个局部最小值，困在那里出不来：算法并没有出错，它只是不知道别处还有更低的谷。
    • 打开"盆地图"，把起点在两个颜色的交界处来回拖动：相隔不到 0.5 的两个起点，终点可以完全不同。

    注意：算法停下的地方，只是"梯度为零"的地方，它自己无法判断这是全局还是局部最低。想知道，只能换几个起点重来，比较结果。这就是机器人里"多起点重启"（multi-start）这种朴素方法的来历。

    核心洞见

    优化器给你的是"从这个起点出发能走到的最近的谷底"，不是"全世界最好的答案"。所以一个优化问题的难度，一半在于怎么走，一半在于地形本身有多少个谷。下一节（6.2 凸性）会讲：什么样的地形保证只有一个谷。

    4三个要素：变量、代价、约束

    现在把问题拆成三件事。决策变量（黄色的"你的猜测"）是你能动的东西，它决定了你站在哪里。代价函数（青色的等高线）给每个位置打分，分越低越好，它的最低点（目标 \(\mathbf c\)）是"如果没有任何限制，你最想去的地方"。约束（划掉的红色区域）规定哪些位置根本不能去。

    有了约束，最优解常常不在代价最低的地方，而是被推到可行区域的边界上。下图里，拖动目标 \(\mathbf c\)，看最优点 \(\x^\star\)（紫色）怎样被约束拦住；再拖动黄色的猜测点，亲自当一回优化器。

    FIG 2可行区域与最优点
    代价 \(f(\x)=\tfrac12(\x-\mathbf c)^\top W(\x-\mathbf c)\)，\(W=\mathrm{diag}(1,2.5)\)，所以等高线是椭圆。最优点 \(\x^\star\) 由程序在整个可行区域上逐点搜索再局部细化得到（这里只有两个变量，可以暴力搜；变量一多就只能靠 6.3 起的迭代算法）。"障碍物"模式的可行区域有个洞，它不是凸集（6.2）。
    试一试
    • 把目标 \(\mathbf c\) 拖到不可行区域里，观察 \(\x^\star\) 落在边界上，而不是 \(\mathbf c\) 处。
    • 目标 \(\mathbf c\) 留在不可行区域（例如默认位置）时，不看紫色圆圈，把黄色猜测点拖到可行且代价与最优值相差不到 0.03 的位置。
    • 选"障碍物"，把 \(\mathbf c\) 放在障碍物正中，看最优点跳到障碍物的哪一侧。再微微挪动 \(\mathbf c\)，它会不会突然跳到另一侧？

    5例子：把圆拟合到测量点上

    标定问题是最典型的"代价 = 误差平方和"。设想一个激光雷达只扫到了一个圆柱的一段弧，你想知道圆柱的圆心和半径。决策变量是三个数 \(\boldsymbol\theta=(c_x,c_y,r)\)；代价是每个测量点到圆的距离的平方和：

    $$ f(\boldsymbol\theta)=\tfrac12\sum_{i=1}^{N}\Big(\big\|\mathbf p_i-\mathbf c\big\|-r\Big)^2 $$

    红色短线就是每个点的残差。先自己拖动圆心和半径手柄，把代价压低；再按"优化"，让梯度下降（带回溯线搜索，6.5）接手。最后打开"离群点"，看两个错误测量如何把答案拉歪：代价函数本身，决定了"最好"是什么意思。

    FIG 3圆拟合：三个决策变量
    代价随迭代（对数坐标）
    空心圆手柄：圆心；方块手柄：半径。虚线圆是当前的 \(\boldsymbol\theta\)，实线圆是不带离群点时的最优拟合（程序预先算好的参考）。每次迭代用 \(\nabla f\) 做下降，步长由 Armijo 回溯确定。
    • 不用优化器，只靠拖动手柄，把代价压到最优值附近（差不到 0.05）。
    • 按"优化"，看代价在几十步里降到最低（梯度小于 1e-6）。
    • 打开"含离群点"，再优化：新的最优圆和干净数据的最优圆差得多远？

    6再看数学

    提示：悬停在公式中带底色的符号上，FIG 2 中对应的部分（代价、约束、猜测点、最优点）会被高亮。

    一个优化问题的标准写法是：

    $$ \min_{\hl{var}{\cv{\x}}\in\R^n}\ \ \hl{cost}{\cx{f(\x)}}\qquad\text{s.t.}\qquad \hl{constr}{g_i(\x)\le 0,\ \ i=1,\dots,m,\qquad h_j(\x)=0,\ \ j=1,\dots,p} $$
    • \(\x\in\R^n\)：决策变量（也叫优化变量、参数）。机器人里可以是关节角、位姿、路径点、网络权重。
    • \(f:\R^n\to\R\)：代价函数（目标函数、损失）。必须是一个实数；多个目标要先加权合成一个，如 \(f=f_{\text{误差}}+\lambda f_{\text{能耗}}\)。
    • \(g_i(\x)\le0\)、\(h_j(\x)=0\)：不等式约束与等式约束。满足全部约束的 \(\x\) 组成可行集 \(\Omega\)。没有约束时 \(\Omega=\R^n\)，称为无约束优化。

    最大化只是换个符号：\(\max f\) 等价于 \(\min(-f)\)，所以只需要研究最小化。

    什么叫"最小值"

    $$ \hl{opt}{\cw{\x^\star}}\ \text{is a }\textbf{global}\text{ minimizer if } f(\cw{\x^\star})\le f(\x)\ \ \forall\,\x\in\Omega;\qquad \textbf{local}\text{ if this holds for all }\x\in\Omega\text{ with }\|\x-\cw{\x^\star}\|<\varepsilon $$

    在没有约束、\(f\) 光滑的情形，局部最小值一定满足一阶必要条件 \(\nabla f(\x^\star)=\mathbf 0\)（3.2 节：梯度为零，谷底不倾斜），以及二阶必要条件 \(\nabla^2 f(\x^\star)\succeq0\)（3.5 节：向各个方向都是"向上弯"的）。注意这只是必要条件：梯度为零的点还可能是山顶或鞍点。有了约束，最优点处梯度一般不再为零，条件变成 6.7 节的 KKT 条件。

    机器人问题决策变量 \(\x\)代价 \(f\)约束
    逆运动学关节角 \(\mathbf q\)\(\tfrac12\|\mathrm{FK}(\mathbf q)-\mathbf t\|^2\)关节限位，避碰
    标定外参、内参、偏置重投影误差平方和旋转矩阵须正交
    轨迹优化各时刻的路径点 / 控制量平滑度 + 能耗 + 到达误差动力学、避障、力矩上限
    策略学习网络权重 \(\mathbf w\)动作预测误差通常无约束

    7机器人中的它：逆运动学的"关节空间地形"

    两连杆机械臂，连杆长 1.0 和 0.8。把手爪送到目标 \(\mathbf t\)，就是最小化 \(f(\mathbf q)=\tfrac12\|\mathrm{FK}(\mathbf q)-\mathbf t\|^2\)。决策变量是两个关节角，所以代价是一张二维地形，右图就是它。横轴 \(q_1\)，纵轴 \(q_2\)，都在 \(-\pi\) 到 \(\pi\) 之间。

    你会看到两个谷底：一个是"肘朝上"，一个是"肘朝下"，手爪都碰到了目标，代价同样为零。它们都是全局最小值，而且别忘了地形是周期的：\(q_1=\pi\) 的右边缘和 \(-\pi\) 的左边缘其实是同一个位置（后面的 6.10 节会把这叫做"流形"）。拖动右图的黄色手柄选择起点，按"下降"，看机械臂最终选了哪一种肘姿态。

    FIG 4逆运动学：工作空间与关节空间
    左：工作空间，拖动黄色目标；紫色虚线是两个解析解对应的姿态。右：关节空间里的代价地形，圆点是两个全局最小值，拖动手柄选起点。"下降"是真实的梯度下降 \(\mathbf q\leftarrow\mathbf q-0.3\nabla f\)，其中 \(\nabla f=J^\top\mathbf e\)（3.3、3.4 节）。目标越出臂展（半径 1.8）时两个谷合并成一个，且最低点高于 0。
    • 固定一个目标，分别从两个不同的起点下降，让机械臂各落到肘朝上和肘朝下两种姿态。
    • 把目标拖出臂展（虚线圆之外），看两个谷如何合并、最低点为何不再是 0。
    真实系统里

    几乎所有数值 IK 求解器都是"迭代 + 从上一个姿态出发"：控制循环每个周期都把上一周期的解当作起点（热启动），这样机械臂总在同一个谷里连续运动，不会突然从肘朝上翻到肘朝下。TRAC-IK 等库还会在求解失败时随机重启：这就是前面的"多起点"。7 自由度机械臂有无穷多个解（零空间），代价地形里的谷底变成一整条同样低的"谷底曲线"，6.7 节的约束和 6.8 节的二次规划会处理这种冗余。

    8常见误区

    误区 1：优化器找到的就是"最好的"

    它只保证（在好的情形下）找到一个局部最小值。除非问题是凸的（6.2 节），否则要用多起点、热启动或全局方法去弥补。拿到结果后，至少检查它是不是你想要的那个"姿态"。

    误区 2：优化器解的是你写下的代价，而不是你心里想的

    FIG 3 的离群点就是例子：平方误差对大残差格外敏感，两个坏点就把整个圆拽歪了。代价函数是对"好"的数学翻译，翻译错了，优化得越成功，结果越离谱。强化学习里的"奖励黑客"（reward hacking）是同一件事。

    误区 3：约束可以当成"代价"随便处理

    给越界加一个惩罚项确实常用（6.7 节会讲），但它给的是"尽量别越界"，而不是"绝不越界"。关节限位、力矩上限这类硬约束，用惩罚项可能在极端情况下被突破；需要保证时，要用真正的约束优化方法。

    误区 4：忘了角度是"绕圈"的

    关节角 \(\pi\) 和 \(-\pi\) 是同一个姿态，但在数值上相差 \(2\pi\)。若直接把角度当普通实数做差，代价会在 \(\pm\pi\) 处出现假的"悬崖"。旋转矩阵、四元数等姿态的优化更需要小心：它们不是普通向量空间，这是 6.10 节"流形上的优化"的出发点。

    9小练习

    1. 在"数值逆运动学"这个优化问题里，决策变量是什么？
    决策变量是你可以调节、优化器要去寻找的东西：关节角 \(\mathbf q\)。目标 \(\mathbf t\) 和连杆长度是问题的已知数据；末端与目标的距离是（取平方后的）代价函数的值。
    2. 求解 \(\min (x-3)^2\) s.t. \(x\le 1\)，最优解和最优值是多少？此时梯度为零吗？
    \(x=3\) 不可行。在 \(x\le1\) 上，\((x-3)^2\) 越靠近 1 越小，所以 \(x^\star=1\)，值为 \((1-3)^2=4\)。此处 \(f'(1)=2(1-3)=-4\neq0\)：约束把最优点推到了边界，梯度不再为零。这正是 6.7 节要处理的情形：梯度被约束的"力"抵消，而不是自己消失。
    3. 梯度下降停在了一个梯度为零的点，代价为 0.8。你怎样才能知道它是不是全局最小值？
    梯度为零只是局部最小值、局部最大值或鞍点的共同特征。局部信息（梯度、Hessian）只能证明"附近最低"。若问题是凸的（6.2 节），局部最小就是全局最小；否则要靠多起点重启等办法，并接受"只是目前找到的最好答案"。
    4. 一个目标在臂展之内的两连杆平面机械臂，其 IK 代价 \(\tfrac12\|\mathrm{FK}(\mathbf q)-\mathbf t\|^2\) 在 \((-\pi,\pi]^2\) 内有几个全局最小值？为什么？
    由余弦定理，\(\cos q_2=(\|\mathbf t\|^2-L_1^2-L_2^2)/(2L_1L_2)\) 有 \(q_2=\pm\arccos(\cdot)\) 两个解，每个对应一个 \(q_1\)。只有当目标恰好在臂展边界（手臂伸直）或最小可达半径处时，两个解才合并为一个；目标在臂展之外则无零代价解。三连杆（冗余）臂才有无穷多个解。

    10延伸阅读

    • S. Boyd & L. Vandenberghe, Convex Optimization, Ch. 1 & 4.1 — 优化问题的标准形式、可行集与最优性的定义（可免费下载）
    • J. Nocedal & S. Wright, Numerical Optimization, 2nd ed., Ch. 1–2 — 数学形式、局部与全局解、无约束最优性条件
    • K. Lynch & F. Park, Modern Robotics, Ch. 6.2 — 用牛顿类迭代求解数值逆运动学