1 引言（Introduction）

AUV一直是智能装备领域的研究热点，已经在海洋科学研究、海洋资源调查和海洋安全保障等方面得到广泛的应用^[1-2]。近年来，研究人员将AUV作为观测器用于对鲸鱼、海豚等海洋生物生活习性的调查，这些海洋生物的运动速度较快，要求AUV尽快与目标汇合并保持跟踪^[3]。在此，本文将这些海洋生物称作“运动目标”。

AUV在对水下运动目标进行追踪时，通常依靠自身携带的声学测量系统，基于被动接收到的运动目标的方位和距离信息，对运动目标的航行参数进行在线估算，这种方法一般称为方位—距离平差法^[4]。

在方位—距离平差法的基本框架下，采用经典最小二乘算法对目标运动参数进行分析时，基于全部采样数据完成一次迭代处理。随着时间的推移，累积采样的数据越来越多，采用这种一次完成迭代过程的算法需要存储所有的数据，严重耗费微处理器的内存资源，不能满足目标运动参数实时解算的要求^[5]。

为了解决该问题，本文提出了一种迭代优化算法，该算法利用渐消记忆递推最小二乘（FMRLS）算法对运动目标的航行参数进行解算，AUV每取得一个目标方位和距离数据，就根据新数据对目标运动参数估计量进行修正，从而得到改善后的新的目标运动参数估计量^[6-8]；同时为了解决FMRLS算法有时带来的数值不稳定问题，将平方根算法与FMRLS算法相结合。这种新的迭代优化算法既能保证数据的快速收敛，又能确保数值的稳定性。

2 方位—距离平差法的计算模型（Calculation model of the bearing-distance adjustment method）

如图 1所示，在运动目标等速直航的假定条件下，其航向线为一条直线，该直线方程的表示可分2种情况进行讨论。

(1) 一般情况，即直线的斜率存在。此时，可用斜截式直线方程$ y=Kx+D_{0} $来表示，其中$ K $为运动目标航向线的斜率，$ D_{0} $为运动目标航向线在纵轴上的截距，即所求的运动目标初始距离。

(2) 特殊情况，即直线的斜率不存在。此时，可用一般的直线方程$ ax+by+c=0 $来表示，依据运动目标的实时方位和距离信息，对直线的3个参数$ a $、$ b $、$ c $进行辨识，从而确定直线的方程。

先考虑一般情况，以AUV初始位置点$ W_{0} $为坐标原点、以运动目标初始方位$ F_{0} $为坐标纵轴建立直角坐标系。$ X_{{\rm m}0} $为运动目标的初始可视角，$ H_{\rm w} $和$ H_{\rm m} $分别为AUV与目标的运动方向，$ \alpha $为运动目标航向线与坐标轴横轴的夹角。$ t_{i} $时刻运动目标位置点坐标$ M_{i} (x_{i}, y_{i} $) 满足以下方程式：

$ \begin{align} \begin{cases} {x_{i} =J_{{0i{\rm S}}} +D_{i} \sin \Delta F_{i}} \\ {y_{i} =J_{{0i{\rm C}}} +D_{i} \cos \Delta F_{i}} \end{cases} \end{align} $

(1)

其中$ \Delta F_{i} =F_{i} -F_{0} $，$ F_{0} $为$ t_{0} $时刻测量及系统采样输入的运动目标方位信息，$ F_{i} $、$ D_{i} $分别为$ t_{i} $时刻测量及系统采样输入的运动目标方位和距离，$ J_{{0i{\rm S}}} $、$ J_{{0i{\rm C}}} $为$ t_{i} $时刻系统累计的AUV总横向位移和纵向位移。

声学系统测量及采样输入的运动目标方位和距离信息必然存在误差；同样，AUV根据其导航设备推算出的横向位移和纵向位移也存在一定的误差。直线的斜截式方程$ y=Kx+D_{0} $可写成

$ \begin{align} J_{0i{\rm C}} +D_{i} \cos \Delta F_{i} -K(J_{ 0i\rm S} +D_{i} \sin \Delta F_{i})-D_{0} =\varepsilon_{i} \end{align} $

(2)

根据递推最小二乘算法的基本原理，以及AUV采集到的实时运动目标方位和距离信息，对方程式(2) 进行系统辨识，得到直线的斜率$ K $及其在纵轴上的截距$ D_{0} $。

在解算得到$ K $和$ D_{0} $后，可以得到以下变量的值，其中$ t_{0i} =t_{i} -t_{0} $。

