基于改进OPTICS算法的磁罗盘椭圆拟合误差补偿方法

缪玲娟，吴子昊，周志强，李正帅

（北京理工大学 自动化学院，北京 100089）

磁罗盘无需外部输入，能够根据测量得到的地磁矢量信息来解算当前航向角，具有体积小、隐蔽性高、价格低等优点，被广泛应用于航天、航海、车载导航、行人导航等领域[1,2]。针对车载导航这一领域，在低精度的惯性导航系统（INS）/全球导航卫星系统（GNSS）组合导航系统中引入磁罗盘，组成新的组合导航系统，以提高载体航向角的解算精度。在实际应用场景下，由于磁罗盘自身的器件制造误差和外界干扰磁场等因素的影响，导致航向角的解算出现误差。若不对这些误差进行补偿校正，则会直接影响系统航向解算结果的精度。受到车辆运动学的约束，其横滚角、俯仰角的变化范围较小，因此只需对磁罗盘进行二维平面下的水平校正。目前，二维平面下，对磁罗盘常用的误差补偿方法有十二位置法、二十四位置法、椭圆拟合法等。前两种方法均需要引入外部的高精度航向基准，常用于实验室场景下的转台标定，而在动态的车载导航领域中应用较少。

文献[3]采用十二参数椭球拟合方法对磁强计进行磁场干扰的校正，并利用BP神经网络对复杂环境下的地磁场补偿模型进行训练。文献[4]以静态校正得到的补偿系数为初始值，采用递推最小二乘法实时更新椭球系数，实现在线校正。文献[5]额外使用一个固定在水平转台上并连续旋转的三轴磁强计，通过检测正弦失真来估计该区域的磁干扰程度，改变扩展卡尔曼滤波器（Expanded Kalman Filter, EKF）中的系数。文献[6]采用牛顿-拉夫逊迭代对磁场干扰补偿系数进行分步求取，减小外部磁场干扰对相关参数估计的影响。文献[7-9]在椭圆拟合算法中引入了智能算法，在提升精度的同时，也会造成计算量的增加，降低系统的实时性。文献[10]首先结合t分布确定拒绝域，再使用Grubbs检验法来检测数据中的离群点。文献[11]提出一种基于DBSCAN（Density-based Spatial Clustering of Applications with Noise）的椭圆拟合算法，利用DBSCAN算法和莱特准则对原始数据中的奇异值进行剔除，再利用最小二乘法对椭圆曲线进行拟合，并根据拟合参数对磁罗盘的实时数据进行补偿。

当原始数据出现奇异误差点时，文献[3-6]采用的拟合补偿算法无法对其进行过滤以保证拟合的精度。文献[10]中的Grubbs检验法每次只能检测出一个离群点，当数据量较大时，该方法的实时性会明显降低。文献[11]中DBSCAN算法的聚类结果受自身参数影响很大，在不同的使用场景下，需要对自身的两个参数进行调整，因此该算法的应用场景十分有限。

与DBSCAN算法相比，OPTICS（Ordering Points to Identify the Clustering Structure）算法不仅能够对多个误差点进行识别，而且对其输入参数不敏感，无需反复调整参数。为了降低OPTICS算法的计算量，文献[12]先使用DBSCAN算法将原始数据分为两个长度相等的子集，再使用OPTICS算法对不同密度的数据进行分类。文献[13]通过存储每个点的前置信息，并在聚类提取过程中考虑这些信息。文献[14]将数据集合划分为一定数量的网格单元，利用质心点代替网格数据点集的方法对数据点进行压缩，从而提高算法运行速度。

本文对OPTICS算法进行了改进，通过减少队列排序次数来降低计算量；在椭圆拟合过程中，引入改进后的OPTICS算法，对磁罗盘采集的原始数据进行预处理，在过滤奇异误差点后，再进行最小二乘椭圆拟合，对误差进行补偿。数据预处理过程能够有效保障椭圆拟合的精度，降低最终航向解算的误差。

1 磁罗盘原理及其误差分析

假设磁罗盘的三轴xm、ym、zm分别与所在载体坐标系（b系）的三轴xb、yb、zb重合，指向载体的右、前、上三个方向；地理坐标系（n系）的三轴xn、yn、zn分别指向东、北、天三个方向。在使用磁罗盘测量航向角之前，若载体的俯仰角θ或横滚角γ不为0，则需要进行倾角补偿[15]：

