RRT（快速随机搜索树）算法原理及代码实践

RRT算法简介

RRT 算法为一种递增式的路径规划算法，算法不断在搜索空间中随机生成采样点，如果该点位于无碰撞位置，则寻找搜索树中离该节点最近的结点为基准结点，由基准结点出发以一定步长朝着该随机结点进行延伸，延伸线的终点所在的位置被当做新的有效结点加入搜索树中。这个搜索树的生长过程一直持续，直到目标结点与搜索树的距离在一定范围以内时终止。随后搜索算法在搜索树中寻找一条连接起点到终点的最短路径。

RRT算法原理

该博客叙述得十分清楚明白，后面的代码实践基本是按照该思路得到：
链接: link

MALTAB代码实现

使用到的地图：

碰撞检测代码：
collisionChecking.m

function feasible=collisionChecking(startPose,goalPose,map)feasible=true;
dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));
for r=0:0.5:sqrt(sum((startPose-goalPose).^2))posCheck = startPose + r.*[sin(dir) cos(dir)];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,2) && point(2)>=1 && point(2)<=size(map,1) && map(point(2),point(1))==255)feasible=false;
end

节点编号代码：
node_index.m

function n_index = node_index(T_LIST,xval,yval)%This function returns the index of the location of a node in the T_LISTi=1;while ( T_LIST(i,1) ~= xval || T_LIST(i,2) ~= yval )i=i+1;endn_index=i;
end

主函数：
RRT_main.m

