简介这份资源面向无人机编程与嵌入式开发学习者聚焦九轴传感器校准这一飞控关键环节提供一套可直接参考的工程源码帮助解决加速度计、陀螺仪与磁力计在实际使用中零点偏差、初始偏角、漂移及硬铁软铁干扰等问题。压缩包共238个文件以117个.h头文件与98个.c源文件为主体辅以少量汇编、配置与说明文件整体约1.33MB代码基于STM32 HAL库组织涵盖定时器、I2C、队列与任务调度等模块便于按功能定位校准实现。目前已有533人学习下载。读者可从中了解原始数据采集、误差补偿与姿态角解算的完整流程掌握校准参数的计算与应用思路并对照工程结构理解传感器融合在飞控中的落地方式为自主导航与稳定控制打下基础。1. 无人机九轴传感器标定一个压缩包能省掉多少试飞飞控日志里姿态角总在悬停时缓慢漂移PID 调了三轮还是压不住最后发现根因不在控制环而在九轴传感器没标定干净。加速度计零偏、磁力计硬铁软铁误差、陀螺仪温漂任何一项没处理后面所有算法都在错误的地基上盖楼。UAV021_3-9AxisSensor_Calibrate_Simple.zip就是冲着这个痛点来的一套面向无人机编程篇的九轴传感器标定示例工程把加速度计六面法、磁力计椭球拟合、陀螺仪静态零偏估计串成一条可复现的流程。它适合正在做飞控二次开发、传感器融合调试的从业者也适合刚接触 IMU 标定、想把理论落到代码上的新手。压缩包不大但里面每一步都对应一个真实会翻车的环节下面按我拆包的顺序讲。2. 拆包看结构标定工程里到底放了什么拿到压缩包先别急着编译先看清楚目录组织能省掉后面一半的迷茫。这个工程的组织方式比较典型原始采样数据、标定算法脚本、参数输出文件、验证用的可视化脚本各占一块。我一般会先列一遍文件树确认数据格式和脚本入口再决定从哪个环节切入。2.1 目录与文件职责划分解压后大致会看到这样几类内容不同版本命名可能略有差异以实际为准目录/文件职责使用时机data/或raw/存放加速度计、陀螺仪、磁力计的原始采样标定前先采集或替换calib_accel.*六面法加速度计零偏与尺度因子估计第一步calib_mag.*磁力计椭球拟合输出硬铁偏移与软铁矩阵第二步calib_gyro.*静态零偏估计可与加速度计并行params/或output/标定结果落盘供飞控读取标定完成后verify/或plot_*标定前后对比可视化收尾验证这个划分的意义在于标定不是一次性动作而是「采集—估计—写回—验证」的闭环。把数据和算法分开意味着你可以用同一套脚本反复处理不同批次的采样而不用改代码。常见做法是把每次采集的数据按时间戳命名存进data/标定脚本只认目录不认文件名这样换一批数据直接跑就行。2.2 先确认数据格式再动手在跑任何脚本之前先打开一个采样文件看列定义。九轴数据常见的排列是ax, ay, az, gx, gy, gz, mx, my, mz单位分别是 m/s²、rad/s 或 °/s、高斯或微特斯拉。单位不统一是后面最大的坑之一加速度计按 g 还是 m/s²、陀螺仪按度还是弧度直接决定零偏和尺度因子的量级。我习惯在脚本入口处先做一次单位归一化把所有量统一到 SI 单位再进算法避免中途换算出错。import numpy as np # 读取原始九轴采样假设为逗号分隔、无表头 raw np.loadtxt(data/imu_sample.csv, delimiter,) # 列顺序ax ay az gx gy gz mx my mz acc raw[:, 0:3] # 单位 m/s^2 gyro raw[:, 3:6] # 单位 rad/s若原始为 deg/s 需乘 pi/180 mag raw[:, 6:9] # 单位 uT # 单位归一化若陀螺仪原始为 deg/s取消下面注释 # gyro np.deg2rad(gyro) print(acc mean:, acc.mean(axis0)) print(gyro mean:, gyro.mean(axis0)) print(mag mean:, mag.mean(axis0))这段代码只做一件事把数据读进来、统一单位、打印三轴均值。别小看均值加速度计静止时三轴均值应该接近重力矢量方向上的分量磁力计均值能粗略反映当地磁场强度陀螺仪均值就是静态零偏的初估。如果打印出来的加速度模长离 9.8 差很远先别往下走回头查采集时的放置姿态和单位设置。参数上唯一要改的是列顺序和分隔符跟你的实际文件对齐即可。3. 加速度计六面法零偏和尺度因子怎么估加速度计标定的核心思路很朴素把传感器分别以六个面朝下静置每个姿态下理论重力矢量是已知的实测值和理论值的差异就暴露了零偏和尺度误差。这个方法的门槛不在数学而在采集时的姿态控制和数据质量。3.1 六面法的数学模型设真实加速度为 $a_{true}$实测为 $a_{meas}$一阶模型写成$$a_{meas} S \cdot a_{true} b$$其中 $S$ 是三轴尺度因子构成的对角矩阵忽略轴间耦合时$b$ 是三轴零偏。六个面朝下时$a_{true}$ 分别是 $\pm g$ 沿某一轴、其余两轴为零。把六组方程联立每个轴都能解出一组 $S$ 和 $b$。工程上更常用的是「模长约束法」不管姿态如何静止时加速度模长都应等于当地重力加速度用大量不同姿态的采样拟合出使模长最接近 $g$ 的参数。六面法胜在采样少、可解释性强适合快速上手。我一般会先用六面法拿到初值再用模长约束做一轮精修。六面法的采集要求是每个面静置足够长时间几秒到十几秒取均值消除噪声。姿态没摆正、桌面不水平都会让「理论值」本身带误差这是后面要重点排查的。3.2 采集与估计的完整脚本import numpy as np g 9.80665 # 当地重力加速度按纬度微调 # 六个姿态的实测均值每个姿态静置后取平均 # 顺序X朝下, -X朝下, Y朝下, -Y朝下, Z朝下, -Z朝下 meas np.array([ [ g, 0, 0], [-g, 0, 0], [ 0, g, 0], [ 0, -g, 0], [ 0, 0, g], [ 0, 0, -g], ], dtypefloat) # 实际使用时替换为从 data/ 读取的六组均值 # meas np.array([np.loadtxt(f) for f in six_files]) S np.zeros(3) b np.zeros(3) for axis in range(3): pos meas[2 * axis, axis] # 该轴正向朝下实测 neg meas[2 * axis 1, axis] # 该轴负向朝下实测 S[axis] (pos - neg) / (2 * g) # 尺度因子 b[axis] (pos neg) / 2 # 零偏 print(scale:, S) print(bias :, b) # 标定后校正 def correct_accel(a_raw): return (a_raw - b) / S逻辑说明每个轴用正向和负向两次测量两者之差消掉零偏、留下两倍重力乘尺度因子两者之和消掉尺度项、留下两倍零偏。这是六面法最干净的解法。参数上要注意g的取值低纬度地区用 9.78 左右、中纬度用 9.80 左右差 0.02 对高精度应用是有影响的。S理论上应接近 1如果算出来偏离 1 超过 5%先怀疑采集姿态或单位而不是传感器坏了。校正函数correct_accel就是标定结果的使用方式飞控里读到的原始值先减零偏再除尺度因子。3.3 模长约束精修六面法只用了六个点噪声和姿态误差会被放大。用模长约束做最小二乘精修能把几十组不同姿态的采样都用上from scipy.optimize import least_squares def residual(p, acc_raw): s p[0:3] b p[3:6] a_corr (acc_raw - b) / s return np.linalg.norm(a_corr, axis1) - g # 初值来自六面法 p0 np.concatenate([S, b]) res least_squares(residual, p0, args(acc_all,), methodlm) S_ref, b_ref res.x[0:3], res.x[3:6] print(refined scale:, S_ref) print(refined bias :, b_ref)这里acc_all是包含各种姿态的采样矩阵。残差定义为校正后模长与g之差优化器会同时调整尺度和零偏让残差最小。用lm方法是因为问题维度低、初值已经不错收敛快。如果残差平方和降不下去多半是采样里混进了运动状态的数据静止采集这一条必须守住。4. 磁力计椭球拟合硬铁软铁一次讲透磁力计是九轴里最容易翻车的一环。无人机上电机、电调、电池走线都会产生磁场干扰硬铁干扰是固定偏移、软铁干扰是随姿态变化的缩放和轴间耦合。不标定直接拿磁力计算航向罗盘能偏出几十度飞起来就是画圈。4.1 为什么是椭球而不是球理想情况下传感器绕各个方向旋转采集到的磁场点应分布在一个球面上球心在原点、半径等于当地磁场强度。硬铁干扰把球心平移软铁干扰把球拉成椭球。所以标定的任务就是拟合出一个椭球求出它的球心硬铁偏移和形状矩阵软铁矩阵再把椭球变换回单位球。数学上椭球一般式写成$$(m - c)^T W (m - c) 1$$其中 $c$ 是球心即硬铁偏移$W$ 是正定对称矩阵软铁校正矩阵与 $W$ 的特征分解相关。工程上常用最小二乘直接拟合一般二次曲面再从中提取 $c$ 和 $W$。4.2 椭球拟合脚本与参数提取import numpy as np def fit_ellipsoid(mag): # mag: N x 3 磁力计采样 x, y, z mag[:, 0], mag[:, 1], mag[:, 2] # 一般二次曲面: ax^2by^2cz^22dxy2exz2fyz2gx2hy2iz 1 D np.column_stack([ x*x, y*y, z*z, 2*x*y, 2*x*z, 2*y*z, 2*x, 2*y, 2*z ]) ones np.ones(len(mag)) coef, *_ np.linalg.lstsq(D, ones, rcondNone) a, b, c, d, e, f, g, h, i coef A np.array([[a, d, e], [d, b, f], [e, f, c]]) # 球心 -A^{-1} * [g, h, i] center -np.linalg.solve(A, np.array([g, h, i])) # 软铁矩阵将椭球归一化为球 T np.array([g, h, i]) k 1 center A center # 常数项修正 W A / k # 特征分解得到软铁变换 eigval, eigvec np.linalg.eigh(W) soft eigvec np.diag(np.sqrt(eigval)) eigvec.T return center, soft center, soft fit_ellipsoid(mag_all) print(hard iron offset:, center) print(soft iron matrix:\n, soft) def correct_mag(m_raw): return soft (m_raw - center).T逻辑说明D矩阵把每个采样映射到二次曲面的基函数上最小二乘解出九个系数。A是二次项矩阵球心由线性项和A的逆求出。软铁矩阵通过对W做特征分解再开方得到本质是把椭球各轴缩放到同一尺度。参数上要注意采样必须覆盖足够多的方向只在一个平面转圈拟合出来的椭球在第三维上是病态的lstsq会给出离谱结果。我一般要求采样点覆盖球面的多个象限且每个方向附近都有点。4.3 采样质量决定标定上限磁力计标定的精度几乎完全由采样质量决定算法本身没什么玄学。采集时把无人机拿在手里绕三个轴各转几圈尽量让磁力计朝向覆盖全空间远离电机通电状态和金属桌面。采完之后先画个散点图看分布如果点集中在一小片区域别急着拟合回去重采。常见做法是采几百到上千个点采样率不用高但空间覆盖要全。拟合完成后用校正函数把原始点变换一遍看变换后的模长是否稳定在当地磁场强度附近波动超过几个百分点就说明软铁矩阵没拟合好。5. 避坑与排查标定流程里最容易翻车的五件事5.1 标定后姿态反而更飘现象跑完标定写回参数悬停时姿态漂移比标定前还明显。原因多半是加速度计尺度因子符号搞反或者校正公式写成了a*S b而不是(a-b)/S。六面法里正向和负向的减法顺序一旦颠倒S会变成负值校正后重力方向整个翻转。解决标定完先做静态验证静止放置时校正后的加速度模长应接近g、方向应指向重力方向不满足就回头检查公式和符号。5.2 磁力计拟合报矩阵奇异现象np.linalg.solve抛奇异矩阵错误或解出的球心数值离谱。原因是采样点共面或近似共面二次项矩阵A秩亏。手持旋转时如果只绕竖直轴转、没有俯仰和横滚就会出这个问题。解决重采强制覆盖三个轴向的旋转或者在拟合前做主成分分析确认三个主方向都有足够方差再进最小二乘。5.3 陀螺仪零偏随温度跑现象冷机标定的零偏飞了几分钟后姿态积分明显偏移。陀螺仪零偏对温度敏感室温静态标定只能拿到某个温度点的值。解决如果工程支持做温补表在不同温度点各采一段静态数据拟合零偏随温度的变化不支持温补的话至少在上电后静置预热一段时间再采零偏别冷机直接标。这是血泪经验冷机标出来的零偏在飞行中段基本不可信。5.4 单位混用导致参数量级错误现象标定输出的零偏数值大得离谱或者校正后数据完全失真。九轴数据里加速度计可能是 g 也可能是 m/s²陀螺仪可能是度也可能是弧度磁力计可能是高斯也可能是微特斯拉。解决在脚本入口统一单位并在标定前后各打印一次数据范围做交叉验证。我习惯在参数文件里显式写单位注释避免下次换数据时又踩一遍。5.5 把运动数据混进静态标定现象加速度计模长约束的残差降不下去磁力计椭球拟合的残差分布很散。原因是采集时手抖、无人机没完全静止或者采集脚本在电机转动时还在记录。解决采集阶段加一个静止判据比如连续若干帧的角速度模长低于阈值才记录标定前对数据做一遍粗筛剔除模长明显偏离的离群点。这个黑匣子不开后面调参全是白费。6. 验证与写回让标定结果真正进飞控标定算出参数只是半程验证和写回才是闭环。我一般分两步验证先离线用校正函数处理一遍原始数据看统计指标再把参数按飞控要求的格式写进配置文件上机做静态和低速验证。6.1 离线验证的量化指标指标计算方式合格参考加速度模长均值校正后norm(a)均值接近当地g偏差 1%加速度模长标准差校正后norm(a)标准差越小越好反映噪声与残差磁力计模长波动校正后norm(m)的相对标准差 3% 视为拟合良好陀螺仪静态均值静止段角速度均值接近零残余即零偏a_corr correct_accel(acc_all) m_corr correct_mag(mag_all).T print(acc norm mean/std:, np.linalg.norm(a_corr, axis1).mean(), np.linalg.norm(a_corr, axis1).std()) print(mag norm rel-std:, np.linalg.norm(m_corr, axis1).std() / np.linalg.norm(m_corr, axis1).mean())这几个数字比任何主观判断都可靠。加速度模长均值偏离g超过 1%说明尺度或零偏还有系统误差磁力计模长相对标准差超过 3%说明软铁矩阵没拟合到位或者采样覆盖不够。别跳过这一步直接上机空中发现问题代价太大。6.2 写回飞控的参数格式不同飞控的参数组织方式不一样常见做法是把零偏和尺度因子写成数组磁力计写成偏移向量加 3x3 矩阵。写回时注意两点一是单位与飞控内部一致二是参数顺序与飞控读取顺序一致。我习惯在写回脚本里加一个回读校验写完立刻读出来和内存里的值比对防止格式转换时静默截断。参数文件里保留标定日期和采集条件注释下次排查时能对上号。6.3 上机验证的顺序先静态无人机放平看姿态角是否稳定、航向是否随转动平滑变化。再低速手动转动看姿态跟随有没有滞后或跳变。最后才进飞行验证。每一步都记录日志出问题能回溯到是哪个环节的参数不对。从那以后我每次标定完都强制走一遍「离线指标—静态—低速—飞行」这条链哪怕时间紧也不跳步因为跳过的那一步往往就是后面炸机报告的根因。希望这套流程帮到你少走几轮试飞的弯路。本文还有配套的精品资源点击获取 
长河君 · 2026/10/26 13:51:10 · 阅读 1.2k 无人机九轴传感器标定实战：从六面法到椭球拟合的完整流程
简介这份资源面向无人机编程与嵌入式开发学习者聚焦九轴传感器校准这一飞控关键环节提供一套可直接参考的工程源码帮助解决加速度计、陀螺仪与磁力计在实际使用中零点偏差、初始偏角、漂移及硬铁软铁干扰等问题。压缩包共238个文件以117个.h头文件与98个.c源文件为主体辅以少量汇编、配置与说明文件整体约1.33MB代码基于STM32 HAL库组织涵盖定时器、I2C、队列与任务调度等模块便于按功能定位校准实现。目前已有533人学习下载。读者可从中了解原始数据采集、误差补偿与姿态角解算的完整流程掌握校准参数的计算与应用思路并对照工程结构理解传感器融合在飞控中的落地方式为自主导航与稳定控制打下基础。1. 无人机九轴传感器标定一个压缩包能省掉多少试飞飞控日志里姿态角总在悬停时缓慢漂移PID 调了三轮还是压不住最后发现根因不在控制环而在九轴传感器没标定干净。加速度计零偏、磁力计硬铁软铁误差、陀螺仪温漂任何一项没处理后面所有算法都在错误的地基上盖楼。UAV021_3-9AxisSensor_Calibrate_Simple.zip就是冲着这个痛点来的一套面向无人机编程篇的九轴传感器标定示例工程把加速度计六面法、磁力计椭球拟合、陀螺仪静态零偏估计串成一条可复现的流程。它适合正在做飞控二次开发、传感器融合调试的从业者也适合刚接触 IMU 标定、想把理论落到代码上的新手。压缩包不大但里面每一步都对应一个真实会翻车的环节下面按我拆包的顺序讲。2. 拆包看结构标定工程里到底放了什么拿到压缩包先别急着编译先看清楚目录组织能省掉后面一半的迷茫。这个工程的组织方式比较典型原始采样数据、标定算法脚本、参数输出文件、验证用的可视化脚本各占一块。我一般会先列一遍文件树确认数据格式和脚本入口再决定从哪个环节切入。2.1 目录与文件职责划分解压后大致会看到这样几类内容不同版本命名可能略有差异以实际为准目录/文件职责使用时机data/或raw/存放加速度计、陀螺仪、磁力计的原始采样标定前先采集或替换calib_accel.*六面法加速度计零偏与尺度因子估计第一步calib_mag.*磁力计椭球拟合输出硬铁偏移与软铁矩阵第二步calib_gyro.*静态零偏估计可与加速度计并行params/或output/标定结果落盘供飞控读取标定完成后verify/或plot_*标定前后对比可视化收尾验证这个划分的意义在于标定不是一次性动作而是「采集—估计—写回—验证」的闭环。把数据和算法分开意味着你可以用同一套脚本反复处理不同批次的采样而不用改代码。常见做法是把每次采集的数据按时间戳命名存进data/标定脚本只认目录不认文件名这样换一批数据直接跑就行。2.2 先确认数据格式再动手在跑任何脚本之前先打开一个采样文件看列定义。九轴数据常见的排列是ax, ay, az, gx, gy, gz, mx, my, mz单位分别是 m/s²、rad/s 或 °/s、高斯或微特斯拉。单位不统一是后面最大的坑之一加速度计按 g 还是 m/s²、陀螺仪按度还是弧度直接决定零偏和尺度因子的量级。我习惯在脚本入口处先做一次单位归一化把所有量统一到 SI 单位再进算法避免中途换算出错。import numpy as np # 读取原始九轴采样假设为逗号分隔、无表头 raw np.loadtxt(data/imu_sample.csv, delimiter,) # 列顺序ax ay az gx gy gz mx my mz acc raw[:, 0:3] # 单位 m/s^2 gyro raw[:, 3:6] # 单位 rad/s若原始为 deg/s 需乘 pi/180 mag raw[:, 6:9] # 单位 uT # 单位归一化若陀螺仪原始为 deg/s取消下面注释 # gyro np.deg2rad(gyro) print(acc mean:, acc.mean(axis0)) print(gyro mean:, gyro.mean(axis0)) print(mag mean:, mag.mean(axis0))这段代码只做一件事把数据读进来、统一单位、打印三轴均值。别小看均值加速度计静止时三轴均值应该接近重力矢量方向上的分量磁力计均值能粗略反映当地磁场强度陀螺仪均值就是静态零偏的初估。如果打印出来的加速度模长离 9.8 差很远先别往下走回头查采集时的放置姿态和单位设置。参数上唯一要改的是列顺序和分隔符跟你的实际文件对齐即可。3. 加速度计六面法零偏和尺度因子怎么估加速度计标定的核心思路很朴素把传感器分别以六个面朝下静置每个姿态下理论重力矢量是已知的实测值和理论值的差异就暴露了零偏和尺度误差。这个方法的门槛不在数学而在采集时的姿态控制和数据质量。3.1 六面法的数学模型设真实加速度为 $a_{true}$实测为 $a_{meas}$一阶模型写成$$a_{meas} S \cdot a_{true} b$$其中 $S$ 是三轴尺度因子构成的对角矩阵忽略轴间耦合时$b$ 是三轴零偏。六个面朝下时$a_{true}$ 分别是 $\pm g$ 沿某一轴、其余两轴为零。把六组方程联立每个轴都能解出一组 $S$ 和 $b$。工程上更常用的是「模长约束法」不管姿态如何静止时加速度模长都应等于当地重力加速度用大量不同姿态的采样拟合出使模长最接近 $g$ 的参数。六面法胜在采样少、可解释性强适合快速上手。我一般会先用六面法拿到初值再用模长约束做一轮精修。六面法的采集要求是每个面静置足够长时间几秒到十几秒取均值消除噪声。姿态没摆正、桌面不水平都会让「理论值」本身带误差这是后面要重点排查的。3.2 采集与估计的完整脚本import numpy as np g 9.80665 # 当地重力加速度按纬度微调 # 六个姿态的实测均值每个姿态静置后取平均 # 顺序X朝下, -X朝下, Y朝下, -Y朝下, Z朝下, -Z朝下 meas np.array([ [ g, 0, 0], [-g, 0, 0], [ 0, g, 0], [ 0, -g, 0], [ 0, 0, g], [ 0, 0, -g], ], dtypefloat) # 实际使用时替换为从 data/ 读取的六组均值 # meas np.array([np.loadtxt(f) for f in six_files]) S np.zeros(3) b np.zeros(3) for axis in range(3): pos meas[2 * axis, axis] # 该轴正向朝下实测 neg meas[2 * axis 1, axis] # 该轴负向朝下实测 S[axis] (pos - neg) / (2 * g) # 尺度因子 b[axis] (pos neg) / 2 # 零偏 print(scale:, S) print(bias :, b) # 标定后校正 def correct_accel(a_raw): return (a_raw - b) / S逻辑说明每个轴用正向和负向两次测量两者之差消掉零偏、留下两倍重力乘尺度因子两者之和消掉尺度项、留下两倍零偏。这是六面法最干净的解法。参数上要注意g的取值低纬度地区用 9.78 左右、中纬度用 9.80 左右差 0.02 对高精度应用是有影响的。S理论上应接近 1如果算出来偏离 1 超过 5%先怀疑采集姿态或单位而不是传感器坏了。校正函数correct_accel就是标定结果的使用方式飞控里读到的原始值先减零偏再除尺度因子。3.3 模长约束精修六面法只用了六个点噪声和姿态误差会被放大。用模长约束做最小二乘精修能把几十组不同姿态的采样都用上from scipy.optimize import least_squares def residual(p, acc_raw): s p[0:3] b p[3:6] a_corr (acc_raw - b) / s return np.linalg.norm(a_corr, axis1) - g # 初值来自六面法 p0 np.concatenate([S, b]) res least_squares(residual, p0, args(acc_all,), methodlm) S_ref, b_ref res.x[0:3], res.x[3:6] print(refined scale:, S_ref) print(refined bias :, b_ref)这里acc_all是包含各种姿态的采样矩阵。残差定义为校正后模长与g之差优化器会同时调整尺度和零偏让残差最小。用lm方法是因为问题维度低、初值已经不错收敛快。如果残差平方和降不下去多半是采样里混进了运动状态的数据静止采集这一条必须守住。4. 磁力计椭球拟合硬铁软铁一次讲透磁力计是九轴里最容易翻车的一环。无人机上电机、电调、电池走线都会产生磁场干扰硬铁干扰是固定偏移、软铁干扰是随姿态变化的缩放和轴间耦合。不标定直接拿磁力计算航向罗盘能偏出几十度飞起来就是画圈。4.1 为什么是椭球而不是球理想情况下传感器绕各个方向旋转采集到的磁场点应分布在一个球面上球心在原点、半径等于当地磁场强度。硬铁干扰把球心平移软铁干扰把球拉成椭球。所以标定的任务就是拟合出一个椭球求出它的球心硬铁偏移和形状矩阵软铁矩阵再把椭球变换回单位球。数学上椭球一般式写成$$(m - c)^T W (m - c) 1$$其中 $c$ 是球心即硬铁偏移$W$ 是正定对称矩阵软铁校正矩阵与 $W$ 的特征分解相关。工程上常用最小二乘直接拟合一般二次曲面再从中提取 $c$ 和 $W$。4.2 椭球拟合脚本与参数提取import numpy as np def fit_ellipsoid(mag): # mag: N x 3 磁力计采样 x, y, z mag[:, 0], mag[:, 1], mag[:, 2] # 一般二次曲面: ax^2by^2cz^22dxy2exz2fyz2gx2hy2iz 1 D np.column_stack([ x*x, y*y, z*z, 2*x*y, 2*x*z, 2*y*z, 2*x, 2*y, 2*z ]) ones np.ones(len(mag)) coef, *_ np.linalg.lstsq(D, ones, rcondNone) a, b, c, d, e, f, g, h, i coef A np.array([[a, d, e], [d, b, f], [e, f, c]]) # 球心 -A^{-1} * [g, h, i] center -np.linalg.solve(A, np.array([g, h, i])) # 软铁矩阵将椭球归一化为球 T np.array([g, h, i]) k 1 center A center # 常数项修正 W A / k # 特征分解得到软铁变换 eigval, eigvec np.linalg.eigh(W) soft eigvec np.diag(np.sqrt(eigval)) eigvec.T return center, soft center, soft fit_ellipsoid(mag_all) print(hard iron offset:, center) print(soft iron matrix:\n, soft) def correct_mag(m_raw): return soft (m_raw - center).T逻辑说明D矩阵把每个采样映射到二次曲面的基函数上最小二乘解出九个系数。A是二次项矩阵球心由线性项和A的逆求出。软铁矩阵通过对W做特征分解再开方得到本质是把椭球各轴缩放到同一尺度。参数上要注意采样必须覆盖足够多的方向只在一个平面转圈拟合出来的椭球在第三维上是病态的lstsq会给出离谱结果。我一般要求采样点覆盖球面的多个象限且每个方向附近都有点。4.3 采样质量决定标定上限磁力计标定的精度几乎完全由采样质量决定算法本身没什么玄学。采集时把无人机拿在手里绕三个轴各转几圈尽量让磁力计朝向覆盖全空间远离电机通电状态和金属桌面。采完之后先画个散点图看分布如果点集中在一小片区域别急着拟合回去重采。常见做法是采几百到上千个点采样率不用高但空间覆盖要全。拟合完成后用校正函数把原始点变换一遍看变换后的模长是否稳定在当地磁场强度附近波动超过几个百分点就说明软铁矩阵没拟合好。5. 避坑与排查标定流程里最容易翻车的五件事5.1 标定后姿态反而更飘现象跑完标定写回参数悬停时姿态漂移比标定前还明显。原因多半是加速度计尺度因子符号搞反或者校正公式写成了a*S b而不是(a-b)/S。六面法里正向和负向的减法顺序一旦颠倒S会变成负值校正后重力方向整个翻转。解决标定完先做静态验证静止放置时校正后的加速度模长应接近g、方向应指向重力方向不满足就回头检查公式和符号。5.2 磁力计拟合报矩阵奇异现象np.linalg.solve抛奇异矩阵错误或解出的球心数值离谱。原因是采样点共面或近似共面二次项矩阵A秩亏。手持旋转时如果只绕竖直轴转、没有俯仰和横滚就会出这个问题。解决重采强制覆盖三个轴向的旋转或者在拟合前做主成分分析确认三个主方向都有足够方差再进最小二乘。5.3 陀螺仪零偏随温度跑现象冷机标定的零偏飞了几分钟后姿态积分明显偏移。陀螺仪零偏对温度敏感室温静态标定只能拿到某个温度点的值。解决如果工程支持做温补表在不同温度点各采一段静态数据拟合零偏随温度的变化不支持温补的话至少在上电后静置预热一段时间再采零偏别冷机直接标。这是血泪经验冷机标出来的零偏在飞行中段基本不可信。5.4 单位混用导致参数量级错误现象标定输出的零偏数值大得离谱或者校正后数据完全失真。九轴数据里加速度计可能是 g 也可能是 m/s²陀螺仪可能是度也可能是弧度磁力计可能是高斯也可能是微特斯拉。解决在脚本入口统一单位并在标定前后各打印一次数据范围做交叉验证。我习惯在参数文件里显式写单位注释避免下次换数据时又踩一遍。5.5 把运动数据混进静态标定现象加速度计模长约束的残差降不下去磁力计椭球拟合的残差分布很散。原因是采集时手抖、无人机没完全静止或者采集脚本在电机转动时还在记录。解决采集阶段加一个静止判据比如连续若干帧的角速度模长低于阈值才记录标定前对数据做一遍粗筛剔除模长明显偏离的离群点。这个黑匣子不开后面调参全是白费。6. 验证与写回让标定结果真正进飞控标定算出参数只是半程验证和写回才是闭环。我一般分两步验证先离线用校正函数处理一遍原始数据看统计指标再把参数按飞控要求的格式写进配置文件上机做静态和低速验证。6.1 离线验证的量化指标指标计算方式合格参考加速度模长均值校正后norm(a)均值接近当地g偏差 1%加速度模长标准差校正后norm(a)标准差越小越好反映噪声与残差磁力计模长波动校正后norm(m)的相对标准差 3% 视为拟合良好陀螺仪静态均值静止段角速度均值接近零残余即零偏a_corr correct_accel(acc_all) m_corr correct_mag(mag_all).T print(acc norm mean/std:, np.linalg.norm(a_corr, axis1).mean(), np.linalg.norm(a_corr, axis1).std()) print(mag norm rel-std:, np.linalg.norm(m_corr, axis1).std() / np.linalg.norm(m_corr, axis1).mean())这几个数字比任何主观判断都可靠。加速度模长均值偏离g超过 1%说明尺度或零偏还有系统误差磁力计模长相对标准差超过 3%说明软铁矩阵没拟合到位或者采样覆盖不够。别跳过这一步直接上机空中发现问题代价太大。6.2 写回飞控的参数格式不同飞控的参数组织方式不一样常见做法是把零偏和尺度因子写成数组磁力计写成偏移向量加 3x3 矩阵。写回时注意两点一是单位与飞控内部一致二是参数顺序与飞控读取顺序一致。我习惯在写回脚本里加一个回读校验写完立刻读出来和内存里的值比对防止格式转换时静默截断。参数文件里保留标定日期和采集条件注释下次排查时能对上号。6.3 上机验证的顺序先静态无人机放平看姿态角是否稳定、航向是否随转动平滑变化。再低速手动转动看姿态跟随有没有滞后或跳变。最后才进飞行验证。每一步都记录日志出问题能回溯到是哪个环节的参数不对。从那以后我每次标定完都强制走一遍「离线指标—静态—低速—飞行」这条链哪怕时间紧也不跳步因为跳过的那一步往往就是后面炸机报告的根因。希望这套流程帮到你少走几轮试飞的弯路。本文还有配套的精品资源点击获取