ArduPilot开源飞控之MagFit工具

1. 源由

懒人有懒福，问题是为了偷懒，为了远航，为了更加安全，首先面对的就是一堆坑！
所以，为了“偷懒”，必须先勤奋，这也是我们一贯的作风。为了达成目的，我们始终在不断学习，理解问题，解决问题。

鉴于：

1. 上次inav炸鸡，很早的事情，从逻辑上来说应该已经fix了，但是没有再去尝试过。
2. 穿越机的手动优雅降落，对于操作不太熟练的我，可能有些许困难，所以希望能LAND自动降落。
3. 随着装上飞机的东西越来越多，飞机越来越贵重，还是需要各种异常，能安全返航。
4. 远航看看风景也是一种享受，为了安全、可靠、自由飞行，还是需要确保自动返航。

所有这些问题，在户外需要GPS定位，Ardupilot在导航定位方面，离不开磁力计。

• 磁力计提供磁北方向 → 结合GPS真北数据与实时磁偏角 → 解算准确航向。
• 磁极加速漂移要求固件定期更新磁偏角参数。

本章就是折腾这个磁力计校准的问题。由于地球磁场受到众多干扰源影响，磁力计的校准始终是小型机上最难校准的部件之一。

2. 磁场来源

所有问题的解决，最重要的首先是要理解问题。而理解问题最重要的是学习已知知识。

地球磁场的形成是一个复杂的地球物理过程，主要与地球内部的液态外核运动相关。这一现象被称为 “地核发电机理论”（Dynamo Theory）。

2.1 地球外核感应电流

关键：液态外核的流动是地球磁场产生的核心。

2.1.1 地球内部从外到内分层结构

• 地壳（固态岩石）
• 地幔（固态但可塑性流动）
• 外核（液态铁、镍及少量轻元素，如硫、氧）
• 内核（固态铁镍合金，温度约5700°C，但因高压保持固态）

2.1.2 当液态金属（如地球外核的铁镍合金）地球自转受到科里奥利力

• 宏观呈中性：金属由正离子晶格和自由电子组成，正负电荷总量相等
• 微观可动性：自由电子可在金属内部自由移动，而正离子晶格相对固定；受到洛伦兹力作用，

(1) 自由电子受洛伦兹力作用

• 洛伦兹力公式：
  $$\mathbf{F}=q(\mathbf{v}\times \mathbf{B})$$

  • $q$：电荷（自由电子带负电 $q=-e$）
  • $\mathbf{v}$：金属整体运动速度
  • $\mathbf{B}$：外部磁场

• 电子偏转：自由电子因洛伦兹力向金属某一侧聚集（方向由 $\mathbf{v}\times \mathbf{B}$ 决定）。

(2) 电荷分离与电场建立

• 电荷分离：电子聚集导致金属内部出现电势差（一端电子过剩带负电，另一端正离子暴露带正电）。
• 电场形成：电荷分离产生电场 $\mathbf{E}$，方向与电子受力的洛伦兹力相反。

(3) 动态平衡与电流产生

• 平衡条件：当电场力 $q\mathbf{E}$ 与洛伦兹力平衡时，电荷停止分离：
  $$q\mathbf{E}=-q(\mathbf{v}\times \mathbf{B})\Rightarrow \mathbf{E}=-\mathbf{v}\times \mathbf{B}$$
• 感应电流：在闭合回路中，电场驱动电子流动形成电流（若金属孤立，则仅存在内部电势差而无宏观电流）。

2.1.3 数学简化模型

考虑一段液态金属以速度 $\mathbf{v}$ 在磁场 $\mathbf{B}$ 中运动：

• 感应电场：$\mathbf{E}=-\mathbf{v}\times \mathbf{B}$
• 电流密度：$\mathbf{J}=\sigma \mathbf{E}=-\sigma (\mathbf{v}\times \mathbf{B})$
  （$\sigma$ 为电导率）
• 磁场增强：电流产生的新磁场通过麦克斯韦方程与原有磁场叠加，形成自激发电机效应。

2.2 地核发电机效应的原理

想象地球内部有一台巨大的 “液体发电机”：

• 燃料：内核热量 + 放射性衰变 → 驱动液态金属流动。
• 涡轮：地球自转 → 扭转流体路径。
• 导线：导电流体（液态铁镍）切割磁感线 → 产生电流。
• 输出：电流生成磁场，磁场又反馈增强流体运动，形成自持循环。

2.2.1 对流驱动

• 热量来源：
  • 内核冷却释放热量（原始形成余热）。
  • 放射性元素（如铀、钍、钾）衰变产热。
  • 液态外核凝固释放潜热（轻元素上浮，铁镍下沉至内核）。
