张亚文,莫明岗,马小艳,江 薇

(北京自动化控制设备研究所, 北京 100074)

一种基于集中滤波的SINS/DVL/USBL水下组合导航算法

张亚文,莫明岗,马小艳,江 薇

(北京自动化控制设备研究所, 北京 100074)

针对水下运载体的长时间高精度导航问题,提出一种基于集中滤波的捷联惯性导航系统(SINS)/声学多普勒测速仪(DVL)/超短基线(USBL)水下组合导航方法,建立了杆臂在线估计的SINS/DVL速度观测模型和SINS/USBL相对量测信息观测模型,并利用参考信息判定结果实时更新量测方程及维数,既能保证精度,还能一定程度上减少运算量。理论仿真和湖面试验表明,本算法能够实现长时间高精度的水下导航定位,具有可行性。

组合导航;集中Kalman滤波;SINS;DVL;USBL

0 引言

相对陆空导航,水下导航因具有环境复杂、信息源少、隐蔽性要求高等特点而格外困难[1]。目前国际常用的水下导航定位技术手段有惯性导航技术、DVL航位推算、水声定位技术以及组合导航技术等[2]。单一的导航系统由于自身存在的不足,已无法满足现代导航系统高精度高可靠性的要求,例如,捷联惯导系统能够为水下运载体提供完备导航信息,但其误差随时间积累;多普勒测速仪虽能够测得高精度的速度信息,但位置误差仍随航程积累;超短基线(USBL)水声定位系统虽能够测量应答器相对于基阵的相对位置信息[3],但需要结合惯导的航向姿态信息才能完成导航[4-5]。

组合导航技术利用两种或两种以上的非相似导航系统对同一导航信息做测量并解算以形成量测量,从这些量测量中计算出各导航系统的误差并校正[6],能够提高水下航行器的提导航精度。常用的组合导航方式有SINS/DVL组合导航和SINS/USBL组合导航。然而SINS/DVL组合导航是一种速度组合的模式,能够获得水下航行器实时高精度的速度信息,但位置误差仍随时间增长;SINS/USBL组合导航能够有效抑制导航系统位置误差的发散,但又受水声换能器作用距离的限制,常用于区域范围内的高精度定位;SINS/DVL/USBL组合导航系统能够兼具SINS/DVL组合和SINS/USBL组合导航的优点,达到互补,是实现水下航行器长航时高精度导航的最优技术手段。本文提出了一种基于集中滤波融合思路的SINS/DVL/USBL组合导航算法,建立了相应的组合导航误差模型,并优化了量测噪声算法,通过理论仿真和湖面试验证明了算法的可行性。

1 SINS/DVL/USBL组合导航模型

相对于联邦滤波分散地处理各子导航系统获得导航误差的全局次优估计,集中式Kalman滤波则利用一个Kalman滤波器来集中地处理所有导航子系统的信息,能够获得导航误差状态的全局最优估计。实现工程应用中,集中滤波存在以下主要缺点:1)状态维数高,计算负担重,不利于实时计算;2)容错性能相对较差。然而,随着DSP技术的飞速发展,嵌入式导航计算的计算速度和存储容量都得到了极大的提高,已经能够很好地解决集中滤波状态维数高和运算量大等问题;此外,Kalman滤波计算过程中,观测量是Kalman滤波器与外围的唯一桥梁,滤波器是否被污染,主要取决于量测信息是否正确输入,即在保证严格的参考信息检测的情况下,集中式Kalman滤波器的精度优势还是非常显著的。

本文采取集中滤波的信息融合方法实现SINS/DVL/USBL组合导航,详细滤波框架如图 1所示。以SINS/DVL速度量测量和SINS/USBL相对位置信息量测量共同作为集中Kalman滤波量测量。

图1 SINS/DVL/USBL组合导航滤波原理Fig.1 The scheme of SINS/DVL/USBL integrated navigation theory

1.1 SINS/DVL/USBL组合导航状态模型

1.1.1 SINS子系统状态误差模型

选取惯导载体坐标系b系为前上右坐标系,导航坐标系n系为北天东坐标系。选取传统15维惯导系统误差状态量作为SINS子系统误差状态量[7]为

