给启元机器人装上黑匣子:DCM 框架与 Q1 的站立稳定域地图
给机器人装上黑匣子:DCM 框架与 Q1 的站立稳定域地图
一句话导读:Q1 的闭环步行摔了五次,我给每条机理都起了名字,但拿不出曲线——“踝关节饱和"这种话写进文章,只能算给人留个念想。这一篇把黑匣子补上:DCM(发散质心)记录仪、站立稳定域扫描、非凸平衡点地图、零反馈/反馈对照。所有数字来自
q1-dcm-walk一次运行:开环 3 秒 DCM 跑到 9.18 公里,12 组站立扫描按判定器口径 0 组通过,骨盆锚点 +0.02 m 一处单独摔出 39.66 cm,紧邻的 +0.03 m 却稳稳站着。如果你也卡在"看起来该稳却不稳”,这套测法可以直接抄。

1. 定性归因欠的那笔账
那轮实验结束时,我列了五个失效机理:踝关节角度饱和、位置腿的"刚性传递"、摆动脚擦地、双支撑期换脚冲击、脚底吸收质心误差。每一条都能自圆其说,但每一条都只有一句定性描述——没有曲线、没有阈值、没有"偏差多少度开始摔"。
这在工程上是很尴尬的位置。定性归因听起来很专业,实际上不可反驳:摔了就是因为"踝关节饱和",那怎么验证?把踝关节力矩记录下来看有没有到上限。不记录,这句话就永远只是一个说法。所以这一轮我不急着修步行,先做一件更基础的事:
先把"摔"这件事变成可测量、可回放、可比较的数字。
具体四件事,对应下文的四个脚本:
- DCM 记录仪:把像素级的摔倒视频,换成一条 3 秒内从 5 cm 涨到 9.18 km 的曲线(
01_dcm_basics.py); - 站立稳定域扫描:骨盆高度 × PD 增益 12 组网格,看哪一格的机器人能站住(
04_stand_scan.py); - 非凸平衡点扫描:固定高度增益,只动骨盆锚点 x,扫出稳定/摔倒交替的"群岛"(
04_stand_scan.py --sweep-x); - 反馈对照:零反馈 vs DCM 反馈,同一个规划器、同一组脚步,只差一行反馈项(
02_walk_dcm.py)。
定位说清楚:这篇不解决步行。它做的是给那五次摔倒补证据链,顺便把"位置 PD + 足位 IK"这套结构的稳定性边界画出来。后面要做踝力矩控制,得先知道现在到底卡在哪。

2. DCM 十分钟:把"会不会摔"变成一个数字
整个实验的动力学基础是线性倒立摆(LIPM):
ẍ = ω²(x − p), ω = √(g/z_c)
x 是质心水平位置,p 是 ZMP,z_c 是质心高度。Q1 弯膝站立时 z_c ≈ 0.415 m,ω ≈ 4.86 rad/s。这个方程的全部含义是:只要质心不在 ZMP 正上方,质心就会以 e^(ωt) 的速度加速离开它。时间常数 1/ω ≈ 0.21 s,0.4 秒放大 e^1.94 ≈ 7 倍。
LIPM 的问题是它有两个模态:一个发散、一个收敛。直接看 x 的曲线,两者叠在一起,看不出"现在到底危不危险"。DCM(Divergent Component of Motion,发散质心)就是把这层窗户纸捅破:
ξ = x + ẋ/ω
对时间求导,代入 ẍ = ω²(x − p):
ξ̇ = ẋ + ẍ/ω = ω(ξ − p)
x 的二阶方程被拆成了一个一阶方程。这个式子的物理意义非常直白:ξ 是质心"如果现在把 ZMP 钉死在 p 不动,最终会漂到哪里"的那个位置。ξ > p,质心正在加速往前跑;ξ < p,正在往回掉;ξ = p,暂时静止。
于是"会不会摔"变成了一个纯粹的几何问题:ξ 落在支撑多边形内,踝关节还有救;ξ 冲出支撑多边形边缘,踝关节就无能为力了,必须迈步去"接"它。支撑多边形能覆盖到的 ξ 范围,就是捕获区(capture region)。
这就是 DCM 框架值钱的地方:它不需要你算未来的轨迹,只要在每个时刻算一个数,然后问"这个数在不在脚底板的正上方"。

