基于泰勒级数展开的总体最小二乘水下短基线定位算法研究

侯 华，黄鼎盛，郭胜杰，程 萌

（河北工程大学 信息与电气工程学院，河北 邯郸 056038）

迄今为止，在水下环境中，声波仍是完成水下定位最常用的载体［1］。按照基线长度的不同，水下声学定位系统可分为长基线（Long Baseline，LBL）定位系统、短基线（Short Baseline，SBL）定位系统以及超短基线（Ultra-short Baseline，USBL）定位系统3 类［2］。短基线定位系统价格低廉、安装便捷且定位精度高［3］，是中小型自主式水下导航器（Autonomous Underwater Vehicle，AUV）定位的首选方案。

在短基线定位系统中，需要通过测量声波信号从换能器到达水听器阵列中不同阵元之间的时间差，得到换能器与水听器基阵之间的距离和方位信息，再对这些测距信息进行分析解算，最后根据己知的换能器或是水听器坐标得到待定位目标的具体位置而完成定位［4］。在水环境中，引起AUV定位误差的原因主要有：（1）水声信号在水下传播时声速测量的偏差；（2）多径效应的存在导致信号在传播时产生衰落，使得信号传播时延出现偏差；（3）基阵自身位置的扰动会使定位出现误差等［5］。此外，定位算法的不同更是直接影响到AUV 的定位结果。因此，对水下短基线定位系统中算法的研究十分重要。

Chan 氏定位算法在定位时要求噪声服从高斯分布，因此它在对AUV 定位时适用性不足［6］。最小二乘定位算法计算量小、定位快速，但是精度不高、鲁棒性较差［7］。人工蜂群算法定位精度较高，但无法满足定位的实时性要求［8］。因此，文献［9］基于牛顿迭代对最小二乘定位法进行改进，提出了一种改进最小二乘法的水下测距与定位方法，明显改善了定位精度。文献［10］将多水听器距离值进行信息融合，提出了一种基于卡尔曼滤波的多水听器水下定位算法。文献［11］提出了一种基于并行蚁群算法的长基线定位方法。该方法将水声目标定位问题简化为带约束条件的非线性优化问题，降低了各个误差源对水声目标导航定位精度的影响。考虑到AUV 定位算法的绿色和实时性需求，本文针对浅水环境提出了一种基于泰勒级数展开的总体最小二乘定位算法。与人工蜂群算法相比，该算法的复杂度较低。与最小二乘算法相比，该算法能够大幅度提升定位精度，且稳定性较强。最后，对本文算法进行实验仿真，并与最小二乘算法进行了对比分析。

1 系统设计方案

设计的短基线定位系统包括水面部分和水下部分。水面部分由GPS 接收机、数据处理中心、5G 收发器以及水听器基阵组成，主要完成水声定位信号的接收与定位算法的解算等工作。水下部分安装在水下AUV 上，主要任务是完成水声信号的调制与发射，由扩频信号发生仪、数据处理中心、驱动控制中心以及发射换能器组成，且与AUV 时间同步。基阵由4 个水听器和1 个全向发射换能器组成，布放形式为岸基布放。4 个水听器用于接收待定位目标发射的水声信号，且其与水面浮标相连接，按照一定阵型摆放。由于浮标位置是已知的，故水听器坐标位置也是已知的。全向发射换能器被安装在AUV 上，用来发射定位信号。系统通过测量水声信号在AUV 与各水听器间的传播时延差计算其坐标信息。

2 泰勒级数展开的总体最小二乘定位算法原理

在浅水环境下，声速存在的梯度变化可以忽略［12］。本文认为，声波在水中的传播速率为恒定速率1 500 m/s。定位过程中，换能器发射的水声信号经传播到达水听器基阵的时间差可以用广义互相关时延估计算法得到。因此，在短基线定位系统中，可以用距离差代替时间差。待定位AUV 与4 个水听器所在的浮标阵距离可以表示为：

其中，第i个浮标的坐标为(xi,yi,zi)，待定位水下AUV 的位置为(x,y,z)，ti为从AUV 上的发射换能器发射声波经传播达到第i个水听器所在的浮标阵的时间，c为声波传播的速度。由定位空间方程求差处理得到线性方程：

