基于径向基函数神经网络辅助容积卡尔曼滤波的多自主水下航行器协同定位方法

徐博，李盛新，金坤明，王连钊

(哈尔滨工程大学 自动化学院，黑龙江 哈尔滨 150001)

0 引言

自主水下航行器(AUV)作为海洋探测的倍增器，具有控制灵活、价格低廉等特点，是近年来海洋工程领域的重点研究内容之一。多AUV组成的协作系统不仅可以胜任单AUV能够承担的任务，例如海底环境勘探、收集水下情报等，而且效率高、可靠性好，具有重要的应用前景[1]。在水下环境中，受海况变化、海底环境信息缺乏等条件制约，一些环境探测及监测任务执行过程中，需要多AUV协作系统在部分工作区间没有或基本没有外界辅助位置信息的情况下完成，导航问题成为发展AUV所面临的重要挑战。多AUV协同定位是随着多AUV协作系统的应用发展而开发的一种新型水下定位技术。它不仅可以实现高精度水下定位，而且成本低、实现简单、可靠性好、无区域限制，受到世界各国的广泛重视[2-3]。

准确的位置信息对AUV实现自主工作，尤其对多AUV组成的协作系统是至关重要的。人们提出很多基于状态空间模型的滤波方法[4-5]，但在实际的协同导航中，由于水下环境的随机性和复杂性，多AUV协同定位系统的定位性能受到系统内部和外部等多种因素的制约[6]，例如洋流漂移、传感器误差及水下测距精度等引起的系统传感器输入误差和量测误差。针对这种情况，一般可以通过采用更高精度的传感器应用到多AUV协同定位系统中，或者建立更符合系统实际运动规律的状态空间模型，但这会增加协同定位系统的成本，增大算法复杂度。故需要确定一种有效的方法，在不增加成本和算法复杂性的前提下保证协同定位的精度。

神经网络可以实现输入输出的任意非线性映射，并且对非线性滤波具有强映射和鲁棒性，目前已经应用于很多领域。文献[7]利用反向传播(BP)神经网络建立了液压振动台正弦加速度响应的谐波辨识方法,并在液压振动台上进行了实验，验证了所提出的方案能有效地识别各次谐波的相位以及幅值，具有较高的识别精度。文献[8]基于BP神经网络辅助扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)，并对两个运动目标进行了仿真实验，结果表明使用BP神经网络优化非线性滤波可以提高滤波估计性能，但应用BP神经网络进行学习训练时，是采用负梯度下降法进行的权值调节，这是一种收敛速度较慢的权值调节方法。文献[9]将粒子滤波与多层感知器神经网络相结合，实现了很好的训练效果，但训练效果理想的前提是需要巨大的粒子数目，这将导致系统计算复杂度增大，在实际应用中难以实现。文献[10]将径向基函数(RBF)神经网络应用于洪水预报，分别建立4种RBF神经网络洪水预报模型，试验结果验证了RBF神经网络运算速度快、预报准确度高。RBF神经网络可以将输入信息映射得到输出信息，近似于人脑的功能和结构，其逼近函数能力、分类识别能力和学习速度等方面都好于BP神经网络，适用于解决水下环境复杂情况下的多AUV协同定位问题。

另外，由于容积卡尔曼滤波(CKF)通过点估计的方法，不需要通过线性处理方法将非线性系统线性化，从而提高了非线性函数的拟合精度，同时也避免了在高维系统中UKF容易出现数值不稳定现象，理论推导更加严谨。文献[11]对比了UKF和CKF方法的精度和适用范围，仿真结果表明CKF方法性能更好，尤其对于高维非线性系统，CKF的适用性和稳定性更强。

基于以上分析，考虑到系统状态方程和量测方程的非线性及滤波运算复杂度，本文提出基于RBF神经网络辅助CKF的多AUV协同定位方法。利用RBF神经网络的输出补偿CKF估计，进而提高存在系统误差情况下，多AUV协同定位系统的鲁棒性和准确性，并通过湖试数据进行了仿真验证。结果表明，本文所提的定位方法在多AUV协同定位中具有更好的定位精度和稳定性。

