路径规划——RRT-Connect算法

路径规划——RRT-Connect算法

算法原理

RRT-Connect算法是在RRT算法的基础上进行的扩展,引入了双树生长,分别以起点和目标点为树的根节点同时扩展随机树从而实现对状态空间的快速搜索。在此算法中以两棵随机树建立连接为路径规划成功的条件。并且,在搜索过程中使用了贪婪搜索的方法,在搜索的过程中,两棵树是交替扩展的,与RRT算法不同的是,RRT-Connect算法并不是每次扩展都会进行随机采样,而是第一棵树先随机采样进而扩展一个新的节点node_new,然后第二棵树利用node_new节点往相同的方向进行多次扩展直到扩展失败才会开始下一轮的交替扩展或者与另一棵树能够建立连接了从而满足路径规划完成的条件。

这种双向的RRT算法比原始RRT算法的搜索速度更快,因为除了使用双树扩展搜索,两棵树在扩展时还是朝着对方的方向进行扩展的,并不是完全随机的。

具体的算法流程可结合上述原理以及RRT算法的实现流程。

算法实现

"""@File: rrt-connect.py@Brief: RRT-Connect algorithm for pathplanning@Author: Benxiaogu@Github: https://github.com/Benxiaogu@CSDN: https://blog.csdn.net/weixin_51995147?type=blog@Date: 2024-11-13
"""
import numpy as np
import random
import math
from itertools import combinations
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import matplotlib.patches as patchesclass RRTConnect:def __init__(self,start,goal,obstacles,board_size,max_try,max_dist,goal_sample_rate,env) -> None:self.start = self.Node(start,None,0)self.goal = self.Node(goal,None,0)self.obstacles = obstaclesself.board_size = board_sizeself.max_try = max_try # Number of iterationsself.max_dist = max_dist # Maximum sampling distanceself.goal_sample_rate = goal_sample_rateself.env = envself.inflation = 1self.searched = []class Node:def __init__(self,position,parent,cost) -> None:self.position = positionself.parent = parentself.cost = costdef run(self):cost,path,expand = self.plan()self.searched = expandself.visualize(cost,path)def plan(self):nodes_forward = {self.start.position: self.start}nodes_back = {self.goal.position: self.goal}for iter in range(self.max_try):# Generate a random nodenode_rand = self.get_random_node()# Get the nearest neighbor nodenode_near = self.get_nearest_neighbor(list(nodes_forward.values()),node_rand)# Get the new nodenode_new = self.get_new_node(node_rand,node_near)if node_new:nodes_forward[node_new.position] = node_newnode_near_b = self.get_nearest_neighbor(list(nodes_back.values()), node_new)node_new_b = self.get_new_node(node_new,node_near_b)if node_new_b:nodes_back[node_new_b.position] = node_new_b# Greedy extendingwhile True:for node_position, node in nodes_back.items():if node.position == node_new.position:print("final")cost, path = self.extractPath(node_new, nodes_back, nodes_forward)expand = self.get_expand(list(nodes_back.values()), list(nodes_forward.values()))print("Exploring {} nodes.".format(iter))return cost, path, expandnode_new_b2 = self.get_new_node(node_new,node_new_b)if node_new_b2:nodes_back[node_new_b2.position] = node_new_b2node_new_b = node_new_b2else:breakif len(nodes_back) < len(nodes_forward):nodes_forward, nodes_back = nodes_back, nodes_forwardreturn 0, None, Nonedef get_random_node(self):"""Return a random node."""if random.random()>self.goal_sample_rate:node = self.Node((random.uniform(0,self.env.height),random.uniform(0,self.env.width)),None,0)else:node = self.goalreturn nodedef get_nearest_neighbor(self,node_list,node):"""Return node that is nearest to 'node' in node_list"""dist = [self.distance(node, n) for n in node_list]node_near = node_list[int(np.argmin(dist))]return node_neardef get_new_node(self,node_rand,node_near):"""Return node found based on node_near and node_rand."""dx = node_rand.position[0] - node_near.position[0]dy = node_rand.position[1] - node_near.position[1]dist = math.hypot(dx,dy)theta = math.atan2(dy, dx)d = min(self.max_dist,dist)position = ((node_near.position[0]+d*math.cos(theta)),node_near.position[1]+d*math.sin(theta))node_new = self.Node(position,node_near,node_near.cost+d)if self.isCollision(node_new, node_near):return Nonereturn node_newdef isCollision(self,node1,node2):"""Judge collision from node1 to node2 """if self.isInObstacles(node1) or self.isInObstacles(node2):return Truefor rect in self.env.obs_rectangle:if self.isInterRect(node1,node2,rect):return Truefor circle in self.env.obs_circle:if self.isInterCircle(node1,node2,circle):return Truereturn Falsedef distance(self,node1,node2):dx = node2.position[0] - node1.position[0]dy = node2.position[1] - node1.position[1]return math.hypot(dx,dy)def isInObstacles(self,node):"""Determine whether it is in obstacles or not."""x,y = node.position[0],node.position[1]for (ox,oy,w,h) in self.env.boundary:if ox-self.inflation<x<ox+w+self.inflation and oy-self.inflation<y<oy+h+self.inflation:return Truefor (ox,oy,w,h) in self.env.obs_rectangle:if ox-self.inflation<x<ox+w+self.inflation and oy-self.inflation<y<oy+h+self.inflation:return Truefor (ox,oy,r) in self.env.obs_circle:if math.hypot(x-ox,y-oy)<=r+self.inflation:return Truereturn Falsedef isInterRect(self,node1,node2,rect):""""Judge whether it will cross the rectangle when moving from node1 to node2"""ox,oy,w,h = rectvertex = [[ox-self.inflation,oy-self.inflation],[ox+w+self.inflation,oy-self.inflation],[ox+w+self.inflation,oy+h+self.inflation],[ox-self.inflation,oy+h+self.inflation]]x1,y1 = node1.positionx2,y2 = node2.positiondef cross(p1,p2,p3):x1 = p2[0]-p1[0]y1 = p2[1]-p1[1]x2 = p3[0]-p1[0]y2 = p3[1]-p1[0]return x1*y2 - x2*y1for v1,v2 in combinations(vertex,2):if max(x1,x2) >= min(v1[0],v2[0]) and min(x1,x2)<=max(v1[0],v2[0]) and \max(y1,y2) >= min(v1[1],v2[1]) and min(y1,y2) <= max(v1[1],v2[1]):if cross(v1,v2,node1.position) * cross(v1,v2,node2.position)<=0 and \cross(node1.position,node2.position,v1) * cross(node1.position,node2.position,v2)<=0:return Truereturn Falsedef isInterCircle(self,node1,node2,circle):"""Judge whether it will cross the circle when moving from node1 to node2"""ox,oy,r = circledx = node2.position[0] - node1.position[0]dy = node2.position[1] - node1.position[1]# print("isInterCircle-dx:",dx)# print("isInterCircle-dy:",dy)d = dx * dx + dy * dyif d==0:return False# Projectiont = ((ox - node1.position[0]) * dx + (oy - node1.position[1]) * dy) / d# The projection point is on line segment ABif 0 <= t <= 1:closest_x = node1.position[0] + t * dxclosest_y = node1.position[1] + t * dy# Distance from center of the circle to line segment ABdistance = math.hypot(ox-closest_x,oy-closest_y)return distance <= r+self.inflationreturn Falsedef extractPath(self, node_middle, nodes_back, nodes_forward):""""Extract the path based on the closed set."""if self.start.position in nodes_back:nodes_forward, nodes_back = nodes_back, nodes_forward# forwardnode = nodes_forward[node_middle.position]path_forward = [node.position]cost = node.costwhile node.position != self.start.position:node_parent = nodes_forward[node.parent.position]node = node_parentpath_forward.append(node.position)# backwardnode = nodes_back[node_middle.position]path_back = []cost += node.costwhile node.position != self.goal.position:node_parent = nodes_back[node.parent.position]node = node_parentpath_back.append(node.position)path = list(reversed(path_forward))+path_backreturn cost, pathdef get_expand(self, nodes_back, nodes_forward):expand = []tree_size = max(len(nodes_forward), len(nodes_back))for tr in range(tree_size):if tr < len(nodes_forward):expand.append(nodes_forward[tr])if tr < len(nodes_back):expand.append(nodes_back[tr])return expanddef visualize(self, cost, path):"""Plot the map."""...

