前置 3 · 运动控制与抓取规划：从反馈闭环到物理接触¶

这章写给熟悉机器学习、想弄明白底层控制与物理接触直觉的读者。控制理论与统计学习的思路截然不同：这里没有离散的推理等待，只有硬实时、连续动力学反馈与严苛的物理接触约束。摸清反馈控制律与接触力学的脾气，后面读到视觉语言动作模型（VLA）与端到端具身策略时，许多底层的设计取舍才会真正立得住。

实时控制的本质：高频刚性反馈循环与时间尺度分层¶

机器学习的标准流程讲究请求与响应：喂入输入张量，前向传播跑完吐出结果，计算流程就算结束了。机器人的底层控制全不是这回事，它本质上是一套永不停歇的高频确定性闭环：

while 机器人处于使能状态:
    读取传感器状态 (编码器、IMU、力/力矩传感器)
    计算跟踪误差 (实际状态 vs 期望轨迹)
    根据控制律解算控制量 (关节力矩、电机电流)
    下发指令至底层驱动器
    等待硬实时周期中断 (典型周期 1 ms)

控制循环的执行频率与时间抖动（jitter），直接决定了机械系统的动态性能与稳定边界。工业级伺服驱动里的电流环与力矩环，刷新频率通常落在 1 kHz 至 4 kHz；到了复杂的仿人双足系统上，整套系统更是呈现出清晰的时间尺度分层（Frequency Hierarchy）：

关节级伺服与电流环 (1–4 kHz)：交由底层 DSP、FPGA 或硬实时操作系统（如 Linux PREEMPT_RT、Xenomai）托管，直接闭环电机磁场定向控制（FOC）与高频力矩响应。
笛卡尔阻抗与全身动力学规划 (100–500 Hz)：解算逆运动学、重力补偿与接触力平衡，确保末端和躯干稳稳跟住期望轨迹。
高阶动作策略与 VLA 推理 (10–30 Hz)：接收多视角视觉输入与自然语言指令，输出连续的多步动作轨迹块（Action Chunks）。
长时程语义任务规划 (0.1–1 Hz)：借助大语言模型拆解任务逻辑，完成环境拓扑推理与子目标调度。

频率分层是物理系统抵御不确定性的核心架构法则。底层控制拿毫秒级的极小计算延迟换取高频抗扰能力，上层策略花数百毫秒的计算预算换取全局语义理解。大语言模型跑一次前向传播，消耗数十毫秒甚至数秒并不致命；可机器人的底层控制回路必须在 1 毫秒内硬性跑完状态采样、矩阵求解与信号下发，容不得任何操作系统垃圾回收（GC）停顿或线程阻塞。正因如此，机器人的底层控制栈始终由 C++ 实时线程或嵌入式微码写就。

PID 控制器：经典误差驱动与稳态边界¶

比例-积分-微分（PID）控制器是经典控制理论的基石。即便在今天，工业界 80% 以上的物理执行器也依然靠它维持稳定运转。

设受控关节当前角度为 $q(t)$，期望目标角度为 $q^(t)$，定义瞬时跟踪误差为 $e(t) = q^(t) - q(t)$。在连续时间下，PID 控制律表达为：

$$u(t) = K_p e(t) + K_i \int_0^t e(\tau) \, d\tau + K_d \frac{d e(t)}{dt}$$

其中 $u(t)$ 为输出给电机的控制指令（如期望电流或力矩）：

比例项 ($K_p$)：等效于虚拟弹簧刚度。误差拉得越开，施加的纠偏力矩就越大；但单靠比例控制在惯性系统中必然导致持续震荡，且在重力或恒定外力作用下存在固有的稳态静差。
积分项 ($K_i$)：自带对历史误差的累积记忆，专门用于消除由库仑摩擦、系统重力或负载变化引起的稳态偏差；然而积分项若缺少抗饱和（Anti-windup）保护，极易引发积分饱和并破坏系统稳定性。
微分项 ($K_d$)：根据误差变化率提供动态阻尼（Damping），预判未来趋势并抑制系统超调与动能震荡。

在实际工业部署中，工程师常按工况裁剪这三项：机械臂关节位置控制多采用 PD 控制（配合前馈重力补偿以规避积分滞后），而在恒力打磨等接触力控制中，则倾向于采用 PI 控制来彻底消除稳态法向力误差。

