CN114200966A - 一种基于感知信息的无人航行器目标定方位等距跟踪方法 - Google Patents

Abstract

本发明涉及目标跟踪领域，具体说是一种基于感知信息的无人航行器目标定方位等距跟踪方法，包括以下步骤：1)航行器获取目标的感知信息和航行器的航行器信息；2)对感知信息进行数据处理，获取经滤波平滑处理后的感知信息；3)设定目标相对于航行器的方位，设定距离数值D，建立虚拟目标运动坐标系，通过坐标系变化矩阵，得到航行器在虚拟目标运动坐标系下的位置；4)航行器通过速度调节策略对期望速度进行调节；5)根据目标航向，航行器解算航行器的期望航向；6)将期望速度和期望航向输出形成闭环，重复步骤1)～5)。本发明安全稳定，可靠性高。能够根据感知信息实时调节航行速度、航向，即使在较差海况下，亦能够安全稳定的完成任务。

Description

一种基于感知信息的无人航行器目标定方位等距跟踪方法

技术领域

本发明涉及目标跟踪领域，具体说是一种基于感知信息的无人航行器目标定方位等距跟踪方法。

背景技术

水面/水下无人航行器作为探知海洋丰富资源的一种智能多功能探测设备搭载平台，需要在无人状态下能够自主完成规划任务。目前，在水面/水下无人航行器和目标跟踪技术领域，可实现对锁定目标进行基于感知信息下的目标跟踪任务，但在针对一些目标状态未知、目标运动状态变化较大且频繁、所处环境干扰较大的情况下，易出现跟踪状态不稳定、跟踪距离无法保证等状况。在实际工程中，如对高速机动且速度变化较频繁的航行器进行稳定跟踪，对友方航行器进行伴行时，缺乏较好的抗干扰能力，因此需要设计出一种能够基于实时感知信息的对高低频变速的水面/水下动态目标进行定方位等距跟踪方法应用到水面/水下无人航行器上，以无人航行器为平台快速、高效、安全的完成任务，来提高相关工作的效率、安全性、稳定性。

发明内容

针对现有技术的不足，本发明提供一种基于感知信息的水面/水下无人航行器目标定方位等距跟踪方法，解决对高低频变速的水面/水下动态目标进行定方位等距跟踪不够稳定的问题，尤其在目标状态未知、目标运动状态变化较大且频繁、所处环境干扰较大的情况下，易出现跟踪状态不稳定、跟踪距离无法保证等状况。

本发明为实现上述目的所采用的技术方案是：一种基于感知信息的无人航行器目标定方位等距跟踪方法，包括以下步骤：

1)航行器通过量测设备获取目标的感知信息；通过定位设备获取航行器的航行器信息；

2)航行器对获取目标的感知信息进行数据处理，得到卡尔曼滤波方法所需参数，根据卡尔曼滤波方法所需参数，航行器对感知信息进行卡尔曼滤波平滑处理，获取经滤波平滑处理后的感知信息；

3)设定目标相对于航行器的方位，设定距离数值D，根据目标相对于航行器的方位与设定距离数值，建立虚拟目标运动坐标系，通过坐标系变化矩阵，得到航行器在虚拟目标运动坐标系下的位置；

4)航行器通过速度调节策略对期望速度进行调节；

5)根据目标航向，航行器解算航行器的期望航向；

6)将期望速度输出至航行器的转速控制器进行控制，以及将期望航向输出至航行器的航向控制器进行控制，形成闭环，重复步骤1)～5)，以实现目标定方位等距跟踪。

所述感知信息包括，目标的中心位置、航向和航速；所述航行器信息，包括航行器的位置、航向和航速。

所述步骤2)具体为：

航行器将量测设备获取的目标的感知信息与航行器获取的目标的实际位置、航向和航速进行比对，得到卡尔曼滤波方法所需参数以及感知目标位置的最大变化距离D_max；

确定参数后，通过卡尔曼滤波方法对感知信息分别进行卡尔曼滤波平滑处理。

