CN112327850B - 一种水面无人艇路径规划方法 - Google Patents

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，记为x₁，在地图随机生成一点p_rand，将起点与p_rand连线，在起点后一个步长距离n处取p_new，并将其加入x_list，记为x₂；生成新的p_rand，将x_list中的所有点与p_rand连线，并在一个步长距离n后生成p_new，将所有p_new与其对应的x_i连线，如果线上有障碍物或者p_new本身就在障碍物中，则去掉该点，在剩余的点中，选取距离p_rand最近的p_new作为x_new，加入到x_list中，并记录其对应的父节点；反复执行上述步骤，不断地将搜索到的x_new加入x_list中，完成对全地图的搜索，直到x_new距离终点不超过步长n，完成对路径的搜索，并从终点不断搜索其父节点，完成路径的输出。

进一步地，所述步骤S12具体包括：

将原步长n拓展为原步长的2-3倍，记为n₁，从起点开始，搜索在新步长n₁范围内的路径节点，并选取其中序号最大且与起点之间连线上无障碍物的节点，直接与起点连线，代替其中的各个节点；同时将选取的节点作为新的起点，以同样的方式选取下一节点，直到搜索到终点；再将步长新步长n₁拓展为原步长的1.5-3倍，记为n₂，再次进行搜索，反复搜索3-5次，将路径中不必要的节点去掉，只保留必要的路径点。

进一步地，所述步骤S13具体包括：

S131、基于一个包括n个路径点的全局路径x，设x＝xbest，即当前最优路径，假设天牛的头部朝向随机，并用n×2维的随机数dir表示此方向；

S132、对dir进行归一化处理：

dir＝rands(n,2)

dir＝dir/norm(dir)

S133、设置须的气味函数，即适应度函数，如下：

f(x)＝k₁·length(x)+k₂·turning(x)

其中，k₁、k₂为权重系数，length(x)为路径的总长度，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·d₀

xr＝xbest+dir·d₀

d₀＝step/c

其中，xbest为当前最优路径，d₀为天牛搜索范围，step为天牛移动步长，c为常数；由此，确定下一步天牛的位置，即对路径进行更新：

x_new＝x-step·dir·sign[f(xl)-f(xr)]

S136、基于更新后的路径x_new，比较更新后的路径x_new与原路径的适应性，当更新后的路径x_new的适应度值低于原路径时，则有：

S137、在对当前最优路径xbest进行更新以后，反复进行迭代，为步长设置一个衰减参数eta，每次迭代后对步长进行更新：

step＝step·eta

采用衰减系数eta后，天牛须搜索算法先进行大范围的搜索，在进行小范围的搜索，在迭代一定次数后，将步长step重置，搜索到全局最优解，获得最优的全局路径。

进一步地，所述步骤S2具体包括：

S21、考虑最大速度限制、最大加速度限制以及制动速度限制，确定最终的速度空间；

S22、基于确定的所述速度空间，通过评估函数G(v,ω)选取下一时刻的速度与角速度：

其中，α₁、β₁、γ₁；α₂、β₂、γ₂分别为两组不同的权重系数，velocity(v,ω)为下一时刻的速度值，以确保水面无人艇运动的速度，heading(v,ω)是对船艏向的评估函数，计算公式如下：

heading(v,ω)＝180-|goaltheta-theta|

goaltheta是目标点相对当前位置的方向角，theta为当前的船艏向，当两者差值更小时，heading(v,ω)的值更大；dist(v,ω)为水面无人艇距离障碍物最近的距离，为了避免占据太大的权重，该函数最大值为2倍的障碍物影响范围R，超过该值的dist(v,ω)值直接取2R，即

其中，norm()为计算距离函数，X₀为无人船当前所在位置，X_obs为障碍物所在位置，R为障碍物影响范围。

进一步地，所述步骤S21具体包括：

S211、考虑最大速度限制：用V_lim表示最大最小速度限制下的速度空间，则有：

