一种改进的粒子滤波算法在SINS初始对准中的应用

徐晓苏，刘心雨

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 东南大学 仪器科学与工程学院，南京 210096）

一种改进的粒子滤波算法在SINS初始对准中的应用

徐晓苏1,2，刘心雨1,2

（1. 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 东南大学 仪器科学与工程学院，南京 210096）

在实际工程环境中，针对捷联惯导系统（SINS）大失准角初始对准中噪声统计特性未知的问题，设计了一种基于H∞滤波算法的鲁棒无迹粒子滤波算法（RUPF）。通过将无迹卡尔曼滤波算法（UKF）和鲁棒环节引入到粒子滤波(PF)的重要性密度函数中，得到了RUPF算法，提高了算法的鲁棒性。通过半物理实验，将RUPF算法与无迹粒子滤波算法（UPF）在SINS静基座大失准角对准中的性能进行了比较，在不同实验条件下，航向失准角精度至少提高了40%，对准精度优于0.05°，对准时间减少了约 50 s。实验结果表明，RUPF算法可以以较高的精度和较快的速度完成大失准角初始对准，且对准精度和对准速度均优于UPF算法。

鲁棒无迹粒子滤波；捷联惯性导航系统；初始对准；大失准角

在捷联惯性导航（SINS）中，初始对准是一项关键技术，其精度直接影响到惯性导航的精度。当初始失准角较小时，SINS 的误差模型可以简化成线性模型，卡尔曼( Kalman) 滤波器是有效的滤波工具; 当初始失准角较大时，需建立 SINS非线性误差模型，需利用非线性滤波器才能准确估计出失准角。

目前使用较为广泛的非线性滤波器有三大类：扩展卡尔曼滤波（EKF）[1]、无迹卡尔曼滤波（UKF）[2]和粒子滤波（PF）[3]。EKF算法是将非线性函数进行泰勒级数展开并保留线性项以获取线性模型，对于弱非线性系统可以取得较好的滤波效果，但对于强非线性系统滤波效果较差，EKF算法会不可避免地引入截断误差且雅可比矩阵计算较为复杂。UKF是利用UT变换得到一组 sigma采样点，经过非线性函数映射，得到随机变量的均值与方差，这种算法避免了线性化误差，具有较高的估计精度，但是随着系统维数的增大，UKF算法估计精度急剧下降，甚至可能会发散。PF算法是一种基于随机采样的滤波方法，它是一个完全的非线性器，该方法直接根据概率密度计算条件均值，理论上不受模型线性和高斯假设的限制，可用于非线性非高斯的随机系统。

PF算法的核心是合理选择重要性密度函数，重要性密度函数与真实的密度越接近，滤波效果就越好，反之则越差，甚至有可能发散。文献[4]和文献[5]通过将粒子滤波与EKF或UKF组合起来的方法，改善粒子滤波器性能。在这些方法中，重要性密度函数由EKF或UKF来确定，这样既可以解决粒子退化的问题，又能使粒子更新时获得量测量的最新验后信息，有利于粒子移向似然比高的区域[6]。

然而以上滤波方法都需要知道噪声的一些统计信息。但惯性系统的固有特性以及系统初始对准时的实际工作环境具有随机不确定性，因此其噪声统计特性的数学描述具有明显的不确定性，那么在噪声统计信息未知情况下，滤波精度极有可能出现急剧下降甚至发散。针对模型的非线性以及未知模型噪声统计特性的问题，本文将鲁棒性较强的H∞滤波算法与无迹粒子滤波算法（UPF）相结合得到鲁棒无迹粒子滤波算法（RUPF），用来提高初始对准的估计精度与鲁棒性。

1 简化UPF算法

由UKF产生PF的重要性密度函数称为无迹粒子滤波器(UPF)，由UKF产生的重要性密度函数与真实状态概率密度函数的支集重叠部分更大，估计精度较高。在这个算法中，每个量测量时刻采用UKF对粒子进行迭代，并利用这些量测值对粒子进行重采样。这就好比同时运用N个卡尔曼滤波器（每一个粒子对应一个滤波器），在每次量测后再进行一步重采样。