• 对流运动：热量差异导致液态金属上升、冷却后下沉，形成循环对流。

2.2.2 地球自转与科里奥利力

• 地球自转使对流流体路径发生偏转（类似台风旋转），形成螺旋状的流体运动（“泰勒柱”）。
• 这种螺旋运动切割原有的微弱磁场，诱导电流（法拉第电磁感应定律）。

2.2.3 电流产生磁场

• 电流与运动的导电流体相互作用，通过 “自激发电机” 过程放大初始磁场，形成稳定的地球磁场。
• 该过程类似电动机：流体运动（动能）→ 电磁感应（电能）→ 磁场（磁能）。

2.3 地球磁场的动态特性

• 磁极漂移 → 磁偏角变化 → 导航系统需动态校准。
• 磁极翻转 → 磁偏角混乱 → 传统磁导航短期失效。
• 磁场屏蔽 → 维持地磁方向基准 → 真北-磁北关系稳定（长期）。

2.3.1 磁极漂移

• 现象：地核流体运动导致磁北极持续移动，近年速度加快至 约60公里/年（1900年至今已移动超过1000公里）。
• 对磁偏角的影响：
  • 磁北极移动 → 同一地点的 磁偏角（磁北与真北的夹角）持续变化。
  • 实例：北美洲部分地区的磁偏角每年变化约0.1°~0.2°。
• 导航修正：航空、航海需定期更新磁偏角数据（如依赖 世界地磁模型WMM）。

2.3.2 磁极翻转

• 周期：平均每20万~100万年发生一次极性反转（最近一次为 78万年前的松山-布容倒转）。
• 翻转期间影响：
  • 磁场强度下降至约10% → 磁偏角剧烈波动，导航系统失效。
  • 磁层保护减弱 → 地表辐射增强，可能引发生物适应挑战。

2.3.3 磁场屏蔽作用

• 磁层结构：地球磁场延伸至太空形成保护层，偏转太阳风带电粒子。
• 与真北的关联：
  • 磁层稳定性依赖磁场方向 → 若磁极快速漂移或翻转，可能导致磁层畸变，间接影响极区导航（如极地航路依赖磁北修正）。

2.3.4 磁偏角与真北方向（新增）

• 定义：
  • 真北（地理北极）：地球自转轴北极点方向，固定不变。
  • 磁北：磁力计指向的磁北极方向，随磁极漂移动态变化。
  • 磁偏角：真北与磁北之间的夹角（东偏或西偏）。
• 实际意义：
  • 导航校准：指南针、手机地图需根据当地磁偏角修正方向（如中国东部磁偏角约-6°，需向西调整）。
  • 历史记录：古代航海日志的磁偏角数据可反推历史地磁场变化。
• 动态更新：
  • 国际组织每5年发布新版地磁模型（如WMM2020），修正磁偏角参数。

3. 磁场检测

磁场存在
 └──> 磁阻效应（AMR电阻变化）  
     └──> HMC5883电桥输出差分电压  
         └──> 信号处理与数字化  
             └──> 三轴磁场数据  
                 └──> 应用层方向解算  

3.1 磁场检测的核心原理

磁场检测的核心是 将磁场强度或方向转化为可测量的电信号，常用技术包括：

• 霍尔效应：磁场垂直于电流时，导体两侧产生电压差。
• 磁阻效应：材料的电阻随磁场方向或强度变化。
• 磁通门技术：利用磁性材料饱和特性检测交变磁场。

磁阻效应因其高灵敏度和低功耗，成为现代磁力计（如HMC5883）的核心技术。

3.2 磁阻效应（Magnetoresistance）

• 定义 材料的电阻随外加磁场方向或强度变化的物理现象。
• 类型
  • 各向异性磁阻（AMR）：电阻随磁场与电流方向夹角变化（HMC5883采用此技术）。
  • 巨磁阻（GMR）：多层磁性结构中电阻变化显著，用于硬盘磁头。
  • 隧道磁阻（TMR）：量子隧穿效应导致超高灵敏度，用于精密传感器。
• AMR效应原理
  • 材料：坡莫合金（铁镍合金），晶体结构各向异性。
  • 机制：
    • 磁场方向与电流平行 → 电子散射少 → 电阻低。
    • 磁场方向与电流垂直 → 电子散射多 → 电阻高。
  • 输出：惠斯通电桥将电阻变化转为差分电压信号。

3.3 HMC5883磁力计的工作流程

HMC5883是一款基于Honeywell’s Anisotropic Magnetoresistive (AMR)效应的三轴数字磁力计，其工作原理如下：