在这里插入图片描述
完整代码:PathPlanning

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/diannao/60651.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

2024游戏陪玩app源码的功能介绍/线上陪玩交友上线即可运营软件平台源码搭建流程

一个完整的陪玩交友系统从概念到实现再到维护的全过程得以清晰展现。每一步都需要团队的紧密协作与细致规划&#xff0c;以确保系统既满足用户需求&#xff0c;又具备良好的稳定性和可扩展性。 基础框架 移动端开发框架&#xff1a;如uniapp&#xff0c;它支持多平台开发&…

缓冲式线程池C++简易实现

前言 : 代码也比较短&#xff0c;简单说一下代码结构&#xff0c;是这样的&#xff1a; SyncQueue.hpp封装了一个大小为MaxTaskCount的同步队列&#xff0c;这是一个模板类&#xff0c;它在线程池中承担了存放任务等待线程组中的线程来执行的角色。最底层是std::list<T>…

Unity资源打包Addressable AA包

从零到一 很多资料都是通过一步步设置讲解的&#xff0c;有时很想先快速实现&#xff0c;再了解细节。 下面就是远程加载Cube.prefab然后实例化简单的代码。 代码中可以不需要远程的网址&#xff0c;不需要资源下载的位置&#xff0c;不需要判断是否已经下载到本地。 那是如…

