引言

磁钉定位过程中会受到环境干扰磁场(包括地球磁场和外部磁场干扰源)的影响。地磁辐射范围广，阵列中所有传感器受到的干扰基本一致;外部磁场干扰源，如通电导线、小磁铁、电机、铁磁性材料等，辐射范围一般有限，离干扰源近的传感器受影响大，离干扰源远的传感器受影响小。根据干扰源对阵列上传感器的影响程度，可分为局部传感器受到干扰和全局传感器受到干扰。

一、基于图优化和鲁棒核函数的抗磁场干扰方法

图优化是图论和优化理论的结合，是解决位置同时排列和图构建问题的流行方法，在其他领域也有很多应用。图优化的优点在于可以将复杂的问题以因子图的形式表现出来，因此更直观。因子图是由表示估计变量的节点和对应于约束的边组成的。一旦因子图被建立，标准的优化方法(例如gaus-newton，LM)，可以最大限度地调整节点以符合限制条件。因此，可以把精力放在因子图的制作上。

1、磁钉定位问题的图形式表示

如图1所示，我们使用因子图表示磁钉定位问题，其中圆圈表示变量节点（VariableNodes），正方形表示因子节点（FactorNodes）。变量节点表示待优化的参数，即不同时间下所对应的磁铁姿态。

因子节点表示约束，即磁传感器的观测值和待优化参数的内在约束。边（Edges）将变量节点与因子节点关联起来，表示作用于待优化参数的约束。

因子图是一种二分图，可表示为

。该因子图由因子节点集合ℱ、变量节点集合V和一系列边ℰ组成。

其中，因子节点H0∈ℱ得到的内部约束，因子节点Sl∈ℱ(l=1,2,…,N)由第l个磁传感器的观测值得到。变量节点vt∈V表示t时刻待估计的参数（即磁钉位姿），而边ℰ表示因子节点和变量节点的关系。对于磁钉定位方法，这些关系式代表了磁传感器的测量约束和磁钉姿态的内在约束。

图1磁钉定位方法的因子图

测量约束是期望观测值Bl=(Blx,Bly,Blz)和真实观测值Sl=(Slx,Sly,Slz)之间的残差，如第l个磁传感器的测量约束为

（公式1）

磁钉姿态的内在约束为

（公式2）

换句话来说，elx(v),elx(v),elz(v)和g(v)对应于因子图的边。磁钉姿态的内在约束是固定的，在整个定位过程都一直存在，而测量约束随磁铁姿态的变化而变化。

2、鲁棒最小二乘问题

根据因子图，我们可以得到损失函数

（公式3）

它可以重写为如下形式

（公式4）

式中，M=3×N+1，ρ2是二次函数，即ρ2(x)=x2。所以，残差ei(v)对损失函数E(v)的作用呈二次方。因此，某个潜在的异常观测值（离群点）会对磁钉位姿估计产生显著的负面影响。对于因子图，异常观测值对应于图的异常边。

当磁传感器受到干扰源（如小磁铁）干扰时，图中可能出现异常边缘。为了提高对异常边的鲁棒性，引入一个鲁棒核函数来代替二次函数ρ2。在本研究中，使用Huber函数ρH，

（公式5）

式中，δ是Huber函数的阈值。核函数ρH对较小的|x|值呈二次方，而对较大的|x|值呈线性。此外，Huber核函数仍然是一个凸函数，没有在损失函数E(v)中引入新的局部极小值。

将Huber核函数应用到公式4中，得到鲁棒型损失函数

（公式6）

其中，权值wi的计算公式为

（公式7）

最后，通过最小化鲁棒型损失函数，可以估计出磁钉的位姿v，即

（公式8）

当磁传感器阵列获得新的传感器值时，将启动新的优化过程以获得最新的尖峰位置。优化过程相当于找到尽可能符合所有限制条件的变量节点v。下面，将说明解决公式8的最优化方法。