1 多AUV协同定位状态空间模型

假设多AUV群体中有N个领航AUV，M个跟随AUV，为了研究方便先考虑2个领航AUV对1个跟随AUV进行交替领航协同定位的情况。基于相对位置量测的领航- 跟随式多协同定位示意图如图1所示，图1中椭圆面积表示定位误差协方差。领航AUV装备全球定位系统(GPS)或基于惯性导航设备、多普勒速度计程仪(DVL)和压力传感器的高精度组合导航系统，跟随AUV装备由罗经和DVL组成的低精度航位推算系统，以及压力传感器。此外领航AUV和跟随AUV都配备了水声调制解调器，在协同定位过程中，跟随AUV通过水声设备每隔一段时间间隔与领航AUV进行相对距离量测，利用领航AUV的导航位置信息和相对距离量测进行信息融合(协方差椭圆如图1中实线椭圆面积明显减小)，而在非协同定位时刻，跟随AUV仍采用航位推算的方法。这种周期性协同定位有效降低了跟随AUV的定位误差，满足长航时、高精度要求。

图1 相对位置量测的协同定位示意图Fig.1 Cooperative localization in relative position measurement

(1)

式中：xk和yk分别为跟随AUV在tk时刻的东向和北向位置；δt为航位推算周期；vk为跟随AUV在tk时刻的前向速度；θk为跟随AUV在tk时刻的航向角；wx,k和wy,k为零均值且相互独立的Gauss白噪声。

(2)

定义Xk=[xk,yk]T为跟随AUV在tk时刻的状态量，wk=[wx,k,wy,k]T为过程噪声向量，εk=[εk]T为量测噪声向量，且有

(3)

(4)

Qk和Rk分别为假设已知的系统噪声协方差阵和量测噪声协方差阵。

基于(1)式和(2)式，建立多AUV协同定位系统的离散时间状态空间模型为

(5)

2 CKF协同定位方法

考虑到协同定位系统状态方程和量测方程的非线性以及计算的复杂度，引入CKF. CKF方法采用Spherical-Radial准则得到等权值的容积点，再利用统计数值积分原理计算非线性变换后的随机变量均值和协方差。考虑如下求积问题：

(6)

式中：I(f)是所求积分，被积函数定义在笛卡尔坐标系中；Rn是n维积分域，n为状态的维数；f(x)为非线性函数，x为状态向量。将(6)式变换为球径求容积的形式，然后运用球径求积规则进行求解。

在球面径向变换中，把笛卡尔向量x∈Rn转换成径向标量r和方向向量y. 假设x=ry，yTy=1，得xTx=r2，r∈[0，∞)，(6)式的积分可在球径坐标系中表示为

(7)

式中：Un为半径为1的球表面，即Un={y∈Rn|yTy=1}；σ(·)为球面测度。

进一步化简，将(6)式表示为

(8)

(9)

对(8)式和(9)式采用3阶容积原则，n维标准正态分布与非线性函数乘积的积分可近似为

(10)

(11)

在得到容积点后，通过这些点经过非线性系统结合滤波中的时间与量测更新对系统状态进行估计，能够较为准确地获得状态更新及状态协方差矩阵更新[12-17]。

由上述过程可以看出，CKF利用2n个等权值的容积点，计算复杂积分和所需的统计量，完成滤波周期内的时间更新和量测更新。

1)时间更新。假设状态向量Xk=[xk,yk]T在tk时刻系统后验概率状态k|k以及概率密度p(Xk|Zk)=N(k|k,Pk|k)是已知的，通过Cholesky将状态误差协方差Pk|k分解为

(12)

计算Cubature点

Xi,k|k=Sk|kξi+k|k，

(13)

