DA-LM 算法在MEMS 加速度传感器误差校正中的应用研究∗

乔美英许城宽汤夏夏高翼飞史建柯

(河南理工大学电气工程与自动化学院，河南 焦作454000)

MEMS 三轴加速度计因成本低、体积小、低功耗，广泛应用于无人机、无人车、无人船、机器人、医疗辅助器械及随钻等设备[1]。 但由于受自身工艺及应用环境等因素，导致三轴加速度计测量精度严重降低，无法满足高精度测量系统的要求。 随钻轨迹测量(Trace Measurement While Drilling，TMWD)系统利用MEMS 加速度计解算出的姿态角绘制轴迹，为保证所绘轴迹的准确性，需要不断对提高加速度计解算精度的相关技术进行创新和优化，以减少实际作业过程中钻孔真实轨迹与理想轨迹的偏差。

已有相关文献给出加速度计的校正方法，例如加速度计的最大似然校正算法[2]，同时实现加速度计三轴非正交误差的补偿，补偿的重点在于参数的精确估计，但该方法无法补偿未对准误差。 较为流行的基于椭球假设的三轴加速度计误差标定与补偿[3]，该方法也考虑了非正交误差，使得测量精度更高，但椭球拟合法对于原始的数据要求较高，且无法弥补未对准误差。 文献[4]是矢量传感器对非对准误差及其校正，该方法利用椭球拟合及点积不变相结合的方法，弥补了椭球拟合不能校正非对准误差的缺陷，但这种方法实现起来需要很高的实验条件，且点积不变法对参考矢量精度有要求，否则算法效果不佳，并且该算法需要额外的矢量辅助运算，不利于校正单一传感器。 高斯牛顿迭代算法[5]，其优点是操作简单，优化速度快，但优化参数过多会发生部分参数局部最优情况，无法满足所有参数一同得出最优解。 以上算法尽管在一定的条件下可以达到预期的效果，但存在以下问题:需要大量的实测数据；较多的数据易造成算法对数据的依赖性；面对非线性较强函数尤其是迭代参数较多的情况下，不加以约束易造成死循环，最终使得所有参数无法到达最优点。

针对上述问题本文利用Dragonfly Algorithm(DA)与Levenberg-Marquardt(LM)的联合优化算法，DA是一种多群体智能优化算法，能够实现目标参数的全局寻优，缺陷是局部搜索能力不高，但可以利用LM 算法强力的局部寻优能力再次优化参数。 校正之后的加速度计坐标系实现三轴正交，但在制造的过程中受到工艺水平的限制，加速度计与载体坐标系三轴之间存在未对准角度，表现为两个坐标系之间存在旋转矩阵，本文针对未对准问题给出补偿策略，相较于传统的方法易于实现。 为了提高数据的拟合效果，实验采集数据时用等夹角法[5]旋转双轴转台，使数据在空间中呈均匀分布。

1 误差分析和建模

MWD 的传感器模型与通用的航姿参考系统(Attitude and Heading Reference System，AHRS)基本相同，因此MWD 的加速度计误差模型也可以用统一误差模型来表示。 MEMS 加速度传感器在随钻中的布局可参见文献[6]。 MEMS 加速度计误差来源主要有比例系数误差、零点漂移误差、未对准误差以及非正交误差[7]等。 依据误差来源写为误差模型:

U为加速度计不受误差干扰的无偏值；D为比例因子误差；T为未对准误差；S为非正交误差；B为加速度计偏差；V为加速度计测量值。 在使用DA-LM 估计误差参数时，暂不考虑未对准误差T。让SD＝K，由式(1)可得:

通过求得K及B矩阵参数可实现对加速度计误差补偿。

1.1 误差模型变换

加速度计输出三个数据量:Vx，Vy，Vz分别对应当地重力场x，y，z三个方向的分量。 对式(1)展开可将加速度计的误差矩阵方程写为:

