根據查閱網上關于RRT算法的原理及代碼，再結合自己的理解，使用MATLAB實現RRT算法生成路徑的整個過程。RRT的原理部分我不再贅述，網上介紹的文章還是很多的。
下面主要展示自己編的RRT算法，并使用MATLAB自帶的RRT路徑規劃函數進行簡單比對。

RRT算法自編代碼實現

clear; close all; C=zeros(30,30);%一般默認權值大于0.65的網格為有障礙物的網格 %注意障礙物的權值按行列生成，而路徑曲線是以左下角為原點生成，注意兩者的區別 %C為生成整個網格地圖的各個網格權值的矩陣 C(15:18,10:16)=0.9*ones(4,7); C(21:26,15:27)=0.9*ones(6,13); C(6:12,3:15)=0.9*ones(7,13); %生成帶權值的網格地圖 costmap = vehicleCostmap(C,'CellSize',1);%每個網格邊長1米 %設置障礙物的膨脹范圍 ccConfig =inflationCollisionChecker('CenterPlacements',[0.2 0.5 0.8],'InflationRadius',0); costmap.CollisionChecker = ccConfig; %畫出整個地圖 plot(costmap);hold on; set(gca,"XTick",0:1:30); set(gca,"YTick",0:1:30); grid on;%hold on; legend("off");%檢測有障礙物的區域 occMat = checkOccupied(costmap);x_start=[1,1];%起點坐標 goal=[27,27];%終點坐標 grow_distance=1;%生長步長 goal_radius=1.5;%搜索停止閾值，如果新生成的節點距離終點距離小于1.5米，則停止搜索路徑 %畫出起點 plot(x_start(1),x_start(2),'k>','MarkerFaceColor','g','MarkerSize',9);hold on; %畫出終點 plot(goal(1),goal(2),'ko','MarkerFaceColor','r','MarkerSize',9);hold on; %使用結構體存儲子節點，父節點，以及父節點在子節點中的位置 tree.child=[]; tree.parent=[]; tree.parent_idx=[]; %初始化結構體tree tree.child=x_start; tree.parent=x_start; tree.parent_idx=1;flag=1; while flag%在地圖范圍內隨機生成一個點(rd_x，rd_y)rd_x=30*rand(1);rd_y=30*rand(1);fix_rd_x=fix(rd_x)+1;fix_rd_y=fix(rd_y)+1;%調用自定義函數cal_distance，計算新隨機點離子節點tree.child中哪個子節點最近，%返回最近的那個子節點行號、兩者的連線和x坐標軸的夾角、最短距離[angle,min_idx,distance]=cal_distance(rd_x,rd_y,tree);%隨機點的坐標轉換為權值矩陣上網格所在的行列，并判斷該點是否在障礙物上，%如果該點不在障礙物上，并且最短距離大于grow_distance（防止在計算新節點時超邊界），則生成新節點if (occMat(31-fix_rd_y,fix_rd_x)==false) && (distance>grow_distance)%此處注意路徑曲線上的點坐標和障礙物權值矩陣之間對應關系的轉換%沿著上述夾角方向按生長步長生成新節點new_node_x=tree.child(min_idx,1)+grow_distance*cos(angle);new_node_y=tree.child(min_idx,2)+grow_distance*sin(angle);new_node=[new_node_x,new_node_y];%沿著上述夾角方向，在生長步長內生成100個中間點，點越多，則檢測碰撞的精度越高，當然計算量也會越大L=linspace(0,grow_distance,100);direc_x=double(rd_x>tree.child(min_idx,1))*2-1;%判斷新節點相對于父節點在x軸上的方向（正方向或負方向）direc_y=double(rd_y>tree.child(min_idx,2))*2-1;%判斷新節點相對于父節點在y軸上的方向（正方向或負方向）x_cha=tree.child(min_idx,1)+direc_x*(L*abs(cos(angle)));%100個中間點的橫坐標y_cha=tree.child(min_idx,2)+direc_y*(L*abs(sin(angle)));%100個中間點的縱坐標cha_occMat=[];%計算線段上所有這100個點是否位于障礙物上for j=1:length(x_cha)cha_occMat=[cha_occMat,double(occMat(31-(fix(y_cha(j))+1),fix(x_cha(j))+1)==false)]; end%所有的點都不處在障礙物上，才將上述生產的新節點新增到結構體中if sum(cha_occMat)==length(x_cha)tree.child(end+1,:)=new_node;tree.parent(end+1,:)=[tree.child(min_idx,1),tree.child(min_idx,2)];tree.parent_idx(end+1)=min_idx;plot(rd_x,rd_y,'.r');hold on;%畫出隨機生成的點plot(new_node_x,new_node_y,'.g');hold on;%畫出新節點plot([tree.child(min_idx,1),new_node_x],[tree.child(min_idx,2),new_node_y],'b-');hold on;%畫出新節點及其父節點的連線end%如果新節點到終點的距離小于搜索停止閾值，則停止搜索，%并將終點加入到tree中if sqrt((new_node_x-goal(1,1))^2+(new_node_y-goal(1,2))^2)<=goal_radiustree.child(end+1,:)=goal;tree.parent(end+1,:)=new_node;tree.parent_idx(end+1)=size(tree.parent,1)-1;break;endend end%畫出生成的路徑 tem1=length(tree.parent_idx);%從終點所在的行開始搜索 %zuiyou_idx存儲生成路徑的點所在tree中的行號 zuiyou_idx=tem1;%首先將終點所在的行加入進去 while tem1>1 %如果沒有到起點的行號（也就是第一行），則一直找下去zuiyou_idx(end+1)=tree.parent_idx(tem1);%該行的parent_idx存著該行父節點parent在子節點child中的行號tem1=tree.parent_idx(tem1);%使用當前行的parent_idx更新tem1 end zuiyou_lujing=tree.child(zuiyou_idx,:); %畫出所生成的路徑 plot(zuiyou_lujing(:,1),zuiyou_lujing(:,2),'-r',"LineWidth",2); hold off;

