RRT——rapidly exploring random tree，即快速擴展隨機樹，是基于隨機采樣的路徑規劃算法，以起點為根節點，在搜索空間中隨機產生節點，通過進行步長計算、碰撞檢測等操作來產生新的子節點，直到采樣至終點為止。偽代碼如圖所示：

圖源： https://www.cnblogs.com/long5683/p/11795987.html

matlab代碼實現：
main函數：

clc clear close all %障礙物頂點初始化 obs(1).index=1; obs(1).x=[3,4,3,2,3]; obs(1).y=[1,2,3,3,1]; obs(2).index=2; obs(2).x=[6,7,7,5,6]; obs(2).y=[3,3,5,5,3]; %10*6的空間 x_range=[0,10];%橫軸范圍約束 y_range=[0,6];%縱軸范圍約束 init=[1,1];%起始點坐標 %第一個節點賦值 T.vertex(1).x=init(1); T.vertex(1).y=init(2); T.vertex(1).parent=T.vertex(1);%起始點的父節點是自身 T.vertex(1).index=1;%節點的索引goal=[9,5];%目標點坐標 end_radiu=1;%距離目標點多遠可以終止搜索 %繪制初始圖像 figure plot(init(1),init(2),'o','MarkerSize',5,'Color','g')%繪制起點 hold on plot(goal(1),goal(2),'o','MarkerSize',5,'Color','b')%繪制終點 rectangle('position',[goal(1)-end_radiu,goal(2)-end_radiu,2*end_radiu,2*end_radiu],'curvature',[1,1]);%繪制終止域范圍 axis equal xlim([0,10]); ylim([0,6]); for obs_index=1:size(obs,2)%繪制障礙物pause(0.1);hold onplot(obs(obs_index).x,obs(obs_index).y)fill(obs(obs_index).x,obs(obs_index).y,'b') end StepSize=0.6;%采樣步長 i=1; while sqrt((T.vertex(i).x-goal(1))^2+(T.vertex(i).y-goal(2))^2)>end_radiu %如果未達到終止條件，則繼續進行采樣i=i+1;%索引更新[index,new_point]=Near(T,x_range,y_range,StepSize);%生成隨機點，并返回樹上距離隨機點最近的點索引feasible=check_Collision(obs,new_point,T.vertex(index).x,T.vertex(index).y);%碰撞檢測，true表示發生碰撞if ~feasibleT.vertex(i).x=new_point(1);%給新的可行節點賦值T.vertex(i).y=new_point(2);T.vertex(i).parent=T.vertex(index);T.vertex(index).son=T.vertex(i);T.vertex(i).index=i;%第i個節點的索引elsei=i-1;continue;endpause(0.1);hold onplot(T.vertex(i).x,T.vertex(i).y,'*','MarkerSize',5,'Color','r');line([T.vertex(i).x,T.vertex(index).x],[T.vertex(i).y,T.vertex(index).y],'Color','g'); end line([T.vertex(i).x,goal(1)],[T.vertex(i).y,goal(2)]);%最后一個點與終點連線 path=[goal(1),goal(2)]; route=T.vertex(i); %繪制路線 while 1path=[path;route.x,route.y];if route.index==1breakendroute=route.parent; end for i=1:size(path,1)-1line([path(i,1),path(i+1,1)],[path(i,2),path(i+1,2)],'Color','r','linewidth',3) end disp('done!')

Near函數產生新的隨機采樣點，同時還要計算樹上與該點距離最近的節點，并返回其在樹上的索引值。

function [index,new_point]=Near(T,x_range,y_range,StepSize)%產生新的節點，并返回樹上距該點最近點的索引 rand_point=[x_range(2)*rand, y_range(2)*rand];%產生隨機點 min_dist=sqrt((rand_point(1)-T.vertex(1).x)^2+(rand_point(2)-T.vertex(1).y)^2); index=1; for i=1:size(T.vertex,2)dist=sqrt((rand_point(1)-T.vertex(i).x)^2+(rand_point(2)-T.vertex(i).y)^2);if dist<min_distmin_dist=dist;index=i;end end if min_dist<=StepSize%在步長范圍內直接使用該點new_point(1)=rand_point(1);new_point(2)=rand_point(2); elsetheta = atan2(rand_point(1)-T.vertex(index).x,rand_point(2)-T.vertex(index).y);new_point(1)=T.vertex(index).x+StepSize*sin(theta);new_point(2)=T.vertex(index).y+StepSize*cos(theta); end

check_Collision函數用于對新生成的節點進行碰撞檢測，方法是在新生成的節點與樹上最近點之間進行定步長采樣，計算采樣點是否與障礙物發生碰撞，因此采樣步長需要足夠小，防止誤判。

function feasible=check_Collision(obs,new_point,x,y) min_dist=sqrt((new_point(1)-x)^2+(new_point(2)-y)^2); theta = atan2(new_point(1)-x,new_point(2)-y); feasible=false; for k=1:size(obs,2)for step=0:0.001:min_distcheck_point.x=x+step*sin(theta);check_point.y=y+step*cos(theta);feasible=isinPolygon(check_point,obs(k));if feasiblereturn;endend end

判斷指定點與凸多邊形的關系可見以下文章，此處不再贅述：

https://blog.csdn.net/qq_36250209/article/details/123763849

function in_out=isinPolygon(Q,obs)%判斷一個點是否在凸多邊形之外——利用目標點與多邊形各頂點構成的面積與多邊形面積進行計算判斷 %如果面積之和大于多邊形面積，則在外部，否則在內部 %將多邊形面積計算轉化為各個三角形的面積計算，可以簡化計算過程 %輸入：目標點的坐標，凸多邊形的頂點坐標 最大邊數為五邊形 輸入的多邊形頂點按逆時針或順時針順序輸入 %輸出：判斷結果 S1=calc_triangle(Q.x,Q.y,obs.x(1),obs.y(1),obs.x(2),obs.y(2)); S2=calc_triangle(Q.x,Q.y,obs.x(2),obs.y(2),obs.x(3),obs.y(3)); S3=calc_triangle(Q.x,Q.y,obs.x(3),obs.y(3),obs.x(4),obs.y(4)); S4=calc_triangle(Q.x,Q.y,obs.x(4),obs.y(4),obs.x(5),obs.y(5)); S = calc_triangle(obs.x(1),obs.y(1),obs.x(2),obs.y(2),obs.x(3),obs.y(3))+ ...calc_triangle(obs.x(1),obs.y(1),obs.x(3),obs.y(3),obs.x(4),obs.y(4)); if S==S1+S2+S3+S4in_out=true; elsein_out=false; end end function S=calc_triangle(x0,y0,x1,y1,x2,y2)%計算三角形面積——利用三角形各點與橫坐標垂直連線構成的梯形面積相加減計算三角形面積S=0.5*abs(x0*y1+x1*y2+x2*y0-x0*y2-x1*y0-x2*y1); end

運行結果如下圖所示：

終點處的圓表示的是終止搜索范圍，當采樣點進入該范圍內，即可直接連接至終點，不過，程序中并未對此步驟進行碰撞檢測，可以根據自己的需要進行修改完善。

總結

以上是生活随笔為你收集整理的路径规划——RRT算法实现的全部內容，希望文章能夠幫你解決所遇到的問題。

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