基于MEMS陀螺仪辅助的粒子群优化磁力计校正

2018-11-21原雨佳陈兴邦

导航定位与授时 2018年6期

原雨佳，王伟，陈兴邦

(哈尔滨工程大学自动化学院，哈尔滨 150001)

0 引言

磁力计是一种测量磁场强度的仪器，通过对其三轴分量的计算能够得到导航中重要的航向角信息，具有无累计误差的优点。然而磁力计有不可忽视的零位、灵敏度、非正交误差，以及周围环境影响造成的软硬磁误差，这些都会造成其输出不准确，从而影响解算航向角的精度。因此，在使用前需要对其进行误差分析，并采用有效的算法进行补偿校正，提高其实用性。

目前针对磁力计校正的算法有很多。椭球拟合法是一种常见的磁力计校正算法，它不需要其他辅助信息，依据将未校正的椭球分布的磁力计数据还原成球型的原理实现校正。虽然此种算法原理简单、计算量小，但是它需要空间中各个方向的采样数据，操作复杂[1-3]。极大似然估计法同样不需要辅助信息，但它对初值的选取有较高的要求，而且计算量大[4-6]。除了以上两种无姿态的离线校正算法，工程中更为常用的是利用非线性卡尔曼滤波算法实现对误差参数的在线校正，该算法具有良好的实时性，但是难以预估传感器的滤波初值和噪声分布[7-9]。为了解决传统磁力计校正算法中采样点多和初值条件严苛等问题，武元新提出了将粒子群优化(Particle Swarm Optimization，PSO)算法用于磁力计校正中，此种算法使用更加精准的非线性误差模型，并且无需良好的初值[10]。之后他对此算法进一步优化，提出了增强型粒子群优化(Stretching Particle Swarm Optimization，SPSO)算法，避免了粒子过早陷入局部最优，但是运算时间长[11]。目前的PSO算法多数采用校正前后的磁场总量不变作为适应函数，只能用于磁力计简化模型，需要假设传感器至少有一个轴与载体系重合，从而将12个未知参数简化成9个或6个参数进行求解[12-13]。

针对传统PSO算法用于磁力计校正中估计参数个数少的问题，本文提出了一种基于微机电系统(Micro-Electro-Mechanical System，MEMS)陀螺仪辅助的粒子群优化算法对磁力计进行校正，为了避免算法过早陷入局部最优，并且在动态环境中具有较好的适应性，采用了随机漂移粒子群优化算法[14]。通过理论仿真和实测数据证明，该算法能够有效地补偿磁力计误差，具有更好的工程实用性。

1 磁力计误差模型

根据对磁力计的灵敏度、零位、非正交以及软硬磁误差的分析，建立如下的磁力计误差模型[15]。

(1)

其中：

S=SscSnoSsi

b=bm+bhi

(2)

(3)

2 基于陀螺仪辅助的随机漂移粒子群优化算法

粒子群算法是一种仿效鸟类觅食过程的优化算法，通过迭代不断提高与目标函数有关的种群的候选解，即通过位置与速度更新公式寻找适应目标函数的粒子，每个粒子的移动不仅受自身经验的影响，也受当前种群中最优位置的影响。与其他算法相比，粒子群算法需要设置和调整的参数较少，而且收敛速度快，近些年发展迅猛。

2.1 目标函数

粒子群滤波中有2个重要的极值，一个是粒子个体最优值，用Pbest表示，另一个是种群的全局最优值，用Gbest表示。算法的目的就是寻找Gbest使目标函数值最小。假设待解决问题的粒子维数为D，种群中粒子个数为m，在第k次迭代中，第i个粒子的位置向量可以表示为：

Pi(k)=(pi1(k),pi2(k),pi3(k),…,piD(k))T

(4)

速度向量可以表示为：

Vi(k)=(vi1(k),vi2(k),vi3(k),…,viD(k))T

(5)

从磁力计误差模型中可以看出，校正算法的目的就是求解公式中的S和b，所以每个粒子可以用如下的数学式表示：

(6)

其中：

(7)

从误差模型上可以看出，各种误差对各轴向产生的影响是不同的，在有陀螺仪辅助的情况下，通过陀螺仪解算姿态角信息，在已知地磁场的情况下得到理想载体系下的磁场矢量，此磁场矢量应与磁力计补偿后的磁场矢量相同。由此可以建立磁场矢量适应函数，将其写为下面的数学表达式