ux，uy，uz为真实重力场分量；εx，εy，εz为非正交误差参数；fx，fy，fz为加速度计三轴对应的比例因子误差，Bx，By，Bz为三轴偏差值，其中以x轴的输出Vx为例:

当地的重力场在加速度计的三轴投影:

｜g｜为重力加速度绝对值；α，β，γ分别对应重力三个方位之间的夹角[8]，式(5)代入式(4)写为

利用式(5)、式(6)得出:

同理Vy，Vz也能够写成如式(7)的形式。 联立可写为等式:

式(9)ax～cz为式(8)的输入值，该输入由加速度计的输出提供。 LM 算法优化式(8)的误差参数，该算法局部收敛能力很强但全局收敛能力较弱，因此DA 先作初始优化使误差参数达到全局收敛，输出的参数结果再用LM 优化。

1.2 初始参数选取

DA 是一种仿生学算法，它模拟蜻蜓在自然界中的静态和动态蜂群行为[9]。 该算法利用蜻蜓的习性分为:分离、排队、结盟、寻找猎物和躲避天敌这5 类行为。 算法中的参数值:分离权重、对齐权重、凝聚权重、猎物权重因子、天敌权重因子作为蜻蜓的行为度，以更新蜻蜓的位置，该算法的优势是能够改善给定问题的初始随机参数，收敛于全局最优值。

初值优化算法采用椭球方程给出评价函数[10]，因为经仿真分析DA 与椭球拟合的联合校正法可以消除零偏误差及白噪声的干扰，并且方程简便，易于处理。 式(2)作平方可得:

G＝(K-1)T·K-1＝sTs，构造DA 的评价函数:

式中:A＝‖U‖2是加速度计无偏模值的平方，V＝(V2xV2yV2zVxVyVyVzVzVxVxVyVz)，η＝(abcdeflmng)T，a～g是带有误差信息的椭球参数。 DA 求解(11)中的参数向量η，可构成如下矩阵:

s可通过奇异值分解求得:

ΣTΣ为G的特征值组成的对角阵，W是G特征值对应特征向量组成的矩阵，因此s写成:

求得偏差B:

根据式(2)可获得经过DA 优化的无偏值U′:

根据式(3)求得LM 算法的初值:

在DA 优化后的参数结果中，经过仿真实验，偏差B可精确求得，所以待优化的误差参数由原先的9 个变为了6 个:

其值分别对应于误差参数fx～εz。

2 LM 最优估计

LM 法是解决非线性最小二乘问题的一种方法，获得非线性方程的最优解。 LM 算法同时具备梯度及牛顿法的特征，对于连续微分的线性系统，LM 法可以通过更改步长找到局部最优解。 首先定义几个参数:Jk是对应于θk的Jacobi 矩阵，dk是每次更新θk的步长，优化精度εk＝1e-4，λk是学习因子，αk是增益系数。

2.1 LM 算法实现流程

Step 1 经过1.2 重新定义目标函数:

Bv已成为无偏量，θk作为初值，计算Jk，并且令λk＝1/N×trace(JTk·Jk)为初始学习因子:Step 2 如果‖JkF(θk)‖≤εk，停止迭代，输出估计的参数值。 如果条件未达成，则计算步长:

I是与Jk具有相同阶数的单位矩阵。F(θk)对每个待求参数求偏导:

n个数据6 个参数构成n×6 的雅可比矩阵Jk。

Step 3 计算增益系数:

如果αk>0，则θk＋1＝θk＋dk；若不满足该条件，则θk＋1＝θk。 Aredk表示目标函数实际差值，Predk表示目标函数预测差值。αk用来检验当前预测参数的好坏。 更新λk:

Step 4 令k＝k＋1，并返回Step 2。

2.2 LM 算法改进

实际获得的数据带有噪声等不确定因素的影响，在第1 节给出误差模型，没有考虑时变等误差[11]，这些误差将直接作用于原始数据，数据的异常使得雅可比矩阵Jk出现偏差，随着时间的推移，更多的异常值最后会导致算法局部寻优失效。 因此LM 算法的核心是调整学习率，其目的是实现两种算法适当的组合:梯度下降法的一致误差减小与高斯-牛顿算法的局部收敛性。 所以重点是对学习率λk的调整。 本文使用一种新颖的λk调整策略[12]:

