第98章：多机协作 Loco-Manipulation——任务分解、延迟鲁棒与局部自治¶

元信息	值
难度	⭐⭐⭐⭐（多机器人协同控制 + 分布式优化 + 工程系统）
预计时间	1.5 周（25-35 小时）
前置依赖	复合/20_浮动基座臂统一动力学、复合/30_多模态MPC、复合/120_底盘臂联合规划、复合/250_力敏感人形LocoMani；足式/90_WBC分层优化与TSID（WBC QP 基础）；足式/110_OCS2完整栈（MPC_MRT 接口）
本章主线	把单机行走操作扩展为多机协作搬运、同步操作和安全接管
实验平台	双 Go2+Z1、双 G1、Go2+Z1 与轮式机械臂异构组合
核心问题	多台机器人共同约束一个物体时，如何在延迟、丢包、局部故障和模型不准下保持任务安全

98.0 前置自测¶

#	问题	若答不出应回顾
1	写出单台四足+臂的统一动力学方程，并说明末端 wrench 如何通过雅可比进入广义力。	复合/20_浮动基座臂统一动力学
2	为什么复合 MPC 中行走稳定性通常比末端跟踪权重高一到两个数量级？	复合/30_多模态MPC
3	MPC 与 WBC 的频率分工是什么？MPC 解如何插值到 500 Hz 控制循环？	OCS2 与 WBC 章节
4	SLAM 位姿协方差如何影响导航代价、落足代价或安全边界？	SLAM 与感知控制章节
5	双机搬运长杆时，哪些量属于物体状态，哪些量属于每台机器人本体状态？	本章 98.1

本章目标¶

能把多机器人行走操作任务拆成物体层、队形层、单机层、关节层四个可验证问题。
能判断集中式 MPC、分布式优化、阻抗一致性和规则化状态机各自适合的协作强度。
能把通信延迟、时钟偏差、丢包和带宽约束转化为控制系统中的数据新鲜度约束。
能设计局部自治与全局协调的接口，使单机在网络退化时仍能安全降级。
能搭建双 Go2+Z1 或双 G1 协作搬运的仿真实验，并给出稳定性、同步误差和通信鲁棒性指标。

本章地图¶

多机协作 Loco-Manipulation
├── 任务层：长杆搬运、双机开门、多人形协作搬箱、异构机器人交接
├── 物体层：物体位姿、物体 twist、合 wrench、内力、抓取约束
├── 协调层：角色分配、时间同步、队形约束、任务进度一致
├── 单机层：局部 MPC、WBC、力控、避障、跌倒检测
├── 通信层：时钟、延迟、丢包、QoS、数据新鲜度
└── 安全层：局部自治、断连降级、释放策略、恢复协议

本质洞察：多机协作的核心不是让所有机器人共享一个超大控制器，而是让每台机器人知道自己必须保证什么、可以协商什么、网络变差时还保留什么安全动作。

前面章节讨论的行走操作大多是单机问题。

单机系统中，机器人只需要在自己的动力学、接触约束和环境约束下跟踪目标。

多机系统则不同。

一旦两台或更多机器人同时接触同一个物体，每台机器人的末端力都会通过物体传给其他机器人。

这时，“每台机器人都把自己的末端误差降到最小”并不等价于“整个任务稳定完成”。

如果目标物体是长杆，左侧机器人稍微快一点，右侧机器人稍微慢一点，物体内部就会出现弯曲力矩。

如果目标物体是箱子，两台机器人夹持点标定相差 2 cm，高刚度位置控制就可能把箱子夹坏。

如果目标物体是门，两台机器人从不同侧面施力，门铰链会把它们的动作强耦合在一起。

因此，多机协作的第一原则是对象中心，而不是机器人中心。

对象中心的意思是：先问物体应该如何运动、物体受力是否安全、物体和机器人之间的接触是否可靠，再问每台机器人如何实现自己的部分。

这与单机行走操作中的“末端中心”有明显不同。

末端中心关注的是每个末端到目标的误差。

对象中心关注的是所有末端共同作用后的物体状态。

这个视角会贯穿本章。

98.1 从单机到多机：问题边界与任务分类 ⭐⭐¶

动机：为什么不能简单复制单机控制器¶

单机行走操作的控制目标通常可以写成：

\[ \min_{x,u} J_{\text{base}}(x,u)+J_{\text{ee}}(x,u)+J_{\text{safe}}(x,u) \]

其中 \(J_{\text{base}}\) 保证基座稳定，\(J_{\text{ee}}\) 追踪末端位姿，\(J_{\text{safe}}\) 处理限位、碰撞和力矩。

两台机器人协作时，如果把这个目标复制两份，得到的是：

\[ \min_{x_1,u_1} J_1+\min_{x_2,u_2} J_2 \]

这个写法缺少最关键的量：物体状态 \(x_o\)。

更准确的多机协作目标应包含：

\[ X=[x_o,x_1,x_2,\ldots,x_N] \]

其中 \(x_o\) 是物体状态，\(x_i\) 是第 \(i\) 台机器人的状态。

如果物体被多台机器人刚性抓住，则每台机器人的末端位姿必须满足：

\[ T_{w e_i}(q_i)=T_{w o}T_{o e_i}^{\text{grasp}} \]

这里 \(T_{w o}\) 是物体位姿，\(T_{o e_i}^{\text{grasp}}\) 是第 \(i\) 台机器人相对物体的抓取变换。

这个约束说明了多机协作与单机操作的根本差异。

单机末端跟踪的是外部目标。

多机末端跟踪的是同一个物体状态派生出的相容目标。

如果这些目标不相容，机器人之间会通过物体互相打架。

三类协作任务¶

类型	物理耦合	典型任务	控制重点
刚性协同	多台机器人共同约束同一个刚体	双机搬箱、双人形抬桌、双臂搬长杆	物体位姿、内力、同步
柔性协同	物体或连接具有柔性	搬软管、拉电缆、协作铺布	阻抗、张力、形状估计
松耦合协同	共享任务目标但物理接触弱	多机整理房间、多机器人巡检和开门	任务分配、区域覆盖、冲突避免

刚性协同最接近“多台机器人组成一个大机器人”。

柔性协同最容易出现建模误差，因为柔性物体状态本身难以完整观测。

松耦合协同则更像任务规划和多机调度问题。

本章重点放在刚性协同和带少量柔顺的协同搬运，因为它们最能体现 locomotion、manipulation 和 communication 的耦合。

四层状态分解¶

多机协作系统可以分成四层状态。

第一层是物体状态。

物体状态包括：

\[ x_o=[p_o,R_o,v_o,\omega_o] \]

第二层是队形状态。

队形状态描述各机器人相对物体或相对队形中心的位置。

第三层是单机状态。

单机状态包括浮动基座、本体速度、关节角、关节速度、接触状态和局部地图。

第四层是执行状态。

执行状态包括电机温度、力矩裕度、通信数据年龄、控制模式和安全状态。

这四层不能混在一起。

物体层适合低频全局协调。

单机层适合局部 MPC 与 WBC。

执行层必须保持硬实时和本地闭环。

工程分层¶

层级	职责	输入	输出	典型频率
L0 任务语义	识别搬运、交接、同步操作类型	语言指令、任务脚本	物体任务图	0.1-2 Hz
L1 物体规划	生成物体位姿与速度参考	地图、目标、抓取点	物体轨迹	1-10 Hz
L2 协调分配	分配角色、wrench、进度	物体轨迹、单机能力	单机合同	10-50 Hz
L3 单机控制	局部 MPC/WBC 执行	合同、状态估计	关节命令	100-1000 Hz
L4 硬件保护	限幅、急停、热保护	电流、温度、姿态	安全停机或降级	1 kHz+

这里的“合同”是后文的核心概念。

合同不是法律概念，而是控制接口。

它说明某台机器人在某段时间内承诺满足什么目标、允许多大误差、最大能施加多大力、失去通信后如何处理。

决策表¶

问题	优先选择	不宜选择	判断信号
机器人数量少且网络稳定	集中式或半集中式	纯局部规则	联合状态维度仍可实时求解
物体刚性强且负载大	物体中心协调	各自末端独立跟踪	内力上升、夹持力过大
任务只需区域覆盖	松耦合任务图	刚性队形控制	共享状态很少且接触弱
物体柔性明显	阻抗一致性	高刚度位置同步	轻微误差导致大力
人旁高安全任务	局部自治优先	全局命令直达硬件	需要毫秒级安全反应