3、目标函数误差最小化

根据公式6，鲁棒型损失函数可以重写为如下形式

（公式9）

其中fi(v)=wi∙ei(v)。

公式8可以用LM最优化算法求解。假设待优化参数的初始估计值为vˆ，则误差函数f(v)可由泰勒展开式近似为

（公式10）

式中，J(vˆ)是误差函数f(v)的雅可比矩阵，将公式10代入到公式9中，得到

（公式11）

让f(vˆ)TJ(vˆ)=g，J(vˆ)TJ(vˆ)=H，有

（公式12）

在上式所示的局部近似下，Δv*可通过求解线性系统获得

（公式13）

通过上式求解Δv*就是所谓的Gauss-Newton法，而LM算法则是引入了一个阻尼参数λ到公式13中来控制收敛速度，即Δv*通过求解下式获得

（公式14）

最后，将Δv*与初始估计值相加，可得到迭代一次后的解，即

（公式15）

在迭代过程中，λ根据以下策略进行更新

（公式16）

式中，ρ是近似模型与实际函数之差确定的增益比，即

（公式17）

当ρ值比较大时，表示L(Δv)是EH(v+Δv)的良好近似值，这种情况下λ值将会变小。当ρ比较小（甚至为负值）时，表示L(Δv)与EH(v+Δv)相差比较大，这种情况下λ值将会增大。

4、实验结果及讨论

本文提出的基于图优化和稳健性核心函数的定位算法，目的是验证抗干扰能力。因此，在这个实验中，不限制别针在空间中的运动维度，让别针在空间中以6维的速度运动。

1、 采用原生标准的LM算法的磁钉定位算法（MPA_LM）。

2、 本研究提出的基于图优化和鲁棒核函数的抗磁场干扰方法（MPA_GO），该方法在g2o(generalgraphoptimization)图优化框架实现。Huber核函数的阈值通过实验确定，最终设置为2×10-6。

4.1无磁干扰情况下两种定位方法精度对比

实验中，利用标定板对磁钉定位系统的精度进行了测试。磁钉到磁传感器阵列在Z轴方向上的距离分别设为66mm和86mm。在每个距离处，均匀选择25个测量点。各测量点的位置和姿态误差按公式计算，结果如图2所示。

图2两种定位方法在没有磁干扰情况下的定位精度。(a)MPA_LM和MPA_GO的定位误差;(b)MPA_LM和MPA_GO的姿态误差

从结果可以看出，MPA_LM和MPA_GO的精度是相似的，即在没有磁干扰的情况下，两者都能获得优异的定位性能。

4.2不同初始估计值情况下两种定位方法收敛时间对比

为了进行比较，两种定位算法都运行在嵌入式处理器JetsonTX2(NVIDIA，美国)上。收敛时间是指尖峰定位算法从初始估计值到全局最优解所需的时间。将磁铁放置在位姿(a,b,c,m,n,p)=(0,0,86mm,0,0,1)的位置上，分别对MPA_LM和MPA_GO指定了不同的初始估计。具体的初始估计值和结果如表1所示。

从表1可以看出，MPA_GO的收敛时间比MPA_LM的长，但是MTA_GO的收敛时间小于40ms（即磁定位系统的输出频率大于25HZ），可以满足大多数应用的需求。

表1MPA_LM和MPA_GO收敛时间对比

4.3鲁棒核函数阈值确定

MPA_GO的抗干扰能力受Huber函数阈值的影响。因此，实验中我们用一个小磁铁作为磁场干扰源，并用不同的Huber函数阈值测试磁钉定位方法在有磁场干扰环境下的定位精度，如图3所示。其中，待定位的磁钉以姿态为(a,b,c,m,n,p)=(0,0,86mm,0,0,1)放置。当一块小的钕铁硼磁体（等级：N35，Φ3×3mm）离第1个传感器的距离为30mm（或40mm）时，在第1个传感器处产生约84×10-6T（或35×10-6T）的磁场。

