CN111207748A - 基于感知信息的水面/水下无人航行器目标环绕跟踪方法 - Google Patents

Abstract

本发明涉及一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，包括在航行器运动过程中，由量测设备提供感知信息,对感知信息进行数据处理，并根据数据处理结果设定航行器速度和环绕半径,进行虚拟目标点划分，得到虚拟目标点序列；根据航行器当前位置和姿态信息确定首虚拟目标点和环绕方向；通过坐标变换，使航行器沿虚拟目标点序列进行运动，并通过切换判据，依次跟踪每个虚拟目标点，实现目标环绕跟踪。本发明操作简单，实用性强，具有较强的抗干扰能力，其必要参数设置规则简单且有规律，可较大程度提高工作效率，节约时间和人工成本。

Description

基于感知信息的水面/水下无人航行器目标环绕跟踪方法

技术领域

本发明涉及目标跟踪技术领域，具体地说是一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法。

背景技术

水面/水下无人航行器作为探知海洋丰富资源的一种智能多功能探测载体，需要在无人状态下能够自主完成规划任务。目前，在水面/水下无人航行器和目标跟踪技术领域，可实现对锁定目标进行基于感知信息下的目标跟踪任务，但在针对一些对目标信息要求更加具体的实际工程中，如对于突发事故的船只，人工无法靠近或抵达，不能获得其最新发展动态和及时处理，因此，需要设计出一种能够基于实时感知信息的对静态、动态目标进行360°环绕跟踪方法应用到水面/水下无人航行器上，以无人航行器为平台快速、高效、安全的完成任务，来提高相关工作的效率、安全性。

发明内容

针对现有技术的不足，本发明提供一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，解决对于突发事故的船只，人工无法靠近或抵达，不能获得其最新发展动态和及时处理的问题。

本发明为实现上述目的所采用的技术方案是：

一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，包括以下步骤：

步骤1：在航行器运动过程中，由量测设备提供感知信息，所述感知信息包括目标位置、航向及航速；

步骤2：对感知信息进行数据处理，并根据数据处理结果设定航行器速度和环绕半径；

步骤3：根据目标位置、航向、航速、航行器的位置和姿态信息、航行器速度和环绕半径进行虚拟目标点划分，得到虚拟目标点序列；

步骤4：根据航行器当前位置和姿态信息确定首虚拟目标点和环绕方向；

步骤5：通过坐标变换，使航行器沿虚拟目标点序列进行运动，并通过切换判据，依次跟踪每个虚拟目标点，实现目标环绕跟踪。

所述数据处理包括：

对目标的航向和航速通过卡尔曼滤波算法进行滤波平滑处理，得到滤波后的目标航向和航速，并限制其单位时间内变化距离不大于L，L为距离参数。

所述航行器速度的设定包括：

输入量测目标速度，通过以下公式计算出航行器速度值，即：

其中，v_a为量测目标速度，v为航行器速度值，e是自然对数的底数。

所述环绕半径为：

R＝floor(η₁·v³+η₂·v²+η₃·v+η₄)

floor()为取数字整数函数，η₁是三阶速度参数，η₂是二阶速度参数，η₃是速度变化率参数，η₄是偏移参数，v为航行器速度值，R是环绕半径。

还包括在航行过程中，当轨迹偏离预计航线时，对轨迹进行重规划，具体为：在已经切换过虚拟目标点条件下，进行虚拟目标点划分以及首虚拟目标点和环绕方向的重新确定。

所述虚拟目标点划分包括：

根据设定环绕半径、航行器速度，计算虚拟目标点数量：

其中，R为设定环绕半径，v为航行器速度，λ₁为比例系数，λ₂为速度参数，λ₃偏移参数。

所述虚拟目标点序列为以目标位置为圆心，环绕半径为半径的圆与X轴正方向交点为起点，在该圆上等间隔提取N个点，顺时针或逆时针进行排序得到的点的集合。

所述首虚拟目标点和环绕方向的确定包括：航行器初始位置P_Vehicle在圆内部时和航行器初始位置P_Vehicle在圆外部时；