⚠️ 常见陷阱¶

⚠️ 把多机等同于多线程控制

错误想法：每台机器人运行同一套单机控制器，再共享目标即可。

典型现象：物体抖动，夹持力互相抵消，队形持续拉扯。

根本原因：多机耦合通过物体和环境发生，不通过软件接口发生。

正确做法：先定义物体层状态和协作约束，再定义单机控制器。

⚠️ 忽略异构能力

错误想法：所有机器人力矩、工作空间、传感器和延迟都差不多。

典型现象：强机器人被限制，弱机器人过载。

根本原因：多机系统的瓶颈通常由能力最弱的单元决定。

正确做法：在任务分配前建立能力表和在线裕度估计。

⚠️ 只看队形误差

错误想法：两台机器人相对位置保持住，协作就稳定。

典型现象：队形看似稳定，物体内部力却持续升高。

根本原因：队形误差不能反映抓取点误差、内力和接触状态。

正确做法：同时记录物体位姿、内力、接触力和单机支撑裕度。

练习¶

给双 Go2+Z1 搬运长杆任务画出 \(x_o,x_1,x_2,z\) 的状态分块。
列出刚性协同、柔性协同、松耦合协同各两个任务例子。
解释为什么“队形误差小”不必然代表“物体受力安全”。

有了问题边界，下一步是把一个看似连续的大任务拆成可调度、可验证、可降级的子任务。

98.2 任务分解：从物体任务图到单机合同 ⭐⭐⭐¶

动机：协作任务的难点在阶段之间¶

协作搬运不是一个连续指令，而是一串阶段。

典型双机搬箱包含：

接近物体。
对齐抓取点。
建立接触。
逐步转移负载。
协同移动。
对齐放置位置。
缓慢卸载。
释放并撤退。

每个阶段的控制目标不同。

接近阶段关心路径和避障。

建立接触阶段关心末端误差和接触力门限。

负载转移阶段关心支撑裕度和力矩裕度。

协同移动阶段关心物体轨迹、同步误差和内力。

释放阶段关心物体是否已经稳定接触地面。

如果用一个控制器权重覆盖所有阶段，调参会非常困难。

任务图¶

任务图用节点表示阶段，用边表示进入条件。

不要用固定时间作为主要切换条件。

接触建立不是“过了 2 秒”就完成。

接触建立应由位姿误差、接触力、数据新鲜度和双方确认共同判断。

一个简单任务图可以写成：

APPROACH
  → PRE_GRASP      条件：两机到达预抓取区域
  → CONTACT        条件：末端误差 < 2 cm，网络新鲜度 < 80 ms
  → LOAD_TRANSFER  条件：接触力进入安全范围
  → CO_MOVE        条件：物体离地，支撑裕度足够
  → PLACE          条件：到达放置区域
  → RELEASE        条件：物体稳定接触支撑面
  → RETREAT        条件：两机解除接触

这个图比“给两个机器人一条轨迹”更有工程价值。

它让控制系统知道什么时候该等待队友，什么时候该继续，什么时候该降级。

单机合同¶

单机合同可以写成：

\[ C_i=(T_{o e_i}^{\text{ref}},W_i^{\max},\epsilon_i,t_{\text{valid}},a_i^{\text{fallback}}) \]

其中：

字段	含义
\(T_{o e_i}^{\text{ref}}\)	末端相对物体的期望抓取变换
\(W_i^{\max}\)	允许施加的最大 wrench 或力范围
\(\epsilon_i\)	位姿误差容许管
\(t_{\text{valid}}\)	合同有效期
\(a_i^{\text{fallback}}\)	合同失效时的本地动作

合同的关键是边界清晰。

全局协调层不发布关节角。

全局协调层发布的是物体目标和单机合同。

单机控制器在合同范围内自行处理局部避障、姿态稳定、关节限位和支撑裕度。

这就是多机系统中的“弱集中、强自治”。

角色分配¶

多机协作中的角色不一定是 leader/follower。

更实用的角色包括：

角色	主要职责	适合条件
承重者	提供主要竖直支撑力	力矩裕度大、支撑稳定
导向者	控制物体方向和路径	感知质量好、前方视野好
稳定者	抑制物体摆动和内力	末端力控好、阻抗可调
观察者	提供物体 pose 和环境观测	传感器视角好
恢复者	在队友故障时执行放置或支撑	工作空间覆盖安全放置区

角色可以随任务阶段变化。

起步时，视野好的机器人可以做导向者。

过窄门时，靠前机器人可以做路径观察者。

放置时，支撑稳定的机器人可以做承重者。

工程分层¶

层级	职责	输入	输出	典型频率
任务图层	生成阶段和进入条件	任务语义、地图	阶段图	0.1-1 Hz
合同层	把物体任务转为单机约束	阶段图、能力表	单机合同	1-10 Hz
监督层	检查合同是否仍有效	状态估计、网络状态	继续、暂停、降级	10-50 Hz
执行层	在合同内优化动作	局部地图、动力学状态	力矩或目标角	100-1000 Hz

决策表¶

问题	优先选择	不宜选择	判断信号
物体轨迹已知	按物体坐标分解合同	按世界坐标固定末端	物体姿态变化大
队友能力差异明显	能力加权角色分配	均分 wrench	某台力矩裕度低
接触建立阶段	同步屏障条件	固定时间切换	接触力噪声大
放置阶段	地面接触确认后释放	到达位置立即松手	物体仍有速度
任务时间很长	合同分段刷新	一次性发布全程轨迹	环境和队友状态变化

代码示例：对象中心任务图¶

from dataclasses import dataclass
from enum import Enum
from typing import Callable, Dict, List


class Phase(Enum):
    APPROACH = "approach"
    PRE_GRASP = "pre_grasp"
    CONTACT = "contact"
    LOAD_TRANSFER = "load_transfer"
    CO_MOVE = "co_move"
    PLACE = "place"
    RELEASE = "release"


@dataclass
class TeamState:
    # 中文注释：物体位姿、每台机器人状态、网络数据年龄、接触力都应带时间戳
    object_pose: object
    robot_state: Dict[str, object]
    contact_force: Dict[str, float]
    data_age_ms: Dict[str, float]
    support_margin: Dict[str, float]


@dataclass
class Transition:
    src: Phase
    dst: Phase
    guard: Callable[[TeamState], bool]
    reason: str


def contact_ready(state: TeamState) -> bool:
    # 中文注释：接触阶段不能只看末端位置，还要看接触力和网络新鲜度
    force_ok = all(8.0 < f < 30.0 for f in state.contact_force.values())
    network_ok = all(age < 80.0 for age in state.data_age_ms.values())
    support_ok = all(margin > 0.04 for margin in state.support_margin.values())
    return force_ok and network_ok and support_ok


def build_task_graph() -> List[Transition]:
    return [
        Transition(Phase.APPROACH, Phase.PRE_GRASP,
                   lambda s: True,
                   "两机到达预抓取区域"),
        Transition(Phase.PRE_GRASP, Phase.CONTACT,
                   contact_ready,
                   "接触力、支撑裕度和数据新鲜度均满足要求"),
        Transition(Phase.CONTACT, Phase.LOAD_TRANSFER,
                   lambda s: min(s.contact_force.values()) > 12.0,
                   "开始逐步转移负载"),
        Transition(Phase.LOAD_TRANSFER, Phase.CO_MOVE,
                   lambda s: all(m > 0.06 for m in s.support_margin.values()),
                   "负载稳定且单机仍有支撑裕度"),
    ]

这段示例的重点是把阶段切换写成可测量条件。

多机协作中最危险的切换通常发生在接触建立和负载转移，因此条件要同时包含力、姿态、网络和队友确认。

⚠️ 常见陷阱¶

⚠️ 用时间代替条件

错误想法：3 秒后必然进入搬运阶段。

典型现象：一台机器人未夹紧，另一台已经抬起。

根本原因：接触建立是事件，不是纯时间过程。

正确做法：用力阈值、位姿误差和双方确认共同触发。

⚠️ 合同过窄

错误想法：把目标、姿态、力都设成硬等式。

典型现象：局部 MPC 经常不可行。

根本原因：协作对象有柔性，测量有误差，网络有延迟。

正确做法：给合同设置容许管和优先级。

⚠️ 角色固定不变

错误想法：一台机器人永远主导，另一台永远跟随。