V_lim＝{(v,ω)|v∈[v_min,v_max]∧ω∈[ω_min,ω_max]}

其中，v_min、v_max分别表示水面无人艇的最小、最大速度，ω_min、ω_max分别表示水面无人艇的最小、最大角速度；

S212、考虑最大加速度限制：用V_acc表示最大加速度限制下的速度空间，则有：

V_acc＝{(v,ω)|v∈[v_now-Δv·t,v_now+Δv·t]∧ω∈[ω_now-Δω·t,ω_now+Δω·t]}

其中，v_now、ω_now表示水面无人艇当前的速度和角速度，Δv、Δω表示水面无人艇的最大加速度、角加速度，t表示时间；

S213、考虑制动速度限制：设制动速度限制下的速度空间为V_safe，则有：

其中，dist(v,ω)是当前(v,ω)下轨迹与障碍物的最近距离，Δv、Δω表示加速度和角加速度；

S214、基于最大加速度限制、最大加速度限制以及制动速度限制，得到最终的速度空间V_dwa：

V_dwa＝V_lim∩V_acc∩V_safe。

进一步地，所述步骤3具体包括：

S31、当障碍物进入水面无人艇安全范围d_s时，假设无人船坐标为[x_usv,y_usv]，动态障碍物坐标为obs[x_o,y_o]，则设置一个临时障碍物obs₁[x₁,y₁]，即：

其中，Δt₁为设置的预测时间，d_s为无人艇搜索范围，dist为障碍物距离无人艇的距离，θ_o为障碍物航向，通过对二者的比较，判断是否有发生碰撞的危险；

S32、根据障碍物与无人船当前速度，预测未来一段时间内二者的距离，并取其中的最小值，即：

其中，norm()为取两点间距离函数，X_i[x_i,y_i]为无人艇的预测位姿，为障碍物的预测位姿，t_n为采样总时间，Δt为采样时间间隔；

S33、如果检测到dist_pre小于无人船避碰安全距离d_col，则设定第二个临时障碍物obs₂[x₂,y₂]，即：

其中，Δt₂小于Δt₁。

进一步地，所述步骤3还包括如下步骤：

S34、对交叉相遇的状况进行单独处理，计算障碍物方向θ_o与无人船朝向θ₀的夹角Δθ，如果二者夹角在15°到135°范围内时，即处于交叉会遇局面，则设定一个临时目标点goal_temp[x_gt,y_gt]，即：

其中，obs[x_o,y_o]为障碍物当前位置，v_o为障碍物速度，θ_o为障碍物当前航向，Δt₃为设定的时间间隔，goal₀[x_g,y_g]为正常的目标点，从上式可以得到一个与障碍物运动方向相反的临时目标点，使无人艇向障碍物后方移动，结束交叉会遇局面。

进一步地，所述步骤32中：

无人艇的预测位姿X_i[x_i,y_i]的计算公式如下：

其中，X₀[x₀,y₀]为水面无人艇的当前位置，v为无人艇速度，θ_i为无人艇预测航向，θ₀为水面无人艇当前航向，ω为无人艇当前角速度；

障碍物的预测位姿的计算公式如下：

其中，obs₀[x_o,y_o]为障碍物当前位置，v_o为障碍物当前速度，为障碍物的预测航向，/>为障碍物当前航向，ω_o为障碍物当前角速度。

较现有技术相比，本发明具有以下优点：

1、本发明提供的水面无人艇路径规划方法，其全局路径规划算法简单，计算量小，搜索速度快，可以快速实现全局最优路径的搜索。

2、本发明提供的水面无人艇路径规划方法，其局部路径规划通过动态参数，保证了运行速度，避免目标不可达。

3、本发明提供的水面无人艇路径规划方法，采用临时障碍物和临时目标点的策略，取得更安全、更高效的动态障碍物避碰效果。

基于上述理由本发明可在无人船等领域广泛推广。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明方法流程图。

图2为本发明实施例提供的改进RRT算法得到的路径。