设系统方程和量测方程分别为

式中：状态方程为非线性方程，量测方程为线性方程；Xk是系统状态向量； f(·)是非线性函数；Hk为量测阵；Vk为量测噪声序列，Wk为系统激励噪声序列，其中Wk和 Vk为互不相关的高斯分布白噪声，均值为零，方差阵分别为Qk和Rk。

文献[7]说明了简化UKF算法，使用简化UKF算法产生PF的重要性密度函数，由此可以推出简化UPF算法。该算法步骤如下：

步骤 1：初值确定（k= 0）。根据先验概率密度p(X0)采样生成粒子初始值X，i= 1,2,…,N 。

步骤2：简化UKF滤波（k=1,2…）。根据简化UKF算法对每个粒子进行时间更新和量测更新，计算简化UKF滤波后的系统状态估计 以及协方差Pki。

步骤3：重要性采样（k=1,2…）。依据简化UKF滤波结果构造建议分布函数：

步骤4：二次采样。采用SIR法或残差二次采样法对原始粒子集进行二次采样，重新计算权重系数Wkj=，从而得到新的支撑粒子集：

步骤5：根据二次采样粒子得到滤波值。

2 H∞滤波与RUPF算法

2.1 H∞滤波算法

H∞滤波算法是从H∞线性控制理论的基础上发展起来的，它不需要知道环境噪声的统计特性等先验信息。其本质就是建立一个从干扰输入到误差输出的H∞范数最小的滤波器，用于解决滤波系统和外界干扰存在不确定性的问题，因此该算法对系统模型误差和外界干扰具有很强的鲁棒性[8-9]。考虑如下随机线性离散时间系统：

Xk是系统状态向量；Φk,k-1为状态转移矩阵；Hk为量测阵； Vk为量测噪声序列；Wk为系统激励噪声序列； Lk是自定义矩阵（假设它满秩）。根据文献[10]，H∞滤波算法状态方程如下：

步骤1：初值确定：

步骤2：状态估计：

步骤3：估计状态线性组合：

步骤4：滤波增益：

步骤5：计算R,ek：

步骤6：计算Riccati方程：

为了将UPF滤波算法应用于H∞滤波器，首先对线性H∞滤波方法中实现状态估计协方差阵递推的Riccati方程进行转换，状态协方差阵Pk的H∞鲁棒更新形式可以写成[11]：

下标k|k-1表示由k-1时刻求得k时刻的一步预测。

矩阵这样变换后，H∞滤波便与卡尔曼滤波具有相同的形式，不同之处是在卡尔曼滤波算法基础上引入了γ来调节系统的鲁棒性，只有当γ满足如下条件时，非线性的H∞滤波算法才能够正常工作。

γ是调节算法鲁棒性与稳定性的“调节因子”。如果γ过大，则算法的鲁棒性将会降低；如果γ取的过小，在滤波过程中，有可能无法满足上式中的限制条件，造成滤波器无法正常工作。

对于非线性系统模型，利用UPF算法求解H∞滤波器中随机变量经非线性变换之后的均值和方差，就可得到RUPF非线性滤波算法。

2.2 RUPF算法

设系统方程和量测方程如式(1)所示，不同的是Wk和Vk分别为统计特性未知的系统噪声和观测噪声。假设Wk和Vk满足：

其中：δij为δ函数；Qk为系统噪声的方差阵，Rk为量测噪声的方差阵，实际上这两者统计特性未知，这只是估计值，使用H∞滤波算法可以提高算法的鲁棒性，使估计值接近真实值，以适应不同的环境噪声统计特性。

RUPF滤波算法流程如下：

步骤1：初始化

假设初始状态的概率密度函数p(X0) 已知，方差阵为P0。基于p(X0)生成粒子初值 χ，并选取ω=p(χ0(i))，i=1,2,…,N。选择N时需综合考虑计算量与估计精度。为了简化计算，假设每个粒子都服从正太分布N(,P0i)，其中，0(i)=χ0(i)，=P0。对于i=1,2,…,N ，k=1,2,3,…执行。

