一种基于自适应神经模糊推理系统的多AUV协同定位方法

徐 博，李盛新，王连钊，段腾辉，姚 贺

（1.哈尔滨工程大学 自动化学院，哈尔滨 150001；2.湖南航天机电设备与特种材料研究所， 长沙 410205；3.中船航海科技有限责任公司，北京 100071）

自主水下航行器（Autonomous Underwater Vehi- cles,AUV）是一种集人工智能、计算机软件、传感器等先进技术于一体的现代技术产品。随着人类在海洋探索和开发作业的不断深入，单一AUV的使用已经不能满足当前的需求。因此，研究人员将目光投入到由多个AUV组成的协作系统上。然而复杂的水下环境限制了协同系统的应用，在环境探测和监测等大多数水下任务中，需要多AUV协作系统在部分工作区间没有或基本没有外界辅助位置信息的情况下完成，导航定位问题成为发展AUV所面临的重要挑战[1-3]。陀螺仪和加速度计组成的惯性导航系统（Inertial Navigation System,INS）是目前最常用的水下导航方法。然而，其固有的导航误差随时间积累的特性又不利于长航时的高精度导航[4]。随着水声通信技术的进步，基于水声通信网络的多AUV协同定位技术得到了越来越多的关注和发展。AUV间通过水声通信实现信息共享，通过水声测距实现相互观测，进而通过信息融合技术实现导航定位误差的协同校正。在协同定位系统中，少数领航AUV配备高精度INS，领航AUV可以通过水声调制解调器辅助其他配备低精度传感器的跟随AUV，这种定位方法不仅能提高系统整体的导航性能，而且具有成本低、实现简单、无区域限制等优点，是近年来海洋工程领域的研究热点之一。

对AUV的位置进行估计是水下协同作业中一个至关重要的问题，基于状态空间模型的滤波技术是目前应用最广泛的一种方法，它可以实现统计意义上的最优估计，获得高精度的状态估计信息。然而，考虑到水下环境的不确定性和水声通信在实际应用中的复杂性，多AUV协同定位系统的定位性能受到系统内部和外部多种因素的制约[5]。此外，恶劣的水下环境也可能导致水下机器人在一段时间内无法进行通信。在这种情况下，位置估计一般可以仅通过滤波的时间更新进行一步预测得到，但是位置误差在不进行测量更新的情况下会迅速发散。相关学者提出了一些基于间歇观测信息的卡尔曼滤波方法[6-7]。然而，所提出的方法计算复杂，在通信包丢失数分钟情况下，状态估计的精度无法保证，因此有必要探讨一种合适的方法，保证系统在长时间通信包丢失情况下多AUV协同定位精度。

自适应模糊推理系统（Adaptive Neuro-Fuzzy Infe- rence System,ANFIS）是模糊推理系统（FIS）和神经网络（NN）的结合，在许多领域得到了广泛的应用[8]。文献[9]将ANFIS用于无线传感器网络的精确定位中，试验结果表明相对于对数正态阴影模型法的粗略估计，采用ANFIS优化，室内/外距离估计精度均有明显提高；文献[10]在ANFIS的基础上进行了改进，提出了一种基于人工智能的综合INS/GPS测量方法，并利用实际数据对其性能进行了评价，结果表明与扩展卡尔曼滤波相比，在没有GPS信号的情况下，仅使用低成本惯性传感器在GPS信号丢失情况下定位性能有了很大改进。ANFIS以其处理高度非线性问题的效率而闻名，它是对传统卡尔曼滤波器的一种改进，特别是对动态环境下输入数据的复杂性和不确定性的有效处理，适用于解决水下环境复杂情况下的多AUV协同定位问题。

基于以上分析，考虑到系统状态方程和量测方程的非线性及滤波运算复杂度，为了提高水下机器人在恶劣水下环境下的定位精度，本文提出了基于ANFIS辅助非线性滤波的多AUV协同定位方法。在协同定位系统正常通信时，跟随AUV利用领航AUV位置信息和相对距离信息进行信息融合估计位置，同时对ANFIS模型进行训练。当协同系统通信数据包丢失时，利用训练好的ANFIS预测跟随AUV的位置。实验结果表明，本文所提方法在多AUV协同定位中具有很好的定位精度和稳定性。

