人型机器人ZMP零力矩点控制：从倒立摆模型到动态步态稳定性实战

1. 从零力矩点说起：人型机器人为什么离不开ZMP

人型机器人走路这件事，外行看热闹，内行看门道。很多人第一次接触双足机器人控制，脑子里想的都是关节怎么转、步态怎么规划，但真正上手之后才会发现，最核心的问题其实只有一个——机器人会不会摔。而判断它会不会摔，最经典、最基础、也最经得起时间考验的工具，就是零力矩点（Zero Moment Point，简称ZMP）。

我第一次认真研究ZMP，是在调试一台小型双足平台的时候。当时机器人静态站着没问题，一旦迈步就往前扑，调了无数遍PID都没用。后来才意识到，问题根本不在关节控制，而在于整个身体的质心投影和地面反作用力之间的关系没搞对。ZMP这个概念，就是用来描述这个关系的。

简单来说，ZMP是地面上的一个点，在这个点上，地面反作用力的合力矩在水平方向上的分量为零。换句话说，如果机器人脚底和地面之间的压力分布可以等效为一个合力，那么ZMP就是这个合力的作用点。只要ZMP落在支撑多边形（也就是脚掌与地面接触形成的区域）内部，机器人就不会绕边缘翻倒。这个判据非常直观：ZMP在脚掌范围内，稳；ZMP跑到脚掌外面，倒。

这套理论最早由南斯拉夫的Vukobratović教授在1960年代末提出，后来成为整个人型机器人领域的基石。日本本田的ASIMO、索尼的QRIO、波士顿动力的Atlas，早期步态规划都绕不开ZMP。即使现在很多团队转向了基于模型预测控制（MPC）或者强化学习的方案，ZMP仍然是最常用的稳定性约束之一。你可以不直接用ZMP做控制器，但你很难完全忽略它。

这篇文章适合谁看？如果你正在做双足机器人、外骨骼、或者任何需要动态平衡的运动控制项目，ZMP是你必须吃透的概念。如果你只是刚接触机器人运动控制，想找一个切入点，ZMP也是最好的起点之一——它数学上不复杂，物理意义清晰，而且有大量可复现的实操路径。下面我会从整体设计思路、核心细节、实操过程、常见问题几个方面，把ZMP这套东西拆开讲清楚。

2. 整体设计思路：为什么用ZMP而不是别的

2.1 ZMP的核心逻辑与适用边界

ZMP的本质，是把一个复杂的多刚体动力学问题，简化成一个地面上的点是否在安全区域内的问题。这个简化非常聪明，因为它抓住了双足行走最关键的矛盾：机器人是一个倒立摆。你的身体重心越高，脚掌越小，就越容易倒。ZMP就是用来量化这个“容易倒”的程度。

但ZMP不是万能的。它有几个明确的适用边界。第一，它假设地面是刚性且水平的，脚底和地面之间没有滑动。如果地面是软的、斜的，或者脚底打滑，ZMP判据就会失效。第二，ZMP只考虑水平方向的力矩，不直接处理绕垂直轴的旋转。如果机器人需要快速转身，ZMP本身不够，还需要配合摩擦锥约束。第三，ZMP是一个准静态或者动态判据，它不直接告诉你关节该怎么动，只告诉你“当前状态安不安全”。

我见过不少新手一上来就想用ZMP做全身控制，结果发现计算出来的ZMP一直在支撑多边形边缘疯狂试探，机器人走起来像喝醉酒。问题不在于ZMP错了，而在于他们把ZMP当成了控制器，而不是约束。正确的思路是：用ZMP做稳定性判据，用别的算法（比如逆运动学、MPC、或者简单的步态生成器）去生成关节轨迹，然后不断检查ZMP是否越界。

2.2 为什么不用纯静态平衡或者纯动力学

