0 引言

根据水声定位所依据信息的不同，现有的水声无源定位方法可以分为 2 类。第 1 类是基于信号到达方向（Direction of Arrival，DOA）的定位方法，第 2 类是基于信号到达时间（Time of Arrival，TOA） 的定位方法，TOA 定位方法要求接收机和发射机精确的时间同步；若信号收发机时间不同步，此时可使用基于信号到达时间差（Time Difference of Arrival，TDOA）的定位方法，基站间需要进行精确的时间同步。根据观测站数量的不同，无源定位可分为单站无源定位和多站无源定位^[1]。在单站无源定位和多站无源定位中，基站的位置分布极大地影响了定位精度^[2]。因此，如何规划机动观测平台的路径，实现定位节点的合理分布，对于提高目标定位精度至关重要。

无源定位中常用的衡量定位精度的指标有圆概率误差^[3]、费歇尔信息矩阵（Fisher Information matrix，FIM）^[4]、克拉美罗下界（Cramer-Rao Lower Bound，CRLB）和几何精度因子（Geometric Dilution of Precision，GDOP）等。众多研究采用以上指标对观测平台的轨迹进行了优化，文献^[5]采用最小化广义克拉美罗下界的迹为优化准则对 AOA 协同定位下多无人机的路径规划进行优化；文献^[6]采用 FIM 行列式的最大值作为目标函数，优化了 TDOA 定位无人机群的轨迹；文献^[7]采用最小化 GDOP 为准则，优化了单测向观测器的运动轨迹； 上述研究均通过选取合适的指标和优化准则，对观测器的轨迹进行了优化并提高了定位精度。

然而，在针对如何提升定位精度，优化观测器轨迹的同时，也应考虑来自障碍物或其他威胁等约束条件的影响。路径规划算法具有在复杂环境中为观测器规划出一条高效避障且威胁小的最优路径或次优路径的优点，常见的路径规划算法有：人工势场法^[8]、动态窗口法（Dynamic Window Approach，DWA）^[9]、A*算法、蚁群算法、遗传算法^[10]和粒子群算法^[11]等。路径规划算法之间不仅可以相互融合，而且还能与实际的任务场景相结合。文献^[12]考虑了 UAV 导航和双基地合成孔径雷达图像分辨率 2 方面问题，提出了一种基于约束自适应多目标差分进化的路径规划算法。路径规划算法中 DWA 算法不仅实现相对简单，而且可以通过改变采样策略、评价函数以及权重系数对算法进行调整和扩展，以适应不同的应用场景； 文献^[13]将障碍物数量因子与方向角变化因子引入评价函数，提高了机器人障碍物效果的同时保障了安全性；文献^[14]提出了基于障碍物信息的速度权重动态调整机制，增强了无人机穿越密集障碍区域能力。

本文首先介绍了单观测平台基于 TDOA 定位方法定位水下目标的基本原理；分析了不同航迹下的定位精度；针对定位精度受航迹影响的问题，提出了一种基于改进 DWA 算法的航路规划方法。引入了以 HDOP 最小化为准则构造的定位精度评价子函数，并使定位精度和航向角评价函数的权重系数可动态调整；以期规划出无碰撞且定位精度较高的航路。

1 定位态势

水下静止目标的定位态势如图1 所示。坐标系满足右手定则，X 轴、Y 轴和 Z 轴分别指向正东、正北和垂直向上；被定位目标位于海底，坐标为 ${\mathit{X}}_{\mathrm{s}}={\left[x_{\mathrm{s}}，y_{\mathrm{s}}，z_{\mathrm{s}}\right]}^{\mathrm{T}}$；单艘观测船按规划航迹在海面上航行，接收由声信标发射的周期声信号。设被定位目标的声脉冲的初始发射时刻为$t_0$，周期为$t_{\mathrm{s}}$，则信号的到达时间可以表示为

式中：n 为周期号；$t_n$为第 n 个周期声信号的到达时间；$d_n$为$t_n$时刻观测船到目标的距离，$d_n=∥{\mathit{X}}_n-{\mathit{X}}_{\mathrm{s}}∥;{\mathit{X}}_n={\left[x_n，y_n，z_n\right]}^{\mathrm{T}}$为$t_n$时刻观测船的坐标，由 GPS 测得；c 为水中声速。