当航行器初始位置P_Vehicle在圆内部时：确定环绕方向，计算首虚拟目标点位置；

环绕方向为：以中心点为原点建立运动坐标系，设航行器在运动坐标系下坐标点为(x,y)，当前目标航向角为θ_aim，当前无人航行器艏向角为θ，

式中sign()为取数字符号函数，deal()为是对括号内的取值规范化(在[0,2π]区间)，H≥0时，环绕方向为顺时针，否则为逆时针；

首虚拟目标点位置：

其中，floor()为取数字整数函数，R为环绕半径，v为航行器速度值，λ₁为比例系数，λ₂为速度参数，λ₃偏移参数，ψ_adddec为由航行器位置P_Vehicle、量测目标位置O、虚拟目标点位置P_aim所形成的ΔOP_VehicleP_aim中的∠P_aimOP_Vehicle值；

当航行器初始位置P_Vehicle在圆外部时：确定环绕方向，计算首虚拟目标点位置；

环绕方向：以航行器位置为原点，若航行器的艏向与y轴的正方向夹角小于90°，则环绕方向为顺时针，否则为逆时针；

首虚拟目标点位置：在运动坐标系下，依据环绕方向确定切点位置P₁，在虚拟目标点序列中距离P₁最近的点则为首虚拟目标点。

所述坐标变换为：

将所有虚拟目标点在运动坐标系下的坐标转换为在大地坐标系下的坐标，即：

已知在运动坐标系下的虚拟目标点序列为P_aim[]，设转换矩阵为：

deal()为是对括号内的取值规范化(在[0,2π]区间)，θ_aim为目标航向角，θ为当前无人航行器艏向角；

此时虚拟目标点序列在大地坐标系下的虚拟目标点序列

为：

其中，P_aim[]为运动坐标系下的虚拟目标点序列，R_exchange为转换矩阵。

所述切换判据为：在大地坐标系下，根据当前跟踪虚拟目标点P_aim[a]位置与航行器位置，及虚拟目标点在大地坐标系下的位置，实时调节航行器的期望艏向角，根据切换条件对虚拟目标点进行切换；

所述切换条件为：

(x＞x₀)∩(y＞y₀)

其中，x、y分别为在虚拟目标坐标系下航行器位置的横、纵坐标，x₀为x轴偏移参数，y₀为y轴偏移参数；

所述的虚拟目标坐标系为：

以虚拟目标点为原点，以量测目标位置到虚拟目标点为原点方向为x轴正向，根据右手定则建立坐标系。

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

1.方法简单，应用范围广。本发明本发明仅需要一个配备感知设备的水面/水下无人航行器即可，适用于海洋上的有同样需求的各类航行器。

2.安全稳定，可靠性高。能够根据感知信息实时调节环绕速度、环绕半径，结合重规划策略，即使在较差海况下，亦能够安全稳定的完成任务

附图说明

图1是本发明的动态虚拟目标点跟踪算法实现流程图；

图2是本发明的动态虚拟目标点跟踪算法原理示意图；

图3是本发明的初始位置位于设定半径圆内部的首虚拟目标点计算图；

图4是本发明的初始位置位于设定半径圆外部的首虚拟目标点计算图；

图5是本发明的动态虚拟目标点切换判据原理图。

具体实施方式

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

为使本发明的上述目的、特征和有点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但本发明能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施的限制。

需要说明的是，当元件被称为“设置于”另一个元件，它可能直接在另一个元件上，或也可以存在居中的元件。当一个元件被称为是“连接”另一个元件，它可以直接连接到另一个元件或者可能同时存在居中元件。本文所使用的属于“前”、“后”、“左”、“右”以及类似的标书只是为了说明的目的，并不表示唯一的实施方式。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

如图1所示为本发明的动态虚拟目标点跟踪算法实现流程图。

实现流程为：