总体最小二乘法的思想在于同时利用扰动矩阵和扰动向量调整A和B，以校正二者内存在的噪声，构造增广矩阵C=[-B|A]，利用增广矩阵的奇异值分解求得最小二乘解。当同时存在阵元位置误差和测距误差时，rank{C}=4，因此对其进行奇异值分解得：

式中：U、V分别为增广矩阵的左、右奇异矩阵；Σ是半正定对角矩阵；uk和vk分别为左、右奇异向量；uk为矩阵U的第k列；vk为矩阵V的第k列；σk为C对应的奇异值，并满足σ1≥σ2≥σ3≥σ4。当没有测量误差时，则有rank{C}=3，此时σ4=0，且Ax=B存在唯一解；当测量误差存在时，则有rank{C}=4，此时σ4≠0。但是，当测量误差不是很大时，Ax=B存在唯一的总体最小二乘算法的解为：

将此总体最小二乘估计的待定位AUV 位置作为初值(x0,y0,z0)，在初值点进行泰勒级数展开，并采用多次加权最小二乘迭代运算完成目标的定位。

令：

对式（10）在(x0,y0,z0)处进行泰勒级数展开，忽略高阶项，则有：

则一阶项Un可以表示为：

初值(x0,y0,z0)为总体最小二乘估计值，则通过AUV 到水听器阵的测量距离Rn即可求出(Δx,Δy,Δz)。根据给出的门限η判断式（14）是否成立：

若满足条件，则迭代结束；若不满足条件，则将(x0+Δx,y0+Δy,z0+Δz)作为新的初始坐标进行计算，直到满足迭代条件，则坐标为AUV 定位的结果。

3 仿真实验与性能分析

本文的短基线定位系统环境设置如下：水听器阵的4 个浮标阵元的坐标为（464，241，75）、（628，347，118）、（726，532，132）和（948，682，154），待定位的水下AUV 的位置设置为（754，153，200）。在不同测距条件下，通过1 000 次蒙特卡洛仿真实验，对本文提出的算法与最小二乘算法进行定位误差比对，结果如图1 所示。

从图1 可以看出，随着AUV 与水听器基阵测距误差的增大，最小二乘定位算法的误差曲线增长较为急促，而本文算法的误差曲线增长平缓。这是因为本文算法在构造最小二乘解时不但考虑了测距误差，还考虑了基阵自身的扰动误差，故在测距误差相同的条件下，定位误差后者始终优于前者。显然，泰勒级数展开的总体最小二乘定位算法较最小二乘定位算法有较高的定位精度。

在两种不同定位算法下对AUV 进行实时运动轨迹定位仿真。AUV 从初始位置（754，153，200）出发，在水下进行匀速直线运动。图2 为AUV在二维空间下最小二乘定位算法与本文提出算法的实时定位轨迹及预设轨迹图。

由图2 可知，在AUV 二维空间匀速直线运动时，最小二乘算法的定位轨迹随着时间的变化在AUV 运动附近上下波动较大，而本文提出的算法定位轨迹在AUV 运动附近波动较小，曲线较为稳定，轨迹更加平稳，实时定位精度较最小二乘算法好。这是由于本文算法在求解非线性方程组时考虑了基阵的扰动误差，并构造误差的增广矩阵进行奇异值分解，大大提高了对AUV 的实时定位精度。此外，泰勒级数展开的思想使得定位结果在初始值进行迭代优化，增强了实时定位的稳定性。显然，在AUV 进行匀速直线运动时，本文算法对AUV 的实时定位性能强且定位轨迹波动较小，在提高最小二乘定位精度的同时具有较强的鲁棒性。

4 结语

本文提出的基于泰勒级数展开的总体最小二乘定位算法不依赖于水压计、罗经以及DVL 等复杂设备和AUV 的先验信息，在工程应用中对中小型AUV 的定位适应性较好。仿真实验表明，本文算法精度较高。在AUV 实时定位中，该算法定位轨迹较为平稳，总体上满足AUV 在浅水中实现高精度实时定位的应用需求。本文提出的算法为提升水声定位系统的精度提供了一种有效途径，今后还需进一步开展不同环境下的AUV 深水实验来提高定位算法的可靠性和精确性。