CN115599102B - 一种基于避障信息融合的无人船路径跟踪方法 - Google Patents

一种基于避障信息融合的无人船路径跟踪方法 Download PDF

Info

Publication number
CN115599102B
CN115599102B CN202211347913.1A CN202211347913A CN115599102B CN 115599102 B CN115599102 B CN 115599102B CN 202211347913 A CN202211347913 A CN 202211347913A CN 115599102 B CN115599102 B CN 115599102B
Authority
CN
China
Prior art keywords
point
obstacle
ship
alphak
expected
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
CN202211347913.1A
Other languages
English (en)
Other versions
CN115599102A (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.)
China Ship Development and Design Centre
Original Assignee
China Ship Development and Design Centre
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 China Ship Development and Design Centre filed Critical China Ship Development and Design Centre
Priority to CN202211347913.1A priority Critical patent/CN115599102B/zh
Publication of CN115599102A publication Critical patent/CN115599102A/zh
Application granted granted Critical
Publication of CN115599102B publication Critical patent/CN115599102B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/0206Control 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

一种基于避障信息融合的无人船路径跟踪方法
技术领域
本发明涉及路径规划技术,尤其涉及一种基于避障信息融合的无人船路径跟踪方法。
背景技术
视线导航法是无人船航迹跟踪控制的理论基础,该算法成熟可行,通过简单的推导得出无人船平台跟踪期望航向点所需要的航向角,结合PID控制器,消除航向角偏差,使无人船的当前航向角收敛于期望航向角,以便自航模能够航行在期望航迹线上并且不断向目标航向点靠近,最终行驶到目标航向点,完成对目标点的跟踪控制。视线导航法最大的优点在于其不依赖于被控对象的模型,即可以在模型参数不确定的情况下,或者外界扰动对船模影响较大的环境中设计控制器,完成对目标模型的控制;其次,视线导航法设计简便,抗干扰能力强,控制效果出色也是其被使用的主要因素。
发明内容
本发明要解决的技术问题在于针对现有技术中的缺陷,提供一种基于避障信息融合的无人船路径跟踪方法。
本发明解决其技术问题所采用的技术方案是:一种基于避障信息融合的无人船路径跟踪方法,包括以下步骤:
1)采用类人工势场方法,将与无人船趋碰的障碍物目标抽象为实时移动的斥力源,将斥力矢量正交分解为基坐标系下的目标避碰避障的运动分量,获得避碰运动分量;
1.1)对于被控平台周围区域内每一个障碍物Obstacle i,设被控平台距障碍物Obstacle i的距离为di,将其按照正东正北坐标系正交分解,正东方向分量为temp[i].x,正北方向分量为temp[i].y;构造函数:
1.2)将所有障碍物Obstacle i的上述分量a[i]、b[i]进行求和,即对所有障碍物“斥力”进行矢量求和,之后乘以与被控平台最小回转半径相关的超参数β,获得避碰运动分量;
超参数β,经试验室虚实结合LVC测试,默认取为200;
2)采用改进的视线导航法进行航迹跟踪,同时合成期望航向时的避碰运动分量,获得最终的期望航向角;
设航迹跟踪当前的期望位置是Pk+1=[xk+1,yk+1]T,上一次的期望位置是Pk=[xk,yk]T,被控船舶的当前位置记为S=[x,y]T,下一次的期望位置是Pk+1=[xk+1,yk+1]T
被控平台的预设跟踪路径为Pk Pk+1 Pk+2,Pk+1 Pk+2在正北方向上的分量为pointye,在正东方向上的分量为pointxe;过Pk+1点向正南方向做延长线,过S点向正东方向做延长线,二者相交于点A;过点A引Pk+2Pk+1延长线的垂线,相交于点C;过点S做Pk+2Pk+1的平行线,交AC于B;
设Pk+1 Pk+2与正东方向的夹角为alphak,则
∠BAPk+1=∠BSA=alphak
BC=-trackxe×sin(alphak)+trackye×cos(alphak)
在此几何关系上,融合避碰运动分量,构造变量e:
e=-(trackxe-uxy.x)×sin(alphak)+(trackye-uxy.y)×cos(alphak)
设delta_data为与被控平台最小回转半径、航速相关的超参数,默认取为50,则输出的期望航向角psid:
psid=alphak+psir=alphak+tan2(e,delta_data)。
基于多步的线跟踪方式同时需要改进到达路径点的判别条件,现行LOS算法中是假设存在一个以船舶所跟踪的当前期望航向点(xk+1,yk+1)为圆心,以R0为半径的圆,若某一时刻,被控船舶的当前位置P=[x,y]T满足以下条件:
(xk+1-x)2+(yk+1-y)2≤R0 2
则需要将跟踪的当前期望航向点转换为下一个期望航向点。
改进后的导航算法中判别条件如下:
假设存在一个以船舶所跟踪的当前期望航向点Pk+2(xk+2,yk+2)为圆心,以R0=Pk+ 2Pk+1为半径的圆,若某一时刻,被控船舶的当前位置S=[x,y]T满足以下条件:
s2+e2≤R0 2
则需要将跟踪的当前期望航向点转换为下一个期望航向点;其中,s为SA的距离。
3)基于改进算法的PID控制器设计,为了使航向角与算法输出期望航向的偏差趋近于零,即使得被控平台当前航向角跟踪上期望航向角,构建简单、有效的PID控制回路;
通过设定合理的PID控制器参数,便可得到稳定收敛的控制规律,使航向角偏差ψe趋于零;
输出舵角δ为:
航向角偏差为
ψe(t)=psid-ψ(t)
通过不断控制,只要ψe(t)存在,那么便会存在一个舵角δ使ψe(t)趋向于零,使被控平台达到期望航向。
本发明产生的有益效果是:
本发明采用改进的视线导航法相较于现有视线导航法能够有效减小在路径点附近处的横向误差,使得被控平台实际航迹与预期路径更为贴合,同时能够实现平台动态避障功能。
附图说明
下面将结合附图及实施例对本发明作进一步说明,附图中:
图1是本发明实施例的视线导航法航迹跟踪算法原理图;
图2是本发明实施例的视线导航法航迹跟踪实现原理图;
图3是本发明实施例的避碰运动分量各计算变量示意图;
图4是本发明实施例的改进的视线导航法原理图;
图5是本发明实施例的改进的视线导航法路径点到达判别条件示意图;
图6是本发明实施例的PID控制器示意图;
图7是本发明实施例的改进视线导航法与现有视线导航法运行结果对比示意图;
图8是本发明实施例的集群路径跟踪时动态避碰示意图;
图9是本发明实施例的集群路径跟踪时静态避碰示意图;
图10是本发明实施例的方法流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅用以解释本发明,并不用于限定本发明。
如图1所示,视线导航法的原理为如果使被控船舶平台的航向保持对准视线角(LOS角),那么经过适当的控制,就能使被控船舶到达期望的位置,达到航迹跟踪的效果。如图1所示,假设航迹跟踪当前的期望位置是Pk+1=[xk+1,yk+1]T,上一次的期望位置是Pk=[xk,yk]T,被控船舶的当前位置记为P=[x,y]T,并假设Pk+1、Pk可以进行测量,则LOS角能够通过下面公式计算得到:
式中ΨLOS∈[-π,π],为LOS角。同时引入一个可视距离Δ来协助完成,原理如图2所示。
期望航向点PLOS=[xLOS,yLOS]T与被控船舶当前位置在期望航迹上的投影点相聚Δ=nLpp,这里n取2到5的整数,Lpp为船舶长度,此时,LOS控制算法可以写成:
式中,e表示被控船舶平台的横向跟踪误差,αk是大地坐标系正北方向与期望航迹线PkPk+1的夹角。设航向角偏差Ψe=αφ-Ψ,当Ψe趋于零的时候,被控平台即向目标航向点行驶,最终到达期望航迹,完成路径跟踪。
本发明基于成熟LOS算法进行改进,由单步点跟踪改为多步线跟踪,提升被控目标航迹与预期路径的贴合度;采用类人工势场方法,将渐近趋碰的目标抽象为实时移动的斥力源,将斥力矢量正交分解为基坐标系下的目标避碰避障的运动分量,通过乘以膨胀系数,与路径跟踪的运动分量进行矢量合成,实现平台路径跟踪中的动态避障功能。
如图10,一种基于避障信息融合的无人船路径跟踪方法,包括以下步骤:
1)采用类人工势场方法,将渐近趋碰的目标抽象为实时移动的斥力源,将斥力矢量正交分解为基坐标系下的目标避碰避障的运动分量,通过乘以膨胀系数,与路径跟踪的运动分量进行矢量合成,实现平台路径跟踪中的动态避障功能。
如图3所示,对于被控平台周围区域内每一个障碍物Obstacle i,以Obstacle1为例,设被控平台距障碍物Obstacle1的距离为d,将其按照正东正北坐标系正交分解,正东方向分量为temp[1].x,正北方向分量为temp[1].y。参考斥力模型,构造函数:
R、r是为了避碰设置的距离,船与障碍物距离小于R时斥力开始生效,离r越近斥力会变的非常大,目的是为了绝不让船进入r范围内。本实施例中,r设为两倍最小回转半径,R设为3倍r。
将所有障碍物Obstacle i的上述分量a[i]、b[i]进行求和,即对所有障碍物“斥力”进行矢量求和。之后乘以与被控平台最小回转半径相关的超参数β,经试验室虚实结合LVC测试,默认取为200。
2)在现行成熟LOS视线导航法基础上进行改进,将基于单步的点跟踪方式改进为基于多步的线跟踪方式,即现行LOS算法下被控平台在Pk Pk+1段航行时,关注目标点仅在Pk+1一点,是基于单步的点跟踪方式;改进后的视线导航法同时考虑距离上趋近Pk+1,航向角上趋近于Pk+1 Pk+2,这样能够有效在过Pk+1点后减小调试回转区域,同时合成期望航向时考虑避碰避障分量的因素,能够动态避障避碰的同时,提升被控目标航迹与预期路径的贴合度。
如图4所示,被控平台的预设跟踪路径为Pk Pk+1 Pk+2,Pk+1 Pk+2在正北方向上的分量为pointye,在正东方向上的分量为pointxe,过Pk+1点向正南方向做延长线,过被控船舶的当前位置S点向正东方向做延长线,二者相交于点A,过点A引Pk+2Pk+1延长线的垂线,相交于点C,过点S做Pk+2Pk+1的平行线,交AC于B。
设Pk+1 Pk+2与正东方向的夹角为alphak,则
∠BAPk+1=∠BSA=alphak
BC=-trackxe×sin(alphak)+trackye×cos(alphak)
在此几何关系上,融合避碰运动分量,构造变量e:
e=-(trackxe-uxy.x)×sin(alphak)+(trackye-uxy.y)×cos(alphak)
设delta_data为与被控平台最小回转半径、航速相关的超参数,默认取为50,则输出的期望航向角
psid=alphak+psir=alphak+tan2(e,delta_data)
基于多步的线跟踪方式同时需要改进到达路径点的判别条件,现行LOS算法中是假设存在一个以船舶所跟踪的当前期望航向点(xk+1,yk+1)为圆心,以R0为半径的圆,若某一时刻,被控船舶的当前位置P=[x,y]T满足以下条件:
(xk+1-x)2+(yk+1-y)2≤R0 2
则需要将跟踪的当前期望航向点转换为下一个期望航向点。
R0是为了判定船是否踩点,到达路径点。所以R0一定是大于r的,本实施例中R0设成两倍r。
改进后的导航算法判别条件如图5所示
假设存在一个以船舶所跟踪的当前期望航向点Pk+2(xk+2,yk+2)为圆心,以R0=Pk+ 2Pk+1为半径的圆,若某一时刻,被控船舶的当前位置P=[x,y]T满足以下条件:
s2+e2≤R0 2
则需要将跟踪的当前期望航向点转换为下一个期望航向点。
3)基于改进算法的PID控制器设计,为了使航向角与算法输出期望航向的偏差趋近于零,即使得被控平台当前航向角跟踪上期望航向角,构建简单、有效的PID控制回路。
从图6可以看出,通过设定合理的PID控制器参数,便可得到稳定收敛的控制规律,使ψe趋于零。输出舵角δ为:
航向角偏差为
ψe(t)=psid-ψ(t)
通过不断控制,只要ψe(t)存在,那么便会存在一个舵角δ使ψe(t)趋向于零,使被控平台达到期望航向。
图7为本发明改进视线导航法与现有视线导航法运行结果对比示意图;其中,图7(a)为基于质点模型的运行结果对比示意图,左图为原有算法运行结果,右图为改进视线导航法运行结果;图7(b)为基于考虑转角约束的刚体模型的运行结果对比示意图,左图为原有算法运行结果,右图为改进视线导航法运行结果;
将本发明所述的路径跟踪控制方法方案封装成模块,可以获得一种基于避障信息融合的无人船路径跟踪装置;
在珠海婆湾山附近海域进行了实船测试,结果显示该路径跟踪控制方法能够很好的支撑无人艇路径跟踪、动态避障,并且与集群控制算法有着很好的兼容性。在岸基基地构建LVC仿真试验环境,将实艇航控设备实物接入,其余用软件模拟器方式接入,构建数字化仿真环路,测试路径跟踪效果及对动态、静态障碍物的处理情况。
如图8所示,集群中各艇都能很好地沿预设路径行进,并能够及时地对相向驶来的另一条艇进行避碰机动。
如图9所示,集群中各艇都能很好地沿预设路径行进,并能够预设路径上的静态障碍物进行避碰机动。
应当理解的是,对本领域普通技术人员来说,可以根据上述说明加以改进或变换,而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (2)