01_dcm_basics.py 里把这件事做成了纯 numpy 实验(不加载 MuJoCo,3 秒、1 ms 步长)。场景很朴素:t=0.5 s 时给 DCM 参考一个 5 cm 的阶跃(“我想往前走 5 厘米”),然后让 ZMP 直接钉在参考位置上——这正是"位置腿把质心误差硬传给脚底"在动力学上的本质。
结果是失控的:3 秒后 ξ 达到 918046.8 cm,约 9.18 公里。这不是数值爆炸,是 e^(4.86×2.5) ≈ 1.9×10⁵ 乘以 5 cm 的严格推论。开环时那个"参考位置"根本不是一个平衡点,它只是把不稳定极点暴露得更快。
同一张图下半部分给了反馈版本:
p = ξ − (k_ξ/ω)(ξ_ref − ξ)
代进 ξ̇ = ω(ξ−p) 得 ė = −k_ξ e,误差是纯一阶衰减。k_ξ 取 2 / 4 / 8 时收敛时间实测 1.61 / 0.80 / 0.40 s,正好是 1/k_ξ 的比例。这就是"用 ZMP 位置换收敛速度"的交易:k_ξ 越大追得越快,但需要的 ZMP 位移越大——而 ZMP 物理上只能在脚底范围内移动。第 6 节那个"反馈反而加大冲击"的事故,根子就在这里。

图 2 是捕获区的概念版。支撑区画成 0~10 cm 的绿色带,t=1 s 给 DCM 一个瞬时冲击:冲击后 ξ=2 cm,还在带内,踝策略能处理;ξ=6 cm,出了带边缘,ZMP 钉在带前缘也拉不回来,只能迈步。图 2 说的是"ξ 出界就救不回来";第 6 节的 60 N 推力会把 ξ 直接打到 25 cm 以上,三档应对全摔——那是同一件事的量级版。
3. 动手 1:给 Q1 装 DCM 记录仪
理论再漂亮,落到 MuJoCo 里第一件事是:ξ 怎么算。
质心位置直接读 data.subtree_com[pelvis_id],质心速度却要自己差分。MuJoCo 的 qvel 是广义速度,用雅可比投影到质心也行,但对这个实验来说直接差分最省事——问题只在噪声:1 kHz 步进、接触冲击每帧都在抖,差分出来的速度毛刺能把 ξ 顶上几百厘米。
所以记录仪里有个速度 EMA:
com = data.subtree_com[pelvis_id]
if com_prev is None:
com_prev = com.copy()
v_raw = np.array([com[0] - com_prev[0], com[1] - com_prev[1]]) / dt_sim
v_ema = 0.5 * v_ema + 0.5 * v_raw # 速度 EMA 去噪
com_prev = com.copy()
xi_meas = np.array([com[0] + v_ema[0] / omega,
com[1] + v_ema[1] / omega])
三行有效代码,0.5/0.5 是随手定的,没调参——它的作用是压掉单帧毛刺,代价是约 2 ms 的相位滞后,对 ω=4.86 的系统可以忽略。真要上真机,这一步得换成 IMU + 编码器的卡尔曼融合,仿真里先怎么简单怎么来。
记录仪的价值不在于算了什么,而在于它同时留下两条曲线:规划器给的 ξ_ref 和 实际测的 ξ_meas。开头列的"五个机理",全都是从这两条线的差里读出来的:
| 记录项 | 含义 | 对应的失效机理 |
|---|---|---|
| ξ_ref(规划) | 有界 LIPM 求解器给的 DCM 轨迹 | — |
| ξ_meas(实测) | 由质心位置/速度反算 | 偏差即"执行器没跟上" |
| 踝关节力矩 | data.actuator_force | 机理 1:踝饱和 |
| 接触点位置 | data.contact 里 foot 的 geom | 机理 3:摆动脚擦地 |
| root_z | 骨盆高度 | 摔倒判据(唯一可靠的) |
最后一行是这一篇最反直觉的发现,第 4 节会把它摆到台面上:pitch 角不能用来判摔倒,root_z 才能。
4. 动手 2:站立稳定域扫描——升高 2 厘米就全摔
闭环步行是从"弯膝站立姿态"上起步的。弯膝多深?PD 增益给多大?这两个参数当时是手调的,感觉差不多就上。这一节把它们变成一张网格。
04_stand_scan.py 的扫描设计:
- 骨盆高度 z ∈ {0.42, 0.44, 0.46} m;
- PD 增益倍率 kp_scale ∈ {1.0, 1.5, 2.0, 3.0}(基准是髋膝 150/12、踝 100/8);
- 每组在目标姿态上压地 4 mm,带重力前馈,纯位置 PD 跑 3 秒;
- 判定:3 秒内质心水平漂移 < 2 cm 且 |pitch| < 5°;摔倒判据是
root_z < 0.35 m。
每组的 IK 都在给定骨盆位姿上现场解,双脚踝目标固定在 (0, ±FOOT_Y, ANKLE_Z)——也就是说,骨盆越高,腿越直。这个细节后面会变成整篇文章最大的坑。

