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

前言

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

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

迷惑的表象：“劈叉”

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

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

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

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

每次看到都心肺停止

这个奇怪的现象会出现在

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

错误的探索

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

先是在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. 不同版本触发概率不同：某个最初版本最容易，其他版本也能触发但概率低。

先排除卡尔曼，再抓现行

为了好排查，就直接挑最容易触发的那个版本来做。
一开始当然是怀疑卡尔曼，毕竟数据是经过它才变坏的。可排查下来发现，数据在进入卡尔曼滤波器之前就已经是 NaN 了——这一点在调试器里看得很清楚。卡尔曼是"受害者"而不是"制造者"，它干的事情只是把一个一次性出现的 NaN 变成永远的 NaN。
既然源头在前面，那就得往前抓。办法是在每一个可能制造 NaN 的位置都放一个检查点，一旦输出 NaN 就把当时的输入参数锁存下来，保住第一现场。用一个全局变量的结构体来记录，方便在 keil 的 debug 模式下翻看。
最后抓到的是：左腿的 vmc 解算任务里出现了第一个源头的 NaN，当时 phi1 = 3.0656 rad，phi4 = 0.0760 rad。
有了现场，剩下的就是把它代回去，看看到底是哪一步算炸了。

关键的一步：用 float32 复现

这一步交给 AI 去做。这里还走了一段弯路：AI 一开始是用 double 精度算的，和 MCU 上实际部署的不一样，所以怎么都对不上。精度这件事，还是得盯住。
改成 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，压根不会除零。这就是之前用 double 复现不出来的原因。
NaN 的来路算是清楚了，但问题还剩一堆：

1. 这个除法为什么会出问题，它到底代表什么？
2. 为什么在某些版本的代码里更容易出现？
3. 为什么仿真里从来没遇到过？

这个除法到底是什么意思

锁存器现场里 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），量化之后的角度组合恰好落到那个"精确的零"上，就触发了。
  那就干脆把这些点全找出来。让 AI 用和板上逐字相同的 float32 公式，配上电机反馈真实的量化网格（收发也照抄电机驱动来模拟），全部枚举：

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

同一套网格换成 float64 再算一遍：0 组命中。所以真正把命中概率从 1e-13 抬到 1/68000 的就是 float32，本质上是公式里有个可去奇点。

那些现象终于都对上了

回头再看当初那些想不通的地方，其实都落在同一个根因上。
静止的时候几乎不触发，是因为实际位形总会稍微偏离 phi1+phi4=π 这条曲线——姿态有点小偏差，两条腿的 L0 也不一样——编码器那点抖动很难正好打中那 332 个精确零点。
运动起来大概率触发，是因为晃动的时候 phi1+phi4 反复扫过 π，每次穿越都在曲线附近撒下一批采样点，命中机会一下就大了。
纯轮速那条路不触发，是因为它根本不经过 VMC_Speed；而且那条路径没有状态，坏一帧自己就恢复了。
融合路径既会触发又不可恢复，是因为卡尔曼和积分器会把瞬时的 NaN 锁存成永久的 NaN。
FDCAN2 最容易触发，原因有三个：一是腿长目标给到 0.19，腿更贴近奇异曲线；二是 KF 的 Q 给的是 1.0，别的版本是 0.1，增益更猛、速度跟得更激进；三是腿部自由运动范围大，扫过奇异曲线的频率高。
至于电机疯转撞限位，那是 NaN 力矩的结果，不是原因。
再和参考版、仿真对一下就更清楚了。参考版 DM_Balance 和仿真里的速度解算用的都是数值差分——仿真是 Derivative 块，参考版直接是 (L0-last)/dt；只有我们这边不一样，用的是 MATLAB 符号工具箱生成的解析雅可比 VMC_Speed.c，里面带着 1/分母。结果就是：实机的问题版会出 NaN，实机的修复版（钳位，或者代数化简）不出，仿真和参考版都不出。VMC_Speed.c 是整个项目里唯一"仿真和参考版都没有"的东西，NaN 也只从它这儿来。

根因

把因果链从上到下捋一遍。
根子在 VMC_Speed 里那个 1/(XC-0.044)。C 点跑到髋正上方，也就是 phi1+phi4=π 的时候，分母在数学上就趋近于 0——解析雅可比在奇异位形本来就会发散，这里是个可去奇点。
本来 double 下这个分母是 6.5e-9，还算个非零数，可 float32 把它舍入成了 0.0，于是真的执行了一次 1/0。
1/0 得到 Inf，Inf 再参与后面的运算，Inf×0、Inf−Inf，结果就是 NaN。
最后一步是放大：卡尔曼滤波是有状态的，NaN 进去一次，xhat 和 P 就永久是 NaN，再也回不来。一个瞬时的 NaN 就这样被它变成了永久故障。

修复和验证

现在上机的是钳位（方案 A），代码在 D:\Create\Legbot\程序\fdcan3\FDCAN2，commit 1cfa830。对 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 没再出现。到这儿问题算是闭环了。
   另外还留了几个兜底的错误处理，治不了病，但能挡住坏数据：
6. KF 入口哨兵：isnan(acc)||isnan(vel) 就跳过这一帧，出口是 NaN 就重新 Init；
7. Motor_MIT 编码前 isnan(torq)→0，这是发给电机前的最后一道闸；
8. Mahony 的加速度归一化，normal < 1e-6f 就跳过加计修正。
   其实还有两个备选，比钳位更好，只是当时没上。
   方案 B 是代数化简，把 1/t28 这个可去奇点直接消掉，NaN 在数学上不可能产生，也没有精度损失。数值等价已经验过，随时可以替换方案 A。化简是这样：

// 原式: 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 块、参考版的做法完全一致，结构上就没有分母。代价是会引入量化噪声，不过卡尔曼压得住。

---

总结

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