基于四元数姿态估计的捷联惯导初始对准算法

周晓仁，赵鹤鸣

(苏州大学电子信息学院，江苏 苏州 215006)

捷联惯导系统定位的本质是依靠积分来获取载体的姿态，速度和位置的信息，所以进行导航的前提是要有三者的初始信息。初始位置和速度可以从全球定位系统(global positioning system，GPS)获得，而初始姿态获得的途径是初始对准，它直接影响到捷联惯导系统的导航精度。

捷联惯导系统初始对准的重点在于初始姿态阵的求解，它通常可分为滤波估计和确定性算法两种方法[1]。初始姿态的确定算法早期研究主要源于解决卫星姿态的确定问题。1967年，Wahba将“根据矢量观测来确定姿态的问题”描述为带约束的最小二乘问题，首先给出了表达式，即所谓的Wahba问题[2]。后来许多学者经过对Wahba问题的研究后推导出了众多在矢量观测的基础上求解姿态的算法[3－6]。在确定性算法中，近年来文献[7－8]提出的基于最优化的对准(optimization-based alignment，OBA)算法以及速度积分公式法在中高精度捷联惯导系统动基座初始对准中得到了广泛的应用，该方法通过加入外部辅助设备如GPS或者里程计等的信息，能够构造出多组的参考矢量与观测矢量。然后采用最优估计法，可以抑制因为观测矢量异常而造成的影响并且有效地加快了初始对准的速度[9－13]。然而对于微机电(micro electro mechanical system，MEMS)低精度捷联惯导系统，其陀螺仪的零偏一般是几度甚至为几十度每小时，这就会大幅度加剧观测矢量的累计误差，从而导致对准结果不理想。

另一种求解姿态的算法为滤波估计法，最常用的技术就是扩展卡尔曼滤波(extended Kalman filter，EKF)。状态估计法采用动力学模型对姿态参数及相关误差参数进行实时估计[14]。但是扩展卡尔曼滤波容易引入线性误差，从而降低滤波精度，且部分非线性函数的Jacobian矩阵计算困难。文献[15]提出了无迹卡尔曼滤波(unscented Kalman filter，UKF)，与EKF不同，UKF用一系列近似高斯分布的采样点，通过无迹变换递推和更新状态与误差协方差，而不需要计算Jacobian矩阵，对非线性方程的精度相当于二阶泰勒展开。

滤波估计法的优势在于能够把陀螺的零偏加入状态向量中进行估计，从而能够抑制陀螺零偏对于观测矢量的影响。本文采用UKF四元数姿态估计进行初始对准，最后，进行了仿真与跑车实验验证了所提算法的正确性。

1 姿态确定初始对准基本原理

取i为对准起始时刻地球系作为初始惯性系，并且不随地球运动而改变。b为右－前－上载体坐标系；n为东－北－天导航坐标系；n0为对准起始时刻导航坐标系，与对准起始时刻的n系重合，并且相对惯性坐标系不变；b0为对准起始时刻载体坐标系，与对准起始时刻的b系重合，并且相对惯性坐标系不变。惯导系统的比力方程可以表示为:

式中，fb表示比力，gn表示地球重力加速度在导航系的投影。由方向余弦矩阵的链式法则可知:

将式(2)代入式(1)并整理可得:

将式(4)两边同时积分可得:

式中，

由于GPS采样时间与捷联惯性导航系统(strapdown inertial navigation system，SINS)采样时间不一致，此处假设GPS采样时间为Δtg，SINS采样时间为Δts，并且有Δtg＝SΔts(S∈R＋)。以GPS量测更新时刻计算离散化矢量。经过离散化可得

式中，t＝MΔtg，tk＝kΔtg，Δtg＝tk＋1－tk。具体的离散计算方式可以参考文献[8]。

式(8)是典型的Wahba问题，其对应的Wahba损失函数为

2 姿态估计初始对准基本原理

由第一章的分析可知，姿态确定算法通过求解最优初始姿态矩阵，再结合式(2)、式(3)来计算姿态矩阵。但是，由于陀螺零偏和噪声的影响造成陀螺仪输出的值与真值之间存在误差，由式(2)、式(3)可知在低精度传感器下姿态阵的准确性将会受到很大的影响。姿态估计是指建立预测模型和量测模型，然后通过滤波算法来对姿态矩阵迭代计算。在本文中，预测模型根据姿态矩阵的更新方程建立，量测模型根据速度积分公式法所建立。在本文中，将陀螺零偏加入到姿态更新方程中可以有效地减少陀螺零偏所带来的影响。

2.1 预测模型

在姿态估计中，陀螺仪的模型如下所示:

该式为姿态估计的预测模型。本文采用了二子样算法进行姿态矩阵的更新。记四元数的预测模型为:

2.2 量测模型

由文献[16]可知，方向余弦矩阵的链式法则:

将式(16)代入惯导比力方程式(1)整理可得:

式中，

经过离散化:

该式为姿态估计的量测模型，其中

记四元数的量测模型为:

式中，η为量测噪声，是均值为零的高斯白噪声。若，则

3 无迹四元数姿态估计

将UKF用于四元数姿态估计的方法称为无迹四元数估计器(unscented quaternion estimator，USQUE)。若采用四元数作为状态变量参与姿态估计会有四元数归一化的问题。Lefferts提出了成立条件:在姿态误差比较小的情况下有效，但是由于累计的舍入误差会影响进一步的运算[17]。所以如果四元数作为状态变量，那么四元数的归一化约束条件很有可能不再成立。通常，在USQUE中采用无约束条件的三元素参数在滤波过程中作为状态向量来进行运算。在所有的三元素参数中，修正罗德里格参数(modified Rodrigues parameters，MRP)的非奇异角度最大(为2π)，所以将MRP作为首选姿态参数[18]。

