CN111982098A - 一种用于两个运动auv的同时定位方法 - Google Patents

Abstract

本发明公开一种用于两个运动AUV的同时定位方法。该发明基于改进的扩展卡尔曼滤波得到两个AUV的同时定位方法，适用于存在运动过程噪声及观测噪声且运动模型不精确的情况下，两个AUV的同时定位。本发明主要包括：基于卡尔曼滤波的两个AUV同时定位方法，基于实际航行数据训练得到的AUV模型梯度下降决策树。基于模型梯度下降决策树的改进扩展卡尔曼滤波。本发明使AUV在过程噪声及观测噪声干扰下完成同时定位，减小模型不精确对定位精度的影响，方法简单，应用范围广。

Description

一种用于两个运动AUV的同时定位方法

技术领域

本发明涉及自主水下机器人(AUV，Autonomous Underwater Vehicle)的定位方法，更具体的说，是存在过程噪声及观测噪声干扰下两个运动AUV的同时定位方法。

背景技术

AUV完成任何预定使命的必备条件为准确的水下导航，即“Where am I”问题，这是决定AUV能否自主执行并完成任务的关键技术。本文以AUV动态自主回收为背景，待回收AUV需要知悉其自身及携带回收装置的AUV的位置信息。由于水下环境及其自身携带传感器的限制，AUV的运动过程存在过程噪声，AUV之间的相对距离的测量存在观测噪声。噪声的存在不利于实现AUV的同时定位。

在AUV水下导航领域，同时定位与构图(SLAM，Simultaneous Localization AndMapping)成为近几年的研究热点。在AUV自身位置不确定的情况下，利用所携带的传感器反复观测环境中的特征，从而完成AUV自身的定位及特征位置的校正。在所有的SLAM方法中，基于扩展卡尔曼滤波的同时定位及构图方法(EKF-SLAM，Extended Kalman Filter-SLAM)因为其简单和通用性强的特点而被广泛应用。EKF-SLAM方法的不足之处在于其定位精度受AUV的运动模型精度及噪声统计精度影响。

发明内容

本发明要解决的技术问题是提供一种定位方法，当存在过程噪声和观测噪声干扰时，该方法能够实现两个运动AUV的同时定位。

本发明为实现上述目的所采用的技术方案是：一种用于两个运动AUV的同时定位方法，包括以下步骤：

利用AUV的实际航行数据回归拟合AUV运动模型的梯度下降决策树，得到AUV单位时间模型航行里程到实际航行里程的映射关系；

定义两个AUV的初始状态和初始状态估计误差协方差并初始化；定义过程噪声和观测噪声的统计特性并初始化；

两个AUV相互通信共享其控制输入信息；根据两个AUV的控制输入量计算单位时间的模型航行里程，并根据上述映射关系得到AUV的实际航行里程，进而分别预测两个AUV的状态和状态估计误差协方差；

利用超短基线测出两个AUV在x、y方向的相对距离，并分别更新两个AUV的状态和状态估计误差协方差，从而实现两个运动AUV的同时定位。

所述AUV的实际航行数据包括：待回收AUV-Ⅰ和携带回收装置的AUV-Ⅱ在过去的实际航行中由传感器记录的AUV的速度v、方向ψ及位置信息(x,y)。

所述回归拟合AUV运动模型的梯度下降决策树包括：

通过AUV-Ⅰ的实际航行速度v_v、实际航向角ψ_v、单位时间dt以及运动模型f分别计算得到AUV-Ⅰ在x、y方向走过的距离Δx_v、Δy_v；利用AUV-Ⅰ搭载的传感器分别记录dt时间内在x、y方向走过的实际距离dx_v、dy_v；定义{(Δx_v,dx_v)}为第一个GBDT训练集的样本、{(Δy_v,dy_v)}为第二个GBDT训练集的样本；

通过AUV-Ⅱ的实际航行速度v_f、实际航向角ψ_f、单位时间dt以及运动模型f分别计算得到AUV-Ⅱ在x、y方向走过的距离Δx_f、Δy_f；利用AUV-Ⅱ搭载的传感器分别记录dt时间内在x、y方向走过的实际距离dx_f、dy_f；定义{(Δx_f,dx_f)}为第三个GBDT训练集的样本、{(Δy_f,dy_f)}为第四个GBDT训练集的样本；