典型现象：过窄门、转弯或放置阶段出现不必要的拉扯。

根本原因：不同阶段对视野、力矩和工作空间的要求不同。

正确做法：角色随阶段和能力裕度切换。

练习¶

为“双机搬箱上台阶”画 6 个任务图节点和每条边的进入条件。
设计一个承重者和导向者角色分配表，说明每个角色的输入输出。
把物体轨迹 \(T_{wo}(s)\) 转成左机和右机的末端合同。

任务合同把“谁做什么”说清楚，但协作搬运还需要解释“力如何在多台机器人之间分配”。

98.3 协同搬运动力学：抓取矩阵、内力与虚拟耦合 ⭐⭐⭐⭐¶

动机：物体运动由合 wrench 决定¶

协作搬运不是两个末端都跟踪同一条线。

更准确地说，多台机器人通过接触点对物体施加 wrench，这些 wrench 合成物体所受的总 wrench。

物体运动由总 wrench 决定。

物体是否被夹坏、滑落或拉扯，则由内力决定。

这两个量必须分开。

如果只关心合 wrench，夹持内力可能过大。

如果只关心每个夹爪的力，物体可能无法按目标轨迹运动。

抓取矩阵¶

设第 \(i\) 个接触点相对物体坐标系的位置为 \(r_i\)。

物体 wrench 采用“上力矩、下合力”的顺序：

\[ w_o= \begin{bmatrix} \tau_o\\ F_o \end{bmatrix} \in \mathbb{R}^6 \]

如果第 \(i\) 个接触点只提供三维接触力 \(F_i\in\mathbb{R}^3\)，而不提供接触力矩，则将所有接触力堆叠为：

\[ f_c=[F_1^\top,F_2^\top,\ldots,F_N^\top]^\top\in\mathbb{R}^{3N} \]

抓取矩阵 \(G\) 把接触力映射为物体 wrench：

\[ w_o=G f_c \]

物体动力学为：

\[ M_o\dot v_o+h_o=G f_c+f_{\text{ext}} \]

抓取矩阵的每个块可以写成：

\[ G_i= \begin{bmatrix} [r_i]_\times\\ I_3 \end{bmatrix} \]

这表示接触力 \(F_i\) 对物体产生线力，同时通过力臂 \(r_i\) 产生力矩。\([r_i]_\times\) 是 \(r_i\) 的反对称矩阵（skew-symmetric matrix），它实现了叉积运算：\([r_i]_\times F_i = r_i \times F_i\)，即力臂与力的叉积产生力矩。

为什么 \(G_i\) 是这个形式？ 这可以从刚体静力学推导。接触力 \(F_i\) 作用在物体上距质心 \(r_i\) 处。根据力的等效搬移定理，这个力等效于：在质心处施加同样的力 \(F_i\)（产生线加速度），加上一个力矩 \(r_i \times F_i\)（产生角加速度）。把这两项堆叠为 6D wrench，就得到 \(G_i\)。

完整抓取矩阵示例：对双端夹持长杆（两个接触点），如果杆长 \(L\)，两端相对物体中心的位置为 \(r_1 = (-L/2, 0, 0)^T\) 和 \(r_2 = (L/2, 0, 0)^T\)，则：

\[ G = \begin{bmatrix} [r_1]_\times & [r_2]_\times \\ I_3 & I_3 \end{bmatrix} \in \mathbb{R}^{6 \times 6} \]

当 \(L\) 足够长时，\(G\) 满秩——两个三维力可以产生任意 6D 物体 wrench。这意味着双端夹持可以完全控制长杆的运动。但如果两个接触点距离太近（\(L \to 0\)），\(G\) 的秩会下降——两个力几乎在同一点，无法产生独立的力矩，协作变得不稳定。

回顾足式/90_WBC分层优化与TSID 中零空间投影的概念：\(G\) 的零空间表示不改变物体运动的力分量。在那里我们用零空间完成次要任务；在这里，零空间对应**内力**——这个类比帮助理解为什么多机协作中的内力控制与 WBC 中的优先级控制本质相同。

如果夹爪还能施加局部接触力矩 \(\tau_i\)，则应先把每个接触点的 6D 局部 wrench 通过 wrench 坐标变换映射到物体坐标系，再堆叠成 \(6N\) 维接触 wrench。本节使用三维点接触力版本，是因为协作搬运和多机夹持中最常见的力分配问题正是这一形式。

内力¶

接触力可以分解为两部分：

\[ f_c=G^\dagger w_o+N_G f_{\text{int}} \]

其中 \(G^\dagger\) 是 \(G\) 的伪逆，\(N_G\) 是 \(G\) 的零空间基。

为什么可以这样分解？ 这是线性代数中基本子空间定理的直接应用。\(\mathbb{R}^{3N}\)（接触力空间）可以分解为 \(G\) 的行空间和零空间的正交直和：

\[ \mathbb{R}^{3N} = \text{Row}(G) \oplus \text{Null}(G) \]

\(G^\dagger w_o\) 位于 \(G\) 的行空间——它是产生目标物体 wrench \(w_o\) 的**最小范数**接触力（回顾足式/90 中伪逆的最小范数性质）。\(N_G f_{\text{int}}\) 位于 \(G\) 的零空间——因为：

\[ G N_G=0 \]

所以 \(N_G f_{\text{int}}\) 不改变物体运动。

它就是内力。

类比：内力分解与 WBC 中的零空间投影完全同构。在 WBC 中，高优先级任务使用关节空间的一部分，次要任务只能在零空间中"活动"。在多机协作中，物体运动使用接触力空间的一部分，内力只能在零空间中"活动"。两者的数学结构相同——都是"一次空间 + 正交补空间"的分解。

内力为什么重要？ 内力不是坏事。适度内力提供夹持稳定。考虑两台机器人水平夹持一个盒子：如果没有内力（两侧夹持力为零），盒子靠重力下滑时没有摩擦力阻止。如果施加适当的内力（两侧向内挤压），摩擦力 \(\mu F_{int}\) 可以抵消重力。

过大内力会导致物体变形、夹爪打滑（法向力过大导致打滑风险看似降低，但挤压产生的力矩可能导致姿态失控）、电机过热或基座失稳。

多机搬运中的一个关键控制目标是把内力维持在安全区间，而不是把它完全压到零。具体的安全区间取决于物体材料强度、夹爪摩擦系数和任务要求：

\[ f_{\text{int}}^{\min} \leq \|N_G f_{\text{int}}\| \leq f_{\text{int}}^{\max} \]

其中 \(f_{\text{int}}^{\min}\) 由防滑条件决定，\(f_{\text{int}}^{\max}\) 由物体强度和电机力矩裕度决定。

虚拟耦合¶

真实系统不可能满足完美刚体约束。

抓取点有标定误差。

物体有微小柔性。

网络有延迟。

单机状态估计有漂移。

因此，工程中常常使用虚拟弹簧和虚拟阻尼：

\[ f_i=K_i e_i+D_i\dot e_i \]

这里 \(e_i\) 是第 \(i\) 台机器人末端相对合同的误差。

在 SE(3) 上，可以写成：

\[ e_i=\log\left((T_{o e_i}^{\text{ref}})^{-1}T_{o e_i}\right)^\vee \]

虚拟耦合的作用是用有限刚度替代无限刚度。

它允许小误差存在，从而吸收延迟和标定误差。

阻抗一致性¶

阻抗一致性不是让两台机器人具有完全相同的阻抗参数。

它是让多台机器人对物体表现出一致的等效阻抗。

例如，承重者可以有较高竖直刚度。

导向者可以有较高水平阻尼。

观察者可以在接触中保持低刚度，以减少对物体的干扰。

工程分层¶

层级	职责	输入	输出	典型频率
物体层	估计 \(M_o,h_o,G\)	抓取点、物体状态	期望合 wrench	50-200 Hz
内力层	调节 \(f_{\text{int}}\)	接触力、滑移指标	夹持力参考	100-500 Hz
阻抗层	吸收微小不同步	末端误差、力误差	末端 wrench	200-1000 Hz
关节层	实现 wrench 或位置阻抗	雅可比、动力学	关节力矩	500-1000 Hz

决策表¶

场景	推荐方法	不宜方法	判断信号
物体刚性且传感可靠	抓取矩阵 + 内力控制	纯位置同步	力矩可观测
物体柔性或接触不准	虚拟耦合	硬约束 MPC	微小误差导致大力
无末端力传感器	阻抗 + 电流估计	精确力分配	力估计噪声大
物体易碎	限制内力上界	高夹持力保守策略	夹持力本身有风险
负载较重	承重角色明确化	均匀分力	某台支撑裕度更高

