CN111610786B - 基于改进rrt算法的移动机器人路径规划方法 - Google Patents

Abstract

本发明提供一种室内环境下基于改进RRT算法的移动机器人路径规划方法。首先建立机器人工作环境的高精度栅格地图，并进行地图分割，然后采用邻域扩展策略确定下一个需要搜索的网格，并用随机采样实验确定搜索网格的类型以及有效采样点数量，接着采用邻域最优原则将有效采样点添加到随机树中，并基于记忆策略对网格进行状态更新，最后对得到的规划路径采用剪枝算法和贝塞尔曲线进行平滑处理。地图分割与障碍边缘检测思想的引入，提高了RRT算法在狭窄通道等复杂地图中的采样效率；记忆机制与邻域扩展策略的引入，提高了算法在复杂环境中的规划成功率；按照邻域最优的方式来选择父节点，解决了算法随机性较强、路径非最优等问题。

Description

基于改进RRT算法的移动机器人路径规划方法

技术领域

本发明涉及移动机器人路径规划技术领域，尤其涉及一种基于改进RRT算法的移动机器人路径规划方法。

背景技术

随着科技的发展与进步，智能机器人在人类生活中的应用越来越广泛。路径规划作为智能机器人的关键技术，是其执行各类高级任务、实现自主导航的基础，其目的是在具有障碍物的环境中寻找一条从起始点到目标点的无碰撞路径。目前常用的路径规划算法主要有快速扩展随机树(RRT)、随机路图法(PRM)、A-star、迪杰斯特拉(Dijkstra)、人工势场、模糊逻辑算法、粒子群算法、蚁群算法等。其中，A-star与Dijkstra算法不适合大规模或高精度的网格地图，随着地图尺寸的增大，运行时间急剧增加；人工势场与模糊逻辑算法受环境结构的影响，容易陷于局部最小值，从而导致路径规划失败；粒子群、蚁群等智能仿生算法存在计算量大、实时性差的问题，且算法中相关参数的设置严重影响规划的成功率，因而无法有效适应各种不同类型的环境；RRT与PRM只能保证概率上的完备性，存在多次规划结果不一致、路径非最优、复杂环境中成功率低等缺点。

快速扩展随机树(RRT)是一种基于采样的规划算法，它克服了其他规划算法在高维空间中时间复杂度高的缺陷，此外它还可以结合移动机器人的运动学或动力学约束来进行路径规划。然而，RRT算法的规划成功率受环境结构或障碍物形状的影响：当环境中不存在狭窄通道且所有障碍物都是凸的，算法能以较高的成功率规划出路径；反之，若环境中存在一些凹形障碍物时，比如室内场景中的“墙”，算法极易陷入局部最小值，导致算法的规划成功率较低。同时RRT算法的随机性较强，规划的路径往往远离最优解。尽管RRT*算法通过引入“重布线”过程，能使路径达到渐进最优，但这种最优牺牲了RRT算法快速收敛的性能，不适合用作实时路径规划。

发明内容

针对RRT算法存在的路径非最优、复杂环境中规划成功率低，以及RRT*算法存在的实时性差等问题，本发明提出一种基于改进RRT算法的移动机器人路径规划方法，克服上述现有技术的缺陷，提高算法在复杂环境中规划成功率以及实时性，降低路径的随机性，使得规划出的路径尽量接近最优。

为实现上述技术效果，本发明提出了一种基于改进RRT算法的移动机器人路径规划方法，包括以下步骤：

步骤1：通过移动机器人配备的视觉相机、激光雷达传感器、超声波传感器、红外传感器采集机器人的工作环境信息，然后利用信息融合以及SLAM技术建立移动机器人工作环境的地图Map；

步骤2：定义移动机器人的当前位置点为起始点Q_start，移动机器人的任务地点为目标点Q_goal；

步骤3：按照预设搜索步长S将地图Map分割成数量为m×n的网格，即Map＝{R(i,j)}，R(i,j)表示位于地图Map中第i行、第j列的网格，其中i＝0,1,…,m,j＝0,1,…,n，然后将起始点Q_start所在的网格标记为起始网格R(X_start,Y_start)，将目标点Q_goal所在的网格标记为目标网格R(X_goal,Y_goal)，其中X_start、Y_start分别表示起始网格在地图Map上的横、纵坐标，X_goal、Y_goal分别表示目标网格在地图Map上的横、纵坐标；