利用梯度下降决策树算法训练上述四个GBDT训练集，分别得到Δx_v到dx_v、Δy_v,到dy_v、Δx_f到dx_f、Δy_f到dy_f的映射关系。

所述利用梯度下降决策树算法训练上述四个GBDT训练集，包括：

设定最大迭代次数为T。损失函数为L。对于训练集样本{(Δx₁,dx₁),(Δx₂,dx₂)…(Δx_m,dx_m)}，其中(Δx_i,dx_i),(i＝1…m)为训练样本集的第i个样本变量，m为样本数量。

初始化弱学习器：

式中，c为使样本集损失函数之和取得最小值的参数。f₀为损失函数之和，称为初始学习器。

对迭代轮数t＝1,2,…T有：

对样本i＝1,2,…m，计算负梯度：

式中，r_ti为在第t轮迭代中第i个样本损失函数负梯度。f_t-1(Δx)是t-1轮的学习器。

利用(Δx_i,r_ti),(i＝1,2…m)拟合一颗回归树，得到第t颗回归树，其对应的叶子区域为R_tj,j＝1,2…J。其中J为回归树的叶子节点个数；

对叶子区域j＝1,2…J，计算最佳拟合值：

式中，c为使第t颗回归树中第j个叶子区域样本的损失函数最小的参数，c_tj为第t轮迭代中第j个叶子区域的最小损失函数之和。

更新强学习器：

式中，I为单位矩阵。f_t(Δx)是第t轮学习器。

得到强学习器：

式中，f(Δx)为最终得到的强学习器。

所述运动模型为：

式中，Δx(k)、Δy(k)分别是k-1时刻到k时刻内AUV在x方向和y方向走过的距离，ψ_k，ψ_k-1分别是k时刻和k-1时刻AUV的航向角，v是AUV的航行速度，G为偏航角。

所述定义两个AUV的初始状态和初始状态估计误差协方差并初始化包括：

定义EKF-SLAM算法的状态变量X(k)＝[X_v(k) X_f(k)]^T；式中，X_v(k)＝[x_v,k y_v,kψ_v,k]^T是k时刻AUV-Ⅰ位置的横、纵坐标以及航向角的状态变量，X_f(k)＝[x_f,k y_f,k ψ_f,k]^T是k时刻AUV-Ⅱ位置的横、纵坐标以及航向角的状态变量；

根据AUV-Ⅰ和AUV-Ⅱ初始时刻的位置信息和航向角信息初始化状态变量X(k)；初始化k时刻状态估计误差协方差矩阵P(k)，初始时刻P(0)为6行6列的零矩阵。

所述定义过程噪声和观测噪声的统计特性并初始化包括：

假设系统过程噪声和观测噪声皆为零均值高斯白噪声，定义统计特性如下：

E[n₁(k)]＝0,E[n₁(k)n₁(k)^T]＝Q₁

E[n₂(k)]＝0,E[n₂(k)n₂(k)^T]＝Q₂

E[m(k)]＝0,E[m(k)m(k)^T]＝R

E[n₁(k)n₂(j)^T]＝0,E[n₁(k)m(j)^T]＝0,E[n₂(k)m(j)^T]＝0

其中，AUV-Ⅰ的高斯运动过程噪声为n₁，AUV-Ⅱ的高斯运动过程噪声为n₂，观测噪声为m，E表示期望，k、j表示时刻；

初始化：n₁的均值为零，方差为Q₁；n₂的均值为零，方差为Q₂；m的均值为零，方差为R；n₁、n₂和m之间彼此不相关，即它们之间的协方差为零。

所述两个AUV相互通信共享其控制输入信息为：

定义k时刻AUV-Ⅰ的控制输入变量u_v(k)＝[v_v，G_v]，AUV-Ⅱ的控制输入变量u_f(k)＝[v_f，G_f]；其中，V表示速度，G表示偏航角，下标v表示AUV-Ⅰ，下标f表示AUV-Ⅱ；

