双向RRT算法原理和实现（python）

一点点碎碎念

       过几天就要出去实习了，心情有一点点小激动，在整理之前的代码时，发现了去年4月份写的RRT代码，那会前前后后花了一个月的时间去学习原理，绘图，手敲代码，中间也报错了很多次，没办法，菜鸟是这样的。不过好在最后运行成功了，当时特别开心，成就感满满。这里也要感谢我的同门，当时我两为了解决避障问题，在纸上涂涂画画，哈哈。

RRT原理

       传统 RRT 算法计算流程可分为两个阶段，第一个阶段是在任务环境中，以算法初始点作为根节点，通过随机采样、逐步迭代地方式，向任务环境中进行扩展，形成一棵随机树，通过判定采样点是否包含目标点或在目标范围内，来判断是否完成随机树的构造，结束采样；第二阶段是在随机树中从目标点依次往回遍历，直到寻回根节点，即可得到一条从初始点到目标点的有效路径。RRT的具体扩展方式如图所示。

        首先将初始起点Xstart作为路径树的根节点，在所要搜索路径的环境中，随机选取一个采样点Xrand，找到路径树中距离采样点Xrand最近的一个点Xnear，若路径树中只有Xstart，则将Xstart当作Xnear。将Xnear沿Xnear—Xrand方向扩展一定的步长d，将扩展的另一端点记作Xnew，判断Xnear到Xnew之间的连线是否会碰到障碍物，如果没有碰到障碍物，将Xnew加入路径树，路径树中产生新的子节点和新的路径。如果Xnear和Xnew之间的连线会碰到障碍物，则舍弃当前的Xnew和xrand，重新进行采样，继续上面的扩展步骤，直到Xnew和终点Xgoal之间没有障碍物，且步长不大于d，则将Xgoal加入路径树，至此路径搜索结束。根据已经生成的路径树，从Xgoal开始向前寻找父亲节点，直到寻找到初始点Xstart，可以得到一条避障的可行路径，RRT算法的路径规划结束。

双向RRT

       传统 RRT 算法采用单一随机树生成的思想，以一个初始点向一个目标点随机扩展，构造简单，收敛速度过慢，因此，为了加快算法收敛速度，基于双向搜索的思想，提出双向随机扩展法（bidirectional RRT， BI-RRT），分别将起始点和目标点作为初始点，构建两棵随机扩展树，一棵利用 RRT 从起始点往目标点生成，另一棵从目标点往起始点生成，如图所示。

        BI-RRT通过两棵随机树的初始化和生长，完成在复杂环境中对路径的搜索。第一棵路径树，以起点Xstart为根结点，朝着第二棵路径树扩展。第二棵路径树以终点Xgoal为根节点，朝着第一棵路径树扩展。当两棵路径树的最新的叶子节点相同或者距离小于步长t，则认为完成路径规划。

代码

       代码部分当时写的时候，我都加了中文注释 ，不过运行的时候还是有一个bug，就是如果随机采样的时候，两个节点距离过近，作为分母的距离可能会为0，会导致报错，不过这个情况很少会出现，我就没有修改，如果运行报错，就再重新运行一次。

import itertools
import random
import matplotlib.pyplot as plt
import numpy as np
import  matplotlib.ticker as ticker
from  math import  sqrt
import math
from matplotlib.patches import Ellipse, Circle


# plt_1 = plt.figure(figsize=(7, 7))           #画布长宽设置
plt.figure(figsize=(8,8),dpi=100)
ax=plt.axes()
plt.axis([50, 550, 50, 550])
ax.xaxis.set_major_locator(ticker.MultipleLocator(50))    #x轴刻度线50
ax.yaxis.set_major_locator(ticker.MultipleLocator(50))    #y轴刻度线50

plt.plot(100, 100, marker='o', color='b',ms=10)  #起点位置
plt.plot(500,500,marker='o',color='b',ms=10)     #终点位置

