一种基于混合元启发算法优化ANFIS的AUV协同定位方法与流程

一种基于混合元启发算法优化anfis的auv协同定位方法
技术领域
1.本发明属于水下多航行器协同导航技术领域，具体涉及一种基于混合元启发算法优化anfis的auv协同定位方法。


背景技术：

2.多auv协作系统可以高效地开展复杂海洋作业任务，而进行多auv协同作业首先需要确定每个auv的位置，精准的定位能力是实现协同作业的技术保障之一。随着水声通信技术的进步，基于水声通信网络的多auv协同定位技术得到了越来越多的关注和发展。auv间通过水声通信实现信息共享，通过水声测距实现相互观测，进而通过信息融合技术实现定位误差的协同校正，达到提高多auv集群整体定位能力的目的。在实际运行中，由于多auv协作系统的作业环境复杂并且充满不确定性，极易导致水声通信/测距系统故障或出现噪声异常值等情况发生。例如连续出现量测噪声野值，量测信息长期不更新，量测误差累积增长，以及量测信息连续丢失等情况。这时，仅基于状态空间模型的滤波技术将很难得到auv的准确位置信息。anfis(自适应神经模糊推理系统)具有良好的适应系统变化的学习能力，以及高效处理非线性问题的能力，成为了优化导航算法最受欢迎的技术之一。它可以对传统kalman滤波器进行改进，特别是对动态环境下输入数据的复杂性和不确定性的有效处理，适用于解决复杂水下环境的多auv协同定位问题。但同时，传统的anfis采用了梯度下降法和最小二乘法对隶属度参数进行调整，使得在每一步梯度计算时都可能会出现陷入局部最小值的问题。近年来，人工智能技术在各领域的研究成果斐然，其自学习能力、适应性强、不受模型约束、非线性等特点，能够弥补导航系统测量信息发生的不确定性、模糊性等导致的传统方法失效的缺陷。元启发算法这一类智能算法采用一定的策略接收劣质解，能避免陷入局部最优，可以有效地解决基于梯度方法的问题。将人工智能方法引入到传统的滤波方法中，对各种噪声异常问题进行量测预测，能更好优化预测过程，显著提高协同定位精度。


技术实现要素：

