上一节：4500元，手搓一台会走路的人形机器人| 第 11 章 IMU标定与数据滤波

第四部分：平衡控制算法

第 12 章 ZMP站立平衡：从理论到稳定站立 把机器人推倒一万次后，它终于学会了站稳

12.1 ZMP理论的直觉化理解

在开始写代码之前，我需要先理解ZMP这个概念。名字听起来很唬人，但本质极其简单。

12.1.1 你站着的时候发生了什么

请你站起来，双脚并拢，闭上眼睛。感受脚底——压力是不是集中在脚掌的某个位置？站得笔直时，压力大概在脚掌中央偏后。稍微往前倾，压力会往前移。继续前倾，当压力移出脚掌范围的那一刻——你就倒了。这个「压力集中的那个点」，就是ZMP（Zero Moment Point，零力矩点）。
ZMP的核心规则只有一条：只要ZMP在脚掌支撑面内，机器人就稳定；ZMP一旦超出支撑面，机器人必然倾倒。这不是经验，是牛顿力学在双足站立上的直接推论。

12.1.2 一个形象类比

想象一个倒立摆——一根杆子，底部插在可以滑动的滑块上。杆子偏了，你移动滑块去追杆子的重心。只要滑块够快，杆子就能一直立着。这就是ZMP控制的核心思想：用脚踝的微小移动，去追那个不断偏转的重心投影。双足机器人本质上就是一个倒立摆——只不过它有两个脚，站立时支撑面是两只脚掌围成的凸多边形。

12.1.3 为什么ZMP不需要看公式

很多教材一上来就是满页积分公式。但说实话，我写代码的时候一次都没算过那些积分。因为脚底装了压力传感器，ZMP直接用传感器数据算出来——四个压力传感器读数做加权平均，就是这个瞬间的压力中心坐标。对DIY爱好者来说，ZMP不需要你懂拉格朗日力学。你只需要知道：ZMP在哪（传感器告诉你），ZMP应该在哪（脚掌支撑面中心）。如果偏了，用脚踝拉回来。就这么简单。
ZMP不是魔法，它是你脚底压力的那个「重心」。它唯一的要求就是：待在脚掌范围内。

12.2 PD控制：让机器人学会「反抗」

知道ZMP在哪之后，下一个问题：怎么把它拉回来？答案是PD控制。P是弹簧（偏得越多，拉得越狠），D是阻尼器（动得越快，刹车越猛）。两者配合，缺一不可。
没有P，机器人就是一根软面条，一推就倒。没有D，机器人就像被拉满又松开的弹簧，来回振荡停不下来。P让你站稳，D让你不抖。
为什么不用PID中的I（积分项）？I项会累积历史误差——机器人站立时有微小的持续偏置（比如腰部电机偏了半度），I项不断累积这个偏置，越来越大，直到某个瞬间突然释放——机器人猛地一歪，倒了。在100Hz的实时平衡控制里，I项是定时炸弹。所以我们只用PD。

12.2.1 实际PD参数

经过大量调参，最终确定的PD参数如下。这些参数来自YAML配置文件，由控制器在启动时加载：

# 踝关节 PD 参数 (YAML配置)
ankle_roll_kp: 0.8        # 踝 Roll P 增益
ankle_roll_kd: 0.06       # 踝 Roll D 增益
ankle_pitch_kp: 0.6       # 踝 Pitch P 增益
ankle_pitch_kd: 0.04      # 踝 Pitch D 增益
max_ankle_roll: 0.15      # 踝 Roll 最大角度 (~8.6°)
max_ankle_pitch: 0.15     # 踝 Pitch 最大角度 (~8.6°)

注意P和D的比值大约在10:1到15:1之间。Pitch方向（前后）的P比Roll方向（左右）小，因为前后有更大的脚掌支撑面，容错空间大。左右方向支撑面窄，需要更快的响应。

12.2.2 PD计算的真实代码

控制器中PD计算的核心逻辑如下。v13标准坐标系统一后，控制器层已不做任何轴交换——IMU的pitch（前后倾斜）直接映射到ankle_pitch（前后踝关节）补偿，IMU的roll（左右倾斜）直接映射到ankle_roll（左右踝关节）补偿。这是v13的核心改进：方向转换只在IMU驱动层和舵机指令出口两个边界处理，中间所有算法层直接消费标准坐标系：