式中，r_max表示移动机器人的最大半径；

步骤4：采用邻域扩展策略确定下一个需要搜索的网格R_next；

步骤5：对网格R_next进行随机采样实验，确定出网格R_next中有效采样点的数量num；

步骤6：根据邻域最优原则，为每个有效采样点寻找父结点，将找到父结点的有效采样点添加到随机树T中；

步骤7：采用记忆策略对网格R_next进行状态更新，同时更新网格R_next的路径代价；

步骤8：重复步骤4～步骤7，直至目标点Q_goal被成功添加到随机树T中，然后使用回溯法寻找一条从目标点到起始点的待平滑规划路径；

步骤9：采用剪枝算法删除待平滑规划路径中的冗余结点，然后采用贝塞尔曲线对删除冗余结点后的待平滑规划路径进行平滑处理，得到可用于轨迹跟踪的运动曲线，作为跟踪移动机器人的规划路径。

所述步骤4具体表述为：

步骤4.1：对已搜索网格的集合L_past进行初始化，记为L_past＝{R(X_start,Y_start)}；

步骤4.2：利用公式(2)～公式(3)计算出下一步需要搜索的网格集合L_next；

L_next＝{∪N(x,y)-L_past|R(x,y)∈L_past} (2)

N(x,y)＝{R(i,j)|R(i,j)∈Map,0<|x-i|≤1,0<|y-j|≤1} (3)

式中，R(x,y)表示已搜索的网格，N(x,y)表示已搜索网格R(x,y)的邻域；

步骤4.3：利用公式(4)预估集合L_next中各网格

到起始点Q_start的路径代价

然后选择最小路径代价所对应的网格作为下一步需要搜索的网格R_next；

式中，

表示下一步可能会搜索到的网格，C(R(i,j))表示网格R(i,j)到起始点Q_start的路径代价，/>

表示网格/>

的邻域，/>

分别表示网格/>

在Map中的横、纵坐标。

所述步骤5具体表述为：

步骤5.1：对网格R_next进行N次随机采样实验，利用公式(6)计算出网格R_next中自由空间所占的面积比例P，

P＝M/N (6)

式中，M表示落在自由空间中的样本点个数；

步骤5.2：根据P判断网格R_next的类型，当P＝0时，表示网格R_next为障碍网格；当P＝1时，表示网格R_next为自由网格；当0<P<1时，表示网格R_next为障碍边缘网格；

步骤5.3：利用公式(7)确定网格R_next中的有效采样点的数量num，所述有效采样点为随机采样实验中落在自由空间内的样本点，

式中，ρ表示网格R_next中有效采样点的撒播密度，k表示表示密度加权因子；

步骤5.4：将num个有效采样点表示成集合L_sample＝{Q_h|h＝1,2,…,num}，Q_h表示第h个有效采样点。

所述步骤6具体表述为：

步骤6.1：在有效采样点Q_h所在网格R_next的邻域内，利用公式(8)～公式(9)计算出使Q_h的路径代价最小且不发生碰撞的结点作为Q_h的父结点Q_parent，

C(Q_h)＝min{C(Q_parent)+w_L||Q_h-Q_parent||+w_sα(Q_ancestor,Q_parent,Q_h)},Q_parent∈T∩N(R_next) (8)

式中，C(Q_h)表示结点Q_h到起始点Q_start的路径代价，N(R_next)表示网格R_next的邻域，Q_parent表示落入N(R_next)内的且已经被添加到随机树T中的结点，C(Q_parent)表示结点Q_parent到起始点Q_start的路径代价，w_L表示路径长度的权重，w_s表示路径光滑度的权重，||Q_h-Q_parent||表示有效采样点Q_h到结点Q_parent的欧氏距离，Q_ancestor表示结点Q_parent的父结点，α(Q_ancestor,Q_parent,Q_h)表示路径在结点Q_parent处的转向角，v₁表示由结点Q_ancestor指向结点Q_parent的方向向量，v₂表示由结点Q_parent指向有效采样点Q_h的方向向量；

