CN107168335B - 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 - Google Patents
一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 Download PDFInfo
- Publication number
- CN107168335B CN107168335B CN201710504472.4A CN201710504472A CN107168335B CN 107168335 B CN107168335 B CN 107168335B CN 201710504472 A CN201710504472 A CN 201710504472A CN 107168335 B CN107168335 B CN 107168335B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- obstacle avoidance
- dynamic
- boat
- guidance
- 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 43
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 title claims abstract description 10
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 67
- 230000008569 process Effects 0.000 claims abstract description 18
- 230000007704 transition Effects 0.000 claims abstract description 18
- 238000011217 control strategy Methods 0.000 claims abstract description 8
- 238000012163 sequencing technique Methods 0.000 claims abstract description 7
- 230000003068 static effect Effects 0.000 claims description 40
- 238000001514 detection method Methods 0.000 claims description 29
- 230000004888 barrier function Effects 0.000 claims description 11
- 230000033001 locomotion Effects 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 11
- 238000011156 evaluation Methods 0.000 claims description 6
- 238000013178 mathematical model Methods 0.000 claims description 6
- 102100029469 WD repeat and HMG-box DNA-binding protein 1 Human genes 0.000 claims description 4
- 101710097421 WD repeat and HMG-box DNA-binding protein 1 Proteins 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000014509 gene expression Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 10
- 238000009395 breeding Methods 0.000 description 4
- 230000001488 breeding effect Effects 0.000 description 3
- 230000007547 defect Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 241000233948 Typha Species 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000013016 damping Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000007480 spreading Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 208000034699 Vitreous floaters Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000005094 computer simulation Methods 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
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000012530 fluid Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000001537 neural effect Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 239000002699 waste material Substances 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
-
- 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/12—Target-seeking control
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)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种考虑混合多目标避障的水面无人艇路径跟踪制导方法,将制导过程分为路径跟踪和避障操纵制导两种模式,以改进DVS制导算法为基本构架,由GVS动态规划出由直线和曲线组成的光滑参考路径,路径跟踪模式和避障操纵模式分别对应不同的DVS的引导变量。对于多个或混合的障碍物的避障制导,首先将根据优先级排序及避障操纵条件评判确定当前的避障目标,启动避障操纵模式,过渡函数保证DVS引导变量在不同模式切换之间的光滑性。本发明对多种控制策略具有适用性,便于同现有先进控制算法结合,控制算法的作用在于保证实船对DVS的收敛,保证制导策略的有效性。
Description
技术领域
本发明涉及船舶控制工程与船舶自动化航行领域,更具体地,涉及一种考虑混合多目标避障的水面无人艇路径跟踪制导方法。
背景技术
通过制导策略实现船舶的航迹保持及避碰/避障是船舶运动控制的关键技术。船舶路径跟踪的制导算法目前应用较广泛的主要有可视距离(LOS)制导算法和动态虚拟小船(DVS)制导算法。避障算法可以借鉴移动机器人和陆上小车的控制,采用基于非线性控制理论稳定极限环(stable limit cycles)概念的障碍物规避算法,即极限环避障算法,该避障算法具有抖振小、安全性能高、避障轨迹平滑的优点。将上述船舶路径跟踪的制导算法同避障算法相结合的制导策略成为近年来研究的热点。下面就现有的路径跟踪的船舶制导算法同极限环避障算法相结合的制导策略进行简要介绍。
文献[1]中给出了LOS制导算法结合避障算法的制导策略。LOS制导算法的基本原理如图1所示:在前向距离▽固定的前提下,LOS制导算法根据实船位置与直线参考路径上的点AF之间的几何关系获得每一采样时间点的路径跟踪期望艏摇角ψlos,前进速度up保持不变,进而引导船舶实现对直线参考路径的跟踪。该制导的策略能够使实船指数收敛到参考直线路径上。
图1中,Pi-1Pi为实船当前跟踪的直线路径,d为实船到路径的垂线距离,(x,y)为实船的当前位置坐标,则可以得到:
考虑实船的欠驱动特性,在期望艏向角中引入漂角的补偿,则可以得到路径跟踪期望艏向角:
当船舶进入航路点Pi的转向边界环(circle of acceptance)时,船舶的跟踪路径切换为PiPi+1。
从上述推导可以看出:LOS制导算法不具备转向点处的路径规划功能,如果在转向点处遇到障碍物会出现制导算法的失效。同时,该算法还不满足“一切参考路径均可以由虚拟小船产生的假设”,难以同路径跟踪控制的相关研究结果结合。
当单船会遇时,通过虚拟定义在会遇船舶上的探测环感知其相对于实船的位置。在路径跟踪模式下,若避障操纵条件符合,则实船由路径跟踪模式切换到避障操纵模式,前进运动由up加速到避障速度uoa,且按照避障期望艏向角ψoa的导引航行。避障期望艏向角可以导引实船收敛到会遇船舶半径小于探测半径的稳定安全极限环上,确保了实船和会遇船舶之间的安全距离。该避障策略的参数说明如图2所示,其中,G表示参考路径。
如图2所示,探测环半径为Rm,安全极限环半径为Ro,当实船进入到探测环内部时,若实船到来船距离σ满足如下避障操纵条件则启动避障操纵模式。
由图2所示的避障策略几何关系可知:
避障操纵模式下,避障期望艏向角ψoa选取如下:
式(5)中,误差e=σ-Ro;Δ为避障操纵模式下手动设定的前向距离(Lookingahead distance);λ根据COLREGs的要求选取为±1,+1表示以顺时针的方向环绕安全极限环,-1表示以逆时针的方向环绕;k用于补偿会遇船舶运动对艏向角ψoa造成的影响,定义为:
在控制算法能够保证实船实际艏向角和实际前进速度对期望艏向角和前进速度的有效收敛前提条件下,本制导策略根据当前障碍物的位置及避障操纵条件,在路径跟踪模式和避障操纵模式之间来回切换。当两船会遇时,首先执行避碰任务,避障期望艏摇角的选取策略会保证实船收敛到来船的安全极限环上,当避障操纵条件不满足时,实船从新回到直线参考路径的路径跟踪任务上。与之对应,实船的期望前进速度也会根据任务模式的不同而进行切换。该制导策略的执行过程如流程图3所示。
具备曲线段路径规划能力非常重要。对于路径跟踪控制部分,不仅需要对直线参考路径进行规划,而且还需要在转向点附件进行曲线段路径规划;对于避障控制部分,由于障碍物出现的位置不确定,所以,曲线段路径规划对避障的有效性尤为重要。
另外,船舶避障/避碰行为主要发生在狭窄繁忙水道或渔船作业区等复杂海洋环境下,避障策略应尽可能应对多种避碰/避障条件并具有多适应性,以体现船舶高度智能化航行的需求。
同时,船舶路径跟踪及避障制导的有效性只有通过控制策略得以保证,制导算法应便于同控制算法相结合。
因此,文献[1]中所阐述的LOS制导算法综合避障控制的制导策略存在的缺陷,总结如下:
(1)该制导算法基于LOS路径跟踪制导算法建立,因此也沿袭了LOS制导算法的缺陷。该算法不具备转向点处的路径规划功能,如果在转向点处遇到障碍物会出现制导算法的失效;该算法不满足“一切参考路径均可以由虚拟小船产生的假设”,难以同欠驱动船舶路径跟踪控制相关研究结果结合。
(2)该制导算法不具备对多障碍物避障的功能,仅仅适用于单船会遇避碰的情况,不适用于复杂海洋环境下及高速船的避障控制。另外,该制导算法路径跟踪模式和避障操纵模式的切换过程存在阶跃,切换过程不平滑。
参考文献
[1]S.Moe,K.Y.Pettersen.Set-Based Line-of-Sight(LOS)Path Followingwith Collision Avoidance for Underactuated Unmanned Surface Vessel[C].24thMediterrean Conference on Control and Automation,2016:402-409.
发明内容
本发明的目的在于克服现有技术存在的上述缺陷,提供一种便于同控制算法相结合且具有曲线段路径规划能力的DVS制导算法,该制导算法不仅克服了LOS制导算法的缺陷,还在一定程度上满足了路径跟踪的时间要求。另外,本发明设置了避碰优先级及相应的艏向角及速度规划策略,实现了船舶对多静止障碍物、多动态障碍物及混合障碍物的有效避障。
为实现上述目的,本发明的技术方案如下:
一种考虑混合多目标避障的水面无人艇路径跟踪制导方法,包括以下步骤:
S1:设定航路点信息W1,W2,…,Wn,分别建立导引虚拟小船和动态虚拟小船的运动数学模型;
S2:导引虚拟小船根据制导算法规划出参考路径,并始终沿参考路径行驶,启动路径跟踪制导模式,动态虚拟小船的艏向角时刻等于导引虚拟小船相对于动态虚拟小船的真方位角,并向实船发出制导命令,以引导实船进行路径跟踪,实船不断获取动态虚拟小船的导引变量;
S3:在采样时间点进行采样,判断动态虚拟小船是否进入障碍物探测环,如进入障碍物探测环,进入障碍物排序过程,执行S4;如未进入障碍物探测环,保持路径跟踪制导模式,执行S6;
S4:进入障碍物排序过程:对避障物的优先级进行排序,得到障碍物的优先级序列,选取最高优先级障碍物作为当前避障目标,判断当前避障目标是否满足避障操纵条件,若当前避障目标满足避障操纵条件,则进入对当前障碍物的避障制导模式,即执行S5;若当前避障目标不满足避障操纵条件,选取次优先级的障碍物作为当前避障目标;若所有障碍物都不满足避障操纵条件,则进入路径跟踪模式,执行S6;
S5:进入避障制导模式:在每一采样时间点,判断当前障碍物的避障操纵条件是否满足,如满足避障操纵条件,则启动对当前障碍物的避障制导模式,获取动态虚拟小船的导引变量,引导实船;如不满足避障操纵条件,则从障碍物优先级序列中去除当前障碍物,并执行S4;
S6:根据动态虚拟小船的导引变量和控制策略,实时调整实船的执行器输入,控制实船跟踪动态虚拟小船;
S7:测量实船位置,判断是否到达终点,如果是,结束船舶航行,如果否,则执行S2。
进一步地,S1中,所述导引虚拟小船和动态虚拟小船的运动数学模型的表达式为:
进一步地,S4中,所述的障碍物优先级序列的生成方法为:
S41:将障碍物分为静态障碍物和动态障碍物,静态障碍物的优先级高于动态障碍物的优先级;
S42:静态障碍物的优先级与动态虚拟小船到该静态障碍物安全极限环的距离e大小成反比,e最小的障碍物拥有最高的优先级;
S43:动态障碍物的优先级由下列公式确定
其中,F为评价函数,Rm为障碍物探测环的半径,Ro为安全极限环的半径,e为动态虚拟小船到该动态障碍物的安全极限环的距离,为动态虚拟小船在路径跟踪模式下其与障碍物距离的导数,udo表示动态虚拟小船的避障速度,为一个常数,其值要大于所有动态障碍物的速度,为权重参数。
进一步地,S4中,所述避障操纵条件分为对静态障碍物的避障操纵条件和对动态障碍物的避障操纵条件,所述对静态障碍物的避障操纵条件为:动态虚拟小船的艏向在动态虚拟小船到静态障碍物的安全极限环的两条切线之间;所述对动态障碍物的避障操纵条件为下列公式:
进一步地,路径跟踪制导模式和避障制导模式切换时以及不同障碍物的避障制导模式切换时,采用如下时间过渡函数:
其中,α(t)为时间过渡函数,tc为起始切换时间点,ts为手动设定的过渡时间。
进一步地,S5所述的避障制导模式中,动态虚拟小船的速度ud调整为避障速度,艏向角按照下列方法选取:
S51:如当前避障目标为静态障碍物,则
其中,ψdo为动态虚拟小船在避障制导模式中的艏向角,φ为静态障碍物相对动态虚拟小船的真方位角,Δ为设定的前向距离,λ=±1决定动态虚拟小船避障时环绕静态障碍物的方向,+1为顺时针方向环绕静态障碍物的安全极限环,-1为逆时针方向环绕静态障碍物的安全极限环;
S52:如当前避障目标为动态障碍物,则
其中,φ为动态障碍物相对动态虚拟小船的真方位角,Δ为设定的前向距离,k为补偿参数,λ=±1,+1为顺时针环绕动态障碍物的安全极限环,-1为逆时针环绕动态障碍物的安全极限环。
进一步地,S2中,所述动态虚拟小船的导引变量包括艏向角ψd和前进速度ud,艏向角ψd的计算公式为:
其中,g代表GVS,d代表DVS,(x,y)为位置坐标;
前进速度ud的计算公式为:
其中,kd为用于调整收敛速度快慢的设定参数,ldg为动态虚拟小船到导引虚拟小船的距离,ug为导引虚拟小船的前进速度,ψg为导引虚拟小船的艏向角,ψd为动态虚拟小船的艏向角,ldbset为实船到动态虚拟小船的距离的上限设定,ldb为实船到动态虚拟小船的距离。
进一步地,S3中,所述障碍物探测环的半径Rm根据障碍物的大小确定,障碍物越大,其值越大。
从上述技术方案可以看出,本发明通过将制导过程分为路径跟踪和避障操纵制导两种模式,以改进DVS制导算法为基本构架,由GVS动态规划出由直线和曲线组成的光滑参考路径,路径跟踪模式和避障操纵模式分别对应不同的DVS的引导变量。对于多个或混合的障碍物的避障制导,首先将根据优先级排序及避障操纵条件评判确定当前的避障目标,启动避障操纵模式,过渡函数保证DVS引导变量在不同模式切换之间的光滑性。本发明对多种控制策略具有适用性,便于同现有先进控制算法结合,控制算法的作用在于保证实船对DVS的收敛,保证制导策略的有效性。
附图说明
图1是现有技术中LOS制导算法的基本原理图;
图2是现有技术中单船会遇的避障策略的示意图;
图3是现有技术中LOS路径跟踪及避障的制导算法的流程图;
图4是本发明的改进的DVS制导算法的基本原理图;
图5是一种多静态目标避障态势的示意图;
图6是一种多动态目标避障态势的示意图;
图7是一种混合多目标避障态势的示意图;
图8是本发明的混合多目标避障态势中的参数示意图;
图9是本发明的考虑混合多目标避障的水面无人艇路径跟踪制导方法的流程图;
图10是大连海事大学教学实习船“育鲲”轮的示意图;
图11是蒲福风6级海况下的三维波面示意图;
图12~图15分别是t=240s,290s,345s,600s时的实船轨迹图;
图16是一具体实施例中的DVS引导变量的时间变化曲线;
图17是一具体实施例中的控制输入一螺距P和舵角δ的时间变化曲线。
具体实施方式
下面结合附图,对本发明的具体实施方式作进一步的详细说明。
需要说明的是,在下述的具体实施方式中,在详述本发明的实施方式时,为了清楚地表示本发明的结构以便于说明,特对附图中的结构不依照一般比例绘图,并进行了局部放大、变形及简化处理,因此,应避免以此作为对本发明的限定来加以理解。
在以下本发明的具体实施方式中,请参阅图9,图9是本发明的考虑混合多目标避障的水面无人艇路径跟踪制导方法的流程图,结合参考图4~图8,对本发明进一步理解。如图9所示,本发明的考虑混合多目标避障的水面无人艇路径跟踪制导方法,包括以下步骤:
S1:设定航路点信息W1,W2,…,Wn,分别建立导引虚拟小船和动态虚拟小船的运动数学模型。
S2:导引虚拟小船根据制导算法规划出参考路径,并始终沿参考路径行驶,启动路径跟踪制导模式,动态虚拟小船的艏向角时刻等于导引虚拟小船相对于动态虚拟小船的真方位角,并向实船发出制导命令,以引导实船进行路径跟踪,实船不断获取动态虚拟小船的导引变量。
在上述步骤中,请参阅图4,图4是改进的动态虚拟小船(DVS)的制导算法的原理图。在本制导算法中,导引虚拟小船(GVS)和DVS均为不考虑惯性力和阻尼力的理想小船,仅考虑平面运动,它们分别采取独立的运动数学方程为:
GVS根据设定航路点Wi-1,Wi,Wi+1的信息,依据传统DVS制导算法规划出参考路径,即GVS的位置和姿态时间序列。参考路径分为直线参考路径部分和航路点处的曲线参考路径部分,曲线段参考路径通过圆弧的内插法产生,该曲线段参考路径解决了现有技术中在航路点附件不能引导的问题。设定GVS的纵向速度ur和转艏角速度rr,满足实船执行器性能,按照公式(7)的运动学关系沿着直线及曲线段参考路径航行。
在传统DVS制导算法中,在实船与GVS之间生成一个DVS作为实船的直接追踪目标,其位置固定在实船和GVS的连线上,DVS的位置受到约束,不适应多目标避障/避碰的制导。为了进一步满足避障/避碰制导的要求,本发明对DVS制导算法进行了改进,即DVS的生成不再受GVS和实船位置的限制,将其视为按照公式(7)所示运动学关系独立航行的目标。在路径跟踪模式下,DVS艏向角时刻ψd选取为GVS相对于DVS的真方位角ψdp,即式(8):
其中,g代表GVS,d代表DVS,(x,y)为位置坐标。
为了保证路径跟踪模式下DVS对GVS位置的指数收敛,将路径跟踪DVS速度定义为式(9),令ud=udp
其中,kd为用于调整收敛速度快慢的设定参数,其值越大,udp越大,DVS对GVS收敛越快,由此可能造成实船执行器输入越大,所以应根据执行器能力折中选取。ldbset为实船到DVS距离的上限设定,其值的选取也由船舶的执行器能力确定,ldg为动态虚拟小船到导引虚拟小船的距离,ug为导引虚拟小船的前进速度,ψg为导引虚拟小船的艏向角,ψd为动态虚拟小船的艏向角,ldb为实船到动态虚拟小船的距离。从式(9)可以看出,DVS路径跟踪速度和实船到DVS的距离ldb成线性关系。当ldb=ldbset时,速度udp=0,这说明在路径跟踪模式下,DVS不会超出以实船为中心ldbset为半径的圆范围内。该设定保证了在路径跟踪模式下,不会出现因实船距离DVS位置过远而产生的执行器输入过大,该设计考虑了执行器的饱和特性。
由以上论述可知,改进DVS制导算法中的DVS是独立的,可根据其相对于障碍目标的位置实时调整DVS姿态命令以实现避碰/避障引导。
S3:在采样时间点进行采样,判断动态虚拟小船是否进入障碍物探测环,如进入障碍物探测环,进入障碍物排序过程,执行S4;如未进入障碍物探测环,保持路径跟踪制导模式,执行S6。
S4:进入障碍物排序过程:对避障物的优先级进行排序,得到障碍物的优先级序列,选取最高优先级障碍物作为当前避障目标,判断当前避障目标是否满足避障操纵条件,若当前避障目标满足避障操纵条件,则进入对当前障碍物的避障制导模式,即执行S5;若当前避障目标不满足避障操纵条件,选取次优先级的障碍物作为当前避障目标;若所有障碍物都不满足避障操纵条件,则进入路径跟踪模式,执行S6。
S5:进入避障制导模式:在每一采样时间点,判断当前障碍物的避障操纵条件是否满足,如满足避障操纵条件,则启动对当前障碍物的避障制导模式,获取动态虚拟小船的导引变量,引导实船;如不满足避障操纵条件,则从障碍物优先级序列中去除当前障碍物,并执行S4。
S6:根据动态虚拟小船的导引变量和控制策略,实时调整实船的执行器输入,控制实船跟踪动态虚拟小船。
在上述过程中,首先对DVS是否进入障碍物探测环做一个判断。如果进入,则需由路径跟踪制导模式改为避障制导模式,在避障制导模式中,根据障碍物的种类和个数对障碍物进行避障优先级的排序,依次完成避障任务。
为便于避障优先级的排序,本发明将避障任务分成如图5~图7所示的三种避障态势(图5中的H表示静态障碍物,图6中的I表示动态障碍物):分别为多静态目标避障态势、多动态目标避障态势和混合多目标避障态势。所谓多目标避障态势是指DVS处于多个障碍物的探测环内。因为控制算法会保证实船快速收敛到DVS上,所以假设DVS拥有障碍物的探测能力是合理的。本发明通过分配障碍物的优先级逐一对多个障碍物进行避障,当对优先级最高的障碍物进行避障时,不考虑其他障碍物。因为本制导策略可以保证DVS快速收敛到障碍物的安全极限环(如图8所示具有半径Ro的实线圆)上,而障碍物之间(诸如多艘船舶之间,船舶与岛礁之间)也存在安全距离,即障碍物的安全极限环之间不会相交,所以当DVS对某个障碍物进行避障时,不会受到其他障碍物的干扰。图8中所示的障碍物探测环的半径Rm和安全极限环的半径Ro根据障碍物的大小确定,障碍物越大,其值越大。实际上,障碍物探测环和安全极限环是实船探测能力的一种体现。本发明规定多静态避障根据图8中所示DVS距离安全极限环的距离e的大小确定静态障碍物的优先级,e最小的障碍物拥有最高的优先级,依次类推;对于多动态避障,则根据评价函数的最大化原则决定优先级,评价函数的公式如下:
其中,F为评价函数,Rm为障碍物探测环的半径,Ro为安全极限环的半径,e为动态虚拟小船到该动态障碍物的安全极限环的距离,为动态虚拟小船在路径跟踪模式下其与障碍物距离的导数,udo表示动态虚拟小船的避障速度,为一个常数,其值要大于所有动态障碍物的速度,以保证避障的有效性,同时满足实船执行器性能的要求;为权重参数,其值越大,则表示在评价函数中DVS与障碍物的距离越重要,DVS对障碍物的相对速度越不重要,反之亦然。对于同时存在静态和动态障碍物的情况,该制导策略规定静态障碍物相比动态具有更高的优先级,在对全部静态障碍物的优先级进行排序后,再对动态障碍物的优先级进行排序。
在确定障碍物优先级顺序之后,则根据避障操纵条件是否满足决定是否启动避障操纵模式。如图8所示,对于静态障碍物,其避障操纵条件为DVS的艏向在切线l1和l2之间;对于动态障碍物,其避障操纵条件如下式所示:
其中,σ为动态虚拟小船到动态障碍物的距离,Ro为动态障碍物的极限安全环的半径,Rm为动态障碍物的探测环的半径,为路径跟踪模式下动态虚拟小船到动态障碍物距离的导数。即,当动态虚拟小船进入动态障碍物的安全极限环内时,说明马上需要避障,否则会发生碰撞;当动态虚拟小船进入探测环内并且路径跟踪模式下动态虚拟小船到动态障碍物距离的导数小于0时,说明有碰撞的危险,需要采取措施避免碰撞。
如果较高优先级的障碍物不满足避障操纵条件,则算法转而寻找下一优先级的避障操纵条件;如果所有障碍物都不满足避障操纵条件,则算法保持路径跟踪模式不变;如果当前优先级障碍物满足避障操纵条件,则启动避障操纵模式。启动避障操纵模式后的每一采样时间点,制导算法不再对障碍物优先级进行排序,只判断当前障碍物的避障操纵条件是否依旧满足,若不满足,则退出避障操纵模式,判断DVS与障碍物的相对位置,若DVS处于障碍物探测环内,则重新开始优先级排序,否则启动路径跟踪模式。
在避障操纵模式下,首先DVS速度ud会调整为目标避障速度udo。对静态障碍目标的避障DVS艏向角选取为下式:
其中,ψdo为动态虚拟小船在避障制导模式中的艏向角,φ为静态障碍物相对动态虚拟小船的真方位角,Δ为设定的前向距离,其值越小,DVS对极限环的收敛速度越快,由此可能造成实船转向幅度较大,所以应当根据实船转向能力灵活选取。λ=±1决定环绕静态障碍物的方向,+1为顺时针环绕静态障碍物的安全极限环,-1为逆时针环绕静态障碍物的安全极限环。
对动态障碍目标的避障DVS艏向角选取为:
其中,φ为动态障碍物相对动态虚拟小船的真方位角,Δ为设定的前向距离,k为补偿参数,其定义同公式(6),V0=uccos(π-φ+θ),uc为当前动态障碍物的速度,λ=±1,+1为顺时针环绕动态障碍物的安全极限环,-1为逆时针环绕动态障碍物的安全极限环。当动态障碍物为单船时,λ的选取应满足《国际海上避碰规则》(COLREGs)的要求,当动态障碍物为其它,例如漂浮物等,按照以下原则选取:
其中,ψdp为导引虚拟小船相对于动态虚拟小船的真方位角,为动态障碍物相对动态虚拟小船的真方位角。即当导引虚拟小船相对于动态虚拟小船的真方位角小于等于动态障碍物相对动态虚拟小船的真方位角时,则λ=+1,顺时针环绕动态障碍物;当导引虚拟小船相对于动态虚拟小船的真方位角小于等于动态障碍物相对动态虚拟小船的真方位角时,则λ=-1,逆时针环绕动态障碍物。式(14)的障碍物环绕方向选取原则保证了对障碍物的最小环绕长度,减小了航程浪费。
为了保证DVS制导变量(ud,ψd)在避障操纵和路径跟踪两种模式切换时以及不同障碍物避障之间的切换时的光滑性,引入如式(15)所示的时间过渡函数:
其中,α(t)为时间过渡函数,tc为起始切换时间点,ts为手动设定的过渡时间,其值的选取应在不影响避障安全性能的前提下尽可能地保证过渡平缓。以ψd为例,其切换过程如式(16)所示:
ψd(t)=(1-α)ψd_start+αψd_end (16)
其中,ψd将在ts时间内完成从切换起始艏向角ψd_start到目标模式艏向角ψd_end的过渡,其过渡过程是光滑的。对于ud而言,因为目标避障速度udo是人为设定的,且对于所用障碍物都不变,所以ud的过渡仅有udp→udo,udo→udp两种情况。
S7:测量实船位置,判断是否到达终点,如果是,结束船舶航行,如果否,则执行S2。
综上所述,本发明将制导过程分为路径跟踪和避障操纵制导两种模式,以改进DVS制导算法为基本构架,由GVS动态规划出由直线和曲线组成的光滑参考路径,路径跟踪模式和避障操纵模式分别对应不同的DVS的引导变量。对于多个或混合的障碍物的避障制导,首先将根据优先级排序及避障操纵条件评判确定当前的避障目标,启动避障操纵模式,过渡函数保证DVS引导变量在不同模式切换之间的光滑性。本发明还可以对多种控制策略具有适用性,便于同现有先进控制算法结合,控制算法的作用在于保证实船对DVS的收敛,保证制导策略的有效性。
为了验证本发明所提出制导算法的有效性,这部分将以大连海事大学教学实习船“育鲲”(如图10)为被控对象,利用matlab开展计算机仿真实验。表1给出了“育鲲”轮的主要尺度参数。这里采用三自由度欠驱动数学模型(17),其相关水动力系数基于2013年9月“育鲲”轮开展的系列实船操纵性试验,利用先进***辨识算法获得。
表1.“育鲲”轮主要参数
其中高阶流体动力项表达式为
其中,P为螺距,δ为舵角,作为控制***的输入。
该实例中,计划航线由4个航路点W1(200,0),W2(200,1000),W3(1200,1500),W4(1200,2500)确定。在计划航线附近设置多静态障碍物、多动态障碍物以及动静混合障碍物,模拟船舶在穿越多岛礁水域和航运繁忙水域的情况。船舶初始状态为[x,y,ψ,u,v,r]t=0=[0m,0m,90deg,0m/s,0m/s,0deg/s],制导算法参数设置为:ug=10Kn,udo=12Kn,ldbset=200m,kd=0.05,Δ=20m,ts=2s为了更加贴近真实环境,仿真中考虑了风、浪、流的海洋环境干扰。控制算法鲁棒神经阻尼控制律,这一应用体现了本发明与先进控制算法的良好结合。
仿真实验所使用环境干扰为:风速(蒲福风6级)Vwind=15.25m/s,风向ψwind=50deg;海浪干扰由风干扰模型耦合产生,即为在蒲福风7级情况下充分成长生成的海浪,图11给出了试验海浪干扰的三维视图;海流Vcurrent=0.5m/s,流向βcurrent=280deg。图12~图15给出了在上述实验条件下,4个时间点采用本发明的制导策略得到的实船轨迹图。可以看出该制导和控制策略保证了实船在避障时有效地收敛到障碍物的安全极限环上,避障结束后快速回归到路径跟踪模式上,单船避障满足COLREGs的要求,多船避障按照优先级顺序执行避障任务。图16给出了制导策略得到的引导变量在整个实验过程中随时间的变化,可以看出制导策略中过渡函数的引入保证了模式切换之间引导变量光滑的过渡。图17给出了实验过程中的控制输入—螺距P和舵角δ,控制输入满足执行器有界的要求,抖振较小,可以直接作为执行器的输入,贴近工程实际。
通过上述仿真实验,并同已有研究进行对比,现将本发明带来的有益效果总结为以下3点:
1)本发明提出了一种适用于存在多静态目标、多动态目标和混合多目标障碍物的复杂航行态势的改进DVS制导技术,适用范围更广。所提出的制导算法创造性地将船舶路径跟踪制导同混合避障制导结合。本策略下的混合避障制导机制具有多静态障碍物避障、多动态障碍物避障和混合多目标障碍物避障的能力,在结束避障任务后,船舶可以迅速回归到路径跟踪任务上,其适用范围更广。
2)基于极限环的避障策略可以确保船舶快速地收敛到障碍物的安全极限环上,安全性能较高。该算法中,通过合理设计避障目标的安全极限环和探测环(由船载设备的测量能力决定,如雷达、摄像机等)可以灵活调整船舶避障操纵的安全域度和避障操纵后恢复航线的收敛速度。实际工程中,避障目标的安全极限环和探测环是实船探测能力的一种体现,实船进入障碍物探测环的过程等效于障碍物进入实船探测范围的过程,故本制导算法便于工程实现。
3)本发明提出的制导策略继承了DVS制导策略的优点,能够在直线段、曲线段航线以及多种船舶航行态势下提供合理有效的制导机制,引导船舶完成智能航行任务;该策略具有控制理论的通用性,为先进控制算法和工程实践的结合搭建了桥梁;该策略考虑了船舶执行装置的限制,符合“绿色、节能”的主题。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,根据本发明的技术方案及其发明构思加以等同替换或改变,都应涵盖在本发明的保护范围之内。
Claims (5)
1.一种考虑混合多目标避障的水面无人艇路径跟踪制导方法,其特征在于包括以下步骤:
S1:设定航路点信息W1,W2,…,Wn,分别建立导引虚拟小船和动态虚拟小船的运动数学模型;其中,所述导引虚拟小船和动态虚拟小船的运动数学模型的表达式为:
S2:导引虚拟小船根据制导算法规划出参考路径,并始终沿参考路径行驶,启动路径跟踪制导模式,动态虚拟小船的艏向角时刻等于导引虚拟小船相对于动态虚拟小船的真方位角,并向实船发出制导命令,以引导实船进行路径跟踪,实船不断获取动态虚拟小船的导引变量;其中,所述动态虚拟小船的导引变量包括艏向角ψd和前进速度ud,艏向角ψd的计算公式为:
其中,d代表动态虚拟小船,g代表导引虚拟小船,(x,y)为位置坐标;
前进速度ud的计算公式为:
其中,kd为用于调整收敛速度快慢的设定参数,ldg为动态虚拟小船到导引虚拟小船的距离,ug为导引虚拟小船的前进速度,ψg为导引虚拟小船的艏向角,ψd为动态虚拟小船的艏向角,ldbset为实船到动态虚拟小船的距离的上限设定,ldb为实船到动态虚拟小船的距离;
S3:在采样时间点进行采样,判断动态虚拟小船是否进入障碍物探测环,如进入障碍物探测环,进入障碍物排序过程,执行S4;如未进入障碍物探测环,保持路径跟踪制导模式,执行S6;
S4:进入障碍物排序过程:对避障物的优先级进行排序,得到障碍物的优先级序列,选取最高优先级障碍物作为当前避障目标,判断当前避障目标是否满足避障操纵条件,若当前避障目标满足避障操纵条件,则进入对当前障碍物的避障制导模式,即执行S5;若当前避障目标不满足避障操纵条件,选取次优先级的障碍物作为当前避障目标;若所有障碍物都不满足避障操纵条件,则进入路径跟踪模式,执行S6;
S5:进入避障制导模式:在每一采样时间点,判断当前障碍物的避障操纵条件是否满足,如满足避障操纵条件,则启动对当前障碍物的避障制导模式,获取动态虚拟小船的导引变量,引导实船;如不满足避障操纵条件,则从障碍物优先级序列中去除当前障碍物,并执行S4;其中,S5所述的避障制导模式中,动态虚拟小船的速度ud调整为避障速度,艏向角按照下列方法选取:
S51:如当前避障目标为静态障碍物,则
其中,ψdo为动态虚拟小船在避障制导模式中的艏向角,φ为静态障碍物相对动态虚拟小船的真方位角,e为动态虚拟小船到该静态障碍物的安全极限环的距离,Δ为设定的前向距离,λ=±1决定动态虚拟小船避障时环绕静态障碍物的方向,+1为顺时针方向环绕静态障碍物的安全极限环,-1为逆时针方向环绕静态障碍物的安全极限环;
S52:如当前避障目标为动态障碍物,则
其中,φ为动态障碍物相对动态虚拟小船的真方位角,Δ为设定的前向距离,e为动态虚拟小船到该动态障碍物的安全极限环的距离,λ=±1,+1为顺时针环绕动态障碍物的安全极限环,-1为逆时针环绕动态障碍物的安全极限环;补偿参数k用于补偿会遇船舶运动对艏向角造成的影响,其定义为:
uc为当前动态障碍物的速度;udo表示动态虚拟小船的避障速度;
S6:根据动态虚拟小船的导引变量和控制策略,实时调整实船的执行器输入,控制实船跟踪动态虚拟小船;
S7:测量实船位置,判断是否到达终点,如果是,结束船舶航行,如果否,则执行S2。
2.根据权利要求1所述的考虑混合多目标避障的水面无人艇路径跟踪制导方法,其特征在于,S4中,所述的障碍物优先级序列的生成方法为:
S41:将障碍物分为静态障碍物和动态障碍物,静态障碍物的优先级高于动态障碍物的优先级;
S42:静态障碍物的优先级与动态虚拟小船到该静态障碍物安全极限环的距离e大小成反比,e最小的障碍物拥有最高的优先级;
S43:动态障碍物的优先级由下列公式确定
5.根据权利要求1所述的考虑混合多目标避障的水面无人艇路径跟踪制导方法,其特征在于,S3中,所述障碍物探测环的半径Rm根据障碍物的大小确定,障碍物越大,其值越大。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710504472.4A CN107168335B (zh) | 2017-06-28 | 2017-06-28 | 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710504472.4A CN107168335B (zh) | 2017-06-28 | 2017-06-28 | 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107168335A CN107168335A (zh) | 2017-09-15 |
CN107168335B true CN107168335B (zh) | 2020-06-23 |
Family
ID=59827740
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710504472.4A Active CN107168335B (zh) | 2017-06-28 | 2017-06-28 | 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107168335B (zh) |
Families Citing this family (27)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107544500B (zh) * | 2017-09-18 | 2020-12-29 | 哈尔滨工程大学 | 一种考虑约束的无人艇靠泊行为轨迹规划方法 |
CN107966152B (zh) * | 2017-11-22 | 2021-05-07 | 大连海事大学 | 一种具有碰撞风险预测机制的避碰及路径跟踪制导方法 |
CN108459602B (zh) * | 2018-02-28 | 2021-03-30 | 哈尔滨工程大学 | 多障碍复杂环境下欠驱动无人艇的自主靠泊方法 |
CN109489672B (zh) * | 2018-03-25 | 2022-03-18 | 哈尔滨工程大学 | 考虑海流与无人艇动力学影响的节能a星路径规划方法 |
US11126199B2 (en) * | 2018-04-16 | 2021-09-21 | Baidu Usa Llc | Learning based speed planner for autonomous driving vehicles |
CN108762074B (zh) * | 2018-05-23 | 2020-12-29 | 大连海事大学 | 一种面向恶劣海况船舶安全保障能力提升的船舶控制方法 |
CN108563113B (zh) * | 2018-06-12 | 2020-11-20 | 哈尔滨工程大学 | 舰船用多回路无模型自适应艏向控制方法 |
CN109752957B (zh) * | 2019-01-11 | 2021-07-30 | 大连海事大学 | 一种无人艇的制导指令调节器结构及设计方法 |
CN109916419A (zh) * | 2019-03-12 | 2019-06-21 | 哈尔滨工程大学 | 一种面向对象的混合遗传算法无人艇实时路径规划方法 |
CN109857124B (zh) * | 2019-03-22 | 2021-09-24 | 大连海事大学 | 基于速度与航向双重制导的无人船精确路径跟踪控制方法 |
CN109960262B (zh) * | 2019-03-25 | 2020-05-19 | 华中科技大学 | 一种基于几何法的无人艇动态避障方法和*** |
CN110018687B (zh) * | 2019-04-09 | 2022-03-04 | 大连海事大学 | 基于强化学习方法的无人水面船最优轨迹跟踪控制方法 |
CN110083159B (zh) * | 2019-05-14 | 2022-03-08 | 江苏科技大学 | 基于sbg和动态窗口约束的无人船自主动态避碰方法 |
CN110134130A (zh) * | 2019-06-14 | 2019-08-16 | 西交利物浦大学 | 一种基于改进角度势场法的无人船自主避障方法 |
CN110758669B (zh) * | 2019-10-30 | 2021-01-26 | 青岛科技大学 | 一种开阔繁忙水域的双层相对领域获取方法 |
CN111487966B (zh) * | 2020-04-13 | 2022-09-09 | 哈尔滨工程大学 | 一种基于航路点的水面无人艇自适应路径跟踪控制方法 |
CN111798701B (zh) * | 2020-07-07 | 2022-07-26 | 中国船舶工业***工程研究院 | 无人艇路径跟踪控制方法、***、存储介质及终端 |
CN111951609B (zh) * | 2020-07-24 | 2022-08-09 | 深圳市云洲创新科技有限公司 | 桥梁主动防撞预警方法、装置、设备及存储介质 |
CN112422184B (zh) * | 2020-09-28 | 2022-05-27 | 东方红卫星移动通信有限公司 | 一种用于空间光通信的粗指向装置的转动控制方法及装置 |
CN113031598B (zh) * | 2021-03-02 | 2024-02-06 | 中国人民解放***箭军工程大学 | 一种路径跟踪避障制导方法及*** |
CN113093742B (zh) * | 2021-03-30 | 2023-05-26 | 大连海事大学 | 一种自动规避多障碍物的无人船路径跟踪*** |
CN112947494B (zh) * | 2021-04-22 | 2022-10-04 | 中国船舶重工集团公司第七0七研究所九江分部 | 一种基于模糊pid的船舶自动靠泊控制方法 |
CN113189992B (zh) * | 2021-04-26 | 2023-05-12 | 四川大学 | 一种基于局部定向的群体智能避障方法 |
CN113176788B (zh) * | 2021-04-27 | 2022-08-16 | 北京理工大学 | 一种基于变前向距离los制导律的飞行器路径跟踪方法 |
CN113296401B (zh) * | 2021-05-17 | 2022-04-19 | 哈尔滨工业大学 | 一种基于切换策略的直接避障跟踪控制方法及存储介质 |
CN114185351B (zh) * | 2021-12-06 | 2023-12-01 | 北京云迹科技股份有限公司 | 消毒机器人的作业方法及装置 |
CN114527744B (zh) * | 2022-01-11 | 2024-04-02 | 燕山大学 | 基于天牛须搜索优化的无人帆船路径跟踪制导方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104020771B (zh) * | 2014-06-13 | 2017-03-01 | 大连海事大学 | 一种基于动态虚拟小船制导算法的欠驱动船舶路径跟踪规划方法 |
CN106324619A (zh) * | 2016-10-28 | 2017-01-11 | 武汉大学 | 一种变电站巡检机器人自动避障方法 |
-
2017
- 2017-06-28 CN CN201710504472.4A patent/CN107168335B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN107168335A (zh) | 2017-09-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107168335B (zh) | 一种考虑混合多目标避障的水面无人艇路径跟踪制导方法 | |
Wang et al. | A COLREGs-based obstacle avoidance approach for unmanned surface vehicles | |
Tan et al. | Fast marching square method based intelligent navigation of the unmanned surface vehicle swarm in restricted waters | |
CN106959698B (zh) | 一种路径跟踪避障制导方法 | |
CN109828570B (zh) | 一种自适应边界层水面无人艇控制导引方法 | |
Lazarowska | A discrete artificial potential field for ship trajectory planning | |
Amundsen et al. | Autonomous ROV inspections of aquaculture net pens using DVL | |
CN109283842A (zh) | 一种无人艇航迹跟踪智能学习控制方法 | |
CN107966152B (zh) | 一种具有碰撞风险预测机制的避碰及路径跟踪制导方法 | |
JP2004042884A (ja) | 移動体の操縦方法及び装置 | |
WO2021230356A1 (ja) | 船舶の自動誘導方法、船舶の自動誘導プログラム、船舶の自動誘導システム、及び船舶 | |
CN106094606A (zh) | 一种水面无人艇导航与控制遥操作平台 | |
CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制*** | |
Du et al. | Cooperative multi-agent control for autonomous ship towing under environmental disturbances | |
Wang et al. | A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments | |
Fan et al. | Formation control of multiple unmanned surface vehicles using the adaptive null-space-based behavioral method | |
He et al. | Dynamic adaptive intelligent navigation decision making method for multi-object situation in open water | |
Guan et al. | Autonomous collision avoidance of unmanned surface vehicles based on improved A-star and dynamic window approach algorithms | |
Wang et al. | Path following control of the wave glider in waves and currents | |
Song et al. | Method of emergency collision avoidance for unmanned surface vehicle (USV) based on motion ability database | |
Nie et al. | Planar path-following tracking control for an autonomous underwater vehicle in the horizontal plane | |
Sans-Muntadas et al. | Spiral path planning for docking of underactuated vehicles with limited FOV | |
Raimondi et al. | Hierarchical fuzzy/lyapunov control for horizontal plane trajectory tracking of underactuated AUV | |
CN113359737A (zh) | 一种考虑队形伸缩的船舶编队自适应事件触发控制方法 | |
Liu et al. | Real-time control allocation for autonomous surface vehicle using constrained quadratic programming |
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 |