LM优化算法的IMU阵列在线校准

刘国念 江金光 吴家骥 龚一民 杜 莹

1 武汉大学卫星导航定位技术研究中心，武汉市珞喻路129号，430079 2 武汉梦芯科技有限公司，武汉市高新大道980号，430079

低成本的MEMS IMU由于随机误差较大，应用于惯性导航系统时误差增长较快。利用多个传感器构建IMU阵列，通过不同的融合方式融合冗余的观测数据能有效降低随机误差[1]。但由于集成电路制造工艺的缺陷，每个IMU的输出都会存在因零偏、比例因子和非正交[2]偏离理想值引起的差异。此外，多个传感器放置在一起时，安装误差也会导致IMU的输出存在差异。因此，为了更好地运用IMU，在使用前需要对传感器进行校准。

传统的IMU阵列的校准需要通过昂贵的精密转台提供所需的已知激励[3-5]。但转台的使用增加了IMU阵列的使用成本，因此，不使用转台的IMU阵列在线校准方法成为研究热点。在静态条件下，可利用地球重力加速度作为激励校准加速度计[6]误差参数，但由于地球自转角速度数值较小，无法从MEMS IMU的噪声中分离，缺少参考激励使得陀螺仪误差校准方法较少使用。Rohac等[7]使用激光陀螺获得参考激励的精确数值，但增加了额外成本。Carlsson等[8]提出一种块坐标下降法(block coordinate descent, BCD)联合估计误差参数(包含IMU之间的距离)和载体的运动参数，但该方法需要提供较大激励，难以控制输入。

本文提出一种IMU阵列在线校准方法，通过对IMU阵列的误差参数构建相应的损失函数，利用LM优化算法估计加速度计和陀螺仪的误差参数。该方法无需任何外部参考设备，只需在手机支架上获得一系列静置和运动数据。

1.1 IMU误差建模

(1)

(2)

(3)

1.2 损失函数

1.2.1 加速度计损失函数

将第k个加速度计需要校准的参数定义为：

(4)

静止状态下，加速度计的比力输出只与当地重力加速度有关，则加速度计输出估计函数为：

(5)

将导航坐标系定义为北东地，载体坐标系定义为前右下，此时：

(6)

式中，φ、γ分别为横滚角、俯仰角，g为当地的重力加速度。

由式(6)可以看出，每个静态位置存在横滚角和俯仰角2个未知数，因此加速度计在每个位置多输出了1个冗余量。若要校准加速度计的所有参数，至少需要放置12个或者9个非共面的位置。本文采用一种多个位置的静态校准方式校准加速度计，将加速度计校准参数的损失函数建模为：

(7)

1.2.2 陀螺仪损失函数

式中，tc为冷水平均温度，℃；
t2′为冷水进口温度，℃；
t2″为冷水出口温度，℃；
Rec为冷水侧雷诺数；
V2为冷水流速，m/s；
vc为冷水运动粘度，m/s；
λc为冷水侧导热系数，W/m·K；
Prc为冷水侧普朗特数；
Nu2为冷水侧努塞尔系数；
h2为冷水侧换热系数，W/m2·K.

将第k个陀螺仪需要校准的参数定义为：

(8)

由于MEMS工艺制造的IMU噪声太大，地球自转角速度无法分离而不能成为参考源。静置的加速度输出通过校准后具有很好的稳定性，且能得到各个静态位置重力的投影值，本文将其作为陀螺仪校准的参考值。在假定陀螺仪是无偏的前提下，即陀螺仪的数据通过一定时间内的平均可以得到零偏，利用2个静态位置之间转动过程中陀螺仪的数据，通过数值积分方法得到2个静态位置之间的旋转矩阵。利用静态位置和旋转矩阵估计下一个静态位置的重力投影估计值(式(9))，进而构建陀螺仪损失函数(式(10))：

(9)

(10)

1.3 算法设计

式(7)和式(10)是非线性函数，本文利用LM优化算法迭代求解出待定误差参数最优解。LM优化算法对于初值的设定比较严格，初值误差较大会使迭代进入局部最优。因此，除了陀螺仪的零偏，将加速度计和陀螺仪的确定性误差初值都设定为理想值，N个静态位置的横滚角和俯仰角的初值使用加速度计的原始输出计算：

(11)

(12)

其中，[p]i为向量p中的第i维数值。

数值积分使用经典的4阶Runge-Kutta积分，通过对2个历元之间的角速度积分得到当前历元的姿态四元素，并通过递推得到2个静态位置之间的旋转矩阵[9]。

图1为在线校准算法流程，首先利用加速度计的方差构建一个静态检测器，将静态和动态数据分离；
然后使用LM算法和静态数据对式(7)进行迭代求解，当整体误差和小于阈值时，加速度计校准完成；
最后利用加速度计校准后的静态数据和动态数据对式(10)进行迭代求解，得到陀螺仪的误差参数。

图1 在线校准算法流程Fig.1 Flow chart of online calibration algorithm

为验证本文算法的有效性，分别进行仿真实验、实测校准实验和车载导航实验。

