机器人中的优化：让约束决定运动

前面的文章已经分别回答了许多问题：

• 逆运动学寻找能够到达目标的关节构型；
• 轨迹生成把起点与终点连接成连续运动；
• 拉格朗日动力学描述力矩怎样产生加速度；
• 约束雅可比与拉格朗日乘子描述闭环和接触力；
• 计算力矩与阻抗控制让机器人跟踪轨迹并与环境交互。

这些方法看起来分散在运动学、规划和控制的不同章节里，背后却共享同一种语言：

$$\begin{aligned} \min_z\quad & \mathcal J(z)\\ \text{s.t.}\quad & g(z)=0\\ & h(z)\le0 \end{aligned}$$

$z$ 是机器人可以决定的量，$\mathcal J$ 衡量不同可行方案的优劣，$g$ 与 $h$ 则规定物理、几何和安全边界。

优化最有力量的地方，正是这三者的分工：

约束定义机器人能够怎样运动，代价只在可行运动中选择更合适的一条。

为了让这条主线落到具体任务上，本文始终考虑同一个例子：一台七自由度机械臂端着一杯水，要把杯子穿过货架开口并放到指定位置。杯口需要保持朝上，机械臂不能碰到货架，关节不能越限，运动还要平稳；如果任务在线执行，机器人还要根据新的测量不断修正后续动作。

这条路线会不断回答同一个问题：为了让机器人真正完成任务，还缺少哪一类信息？我们先建立优化的基本语法，再从一个控制周期里的最小二乘逆运动学出发；加入几何边界后得到带约束的差分 IK，把时间视野拉长后得到轨迹优化，再补上动力学、接触和实时反馈，最终走到模型预测控制。

1. 先把任务翻译成决策变量、代价和约束

任何优化建模都可以先问三个问题。

机器人这次可以决定什么？

求一个姿态时，决策变量可以是关节角 $q$；差分逆运动学中，它可以是下一步关节速度 $v$；轨迹优化中，它会扩展成整段状态和控制：

$$z={x_0,u_0,x_1,u_1,\ldots,x_N}$$

什么叫“更好”？

机械臂可以偏好更短的运动、更小的力矩、更平滑的速度，或者更接近某个舒适姿态。这些偏好进入代价函数：

$$\mathcal J =w_p\mathcal J_{task} +w_u\mathcal J_{effort} +w_s\mathcal J_{smooth}$$

哪些条件必须满足？

关节上下限、动力学方程、闭环条件和刚体非穿透通常直接写成约束：

$$q_{min}\le q\le q_{max}$$ $$\dot x=f(x,u)$$ $$d_{collision}(q)\ge d_{safe}$$

这三类元素不能随意互换。把“不要碰撞”仅仅写成一个权重很大的惩罚项，求解器仍可能为了降低其他代价而接受少量穿透；把“尽量少动腕关节”写成硬约束，又会无谓缩小可行域。

一个实用判断是：

• 违反后会破坏物理、安全或任务定义的条件，优先写成约束；
• 多种可行方案之间的偏好，写成代价；
• 允许偶尔放宽的条件，可以加入松弛变量，并对松弛量施加高代价。

例如，货架碰撞属于硬边界，而“肘部尽量远离人体”可以根据应用选择硬约束或带松弛的软约束。若环境误差可能让后者暂时无法满足，可以引入松弛变量：

$$d_{elbow}(q)+s\ge d_{prefer}, \qquad s\ge0$$

再把 $\rho_s s^2$ 加入代价。这样，求解器既不会因为一个偏好条件而立刻无解，也会明确报告它放宽了多少。安全距离能否采用这种方式，仍然取决于系统风险；不能容忍的碰撞边界不应仅靠有限权重保护。

决策变量、代价、硬约束和可松弛条件由此各自有了位置。接下来先把这套语法用在最小的问题上：一个控制周期内，机械臂应该选择怎样的关节速度。

2. 从最小的问题开始：伪逆与最小二乘

在这个短暂的控制周期里，末端只需要完成一个很小的位姿修正。雅可比把它写成局部线性关系：

$$V=J(q)v$$

给定期望末端速度 $V_d$，最常见的伪逆解：

$$v=J^\dagger V_d$$

可以等价地理解为：

