CN110262473B - 一种基于改进Bi-RRT算法的无人艇自动避碰方法 - Google Patents

Abstract

本申请公开了一种基于改进Bi‑RRT算法的无人艇自动避碰方法，包括：步骤1，采用并行算法，以无人艇的当前位置和目标位置分别建立第一搜索树和第二搜索树；步骤2，分别根据预设探索步长、父节点坐标，确定第一延伸子节点坐标和第二延伸子节点坐标；步骤3，当判定第一延伸子节点坐标或第二延伸子节点坐标位于障碍物区域外时，根据第一延伸子节点坐标更新第一搜索树，或根据第二延伸子节点坐标更新第二搜索树执行步骤2；步骤4，当判定第一延伸子节点坐标和第二延伸子节点坐标之间的距离小于或等于预设探索步长时，将路径点之间的连线记作调整航线。通过本申请中的技术方案，有利于使父节点远离障碍物，并向目标方向延伸，增强了避碰方法的实时性。

Description

一种基于改进Bi-RRT算法的无人艇自动避碰方法

技术领域

本申请涉及无人艇自动避碰的技术领域，具体而言，涉及一种基于改进Bi-RRT算法的无人艇自动避碰方法。

背景技术

水面无人艇(unmanned surface vehicle，USV)因其具有自主程度高、航速快、隐身性能好及机动性强等特点被广泛应用在水文探测、海事巡航以及军事作战等领域。近年来随着人工智能技术的再一次兴起及自动驾驶技术的大规模普及，无人艇作为智能技术与传统船舶学科交叉融合的产物，已成为国内外智能装备的研究热点之一。水面无人艇实现智能航行关键技术点之一在于自动避碰，自动避碰技术一定程度上能反映无人艇智能化水平的高低。

而现有技术中，无人艇自动避碰方法主要分为三种，一是船舶领域模型法，二是基于速度障碍原理的避碰方法，三是将其转化为路径规划问题。其中，第一种船舶领域模型法主要思路是：首先根据目标船相对于本船的速度和航向求得两船相遇的最近会遇距离(distance to closet point of approach,DC-PA)和到达最近会遇点的时间(time tocloset point of approach,TCPA)，从而根据两船之间的DC-PA的大小确定是否存在碰撞危险，根据TCPA的大小可以粗略确定其危险程度。该方法大都局限于某些或某类因素如船速、船长、会遇态势等易得的数据，而对于环境及无人艇操纵性能等不确定因素极少考虑，这显然大大降低了其在无人艇避碰技术中的有效性和合理性。

第二种基于速度障碍原理的避碰方法主要思路是：根据无人艇和避障物的对地速度计算其相对速度，分析判断二者的相对速度和相对位置之间的角度关系，得出避碰的可行路径范围，并通过改变无人艇的运动方向来完成对障碍物的避让。该方法需要不断判断与计算无人艇与障碍物之间的方位关系，尽管数学逻辑简单但计算量大，智能化程度较低。

第三种路径规划方法主要思路是：将智能优化方法如遗传算法、鱼群算法运用在避碰路径规划技术上。该方法尽管智能化程度较高但是往往忽略无人艇避碰问题中的实际态势，规划出的路径往往无法直接应用在实际工程问题。

发明内容

本申请的目的在于：提高避碰方法的有效性和合理性，并提高避碰方法的响应速率。

本申请的技术方案是：提供了一种基于改进Bi-RRT算法的无人艇自动避碰方法，该避碰方法适用于，无人艇根据设定航线航行过程中，无人艇获取到障碍物信息后，对设定航线的调整，障碍物信息包括障碍物坐标和障碍物尺寸，该避碰方法包括：步骤1，采用并行计算方法，以无人艇的当前位置建立第一搜索树，将当前位置记作第一父节点坐标，并以无人艇的目标位置建立第二搜索树，将目标位置记作第二父节点坐标，其中，目标位置由障碍物信息和设定航线确定；步骤2，分别根据预设探索步长、第一父节点坐标和第二父节点坐标，确定第一延伸子节点坐标和第二延伸子节点坐标；步骤3，根据障碍物信息和安全半径，确定障碍物区域，当判定第一延伸子节点坐标位于障碍物区域外时，根据第一延伸子节点坐标更新第一搜索树，并将第一延伸子节点坐标记作下一阶段第一搜索树的第一父节点坐标，执行步骤2，当判定第二延伸子节点坐标位于障碍物区域外时，根据第二延伸子节点坐标更新第二搜索树，并将第二延伸子节点坐标记作下一阶段第二搜索树的第二父节点坐标，执行步骤2；步骤4，当判定第一延伸子节点坐标和第二延伸子节点坐标之间的距离小于或等于预设探索步长时，将第一延伸子节点坐标和第二延伸子节点坐标记作路径点，并将路径点之间的连线记作调整航线。