1. 传感单元：

• 三个正交的AMR传感器（X/Y/Z轴），分别检测对应方向的磁场分量。
• 每个AMR单元通过惠斯通电桥输出差分电压。

2. 信号处理：

• 放大与滤波：低噪声放大器提升信号强度，滤波电路抑制高频噪声。
• 模数转换（ADC）：将模拟电压转为数字信号（如12位分辨率）。

3. 数据输出：

• 通过I²C接口输出三轴磁场数据（单位：高斯或特斯拉）。
• 内置温度补偿电路，减少热漂移误差。

4. 校准与计算：

• 用户需进行硬铁/软铁校准，消除固定偏移和轴间干扰。
• 结合加速度计数据（倾斜补偿）计算真实航向角。

5. 实际应用中的关键问题

• 校准需求：
  • 硬铁干扰：设备内永磁体导致固定偏移，需减法校准。
  • 软铁干扰：外部铁磁材料扭曲磁场，需矩阵校正（如椭圆拟合）。
• 温度影响：
  • AMR材料电阻随温度变化，HMC5883内置补偿电路仍需避免高温环境。
• 多传感器融合：
  • 与陀螺仪、加速度计数据融合（如卡尔曼滤波），提高航向精度。

4. 校准方法

Ardupilot的MagFit通过灵活组合偏移、比例、软铁矩阵、电机干扰和加权策略，实现了复杂环境下的高精度磁力计校准。其核心是通过扩展设计矩阵 $\mathbf{A}$ 和参数向量 $\mathbf{x}$，将非线性问题转化为线性最小二乘问题，最终通过数值方法求解全局最优参数。

4.1 Offsets Only（仅偏移校准）

目标：消除硬铁干扰（固定磁场偏移）。
公式：
$${\mathbf{B}}_{\text{corrected}}={\mathbf{B}}_{\text{raw}}-\text{offset}$$
参数求解：

• 构造线性方程组：
  $$\mathbf{A}\cdot \mathbf{x}=\mathbf{b}$$
  其中：
  • $\mathbf{A}$ 是设计矩阵，每行对应一个数据点的 $[1,0,0]$（X轴）、$[0,1,0]$（Y轴）、$[0,0,1]$（Z轴）。
  • $\mathbf{x}=[{\text{offset}}_x,{\text{offset}}_y,{\text{offset}}_z{]}^T$
  • $\mathbf{b}$ 为理想磁场模长约束（$\Vert {\mathbf{B}}_{\text{corrected}}\Vert =\Vert {\mathbf{B}}_{\text{earth}}\Vert$）。
• 通过最小二乘法（SVD或QR分解）求解 $\mathbf{x}$。

4.2 Offsets and Scale（偏移与比例因子校准）

目标：校正硬铁偏移和各轴灵敏度差异（比例误差）。
公式：
$${\mathbf{B}}_{\text{corrected}}=\mathbf{S}\cdot ({\mathbf{B}}_{\text{raw}}-\text{offset})$$
其中 $\mathbf{S}=\text{diag}(s_x,s_y,s_z)$ 为对角比例矩阵。
参数求解：

• 扩展设计矩阵 $\mathbf{A}$：
  $$\mathbf{A}=\left[\begin{array}{cc}\mathbf{I}& \text{diag}({\mathbf{B}}_{\text{raw}})\end{array}\right]$$
  • $\mathbf{I}$ 为3×3单位矩阵（对应偏移项）。
  • $\text{diag}({\mathbf{B}}_{\text{raw}})$ 为数据点磁场值的对角矩阵（对应比例因子项）。
• 参数向量 $\mathbf{x}=[{\text{offset}}_x,{\text{offset}}_y,{\text{offset}}_z,s_x,s_y,s_z{]}^T$。
• 通过约束 $\Vert {\mathbf{B}}_{\text{corrected}}\Vert =\Vert {\mathbf{B}}_{\text{earth}}\Vert$ 构建最小二乘问题求解。

4.3 Offsets and Iron（偏移与软铁矩阵校准）

目标：校正硬铁偏移和软铁干扰（轴间耦合、非正交性）。
公式：
$${\mathbf{B}}_{\text{corrected}}=\mathbf{M}\cdot ({\mathbf{B}}_{\text{raw}}-\text{offset})$$
其中 $\mathbf{M}$ 是3×3软铁校正矩阵（包含比例和交叉项）。
参数求解：