XSINS= [δVNδVUδVEφNφUφEδφδh

(1)

本文使用捷联式指北方位系统通用误差模型状态方程为

(2)

其中，惯性导航系统系统矩阵FSINS、系统噪声WSINS和系统噪声分配矩阵GSINS可参考捷联式指北方位系统通用的15维系统矩阵。

1.1.2 DVL子系统状态误差模型

通过试验数据分析发现,DVL测速误差以及DVL到SINS的安装误差等对组合导航精度影响较大。DVL的测速误差主要为受声速引起的刻度系数误差,直接导致参考速度信息精度下降,引起组合导航精度下降;DVL到SINS的安装误差主要包括:DVL到SINS的安装角误差会导致惯性/DVL组合导航信息观测点的空间不一致,引起组合导航精度下降;DVL到SINS的杆臂误差在水下运载体转弯或横摇纵摇时,同样会引起惯导速度与DVL速度测量点空间不一致问题,引起组合导航精度下降。

因此,本文选取DVL刻度系数误差、DVL到SINS的航向安装误差角误差和杆臂作为DVL部分的状态误差量,即DVL子系统误差状态量为

(3)

其中， βd为DVL到SINS的航向安装误差角误差,δKd为DVL刻度系数误差,R=[Rx Ry Rz]T为DVL到惯导的杆臂误差。

建立DVL子系统状态方程为

(4)

其中, FDVL=[05×5]为DVL子系统系统矩阵,GDVL和WDVL为相关噪声矩阵项。

1.1.3USBL子系统状态误差模型

通过实验数据处理发现,USBL自身测角精度以及USBL基阵坐标系到SINS载体坐标系的安装角标定精度对定位精度影响很大。从数值角度粗略分析:如果USBL测量的方位角β或者航向安装误差角 θy有1°误差,则会引入约1.7%R的水平位置误差,而且随着斜距R变大误差会进一步增大。USBL的测角误差在工作范围内都为小角度,且安装角误差通过事先标定后能够收敛到一个小角度范围内,故本文将对USBL自身测角误差和USBL到SINS的安装角误差进行建模,并将其设计为误差状态量。受不同水文气象对声波传输速度的影响[8],USBL的测距误差也一定程度上影响着组合导航的精度,故本文也将USBL测距刻度系数误差δKd设计为Kalman滤波组合导航的误差状态量。

因此,本文选取USBL到惯导的3个安装误差角误差、USBL自身测量误差为USBL子系统误差状态量

(5)

建立USBL子系统的滤波状态方程为

(6)

其中,FUSBL=[06×6]为 USBL子系统系统矩阵,GUSBL和WUSBL为相关噪声矩阵项。

1.1.4 SINS/DVL/USBL组合导航系统状态误差模型

综上所述,本文涉及的SINS/DVL/USBL组合导航集中Kalman滤波状态方程为

(7)

其中,集中Kalman滤波状态量为

(8)

集中Kalman滤波系统矩阵为

1.2 SINS/DVL/USBL组合导航观测模型

本文设计的SINS/DVL/USBL组合导航集中Kalman滤波器的观测模型主要由两部分组成:SINS /DVL速度观测模型、SINS/USBL相对量测信息观测模型。

1.2.1 杆臂在线估计SINS /DVL速度观测模型

(9)

(10)

1.2.2SINS/USBL相对量测信息观测模型

综合分析本研究结果可知，恩施烟区烟叶在海拔540～1 680 m范围内，感官评吸质量对海拔高度较主要化学成分及其协调性敏感，而感官评吸质量是衡量烟叶质量好坏的决定性指标，因此，在恩施州，适当将植烟区域向高海拔地区调整有利于烟叶品质的提高。

(11)

对式(11)求导可得构造的USBL水声基阵坐标系中应答器相对于惯导的高度角、方位角和斜距对应误差项为

(12)

(13)

对式(13)进行误差分析,可得SINS/USBL相对量测信息误差观测方程为

(14)

HS/U= [03×3(Hφ)3×3(HδP)3×306×305×3

(Hθ)3×3(HU)3×3]3×26

(15)

1.2.3 SINS/DVL/USBL组合导航系统观测模型

综上所述可得,本技术方案中 SINS/DVL/USBL组合导航集中Kalman滤波观测方程为

Z=HX+v

(16)

由于DVL和USBL的数据更新频率不一致,这对集中滤波的实现带来了一定的不便。针对上述情况,本算法根据DVL和USBL参考信息的有效性实时判断结果,自适应地改变观测方程的维数,以期望尽可能地挖掘DVL和USBL每个更新周期提供的参考信息,同时减少运算量。DVL数据更新频率为1Hz,而USBL数据更新频率通常为2Hz,甚至更长,但均可配置为DVL数据更新频率的整数倍,故本文采取的具体方法为:

1)当仅DVL有效时,仅利用SINS/DVL速度量测信息进行Kalman滤波,观测方程简化为

