GPS辅助的SINS系统快速动基座初始对准

2020-12-15王伟，刘萌，薛冰

哈尔滨工业大学学报 2020年12期

王伟，刘萌，薛冰

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

捷联式惯性导航系统(strapdown inertial navigation system, SINS)根据陀螺仪输出的角速率和加速度计输出的比力可以计算出载体的姿态、速度和位置，被广泛应用于各种军用和民用行业.SINS系统在进行导航前需要进行初始对准，初始对准的精度直接影响SINS系统的导航精度.因此，精确的初始对准对SINS系统尤为重要.

对于高精度SINS系统，系统可以在静基座环境下通过感应地球自转和重力加速度确定出初始姿态，系统的位置和速度可以通过GPS系统得到.但是在一些紧急情况下，SINS系统需要在运动过程中进行初始对准，传统的静基座对准算法就不再适用.对于低精度的SINS系统，例如MEMS-IMU，系统本身的噪声大于地球自转角速率，同样无法进行静基座对准，需要在外部传感器辅助的情况下通过载体的运动进行初始对准.

近年来，研究人员对动基座初始对准进行了研究，提出了许多动基座对准算法.文献[1]对空中对准算法进行了研究，算法利用加速度计的输出计算初始水平姿态，利用GPS航迹向作为初始航向角，进行粗对准，然后利用自适应扩展卡尔曼滤波进行精对准，文章中只考虑了大方位失准角下的误差模型，但是利用加速度计得到的水平姿态可能会存在较大误差.文献[2]中算法则不用考虑初始姿态信息，使用OBA算法实现空中对准.通过引入初始导航惯性坐标系和初始载体惯性坐标系，将姿态矩阵拆分成3部分，分别为导航系与初始导航惯性系之间的方向余弦矩阵、载体系与初始载体惯性系之间的方向余弦矩阵和初始导航系与初始载体系之间的姿态矩阵.其中，前两个矩阵是时变矩阵，可以通过GPS数据和惯导数据对其进行计算，第3个矩阵为常值矩阵，利用基于优化的四元数法可以实现对常值矩阵的计算.该算法虽然能够实现快速对准，但算法没有考虑惯性器件误差带来的影响.文献[3]对文献[2]中的方法进行了进一步的研究和详细的推导，分别对速度积分和位置积分的效果进行了比较，但是文献[3]指出，该算法只适用于高精度的惯导系统.文献[3]中的算法只是单纯的对姿态进行估计，没有对惯性器件的误差进行补偿，文献[4]针对这一问题进行研究，使用迭代牛顿拉格朗日算法进行解算，对GPS杆臂误差和惯性器件的误差进行估计，然而算法依然只适用于高精度的惯导系统.文献[5-6]在假设水平姿态失准角为小角度的情况下，将方位失准角的三角函数作为状态变量，这样系统模型便转化成线性，减小计算量，但是在对准速度上并没有明显的提高.文献[7]对所有姿态角均为大失准角的情况进行了非线性误差方程的推导，并根据量测方程为线性的特性，将UKF算法进行简化，并证明了算法简化的可行性，减小了UKF算法的计算量.文献[8]用四元数定义姿态误差，对四元数形式的非线性误差方程进行推导，用UKF算法对系统进行滤波，可以实现大失准角的动基座对准，即使是90°的大方位失准角算法也可以收敛，但是算法中没有考虑UKF采样对四元数带来的影响.文献[9]将OBA算法与卡尔曼滤波结合，利用里程计作为辅助传感器对陀螺仪误差引起的失准角进行估计，并将估计结果反馈给OBA算法，从而提高对准精度，算法中虽然考虑了惯性器件误差的影响，但是没有对其进行估计，需要根据经验手动的对陀螺仪误差进行设置.文献[10]针对OBA算法会累积惯性器件误差的问题提出滑动窗积分计算的方法，防止误差的累积，但是该方法需要对窗口内的数据进行存储和积分运算，计算量会随着窗口的增大而增大.文献[11]中，根据飞机起飞前的运动特点建立了飞机的运动约束模型，从而达到在不增加其他传感器的情况下增加量测信息的效果，进一步提高动基座对准的速度和精度，但是这种方法只适用于飞机在跑道上的动基座对准，有一定的局限性.文献[12]使用IIR滤波器去除加速度计中的高频噪声，对提高对准精度有一定的帮助，但是没能从根本上解决惯性器件误差带来的问题.

