﻿ 基于迭代优化算法的AUV水下运动目标航行参数估计
 机器人 2022, Vol. 44 Issue (2): 203-211 0

KANG Xiaodong, LI Yiping. Motion Parameters Estimation of Underwater Moving Target Based onIterative Optimization Algorithm for an AUV[J]. ROBOT, 2022, 44(2): 203-211.

1. 上海电机学院电气学院, 上海 201306;
2. 中国科学院沈阳自动化研究所机器人学国家重点实验室, 辽宁 沈阳 110016;
3. 中国科学院机器人与智能制造创新研究院, 辽宁 沈阳 110169;
4. 辽宁省水下机器人重点实验室, 辽宁 沈阳 110016

Motion Parameters Estimation of Underwater Moving Target Based onIterative Optimization Algorithm for an AUV
KANG Xiaodong1 , LI Yiping2,3,4
1. Department of Electrical Engineering, Shanghai Dianji University, Shanghai 201306, China;
2. State Key Laboratory of Robotics, Shenyang Institute of Automation, Chinese Academy of Sciences, Shengyang 110016, China;
3. Institutes for Robotics and Intelligent Manufacturing, Chinese Academy of Sciences, Shenyang 110169, China;
4. Key Laboratory of Marine Robotics, Liaoning Province, Shenyang 110016, China
Abstract: In order to solve the technical problem of real-time dynamic tracking of underwater moving targets for autonomous underwater vehicle (AUV), an iterative optimization algorithm is proposed, which combines the fading memory recursive least square (FMRLS) algorithm with the square root algorithm.It makes full use of the fast convergence performance of FMRLS algorithm, and uses the square root algorithm to solve the numerical instability problem in the iterative process.The iterative optimization algorithm can quickly calculate the initial distance, the heading angle and the moving direction of the moving target, and the numerical convergence time is about 3 min, as well as the target moving speed can be converged in about 5 min.With the proposed algorithm, the converge time is short, the computing velocity is high, and moreover, an AUV can keep hovering without any form of maneuverer from itself.Those advantages of the proposed algorithm make it a perfect solution for practical engineering problems of underwater moving target tracking.
Keywords: fading memory    recursive least square    square root algorithm    underwater moving target    motion parameters estimation

1 引言（Introduction）

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

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

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

 图 1 AUV与运动目标之间的几何关系 Fig.1 Geometric relationship between the AUV and the moving target

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

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

 \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)

(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)

 \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)

4 平方根算法（Square root algorithm）

 \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)

(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 算法描述

 图 2 迭代优化算法的计算流程 Fig.2 Calculation process of the iterative optimization algorithm
5.2 各种情况下变量范围的确定

 图 3 种情况下移动目标与AUV之间的相对运动状态图 Fig.3 Relative motion states between the moving target and the AUV in the 4 situations

 \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)

 \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)

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

6.2.2 仿真分析

(1) 当AUV匀速直航时

 图 7 AUV匀速直航时的目标运动参数解算结果 Fig.7 Calculation results of target motion parameters when AUV sails in a straight line at a constant speed

(2) 当AUV悬停时

 图 8 AUV悬停时的目标运动参数解算结果 Fig.8 Calculation results of target motion parameters when AUV is hovering

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

 图 9 AUV纯方位目标跟踪解算结果 Fig.9 Calculation results of bearing-only target tracking

7 结论（Conclusion）

 [1] 徐会希, 姜成林. 基于USV与AUV异构平台协同海洋探测系统研究综述[J]. 中国科学院大学学报, 2021, 38(2): 145-159. Xu H X, Jiang C L. Heterogeneous oceanographic exploration system based on USV and AUV: A survey of developments and challenges[J]. Journal of University of Chinese Academy of Sciences, 2021, 38(2): 145-159. [2] 李一平, 李硕, 张艾群. 自主/遥控水下机器人研究现状[J]. 工程研究, 2016, 8(2): 217-222. Li Y P, Li S, Zhang A Q. Research status of autonomous & remotely operated vehicle[J]. Journal of Engineering Studies, 2016, 8(2): 217-222. [3] 王艳艳, 刘开周, 封锡盛. AUV纯方位目标跟踪轨迹优化方法[J]. 机器人, 2014, 36(2): 179-184. Wang Y Y, Liu K Z, Feng X S. Optimal AUV trajectories for bearings-only tracking[J]. Robot, 2014, 36(2): 179-184. [4] 董志荣. 主动声呐浮标目标运动分析数学模型[J]. 电光与控制, 2007, 14(1): 5-9. Dong Z R. Mathematical models for target moving analysis through active sonobuoy[J]. Electronics Optics & Control, 2007, 14(1): 5-9. DOI:10.3969/j.issn.1671-637X.2007.01.002 [5] Zhang S, Zheng W X. Performance analysis of compressive diffusion normalized LMS algorithm with link noises[C]//IEEE Internatioanl Symposium on Circuits and Systems. Piscataway, USA: IEEE, 2021. DOI: 10.1109/ISCAS51556.2021.9401635. [6] 方桂花, 王鹤川, 高旭. 基于动态遗忘因子递推最小二乘法的永磁同步电机参数辨识算法[J]. 计算机应用与软件, 2021, 38(1): 280-283. Fang G H, Wang H C, Gao X. Parameter identification algorithm of permanent magnet synchronous motor based on dynamic forgetting factor recursive least square method[J]. Computer Applications and Software, 2021, 38(1): 280-283. DOI:10.3969/j.issn.1000-386x.2021.01.047 [7] 钱虹, 江诚, 潘岳凯, 等. 基于自适应遗忘因子RLS算法的稳压器模型在线辨识[J]. 核动力工程, 2019, 40(6): 124-129. Qian H, Jiang C, Pan Y K, et al. Online identification of regulator model based on adaptive forgetting factor RLS algorithm[J]. Nuclear Power Engineering, 2019, 40(6): 124-129. [8] 冀大雄, 封锡盛, 刘开周, 等. 综合权值递推最小二乘法估计从UUV航行参数[C]//中国仪器仪表与测控技术报告大会论文集. 2008: 310-312. Ji D X, Feng X S, Liu K Z, et. al. Slave-UUV motion analysis using RLS algorithm with synthetical weight[C]//Proceedings of China Instrumentation and Measurement and Control Technology Report Conference. 2008: 310-312. [9] 韩曾晋. 自适应控制[M]. 北京: 清华大学出版社, 1995. Han Z J. Adaptive control[M]. Beijing: Tsinghua University Press, 1995. [10] 张树春, 胡广大. 平方根滤波及其在目标跟踪方面的应用[J]. 哈尔滨工业大学学报, 2008, 40(5): 700-704. Zhang S C, Hu G D. Square root filtering and its application in target tracking[J]. Journal of Harbin Institute of Technology, 2008, 40(5): 700-704. DOI:10.3321/j.issn:0367-6234.2008.05.007 [11] Alexandri T, Diamant R. A reverse bearings only target motion analysis for autonomous underwater vehicle navigation[J]. IEEE Transactions on Mobile Computing, 2019, 18(3): 494-506. DOI:10.1109/TMC.2018.2840997 [12] Clavard J, Pillon D, Pignol A C, et al. Bearings-only target motion analysis of a source in a circular constant speed motion from a non-manueuvering platform[C]//14th International Conference on Information Fusion. Piscataway, USA: IEEE, 2011.