Z=(ZS/D)3×26=(HS/D)3×26X26×1+v

(17)

2)当DVL和USBL同时有效时,采用式(15)进行Kalman滤波估计。

由于集中滤波模型建立时统一考虑了两种观测模型所需的相关状态误差量,故上述两种滤波状态可直接进行切换。

2 SINS/DVL/USBL组合导航集中Kalman滤波方程

对式(8)和式(15)进行离散化处理可得SINS/DVL/USBL组合导航集中Kalman滤波模型为:

(18)

其中，Φk+1/k为第k时刻到第k+1时刻的一部状态转移矩阵,Γk为系统噪声驱动阵,Wk和Vk+1均为白噪声,且方差阵依次为Q、R。

本方法采用标准Kalman滤波基本方程,各变量的定义与标准离散Kalman滤波的变量定义保持一致:

Pk=(I-KkHk)Pk/k-1

(19)

选择合适的滤波初值(状态初始值X0,初始估计均方误差阵P0,系统噪声初始方差阵Q0以及量测噪声方差阵R0),按照式(19)进行Kalman滤波计算,待Kalman滤波器稳定后,利用Kalman滤波得到的惯导速度误差、位置误差和航向姿态误差等误差状态估计值Xk实时修正惯导速度、位置和航向姿态角,可以得到高精度的SINS/DVL/USBL组合导航结果。

3 理论数据仿真分析

惯导初始地理位置为北纬39.8°、东经116.2°、高度等于0,以4m/s(约6节)的前向速度按图2所示航迹行驶,其中应答器位置为[39.802026764,-50,116.198185395],理想初始航向姿态角均为0°。惯导采样周期为5ms,DVL测速更新周期为1s,USBL数据更新周期为2s。各传感器误差和初始误差设置如表1所示。

表1 传感器误差和导航初始误差

按照本文设计的SINS/DVL/USBL组合导航算法进行仿真,并对SINS/DVL 组合导航、SINS/USBL组合导航、联邦滤波以及集中滤波定位结果进行对比,如表2所示。仿真航迹如图2所示,定位误差曲线如图3～图5所示。

表2 1σ定位精度对比

从图3～图5位置误差曲线可以看出,本文设计的SINS/DVL/USBL组合导航集中滤波算法能够获得较高的定位精度,且精度优于其他三种情况,具有理论可行性。

图2 仿真航迹Fig.2 The navigation track of simulation

图3 北向位置误差Fig.3 North position error

图4 东向位置误差Fig.4 East position error

图5 水平定位误差Fig.5 Level position error

4 湖面实验数据处理分析

湖面试验系统主要包括:光纤捷联惯导系统(SINS)、DVL测速仪、USBL水声定位系统和DGPS系统。光纤陀螺漂移优于0.02(°)/h,加表零偏优于30μg(1σ);DVL系统采用RDI公司的WHN300kHZ的多普勒测速仪,数据更新周期为2s,测速精度为0.4%±4cm/s;USBL系统采用NavQuest公司的Track Link 1500HA水下基阵和应答器,其原始输出为高度角、方位角和斜距,其测角精度优于1°,定位精度为0.5%,数据更新周期为2s。

按照本文设计的基于集中SINS/DVL/USBL组合导航算法对湖面试验采集的一组导航原始试验数据进行处理,并以DGPS提供的高精度位置为基准信息考核组合导航算法的定位精度,同时对SINS/DVL 组合导航、SINS/USBL组合导航、联邦滤波以及集中Kalman滤波的导航结果进行对比,定位精度如表3所示,组合导航初始时刻,先用DGPS对惯导解算的位置速度进行重置,以避免原始积累的误差对组合导航精度考核产生影响[10]。

表3 湖面试验定位精度

从表3湖面试验定位精度可以看出:本文提出的基于集中滤波的SINS/DVL/USBL组合导航定位精度(1σ)优于SINS/DVL、SINS/USBL以及联邦滤波的组合导航结果。