本文在OBA算法的基础上，推导了陀螺仪误差与载体系和计算载体系的失准角之间的关系，并根据此关系建立非线性误差方程，然后利用UKF算法对失准角和陀螺仪常值漂移进行估计，将估计结果反馈给OBA算法，从而提高对准的精度，使算法可应用于低精度的惯导系统.考虑到量测噪声的不确定性，引入自适应滤波算法对量测噪声进行估计，提高系统的稳定性.

1 OBA动基座对准算法

1.1 姿态矩阵分解

(1)

(2)

(3)

1.2 速度积分计算

在不考虑误差的情况下，比力方程可表示为

(4)

(5)

(6)

对式(6)两边同时积分并整理可得：

(7)

其中：

(8)

(9)

OBA算法在计算方式上完全解析，不需要像传统的卡尔曼滤波一样将部分变量用上一时刻的代替，但是OBA算法没有考虑惯性器件误差带来的影响，因此OBA算法更适合用在高精度SINS系统中，当用在低精度SINS系统中时，该算法的可靠性就无法得到保障.本文接下来将对SINS/GPS组合系统的误差进行分析，对SINS系统的误差方程做一个新的推导，使其能够与OBA算法结合，然后建立一种新的非线性系统模型，利用自适应无迹卡尔曼滤波算法对失准角和陀螺仪误差进行估计，从而在保证算法对准速度的同时提高对准的精度.因为算法中考虑了陀螺仪的误差影响，所以算法可以用在低精度的SINS系统中.

2 系统的误差分析及建模

2.1 系统误差分析

(10)

(11)

式中：式(10)中的φn可由式(11)计算得到，矢量φn为导航系由tk-1时刻到tk时刻的等效旋转矢量；‖φn‖为矢量φn的模.

(12)

(13)

(14)

式中：式(13)中的φb可由式(14)计算得到，矢量φb为载体系由tk-1时刻到tk时刻的等效旋转矢量，其计算方式采用双子样计算；Δθ1、Δθ2为相邻两时刻的陀螺仪角增量.

(15)

(16)

将式(16)带入式(15)可得：

(17)

对式(17)两端求导可得：

(18)

(19)

(20)

将式(3)、(15)、(16)、(17)、(19)、(20)带入式(18)可得：

(21)

(22)

整理式(22)可得：

(23)

(A1×)(A2×)-(A2×)(A1×)=(A1×A2)×.

(24)

则式(23)可根据式(24)简化为

(25)

式(25)进一步化简有

(26)

(27)

式(27)为载体系与计算载体系之间的误差方程，该误差方程反映了陀螺仪误差对SINS系统姿态估计精度的影响，通过式(27)可以对SINS系统进行建模，从而估计出SINS系统的失准角和陀螺仪误差，提高算法的对准精度和适用范围.

2.2 SINS/GPS系统建模

2.2.1 系统状态方程

根据式(27)，选取等效旋转矢量φ和陀螺仪常值漂移εb作为状态变量X，即

(28)

(29)

式中Wk是均值为零、协方差为Qk的系统白噪声.

2.2.2 系统量测方程

在tk时刻，式(8)可表示为

(30)

式(30)继续变换可得：

(31)

(32)

根据式(31)、(32)，式(7)可表示为

(33)

(34)

(35)

对式(35)化简并整理得

(36)

将系统的量测方程表示为：

Zk=HkXk+Vk,

(37)

(38)

(39)

式(36)、(37)相对应，其对应关系如式(38)、(39)所示.量测方程中的Vk是均值为零的量测白噪声，其协方差记为Rk.

3 自适应无迹卡尔曼滤波算法

由于系统的状态方程是非线性的，所以普通卡尔曼滤波不适合本文中的系统.无迹卡尔曼滤波在UT变换的基础上，根据KF滤波算法的原理，使用一系列的采样点进行采样计算来逼近系统的非线性分布，从而减小系统线性化带来的误差，提高滤波的精度[13].所以本文中选择UKF算法对状态变量进行滤波估计.

滤波过程中使用式(28)作为滤波的状态变量，使用式(29)和式(38)作为UKF滤波时的状态方程和量测方程.根据式(29)和式(37)可以看出，系统的状态方程为非线性，量测方程为线性，所以在使用UKF算法时可以对量测更新部分做进一步简化，使用线性卡尔曼滤波的方式进行量测更新，从而减少采样次数和计算量.简化后的量测更新如下：