F2,i=argminf2(S,b)

(8)

2.2 随机漂移粒子群优化算法

传统PSO算法在一定的迭代后，粒子速度会慢慢减小，使得种群收敛到某个最优值，一旦环境发生变化，失去多样性的种群很难对此做出快速响应。在实际磁力计使用中，周围有不可避免的磁性物质，这些都会对磁力计输出造成影响，因此，本文采用一种具有较强全局搜索能力的随机漂移粒子群优化(Rondom Drift PSO，RDPSO)算法。受金属导体内自由电子运动模型的影响，通过将最优值的搜索过程类比于自由电子向具有最小势能位置的运动，从而提高算法的搜索能力[16]。

该算法的具体流程为：

1)粒子群初始化

初始化粒子个数m，粒子维数D，权重的最大值ωmax和最小值ωmin，最大的迭代次数kmax，速度限制的最大值Vmax和最小值Vmin，位置限制的最大值Pmax和最小值Pmin。按照式(9)初始化粒子速度和位置。

Pi,j(k=0)=Pminj+rand(0,1)(Pmaxj-Pminj)

Vi,j(k=0)=Vmin+rand(0,1)(Vmax-Vmin)

(9)

式中，Pminj和Pmaxj分别是每个粒子第j维对应的位置最大值和最小值，对于磁力计校正，前9维对应的位置限定范围要小于后3维偏置对应的位置范围，这样设置可以为滤波算法提供更实用的初值。

在初始化阶段，粒子的个体最优Pbesti(k=0)≡Pi(k=0)，通过式(8)计算每个粒子的适应值，选择最优的位置为种群最优值Gbest。

2)粒子群更新

与传统PSO算法速度更新方式不同，RDPSO算法中粒子运动包括两部分，即定向漂移运动和无规则随机热运动，数学表达式为：

Vij(k+1)=Vrij(k+1)+Vdij(k+1)

(10)

式中，k是迭代次数，i(i=1,2,3,…,m)表示粒子群中第i个粒子，j(j=1,2,3,…,D)表示粒子的第j维，Vrij(k+1)表示无规则随机热运动，Vdij(k+1)表示定向漂移运动。

热运动可以表示成：

Vrij(k+1)=α|Mj(k)-pij(k)|φij(k)

(11)

其中：

(12)

(13)

(14)

式中，α是热系数，表征算法的全局搜索能力，本文改变其为固定常数的情况，设置为随迭代次数增加而减小的S型函数，数学表达式为式(12)。Mj(k)是种群粒子个体历史最优位置的平均值，数学表达式为式(13)。φij(k)是正态随机分布函数，s和uij(k)是(0,1)之间的2个不同随机数，采用式(14)求解。

粒子的定向漂移运动可以定义为：

Vdij(k+1)= rand(0,1)c1(pbestij(k)-pij(k))+

rand(0,1)c2(gbestj(k)-pij(k))

(15)

式中，c1和c2分别是自我认知和社会认知,分别代表粒子自身位置最优和种群位置最优的影响；rand(0,1)是(0,1)之间的随机数。

综上，RDPSO算法的粒子速度和位置公式为：

vij(k)=α|Mj(k)-pij(k)|φij(k)+

rand(0,1)c1(pbestij(k)-pij(k))+

rand(0,1)c2(gbestj(k)-pij(k))

(16)

pij(k+1)=pij(k)+vij(k+1)

(17)

按照上式对粒子的速度和位置进行更新，同时在更新过程中限制速度和位置在所定范围内。

3)搜索全局最优

因为本文中的问题是解决最小值问题，因此种群的目标就是找到最小的适应值，对于每个粒子，如果新的适应值小于Pbest对应的适应值，则更新当前位置的Pbest，否则Pbest不变。可以将Pbest写为：

(17)

同样的，如果新的Pbest对应的适应值小于此时Gbest对应的适应值，则更新Gbest，否则Gbest不变。可以将Gbest写为：

(18)

其中，f是目标函数，即式(8)。

4)迭代

迭代停止的方法有两种：种群的适应值达到设定的最小值；迭代次数达到最大。本文使用的是第二种方法，如果迭代超出kmax，则迭代停止，此时的Gbest就是要求解的参数矩阵。

由此可见，此算法就是通过迭代追踪个体历史最优值和种群历史最优值，其流程图如图1所示。

3 仿真实验分析

