企业官网建设流程全解析

简介：这份资源是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[k+1] = 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), method='SLSQP', bounds=[(-0.3, 0.3)] * N) return res.x[0]

Sx和Su是预测矩阵，把未来 N 步状态表示成当前状态和控制序列的线性组合。bounds限制 ZMP 不能超出物理可达范围，这个约束比 LQR 的软权重更硬。MPC 的坑在于求解时间：SLSQP在 N=15 时单次约 1 到 3ms，如果控制周期是 1ms 就跑不动，需要换 OSQP 或手写 QP 求解器。另外p_ref名义轨迹如果和实际步态差太多，MPC 会一直顶到边界，表现为机器人持续前倾。

3.3 两种控制器的选型对比

选型建议：先上 LQR 把基本平衡跑通，确认状态估计和 ZMP 计算没问题，再换 MPC 处理复杂场景。直接上 MPC 容易在状态估计不准时把问题归咎于优化器，浪费大量调试时间。

4. 仿真到实机的平衡算法调试与排错

4.1 在 MuJoCo 里搭一个最小平衡测试场景

仿真阶段建议用 MuJoCo，它的接触求解稳定，适合验证 ZMP 和质心轨迹。最小场景只需要一个浮动基座加四条腿，地面设摩擦系数 0.8 到 1.0。

<mujoco> <option timestep="0.001" gravity="0 0 -9.81"/> <worldbody> <geom name="floor" type="plane" size="5 5 0.1" friction="1.0 0.005 0.0001"/> <body name="trunk" pos="0 0 0.4"> <freejoint/> <geom type="box" size="0.2 0.15 0.05" mass="8"/> <!-- 四条腿的关节和连杆省略，按实际结构补 --> </body> </worldbody> </mujoco>

friction第一个值是滑动摩擦系数，设太低机器人会打滑，ZMP 算出来全是边界值。timestep设 0.001 是为了让接触求解收敛，控制周期可以在代码里降频到 0.01。仿真里先测三件事：静止站立时 ZMP 是否在支撑多边形中心、给质心一个初始速度后能否在 2 秒内收敛、侧向推力 20N 持续 0.1 秒后是否恢复。

4.2 实机调试的 5 个必查项

仿真过了不代表实机能跑，足式机器人从仿真到实机的差距主要在状态估计和执行延迟。按下面顺序排查：

1. 质心位置标定：用悬挂法或称重法测实际质心，和 URDF 里的值对比，偏差超过 2 厘米就要改模型。
2. IMU 姿态延迟：用示波器测 IMU 数据到控制器的延迟，超过 5ms 要换滤波或降截止频率。
3. 关节零位偏差：每个关节给零力矩，看实际角度和编码器读数差，超过 0.5 度要重新标定。
4. 足端接触检测：用足底开关或关节电流判断着地，接触检测延迟直接决定 ZMP 计算是否及时。
5. 控制周期抖动：用 GPIO 翻转测实际控制周期，抖动超过 20% 会让 LQR 增益失配。

提示：实机第一次跑平衡时把Q里位置权重降到仿真值的 1/5，R放大 5 倍，先让机器人「软」一点，确认不摔再逐步加硬。

4.3 常见振荡与发散的原因对照

这张表里的现象在调试现场基本能覆盖八成问题。关键是每次只改一个参数，改完记录现象，否则多个参数耦合时根本分不清是谁的锅。

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, g=9.81): return x + x_dot * np.sqrt(z_c / g) def should_step(xi, polygon, margin=0.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) < margin

margin设 3 厘米是经验值，太小会频繁迈步，太大则反应迟钝。注意捕获点公式假设质心高度恒定，如果机器人下蹲或跳跃，z_c变化时要重新算。

5.2 外力估计与扰动前馈补偿

纯反馈控制在遇到持续外力（比如被人推着走）时会有稳态误差，因为 LQR 和 MPC 都假设扰动是脉冲式的。进阶做法是用动量观测器估计外力，再把估计值前馈到 ZMP 目标里。

class MomentumObserver: def __init__(self, mass, dt, cutoff=20.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_ext

cutoff是低通滤波截止频率，设 20Hz 能滤掉关节噪声但保留推力信号。估计出的f_ext除以mass得到等效加速度，加到 ZMP 目标上做前馈。这个技巧在协作场景里特别有用，机器人被推时不会硬顶，而是顺着力的方向调整步态。

5.3 平衡算法的验证清单

最后给一份可执行的验证清单，按顺序跑完基本能确认平衡算法是否可靠：

跑完这张表，平衡算法从「能站」到「推不倒」的路径基本就闭环了。剩下的就是根据具体机型调Q、R、z_c和捕获点margin，这些参数没有万能值，只能按实测现象迭代。

本文还有配套的精品资源，点击获取