有人会问，既然ZMP这么麻烦，为什么不干脆让机器人慢慢走，每一步都静态稳定？答案是效率。静态行走要求质心投影始终在支撑脚内，这意味着机器人必须频繁地停下来调整重心，步速极慢，能耗极高。ASIMO早期演示的静态步态，走一步要好几秒，看起来像老爷爷过马路。而动态行走允许质心短暂地“失控”，利用惯性往前冲，ZMP可以在支撑多边形边缘甚至短暂越界，只要整体轨迹能拉回来。这就是为什么ZMP在动态步态里更有价值。

另一个极端是纯动力学控制，比如用全身动力学加QP优化。这套方法理论上更精确，但计算量大，对模型精度要求极高，而且调参非常痛苦。ZMP的好处是降维：你不需要精确知道每个连杆的惯量，只需要知道整体质心和角动量的大致范围，就能做出一个可用的稳定性判断。对于大多数中小型双足平台，ZMP加上简单的倒立摆模型，已经能走出相当稳定的步态。

2.3 方案选型：从倒立摆到ZMP的完整链路

我实际用的方案，通常是这样一条链路：步态规划器生成质心轨迹和落脚点 → 用线性倒立摆模型（LIPM）计算ZMP → 检查ZMP是否在支撑多边形内 → 如果越界，调整质心轨迹或者落脚点 → 逆运动学解算关节角 → 底层关节伺服。

这条链路里，LIPM是关键。它假设机器人是一个质量集中在质心的倒立摆，腿是质量为零的伸缩杆。这个假设在质心较高、腿部质量较小的情况下非常准。LIPM的动力学方程很简单：质心水平加速度与质心到ZMP的距离成正比。具体来说，如果质心高度是h，重力加速度是g，那么ZMP的位置x_zmp = x_com - (h/g) * x_com_ddot。这个公式就是ZMP计算的核心。

为什么选LIPM而不是完整动力学？因为完整动力学需要知道每个连杆的质量、惯量、关节角，计算量大，而且模型误差会直接反映到ZMP上。LIPM只需要质心高度和质心水平位置，参数少，鲁棒性好。实测下来，对于身高1米以内、质心高度0.5米左右的小型双足，LIPM算出来的ZMP和实际测力台数据误差在1-2厘米以内，完全够用。

3. 核心细节解析：ZMP计算与支撑多边形

3.1 ZMP的数学定义与离散计算

ZMP的严格定义是：地面上一点p，使得地面反作用力绕p点的力矩在水平方向的分量为零。对于多刚体系统，ZMP可以通过以下公式计算：

x_zmp = (Σ m_i * (x_i * (z_i_ddot + g) - z_i * x_i_ddot)) / (Σ m_i * (z_i_ddot + g))

其中m_i是第i个连杆的质量，x_i, z_i是连杆质心位置，x_i_ddot, z_i_ddot是加速度。这个公式看起来复杂，但实际用的时候，如果你用LIPM简化，就变成前面说的那个简单形式。

在代码里，我通常这样实现：

def compute_zmp(com_pos, com_acc, com_height, g=9.81): """ com_pos: 质心水平位置 (x, y) com_acc: 质心水平加速度 (ax, ay) com_height: 质心高度 """ zmp_x = com_pos[0] - (com_height / g) * com_acc[0] zmp_y = com_pos[1] - (com_height / g) * com_acc[1] return zmp_x, zmp_y

这个函数就是整个ZMP控制的核心。你可能会问，为什么是减号？因为当质心往前加速时，ZMP会往后移，这是倒立摆的物理直觉：你往前冲，脚底压力会往后压，防止你往前倒。

3.2 支撑多边形的构建与动态更新

支撑多边形是ZMP必须落在里面的区域。对于双足机器人，支撑多边形取决于当前哪些脚着地。单脚支撑时，支撑多边形就是那只脚的脚掌轮廓；双脚支撑时，是两只脚掌轮廓的凸包。

实际实现时，我不会去算精确的凸包，而是用脚掌的四个角点或者八个角点来近似。比如一只脚长20厘米、宽10厘米，我就取四个角：(+0.1, +0.05), (+0.1, -0.05), (-0.1, +0.05), (-0.1, -0.05)，然后根据脚的位置和朝向变换到世界坐标系。双脚支撑时，把两只脚的角点合在一起，取凸包。