图3为本发明实施例提供的天牛须搜索算法优化后的路径。

图4为本发明实施例提供的动态窗口法静态避障仿真。

图5为本发明实施例提供的动态障碍物追越会遇避障仿真。

图6为本发明实施例提供的动态障碍物交叉会遇避障仿真。

图7为本发明实施例提供的动态障碍物对遇避障仿真。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

如图1所示，本发明提供了一种水面无人艇路径规划方法，包括如下步骤：

S1、全局路径规划：基于改进的RRT算法生成一个改进全局路径，再通过天牛须搜索算法对改进全局路径进行优化，获得最优的全局路径；

具体实施时，优选的，所述步骤S1具体包括：

S11、基于RRT算法生成一个随机的初始路径；

将起点作为随机树的初始点x_new，列入x_list，记为x₁，在地图随机生成一点p_rand，将起点与p_rand连线，在起点后一个步长距离n处取p_new，并将其加入x_list，记为x₂；生成新的p_rand，将x_list中的所有点与p_rand连线，并在一个步长距离n后生成p_new，将所有p_new与其对应的x_i连线，如果线上有障碍物或者p_new本身就在障碍物中，则去掉该点，在剩余的点中，选取距离p_rand最近的p_new作为x_new，加入到x_list中，并记录其对应的父节点；反复执行上述步骤，不断地将搜索到的x_new加入x_list中，完成对全地图的搜索，直到x_new距离终点不超过步长n，完成对路径的搜索，并从终点不断搜索其父节点，完成路径的输出。

S12、对生成的所述初始路径中的节点进行一次再选择，以去掉所述初始路径中不必要的节点，只保留必要的路径点，生成一个改进全局路径；

将原步长n拓展为原步长的2-3倍，记为n₁，从起点开始，搜索在新步长n₁范围内的路径节点，并选取其中序号最大且与起点之间连线上无障碍物的节点，直接与起点连线，代替其中的各个节点；同时将选取的节点作为新的起点，以同样的方式选取下一节点，直到搜索到终点；再将步长新步长n₁拓展为原步长的1.5-3倍，记为n₂，再次进行搜索，反复搜索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)＝k₁·length(x)+k₂·turning(x)

其中，k₁、k₂为权重系数，length(x)为路径的总长度，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·d₀

xr＝xbest+dir·d₀

d₀＝step/c

其中，xbest为当前最优路径，d₀为天牛搜索范围，step为天牛移动步长，c为常数；由此，确定下一步天牛的位置，即对路径进行更新：

x_new＝x-step·dir·sign[f(xl)-f(xr)]

S136、基于更新后的路径x_new，比较更新后的路径x_new与原路径的适应性，当更新后的路径x_new的适应度值低于原路径时，则有：

S137、在对当前最优路径xbest进行更新以后，反复进行迭代，为步长设置一个衰减参数eta，每次迭代后对步长进行更新：

step＝step·eta

采用衰减系数eta后，天牛须搜索算法先进行大范围的搜索，在进行小范围的搜索，在迭代一定次数后，将步长step重置，搜索到全局最优解，获得最优的全局路径。

经过RRT算法的随机路径搜索，以及天牛须搜索算法对路径的优化，已经基本得到了一条最优的全局路径，但是在实际的海域中，会有很多随机出现的未知障碍物，因此需要局部路径规划来确保航行的安全性。

S2、局部路径规划：采用动态窗口法进行局部路径规划，并通过参数调整避免陷入目标不可达的局面；

具体实施时，优选的，所述步骤S2具体包括：

S21、考虑最大速度限制、最大加速度限制以及制动速度限制，确定最终的速度空间；

S211、考虑最大速度限制：水面无人艇的速度和角速度都是有一定限制的，因此在设计动态窗口时，首先要考虑的就是其能到达的最大和最小速度，用V_lim表示最大最小速度限制下的速度空间，则有：

V_lim＝{(v,ω)|v∈[v_min,v_max]∧ω∈[ω_min,ω_max]}