所述步骤3)中建立虚拟目标运动坐标系，具体为：

设定以目标为原点O，目标航向为Y轴正方向，建立目标运动坐标系XOY，设定目标相对于航行器的方位ψ_set及距离数值D，得到目标相对于航行器方位ψ_set和距离D的交点，即虚拟目标点，设定虚拟目标点为原点o，目标航向为坐标系y轴正方向，根据右手定则，建立虚拟目标运动坐标系xoy。

所述步骤3)中通过坐标系变化矩阵，得到航行器在虚拟目标运动坐标系下的位置所述航行器位置转至虚拟目标运动坐标系下位置时，坐标变化矩阵为：

其中，θ为经过卡尔曼滤波平滑处理的目标航向。

所述步骤4)中速度调节策略，具体为：

ΔV_v＝Kp·L_E+Ki·L_EA+Kd·L_EG

ΔV＝ΔV_v+ΔV_h

L_E＜L_{E_MAX},L_EA＜L_{EA_MAX}

其中，ΔV_v为航行器的纵向速度调节大小，ΔV_h为航行器横向速度调节大小，ΔV为航行器期望速度调节大小，p为横向距离比例值，x为虚拟目标运动坐标系下横坐标，x_set为距离参数，Kp为比例系数，Ki为积分系数，Kd为微分系数，L_E为纵向距离变化，L_EA为纵向距离变化积分项，L_EG为纵向距离变化微分项，L_{E_MAX}为纵向距离变化最大值，L_{EA_MAX}为纵向距离变化积分最大值；

将ΔV与当前速度叠加得到航行器的期望速度V_{Vehicle_Aim}。

航行器期望速度与目标速度之间的约束条件为：

设定航行器期望速度V_{Vehicle_Aim}，与目标速度有如下关系：

fabs(V_{Vehicle_Aim}-V_Aim)≤V_Set

其中，fabs()为取浮点型数据绝对值，V_{Vehicle_Aim}为航行器期望速度，V_Aim为目标的速度，V_Set为设定的航行器与目标的速度间的最大差值，V_{Aim_Set1}为设定的目标的速度阈值，V_Set1为低于速度阈值时的设定的航行器与目标的速度间的最大差值V_Set值，V_Set2为不低于速度节点时的设定的航行器与目标的速度间的最大差值V_Set值；

同时，依据航行器实航速度，设定最大期、最小期望速度，航行器期望速度满足以下关系式：

V_{Vehicle_Aim_MIN}≤V_{Vehicle_Aim}≤V_{Vehicle_Aim_MAX}。

所述步骤5)中，具体为：

当前航行器的位置与目标的位置相关，航行器的期望航向具体为以下关系式表示：

x_virtual＝x_{Aim_virtual}

y_virtual＝α·y_{Aim_virtual}+β

ψ_{Aim_virtual}＝atan2(y_virtual-y,x_virtual-x)

ψ_Aim＝Angtrans(ψ+ψ_{Aim_virtual}-π/2)

其中，Angtrans()为将航向规范至[-π,π]区间的转换函数，ψ_Aim为航行器在大地坐标系下的期望航向，ψ为航行器在大地坐标系下的航向，ψ_{Aim_virtual}为航行器在虚拟目标运动坐标系下的期望航向，atan2()为求取正切值函数，x_virtual和y_virtual为中间变量，x为航行器在虚拟目标运动坐标系下横坐标，y为航行器在虚拟目标运动坐标系下纵坐标，x_{Aim_virtual}为目标在虚拟目标运动坐标系下的横坐标，y_{Aim_virtual}为目标在虚拟目标运动坐标系下的纵坐标，α为比例系数，β为距离偏移量；

所述Angtrans()转换函数中内容为：将输入变量取相反数后加上

所得角度转换至[-π,π]区间中，输出结果即为期望航向。

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

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

2.安全稳定，可靠性高。能够根据感知信息实时调节航行速度、航向，即使在较差海况下，亦能够安全稳定的完成任务。