在进行UKF算法滤波时，量测噪声方差矩阵R设置的不准确会导致系统滤波的不稳定.在本文中，量测方程是由速度、比力等的积分构成的，很难通过手动调试将量测噪声方差矩阵设置准确.针对这一问题，本文引入Sage-Husa自适应滤波算法对量测噪声方差矩阵R进行实时估计，从而提高系统的滤波精度和稳定性.对量测噪声方差矩阵R的估计公式如下：

4 算法的计算机仿真

4.1 仿真轨迹设置

在生成仿真轨迹时主要是对SINS系统中陀螺仪和加速度计的参数、GPS系统的参数、SINS系统的初始条件和SINS系统的运动状态进行设置.

1)仿真参数设置为：陀螺仪的常值漂移和随机噪声分别为1.0°/h和0.1°/h，加速度计的常值零偏和随机噪声分别为100 ug和50 ug，陀螺仪和加速度计的输出频率均为100 Hz；GPS系统的速度误差为0.05 m/s，位置误差为5 m，GPS系统的输出频率为10 Hz.

2)SINS系统的初始状态设置为：SINS系统的初始位置为北纬45°和东经126°，初始姿态为航向角45°、俯仰角0°和横滚角0°，初始速度为东向速度5 m/s、北向速度5 m/s和天向速度0 m/s.

3)SINS系统的状态变化为：载体匀速运动50 s→左转弯90°，用时5 s→匀速运动50 s→右转弯90°，用时5 s→匀速运动50 s→左转弯90°，用时5 s→匀速运动50 s→右转弯90°，用时5 s→匀速运动50 s，总的运动时长为270 s.图1为载体运动轨迹变化，图2为载体的航向角变化.

图1 仿真运动轨迹

图2 航向角变化

4.2 仿真结果

使用设计好的运动轨迹对所提出的快速动态对准算法(FIMA)以及现有的算法进行仿真对比.图3为仿真过程中OBA算法与所提出的FIMA算法的姿态误差对比图，由图2中效果可以看出，使用所提出的FIMA算法航向角误差δYaw在80 s可以收敛到0.2°，并且航向角误差能够始终保持在0.2°以内，而OBA算法虽然也可以快速收敛，但是其航向角误差会随着对准时间的延长而变大.图中两算法的俯仰角误差δPitch和横滚角误差δRoll均能快速收敛，且收敛的精度相差不大，但是FIMA算法的水平姿态误差更稳定.图中黑色箭头所指部分红色和蓝色凸起是由于参考姿态与算法解算姿态频率不一致引起的.

图3 与OBA算法的姿态误差对比图

图4为FIMA算法与文献[1]中的EKF算法的姿态误差对比图，因为FIMA算法针对的是任意初始姿态，所以为了对比算法的效果，在仿真时EKF算法的姿态初始误差设置为航向角误差50°、俯仰角误差15°和横滚角误差15°的大失准角误差.由图中效果可以看出，与非线性EKF动态对准算法相比，当姿态初始值存在大失准角时，FIMA算法的航向角对准速度更快，水平姿态的对准效果更稳定.

图4 与EKF算法的姿态误差对比图

图5是FIMA算法与EKF算法关于陀螺仪零偏估计的对比图，EKF算法的仿真条件与姿态误差对比的仿真条件相同.图5中，黑色虚线是仿真轨迹中加入的陀螺仪常值漂移，大小为1 °/h，图中的零偏估计效果显示，FIMA算法对x轴和y轴的陀螺仪零偏估计精度高于EKF算法，而两种算法对z轴陀螺仪零偏的估计都不太准确，这一现象与仿真轨迹运动方式的设计过于单一有关.图5的仿真效果说明，FIMA算法可以实现对陀螺仪零偏的估计.

图5 与EKF算法的陀螺仪零偏估计对比

在本文的算法中，由于量测方程均有积分项构成，所以量测噪声难以确定，因此本文中FIMA算法引入自适应滤波，对量测噪声进行实时估计.图6为FIMA算法在加入自适应算法之前与正常的FIMA算法的仿真对比.对比结果表明，加入自适应的FIMA算法姿态精度更高，估计结果也更稳定.

图6 与非自适应FIMA算法的姿态误差对比

5 跑车实验验证

5.1 跑车实验设备

