[发明专利]一种水面/水下无人航行器圆形轨迹跟踪方法

说明书

本发明涉及一种水面/水下无人航行器圆形轨迹跟踪方法，包括以下步骤：根据轨迹中心点位置、航行器的位置和姿态信息、轨迹半径、航行速度，进行势点划分，得到势点序列；根据航行器当前位置和姿态信息确定首势点和环行方向；航行器沿势点序列进行运动，并通过切换判据，依次跟踪每个势点，实现圆形轨迹跟踪。本发明操作简单，安全实用，可良好适应海洋复杂情况，其参数设置可根据试验进行固化，能够提高工作效率，节约时间和人工成本。

技术领域

本发明涉及轨迹跟踪技术领域，具体地说是一种水面/水下无人航行器圆形轨迹跟踪方法。

背景技术

水面/水下无人航行器作为探知海洋丰富资源的一种智能多功能探测载体，需要在无人状态下能够自主完成规划任务。在完成水平面控制器设计后，亦需具有对典型航迹的跟踪能力，而圆形轨迹跟踪能够体现航行器的操纵性以及跟踪能力。传统的基于积分视线导引跟踪方法，对直线的跟踪能力较好，理论上也能够对圆形轨迹进行跟踪。但在实际工程中，在圆形轨迹跟踪时其参数调节较为复杂，且受环境因素以及航行器自身特性影响，达不到期望的效果，尤其是在航行器高速运动状态下，容错性较差，对提供数据的设备精度要求较高。同时，试验过程中，浪费精力和时间，且在航行器高速航行过程中，航行器的安全性得不到保障。

发明内容

针对现有技术的不足，本发明提供一种水面/水下无人航行器圆形轨迹跟踪方法，解决传统的理论上圆形轨迹跟踪在实际应用时的抗干扰能力差、容错性差、对设备量测精度要求较高等问题。

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

一种水面/水下无人航行器圆形轨迹跟踪方法，包括以下步骤：

步骤1：根据轨迹中心点位置、航行器的位置和姿态信息、轨迹半径、航行速度，进行势点划分，得到势点序列；

步骤2：根据航行器当前位置和姿态信息确定首势点和环行方向；

步骤3：航行器沿势点序列进行运动，并通过切换判据，依次跟踪每个势点，实现圆形轨迹跟踪。

所述势点划分包括：

根据轨迹半径、航行速度，计算势点数量：

β＝απ·(floor(2πR·(v·d_f)^-1)+λ)^-1

其中，R为轨迹半径，v为航行速度，d_f为速度参数，α为轨迹参数，λ为偏移参数，β为每两个势点所对应扇形的圆心角，n为势点个数。

所述首势点和环行方向的确定分为航行器初始位置P_Vehicle在轨迹内部和航行器初始位置P_Vehicle在轨迹外部；

当航行器初始位置P_Vehicle在轨迹内部时：首先确定轨迹走向，再计算首势点位置；

所述确定轨迹走向包括：

以中心点为原点建立坐标系。设航行器坐标点为(x,y)，艏向为θ；

ω＝sign(θ-180)·x

其中，sign()为取数字符号函数，ω≥0时，轨迹走向为顺时针，否则为逆时针；

所述计算首势点位置包括：