CN106959698B - 一种路径跟踪避障制导方法 - Google Patents
一种路径跟踪避障制导方法 Download PDFInfo
- Publication number
- CN106959698B CN106959698B CN201710374707.2A CN201710374707A CN106959698B CN 106959698 B CN106959698 B CN 106959698B CN 201710374707 A CN201710374707 A CN 201710374707A CN 106959698 B CN106959698 B CN 106959698B
- Authority
- CN
- China
- Prior art keywords
- canoe
- virtual canoe
- dynamic
- dynamic virtual
- ship
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 67
- 230000033001 locomotion Effects 0.000 claims abstract description 23
- 238000012544 monitoring process Methods 0.000 claims abstract description 8
- 230000004888 barrier function Effects 0.000 claims description 16
- 238000001514 detection method Methods 0.000 claims description 16
- 230000004913 activation Effects 0.000 claims description 12
- 238000012360 testing method Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 238000005070 sampling Methods 0.000 claims description 5
- 238000011156 evaluation Methods 0.000 claims description 4
- 235000013399 edible fruits Nutrition 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 6
- 238000011160 research Methods 0.000 description 5
- 241000251468 Actinopterygii Species 0.000 description 4
- 230000008859 change Effects 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 4
- 230000003044 adaptive effect Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 238000011835 investigation Methods 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000000227 grinding Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000002121 ultrasonic speckle velocimetry Methods 0.000 description 2
- 241000219000 Populus Species 0.000 description 1
- 238000007605 air drying Methods 0.000 description 1
- 230000033228 biological regulation Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000012530 fluid Substances 0.000 description 1
- 210000005036 nerve Anatomy 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 150000003839 salts Chemical class 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 238000012549 training Methods 0.000 description 1
- 239000002699 waste material Substances 0.000 description 1
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)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种路径跟踪避障制导方法,包括以下步骤:(1)启动制导模式,执行路径规划获取GVS运动状态变量的时间序列信号;(2)执行路径跟踪制导算法,获取DVS的ψdp和udp;(3)实时监测DVS到障碍物安全圆族边界的最小距离lmin,并根据lmin确定DVS的ψd;(4)计算DVS的ud和rd;(5)判断DVS与GVS的距离,如果其大于等于阈值距离,则GVS运动状态保持不变;否则,则GVS运动状态信息按照其时间序列的排序下移;(6)将DVS制导信息代入控制回路,并更新下一时间点DVS位置信息;(7)测量实船位置判断是否到达目的地,如果“是”,结束航行;如果“否”,时间更新并进入步骤(2)。本发明实现了船舶行驶过程中制导算法与避障算法的相结合,可以更精确的控制船舶进行制导和避障。
Description
技术领域
本发明涉及船舶控制工程与船舶自动化航行装备应用领域,尤其是涉及一种路径跟踪避障制导算法。
背景技术
制导算法是解决船舶运动航迹保持问题的必不可少的技术,传统的基于航路点的船舶路径规划,要求船舶沿着设定航线自动航行,归结于路径跟踪的制导问题[1]。然而,海洋环境具有多变性,船舶在航行过程中经常会发生避障行为,尤其在穿越渔区和途径狭窄或繁忙水道时,这就要求船舶制导算法具有避障功能。避障算法,起源于机器人控制的研究,常用的方法有势场法,速度避障法和动态窗法,其中动态窗法将船舶前进速度和艏摇速度在采样周内可达到的范围以动态窗的形式进行描述,尤其适合解决高速无人船的多目标避障问题,下面对文献[2]提出的基于动态窗的避障算法进行技术特征的简要介绍。
该算法在对设定航路点进行追踪的同时,对探测范围内的多个目标进行避障。要求已知船舶在采样周期ts内前进速度的动态窗口VT和艏向角θUSV的动态窗口VHead。按照式(8)的最大化原则选取最优艏向角
FE(θ)=ε·fhead(θ)+(1-ε)·fsafe(θ) (8)
其中,θgoal是航路点对于实船的真方位角,Bobs是探测半径dnear内障碍物造成的不可行真方位角的集合,αi是探测半径内障碍物中心对于实船的真方位角,ε是可调整参数。根据式(11)***当前制导坐标点PInsert,DInsert为一个小于探测半径的距离。
进而通过式(12)获得当前制导艏向角作为当前艏向角的跟踪目标。
定义按照式(13),(14)定义当前制导速度。
因为该避障算法仅考虑实船相对方位角范围内的障碍物,所以给出的定义如式(15)所示。
η1的定义如式(16)所示。是当前转艏角速度。
为了防止速度过大造成碰撞,实船速度应当满足式(17),如图1所示。
化简可得:
则vmax应当满足式(19),rmax是探测范围内最大障碍物的半径。η2应当满足式(20)。
通过以下流程实时获取船舶目标艏摇角和目标速度对实船进行避障制导。最终可以获得如图3所示的避障效果。
现有面向大型船舶制导算法的研究多是针对路径跟踪提出。其中,LOS制导算法[3]发展较为成熟且被广泛应用,然而却存在无法对转向部分的曲线段进行航迹保持,以及难以进行路径规划等缺点。DVS制导算法[4]通过导引虚拟小船实现直线段及曲线段的路径规划,从根本上克服了LOS制导算法的缺点,通过生成动态虚拟小船作为直接跟踪目标,避免了执行器输入过大。然而,基于动态虚拟小船的生成方式,传统DVS制导算法难以同避障算法相结合。
在航海领域,避障算法的研究目前主要针对小型无人艇,常用的避障算法有动态窗算法[2],速度避障法[5],势场避障法[6]等。其中动态窗算法仅考虑了船舶的前进和艏摇运动,没有考虑横荡运动,故不适应于考虑欠驱动特性的大型船舶。速度避障法有计算简便的数学特性,却仅需要假设船舶运动速度是线性的,且难以同路径跟踪制导算法结合。势场法需要对环境提前预知,且在通过狭窄通道时极易失去稳定,造成振荡,故不适用于具有多变性的真实海况。
在过去的研究中,避障算法和路径跟踪制导算法的研究往往是分开的。从图3可以看出,避障算法往往仅考虑对航路点的跟踪,对于大型商船而言这是不切实际的。同时,大型商船在进行路径跟踪时,经常会遇到作业中的小型渔船群,岛屿礁石等一定程度上不可预测的静止或慢时变目标。一种能够进行路径跟踪同时考虑多目标避障的制导算法成为亟待解决的研究点。文献[7]和[8]分别基于LOS制导算法和Serret-Frenet框架提出了一种考虑单目标避障的路径跟踪制导算法,该算法对多目标障碍物无效且仍然没有对曲线段进行路径规划的能力。文献[9]基于龙格库塔法提出了一种具有路径规划和多目标避障能力的制导算法,却无法适用于具有欠驱动特性的大型商船。
发明内容
鉴于已有技术存在的缺陷,本发明公开了一种路径跟踪避障制导方法,包括以下步骤:
一种路径跟踪避障制导方法,包括以下步骤:
(1)执行路径规划并获取导引虚拟小船的时间序列信号;
(2)根据动态虚拟小船和导引虚拟小船的位置信息,计算动态虚拟小船的路径跟踪艏向角ψdp和路径跟踪前进速度udp;
(3)实时监测动态虚拟小船到障碍物安全圆族边界的最小距离lmin,并根据实时监测的最小距离lmin确定动态虚拟小船的最终艏向角ψd;
(4)计算动态虚拟小船的最终前进速度ud和转艏角速度rd;
(5)判断动态虚拟小船与导引虚拟小船间的距离ldr与设定的阈值距离ldrset的大小,如果动态虚拟小船与导引虚拟小船的距离ldr大于等于阈值距离ldrset,则导引虚拟小船下一时间点的运动状态保持当前状态不变;如果动态虚拟小船与导引虚拟小船的距离ldr小于阈值距离ldrset,则导引虚拟小船下一时间点运动状态信息按照其时间序列的排序下移;
(6)将制导信息输入到控制回路中并更新动态虚拟小船下一时间点的位置信息;
(7)测量实船位置,判断是否到达目的地,如果“是”,结束船舶航行;如果“否”,时间更新t=t+1并进入步骤(2)。
进一步地,在步骤(2)中所述动态虚拟小船的路径跟踪艏向角ψdp采用公式(2)进行计算:
其中,(xr,yr),(xd,yd)分别为导引虚拟小船和动态虚拟小船的坐标;
所述动态虚拟小船的路径跟踪前进速度udp采用公式(3)进行计算:
udp=klldr+ur cos(ψdp-ψr) (3)
其中ldr为动态虚拟小船与导引虚拟小船间的距离,kl为预设参数,其数值大小是根据动态虚拟小船在路径跟踪情况下对导引虚拟小船位置的收敛速率进行设置,其值越大,收敛越快,但考虑到执行器的限制,应当适当选取。
进一步地,在步骤(3)中所述根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离lmin确定动态虚拟小船的最终艏向角ψd包括:
1)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin小于等于船舶障碍物探测设备的激活圈半径Ractive时,执行避障算法以确定最终艏向角ψd:
避障算法原理为:根据船舶上障碍物探测设备(如导航雷达,摄像设备等)的作用半径确定避障算法的探测圈半径Rtest和激活圈半径Ractive,当障碍物进入探测圈时,在探测到的障碍物的边缘生成半径为Rs的安全圆族,此时若安全圆族部分或全部进入激活圈,从路径跟踪制导算法给出的动态虚拟小船路径跟踪艏向角ψdp开始,向两侧搜索能够避免与圆族相交的最近的艏向角和
按照评价函数(4)的最小化原则从两个艏向角和中选取最优值作为制导算法中动态虚拟小船的最终艏向角ψd:
其中i=1,2,ε是自定义的权重值,ψd(j-1)代表上一个采样时间点的最终艏向角;
2)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin大于船舶中障碍物探测设备的激活圈半径Ractive时,动态虚拟小船的最终艏向角ψd=ψdp。
进一步地,在所述步骤(4)中,所述动态虚拟小船的最终前进速度ud采用公式(5)(6)进行计算:
其中,lmin为障碍物安全圆族边界到动态虚拟小船的最小距离,Rc∈[0,Ractive/2],为手动设置的一个阈值,udo为手动设置的避障过程的最小值,ldbset为手动设置的实船到动态虚拟小船间距离上界,ldb为实船距离动态虚拟小船的实际距离,u′dp值计算过程如下:首先设置动态虚拟小船路径跟踪速度最大值udpmax,按照公式(3)计算动态虚拟小船路径跟踪速度udp,若udp≥udp max,则保持u′dp=udp max,否则令u′dp=udp;
所述动态虚拟小船的转艏角速度rd采用如公式(7)所示的1阶滤波器进行估计:
其中,βψd为动态虚拟小船艏向角ψd的估计值,τ为设定参数。
本发明具有以下有益效果:
1)本发明的制导算法解决了船舶在路径跟踪过程中对多目标进行避障的现实问题,提高了船舶的自动化程度。基于就近原则的避障算法简单易于实现,避免了航程的浪费且保证了航行安全。
2)本发明继承了传统DVS算法的优点,解决了“LOS算法无法进行曲线段路径规划”的问题,适用于高精度航迹保持控制中,同时,易于同欠驱动问题的先进控制算法结合。考虑船舶执行装置的限制,本发明进行了DVS的速度规划,相比传统DVS制导算法,具有异曲同工之妙。
附图说明
图1是现有技术中最小半径限制原理图;
图2是现有技术中基于动态窗的避障算法执行流程图;
图3是现有技术中基于动态窗的避障算法2维效果图;
图4是本发明的船舶路径跟踪控制逻辑结构图;
图5是现有技术中导引虚拟小船的航路点处弧线路径规划示意图;
图6是本发明改进的动态虚拟小船制导算法基本原理;
图7是本发明的避障算法基本原理;
图8是本发明中u′d速度规划示意图;
图9是本发明跟踪避障制导算法执行流程图;
图10为本发明实施例的船舶示意图;
图11为实施例中实验海浪模型干扰三维视图;
图12为实施例中模拟真实海洋环境下路径跟踪避障2-D效果图;
图13为实施例中船舶运动姿态变量u,v,ψ时间变化曲线示意图;
图14为实施例中执行装置控制输入n,δ时间变化曲线示意图。
具体实施方式
为使本发明的技术方案和优点更加清楚,下面结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚完整的描述。
如图4和图9所示为本发明公开的路径跟踪避障制导算法的船舶路径跟踪控制逻辑结构图和跟踪避障制导算法执行流程图(图中GVS表示导引虚拟小船,DVS表示动态虚拟小船)。如图4所示船舶的整个执行模块分为制导***和控制***两部分,制导模块采用本发明的提出的路径跟踪避障制导算法。
如图9所示本发明提出的路径跟踪避障制导算法具体步骤如下:
(1)执行路径规划并获取导引虚拟小船的时间序列信号。
根据驾驶员设定的航路点位置信息计算出船舶的路径跟踪参考路径,根据导引虚拟小船的运动学特性(1),获取导引虚拟小船运动状态信息的时间序列信号(导引虚拟小船的艏向角ψr、前进速度ur、位置坐标(xr,yr)按照时间顺序的排列);
如图6所示从左至右依次为实船、动态虚拟小船和导引虚拟小船。导引虚拟小船和动态虚拟小船均具有如公式(1)的运动学特性:
其中(xr,yr)、(xd,yd)分别为导引虚拟小船和动态虚拟小船的坐标,ur、ud分别为导引虚拟小船的前进速度和动态虚拟小船的最终前进速度,ψr、ψd分别为导引虚拟小船的艏向角和动态虚拟小船的最终艏向角。导引虚拟小船的前进速度ur和艏向角ψr与现有技术中动态虚拟小船路径跟踪制导算法的计算方法相同。
选取导引虚拟小船运动状态信息时间序列的第一组数据作为导引虚拟小船初始状态信息;实船所有运动状态信息可以通过船用导航设备(GPS、计程仪等)测得;对动态虚拟小船的位置信息进行设定,其位置处于实船和导引虚拟小船的连线中点上,其全部运动状态信息可通过下述步骤补全。
(2)根据动态虚拟小船和导引虚拟小船的当前位置信息,计算动态虚拟小船的路径跟踪艏向角ψdp和路径跟踪前进速度udp。动态虚拟小船的路径跟踪艏向角ψdp采用公式(2)进行计算:
动态虚拟小船的路径跟踪前进速度udp采用公式(3)进行计算:
udp=klldr+ur cos(ψdp-ψr) (3)
其中ldr为动态虚拟小船与导引虚拟小船间的距离,kl为预设参数,其数值大小是根据动态虚拟小船在路径跟踪情况下对导引虚拟小船位置的收敛速率进行设置,其值越大,收敛越快,但考虑到执行器的限制,应当适当选取。
区别于传统动态虚拟小船路径跟踪制导算法,实船相对导引虚拟小船位置不限制动态虚拟小船的生成。在该算法中,其本身按照(1)所示的运动学关系独立航行,从公式(2)和(3)可知,动态虚拟小船的路径跟踪艏向角ψdp和路径跟踪速度udp通过其本身对于导引虚拟小船的关系生成,其运动过程是连续的,便于同后续避障策略结合。
(3)实时监测动态虚拟小船到障碍物安全圆族边界的最小距离lmin,并根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离lmin确定动态虚拟小船的最终艏向角ψd;
根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离lmin确定动态虚拟小船的最终艏向角ψd包括:
1)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin小于等于船舶障碍物探测设备的激活圈半径Ractive时,执行避障算法以确定最终艏向角ψd:
如图7所示避障算法原理为:根据船舶上障碍物探测设备(如导航雷达,摄像设备等)的作用半径确定避障算法的探测圈半径Rtest和激活圈半径Ractive。因为本制导算法要求通过控制算法保证实船对动态虚拟小船位置的快速收敛,所以假设探测圈和激活圈均是以动态虚拟小船的位置为圆心的。当障碍物进入动态虚拟小船探测圈时,在探测到的障碍物的边缘生成半径为Rs的安全圆族,此时若安全圆族全部或部分进入激活圈(lmin≤Ractive),则从路径跟踪制导算法给出的动态虚拟小船路径跟踪艏向角ψdp开始,向两侧搜索能够避免与圆族相交的最近的艏向角和
按照评价函数(4)的最小化原则从两个艏向角和中选取最优值作为制导算法中动态虚拟小船的最终艏向角ψd:
其中i=1,2,ε是自定义的权重值,ψd(j-1)代表上一个采样时间点的最终艏向角;
该评价函数不仅保证了最终艏向角ψd距离ψdp偏差小,同时具有连续性,避免了最优值来回切换引起的抖振。另外,因为障碍物边界的圆族是逐渐进入激活圈的,所以路径跟踪和避障算法的切换是光滑的。
2)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin大于船舶中障碍物探测设备的激活圈半径Ractive时,动态虚拟小船的最终艏向角ψd=ψdp。当障碍物在动态虚拟小船探测圈外时,理论上lmin是无法测得的,此时定义lmin=Rtest。
(4)计算动态虚拟小船的最终前进速度ud和转艏角速度rd;
在该步骤中,出于安全性的考虑,船舶在遇到障碍物时需要降速,同时,考虑到执行器有界性,动态虚拟小船在距离实船较远时,应当降速等待实船追踪,本发明将采用公式(5)和(6)对动态虚拟小船的最终前进速度ud进行规划:
其中,lmin为上述步骤定义的最小距离,Rc∈[0,Ractive/2],为手动设置的一个阈值,其值的大小决定了过渡区间(如图8所示,过渡区间表示u′dp过渡到udo对应的lmin区间)的大小,为了保证避障的安全性,选取时应在保证一定过渡区间的原则下尽量取大,udo为手动设置的避障过程的动态虚拟小船速度最小值,选取时应满足避碰过程中船舶安全性的要求,ldbset为手动设置的实船到动态虚拟小船间距离上界,其值的选取要满足控制算法作用下实船对动态虚拟小船追踪的要求,实船对动态虚拟小船追踪越快,ldbset可选取越大。ldb为实船距离动态虚拟小船的实际距离。
公式(6)中的u′d采用公式(5)进行规划,图8所示为采用公式(5)对u′d进行规划的示意图。其中,公式(5)中的u′dp值计算过程如下:首先设置动态虚拟小船路径跟踪速度最大值udpmax,按照公式(3)计算动态虚拟小船路径跟踪速度udp,若udp≥udp max,则保持u′dp=udp max,否则令u′dp=udp。跟据公式(6)可知当ldb等于ldbset时ud=0,动态虚拟小船保持原地不动,又知控制回路会保证实船收敛到动态虚拟小船的位置上,由此保证了动态虚拟小船距离实船不会超过ldbset。
采用如公式(7)所示的1阶滤波器进行估计动态虚拟小船的转艏角速度rd:
其中,βψd为动态虚拟小船艏向角ψd的估计值,τ为设定参数,其值的大小是根据滤波器的滤波性能和对rd估计的准确性进行设定,其值越大,滤波性能越好,但估计的准确性越低,选取时要两者兼顾。
(5)判断动态虚拟小船与导引虚拟小船间的距离ldr与设定的阈值距离ldrset的大小,如果动态虚拟小船与导引虚拟小船的距离ldr大于等于阈值距离ldrset,则导引虚拟小船的下一时间点运动状态保持当前采样时间点的运动状态不变,如果动态虚拟小船与导引虚拟小船的距离ldr小于阈值距离ldrset,则导引虚拟小船下一时间点的运动状态按照其时间序列的排序下移。
在动态虚拟小船执行避障算法的过程中,前进速度会下降,而导引虚拟小船的速度始终为ur,如果不对导引虚拟小船加以限制,则会造成动态虚拟小船距离导引虚拟小船过远,影响路径跟踪效果。因此,设置动态虚拟小船与导引虚拟小船间的阈值距离ldrset,通过限制动态虚拟小船与导引虚拟小船的距离ldr,可以实现精确路径跟踪,当ldr≥ldrset时,导引虚拟小船的下一时间点运动状态会保持当前采样时间点的状态不变,这相当于导引虚拟小船会对动态虚拟小船进行等待,避免动态虚拟小船距离导引虚拟小船过远。
(6)将制导信息输入到控制回路中并更新动态虚拟小船下一时间点的位置信息。
将动态虚拟小船的当前状态变量(最终艏向角ψd,转艏角速度rd,最终速度ud,位置坐标xd,yd)作为制导信息代入到控制器中,控制实船收敛到动态虚拟小船的位置上。根据动态虚拟小船的运动学公式(1)和当前状态变量信息计算下一时间点的动态虚拟小船的位置信息。
(7)测量实船位置判断是否到达目的地,如果“是”,结束船舶航行;如果“否”,时间更新t=t+1并进入步骤(2)。
实施例:为了验证本发明所提出制导算法的有效性,这部分将以大连海事大学科研实习船“育鲲”轮为被控对象(见图10),开展***仿真实验进行算法测试。表1给出了“育鲲”轮主要参数,其数学模型采用式(21)所示的三自由度非线性机理模型,其水动力系数基于相关实船操纵性试验数据进行***辨识获取,关于模型辨识建模的理论成果已在国际期刊ISA Transactions公开发表,在此不做赘述。
表1.“育鲲”轮主要参数
其中,高阶流体动力项表达式为
该实验中,计划航线由5个航路点W1(200,0),W2(200,600),W3(800,900),W4(800,1500),W5(1400,1800)确定。考虑“育鲲”轮穿过渔船作业区的情况,渔船在作业过程中可以视为静止或慢时变的目标,随机在计划航线附近生成多个障碍物。船舶初始状态为[x,y,ψ,u,v,r]t=0=[0m,0m,90deg,0m/s,0m/s,0deg/s],制导算法参数设置为:Rtest=130m,Ractive=100m,Rs=10m,ε=0.6,Rc=30m,udp max=20m/s,udo=4m/s,ldbset=80m,ldrset=100m,kl=0.2,τ=0.1。为了更加贴近真实环境,实验中考虑了风、浪、流对控制***的扰动,相关干扰机理模型参见文献[10]。控制算法采用文献[4]提出的鲁棒神经自适应控制律,这一应用体现了本发明与先进控制算法的良好结合。
仿真实验所使用环境干扰为:风速(蒲福风7级)Vwind=15.25m/s,风向ψwind=50deg;海浪干扰由风干扰模型耦合产生,即为在蒲福风7级情况下充分成长生成的海浪,图11给出了试验海浪干扰的三维视图;海流Vcurrent=0.5m/s,流向βcurrent=280deg。图12-14给出了在上述实验条件下,利用动态虚拟小船DVS路径跟踪避障制导算法得到的结果。从图12可以看出,在导引虚拟小船GVS进行直线和曲线路径规划的基础上,动态虚拟小船DVS能够有效跟踪GVS轨迹并灵活地进行多障碍物的避障,导引船舶实现有效的航迹保持及避障控制。图13和图14分别给出了船舶运动姿态变量u,v,ψ和控制输入n,δ的时间变化曲线,可以看出,船舶姿态变化平稳,基于该制导算法的控制输入符合航海实践中执行装置的要求,通过推进装置与舵机的配合,可实现精确的路径跟踪及避障控制。
文献1:张显库.船舶运动简捷鲁棒自适应控制.北京:科学出版社,2012.
文献2:Tang P,Zhang R,Liu D,et al.Local reactive obstacle avoidanceapproach for high-speed unmanned surface vehicle[J].Ocean Engineering,2015,106:128-140.
文献3:Fossen T I,Breivik M,and Skjetne R.Line of sight path followingof underactuated marine craft[C]//Ifac Conference on Manoeuvering and Controlof Marine Craft.2003:244-249.
文献4:Guoqing Zhang,Xianku Zhang.A novel DVS guidance principle androbust adaptive path-following control for underactuated ships using lowfrequency gain-learning[J].ISA Transactions,2015,56:75-85.
文献5:Kuwata Y,Wolf M T,Zarzhitsky D,et al.Safe Maritime AutonomousNavigation With COLREGS,Using Velocity Obstacles[J].IEEE Journal of OceanicEngineering,2014,39:110-119.
文献6:Koren Y,Borenstein J.Potential field methods and their inherentlimitations for mobile robot navigation[C]//IEEE International Conference onRobotics and Automation.1991:1398-1404.
文献7:Moe S,Pettersen K Y.Set-based Line-of-Sight(LOS)path followingwith collision avoidance for underactuated unmanned surface vessel[C]//Mediterranean Conference on Control and Automation.2016.
文献8:Zereik E,Sorbara A,Bibuli M,et al.Priority Task Approach forUSVs’Path Following Missions with Obstacles Avoidance and Speed Regulation[C]//Ifac Conference on Manouvering and Control of Marine Craft.2015,48:25-30.
文献9:Soltan R A,Ashrafiuon H,Muske K R.ODE-based obstacle avoidanceand trajectory planning for unmanned surface vessels[J].Robotica,2011,29(5):691-703.
文献10:贾欣乐,杨盐生.船舶运动数学模型.大连:大连海事大学出版社,1999。
Claims (3)
1.一种路径跟踪避障制导方法,其特征在于包括以下步骤:
(1)执行路径规划并获取导引虚拟小船的时间序列信号;
(2)根据动态虚拟小船和导引虚拟小船的位置信息,计算动态虚拟小船的路径跟踪艏向角ψdp和路径跟踪前进速度udp;
(3)实时监测动态虚拟小船到障碍物安全圆族边界的最小距离lmin,并根据实时监测的最小距离lmin确定动态虚拟小船的最终艏向角ψd,
所述根据实时监测的动态虚拟小船到障碍物安全圆族边界的最小距离lmin确定动态虚拟小船的最终艏向角ψd包括:
1)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin小于等于船舶障碍物探测设备的激活圈半径Ractive时,执行避障算法以确定最终艏向角ψd:
避障算法原理为:根据船舶上障碍物探测设备的作用半径确定避障算法的探测圈半径Rtest和激活圈半径Ractive,当障碍物进入探测圈时,在探测到的障碍物的边缘生成半径为Rs的安全圆族,此时若安全圆族部分或全部进入激活圈,从路径跟踪制导算法给出的动态虚拟小船路径跟踪艏向角ψdp开始,向两侧搜索能够避免与圆族相交的最近的艏向角和
按照评价函数(4)的最小化原则从两个艏向角和中选取最优值作为制导算法中动态虚拟小船的最终艏向角ψd:
其中i=1,2,ε是自定义的权重值,ψd(j-1)代表上一个采样时间点的最终艏向角;
2)当动态虚拟小船到障碍物安全圆族边界的最小距离lmin大于船舶中障碍物探测设备的激活圈半径Ractive时,动态虚拟小船的最终艏向角ψd=ψdp;
(4)计算动态虚拟小船的最终前进速度ud和转艏角速度rd;
(5)判断动态虚拟小船与导引虚拟小船间的距离ldr与设定的阈值距离ldrset的大小,如果动态虚拟小船与导引虚拟小船的距离ldr大于等于阈值距离ldrset,则导引虚拟小船下一时间点的运动状态保持当前状态不变;如果动态虚拟小船与导引虚拟小船的距离ldr小于阈值距离ldrset,则导引虚拟小船下一时间点运动状态信息按照其时间序列的排序下移;
(6)将制导信息输入到控制回路中并更新动态虚拟小船下一时间点的位置信息;
(7)测量实船位置,判断是否到达目的地,如果“是”,结束船舶航行;如果“否”,时间更新t=t+1并进入步骤(2)。
2.根据权利要求1所述的路径跟踪避障制导方法,其特征在于:在步骤(2)中所述动态虚拟小船的路径跟踪艏向角ψdp采用公式(2)进行计算:
其中,(xr,yr),(xd,yd)分别为导引虚拟小船和动态虚拟小船的坐标;
所述动态虚拟小船的路径跟踪前进速度udp采用公式(3)进行计算:
udp=klldr+ur cos(ψdp-ψr) (3)
其中ldr为动态虚拟小船与导引虚拟小船间的距离,ψr为导引虚拟小船的艏向角、ur为前进速度,kl为预设参数,其数值大小是根据动态虚拟小船在路径跟踪情况下对导引虚拟小船位置的收敛速率进行设置,其值越大,收敛越快,但考虑到执行器的限制,应当适当选取。
3.根据权利要求2所述的路径跟踪避障制导方法,其特征在于:在所述步骤(4)中,所述动态虚拟小船的最终前进速度ud采用公式(5)(6)进行计算:
其中,lmin为障碍物安全圆族边界到动态虚拟小船的最小距离,Rc∈[0,Ractive/2],为手动设置的一个阈值,udo为手动设置的避障过程的最小值,ldbset为手动设置的实船到动态虚拟小船间距离上界,ldb为实船距离动态虚拟小船的实际距离,Rtest为探测圈半径,Ractive为激活圈半径,u′dp值计算过程如下:首先设置动态虚拟小船路径跟踪速度最大值udpmax,按照公式(3)计算动态虚拟小船路径跟踪速度udp,若udp≥udpmax,则保持u′dp=udpmax,否则令u′dp=udp;
所述动态虚拟小船的转艏角速度rd采用如公式(7)所示的1阶滤波器进行估计:
其中,βψd为动态虚拟小船艏向角ψd的估计值,τ为设定参数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710374707.2A CN106959698B (zh) | 2017-05-24 | 2017-05-24 | 一种路径跟踪避障制导方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710374707.2A CN106959698B (zh) | 2017-05-24 | 2017-05-24 | 一种路径跟踪避障制导方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106959698A CN106959698A (zh) | 2017-07-18 |
CN106959698B true CN106959698B (zh) | 2019-11-22 |
Family
ID=59482158
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710374707.2A Active CN106959698B (zh) | 2017-05-24 | 2017-05-24 | 一种路径跟踪避障制导方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106959698B (zh) |
Families Citing this family (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107544534A (zh) * | 2017-10-16 | 2018-01-05 | 中国矿业大学 | 一种基于bds、ins的植保无人机自动精细作业及避障方法 |
CN107966152B (zh) * | 2017-11-22 | 2021-05-07 | 大连海事大学 | 一种具有碰撞风险预测机制的避碰及路径跟踪制导方法 |
CN108459602B (zh) * | 2018-02-28 | 2021-03-30 | 哈尔滨工程大学 | 多障碍复杂环境下欠驱动无人艇的自主靠泊方法 |
CN108958245A (zh) * | 2018-06-29 | 2018-12-07 | 北京理工大学 | 一种基于时间序列的无人车路径跟踪算法 |
CN108922088A (zh) * | 2018-07-05 | 2018-11-30 | 中国船舶重工集团公司第七六研究所 | 无人船防盗***及其控制方法 |
CN108981716B (zh) * | 2018-08-22 | 2020-06-16 | 集美大学 | 一种适用于内陆和近海无人船的路径规划方法 |
US10990099B2 (en) * | 2018-11-16 | 2021-04-27 | Great Wall Motor Company Limited | Motion planning methods and systems for autonomous vehicle |
TWI714040B (zh) * | 2019-03-27 | 2020-12-21 | 財團法人船舶暨海洋產業研發中心 | 船舶導航系統及其導航方法 |
CN109976349B (zh) * | 2019-04-12 | 2021-09-24 | 大连海事大学 | 一种含约束无人船的路径跟踪制导与控制结构的设计方法 |
CN112035992B (zh) * | 2019-05-14 | 2024-01-09 | 中国科学院沈阳自动化研究所 | 一种基于多目标优化的自主遥控水下机器人共享控制方法 |
CN110083159B (zh) * | 2019-05-14 | 2022-03-08 | 江苏科技大学 | 基于sbg和动态窗口约束的无人船自主动态避碰方法 |
CN110415562B (zh) * | 2019-06-14 | 2021-03-02 | 青岛科技大学 | 一种受限水域冲突会遇船舶集群态势分析方法 |
CN110444046B (zh) * | 2019-06-14 | 2021-03-02 | 青岛科技大学 | 一种受限水域非冲突会遇船舶集群态势分析方法 |
CN110398248B (zh) * | 2019-07-22 | 2023-02-28 | 广东华风海洋信息***服务有限公司 | 一种基于网格优化的航海避障算法 |
CN111928853B (zh) * | 2020-07-30 | 2023-06-02 | 西南电子技术研究所(中国电子科技集团公司第十研究所) | 复杂环境下空基平台快速航路规划方法 |
CN111930119B (zh) * | 2020-07-31 | 2022-04-08 | 河海大学 | 流速自适应的无人船自主规划路径和运动精确跟踪方法 |
CN112729303B (zh) * | 2020-12-19 | 2023-04-25 | 华瑞物流股份有限公司 | 一种海运路线规划方法、***、设备和存储介质 |
CN113359737A (zh) * | 2021-06-17 | 2021-09-07 | 大连海事大学 | 一种考虑队形伸缩的船舶编队自适应事件触发控制方法 |
CN115182408B (zh) * | 2022-08-10 | 2024-03-15 | 江苏科技大学 | 一种内河水域清理器避障方法及自主移动方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104020771A (zh) * | 2014-06-13 | 2014-09-03 | 大连海事大学 | 一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法 |
CN104020776A (zh) * | 2014-06-13 | 2014-09-03 | 哈尔滨工程大学 | Uuv对拥有多个禁航区的运动母船的跟踪方法 |
CN105955268A (zh) * | 2016-05-12 | 2016-09-21 | 哈尔滨工程大学 | 一种考虑局部避碰的uuv动目标滑模跟踪控制方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10019006B2 (en) * | 2015-04-08 | 2018-07-10 | University Of Maryland, College Park | Surface vehicle trajectory planning systems, devices, and methods |
-
2017
- 2017-05-24 CN CN201710374707.2A patent/CN106959698B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104020771A (zh) * | 2014-06-13 | 2014-09-03 | 大连海事大学 | 一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法 |
CN104020776A (zh) * | 2014-06-13 | 2014-09-03 | 哈尔滨工程大学 | Uuv对拥有多个禁航区的运动母船的跟踪方法 |
CN105955268A (zh) * | 2016-05-12 | 2016-09-21 | 哈尔滨工程大学 | 一种考虑局部避碰的uuv动目标滑模跟踪控制方法 |
Non-Patent Citations (2)
Title |
---|
"Set-Based Line-of-Sight (LOS) Path Following with Collision Avoidance for Underactuated Unmanned Surface Vessel";Signe Moe 等;《24th Mediterranean Conference on Control and Automation》;20160624;全文 * |
"Synchronization Motion Tracking Control of Multiple Underactuated Ships With Collision Avoidance";K.D.Do;《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》;20160531;第63卷(第5期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN106959698A (zh) | 2017-07-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106959698B (zh) | 一种路径跟踪避障制导方法 | |
CN107168335B (zh) | 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 | |
CN105955268B (zh) | 一种考虑局部避碰的uuv动目标滑模跟踪控制方法 | |
US9816812B2 (en) | Systems and methods for automated vessel navigation using sea state prediction | |
CN105022270B (zh) | 基于速度矢量坐标系的船自动避碰方法 | |
CN104020771B (zh) | 一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法 | |
Zhang et al. | An adaptive obstacle avoidance algorithm for unmanned surface vehicle in complicated marine environments | |
CN113759939B (zh) | 一种受限水域智能航行方法及装置 | |
Han et al. | Automatic berthing for an underactuated unmanned surface vehicle: A real-time motion planning approach | |
CN107966152B (zh) | 一种具有碰撞风险预测机制的避碰及路径跟踪制导方法 | |
CN104571128B (zh) | 基于距离与视差信息的水下机器人避障方法 | |
CN112650231B (zh) | 一种实现避碰和避障的欠驱船编队控制*** | |
CN108829134A (zh) | 一种深海机器人实时自主避障方法 | |
Zhang et al. | A real-time multi-ship collision avoidance decision-making system for autonomous ships considering ship motion uncertainty | |
CN115903800A (zh) | 一种基于领导者协同的多无人船编队的严格安全控制方法 | |
Nie et al. | Planar path-following tracking control for an autonomous underwater vehicle in the horizontal plane | |
CN114355899A (zh) | 一种基于动态导航船域与速度障碍的无人船避碰方法 | |
bin Mansor | Motion control algorithm for path following and trajectory tracking for unmanned surface vehicle: A review paper | |
Sun et al. | An innovative distributed self-organizing control of unmanned surface vehicle swarm with collision avoidance | |
CA3102764A1 (en) | Dynamic trawl steering system | |
CN113359737A (zh) | 一种考虑队形伸缩的船舶编队自适应事件触发控制方法 | |
Yu et al. | Underwater cable tracking control of under-actuated AUV | |
Zhang et al. | A new hybrid path planning method for the sailboat architecture wave glider in the wind field environment | |
Chen et al. | Improved line-of-sight nonlinear trajectory tracking control of autonomous underwater vehicle exposed to high variable speed ocean currents | |
Salarieh et al. | Trajectory optimization for a high speed planing boat based on Gauss pseudospectral method |
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 |