路径规划算法——RRTStar
RRTStar算法概述
RRTStar算法的主要特征是能快速的找出初始路径,之后随着采样点的增加,不断地进行优化直到找到目标点或者达到设定的最大循环次数。RRTStar算法是渐进优化的,也就是随着迭代次数的增加,得出的路径是越来越优化的,而且永远不可能在有限的时间中得出最优的路径。
MATLAB代码如下
链接:https://pan.baidu.com/s/1028q7QxFsPKjnJ5mURatWg
提取码:5ncl
RRTStar算法代码分析
1、RRTStar函数
clear all; close all; clc;
%% 参数初始化
x_I = 1; y_I = 1; % 设置初始点
x_G = 730; y_G = 730; % 设置目标点
GoalThreshold = 30; % 设置目标点阈值
Delta = 30; % 设置扩展步长 default:30
RadiusForNeib = 80; % rewire的范围,半径r
MaxIterations = 2500; % 最大迭代次数
UpdateTime = 50; % 更新路径的时间间隔
DelayTime = 0.0; % 绘图延迟时间
%% 建树初始化:T是树,v是节点
T.v(1).x = x_I; % 把起始节点加入到T中
T.v(1).y = y_I;
T.v(1).xPrev = x_I; % 节点的父节点坐标:起点的父节点是其本身
T.v(1).yPrev = y_I;
T.v(1).totalCost = 0; % 从起始节点开始的累计cost,这里取欧氏距离
T.v(1).indPrev = 0; % 父节点的索引
%% 开始构建树
figure(1);
ImpRgb = imread('map.png');
Imp = rgb2gray(ImpRgb);
imshow(Imp)
xL = size(Imp,1); % 地图x轴长度
yL = size(Imp,2); % 地图y轴长度
hold on
plot(x_I, y_I, 'mo', 'MarkerSize',10, 'MarkerFaceColor','m'); % 绘制起点和目标点
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
count = 1;
pHandleList = [];
lHandleList = [];
resHandleList = [];
findPath = 0;
update_count = 0;
path.pos = [];
for iter = 1:MaxIterations%Step 1: 在地图中随机采样一个点x_rand (Sample)x_rand = [unifrnd(0,xL),unifrnd(0,yL)]; %产生随机点(x,y)%Step 2: 遍历树,从树中找到最近邻近点x_near (Near)minDis = sqrt((x_rand(1) - T.v(1).x)^2 + (x_rand(2) - T.v(1).y)^2);minIndex = 1;for i = 2:size(T.v,2) % T.v按行向量存储,size(T.v,2)获得节点总数distance = sqrt((x_rand(1) - T.v(i).x)^2 + (x_rand(2) - T.v(i).y)^2); %两节点间距离if(distance < minDis)minDis = distance;minIndex = i; end endx_near(1) = T.v(minIndex).x; % 找到当前树中离x_rand最近的节点x_near(2) = T.v(minIndex).y;temp_parent = minIndex; % 临时父节点的索引temp_cost = Delta + T.v(minIndex).totalCost; % 临时累计代价%Step 3: 扩展得到x_new节点 (Steer)theta = atan2((x_rand(2) - x_near(2)),(x_rand(1) - x_near(1)));x_new(1) = x_near(1) + cos(theta) * Delta;x_new(2) = x_near(2) + sin(theta) * Delta; %plot(x_rand(1), x_rand(2), 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');%plot(x_new(1), x_new(2), 'bo', 'MarkerSize',10, 'MarkerFaceColor','b');% 检查节点是否是collision-freeif ~collisionChecking(x_near,x_new,Imp) continue; %有障碍物end%Step 4: 在以x_new为圆心,半径为R的圆内搜索节点 (NearC)disToNewList = []; %每次循环要把队列清空nearIndexList = [];for index_near = 1:countdisTonew = sqrt((x_new(1) - T.v(index_near).x)^2 + (x_new(2) - T.v(index_near).y)^2);if(disTonew < RadiusForNeib) % 满足条件:欧氏距离小于RdisToNewList = [disToNewList disTonew]; % 满足条件的所有节点到x_new的costnearIndexList = [nearIndexList index_near]; % 满足条件的所有节点基于树T的索引endend %Step 5: 选择x_new的父节点,使x_new的累计cost最小 (ChooseParent)for cost_index = 1:length(nearIndexList) % cost_index是基于disToNewList的索引,不是整棵树的索引costToNew = disToNewList(cost_index) + T.v(nearIndexList(cost_index)).totalCost;if(costToNew < temp_cost) % temp_cost为通过minDist节点的路径的costx_mincost(1) = T.v(nearIndexList(cost_index)).x; % 符合剪枝条件节点的坐标x_mincost(2) = T.v(nearIndexList(cost_index)).y;if ~collisionChecking(x_mincost,x_new,Imp) continue; %有障碍物endtemp_cost = costToNew;temp_parent = nearIndexList(cost_index);endend%Step 6: 将x_new插入树T (AddNodeEdge)count = count+1; %最新节点的索引T.v(count).x = x_new(1); T.v(count).y = x_new(2); T.v(count).xPrev = T.v(temp_parent).x; T.v(count).yPrev = T.v(temp_parent).y;T.v(count).totalCost = temp_cost; T.v(count).indPrev = temp_parent; %其父节点x_near的indexl_handle = plot([T.v(count).xPrev, x_new(1)], [T.v(count).yPrev, x_new(2)], 'b', 'Linewidth', 2);p_handle = plot(x_new(1), x_new(2), 'ko', 'MarkerSize', 4, 'MarkerFaceColor','k');pHandleList = [pHandleList p_handle]; %绘图的句柄索引即为countlHandleList = [lHandleList l_handle];pause(DelayTime);%Step 7: 剪枝 (rewire)for rewire_index = 1:length(nearIndexList)if(nearIndexList(rewire_index) ~= temp_parent) % 若不是之前计算的最小cost的节点newCost = temp_cost + disToNewList(rewire_index); % 计算neib经过x_new再到起点的代价 if(newCost < T.v(nearIndexList(rewire_index)).totalCost) % 需要剪枝x_neib(1) = T.v(nearIndexList(rewire_index)).x; % 符合剪枝条件节点的坐标x_neib(2) = T.v(nearIndexList(rewire_index)).y;if ~collisionChecking(x_neib,x_new,Imp) continue; %有障碍物endT.v(nearIndexList(rewire_index)).xPrev = x_new(1); % 对该neighbor信息进行更新T.v(nearIndexList(rewire_index)).yPrev = x_new(2);T.v(nearIndexList(rewire_index)).totalCost = newCost;T.v(nearIndexList(rewire_index)).indPrev = count; % x_new的索引%delete(pHandleList());%delete(lHandleList(nearIndexList(rewire_index)));lHandleList(nearIndexList(rewire_index)) = plot([T.v(nearIndexList(rewire_index)).x, x_new(1)], [T.v(nearIndexList(rewire_index)).y, x_new(2)], 'r', 'Linewidth', 2);%pHandleList = [pHandleList p_handle]; %绘图的句柄索引即为count%lHandleList = [lHandleList l_handle];endendend%Step 8:检查是否到达目标点附近 disToGoal = sqrt((x_new(1) - x_G)^2 + (x_new(2) - y_G)^2);if(disToGoal < GoalThreshold && ~findPath) % 找到目标点,此条件只进入一次findPath = 1;count = count+1; %手动将Goal加入到树中Goal_index = count;T.v(count).x = x_G; T.v(count).y = y_G; T.v(count).xPrev = x_new(1); T.v(count).yPrev = x_new(2);T.v(count).totalCost = T.v(count - 1).totalCost + disToGoal;T.v(count).indPrev = count - 1; %其父节点x_near的indexendif(findPath == 1)update_count = update_count + 1;if(update_count == UpdateTime)update_count = 0;j = 2;path.pos(1).x = x_G; path.pos(1).y = y_G;pathIndex = T.v(Goal_index).indPrev;while 1 path.pos(j).x = T.v(pathIndex).x;path.pos(j).y = T.v(pathIndex).y;pathIndex = T.v(pathIndex).indPrev; % 沿终点回溯到起点if pathIndex == 0breakendj=j+1;end for delete_index = 1:length(resHandleList)delete(resHandleList(delete_index));endfor j = 2:length(path.pos)res_handle = plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'g', 'Linewidth', 4);resHandleList = [resHandleList res_handle];endendend pause(DelayTime); %暂停DelayTime s,使得RRT*扩展过程容易观察
endfor delete_index = 1:length(resHandleList)delete(resHandleList(delete_index));
end
for j = 2:length(path.pos)res_handle = plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'g', 'Linewidth', 4);resHandleList = [resHandleList res_handle];
enddisp('The path is found!');
障碍物函数
function feasible=collisionChecking(startPose,goalPose,map)feasible=true;
%dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));
dir = atan2(goalPose(2)-startPose(2),goalPose(1)-startPose(1));
for r = 0:0.5:sqrt(sum((startPose-goalPose).^2)) %以0.5为步长,从startPose开始递增的检查是否有障碍%posCheck = startPose + r.*[sin(dir) cos(dir)]; %直线距离增加0.5后的坐标posCheck = startPose + r.*[cos(dir) sin(dir)]; %直线距离增加0.5后的坐标%将一个小数(x,y)向4个方向取整,确保该点没有触碰障碍if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) ...&& feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) ...&& feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))feasible = false;break;endif ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map)feasible = false; endendfunction feasible = feasiblePoint(point,map)
feasible = true;
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 ...&& point(2)<=size(map,2) && map(point(2),point(1))==255)feasible = false; %有障碍
end
运算结果
路径规划算法——RRTStar相关推荐
- RRT、RRT-connect、RRT*等算法、A*等等路径规划算法
1 原始RRT算法运行结果:python,这里以2D_rrt及其衍生相关算法为例(边做边更新) CV搬运工们,先上github连接:(点个赞呗)(不想要拿github包的后面有现成代码)GitHub ...
- 自动驾驶路径规划——基于概率采样的路径规划算法(RRT、RRT*)
目录 1. RRT算法背景 1.1 RRT算法核心思想 1.2 RRT算法优缺点 2. 经典RRT算法 2.1 RRT算法流程 2.2 RRT伪代码 3. 基于目标概率采样 4. RRT*算法 4.1 ...
- 中国物流供应链“零的突破”!阿里路径规划算法入围运筹学“奥斯卡”
鱼羊 发自 凹非寺 量子位 报道 | 公众号 QbitAI 最新消息,中国物流力量,刚在全球运筹和管理科学界的最高荣誉中,实现零的突破! 1月15日,国际运筹学与管理科学学会(INFORMS)公布了2 ...
- 机器人学习--路径规划算法
参考网址:https://mp.weixin.qq.com/s?__biz=MzI5MDUyMDIxNA==&mid=2247520667&idx=3&sn=47ee67915 ...
- 多边形之间相交求交点的算法_路径规划算法总结
本文来自知乎网友@搬砖的旺财,地平线机器人算法工程师.作者根据自己本科和硕士阶段的学习经历,整理归纳了所接触过的规划算法. 1.自主机器人近距离操作运动规划体系 在研究自主运动规划问题之前,首先需建立 ...
- 基于SSD的自动路径规划算法
目录 1.场景需求 2.路径规划算法简介 2.1 .PRM算法简介 2.2.RRT算法简介 3.基于SSD的自动路径规划算法简介 4.基于SSD的自动路径规划算法详解 4.1.利用外置摄像头获取图像或 ...
- 路径规划算法_自动驾驶汽车路径规划算法浅析
自动驾驶汽车的路径规划算法最早源于机器人的路径规划研究,但是就工况而言却比机器人的路径规划复杂得多,自动驾驶车辆需要考虑车速.道路的附着情况.车辆最小转弯半径.外界天气环境等因素. 本文将为大家介绍四 ...
- 多机器人路径规划的代码_知荐 | 地平线机器人算法工程师总结六大路径规划算法...
来源 | 知乎 知圈 | 进"高精度地图社群",请加微信15221054164,备注地图 目录 1 自主机器人近距离操作运动规划体系········1.1 单个自主机器人的规划体系 ...
- python无人机路径规划算法_RRT算法在Python中的实现,快速,拓展,随机,树
""" <基于智能优化与RRT算法的无人机任务规划方法研究>博士论文 <基于改进人工势场法的路径规划算法研究>硕士论文 ""& ...
最新文章
- 递归函数反向显示单链表
- 怎么看我装的sql能不能用_深入浅出sql优化(三)之单表索引优化
- Geoserver的ImageMosaic数据源添加以及服务发布
- Java编程基础12——Eclipse使用Object类型
- ++x 和 x++的区别
- iOS图形处理概论:OpenGL ES,Metal,Core Graphics,Core Image,GPUImage,OpenCV等
- Ubuntu 6.10 发布
- [阅读笔记]《解析卷积神经网络_深度学习实践手册》魏秀参著
- dualbootpatcher下载_多系统软件Dual Boot Patcher教程(多图)
- 安装Sql Server 2000时提示“安装Sql挂起”的解决方案
- 如何用优盘安装服务器操作系统,使用优盘安装服务器
- android模拟器高德地图,【高德地图电脑版】高德地图电脑版官方下载 含安卓模拟器 车机版-趣致软件园...
- 数据挖掘——决策树和K近邻
- 2019-3-13-win10-uwp-使用-ScaleTransform-放大某个元素
- 西门子在华启动“零碳先锋计划”;希尔顿欢朋在华项目签约数突破600 | 美通社头条...
- Haskell大世界+思考
- 【问题】vcenter7升级遇到“Exception in invoking authentication handler unidentifiable C++ exception”
- 著书立说,就现在——IT东方会T-Book出书专场第二期圆满举办
- 在html中 常见的块级元素有哪些,常见的css块级元素有哪些
- 2020年的奋斗目标
热门文章
- 深思考人工智能再获行业嘉奖 人工智能垂直行业最具投资价值企业
- 2022年下半年网络规划设计师下午真题及答案解析
- 【微信小程序】注册小程序账号、做一个案例——你好我的小程序
- 24考研|高等数学的基础概念定理(一)——第一章|函数、极限、连续
- otrs软件_开源ITIL管理工具OTRS简单介绍
- php date 怎么处理 夏令时 冬令时的切换
- OSChina 初十乱弹 ——我说能,你就信啦?
- MATALB- robotic Toolbox中机械手运动时末端运动轨迹保留
- javascript中的this到底是指什么(一)?
- docker-将自己的Linux打包为镜像