结果非常干脆:
| 骨盆高度 | kp×1.0 | kp×1.5 | kp×2.0 | kp×3.0 |
|---|---|---|---|---|
| 0.42 m | 稳 3.07 cm | 稳 2.46 cm | 稳 2.24 cm | 稳 2.15 cm |
| 0.44 m | 摔 12.35 cm | 摔 11.59 cm | 摔 11.11 cm | 摔 11.16 cm |
| 0.46 m | 摔 31.23 cm | 摔 19.84 cm | 摔 28.93 cm | 摔 32.29 cm |
(表内数字是 3 秒最大质心漂移;“稳”=root_z 未跌破 0.35 m)
三件事值得逐条说。
第一,0.42 m 是孤零零的一行。 抬高 2 cm,增益从 1 倍拉到 3 倍,四组全部摔倒。增益在 0.42 m 这一行确实有用——漂移从 3.07 cm 降到 2.15 cm,约 −30%——但它的作用范围只有这 2 厘米。PD 增益不是万能的,它只在腿还有余量的时候有效:0.44 m 时膝角已经很接近伸直,PD 再硬也只是把腿"锁"得更死,锁得越死,踝关节越没有修正空间,摔得越快。
第二,判定器报了"稳定工况 0/12"。 这个输出一开始把我唬住了——明明 z=0.42 的四组 root_z 都没掉下去,凭什么说 0 组稳定?翻代码发现是漂移阈值卡在 2 cm,而这一行最好的成绩是 2.15 cm,全都差一点点。
这是一个典型的判定口径事故:fell=False 说明它站住了,drift < 2 cm 说明它站得很定,两个条件被我塞进了同一个"稳定"里。真实情况介于两者之间——机器人没有倒,但一直在以毫米级的速度缓慢蠕移,3 秒 2~3 cm。这种状态该叫什么?我后来在总结里把它和"站得定"分开写成两列,判定器保留严格口径(宁可漏报),文章里如实标注。测量工具的第一个坑永远是阈值,而不是公式。
第三,pitch 完全救不了场。 看这一列的数字:z=0.44 的 pitch 最大值是 1.7° / 0.76° / 1.22° / 0.16°,z=0.46 是 0.12° / 0.13° / 0.06° / 0.04°。八组全部摔倒的工况,pitch 最大值不超过 1.7°。
如果你按常识写判据(“机身倾角超过 30° 就算摔”),这八组会被全部判成"站着"。原因也简单:这些摔法不是整体翻转,而是腿折叠、骨盆下沉、人往前滑出一段——身体大部分时间还是竖着的,只是矮了、歪了一点。
对照组是第 6 节的步行:同样是这套判据,pitch 峰值 80.8°,root_z 最低 0.044 m。两种摔法在 pitch 上差了两个数量级,在 root_z 上却都是"跌破 0.35 m"。

图 9 是那组步行抽的三帧(1.5 / 2.3 / 3.0 s),同一套判据下拍的。A 帧还在原地站着;B 帧身体已经整体后倾;C 帧躺平,root_z 最低 0.044 m。
把这三帧和本节开头那 8 组"蹲着摔"摆在一起,就是为什么我在判据里只用 root_z:同一个 root_z 阈值(0.35 m)能同时抓住"整体翻转"和"折叠下沉"两种摔法,pitch 一个都抓不住(前者太晚,后者永远不到)。
# 判摔倒:只看根节点高度,不看倾角
fell = data.qpos[2] < 0.35 # root_z
# pitch 照常记录,但只当"姿态变化"看
pitch = np.degrees(np.arctan2(
2 * (quat[0] * quat[1] + quat[2] * quat[3]),
1 - 2 * (quat[1] ** 2 + quat[2] ** 2)))
这段代码还有一件事要交代:机器人翻转超过 90° 之后,pitch 会在 ±180° 之间跳——图 5 右下角那条紫线就是从 +50° 甩到 −70°、再翻回 +80°。这不是 bug,是欧拉角表示的固有缺陷:一个角度数字表达不了"翻过去了"这件事。所以它更不能当摔倒判据,只能当"姿态变化有多剧烈"的参考。要做摔倒瞬间的朝向分析,直接用旋转矩阵第三列算重力投影,别绕欧拉角。
5. 动手 3:平衡点扫描——稳定区是一串群岛
上一节扫的是"骨盆多高、增益多硬"。这一节换个轴:高度和增益都固定(z=0.42、kp×2),只动骨盆锚点 x,从 −0.01 m 扫到 +0.04 m,步长 1 cm。
为什么扫这个?因为闭环步行里,骨盆参考轨迹是由规划器给的,锚点差一厘米,整条腿的 IK 解都会变。这是"参数敏感性"最直接的测法。

