0 引　言

在科技时代飞速发展的今天，水下作业领域面临着复杂且十分具有挑战性的任务。水下机器人作为执行水下任务的关键工具，对于其末端控制器的精准跟踪显得更加迫切。由于水下环境特殊且恶劣，存在光线恶劣和水流湍急等，给水下作业带来十分大的阻碍。传统的水下作业方式，例如潜水员直接作业，不仅具有比较高的危险指数，同时还会受到人体生理的限制，造成作业的长度和深度都十分有限，无法满足日益增长的深海探索、海洋资源开发、水下救援以及水下基础设施建设等需求。为此，深入研究水下机器人末端执行器位置动态跟踪方法^{[1 − 2]}，对于推动水下作业技术的发展，拓展人类对海洋资源的利用以及保障水下作业的安全性都具有极为重要的意义。

近期，相关研究者对水下机器人末端执行器位置动态跟踪^{[3 − 4]}方面的内容展开大量研究，并且取得了较为显著的研究成果。韩勇强等^[5]在传统YOLOv3检测网络的基础上，构建扩维滤波器，进而展开匹配和跟踪处理，有效实现目标跟踪。对于高速运动的水下目标，存在延迟，进而导致最终的跟踪结果不够精准。闫景昊等^[6]应用最小二乘法组建自适应预测控制器，通过拉盖尔函数重构控制器，根据控制器最终有效实现轨迹跟踪。在系统参数出现微小变化时，控制器会作出比较大的调整，进而造成系统不稳定，严重影响最终的跟踪结果。王平波等^[7]主要通过改进的AIMM-UKF算法完成水下目标跟踪。在水下环境比较复杂的情况下，无法精准对目标展开跟踪处理。Song等^[8]主要应用PF-RAE-TBD方法完成跟踪处理。在水下环境存在干扰因素的情况下，无法对目标展开精准跟踪。

结合上述方法，提出一种水下机器人末端执行器位置动态跟踪方法。

1 水下机器人末端执行器位置动态跟踪方法 1.1 水下机器人末端执行器位置在线定位

在水下机器人末端执行器位置动态跟踪前期，优先展开在线定位处理，主要通过TDOA方法和DOA方法实现。

1）TDOA方法

设定水下无线传感器阵列网络是由$ n $个声呐节点构成。设定目标$ h $到第$ i $个节点$ {h_i} $的距离为$ {D_i} $，则对应的计算式为：

$ {D_i} = \sqrt {{{\left( {x - {x_i}} \right)}^2} + {{\left( {y - {y_i}} \right)}^2} + {{\left( {z - {z_i}} \right)}^2}} 。$

(1)

式中：$ \left( {x,y,z} \right) $为目标$ h $位于水下任意一点；$ ({x_i},{y_i},{z_i}) $为节点$ {h_i} $的坐标。

所有水下传感器均采用被动探测模式运行，若以$ {h_1} $节点检测到信号的时间作为基准，则可通过广义互相关算法等时延估计方法，计算出各节点$ {h_i} $相对于节点$ {h_1} $的时间偏移量，即时延差$ {\phi _{1i}} $，据此可得：

$ {D_i} = \left\{ \begin{gathered} \Delta {D_1} + \Delta {D_{1i}}，\\ {D_1} + c{\phi _{1i}}。\\ \end{gathered} \right. $

(2)

式中：$ \Delta {D_{1i}} $为$ {D_i} $和$ {D_1} $之间的距离差值；$ c $为水中的声速。依据上述分析，获取如下方程组：

$ \left\{ \begin{gathered} {\left( {x - {x_1}} \right)^2} + {\left( {y - {y_1}} \right)^2} + {\left( {z - {z_1}} \right)^2} = D_1^2，\\ {\left( {x - {x_2}} \right)^2} + {\left( {y - {y_2}} \right)^2} + {\left( {z - {z_2}} \right)^2} = {\left( {{D_1} + \Delta {D_{12}}} \right)^2}，\\ \;\;\;\;\;\;\;\;\;\;\;\;\; \vdots \\ {\left( {x - {x_n}} \right)^2} + {\left( {y - {y_n}} \right)^2} + {\left( {z - {z_n}} \right)^2} = {\left( {{D_1} + \Delta {D_{1n}}} \right)^2}。\\ \end{gathered} \right. $

(3)

可知，当获取声呐阵列的空间坐标$ \left( {{x_i},{y_i},{z_i}} \right) $及各节点间的时延差$ {\phi _{1i}} $后，其空间坐标可通过全球定位系统确定，而时延参数则可通过信号处理技术估算获得。这些参数经由无线通信链路传输至水下航行器的控制系统展开实时解算，最终确定水下机器人末端执行器的空间位置。对式(3)展开化简，则可以得到：

$ \boldsymbol{R}\omega=\boldsymbol{E}。$

(4)

式中：$ {\boldsymbol{R}} $为系数矩阵；$ \omega $为各个元素对应声呐节点对的测量值；$ {\boldsymbol{E}} $为声呐节点对的测量值，即观测向量。但是式(4)给出的方程组为矛盾方程组，其精确解$ \omega $不存在，但是可以获取其在某种意义下的近似解。因此，将最小的残差平方和作为优化依据，计算式为：

$ \min \left\| \boldsymbol{R}\omega-\boldsymbol{E} \right\| _2^2。$

(5)

引入最小二乘原理^{[9 − 10]}，采用式(6)得到$ \omega $的近似解$ \tilde \omega $：

$ \tilde{\boldsymbol{\omega}}=\left(\boldsymbol{R}^{\mathrm{T}}\boldsymbol{R}\right)^{-1}\boldsymbol{R}^{\mathrm{T}}\boldsymbol{E}。$

(6)

式中：$ \mathrm{T} $为共轭转置。式(6)即使在多节点的情况下，仍然适用。

2）DOA方法

在分布式无线传感网络中，方位参数与时间差参数构成核心测量值，多节点测向交汇是典型的定位技术。考虑系统包含$ n $个阵列单元，若节点$ {h_i} $测得目标的方位角和俯仰角，则目标角度信息与阵列单元空间坐标的约束关系可描述为：

$ \left\{ \begin{gathered} \tan {\alpha _i} = \frac{{y - {y_i}}}{{x - {x_i}}}，\\ \tan {\theta _i} = \frac{{\sqrt {{{\left( {y - {y_i}} \right)}^2} + {{\left( {x - {x_i}} \right)}^2}} }}{{z - {z_i}}}。\\ \end{gathered} \right. $

(7)

经过变形，式(7)等价于以下线性方程组：

$ \left\{ \begin{gathered} x\sin {\alpha _i} - y\cos {\alpha _i} = {x_i}\sin {\alpha _i} - {y_i}\cos {\alpha _i}，\\ x\left| {\sec {\alpha _i}} \right| - z\tan {\theta _i} = {x_i}\left| {\sec {\alpha _i}} \right| = {z_i}\tan {\theta _i},{\alpha _i} \in \left( { - \frac{{\text{π}} }{2},\frac{{\text{π}} }{2}} \right)。\\ \end{gathered} \right. $

(8)

将式(8)表示为：

$ \boldsymbol{I}p=N。$

(9)

式中：$ \boldsymbol{I} $为线性系数矩阵；$ p $为目标点坐标；$ N $为每个元素对应一个阵列节点的测量值。

在方位角$ {\alpha _i} $和俯仰角$ {\theta _i} $对应的估计值存在误差的情况下，将目标位置在最小二乘意义下的解表示为：

$ \tilde {\boldsymbol{\omega}} = {\left( {{{\boldsymbol{I}}^{{\rm{T}}}}{\boldsymbol{I}}} \right)^{ - 1}}{{\boldsymbol{I}}^{{\rm{T}}}}{\boldsymbol{I}}N。$

(10)

分析式(4)和式(9)所表示的方程组，由于矩阵中$ \omega $和$ p $的待解元素存在显著差异，造成2个方程组的合并存在一定难度。因此，对式(9)展开推广，获取同解的方程组，将其表示为：

$ \overset{\scriptscriptstyle\frown}{{\boldsymbol{I}}} \overset{\scriptscriptstyle\frown}{p} = N。$

(11)

式中：$ \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{{\boldsymbol{I}}} $和$ \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{p} $分别为变换处理后的系数矩阵和目标点坐标。

将式(4)和式(11)两者联立，进而获取以下矩阵：

$ {\left[ \begin{gathered} {\boldsymbol{R}} \\ {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{{\boldsymbol{I}}} } \\ \end{gathered} \right]_{\left( {3n - 1} \right) \times 4}} \cdot \omega = {\left[ \begin{gathered} {\boldsymbol{E}} \\ N \\ \end{gathered} \right]_{\left( {3n - 1} \right) \times 1}}。$

(12)

应用最小二乘原理对式(12)展开求解，则可以获取水下机器人末端执行器位置在线定位结果$ \tilde \omega \left( {x,y,z} \right) $，对应的计算式为：

$ \tilde \omega \left( {x,y,z} \right) = \left\| {{{\left( {{{\boldsymbol{R}}^{{\rm{T}}}}{\boldsymbol{R}} + {{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{{\boldsymbol{I}}} }^{{\rm{T}}}}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{{\boldsymbol{I}}} } \right)}^{ - 1}}\left( {{{\boldsymbol{R}}^{{\rm{T}}}}{\boldsymbol{E}} + {{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{{\boldsymbol{I}}} }^{{\rm{T}}}}N} \right)} \right\|。$

(13)

1.2 水下机器人末端执行器位置动态跟踪

完成水下机器人末端执行器位置在线定位处理后，通过改进的高斯混合概率假设密度滤波器跟踪各个子带输出的水下机器人末端执行器位置估计结果，相同目标在不同子带的更新结果应用广义协方差交集准则展开加权融合处理，详细的操作步骤为：

1）将$ t $时刻水下机器人末端执行器目标的状态向量表示为$ \boldsymbol{x}_t=\left[\beta_t,\dot{\beta}_t\right] $，其中，$ {\beta _t} $和$ {\dot \beta _t} $分别为方位和方位变化率。机器人末端执行器的运动模型应用式(14)所示的递归高斯模型为：

$ {{\boldsymbol{x}}_t} = {{\boldsymbol{E}}_{t - 1}}{x_{t - 1}} + {m_{t - 1}}。$

(14)

式中：$ {{\boldsymbol{E}}_{t - 1}} $为状态转移矩阵；$ {m_{t - 1}} $为均值为0和方差为$ {s_{t - 1}} $的高斯白噪声。其中，$ {E_{t - 1}} $和$ {s_{t - 1}} $对应的计算式为：

$ {{\boldsymbol{E}}_{t - 1}} = \left[ \begin{gathered} 1\;\;\;{t_a} \\ 0\;\;\;1 \\ \end{gathered} \right],{s_{t - 1}} = {\sigma ^2}\left[ \begin{gathered} {{\;t_a^4} \mathord{\left/ {\vphantom {{\;t_a^4} {4\;\;\;{{\;t_a^3} \mathord{\left/ {\vphantom {{\;t_a^3} 2}} \right. } 2}}}} \right. } {4\;\;\;{{\;t_a^3} \mathord{\left/ {\vphantom {{\;t_a^3} 2}} \right. } 2}}} \\ {{\;t_a^3} \mathord{\left/ {\vphantom {{\;t_a^3} {4\;\;\;\;\;\;t_a^2}}} \right. } {4\;\;\;\;\;\;t_a^2}} \\ \end{gathered} \right]。$

(15)

式中：$ {t_a} $为采样间隔；$ {\sigma ^2} $为噪声的标准差。

将子带水下机器人末端执行器位置估计结果作为跟踪方法的输入，则能够将量测模型建立为线性高斯模型，如下：

$ {\beta _t} = \left\| {{{\boldsymbol{J}}_t}{x_t} + {m_t}} \right\|。$

(16)

式中：$ {{\boldsymbol{J}}_t} $为量测矩阵。

2）展开预测处理，预测过程主要包含预测存活目标和新生目标。

1）预测存活目标

将$ t - 1 $时刻存活目标的强度函数$ {r_{t - 1}} $和标签集$ {L_{t - 1}} $分别表示为：

$ \left\{ \begin{gathered} {r_{t - 1}} = \sum\limits_{j = 1}^{{R_{t - 1}}} {{w_t}U\left( {x;b_{t - 1}^{\left( j \right)},c_{t - 1}^{\left( j \right)}} \right)}，\\ {L_{t - 1}} = \left\{ {{l_1},{l_2}, \cdots ,{l_{t - 1}}} \right\}。\\ \end{gathered} \right. $

(17)

式中：$ {R_{t - 1}} $为目标数；$ {w_t} $为权值；$ U\left( {x;b_{t - 1}^{\left( j \right)},B_{t - 1}^{\left( j \right)}} \right) $为均值为$ b_{t - 1}^{\left( j \right)} $方差为$ B_{t - 1}^{\left( j \right)} $的高斯分布；$ b_{t - 1}^{\left( j \right)} $和$ B_{t - 1}^{\left( j \right)} $分别为均值和方差。根据$ t - 1 $时刻存活目标的强度函数$ {r_{t - 1}} $获取$ t $时刻预测的存活目标强度函数$ {r_{t|t - 1}} $，对应的计算式为：

$ {r_{t|t - 1}} = \sum\limits_{j = 1}^{{R_{t - 1}}} {{w_{t|t - 1}}U\left( {x;b_{t|t - 1}^{\left( j \right)},B_{t|t - 1}^{\left( j \right)}} \right)}。$

(18)

2）预测新生目标

对于水下机器人末端执行器位置动态跟踪而言，应用常规的高斯混合概率假设密度滤波器展开目标跟踪^{[11 − 12]}，会出现跟踪性能严重退化的目的。因此，将密度聚类算法嵌入到高斯混合概率假设密度滤波器中，进而有效实现自适应估计新生目标强度。基于上述分析，应用基于密度的聚类算法对子带水下机器人末端执行器位置估计结果展开聚类处理，划分到相同簇的测量值则判定为来自新生目标。通过式(19)估计$ t $时刻的新生目标强度函数$ {\rho _t} $：

$ {\rho _t} = \sum\limits_{j = 1}^{{R_{\rho ,t}}} {w_{{\rho _t}}^{\left( i \right)}U\left( {x;b_{{\rho _t}}^{\left( j \right)},B_{{\rho _t}}^{\left( j \right)}} \right)}。$

(19)

将新生目标的标签集$ {L_{{\rho _t}}} $表示为：

$ {L_{{\rho _t}}} = \left\{ {{l_{{\rho _t},1}},{l_{{\rho _t},2}}, \cdots ,{l_{{\rho _t},t}}} \right\}。$

(20)

基于上述分析，得到预测的目标强度函数$ {r_{t|t - 1}} = {r_{t|t - 1}} + {\rho _t} $，进而获取式(20)所示的预测标签集$ {L_{t|t - 1}} $：

$ {L_{t|t - 1}} = {L_{t|t - 1}} \cup {L_{\rho |t - 1}}。$

(21)

3）完成上述操作后，对目标强度函数$ {r_{t,q}} $展开更新处理，对应的计算式为：

$ {r_{t,q}} = \left( {1 - {p_{t,q}}} \right){r_{t|t - 1}} + \sum\limits_{i = 1}^n {\sum\limits_{j = 1}^{{R_{\rho ,t}}} {w_{t,q}^{\left( l \right)}U\left( {x;b_{t,q}^{\left( l \right)},B_{t,q}^{\left( l \right)}} \right)} }。$

(22)

式中：$ {p_{t,q}} $为在$ t $时刻第$ q $个子带的水下机器人末端执行器位置估计结果。考虑到预测的目标强度函数会被测量值集合$ {p_{t,q}} $中的测量值展开遍历更新，将标签集的更新结果$ {L_{t,q}} $表示为：

$ {L_{t,q}} = {L_{t|t - 1}} \cup L_{t|t - 1}^{{\beta _1}} \cup \cdots L_{t|t - 1}^{{\beta _t}}。$

(23)

3）融合处理

依据更新获取的目标标签信息，对相同目标在不同子带的水下机器人末端执行器位置估计结果中标签为$ {l_t} $的分量构成的高斯混合集$ \Big\{ \left( {w_t^{\left( 1 \right)},b_t^{\left( 1 \right)}, B_t^{\left( 1 \right)}} \right), \left( {w_t^{\left( 2 \right)},b_t^{\left( 2 \right)},B_t^{\left( 2 \right)}} \right), \cdots ,\left( {w_t^{\left( q \right)},b_t^{\left( q \right)},B_t^{\left( q \right)}} \right) \Big\} $，应用广义协方差交集准则对子带跟踪结果展开融合，输出权值高于阈值$ \Delta c $的均值以及对其对应的标签作为水下机器人末端执行器位置动态跟踪结果$ {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{X} _t} $，对应的计算式为：

$ {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{X} _t} = \left\{ {b_t^{\left( q \right)},l_t^{\left( q \right)}|w_t^{\left( q \right)} \gt \Delta c} \right\}。$

(24)

2 实　验 2.1 实验准备

为了有效验证所提水下机器人末端执行器位置动态跟踪方法的有效性，在设定的环境下展开实验分析：

1）水池环境

水池大小：300 m×300 m。

2）水质条件

水温：20±2 ℃；浊度/NTU：5.0。

3）干扰源设置

在水池中布置3 个模拟水流装置，形成不同流速和流向的水流，有效模拟自然水流干扰。

4）计算机设备

配备 Intel Core i9 - 13900K 处理器，64 GB 内存，NVIDIA RTX 4090显卡的高性能计算机。

5）传感器套件

为水下机器人配备高精度惯性测量单元。

相关的实验参数设置：①子带数量10个；②采样频率：100 Hz；③权重：0.55；④干扰水流速度范围：最小0.05 m/s，最大0.50 m/s；⑤迭代次数500次。

2.2 实验结果与分析

在设置的实验参数和实验环境下，分别测试不同方法的水下机器人末端执行器位置在线定位性能，将改进灰狼群优化方法和单双目切换方法作为对比方法，选取5个机器人作为定位目标，分别应用上述方法展开定位处理，对应的实验结果如表1所示。

可以看出，对于5个需要定位的水下机器人末端执行器，改进灰狼群优化方法对编号为3和4的目标定位过程中均出现偏差，最大偏差为2 m；单双目切换方法对编号为1、2和3的目标定位中出现偏差，最大偏差高达5 m；而应用所提方法均可以对各个测试目标展开精准定位，偏差为0，可以得到高准确率的水下机器人末端执行器位置在线定位结果。这是因为所提方法采用的TDOA在多径效应不严重的情况下定位精度较高，而DOA在信号方向特征明显时定位效果较好，二者结合可以从不同维度获取位置信息，从而减少了定位产生的偏差。

在设定的水池环境下，将DFI一体化网络方法和拉盖尔函数方法作为对比方法，应用3种方法展开水下机器人末端执行器位置动态跟踪处理，详细的实验结果如图1所示。

可以看出，分别应用不同方法展开水下机器人末端执行器位置动态跟踪处理后，从跟踪结果的稳定性来看，DFI一体化网络方法受环境干扰影响较大，跟踪曲线波动明显，在遇到复杂水流干扰时，偏差值急剧增大，难以持续稳定地对水下机器人末端执行器展开跟踪。拉盖尔函数方法虽有一定的抗干扰能力，但在多源数据融合处理上存在不足，导致在动态变化的水下环境中，跟踪精度逐渐下降，无法满足高精度的跟踪需求。而通过所提方法则可以精准展开水下机器人末端执行器位置动态跟踪，实时掌握水下机器人末端执行器的运行状态。这得益于所提方法采用高斯混合模型对目标状态进行建模，能够灵活地描述目标状态的不确定性和多模态特性，改进的滤波器能够同时处理多个频率子带的信息，充分挖掘各子带信息中的有效成分，抑制噪声和干扰，提高了对目标位置和运动状态的估计精度。

为了更进一步验证所提方法在跟踪方面的优越性，将跟踪曲线波动熵作为评价指标，波动熵反映了跟踪曲线的不规则程度，熵值越低，表明跟踪过程越稳定，受外界干扰影响越小。图2为各个测试方法的跟踪曲线波动熵变化情况。

可以看出，相比于DFI一体化网络方法和拉盖尔函数方法，所提方法的跟踪曲线波动熵明显更低，且均在0.10以下，意味着所提方法受外界干扰因素的影响更小，能够更为稳健地持续跟踪目标，整体的跟踪性能更优。这是因为所提方法通过广义协方差交集准则综合考虑各子带信息，充分利用各子带信息的优势，弥补单个子带信息的不足，得到一个更为准确、可靠的位置动态跟踪结果。这种融合方式增强了机器人对各种干扰因素的抵抗能力，使得跟踪曲线不会因个别子带受干扰而出现大幅波动，始终保持在相对稳定的范围内，进而有效控制了跟踪曲线波动熵，使其维持在较低水平。

3 结　语

本文提出一种水下机器人末端执行器位置动态跟踪方法。通过大量实验分析表明，所提方法对测试目标的定位偏差为0，有助于减少因位置偏差导致的作业失误和重复操作，降低作业成本和风险。同时对水下机器人的跟踪轨迹与真实轨迹大致拟合，可以准确定位水下机器人末端执行器位置，并且通过多源定位方法融合、滤波器处理以及信息融合准则的综合运用，减少了外界干扰对跟踪系统的影响，有效降低了跟踪曲线波动熵至0.10以下，使跟踪结果更加稳定，实现了定位和跟踪性能的大幅提升，有助于推动该领域技术的发展。