简介这是一份面向无人机编程开发者的九轴传感器校准源码工程重点解决加速度计、陀螺仪与磁力计的零点校准、漂移补偿及磁场干扰消除并将三路数据融合为准确的三维姿态信息。包内共二百三十八个文件以一百一十七个头文件与九十八个C源文件为主构成完整的STM32嵌入式工程另有readme说明、uvprojx工程配置、hex固件以及链接脚本、调试配置等辅助文件整体压缩包大小约一点三三兆字节目录组织清晰便于直接阅读或二次开发。已有五百三十人浏览学习。开发者可从源码中学习传感器原始数据采集、预处理、校准参数估计与姿态输出等完整流程理解硬铁与软铁效应及最小二乘拟合等实际应用从而提升无人机导航与控制的稳定性与精度。 去年做 UAV021 这套飞控时最让我头疼的不是姿态解算算法而是传感器校准。飞控板、惯导模块换了三种每次上电出来一批奇奇怪怪的数据——加速度计静止时读数 1.2g陀螺仪在桌上放着每分钟都能漂好几度磁力计更是魔幻转一圈画出来根本不是球。后来我把 3-9 轴传感器的校准逻辑统一梳理了一遍做成了一个极简工具包也就是标题里这个 UAV021_3-9AxisSensor_Calibrate_Simple.zip。这篇博文不聊虚的直接讲清楚三件事3 轴、6 轴、9 轴传感器分别校什么、为什么这么校、你拿到这个包之后怎么把手里的传感器数据校到能用的水平。方案面向无人机飞控、机器人惯导、手持设备姿态解算这些场景适合正在做原型验证的工程师也适合刚把传感器模块点亮、想搞明白校准是怎么回事的初学者。1. 在动手之前先想明白传感器为什么要校准1.1 传感器本身的误差不校准姿态就不是真实姿态MEMS 传感器内部是一个微米级的机械结构加工过程中的材料差异、封装应力、电路放大倍率偏差都会导致芯片输出的原始数据跟真实物理量对不上。最典型的表现有三种。第一种是零偏bias。静止的时候加速度计读出来不是 0 或 1g陀螺仪读出来的角速度不是 0磁力计也带一个固定偏移。第二种是标度因子误差。同样 1g 的重力不同芯片或者同一芯片不同轴的放大倍率不完全一致表现是读到的数值成比例偏大或偏小。第三种是交叉轴耦合。三个轴理论上互成 90 度实际生产过程中做不到完全垂直某个轴转动时会在另外两个轴上产生串扰。在飞控这种系统里这些误差会被姿态解算算法放大。加速度计的零偏导致横滚角、俯仰角出现固定偏差陀螺仪零偏在积分时累积成角度漂移磁力计偏差让航向角整体偏移严重时连返航航线都转一个角度。这些问题靠算法很难完全消化补偿越早越好。所以校准这件事本质上就是拿外部已知的基准反推出传感器各轴的零偏、标度和交叉轴参数之后在每次读取数据时做一次逆变换把“实际读到的值”还原成“真实的物理量”。1.2 3轴、6轴、9轴校的东西完全不同标题里的“3-9AxisSensor”指的是一套工具包覆盖从 3 轴到 9 轴的传感器模块。具体点说3 轴通常指单独的加速度计或者单独的磁力计6 轴就是 IMU也就是三轴加速度计加三轴陀螺仪9 轴则是 IMU 外加三轴磁力计也叫 AHRS 模块。不同轴数需要校准的参数完全不一样甚至同样的传感器放在不同设备上校准需求也不同。我做了个表基本把项目里最常见的几类模块和校准项列全了模块类型组成常见用途需要校准的参数3轴加速度计加速度计倾斜角测量、低端云台零偏、标度因子3轴磁力计磁力计电子罗盘、航向硬磁偏移、软磁矩阵6轴IMU加速度计陀螺仪飞控姿态、动作捕捉加速度零偏/标度、陀螺零偏9轴AHRSIMU磁力计无人机航向、机器人导航上述全部Simple 校准包的设计思路就是按这个表来的加速度计和磁力计都采用“采集足够多的方向数据再拟合模型参数”的方式陀螺仪则采用“静态采集取均值”的方式。全程不依赖转台、分度头这类专业夹具手和一张水平桌面就是主要工具所以叫 Simple。它不追求科研级精度目标是让飞控在原型阶段就有一个可信的数据源。2. 三种核心校准原理搞懂了你就能自己写校准脚本2.1 加速度计六面法把重力当尺子重力方向恒定、大小稳定这是加速度计校准的天然基准。MEMS 加速度计的量程一般是 ±2g、±4g、±8g、±16g常见飞控模块默认量程在 ±4g 或 ±8g重力在其中的数值约等于 1g。六面法的思路非常朴素把传感器模块依次摆放成六个姿态让 XYZ 三个轴分别朝向正负重力方向。比如水平放在桌面上安装时 Z 轴朝天那理论上 az 1gax ay 0翻过来 Z 轴朝下az -1g。同理X 轴朝天和朝下各采一组Y 轴朝天和朝下各采一组一共六组数据。对每个轴建立线性关系raw scale × true bias把六组数据代进去就能解出该轴的标度因子 scale 和零偏 bias。实际采集时我不会只用一组数据去算而是每个姿态保持静止 5 到 10 秒串口以 100Hz 到 200Hz 连续输出把这几百个点取平均用来抵抗噪声。解算代码很简单核心就是最小二乘甚至直接用正反两组数据做加减就能算出来。六面法听起来简单实际操作时要特别注意桌面必须水平模块摆放要尽量贴近真实安装方向。如果桌面有倾斜重力的分量会分配到不想让它出现的轴上求出来的参数就是错的。另一个容易忽视的点是六面姿态必须包含“轴朝上”和“轴朝下”两个方向只做四个侧面会少一个方向的约束标度因子的计算结果会出现系统性偏差。2.2 陀螺仪零偏静止是最好的参考陀螺仪测量角速度有一个特性静止时理想输出应该是零实际却总有一个小的偏移这就是零偏bias。这个偏移量很小通常在 ±0.05°/s 到 ±0.5°/s 之间但积分之后就不是小事了——假设 bias 是 0.1°/s积分一分钟就偏 6°三分钟偏 18°飞控姿态在视觉上就是明显的缓慢旋转。所以陀螺仪校准的原理最简单让模块完全静止采集 30 秒左右的原始角速度数据求平均平均值就是零偏。之后每次读取角速度时减去这个平均值即可。但这里有两个容易踩坑的细节。第一个是“静止”必须是真正的静止不能手拿着模块因为人手有微颤平均之后虽然能消掉一部分但残余低频分量会影响零偏精度。最好把模块放在桌面或者用泡沫胶固定在支架上。第二个是模块刚上电时陀螺仪内部还在自检和热稳定直接采集会得到一个偏大的零偏。我习惯上电后等 30 秒再开始采集等芯片温度稳定后数据更接近真实工作状态。至于陀螺仪的标度因子普通无人机场景不需要太较真因为姿态解算大量使用陀螺仪数据的差分也就是短时间内的角增量标度因子误差对短期姿态影响不大但对长期航向积分会产生明显误差。Simple 版本里我把标度校准关掉了只处理零偏对大多数飞控都够用。2.3 磁力计椭球拟合把磁场“球”还原回来磁力计的校准和其他传感器不太一样因为地磁场本身很弱、很均匀但设备周围总有铁磁材料在扰动它。这种干扰分成两类。一类叫硬磁干扰来自电机永磁体、电调电感、螺丝等固定磁性材料。它会给磁力计三个轴附加一个固定磁场偏移表现出来就是传感器转一圈磁场数据点组成了一个球但球心不在原点。另一类叫软磁干扰来自周围材料对磁场线的扭曲比如飞控板上的铜箔走线、电池会让三个轴的实际灵敏度和方向发生变化表现出来就是球变成了椭球。所以磁力计校准的目标非常明确把采集到的磁场数据点从“中心偏移的椭球”还原成“中心在原点的球”。这就是椭球拟合。在 Simple 包里磁力计校准的核心是采集数据——手持模块在空间中缓慢旋转尽量覆盖每个方向至少要覆盖球面的上下左右前后然后脚本用最小二乘拟合椭球方程 x^T A x b^T x c 0解出中心偏移和变换矩阵校准后的数据就是 y M·(x - offset)。最后再归一化到设定的地磁强度默认按地磁场在当地的模值归一化。这里补充一句椭球拟合需要的数据点数量一般建议至少 500 个如果旋转覆盖不全面解出的矩阵会出现奇异表现为某一轴的比例因子特别大或者求出来的中心偏移明显不合理。所以这一步最关键的其实不是算法而是采集动作。3. 从串口采数据到写入参数完整实操记录3.1 工具包里面有什么解开 UAV021_3-9AxisSensor_Calibrate_Simple.zip里面是这几个文件README.md使用说明和采集姿势示意capture_serial.py串口采集脚本从传感器模块读取原始数据并保存 CSVcalibrate_accel.py加速度计六面法解算calibrate_gyro.py陀螺仪零偏解算calibrate_mag.py磁力计椭球拟合并生成参数params.yaml校准结果输出模板依赖使用 Python 标准库加 numpy、scipy、pyserial。环境准备pip install numpy scipy pyserial采集前把传感器模块通过 USB 串口接到电脑确认串口号Linux 下是 /dev/ttyUSB0 或者 /dev/ttyACM0Windows 下是 COM3 这类在脚本里改好串口参数。传感器模块输出的原始数据格式统一约定成文本行accel x y z gyro x y z mag x y z脚本会按行解析分门别类存成三个 CSV。3.2 加速度计和陀螺仪的校准执行校准加速度计时建议先给模块画一个坐标系贴在壳上方便确认姿态。按顺序摆放六个姿态Z 轴朝上、Z 轴朝下、X 轴朝上、X 轴朝下、Y 轴朝上、Y 轴朝下。每个姿态保持 5 秒以上capture_serial.py 会在这段时间里持续记录。采集完成运行python calibrate_accel.py data_accel.csv脚本输出类似accel: scale: [1.0023, 0.9978, 1.0041] bias: [-0.012, 0.009, 0.015]这里 scale 是标度因子bias 是零偏单位都是 g。校准公式为calibrated scale × (raw - bias)。得到参数后做一次验证把模块随便放在一个非水平面比如斜靠着书本读取校准后的加速度模值应该非常接近 1.00g误差控制在 ±0.03g 以内就算合格。陀螺仪校准更简单模块静止后采集 30 秒python capture_serial.py --gyro-only 30 python calibrate_gyro.py data_gyro.csv输出的是三轴零偏单位是每秒角度比如 [-0.083, 0.021, -0.115]。脚本会同时打印这段采集数据的标准差如果某个轴标准差大于 0.02°/s基本可以判断模块没有真正静止或者采集过程中有人碰到了桌面需要重采。把最终零偏填到配置里就完成了。3.3 磁力计采集与拟合磁力计数据最容易受环境干扰采集时先检查周围把手机、铁块、带磁扣的工具包都拿远模块尽量用支架固定拿在手里人不要贴着模块。然后手持模块开始缓慢旋转。我习惯的轨迹是类似画“无穷大”符号不断改变模块朝向保证三个轴都充分覆盖球面。采集时间控制在 2 到 3 分钟大约能收获 5000 个点左右这个数量足够拟合。python calibrate_mag.py data_mag.csv脚本会输出硬磁偏移offset和 3x3 软磁变换矩阵matrix并给出拟合椭球的残差。残差如果大于 0.05归一化单位说明数据覆盖面不够或者有干扰源需要重采。3.4 把参数写进飞控并验证拿到三组参数后写入配置。如果是自研飞控直接填进 sensor_config.yamlaccel: scale: [1.0023, 0.9978, 1.0041] bias: [-0.012, 0.009, 0.015] gyro: bias: [-0.083, 0.021, -0.115] mag: matrix: [...] offset: [...]写入后重启飞控在调试终端观察三件事静止时加速度模值是否接近 1.00陀螺仪积分角度在静止 1 分钟内是否基本不变旋转设备时航向角能否跟随且回正时没有明显漂移。三项都过校准流程就算完成了。4. 常见问题与排查技巧4.1 采集数据时最常见的三个低级错误六面法的“水平”假设经常被忽略。我试过在普通桌面上直接摆模块结果桌面本身有微小倾斜六面法求出来的零偏没问题但标度因子出现了百分之二三的偏差。解决方法是换一块玻璃水平台或者用垫片找平。磁力计采集时要注意周围的动态磁场干扰。电动螺丝刀、手机、带磁铁的扬声器这些东西贴近模块会导致采样点里有大量离群值椭球拟合会整体被拉偏。如果你看拟合残差突然变大先检查环境。陀螺仪采集太急也容易出问题。模块刚插上 USB 就开录此时陀螺仪内部温漂明显采集得到的 bias 比正常工作时的值大很多。等 30 秒再采效果完全不一样。4.2 磁力计拟合结果偏得离谱先检查覆盖有一次我在室内校准拟合出来的椭球中心偏移居然有 50 微特明显不合理。后来发现问题出在采集动作我一直用右手握着模块转了半天只覆盖了上半球下半球数据点极少拟合算法为了凑这些点硬把椭球中心拉到一端。重新做了一次慢速的“绕三个轴分别画整圆”采集覆盖完整后中心偏移回到了正常的 12 微特。这个经验让我把采集动作的指引写进了 README先是绕 Z 轴转两个整圈然后绕 Y 轴转两个整圈最后绕 X 轴转两个整圈转的时候模块自身的朝向也在缓慢改变这样能保证数据点均匀分布在球面。尤其是最后那个动作很多人会忽略结果数据集中在赤道平面附近拟合出的 Z 轴参数虚高。4.3 校准后静止姿态还是会漂移如果你确认陀螺仪零偏采得没问题加速度计和磁力计也校完了但静止 1 分钟后欧拉角依然缓慢漂第一步检查温度。MEMS 陀螺仪对温度的敏感程度超过大多数人想象我做过一个对比模块在 25 度恒温下校准零偏 0.021°/s拿到阳光下升温到 40 度后偏到了 0.09°/s积分一分钟就偏 5.4°。Simple 版本里没有做温补这是有意取舍因为温补需要对每个芯片单独做多点温度实验工作量成倍上升。如果你的场景对温度漂移敏感至少要做到每次上电后等模块温度稳定再进行姿态初始化或者把 Simple 包的零偏采集流程嵌入到每次上电后的自动校准流程里。4.4 一套参数能通吃所有场景吗不能。加速度计和陀螺仪的参数在同一个模块上相对稳定但磁力计参数几乎跟安装位置强相关。同一块飞控板换一套电调和电机磁力计的硬磁偏移就会明显变化。所以我的建议是磁力计校准在整机状态做也就是说把所有可能影响磁场的部件都装好、上电再进行校准这样参数才贴近实际飞行状态。Simple 包里的采集脚本也倾向于整机状态下使用。这套 Simple 校准包我在 UAV021 项目里用到现在加速度计、陀螺仪、磁力计三个模块的标定流程合计不到 15 分钟比之前动不动上转台、画椭球的折腾省了大量时间。它的定位从来不是高精度的科研校准而是让飞控在原型阶段有一个可信的数据源。如果你校完数据发现某个轴的参数特别异常先别急着怀疑算法回头看看安装结构——传感器芯片周围那颗螺丝是不是带磁了固定结构是不是在转动时产生了微小的相对位移。这类机械层面的干扰软件再改也补不回来。校准是一门工程不是跑个脚本就完事把这个包用顺手了后面换传感器型号、换飞控板你都能很快完成标定不慌。本文还有配套的精品资源点击获取