1.一种基于避障信息融合的无人船路径跟踪方法,其特征在于,包括以下步骤:1)采用类人工势场方法,将与无人船趋碰的障碍物目标抽象为实时移动的斥力源,将斥力矢量正交分解为基坐标系下的目标避碰避障的运动分量,获得避碰运动分量;
1.1)对于被控平台周围区域内每一个障碍物Obstaclei,设被控平台距障碍物Obstaclei的距离为di,将其按照正东正北坐标系正交分解,正东方向分量为temp[i].x,正北方向分量为temp[i].y;构造函数:
其中,R和r是为了避碰设置的距离,当船与障碍物距离小于R时斥力开始生效,当船与障碍物距离r越近斥力会变的非常大,设置距离r不让船进入障碍物r范围;
1.2)将所有障碍物Obstacle i的上述分量a[i]、b[i]进行求和,即对所有障碍物“斥力”进行矢量求和,之后乘以与被控平台最小回转半径相关的超参数β,获得避碰运动分量;
2)采用改进的视线导航法进行航迹跟踪,同时合成期望航向时的避碰运动分量,获得最终的期望航向角;
设航迹跟踪当前的期望位置是Pk+1=[xk+1,yk+1]T,上一次的期望位置是Pk=[xk,yk]T,被控船舶的当前位置记为S=[x,y]T,下一次的期望位置是Pk+2=[xk+2,yk+2]T
被控平台的预设跟踪路径为Pk Pk+1 Pk+2,Pk+1 Pk+2在正北方向上的分量为pointye,在正东方向上的分量为pointxe;过Pk+1点向正南方向做延长线,过S点向正东方向做延长线,二者相交于点A;过点A引Pk+2Pk+1延长线的垂线,相交于点C;过点S做Pk+2Pk+1的平行线,交AC于B;
设Pk+1Pk+2与正东方向的夹角为alphak,则∠BAPk+1=∠BSA=alphak
BC=-trackxe×sin(alphak)+trackye×cos(alphak)
在此几何关系上,融合避碰运动分量,构造变量e:
e=-(trackxe-uxy.x)×sin(alphak)+(trackye-uxy.y)×cos(alphak)
设delta_data为与被控平台最小回转半径、航速相关的超参数,则输出的期望航向角psid:
psid=alphak+psir=alphak+tan2(e,delta_data);
改进后的导航方法中到达路径点的判别条件如下:
假设存在一个以船舶所跟踪的当前期望航向点Pk+2(xk+2,yk+2)为圆心,以R0=Pk+2Pk+1为半径的圆,若某一时刻,被控船舶的当前位置S=[x,y]T满足以下条件:
s2+e2≤R0 2
则需要将跟踪的当前期望航向点转换为下一个期望航向点;其中,s为SA的距离。
2.根据权利要求1所述的基于避障信息融合的无人船路径跟踪方法,其特征在于,所述步骤2)中基于改进的视线导航法的PID控制器设计如下:
通过设定PID控制器参数,使航向角偏差ψe趋于零;
输出舵角δ为:
航向角偏差为
ψe(t)=psid-ψ(t)
通过不断控制,只要ψe(t)存在,便存在一个舵角δ使ψe(t)趋向于零,使被控平台达到期望航向。
CN202211347913.1A 2022-10-31 2022-10-31 一种基于避障信息融合的无人船路径跟踪方法 Active CN115599102B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211347913.1A CN115599102B (zh) 2022-10-31 2022-10-31 一种基于避障信息融合的无人船路径跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211347913.1A CN115599102B (zh) 2022-10-31 2022-10-31 一种基于避障信息融合的无人船路径跟踪方法

