什么叫页面价格,东营优化网站,网站建设目标概括,深圳百度网站优化《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文《基于改进人工势场法的路径规划算法研究》硕士论文import matplotlib.pyplot as pltimport randomimport mathimport copyshow_animation Trueclass Node(object):《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文《基于改进人工势场法的路径规划算法研究》硕士论文import matplotlib.pyplot as pltimport randomimport mathimport copyshow_animation Trueclass Node(object):RRT Nodedef __init__(self, x, y):self.x xself.y yself.parent Noneclass RRT(object):Class for RRT Planningdef __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 nodestaticmethoddef 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_indexstaticmethoddef 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 offwhile 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, rndNone):Draw Graphprint(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, ms10*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, ms10*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_listobstacle_list)path rrt.planning()print(path)# Draw final pathif show_animation:plt.close()rrt.draw_static(path)if __name__ __main__:main()