这里有个坑：支撑多边形不是固定不变的。当机器人抬脚时，支撑多边形会从双脚变成单脚，面积瞬间缩小一半。如果质心轨迹没有提前调整，ZMP很容易在切换瞬间越界。我的做法是，在步态规划阶段就预留一个安全裕度，比如让ZMP始终保持在支撑多边形内缩5厘米的区域内。这样即使有模型误差或者地面不平，也不容易翻。

3.3 质心高度与ZMP灵敏度的关系

质心高度h对ZMP的影响非常大。从公式x_zmp = x_com - (h/g) * x_com_ddot可以看出，h越大，同样的质心加速度会导致ZMP偏移越大。这意味着高个子机器人更难控制，因为质心高，稍微一加速ZMP就跑到脚掌外面了。

我实测过，同一台机器人，质心高度从0.4米调到0.6米，同样的步态参数下，ZMP波动范围扩大了约50%。所以如果你在调试时发现机器人走路晃得厉害，可以先试试降低质心——比如把电池或者重物往下放。这是最直接有效的稳定手段，比调任何参数都管用。

另外，h也不是越小越好。质心太低，腿部伸缩空间受限，步幅做不大，而且能耗会增加。一般小型双足，质心高度在0.3-0.5米比较合适；中型人型，0.6-0.9米；大型的，1米以上就要非常小心了。

4. 实操过程：从步态生成到ZMP验证

4.1 线性倒立摆步态生成

我用的步态生成方法，是基于LIPM的解析解。给定一个步长和步时，可以算出质心的自然轨迹。LIPM的方程是：

x_com_ddot = (g / h) * (x_com - x_zmp)

如果假设ZMP固定在支撑脚中心，那么质心轨迹是一个双曲余弦函数。但实际行走时，ZMP会随着脚步移动，所以更常用的方法是预览控制：提前知道未来几步的ZMP位置，然后反推质心轨迹。

具体步骤：

1. 确定落脚点序列，比如每步0.2米，步时0.5秒。
2. 假设ZMP在单脚支撑时位于脚掌中心，双脚支撑时线性过渡。
3. 用LIPM的离散递推公式，从初始质心位置和速度出发，计算未来N步的质心轨迹。
4. 检查每一步的ZMP是否在支撑多边形内。
5. 如果越界，调整初始速度或者落脚点，重新计算。

这个过程我通常用Python写一个离线规划器，跑一遍看轨迹图。如果ZMP曲线始终在支撑多边形内，就把质心轨迹和落脚点发给机器人执行。

4.2 逆运动学解算与关节角生成

质心轨迹有了，接下来要解算关节角。对于双足机器人，通常用解析逆运动学，因为腿是6自由度或者5自由度的串联结构。以常见的6自由度腿为例：髋关节3个（roll, pitch, yaw），膝关节1个（pitch），踝关节2个（roll, pitch）。给定脚的位置和姿态，以及髋关节位置，可以解析求出所有关节角。

这里的关键是髋关节位置。髋关节位置由质心位置和上半身姿态决定。如果上半身保持竖直，髋关节就在质心正下方一定距离。如果上半身有俯仰，髋关节会前后偏移。我一般让上半身尽量保持竖直，减少耦合。

逆运动学解算完，还要做关节限位检查和速度限幅。我踩过的坑是：规划出来的质心轨迹很漂亮，但逆解出来的膝关节角超过了机械限位，机器人直接卡死。所以每次解算完，都要检查一遍关节角是否在范围内，速度是否超过电机最大转速。

4.3 ZMP实时验证与安全策略

离线规划好了，实际跑的时候还要做实时ZMP验证。我用的是足底压力传感器或者六维力/力矩传感器，装在脚踝或者脚底。传感器数据经过滤波后，可以直接算出实际ZMP。如果实际ZMP和规划ZMP偏差超过阈值，比如3厘米，就触发安全策略：要么减速，要么切换成静态站立，要么直接断电保护。