代码示例：内力分解¶

import numpy as np


def damped_pinv(G, damping=1e-4):
    # 中文注释：阻尼伪逆比普通伪逆更适合接近奇异的抓取构型
    m, n = G.shape
    return G.T @ np.linalg.inv(G @ G.T + damping * np.eye(m))


def split_contact_force(G, desired_object_wrench, raw_contact_force):
    """把接触力分解为产生物体运动的部分和内力部分。"""
    G_pinv = damped_pinv(G)
    motion_force = G_pinv @ desired_object_wrench
    null_projector = np.eye(G.shape[1]) - G_pinv @ G
    internal_force = null_projector @ raw_contact_force
    return motion_force, internal_force


def internal_force_metric(G, contact_force):
    # 中文注释：该指标越大，说明越多接触力没有用于推动物体，而是在内部拉扯
    _, internal = split_contact_force(G, G @ contact_force, contact_force)
    return np.linalg.norm(internal)

⚠️ 常见陷阱¶

⚠️ 把内力全部压到零

错误想法：内力不改变物体运动，所以应完全消除。

典型现象：物体在夹爪中滑动。

根本原因：夹持需要法向内力提供摩擦裕度。

正确做法：给内力设置安全区间，而不是追求零。

⚠️ 刚性约束过强

错误想法：高刚度位置环能让两台机器人更同步。

典型现象：长杆振动，腿部被拖拽。

根本原因：真实系统存在柔性、延迟和标定误差。

正确做法：在物体坐标中使用有限刚度阻抗。

⚠️ 只看末端力

错误想法：每台机器人末端力在安全范围内，物体就安全。

典型现象：单机力都不大，但合力矩让物体旋转失控。

根本原因：物体受的是所有接触 wrench 的合成结果。

正确做法：记录 \(G f_c\) 和 \(N_G f_c\)，同时看合 wrench 与内力。

练习¶

给双端夹持长杆写出 \(G\in\mathbb R^{6\times 6}\) 或 \(G\in\mathbb R^{6\times 12}\) 的物理含义。
解释 \(G\) 的零空间为什么对应内力。
设计一个阻抗参数调度：起步低刚度、稳定搬运中刚度、放置前低刚度。

物体动力学说明了协作的物理核心；如果把所有机器人放入同一个优化问题，就得到集中式联合 MPC。

98.4 集中式联合 MPC：理论最优与维度爆炸 ⭐⭐⭐¶

动机：集中式是最清楚的数学上界¶

集中式联合 MPC 把所有机器人和物体放进一个 OCP。

它的优点非常明显。

所有变量在同一个优化器中。

所有约束同时可见。

物体动力学、队形约束、机器人动力学、摩擦锥、自碰撞和环境碰撞都可以统一表达。

如果计算资源无限，通信绝对可靠，状态同步没有延迟，那么集中式方案通常是最直接的选择。

但真实机器人没有这些条件。

联合 OCP¶

联合状态：

\[ X=[x_o,x_1,x_2,\ldots,x_N] \]

联合输入：

\[ U=[u_1,u_2,\ldots,u_N] \]

目标函数：

\[ \min_{X,U}\int_0^T \ell_o(x_o)+\sum_i \ell_i(x_i,u_i)+\ell_{\text{sync}}(X)+\ell_{\text{safe}}(X,U)\,dt \]

约束包括：

\[ \dot x_i=f_i(x_i,u_i) \]

\[ M_o\dot v_o+h_o=G f_c+f_{\text{ext}} \]

\[ T_{w e_i}(q_i)=T_{w o}T_{o e_i}^{\text{grasp}} \]

\[ f_{c,i}\in \mathcal K_{\text{fric}} \]

\[ d_{\text{collision}}(X)\ge d_{\min} \]

这个问题形式优雅，但维度很快爆炸。

维度估算¶

系统	单机 \(n_v\)	双机 \(n_v\)	物体状态	联合优化压力
双 Go2	18	36	12	中
双 Go2+Z1	24	48	12	高
双 G1 29-DoF	35 左右	70 左右	12	很高
双 G1 + 灵巧手	50+	100+	12	极高

即使采用 centroidal dynamics 简化，接触力、末端约束和碰撞约束也会快速增加。

集中式联合 MPC 更适合三个场景。

第一，小规模强耦合任务。

第二，离线轨迹生成或仿真基准。

第三，局部短时协调，例如两台机器人共同抬起物体的前 0.5 秒。

与单机复合 MPC 的关系¶

单机复合 MPC 已经面对行走稳定和末端操作之间的权衡。

多机集中式 MPC 又增加了机器人之间的同步和内力。

可以把单机问题看成：

\[ J=J_{\text{locomotion}}+J_{\text{manipulation}}+J_{\text{safety}} \]

多机问题则变成：

\[ J=J_{\text{object}}+\sum_i J_{\text{robot},i}+J_{\text{sync}}+J_{\text{internal}}+J_{\text{communication}} \]

其中 \(J_{\text{communication}}\) 不是传统 MPC 中常见的物理项。

它表示数据年龄、同步误差和通信可用性带来的约束收紧。

工程分层¶

层级	职责	输入	输出	典型频率
建模层	构建联合状态和约束	URDF、物体参数	OCP 描述	离线
求解层	SQP/DDP/采样 MPC	当前联合状态	联合轨迹	10-50 Hz
下发层	拆分联合解	联合策略	单机参考	50-200 Hz
执行层	WBC 跟踪	单机参考	关节命令	500-1000 Hz

决策表¶

决策因素	适合集中式	不适合集中式
机器人数量	2 台，最多 3 台	3 台以上且强耦合
网络	局域网稳定，时钟同步好	无线网络尾延迟大
任务	抬起、放置、短时强协作	长时间巡检、分散操作
计算	工控机或工作站	嵌入式算力紧张
安全	可离线验证关键段	需要长期在线自治

⚠️ 常见陷阱¶

⚠️ 把集中式当作唯一方案

错误想法：理论最优就应直接部署。

典型现象：求解超时，通信断连后全局停摆。

根本原因：最优性和可用性不是同一个指标。

正确做法：把集中式用于基准、局部短时任务或上层计划。

⚠️ 忽略同步采样

错误想法：把各机器人最近一次状态拼成联合状态即可。

典型现象：优化器追踪一个真实世界中从未同时存在的系统状态。

根本原因：多机测量天然异步。

正确做法：按时间戳插值或丢弃过期状态。

练习¶

估算双 Go2+Z1 集中式 OCP 的状态维度和输入维度。
列出集中式 MPC 的三种适用场景和三种不适用场景。
把集中式 OCP 的约束按单机、物体、通信、安全四类归类。

集中式模型给出了全局视角，但实时工程经常需要把它拆成可并行的局部优化。

98.5 分布式 ADMM 与一致性协调 ⭐⭐⭐⭐¶

动机：共享低维耦合变量¶

分布式优化的目标是让每台机器人解自己的问题，同时通过少量共享变量保持一致。

共享变量可以是物体位姿短轨迹。

共享变量也可以是物体 twist、接触 wrench、进度变量或队形中心。

关键是不要共享所有关节轨迹。

如果每台机器人都上传完整状态和完整控制轨迹，分布式方法很快退化成低效的集中式方法。

ADMM 形式与推导¶

ADMM 的来源：ADMM（Alternating Direction Method of Multipliers）结合了对偶分解（dual decomposition）的可分解性和增广拉格朗日（augmented Lagrangian）的收敛稳定性。它最早由 Glowinski & Marroco (1975) 和 Gabay & Mercier (1976) 独立提出，Boyd et al. (2011) 的综述使其在分布式优化中广泛应用。

问题形式：多机协作优化可以写成一致性约束形式——每台机器人有自己的局部变量 \(y_i\)（包含本机轨迹、力矩等），但必须对一个共享变量 \(z\)（如物体 twist 轨迹）达成一致：

\[ \min_{y_1, \ldots, y_N, z} \sum_{i=1}^N J_i(y_i) \quad \text{s.t.} \quad A_i y_i = z, \quad i = 1, \ldots, N \]

直接求解这个耦合问题需要集中式求解器。ADMM 的核心思想是：用增广拉格朗日替代原始约束，然后**交替**优化局部变量和全局变量。

