四足开发实战（八）：步态篇——状态估计与 trot 行走
模型稳定站立后，下一步需要在 1 kHz 控制环中回答三个问题：机身位置、足端位置与下一步落点。有实机经验者普遍认同一条规律：一台无法直线行走的机器人，多数情况源于估计问题而非控制问题。即使控制律设计精巧，姿态漂移 0.5°、速度误差 0.2 m/s，也会使十米直线偏移半米；此时若仅针对步态参数反复调试数日，现象看似“控制失效”，实际原因却在估计环节。因此本文的顺序与之相反：先估计、再接触、再步态、最后才是控制。上一篇已将模型与运动学写入文件（四足开发实战（七）：建模篇——运动学与 URDF 从零写起），本文将该套 FK/IK 与 1 kHz 控制环连接起来。信息流为单向链路：IMU 与编码器输入估计，估计与接触检测输入步态，步态与阻抗输入 1 kHz 循环；每一环节的输入都由上一环节的验收标准支撑，出现问题时可依据现象定位到对应环节。
姿态估计：先确定机身姿态
姿态估计采用 Mahony 互补滤波，而非 ESKF。理由较为实际：Mahony 约一百五十行，ESKF 约六百行，而 v1 阶段只缺少姿态，速度交由腿部里程计计算。升级 ESKF 设定了三个触发条件：需要长时间双脚离地的速度估计（跳跃、摔倒过程）、加速度计零偏大到静止积分十秒漂移半米，或需要接入视觉做 VIO。在此之前，用 Mahony 将姿态稳定即可，其在 500 Hz 下占用的 CPU 不到 3%【TODO(实测)】。三个触发条件中最易被忽略的是第二条——加速度计零偏，Mahony 只修正姿态、不处理零偏。测量方法也较简单：上电标定完成后，让纯 IMU 速度积分十秒，漂移半米即说明零偏已不可忽略；若继续使用，直线行走会愈发漂移。估计任务本身无需追求 1 kHz：步频仅 2.5 Hz，姿态动态的主要激励来自支撑切换，500 Hz 采样已富余一个数量级，节省的控制节拍可留给步态与通信。两条路线的对比如下：
| Mahony 互补滤波 | ESKF（误差状态卡尔曼滤波） | |
|---|---|---|
| 状态 | 四元数 4 维 + 陀螺零偏 3 维 | 位置/速度/姿态/零偏 15 维 |
| 代码量 | ~150 行 | ~600 行 |
| 算力 | 500 Hz 占 < 3% CPU【TODO(实测)】 | 500 Hz 占 10~15%【TODO(实测)】 |
| 强项 | 简单、抗振动、无矩阵运算 | 能估加速度计零偏、能给速度 |
| 弱项 | 不能给平移速度 | 调参多、线性化点要维护 |
v1 阶段的目标是“先运行起来”，右侧所列强项暂用不上，左侧弱项恰由腿部里程计补齐——路线选择实为需求与成本的匹配，而非技术优劣之分。
Mahony 的核心仅需三行。四元数约定为机体系到世界系，q = [w, x, y, z]；先去除陀螺零偏 ω = ω_gyro − b_g，再用加速度计测得的重力方向与估计重力方向叉乘得到误差 e = a_hat × v_est，将误差的 P 项与 I 项加回角速度，最后进行四元数积分：
$$
\dot q = \frac{1}{2}, q \otimes \begin{bmatrix} 0 \ \omega_c \end{bmatrix}, \qquad \omega_c = \omega - b_g + K_p, e + K_i \int e, dt
$$
两个参数按以下量级即可运行：Kp = 1.0 s⁻¹、Ki = 0.02 s⁻²，更新率 500 Hz，加速度计前置一个 10 Hz 的一阶低通。必须加入一项门控——加速度计仅在模长处于 0.5g 到 1.5g 之间时参与修正，因为走路落地冲击常态超过 2g，若不门控，修正项会将冲击噪声引入、反复拉动姿态。叉乘顺序写反的症状较为典型：姿态越修正越偏离，且仅在晃动时明显、静止时不易察觉。叉乘的方向含义值得记忆一次：a_hat 与 v_est 归一化后，叉乘的模近似等于二者夹角的正弦，方向恰为使估计重力方向转回实测方向的旋转轴——因此误差可直接作为角速度使用；方向正确时它将姿态拉回，写反时它将姿态推离，这正是“越修正越偏离”的根因。相关参数汇总为一张表，照此即可先将姿态运行起来：
| 参数 | 示例值 | 说明 |
|---|---|---|
| Kp | 1.0 s⁻¹ | 越大跟加速度计越快，振动也越大 |
| Ki | 0.02 s⁻² | 只修慢漂，别超过 Kp/20 |
| 加速度低通 | 10 Hz 一阶 | 截止频率低于步频 2.5 Hz 的三倍即可 |
| 陀螺量程 | ±2000 dps | 摔倒瞬间可能到 1000 dps |
| 加速度量程 | ±16 g | 落地冲击 8~10 g 常见 |
| 更新率 | 500 Hz | 每 2 ms 一次，和估计任务同拍 |
| 门控 | 0.5~1.5 g | 超范围只积分陀螺 |
量程取较大值的原因：落地冲击 8~10 g、摔倒瞬间角速度达 1000 dps 均属常见，量程宜大不宜小——饱和后的读数并非噪声而是假信号，任何滤波都无法恢复。整个滤波器连同四元数乘法不足三十行，核心一次更新如下（quat_mul 为标准四元数乘法）：
void mahony_update(mahony_t *m, const float gyr[3], const float acc[3], float dt) { |
该段有两个要点：门控写在 if 中，误差积分仅在校正分支累加，冲击出现时既不修正也不积分；每步归一化不可省略，四元数若不归一化，运行十分钟后旋转矩阵即开始发散。
上电初始化同样不可省略，可固定为一套流程。整机水平静置 3 秒，采集 1500 组陀螺数据取均值作为零偏 b_g——均值超过 0.05 rad/s 即判定为“未放稳”，应直接重来，不应把振动当作零偏写入。由加速度均值计算初始 roll/pitch：
$$
\mathrm{roll} = \operatorname{atan2}(a_y, a_z), \qquad \mathrm{pitch} = \operatorname{atan2}!\left(-a_x,\ \sqrt{a_y^2 + a_z^2}\right)
$$
由于缺少磁力计，yaw 只能置零作为相对量，初始四元数由这三个角合成。yaw 为相对量这一点需在操作中注意：断电重启后朝向归零，与上次朝向不一致——因此验收中横向偏差比绝对朝向更重要，朝向靠地面标线人工对齐。零偏按温度分三段写入 flash（20/40/60 °C 各一段【示例值】），上电按当前温度插值；连续工作温升超过 10 °C 时重做一次——这就是零偏分段标定：陀螺零偏随温度漂移，单点标定在热机后即不准确，分段插值才是可长期使用的方案。静止时的残差 |ω − b_g| 应小于 0.01 rad/s，若达不到，应先检查 IMU 供电纹波与减振，而不应立即修改滤波参数。再补充安装矩阵 R_bi 以修正 IMU 与机身的安装角度——从 CAD 读出旋转矩阵直接写入配置，水平台静止时加速度归一化后应等于 [0,0,+1]，残差超过 0.5° 时用六面法重标：让 IMU 依次六个面朝上静置读数，每个面的重力方向在机体系下均为已知坐标轴，六个读数一起最小二乘拟合 R_bi——装拆两次、重标两次，其重复性本身即反映安装应力。IMU 不在质心时，杆臂项 a_imu = a_com + α×r + ω×(ω×r) 在偏心小于 2 cm、角速度小于 5 rad/s 时可直接忽略。两个分量中 α×r 通常比 ω×(ω×r) 大一个量级：角加速度较大时，厘米级偏心即可产生 0.1g 的假加速度，直接污染门控判据。系统的姿态估计理论（ESKF、可观性等）在 姿态与状态估计：互补滤波、Mahony、Madgwick、EKF、UKF、MSCKF、粒子滤波 中展开，本文只保证工程版可落地。
得到姿态后，平移速度由腿部里程计计算。其假设较为朴素：支撑脚在世界系中保持不动。于是每条支撑腿给出一个约束 v_b + ω × p_i = −J_i·q̇_i，将两到四条支撑腿的方程叠加，对六维的 [v_b; ω] 做最小二乘：
$$
\begin{bmatrix} I_3 & -[p_i]_\times \end{bmatrix} \begin{bmatrix} v_b \ \omega \end{bmatrix} = -J_i,\dot q_i, \qquad i \in \mathcal{S}
$$
trot 的两脚支撑恰好为六方程六未知，但两只脚共线时矩阵会病态，需加入 1e-6 的正则项；四脚支撑则超定、抗噪性更好。此处有一个性价比较高的小技巧：将解出的 ω_z 回馈用于在线修正陀螺零偏，仅需一步 b_g += 0.01·(ω_z − ω_gyro_z)·dt，十米直线的横向偏差即可从米级降至分米级——该效果的来源最初被误判为步态参数调对，后经排查确认是这一步的贡献。其原理可概括为：直线行走时横向偏差本质是 yaw 漂移在距离上的积分，修正 ω_z 即等于修正走偏。与 IMU 的融合第一版用互补滤波即可：v_w += (R·a_b + g_w)·dt 积分，再按 Kv = 2~5 Hz【示例值】向腿部速度收敛，支撑期以腿部速度为准、摆动期以 IMU 为准。此处 Kv 的含义需明确：它是“腿部速度向 IMU 速度收敛的带宽”，不应做成硬开关——将 Kv 按支撑腿数线性缩放，腿多时偏向腿部速度、腿少时偏向 IMU，过渡自然且不冲突。此外支撑腿数从 2 变 4 的瞬间，最小二乘解会产生跳变，应加入 10~20 Hz 的低通抹平解算跳变——不应把解算跳变当作速度误差送入闭环。打滑会破坏支撑脚不动的假设，因此需监视残差：单腿残差超过 0.15 m/s 且持续 20 ms 即将其移出支撑集；四条腿同时超阈则判定整机打滑，冻结里程计 100 ms 退回纯 IMU；支撑脚在接触后上升超过 5 mm 则判定为“蹬空”，提前结束该腿的支撑。冻结 100 ms 的时长有其依据：过短会使单步噪声反复触发，过长会使速度环获得一段失效数据——100 ms 为四分之一个步态周期，足以让打滑过程结束，又不会使闭环长时间失效。
接触检测：判断落地状态并不简单
接触检测看似简单，实现时却是最易被低估的环节之一。三种手段各有盲区，先列出一张表：
| 手段 | 判据 | 延迟 | 盲区 |
|---|---|---|---|
| 电流阈值 | i_q 幅值 > I_th，持续 3 ms |
3~5 ms | 轻触、低温、电池电压低 |
| 力传感器 | F_z > 5 N，持续 2 ms |
2~3 ms | 没装就没有 |
| 编码器突变 | 足端高度下降速率突变 / 关节速度反号 | 5~10 ms | 慢速落地不明显 |
因此工程上应至少将两种手段“与”起来使用，不应只依赖一种。v1 未装力传感器，故以电流为主判据、编码器的高度突变为辅判据，两者均确认才进入支撑，任一消失即提前退出。接触检测同时是估计与步态之间的裁判：里程计仅使用支撑腿，相位仅对触地的腿生效，误判一条腿会使速度与相位同时出错——因此它在整定表中排在估计之后、步态之前。
以电流为例，阈值并非随意设定，可固定为五步：
- 吊起整机，让目标腿悬空，记录 10 s 空载
i_q的均值I0与噪声σ； - 落地站立，记录该腿的支撑电流均值
I1； - 阈值取
I_th = I0 + 0.5·(I1 − I0)，同时要求大于I0 + 5σ——参考机器上I0≈0.3 A、I1≈2.5 A，得到约 1.4 A【示例值】； - 进行 200 次起落统计验收：漏检（应支撑而未支撑）小于 0.5%，误检（未支撑而判为支撑）小于 1%；
- 加入滞回与去抖：进入支撑用
I_th，退出用0.7·I_th，再去抖 3~5 ms。
第 5 步的方向不应颠倒：进入支撑应“实”（用全阈值），退出应“虚”（用七折阈值），否则会在接触边界上反复抖动。另有一个易耗费两周的细节：电池从满电到低电过程中，同样工况下电流整体变小，若阈值不随母线电压补偿，低电时即会漏检——应将 I_th 按母线电压归一化一次。力传感器版的阈值仅一条：F_z > 5 N 持续 2 ms，加入之后电流法的漏检基本消失——预算允许时应优先安装，比将电流阈值调到极致更划算。没有统计验收的接触检测等同于没有接触检测。整套逻辑实现为四状态的状态机：摆动相电流超阈值进入“触地”，去抖 3 ms 进入“支撑”，相位走到占空比且电流下降进入“离地”，足端实际离地 1 cm 才回到摆动相。采用四状态而非两状态的原因：触地与离地各增加一个暂态，去抖与确认均在这两个暂态中完成，正式进入“支撑”的信息才是干净可用的；将去抖放入暂态，比在状态转移边上到处添加延时更易维护。另有一个分工需明确：进入支撑依据接触（物理事实：实际触地），退出支撑依据相位（时间规划：应当抬起），两者各司其职，互不越权。
trot 的相位生成较为简单：全局相位按步频累加 φ += f·dt 再取小数部分（相位增量写成 f·dt 而非按周期计数，是为保证步频变化时相位导数天然连续——后文步频斜坡的方法依赖此点），四条腿各加一个固定偏移，小于占空比 β 的部分即支撑相。trot 的偏移为 0、0.5、0.5、0，β 取 0.5——对角两条腿始终同相，机身由两条对角腿交替支撑，故称为对角小跑；β=0.5 时任意时刻恰好两条腿触地，压至 0.5 以下会出现四脚同时离地的腾空段，已不属于 trot。步频的上下界各有机械层面的原因：低于 1.5 Hz 支撑期拉长，单腿电流难以承受、机身高度起伏增大；高于 3.5 Hz 减速器与电机绕组的发热同时上升。12 kg 这一体量从 2.5 Hz 起步最为稳定。参数按以下量级起步，汇总为一张表：
| 参数 | 示例值 | 约束 |
|---|---|---|
| 步频 f | 2.5 Hz | 1.5~3.5 Hz：太高电流大，太低站不稳 |
| 占空比 β | 0.5 | trot 取 0.5~0.6，低于 0.5 会四脚离地 |
| 步长 S | 0.20 m | 受工作空间限制，S ≤ 0.30 m |
| 抬腿高度 h | 0.08 m | 0.05~0.12 m，按越障需求 |
| 速度 | 1.0 m/s | v = S/(βT)，由步频和步长定死 |
速度由 v = S/(βT) 确定——步频与步长确定后，速度没有第二个自由度，因此后续速度闭环调整的是步长而非步频，这也是转向不可改变频率的原因。步长另有一个硬性约束，源于第 7 章的工作空间：腿长 0.44 m、站高 0.40 m 时，水平可达距离仅 √(0.44² − 0.40²) ≈ 0.18 m，步长需满足 S/2 < 0.15 m，故步长上限为 S ≤ 0.30 m。若要达到第 1 章设定的 1.5 m/s，S = 1.5 × 0.5 × 0.4 = 0.30 m，恰好触及上限——必须同时将站立高度降至 0.36 m（水平可达 √(0.44² − 0.36²) ≈ 0.25 m）或加长小腿，否则高速档 IK 必然解到腿伸直、膝盖达到限位。该上限并非固定值，而是随模型变化：站高降至 0.36 m 时可达 0.25 m，上限即放宽至 0.50 m，加长小腿同理——第 7 篇的模型一旦改动，此处必须重算，不应沿用旧值。摆动轨迹用五次多项式 10s³ − 15s⁴ + 6s⁵ 从离地点插值到落脚点：
$$
s = \frac{\varphi_i - \beta}{1 - \beta}, \qquad p(s) = p_0 + (p_1 - p_0)\left(10s^3 - 15s^4 + 6s^5\right)
$$
抬腿高度沿同一条曲线，z(s) = z_0 + h·(10s³ − 15s⁴ + 6s⁵)。起终点速度与加速度均为零，落地不产生冲击——s→1 时足端速度平滑归零，这正是“不冲击”的数学来源；改用梯形速度曲线更简单，但起终点加速度存在阶跃，落地瞬间会产生可闻的冲击声，五次多项式仅多出数行代码，不应省略。支撑相足端在机体系内以 −v·dt 匀速后移。落脚点按 Raibert 方法设置：p_foot = p_hip + v·T_st/2 + k·(v − v_cmd)，k 取 0.03~0.08 s【示例值】——支撑期身体会前移，足端需提前放置，速度有误差时再补充一项补偿。只调整步长而不调整落脚点的机器人，加速时会越来越前倾，这是闭环中最易被省略的一环。k 的量纲为时间：速度误差乘以时间得到位置补偿。k 越大，落脚点越激进，地面摩擦不足时激进会导致打滑，因此上限设在 0.08 s，先求稳定再求速度。
站立阻抗：Kp/Kd 由推导得出
站立从最朴素的关节阻抗开始：
$$
\tau = K_p,(q_d - q) + K_d,(\dot q_d - \dot q) + g(q), \qquad K_d \approx 2,\zeta \sqrt{K_p J}
$$
g(q) 为重力前馈，用 URDF 的质量质心计算；若无法计算，可先采集“站立不动时各关节电流”换算为力矩作为常数表，同样可用。常数表法的局限需明确：它仅在参考站姿附近有效，腿部下蹲或抬起后查表电流即失准——因此 v1 仅允许其用于站立兜底，不允许用于行走。参考站姿即第 7 篇反解得到的 q=(0, 0.5284, 1.0567)（髋 roll、髋 pitch、膝），提前写入 stand_pose.yaml，此后所有“站立”均以其为参考，不应每次临时调整；站姿中髋 roll 为 0 表示站立基准不要求髋外展，加入 ±0.1 rad 的偏置可微调四腿间距、使落地重心更稳——该偏置仅允许在站立参数中修改，不应在步态中改动。整定全程先设定 [12, 15, 20] N·m 的力矩软限（依次为髋 roll、髋 pitch、膝），先限位再讨论增益。站立验收的 2 cm 漂移的测量方法：地面画十字，站 5 分钟后观察足端印迹的移动——不应只看姿态，四条腿协同漂移在姿态中不可见。第 7 篇另有一句关键结论：站立过直意味着膝逼近奇异区，落地冲击会直接作用于关节限位，q3 = 1.0567 rad 保留的约 13% 屈曲量即膝缓冲冲击的力矩余量——站立控制全程应守住该参考角，不应使“站立过直”变为“站立失稳”。
Kp 的起始值可由推导得出：单腿静载约 30 N（12 kg 均分四腿），力臂 0.20 m，膝力矩约 6 N·m；无前馈时允许下垂 0.05 rad，则需 Kp ≈ 6/0.05 = 120 N·m/rad；加入前馈后 Kp 仅需抵抗扰动，取 40~80 即可【示例值】。Kd 按 2ζ√(Kp·J)，转动惯量取 0.02~0.05 kg·m²、阻尼比 0.7~1.0：以 Kp=60、J=0.03、ζ=0.9 代入，Kd ≈ 2×0.9×√(60×0.03) ≈ 2.4，取 1.5~3.5 即落入合理区间【示例值】。应先软后硬，不应一步到位。推临界振荡时一次仅调整一条腿的 Kp，其余腿保持前馈；记录振荡周期，回退一半再加 Kd，比从零开始试调更快。整定顺序应固定，不应一开始就全部加入：
- 先校准
g(q)：观察静止电流是否接近零，前馈不准时即使 Kp 再大也无法站平； - 再加 Kp：推到临界振荡，回退一半；
- 然后加 Kd：按上述公式取 1.5~3.5 抑制超调；
- 最后加 20 Hz 低通，将编码器噪声排除在微分之外。
第二版可改用虚拟腿：足端力 F = Kp_p(p_d − p) + Kd_p(ṗ_d − ṗ) + F_ff 再 τ = JᵀF，其优点是可直接控制“腿的刚度”，代价是需要处理雅可比奇异，第一版暂不使用。虚拟腿的 Kp_p/Kd_p 与关节 Kp/Kd 含义不同：前者为足端刚度（N/m），后者为关节刚度（N·m/rad），换算需经雅可比，第一版不应混用。
行走分三步，附一张不允许跳级的表
行走不应一步到位，应分三步进行，且一步都不可跳过：
- 第一步为开环 trot：
S=0.15 m、f=2.0 Hz、h=0.06 m，仅运行相位与摆动轨迹、不闭环，目标是十步不倒；首次试运行仍挂保险绳、速度限 0.3 m/s、急停随时可用，前十余次跌倒都发生在该保护范围内； - 第二步为速度闭环：
S = S0 + k_v·(v_cmd − v_meas)，k_v取 0.1~0.2，步长钳制在 0.10~0.28 m，同时将落脚点按上述 Raibert 公式一并闭环——速度闭环的一半效果来自落脚点； - 第三步转向采用差动步长——左右腿步长一侧加
k_ω·ω_cmd、一侧减，k_ω取 0.02~0.05，角速度限幅 ±1.0 rad/s——不可采用差动频率，频率变化会使相位导数跳变，机器人会突然跛行。
差动步长可实现的转弯速度可估算：S=0.20 m、k_ω=0.05、ω_cmd=1.0 rad/s 时，内外腿步长为 0.15/0.25 m，均未触及 0.28 的钳制值；若需更急，应先减速再转向，不应使外侧腿触及工作空间上限。
三步之外还有两个补充。姿态保持将 roll/pitch 的 PD 输出叠加到髋 roll、髋 pitch 的力矩偏置上，高度环调整站立高度、不修改相位。躯干角的 PD 不应过大：角刚度主要靠支撑腿的位置差自然提供，PD 仅补偿偏差，增益过大会与步态耦合，表现为每走一步机身晃动一次。1 kHz 循环的顺序应固定，牵一发动全身：IMU 采样 → 姿态与里程计（500 Hz 分摊）→ 步态相位 → 足端轨迹 + IK → 关节阻抗 → RS485 轮询下发，数据流如下：
flowchart LR |
虚线即前文所述 ω_z 在线修偏回路：里程计解出的角速度回流到姿态估计，将陀螺零偏拉回。另有一个实机特有的细节：RS485 为半双工轮询，一拍 1 ms 问询六个节点（16 字节 @ 4 Mbps 约 32 µs，加上换向与帧间隙，六个节点约 0.3 ms），控制律实际使用的是上一拍反馈，等效延迟 1~2 ms——调整 Kp 应按该延迟计算，不能按“理想 1 ms”强行取值，否则会出现高频抖腿、地面留下震痕；调增益受阻的排查中，有一半时间是在处理这一延迟带来的问题。
整个控制从零到可行走，可排成一张不允许跳级的表，最后一列为真实项目中的典型耗时，用于设定合理预期：
| 阶段 | 入口条件 | 操作 | 通过标准 | 典型耗时 |
|---|---|---|---|---|
| 1 悬空 | 单腿能力矩跟踪 | 吊架，正弦扫频 + 阶跃 | 无碰撞，力矩 < 30% 峰值 | 1 天 |
| 2 站立 | 悬空姿态稳 | 落地，加 g(q)，Kp 40~80 | 5 min 漂移 < 2 cm | 2 天 |
| 3 原地踏步 | 站立稳 | 只跑相位，抬腿 0.05 m | 100 步相位不跳 | 1 天 |
| 4 低速直行 | 踏步稳 | S=0.10 m，0.3 m/s | 10 m 不摔 | 3 天 |
| 5 转向 | 直行稳 | 差动步长 | 90° 超调 < 10% | 2 天 |
| 6 加速 | 转向稳 | 0.5→1.5 m/s 分档 | 5 min 无过热 | 3 天 |
流程画成图是一条单向路径：
flowchart LR |
图中箭头仅画出正向，任一阶段未达通过标准即退回上一阶段重做——顺序不可跳过，倒退允许。表中的耗时为连续工作日，六段合计 12 天，其中直行与加速占一半——主要时间应预留给“可走但不稳”的打磨，而非站立不动时修改代码。若站立未通过即开始练走，跌倒后将难以区分是估计、步态还是增益的问题——这是所有初学者中耗时最长的一条弯路。
MPC 在何种条件下才值得采用
最后是 MPC/WBC，其采用原则是“先判断是否应当引入”。判断不依据论文热度，而看三件事：跌倒发生的场景、团队能否维护求解器、剩余交付时间：
| 情况 | 判断 |
|---|---|
| 平地 0.8 m/s 能稳走，只是偶尔抖 | 不上，去调步态和阻抗 |
| 一上台阶、斜坡或负重变化就摔 | 值得上：摩擦锥、力上限这些约束 PID 和阻抗表达不了，SRBD-MPC 能把它们显式写进优化 |
| 需要显式限制摩擦、防打滑 | 值得上 |
| 团队没人愿意维护 QP 求解器 | 先别碰，把步态调稳比什么都强 |
| 只剩两周交付 | 先别碰 |
判断表中“偶尔抖”的抖动，应与增益抖腿的抖动区分：前者幅度为几毫米、频率为步频的整数倍，后者为几十赫兹的高频抖动——前者采用 MPC 无效，后者采用 MPC 同样无效。确定引入后，最小版本如下：单刚体模型（将四条腿均视为无质量执行器，仅保留躯干动力学——以精度换取 QP 在 1~2 ms 内解完，12 kg 量级足够），状态 12 维 x = [p(3), θ(3), v(3), ω(3)]，输入为四足接触力 12 维 u = [f_1; f_2; f_3; f_4]；预测 10 步、步长 0.03 s，即 100 Hz 重算、考察 0.3 s 的时域——略小于一个步态周期（0.4 s），保证至少覆盖到下一条腿落地。约束仅三条：f_z ≥ 0（足端不能向下拽地）、切向力在摩擦锥内（μ=0.5【示例值】，干地面上橡胶足实测 0.7~0.8，取 0.5 属保守取值，换取 QP 求解稳定）、f_z ≤ 200 N【示例值】。100 Hz 重算意味着每次仅下发 QP 解的第一拍，后续 9 拍在下一周期重新计算——滚动时域最易在移植中被遗漏的一点正是此处，将整条序列全部下发等于开环运行 0.3 s。代价即状态跟踪加力正则：
$$
\min_{f_0,\dots,f_{N-1}} ; \sum_{k=0}^{N-1} \left( | x_k - x_{ref} |_Q^2 + | f_k |R^2 \right) \quad \text{s.t.} \quad f_z \ge 0, \quad |f_x|, |f_y| \le \mu, f_z, \quad f_z \le f{max}
$$
离散化后即一个线性时不变系统 x_{k+1} = A x_k + B u_k，A、B 用十余行即可构造（质量 12 kg、惯量 diag([0.05, 0.15, 0.15])【示例值】，按 URDF 更新）：
def build_ab(DT=0.03, M=12.0): |
skew(r) 即 r× 的 3×3 反对称矩阵。求解器上位机使用 OSQP、主控 H723 上改用 qpOASES/HPIPM，单次 0.5~2 ms【TODO(实测)】。MPC 仅产出四个接触力，分配到 12 个关节第一版用 τ = Jᵀf 即可；需同时管理躯干姿态、摆动足轨迹、力矩上限时再引入分层 QP，任务优先级固定为安全 > 躯干姿态 > 质心 > 摆动足——该顺序并非口号，而是堆叠顺序：下一层的解空间是上一层的最优子空间，无解时向下降一层，安全约束在最上层始终满足。OSQP 依赖稀疏矩阵运算，H723 上没有现成的稀疏线性代数库，因此主控改用 qpOASES（稠密，该规模足够）或 HPIPM；移植验收标准较严格——同一组 A、B 与参考轨迹，上位机与主控必须解出相同的 f 序列，逐元素差小于 1e-6。完整的 MPC 推导见 四足机器人的几条控制路线：运动学、WBC、MPC、RL。
五个常见问题与最终验收
本章最常见五个问题，均值得提前了解其症状。相位跳变表现为某一拍某条腿突然加速或拖地、机身随之侧倾，原因是修改步频或占空比时相位导数突变，处理办法是步频用 0.2 s 的斜坡调整、换步态前先将速度降至 0.2 m/s。落脚点超出工作空间表现为腿完全伸直、电流飙升、膝盖达到限位，IK 解算后应检查关节限位，超限时把落脚点回收两厘米。增益抖腿表现为高频抖动声、地面有震痕，多半是 Kp 逼近 RS485 那 1~2 ms 的等效延迟而取值过强，应降低 30~50% 再加 Kd 抑制。接触误判表现为支撑腿被判为摆动腿、机身突然下沉一段，原因多半是电流阈值过于贴近空载、去抖时间过短、或电池电压下降后电流整体变小，处理是滞回 20~30%、去抖 3~5 ms、电流与编码器“与”起来、阈值随母线电压补偿。IMU 漂移导致越走越偏，表现为十米偏移 0.5~1 m 且总偏向同一侧，原因是零偏未标定或温漂、安装矩阵错误、振动使加速度计偏移，处理即前文所述三项：上电静置 3 秒标定、ωz 在线修偏、加减振垫并重标安装矩阵。这五个问题的症状中均隐含“估计”的影响，这也是本文从估计写起的原因。
最后为强化学习预留一个接口：策略 50 Hz 输出 12 维关节目标增量，底层 1 kHz PD 跟踪，观测 47 维（角速度、重力投影、指令、关节角、关节速、上一步动作、相位各按 3/3/3/12/12/12/2 排列），两端必须共用同一份定义，否则 sim2real 必然失效。切换采用状态机而非硬切：站立稳定 3 秒进入就绪，策略连续 10 帧有效才接管，姿态超限、力矩饱和或人工急停时立即退回 trot，退回时相位从零重新开始、速度先降至 0.2 m/s。
至此，验收标准较为明确：静止 60 秒姿态漂移小于 0.5°，十米直线横向偏差小于 0.3 m，原地转 90° 超调小于 10%，0.8 m/s 连续行走五分钟不触发保护，侧向推 50 N 半秒内恢复。这些数值均可从 1 kHz 日志中回放验证，不依赖主观判断。每项至少测三次，取最差一次写入记录——验收记录的是下限，取平均值会掩盖偶发问题。交付物也应集中整理：param_v1.yaml（步态、增益、阈值、安装矩阵，全部带版本号）、估计三件 mahony.c/leg_odom.c/contact.c、控制三件 gait.c/stand.c/walk.c，外加一段 5 min 的 1 kHz 日志——这些即下一篇仿真对拍时“同一套代码”的全部含义。
下一篇将讨论如何将这一段实机调参迁移到 MuJoCo：使用同一份 URDF、同一套控制代码，在仿真中先覆盖各类跌倒场景，再讨论 sim2sim 与 sim2real（四足开发实战（九）：仿真篇——MuJoCo 与 sim2sim）。
