文章快速检索  
  高级检索
基于Unscented卡尔曼滤波算法在海底地形辅助导航中的应用
潘晓宏1, 赵龙1, 2
1. 北京航空航天大学 数字导航中心, 北京 100191
2. 北京航空航天大学 飞行器控制一体化技术重点实验室, 北京 100191    
摘要:为了解决SITAN海底地形辅助导航算法将非线性过程线性化,容易产生不稳定甚至滤波发散的问题,提出了一种基于Unscented卡尔曼滤波的海底地形辅助导航算法,该算法无需对非线性系统的状态方程和观测方程进行线性化,直接采用UT变换(unscented transform, UT)近似非线性函数的概率密度分布,并求取变换后的点集的均值和方差。利用海底地形SEAMAPnewHill100为真实海底地形数据对算法进行了测试。实验结果表明,该算法较传统算法有较强的精确性和鲁棒性。
关键词地形辅助导航     数字unscented卡尔曼滤波化设计     UT变换     鲁棒性    
Application of an unscented Kalman filter algorithm in the seabed terrain aided navigation
PAN Xiaohong1, ZHAO Long1,2     
1. Digital Navigation Center, Beihang University, Beijing 100191, China
2. Science and Technology on Aircraft Control Laboratory, Beihang University, Beijing 100191, China
Abstract:In order to solve the problem that the linearization of the nonlinear process of the SITAN seabed terrain aided navigation algorithm is prone to instability and even the filtering divergence, a seabed terrain aided navigation algorithm based on unscented Kalman filter is proposed. This algorithm does not need nonlinear state and observation equations to be linearized, directly using the unscented transform (UT) to approximate probability density distribution of nonlinear functions, and obtains the mean and variance of the transformed point set. It uses the seabed terrain SEAMAPnewHill100 as real terrain data to test this algorithm. The experimental results show that the algorithm is more precise and robust than the traditional algorithms.
Key words: terrain aided navigation     unscented Kalman filter     unscented transform     robust    

水下精确导航是潜艇长期安全航行和武器系统实施精确打击的基本保障[1]。惯性导航系统由于能自主、连续、实时、准确地向潜艇提供导航信息,已成为潜艇综合导航系统的必要设备。然而,由于惯性导航系统的导航误差随时间而累积,必须定期用外部信息对其进行校准。目前多采用天文导航信息、无线电导航信息和卫星导航信息来校准惯导。为接收外部校准信息,潜艇需浮出或接近水面,从而牺牲了隐蔽性,使潜艇的安全受到威胁。另外,无线电导航和卫星导航在战时极易受电磁干扰而失去作用。海底地形匹配是为解决惯导系统水下校准问题而提出的一种新技术。该技术利用数字海底地形图和水下地形特征探测仪作为辅助手段来修正惯导系统误差,从而构成一种新型导航系统——海底地形匹配辅助导航系统。该系统是一种完全自主式导航系统,潜艇无需浮出或接近水面,即可在水下完成对惯导系统校正,从而满足潜艇对导航系统的要求。

1 海底地形辅助导航原理

海底地形匹配技术与飞机中使用的地形匹配技术相类似,是将潜艇上传感器测到的地形特征与艇载数字海图中给出潜艇位置附近的地形特征相匹配,从而获得潜艇精确位置。由于水深数据能直接反映海底地形变化,因此潜艇上的传感器采用能够测量水深数据的测深测潜仪,地形图则采用数字水深图。海底地形辅助导航系统由惯导系统、测深测潜仪、数字海图及数据处理计算机等几部分组成,如图 1所示。

图 1 海底地形辅助导航系统原理

海底地形辅助导航系统将惯导系统提供的导航信息、测深测潜仪实测的水深数据信息送给数据处理计算机[1, 2, 3, 4, 5],数据处理计算机根据惯导系统的位置信息从数-字 海图中提取相关的水深数据,然后采用一定的匹配算法将实测的水深数据与从海图上读出的水深数据进行匹配,得到最佳匹配点。利用该匹配点位置信息对惯导系统进行校正,便可提高惯导定位精度。

2 基于扩展卡尔曼滤波的SITAN算法的缺点

SITAN使用扩展卡尔曼滤波(extended Kalman filter ,EKF)将非线性过程线性化,故对于观测量变化较小的情况,可以通过局部线性化来较好地完成定位估计。

图 2 平滑处理及线性化