$$\min_v \quad \frac12\|Jv-V_d\|_2^2$$

当精确跟踪存在多组解时，Moore–Penrose 伪逆还会选择欧氏范数最小的速度。加入阻尼后：

$$\min_v \quad \frac12\|Jv-V_d\|_2^2 +\frac{\rho}{2}\|v\|_2^2$$

这就是阻尼最小二乘。第一项希望末端跟上命令，第二项抑制过大的关节速度，尤其能缓和奇异值很小时的速度放大。

用优化的眼光回看，伪逆不再是一条孤立公式。它只是“任务误差最小”这一特定目标、在没有显式边界时得到的闭式解。

真正的机器人还会带来更多条件：

$$v_{min}\le v\le v_{max}$$ $$q_{min}\le q+h v\le q_{max}$$ $$a_{min} \le \frac{v-v_{meas}}{h} \le a_{max}$$

其中 $h$ 是一个控制周期。此时问题变成带线性约束的二次规划（Quadratic Program, QP）：

$$\begin{aligned} \min_v\quad &\frac12\|Jv-V_d\|_W^2 +\frac{\rho}{2}\|v\|_2^2\\ \text{s.t.}\quad &v_{min}\le v\le v_{max}\\ &q_{min}\le q+h v\le q_{max}\\ &a_{min}\le(v-v_{meas})/h\le a_{max} \end{aligned}$$

伪逆给出一个理想修正方向；QP 则在机器人真正能够执行的速度集合里，寻找最接近期望任务速度的一步。到这里，关节自身的速度、位置与加速度边界已经进入模型，但“机械臂旁边还有一个货架”仍未被表达。

3. 把货架放进来：碰撞约束怎样进入一个周期

要让这一步动作在真实场景中成立，还需要把环境几何翻译成关于 $v$ 的约束。设机械臂与货架之间的最小距离为：

$$d(q)$$

在当前构型附近做一阶近似：

$$d(q+h v) \approx d(q)+h\nabla d(q)^Tv$$

要求下一步保持安全距离：

$$d(q)+h\nabla d(q)^Tv \ge d_{safe}$$

这仍然是关于 $v$ 的线性不等式，可以直接加入差分 IK 的 QP。杯口朝上也可以通过姿态雅可比进入目标或约束；肘部远离货架、腕部速度限幅和关节居中则继续增加新的行。

图中的多边形代表所有满足关节、加速度和碰撞线性化约束的速度。无约束最小二乘解可能落在可行域之外；QP 会把答案移动到可行域内最合适的位置。

现在，期望末端速度、关节边界和货架距离第一次出现在同一个控制问题中。增加一条限制时，也不需要重新推导一套专用逆运动学公式，只需增加相应的代价或约束。

不过它仍然只看一个很短的时间尺度。机械臂面对货架时，当前一步向左、向右都可能安全；哪一边最终能够绕过障碍，需要观察未来一整段运动。

4. 把一个速度扩展到一整条路径

上一节留下的问题来自视野太短：单步 QP 的决策变量只有 $v$，它知道下一步是否安全，却看不到几秒后能否到达目标。轨迹优化解决这个问题的第一步，就是把决策变量沿时间展开：

$$Q={q_0,q_1,\ldots,q_N}$$

此时可以同时表达：

• 起点和终点；
• 每个时刻的关节限位；
• 沿途碰撞距离；
• 杯口姿态；
• 相邻路点之间的速度与平滑性。

一个简化的运动学轨迹优化可以写成：

$$\begin{aligned} \min_{q_0,\ldots,q_N}\quad &\sum_{k=0}^{N-1} \|q_{k+1}-q_k\|_W^2\\ \text{s.t.}\quad &q_0=q_{start},\quad q_N\in\mathcal Q_{goal}\\ &q_{min}\le q_k\le q_{max}\\ &d(q_k)\ge d_{safe}\\ &R_{cup}(q_k)e_z=e_z \end{aligned}$$

这里 $\mathcal Q_{goal}$ 可以包含所有满足目标位姿的关节构型；最后一条约束要求杯口的法向保持竖直。

现在，绕过货架左侧还是右侧，不再由某个控制周期的局部梯度独自决定。求解器会比较整条候选路径的可行性与代价。

