RRT算法又稱為快速隨機擴展數算法，是一種普適路徑規劃算法， 為什么說是普適算法，因為它什么樣的苛刻的條件都會極大的可能性找到一條路徑。

但是這樣的算法也往往會伴隨缺點：

1.每次迭代都是在隨機找點，這就導致了迭代時間很大程度依賴運氣

2.由于這種算法的隨機性強，導致在很簡單的避障環境下也很難快速找到一條路徑

本文針對最初始的RRT算法(沒經過任何對算法的改進)進行代碼上的仿真,意在多個障礙物的條件下，找到一條路徑即可，算法本身不涉及優化，所以對尋找到的路徑也沒有做任何優化。

RRT算法模型在網上有詳細的解釋，而且非常通俗易懂，這里不做贅述，只給出了代碼部分實現。這是本人對RRT算法的一些自己的見解，或許有些淺顯，只是希望能給你們或多或少的幫助，不喜勿噴。

（ps：因為有很多人問我相關的問題，再就是這個代碼本身寫的并不是很好，所以我寫了一篇代碼可讀性較好的RRT算法文章，在我的另一篇博客里《手把手教RRT算法》，里面還附帶了整個視頻教學的資料地址，內容可以說是非常詳細，很適合初學者）

代碼簡介：

在一個充滿長方體和圓柱體障礙物的空間，給出任意一些點作為目標點進行路徑規劃

?迭代完畢后如下部分紅色線所示，為找到的路徑

?代碼部分：

主函數，文件命名為rrtmain.m

%% 清空變量clear clc%% 導入數據 [dat_chicun,dat_jiaodian,dat_xia] = xlsData();%% 畫圖 color_mat=rand(size(dat_jiaodian,1),3); %生成隨機矩陣作為顏色用 hold on %畫在同一個圖像上 grid on % 畫網格 for k1=1:size(dat_jiaodian,1)plotcube(dat_chicun(k1,[2,1,3]),dat_jiaodian(k1,1:3),0.6,color_mat(k1,:))%這個是畫長方體障礙物的函數 endfor k2=1:size(dat_xia,1)plot_cylinder(dat_xia(k2,1:3),dat_xia(k2,4),dat_xia(k2,5),1,rand(1,3));%這個是畫圓柱體障礙物的函數 end axis([0 2000 0 2000 -200 600 ]) %設置圖像的可視化范圍 axis equal % 圖像坐標軸可視化間隔相等 xlabel('x'); ylabel('y')%% 數據重處理num_cube = size(dat_jiaodian(2:end,:),1); %長方體障礙物個數15 [dat_cube,dat_pingban] = cube_coor_deal(dat_jiaodian); %將長方體角點元素存儲在元胞數組，方便索引 num_cylinder = size(dat_xia,1); %圓柱體障礙物個數 dat_cylinder = cylinder_coor_deal(dat_xia); %將圓柱體數據存放在元胞數組里，方便訪問% num_cube: 長方體障礙物個數 % dat_cube: 長方體角點的元胞數組 % dat_pingban: 平板角點元胞數組 % num_cylinder: 圓柱體障礙物個數 % dat_cylinder: 圓柱體障礙物數據元胞%% rrt算法部分 GG = [704 700 -30;612 700 -30;537 680 -30;447 980 -30];%目標點坐標，有多個目標點就將每個目標點坐標放在這里 PATH = []; for k3 = 1:size(GG,1)-1start = GG(k3,:);goal = GG(k3+1,:);path_best = RRT(num_cube,dat_cube,num_cylinder,dat_cylinder,start,goal);%每次找到最好路徑存放在這個變量里%注意：這個路徑點是經過冗余點處理以后的，所以點很少 % line(path_best(:,1),path_best(:,2),path_best(:,3),'LineWidth',2,'Color','b');%將找到路徑的點集合用直線畫出來PATH = [PATH;path_best]; end

---