1 多AUV 协同定位系统描述

本文研究了基于领航-跟随编队形式的多AUV协同定位问题。领航AUV配备高精度惯性导航设备、深度传感器（即压力传感器）和多普勒速度计程仪（Doppler Velocity Log,DVL），可获得高精度的位置信息；跟随AUV装备低精度的惯性导航设备，可以依靠DVL、罗经和深度传感器进行粗略的航位推算。在实际应用中，利用深度传感器可以获取AUV的精确深度信息。因此，水平位置的计算不受深度信息的影响，三维定位问题可以简化为二维[11]。

定义跟随AUV在tk时刻的位置向量为Xk= [λk,Lk]T，基于声学测距的协同定位系统状态方程为：

式中，λk、Lk分别为AUV 在tk时刻的经度位置与纬度位置；Δt为采样时间；vk、ωk分别为由DVL 测得的跟随AUV 的前向速度与右舷速度；θk为罗经所测AUV 航向，这里航向角为AUV 的前向与北向的夹角，范围为(-180°,180°)，顺时针方向为正；h k为跟随AUV的深度信息，由深度传感器获得；wk=[wλ ,k,wL,k]T为过程噪声向量；RM,k和RN,k分别是子午线的曲率半径和卯酉线的曲率半径，定义为：

式中，Re=6 378 137 m，e=1/298.257。

领航AUV 与跟随AUV 都安装了水声调制解调器，领航AUV 通过水声调制解调器向跟随AUV 周期性的广播自身位置。领航AUV 和跟随AUV 之间的坐标位置关系为：

式中：rk为领航AUV 与跟随AUV 之间的相对距离；hkm为领航AUV 的深度；ak、b k、a km和bkm定义如下：

由于压力传感器可以准确地获取深度信息，因此可以将三维距离量测简化为二维距离量测，如下所示：

式中，δk为量测噪声。

基于式(1)(9)，建立多AUV 协同定位系统的离散时间状态空间模型（状态方程和量测方程）：

式中，f(X k-1,vk,ωk,θk)和分别为状态转移函数和量测函数，定义为：

2 自适应神经模糊推理系统

ANFIS 借鉴NN 的学习机制，将其应用于FIS 中，使得FIS 也能够通过收集数据进行训练[12]。在FIS 中，一些隶属函数是通过输入来给出的，输入函数相互组合或交互的方式称为规则，这些规则分为前件参数和后件参数[13]。本文所采用的ANFIS 技术中，考虑了三个输入、一个输出和一阶Takagi-Sugeno 方法。输入、输出和规则描述如下：

输入：时间(T)、航推位置(L)、速度(V)；

输出：滤波输出的位置；

规则1：

规则2：

ANFIS 结构由五层组成，如图1所示。圆圈和正方形分别代表固定节点和自适应节点。

Layer 1:该层称为模糊层，该层中的所有节点都被视为自适应节点。在此层中输出值O1,i可以表示为：

式中，T、L、V是节点的输入，Ai、Bi、C i分别为与节点T、L、V有关的模糊集合。这里Ai、Bi、C i的隶属函数被认为是广义钟形隶属度函数（gbellmf），定义如下：

式中，ai、b i、c i为可以改变gbellmf形状的参数，称为前件参数，参数数值的变化影响隶属度函数。

Layer 2:该层称为规则层，节点标记为Π。该层实现了模糊推理过程，每个节点的输出表示某一条规则的可信度。该层的输出值O2,i是通过所有输入成员函数相乘来计算：

式中，wi表示第i条规则的激励强度。

Layer 3:该层节点被标记为N。将前一层的输出做归一化处理，输出为处理后的强度值：

式中，为第三层的输出值。

