第4章磁力计校准实战

第4章：磁力计校准实战——椭球拟合校准法，基于最小二乘的硬铁/软铁补偿

各位同学，欢迎来到磁力计校准的实战环节。

说实话，在惯性导航这个行当里摸爬滚打这么多年，我见过太多因为磁力计没校准好，导致整个姿态系统崩掉的案例。偏航角漂移，说白了，十有八九是磁力计没伺候好。今天咱们就来彻底解决这个问题。

4.1 为什么磁力计需要校准？

你想想看，一个理想的磁力计，在干净的环境里旋转一圈，它输出的三轴数据应该落在一个球面上。球心在原点，半径就是当地地磁场强度。

但现实很骨感。电路板上的电流、旁边的马达、甚至螺丝刀，都会干扰它。这些干扰分为两类：

硬铁干扰：由永磁体或固定电流产生。它像一个固定的偏置，把球心从原点推走了。
软铁干扰：由铁磁性材料（比如钢壳）产生。它会扭曲磁场，把球变成椭球。

所以，我们校准的目标就两个：把球心拉回原点，把椭球捏回球。

核心结论：未经校准的磁力计，偏航角误差可能达到30度以上。校准后，可以控制在1-2度以内。

4.2 椭球拟合的数学原理

嗯，这里要稍微上点数学了。不过别怕，我会用最直白的话讲清楚。

一个椭球方程长这样：

(x - x0)² / a² + (y - y0)² / b² + (z - z0)² / c² = 1

其中 (x0, y0, z0) 是椭球中心，a、b、c 是三个轴的长度。我们的任务就是：从一堆测量点里，算出这6个参数。

怎么算？最小二乘法。说白了就是：找一组参数，让所有测量点到这个椭球面的距离平方和最小。

我个人习惯把问题转化成线性形式。把椭球方程展开，整理成：

Ax² + By² + Cz² + Dx + Ey + Fz + G = 0

然后，用最小二乘解这个线性方程组。解出A到G，再反算出椭球中心和半径。我在项目中遇到过，直接用非线性优化容易陷入局部最优，线性化之后稳定多了。

4.3 实战：采集数据

校准的第一步，是采集足够多的数据。你需要拿着设备，在空间里做各种姿态旋转。

我建议你这样操作：

把设备固定在一个非铁磁性平台上（比如塑料板）
缓慢旋转，覆盖所有角度
采集至少200个点，越多越好
确保数据点均匀分布在球面上

注意：采集时远离大型金属物体和强磁场源。我曾经在实验室里采集数据，结果旁边有个铁皮柜子，校准出来全是错的。找了半天才发现是柜子的问题。

4.4 实战：最小二乘拟合代码

下面是我常用的Python代码片段。它接受原始磁力计数据，返回校准参数。

import numpy as np

def ellipsoid_fit(data):
    """
    椭球拟合，基于最小二乘法
    data: Nx3 的原始磁力计数据
    返回: 中心 (cx, cy, cz), 半径 (rx, ry, rz)
    """
    x = data[:, 0]
    y = data[:, 1]
    z = data[:, 2]
    
    # 构造矩阵 A 和向量 b
    A = np.column_stack([x**2, y**2, z**2, x, y, z, np.ones_like(x)])
    b = -np.ones_like(x)
    
    # 最小二乘解
    params, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
    
    # 提取参数
    A, B, C, D, E, F, G = params
    
    # 计算椭球中心和半径
    cx = -D / (2 * A)
    cy = -E / (2 * B)
    cz = -F / (2 * C)
    
    rx = np.sqrt((D**2/(4*A**2) + E**2/(4*B**2) + F**2/(4*C**2) - G/A))
    ry = np.sqrt((D**2/(4*A**2) + E**2/(4*B**2) + F**2/(4*C**2) - G/B))
    rz = np.sqrt((D**2/(4*A**2) + E**2/(4*B**2) + F**2/(4*C**2) - G/C))
    
    return (cx, cy, cz), (rx, ry, rz)

小技巧：如果数据点分布不均匀，可以在拟合前对数据进行加权。靠近球心的点权重小一点，远离球心的点权重大一点。这样拟合出来的椭球更稳定。

4.5 硬铁/软铁补偿

拟合出椭球参数后，补偿就很简单了：

def calibrate_mag(raw, center, radius):
    """
    磁力计校准
    raw: 原始数据 (Nx3)
    center: 椭球中心 (cx, cy, cz)
    radius: 椭球半径 (rx, ry, rz)
    返回: 校准后的数据
    """
    # 减去硬铁偏置
    centered = raw - center
    
    # 缩放软铁畸变
    scaled = centered / radius
    
    return scaled

说白了，就是两步：先平移，再缩放。平移消除硬铁干扰，缩放消除软铁干扰。

校准后的数据，应该落在一个单位球面上。你可以画个图验证一下：

原始数据：散乱分布，中心不在原点
校准后数据：均匀分布在单位球面上

4.6 知识体系总览

下面这张图，帮你理清整个校准流程的逻辑：

4.7 避坑指南

做磁力计校准，有几个坑我踩过，分享给你：

数据量不够：少于50个点，拟合出来的椭球可能严重失真。我建议至少200个点。
姿态覆盖不全：如果只在一个平面内旋转，数据点会落在一个圆环上，无法拟合出椭球。一定要做全姿态旋转。
动态采集：不要在快速运动时采集数据。加速度干扰会混入磁力计读数。慢慢转，稳着来。
温度变化：磁力计对温度敏感。如果工作温度范围大，最好在不同温度下分别校准。

我曾经在一个无人机项目上，校准参数在实验室里跑得好好的，一到户外就偏。查了两天才发现，是户外温度比实验室低了15度，磁力计偏置变了。从那以后，我都在目标温度环境下做校准。

4.8 校准效果评估

校准完了，怎么知道好不好？我一般看三个指标：

指标	计算方法	合格标准
中心偏移量	sqrt(cx² + cy² + cz²)	< 0.1 (归一化后)
半径一致性	max(rx, ry, rz) / min(rx, ry, rz)	< 1.1
残差	平均点到球面距离	< 0.05

如果这三个指标都达标，恭喜你，校准成功了。偏航角漂移的问题，基本可以根治。

最后说一句：磁力计校准不是一劳永逸的。如果设备结构变了（比如换了外壳），或者工作环境变了，记得重新校准。我一般会在产品出厂前做一次，用户使用半年后建议再做一次。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录