步骤6.2：将找到父结点的有效采样点添加到随机树T中，如果添加过程中，有效采样点Q_h与Q_parent的连线

与障碍物发生碰撞，则不能将有效采样点Q_h添加到随机树中；

步骤6.3：重复步骤6.1～步骤6.2，计算每个有效采样点的父结点并添加到随机树T中。

所述步骤7具体表述为：根据网格R_next内的num个有效采样点是否均能被成功添加到随机树T中，将网格R_next的状态更新分为三种情况：

1)如果num个有效采样点均能被成功添加到随机树T中，则将网格R_next标记为已搜索网格，并将网格R_next添加到L_past中，然后利用公式(10)更新R_next的路径代价C(R_next)，最后转至步骤4.1继续执行；

C(R_next)＝(∑C(Q_h))/num (10)

2)如果num个有效采样点中没有一个被成功添加到随机树T中，则将网格R_next标记为未搜索网格，并从L_next中移除R_next，最后转至步骤4.3继续执行；

3)如果num个有效采样点中部分的有效采样点被成功添加到随机树T中，则将网格R_next标记为未搜索网格，并从L_next中移除R_next，然后根据成功添加到随机树T中的有效采样点，利用公式(10)更新R_next的路径代价C(R_next)，最后转至步骤4.3继续执行。

本发明的有益效果是：

本发明提出了一种基于改进RRT算法的移动机器人路径规划方法，针对基本RRT算法及RRT*算法存在的问题从以下三个方面进行了改进：

1)把地图分割与障碍边缘检测思想引入到RRT基本框架中，提高了算法在狭窄通道等复杂地图中的采样效率；

2)将记忆机制与邻域扩展策略引入到RRT基本框架中，避免了算法在局部陷阱区域的过度搜索问题，提高了算法在复杂环境中的规划成功率；

3)结合路径长度、路径平滑度等多种性能指标来设计代价函数，并按照邻域最优的方式来选择父节点，解决了RRT算法随机性较强、路径非最优或次优等问题；

实践证明，本发明提供的方法不但适用于简单的结构化环境，而且也适用于复杂的非结构化环境。

附图说明

图1为本发明中的基于改进RRT算法的移动机器人路径规划方法流程图。

图2为本发明中的剪枝算法的流程图。

图3为本发明中的采用贝塞尔曲线平滑路径的示意图。

图4为本发明中的凸障碍环境与非凸障碍环境的样例图，其中图(a)表示凸障碍环境的样例图，图(b)表示非凸障碍环境的样例图。

图5为本发明中的基本RRT算法在凸障碍环境与非凸障碍环境的三次仿真结果图，其中图(a)～图(c)表示在凸障碍环境中的仿真结果图，图(d)～图(f)表示在非凸障碍环境中的仿真结果图。

图6为本发明中的基于改进RRT算法的移动机器人路径规划方法在不同地图中的仿真结果图，其中图(a)～图(d)分别表示4种不同地图的仿真结果图。

图7为基本RRT算法、基本RRT*算法以及本发明中的改进RRT算法的移动机器人路径规划方法在路径长度方面的对比图。

图8为基本RRT算法、基本RRT*算法以及本发明中的改进RRT算法的移动机器人路径规划方法在规划时间方面的对比图。

具体实施方式

下面结合附图和具体实施实例对发明做进一步说明。

如图1所示，一种基于改进RRT算法的移动机器人路径规划方法，包括以下步骤：

步骤1：通过移动机器人配备的视觉相机、激光雷达传感器、超声波传感器、红外传感器采集机器人的工作环境信息，然后利用信息融合以及SLAM(SimultaneousLocalization And Mapping简称SLAM)技术建立移动机器人工作环境的地图Map，其中，视觉相机采集障碍物的图像信息，激光雷达传感器、超声波传感器采集周围物体的位置信息，红外传感器采集移动机器人与障碍物之间的距离信息，图4～图6中，黑色部分为障碍区域，白色部分为自由区域；

步骤2：定义移动机器人的当前位置点为起始点Q_start，移动机器人的任务地点为目标点Q_goal；