结果不是一个区间,是一串岛:
| 骨盆锚点 x | 质心起点 | 质心终点 | 3 秒漂移 | 判定 |
|---|---|---|---|---|
| −0.01 m | −0.0022 | −0.0170 | 2.29 cm | 稳 |
| 0.00 m | +0.0055 | −0.0069 | 1.89 cm | 稳 |
| +0.01 m | +0.0133 | +0.0032 | 1.54 cm | 稳 |
| +0.02 m | +0.0182 | −0.3784 | 39.66 cm | 摔 |
| +0.03 m | +0.0259 | −0.0062 | 5.67 cm | 稳 |
| +0.04 m | +0.0370 | +0.0217 | 2.32 cm | 稳 |
岛 A 是 [−1, +1] cm,悬崖在 +2 cm,岛 B 是 [+3, +4] cm。
一个点摔、两个点又稳,这不是随机噪声——MuJoCo 是确定性仿真,同一脚本、同一组参数重跑会得到同一条曲线。所以问题只能出在"这一组的初始条件跳到了另一个解支上"。
+0.02 m 这一行的质心终点是 −0.3784 m:它不只是摔,它是往后退了将近 40 厘米才倒。而它的邻居 +0.03 m 稳稳收在 −0.006 m。同一个控制器、同一组增益、只差 1 厘米锚点,结局差 40 厘米的位移。

我一开始以为是摩擦或者接触的问题,把地面摩擦系数从官方 XML 的 0.6 覆盖到 1.0 重跑——12 组结果逐字节完全相同(这一点第 7 节展开,它本身就是一个坑)。摩擦排除了,那就只剩一个嫌疑:IK 解出来的姿态不一样。
于是做了一组对照:同一个 try_stand_x,一次正常调用 ik.seed() 把腿摆到预弯膝姿态再解,一次不 seed、直接从 qpos0(直膝)开始解。其余全部不变。
| 骨盆锚点 x | 有 seed 的膝角 | 有 seed 漂移 | 无 seed 的膝角 | 无 seed 漂移 |
|---|---|---|---|---|
| −0.01 m | +0.176 rad | 2.12 cm 稳 | +0.159 rad | 0.36 cm 稳 |
| 0.00 m | +0.179 rad | 1.79 cm 稳 | +0.159 rad | 0.07 cm 稳 |
| +0.01 m | +0.182 rad | 1.46 cm 稳 | +0.159 rad | 0.23 cm 稳 |
| +0.02 m | +0.013 rad | 39.12 cm 摔 | +0.159 rad | 0.52 cm 稳 |
| +0.03 m | +0.016 rad | 5.39 cm 稳 | +0.161 rad | 7.15 cm 稳 |
| +0.04 m | +0.209 rad | 2.31 cm 稳 | +0.159 rad | 1.10 cm 稳 |
(两组在同一脚本、同一增益、同一模型下跑出;数值与 04 主表相差不超过 0.5 cm,属实现细节差异,不影响结论)
答案在膝角那一列:有 seed 时,+0.02 m 和 +0.03 m 这两行解出来的膝角是 0.013 rad 和 0.016 rad——几乎是直膝;其余行是 0.176~0.209 rad 的弯膝。无 seed 时,六行全都收敛到 0.159 rad。
也就是说,+0.02 m 这个锚点下,DLS 从预弯膝起点出发,最终收敛到了直膝解(膝角 0.013 rad),足位误差同样进了门限——同一个目标位姿存在两个解,求解器选了物理上几乎不能站的那一个。这不是收敛失败,是收敛得太"成功":误差范数达标就算完事,没人管解出来的姿态还有没有控制余量。
直膝意味着踝关节的力臂和修正余量都趋近于零(弱奇异方向),3 秒里任何一点扰动都只能靠髋关节兜——而髋关节在位置 PD 下只会把误差往脚底传。这就是 39.66 cm 的来路。
所以"平衡点是 IK 分支的函数"这句话,落到工程上就是三条:
- 不要用单一 IK 解验证稳定性。至少跑两个起点,看解出来的姿态是否一致;
- 把解出来的关节角当一等公民记录,不能只记 com 和漂移——这次要不是对比膝角,我根本不知道是分支问题;
- 弱奇异方向要显式惩罚。直膝/伸肘附近加一个姿态先验(比如膝角下限 0.15 rad),比调增益有效得多。
顺带一个反直觉的结论:无 seed 的那一组反而更稳(漂移普遍小一个量级,包括 +0.02 m)。因为全部落在同一个直膝附近的分支上,姿态一致、质心偏低。但这不是"直膝更好"——它的踝余量依然接近零,只是这批 3 秒静置测试还没逼出问题。稳定不等于好,只是还没被推。
(上面这三条,把开头列的"五个机理"压缩成了一句话:摔倒的第一现场往往不在控制律,而在姿态求解器选择了哪个解支。)
6. 动手 4:反馈对照——加了一行代码,摔得更"体面"
第 2 节推导的反馈式看起来是个万灵药:p = ξ − (k/ω)(ξ_ref − ξ),代进去误差就是纯指数衰减。那把它加进那套闭环步行里,是不是就能不摔了?
02_walk_dcm.py 就是做这个对照的。同一个有界 LIPM 规划器、同一组脚步(2 步 × 5 cm,计划位移 0.1 m)、同一个 PD 增益,只改一行在线修正:
if case == "feedback":
fx = np.clip(k_com * (xi_ref[0] - xi_meas[0]), -fb_clamp, fb_clamp)
else:
fx = 0.0
pelvis_ref[0] += fx # 只动骨盆参考的 x
三档:零反馈(k_com=0)、反馈(k_com=0.15,限幅 12 mm)、反馈收紧(k_com=0.15,限幅 4 mm)。

