0 引　言

协同思想在多机器人定位、无线传感网络定位以及无线移动网络定位等领域得到了广泛的研究和发展。随着水下声学通信技术的发展，基于水声通信网络的多自主水下航行器（AUV）协同定位技术得到了越来越多的关注^[1]。GNSS信号在水下环境高度衰减，水深超过几米的情况下无法使用^[2]。随着AUV技术的发展以及某些特殊的使命任务需求，进一步增强AUV的导航自主性和隐蔽性，减少利用水面锚节点，采用水下航行器进行协同定位正逐渐成为当前水下定位导航领域的研究热点。

采用AUV协同定位不仅可以提高AUV集群整体位置精度和定位鲁棒性，而且实现简单、成本低、可靠性好，且不受AUV活动区域限制，为多AUV协同应用提供重要的技术保障，具有重要的应用价值^[3]。

本文提出一种基于自适应残差加权的AUV集群分布式协同定位算法（ARW- UWASN）。集群网络内领航AUV可获得高精度的位置信息，AUV之间可进行通信和距离测量，采用协同定位算法对网络内其余携带低精度导航装置的普通AUV进行定位，更新低精度AUV位置信息，从而逐步确定网络中每个AUV节点的精确位置。该算法可以提高AUV集群网络范围内普通AUV节点的定位精度和定位覆盖率，减小节点能量消耗，为水下移动节点协同定位导航的应用与发展提供理论支撑。

1 水下移动节点定位技术 1.1 水下移动节点定位方法

根据水下定位技术的原理不同，目前水下定位方法主要分为3类：基于水下声学传感器网络（underwater acoustic sensor network，UWASN）的声学定位^[4]、基于地理信息匹配的定位（simultaneous localization and mapping，SLAM）^[5]和基于惯性导航系统的定位（inertial navigation system，INS）^[6]。基本上，水声定位技术有两大类，即基于距离的定位^[7]和无距离的定位^[8]。基于距离的定位算法，首先使用TDOA，TOA，AOA和RSSI估计到一些锚定传感器节点的距离或角度，然后通过三边测量（2D）或四边形测量（3D）将这些距离估计转换为位置信息；无距离算法查询传感器节点的局部拓扑和位置估计，这些位置估计是从附近的锚传感器节点获得。

本文采用基于距离测量的UWASN声学定位技术，定位算法分为普通AUV节点定位和领航AUV节点定位。普通AUV节点接收来自领航AUV节点的信息，领航AUV通过浮出水面接收GNSS信息或者与水面浮标或海底潜标通信进行自定位。

1.2 协同定位网络结构

水下AUV节点由于受到通信距离和供能的影响，需要采取有效的定位方式，在有限的通信范围内，不过多消耗节点能量的同时保证定位的精度和覆盖范围。因此本文采用分布式定位方案，每个传感器节点单独定位或节点自由定位，然后将这些数据分别发送给参考节点。

AUV利用航位推算方法可以在水下自定位，领航AUV采用昂贵的惯性导航进行航位推算，在短时间内可以保持高精度的位置信息。本文协同定位是一种分布式的、基于距离估计的定位协议。在AUV运行周期中，领航AUV每间隔T时间广播其位置信息，广播格式定义为{Id；SendTime；Type；Weight；Location}，其中Id为节点的唯一身份标识，SendTime为广播时刻，Type为AUV节点类型，Weight为节点权重，Location为节点位置信息。假设网络内节点是同步的，在接收到多个领航AUV信息后，普通AUV节点可以估计它们自身的位置坐标。

2 AUV集群节点分布式逐级协同定位方案

由于水下AUV节点分布区域的广泛性和节点的移动性，不是所有普通节点都能有效地接收到领航节点的位置广播信息，当普通AUV节点不能接收到足够的领航AUV节点广播信息时，通常采取的方法为：

1）等待接收足够数量领航AUV节点位置信息再定位；

2）直接将已完成定位的普通AUV节点升级为具有精确位置信息的参考节点，接收其位置信息进行定位。

