Autonomous_Exploration_Development_Environment的学习笔记(三)

已于 2023-10-11 20:39:17 修改
阅读量2.3k 收藏 19
点赞数 7
分类专栏： Tare_planner学习教程 文章标签： c++ 自动驾驶 linux
于 2022-04-26 21:07:40 首次发布

CMU的自主环境探索利用matlab生成前向三次样条路径点

在CMU开源的自主环境探索算法中，机器人或者无人机的主要是靠matlab进行前向三次样条曲线生成路径点的方式进行的，这部分程序在matlab中运行并且生成相应的文件，从而为local_planner提供帮助。

1.采样路径的生成

利用车辆的运动学特性，进行时间维度上的离散，从而通过预测得到未来一段时间机器人的状态，从而实现了采样路径的生成。下面首先分析这一部分的代码。

dis = 1.0;                  // 前向预测的距离 单位:m
angle = 27;					// 车辆转向的角度 单位:度
deltaAngle = angle / 3;		// 将角度按照3°进行平分
scale = 0.65;				// 角度的缩放系数

在上述定义了一些机器人的初始的状态参数，可以根据你的机器人进行参数的修改

pathStartAll = zeros(4, 0);
pathAll = zeros(5, 0);
pathList = zeros(5, 0);
pathID = 0;
groupID = 0;

这些是一些关于路径输出的一些存储值。

for shift1 = -angle : deltaAngle : angle
    wayptsStart = [0, 0, 0;
                   dis, shift1, 0];
    
    pathStartR = 0 : 0.01 : dis;
    pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR);
    
    pathStartX = pathStartR .* cos(pathStartShift * pi / 180);
    pathStartY = pathStartR .* sin(pathStartShift * pi / 180);
    pathStartZ = zeros(size(pathStartX));
    
    pathStart = [pathStartX; pathStartY; pathStartZ; ones(size(pathStartX)) * groupID];
    pathStartAll = [pathStartAll, pathStart];
    
    for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1
        for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2
                waypts = [pathStartR', pathStartShift', pathStartZ';
                          2 * dis, shift2, 0;
                          3 * dis - 0.001, shift3, 0;
                          3 * dis, shift3, 0];

                pathR = 0 : 0.01 : waypts(end, 1);
                pathShift = spline(waypts(:, 1), waypts(:, 2), pathR);

                pathX = pathR .* cos(pathShift * pi / 180);
                pathY = pathR .* sin(pathShift * pi / 180);
                pathZ = zeros(size(pathX));

                path = [pathX; pathY; pathZ; ones(size(pathX)) * pathID; ones(size(pathX)) * groupID];
                pathAll = [pathAll, path];
                pathList = [pathList, [pathX(end); pathY(end); pathZ(end); pathID; groupID]];
                
                pathID = pathID + 1;

                plot3(pathX, pathY, pathZ);
        end
    end
    
    groupID = groupID + 1
end

上述代码开启了三个for循环，也是路径生成的主要函数，我们一个一个for循环来进行解析；

wayptsStart = [0, 0, 0;
               dis, shift1, 0];
    
    pathStartR = 0 : 0.01 : dis;
    pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR);

上述中定义了wayptsStart矩阵，然后对(0,0)到(1,-27)按照0.01进行了数据的插值.

    pathStartX = pathStartR .* cos(pathStartShift * pi / 180);
    pathStartY = pathStartR .* sin(pathStartShift * pi / 180);
    pathStartZ = zeros(size(pathStartX));
    
    pathStart = [pathStartX; pathStartY; pathStartZ; ones(size(pathStartX)) * groupID];
    pathStartAll = [pathStartAll, pathStart];

这些变量主要是进行路径起始点的转换与存储的，最终所有的路径都存在了pathStartAll变量中。

    for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1
        for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2
                waypts = [pathStartR', pathStartShift', pathStartZ';
                          2 * dis, shift2, 0;
                          3 * dis - 0.001, shift3, 0;
                          3 * dis, shift3, 0];

                pathR = 0 : 0.01 : waypts(end, 1);
                pathShift = spline(waypts(:, 1), waypts(:, 2), pathR);

                pathX = pathR .* cos(pathShift * pi / 180);
                pathY = pathR .* sin(pathShift * pi / 180);
                pathZ = zeros(size(pathX));

                path = [pathX; pathY; pathZ; ones(size(pathX)) * pathID; ones(size(pathX)) * groupID];
                pathAll = [pathAll, path];
                pathList = [pathList, [pathX(end); pathY(end); pathZ(end); pathID; groupID]];
                
                pathID = pathID + 1;

                plot3(pathX, pathY, pathZ);
        end
    end