**增广拉格朗日**为：

\[ L_\rho = \sum_i J_i(y_i) + \sum_i \lambda_i^T (A_i y_i - z) + \frac{\rho}{2} \sum_i \|A_i y_i - z\|^2 \]

其中 \(\lambda_i\) 是对偶变量，\(\rho > 0\) 是惩罚参数。第三项是增广项——它让优化landscape更加光滑，改善收敛性。

ADMM 的三步迭代：

局部步（每台机器人并行求解）：

\[ y_i^{k+1}= \arg\min_{y_i} J_i(y_i)+ \frac{\rho}{2}\|A_i y_i-z^k+\lambda_i^k\|^2 \]

这一步中，每台机器人只需要知道上一轮的全局变量 \(z^k\) 和自己的对偶变量 \(\lambda_i^k\)——不需要知道其他机器人的状态。增广项 \(\frac{\rho}{2}\|A_i y_i - z^k + \lambda_i^k\|^2\) 的作用是"把局部解拉向全局一致"。

一致性步（中心节点或分布式协商）：

\[ z^{k+1}= \Pi_{\mathcal Z} \left( \frac{1}{N}\sum_i(A_i y_i^{k+1}+\lambda_i^k) \right) \]

\(\Pi_{\mathcal Z}\) 是到可行集 \(\mathcal{Z}\) 的投影（如物体位姿约束）。如果 \(\mathcal{Z} = \mathbb{R}^n\)，投影就是简单平均。

对偶更新（每台机器人独立更新）：

\[ \lambda_i^{k+1}= \lambda_i^k+A_i y_i^{k+1}-z^{k+1} \]

对偶变量累积了"局部解与全局一致的差距"。它的作用类似于积分控制——即使局部步没有完全满足一致性约束，对偶变量会逐步增大惩罚，把局部解拉回一致。

本质洞察：ADMM 的三步可以理解为"局部自主决策 → 全局民主协商 → 记住分歧以便下次修正"。这与多机协作的工程需求天然吻合——每台机器人有自己的动力学和局部感知（局部步），需要就物体运动达成一致（一致性步），且要记住历史分歧以改善未来协调（对偶更新）。

在控制中，ADMM 通常不会迭代到严格收敛。

更常见的做法是固定 1-3 轮迭代，然后只执行第一小段控制。

下一周期重新测量、重新 warm-start。

这就是分布式 receding horizon。

收敛性：ADMM 在凸问题上保证收敛到全局最优。但多机行走操作的动力学是非凸的。实践中，热启动 + 少量迭代 + receding horizon 的组合在大多数情况下足够——每个控制周期的 ADMM 残差不需要为零，只要随时间缓慢下降即可。如果残差持续增大，说明系统在发散——应触发降级。

共享变量如何选择¶

共享变量	维度	优点	缺点
物体位姿轨迹	\(6N_h\)	直观，任务一致性强	对姿态误差敏感
物体 twist 轨迹	\(6N_h\)	平滑，适合搬运	需要积分得到位姿
接触 wrench 分配	\(6N N_h\)	力一致性好	需要力估计
任务进度 \(s\)	\(N_h\)	带宽低	无法表达力和姿态
队形中心	\(3N_h\)	适合松耦合	不足以处理刚性物体

双机搬运长杆时，物体 twist 是一个很实用的共享变量。

它比完整物体位姿轨迹更平滑。

它比完整接触力分配维度更低。

对于力敏感任务，可以同时共享物体 twist 和低频接触力摘要。

能力加权一致性¶

简单平均并不总是合理。

如果一台机器人力矩裕度大、定位更准、通信更新更及时，它在一致性变量中的权重应更高。

可以定义：

\[ w_i=\alpha m_i+\beta c_i+\gamma r_i \]

其中 \(m_i\) 是力矩裕度，\(c_i\) 是定位置信度，\(r_i\) 是通信新鲜度。

一致性更新变成加权平均：

\[ z=\frac{\sum_i w_i z_i}{\sum_i w_i} \]

这个公式很简单，但工程效果明显。

它避免弱单元把全队拖入不可行状态。

工程分层¶

层级	职责	输入	输出	典型频率
局部优化	每机求自己的 OCP	本体状态、局部地图、上一轮 \(z\)	局部候选轨迹	10-100 Hz
一致性更新	融合共享变量	局部候选、权重	全局 \(z\)	10-100 Hz
对偶更新	累计不一致代价	局部残差	对偶变量	10-100 Hz
执行保护	只执行第一小段	最新合同	局部命令	500-1000 Hz

代码示例：分布式协调循环¶

import numpy as np


class DistributedCoordinator:
    def __init__(self, rho: float, horizon: int):
        self.rho = rho
        self.horizon = horizon
        self.z_object = np.zeros((horizon, 6))
        self.dual = {}

    def local_solve(self, robot_id, local_state, contract):
        # 中文注释：真实实现中这里调用本机 MPC；示例只保留接口
        local_plan = np.zeros_like(self.z_object)
        local_force = np.zeros((self.horizon, 6))
        return local_plan, local_force

    def consensus_update(self, local_plans, weights):
        # 中文注释：能力强、力矩裕度高、定位更准的机器人权重可以更大
        total = np.zeros_like(self.z_object)
        denom = 0.0
        for rid, plan in local_plans.items():
            w = weights.get(rid, 1.0)
            total += w * plan
            denom += w
        return total / max(denom, 1e-6)

    def run_one_round(self, team_state, contracts, weights):
        local_plans = {}
        local_forces = {}
        for rid, contract in contracts.items():
            local_plans[rid], local_forces[rid] = self.local_solve(
                rid, team_state.robot_state[rid], contract
            )

        z_new = self.consensus_update(local_plans, weights)

        for rid, plan in local_plans.items():
            self.dual[rid] = self.dual.get(rid, 0.0) + plan - z_new

        self.z_object = z_new
        return z_new, local_forces

决策表¶

问题	优先选择	不宜选择	判断信号
通信带宽有限	共享低维 \(z\)	共享完整状态轨迹	网络拥塞
实时预算紧	固定少量迭代	追求完全收敛	残差下降但超时
机器人能力不同	加权平均或主从融合	简单算术平均	弱单元残差大
接触力难测	共享物体 twist	共享完整 wrench	力估计噪声大
任务松耦合	共享进度或区域	强制物体级一致	机器人无共同物体

⚠️ 常见陷阱¶

⚠️ 一致性变量选错

错误想法：把所有关节轨迹都作为共享变量最精确。

典型现象：通信量巨大且收敛慢。

根本原因：协作只需要共享耦合变量。

正确做法：优先共享物体状态、wrench 或进度。

⚠️ 把 ADMM 收敛当成控制稳定

错误想法：残差小就可以提高执行速度。

典型现象：系统仍因延迟或接触冲击失稳。

根本原因：优化一致不等于闭环鲁棒。

正确做法：叠加局部 WBC 稳定约束和安全滤波。

练习¶

为双机长杆搬运选择三种可能的共享变量，并比较维度。
写出两轮 ADMM 后每台机器人需要发送的数据。
讨论固定 2 次迭代与收敛到阈值两种策略在实时控制中的取舍。

分布式优化能降低集中式压力，但它把通信质量变成控制系统的一部分。

98.6 通信延迟、时钟偏差与数据新鲜度 ⭐⭐⭐⭐¶

动机：网络是控制系统的一部分¶

多机协作中，网络不是透明管道。

它有延迟。

它有抖动。

它会丢包。

它还有时钟偏差。

控制器真正使用的不是“最新数据”，而是带时间戳的数据。

因此要定义数据年龄：

\[ a_i(t)=t-t_i^{\text{stamp}} \]

如果 \(a_i(t)\) 超过门限，就不应继续把这条数据当作当前状态。

延迟对协作的影响¶

假设搬运速度为 \(0.5\ \text{m/s}\)。

如果队友状态延迟 50 ms，则位置误差约为：

\[ 0.5\times0.05=0.025\ \text{m} \]

也就是 2.5 cm。

对单机导航而言，这可能可以接受。

对双机刚性抓取而言，2.5 cm 已经足以产生明显内力。

如果末端阻抗刚度为 \(1000\ \text{N/m}\)，这 2.5 cm 误差对应约 25 N 的附加力。

这还没有考虑旋转误差和力臂。

因此，高频力协作不能跨网络闭环。