步骤3：按照预设搜索步长S将地图Map划分成数量为m×n的网格，即Map＝{R(i,j)}，R(i,j)表示位于地图Map中第i行、第j列的网格，其中i＝0,1,…,m,j＝0,1,…,n，然后将起始点Q_start所在的网格标记为起始网格R(X_start,Y_start)，将目标点Q_goal所在的网格标记为目标网格R(X_goal,Y_goal)，其中X_start、Y_start分别表示起始网格在地图Map上的横、纵坐标，X_goal、Y_goal分别表示目标网格在地图Map上的横、纵坐标；

式中，r_max表示移动机器人的最大半径；

步骤4：采用邻域扩展策略确定下一个需要搜索的网格R_next，具体表述为：

步骤4.1：对已搜索网格的集合L_past进行初始化，记为L_past＝{R(X_start,Y_start)}；

步骤4.2：利用公式(2)～公式(3)计算出下一步需要搜索的网格集合L_next；

L_next＝{∪N(x,y)-L_past|R(x,y)∈L_past} (2)

N(x,y)＝{R(i,j)|R(i,j)∈Map,0<|x-i|≤1,0<|y-j|≤1} (3)

式中，R(x,y)表示已搜索的网格，N(x,y)表示已搜索网格R(x,y)的邻域；

步骤4.3：利用公式(4)预估集合L_next中各网格

到起始点Q_start的路径代价

然后选择最小路径代价所对应的网格作为下一步需要搜索的网格R_next；

式中，

表示下一步可能会搜索到的网格，C(R(i,j))表示网格R(i,j)到起始点Q_start的路径代价，/>

表示网格/>

的邻域，/>

分别表示网格/>

在Map中的横、纵坐标。

步骤5：对网格R_next进行随机采样实验，确定出网格R_next中有效采样点的数量num，具体表述为：

步骤5.1：对网格R_next进行N次随机采样实验，利用公式(6)计算出网格R_next中自由空间所占的面积比例P，

P＝M/N (6)

式中，M表示落在自由空间中的样本点个数；

步骤5.2：根据P判断网格R_next的类型，当P＝0时，表示网格R_next为障碍网格；当P＝1时，表示网格R_next为自由网格；当0<P<1时，表示网格R_next为障碍边缘网格；

步骤5.3：利用公式(7)确定网格R_next中的有效采样点的数量num，所述有效采样点为随机采样实验中落在自由空间内的样本点，

式中，ρ表示网格R_next中有效采样点的撒播密度，k表示表示密度加权因子；

步骤5.4：将num个有效采样点表示成集合L_sample＝{Q_h|h＝1,2,…,num}，Q_h表示第h个有效采样点。

步骤6：根据邻域最优原则，为每个有效采样点寻找父结点，将找到父结点的有效采样点添加到随机树T中，具体表述为：

步骤6.1：在有效采样点Q_h所在网格R_next的邻域内，利用公式(8)～公式(9)计算出使Q_h的路径代价最小且不发生碰撞的结点作为Q_h的父结点Q_parent，

C(Q_h)＝min{C(Q_parent)+w_L||Q_h-Q_parent||+w_sα(Q_ancestor,Q_parent,Q_h)},Q_parent∈T∩N(R_next) (8)

式中，C(Q_h)表示结点Q_h到起始点Q_start的路径代价，N(R_next)表示网格R_next的邻域，Q_parent表示落入N(R_next)内的且已经被添加到随机树T中的结点，C(Q_parent)表示结点Q_parent到起始点Q_start的路径代价，w_L表示路径长度的权重，w_s表示路径光滑度的权重，||Q_h-Q_parent||表示有效采样点Q_h到结点Q_parent的欧氏距离，Q_ancestor表示结点Q_parent的父结点，α(Q_ancestor,Q_parent,Q_h)表示路径在结点Q_parent处的转向角，v₁表示由结点Q_ancestor指向结点Q_parent的方向向量，v₂表示由结点Q_parent指向有效采样点Q_h的方向向量；

步骤6.2：将找到父结点的有效采样点添加到随机树T中，如果添加过程中，有效采样点Q_h与Q_parent的连线

与障碍物发生碰撞，则不能将有效采样点Q_h添加到随机树中；

步骤6.3：重复步骤6.1～步骤6.2，计算每个有效采样点的父结点并添加到随机树T中。