式中:mu＝max[0.4，min(αk，10)]，Qu为:

式(22)至式(24)对于学习因子λk的策略相比于式(22)增强了收敛性，对于二次方的目标函数提高了收敛速度。 求得的误差参数通过式(2)进行补偿。 综上所述DA-LM 算法流程如图1 所示。

图1 DA-LM 算法流程图

3 未对准误差校正

3.1 未对准误差分析

未对准误差是传感器坐标系与载体坐标系之间的三轴未对准偏差[13]，第1、2 小节DA-LM 估计并未考虑未对准误差，而传感器框架的未对准会大大降低导航系统的准确性，在常规的校准策略中无法确定这些失准，但是可以将该误差与加速度计其他误差区别开来进行估计。 该误差来源主要由安装工艺造成，如图2 所示，其中G代表加速度计坐标，P代表载体坐标。

图2 加速度计及载体之间的未对准

对于未对准校准策略，如地磁场矢量与重力场矢量之间的恒定角度求得旋转矩阵[14]，但是该方法需要额外的磁传感器信息作为辅助，利用已知的无偏地磁场信息校准传感器，分别旋转平台的三个坐标轴导出旋转矩阵。 这种方法对于校准设备要求较高，每次校准都需要平台的一个轴与地磁场相应方向对准。 但是对此方法可作改进运用于加速度计的校准。

3.2 未对准校正策略

定义加速度传感器框架为一个正交框架:Dm，定义平台框架为Dp。Dm与Dp三轴之间存在有小角度(δxy，δyz，δxz)，所以平台框架相对于正交框架存在一个旋转矩阵R:是加速度计测量值，Dpx，Dpy，Dpz是地球重力场的三个分量。 定义[px，py，pz]分别为平台的三个旋转轴。 首先绕py轴旋转，此时Dpy输出为常值，收集加速度计数据，然后通过最小二乘法估计:

R21，R22，R23分别为R第二行参数，通过该行参数可以求解第一行参数，加速度计数据可求得横滚角γ，而因此有:

γ作为真实的横滚角，使平台处于不同的姿态，收集传感器数据，利用最小二乘法估计R的第一行参数。 同理，利用俯仰角及前两步估计的参数可求得R第三行参数:

式中:θ为真实的俯仰角数值，该值通过θ＝求得，旋转矩阵R的所有参数均已求出。 该校准策略的优点是不需要平台的每个轴都与参考矢量的方向对齐。

4 实验及结果分析

4.1 仿真实验

MATLAB 仿真实验，单位球面上均匀取点，每个点对应在球面上的各个方向，采用等夹角均匀取点法形成64 个点，设置的误差参数如表1 所示。

表1 误差参数设置

将以上参数作用到数据点上，生成加速度干扰数据。 用DA 获得误差参数初值，首先需要设置DA初始参数，如表2 所示。

表2 DA 初始参数设置

当数据未加入旋转矩阵干扰的优化结果如表3所示，可以看出优化的部分参数能够达到设定值，不过有些参数与设定值偏离较大。 但是DA 能为下面的工作提供强有力的帮助:能够精确估计偏差，获得椭球参数的全局最优解，这为LM 算法提供了良好的初值运算。

表3 加速度计校正参数(未加入旋转矩阵)

当数据加入了旋转矩阵干扰的优化结果如表4。

表4 加速度计校正参数(加入旋转矩阵)

为了凸显算法的作用，根据不同情况分别给出仿真结果，如图3～图7。

图3 原始与干扰数据

图4 DA 补偿数据

图5 LM-DA 补偿数据

图6 未对准补偿

图7 算法综合对比

