路径规划 | 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 PoseGoal Pose选项设置起点和终点位姿

在这里插入图片描述

3.3 选择规划器规划

RMPVPlanner选项中选择具体的规划器,点击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深度学习实战》
  • 《机器学习强基计划》
  • 《运动规划实战精讲》

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇

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

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

相关文章

十四届蓝桥杯STEMA考试Python真题试卷第二套第五题

来源:十四届蓝桥杯STEMA考试Python真题试卷第二套编程第五题 本题属于迷宫类问题,适合用DFS算法解决,解析中给出了Python中 map() 和列表推导式的应用技巧。最后介绍了DFS算法的两种常见实现方式——递归实现、栈实现,应用场景——迷宫类问题、图的连通性、树的遍历、拓朴排…

【CSS】——基础入门常见操作

阿华代码&#xff0c;不是逆风&#xff0c;就是我疯 你们的点赞收藏是我前进最大的动力&#xff01;&#xff01; 希望本文内容能够帮助到你&#xff01;&#xff01; 目录 一&#xff1a;CSS引入 二&#xff1a;CSS对元素进行美化 1&#xff1a;style修饰 2&#xff1a;选…

RV1126-SDK学习之OSD实现原理

RV1126-SDK学习之OSD实现原理 前言 本文简单记录一下我在学习RV1126的SDK当中OSD绘制的原理的过程。 在学习OSD的过程当中&#xff0c;可能需要补充的基础知识&#xff1a; OSD是什么&#xff1f; BMP图像文件格式大致组成&#xff1f; 图像调色&#xff08;Palette&#…

Vehicle OS软件平台解决方案

在智能汽车快速迭代的趋势之下&#xff0c;广义操作系统Vehicle OS应运而生&#xff0c;针对应用软件开发周期缩短和底层硬件迭代速度加快的背景&#xff0c;Vehicle OS将应用软件开发和底层硬件迭代解耦。它降低了迭代工作量并节约成本&#xff0c;用标准化的接口来助力软件定…

Chromium Mojo(IPC)进程通信演示 c++(1)

网上搜索关于mojo教程 多数都是理论 加上翻译谷歌mojo文档的&#xff0c;但是如何自定义两个进程使用mojo通信呢&#xff1f;看下面的完整例子介绍&#xff1a;&#xff08;本人也是参考谷歌代码例子改编而成&#xff09; 本文演示了client.exe和service.exe 通过mojo::Incomin…

sparkSQL面试题

一、查询所有数学课程成绩大于语文课程成绩的学生学号 数据 1,yuwen,43 1,shuxue,55 2,yuwen,77 2,shuxue,88 3,yuwen,98 3,shuxue,65 3,yingyu,88 基本步骤&#xff1a; 进行行转列比较语文与数学的成绩 SQL代码&#xff1a; with t1 as(SELECT id,sum(if(name yuwen,sc…

算法|牛客网华为机试21-30C++

牛客网华为机试 上篇&#xff1a;算法|牛客网华为机试10-20C 文章目录 HJ21 简单密码HJ22 汽水瓶HJ23 删除字符串中出现次数最少的字符HJ24 合唱队HJ25 数据分类处理HJ26 字符串排序HJ27 查找兄弟单词HJ28 素数伴侣HJ29 字符串加解密HJ30 字符串合并处理 HJ21 简单密码 题目描…

浅谈QT中Tab键的切换逻辑

浅谈QT中Tab键的切换逻辑 无意中发现在输入界面中按下Tab键时&#xff0c;没有按照预想的顺序切换焦点事件&#xff0c;如下图所示 这个现象还是很有趣&#xff0c;仔细观察了下&#xff0c;默认的切换顺序是按照控件拖入顺序&#xff0c;那么知道了这个问题想要解决起来就很简…

科研绘图系列:R语言组合连线图和箱线图(linechart+boxplot)

文章目录 介绍加载R包数据数据预处理画图1画图2系统信息介绍 连线图(Line Chart)是一种常用的数据可视化图表,它通过将一系列数据点用直线段连接起来来展示数据随时间或有序类别变化的趋势。以下是连线图可以表示的一些内容: 时间序列数据:展示数据随时间变化的趋势,例如…