$ \begin{align} \begin{cases} \alpha ={\rm{\pi }} -\arctan K\; (K>0)\\ \qquad {\rm{或 }}\;\alpha ={\rm{\pi }} +\arctan K\; (K<0) \\ X_{\rm m0} =\alpha -90^{{\circ}} \\ H_{\rm m} =F_{0} +180^{{\circ}}-X_{\rm m0} \\ V_{\rm m} =\dfrac{(D_{0} -J_{0i\rm C})\sin \Delta F_{i} +J_{0i\rm S} \cos \Delta F_{i}} {t_{0i} \sin (X_{\rm m0} +\Delta F_{i})} \end{cases} \end{align} $

(3)

对于非特殊情况（$ a\ne 0 $），方程式$ ax+by+c=0 $可以写成$ x=-b/ay-c/a $的形式，令$ K'=-b/a $，$ D'_{0} =-c/a $，根据声学测量系统获得的实时运动目标方位和距离信息，可以解算得到$ K' $和$ D'_{0} $。

3 FMRLS算法（FMRLS algorithm） 3.1 算法描述

令$ y(i)=J_{0i\rm C} +D_{i} \cos \Delta F_{i} $，$ {\mathit{\boldsymbol{\theta}}}=[{K, D_{0}}]^{\rm T} $，$ {\mathit{\boldsymbol{\varphi}}}_{i}^{\rm T} =[J_{0i\rm S} +D_{i} \sin \Delta F_{i}, 1] $，则方程式(2) 可写成

$ \begin{align} y(i)={\mathit{\boldsymbol{\varphi}}}_{i}^{\rm T} {\mathit{\boldsymbol{\theta}}} +\varepsilon_{i} \end{align} $

(4)

依照FMRLS算法对方程式(4) 进行辨识，实时递推公式可表示成

$ \begin{align} \hat{\mathit{\boldsymbol{\theta}}}_{N+1}=\;&\hat{\mathit{\boldsymbol{\theta}}}_{N} +\frac{{\mathit{\boldsymbol{P}}}_{N} \mathit{\boldsymbol{\varphi}}_{N+1}} {\mathit{\boldsymbol{\varphi}}_{N+{1}}^{\rm T} {\mathit{\boldsymbol{P}}}_{N} {\mathit{\boldsymbol{\varphi}}}_{N+1} +\rho}\times\\ &[y(N+1)-{\mathit{\boldsymbol{\varphi}}}_{N+1}^{\rm T} \hat{\mathit{\boldsymbol{\theta}}}_{N}] \end{align} $

(5)

$ \begin{align} {\mathit{\boldsymbol{P}}}_{N+1} =\;&{\mathit{\boldsymbol{P}}}_{N} -\frac{{\mathit{\boldsymbol{P}}}_{N} {\mathit{\boldsymbol{\varphi}}}_{N+1} {\mathit{\boldsymbol{\varphi}}}_{N+1}^{\rm T} {\mathit{\boldsymbol{P}}}_{N}} {{\mathit{\boldsymbol{\varphi}}}_{N+1}^{\rm T} {\mathit{\boldsymbol{P}}}_{N} {\mathit{\boldsymbol{\varphi}}}_{N+1} +\rho} \end{align} $

(6)

其中常数$ \rho $为一个小于或等于1的遗忘因子^[9]，定义以下公式：

$ \begin{align} y_{k} &=J_{0k\rm C} +D_{k} \cos \Delta F_{k} \end{align} $

(7)

$ \begin{align} {\mathit{\boldsymbol{X}}}_{k} &=[J_{0k\rm S} +D_{k} \sin \Delta F_{k}, 1]^{\rm T} \end{align} $

(8)

$ \begin{align} {\mathit{\boldsymbol{K}}}(k+1)&={\mathit{\boldsymbol{P}}}(k){\mathit{\boldsymbol{\varphi}}}_{k+1} /\big(\rho +{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{P}}}(k){\mathit{\boldsymbol{\varphi}}}_{k+1}\big) \end{align} $

(9)

$ \begin{align} {\mathit{\boldsymbol{\theta}}} (k+1)&={\mathit{\boldsymbol{\theta}}} (k)+{\mathit{\boldsymbol{K}}}(k+1)\big(y_{k+1} -{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{\theta}}} (k)\big) \end{align} $

(10)

$ \begin{align} {\mathit{\boldsymbol{P}}}(k+1)&=\big({\mathit{\boldsymbol{P}}}(k)-{\mathit{\boldsymbol{K}}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{P}}}(k)\big)/\rho \end{align} $

(11)

矩阵$ {\mathit{\boldsymbol{P}}} $是一个$ {2}\times {2} $的方阵，FMRLS算法下运动目标航行参数实时估计的大致步骤如下：