tree_list1=[[100,100,[90,90]]]      #靠近起点的路径树，包括起点xstart,后面是伪父亲节点
tree_list2=[[500,500,[550,550]]]
t=10                              #xnear和xrand中间选择的固定步长


xhinder=[[290,100,20],
         [430,120,20],
         [180,170,20],
         [330,190,20],
         [530,200,20],
         [90,220,20],
         [420,250,20],
         [470,300,20],
         [90,410,20],
         [170,400,20],
         [270,420,20],
         [400,420,20],
         [520,410,20],
         [230,480,20],
         [150,290,50],
         [400,350,20],
         [350,250,20],
         [370,490,50],
         [270,320,70]]

xstart=[100,100,[90,90]]    #起点坐标  没有父亲节点
xgoal=[500,500,[550,550]]     #终点坐标
for i in xhinder:
    cir1 = Circle(xy=(i[0], i[1]), radius=i[2], alpha=1)
    ax.add_patch(cir1)

#靠近起点这边生成的xrand1
def product_rand1(tree_list1):    #在地图中随机产生一个节点(不存在于这棵路径树中)
    x_width=550      #论文效果图的长
    y_width=550      #论文效果图的宽度
    random_point = list(itertools.product(range(50,x_width, 10), range(50,y_width, 10)))  # 生成所有图上存在的节点，
    #接上面   按论文生成坐标最小50，最大550，距离为10
    xrand1 = random.sample(random_point, 1)  # 从random_point生成的所有地图点中，随机选取一个点
    xrand1 = list(xrand1[0])  # sample函数返回的是一个元组，转换为一个列表
    # tree_list=np.array(tree_list)
    while xrand1 in tree_list1:        #判断新节点是否在路径树中，如果在则重新选择节点
        xrand1=random.sample(random_point,1)
        xrand1=list(xrand1[0])
    return  xrand1


#判断两点之间是否有障碍物
def obstacle(xnew1,xnew2):    #xnew2是xnew1的下一个节点
    A=xnew2[1]-xnew1[1]
    B=xnew1[0]-xnew2[0]
    C=xnew1[0]*(xnew1[1]-xnew2[1]) + xnew1[1]*(xnew2[0]-xnew1[0])
    #判断xnew2到xnew1是否碰到障碍物
    num=0
    for j in range(len(xhinder)):

        d=abs(A*xhinder[j][0]+B*xhinder[j][1]+C)/(sqrt(A**2+B**2))
        ms = xhinder[j][2]+5  #初步假设的障碍物直径
        if d <= ms:  #两点之间连线不一定过障碍物，继续判断
            #先判断xnew1和xnew2是否在圆内
            p_1=sqrt((xnew1[0] - xhinder[j][0]) ** 2 + (xnew1[0] - xhinder[j][1]) ** 2)
            p_2=sqrt((xnew2[0] - xhinder[j][0]) ** 2 + (xnew2[1] - xhinder[j][1]) ** 2)
            if p_1>ms and p_2>ms:
                #继续判断，求垂足
                a = get_foot([xhinder[j][0], xhinder[j][1]], [xhinder[j][0], xhinder[j][1], xnew2[0], xnew2[1]])
                if a[0] >max(xnew1[0], xnew2[0]) or a[0] < min(xnew1[0], xnew2[0]):
                    num += 1
            else: #过障碍物
                pass
        else:         #否则没有过障碍物，是一个合格的可加入到路径树的节点
            num+=1
    return num

#靠近终点这边生成的xrand2
def product_rand2(tree_list2):    #在地图中随机产生一个节点(不存在于这棵路径树中)
    x_width=550      #论文效果图的长
    y_width=550      #论文效果图的宽度
    random_point = list(itertools.product(range(50,x_width, 10), range(50,y_width, 10)))  # 生成所有图上存在的节点，
    #接上面   按论文生成坐标最小50，最大550，距离为10
    xrand2 = random.sample(random_point, 1)  # 从random_point生成的所有地图点中，随机选取一个点
    xrand2 = list(xrand2[0])  # sample函数返回的是一个元组，转换为一个列表
    # tree_list=np.array(tree_list)
    while xrand2 in tree_list2:        #判断新节点是否在路径树中，如果在则重新选择节点
        xrand2=random.sample(random_point,1)
        xrand2=list(xrand2[0])
    return  xrand2