Layer 4:这一层称为去模糊层，该层中的所有节点都是自适应节点。该层在归一化的激励强度和结果函数之间创建一个自适应关联函数，输出值是第三层和第一层的值的乘积：

式中，m i、pi、q i、r i是结果参数，通常称为后件参数。

Layer 5:该层中唯一的节点是固定节点，标记为Σ。计算ANFIS 的总输出：

ANFIS中的模糊隶属度函数和模糊规则是通过大量已知数据进行学习训练而获得的，而不是根据经验或直觉任意给出的，这对于尚未完全理解或具有复杂特征的系统尤为重要[12]。利用该学习过程，对FIS 的前件和后件参数进行优化调整，以准确地表达输入空间和输出空间之间的实际数学关系。在每次迭代训练中，可以减少实际输出与预期输出之间的误差。当达到预定的训练次数或错误率时停止训练。梯度下降法用于非线性前件参数的优化，而均值最小二乘法用于线性后件参数的调整[14]。

3 基于ANFIS 辅助EKF 的协同定位

考虑到系统状态方程和量测方程的非线性及滤波运算复杂度，引入扩展卡尔曼滤波。对于非线性系统，EKF 是一种行之有效的滤波方法，但是在量测信息长时间丢失的情况下，EKF 的状态估计性能会降低，故在EKF 中引入ANFIS 提出基于ANFIS-EKF 的协同定位方法，保证通信包丢失时跟随AUV 的估计位置精度。

3.1 基于EKF 的协同定位算法

由于式(1)(9)组成的系统模型为非线性系统，因此需采用非线性滤波方法进行状态估计，这里采用EKF 来实现协同定位算法。假定tk-1时刻的后验概率估计和后验概率密度函数已知，算法过程如下：

时间更新：

量测更新：

式中： (·)-1表示矩阵的逆运算； (·)T表示矩阵的转置运算；和Pk|k-1分别为预测状态向量和相应的预测误差协方差阵；Kk为卡尔曼滤波增益；和Pk|k分别为估计状态向量和相应的状态误差协方差阵；F k为f(X k,v k,wk,θk)关于的雅克比矩阵；Gk为f(X k,v k,wk,θk)关于uk=(v k,wk,θk)T的雅克比矩阵；Hk为关于的雅克比矩阵。

式中，Ak=(RN,k+hk)cosLk，Bk=RM,k+hk。

3.2 基于ANFIS-EKF 的协同定位方法

基于水声通信的协同定位系统，利用EKF 可以实时估计位置。然而，由于水下环境的不确定性和复杂性，水声通信信道具有很高的不可靠性。跟随AUV 可能无法接收领航AUV 的测量和位置信息，即通信数据包丢失，这将导致多AUV 协作系统的定位误差迅速发散。

针对上述问题，提出了一种基于ANFIS 的实时数据融合方法。当水声通信系统正常工作时建立ANFIS 模型，ANFIS 的输入包括时间、经度速度和纬度速度、航位推算位置，利用滤波估计的位置信息用作输出数据。通过对ANFIS 模型参数的训练，得到了AUV 位置随时间、速度和DR 位置的变化规律。ANFIS 利用“历史数据”实现对未来结果的预测，该方法不增加滤波的计算量。模型更新结构和数据预测结构如图2所示。

该协同定位方法根据测量信息的状态，分为模型训练过程和预测过程两部分。当领航AUV 的位置信息及与跟随AUV 间的相对距离信息可以通过水声通信正常传输到跟随AUV 时，ANFIS 模型处于更新学习过程中，此时AUV 的位置由滤波估计得到。同时，利用该阶段收集的数据对ANFIS 模型进行训练，收集的数据为：时间tk、通过航位推算获得的经纬度位置信息、EKF 估算的经/纬度位置信息测量的速度信息、罗经测量的航向信息。经度和纬度方向的速度信息分别为vλ,k和vL,k：

ANFIS 是一个多输入单输出系统，经度位置和纬度位置可分别由两个模型预测。和为模型1 的输入和输出信息，和为模型2的输入和输出信息。在此过程中，通过收集的数据对ANFIS 进行训练，得到非线性前件参数和线性后件参数的最优值，从而获得位置随时间、速度和航位推算位置的变换规律。