步骤2：对于采样时刻 k=1,2,3,…进行RUKF滤波计算：

1）计算σ样本点：

2）时间更新

正态分布β=2.

3）量测更新

步骤3：选取重要性函数

步骤4：计算如下权重系数：

步骤6：根据二次采样粒子计算滤波值：

RUPF算法通过调整γ值来调整Pk，以牺牲一定的精度为代价来换取滤波算法的鲁棒性。参数γ控制状态估计在最不利条件下的估计误差，约束水平γ越小，则系统的鲁棒性越强。

3 SINS大失准角非线性误差模型

本文将RUPF算法应用于捷联惯导系统中。本节将依据参考文献[12]建立以 SINS误差方程为基础的组合导航系统非线性误差方程和线性量测方程。

选取“东北天”地理坐标系为导航坐标系n系；选取载体“右前上”坐标系为载体坐标系b系。n系先后经过3次欧拉角转动至b系，三个欧拉角记为航向角，纵摇角，横摇角；n系与b系之间的姿态矩阵记为 C；真实姿态角记为；真实速度记为；真实地理坐标系为P=[L λ H]T，其中，L是纬度，λ是经度，H是高度；SINS解算出的姿态角记为，速度记为，地理坐标系为；SINS解算出的数学平台记为n′系，n′系与 b系之间的姿态矩阵记为 C；记姿态角误差为；速度误差为，位置误差为。则非线性误差模型如下：

cφi和sφi分别代表cosφi和sinφi，i=n,u,e。

非线性误差方程建立过程如下：

以采样周期T作为滤波周期，可以使用四阶龙格-库塔积分方法，以T为步长将其离散化，记离散后状态滤波方程为

线性量测方程建立过程如下：

对SINS的水平速度输出作如下分解：

式中：VE和VN为载体的理想速度。若载体有线运动时，该速度可由GPS等设备提供。在静基座下载体无线运动，因此

同理对SINS的纬度输出和经度输出做如下分解：

式中，L和λ为载体的理想纬度和经度，该位置信息可由 GPS等设备提供。在静基座下，载体无位置移动且载体所在的地理位置精确已知，即L和λ已知。所以量测方程为

式中，V为噪声阵，量测矩阵为

同样以T作为滤波周期，并以T作为步长进行简单离散化，得离散化后的量测方程为

综上，由状态方程和量测方程组成如下非线性滤波方程为

4 半物理实验结果与分析

SINS解算模块采集到惯性测量单元（IMU）模块输出的陀螺输出值和加速度计输出值进行捷联解算，得到姿态角、姿态矩阵、速度、位置等信息；静基座下载体真实速度为0，且真实位置信息已知。将SINS输出的信息输入到RUPF滤波器中，进行信息的滤波处理，系统方案如图1所示。

为了进一步验证RUPF算法的有效性，利用实验室的光纤陀螺捷联惯性系统设备（FOSN）在三轴高精度转台上进行实际环境下的工程验证。三轴转台所在实验室的地理位置为北纬32.057 305°N，东经118.786 389 °E。分别采用UPF算法和RUPF算法进行SINS静基座大失准角初始对准的半物理实验，观察实际情况下RUPF算法的效果。实验时，将 FOSN固联在三轴转台上，标定出FOSN与转台之间的安装误差角，采集FOSN捷联惯性仪表敏感的角速度信息和加速度信息，利用所设计的算法完成导航运算。有以下四种情况：

图1 系统方案图Fig.1 System schematic diagram

当真实的初始姿态角为θ=0°、γ=0°、Ψ=0°时，初始姿态角为θ=˜ 5°、γ=˜ 5°、Ψ=˜ 20°和θ=˜ 15°、γ=˜ 15°、Ψ=˜ 30°。

当真实的初始姿态角为θ=0°、γ=0°、Ψ=90°时，初始姿态角为θ=˜ 5°、γ=˜ 5°、Ψ=˜ 115°和θ=˜ 10°、γ=˜ 10°、Ψ=˜ 130°。

