[发明专利]基于声学测量网络的水下多AUV协同定位编队拓扑结构优化方法

权利要求书

1.基于声学测量网络的水下多AUV协同定位编队拓扑结构优化方法，其特征在于，该方法包括以下步骤：

步骤1：建立多AUV协同定位模型；

其中，i＝1,2,3...n，n为领航AUV的数目；j＝1,2,3,...m，m为随从AUV的数目；r_ij为第i个领航AUV与第j个随从AUV之间的真实距离；l_ij为第i个领航AUV与第j个随从AUV之间的量测距离；p_i＝[x_i,y_i,z_i]^T为第i个领航AUV在导航坐标系中的位置，x_i为第i个领航AUV在x轴方向的坐标，y_i为第i个领航AUV在y轴方向的坐标，z_i为第i个领航AUV在z轴方向的坐标；q_j＝[x_j,y_j,z_j]^T为第j个随从AUV在导航坐标系中的位置，x_j为第j个随从AUV在x轴方向的坐标，y_j为第j个随从AUV在y轴方向的坐标，z_j为第j个随从AUV在z轴方向的坐标；ω_ij为噪声信号，ω_ij＝σ(1+ηr_ij^γ)，σ为量测噪声常数；η为量测噪声相关的常数，γ为距离相关的常数；

步骤2：推算各随从AUV的Fisher信息矩阵；

步骤3：根据各随从AUV的Fisher信息矩阵，构建系统评价函数；

其中，jυ为一定区域内，第υ个可能的随从AUV位置；

A_ij＝cos(α_ij)cos(β_ij)

B_ij＝sin(α_ij)cos(β_ij)

其中，α_ij为第j个随从AUV到第i个领航AUV的向量在水平面的投影与x轴在逆时针方向上的夹角，其范围为0°到360°；β_ij为第j个随从AUV到第i个领航AUV的向量在垂直于水平面的二维平面上投影与水平面的夹角，其范围为0°到90°；

步骤4：根据系统评价函数，计算最优编队拓扑结构约束条件；

步骤5：引入退火思想完成步进递推过程；步骤5.1：初始化；

(5.1.1)初始化温度T；

(5.1.2)初始化解状态；

对每一个随从AUV来说，通过当前各个领航AUV的横坐标x_i和纵坐标y_i计算其Fisher信息矩阵的行列式|FIM(q_j)|，之后将Fisher信息矩阵的行列式取对数并求和，即得到在当前步骤的评价函数，记为F[t]；

(5.1.3)初始化每个T值时的迭代次数k；

其中，初始温度应足够大，温度下降率趋于1，保证温度下降足够慢，同时每个T值时的迭代次数k足够大，从而保证解状态跑遍整个区域；

步骤5.2：在邻域内产生新解；

步骤5.3：通过得到的新的领航AUV的位置参数x_inew和y_inew计算最优评价函数F_new；

如果F_new＞F_old，则说明新的领航AUV位置能够提升系统的可观测性，那么将新的领航AUV编队拓扑结构作为基础，回到步骤2进行下一步计算；

如果F_new＜F_old，则说明新的领航AUV位置不一定能够提升系统的可观测性，由于此时不能排除局部最优解的情况，因此以Metropolis准则接受新解作为当前解，同样回到步骤2进行下一步计算；

计算得到新的最优评价函数F_new导致温度变化，使得领航AUV的位置参数发生变化如下式所示：

x_{i_new}＝x_{i_old}+μ·T

y_{i_new}＝y_{i_old}+μ·T

其中，μ可选择为(-1,1)上任意一个常数，此时产生新解的邻域的大小与温度高低直接相关，随着温度的降低，产生新解的邻域范围逐渐缩小；

步骤6：根据约束条件验证最终编队拓扑结构，找到最优位置布局。