图3加入了误差参数的数据呈现出倾斜的椭球体，所有数据点都偏离了单位球体，因偏差的作用干扰数据的原点偏离了中心坐标点，比例因子，非正交误差与未对准误差让球体发生了畸变，数据点的模值均大于1。

图4 中，经过了DA 优化的数据球体大小骤减明显，全局搜索已起到作用，并且校正后的球体圆点已在坐标轴中心，但是球体依然处于倾斜状态，由于不能完全校准参数，使得大量数据其模值小于1。

图5 所示LM 优化DA 的结果参数，球体几乎恢复正常情况，数据点的模值均在1 附近，但是LM 优化后的球体依然处于倾斜状态，因为目前还未校正未对准误差。

图6 中是对数据点分布在不同方向的拟合情况，在这一步中已加入了3.2 小节的未对准校正策略，可以发现经过了旋转矩阵R的补偿后，干扰后的数据点能够拟合在球面上。

图7 所示经过DA 的数据集能够回到坐标轴中心，但数据分布在单位球体的内部，再次运用LM 算法可以将数据集拟合在球面上，但是与原数据的分布在方向上有偏差，经过旋转矩阵R的补偿，受干扰的数据集能够拟合到原数据集上。 在有关磁传感器的校正方法中[15-18]同样能够获得类似于图3～图7 的仿真结果，但本文校正策略不适用于磁传感器误差校正，因为额外存在的软硬误差，无法建立起1.1 小节的误差模型，必须给出针对性的校正方法实现类似于本文图5 的仿真结果。 但本文给出的对准策略能够用于磁传感器，实现类似于图7 的仿真结果，这为以后校正磁传感器的工作提供帮助。

4.2 实测数据验证

为进一步验证算法的有效性，采用自主研发的随钻测斜仪进行现场实验， 该测斜仪主要由STC15F60S2 单片机；ADI 公司推出的基于iMEMS 技术的三轴、数字输出加速度传感器:ADXL345；还包括震动传感器、Flash 存储器(w25x16)、外部时钟模块(DS1302)、蓝牙模块(hc-06)、锂电池(18650)及本安型的电源电路等组成。 其中ADXL345 加速度计传感器拥有最高13 位分辨率，功耗小，灵敏度高，最高达3.9 mg/LSB，以及量程可变范围广等优点。 实验过程中ADXL345 测量的数据通过UART 协议传输到单片机，并存储到Flash 中，最后由蓝牙发送到上位机，用MATLAB 语言编写代码处理数据。 随钻测斜仪外壳，及内部的测量短节如图8 所示。

图8 随钻测斜仪

4.3 参数拟合校正

测斜仪安置固定于无磁转台上，如图9 所示。操作上位机使转台处于不同姿态，通过加速度计获得转台在不同姿态下三个方位的重力分量。

图9 双轴转台

转台X，Y，Z三轴分别转过一定角度，操作转台使其俯仰角、航向角旋转次数、航向角间隔分别为:30°，8，45°；60°，14，25.7143°；90°，16，22.5°；120°，14，25.7143°；150°，8，45°；180°，1。 存储芯片共记录以上68 个加速度计输出点。 利用本文提出算法估计误差参数，其结果如表5。

表5 加速度计校正参数结果

4.4 俯仰角补偿结果

分别采用不同算法补偿误差参数:①单独采用DA补偿数据点，不采用LM 法与未对准补偿；②DA-LM拟合数据，不采用未对准补偿，；③采用DA-LM 并加入未对准补偿。 三种补偿方式的实验结果依次见图10～图12。 在图10 可以发现DA 拟合的数据点可以补偿误差，俯仰角误差有所减小，但是部分误差角仍然较大，因为DA 可以估计传感器误差参数，但是由于局部寻优能力较差，部分误差参数并不能估算出最优解，尤其是不能校正未对准偏差。 图11 经过了DA-LM 的优化其俯仰角误差明显减小，DA 提供的初值给予LM 很大的帮助，LM 优秀的局部搜索能力能够快速找到极值点，误差参数的估计精度有所提高。但是加速度计与测斜仪之间的三轴未对准角度影响加速度计角度的解算。