2 TDOA 定位方法与精度分析

2.1 TDOA 定位方法基本原理

观测船对目标的 TDOA 观测方程可写为

式中：${\widehat{t}}_{ij}$为第$n_i$个声信号与第$n_j$个声信号的 TDOA 测量值；$e_{ij}$为 TDOA 测量误差。

如图2 所示，观测船在定位过程中，会接收若干个周期声信号，因此在已驶航路上会存在若干个定位节点，引入$N\times 1$矩阵（集合$\mathrm{\Gamma }$包含 N 个可用的定位节点）

$\begin{array}{c}\widehat{\mathit{t}}={\left[\begin{array}{c}{\widehat{t}}_{ij}\\ ⋮\end{array}\right]}_{(i,j)\in \Gamma },N={\left[\begin{array}{c}{n}_i-n_j\\ ⋮\end{array}\right]}_{(i,j)\in \Gamma },\\ d={\left[\begin{array}{c}{d}_{n_i}-d_{n_j}\\ ⋮\end{array}\right]}_{(i,j)\in \Gamma },e={\left[\begin{array}{c}{e}_{ij}\\ ⋮\end{array}\right]}_{(i,j)\in \Gamma }\end{array}$

此时，基于 TDOA 定位方法的数学模型写为

基于 TDOA 的定位问题实际上就是对式（3）方程组的求解问题。如图2 所示，不利用冗余定位节点，至少需要在可用定位节点集合中选取出 3 个定位解算节点，构成双曲线交汇模型才可以解算出目标的水平坐标^[15]，此时对目标的定位转化为对以下方程组的求解

2.2 TDOA 定位精度分析

引入水平精度因子 HDOP 作为评价定位精度的指标，HDOP 的值越小，定位精度越高^[16]。HDOP 的表达式为

式中：$\mathrm{\Delta }\mathit{r}={\left[\mathrm{\Delta }{x}_{\mathrm{s}}，\mathrm{\Delta }{y}_{\mathrm{s}}\right]}^{\mathrm{T}}$表示定位解算结果与目标真实位置的偏差。

设${\mathit{D}}_x=E\left(\mathrm{\Delta }\mathit{r}\mathrm{\Delta }{\mathit{r}}^{\mathrm{T}}\right)$为目标水平坐标测量误差的协方差矩阵， D_x 的计算公式为

式中： D_c、 D_t、 D_z 和 D_GPS分别是声速测量误差、到达时间测量误差、目标深度测量误差和观测船水平坐标测量误差的协方差矩阵；M_x、M_c、M_t、M_z 和 M_GPS 分别是$\mathit{F}={\left[f_1，f_2\right]}^{\mathrm{T}}$关于变量$\left[x_{\mathrm{s}}，y_{\mathrm{s}}\right]、c、\left[t_1，t_2，t_3\right]、z_{\mathrm{s}}$和$\left[x_1，y_1，x_2，y_2，x_3，y_3\right]$的雅可比矩阵。假设各测量误差均是相互独立且服从零均值高斯分布的随机变量，则上述协方差矩阵和雅可比矩阵的表达式为

$\begin{array}{c}{\mathit{D}}_c={\sigma }_c^2,{\mathit{D}}_t=diag\left({\sigma }_t^2,{\sigma }_t^2,{\sigma }_t^2\right),{\mathit{D}}_z={\sigma }_z^2,\\ {\mathit{D}}_{\mathrm{G}\mathrm{P}\mathrm{S}}=diag\left({{\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}}^2,{\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}{}^2,{\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}{}^2,{\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}{}^2,{\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}{}^2,{\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}{}^2\right),\\ {\mathit{M}}_x=\frac{\partial \left(f_1,f_2\right)}{\partial \left(x_{\mathrm{s}},y_{\mathrm{s}}\right)},{\mathit{M}}_c=\frac{\partial \left(f_1,f_2\right)}{\partial c},{\mathit{M}}_t=\frac{\partial \left(f_1,f_2\right)}{\partial \left(t_1,t_2\right)},\\ {\mathit{M}}_z=\frac{\partial \left(f_1,f_2\right)}{\partial z_{\mathrm{s}}},{\mathit{M}}_{\mathrm{G}\mathrm{P}\mathrm{S}}=\frac{\partial \left(f_1,f_2\right)}{\partial \left(x_1,y_1,x_2,y_2,x_3,y_3\right)}\end{array}$