定义以下变量

(14)

通过系统状态的转移矩阵函数传递Cubature点

(15)

tk+1时刻的状态预测值通过加权求和得到

(16)

计算tk+1时刻的状态误差协方差预测值

(17)

2)量测更新。通过Cholesky将状态误差协方差预测值Pk+1|k分解为

(18)

计算Cubature点

Xi,k+1|k=Sk+1|kξi+k+1|k，

(19)

通过系统的量测函数传递Cubature点

(20)

tk+1时刻的观测预测值通过加权求和得到

(21)

计算tk+1时刻的量测误差协方差预测值为

(22)

互协方差矩阵为

(23)

估计卡尔曼滤波增益为

(24)

tk+1时刻的状态估计k+1|k+1及其误差协方差矩阵Pk+1|k+1分别为

k+1|k+1=k+1|k+Kk+1(Zk+1-k+1|k)，

(25)

(26)

3 RBF神经网络辅助CKF

3.1 径向基神经网络原理

RBF神经网络和多层前向网络的结构相类似，是3层前向网络，分别为输入层、隐含层、输出层。输入层到隐含层的变换是非线性的，隐含层到输出层的变换是线性的[18-20]。其基本思想为：

1)将RBF作为隐单元的“基”，从而构成隐含层空间，并将输入向量映射到隐含层空间；

2)当确定RBF的中心点后，即可得到映射关系；

3)隐含层空间到输出层空间线性映射。

RBF神经网络具有紧凑的拓扑结构，单层的隐含层使其自身结构更加简单且更容易实现，这些优点可以让RBF神经网络更好地逼近非线性函数。RBF神经网络的结构如图2所示。

图2 RBF神经网络结构Fig.2 RBF neural network structure

图2中：X=(x1x2…xp)T为网络输入层的输入向量；(φ1φ2…φb)为神经网络隐含层的b个任一节点的激活函数，即RBF；Y=(y1y2…ys…yd)T为网络的输出向量；w为输出权矩阵，其中wjs为隐含层第j个节点与输出层第s个节点间的连接权值，j=1,2,…,b，s=1,2,…,d.

RBF网络要训练的参数有3个，即基函数的中心和方差以及隐含层与输出层之间的连接权值。RBF神经网络中常用的RBF是高斯函数，因此激活函数可表示为

(27)

式中：‖·‖ 为欧式范数；cj为高斯函数中心；σj为高斯函数的方差，即神经元的宽度。

径向基神经网络的输出为

(28)

式中：cj为网络隐含层节点的中心；wjs为隐含层到输出层的连接权值；ys为与输出样本对应网络的第s个输出节点的实际输出。

方差σj可由(28)式求解

(29)

式中：cmax为所选取中心的最大距离。

连接权值通过最小二乘法计算得到，即

(30)

3.2 RBF神经网络辅助CKF的多AUV协同定位

1)量测值Zk和预测量测值k+1|k之差(Zk-k+1|k)，也称为新息；

3) CKF增益Kk+1.

CKF状态更新估计值与准确状态之间的误差值反映了滤波方法性能的好坏，若能估计出滤波误差值补偿状态更新的滤波估计值，便能直接提高CKF方法精度。

RBF神经网络辅助CKF方法的多AUV协同定位方法的基本思路分为两个部分：

第2部分为通过神经网络估计滤波误差补偿滤波状态更新估计值。利用CKF方法对tk～te时段数据进行滤波估计，可得到状态更新中的新息(Zk-k+1|k)、预测误差(k+1|k-k+1|k+1)及滤波增益Kk+1，并将其当做训练完成后的RBF神经网络输入值代入输入层，得到滤波误差将神经网络输出的估计滤波误差与滤波估计值k+1|k+1相加，则可以得到新的更为准确的状态估计值。

RBF神经网络辅助CKF方法的原理图如图3所示。

图3 RBF神经网络辅助CKF方法的原理图Fig.3 Principle diagram of CKF algorithm aided by RBF neural network