上述任一项技术方案中，进一步地，根据障碍物信息和安全半径，确定障碍物区域，具体包括：步骤31，根据障碍物坐标和障碍物尺寸，计算障碍物中心坐标；步骤32，根据障碍物中心坐标、障碍物尺寸和预设安全距离，确定安全半径；步骤33，根据障碍物中心坐标和安全半径，确定障碍物区域。

上述任一项技术方案中，进一步地，步骤3还包括：步骤34，计算第一父节点坐标与障碍物区域之间的圆切角，并将圆切角的1/2倍记作锥形碰撞角；步骤35，计算第一父节点坐标、第一延伸子节点坐标和障碍物中心坐标之间的夹角；步骤36，根据锥形碰撞角和夹角之间的大小关系，利用障碍物排斥向量、目标点吸引向量与避碰危险度系数，修订第一延伸子节点坐标，并根据修订后的第一延伸子节点坐标更新第一搜索树。

上述任一项技术方案中，进一步地，修订第一延伸子节点坐标具体包括：

当判定夹角小于或等于锥形碰撞角时，根据第一修订公式，修订第一延伸子节点坐标，第一修订公式为：

式中，

为障碍物排斥向量，ω为避碰危险度系数，S为预设探索步长，P₀为障碍物中心坐标，P_p为第一父节点坐标，P_r为随机点，L_s为安全避碰临界距离，

为第一过渡向量，P′_n为修订后的第一延伸子节点坐标；

当判定夹角大于锥形碰撞角时，根据第二修订公式，修订第一延伸子节点坐标，第二修订公式为：

式中，

为目标点吸引向量，P_g为目标位置，

为第二过渡向量，P″_n为修订后的第一延伸子节点坐标。

上述任一项技术方案中，进一步地，预设探索步长为单步探索步长。

本申请的有益效果是：

针对上述问题本申请提出一种基于改进Bi-RRT算法的无人艇自动避碰方法，该方法将速度障碍原理与智能优化方法有机地结合起来，不仅改进了经典的Bi-RRT智能算法提高了算法的实时性，而且与速度障碍原理相结合考虑了无人艇避碰的实际态势，具有一定的工程应用价值。

针对Bi-RRT算法扩展操作中父节点坐标延伸方向位于锥形碰撞区内的情形，提出“避碰危险度系数”与“障碍物排斥向量”，使父节点坐标延伸方向有远离障碍物中心的趋势，减少搜索树延伸失败次数。

针对算法实时性问题提出两棵搜索树并行延伸扩展的方式以及当父节点坐标延伸方向位于锥形碰撞区外时触发“目标吸引向量”，以加速算法的收敛，增强算法的实时性。

附图说明

本申请的上述和/或附加方面的优点在结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是根据本申请的一个实施例的基于改进Bi-RRT算法的无人艇自动避碰方法的示意流程图；

图2是根据本申请的一个实施例的航线仿真示意图；

图3是根据本申请的一个实施例的延伸节点坐标位于锥形碰撞区内的示意图；

图4是根据本申请的一个实施例的延伸节点坐标位于锥形碰撞区外的示意图。

具体实施方式

为了能够更清楚地理解本申请的上述目的、特征和优点，下面结合附图和具体实施方式对本申请进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互结合。

在下面的描述中，阐述了很多具体细节以便于充分理解本申请，但是，本申请还可以采用其他不同于在此描述的其他方式来实施，因此，本申请的保护范围并不受下面公开的具体实施例的限制。

在本申请中引入双向快速搜索随机树(Bidirectional Rapid-explorationRandom Tree，Bi-RRT)算法，并针对Bi-RRT算法扩展操作中，父节点坐标延伸方向位于锥形碰撞区内的情形，引入“避碰危险度系数”与“障碍物排斥向量”，使父节点坐标延伸方向具有远离障碍物中心的趋势。同时，针对算法实时性问题，采用两棵搜索树并行延伸扩展的方式，以及当父节点坐标延伸方向位于锥形碰撞区外时触发“目标吸引向量”，以加速算法的收敛。

如图1所示，本实施例提供了一种基于改进Bi-RRT算法的无人艇自动避碰方法，避碰方法适用于，无人艇根据设定航线航行过程中，无人艇获取到障碍物信息后，对设定航线的调整，障碍物信息包括障碍物坐标和障碍物尺寸，避碰方法包括：