2.1 仿真实验

表1 ICM42688P误差标称参数Tab.1 Error nominal parameters of ICM42688P

利用文献[10]的开源MATLAB惯导工具箱中的轨迹生成器进行如式(13)和(14)的拓展，得到IMU阵列的仿真数据，其频率为100 Hz：

(13)

(14)

图2 IMU阵列分布Fig.2 Distribution of IMU array

将校准结果与设定的误差参数作差得到参数的残差值，结果如图3所示。从图3(a)看出，加速度计和陀螺仪的零偏精度分别达到1.3×10-4m/s2和1.3×10-3dps；
从图3(b)看出，加速度计和陀螺仪的尺度变换矩阵精度分别达到3.3×10-5和7.9×10-5，校准结果与设定的误差参数值一致性较好。

图3 残差统计Fig.3 Residuals statistics

2.2 实测校准实验

实测校准实验使用实验室内部设计的一款IMU阵列，其中IMU使用InvenSense公司的ICM系列产品。为兼顾IMU阵列的性能和MCU管脚的使用情况，所设计的阵列由9个IMU构成3×3的矩阵，如图4(a)所示。在IMU阵列的驱动设计过程中，使用串行外设接口(serial peripheral interface, SPI)通信协议，并利用ICM42688P内部的FIFO获取原始数据，减少SPI片选信号的开关次数，提高系统的时间同步可靠性。加速度计量程设置为±2g，陀螺仪量程设置为±250 dps，量程范围与常用的车载量程设置相同，便于将校准参数应用于车载导航解算中。

图4 IMU阵列及校准平台Fig.4 IMU array and calibration platform

阵列校准平台如图4(b)，该校准平台上是一个手机支架，可以进行360°手动旋转，并且在数据采集过程中，能使阵列数据在运动后短时间内稳定为静态数据，同时保证转动角速度控制在量程范围内。为降低加速度的噪声以及减少Runge-Kutta积分过程造成的误差，将IMU阵列在校准平台上的静置时间设定为8～10 s，动态间隔时间设定为1～4 s，每组校准实验采集35～50个静态和动态数据。表2、3分别是8组校准实验的均值和误差参数范围统计。可以看出，加速度计零偏及尺度变换波动范围分别为-83～59 mg和-0.005～0.008，陀螺仪零偏及尺度变换波动范围分别为-1.1～0.54dps和0.013～0.016。ICM42688P的各项误差数值都偏小，因此对传感器补偿后性能提升有限制。

表2 校准实验均值统计Tab.2 Statistics of mean value of calibration experiments

表3 IMU阵列误差参数范围统计Tab.3 Error parameter range statistics of IMU array

2.3 车载导航实验

使用图4(a)中的IMU阵列进行车载动态组合导航实验。将IMU阵列数据通过直接平均的方式拟合成一个虚拟IMU，并利用虚拟IMU与GNSS进行松组合导航；
通过后处理设置GNSS中断，中断时间为20 s，中断间隔为60 s，比较GNSS中断后校准前后的导航误差。参考设备为高精度、具有双频RTK的NovAtel CPT6，定位精度可达cm级。由于ICM42688P的加速度计高程方向存在低频噪声、误差较大，因此本文只评价导航结果的平面误差。

图5展示了一组GNSS中断后动态导航的平面、北向和东向误差。可以看出，校准后的导航误差精度整体有所提升，尤其是误差较大的路段。GNSS中断后的不同时间段误差发散程度不一致是由IMU的随机误差引起的，随机误差越大，误差发散越快。校准后，能补偿IMU阵列的确定性误差，进行数据融合时能有效降低随机误差，在随机误差较大的路段，校准后的IMU阵列能大幅度降低随机误差，改善效果较为明显。

图5 动态导航误差Fig.5 Dynamic navigation errors

3组不同GNSS中断实验的平面误差RMS统计结果如表4所示(t_sta为起始导航时刻，t_sta+50 s为起始中断时刻)。可以看出，IMU阵列校准后，3组中断实验精度均有提升，平均提升19.1%。精度提升数值较小可能是因为所使用的IMU阵列的误差数值较小，同时直接平均的数据融合方式对随机误差的降低有限；
另外，大量的数值积分会存在计算误差，使得校准的数值精度下降，进而影响动态导航的精度。

表4 动态导航平面RMS值Tab.4 Plane RMS of dynamic navigation

本文提出一种在线IMU阵列校准方式，分别对加速度计和陀螺仪构建损失函数，利用求解非线性函数的LM优化算法，迭代得出IMU阵列的各个误差项。仿真实验结果显示，加速度计校准的零偏和尺度变换误差残差精度达到1.3×10-4m/s2和3.3×10-5，陀螺仪校准的零偏和尺度变换误差残差精度达到1.3×10-3dps和7.9×10-5，误差参数设定值与校准值一致性较好。3组实物测试结果显示，校准后IMU阵列的动态导航精度平均提升19.1%，表明该方法能有效校准IMU阵列的误差参数，提高动态导航性能。