步骤7：采用记忆策略对网格R_next进行状态更新，同时更新网格R_next的路径代价，具体表述为：根据网格R_next内的num个有效采样点是否均能被成功添加到随机树T中，将网格R_next的状态更新分为三种情况：

1)如果num个有效采样点均能被成功添加到随机树T中，则将网格R_next标记为已搜索网格，并将网格R_next添加到L_past中，然后利用公式(10)更新R_next的路径代价C(R_next)，最后转至步骤4.1继续执行；

C(R_next)＝(∑C(Q_h))/num (10)

2)如果num个有效采样点中没有一个被成功添加到随机树T中，则将网格R_next标记为未搜索网格，并从L_next中移除R_next，最后转至步骤4.3继续执行；

3)如果num个有效采样点中部分的有效采样点被成功添加到随机树T中，则将网格R_next标记为未搜索网格，并从L_next中移除R_next，然后根据成功添加到随机树T中的有效采样点，利用公式(10)更新R_next的路径代价C(R_next)，最后转至步骤4.3继续执行。

步骤8：重复步骤4～步骤7，直至目标点Q_go_al被成功添加到随机树T中，然后使用回溯法寻找一条从目标点到起始点的待平滑规划路径；

步骤9：采用剪枝算法删除待平滑规划路径中的冗余结点，然后采用贝塞尔曲线对删除冗余结点后的待平滑规划路径进行平滑处理，得到可用于轨迹跟踪的运动曲线，作为跟踪移动机器人的规划路径。

本发明中采用的改进RRT算法的伪代码如表1所示，所用编程软件为python。

本发明为解决RRT算法生成的路径存在大量不必要冗余转折点问题，使用剪枝算法对步骤8获得的规划路径进行平滑处理，如图2所示，具体包括以下步骤：

步骤9.1：将步骤8获得的折线路径上的所有结点依次标记为P₀,P₁,…,P_s-1，其中s为路径上包括起始点与目标点在内的转折点的数量，其中P₀和P_s-1分别代表起始点与目标点，令缓存路径为path＝{P₀}。

步骤9.2：尝试连接点P_start初始化为起始点P₀，另一尝试连接点记为P_end，这里使用二分法来实现剪枝操作：(a)使用low和high来表示尝试连接点P_end下标的下限与上限，其初始值分别为low＝start+1、high＝s-1；(b)将end初始化为high，判断P_start与P_end的连线是否会穿越障碍物；(c)若穿越障碍物，则令high＝end-1，end＝(low+high)/2，返回步骤(b)；(d)若不穿越障碍物，则令low＝end，end＝(low+high+1)/2，返回步骤(b)；(e)重复上述步骤(b)(c)(d)，当low＝end时，将P_end添加到path中。

步骤9.3：将P_start赋值为P_end，并重复步骤9.2；

步骤9.4：重复步骤9.3，直到将P_s-1添加到path中，然后依次连接path中结点便可得到剪枝后的路径。

经过剪枝操作后的路径依然是折线段，不符合机器人运动学或动力学约束，因此继续使用贝塞尔曲线来对路径进行平滑处理，如图3所示；

为验证本发明提出的基于改进RRT算法的移动机器人路径规划方法的有效性和可行性，在Intel Core i7-6700处理器，主频为3.4Ghz，内存为8GB的硬件环境下，利用python进行模拟仿真，地图为900×900的二维平面，仿真实验中设置搜索步长为S＝10。

表1改进RRT算法伪代码表

RRT算法是一种对环境敏感的概率完备搜索算法，当环境的结构较为复杂时算法的规划成功率较低，特别是环境中存在狭窄通道或非凸障碍物时，算法常常会因为陷入局部最优而导致规划失败，这里根据障碍物的凹凸性，将环境分成凸障碍环境与非凸障碍环境，凸障碍环境是指环境中所有障碍物都凸的，非凸障碍环境则是指环境中存在凹形障碍物，凸障碍环境与非凸障碍环境的样例图如图4所示。

