移动USBL测距辅助的UUV协同导航定位方法

王银涛,贾晓宝,崔荣鑫,严卫生

(1.西北工业大学航海学院,陕西西安 710072;2.水下信息与控制国家级重点实验室,陕西西安 710072;3.中国航空工业集团公司洛阳电光设备研究所,河南洛阳 471023)

1 引言

精确的导航定位能力是无人水下航行器(unmanned underwater vehicle,UUV)顺利完成各种任务的重要前提与保障.常见的水下导航方法有航位推算导航[1]、惯性导航[2]、地球物理导航[3]、声学导航[4]等,虽然都具有各自的特点,但又相应的具有一些自身难以克服的缺陷.例如:航位推算导航定位精度低而且存在误差累积;惯性导航虽然可以在短时间内获得较高的定位精度,但仍然存在误差累积的问题,不适用于长时间或远距离的水下导航;地球物理导航实时性和适用性则受环境条件限制.

长基线(long base line,LBL)、短基线(short base line,SBL)、超短基线(ultra short base line,USBL)声学定位系统是基于声学原理,声信标在收到UUV发来的信号后会在极短延迟后进行应答,根据声信号往返的时间、声信标位置和传播速度就可以计算UUV实际位置[5].其中,超短基线由于尺寸小、安装方便等特点,比较适合用于水下运动载体间的通信和定位[6].文献[7]结合捷联惯导系统(stapdown inertial navigation system,SINS)短时间内导航精度高和USBL在一定范围内定位精度高的特点,提出了一种带有卡方检验的SINS/USBL集成导航容错方法,实验结果表明该方法可以抑制USBL异常信息对导航精度的影响,并保证了组合导航系统的稳定性和高精度.Jalving在文献[8]中利用卡尔曼滤波器(Kalman filter,KF)技术把USBL和SINS的输出信息进行数据融合和滤波处理得到更加精确的导航信息.文献[9]为了提高导航精度,采用改进的EKF滤波对系统的随机误差进行补偿.在文献[10]中,Morgado等人提出了一种紧组合技术,将超短基线系统与惯性导航系统相关参数直接组合起来对UUV的导航精度进行辅助提升.上述方法的性能通常受声信标布放位置以及工作适用距离限制.为扩大信标适用距离和工作范围,针对USV和UUV协同导航的研究,严浙平等人[11]分析了USBL水声定位数据和GPS数据,证明两者可进行组合导航.多UUV协同导航定位[12]是在单体UUV导航定位和多UUV协同基础上发展而来的水下导航定位技术新方法,其原理是利用与集群中个别或部分装配高精度导航设备的UUV相对位置关系提高自身导航精度.此类方法虽然拓宽了基线布放位置和区域的限制,但参考UUV导航误差累积的问题依然存在.

本文提出一种基于无人水面船(unmanned surface vehicle,USV)辅助的UUV导航新方法,利用USV自身的移动性及布设在上面的的USBL为UUV提供长距离大范围的高精度导航定位服务.论文首先建立SINS的动态误差模型,在此基础上设计构建USBL辅助SINS的组合导航系统状态方程和量测方程,并分别利用卡尔曼滤波算法和自适应卡尔曼滤波两种算法对位置信息进行融合提升导航精度.论文对提出的算法进行了仿真和实验验证,表明了提出的改进型自适应卡尔曼滤波算法的有效性和可行性.

2 组合导航系统的模型

2.1 SINS系统误差模型

UUV状态误差矢量方程中,一般包含位置误差(包括经度误差、纬度误差和深度方向的误差)、速度误差(包括东向速度误差、北向速度误差、天向速度误差)以及3个姿态角的误差(包括俯仰角、横滚角、航向角、加速度计的常值漂移和陀螺仪的常值漂移).

因此,惯性导航系统的完整误差状态矢量表述为

式中:ϕ=[φEφNφU]为姿态误差矢量,δv=[δvEδvNδvU]为速度误差矢量,δp=[δL δλ δh]代表位置误差矢量,ε=[εxεyεz]为陀螺仪漂移误差矢量,∇=[∇x∇y∇z]为加速度计漂移误差矢量.

2.2 组合导航系统状态方程

采用USBL与SINS设计组合导航系统时,一般选用卡尔曼滤波进行组合,通常使用间接法进行估计,因此滤波器的状态量选为导航参数的误差,主要是对导航参数的误差进行估计,然后通过估计值去校正SINS的导航参数.

USBL与SINS的组合可以有不同的方式,根据选取的状态参数,可分为两类,一类是松组合,一类是紧组合.松组合主要是位置和速度信息作为组合量;紧组合主要依据设备测量的原始信息进行组合,这里主要是斜距和斜距差.由于本文所采用的USBL无法输出斜距和斜距率信息,本论文提出一种不依赖斜距和斜距率的组合导航方法,并推导出组合导航状态方程.

导航系统的状态方程主要由惯性导航系统的误差状态组成,将平台误差角方程、速度误差方程、位置误差方程和惯性仪器的误差方程综合到一起,可表示为

同理,Fb可以写为下述矩阵形式,即