但离散路点也带来一个新陷阱：只在 $q_k$ 上检查碰撞，不代表 $q_k$ 与 $q_{k+1}$ 之间安全。实际系统需要连续碰撞检查、足够密的离散化，或者对整段样条施加有界误差的几何约束。

运动学轨迹优化因此回答了“从哪里绕过去”，却还不能回答“能否按这个速度绕过去”。质量、惯性、重力与执行器力矩仍然缺席，下一步需要把路径放回真实动力学中。

5. 再补上动力学：路径必须能由力矩产生

几何路径只规定构型随时间如何变化。对端水绕障任务而言，如果转弯太急、时间又太短，所需力矩可能超过电机能力，杯中的水也会因过大的加速度明显晃动。为了判断这段变化能否由机器人真正产生，需要把位置和速度合并为状态：

$$x= \begin{bmatrix} q\\ \dot q \end{bmatrix}$$

控制输入记作 $u$。对力矩控制机械臂，$u=\tau$；对移动机器人，它也可以是轮端力、转向角或推力。

连续动力学为：

$$\dot x=f(x,u)$$

经过离散化后：

$$x_{k+1}=F_h(x_k,u_k)$$

动力学轨迹优化把状态和控制都作为决策变量：

$$z={x_0,u_0,x_1,u_1,\ldots,x_N}$$

并用动力学方程把相邻时刻连接起来：

$$\begin{aligned} \min_{x_{0:N},u_{0:N-1}}\quad &\ell_f(x_N) +\sum_{k=0}^{N-1}\ell(x_k,u_k)\\ \text{s.t.}\quad &x_0=x_{measured}\\ &x_{k+1}=F_h(x_k,u_k)\\ &x_k\in\mathcal X,\quad u_k\in\mathcal U \end{aligned}$$

此时，力矩上限会主动塑造速度曲线。重负载姿态可能要求机械臂提前减速；欠驱动系统甚至会选择先向目标反方向运动，以积累足够能量再完成摆起。

这正是 MIT Underactuated Robotics 课程强调动力学的原因：对一台低速、全驱动机械臂，运动学路径常常足够实用；对摆杆、四旋翼、腿式机器人和高速操作，动力学本身会决定哪些运动能够发生。

不过，$\dot x=f(x,u)$ 仍是一条连续时间微分方程，通用数值求解器不能直接优化“无限多个时刻”。要把动力学交给计算机，还需要选择有限维的轨迹表示。

6. 把连续动力学交给计算机：Shooting 与直接转录

这正是 Shooting 与直接转录要解决的问题：它们不改变动力学本身，而是规定优化器通过哪些有限变量看见整段运动。

直接 Shooting 主要把控制序列作为决策变量：

$$U={u_0,u_1,\ldots,u_{N-1}}$$

给定 $x_0$ 后，通过前向仿真得到整段状态。变量较少，动力学天然满足；但后段状态依赖前面所有控制，长时域梯度可能变得敏感，状态约束也只能通过这条长计算链间接处理。

直接转录（Direct Transcription） 同时优化 $x_k$ 与 $u_k$，再增加动力学缺陷约束。以前向欧拉离散为最简单的例子：

$$x_{k+1}-x_k -h_kf(x_k,u_k) =0$$

变量数量增加了，但每条动力学约束只连接相邻节点，雅可比矩阵因此具有稀疏结构，状态和接触约束也更容易直接加入。

直接配点（Direct Collocation）进一步使用分段多项式表示状态和输入，并在区间内部的配点处满足动力学。它能用较少节点获得更高的积分精度，也是机器人轨迹优化中很常见的形式。

这里需要保留一个重要判断：离散优化得到的“动力学可行”只精确到离散方法和求解容差。求解结束后仍应使用更严格的数值积分重新仿真，并检查节点之间的力矩、碰撞和状态约束。

至此，优化器已经能同时选择状态和力矩，但它默认每个时刻的接触关系事先已知。若机械臂需要推开物体、手指需要重新抓取，或机器人需要决定何时落脚，接触时序本身也会成为未知量。

7. 当接触时序也未知：让接触模式进入优化

端着杯子绕开货架时，理想情况始终是“没有接触”，并不需要额外选择接触时序。但优化框架还可以继续覆盖推物体、重新抓取和落脚等任务；一旦接触的建立与分离也需要由求解器决定，上一篇文章中的互补条件就提供了合适的表达：

