一种水面/水下无人航行器圆形轨迹跟踪方法技术

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

A circular trajectory tracking method for surface / underwater UAV

【技术实现步骤摘要】
一种水面/水下无人航行器圆形轨迹跟踪方法
本专利技术涉及轨迹跟踪
，具体地说是一种水面/水下无人航行器圆形轨迹跟踪方法。
技术介绍
水面/水下无人航行器作为探知海洋丰富资源的一种智能多功能探测载体，需要在无人状态下能够自主完成规划任务。在完成水平面控制器设计后，亦需具有对典型航迹的跟踪能力，而圆形轨迹跟踪能够体现航行器的操纵性以及跟踪能力。传统的基于积分视线导引跟踪方法，对直线的跟踪能力较好，理论上也能够对圆形轨迹进行跟踪。但在实际工程中，在圆形轨迹跟踪时其参数调节较为复杂，且受环境因素以及航行器自身特性影响，达不到期望的效果，尤其是在航行器高速运动状态下，容错性较差，对提供数据的设备精度要求较高。同时，试验过程中，浪费精力和时间，且在航行器高速航行过程中，航行器的安全性得不到保障。
技术实现思路
针对现有技术的不足，本专利技术提供一种水面/水下无人航行器圆形轨迹跟踪方法，解决传统的理论上圆形轨迹跟踪在实际应用时的抗干扰能力差、容错性差、对设备量测精度要求较高等问题。本专利技术为实现上述目的所采用的技术方案是：一种水面/水下无人航行器圆形轨迹跟踪方法，包括以下步骤：步骤1：根据轨迹中心点位置、航行器的位置和姿态信息、轨迹半径、航行速度，进行势点划分，得到势点序列；步骤2：根据航行器当前位置和姿态信息确定首势点和环行方向；步骤3：航行器沿势点序列进行运动，并通过切换判据，依次跟踪每个势点，实现圆形轨迹跟踪。所述势点划分包括：根据轨迹半径、航行速度，计算势点数量：β＝απ·(floor(2πR·(v·df)-1)+λ)-1其中，R为轨迹半径，v为航行速度，df为速度参数，α为轨迹参数，λ为偏移参数，β为每两个势点所对应扇形的圆心角，n为势点个数。所述首势点和环行方向的确定分为航行器初始位置PVehicle在轨迹内部和航行器初始位置PVehicle在轨迹外部；当航行器初始位置PVehicle在轨迹内部时：首先确定轨迹走向，再计算首势点位置；所述确定轨迹走向包括：以中心点为原点建立坐标系。设航行器坐标点为(x,y)，艏向为θ；ω＝sign(θ-180)·x其中，sign()为取数字符号函数，ω≥0时，轨迹走向为顺时针，否则为逆时针；所述计算首势点位置包括：其中，为量测目标位置O和航行器位置PVehicle与x轴所形成的角度，为由航行器位置PVehicle、量测目标位置O、势点位置Paim所形成的ΔOPVehiclePaim中的∠PaimOPVehicle值，β为每两个势点所对应扇形的圆心角。当航行器初始位置PVehicle在轨迹外部时：首先确定轨迹走向，即以航行器位置为原点，若航行器的艏向与y轴的正方向夹角小于90°，则轨迹走向为顺时针，否则为逆时针；再计算首势点位置，即在坐标系下，依据轨迹走向确定切点位置P1，在势点序列中距离P1最近的点则为首势点。所述势点序列为以圆形轨迹与X轴正方向交点为起点，在圆形轨迹上等间隔提取n个点，顺时针或逆时针进行排序得到的点的集合。所述切换判据为根据当前跟踪势点Paim[a]位置与航行器位置，实时求解航行器的期望艏向角，直至完成对当前势点的跟踪后根据切换条件切换至下个势点，继续跟踪。所述切换条件为：(Xn＞x0)∩(Yn＞y0)∪(d＞dm)其中，Xn、Yn分别为在势点坐标系下航行器位置的横、纵坐标，x0为x轴偏移参数，y0为y轴偏移参数，d为航行器与势点的距离，dm为长度参数。所述的势点坐标系为：以势点为原点，以量测目标位置到势点为原点方向为x轴正向，根据右手定则建立坐标系。本专利技术具有以下有益效果及优点：1.方法简单，应用范围广。本专利技术本专利技术仅需要一个具备良好航行控制状态的水面/水下无人航行器即可，并且适用于海洋上的有同样需求的各类航行器。2.安全稳定，可靠性高，抗干扰能力强，容错性高，对设备量测精度要求较为宽松。能够适用于较差海况下的环境，在航行器高速运动状态下，亦可实现对圆形轨迹的安全稳定跟踪。附图说明图1是本专利技术的势点跟踪算法实现流程图；图2是本专利技术的势点跟踪算法原理示意图；图3是本专利技术的初始位置位于轨迹内部的首势点计算图；图4是本专利技术的初始位置位于轨迹外部的首势点计算图；图5是本专利技术的势点切换判据原理图。具体实施方式下面结合附图及实施例对本专利技术做进一步的详细说明。为使本专利技术的上述目的、特征和有点能够更加明显易懂，下面结合附图对本专利技术的具体实施方式做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本专利技术。但本专利技术能够以很多不同于在此描述的其他方式来实施，本领域技术人员可以在不违背专利技术内涵的情况下做类似改进，因此本专利技术不受下面公开的具体实施的限制。需要说明的是，当元件被称为“设置于”另一个元件，它可能直接在另一个元件上，或也可以存在居中的元件。当一个元件被称为是“连接”另一个元件，它可以直接连接到另一个元件或者可能同时存在居中元件。本文所使用的属于“前”、“后”、“左”、“右”以及类似的标书只是为了说明的目的，并不表示唯一的实施方式。除非另有定义，本文所使用的所有的技术和科学术语与属于本专利技术的
的技术人员通常理解的含义相同。本文中在专利技术的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本专利技术。如图1所示是本专利技术的势点跟踪算法实现流程图。根据轨迹中心点位置、轨迹半径、航行速度的要求，充分考虑航行器本身运动特性，完成势点划分、首势点选取及环行方向选择。航行器在运动过程中实时计算到达势点所需的航向角，通过水平面航向控制器完成航向闭环，通过势点切换判据，航行器依次跟踪每个势点，进而实现圆形轨迹跟踪。势点划分是根据轨迹半径、航行速度并依据航行器本身特性，计算势点数量：β＝απ·(floor(2πR·(v·df)-1)+λ)-1其中，R为轨迹半径，v航行速度，df设定参数，α设定参数，λ设定参数，β为每两个势点所对应扇形的圆心角。算法输出经过控制计算，通过水平面控制器、定位设备闭环，实时调节航行器艏向。如图2所示是本专利技术的势点跟踪算法原理示意图；在确定了圆形轨迹的中心O及半径R后，将圆形轨迹等距划分成n个点(本示意图中n为6，即A、B、C、D、E、F点)，图左为上一时刻的航行器位置关系图，其所跟踪的势点为A势点，当前的环行方向为逆时针方向，在航行器朝向A势点运动后，当其位置达到切换判据后，进行势点的切换至B势点。然后航行器通过不断地运动及势点的切换，达到对有序的A、B、C、D、E、F六个势点的跟踪，进而实现对圆形轨迹的跟踪。如图3所示是本专利技术的初始位置位于轨迹内部的首势点计算图；对于首个势点的选择，将根据航行器初始位置在圆形轨迹内部或外部两种本文档来自技高网...

