记录解决轮腿机器人计算NaN问题的过程

前言

这个bug改起来并不轻松，一整年的坚持与准备，被一个月都修不好的bug打败了，最终筹备很久的项目也因此搁浅。直到一年后，才重新拾起来修复，得以了却了心结。感谢2024.10-2025.10曾经努力的我们。

image

小美人看的我心都化了，是谁画了这么dio的车？

迷惑的表象：“劈叉”

在完成轮腿机器人项目中，由于分电板有点小问题，一个板子只能连四个电机，而关节加轮子一共6个。打板需要时间，调试只能分开调了。是从轮子开始调，先简单用轮速跳调了一些LQR平衡控制和位移控制。
然后单独调试关节什么的，搞完了vmc，腿长pid的部署。
最后新板子到了，整体联调又解决了一大堆问题。之后发现在光滑的瓷砖地板上很容易打滑导致轮子抖动。

于是参考玺佬的文章，将轮子算出的速度和上层姿态解算的估算值，利用卡尔曼滤波的做一个数据融合解算，算出来一个相对准确的位移。

然后就遇到了机器会莫名其妙在运动中“劈叉”。表现为两条腿在不同的方向上打到限位，造成“劈叉”的现象
由于限位是打印件，这个劈叉现象还曾经撞坏过限位。可以确定的是一定是一个很大的力矩。要么是有一个离谱的指令，要么是电机疯了。

下图就是劈叉的图片，图中黄色是限位 蓝色是腿部的走向 绿色是限位和腿部接触的地方。图中可以看到另一边的腿朝着相反的方法打过限位了。就像是劈叉一样。
e294dca3ef9f56dddc1a31bd03ac65c5

每次看到都心肺停止

这个奇怪的现象会出现在

  • 机器人进行一定幅度的运动后
  • 机器人在平衡后，进行一定幅度的干扰后
  • 在某些代码下，会出现上电就触发
  • 一旦出现，便无法恢复

错误的探索

从一开始直到最后放弃，都以为是上层控制算法，以及机体建模的问题。这也是难免的，因为只看到了“劈叉”的现象，以为是姿态发散了。但是这样的方向忽略了本质。

先是在B站上找有没有人遇到一模一样的情况。还真找到了一个同样的，标题是什么鲁棒性拉满了，视频里的轮腿也会劈叉，但是劈叉后马上又纠正了回去，之下相比确实鲁棒性拉满了。当时忽略了两件事：

  1. 没有鲁棒性的真相是不可恢复性，就是劈叉了，电机直接想卡死了一样堵转了，后面再发什么指令都不管用了。
  2. 视频里明显是慢慢发散的，而这边的劈叉是，一瞬间，卡一下，打到限位上了，速度之快，绝不是控制指令造成的，并且不管怎么调整参数，都是一模一样的瞬间卡死，速度之快，摄像头都无法捕捉，注定是电机疯了。

这两条证据都表明劈叉不是控制层的问题，而只是程序问题的表象，只是当时思路局限，没有意识到。

由于是从仿真中来，就现在仿真中寻找问题。这个问题在整个仿真阶段都没遇到过。那就是sim2real的问题了，一定是仿真和现实不对等的问题，有忽略的条件，因为在次之前就已经遇到过仿真中有忽略的现实条件，当时是这么想的。

当时想到了一种可能，其中一条腿方向反了。在仿真里，故意把一条腿的输入取反，一开始能正常站立，保持平衡，也对的上，加点扰动或者让机器人位移，居然真的一点点劈叉了！
高兴的不行，以为终于解决了，急头白脸的在真机的部署。结果很令人失望。原本还能保持的平衡不能保持了不说，而且依然劈叉...

关键点NAN的发现

偶然间想到，没用姿态解算融合之前，似乎一直没有出现过这种情况！赶紧去试，发现居然是真的，直接用轮速结果算位移，真的没有劈叉了！很激动，盲目的探索终于找到了一些眉目

于是从数据本身入手，用全局变量记录所有经过卡尔曼滤波的变量，用keil的debug功能看，发现了，每次劈叉的时候，都有一个奇怪的值：NAN。所以应该就是NAN导致的劈叉