图10 DA 补偿结果

图11 DA-LM 补偿结果

图12 DA-LM 及加入未对准补偿的结果

加速度计与测斜仪三轴存在角度偏差，因此加速计坐标系与测斜仪坐标系之间存在一个旋转矩阵。 图12 所示没有经过未对准补偿的部分数据点仍然存在较大偏差，在20～30 测量数据点之间俯仰角误差达到0.85°。 用3.2 小节的未对准策略，旋转转台的X轴，让Y轴上升20°，然后旋转Y 轴估计旋转矩阵的第二行元素，然后随意旋转平台的三轴分别估计旋转矩阵的第一行和第三行元素。 图12 中补偿结果明显，俯仰角不存在较大的偏差值。 补偿前俯仰角最大误差为4.7°，补偿后俯仰角最大误差为0.39°，其精度提高了一个数量级。

4.5 横滚角补偿结果

图13 转台水平旋转横滚角误差

转台保持水平不变，让转台Z轴旋转一周取得62 个数据点，观察加速度计解算横滚角的情况。 如图13 所示，横滚角在其起始旋转阶段及结束旋转阶段偏差较大，最大偏差超过了0.3°。 在置于平台俯仰角分别为30°(水平旋转8 次)；60°(水平旋转14次)；90°(水平旋转16 次)；120°(水平旋转14 次)；150°(水平旋转8 次)；180°(水平旋转1 次)时分别记录横滚角输出(如图14 所示)，横滚角最大偏差达超过了0.9°，能够说明俯仰角对横滚角的解算是有影响的，尤其是未对准误差，经过DA-LM 并加入未对准补偿的情况如图14。

图14 补偿前后横滚角误差

可以看出补偿结果明显，说明传感器误差能够有效估计，可将横滚角误差控制在0.3°左右。 以均方根误差(Root Mean Square Error，RMSE)衡量校正前后加速度计解算姿态角的偏差。

式中:wk代表实验中真实的俯仰角与横滚角，Δ代表由三轴加速度计解算得到的俯仰角及横滚角。

由表6 所示，本文整套算法优于DA-LM 算法，证明未对准对姿态的解算影响不小。 本文提出的整套算法相比于DA 估计精度提高了4 倍。

表6 不同补偿下姿态角的均方根误差

图15 所示，校正后的数据点模值正常，同时也证明了本文算法的有效性。

图15 校准前后重力场模值比较

4.6 算法计算效率

由图1，DA 中适应度值计算至更新蜻蜓位置的迭代环节决定了该算法计算效率，最大迭代次数Max 越大算法运行时间越长；LM 算法运行效率的高低在于增益系数αk的计算、参数θk以及学习率λK更新，但LM 算法本身计算效率较高，且本文所用λK的更新策略极大提高了算法的收敛速度，故LM在计算效率方面要远高于DA。 采用双轴转台实验数据验证DA-LM 算法的计算效率，DA 最大迭代次数100，LM 算法最大迭代次数500，用MATLAB 中etime( )与clock 的组合代码计算程序运行时间。验证计算效率时不考虑未对准策略，因为该策略采用的是最小二乘估计，输出结果很快，对本文整套算法的运行时间无太大影响。

表7 所示，LM 算法相比于DA 其运行效率超出60%，但DA-LM 算法整体运行时间不超过3 s。

表7 不同算法运行时间

5 结论

本文通过对加速度计的误差建模，分析误差参数与误差方程的关系，提出DA-LM 算法。 首先利用DA对误差参数的全局寻优然后利用LM 算法局部寻优获得最优值，通过估计旋转矩阵实现加速度计坐标系与载体坐标系的对准。 本文所提出对加速度计误差的一种完整补偿方案，从实验结果可以看出，加速度计对俯仰角与横滚角的解算精度明显提高，证明该方法在随钻测量系统中的应用具有一定的参考性。