• 扩展设计矩阵 $\mathbf{A}$：
  $$\mathbf{A}=\left[\begin{array}{cccc}\mathbf{I}& {\mathbf{B}}_{\text{raw},x}\cdot \mathbf{I}& {\mathbf{B}}_{\text{raw},y}\cdot \mathbf{I}& {\mathbf{B}}_{\text{raw},z}\cdot \mathbf{I}\end{array}\right]$$
  • $\mathbf{I}$ 为3×3单位矩阵，每个磁场分量展开为矩阵块。
• 参数向量 $\mathbf{x}=[{\text{offset}}_x,{\text{offset}}_y,{\text{offset}}_z,m_{11},m_{12},m_{13},\dots ,m_{33}{]}^T$（共12个参数）。
• 通过最小二乘法求解，约束校正后磁场模长恒定。

4.4 Motor Calibration（电机干扰校准）

目标：补偿电机旋转产生的动态磁场干扰（如无人机电机电流变化）。
公式：
$${\mathbf{B}}_{\text{corrected}}=\mathbf{M}\cdot ({\mathbf{B}}_{\text{raw}}-\text{offset})+\mathbf{P}(r)$$
其中 $\mathbf{P}(r)$ 是电机转速 $r$ 的多项式函数，例如：
$$\mathbf{P}(r)={\mathbf{k}}_0+{\mathbf{k}}_{1}r+{\mathbf{k}}_2{r}^2$$
参数求解：

• 扩展设计矩阵 $\mathbf{A}$：
  $$\mathbf{A}=\left[\begin{array}{cc}{\mathbf{A}}_{\text{previous}}& \mathbf{R}\end{array}\right]$$
  • ${\mathbf{A}}_{\text{previous}}$ 为前三种校准方法的设计矩阵。
  • $\mathbf{R}$ 包含电机转速 $r$ 的多项式项（如 $[1,r,r^2]$）。
• 参数向量 $\mathbf{x}$ 包含原有参数和电机补偿系数 ${\mathbf{k}}_0,{\mathbf{k}}_1,{\mathbf{k}}_2$。
• 需在电机不同转速下采集数据，联合求解所有参数。

4.5 Weights（加权校准）

目标：根据姿态频率分配权重，提升校准鲁棒性（避免某些姿态数据不足导致过拟合）。
公式：
在最小二乘问题中引入权重矩阵 $\mathbf{W}$：
$$\Vert \mathbf{W}\cdot (\mathbf{Ax}-\mathbf{b}){\Vert }^2\to \text{min}$$
权重分配：

• 每个数据点的权重 $w_i$ 反比于该姿态的出现频率。
• 例如：若姿态角 $({\theta }_i,{\varphi }_i)$ 在数据集中出现次数少，则赋予更高权重 $w_i=1/(p({\theta }_i,{\varphi }_i)+ϵ)$，其中 $p$ 为姿态概率密度，$ϵ$ 防止除零。

5. 实测

Ardupilot Web Tool

• MagFit Tool
• MagFit Source

5.1 问题汇总

早期问题：

• Why “error compass variance” when flip?
• Copter 4.4.0 “Error Compass Variance” during a big turn in mission, any idea?

近期问题：

• “EKF3 IMU1 MAG0 IN-FLIGHT YAW ALIGNMENT” makes auto yaw when takeoff
• Why magfit’s output result is out of parameter range?
• EKF3 IMU1 MAG0 ground mag anomaly, yaw re-aligned

延伸问题：

• Why home icon is flicking, not locked?
• Does ardupilot 4.6.x support RTL without compass? - Graceful Degraded RTL

5.2 改善：多次（>10）测试，飞行超5min

校准步骤：

1. 通过Mission Planner配置磁力计（尤其是方向COMPASS_ORIENT）
2. 通过Mission Planner进行简单校准（无电机干扰，室内环境）
3. 通过简单绕圈+八字飞行，持续3~5分钟，记录飞行日志
4. 使用MagFit对飞行日志进行校准

未见明显效果！

5.3 改善：核对之前校准数据，重新通过MP校准

以前Betaflight中的CW90FLIP，而目前Ardupilot发现系统上显示是YAW180。

重新在Ardupilot中设置对应的Yaw90Roll180，校准成功以后，实飞后，系统自动修改为了YAW180。*

注：笔者使用的是BN-880模块，该模块之前似乎并并不存在磁力计参数校准超限情况。

7. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot开源代码之AP_GPS
【3】ArduPilot之GPS Glitch问题&M8N模块配置
【4】ArduPilot Kakute F7 AIO DIYF450 之GPS配置
【5】BN880 GPS u-center_v22.07工具配置方法
【6】Betaflight BN880 GPS 简单测试
【7】BetaFlight Mark4 H7 Dual270 + BN880 + CRSF 配置存档