利用UPF、RUPF算法进行SINS静基座大失准角初始对准，对准时间为600 s。两种算法在上述两种大失准角情况下的失准角估计误差统计表如表1和表2所示，其中均值与标准差是用在对准结束后100 s内，即600 s至700 s时间段内数据计算出的。四种情况下失准角估计误差曲线分别如图2(a)、2(b) 和图3(a)、3(b)所示。图 2和表 1真实的初始姿态角为θ=0°、γ=0°、Ψ=0°；图 3和表 2真实的初始姿态角为θ=0°、γ=0°、Ψ=90°。

由图2可知，在静基座大失准角情况下，当真实的初始姿态角为θ=0°、γ=0°、Ψ=0°，设置初始姿态角为θ=˜5°、γ=˜5°、Ψ=˜ 20°时，这两种方法水平失准角误差曲线较为接近，对准精度高，时间短，水平对准精度在 0.012°以内，水平失准角大约在 200 s后基本收敛。但在航向失准角方面RUPF算法精度明显优于UPF算法，RUPF算法误差在0.027°而 UPF算法误差在0.055°，精度提高了约50%。RUPF在对准时间约为350 s时航向失准角基本收敛，而UPF在400 s时收敛；当设置初始失准角为θ=˜15°、γ=˜15°、Ψ=˜ 30°时，这两种方法水平失准角误差曲线也较为接近，但航向失准角RUPF算法精度明显优于UPF算法，

RUPF算法误差在0.042°而 UPF算法误差在0.083°，精度提高了约50%。RUPF在对准时间约为400 s时航向失准角基本收敛，而UPF在450 s时收敛。

表1 失准角估计误差统计Tab.1 Statistics on misalignment estimation errors of UPF and RUPF

表2 失准角估计误差统计Tab.2 Statistics on misalignment estimation errors of UPF and RUPF

图2(a) 失准角估计误差Fig.2(a) Estimation errors of misalignment angle

图3(a) 失准角估计误差Fig.3(a) Estimation errors of misalignment angle

图2(b) 失准角估计误差Fig.2(b) Estimation errors of misalignment angle

图3(b) 失准角估计误差Fig.3(b) Estimation errors of misalignment angle

与上述结论一样，由图3可知，当真实的初始姿态角为θ=0°、γ=0°、Ψ=90°，设置初始姿态角为θ=˜5°、γ=˜5°、Ψ=˜ 115°时，这两种方法水平失准角误差曲线较为接近，但在航向失准角方面RUPF算法精度明显优于UPF算法，RUPF算法误差在0.0328°，而 UPF算法误差在0.0571°，精度提高了约42.5%。RUPF在对准时间约为400 s时，航向失准角基本收敛，而UPF在450 s时收敛；当设置初始姿态角为θ=˜5°、γ=˜5°、Ψ=˜ 115°时，这两种方法水平失准角误差曲线较为接近，但在航向失准角方面RUPF算法精度明显优于UPF算法，RUPF算法误差在0.0499°，而 UPF算法误差在0.0909°，精度提高了约40%。RUPF在对准时间约为450 s时航向失准角基本收敛，而UPF在500 s时收敛。当失准角增大后，两种算法的对准收敛速度都在变慢，精度也在下降，但RUPF算法下降较小。

通过以上分析可以发现，在实际工程环境中，当失准角较大的情况下，RUPF算法可以以较高的精度和较快的速度完成初始对准，且对准精度特别是航向角对准精度高于UPF算法，对准速度也更快。

5 结 论

本文采用鲁棒无迹粒子滤波(RUPF)算法对SINS静基座大失准角初始对准进行了研究。鲁棒环节的引入使得RUPF算法具有很强的鲁棒性。在半物理实验中将RUPF算法与UPF算法进行了对比，实验结果表明，在实际工程环境中，当系统噪声阵和观测噪声阵无法准确获知并且失准角较大时，RUPF算法的初始对准精度较高，对准速度较快，并且该算法对准误差，特别是航向角对准误差，远小于UPF算法。因此该算法具有很强的工程应用价值。

（Reference）：

[1] Sebesta K D, Boizot N. A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system [J]. IEEE Transactions on Industrial Electronics, 2014, 61(1): 495-503.

