文|大核有料

编辑|大核有料

如今无人机及其编队在许多领域发挥着越来越重要的作用，例如生活服务，工业运输和抢险救灾等。

路径规划是扩展移动机器人应用范围，提高执行任务复杂程度，实现全自主导航的首要问题，其目的是在任务空间中找到一条从初始点到目标点，无碰撞的最优路径。

目前应用广泛的路径规划算法主要分为两类，分别是图规划算法和空间采样算法，其中图规划算法包括A*算法、Dijkstra算法、人工势场法等。

随着诸多学者的研究改进，图规划算法的实时性和动态适应性逐渐提升，但多数算法仍存在路径质量差、未考虑动力学约束等问题。

空间采样算法中的状态空间采样能够在大面积、高纬度的空间中快速生成路径，且具有概率完备性。

其中快速遍历随机树(Rapidly-exploringrandomtree,RRT)是一种通过在二维或多维空间中单点采样，随机搜索的路径规划方法。

相比于概率地图方法(Probabilisticroadmaps，PRM)，RRT算法采用树状结构，查询效率更高。

为了保证移动机器人的动态可行性，在获取路径后需要对其进行平滑优化。其中，固定尺寸的廊道约束没有考虑到障碍物的影响，可能会导致近似最优路径中靠近障碍物的拐点附近发生碰撞。

针对以上问题，本文提出了一种面向目标的区域采样双向RRT*(bidirectionalregionalsamplingRRT*,BRS-RRT*)路径规划算法。首先，该算法采用改进的双向贪婪策略获得初始解；以Informed-RRT*算法椭圆形子集的思想为启发，沿初始解向外扩展，形成动态采样区域；

在该区域内迭代搜索，利用节点重构的方法添加全局更近采样点，移除冗余采样点，缩短了路径长度；最后采用中间点插值和三次样条相结合的对路径进行平滑处理，从而保证轨迹的无碰撞性和连续性，最后通过MATLAB和ROS仿真实验证明算法的有效性和可靠性。

«——【·相关算法基本原理·】——»

RRT*算法RRT*算法是一种在不完全约束下进行全局路径规划的单向搜索方法，以sstart为起始点构建的随机树在给定步长下向目标点sgoal逐渐扩展，到达目标点附近区域后结束搜索过程，形成初始路径。RRT*算法步骤如下：

Step1：RRT*算法首先在任务空间中以sstart为起点向外搜索，生成随机节点rands。

Step2：在遍历随机树后找到离随机节点srand欧氏距离最近的点snear，从最近节点snear开始在srand方向上扩展给定步长得到新的节点snews，此时snears成为snew的父节点。

Step3：在以snew为圆心，以给定距离R为半径的圆形区域中选择路径代价更小的节点作为snew的新的父节点snew_parent。对新选父节点进行碰撞检测，若是碰到任务空间中的障碍物，则舍去，若没有碰到任务空间中的障碍物，则将该节点snew_parent添加到相应树中。

Step4：将近邻范围内节点的父节点改为snew，若可以降低路径总成本，则断开该节点smin与原父节点的连接，转而与snew相连，完成重新布线。如图1所示，红线为重新布线的结果。

Step5：重复以上过程，使搜索路径实现渐近最优性。

Informed-RRT*算法：在找到初始解之前，Informed-RRT*算法和RRT*算法步骤一致。在获得初始解后，根据当前迭代得到的sstart，sgoal和cbest构建椭圆形子集区域为：

«——【·BRS-RRT*算法·】——»

改进的双向搜索策略Informed-RRT*算法缺乏处理复杂环境的能力，属于单向盲目搜索，搜索效率较低，因此改进算法在双向搜索的基础上改变了扩展方向。

在找到初始解之前，BRS-RRT*算法获取路径的方法与Bi-RRT算法相似，即在双向搜索策略的基础上做出改进，以提高程序运行速度并增强目标导向性。

如图2(a)所示，Bi-RRT算法的随机采样可能会造成双向随机树连接困难，降低了算法收敛速度。

另外，由于两颗随机树均以对方的起点为目标点进行扩展，当存在障碍物时，为了绕过障碍物可能会生成大量冗余节点，降低了路径的连续性。