这2种常规方案都有其固有的缺点：方案1，当网络中领航AUV节点数量较少时，可能存在大量普通AUV节点出现不能定位的情况；方案2，直接利用已完成定位的普通节点对待定位节点进行定位，可能会因为已定位的普通节点存在较大的误差，导致待定位节点出现更大的自定位误差。

因此，针对上述缺点，为了使待定位AUV获得更高的定位精度和更大的定位覆盖率，本文提出一种分布式逐级定位方案，采用基于自适应残差加权的AUV集群分布式协同定位算法（ARW- UWASN），利用所有可以获得的领航AUV和位置误差投影统计值达标的已定位AUV节点（参考节点）位置信息对待定位AUV进行位置估计，每3组领航AUV和参考AUV节点可定位估计出一组待定位AUV的潜在位置，一个待定位AUV节点的位置可以表示为所有潜在位置估计的加权平均值，使用残差权重来计算最终的定位结果。

AUV集群网络分布式逐级定位方案的结构如图1所示。其中的节点主要包括：领航AUV节点、普通待定位AUV节点、参考AUV节点。领航AUV携带高精度的导航装备，可在一定时间内获得自身高精度参考位置，在一定时间后位置信息精度降低时，到达水面从GPS或者北斗接收定位信息，对自身位置信息进行校准，然后再潜入水中进行航位推算，在水下进行自定位导航。同时领航AUV可以直接与水面浮标和海底潜标进行通信，获得实时高精度位置信息，从而进行自身位置估计。普通AUV价格便宜，复杂度低，不能浮出水面进行GNSS同步，也不能直接与浮标或潜标通信，只能与相邻的AUV节点通信。

AUV集群网络范围内每个待定位AUV节点的定位过程如图2所示。

该算法主要包括两部分：

1）可以直接与领航AUV通信的普通AUV节点的定位

可直接接收到领航节点信息的定位方式如图1中待定位AUV节点A所处区域所示，待定位节点A侦收到领航节点AUV1，AUV2，AUV3发送的广播信息，直接根据其接收到的信息计算自身的位置坐标值。

2）无法直接与领航AUV通信的普通AUV节点的定位

不是每个待定位节点都能接收到3个或更多领航节点信息，在定位了普通待定位节点，普通节点位置误差统计值小于位置精度阈值后，可将其升级为新的参考节点对周围其他待定位普通节点进行定位。如图1中AUV节点B所处区域所示，节点B与其他领航节点的距离超过通信半径范围，因此在逐级定位的过程中，将已完成定位的普通节点AUV4和AUV5升级为参考节点，利用1个领航节点和2个参考节点，根据与领航节点和参考节点的相对关系计算出自身位置值。

这样，先将距离领航节点通信半径范围内的待定位节点完成定位，随后定位出下一通信半径距离的待定位节点，直至网络内所有待定位节点完成定位。

3 基于自适应残差加权的AUV集群协同定位算法 3.1 待定位节点潜在位置估计

潜在位置估计是利用领航或参考AUV节点与待定位AUV节点的测距求解位置信息。根据速度和广播信息的始末时间计算出节点的距离后，可列出如下方程：

${d_{{r_i}}}{\text{ = }}\sqrt {{{(x - {x_i})}^2} + {{(y - {y_i})}^2} + {{(z - {z_i})}^2}} （i=1\cdots N。$

(1)

式中： $({x_i},{y_i},{z_i})$ 为第 $i$ 个领航节点或参考节点的坐标， $(x,y,z)$ 为待定位的普通节点的坐标，N为待定位节点能够接收到的节点广播的数量， ${d_{{r_i}}}$ 为参考节点 $i$ 与待定位节点的距离。

AUV深度可由深度计获得，上述方程只需要3组便可解得一组待定位普通AUV节点的位置坐标。

3.2 自适应残差加权协同定位算法

为了提高定位结果的鲁棒性和精度，对每一组测量得到的潜在位置 ${P_i}({\hat x_i},{\hat y_i},{\hat z_i})$ 和距离测量 ${d_{{r_i}}}$ 残差进行加权处理，残差可表示为：

${\hat \zeta _i}{\text{ = }}{\hat P_i} - {d_{{r_i}}}。$

(2)

对于 $N$ 维测量残差序列 ${\zeta _i}$ 和预测残差序列 ${\lambda _i}$ ，采用加权函数后的均值和协方差分别表示为：