为了验证基本RRT算法在这两种环境中的表现，分别在这两种环境中进行50次仿真，并设置最大迭代次数为10000次，实验结果显示：对于凸障碍环境，算法的搜索成功率为100％，且收敛速度较快，平均迭代次数为1853次；而当环境为非凸环境时，算法的搜索成功率为38％，平均迭代次数为7568次；在图5中随机展示了三组仿真结果，其中图(a)～图(c)表示在凸障碍环境中的仿真结果图，图(d)～图(f)表示在非凸障碍环境中的仿真结果图，图中灰色线表示改进RRT算法所生成的随机树，黑色细线表示随机树中从目标点到起始点的路径，黑色粗线表示经剪枝操作后得到的平滑路径，仿真表明RRT算法在非凸障碍环境中容易在局部最小区域出现过度采样的情况，且每次生成的路径常常偏差很大。

为了验证本发明改进RRT算法适应环境的能力，下面在不同环境中进行仿真实验，已经按照机器人的尺寸对障碍物进行了膨胀处理，实验结果如图6～图8所示，在图6中给出的四种仿真地图中，其中灰色线表示改进RRT算法所生成的随机树，黑色细线表示随机树中从目标点到起始点的路径，黑色粗线表示经剪枝操作后得到的平滑路径；从仿真结果可以看出，本发明给出的改进RRT算法搜索出来的路径是最优的，且能够在一个极其复杂的环境中规划出一条路径。

为了验证本发明改进RRT算法的环境适应能力，下面分别使用基本RRT算法、基本RRT*算法以及本文改进RRT算法在图6中图(a)～图(d)所示的四张地图中进行多次实验仿真，表2统计了这些算法在不同环境中的规划成功率，这里将最大迭代次数设置为40000次，表2中的Map1～Map4分别对应图6中图(a)～图(d)所示的四种不同地图。

表2基本RRT算法、基本RRT*算法与改进RRT算法的实验结果对比分析表

从表2可以看出，无论是简单的凸障碍环境，还是复杂的非凸障碍环境，本发明改进RRT算法具有极强的适应能力。

为比较基本RRT、基本RRT*以及改进RRT算法的稳定性与实时性，接下来在图6中的如图(b)所示的第二种类型地图中进行多次仿真实验，图7和图8分别统计了这三种算法在路径长度和计算时间两个方面的实验数据，仿真结果显示，无论是路径长度还是计算时间，改进RRT算法在多次仿真中都具备稳定性。

Claims (1)

1.一种基于改进RRT算法的移动机器人路径规划方法，其特征在于，包括如下步骤：

步骤1：通过移动机器人配备的视觉相机、激光雷达传感器、超声波传感器、红外传感器采集机器人的工作环境信息，然后利用信息融合以及SLAM技术建立移动机器人工作环境的地图Map；

步骤2：定义移动机器人的当前位置点为起始点Q_start，移动机器人的任务地点为目标点Q_goal；

步骤3：按照预设搜索步长S将地图Map划分成数量为m×n的网格，即Map＝{R(i,j)}，R(i,j)表示位于地图Map中第i行、第j列的网格，其中i＝0,1,…,m,j＝0,1,…,n，然后将起始点Q_start所在的网格标记为起始网格R(X_start,Y_start)，将目标点Q_goal所在的网格标记为目标网格R(X_goal,Y_goal)，其中X_start、Y_start分别表示起始网格在地图Map上的横、纵坐标，X_goal、Y_goal分别表示目标网格在地图Map上的横、纵坐标；

式中，r_max表示移动机器人的最大半径；

步骤4：采用邻域扩展策略确定下一个需要搜索的网格R_next；

步骤5：对网格R_next进行随机采样实验，确定出网格R_next中有效采样点的数量num；

步骤6：根据邻域最优原则，为每个有效采样点寻找父结点，将找到父结点的有效采样点添加到随机树T中；

步骤7：采用记忆策略对网格R_next进行状态更新，同时更新网格R_next的路径代价；

步骤8：重复步骤4～步骤7，直至目标点Q_goal被成功添加到随机树T中，然后使用回溯法寻找一条从目标点到起始点的待平滑规划路径；

步骤9：采用剪枝算法删除待平滑规划路径中的冗余结点，然后采用贝塞尔曲线对删除冗余结点后的待平滑规划路径进行平滑处理，得到可用于轨迹跟踪的运动曲线，作为跟踪移动机器人的规划路径；

所述步骤4具体表述为：

步骤4.1：对已搜索网格的集合L_past进行初始化，记为L_past＝{R(X_start,Y_start)}；