%***************************************
%Author: Wang Liang
%Date: 2022-06-21
%***************************************
%% 流程初始化
clc;
clear all;
close all;x_I=1; y_I=1;           % 设置初始点
x_G=700; y_G=700;       % 设置目标点   这里是地图上的像素点
Thr=30;                 % 设置目标点阈值
Delta= 30;              % 设置扩展步长
%% 建树初始化
T.v(1).x = x_I;         % T是我们要做的树，v是节点，这里先把起始点加入到T里面来
T.v(1).y = y_I;         % 将起始点的坐标加入到结点中
T.v(1).xPrev = x_I;     % 起始节点的父节点仍然是其本身
T.v(1).yPrev = y_I;
T.v(1).dist=0;          % 从父节点到该节点的距离，这里可取欧氏距离
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, 'ro', 'MarkerSize',5, 'MarkerFaceColor','r');
plot(x_G, y_G, 'go', 'MarkerSize',5, 'MarkerFaceColor','g');% 绘制起点和目标点
count=1;
for iter = 1:3000p_rand=[];%Step 1: 在地图中随机采样一个点x_rand%提示：用（p_rand(1),p_rand(2)）表示环境中采样点的坐标p_rand(1)=ceil(rand()*xL); % rand()生成的是0~1均匀分布的随机数，乘以800再向上取整，数便为[1,800]间的整数p_rand(2)=ceil(rand()*yL);p_near=[];%Step 2: 遍历树，从树中找到最近邻近点x_near %提示：x_near已经在树T里min_distance = 1000;for i=1:count distance = sqrt( ( T.v(i).x - p_rand(1) )^2 + ( T.v(i).y - p_rand(2) )^2 );if distance < min_distancemin_distance = distance;index = i;endendp_near(1) = T.v(index).x;p_near(2) = T.v(index).y;    % 找到采样点的最近邻近点p_new=[];%Step 3: 扩展得到x_new节点%提示：注意使用扩展步长Deltap_new(1) = p_near(1) + round( ( p_rand(1)-p_near(1) ) * Delta/min_distance );p_new(2) = p_near(2) + round( ( p_rand(2)-p_near(2) ) * Delta/min_distance );%检查节点是否是collision-freeif ~collisionChecking(p_near,p_new,Imp) continue;endcount=count+1;%Step 4: 将x_new插入树T %提示：新节点x_new的父节点是x_nearT.v(count).x = p_new(1);         T.v(count).y = p_new(2); T.v(count).xPrev = p_near(1);    T.v(count).yPrev = p_near(2);T.v(count).dist = min_distance;          %Step 5:检查是否到达目标点附近 %提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环new_distance = sqrt( ( p_new(1) - x_G )^2 + ( p_new(2) - y_G )^2 );if new_distance <= Thrplot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_newline( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_newline( [x_G p_new(1)], [y_G p_new(2)], 'Marker','.','LineStyle','-'); %连接x_Target和x_newbreak;end%Step 6:将x_near和x_new之间的路径画出来%提示 1：使用plot绘制，因为要多次在同一张图上绘制线段，所以每次使用plot后需要接上hold on命令%提示 2：在判断终点条件弹出for循环前，记得把x_near和x_new之间的路径画出来plot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_newline( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_newhold on;pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察
end%% 画出路径
T_LIST = zeros(size(T.v, 2), 5);  % size(),获取矩阵T.v的函数
for i=1:size(T.v, 2)T_LIST(i,1) = T.v(i).x;T_LIST(i,2) = T.v(i).y;T_LIST(i,3) = T.v(i).xPrev;T_LIST(i,4) = T.v(i).yPrev;T_LIST(i,5) = i;
endpath = [];
path_count = 1;
path(path_count,1) = x_G;
path(path_count,2) = y_G;
path_count = path_count + 1;
path(path_count,1) = p_new(1);
path(path_count,2) = p_new(2);
n_index = node_index(T_LIST, p_new(1), p_new(2));
path_count = path_count + 1;
path(path_count,1) = T_LIST(i,3);
path(path_count,2) = T_LIST(i,4);
while path(path_count,1) ~= x_I || path(path_count,2) ~= y_Inew_n_index = node_index(T_LIST, path(path_count,1), path(path_count,2));path_count = path_count + 1;path(path_count,1) = T_LIST(new_n_index,3);path(path_count,2) = T_LIST(new_n_index,4);n_index = new_n_index;
endfor i=size(path,1)-1 :-1: 1line( [path(i,1) path(i+1,1)], [path(i,2) path(i+1,2)], 'Marker','.','LineStyle','-','color','r'); %连接x_near和x_newhold on;pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察
end

运行结果

C++代码实现

待更新…

RRT（快速随机搜索树）算法原理及代码实践相关推荐

Bagging与随机森林算法原理小结
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达在集成学习原理小结中,我们讲到了集成学习有两个流派,一个是boos ...
多类线性分类器算法原理及代码实现 MATLAB
多类线性分类器算法原理及代码实现 MATLAB 一.算法原理下面举例说明为何蓝圈部分在case2中是确定的而在case1中不确定: 二.代码实现 1.HK函数 function [] = HK(w1 ...
论文｜Node2vec算法原理、代码实战和在微信朋友圈的应用
1 概述 Node2vec是2016年斯坦福教授 Jure Leskovec.Aditya Grover提出的论文,论文的下载链接为:https://arxiv.org/pdf/1607.00653. ...
Spark 随机森林算法原理、源码分析及案例实战
图 1. Spark 与其它大数据处理工具的活跃程度比较回页首环境要求操作系统:Linux,本文采用的 Ubuntu 10.04,大家可以根据自己的喜好使用自己擅长的 Linux 发行版 Jav ...
SSD算法原理与代码（三）
说明:这几篇文章是讲解SSD,从算法原理.代码到部署到rk3588芯片上的过程.环境均是TF2.2,具体的安装过程请参考网上其他的文章. 一.SSD简介 SSD算法是一个优秀的one-stage目标检 ...
计算机图形学--中点椭圆算法原理及代码实现
目录椭圆的几何特性: 算法原理: 代码实现: 说明,我们这里讨论的椭圆都是对称轴平行于坐标轴的椭圆,对于其他方程较为复杂的椭圆我们不做讨论. 椭圆的几何特性: 首先我们考虑椭圆的几何特性.椭圆是抽对 ...
深度强化学习-D3QN算法原理与代码
Dueling Double Deep Q Network(D3QN)算法结合了Double DQN和Dueling DQN算法的思想,进一步提升了算法的性能.如果对Doubel DQN和Duelin ...
萤火虫算法_40多种智能优化算法原理和代码分享
40多种智能优化算法原理和代码分享 <智能优化算法讲解>PDF下载地址: <智能优化算法原理讲解>PDFmianbaoduo.com 包括: 1.海鸥算法SOA 智能优化算法 ...
《机器学习：算法原理与编程实践》的读书笔记：SMO部分最难，大部分代码基于Scikit-Learn，决策树其实用处不大
机器学习:算法原理与编程实践目录 [隐藏] 1 机器学习的基础 2 中文文本分类 3 决策树的发展 4 推荐系统原理 5 梯度寻优 6 神经网络初步 7 预测的技术与哲学 8 万能分类器:SVM 9 ...