先看零反馈那张图(图 5)。左上角是前进方向:虚线是规划给 DCM 的参考,2 秒后开始上行,一路爬到 +9 cm;绿色是实测 ξ,2.5 秒后掉头往下,滑到 −39 cm。一个往上、一个往下,方向反得非常干脆。 左下角"反馈修正 δ"那一格是一条贴着零的直线——K=0,这张图里没有任何在线修正,正好当基座。右下角是姿态与高度:俯仰从 0 跳到 +50°,再甩到 −70°,6 秒后又翻到 +80°;root_z 则从 2.5 秒起一路掉到 4 cm 附近。
这就是肉眼看视频、做定性归因必然失效的原因:肉眼能看见摔,看不见"已经开始摔"的那个点,而那个点在曲线上是一目了然的折点。
| 档位 | 实际位移 | 计划位移 | DCM 峰值偏差 | pitch 峰值 | root_z 最低 | 结论 |
|---|---|---|---|---|---|---|
| 零反馈 | −0.326 m | +0.100 m | 41.07 cm | 80.8° | 0.044 m | 摔 |
| 反馈 12 mm | −0.255 m | +0.100 m | 33.45 cm | 83.2° | 0.046 m | 摔 |
| 反馈 4 mm | −0.214 m | +0.100 m | 29.33 cm | 82.3° | 0.043 m | 摔 |
注:这三行是当时的留档值。按第 9 节的命令重跑,零反馈与 4 mm 两档逐位复现(41.07 / 29.33),12 mm 这档给的是 33.23 cm、后退 25.3 cm,比表里少 0.2 cm。同一命令连跑两次都是 33.23,所以不是随机噪声;差在哪一次改动上我没能回溯出来——当时那一版脚本没有单独留档。表里保留原值,图 7 和下面的比例都按原值算。

三件事,一件比一件打击人。
第一,反馈是有效的,而且效果可量化。 DCM 峰值从 41.07 降到 33.45(−18.6%),限幅收紧后降到 29.33(−28.6%);后退距离也从 32.6 cm 收到 21.4 cm。方向是对的,量级也对——这证明第 2 节那个 ė = −k e 的推导在 MuJoCo 里没有失效。
第二,它一点没改变"摔"这个结局。 三档的 root_z 最低值分别是 0.044 / 0.046 / 0.043 m,pitch 峰值 80.8° / 83.2° / 82.3°——摔得同样彻底,甚至更"横"。
第三,也是最要命的:方向反了。 规划和执行都朝 +x,计划走 0.1 m,实际三档全部后退 21~33 cm。反馈只是把这个"反向"缩小了一点,从来没有把它翻正。
原因在第 2 节其实已经埋好了:反馈项改的是 ZMP 位置,而 ZMP 只能决定"质心此刻往哪加速",它管不了"脚往哪落"。摆动脚擦地、位置腿的刚性传递,全发生在落足那一瞬间——脚落错了地方,后面用 ZMP 怎么补都是在错误的地基上做微调。DCM 反馈修的是"过程",修不了"目标"。
顺手把推力实验的数据也摆出来。03_push_recovery.py 在站立段 t=1.0 s 给骨盆一个 60 N、0.15 s 的 x 向冲击,三种应对:
| 工况 | 全程 DCM 峰值偏差 | 推力窗口内峰值 | root_z 最低 | 结论 |
|---|---|---|---|---|
| 纯 PD(baseline) | 60.35 cm | 25.04 cm | 0.049 m | 摔 |
| DCM 反馈 | 50.15 cm | 35.21 cm | 0.047 m | 摔 |
| DCM 反馈 + 捕获步 | 50.15 cm | 35.21 cm | 0.047 m | 摔 |

