轮足机器人-IMU部分

陀螺仪测量原理与误差

机器人保持平衡，需要知道机身现在倾斜多少，也需要知道它正在以多快的速度倾斜。陀螺仪直接提供角速度；姿态解算把这些角速度和其他传感器的信息组合起来，才得到倾角。

陀螺仪究竟测到了什么

常见振动式 MEMS 陀螺仪让微小结构持续振动。结构发生转动时，科里奥利效应产生与角速度相关的响应，传感器把这个响应转成数字量。对应用程序来说，三轴读数是绕传感器 x、y、z 轴的角速度，不是三个欧拉角的变化率。

可以先把每根轴理解为一根穿过芯片的转轴。绕哪根轴转，那根轴对应的读数就显著变化；正负由右手定则决定。将手掌弯曲的方向作为正转方向，大拇指指向转轴的正方向。

一维、固定转轴的情况下：

若角速度恒定为 90°/s，持续 2 秒，就转了 180°。数字系统用离散求和近似积分：

这个公式适合解释单轴运动。三维运动中，转动顺序会影响最终姿态，因此不能把三个陀螺仪读数分别积分后，直接认作 roll、pitch、yaw。第三篇会给出正确的四元数更新方法。

原始整数怎样变成角速度

传感器寄存器提供的是整数，必须配合量程和灵敏度换算。例如 BMI088 在 ±2000°/s 量程下，厂家给出的灵敏度为 16.384 LSB/(°/s)。因此：