AUV-Ⅱ将其控制输入u_f(k)通过水声通讯设备发送至AUV-Ⅰ的主控计算机。

所述根据两个AUV的控制输入量计算单位时间的模型航行里程，并根据上述映射关系得到AUV的实际航行里程，进而分别预测两个AUV的状态和状态估计误差协方差：

根据AUV-Ⅰ和AUV-Ⅱ的控制输入变量u_v(k)＝[v_v，G_v]、u_f(k)＝[v_f，G_f]计算AUV-Ⅰ和AUV-Ⅱ单位时间的模型航行里程，并根据映射关系得到AUV-Ⅰ和AUV-Ⅱ在x、y方向单位时间dt内走过的距离dx_v，dy_v，dx_f，dy_f；

从而得到预测状态：

状态估计误差协方差的预测方程为：

其中，

为k-1时刻的状态估计值，

为k时刻的状态预测值；P(k-1)为k-1时刻的状态估计误差协方差，P(k|k-1)为k时刻的状态估计误差协方差的预测值，0_3×3为3行3列的零矩阵，0_2×2为2行2列的零矩阵，F_v(k)和F_f(k)分别是非线性运动方程f(·)对状态X_V和X_f在当前最优估计处的雅可比矩阵；W₁(k)和W₂(k)分别是非线性运动方程f对噪声n₁(k)和n₂(k)的雅可比矩阵。

所述利用超短基线测出两个AUV在x、y方向的相对距离为：AUV-Ⅱ携带超短基线的收发器，AUV-Ⅰ携带超短基线的应答器，AUV-Ⅱ分别解算两个AUV在x方向和y方向之间相对距离，并通过水声通讯设备发送至AUV-Ⅰ的主控计算机。

所述分别更新两个AUV的状态和状态估计误差协方差为：

a.当接收到最新的观测值Z(k)＝[Z(x)，Z(y)]^T时，计算观测值残差矩阵υ(k)及残差协方差矩阵S(k)和卡尔曼增益K(k)，如下：

S(k)＝H(k)P(k|k-1)H(k)^T+R

K(k)＝P(k|k-1)H^T(k)S(k)^-1

式中，H(k)是非线性传感器观测方程h对X(k)的偏导数的雅可比矩阵，非线性观测方程h为：

式中，ψ_f为AUV-Ⅱ的航向角，α为中间变量，x_v,y_v分别为AUV-Ⅰ位置的横纵坐标，x_f,y_f分别为AUV-Ⅱ位置的横纵坐标，Z(x)、Z(y)分别表示由相对距离转换得到的AUV-Ⅰ在AUV-Ⅱ坐标系下的位置的横纵坐标；

b.按照下式更新状态和状态估计误差协方差矩阵：

P(k)＝(I-K(k)H(k))P(k|k-1)

式中，I为单位矩阵，

为k时刻状态的估计值，P(k)为k时刻状估计误差协方差。

本发明具有以下优点及有益效果：

1、具有实际应用价值。本发明以AUV动态自主回收为背景，携带回收装置的AUV回收另一个待回收AUV。AUV的使命结束，能源补充或日常维护需要其回收至岸上。AUV的动态自主回收可以实现高海况或极端条件下的AUV回收，减少了作业人员的风险，减少了对回收时间及回收地点的限制。两个运动AUV的同时定位技术是AUV动态自主回收归航过程的基础。

2、具有实际意义。受传感器及自身运动噪声干扰，AUV对其自身和回收平台的定位精度有限。较大的定位误差将导致回收任务的失败。因此，本发明可以有效提高AUV动态自主回收的成功率。

附图说明

图1是本发明的坐标系示意图；

图2是本发明的EKF-SLAM示意图；

图3是本发明的工作流程图；

图4是AUV-Ⅰ和AUV-Ⅱ的估计轨迹和真实轨迹；

图5是AUV-Ⅰ和AUV-Ⅱ的定位误差；

图6是AUV-Ⅰ和AUV-Ⅱ的估计轨迹和真实轨迹；

图7是AUV-Ⅰ和AUV-Ⅱ的定位误差；