在航行器运动过程中，由量测设备提供感知目标信息，输入信息包括目标位置经纬度、航向及航速。首先对输入的目标信息进行数据处理，然后通过输入的当前目标信息中的深度，通过已知的公式进行航行器速度设定，同时通过国公式解算出环绕半径，完成虚拟目标点划分、首虚拟目标点选取及环行方向选择以及坐标变换，计算各点在运动坐标系中的位置。航行器在运动过程中实时计算到达虚拟目标点所需的航向角，通过航向控制器完成航向闭环，配合重规划，通过虚拟目标点切换判据，航行器依次跟踪有序虚拟目标点，进而实现对目标的环绕跟踪控制。

如图2所示是本发明的动态虚拟目标点跟踪算法原理示意图。

图中O为初始时刻量测目标的位置，以O为原点、东向为x轴正向、北向为y轴正向建立大地坐标系X_EOY_E，以O为原点、量测目标艏向为y轴正向，根据右手定则建立初始运动坐标系X_SOY_S，建立大地坐标系X_EOY_E原点。更新虚拟目标点信息。以o_a为原点、

方向为x轴，根据右手定则建立初始虚拟目标运动坐标系，航行器朝当前所跟踪的虚拟目标点运动。O₁为下一时刻量测目标的位置，以O₁为原点、量测目标艏向为y轴正向，根据右手定则建立下一时刻运动坐标系X'_SOY_S'，更新此时的虚拟目标点在X'_SOY_S'坐标系下的信息。以o_a1为原点、

方向为x轴，根据右手定则建立初始虚拟目标运动坐标系，设航行器在此坐标系下坐标为(x,y)，然后通过切换判据判定是否对虚拟目标点进行切换。重复以上操作，完成对有序的虚拟目标点序列的依次跟踪，终实现对基于感知信息的目标进行环绕跟踪。

目标环绕跟踪方法包括：

步骤一：

输入数据处理说明如下：

首先对目标航向、速度进行滤波平滑处理，方法选用卡尔曼滤波算法，结合量测感知设备精度，调整滤波算法设定系数，得到更加准确稳定的目标信息。位置信息的准确性直接决定着环绕跟踪的成功与否，因此，结合实际情况，对目标速度进行合理化限制，重新计算当前目标的位置。

步骤二：

输入数据处理中的航行器速度设定说明如下：

输入量测目标速度，通过以下公式计算出航行器速度值，即：

其中，v_a为量测目标速度，v为航行器速度值。e是自然对数的底数。

步骤三：

环绕半径设置说明如下：

R＝floor(η₁·v³+η₂·v²+η₃·v+η₄)

floor()为取数字整数函数，η₁是三阶速度参数，η₂是二阶速度参数，η₃是速度变化率参数，η₄是偏移参数，v为航行器速度值，R是环绕半径。

步骤四：

虚拟目标点划分方法说明如下：

根据设定环绕半径、航行器速度，计算虚拟目标点数量：

其中，R为设定环绕半径，v为航行器速度，λ₁为比例系数，λ₂为速度参数，λ₃偏移参数。

步骤五：

所述的首虚拟目标点选取及环行方向选择是环行方向与首虚拟目标点的选取相关，环绕方向一旦确定，则不再改变。对于首个虚拟目标点的选择，将根据航行器初始位置在设定半径圆内部或外部两种情况进行分析。

如图3所示是本发明的初始位置位于设定半径圆内部的首虚拟目标点计算图。

当航行器初始位置P_Vehicle在圆内部时，应先确定环绕方向以及虚拟目标点序列P_aim[]，之后再计算首虚拟目标点位置。

环绕方向根据航行器艏向、位置、中心点位置进行判定。以中心点为原点建立运动坐标系。设航行器在运动坐标系下坐标点为(x,y)，当前目标航向角为θ_aim，当前无人航行器艏向角为θ。

式中sign为取数字符号函数，deal为是对括号内的取值规范化，H≥0时，环绕方向为顺时针，否则为逆时针。

根据相对位置关系计算首虚拟目标点位置。设首个虚拟目标点位置为P_aim[a]，则首虚拟目标点索引a求解为：