这是内层的两个循环，可以看到shift2在一个值固定的情况下，shift3开始循环，按照上述的循环方式，一次shift3生成一条路径，编号加1，这样一个固定的shift2的情况下，会生成7条路径，而一个shift2循环会循环7次，所以在两层内循环中会生成49条路径。此时路径组的groupID=0，path_IDmax =49。由于在插值的时候，每一个shitf2下的七条路径都会经经过(1，27)点和(2，shift2)点，所以就会出现下图中每组路径有两个交点的情况。至于为什么这么设计，楼主也没想明白，根据后续算法需求，需要进行探测距离的转换，所以才这么设计了交点吧。
在这里插入图片描述

整个循环结束后，我们可以得到如下的路径图。
在这里插入图片描述

从上图中可以看出，总共有7个group，每个group有49条路径，总共有343条路径。如果是无人机的话，由于具有z轴方向的路径，所以无人机的路径会更多。

fileID = fopen('startPaths.ply', 'w');
fprintf(fileID, 'ply\n');
fprintf(fileID, 'format ascii 1.0\n');
fprintf(fileID, 'element vertex %d\n', size(pathStartAll, 2));
fprintf(fileID, 'property float x\n');
fprintf(fileID, 'property float y\n');
fprintf(fileID, 'property float z\n');
fprintf(fileID, 'property int group_id\n');
fprintf(fileID, 'end_header\n');
fprintf(fileID, '%f %f %f %d\n', pathStartAll);
fclose(fileID);

fileID = fopen('paths.ply', 'w');
fprintf(fileID, 'ply\n');
fprintf(fileID, 'format ascii 1.0\n');
fprintf(fileID, 'element vertex %d\n', size(pathAll, 2));
fprintf(fileID, 'property float x\n');
fprintf(fileID, 'property float y\n');
fprintf(fileID, 'property float z\n');
fprintf(fileID, 'property int path_id\n');
fprintf(fileID, 'property int group_id\n');
fprintf(fileID, 'end_header\n');
fprintf(fileID, '%f %f %f %d %d\n', pathAll);
fclose(fileID);

fileID = fopen('pathList.ply', 'w');
fprintf(fileID, 'ply\n');
fprintf(fileID, 'format ascii 1.0\n');
fprintf(fileID, 'element vertex %d\n', size(pathList, 2));
fprintf(fileID, 'property float end_x\n');
fprintf(fileID, 'property float end_y\n');
fprintf(fileID, 'property float end_z\n');
fprintf(fileID, 'property int path_id\n');
fprintf(fileID, 'property int group_id\n');
fprintf(fileID, 'end_header\n');
fprintf(fileID, '%f %f %f %d %d\n', pathList);
fclose(fileID);

从上述程序中可以看出存储了三个文件startPaths.ply,paths.ply,pathList.ply 三个文件，其中startPaths.ply中存储的是记录了第一次采样的路径点，是一个4×707的矩阵，其中一个矩阵大小为{x,y,z,groupID}。这里解释一下为啥是707个矩阵，是一个这个路径只保存了0-1之间的路径，一条路径只有101个点，所以7组路径是707个路径点。

pathAll:该文件则记录了三次采样所生成的所有路径点，是一个5 ∗ 103243 的矩阵，即在每一个路径组中，包含14749个路径点（103243/7 = 14749）。而每一条路径包括301个路径点（14749/7/7 = 301）。

pathList:该文件记录了每条路径的最后一个路径点，是一个5×343的矩阵，即7×7×7个末端点。

2.体素网格产生

对于碰撞检查，使用覆盖了传感器范围的体素网格。根据样条距离，传感器范围为3.2*4.5,在该区域生成体素网格，如下图所示，在这里考虑了车辆半径的遮挡。在生成体素网格时，定义如下参数，

voxelSize = 0.02; // 体素方格大小
searchRadius = 0.45; //
offsetX = 3.2;  // 范围
offsetY = 4.5;
voxelNumX = 161; //数量 offsetX/voxelSize 
voxelNumY = 451;

在外层循环中，从外向内计算，同采样的路径一样，靠近车体的位置Y的宽度也会随着scaleY减小。

