基于Levenberg-Marquardt算法的无人机多传感器校正方法研究

肖舜仁，胡青春，李妮妮，陈兴彬

(1.华南理工大学机械与汽车工程学院，广东广州 510641；2.广州机械科学研究院有限公司，广东广州 510700；3.中汽检测技术有限公司，广东广州 510700；4.广东省生产力促进中心，广东广州 510075)

0 前言

四旋翼无人机依靠自身搭载的低成本MEMS(Microelectro Mechanical Systems)传感器实现姿态的实时获取是其在诸多应用领域中正常控制作业的先行条件，而基于MEMS的惯性传感器如三轴加速度计、三轴陀螺仪在使用过程中易受安装、测量、信号传输等误差的影响，导致无人姿态信息获取的精度降低。因此，在无人机投入使用前，需对其搭载的MEMS传感器进行误差校正补偿，以获得更为精确的传感器数据，为后续进行多源传感器信息融合与姿态解算奠定基础。国内外学者对MEMS传感器的校正补偿问题已进行了大量的研究。宋颖、刘百奇等对加速度计进行了较高精度的校正，但两者都借助了精密仪器，成本高且局限性大。张红宇、陈剑等人采用了无需转台的快速校正方法，完成对加速度计的校正补偿，但前者校正精度不足，后者需先后进行粗校正和精确校正，较为繁琐。原雨佳等使用改进的粒子群优化算法对磁力计误差模型中的12个未知参数进行了最优估计，该方法校正补偿精度较高，但算法实现较为繁琐且仅能进行参数离线估计。WU等基于L-M算法实现磁力计数据的球体拟合，得到球体半径、硬磁和软磁误差的最优估计。王坚等人使用改进的L-M算法，基于校正后的加速度计以及动态旋转完成陀螺仪误差参数的快速估计，但该方法未考虑磁力计与其他两种传感器之间的相互影响，且仅通过加速度计无法得出偏航角，联合校正存在局限性。

为解决上述传感器校正过程中存在的需依靠精密仪器、校正繁琐及联合校正局限性的问题，本文作者提出一种简便的基于L-M算法的加速度计18位置快速校正和陀螺仪联合校正方法，采用四元数进行姿态表征，将校正后的比力和磁感应强度数据转换为陀螺仪校正的参考四元数值，实现陀螺仪误差模型中待求参数的最优估计。

1 传感器误差来源与建模

1.1 误差机制分析

常见的无人机MEMS传感器包括三轴加速度计、三轴陀螺仪和三轴磁力计。根据各传感器测量原理和误差机制的区别，可将其安装、测量、信号传输误差大致可分为：非正交误差、尺度因子和零偏误差。但对磁力计而言，除以上误差外，无人机自身部件的磁场分布和外界电磁干扰而产生的硬磁和软磁误差才是磁力计的主要误差来源。

1.1.1 非正交误差

三轴传感器的非正交误差是指在制造和安装过程中，由于无法保证3个测量轴两两正交而引起的测量误差。定义三轴传感器的坐标系为SF，而理想的传感器正交坐标系SOF，两者之间的联系是：(1) SOF的轴与SF的轴重合；(2) SOF的轴在SF的轴与轴构成的平面上。

假设无人机机体坐标系为严格正交的BF，则非正交坐标系SF与正交坐标系BF的偏差如图1所示。

图1 三轴传感器坐标系与机体正交坐标系间的偏差

、、为机体正交坐标系三轴方向，、、为三轴传感器矢量方向。对于小角度而言，非正交坐标系下的测量值在机体正交坐标系下的变换关系如式(1)：

=

(1)

式中：与分别代表传感器坐标系与机体系下传感器测得的三轴数据；是指第个传感器绕第个机体坐标系BF轴的旋转角度。本文作者所提出的传感器校正方法，需假设机体正交坐标系BF与理想坐标系SOF重合，即、、都为0，则三轴传感器的非正交矩阵可简化为

(2)

1.1.2 尺度因子

三轴传感器的尺度因子是指传感器各测量轴的灵敏度、测量信号的放大电路特性不完全相同而引起的测量误差。尺度因子误差矩阵为

(3)

式中:、、为传感器的三轴尺度因子。

1.1.3 零偏误差

零偏是指输入为零时传感器的输出值，实际测量时，传感器测量值与实际值之间存在一个固定的差值，其矩阵表示为

(4)

式中:、、为传感器的三轴零偏。

1.1.4 硬磁和软磁误差

硬磁误差是由无人机上装载在磁力计附近的电机、通电导线等设备中的硬磁材料产生的恒定磁场引起的，该误差相对于磁力计而言，大小和方向是固定的。软磁误差主要是由地球磁场与磁力计周边的软磁材料作用而产生的局部干扰磁场所产生的。

1.2 误差模型

1.2.1 三轴加速度计误差模型