$$0\le\lambda_n \;\perp\; g(q)\ge0$$

其中 $g(q)$ 是法向间隙，$\lambda_n$ 是法向接触力。把这条互补条件放进每个轨迹节点，求解器便可以同时决定：

• 什么时候与环境分离；
• 什么时候建立接触；
• 接触后传递多大法向力与摩擦力；
• 这些力怎样通过动力学改变运动。

这类方法通常称为接触隐式轨迹优化。对于机械臂推物体、腿式机器人落脚或手指重新抓取，它减少了人工枚举接触时序的工作。

代价是问题会变得更难。互补条件和摩擦锥带来非光滑、非凸与多模式结构；求解结果高度依赖初值、尺度和松弛策略。工程上常结合预设接触序列、互补松弛、逐步收紧约束或多个初值，而不是期待一次局部优化自动发现所有动作模式。

这也提醒我们：模型能够表达复杂动作，并不等于求解器一定能找到它。问题越接近真实机器人，数值求解本身就越值得单独检查。

8. 模型写完了，为什么仍然可能失败

从绕障路径到接触时序，机器人几何、旋转、碰撞和非线性动力学通常会形成非凸问题。局部求解器找到的是某个初值附近的解，因此会出现：

• 左绕与右绕对应不同局部极小值；
• 初始轨迹穿过障碍物时，碰撞梯度可能无法提供有效脱困方向；
• 离散节点过少时，求解器利用节点之间的空隙“穿过”障碍；
• 量纲相差很大时，位置、姿态和力矩梯度会严重失衡；
• 约束互相冲突时，求解器只能返回不可行或停在较大残差处。

实用流程通常包括：

1. 用解析 IK、采样规划或上一条轨迹生成较好的初值；
2. 对时间、碰撞余量或互补约束使用逐步收紧的 continuation；
3. 对明显不同的路径拓扑使用多个种子；
4. 统一变量尺度，并记录每一类约束残差；
5. 用高精度仿真和连续碰撞检查重新验证结果。

求解器报告 success 只代表内部终止条件已经满足。机器人真正关心的是：动力学残差是否足够小、整段运动是否无碰撞、执行器是否饱和，以及把轨迹交给更精确模型后是否仍然可行。

即使这些离线检查全部通过，轨迹在真实世界中仍会受到模型误差和外力扰动。于是最后一个缺口出现了：规划需要根据最新状态不断更新。

9. 让规划跟上真实世界：从轨迹优化到 MPC

离线轨迹优化使用一个固定初始状态，求出未来几秒的状态和控制。把它直接开环执行到底，相当于假设机器人始终沿预测前进；真实系统中的模型误差、外力、状态估计偏差和移动障碍会逐步打破这个假设。

模型预测控制（Model Predictive Control, MPC）把同一个有限时域优化放进反馈循环：

1. 测量或估计当前状态 $x_{meas}$；
2. 以 $x_0=x_{meas}$ 为初始约束，优化未来 $N$ 步；
3. 只执行第一个控制量 $u_0^*$；
4. 下一个控制周期重新测量、平移预测窗口并再次求解。

MPC 的反馈来自“重新求解”。如果杯子受扰后稍微偏离，下一次优化会从新的真实状态出发；如果货架附近出现新的障碍，后续预测也会随约束更新。

线性系统、二次代价和线性约束通常形成 QP，可以在较高频率稳定求解；非线性机器人模型则对应非线性 MPC，计算成本与初始化要求都会更高。上一周期的最优轨迹向前平移后，常被用作下一次求解的 warm start。

MPC 也没有自动消除所有风险。预测时域太短时，它可能看不到更远处的死路；模型失配会让预测偏离现实；严格约束还可能在扰动后突然不可行。终端代价、终端可行集、鲁棒约束、软约束和独立安全控制器，都是让 MPC 从演示走向系统时需要考虑的部分。

从单步伪逆到在线重算，主线现在已经闭合。接着回到工程问题：同一个任务究竟需要走到哪一层。

10. 回到同一个任务：应该选择哪一层

回到机械臂端水穿过货架的任务，可以按需求选择优化层次。