在完成倾角补偿后，按照式(2)解算磁航向角φ：

得到磁航向角φ后，再按照式(3)进行磁偏角补偿，最终得到航向角ψ：

其中，Dec为当地磁偏角，可根据地球磁场模型获知。

在测量磁航向角的过程中，磁罗盘会受到各种干扰，造成一定的误差。磁罗盘的误差大致可分为以下五种：测量噪声、制造误差、安装误差、姿态误差和罗差[1]。测量噪声指在数据采集过程中存在的奇异误差点，包括有色噪声和异常数据，通常是由于环境干扰磁场发生突变或传感器发生故障引起的[10]；制造误差包括零位误差、灵敏度误差和非正交误差；安装误差指安装传感器时，由于传感器坐标系和载体坐标系不重合而引起的误差；姿态误差指的是由于载体的横滚角和俯仰角的误差引起的航向误差，不属于磁航向系统本身；罗差是由于传感器周围的铁磁材料影响而产生的磁航向误差，是利用地磁场测量磁航向时所特有的一种误差，分为硬磁材料引起的罗差和软磁材料引起的罗差两种。在以上五种误差中，罗差对航向角的影响最大，对罗差进行补偿是保证磁航向测量精度的基础。

硬磁材料相当于永久磁铁，其磁场强度和方向可认为是恒定不变的，对磁航向误差的影响与零位误差类似。当硬磁材料离传感器较近时，所引起的磁航向误差较大，是磁航向误差的主要部分。软磁材料本身不产生磁场，但是在被环境磁场磁化后，会产生干扰磁场，干扰磁场的大小和方向与环境磁场及软磁材料本身都有关系，软磁材料对磁罗盘的影响较为复杂。

2 最小二乘椭圆拟合算法

2.1 最小二乘法

最小二乘法是一种常用的拟合算法，该算法的原理是求使得测量误差的平方和最小时的方程系数。

椭圆方程可表示为如下形式：

对于n个数据点的情况，将式(4)扩展，并写成矩阵形式，有：

其中，

由于X是5维列向量，即式(4)中有5个待解算系数，所以在式(5)中，需要满足rank(W)=5这个条件。

根据最小二乘法的原理，有：

从而求解出式(4)中的待求解系数a、b、c、d、e。

2.2 椭圆拟合算法

在图1中，x轴、y轴分别表示磁罗盘指向载体右向和前向的两个轴。当无外界磁干扰且xOy平面与当地水平面平行时，磁罗盘水平转动一周，x轴、y轴两个水平方向磁场矢量的输出为圆心在原点的圆形，如曲线A所示；软磁干扰使磁罗盘输出变成椭圆，并且椭圆的长轴与坐标轴成一定角度，如曲线B所示；硬磁干扰使磁罗盘输出曲线的圆心发生偏移，如曲线C所示[1]。

图1 罗差影响示意图Fig.1 Impact of magnetic deviation

当不存在任何误差时，载体水平转动一周，磁罗盘的x轴和y轴上真实的磁场强度分别为：

其中，H0为地磁场的水平分量，φ为磁航向角，变化范围为0～360°，地磁北向定义为0，北偏东定义为正方向。

考虑到实际应用场景中的误差，磁罗盘x轴和y轴的测量值H xm、Hym和真实值H x、Hy之间的关系可表示为：

其中，axx、axy、ayx和ayy由软磁误差和部分制造误差决定，bx和by由硬磁误差和部分制造误差决定。

结合磁罗盘的误差来源，可以分步描述从圆变化到椭圆的过程[16]：

其中：

kx、ky是与灵敏度相关的系数，φp是与制造误差及软磁误差相关的系数，φs是与安装误差相关的系数。正交矩阵ΦS将圆旋转了一个角度φs，而椭圆拟合无法补偿安装误差，需通过其他方法进行补偿，在此，假定已完成磁罗盘安装误差角的补偿，令φs=0，再根据式(8)，得：

进一步有：

联立式(6)和式(10)，并求Hx、Hy的平方和得：

其中：

将式(11)变换为如下形式：

式(13)中的系数A、B、C、D、E由最小二乘法求出。

对于系数kx和ky，在求解航向时，仅和其比值k x/ky有关[1]，所以为了方便计算，令kx=1，再联立式(11)(12)(13)，得：

