SS-IUKF算法及其在成像声纳组合导航中的应用*

李厚全，李伟刚，孙荣光，刘海光，丁　浩

（1.海军潜艇学院，山东　青岛　266042；2.海军航空工程学院，山东　烟台　264001）

SS-IUKF算法及其在成像声纳组合导航中的应用*

李厚全1，2，李伟刚1，孙荣光1，刘海光1，丁浩1

（1.海军潜艇学院，山东青岛266042；2.海军航空工程学院，山东烟台264001）

摘要：利用成像声纳对海底进行探测时需要高精度的导航信息。针对导航高维滤波问题，文中在球面单形sigma点变换的基础上，将其与迭代UKF相结合，提出了球面单形迭代无迹卡尔曼滤波（SS-IUKF）算法，并应用于捷联惯性导航系统（SINS）/多普勒流速剖面仪（ADCP）组合导航系统。仿真结果表明，SS-IUKF算法相比UKF、SR-UKF、EKF算法提高了滤波精度和计算速度，具有一定的工程应用价值。

关键词：无迹卡尔曼滤波，组合导航，非线性滤波，水下航行器

0　引言

对于非线性系统来说，通过改进测量更新方法可以达到提高非线性近似精度的目的。易于工程实现的方法是使用迭代测量更新的方法，在EKF上已经得到了很多成功应用［1-4］。但EKF只能精确到一阶近似，甚至有时雅克比矩阵无法求解等问题，虽然粒子滤波、无迹卡尔曼滤波（UKF）算法精度有很大提高，但计算量较大，尤其是对于高维导航模型滤波问题来说实时性难以满足要求。针对以上存在的问题，本文将球面单形sigma点变换和迭代UKF相结合，使用状态的估计值代替预测值重新进行UT变换，得到滤波参数并再次利用观测值进一步改善状态估计。球面单形sigma点变换只需要n+2个点［5-6］，相比传统UT变换大大减小了计算量，同时迭代算法的引入提高了状态估计的精度。

1　导航系统方程

在借鉴以往研究成果的基础上，文中导航系统误差模型采用四元数法建立捷联惯性导航系统SINS/多普勒测速仪DVL误差模型［7-8］，建立系统状态方程和观测方程如下：

状态方程中状态变量为：

将SINS系统速度误差与DVL测速误差之差作为系统量测值如下：

2　SS-IUKF算法

对于式（3）和式（4）确定的非线性系统，经过改进后的球面单形迭代UKF（SS-IUKF）的具体算法如下：

状态变量和方差初始化：

对于k-1测量时刻计算sigma点：

（a）选取权值W0，且满足0≤W0≤1。

（c）初始化sigma点向量序列：

建立SS-IUKF的时间更新方程如下：

SS-IUKF的量测更新方程如下：

对于，t=0，…，M，

其中，式（8）中SSUT表示对参数进行计算球面单形sigma点；M表示迭代次数。则SS-IUKF的滤波估计输出为：

以上是SS-IUKF算法的实现。SS-IUKF算法，仅需n+2点的SSUT，相比传统的2n+1点UT方法，减少了近一半的运算量。在测量更新阶段，SS-IUKF相比UKF算法增加了M次迭代运算［9］（M一般取值较小），迭代计算所产生的计算量相比sigma点计算节省的计算量来说可以忽略不计。综上，组合导航系统采用SS-IUKF算法滤波比UKF算法节省近一半的计算量。

3　算法仿真与分析

对于SINS/DVL导航系统，水平面内误差不断增大的内在特性是由于传感器误差信息的积累（包括SINS随机游走误差）引起的。初始仿真参数中，SINS陀螺常值漂移0.1°/h，加速度计常值漂移±500 μg；ADCP测速误差水平速度误差0.2%，垂直测速误差0.6%。初始纬度60°，经度120°。100次蒙特卡洛仿真计算的结果如下页图1～图4所示，分别采用EKF、UKF、SR-UKF以及SS-IUKF 4种算法。