其中，v_min、v_max分别表示水面无人艇的最小、最大速度，ω_min、ω_max分别表示水面无人艇的最小、最大角速度；

S212、考虑最大加速度限制：水面无人艇的加速度往往是有限的，相比于陆地的移动机器人和无人机，水面无人艇加速度性能会更受限制，因此加速度限制也是水面无人艇必须考虑的限制因素，用V_acc表示最大加速度限制下的速度空间，则有：

V_acc＝{(v,ω)|v∈[v_now-Δv·t,v_now+Δv·t]∧ω∈[ω_now-Δω·t,ω_now+Δω·t]}

其中，v_now、ω_now表示水面无人艇当前的速度和角速度，Δv、Δω表示水面无人艇的最大加速度、角加速度，t表示时间；

S213、考虑制动速度限制：为了确保水面无人艇不会与障碍物发生碰撞，也要考虑水面无人艇能否及时制动。当无人艇距离障碍物非常近时，其速度也应降低，以确保在发生撞击前无人艇可以停止，设制动速度限制下的速度空间为V_safe，则有：

其中，dist(v,ω)是当前(v,ω)下轨迹与障碍物的最近距离，Δv、Δω表示加速度和角加速度；

S214、基于最大加速度限制、最大加速度限制以及制动速度限制，得到最终的速度空间V_dwa：

V_dwa＝V_lim∩V_acc∩V_safe。

考虑到目标不可达的主要原因是评估函数中heading(v,ω)和dist(v,ω)两个参数共同导致的，当水面无人艇在朝向目标点行驶时，其heading(v,ω)值是最大的，且速度值越小，其dist(v,ω)越大，这就导致了水面无人艇的速度保持在极低的值。为了解决这一问题，本部分采用动态参数的策略，当水面无人艇的速度值很小时，认为有目标不可达的趋势，则采取另一套评估函数的权重系数α、β、γ，提高γ值，减小α、β值，使velocity(v,ω)对评估函数的值影响更大，从而跳出局部最优解的局面。即：

S22、基于确定的所述速度空间，通过评估函数G(v,ω)选取下一时刻的速度与角速度：

其中，α₁、β₁、γ₁；α₂、β₂、γ₂分别为两组不同的权重系数，通过动态参数变化，可以有效地解决目标不可达的问题，同时，新的评估函数也能确保水面无人艇全程高速运行，更快到达目标点。velocity(v,ω)为下一时刻的速度值，以确保水面无人艇运动的速度，heading(v,ω)是对船艏向的评估函数，计算公式如下：

heading(v,ω)＝180-|goaltheta-theta|

goaltheta是目标点相对当前位置的方向角，theta为当前的船艏向，当两者差值更小时，heading(v,ω)的值更大；dist(v,ω)为水面无人艇距离障碍物最近的距离，为了避免占据太大的权重，该函数最大值为2倍的障碍物影响范围R，超过该值的dist(v,ω)值直接取2R，即

其中，norm()为计算距离函数，X₀为无人船当前所在位置，X_obs为障碍物所在位置，R为障碍物影响范围。

在水面无人艇的运行环境中，可能会受到不同方向的动态障碍物带来的危险，大致可以将动态障碍物的会遇局面分为三类，即对遇、交叉、会遇。本部分针对动态障碍物，提出了新的避障策略，以达到更高效、更安全的避碰效果，使无人船尽快结束避碰局面。当障碍物在运动时，简单的对障碍物当前位置进行躲避显然不能达到最好的效果，障碍物在不断运动，其一段时间后的位置其实是容易发生碰撞的位置，将下一时刻障碍物的位置作为障碍物点，可以提前使无人艇避开会遇局面。

S3、基于不同的动态障碍物会遇情况，采用不同的避碰策略，使水面无人艇在短时间内结束会遇局面，进而达到最好的动态障碍物躲避效果。

具体实施时，优选的，所述步骤3具体包括：