为什么会这样呢？当时轮速是没有接卡尔曼滤波的，而姿态数据融合用到了卡尔曼滤波，那就感觉是卡尔曼滤波的问题。卡尔曼滤波用的是RoboMaster广泛使用的公开库，肯定不是算法的问题。可能是参数问题导致融合结果发散了。尝试了改变卡尔曼滤波的Q/R矩阵，等等，都没有任何作用...

后来又想，可能是姿态解算的数据有问题，重算了一遍还真找到问题了，可是怎么再修正，穷举公式的组合都没办法修复
最后自己也意识到了：卡尔曼就是个滤波的，本来就要解决数据错误的问题，就算融合的其中一个数据再错又怎么办呢？就算故意在仿真里给一个很离谱的上位解算的值，仿真里的机器人运动也没有很大的问题......

很久很久之后，问客服才知道：达妙电机收到到NAN的话就会在电机正方向上疯转，NAN指令经卡尔曼滤波，传导到了can指令里，最后发给电机，造成了“劈叉”的假象，原来，根本不是什么控制算法的问题。所谓的劈叉，不过是疯转的电机撞到限位，并且两边电机由于安装对的方向不通过，正方向正好相反，所以看上去像劈叉。

放弃

其实当时不止写出来的思路，还又有各种各样杂七杂八的修复和思路，从结果看上确实修复了很多算法的错误，也确实把一上电就劈叉的情况慢慢搞成了需要长时间复杂的干扰才出现劈叉的情况，可是劈叉现象一直存在，像翻不过去的大山，有几次看起来解决了，还没高兴一会咔一声劈叉，就击碎了大家的激情，都被泼了冷水一样，几次下来，都很受打击，慢慢都没有干劲了。

也有继续追查NAN，发现，卡尔曼滤波不生产NAN，只是NAN的搬运工。NAN的数据在卡尔曼滤波之前已经输入，在进入滤波前已经有了NAN，本应该继续向上追查，但是面对simulink生成的垃圾可读性代码，以及庞大的上层计算链路，在那样的士气下，也没有那个勇气去追查下去了。

不过既然轮速算的能用，没办法了，打滑就打滑吧，轮速加卡尔曼滤波，勉强用一下，其他也不管了。

当时都已进入十月份了，大家的耐心都耗尽了，这个问题出现一个月来什么进展都没有，气氛越来越压抑，几乎要出现吵架的地步。各自也有各自的事情也有要忙的了，一切就这样不了了之了。曾经轰轰烈烈煞费苦心筹备一年的计划就这样烂尾了，留下的只有不解之迷和遗憾。

真正解决

一年后的今天，利用ai的辅助，终于找到了困扰已久的原因：原来是simulink生成的雅可比计算的代码，加上float32的精度限制，导致运算中出现了除以0的情况，即有几个导致出现NAN的奇异点，机器人运动的时候扫到那几个奇异点的位置，就会生成NAN，并且NAN经过传到链路传到了电机上，而由于卡尔曼滤波被一个NAN污染了，之后所有的结果都是NAN了，所以一直给电机发NAN，这就是不可恢复的原因。

之所以轮速控制没出问题，并不是轮速模式下NaN没有产生，是因为轮速没有用到姿态解算算出来的数据，而且那个数据在其他地方也没用到。

现象回顾

在回顾一遍所有的问题：

  1. 姿态融合计算轮速（observer.c 中融合轮毂电机 + IMU 加速度 + VMC 腿部运动）时出现 NaN（调试器显示 none）；
  2. NaN 一旦出现不可自行恢复，卡尔曼输出 vel_acc 永久 NaN → State.dx NaN → LQR 轮毂力矩 NaN → 电机指令变空/失控，电机收到 NaN 指令后疯转撞限位；
  3. 只用轮毂电机数据（纯轮速路径）从不出问题；
  4. 静止平衡几乎不触发，剧烈摇晃/运动大概率触发；
  5. 不同版本触发概率不同：某个最初版本最容易，其他版本也能触发但概率低。

逐步收网

为了便于排查，就选用最容易触发的版本排查

第 1 步：排除卡尔曼滤波器