# 踝关节PD补偿 (zmp_walking_controller.py)
# v13自然映射: pitch(前后) → ankle_pitch, roll(左右) → ankle_roll
ankle_pitch_comp = -(kp_pitch * self._filt_pitch
                      + kd_pitch * self._filt_gyro_y)
ankle_pitch_comp = max(-max_pitch,
                        min(max_pitch, ankle_pitch_comp))
ankle_roll_comp = kp_roll * self._filt_roll \
                  + kd_roll * self._filt_gyro_x
ankle_roll_comp = max(-max_roll,
                       min(max_roll, ankle_roll_comp))

注意ankle_pitch_comp前面那个负号——这就是下一节要讲的PD符号错误的核心。如果这个负号丢了，机器人就不是在「反抗」推力，而是在「放大」推力。
P是弹簧，拉你回来；D是阻尼器，让你别抖。I是累积误差，在站立平衡里是定时炸弹。只用PD，是最安全的选择。

12.3 站立姿势的确定

有了PD控制的理论，在调参之前还有一个更基础的问题：机器人以什么姿势站立？

12.3.1 为什么不能归中到0度

直觉上，你可能会说「把所有关节归零，让机器人站直」。这就是我犯的第一个大错误。我把所有舵机设为0度（中位120度），结果机器人直接往后倒。原因有三：结构不对称（电池在背后、主控板在胸口），重心天然偏后；膝盖伸直时处于「锁死」状态，没有缓冲空间；而且踝关节没有对抗张力，无法形成PD控制需要的操作空间。

12.3.2 站立姿势的确定过程

我用了关节测试工具逐关节确定姿势。核心思路是：先让踝关节制造「往前倒」的趋势（强dorsiflexion），再让腰部后仰把重心拉回来，形成对抗平衡，实际上，人类也是这个站立姿势的，特别的，我其实有点按照人猿的站立姿势来摆的这个姿势，还挺管用。这样PD控制器就有操作空间了——它可以在对抗中做微调，而不是在松垮垮的状态下做大幅调整。
经过反复测试，最终确定的站立姿势如下：

关节	舵机ID	角度	弧度	说明
髋关节侧向 (hip_roll)	ID6/11	8°	0.140 rad	外展
髋关节前后 (hip_pitch)	ID5/10	-5°	-0.087 rad	微前倾
膝关节 (knee)	ID4/9	21°	0.367 rad	弯曲缓冲
踝关节侧向 (ankle_roll)	ID3/8	-8°	-0.140 rad	内翻
踝关节前后 (ankle_pitch)	ID2/7	-11°	-0.192 rad	dorsiflexion（脚尖上翘）
腰部 (torso)	ID20	-9°	-0.157 rad	后仰

这个姿势的设计原理：踝-11度（强dorsiflexion）把压力中心推到脚掌前部，制造「往前倒」的趋势。腰-9度（后仰）把上半身重心往后拉，抵消前倾。两者形成对抗平衡——踝往前推，腰往后拉，机器人在中间直立。有了这个对抗，PD控制器就有了微调的空间。
站立姿势的核心不是「找到正确的角度」，而是「建立对抗平衡」。踝往前推，腰往后拉，给PD控制器创造操作空间。

12.4 PD符号错误的血泪史

这是整个项目中我印象最深的一段经历。一个负号的正负，花了三天才找到。如果你正在做平衡控制，这一节可能会帮你省掉三天。

12.4.1 现象：越推越倒

我花了一个下午写好PD平衡代码，兴冲冲地部署测试。轻轻推了一下机器人的肩膀——它没有晃，而是直接加速往我推的方向倒了下去。不是「没站稳」那种倒，是「主动加速」那种倒。我把Kp翻了一倍，倒得更快。再翻一倍，还没碰到它，它自己就开始抖，然后倒了。试了各种Kp和Kd组合，结果都一样：推一下，往推的方向加速倒。区别只是倒得快还是慢。

12.4.2 排查过程