S31、当障碍物进入水面无人艇安全范围d_s时，假设无人船坐标为[x_usv,y_usv]，动态障碍物坐标为obs[x_o,y_o]，则设置一个临时障碍物obs₁[x₁,y₁]，即：

其中，Δt₁为设置的预测时间，d_s为无人艇搜索范围，dist为障碍物距离无人艇的距离，θ_o为障碍物航向，通过对二者的比较，判断是否有发生碰撞的危险；

S32、根据障碍物与无人船当前速度，预测未来一段时间内二者的距离，并取其中的最小值，即：

其中，norm()为取两点间距离函数，X_i[x_i,y_i]为无人艇的预测位姿，为障碍物的预测位姿，t_n为采样总时间，Δt为采样时间间隔；

S33、如果检测到dist_pre小于无人船避碰安全距离d_col，则设定第二个临时障碍物obs₂[x₂,y₂]，即：

其中，Δt₂小于Δt₁。

在动态障碍物避障时，交叉会遇是较为危险的一种状况，避碰难度也较大，因此本部分对交叉相遇的状况进行单独处理，即：

S34、对交叉相遇的状况进行单独处理，计算障碍物方向θ_o与无人船朝向θ₀的夹角Δθ，如果二者夹角在15°到135°范围内时，即处于交叉会遇局面，则设定一个临时目标点goal_temp[x_gt,y_gt]，即：

其中，obs[x_o,y_o]为障碍物当前位置，v_o为障碍物速度，θ_o为障碍物当前航向，Δt₃为设定的时间间隔，goal₀[x_g,y_g]为正常的目标点，从上式可以得到一个与障碍物运动方向相反的临时目标点，使无人艇向障碍物后方移动，结束交叉会遇局面。

本发明实施例还提供了仿真验证，具体如下：

全局路径规划

本部分为基于改进的RRT算法和天牛须搜索算法的全局路径规划，如图2所示，为改进RRT算法得到的路径，红色路径为RRT算法得到的初始路径，蓝色路径为进行节点选择后的优化路径。可以看出，两条路径都距离障碍物一定的距离，保证了路径的安全性，且蓝色路径省去了红色路径中多余的节点，使路径更加平滑，成为可行且能保证安全的路径。如图3所示，为天牛须搜索算法优化后的路径。红色为改进RRT算法获取的路径，蓝色为天牛须算法优化后的路径，蓝色路径在保证安全性的前提下，在路径长度和路径转角上得到了优化。

局部路径规划

本部分基于改进动态窗口法的局部路径规划仿真，如图3所示，为躲避静态障碍物的仿真结果，从运动轨迹来看，无人艇能够保持与障碍物的安全距离，并且能保证无人艇全程高速运行，避免了局部最优解造成的无人艇难以到达目标点的问题，达到了很好的避障效果。图4-图6为躲避动态障碍物的仿真结果，其中蓝色圆形为水面无人艇，红色为障碍物。在仿真中，设置了对遇、追越、交叉三种会遇局面，可以看到，通过在障碍物预测位置设置临时障碍物，可以实现提前避开障碍物，且通过在交叉会遇时，设置临时目标点，可以使无人艇从障碍物后方绕行，保证安全且更加高效，避免了过于冒进的避碰策略，提前避免了碰撞风险。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (6)

1.一种水面无人艇路径规划方法，其特征在于，包括如下步骤：

S1、全局路径规划：基于改进的RRT算法生成一个改进全局路径，再通过天牛须搜索算法对改进全局路径进行优化，获得最优的全局路径；所述步骤S1具体包括：

S11、基于RRT算法生成一个随机的初始路径；所述步骤S11具体包括：