将式(14)代入式(10)，即可将磁罗盘测量数据Hxm、Hym校正为真实值Hx、Hy，并按照式(2)解算得到磁航向角φ，再按照式(3)进行磁偏角补偿，最终得到航向角ψ。

通过椭圆拟合，对磁罗盘的原始测量数据进行校正，减小罗差和制造误差的影响，从而提高最终航向角的解算精度。

3 基于改进OPTICS的最小二乘椭圆拟合算法

如果磁罗盘的实际测量数据Hxm、Hym基本分布在一个椭圆圆周上，那么直接利用最小二乘椭圆拟合算法，就可以获得较好的误差补偿效果。但是如果原始数据中出现了偏离椭圆圆周较大的数据点，直接利用最小二乘椭圆拟合就难以获得较好的拟合效果。这种情况下，需要对原始数据进行预处理，先剔除这些奇异误差点，再进行拟合，从而保证航向角解算精度，本文提出了一种基于改进OPTICS算法的数据预处理方法。

3.1 OPTICS算法

OPTICS算法是一种基于密度的聚类算法，密度聚类的核心思想是用一个点的ε邻域内的点数来衡量该点所在空间的密度。OPTICS算法是DBSCAN算法的一种有效扩展，主要解决了DBSCAN算法对输入参数（邻域半径ε和最小邻域点个数m）敏感的问题，增强了算法的适用性。

下面给出一些定义[17]：

（1）核心对象：如果对象点p的ε邻域内点的个数大于或等于设置的阈值m，则称对象点p为核心对象。

（2）核心距离：对象点p的核心距离是使得其成为核心对象的最小半径。以对象点p为例，找到以p点为圆心，且刚好满足最小邻域点个数m的最外层对象点x′，则点p到点x′的距离称为点p的核心距离。

（3）可达距离：对象点p的可达距离取决于计算时选择的一个核心对象x，如果点p到点x的距离大于点x的核心距离，则点p的可达距离为点p到点x的实际距离；反之，则点p的可达距离为点x的核心距离。

（4）直接密度可达对象：如果对象点p是核心对象，且对象点q在点p的ε邻域内，则称对象点q是点p的直接密度可达对象。

（5）有序队列：用于存储核心对象及其直接密度可达对象，并根据可达距离进行升序排列。

（6）结果队列：用于存储数据点的输出顺序。

OPTICS算法从一个随机选定的对象点出发，朝着数据最为密集的区域扩张，最终将所有对象组织成一个有序序列[17]。算法流程如图2中的实线部分所示，其中，L代表待聚类的集合，Q代表有序队列，R代表结果队列。

图2 OPTICS算法流程图Fig.2 Flow-process diagram of OPTICS algorithm

在聚类过程中，一个对象可能同时在多个核心对象的ε邻域内，当出现更小的可达距离时，使用该可达距离代替原可达距离，对该点的可达距离进行更新，并将有序队列Q按照可达距离重新升序排列。

得到结果队列R后，还需要分类整理出最终的聚类结果，分类过程如图3所示。

图3 聚类结果的分类流程Fig.3 Classification process of clustering results

通过引入OPTICS算法，对原始数据进行预处理，可以剔除奇异误差点，为航向角解算的精度提供保障。

3.2 改进的OPTICS算法

本文提出的改进方式主要用来提高算法的运行效率，保证实时性。

在图2实线部分所示的OPTICS算法中，获取核心对象p的ε邻域对象点N后，把所有N点加入有序队列Q，并将Q按可达距离升序排列。该排列过程的目的仅仅是为了保证接下来从Q中取出的第一个点为可达距离最小的点，而Q中除了第一个点之外的其他点并没有体现出排序的实际作用，从而造成了计算时间的浪费。

针对该点不足，提出以下改进策略：取消队列Q的有序性，即将原来的有序队列Q改为无序队列Q，把所有对象点N加入队列Q并更新可达距离后，不再对其进行升序排列，由于此时的队列Q是无序队列，在获取可达距离最小的点时，需要进行一次遍历，寻找并取出队列Q中可达距离最小的点。针对OPTICS算法的改进如图2中的虚线部分所示。

为了获取可达距离最小的点，常规的OPTICS算法需要对队列Q进行排序。当队列Q的长度为n时，在现有的排序算法中，平均情况下的时间复杂度最低为O（nlog2n）。在完成上述改进后，只需对队列Q进行一次遍历，即可取出可达距离最小的点，时间复杂度为O（n）。改进后的算法能够有效降低时间复杂度，减小计算量，提高运行效率，且队列Q的长度越长，改进效果越明显。

