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

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

【技术实现步骤摘要】
基于感知信息的水面/水下无人航行器目标环绕跟踪方法
本专利技术涉及目标跟踪
，具体地说是一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法。
技术介绍
水面/水下无人航行器作为探知海洋丰富资源的一种智能多功能探测载体，需要在无人状态下能够自主完成规划任务。目前，在水面/水下无人航行器和目标跟踪
，可实现对锁定目标进行基于感知信息下的目标跟踪任务，但在针对一些对目标信息要求更加具体的实际工程中，如对于突发事故的船只，人工无法靠近或抵达，不能获得其最新发展动态和及时处理，因此，需要设计出一种能够基于实时感知信息的对静态、动态目标进行360°环绕跟踪方法应用到水面/水下无人航行器上，以无人航行器为平台快速、高效、安全的完成任务，来提高相关工作的效率、安全性。
技术实现思路
针对现有技术的不足，本专利技术提供一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，解决对于突发事故的船只，人工无法靠近或抵达，不能获得其最新发展动态和及时处理的问题。本专利技术为实现上述目的所采用的技术方案是：一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，包括以下步骤：步骤1：在航行器运动过程中，由量测设备提供感知信息，所述感知信息包括目标位置、航向及航速；步骤2：对感知信息进行数据处理，并根据数据处理结果设定航行器速度和环绕半径；步骤3：根据目标位置、航向、航速、航行器的位置和姿态信息、航行器速度和环绕半径进行虚拟目标点划分，得到虚拟目标点序列；步骤4：根据航行器当前位置和姿态信息确定首虚拟目标点和环绕方向；步骤5：通过坐标变换，使航行器沿虚拟目标点序列进行运动，并通过切换判据，依次跟踪每个虚拟目标点，实现目标环绕跟踪。所述数据处理包括：对目标的航向和航速通过卡尔曼滤波算法进行滤波平滑处理，得到滤波后的目标航向和航速，并限制其单位时间内变化距离不大于L，L为距离参数。所述航行器速度的设定包括：输入量测目标速度，通过以下公式计算出航行器速度值，即：其中，va为量测目标速度，v为航行器速度值，e是自然对数的底数。所述环绕半径为：R＝floor(η1·v3+η2·v2+η3·v+η4)floor()为取数字整数函数，η1是三阶速度参数，η2是二阶速度参数，η3是速度变化率参数，η4是偏移参数，v为航行器速度值，R是环绕半径。还包括在航行过程中，当轨迹偏离预计航线时，对轨迹进行重规划，具体为：在已经切换过虚拟目标点条件下，进行虚拟目标点划分以及首虚拟目标点和环绕方向的重新确定。所述虚拟目标点划分包括：根据设定环绕半径、航行器速度，计算虚拟目标点数量：其中，R为设定环绕半径，v为航行器速度，λ1为比例系数，λ2为速度参数，λ3偏移参数。所述虚拟目标点序列为以目标位置为圆心，环绕半径为半径的圆与X轴正方向交点为起点，在该圆上等间隔提取N个点，顺时针或逆时针进行排序得到的点的集合。所述首虚拟目标点和环绕方向的确定包括：航行器初始位置PVehicle在圆内部时和航行器初始位置PVehicle在圆外部时；当航行器初始位置PVehicle在圆内部时：确定环绕方向，计算首虚拟目标点位置；环绕方向为：以中心点为原点建立运动坐标系，设航行器在运动坐标系下坐标点为(x,y)，当前目标航向角为θaim，当前无人航行器艏向角为θ，式中sign()为取数字符号函数，deal()为是对括号内的取值规范化(在[0,2π]区间)，H≥0时，环绕方向为顺时针，否则为逆时针；首虚拟目标点位置：其中，floor()为取数字整数函数，R为环绕半径，v为航行器速度值，λ1为比例系数，λ2为速度参数，λ3偏移参数，ψadddec为由航行器位置PVehicle、量测目标位置O、虚拟目标点位置Paim所形成的ΔOPVehiclePaim中的∠PaimOPVehicle值；当航行器初始位置PVehicle在圆外部时：确定环绕方向，计算首虚拟目标点位置；环绕方向：以航行器位置为原点，若航行器的艏向与y轴的正方向夹角小于90°，则环绕方向为顺时针，否则为逆时针；首虚拟目标点位置：在运动坐标系下，依据环绕方向确定切点位置P1，在虚拟目标点序列中距离P1最近的点则为首虚拟目标点。所述坐标变换为：将所有虚拟目标点在运动坐标系下的坐标转换为在大地坐标系下的坐标，即：已知在运动坐标系下的虚拟目标点序列为Paim[]，设转换矩阵为：deal()为是对括号内的取值规范化(在[0,2π]区间)，θaim为目标航向角，θ为当前无人航行器艏向角；此时虚拟目标点序列在大地坐标系下的虚拟目标点序列为：其中，Paim[]为运动坐标系下的虚拟目标点序列，Rexchange为转换矩阵。所述切换判据为：在大地坐标系下，根据当前跟踪虚拟目标点Paim[a]位置与航行器位置，及虚拟目标点在大地坐标系下的位置，实时调节航行器的期望艏向角，根据切换条件对虚拟目标点进行切换；所述切换条件为：(x＞x0)∩(y＞y0)其中，x、y分别为在虚拟目标坐标系下航行器位置的横、纵坐标，x0为x轴偏移参数，y0为y轴偏移参数；所述的虚拟目标坐标系为：以虚拟目标点为原点，以量测目标位置到虚拟目标点为原点方向为x轴正向，根据右手定则建立坐标系。本专利技术具有以下有益效果及优点：1.方法简单，应用范围广。本专利技术本专利技术仅需要一个配备感知设备的水面/水下无人航行器即可，适用于海洋上的有同样需求的各类航行器。2.安全稳定，可靠性高。能够根据感知信息实时调节环绕速度、环绕半径，结合重规划策略，即使在较差海况下，亦能够安全稳定的完成任务附图说明图1是本专利技术的动态虚拟目标点跟踪算法实现流程图；图2是本专利技术的动态虚拟目标点跟踪算法原理示意图；图3是本专利技术的初始位置位于设定半径圆内部的首虚拟目标点计算图；图4是本专利技术的初始位置位于设定半径圆外部的首虚拟目标点计算图；图5是本专利技术的动态虚拟目标点切换判据原理图。具体实施方式下面结合附图及实施例对本专利技术做进一步的详细说明。为使本专利技术的上述目的、特征和有点能够更加明显易懂，下面结合附图对本专利技术的具体实施方式做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本专利技术。但本专利技术能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背专利技术内涵的情况下做类似改进，因此本专利技术不受下面公开的具体实施的限制。需要说明的是，当元件被称为“设置于”另一个元件，它可能直接在另一个元件上，或也可以存在居中的元件。当一个元件被称为是“连接”另一个元件，它可以直接连接到另一个元件或者可能同时存在居中元件。本文所使用的属于“前”、“后”、“左”、“右”以及类似本文档来自技高网...