如图2(b)所示，BRS-RRT*算法中，从起始点sstart延伸的树Ta将终点sgoal作为目标，而从终点延伸的树Tb将最近添加的节点作为目标，以减少连接两棵树所产生的节点个数，从而加快搜索速度。

其中黑线和红线分别表示以sstart和sgoal为起始点采用贪婪扩展的随机树Ta和Tb。

采样约束：Informed-RRT*算法通过创建椭圆形采样区域，提高算法的收敛性。但在狭窄通道及复杂迷宫中，其采样区域面积难以随迭代次数的增加而减小。

为克服以上问题，在采用改进的Bi-RRT算法获得初始解后，BRS-RRT*算法可选择性对其采取基于最小转弯半径的路径修剪，沿修剪路径扩展，并在扩展区域内进行启发式迭代采样，提高采样效率。

为了确定自适应扩展距离，首先根据地图的大小和障碍物占比来调整初始扩展距离distbase。

如式(2)所示，其中L和W表示地图的长度和宽度，k表示初始扩展系数，若环境复杂度较高或通道较窄，可选择较大的k值以获得较小的采样区域。

实验采用二维地图尺寸为500×500m，经过多次实验确定k=6。

BRS-RRT*算法继承了RRT*算法的渐近最优性，因此需要基于抽样迭代原理设计动态展开系数来定义扩展区域。

如式(3)所示，iinit表示获得初始解时的迭代次数，imax表示代码设置的最大迭代次数，动态展开系数n随着当前迭代次数i的增大而减小，同时在反余切函数的限制下，n的取值范围是(0.75,1)。实际扩展距离dist如式(4)所示。

如图3所示，实际扩展区域可以看作由相邻路径节点之间的连线垂直向外扩展长度dist而形成的大小不一的四边形拼接而成。

只要确定这些四边形的顶点位置，就可以明确扩展区域的具体范围。图3中，si表示路径节点，相邻两条线段的单位向量可确定其角平分线ie，平分角为i，s'i和s''i表示相应的扩展顶点，计算公式如下：

节点重构：BRS-RRT*算法的初始解完全基于随机采样点的连接，导致位于轨迹拐点处的部分采样点距离障碍物不够近。

节点重构策略能够生成离障碍物更近，全局更短的样本点，从而缩短轨迹长度。

将初始路径的轨迹点集定义为IPT，重构路径的轨迹点集定义为OPT，节点重构的目标是为了找到全局更短的OPT，其中：

在得到初始解后，算法开启首次优化迭代，令IPT=OPT。定义初始路径轨迹点的累计距离和为Cd，如式(10)所示，Cdi指Cd的第i个元素。

基于起始点和目标点之间的距离进行随机采样，生成两个随机距离ranD1和ranD2。

在优化路径轨迹两点isOP和sj+1OP之间的区域插入随机值α1和α2，其中i满足不等式(13)，j满足不等式(14)，α1和α2由式(15)、(16)、(17)、(18)得到。

经过以上公式计算可获得α1和α2之间的连接路径，从而得到更新的优化路径轨迹点，如式(19)所示：

令更新的IPTm=OPTm，重复上述过程，直到到达算法设置的最大迭代次数。此节点重构方法可以使全局较远的初始路径的采样点被移除，全局更优的采样点添加至路径中。

从下图4可以看出，随着迭代次数的增加，路径长度不断减小，其中绿色虚线表示初始路径，红色实线表示优化路径。

以BRS-RRT*算法生成的路径节点和映射信息作为输入计算出一条初始光滑轨迹，对路径中每个相邻离散点之间连接的直线段进行碰撞检测。

若发生碰撞，则在最靠近碰撞点的节点si与其上一个节点si-1之间插入一个中间节点si。随后对新路径重复以上步骤，直至获得无碰撞路径。

图5中，蓝色虚线为BRS-RRT*算法生成路径，红色实线为平滑处理后的路径。

如图5(a)所示，若无插值节点的添加，平滑处理后的路径有很大概率会与障碍物发生碰撞。

«——【·仿真实验及结果分析·】——»

本文将BRS-RRT*算法的仿真实验结果与RRT*和Informed-RRT*算法在3种不同的二维地图中进行对比，以验证改进算法的有效性。

3种算法均在MATLAB2023a中独立运行50次，并整理计算出各项数据的平均值。地图实验设置参数如表1所示。