图8是AUV-Ⅰ和AUV-Ⅱ的估计轨迹和真实轨迹；

图9是AUV-Ⅰ和AUV-Ⅱ的定位误差。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明由两台AUV、AUV上搭载的位姿传感器和超短基线，水声通信机以及计算机组成。需要如下设备：

控制计算机：用于实时监控AUV位姿信息，根据AUV-Ⅰ和AUV-Ⅱ的输入信息预测AUV-Ⅰ和AUV-Ⅱ的位置信息。根据超短基线的测量值更新AUV-Ⅰ和AUV-Ⅱ的位置信息。

超短基线：超短基线的收发器安装于AUV-Ⅱ上，应答器安装于AUV-Ⅰ上。AUV-Ⅱ解析得到AUV-Ⅰ在其坐标系下的位置的横纵坐标，即Z(x)和Z(y)。测量坐标系的说明如图1所示。

推进器、舵等航行动力设备：驱动AUV运动以完成回收。

位姿传感器：获得AUV自身航速、航向信息

水声通信设备：实现AUV-Ⅰ和AUV-Ⅱ的实时通信。AUV-Ⅱ将其自身的速度和偏航角的控制输入量u_f(k)＝[v_f G_f]及超短基线测得的相对距离Z(x)和Z(y)发送至AUV-Ⅰ的主控计算机。

EKF-SLAM的过程如图2所示，为了改善模型不精确对同时定位精度的影响，在EKF-SLAM的状态预测阶段做出改进。

GBDT-EKF-SLAM的过程如图3所示，与原EKF-SLAM算法的不同点在于状态预测阶段使用GBDT的输出代替模型计算值。

初始时刻AUV-Ⅰ和AUV-Ⅱ航行至相同深度，因此将回收过程简化至二维平面运动。AUV-Ⅰ根据自身和AUV-Ⅱ的控制输入，即u_v(k)＝[v_v G_v]和u_f(k)＝[v_f G_f]，利用梯度下降树预测AUV-Ⅰ和AUV-Ⅱ的状态及状态估计误差协方差。当接收到超短基线的测量值时，更新AUV-Ⅰ和AUV-Ⅱ的状态及状态估计误差协方差。

第1步：拟合梯度下降树

通过AUV-Ⅰ的实际航行速度v_v，实际航向角ψ_v，单位时间dt以及运动模型f分别计算得到AUV-Ⅰ在x,y方向走过的距离Δx_v,Δy_v。AUV-Ⅰ传感器记录的dt时间x方向和y方向走过的距离分别为dx_v,dy_v。。第一个GBDT训练集样本为{(Δx_v,dx_v)}。第二个GBDT训练集样本为{(Δy_v,dy_v)}。通过AUV-Ⅱ的实际航行速度v_f，实际航向角ψ_f，单位时间dt以及运动模型f分别计算得到AUV-Ⅱ在x,y方向走过的距离Δx_f,Δy_f。AUV-Ⅱ传感器记录的dt时间x方向和y方向走过的距离分别为dx_f,dy_f。第三个GBDT训练集的样本为{(Δx_f,dx_f)}。第四个GBDT训练集的样本为{(Δy_f,dy_f)}。以第一个GBDT为例，通过GBDT算法的训练后，得到Δx_v到dx_v的映射关系

训练集构建完毕后，开始GBDT算法的迭代过程。GBDT算法采用的弱学习器限定为分类回归树(CART，Classification And Regression Tree)。CART算法具体如下：

假设样本空间为{(Δx₁,dx₁)…(Δx_m,dx_m)}，m为样本数量。

1.遍历输入空间，找到最优切分特征j与切分点s。

式中，样本集中Δx小于s的样本被划分至区域R₁，Δx大于s的样本被划分至R₂。c₁和c₂分别是使R₁和R₂中样本损失函数之和最小的参数。

2.对于得到的最优切分(j,s)划分区域，并且得到输出值

R₁(j,s)＝{Δx|Δx_j≤s},R₂(j,s)＝{Δx|Δx_j>s}

式中，

为R_m区域中样本变量y的均值。

3.对两个子区域递归调用步骤1，2直到满足停止条件。