步骤1，采用并行计算方法，以无人艇的当前位置建立第一搜索树T_a，将当前位置记作第一父节点坐标，并以无人艇的目标位置建立第二搜索树T_b，将目标位置记作第二父节点坐标，其中，目标位置由障碍物信息和设定航线确定；

具体地，由于本实施例中采用并行计算方法，即第一搜索树T_a和第二搜索树T_b的延伸方式相同，因此，本实施例中仅以第一搜索树T_a为例进行说明。

如图2所示，在本实施例中定义预定航线OS为二维坐标系第一象限角平分线上从点(0,0)至点(100,100)线段，设定障碍物1的半径(障碍物尺寸或安全半径)为10，其障碍物中心坐标(障碍物坐标)位于预设航线OS上，障碍物2和障碍物3的半径分别为10和15，且障碍物2和障碍物3与预设航线OS之间的距离不超过25，航迹恢复点B(目标位置)的坐标为(65,65)。无人艇沿设定航线OS以速度V航行到当前位置点A(40,40)时，无人艇获取到障碍物信息，并确定目标位置的坐标为点B(65,65)。

步骤2，分别根据预设探索步长、第一父节点坐标和第二父节点坐标，确定第一延伸子节点坐标和第二延伸子节点坐标，优选地，预设探索步长为单步探索步长。

具体地，在步骤2中，首先将二维坐标系第一象限中的四个点(0，0)、(0，100)、(100，100)和(100，0)之间的区域作为位姿空间信息，在笛卡尔坐标系进行二维建模，得到位姿空间在x轴上的范围[x₁,x₂]，在y轴上的范围[y₁,y₂]，在[x₁,x₂]范围内生成随机数x_rand，在[y₁,y₂]范围内生成随机数y_rand，从而构成随机点P_r(x_rand,y_rand)。以第一父节点坐标为起始点，即点A(40，40)，按照设定的单步探索步长向随机点P_r的方向进行延伸，确定第一延伸子节点坐标P_n。

步骤3，根据障碍物信息和安全半径，确定障碍物区域，并判断第一延伸子节点坐标是否位于障碍物区域外。若第一延伸子节点坐标位于障碍物区域内，丢弃该第一延伸子节点坐标，重新执行步骤2。

具体地，如图2所示，在本实施例中，设定障碍物区域为障碍物1的半径所覆盖的区域(障碍物1区域)，但是考虑到在无人艇实际航行的过程中，障碍物区域通常为不规则区域，因此，在本实施例的一个实现方式中，确定障碍物区域的方法为：

步骤31，根据障碍物坐标和障碍物尺寸，采用质心算法，计算障碍物中心坐标，此时，设定障碍物的密度为1；

步骤32，根据障碍物中心坐标、障碍物尺寸和预设安全距离，确定安全半径，其中，在确定安全半径时，根据障碍物尺寸计算障碍物的边缘点与障碍物中心坐标之间距离的最大值，再将该最大值与预设安全距离求和，得到安全半径，其中，预设安全距离可根据实际情况进行调整；

步骤33，根据障碍物中心坐标和安全半径，确定障碍物区域，即以障碍物中心坐标为圆点，安全半径为半径，确定圆形障碍物区域。

优选地，判断第一延伸子节点坐标是否位于障碍物区域外的方法为：计算第一延伸子节点坐标P_n与障碍物中心坐标P₀之间的欧几里得距离，若其欧几里得距离小于或等于障碍物1的半径，则认为第一延伸子节点坐标P_n位于障碍物1区域内，否则，认为第一延伸子节点坐标P_n位于障碍物1区域外。

在本实施例中，确定障碍物区域(障碍物1区域)后，当判定第一延伸子节点坐标位于障碍物区域外时，根据第一延伸子节点坐标更新第一搜索树，并将第一延伸子节点坐标记作下一阶段第一搜索树的第一父节点坐标，执行步骤2，重新确定下一阶段的第一延伸子节点坐标。

在本实施例的另一种实现方式中，判定第一延伸子节点坐标P_n位于障碍物1区域外后，更新第一搜索树之前，该避碰方法还包括：

步骤34，计算第一父节点坐标与障碍物区域之间的圆切角，并将圆切角的1/2倍记作锥形碰撞角；

具体地，如图3所示，以第一父节点坐标P_p为顶点，做障碍物1区域的两条切线，切点分别为E和F，E、P_p和F三点之间的连线构成锥形碰撞区，计算∠EP_pF的大小，即圆切角，将圆切角∠EP_pF的1/2倍记作锥形碰撞角c，即∠EP_pP₀或∠FP_pP₀。