附图说明

图1是本发明的目标跟踪算法实现流程图；

图2是本发明的目标跟踪算法原理示意图。

具体实施方式

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

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

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

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

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

实现流程为：

在航行器运动过程中，由量测设备提供感知目标信息，由定位设备提供航行器信息，输入信息包括目标位置经纬度、航向及航速，航行器位置经纬度、航向及速度。首先对输入的目标信息进行数据处理，然后根据设定的跟踪目标相对方位和距离确立虚拟目标点，以虚拟目标点为原点，目标运动方向为Y轴正方向，根据右手定则建立虚拟目标运动坐标系，并通过坐标变换解算航行器在该坐标系下的位置，在校准源设备中获取航行器速度、航向信息，通过速度调节策略输出航行器的期望速度，经过转速控制器实现速度调节闭环控制，通过目标航向解算输出航行器期望航向，以航向控制器实现航向调节闭环控制。由此，实现对目标的稳定定方位等距跟踪。

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

图中O为量测目标的位置，以O为原点、目标航向为Y轴正向，根据右手定则建立运动坐标系XOY，设定跟踪目标相对方位ψ_Set、距离为D，得到虚拟目标点P_{Aim_Virtual}，即o，以o为原点、量测目标艏向为y轴正向，根据右手定则建立虚拟目标运动坐标系xoy。设航行器经过坐标变换后在该虚拟目标运动坐标系下的坐标为(x,y)，然后，通过速度调节策略和目标航向解算出期望转速、航向，经过转速控制器、航向控制器实现闭环控制，达到对目标进行稳定跟踪的目的。

目标跟踪方法包括：

步骤一：

输入数据处理说明如下：

初次实验中，航行器将量测设备获取的目标的感知信息与航行器获取的目标的实际位置、航向和航速进行比对，以获得最优的卡尔曼滤波方法所需参数、感知目标位置的最大变化距离D_Max，D_Max为距离参数。所述的所需参数包括：过程激励噪声的协方差Q、测量噪声协方差R、先验估计协方差P。然后，对目标的航向和航速进行卡尔曼滤波平滑处理，并限制其单位时间内变化距离不大于η*D_Max，η为比例系数。

步骤二：

建立坐标系，并进行坐标变换说明如下：

如图2所示是本发明的目标跟踪算法原理示意图，图中O为量测目标的位置，以O为原点、目标航向为Y轴正向，根据右手定则建立运动坐标系XOY，设定跟踪目标相对方位ψ_Set、距离为D，得到虚拟目标点P_{Aim_Virtual}，即o，以o为原点、量测目标艏向为y轴正向，根据右手定则建立虚拟目标运动坐标系xoy。其中，航行器位置转至虚拟目标运动坐标系下位置时，坐标变化矩阵为：

其中，θ为经过卡尔曼滤波平滑处理的目标航向。

步骤三：速度调节策略说明如下：

ΔV_v＝Kp·L_E+Ki·L_EA+Kd·L_EG

ΔV＝ΔV_v+ΔV_h

L_E＜L_{E_MAX},L_EA＜L_{EA_MAX}

其中，ΔV_v为航行器的纵向速度调节大小，ΔV_h为航行器横向速度调节大小，ΔV为航行器期望速度调节大小，p为横向距离比例值，x为虚拟目标运动坐标系下横坐标，x_set为距离参数，Kp为比例系数，Ki为积分系数，Kd为微分系数，L_E为纵向距离变化，L_EA为纵向距离变化积分项，L_EG为纵向距离变化微分项，L_{E_MAX}为纵向距离变化最大值，L_{EA_MAX}为纵向距离变化积分最大值；

将ΔV与当前速度叠加得到航行器的期望速度V_{Vehicle_Aim}。

航行器实时速度调节是通过解算其在虚拟目标运动坐标系下的位置进行设计的，具体为：设定航行器期望速度V_{Vehicle_Aim}，与目标速度有如下关系：

