基于Python的RRT算法实现

本文为博主原创内容，未经博主允许不得转载。尊重他人知识成果，共同建立良好学习分享氛围，谢谢！

一、RRT算法原理及实现效果

  关于RRT算法的原理，博主之前看到的RRT算法原理图解这篇文章讲的很好，通俗易懂，所以若不了解RRT算法的原理，推荐各位看这个了解下。

  本文主要是基于Python实现RRT算法，先来看一下RRT算法实现后的效果图。

二、RRT算法python代码实现

  具体实现代码如下，博主写的可能有些复杂，不过其中大部分代码注释的很详细了，在这里我就不在赘述了。

import matplotlib.pyplot as plt
import random
import math
import time

def main():
    starttime = time.time()#    程序运行时间计时开始
    region1 = region(100,100)#    定义单元区域
    obstacle_area1 = obstacle_area((50,0),(50,80),(60,0),(60,80))#    定义障碍物区域
    tree_point = []  #    存放起点、终点和所有的可用衍生点
    futher = {}  #     存放所有可用衍生点的父节点
    step = 3#    步进系数
    breed_point = 'breed_point is invalid'
    draw_tree_pointx = []#    存放可用衍生点的横坐标
    draw_tree_pointy = []#    存放可用衍生点的纵坐标
    draw_plan_pointx = []#    存放从起点到终点的计划路径点的横坐标
    draw_plan_pointy = []#    存放从起点到终点的计划路径点的纵坐标

    initial_point = point()#    实例化起点对象
    initial_point.set_position(10,10)#    设定起点坐标
    initial_point.set_state(in_obstacle_area(initial_point,obstacle_area1,region1))#    设定起点状态
    if initial_point.state == False:#    若起点在障碍物区域，直接退出
        print("initial point is live in obstacle region , please choose initial point again")
        return

    terminal_point = point()#    实例化终点对象
    terminal_point.set_position(90,50)#    设定终点坐标
    terminal_point.set_state(in_obstacle_area(terminal_point,obstacle_area1,region1))#    设定终点状态
    if terminal_point.state == False:#    若终点在障碍物区域，直接退出
        print("terminal point is live in obstacle region , please choose terminal point again")
        return

    tree_point.append([initial_point.x, initial_point.y])#   把起点加入tree_point集合列表中

    while (breed_point == 'breed_point is invalid') or (math.sqrt((tree_point[-1][0] - terminal_point.x) ** 2 + (tree_point[-1][1] - terminal_point.y) ** 2) > 3):
        radom_point = random_num(region1,obstacle_area1)#   生成有效随机点
        min_point = MinPoint(tree_point, radom_point)#   从tree_point合集列表中选取距离有效随机点最近的点
        breed_point = BreedPoint(min_point, radom_point, step, obstacle_area1,region1)#   在距离有效随机点最近的点的基础上选取有效的衍生点
        if breed_point != 'breed_point is invalid':
            futher[tuple(breed_point.get_position())] = tuple(breed_point.get_futher())#    记录有效衍生点的父节点
            tree_point.append([breed_point.x, breed_point.y])#    将有效衍生点加入到衍生点集合列表
        draw_tree_pointx.append(tree_point[-1][0])#    记录可用衍生点的横坐标
        draw_tree_pointy.append(tree_point[-1][1])#    记录可用衍生点的纵坐标

    futher[tuple(terminal_point.get_position())] = tuple(tree_point[-1])#    将终点的父节点设置为最后加入的tree_point点
    tree_point.append([terminal_point.x, terminal_point.y])#    把终点加入tree_point集合列表中

    current_cheak_point = tuple(terminal_point.get_position())
    while futher[current_cheak_point] != tuple(initial_point.get_position()):#    从终点开始往回寻找父节点，直到找到起点为止
        draw_plan_pointx.append(current_cheak_point[0])#    记录从起点到终点的计划路径点的横坐标
        draw_plan_pointy.append(current_cheak_point[1])#    记录从起点到终点的计划路径点的纵坐标
        current_cheak_point = futher[current_cheak_point]
    draw_plan_pointx.append(current_cheak_point[0])  # 记录父节点为起点的点的横坐标
    draw_plan_pointx.append(initial_point.x)#    记录起点的横坐标
    draw_plan_pointy.append(current_cheak_point[1])  # 记录父节点为终点的点的横坐标
    draw_plan_pointy.append(initial_point.y)#    记录终点的横坐标

    plt.scatter(draw_tree_pointx, draw_tree_pointy, c='green', s=3)#    画出所有的可用衍生点
    plt.plot([obstacle_area1.obstacle_point1[0],obstacle_area1.obstacle_point2[0],obstacle_area1.obstacle_point4[0],obstacle_area1.obstacle_point3[0],obstacle_area1.obstacle_point1[0]],[obstacle_area1.obstacle_point1[1],obstacle_area1.obstacle_point2[1],obstacle_area1.obstacle_point4[1],obstacle_area1.obstacle_point3[1],obstacle_area1.obstacle_point1[1]],color='r', markerfacecolor='blue', marker='o')#    画处障碍物区域
    plt.plot(draw_plan_pointx, draw_plan_pointy, color='r', markerfacecolor='blue', marker='o')#    画出从起点到终点的计划路径点
    plt.axis("equal")
    plt.grid(True)
    plt.title('initial_point' + str(initial_point.get_position()) + ' , terminal_point:' + str(terminal_point.get_position()))
    plt.show()

    endtime = time.time()#    程序运行时间计时结束
    dtime = endtime - starttime#    计算程序运行时长
    print("程序运行时间：%.8s s" % dtime)  #    显示到微秒