(1) 给定初始值$ {\mathit{\boldsymbol{\theta}}} (0) $和$ {\mathit{\boldsymbol{P}}}(0) $，输入初始数据，设置$ k=1 $。

(2) 进行第$ k+1 $次采样，得到运动目标的纵、横坐标值$ y_{k+1} $、$ x_{k+1} $。

(3) 计算预测误差$ \varepsilon_{k+1} =y_{k+1} -{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{\theta}}} (k) $。

(4) 依据式(9) 计算增益$ {\mathit{\boldsymbol{K}}} (k+1) $。

(5) 根据方程式(10) 修正估计值$ {\mathit{\boldsymbol{\theta}}} (k+1) $。

(6) 按照方程式(11) 计算协方差$ {\mathit{\boldsymbol{P}}}(k+1) $。

(7) $ k+1\Rightarrow k $，返回本流程步骤(2)，如此循环。

3.2 数值不稳定性证明

在对目标运动参数进行估算时，递推参数估计是由字长有限的微处理器来实现的，需要进行上万次的迭代运算，而在迭代运算过程中，并不是每种递推算法都能保证数值的稳定性。同理，在本文的FMRLS算法中，也存在数值不稳定的问题，下面来给予证明。

证明  在本文所考虑的有2个参数的运动目标航行参数估计的解算模型中，依据方程式(10) 可以得到以下参数估计误差方程：

$ \begin{align} \Delta {\mathit{\boldsymbol{\theta}}} (k\!+\!1) \! &={\mathit{\boldsymbol{\theta}}} -{\mathit{\boldsymbol{\theta}}}(k+1) \\ &={\mathit{\boldsymbol{\theta}}} -{\mathit{\boldsymbol{\theta}}} (k)-{\mathit{\boldsymbol{K}}}(k+1)\big(y_{k+1} -{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{\theta}}} (k)\big) \\ &=\Delta {\mathit{\boldsymbol{\theta}}} (k)-{\mathit{\boldsymbol{K}}}(k\!+\!1)y_{k+1} +{\mathit{\boldsymbol{K}}}(k\!+\!1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{\theta}}} (k) \\ &=\big({\mathit{\boldsymbol{I}}}-{\mathit{\boldsymbol{K}}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} \big)\Delta {\mathit{\boldsymbol{\theta}}} (k)-{\mathit{\boldsymbol{K}}}(k+1)\varepsilon_{k+1} \end{align} $

(12)

这是一个随机误差方程，其中$ \mathit{\boldsymbol{I}} $为$ 2\times 2 $的单位阵，$ \mathit{\boldsymbol{K}} $为$ 2\times 1 $的列向量，$ \mathit{\boldsymbol{P}} $为$ 2\times 2 $的方阵，$ {\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} $为$ 1\times 2 $的行向量。当行列式$ |{\mathit{\boldsymbol{I}}}-{\mathit{\boldsymbol{K}}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} |>1 $且方阵$ {\mathit{\boldsymbol{I}}}-{\mathit{\boldsymbol{K}}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} $正定时，特别当方阵$ {\mathit{\boldsymbol{K}}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} $的2个特征根$ \lambda_{1} $、$ \lambda_{2} $都小于0时，这个随机差分方程是不稳定的，参数估计误差将越来越大。

由增益的递推公式，可得：

$ \begin{align} {\mathit{\boldsymbol{K}}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} =\frac{{\mathit{\boldsymbol{P}}}(k){\mathit{\boldsymbol{\varphi}}}_{k+1} {\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T}} {\rho +{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{P}}}(k){\mathit{\boldsymbol{\varphi}}}_{_{k+1}}} \end{align} $

(13)

而方阵$ {\mathit{\boldsymbol{\varphi}}}_{k+1} {\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} $是非负定的。因此，随机误差方程不稳定的条件等价于

$ \begin{align} \begin{cases} {{\mathit{\boldsymbol{P}}}(k)\rm{\; 的特征根都小于}\; 0} \\ {|{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{P}}}(k){\mathit{\boldsymbol{\varphi}}}_{k+1} |<\rho} \end{cases} \end{align} $

(14)

这就是说，一旦方阵$ {\mathit{\boldsymbol{P}}}(k) $的2个特征值都变为负，参数估计就不稳定，直到$ |{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{P}}}(k){\mathit{\boldsymbol{\varphi}}}_{k+1} |>\rho $为止。

为了避免上述问题的产生，防止在递推过程中方阵$ \mathit{\boldsymbol{P}} $的2个特征根都变为负，本文采用平方根算法进行补偿。

4 平方根算法（Square root algorithm）

在方程式(4) 的基础上，定义以下公式：