图3有磁场干扰的实验环境

图4MPA_GO的定位精度随阈值的不同而变化（小磁铁与第1个传感器的距离为30mm）；(a)MPA_GO在不同阈值情况下的位置误差；(b)MPA_GO在不同阈值情况下的姿态误差

图5MPA_GO的定位精度随阈值的不同而变化（小磁铁与第1个传感器的距离为40mm）；(a)MPA_GO在不同阈值情况下的位置误差；(b)MPA_GO在不同阈值情况下的姿态误差

由图4可知，当阈值δ为1×10-6时，MTA_GO对磁场干扰强度为84×10-6T的抗干扰性能最好。与图5的结果进行对比可知，不同的磁场干扰强度对阈值的选取影响不大，都使MPA_GO达到较佳的抗干扰能力。

此外，在阈值为1×10-6的附近，对MTA_GO的迭代次数进行了测试，优化算法的初始估计值设为（30mm,30mm,10.6mm,0,0,1），实验结果见表2。

表2不同阈值下MTA_GO的迭代次数

由表2的结果可知，当阈值为1×10-7时，迭代次数明显增加，主要原因是残差ei(v)对损失函数E(v)的影响是线性的，而不是二次方的。

综合图4、图5和表2的结果，我们将MPA_GO的Huber函数阈值设为2×10-6。

4.4两种定位方法抗磁干扰能力对比

如图3所示，我们将小磁铁作为磁干扰源放在离第1个磁传感器40mm远的地方。图6显示了MPA_GO的定位精度，其中前25个样本是在目标磁铁和传感器阵列之间的距离为66mm时采集的，后25个样本是在距离为86mm时测量的。

图6两种定位方法在没有磁干扰情况下的定位精度。(a)MPA_LM和MPA_GO的定位误差;(b)MPA_LM和MPA_GO的姿态误差

由图6的结果可知，当存在磁场干扰时，MPA_GO的定位精度优于

MPA_LM，即MPA_GO的抗干扰性能优于MPA_LM。

4.5实验结果讨论

图优化是图论和优化理论的组合，也可以认为是优化算法。因此，MPA_LM和MPA_GO均可在没有磁场干扰的情况下达到同样的定位精度。基于图优化和鲁棒核函数的磁钉定位算法有两个优点：1、MPA_GO可以用图形表示，这使得它更直观；2、由于采用Huber核函数，MPA_GO具有更好的抗干扰性能。根据先验知识增加对BT、位置分量p和姿态分量c约束，这些约束在整个定位过程都存在，和H0的内在约束一致，因此该定位算法可用如图7所示的超图来表示。

图7磁钉定位方法的因子图

其次，MPA_GO算法的收敛时间比MPA_LM算法长。原因之一是MPA_GO需要额外的计算资源来将因子图转换为目标函数；另一个原因是MPA_GO的实现是基于g2o框架的。g2o具有很强的通用性和可扩展性，但降低了其运行效率。但是，我们可以通过以下方法缩短MPA_GO的收敛时间：

1、 采用更高性能处理器，如IntelCorei7处理器。本研究的结果是基于低功耗、低运算能力的嵌入式处理器。

2、 提供一个更接近真实值的初始估计值。在实时跟踪过程中，磁钉定位算法采用热启动方式，即当前定位的结果作为下一次定位迭代的初始估计值。因此，在实际应用中，收敛时间小于40ms。

最终当一个或多个传感器受到干扰时，pm_go能够正常工作。图优化的目标是尽可能找到满足所有限制条件的变量节点的参数值。当一个磁传感器受到干扰源的干扰时，被干扰的传感器的观测值会与其他传感器的观测值有很大差异，并在图表中出现潜在的异常边缘。Huber损失函数通过减少异常边缘的权重来降低异常边对定位结果的负面影响。但是，如果干扰源对所有传感器都有类似的影响，MPA_GO的定位性能也会变得很差，因为在这种情况下，因子图中无法判断哪些是异常边。这种情况下磁场干扰相当于地磁场，可以采用下面介绍的基于磁场模型的干扰磁场分离算法来解决。