式中：$\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}（\bullet ）$ 表示对角矩阵；${\sigma }_c，{\sigma }_t，{\sigma }_z，{\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}$分别表示声速、信号到达时间、目标深度和观测船水平坐标测量的均方根误差。

对 2 种航路下 TDOA 法的定位精度进行分析，参数设置如下：声速为 1 500 m/s；目标位于以原点为中心的 400 m$\times$400 m 区域内，深度为 50 m；${\sigma }_c，{\sigma }_t，{\sigma }_z$和${\sigma }_{\mathrm{G}\mathrm{P}\mathrm{S}}$分别为 1.5 m/s、1 ms、1 m 和 2 m。2 种航路下的定位精度分布情况如图3 所示，其中灰线为观测船的航路，红点为在航路上选取的定位解算节点，定位解算节点从两段距离相同的航路上被等长选取。由图3 可知，航路 1 对目标区域的平均定位精度（3.16 m）比航路 2 的平均定位精度（6.95 m） 高；但是航路 1 的盲区（黄色区域所示）比航路 2 要大。根据 2 条航路下定位精度分布的特点，航路 1 和航路 2 分别对应已经确定目标大致区域后的精确定位阶段和未确定目标大致区域时的粗略定位阶段。定位节点包围目标并呈等角分布是 TDOA 定位的最优定位节点分布形式^[17]。如图3（a）所示，航路 1 的圆形航路下，更容易选取包围目标且接近等角分布的定位节点。因此，在精确定位阶段，观测船应尽可能地航行在环绕目标的圆形航路下。

由定位精度分析可知，定位精度不仅受测量误差的影响，而且与定位节点的位置分布密切相关。由图2 可知，观测船的航路在很大程度上影响了定位节点的分布。因此，为提升定位精度，有必要对观测船的航路进行规划。

3 航路规划方法

3.1 传统 DWA 算法原理

DWA 算法包含了 3 部分：1）根据自身动力学性能和环境障碍物的限制生成动态窗口并进行速度采样；2）基于运动学模型和采样的速度进行轨迹预测；3）根据评价函数对预测轨迹进行评估。

观测船在水面上执行定位任务时，速度采样空间所受的限制如下。

1）观测船最小最大航速、转向速度限制。

式中：$v_{min}$和$v_{max}$为观测船的最小航行速度和最大航行速度；${\omega }_{min}$和${\omega }_{max}$为观测船的最小转向速度和最大转向速度。

2）观测船最小最大航行加速度、转向加速度限制。

式中：$v_a$和${\omega }_a$分别为观测船当前航行速度和转向速度；$\dot{v}$和$\dot{\omega }$分别是观测船的最大航行加速度和最大转向加速度。

3）环境障碍物限制。

式中：distance$（v，\omega ）$为预测航迹到最近障碍物的距离，保证了当前速度下观测船可在碰到障碍物前停止。

动态窗口的速度采样空间V_r 即为上述 3 个集合的交集，满足

在每个离散时间间隔$\mathrm{\Delta }t$内，对观测船的运动轨迹做近似化处理，看作是直线；则观测船的运动学模型可写为

式中：$x_k$，$y_k$和${\theta }_k$分别为观测船 k 时刻的 X 轴坐标、Y 轴坐标和航向角；$v_k$和${\omega }_k$为观测船 k 时刻的航行速度大小和转向速度。

DWA 算法在速度采样空间V_r 中离散化地选择可行的航行速度和转向速度，基于观测船的运动学模型，就可向前预测出一段时间T 后的航迹。航迹的预测如图4 所示，绿色实线表示预测航迹。

对于每一条预测航迹，通过评价函数来评估其优劣程度。最终，将最优预测航迹对应的航行速度和转向速度作为控制量，控制观测船航行并执行任务。传统 DWA 算法的评价函数为

