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

权利要求书

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

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

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

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

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

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

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

所述的步骤S3中，通过随机规律生成新子节点q_new，并判断当前树扩展至新子节点后是否与空间内的障碍物发生碰撞的方法为：

S301：遍历当前树上的每个节点，分别计算每个节点与随机点的空间距离，寻找离随机点最近的节点q_nearest；以相同方式遍历非当前树中的每个节点，寻找离随机点最近的节点q_goal；

S302：生成一个随机数Rand()，若Rand()大于设定选择概率值P，则选择以q_nearest为起点朝q_rand扩展一个步长得到q_new，其中q_rand为随机点；若Rand()小于设定值P，则选择以q_nearest为起点朝q_goal扩展一个步长得到q_new；

S303：检测当前树扩展至q_new之后，q_nearest与q_new的连线是否与环境中的障碍物发生碰撞：采用包围球法实现连线与环境的碰撞检测，将环境中的障碍物用能够完全包围障碍物且半径最小的球体包围住，然后检测连线和包围球之间是否碰撞。

2.根据权利要求1所述的基于STAR算法优化的快速BI_RRT避障轨迹规划方法，其特征在于，所述的步骤S2中，选择其中一棵树并生成随机点的具体方法为：

S201：以相同的几率在两棵结构树中任选一棵；

S202：生成两个随机数，分别作为随机点的横坐标和纵坐标，得到随机点。

3.根据权利要求1中所述的基于STAR算法优化的快速BI_RRT避障轨迹规划方法，其特征在于，所述的步骤S4中，获得邻近节点q_near的方法为：

S401：以q_new为球心，以5～15倍步长R为半径，建立一个球体，将被包围在该球体内的当前树上的所有的点的集合设为Q_near；

S402：对Q_near中的每个点q_near构造边(q_new,q_near)，通过包围球法进行碰撞检测，剔除所有将与环境产生碰撞的边所对应的q_near，剔除后更新Q_near，得到无碰撞点集；

S403：遍历更新后的无碰撞点集，计算每个点与q_new连线(q_new,q_near)的长度，选择长度最短的边(q_new,q_near)用于扩展当前树，得到该最短边对应的q_near。