输入空间划分为M个部分R₁,R₂,…R_M，回归树模型表示为：

式中，I为单位矩阵，f(x)为回归树的输出。

设定GBDT算法中共包含6棵CART树，每颗树上有6个叶子节点。选择均方差L(dx,f(x))＝(dx-f(x))²作为GBDT算法的回归损失函数。运行GBDT算法得到AUV-Ⅰ和AUV-Ⅱ的模型决策树。

第2步：完成预测工作

AUV-Ⅰ通过水声通讯设备接收AUV-Ⅱ的控制输入量。AUV的运动模型如下：

上式中，f为AUV的运动模型，Δx(k),Δy(k)分别是k-1时刻到k时刻(dt时间)AUV在x方向和y方向走过的距离。ψ_k，ψ_k-1分别是k时刻和k-1时刻AUV的航向角。v是AUV的航行速度，G为偏航角。通过运动模型f计算dt时间内AUV-Ⅰ在x方向走过的距离Δx，并以此作为GBDT算法的输入，已经训练完毕的决策树输出AUV-Ⅰ在x方向走过的实际距离dx_v。同理，得到AUV-Ⅰ在y方向走过的实际距离dy_v，AUV-Ⅱ在x方向走过的实际距离dx_f，AUV-Ⅱ在y方向走过的实际距离dy_f。根据k-1时刻AUV-Ⅰ和AUV-Ⅱ的状态及GBDT算法的输出，得到k时刻AUV-Ⅰ和AUV-Ⅱ的预测状态，即：

K时刻状态估计误差协方差矩阵的计算方式同传统的EKF-SLAM，即：

第3步：完成更新工作

AUV-Ⅰ携带超短基线的应答器(transponder)，AUV-Ⅱ携带超短基线的收发器(transceiver)。AUV-Ⅱ解析得到AUV-Ⅰ在其坐标系下位置的横纵坐标，并通过水声通讯设备发送至AUV-Ⅰ的主控计算机。AUV-Ⅰ接收到观测值后，根据观测模型计算两者之间的估计测量值，并将真实测量值和估计测量值做差得到残差矩阵。计算卡尔曼增益，并更新AUV-Ⅰ和AUV-Ⅱ的状态和状态估计误差协方差。

采用两个AUV已完成动态对接的试验数据来验证本文定位算法的有效性。由于很难准确测得AUV水下航行的运动过程噪声及USBL的观测噪声，因此，噪声方差皆是根据经验估计所得。超短基线的定位误差根据作业距离的变化而变化。当作业距离大时，测量误差很大。因此，本文假设作业距离超过50米时，测量值由于误差太大不可信。当作业距离小于50米时，假设其观测误差的方差为[5 0；0 5]。AUV-Ⅰ和AUV-Ⅱ的运动过程噪声皆为[0.1² 0；01²]。试验数据分析结果如下：

1)AUV-Ⅰ的初始状态为[800 300 235°]^T，AUV-Ⅱ的初始状态为[630 185 235°]^T。AUV-Ⅰ和AUV-Ⅱ分别以1.5节和1节的期望速度航行。分别采用EKF-SLAM和GBDT-EKF-SLAM定位算法，定位结果如图4-5：

2)AUV-Ⅰ的初始状态为[800 300 235°]^T，AUV-Ⅱ的初始状态为[620 210 235°]^T。AUV-Ⅰ和AUV-Ⅱ分别以1.5节和1节的期望速度航行。分别采用EKF-SLAM和GBDT-EKF-SLAM定位算法，定位结果如图6-7：

3)AUV-Ⅰ的初始状态为[800 300 235°]^T，AUV-Ⅱ的初始状态为[624 154 235°]^T。AUV-Ⅰ和AUV-Ⅱ分别以1节和0.5节的期望速度航行。分别采用EKF-SLAM和GBDT-EKF-SLAM定位算法，定位结果如图8-9所示。