高频力控必须留在本机。

网络只应传低频意图、物体状态、接触摘要和合同更新。

延迟补偿¶

短时延迟可以用预测补偿：

\[ \hat x_j(t)=x_j(t-d)+\dot x_j(t-d)d \]

这是一阶常速度预测。

它适合短时间、低加速度的协作搬运。

如果延迟过长，继续外推会比丢弃数据更危险。

这时应进入降级模式。

QoS 选择¶

ROS 2 或 DDS 中，常见 QoS 包括 reliable、best effort、deadline、lifespan 和 history。

多机协作中，状态摘要和合同更新通常适合 reliable。

高频可视化点云通常适合 best effort。

过期合同应使用 lifespan。

控制层应检查 deadline miss。

工程分层¶

层级	职责	输入	输出	典型频率
时间层	同步时钟和时间戳	PTP/NTP、硬件触发	统一时间基	1-100 Hz
网络层	统计延迟与丢包	DDS/UDP/TCP 数据	延迟分布	10-100 Hz
预测层	补偿短时延迟	过期状态、速度	队友当前估计	50-200 Hz
降级层	处理超时和断连	数据年龄、残差	安全动作	10-100 Hz

代码示例：数据新鲜度滤波¶

#include <array>

struct StampedPose {
  double stamp_sec = 0.0;
  std::array<double, 3> position{0.0, 0.0, 0.0};
  std::array<double, 3> velocity{0.0, 0.0, 0.0};
};

struct FreshnessResult {
  bool usable = false;
  double age_ms = 1e9;
  StampedPose predicted;
};

FreshnessResult predict_if_fresh(const StampedPose& msg,
                                 double now_sec,
                                 double max_age_ms) {
  FreshnessResult out;
  out.age_ms = (now_sec - msg.stamp_sec) * 1000.0;
  out.usable = out.age_ms >= 0.0 && out.age_ms < max_age_ms;
  out.predicted = msg;

  if (!out.usable) {
    return out;
  }

  // 中文注释：用常速度模型补偿短时延迟，长时延迟交给降级状态机处理
  const double dt = now_sec - msg.stamp_sec;
  for (int k = 0; k < 3; ++k) {
    out.predicted.position[k] += msg.velocity[k] * dt;
  }
  return out;
}

决策表¶

场景	通信策略	不宜策略	原因
低带宽长距离	发送意图和低频状态	发送高频关节数据	丢包率上升
局域网强协作	reliable + deadline	无时间戳话题	同步误差小但仍需检查
高频力协同	本地力闭环为主	跨网络闭环力控	网络尾延迟不可控
大点云共享	best effort + 压缩	reliable 全量点云	旧点云无控制价值
合同更新	reliable + lifespan	永久有效消息	合同过期很危险

⚠️ 常见陷阱¶

⚠️ 只看平均延迟

错误想法：平均 5 ms 就安全。

典型现象：偶发 80 ms 导致物体猛拉。

根本原因：控制失败常由尾部延迟触发。

正确做法：记录 p50、p90、p99 和连续丢包长度。

⚠️ 跨网络做高频力闭环

错误想法：队友力传感器可以直接用于本机 500 Hz 控制。

典型现象：力环振荡。

根本原因：网络延迟接近或超过力控相位裕度。

正确做法：高频力控留在本机，网络只传低频意图。

练习¶

设计一个数据新鲜度门限表：物体位姿、队友基座、队友末端、接触力各自门限是多少。
推导 50 ms 延迟下，0.5 m/s 搬运速度造成的位姿预测误差。
解释 reliable QoS 与 best effort QoS 在协作搬运中的取舍。

通信质量不可控，因此多机系统必须具备局部自治，而不是等待全局层永远可用。

98.7 局部自治与全局协调：合同、权限与降级 ⭐⭐⭐⭐¶

动机：断连时不能保持最后一个危险命令¶

局部自治是多机协作的安全底座。

每台机器人必须能在没有新全局命令时保持平衡、限制力、保护物体和保护自身。

全局协调负责目标一致和资源分配。

局部自治负责毫秒级安全。

这两个职责不能互相替代。

如果全局层直接发布关节角或力矩，局部控制器就很难处理突然出现的障碍、支撑裕度下降和队友断连。

如果每台机器人完全自治，又可能出现队形撕裂、物体目标冲突和任务停滞。

合同是两者之间的边界。

四类权限¶

权限	含义	谁应拥有
任务权限	选择阶段、角色和目标	全局任务层
运动权限	在合同内选择局部轨迹	单机 MPC
接触权限	调整阻抗、力和释放动作	单机监督层
急停权限	立即保护硬件和人	本机硬件保护层

急停权限必须本地化。

不能等待全局确认。

接触权限也应尽量本地化。

因为接触状态变化往往比网络更新更快。

局部安全动作¶

局部自治至少要有四个安全动作：

保持。
顺应。
放置或释放。
撤退。

保持表示暂停物体进度，同时维持当前稳定姿态。

顺应表示降低末端刚度，限制内力增长。

放置表示寻找最近安全支撑面，缓慢降低物体。

释放表示卸载力后松开夹持。

撤退表示远离物体或危险区域。

这些动作都必须在仿真中演练。

不能只写在接口说明中。

合同有效性¶

合同有效性可以写成：

\[ valid_i=(a_i<a_{\max})\land(h_i(x_i)>0)\land(r_i<r_{\max}) \]

其中 \(a_i\) 是数据年龄，\(h_i\) 是本机安全裕度，\(r_i\) 是合同残差。

如果合同失效，本机不应继续追踪合同目标。

它应进入降级动作。

工程分层¶

层级	职责	输入	输出	典型频率
全局协调	任务分配与目标一致	任务图、状态摘要	合同	1-10 Hz
局部监督	判断合同有效性	合同、传感器、网络状态	执行模式	50-200 Hz
局部控制	跟踪或降级	执行模式、局部状态	力矩	500-1000 Hz
硬件保护	电机限幅和急停	电流、温度、姿态	断电、抱闸或软停	1 kHz+

代码示例：局部自治状态机¶

from enum import Enum


class LocalMode(Enum):
    TRACK_CONTRACT = 0
    HOLD_OBJECT = 1
    COMPLIANT_HOLD = 2
    PLACE_OBJECT = 3
    RELEASE_OBJECT = 4
    RETREAT = 5


class LocalSupervisor:
    def __init__(self):
        self.mode = LocalMode.TRACK_CONTRACT

    def update(self, risk):
        # 中文注释：risk 是监控层给出的结构化风险，不是单一布尔值
        if risk.robot_fallen:
            self.mode = LocalMode.RELEASE_OBJECT
        elif risk.peer_fallen:
            self.mode = LocalMode.PLACE_OBJECT
        elif risk.data_age_ms > 500.0:
            self.mode = LocalMode.COMPLIANT_HOLD
        elif risk.internal_force > risk.internal_force_limit:
            self.mode = LocalMode.COMPLIANT_HOLD
        elif risk.support_margin < 0.02:
            self.mode = LocalMode.HOLD_OBJECT
        else:
            self.mode = LocalMode.TRACK_CONTRACT
        return self.mode

    def command_policy(self):
        if self.mode == LocalMode.TRACK_CONTRACT:
            return "执行合同轨迹，同时保持局部避障和稳定约束"
        if self.mode == LocalMode.HOLD_OBJECT:
            return "停止物体进度，保持当前支撑和低速姿态调整"
        if self.mode == LocalMode.COMPLIANT_HOLD:
            return "降低末端刚度，限制内力增长"
        if self.mode == LocalMode.PLACE_OBJECT:
            return "寻找最近安全放置区域，缓慢降低物体"
        if self.mode == LocalMode.RELEASE_OBJECT:
            return "卸载力并松开夹持，保护本体平衡"
        return "撤退到安全站位"

决策表¶

场景	推荐降级	不宜动作	判断信号
通信短暂超时	保持 + 低刚度顺应	立即松手	物体仍稳定
队友跌倒	放置或释放物体	继续跟踪物体轨迹	队友姿态越界
内力上升	降低刚度并暂停进度	加快完成任务	\(N_G f_c\) 增大
本机支撑裕度低	保持或撤退	增大末端力	CoP 接近边界
全局目标冲突	局部安全优先	服从最新命令	合同残差增大

⚠️ 常见陷阱¶

⚠️ 全局层过度控制

错误想法：全局层知道全局目标，所以可以发布关节角或力矩。

