CN114527747A - 一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法 - Google Patents
一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法 Download PDFInfo
- Publication number
- CN114527747A CN114527747A CN202210049835.0A CN202210049835A CN114527747A CN 114527747 A CN114527747 A CN 114527747A CN 202210049835 A CN202210049835 A CN 202210049835A CN 114527747 A CN114527747 A CN 114527747A
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- path
- current
- initial path
- point
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 19
- 230000003044 adaptive effect Effects 0.000 claims description 6
- 239000013598 vector Substances 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000014509 gene expression Effects 0.000 description 1
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
技术领域
本发明涉及了一种轨迹跟踪方法,具体涉及一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法。
背景技术
船舶的轨迹跟踪是指自动驾驶车辆或船只等载体从某一处出发,在规定的时间内通过轨迹跟踪系统的控制到达原本设定的位置上。高性能的轨迹跟踪算法是水面无人船所迫切需要的。
目前,基于预瞄点的轨迹跟踪控制方法被普遍应用,根据预瞄点计算跟踪方案。但预瞄点距离多为人工预设,将会带来一系列可能的问题,主要表现为大曲率弯道前后出现超前转向与滞后转向的现象、复杂道路中的跟踪精度不高、初始偏差大时控制效果差、与预设路径偏差不大时过于频繁进行调整等问题。
发明内容
为了解决背景技术中存在的问题,本发明所提供一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法,能根据无人船与路径的情况,自适应地决策控制方案,改变预瞄点距离大小,以充分利用路径信息提高跟踪精度。
本发明采用的技术方案是:
本发明方法包括如下步骤:
步骤100,预设无人船航行的初始路径;
步骤200,控制无人船按照初始路径航行,获取无人船的实时运动状态;
步骤300,在理想情况下,无人船应当尽可能跟随初始路径航行,在受到海面环境干扰的情况下,无人船会偏离初始路径航行;此时根据无人船的实时运动状态,判断无人船真实的航行路径是否偏离初始路径,若航行路径偏离初始路径,则获取初始路径上的目标点;
步骤400,根据目标点获取无人船的偏航距离和偏航角;
步骤500,根据无人船的实时运动状态、偏航距离和偏航角,校正无人船的实时运动状态。
所述的步骤100中,预设的无人船航行的初始路径即无人船的待跟踪路径,通常为一系列路径点或函数式构成的曲线路径,以初始路径中第一个路径点作为坐标原点,纬线为x轴,经线为y轴,初始路径的路径点表示为:
P(i)=[px(i),py(i)]
其中,P(i)为初始路径中第i个路径点,px(i)为第i个路径点的x轴坐标,py(i)为第i个路径点的y轴坐标。
所述的步骤200中,无人船的实时运动状态包括当前时刻t无人船的航向角、航行速度v(t)和当前位置[x(t),y(t)],其中,x(t)为无人船在当前时刻t的x轴坐标,y(t)为无人船在当前时刻t的y轴坐标。
所述的步骤300中,判断无人船真实的航行路径是否偏离初始路径:
若无人船真实的航行路径不偏离初始路径,则继续控制无人船按照初始路径航行;
若无人船真实的航行路径偏离初始路径,即航行路径相对初始路径产生横向偏差,则进行以下判断:
若无人船真实的航行路径是第一次偏离初始路径,则遍历初始路径中所有的路径点,向当前无人船的前方取距离无人船的当前位置最近的一个路径点作为目标点;
若无人船真实的航行路径不是第一次偏离初始路径,则从无人船上一次偏离初始路径获取的目标点P(m)开始,遍历无人船前方的初始路径中间的所有路径点,获取距离无人船的当前位置最近的一个路径点作为当前的目标点,其中,P(m)为初始路径中第m个路径点,v(t)为当前无人船的航行速度,Δt为由控制无人船航行时发出的控制信号的控制频率的单位时间,s为初始路径的路径点间的平均距离。
所述的步骤400中,将目标点在初始路径上的切线作为投影线,获取无人船的当前位置至目标点之间的直线在投影线上的投影长度作为偏航距离D,获取无人船的当前位置在真实的航行路径上的切线与投影线之间的夹角作为偏航角α。
航向角的调整:
根据当前无人船的偏航距离D与偏航角α,进行以下判断,调整当前无人船的航向角:
航行速度的调整:
根据当前无人船的偏航距离D与偏航角α,同时进行以下判断,调整当前无人船的航行速度:
若D≤λ且时,或者D≤λ且时,则在当前无人船前方的初始路径中,从目标点开始,以相同距离d1取两个初始路径上的点作为预瞄点,d1=v(t)*Δt,v(t)为当前无人船的航行速度,Δt为由控制无人船航行时发出的控制信号的控制频率的单位时间;并进行以下判断:
若D>λ时,则在当前无人船前方的初始路径中,从目标点开始,按照以下公式以距离dj取第j个预瞄点:
其中,κj为第j个预瞄点在初始路径中的曲率,j=1、2、3;
共取三个预瞄点P1、P2和P3,计算预瞄点距离d,d=d1+d2+d3;记当前目标点为P0,计算无人船的航向角分别与向量P0P1、P0P2和P0P3的夹角α1、α2和α3,再按照以下公式计算当前无人船的航向转角为:
其中,T0为预设跟随时间常数,根据发出控制信号的控制设备的实际情况调整;
根据航向转角进行以下判断:
本发明的有益效果是:
1、跟随预瞄点的轨迹控制方式可以兼顾航向误差和轨迹误差,相比于双闭环控制舵向误差和轨迹误差,具备更好地轨迹流畅性和舵向稳定性。
2、不同速度和不同轨迹曲率下预瞄点的位置可以自适应调节以兼顾轨迹跟踪精度和航行稳定性。
附图说明
图1为自适应预瞄点和投影定位的轨迹跟踪方法流程图;
图2为本发明投影长度示意图。
具体实施方式
下面结合附图及具体实施例对本发明作进一步详细说明。
如图1所示,本发明方法包括如下步骤:
步骤100,预设无人船航行的初始路径。
步骤100中,预设的无人船航行的初始路径即无人船的待跟踪路径,通常为一系列路径点或函数式构成的曲线路径,如图2所示,以初始路径中第一个路径点作为坐标原点,纬线为x轴,经线为y轴,初始路径的路径点表示为:
P(i)=[px(i),py(i)]
其中,P(i)为初始路径中第i个路径点,px(i)为第i个路径点的x轴坐标,py(i)为第i个路径点的y轴坐标。
步骤200,控制无人船按照初始路径航行,获取无人船的实时运动状态。
步骤200中,无人船的实时运动状态包括当前时刻t无人船的航向角、航行速度v(t)和当前位置[x(t),y(t)],其中,x(t)为无人船在当前时刻t的x轴坐标,y(t)为无人船在当前时刻t的y轴坐标。
步骤300,在理想情况下,无人船应当尽可能跟随初始路径航行,在受到海面环境干扰的情况下,无人船会偏离初始路径航行;此时根据无人船的实时运动状态,判断无人船真实的航行路径是否偏离初始路径,步骤300中,判断无人船真实的航行路径是否偏离初始路径:
若无人船真实的航行路径不偏离初始路径,则继续控制无人船按照初始路径航行;
若无人船真实的航行路径偏离初始路径,即航行路径相对初始路径产生横向偏差,则进行以下判断:
若无人船真实的航行路径是第一次偏离初始路径,则遍历初始路径中所有的路径点,向当前无人船的前方取距离无人船的当前位置最近的一个路径点作为目标点;
若无人船真实的航行路径不是第一次偏离初始路径,则从无人船上一次偏离初始路径获取的目标点P(m)开始,遍历无人船前方的初始路径中间的所有路径点,获取距离无人船的当前位置最近的一个路径点作为当前的目标点,其中,P(m)为初始路径中第m个路径点,v(t)为当前无人船的航行速度,Δt为由控制无人船航行时发出的控制信号的控制频率的单位时间,s为初始路径的路径点间的平均距离。
若航行路径偏离初始路径,则获取初始路径上的目标点。
步骤400,根据目标点获取无人船的偏航距离和偏航角。
如图2所示,步骤400中,将目标点在初始路径上的切线作为投影线,获取无人船的当前位置至目标点之间的直线在投影线上的投影长度作为偏航距离D,获取无人船的当前位置在真实的航行路径上的切线与投影线之间的夹角作为偏航角α。
步骤500,根据无人船的实时运动状态、偏航距离和偏航角,校正无人船的实时运动状态。
航向角的调整:
根据当前无人船的偏航距离D与偏航角α,进行以下判断,调整当前无人船的航向角:
航行速度的调整:
根据当前无人船的偏航距离D与偏航角α,同时进行以下判断,调整当前无人船的航行速度:
若D≤λ且时,或者D≤λ且时,则在当前无人船前方的初始路径中,从目标点开始,以相同距离d1取两个初始路径上的点作为预瞄点,d1=v(t)*Δt,v(t)为当前无人船的航行速度,Δt为由控制无人船航行时发出的控制信号的控制频率的单位时间;并进行以下判断:
若D>λ时,则在当前无人船前方的初始路径中,从目标点开始,按照以下公式以距离dj取第j个预瞄点:
其中,kj为第j个预瞄点在初始路径中的曲率,j=1、2、3;
共取三个预瞄点P1、P2和P3,计算预瞄点距离d,d=d1+d2+d3;记当前目标点为P0,计算无人船的航向角分别与向量P0P1、P0P2和P0P3的夹角α1、α2和α3,再按照以下公式计算当前无人船的航向转角为:
其中,T0为预设跟随时间常数,根据发出控制信号的控制设备的实际情况调整;
根据航向转角进行以下判断:
Claims (6)
1.一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法,其特征在于:
步骤100,预设无人船航行的初始路径;
步骤200,控制无人船按照初始路径航行,获取无人船的实时运动状态;
步骤300,根据无人船的实时运动状态,判断无人船真实的航行路径是否偏离初始路径,若航行路径偏离初始路径,则获取初始路径上的目标点;
步骤400,根据目标点获取无人船的偏航距离和偏航角;
步骤500,根据无人船的实时运动状态、偏航距离和偏航角,校正无人船的实时运动状态。
2.根据权利要求1所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法,其特征在于:
所述的步骤100中,预设的无人船航行的初始路径为一系列路径点构成的曲线路径,以初始路径中第一个路径点作为坐标原点,纬线为x轴,经线为y轴,初始路径的路径点表示为:
P(i)=[px(i),py(i)]
其中,P(i)为初始路径中第i个路径点,px(i)为第i个路径点的x轴坐标,py(i)为第i个路径点的y轴坐标。
3.根据权利要求2所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法,其特征在于:
所述的步骤200中,无人船的实时运动状态包括当前时刻t无人船的航向角、航行速度v(t)和当前位置[x(t),y(t)],其中,x(t)为无人船在当前时刻t的x轴坐标,y(t)为无人船在当前时刻t的y轴坐标。
4.根据权利要求2所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法,其特征在于:
所述的步骤300中,判断无人船真实的航行路径是否偏离初始路径:
若无人船真实的航行路径不偏离初始路径,则继续控制无人船按照初始路径航行;
若无人船真实的航行路径偏离初始路径,则进行以下判断:
若无人船真实的航行路径是第一次偏离初始路径,则遍历初始路径中所有的路径点,向当前无人船的前方取距离无人船的当前位置最近的一个路径点作为目标点;
5.根据权利要求3所述的一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法,其特征在于:
所述的步骤400中,将目标点在初始路径上的切线作为投影线,获取无人船的当前位置至目标点之间的直线在投影线上的投影长度作为偏航距离D,获取无人船的当前位置在真实的航行路径上的切线与投影线之间的夹角作为偏航角α。
6.根据权利要求1所述的一种基于自适应预瞄点和投影位的无人船轨迹跟踪方法,其特征在于:
航向角的调整:
根据当前无人船的偏航距离D与偏航角α,进行以下判断,调整当前无人船的航向角:
航行速度的调整:
根据当前无人船的偏航距离D与偏航角α,同时进行以下判断,调整当前无人船的航行速度:
若D≤λ且时,或者D≤λ且时,则在当前无人船前方的初始路径中,从目标点开始,以相同距离d1取两个初始路径上的点作为预瞄点,d1=v(t)*Δt,v(t)为当前无人船的航行速度,Δt为由控制无人船航行时发出的控制信号的控制频率的单位时间;并进行以下判断:
若D>λ时,则在当前无人船前方的初始路径中,从目标点开始,按照以下公式以距离dj取第j个预瞄点:
其中,κj为第j个预瞄点在初始路径中的曲率,j=1、2、3;
共取三个预瞄点P1、P2和P3,计算预瞄点距离d,d=d1+d2+d3;记当前目标点为P0,计算无人船的航向角分别与向量P0P1、P0P2和P0P3的夹角α1、α2和α3,再按照以下公式计算当前无人船的航向转角为:
其中,T0为预设跟随时间常数;
根据航向转角进行以下判断:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210049835.0A CN114527747A (zh) | 2022-01-17 | 2022-01-17 | 一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210049835.0A CN114527747A (zh) | 2022-01-17 | 2022-01-17 | 一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114527747A true CN114527747A (zh) | 2022-05-24 |
Family
ID=81621459
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210049835.0A Pending CN114527747A (zh) | 2022-01-17 | 2022-01-17 | 一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114527747A (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115571299A (zh) * | 2022-12-08 | 2023-01-06 | 亿海蓝(北京)数据技术股份公司 | 船舶的偏航判定方法、装置、可读存储介质和船舶 |
CN116627145A (zh) * | 2023-07-25 | 2023-08-22 | 陕西欧卡电子智能科技有限公司 | 无人游船的自主航行控制方法及系统 |
CN116736864A (zh) * | 2023-07-12 | 2023-09-12 | 北鲲睿航科技(上海)有限公司 | 一种自主航行控制方法及系统 |
CN117742352A (zh) * | 2024-02-21 | 2024-03-22 | 陕西欧卡电子智能科技有限公司 | 基于矢量推进无人船的轨迹控制方法、装置、设备及介质 |
CN116736864B (zh) * | 2023-07-12 | 2024-05-31 | 北鲲睿航科技(上海)有限公司 | 一种自主航行控制方法及系统 |
-
2022
- 2022-01-17 CN CN202210049835.0A patent/CN114527747A/zh active Pending
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115571299A (zh) * | 2022-12-08 | 2023-01-06 | 亿海蓝(北京)数据技术股份公司 | 船舶的偏航判定方法、装置、可读存储介质和船舶 |
CN115571299B (zh) * | 2022-12-08 | 2023-03-03 | 亿海蓝(北京)数据技术股份公司 | 船舶的偏航判定方法、装置、可读存储介质和船舶 |
CN116736864A (zh) * | 2023-07-12 | 2023-09-12 | 北鲲睿航科技(上海)有限公司 | 一种自主航行控制方法及系统 |
CN116736864B (zh) * | 2023-07-12 | 2024-05-31 | 北鲲睿航科技(上海)有限公司 | 一种自主航行控制方法及系统 |
CN116627145A (zh) * | 2023-07-25 | 2023-08-22 | 陕西欧卡电子智能科技有限公司 | 无人游船的自主航行控制方法及系统 |
CN116627145B (zh) * | 2023-07-25 | 2023-10-20 | 陕西欧卡电子智能科技有限公司 | 无人游船的自主航行控制方法及系统 |
CN117742352A (zh) * | 2024-02-21 | 2024-03-22 | 陕西欧卡电子智能科技有限公司 | 基于矢量推进无人船的轨迹控制方法、装置、设备及介质 |
CN117742352B (zh) * | 2024-02-21 | 2024-05-10 | 陕西欧卡电子智能科技有限公司 | 基于矢量推进无人船的轨迹控制方法、装置、设备及介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114527747A (zh) | 一种基于自适应预瞄点和投影定位的无人船轨迹跟踪方法 | |
CN110001637B (zh) | 一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置及控制方法 | |
CN109131325B (zh) | 智能驾驶汽车的三维可拓预瞄切换的车道保持控制方法 | |
WO2021230356A1 (ja) | 船舶の自動誘導方法、船舶の自動誘導プログラム、船舶の自動誘導システム、及び船舶 | |
CN110466707B (zh) | 一种应用于小型高速船的随动减摇装置 | |
CN112947494B (zh) | 一种基于模糊pid的船舶自动靠泊控制方法 | |
CN110006419A (zh) | 一种基于预瞄的车辆轨迹跟踪点确定方法 | |
CN108415423A (zh) | 一种高抗扰自适应路径跟随方法及系统 | |
CN108646766A (zh) | 一种基于协同控制的无人船安全跟踪方法及跟踪系统 | |
CN112558642A (zh) | 一种适用于异构多无人系统的海空联合围捕方法 | |
CN114527744B (zh) | 基于天牛须搜索优化的无人帆船路径跟踪制导方法 | |
CN108362296A (zh) | Auv与接驳站水下对接的末端视觉导引方法 | |
CN112180972A (zh) | 基于前轮转弯技术的无人机航线确定方法 | |
CN109582026B (zh) | 基于自整定视线与漂角补偿的自主水下航行器路径跟踪控制方法 | |
CN113741450B (zh) | 一种车辆自动驾驶横向自适应控制方法 | |
CN111845739A (zh) | 车辆行驶控制装置 | |
JP3472831B2 (ja) | 船舶の航路保持制御方法及び装置並びに船舶 | |
CN113093804A (zh) | 基于反演滑模控制的无人艇编队控制方法和控制系统 | |
CN111674406A (zh) | 自动驾驶系统车辆横向控制的方法 | |
CN116610122A (zh) | 一种稳定航速的无人艇路径跟踪方法及无人艇 | |
CN116339314A (zh) | 一种基于自适应滑模的欠驱动无人艇航迹跟踪控制方法 | |
CN115686004A (zh) | 一种基于无人船分散式编队控制的路径动态规划方法 | |
US20220126959A1 (en) | Tracking system and tracking method for water-surface objects, and marine vessel including tracking system for water-surface objects | |
CN114047743A (zh) | 一种带预测的无人艇目标跟踪控制方法与系统 | |
CN114035567A (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 | ||
CB02 | Change of applicant information | ||
CB02 | Change of applicant information |
Country or region after: China Address after: 316021 Zhoushan campus of Zhejiang University, No.1 Zheda Road, Dinghai District, Zhoushan City, Zhejiang Province Applicant after: ZHEJIANG University Address before: 310058 Yuhang Tang Road, Xihu District, Hangzhou, Zhejiang 866 Applicant before: ZHEJIANG University Country or region before: China |