图4、6、8中蓝色线和黑色线分别表示AUV-Ⅰ和AUV-Ⅱ在x方向，y方向的估计轨迹。红色线表示由IMU记录的AUV-Ⅰ和AUV-Ⅱ的真实轨迹。图5、7、9中蓝色线和洋红色线分别表示采用基于EKF-SLAM和GBDT-EKF-SLAM估计位置与IMU记录位置的偏差。从上图可以看出，尽管USBL存在较大的观测噪声，基于EKF-SLAM和GBDT-EKF-SLAM的定位算法均将定位误差控制在了一个可接受范围内。并且基于GBDT-EKF-SLAM的定位效果优于原始的EKF-SLAM算法。

Claims (10)

1.一种用于两个运动AUV的同时定位方法，其特征在于包括以下步骤：

利用AUV的实际航行数据回归拟合AUV运动模型的梯度下降决策树，得到AUV单位时间模型航行里程到实际航行里程的映射关系；

定义两个AUV的初始状态和初始状态估计误差协方差并初始化；定义过程噪声和观测噪声的统计特性并初始化；

两个AUV相互通信共享其控制输入信息；根据两个AUV的控制输入量计算单位时间的模型航行里程，并根据上述映射关系得到AUV的实际航行里程，进而分别预测两个AUV的状态和状态估计误差协方差；

利用超短基线测出两个AUV在x、y方向的相对距离，并分别更新两个AUV的状态和状态估计误差协方差，从而实现两个运动AUV的同时定位。

2.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述AUV的实际航行数据包括：待回收AUV-Ⅰ和携带回收装置的AUV-Ⅱ在过去的实际航行中由传感器记录的AUV的速度v、方向ψ及位置信息(x,y)。

3.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述回归拟合AUV运动模型的梯度下降决策树包括：

通过AUV-Ⅰ的实际航行速度v_v、实际航向角ψ_v、单位时间dt以及运动模型f分别计算得到AUV-Ⅰ在x、y方向走过的距离Δx_v、Δy_v；利用AUV-Ⅰ搭载的传感器分别记录dt时间内在x、y方向走过的实际距离dx_v、dy_v；定义{(Δx_v,dx_v)}为第一个GBDT训练集的样本、{(Δy_v,dy_v)}为第二个GBDT训练集的样本；

通过AUV-Ⅱ的实际航行速度v_f、实际航向角ψ_f、单位时间dt以及运动模型f分别计算得到AUV-Ⅱ在x、y方向走过的距离Δx_f、Δy_f；利用AUV-Ⅱ搭载的传感器分别记录dt时间内在x、y方向走过的实际距离dx_f、dy_f；定义{(Δx_f,dx_f)}为第三个GBDT训练集的样本、{(Δy_f,dy_f)}为第四个GBDT训练集的样本；

利用梯度下降决策树算法训练上述四个GBDT训练集，分别得到Δx_v到dx_v、Δy_v,到dy_v、Δx_f到dx_f、Δy_f到dy_f的映射关系。

4.根据权利要求3所述的一种用于两个运动AUV的同时定位方法，其特征在于所述运动模型为：

式中，Δx(k)、Δy(k)分别是k-1时刻到k时刻内AUV在x方向和y方向走过的距离，ψ_k，ψ_k-1分别是k时刻和k-1时刻AUV的航向角，v是AUV的航行速度，G为偏航角。

5.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述定义两个AUV的初始状态和初始状态估计误差协方差并初始化包括：

定义EKF-SLAM算法的状态变量X(k)＝[X_v(k) X_f(k)]^T；式中，X_v(k)＝[x_v,k y_v,k ψ_v,k]^T是k时刻AUV-Ⅰ位置的横、纵坐标以及航向角的状态变量，X_f(k)＝[x_f,k y_f,k ψ_f,k]^T是k时刻AUV-Ⅱ位置的横、纵坐标以及航向角的状态变量；

根据AUV-Ⅰ和AUV-Ⅱ初始时刻的位置信息和航向角信息初始化状态变量X(k)；初始化k时刻状态估计误差协方差矩阵P(k)，初始时刻P(0)为6行6列的零矩阵。

6.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述定义过程噪声和观测噪声的统计特性并初始化包括：

假设系统过程噪声和观测噪声皆为零均值高斯白噪声，定义统计特性如下：

E[n₁(k)]＝0,E[n₁(k)n₁(k)^T]＝Q₁