如图6所示，map1为简单障碍物地图，用于验证BRS-RRT*算法在常规障碍物环境中的响应速度和路径代价。

由表2数据可得，RRT*算法在运行过程中由于其随机采样的特性会产生大量冗余节点。

Informed-RRT*算法虽然引入启发式椭圆子集从而控制了节点的生长，但是耗时较长。相比之下，BRS-RRT*算法不仅通过双向贪婪搜索降低了路径代价及运行时间，还采用节点重构策略去除了部分冗余节点。

在map1中，BRS-RRT*算法比Informed-RRT*算法的轨迹节点数减少16.97%，路径长度减小8.44%，运行时间降低75%。

map2为狭窄通道地图，用于验证BRS-RRT*算法在狭小走廊障碍物环境中的收敛速度。

如图7所示，单向搜索算法由于其采样的盲目性，导致大量节点聚集于障碍物的一侧。

狭窄通道增大了起点和终点间的路径代价，使得Informed-RRT*算法的椭圆采样空间难以随迭代次数的增加而缩小。

BRS-RRT*算法则能够产生均匀分布的双向采样节点，提高了收敛速度，即使在狭小的走廊中也能获取有效的采样空间。

尽管在map2中，本文算法的节点数和路径长度与Informed-RRT*算法相近，但运行时间降低了76.42%，突显了双向搜索和沿初始路径展开采样的优越性。

map3为集群障碍物地图，用于验证BRS-RRT*算法划定采样范围的策略对节点的聚拢效果及规划效率的影响。由于map3的起点和终点距离较远且障碍物分布密集，Informed-RRT*算法生成的椭圆采样空间较大。

相较于另外两种算法，BRS-RRT*算法产生的节点大多分布于初始路径周围，有效地限制了节点的采样范围。map3中，BRS-RRT*算法比Informed-RRT*算法的路径长度减小7.13%，运行时间降低87.43%。

路径平滑优化分析：虽然BRS-RRT*算法已经通过改进双向随机树的扩展方向和设置采样约束，实现了更具方向性，更高效，代价更小的路径规划结果，但为了确保移动机器人在实际运行过程中的动态可行性，本文采用中间点插值和三次样条相结合的方法对路径进行进一步平滑处理。

如图9(a)所示，蓝色虚线表示BRS-RRT*算法的搜索路径，红色实线表示平滑处理后的路径。经过优化后，轨迹在拐角处更加平滑。

将map2作为测试平台，计算出搜索路径和平滑处理后的路径的一阶导数曲线，结果如图9(b)所示，蓝色虚线表示的优化路径导数曲线出现明显拐点，而红色实线表示的平滑路径导数曲线是二阶连续的，进一步证明了平滑处理的有效性。

ROS仿真实验：本文利用ROS平台中的移动机器人Turtlebot3进行仿真实验，以验证算法的有效性。

首先，在Gazebo仿真环境中创建实验场景，如图10所示。

随后，借助Rviz可视化工具，使用键盘控制Turtlebot3小车在仿真环境中连续移动，通过应用gmapping算法生成并保存相应的二维地图。

分别使用以下三种算法作为Turtlebot3的全局规划器在地图中进行导航。

如图11所示，地图呈现的白色区域为自由空间，黑色区域则代表地图边界和障碍物分布情况，黑色实线为导航轨迹。

通过对图11和表5的结合分析可得，ROS仿真实验与MATLAB二维实验结果一致，即BRS-RRT*算法相较于Informed-RRT*算法能高效地规划出更短的路径，其中路径长度减小19.87%，规划时间降低58.26%。

«——【·结语·】——»

针对 Informed-RRT*算法的随机采样导致目标性差，收敛速度低，易产生冗余节点等局限性等问题，提出了一种面向目标的区域采样双向RRT*算法。首先，利用改进的Bi-RRT算法进行随机采样，获得初始解。

随后，将初始路径膨胀为动态采样区域，结合重选节点的方法在该区域内不断迭代搜索，缩短路径长度，提高算法收敛效率。

最后，采用基于三次样条和中间点插值相结合的方法，使路径更加平滑，提高其动态可行性。

通过MATLAB和ROS仿真实验，在3种二维地图和Gazebo环境中，将BRS-RRT*算法与另外两种RRT*算法进行对比分析，证明了本文算法能够为各类环境下的路径规划问题提供高效，可靠的解决方案。