Publications (2)

Publication Number Publication Date
CN115599102A CN115599102A (zh) 2023-01-13
CN115599102B true CN115599102B (zh) 2024-04-30

Family

ID=84851411

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211347913.1A Active CN115599102B (zh) 2022-10-31 2022-10-31 一种基于避障信息融合的无人船路径跟踪方法

Country Status (1)

Country Link
CN (1) CN115599102B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011008313A (ja) * 2009-06-23 2011-01-13 Yamaha Motor Co Ltd 走行方向制御装置および移動体
CN108803612A (zh) * 2018-06-27 2018-11-13 青岛黄海学院 一种海流影响下的无人巡检船直线航迹跟踪实现方法
CN110471427A (zh) * 2019-09-06 2019-11-19 大连海事大学 一种基于路径规划与人工势场法的船舶编队智能避碰方法
CN112327885A (zh) * 2020-12-01 2021-02-05 大连海事大学 一种无人船自适应全局-局部混合路径规划的方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TWI756647B (zh) * 2020-03-18 2022-03-01 財團法人船舶暨海洋產業研發中心 基於人工勢場法的船舶避碰方法及系統

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011008313A (ja) * 2009-06-23 2011-01-13 Yamaha Motor Co Ltd 走行方向制御装置および移動体
CN108803612A (zh) * 2018-06-27 2018-11-13 青岛黄海学院 一种海流影响下的无人巡检船直线航迹跟踪实现方法
CN110471427A (zh) * 2019-09-06 2019-11-19 大连海事大学 一种基于路径规划与人工势场法的船舶编队智能避碰方法
CN112327885A (zh) * 2020-12-01 2021-02-05 大连海事大学 一种无人船自适应全局-局部混合路径规划的方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
无人帆船自主避障算法;徐建云;许劲松;侯春晓;;船舶工程;20180825(08);全文 *
无人水下航行器全局路径规划及有限时间跟踪控制;龚俊斌等;《上海海事大学学报》;20220331;全文 *