#生成xnear1
def product_near1(tree_list1,xrand1):   #找到路径树中距离新节点xrand距离最短的一个节点xnear
    m=np.inf                         #np.inf是一个无穷大的浮点数，用来过渡
    for i in range(0,len(tree_list1)):    #对整个路径树中的节点进行遍历
        if abs(tree_list1[i][0]-xrand1[0])+abs(tree_list1[i][1]-xrand1[1])<m:
            m=abs(tree_list1[i][0]-xrand1[0])+abs(tree_list1[i][1]-xrand1[1])
            xnear1=[tree_list1[i][0],tree_list1[i][1]]
        #对路径树中的节点和下xrand间距离，采用曼哈顿距离，如果距离比m小，就放入m中
    return xnear1      #返回第一棵路径树中最靠近新节点xrand1的点

#生成xnear2
def product_near2(tree_list2,xrand2):   #找到路径树中距离新节点xrand距离最短的一个节点xnear
    m=np.inf                         #np.inf是一个无穷大的浮点数，用来过渡
    for i in range(0,len(tree_list2)):    #对整个路径树中的节点进行遍历
        if abs(tree_list2[i][0]-xrand2[0])+abs(tree_list2[i][1]-xrand2[1])<m:
            m=abs(tree_list2[i][0]-xrand2[0])+abs(tree_list2[i][1]-xrand2[1])
            xnear2=[tree_list2[i][0],tree_list2[i][1]]
        #对路径树中的节点和下xrand间距离，采用曼哈顿距离，如果距离比m小，就放入m中
    return xnear2      #返回路径树中最靠近新节点xrand的点

#生成xnew1
def decide_direction1(xrand1,xnear1,t):  #为xrand1和xnear1中间设定的步长
    #z_value是斜边长，xrand1和xnear1两点坐标之差的平方相加，取绝对值，取根号
    z_value1=sqrt(abs((xrand1[0]-xnear1[0])**2+(xrand1[1]-xnear1[1])**2))
    cos_value1=(xrand1[0]-xnear1[0])/z_value1    #斜边下的那个角的余弦值
    sin_value1=(xrand1[1]-xnear1[1])/z_value1    #斜边下的那个角的正弦值
    #新节点xnew1的x坐标用正余弦值来算
    xnew1=[xnear1[0]+t*cos_value1,xnear1[1]+t*sin_value1,[xnear1[0],xnear1[1]]]
    return  xnew1     #返回一个成功的新节点xnew1

#生成xnew2
def decide_direction2(xrand2,xnear2,t):  #为xrand2和xnear2中间设定的步长
    #z_value是斜边长，xrand2和xnear2两点坐标之差的平方相加，取绝对值，取根号
    z_value2=sqrt(abs((xrand2[0]-xnear2[0])**2+(xrand2[1]-xnear2[1])**2))
    cos_value=(xrand2[0]-xnear2[0])/z_value2    #斜边下的那个角的余弦值
    sin_value=(xrand2[1]-xnear2[1])/z_value2    #斜边下的那个角的正弦值
    #新节点xnew2的x坐标用正余弦值来算
    xnew2=[xnear2[0]+t*cos_value,xnear2[1]+t*sin_value,[xnear2[0],xnear2[1]]]
    return  xnew2     #返回一个成功的新节点xnew2

