目錄

1 RRT算法的簡介

2 RRT算法原理

2.1 算法流程

2.2 算法偽代碼

2.3 算法流程圖

3 RRT算法matlab實現

3.1 測試地圖

3.2 distance函數

3.3 RRT算法

3.4 動畫效果

4 RRT的缺陷

---

1 RRT算法的簡介

天下武功唯快不破，快是 RRT 的最大優勢。RRT 的思想是快速擴張一群像樹一樣的路徑以探索空間的大部分區域，找到可行的路徑。RRT 算法是一種對狀態空間隨機采樣的算法，通過對采樣點進行碰撞檢測，避免了對空間的精確建模帶來的大計算量，能夠有效地解決高維空間和復雜約束的路徑規劃問題。與PRM類似，該方法是概率完備且非最優的。可以輕松處理障礙物和差分約束（非完整和動力學）的問題，并被廣泛應用于機器人路徑規劃。

2 RRT算法原理

2.1 算法流程

（1）設定初始點??與目標點?，自行設定狀態采樣空間?

（2）進行隨機采樣得到采樣點 ，如果采樣點 在障礙物內，則重新隨機采樣

（3）若不在障礙物內，計算該采樣點??與集合 ?(已經生成的節點) 中的所有節點之間的距離，得到離得最近的節點?，再從節點??以步長??走向節點 ?，生成一個新的節點 ，若??與 ?的連線 ?經過障礙物，則重新隨機采樣

（4）經過反復迭代，生成一個隨機擴展樹，當隨機擴展樹中的子節點進入了我們規定的目標區域，便可以在隨機擴展樹中找到一條由從初始點到目標點的路徑。

2.2 算法偽代碼

可以將偽代碼與上述算法流程對照起來看

2.3 算法流程圖

3 RRT算法matlab實現

3.1 測試地圖

%隨機生成障礙物 function [f,n1]=ob(n) f=[];%儲存障礙物信息 n1=n;%返回障礙物個數 p=0; for i=1:nk=1;while(k)D=[rand(1,2)*60+15,rand(1,1)*1+3];%隨機生成障礙物的坐標與半徑，自行調整if(distance(D(1),D(2),90,90)>(D(3)+5)) %與目標點距離一定長度，防止過多阻礙機器人到達目標點k=0;endfor t=1:p %障礙物之間的距離不能過窄，可自行調整去測試if(distance(D(1),D(2),f(3*t-2),f(3*t-1))<=(D(3)+f(3*t)+5))k=1;endendend%畫出障礙物aplha=0:pi/40:2*pi;r=D(3);x=D(1)+r*cos(aplha);y=D(2)+r*sin(aplha);fill(x,y,'k');axis equal;hold on;xlim([0,100]);ylim([0,100]);f=[f,D];p=p+1;%目前生成的障礙物個數 end hold all;

3.2 distance函數

function f=distance(x,y,x1,y1)f=sqrt((x-x1)^2+(y-y1)^2);

3.3 RRT算法

clc clear all [f,n1]=ob(10);%隨機生成障礙物 Xinit=[1,1];%定義初始點 Xgoal=[90,90];%定義目標點 plot(Xinit(1),Xinit(2),'ro'); plot(Xgoal(1),Xgoal(2),'ko'); T=[Xinit(1),Xinit(2)];%已生成節點集合用順序表的數據結構存儲 Xnew=Xinit; D(1)=0;%初始節點的父節點指向0 while distance(Xnew(1),Xnew(2),Xgoal(1),Xgoal(2))>3 %進入目標區域Xrand=round(rand(1,2)*100)+1;%狀態采樣空間：橫縱坐標均為整數，范圍1~100k=1;%進入循環while k==1k=0;%初始化采樣成功for i=1:n1if distance(Xrand(1),Xrand(2),f(i*3-2),f(i*3-1))<(f(i*3)+1)%判斷隨機采樣點是否在障礙物內k=1;%采樣不成功break;endendXrand=round(rand(1,2)*100);%重新采樣endmin=10000;for i=1:size(T,2)/2 %遍歷已生成節點集合if distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2))<min %得到最近的節點min=distance(T(2*i-1),T(2*i),Xrand(1),Xrand(2));Xnear=[T(2*i-1),T(2*i)];endendstepsize=3/distance(Xnear(1),Xnear(2),Xrand(1),Xrand(2));%以一定步長前進，這里選擇3Xnew=[Xrand(1)*stepsize+Xnear(1)*(1-stepsize),Xrand(2)*stepsize+Xnear(2)*(1-stepsize)];t=0;%初始狀態未碰撞if Xnear(1)>Xnew(1)caiyang=-0.001;elsecaiyang=0.001;endfor i=Xnear(1):caiyang:Xnew(1)%均勻采樣進行碰撞檢測for j=1:n1if distance(f(3*j-2),f(3*j-1),i,Xnear(2)+(i-Xnear(1))/(Xnew(1)-Xnear(1))*(Xnew(2)-Xnear(2)))<=(f(3*j)+1)t=1;%代表碰撞break;endendif t==1break;endendif t==0T=[T,Xnew(1),Xnew(2)];for i=1:size(T,2)/2 %遍歷已生成節點集合if (T(i*2-1)==Xnear(1))&&(T(i*2)==Xnear(2)) %得到最近的節點的索引D(size(T,2)/2)=i;%記錄父節點break;endendplot([Xnew(1),Xnear(1)],[Xnew(2),Xnear(2)],'b-');hold on;pause(0.005);plot(Xnew(1),Xnew(2),'r.');xlim([0,100]);ylim([0,100]);end end i=size(T,2)/2; jg=[i]; while D(i)i=D(i); %通過鏈表回溯if D(i)~=0jg=[D(i),jg];%存儲最短路徑通過的節點end end Fx=T(jg(1)*2-1);Fy=T(jg(1)*2); i=2; while jg(i)~=size(T,2)/2x=T(jg(i)*2-1);y=T(jg(i)*2);plot([x,Fx],[y,Fy],'g-');hold on;pause(0.05);Fx=x;Fy=y;i=i+1; endplot([T(jg(i)*2-1),Fx],[T(jg(i)*2),Fy],'g-');hold on;pause(0.05);

3.4 動畫效果

4 RRT的缺陷

（1）很明顯RRT算法得到的路徑不是最優的

（2）RRT算法未考慮運動學模型

（3）RRT算法對于狹小的通道的探索性能不好，如下圖的對比，有可能探索不到出口

?

（4）沒有啟發信息的RRT像無頭蒼蠅，探索空間完全靠運氣，如下圖

?針對上述缺陷，又出現了很多RRT算法的變種，以后的文章中會介紹。

總結

以上是生活随笔為你收集整理的【自动驾驶轨迹规划之RRT算法】的全部內容，希望文章能夠幫你解決所遇到的問題。

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