从统计学习视角审视：PID 本质上是由极少几个状态量（误差及其积分、微分）驱动的紧凑线性反馈策略。尽管深度神经网络理论上能够拟合任意连续函数，但在端到端轨迹生成中，单凭开环数据回归极难保证每一步输出都天然自带类似 PD 律的渐近收敛性与相角裕度。许多 VLA 模型在真实机械臂上运行时出现的“轨迹高频颤动”或“目标点附近犹豫不决”，物理根源往往正是策略网络未能内生学得高频微分阻尼与稳态收敛机制。

阻抗与导纳控制：环境交互与柔顺接触动力学¶

当机械臂从自由空间运动进入物理接触状态（如桌面装配、开关柜门、擦拭台面），传统的刚性位置控制便会暴露出致命缺陷：一旦目标轨迹与环境几何产生微米级干涉，由于机械臂结构的高刚度，接触力会瞬间激增，导致关节电机堵转、减速箱齿轮崩裂或损坏被操作物体。

阻抗控制（Impedance Control）通过动力学反馈，将机械臂末端与环境的交互行为在物理上等效为一个虚拟的“质量-弹簧-阻尼”系统：

$$M_d (\ddot{x}^ - \ddot{x}) + D_d (\dot{x}^ - \dot{x}) + K_d (x^* - x) = F_{\text{ext}}$$

在准静态接触或忽略高频加速度项的工况下，公式可简化为力与形变的经典关系：

$$F_{\text{ext}} = K (x^* - x) - D \dot{x}$$

$K$ 为虚拟刚度系数（Stiffness）：$K$ 值越大，末端抗外力变形能力越强，轨迹跟踪精度越高；$K$ 值越小，末端越柔顺，对未知障碍物的碰撞冲击越小。
$D$ 为虚拟阻尼系数（Damping）：负责吸收接触动能，抑制碰撞瞬态的弹跳与高频振颤。

在这套控制律下，机械臂表现出仿生肌肉般的顺应性：受到外力推挤时主动退让，外力撤销后平滑回弹。

阻抗控制要跑得稳，底子全在高保真度的力觉感知硬件上。主流方案包括关节端集成的扭矩传感器（如 Franka Research 3、KUKA LBR iiwa）或腕部六维力/力矩传感器（F/T Sensor）。缺乏专用力传感器的低成本机械臂若仅依靠电机母线电流估算关节力矩，往往因谐波减速器非线性摩擦与迟滞效应产生 30% 至 50% 的测量误差，难以在微牛顿至数牛顿级别完成精细的柔顺装配。

阻抗控制的对偶形式为导纳控制（Admittance Control）：它的逻辑刚好相反，硬件通过传感器测量外部接触力 $F_{\text{ext}}$，以此为输入解算虚拟动力学方程，反向输出位置或速度修正量供底层高刚度位置环跟踪。导纳控制常用于重负载、高惯量的传统工业机械臂精密装配场景。

在具身大模型架构中，当前主流 VLA 策略通常输出笛卡尔空间期望位姿序列，下游经由 100–500 Hz 的笛卡尔阻抗控制器转化为各关节伺服电流。这套分层解耦机制确保了上层神经网络即使输出带噪轨迹，物理机械臂也不会因微小干涉而引发破坏性的刚性碰撞。

位置控制、力控制与混合力位控制¶

规划机械臂的宏观任务时，手头要对付的目标大致分作两类：

位置控制 (Position Control)：把末端稳稳送到三维空间的指定位姿，在自由空间里搬运物件、悬空点胶或是沿固定轨迹巡航，全靠它来走位。
力控制 (Force Control)：让末端沿着特定法向压住物体，分毫不差地维持设定的接触力大小。给工件恒力抛光、打磨表面或是用探针触测，靠的都是这股受控的力道。

真到了复杂的物理交互场景，单抓位置或单抓力都很难把事情办妥，这就必须引入混合力/位控制 (Hybrid Position/Force Control)。依照约束正交性原理（Mason Compliance Frame），整个操作空间在几何上恰好能拆解为两个互补的正交子空间：

以桌面擦拭任务为例： - 在台面切向的 $X$ 轴与 $Y$ 轴方向上，末端处于自由运动子空间，采用位置控制精准走完清洁轨迹； - 垂直于台面的 $Z$ 轴方向则落入几何接触约束子空间，采用力控制恒定施加 10 N 的下压力，顺势化解台面平整度公差与机械臂安装倾角。

阻抗与导纳控制则给出了另一种优美的解法，在位置控制（刚度 $K \to \infty$）与纯力控制（刚度 $K \to 0$）之间搭起天平，允许控制器在这两端之间连续权衡。