结合三轴加速度计的非正交误差、尺度因子和零偏误差，建立加速度计误差模型为

(5)

1.2.2 三轴陀螺仪误差模型

结合三轴陀螺仪的非正交误差、尺度因子和零偏误差，建立陀螺仪误差模型为

(6)

1.2.3 三轴磁力计误差模型

同理，结合三轴磁力计的非正交误差、尺度因子、零偏误差、硬磁和软磁误差，建立磁力计误差模型为

=(+′)+

(7)

式中:为校正前磁力计测得的磁感应强度；为校正后磁力计在机体正交坐标系下的理想磁感应强度；为软磁误差；′为硬磁误差。为简化矩阵运算，将非正交误差矩阵、尺度因子矩阵与软磁误差矩阵相结合，又对零偏、硬磁误差一并校正，则上述磁力计误差模型可简化为:=(+)。

其中:为比例矩阵；为三轴偏置矩阵。误差模型展开形式如下：

(8)

2 基于L-M算法的多传感器校正

2.1 Levenberg-Marquardt算法

2.1.1 非线性最小二乘问题

非线性最小二乘问题在拟合数据、参数估计和函数逼近等方面有着广泛的应用。其指标函数由若干个误差函数()的平方和组成，一般形式为

(9)

(10)

其中:()为()的Jacobian矩阵；()为Hessian矩阵。解得牛顿法迭代步为

Δ=-()()

(11)

高斯牛顿法对(+Δ)进行一阶Taylor展开，并用Jacobian矩阵的乘积近似代替牛顿法中的二阶Hessian矩阵，运算得高斯牛顿法迭代步为

Δ=-(()())(()())

(12)

综上，牛顿法在迭代优化时运用了梯度和梯度变化率的信息，虽然收敛速度快，但需消耗相当大的计算量，且当Hessian矩阵为奇异矩阵时，迭代搜索将会失败；高斯牛顿法提高了迭代运算效率，但近似Hessian矩阵()()仍可能是奇异矩阵。

2.1.2 L-M算法

L-M算法可看做是两种最优化方法：最速下降法和高斯-牛顿法的结合，可用于解决上述Jacobian矩阵非满秩导致的奇异问题。L-M算法将高斯牛顿法的迭代步改进为

≥0

(13)

其中:为单位矩阵；当阻尼系数较大时，μ占主要地位，此时L-M算法相当于最速下降法；当阻尼系数较小时，近似Hessian矩阵()()占首要地位，此时L-M算法将近似为高斯牛顿法。

2.2 基于L-M算法的三轴加速度计校正

应用L-M算法校正三轴加速度计，需估计以下参数：

(14)

加速度计误差模型可记为

(15)

(16)

使用L-M算法进行迭代前，需给定的初始值，首先求出()中9个待求参数的Jacobian矩阵()，并根据经验设置阻尼系数的初值为

(17)

式中:为矩阵的对角元素；为经验给定的正常数。

另设误差函数为(+)，有=Δ，对(+)进行一阶Taylor展开，得：

(18)

指标函数(+)可近似表示为

(19)

式中:=()；=()。

在L-M算法的迭代过程中，可根据增益系数的正负符号来判断当前迭代点是否准确有效，且可根据增益系数来计算下一步迭代的阻尼系数。增益系数的计算如下：

(20)

其中，

(21)

根据式(13)可知，和-都为正，所以(0)-()>0。若>0，表明当前指标函数拥有下降趋势，此时可减小的值，使下一次迭代更接近于高斯牛顿迭代；当≤0时，应增大的值，减小步长并使得下一次迭代更接近于最速下降迭代。在迭代过程中，当变量满足一定收敛条件时，迭代停止，判定算法迭代收敛，可将此时待求参数的估计作为最优估计输出。

基于L-M算法进行三轴加速度计校正，建立迭代校正流程如图2所示，个别参数采用默认或经验值。

图2 基于L-M算法的加速度计迭代校正流程

2.3 基于L-M算法的三轴磁力计校正

使用L-M算法校正三轴磁力计，需估计以下参数：

=[]

(22)

(23)

(24)

2.4 基于L-M算法的三轴陀螺仪联合校正

使用L-M算法校正三轴陀螺仪，需估计以下参数：

(25)

文中所提出的三轴陀螺仪校正方法，是在已校正的加速度计和磁力计的基础上对陀螺仪进行联合校正，以此估计未知参数：

(26)

则第次测量校正得到的欧拉角为

(27)

其中:

为避免奇异问题，使用四元数进行姿态更新，将上述第次测量校正得到的欧拉角转换为相应的四元数∈。

定义操作如下：

(28)

综上，陀螺仪联合校正原理可表示为：期望估计值′应尽可能与校正后的比力和磁场强度转换而成的四元数相接近。因此，可建立以下优化函数来估计待求参数：

=1,2,…,

(29)

3 校正实验流程与结果分析