实测中，我发现滤波很关键。原始力传感器信号噪声很大，直接算ZMP会跳来跳去。我一般用二阶低通滤波器，截止频率设在20-30Hz。截止频率太低，ZMP响应滞后，来不及保护；太高，噪声滤不掉。这个参数需要根据机器人质量和步频来调。

另外，安全策略要有分级。轻微偏差，只报警不动作；中等偏差，降低步速；严重偏差，立即停止。我见过有人一上来就设很紧的阈值，结果机器人走两步就停，根本没法用。阈值要慢慢收紧，先保证能走，再保证走稳。

5. 常见问题与排查技巧实录

5.1 ZMP计算值跳动剧烈怎么办

这是最常见的问题。原因通常有三个：传感器噪声、质心估计误差、或者加速度计算不准。排查顺序：先看原始力传感器信号，如果噪声大，加滤波；如果滤波后还跳，检查质心位置估计，是不是用了错误的连杆质量；最后检查加速度，是不是用了差分导致噪声放大。

我的经验是，加速度最好用规划值，不要用传感器差分值。规划值平滑，差分值噪声大。如果非要用传感器，至少加一个卡尔曼滤波器。

5.2 机器人走路时ZMP总是偏向一侧

这通常是机械不对称或者质心偏移导致的。比如电池装偏了，或者某条腿的电机重一些。解决办法：先做一次静态标定，让机器人静止站立，测量实际ZMP，然后调整质心估计的偏移量，直到ZMP落在脚掌中心。这个偏移量可以写进配置文件，每次启动时加载。

5.3 步态切换时ZMP越界

单脚支撑和双脚支撑切换时，支撑多边形突变，ZMP容易越界。我的做法是：在切换前提前移动质心，让ZMP平滑过渡。具体来说，在双脚支撑阶段，让ZMP从一只脚的中心线性移动到另一只脚的中心，而不是瞬间跳变。这个过渡时间一般取步时的20%-30%。

5.4 常见问题速查表

5.5 独家避坑技巧

第一个技巧：先用仿真跑通再上真机。我见过太多人直接上真机调ZMP，结果摔了几次之后信心全无。用Gazebo或者PyBullet建一个模型，把ZMP计算和步态生成跑一遍，确认轨迹合理再上真机。仿真里摔一百次都不心疼。

第二个技巧：从静态站立开始调。不要一上来就走。先让机器人站着，看ZMP是否在脚掌中心。如果站着都不稳，走路肯定没戏。静态稳了，再试单脚支撑，再试慢速行走，最后才加速。

第三个技巧：记录数据，事后分析。每次跑完，把ZMP、质心、关节角、传感器数据都存下来。摔了之后回放，看ZMP是在哪个时刻越界的，越界前质心在干什么。这比盯着机器人看有用得多。

6. 从ZMP到更高级的运动控制

ZMP是入门，但不是终点。实际项目中，我通常会把ZMP和**模型预测控制（MPC）**结合。MPC可以在线优化质心轨迹和落脚点，同时把ZMP作为约束。这样机器人可以走得更快、更自然，而且能应对一定的外部扰动。

另一个方向是全身控制。ZMP只考虑质心和地面反作用力，不直接处理手臂和上半身的运动。如果机器人需要边走边操作，比如开门、搬东西，就需要把手臂动力学也纳入进来。这时候ZMP仍然是稳定性判据，但控制变量多了很多。

还有一个趋势是基于学习的步态。用强化学习训练双足行走，可以不需要显式的ZMP计算。但即使在这种方案里，ZMP也常被用作奖励函数的一部分，鼓励策略保持稳定。所以ZMP这套东西，学透了不亏。

我个人在实际操作中的体会是，ZMP最大的价值不是它有多精确，而是它提供了一个可解释、可调试、可复现的框架。当你面对一台走路摇摇晃晃的机器人时，ZMP能告诉你问题出在哪：是质心太高，是落脚点太近，还是加速度太大。这种诊断能力，比任何黑箱控制器都珍贵。