#动力学约束  偏航角约束
def cal_ang(point_1, point_2, point_3):
    """
    根据三点坐标计算夹角
    :param point_1: 点1坐标
    :param point_2: 点2坐标
    :param point_3: 点3坐标
    :return: 返回任意角的夹角值，这里只是返回点2的夹角
    """
    a = math.sqrt(
        (point_2[0] - point_3[0]) * (point_2[0] - point_3[0]) + (point_2[1] - point_3[1]) * (point_2[1] - point_3[1]))
    b = math.sqrt(
        (point_1[0] - point_3[0]) * (point_1[0] - point_3[0]) + (point_1[1] - point_3[1]) * (point_1[1] - point_3[1]))
    c = math.sqrt(
        (point_1[0] - point_2[0]) * (point_1[0] - point_2[0]) + (point_1[1] - point_2[1]) * (point_1[1] - point_2[1]))
    # A = math.degrees(math.acos((a * a - b * b - c * c) / (-2 * b * c)))
    B_1 = math.degrees(math.acos((b * b - a * a - c * c) / (-2 * a * c)))
    # C = math.degrees(math.acos((c * c - a * a - b * b) / (-2 * a * b)))
    return B_1

def get_foot(point, line):
    start_x, start_y = line[0], line[1]
    end_x, end_y = line[2], line[3]
    pa_x=point[0]
    pa_y=point[1]

    p_foot = [0, 0]
    if line[0] == line[3]:
        p_foot[0] = line[0]
        p_foot[1] = point[1]
        return p_foot
    k = (end_y - start_y) * 1.0 / (end_x - start_x)
    a = k
    b = -1.0
    c = start_y - k * start_x
    p_foot[0] = (b * b * pa_x - a * b * pa_y - a * c) / (a * a + b * b)
    p_foot[1] = (a * a * pa_y - a * b * pa_x - b * c) / (a * a + b * b)
    return p_foot


num=0
xrand1=product_rand1(tree_list1)
xnear1=product_near1(tree_list1,xrand1)
xnew1=decide_direction1(xrand1,xnear1,t)  #分别生成第一棵树的xrand1 xnear1 xnew1
xnear_father=[]                   #存放xnear的父亲节点
for i in tree_list1:                 #遍历寻找xnear的父亲节点
    if xnear1[0]==i[0] and xnear1[1]==i[1]:
        xnear_father=i[2]
        break
xrand2=product_rand2(tree_list2)
xnear2=product_near2(tree_list2,xrand2)
xnew2=decide_direction2(xrand2,xnear2,t)  #分别生成第二棵树的xrand1 xnear1 xnew1

num_1=obstacle(xnear1,xnew1)
if num_1==len(xhinder):      #没有过障碍物，xnew加入路径树
    tree_list1.append(xnew1)   #加入路径树
    plt.plot(xnew1[0], xnew1[1], marker='o', color='orange', ms=10)
else:   #过障碍物，重新采样点xrand
    xrand1 = product_rand1(tree_list1)

num_2=obstacle(xnear2,xnew2)
if num_2==len(xhinder):      #没有过障碍物，xnew加入路径树
    tree_list2.append(xnew2)   #加入路径树
    plt.plot(xnew2[0], xnew2[1], marker='o', color='yellow', ms=10)
else:   #过障碍物，重新采样点xrand
    xrand2 = product_rand2(tree_list2)