$ \begin{align} &{{\mathit{\boldsymbol{f}}}}_{k+1} ={{{\mathit{\boldsymbol{S}}}}^{\rm T}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1} \end{align} $

(15)

$ \begin{align} &\beta_{k+1} =\rho +{{\mathit{\boldsymbol{f}}}}_{k+1}^{\rm T} {{\mathit{\boldsymbol{f}}}}_{k+1} \end{align} $

(16)

$ \begin{align} &\alpha_{k+1} =1/{(\beta_{k+1} +\sqrt{\rho \beta_{k+1}})} \end{align} $

(17)

$ \begin{align} &{\mathit{\boldsymbol{Q}}}(k+1)={\mathit{\boldsymbol{S}}}(k+1){{\mathit{\boldsymbol{f}}}}_{k+1} /\beta_{k+1} \end{align} $

(18)

$ \begin{align} &{\mathit{\boldsymbol{\theta}}} (k+1)={\mathit{\boldsymbol{\theta}}} (k)+{\mathit{\boldsymbol{Q}}}(k+1)\big(y_{k+1} -{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{\theta}}} (k)\big) \end{align} $

(19)

$ \begin{align} &{\mathit{\boldsymbol{S}}}(k+1)=1/{\sqrt{\rho}} \big({{\mathit{\boldsymbol{I}}}-\alpha_{k+1} \beta_{k+1} {\mathit{\boldsymbol{Q}}}(k+1){\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T}}\big){\mathit{\boldsymbol{S}}}(k) \end{align} $

(20)

其中$ \mathit{\boldsymbol{S}} $、$ \mathit{\boldsymbol{Q}} $为$ {1}\times 2 $的行向量，$ {\mathit{\boldsymbol{f}}} $ ^[10]在本文情况中为$ {1}\times {1} $的量。平方根算法下运动目标航行参数实时估计的大致步骤如下：

(1) 给定初始值$ {\mathit{\boldsymbol{\theta}}} (0) $和$ {\mathit{\boldsymbol{S}}}(0) $，输入初始数据，设置$ k=1 $。

(2) 进行第$ k+1 $次采样，得$ y_{k+1} $、$ x_{k+1} $。

(3) 计算预测误差$ \varepsilon_{k+1} =y_{k+1} -{\mathit{\boldsymbol{\varphi}}}_{k+1}^{\rm T} {\mathit{\boldsymbol{\theta}}} (k) $。

(4) 依据式(18) 计算增益$ {\mathit{\boldsymbol{Q}}}(k+1) $。

(5) 根据方程式(19) 修正估计值$ {\mathit{\boldsymbol{\theta}}} (k+1) $。

(6) 按照方程式(20) 计算协方差$ {\mathit{\boldsymbol{S}}}(k+1) $。

(7) $ k+1\Rightarrow k $，返回本流程步骤(2)，如此循环。

5 迭代优化算法（Iterative optimization algorithm） 5.1 算法描述

在目标运动参数实时解算过程中，并行利用FMRLS算法和平方根算法，并在2种算法内部比较相邻2个仿真步长解算出的数值偏差，依据偏差来选择最优的目标运动参数。该算法的计算流程如图 2所示。

5.2 各种情况下变量范围的确定

在AUV利用方位—距离平差法对目标运动参数进行解算的过程中，根据目标航行直线的斜率及其速度符号可以确定目标的运动方向，但目标运动速度的解算是最难收敛的，也最易受其他变量的扰动影响。由式(3) 可以看出：目标运动速度跟多个变量有关，既跟初始距离、初始舷角有关，又跟目标方位的变化量、AUV的横向和纵向位移有关，任何一个变量发生变化，都会使目标速度信息发生改变。因此，对各个变量的变化范围有一个明确的定义是正确进行目标运动参数解算的前提。

根据运动目标与AUV之间的相对运动状态，可将目标分为以下4种情况：目标处于AUV坐标系的第一、第二、第三以及第四象限，具体情形如图 3所示。

由图 3可以看出，运动目标方位变化量的正切函数可表示为

$ \begin{align} {\rm{tan}}\Delta F_{i} =\frac{t_{0i} V_{\rm m} \sin X_{\rm m0} -J_{0i\rm S}} {D_{0} -t_{0i} V_{\rm m} \cos X_{\rm m0} -J_{0i\rm C}} =\frac{\sin \Delta F_{i}} {\cos \Delta F_{i}} \end{align} $

(21)

由式(16) 可推导出此时的目标运动速度为

$ \begin{align} V_{\rm m} =\frac{(D_{0} -J_{0i\rm C})\sin \Delta F_{i} +J_{0i\rm S} \cos \Delta F_{i}} {t_{0i} \sin (X_{\rm m0} +\Delta F_{i})} \end{align} $