fabs(V_{Vehicle_Aim}-V_Aim)≤V_Set

其中，fabs()为取浮点型数据绝对值，V_{Vehicle_Aim}为航行器期望速度，V_Aim为目标的速度，V_Set为设定的航行器与目标的速度间的最大差值，V_{Aim_Set1}为设定的目标的速度阈值，V_Set1为低于速度阈值时的设定的航行器与目标的速度间的最大差值V_Set值，V_Set2为不低于速度节点时的设定的航行器与目标的速度间的最大差值V_Set值；

同时，依据航行器实航速度，设定最大期、最小期望速度，航行器期望速度满足以下关系式：

V_{Vehicle_Aim_MIN}≤V_{Vehicle_Aim}≤V_{Vehicle_Aim_MAX}。

步骤四：

目标航向计算说明如下：

当前航行器的位置与目标的位置相关，航行器的期望航向具体为以下关系式表示：

x_virtual＝x_{Aim_virtual}

y_virtual＝α·y_{Aim_virtual}+β

ψ_{Aim_virtual}＝atan2(y_virtual-y,x_virtual-x)

ψ_Aim＝Angtrans(ψ+ψ_{Aim_virtual}-π/2)

其中，Angtrans()为将航向规范至[-π,π]区间的转换函数，ψ_Aim为航行器在大地坐标系下的期望航向，ψ为航行器在大地坐标系下的航向，ψ_{Aim_virtual}为航行器在虚拟目标运动坐标系下的期望航向，atan2()为求取正切值函数，x_virtual和y_virtual为中间变量，x为航行器在虚拟目标运动坐标系下横坐标，y为航行器在虚拟目标运动坐标系下纵坐标，x_{Aim_virtual}为目标在虚拟目标运动坐标系下的横坐标，y_{Aim_virtual}为目标在虚拟目标运动坐标系下的纵坐标，α为比例系数，β为距离偏移量；

所述Angtrans()转换函数中内容为：将输入变量取相反数后加上

所得角度转换至[-π,π]区间中，输出结果即为期望航向。

步骤五：算法输出经过控制计算，通过转速控制器、航向控制器控制执行机构，实时调节航行器速度、艏向。重复步骤一至步骤五，即可实现对基于感知信息的目标进行定方位等距跟踪。

Claims (8)

1.一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，包括以下步骤：

1)航行器通过量测设备获取目标的感知信息；通过定位设备获取航行器的航行器信息；

2)航行器对获取目标的感知信息进行数据处理，得到卡尔曼滤波方法所需参数，根据卡尔曼滤波方法所需参数，航行器对感知信息进行卡尔曼滤波平滑处理，获取经滤波平滑处理后的感知信息；

3)设定目标相对于航行器的方位，设定距离数值D，根据目标相对于航行器的方位与设定距离数值，建立虚拟目标运动坐标系，通过坐标系变化矩阵，得到航行器在虚拟目标运动坐标系下的位置；

4)航行器通过速度调节策略对期望速度进行调节；

5)根据目标航向，航行器解算航行器的期望航向；

6)将期望速度输出至航行器的转速控制器进行控制，以及将期望航向输出至航行器的航向控制器进行控制，形成闭环，重复步骤1)～5)，以实现目标定方位等距跟踪。

2.根据权利要求1所述的一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，所述感知信息包括，目标的中心位置、航向和航速；所述航行器信息，包括航行器的位置、航向和航速。

3.根据权利要求1或2所述的一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，所述步骤2)具体为：

航行器将量测设备获取的目标的感知信息与航行器获取的目标的实际位置、航向和航速进行比对，得到卡尔曼滤波方法所需参数以及感知目标位置的最大变化距离D_max；

确定参数后，通过卡尔曼滤波方法对感知信息分别进行卡尔曼滤波平滑处理。

4.根据权利要求1所述的一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，所述步骤3)中建立虚拟目标运动坐标系，具体为：