其中，floor()为取数字整数函数，R为环绕半径，v为航行器速度值，λ₁为比例系数，λ₂为速度参数，λ₃偏移参数，

为由航行器位置P_Vehicle、量测目标位置O、虚拟目标点位置P_aim所形成的ΔOP_VehicleP_aim中的∠P_aimOP_Vehicle值。

如图4所示为初始位置位于设定半径圆外部的首虚拟目标点计算图。

当航行器初始位置ψ_usv在圆外部时，同样需要先确定环绕方向以及虚拟目标点序列P_aim[]，之后再计算首虚拟目标点位置。

以航行器位置为原点，若航行器的艏向与y轴的正方向夹角小于90°，则环绕方向为顺时针，否则为逆时针。虚拟目标点序列计算方法相同。

在运动坐标系下，依据环绕方向确定切点位置P₁，在虚拟目标点序列中距离P₁最近的点则为首虚拟目标点。为保证航行器快速的进入目标环绕状态，将虚拟目标点位置沿当前目标位置至当前虚拟目标点位置方向移动距离P_Aim1P_First，形成外包围式渐进逼近期望路线，达到快速稳定效果。

步骤六：

坐标变换，说明如下：

所述的坐标变换求取运动坐标系下所有虚拟目标点对应的坐标，在需要时转换为大地坐标系参与计算。

已知在运动坐标系下的虚拟目标点序列为P_aim[]，设转换矩阵为：

其中，deal()为是对括号内的取值规范化(在[0,2π]区间)，θ_aim为目标航向角，θ为运动坐标系y轴正向与东向的夹角，即当前无人航行器艏向角。此时虚拟目标点序列在大地坐标系下的虚拟目标点序列

为：

其中，P_aim[]为运动坐标系下的虚拟目标点序列，R_exchange为转换矩阵。

步骤七：

重规划，说明如下：

所述的重规划是在航行过程中，轨迹偏离预计航线，无法实现较好效果的目标环绕。需要在已经切换过虚拟目标点条件下，进行虚拟目标点划分以及首虚拟目标点的重新选择。

步骤八：

如图5所示是本发明的动态虚拟目标点切换判据原理图。

所述的虚拟目标点切换判据是在大地坐标系下，根据当前跟踪虚拟目标点位置与航行器位置以及虚拟目标点在大地坐标系下的位置，实时调节航行器的期望艏向角，设定切换判据实现对虚拟目标点的切换，进而实现对目标的环绕跟踪。以虚拟目标点为原点，以量测目标位置到虚拟目标点为原点方向为x轴正向，根据右手定则建立虚拟目标坐标系。设当前航行器在该坐标系下坐标为(x,y)，切换条件为：

(x＞x₀)∩(y＞y₀)

其中，x、y分别为在虚拟目标坐标系下航行器位置的横、纵坐标，x₀为x轴偏移参数，y₀为y轴偏移参数。

步骤九：

算法输出经过控制计算，通过水平面控制器控制执行机构，实时调节航行器艏向。重复步骤一至步骤九，即可实现对基于感知信息的目标进行环绕跟踪。

Claims (10)

1.一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：包括以下步骤：

步骤1：在航行器运动过程中，由量测设备提供感知信息，所述感知信息包括目标位置、航向及航速；

步骤2：对感知信息进行数据处理，并根据数据处理结果设定航行器速度和环绕半径；

步骤3：根据目标位置、航向、航速、航行器的位置和姿态信息、航行器速度和环绕半径进行虚拟目标点划分，得到虚拟目标点序列；

步骤4：根据航行器当前位置和姿态信息确定首虚拟目标点和环绕方向；

步骤5：通过坐标变换，使航行器沿虚拟目标点序列进行运动，并通过切换判据，依次跟踪每个虚拟目标点，实现目标环绕跟踪。

2.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：所述数据处理包括：

对目标的航向和航速通过卡尔曼滤波算法进行滤波平滑处理，得到滤波后的目标航向和航速，并限制其单位时间内变化距离不大于L，L为距离参数。