式中： heading$（v，\omega ）$为航向角评价函数，$\text{heading}（v，\omega ）=\pi -\theta ，\theta$为观测船在预测航迹末端时的航向和期望航向之间的角度偏差，期望航向为由预测航迹末端指向目标点的方向； dist$（v，\omega ）$为避障评价函数，$\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}（v，\omega ）=\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{t}\mathrm{a}\mathrm{n}\mathrm{c}\mathrm{e}（v，\omega ）$，评估预测航迹到最近障碍物的距离，当观测船与障碍物之间的距离超过一定值时，设置为常数；velocity$（v，\omega ）$为速度评价函数，$\mathrm{v}\mathrm{e}\mathrm{l}\mathrm{o}\mathrm{c}\mathrm{i}\mathrm{t}\mathrm{y}（v，\omega ）=v$，评估预测航迹对应的速度大小；$\alpha ，\beta ，\gamma$分别为各评价子函数的权重系数；$\sigma$表示对各评价子函数进行归一化。

3.2 改进 DWA 算法

传统 DWA 算法只考虑了如何在躲避障碍物的同时快速地抵达目标，并未考虑定位精度问题。因此，本文在传统 DWA 算法的评价函数的基础上，引入定位精度评价子函数，以提升对目标的定位精度。

如图5 所示，通常情况下，从历史航迹中选取 1 号定位解算节点和 2 号定位解算节点，当前时刻选取为 3 号定位解算节点，通过式（1）对目标进行实时定位解算，设 1 号和 2 号定位解算节点的坐标分别为 X_h，1 和 X_h，2 。由式（11）向前预测某条航迹的同时，可根据声信号发射周期 $t_{\mathrm{s}}$，推算出观测船在该条预测航迹上接收到未来 M 个声信号的大致位置$\left(M\times t_{\mathrm{s}}\le T\right)$，并将此 M 个点作为预测定位解算节点（如图5 中红色圆点所示），设该条预测航迹上第 j 个预测定位解算节点的坐标为 ${\mathit{X}}_{p，j}，j\in [1，M]$，则该条预测航迹上的预测定位解算节点坐标集为，$\left\{{\mathit{X}}_{p，1}，{\mathit{X}}_{p，2}，\cdots ，{\mathit{X}}_{p，M}\right\}$。

本文以 HDOP 最小化为优化准则，对预测航迹进行针对定位精度的评价。由 HDOP 的计算式可知，在测量误差已知或已假设的条件下，HDOP 是只与目标和 3 个定位解算节点的坐标有关的函数，但是在定位问题中目标的真实坐标 X_s 始终未知，因此计算 HDOP 时，由定位解算结果${\widehat{\mathit{X}}}_{\mathrm{s}}$替代。HDOP 值越大，定位精度越低，定位精度评价函数值与 HDOP 值呈负相关。因此，定义该条预测航迹的定位精度评价函数为

式中：$\overline{\mathrm{H}\mathrm{D}\mathrm{O}\mathrm{P}}（v，\omega ）$表示该条预测航迹的平均定位精度值，hdop$（v，\omega ）$的引入使得观测船更倾向于选择定位精度较高的航迹。

整个过程采用边定位目标边规划航路的策略，在定位初期阶段，观测船距离目标位置较远，定位误差较大，定位解算结果 ${\widehat{\mathit{X}}}_{\mathrm{s}}$不准确， hdop$（v，\omega ）$的可靠性差，观测船应尽量快地抵达目标所在区域。随着观测船与目标间距离的减小，信噪比增大，时延估计标准差${\sigma }_t$反比于信噪比^[18]，声呐对信号到达时间 t_n 的测量精度提高，定位结果${\widehat{\mathit{X}}}_{\mathrm{s}}$逐渐趋于真值 X_s，计算出的 hdop$（v，\omega ）$逐渐可靠。因此，在观测船靠近目标的定位过程中，随着观测船与目标距离的减小，定位精度评价函数的权重应该逐渐增加，航向角评价函数的权重逐渐降低。

如图6 所示，考虑到水下定位所使用的声呐垂直开角范围有限^[19]，为避免观测船过于靠近目标，导致目标进入盲区而丢失信号，需设计一个最小水平距离 r_m。综上所述，改进后的评价函数为

式中：$\lambda$为定位精度评价函数的权重系数； p 为观测船到目标位置的水平距离。