步骤4.2：利用公式(2)～公式(3)计算出下一步需要搜索的网格集合L_next；

L_next＝{∪N(x,y)-L_past|R(x,y)∈L_past} (2)

N(x,y)＝{R(i,j)|R(i,j)∈Map,0<|x-i|≤1,0<|y-j|≤1} (3)

式中，R(x,y)表示已搜索的网格，N(x,y)表示已搜索网格R(x,y)的邻域；

步骤4.3：利用公式(4)预估集合L_next中各网格

到起始点Q_start的路径代价

然后选择最小路径代价所对应的网格作为下一步需要搜索的网格R_next；

式中，

表示下一步可能会搜索到的网格，C(R(i,j))表示网格R(i,j)到起始点Q_start的路径代价，/>

表示网格/>

的邻域，/>

分别表示网格/>

在Map中的横、纵坐标；

所述步骤5具体表述为：

步骤5.1：对网格R_next进行N次随机采样实验，利用公式(6)计算出网格R_next中自由空间所占的面积比例P，

P＝M/N (6)

式中，M表示落在自由空间中的样本点个数；

步骤5.2：根据P判断网格R_next的类型，当P＝0时，表示网格R_next为障碍网格；当P＝1时，表示网格R_next为自由网格；当0<P<1时，表示网格R_next为障碍边缘网格；

步骤5.3：利用公式(7)确定网格R_next中的有效采样点的数量num，所述有效采样点为随机采样实验中落在自由空间内的样本点，

式中，ρ表示网格R_next中有效采样点的撒播密度，k表示表示密度加权因子；

步骤5.4：将num个有效采样点表示成集合L_sample＝{Q_h|h＝1,2,…,num}，Q_h表示第h个有效采样点；

所述步骤6具体表述为：

步骤6.1：在有效采样点Q_h所在网格R_next的邻域内，利用公式(8)～公式(9)计算出使Q_h的路径代价最小且不发生碰撞的结点作为Q_h的父结点Q_parent，

C(Q_h)＝min{C(Q_parent)+w_L||Q_h-Q_parent||+w_sα(Q_ancestor,Q_parent,Q_h)},Q_parent∈T∩N(R_next) (8)

式中，C(Q_h)表示结点Q_h到起始点Q_start的路径代价，N(R_next)表示网格R_next的邻域，Q_parent表示落入N(R_next)内的且已经被添加到随机树T中的结点，C(Q_parent)表示结点Q_parent到起始点Q_start的路径代价，w_L表示路径长度的权重，w_s表示路径光滑度的权重，||Q_h-Q_parent||表示有效采样点Q_h到结点Q_parent的欧氏距离，Q_ancestor表示结点Q_parent的父结点，α(Q_ancestor,Q_parent,Q_h)表示路径在结点Q_parent处的转向角，v₁表示由结点Q_ancestor指向结点Q_parent的方向向量，v₂表示由结点Q_parent指向有效采样点Q_h的方向向量；

步骤6.2：将找到父结点的有效采样点添加到随机树T中，如果添加过程中，有效采样点Q_h与Q_parent的连线

与障碍物发生碰撞，则不能将有效采样点Q_h添加到随机树中；

步骤6.3：重复步骤6.1～步骤6.2，计算每个有效采样点的父结点并添加到随机树T中；

所述步骤7具体表述为：根据网格R_next内的num个有效采样点是否均能被成功添加到随机树T中，将网格R_next的状态更新分为三种情况：

1)如果num个有效采样点均能被成功添加到随机树T中，则将网格R_next标记为已搜索网格，并将网格R_next添加到L_past中，然后利用公式(10)更新R_next的路径代价C(R_next)，最后转至步骤4.1继续执行；

C(R_next)＝(∑C(Q_h))/num (10)

2)如果num个有效采样点中没有一个被成功添加到随机树T中，则将网格R_next标记为未搜索网格，并从L_next中移除R_next，最后转至步骤4.3继续执行；

3)如果num个有效采样点中部分的有效采样点被成功添加到随机树T中，则将网格R_next标记为未搜索网格，并从L_next中移除R_next，然后根据成功添加到随机树T中的有效采样点，利用公式(10)更新R_next的路径代价C(R_next)，最后转至步骤4.3继续执行。

Images