图2 基于ANFIS-EKF 的协同定位方法原理图 Fig.2 Schematic diagram of cooperative localization method based on ANFIS-EKF

当通信包丢失时，系统进入ANFIS 预测过程。在此过程中，ANFIS 可以根据时间、航位推算位置以及经度方向和纬度方向的速度来预测跟随AUV 的位置。当通信丢包的时间较短时，协同定位系统可以通过滤波的时间更新而不进行测量更新得到跟随AUV 的位置估计，短时间内定位误差不会有明显变化。故本文通过设置阈值的方式，当通信丢包时间小于5 s 时，将滤波的一步预测位置作为实际估计位置；当通信丢包时间大于5 s 时，系统进入预测过程，以ANFIS 预测的位置作为实际估计位置。通过以上两种过程的切换，ANFIS 可以有效地处理通信包丢失状态下的定位问题，这为协同定位系统通信包丢失的问题提供了一种有效的辅助定位方案。

4 基于ANFIS-EKF的协同定位方法半实物仿真验证

4.1 半实物仿真情况概述

本文所提出协同定位方法的有效性和稳定性通过湖试采集的后处理数据验证，湖水试验中使用了3 艘勘探船模拟AUV，其中两艘充当领航AUV，一艘充当跟随AUV。跟随AUV 利用水声调制解调器周期性地获取领航AUV 的位置及领航AUV 与跟随AUV 之间的相对距离。基于领航-跟随模式的多AUV 协同定位如图3所示，图中椭圆面积表示定位误差协方差，红色虚线椭圆表示AUV 只依据自身装备惯性导航系统进行定位的误差协方差，蓝色实线表示跟随AUV通过水声设备进行协同定位的定位误差协方差，这种周期性协同定位方法有效地减小了定位误差。

图3 基于交替量测的协同定位示意图 Fig.3 Schematic diagram of cooperative location based on alternating measurement

领航AUV 和跟随AUV 都配备了GPS 和水声调 制解调器。GPS 用来提供基准参考位置，AUV 之间通过水下声学调制解调器进行通信。此外，跟随AUV配备了DVL 和罗经，传感器性能参数如表1所示。

表1 传感器的性能参数 Tab.1 Performance parameters of sensors

领航AUV 和跟随AUV 的实际轨迹如图4所示，试验总时间为1730 s，采样周期为Δt=1 s。图4中粗线型表示协同定位系统在1150～1350 s 时水声通信包丢失。初始状态估计由GPS 提供，初始状态估 计误差协方差矩阵设为P0=diag[1 m,1 m]2，过程噪声协方差矩阵为Qk=diag[1 m,1 m]2，测量噪声协方差为Rk=diag[5 m]2。

图4 领航AUV 和跟随AUV 的真实轨迹 Fig.4 True trajectory of leader AUV and follower AUV

图5 两个领航AUV 的测量交替情况说明 Fig.5 Illustration of the measurements received from two leader AUVs

在每一采样时刻，两个领航AUV 中只有一个与跟随AUV 通信，跟随AUV 与两个领航AUV 的量测交替情况如图5所示。标志位等于1 时，跟随AUV与领航AUV-1 进行通信；标志位等于2 时，跟随AUV与领航AUV-2 进行通信，标志位等于0 时，跟随AUV不与任何领航AUV 进行通信。

由图5可以看出，在1150～1350 s 时，跟随AUV与领航AUV 之间的通信包连续丢失201 s。在接下来的实验描述中，本文将使用上述通信包丢失的数据作为量测信息进行算法验证。

4.2 协同定位算法验证

为验证本文提出方法的有效性，采用定位误差（Localization Error,LE）、平均定位误差（Averaged Localization Error,ALE）、均方根误差（Root Mean Square Error,RMSE）和最大误差（Maximum error,ME）作为多AUV 协同定位性能指标，定义如下：