步骤35，计算第一父节点坐标、第一延伸子节点坐标和障碍物中心坐标之间的夹角d，即∠P_nP_pP₀；

步骤36，根据锥形碰撞角和夹角之间的大小关系，利用障碍物排斥向量、目标点吸引向量与避碰危险度系数，修订第一延伸子节点坐标，并根据修订后的第一延伸子节点坐标更新第一搜索树。

优选地，修订第一延伸子节点坐标具体包括：

步骤361，当判定夹角d小于或等于锥形碰撞角c时，即第一延伸子节点坐标P_n位于锥形碰撞区内，如图3所示，根据第一修订公式，修订第一延伸子节点坐标，得到修订后的第一延伸子节点坐标P′_n，第一修订公式为：

式中，

为障碍物排斥向量，ω为避碰危险度系数，S为预设探索步长，P₀为障碍物中心坐标(安全半径)，P_p为第一父节点坐标，P_r为随机点，L_s为安全避碰临界距离，

为第一过渡向量，P′_n为修订后的第一延伸子节点坐标；

步骤362，当判定夹角d大于锥形碰撞角c时，即第一延伸子节点坐标P_n位于锥形碰撞区外，如图4所示，根据第二修订公式，修订第一延伸子节点坐标，得到修订后的第一延伸子节点坐标P″_n，第二修订公式为：

式中，

为目标点吸引向量，P_g为目标位置，

为第二过渡向量，P″_n为修订后的第一延伸子节点坐标。

在本实施例中，当判定第二延伸子节点坐标位于障碍物区域外时，根据第二延伸子节点坐标更新第二搜索树，并将第二延伸子节点坐标记作下一阶段第二搜索树的第二父节点坐标，执行步骤2。

具体地，在并行计算方法中，对第二延伸子节点坐标的修订与上述第一延伸子节点坐标的修订方法相同，具体为：

当判定夹角小于或等于锥形碰撞角时，根据第三修订公式，修订第二延伸子节点坐标，第三修订公式为：

式中，

为障碍物排斥向量，ω为避碰危险度系数，S为预设探索步长，P₀为障碍物中心坐标(安全半径)，P_p为第一父节点坐标，P_r为随机点，L_s为安全避碰临界距离，

为第一过渡向量，P′_n为修订后的第一延伸子节点坐标；

当判定夹角大于锥形碰撞角时，根据第四修订公式，修订第二延伸子节点坐标，第四修订公式为：

式中，

为起始点吸引向量，P_g为当前位置，

为第四过渡向量，P″_n为修订后的第二延伸子节点坐标。

步骤4，根据上述步骤2至3中的方法对第一搜索树T_a和第二搜索树T_b进行延伸扩展，并判断第一延伸子节点坐标和第二延伸子节点坐标之间的距离(如欧几里得距离)是否小于或等于预设探索步长(单步探索步长)。当判定第一延伸子节点坐标和第二延伸子节点坐标之间的距离小于或等于预设探索步长时，可以认定第一搜索树T_a和第二搜索树T_b相遇，将第一延伸子节点坐标和第二延伸子节点坐标记作路径点，并将路径点之间的连线记作调整航线，依据调整航线对障碍物1区域进行避碰。

采用本实施例中的方法进行仿真实验，航线仿真结果如图2所示，无人艇将沿“+”线中的路径点，由当前位置点A运动至目标位置点B，其仿真数据如表1所示。

表1

由仿真结果可知，无人艇按照改进Bi-RRT算法规划出的避碰路径点航行至航迹恢复点的路径短，路径转向点少，路径平滑。仿真实验结果表明，本发明经过改进的Bi-RRT算法效果较优，能够直接生成无人艇避碰的路径，验证了该算法的有效性和较优性。

以上结合附图详细说明了本申请的技术方案，本申请提出了一种基于改进Bi-RRT算法的无人艇自动避碰方法，包括：步骤1，采用并行计算方法，以无人艇的当前位置和目标位置分别建立第一搜索树和第二搜索树；步骤2，分别根据预设探索步长、父节点坐标，确定第一延伸子节点坐标和第二延伸子节点坐标；步骤3，当判定第一延伸子节点坐标或第二延伸子节点坐标位于障碍物区域外时，根据第一延伸子节点坐标更新第一搜索树，或根据第二延伸子节点坐标更新第二搜索树执行步骤2；步骤4，当判定第一延伸子节点坐标和第二延伸子节点坐标之间的距离小于或等于预设探索步长时，将路径点之间的连线记作调整航线。通过本申请中的技术方案，有利于使父节点远离障碍物，并向目标方向延伸，增强了避碰方法的实时性。