一个 raw = 16384 的示例读数对应 1000°/s，约 17.4533 rad/s。量程改变后，换算系数也必须改变。参考 $Bosch BMI088 官方技术资料$ (https://www.bosch-sensortec.com/en/products/motion-sensors/imus/bmi088)。

当前工程的 bmi088_driver.h 定义 BMI088_GYRO_2000_SEN 约为 0.0010652644，驱动把原始整数乘以它，输出已经是 rad/s。Mahony_update 内部那几行"度转弧度"代码目前处于注释状态，与该输入单位相配。再次乘 π/180 会把角速度缩小约 57.3 倍。

静止时为什么不等于零

一个实用的测量模型是：

其中 S 概括比例因子和轴间误差，b 是随温度和时间变化的零偏，n 是随机噪声。把真实角速度设为零，输出仍然可能带有平均偏移和随机波动。

零偏与噪声造成的现象不同。假设某轴存在恒定零偏 0.2°/s，纯积分 60 秒后就会出现约 12° 的角度误差。零均值随机噪声则会让每次实验产生不同的游走轨迹。两者都不能靠"多积分一会儿"自动消除。 $ADI 对陀螺仪漂移的解释$ (https://www.analog.com/en/resources/analog-dialogue/raqs/raq-issue-139.html)说明了这些误差来源。

应区分几个容易混淆的词：

对于近似白噪声，角速度标准差可按 σ_ω ≈ N_ω√B_eq 估算，B_eq 是等效噪声带宽，通常不能直接用采样率或截止频率代替。滤波越窄，输出通常越平滑，同时会带来动态延迟。相关概念见 $ADI 低噪声反馈控制文章$ (https://www.analog.com/en/resources/analog-dialogue/articles/low-noise-feedback-control.html)。

怎样做第一版零偏标定

先把机器人固定，确保机身没有转动，再记录一段连续样本。每个轴分别求平均：

例如可以先用 1 kHz 的 5000 个样本做 5 秒实验；这只是学习用起点。采样是否独立、机械振动、滤波和温度变化都会影响结果，不能仅凭样本数决定标定质量。

同时计算标准差，并检查标定期间是否有明显转动。只看加速度模长接近 g 并不足以证明静止：设备可能正在绕竖直轴匀速转动。可以综合陀螺仪幅值、方差、轮速、支撑状态和操作者已固定设备的条件。

温度稳定后标定更容易重复。当前 IMU_Task 的加热目标是 40°C，但温控不等于零偏已经被校准。应记录从冷启动到热稳定期间的温度和角速度均值，再判断是否需要 b(T) 补偿。

加速度计为什么也出现在姿态算法里

加速度计测量比力：

本教程世界系向上，因此 g^W = $0, 0, -g$ 。静止时 a^W = 0，水平机体系读数约为 $0, 0, +g$ ；自由落体时 a^W ≈ g^W，理想读数接近零。

这说明加速度计并不是直接输出"运动加速度"的仪器。静止时，它的方向能提供倾斜参考；机器人加速、刹车或落地时，运动产生的比力又会混入这个方向。

例如水平机身向前以 2 m/s² 加速，理想比力为 $2, 0, 9.81$ 。若误当作纯静止参考，会得到约 atan(2/9.81) = 11.5° 的假倾角。与此同时，它的模长仅比 g 大约 2.1%。因此"模长接近 g"并不能可靠排除加速干扰。

练习

恒定零偏为 0.05°/s，积分 120 秒会漂多少？
读数 180°/s，怎样换成 rad/s？
同一传感器从 ±2000°/s 改为 ±500°/s 后，旧系数还能用吗？
自由落体时，能否把加速度方向归一化后用来纠正姿态？

答案依次为：

约 6°；

π rad/s；

不能，应匹配新灵敏度；

不能，接近零的向量没有可靠方向，且除以其长度会产生数值问题。

坐标系与四元数

四元数值得学习，是因为它让连续的三维旋转可以稳定地表示和组合。先理解"坐标表达如何变化"，再记公式，通常比从 i、j、k 的代数规则开始更容易进入工程应用。

先确定向量在哪个坐标系

机体系 B 随机器人转动，世界系 W 固定。一个物理向量可以在两套坐标系中有不同的三个分量。例如机器人转向后，机身的"前方"始终是机体系 x 方向，但它在世界系中的方向改变了。

本教程定义 R_WB 和 q_WB 都把 B 系分量变为 W 系分量：

q 的存储顺序为 $w, x, y, z$ ，采用 Hamilton 乘法。公式、代码和外部库都要同时对齐"存储顺序、乘法定义、变换方向、角速度表达坐标系"这四件事。四元数惯例存在多种合法选择， $Solà 论文的第 3 节$ (https://arxiv.org/html/1711.02508v1)专门讨论它们。

从绕轴旋转理解四元数

任意单次三维旋转都能用单位轴 u 和角度 θ 描述。对应单位四元数是：

所以 q0 通常是 cos(θ/2)，q1、q2、q3 共同编码旋转轴和角度；它们不是 roll、pitch、yaw 的另一种排列。θ 用弧度。

例如绕 +z 转 90°：

没有旋转时 q = $1, 0, 0, 0$ 。绕 +x 转 180° 时 q = $0, 1, 0, 0$ 。

单位四元数满足 w²+x²+y²+z² = 1。四个数带着一个长度约束，因此仍只表达三个独立的旋转自由度。q 与 -q 表示相同姿态；保存日志、插值和计算姿态差时应记住这一点。

四元数乘法怎样写

把 p 写成 $p_w, p_v$ ，q 写成 $q_w, q_v$ ，Hamilton 乘法是：

点积负责标量部分，叉积是乘法通常不能交换的来源。其 C 形式为：

cpp 复制代码

/* Both inputs are copied to locals so the result may alias an input. */
void quat_mul(const float p[4], const float q[4], float out[4])
{
    const float a=p[0], b=p[1], c=p[2], d=p[3];
    const float e=q[0], f=q[1], g=q[2], h=q[3];
    out[0] = a*e - b*f - c*g - d*h;
    out[1] = a*f + b*e + c*h - d*g;
    out[2] = a*g - b*h + c*e + d*f;
    out[3] = a*h + b*g - c*f + d*e;
}

这段代码只实现代数乘法，不负责输入有效性检查和归一化。

单位四元数的共轭 q* = $w, -x, -y, -z$ 同时也是逆。一般非单位四元数的逆还要除以模长平方，不能直接用共轭代替。

怎样用四元数旋转向量

把三维向量 v 写成纯四元数 $0, v$ ，则：

拿上面的 z 轴 90° 旋转做检查，应得到：

如果结果变为 $0,-1,0$ ，优先检查是否反用了共轭、变换方向或转角符号。

对单位 q = $w, u$ ，还可以用等价公式减少运算：

需要旋转大量向量时，可以先构造旋转矩阵，再复用它。只有一两个向量时，直接计算未必更慢；实际取舍应在目标处理器上测量。

乘法的左右顺序意味着什么

假设当前 q_WB 已知，机体系下的小旋转增量为 δq_B，则：

如果增量是在世界系表达的 δq_W，则：

两条式子描述的是同一个原则：增量在哪个坐标系表达，决定它作用在哪一侧。对于固定坐标系的主动旋转组合，q_y⊗q_x 表示先应用 q_x，再应用 q_y。

例如对 v = $0,0,1$ ：

先绕固定 x 转 90°，再绕固定 y 转 90°，最终是 $0,-1,0$ 。
先绕固定 y 转 90°，再绕固定 x 转 90°，最终是 $1,0,0$ 。

这两个结果不同，所以把乘法换序会改变姿态，不能只靠"编译通过"验证算法。

欧拉角放在哪一层使用

在本教程的 ZYX 约定下：

φ、θ、ψ 分别为 roll、pitch、yaw。对单位四元数可计算：

四元数状态避免了欧拉角在 pitch = ±90° 时的参数化奇异性，但转回欧拉角后，roll 和 yaw 仍会出现不唯一或跳变。这个转换适合显示与规定工作区间内的控制接口，不能把四元数内部状态也当作三个独立角度更新。

控制器若只工作在直立附近，可以使用约定明确的 pitch。姿态范围扩大后，应重新考虑用机身方向向量或姿态误差表达控制目标。

练习

写出绕 +y 转 60° 的四元数。
q 和 -q 直接逐分量相减，是否能反映真实姿态误差？
单位四元数表达不变，把它四个分量都乘 2 后，能否直接继续当作单位旋转使用？

答案：

约 $0.8660254, 0, 0.5, 0$ ；

不能，二者姿态相同；

不能，应先归一化。比较两个单位姿态可用 2 acos(clamp(|q_a·q_b|,0,1)) 得到最小转角；

很小误差时可采用更稳定的 atan2 形式。

由角速度计算四元数

把陀螺仪接到单片机之后，真正连接传感器和姿态状态的是积分器。这一篇从很短时间内的转角出发，推导工程里常见的四元数更新代码，再讨论它什么时候准确、什么时候需要换方法。

沿用前篇约定：q = q_WB，标量在前，机体系角速度 ω_B 的单位为 rad/s，时间步长 Δt 的单位为秒。

从一个很小的旋转开始

在足够短的时间内，先把角速度看作常量：

旋转增量的四元数为：

α = 0 时，后一个系数使用极限 1/2。因为角速度在机体系表达：

在"区间内角速度恒定"这个假设下，上式给出精确的有限旋转更新。它不代表真实连续角速度已经被完全重建：样本之间快速变化的信息仍然可能丢失。

为什么公式里会有二分之一

当 α 很小时，cos(α/2) ≈ 1，sin(α/2)/α ≈ 1/2，于是：

代入乘法：

取极限得到：

因此 1/2 来自四元数的半角关系，不是用来平均前后两次采样的系数。

设 q = $w,x,y,z$ ，ω = $ω_x,ω_y,ω_z$ ，展开导数：

这就是当前 mahony_ahrs.c 在"Integrate rate of change of quaternion"下面四条加法的来源。更系统的连续与离散旋转积分讨论见 $Solà 论文第 4 节$ (https://arxiv.org/html/1711.02508v1)。

为什么必须保存旧分量

前向欧拉更新需要同一时刻的 w、x、y、z：

cpp 复制代码

float ow=q[0], ox=q[1], oy=q[2], oz=q[3];
float h=0.5f*dt;

q[0] = ow + (-ox*gx - oy*gy - oz*gz)*h;
q[1] = ox + ( ow*gx + oy*gz - oz*gy)*h;
q[2] = oy + ( ow*gy - ox*gz + oz*gx)*h;
q[3] = oz + ( ow*gz + ox*gy - oy*gx)*h;

如果先更新 q $0$ ，后面又拿更新后的 q $0$ 算 q $1$ ，就不再是推导中的前向欧拉方法。误差可能随姿态和角速度变化，表现得很像传感器本身不稳定。

当前源码用 qa、qb、qc 保存旧 q0、q1、q2；旧 q3 在最后一条赋值之前仍未改变，所以这种写法也成立。

为什么要归一化

前向欧拉更新一般不保持四元数长度。设旧 q 的长度为 1，单步转角模长为 α，则这一阶更新的长度平方为：

因此通常每次更新后计算：

归一化修复了长度约束，但不能补回积分方向和转角的截断误差，更不能纠正传感器零偏。输入 dt 错误时，每拍都归一化也会得到错误姿态。

计算前先确认模长平方有限且大于合理下限。对零四元数做归一化没有意义。若状态损坏，应报告故障并重新进行有效的姿态初始化；在控制仍运行时直接换成 $1,0,0,0$ ，可能造成姿态突跳。

怎样判断欧拉积分够不够

关键量是单步转角 α = |ω|Δt，而不只是采样率。

归一化之后，一次欧拉更新对应的实际转角为：

泰勒展开得 α_E ≈ α - α³/12。举两个同为 1 ms 周期的手算例子：

这只能估算恒定转轴下的积分截断误差。传感器零偏、样本时延和振动可能远大于它，所以没有必要一开始就用复杂积分器掩盖时间或单位错误。

恒定角速度运动时，可以把小误差跨步累计估算；变化转轴运动则不能简单按标量角度相加。

有限旋转更新的实现要点

若需要更准确的单步更新，可以计算 δq 后调用第二篇的 quat_mul。小角度时用级数避免 0/0：

cpp 复制代码

/* Precondition: q is valid and normalized, gyro is finite, dt > 0. */
float dx = gx*dt, dy = gy*dt, dz = gz*dt;
float a2 = dx*dx + dy*dy + dz*dz;
float c, s;

if (a2 < 1e-6f) {        /* illustrative threshold, angle in radians */
    c = 1.0f - a2/8.0f + a2*a2/384.0f;
    s = 0.5f - a2/48.0f + a2*a2/3840.0f;
} else {
    float a = sqrtf(a2);
    c = cosf(0.5f*a);
    s = sinf(0.5f*a)/a;
}
float dq[4] = {c, s*dx, s*dy, s*dz};
quat_mul(q, dq, q);

这是教学用核心片段。部署时还要检查 dt、输入与中间结果的有限性，并考虑浮点误差带来的归一化需求。

三角函数可能增加计算量，应在目标 MCU 上测量。中点法、使用区间平均角速度和更高阶方法是其他选择，但前提都是样本和时间含义明确。转轴在样本间迅速改变时，还涉及旋转不交换造成的圆锥误差；这应在基础积分和采样链路验证后学习。

时间步长怎样进入算法

源码 Mahony_Init(1000.0f) 把 invSampleFreq 设为 0.001 秒。只有每次更新确实代表约 1 ms 的新角速度数据时，这个设定才匹配。

例如真实每 1.1 ms 更新一次，却一直使用 1 ms，恒定转动的积分角度会约少 9.1%。任务名义周期、传感器输出周期和样本时间戳是不同概念。

使用实际 dt 也不是万能补偿。如果任务延误 20 ms，只读到最后一个瞬时样本，就无法从一个样本重建这 20 ms 内发生过的全部转动。FIFO、数据就绪中断和逐样本时间戳有助于保留缺失的信息。

练习

初始 q = $1,0,0,0$ ，gz = π/2 rad/s，持续 1 秒。用 1000 次 dt = 0.001 秒更新：

期望的最终四元数是什么？
交换乘法左右顺序，在这个单轴例子里一定能发现错误吗？
如何设计更容易发现乘法顺序错误的例子？

答案：

约 $0.70710678,0,0,0.70710678$ ；

不一定，初始单位姿态和单轴运动可能掩盖顺序问题；

从已有 x 轴旋转的姿态开始，再加入机体系 z 轴旋转，并检查机身方向。

Mahony姿态融合与调参

陀螺仪的短时间动态响应很好，但积分会漂；加速度计在近似静止时能提供倾斜参考，但机器人一运动就可能受到干扰。Mahony 的核心是用方向误差反馈修正角速度，再用修正后的角速度更新四元数。

先用一维互补滤波建立直觉

在直立附近、近似只绕一个固定轴转动时，可以先写：

陀螺仪负责跟踪快速变化，加速度角把长期结果拉向倾斜参考。若把连续一阶纠偏用后向欧拉近似，可选 α = τ/(τ+Δt)，这里 τ 是纠偏时间常数。不同离散方式也可得到不同 α 公式，所以不要把某个固定系数搬到任意采样率。

例如 τ = 0.5 s，Δt = 0.001 s，α 约为 0.998。这个数大并不表示加速度计无关紧要；小修正会持续积累。这一标量公式只是直观入口，大角度三维运动需要在旋转空间中处理。

四元数如何预测静止参考方向

在世界系中，静止比力参考方向是 e_z = $0,0,1$ 。当前姿态估计为 q 时，把它转换到机体系;

若加速度计的比力测量 f 可信且非零，归一化得 v = f/|f|。于是有两个方向：传感器测到的 v，以及四元数预测的 v_hat。它们的差异就是纠偏来源。

当前代码 halfvx、halfvy、halfvz 等于上述预测向量的一半。halfvz 使用 w² - 0.5 + z²，与 0.5 - x² - y² 在单位四元数条件下相等。

为什么使用叉积

定义误差：

它的方向给出需要修正的轴，长度是两向量夹角的正弦。小误差时 sin(δθ) ≈ δθ，反馈就近似与角度误差成正比。

检查符号可以用一个具体场景：真实机身水平，所以 v = $0,0,1$ ；估计姿态却绕 x 正向偏了一个小角 φ，此时 v_hat ≈ $0,φ,1$ 。叉积得到 e ≈ $-φ,0,0$ ，把它加到角速度上会让估计角减小，方向正确。

反过来计算 v_hat × v 会改变符号。若其他公式不变，就会把纠偏变成发散。不能只凭代码注释里的"estimated"和"measured"决定顺序，应结合整个约定检查。

Mahony 经典互补观测器用方向观测构造纠正量，并可估计陀螺仪零偏。原始算法资料和旧版 C 实现入口见 $x-io 作者资料页$ (https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/)。

比例反馈与零偏积分

一种统一的写法是：

然后将 ω_c 送入第三篇的四元数积分器。比例反馈让倾斜误差收敛；偏置积分吸收持续存在且可由观测识别的陀螺仪偏差。

有些代码不显式保存 b_hat，而是保存积分反馈 I = -b_hat，于是 I_dot = k_I e，ω_c = ω_m + I + k_P e。两种记法符号看似不同，含义一致。

当前源码对应关系如下：

当前 twoKpDef = 2×0.5，twoKiDef = 2×0，所以默认 k_P = 0.5，积分纠偏关闭。不要看到 twoKp 的数值为 1 就把有效 k_P 当作 1，也不要把 halfex 再人为乘错一个 2。

六轴为什么不能稳定绝对航向

只观测一条竖直参考向量时，绕这条向量旋转不会改变观测。两种不同 yaw 可能产生完全相同的静止加速度读数，所以仅靠加速度计无法区分它们。

由此可推导：理想六轴 IMU 的重力观测不能提供绝对 yaw 参考。在常见静止水平工况下，竖直轴零偏也无法通过该方向纠偏识别。真实运动可改变部分偏置分量的可观测性，但不会凭空产生绝对航向参考。

本工程调用 Mahony_update 时传入零磁力计参数，走 MahonyAHRSupdateIMU。因此 yaw 有初始相对意义，时间长了会漂。若需要长期航向约束，可研究磁力计、视觉、外部定位或轮式运动约束；轮编码器也有打滑和累计误差，不能自动变成绝对北向。

加速时为什么会估计出假倾角

加速度计测到的是 f = R^T(a-g)。当 a 明显不为零，归一化 f 得到的方向已经不是静止参考方向。此时 k_P 越大，算法越容易把运动比力解释为机身倾斜。

第一篇的 2 m/s² 水平加速例子尤其适合轮腿机器人：它可以制造约 11.5° 的假倾角，但模长仅变化约 2.1%。因此仅用 |f| 是否接近 g 作门控，会漏过一些明显干扰。

学习时可先给反馈乘置信权重：

例如用模长偏差 r = ||f|-g| 构造 λ_norm = clamp(1-r/δ_a,0,1)。这里 δ_a 的单位为 m/s²，是需要通过日志选择的阈值。这只是一个教学示例，应再结合方向残差、轮速加速度、接触状态和振动指标判断。

方向误差大也可能是初始姿态错误，因此不能简单把大残差永远拒绝。需要初始化、拒绝超时与恢复机制。作者维护的后续 $Fusion 实现$ (https://github.com/xioTechnologies/Fusion)提供加速度干扰拒绝和恢复设计，可用于学习；这些机制并未自动包含在本工程当前 Mahony 代码中。

加速度不可信时，偏置积分也应暂停或适当减弱，并设置限幅。否则算法可能把一段持续加速当成"陀螺仪零偏"，等机器人停止后仍带着错误补偿。

怎样开始调参

先验证轴映射、单位、时间步长与静止初始化。它们错了，调增益通常只会掩盖现象。
令积分项为零，用静止倾斜与缓慢回正观察比例反馈。记录收敛时间，同时检查是否有振荡。
对小误差、参考稳定的一轴模型，误差近似满足 ε_dot = -k_P ε，时间常数约为 1/k_P。这只是估算，实际实现还受滤波和采样影响。
加入加速、刹车与落地日志，观察估计 pitch 与陀螺仪积分是否被加速度反馈拉偏。
完成静止零偏标定之后，再研究小积分项、可信度门控和积分限幅。

用同一段离线日志比较参数，可以排除每次手动动作不同造成的错觉。至少同时观察姿态、原始角速度、加速度模长、方向误差、温度和 dt。

练习

零偏没有变化，单纯增大 k_P，能保证加速时的姿态更准确吗？
halfex = e_x/2，twoKp = 2×0.5 时，加到 gx 的纠偏是多少？
算法估计已有 5° 的 roll 误差，k_P = 0.5，理想小误差模型经过 2 秒后约剩多少？

答案：

不能，可能更快跟随假倾角；

0.5e_x；

约 5/e = 1.84°。最后一个计算假设参考稳定、没有其他噪声和运动干扰。

嵌入式优化

姿态算法部署到机器人以后，误差常常来自数据链路、时间和数值边界。优化目标应同时包含姿态误差、延迟、任务最坏执行时间和异常恢复行为。只看输出曲线是否光滑，或者某个数学函数是否少用了几次乘法，都不足以判断改善。

先画出完整的数据链路

从左到右逐层验证，比较容易定位问题。若角速度换算已经错误，后面采用更复杂的滤波器也不能恢复物理量。

任务运行一次不等于传感器更新一次

当前工程有三项彼此不同的配置：

这些是源码设定，不是硬件实测采样统计。加速度计与陀螺仪各自更新，而且串行读取也可能使时间不同；任务轮询还可能重复拿到旧值。

应区分四个时间：传感器采样时间、数据可读时间、MCU 开始读取时间、控制器使用时间。当前 ImuSample 的 tick 在计算后填写，更接近软件发布时刻，不能直接视作传感器的真实采样时刻。

改进可以从数据就绪信号、传感器同步机制、FIFO 与高分辨率时间戳入手。Bosch 在 $BMI088 官方页面$ (https://www.bosch-sensortec.com/en/products/motion-sensors/imus/bmi088)提供数据同步应用说明入口；具体模式、引脚连接与延迟应按相应说明核对。

若选择用真实 dt，需要同时定义丢样处理策略。对超过上限的 dt 不能静默改成正常周期再宣称结果连续；应标记数据质量下降，并由控制层决定是否继续使用。限幅 dt 只能作为明确的异常策略，不能补回缺失运动。

轴映射要对加速度和角速度保持一致

传感器板安装方向通常与机身轴不完全相同。应定义固定旋转 R_BS，把传感器系 S 的角速度和比力都变换到机体系：

若通过交换坐标和改正负实现，先保证这组映射仍是右手旋转，矩阵行列式应为 +1。只反一根轴可能构成反射，不能直接混入原来的四元数旋转公式。

先验证手动绕各机身轴正转时的陀螺仪符号，再检查静止倾斜时的比力分量。轮腿控制里还可能定义"前倾为正"等应用符号，应在传感器坐标转换之后单独说明。

温控与零偏估计怎样配合

当前代码把 RefTemp 设为 40°C，并每 10 次 IMU 循环执行一次温控计算。稳定温度能改善误差重复性，但仍要测量目标温度下的零偏。加热启动阶段的零偏可能不同，应把温度曲线与姿态漂移一起记录。

第一版可在热稳定、确认静止的窗口估计每轴均值。进一步可以在多个稳定温度点拟合：

先检查一次模型是否足够，再增加二次项。拟合的温度范围、升降温差异和传感器个体都影响效果，不能从一个模块直接复制到另一块板。

如果同时使用离线温度补偿、开机静止标定和 Mahony 积分反馈，应明确三者各自补偿的残差，避免把同一偏置重复扣除。

先处理数值边界再做近似运算

对每组输入至少检查：数值有限、时间有效、量程是否饱和、样本是否过期。饱和数据可能完全有限，因此 isfinite 不能代替量程检查。

加速度归一化前先检查平方模长：

cpp 复制代码

float n2 = ax*ax + ay*ay + az*az;
if (isfinite(n2) && n2 > accel_norm2_min) {
    float inv = 1.0f/sqrtf(n2);
    /* Normalize and evaluate confidence before applying feedback. */
} else {
    /* Skip accelerometer feedback and publish reduced data quality. */
}

accel_norm2_min 的单位是 (m/s²)²，应根据传感器噪声、自由落体判据与工作范围确定。并不是任何非零向量都值得用于姿态纠偏。

欧拉角转换的 asin 输入也应在确认有限之后限制到 $-1,1$ 。四元数模长即使只偏离单位值一点，也可能使理论上等于 1 的表达式浮点计算后略大于 1。

当前 Mahony_computeAngles 直接调用 asinf，未在该处钳位；当前任务会检查最终角度是否有限。学习改进时，应把"避免无意义的 NaN"和"发现已经损坏的状态"分开处理。

快速倒平方根是否值得用

旧版常见算法使用位操作给 1/√x 生成初值，再进行牛顿迭代：

当前源码里同时存在两条路径：常规更新所用的 invSqrt 调用 __sqrtf；初始化末尾则调用使用 0x5f3759df 的 Mahony_invSqrt。因此不能只看到了"魔数"就认定整个姿态任务都在使用它。

位强转实现中的 long 位宽、IEEE 754 表示以及 C 严格别名规则都涉及可移植性问题。即便在当前编译器上可用，搬到其他平台前也应重新验证。需要位转换时可用 uint32_t 和 memcpy 表达意图，同时验证数据表示与字节数。

当前 Keil 工程配置为 Cortex M7 并启用 FPU。硬件平方根、除法、函数调用和编译优化的综合成本应实测；老式近似算法未必更快。Arm 也提供 $CMSIS DSP 四元数归一化接口$ (https://arm-software.github.io/CMSIS-DSP/main/group__QuatNormalized.html)，使用前仍需核对本地库版本和边界行为。

快速 atan2 的误差怎样影响控制

当前 mahony_ahrs.c 定义了名为 arm_atan2_f32 的本地近似函数，核心多项式是：

它不是从函数名就能认定的官方库实现。用它替换 atan2f 之前，应扫描整个工作范围，记录最大角度误差、误差分布、执行时间，以及接近零输入时的行为。

本地脚本 quaternion_lab.py 扫描 0 到 1 的基础区间，按 0.00001 间隔与标准 atan 比较，最大角度误差约为 0.285302°，出现在 r 约为 0.78138 处；象限映射在理想有限输入下保持该误差尺度，板端还应检查单精度舍入与极小值。若想完整替换，也要覆盖坐标轴、四个象限和跨 ±π 的连续性。

当前近似用于 roll 和 yaw；pitch 使用 asinf，因此这项近似误差不能直接归因到 pitch。误差若进入姿态反馈，其影响大致为控制增益乘以角误差，具体是否重要取决于系统指标。

若控制器确实需要某个角度保持 1 kHz，就保留那个角度的更新，只把纯显示需要的量降频。不能假定"欧拉角都只用于显示"，然后把控制输入也降到低频。

滤波要把相位延迟算进去

以一阶连续低通为例：

若 f_c = 20 Hz，在 10 Hz 处就有约 -26.6° 相移。纯延迟 T 的相位为 -360fT 度；例如 5 ms 在 20 Hz 处产生 -36°。这些数字是说明影响的算例，不是当前机器人的实测带宽。

因此抑制振动时，应同时观察控制相位裕度和采样总延迟。截止频率压得过低，虽然曲线平滑，却可能让控制器总在纠正过去的状态。 $ADI 的控制反馈文章$ (https://www.analog.com/en/resources/analog-dialogue/articles/low-noise-feedback-control.html)讨论了滤波噪声与相位延迟之间的联系。

数字滤波发生在采样之后，已经混叠到低频的振动通常无法再靠普通低通区分。应先考虑安装刚度、共振、传感器内部滤波和采样率，再决定数字低通或陷波器。

当前源码中适合继续研究的细节

MahonyAHRSinit 使用 init_pitch = atan2f(-ax, az)。在本教程 ZYX 约定及静止条件下，通常应使用 atan2f(-ax, sqrtf(ay*ay+az*az)) 才能在同时存在 roll 时正确恢复常用范围内的 pitch。当前写法在 roll 接近零时接近正确，但组合倾斜会产生初值误差。

例如真实 roll = 30°、pitch = 20° 时，归一化静止比力为 $-sin20°, sin30°cos20°, cos30°cos20°$ 。代入当前 pitch 初始化式得到约 22.80°。可以先用这个合成例子研究公式，再用实物姿态和实际安装坐标验证。

另一个接口细节是 anglesComputed。当前六轴更新函数末尾没有像九轴通用路径那样清除该标志；但当前 IMU_Task 每拍都显式调用 Mahony_computeAngles，因此这条调用链会刷新角度。若其他调用者只执行六轴更新再调用 getter，就要重新核对缓存是否过期。

这些是对根目录源码的学习观察。实际优化应以所构建目标和板端日志为准。

怎样测量一次优化是否有效

把计算成本拆成 SPI 读取、方向反馈、四元数更新、归一化、角度转换和发布几个阶段。支持时可用 DWT 周期计数器或高分辨率定时器；注意测量开销、中断插入、计数器回绕和 CPU 主频。

记录平均值、分位数和最大执行时间，并区分计算耗时与端到端样本年龄。任务使用 1 ms 周期时，不能只证明平均计算时间低于 1 ms，还要观察最坏情况与调度抖动。

比较优化前后时使用同一组输入，至少包含：

先追求正确、可解释、可复现，再根据实测热点优化。对轮腿机器人，采样同步和误差反馈质量的收益往往值得优先评估；是否比更换数学函数更重要，应由同一批日志的比较决定。

练习

为什么只检查姿态函数平均耗时不够？
将 1 kHz 输出做成非常平滑的曲线，能否证明控制更稳定？
某次任务晚到 10 ms，使用真实 dt 之后，丢失的运动信息是否全部恢复？
为什么快速 atan2 的误差不能直接解释本工程的 pitch 偏差？

答案：

调度还受最大耗时、中断和数据年龄影响；

不能，滤波可能增加关键相位延迟；

不能，缺失区间只有一个样本时无法重建全部运动；

该代码的 pitch 路径使用 asinf。