[2] Li W, Wang J, Lu L, et al. A novel scheme for DVL-aided SINS in-motion alignment using UKF techniques[J]. Sensors, 2013, 13(1): 1046-1063.

[3] Chen Z, Qu Y, Zhang T, et al. Hybrid adaptive particle swarm optimized particle filter for integrated navigation system[J]. Computer Modeling in Engineering & Sciences, 2015, 106(6): 379-393.

[4] Chen X, Shen C, Zhao Y. Study on GPS/INS system using novel filtering methods for vessel attitude determination [J]. Mathematical Problems in Engineering, 2013(1): 289-325.

[5] Jwo D J, Yang C F, Chuang C H, et al. A novel design for the ultra-tightly coupled GPS/INS navigation system[J]. Journal of Navigation, 2012, 65(4): 717-747.

[6] 秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安: 西北工业大学出版社, 2011. Qin Yong-yuan, Zhang Hong-yue,Wang Shu-hua. Kalman filter and the principle of integrated navigation[M]. Xi’an, China: Northwestern Polytechnical University Press, 2011.

[7] 严恭敏, 严卫生, 徐德民. 简化UKF滤波在SINS大失准角初始对准中的应用[J]. 中国惯性技术学报, 2008, 16(3): 253-264. Yan Gong-min, Yan Wei-sheng, Xu De-min. Application of simplified UKF in SINS initial alignment for large misalignment angles[J]. Journal of Chinese Inertial Technology, 2008, 16(3): 253-264.

[8] Wan-xin S. Application of H∞ filtering algorithm in SINS/GPS integrated navigation system[C]//2014 2nd International Conference on Information Technology and Electronic Commerce. IEEE, 2014: 72-76.

[9] Yu F, Lv C, Dong Q. A novel robust H∞ filter based on Krein space theory in the SINS/CNS attitude reference system[J]. Sensors, 2016, 16(3): 396.

[10] Liu X, Xu X, Wang L, et al. H∞filter for flexure deformation and lever arm effect compensation in M/S INS integration[J]. International Journal of Naval Architecture and Ocean Engineering, 2014, 6(3): 626-637.

[11] Einicke G A, White L B. Robust extended Kalman filtering[J]. IEEE Transactions on Signal Processing, 1999, 47(9): 2596-2599.

[12] 孙进, 徐晓苏, 刘义亭, 等. 基于自适应无迹粒子滤波的SINS大方位失准角初始对准[J]. 中国惯性技术学报, 2016, 24(2): 154-159. Sun Jin, Xu Xiao-su, Liu Yi-ting, et al. Initial alignment of large azimuth misalignment in SINS based on adaptive unscented particle filter[J]. Journal of Northwestern Polytechnical University, 2016, 24(2): 154-159.

Improved particle filter algorithm in SINS initial alignment

XU Xiao-su1,2, LIU Xin-yu1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China)

In real engineering environments, the noise statistical characteristics are unknown in the initial alignment of SINS with large misalignment angle. To solve this problem, a RUPF algorithm is designed based on H∞filtering algorithm. By combining UKF algorithm and robust link into importance density function in PF, the RUPF algorithm is obtained to improve the robustness of this algorithm. By means of emi-physical experiment, the filter performance of RUPF and UPF in SINS initial alignment on a static base is compared with that of large misalignment angles under various experimental conditions, which show that the accuracy of heading misalignment is increased by at least 40%, the alignment accuracy is better than 0.05°, and the alignment time is reduced about 50 s. These results show that the RUPF can realize the initial alignment of SINS with large misalignment angles, whose alignment accuracy and alignment speed are higher than those of UPF.

robust unscented particle filter; SINS; initial alignment; large misalignment angle

U666.1

：A

2016-03-30；

：2016-04-12

国家自然科学基金项目（51175082，61473085）资助

徐晓苏（1961—），男，博士生导师，从事测控技术与导航定位领域的研究。E-mail: xxs@seu.edu.cn

1005-6734(2016)03-0299-07

10.13695/j.cnki.12-1222/o3.2016.03.005