二、基于磁场模型的地磁场估计与干扰分离算法

因为传感器阵列是固定在物流机器人上的，所以磁传感器阵列可以在广阔的空间里位移和旋转。在这种情况下，传感器测量的三个轴的地磁成分会发生显著变化，这时如果不能正确地估计地磁成分并去除地磁成分，那么磁钉定位的精度就会大大降低。

1. 境磁场分析与建模

如果没有外部干扰，如图8(a)所示，环境中的地磁具有一定的大小和方向。但在实际环境中，磁场会受到环境中的硬磁和软磁的干扰。硬磁干扰如图8(b)所示，对地磁产生附加的干扰磁场。硬磁干扰源是指电气机械、电流导线、音响设备等能自发产生磁场的物体。软磁干扰主要是由可磁化物质扭曲地磁场引起的，包括镍、铁和其他干扰的磁性材料。地磁通过这些铁磁性材料时，磁场会发生扭曲，如图8(c)所示。

图8不同情况下地磁场分布示意图；(a)无磁场干扰;(b)硬磁干扰;(c)软磁干扰

通常，地磁会受到硬磁和软磁的干扰，如图9所示。地磁经过干扰，在空间上是不均匀的，但是在传感器阵列的范围(本研究所采用的传感器阵列的尺寸是150×150mm2)下，基本上可以设想均匀强磁场。也就是说，传感器阵列上的各个传感器测量的地磁值是一致的，不考虑传感器本身的测量误差。

图9传感器阵列附近的地磁场分布

传感器采集到的数据主要由地磁场和磁钉产生的磁场叠加而成，其中，磁钉产生的磁场符合磁偶极子模型。因此，磁传感器阵列中的第l个传感器测得的值为

（公式18）

其中，Bl=(Blx,Bly,Blz)为圆柱形磁钉产生的磁场，G=(Gx,Gy,Gz)是地球产生的地磁场（由于假设传感器阵列范围内为匀强磁场，故对于阵列上的任意一个传感器，该地磁分量都一样，为G，而非Gl），ε为测量噪声（正常情况下服从高斯分布，且远小于地磁场和磁钉产生的磁场，因此本研究忽略该分量）。由于圆柱形磁场产生的磁场符合磁偶极子模型，可得

（公式19）

从上式可以看出，传感器的测量值由地磁场和磁铁的位置和方向共同决定。通过公式19所示的环境磁场模型，结合参数估计算法就可以计算出地磁场、磁铁的位置和方向。

2. 磁钉位姿和地磁场估计算法

传感器阵列在平移和旋转时，传感器测量到的地磁场三轴分量(Gx,Gy,Gz)也会随着变化。本研究将(Gx,Gy,Gz)当作未知量进行估计，结合磁铁的位姿，总的待估计参数向量v=[BT,a,b,c,m,n,p,Gx,Gy,Gz]T(v∈R10)。假设第l个传感器的实际观测值Sl=(Slx,Sly,Slz)，期望观测值为Bl’=（Blx’Bly’Blz’），实际观测值和期望观测值之间的残差为

（公式20）

目标函数为

（公式21）

通过最小化目标函数E(v)，可以估计出未知参数向量v，即

（公式22）

基于迭代的最优化算法可以用来估计未知参数向量v，与现有的元启发式算法以及传统方法相比，WOA算法的全局收敛能力具有明显的优势。

但是WOA算法的的精度和速度都低于基于梯度的LM优化算法，因此本研究采用WOA和LM混合算法来估计未知参数。为了使WOA算法给出的初始估计值更为准确，采用如下策略流程：

1、 重复执行WOA算法，独立搜索3次解；