若有误差四元数为δq＝[δρTδq4]T，则对应的MRP为:

式中，a为一个取值范围为0到1的常数，并且f是一个比例因子。在USQUE中，设状态向量xk＝[δpkεk]。

3.1 算法初始化

设k－1时刻的状态最优估计向量为，相应的状态协方差矩阵为Pk－1，并且最优估计四元数为。

3.2 预测更新

先计算Sigma点集χk－1:

式中，n为状态向量的维数，κ为满足加权和的系数，通常κ＝3－n。向量矩阵中其中第i(i∈[1，2n＋1])个向量的误差四元数δqk－1(i)＝可以由式(26)的逆运算得到:

将姿态方向余弦矩阵以四元数的形式和陀螺零偏的Sigma点集代入式(15)的姿态更新方程(预测模型)得到预测四元数Sigma点集:

由于陀螺零偏可以视为常值，所以预测陀螺零偏Sigma点集为:

预测姿态四元数取预测四元数Sigma点集的第一组向量:

预测误差四元数Sigma点集为:

因为，由式(26)得对应的预测MRP的Sigma点集为:

3.3 量测更新

量测预测的Sigma点集为:

量测预测的均值和协方差矩阵为:

式中，Rk为量测噪声矩阵。互相关协方差矩阵为:

滤波增益为:

状态估计向量为:

状态估计协方差阵:

3.4 姿态更新

最优四元数估计为:

4 实验与分析

4.1 仿真实验

为验证本文提出的算法，本节设计了仿真实验，将USQUE法与OBA法(文献[8])以及改进无迹四元数估计器(modified unscented quaternion estimator，MUSQUE)(文献[19])进行对比，从算法的对准精度进行验证分析。

仿真实验过程中，传感器误差参数设定如表1所示。设定惯性传感器数据输出频率为200 Hz，GPS数据输出频率为1 Hz，GPS量测噪声设置:速度为0.1 m/s，位置为1 m。

表1 传感器误差特性

表2 载体运动信息

载体运动轨迹如图1所示，载体的真实姿态角如图2、图3所示。运动过程持续300 s，运动过程中载体进行转向运动。图4为载体的运动速度。

图1 载体运动轨迹图

图2 真实水平姿态角

图3 真实航向角

图4 载体运动速度

图5～图7分别表示俯仰角，横滚角和航向角的对准误差曲线。由图5和图6可知，三种方法的对准误差在300 s左右可以达到相似的对准精度，俯仰角误差小于0.05°，横滚角误差小于0.1°。图7的航向角对准误差表明，传统OBA法随着对准时间的增加会因为陀螺仪的零偏导致的累计误差使得对准误差曲线呈现波动特性。而采用USQUE法和MUSQUE法后对准误差更加稳定。200 s～300 s之间OBA法的航向角误差均值为0.983 7°，USQUE法的航向角误差均值为0.462 7°，MUSQUE法的航向角误差均值为0.566 7°。因此USQUE法的精度相较于传统OBA法更高且与MUSQUE法的精度相近。

图5 俯仰误差

图6 横滚误差

图7 航向误差

4.2 跑车实验

为验证本文提出的算法，现设计跑车实验进行验证。实验过程中采用自制的微型惯性测量单元(miniature inertial measurement unit，MIMU)，惯性传感器采样频率为200Hz。其陀螺仪和加速度计性能参数参见表3所示。

表3 自制的MIMU传感器特性

GPS的采样频率为1 Hz。在对准之前，系统工作在GPS与惯导组合导航模式下，以光纤惯性导航与GPS组合导航系统的输出姿态值作为参考值进行精度评估，光纤陀螺和加速度计的零偏稳定性分别为0.01 °/h，100μg，采样频率为200 Hz。图8～图11分别为车载实验平台，载体运动轨迹和载体参考姿态角。图12为GPS速度输出，图13～图15分别为俯仰角，横滚角和航向角对准误差曲线图。

图8 车载实验平台

图11 参考航向角

图12 GPS速度输出

图13 俯仰误差

图15 航向误差

图9 载体运动轨迹

图10 参考水平姿态角

从图13～图14中可以看出三种方法的两个水平姿态误差角在经过100 s后都在0.2°以下。图15的航向角误差曲线表明了OBA法在陀螺零偏的影响下得到的对准精度不如USQUE法和MUSUQE法。200 s～300 s之间OBA法的航向角误差均值为0.681 2°，USQUE法的航向角误差均值为0.201 4°，MUSQUE法的航向角误差均值为0.130 1°。因此跑车实验进一步验证了:在航向角误差方面，USQUE法具有与MUSQUE法相似的精度，且比OBA算法更优。

图14 横滚误差

5 结论

针对姿态确定性算法在低精度捷联惯导系统初始对准中易受陀螺零偏的影响，本文研究了一种基于UKF的四元数姿态估计的动基座初始对准算法USQUE。通过对陀螺零偏进行建模并加入状态向量进行估计，从而减小零偏的影响。通过仿真与跑车实验，验证了本文设计的算法在航向角对准精度上相较于传统的方法更有优势，并且在精度上与MUSQUE法相似。