RRT算法二维避障的MATLAB实现

在指定大小的范围内通过输入初始点和目标点以及障碍物信息可以达到避障效果，下面是代码运行后的效果图。

如果想要源代码文件也可加群获取群文件领代码，群号：700401314

main函数

%% 清空变量
clear;
clc

%% 用户可定义参数

road_len = 100;      %道路长度
left_down = [1.25,0];%左下角起始点
radius = 3;   %障碍物半径
car_wid = 2.5;  %车宽
po = radius+car_wid/2;%障碍影响距离
road_wid = 42-car_wid;%道路宽度


left_up = [left_down(1),left_down(2)+road_len];%左上角起始点
right_up = [left_down(1)+road_wid,left_down(2)+road_len];  %右上角起始点
right_down = [left_down(1)+road_wid,left_down(2)];  %右下角起点

% x_I=left_down(1); y_I=left_down(2);           % 设置初始点
% x_G=right_up(1); y_G=right_up(2);             % 设置目标点

x_G=42; y_G=100;           %  设置目标点
x_I=0; y_I=0;             % 设置初始点


goal(1)=x_G;
goal(2)=y_G;

obj_mat = [ 10  20  30  40 ;
           50 50 50 50  ];     %障碍物的坐标


%% rrt

Thr=5;                 %设置目标点阈值 当到这个范围内时则认为已到达目标点
Delta= 3;              % 设置扩展步长，扩展结点允许的最大距离
iter_max = 5000;
iter_flag = 1;
while iter_flag
    %% 建树初始化
    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;     %父节点的索引
    count=1;
    

    
    for iter = 1:iter_max
        %此函数表示在空间中找到一个随机点作为x_rand
        x_rand=Sample_rrt(left_down,road_len,road_wid,goal);
        
        %遍历树，从树中找到最近邻近点x_near   
        [x_near,index]= Near_rrt(x_rand,T);
        
        %扩展得到x_new节点
        x_new=Steer_rrt(x_rand,x_near,Delta);
        
        %检查新节点是否不发生碰撞
        if ~collisionChecking_rrt(x_near,x_new,obj_mat,po) %如果有障碍物则跳出
            continue;
        end
        count=count+1;
        
        %将x_new插入树T
        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=sqrt((x_new(1)-goal(1))^2+(x_new(2)-goal(2))^2);          %从父节点到该节点的距离，这里可取欧氏距离
        T.v(count).indPrev = index;     %父节点的索引
        %检查是否到达目标点附近
        %提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环
        if sqrt((x_new(1)-goal(1))^2+(x_new(2)-goal(2))^2) < Thr
            break;
        end
        
    end
    if iter==iter_max
        iter_flag = 0;
        disp(['当障碍物数为',num2str(size(obj_mat,2)),'没有找到路径']);
        
    else
        iter_flag = 0;
        disp(['当障碍物数为',num2str(size(obj_mat,2)),'时可以找到路径'])
        %保存路径的点
        Path = draw_picture2(obj_mat,radius,left_down,left_up,right_down,x_G,y_G,T,x_I,y_I,car_wid,iter_max,1);

    end
end

碰撞检测函数

function feasible=collisionChecking_rrt(x_near,x_new,obj_mat,po)%冲突检查：判断起始点到终点之间是否有障碍物
% collisionChecking(x_near,x_new,Imp) %如果有障碍物则跳出,没有障碍返回true
feasible=true;%可行的，可执行的
dir=atan2(x_new(2)-x_near(2),x_new(1)-x_near(1));%目标点和起始点之间的角度
num = size(obj_mat,2);   %障碍物个数
index = 0;
for r=linspace(0,sqrt(sum((x_near-x_new).^2)),20)%sqrt(sum((startPose-goalPose).^2)):两点间的距离
    posCheck = x_near + r.*[cos(dir) sin(dir)];%以0.5的间隔得到中间的点
    for k1 = 1:num
        if sqrt((posCheck(1)-obj_mat(1,k1))^2+(posCheck(2)-obj_mat(2,k1))^2)<po
            feasible = false;
            index = 1;
            break;
        end
    end
    if index 
        break;
    end
end
%     if ~(feasiblePoint(ceil(posCheck),Imp) && feasiblePoint(floor(posCheck),Imp) && ...
%             feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],Imp) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],Imp))
%         feasible=false;break;
%     end
%     if ~feasiblePoint([floor(x_new(1)),ceil(x_new(2))],Imp), feasible=false; end


 

画图函数

function Path = draw_picture2(obj_mat,radius,left_down,left_up,right_down,x_G,y_G,T,x_I,y_I,car_wid,iter_max,flag)

Path = zeros(iter_max,2);
for KK = 1:size(obj_mat,2)
    
    plot(obj_mat(1,KK)+radius*cos(0:0.01:2*pi),obj_mat(2,KK)+radius*sin(0:0.01:2*pi),'r-')
    hold on
    axis([-10 50 -10 110])
    
    axis equal
    
    
end

plot([left_down(1)-car_wid/2 right_down(1)+car_wid/2 right_down(1)+car_wid/2 left_down(1)-car_wid/2 left_down(1)-car_wid/2],...
    [left_down(2) left_down(2) left_up(2) left_up(2) left_down(2)],'LineStyle','-','Color',[0.8500 0.3250 0.0980],'LineWidth',2)

if flag
    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 k1 = 0:length(path.pos)-2
        Path(k1+1,1) = path.pos(end-k1).x;
        Path(k1+1,2) = path.pos(end-k1).y;
        plot([path.pos(end-k1).x path.pos(end-k1-1).x],[path.pos(end-k1).y path.pos(end-k1-1).y],'b-', 'Linewidth',1);
        pause(0.1)
    end
    Path(k1+2,1) = path.pos(end-k1-1).x;
    Path(k1+2,2) = path.pos(end-k1-1).y;
    Path(k1+3:end,:) = [];

    
end

text(x_I,y_I,'起点');
text(x_G,y_G,'终点')


end

function [x_near,index]=Near_rrt(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_rrt(left_down,road_len,road_wid,goal)

if unifrnd(0,1)<0.5
   x_rand(1)= left_down(1)+unifrnd(0,1.1)* road_wid;   % 均匀采样
   x_rand(2)= left_down(2)+unifrnd(0,1.1)* road_len;   % random sample
else
   x_rand=goal;
end
 

function X_new=Steer_rrt(x_rand,x_near,Delta)
theta = atan2(x_rand(2)-x_near(2),x_rand(1)-x_near(1));  % direction to extend sample to produce new node
X_new = x_near(1:2) + Delta * [cos(theta)  sin(theta)];