最近人形机器人的热度确实很高各种机器人运动会、竞技展示和实验室测试视频频繁出现在信息流里。很多人看这类视频时注意力往往不是被“稳稳走完全程”的机器人吸引而是被那些走路摇摇晃晃、被轻轻一碰就踉跄好几步、甚至径直摔倒在地的机器人吸引。网友把这调侃为“醉酒机器人”这个说法虽然戏谑但背后其实是人形机器人运动控制里非常核心的问题姿态失稳。本文不聊段子而是从“机器人走路像喝醉”这个现象切入完整拆解人形机器人平衡控制的原理和工程实现包括姿态失稳的根本原因、ZMP 稳定判据、IMU 姿态估计、PID/MPC 控制思路并提供一个用 Python 编写的简化平衡控制仿真代码帮助你直观理解“参数调不好机器人就会像喝醉一样摔倒”这件事。如果你对机器人控制感兴趣、准备入门双足机器人或者正在做步态控制相关项目但经常被“站不稳”“一推就倒”的问题卡住这篇文章可以直接对照参考。1. 从“姿态失控”说起人形机器人为什么会出现醉酒式摇晃1.1 一个容易引发误解的话题首先需要明确一点机器人不会“喝酒”也不存在“醉酒模式”。所谓“醉酒机器人”本质上是机器人步态控制系统在受到外部扰动或自身状态估计偏差时步态表现出明显的摇晃、跌倒趋势。在各类机器人运动会上这种情况尤其常见。比赛场地地面摩擦力、光照、观众席震动、机器人之间的碰撞都会成为外部扰动源。一旦控制器没有及时补偿机器人就会出现类似人类喝醉后走路不稳的状态重心偏移、步幅错乱、上肢摆动幅度异常、最终摔倒。所以“醉酒机器人”是运动控制鲁棒性不足的直观表现而不是某种功能缺陷。理解这一点才能用正确的技术视角去分析问题。1.2 人形机器人走路的本质难点为什么四足机器人走路比双足机器人稳得多核心在于支撑面积。人类单脚站立时脚掌与地面的接触面积很小而人体重心又比较高这就构成了一个典型的“高重心、小支撑面”系统。双足机器人也一样它在行走过程中大部分时间都处于单腿支撑状态等效支撑多边形非常狭窄。更难处理的是自由度多。人形机器人一条腿通常有 5 到 6 个自由度加上髋关节、膝关节、踝关节再加上上肢的配重摆动整机自由度可以达到 20 到 30 个。每走一步控制器都要协调几十个关节的力矩和位置任何一环延迟或者误差过大都会反映到整体姿态上。另外人形机器人的动力学是强非线性系统。行走过程中质心高度变化、惯性力变化、地面反作用力变化都让精确建模变得困难。这就是为什么很多机器人团队在仿真里跑得很流畅一旦放到真机上就开始“打醉拳”。1.3 摔倒前发生了什么绝大多数摔倒并不是瞬间发生的而是有一个渐进过程。第一步外部扰动导致机器人姿态角发生偏移比如上身开始向前倾。 第二步IMU 等传感器检测到姿态变化控制器开始输出补偿力矩。 第三步如果补偿力矩不够或响应太慢质心投影点会逐渐向支撑脚边缘移动。 第四步当质心投影点超出支撑多边形地面无法提供足够的力矩来恢复姿态机器人就会摔倒。在运动控制中判断机器人是否稳定的一个关键指标是 ZMP也就是零力矩点。这一点在下文会详细展开。简单来说机器人摔倒不是因为“没站稳”而是因为 ZMP 跑出了脚掌范围。2. 人形机器人运动控制系统的基本构成要解决“醉酒式摇晃”首先得知道一个完整的人形机器人运动控制系统由哪些部分组成。2.1 感知层IMU 与关节编码器感知层负责告诉控制器“机器人现在处于什么状态”。最核心的传感器是 IMU即惯性测量单元。IMU 内部包含三轴加速度计和三轴陀螺仪加速度计可以测量重力方向陀螺仪可以测量角速度。通过融合这两类数据控制器可以估计机器人的俯仰角、横滚角和偏航角。除了 IMU关节编码器也很重要。它用于测量每个关节的实际角度和角速度让控制器知道各个关节当前的真实位置而不是只依赖指令值。在一些高端人形机器人上足底还会安装六维力传感器用来测量地面反作用力的方向和大小这也是判断稳定性的重要数据来源。2.2 决策层步态规划与平衡控制决策层是整个系统的“大脑”。步态规划负责生成机器人的行走动作包括髋关节和膝关节的轨迹、脚掌的抬起高度、步幅和步频等。常见的步态规划方法有基于倒立摆模型的步态生成、基于 ZMP 的步态生成以及近年来比较热门的强化学习步态生成。平衡控制则负责在行走过程中实时修正姿态偏差。它的输入是 IMU 姿态估计结果和当前关节状态输出是各个关节的补偿指令。平衡控制策略可以是简单的 PID也可以是复杂的模型预测控制甚至全身动力学控制。在真实系统中步态规划和平衡控制往往是分层的规划层给出期望参考轨迹控制层负责跟踪并补偿扰动。2.3 执行层关节伺服与力矩控制执行层是控制器指令的最终体现。人形机器人关节通常采用电机加减速器的方式驱动例如无框力矩电机配合谐波减速器。控制器计算出的位置指令或力矩指令会发送给关节伺服驱动器驱动器再通过电流环控制电机输出力矩。执行层的性能直接影响控制效果。如果关节响应速度慢、带宽不够控制器计算得再精确也没有用。很多机器人出现“软绵绵”的晃动就是因为执行层力矩响应跟不上决策层的指令。3. 环境准备与仿真工具选择在正式写代码之前先介绍一下常见的仿真和开发环境。本文后续的实战案例会采用纯 Python 实现方便读者在没有机器人硬件的情况下也能运行。3.1 常用工具链人形机器人开发常用的工具链有这几类第一类是动力学仿真平台例如 Gazebo、Webots、MuJoCo。Gazebo 通常和 ROS 2 配合使用适合做完整的机器人仿真MuJoCo 则被很多强化学习项目采用仿真速度快适合批量训练。第二类是数学模型工具例如 Python 加 NumPy 和 SciPy。对于理解控制原理、快速验证算法来说纯数学仿真成本最低也最容易调试。第三类是控制器开发工具例如 MATLAB Simulink。很多高校和企业在设计控制器时会用 Simulink 做快速原型验证然后再把算法移植到 ROS 或嵌入式代码里。如果你是第一次接触人形机器人控制我的建议是先用纯 Python 仿真理解平衡控制的本质再逐步迁移到 Gazebo 或 MuJoCo 进行更接近真实的仿真最后再上真机。这个顺序能帮你避开大量不必要的环境配置坑。3.2 版本与依赖说明本文的 Python 仿真示例依赖以下环境Python 3.8 及以上版本标准库 math用于数学计算如果使用 ROS 2建议选择与你的操作系统版本匹配的发行版例如 Ubuntu 22.04 对应 ROS 2 Humble具体版本号在机器人领域差异很大不同仓库、不同驱动、不同仿真器之间的兼容性都需要根据你的实际项目调整。本文示例的重点是演示控制逻辑不是绑定某个特定版本。3.3 为什么先用简化模型人形机器人全身动力学非常复杂直接对它进行控制设计新手很容易被淹没在矩阵运算和关节耦合里。一个非常经典的做法是把人形机器人简化为倒立摆模型。倒立摆虽然结构简单但它和双足机器人有共同的核心矛盾重心高于支撑点系统天然不稳定需要主动控制才能维持平衡。理解了一维倒立摆的平衡控制也就理解了人形机器人姿态稳定的基础。因此下文实战部分将建立一维倒立摆仿真通过 PID 控制让摆杆保持竖直并在运行到一半时施加外部扰动模拟“被撞了一下”的场景。4. 核心原理ZMP、姿态估计与平衡控制在进入代码之前先把三个关键概念讲清楚。4.1 ZMP判断是否倒下的关键指标ZMP 的全称是 Zero Moment Point中文一般翻译为零力矩点。它定义在支撑平面内是地面反作用力的作用点。这里可以把它理解为机器人脚底“力量汇聚的中心”。当机器人站立时如果 ZMP 落在脚掌范围内地面能够提供足够的支反力矩来维持姿态一旦 ZMP 超出脚掌范围机器人就失去了恢复平衡的能力。ZMP 的计算可以由总力矩和总力推出通常需要知道各关节的惯性参数和当前力传感器数据。在简化模型中可以把它近似看作质心在支撑平面上的投影点。对于双足机器人支撑多边形就是脚掌区域单脚支撑时就是一只脚掌的区域。判断机器人是否稳定可以用简单的几何判断实现例如# 文件路径zmp_check_demo.py # 以矩形脚掌为例定义支撑区域边界 x_min -0.1 # 脚掌 x 方向最小边界单位 m x_max 0.1 # 脚掌 x 方向最大边界单位 m y_min -0.05 # 脚掌 y 方向最小边界单位 m y_max 0.05 # 脚掌 y 方向最大边界单位 m # 假设由 IMU 与力传感器计算得到当前 ZMP 位置 zmp_x 0.03 zmp_y 0.0 if x_min zmp_x x_max and y_min zmp_y y_max: stable True print(ZMP 在支撑区域内当前稳定) else: stable False print(ZMP 超出支撑区域机器人即将倾倒)这段代码只是示意实际系统中 ZMP 的计算要复杂得多但稳定判据的思路是相通的。机器人在行走过程中步态规划的目标之一就是让 ZMP 轨迹始终保持在支撑多边形内部并且尽量靠近中心。4.2 用 IMU 估计当前姿态控制器要输出正确的补偿力矩前提是知道机器人当前倾斜了多少。IMU 中陀螺仪会测量角速度对角速度积分可以得到角度变化量但陀螺仪存在零偏积分会随时间漂移。加速度计可以测量加速度方向静止或缓慢运动时可以计算出重力方向与机器人本体的夹角但加速度计对运动加速度敏感在机器人快速摆动时测量值会引入较多噪声。因此实际系统通常使用互补滤波或者卡尔曼滤波把这两类数据融合起来。一个最简化的互补滤波思路如下# 文件路径imu_complementary_filter_demo.py import math # 假设陀螺仪得到的俯仰角速度单位 rad/s gyro_rate 0.02 # 假设加速度计计算得到的俯仰角单位 rad accel_angle 0.03 # 上一时刻的估计角度 previous_angle 0.0 dt 0.01 # 先由陀螺仪积分得到角度增量 angle_gyro previous_angle gyro_rate * dt # 融合系数 alpha实际需要根据传感器特性调整 alpha 0.98 # 互补滤波输出 angle alpha * angle_gyro (1 - alpha) * accel_angle print(f融合后俯仰角估计值: {math.degrees(angle):.3f}°)互补滤波的核心思想是陀螺仪在短时间内可信所以给高权重加速度计在长时间内没有漂移所以用较小权重来持续纠正积分漂移。这样既能获得平滑的角度又能避免长期漂移。在实际人形机器人中姿态估计输出会作为平衡控制器的反馈量。如果姿态估计偏差较大控制器会基于错误的状态输出错误的力矩机器人自然就会摇晃。4.3 PID 与 MPC 的角色PID 控制器结构简单、计算量小是人形机器人底层关节控制和姿态控制中最常见的方法。PID 控制器的输出由三部分组成比例项负责对当前误差做出反应积分项负责消除累积稳态误差微分项负责抑制误差变化速度提供阻尼效果。在姿态平衡控制中输入通常是当前姿态角与目标姿态角的误差输出是补偿力矩。当机器人在向前倾斜时控制器会输出一个反向力矩把姿态推回竖直方向。但 PID 的局限在于它只关注“当前”的误差缺乏对未来状态的预测能力。对于人形机器人这种强耦合、多关节系统尤其是在行走过程中单纯靠 PID 很难做到兼顾动态性和稳定性。MPC即模型预测控制会利用系统动力学模型对未来一段时间内的状态进行预测并在满足约束条件的前提下寻找最优控制输入。MPC 的优点是可以显式处理关节角度限制、力矩限制、ZMP 约束等因此被广泛应用于双足机器人的步态控制和全身动态控制。真实系统通常采用分层结构上层用 MPC 规划步态和质心轨迹下层用 PID 或力矩控制跟踪关节目标。这样既能利用 MPC 的预测能力又能利用 PID 的实时性和简单性。5. 完整实战用 Python 实现一个简化姿态平衡控制这一部分我们用一个完整的一维倒立摆仿真来模拟人形机器人姿态失稳和恢复过程。5.1 项目结构与思路创建以下文件balance_demo/ └── balance_demo.py仿真模型采用一维倒立摆。角度 theta 为 0 时表示摆杆竖直向上角度为正表示向一侧倾斜。控制器输出关节力矩 u目标是让摆杆保持在 theta 为 0 的位置。为了模拟机器人被碰撞仿真程序会在第 3 秒开始施加一个短暂的外部力矩扰动持续 0.1 秒。如果控制器参数合理摆杆会在短暂偏移后恢复竖直状态如果参数不合适摆杆角度会持续发散类似于机器人摔倒。5.2 完整代码# 文件路径balance_demo.py import math # 物理参数 m 1.0 # 摆杆质量单位 kg L 0.5 # 摆杆质心到转轴的距离单位 m g 9.81 # 重力加速度单位 m/s^2 dt 0.001 # 仿真步长单位 s sim_time 10.0 # 控制器参数PID Kp 80.0 Ki 10.0 Kd 15.0 # 初始状态 theta 0.05 # 初始角度单位 rad约为 2.86° omega 0.0 # 初始角速度单位 rad/s integral 0.0 # 扰动参数 disturbance_start 3.0 disturbance_duration 0.1 disturbance_magnitude 0.8 t 0.0 i 0 print(时间(s) 角度(°) 角速度(rad/s) 控制力矩(N·m)) while t sim_time: # 目标角度为 0计算当前误差 error 0.0 - theta integral error * dt derivative -omega # 目标角速度为 0误差微分为负的当前角速度 # PID 控制力矩 u Kp * error Ki * integral Kd * derivative # 外部扰动在设定时间窗口内施加一个力矩脉冲 disturbance 0.0 if disturbance_start t disturbance_start disturbance_duration: disturbance disturbance_magnitude # 简化一维倒立摆动力学 # θ (g / L) * θ u / (m * L^2) disturbance / (m * L^2) theta_ddot (g / L) * theta u / (m * L * L) disturbance / (m * L * L) # 欧拉积分 omega theta_ddot * dt theta omega * dt # 角度归一化到 [-π, π] theta (theta math.pi) % (2 * math.pi) - math.pi # 每 0.1 秒打印一次 if i % 100 0: print(f{t:6.2f} {math.degrees(theta):8.3f} {omega:12.4f} {u:12.4f}) t dt i 1 print(仿真结束。)这段代码可以直接复制运行不需要安装任何第三方库。运行命令如下python balance_demo.py5.3 代码逐段解释先看物理参数区。m 是摆杆质量L 是质心到转轴的距离g 是重力加速度dt 是仿真步长。这里把仿真步长设置为 0.001 秒也就是 1 毫秒比真实控制周期要快目的是让欧拉积分的误差足够小。PID 参数区定义了 Kp、Ki、Kd。这三个参数对系统行为影响巨大Kp 越大姿态角误差产生的恢复力矩越大系统响应越快但过大会引起振荡。Ki 用于消除稳态误差但如果初始条件已经接近目标积分项过大反而容易造成超调。Kd 相当于阻尼能够在角度快速变化时提供一个反向力矩抑制振荡。初始状态中theta 设置为 0.05 弧度相当于摆杆起始就有约 2.86° 的倾斜。这样仿真一开始就能看到控制器如何把摆杆拉回竖直位置。动力学方程是整段代码的核心。theta_ddot 表示角加速度它由三部分组成重力项、控制力矩项和外部扰动项。重力项是发散的角度越大把它继续推倒的角加速度就越大控制力矩项是稳定项由 PID 输出决定外部扰动项只在设定的时间窗口内非零。在积分部分采用欧拉法更新角速度和角度。欧拉法的优点是简单直观缺点是在大步长下精度较差。这里因为 dt 只有 0.001 秒精度足够。最后每 100 个仿真步长打印一次数据也就是每 0.1 秒打印一行方便观察姿态随时间的变化。5.4 运行与结果分析如果你按上面的参数运行会观察到以下现象0 到 3 秒摆杆从初始倾斜状态逐渐被拉回竖直方向角度接近 0。3 秒到 3.1 秒外部扰动短暂增大角度摆杆出现明显偏移。3.1 秒之后控制器继续输出反向力矩角度逐渐恢复至 0 附近。预期输出的第一行大致如下时间(s) 角度(°) 角速度(rad/s) 控制力矩(N·m) 0.00 2.863 0.0000 -5.5000为什么第一行控制力矩是负值因为初始角度为正PID 误差为负控制器会输出负力矩把摆杆往相反方向压。你可以尝试修改 Kp、Ki、Kd 的值观察现象如果把 Kp 调到 200系统响应会变快但可能出现明显的振荡。如果把 Kd 调为 0系统阻尼不足摆杆会在竖直位置附近来回摆动。如果三个系数都调得很小摆杆在扰动后会缓慢恢复甚至直接发散。你可以把扰动 magnitude 从 0.8 改到 2.0 再运行一次就能直观感受到“外部冲击过大控制器也无能为力”的情况。这和人形机器人在运动会上被大力碰撞后摔倒是一个道理。6. 常见问题与排查思路在调试人形机器人平衡控制时你可能会遇到下面这些典型现象。这里整理了一份排查思路。问题现象常见原因解决思路机器人启动后一直朝固定方向倾倒重心偏置、IMU 安装角度偏移、初始姿态估计错误检查机械重心是否居中校准 IMU 零偏和安装角确认初始姿态角的目标值是否合理小扰动后姿态发散无法恢复控制增益太小、执行器响应过慢、关节力矩饱和增大 Kp 和 Kd检查电机是否达到力矩上限缩短控制周期机器人持续高频抖动Kd 过大、传感器噪声放大、机械结构刚度不足适当降低 Kd对 IMU 数据增加滤波检查机械连接是否有间隙仿真稳定但真机不稳定模型误差、传动延迟、摩擦力、结构柔性先做硬件在环仿真逐步加入模型误差和延迟再进行真机参数整定行走时单脚支撑阶段容易摔倒ZMP 轨迹偏移、步态规划不合理检查步态周期内 ZMP 是否始终落在支撑多边形内调整步幅和上身预倾斜角度6.1 机器人一直朝一个方向倒这是最常见的问题。很多情况下不是因为 PID 没调好而是因为初始姿态估计就不准。例如 IMU 安装时本身倾斜了 2 度或者陀螺仪零偏未补偿控制器会认为机器人已经竖直实际上机器人已经歪了。这种情况下控制器的目标姿态本身是错的无论怎么调参数都很难站稳。排查时先做静态校准把机器人放在水平平台上读取 IMU 输出的姿态角确认输出角度接近 0。如果不接近需要先标定传感器零偏和安装角。6.2 机器人在小扰动下发散如果机器人静止时能站稳但轻轻一碰就倒问题很可能出在控制器的抗扰能力不足。在 PID 控制中恢复力矩取决于姿态误差的大小和变化速度。当外部扰动导致角度快速变化时Kd 项会提供最强的抑制力矩。如果 Kd 太小系统在受到冲击后会先出现较大的角度偏移再慢慢恢复一旦偏移量太大重力项造成的角加速度会超过控制器能提供的恢复力矩系统就会发散。这就像一个平衡木运动员如果重心已经偏离支撑点很远了肌肉力量再大也拉不回来。所以抗扰控制的关键是在扰动发生的前期尽可能快速输出反向力矩。6.3 机器人出现高频抖动抖动通常是因为控制器发出的力矩在大幅交替变化。常见原因有两个一是 Kd 太大导致控制器对微小角速度噪声过度反应二是传感器原始数据噪声大而系统没有做足够滤波。解决办法是降低 Kd 或提高传感器滤波强度。但要注意过度滤波会增加相位延迟反而降低控制带宽。实际工程中需要在噪声抑制和响应速度之间取平衡。6.4 仿真稳定但真机不行这一类问题最让人头疼。仿真环境是理想化的没有通信延迟、没有关节摩擦、没有结构柔性、电机力矩输出即时且无饱和。真机完全不一样。应对方法是逐步逼近现实。先在仿真中人为加入控制周期延迟、传感器噪声、力矩饱和约束观察控制算法是否还能稳定。如果仿真中加入这些因素后仍然稳定再上真机调试成功率会高很多。7. 工程实践与落地建议除了调好控制器工程上的做法同样重要。下面整理一些在真实人形机器人项目中值得关注的实践建议。7.1 从仿真到真机的必经步骤很多人希望直接把仿真调好的参数烧进真机结果往往很惨。更稳妥的流程是第一步先在纯数学仿真中验证算法逻辑确认没有算法层面的错误。 第二步在 Gazebo 或 MuJoCo 中验证基于物理引擎的仿真加入碰撞、摩擦等真实因素。 第三步在真机前先做硬件在环测试用真实控制器连接仿真模型验证代码路径和通信链路。 第四步把机器人用吊架或平衡台悬挂起来在受限空间内测试腿部运动。 第五步再逐步放开自由度从站立平衡开始逐步过渡到行走。每一步都加上安全保护措施可以大幅降低摔坏机器人的风险。7.2 安全边界设计人形机器人关节多、体积大、力矩高一旦失控对周围设备和人员都很危险。在机械层面应该设计关节限位防止关节超出运动范围后造成机械损坏。在控制层面需要实现力矩限制当指令力矩超过安全阈值时自动截断。在软件层面要加入看门狗机制如果控制器任务超时或传感器数据异常系统应该进入急停状态而不是继续执行错误指令。对干扰动实验也建议先在仿真中确认扰动范围再在真机上从很小的冲击力度开始测试。7.3 主控芯片与算力选型注意点最近关于人形机器人芯片的讨论很多例如全志科技等国内芯片厂商在机器人主控方向的布局就受到不少关注。具体型号、算力参数和 SDK 支持情况建议以厂商官方资料为准。从工程经验来看选择人形机器人主控芯片主要看几个方面第一是接口丰富度。人形机器人需要连接 IMU、关节伺服驱动器、力传感器、摄像头、激光雷达等设备主控芯片需要提供足够的 UART、SPI、I2C、CAN、USB 和以太网接口。第二是实时性。运动控制对实时性要求很高一般需要在 1 kHz 左右的控制频率下运行。普通 Linux 系统如果不做实时补丁很难保证稳定的控制周期。第三是算力余量。如果后续要接入视觉识别或强化学习推理需要预留足够的 NPU 或 GPU 算力。如果只做运动控制算力要求相对较低但对实时性要求更高。第四是生态和开发效率。芯片的 SDK 是否完善、示例代码是否丰富、底层驱动是否稳定直接影响项目开发周期。7.4 参数整定与日志记录平衡控制的参数整定尤其是 PID 参数不能靠直觉乱调。建议遵循顺序先调 Kp让系统有足够的恢复能力再调 Kd增加阻尼抑制振荡最后再根据稳态误差情况决定是否引入 Ki。所有调试过程都要记录日志。日志至少应包含时间戳、各关节目标角度、实际角度、IMU 姿态角、控制输出力矩、是否出现饱和。没有日志的调试相当于盲人摸象很多真机问题都需要回放日志才能定位。在代码工程中推荐把控制器参数放到配置文件里而不是硬编码在源码中。这样可以在不重新编译的情况下调整 Kp、Kd 等参数提高调试效率。8. 总结与进一步学习方向本文从“机器人走路像喝醉”这个现象出发讨论了人形机器人平衡控制的核心问题。你可以把这篇文章看作一个起点它解释了一个常见的工程现象给出了一个能运行的最小仿真以及一套排查姿态失稳的方法论。如果你把 balance_demo.py 中的参数多改几组你会直观地感受到 Kp 和 Kd 对系统稳定性有多么关键。把扰动幅度从 0.8 改成 2.0你会理解为什么机器人在面对强烈碰撞时即使控制器正常也可能“救不回来”。这种直观感受比死记控制公式有价值得多。下一步的学习方向可以考虑这样安排一是去学习倒立摆模型的完整推导理解为什么人形机器人步态规划经常以倒立摆为基础模型。 二是从 PID 升级到 LQR再做 MPC逐步掌握基于模型的最优控制方法。 三是了解全身动力学控制理解各个关节之间的耦合关系以及如何用优化方法求解全身控制问题。 四是关注强化学习在步态生成中的应用这是目前人形机器人领域最活跃的方向之一。人形机器人的真正难点从来不是“能走”而是“怎么走都摔不倒”。理解了这个目标你再看那些被网友调侃为“醉酒机器人”的视频视角会完全不一样。