这里出现了这一篇最有意思的反常:反馈把"全程最大偏差"压低了 17%,却把"冲击窗口内的瞬时偏差"抬高了 41%(25.04 → 35.21 cm)。
我的解释是:60 N 打过来的 0.15 秒里,反馈项要求 ZMP 立刻反向移动,但位置腿的 PD 只有那么大带宽、限幅又卡在 5 mm,参考跑得比实际快,ξ_ref − ξ_meas 反而被拉得更大。你的修正量在冲击瞬间变成了另一种扰动。 这是"反馈饱和"在数字上的样子——不是反馈错了,是执行器跟不上反馈的算盘。(这一段是机制推测,没有单独做消融实验验证,读者可以自行存疑。)
至于"捕获步"那一行为什么和"反馈"逐字段相同,查代码后真相很朴素:
if capture and str(ref["phase"][k]) == "ss": # 只在单支撑相生效
cap_side = str(ref["swing"][k])
if abs(e[0]) > args.cap_thresh:
cap_x = float(np.clip(-args.k_step * e[0], -args.cap_back, args.cap_fwd))
捕获步的门槛是 phase == "ss"(单支撑相)。而这次推力打在 --push-t 1.0,默认 --t-stand 2.0 还没走完——机器人正处于双支撑站立段,整个 2 秒里 phase 从来没变成 "ss",捕获步一次都没醒。
这不是数值 bug,是设计缺口:捕获步只为步行写,站立段的抗推它压根不管。所以那一行不是"捕获步起了作用",而是"捕获步根本没上班",两档自然逐字段相同。我在第 8 节把这件事记入边界。
7. 踩坑实录:五个让我重跑的坑
这一节按"浪费了多少时间"排序。每个坑都能在仓库里找到痕迹,不是复盘时凭空想出来的。
坑 1:判定阈值先坑了自己
04_stand_scan.py 的判定是"漂移 < 2 cm 且 |pitch| < 5°",结果 z=0.42 那一行最好的成绩是 2.15 cm——全被判成失败,输出一行冷冰冰的"稳定工况 0/12"。
我盯着这个 0 看了很久,以为是物理结论(“这个控制器根本站不住”),直到去翻每组的 fell 字段才发现四组全部 False。判据把"站得住"和"站得定"混成了一个词,而工程里这两件事差一个量级的重要程度。
教训:判定器要输出原始量,让报告层去分档。fell 和 drift 分开列,别在测量层做结论。
坑 2:改了一个"肯定有关"的参数,结果逐字节相同
平衡点扫描发现 x=+0.02 m 单独摔 39.66 cm,第一反应是"打滑"。官方 XML 的地面摩擦是 0.6,仿真里我习惯覆盖到 1.0,猜测是 0.6 太小。
于是把 --mu 从 1.0 改回 0.6 重跑 12 组,然后把两个 CSV 逐行比对:
Compare-Object (Get-Content results\04_stand_scan\stand_scan.csv) `
(Get-Content results\04_stand_scan\stand_scan_mu06.csv)
→ 0
(stand_scan_mu06.csv 就是 --mu 0.6 那一轮的原样输出,留在目录里当证据。)
差异行数 0,连浮点尾数都一样。 因为这个实验里机器人是"原地慢慢塌",脚根本没打过滑——摩擦系数在这个工况下不是瓶颈,改成 0.05 也不会变。
教训:改参数之前先证明它是瓶颈。判断方法很便宜:把参数推到极端(摩擦拉到 10 或者 0.01),如果结果不变,那它就不在因果链上。
坑 3:IK 解出来的姿态,决定了机器人站不站得住
这个坑在第 5 节已经展开,这里只补一句为什么它排第三:因为它花了最久。从"怀疑摩擦"到"怀疑接触"到"怀疑增益",最后才想到打印膝角。一行 print(qleg[3]) 省了我两个小时。
坑 4:捕获步是"步行专用"的死代码
60 N 推力实验里,capture 档和 feedback 档的输出逐字段相同。我第一反应是"捕获步逻辑写错了",翻了半小时,最后发现门槛是 phase == "ss"——而推力打在站立段,phase 永远是双支撑。
这不算 bug(捕获步本来就是给步行写的),但它暴露了一个更普遍的问题:一个只在特定相位生效的策略,放进"全相位"的对照实验里,就会变成沉默的对照组。 实验设计要么限定相位,要么在报告里明确写"该档在本工况下不生效"。
坑 5:重力前馈关掉,站立静差把你推下去
03_push_recovery.py 里留着一行注释:
ap.add_argument("--gff", type=int, default=1,
help="重力前馈开关(旧版误设为关,站立静差致提前失稳)")
早期版本没开重力前馈,纯位置 PD 在站立段就带着几毫米的静差,2 秒后机器人已经开始往一侧倒——我一度以为那就是"推力实验的基线响应"(“没推它也摔”)。开了前馈之后,baseline 才稳定走完站立段并进入推力窗口,三档才有了可比性。
顺带记一个参数分散的坑:01_dcm_basics.py 里 z_c 取 0.415(ω=4.862),02/03 走 --com-height 0.42(ω=4.833)。两个 ω 都对,但如果你把两个脚本的曲线叠在一张图上,"同一条 DCM 曲线为什么对不齐"会折磨你半小时。同一个物理量在仓库里有几个默认值,就应该有几个显式的说明。
8. 诚实的边界
这篇的结论都能复现,但它们的适用边界比结论本身更值得写清楚。
一、反馈不改变"摔不摔",只改变"摔的过程"。 三档步行的 DCM 峰值降了 28.6%,位移少退了 11 cm,但 root_z 一样跌破 0.05 m。如果有人拿"DCM 峰值下降"来论证"反馈有效",这个实验支持前半句,不支持后半句。
二、8 步规划本身超出了这套结构的能力。 计划走 +0.1 m,实际三档全部后退 21~33 cm。这不是反馈的锅,是"位置腿把力传反"的直接后果。换控制器之前,先修结构。
三、3 秒静置测试证明不了"稳定"。 第 5 节里"无 seed 更稳"的那组,漂移只有 0.07~0.52 cm,看着漂亮——但那是直膝近奇异姿态,踝关节余量接近零。给它一个 60 N 的推力,大概率立刻崩(有 seed 的弯膝组在 60 N 下也崩了)。静置测试只能筛掉明显不行的,筛不出"看起来行"的。
四、捕获步在本篇所有实验里都没有生效。 它只写在 03_push_recovery.py 里,而那一组推力打在站立段(双支撑),门槛 phase == "ss" 永不满足;步行脚本 02_walk_dcm.py 里根本没有捕获步代码。所以"捕获步能救 60 N"在本仓库里没有任何数据支持。谁要引用,请只引"代码存在、逻辑未验证"。
五、没有真机数据,也没有对照步态。 这一篇所有数字来自 MuJoCo 3.17 + 官方 Q1 XML,PD 增益、摩擦、接触参数都是仿真值。而"真机上踝关节会用完行程"这件事,在这里依然无法验证。
六、ω 被当成了常数。 站立段 z_c 取 0.415~0.42 m,但弯膝深度变化时质心高度会跟着变,ω 也会变(z_c 下降 2 cm,ω 上升约 2.4%)。本篇所有反馈增益都是按固定 ω 整定的,姿态一变,增益的物理含义就漂了。
9. 复现清单
工程在 q1-dcm-walk/,Python 3.11 + MuJoCo 3.17.0,全部离线可跑。01~04 与 06 是纯计算,不需要图形环境;摔倒定格帧和演示页要用离屏渲染,需要 OpenGL(本机是 Windows + 集显,跑得动)。所有脚本都用 Path(__file__) 反推仓库根,里面没有本机绝对路径。
q1-dcm-walk/
├─ scripts/
│ ├─ common.py # 模型加载、PD 力矩、重力前馈
│ ├─ foot_ik.py # 阻尼最小二乘足位 IK(含 seed 与多起点)
│ ├─ dcm_planner.py # 有界 LIPM 求解 + DCM 轨迹
│ ├─ dcm_monitor.py # 彩蛋:k_ξ 滑块监视器(纯 numpy,不用 MuJoCo)
│ ├─ walk_common.py # 标定、参考姿态生成
│ ├─ 01_dcm_basics.py # 纯 numpy:开环发散 / 反馈收敛 / 捕获区
│ ├─ 02_walk_dcm.py # 步行 + DCM 记录仪(零反馈 / 反馈 / 收紧)
│ ├─ 03_push_recovery.py # 站立段 60 N 推力,三档应对
│ ├─ 04_stand_scan.py # 站立域 3×4 扫描 + 平衡点岛扫描
│ ├─ 06_ik_branch.py # IK seed 对照(第 5 节解支表)
│ ├─ make_figures.py # D3/D4/D10 汇总图
│ ├─ make_d7.py # D7 三档对照
│ └─ make_fig_d9.py # 摔倒三帧拼图(D9)
├─ webui/ # 交互演示页(实时 3D + 四张诊断图)
└─ results/ # 全部 JSON/CSV/PNG 产物
按顺序:
python scripts/01_dcm_basics.py # 3 秒发散到 9.18 km,反馈 0.4~1.6 s 收敛
python scripts/02_walk_dcm.py --steps 2 --k-com 0 --out results/02_walk_nofb
python scripts/02_walk_dcm.py --steps 2 --k-com 0.15 --out results/02_walk_fb
python scripts/02_walk_dcm.py --steps 2 --k-com 0.15 --fb-clamp 0.004 \
--out results/02_walk_fb_safe # 三档步行(每档一个目录)
python scripts/02_walk_dcm.py --steps 2 --k-com 0 --out results/05_fall_frames \
--snap 1.5,2.3,3.0 # 第 5 节的三张定格帧(离屏渲染)
python scripts/03_push_recovery.py # 60 N 推力,baseline/feedback/capture
python scripts/04_stand_scan.py # 站立域 12 组
python scripts/04_stand_scan.py --sweep-x # 平衡点岛(-1..+4 cm)
python scripts/04_stand_scan.py --mu 0.6 --out results/_mu06
# 坑 2 的摩擦对照,再和第 4 节那份 diff
python scripts/06_ik_branch.py # IK seed 对照(两组各 6 个锚点)
python scripts/make_figures.py # D3/D4/D10
python scripts/make_d7.py # D7 三档对照
python scripts/make_fig_d9.py # 三张 snap 拼成 D9
图 1/2/5/8 没有独立作图脚本,它们是 01/02/03 脚本顺手存下的 dcm_divergence.png、capture_region.png、walk_curves.png、push_recovery_curves.png;正文里的 D 编号是这些产物重新排版后的版本,曲线可复现,版式不可——排版脚本没随仓库提供。D3/D4/D10 和 D7 是独立脚本画的(make_figures.py、make_d7.py),输入全是 results/ 下的 CSV/JSON,重跑得到同一组数。D9 的三帧是离屏渲染:summary.json 逐位一致,但 PNG 的字节哈希会变(相机每步跟着 qpos 走,渲染器不保证逐字节可复现)。
演示页:
python webui/server_dcm.py # 打开 http://127.0.0.1:8769

页面里能直接点 30/50/60 N 推力、切三种相机、拖捕获区沙盒的增益和冲量滑块。它读的是 results/ 里的真实 CSV,不是前端硬编码的示意数据——换句话说,演示页上的每根柱子都能在结果目录里找到对应文件。
界面分五块,正好对应本篇的五个实验:01 实时舞台(MuJoCo 离屏渲染,1 kHz 物理)、02 DCM 黑匣子(ξ 参考与实测的双线 + ±6 cm 捕获带)、03 站立稳定域热图(04_stand_scan 的 3×4 网格)、04 非凸平衡点柱图(--sweep-x 的六个点)、05 捕获区沙盒(ξ̇ = ω(ξ−p) 的实时积分,拖滑块就能看到"增益太小衰不掉、太大则 ZMP 需求被推离支撑区")。
10. 写在最后
把这条线串起来看,形状已经比较清楚了:
- 问题在结构:位置腿 + 22 关节纯位置 PD,看着能站稳,其实没有余量;
- 规划器能算轨迹:有界 LIPM + 足位 IK,把目标轨迹算出来;
- 失败是真实的:五个失效机理,早先只能靠肉眼和切片图归因;
- 这一篇装上黑匣子:DCM 记录仪、稳定域地图、非凸平衡点、反馈对照。
这一篇最大的收获,不是"反馈降低了 18.6% 的 DCM 峰值"这种数字,而是两件更朴素的事:
第一,摔倒的第一现场往往不在控制律。 x=+0.02 m 那个孤零零的悬崖,是 IK 分支选错了直膝解;翻遍增益、摩擦、接触参数都找不到原因。控制器的调试方向如果一开始就错了,跑得再快也是在错的方向上烧时间。
第二,测量工具本身会骗你。 判定阈值把成功的工况判成 0/12;pitch 在八组摔倒里最大只有 1.7°;捕获步一行代码看似在跑,其实在睡觉。先怀疑量具,再怀疑物理。
下一步往哪走?两个方向,我倾向于第一个:
- 换执行层:把踝关节从位置 PD 换成力矩/导纳控制,直接测"踝关节还剩多少行程",把机理 1(踝关节饱和)从定性变成数字;
- 换结构:保留位置 PD,但把足位 IK 的分支选择显式化(姿态先验 + 弱奇异惩罚),看能不能让稳定岛从"一点宽"扩到"三厘米宽"。
两条路都要先回答同一个问题:你现在的稳定裕度,到底是在哪个自由度上被吃掉的? 这一篇给的工具箱,就是为了回答它。
DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐

所有评论(0)