路径规划——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
相关文章:

路径规划——RRT-Connect算法
路径规划——RRT-Connect算法 算法原理 RRT-Connect算法是在RRT算法的基础上进行的扩展,引入了双树生长,分别以起点和目标点为树的根节点同时扩展随机树从而实现对状态空间的快速搜索。在此算法中以两棵随机树建立连接为路径规划成功的条件。并且&…...

数据科学与SQL:如何计算排列熵?| 基于SQL实现
目录 0 引言 1 排列熵的计算原理 2 数据准备 3 问题分析 4 小结 0 引言 把“熵”应用在系统论中的信息管理方法称为熵方法。熵越大,说明系统越混乱,携带的信息越少;熵越小,说明系统越有序,携带的信息越多。在传感…...

Redis/Codis性能瓶颈揭秘:网卡软中断的影响与优化
目录 现象回顾 问题剖析 现场分析 解决方案 总结与反思 1.调整中断亲和性(IRQ Affinity): 2.RPS(Receive Packet Steering)和 RFS(Receive Flow Steering): 近期,…...

微知-DOCA ARGP参数模块的相关接口和用法(config单元、params单元,argp pipe line,回调)
文章目录 1. 背景2. 设置参数的主要流程2.1 初始化2.2 注册某个params的处理方式以及回调函数2.4 定义好前面的params以及init指定config地点后start处理argv 3. 其他4. DOCA ARGP包相关4.1 主要接口4.2 DOCA ARGP的2个rpm包4.2.1 doca-sdk-argp-2.9.0072-1.el8.x86_64.rpm4.2.…...
PostgreSQL高可用Patroni安装(超详细)
目录 一 安装Patroni 0 Patroni 对Python的版本要求 1 卸载原来的Python 3.6 版本 2 安装Python 3.7 之上版本 3 安装依赖 psycopg3 4 安装patroni 5 卸载 patroni 二 安装ETCD 1 使用 yum 安装 etcd 2 etcd 配置文件 3 管理 etcd 4 设置密码 5 常用命令 三 安装…...

mcu之,armv7架构,contex-M4系列,时钟树,中断,IO架构(一)
写这篇文章的目的,是记录一下arm架构的32mcu,方便记忆芯片架构原理,方便我展开对,BootLoader的研究。 arm架构,时钟树,先做个记录,有空写。...
论文解析:基于区块链的去中心化服务选择,用于QoS感知的云制造(四区)
目录 论文解析:基于区块链的去中心化服务选择,用于QoS感知的云制造(四区) 基于区块链的去中心化云制造服务选择方法 一、核心内容概述 二、核心创新点及原理与理论 三、实验与理论分析 PBFT(实用拜占庭容错) 论文解析:基于区块链的去中心化服务选择,用于QoS感知的…...