MySQL之索引(2)(B树、B+树、索引分类、聚集索引、二级索引、回表查询)

目录 一、B树结构索引&#xff08;B-树&#xff09; &#xff08;1&#xff09;特点。 &#xff08;2&#xff09;问题&#xff1a;范围查询效率&#xff1f;&#xff1f; &#xff08;3&#xff09;缺点。 1、查询的不稳定性。 2、各叶子节点无联系。 3、IO资源的消耗较多。 二…

翼鸥教育:从OceanBase V3.1.4 到 V4.2.1,8套核心集群升级实践

引言&#xff1a;自2021年起&#xff0c;翼鸥教育便开始应用OceanBase社区版&#xff0c;两年间&#xff0c;先后部署了总计12套生产集群&#xff0c;其中核心集群占比超过四分之三&#xff0c;所承载的数据量已突破30TB。自2022年10月&#xff0c;OceanBase 社区发布了4.2.x 版…

ubuntu使用DeepSpeech进行语音识别(包含交叉编译)

文章目录 前言一、DeepSpeech编译二、DeepSpeech使用示例三、核心代码分析1.创建模型核心代码2.识别过程核心代码 四、交叉编译1.交叉编译2.使用 总结 前言 由于工作需要语音识别的功能&#xff0c;环境是在linux arm版上&#xff0c;所以想先在ubuntu上跑起来看一看&#xff…

Go语言入门教案

文章目录 一、教学目标二、教学重难点&#xff08;一&#xff09;重点&#xff08;二&#xff09;难点 三、教学方法四、教学过程&#xff08;一&#xff09;Go语言简介&#xff08;二&#xff09;环境搭建1. 下载和安装Go语言开发环境2. 配置Go语言环境变量3. 命令行查看Go语言…

普通人如何做好AI数字人直播带货月入10W?

在科技飞速发展的今天&#xff0c;AI数字人直播正以惊人的速度崛起&#xff0c;为直播领域带来了一场前所未有的变革。那到底AI数字人直播前景怎么样&#xff0c;是怎样一个形式&#xff0c;普通人能够利用Ai数字人直播赚取到收益吗&#xff1f; 首先讲到AI数字人直播很多人想的…

飞牛私有云访问外网

飞牛私有云 fnOS NAS 是一款有着卓越的性能以及强大的兼容性和智能化的管理界面&#xff0c;它之所以能在 NAS 市场中脱颖而出&#xff0c;是因为 fnOS 基于最新的 Linux 内核&#xff08;Debian发行版&#xff09;深度开发&#xff0c;不仅兼容主流 x86 硬件&#xff0c;还支持…

论文 | The Capacity for Moral Self-Correction in LargeLanguage Models

概述 论文探讨了大规模语言模型是否具备“道德自我校正”的能力&#xff0c;即在收到相应指令时避免产生有害或偏见输出的能力。研究发现&#xff0c;当模型参数达到一定规模&#xff08;至少22B参数&#xff09;并经过人类反馈强化学习&#xff08;RLHF&#xff09;训练后&…