#先判断xnew1到xnew2之间的距离是否小于60
while  (sqrt((xnew2[0]-xnew1[0])**2+(xnew2[1]-xnew1[1])**2))>60:
    xrand1=product_rand1(tree_list1)
    xnear1=product_near1(tree_list1,xrand1)
    xnew1=decide_direction1(xrand1,xnear1,t)  #分别生成第一棵树的xrand1 xnear1 xnew1
    xnear_father = []  # 存放xnear的父亲节点
    calculate_1=0
    for i in tree_list1:  # 遍历寻找xnear的父亲节点
        if xnear1[0] == i[0] and xnear1[1] == i[1]:
            xnear_father = i[2]
            break
    xrand2=product_rand2(tree_list2)
    xnear2=product_near2(tree_list2,xrand2)
    xnew2=decide_direction2(xrand2,xnear2,t)  #分别生成第二棵树的xrand1 xnear1 xnew1
    calculate_2=0
    num_1 = obstacle(xnear1, xnew1)
    if num_1 == len(xhinder):  # 没有过障碍物，xnew加入路径树
        tree_list1.append(xnew1)  # 加入路径树
        plt.plot(xnew1[0], xnew1[1], marker='o', color='orange', ms=10)
    else:  # 过障碍物，重新采样点xrand
        xrand1 = product_rand1(tree_list1)

    num_2=obstacle(xnear2,xnew2)
    if num_2==len(xhinder):      #没有过障碍物，xnew加入路径树
        tree_list2.append(xnew2)   #加入路径树
        plt.plot(xnew2[0], xnew2[1], marker='o', color='yellow', ms=10)
    else:   #过障碍物，重新采样点xrand
        xrand2 = product_rand2(tree_list2)


#先画第一棵树的路径
father_list1=[[xnew1[0],xnew1[1]]]        #第一次是xnew1最后的位置
n=len(tree_list1)-1             #路径树索引值
father_list1.append([xnew1[0],xnew1[1]])
list1 = xnew1[2]  # 存放该节点(最后一个节点)的父亲节点坐标
print('检验第一棵树最后一个节点的父亲节点并输出:',list1)
while [100,100] not in father_list1:
    for i in tree_list1:
        if i[0]==list1[0] and i[1]==list1[1]:
            father_list1.append([i[0],i[1]])     #将父亲节点加入到father_list
            list1=i[2]

list2=[]
list3=[]
sum1=0     #路径的长度
for i in range(len(father_list1)):
    list2.append(father_list1[i][0])      #父亲节点的横坐标
    list3.append(father_list1[i][1])      #父亲节点的纵坐标
    if i < len(father_list1)-1:
        sum1+=sqrt((father_list1[i+1][0]-father_list1[i][0])**2+
                  (father_list1[i+1][1]-father_list1[i+1][1])**2)
print('生成的第一棵树最优路径的长度为:',sum1)   #生成最优路劲的长度
plt.plot(list2,list3,linewidth=3,color='black')

#再画第二棵树的路径
father_list2=[[xnew2[0],xnew2[1]]]        #第一次是xnew2最后的位置
n=len(tree_list2)-1             #路径树索引值
father_list2.append([xnew2[0],xnew2[1]])
list4 = xnew2[2]  # 存放该节点(最后一个节点)的父亲节点坐标
print('检验第二棵树最后一个节点的父亲节点并输出:',list4)
while [500,500] not in father_list2:
    for i in tree_list2:
        if i[0]==list4[0] and i[1]==list4[1]:
            father_list2.append([i[0],i[1]])     #将父亲节点加入到father_list2
            list4=i[2]

list5=[]
list6=[]
sum2=0     #路径的长度
for i in range(len(father_list2)):
    list5.append(father_list2[i][0])      #父亲节点的横坐标
    list6.append(father_list2[i][1])      #父亲节点的纵坐标
    if i < len(father_list2)-1:
        sum2+=sqrt((father_list2[i+1][0]-father_list2[i][0])**2+
                  (father_list2[i+1][1]-father_list2[i+1][1])**2)
print('生成的第二棵树最优路径的长度为:',sum2)   #生成最优路劲的长度
plt.plot(list5,list6,linewidth=3,color='black')

print('总路径长度：',sum1+sum2)
print('生成的路径点',len(tree_list1)+len(tree_list2))
plt.title('BI-RRT')
plt.grid(True)
plt.show()

运行结果 

       下图中，蓝色圆为障碍物，橙色节点和黄色节点，分别为起点和终点两端进行搜索路径时的采样点，分别连接后可以得到两条路径，我把两条路径最后的节点设置了一个间距，方便观察。