E[n₂(k)]＝0,E[n₂(k)n₂(k)^T]＝Q₂

E[m(k)]＝0,E[m(k)m(k)^T]＝R

E[n₁(k)n₂(j)^T]＝0,E[n₁(k)m(j)^T]＝0,E[n₂(k)m(j)^T]＝0

其中，AUV-Ⅰ的高斯运动过程噪声为n₁，AUV-Ⅱ的高斯运动过程噪声为n₂，观测噪声为m，E表示期望，k、j表示时刻；

初始化：n₁的均值为零，方差为Q₁；n₂的均值为零，方差为Q₂；m的均值为零，方差为R；n₁、n₂和m之间彼此不相关，即它们之间的协方差为零。

7.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述两个AUV相互通信共享其控制输入信息为：

定义k时刻AUV-Ⅰ的控制输入变量u_v(k)＝[v_v，G_v]，AUV-Ⅱ的控制输入变量u_f(k)＝[v_f，G_f]；其中，V表示速度，G表示偏航角，下标v表示AUV-Ⅰ，下标f表示AUV-Ⅱ；

AUV-Ⅱ将其控制输入u_f(k)通过水声通讯设备发送至AUV-Ⅰ的主控计算机。

8.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述根据两个AUV的控制输入量计算单位时间的模型航行里程，并根据上述映射关系得到AUV的实际航行里程，进而分别预测两个AUV的状态和状态估计误差协方差：

根据AUV-Ⅰ和AUV-Ⅱ的控制输入变量u_v(k)＝[v_v，G_v]、u_f(k)＝[v_f，G_f]计算AUV-Ⅰ和AUV-Ⅱ单位时间的模型航行里程，并根据映射关系得到AUV-Ⅰ和AUV-Ⅱ在x、y方向单位时间dt内走过的距离dx_v，dy_v，dx_f，dy_f；

从而得到预测状态：

状态估计误差协方差的预测方程为：

其中，

为k-1时刻的状态估计值，

为k时刻的状态预测值；P(k-1)为k-1时刻的状态估计误差协方差，P(k|k-1)为k时刻的状态估计误差协方差的预测值，0_3×3为3行3列的零矩阵，0_2×2为2行2列的零矩阵，F_v(k)和F_f(k)分别是非线性运动方程f(·)对状态X_V和X_f在当前最优估计处的雅可比矩阵；W₁(k)和W₂(k)分别是非线性运动方程f对噪声n₁(k)和n₂(k)的雅可比矩阵。

9.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述利用超短基线测出两个AUV在x、y方向的相对距离为：AUV-Ⅱ携带超短基线的收发器，AUV-Ⅰ携带超短基线的应答器，AUV-Ⅱ分别解算两个AUV在x方向和y方向之间相对距离，并通过水声通讯设备发送至AUV-Ⅰ的主控计算机。

10.根据权利要求1所述的一种用于两个运动AUV的同时定位方法，其特征在于所述分别更新两个AUV的状态和状态估计误差协方差为：

a.当接收到最新的观测值Z(k)＝[Z(x)，Z(y)]^T时，计算观测值残差矩阵υ(k)及残差协方差矩阵S(k)和卡尔曼增益K(k)，如下：

S(k)＝H(k)P(k|k-1)H(k)^T+R

K(k)＝P(k|k-1)H^T(k)S(k)^-1

式中，H(k)是非线性传感器观测方程h对X(k)的偏导数的雅可比矩阵，非线性观测方程h为：

式中，ψ_f为AUV-Ⅱ的航向角，α为中间变量，x_v,y_v分别为AUV-Ⅰ位置的横纵坐标，x_f,y_f分别为AUV-Ⅱ位置的横纵坐标，Z(x)、Z(y)分别表示由相对距离转换得到的AUV-Ⅰ在AUV-Ⅱ坐标系下的位置的横纵坐标；

b.按照下式更新状态和状态估计误差协方差矩阵：

P(k)＝(I-K(k)H(k))P(k|k-1)

式中，I为单位矩阵，

为k时刻状态的估计值，P(k)为k时刻状估计误差协方差。

Images