【技术保护点】
1.一种水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：包括以下步骤：/n步骤1：根据轨迹中心点位置、航行器的位置和姿态信息、轨迹半径、航行速度，进行势点划分，得到势点序列；/n步骤2：根据航行器当前位置和姿态信息确定首势点和环行方向；/n步骤3：航行器沿势点序列进行运动，并通过切换判据，依次跟踪每个势点，实现圆形轨迹跟踪。/n

【技术特征摘要】
1.一种水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：包括以下步骤：
步骤1：根据轨迹中心点位置、航行器的位置和姿态信息、轨迹半径、航行速度，进行势点划分，得到势点序列；
步骤2：根据航行器当前位置和姿态信息确定首势点和环行方向；
步骤3：航行器沿势点序列进行运动，并通过切换判据，依次跟踪每个势点，实现圆形轨迹跟踪。


2.根据权利要求1所述的水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：所述势点划分包括：
根据轨迹半径、航行速度，计算势点数量：
β＝απ·(floor(2πR·(v·df)-1)+λ)-1



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


3.根据权利要求1所述的水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：所述首势点和环行方向的确定分为航行器初始位置PVehicle在轨迹内部和航行器初始位置PVehicle在轨迹外部；
当航行器初始位置PVehicle在轨迹内部时：首先确定轨迹走向，再计算首势点位置；
所述确定轨迹走向包括：
以中心点为原点建立坐标系。设航行器坐标点为(x,y)，艏向为θ；
ω＝sign(θ-180)·x
其中，sign()为取数字符号函数，ω≥0时，轨迹走向为顺时针，否则为逆时针；
所述计算首势点位置包括：



其中，为量测目标位置O和航行器位置PVehicle与x轴所形成的角度，ψadddec为由航行器位置PV...

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