式中，(xk,yk)为GPS 提供的tk时刻跟随AUV 的基准参考位置；为tk时刻协同定位估计位置；T为试验数据长度。

MATLAB 的神经网络工具箱为ANFIS 提供了封装的训练函数，为了得到ANFIS 模型结构，我们使用网格划分（Grid Partition,GP）方法来训练ANFIS，从而确定每个输入变量的隶属度函数个数和规则个数，仿真计算机配置为Intel Core i5-8500@3.00Hz。考虑的ANFIS 有三个输入，分别为时间、航位推算的位置和跟随AUV 的速度，每个输入变量都有3 个钟形隶属度函数，故规则的数量为27 条。ANFIS 的主要任务是通信包丢失时实时预测跟随AUV 的位置信息。

图6 跟随AUV 经度估计位置的对比 Fig.6 Comparison on estimated values of longitude position of follower AUV

图7 跟随AUV 纬度估计位置的对比 Fig.7 Comparison on estimated values of latitude position of follower AUV

图8 跟随AUV 轨迹对比图 Fig.8 Comparison on follower AUV trajectories obtained by different methods

图9 跟随AUV 定位误差对比图 Fig.9 Comparison on positioning errors of follower AUV obtained by different methods

实验结果如图6～9 所示。图6和图7分别为不同 定位方法估计的跟随AUV 经度位置和纬度位置的比较图，图8为不同定位方法估计得到的跟随AUV 轨迹对比，图9为不同定位方法估计位置与基准位置的距离误差。

从图6～9 可以看出，航位推算位置逐渐偏离真实轨迹，这是因为航位推算只依赖DVL 和罗经提供的速度和航向信息计算位置，累积误差随时间而增大。在1150～1350 s 时间段内，由于跟随AUV 与领航AUV 之间的通信包丢失，EKF 量测更新过程无法进行，滤波估计的定位误差迅速增加。但由于ANFIS 具有利用“历史数据”预测未来结果的能力，所以通过收集1～1149 s之间的传感器及滤波输出的数据对ANFIS 进行训练，即得到跟随AUV 位置随时间、速度及航位推算位置之间的变换规律，使得在通信包丢失期间ANFIS-EKF 方法可以有效抑制定位误差的发散，实现更准确的位置预测。在1150～1350 s 期间（即通信包丢失的时间段内），不同协同定位方法的定位性能比较如表2所示。

表2 不同协同定位方法在通信包丢失时定位性能比较 Tab.2 Cooperative location performance of different methods when communication packet loss

由表2协同定位性能比较结果可以看出，多AUV协同定位系统在跟随AUV 与领航AUV 之间通信包丢失时，基于ANFIS-EKF 比仅基于EKF 的协同定位方法平均定位误差减少78%，均方根误差减少77%，最大定位误差减小62 m，具有更好的协同定位精度。基于ANFIS-EKF 的协同定位方法在通信包丢失时可替代EKF 滤波估计，更准确地预测跟随AUV 的位置。

5 结 论

本文针对多AUV 协同系统通信包丢失的情况，结合传统的卡尔曼滤波估计，提出了一种基于ANFIS- EKF 的协同定位方法，并通过湖试数据进行了仿真实验验证。主要贡献如下：

1）建立了基于地球坐标系的多AUV 协同导航算法的系统定位模型，提出基于ANFIS-EKF 的多AUV协同定位方法，给出了基于ANFIS-EKF 协同定位方法的详细流程；

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

本文提出的基于ANFIS-EKF 协同定位方法可有效保证量测丢失情况下跟随AUV 的定位性能，为进一步研究多AUV 协同定位系统、提高系统协同定位性能提供了可行性思路和方法。但在实际应用过程中，即使跟随AUV 接收到了通信包，也极有可能由于水下环境的不确定性使得量测数据受野值、水声延迟以及洋流等影响，导致滤波估计定位误差变大，这将间接影响ANFIS 模型训练的准确性。这些问题仍需要深入研究与分析，接下来的工作将着重探索在恶劣水下环境中如何保证ANFIS 模型训练的准确性。