本发明属于声学测量领域，具体涉及一种基于声学测量网络的水下多AUV协同定位编队拓扑结构优化方法。本发明在多随从AUV情况下，考虑了声学测距误差与距离的相关性，具有更高的实用价值；针对位置信息的不确定性，根据相应的概率密度采用蒙特卡洛方法对可能分布区域内的编队构型进行优化设计；采用基于退火思想的步进递推的策略，不管主艇初始位置在何处，均可经过迭代步骤找到其最优位置布局；本发明引入Metropolis准则作为判断是否接受新解作为当前解的准则之一，可有效改善当局部最优解出现时迭代不再继续进行的情况；本发明的迭代过程产生新解邻域的大小与温度高低直接相关，增加最终结果的精确性。
1.基于声学测量网络的水下多AUV协同定位编队拓扑结构优化方法，其特征在于，该方法包括以下步骤：
其中，i＝1,2,3...n，n为领航AUV的数目；j＝1,2,3,...m，m为随从AUV的数目；rij为第i个领航AUV与第j个随从AUV之间的真实距离；lij为第i个领航AUV与第j个随从AUV之间的量T
测距离；pi＝[xi,yi,zi] 为第i个领航AUV在导航坐标系中的位置，xi为第i个领航AUV在x轴方向的坐标，yi为第i个领航AUV在y轴方向的坐标，zi为第i个领航AUV在z轴方向的坐标；qjT
＝[xj,yj,zj] 为第j个随从AUV在导航坐标系中的位置，xj为第j个随从AUV在x轴方向的坐标，yj为第j个随从AUV在y轴方向的坐标，zj为第j个随从AUV在z轴方向的坐标；ωij为噪声γ
信号，ωij＝σ(1+ηrij )，σ为量测噪声常数；η为量测噪声相关的常数，γ为距离相关的常数；
步骤3：根据各随从AUV的Fisher信息矩阵，构建系统评价函数；
其中，jυ为一定区域内，第υ个可能的随从AUV位置；
Aij＝cos(αij)cos(βij)Bij＝sin(αij)cos(βij)其中，αij为第j个随从AUV到第i个领航AUV的向量在水平面的投影与x轴在逆时针方向上的夹角，其范围为0°到360°；βij为第j个随从AUV到第i个领航AUV的向量在垂直于水平面的二维平面上投影与水平面的夹角，其范围为0°到90°；
步骤4：根据系统评价函数，计算最优编队拓扑结构约束条件；
步骤5：引入退火思想完成步进递推过程；步骤5.1：初始化；
对每一个随从AUV来说，通过当前各个领航AUV的横坐标xi和纵坐标yi计算其Fisher信息矩阵的行列式|FIM(qj)|，之后将Fisher信息矩阵的行列式取对数并求和，即得到在当前步骤的评价函数，记为F[t]；
其中，初始温度应足够大，温度下降率趋于1，保证温度下降足够慢，同时每个T值时的迭代次数k足够大，从而保证解状态跑遍整个区域；
步骤5.3：通过得到的新的领航AUV的位置参数xinew和yinew计算最优评价函数Fnew；
如果Fnew＞Fold，则说明新的领航AUV位置能够提升系统的可观测性，那么将新的领航AUV编队拓扑结构作为基础，回到步骤2进行下一步计算；
如果Fnew＜Fold，则说明新的领航AUV位置不一定能够提升系统的可观测性，由于此时不能排除局部最优解的情况，因此以Metropolis准则接受新解作为当前解，同样回到步骤2进行下一步计算；
计算得到新的最优评价函数Fnew导致温度变化，使得领航AUV的位置参数发生变化如下式所示：
xi_new＝xi_old+μ·Tyi_new＝yi_old+μ·T其中，μ可选择为(‑1,1)上任意一个常数，此时产生新解的邻域的大小与温度高低直接相关，随着温度的降低，产生新解的邻域范围逐渐缩小；
步骤6：根据约束条件验证最终编队拓扑结构，找到最优位置布局。
基于声学测量网络的水下多AUV协同定位编队拓扑结构优化
方法
技术领域
[0001] 本发明属于声学测量领域，具体涉及一种基于声学测量网络的水下多AUV协同定位编队拓扑结构优化方法。
背景技术
[0002] 协同定位是目前中间层区域多自主式水下航行器最有效的导航方法之一。航行器编队拓扑结构会对整个系统的可观测性产生直接影响，从而影响多自主水下航行器协同定
位精度。因此，考虑水声测距误差与距离的相关性，对协同定位系统的编队拓扑结构进行优
化，使系统的可观测性达到最优，是协同定位方向需要深入研究的课题。目前针对多领航、
多随从AUV 系统，多采用Fisher信息矩阵推导评价函数，采取基于偏导数的梯度算法进行
位置的迭代。然而，该方法的求解过程依赖于初始位置的选取，若初始位置选取不当，往往
存在陷入局部最优解的危险，而无法得到最优拓扑结构。
[0003] 本发明考虑和距离相关的水声噪声，采用引入退火思想的步进递推策略对多AUV协同定位系统的最优编队拓扑构型的优化方法进行了研究。现在常用的令对随从AUV取
Fisher 信息矩阵的行列式作为评价函数的方法，在AUV编队规模日益扩大的情况很难通过
计算机解算得到最优编队拓扑结构；基于偏导数的迭代策略虽然可以在一定程度上避免多
解或无解情况的产生，但在初始位置选取不当时往往受限于局部最优解而无法得到真正最
优的拓扑结构。
[0004] 针对目前在设计多AUV协同定位系统编队构型的求解困难的问题，本发明在基于Fisher 信息矩阵理论的基础上，综合考虑多随从AUV、随从AUV位置不确定以及与距离相关
的水声噪声来推导评价函数，提出一种基于退火思想的步进递推策略，最终实现快速递推
得到最优编队拓扑结构。本发明可用于多AUV协同定位系统的编队构型优化领域。
发明内容
[0005] 本发明的目的在于提供一种能够在随从AUV位置不确定情形下，领航AUV从任意初始位置出发，完成多AUV协同定位系统编队构型快速优化设计的方法。
[0006] 本发明的目的是这样实现的：
[0007] 基于声学测量网络的水下多AUV协同定位编队拓扑结构优化方法，该方法包括以下步骤：
[0008] 步骤1：建立多AUV协同定位模型；
[0009] 步骤2：推算各随从AUV的Fisher信息矩阵；
[0010] 步骤3：根据各随从AUV的Fisher信息矩阵，构建系统评价函数；
[0011] 步骤4：根据系统评价函数，计算最优编队拓扑结构约束条件；
[0012] 步骤5：引入退火思想完成步进递推过程；
[0013] 步骤5.1：初始化；
[0014] 步骤5.2：在邻域内产生新解；
[0015] 步骤5.3：通过得到的新的领航AUV的位置参数xinew和yinew计算最优评价函数Fnew；
[0016] 步骤6：根据约束条件验证最终编队拓扑结构，找到最优位置布局。
[0017] 本发明还包括：
[0018] 步骤1所述的多AUV协同定位模型为：
[0019]
[0020]
[0021] (i＝1,2,3...n，j＝1,2,3,...m)
[0022] 式中，rij为第i个领航AUV与第j个随从AUV之间的真实距离；lij为第i个领航AUV T
与第j个随从AUV之间的量测距离；pi＝[xi,yi,zi]为第i个领航AUV在导航坐标系中的位置，
xi为第i个领航AUV在x轴方向的坐标，yi为第i个领航AUV在y轴方向的坐标，zi为第i个领航
T
AUV在z轴方向的坐标；qj＝[xj,yj,zj]为第j个随从AUV在导航坐标系中的位置，xj为第j个
随从AUV在x轴方向的坐标，yj为第j个随从AUV在y轴方向的坐标， zj为第j个随从AUV在z轴
γ
方向的坐标；n为领航AUV的数目，m为ωij为随从AUV的数目；噪声信号，且ωij＝σ(1+ηrij )，
σ为量测噪声常数；η为量测噪声相关的常数，γ为距离相关的常数。
[0023] 步骤2所述的推算各随从AUV的Fisher信息矩阵为：
[0024]
[0025] 式中：向量
[0026] 步骤3所述的系统评价函数表达式为：
[0027]
[0028] 式中：f(qj)为概率密度函数，其具体形式会根据随从AUV的工作任务决定；Dj为随从 AUV可能分布的范围，根据概率密度函数决定，用蒙特卡洛方法化简为如下形式：
[0029]
[0030] 式中，第一参数Aijv、第二参数Bijv、第三参数Mijv表示为：
[0031]
[0032] jυ为一定区域内，第υ个可能的随从AUV位置，在该域内根据构建的可观测性评价函数，解得从艇j的最优位置。
[0033] 步骤4所述的计算最优编队拓扑结构约束条件为：
[0034]
[0035]
[0036] 用蒙特卡洛方法化简成如下形式：
[0037]
[0038]
[0039] 步骤5.1所述的初始化包括以下步骤：
[0040] (5.1.1)初始化温度T；
[0041] (5.1.2)初始化解状态；
[0042] (5.1.3)初始化每个T值时的迭代次数k；
[0043] 对每一个随从AUV来说，通过当前各个领航AUV的横坐标xi和纵坐标yi计算其Fisher 信息矩阵的行列式|FIM(qj)|，之后将Fisher信息矩阵的行列式取对数并求和，即
得到在当前步骤的评价函数，记为F[t]，作为初始化的解状态；其中温度下降率趋于1。
[0044] 步骤5.3所述计算最优评价函数Fnew，之后会产生两种情况：
[0045] 情况1：Fnew＞Fold，则说明新的领航AUV位置能够提升系统的可观测性，将新的领航 AUV编队拓扑结构作为基础，回到步骤2进行下一步计算；
[0046] 情况2：Fnew＜Fold，则说明新的领航AUV位置不一定能够提升系统的可观测性，此时不能排除局部最优解的情况，因此以Metropolis准则接受新解作为当前解，同样回到步骤2
进行下一步计算；
[0047] 计算得到新的最优评价函数Fnew导致温度变化，使得领航AUV的位置参数发生变化如下式所示：
[0048] xi_new＝xi_old+μ·T
[0049] yi_new＝yi_old+μ·T
[0050] 其中，μ可选择为(‑1,1)上任意一个常数，此时产生新解的邻域的大小与温度高低直接相关，随着温度的降低，产生新解的邻域范围逐渐缩小。
[0051] 本发明的有益效果在于：
[0052] 1.本发明在多随从AUV情况下，考虑了声学测距误差与距离的相关性，具有更高的实用价值；
[0053] 2.本发明针对位置信息的不确定性，根据相应的概率密度采用蒙特卡洛方法对可能分布区域内的编队构型进行优化设计，针对实际中可能出现的问题有较好的应对方案；
[0054] 3.本发明采用基于退火思想的步进递推的策略，递推过程的进行与初始值的选取无关，也就是说，不管主艇初始位置在何处，均可经过迭代步骤找到其最优位置布局；
[0055] 4.本发明引入Metropolis准则作为判断是否接受新解作为当前解的准则之一，可有效改善当局部最优解出现时迭代不再继续进行的情况，是一种以概率l收敛于全局最优
解的全局优化算法；
[0056] 5.本发明的迭代过程产生新解邻域的大小与温度高低直接相关，随着温度的降低，产生新解的邻域逐渐缩小至一个极小的范围，这样可以在递推的过程中逐步减小领航
AUV位置递推移动的距离，增加最终结果的精确性。
附图说明
[0057] 图1为本发明的流程图。
[0058] 图2为随从AUV位置不确定时交叉计算示意图。
[0059] 图3为角度投影示意图。
具体实施方式
[0060] 下面结合附图对本发明做进一步描述：
[0061] 步骤一：建立多AUV协同定位模型；
[0062] 建立多AUV协同定位模型时，采用的模型为：
[0063]
[0064] 式中：
[0065] ωij＝σ(1+ηrijγ),
[0066]
[0067] (i＝1,2,3...n，j＝1,2,3,...m)
[0068] lij为第i个领航AUV与第j个随从AUV之间的量测距离；pi＝[xi,yi,zi]T为第i个领T
航 AUV在导航坐标系中的位置；qj＝[xj,yj,zj] 为第j个随从AUV在导航坐标系中的位置；
ωij为噪声信号；rij为第i个领航AUV与第j个随从AUV之间的真实距离；σ为量测噪声常数；η
和γ均为量测噪声和距离相关的常数。
[0069] 步骤二：推算各随从AUV的Fisher信息矩阵；
[0070] 推算各随从AUV的Fisher信息矩阵时，采用的算法模型为：
[0071]
[0072] 式中：
[0073]
[0074] 步骤三：推导系统评价函数表达式；
[0075] 考虑随从AUV位置的不确定性，推导系统评价函数表达式时采用的算法模型为：
[0076]
[0077] 式中：f(qj)为概率密度函数，其具体形式会根据随从AUV的工作任务决定；Dj为随从AUV 可能分布的范围，根据概率密度函数决定。用蒙特卡洛方法可以化简为如下形式：
[0078]
[0079] 式中：
[0080]
[0081] jυ为一定区域内，第υ个可能的随从AUV位置，如令υ＝10。在该域内根据构建的可观测性评价函数，解得从艇j的最优位置。交叉计算示意图如图2所示。
[0082] Aij＝cos(αij)cos(βij)
[0083] Bij＝sin(αij)cos(βij)
[0084] αij为第j个随从AUV到第i个领航AUV的向量在水平面的投影与x轴在逆时针方向上的夹角，其范围为0°到360°，βij为第j个随从AUV到第i个领航AUV的向量在垂直于水平面的
二维平面上投影与水平面的夹角，其范围为0°到90°。角度投影示意图如图3所示。
[0085] 步骤四：计算最优编队拓扑结构约束条件；
[0086] 计算最优编队拓扑结构约束条件时，采用的算法模型为：
[0087]
[0088]
[0089] 用蒙特卡洛方法可以化简成如下形式：
[0090]
[0091]
[0092] 步骤五：引入退火思想完成步进递推过程；
[0093] 具体实施步骤如下：
[0094] (1)初始化：
[0095] a.初始化温度T；
[0096] b.初始化解状态；
[0097] 对每一个随从AUV来说，通过当前各个领航AUV的横坐标xi和纵坐标yi计算其Fisher 信息矩阵的行列式|FIM(qj)|，之后将Fisher信息矩阵的行列式取对数并求和，即可
以得到在当前步骤的评价函数，记为F[t]。
[0098] c.初始化每个T值时的迭代次数k。
[0099] 其中，初始温度应足够大，温度下降率趋于1，保证温度下降足够慢，同时每个T值时的迭代次数k足够大，从而保证解状态跑遍整个区域。
[0100] (2)在邻域内产生新解；
[0101] (3)通过得到的新的领航AUV的位置参数xinew和yinew计算最优评价函数Fnew。此时可能会产生两种情况：
[0102] ·如果Fnew＞Fold，则说明新的领航AUV位置能够提升系统的可观测性。那么将新的领航AUV编队拓扑结构作为基础，回到步骤(2)进行下一步计算。
[0103] ·如果Fnew＜Fold，则说明新的领航AUV位置不一定能够提升系统的可观测性。由于此时不能排除局部最优解的情况，因此以Metropolis准则接受新解作为当前解，同样回到
步骤 (2)进行下一步计算。
[0104] 温度变化使得领航AUV的位置参数发生变化如下式所示：
[0105] xi_new＝xi_old+μ·T
[0106] yi_new＝yi_old+μ·T
[0107] 其中，μ可选择为(‑1,1)上任意一个常数，此时产生新解的邻域的大小与温度高低直接相关。随着温度的降低，产生新解的邻域范围逐渐缩小。
[0108] 步骤六：根据约束条件验证最终编队拓扑结构。