图6～图9所示为几种不同组合导航算法的水平定位误差对比图,图中曲线对应载体的行驶轨迹,黑色箭头表示起点位置及行驶方向,色彩由红(5m误差)到蓝(0m)表示误差递减。对比图6和图9可知,第二圈开始集中Kalman滤波(图9)精度明显优于SINS/DVL组合精度(图6),即表明集中滤波由于参考信息的增加,其长时间导航精度得到进一步提高;由图7可知,SINS/USBL组合导航由于开始时滤波器未完全收敛,第一圈时位置误差较大,可达5m左右;图8中联邦滤波能利用SINS/DVL滤波器的快速收敛，一定程度上改善图6第一圈出现的定位误差较大段(深红色轨迹段);对比图7、图8和图9可知,集中滤波导航结果不仅能够更快地估计出各导航误差,且其精度优势更为明显。综上所述,本文设计的基于集中滤波的SINS/DVL/USBL水下组合导航算法具有较高的精度,即验证了湖面试验数据情况下本算法的可行性。

图6 SINS/DVL组合导航结果Fig.6 The navigation result of SINS/DVL

图7 SINS/USBL组合导航结果Fig.7 The navigation result of SINS/USBL

图8 联邦滤波导航结果Fig.8 The navigation result of SINS/DVL/USBL based on the federal-KF

图9 集中滤波导航结果Fig.9 The navigation result of SINS/DVL/USBL based on the Concentrated-KF

5 结论

本文利用集中Kalman滤波方法,提出一种基于集中滤波的SINS/DVL/USBL水下组合导航方法,建立了组合导航状态方程和量测方程,并根据当前参考信息判定结果实时更新量测方程及维数,在减少运算量的同时保证了组合导航精度。理论仿真和湖面试验表明,本算法能够实现长时间高精度的水下导航定位,具有可行性。

[1] 马伟峰, 胡震. AUV 的研究现状与发展趋势[J]. 火力与指挥控制, 2008, 33(6): 10-13.

[2] 周保军, 刘硕. 水下导航技术研究[J].现代导航, 2012, 3(1): 19-23.

[3] Link Quest Inc.TrackLink 1500 USBL Tracking System, User’s Guide Version 1500.7.2.

[4] 兰华林，孙大军，张殿伦，等. 单应答器导航深海实验[J]. 海洋工程，2007, 25(2)：110-113.

[5] 隋海琛，田春和，韩德忠，等. 水下定位系统误差分析[J].水道港口, 2010, 31(1):69-72.

[6] 秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M].西安：西北工业大学出版社，2004.

[7] 秦永元. 惯性导航[M]. 北京:科学出版社, 2006.

[8] 冯守珍, 吴永亭, 唐秋华. 超短基线声学定位原理及其应用[J]. 海岸工程, 2002, 21(4): 13-18.

[9] 张亚文, 莫明岗, 马小艳, 等. 一种基于相对测量信息的SINS/USBL组合导航算法[J]. 导航定位与授时, 2014, 3(2): 7-13.

[10] 唐康华, 江明明, 翁炬. 基于自适应滤波的水下SINS/相控阵DVL组合导航算法设计[J]. 中国惯性技术学报, 2013, 21(1): 66-70.

An Algorithm of Underwater SINS/DVL/USBL Integrated Navigation Based on Concentrated Filtering

ZHANG Ya-wen， MO Ming-gang， MA Xiao-yan， JIANG Wei

(Beijing Institute of Automatic Control Equipment, Beijing 100074, China)

For long-time and high-precision navigation of UUV, an integrated navigation algorithm of SINS,DVL and USBL which is based on the Concentrated-KF is designed, then a SINS/DVL observation model which can compensate the arm error in real time and a SINS/USBL model which observes the relative measurement information are established. Besides, on the basis of the result of reference information judgement, the realtime updates of the Filter observation equation and its dimension are achievedwithout increasing of the computation or losing the navigation precision. Finally, the results of theoretical simulation and data analysis of lake test show the advantage and the practicability of the new integrated navigation algorithm.

Integrated navigation; Concentrated-KF; SINS; DVL; USBL

10.19306/j.cnki.2095-8110.2017.01.005

2016-11-20；

2016-12-07。

海军“十三五”预先研究项目(3020603030403)

张亚文(1990-),男,硕士,助理工程师,主要从事惯性/水声组合导航技术研究。

V249

A

2095-8110(2017)01-0025-07