将起点作为随机树的初始点x_new，列入x_list，记为x₁，在地图随机生成一点p_rand，将起点与p_rand连线，在起点后一个步长距离n处取p_new，并将其加入x_list，记为x₂；生成新的p_rand，将x_list中的所有点与p_rand连线，并在一个步长距离n后生成p_new，将所有p_new与其对应的x_i连线，如果线上有障碍物或者p_new本身就在障碍物中，则去掉该点，在剩余的点中，选取距离p_rand最近的p_new作为x_new，加入到x_list中，并记录其对应的父节点；反复执行上述步骤，不断地将搜索到的x_new加入x_list中，完成对全地图的搜索，直到x_new距离终点不超过步长n，完成对路径的搜索，并从终点不断搜索其父节点，完成路径的输出；

S12、对生成的所述初始路径中的节点进行一次再选择，以去掉所述初始路径中不必要的节点，只保留必要的路径点，生成一个改进全局路径；所述步骤S12具体包括：

将原步长n拓展为原步长的2-3倍，记为n₁，从起点开始，搜索在新步长n₁范围内的路径节点，并选取其中序号最大且与起点之间连线上无障碍物的节点，直接与起点连线，代替其中的各个节点；同时将选取的节点作为新的起点，以同样的方式选取下一节点，直到搜索到终点；再将步长新步长n₁拓展为原步长的1.5-3倍，记为n₂，再次进行搜索，反复搜索3-5次，将路径中不必要的节点去掉，只保留必要的路径点；

S13、将生成的所述改进全局路径作为初解，采用天牛须搜索算法对所述改进全局路径进行优化，生成最优的全局路径；所述步骤S13具体包括：

S131、基于一个包括n个路径点的全局路径x，设x＝xbest，即当前最优路径，假设天牛的头部朝向随机，并用n×2维的随机数dir表示此方向；

S132、对dir进行归一化处理：

dir＝rands(n,2)

dir＝dir/norm(dir)

其中，norm()为取两点间距离函数；

S133、设置须的气味函数，即适应度函数，如下：

f(x)＝k₁·length(x)+k₂·turning(x)

其中，k₁、k₂为权重系数，length(x)为路径的总长度，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·d₀

xr＝xbest+dir·d₀

d₀＝step/c

其中，xbest为当前最优路径，d₀为天牛搜索范围，step为天牛移动步长，c为常数；由此，确定下一步天牛的位置，即对路径进行更新：

x_new＝x-step·dir·sign[f(xl)-f(xr)]

S136、基于更新后的路径x_new，比较更新后的路径x_new与原路径的适应性，当更新后的路径x_new的适应度值低于原路径时，则有：

S137、在对当前最优路径xbest进行更新以后，反复进行迭代，为步长设置一个衰减参数eta，每次迭代后对步长进行更新：

step＝step·eta

采用衰减系数eta后，天牛须搜索算法先进行大范围的搜索，在进行小范围的搜索，在迭代一定次数后，将步长step重置，搜索到全局最优解，获得最优的全局路径；

S2、局部路径规划：采用动态窗口法进行局部路径规划，并通过参数调整避免陷入目标不可达的局面；

S3、基于不同的动态障碍物会遇情况，采用不同的避碰策略，使水面无人艇在短时间内结束会遇局面，进而达到最好的动态障碍物躲避效果。

2.根据权利要求1所述的一种水面无人艇路径规划方法，其特征在于，所述步骤S2具体包括：

S21、考虑最大速度限制、最大加速度限制以及制动速度限制，确定最终的速度空间；

S22、基于确定的所述速度空间，通过评估函数G(v,ω)选取下一时刻的速度与角速度：

其中，α₁、β₁、γ₁；α₂、β₂、γ₂分别为两组不同的权重系数，velocity(v,ω)为下一时刻的速度值，以确保水面无人艇运动的速度，heading(v,ω)是对船艏向的评估函数，计算公式如下：

heading(v,ω)＝180-|goaltheta-theta|

goaltheta是目标点相对当前位置的方向角，theta为当前的船艏向，当两者差值更小时，heading(v,ω)的值更大；dist(v,ω)为水面无人艇距离障碍物最近的距离，为了避免占据太大的权重，该函数最大值为2倍的障碍物影响范围R，超过heading(v,ω)的dist(v,ω)值直接取2R，即

