路径规划 | ROS中多个路径规划算法可视化与性能对比分析
目录
- 0 专栏介绍
- 1 引言
- 2 禁用局部规划器
- 3 路径规划定性对比实验
- 3.1 加载路径规划器和可视化插件
- 3.2 设置起点和终点
- 3.3 选择规划器规划
- 3.4 不同规划器对比
- 3.5 路径保存和加载
- 4 路径规划定量对比实验
- 4.1 计算规划耗时
- 4.2 计算规划长度
- 4.3 计算拓展节点数
- 4.4 计算路径曲率
- 4.5 计算路径转角
0 专栏介绍
🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备!详细介绍全局规划(图搜索、采样法、智能算法等);局部规划(DWA、APF等);曲线优化(贝塞尔曲线、B样条曲线等)。
🚀详情:图解自动驾驶中的运动规划(Motion Planning),附几十种规划算法
1 引言
在《运动规划实战精讲》系列中,我们介绍了非常多种路径规划算法,不同的算法有不同的工作原理及其适用场景。例如,A*算法通过启发式方法进行路径搜索,能够在较小的搜索空间内找到较优解,而RRT算法则通过随机采样快速扩展搜索树,适合高维空间的路径规划。
自然地,我们希望能够有一种方式定性、定量地对比不同算法的性能,比如计算效率、资源消耗等。本节通过设计一个可视化插件的形式,帮助大家进行规划算法的对比实验,并提供了常用的定量指标计算方法,最终效果如下所示

2 禁用局部规划器
在ROS的导航功能包集navigation中提供了move_base功能包,用于实现导航功能。move_base功能包提供了基于动作(action)的路径规划实现,move_base可以根据给定的目标点,控制机器人底盘运动至目标位置,并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。我们知道,move_base主要由全局路径规划与本地路径规划组成。