为陀螺仪和加速度计的误差状态所对应的状态矩阵,由于文中将两者的误差考虑为白噪声,因此这里Fc取零矩阵,0∈R9×6.G(t)为系统噪声矩阵,这里取为单位阵.W(t)=[ωgxωgyωgzωaxωayωaz]T表示惯性器件的噪声,其中:[ωgxωgyωgz]T表示陀螺随机白噪声,[ωaxωayωaz]T代表加速度计随机白噪声.

2.3 组合导航系统量测方程

由于本文中用到的USBL无法输出斜距信息,因此这里USBL/SINS组合导航系统的选择的是基于位置的松组合方法,系统的量测值为USBL和SINS输出位置的差值.为了便于进行组合导航解算,一般选择将位置信息转化为地球直角坐标系下的位置信息,假设水下航行器UUV在地球空间直角坐标系的位置为[x y z],则地球坐标系与地球空间直角坐标系之间的转换关系为

式中:RN为地球半径,h为当地海拔高度,λ为经度,φ代表纬度,e代表偏心率.对式(3)两端求解全微分得

为了便于后续测量方程的推导,将上式改写为矩阵形式

USBL定位设备对水下UUV进行定位,并将定位结果和USV的位置通过通信手段,以一定的时间间隔同时发送给UUV,这样UUV既可以对其位置进行精确估计,同时也可以对UUV所装备的SINS设备进行校正.当USBL安装外置GNSS后,通过USBL可以解算出UUV的位置信息,包括经度、纬度和UUV的深度信息.而UUV自身安装的SINS设备同样可以解算出经纬度和UUV的水深信息,通过两个设备作差,得到量测信息.所以量测值Z可以选为下式

式中:Lu,λu,hu代表USBL设备输出的UUV的位置,分别为纬度、经度和高度;Li,λi,hi代表由捷联惯导设备输出的UUV的位置,分别为纬度、经度和高度.由于SINS和USBL输出的位置信息中均存在量测误差,所以根据式(6)可以将量测值变为如下形式:

式中:δLu,δλu,δhu分别代表USBL输出的纬度、经度和深度误差值;δLi,δλi,δhi分别代表UUV自身安装的SINS输出的纬度、经度和深度误差值;L,λ,h代表UUV的真实状态.根据式(1)选取的USBL辅助SINS导航的状态向量X,结合式(7)可以推导出基于位置的USBL辅助SINS导航的量测方程为

式中:量测矩阵H=[03×6I3×303×6],V为系统的量测噪声,这里设置为零均值的高斯白噪声.

结合式(8)和式(2)可以得到基于USBL辅助的INS导航系统的状态方程和量测方程

式中:X(t)为导航系统的状态,选为SINS的状态误差和惯性仪表误差;F(t)为状态转移矩阵;G(t)是系统的过程噪声驱动矩阵;Z(t)是导航系统的量测量,为USBL定位结果与SINS定位结果的差值;W(t),V(t)分别为系统噪声和量测噪声,其中:E{W(t)}=0,E{W(t)WT(τ)}=Q(t)δ(t-τ);E{V(t)}=0,E{V(t)VT(τ)}=R(t)δ(t-τ);Q(t)为系统的状态方差矩阵;R(t)为系统的量测方差矩阵;δkj为克罗内克函数,表达式为

3 自适应卡尔曼滤波算法设计

自适应卡尔曼滤波(adaptive Kalman filter,AKF)是指在利用测量数据进行滤波的同时,不断地由滤波本身去判断系统的动态是否有变化,对模型参数和噪声统计特性进行估计和修正,以改进滤波设计、缩小滤波的实际误差.卡尔曼滤波算法需已知系统噪声协方差和量测噪声协方差,并且系统噪声协方差W和量测噪声协方差V的精度与滤波器的滤波效果有紧密的联系.由于水下环境恶劣,实际系统中存在各种噪声影响,难以获得准确的W和V,导致滤波器估计精度下降,因此采用AKF算法对噪声协方差进行自适应估计.

本文对状态方程(9)采用AFK算法,其过程如下:

步骤1计算一步预测状态值及状态误差协方差矩阵:

一步状态预测:

一步状态误差协方差预测:

其中Γk|k-1为过程噪声矩阵.

步骤2卡尔曼滤波增益计算,状态更新和状态误差协方差更新:

滤波增益计算:

其中Hk为量测矩阵.状态更新:

式中:令εk=其代表新息或预测残差,可以用作检验卡尔曼滤波的误差.

状态误差协方差矩阵更新:

步骤3系统噪声和量测噪声的自适应调整:

系统的噪声估计公式为

系统的量测噪声估计公式为

在式(15)-(16)中:εk代表新息;sk表示加权系数,给予旧数据和新数据不同的权重,使新数据在滤波运算中发挥主要作用.加权系数一般用公式进行计算,其中0＜a＜1,这里a=0.96.

4 数值仿真及结果分析