最初怀疑卡尔曼。排查确认：数据在进入卡尔曼滤波器之前就已经是 NaN（在调试器中观察到的）。卡尔曼是"受害者"而非"制造者"，导致出现一次性 NaN 就永远是NaN。

第 2 步：定位源头的NaN

加NaN检查点，在每个可能制造 NaN 的位置检测，一旦输出NaN就锁存当时的输入参数，保存第一现场。
这里用了一个全局变量的结构体来记录，方便再keil的debug模式下查看
最终抓到的源头：在左腿的vmc解算任务中抓到第一个源头的NaN，记录下此时phi1=3.0656 rad, phi4=0.0760 rad。

第 3 步：float32 精确复现（关键突破）

让AI去复现NaN的计算（最初AI用的是double精度计算，和MCU实际部署的不一样，所以也走了一段弯路，所以还是要注意精度问题）

AI 用 MCU 实际精度（float32）逐行复现 VMC_Speed 在锁存器现场的计算：

t28 = (-t10_tmp - t70*0.18F) + 0.044F   // = -(XC - 0.044)，C点相对髋中心水平偏移
     = 0.0                                ← float32 下恰好舍入为 0！
t60 = t28 * t28 = 0.0
t52 = 1.0F / t28 = Inf                    ← 除零！
→ 后续 Inf×0 / Inf−Inf = NaN

而在double 精度下 t28 = 6.5e-9 ≠ 0，不会除零——这就是为什么AI用 double 复现一直无法复现的误导原因。
事已至此，已经找到了NaN出现的真实原因，但是还有很多问题遗留：

  1. 为什么会有这样的问题？究竟有什么含义
  2. 为什么在某些版本的代码中更容易出现？
  3. 为什么仿真里面从未遇到过？

第 4 步：确认几何含义

锁存器现场 C 点坐标 x = 44.0 mm = l5/2（髋中心）。C 点在髋正上方时，1/(XC-0.044) 分母过零——这是五连杆的竖直奇异位形。

用仿真文件的杆长参数（l1=l4=84, l2=l3=180, l5=88mm）验证：

  • 奇异曲线是 phi1 + phi4 = π（五连杆左右对称、C 点在腿中线上）；
  • 这条奇异曲线正好穿过机器人站立平衡的名义位形（腿竖直对称）；
  • 由于电机编码器是量化的（16bit，步长 3.8e-4 rad），当量化后的角度组合恰好落在"精确零"上时触发。

第 5 步：穷举量化网格验证

为了找出究竟有多少个这样的奇异点，让AI用板上逐字相同的 float32 公式 + 电机反馈真实量化网格（照抄电机驱动来模拟收发）穷举：

22,509,501 组 (p_int2, p_int3) 组合:
  NaN = 332 组，全部满足: 除数 t28 == 0.0f（精确零）

float64 同网格：0 组命中。结论：float32 是把命中概率从 ~10⁻¹³ 抬到 1/68000 的根因，是公式里的可去奇点导致的。

第 6 步：修复验证（钳位方案）

对 VMC_Speed 全部分母加钳位（5 处）：

  1. t52 = 1/(t36+t44)：非有限 → 1e6
  2. sqrt 判别式：钳 ≥0（防 sqrt 负数，保险）
  3. t59：钳下限 1e-6（防 1/t59）
  4. t28(XC-0.044)：钳 ±1e-3（实测元凶）
  5. t48、1/sqrt、1/t60：非有限 → 1e6

float32 验证：锁存器现场钳位后 spd 有限，NaN 不再产生。

实机测试：✅ 通过，NaN 不再出现。至此，问题完成闭环

根因总结

因果链（四层）

第1层 根因: VMC_Speed 的除法分母 1/(XC-0.044)
       └─ C 点在髋正上方(phi1+phi4=π)时, 分母数学上趋近 0
          (解析雅可比在奇异位形本来就会发散, 可去奇点)

第2层 触发: float32 精度
       └─ double 下分母=6.5e-9(非零), float32 下被舍入成 0.0
          → 真正执行 1/0

第3层 结果: Inf → NaN
       └─ 1/0=Inf, Inf 参与后续运算(Inf×0, Inf−Inf) → NaN