for indX = 0 : voxelNumX - 1
    x = offsetX - voxelSize * indX;
    scaleY = x / offsetX + searchRadius / offsetY * (offsetX - x) / offsetX;
    for indY = 0 : voxelNumY - 1
        y = scaleY * (offsetY - voxelSize * indY);
    end
end

面对scaleY的计算详细说明：
在这里插入图片描述
根据相似三角形，可以算出黑色框包围的地方，这些地方就是需要生成体素网格的地方，按照提速网格的大小可以直接遍历进行生成，如果需要推导公式，则参考链接(https://blog.csdn.net/qq_39266065/article/details/119734986?spm=1001.2014.3001.5502)。

3.碰撞检测

在生成体素网格后，为了在局部路径的筛选中更高效，将每个网格与路径索引离线计算。通过查找路径集合中的路径点周围指定距离内的邻居，并且该邻居点属于体素网格的集合。最终得到每一个体素网格与待遮挡路径之间的索引关系表。
rangesearch()函数：查找某个点周围指定距离内的所有邻居。

[ind, dis] = rangesearch(pathAll(1 : 2, :)', voxelPoints, searchRadius);

最后记录了每个记录每个体素网格附近对应的路径号，从而生成了碰撞检测的文件。

名字好难想啊啊啊啊
关注
  • 7
    点赞
  • 踩
  • 19
    收藏
    觉得还不错? 一键收藏
  • 6
    评论
专栏目录
导航局部路径规划eb算法的详细代码注释
03-25
导航局部路径规划eb算法的详细代码注释，里面包括自己的见解和debug测试结果，特别适合初学者进行研究学习。
CMU团队开源的局部路径导航算法 localPlanner 代码详解以及如何适用于现实移动机器人
热门推荐
秃头队长
09-21 1万+
开源了很久的局部规划器算法，实际使用效果也非常不错，不过一直没仔细去阅读，最近有时间对这部分代码仔细梳理一下。 在阅读localPlanner局部路径规划代码之前，首先需要了解其局部路径的生成过程。 详细解析可参考博客：Matlab用采样的离散点做前向模拟三次样条生成路径点 GitHub地址：https://github.com/HongbiaoZ/autonomous_exploration_development_environment/tree/noetic/src/local_planner Aut
一.无人车导航：CMU团队开源自主导航和规划算法框架
weixin_36773706的博客
10-13 4034
autonomous_exploration_development_environment是CMU团队开源出来的无人车仿真环境，可以自己录制地图，然后部署在仿真环境中，跑自己写的规划和自动导航算法。TARE Planner和DSV Planner是CMU团队开源出来的自主探索算法框架，而FAR Planner 是CMU团队开源出来的规划算法框架。安装仿真环境安装依赖和源码TARE Planner安装运行TARE Planner效果：DSV Planner安装安装依赖安装源码。
【CMU-自主导航与规划】Autonomous Exploration Development Environment 配置与运行
hongliyu_lvliyu的博客
01-30 1548
CMU团队无人车自主导航与规划仿真平台 Autonomous Exploration Development Environment
CMU-自主探索导航系统（TARE & FAR Planner）学习-All in one
qq_42994487的博客
04-22 9194
CMU-自主探索导航系统（TARE & FAR Planner）学习-All in one
Matlab用采样的离散点做前向模拟三次样条生成路径点
秃头队长
09-16 4008
源码地址：https://www.cmu-exploration.com/development-environment 用采样的离散点做前向模拟，基于机器人当前状态，预测一段范围内可能出现的情况。 for shift1 = -angle : deltaAngle : angle %角度 wayptsStart = [0, 0, 0; 3, shift1, 0]; pathStartR = 0 : 0.01 : dis; pathStartS
Autonomous_Exploration_Development_Environment的学习笔记(一)
无人驾驶好难啊啊啊的博客
03-26 4835
Tare_Planner的学习笔记(一) CMU机器人研究所于2021年7月开源全套移动机器人自主导航和探索框架,其主要算法都出自近两年CMU发出的顶会论文.该框架主要分为自主探索环境和探索planner两部分: 其主要效果在链接中主要表现出来,笔者看到后很是震撼，所以接下来将针对tare_planner算法进行一系列的学习，并且会将学习心得记录在博客上，欢迎有共同爱好的朋友一起学习探讨。 一、自主探索开发环境 该环境旨在利用系统开发和机器人部署进行地面自主导航和探索。包含各种仿真环境、自主导航模块（如防撞
CMU的local_planner学习笔记
qq_35971623的博客
01-31 1638
ros::Subscriber subOdometry = nh.subscribe//当接收到/state_estimation话题消息更新时启动odometryHandler回调函数 （消息格式：nav_msgs::Odometry消息内容：关于机器人或车辆的位置和方向（姿态）信息）pcl::PointCloud::Ptr freePaths(new pcl::PointCloud());
Autonomous_Obstacle_Avoidance_Car.rar_Avoding car_SIMULINK_ardui
07-14
标题中的"Autonomous_Obstacle_Avoidance_Car"指的是一个自动避障汽车项目，而"Ardui"通常指的是Arduino，这是一个开源电子平台，常用于DIY电子项目和机器人制作。"SIMULINK_ardui"表明这个项目是通过Simulink进行...
RaimSt.rar_Autonomous_FAULT DETECTION _RAIM_基线 matlab_故障检测机
07-15
该MATLAB程序确定用于非精确接近飞行阶段的接收机自主完整性监测（RAIM）基线标准算法（恒速率算法）和指定的输入数据集的可用性和故障标志（故障检测）。
FLAIRS00-001_Autonomous_
09-29
标题“FLAIRS00-001_Autonomous_”和描述“Autonomous Automobile Behavior”涉及的主题是自动驾驶汽车的行为研究。在这个领域，我们探讨的是如何让车辆在没有人类驾驶员干预的情况下安全、有效地运行。这一技术是...
PDF FILES_nonlinearcontrol_Autonomous_ProgrammableLogic_Discreti
09-29
标题中的“PDF FILES_nonlinearcontrol_Autonomous_ProgrammableLogic_Discreti”暗示了讨论的主题涉及非线性控制、自主系统、可编程逻辑控制器（PLC）以及离散化等概念。这些关键词都是信息技术与自动化领域的重要...
ihar_Intelligentsystem_IntelligentSystems_Autonomous_matlab_
09-30
/20200831/1a26f9d6ba57683bc650091bf54a94f3.zipA great deal of research effort is being expended at present to developintelligent autonomous systems. A brief overview of intelligent autonomous system ...
CMU,[gazebo]process has died.报错解决.VLP-16.velodyne
skjfhsef的博客
08-19 979
cmu，[gazebo]process has died. velodyne.
Autonomous_Exploration_Development_Environment的学习教程(四)
无人驾驶好难啊啊啊的博客
05-07 3045
tare_planner学习教程(四) 本教程主要完成local_planner的程序解析，这是该序列教程的第四篇。由于最近楼主的事情较多，更新的较慢，有感兴趣的同学留言，楼主可以根据你们的需要进行后面内容的更新。 1.local_planner的参数解析 下面是local_planner的参数，具体的参数见下表 #define PLOTPATHSET 1 // 显示路径集的控制量 string pathFolder;
初始C++
最新发布
2302_81016149的博客
07-08 806
在C语言我们学习函数的时候，我们学习过生命周期这个概念。那么，什么是生命周期呢？生命周期通常指的是一个对象或变量从创建到销毁所经历的时间段。那么，命名空间是什么？咱们可以这样理解：在这个空间的生命周期内，对这个空间内的全部标识符起一个名字，可对其进行引用，可类似与C语言中的结构体。#include<stdlib.h>//此处不包此头文件代码可正常运行，那该如何解决此处情况呢？int main()//此处编译不通过原因为：在不包头文件前为变量，包含后为函数。return 0;
"ros_autonomous_slam" 自主探索
07-25
你好！"ros_autonomous_slam" 是一个基于ROS（机器人操作系统）的自主探索和建图系统。它结合了自主导航和SLAM（同时定位与地图构建）技术，使机器人能够在未知环境中进行自主探索并创建地图。 通过ROS的导航功能包，"ros_autonomous_slam" 可以实现机器人的路径规划和障碍物避障。同时，它还利用SLAM算法来同时估计机器人的位置和构建环境地图。这样，机器人就能够在未知环境中实现自主导航和探索。 "ros_autonomous_slam" 提供了一套完整的软件框架和算法，使开发者能够方便地构建自主探索和建图的机器人系统。开发者可以根据具体的需求进行配置和定制，以实现不同场景下的自主探索任务。 希望这个回答能对你有帮助！如果你还有其他问题，请随时提问。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 6
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值