由式（15）可知，在远距离情况下（$p\gg r_m$），$\epsilon$ 的值较小，此时 heading$（v，\omega ）$对评价函数的影响较大，观测船快速地向目标区域靠近。观测船在接近目标的过程中，$\epsilon$的值趋于 1，此时 heading$（v，\omega ）$对评价函数的影响降低，hdop$（v，\omega ）$对评价函数的影响增大，航路的规划更多地取决于 hdop$（v，\omega ）$。

4 仿真验证

通过仿真实验对本文所提算法的有效性进行验证。水下目标的水平坐标设为[–400，–400] m，深度为 70 m，发射周期为 1 s；声速为 1500 m/s；最小水平距离 r_m 为 180 m；观测船起点为[–700，800] m；环境中设置 8 个障碍物，其位置坐标分别为[–200， 0] m，[–200，200] m，[–200，–300] m，[170，–200] m， [70，–200] m，[–400，–700] m，[100，100] m，[200， 200]m，半径分别为 30 m，20 m，30 m，30 m，20 m， 20 m，30 m，40 m；测量误差的参数设置同定位精度分析时一致，仿真参数见表1，其中动力学性能参数参考自文献^[20]。本文进行了 3 种航路下的仿真实验，分别是航路 1：常规匀速直线航路；航路 2：基于传统 DWA 算法规划出的直达目标区域的航路；航路 3：基于改进 DWA 算法规划出的航路。

1）航路 1。

规划的航路如图7（a）所示，其中蓝色为规划的航路，黑色为障碍物，红色星号为目标，蓝色圆点为所选取的 3 个定位节点。为简化处理，仿真中所用的节点选取策略为：当前位置为 3 号定位节点，并往前每隔 100 个周期号选取一个定位节点；定位结果如图7（b）所示；整个定位过程中定位误差如图7（c）所示。此常规匀速直线航路为基于传统 DWA 算法向预设终点引导而规划出的，预设终点的位置为[800，–100] m。

2）航路 2 和航路 3。

图8（a）和图8（d）中前一段蓝色航路与图7（a）中一致，同为基于传统 DWA 算法向预设终点引导而规划出的直线航路。此段蓝色航路期间 TDOA 定位的基线长度随着船的航行而不断增长，定位精度随基线的增长而提升，待基线增长至一定长度，定位解算结果较可靠之后，可得出目标所在大致区域，然后再基于传统 DWA 算法和改进 DWA 算法规划航路（如图8（a）和图8（d）中红色航路所示）。

由图8（c）可知，当观测船直接向目标抵近时，定位误差很大，甚至无法进行定位解算。这是因为 TDOA 定位方法在基线方向上的定位精度很低，尤其当目标处于定位节点包围区域外并和 3 个定位节点的任何 2 个趋于共线时，定位精度变得极差，无法对声源进行有效定位。因此，当使用单观测平台基于 TDOA 定位方法对目标进行定位时，不能使观测船直接向目标所在区域抵近。对比图7 和图8 可以看出，改进 DWA 算法规划航路下的定位结果更接近于目标真实位置，定位误差更小，并且观测船在抵达最小水平距离 r_m 后，在定位精度评价函数的作用下自动选择了定位精度更高的圆形航路绕目标航行。传统 DWA 算法规划的航路 1 在整个定位过程中定位精度稳定，均方根误差为 55.9 m；改进 DWA 算法规划的航路3在整个定位过程中的均方根误差为 31.2 m，后半段的均方根误差为 5.3 m；航路 2 因其无法有效定位，故不讨论其误差。因此，改进 DWA 算法规划的航路不仅有效避开了障碍物，而且由于定位精度评价函数的引入，航路对应的定位精度得以提高。

5 结束语

针对单艘观测船 TDOA 定位水底静止目标的问题，本文提出了一种基于改进 DWA 算法的航路规划方法。首先，结合 DWA 算法向前预测的特点，设计了一种定位精度评价子函数，提升了定位精度。其次，根据观测船与目标的距离关系，平衡了航向角评价子函数和定位精度评价子函数的权重系数，使航路规划方法更加贴合定位任务。最后，通过仿真实验对比了传统 DWA 算法和改进 DWA 算法，证明了基于改进 DWA 算法所规划出的航路相比传统算法的定位精度有显著提高。

参考文献