快速拓展随机树路径规划（RRT) python

快速拓展随机树路径规划（RRT) python

快速拓展随机树路径规划（RRT)基本方法： 1.创建节点类，属性有xy坐标值和父节点； 2.将起点加入节点列表中； 3.在地图中随机产生一个点， 遍历节点列表中每个节点，计算与该随机点的距离， 找到与该随机节点a距离最短的节点b， 计算两节点的方向夹角 以节点b(列表中的节点)为起点，沿夹角方向生长单位长度，并在终点生成一个新节点 将该节点加入节点列表中 if 新节点距离终点距离小于给定值，将终点加入节点列表，路径找到 else 重复上述循环遍历

下面是引用别人的python代码：

import numpy as np import matplotlib.pyplot as plt import random import math import copy show_animation = True class Node(object): """ RRT Node """ def __init__(self, x, y): self.x = x self.y = y self.parent = None class RRT(object): """ Class for RRT Planning """ def __init__(self, start, goal, obstacle_list, rand_area): """ Setting Parameter start:Start Position [x,y] goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:random sampling Area [min,max] """ self.start = Node(start[0], start[1]) self.end = Node(goal[0], goal[1]) self.min_rand = rand_area[0] self.max_rand = rand_area[1] self.expandDis = 1.0 self.goalSampleRate = 0.05 # 选择终点的概率是0.05 self.maxIter = 500 self.obstacleList = obstacle_list self.nodeList = [self.start] def random_node(self): node_x = random.uniform(self.min_rand, self.max_rand) node_y = random.uniform(self.min_rand, self.max_rand) node = [node_x, node_y] return node def get_nearest_list_index(self,node_list, rnd): """ :param node_list: :param rnd: :return: """ d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list] min_index = d_list.index(min(d_list)) return min_index def collision_check(self,new_node, obstacle_list): a = 1 for (ox, oy, size) in obstacle_list: dx = ox - new_node.x dy = oy - new_node.y d = math.sqrt(dx * dx + dy * dy) if d <= size: a = 0 # collision return a # safe def planning(self): """ Path planning animation: flag for animation on or off """ while True: # Random Sampling if random.random() > self.goalSampleRate: rnd = self.random_node() else: rnd = [self.end.x, self.end.y] # Find nearest node min_index = self.get_nearest_list_index(self.nodeList, rnd) # print(min_index) # expand tree nearest_node = self.nodeList[min_index] # 返回弧度制 theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x) new_node = copy.deepcopy(nearest_node) new_node.x += self.expandDis * math.cos(theta) new_node.y += self.expandDis * math.sin(theta) new_node.parent = min_index if not self.collision_check(new_node, self.obstacleList): continue self.nodeList.append(new_node) # check goal dx = new_node.x - self.end.x dy = new_node.y - self.end.y d = math.sqrt(dx * dx + dy * dy) if d <= self.expandDis: print("Goal!!") break if True: self.draw_graph(rnd) path = [[self.end.x, self.end.y]] last_index = len(self.nodeList) - 1 while self.nodeList[last_index].parent is not None: node = self.nodeList[last_index] path.append([node.x, node.y]) last_index = node.parent path.append([self.start.x, self.start.y]) return path def draw_graph(self, rnd=None): """ Draw Graph """ print('aaa') plt.clf() if rnd is not None: plt.plot(rnd[0], rnd[1], "^g") for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], "-g") for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, "sk", ms=10*size) plt.plot(self.start.x, self.start.y, "^r") plt.plot(self.end.x, self.end.y, "^b") plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.grid(True) plt.pause(0.01) def draw_static(self, path): plt.clf() for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], "-g") for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, "sk", ms=10*size) plt.plot(self.start.x, self.start.y, "^r") plt.plot(self.end.x, self.end.y, "^b") plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.plot([data[0] for data in path], [data[1] for data in path], '-r') plt.grid(True) plt.show() def main(): print("start RRT path planning") obstacle_list = [ (5, 1, 2), (3, 6, 3), (3, 8, 2), (3, 5, 2), (6, 5, 2)] # Set Initial parameters rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list) path = rrt.planning() print(path) # Draw final path if show_animation: plt.close() rrt.draw_static(path) if __name__ == '__main__': main()

对程序进行分步说明：

class Node(object): def __init__(self, x, y): self.x = x self.y = y self.parent = None

创建节点类，属性有xy坐标值和父节点；

