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

权利要求书

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

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

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

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

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

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

所述首虚拟目标点和环绕方向的确定包括：航行器初始位置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₁最近的点则为首虚拟目标点。

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

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

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

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

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

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

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

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

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