一种基于RRT的改进全局路径规划方法

一种基于rrt的改进全局路径规划方法
技术领域
1.本发明涉及无人驾驶路径规划领域，具体涉及一种基于rrt的改进全局路径规划方法。


背景技术：

2.随着计算机技术、信息技术高速发展，自动化、智能化成为未来科技发展的趋势，无人驾驶汽车作为智能化的核心板块之一，不仅代表了高新科技水平，还满足了人们对于汽车技术的迫切需求。无人驾驶领域中的路径规划是指在一定环境模型的基础上，给无人车辆规划一条安全稳定的路径，即给定起始点和目标点，规划一条符合车辆约束的安全行进路径。同时为了保证车辆的行驶安全性和乘坐的舒适性，采用优化和平滑算法进行路径的优化，使无人车辆能够沿着规划路径预期连续性行进。
3.目前，熟知rrt路径规划算法存在以下优点：(1)算法的探索能力强于图搜索算法；(2)不需要对环境进行预先建模；(3)算法具有完备性并且能够快速提供解决方案。同时rrt路径规划算法存在以下缺点：(1)随机性高，搜索具有无向性，容易造成许多不必要的消耗(时间、内存等)；(2)存在规划路径曲折，超过最大转向角度问题。


技术实现要素：

4.本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。
5.为了实现本发明的这些目的和其它优点，提供了一种基于rrt的改进全局路径规划方法，在采样的路径规划rrt算法中，在预定的最高采样次数k内，通过统计连续采样成功次数和连续失败次数对周边静态环境的复杂度进行预判，并基于预判结果自适应选择激进搜索扩展策略或随机搜索扩展策略，以得到改进的全局路径规划算法。
6.优选的是，所述预判的方法被配置为包括：
7.步骤一：设定起始点和目标点，利用激进扩展方式对起始点、目标点进行连接，如连接成功则直接获得初步的规划路径，否则进入步骤二对环境的复杂度进行判断，以对扩展方式进行二次选择；
8.步骤二：当采样次数不大于k时，随机生成一个(0,1)之间的数(random(0,1))，与初始设定的偏置概率p
ss
进行比较，当(random(0,1))小于p
ss
时选择激进扩展策略，反之则为随机扩展策略；
9.步骤三：如果选择的是激进扩展策略，且在激进扩展策略中能找到起始点到目标点的路径，则初步的规划路径结束，否则将部分无碰撞的路径添加到树中，返回步骤二；
10.如果选择的是随机扩展策略，则返回步骤二；
11.其中，在随机扩展策略、激进扩展策略中，在每一次返回步骤二时，都根据当前采样对p
ss
进行修正，直到采样次数超过k的值或返回成功。
12.优选的是，在步骤三中，将p
ss
的初始值设定为0.2，对p
ss
进行修正的是通过公式一
得到对应的修正值p
ss
(j+1)：
[0013][0014]
其中，n
succ
表示连续扩展成功的次数，n
fail
表示连续扩展失败的次数，且n
succ
和n
fail
两者为互斥量，在可扩展和不可扩展的情况下，一个被累积，另一个被清零；
[0015]
l
step
表示当前扩展的路径长度，l
max
表示树中最长可扩展的路径长度，l
step
＝max{l
step
，l
max
}表示当前扩展的路径长度l
step
和树中最长可扩展的路径长度l
max
。
[0016]
优选的是，还包括步骤四，在初始路径规划后，通过三次b样条曲线优化当前路径做平滑处理，以得到最终路径。
[0017]
优选的是，三次b样条曲线拟合公式二为：
[0018][0019]
其中，公式二中的xi为初始规划路径中每个点位，而公式二中的基函数公式为：
[0020][0021]
基于公式二和公式三得拟合曲线点公式四：
[0022][0023]
其中，在公式四中t∈[0,1],其中x(t)为四个控制点根据三次b样条曲线拟合得到的点集。
[0024]
优选的是，在步骤三的激进扩展策略中，若起始点或中途探索点与终点能直接连接，则进入步骤四，若起始点或中途探索点与终点中间有障碍物，则将从起点或中途探索点出发部分到障碍物之间所在的路径进行保留，而障碍物与终点之间的路径进行删除，并返回至步骤二中。
[0025]
本发明至少包括以下有益效果：本发明通过在rrt算法的基础上对所处的周边静态环境进行一个预判，降低不必要的时间和空间消耗，并引入激进的搜索扩展策略加快算法的收敛，其能够在不同的环境中高效快速完成探索环境的问题，同时也能够快速的实现和实施，是一种高效简单容易实施的规划方式。
[0026]
本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。
附图说明
[0027]
图1为本发明基于rrt的改进全局路径规划的流程示意图；
[0028]
图2为本发明在实际规范时的运行效果示意图；图3为在激进扩展方式中，起点或中途探索点与终点直接连接的示意图；
图4为在激进扩展方式中，起点或中途探索点与终点之间具有障碍物的处理示意图。
具体实施方式
[0029]
下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。
[0030]
本发明的目的是为了解决在利用rrt算法进行全局路径规划过程中，规划出的路径曲折，超过最大转向角度并且需要提高规划的整体效率而提出的解决方案。
[0031]
本发明基于rrt的改进全局路径规划方法的整体设计思路是：该发明在主流的基于采样的路径规划rrt算法的基础上进行改进，传统rrt规划算法由于随机性过强、路径过于曲折等问题，在实际的应用情况当中，对于时间的消耗较高，对于探索情况的记录占用内存资源大，不利于后续的控制跟踪模块直接进行参考使用。本发明能够解决传统规划算法耗费计算资源和耗时以及不易实施等问题，同时本发明能够稳定的规划一条路径，解决了当前算法曲折、终端不精确的问题。针对基本场景下的静态环境进行分类处理，灵活变动搜索扩展策略，根据不同的搜索策略，该算法可以在宽阔场地亦或是较为复杂的环境中都能提供较快的解决方案，即可快速规划一条满足条件的安全路径。
[0032]
本发明采用一种检测可行驶区域(即本发明方案中描述的通过多次采样检测可行驶区域，理论上多次采样中连续成功的次数越多，说明环境就约开阔，反之则相反)和激进扩展的方式，通过统计连续采样成功次数和连续失败次数可以判断当前环境是处于宽阔敞亮地带或者是狭窄闭塞的街道，进而灵活的调整自身的扩展策略。最后将所获得的路径点，采用b样条曲线进行路径平滑优化。
[0033]
具体来说，如图1，本发明基于rrt的改进全局路径规划方法包括以下步骤：
[0034]
步骤一：设定目标点，算法会优先尝试利用激进扩展方式进行连接起始点和终点，在较为简单的场景下更为快速高效。如果连接不成功，则对环境进行判断，对扩展方式进行二次选择。
[0035]
步骤二：当采样次数不大于设定的最高采样次数时，随机生成一个(0,1)之间的数(random(0,1))，与初始设定的p
ss
进行比较，当随机生成的概率小于p
ss
时选择激进扩展策略，反之则为随机扩展策略。
[0036]
步骤三：如果选择了激进扩展策略，结果是会找到一条到达目标的路径，至此规划结束(返回成功)或者将部分无碰撞的路径添加到树中(返回失败)。
[0037]
步骤四：不论选择激进扩展策略还是随机扩展策略，都会对偏置概率p
ss
相关影响参数产生影响，修改对应的参数值并根据公式计算出新的p
ss
值。返回步骤二进行执行，直到超过最大采样数或返回成功。
[0038]
步骤五：将返回成功后的路径进行b样条曲线拟合路径点，使路径能够更加平滑，让车辆在跟踪路径时能够进行连续性跟踪，得到如图2的在实际规范时的运行效果示意图，在图2中，因为rrt探索步长是一定的且是直线，故图中的点划线为扩展搜索结果，细实线为利用b样条优化结果，而虚线为实际车辆的行驶结果。
[0039]
步骤六：通过拟合点位发送给路径跟踪控制模块，进行路径跟踪。
[0040]
进一步地，本发明的步骤一到步骤二可以细化为以下内容：
[0041]
为了解决在实际应用中能快速高效的规划出一条高质量安全的路径，需要对环境信息进行预判，自适应的选择搜索扩展策略，而自适应的选择搜索扩展策略的方式如表1的扩展策略所示：
[0042]
选择方式概率(p
ss
)p
ss
≤random(0,1)p
ss
＞random(0,1)扩展方法随机扩展激进扩展
[0043]
表1
[0044]
据表1可知，当随机生成一个(0,1)之间的数大于设定概率p
ss
时，我们将选择随机扩展方式，反之则选择激进扩展方式，在本方案中，对于激进的扩展方式，其可以看作是当前点位与目标点的直接连接，具体以图3为例，在激进的扩展中，以s代表起点或中途探索点，g点表示终点，若可以直接连接，将路径分解后用b样条整体模拟。又如以图4为例，在激进的扩展中，以s代表起点或中途探索点，g点表示终点，若中间有障碍物，则将从起点或中途探索点出发部分到障碍物之间所在的路径(即图4中打钩部分)进行保留，而障碍物与终点之间的路径进行(即图4中打叉部分)删除。
[0045]
更进一步地，本发明步骤三到步骤四可以细化为以下内容：
[0046]
为了实现自适应调整参数，将初始的p
ss
＝0.2；调用激进扩展策略后，会根据搜索树是否扩展更新概率p
ss
。表示为
[0047][0048]
式中：
[0049]
j表示采样次数
[0050]nsucc
表示连续扩展成功的次数；
[0051]nfail
表示连续扩展失败的次数；
[0052]nsucc
和n
fail
两者为互斥量，在可扩展和不可扩展的情况下，一个被累积，另一个被清零；
[0053]
l
step
表示当前扩展的路径长度；
[0054]
l
max
表示树中最长可扩展的路径长度；
[0055]
l
step
＝max{l
step
，l
max
}表示当前扩展的路径长度l
step
和树中最长可扩展的路径长度l
max
。
[0056]
当环境处于开阔敞亮的地方，连续扩展的次数会随之增加，p
ss
会被加大，采用激进扩展的机率更高，但是遇到杂乱的环境时，p
ss
也会随之减小，快速适应环境的变化。该方式不仅可以加快搜索树的生长速度，避免一些无意义的搜索，还可以提高规划轨迹的终端状态精度。
[0057]
在步骤五中，将获得的路径，再进行三次b样条曲线优化当前路径做平滑处理，生成最终路径。由于rrt算法在搜索过程中存在曲折，转折角度过大的情况，同时由于控制点位非线性的特性，故采用样条优化曲线进行拟合路径。
[0058]
根据三次b样条曲线拟合公式：
[0059][0060]
其中基函数为：
[0061][0062]
式(2)中xi为初始规划路径中每个点位。
[0063]
根据(2)和(3)公式得拟合曲线点公式：
[0064][0065]
公式(4)中t∈[0,1],其中x(t)为四个控制点根据三次b样条曲线拟合得到的点集。
[0066]
本发明基于rrt的改进全局路径规划方法的有益效果是，能够在不同的环境中高效快速完成探索环境的问题，同时也能够快速的实现和实施，是一种高效简单容易实施的规划方式，相对于现有技术而言，通过在算法的处理过程中尽可能减少的采样次数，以使其计算时间相比原有的较多采样次数来说，可以得到有效的控制，同时控制算法处理过程中对内存的占有率。
[0067]
以上方案只是一种较佳实例的说明，但并不局限于此。在实施本发明时，可以根据使用者需求进行适当的替换和/或修改。
[0068]
这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。
[0069]
尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。