Also Published As

Publication number Publication date
CN115599102A (zh) 2023-01-13

Similar Documents

Publication Publication Date Title
Lekkas et al. Line-of-sight guidance for path following of marine vehicles
CN102722177B (zh) 具有pid反馈增益的自主水下航行器三维直线路径跟踪控制方法
JP5042906B2 (ja) 船舶用自動操舵装置
CN111487966A (zh) 一种基于航路点的水面无人艇自适应路径跟踪控制方法
CN106444806A (zh) 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法
CN113126644B (zh) 基于自适应视线法的无人机三维航迹跟踪方法
CN113625702B (zh) 基于二次规划的无人车同时路径跟踪与避障方法
CN105628045A (zh) 一种无人机跟拍路径规划与跟踪方法
JP6882243B2 (ja) 避航支援装置
CN113296505B (zh) 一种基于速变los的无人船多模式路径跟踪控制方法
CN109992009B (zh) 一种基于距离量测的移动目标环绕跟踪方法
Kim et al. Way-point tracking for a hovering AUV by PID controller
Sans-Muntadas et al. Spiral path planning for docking of underactuated vehicles with limited FOV
CN114740859A (zh) 一种船只自动悬停方法及系统
Sujit et al. AUV docking on a moving submarine using a KR navigation function
Hoy et al. Collision-free navigation of an autonomous unmanned helicopter in unknown urban environments: sliding mode and MPC approaches
CN115599102B (zh) 一种基于避障信息融合的无人船路径跟踪方法
CN109062198A (zh) 一种船舶的航行控制方法、装置、设备及介质
CN116339355B (zh) 水下航行器及其编队跟踪控制方法和装置
US20220001970A1 (en) Control target generation device and ship-steering control device
CN112034866B (zh) 一种水下机器人跟踪制导的方法及装置
de la Cruz et al. A streamlined nonlinear path following kinematic controller
CN114217639A (zh) 基于无人机指定航向穿越视觉目标点的导引方法及系统
Miotto et al. UUV on-board path planning in a dynamic environment for the Manta test vehicle
Barua et al. Path planning for an identification mission of an Autonomous Underwater Vehicle in a lemniscate form

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