上周验收演示，我花了整整一天调试的全身运动控制方案在平地上已经走得行云流水，结果把场地从硬地胶挪到草坪，机器人三秒钟就跪了。全实验室围着那块草皮愣住，问题从来不在算法能不能走平路，而在于你怎么证明它在任何场地、任何扰动下不会翻车。这篇想聊的就是全身运动控制真正上硬件之前必须过的四道关：鲁棒性评测、摔倒恢复、安全层设计、实时性保障。正在做双足或四足样机联调、或者刚从RL仿真往实机迁移的工程师，可以照着搭一套自己的测试流程。
1. 全身运动控制为什么先要过“评测”这一关
1.1 只看轨迹误差，你的控制器在实机上迟早翻车
很多团队评测全身运动控制，习惯性盯三个数：质心轨迹误差、关节跟踪误差、末端执行器位置误差。这三个数当然重要，但如果你只盯着它们，实机上大概率会遇到“指标很漂亮，机器人却一推就倒”的诡异局面。
我印象很深的一次实验：某套策略在仿真里质心轨迹误差只有2厘米，末端跟踪误差也在毫米级，但把ZMP（零力矩点）随时间变化的曲线拉出来一看，它长期在支撑多边形边界上反复横跳。平地走没问题，因为步态规划本身有一定冗余，但只要来一个横向的小扰动，ZMP立刻越界，支撑脚打滑，整个身体就带倒了。轨迹误差是控制层指标，它无法告诉你系统离“失去平衡”还有多少距离。
评测全身运动控制，本质上评测的是“控制策略在有干扰、有不确定性、有硬件限制的世界里还能不能保住稳定性”。这需要一套分层指标：任务层看结果，控制层看动作质量，安全层看底线状态。三层各看各的，任何一层出问题，都不能说这个控制器达到了部署标准。
1.2 一套可复用的三层评测指标体系
我们实验室现在跑评测，固定用下面这套三层指标体系，你在自己的项目里可以直接抄作业：
| 层级 | 核心指标 | 常用度量手段 | 说明 |
|---|---|---|---|
| 任务层 | 任务成功率、路径跟踪误差、末端操作误差、速度完成率 | 运动捕捉、里程计、末端视觉标记 | 反映“任务做没做成”，是最高层的验收标准 |
| 控制层 | 质心轨迹误差、姿态角误差、动量偏差、关节力矩平滑度、能量消耗 | 编码器、IMU、关节力矩传感器 | 反映“动作做得好不好”，是调试时的主战场 |
| 安全层 | 最大力矩越限次数、ZMP越界时间占比、接触力锥违反次数、自碰撞标记、摔倒次数、恢复时间 | 力/力矩传感器、接触检测、仿真碰撞检测 | 反映“会不会出事”，是部署的红线 |
控制层的关节力矩平滑度这个指标容易被忽略。它一般用相邻控制周期力矩指令的差分绝对值来算，如果这个数经常出现峰值，说明策略在输出高频抖振，实机上电机会发热、减速器会磨损，长期跑必然出问题。我们在一次评测中发现某策略平均跟踪误差比另一套小30%，但力矩差分峰值高出4倍，上实机跑了十分钟电机就开始报警，直接被淘汰。
1.3 评测场景矩阵：从真实任务倒推扰动清单
评测场景不能拍脑袋，最好的做法是从实际应用场景倒推，把每一种可能遇到的干扰变成具体的测试用例。比如：
| 实际场景 | 对应扰动类型 | 评测用例 |
|---|---|---|
| 室外巡检 | 松软地面、风载荷、灌木挂绊 | 草皮/沙地行走、肩部横向脉冲推力、脚踝高度随机绊索 |
| 物资搬运 | 负载突变、重心外移 | 托盘突然增重10%、单侧负重、快速下蹲搬运 |
| 人机交互 | 突发推力、拉扯 | 腰部高度横推、背部后拽、突然中断支撑 |
| 非结构地形 | 踩空、打滑、台阶高差 | 单脚踩空、油污地面单脚打滑、5cm高度差路面 |
每一个用例都要能转成可执行的测试脚本：扰动力的大小、方向、持续时间、施加位置，全部提前写进实验计划。评测的意义不是让控制器“看起来稳”，而是让每一次的“稳”都有数据支撑、可以复现、可以对比。
2. 鲁棒性测试：从仿真注扰到实机强推的一整套做法
2.1 扰动注入要覆盖接触、负载、地面三类变化
鲁棒性测试的核心是“扰动注入”，但很多人把这件事做成了“随便推机器人一下”。实际上，一套合格的扰动注入矩阵至少要覆盖三类变化：接触类扰动、负载类扰动、地面类扰动。
接触类扰动最常见的是推力。仿真里推荐用参数化的脉冲力去测试：力幅大约取机器人自重的5%~15%，持续时间在0.2~0.5秒，施加位置分腰部高度和肩部高度两组，方向分矢状面正推、后推和额状面侧推。腰部高度的力主要考验底盘的抗扰动能力，肩部高度的力会同时产生倾覆力矩，对全身协调是更严格的考核。
负载突变是个很有效的测试手段。在机器人背上放一个可以电磁释放的负载，控制器稳定行走后突然释放，让负载瞬间离开身体。这种测试能暴露出策略对质心位置和质量变化的敏感度。我们自己测试时就发现，一套在恒定负载下表现完美的策略，在负载突降后会有一个明显的重心后仰动作，如果没有后续补偿，三步之内就会失衡。
地面突变主要是摩擦系数变化和高度差。实机上最省事的做法是铺一块带油污的金属板，或者在地面随机垫一块2~5厘米的硬质台阶。这类扰动不需要额外设备，但对接触模型是非常直接的考验。
2.2 每个扰动场景要跑多少轮才有统计意义
很多团队每个扰动场景只跑三五次，觉得“没事就行”。这个样本量在统计学上完全不够。全身运动控制本身带有随机性，电机摩擦、地面接触、传感器噪声都有不确定性，单次成功说明不了任何问题。
我现在的操作标准是：每个场景至少重复20次，记录五个量——成功率、恢复时间、质心最大偏移、支撑脚是否打滑、任务是否中断。统计的时候不要只看平均值，要重点看95百分位，也就是“最差的那5%表现”。控制器能否部署，往往不是由平均表现决定的，而是由最差表现决定的。一次突然的大偏移，在真实场景里就是一次摔倒、一次损坏。
分享一个仿真和实机一致性校验的小技巧。在仿真里对所有扰动场景跑同样的脚本，然后对比实机结果。如果仿真成功率90%、实机只有40%，优先怀疑三件事：摩擦力矩和接触模型建得不像、执行器延迟没建模、转动惯量参数偏了。这三个是仿真与实机之间的最大鸿沟。
2.3 实机物理扰动的三种推荐手法
实机测试里，人肉推是最不推荐的方式，因为人的发力终究带主观性，每次的方向、速度、力的分布都不一样，数据无法复现。我推荐三种更可控的手法：
推杆测试。用一根带力传感器的推杆从固定高度水平推向机器人。优点是可以控制用力大小，缺点是推杆接触后有摩擦，适合做渐进推力测试。测试时让推力从10%体重缓慢增加，记录机器人能承受的最大推力。
撞锤测试。适合模拟撞击类扰动，比如门被撞开、障碍物碰撞。撞锤的动能可以通过摆臂角度控制，能量大小可以精确计算。但注意用撞锤时一定要在机器人背侧加保险绳，防止被撞倒后直接硬砸地面。
释放绳法。这是我认为最值得试的。拿一根高强绳拉住机器人，给一个持续阻力，等控制器进入稳定状态后瞬间释放挂钩，产生一个非常干净的“阶跃撤力”扰动。相比推力，这种方法没有接触摩擦带来的拖拽，扰动释放后系统完全自由，能测出真正的动态恢复能力。很多摔倒动态恢复的测试都是用这种方式做的。
无论用哪种方法，测试时都要保证两件事：有急停开关能在2米外一键断电，有软质吊绳或者海绵垫兜底，防止实机损坏。同时记录数据的时间戳要对齐，视频、日志、力传感器三路数据同步，不然事后根本没法复盘。
3. 摔倒恢复不是“爬起来”，是一条完整的决策链路
3.1 从正常运动到摔倒，至少要留三档状态
很多团队设计摔倒恢复时就是一个状态机：正常运动，摔倒了，爬起来。这样设计基本等于没有设计。真实物理世界中，从“正常”到“躺倒在地”之间有一连串连续状态，每个状态需要不同的应对策略。
我建议至少保留三档中间状态。第一档是失稳预警状态，此时检测到ZMP接近支撑边界、质心速度异常，但机器人还没失去平衡，策略应该做的是调整步幅、压低重心、张开双臂备用。第二档是保护缓冲状态，此时已经无法避免接触地面，策略要主动屈膝、收臂、蜷缩，用可控的方式落向地面，减少冲击。第三档是倒地状态，才轮到恢复动作。
这样设计的好处是避免“一次检测一个动作”这种脆弱的逻辑。实际测试中经常出现一种情况：机器人只是暂时失稳，还没倒，立即触发高强度恢复动作反而干扰了自身平衡。合理的状态机应该具备降级能力——先尝试低成本稳定，再逐级升级到保护动作。
3.2 检测-保护-恢复的时序预算是设计核心
摔倒恢复设计里最容易忽略的是时间预算。从机器人开始失稳到身体接触地面，留给系统的反应窗口极短。以一台身高1.2米、重量30公斤的小型双足机器人为例，从失去平衡到身体倾斜到支撑多边形之外再到落地，大约只有0.5到0.8秒。
这意味着摔倒检测必须在50~100毫秒内完成。检测信号不能只依赖IMU，因为IMU在剧烈运动时会饱和、会漂移，我见过太多因为IMU瞬时噪声导致的误检测。可靠的摔倒检测应该是多维信号融合：IMU的姿态角速度和加速度、四个脚的足底力突变、关节位置速度异常，多个信号加权投票。
检测触发后，保护动作要在下一个控制周期内启动，也就是10毫秒以内。这里强调一下，保护动作不是发力去撑住，而是放松去缓冲。很多初学者会把保护动作设计成“拼命撑地”，结果肩关节和髋关节承受了巨大的冲击扭矩，轻则过载报警，重则结构损坏。正确的保护是关节切换成低增益柔顺模式，让身体像一个弹簧一样吸收落地能量。
恢复动作反而是过程最长的阶段，从倒地到重新站立，一般需要3到10秒。这和三观直觉相反，但非常重要：恢复动作是在低重心下做大范围关节运动，电机需要输出大扭矩，如果急于快速起立，重心的加速度过大，很容易在站起来的过程中二次摔倒。
3.3 恢复策略必须按姿态分派
摔倒后的身体姿态千差万别，一套恢复策略打天下完全不现实。不同姿态必须对应不同的恢复序列：
| 摔倒姿态 | 恢复策略 | 设计要点 |
|---|---|---|
| 俯卧（趴着） | 手臂撑地→抬起躯干→转为跪蹲→站立 | 撑地时注意手腕角度，防止腕关节过载 |
| 仰卧（躺着） | 翻身到侧卧→蜷缩→转为四足支撑→站立 | 翻身动作要慢，避免躯干扭转过快 |
| 侧倒 | 屈髋屈膝→上臂支撑→转为俯卧或蹲起 | 上臂支撑角度直接影响成功率 |
| 坐倒 | 重心前移→双脚踩地→臀部离地→站立 | 核心是保持脚底不打滑 |
每一个恢复策略都应该在仿真里先做危险姿态测试：直接从不同高度、不同方向把机器人“放入”倒姿态，再启动恢复策略，记录成功率和关节力矩峰值。这里有一个建议：恢复动作的幅度越小越好，重心高度越低越安全。不要为了美观设计大幅度的起身动作，稳才是第一位。
另外，恢复完成之后要有一个“回归正常运动”的过渡期，不要直接从静态站立切到快速行走。我们会强制在恢复完成后保持站立状态1到2秒，让状态估计器和接触检测稳定下来。
4. 安全层到底该放在哪：限幅、过滤与保护性行为
4.1 力矩限幅不是简单clip，要跟接触力锥一起看
安全层的第一道防线是执行器力矩限幅，但很多人直接写一行代码把力矩指令clip到最大值，这就有点太草率了。简单clip的问题是：它会让力矩指令在限幅边界上反复切换，等效于高频抖振，反而把能量注入到系统里。
更稳的做法是同时限制力矩大小和力矩变化率。力矩大小上限来自电机峰值扭矩和减速器额定扭矩，变化率上限来自机械结构的冲击耐受能力。你在控制周期里计算出来的力矩指令，先做变化率限制，再做幅值限制，这样可以避免指令在边界处振荡。
关节力矩之外还要看接触力锥。全身运动控制在支撑脚上会产生法向力和切向力，切向力如果超过摩擦系数乘以法向力，脚就会打滑。这个约束在仿真里很好加，但在实机上经常是“隐性”的——你看到机器人突然滑倒，回溯日志才发现脚底切向力已经超了很长时间。所以在安全层里必须实时计算每个支撑脚的接触力是否符合摩擦锥约束，一旦接近边界就主动调整质心加速度指令，而不是等打滑发生了再补救。
4.2 安全过滤层：用投影把危险指令拉回安全集
安全层的第二道防线是独立的安全过滤层。这个模块夹在控制器输出和执行器之间，它不直接参与运动控制，却拥有“否决权”。它的工作原理可以这样理解：控制器输出一个指令，安全过滤器检查这个指令是否会破坏安全约束，如果会，就把它“投影”回离它最近的安全指令，让机器人既能执行保护性的动作，又不至于瞬间跳出安全边界。
工程实现上，常见做法是把安全约束转成一个二次规划问题：目标函数是“新指令和原指令尽量接近”，约束是满足力矩上限、接触力锥和执行器加速度上限。解出来就是安全过滤后的指令。这个QP规模很小，一般0.5到1毫秒内能解完，完全来得及跑在实时控制回路里。
我把安全过滤层设计成独立线程运行，和主控制线程分离，之间通过无锁环形缓冲传递数据。硬件层面还单独做了一路看门狗和急停电路，软件安全层再快，也不能替代硬件级的物理断电器。安全层就是最后一道闸，宁可不动作，不能乱动作。
4.3 别把安全层调得太“焦虑”
安全层太保守，同样会翻车。我们做过一次很典型的反向实验：为了“更加安全”，把安全层的力矩阈值设成理论极限值的0.8倍，结果机器人正常走路时安全层频繁触发，指令时不时被拉回来一截，步态被切得支离破碎，走到第三步终于因为姿态被破坏而摔倒。
安全层触发本质上是对原控制指令的强行干预，每次干预都会引入跟踪误差。如果干预太频繁，系统的相位会被反复打乱，效果反而比不干预更差。现在我把安全阈值设置在理论值的1.1倍到1.15倍，留出适当余量，只有在真正逼近危险时出手。同时，安全层触发后不要立刻撤掉，要有一个平滑退出的过程，把指令低通滤波过渡回正常控制，否则出口处又会产生新的阶跃。
5. 实时性不是控制频率，是端到端的时间预算
5.1 全链路时间预算怎么拆
控制器宣称“1kHz控制频率”，只代表主控制循环的频率，但真正决定实时性的是从传感器采样到执行器力矩生效的端到端时延。下面是我们常用的时间预算参考表：
| 模块 | 典型耗时 | 说明 |
|---|---|---|
| 传感器采样（IMU/编码器/力传感器） | 0.2~1ms | 取决于总线类型，EtherCAT一般低于0.5ms |
| 状态估计（EKF/接触检测/浮动基座估计） | 0.5~2ms | 接触检测的延迟是最大变量 |
| 控制器计算（MPC+全身控制或RL推理） | 2~8ms | QP求解器、神经网络推理都有波动 |
| 安全过滤层 | 0.3~1ms | 独立线程运行，不能占主线程时间 |
| 通信与执行器 | 0.5~3ms | 电机驱动器的力矩响应带宽决定了上限 |
累计下来，端到端时延控制在10毫秒以内是比较健康的状态。这里特别提醒一点：时间预算要留出至少30%的余量。因为控制循环偶尔会遇到缓存未命中、总线重传、系统调度抖动这些偶发高负载，如果预算排得太满，一次抖动就会导致控制周期超出，执行器拿到的就是过时指令。
5.2 非实时操作偷偷吃掉控制周期
我在排查实时性问题时，发现最大的敌人往往不是什么算法复杂度，而是一些不起眼的非实时操作。最常见的就是在控制回调里做动态内存分配——一个std::vector在循环里反复push_back就足够触发堆锁，让线程卡上好几毫秒。控制循环里的所有数据结构都应该在初始化阶段预分配，用固定大小数组或者环形缓冲区。
第二个坑是日志打印。在控制循环里直接printf或者往ROS topic发消息，I/O阻塞会直接把控制周期打爆。正确的做法是异步日志：控制线程只把数据写进环形缓冲区，另一个非实时线程负责落盘和发布。我们实测过，把日志从控制线程摘出来之后，控制周期最大抖动从原本的4.2毫秒降到了0.3毫秒。
第三个坑是线程调度。Linux默认的CFS调度器不会保证控制线程的实时性，必须在启动时用chrt把控制线程设置成实时线程。比如：
chrt -f -p 90 PID-f表示SCHED_FIFO，90是优先级，数字越大优先级越高。注意优先级不能设到99，那是内核最高优先级线程保留的。同时要检查CPU核心隔离，用isolcpus把控制线程绑在专用的CPU核心上，避免和其他进程争抢。
5.3 实测端到端时延：没有示波器也能测
时延不是靠“感觉”算出来的，要实测。最可靠的手段是GPIO翻转法：控制线程开始计算前把一块GPIO拉高，执行器的电机驱动收到力矩指令后发一个确认信号把另一块GPIO拉低，用示波器或者逻辑分析仪直接量两块GPIO之间的时间差。这是物理层面的测量，不受软件日志影响，最真实。
如果没有示波器，也可以在软件里做近似测量。在控制循环里维护一个时间戳队列，记录每个周期“传感器数据到达时间”和“控制指令发出时间”的间隔，在线统计分布。这个方法虽然测不到通信链路和执行器的时延，但至少能把握住计算侧的延迟水平。
还有一个很容易被忽略的测量维度：感知链路延迟。用外部运动捕捉系统作为位置真值，和机载状态估计器的输出做对比，通过相位差可以估算从传感器到状态估计的延迟。如果发现状态估计的输出比真值滞后超过20毫秒，就要警惕滤波算法是否过度引入相位延迟。
6. 真正部署到硬件后，最容易翻车的三个细节
6.1 为什么仿真里很稳的策略，实物一上就“脆”
很多人在仿真里把鲁棒性测试跑得漂漂亮亮，上实机第一天就发现控制器“变脆”了。最常被忽视的原因是模型参数偏差。转动惯量偏差10毫米，质心高度差一点点，步态的表现就可能天差地别，尤其是动态行走中，质心加速度对惯性参数极其敏感。实机上的连杆质量、质心位置、转动惯量，几乎不可能跟CAD模型完全一致，所以部署前一定要做参数辨识。
执行器延迟是另一个大坑。仿真里力矩指令往往被当成即时生效，但实机上电机、驱动器和机械传动系统会引入几十毫秒的动态延迟。如果策略训练时没有把这个延迟建模进去，部署后就会发现控制指令始终“慢半拍”。解决办法是在仿真里把执行器建模成一阶或者二阶系统，加入延迟和带宽限制，重新做域随机化训练。
还有一个非常现实的问题：电机温度升高会导致力矩下降。很多策略在训练时假定执行器随时能输出峰值力矩，但实际上连续运行十几分钟后，电机温度上来，输出力矩会下降20%以上。如果策略过度依赖峰值力矩工作，就会出现“头几分钟稳定，后面越来越弱”的现象。我现在会在评测中加入“热态测试”：让机器人连续运动半小时后，再跑一遍同样的鲁棒性测试场景，看表现是否退化。
6.2 状态估计的延迟可能比控制器算得再快都致命
全身运动控制对状态估计的依赖程度远超普通运动控制。因为它的很多决策都依赖“身体当前处于什么姿态”“哪些脚在接触地面”“质心在什么位置”。状态估计一旦有延迟，控制器算得再快也白搭，它基于的信息本身就是错的。
我见过一个非常典型的案例：接触检测延迟30毫秒，控制器已经认为机器人还在摆动相，实际上脚早已落地。恢复策略在错误相位启动，机器人明明已经半蹲在地，却执行了一个“空中收脚”的动作，结果当然是更猛烈地摔下去。
解决思路是多源信息校验：足底力、IMU、关节运动学观测器三路信号做投票，任何一路信号和另外两路严重冲突时，优先采用变化最陡峭的信号作为触发条件。同时给所有状态估计输出打上时间戳，在控制器的状态预测阶段做时延补偿。实机调参时我会专门用一个动作来验证状态估计时延：让机器人做一次快速下蹲，对比外部运动捕捉和机载估计的相位差。
6.3 连续运行半小时以上的漂移陷阱
短时间测试和长时间部署是两回事。随着运行时间拉长，电池电压下降、关节温度升高、润滑油粘度变化，都会导致关节响应特性漂移。一套在刚充满电时调好的参数，到电池电压降低后，同样的控制指令输出的实际力矩会有明显偏差。
我现在部署前的最后一件事，是做一次30分钟以上的长时跑测，每5分钟记录一次关节温度、电池电压、CPU占用和实际响应时延，绘制漂移曲线。如果关节温升超过阈值，触发降级策略：自动限制运动速度和幅度，而不是直接停机。这个降级策略是软件里的一个独立模块，根据温度、电压和CPU负载动态调整控制器的速度和加速度上限。
另外，CPU占用率随时间缓慢上升这种“内存泄漏型”问题也很常见，我用perf和top监控控制进程，连续跑一小时后如果CPU占用率上升超过5%，就要怀疑是否有未释放的资源或者不断增长的缓存数据。部署前把这三件事跑一遍，比在现场抓瞎强得多。