利用MATLAB对USBL辅助SINS导航系统进行仿真,分别对传统的卡尔曼滤波和改进的自适应卡尔曼滤波算法进行仿真.仿真中,UUV实际轨迹设定:X=t,Y=100cos(0.05t),Z=-40;USBL参数设定为量测误差r≤0.01m.UUV的实际轨迹与导航轨迹如图1所示.

图1 UUV实际轨迹与导航轨迹Fig.1 UUV actual trajectory and navigation trajectory

通过图1的局部图可以看出,仅利用SINS进行导航,随着时间的增加会产生较大的误差,而通过USBL的辅助,利用滤波算法进行位置的融合,可以提高UUV的导航精度.

为了便于对比,分别绘制传统卡尔曼滤波和改进的自适应卡尔曼滤波得到的位置估计曲线,如图2和图3所示.由图可知,通过AKF算法可以得到更高的导航精度.

图2 KF位置估计曲线Fig.2 KF position estimation curve

图3 AKF位置估计曲线Fig.3 AKF position estimation curve

USBL/SINS组成的组合导航系统解算的姿态角,速度及位置误差曲线如图4-6所示.

图4 姿态误差曲线Fig.4 Attitude error curve

在USBL测距辅助作用下,UUV俯仰和横滚角度误差稳定在0.1°,最大航向误差在KF下有1.5°,而在AKF下只有0.4°,同时在北向、东向和天向位置误差均稳定在0.2m以内.

通过对误差曲线的分析可知:若仅利用惯性导航系统进行导航解算时,随着位置误差的逐渐累积,曲线会发散;利用USBL/SINS组成的组合导航系统的定位信息进行融合后,系统的导航参数得到较大提高,并且利用KF和AKF均能够有效的估计出各个误差角,克服纯惯导造成的速度和位置随时间发散的问题.同KF估计方法相比,AKF方法估计精度更高.

图5 USBL/SINS组合导航的速度误差曲线Fig.5 Velocity error curve of USBL/SINS integrated navigation

图6 USBL/SINS组合导航的位置误差曲线Fig.6 Position error curve of USBL/SINS integrated navigation

5 实验结果及分析

为验证本文所提出的组合导航方法的有效性,本文使用某型双体无人船(图7)和BlueROV2(图8)在西北工业大学启翔湖进行了USBL/SINS组合导航实验,其中:无人船用于搭载USBL,ROV用于搭载OEM应答器.无人船平台通过iNet300与地面站进行通信,通过USBL与ROV进行通信.由于湖面水域以及水深的限制,实验在80m×20m的水域范围内进行,湖上实验场地如图9所示.实验过程中,BlueROV2在水下以直线轨迹航行.

图7 黄色双体无人船Fig.7 Yellow catamaran USV

图8 蓝色遥控水下航行器Fig.8 BlueRov2

图9 湖面实验场地Fig.9 Experimental site on the lake

无人船和ROV的航行轨迹如图10所示.通过USV辅助UUV实现了较高精度的导航,分析分别利用KF方法和AKF方法导航的效果.组合导航实验结果如图11所示.

图10 USV静止、UUV航行实验结果Fig.10 USV static and UUV sailing test results

图11 UUV松组合导航实验结果Fig.11 UUV pine integrated navigation results

为了比较两种滤波方法的组合效果,分别绘制KF滤波和AKF滤波实现的松组合导航系统的轨迹跟踪曲线,分别如图12和图13所示.对比图12和图13可知,利用AKF算法估计的效果更佳平滑,而且可以避免KF造成的发散问题.

图12 KF轨迹跟踪曲线Fig.12 KF trajectory tracking curve

图13 AKF轨迹跟踪曲线Fig.13 AKF trajectory tracking curve

位置误差曲线和速度误差曲线分别如图14和图15所示.根据图14可知,采用KF和AKF均可以实现对组合导航系统误差的抑制,但是利用改进的AKF算法,当存在较大的误差时,同样可以实现对UUV导航系统输出的位置信息进行校正.由图15可知,采用改进的AKF算法较KF估计效果更好,可以实现对UUV导航系统输出的速度信息进行校正,避免了KF在较大噪声时的发散问题.

图14 KF和AKF位置误差的USBL/SINS组合导航位置误差曲线对比图Fig.14 USBL/SINS combined navigation position error curve comparison of position error of KF and AKF

图15 KF和AKF速度误差的USBL/SINS组合导航曲线对比图Fig.15 Comparison of speed error of KF and AKF(USBL and SINS integrated navigation)

姿态误差曲线如图16所示.根据图16可知,采用KF算法和AKF算法均可以抑制误差发散,但是改进的AKF算法相比KF算法,具有更好的抑制效果.

图16 KF和AKF航向角误差曲线对比图Fig.16 Comparison of heading angle error of KF and AKF

6 结束语

本文设计提出一种USV辅助UUV导航定位方法,通过利用USV携带的USBL获得UUV的定位信息,与UUV自身惯导信息融合,得到一种UUV组合导航模型,并基于自适应卡尔曼滤波对UUV运动状态进行估计.论文分别基于仿真和现场实验对提出的方法进行了验证,表明了算法的有效性和可行性.论文结果对UUV的组合导航具有一定的理论指导意义和实际应用价值.