本申请中的步骤可根据实际需求进行顺序调整、合并和删减。

本申请装置中的单元可根据实际需求进行合并、划分和删减。

尽管参考附图详地公开了本申请，但应理解的是，这些描述仅仅是示例性的，并非用来限制本申请的应用。本申请的保护范围由附加权利要求限定，并可包括在不脱离本申请保护范围和精神的情况下针对发明所作的各种变型、改型及等效方案。

Claims (4)

1.一种基于改进Bi-RRT算法的无人艇自动避碰方法，其特征在于，所述避碰方法适用于，无人艇根据设定航线航行过程中，所述无人艇获取到障碍物信息后，对所述设定航线的调整，所述障碍物信息包括障碍物坐标和障碍物尺寸，所述避碰方法包括：

步骤1，采用并行计算方法，以所述无人艇的当前位置建立第一搜索树，将所述当前位置记作第一父节点坐标，并以所述无人艇的目标位置建立第二搜索树，将所述目标位置记作第二父节点坐标，其中，所述目标位置由所述障碍物信息和所述设定航线确定；

步骤2，分别根据预设探索步长、所述第一父节点坐标和所述第二父节点坐标，确定第一延伸子节点坐标和第二延伸子节点坐标；

步骤3，根据所述障碍物信息和安全半径，确定障碍物区域，当判定所述第一延伸子节点坐标位于所述障碍物区域外时，根据所述第一延伸子节点坐标更新所述第一搜索树，并将所述第一延伸子节点坐标记作下一阶段所述第一搜索树的第一父节点坐标，执行步骤2，

当判定所述第二延伸子节点坐标位于所述障碍物区域外时，根据所述第二延伸子节点坐标更新所述第二搜索树，并将所述第二延伸子节点坐标记作下一阶段所述第二搜索树的第二父节点坐标，执行步骤2，其中，所述步骤3还包括：

步骤34，计算所述第一父节点坐标与所述障碍物区域之间的圆切角，并将所述圆切角的1/2倍记作锥形碰撞角；

步骤35，计算所述第一父节点坐标、第一延伸子节点坐标和所述障碍物中心坐标之间的夹角；

步骤36，根据所述锥形碰撞角和所述夹角之间的大小关系，利用障碍物排斥向量、目标点吸引向量与避碰危险度系数，修订所述第一延伸子节点坐标，并根据修订后的所述第一延伸子节点坐标更新所述第一搜索树；

步骤4，当判定所述第一延伸子节点坐标和所述第二延伸子节点坐标之间的距离小于或等于所述预设探索步长时，将所述第一延伸子节点坐标和所述第二延伸子节点坐标记作路径点，并将所述路径点之间的连线记作调整航线。

2.如权利要求1所述的基于改进Bi-RRT算法的无人艇自动避碰方法，其特征在于，根据所述障碍物信息和安全半径，确定障碍物区域，具体包括：

步骤31，根据所述障碍物坐标和所述障碍物尺寸，计算障碍物中心坐标；

步骤32，根据所述障碍物中心坐标、所述障碍物尺寸和预设安全距离，确定所述安全半径；

步骤33，根据所述障碍物中心坐标和所述安全半径，确定所述障碍物区域。

3.如权利要求1所述的基于改进Bi-RRT算法的无人艇自动避碰方法，其特征在于，修订所述第一延伸子节点坐标具体包括：

当判定所述夹角小于或等于所述锥形碰撞角时，根据第一修订公式，修订所述第一延伸子节点坐标，所述第一修订公式为：

式中，

为所述障碍物排斥向量，ω为所述避碰危险度系数，S为所述预设探索步长，P₀为所述障碍物中心坐标，P_p为所述第一父节点坐标，P_r为随机点，L_s为安全避碰临界距离，

为第一过渡向量，P′_n为修订后的所述第一延伸子节点坐标；

当判定所述夹角大于所述锥形碰撞角时，根据第二修订公式，修订所述第一延伸子节点坐标，所述第二修订公式为：

式中，

为所述目标点吸引向量，P_g为所述目标位置，

为第二过渡向量，P″_n为修订后的所述第一延伸子节点坐标。

4.如权利要求1所述的基于改进Bi-RRT算法的无人艇自动避碰方法，其特征在于，所述预设探索步长为单步探索步长。

Images