"""《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文《基于改进人工势场法的路径规划算法研究》硕士论文"""import matplotlib.pyplot as pltimport randomimport mathimport copyshow_animation = Trueclass Node(object):"""RRT Node"""def __init__(self, x, y):self.x = xself.y = yself.parent = Noneclass RRT(object):"""Class for RRT Planning"""def __init__(self, start, goal, obstacle_list, rand_area):"""Setting Parameterstart: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.0self.goalSampleRate = 0.05 # 选择终点的概率是0.05self.maxIter = 500self.obstacleList = obstacle_listself.nodeList = [self.start]def random_node(self):"""产生随机节点:return:"""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@staticmethoddef get_nearest_list_index(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@staticmethoddef collision_check(new_node, obstacle_list):a = 1for (ox, oy, size) in obstacle_list:dx = ox - new_node.xdy = oy - new_node.yd = math.sqrt(dx * dx + dy * dy)if d <= size:a = 0 # collisionreturn a # safedef planning(self):"""Path planninganimation: flag for animation on or off"""while True:# Random Samplingif random.random() > self.goalSampleRate:rnd = self.random_node()else:rnd = [self.end.x, self.end.y]# Find nearest nodemin_index = self.get_nearest_list_index(self.nodeList, rnd)# print(min_index)# expand treenearest_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_indexif not self.collision_check(new_node, self.obstacleList):continueself.nodeList.append(new_node)# check goaldx = new_node.x - self.end.xdy = new_node.y - self.end.yd = math.sqrt(dx * dx + dy * dy)if d <= self.expandDis:print("Goal!!")breakif True:self.draw_graph(rnd)path = [[self.end.x, self.end.y]]last_index = len(self.nodeList) - 1while self.nodeList[last_index].parent is not None:node = self.nodeList[last_index]path.append([node.x, node.y])last_index = node.parentpath.append([self.start.x, self.start.y])return pathdef 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):"""画出静态图像:return:"""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, 1),(3, 6, 2),(3, 8, 2),(1, 1, 2),(3, 5, 2),(9, 5, 2)]# Set Initial parametersrrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list)path = rrt.planning()print(path)# Draw final pathif show_animation:plt.close()rrt.draw_static(path)if __name__ == '__main__':main()
参考
/yangmingustb/p/9013534.html