2、 选择最优的一个解（适应度值最小的那个）作为LM算法的初始估计值；

3、 在后续的定位过程中，LM算法计算出结果作为下一次迭代的初始估计值。下面重点介绍一下WOA算法估计磁钉位姿和地磁场。

三、基于鲸鱼寻优算法WOA的初值估计

如图10所示，座头鲸在觅食过程中，首先下潜12米左右，然后开始在猎物周围边螺旋式上游并产生泡泡，最终形成气泡网。WOA算法是受这种觅食行为的启发而提出的，它采用三个算子来模拟座头鲸对食物的随机搜索、环绕式包围和气泡网攻击。

图10座头鲸的觅食行为

假设某个区域只有一个最大的食物，鲸鱼群在这个区域内随机寻找食物。寻找食物的最有效的方法就是在离食物最近的鲸鱼周围寻找。假设食物代表估计参数的真实值，在初始阶段，WOA算法在估计参数的空间中随机初始化一系列鲸鱼个体，每个鲸鱼代表估计参数的可能解。解的“好坏”由目标函数E(v)的大小即适合度值(fitnessvalue)来评价。鲸群中适合度最小的鲸鱼离食物最近，鲸鱼的位置被称为全优解。

1、环绕式包围行为建模

假设V*(t)表示鲸鱼群体中离食物最近的鲸鱼个体的位置，即当前状态下的全局最优解，则其它鲸鱼个体会朝这个最优解的方向来更新它们的位置，该过程表示为如下

（公式23）

（公式24）

其中，t表示当前迭代次数，A和C是系数向量，V(t)表示鲸鱼个体的位置，||表示取决对值，运算符表示向量之间是逐个元素相乘(element-by-element)。向量A和C的计算方式如下

（公式25）

（公式26）

其中，a在迭代过程中从2线性减小到0，r是[0,1]的随机向量。所以，向量A是在区间[-a,a]内的随机值。

2、气泡网攻击觅食阶段

下面通过两种方法来建立鲸鱼的气泡网攻击觅食行为的数学模型：

1、环绕式收缩包围觅食机制

这种环绕式收缩包围觅食机制通过减小公式25中的a值来实现。因为向量A的波动幅度会随着a的减小而缩小，而在迭代过程中a会从2线性减少到0。

2、旋式包围觅食机制

为了模仿座头鲸的螺旋状运动，首先计算座头鲸和食物之间的位置，然后结合该位置在二者之间建立一个对数螺旋方程，如下所示

（公式27）

其中D’=|V*(t)−V(t)|为座头鲸和食物（即适应度值最小的鲸鱼位置）之间的距离，b是控制螺旋圈大上的参数（一般设为1），l是[-1,1]范围内的随机数。

座头鲸在猎食过程中，一边螺旋形状运动，一边收缩包围圈。数学上为了模拟这种行为，将该觅食过程分解成两个独立的运动，在更新鲸鱼位置时收缩包围和螺旋运动的概率各占一半，即

（公式28）

其中p是[0,1]中的随机数。

3、随机搜索觅食阶段

座头鲸除了气泡网攻击觅食外，还有随机寻找猎物。在随机搜索觅食阶段，座头鲸是根据群体内鲸鱼之间的位置进行随机搜索，即当前座头鲸的位置更新是随机选择一头座头鲸作为参考，而攻击觅食阶段则是选择适应度值最小的鲸鱼作为参考。这里根据随机向量A的值来决定座头鲸的行为，当|A|>1时表示处于攻击觅食阶段，当|A|≥1表示处于随机搜索觅食阶段。由于向量A是在区间[-a,a]内的随机值，且a在迭代过程中从2线性减小到0，所以随机搜索觅食阶段主要发生在迭代开始的前半阶段。上面过程用数学模型表示为

（公式29）

（公式30）

其中Vrand是从当前群体中随机选中的鲸鱼所在的位置。