(22)

依据方程式(22) 中各个变量的取值范围，可以推导出以上4种情况下目标运动速度的符号及其量值，具体如表 1所示。

6 仿真实验与分析（Simulation experiments and analyses） 6.1 遗忘因子的数值确定

因FMRLS算法以及平方根算法中均含有遗忘因子$ \rho $，如何选择合适的遗忘因子，既保证数值的快速收敛，又简化目标运动参数解算过程，是迭代优化算法必须解决的首要问题。

在此，给定一种具体情形：目标与AUV之间的初始距离为1200 m、目标航向角为$ 155^{{\circ}} $、航速1.5 m/s，AUV做匀速直线运动，航向角为$ 80^{{\circ}} $、航速为0.5 m/s，目标方位角误差为$ 3^{{\circ}} $，仿真步长为0.5 s。变换遗忘因子$ \rho $的数值并对比各数值下的目标运动参数解算结果，由此确定合适的遗忘因子。

设定$ \rho_{1} =0.99 $、$ \rho_{2} =0.999 $、$ \rho_{3} =0.9999 $、$ \rho_{4} =1.0 $，对这4种遗忘因子下的目标运动参数进行迭代计算，仿真场景及其解算结果如图 4~图 6所示。

从仿真结果可以看出，在800 s的仿真时间内，遗忘因子小于或等于0.99时，方位—距离平差法解算出来的目标运动参数都是发散的，遗忘因子数值越接近1.0；目标运动参数解算收敛的时间越短，解算的效果也越好。在此，我们选择的遗忘因子数值为0.999（解算精度已满足实际需求）。

6.2 各种场景下的目标航行参数分析 6.2.1 基本假设

为了充分证明迭代优化算法在运动目标航行参数解算中的有效性，设计了6种典型场景，具体见表 2。场景1~场景4为AUV匀速直线航行，运动目标分别处于AUV坐标系的第一、第二、第三和第四象限；场景5为AUV悬停，运动目标从AUV坐标系的第一象限向第四象限穿越，场景6为AUV悬停，运动目标从AUV坐标系的第二象限向第三象限穿越。

在仿真实验中，设定运动目标的方位测量误差为$ 4^{\circ} $，距离测量误差为$ 15\% D $，$ D $表示真实距离，场景1~4的仿真时间为1 000 s，场景5、6的仿真时间为500 s，仿真步长为0.5 s。

6.2.2 仿真分析

(1) 当AUV匀速直航时

场景1~场景4运动目标航行参数的解算结果如图 7所示，解算的参数主要包括运动目标的初始距离、运动速度和航向角。

从图 7可看出，在给定的运动目标距离和方位测量误差下，迭代优化算法能快速解算出运动目标的初始距离、航向角及运动方向，数值收敛时间约3 min，目标速度信息也能够在5 min左右收敛，并且目标的初始距离越近，数值收敛的速度越快。

(2) 当AUV悬停时

场景5和场景6这2种情况下，运动目标的航行参数解算结果如图 8所示。解算的参数包括运动目标的初始距离、方位角、运动速度、航向角以及初始视角。

从仿真结果可以看出，当运动目标与AUV之间的相对运动状态发生象限穿越，甚至当运动目标的相对方位发生360°突变时，迭代优化算法解算出来的目标运动参数还能继续保持收敛。

6.3 与纯方位目标跟踪算法的对比分析

为了进一步验证迭代优化算法相较于其他算法的优越性，选用场景1，利用纯方位目标跟踪算法^[11-12]对目标的航行参数进行解算，解算结果如图 9所示。

由图 9可以看出，当采用纯方位目标跟踪算法时，为了确保目标运动参数的解算数据趋于收敛，AUV必须做机动航行，这对于AUV的机动性能提出了更高的要求，对其机载电池容量也是一个不小的挑战。

7 结论（Conclusion）

本文依据AUV采样输入的运动目标方位和距离信息，采用方位—距离平差法对目标航行参数进行解算，针对经典最小二乘算法难以满足目标运动参数实时解算要求这一问题，在给出渐消记忆递推最小二乘（FMRLS）算法以及该算法可能带来的数值不稳定性证明的基础上，提出一种将FMRLS算法与平方根算法相结合的迭代优化算法，以保证数据的快速收敛和数值的稳定性。多种场景下的计算机仿真结果证明，该迭代优化算法的鲁棒性好，无需AUV作任何机动航行，甚至可以悬停，适合应用于AUV对水下运动目标进行追踪，为这一工程实际问题提供了一种有效解决办法。