根據(jù)查閱網(wǎng)上關(guān)于RRT算法的原理及代碼，再結(jié)合自己的理解，使用MATLAB實(shí)現(xiàn)RRT算法生成路徑的整個(gè)過(guò)程。RRT的原理部分我不再贅述，網(wǎng)上介紹的文章還是很多的。
下面主要展示自己編的RRT算法，并使用MATLAB自帶的RRT路徑規(guī)劃函數(shù)進(jìn)行簡(jiǎn)單比對(duì)。

RRT算法自編代碼實(shí)現(xiàn)

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

其中自定義函數(shù)如下：

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

運(yùn)行代碼幾次的展示結(jié)果如下：

MATLAB自帶RRT算法實(shí)現(xiàn)

C=zeros(30,30);%一般默認(rèn)權(quán)值大于0.65的網(wǎng)格為有障礙物的網(wǎng)格 %注意障礙物的權(quán)值按行列生成，而路徑曲線是以左下角為原點(diǎn)生成，注意兩者的區(qū)別 %C為生成整個(gè)網(wǎng)格地圖的各個(gè)網(wǎng)格權(quán)值的矩陣 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); %生成帶權(quán)值的網(wǎng)格地圖 costmap = vehicleCostmap(C,'CellSize',1);%每個(gè)網(wǎng)格邊長(zhǎng)1米 %設(shè)置障礙物的膨脹范圍 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; %畫(huà)出整個(gè)地圖 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]; % 起始點(diǎn)位置及姿態(tài) goalPose = [27, 27, 90]; %終點(diǎn)位置及姿態(tài)planner = pathPlannerRRT(costmap); refPath = plan(planner,startPose,goalPose);%規(guī)劃路徑plot(planner);legend("off"); hold off

運(yùn)行代碼幾次的展示結(jié)果如下：



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

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。