$\bar \zeta = {\left[\sum\limits_{i = 1}^N {{\omega _{{\zeta _i}}}} \right]^{ - 1}} \cdot \left[\sum\limits_{i = 1}^N {{\omega _{{\zeta _i}}}{\zeta _i}} \right]，$

(3)

${\bar \lambda _r} = {\left[\sum\limits_{i = 1}^N {{\omega _{{\lambda _i}}}} \right]^{ - 1}} \cdot \left[\sum\limits_{i = 1}^N {{\omega _{{\lambda _i}}}{\lambda _i}}\right]。$

(4)

式中： ${\omega _{{\zeta _i}}}$ 和 ${\omega _{{\lambda _i}}}$ 分别为第i个潜在位置测量残差和预测残差的权值系数，采用投影统计法计算位置误差统计值^[9]。具体方法如下：

1）求取矩阵 $[\begin{array}{*{20}{c}} {{h_1}}\;{{h_2}}\; \cdots \;{{h_m}} \end{array}]$ 列空间中位数

${\boldsymbol {m}} = {[median({{\boldsymbol{h}}_{j1}}),\;median({{\boldsymbol{h}}_{j2}}),...,median({{\boldsymbol{h}}_{jn}})]^{\rm{T}}}。$

(5)

2）计算每一个单独数据向量 ${{h}_j}$ 相对于中位数 $m$ 的单位向量

${{\boldsymbol{v}}_j} = ({{\boldsymbol{h}}_j} - {\boldsymbol{m}})/\left\| {{{\boldsymbol{h}}_j} - {\boldsymbol{m}}} \right\| \text{，} j = 1,2,...,m。$

(6)

3）标准化投影

对于每一个单位方向向量 ${{\boldsymbol{v}}_j}$ ：

①将数据向量h投影到每一个单位向量 ${v_j}$

${z_{1j}} = {\boldsymbol{h}}_1^{\rm{T}}{{\boldsymbol{v}}_j}；{z_{2j}} = {\boldsymbol{h}}_2^{\rm{T}}{{\boldsymbol{v}}_j};...；{z_{mj}} = {\boldsymbol{h}}_m^{\rm{T}}{{\boldsymbol{v}}_j},j = 1,2,...,m。\\[-20pt]$

(7)

②对于每一个单位方向上的投影值进行中位数求取

$median({z_j}) = median\{ {z_{1j}},{z_{2j}},...,{z_{mj}}\}。$

(8)

③计算中位数绝对偏差

$mad({z_j}) = c \cdot median|{z_i} - median({z_j})|。$

(9)

式中：c的值为1.4826，是高斯噪声分布下的费舍尔一致性校正因子，用以保证结果的无偏性。

④计算标准化投影

${P_{ij}} = \frac{{\left\| {{z_{ij}} - median({z_j})} \right\|}}{{mad({z_i})}} 。$

(10)

4）计算投影统计

$P{S_i} = \max \{ {p_{i1}},{p_{i2}},...,{p_{im}}\} \text{，} i = 1,2,...,m。$

(11)

得到的投影统计值表示结果偏离样本的程度大小，通常取 ${\chi ^2}$ 分布97.5%置信度为异常判断阈，在进行位置估计结果异常值判断的时候，如果计算得到的位置误差投影统计值大于判断阈，可以认为该位置估计结果为一异常值，将不能用于下一步的定位计算。

待定位节点潜在位置估计的精确度与所有有效残差的和成反比，所有有效残差的和的倒数代表了权重因子，潜在位置 ${P_i}({\hat x_i},{\hat y_i},{\hat z_i})$ 的权重表示如下：

${\omega _i} = {\left[\sum\limits_{i = 1}^N {{{\hat \zeta }_{_i}}} \right]^{ - 1}}。$

(12)

利用加权估计，待定位节点的位置估计为：

$X(\hat x,\hat y,\hat z) = \left[\sum\limits_{i = 1}^N {{\omega _i}\cdot{P_i}({{\hat x}_i},{{\hat y}_i},{{\hat z}_i})} \right]{\left[\sum\limits_{i = 1}^N {{\omega _i}} \right]^{ - 1}}。$