这种控制特质，恰恰照出了当下具身数据底座的软肋：主流遥操作采集平台（如 ALOHA、Mobile ALOHA、UMI）记录的大多是人类操作员在笛卡尔空间里的位置运动学轨迹，很少能留下高保真度的多维接触力流数据。人在遥操作时，主要也是仗着视觉闭环在微调手部位置，并非直接掌控细微的力矩。那些需要高精度力控与接触敏感的任务，之所以单凭海量互联网视频或缺少力觉的遥操作数据怎么也泛化不开，物理瓶颈就在这里。

轨迹生成：从多项式样条、DMP 到扩散动作轨迹¶

机械臂要从构型 A 平滑过渡到构型 B，首先得在连续时间轴上把这条可行的轨迹表征并生成出来。

直线线性插值 (Linear Interpolation)：在关节空间或笛卡尔空间里按固定步长拉出一条直线。这种做法最大的硬伤，在于起点与终点的加速度会发生瞬时跃变，生生砸出剧烈的机械冲击与结构振动。

多项式样条曲线 (Cubic / Quintic Splines & B-Splines)：工业运动控制里的标准打法。借助五次多项式或高阶 B 样条，关节位置、角速度与角加速度能在整个时间域内保持连续可导（满足 $C^2$ 连续性），从几何上抹掉机械冲击。无论是 MoveIt 还是主流工业控制器，内部都嵌着成熟的高阶样条轨迹生成模块。

动态运动原语 (Dynamic Movement Primitives, DMP)：由 Stefan Schaal 等人奠基的非线性动力学轨迹表征框架。DMP 把复杂的运动拆解开来，一边是渐近稳定的规范吸引子系统（Canonical Attractor System），另一边是由局部基函数拟合的非线性强迫项（Forcing Term）。它的杀手锏在于只要单次人类示范轨迹就能完成参数拟合，还能靠缩放目标点与时间常数，顺畅泛化到不同的起止位姿，在很长一段时间里主导着模仿学习领域。

生成式动作轨迹分布 (Diffusion Policy & Flow Matching)：以 Diffusion Policy、ACT 与 $\pi_0$ 为代表的新一代具身模型，甩开了显式动力学微分方程的形式约束，转而仰仗表达力更强的扩散模型与流匹配机制，直接在离散时间步上拟合多峰动作序列的分布。

从多项式样条、DMP 一路走到扩散动作轨迹，轨迹表征的演化脉络透着清晰的工程范式转向：机器人学正告别过去精心手工构造的解析归纳偏置，大步迈向高容量数据驱动的分布建模。

抓取规划：几何闭合、力闭合与接触力学¶

抓取（Grasping）是机械臂与环境物料建立稳定力学约束的第一步。第 4 章会全面剖析开放场景抓取的工程破局点，此处先要把核心的力学定义与几何约束立清楚。

抓取任务的形式化定义： - 输入：场景的多模态感知数据（RGB 图像、深度点云或目标的三维包围盒）。 - 输出：末端夹爪在任务空间的目标位姿 $T \in SE(3)$，连同夹爪指间开度（Gripper Width）。

末端执行器形态分类¶

平行双指夹爪 (Parallel-Jaw Gripper)：工业界与学术界最普及的形态，结构刚度高，运动学建模清晰，但在对付复杂曲面与非凸几何体时，手指能吃到的接触面积非常有限。
真空吸盘 (Suction Cup)：靠气压差吸附平整或微曲表面。在 Amazon 仓储机器人这类物流分拣一线，每天数以百万计的包裹流转全靠它提拉，只是它高度依赖物体表面的平整度与气密性。
多指拟人灵巧手 (Multi-Finger Dexterous Hand)：生着 3 至 5 根手指，配上多达 10 到 20 个主动自由度，抓取姿态极度灵活，也给手内重构（In-hand Manipulation）留足了空间。代价则是高昂的机械造价、娇贵的零件寿命，外加控制状态空间的急剧膨胀。
柔顺软体夹爪 (Soft Gripper)：采用硅胶气动网格或变刚度弹性体搭建，借由材料自身的形变自适应包裹生鲜浆果、禽蛋这类易碎物体，代价则是接触力精度与空间定位精度的折损。

抓取力学的核心判定：力闭合与形闭合¶

       形闭合 (Form Closure)                   力闭合 (Force Closure)
      ┌──────────────────────┐               ┌──────────────────────┐
      │   几何死锁，无惧摩擦   │               │   摩擦锥覆盖外力空间  │
      │      [ 机械卡槽 ]     │               │   F_t <= μ * F_n     │
      │        ┌────┐        │               │   ◄───[ 物体 ]───►   │
      │        │ 物 │        │               │    F_1       F_2     │
      │   ─────┴────┴─────   │               │   (依赖接触摩擦维持)  │
      └──────────────────────┘               └──────────────────────┘