【技术保护点】
1.一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：包括以下步骤：/n步骤1：在航行器运动过程中，由量测设备提供感知信息，所述感知信息包括目标位置、航向及航速；/n步骤2：对感知信息进行数据处理，并根据数据处理结果设定航行器速度和环绕半径；/n步骤3：根据目标位置、航向、航速、航行器的位置和姿态信息、航行器速度和环绕半径进行虚拟目标点划分，得到虚拟目标点序列；/n步骤4：根据航行器当前位置和姿态信息确定首虚拟目标点和环绕方向；/n步骤5：通过坐标变换，使航行器沿虚拟目标点序列进行运动，并通过切换判据，依次跟踪每个虚拟目标点，实现目标环绕跟踪。/n

【技术特征摘要】
1.一种基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：包括以下步骤：
步骤1：在航行器运动过程中，由量测设备提供感知信息，所述感知信息包括目标位置、航向及航速；
步骤2：对感知信息进行数据处理，并根据数据处理结果设定航行器速度和环绕半径；
步骤3：根据目标位置、航向、航速、航行器的位置和姿态信息、航行器速度和环绕半径进行虚拟目标点划分，得到虚拟目标点序列；
步骤4：根据航行器当前位置和姿态信息确定首虚拟目标点和环绕方向；
步骤5：通过坐标变换，使航行器沿虚拟目标点序列进行运动，并通过切换判据，依次跟踪每个虚拟目标点，实现目标环绕跟踪。


2.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：所述数据处理包括：
对目标的航向和航速通过卡尔曼滤波算法进行滤波平滑处理，得到滤波后的目标航向和航速，并限制其单位时间内变化距离不大于L，L为距离参数。


3.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：所述航行器速度的设定包括：
输入量测目标速度，通过以下公式计算出航行器速度值，即：



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


4.根据权利要求1所述的基于感知信息的水面/水下无人航行器目标环绕跟踪方法，其特征在于：所述环绕半径为：
R＝floor(η1·v3+η2·v2+η3·v+η4)
floor()为取数字整数函数，η1是三阶速度参数，η2是二阶速度参数，η3是速度变化率参数，η4是偏移参数，v为航行器速度值，R是环绕半径。


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


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



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


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


8.根据权利要求1所述的基于感知信息的...

【专利技术属性】
技术研发人员：宋吉广，李德隆，冯亮，林扬，孙铁铭，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：辽宁;21