【自动驾驶轨迹规划之RRT算法】
目录
1 RRT算法的简介
2 RRT算法原理
2.1 算法流程
2.2 算法伪代码
2.3 算法流程图
3 RRT算法matlab实现
3.1 测试地图
3.2 distance函数
3.3 RRT算法
3.4 动画效果
4 RRT的缺陷
1 RRT算法的简介
天下武功唯快不破,快是 RRT 的最大优势。RRT 的思想是快速扩张一群像树一样的路径以探索空间的大部分区域,找到可行的路径。RRT 算法是一种对状态空间随机采样的算法,通过对采样点进行碰撞检测,避免了对空间的精确建模带来的大计算量,能够有效地解决高维空间和复杂约束的路径规划问题。与PRM类似,该方法是概率完备且非最优的。可以轻松处理障碍物和差分约束(非完整和动力学)的问题,并被广泛应用于机器人路径规划。
2 RRT算法原理
2.1 算法流程
(1)设定初始点 与目标点 ,自行设定状态采样空间
(2)进行随机采样得到采样点 ,如果采样点 在障碍物内,则重新随机采样
(3)若不在障碍物内,计算该采样点 与集合 (已经生成的节点) 中的所有节点之间的距离,得到离得最近的节点 ,再从节点 以步长 走向节点 ,生成一个新的节点 ,若 与 的连线 经过障碍物,则重新随机采样
(4)经过反复迭代,生成一个随机扩展树,当随机扩展树中的子节点进入了我们规定的目标区域,便可以在随机扩展树中找到一条由从初始点到目标点的路径。
2.2 算法伪代码
可以将伪代码与上述算法流程对照起来看
2.3 算法流程图
3 RRT算法matlab实现
3.1 测试地图
%随机生成障碍物
function [f,n1]=ob(n)
f=[];%储存障碍物信息
n1=n;%返回障碍物个数
p=0;
for i=1:nk=1;while(k)D=[rand(1,2)*60+15,rand(1,1)*1+3];%随机生成障碍物的坐标与半径,自行调整if(distance(D(1),D(2),90,90)>(D(3)+5)) %与目标点距离一定长度,防止过多阻碍机器人到达目标点k=0;endfor t=1:p %障碍物之间的距离不能过窄,可自行调整去测试if(distance(D(1),D(2),f(3*t-2),f(3*t-1))<=(D(3)+f(3*t)+5))k=1;endendend%画出障碍物aplha=0:pi/40:2*pi;r=D(3);x=D(1)+r*cos(aplha);y=D(2)+r*sin(aplha);fill(x,y,'k');axis equal;hold on;xlim([0,100]);ylim([0,100]);f=[f,D];p=p+1;%目前生成的障碍物个数
end
hold all;
3.2 distance函数
function f=distance(x,y,x1,y1)f=sqrt((x-x1)^2+(y-y1)^2);
3.3 RRT算法
clc
clear all
[f,n1]=ob(10);%随机生成障碍物
Xinit=[1,1];%定义初始点
Xgoal=[90,90];%定义目标点
plot(Xinit(1),Xinit(2),'ro');
plot(Xgoal(1),Xgoal(2),'ko');
T=[Xinit(1),Xinit(2)];%已生成节点集合用顺序表的数据结构存储
Xnew=Xinit;
D(1)=0;%初始节点的父节点指向0
while distance(Xnew(1),Xnew(2),Xgoal(1),Xgoal(2))>3 %进入目标区域Xrand=round(rand(1,2)*100)+1;%状态采样空间:横纵坐标均为整数,范围1~100k=1;%进入循环while k==1k=0;%初始化采样成功for i=1:n1if distance(Xrand(1),Xrand(2),f(i*3-2),f(i*3-1))<(f(i*3)+1)%判断随机采样点是否在障碍物内k=1;%采样不成功break;endendXrand=round(rand(1,2)*100);%重新采样endmin=10000;for i=1:size(T,2)/2 %遍历已生成节点集合if distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2))<min %得到最近的节点min=distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2));Xnear=[T(2*i-1),T(2*i)];endendstepsize=3/distance(Xnear(1),Xnear(2),Xrand(1),Xrand(2));%以一定步长前进,这里选择3Xnew=[Xrand(1)*stepsize+Xnear(1)*(1-stepsize),Xrand(2)*stepsize+Xnear(2)*(1-stepsize)];t=0;%初始状态未碰撞if Xnear(1)>Xnew(1)caiyang=-0.001;elsecaiyang=0.001;endfor i=Xnear(1):caiyang:Xnew(1)%均匀采样进行碰撞检测for j=1:n1if distance(f(3*j-2),f(3*j-1),i,Xnear(2)+(i-Xnear(1))/(Xnew(1)-Xnear(1))*(Xnew(2)-Xnear(2)))<=(f(3*j)+1)t=1;%代表碰撞break;endendif t==1break;endendif t==0T=[T,Xnew(1),Xnew(2)];for i=1:size(T,2)/2 %遍历已生成节点集合if (T(i*2-1)==Xnear(1))&&(T(i*2)==Xnear(2)) %得到最近的节点的索引D(size(T,2)/2)=i;%记录父节点break;endendplot([Xnew(1),Xnear(1)],[Xnew(2),Xnear(2)],'b-');hold on;pause(0.005);plot(Xnew(1),Xnew(2),'r.');xlim([0,100]);ylim([0,100]);end
end
i=size(T,2)/2;
jg=[i];
while D(i)i=D(i); %通过链表回溯if D(i)~=0jg=[D(i),jg];%存储最短路径通过的节点end
end
Fx=T(jg(1)*2-1);Fy=T(jg(1)*2);
i=2;
while jg(i)~=size(T,2)/2x=T(jg(i)*2-1);y=T(jg(i)*2);plot([x,Fx],[y,Fy],'g-');hold on;pause(0.05);Fx=x;Fy=y;i=i+1;
endplot([T(jg(i)*2-1),Fx],[T(jg(i)*2),Fy],'g-');hold on;pause(0.05);
3.4 动画效果
4 RRT的缺陷
(1)很明显RRT算法得到的路径不是最优的
(2)RRT算法未考虑运动学模型
(3)RRT算法对于狭小的通道的探索性能不好,如下图的对比,有可能探索不到出口
(4)没有启发信息的RRT像无头苍蝇,探索空间完全靠运气,如下图
针对上述缺陷,又出现了很多RRT算法的变种,以后的文章中会介绍。
【自动驾驶轨迹规划之RRT算法】相关推荐
- 【自动驾驶轨迹规划之DWA算法】
目录 1 DWA算法简介 2 运动学模型 2.1直线轨迹模型 2.2 圆弧直线模型 2.3 车辆二自由度模型
- 【自动驾驶轨迹规划之安全行驶走廊】
目录 1 原理剖析 1.1 安全飞行走廊(SFC) 1.2 安全行驶走廊(STC) 1.2.1 车辆外形建模 1.2.2 环境建模 1.3 生成安全行驶走廊的伪代码 1.4 建立约束限制 1.5最优控 ...
- python 非线性规划_自动驾驶运动规划-Hybird A*算法(续)
Hybird A*算法保证生成的路径是车辆可实际行驶的,但它仍然包含很多不必要的车辆转向操作,我们可以对其进行进一步的平滑和优化. Objective Function 对于Hybird A*生成的车 ...
- 【自动驾驶轨迹规划之dubins曲线与reeds-shepp曲线】
目录 1 dubins曲线的简介 2 dubins 曲线的实现与计算 2.1 找到圆心 2.2 找到切点 2.3 画出dubins曲线并计算路径长度 2.4 车辆外形建模 2.5 车辆沿dubins曲 ...
- 【自动驾驶轨迹规划之地图结构】
目录 1 基础地图结构的分类 1.1 2D地图 1.1.1 栅格地图 1.1.2 拓扑地图 1.1.3 导航网格图 1.2 3D地图 1.2.1 栅格地图 1.2.2 八叉树地图 1.2.3 点云地图 ...
- 《整体决策的统一框架和基于时空的高速路自动驾驶轨迹规划》论文分析
文献分析 这篇<整体决策的统一框架和基于时空的高速路自动驾驶轨迹规划>论文,针对过往前任研究的一些不足,建立了决策规划的三个模块,这三个模块针对短期(10hz,一秒运行十次),中期(1hz ...
- 自动驾驶路径规划五大常用算法(Dijkstra/人工势场/图搜索等)
编辑 | 希骥智能网联汽车 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 点击进入→自动驾驶之心[规划控制]技术交流群 后台回复[规划控制综述]获取自动驾驶 ...
- 【自动驾驶轨迹规划之最优控制】
目录 1 车辆动力学约束 1.1 模型的采取 1.2 模型的假设 1.3 运动学模型的微分方程组 1.4 控制变量的选择 2 边值约束 2.1 简单的约束 2.2 较复杂的约束 3 碰撞躲避约束 4 ...
- 【自动驾驶决策规划】PRM算法
1 基于采样的路径规划方法 路径搜索常用方式之一是基于网格的的方法(grid-based method),如A*算法,但基于网格的方法复杂度较高,与求解空间的维度相关,且得到的路径比较僵硬,对于车辆或 ...
- 【自动驾驶轨迹规划之路径规划总结】
目录 1 路径规划问题的介绍 2 路径规划算法的分类 3 各种路径规划算法的简介 4 路径规划算法的优缺点分析 本篇文章参考Apollo轨迹规划算法综述. 接下来的几篇文章将主要围绕着全局路径规划的常 ...
最新文章
- 专属自己的二维游戏引擎【二】
- 2.7 迁移学习-深度学习第三课《结构化机器学习项目》-Stanford吴恩达教授
- 5个无聊Python程序,用Python整蛊你的朋友们吧
- P3295-[SCOI2016]萌萌哒【ST表,并查集】
- 华为平板电脑_华为对标微软推平板电脑 能否打造软件生态?
- set column oracle,Oracle 用一个表字段更新另一个表字段三种方法
- 使用RNN预测股票价格系列二
- TikTok:将禁止加密货币广告投放
- MSSQL数据库中row_number()函数
- 数据--第51课 - 二叉排序树
- 2018年度总结 - 黑子
- 远程读取mysql_远程获取数据库和文件的方法
- 1236mysql_mysql报1236错误解决方法
- NX/UG二次开发简单干涉
- java 获取IP地址 无法获取到真实的IP地址
- 1.2 控制LED闪烁
- 容联云通讯_提供网络通话、视频通话、视频会议、云呼叫中心、IM等融合通讯能力开放平台。...
- 潘石屹陈志武谈房地产:北京等城市泡沫已很严重(载自新浪)
- saltstack自动化运维一键部署rpm安装httpd和源码安装nginx
- Class类的简单介绍
热门文章
- 计步 android 源码,android版简易计步器源码
- 最新华为悦盒V9_V9U_V9E_V9A_V8通用傻瓜式破解
- 手机APP测试如何进行兼容性测试?
- [Hello World教程] 使用HBuilder和Uni-app 生成一个简单的微信小程序DEMO
- 一键解决局域网共享之批处理
- 《码出高效-阿里巴巴Java开发手册-泰山版》阿里最新出品的泰山版Java开发手册,告别垃圾代码
- 【WPF】调用线程必须为STA,因为许多UI组件都需要
- html手机验证码登录页面代码,htmlunit 模拟登录 数字验证码(示例代码)
- Activiti6教程一
- Activiti6--入门学习--监听器