综合上面分析，使用WOA算法估计磁钉位姿和地磁场初值的流程如下：

Step1初始化鲸鱼种群Vi(i=1,2,…,n)，主要是随机初始化磁钉的位姿和地磁场值（在参数区间范围内），迭代次数t=0；

Step2根据公式21计算每个鲸鱼个体Vi(t)的适应度值，选择适应度值最小的鲸鱼作为V*(t)；

Step3更新参数a,A,C,l和p

Step4根据公式28更新每个鲸鱼个体的位置；在式中p的情况下，若|A|≥1，则是采用公式30进行更新，反之，则是采用公式24进行更新；

Step5若符合终止迭代条件（一般为适应度值足够小或者达到预设的最大迭代次数），则结束，返回当前适应度值最小的鲸鱼个体V*(t)所在的位置作为结果；否则，t=t+1，转到Step2。

四、实验结果

为了验证所提算法的有效性和优越性，实现了另外两种分离地磁场干扰的方法作对比：

1、 传统常见的地磁干扰分离方法，即在定位初始化时获得地磁分量大小，在后续定位过程中，把磁传感器采集到的数据减去地磁分量；

2、 基于IMU的地磁干扰分离方法，采用IMU估计传感器阵列的阵列，结合初始时刻的地磁分量，来实时估计传感器阵列在某一姿态下对应的地磁场分量。

1、对比算法：基于IMU的地磁场干扰分离算法

基于IMU的地磁场干扰分离算法与本研究所提算法的不同点在于：该方法不需要假设传感器阵列范围内的地磁场为匀强磁场，因此第l个磁传感器的观测值Bl’为

（公式31）

其中，Bl=(Blx,Bly,Blz)为磁钉产生的磁场，Gl=(Glx,Gly,Glz)是地磁场分量（与公式18不同，此处为Gl，而非G），ε为测量噪声，正常情况下服从高斯分布，且远小于地磁场和磁钉产生的磁场，该分量可被忽略。

三轴地磁分量的矢量和为常数，但传感器测量到的三轴地磁分量会随磁传感器阵列的运动旋转而变化。若已知初始时刻的三轴地磁分量以及磁传感器阵列在任意时刻的姿态，就可以估计出任意时刻的三轴地磁分量。

图11显示了一种基于IMU的地磁场和磁钉磁场分离方法，由一个三轴陀螺仪和三轴加速度计组成的IMU安装在磁传感器阵列上，通过IMU信号可以实时估计出磁传感器阵列的姿态信息。本对比实验中采用BMI160惯性传感器（BoschSensortecGmbH，德国），该传感器集成有一个三轴加速度计（满量程非线性度为±0.5%，输出噪声为180μg/根号下Hz）和一个三轴陀螺仪（满量程非线性度为0.1%，输出噪声的均方根（RMS）误差为0.07°/s）。

图11基于IMU的地磁场和磁钉磁场分离方法框图

基于IMU传感器估计的姿态是IMU所在的载体坐标系相对于惯性坐标系{e}的姿态。在本研究中，IMU和磁传感器阵列是钢性连接，且IMU所在的载体坐标系和系统坐标系{s}在三轴坐标系方向上一致。因此，通过IMU得到的姿态(α,β,γ)就是传感器阵列相对于惯性坐标系的姿态。

基于IMU传感器估计和分离地磁场干扰的操作过程如下：

1、 磁定位系统启动时，使得磁铁远离磁传感器阵列；

2、 在上述条件下，将磁传感器测量到的磁场强度值视为地磁分量，第l个传感器观测值记为Gl(0)；

3、 在后续定位过程中，磁传感器的观测值减去初始时刻记录的地磁场分量，获得磁铁产生的磁场强度。

假设初始时刻记录的三轴地磁分量为Gl(0)，当磁传感器阵列旋动运动时，要根据传感器阵列的姿态，估计出当前时刻t下的三轴地磁分量Gl(t)。

