1.本发明涉及路径规划领域，尤其一种复杂环境下基于环境感知的快速探索随机树路径规划方法。
背景技术：
2.随着移动机器人在物流、制造业、医疗等领域的应用越来越广泛，路径规划问题成为了机器人控制与导航的重要研究领域之一。路径规划的目的是在包含障碍物的指定区域中，寻找一条无碰撞的可行路径从起点到达终点。
3.在路径规划算法中，基于采样的方法由于其适用性广、易于实现以及无需构建复杂结构等优势而成为研究热点。其中，rrt算法是一种基于树形结构的随机探索算法，可以处理高维空间中的路径规划问题，并且不需要事先建立网格或其他数据结构。然而，由于采样的随机性，rrt算法对环境变化较为敏感，在遇到迷宫、狭窄通道等复杂环境时，其性能显著下降，且可能无法找到最优路径。
4.针对上述问题，许多学者提出了rrt算法的改进版本。例如，rrt-connect将rrt算法扩展为双向搜索，提高了搜索效率；fast-rrt使用了快速采样和剪枝策略来加速路径搜索。在这些改进算法中，rrt*被广泛认为是在搜索过程中找到最优路径的一种有效方法。
5.尽管这些算法取得了一定的成果，但是它们仍然存在一些挑战。首先，这些算法往往对环境状态不敏感，在包含迷宫、狭窄通道、凹形障碍等复杂环境下运行效率低。其次，由于采样的随机性，这些算法生成的路径质量不够理想，在实际应用中需要进行后续调整和优化。
技术实现要素：
6.针对rrt算法存在的复杂环境中运行效率低、难以解决狭窄通道问题以及生成路径质量差等问题，本发明提出一种复杂环境下基于环境感知的快速探索随机树路径规划方法，以解决传统技术存在的缺陷，提高算法在复杂环境中规划运行效率与成功率，使得规划出的路径尽量接近最优。
7.为实现上述目的，本发明是根据以下技术方案实现的：
8.复杂环境下基于环境感知的快速探索随机树路径规划方法，在rrt算法中引入预扩展点和环境感知策略，并提出双向收缩路径优化方法对算法生成的初始路径进行优化，步骤如下：
9.步骤1：初始化参数；
10.所述步骤1的具体过程为：
11.步骤1.1：通过传感器以及slam技术获取地图数据，并建立地图模型map；
12.步骤1.2：设置随机树扩展步长为d
step
；
13.步骤1.3：设置机器人所在位置为起始点x
star
,任务目标位置点为x
goal
；
14.步骤1.4：初始化数组v，用于存储随机树中的可扩展顶点的信息，并具备实时删除
不可再扩展顶点，其中不可再扩展顶点即无预扩展点的树顶点；
15.步骤1.5：将起始点x
start
存入树顶的数组v中，并对x
start
分配3个候选待扩展顶点，其中每个候选待扩展顶点与x
start
间的距离为d
step
,与x
start
连线的夹角均为120
°
，这样可以使搜索树在扩展过程以正六边形的形式扩展，根据蜂巢原理可知，正六边形可以最节省资源地填充整个平面；因此本方法在扩展时以正六边形扩展可以最大程度利用每一个树节点进行空间探索。
16.步骤2：在地图上进行随机采样，若不发生碰撞，将x
new
加入到树顶点集合t中，并设置x
new
的待扩展顶点x
new
candj，其中j为候选待扩展顶点编号(j＝0,1,2)，并使随机树朝采样点扩展，若发生碰撞则转入步骤3，若到达目标点则转入步骤4，否则重复步骤2；
17.所述步骤2的具体过程为：
18.步骤2.1：每次扩展新顶点x
new
后对其预先分配候选待扩展顶点x
new
candj，其中j为候选点编号(j＝0,1,2)，以保证每个顶点的边夹角为120
°
，此外对候选点集为空的顶点失活处理，从顶点数组v中删除；
19.步骤2.2：在地图上进行采样以获得采样点x
rand
；
20.步骤2.3：选择距离采样点x
rand
最近的待扩展顶点xicandj进行随机树扩展,此时xi为x
nearest
点，扩展点为x
new
,并从xi的候选点集中删除xi点对应的待扩展顶点xicandj；
21.步骤2.4：判断x
new
点是否到达任务目标位置点附近，若到达则转入步骤4，否则转入步骤2.5；
22.步骤2.5：判断x
new
与x
nearest
点之间连线是否有障碍物，若无障碍物则判定扩展成功，并将x
new
点加入到树顶点集合t中，否则转入步骤3。
23.步骤3：进行局部有规律的采样以判断碰撞点所处环境，并根据环境类型选择合适的扩展点进行扩展，若到达目标点则转入步骤4，否则返回步骤2；
24.所述步骤3的具体过程为：
25.步骤3.1：对x
nearest
点进行局部采样以获得环境信息；
26.所述步骤3.1的具体过程为：
27.步骤3.1.1：以待扩展顶点x
nearest
点为中心、以扩展步长d
step
为半径，均匀地采样k个点存入采样点点集s；
28.步骤3.1.2：根据采样点点集s的所处位置是否在障碍物区域内分为处于障碍物中的分点集一s
obs
和处于自由区域的分点集二s
free
，选出分点集二s
free
与分点集一s
obs
的边界点存入边界点集s
bdry
，其中边界点集s
bdry
是分点集二s
free
的子集；
29.步骤3.1.3：为保证局部采样能够感知到狭窄通道，已知最小可行驶通道的宽度d
gap
和扩展步长d
step
，为保证局部采样能够采样到通道内部，需使相邻采样点的间距不大于d
gap
，采样点的数量k需要满足公式(1)
[0030][0031]
步骤3.2：局部采样过后根据分点集二s
free
和边界点集s
bdry
的数量关系进行环境分类，将局部环境分为墙壁障碍和通道环境两种情况，当边界点集s
bdry
的顶点数等于2并且分点集二s
free
的顶点数大于2时，判断为墙壁障碍环境，此时直接退出步骤3，并返回步骤2；其
余情况均认定为狭窄通道环境；转入步骤3.3；
[0032]
步骤3.3：当遇到障碍物且判断为狭窄通道环境时，以x
nearest
为圆心，以扩展步长d
step
为半径划分扇形区域，使得扇形区域仅包含连续的分点集二s
free
的点；划分后得到y个扇形区域，分别称为rt(t＝1,2,...,y),选择不包含当前结点的父结点x
parent
点的区域作为待扩展区域，并从中选择无碰撞的分点集二s
free
的点作为x
new
点进行扩展。
[0033]
所述步骤3中通过局部采样进行环境感知，其意义在于：因rrt算法的扩展依据全局采样点的位置而进行扩展，在狭窄通道内全局采样概率较小，继而导致传统rrt算法在面对狭窄通道环境时算法失效；rrv算法需在局部大量采样并通过对采样点进行pca主成分分析才能识别环境类型，算法计算量过大；本方法所提环境感知策略仅需局部简单环形采样即可分辨出所处环境类型，无需进行pca主成分分析计算；故通过引入环境感知策略来将树结点周围环境进行识别区分，进而提高算法在复杂环境下的运行效率。
[0034]
所述步骤3.3中，当扇形区域数量y大于2时，则为多通道岔路情况，当y＝3时，经划分得到三个扇形区域(r1,r2,r3)，因为r1包含x
parent
点，所以从r2和r3区域分别选择两个无碰撞的s
free
点作为x
new
点进行扩展。
[0035]
步骤4：对生成树采用回溯法获得无碰撞路径，采用双向路径点收缩对路径进行优化以提高路径质量。
[0036]
所述步骤4的具体过程为：
[0037]
步骤4.1：使用earrt和剪枝优化策略得到初始优化路径p(x0,x1,...,xn)，其中xi(i＝0,1,...,n)为路径点，i是路径点的顺序；
[0038]
步骤4.2：初始化i＝1；
[0039]
步骤4.3：判断i是否小于n，若小于则将xi放入临时变量x
temp
，将x
i-1
放入用来临时存储前一个路径点的变量x
pre
，将x
i 1
放入用于临时存储后一个路径点的变量x
post
，若不小于则转入步骤4.6；
[0040]
步骤4.4：将当前变量点x
temp
向变量x
post
移动一步；
[0041]
步骤4.5：判断x
temp-x
pre
路径段是否发生碰撞，如果发生碰撞则将x
temp
朝x
pre
方向移动一步，将路径点xi更新为x
temp
，设置i＝i 1然后转入步骤4.3。若无碰撞则转入步骤4.4；
[0042]
步骤4.6：设置i＝n-1；
[0043]
步骤4.7：判断i是否大于0，若大于则将xi放入临时变量x
temp
，将x
i 1
放入所述变量x
pre
，将x
i-1
放入所述变量x
post
中，若不大于则得到优化路径，结束流程；
[0044]
步骤4.8：将当前变量点x
temp
向变量x
post
移动一步；
[0045]
步骤4.9：判断x
temp-x
pre
路径段是否发生碰撞，如果发生碰撞则将x
temp
朝x
pre
方向移动一步，将路径点xi更新为x
temp
，设置i＝i 1然后转入步骤4.7；若无碰撞则转入步骤4.8。
[0046]
所述步骤4中引入双向路径点收缩方法，首先通过剪枝操作来删除冗余路径节点，在此基础上在通过三角形两边之和大于第三边原理对路径点进行移动收缩进而获得更优路径；其意义在于：因基于采样的算法具有随机性，继而生成的最终路径质量较差。为了解决该问题目前已有大量文献提到剪枝优化策略来优化最终路径，但剪枝优化后的路径并非最优，仍有进一步优化的空间，故而提出双向路径点收缩方法来进一步提升最终路径质量。
[0047]
本发明与现有技术相比，具有如下有益效果：
[0048]
本发明提出了一种基于环境感知的快速探索随机树(earrt)路径规划方法，该方
法通过简单局部采样即可判断附近环境类型，无需进行主成分分析(pca)，大大减少算法计算量。同时，本发明还提出了预分配顶点扩展方法、双向收缩优化策略等，可以快速有效地应对迷宫、狭窄通道、凹形陷阱等复杂环境，并优化生成的初解路径。与现有的路径规划算法相比，earrt算法具有以下优点：一是具有较好的环境适应性和鲁棒性，可以在复杂、动态环境中稳定地进行路径规划；二是在不牺牲搜索效率和路径质量的情况下，可以快速生成合理的初解路径；三是方法实现简单，易于推广和应用。
[0049]
本发明的earrt算法有望在移动机器人导航、自主驾驶车辆、无人机飞行路径规划等领域得到广泛应用。
附图说明
[0050]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它附图。
[0051]
图1为本发明的复杂环境下基于环境感知的快速探索随机树路径规划方法的工作原理示意图；
[0052]
图2为局部采样的示意图。
[0053]
图3为进行环境判断的示意图。
[0054]
图4为扩展树遇到岔路时的扩展步骤示意图。
[0055]
图5为在扩展时预分配扩展点的示意图。
[0056]
图6为双向收缩路径优化过程的示意图。
具体实施方式
[0057]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。
[0058]
本发明实施例提出了一种复杂环境下基于环境感知的快速探索随机树路径规划方法，如图1所示，包括以下步骤：
[0059]
步骤1：初始化参数；
[0060]
步骤1的具体过程为：
[0061]
步骤1.1：通过传感器以及slam技术获取地图数据，并建立地图模型map；
[0062]
步骤1.2：设置随机树扩展步长为d
step
；
[0063]
步骤1.3：设置机器人所在位置为起始点x
star
,任务目标位置点为x
goal
；
[0064]
步骤1.4：初始化数组v，用于存储随机树中的可扩展顶点的信息，并具备实时删除不可再扩展顶点；
[0065]
步骤1.5：将起始点x
start
存入树顶的数组v中作为根节点，并对x
start
分配待扩展顶点。
[0066]
所述步骤1.5中，对x
start
分配待扩展顶点是对x
start
分配3个候选待扩展顶点，其中每个待扩展顶点与x
start
间的距离为d
step
,与x
start
连线的夹角均为120
°
。
[0067]
步骤2：在地图上进行随机采样，若不发生碰撞，将x
new
加入到树顶点集合t中，并设置x
new
的待扩展顶点x
new
candj，其中j为候选待扩展顶点编号(j＝0,1,2)，并使随机树朝采样点扩展，若发生碰撞则转入步骤3，若到达目标点则转入步骤4，否则重复步骤2；
[0068]
所述步骤2的具体过程为：
[0069]
步骤2.1：以x
new
为原点计算x
nearest
的角度ang0，并以120
°
为偏向角得到待扩展顶点x
new
candj，如图5所示，若待扩展顶点不与树节点重合则保留该点，否则删除相应待扩展顶点；
[0070]
每次扩展新顶点x
new
后对其预先分配候选待扩展顶点x
new
candj，其中j为候选待扩展顶点编号(j＝0,1,2)，以保证每个顶点的边夹角为120
°
，需要注意的是候选待扩展顶点与树中节点如果发生重合则设置失败，每个顶点都记录对应的候选待扩展顶点x
i.
candj，其中i为树顶点集合t中的顶点编号(i＝0,1,...,n)，此外对候选待扩展顶点点集为空的顶点失活处理，从数组v中删除；
[0071]
步骤2.2：在地图上使用采样器进行采样以获得采样点x
rand
；使得采样点以一定概率采样到目标范围，增加树的生长导向性，例如，采样器有95％的概率在地图随机采样，有5％的概率采样到目标位置区域以增强随机树朝向目标点扩展趋势；
[0072]
步骤2.3：选择距离采样点x
rand
最近的待扩展顶点xicandj进行随机树扩展,此时xi为x
nearest
点，扩展点为x
new
,并从xi的候选点集中删除xi点对应的待扩展顶点xicandj；图5中，因为x1点为失活顶点，已经从数组v中删除，所以x3点被选为x
nearest
，接着从x3点的候选待扩展顶点点集中选择距离x
rand
最近的顶点作为x
new
点，经计算选择x3.cand2作为x
new
点扩展，并从x3的候选待扩展顶点点集中删除x3.cand2。又因为x6.cand2与该点重合，所以需同时从x6的候选待扩展顶点点集中删除x6.cand2。此时的顶点x6的候选待扩展顶点点集为空，故x6成为失活顶点，从数组v中删除x6顶点。
[0073]
步骤2.4：判断x
new
点是否到达任务目标点附近，若到达则转入步骤4，否则转入步骤2.5；
[0074]
步骤2.5：判断x
new
与x
nearest
点之间连线是否有障碍物，若无障碍物则判定扩展成功，并将x
new
点加入到树顶点集合t中，否则转入步骤3。
[0075]
步骤3：进行局部采样以判断碰撞点所处环境，并根据环境类型选择合适的预扩展点进行扩展，若到达目标点则转入步骤4，否则返回步骤2；
[0076]
所述步骤3的具体过程为：
[0077]
步骤3.1：对x
nearest
点进行局部采样以获得环境信息；
[0078]
所述步骤3.1的具体过程为：
[0079]
步骤3.1.1：以待扩展顶点x
nearest
点为中心、以扩展步长d
step
为半径，均匀地采样k个点存入采样点点集s；以k＝16为例，如图2所示；
[0080]
步骤3.1.2：根据采样点点集s的所处位置是否在障碍物区域内分为处于障碍物中的分点集一s
obs
和处于自由区域的分点集二s
free
，具体地，根据采样点点集s的所处位置坐标对应地图中为0还是1来判断是否在障碍物区域内，若为1则判断为障碍物中存入分点集一s
obs
，否则判断为自由空间点存入分点集二s
free
，选出分点集二s
free
与分点集一s
obs
的边界点存入边界点集s
bdry
，其中边界点集s
bdry
是分点集二s
free
的子集，
[0081]
步骤3.1.3：为保证局部采样能够感知到狭窄通道，需要对采样点数量k以及扩展
步长d
step
进一步说明；已知最小可行驶通道的宽度d
gap
和扩展步长d
step
，为保证局部采样能够采样到通道内部，需使相邻采样点的间距不大于d
gap
，采样点的数量k需要满足公式(1)
[0082][0083]
步骤3.2：局部采样过后根据分点集二s
free
和边界点集s
bdry
的数量关系进行环境分类，将局部环境分为墙壁障碍和通道环境两种情况，当边界点集s
bdry
的顶点数等于2并且分点集二s
free
的顶点数大于2时，判断为墙壁障碍环境，此时直接退出步骤3，并返回步骤2；其余情况均认定为狭窄通道环境；转入步骤3.3；
[0084]
步骤3.3：当遇到障碍物且判断为狭窄通道环境时，如图3所示，以x
nearest
为圆心，以扩展步长d
step
为半径划分扇形区域，使得扇形区域仅包含连续的分点集二s
free
的点；划分后得到y个扇形区域，分别称为rt(t＝1,2,...,y),选择不包含当前结点的父结点x
parent
点的区域作为待扩展区域，并从中选择无碰撞的分点集二s
free
的点作为x
new
点进行扩展；
[0085]
步骤3.4:当扇形区域数量y大于2时，则为多通道岔路情况，如图4所示，经划分得到三个扇形区域(r1,r2,r3)，因为r1包含x
parent
点，所以从r2和r3区域分别选择两个无碰撞的s
free
点作为x
new
点进行扩展。
[0086]
步骤4：对生成树采用回溯法获得无碰撞路径，采用双向路径点收缩对路径进行优化以提高路径质量。
[0087]
如图6所示：获得一条可行路径后对路径进行双向收缩优化处理以降低路径质量计算函数c(p)。
[0088]
步骤4.1：从x0移动到x7需要避开一个障碍物，图6中的黑线路径x
0-x
1-x
2-x
3-x
4-x
5-x
6-x7为规划出的原始路径。这条路径存在大量冗余点，采用剪枝策略，在路径无碰撞的基础上，根据三角不等式定理，机器人可以直接从x0点移动到x2点，不必经过x1点，故将x1点认定为冗余点；删除冗余点x1,x3,x5,x6。并将剩余点按顺序连接，得到剪枝优化后的黄色路径x
0-x
2-x
4-x7，称之为σ(x0,x1,...,xn)。
[0089]
步骤4.2：起点x0和终点x7路径点不参与收缩移动，其余点分别进行两次收缩移动。第一次收缩，每个路径点按编号从小到大的顺序，朝编号大的下一路径点移动，并时刻检测是否与前一路径点碰撞，即将碰撞时停止移动，路径点x2朝x4移动，当x
0-x2路径段即将与障碍物碰撞时停止移动。之后路径点x4朝x7移动，在x
2-x4路径段即将与障碍物碰撞时停止移动。当所有路径点完成收缩移动后得到新路径x
0-x
2-x
4-x7，完成第一次收缩。
[0090]
步骤4.3：第二次收缩仅改变收缩顺序，将顺序改为从大到小依次收缩，首先路径点x4朝x2移动，当路径段x
4-x7即将与障碍物发生碰撞时停止移动，之后路径点x2朝x0移动，当路径段x
2-x4即将发生碰撞时停止移动。
[0091]
步骤4.4：当全部路径点完成收缩移动后得到路径x
0-x
2-x
4-x7，此路径即为双向收缩优化策略得到的最终路径。双向收缩优化得到的路径，路径长度更短，接近最优路径。
[0092]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。