3.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：所述航行器速度的设定包括：

输入量测目标速度，通过以下公式计算出航行器速度值，即：

其中，v_a为量测目标速度，v为航行器速度值，e是自然对数的底数。

4.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：所述环绕半径为：

R＝floor(η₁·v³+η₂·v²+η₃·v+η₄)

floor()为取数字整数函数，η₁是三阶速度参数，η₂是二阶速度参数，η₃是速度变化率参数，η₄是偏移参数，v为航行器速度值，R是环绕半径。

5.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，还包括在航行过程中，当轨迹偏离预计航线时，对轨迹进行重规划，具体为：在已经切换过虚拟目标点条件下，进行虚拟目标点划分以及首虚拟目标点和环绕方向的重新确定。

6.根据权利要求1或5所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，所述虚拟目标点划分包括：

根据设定环绕半径、航行器速度，计算虚拟目标点数量：

其中，R为设定环绕半径，v为航行器速度，λ₁为比例系数，λ₂为速度参数，λ₃偏移参数。

7.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，所述虚拟目标点序列为以目标位置为圆心，环绕半径为半径的圆与X轴正方向交点为起点，在该圆上等间隔提取N个点，顺时针或逆时针进行排序得到的点的集合。

8.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，所述首虚拟目标点和环绕方向的确定包括：航行器初始位置P_Vehicle在圆内部时和航行器初始位置P_Vehicle在圆外部时；

当航行器初始位置P_Vehicle在圆内部时：确定环绕方向，计算首虚拟目标点位置；

环绕方向为：以中心点为原点建立运动坐标系，设航行器在运动坐标系下坐标点为(x,y)，当前目标航向角为θ_aim，当前无人航行器艏向角为θ，

式中sign()为取数字符号函数，deal()为是对括号内的取值规范化(在[0,2π]区间)，H≥0时，环绕方向为顺时针，否则为逆时针；

首虚拟目标点位置：

其中，floor()为取数字整数函数，R为环绕半径，v为航行器速度值，λ₁为比例系数，λ₂为速度参数，λ₃偏移参数，

为由航行器位置P_Vehicle、量测目标位置O、虚拟目标点位置P_aim所形成的ΔOP_VehicleP_aim中的∠P_aimOP_Vehicle值；

当航行器初始位置P_Vehicle在圆外部时：确定环绕方向，计算首虚拟目标点位置；

环绕方向：以航行器位置为原点，若航行器的艏向与y轴的正方向夹角小于90°，则环绕方向为顺时针，否则为逆时针；

首虚拟目标点位置：在运动坐标系下，依据环绕方向确定切点位置P₁，在虚拟目标点序列中距离P₁最近的点则为首虚拟目标点。

9.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，所述坐标变换为：

将所有虚拟目标点在运动坐标系下的坐标转换为在大地坐标系下的坐标，即：

已知在运动坐标系下的虚拟目标点序列为P_aim[]，设转换矩阵为：

deal()为是对括号内的取值规范化(在[0,2π]区间)，θ_aim为目标航向角，θ为当前无人航行器艏向角；

此时虚拟目标点序列在大地坐标系下的虚拟目标点序列

为：

其中，P_aim[]为运动坐标系下的虚拟目标点序列，R_exchange为转换矩阵。

10.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，所述切换判据为：在大地坐标系下，根据当前跟踪虚拟目标点P_aim[a]位置与航行器位置，及虚拟目标点在大地坐标系下的位置，实时调节航行器的期望艏向角，根据切换条件对虚拟目标点进行切换；

所述切换条件为：

(x＞x₀)∩(y＞y₀)

其中，x、y分别为在虚拟目标坐标系下航行器位置的横、纵坐标，x₀为x轴偏移参数，y₀为y轴偏移参数；

所述的虚拟目标坐标系为：

以虚拟目标点为原点，以量测目标位置到虚拟目标点为原点方向为x轴正向，根据右手定则建立坐标系。

Images