设定以目标为原点O，目标航向为Y轴正方向，建立目标运动坐标系XOY，设定目标相对于航行器的方位ψ_set及距离数值D，得到目标相对于航行器方位ψ_set和距离D的交点，即虚拟目标点，设定虚拟目标点为原点o，目标航向为坐标系y轴正方向，根据右手定则，建立虚拟目标运动坐标系xoy。

5.根据权利要求1所述的一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，所述步骤3)中通过坐标系变化矩阵，得到航行器在虚拟目标运动坐标系下的位置所述

航行器位置转至虚拟目标运动坐标系下位置时，坐标变化矩阵为：

其中，θ为经过卡尔曼滤波平滑处理的目标航向。

6.根据权利要求1所述的一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，所述步骤4)中速度调节策略，具体为：

ΔV_v＝Kp·L_E+Ki·L_EA+Kd·L_EG

ΔV＝ΔV_v+ΔV_h

L_E＜L_{E_MAX},L_EA＜L_{EA_MAX}

其中，ΔV_v为航行器的纵向速度调节大小，ΔV_h为航行器横向速度调节大小，ΔV为航行器期望速度调节大小，p为横向距离比例值，x为虚拟目标运动坐标系下横坐标，x_set为距离参数，Kp为比例系数，Ki为积分系数，Kd为微分系数，L_E为纵向距离变化，L_EA为纵向距离变化积分项，L_EG为纵向距离变化微分项，L_{E_MAX}为纵向距离变化最大值，L_{EA_MAX}为纵向距离变化积分最大值；

将ΔV与当前速度叠加得到航行器的期望速度V_{Vehicle_Aim}。

7.根据权利要求6所述的一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，航行器期望速度与目标速度之间的约束条件为：

设定航行器期望速度V_{Vehicle_Aim}，与目标速度有如下关系：

fabs(V_{Vehicle_Aim}-V_Aim)≤V_Set

其中，fabs()为取浮点型数据绝对值，V_{Vehicle_Aim}为航行器期望速度，V_Aim为目标的速度，V_Set为设定的航行器与目标的速度间的最大差值，V_{Aim_Set1}为设定的目标的速度阈值，V_Set1为低于速度阈值时的设定的航行器与目标的速度间的最大差值V_Set值，V_Set2为不低于速度节点时的设定的航行器与目标的速度间的最大差值V_Set值；

同时，依据航行器实航速度，设定最大期、最小期望速度，航行器期望速度满足以下关系式：

V_{Vehicle_Aim_MIN}≤V_{Vehicle_Aim}≤V_{Vehicle_Aim_MAX}。

8.根据权利要求1所述的一种基于感知信息的无人航行器目标定方位等距跟踪方法，其特征在于，所述步骤5)中，具体为：

当前航行器的位置与目标的位置相关，航行器的期望航向具体为以下关系式表示：

x_virtual＝x_{Aim_virtual}

y_virtual＝α·y_{Aim_virtual}+β

ψ_{Aim_virtual}＝atan2(y_virtual-y,x_virtual-x)

ψ_Aim＝Angtrans(ψ+ψ_{Aim_virtual}-π/2)

其中，Angtrans()为将航向规范至[-π,π]区间的转换函数，ψ_Aim为航行器在大地坐标系下的期望航向，ψ为航行器在大地坐标系下的航向，ψ_{Aim_virtual}为航行器在虚拟目标运动坐标系下的期望航向，atan2()为求取正切值函数，x_virtual和y_virtual为中间变量，x为航行器在虚拟目标运动坐标系下横坐标，y为航行器在虚拟目标运动坐标系下纵坐标，x_{Aim_virtual}为目标在虚拟目标运动坐标系下的横坐标，y_{Aim_virtual}为目标在虚拟目标运动坐标系下的纵坐标，α为比例系数，β为距离偏移量；

所述Angtrans()转换函数中内容为：将输入变量取相反数后加上

所得角度转换至[-π,π]区间中，输出结果即为期望航向。

Images