其中，norm()为计算距离函数，X₀为无人船当前所在位置，X_obs为障碍物所在位置，R为障碍物影响范围。

3.根据权利要求2所述的一种水面无人艇路径规划方法，其特征在于，所述步骤S21具体包括：

S211、考虑最大速度限制：用V_lim表示最大最小速度限制下的速度空间，则有：

V_lim＝{(v,ω)|v∈[v_min,v_max]∧ω∈[ω_min,ω_max]}

其中，v_min、v_max分别表示水面无人艇的最小、最大速度，ω_min、ω_max分别表示水面无人艇的最小、最大角速度；

S212、考虑最大加速度限制：用V_acc表示最大加速度限制下的速度空间，则有：

V_acc＝{(v,ω)|v∈[v_now-Δv·t,v_now+Δv·t]∧ω∈[ω_now-Δω·t,ω_now+Δω·t]}

其中，v_now、ω_now表示水面无人艇当前的速度和角速度，Δv、Δω表示水面无人艇的最大加速度、角加速度，t表示时间；

S213、考虑制动速度限制：设制动速度限制下的速度空间为V_safe，则有：

其中，dist(v,ω)是当前(v,ω)下轨迹与障碍物的最近距离，Δv、Δω表示加速度和角加速度；

S214、基于最大加速度限制、最大加速度限制以及制动速度限制，得到最终的速度空间V_dwa：

V_dwa＝V_lim∩V_acc∩V_safe。

4.根据权利要求1所述的一种水面无人艇路径规划方法，其特征在于，所述步骤S3具体包括：

S31、当障碍物进入水面无人艇安全范围d_s时，假设无人船坐标为[x_usv,y_usv]，动态障碍物坐标为obs[x_o,y_o]，则设置一个临时障碍物obs₁[x₁,y₁]，即：

其中，Δt₁为设置的预测时间，d_s为无人艇搜索范围，dist为障碍物距离无人艇的距离，θ_o为障碍物航向，通过对二者的比较，判断是否有发生碰撞的危险；

S32、根据障碍物与无人船当前速度，预测未来一段时间内二者的距离，并取其中的最小值，即：

其中，norm()为取两点间距离函数，X_i[x_i,y_i]为无人艇的预测位姿，为障碍物的预测位姿，t_n为采样总时间，Δt为采样时间间隔；

S33、如果检测到dist_pre小于无人船避碰安全距离d_col，则设定第二个临时障碍物obs₂[x₂,y₂]，即：

其中，Δt₂小于Δt₁。

5.根据权利要求4所述的一种水面无人艇路径规划方法，其特征在于，所述步骤S3还包括如下步骤：

S34、对交叉相遇的状况进行单独处理，计算障碍物方向θ_o与无人船朝向θ₀的夹角Δθ，如果二者夹角在15°到135°范围内时，即处于交叉会遇局面，则设定一个临时目标点goal_temp[x_gt,y_gt]，即：

其中，obs[x_o,y_o]为障碍物当前位置，v_o为障碍物速度，θ_o为障碍物当前航向，Δt₃为设定的时间间隔，goal₀[x_g,y_g]为正常的目标点，从上式可以得到一个与障碍物运动方向相反的临时目标点，使无人艇向障碍物后方移动，结束交叉会遇局面。

6.根据权利要求4所述的一种水面无人艇路径规划方法，其特征在于，所述步骤S32中：

无人艇的预测位姿X_i[x_i,y_i]的计算公式如下：

其中，X₀[x₀,y₀]为水面无人艇的当前位置，v为无人艇速度，θ_i为无人艇预测航向，θ₀为水面无人艇当前航向，ω为无人艇当前角速度；

障碍物的预测位姿的计算公式如下：

其中，obs₀[x_o,y_o]为障碍物当前位置，v_o为障碍物当前速度，为障碍物的预测航向，为障碍物当前航向，ω_o为障碍物当前角速度。