图 2所示,当观测量起伏波动较剧烈时,EKF算法在进行局部线性化过程中,线性化直线不能很好拟合曲线,导致引入了较大误差,这样就会降低定位精度[6]。在实际应用中有时需要对一片比较大的区域进行线性化,这就需要对这块区域进行平滑处理,而平滑处理最大的缺点就是会丢失很多有用的信息,这样更有可能会导致EKF不稳定。

3 基于非线性滤波的地形匹配算法数学模型

由于潜艇正下方的地形深度只与潜艇所在的水平面上的地理位置信息有关,因此在建立数学模型时,系统状态取潜艇的位置信息。在东北天坐标系下,系统状态取为东向位置和北向位置,则其状态方程为

式中:x表示东向位置,y表示北向位置,vx表示东向速度,vy表示北向速度,Qk-1~N(0,Qk-1)为系统噪声。系统观测方程为
式中:h(Xk)是地形深度值关于位置(x,y)的非线性函数;Rk~N(0,Rk)为量测误差,其包括测深测潜仪噪声以及数字地图制作噪声。

4 UT变换

UT(unscented transform)是UKF(unscented Kalman Filter)的基础,UT的思想是用固定数量的参数去近似一个高斯分布,这比近似任意的非线性函数或变换更容易。其实现原理为:在原状态分布中按某一规则取一些点,使这些点的均值和方差近似等于原状态分布的数学期望和方差;将这些点带入非线性函数中,相应得到非线性函数值点集,并求取变换后点集的均值和方差。

假设一个非线性变换y=F(x),状态向量xn维随机变量,并已知其均值x和方差矩阵Px,则可通过UT得到2n+1个西格马(SIGMA)点χi和相应的权值Wi来计算y的统计特性,UT的原理图如图 3所示。

图 3 UT原理图

实际上UT就是将状态变量x的取值范围X近似为高斯分布(假设均值x为高斯分布的数学期望,方差矩阵Px为diag[σ12,…,σn2]),然后通过在这个分布下的采样产生有限个西格马点χi(i=1,2,…,2n+1)来代替全体xX。将有限个西格马点χi经过非线性变换yi=F(χi)得到变换点yi(i=1,2,…,2n+1),并假设全体变换点yi所构成的值域y仍然近似服从于高斯分布,通过对变换点yi取加权均值和求加权方差,从而近似得到真实值域y=F(X)分布的数学期望和方差。

UT的具体公式为

其均值和方差的相关权值计算公式为
式中:λ=α2(n+κ)-n是一个比例因子。α决定x周围的西格马点的分布状态,调节α使高阶项的影响达到最小,通常选择0≤α≤1。β用来合并随机变量分布的先验知识,对于高斯分布,其最优值为2[8]

对于高斯分布的情况,当状态变量为单变量时,选择κ=2;当状态变量为多变量时,选择κ=3-n是矩阵的第i列(当P=ATA时,取的第i行;当P=AAT时,取的第i列)。

5 UKF算法的实现过程

传统的SITAN算法分为2个阶段:搜索阶段和跟踪阶段。经实验验证,基于UKF的地形匹配算法可以不需要进行搜索便能有很好的匹配定位性能[6]。UKF算法在地形匹配系统上实现的具体流程如下[7]

1)预测:计算预测状态均值Xk|k-1和预测方差矩阵Pk|k-1表示SIGMA粒子矩阵:

2)更新:计算量测平均值Zk|k-1,量测方差矩阵Sk以及状态量和量测量的互相关方差矩阵Ck

3)计算滤波增益矩阵Kk和更新后的状态均值Xk以及方差矩阵Pk

Xk即为k时刻更新的状态估计值,其中

6 实验仿真与结果分析 6.1 单变量非静态增长模型

单变量非静态增长模型(univariate nonstationary growth model,UNGM)由于具有强非线性和双模特性,使用传统线性滤波器进行匹配将会很困难。鉴于此,实验将通过该模型来比较单纯地使用UKF和EKF进行匹配时的效果,据此评价UKF和EKF的优劣。

UNGM的状态方程和量测方程为[9, 10]

式中:μn~N(0,δμ2),υn~N(0,δυ2)。在本实验中,δμ2=1、δυ2=1、x0=0.1、α=0.5、β=25、γ=8、N=500,余弦函数项模拟了时变噪声产生的影响。

为了定量比较2种滤波器的匹配性能,定义均方误差为

式中:xii时刻的真实状态值;为估计状态值。

2种滤波器对前100次采样数据进行仿真实验的滤波结果如图 4所示;前100次采样数据匹配误差与3δ范围的关系如图 5所示。