计算机毕业设计Python+大模型农产品推荐系统 农产品爬虫 农产品商城 农产品大数据 农产品数据分析可视化 PySpark Hadoop

温馨提示&#xff1a;文末有 CSDN 平台官方提供的学长联系方式的名片&#xff01; 温馨提示&#xff1a;文末有 CSDN 平台官方提供的学长联系方式的名片&#xff01; 温馨提示&#xff1a;文末有 CSDN 平台官方提供的学长联系方式的名片&#xff01; 作者简介&#xff1a;Java领…

一文窥见神经网络

一文窥见神经网络 1.初识神经元1.1 生物神经元1.2 人工神经元1.3 权重的作用1.4 偏置的作用1.5 激活函数的作用1.5.1 线性激活函数1.5.2 非线性激活函数 2. 神经元模型2.1 多输入单神经元模型2.2 一层神经元模型2.3 神经网络&#xff08;多层神经元&#xff09;模型 3. 神经网络…

【视觉SLAM】2-三维空间刚体运动的数学表示

读书笔记&#xff1a;学习空间变换的三种数学表达形式。 文章目录 1. 旋转矩阵1.1 向量运算1.2 坐标系空间变换1.3 变换矩阵与齐次坐标 2. 旋转向量和欧拉角2.1 旋转向量2.2 欧拉角 3. 四元数 1. 旋转矩阵 1.1 向量运算 对于三维空间中的两个向量 a , b ∈ R 3 a,b \in \R^3 …

shell 100例

1、每天写一个文件 (题目要求&#xff09; 请按照这样的日期格式(xxxx-xx-xx每日生成一个文件 例如生成的文件为2017-12-20.log&#xff0c;并且把磁盘的使用情况写到到这个文件中不用考虑cron&#xff0c;仅仅写脚本即可 [核心要点] date命令用法 df命令 知识补充&#xff1…

[Python学习日记-66] 多态与多态性

[Python学习日记-66] 多态与多态性 简介 多态 多态性 鸭子类型 简介 多态与多态性都是面向对象的特征之一&#xff0c;它们都是面向对象编程的一个重要概念&#xff0c;在 Python 当中也有一些独特的见解和用法&#xff0c;下面我们一起来了解一下是怎么回事吧。 多态 多态…

Linux基础1

Linux基础1 Linux基础1学习笔记 ‍ 声明&#xff01; ​​​学习视频来自B站up主 泷羽sec 有兴趣的师傅可以关注一下&#xff0c;如涉及侵权马上删除文章 笔记只是方便各位师傅的学习和探讨&#xff0c;文章所提到的网站以及内容&#xff0c;只做学习交流&#xff0c;其他…

GESP4级考试语法知识(贪心算法(二))

排队接水2代码&#xff1a; #include<iostream> #include<cstdio> #include<algorithm> using namespace std; struct people {int num;int time; }; people s[1001]; int n,r,a[1001]; double sum,ave; bool cmp(people x,people y) {return x.time<y.t…

MySQL45讲 第二十讲 幻读是什么,幻读有什么问题?

文章目录 MySQL45讲 第二十讲 幻读是什么&#xff0c;幻读有什么问题&#xff1f;一、幻读的定义二、幻读带来的问题&#xff08;一&#xff09;语义问题&#xff08;二&#xff09;数据一致性问题 三、InnoDB 解决幻读的方法四、总结 MySQL45讲 第二十讲 幻读是什么&#xff0…

【再谈设计模式】建造者模式~对象构建的指挥家

一、引言 在软件开发的世界里&#xff0c;创建对象是一项基本且频繁的操作。然而&#xff0c;当对象的构造变得复杂&#xff0c;涉及众多属性和初始化步骤时&#xff0c;传统的构造函数方式往往会让代码陷入混乱的泥沼。就如同搭建一座复杂的建筑&#xff0c;若没有合理的规划和…

三、模板与配置(下)

三、模板与配置 8、WXSS模板样式-全局样式和局部样式 类型说明适用情景注意点全局样式定义在 app.wxss 中的样式&#xff0c;作用于每一个页面。当有一些通用的样式规则需要应用于整个小程序时&#xff0c;比如全局的字体大小、颜色、布局等。全局样式可能会被局部样式覆盖&a…