[发明专利]变概率双向快速搜索随机树改进路径规划算法

说明书

本发明涉及一种变概率双向快速搜索随机树改进路径规划算法，首先，在导入状态空间图时，需要根据车辆体积的设定进行状态空间预处理，将状态空间边缘进行扩展保护，防止节点过于接近状态空间边缘，导致碰撞。其次，采用了一种基于节点环境的变概率目标选取策略加快收敛速度，最后，对生成后的路径进行去弯取直，即以第一个节点为起点与后续节点的连线之间是否有障碍物判断，删除多余节点，进行优化路径，减少小车行驶过程中的拐弯次数和总路径长度。在原始双向RRT算法上实现变概率优化算法，利用目标指向的方式提高搜索速度，降低计算量；同时降低路径长度和节点个数，并确保了可通行性。

技术领域

本发明涉及一种智能车路径规划技术，特别涉及一种变概率双向快速搜索随机树改进路径规划算法。

背景技术

智能车路径规划是指智能车在位姿空间中找到一条初始位姿点到目标位姿点的连续无碰撞路径，同时这条路径还要满足环境约束以及智能车本身运动特点。

针对这种情况，人们提出了RRT(快速搜索随机树)算法，该算法核心是随机搜索方式，能够在高维非凸空间实现路径搜索，由于其采用随机采样的规划方式，不需要预处理，而且搜索速度快，在高维空间速度优势尤其明显，近年来在机器人运动规划领域得到了广泛的应用和研究。但是其本身也存在一些缺陷：(1)采样为随机采样，大量无效采样导致算法收敛速度慢；(2) 度量函数(最近邻算法)在存在复杂约束的情况下可能会很难获得有效解； (3)算法的随机性会导致生成的路径不平滑，直接使用路径时会出现大量转折与无效曲线路径。针对基本RRT算法的上述不足，国内外学者也对该算法进行了不断地改进，以适应不同的应用环境。Kuffner和LaValle提出了 Bi-RRT(双向RRT)，从初始状态和目标状态并行生成两棵树，能够快速实现收敛；随后他们又提出了RRT-connect算法，该算法在节点扩展时采用指向性拓展，其指向性拓展的概率也需要在实际应用中根据情况选取。对于RRT算法的随机性所造成的路径不平滑问题，Fraichard和Scheuer提出了用回旋曲线来做平滑处理，但是回旋曲线求解计算量很大；Lau等人采用了5次贝塞尔曲线，但是没有考虑路径曲率的连续性和机器人的自身特点，计算量也较大。

发明内容

本发明是针对快速搜索随机树算法解决机器人运动规划存在的问题，提出了一种变概率双向快速搜索随机树改进路径规划算法，保证搜索速度下确保通行性。

本发明的技术方案为：一种变概率双向快速搜索随机树改进路径规划算法，具体包括如下步骤：

1)根据运行车辆体积的设定进行状态空间预处理：即扩大障碍物区域，预留出安全距离；

2)对变概率双向RRT算法进行路径生成：双向RRT算法生成路径L，在迭代生长路径生成过程中，加入变概率算法，增加了周边环境的约束条件，每生成一个节点后，采用函数detect(q)检测以此节点为行驶车辆中心，行驶车辆所需面积圆内是否存在遮挡，即面积圆是否与障碍物区域有交集，如有交集，根据交集由均匀概率分布随机获得一个概率值P，概率值P与设定的目标偏向概率值 Pset进行对比，Pset小于10％，P大于Pset，则放弃此节点，重新采用随机函数生成节点；当P小于Pset时，则认可此节点，放入对应集合；

3)优化路径：

3.1)初始化：将已生成路径L节点path赋予Q，Q＝(q₀，q₁，q₂…q_n)，并且将 q₀赋予q_temp，i＝1；

3.2)通过checkPath()函数依次判断q_temp与其后续节点q_i∈Q连接线段之间是否存在障碍物，i＜n，判断执行3.3)；当i＞＝n时，进入3.4)；