def BreedPoint(min_point, radom_point, step,obstacle_area,region):#    在距离有效随机点最近的点的基础上选取有效的衍生点
    New_BreedPoint = point()#    实例化衍生点对象
    if (min_point[0] == radom_point.x):#    若有效随机点和距离其最近的点的横坐标一样
        New_BreedPoint.set_position(min_point[0], step + min_point[1])
    else:
        k = (min_point[1] - radom_point.y) / (min_point[0] - radom_point.x)#    计算有效随机点和距离其最近的点之间的斜率
        New_BreedPoint_x = math.cos(math.atan(k)) * step + min_point[0]
        New_BreedPoint_y = math.sin(math.atan(k)) * step + min_point[1]
        New_BreedPoint.set_position(New_BreedPoint_x, New_BreedPoint_y)
    if in_obstacle_area(New_BreedPoint, obstacle_area,region) is False:#   判断点是否在有效区域内
        return 'breed_point is invalid'
    New_BreedPoint.set_futher(min_point)#   设置衍生点的父节点
    return New_BreedPoint

def random_num(region,obstacle_area):#   生成有效随机点
    radom_point = point()#    实例化随机点对象
    radom_point.set_position(random.randint(0, region.length), random.randint(0, region.width))#   生成随机点坐标
    if in_obstacle_area(radom_point, obstacle_area,region) is False:#   判断随机点是否在无效区域内
        random_num(region,obstacle_area)#   重新生成随机节点
    return radom_point

def in_obstacle_area(check_point,obstacle_area,region):#   判断点是否在有效区域内
    if ((check_point.x >= obstacle_area.obstacle_point1[0]) and (check_point.x <= obstacle_area.obstacle_point3[0]) and (check_point.y <= obstacle_area.obstacle_point2[1]) and (check_point.y >= obstacle_area.obstacle_point1[1])) :#   判断点是否在障碍物区域内
        return False
    elif ((check_point.x >= 0) and (check_point.x <= region.length) and (check_point.y >= 0) and (check_point.y <= region.width)):#   判断点是否在单元区域内
        return True
    else:
        return False

def MinPoint(tree_point, radom_point):#   从tree_point合集列表中选取距离有效随机点最近的点
    Distance = []
    for i in range(0, len(tree_point)):
        Distance.append(math.sqrt((tree_point[i][0] - radom_point.x) ** 2 + (tree_point[i][1] - radom_point.y) ** 2))
    return tree_point[Distance.index(min(Distance))]

class point():
    def set_position(self, x, y):
        self.x = x
        self.y = y

    def set_state(self, state):
        self.state = state

    def set_futher(self, futher):
        self.futher = futher

    def get_position(self):
        return [self.x, self.y]

    def get_state(self):
        return self.state

    def get_futher(self):
        return self.futher

class region():
    def __init__(self,length,width):
        self.length = length
        self.width = width

class obstacle_area():
    def __init__(self,obstacle_point1,obstacle_point2,obstacle_point3,obstacle_point4):
        self.obstacle_point1 = obstacle_point1
        #   左下点
        self.obstacle_point2 = obstacle_point2
        #   左上点
        self.obstacle_point3 = obstacle_point3
        #   右下点
        self.obstacle_point4 = obstacle_point4
        #   右上点

if __name__ == '__main__':
    main()