简介围绕MPU9250获取姿态航向角的STM32与MATLAB实现面向嵌入式开发、传感器融合和姿态解算学习者借助正点原子阿波罗F429开发板完成传感器数据读取、基于最小二乘的椭球矫正、滤波处理与四元数Madgwick融合最终解算航向角并可将四元数转换为横滚角与俯仰角适用于课程设计、毕业设计以及云台、飞控等姿态感知原型的算法验证。压缩包约18.73MB文件总数与类型明细暂未提供从标题可知包含STM32工程源码和MATLAB脚本两类前者用于板端采集与解算后者便于算法对照和参数观察。已有4923人学习下载。其价值在于将矫正、滤波、融合到欧拉角输出的链路整理为可复现样例读者可据此理解椭球矫正参数、Madgwick四元数更新和航向角求取过程描述中还提示均值滤波部分存在需自行修正之处为调试排错和二次开发保留了空间。1. 航向角为什么总在漂从九轴数据到姿态解算的认知起点MPU9250 接上 STM32读出九个寄存器跑一遍姿态解算大多数人第一眼看到的现象是航向角在以每分钟几十度的速度匀速旋转。直觉会把问题归到融合算法上但真正的主要原因常常在更前面——磁力计的原始数据落在一个歪斜的椭球面上算法却把它当成标准球在算角度。压在这条链路最前面的关键环节是椭球矫正后面的 Madgwick、四元数、欧拉角转换都要在标定做稳之后才有意义。这份 STM32 与 MATLAB 源码把标定和解算拆成两段分别验证正好让每一步都能落到可核对的数字上。至于为什么要这么拆得从磁力计为什么会歪说起。2. 磁力计椭球矫正为什么硬铁和软铁误差必须用最小二乘消掉2.1 硬铁偏移与软铁形变误差的物理来源在地磁总场强度恒定的环境里不同地区大致在 25 到 65 微特之间一台理想的三轴磁力计在空间里任意旋转输出点会落在一个以原点为球心、半径等于当地地磁总场强度的球面上。这句话是整套标定方法的公理也是判断标定是否成功的唯一标准校正后的所有采样点模长必须接近一个常数。现实中的板子上焊在磁力计旁边的电源电感、扬声器磁钢、甚至拧过螺丝的改锥残留磁化都会叠加一个固定磁场。这个叠加量与姿态无关相当于把整个球心从原点搬走一段距离这就是硬铁偏移。它的量级可以轻松达到几微特到几十微特和地磁总场处于同一个数量级——换句话说误差可以比信号还大。更麻烦的是这个偏移会随温度漂冷机刚上电和跑十分钟之后可能差出一截。软铁误差的来源不一样。高磁导率材料不会自己产生磁场但它会把周围的地磁重新引导让不同方向的磁力线在不同轴上被不同程度地汇聚或削弱。结果是原本的球被拉伸、压扁、旋转变成一个三轴不等、主轴还和传感器坐标轴不对齐的椭球。硬铁只改球心软铁改形状两者叠加后测量点到原点的距离随姿态变化直接喂给解算就会得到一条忽快忽慢的航向角曲线——转得慢的时候误差大转得快的时候误差小这种“角度误差和转速相关”的现象基本可以确定是磁力计没标定。用公式描述真实地磁矢量 m 到测量值 m_raw 之间是一个仿射变换m_raw S·m b。S 是 3×3 的软铁加刻度因子矩阵b 是三维硬铁偏移通常还带一个传感器本身的零偏。把 m S⁻¹(m_raw − b) 代回 |m|² B²B 为当地总场强度得到 (m_raw − b)ᵀ A (m_raw − b) 1其中 A S⁻ᵀS⁻¹/B² 是对称正定矩阵。这个式子的几何意义就是椭球面。标定的任务因此从“求三个偏移”变成了“求 9 个参数”A 的 6 个独立分量加 b 的 3 个分量。2.2 椭球最小二乘把 9 个参数从散点里解出来上一步的式子可以按一般二次曲面展开成 a11x² a22y² a33z² 2a12xy 2a13xz 2a23yz 2d1x 2d2y 2d3z 1。把常数项归一化到右边之后每个采样点贡献一行线性方程未知数正好 9 个——问题从非线性几何拟合降维成了一次线性最小二乘。这是椭球标定被广泛采用的真正原因不需要迭代初值不需要优化器一次矩阵求解就能出结果MATLAB 里一行左除就完事。代价是它对数据的覆盖度极其敏感。如果采样时只在一个平面内转了两圈z 轴方向的二次项几乎没被激励设计矩阵会接近奇异解出来的椭球退化成抛物面或者一根针此时任何求逆都会把噪声放大成天文数字。常见的处理是在求解前检查设计矩阵的条件数超过阈值直接拒绝这次拟合、提示重采样而不是硬着头皮用下去。还有一处容易被忽略的细节右端常数固定为 1意味着拟合结果对数据的整体尺度敏感。原始数据单位是微特还是 LSB会直接改变参数的数值范围进而影响求逆的数值稳定性。我一般会在拟合前把数据减去均值、除以标准差做一次归一化拟合完再把缩放还原回物理单位——这一步多做十行代码能省掉后面一堆“为什么拟合出来的中心点离谱”的排查时间。% raw: N x 3 的原始磁力计采样每行一个姿态下的 [mx my mz] % 返回 center(3x1 硬铁偏移) 与 W(3x3 软铁矫正矩阵) function [W, center] ellipsoid_fit(raw) N size(raw, 1); if N 30 error(采样点太少椭球拟合会严重欠定); end % 归一化改善条件数避免 LSB 与 uT 量级差异导致的病态 mu mean(raw, 1); sd std(raw, 0, 1); p (raw - mu) ./ sd; x p(:,1); y p(:,2); z p(:,3); % 设计矩阵一般二次曲面常数项移到右侧为 1 D [x.^2, y.^2, z.^2, 2*x.*y, 2*x.*z, 2*y.*z, ... 2*x, 2*y, 2*z]; rhs ones(N, 1); % 条件数检查超过 1e8 说明采样姿态覆盖不足直接拒绝 if cond(D) 1e8 error(采样覆盖不足请让三个轴都转到避免只绕单轴旋转); end theta D \ rhs; % 9 个参数顺序与上面的 D 对应 % 组装对称矩阵 A 与一次项向量 d A [theta(1) theta(4) theta(5); theta(4) theta(2) theta(6); theta(5) theta(6) theta(3)]; d theta(7:9); % 椭球中心即硬铁偏移k 是把右端归一到 1 的缩放因子 c -A \ d; k 1 c * A * c; % A/k 特征分解后取平方根才是真正的矫正矩阵 [V, E] eig(A / k); if any(diag(E) 0) error(拟合结果不是椭球检查采样时附近是否有移动的铁磁物体); end Wn V * diag(sqrt(diag(E))) * V; % 把归一化空间的结果还原回原始单位 W Wn ./ sd; center (c .* sd) mu; end这段代码里最值得盯的是三个地方。cond(D) 1e8这道闸门看着粗暴但它拦住的是绝大多数“拟合完发现矫正后数据还是散的”的场面eig(A/k)这里必须先除 k 再开方直接对 A 开方会把尺度算错返回值的单位还原容易被漏掉Wn ./ sd是逐列相除写成矩阵乘法就全错了。参数上theta(1:3)是三个轴的二次项系数theta(4:6)是交叉项theta(7:9)对应一次项。交叉项的存在正是软铁主轴和传感器轴不对齐的数学体现——如果板子上没有明显的软铁材料这三个值会非常接近零拟合出来的椭球主轴基本和坐标轴平行此时 W 退化成对角阵也就解释了为什么有些简易标定只做三轴缩放也能凑合。2.3 从参数到矫正矩阵特征分解与球面验证得到 A 和 b 之后还不能直接用。校正的目标是找矩阵 W 和中心 c使得 W·(m_raw − c) 的模长恒等于 1。把 (m_raw − b)ᵀ A (m_raw − b) 1 里的 A 做特征分解 A R D Rᵀ定义 W D^{1/2} Rᵀ就有 |W(m_raw − b)|² 1。也就是说矫正矩阵是 A 的“平方根”不是 A 本身。这一步写错的人不少直接拿 A 去乘椭球的形状变了但尺度还是错的后面融合时磁力计和加速度计的权重配比会跟着全歪表现出来就是航向角收敛得特别慢或者被磁干扰拽着走。验证方法很直接对校正后的数据算每个点的模长看均值和标准差% cal: 矫正后的 N x 3 数据 cal (W * (raw - center)); norm sqrt(sum(cal.^2, 2)); fprintf(模长均值 %.3f标准差 %.3f离散度 %.2f%%\n, ... mean(norm), std(norm), 100*std(norm)/mean(norm)); % 经验阈值离散度 2% 可用 5% 基本等于没标定理想情况下离散度应该小于 2%。如果离散得厉害要么采样时周围有铁磁物体在移动要么拟合本身已经病态。另一个检查是看硬铁偏移center的模长——超过几百 LSB对应几十微特就要怀疑采样环境而不是相信这个结果。我见过最典型的一次是标定完后离散度 0.8%看起来完美但中心点偏移大到把地磁矢量整个翻了个方向最后发现是把板子放在金属桌面上采的样椭球拟合诚实地把桌子也一起拟合进去了。3. Madgwick 的梯度下降一个归一化步长为什么能压住航向漂移3.1 四元数积分与漂移的数学来源陀螺仪输出角速度 ω四元数微分方程是 q̇ 0.5 · q ⊗ [0, ω]。离散化后 q_{k1} q_k 0.5·Δt·q_k ⊗ [0, ω]再做归一化。这个式子的误差来源有三层零偏让静止输出不为零积分后角度线性增长刻度因子误差让转速越大误差越大一阶欧拉近似在角速度变化剧烈时引入截断误差。三层里前两层是主要问题第三层在 100 Hz 以上采样时基本可以忽略。关键在于误差的累积形态。零偏 0.5°/s一分钟后是 30°十分钟就是 300°——不修正的话航向角完全没有参考价值。而加速度计提供的是重力矢量方向它只能约束俯仰和横滚两个自由度绕重力轴的旋转在重力观测里完全不可见。这就是六轴融合加速度加陀螺的航向角一定漂的根本原因不是算法不行是物理上没有对应的观测量。磁力计补上了这唯一的缺失维度。它测的是地磁矢量在机体坐标系的投影绕重力轴转多少投影就跟着转多少于是航向角变得可观测。代价是磁力计对干扰极度敏感第 2 章那一整套椭球标定本质上就是把这个新引入的观测量修到勉强可用。3.2 目标函数把两个观测拼成一个残差向量Madgwick 的思路是把“姿态应该让重力方向对得上”和“姿态应该让地磁方向对得上”写成两个残差拼成一个 6 维列向量 f然后沿 f 的负梯度方向修正四元数。重力残差直接用四元数旋转矩阵的第三列机体系下的重力方向减去归一化后的加速度测量三个分量分别是 2(q1q3 − q0q2)、2(q0q1 q2q3)、2(0.5 − q1² − q2²)。这三个式子不是拼凑出来的它们就是旋转矩阵的对应元素把 q 转成矩阵再取第三列结果一定一致。磁力计的处理绕一点。地磁矢量在水平方向和垂直方向都有分量水平分量指向磁北这个“北”就是要找的参考。做法是先把磁力计测量旋转到地坐标系得到 h q ⊗ m ⊗ q*取 bx √(hx² hy²)、bz hz构造只有 x 和 z 分量的参考矢量 b [0, bx, 0, bz]。这个技巧的妙处在于它不依赖当地的磁偏角和磁倾角先验每次都用当前姿态自己算参考方向换一个城市不需要重新人工输入磁倾角。把 f_g 和 f_b 竖着拼成 6×1雅可比 J 是 6×4梯度就是 Jᵀf。这里对梯度做归一化不是可选项——不归一化的话梯度幅值随残差大小变化步长就变得不可控残差大的时候一步跨过头残差小的时候又推不动表现出来就是姿态在原地抖。3.3 beta 的物理含义为什么归一化梯度是关键融合公式是 q_{k1} q_k (q̇_gyro − β · ∇f/|∇f|) · Δt。β 的单位和陀螺仪一样是 rad/s它近似等于算法能容忍的陀螺零偏上限。直观理解当梯度方向和陀螺积分方向不一致时β 决定了算法更信任谁。更准确的解释来自控制论。把归一化梯度看成误差反馈项这个式子等价于一阶低通加高通的互补滤波β 对应的就是截止频率。β 取 0.03 时姿态非常平滑但收敛慢上电后要好几秒才稳下来β 取 0.5 时响应快但加速度计的噪声和磁力计的抖动会直接透到姿态上。工程上常见取值落在 0.03 到 0.1 之间采样率高的时候可以适当调大因为每秒钟做的修正次数多了单次步长可以小一些。和 Mahony 的 PI 互补滤波对比Mahony 把误差分成比例项和积分项积分项顺便把陀螺零偏估计出来Madgwick 只有比例项靠梯度下降的几何形式避开了显式调积分增益。两者稳态精度接近Madgwick 计算量略小不维护积分状态代价是对零偏的抑制靠 β 硬扛不能像 Mahony 那样自适应地把零偏估出来。如果板子发热明显、零偏随温度漂得厉害Mahony 的积分项反而更省心如果是电池供电、算力紧张Madgwick 更合适。4. STM32 端落地从寄存器读数到四元数更新的实现细节4.1 量程、DLPF 与磁力计的读取时序MPU9250 的陀螺和加速度是同一颗 die磁力计是另一颗 die 挂在内部总线上。要拿磁力计数据两条路把 MPU9250 设成旁路模式主控直接访问磁力计地址或者让 MPU9250 工作在主机模式由它定时去读磁力计并放到扩展寄存器里。前者实现简单代价是磁力计和陀螺不能严格同步后者同步性好但配置繁琐、调试时容易卡在寄存器没配对。量程直接决定分辨率。陀螺仪 ±2000 dps 时每 LSB 是 0.061 dps±250 dps 时是 0.0076 dps差了八倍。航向角应用通常转速不高选 ±500 或 ±1000 就够没必要为了“留余量”牺牲精度。加速度计选 ±4g磁力计固定 16 位输出、灵敏度 0.15 微特每 LSB。一条硬约束是磁力计的最高输出率只有 100 Hz16 位模式而陀螺可以跑到 1000 Hz。融合频率如果跟着陀螺走磁力计就必须降采样复用——每 10 个周期才更新一次带磁修正其余周期只做重力修正。这一点在代码里体现为一个标志位写漏了会导致同一份磁数据被反复使用航向角被过度修正表现出来是静止时航向角一点点被磁力计拽向某个固定值而不是稳稳停住。4.2 解算循环的 C 实现/* 四元数状态q0 为实部beta 单位 rad/s */ typedef struct { float q0, q1, q2, q3; float beta; float invSampleFreq; /* 1 / 采样频率 */ } madgwick_t; /* 每条轴参数gx~gz 单位 rad/sax~az 与 mx~mz 必须已归一化 */ void madgwick_update(madgwick_t *s, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float q0 s-q0, q1 s-q1, q2 s-q2, q3 s-q3; float hx, hy, bx, bz, norm; float f[6], J[6][4], grad[4]; /* 磁力计旋转到地坐标系h q * m * q^-1 */ hx 2.0f*(mx*(0.5f - q2*q2 - q3*q3) my*(q1*q2 - q0*q3) mz*(q1*q3 q0*q2)); hy 2.0f*(mx*(q1*q2 q0*q3) my*(0.5f - q1*q1 - q3*q3) mz*(q2*q3 - q0*q1)); bx sqrtf(hx*hx hy*hy); /* 水平分量模长即北方向强度 */ bz 2.0f*(mx*(q1*q3 - q0*q2) my*(q2*q3 q0*q1) mz*(0.5f - q1*q1 - q2*q2)); /* 前三个是重力残差后三个是地磁残差 */ f[0] 2.0f*(q1*q3 - q0*q2) - ax; f[1] 2.0f*(q0*q1 q2*q3) - ay; f[2] 2.0f*(0.5f - q1*q1 - q2*q2) - az; f[3] 2.0f*bx*(0.5f - q2*q2 - q3*q3) 2.0f*bz*(q1*q3 - q0*q2) - mx; f[4] 2.0f*bx*(q1*q2 - q0*q3) 2.0f*bz*(q0*q1 q2*q3) - my; f[5] 2.0f*bx*(q0*q2 q1*q3) 2.0f*bz*(0.5f - q1*q1 - q2*q2) - mz; /* 雅可比 J df/dq6 行 4 列逐项对照上式求偏导得到 */ J[0][0] -2.0f*q2; J[0][1] 2.0f*q3; J[0][2] -2.0f*q0; J[0][3] 2.0f*q1; J[1][0] 2.0f*q1; J[1][1] 2.0f*q0; J[1][2] 2.0f*q3; J[1][3] 2.0f*q2; J[2][0] 0.0f; J[2][1] -4.0f*q1; J[2][2] -4.0f*q2; J[2][3] 0.0f; J[3][0] -2.0f*bz*q2; J[3][1] 2.0f*bz*q3; J[3][2] -4.0f*bx*q2 - 2.0f*bz*q0; J[3][3] -4.0f*bx*q3 2.0f*bz*q1; J[4][0] -2.0f*bx*q3 2.0f*bz*q1; J[4][1] 2.0f*bx*q2 2.0f*bz*q0; J[4][2] 2.0f*bx*q1 2.0f*bz*q3; J[4][3] -2.0f*bx*q0 2.0f*bz*q2; J[5][0] 2.0f*bx*q2; J[5][1] 2.0f*bx*q3 - 4.0f*bz*q1; J[5][2] 2.0f*bx*q0 - 4.0f*bz*q2; J[5][3] 2.0f*bx*q1; /* 梯度 J^T * f归一化后只保留方向步长由 beta 统一决定 */ for (int i 0; i 4; i) { grad[i] 0.0f; for (int j 0; j 6; j) grad[i] J[j][i] * f[j]; } norm sqrtf(grad[0]*grad[0] grad[1]*grad[1] grad[2]*grad[2] grad[3]*grad[3]); norm (norm 0.0f) ? (1.0f / norm) : 0.0f; /* 残差为零时不修正防除零 */ /* 陀螺积分项减去 beta 倍归一化梯度再乘采样周期 */ float d0 0.5f*(-q1*gx - q2*gy - q3*gz) - s-beta*grad[0]*norm; float d1 0.5f*( q0*gx q2*gz - q3*gy) - s-beta*grad[1]*norm; float d2 0.5f*( q0*gy - q1*gz q3*gx) - s-beta*grad[2]*norm; float d3 0.5f*( q0*gz q1*gy - q2*gx) - s-beta*grad[3]*norm; s-q0 d0 * s-invSampleFreq; s-q1 d1 * s-invSampleFreq; s-q2 d2 * s-invSampleFreq; s-q3 d3 * s-invSampleFreq; /* 每步必须归一化否则浮点误差会让模长慢慢偏离 1 */ norm sqrtf(s-q0*s-q0 s-q1*s-q1 s-q2*s-q2 s-q3*s-q3); norm (norm 0.0f) ? (1.0f / norm) : 0.0f; s-q0 * norm; s-q1 * norm; s-q2 * norm; s-q3 * norm; }参数含义逐条对照gx/gy/gz是陀螺仪原始值乘上量程系数再转成弧度每秒很多人忘了那一步 0.0174 的度数转弧度导致姿态转得慢十几倍。ax~az和mx~mz必须先归一化因为在 f 的表达式里加速度和磁力计都是单位向量不归一化会让残差量纲不一致等价于给重力和地磁配了不同的权重。beta在 100 Hz 采样下取 0.05 左右是个稳妥的起点。末尾的归一化不能省跑几十分钟后模长漂到 0.99 时欧拉角会出现肉眼可见的慢漂。4.3 欧拉角转换与航向角的连续性处理#define RAD_TO_DEG 57.29578f /* 四元数转欧拉角输出单位度 */ void quat_to_euler(const madgwick_t *s, float *roll, float *pitch, float *yaw) { float q0 s-q0, q1 s-q1, q2 s-q2, q3 s-q3; /* roll 绕机体 x 轴atan2 的两个参数对应 sin 与 cos 的线性组合 */ *roll atan2f(2.0f*(q0*q1 q2*q3), 1.0f - 2.0f*(q1*q1 q2*q2)) * RAD_TO_DEG; /* pitch 用 asin必须夹到 [-1,1] 浮点误差让参数超出范围时 asinf 会返回 NaN 并污染整条链路 */ float sp 2.0f*(q0*q2 - q3*q1); if (sp 1.0f) sp 1.0f; if (sp -1.0f) sp -1.0f; *pitch asinf(sp) * RAD_TO_DEG; /* yaw 即航向角值域 (-180, 180]磁北为 0 */ *yaw atan2f(2.0f*(q0*q3 q1*q2), 1.0f - 2.0f*(q2*q2 q3*q3)) * RAD_TO_DEG; }atan2f输出范围是 (-180, 180]从 179 度转到 -179 度时会突跳 358 度。如果要统计累计转角或者要把航向角送到一个会做差分的控制器里必须先解缠/* 连续航向角跨越 ±180 边界时累加圈数total 可无限增长 */ static int turns 0; static float last_yaw 0.0f; float delta yaw - last_yaw; if (delta 180.0f) turns--; if (delta -180.0f) turns; last_yaw yaw; float total yaw turns * 360.0f;另外要注意 pitch 在正负 90 度附近会遇到万向节锁此时 roll 和 yaw 的耦合会退化同一姿态对应无数组 roll/yaw 解。四轴飞行器的俯仰角很少超过 60 度通常碰不到机械臂腕部关节就会遇上那种场合应该直接输出四元数或者旋转矩阵不要转欧拉角。5. 排查姿态解算链路上五个高频翻车点5.1 静止时航向角匀速旋转现象板子平放在桌上不动航向角以稳定速率爬升或下降速率从每秒零点几度到几度不等。原因陀螺仪零偏没校准。MPU9250 出厂的零偏就有几个 LSB对应零点几度每秒经过积分直接变成线性漂移。β 只能压制、不能消除因为归一化梯度只在有残差时才起作用而静止时重力残差恰好为零。解决上电后静置采样 2 到 3 秒把三轴陀螺读数求均值作为零偏之后每次减去。温度变化大的场合还要做温度分段冷机和热机各采一组零偏按片上温度传感器线性插值。我一般会在结构体里加三个 float 存零偏初始化函数里阻塞采样这几秒钟的等待换来的是可用性上的巨大差别。5.2 靠近电机或金属桌面后航向角突跳现象本文还有配套的精品资源点击获取