形闭合 (Form Closure)：纯粹依靠夹爪与物体的几何干涉，彻底锁死物体在笛卡尔空间里的全部 6 个运动自由度。形闭合完全不挑接触面的摩擦系数，就能彻底限制相对位移，就像双指自上方卡住带凹槽的把手那样。
力闭合 (Force Closure)：在有限的摩擦系数 $\mu$ 约束下，夹爪施加正向法向力，让各个接触点的库仑摩擦锥（Friction Cone）并集在力螺旋（Wrench）空间内足以抵消任意方向的外部扰动力与力矩。一只手从光滑表面捏起水杯，便是最典型的力闭合。

对向抓取 (Antipodal Grasp) 是力闭合在平行双指夹爪上的经典几何特例：两处接触点的连线必须落入彼此接触法向的摩擦锥内部，且该连线要尽可能穿过物体重心。这一简化直接把高维抓取搜索空间利落地降维为对接触法向点对的采样与评估。

现代数据驱动抓取算法（如 Dex-Net、GG-CNN、6-DoF GraspNet）的核心逻辑，正是训练深度神经网络从点云或表面法向量分布中，快速回归出高概率达成力闭合的候选对向位姿。

开放场景抓取的物理瓶颈：从结构化分拣到非结构化物性泛化¶

工厂流水线上的料箱分拣（Bin Picking），靠着确定的工件 CAD 模型与结构光相机，早就稳稳做进了工程实战。可一旦换到家庭或开放服务场景，通用的自主抓取立马撞上几堵硬墙：

物体几何与物性分布的长尾性。工业料箱里的工件有着确定不移的几何形状与材料刚度；家庭环境里则是另一番天地：外卖包装袋、折叠织物、盛着半杯水的玻璃杯与新鲜果蔬，弹性模量、表面摩擦和阻尼特性千差万别，抓取策略根本无法继续依仗静态刚体假设。

复杂接触动力学的不可微分性。在动力学仿真中，刚体碰撞与摩擦切换牵涉到互补性条件（LCP）与接触流形的突变。盛水容器的重心晃动、蓬松织物的非线性形变，加上软包装受力后的微小滑移，全都超出了经典刚体动力学求解器能够精准表达的范围。

任务语义对抓取姿态的功能性约束 (Task-Conditioned Affordance)。在非结构化场景里，单单维持力闭合远远不够：抓取水果刀必须握持刀柄而非刃口，端起热咖啡杯必须捏紧侧耳免遭烫伤，拿取装满液体的量筒更得一路保持竖直。抓取当下的位姿，必须与接下来的操作意图产生深度的因果绑定。

接触瞬态触觉反馈的缺失。光靠多视角相机的纯视觉感知，夹爪完全闭合的那一瞬间必然会遭遇视线遮挡。机器人无法直接摸清被夹物体的实时接触形变、法向载荷分布与微滑移先兆，手上力道稍欠一点容易滑脱掉落，捏得稍重一点又会直接夹碎易损物体。

移动底盘与足式控制：从轮式差速、导航栈到足式 MPC¶

当操作任务离开固定的台面、扩展到大范围移动空间，底盘本身的运动学约束与平衡控制，便成了系统必须接纳的新变量。

移动底盘的运动学特性¶

差速驱动底盘 (Differential Drive)：左右两组驱动轮各自独立控制转速，属于典型的二维非完整约束（Non-holonomic）系统。车体无法直接朝侧面横移，转向结构却很简捷，机械寿命也长。
麦克纳姆全向轮 (Mecanum)：轮毂边缘排布着一圈倾斜滚子，靠四个车轮的差动配合，底盘在平面上能以零转弯半径完成任意平移与原地旋转。遇到粗糙路面或过坎越障，车身震动剧烈，小滚子磨损也快。
阿克曼转向底盘 (Ackermann Steering)：模仿汽车前轮转向的机械结构，高速行驶时平稳扎实，只是拐弯受最小转弯半径限制，无法原地回转。

像 ROS2 Nav2 这类工业级导航系统，靠着严密的分层架构，把底盘从起点稳妥送到目的地： - 全局路径规划器 (Global Planner)：在静态拓扑地图或占据栅格地图上，用 $A^$ 或 Dijkstra 算法解算一条避开所有障碍的最优折线。 - 局部轨迹规划器 (Local Planner)：运行 TEB（Timed Elastic Band）或 MPPI（Model Predictive Path Integral）这类算法，时刻对照动态代价地图（Costmap），高频算出符合底盘动力学限制的速度指令 $(v, \omega)$。 - 状态恢复行为树 (Recovery Behaviors)*：一旦底盘陷入死锁或局部极值，行为树便主动介入，调用原地自转扫描或倒车逻辑帮它脱困。

