无人机在悬停或者低速巡航时航向角漂移是让很多人头疼的问题。飞着飞着一看地面站机头方向已经偏了十几度返航时对不准起降点甚至原地打转。这个现象背后多半是姿态解算里航向角的融合没做好。ICM-20948这颗九轴传感器集成了三轴陀螺仪、三轴加速度计和三轴磁力计价格不高、资料够用是中小型飞控和自研飞控板里很常见的选择。但很多人拿到它之后只用了陀螺仪和加速度计做姿态磁力计要么没接、要么接了不会用结果航向角只能靠陀螺仪积分时间一长必然漂。这篇内容就围绕ICM-20948把陀螺仪和磁力计怎么配合、航向角怎么算、代码怎么写讲清楚适合正在做飞控、云台、机器人定位的开发者参考也适合刚接触九轴传感器、想把航向角做稳的人。1. 航向角为什么会漂ICM-20948能解决什么1.1 陀螺仪积分漂移的物理本质先把这个问题的根说透。陀螺仪输出的是角速度单位是度每秒或者弧度每秒。要得到角度必须对角速度做时间积分。积分这件事本身没问题问题出在陀螺仪有零偏。所谓零偏就是传感器在完全静止时输出并不是严格的零而是有一个很小的偏置值比如0.5度每秒。这个偏置在积分过程中会被不断累加一分钟就是30度十分钟就是300度航向角直接废掉。零偏还分两种。一种是固定零偏上电后基本不变可以通过静止校准减掉。另一种是温漂零偏随着温度变化而缓慢变化芯片发热、环境温度改变都会让它动。固定零偏好处理温漂零偏就麻烦了你不可能每隔几分钟就停下来重新校准一次。所以单靠陀螺仪航向角在几十秒到几分钟的尺度上就会明显漂移这是物理层面的限制不是代码写得不好。加速度计能帮忙修正俯仰和横滚因为重力方向是天然的参考。但加速度计对航向角完全无能为力因为重力矢量是垂直的绕垂直轴旋转不改变重力在三个轴上的投影。这就是为什么只用六轴陀螺仪加速度计的方案航向角必然漂。要修正航向必须引入一个能感知水平方向绝对参考的传感器磁力计就是干这个的。1.2 磁力计提供的绝对航向参考地球磁场在水平方向有一个稳定的分量指向磁北。磁力计测量三个轴的磁场强度通过计算水平面内磁场矢量的方向就能得到相对于磁北的航向角。这个参考是绝对的不会随时间漂移只要周围没有强磁干扰它就能长期稳定地给出航向。但磁力计也有它的问题。第一它测的是总磁场包括地球磁场和周围所有铁磁物质、电流产生的干扰磁场。无人机上的电机、电调、电池线通电后都会产生磁场直接污染磁力计读数。第二磁力计本身有零偏和标度因数误差三个轴的灵敏度不完全一致不校准的话算出来的航向角会有周期性误差。第三磁力计动态响应慢电机转速变化时干扰剧烈不能单独用它做航向必须和陀螺仪融合。所以正确的思路是陀螺仪提供短期高动态的航向变化磁力计提供长期绝对参考两者通过互补滤波或者卡尔曼滤波融合。陀螺仪负责快磁力计负责稳各取所长。ICM-20948把这三样东西集成在一颗芯片里通过I2C或SPI读取省去了外接磁力计的麻烦而且三者出厂时已经做了基本的轴对齐用起来比分立方案省心。1.3 ICM-20948相比MPU6050的关键差异很多人是从MPU6050转过来的这里必须说清楚两者的区别。MPU6050是六轴只有陀螺仪和加速度计没有磁力计。用MPU6050做航向角只能靠陀螺仪积分漂移是必然的除非你外接一个HMC5883或者QMC5883之类的磁力计但外接又涉及轴对齐和安装位置问题。ICM-20948是九轴内部集成了AK09916磁力计。注意AK09916是作为ICM-20948的从设备挂在内部I2C总线上的主控要读磁力计数据需要通过ICM-20948的I2C主机接口去访问而不是直接读ICM-20948的寄存器。这一点是新手最容易卡住的地方很多人以为磁力计数据和其他数据一样直接读就行结果读出来全是零或者固定值。另外ICM-20948支持DMP数字运动处理器可以把姿态解算放到芯片内部做减轻主控负担。但DMP的配置比较复杂而且官方DMP库对磁力计的支持在不同版本里表现不一致。对于想搞清楚原理、方便调试的开发者我更建议先用原始数据自己做融合把航向角算明白再考虑要不要上DMP。2. 硬件连接与寄存器配置的实操细节2.1 供电、总线和中断引脚的接法ICM-20948的供电范围是1.71V到3.6VVDD和VDDIO要分别接。VDD是核心供电VDDIO是IO口供电两者可以都接3.3V但如果主控是1.8V逻辑VDDIO就要接1.8V否则I2C电平不匹配。这一点在画板子的时候就要注意不要想当然全接3.3V。总线方面ICM-20948支持I2C和SPI。I2C的话AD0引脚决定地址接地是0x68接VDDIO是0x69。SPI的话CS片选拉低使能时钟最高可以到7MHz。对于飞控这种对实时性有要求的场景SPI比I2C快而且不受总线仲裁影响建议优先用SPI。但SPI占引脚多如果主控引脚紧张I2C在400kHz下也够用只是读取频率上不去融合时要注意采样率。中断引脚INT1和INT2可以配置成数据就绪中断这样主控不用轮询数据准备好了中断通知能降低CPU占用也能保证采样时刻的确定性。做航向角融合时采样时刻的确定性很重要因为积分对时间间隔敏感如果读取时间抖动大积分误差也会变大。建议把数据就绪中断接上在中断里置标志位主循环里读取和计算。2.2 唤醒、时钟源与量程设置ICM-20948上电后默认处于睡眠模式必须先唤醒。流程是先读WHO_AM_I寄存器确认通信正常值应该是0xEA。然后写PWR_MGMT_1寄存器清除SLEEP位同时选择时钟源。时钟源建议选自动选择或者内部RC不要用外部时钟除非你确实接了晶振。选自动的话芯片会优先用陀螺仪时钟稳定性比内部RC好。量程设置要根据应用场景来。陀螺仪量程有±250、±500、±1000、±2000度每秒几档。无人机做航向角角速度一般不会超过±500度每秒选±500的话分辨率是65.5 LSB每度每秒比±2000的16.4 LSB精细四倍。量程越小分辨率越高但饱和风险越大。如果你做的是穿越机这种剧烈机动的选±1000或±2000保险一些。加速度计量程有±2g到±16g航向角融合里加速度计主要用来算俯仰横滚±4g或±8g够用。磁力计AK09916的量程固定是±4900微特斯拉分辨率0.15微特斯拉这个不用配。但磁力计的测量模式要设置有单次测量、连续测量10Hz、20Hz、50Hz、100Hz几档。航向角融合建议用连续测量50Hz或100Hz保证数据更新率跟得上陀螺仪。2.3 通过I2C主机接口读取磁力计数据这是整个配置里最关键也最容易出错的一步。AK09916挂在ICM-20948内部的I2C总线上主控要读它得通过ICM-20948的I2C主机接口。具体流程是先配置I2C_MST_CTRL寄存器设置内部I2C时钟一般400kHz。然后配置I2C_SLV0_ADDR、I2C_SLV0_REG、I2C_SLV0_CTRL让ICM-20948自动去读AK09916的某个寄存器把数据放到EXT_SLV_SENS_DATA寄存器里。主控读EXT_SLV_SENS_DATA就能拿到磁力计数据。AK09916的WHO_AM_I寄存器地址是0x01值应该是0x09。读之前先确认这个值确认磁力计在线。磁力计数据寄存器从0x11开始连续6个字节是X、Y、Z的低高字节小端格式。注意AK09916的数据是16位有符号数但实际有效位是14位最高两位是符号扩展读出来直接当int16用就行。还有一个坑AK09916有数据就绪标志在ST1寄存器里。如果不等就绪就读可能读到旧数据或者数据不一致。稳妥的做法是配置ICM-20948的I2C主机去读ST1判断就绪后再读数据。但这样会增加总线开销简单做法是保证读取频率低于磁力计输出频率比如磁力计设100Hz主控50Hz读一次基本不会读到重复数据。3. 从原始数据到航向角的完整计算链路3.1 陀螺仪零偏校准与温度补偿上电后第一件事是陀螺仪零偏校准。把无人机放在水平静止的台面上采集几百到一千个陀螺仪样本求平均值这个平均值就是零偏。之后每次读取陀螺仪数据减去这个零偏再积分。采集样本时要注意台面要稳不能有振动电机不能通电否则采到的零偏不准。零偏校准的样本数怎么定理论上越多越准但太多浪费时间。我一般采1000个在1kHz采样率下就是1秒够用了。采完之后可以算一下标准差如果标准差很大说明有振动或者干扰要重新采。标准差应该在零点几LSB的量级如果到了几十LSB肯定有问题。温度补偿这块ICM-20948内部有温度传感器可以读出来。严格的做法是建立零偏随温度变化的曲线在不同温度下分别校准然后运行时根据当前温度插值补偿。但这对大多数应用来说过于复杂。实际做法是如果工作环境温度变化不大比如室内或者常温户外固定零偏校准就够了。如果温度变化大可以在陀螺仪静止时检测零偏变化做缓慢的自适应修正但这需要判断静止状态逻辑复杂一些。3.2 磁力计的椭球校准与硬磁软磁修正磁力计不校准航向角误差可以到几十度校准之后能压到一两度以内。校准分两步硬磁校准和软磁校准。硬磁干扰是固定的磁场偏置比如无人机上的螺丝、电机磁钢它们产生一个恒定的附加磁场让磁力计的测量球心偏离原点。修正方法就是找到这个球心把测量值减去球心。实际操作是把无人机拿在手里缓慢旋转各个方向采集大量磁力计数据然后求三个轴的最大值和最小值球心就是最大值最小值/2。这个方法简单但要求旋转时覆盖所有方向采集点要均匀。软磁干扰是铁磁材料对磁场的扭曲让测量球变成椭球。修正需要一个3x3的变换矩阵把椭球变回球。这个矩阵可以通过椭球拟合算法求出来比如最小二乘拟合。代码实现上采集数据后解一个线性方程组得到椭球参数再转换成变换矩阵。这个过程计算量不大可以在上位机做把矩阵存到飞控里用。校准质量怎么判断校准后把磁力计数据归一化模长应该接近1。如果模长在0.9到1.1之间波动说明校准还行。如果波动超过20%说明校准没做好或者周围有动态干扰。注意校准要在实际安装状态下做磁力计装上无人机之后再校准因为安装位置的铁磁物质会影响磁场分布。3.3 用互补滤波融合陀螺仪与磁力计融合的思路很直接陀螺仪积分得到航向角变化磁力计算出绝对航向角两者做加权平均。互补滤波的公式是航向角 α * (航向角 陀螺仪角速度 * dt) (1 - α) * 磁力计航向角α是权重接近1比如0.98。这样陀螺仪的短期变化被保留磁力计的长期参考慢慢把漂移拉回来。α越大对磁力计依赖越小动态响应好但漂移修正慢α越小磁力计影响越大长期稳但磁干扰时航向角会跳。这里有个细节磁力计算出的航向角是绝对角度范围0到360度而陀螺仪积分出来的是连续角度可能超过360或者小于0。融合时要先把两者统一到同一个参考系处理角度环绕问题。比如磁力计算出350度陀螺仪积分算出-5度它们其实差5度不能直接相减。做法是把角度差归一化到-180到180度之间再做加权。磁力计航向角的计算要用到俯仰和横滚角做倾斜补偿。因为磁力计是固定在机体上的机体倾斜时磁力计测到的水平分量不是真正的水平分量。补偿公式是Xh Xm * cos(pitch) Zm * sin(pitch) Yh Xm * sin(roll) * sin(pitch) Ym * cos(roll) - Zm * sin(roll) * cos(pitch) 航向角 atan2(-Yh, Xh)其中Xm、Ym、Zm是磁力计原始数据pitch和roll来自加速度计解算。不做倾斜补偿的话机体倾斜时航向角误差会很大倾斜30度可能带来十几度误差。4. C代码实现与调试中的常见问题4.1 传感器驱动层的读写封装先写底层读写。以SPI为例ICM-20948的SPI读操作需要把寄存器地址的最高位置1写操作最高位清0。读的时候先发地址再发一个空字节收数据。代码大概这样uint8_t icm20948_read_reg(uint8_t reg) { uint8_t tx[2] {reg | 0x80, 0x00}; uint8_t rx[2]; spi_transfer(tx, rx, 2); return rx[1]; } void icm20948_write_reg(uint8_t reg, uint8_t val) { uint8_t tx[2] {reg 0x7F, val}; spi_transfer(tx, NULL, 2); }批量读用连续地址读效率高。读磁力计数据要通过EXT_SLV_SENS_DATA寄存器一次读6个字节。注意读之前要确保I2C主机已经配置好并且正在工作否则读出来是旧数据。初始化顺序很重要先复位延时读WHO_AM_I确认然后唤醒配置时钟配置量程和采样率配置中断配置I2C主机读磁力计最后使能。每一步之间要加适当延时芯片内部状态切换需要时间太快了配置不生效。4.2 姿态解算主循环的代码结构主循环的结构建议是中断置标志主循环检测标志后读取数据、计算、输出。读取时一次性把加速度计、陀螺仪、磁力计、温度都读出来保证时间戳一致。计算顺序是先用加速度计算俯仰横滚再用俯仰横滚和磁力计算航向最后用陀螺仪和磁力计航向做互补滤波。void attitude_update(float dt) { // 读取原始数据 read_imu_raw(acc, gyro, mag); // 加速度计解算俯仰横滚 float pitch atan2f(-acc.x, sqrtf(acc.y*acc.y acc.z*acc.z)); float roll atan2f(acc.y, acc.z); // 磁力计倾斜补偿后算航向 float mx mag.x * cosf(pitch) mag.z * sinf(pitch); float my mag.x * sinf(roll)*sinf(pitch) mag.y*cosf(roll) - mag.z*sinf(roll)*cosf(pitch); float mag_yaw atan2f(-my, mx) * RAD_TO_DEG; // 陀螺仪积分 gyro_yaw (gyro.z - gyro_bias_z) * dt; // 角度差归一化 float diff mag_yaw - gyro_yaw; while (diff 180.0f) diff - 360.0f; while (diff -180.0f) diff 360.0f; // 互补滤波 gyro_yaw (1.0f - ALPHA) * diff; // 归一化到0-360 while (gyro_yaw 360.0f) gyro_yaw - 360.0f; while (gyro_yaw 0.0f) gyro_yaw 360.0f; }这段代码里dt的准确性很关键。如果用固定定时器中断触发dt就是固定的。如果用主循环计时dt会抖动积分误差大。建议用定时器以固定频率触发姿态更新比如500Hz或1kHz。4.3 磁干扰导致航向跳变的处理实际飞行中电机一转磁力计数据就乱。这时候如果还硬融合航向角会跟着跳飞控输出会抖。处理办法是检测磁干扰强度干扰大时降低磁力计权重甚至暂时不融合只靠陀螺仪短时维持。怎么检测磁力计校准后模长应该接近1如果模长偏离1超过阈值比如0.7到1.3之外说明有干扰。另一个指标是磁力计航向角和陀螺仪积分航向角的差值如果差值突然变大也可能是干扰。检测到干扰后把互补滤波的α临时调到接近1让磁力计几乎不起作用等干扰过去再慢慢恢复。还有一种情况是磁力计数据本身跳变比如I2C读取出错读到了0或者最大值。这种要在驱动层做数据有效性检查连续几个周期数据不变或者超出合理范围就标记为无效融合时跳过。4.4 调试工具与验证方法调试航向角光看代码不行得有工具。最简单的办法是把数据通过串口打到上位机用波形显示。看三条曲线陀螺仪积分航向、磁力计航向、融合后航向。静止时融合航向应该稳定缓慢转动时应该跟得上快速转动时应该平滑不跳。验证精度的话可以用一个已知的参考比如把无人机对准正北看航向角是不是接近0度。或者用一个高精度的转台转固定角度对比。没有转台的话可以在地面画一个圆每隔45度做一个标记手动转动对比。还有一个实用技巧把磁力计校准后的数据画成三维散点图应该是一个球面。如果明显是椭球或者有偏移说明校准没做好。这个在上位机用Python的matplotlib几行代码就能画比盲调高效得多。5. 几个容易踩的坑和我的实际经验5.1 磁力计轴对齐与安装方向ICM-20948内部磁力计的轴和陀螺仪加速度计的轴理论上是对齐的但实际安装到板子上板子又装到无人机上坐标系会变。如果磁力计的X轴和陀螺仪的X轴方向不一致算出来的航向角会差一个固定角度。这个角度可以在校准的时候一并测出来作为偏置补偿掉。安装位置也很讲究。磁力计要尽量远离电机、电调和电源线。如果板子只能装在中间那至少让磁力计那一侧远离大电流走线。我见过有人把飞控装在电池正上方磁力计读数被电池磁场污染得没法用最后只能加长支架把飞控挪开。5.2 采样率与滤波的匹配陀螺仪采样率、磁力计采样率、融合更新率这三个要匹配好。融合更新率应该等于或高于陀螺仪采样率否则会丢数据。磁力计采样率可以低于融合率因为磁力计本身带宽低更新太快也没用反而增加总线负担。滤波方面陀螺仪数据建议做一点低通滤波截止频率根据应用定无人机一般50到100Hz。但滤波会引入相位延迟延迟大了融合会振荡。所以滤波不能太狠要权衡。磁力计数据本身噪声大但更新率低可以做滑动平均窗口不要太大3到5个点就行。5.3 上电顺序与初始化失败的排查有时候上电后读WHO_AM_I不对或者磁力计读不到。排查顺序是先确认供电和电平用示波器看VDD和VDDIO是否正常。然后确认SPI或I2C波形看时钟和数据线有没有信号。如果通信正常但WHO_AM_I不对可能是芯片没复位干净加长复位后的延时。如果磁力计读不到检查I2C主机配置确认AK09916的WHO_AM_I能读到0x09。还有一个隐蔽的问题ICM-20948的I2C主机和主控的I2C是两套独立的总线配置时不要搞混寄存器。主控访问ICM-20948用一套地址ICM-20948访问AK09916用另一套两者通过芯片内部桥接。理解这个结构调试起来就清晰了。5.4 长时间运行后的漂移复现与验证做完融合后要做长时间静止测试至少跑半小时看航向角漂多少。好的融合应该在一两度以内。如果漂了几十度说明磁力计没起作用或者校准有问题。然后做动态测试手动转动无人机看航向角跟随是否平滑有没有跳变。我自己的经验是磁力计校准做得好互补滤波参数调得合适静止半小时航向漂移能控制在1度以内动态跟随误差在2到3度。这个精度对于大多数无人机航向控制够用了。如果要求更高就得上卡尔曼滤波把陀螺仪零偏也作为状态量估计能进一步压漂移但计算量和调参复杂度都上去了。最后分享一个调试时的小习惯每次改完融合参数先在地面静止看十分钟波形确认没有慢漂和振荡再上机测试。地面能发现的问题不要带到空中去发现。航向角这个东西地面看着稳空中不一定稳但地面都不稳空中肯定不稳。