基于快速扩展随机树（RRT /
rapidly exploring random tree）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似，该方法是概率完备且不最优的。
RRT是一种多维空间中有效率的规划方法。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。基本RRT算法如下面伪代码所示：
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))%sqrt(sum((startPose-goalPose).^2)):两点间的距离
posCheck = startPose + r.*[sin(dir) cos(dir)];%以0.5的间隔得到中间的点
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;
end
if ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end
end
function 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)%map(point(2),point(1))==255：没有障碍物
feasible=false;
end
function distance=Distance(start_Pt,goal_Pt)
distance=sqrt((start_Pt(1)-goal_Pt(1))^2+(start_Pt(2)-goal_Pt(2))^2);
function [X_near,index]=Near(X_rand,T)
min_distance=sqrt((X_rand(1)-T.v(1).x)^2+(X_rand(2)-T.v(1).y)^2);
for T_iter=1:size(T.v,2)
temp_distance=sqrt((X_rand(1)-T.v(T_iter).x)^2+(X_rand(2)-T.v(T_iter).y)^2);
if temp_distance<=min_distance
min_distance=temp_distance;
X_near(1)=T.v(T_iter).x
X_near(2)=T.v(T_iter).y
index=T_iter;
end
end
function X_rand=Sample(map,goal)
% if rand<0.5
% X_rand = rand(1,2) .* size(map); % random sample
% else
% X_rand=goal;
% end
if unifrnd(0,1)<0.5
X_rand(1)= unifrnd(0,1)* size(map,1); % 均匀采样
X_rand(2)= unifrnd(0,1)* size(map,2); % random sample
else
X_rand=goal;
end
function X_new=Steer(X_rand,X_near,StepSize)
theta = atan2(X_rand(1)-X_near(1),X_rand(2)-X_near(2)); % direction to extend sample to produce new node
X_new = X_near(1:2) + StepSize * [sin(theta) cos(theta)];
% dir_x = X_rand(1)- X_near(1);
% dir_y = X_rand(2)- X_near(2);
% dir = sqrt(dir_x^2 + dir_y^2);
% X_new(1) = dir_x * StepSize/dir+X_near(1);
% X_new(2) = dir_y * StepSize/dir+X_near(2);
function X_rand=Sample(map,goal)
% if rand<0.5
% X_rand = rand(1,2) .* size(map); % random sample
% else
% X_rand=goal;
% end
if unifrnd(0,1)<0.5
X_rand(1)= unifrnd(0,1)* size(map,1); % 均匀采样
X_rand(2)= unifrnd(0,1)* size(map,2); % random sample
else
X_rand=goal;
end
%***************************************
%Author: Chenglong Qian
%Date: 2019-11-5
%***************************************
%% 流程初始化
clear all; close all;
x_I=1; y_I=1; % 设置初始点
x_G=700; y_G=700; % 设置目标点
goal(1)=x_G;
goal(2)=y_G;
Thr=50; %设置目标点阈值 当到这个范围内时则认为已到达目标点
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('newmap.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',10, 'MarkerFaceColor','r');
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');% 绘制起点和目标点
count=1;
for iter = 1:3000
% x_rand=[];
%Step 1: 在地图中随机采样一个点x_rand
%提示：用（x_rand(1),x_rand(2)）表示环境中采样点的坐标
x_rand=Sample(Imp,goal);
% x_near=[];
%Step 2: 遍历树，从树中找到最近邻近点x_near
%提示：x_near已经在树T里
[x_near,index]= Near(x_rand,T);
plot(x_near(1), x_near(2), 'go', 'MarkerSize',2);
% x_new=[];
%Step 3: 扩展得到x_new节点
%提示：注意使用扩展步长Delta
x_new=Steer(x_rand,x_near,Delta);
%检查节点是否是collision-free
if ~collisionChecking(x_near,x_new,Imp) %如果有障碍物则跳出
continue;
end
count=count+1;
%Step 4: 将x_new插入树T
%提示：新节点x_new的父节点是x_near
T.v(count).x = x_new(1);
T.v(count).y = x_new(2);
T.v(count).xPrev = x_near(1); % 起始节点的父节点仍然是其本身
T.v(count).yPrev = x_near(2);
T.v(count).dist=Distance(x_new,x_near); %从父节点到该节点的距离，这里可取欧氏距离
T.v(count).indPrev = index; %父节点的索引
%Step 5:检查是否到达目标点附近
%提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环
if Distance(x_new,goal) < Thr
break;
end
%Step 6:将x_near和x_new之间的路径画出来
%提示 1：使用plot绘制，因为要多次在同一张图上绘制线段，所以每次使用plot后需要接上hold on命令
%提示 2：在判断终点条件弹出for循环前，记得把x_near和x_new之间的路径画出来
% plot([x_near(1),x_near(2)],[x_new(1),x_new(2)]);
% hold on
line([x_near(1),x_new(1)],[x_near(2),x_new(2)]);
pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察
end
%% 路径已经找到，反向查询
if iter < 2000
path.pos(1).x = x_G; path.pos(1).y = y_G;
path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
pathIndex = T.v(end).indPrev; % 终点加入路径
j=0;
while 1
path.pos(j+3).x = T.v(pathIndex).x;
path.pos(j+3).y = T.v(pathIndex).y;
pathIndex = T.v(pathIndex).indPrev;
if pathIndex == 1
break
end
j=j+1;
end % 沿终点回溯到起点
path.pos(end+1).x = x_I; path.pos(end).y = y_I; % 起点加入路径
for j = 2:length(path.pos)
plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
end
else
disp('Error, no path found!');
end
参考链接：https://www.cnblogs.com/flyinggod/p/8727951.html
评论（1）
您还未登录，请登录后发表或查看评论