从仿真结果可以看出，EKF算法的结果与采用线性模型的卡尔曼滤波算法精度相当。UKF及其改进算法的精度均比EKF估计精度有所提高，SS-IUKF的精度相比UKF以及SR-UKF的精度也略有提高。因此，仿真算法精度按从高至低的排序依 次 是SS-IUKF＞SR-UKF＞UKF＞EKF。SS-IUKF、SR-UKF、UKF以及EKF虽然具有相同的计算复杂度O（n3），但计算效率却相差很大，SS-IUKF的计算量大约为UKF的50%左右。经仿真计算，4种算法的计算运行时间量依次为UKF＞SR-UKF＞EKF＞SS-IUKF。因此，文中改进提出的SS-IUKF算法不仅可以提高滤波精度，同时减小了计算量。

图1　位置误差

图2　姿态误差角

图3　陀螺漂移误差

4　结论

针对水下航行器SINS/DVL组合导航系统中采用四元数法建立的非线性导航模型，文中提出了SS-IUKF非线性滤波算法并应用于上述模型。仿真结果有效地证明SS-IUKF用于组合导航系统的非线性误差模型，相比UKF、平方根UKF（SR-UKF）两种算法不仅提高了估计精度，还大大降低了计算量。对于组合导航系统这种高维滤波问题具有一定的应用价值。

图4　多普勒误差

参考文献：

［1］张丽，赵春霞.一种基于迭代EKF的FastSLAM算法［J］.山东大学学报：工学版，2012，42（4）：41-47.

［2］石波，卢秀山，陈允芳.应用EKF平滑算法提高GPS/INS定位定姿精度［J］.测绘科学技术学报，2012，29（5）：334-338.

［3］BELLAIRE R L，KAMEN E W，ZABIN S M.A new nonlinear iterated filter with applications to target tracking［C］//Proc of the SPIE，Atlantic City，1995：240-251.

［4］KERR T H.Streamlining measurement iteration for EKF target tracking［J］.IEEE Trans Aerospace and Electronic Systems，1991，27（2）：408-421.

［5］JULIER S J.The spherical simplex unscented transformation ［C］//Proc Amer Control Conf，2003，3：2430-2435.

［6］JULIER S J，UHLMANN J K.Reduced sigma point filter for the propagation of means and covariances through nonlinear transformations［C］//Proc of American Control Conf.Jefferson City，2002：887-892.

［7］李厚全，李恒，唐劲松，等.Unscented粒子滤波算法在合成孔径声纳组合导航中的应用研究［J］.海军工程大学学报，2010，22（3）：26-31：

［8］曹娟娟，房建成，盛蔚，等.低成本多传感器组合导航系统在小型无人机自主飞行中的研究和应用［J］.航空学报，2009，30（10）：1923-1929.

［9］KERR T H.Streamlining measurement iteration for EKF target tracking［J］.IEEE Trans Aerospace and Electronic Systems，1991，27（2）：408-421.

［10］姬晓琴，高晓颖.低轨卫星紧组合导航UKF方法［J］.哈尔滨工业大学学报，2012，44（7）：135-138.

中图分类号：U675.7

文献标识码：A

文章编号：1002-0640（2016）04-0154-03

收稿日期：2015-03-17修回日期：2015-05-07

*基金项目：博士后基金资助（2014M552658）

作者简介：李厚全（1981-），男，山东费县人，博士后，讲师。研究方向：水声工程，武器系统仿真。

Research on SS-IUKF Algorithm and Inertial Navigation System Underwater used in Imaging Sonar

LI Hou-quan1，2，LI Wei-gang1，SUN Rong-guang1，LIU Hai-guang1，DING Hao1
（1.Navy Submarine Academy，Qingdao 266042，China；2.Naval Aeronautical Engineering Institute，Yantai 264001，China）

Abstract：Inertial navigation system with high precision is needed when seabed is to be imaged by UUV.This paper proposes a new algorithm named the spherical simplex square root unscented Kalman filter（SS-IUKF）base on the UKF arithmetic and sigma transformation which is applied to the combinational navigation system based on SINS and ADCP.The result of simulation indicates that the new algorithm can improve the navigation precision and enhance the computation efficiency.

Key words：unscented kalman filter（SS-IUKF），combinational navigation,nonlinear filter, underwater vehicle