[发明专利]一种基于STAR算法优化的快速BI_RRT避障轨迹规划方法

说明书

本发明公开了一种基于STAR算法优化的快速BI_RRT避障轨迹规划方法。方法分为两步进行，第一步主要完成两个结构树快速向对方的方向扩展并产生新的节点；第二步主要完成对生长后的节点集合进行最优路径选择。本发明通过对双向生长树的生长规律进行设计，使得双向生长树的随机性更弱，更快的离开初始区域，利用对生成的路径进行重选父节点以及重新焊接树的操作，使得最终路径趋近于最优路径，提升了避障轨迹规划的效率。

技术领域

本发明涉及避障轨迹规划领域，特别涉及一种基于STAR算法优化的快速BI_RRT避障轨迹规划方法。

背景技术

基于快速扩展随机树(RRT)的路径规划算法作为一种空间内避障轨迹规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标点附近区域，便可以在随机树中找到一条由从初始点到目标点的路径。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，

RRT是作为一种基于随机搜索理念的路径规划方法，在面对复杂障碍物环境时，其搜索效率较低，同时基于RRT算法生成的联通路径往往存在很多折返。对于RRT路径规划算法的弊端有两个主要改进：RRT-Connect与RRT*算法。RRT-Connect算法分别从起始点和目标点各生成一颗RRT树，选择其中一颗RRT树开始生长，当新的生长节点与障碍物发生碰撞时，选择另一棵树开始生长，通过起始点与终止点双向生长RRT树，大大提高了计算出联通路径的效率，但其生成的轨迹与RRT算法一样，其路径代价较大；RRT*算法对RRT算法生成的树进行修剪操作，对于将新生成的节点在一定搜索半径内搜索父节点使其到初始节点的路径最短，对于搜索范围内的其他节点，如将其父节点改为该新节点时，路径更短，则其父节点为该新节点。RRT*算法能通过设置采样点数量对RRT算法生成的树进行修剪操作使得路径更优，其生成联通路径效率与RRT算法类似。

发明内容

RRT-Connect算法作为一种双向搜索的RRT算法，通过起点RRT树与终点RRT树的双向搜索，能以更快的速度形成联通路径，但其联通路径往往都比较长，且存在折返，同时两颗RRT树的生长方向随机无规律，使得其联通速度较慢；RRT*作为一种渐进优化的路径规划算法，根据其节点的增加，能够计算出一条渐近最短的路径，但其计算出初始联通路径的时间代价与RRT算法接近。为兼顾路径计算的时间代价与路径的长度代价，需要结合二者的优点，扬长避短对算法进行改进；

为了实现上述技术目的，本发明的技术方案是，

一种基于STAR算法优化的快速BI_RRT避障轨迹规划方法，包括以下步骤：

S1：以需要进行避障的轨迹起点和目标点分别作为两个不同的结构树的根，以分别建立两个结构树；

S2：随机选择其中一颗树作为当前树，在起点和目标点所在的空间中生成随机点；

S3：根据随机点的空间位置，在两棵树上寻找最近节点，按照随机规律，为当前树生成新子节点，并判断当前树扩展至新子节点后是否与空间内的障碍物发生碰撞；若未发生碰撞，则该新子节点可行并执行S4，否则跳转至S2循环执行；

S4：根据新子节点的位置建立周围点点集，并在周围点点集内选取一邻近节点，使得新子节点与邻近节点的连线不会与环境发生碰撞，且邻近节点到当前树根节点长度最短；

S5：确定连线接入到当前树中的连接方式，即新子节点、邻近节点在树中的先后顺序以及父子关系，扩展当前树；

S6：判断经过扩展后，两棵树是否已经相连，若相连，则避障轨迹规划方法完成；否则返回至S2循环以上步骤，直至两棵树相连。