function dist = calculate_distance3(mat_start,mat_goal) %% 此函數是計算三維點的距離的 dist = sqrt((mat_start(1)-mat_goal(1))^2+(mat_start(2)-mat_goal(2))^2+(mat_start(3)-mat_goal(3))^2); function flag = collision_checking_cube(num_cube,dat_cube,coor_new,coor_near,Delta,deta) %發生碰撞返回1 flag = 0; % flag1 = 0; % flag2 = 0; % flag2_mid1 = 0; % flag2_mid2 = 0; % flag2_mid3 = 0; for k1=1:num_cubex_min = min(dat_cube{k1}(:,1));x_max = max(dat_cube{k1}(:,1));y_min = min(dat_cube{k1}(:,2));y_max = max(dat_cube{k1}(:,2));z_min = min(dat_cube{k1}(:,3));z_max = max(dat_cube{k1}(:,3));for r=0:Delta/deta:Deltacoor_mid = rrt_steer(coor_new,coor_near,r);if ((x_min<coor_mid(1))&&(coor_mid(1)<x_max))&&((y_min<coor_mid(2))&&(coor_mid(2)<y_max))&&((z_min<coor_mid(3))&&(coor_mid(3)<z_max))flag = 1;break;endendend function flag = collision_checking_cylinder(num_cylinder,dat_cylinder,coor_new,coor_near,Delta,deta) %發生碰撞返回1 flag = 0;for k1=1:num_cylinderx_coor = dat_cylinder{k1}(1);y_coor = dat_cylinder{k1}(2);z_coor = dat_cylinder{k1}(3);R = dat_cylinder{k1}(4)/2;height = dat_cylinder{k1}(5);for r=Delta/deta:Deltacoor_mid = rrt_steer(coor_new,coor_near,r);if (((x_coor-coor_mid(1))^2+(y_coor-coor_mid(2))^2-R^2) < 0) && (z_coor<coor_mid(3)) && (coor_mid(3)<z_coor+height)flag = 1;break;endend end function [dat_cube,dat_pingban] = cube_coor_deal(dat_jiaodian) %% 將15個障礙物角點存放在15個元胞數組里面 mid1 = dat_jiaodian(1:end,:); lin = size(mid1,1); col = size(mid1,2); mid3 = zeros(col/3,3); dat_cube_mid = cell(lin,1);for k1=1:linfor k2=1:col/3mid2 = mid1(k1,3*(k2-1)+1:3*(k2-1)+3);mid3(k2,:) = mid2;enddat_cube_mid{k1}(:,:) = mid3; end dat_cube = dat_cube_mid; dat_cube(1,:)=[]; dat_pingban{1} = dat_cube_mid{1}; function dat_cylinder = cylinder_coor_deal(dat_xia) %% 將16個圓柱障礙物直徑和高度等參數放在元胞數組里，至于為什么存在元胞數組里面，沒有為什么，個人愛好 lin = size(dat_xia,1); dat_cylinder=cell(lin,1); for k1=1:lindat_cylinder{k1}=dat_xia(k1,:); end function path_best = delete_redundant_points(path,num_cylinder,dat_cylinder,num_cube,dat_cube)num_points = size(path,1); count = 1; start_point = path(1,:); index = zeros(1,num_points); for k1 = 1:num_points-2count = count+1;final_point = path(count+1,:);if (collision_checking_cylinder(num_cylinder,dat_cylinder,final_point,start_point,calculate_distance3(final_point,start_point),3)||...collision_checking_cube(num_cube,dat_cube,final_point,start_point,calculate_distance3(final_point,start_point),3))start_point = path(count,:);elseindex(count) = count;end end index(index==0) = []; path([index(end:-1:1)],:) = []; path_best = path; function plot_cylinder(coor,diameter,height,facealpha,color)%% plot_cylinder(dat_xia(k2,1:3),dat_xia(k2,4),dat_xia(k2,5),1,rand(1,3)); % 第一個參數是圓柱體的底部圓心坐標值，第二個參數是圓柱體直徑，第三個參數是圓柱高度 % 第四個參數是透明度，第五個參數是顏色矩陣%% 函數解釋：把這個函數當做黑箱處理，只需要記住函數的輸入就可以，知道是干什么的，內部 %% 實現過于復雜，很難解釋清楚% coor: 中心坐標 % diameter: 直徑 % height: 高度 % facealpha: 透明度 % color: 顏色r = diameter/2; theta = 0:0.3:pi*2; hold onfor k1 = 1:length(theta)-1X=[coor(1)+r*cos(theta(k1)) coor(1)+r*cos(theta(k1+1)) coor(1)+r*cos(theta(k1+1)) coor(1)+r*cos(theta(k1))];Y=[coor(2)+r*sin(theta(k1)) coor(2)+r*sin(theta(k1+1)) coor(2)+r*sin(theta(k1+1)) coor(2)+r*sin(theta(k1))];Z=[coor(3),coor(3),coor(3)+height,coor(3)+height];h=fill3(X,Y,Z,color);set(h,'edgealpha',0,'facealpha',facealpha) endX=[coor(1)+r*cos(theta(end)) coor(1)+r*cos(theta(1)) coor(1)+r*cos(theta(1)) coor(1)+r*cos(theta(end))]; Y=[coor(2)+r*sin(theta(end)) coor(2)+r*sin(theta(1)) coor(2)+r*sin(theta(1)) coor(2)+r*sin(theta(end))]; Z=[coor(3),coor(3),coor(3)+height,coor(3)+height]; h=fill3(X,Y,Z,color); set(h,'edgealpha',0,'facealpha',facealpha)fill3(coor(1)+r*cos(theta),coor(2)+r*sin(theta),coor(3)*ones(1,size(theta,2)),color) fill3(coor(1)+r*cos(theta),coor(2)+r*sin(theta),height+coor(3)*ones(1,size(theta,2)),color) view(3) function plotcube(varargin) %% plotcube(dat_chicun(k1,[2,1,3]),dat_jiaodian(k1,1:3),0.6,color_mat(k1,:)) %% 第一個參數是每個長方體的長寬高數值，第二個參數是角點的第一個坐標值，第三個參數是透明度，范圍是0-1。 %% 第四個參數是顏色矩陣[a,b,c] abc每個值范圍是0-1 %% inArgs = { ...[10 56 100] , ... % Default edge sizes (x,y and z)[10 10 10] , ... % Default coordinates of the origin point of the cube.7 , ... % Default alpha value for the cube's faces[1 0 0] ... % Default Color for the cube};inArgs(1:nargin) = varargin;[edges,origin,alpha,clr] = deal(inArgs{:});XYZ = { ...[0 0 0 0] [0 0 1 1] [0 1 1 0] ; ...[1 1 1 1] [0 0 1 1] [0 1 1 0] ; ...[0 1 1 0] [0 0 0 0] [0 0 1 1] ; ...[0 1 1 0] [1 1 1 1] [0 0 1 1] ; ...[0 1 1 0] [0 0 1 1] [0 0 0 0] ; ...[0 1 1 0] [0 0 1 1] [1 1 1 1] ...};XYZ = mat2cell(...cellfun( @(x,y,z) x*y+z , ...XYZ , ...repmat(mat2cell(edges,1,[1 1 1]),6,1) , ...repmat(mat2cell(origin,1,[1 1 1]),6,1) , ...'UniformOutput',false), ...6,[1 1 1]);cellfun(@patch,XYZ{1},XYZ{2},XYZ{3},...repmat({clr},6,1),...repmat({'FaceAlpha'},6,1),...repmat({alpha},6,1)...);view(3); function path_best = RRT(num_cube,dat_cube,num_cylinder,dat_cylinder,start,goal) %% 流程初始化Delta=2; % 設置擴展步長，擴展結點允許的最大距離，這個數據越大迭代越快，但是比最優解效果越差 max_iter = 10000; % 最大迭代次數，如果超過這個次數還沒找到路徑則認為找不到路徑 Map = [goal(1)-start(1),goal(2)-start(2),goal(3)-start(3)]; count = 1;%% 構建初始化樹 T.x(1) = start(1); T.y(1) = start(2); T.z(1) = start(3); T.xpar(1) = goal(1); T.ypar(1) = goal(2); T.zpar(1) = goal(3); T.dist(1) = 0; T.indpre(1) = 0;tic for iter = 1:max_iter% step1: 在地圖上隨機采樣coor_rand = rrt_sample(Map,goal,start); %在空間進行隨機采樣，coor_rand是一個 1×3 的數組% plot3(coor_rand(1),coor_rand(2),coor_rand(3),'r*')% step2 : 遍歷樹，找到最近的父節點[coor_near,coor_index] = rrt_near(coor_rand,T);% step3: 擴展得到新的節點coor_new = rrt_steer(coor_rand,coor_near,Delta);% step4: 碰撞檢測,發生碰撞就會返回1flag1 = collision_checking_cube(num_cube,dat_cube,coor_new,coor_near,Delta,3);%這部分是檢測是否和長方體障礙物碰撞的函數flag2 = collision_checking_cylinder(num_cylinder,dat_cylinder,coor_new,coor_near,Delta,3);%這部分是檢測是否和圓柱體障礙物碰撞的參數if flag1 || flag2continue;endcount = count+1;% step5:將新點插入進去T.x(count) = coor_new(1);T.y(count) = coor_new(2);T.z(count) = coor_new(3);T.xpar(count) = coor_near(1);T.ypar(count) = coor_near(2);T.zpar(count) = coor_near(3);T.dist(count) = calculate_distance3(coor_new,coor_near);T.indpre(count) = coor_index;line([coor_near(1),coor_new(1)],[coor_near(2),coor_new(2)],[coor_near(3),coor_new(3)],'LineWidth',1);pause(0.1); %暫停0.1s，使得RRT擴展過程容易觀察；% 注意： pause函數時暫停函數，如果為了顯示動畫則這部分十分重要，不過不加上則會靜止動畫，不能展示動圖% step6:每次迭代出新點后都檢查一遍是否可以直接和終點相連if ~(collision_checking_cylinder(num_cylinder,dat_cylinder,goal,coor_new,calculate_distance3(goal,coor_new),20)||...collision_checking_cube(num_cube,dat_cube,goal,coor_new,calculate_distance3(goal,coor_new),20))count = count+1;T.x(count) = goal(1);T.y(count) = goal(2);T.z(count) = goal(3);T.xpar(count) = coor_new(1);T.ypar(count) = coor_new(2);T.zpar(count) = coor_new(3);T.dist(count) = calculate_distance3(coor_new,goal);T.indpre(count) = 0;line([goal(1),coor_new(1)],[goal(2),coor_new(2)],[goal(3),coor_new(3)],'LineWidth',3,'MarkerSize',2);pause(0.1); %暫停0.1s，使得RRT擴展過程容易觀察break;endendtoc% 算法找到路徑點后找到到達終點的父代點集合存儲在path變量中 if iter>max_itererror('超過最大迭代次數，路徑規劃失敗'); end path(1,1) = T.x(end);path(1,2) = T.y(end);path(1,3) = T.z(end); path(2,1) = T.x(end-1);path(2,2) = T.y(end-1);path(2,3) = T.z(end-1); count2 = 2; ind_pre = T.indpre(end-1); if iter<=max_iterwhile ~(ind_pre==0)count2 = count2+1;path(count2,1) = T.x(ind_pre);path(count2,2) = T.y(ind_pre);path(count2,3) = T.z(ind_pre);ind_pre = T.indpre(ind_pre);endend % line(path(:,1),path(:,2),path(:,3),'LineWidth',1,'Color','r');%% RRT算法找到新點全部集合，接下來要去除冗余點 path_best = delete_redundant_points(path,num_cylinder,dat_cylinder,num_cube,dat_cube); line(path_best(:,1),path_best(:,2),path_best(:,3),'LineWidth',3,'Color','r'); function [coor_near,coor_index] = rrt_near(coor_rand,T)min_distance = calculate_distance3(coor_rand,[T.x(1),T.y(1),T.z(1)]);for T_iter=1:size(T.x,2)temp_distance=calculate_distance3(coor_rand,[T.x(T_iter),T.y(T_iter),T.z(T_iter)]);if temp_distance<=min_distancemin_distance=temp_distance;coor_near(1)=T.x(T_iter);coor_near(2)=T.y(T_iter);coor_near(3)=T.z(T_iter);coor_index=T_iter;end end function coor_rand = rrt_sample(Map,goal,start)rat = 1.5; if unifrnd(0,1)<0.5coor_rand(1)= unifrnd(-0.2,rat)* Map(1); coor_rand(2)= unifrnd(-0.2,rat)* Map(2); coor_rand(3)= unifrnd(-0.2,rat)* Map(3); coor_rand = coor_rand+start; elsecoor_rand=goal; end function coor_new = rrt_steer(coor_rand,coor_near,Delta)deltaX = coor_rand(1)-coor_near(1);deltaY = coor_rand(2)-coor_near(2);deltaZ = coor_rand(3)-coor_near(3);r = sqrt(deltaX^2+deltaY^2+deltaZ^2);fai = atan2(deltaY,deltaX); theta = acos(deltaZ/r);R = Delta;x1 = R*sin(theta)*cos(fai);x2 = R*sin(theta)*sin(fai);x3 = R*cos(theta);coor_new(1) = coor_near(1)+x1;coor_new(2) = coor_near(2)+x2;coor_new(3) = coor_near(3)+x3;end function PATH = smooth_deal(PATH)hold on; x = PATH(:,1)'; y = PATH(:,2)'; z = PATH(:,3)'; %三次樣條插值 t1=1:1:size(PATH,1); t=1:0.5:size(PATH,1); XX=spline(t1,x,t); YY=spline(t1,y,t); ZZ=spline(t1,z,t); plot3(XX,YY,ZZ,'r-')view(3) function [dat_chicun,dat_jiaodian,dat_xia] = xlsData() %% 十五個長方體障礙物的寬，長，高，第一行不是，第一行是地板，不作為障礙物dat_chicun = [0.1, 0.1, 0.1;624, 358, 700;140, 173, 267;121, 210, 105;150, 90, 130;150, 90, 130;115, 88, 122;140, 103, 142;140, 103, 142;135, 75, 91;75, 160, 216;75, 160, 216;111, 60, 98;118, 44, 31;75, 160, 216;75, 88, 125]; %% 十五個長方體障礙物的角點坐標，第一行不是dat_jiaodian = [0,0,0,0,0,1,1767,0,1,1767,0,0,1767,1679,0,1767,1679,1,0,1679,0,0,1679,1;704,573,-80,704,573,620,1062,573,620,1062,573,-80,1062,1197,-80,1062,1197,620,704,1197,-80,704,1197,620;1550,1539,1,1550,1539,268,1723,1539,268,1723,1539,1,1723,1679,1,1723,1679,268,1550,1679,1,1550,1679,268;150,1280,-74,150,1280,31,360,1280,31,360,1280,-74,360,1401,-74,360,1401,31,150,1401,-74,150,1401,31;264,1080,1,264,1080,131,354,1080,131,354,1080,1,354,1230,1,354,1230,131,264,1230,1,264,1230,131;264,910,1,264,910,131,354,910,131,354,910,1,354,1060,1,354,1060,131,264,1060,1,264,1060,131;398,1020,1,398,1020,123,486,1020,123,486,1020,1,486,1135,1,486,1135,123,398,1135,1,398,1135,123;447,858,-40,447,858,102,550,858,102,550,858,-40,550,998,-40,550,998,102,447,998,-40,447,998,102;447,710,-40,447,710,102,550,710,102,550,710,-40,550,850,-40,550,850,102,447,850,-40,447,850,102;537,565,-41,537,565,50,612,565,50,612,565,-41,612,700,-41,612,700,50,537,700,-41,537,700,50;1450,1320,1,1450,1320,217,1610,1320,217,1610,1320,1,1610,1395,1,1610,1395,217,1450,1395,1,1450,1395,217;1450,1201,1,1450,1201,217,1610,1201,217,1610,1201,1,1610,1276,1,1610,1276,217,1450,1276,1,1450,1276,217;1170,579,-48,1170,579,50,1230,579,50,1230,579,-48,1230,690,-48,1230,690,50,1170,690,-48,1170,690,50;451,1160,1,451,1160,32,495,1160,32,495,1160,1,495,1278,1,495,1278,32,451,1278,1,451,1278,32;1390,640,1,1390,640,217,1550,640,217,1550,640,1,1550,715,1,1550,715,217,1390,715,1,1390,715,217;1262,525,1,1262,525,126,1350,525,126,1350,525,1,1350,600,1,1350,600,126,1262,600,1,1262,600,126];%% 前三列是十六個圓柱形障礙物底部圓心坐標，第四列是直徑，第五列是高度dat_xia = [ 952,1330,-51,50,181;1032,1330,-51,50,181;1112,1330,-51,50,181;1430,1079,-51,40,145;1420,1032,-51,40,145;1410,985,-51,40,145;882,1330,-45,30,123;707,1330,-51,50,181;607,1330,-45,30,123;1450,1600,-43,30,123;1310,500,-53,50,167;1310,430,-53,50,167;1112,1570,1,130,310;960,1570,1,60,215;360,1570,1,180,370;657,1570,1,180,370];

總結：

代碼或許有點長，但是劃分為多個子函數就會顯得整潔的多。具體操作步驟，將每個代碼復制到一個新的腳本文件中，除了主函數命名隨意以外，其他腳本文件命名均為函數名稱。具體形式如下即可。

?每個子函數都有注釋，主函數里面也加上了相應的注釋，希望能對你們有幫助，感謝大家的支持~

總結

以上是生活随笔為你收集整理的RRT算法三维避障的MATLAB实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。