简介《足式机器人平衡算法》是机器人学家马克·雷伯特的经典著作以PDF文档形式提供面向机器人学研究者、算法工程师和高年级学生系统讲述足式机器人动态平衡与运动控制的核心问题。资源包内只有1个PDF文件整体约7.13MB便于下载后离线研读。目前已有394人学习浏览受到机器人领域读者关注。书中系统梳理了主动平衡、运动控制、动力学建模、传感器融合及反馈控制等关键技术详细介绍了弹跳杆机器人、三维空间动态运动、多腿协调等经典案例并融合早期步行装置探索和计算机仿真阐释如何理解动物的奔跑机制并迁移到机器控制上。全书从基础原理到系统实现层次分明动态腿部系统的分解控制思路至今仍有重要参考价值适合作为足式机器人方向的研究入门与进阶参考。1. 足式机器人平衡算法从“看模型”到“能站稳”的落地路径在跑步机上被人用力推了一把四足机器人踉跄两步又稳稳站住——这类视频几乎成了动态平衡算法的招牌镜头。这套能力并非单纯靠电机扭矩硬扛而是源自一种思路把机器人看成倒立摆先算清质心该往哪走、脚该踩在哪里再让关节去执行这个轨迹。经典专著《Legged Robots that Balance》把这个思路系统化了足式平衡的本质是动态控制不是静态顶住。这篇文章按这条技术路线展开先把 LIPM 模型、角动量与 ZMP 判据讲透再落到状态估计、步态规划和关节力矩分配的完整实现最后给出参数设置和几个高概率踩坑现场的排查办法。它适合想在仿真或实体机上做出动态步态的机器人开发者参考。2. 动态平衡三大件LIPM 模型、角动量控制与 ZMP 判据2.1 关键推导线性倒立摆为什么是平衡算法的地基绝大多数足式平衡方案都从线性倒立摆LIPM出发。把机器人等效成一根立在支撑脚上的杆质心高度恒定、脚底不打滑、脚踝只输出滚转和俯仰力矩就得到了最简单的平衡模型。横向和纵向可以解耦每个方向只剩一个方程质心运动由重力加速度和质心高度决定omega sqrt(g / zc)这里 g 是重力加速度zc 是质心高度。omega 是这个倒立摆的自然频率它决定了系统响应的快慢。解这个方程会得到双曲函数形式的轨迹意味着质心天然是发散的也就是说不倒反而是需要主动控制的。下面这段代码是 LIPM 轨迹生成的最小实现输入起点位置、起点速度和目标点输出一条质心位置轨迹。import numpy as np def generate_lipm_trajectory( zc, x_start, vx_start, x_target, duration, dt0.002 ): # zc: 质心高度米 # x_start: 初始质心位置 # vx_start: 初始质心速度 # x_target: 期望质心终点 # duration: 轨迹时长秒 omega np.sqrt(9.81 / zc) t np.arange(0.0, duration dt, dt) # LIPM 解析解写成双曲函数形式 a vx_start / omega b x_start - x_target x x_target b * np.cosh(omega * t) a * np.sinh(omega * t) v omega * (b * np.sinh(omega * t) a * np.cosh(omega * t)) return t, x, v这段代码生成的是开环轨迹实际使用中要配合反馈控制否则轨迹里的双曲发散项会越来越大。需要注意的是duration 不能设太长一般取步态周期的一半以下让模型始终处在一个可修正的范围内。zc 一定要用机器人实际质心高度很多翻车案例只是因为把机架中心高度当成了质心高度。LIPM 的意义在于把复杂的多关节平衡问题降维成一个二阶系统所有后续的步态规划、预览控制和全身控制都建立在这个简化模型上。不要小看这个简化它抓住了平衡问题的核心矛盾质心必须靠脚底反作用力来加速而脚的落点决定了力能从哪里产生。2.2 角动量与转向ZMP 没提的半个世界LIPM 只描述了质心的平动没有描述身体的转动。实际机器人在转弯、跳起和落地时身体需要获得或消散角动量这部分机制来自飞轮模型Flywheel Inverted Pendulum Model。把躯干想象成一个飞轮通过髋关节和踝关节施加力矩就能在脚底不打滑的条件下改变机体的角速度。一个典型的场景是单腿支撑下的原地转弯。机器人要改变朝向需要给躯干一个角速度但这个角速度必须借助摆腿的反作用来平衡否则整个身体会因为角动量守恒而歪向相反方向。很多入门方案在做转弯时直接给轨迹叠加角度偏移结果一转弯就摔倒原因就在这里。在步态规划中加上角动量约束意味着每一时刻都要记录躯干的角动量目标、角动量变化率上限以及支撑脚能提供的力矩极限。落地阶段的摆腿尤其依赖这个机制腾空阶段身体角动量守恒落地前必须通过摆腿或摆臂提前分配角动量才能让落地后的躯干姿态不产生过大的角速度扰动。2.3 ZMP 判据必要条件而非充分条件ZMP零力矩点是足式平衡里引用最多的稳定性判据它的物理含义是脚底压力分布的中心。只要这个点在支撑多边形内部理论上脚底不会出现翻转力矩机器人就是动态稳定的。LIPM 轨迹是否合理最终也要通过检查 ZMP 是否落在支撑多边形内来验证。实际工程里ZMP 的计算依赖地面反作用力数据或动力学模型。在四足机器人上每个支撑脚的足端力传感器提供当前脚的反力把所有支撑脚的力合成到地面就能求出总力作用点的位置。简单做法是只查 x、y 两个方向def compute_zmp(foot_forces, foot_positions): # foot_forces: N x 3 数组每行是 [fx, fy, fz] # foot_positions: N x 2 数组每行是 [x, y] f_total np.sum(foot_forces, axis0) if abs(f_total[2]) 1e-6: return np.array([0.0, 0.0]) # 总力矩对 x、y 方向的作用点 zmp_x -np.sum(foot_forces[:, 2] * foot_positions[:, 0]) / f_total[2] zmp_y -np.sum(foot_forces[:, 2] * foot_positions[:, 1]) / f_total[2] return np.array([zmp_x, zmp_y])这段代码把 ZMP 简化为竖直力在水平面上的加权中心逻辑清晰适合在线实时计算。但它忽略了水平力分量的作用在高速加减速或强扰动时会失真。还有一个认知很关键ZMP 是必要条件不是充分条件。ZMP 长期保持在支撑多边形内只能说明没有绕支撑多边形边界翻转的趋势并不能保证角动量不饱和、脚底不滑移。跳跃腾空阶段根本没有地面反力ZMP 直接失效这时候必须回到角动量守恒来规划姿态。因此成熟方案通常把 ZMP 与角动量约束、摩擦锥约束一起放进优化框架而不是单独依赖。3. 把平衡算法搬上机器人状态估计、步态规划与关节力矩计算3.1 状态估计IMU 和足端信息怎样融合出可靠质心状态控制器要工作首先要知道机体现在在哪。平衡控制需要质心位置、质心速度、躯干姿态和角速度这几个量。IMU 直接给角速度和加速度关节编码器配合正运动学能给每条腿末端的位置足端力传感器则切除了一条重要信息脚到底有没有压实地面。很多初学者直接把加速度计读数积分成速度再积分成位置结果几十秒内漂移就能让机器人飞走。原因是加速度计分不清重力加速度和机体运动加速度任何倾斜误差都会被积分方法放大。常见做法是 IMU 加速度与陀螺仪做互补滤波用加速度计的静态姿态修正陀螺仪的积分漂移。下面这段代码是一个可用的互补滤波姿态估计器输入原始 IMU 数据输出滚转角和俯仰角。import math class AttitudeEstimator: def __init__(self, alpha0.98): # alpha 越接近 1 越信任陀螺仪积分 self.alpha alpha self.roll 0.0 self.pitch 0.0 self.init False def update(self, accel, gyro, dt): # accel: [ax, ay, az]单位 m/s^2 # gyro: [gx, gy, gz]单位 rad/s roll_acc math.atan2(accel[1], accel[2]) pitch_acc math.atan2( -accel[0], math.sqrt(accel[1] ** 2 accel[2] ** 2) ) if not self.init: self.roll, self.pitch roll_acc, pitch_acc self.init True return self.roll, self.pitch # 陀螺仪积分加速度计修正 self.roll self.alpha * (self.roll gyro[0] * dt) \ (1 - self.alpha) * roll_acc self.pitch self.alpha * (self.pitch gyro[1] * dt) \ (1 - self.alpha) * pitch_acc return self.roll, self.pitch这个估计器在小角度、短时运动内表现良好但 alpha 是一个固定值意味着无论机器人处于什么加速度状态对加速度计的信任程度都一样。足式机器人在起步、刹车和跳跃阶段加速度很大此时加速度计的输出混入了大量运动加速度不能当作重力方向来修正姿态。更稳的做法是在动态阶段降低 alpha在静态或匀速阶段恢复 alpha甚至完全用足端力传感器的反力方向来判断真实重力方向。质心位置估计是另一件麻烦事。常见方案是用 IMU 姿态和腿部正运动学推算支撑脚相对机身的位置再结合支撑脚地面位置认为不变这一约束反推质心在惯性系下的坐标。腿式机器人的质心通常不在机架中心安装电池、负载之后必须重新标一次质心位置否则后面所有 ZMP 计算都会带系统性偏差。3.2 步态规划生成质心轨迹与摆动腿轨迹有了状态估计下一步是规划。步态规划的核心是回答两个问题质心下一步要怎么走摆动腿下一步要踩到哪。LIPM 轨迹生成负责第一个问题五次多项式摆腿负责第二个问题。摆腿轨迹必须满足起点和终点的位置、速度、加速度都为平滑值否则关节会瞬间产生冲击电流。完整一点的步态规划会同时输出质心轨迹和摆动腿轨迹并保证两者在时序上不冲突支撑相期间质心快速移动摆动相期间摆动腿抬起并快速落到目标点。下面代码把这两个模块放在同一个类里。class GaitPlanner: def __init__(self, zc, gait_cycle0.6, swing_height0.06): self.zc zc self.gait_cycle gait_cycle self.swing_height swing_height self.omega np.sqrt(9.81 / zc) def lipm_state(self, x0, vx0, x_target, t): # 给定目标点用LIPM解析解计算质心状态 a vx0 / self.omega b x0 - x_target x x_target b * np.cosh(self.omega * t) \ a * np.sinh(self.omega * t) v self.omega * ( b * np.sinh(self.omega * t) a * np.cosh(self.omega * t) ) return x, v def swing_foot(self, p_start, p_end, swing_time, t): # 五次多项式摆腿起脚落地速度加速度均为零 s min(t / swing_time, 1.0) u 10 * s ** 3 - 15 * s ** 4 6 * s ** 5 pos_xy p_start u * (p_end - p_start) # 高度方向用抛物线中点最高 pos_z 4 * self.swing_height * s * (1 - s) return np.array([pos_xy[0], pos_xy[1], pos_z])swing_time 一般取 0.2 到 0.4 秒需要与 gait_cycle 匹配。摆动高度 0.04 到 0.08 米是四足机器人的常见经验区间太低容易绊倒太高会显著增加关节峰值力矩。步态周期的选择和机器人腿长、执行器带宽强相关腿越长越适合长周期电机响应越慢越不能用太短的周期。跑这些参数时把轨迹打印成曲线会比盯机器人本体容易发现问题。3.3 关节力矩计算把期望加速度变成脚底力和电机电流规划层输出的是质心加速度和落脚点执行层需要把它变成每个关节的力矩。中间必经的一步是计算地面反作用力也就是把期望的机身合力、合力矩分配到每个支撑脚上。这一步需要满足脚底不滑动、不脱离这两个基本物理约束。下面这个函数是四足机器人起步阶段最常见的力分配写法用距离加权重的方式把期望力分配到支撑脚上。def distribute_ground_force(f_total, zmp, foot_positions): # f_total: 期望地面总反作用力 [fx, fy, fz] # zmp: 期望零力矩点 (x, y) # foot_positions: 支撑脚位置列表 N x 2 n len(foot_positions) foot_positions np.asarray(foot_positions, dtypefloat) # 离ZMP越近的脚承担更大的水平力 dist np.linalg.norm(foot_positions - np.array(zmp), axis1) 1e-6 w (1.0 / dist) / np.sum(1.0 / dist) forces np.zeros((n, 3)) forces[:, 0] w * f_total[0] forces[:, 1] w * f_total[1] forces[:, 2] f_total[2] / n return forces竖直力等分是第一个近似它无法精确控制 ZMP 在 x 方向上的位置因为真实 ZMP 是由各脚竖直力的空间分布决定的。如果想精确控制 ZMP必须把竖直力分配也纳入优化变量。水平力按距离加权保证了脚底合力方向大致指向 ZMP在扰动不大、步态稳定时够用遇到复杂地形或强冲击时会暴露问题。完整方案一般用二次规划QP做全身控制目标函数是让实际脚底力尽量接近期望值约束条件包括竖直力为正、水平力小于摩擦系数乘竖直力、脚底力矩不能超出地面接触范围。关节力矩的最终求解还要靠雅可比矩阵把脚底力映射到支撑腿各关节再叠加机身姿态的 PD 控制量。初学者不需要一上来就上 QP先把力分配跑通再逐步加上摩擦锥约束和力矩限制路径更顺。4. 平衡控制的关键参数与调参顺序先调谁、后调谁、调多少4.1 核心参数一览与物理解释平衡控制的参数不多但每一个都直接决定系统行为。调参没有银弹但有一个合理顺序。先看一组基础参数这些参数之间相互制约单独调大或调小都会引发连锁反应。参数经验范围对系统的影响质心高度 zc0.5 ~ 0.75 倍腿长决定 LIPM 自然频率影响轨迹发散速度步态周期0.4 ~ 1.0 s越短越激进对执行器带宽要求越高摆动相占空比0.6 ~ 0.8决定单腿支撑期的比例影响稳定性摆腿高度0.04 ~ 0.08 m太低绊脚太高浪费力矩关节位置 P 增益20 ~ 80越高越硬过高引起振荡关节阻尼 D 增益0.3 ~ 1.0抑制振荡过高会让响应迟钝摩擦系数 mu0.4 ~ 0.8影响力分配允许的最大水平力控制频率200 ~ 1000 Hz低于需求会出现极限环式抖动表中的 P、D 增益是关节位置控制里的常见量级具体数值差别很大先从范围下限试起比较安全。控制频率方面LIPM 的自然频率通常在 2 到 4 赫兹左右但关节控制回路需要比它高两个数量级否则相位延迟会把稳定的模型控制变成不稳定的反馈控制。4.2 调参顺序先静态站立再原地踏步最后才动态我的习惯是从静态站立开始机器人四条腿落地所有关节锁成直线腿。目标是机身水平、零速度、耗电最小。这一步如果都稳不住后面动态步态完全没有意义。静态站立要调的是姿态估计的收敛速度和关节 PD 增益重点看电机是否发烫、机身是否高频抖动。原地踏步是第二个阶段四条腿轮流抬起放下质心保持不动。这一阶段主要检验摆腿轨迹的平滑性和落地时机观察每个支撑切换时刻机身是否有冲击。如果每次换腿都“咯噔”一下大多是摆动相末点的速度和加速度没归零或者落地时刻比规划早了一个控制周期。慢速行走才进入真正的平衡控制。先走很小的步幅让 ZMP 始终在支撑多边形中心附近再逐步加步长和速度。速度提不上去时优先检查是否踩到了角动量饱和的边界而不是盲目增大关节增益。调参是倒着找源头的过程现象在末端病根往往在中段。4.3 仿真中的三条必测用例平地行走、推扰、斜坡实体机之前我建议在仿真里先过三条用例。第一条是平地长距离直行速度从 0.2 m/s 逐步升到 0.8 m/s跑至少 100 步统计每一步的 ZMP 是否越界、每条腿的峰值力矩是否超限。第二条是随机方向推扰测试在机身不同位置施加持续 0.2 秒的横向力观察恢复时间和最大偏移量。第三条是带坡度行走范围从 5 度到 15 度。这条用例最容易被跳过但最值得做因为斜坡会同时考验姿态估计、质心位置估计和力分配三条链路。如果在仿真里斜坡上站不住先不要怪模型回到状态估计里查质心高度是不是没跟随地形变化支撑脚是不是被错误地当成了同一水平面。5. 平衡落地的五个翻车场景现象、原因与排查办法5.1 控制频率不足导致的持续振荡现象机器人明明参数都调好了但站姿状态下机身高频抖动电流声音发闷电机发热明显频率大约在 5 到 15 赫兹之间。原因关节控制回路频率太低反馈带来的相位延迟让原本用来增稳的阻尼项变成了激励项。LIPM 的自然频率不高但关节 PD 的调节频率必须远高于模型频率一般不低于 400 赫兹否则系统进入极限环。解决先把控制循环频率提上去能到 1000 赫兹最好。计算量大的优化求解放到单独线程关节控制主循环只保留简单的位置和力矩换算保证周期稳定。频率提上来之后原来的增益值通常会显得不够可以按比例上调。5.2 转弯时突然摔倒ZMP 还在支撑多边形内现象直行很稳一旦进入转弯步态机身朝弯道内侧翻滚看起来像被甩出去但回看日志发现 ZMP 全程都在支撑多边形内部。原因ZMP 只能判断“脚底有没有翻转趋势”不能判断“身体转动动量是否失控”。转弯指令没有经过角动量规划身体为了跟上朝向变化产生了过大的角速度支撑腿无法提供足够的反向力矩。解决在规划层加入角动量轨迹转弯前先安排一个减速阶段让躯干角加速度的峰值降低。落地规划也要同步调整内侧腿的落脚点要向目标转向方向预移而不是等身体歪了再去追。能处理这个问题的框架在控制频率方面一般也有更高的容错度。5.3 脚底打滑导致的力分配失效现象机器人在瓷砖、浅沙或微湿地面行走时某条腿明显向外滑姿态歪斜后越走越差偶尔伴随尖锐的打滑噪声。原因力分配没有约束摩擦锥水平力与竖直力的比值超过了实际摩擦系数脚底只能滑动。ZMP 计算在这种状态下得到的是一个虚假的“稳定”结果因为地面反力已经被摩擦力主导。解决在力分配模块里加入摩擦锥约束def clamp_friction(f, mu): # f: 期望脚底力 [fx, fy, fz] f np.asarray(f, dtypefloat) fxy np.hypot(f[0], f[1]) fz max(f[2], 1e-6) if fxy mu * fz: scale mu * fz / fxy f[0] * scale f[1] * scale return f这个函数把水平力投影到摩擦锥内部作为一种硬裁剪。正式方案里一般在 QP 里作为约束项处理线上裁剪只适合作为兜底。5.4 摆动腿落地时机滞后现象摆动腿落地时没有落在规划位置而是落后几厘米机身重心被带偏步态周期越走越乱。原因规划层的摆动轨迹没有考虑执行器的延迟。电机的力矩建立、减速器的摩擦、关节的柔性让腿的实际响应总比指令慢导致落地位置始终沿行进方向滞后。解决给规划加超前量把摆腿的期望终点向前移动一个控制周期乘以当前速度的距离。如果步态周期是 0.6 秒控制周期是 0.002 秒速度是 0.5 m/s超前量大约是 0.001 米量看起来不大但在高频行走时影响很明显。更彻底的做法是在控制链路中显式加入延迟补偿模型但大多数场景用超前落点就能缓解。5.5 斜坡上姿态缓慢漂移现象在斜坡上静止站立时机器人姿态逐渐偏离水平重新上电后恢复几分钟后又偏回去。原因质心位置估计没有随地形更新所有支撑脚被建模成同一高度斜坡上后方腿的实际高度比模型高控制器认为机身已经前倾于是持续反向修正导致真实姿态反而漂移。解决用腿部正运动学实时更新每只支撑脚的地面高度质心位置估计也以此为参考。同时检查姿态估计器在持续加速场景下的表现必要时加入足端力方向作为重力方向的观测因为静态站立时足端力方向可以准确反映重力方向。这条排查完再看是否需要重新标定关节中位很多斜坡漂移最终都追到了编码器零位偏差上。6. 验证一个“推不倒”的机器人抗扰测试与约束检查脚本6.1 一个随时可跑的约束检查脚本平衡算法调试过程中最需要的是快速判断当前控制状态是否越界。把 ZMP、地面反力、摩擦锥三个关键量集中到一个脚本里每次测试后直接输出是否通过能省掉一半的玄学调试时间。import math def point_in_polygon(point, polygon): # 射线法判断点是否在多边形内 x, y point n len(polygon) inside False j n - 1 for i in range(n): xi, yi polygon[i] xj, yj polygon[j] if (yi y) ! (yj y): cross_x (xj - xi) * (y - yi) / (yj - yi 1e-6) xi if x cross_x: inside not inside j i return inside def check_balance_constraints(zmp, support_polygon, foot_forces, mu0.6): # 检查ZMP是否在支撑多边形内 if not point_in_polygon(zmp, support_polygon): return False # 检查每只脚是否满足摩擦锥与压力下限 for f in foot_forces: if f[2] 1.0: return False if math.hypot(f[0], f[1]) mu * f[2]: return False return Truesupport_polygon 由当前支撑脚的坐标按顺序构成zmp 由足端力实时计算。这个脚本不负责平衡只负责把违规情况尽早暴露出来。每条腿的竖直力下限 1.0 牛只是示例值实际按机器人重量和支撑腿数换算。6.2 抗扰测试的标准化做法验证一个平衡控制器是否合格不是看它在视频里被推一把能稳住而是看它能否在受控条件下量化表现。标准做法是记录三次重要数据扰动施加位置、最大质心偏移量、恢复稳态的时间。同一个扰动力重复 10 次以上取中位数而不是平均值因为偶发的大偏移会拉偏平均值。我习惯再加一条扰动结束后 3 秒内ZMP 与期望值的偏差必须连续保持在支撑多边形半径的 10% 以内才算恢复完成。这一条比网上的“只要能站起来就行”严格得多但它是后续走向复杂地形的基线。这套方案是我从“能站住”到“能走稳”过程中用得最扎实的一批检查手段。回头看最容易翻车的地方不是模型选错了而是状态估计和力分配没有做边界检查。先把这四层约束跑在日志里再去调步态参数效率会高很多。希望帮到你。本文还有配套的精品资源点击获取