典型现象：局部避障与稳定恢复失效。

根本原因：全局层频率和信息都不足以做毫秒级控制。

正确做法：全局层发布合同，局部层自行优化。

⚠️ 降级动作未演练

错误想法：安全放下物体是很简单的动作。

典型现象：真实故障时动作不可行。

根本原因：降级也是控制任务，必须仿真和实测。

正确做法：把保持、顺应、释放、撤退做成可测试状态。

练习¶

为双机搬运设计 6 个局部执行模式，并写出进入条件。
解释为什么急停不是唯一安全动作。
设计队友断连 0.5 s、2 s、5 s 三档降级策略。

局部自治解决了安全边界，但多机定位还要求所有机器人对世界和物体有一致坐标理解。

98.8 多机一致定位与 anchor SLAM ⭐⭐⭐¶

动机：协作需要共享参考系¶

多机协作要求“我看到的物体”和“队友看到的物体”在同一个坐标系统中解释。

单机 SLAM 只保证本机轨迹自洽。

多机任务需要跨机器人位姿一致。

anchor frame 的作用是给协作任务提供一个稳定、可共享的参考系。

anchor 可以是固定 AprilTag。

anchor 可以是已知地图框架。

anchor 可以是稳定地标。

anchor 也可以是多机位姿图优化出的任务参考系。

多机位姿图¶

多机位姿图包含：

每台机器人的轨迹节点。
本机里程计边。
回环边。
机器人之间的相对观测边。
共同观测物体产生的地标边。

相对约束可以写成：

\[ e_{ij}=\log\left(Z_{ij}^{-1}T_i^{-1}T_j\right) \]

其中 \(Z_{ij}\) 是第 \(i\) 台机器人对第 \(j\) 台机器人或共同地标的相对观测。

如果两个机器人共同观察同一个物体，则物体地标也能帮助对齐坐标系。

任务不一定需要完整地图合并¶

多机协作不总需要把所有地图合并成一张巨大地图。

双机搬运长杆时，任务真正需要的是：

两台机器人的相对位姿。
物体位姿。
搬运路径附近的障碍和可落足区域。
这些量的协方差和时间戳。

完整地图合并可能浪费带宽。

任务相关的局部一致性通常更重要。

协方差传播¶

控制层不应只接收 pose 均值。

它还应接收协方差。

如果物体 pose 由机器人 pose 和相机观测组合得到：

\[ T_{a o}=T_{a i}T_{i o} \]

协方差可线性近似传播：

\[ \Sigma_{a o}=J_i\Sigma_iJ_i^\top+J_z\Sigma_zJ_z^\top \]

协方差越大，协调层越应降低刚度、增加安全裕度或触发主动观察。

工程分层¶

层级	职责	输入	输出	典型频率
本机里程计	高频局部状态	IMU、关节、视觉	本机 pose	100-400 Hz
多机约束	相对位姿和共同观测	相机、LiDAR、标志物	因子图边	1-30 Hz
一致优化	融合多机轨迹	因子图	anchor pose	1-10 Hz
控制接口	输出任务 frame 和协方差	anchor、物体观测	参考与裕度	10-50 Hz

决策表¶

场景	推荐方案	不宜方案	判断信号
室内实验	AprilTag/固定 anchor + SLAM	只靠轮速里程计	可布置环境
大范围巡检	多机回环 + 相对观测	全量地图同步	带宽受限
物体是任务核心	物体地标进入图优化	把物体当外点丢弃	物体被持续观测
短时搬运	局部 anchor 即可	全局地图合并	任务区域小
anchor 不稳定	固定任务 frame 并平滑更新	频繁切换 anchor	控制目标跳变

⚠️ 常见陷阱¶

⚠️ 只共享地图不共享不确定性

错误想法：pose 坐标够用。

典型现象：MPC 对漂移区域过于自信。

根本原因：控制需要知道可信程度。

正确做法：传递均值和协方差，并按协方差收紧约束。

⚠️ anchor 频繁切换

错误想法：看到哪个参考就用哪个。

典型现象：物体目标跳变。

根本原因：控制层不适合承受坐标跳变。

正确做法：对 anchor 切换做平滑过渡或固定任务 anchor。

练习¶

画一个双机位姿图，包含本机里程计边、相对观测边、物体地标边。
推导两台机器人共同观测同一物体时，物体位姿协方差如何影响协调层权重。
解释为什么多机协作不总需要完整地图合并。

坐标一致让协作成为可能；行为编排则决定系统在复杂场景中如何推进和恢复。

98.9 行为编排、恢复与安全不变量 ⭐⭐⭐¶

动机：恢复是任务设计的一部分¶

多机协作任务通常持续时间长、阶段多、失败模式多。

抓取可能失败。

队友可能迟到。

物体可能被卡住。

网络可能短暂断连。

局部地形可能不可落足。

如果只写主流程，系统第一次遇到真实世界就会停在中间。

恢复策略不是异常处理，而是任务设计的一部分。

状态机与行为树¶

有限状态机适合强时序任务。

例如双机搬运的阶段顺序非常明确。

行为树适合多种恢复动作和可组合技能。

例如家庭整理任务中，机器人可能需要重新观察、重新抓取、绕开障碍或请求队友接管。

在多机协作中，两者可以结合。

上层使用状态机保证物理阶段顺序。

每个阶段内部使用行为树处理恢复。

安全不变量¶

安全不变量是任何阶段都必须维持的条件。

常见不变量包括：

本机支撑裕度大于阈值。
物体内力低于阈值。
物体高度和速度在安全范围内。
队友距离不小于安全距离。
数据年龄小于门限。
末端接触力不超过夹爪或物体限制。
关节力矩和温度在安全范围内。

可以定义安全集合：

\[ \mathcal S= \{x\mid h_{\text{support}}>0,\ f_{\text{int}}<f_{\max},\ a<a_{\max}\} \]

恢复目标不是继续追踪原轨迹。

恢复目标是回到安全集合内部。

恢复动作优先级¶

风险	首选恢复	次选恢复	禁止动作
内力过大	降低刚度并暂停	重新标定抓取点	加大位置刚度
队友失稳	放置物体	释放并撤退	继续拉动物体
本机支撑边界小	停止物体进度	调整脚步或姿态	加速搬运
物体接触环境	降低速度	重新规划路径	强行通过
目标 pose 不可信	主动观察	降低追踪权重	高刚度追踪

工程分层¶

层级	职责	输入	输出	典型频率
编排层	阶段推进与恢复选择	任务图、状态摘要	下一阶段	1-10 Hz
监控层	计算安全不变量	状态、力、网络	风险等级	10-100 Hz
恢复层	执行保持、放置、撤退	风险等级	恢复合同	10-200 Hz
控制层	安全滤波	恢复合同	力矩命令	500-1000 Hz

⚠️ 常见陷阱¶

⚠️ 恢复追求原目标

错误想法：跌倒边缘仍应尽快回到物体轨迹。

典型现象：越修越危险。

根本原因：恢复的第一目标是回到安全集合。

正确做法：恢复目标设为扩大稳定裕度，而不是最小化原轨迹误差。

⚠️ 监控只看本机

错误想法：单机姿态正常就可以继续。

典型现象：队友已失稳仍拉动物体。

根本原因：协作风险由系统状态决定。

正确做法：监控本机、队友、物体和网络四类指标。

练习¶

设计一个“抓取失败 → 重新对齐 → 二次抓取 → 放弃”的行为树。
给多机搬运定义 5 个安全不变量。
讨论学习策略输出动作时，安全滤波应放在哪一层。

最后需要把上述理论落到工程栈：消息、线程、仿真、日志和评测。

98.10 工程实现：ROS 2、仿真栈与评测指标 ⭐⭐⭐¶

动机：协作系统必须可观测、可回放、可压测¶

多机协作不是单个算法，而是一组可观测、可回放、可压测的系统接口。

工程实现的关键是把时间戳、坐标系、合同、状态摘要和故障信号标准化。

仿真阶段应先注入延迟、丢包、标定误差和队友故障，再进入真实硬件。

只在理想仿真中测试，会高估系统稳定性。

没有统一日志，失败后无法区分控制问题、网络问题、感知问题和硬件问题。

没有指标，协作效果容易被视频观感替代。

评测指标¶

协作系统至少应记录四类指标。

第一类是任务指标。

包括任务成功率、任务时间、路径长度和放置误差。

