考虑转角约束的Bi-RRT无人艇多艇导航方法及设备

本发明属于无人艇领域，更具体地，涉及一种考虑转角约束的bi-rrt无人艇多艇导航方法及设备。

背景技术：

无人艇导航是其安全自主航行的关键技术，而多艇航行规划是保证导航效率的关键，对任务协调和高效作业有重要意义。多艇导航包含任务分配、全局航路规划和航路协调策略，对于多艇而言，在建立全局航路基础上，还需要考虑协同舰艇通过的优先级，采取相应的避碰策略，能够及时有效地调整航向和速度。保证目标任务的前提下，尽可能快速、安全航行。

快速扩展随机树rrt(rapidly-exploringrandomtrees，rrt)算法基于采样的运动规划思想，不需要建立空间信息模型，有着状态空间概率完备、搜索速度快等优点，在无人机、自主移动机器人等领域的应用较多，研究也较为成熟，但是在无人艇领域研究还较少。连接型双向快速拓展随机树(bi-rrt)具有生成速度快，适应性强的特点。无人艇高航速，实时性强，bi-rrt算法优点突出，可以适应场景需求。但是bi-rrt算法在多艇航行规划还存在两个主要问题：

(1)bi-rrt算法在全局规划时随机性强收敛速度慢，连接点路径曲折；

(2)bi-rrt可以对全局地图内的障碍物进行避碰，但是对多艇的节点冲突却难以解决。

因此，有必要提出一种新的bi-rrt全局路径规划方法，以解决上述问题。

技术实现要素：

基于现有技术的以上缺陷或改进需求，本发明提供了一种考虑转角约束的bi-rrt无人艇多艇导航方法及设备，通过多任务全局规划和单航路转角约束，结合基于优先级的协调避碰策略，实现多无人艇多任务导航。

为实现上述目的，按照本发明的一个方面，提供了一种考虑转角约束的bi-rrt无人艇多艇导航方法，包括如下步骤：

步骤1：根据实际工作环境，按照到目标点总路径直线距离最短原则生成无人艇的多目标点任务方案，并根据预设的任务释放顺序将各个目标点任务方案分配给各自对应的无人艇；

步骤2：根据实际工作环境的电子海图和各无人艇接受的分配任务，首先对各个无人艇分别进行全局规划，具体地，依据步骤1分配的目标点任务方案，采用考虑转角约束的bi-rrt对单个无人艇进行航路规划，考虑随机树拓展以及连接点末端的无人艇转角约束；再将规划的各个可行航路实时发送给对应的无人艇，并储存航路状态空间；

步骤3：在航行过程中，根据无人艇当前所处环境的目标信息与位置信息进行多艇任务规划序列的调整，判断现有航路状态空间中是否存在航路冲突，若存在航路冲突则在冲突区域按照设定的优先通过顺序依次通过，以进行航路协调避碰；

步骤4：判断无人艇是否到达目标点，若已到达则释放该无人艇对应航路的所有节点，否则继续步骤3；

步骤5：判断航路状态空间是否为空，即所有无人艇是否都已到达任务目标点，若均到达则任务结束，否则继续返回步骤3。

进一步地，步骤1中，对周围环境进行分析后，根据预定目标，基于可选择的动作及所提供的资源限制，综合制定出实现多目标任务的动作序列；在对任务进行分配时，将多目标点任务一一对应分配给各无人艇，预规划各无人艇到达相应目标点的路径方案；再计算各个路径方案中所有无人艇的行驶距离总和以及各路径交叉节点数量总和，选取代价加权最小的一个方案作为全局导航任务方案。

进一步地，所述代价加权的目标函数公式如下：

式中：

f—任务分配代价函数；

n—任务数量，亦即无人艇数量；

μ1—距离系数；

μ2—交叉点数量系数；

lengthk—第k条路径的距离；

nodek—第k条路径的交叉节点数量。

进一步地，步骤2中，考虑随机树拓展以及连接点末端的无人艇转角约束，具体包括：

在固定坐标系下，路径节点与步长δs，路径曲率角θ之间的关系模型为：

sx＝x+δscos(θ)

sy＝y+δssin(θ)

其中，(x,y)为上一个节点，(sx,sy)为拓展后的节点；对于单个随机树，相邻两个节点之间的夹角存在限制，与曲率和步长的关系表示为全局路径角度约束：

δθ≤ρmaxδs＝θthreshold

其中，ρ为路径上节点的实时曲率，ρmax为该条路径的最大曲率，δs为相邻节点之间的弧长(步长)，θthreshold为允许的最大转角，δθ为拓展节点的转向角度；

对于双向连接型rrt规划，δθ用向量表示为：

δθ1i＜θthreshold,δθ2j＜θthreshold

表示第一棵随机树t1的上一步拓展步长向量，表示t1上新节点ξnew的拓展节点向量，i、j是节点编号；表示第二棵随机树t2的上一步拓展步长向量，表示t2上新节点ξ'new的拓展节点向量；δθ1i、δθ2j分别为ξnew、ξ'new对应的拓展节点转向角度，θthreshold是无人艇最大转角。

进一步地，在每一步拓展中判断两棵随机树的拓展节点之间的距离当小于阈值ε时表示t1和t2相遇：

在相遇之时判断拓展节点ξnew和ξ'new的连线是否满足转角约束，该连线的转向角度γ表示为：

若γ小于无人艇最大转角θthreshold，则拓展节点满足转角约束，否则认为相遇无效，交换t1和t2的拓展顺序，继续下一步搜索拓展，直到找到满足符合转角约束的连接。

进一步地，步骤3中采用基于优先级的航路协调避碰策略，包括优先权的制定，其中：

同时有多个无人艇申请同一交叉节点资源时，根据各无人艇当前状况以及无人艇续航状态加权计算任务优先权priority：

其中，a、b、c是加权系数，为经验值；emergency表示任务紧急程度，distance表示到目标点的剩余距离，crossnum表示剩余路段中交叉节点的数量；energy表示无人艇剩余电量，用于衡量无人艇续航状态；

priority值越大则优先级越高，在易发生碰撞的区域按照优先级高低顺序先后通过以进行避碰。

进一步地，步骤3中采用基于优先级的航路协调避碰策略，还包括航路避碰协调策略的制定，具体包括碰撞预测与变速避碰，其中：

碰撞预测：

记交叉节点区域为dis，当船只进入该区域即当d(t)＜dis时触发碰撞预测，对各无人艇之间的趋势路径进行等时间间隔采样，实时计算各无人艇艇在各个时刻t与交叉节点的距离d(t)：

其中，无人艇在t时刻到达交叉节点的两个方向的距离δx(t)、δy(t)为：

(xt,yt)为交叉节点的坐标，(x,y)为无人艇当前位置，v为无人艇当前速度，为当前朝向角；若同一时刻在dis区域内有多个无人艇，则该航路节点不满足安全性要求，为危险区域，该交叉节点为冲突节点，易发生碰撞；

变速避碰：

设dmin为最小变速距离，δd(t)为两个无人艇离冲突节点的距离之差；协调避碰过程实现如下：

(1)当同一时刻多个无人艇满足dmin＜δd(t)，d(t)＜dis时，触发变速协调策略：在冲突节点该区域内根据制定的优先权，priority权值高的优先占用节点资源，加速通过该节点，其余无人艇减速，直到前一艘无人艇驶离dis区域后一优先级的无人艇再开始加速通过；

(2)当d(t)≤dmin时，同一时刻只能允许该区域只有一艘无人艇通过，其余无人艇暂停等待；当无人艇驶离该区域，释放该节点资源，按照优先权队列的顺序依次通过剩余无人艇。

为了实现上述目的，按照本发明的另一个方面，提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现如前任一项所述的考虑转角约束的bi-rrt无人艇多艇导航方法。

为了实现上述目的，按照本发明的另一个方面，提供了一种考虑转角约束的bi-rrt无人艇多艇导航设备，包括如前所述的计算机可读存储介质以及处理器，处理器用于调用和处理计算机可读存储介质中存储的计算机程序。

总体而言，本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

1、本发明针对多无人艇的多任务目标导航规划，先考虑无人艇大场景、多任务以及灵活性，基于目标点总路径直线距离最短原则及预设的任务释放顺序进行全局任务分配，初步获得各无人艇的全局航路规划情况；在此基础上，提出一种考虑转角约束的bi-rrt全局路径规划方法，该方法考虑无人艇的转角约束，解决单个无人艇路径优化随机性强的问题，使得航路平滑，并且避免连接节点曲折角过大，从而获得全局路径状态空间；同时，在已有全局路径状态空间的基础上，提出了基于优先权的航路协调避碰策略，在容易发生冲突的部位按照优先通过顺序依次通过实现协调避碰，从而完成无人艇多艇导航的快速规划与协调避碰。

2、在任务规划过程中充分考虑环境及资源情况，确保全局导航任务分配优化，提高航路规划效率，具有较高的灵活性。

3、全局考虑无人艇bi-rrt随机树拓展以及连接点末端的转角约束，确保规划出的路径是符合无人艇运动限制的可行航路，在转角约束中同时引入弧长限制进行全局路径角度约束，可以进一步限制算法随机性，提高搜索效率，使得随机树的扩展更加贪婪和明确，更带有启发性。

4、由于bi-rrt算法中的连接方式对连接点的约束较弱，本发明在每一步拓展中加入判断两棵树新节点之间的距离，以对连接方式进行优化，通过判断节点连线对转角约束的满足情况对双树拓展顺序进行自适应调整，从而约束新拓展节点的更新情况，以使双树拓展进度尽量保持一致，从而加速收敛，同时使得路径规划结果更为合理性。

5、综合考虑任务紧急程度、任务剩余时间(以距离和交叉节点衡量)以及无人艇的续航状态来设定无人艇通过冲突区域的优先级，能够通过调节加权系数来适应不同的任务场景，使得优先级的制定更为合理，确保任务的顺利完成。

6、只在进入交叉节点区域时触发碰撞预测可以节省处理器能源，并且通过无人艇的位置、速度、朝向能够提前预测危险区域及冲突的发生情况，因而有利于提前进行协调避碰。根据设定的优先级采用变速避碰策略，使存在冲突的多个无人艇迅速顺利通过危险区域，提高任务执行效率，尽量减少对总任务进程的影响。

7、本发明还适于在上位机中央控制器中对所有的无人艇统一进行全局路径规划和调度，以便于多艇协调避碰，保证多艇导航的高效性。

附图说明

图1是考虑转角约束的bi-rrt的多无人艇导航方法流程图。

图2是考虑转角约束的bi-rrt全局路径规划流程图。

图3是随机拓展树新节点转角约束示意图。

图4是末端连接点转角约束示意图。

图5是基于变速调节的航路协调避碰策略。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

在本发明的优选实施例中所提出的一种考虑转角约束的bi-rrt无人艇多艇导航方法，其主要构思在于，针对各无人艇的全局航路的建立，基于角度约束模型提出考虑转角的bi-rrt全局路径规划方法，该方法考虑无人艇的转角约束，解决随机性强的问题，使得航路平滑，并且避免连接节点曲折角过大。同时，在已有全局路径状态空间的基础上，提出了基于优先权的航路协调避碰策略，通过上位机中央控制器协调进行局部反应式导航。

首先，考虑转角约束的bi-rrt对每个无人艇进行全局航路规划，生成带有转角约束的无人艇可行航路。按照任务分配方案再将可行航路实时发送给对应的无人艇，运动过程中实时判断现有航路状态空间中是否存在冲突，确定是否需要触发避碰策略，直到无人艇到达目标点，释放该条航路所有节点。直到所有的无人艇都到达了目标点，释放了所有的航路节点，整个规划流程终止。整个过程考虑无人艇环境的大场景和运动的灵活性，要求规划的高效性，考虑转角约束的bi-rrt的航路规划算法中，双向bi-rrt从初始状态点和目标状态点同时生长两棵快速扩展随机树来搜索状态空间，带有启发性的扩展使得树的扩展更加贪婪和明确，效率更高。同时，考虑了拓展节点及连接节点末端的最大角度限制来满足无人艇的运动约束。此后的每一条航路生成完成实时下发到对应的无人艇，上位机中央控制器实时监测每个无人艇的状态并进行碰撞检预测，采取相应的航路协调策略。

下面结合附图对本发明的应用原理做详细描述。

本发明优选的一种考虑转角约束的bi-rrt无人艇多艇导航方法，如附图1的流程图所示。

步骤1：以无人艇高效航行为目的，结合实际环境，考虑最短直线路径总和以及最少交叉节点的代价加权，生成各无人艇多目标点任务方案，根据任务释放顺序(优选为任务紧急程度)初步规划所有无人艇的全局航路，即，将各目标点一一对应分配给各个无人艇，并设定各个无人艇的出发顺序。

步骤2：无人艇根据电子海图和接受的分配任务首先进行全局规划。依据规划的方案考虑转角约束的bi-rrt对各个无人艇进行航路规划，考虑随机树拓展以及连接点末端的无人艇转角约束。再将可行航路实时发送给对应的无人艇，储存航路状态空间。

步骤3：在航行过程中，任务规划模块会根据当前的环境目标信息与位置信息进行多艇任务规划序列的调整，判断现有航路状态空间中是否存在冲突，存在冲突则触发基于优先级的航路协调避碰策略。

步骤4：判断无人艇是否到达目标点，若已到达则释放该条航路所有节点，否则继续步骤3。

步骤5：判断航路状态空间是否为空，即所有无人艇是否都已到达任务目标点，若到达目标点结束，否则继续返回步骤3。

进一步的，步骤1中，进行全局任务规划时，按照到目标点总路径直线距离最短原则，各无人艇多目标任务的生成，具体包括：

任务规划是对周围环境进行分析后，根据预定目标，对可选择的动作及所提供的资源限制进行推理，综合制定出实现多目标任务的动作序列。在对任务进行分配时，系统对所有多目标点任务分配给各无人艇，规划出相应的路径方案。再计算各个方案中所有无人艇的直线行驶距离总和以及各直线路径交叉节点数量总和，选取代价加权最小的一个方案。其目标函数公式如下：

式中：

f—任务分配代价函数；

n—任务数量或者无人艇数量；

μ1，μ2—距离系数，交叉点数量系数；

lengthi—第i条直线路径的距离；

nodei—第i条路径的交叉节点数量。

确定任务分配方案后，根据任务的释放顺序依次进行全局航路规划，例如设定各无人艇的出发顺序。

进一步的，步骤2中，考虑随机树拓展以及连接点末端的无人艇转角约束，具体包括：

图2为考虑转角约束的bi-rrt全局路径规划流程图，图3则是随机拓展树新节点转角约束示意。两棵随机树的节点拓展需要加转角约束判断，另外相遇的连接点末端也加上角度判断，若不满足转角约束，舍弃当前拓展的节点，进行下一步搜索。

在固定坐标系下，路径节点与步长δs，路径曲率角θ之间的关系模型为：

sx＝x+δscos(θ)

sy＝y+δssin(θ)

其中，(x,y)为上一个节点，(sx,sy)为拓展后的节点；为了限制了算法的随机性从而提高搜索效率，对于单个随机树，相邻两个节点之间的夹角存在限制，与曲率和步长的关系表示为全局路径角度约束：

δθ≤ρmaxδs＝θthreshold

其中，ρ为路径上节点的实时曲率，ρmax为该条路径的最大曲率，δs为相邻节点之间的弧长(步长)，θthreshold为允许的最大转角，δθ为拓展节点的转向角度；

对于双向连接型rrt规划，δθ用向量表示为：

表示第一棵随机树t1的上一步拓展步长向量，表示t1拓展节点向量；表示第二棵随机树t2的上一步拓展步长向量，表示t2拓展节点向量。δθ1i,δθ2j为拓展节点转向角度，θthreshold是无人艇最大转角，应当满足：

δθ1i＜θthreshold,δθ2j＜θthreshold

bi-rrt算法中的连接方式对连接点的约束较弱，因此需要对连接方式进行优化，在每一步拓展中加入判断两棵树新节点之间的距离当小于阈值ε表示t1和t2相遇：

在相遇之时判断节点ξnew,ξ'new连线是否转角约束，连接节点末端的转向角度γ可以表示为：

图4所示为末端连接点转角约束示意图，连接点的转角约束为：当连接节点末端的转向角度γ小于无人艇最大转角θthreshold，拓展节点满足转角约束，否则认为相遇无效，交换搜索随机树的拓展顺序继续下一步搜索拓展，直到找到满足符合转角约束的连接。在图中，γ会大于θthreshold，此时相遇无效，下一步搜索以t2开始拓展，设ξj为距离随机点最近的节点，拓展新节点ξ'new2，再判断节点末端的转向角度，此时满足运动约束要求，终止拓展并记录路径节点。

进一步的，步骤3中，基于优先级的航路协调避碰策略，具体包括：

为保证系统的协调性，各无人艇的传感器信息都传送给上位机中央控制器，中央控制器再根据所有掌握的信息来统一规划各个无人艇。基于优先级的航路协调避碰策略分为优先权的制定，航路避碰协调策略。

进一步的，所述的优先权的制定，具体包括：

尽管在步骤1中已经根据任务的紧急程度在进行优先规划全局航路，但是在航行过程中，若在交叉节点一定区域内出现多个无人艇，即同时有多个无人艇申请同一交叉节点资源时，根据各无人艇当前运行状况，例如该任务紧急程度emergency，所需执行时间(到目标点的剩余距离distance，剩余路段中交叉节点的数量crossnum)，无人艇续航状态如无人艇剩余电量energy，可能需要对该交叉节点处的任务优先权重新进行局部规划。，交叉节点处的任务优先权priority按照加权计算：

其中，a、b、c是加权系数，均为经验值。根据不同的工作场景以及任务需求，可以通过调节a、b、c的值来改变局部任务优先顺序，以更好地适应工作任务要求。

进一步的，所述的航路避碰协调策略，具体包括：

为解决节点冲突问题，包含碰撞预测与变速避碰。交叉节点区域为dis，当船只进入该区域即当d(t)＜dis时触发碰撞预测，对艇间的趋势路径进行等时间间隔采样，实时计算各艇在各个时刻与交叉节点的距离d(t)。如图5所示，艇o和艇t将驶向统一节点(xt,yt)，艇o在t时刻到达交叉节点的两个方向的距离δx0(t)，δy0(t)分别为：

艇t在t时刻到达交叉节点的两个方向的距离δx1(t)，δy1(t)分别为：

其中，两艇对应的，(xt,yt)为交叉节点的坐标，(x0,y0),(x1,y1)为无人艇当前位置，v0，v1为无人艇当前速度，为当前朝向角，d0(t),d1(t)为各个时刻与交叉节点的距离。若同一时刻在dis区域内有多个无人艇，则该航路节点不满足安全性要求，为危险区域，该交叉节点为冲突节点。

设dmin为最小变速距离，δd(t)＝|d0(t)-d1(t)|为离冲突节点的距离之差。当同一时刻多个无人艇满足dmin＜δd(t)，d(t)＜dis时，触发变速协调策略，在冲突节点该区域内根据制定的优先权，priority权值高的优先占用节点资源，加速通过该节点，其余无人艇减速，直到前一艘艇驶离dis区域即其对应的d(t)＞dis才开始加速；当d(t)≤dmin时，同一时刻只能允许该区域只有一艘无人艇通过，其余无人艇暂停等待。当无人艇驶离该区域，释放该节点资源，按照优先权队列的顺序依次通过剩余无人艇。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。