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

本发明专利技术属于无人艇领域，公开了一种考虑转角约束的Bi‑RRT无人艇多艇导航方法及设备。该方法通过全局任务分配，基于分配的任务规划考虑节点连接的各个无人艇航路，对航路节点进行碰撞判断和协调避碰，并通过节点释放来判断无人艇是否到达对应的目标点，最后直到所有的无人艇都到达目标点。整个过程考虑无人艇大场景，多任务以及灵活性，所以航路规划应当确保高效性，考虑转角约束的Bi‑RRT的航路规划算法中，考虑了拓展节点和连接节点末端的转角约束以适应无人艇的运动约束，带有启发性的扩展使得树的扩展更加贪婪和明确，效率更高。

【技术实现步骤摘要】
考虑转角约束的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时，触发变速协调策略：在冲突节点该区域内根据制定的本文档来自技高网...

【技术保护点】
1.一种考虑转角约束的Bi-RRT无人艇多艇导航方法，其特征在于，包括如下步骤：/n步骤1：根据实际工作环境，按照到目标点总路径直线距离最短原则生成无人艇的多目标点任务方案，并根据预设的任务释放顺序将各个目标点任务方案分配给各自对应的无人艇；/n步骤2：根据实际工作环境的电子海图和各无人艇接受的分配任务，首先对各个无人艇分别进行全局规划，具体地，依据步骤1分配的目标点任务方案，采用考虑转角约束的Bi-RRT对单个无人艇进行航路规划，考虑随机树拓展以及连接点末端的无人艇转角约束；再将规划的各个可行航路实时发送给对应的无人艇，并储存航路状态空间；/n步骤3：在航行过程中，根据无人艇当前所处环境的目标信息与位置信息进行多艇任务规划序列的调整，判断现有航路状态空间中是否存在航路冲突，若存在航路冲突则在冲突区域按照设定的优先通过顺序依次通过，以进行航路协调避碰；/n步骤4：判断无人艇是否到达目标点，若已到达则释放该无人艇对应航路的所有节点，否则继续步骤3；/n步骤5：判断航路状态空间是否为空，即所有无人艇是否都已到达任务目标点，若均到达则任务结束，否则继续返回步骤3。/n

【技术特征摘要】
1.一种考虑转角约束的Bi-RRT无人艇多艇导航方法，其特征在于，包括如下步骤：
步骤1：根据实际工作环境，按照到目标点总路径直线距离最短原则生成无人艇的多目标点任务方案，并根据预设的任务释放顺序将各个目标点任务方案分配给各自对应的无人艇；
步骤2：根据实际工作环境的电子海图和各无人艇接受的分配任务，首先对各个无人艇分别进行全局规划，具体地，依据步骤1分配的目标点任务方案，采用考虑转角约束的Bi-RRT对单个无人艇进行航路规划，考虑随机树拓展以及连接点末端的无人艇转角约束；再将规划的各个可行航路实时发送给对应的无人艇，并储存航路状态空间；
步骤3：在航行过程中，根据无人艇当前所处环境的目标信息与位置信息进行多艇任务规划序列的调整，判断现有航路状态空间中是否存在航路冲突，若存在航路冲突则在冲突区域按照设定的优先通过顺序依次通过，以进行航路协调避碰；
步骤4：判断无人艇是否到达目标点，若已到达则释放该无人艇对应航路的所有节点，否则继续步骤3；
步骤5：判断航路状态空间是否为空，即所有无人艇是否都已到达任务目标点，若均到达则任务结束，否则继续返回步骤3。


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


3.如权利要求2所述的一种考虑转角约束的Bi-RRT无人艇多艇导航方法，其特征在于，所述代价加权的目标函数公式如下：



式中：
F—任务分配代价函数；
n—任务数量，亦即无人艇数量；
μ1—距离系数；
μ2—交叉点数量系数；
Lengthk—第k条路径的距离；
Nodek—第k条路径的交叉节点数量。


4.如权利要求1～3任一项所述的一种考虑转角约束的Bi-RRT无人艇多艇导航方法，其特征在于，步骤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上新节点ξn'ew的拓展节点向量；Δθ1i、Δθ2j分别为ξnew、ξn'ew...

【专利技术属性】
技术研发人员：谢远龙，吴昊，王书亭，蒋立泉，孟杰，李鹏程，吴天豪，孙浩东，付清晨，严一鸣，
申请(专利权)人：华中科技大学，
类型：发明
国别省市：湖北;42