(13)

如果待定位节点收到的领航和参考节点的信息数量少于3时，则无法通过以上算法估算出位置信息，需要等待下一个定位周期，待收到领航和参考节点的信息数大于或等于3时再进行定位。

4 仿真与结果分析

利用Matlab仿真软件对提出的ARW- UWASN定位算法进行仿真，评估算法性能，并与常规非加权自适应定位算法（UWASN）进行比较。

设置领航AUV节点自定位位置误差为2 m。将全部领航AUV节点与待定位AUV节点随机的分布于1000 m×1000 m的区域。设置网络中AUV节点总数量为200个，其中，领航AUV节点比例为0.2（数量为40个），待定位普通AUV节点数量为160个。网络中节点部署情况和节点邻居关系图如图3和图4所示。

根据上述AUV网络节点部署情况，对普通待定位节点进行定位仿真，采用平均定位误差来评估本文算法的定位性能，平均定位误差定义为估计位置到真实位置的欧式距离与通信半径的比值^[10]，计算公式如下：

${{Localization\_error}} = \frac{\displaystyle{\sum\limits_{i = 1}^N {\sqrt {{{({x_i} - {{\hat x}_i})}^2} + {{({y_i} - {{\hat y}_i})}^2}{\text{ + }}{{({z_i} - {{\hat z}_i})}^2}} } }}{{N \times R}}。$

(14)

式中： ${{Localization\_error}}$ 为平均定位误差；N为待定位AUV节点数量；R为通信半径。

从不同的领航AUV节点比例数量、节点的通信半径大小和网络内节点总数3个方面，分析定位算法对定位精度和定位覆盖范围的影响，仿真条件同上。

1）领航AUV节点数量比例的影响

图5为领航AUV节点比例对平均定位误差的影响，图6为领航AUV节点比例对不可定位AUV节点数量的影响。

可以看出，在领航AUV节点比例逐渐增加时，2种算法的定位误差都随着领航节点比例增加而减小，但本文算法的定位误差明显小于常规算法，常规算法的平均定位误差由0.0114下降至0.0030，而本文算法的平均定位误差由0.0052下降至0.0018；本文算法不可定位节点数接近0，常规算法在领航AUV比例较小时，不可定位节点数较多。在领航AUV比例较小的情况下，本文的算法在定位精度和定位范围上优势更为明显。

2）AUV节点通信半径的影响

图7为通信半径对本文算法与常规算法的平均定位误差影响图，图8为不可定位节点数对比图。

可以看出，随着通信半径从的增大，本文算法的平均定位误差始终优于常规算法；本文算法所有节点均可定位，常规算法不可定位节点数逐渐减少并趋于0。因此，本文算法在提高定位精度的同时，可以减小AUV节点的通信距离，减小节点能量消耗。

3）节点总数的影响

图9为节点总数对本文算法与常规算法的平均定位误差的影响，图10为不可定位节点数对比图。

可以看出，随着节点总数的增加，领航节点的数增多，待定位节点收到的参考节点的广播信息增多，潜在位置估计次数上升，平均定位误差逐渐降低。在节点数小于200时，虽然常规算法平均误差小于本文算法，但是本文算法几乎可以定位网络内所有节点；常规算法在节点总数较少时，不可定位节点数量较多；本文算法在节点总数达到200左右，定位误差趋于稳定。

综上仿真结果表明，本文ARW-UWASN算法与常规UWASN算法相比，在上述3种影响因素下，本文算法均具有较高的定位精度和较好的定位覆盖率，节点网络内定位精度平均可提高约30%。

5 结　语

本文提出一种基于自适应残差加权的分布式协同定位算法（ARW-UWASN），解决了常规算法如果领航AUV节点密度太小，部分待定位普通节点无法接收到领航节点信号，导致部分待定位节点始终无法定位的问题。本文采用逐级定位解决领航AUV节点密度稀疏导致无法定位的问题，同时采用自适应残差加权算法，结合参考节点位置信息实现对待定位节点位置的加权估计。仿真结果表明，本文算法与常规算法相比，减小了节点能量消耗，有效提高了定位精度和定位覆盖率。