CN111290376B - 一种水面/水下无人航行器圆形轨迹跟踪方法 - Google Patents

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

Info

Publication number
CN111290376B
CN111290376B CN201811397613.8A CN201811397613A CN111290376B CN 111290376 B CN111290376 B CN 111290376B CN 201811397613 A CN201811397613 A CN 201811397613A CN 111290376 B CN111290376 B CN 111290376B
Authority
CN
China
Prior art keywords
point
aircraft
potential point
track
potential
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811397613.8A
Other languages
English (en)
Other versions
CN111290376A (zh
Inventor
刘肖宇
宋吉广
李德隆
林扬
刘阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenyang Institute of Automation of CAS
Original Assignee
Shenyang Institute of Automation of CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenyang Institute of Automation of CAS filed Critical Shenyang Institute of Automation of CAS
Priority to CN201811397613.8A priority Critical patent/CN111290376B/zh
Publication of CN111290376A publication Critical patent/CN111290376A/zh
Application granted granted Critical
Publication of CN111290376B publication Critical patent/CN111290376B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

一种水面/水下无人航行器圆形轨迹跟踪方法
技术领域
本发明涉及轨迹跟踪技术领域，具体地说是一种水面/水下无人航行器圆形轨迹跟踪方法。
背景技术
水面/水下无人航行器作为探知海洋丰富资源的一种智能多功能探测载体，需要在无人状态下能够自主完成规划任务。在完成水平面控制器设计后，亦需具有对典型航迹的跟踪能力，而圆形轨迹跟踪能够体现航行器的操纵性以及跟踪能力。传统的基于积分视线导引跟踪方法，对直线的跟踪能力较好，理论上也能够对圆形轨迹进行跟踪。但在实际工程中，在圆形轨迹跟踪时其参数调节较为复杂，且受环境因素以及航行器自身特性影响，达不到期望的效果，尤其是在航行器高速运动状态下，容错性较差，对提供数据的设备精度要求较高。同时，试验过程中，浪费精力和时间，且在航行器高速航行过程中，航行器的安全性得不到保障。
发明内容
针对现有技术的不足，本发明提供一种水面/水下无人航行器圆形轨迹跟踪方法，解决传统的理论上圆形轨迹跟踪在实际应用时的抗干扰能力差、容错性差、对设备量测精度要求较高等问题。
本发明为实现上述目的所采用的技术方案是：
一种水面/水下无人航行器圆形轨迹跟踪方法，包括以下步骤：
步骤1：根据轨迹中心点位置、航行器的位置和姿态信息、轨迹半径、航行速度，进行势点划分，得到势点序列；
步骤2：根据航行器当前位置和姿态信息确定首势点和环行方向；
步骤3：航行器沿势点序列进行运动，并通过切换判据，依次跟踪每个势点，实现圆形轨迹跟踪。
所述势点划分包括：
根据轨迹半径、航行速度，计算势点数量：
β＝απ·(floor(2πR·(v·df)-1)+λ)-1
Figure BDA0001875552160000021
其中，R为轨迹半径，v为航行速度，df为速度参数，α为轨迹参数，λ为偏移参数，β为每两个势点所对应扇形的圆心角，n为势点个数。
所述首势点和环行方向的确定分为航行器初始位置PVehicle在轨迹内部和航行器初始位置PVehicle在轨迹外部；
当航行器初始位置PVehicle在轨迹内部时：首先确定轨迹走向，再计算首势点位置；
所述确定轨迹走向包括：
以中心点为原点建立坐标系。设航行器坐标点为(x,y)，艏向为θ；
ω＝sign(θ-180)·x
其中，sign()为取数字符号函数，ω≥0时，轨迹走向为顺时针，否则为逆时针；
所述计算首势点位置包括：
Figure BDA0001875552160000022
其中，
Figure BDA0001875552160000023
为量测目标位置O和航行器位置PVehicle与x轴所形成的角度，
Figure BDA0001875552160000024
为由航行器位置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
Figure BDA0001875552160000041
其中，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所示是本发明的初始位置位于轨迹内部的首势点计算图；
对于首个势点的选择，将根据航行器初始位置在圆形轨迹内部或外部两种情况进行分析。
航行器初始位置PVehicle在轨迹内部时，应先确定轨迹走向以及势点序列Paim[]，之后再计算首势点位置。
轨迹走向根据航行器艏向、位置，中心点位置进行判定。以中心点为原点建立坐标系。设航行器坐标点为(x,y)，艏向为θ；
ω＝sign(θ-180)·x
其中，sign()为取数字符号函数，ω≥0时，轨迹走向为顺时针，否则为逆时针。
根据相对位置关系计算首势点位置。已知航行器位置PVehicle，ψVehicle是量测目标位置O和航行器位置PVehicle连线与x轴的夹角大小，艏向角，
Figure BDA0001875552160000051
为由航行器位置PVehicle、量测目标位置O、势点位置Paim所形成的ΔOPVehiclePaim中的∠PaimOPVehicle值，设首个势点位置为Paim[a]，则首势点索引a求解为：
Figure BDA0001875552160000061
Figure BDA0001875552160000062
为量测目标位置O和航行器位置PVehicle与x轴所形成的角度。
如图4所示是本发明的初始位置位于轨迹外部的首势点计算图；
航行器初始位置ψusv在轨迹外部时，同样需要先确定轨迹走向以及势点序列Paim[]，之后再计算首势点位置。
以航行器位置为原点，若航行器的艏向与y轴的正方向夹角小于90°，则轨迹走向为顺时针，否则为逆时针。势点序列计算方法相同。
在坐标系下，依据轨迹走向确定切点位置P1，在势点序列中距离P1最近的点则为首势点。
如图5所示是本发明的势点切换判据原理图。
势点切换判据包括：
在坐标系下，根据当前跟踪势点Paim[a]位置与航行器位置，实时求解航行器的期望艏向角，直至完成对当前势点的跟踪后切换至下个势点，继续跟踪。
航行器与势点位置关系如下图所示，由当前势点Paim[a]建立坐标系，
Figure BDA0001875552160000063
为X'轴，逆时针方向建立Y'轴。航行器在X'Y'坐标系下位置为(Xn,Yn)，与势点距离为d。当满足如下条件时，切换至下一势点继续跟踪。切换条件为：
(Xn＞x0)∩(Yn＞y0)∪(d＞dm)
其中，x0为x轴偏移参数，y0为y轴偏移参数，dm为长度参数。

Claims (4)

1.一种水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：包括以下步骤：
步骤1：根据轨迹中心点位置、航行器的位置和姿态信息、轨迹半径、航行速度，进行势点划分，得到势点序列；
所述势点划分包括：
根据轨迹半径、航行速度，计算势点数量：
β＝απ·(floor(2πR·(v·df)-1)+λ)-1
Figure FDA0002931822350000011
其中，R为轨迹半径，v为航行速度，df为速度参数，α为轨迹参数，λ为偏移参数，β为每两个势点所对应扇形的圆心角，n为势点个数；
所述势点序列为以圆形轨迹与X轴正方向交点为起点，在圆形轨迹上等间隔提取n个点，顺时针或逆时针进行排序得到的点的集合；
步骤2：根据航行器当前位置和姿态信息确定首势点和环行方向；
所述首势点和环行方向的确定分为航行器初始位置PVehicle在轨迹内部和航行器初始位置PVehicle在轨迹外部；
当航行器初始位置PVehicle在轨迹内部时：首先确定轨迹走向，再计算首势点位置；
所述确定轨迹走向包括：
以中心点为原点建立坐标系，设航行器坐标点为(x,y)，艏向为θ；
ω＝sign(θ-180)·x
其中，sign()为取数字符号函数，ω≥0时，轨迹走向为顺时针，否则为逆时针；
所述计算首势点位置包括：
Figure FDA0002931822350000012
其中，
Figure FDA0002931822350000021
为量测目标位置O和航行器位置PVehicle与x轴所形成的角度，ψadddec为由航行器位置PVehicle、量测目标位置O、势点位置Paim所形成的ΔOPVehiclePaim中的∠PaimOPVehicle值，β为每两个势点所对应扇形的圆心角；
当航行器初始位置PVehicle在轨迹外部时：
首先确定轨迹走向，即以航行器位置为原点，若航行器的艏向与y轴的正方向夹角小于90°，则轨迹走向为顺时针，否则为逆时针；
再计算首势点位置，即在坐标系下，依据轨迹走向确定切点位置P1，在势点序列中距离P1最近的点则为首势点；
步骤3：航行器沿势点序列进行运动，并通过切换判据，依次跟踪每个势点，实现圆形轨迹跟踪。
2.根据权利要求1所述的水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：所述切换判据为根据当前跟踪势点Paim[a]位置与航行器位置，实时求解航行器的期望艏向角，直至完成对当前势点的跟踪后根据切换条件切换至下个势点，继续跟踪。
3.根据权利要求2所述的水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：所述切换条件为：
(Xn＞x0)∩(Yn＞y0)∪(d＞dm)
其中，Xn、Yn分别为在势点坐标系下航行器位置的横、纵坐标，x0为x轴偏移参数，y0为y轴偏移参数，d为航行器与势点的距离，dm为长度参数。
4.根据权利要求3所述的水面/水下无人航行器圆形轨迹跟踪方法，其特征在于：所述的势点坐标系为：以势点为原点，以量测目标位置到势点为原点方向为x轴正向，根据右手定则建立的坐标系。
CN201811397613.8A 2018-11-22 2018-11-22 一种水面/水下无人航行器圆形轨迹跟踪方法 Active CN111290376B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811397613.8A CN111290376B (zh) 2018-11-22 2018-11-22 一种水面/水下无人航行器圆形轨迹跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811397613.8A CN111290376B (zh) 2018-11-22 2018-11-22 一种水面/水下无人航行器圆形轨迹跟踪方法

Publications (2)

Publication Number Publication Date
CN111290376A CN111290376A (zh) 2020-06-16
CN111290376B true CN111290376B (zh) 2021-05-04

Family

ID=71026569

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811397613.8A Active CN111290376B (zh) 2018-11-22 2018-11-22 一种水面/水下无人航行器圆形轨迹跟踪方法

Country Status (1)

Country Link
CN (1) CN111290376B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113963025B (zh) * 2021-10-22 2022-08-16 西北工业大学深圳研究院 水下自适应机动目标快速跟踪及追踪方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101833333A (zh) * 2009-12-23 2010-09-15 湖南大学 水下清洗作业机器人未知环境路径规划方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7760587B2 (en) * 2007-01-04 2010-07-20 Ocean Acoustical Services and Instrumentation Systems (OASIS), Inc. Methods of and systems for monitoring the acoustic transmission conditions in underwater areas using unmanned, mobile underwater vehicles
CN104020770B (zh) * 2014-06-13 2015-07-22 哈尔滨工程大学 基于多项式的uuv空间轨迹规划方法
CN106896363B (zh) * 2015-12-17 2019-06-11 中国科学院沈阳自动化研究所 一种水下目标主动跟踪航迹起始方法
CN105929842B (zh) * 2016-04-20 2019-01-11 哈尔滨工程大学 一种基于动态速度调节的欠驱动uuv平面轨迹跟踪控制方法
CN105843234B (zh) * 2016-05-12 2018-08-31 哈尔滨工程大学 一种uuv对圆形障碍物几何绕行的二维航路规划方法
CN108490961B (zh) * 2018-03-23 2020-11-10 哈尔滨工程大学 一种多auv动态圆弧编队控制方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101833333A (zh) * 2009-12-23 2010-09-15 湖南大学 水下清洗作业机器人未知环境路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Design of a modular propulsion system for MARTA AUV;Allotta, B.,等;《In Proceedings of the MTS/IEEE OCEANS’15 Congress》;20151231;第1-8页 *
Effects of front shapes of mini revolving AUV on sailing resistance characteristics;Meng L S,等;《IEEE International Conference on Information and Automation》;20151231;第1795-1799页 *

Also Published As

Publication number Publication date
CN111290376A (zh) 2020-06-16

Similar Documents

Publication Publication Date Title
CN107544500B (zh) 一种考虑约束的无人艇靠泊行为轨迹规划方法
CN104571112B (zh) 基于转弯曲率估计的无人车横向控制方法
CN109947123A (zh) 一种基于视线导引律的无人机路径跟踪与自主避障方法
CN104267736A (zh) 一种帆船自主控制方法、装置及帆船
CN106970646B (zh) 基于自适应积分反步的四旋翼飞行器控制方法
CN104765368A (zh) 一种基于模糊自适应算法的无人艇航向航速协同控制方法
CN106598051B (zh) 一种基于动力矢量的航迹控制方法
CN105843234A (zh) 一种uuv对圆形障碍物几何绕行的二维航路规划方法
CN105527967A (zh) 一种auv反演对接控制方法
CN108460210B (zh) 一种基于噪残差和协方差匹配的动力定位系统噪声特性实时估计方法
CN113220000B (zh) 一种面向水下探测作业的无人艇路径跟踪预设性能控制方法及系统
CN111487976B (zh) 一种倒车轨迹跟踪方法
CN109946976B (zh) 一种宽航速auv运动控制方法
CN113625702B (zh) 基于二次规划的无人车同时路径跟踪与避障方法
CN111290376B (zh) 一种水面/水下无人航行器圆形轨迹跟踪方法
CN114779791A (zh) 一种波浪滑翔器位置保持方法及系统
CN113050684A (zh) 一种面向突发威胁的无人机航迹规划算法
CN115202349A (zh) 基于通信干扰的多移动机器人协同编队控制方法、装置、设备及存储介质
CN112327872A (zh) 面向溢油围捕的双无人艇协同轨迹跟踪方法
CN111240362A (zh) 一种智能引导飞行器转弯的控制方法及装置
CN104677359A (zh) 一种动态载体运行轨迹预测预报的方法
CN113093804B (zh) 基于反演滑模控制的无人艇编队控制方法和控制系统
CN111176292B (zh) 一种基于侧边距的波浪滑翔器路径跟踪控制方法
CN111207748B (zh) 基于感知信息的水面/水下无人航行器目标环绕跟踪方法
CN114217639B (zh) 基于无人机指定航向穿越视觉目标点的导引方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant