简介这份资源是Marc H. Raibert所著《Legged Robots that Balance》的中文整理文档面向足式机器人、动态平衡控制与人工智能方向的研究生、工程师及科研人员帮助读者系统理解腿足式机器人如何通过主动平衡、运动控制与动力学建模实现稳定行走与奔跑。内容涵盖主动平衡、运动控制分解、动力学模型、传感器融合与反馈控制等关键技术并回顾了从弹跳杆原型到Leg实验室步行车辆研究的发展脉络涉及爬坡、跨越障碍等复杂运动场景。资源包共1个PDF文件大小约7.13MB便于在电脑或平板上直接阅读与检索。目前已有393人学习适合希望深入动态腿部系统控制这一开放问题的读者作为理论参考与算法入门材料。1. 从「站不稳」到「推不倒」足式机器人平衡算法到底在算什么很多人第一次接触足式机器人注意力都在关节电机和步态规划上觉得腿抬得够高、落点够准就能走。真跑起来才发现机器人走直线没问题被人从侧面轻推一下就倒或者在斜坡上越走越飘。问题不在腿在平衡算法——它要解决的是「机器人此刻离摔倒还有多远以及用多大力、多快把自己拉回来」。《Legged Robots that Balance》这个标题指向的正是这套核心逻辑把机器人当成一个受重力、惯性、地面反作用力共同作用的动力学系统用状态估计判断当前是否稳定再用控制律把状态拉回稳定域。它适合做足式机器人控制、运动规划、仿真到实机迁移的工程师也适合想从零理解「为什么双足比四足难」的开发者。这一章先把平衡问题的边界划清楚后面几章再落到可复现的算法和参数。2. 足式机器人平衡算法的动力学基础与状态建模2.1 为什么倒立摆模型是足式平衡的起点足式机器人平衡最常用的简化模型是线性倒立摆LIPM。把机器人整体质量压到一个质心点上腿当成无质量伸缩杆地面接触点固定质心在重力作用下绕接触点做倒立摆运动。这个模型丢掉了关节细节但保留了平衡问题最本质的矛盾质心一旦越过支撑多边形边界重力矩就会把机器人拉倒而且越倒越快。LIPM 的动力学方程可以写成质心水平加速度与质心相对接触点偏移成正比x_ddot (g / z_c) * (x - p)其中x是质心水平位置p是接触点压力中心 ZMP位置z_c是质心高度g是重力加速度。这个式子说明一个反直觉结论想让质心往右加速反而要把接触点放到质心左边。很多初学者调平衡时凭直觉把落脚点往质心方向挪结果越挪越倒就是因为没吃透这个符号关系。2.2 状态空间建模把平衡问题写成可计算的形式实际控制器不会直接解微分方程而是把 LIPM 离散成状态空间形式。取状态向量s [x, x_dot, x_ddot]输入为 ZMP 位置p连续系统可以写成import numpy as np g 9.81 z_c 0.8 # 质心高度单位米 # 连续时间状态矩阵 A 和输入矩阵 B A np.array([[0, 1, 0], [0, 0, 1], [0, 0, 0]]) A[2, 0] g / z_c # 质心加速度对位置的敏感度 B np.array([[0], [0], [-g / z_c]]) # 零阶保持离散化dt 为控制周期 dt 0.01 Ad np.eye(3) A * dt (A A) * dt**2 / 2 Bd B * dt (A B) * dt**2 / 2这段代码把连续 LIPM 离散成s[k1] Ad s[k] Bd p[k]方便在数字控制器里按周期递推。z_c是关键参数质心越低g/z_c越大系统越不稳定对控制带宽要求越高。四足机器人通常把z_c设在 0.3 到 0.5 米双足人形在 0.8 到 1.0 米这个值直接决定后面 LQR 或 MPC 的权重怎么调。2.3 支撑多边形与稳定判据的工程化计算理论上的稳定判据是 ZMP 落在支撑多边形内但工程上要算的是「离边界还有多少余量」。支撑多边形由当前着地足端位置决定四足站立时是四个足端围成的四边形双足单脚支撑时是一只脚的轮廓。常用做法是把足端位置投影到水平面用凸包算法求多边形再算 ZMP 到各边的距离。from scipy.spatial import ConvexHull import numpy as np # 四个足端水平坐标单位米 foot_xy np.array([[0.2, 0.15], [0.2, -0.15], [-0.2, 0.15], [-0.2, -0.15]]) hull ConvexHull(foot_xy) polygon foot_xy[hull.vertices] # 按逆时针排列的支撑多边形顶点 def zmp_margin(zmp, polygon): 计算 ZMP 到支撑多边形各边的最小有符号距离 margins [] n len(polygon) for i in range(n): p1 polygon[i] p2 polygon[(i 1) % n] edge p2 - p1 normal np.array([-edge[1], edge[0]]) normal normal / np.linalg.norm(normal) dist np.dot(zmp - p1, normal) margins.append(dist) return min(margins)zmp_margin返回正值表示 ZMP 在多边形内负值表示已经越界。工程上一般要求余量大于 2 到 3 厘米低于这个值就触发步态切换或减速。注意凸包顶点顺序会影响法线方向如果算出来符号反了把normal取反即可这是最常见的实现坑。3. 用 LQR 和 MPC 实现足式机器人平衡控制3.1 LQR 控制器的最小实现与权重调参LQR 是足式平衡里最常用的基线控制器因为它计算量小、能跑在 1kHz 以上的控制周期里。目标是最小化状态偏差和控制量的加权平方和J sum( s[k]^T Q s[k] p[k]^T R p[k] )Q是 3x3 状态权重矩阵R是标量或 1x1 输入权重。调参经验是先调Q里位置项的权重让机器人对位置偏差敏感再调速度项抑制振荡R越大控制越柔和但恢复越慢。from scipy.linalg import solve_discrete_are import numpy as np # 沿用上一章的 Ad, Bd Q np.diag([100.0, 10.0, 1.0]) # 位置、速度、加速度权重 R np.array([[0.01]]) # ZMP 控制量权重 # 求解离散代数 Riccati 方程 P solve_discrete_are(Ad, Bd, Q, R) K np.linalg.inv(Bd.T P Bd R) (Bd.T P Ad) # 在线控制根据当前状态算 ZMP 修正量 def lqr_control(state, K, p_nominal): return p_nominal - float(K state)K是 1x3 反馈增益p_nominal是步态规划给出的名义 ZMP。Q和R的比例决定闭环极点位置Q相对R越大响应越快但容易抖反之越稳但恢复慢。实际调试时先把R设大确认机器人不抖再逐步减小R直到恢复速度满意。注意solve_discrete_are要求Ad和Bd可控如果z_c设得离谱导致矩阵奇异会直接报错。3.2 MPC 滚动优化把未来几步的平衡一起算进去LQR 只看当前状态遇到连续扰动或需要提前减速的场景就不够用。MPC 在预测时域内优化一串 ZMP 轨迹能提前把质心速度压下来。常见做法是预测 10 到 20 步控制周期 10ms预测时域 0.1 到 0.2 秒。import numpy as np from scipy.optimize import minimize N 15 # 预测步数 dt 0.01 def mpc_balance(state, Ad, Bd, N, Q, R, p_ref): 滚动优化求解 ZMP 序列返回第一个控制量 n Ad.shape[0] # 构造预测矩阵 A_pow [np.eye(n)] for i in range(1, N 1): A_pow.append(A_pow[-1] Ad) Sx np.vstack([A_pow[i] for i in range(1, N 1)]) Su np.zeros((N * n, N)) for i in range(1, N 1): for j in range(i): Su[(i - 1) * n:i * n, j:j 1] A_pow[i - 1 - j] Bd Q_bar np.kron(np.eye(N), Q) R_bar np.kron(np.eye(N), R) def cost(u): u u.reshape(-1, 1) pred Sx state Su u return float(pred.T Q_bar pred u.T R_bar u) res minimize(cost, np.zeros(N), methodSLSQP, bounds[(-0.3, 0.3)] * N) return res.x[0]Sx和Su是预测矩阵把未来 N 步状态表示成当前状态和控制序列的线性组合。bounds限制 ZMP 不能超出物理可达范围这个约束比 LQR 的软权重更硬。MPC 的坑在于求解时间SLSQP在 N15 时单次约 1 到 3ms如果控制周期是 1ms 就跑不动需要换 OSQP 或手写 QP 求解器。另外p_ref名义轨迹如果和实际步态差太多MPC 会一直顶到边界表现为机器人持续前倾。3.3 两种控制器的选型对比维度LQRMPC计算量极低矩阵乘法中高需求解 QP控制周期1kHz 以上100Hz 到 500Hz约束处理只能靠权重软约束硬约束ZMP 边界可控抗连续扰动一般好能提前减速调参难度低Q/R 两个旋钮高还要调时域和约束适用场景平地行走、站立平衡斜坡、外力冲击、复杂地形选型建议先上 LQR 把基本平衡跑通确认状态估计和 ZMP 计算没问题再换 MPC 处理复杂场景。直接上 MPC 容易在状态估计不准时把问题归咎于优化器浪费大量调试时间。4. 仿真到实机的平衡算法调试与排错4.1 在 MuJoCo 里搭一个最小平衡测试场景仿真阶段建议用 MuJoCo它的接触求解稳定适合验证 ZMP 和质心轨迹。最小场景只需要一个浮动基座加四条腿地面设摩擦系数 0.8 到 1.0。mujoco option timestep0.001 gravity0 0 -9.81/ worldbody geom namefloor typeplane size5 5 0.1 friction1.0 0.005 0.0001/ body nametrunk pos0 0 0.4 freejoint/ geom typebox size0.2 0.15 0.05 mass8/ !-- 四条腿的关节和连杆省略按实际结构补 -- /body /worldbody /mujocofriction第一个值是滑动摩擦系数设太低机器人会打滑ZMP 算出来全是边界值。timestep设 0.001 是为了让接触求解收敛控制周期可以在代码里降频到 0.01。仿真里先测三件事静止站立时 ZMP 是否在支撑多边形中心、给质心一个初始速度后能否在 2 秒内收敛、侧向推力 20N 持续 0.1 秒后是否恢复。4.2 实机调试的 5 个必查项仿真过了不代表实机能跑足式机器人从仿真到实机的差距主要在状态估计和执行延迟。按下面顺序排查质心位置标定用悬挂法或称重法测实际质心和 URDF 里的值对比偏差超过 2 厘米就要改模型。IMU 姿态延迟用示波器测 IMU 数据到控制器的延迟超过 5ms 要换滤波或降截止频率。关节零位偏差每个关节给零力矩看实际角度和编码器读数差超过 0.5 度要重新标定。足端接触检测用足底开关或关节电流判断着地接触检测延迟直接决定 ZMP 计算是否及时。控制周期抖动用 GPIO 翻转测实际控制周期抖动超过 20% 会让 LQR 增益失配。提示实机第一次跑平衡时把Q里位置权重降到仿真值的 1/5R放大 5 倍先让机器人「软」一点确认不摔再逐步加硬。4.3 常见振荡与发散的原因对照现象可能原因排查方法高频抖动控制周期抖动大、Q 位置权重过高测周期抖动降 Q低频摆动速度项权重不足、IMU 滤波截止频率太低加 Q 速度项提高截止频率缓慢前倾ZMP 名义值偏后、质心标定偏前查 p_ref 和质心位置侧向发散支撑多边形计算错误、足端坐标符号反打印多边形顶点和 ZMP推一下恢复但超调R 太小、MPC 时域太短加 R加预测步数这张表里的现象在调试现场基本能覆盖八成问题。关键是每次只改一个参数改完记录现象否则多个参数耦合时根本分不清是谁的锅。5. 平衡算法的进阶技巧从能站到能抗扰5.1 用捕获点判断「还来不来得及迈步」捕获点Capture Point是平衡算法里最实用的进阶概念。它回答的问题是如果现在不迈步机器人最多能靠踝关节力矩把自己拉回到哪个位置。LIPM 下捕获点公式为xi x x_dot * sqrt(z_c / g)xi就是捕获点。如果xi落在当前支撑多边形内踝关节策略够用如果落在外面必须迈步而且落脚点要超过xi才能停住。工程上把xi和支撑多边形边界的距离作为步态触发条件比单纯看 ZMP 余量更提前能避免「已经来不及迈步」的尴尬。import numpy as np def capture_point(x, x_dot, z_c, g9.81): return x x_dot * np.sqrt(z_c / g) def should_step(xi, polygon, margin0.03): 捕获点离支撑多边形边界小于 margin 时触发迈步 from scipy.spatial import ConvexHull hull ConvexHull(polygon) # 简化判断用凸包顶点到 xi 的最小距离 dists [np.linalg.norm(xi - v) for v in polygon[hull.vertices]] return min(dists) marginmargin设 3 厘米是经验值太小会频繁迈步太大则反应迟钝。注意捕获点公式假设质心高度恒定如果机器人下蹲或跳跃z_c变化时要重新算。5.2 外力估计与扰动前馈补偿纯反馈控制在遇到持续外力比如被人推着走时会有稳态误差因为 LQR 和 MPC 都假设扰动是脉冲式的。进阶做法是用动量观测器估计外力再把估计值前馈到 ZMP 目标里。class MomentumObserver: def __init__(self, mass, dt, cutoff20.0): self.mass mass self.dt dt self.alpha dt / (1.0 / (2 * np.pi * cutoff) dt) self.momentum np.zeros(3) self.f_ext np.zeros(3) def update(self, velocity, joint_torque_effect, gravity_effect): # 动量变化 关节力矩项 重力项 外力 p_dot joint_torque_effect gravity_effect self.momentum p_dot * self.dt residual self.mass * velocity - self.momentum self.f_ext self.alpha * residual (1 - self.alpha) * self.f_ext return self.f_extcutoff是低通滤波截止频率设 20Hz 能滤掉关节噪声但保留推力信号。估计出的f_ext除以mass得到等效加速度加到 ZMP 目标上做前馈。这个技巧在协作场景里特别有用机器人被推时不会硬顶而是顺着力的方向调整步态。5.3 平衡算法的验证清单最后给一份可执行的验证清单按顺序跑完基本能确认平衡算法是否可靠测试项通过标准工具静止站立 30 秒ZMP 余量始终大于 2cm仿真/实机日志脉冲推力 50N/0.1s2 秒内恢复超调小于 5cm推力计持续推力 20N/2s稳态误差小于 3cm推力计斜坡 10 度站立不滑动ZMP 在支撑多边形内可调斜坡单足抬起 0.5 秒不摔倒落地后 1 秒恢复步态触发连续行走 10 米无步态发散ZMP 余量周期性变化运动捕捉跑完这张表平衡算法从「能站」到「推不倒」的路径基本就闭环了。剩下的就是根据具体机型调Q、R、z_c和捕获点margin这些参数没有万能值只能按实测现象迭代。本文还有配套的精品资源点击获取 足式机器人平衡算法：从LQR到MPC的动力学建模与调试
简介这份资源是Marc H. Raibert所著《Legged Robots that Balance》的中文整理文档面向足式机器人、动态平衡控制与人工智能方向的研究生、工程师及科研人员帮助读者系统理解腿足式机器人如何通过主动平衡、运动控制与动力学建模实现稳定行走与奔跑。内容涵盖主动平衡、运动控制分解、动力学模型、传感器融合与反馈控制等关键技术并回顾了从弹跳杆原型到Leg实验室步行车辆研究的发展脉络涉及爬坡、跨越障碍等复杂运动场景。资源包共1个PDF文件大小约7.13MB便于在电脑或平板上直接阅读与检索。目前已有393人学习适合希望深入动态腿部系统控制这一开放问题的读者作为理论参考与算法入门材料。1. 从「站不稳」到「推不倒」足式机器人平衡算法到底在算什么很多人第一次接触足式机器人注意力都在关节电机和步态规划上觉得腿抬得够高、落点够准就能走。真跑起来才发现机器人走直线没问题被人从侧面轻推一下就倒或者在斜坡上越走越飘。问题不在腿在平衡算法——它要解决的是「机器人此刻离摔倒还有多远以及用多大力、多快把自己拉回来」。《Legged Robots that Balance》这个标题指向的正是这套核心逻辑把机器人当成一个受重力、惯性、地面反作用力共同作用的动力学系统用状态估计判断当前是否稳定再用控制律把状态拉回稳定域。它适合做足式机器人控制、运动规划、仿真到实机迁移的工程师也适合想从零理解「为什么双足比四足难」的开发者。这一章先把平衡问题的边界划清楚后面几章再落到可复现的算法和参数。2. 足式机器人平衡算法的动力学基础与状态建模2.1 为什么倒立摆模型是足式平衡的起点足式机器人平衡最常用的简化模型是线性倒立摆LIPM。把机器人整体质量压到一个质心点上腿当成无质量伸缩杆地面接触点固定质心在重力作用下绕接触点做倒立摆运动。这个模型丢掉了关节细节但保留了平衡问题最本质的矛盾质心一旦越过支撑多边形边界重力矩就会把机器人拉倒而且越倒越快。LIPM 的动力学方程可以写成质心水平加速度与质心相对接触点偏移成正比x_ddot (g / z_c) * (x - p)其中x是质心水平位置p是接触点压力中心 ZMP位置z_c是质心高度g是重力加速度。这个式子说明一个反直觉结论想让质心往右加速反而要把接触点放到质心左边。很多初学者调平衡时凭直觉把落脚点往质心方向挪结果越挪越倒就是因为没吃透这个符号关系。2.2 状态空间建模把平衡问题写成可计算的形式实际控制器不会直接解微分方程而是把 LIPM 离散成状态空间形式。取状态向量s [x, x_dot, x_ddot]输入为 ZMP 位置p连续系统可以写成import numpy as np g 9.81 z_c 0.8 # 质心高度单位米 # 连续时间状态矩阵 A 和输入矩阵 B A np.array([[0, 1, 0], [0, 0, 1], [0, 0, 0]]) A[2, 0] g / z_c # 质心加速度对位置的敏感度 B np.array([[0], [0], [-g / z_c]]) # 零阶保持离散化dt 为控制周期 dt 0.01 Ad np.eye(3) A * dt (A A) * dt**2 / 2 Bd B * dt (A B) * dt**2 / 2这段代码把连续 LIPM 离散成s[k1] Ad s[k] Bd p[k]方便在数字控制器里按周期递推。z_c是关键参数质心越低g/z_c越大系统越不稳定对控制带宽要求越高。四足机器人通常把z_c设在 0.3 到 0.5 米双足人形在 0.8 到 1.0 米这个值直接决定后面 LQR 或 MPC 的权重怎么调。2.3 支撑多边形与稳定判据的工程化计算理论上的稳定判据是 ZMP 落在支撑多边形内但工程上要算的是「离边界还有多少余量」。支撑多边形由当前着地足端位置决定四足站立时是四个足端围成的四边形双足单脚支撑时是一只脚的轮廓。常用做法是把足端位置投影到水平面用凸包算法求多边形再算 ZMP 到各边的距离。from scipy.spatial import ConvexHull import numpy as np # 四个足端水平坐标单位米 foot_xy np.array([[0.2, 0.15], [0.2, -0.15], [-0.2, 0.15], [-0.2, -0.15]]) hull ConvexHull(foot_xy) polygon foot_xy[hull.vertices] # 按逆时针排列的支撑多边形顶点 def zmp_margin(zmp, polygon): 计算 ZMP 到支撑多边形各边的最小有符号距离 margins [] n len(polygon) for i in range(n): p1 polygon[i] p2 polygon[(i 1) % n] edge p2 - p1 normal np.array([-edge[1], edge[0]]) normal normal / np.linalg.norm(normal) dist np.dot(zmp - p1, normal) margins.append(dist) return min(margins)zmp_margin返回正值表示 ZMP 在多边形内负值表示已经越界。工程上一般要求余量大于 2 到 3 厘米低于这个值就触发步态切换或减速。注意凸包顶点顺序会影响法线方向如果算出来符号反了把normal取反即可这是最常见的实现坑。3. 用 LQR 和 MPC 实现足式机器人平衡控制3.1 LQR 控制器的最小实现与权重调参LQR 是足式平衡里最常用的基线控制器因为它计算量小、能跑在 1kHz 以上的控制周期里。目标是最小化状态偏差和控制量的加权平方和J sum( s[k]^T Q s[k] p[k]^T R p[k] )Q是 3x3 状态权重矩阵R是标量或 1x1 输入权重。调参经验是先调Q里位置项的权重让机器人对位置偏差敏感再调速度项抑制振荡R越大控制越柔和但恢复越慢。from scipy.linalg import solve_discrete_are import numpy as np # 沿用上一章的 Ad, Bd Q np.diag([100.0, 10.0, 1.0]) # 位置、速度、加速度权重 R np.array([[0.01]]) # ZMP 控制量权重 # 求解离散代数 Riccati 方程 P solve_discrete_are(Ad, Bd, Q, R) K np.linalg.inv(Bd.T P Bd R) (Bd.T P Ad) # 在线控制根据当前状态算 ZMP 修正量 def lqr_control(state, K, p_nominal): return p_nominal - float(K state)K是 1x3 反馈增益p_nominal是步态规划给出的名义 ZMP。Q和R的比例决定闭环极点位置Q相对R越大响应越快但容易抖反之越稳但恢复慢。实际调试时先把R设大确认机器人不抖再逐步减小R直到恢复速度满意。注意solve_discrete_are要求Ad和Bd可控如果z_c设得离谱导致矩阵奇异会直接报错。3.2 MPC 滚动优化把未来几步的平衡一起算进去LQR 只看当前状态遇到连续扰动或需要提前减速的场景就不够用。MPC 在预测时域内优化一串 ZMP 轨迹能提前把质心速度压下来。常见做法是预测 10 到 20 步控制周期 10ms预测时域 0.1 到 0.2 秒。import numpy as np from scipy.optimize import minimize N 15 # 预测步数 dt 0.01 def mpc_balance(state, Ad, Bd, N, Q, R, p_ref): 滚动优化求解 ZMP 序列返回第一个控制量 n Ad.shape[0] # 构造预测矩阵 A_pow [np.eye(n)] for i in range(1, N 1): A_pow.append(A_pow[-1] Ad) Sx np.vstack([A_pow[i] for i in range(1, N 1)]) Su np.zeros((N * n, N)) for i in range(1, N 1): for j in range(i): Su[(i - 1) * n:i * n, j:j 1] A_pow[i - 1 - j] Bd Q_bar np.kron(np.eye(N), Q) R_bar np.kron(np.eye(N), R) def cost(u): u u.reshape(-1, 1) pred Sx state Su u return float(pred.T Q_bar pred u.T R_bar u) res minimize(cost, np.zeros(N), methodSLSQP, bounds[(-0.3, 0.3)] * N) return res.x[0]Sx和Su是预测矩阵把未来 N 步状态表示成当前状态和控制序列的线性组合。bounds限制 ZMP 不能超出物理可达范围这个约束比 LQR 的软权重更硬。MPC 的坑在于求解时间SLSQP在 N15 时单次约 1 到 3ms如果控制周期是 1ms 就跑不动需要换 OSQP 或手写 QP 求解器。另外p_ref名义轨迹如果和实际步态差太多MPC 会一直顶到边界表现为机器人持续前倾。3.3 两种控制器的选型对比维度LQRMPC计算量极低矩阵乘法中高需求解 QP控制周期1kHz 以上100Hz 到 500Hz约束处理只能靠权重软约束硬约束ZMP 边界可控抗连续扰动一般好能提前减速调参难度低Q/R 两个旋钮高还要调时域和约束适用场景平地行走、站立平衡斜坡、外力冲击、复杂地形选型建议先上 LQR 把基本平衡跑通确认状态估计和 ZMP 计算没问题再换 MPC 处理复杂场景。直接上 MPC 容易在状态估计不准时把问题归咎于优化器浪费大量调试时间。4. 仿真到实机的平衡算法调试与排错4.1 在 MuJoCo 里搭一个最小平衡测试场景仿真阶段建议用 MuJoCo它的接触求解稳定适合验证 ZMP 和质心轨迹。最小场景只需要一个浮动基座加四条腿地面设摩擦系数 0.8 到 1.0。mujoco option timestep0.001 gravity0 0 -9.81/ worldbody geom namefloor typeplane size5 5 0.1 friction1.0 0.005 0.0001/ body nametrunk pos0 0 0.4 freejoint/ geom typebox size0.2 0.15 0.05 mass8/ !-- 四条腿的关节和连杆省略按实际结构补 -- /body /worldbody /mujocofriction第一个值是滑动摩擦系数设太低机器人会打滑ZMP 算出来全是边界值。timestep设 0.001 是为了让接触求解收敛控制周期可以在代码里降频到 0.01。仿真里先测三件事静止站立时 ZMP 是否在支撑多边形中心、给质心一个初始速度后能否在 2 秒内收敛、侧向推力 20N 持续 0.1 秒后是否恢复。4.2 实机调试的 5 个必查项仿真过了不代表实机能跑足式机器人从仿真到实机的差距主要在状态估计和执行延迟。按下面顺序排查质心位置标定用悬挂法或称重法测实际质心和 URDF 里的值对比偏差超过 2 厘米就要改模型。IMU 姿态延迟用示波器测 IMU 数据到控制器的延迟超过 5ms 要换滤波或降截止频率。关节零位偏差每个关节给零力矩看实际角度和编码器读数差超过 0.5 度要重新标定。足端接触检测用足底开关或关节电流判断着地接触检测延迟直接决定 ZMP 计算是否及时。控制周期抖动用 GPIO 翻转测实际控制周期抖动超过 20% 会让 LQR 增益失配。提示实机第一次跑平衡时把Q里位置权重降到仿真值的 1/5R放大 5 倍先让机器人「软」一点确认不摔再逐步加硬。4.3 常见振荡与发散的原因对照现象可能原因排查方法高频抖动控制周期抖动大、Q 位置权重过高测周期抖动降 Q低频摆动速度项权重不足、IMU 滤波截止频率太低加 Q 速度项提高截止频率缓慢前倾ZMP 名义值偏后、质心标定偏前查 p_ref 和质心位置侧向发散支撑多边形计算错误、足端坐标符号反打印多边形顶点和 ZMP推一下恢复但超调R 太小、MPC 时域太短加 R加预测步数这张表里的现象在调试现场基本能覆盖八成问题。关键是每次只改一个参数改完记录现象否则多个参数耦合时根本分不清是谁的锅。5. 平衡算法的进阶技巧从能站到能抗扰5.1 用捕获点判断「还来不来得及迈步」捕获点Capture Point是平衡算法里最实用的进阶概念。它回答的问题是如果现在不迈步机器人最多能靠踝关节力矩把自己拉回到哪个位置。LIPM 下捕获点公式为xi x x_dot * sqrt(z_c / g)xi就是捕获点。如果xi落在当前支撑多边形内踝关节策略够用如果落在外面必须迈步而且落脚点要超过xi才能停住。工程上把xi和支撑多边形边界的距离作为步态触发条件比单纯看 ZMP 余量更提前能避免「已经来不及迈步」的尴尬。import numpy as np def capture_point(x, x_dot, z_c, g9.81): return x x_dot * np.sqrt(z_c / g) def should_step(xi, polygon, margin0.03): 捕获点离支撑多边形边界小于 margin 时触发迈步 from scipy.spatial import ConvexHull hull ConvexHull(polygon) # 简化判断用凸包顶点到 xi 的最小距离 dists [np.linalg.norm(xi - v) for v in polygon[hull.vertices]] return min(dists) marginmargin设 3 厘米是经验值太小会频繁迈步太大则反应迟钝。注意捕获点公式假设质心高度恒定如果机器人下蹲或跳跃z_c变化时要重新算。5.2 外力估计与扰动前馈补偿纯反馈控制在遇到持续外力比如被人推着走时会有稳态误差因为 LQR 和 MPC 都假设扰动是脉冲式的。进阶做法是用动量观测器估计外力再把估计值前馈到 ZMP 目标里。class MomentumObserver: def __init__(self, mass, dt, cutoff20.0): self.mass mass self.dt dt self.alpha dt / (1.0 / (2 * np.pi * cutoff) dt) self.momentum np.zeros(3) self.f_ext np.zeros(3) def update(self, velocity, joint_torque_effect, gravity_effect): # 动量变化 关节力矩项 重力项 外力 p_dot joint_torque_effect gravity_effect self.momentum p_dot * self.dt residual self.mass * velocity - self.momentum self.f_ext self.alpha * residual (1 - self.alpha) * self.f_ext return self.f_extcutoff是低通滤波截止频率设 20Hz 能滤掉关节噪声但保留推力信号。估计出的f_ext除以mass得到等效加速度加到 ZMP 目标上做前馈。这个技巧在协作场景里特别有用机器人被推时不会硬顶而是顺着力的方向调整步态。5.3 平衡算法的验证清单最后给一份可执行的验证清单按顺序跑完基本能确认平衡算法是否可靠测试项通过标准工具静止站立 30 秒ZMP 余量始终大于 2cm仿真/实机日志脉冲推力 50N/0.1s2 秒内恢复超调小于 5cm推力计持续推力 20N/2s稳态误差小于 3cm推力计斜坡 10 度站立不滑动ZMP 在支撑多边形内可调斜坡单足抬起 0.5 秒不摔倒落地后 1 秒恢复步态触发连续行走 10 米无步态发散ZMP 余量周期性变化运动捕捉跑完这张表平衡算法从「能站」到「推不倒」的路径基本就闭环了。剩下的就是根据具体机型调Q、R、z_c和捕获点margin这些参数没有万能值只能按实测现象迭代。本文还有配套的精品资源点击获取