本算法使用的环境是在哈尔滨市，依据IGRF12模型[17]计算当地的地磁场矢量为Hi=[242.367;-46.225;-465.459]mG2。为了测试本算法的性能，分别做了以下几个仿真模拟实验。

3.1 参数估计数量对比实验

为了比较本文所提算法与传统PSO算法的参数估计数量，模拟磁力计分别绕X、Y、Z轴旋转一周产生的数据，设置采样周期为0.01s，从生成的3000个点选取均匀分布的30个点，陀螺仪漂移设为6.25(°)/h，具体的参数设置如表1所示。分别使用传统PSO算法和本文所提算法进行校正，校正结果如表2所示。

表1 粒子群优化算法参数设置表

表2 12个误差参数估计结果

从表中可以看出，使用传统算法无法准确地估计出磁力计误差模型中的12个参数，有些参数的估计结果与参考值相差较大。这是因为传统算法只选用约束磁场总量的目标函数，没有考虑磁场的三轴分量，造成当误差矩阵S阵为9个不同参数时，粒子群寻找最优值受到较大干扰。而本文提出的RDPSO算法却能得到较为精准的参数估计结果。

3.2 旋转度测试实验

为了简化校正中操作的复杂度，该实验通过模拟磁力计的几种不同旋转方式，验证算法在各种旋转度下的性能。旋转方式如下：

方式一：模拟磁力计在绕X、Y、Z轴各旋转1周，即在空间中是3个完整的圆形；

方式二：模拟磁力计绕X、Y轴各旋转1周，即在空间中是2个完整的圆形；

方式三：模拟磁力计绕X、Y轴各不完整旋转1周，即在空间中是2个不完整的圆形；

方式四：模拟磁力计绕Z轴旋转1周，即在空间中是1个完整的圆形。

针对四种不同旋转方式获取的采样点，采用RDPSO算法进行校正，得到的参数估计结果如表3所示。

表3 各种旋转度测试结果

从表3中可以看出，当采样点在空间分布为不完整2周时，本文所提算法就能够估计出磁力计误差参数，与空间分布为完整3周时校正精度接近。但如果单独绕一轴旋转，即采样点空间分布为平面1周时，由于粒子的多样性过低，无法估计出磁力计12个误差参数，尤其是对偏置矩阵无法估计。因此，本文所提算法只需要磁力计在空间中绕X、Y、Z中的任意2个轴不完整旋转1周即可实现对误差参数的估计，相对于传统算法大大简化了操作的复杂度。

图2～图4所示为磁力计分别旋转完整3周、完整2周及不完整2周时，磁力计校正前后的采样点分布图。理想情况下，采样点应该分布在以地磁场强度为半径的球上，但由于各种误差的干扰使其偏离。从图中可以看出，校正前的采样点都在球面外侧，经过本文算法校正使其重新分布在球面，说明本算法能够对磁力计误差进行有效补偿。

3.3 动态性能测试实验

为了测试本文所提算法在磁场动态环境中的性能，做了以下的仿真实验。模拟所在磁场环境是变化的，即不再设定磁场强度为一个定值，仿真中将其设置为线性变化的，得到的实验结果如表4所示。

表4 动态性能测试结果

Fg是当粒子为最优位置时，适应函数的值，即通过式(8)得到的值，可以表征算法的精度。因此，可以看出，在磁场变化时，本文所提算法比传统算法的精度高，说明本文所提算法具有更高的动态适应能力。

3.4 陀螺仪精度影响测试实验

为了验证不同陀螺仪精度对算法精度的影响，本文针对几种常见的陀螺仪精度做了仿真实验，得到的结果如表5所示。

表5 陀螺仪精度影响测试结果

从表5可以看出，陀螺仪的精度会对本文算法造成影响，但是影响较小。

4 实测实验验证

为了验证该算法在实际中的有效性，选用以ADIS16488为核心的MEMS-IMU系统作为实验设备，实物如图5所示，它的参数如表6所示。用ADIS16488 MEMS-IMU系统采集数据，同时为了降低陀螺仪误差对算法精度造成的影响，取陀螺仪静态数据的平均值作为零漂减去，再采用本文所提算法、PSO算法和扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法对磁力计校正，得到的参数估计结果如表7所示，利用本文算法得到的磁力计校正前后数据的对比如图6所示。从表中可以看出，本文所提算法精度最高，说明本算法在实际系统中能够实现磁力计的校正。