第4层 放大: 卡尔曼滤波有状态
       └─ NaN 进一次, xhat/P 永久 NaN → 永不恢复
          (把瞬时 NaN 变成永久故障)

现象解释

现象 解释
静止几乎不触发 静止时实际位形略偏离 phi1+phi4=π 曲线（姿态有小偏差、两腿 L0 不同），编码器抖动打不中 332 个精确零点
运动大概率触发 晃动时 phi1+phi4 反复扫过 π，每次穿越都在曲线附近撒采样点，命中概率大增
纯轮速路径不触发 不经过 VMC_Speed，且该路径无状态（坏一帧自愈）
融合路径触发且永久 KF/积分器把瞬态 NaN 锁存成永久 NaN
FDCAN2 最容易触发 ① 腿长目标 0.19 使腿更贴近奇异曲线；② KF Q=1.0（其他版 0.1）增益猛、速度跟随激进；③ 腿部自由运动范围大，扫过奇异曲线频率高
电机疯转撞限位 NaN 力矩的后果，非原因

与参考版/仿真的对照

实机（问题版） 实机（修复版） 仿真 参考版 DM_Balance
速度解算 解析雅可比 VMC_Speed.c（带 1/分母） 钳位（或代数化简） Derivative 块（数值差分） 数值差分 (L0-last)/dt
NaN ✅ 出 ✅ 不出 ❌ ❌

VMC_Speed.c（MATLAB 符号工具箱生成的解析雅可比）是整个项目里唯一"仿真和参考版都没有"的东西，也是 NaN 的独有来源。

修复方案总结

已实施（实机验证通过）

方案 A：VMC_Speed 分母钳位（D:\Create\Legbot\程序\fdcan3\FDCAN2，commit 1cfa830）

  • 5 处除法/sqrt 分母钳位，NaN 不再产生；
  • 未加 KF 拦截（用户要求，便于验证根因）；
  • 实机测试 ✅。

备选（更优，未实施）

方案 B：代数化简——消掉 1/t28 可去奇点，NaN 数学上不可能，无精度损失。已做数值等价验证，随时可替换方案 A。——代数化简消掉 1/t28（可去奇点）：

// 原式: spd[1] = -dphi1*t60*t48*(t52*t59 - (t4/t60)*t32) + dphi4*t60*t48*(t52*(-t22*0.36F) + (t4/t60)*(t2*0.36F));
// 化简: 设 d = t28, t60*d = d², t60*t52 = d, t60*(t4/t60) = t4
spd[1] = -dphi1 * t48 * (d*t59 - t4*t32)
       +  dphi4 * t48 * (-d*t22*0.36F + t4*t2*0.36F);

验证：20 万组随机非零区域与原式数值等价（最大误差 6e-5）；3 个原 NaN 点化简后全部有限且连续。化简后公式数学上不可能产生 NaN，且无钳位引入的局部误差。若后续想彻底根治可换此方案。

方案 C（治本）：数值差分——用 (L0-last_L0)/dt 替代解析雅可比，与仿真 Derivative 块、参考版完全一致，结构上无分母。代价是引入量化噪声（卡尔曼可压制）。

错误处理

有几个安全措施，不能解决问题但是能处理错误：

  1. KF 入口哨兵：isnan(acc)||isnan(vel) 则跳过本帧；出口 NaN 则重新 Init；
  2. Motor_MIT 编码前 isnan(torq)→0：电机指令的最后一道闸；
  3. Mahony 加速度归一化 normal < 1e-6f 跳过加计修正。

总结

回过头来看，一切都是当时为了偷懒用坑爹的Matlab工具箱生成了代码，导致出了大问题。一些算法在单片机上平台的部署还是需要有一定的转换和适配，比如精度问题，比如简化算式，比如除法的处理。整个教训是工程实践能力的缺乏和代码规范的缺失，而且在debug方法上也有很多要学习的。此外，曾经还怀疑过野指针的问题，就是因为代码的架构比较混乱。一个问题用了一年的时间才解决，前路漫漫，还是要继续深入学习啊！

posted @ 2026-09-01 17:46  Hynaya  阅读(13)  评论(0)    收藏  举报