3.1 三轴加速度计校正实验

加速度计校正实验使用无人机上装载的MPU9250型MEMS传感器中集成的三轴加速度计，分别采集无人机18个方向静止时的姿态信息。以机体轴--进行描述，该18个静止姿态分别为：轴垂直向上、轴垂直向下、轴垂直向上、轴垂直向下、轴垂直向上、轴垂直向下；轴与水平方向呈±30°、±45°、±60°夹角；轴与水平方向呈±30°、±45°、±60°夹角。将采集到的加速度计比力数据通过串口发送到上位机中进行记录保存。

筛取上述各静止姿态比力数据并取均值，在MATLAB中使用L-M算法对加速度计误差模型中的待求参数进行最优估计。非正交误差、尺度因子、零偏误差的最优迭代估计结果如下：

(30)

利用上述迭代估计参数进行加速度计误差模型校正，图3为校正前后18个位置的加速度计比力模值与重力加速度的差值随迭代采样的变化。

由图3可知：基于L-M算法对加速度计进行校正后，各组加速度计数据比力模值与重力加速度的差值在0附近小范围波动，远小于校正前，指标函数的值由0.307 3降为0.003 2，下降了98.96%。结果表明：本文作者提出的基于L-M算法的18位置三轴加速度计校正方法，对于低成本的加速度计具有良好的校正效果。

图3 校正前后比力模值与重力加速度之差

3.2 三轴磁力计校正实验

磁力计校正实验使用无人机上装载的MPU9250型MEMS传感器中集成的AK8963型磁力计，分别采集无人机绕机身、、轴和两条机臂轴旋转数周的磁感应强度数据。实验采集的原始数据共计703个样本点，在MATLAB中使用L-M算法对磁力计误差模型中的待求参数进行最优估计，当地磁场强度、比例误差和偏置误差的最优迭代估计结果如下：

利用上述迭代估计参数进行磁力计误差模型校正。图4为磁力计采集的数据初始值和L-M算法校正后磁力计数据的理想值在三维空间的分布状况，图5为校正前后磁感应强度模值的变化状况。

图4 磁力计初始值与L-M算法校正后的数据三维分布

由图4可知:校正前磁力计采集的数据与坐标原点偏差较大，且样本点分布呈近似椭球形；校正后的磁感应强度理想值分布在球心为坐标原点、半径为地磁强度的球面上，表明该三轴磁力计存在较大的偏置误差和较小的比例误差。如图5所示，校正后磁感应强度模值始终保持在35.3 μT附近。以上结果验证了基于L-M算法对三轴磁力计进行校正的有效性。

图5 校正前后磁感应强度模值

3.3 三轴陀螺仪联合校正实验

陀螺仪联合校正实验使用无人机上装载的MPU9250型MEMS传感器中集成的三轴陀螺仪采集角速度数据。具体地，将无人机分别绕机身、、轴正方向匀速旋转3次，每次旋转的初始位置和角度不一，采集旋转前后静态区间内的加速度计、磁力计和两静态区间内的陀螺仪数据。根据陀螺仪联合校正原理，在MATLAB中使用L-M算法对陀螺仪误差模型中的待求参数进行最优估计。非正交误差、尺度因子、零偏误差的最优迭代估计结果如下：

(32)

利用上述迭代估计参数进行陀螺仪校正，图6为校正前后无人机匀速旋转后静态测量的比力和磁感应强度数据解算而成的四元数，与根据RK4方法计算的四元数估计值之差的模值随迭代采样的变化。

图6 校正前后四元数解算值与估计值之差的模

如图6所示：基于L-M算法对陀螺仪进行联合校正后，四元数解算值与估计值之差的模值显著减小，校正指标由0.056 5降为0.014 5。上述结果验证了基于L-M算法对三轴陀螺仪进行联合校正的有效性。

4 结论

基于L-M算法完成了对MPU9250型低成本MEMS传感器误差的快速校正补偿。

(1) 建立传感器简化误差模型，基于不同传感器的校正原理建立优化函数，从而采用L-M算法迭代运算得到误差模型中待求参数的最优估计。

(2)设计传感器校正实验，在不依赖精密仪器的前提下，完成对加速度计、磁力计的快速校正，并解决了陀螺仪联合校正的局限性问题。实验结果表明:文中的校正方法对机载低成本MEMS传感器误差具有良好的校正补偿效果。

(3)在加速度计和磁力计校正时，采用的是单独校正的方法，未考虑两传感器间的相互作用及影响；陀螺仪校正时，由于实验无人机匀速旋转的控制精度不高且磁力计受外界环境影响波动较大，陀螺仪的联合校正未达到预期效果。后续研究将注重探讨加速度计和磁力计的联合校正，提高无人机匀速旋转的控制精度，降低磁力计受外界电磁干扰的影响，提高陀螺仪误差校正补偿的可信度。