3.本发明的目的在于提供一种基于混合元启发算法优化anfis的auv协同定位方法。
4.一种基于混合元启发算法优化anfis的auv协同定位方法，包括以下步骤：
5.步骤1：领航auv与跟随auv进行水声测距，建立包含测距信息的系统状态空间模型；定义auv在k时刻的位置向量为xk＝[λk,lk]
t
，自身状态方程建立为：
[0006][0007]
其中，δt为采样时间；λk、lk分别为经度和纬度位置；wk＝[w
l,k
,w
λ,k
]
t
为过程噪声向量；v
λ,k
、v
l,k
分别为auv在北向和东向上的速度分量，计算方法为：
[0008][0009]
其中，θk为航向角；vk、ωk分别为auv前向和侧向运动速度，由dvl得到；hk为深度信息；re(1-2e 3e
·
sin2lk)和re(1 e
·
sin2lk)分别为子午线和卯酉线的曲率半径；
[0010]
领航auv与跟随auv之间的相对距离为：
[0011][0012]
其中，分别为领航auv的经度和纬度位置；为领航auv的深度；ak、bk、和定义为：
[0013][0014]
系统的离散状态空间模型建立为：
[0015][0016]
其中，过程噪声w
k-1
～n(0,q
k-1
)和量测噪声δk～n(0,rk)均建模为高斯白噪声；q
k-1
为过程噪声协方差矩阵；rk为量测噪声协方差矩阵；f(x
k-1
,vk,ωk,θk)＝[l
k-1
δt
·vl,k
，λ
k-1
δt
·vλ,k
]为状态转移函数；量测函数；
[0017]
步骤2：在导航工作初期使用ackf估计得到anfis需要的特征数据集；
[0018]
步骤2.1：给定n维初始状态向量和初始误差协方差矩阵为：
[0019][0020]
其中，x0为通过gps得到的跟随auv初始位置坐标；初始误差协方差矩阵p
0|0
根据gps定位精度结合经验进行设置；
[0021]
步骤2.2：时间更新；
[0022]
步骤2.2.1：在第k步分解后验误差协方差阵；
[0023][0024]
步骤2.2.2：生成容积点；
[0025][0026]
步骤2.2.3：容积点传播；
[0027]
[0028]
步骤2.2.4：预测状态量；
[0029][0030]
步骤2.2.5：估计先验误差协方差；
[0031][0032]
其中，s
k-1k-1
是p
k-1k-1
的cholesky分解阵；[1]i表示n维单位矩阵i的第i列向量；n表示状态量的维度；和p
kk-1
分别为状态一步预测值和状态一步预测误差协方差矩阵；q为过程噪声协方差阵；
[0033]
步骤2.3：量测更新；
[0034]
步骤2.3.1：估计新息协方差；
[0035][0036][0037][0038][0039][0040]
步骤2.3.2：构造自适应因子ηk；
[0041][0042]
其中，tr(
·
)为矩阵的迹，为新息矩阵，zk为水声测距得到的实时量测值；
[0043]
步骤2.3.3：计算加入自适应因子ηk后的新息协方差矩阵互协方差矩阵卡尔曼滤波增益
[0044][0045][0046]
[0047]
步骤2.3.4：估计后验状态量；
[0048][0049]
步骤2.3.5：更新后验误差协方差；
[0050][0051]
步骤2.4：最终得到anfis模型需要的输入信息为：
[0052][0053]
其中，*为特征信息的标志；为滤波新息；为基于马氏距离构造的特征信息；
[0054]
步骤3：根据步骤2生成的数据，训练anfis-aqpso-ga模型隶属度参数，得到完整的预测模型；
[0055]
步骤4：使用ackf和anfis-aqpso-ga模型在线预测声学测距误差，对量测信息进行补偿修正；利用修正后的信息估计auv的位置状态预测值和状态误差协方差预测值。
[0056]
进一步地，所述步骤3具体为：
[0057]
步骤3.1：搭建anfis模型；
[0058]
anfis使用模糊逻辑将高度互联的神经网络处理函数和输入特征信息转换为所需的输出；在anfis中，隶属函数是通过样本数据来给出的，函数相互组合或交互的方式称为规则，这些规则分为前件参数和后件参数；采用takagi-sugeno模糊系统模型，规则描述如下：
[0059]
规则1：if x＝a1,y＝b1,z＝c
1 then f1＝m1x p1y q1z r1[0060]
规则2：if x＝a2,y＝b2,z＝c
2 then f2＝m2x p2y q2z r2[0061]
规则3：if x＝a3,y＝b3,z＝c
3 then f3＝m3x p3y q3z r3[0062]
式中，ai、bi和ci为模糊集合；mi、pi、qi和ri是结果参数，通常称为后件参数；anfis结构共有五层，每层都有许多具有特定功能的节点；
[0063]
anfis的模型结构由自适应网络和模糊推理系统合并而成，首先通过收集的样本数据提取初始模糊模型，而后根据梯度下降-最小二程混合算法改变系统参数，使系统的输出更贴近真实的输出；在反向传播中，固定后向参数，使用gd算法更新前件参数；在正向传播中，固定前件参数，通过lse算法更新后件参数；第一层到第五层的每次迭代训练过程，可以让模型的输出误差向减小的方向发展，当达到预定的训练次数或误差范围内时，训练停止；
[0064]
在anfis模型中，第一层和第四层节点参数是自适应的，第二层和第三层节点参数固定，第五层为anfis为模型的输出；在学习算法的前向传递中，节点输出从第一层向前推进到第四层，后件参数由最小二乘确定；在反向传递过程中，误差信号从输出层反向传播到输入层，前件参数由梯度下降法调整；anfis通过此迭代自适应学习过程进行学习和训练，确定能够充分拟合训练数据的隶属度函数参数值，各层工作方式如下所述：
[0065]
第一层：模糊层；
[0066][0067]
式中，o
1,i
为该层输出值；和为广义钟形隶属函数，定义为：
[0068][0069]
式中，ai、bi和ci为被称为前件参数，其数值的改变影响隶属度函数；
[0070]
第二层：规则层；
[0071][0072]
该层实现了模糊推理过程，每个节点的输出表示某一条规则的可信度；
[0073]
第三层：归一化层；
[0074][0075]
第四层：去模糊层；
[0076][0077]
第五层：输出层；
[0078][0079]
在每一次迭代训练中，实际输出和期望输出的均方根误差会减小，当到达预定训练次数或误差范围时，停止训练；
[0080]
anfis系统的输入x、y、z在第一层进行模糊化，在第二层将隶属度函数相乘得到每个规则的激励强度，在第三层将每条激励强度做归一化，第四层和第五层的去模糊化后得到确切的输出，系统输出结果为每条规则的结果的加权平均；
[0081]
步骤3.2：采用aqpso-ga算法选取隶属度参数；
[0082]
为了解决传统的基于梯度下降法易陷入局部最优的问题，采取自适应量子粒子群aqpso算法进行参数选择，并混合ga进一步增强算法全局搜索能力；
[0083]
在qpso算法中，粒子的位置更新规则为：
[0084][0085][0086][0087]
式中，mbest
t
表示在第t次迭代时所有粒子最优位置的平均值；n为种群粒子数；d为粒子维度；p
ij,t
为p
ij,t
和g
j,t
之间的随机位置；p
ij,t
表示种群第i个粒子的第j维在第t次迭
代时的最优位置；g
j,t
表示种群的全局最优解的第j维位置；和u
ij,t
均为[0,1]之间的随机数；x
ij,t
表示第i个粒子的第j维在第t次迭代时的位置；β
t
是qpso重要参量，是收缩-扩张系数，用以控制qpso算法的收敛速度，设置为
[0088][0089][0090]
其中，β
i,t
表示在第t次迭代时第i个粒子的收缩-扩张系数；fi表示第i个粒子的适应度值；f
gbest
表示全局最优解g
t
对应粒子的适应度函数，适应度函数选取为：
[0091][0092]
其中，m为样本数；为在第i次迭代参数下fis计算的预测结果；表示训练数据集中第m个样本数据。
[0093]
进一步地，所述步骤3.2中采用ga算法优化粒子更新的规则为：
[0094]
步骤3.2.1：设定aqpso算法中的种群粒子个数和取值范围并赋初值，确定寻优参数的维数和变化范围；
[0095]
步骤3.2.2：将aqpso算法中当前种群内的全部粒子视作ga算法中的染色体，计算每个染色体对应的适应度值；
[0096]
步骤3.2.3：以适应度值为评价指标，收集最佳染色体；
[0097]
步骤3.2.4：利用aqpso算法对当前种群中的粒子进行全局的初步搜索，通过式(29)～(31)对种群中粒子的位置进行更新；
[0098]
步骤3.2.5：利用ga算法对step4中更新不充分的粒子进行初始化编码，对编码化的种群粒子进行交叉算子操作；
[0099]
步骤3.2.6：完成交叉算子操作后，需要对当前种群中的所有染色体进行变异算子操作；
[0100]
步骤3.2.7：对当前更新后的粒子群中的每个粒子，再次计算出各自对应的适应度值，并根据适应度值更新p
i,t
和g
t
；
[0101]
步骤3.2.8：以达到设定的最大迭代次数或全局最优解误差满足系统需要为终止条件。
[0102]
进一步地，所述步骤4中基于anfis-aqpso-ga的协同定位方法分为训练过程和预测过程两个部分：
[0103]
在0＜k≤k
train
时间段，anfis-aqpso-ga算法处于训练过程，主要工作为收集用于训练anfis模型参数的样本数据，并在k
train
时刻开始训练；在使用aqpso-ga算法训练anfis模型的过程中，根据anfis模型隶属度函数，初始化一个规模与隶属度函数的参数数量相同
的粒子群，根据步骤3.2对种群中粒子的位置进行更新，而ga算法在连续的迭代中进行选择、交叉和变异操作，直到达到最大迭代数或全局最优解误差满足系统需要时停止训练，得到anfis-aqpso-ga规则；
[0104]
在k≠k
train
时，假设所有auv与cna因工作需要，需深入水下航行，无法再接收到各航行器的gps位置信息，即无法获得auv与cna之间的基准距离，anfis-aqpso-ga算法进入预测过程；在预测过程中，以每一时刻的和作为输入数据，通过anfis-aqpso-ga模型预测声学测距误差，对通过声学调制解调器得到的测量距离zk进行闭环修正，最后利用修正后的声学距离信息估计auv的位置状态预测值和状态误差协方差预测值。
[0105]
本发明的有益效果在于：
[0106]
本发明基于aqpso-ga方法训练的anfis-aqpso-ga模型具有更好拟合输入输出数据的能力。训练好的anfis-aqpso-ga模型能在实时工作环境中，对丢失量测信息进行补偿，对异常量测信息进行修正，大大降低协同导航整体估计误差，提升长航时缺少基准位置条件下的定位精度。本发明采用anfis-aqpso-ga结构设计的预测结构对协同定位方法本身计算复杂度无明显影响，能满足实时动态条件下的计算需求，且有效提升了协同定位算法的鲁棒性和定位精度。
附图说明
[0107]
图1为anfis结构及优化过程。
[0108]
图2为本发明原理流程图。
[0109]
图3为基础anfis算法训练和预测的结果及统计图。
[0110]
图4为anfis-aqpso-ga算法训练和预测的结果及统计图。
[0111]
图5为优化anfis参数过程中各优化算法适应度函数值的收敛过程图。
[0112]
图6为auv之间的相对距离信息与测距误差图。
[0113]
图7为跟随auv与两主auv量测误差统计图。
[0114]
图8为auv与cna-1/2测距误差预测结果图。
[0115]
图9为基于不同协同定位方法的航位推算轨迹图。
[0116]
图10为基于不同协同定位方法的位置估计误差图。
具体实施方式
[0117]
下面结合附图对本发明做进一步描述。
[0118]
本发明属于水下多航行器协同导航领域，具体涉及一种利用自适应量子行为粒子群-遗传算法(aqpso-ga)优化自适应神经模糊推理系统(anfis)模型参数进行量测误差预测，来提高协同定位精度的方法。
[0119]
本发明的目的在于提供一种在通信量测丢失等异常情况下，采用自适应量子行为粒子群-遗传算法(aqpso-ga)优化自适应神经模糊推理系统(anfis)模型参数，来更快预测声学测距误差的协同定位方法，从而降低各种量测异常引起的负面影响，提高协同定位精度。
[0120]
步骤一：领航auv与跟随auv进行水声测距，建立包含测距信息的系统状态空间模型定义auv在k时刻的位置向量为xk＝[λk,lk]
t
，自身状态方程建立为
[0121][0122]
其中，δt为采样时间；λk、lk分别为经度和纬度位置，单位为rad；wk＝[w
l,k
,w
λ,k
]
t
为过程噪声向量；v
λ,k
、v
l,k
分别为auv在北向和东向上的速度分量，由(2)计算得到。
[0123][0124]
其中，θk为航向角；vk、ωk分别为auv前向和侧向运动速度，由dvl(多普勒计程仪)得到；hk为深度信息；re(1-2e 3e
·
sin2lk)和re(1 e
·
sin2lk)分别为子午线和卯酉线的曲率半径，re＝6378137m，e＝1/298.257。
[0125]
计算领航-跟随auv之间的相对距离为
[0126][0127]
式中，分别为领航auv的经度和纬度位置；为领航auv的深度。ak、bk、和定义为：
[0128][0129]
由运动模型(1)和两侧模型(3)，系统的离散状态空间模型建立为
[0130][0131]
式中，过程噪声w
k-1
～n(0,q
k-1
)和量测噪声δk～n(0,rk)均建模为高斯白噪声；q
k-1
为过程噪声协方差矩阵，rk为量测噪声协方差矩阵；f(x
k-1
,vk,ωk,θk)＝[l
k-1
δt
·vl,k
，λ
k-1
δt
·vλ,k
]为状态转移函数，为(3)中的量测函数。
[0132]
步骤二：在导航工作初期使用ackf(自适应容积卡尔曼滤波)估计得到anfis需要的特征数据集
[0133]
ckf算法可以估计跟随auv的位置状态更新和状态误差协方差矩阵更新。给定n维初始状态向量和初始误差协方差矩阵为
[0134][0135]
式中，x0为通过gps得到的跟随auv初始位置坐标；初始误差协方差矩阵p
0|0
根据gps定位精度结合经验进行设置。ckf估计步骤如下：
[0136]
(1)时间更新
[0137]
在第k步分解后验误差协方差阵：
[0138][0139]
生成容积点：
[0140][0141]
容积点传播：
[0142][0143]
预测状态量：
[0144][0145]
估计先验误差协方差：
[0146][0147]
其中，s
k-1k-1
是p
k-1k-1
的cholesky分解阵；[1]i表示n维单位矩阵i的第i列向量，n表示状态量的维度。和p
kk-1
分别为状态一步预测值和状态一步预测误差协方差矩阵。f(
·
)在(4)中给出，q为过程噪声协方差阵。
[0148]
(2)量测更新
[0149][0150][0151][0152][0153]
估计新息协方差：
[0154][0155]
估计互协方差：
[0156][0157]
估计卡尔曼增益：
[0158][0159]
估计后验状态量：
[0160][0161]
更新后验误差协方差：
[0162][0163]
其中，为量测预测值，p
zz,k|k-1
，p
xz,k|k-1
，和kk分别为自协方差矩阵，互协方差矩阵和卡尔曼滤波增益。和p
kk
分别为状态估计更新值和状态估计误差协方差矩阵。zk为水声modem测量的实时量测值。h(
·
)在(4)中给出，r为测量噪声协方差阵。
[0164]
为提升ckf算法的自适应与鲁棒性，进一步改善ckf算法滤波性能。本发明通过构造自适应因子ηk将ckf算法进行改进得到一种简单有效的自适应容积卡尔曼滤波(ackf)以适应量测突变情况，可以有效提升滤波精度。合理的自适应因子能够平衡协同定位模型与观测信息的权重比，控制异常量测信息对滤波状态估计的影响。由于滤波新息可以反映系统模型的误差量级，若水声观测新息准确可靠，系统模型也是可靠的，则系统模型所预测的测距参数与水声测距所得量测参数之间的新息应该很小；反之，若新息较大，且系统模型可靠，则表明水声测距出现异常值。如此可以依据新息构造自适应因子ηk定义为：
[0165][0166]
其中，tr(
·
)为矩阵的迹，为新息矩阵，zk为水声测距得到的实时量测值。
[0167]
通过式(19)将式(14)(15)变更为
[0168][0169][0170]
式(16)中的增益kk变更为
[0171][0172]
根据以上估计流程，得到anfis模型需要的输入信息为
[0173][0174]
其中，*为特征信息的标志；为滤波新息；为k时刻跟随auv系统模型输出的一步预测；为基于马氏距离构造的特征信息。
[0175]
步骤三：根据步骤二生成的数据，训练anfis-aqpso-ga模型隶属度参数，得到完整的预测模型；
[0176]
(1)搭建anfis模型
[0177]
anfis是一种实用的人工智能方法，使用模糊逻辑将高度互联的神经网络处理函数和输入特征信息转换为所需的输出。在anfis中，隶属函数是通过样本数据来给出的，函数相互组合或交互的方式称为规则，这些规则分为前件参数和后件参数。采用takagi-sugeno模糊系统模型，规则描述如下：
[0178]
规则1：if x＝a1,y＝b1,z＝c
1 then f1＝m1x p1y q1z r1[0179]
规则2：if x＝a2,y＝b2,z＝c
2 then f2＝m2x p2y q2z r2[0180]
规则3：if x＝a3,y＝b3,z＝c
3 then f3＝m3x p3y q3z r3[0181]
式中，ai、bi和ci为模糊集合；mi、pi、qi和ri是结果参数，通常称为后件参数。anfis结构共有五层，每层都有许多具有特定功能的节点。图1展示出了具有三个输入端和具有一个输出端的anfis系统结构及模型优化过程。
[0182]
anfis的模型结构由自适应网络和模糊推理系统合并而成，首先通过收集的样本数据提取初始模糊模型，而后根据梯度下降-最小二程混合算法改变系统参数，使系统的输出更贴近真实的输出。即，在反向传播中，固定后向参数，使用gd算法更新前件参数。在正向传播中，固定前件参数，通过lse算法更新后件参数。第一层到第五层的每次迭代训练过程，可以让模型的输出误差向减小的方向发展，当达到预定的训练次数或误差范围内时，训练停止。
[0183]
在anfis模型中，第一层和第四层节点参数是自适应的，第二层和第三层节点参数固定，第五层为anfis为模型的输出。在学习算法的前向传递中，节点输出从第一层向前推进到第四层，后件参数由最小二乘确定；在反向传递过程中，误差信号从输出层反向传播到输入层，前件参数由梯度下降法调整。anfis通过此迭代自适应学习过程进行学习和训练，确定能够充分拟合训练数据的隶属度函数参数值，各层工作方式如下所述。
[0184]
第一层:模糊层
[0185][0186]
式中，o
1,i
为该层输出值；和为广义钟形隶属函数(gbellmf)，定义为：
[0187][0188]
式中，ai、bi和ci为被称为前件参数，其数值的改变影响隶属度函数。
[0189]
第二层:规则层
[0190][0191]
该层实现了模糊推理过程，每个节点的输出表示某一条规则的可信度。
[0192]
第三层:归一化层
[0193][0194]
第四层:去模糊层
[0195]
[0196]
第五层:输出层
[0197][0198]
在每一次迭代训练中，实际输出和期望输出的均方根误差会减小，当到达预定训练次数或误差范围时，停止训练。
[0199]
anfis系统的输入x，y，z在第一层进行模糊化，在第二层将隶属度函数相乘得到每个规则的激励强度，在第三层将每条激励强度做归一化，第四层和第五层的去模糊化后得到确切的输出，系统输出结果为每条规则的结果的加权平均。
[0200]
(2)采用aqpso-ga算法选取隶属度参数
[0201]
为了解决传统的基于梯度下降法易陷入局部最优的问题，采取自适应量子粒子群(aqpso)算法进行参数选择，并混合ga(遗传算法)进一步增强算法全局搜索能力。
[0202]
在qpso算法中，粒子的位置更新规则为：
[0203][0204][0205][0206]
式中，mbest
t
表示在第t次迭代时所有粒子最优位置的平均值；n为种群粒子数；d为粒子维度；p
ij,t
为p
ij,t
和g
j,t
之间的随机位置；p
ij,t
表示种群第i个粒子的第j维在第t次迭代时的最优位置；g
j,t
表示种群的全局最优解的第j维位置；和u
ij,t
均为[0,1]之间的随机数；x
ij,t
表示第i个粒子的第j维在第t次迭代时的位置；β
t
是qpso重要参量，是收缩-扩张系数(contraction-expansion coefficient)，用以控制qpso算法的收敛速度，通常采用线性减小的方法，设置为：
[0207]
β
t
＝0.5(t
max-t) 0.5
ꢀꢀꢀ
(32)
[0208]
β
t
成为qpso算法中唯一要控制的参数，在算法运行初期，β
t
值较大，收敛速度较慢，具有全局搜索能力；随着迭代次数的增加，β
t
值逐渐减小，收敛速度加快，由较强的得局部所有能力。然而在实际应用中，这种调整方法并不能根据执行过程进行合理的调整，因此需要采用自适应方法调整参数，得到aqpso算法：
[0209][0210]
[0211]
其中，β
i,t
表示在第t次迭代时第i个粒子的收缩-扩张系数；fi表示第i个粒子的适应度值；f
gbest
表示全局最优解g
t
对应粒子的适应度函数。适应度函数选取为：
[0212][0213]
其中，m为样本数，为在第i次迭代参数下fis计算的预测结果，表示训练数据集中第m个样本数据。
[0214]
当error_f较大时，说明粒子远离当前最优位置，应使β
i,t
取较小值，加快收敛速度；当error_f较小时，说明粒子靠近当前最优位置，应使β
i,t
取较大值，扩大搜索范围，避免陷入局部最优。这样，便可将收缩-扩张系数由原来的每次迭代线性减小，更改为在每次迭代中根据适应度函数来修正。
[0215]
在上述aqpso算法中，粒子种群是随机初始化的，在算法迭代之初，种群搜索具有一定的盲目性，这样不仅会降低算法的搜索效率，从而影响算法的稳定性。同时，随着aqpso算法不断的更新迭代，种群粒子的多样性必然减少，从而导致过早陷入局部最优解。
[0216]
采用ga算法优化粒子更新的规则为：
[0217]
1.设定aqpso算法中的种群粒子个数和取值范围并赋初值，确定寻优参数的维数和变化范围；
[0218]
2.将aqpso算法中当前种群内的全部粒子视作ga算法中的染色体，计算每个染色体对应的适应度值；
[0219]
3.以适应度值为评价指标，收集最佳染色体；
[0220]
4.利用aqpso算法对当前种群中的粒子进行全局的初步搜索，通过式(29)～(31)对种群中粒子的位置进行更新；
[0221]
5.利用ga算法对step4中更新不充分的粒子进行初始化编码，对编码化的种群粒子进行交叉算子操作；
[0222]
6.完成交叉算子操作后，需要对当前种群中的所有染色体进行变异算子操作；
[0223]
7.对当前更新后的粒子群中的每个粒子，再次计算出各自对应的适应度值，并根据适应度值更新p
i,t
和g
t
；
[0224]
8.以达到设定的最大迭代次数或全局最优解误差满足系统需要为终止条件。
[0225]
步骤四：使用ackf和anfis-aqpso-ga模型在线预测声学测距误差，对量测信息进行补偿修正
[0226]
基于anfis-aqpso-ga的协同定位方法分为训练过程和预测过程两个部分，图2给出了整体的计算流程。
[0227]
在0＜k≤k
train
时间段，anfis-aqpso-ga算法处于训练过程，主要工作为收集用于训练anfis模型参数的样本数据，并在k
train
时刻开始训练。在使用aqpso-ga算法训练anfis模型的过程中，根据anfis模型隶属度函数，初始化一个规模与隶属度函数的参数数量相同的粒子群，根据公式(29)～(31)对种群中粒子的位置进行更新，而遗传算法在连续的迭代中进行选择、交叉和变异操作，直到达到最大迭代数或全局最优解误差满足系统需要时停止训练，得到anfis-aqpso-ga规则。在k＞k
train
时，假设所有auv与cna(communication and navigation aid，高精度导航设备)因工作需要，需深入水下航行，无法再接收到各航行器
的gps位置信息，即无法获得auv与cna之间的基准距离，anfis-aqpso-ga算法进入预测过程。在预测过程中，以每一时刻的和作为输入数据，通过anfis-aqpso-ga模型预测声学测距误差，对通过声学调制解调器得到的测量距离zk进行闭环修正，最后利用修正后的声学距离信息通过式(17)和(18)估计auv的位置状态预测值和状态误差协方差预测值。
[0228]
需要说明的是，图2所示的算法结构通过k
train
时刻对训练过程和预测过程进行划分，即需要利用0＜k≤k
train
时间段收集样本数据集。而在实际应用中，利用现存数据集对anfis模型进行离线训练，工作时直接执行预测过程同样有效。
[0229]
为进一步说明本发明的有益效果，首先采用公开数据集作进行了仿真测试。基础anfis模型和aqpso-ga算法优化的anifs模型在使用相同数据下的检验结果如图3、图4所示。统计结果表明，基于aqpso-ga方法训练的anfis-aqpso-ga模型预测误差在不同评价指标下均提高了40％以上。同时，为充分验证本发明所提aqpso-ga方法的优越性和稳定性，还对比了使用其他元启发算法优化anfis模型时适应度函数收敛速度与收敛结果情况，如图5所示。
[0230]
然后使用湖上试验数据验证本发明有效性。试验使用了三艘船，均安装了水下声学设备、dvl、gps，其中两艘安装了高精度导航设备(cna-1、cna-2)模拟领航auv，另一艘安装了低精度导航设备模拟跟随auv。auv在执行任务前，均通过gps进行时间校正，以保证时间同步。
[0231]
auv首先向cna-1发送测距请求信号进行水声测距，当cna-1接收到auv发送的请求信号后，根据水声信号传播时间计算二者之间的相对距离；紧接着，cna-1将获得的水声距离信息连同自身位置信息发送给auv，auv利用该参考信息进行协同定位，实现对自身航位推算误差的一次校正。一次校正过程完成后，cna-2重复cna-1的协同过程，进而实现对auv的二次校正。
[0232]
试验数据长度为1700s，基准位置轨迹由gps得到。初始状态误差噪声协方差阵设置为p0＝diag[1m,1m]2，过程和量测噪声协方差阵分别设置为qk＝diag[1m,1m]2和rk＝[3m]2。
[0233]
为全面地考虑水声测距系统可能发生的各种异常情况，以及能够更好的突出本方法所提协同定位方法与现有方法相比的优越性，在收集到数据的基础上添加了若干符合水下实际复杂情况的异常噪声(包括测距信息不更新、测距误差漂移、野值噪声)，如图6、图7所示，将处理后的数据用于验证算法。
[0234]
将数据集中前540s的信息用于训练模型，在540s以后，基于anfis-aqpso-ga的协同定位方法进入预测过程。图8显示了训练后模型可以根据特征信息有效估计声学测距误差。同时，设置补偿阈值为9m，减小计算复杂度。
[0235]
图9绘制了采用不同协同定位方法估计auv位置的航行轨迹，图10统计了图9中航位推算的误差。本发明给出的方法估计误差最低，能有效减小量测野值噪声影响，保证系统稳定运行。
[0236]
本发明的主要优点体现在：
[0237]
1.基于aqpso-ga方法训练的anfis-aqpso-ga模型与基于梯度法训练的anfis模型相比，前者具有更好拟合输入输出数据的能力。通过测试数据分别计算统计的误差结果显
示，前者的各项指标精度均提高40％以上；
[0238]
2.训练好的anfis-aqpso-ga模型能在实时工作环境中，对丢失量测信息进行补偿，对异常量测信息进行修正，大大降低协同导航整体估计误差，提升长航时缺少基准位置条件下的定位精度；
[0239]
3.采用anfis-aqpso-ga结构设计的预测结构对协同定位方法本身计算复杂度无明显影响，能满足实时动态条件下的计算需求，且有效提升了协同定位算法的鲁棒性和定位精度。
[0240]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。