差分 IK QP： 适合操作员遥控或末端速度伺服。它在每个周期处理关节速度、加速度和局部碰撞约束，计算快，但视野很短。

运动学轨迹优化： 适合提前规划一条无碰撞、姿态正确、关节平滑的路径。它能够比较整段绕障方向，却不直接保证快速运动时的力矩可行性。

动力学轨迹优化： 适合高速、重载、欠驱动或接触丰富的动作。状态、力矩和动力学共同决定轨迹，通常在执行前离线或低频求解。

MPC： 适合环境和状态持续变化、同时又需要预测约束的场景。它把有限时域轨迹优化变成在线反馈，但必须满足实时计算和递归可行性要求。

层次越高，表达能力越强，计算和验证成本也越大。低速固定工位上的简单搬运无需强行使用非线性 MPC；接触丰富的动态动作，也很难只靠单步伪逆获得可靠结果。

这四层并非人为拼出的工具清单。MIT 的相关课程也沿着几乎相同的方向组织内容：先扩大决策变量和时间视野，再让规划逐渐吸收动力学与反馈。

11. MIT 的课程为什么沿这条线连接规划与控制

MIT 的 Robotic Manipulation 课程先把伪逆解释为最小二乘，再加入关节速度、位置、加速度和碰撞约束，得到差分 IK 的 QP；随后把单个构型扩展为运动学轨迹优化。

Underactuated Robotics 则从动力学、最优控制与 LQR 出发，把状态和控制轨迹显式放进直接转录与直接配点。得到一条开环轨迹后，再用时变 LQR 等方法稳定它；当轨迹优化足够快，规划器就可以在每个控制周期重新运行，于是得到 MPC。

这条课程主线并没有把动力学、规划和反馈当成三个互不相干的工具箱。它们围绕同一个优化问题逐层改变：

$$\text{局部运动学修正} \longrightarrow \text{整段几何路径} \longrightarrow \text{满足动力学的状态与控制} \longrightarrow \text{反复重算的反馈策略}$$

规划给出未来的选择空间，动力学规定状态怎样演化，反馈则不断把优化问题的初始条件拉回真实世界。MPC 位于三者的交点：它用模型预测未来，用约束组织可行运动，用重复优化形成反馈。

沿着这条线回望，本系列前面出现的雅可比、动力学、接触和控制也就不再是分散的章节，它们恰好构成优化问题中的不同部件。

12. 把这篇文章放回整个系列

现在，前面的文章可以沿优化这条线重新连接起来：

• 雅可比提供局部线性模型 $V=Jv$；
• 逆运动学把目标误差写成最小二乘；
• 关节限位和碰撞把伪逆扩展成 QP；
• 轨迹生成提供路径、时间和连续性要求；
• 拉格朗日动力学成为轨迹优化中的等式约束；
• 接触、摩擦和互补条件让接触模式进入决策；
• 计算力矩和 LQR 稳定已规划轨迹；
• MPC 反复求解有限时域问题，把规划变成反馈。

优化没有替代前面的几何、动力学与控制理论。它更像一个共同的装配面：每一种知识都以代价、等式或不等式的形式进入同一个问题。

当约束表达得足够准确时，许多看似需要手工设计的动作会从可行域中自然出现。机器人如何运动，最终由任务目标、物理规律和安全边界共同决定。

参考与延伸阅读

1. Russ Tedrake, Robotic Manipulation: Basic Pick and Place — Differential inverse kinematics with constraints, MIT.
2. Russ Tedrake, Robotic Manipulation: Motion Planning, MIT.
3. Russ Tedrake, Underactuated Robotics: Trajectory Optimization, MIT.
4. Russ Tedrake, Underactuated Robotics: Linear Quadratic Regulators, MIT.
5. Russ Tedrake, Underactuated Robotics: Feedback Motion Planning, MIT.
6. Drake, MathematicalProgram tutorial.
7. Drake, MultipleShooting class reference.
8. C. R. Hargraves, S. W. Paris, “Direct Trajectory Optimization Using Nonlinear Programming and Collocation,” Journal of Guidance, Control, and Dynamics, 10(4), 1987.
9. D. Q. Mayne, J. B. Rawlings, C. V. Rao, P. O. M. Scokaert, “Constrained Model Predictive Control: Stability and Optimality,” Automatica, 36(6), 2000.