class RRT(object): """ Class for RRT Planning """ def __init__(self, start, goal, obstacle_list, rand_area): """ Setting Parameter start:Start Position [x,y] goal:Goal Position [x,y] obstacleList:obstacle Positions [[x,y,size],...] randArea:random sampling Area [min,max] """ self.start = Node(start[0], start[1]) self.end = Node(goal[0], goal[1]) self.min_rand = rand_area[0] self.max_rand = rand_area[1] self.expandDis = 1.0 self.goalSampleRate = 0.05 # 选择终点的概率是0.05 self.maxIter = 500 self.obstacleList = obstacle_list self.nodeList = [self.start]

定义RRT类的初始属性，

def random_node(self): node_x = random.uniform(self.min_rand, self.max_rand) node_y = random.uniform(self.min_rand, self.max_rand) node = [node_x, node_y] return node

在0-1内产生一个随机的节点坐标

def get_nearest_list_index(self,node_list, rnd): """ :param node_list: :param rnd: :return: """ d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list] min_index = d_list.index(min(d_list)) return min_index

遍历nodelist每个点，找到距离随机点最近的节点，返回节点在该列表中的指针

def collision_check(self,new_node, obstacle_list): a = 1 for (ox, oy, size) in obstacle_list: dx = ox - new_node.x dy = oy - new_node.y d = math.sqrt(dx * dx + dy * dy) if d <= size: a = 0 # collision return a # safe

检测新节点是否在障碍区域内

def planning(self): """ Path planning animation: flag for animation on or off """ while True: # Random Sampling if random.random() > self.goalSampleRate: rnd = self.random_node() else: rnd = [self.end.x, self.end.y]

设置随机点有goalSampleRate = 0.05概率选到终点

min_index = self.get_nearest_list_index(self.nodeList, rnd) # print(min_index) # expand tree nearest_node = self.nodeList[min_index] # 返回弧度制 theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x) new_node = copy.deepcopy(nearest_node) new_node.x += self.expandDis * math.cos(theta) new_node.y += self.expandDis * math.sin(theta) new_node.parent = min_index if not self.collision_check(new_node, self.obstacleList): continue self.nodeList.append(new_node) # check goal dx = new_node.x - self.end.x dy = new_node.y - self.end.y d = math.sqrt(dx * dx + dy * dy) if d <= self.expandDis: print("Goal!!") break if True: self.draw_graph(rnd)

找到与该随机节点a距离最短的节点b， 计算两节点的方向夹角 以节点b(列表中的节点)为起点，沿夹角方向生长单位长度，并在终点生成一个新节点 将该节点加入节点列表中 新节点距离终点距离小于给定值，将终点加入节点列表，路径找到

path = [[self.end.x, self.end.y]] last_index = len(self.nodeList) - 1 while self.nodeList[last_index].parent is not None: node = self.nodeList[last_index] path.append([node.x, node.y]) last_index = node.parent path.append([self.start.x, self.start.y]) return path

last_index为节点列表中除终点以外最后一个点的指针， 如果除终点以外最后一个点的父节点不是None： path列表加入这个节点，再将指针指向其父节点，其父节点的父节点还不为None,则继续循环。 最后返回path列表就为起点到终点的路径

def draw_graph(self, rnd=None): """ Draw Graph """ print('aaa') plt.clf() if rnd is not None: plt.plot(rnd[0], rnd[1], "^g") for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], "-g") for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, "sk", ms=10*size) plt.plot(self.start.x, self.start.y, "^r") plt.plot(self.end.x, self.end.y, "^b") plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.grid(True) plt.pause(0.01) def draw_static(self, path): plt.clf() for node in self.nodeList: if node.parent is not None: plt.plot([node.x, self.nodeList[node.parent].x], [ node.y, self.nodeList[node.parent].y], "-g") for (ox, oy, size) in self.obstacleList: plt.plot(ox, oy, "sk", ms=10*size) plt.plot(self.start.x, self.start.y, "^r") plt.plot(self.end.x, self.end.y, "^b") plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand]) plt.plot([data[0] for data in path], [data[1] for data in path], '-r') plt.grid(True) plt.show()

画图，不解释

def main(): print("start RRT path planning") obstacle_list = [ (5, 1, 2), (3, 6, 3), (3, 8, 2), (3, 5, 2), (6, 5, 2)] # Set Initial parameters rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list) path = rrt.planning() print(path) # Draw final path if show_animation: plt.close() rrt.draw_static(path) if __name__ == '__main__': main()

主函数

程序循行效果如下图：