这种基于RBF神经网络辅助CKF的方法，利用CKF估计的准确性和稳定性以及RBF神经网络的学习能力和速度，可实现更为准确的修正滤波估计值。

4 基于RBF神经网络辅助CKF的多AUV协同定位实验

4.1 实验情况概述

实验利用湖试采集的后处理数据，试验使用3艘勘探船模拟AUV航行，其中两艘充当领航AUV，另一艘充当跟随AUV. 领航AUV和跟随AUV都配备了美国Teledyne Benthos公司产ATM-885声学调制解调器，进行水下单程测距(OWTT)和广播信息，获取速度信息的DVL和提供航向的罗经也安装在AUV上，此外，跟随AUV还配备了GPS，以便提供参考位置。湖试设备现场如图4所示，所用传感器的性能参数如表1所示。

图4 湖试设备现场Fig.4 Experimental equipment used in the the lake test

传感器型号性能参数指标声学调制解调器ATM-885通信范围/m8000GPSOEMV-2RT-2单点定位精度/m1.8(均方根值)罗经H/H HZ001航向精度/(°)0.3DVLDS-99速度精度/kn0.2

湖试数据长度为1 600 s，采样周期δt=1 s，取0～1 200 s数据作为RBF神经网络0～tk时段的训练数据，取1 200～1 600 s数据作为tk～te时段的实验数据，系统的状态方程初始状态0由GPS提供。在每一时刻，跟随AUV只与两艘领航AUV中的一艘进行通信。图5显示了由GPS提供的两艘领航AUV和一艘跟随AUV的真实航行轨迹，其中细线表示AUV前1 200 s的真实航行轨迹，粗线表示1 200 s以后AUV的真实航行轨迹。从图5中可以看出，跟随AUV始终位于两艘领航AUV之间，这样能够提高系统的可观测性。

图5 领航AUV和跟随AUV的真实轨迹Fig.5 True trajectories of Leader AUV and Follower AUV

4.2 状态空间模型输入误差

在实际的多AUV协同定位中，由于建立的状态空间模型不够准确，系统的定位性能会受到各种不定性因素影响，这些影响因素能够导致滤波方法精度下降甚至快速发散。本文在非线性滤波CKF的基础上引入RBF神经网络对CKF估计值进一步修正，可有效提高CKF的估计精度及稳定性。基于RBF神经网络辅助CKF的多AUV协同定位方法在下文中简称为RBFCKF方法。

基于以上描述，利用湖试数据，在建立的状态空间模型上分别添加以下3种不同输入误差：

图6 跟随AUV航向Fig.6 Heading of Follower AUV

图7 跟随AUV东向、北向速度Fig.7 The eastward and northerly velocities of Follower AUV

3)领航AUV和跟随AUV间的量测距离存在均值不为0的高斯噪声。领航AUV与跟随AUV之间的量测距离存在误差时，会严重影响CKF估计精度，本实验在得到的领航AUV与跟随AUV间距离量测数据的基础上，设置均值为15 m、方差为(10 m)2距离量测误差，以验证在跟随AUV量测距离存在未知误差时本文所提方法的有效性。跟随AUV与领航AUV1、领航AUV2之间的量测距离和设置误差的量测距离如图8、图9所示。

图8 领航AUV1与跟随AUV之间的量测距离Fig.8 Distance between Leader AUV 1 and Follower AUV

图9 领航AUV2与跟随AUV之间的量测距离Fig.9 Distance between Leader AUV 2 and Follower AUV

4.3 协同定位方法验证

数学仿真软件MATLAB的神经网络工具箱为RBF网络提供了很多工具箱函数，本文通过newrbe函数创建一个准确的RBF网络。RBF网络的扩展速度函数spread越大，拟合出来的函数会更平滑，但过大的扩展速度会引起数值问题，本实验所取径向基扩展速度的值设为3；RBF隐含层神经元数与输入矢量数相等；隐含层神经元阈值取0.863 2.