足式动态平衡：ZMP 约束与模型预测控制 (MPC)¶

四足与双足机器人摆脱了轮式底盘连续支撑面的几何束缚，控制难度也跟着陡然跃升。足端与地面的接触不再连续，为了防止在断续接触中翻倒，系统必须严格服从零力矩点理论 (Zero Moment Point, ZMP)：地面反作用力的合力对水平轴净力矩为零的那个点，必须时刻被锁在动态足底支撑多边形（Support Polygon）内部。

到了动态行走与高动态奔跑的场景下，波士顿动力、ETH ANYmal 与 MIT Cheetah 这些经典足式平台，把整套控制算法压在两块基石上：凸优化模型预测控制 (Convex MPC) 与全身动力学控制 (Whole-Body Control, WBC)。

滚动时域优化 (Receding Horizon Optimization)：控制器以 50–200 Hz 的频率运转，每次都紧扣当前的质心与足端状态，推演未来 200–1000 ms 的动力学演化轨迹，当场解出关于地面反作用力与接触时序的最优二次规划（QP）问题。
即时下发与循环迭代：在优化算出的多步控制量里，系统只取第一步指令下发给执行器，等下一个时钟周期收到新的机身状态，立刻推翻旧解、重新规划。

这种思路与现代具身策略里的动作块（Action Chunking）机制在数学模型上完全同构：策略模型一次性给出多步预测轨迹，系统却只执行前 $K$ 步动作便再次重规划，靠高频修正换取抗扰动能力，在鲁棒性与动作的前瞻性之间搭起动态平衡。

核心认知与工程判据¶

机器人底层控制是硬实时高频闭环。底层关节伺服工作在 1 kHz 至 4 kHz 的稳态频率，而高阶 VLA 策略的推理窗口通常只有 10 Hz 至 30 Hz。时间尺度上的分层，把动力学扰动与语义决策彻底隔开。
PID 与前馈控制依然是执行机构的最后一道确定性防线。端到端神经网络如果没有底层的高频阻尼兜底和误差收敛保证，一进到真实的物理交互环境，立刻就会暴露出轨迹抖动与稳态漂移。
阻抗与导纳控制是安全接触交互的数学基石。要想做出真正高保真的柔顺交互，系统必须拿到来自关节力矩传感器或末端六维力传感器的真实读数；如果只靠电机电流去反推力矩，在轻载荷工况下的精度根本经不起推敲。
抓取规划必须由几何力闭合迈向任务语义导向。在料箱里抓取标准刚体零件早已不再是难题，可一旦步入开放场景，柔软物体的形变、视线受阻时的盲区，还有指尖微滑移的触觉感知，处处都是卡死抓取成功率的硬瓶颈。
滚动时域优化（MPC）与动作分块（Action Chunking）具有内在统一性。物理世界充满随机扰动，开环的长序列预测跑着跑着必然会漂走发散。唯有高频重规划前瞻轨迹，才是守住动态稳定性的根本法则。

工程实践与思考¶

PID 稳态与动态响应实验。在物理仿真环境或 1-DoF 摆杆控制器上，亲手调节 $K_p$、$K_i$ 与 $K_d$ 增益。故意调大 $K_p$ 与 $K_i$ 来触发系统的超调与积分饱和震荡，随后调大 $K_d$，观察微分阻尼吸收动能的过程，建立对各增益物理意义的直觉。
研读工业协作机械臂阻抗控制架构。查阅 Franka Research 3 (Panda) 的官方技术手册中关于笛卡尔阻抗控制（Cartesian Impedance Control）与零空间投影（Null-space Projection）的推导章节，对比关节端扭矩传感器在重力补偿与碰撞检测中的实际响应延迟。
解构高动态足式机器人的控制边界。分析 Boston Dynamics Atlas 与 ANYbotics ANYmal 的高动态跑酷与野外越障日志，理解凸优化 MPC 与全身动力学控制（WBC）如何在毫秒级时间内分配足端法向反力，体会经典控制理论在高动态物理平衡中的严密性。
对比 Universal Robots UR5e 与 Franka Research 3 的选型参数。整理两款主流机械臂在有效负载（Payload）、重复定位精度（Repeatability）、力控传感模式（电流推算 vs 关节扭矩传感器）与通信接口刷新率上的差异，评估它们在精密柔顺装配与大模型遥操作部署中的工程适配性。

下一章：第 1 章端到端与分层