第二天我开始怀疑硬件。IMU装反了？拿手机水平仪对比——方向是对的。舵机方向反了？单独发指令测试——也是对的。IMU数据有噪声？加了低通滤波——还是一样。第三天，我从最基础的地方重新推导。在纸上画简图：机器人往前倾，pitch为正。PD控制器应该让脚踝往后调整（plantarflexion），把身体往后推。也就是说，正pitch应该产生负的脚踝补偿。我翻开代码，看到了这一行：
ankle_pitch_compensation = Kp * pitch_error + Kd * pitch_rate
问题就在这里。pitch_error为正时，补偿也是正的——脚踝往前调整，恰恰是加速前倾！正确的应该是：
ankle_pitch_compensation = -(Kp * pitch_error + Kd * pitch_rate)
我少了一个负号。加上这个负号后，重新部署，推了一下机器人——它晃了晃，站稳了。那一瞬间，我盯着它看了整整三十秒，又推了它一下——又晃了晃，又站稳了。

12.4.3 教训

这件事教会我一个原则：在验证任何控制算法之前，先做小幅度测试。从±2度初始倾斜开始，确认方向正确后再±5度，都对了才上手推。这个流程看起来繁琐，但它防止了无数次「改了参数就倒，倒了不知道原因」的死循环。
PD符号错误是最隐蔽的bug。它不会报错、不会崩溃，代码看起来完全正确。但它会让你的机器人变成一个「放大器」——你推它，它帮你推自己。先做小角度测试，是唯一能提前发现的方法。

12.5 从抖到稳——PD参数调优

PD符号改对之后，机器人能站稳了。但「能站稳」和「站得稳」是两回事。最初的版本推一下要晃七八秒，需要把参数调到最优。

12.5.1 调参策略

经过大量试错，我总结了一套四阶段调参策略：
阶段一：找P的临界值。 D设为0，只调P。从0.1开始每次加0.05，推到机器人出现持续振荡（不衰减也不发散）。这个临界P值约在0.7-0.8。取临界值的70%-80%作为初始P——约0.5-0.6。
阶段二：调D消除振荡。 P固定后，从0.02开始加D，每次加0.02。当D到0.04-0.06时，推一下机器人晃1-2次就停。D太大（>0.08）机器人会变得迟钝。
阶段三：分别调roll和pitch。 前后和左右的动力学特性不同。前后方向脚掌长、容错大，P可以小一点。左右方向脚掌窄、需要快速响应，P要大一点。最终pitch用Kp=0.6, Kd=0.04；roll用Kp=0.8, Kd=0.06。
阶段四：加入限幅。 极端情况下（猛推），踝关节补偿可能超出机械限位。代码中max_ankle_pitch和max_ankle_roll都设为0.15弧度（约8.6度）。超过限幅的补偿被截断，交给跨步恢复机制处理。

12.5.2 参数平滑切换

还有一个重要细节：不同阶段（站立、行走、恢复）使用不同的PD参数集。如果直接套用，踝关节角度会突变导致抖动。代码中通过smoothstep函数在0.2秒内线性插值过渡，肉眼完全看不出来，但对稳定性至关重要。
代码中通过_set_pd_target()函数管理4个阶段（standing/shuffle/stride/cooldown）的参数切换，每次切换都经过0.2秒平滑过渡。
PD参数不是一成不变的。不同阶段各用一套参数，通过0.2秒平滑过渡。这个细节决定了机器人从「勉强能站」到「稳稳站住」的差距。

12.6 三级响应机制

PD控制只能处理小扰动。当推力大到一定程度，需要逐级调用更多资源。代码中实际形成了三层递进保护：

12.6.1 第一级：踝关节PD（正常站立）

这是最常见的场景。IMU检测到微小倾斜，踝关节做出PD补偿，把ZMP拉回支撑面中心。100Hz控制频率，每10ms响应一次。同时配合足底压力传感器的ZMP前馈，实现互补滤波——IMU提供快速响应，ZMP提供位置精度。

12.6.2 第二级：跨步恢复（Recovery Stride，约40度触发）

当倾斜角度超过0.70弧度（约40度），且连续3帧都超阈值，触发跨步恢复。此外，角速度超过2.0 rad/s也会触发恢复。跨步恢复最多连续5次，每次间隔2秒冷却。步幅限制在5cm以内，用小碎步连续调整。超过5次后放弃，进入冷却期等待人工干预。