图 4中可以发现UKF的匹配性能要优于EKF,例如在采样点30~40之间UKF方法与源信号基本匹配,而EKF方法却相差甚远。此外,图 5图 4对应的匹配误差,从图中可以看出UKF匹配误差基本在3δ范围之内;而 EKF匹配误差有很多在3δ范围之外。因此,UKF滤波结果比EKF滤波结果更可靠,它们对应的均方误差分别为64.729 1和93.842 9。

图 4 前100次采样数据滤波结果

图 5 匹配误差与3δ范围
6.2 海底地形匹配仿真

海底地形SEAMAPnewHill100为真实海底地形数据,地图大小为1 800 m×1 200 m,网格间距为100 m,航行器真实速度v=20 m/s,真实航向角为55 °,高度表噪声为5 m,初始东向、北向定位误差200 m,采样周期10 s,潜艇初始网格位置(40,40),速度误差为0.2 m/s。

为了便于比较,对同一航迹使用了2种匹配算法,这样就可以清楚地看到基于UKF的地形匹配算法相比基于EKF算法的优势,如图 6所示。从图中可以看出与传统基于扩展Kalman滤波的SITAN方法相比,基于UKF的地形匹配方法不需要线性化,能够有效克服地形测量的非线性给系统带来的不利影响,系统稳定性较好。此外,可以发现即使没有经过搜索阶段,基于UKF的地形匹配算法亦能够很快的收敛,而且匹配性能丝毫不亚于SITAN算法,甚至比之更好。

图 6 基于UKF的地形匹配算法与SITAN算法比较
7 结束语

本文通过一种单变量非静态增长模型单纯的将EKF算法和UKF算法的匹配结果进行了比较,结果表明在强非线性情况下,UKF算法的匹配精度明显优于EKF算法。随后通过选用了一块海底地形图作了仿真实验,实验结果同样表明基于UKF算法的地形辅助导航算法不论从收敛速度还是精度上都明显优于基于EKF的地形辅助导航算法。这就说明在处理非线性比较强的问题时,UKF算法更胜于EKF算法。而在实际应用中很多情况下地形变化比较复杂,存在很强的非线性性。所以本文对现实科研工作具有很强的指导意义。由于篇幅原因,本文只是选择了一条航线对2种算法进行了比较,以后的研究者可以在不同地形图、不同航线和一些误差,例如速度误差、初始位置误差或噪声不同的情况下对这2种算法做进一步的比较。

参考文献
[1] 刘光军, 陈晶. 海底地形辅助导航系统仿真技术研究[J]. 计算机仿真, 2000, 17(2): 21-24 .
[2] 郑彤, 蔡龙飞, 王志刚, 等. 地形匹配辅助导航中匹配区域的选择[J] .中国惯性技术学报, 2009, 17(2): 192-196.
[3] 刘准, 佀文芳, 陈哲. 海底地形匹配技术研究[J] . 系统仿真学报, 2004, 16(4): 700-703.
[4] 王英钧. 地形辅助导航综述[J] . 航空电子技术, 1998(1): 24-29.
[5] 周亦军, 郑彤. 基于卡尔曼滤波的地形辅助导航[J] .舰船电子工程, 2012, 32(4): 54-56.
[6] METZGER J, WISOTZKY K, WENDEL J, et al. Sigma-point filter for terrain referenced navigation[C]// AIAA Guidance, Navigation, and Control Conference and Exhibit. San Francisco, USA, 2005: 15-18.
[7] 陆守雷. 海底地形匹配算法研究[D]. 北京: 北京航空航天大学, 2011: 30-31.
[8] HAYKIN S. Kalman filtering and neural networks[M]. New York: John Wiley & Sons, 2001: 221-280.
[9] 李鹏飞, 黄建军, 喻建平, 等. 三种非线性滤波器对UNGM模型跟踪性能的比较[C]// 第八届全国信号与信息处理联合学术会议. 厦门, 中国, 2009: 90-93.
[10] 蔚国强, 杨建业, 张和新. 常用非线性滤波方法比较研究[J]. 电光与控制, 2009, 16(12): 48-69.

文章信息

潘晓宏, 赵龙
PAN Xiaohong, ZHAO Long
基于Unscented卡尔曼滤波算法在海底地形辅助导航中的应用
Application of an unscented Kalman filter algorithm

in the seabed terrain aided navigation
应用科技, 2015, (01): 49-52,56
Applied Science and Technology, 2015, (01): 49-52,56.
DOI: 10.3969/j.issn.1009-671X.201404012

文章历史

收稿日期:2014-04-18
网络出版日期:2015-01-12

相关文章

工作空间