如果希望在给定的起点和终点对比不同的规划算法,就需要临时屏蔽local_planner的功能,防止机器人因为运动造成重规划。局部规划插件的详细设计流程在ROS从入门到精通5-5:局部路径规划插件开发案例(以DWA算法为例),这里直接给出静态局部规划器的代码
bool StaticPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
{if (!initialized_){ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");return false;}return true;
}bool StaticPlanner::isGoalReached()
{if (!initialized_){ROS_ERROR("Static planner has not been initialized");return false;}if (goal_reached_){ROS_INFO("GOAL Reached!");return true;}return false;
}bool StaticPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{if (!initialized_){ROS_ERROR("Static planner has not been initialized");return false;}cmd_vel.linear.x = 0.0;cmd_vel.angular.z = 0.0;return true;
}
3 路径规划定性对比实验
3.1 加载路径规划器和可视化插件
首先打开src/sim_env/config中的planner.yaml文件,配置可选的规划器属性,如下所示
planner:- a_star- dijkstra- gbfs- jps- d_star- lpa_star- d_star_lite- voronoi- theta_star- lazy_theta_star- rrt- rrt_star- rrt_connect- informed_rrt- aco- pso- ga
接着启动项目,在rviz中添加RMPV可视化插件

3.2 设置起点和终点
通过RMPV面板的Start Pose和Goal Pose选项设置起点和终点位姿

3.3 选择规划器规划
在RMPV的Planner选项中选择具体的规划器,点击Start Planning即可开始规划,左侧面板将显示规划器名称、起点、终点、路径长度、颜色等信息

3.4 不同规划器对比
重复3.3的步骤进行不同规划器的规划实验

3.5 路径保存和加载
点击Save Paths可以以json格式保存路径到本地,如图所示。再下一次实验中,可以通过Load Paths复现之前的实验

4 路径规划定量对比实验
4.1 计算规划耗时
耗时指标用cal_time.count()表示
#include <chrono>auto start_time = std::chrono::high_resolution_clock::now();
g_planner_->plan(...);
auto finish_time = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> cal_time = finish_time - start_time;
R_INFO << "Calculation Time: " << cal_time.count();
4.2 计算规划长度
规划长度用path_len表示
for (int i = 1; i < plan.size(); i++)
{const auto& pt1 = plan[i - 1];const auto& pt2 = plan[i];path_len += std::hypot(pt1.x() - pt2.x(), pt1.y() - pt2.y());
}
R_INFO << "Path Length: " << path_len;
4.3 计算拓展节点数
对于图搜索算法,拓展节点数是重要指标,用expand.size()表示
path_found = g_planner_->plan({ g_start_x, g_start_y, tf2::getYaw(start.pose.orientation) },{ g_goal_x, g_goal_y, tf2::getYaw(goal.pose.orientation) }, origin_path, expand);
R_INFO << "Expand Size: " << expand.size();
4.4 计算路径曲率
平均曲率和最大曲率是衡量路径平滑度的重要指标,计算如下
double max_curv = 0.0;
double avg_curv = 0.0;auto calCurv = [](const PathPlanner::Point3d& pt1, const PathPlanner::Point3d& pt2,const PathPlanner::Point3d& pt3) {double ax = pt1.x();double ay = pt1.y();double bx = pt2.x();double by = pt2.y();double cx = pt3.x();double cy = pt3.y();double a = std::hypot(bx - cx, by - cy);double b = std::hypot(cx - ax, cy - ay);double c = std::hypot(ax - bx, ay - by);double cosB = (a * a + c * c - b * b) / (2 * a * c);double sinB = std::sin(std::acos(cosB));double cross = (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);return std::copysign(2 * sinB / b, cross);
};for (int i = 2; i < plan.size(); i++)
{int n = i - 1;const auto& pt1 = plan[i - 2];const auto& pt2 = plan[i - 1];const auto& pt3 = plan[i];double curv = std::fabs(calCurv(pt1, pt2, pt3));max_curv = std::max(curv, max_curv);avg_curv = ((n - 1) * avg_curv + curv) / n;
}
4.5 计算路径转角
平均转角和最大转角是衡量路径平滑度的重要指标,计算如下
double max_angle = 0.0;
double avg_angle = 0.0;auto calAngle = [](const PathPlanner::Point3d& pt1, const PathPlanner::Point3d& pt2,const PathPlanner::Point3d& pt3) {
rmp::common::geometry::Vec2d vec1(pt2.x() - pt1.x(), pt2.y() - pt1.y());
rmp::common::geometry::Vec2d vec2(pt3.x() - pt2.x(), pt3.y() - pt2.y());
return std::acos(vec1.innerProd(vec2) / (vec1.length() * vec2.length()));
};for (int i = 2; i < plan.size(); i++)
{int n = i - 1;const auto& pt1 = plan[i - 2];const auto& pt2 = plan[i - 1];const auto& pt3 = plan[i];double angle = calAngle(pt1, pt2, pt3);max_angle = std::max(angle, max_angle);avg_angle = ((n - 1) * avg_angle + angle) / n;
}
完整工程代码请联系下方博主名片获取
🔥 更多精彩专栏:
- 《ROS从入门到精通》
- 《Pytorch深度学习实战》
- 《机器学习强基计划》
- 《运动规划实战精讲》
- …
相关文章:
路径规划 | ROS中多个路径规划算法可视化与性能对比分析
目录 0 专栏介绍1 引言2 禁用局部规划器3 路径规划定性对比实验3.1 加载路径规划器和可视化插件3.2 设置起点和终点3.3 选择规划器规划3.4 不同规划器对比3.5 路径保存和加载 4 路径规划定量对比实验4.1 计算规划耗时4.2 计算规划长度4.3 计算拓展节点数4.4 计算路径曲率4.5 计…...
使用 PyCharm 构建 FastAPI 项目:零基础入门 Web API 开发
使用 PyCharm 构建 FastAPI 项目:零基础入门 Web API 开发 本文提供了一份完整的 FastAPI 入门指南,涵盖从环境搭建、依赖安装到创建并运行一个简单的 FastAPI 应用的各个步骤。通过 FastAPI 和 Uvicorn,开发者可以快速构建现代化的 Web API…...
Prim算法与Dijstra算法
注:参考如下文章和视频 不能说毫不相干,简直是一模一样(Prim vs Dijkstra) 普里姆和迪杰斯特拉太像了,他们有什么区别? Prim算法和Dijkstra算法区别 文章目录 总结数组元素的更新两种算法的完整代码 普里姆算法算法步骤算法描…...
水经微图IOS版5.6.1发布,新增图源二维码分享并修订徒步模式功能
随时随地,微图一下! 水经微图(以下称“微图”)IOS版5.6.1发布,本次升级主要新增了图源二维码分享功能,以及修订过往足迹的徒步模式功能。 当前版本 当前版本号为:5.6.1 如果你发现该版本中存…...
复现第三周
1.eval执行 1)打开题目 简单进行代码审计,而题目又为eval函数说明这里eval() 会执行传入的任意代码,可以通过 cmd 作为参数执行任意 PHP 代码,这里相当于用cmd作为参数来执行url头命令 2)在url头输入命令cmdsystem("ls&quo…...
Django---数据库(多表关联)
在Django中操作数据库并实现多表关联,主要是通过定义模型(Models)及其关系,然后利用Django ORM(Object-Relational Mapping)执行数据库操作。 定义模型及其关系 首先,需要在models.py文件中定…...
2024系统架构师---论软件可靠性设计及其应用论文
可靠性 软件可靠性是指软件系统在一定的时间内持续无故障运行的能力。 可靠性通常用平均失效等待时间(MTTF)和平均失效间隔时间(MTBF)来衡量。 影响可靠性的因素 从技术的角度来看,影响软件可靠性的主要因素如下。…...
SpringBoot在线教育系统:云部署策略
6系统测试 6.1概念和意义 测试的定义:程序测试是为了发现错误而执行程序的过程。测试(Testing)的任务与目的可以描述为: 目的:发现程序的错误; 任务:通过在计算机上执行程序,暴露程序中潜在的错误。 另一个…...
Zabbix 6.0 部署
目录 一、序章 二、zabbix概念 2.1 zabbix 是什么? 2.2 zabbix 监控原理: 2.3 Zabbix 6.0 新特性: 2.3.1 Zabbix server高可用防止硬件故障或计划维护期的停机 2.3.2 Zabbix 6.0 LTS新增Kubernetes监控功能,可以在Kubernet…...
用Python遍历输出烟感名称和状态
为了使用Python遍历输出烟感名称和状态,您需要首先从SNMP代理(如网络设备或硬件设备)获取这些值。为此,您可以使用第三方库如pysnmp,它允许您轻松地与SNMP代理通信。 首先,您需要安装pysnmp库,…...
Redis的持久化以及性能管理
目录 一、Redis持久化概述 1.什么是Redis持久化 2.持久化方式 3.RDB持久化 3.1概念 3.2触发条件 3.3执行流程 3.4启动时加载 4. AOF持久化 4.1概念 4.2启动AOF 4.3执行流程 4.4启动时加载 5.RDB和AOF的优缺点 二、Redis性能管理 1.查看Redis内存使用 2…...
Docker部署Meta-Llama-3.1-70B-Instruct API openai格式,vLLM速度对比
下载模型 modelscope环境,国内下载更快: conda create -n modelscope python=3.10 conda activate modelscopepip install modelscope命令行下载: https://modelscope.cn/models/LLM-Research/Meta-Llama-3.1-70B-Instruct modelscope download --model LLM-Research/Met…...
USB协议学习
文章目录 USB发展背景发展变化速度等级通讯接口 四种传输主设备 & 从设备主设备从设备 连接与检测高速设备与主机连接USB总线常见的几种状态 枚举过程特点 控制传输学习资料 USB发展背景 发展变化 USB1.1:规范了USB低全速传输; USB2.0:…...
TDengine 数据订阅 vs. InfluxDB 数据订阅:谁更胜一筹?
在时序数据的应用场景中,数据的实时消费和处理能力成为衡量数据库性能和可用性的重要指标。TDengine 和 InfluxDB 作为时序数据库(Time Series Database)中的佼佼者,在数据订阅方面各有特点。但从架构设计、灵活性和系统负载上看&…...
用户批评 SAP 的人工智能战略
在2024年德语SAP用户组织(DSAG)年会上,SAP用户对公司云优先的AI创新策略表示不满。SAP决定将AI功能仅限于云客户,使使用本地部署(on-premises)系统的用户感到被忽视。这种“云优先”策略引发了SAP用户间的广…...
Jest进阶知识:React组件的单元测试
在现代前端开发中,组件是构建应用程序的基本单元。一个组件不仅拥有完整的功能,还能极大地提高代码的复用性。因此,在进行单元测试时,对重要组件进行测试是必不可少的。 Testing Library Testing Library 是一个专门用于测试 We…...
MATLAB——矩阵操作
内容源于b站清风数学建模 数学建模清风老师《MATLAB教程新手入门篇》https://www.bilibili.com/video/BV1dN4y1Q7Kt/ 目录 1.MATLAB中的向量 1.1向量创建方法 1.2向量元素的引用 1.3向量元素修改和删除 2.MATLAB矩阵操作 2.1矩阵创建方法 2.2矩阵元素的引用 2.3矩阵…...
智能数据驱动的风险管理:正大金融科技的创新实践
在不断变化的金融环境中,风险管理成为投资成功的关键因素。正大公司以数据驱动的智能风控体系为核心,通过深度学习、数据分析等技术创新,帮助投资者在复杂的市场条件下实现稳健操作和风险控制。本文将探讨正大如何利用科技手段提升风险管理效…...
贝尔不等式的验证
在量子计算机上运行一个实验,以演示使用Estimator原型违反CHSH不等式。 import numpy as npfrom qiskit import QuantumCircuit from qiskit.circuit import Parameter from qiskit.quantum_info import SparsePauliOpfrom qiskit_ibm_runtime import QiskitRuntim…...
GR2——在大规模视频数据集上预训练且机器人数据上微调,随后预测动作轨迹和视频(含GR1详解)
前言 上个月的24年10.9日,我在朋友圈看到字节发了个机器人大模型GR2,立马去看了下其论文(当然了,本质是个技术报告) 那天之后,我就一直想解读这个GR2来着 然,意外来了,如此文《OmniH2O——通用灵巧且可全…...
第19节 Node.js Express 框架
Express 是一个为Node.js设计的web开发框架,它基于nodejs平台。 Express 简介 Express是一个简洁而灵活的node.js Web应用框架, 提供了一系列强大特性帮助你创建各种Web应用,和丰富的HTTP工具。 使用Express可以快速地搭建一个完整功能的网站。 Expre…...
【Linux】shell脚本忽略错误继续执行
在 shell 脚本中,可以使用 set -e 命令来设置脚本在遇到错误时退出执行。如果你希望脚本忽略错误并继续执行,可以在脚本开头添加 set e 命令来取消该设置。 举例1 #!/bin/bash# 取消 set -e 的设置 set e# 执行命令,并忽略错误 rm somefile…...
Prompt Tuning、P-Tuning、Prefix Tuning的区别
一、Prompt Tuning、P-Tuning、Prefix Tuning的区别 1. Prompt Tuning(提示调优) 核心思想:固定预训练模型参数,仅学习额外的连续提示向量(通常是嵌入层的一部分)。实现方式:在输入文本前添加可训练的连续向量(软提示),模型只更新这些提示参数。优势:参数量少(仅提…...
反向工程与模型迁移:打造未来商品详情API的可持续创新体系
在电商行业蓬勃发展的当下,商品详情API作为连接电商平台与开发者、商家及用户的关键纽带,其重要性日益凸显。传统商品详情API主要聚焦于商品基本信息(如名称、价格、库存等)的获取与展示,已难以满足市场对个性化、智能…...
dedecms 织梦自定义表单留言增加ajax验证码功能
增加ajax功能模块,用户不点击提交按钮,只要输入框失去焦点,就会提前提示验证码是否正确。 一,模板上增加验证码 <input name"vdcode"id"vdcode" placeholder"请输入验证码" type"text&quo…...
多模态商品数据接口:融合图像、语音与文字的下一代商品详情体验
一、多模态商品数据接口的技术架构 (一)多模态数据融合引擎 跨模态语义对齐 通过Transformer架构实现图像、语音、文字的语义关联。例如,当用户上传一张“蓝色连衣裙”的图片时,接口可自动提取图像中的颜色(RGB值&…...
什么?连接服务器也能可视化显示界面?:基于X11 Forwarding + CentOS + MobaXterm实战指南
文章目录 什么是X11?环境准备实战步骤1️⃣ 服务器端配置(CentOS)2️⃣ 客户端配置(MobaXterm)3️⃣ 验证X11 Forwarding4️⃣ 运行自定义GUI程序(Python示例)5️⃣ 成功效果属于结构型设计模式,它的核心意图是将抽象部分与实现部分分离,使它们可以独立地变化。这种模式通过组合关系来替代继承关系,从而降低了抽象和实现这两个可变维度之间的耦合度。 用例子…...