# 恢复触发参数 (YAML配置)
recovery_tilt_threshold: 0.70      # 倾斜触发阈值 (~40°)
recovery_tilt_count: 3             # 连续3帧超阈值才触发
gyro_trigger_threshold: 2.0        # 角速度触发 (rad/s)
recovery_max_retries: 5            # 最多连续恢复5次
recovery_cooldown_time: 2.0        # 恢复冷却时间 (秒)

12.6.3 第三级：摔倒检测（Fall Detection，约45度触发）

当倾斜超过0.79弧度（约45度），且连续400帧（约4秒）超阈值，判定为摔倒。此时控制器归零所有关节，销毁节点，退出进程——不做无谓的挣扎。注意else分支是递减而非清零：偶尔超过阈值但很快回来，不会触发摔倒。只有持续数秒的超阈值倾斜才会。

# 摔倒检测逻辑 (zmp_walking_controller.py)
fall_threshold = 0.79  # rad (~45°)
max_tilt = max(abs(self._imu_roll),
                abs(self._imu_pitch))
if max_tilt > fall_threshold:
    self._fall_counter += 1
    if self._fall_counter >= 400:  # 约4秒
        self._publish_center_angles()
        raise SystemExit('Fall detected')
else:
    self._fall_counter = max(0, self._fall_counter - 1)

三级响应机制的本质是「逐级调用资源」。小问题用脚踝，中问题迈步，大问题放弃。每级都在上一级不够用的时候才介入。不是「越强越好」，而是「够用就好」。

12.7 自稳定验证标准

机器人站住了，你怎么知道它是「真的站住了」，还是「碰巧停在某个角度」？我一开始就没搞清楚这个问题，浪费了很多时间在「假稳定」上。

12.7.1 假稳定陷阱

最常见的情况：绳子挂住了机器人。机器人和电源、PC之间的线缆，有时候机器人往前倾被线拉住，停在那里不动了。IMU显示角度不变，看起来像「稳定站立」了——但实际上是线在撑着，不是PD在起作用。另一种「假稳定」是机器人停在某个非零倾斜角度不动了，不继续倒也不回来——可能是PD死区，可能是舵机无力，可能是机械卡住。

12.7.2 真正的自稳定

真正的自稳定，不是「停在某个角度不动」，而是「围绕0度反复振荡，振幅逐渐减小，最终收敛到0度附近」。这是阻尼振荡的典型特征，也是判断PD参数是否正确的金标准。
验证方法：正方向推→反方向推→侧面推→连续推。每次机器人都能回到0度附近（±2度以内），且恢复时间没有明显变长。如果每次停在不同的角度（比如第一次+3度，第二次-5度），说明PD参数还需调整，或者机械结构有间隙。
真稳定不是「不动了」，而是「推了会回来」。正反方向反复推，每次都能回到0度附近。如果有一次回不来，或者停在某个奇怪的角度，那就不算稳。

12.8 避坑清单

序号	坑	现象	原因	解决
1	PD符号反	越推越倒，正反馈	补偿方向错了	加负号，先做小角度测试
2	P太大	高频抖动	比例增益过大	降低P直到抖动消失
3	P太小	站不稳，像面条	纠正力不够	增大P直到能站稳
4	D太大	反应迟钝	阻尼过大	降低D
5	D太小	来回振荡不衰减	阻尼不够	增大D直到振荡衰减
6	归中0度	站不住直接倒	膝盖锁死重心偏后	用弯曲的站立姿势
7	假稳定	看起来稳但一碰就倒	线挂住或卡住	正反方向反复推验证
8	Ki累积	突然失控	积分项累积误差	不用Ki，只用PD
9	参数突变	切换时抖一下	无平滑过渡	0.2秒smoothstep过渡
10	左右参数相同	左右方向不稳	动力学特性不同	分别调参
11	不限幅	猛推时踝关节超限	超出机械范围	加限幅保护(max_ankle)

以上就是我在ZMP站立平衡上踩过的所有坑。每一个坑都花了至少半天，有些坑（比如PD符号反了）花了三天。希望这个清单能帮你节省时间。

下一节：4500元，手搓一台会走路的人形机器人|第 13 章 跨步恢复：被推倒时如何自救