当前位置: 首页 > news >正文

[python] RRT快速拓展随机树

"""
version1.1,2018-05-09
《基于智能优化与RRT算法的无人机任务规划方法研究》博士论文
《基于改进人工势场法的路径规划算法研究》硕士论文

"""

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):
        """
        产生随机节点
        :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

    @staticmethod
    def 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

    @staticmethod
    def collision_check(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):
        """
        画出静态图像
        :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 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()
RRT快速拓展随机树 python实现

 

转载于:https://www.cnblogs.com/clemente/p/9543106.html

相关文章:

  • 《Python从菜鸟到高手》已经出版,开始连载了,购买送视频课程
  • Clojure基础课程2-Clojure中的数据长啥样?
  • Spring Boot【快速入门】
  • 使用时间器区别网页上的单击和双击
  • css设置全局变量和局部变量
  • 宏观政策转向,消费金融行业能否送别“至暗时刻”?
  • vc code
  • FLIP-24+-+SQL+Client
  • 【转】【WPF】WPF - MVVM - 如何将ComboBox的Selectchange事件binding到ViewModel
  • linux内核中链表代码分析---list.h头文件分析(二)【转】
  • 时间不对导致vSAN服务无法启动
  • C# WinForm 技巧十: winfrom 全屏自适应屏幕分辨率
  • Java 软件高级工程师笔试题
  • Flask 的馈赠
  • redis的GEO实战
  • ----------
  • 深入了解以太坊
  • ECMAScript 6 学习之路 ( 四 ) String 字符串扩展
  • iOS动画编程-View动画[ 1 ] 基础View动画
  • JAVA并发编程--1.基础概念
  • mockjs让前端开发独立于后端
  • opencv python Meanshift 和 Camshift
  • React 快速上手 - 07 前端路由 react-router
  • 蓝海存储开关机注意事项总结
  • 聊聊springcloud的EurekaClientAutoConfiguration
  • 事件委托的小应用
  • 腾讯大梁:DevOps最后一棒,有效构建海量运营的持续反馈能力
  • gunicorn工作原理
  • 容器镜像
  • 选择阿里云数据库HBase版十大理由
  • ​DB-Engines 12月数据库排名: PostgreSQL有望获得「2020年度数据库」荣誉?
  • ​猴子吃桃问题:每天都吃了前一天剩下的一半多一个。
  • #1015 : KMP算法
  • (AngularJS)Angular 控制器之间通信初探
  • (Java岗)秋招打卡!一本学历拿下美团、阿里、快手、米哈游offer
  • (LeetCode 49)Anagrams
  • (Redis使用系列) Springboot 使用Redis+Session实现Session共享 ,简单的单点登录 五
  • (仿QQ聊天消息列表加载)wp7 listbox 列表项逐一加载的一种实现方式,以及加入渐显动画...
  • (附源码)spring boot车辆管理系统 毕业设计 031034
  • (附源码)计算机毕业设计SSM智慧停车系统
  • (蓝桥杯每日一题)平方末尾及补充(常用的字符串函数功能)
  • (求助)用傲游上csdn博客时标签栏和网址栏一直显示袁萌 的头像
  • (十二)python网络爬虫(理论+实战)——实战:使用BeautfulSoup解析baidu热搜新闻数据
  • (五)网络优化与超参数选择--九五小庞
  • (转) ns2/nam与nam实现相关的文件
  • (转)Android学习笔记 --- android任务栈和启动模式
  • (轉貼) UML中文FAQ (OO) (UML)
  • .Mobi域名介绍
  • .NET Framework 和 .NET Core 在默认情况下垃圾回收(GC)机制的不同(局部变量部分)
  • .net 中viewstate的原理和使用
  • .Net(C#)自定义WinForm控件之小结篇
  • .NetCore部署微服务(二)
  • .net网站发布-允许更新此预编译站点
  • .net专家(高海东的专栏)
  • @select 怎么写存储过程_你知道select语句和update语句分别是怎么执行的吗?