CN112327850A - 一种水面无人艇路径规划方法 - Google Patents
一种水面无人艇路径规划方法 Download PDFInfo
- Publication number
- CN112327850A CN112327850A CN202011233694.5A CN202011233694A CN112327850A CN 112327850 A CN112327850 A CN 112327850A CN 202011233694 A CN202011233694 A CN 202011233694A CN 112327850 A CN112327850 A CN 112327850A
- Authority
- CN
- China
- Prior art keywords
- path
- obstacle
- surface vehicle
- unmanned
- new
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000004888 barrier function Effects 0.000 claims abstract description 35
- 241001481710 Cerambycidae Species 0.000 claims abstract description 30
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 29
- 238000010845 search algorithm Methods 0.000 claims abstract description 14
- 230000000694 effects Effects 0.000 claims abstract description 10
- 230000001133 acceleration Effects 0.000 claims description 30
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000011156 evaluation Methods 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 3
- 230000002265 prevention Effects 0.000 abstract description 2
- 238000004088 simulation Methods 0.000 description 9
- 230000008569 process Effects 0.000 description 4
- 230000003068 static effect Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012795 verification Methods 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
本发明提供一种水面无人艇路径规划方法,包括:S1、全局路径规划:基于改进的RRT算法生成一个改进全局路径,再通过天牛须搜索算法对改进全局路径进行优化,获得最优的全局路径;其全局路径规划算法简单,计算量小,搜索速度快,可以快速实现全局最优路径的搜索。S2、局部路径规划:采用动态窗口法进行局部路径规划,并通过参数调整避免陷入目标不可达的局面;其局部路径规划通过动态参数,保证了运行速度,避免目标不可达。S3、基于不同的动态障碍物会遇情况,采用不同的避碰策略,使水面无人艇在短时间内结束会遇局面,进而达到最好的动态障碍物躲避效果。采用临时障碍物和临时目标点的策略,取得更安全、更高效的动态障碍物避碰效果。
Description
技术领域
本发明涉及无人船技术领域,具体而言,尤其涉及一种水面无人艇路径规划方法。
背景技术
水面无人艇是海洋开发的重要设备,可以实现勘探、巡航、测量等诸多功能,是海洋资源开发的重要设备。对于水面无人艇的自主航行,路径决策是一项重要的内容,良好的路径规划算法可以制定更好的路径,使路径的损耗更小,更符合无人船的运动特性。路径规划可以分为全局和局部两个部分,
全局路径规划是结合已有的地图信息,进行路径搜索,现有技术已经较为成熟,其中A*算法、栅格法、Djstra算法、遗传算法、群智能算法等都是常用的算法,其中群智能算法的研究日益深入,新的算法层出不穷,其中,RRT算法的搜索方式,可以确保对整个地图的搜索,因此对任何地形,都可以搜索到可行的路径,只要环境不过于复杂,就可以很快完成路径搜索。但由于RRT算法采用完全随机搜索的方式,路径搜索的方向随机,路径的搜索时间也不定,个别情况下可能搜索时间会比较长,且其生成的路径往往并不是最优的路径,路径会比较曲折,在长度上会远远超出预期,传统的RRT算法由于没有加入各种限制,路径会距离障碍物比较近,对于水面无人艇而言,不能保证其路径的安全性,如果遇到风浪等扰动较大的情况,就会有碰撞的风险。
局部路径规划则是针对未知的静态和动态障碍物进行避障,对水面无人艇的安全非常重要,现阶段常用的方法包括人工势场法、动态窗口法、速度障碍法等。其中动态窗口法结合无人艇自身的运动学特性进行速度限制,是一种理想的避碰算法。在传统的动态窗口法下,会出现目标不可达的问题,当无人艇、障碍物、目标点三点共线时,无人船会保持当前方向持续低速前进,而不能自动进行转向,不能达到目标点。而无人艇从两障碍物中间穿过时也会出现这一问题。
发明内容
根据上述提出的技术问题,而提供一种水面无人艇路径规划方法。本发明首先基于改进的RRT算法生成一个改进全局路径,再通过天牛须搜索算法对改进全局路径进行优化,获得最优的全局路径;其次,采用动态窗口法进行局部路径规划,并通过参数调整避免陷入目标不可达的局面;最后,基于不同的动态障碍物会遇情况,采用不同的避碰策略,使水面无人艇在短时间内结束会遇局面,进而达到最好的动态障碍物躲避效果。
本发明采用的技术手段如下:
一种水面无人艇路径规划方法,包括如下步骤:
S1、全局路径规划:基于改进的RRT算法生成一个改进全局路径,再通过天牛须搜索算法对改进全局路径进行优化,获得最优的全局路径;
S2、局部路径规划:采用动态窗口法进行局部路径规划,并通过参数调整避免陷入目标不可达的局面;
S3、基于不同的动态障碍物会遇情况,采用不同的避碰策略,使水面无人艇在短时间内结束会遇局面,进而达到最好的动态障碍物躲避效果。
进一步地,所述步骤S1具体包括:
S11、基于RRT算法生成一个随机的初始路径;
S12、对生成的所述初始路径中的节点进行一次再选择,以去掉所述初始路径中不必要的节点,只保留必要的路径点,生成一个改进全局路径;
S13、将生成的所述改进全局路径作为初解,采用天牛须搜索算法对所述改进全局路径进行优化,生成最优的全局路径。
进一步地,所述步骤S11具体包括:
将起点作为随机树的初始点x_new,列入x_list,记为x1,在地图随机生成一点p_rand,将起点与p_rand连线,在起点后一个步长距离n处取p_new,并将其加入x_list,记为x2;生成新的p_rand,将x_list中的所有点与p_rand连线,并在一个步长距离n后生成p_new,将所有p_new与其对应的xi连线,如果线上有障碍物或者p_new本身就在障碍物中,则去掉该点,在剩余的点中,选取距离p_rand最近的p_new作为x_new,加入到x_list中,并记录其对应的父节点;反复执行上述步骤,不断地将搜索到的x_new加入x_list中,完成对全地图的搜索,直到x_new距离终点不超过步长n,完成对路径的搜索,并从终点不断搜索其父节点,完成路径的输出。
进一步地,所述步骤S12具体包括:
将原步长n拓展为原步长的2-3倍,记为n1,从起点开始,搜索在新步长n1范围内的路径节点,并选取其中序号最大且与起点之间连线上无障碍物的节点,直接与起点连线,代替其中的各个节点;同时将选取的节点作为新的起点,以同样的方式选取下一节点,直到搜索到终点;再将步长新步长n1拓展为原步长的1.5-3倍,记为n2,再次进行搜索,反复搜索3-5次,将路径中不必要的节点去掉,只保留必要的路径点。
进一步地,所述步骤S13具体包括:
S131、基于一个包括n个路径点的全局路径x,设x=xbest,即当前最优路径,假设天牛的头部朝向随机,并用n×2维的随机数dir表示此方向;
S132、对dir进行归一化处理:
dir=rands(n,2)
dir=dir/norm(dir)
S133、设置须的气味函数,即适应度函数,如下:
f(x)=k1·length(x)+k2·turning(x)
S134、对所述步骤S133中路径的总长度length(x)以及路径的总转角turning(x)进行优化,得到长度更短且更加平滑的路径,计算方法如下:
其中,x(i,1)和x(i,2)分别表示第i个路径点的横、纵坐标,direct(i)表示第i段路径的方向角,Δdirect(i)表示从第i-1段路径到第i段路径的转角;
S135、假设天牛左须的位置为xl,右须的位置为xr,则有:
xl=xbest-dir·d0
xr=xbest+dir·d0
d0=step/c
其中,xbest为当前最优路径,d0为天牛搜索范围,step为天牛移动步长,c为常数;由此,确定下一步天牛的位置,即对路径进行更新:
xnew=x-step·dir·sign[f(xl)-f(xr)]
S136、基于更新后的路径xnew,比较更新后的路径xnew与原路径的适应性,当更新后的路径xnew的适应度值低于原路径时,则有:
S137、在对当前最优路径xbest进行更新以后,反复进行迭代,为步长设置一个衰减参数eta,每次迭代后对步长进行更新:
step=step·eta
采用衰减系数eta后,天牛须搜索算法先进行大范围的搜索,在进行小范围的搜索,在迭代一定次数后,将步长step重置,搜索到全局最优解,获得最优的全局路径。
进一步地,所述步骤S2具体包括:
S21、考虑最大速度限制、最大加速度限制以及制动速度限制,确定最终的速度空间;
S22、基于确定的所述速度空间,通过评估函数G(v,ω)选取下一时刻的速度与角速度:
其中,α1、β1、γ1;α2、β2、γ2分别为两组不同的权重系数,velocity(v,ω)为下一时刻的速度值,以确保水面无人艇运动的速度,heading(v,ω)是对船艏向的评估函数,计算公式如下:
heading(v,ω)=180-|goaltheta-theta|
goaltheta是目标点相对当前位置的方向角,theta为当前的船艏向,当两者差值更小时,heading(v,ω)的值更大;dist(v,ω)为水面无人艇距离障碍物最近的距离,为了避免占据太大的权重,该函数最大值为2倍的障碍物影响范围R,超过该值的dist(v,ω)值直接取2R,即
其中,norm()为计算距离函数,X0为无人船当前所在位置,Xobs为障碍物所在位置,R为障碍物影响范围。
进一步地,所述步骤S21具体包括:
S211、考虑最大速度限制:用Vlim表示最大最小速度限制下的速度空间,则有:
Vlim={(v,ω)|v∈[vmin,vmax]∧ω∈[ωmin,ωmax]}
其中,vmin、vmax分别表示水面无人艇的最小、最大速度,ωmin、ωmax分别表示水面无人艇的最小、最大角速度;
S212、考虑最大加速度限制:用Vacc表示最大加速度限制下的速度空间,则有:
Vacc={(v,ω)|v∈[vnow-Δv·t,vnow+Δv·t]∧ω∈[ωnow-Δω·t,ωnow+Δω·t]}
其中,vnow、ωnow表示水面无人艇当前的速度和角速度,Δv、Δω表示水面无人艇的最大加速度、角加速度,t表示时间;
S213、考虑制动速度限制:设制动速度限制下的速度空间为Vsafe,则有:
其中,dist(v,ω)是当前(v,ω)下轨迹与障碍物的最近距离,Δv、Δω表示加速度和角加速度;
S214、基于最大加速度限制、最大加速度限制以及制动速度限制,得到最终的速度空间Vdwa:
Vdwa=Vlim∩Vacc∩Vsafe。
进一步地,所述步骤3具体包括:
S31、当障碍物进入水面无人艇安全范围ds时,假设无人船坐标为[xusv,yusv],动态障碍物坐标为obs[xo,yo],则设置一个临时障碍物obs1[x1,y1],即:
其中,Δt1为设置的预测时间,ds为无人艇搜索范围,dist为障碍物距离无人艇的距离,θo为障碍物航向,通过对二者的比较,判断是否有发生碰撞的危险;
S32、根据障碍物与无人船当前速度,预测未来一段时间内二者的距离,并取其中的最小值,即:
S33、如果检测到distpre小于无人船避碰安全距离dcol,则设定第二个临时障碍物obs2[x2,y2],即:
其中,Δt2小于Δt1。
进一步地,所述步骤3还包括如下步骤:
S34、对交叉相遇的状况进行单独处理,计算障碍物方向θo与无人船朝向θ0的夹角Δθ,如果二者夹角在15°到135°范围内时,即处于交叉会遇局面,则设定一个临时目标点goaltemp[xgt,ygt],即:
其中,obs[xo,yo]为障碍物当前位置,vo为障碍物速度,θo为障碍物当前航向,Δt3为设定的时间间隔,goal0[xg,yg]为正常的目标点,从上式可以得到一个与障碍物运动方向相反的临时目标点,使无人艇向障碍物后方移动,结束交叉会遇局面。
进一步地,所述步骤32中:
无人艇的预测位姿Xi[xi,yi]的计算公式如下:
其中,X0[x0,y0]为水面无人艇的当前位置,v为无人艇速度,θi为无人艇预测航向,θ0为水面无人艇当前航向,ω为无人艇当前角速度;
较现有技术相比,本发明具有以下优点:
1、本发明提供的水面无人艇路径规划方法,其全局路径规划算法简单,计算量小,搜索速度快,可以快速实现全局最优路径的搜索。
2、本发明提供的水面无人艇路径规划方法,其局部路径规划通过动态参数,保证了运行速度,避免目标不可达。
3、本发明提供的水面无人艇路径规划方法,采用临时障碍物和临时目标点的策略,取得更安全、更高效的动态障碍物避碰效果。
基于上述理由本发明可在无人船等领域广泛推广。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明方法流程图。
图2为本发明实施例提供的改进RRT算法得到的路径。
图3为本发明实施例提供的天牛须搜索算法优化后的路径。
图4为本发明实施例提供的动态窗口法静态避障仿真。
图5为本发明实施例提供的动态障碍物追越会遇避障仿真。
图6为本发明实施例提供的动态障碍物交叉会遇避障仿真。
图7为本发明实施例提供的动态障碍物对遇避障仿真。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
需要说明的是,本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
如图1所示,本发明提供了一种水面无人艇路径规划方法,包括如下步骤:
S1、全局路径规划:基于改进的RRT算法生成一个改进全局路径,再通过天牛须搜索算法对改进全局路径进行优化,获得最优的全局路径;
具体实施时,优选的,所述步骤S1具体包括:
S11、基于RRT算法生成一个随机的初始路径;
将起点作为随机树的初始点x_new,列入x_list,记为x1,在地图随机生成一点p_rand,将起点与p_rand连线,在起点后一个步长距离n处取p_new,并将其加入x_list,记为x2;生成新的p_rand,将x_list中的所有点与p_rand连线,并在一个步长距离n后生成p_new,将所有p_new与其对应的xi连线,如果线上有障碍物或者p_new本身就在障碍物中,则去掉该点,在剩余的点中,选取距离p_rand最近的p_new作为x_new,加入到x_list中,并记录其对应的父节点;反复执行上述步骤,不断地将搜索到的x_new加入x_list中,完成对全地图的搜索,直到x_new距离终点不超过步长n,完成对路径的搜索,并从终点不断搜索其父节点,完成路径的输出。
S12、对生成的所述初始路径中的节点进行一次再选择,以去掉所述初始路径中不必要的节点,只保留必要的路径点,生成一个改进全局路径;
将原步长n拓展为原步长的2-3倍,记为n1,从起点开始,搜索在新步长n1范围内的路径节点,并选取其中序号最大且与起点之间连线上无障碍物的节点,直接与起点连线,代替其中的各个节点;同时将选取的节点作为新的起点,以同样的方式选取下一节点,直到搜索到终点;再将步长新步长n1拓展为原步长的1.5-3倍,记为n2,再次进行搜索,反复搜索3-5次,将路径中不必要的节点去掉,只保留必要的路径点,使生成的路径更短,也更加平滑,更符合水面无人艇的运动特性。本部分还添加了障碍物距离限制,每一段路径都必须距离障碍物达到一定值,否则视为有碰撞危险,路径无效。
S13、将生成的所述改进全局路径作为初解,采用天牛须搜索算法对所述改进全局路径进行优化,生成最优的全局路径。
所述步骤S13具体包括:
S131、基于一个包括n个路径点的全局路径x,设x=xbest,即当前最优路径,假设天牛的头部朝向随机,并用n×2维的随机数dir表示此方向;
S132、对dir进行归一化处理:
dir=rands(n,2)
dir=dir/norm(dir)
S133、设置须的气味函数,即适应度函数,如下:
f(x)=k1·length(x)+k2·turning(x)
S134、对所述步骤S133中路径的总长度length(x)以及路径的总转角turning(x)进行优化,得到长度更短且更加平滑的路径,计算方法如下:
其中,x(i,1)和x(i,2)分别表示第i个路径点的横、纵坐标,direct(i)表示第i段路径的方向角,Δdirect(i)表示从第i-1段路径到第i段路径的转角;turning(x)则为所有Δdirect(i)之和,length(x)为各段路径的长度之和。显然,适应度函数值更低的路径更符合需求。
S135、假设天牛左须的位置为xl,右须的位置为xr,则有:
xl=xbest-dir·d0
xr=xbest+dir·d0
d0=step/c
其中,xbest为当前最优路径,d0为天牛搜索范围,step为天牛移动步长,c为常数;由此,确定下一步天牛的位置,即对路径进行更新:
xnew=x-step·dir·sign[f(xl)-f(xr)]
S136、基于更新后的路径xnew,比较更新后的路径xnew与原路径的适应性,当更新后的路径xnew的适应度值低于原路径时,则有:
S137、在对当前最优路径xbest进行更新以后,反复进行迭代,为步长设置一个衰减参数eta,每次迭代后对步长进行更新:
step=step·eta
采用衰减系数eta后,天牛须搜索算法先进行大范围的搜索,在进行小范围的搜索,在迭代一定次数后,将步长step重置,搜索到全局最优解,获得最优的全局路径。
经过RRT算法的随机路径搜索,以及天牛须搜索算法对路径的优化,已经基本得到了一条最优的全局路径,但是在实际的海域中,会有很多随机出现的未知障碍物,因此需要局部路径规划来确保航行的安全性。
S2、局部路径规划:采用动态窗口法进行局部路径规划,并通过参数调整避免陷入目标不可达的局面;
具体实施时,优选的,所述步骤S2具体包括:
S21、考虑最大速度限制、最大加速度限制以及制动速度限制,确定最终的速度空间;
S211、考虑最大速度限制:水面无人艇的速度和角速度都是有一定限制的,因此在设计动态窗口时,首先要考虑的就是其能到达的最大和最小速度,用Vlim表示最大最小速度限制下的速度空间,则有:
Vlim={(v,ω)|v∈[vmin,vmax]∧ω∈[ωmin,ωmax]}
其中,vmin、vmax分别表示水面无人艇的最小、最大速度,ωmin、ωmax分别表示水面无人艇的最小、最大角速度;
S212、考虑最大加速度限制:水面无人艇的加速度往往是有限的,相比于陆地的移动机器人和无人机,水面无人艇加速度性能会更受限制,因此加速度限制也是水面无人艇必须考虑的限制因素,用Vacc表示最大加速度限制下的速度空间,则有:
Vacc={(v,ω)|v∈[vnow-Δv·t,vnow+Δv·t]∧ω∈[ωnow-Δω·t,ωnow+Δω·t]}
其中,vnow、ωnow表示水面无人艇当前的速度和角速度,Δv、Δω表示水面无人艇的最大加速度、角加速度,t表示时间;
S213、考虑制动速度限制:为了确保水面无人艇不会与障碍物发生碰撞,也要考虑水面无人艇能否及时制动。当无人艇距离障碍物非常近时,其速度也应降低,以确保在发生撞击前无人艇可以停止,设制动速度限制下的速度空间为Vsafe,则有:
其中,dist(v,ω)是当前(v,ω)下轨迹与障碍物的最近距离,Δv、Δω表示加速度和角加速度;
S214、基于最大加速度限制、最大加速度限制以及制动速度限制,得到最终的速度空间Vdwa:
Vdwa=Vlim∩Vacc∩Vsafe。
考虑到目标不可达的主要原因是评估函数中heading(v,ω)和dist(v,ω)两个参数共同导致的,当水面无人艇在朝向目标点行驶时,其heading(v,ω)值是最大的,且速度值越小,其dist(v,ω)越大,这就导致了水面无人艇的速度保持在极低的值。为了解决这一问题,本部分采用动态参数的策略,当水面无人艇的速度值很小时,认为有目标不可达的趋势,则采取另一套评估函数的权重系数α、β、γ,提高γ值,减小α、β值,使velocity(v,ω)对评估函数的值影响更大,从而跳出局部最优解的局面。即:
S22、基于确定的所述速度空间,通过评估函数G(v,ω)选取下一时刻的速度与角速度:
其中,α1、β1、γ1;α2、β2、γ2分别为两组不同的权重系数,通过动态参数变化,可以有效地解决目标不可达的问题,同时,新的评估函数也能确保水面无人艇全程高速运行,更快到达目标点。velocity(v,ω)为下一时刻的速度值,以确保水面无人艇运动的速度,heading(v,ω)是对船艏向的评估函数,计算公式如下:
heading(v,ω)=180-|goaltheta-theta|
goaltheta是目标点相对当前位置的方向角,theta为当前的船艏向,当两者差值更小时,heading(v,ω)的值更大;dist(v,ω)为水面无人艇距离障碍物最近的距离,为了避免占据太大的权重,该函数最大值为2倍的障碍物影响范围R,超过该值的dist(v,ω)值直接取2R,即
其中,norm()为计算距离函数,X0为无人船当前所在位置,Xobs为障碍物所在位置,R为障碍物影响范围。
在水面无人艇的运行环境中,可能会受到不同方向的动态障碍物带来的危险,大致可以将动态障碍物的会遇局面分为三类,即对遇、交叉、会遇。本部分针对动态障碍物,提出了新的避障策略,以达到更高效、更安全的避碰效果,使无人船尽快结束避碰局面。当障碍物在运动时,简单的对障碍物当前位置进行躲避显然不能达到最好的效果,障碍物在不断运动,其一段时间后的位置其实是容易发生碰撞的位置,将下一时刻障碍物的位置作为障碍物点,可以提前使无人艇避开会遇局面。
S3、基于不同的动态障碍物会遇情况,采用不同的避碰策略,使水面无人艇在短时间内结束会遇局面,进而达到最好的动态障碍物躲避效果。
具体实施时,优选的,所述步骤3具体包括:
S31、当障碍物进入水面无人艇安全范围ds时,假设无人船坐标为[xusv,yusv],动态障碍物坐标为obs[xo,yo],则设置一个临时障碍物obs1[x1,y1],即:
其中,Δt1为设置的预测时间,ds为无人艇搜索范围,dist为障碍物距离无人艇的距离,θo为障碍物航向,通过对二者的比较,判断是否有发生碰撞的危险;
S32、根据障碍物与无人船当前速度,预测未来一段时间内二者的距离,并取其中的最小值,即:
S33、如果检测到distpre小于无人船避碰安全距离dcol,则设定第二个临时障碍物obs2[x2,y2],即:
其中,Δt2小于Δt1。
在动态障碍物避障时,交叉会遇是较为危险的一种状况,避碰难度也较大,因此本部分对交叉相遇的状况进行单独处理,即:
S34、对交叉相遇的状况进行单独处理,计算障碍物方向θo与无人船朝向θ0的夹角Δθ,如果二者夹角在15°到135°范围内时,即处于交叉会遇局面,则设定一个临时目标点goaltemp[xgt,ygt],即:
其中,obs[xo,yo]为障碍物当前位置,vo为障碍物速度,θo为障碍物当前航向,Δt3为设定的时间间隔,goal0[xg,yg]为正常的目标点,从上式可以得到一个与障碍物运动方向相反的临时目标点,使无人艇向障碍物后方移动,结束交叉会遇局面。
本发明实施例还提供了仿真验证,具体如下:
全局路径规划
本部分为基于改进的RRT算法和天牛须搜索算法的全局路径规划,如图2所示,为改进RRT算法得到的路径,红色路径为RRT算法得到的初始路径,蓝色路径为进行节点选择后的优化路径。可以看出,两条路径都距离障碍物一定的距离,保证了路径的安全性,且蓝色路径省去了红色路径中多余的节点,使路径更加平滑,成为可行且能保证安全的路径。如图3所示,为天牛须搜索算法优化后的路径。红色为改进RRT算法获取的路径,蓝色为天牛须算法优化后的路径,蓝色路径在保证安全性的前提下,在路径长度和路径转角上得到了优化。
局部路径规划
本部分基于改进动态窗口法的局部路径规划仿真,如图3所示,为躲避静态障碍物的仿真结果,从运动轨迹来看,无人艇能够保持与障碍物的安全距离,并且能保证无人艇全程高速运行,避免了局部最优解造成的无人艇难以到达目标点的问题,达到了很好的避障效果。图4-图6为躲避动态障碍物的仿真结果,其中蓝色圆形为水面无人艇,红色为障碍物。在仿真中,设置了对遇、追越、交叉三种会遇局面,可以看到,通过在障碍物预测位置设置临时障碍物,可以实现提前避开障碍物,且通过在交叉会遇时,设置临时目标点,可以使无人艇从障碍物后方绕行,保证安全且更加高效,避免了过于冒进的避碰策略,提前避免了碰撞风险。
最后应说明的是:以上各实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述各实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (10)
1.一种水面无人艇路径规划方法,其特征在于,包括如下步骤:
S1、全局路径规划:基于改进的RRT算法生成一个改进全局路径,再通过天牛须搜索算法对改进全局路径进行优化,获得最优的全局路径;
S2、局部路径规划:采用动态窗口法进行局部路径规划,并通过参数调整避免陷入目标不可达的局面;
S3、基于不同的动态障碍物会遇情况,采用不同的避碰策略,使水面无人艇在短时间内结束会遇局面,进而达到最好的动态障碍物躲避效果。
2.根据权利要求1所述的一种水面无人艇路径规划方法,其特征在于,所述步骤S1具体包括:
S11、基于RRT算法生成一个随机的初始路径;
S12、对生成的所述初始路径中的节点进行一次再选择,以去掉所述初始路径中不必要的节点,只保留必要的路径点,生成一个改进全局路径;
S13、将生成的所述改进全局路径作为初解,采用天牛须搜索算法对所述改进全局路径进行优化,生成最优的全局路径。
3.根据权利要求2所述的一种水面无人艇路径规划方法,其特征在于,所述步骤S11具体包括:
将起点作为随机树的初始点x_new,列入x_list,记为x1,在地图随机生成一点p_rand,将起点与p_rand连线,在起点后一个步长距离n处取p_new,并将其加入x_list,记为x2;生成新的p_rand,将x_list中的所有点与p_rand连线,并在一个步长距离n后生成p_new,将所有p_new与其对应的xi连线,如果线上有障碍物或者p_new本身就在障碍物中,则去掉该点,在剩余的点中,选取距离p_rand最近的p_new作为x_new,加入到x_list中,并记录其对应的父节点;反复执行上述步骤,不断地将搜索到的x_new加入x_list中,完成对全地图的搜索,直到x_new距离终点不超过步长n,完成对路径的搜索,并从终点不断搜索其父节点,完成路径的输出。
4.根据权利要求2所述的一种水面无人艇路径规划方法,其特征在于,所述步骤S12具体包括:
将原步长n拓展为原步长的2-3倍,记为n1,从起点开始,搜索在新步长n1范围内的路径节点,并选取其中序号最大且与起点之间连线上无障碍物的节点,直接与起点连线,代替其中的各个节点;同时将选取的节点作为新的起点,以同样的方式选取下一节点,直到搜索到终点;再将步长新步长n1拓展为原步长的1.5-3倍,记为n2,再次进行搜索,反复搜索3-5次,将路径中不必要的节点去掉,只保留必要的路径点。
5.根据权利要求2所述的一种水面无人艇路径规划方法,其特征在于,所述步骤S13具体包括:
S131、基于一个包括n个路径点的全局路径x,设x=xbest,即当前最优路径,假设天牛的头部朝向随机,并用n×2维的随机数dir表示此方向;
S132、对dir进行归一化处理:
dir=rands(n,2)
dir=dir/norm(dir)
S133、设置须的气味函数,即适应度函数,如下:
f(x)=k1·length(x)+k2·turning(x)
S134、对所述步骤S133中路径的总长度length(x)以及路径的总转角turning(x)进行优化,得到长度更短且更加平滑的路径,计算方法如下:
其中,x(i,1)和x(i,2)分别表示第i个路径点的横、纵坐标,direct(i)表示第i段路径的方向角,Δdirect(i)表示从第i-1段路径到第i段路径的转角;
S135、假设天牛左须的位置为xl,右须的位置为xr,则有:
xl=xbest-dir·d0
xr=xbest+dir·d0
d0=step/c
其中,xbest为当前最优路径,d0为天牛搜索范围,step为天牛移动步长,c为常数;由此,确定下一步天牛的位置,即对路径进行更新:
xnew=x-step·dir·sign[f(xl)-f(xr)]
S136、基于更新后的路径xnew,比较更新后的路径xnew与原路径的适应性,当更新后的路径xnew的适应度值低于原路径时,则有:
S137、在对当前最优路径xbest进行更新以后,反复进行迭代,为步长设置一个衰减参数eta,每次迭代后对步长进行更新:
step=step·eta
采用衰减系数eta后,天牛须搜索算法先进行大范围的搜索,在进行小范围的搜索,在迭代一定次数后,将步长step重置,搜索到全局最优解,获得最优的全局路径。
6.根据权利要求1所述的一种水面无人艇路径规划方法,其特征在于,所述步骤S2具体包括:
S21、考虑最大速度限制、最大加速度限制以及制动速度限制,确定最终的速度空间;
S22、基于确定的所述速度空间,通过评估函数G(v,ω)选取下一时刻的速度与角速度:
其中,α1、β1、γ1;α2、β2、γ2分别为两组不同的权重系数,velocity(v,ω)为下一时刻的速度值,以确保水面无人艇运动的速度,heading(v,ω)是对船艏向的评估函数,计算公式如下:
heading(v,ω)=180-|goaltheta-theta|
goaltheta是目标点相对当前位置的方向角,theta为当前的船艏向,当两者差值更小时,heading(v,ω)的值更大;dist(v,ω)为水面无人艇距离障碍物最近的距离,为了避免占据太大的权重,该函数最大值为2倍的障碍物影响范围R,超过该值的dist(v,ω)值直接取2R,即
其中,norm()为计算距离函数,X0为无人船当前所在位置,Xobs为障碍物所在位置,R为障碍物影响范围。
7.根据权利要求6所述的一种水面无人艇路径规划方法,其特征在于,所述步骤S21具体包括:
S211、考虑最大速度限制:用Vlim表示最大最小速度限制下的速度空间,则有:
Vlim={(v,ω)|v∈[vmin,vmax]∧ω∈[ωmin,ωmax]}
其中,vmin、vmax分别表示水面无人艇的最小、最大速度,ωmin、ωmax分别表示水面无人艇的最小、最大角速度;
S212、考虑最大加速度限制:用Vacc表示最大加速度限制下的速度空间,则有:
Vacc={(v,ω)|v∈[vnow-Δv·t,vnow+Δv·t]∧ω∈[ωnow-Δω·t,ωnow+Δω·t]}
其中,vnow、ωnow表示水面无人艇当前的速度和角速度,Δv、Δω表示水面无人艇的最大加速度、角加速度,t表示时间;
S213、考虑制动速度限制:设制动速度限制下的速度空间为Vsafe,则有:
其中,dist(v,ω)是当前(v,ω)下轨迹与障碍物的最近距离,Δv、Δω表示加速度和角加速度;
S214、基于最大加速度限制、最大加速度限制以及制动速度限制,得到最终的速度空间Vdwa:
Vdwa=Vlim∩Vacc∩Vsafe。
8.根据权利要求1所述的一种水面无人艇路径规划方法,其特征在于,所述步骤3具体包括:
S31、当障碍物进入水面无人艇安全范围ds时,假设无人船坐标为[xusv,yusv],动态障碍物坐标为obs[xo,yo],则设置一个临时障碍物obs1[x1,y1],即:
其中,Δt1为设置的预测时间,ds为无人艇搜索范围,dist为障碍物距离无人艇的距离,θo为障碍物航向,通过对二者的比较,判断是否有发生碰撞的危险;
S32、根据障碍物与无人船当前速度,预测未来一段时间内二者的距离,并取其中的最小值,即:
S33、如果检测到distpre小于无人船避碰安全距离dcol,则设定第二个临时障碍物obs2[x2,y2],即:
其中,Δt2小于Δt1。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011233694.5A CN112327850B (zh) | 2020-11-06 | 2020-11-06 | 一种水面无人艇路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011233694.5A CN112327850B (zh) | 2020-11-06 | 2020-11-06 | 一种水面无人艇路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112327850A true CN112327850A (zh) | 2021-02-05 |
CN112327850B CN112327850B (zh) | 2023-10-13 |
Family
ID=74315489
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011233694.5A Active CN112327850B (zh) | 2020-11-06 | 2020-11-06 | 一种水面无人艇路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112327850B (zh) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112965492A (zh) * | 2021-02-07 | 2021-06-15 | 武汉理工大学 | 一种船舶运动的控制方法、系统、装置及存储介质 |
CN112987737A (zh) * | 2021-02-26 | 2021-06-18 | 华中科技大学 | 考虑转角约束的Bi-RRT无人艇多艇导航方法及设备 |
CN113296521A (zh) * | 2021-05-26 | 2021-08-24 | 紫清智行科技(北京)有限公司 | 一种基于动态窗口的障碍物局部避障方法 |
CN113917929A (zh) * | 2021-11-11 | 2022-01-11 | 中国船舶重工集团公司第七一九研究所 | 一种基于混合粒子群算法的无人船路径优化方法和系统 |
CN114115239A (zh) * | 2021-11-03 | 2022-03-01 | 中国科学院重庆绿色智能技术研究院 | 一种机器人路径规划方法、系统、设备及介质 |
CN114527744A (zh) * | 2022-01-11 | 2022-05-24 | 燕山大学 | 基于天牛须搜索优化的无人帆船路径跟踪制导方法 |
CN115309161A (zh) * | 2022-08-23 | 2022-11-08 | 南京信息工程大学 | 一种移动机器人路径规划方法、电子设备及存储介质 |
CN115328208A (zh) * | 2022-09-21 | 2022-11-11 | 西华大学 | 一种实现全局动态路径规划的无人机路径规划方法 |
CN115437373A (zh) * | 2022-08-22 | 2022-12-06 | 上海交通大学 | 一种结合rrt算法与vo算法的无人船路径规划方法 |
CN115454071A (zh) * | 2022-09-15 | 2022-12-09 | 燕山大学 | 一种用于跟踪移动目标的路径规划方法 |
CN115494842A (zh) * | 2022-09-15 | 2022-12-20 | 大连海事大学 | 一种基于卫星地图的无人艇遍历路径规划与避碰方法 |
CN117215317A (zh) * | 2023-11-09 | 2023-12-12 | 烟台哈尔滨工程大学研究院 | 一种无人船局部路径规划方法、设备及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2014170471A1 (en) * | 2013-04-17 | 2014-10-23 | Tomtom International B.V. | Methods, devices and computer software for facilitating searching and display of locations relevant to a digital map |
CN110703762A (zh) * | 2019-11-04 | 2020-01-17 | 东南大学 | 一种复杂环境下水面无人艇混合路径规划方法 |
CN110986982A (zh) * | 2019-12-02 | 2020-04-10 | 浙江工业大学 | 基于改进天牛须算法的多机器人路径规划方法 |
CN111290390A (zh) * | 2020-02-25 | 2020-06-16 | 智慧航海(青岛)科技有限公司 | 一种基于天牛须搜索的智能船舶路径规划方法 |
CN111399506A (zh) * | 2020-03-13 | 2020-07-10 | 大连海事大学 | 一种基于动力学约束的全局-局部混合无人船路径规划方法 |
-
2020
- 2020-11-06 CN CN202011233694.5A patent/CN112327850B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2014170471A1 (en) * | 2013-04-17 | 2014-10-23 | Tomtom International B.V. | Methods, devices and computer software for facilitating searching and display of locations relevant to a digital map |
CN110703762A (zh) * | 2019-11-04 | 2020-01-17 | 东南大学 | 一种复杂环境下水面无人艇混合路径规划方法 |
CN110986982A (zh) * | 2019-12-02 | 2020-04-10 | 浙江工业大学 | 基于改进天牛须算法的多机器人路径规划方法 |
CN111290390A (zh) * | 2020-02-25 | 2020-06-16 | 智慧航海(青岛)科技有限公司 | 一种基于天牛须搜索的智能船舶路径规划方法 |
CN111399506A (zh) * | 2020-03-13 | 2020-07-10 | 大连海事大学 | 一种基于动力学约束的全局-局部混合无人船路径规划方法 |
Non-Patent Citations (4)
Title |
---|
成怡;肖宏图;: "融合改进A~*算法和Morphin算法的移动机器人动态路径规划", 智能系统学报, no. 03 * |
文龙贻彬;刘友;穆京京;胡常青;唐军武;: "无人艇航行规则及运动学约束下的改进RRT轨迹规划方法", 导航与控制, no. 01 * |
杨左华;王玉龙;戚爱春;: "基于改进RRT~*算法的无人艇全局避障规划", 舰船科学技术, no. 23, pages 167 - 171 * |
欧阳子路;王鸿东;黄一;杨楷文;易宏;: "基于改进RRT算法的无人艇编队路径规划技术", 中国舰船研究, no. 03 * |
Cited By (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112965492A (zh) * | 2021-02-07 | 2021-06-15 | 武汉理工大学 | 一种船舶运动的控制方法、系统、装置及存储介质 |
CN112987737A (zh) * | 2021-02-26 | 2021-06-18 | 华中科技大学 | 考虑转角约束的Bi-RRT无人艇多艇导航方法及设备 |
CN113296521A (zh) * | 2021-05-26 | 2021-08-24 | 紫清智行科技(北京)有限公司 | 一种基于动态窗口的障碍物局部避障方法 |
CN114115239A (zh) * | 2021-11-03 | 2022-03-01 | 中国科学院重庆绿色智能技术研究院 | 一种机器人路径规划方法、系统、设备及介质 |
CN114115239B (zh) * | 2021-11-03 | 2024-04-12 | 中国科学院重庆绿色智能技术研究院 | 一种机器人路径规划方法、系统、设备及介质 |
CN113917929A (zh) * | 2021-11-11 | 2022-01-11 | 中国船舶重工集团公司第七一九研究所 | 一种基于混合粒子群算法的无人船路径优化方法和系统 |
CN114527744A (zh) * | 2022-01-11 | 2022-05-24 | 燕山大学 | 基于天牛须搜索优化的无人帆船路径跟踪制导方法 |
CN114527744B (zh) * | 2022-01-11 | 2024-04-02 | 燕山大学 | 基于天牛须搜索优化的无人帆船路径跟踪制导方法 |
CN115437373A (zh) * | 2022-08-22 | 2022-12-06 | 上海交通大学 | 一种结合rrt算法与vo算法的无人船路径规划方法 |
CN115309161A (zh) * | 2022-08-23 | 2022-11-08 | 南京信息工程大学 | 一种移动机器人路径规划方法、电子设备及存储介质 |
CN115309161B (zh) * | 2022-08-23 | 2024-06-28 | 南京信息工程大学 | 一种移动机器人路径规划方法、电子设备及存储介质 |
CN115454071A (zh) * | 2022-09-15 | 2022-12-09 | 燕山大学 | 一种用于跟踪移动目标的路径规划方法 |
CN115494842A (zh) * | 2022-09-15 | 2022-12-20 | 大连海事大学 | 一种基于卫星地图的无人艇遍历路径规划与避碰方法 |
CN115454071B (zh) * | 2022-09-15 | 2024-08-27 | 燕山大学 | 一种用于跟踪移动目标的路径规划方法 |
CN115328208A (zh) * | 2022-09-21 | 2022-11-11 | 西华大学 | 一种实现全局动态路径规划的无人机路径规划方法 |
CN117215317A (zh) * | 2023-11-09 | 2023-12-12 | 烟台哈尔滨工程大学研究院 | 一种无人船局部路径规划方法、设备及存储介质 |
CN117215317B (zh) * | 2023-11-09 | 2024-02-09 | 烟台哈尔滨工程大学研究院 | 一种无人船局部路径规划方法、设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN112327850B (zh) | 2023-10-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112327850A (zh) | 一种水面无人艇路径规划方法 | |
CN109885066B (zh) | 一种运动轨迹预测方法及装置 | |
CN112327885B (zh) | 一种无人船自适应全局-局部混合路径规划的方法 | |
CN112242059B (zh) | 基于动机与风险评估的无人驾驶车辆智能决策方法 | |
CN111679678B (zh) | 一种横纵向分离的轨迹规划方法、系统及计算机设备 | |
CN109579854B (zh) | 基于快速扩展随机树的无人车避障方法 | |
CN112506199B (zh) | 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法 | |
CN112462786B (zh) | 一种基于模糊控制策略双窗口算法的无人艇避碰方法 | |
Chen et al. | Foad: Fast optimization-based autonomous driving motion planner | |
CN110262473B (zh) | 一种基于改进Bi-RRT算法的无人艇自动避碰方法 | |
CN113538973B (zh) | 一种基于改进粒子群算法的船舶自动避碰方法 | |
CN114442637A (zh) | 一种无人车局部动态避障路径规划方法 | |
CN112925342B (zh) | 基于改进相互速度障碍法的无人机动态避障方法 | |
CN112965476A (zh) | 一种基于多窗口抽样的高速无人车轨迹规划系统及方法 | |
CN113467476B (zh) | 考虑转角约束的无碰撞检测快速随机树全局路径规划方法 | |
CN114625150B (zh) | 基于危险系数和距离函数的快速蚁群无人艇动态避障方法 | |
CN113985875A (zh) | 基于碰撞预测模型的人工势场无人车动态路径规划方法 | |
CN116952239A (zh) | 一种基于改进a*与dwa融合的无人艇路径规划方法 | |
Yang et al. | Path planning and collision avoidance methods for distributed multi-robot systems in complex dynamic environments | |
CN114527744A (zh) | 基于天牛须搜索优化的无人帆船路径跟踪制导方法 | |
Lim et al. | Online AUV path replanning using quantum-behaved particle swarm optimization with selective differential evolution | |
CN117870708A (zh) | 一种路径规划方法、终端设备以及存储介质 | |
CN113219990B (zh) | 基于自适应邻域与转向代价的机器人路径规划方法 | |
CN111121804B (zh) | 一种具有安全性约束的智能车辆路径规划方法及系统 | |
KR101807397B1 (ko) | 장애물의 위험도에 기반한 장애물 회피 방법 및 장치 |
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 |