其中自定義函數如下：

function [angle, min_idx,distance] = cal_distance(rd_x, rd_y, tree)distance = [];i = 1;while i<=size(tree.child,1)dx = rd_x - tree.child(i,1);dy = rd_y - tree.child(i,2);d = sqrt(dx^2 + dy^2);distance(i) = d;i = i+1;end[distance, min_idx] = min(distance);angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1)); end

運行代碼幾次的展示結果如下：

MATLAB自帶RRT算法實現

C=zeros(30,30);%一般默認權值大于0.65的網格為有障礙物的網格 %注意障礙物的權值按行列生成，而路徑曲線是以左下角為原點生成，注意兩者的區別 %C為生成整個網格地圖的各個網格權值的矩陣 C(15:18,10:16)=0.9*ones(4,7); C(21:26,15:27)=0.9*ones(6,13); C(6:12,3:15)=0.9*ones(7,13); %生成帶權值的網格地圖 costmap = vehicleCostmap(C,'CellSize',1);%每個網格邊長1米 %設置障礙物的膨脹范圍 vdims = vehicleDimensions(2.2,0.6,1.5, ...'FrontOverhang',0.37,'RearOverhang',0.32); numCircles = 3; ccConfig =inflationCollisionChecker(vdims,numCircles,'CenterPlacements',[0.2 0.5 0.8], ...'InflationRadius',0); costmap.CollisionChecker = ccConfig; %畫出整個地圖 figure,plot(costmap);hold on; set(gca,"XTick",0:1:30); set(gca,"YTick",0:1:30); grid on;%hold on; legend("off");startPose = [1, 1, 0]; % 起始點位置及姿態 goalPose = [27, 27, 90]; %終點位置及姿態planner = pathPlannerRRT(costmap); refPath = plan(planner,startPose,goalPose);%規劃路徑plot(planner);legend("off"); hold off

運行代碼幾次的展示結果如下：



注：主要思路參考自這位大神https://blog.csdn.net/m0_55205668/article/details/123922046

總結

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

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