基于欧拉角的捷联惯性组合导航滤波算法比较∗

田佳玉 胡柏青 李开龙 赵仁杰

（海军工程大学 武汉 430033）

1 引言

捷联惯性导航系统（Strapdown Inertial Navigation System，SINS）基于在价格体积以及算法等方面的优势，目前已成为研究的热门领域。惯导系统中惯性测量元件的器件误差积累等问题，会导致导航精度的下降，而组合导航正可以就这一问题进行改善。以捷联惯导为核心，其他导航设备辅助的组合模式，被称为捷联惯性基组合导航，GPS（Global Positioning System，全球定位系统）［1］和 DVL（Doppler Velocity Log，多普勒计程仪）［2］常作为辅助导航设备。组合导航的基础是现代滤波技术，根据惯导系统模型线性或非线性，可将滤波技术分为线性滤波和非线性滤波。线性滤波中最经典的当属卡尔曼滤波（Kalman Filtering，KF）［3］，在非线性滤波中，1995年S.J.Julier和J.K.Uhlmann提出的无迹卡尔曼滤波算法（Unscented Kalman Filter，UKF）估计精度较高且计算量一定［4～5］一直是研究的热点。

除了考虑惯导模型，在捷联惯性基组合导航滤波中，姿态表达方法也是需要重视的。采用不同的姿态表达方法所构造的组合导航滤波算法是不同的，常见的姿态表达方法有三维矢量的Euler角、罗德里格斯参数族以及四维矢量的四元数［6～8］。其中Euler角法仅需要求解三个微分方程，数量较少，但存在一对奇异点［9］在特定点姿角突变，无法得到准确值。在20世纪70年代末期，中国飞行试验研究院黄雪樵［8］提出一种双欧拉法，利用正反两组欧拉方程结合使用可以解决奇异性问题。目前国内外学者对于组合导航滤波算法的研究取得了诸多成果，Hamiltn在进行附属的虚数部研究中，提出了四元数概念［11］，在四元数体系下，乘性拓展卡尔曼滤波是基于四元数与拓展卡尔曼滤波的结合［12～13］，2003年，融合四元数与UKF，Markley、Crassidis提出采用“分层滤波”方式进行滤波估计［14］。

但目前对于Euler角体系下的线性滤波（Euler-KF）、非线性UKF滤波（Euler-UKF）、以及双欧拉UKF滤波（DoEuler-UKF）三种组合滤波算法在实际捷联惯性基组合导航系统中的精度、收敛速度和差异性还未进行研究分析。本文正是基于此，开展相关理论研究，通过仿真实验展开探究三种组合导航滤波方法在应用中的优缺点，为组合导航滤波的选取与使用提供有益参考。

2 捷联惯导基本模型

捷联惯导基本方程包括姿态、速度和位置微分方程，分别为

3 双欧拉角姿态解算法

欧拉角虽然使用简单易懂且独立方程数最少，但是一旦俯仰角达到±90°时方程组就会出现奇异点，进而无法确定横滚角与航向角的大小。于是可以考虑采用正反两组欧拉方程进行交替运算，避开各自奇异点。

动坐标系相对参考坐标系方位，由动坐标系依次绕3个不同轴转动的3个转角来确定，如果三个轴的顺序是y-z-x，则令对应的航向角、俯仰角和横滚角为正欧拉角，分别记为 φ、θ、γ，如图1（a）所示，角速度为。如果变换顺序是y-x-z，则航向角、横滚角和俯仰角为反欧拉角，分别记为φr、γr、θr如图1（b）所示。

图1 正、反欧拉坐标系旋转

两坐标矩阵的转换可分解为三个单一旋转矩阵的连乘，令导航坐标系为n，载体坐标系为b，则相应的正欧拉姿态矩阵可表示为

Cx表示关于x的单位旋转矩阵。

角速度与欧拉角的关系可表示为

由上述可知：θ=±90°为方程组的奇异点，θ=0°或在π附近时，精度会提升，此范围被称为正欧拉方程的精华区。

可知，反欧拉方程的奇异点和精华区与正欧拉方程相反，奇异点在0°或π，精华区在±90°附近，正欧拉方程与反欧拉方程的精华区与奇异区呈现出的是一个倒挂关系。在应用的过程中，一般是以±45°或±135°为界划分精华区和奇异区，如图2。

图2 正、反欧拉角微分方程的精华区与奇异区

4 卡尔曼滤波

xk、zk分别为状态向量和量测向量，Φk|k-1为状态转移矩阵，Γk为噪声矩阵和Hk为量测矩阵，wk、vk分别为系统噪声和量测噪声且

δkj为狄拉克函数，利用量测量获得状态最优估计值xˆk的KF滤波过程由状态一步预测、一步预测均方差、滤波增益、状态最优估计、状态估计均方差阵组成，见式（10）。

5 无迹卡尔曼滤波

UKF是以UT变换为基础，选取相应的Sigma采样点来近似系统状态的先验统计特性，再直接通过非线性方程演化系统状态的后验分布特性。当系统的过程噪声和量测噪声为加性噪声且量测方程是线性的时候能够得到简化的UKF算法，简化的UKF滤波算法如下：

6 基于双欧拉角的无迹卡尔曼滤波

由于欧拉角存在奇异性，本文采用双欧拉角法结合UKF进行滤波，基本的滤波流程如图3所示。

图3 DoEuler-UKF滤波流程

7 实验及分析

7.1 组合导航仿真实验

通过仿真实验对以上三种方法的估计效果进行比较，采用位置作为量测量SINS∕GPS松组合方式，仿真产生的航行轨迹如图4所示，仿真时长1300s。

图4 仿真航迹

设置滤波器的初始条件：

7.2 实验结果分析

实验的主要目的是比较Euler-KF、Euler-UKF和DoEuler-UKF的滤波效果，结果分别如图5～6所示。

图5 姿态角估计误差比较

表1是位置和航向均方根误差（RMSE）统计数据，RMSE方程由式（12）表示：

表1 位置、航向估计误差均方根误差

其中δxˆ是RMSE，T是实验时长，计误差值。

通过分析图5的姿态估计效果，可以发现DoEuler-UKF的精度要明显高于Euler-UKF和Euler-KF，Euler-UKF滤波效果好于Euler-KF。图6是速度误差估计，从收敛过程可以看出，DoEuler-UKF的收敛速度明显比Euler-UKF和Euler-KF要快。通过表1可以看出，在误差精度方面，DoEuler-UKF要明显好于其他两种方法，这是由于Euler-KF方法采用的是线性误差模型，忽略了非线性部分，导致了精度下滑，而Euler-UKF采用的则是非线性误差模型，各项参数考虑更为全面，然而Euler的姿态表示方法存在一定局限性，一旦达到奇异点附近，精度便会受到较大影响，DoEuler-UKF既选用了更为准确的非线性模型，又克服了欧拉角的奇异性问题，因此达到了更为理想的滤波估计效果。

图6 速度估计误差

8 结语

Euler角法作为旋转物理意义最明显的姿态表示方法，奇异点的存在限制了它的精度与应用范围，而双欧拉法，利用正反欧拉方程的变换，巧妙避开奇异区，并对解的精华区划分，有效提高了姿态的精确性。结合不同的惯导模型，对Euler-KF，Euler-UKF与DoEuler-UKF滤波比较实验进行分析：DoEuler-UKF对比Euler-KF和Euler-UKF，航向估计精度分别提高了69.5%与58.7%，速度估计精度分别提高了85.7%与73.6%。