第二类是安全指标。

包括最小支撑裕度、最大内力、最大接触力、最小碰撞距离和急停次数。

第三类是同步指标。

包括物体姿态误差、队形误差、两机进度差和合同残差。

第四类是系统指标。

包括 MPC 求解时间、WBC 周期抖动、网络 p99 延迟、连续丢包长度和日志丢帧。

可以定义同步误差：

\[ e_{\text{sync}}= \max_i\|T_{o e_i}^{\text{ref}}\ominus T_{o e_i}\| \]

可以定义内力指标：

\[ I_{\text{int}}=\|N_G f_c\| \]

可以定义网络风险：

\[ R_{\text{net}}=\alpha d_{99}+\beta p_{\text{loss}}+\gamma a_{\max} \]

实验矩阵¶

实验	变量	范围	观察指标
E1 无延迟基线	协调架构	集中式、分布式、阻抗	同步误差、内力
E2 固定延迟	单向延迟	20/50/100 ms	相位滞后、振动幅值
E3 丢包	随机丢包	1/2/5%	恢复次数、合同超时
E4 抓取误差	抓取点偏差	1/3/5 cm	内力增长、姿态误差
E5 队友故障	一机跪倒或停机	搬运中随机触发	放置成功率
E6 负载变化	杆质量	1/3/5 kg	支撑裕度、力矩裕度

⚠️ 常见陷阱¶

⚠️ 没有网络压测

错误想法：仿真中默认零延迟即可证明算法。

典型现象：真机第一次协作就振荡。

根本原因：延迟改变闭环相位。

正确做法：在仿真中注入延迟、抖动和丢包。

⚠️ 指标只看成功率

错误想法：物体到达目标就算成功。

典型现象：系统接近失稳却被算成功。

根本原因：安全裕度和任务成功同等重要。

正确做法：同时统计内力、支撑裕度、同步误差和恢复次数。

练习¶

为双 Go2+Z1 协作搬运设计 10 个日志字段。
写出一个延迟注入实验矩阵：0/20/50/100 ms 与 0/2/5% 丢包组合。
定义“安全成功”的判据，而不是只定义“到达目标”。

98.11 多机协作架构总表¶

场景	推荐架构	通信内容	单机自治要求	主要风险
双机慢速搬箱	物体中心协调 + 局部 MPC	物体位姿、进度、力摘要	保持与顺应	内力过大
双机搬长杆转弯	分布式 MPC + 队形约束	物体 twist、队形误差	局部避障和限力	旋转同步差
多人形协作抬桌	任务图 + 阻抗一致性	阶段、接触状态、力摘要	放置与释放	一机失稳拖拽
异构机器人交接	主从合同 + 视觉确认	物体 pose、接触确认	接触失败恢复	坐标漂移
大范围多机巡检操作	松耦合任务分配	任务进度、地图摘要	独立避障	任务冲突
高频推拉协作	本地力闭环 + 低频意图	低频意图与安全边界	强力控自治	网络相位滞后

98.12 故障排查手册¶

症状	可能原因	排查步骤	修复方向
物体周期性横向振动	末端阻抗太硬或延迟补偿不足	记录物体横向速度、内力、网络 p99 延迟	降低刚度、增加阻尼、缩短共享变量维度
一台机器人持续后退	角色分配错误或物体参考坐标漂移	比较 anchor 下两机的物体目标	重新估计 anchor，按能力加权分配
协作开始瞬间摔倒	负载转移太快或支撑裕度不足	查看接触建立到抬起阶段的力曲线	增加负载斜坡和支撑姿态检查
内力持续升高但物体不动	两机末端参考不一致	比较 \(T_{o e_i}^{ref}\) 与实测抓取变换	重新标定抓取点，改用柔顺模式
仿真成功真机失败	未注入延迟、丢包和柔性	在仿真回放中加入网络和接触误差	扩展扰动测试矩阵
队友断连后仍拉动物体	局部自治状态机没有接管	检查数据年龄门限和执行模式切换	加入超时保持与放置模式
物体坐标突然跳变	anchor 切换或 SLAM 回环未平滑	查看坐标变换树和时间戳	任务层固定 anchor，控制层平滑重定位
ADMM 残差不收敛	共享变量维度过大或权重不均	分解残差到各机器人和各维度	改选物体 twist 或 wrench 为共享变量

98.13 累积项目：双 Go2+Z1 协作搬运长杆¶

项目目标¶

在 MuJoCo 或 Isaac 中搭建两台四足+臂与一根长杆。

实现对象中心任务图：接近、抓取、负载转移、协同移动、放置、释放。

实现两种协调器：集中式参考协调与分布式一致性协调。

注入 0-100 ms 延迟、0-5% 丢包和 1-5 cm 抓取点误差。

评测任务成功率、内力、同步误差、支撑裕度、网络风险和恢复次数。

模块分解¶

模块	输入	输出	验收标准
object_state_estimator	杆的标记点或仿真真值	杆 pose/twist	RMS 位姿误差 < 2 cm
contract_manager	物体轨迹、抓取点、角色	单机合同	合同包含有效期和降级动作
local_mpc	单机合同、本体状态	末端和基座参考	单机不违反支撑约束
impedance_controller	末端误差、力估计	末端 wrench 或关节命令	内力低于阈值
network_emulator	延迟与丢包参数	扰动后的消息	可复现实验矩阵
supervisor	状态摘要、网络年龄、力	执行模式	断连时 0.5 s 内接管

里程碑¶

第 1 天：单机抓杆并保持阻抗稳定。
第 2 天：双机共享物体 pose，完成静态抬起。
第 3 天：加入物体轨迹和队形约束，完成直线搬运。
第 4 天：加入延迟和丢包，完成数据新鲜度降级。
第 5 天：加入队友故障，完成安全放置。
第 6-7 天：完成实验矩阵和报告。

最小验收命令示例¶

# 运行无延迟基线
uv run scripts/run_team_transport.py --scenario long_bar --delay-ms 0 --drop-rate 0.0

# 注入 50 ms 延迟和 2% 丢包
uv run scripts/run_team_transport.py --scenario long_bar --delay-ms 50 --drop-rate 0.02

# 计算协作指标
uv run scripts/evaluate_team_log.py --log runs/latest --metrics sync internal_force support network

98.14 本章小结¶

知识点	核心结论	工程检验
任务分解	多机任务应以物体和合同为中心	任务图阶段可回放
协同动力学	合 wrench 决定物体运动，内力决定夹持安全	记录 \(G f_c\) 与 \(N_G f_c\)
集中式 MPC	理论清晰但维度和单点失败突出	求解时间和状态同步可测
分布式协调	共享低维耦合变量更实用	ADMM 残差与控制安全分别验证
通信延迟	数据年龄是控制变量的一部分	统计 p50/p90/p99 和连续丢包
局部自治	全局发布合同，局部保证安全	断连降级有仿真用例
一致定位	anchor frame 与协方差必须进入控制接口	坐标跳变有平滑处理
行为恢复	恢复目标是回到安全集合	恢复动作可仿真复现

98.15 延伸阅读与代码路径¶

材料	难度	阅读重点
OCS2 legged_robot 与 mobile_manipulator	⭐⭐⭐	MPC_MRT 接口、参考管理、约束注册
TSID / Hierarchical WBC	⭐⭐⭐	接触约束、末端任务、力矩限制
Kimera-Multi 类多机 SLAM 系统	⭐⭐⭐	多机位姿图、相对观测、地图合并
多机无人机框架 mrs_uav_system	⭐⭐	分布式系统、任务编排、通信接口
MuJoCo 多体接触仿真	⭐⭐	刚体连接、接触力记录、延迟注入

下一章连接：本章把多机协作中的任务、通信、局部自治和一致定位拆开讨论。下一章将回到更一般的复合机器人闭环，把 SLAM、物体级感知、操作策略、MPC/WBC 和学习模块统一到“感知-操作-运动”三闭环架构中。

消息	关键字段	说明
TeamState	时间戳、每机状态摘要、物体状态	协调层输入
ObjectContract	物体轨迹、抓取点、允许误差	合同层输出
LocalRisk	支撑裕度、内力、数据年龄、温度	局部监督输入
ModeEvent	阶段、进入原因、退出原因	行为编排日志
NetworkStats	p50/p90/p99 延迟、丢包率	通信诊断
RecoveryCommand	保持、顺应、放置、释放、撤退	降级动作