3.3 数据归一化

由于处理完的数据具有数据量大、数值大的特点，在最小二乘法中还需要对其进行平方、立方、求和等运算步骤，可能导致寄存器无法将数据正常存储。因此，在最小二乘法之前，先找出所有数据点中Hxm的最大绝对值Hxmmax和Hym的最大绝对值Hymmax，并将每个数据点的Hxm和Hym分别除以Hxmmax和Hymmax，使得每个点的Hxm和Hym均在区间[-1,1]内，再进行最小二乘椭圆拟合，最后对计算结果进行逆变换，从而有效避免了由于数值过大而可能造成的计算问题。

4 实验结果与分析

以三轴磁罗盘为实验对象，将其置于室外平地上，校正过程只考虑磁罗盘水平方向的两个轴向数据，即右向（x轴）和前向（y轴）。先将磁罗盘水平旋转一周以进行椭圆拟合（以下简称校正过程）；再将磁罗盘的前向轴分别指向用双天线差分GNSS（基线长1 m）标定出来的东、南、西和北四个方向（以下简称验证过程），并在四个方向上各自静置7～10 s，以计算航向角误差，对比不同算法下航向角的解算精度。常规椭圆拟合算法和基于改进OPTICS的椭圆拟合算法（以下简称本文算法）校正前后的曲线如图4所示。在本文算法中，邻域半径ε=3.7，最小邻域点个数m=8。

从图4中可以看出，根据原始数据拟合的曲线呈现明显的椭圆。OPTICS算法的作用是剔除原始数据中的奇异误差点。

图4 不同算法的拟合效果Fig.4 Fitting effects of different algorithms

从图4(b)中可以看出，本文算法能够有效识别并剔除奇异误差点，避免这些数据点污染数据集，影响航向角的解算精度。

完成校正过程后，验证过程的方向对准顺序为东、南、西、北向，最终计算得到的航向角如图5所示。

图5 不同算法解算的航向角Fig.5 Heading angle obtained by different algorithms

为了验证不同算法下航向角的解算精度，在验证过程的东、南、西和北四个方向上，取各方向中间的4s时长数据进行分析，根据式(15)计算各误差的均方根（RMS），结果见表1。

表1 不同算法下的误差均方根Tab.1 RMS error of different algorithms

其中，n为数据点个数，ei为第i个误差数据，i=1,2…n。

其中，合计部分通过对由四个方向的全部误差数据组成的合计误差数据矢量求取均方根（RMS）得到。由表1知，与原始数据相比，常规椭圆拟合算法能够有效减小误差，提高航向角解算精度；与常规椭圆拟合算法相比，在出现奇异误差点时，本文算法在四个方向上误差均方根值均有所降低，合计统计数据的航向角误差均方根减小了27.5%，进一步减小了航向角解算误差。

为了对比改进前后的OPTICS算法在运行效率上的差异，同时避免实验结果的偶然性，分别将改进前后的OPTICS算法运行50次，以700个数据点为例，计算得到两种算法的平均耗时，结果如表2所示。

表2 计算耗时对比Tab.2 Comparison of calculation time

由表2可知，改进OPTICS算法的平均计算耗时比传统OPTICS算法降低了43.6%。改进OPTICS算法仅降低计算耗时，对航向解算精度无影响。

5 结 论

本文提出的算法在常规最小二乘椭圆拟合算法进行拟合之前，使用改进的OPTICS算法对原始磁罗盘数据进行预处理，剔除原始数据中的奇异误差点，能够有效防止椭圆拟合过程被奇异误差点污染，提高拟合精度，从而使得航向角的解算精度得以提高。此外，改进后的OPTICS算法降低了计算量，具有更低的时间复杂度。实验结果表明，相比于常规椭圆拟合算法，本文算法将四个方向的合计航向角误差均方根降低了27.5%，提高了航向角的解算精度；改进后OPTICS算法的计算耗时比改进前降低了43.6%，提高了算法的实时性。本文算法可应用在车载导航领域，由于在车载环境中，环境磁场干扰更加严重，奇异误差点数量更多，理论上算法的效果更好，本文算法具有较强的工程实用价值。