[发明专利]一种基于RRT算法改进的车辆规划算法

权利要求书

1.一种基于RRT算法改进的车辆规划算法MT-RRT，其特征在于，步骤如下：

步骤一：已知障碍物的数量、分布情况、起始节点与目标节点，判断起始节点到目标节点的距离是否小于步长和是否与障碍物发生碰撞：如果距离小于步长并且没有与周围障碍物发生碰撞，直接连接起始节点与目标节点，此时直接连接起始节点，与目标节点，寻路完成；否则调用计算引导根节点的方法得到一定数目的引导根节点，放入集合中；该方法具体步骤如下：

(1.1)输入起始节点目标节点，地图上障碍物的数量，分布情况；连接起始节点x_init与目标节点x_goal得到直线l，计算直线的斜率与截距；

(1.2)设置一个间断值Δx₁，从x_init开始到x_goal以横坐标每隔Δx₁的距离为单位，开始对直线l上的点进行碰撞检测，碰撞的n个障碍物，分别求出其碰撞中心点(x_m1,x_m2,x_m3...x_mn)，每个碰撞中心分别做直线l的垂线(l₁,l₂,l₃...l_n)，在每一条垂线上设置间断值Δx₂，以Δx₂为间隔进行碰撞检测，得到第一个无碰撞的点，放进引导根节点的集合newRoot中；

步骤二：将起始节点x_init，目标节点x_goal，与new Root中的引导根节点放入树列表TreeList中，在while循环中，调用程序中生成多棵生长树的方法来扩展TreeList中的每一棵树，该方法需要输入步长step，根节点列表TreeList，以及地图有关信息，引导多棵树的生长直到连接成为一棵树；方法的具体步骤如下：

(2.1)计算TreeList中树的个数m，利用for循环，从1到m遍历TreeList中每一棵树；

(2.2)在遍历当前树T_num时，分别遍历m棵树中除当前树T_num以外的其他树中的每一个节点，得到与当前树最近的树的序号T_{_near}以及树T_{_near}中相距最近的节点的T_n；

(2.3)随机生成一个随机样本点，设置一个阈值Bias，使新产生的节点朝向最近的树具有目标偏向性的进行扩展，对新节点进行碰撞检测，无碰撞则加入到树中；

(2.4)判断新节点与最近的树的根节点并且进行碰撞检测，如果距离小于步长，则合并两棵树；将最近的树中的所有节点添加到当前树中，并在树的列表中删除最近的树，退出当前的for循环；

(2.5)在while循环中，计算树的列表TreeList中树的个数是否为1，如果是，则结束循环,根据树中的节点进行寻路，找到一条从起始节点到目标节点的路径；如果不为1，则继续进行下一次迭代，返回步骤(2.1)。

2.根据权利要求1所述的一种基于RRT算法改进的车辆规划算法MT-RRT，其特征在于，步骤(2.3)中，0阈值Bias1。