假设磁传感器阵列的初始姿态用四元数表示为：

当前时刻t下的姿态为：

则相对姿态变化量为：

（公式32）

根据相对姿态变化量，可计算出当前时刻t下的三轴地磁分量Gl(t)，即

（公式33）

根据上式，我们可以从传感器测量到的磁场强度中减去实时估计出的三轴地磁分量，即可获得磁钉产生的磁场强度。以此类推，传感器阵列上的N个磁传感器均可以通过上述方法获得磁钉产生的磁场强度。

2、WOA算法的全局收敛性

在实验中，磁铁位置参数的范围（边界）设为a,b∈[−80mm,80mm],

c∈[0,300mm]，方向参数m,n,p的范围被限制在[-1,1],BT∈[4×10-8,10×10-8],地磁场参数Gx,Gy,Gz的边界设为[-80uT,80uT]。迭代求解时，种群数n和最大迭代次数都设置为150，参数b=1。收敛的判定条件为适应度值小于1×10-5,位置、姿态和地磁场的欧氏距离分别要小于5mm,0.2，10uT。在磁钉位姿和地磁场不同情况下，共做了10次统计，每次重复执行WOA算法40次，结果如表3所示，其中一次的收敛结果和目标函数值变化曲线见图12。

表3WOA算法全局收敛率统计

图12WOA算法收敛结果;(a)位置参数a,b的收敛结果;(b)目标函数值变化曲线

从上面结果可以看出，WOA算法的全局收敛率达到98.75%，重复执行3次，从中选出最优的，进一步提高了初值估计的可信度。

3、传感器阵列范围内匀强磁场验证

本研究所提出的基于磁场模型的地磁场估计算法的前提是传感器阵列范围内的地磁场为匀强磁场。在该实验初始定位时，所有传感器都减去自身的初始测量值，使得输出值都为0。然后将磁传感器阵列置于一系列不同的姿态下，一些姿态见图13所示。

本研究采用一个具有45°斜面三角形纸盒来使磁传感器阵列处于指定的姿态。(α,β,γ)是描述翻滚角、俯仰角和偏航角的欧拉参数，分别对应于绕系统坐标系X、Y和Z轴的旋转量。在每个姿态下，记录下9个三轴磁传感器测量值，共27个测量值，再对9个传感器的X轴测量值求其标准差（Y轴和Z轴以此类推），结果如图14所示。

图13传感器阵列放置位姿示例

图14传感器阵列9个传感器X-,Y-和Z轴在不同姿态下的标准差情况

从图14可以看出，传感器阵列中的所有传感器X,Y,Z轴的标准差都小于1.5uT，而且标准差中还包括传感器的测量误差。该结果表明传感器阵列范围内的地磁场基本一致，即可近似认为是匀强磁场，所以假设成立。

4、不同姿态下地磁场和磁钉磁场分离结果测试

在实验中，我们将磁传感器阵列置于一系列不同的姿态下。首先让传感器阵列沿系统坐标系的X、Y和Z轴旋转，每次旋转角度间隔90°。接着，让磁传感器阵列沿系统坐标系各个轴进行复合旋转，即传感器阵列沿三个轴都有旋转偏移量，一些姿态见图13。在各个姿态下，我们在标定板上选取4个测量点，每个测量点重复测量10次，计算并记录下三种分离地磁方法下定位系统的定位精度，结果如图15和图16所示。

由于传感器阵列连接一个STM32控制器，使得沿Y轴旋转的角度有限。结合实际应用场景（物流机器人），翻滚角和俯仰角的变化幅度一般比较小。因此，在沿X和Y轴的旋转范围控制在±90°内。

图15定位系统在不同分离地磁方法下的位置误差对比，其中δ表示45°

图16不同分离地磁方法下的姿态误差对比，其中δ表示45°