在第1 200 s时利用GPS进行跟随AUV位置更新，作为多AUV协同定位实验滤波的初始位置。初始时刻的状态估计误差协方差矩阵为P0=diag[1 m,1 m]2；过程噪声和量测噪声协方差矩阵分别为Qk=diag[1 m，1 m]2和Rk=[10 m]2。基于4.2节中协同定位系统输入误差的设置，模拟多AUV协同定位系统状态方程和量测方程存在误差情况下的协同定位实验，验证RBFCKF方法的有效性和优越性，实验结果如图10～图12所示，其中参考CKF协同定位误差为没有添加航向漂移误差、洋流干扰、量测误差情况下对实验数据进行CKF估计的定位误差。实验过程中添加了跟随AUV速度传感器测量噪声(0.3 m/s)2的零均值高斯白噪声，航向角的测量噪声取为(1°)2的零均值高斯白噪声，故3组实验中参考CKF协同定位误差有所变化。

图10 系统输入航向存在漂移误差的实验结果Fig.10 Experimental results of drift error in the input heading

图11 系统输入速度存在洋流干扰的实验结果Fig.11 Experimental results of system input velocity with ocean currents

由图10～图12可以看出，多AUV协同定位系统存在输入误差或外部量测误差时，仅基于CKF协同定位方法的定位误差明显增大，RBFCKF协同定位方法精度接近无误差影响时CKF方法估计的定位精度，由RBFCKF方法估计的跟随AUV定位轨迹更接近跟随AUV的真实轨迹。

图12 量测信息存在噪声干扰的实验结果Fig.12 Experimental results of noise interference measurement information

为进一步验证RBFCKF协同定位方法的定位性能，选择定位误差(LE)、平均定位误差(ALE)、均方根误差(RMSE)等作为多AUV协同定位性能指标[21]，定义分别为

多AUV协同定位系统存在输入误差时，对比CKF和RBFCKF方法的协同定位性能指标如表2所示。

表2 存在误差时多AUV协同定位性能比较

由表2协同定位性能比较结果可以看出，多AUV协同定位系统状态空间模型存在以上3种输入误差时，基于RBFCKF方法的ALE和RMSE仅为CKF方法的30%，具有更好的协同定位精度；协同定位误差的方差和标准差相比基于CKF的协同定位方法均提高一倍左右，协同定位性能更加稳定；多AUV协同定位系统的最大定位误差也明显降低。故当多AUV协同定位系统状态空间模型系统输入不准确时，基于RBFCKF的多AUV协同定位方法的各定位性能，均优于无RBF神经网络辅助的CKF方法方式。

5 结论

本文针对多AUV系统协同定位性能受传感器输入误差和外部量测误差的影响，提出了一种基于RBF神经网络辅助CKF的多AUV协同定位方法，并通过湖试数据进行了仿真实验验证。得出主要结论如下：

1)介绍了多AUV协同导航的背景与研究意义，建立了多AUV协同导航方法的系统定位模型，给出了基于CKF的多AUV协同导航方法的基本方程。

2)提出基于RBFCKF的多AUV协同定位方法，给出了RBF神经网络的基本概念及其辅助CKF的具体流程。

3)对基于CKF和RBFCKF的多AUV协同定位方法通过湖试数据进行了仿真对比。实验结果表明基于RBFCKF的多AUV协同定位方法定位精度明显提高，协同定位性能更加稳定。

本文虽然通过湖试数据进行了实验验证，可以有效提高多AUV协同定位系统的协同定位性能，为进一步研究多AUV协同定位系统、提高系统协同定位性能提供了可行性思路和方法。但由于水下环境的不确定性和水声通信的复杂性，导致系统噪声模型时变以及AUV在水下的欠驱动特性等问题，仍需要深入研究与分析，并在实际操作过程中去探索。