本文中算法的计算机仿真说明了算法的有效性，为了进一步验证算法的可行性，本文使用实际的实验数据进行算法验证.图7为本次跑车实验所用设备.图中，序号1是UPS电源，为整套实验设备供电；序号2是稳压直流电源，为SINS系统供电；序号3是MEMS-IMU，其型号为ADIS16488；序号4为自制的高精度光纤惯导系统，可提供高精度的姿态参考；序号5为GPS天线，GPS接收机使用的是Trimble公司的BD982.实验中采集MEMS-IMU的陀螺仪和加速度计的数据进行算法验证，其中陀螺仪的偏置稳定度为5.1 °/h，加速度计的偏置稳定度为70 ug.

图7 跑车实验设备

5.2 结果及分析

本次跑车实验在哈尔滨工程大学校内进行，汽车的运动轨迹如图8中红线所示.汽车在180 s之前静止不动，180 s之后开始运动，运动过程中的姿态变化如图9所示.

图8 汽车运动轨迹

图9 SINS系统姿态变化

为了验证算法对陀螺仪误差引起的姿态误差的补偿效果，本文先对文献[3]中的OBA算法和本文中的FIMA算法进行跑车实验比较，两种算法均是在MATLAB软件中实现的.在FIMA算法中，状态变量和相应的误差方差矩阵初始值分别设置为X0=06×1和P0=diag([(0.1)11×3(10°/h)11×3])2，其中11×3为元素全为1的一行三列矢量矩阵，算法中的系统噪声和量测噪声根据所用传感器分别设置为Qk=Ts×diag([(5.1°/h)11×3(0)11×3])2×2和R0=diag([(10×π/180)11×3])2，Ts为IMU的采样频率，因为算法中用的是双子样进行的解算，所以离散化得到的Qk需要乘2，量测噪声Rk只需要对初始值R0进行设置，之后则根据自适应算法进行实时估计.

图10是本次跑车实验中FIMA算法与OBA算法解算的姿态误差的对比，汽车是在180 s之后开始运动，两种算法均是在193 s即汽车运动13 s后开始运行.由图中姿态误差可以看出，对于低精度的SINS系统，OBA算法虽然能够快速的确定出姿态角，但是姿态角并没有随着时间的延长而趋于稳定，且航向角误差会越来越大；本文中的FIMA则克服了OBA算法的这一缺点，能够保证算法在低精度的SINS系统中依然能够稳定的收敛.

图10 与OBA算法实测姿态误差对比

图11为FIMA算法姿态误差放大图，由图中航向角误差可以知道，在没有任何初始姿态信息的条件下，算法用15 s的时间便可将航向角误差收敛到3°以内，俯仰角误差和横滚角误差收敛到0.4°以内.在FIMA对准算法运行190 s以后，航向角误差可收敛到1°以内.图12为FIMA算法对陀螺仪零偏的估计结果，估计结果表明，FIMA算法能够实现对陀螺仪零偏的估计.为证明零偏估计的有效性，将FIMA算法估计的陀螺仪零偏写入纯惯导算法中进行补偿，补偿后的效果如图13中所示.经过陀螺仪误差补偿后的纯惯导算法的航向角误差明显好于补偿前的效果，陀螺仪误差的补偿对水平姿态误差影响不大，由此可以看出，FIMA算法对陀螺仪零偏的估计是有效的.

图11 FIMA算法姿态误差

图12 FIMA算法陀螺仪零偏估计

图13 纯惯导算法姿态误差对比

为了体现本文中算法在任意失准角条件下快速对准的优越性，本文对FIMA算法与文献[1]中EKF对准算法用跑车实测数据进行比较.在EKF对准算法运行前，设置航向角误差70°、俯仰角误差和横滚角误差20°的大失准角误差.两种算法的姿态误差对比如图14所示，由于大失准角的原因，EKF算法的航向误差收敛速度明显比FIMA算法慢，EKF算法的水平姿态误差在对准的前期也不如FIMA算法的稳定.

图14 与EKF算法实测姿态误差对比

图15 与其他动基座对准算法的实测姿态误差对比

图16 陀螺仪零偏估计对比

分别为本文中的FIMA算法与文献[14-15]中动基座对准算法的姿态误差和陀螺仪零偏估计的对比图.由姿态误差图可以看出，本文中的FIMA算法的姿态对准精度优于文献[14-15]中的算法.在陀螺仪零偏估计对比图中，FIMA算法对陀螺仪零偏的估计更加稳定和接近真实值，所以FIMA算法的水平姿态误差相对于其他算法更加稳定.