【路径规划】快速随机树法无人机最优路径规划【含Matlab源码 2508期】-Toy模板网

这篇具有很好参考价值的文章主要介绍了【路径规划】快速随机树法无人机最优路径规划【含Matlab源码 2508期】。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

【路径规划】快速随机树法无人机最优路径规划【含Matlab源码 2508期】,Matlab路径规划（初级版）,matlab

⛄一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【路径规划】基于matlab快速随机树法无人机最优路径规划【含Matlab源码 2508期】
点击上面蓝色字体，直接付费下载，即可。

获取代码方式2：
付费专栏Matlab路径规划（初级版）

备注：
点击上面蓝色字体付费专栏Matlab路径规划（初级版），扫描上面二维码，付费29.9元订阅海神之光博客付费专栏Matlab路径规划（初级版），凭支付凭证，私信博主，可免费获得1份本博客上传CSDN资源代码（有效期为订阅日起，三天内有效）；
点击CSDN资源下载链接：1份本博客上传CSDN资源代码

⛄二、RRT算法简介

1 RRT定义
RRT（Rapidly-Exploring Random Tree）算法是一种基于采样的路径规划算法，常用于移动机器人路径规划，适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一个步长向目标点搜索前进，有效躲避障碍物，避免路径陷入局部极小值，收敛速度快。本文通过matlab实现RRT算法，解决二维平面的路径规划问题。
2 地图
为了方便算法的实现，使用离散量来表达环境地图。其中，数值0表示无障碍物的空区域，数值1表示该区域有障碍物。
【路径规划】快速随机树法无人机最优路径规划【含Matlab源码 2508期】,Matlab路径规划（初级版）,matlab
RRT算法中搜索到的顶点坐标为连续点，在地图中产生随机点，算法将通过连续的点构建树。此过程中，对树枝和顶点进行检测，检测顶点所处位置是否是空区域。下载附录中.dat文件，绘制地图。

colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1];
imshow(uint8(map),colormap)

note:数据中的列为x轴，行为y轴

3 RRT算法原理
通过matlab程序构建从起始位置到目标位置的树，并生成连接两个点的路径。使用一颗中心点在起始点的树，而不是两颗树（一个中心点在起始位置，一个中心点在目标位置）。
编写一个matlab函数，输入和输出有相同的形式。

function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)

其中：
map：.mat文件中的地图矩阵
q_start：起点的x和y坐标
q_goal：目标点的x和y坐标
k: 在目标点无法找到是，控制产生搜索树的最大迭代次数为k次
delta_q : q_new 和 q_near之间的距离
p: 将q_goal 作为q_rand 的概率，当随机产生的随机数小于p，将目标点作为随机点q_rand，当随机产生的数大于p时，产生一个随机点作为q_rand
vertices：顶点的x和y坐标，生成随机树的过程中产生的所有的点的坐标都存储在这个矩阵中，第一个点为起点，最后一个点为目标点。是一个2行n列的矩阵
deges：生成随机树的所有树枝，一共有n-1个树枝，因此该矩阵有n-1行，每一行的两列分别表示两个点的索引号。一旦搜索到目标点，最后一行将表示目标点，沿着目标点回溯，即可找到路径
path: 从起始点到目标点的索引，是一个行向量
下面用一个图来表示上面提到的算法里的一些变量：
【路径规划】快速随机树法无人机最优路径规划【含Matlab源码 2508期】,Matlab路径规划（初级版）,matlab

4 障碍物检测
检测树枝（即q_near和q_new之间的edge）是否处于自由空间，可以使用增量法或者等分法，示意图如下(假设两点之间有10个点，左图为为增量检测法，右图为等分法，从示意图中可以看出使用等分法检测次数更少)：
【路径规划】快速随机树法无人机最优路径规划【含Matlab源码 2508期】,Matlab路径规划（初级版）,matlab
在本文中，使用k=10000，delta_q=50,p=0.3, 我们将获得如下结果：

5 路径平滑处理
完成基本的RRT算法之后，我们获得了一条从起点到终点的路径，现在对这条路径进行平滑和降噪处理，处理完成之后，我们将得到一条更短的路径。
采用贪心算法：
连接q_start和q_goal,如果检测到两个点之间有障碍物，则将q_goal替换为前一个点，直到两个点能连接上（中间无障碍物）为止。一旦q_goal被连接上，
在matlab中定义平滑函数：

function [path_smooth] = smooth(map, path, vertices, delta)

其中：
path: 从起始点到目标位置的路径索引号
vertices：树中所有的顶点坐标
delta：增量距离，用来检测路径顶点之间的直接连接是否在自由空间之内，每个edge都被delta分割成几段
path_smooth：经过平滑处理之后，路径点将会减少，用path_smooth记录平滑之后的路径,仍然是一个行向量，记录路径的索引号

平滑处理之后的路径为：
【路径规划】快速随机树法无人机最优路径规划【含Matlab源码 2508期】,Matlab路径规划（初级版）,matlab
6 总结
RRT算法是一种增量式的搜索算法，基于概率的思想，它是一种概率完备的路径优化算法，具有求解速度上的优势。RRT基本算法有其自身缺陷，求解得到的路径通常质量不好，带有棱角，不够光滑。因此需要对路径进行平滑处理，才能得到适合机器人路径跟踪的路径曲线。

⛄三、部分源代码

function My_RRT
clc
clear
close all
%% color map
load maze.mat map
[map_height,map_width]=size(map); %行是高y，列是宽x
q_start = [206, 198]; %q s t a r t ( 1 ) : x宽 , q s t a r t ( 2 ) : y高
q_goal = [416, 612];
colormap=[1 1 1
0 0 0
1 0 0
0 1 0
0 0 1];
imshow(uint8(map),colormap)
hold on
%% rrt tree %行是y坐标，列是x坐标
%initial
vertices=q_start;
edges = [];
K=10000;
delta_q=50;
p=0.3;
q_rand=[];
q_near=[];
q_new=[];
%main loop
plot(q_start(2),q_start(1),‘*b’)
plot(q_goal(2),q_goal(1),‘*y’)
for k = 1:K
arrived=is_goal_arrived(vertices,q_goal,delta_q);
if arrived
vertices=[vertices;q_goal];
edges = [edges;[size(vertices,1),size(vertices,1)-1]];
break;
end
if rand <= p
q_rand = q_goal;%q(1)宽x，q(2)高y
else
q_rand = [randi(map_height),randi(map_width)];
end
if map( q_rand(1,1),q_rand(1,2) ) == 1 %map(1)height,map(2)width
continue;
end