详细解析STM32 GPIO引脚的8种模式
目录 一、输入浮空(Floating Input):GPIO引脚不连接任何上拉或下拉电阻,处于高阻态 1.浮空输入的定义 2.浮空输入的特点 3.浮空输入的应用场景 4.浮空输入的缺点 5.典型配置方式 6.注意事项 二、输入上拉(Inpu…...

【hacker送书第16期】Python数据分析、挖掘与可视化、AI全能助手ChatGPT职场工作效率提升技巧与案例
解锁数据分析与AI应用的双重秘密:全面推广《Python数据分析、挖掘与可视化从入门到精通》与《AI全能助手ChatGPT职场工作效率提升技巧与案例》 前言Python数据分析、挖掘与可视化从入门到精通💕内容简介获取方式 AI全能助手ChatGPT职场工作效率提升技巧与…...

翼鸥教育:从OceanBase V3.1.4 到 V4.2.1,8套核心集群升级实践
引言:自2021年起,翼鸥教育便开始应用OceanBase社区版,两年间,先后部署了总计12套生产集群,其中核心集群占比超过四分之三,所承载的数据量已突破30TB。自2022年10月,OceanBase 社区发布了4.2.x 版…...
WebGIS开发中不同坐标系坐标转换问题
在 JavaScript 中,使用 proj4 库进行坐标系转换是一个非常常见的操作。proj4 是一个支持多种坐标系的 JavaScript 库,提供了从一种坐标系到另一种坐标系的转换功能。 以下是使用 proj4 进行坐标系转换的基本步骤: 1. 安装 proj4 你可以通过…...

【青牛科技】视频监控器应用
1、简介: 我司安防产品广泛应用在视频监控器上,产品具有性能优良,可 靠性高等特点。 2、图示: 实物图如下: 3、具体应用: 标题:视频监控器应用 简介:视频监控器工作原理是光&#x…...

AWTK-WIDGET-WEB-VIEW 实现笔记 (3) - MacOS
MacOS 上实现 AWTK-WIDGET-WEB-VIEW 有点麻烦,主要原因是没有一个简单的办法将一个 WebView 嵌入到一个窗口中。所以,我们只能通过创建一个独立的窗口来实现。 1. 创建窗口 我对 Object-C 不熟悉,也不熟悉 Cocoa 框架,在 ChatGPT…...

PgSQL即时编译JIT | 第1期 | JIT初识
PgSQL即时编译JIT | 第1期 | JIT初识 JIT是Just-In-Time的缩写,也就是说程序在执行的时候生成可以执行的代码,然后执行它。在介绍JIT之前,需要说下两种执行方式:解释执行和编译执行。其中解释执行是通过解释器,将代码逐…...
Go小记:使用Go实现ssh客户端
一、前言 SSH(Secure Shell)是一种用于在不安全网络上安全访问远程计算机的网络协议。它通过加密的方式提供远程登录会话和其他网络服务,保证通信的安全性和数据的完整性。 本文使用golang.org/x/crypto/ssh包来实现SSH客户端 可以通过go …...
Nginx Spring boot指定域名跨域设置
1、Nginx配置跨域: server {listen 80;server_name your-backend-service.com;location / {proxy_pass http://localhost:8080; # Spring Boot应用的内部地址proxy_set_header Host $host;proxy_set_header X-Real-IP $remote_addr;proxy_set_header X-Forwarded-F…...

深入理解Redis(七)----Redis实现分布式锁
基于Redis的实现方式 1、选用Redis实现分布式锁原因: (1)Redis有很高的性能; (2)Redis命令对此支持较好,实现起来比较方便 2、使用命令介绍: (1)SETNX SETNX …...
Database Advantages (数据库系统的优点)
数据库管理系统(DBMS)提供了一种结构化的方式来存储、管理和访问数据,与传统的文件处理系统相比,数据库提供了许多显著的优点。以下是数据库系统的主要优势: 1. Data Integrity (数据完整性) 概念:数据完整…...

Qt桌面应用开发 第五天(常用控件)
目录 1.QPushButton和ToolButton 1.1QPushButton 1.2ToolButton 2.RadioButton和CheckBox 2.1RadioButton单选按钮 2.2CheckBox多选按钮 3.ListWidget 4.TreeWidget控件 5.TableWidget控件 6.Containers控件 6.1QScrollArea 6.2QToolBox 6.3QTabWidget 6.4QStacke…...

初识Linux · 信号处理 · 续
目录 前言: 可重入函数 重谈进程等待和优化 前言: 在前文,我们已经介绍了信号产生,信号保存,信号处理的主题内容,本文作为信号处理的续篇,主要是介绍一些不那么重要的内容,第一个…...

观成科技:隐蔽隧道工具Ligolo-ng加密流量分析
1.工具介绍 Ligolo-ng是一款由go编写的高效隧道工具,该工具基于TUN接口实现其功能,利用反向TCP/TLS连接建立一条隐蔽的通信信道,支持使用Let’s Encrypt自动生成证书。Ligolo-ng的通信隐蔽性体现在其支持多种连接方式,适应复杂网…...

python打卡day49
知识点回顾: 通道注意力模块复习空间注意力模块CBAM的定义 作业:尝试对今天的模型检查参数数目,并用tensorboard查看训练过程 import torch import torch.nn as nn# 定义通道注意力 class ChannelAttention(nn.Module):def __init__(self,…...
云计算——弹性云计算器(ECS)
弹性云服务器:ECS 概述 云计算重构了ICT系统,云计算平台厂商推出使得厂家能够主要关注应用管理而非平台管理的云平台,包含如下主要概念。 ECS(Elastic Cloud Server):即弹性云服务器,是云计算…...
day52 ResNet18 CBAM
在深度学习的旅程中,我们不断探索如何提升模型的性能。今天,我将分享我在 ResNet18 模型中插入 CBAM(Convolutional Block Attention Module)模块,并采用分阶段微调策略的实践过程。通过这个过程,我不仅提升…...

新能源汽车智慧充电桩管理方案:新能源充电桩散热问题及消防安全监管方案
随着新能源汽车的快速普及,充电桩作为核心配套设施,其安全性与可靠性备受关注。然而,在高温、高负荷运行环境下,充电桩的散热问题与消防安全隐患日益凸显,成为制约行业发展的关键瓶颈。 如何通过智慧化管理手段优化散…...
Java 加密常用的各种算法及其选择
在数字化时代,数据安全至关重要,Java 作为广泛应用的编程语言,提供了丰富的加密算法来保障数据的保密性、完整性和真实性。了解这些常用加密算法及其适用场景,有助于开发者在不同的业务需求中做出正确的选择。 一、对称加密算法…...
《C++ 模板》
目录 函数模板 类模板 非类型模板参数 模板特化 函数模板特化 类模板的特化 模板,就像一个模具,里面可以将不同类型的材料做成一个形状,其分为函数模板和类模板。 函数模板 函数模板可以简化函数重载的代码。格式:templa…...
iOS性能调优实战:借助克魔(KeyMob)与常用工具深度洞察App瓶颈
在日常iOS开发过程中,性能问题往往是最令人头疼的一类Bug。尤其是在App上线前的压测阶段或是处理用户反馈的高发期,开发者往往需要面对卡顿、崩溃、能耗异常、日志混乱等一系列问题。这些问题表面上看似偶发,但背后往往隐藏着系统资源调度不当…...

论文阅读:Matting by Generation
今天介绍一篇关于 matting 抠图的文章,抠图也算是计算机视觉里面非常经典的一个任务了。从早期的经典算法到如今的深度学习算法,已经有很多的工作和这个任务相关。这两年 diffusion 模型很火,大家又开始用 diffusion 模型做各种 CV 任务了&am…...
【实施指南】Android客户端HTTPS双向认证实施指南
🔐 一、所需准备材料 证书文件(6类核心文件) 类型 格式 作用 Android端要求 CA根证书 .crt/.pem 验证服务器/客户端证书合法性 需预置到Android信任库 服务器证书 .crt 服务器身份证明 客户端需持有以验证服务器 客户端证书 .crt 客户端身份…...