PKG_CHECK_MODULES(FUSE,fuse)

运行 ./configure 命令报错如下&#xff1a; ./configure: line 13934: syntax error near unexpected token FUSE,fuse ./configure: line 13934: PKG_CHECK_MODULES(FUSE,fuse)解决方案&#xff1a; 命令窗口运行如下命令&#xff0c;安装 pkg-config&#xff1a; sudo …

react18中redux-promise搭配redux-thunk完美简化异步数据操作

用过redux-thunk的应该知道&#xff0c;操作相对繁琐一点&#xff0c;dispatch本只可以出发plain object。redux-thunk让dispatch可以返回一个函数。而redux-promise在此基础上大大简化了操作。 实现效果 关键逻辑代码 store/index.js import { createStore, applyMiddlewar…

Lucene分析器的详细使用(5)

文章目录 第5章 分析器5.1 分析器的组成5.1.1 字符过滤器1&#xff09;HTMLStripCharFilter2&#xff09;PatternReplaceCharFilter3&#xff09;MappingCharFilter4&#xff09;Luke使用字符过滤器 5.1.2 分词器1&#xff09;StandardTokenzier2&#xff09;keywordTokenizer3…

selinux和防火墙

SElinux 1、selinux代表的什么&#xff1f; SELinux是Security-Enhanced Linux的缩写&#xff0c;意思是安全强化的linux。 SELinux 主要由美国国家安全局&#xff08;NSA&#xff09;开发&#xff0c;当初开发的目的是为了避免资源的误用。 SELinux是对程序、文件等权限设置依…

CentOS 7 安装 ntp,自动校准系统时间

1、安装 ntp yum install ntp 安装好后&#xff0c;ntp 会自动注册成为服务&#xff0c;服务名称为 ntpd 2、查看当前 ntpd 服务的状态 systemctl status ntpd 3、启动 ntpd 服务、查看 ntpd 服务的状态 systemctl start ntpdsystemctl status ntpd 4、设置 ntpd 服务开机启…

Oracle OCP认证考试考点详解082系列11

题记&#xff1a; 本系列主要讲解Oracle OCP认证考试考点&#xff08;题目&#xff09;&#xff0c;适用于19C/21C,跟着学OCP考试必过。 51. 第51题&#xff1a; 题目 51.View the Exhibit and examine the description of the tables You execute this SQL statement Whi…

C#属性 Property

属性Property不是变量。 它们是由名为访问器方法来实现的一种方法。 实例属性表示的是实例的某个数据&#xff0c;通过这个数据反映实例当前的状态 静态属性表示的是类型的某个数据&#xff0c;通过这个数据反映类型当前的状态 意义&#xff1a; 防止恶意赋值(通过属性间接访问…

Spring框架的事务管理

目录 一、spring框架事务管理相关的类 1.PlatformTransactionManager接口 2.TransactionDefinition接口 二、spring框架声明式事务管理 1.配置文件的方式 &#xff08;1&#xff09;配置文件 &#xff08;2&#xff09;业务层 &#xff08;3&#xff09;持久层 &#…

angular实现list列表和翻页效果

说明&#xff1a;angular实现list列表和翻页效果 上一页 当前页面 下一页 效果图&#xff1a; step1: E:\projectgood\ajnine\untitled4\src\app\car\car.component.css .example-form-fields {display: flex;align-items: flex-start; }mat-list-item{background: antiquew…

PHP常量

PHP 中的常量是指一旦定义后将不能被改变的标识符。 常量可以用const和define&#xff08;&#xff09;来定义。 PHP常量的特性 不变性: 常量一旦定义&#xff0c;其值不能改变。全局作用域: 常量在定义后&#xff0c;可以在整个脚本的任何地方使用&#xff0c;无需使用 glo…

服务器作业(2)

架设一台NFS服务器&#xff0c;并按照以下要求配置 关闭防火墙 [rootlocalhost ~]# systemctl stop firewalld [rootlocalhost ~]# setenforce 0 配置文件设置&#xff1a; [rootlocalhost ~]# vim /etc/exports 1、开放/nfs/shared目录&#xff0c;供所有用户查询资料 共享…