RRT算法(快速拓展随机树)的Python实现

"""
《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文
《基于改进人工势场法的路径规划算法研究》硕士论文"""import matplotlib.pyplot as plt
import random
import math
import 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()

参考

https://www.cnblogs.com/yangmingustb/p/9013534.html

RRT算法(快速拓展随机树)的Python实现相关推荐

python无人机路径规划算法_快速拓展随机树（RRT)路径规划，python
1 """2 version1.1,2018-05-09 3 <基于智能优化与RRT算法的无人机任务规划方法研究>博士论文4 <基于改进人工势场法的路径 ...
python无人机路径规划算法_RRT算法在Python中的实现,快速,拓展,随机,树
""" <基于智能优化与RRT算法的无人机任务规划方法研究>博士论文 <基于改进人工势场法的路径规划算法研究>硕士论文 ""& ...
rrt算法流程图_RRT算法移动机器人路径规划(快速扩展随机树).pdf
( ) 第 34 卷第 5期南京理工大学学报自然科学版 Vo l. 34 No. 5 20 10年 10 月 Journal of N anj ing Un iversity of Scienc ...
聊聊路径规划算法—快速搜寻随机树算法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨 Li-Jianghao 来源丨古月居快速搜寻随机树(RRT)算法是一个增量式采样的搜寻技术, ...
快速搜索随机树（RRT---Rapidly-exploring Random Trees）入门及在Matlab中演示
转载:http://blog.csdn.net/gpeng832/article/details/71249198?locationNum=1&fps=1 快速搜索随机树(RRT -Rap ...
左上角到右下角的路径 oj_【机器人路径规划】快速扩展随机树(RRT)算法
内容无关:最近的课题内容和机器人运动规划方法有关,我把学习的内容整理成为工具箱上传到了我的github仓库,稍后将会发一篇说明介绍使用方法. XM522706601/robotics_tutorial ...
【运动规划】RRT快速搜索随机树 Rapidly Exploring Random Tree
Randomized Kinodynamic Planning Steven M. LaValle James J. Kuffner, Jr. 1. Exploring the State Space ...
[matlab] 7.快速搜索随机树（RRT---Rapidly-exploring Random Trees) 路径规划
RRT是一种多维空间中有效率的规划方法.它以一个初始点作为根节点,通过随机采样增加叶子节点的方式,生成一个随机扩展树,当随机树中的叶子节点包含了目标点或进入了目标区域,便可以在随机树中找到一条由从初始 ...
【路径规划】基于拓展随机树（RRT）算法的路径规划问题（Matlab代码实现）

RRT算法(快速拓展随机树)的Python实现

RRT算法(快速拓展随机树)的Python实现相关推荐

最新文章

热门文章