传统去地磁方法只适用于传感器阵列固定不动的情况下，因此在该实验中表现很差，平均定位误差为20.91mm和30.78°。在有IMU辅助的情况下，定位系统的位置误差和姿态误差下降到3.74mm和5.21°，但地磁干扰分离的效果仍然比基于模型参数估计的方法（平均定位误差为1.14mm和1.68°）来得差。主要原因是采用IMU本身存在姿态估计误差，使地磁分量估计也存在误差。

基于模型的方法虽然能够获得较为理想的精度，但还是比传感器阵列固定不动时精度来得低一些。主要误差来源是环境磁场建模中引入的近似误差，因为在实际情况下，传感器阵列范围内的地磁场并非完全均匀的。但从结果可以看出，该近似误差对磁钉定位的影响不是很大。

5、大范围运动下地磁场和磁钉磁场分离结果测试

物流机器人正常工作时能在大范围内移动，但现代建筑中含有钢筋等铁磁性物质，会影响地磁在空间中的分布。在实验中，传感器阵列在图17的环境中移动。为了尽可能避免IMU对旋转姿势的估计误差的地磁分离效果，在移动中尽可能保持阵列的平移。磁铁和磁传感器阵列之间的相对位置在移动期间始终不变。磁传感器阵列在移动过程中实时存储磁传感器和IMU数据，在使用相同原始传感器数据的前提下，分别实现了基于IMU和模型的方法消除地磁干扰。图19是表示用模型分离的三轴地磁成分的图。

图17传感器阵列移动轨迹

图18大范围运动下两种地磁分离方法的定位精度对比

图19大范围运动下基于模型分离出的地磁分量

从图18的结果可以看出，基于模型分离地磁的定位系统在整个运动过程中都保持良好的定位精度（位置参数a,b,c的标准差分别为0.2mm,0.16mm,0.15mm），效果明显优于采用IMU的方法（位置参数a,b,c的标准差分别为2.13mm,1.74mm,3.06mm）。基于IMU去除地磁干扰的定位方法精度会变化，主要原因是由于建筑物内有铁磁物质的干扰，使得起点处的地磁场和其它位置的地磁场不一致。图19显示了整个移动过程中地磁场的变化情况，可以看出地磁场会随着位置的不同而不断变化。

综合上面实验结果，基于模型参数估计的方法具有如下优势：

1、 不用增加额外的传感器（如IMU）就可以准确的分离出地磁场分量；

2、去除地磁场干扰的效果好，使得磁钉定位结果精度高；

3、估计出的地磁场分量还可以用于基于地磁指纹的定位方法。

总结

本文以提高磁钉定位精度为主要目的，从磁场干扰水平研究了外部磁场干扰源和地磁两种情况。以下为总结内容：

(1)针对外部磁场干扰源(主要特点是局部传感器受到干扰)提出一种基于图优化和鲁棒核函数的抗磁场干扰方法。首先使用因子图表示磁钉的位置问题，分析局部传感器受干扰磁场的影响，因子图中会出现异常边，采用鲁棒核函数降低异常边的权重。在使用小磁铁作为干扰源的实验中，证实了磁定位算法具有提高抗干扰能力的效果。

(2)地磁干扰时(主要特点是全球传感器受到干扰)，提出基于地磁模型的地磁估计和干扰分离算法。根据分析，磁场与阵列中所有传感器产生的干扰基本一致。也就是说，磁场在传感器阵列范围内几乎是均匀的磁场。结合磁流磁偶极模型进行环境磁场建模，作为磁流矢量的三个参数和磁流姿态的求得参数，在磁流定位过程中实现实时估计和分离出现了。实验结果表明，用这种方法可以准确地推算出地磁，并分离出引脚磁和地磁磁，使引脚的定位精度和鲁棒性得到了飞跃性的提高。虽然该方法的研究对象是地磁，但是如果阵列的所有传感器受到的干扰基本一致的话，也可以适用于其他类似的磁场干扰源。