摘要
基于地圖環境已知下的機器人自主導航，通過Matlab2018實作RRT與雙向RRT演算法實作全域路徑規劃，并將規劃的點列寫入txt檔案，之后通過VS2013讀取點列資料并以一種比較粗糙的控制方法實作對機器人移動路徑控制，實作從起始點運動到目標點的任務，
關鍵詞： RRT；雙向擴展平衡的連結型雙樹RRT；全域路徑規劃；機器人自主導航

一、實驗方法-基于地圖已知全域路徑規劃

傳統的全域路徑規劃演算法有人工勢場法、模糊規則法、遺傳演算法、神經網路、模擬退火演算法、蟻群優化演算法等，但這些方法都需要在一個確定的空間內對障礙物進行建模，計算復雜度與機器人自由度呈指數關系，不適合解決多自由度機器人在復雜環境中的規劃，基于快速擴展隨機樹（RRT / rapidly exploring random tree）的路徑規劃演算法，通過對狀態空間中的采樣點進行碰撞檢測，避免了對空間的建模，能夠有效地解決高維空間和復雜約束的路徑規劃問題，該方法的特點是能夠快速有效地搜索高維空間，通過狀態空間的隨機采樣點，把搜索導向空白區域，從而尋找到一條從起始點到目標點的規劃路徑，適合解決多自由度機器人在復雜環境下和動態環境中的路徑規劃，
本實驗在理解了RRT演算法為基礎，使用雙向擴展平衡的連結型雙樹RRT演算法（雙向RRT），即RRT_Connect演算法來實作在地圖已知情況下的全域路徑規劃問題，采用Matlab與VS2013以txt檔案通信相結合方式，

1.1RRT演算法

圖一：RRT演算法 RRT是一種多維空間中有效率的規劃方法，它以一個初始點作為根節點，通過隨機采樣增加葉子節點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節點包含了目標點或進入了目標區域，便可以在隨機樹中找到一條由從初始點到目標點的路徑，如圖一所示，紅線標注路徑即為RRT演算法找到一條由初始點到目標點路徑， ## 1.2雙向RRT 基本的RRT每次搜索都只有從初始狀態點生長的快速擴展隨機樹來搜索整個狀態空間，如果從初始狀態點和目標狀態點同時生長兩棵快速擴展隨機樹來搜索狀態空間，效率會更高，為此，基于雙向擴展平衡的連結型雙樹RRT演算法，即RRT_Connect演算法被提出， 其基本演算法：  該演算法與原始RRT相比，在目標點區域建立第二棵樹進行擴展，每一次迭代中，開始步驟與原始的RRT演算法一樣，都是采樣隨機點然后進行擴展，然后擴展完第一棵樹的新節點$𝑞𝑛𝑒𝑤$后，以這個新的目標點作為第二棵樹擴展的方向，同時第二棵樹擴展的方式略有不同（15~22行），首先它會擴展第一步得到$𝑞′𝑛𝑒𝑤$，如果沒有碰撞，繼續往相同的方向擴展第二步，直到擴展失敗或者$𝑞′𝑛𝑒𝑤=𝑞𝑛𝑒𝑤$表示與第一棵樹相連了，即connect了，整個演算法結束，當然每次迭代中必須考慮兩棵樹的平衡性，即兩棵樹的節點數的多少（也可以考慮兩棵樹總共花費的路徑長度），交換次序選擇“小”的那棵樹進行擴展，這種雙向的RRT技術具有良好的搜索特性，比原始RRT演算法的搜索速度、搜索效率有了顯著提高，被廣泛應用，首先，Connect演算法較之前的演算法在擴展的步長上更長，使得樹的生長更快；其次，兩棵樹不斷朝向對方交替擴展，而不是采用隨機擴展的方式，特別當起始位姿和目標位姿處于約束區域時，兩棵樹可以通過朝向對方快速擴展而逃離各自的約束區域，這種帶有啟發性的擴展使得樹的擴展更加貪婪和明確，使得雙向RRT演算法較之單向RRT演算法更加有效，

1.3 Matlab與VS2013相結合

如圖二程式設計圖所示，Matlab與VS2013資料通信采用了txt方式，具體操縱說明如下：

圖二 程式設計圖

1.3.1在VS2013中對txt進行操作

（1）將初始點與目標點資料寫入txt以供matlab讀取

void outdata()
{
	INT16 Code[4] = { Initial_rPos.coor_x, Initial_rPos.coor_y, Cur_dPos.coor_x, Cur_dPos.coor_y };//需要存入的資料
	ofstream output;
	output.open("D:\\Users\\Desktop\\Machine\\Plan_client\\Point.txt");//存入的檔案路徑
	if (!output.is_open())
	{
		cout << "the file open fail" << endl;
		exit(1);
	}
	for (int i = 0; i < 4; i++)
	{
		output << Code[i] << " " << endl;
	}
	output.close();
}

（2）在matlab生成坐標點序列后，VS要讀取相應txt檔案，以獲取matlab的點列

void indata()
{
	ifstream input;
	input.open("E:\\Matlab2018a\\Path_Plan\\Plan_path.txt");
	if (!input.is_open())
	{
		cout << "the file open fail" << endl;
		exit(1);
	}
	for (int i = 0; i < 100; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			input >> Trajm[i][j];
			if (Trajm[i][j] != 0)
				steps++;
		}
	}
	input.close();
}

1.3.2在Matlab2018a中對txt進行操作

（1）讀取VS2013寫入的txt檔案獲取初始點與目標點

  Point=load('D:\\Users\\Desktop\\Machine\\Plan_client\\Point.txt')        

（2）將全域規劃的點列寫入txt檔案以供VS讀取

fid = fopen('Plan_path.txt','wt');
mat = int16(New_path);
for i = 1:size(mat, 1)
    fprintf(fid, '%d', mat(i,1));fprintf(fid, '\n');
    fprintf(fid, '%d', mat(i,2));fprintf(fid, '\n');
end

二、實驗程序

2.1從單向RRT出發——發現問題

在matlab2018a撰寫RRT程式，程式見附錄1，實作800*1200像素的圖片上尋找目標點到初始點可行路徑，在這里先不考慮與VS2013的資料通信，選取初始點為（50，50），目標點為（450，450），matlab運行結果如圖三所示，經過反復嘗試，設定步長為30，節點閾值為20時，既可以滿足全域路徑規劃的速度性，又可以避免在臨近目標點出現多次路徑尋找，

圖三：RRT演示

但是在實驗中發現，RRT全域路徑規劃會出現如圖四所示的情況，對于RRT本身來說這是沒有問題的，但是考慮到VS程式中我們假定機器人是一個半徑為15的圓，這樣此圖RRT的軌跡便使機器人與障礙物相撞，為了避免這種情況發生，采取了使障礙物膨脹的方法，

圖四：RRT路徑

此外為了使目標點更好向外擴張，并加快程式的運行速度，我使用時RRT引入了比較常用的概率法：在隨機樹每次的生長程序中，根據隨機概率來決定qrand是目標點還是隨機點，在Sample函式中設定引數Prob，每次得到一個0到1.0的隨機值p，當0<p<Prob的時候，隨機樹朝目標點生長行；當Prob<p<1.0時，隨機樹朝一個隨機方向生長，我設定Prob設定為0.5，經過圖三與圖四對比實驗發現：該方法確實減少了無用路徑擴展，加快了程式的運行速度，

圖五：RRT效果圖

經過文獻查找，上述采用RRT為單向RRT演算法，每次搜索都只有從初始狀態點生長的快速擴展隨機樹來搜索整個狀態空間，如果從初始狀態點和目標狀態點同時生長兩棵快速擴展隨機樹來搜索狀態空間，效率會更高，于是接下來在單向RRT基礎上，進行了雙向RRT實驗，

2.2障礙物膨脹

障礙物膨脹的原理很簡單：以某白像素點為中心，上移20，下移20，左移20，右移20，形成的40*40的區域內進行判斷，縮小計算量，之后如果遍歷該區域，如果有黑像素點，且該黑像素點到白像素點歐式距離小于等于20，則該白像素點變為黑像素點，Matlab程式如下，程式包含了對圖邊緣的處理，其中MapT為標志位，目的是使已經變為黑像素的點不影響之后的判斷，

MapT=logical(ones(800,1200)-1); 
 for i=1:800
    for j=1:1200
        if((map(i,j)==true)&&(MapT(i,j)==false))
            if(i-20>0),m1=i-20;else,m1=1;end
            if(i+20<=800),m2=i+20;else,m2=800;end
            if(j-20>0),n1=j-20;else,n1=1;end
            if(j+20<=1200),n2=j+20;else,n2=1200;end
            num=0;
            for m=m1:m2
              for n=n1:n2
                  if(((m-i)^2+(n-j)^2<=20^2)&&(MapT(m,n)==false))…
&&(num==0)&&(map(m,n)==false)                     
                   map(i,j)=false;MapT(i,j)=true;num=1;end
              end
            end  
        end    
    end
 end

其實作效果如圖六所示，仔細觀察下圖我們發現障礙物膨脹邊緣會不平整，但是因為膨脹20的緣故，機器人在運動時候不會受其影響而出現碰撞的情況，這也就是選取障礙物膨脹20的原因，

圖六：障礙物膨脹圖

2.3 雙向RRT實作

在單向RRT演算法的基礎上實作雙向RRT演算法比較簡單，參考網上資料與自己理解，撰寫雙向RRT的RRT_Connect程式，程式詳見附錄2，運行程式如圖七所示，

a

b

圖七：雙向RRT
圖（a）中藍線表示從初始點向目標點擴展，紅線表示從目標點向初始點擴展，圖中一條綠線表示就是就是最后一步，圖（b）就是雙向RRT獲取的全域路徑，我們可以明顯看到路徑是曲折的，那么VS獲取這樣的路徑點去控制機器人移動是比較費勁，為何不把路徑點去掉一些，使得路徑比較平整呢？

2.4 路徑化直

雙向RRT回傳回來的資料是一系列的坐標點，這些坐標點是隨機擴展出來的，這些點連接接起來的路徑時曲折的，為了使機器人少走彎路，將坐標點去掉一部分，并將路徑化直，該想法實作效果圖如圖八所示，（b）圖將（a）圖的RRT生成點軌跡化直，

a

b 圖八 路徑化直 實作程式如下：基本思想就是對雙向RRT獲取路徑點進行取優，假設路徑點序列為A，A[0]一定與A[1]無障礙相連，那以A[0]為起點依次判斷A[1]、A[2]…是否無障礙相連，假設A[0]與A[i]之間是存在障礙物，那么A[0]與A[i-1]一定是無障礙相連，這樣便可以省去A[1]至A[i-2]的點，之后再以A[i-1]為起點重復上述操作思想，直至遍歷到最后一點，

    New_path(1,:)=path(1,:);
     step=size(path,1);
     j=1;
     sign=0;
     a=New_path(j,:);
     for i=2:step
         b=path(i,:);
         if(a(1)<b(1)),m1=a(1);m2=b(1);else,m1=b(1);m2=a(1);end
         if(a(2)<b(2)),n1=a(2);n2=b(2);else,n1=b(2);n2=a(2);end         
         for m=m1:m2
             for n=n1:n2
                 if(((map(m,n)==false)&&(map(m,n+1)==false))&&(det([a-b;[m,n]-b])*det([a-b;[m,n+1]-b])<=0))...
                 ||(((map(m,n)==false)&&(map(m+1,n)==false))&&(det([a-b;[m,n]-b])*det([a-b;[m+1,n]-b])<=0))...
                 ||(((map(m,n)==false)&&(map(m+1,n+1)==false))&&(det([a-b;[m,n]-b])*det([a-b;[m+1,n+1]-b])<=0))                  
                 sign=1; break;end   
             end
             if(sign==1),break;end            
         end
         if(sign==1),j=j+1;New_path(j,:)=path(i-1,:);a=New_path(j,:);end
         sign=0;
     end
     j=j+1;New_path(j,:)=path(i,:);

2.5 機器人控制

雙向RRT回傳路徑點序列， VS2013只要根據這些點序列控制機器人移動即可，程式控制思想如下：角速度控制與線速度控制分開，機器人先轉角度，在移動，粗糙一點，直接計算角度差與距離差，通過固定幀數平攤角度差與距離差賦值給角速度與線速度，這樣便可以實作對機器人控制，程式見附錄3，

三、實驗結果

3.1雙向RRT實作

雙向RRT路徑點規劃結果如圖九所示，這里去掉膨脹后顯示的路徑規劃，實際上也就是機器人移動路徑，

a

b

c

d 圖九：RRT路徑規劃圖

通過實驗發現，Matalb2018a實作雙向RRT全域路徑規劃回傳了精確的點序列，方便了在VS2013端實作對機器人運動的控制，

3.2機器人控制實作

通過設定斷點方式并利用txt文本檔案實作VS2013與Matlab2018a通信的操作，便可實作對機器人控制，通過上述圖九回傳的全域規劃路徑，并利用之前對機器人控制方法可以實作對機器人控制，控制結果如圖十所示，下面的圖(a)引數設定為10幀，及每一次機器人轉10次旋轉到下一次要運動方向，機器人位移10次便到達下一個位置點，同理圖(b)與圖?設定為50幀，而圖(d)引數設定為100幀，

a

b

c

d 圖十：機器人控制結果圖

通過實驗結果，VS2013獲取雙向RRT的路徑規劃點序列后，通過粗糙的控制方法是可以實作機器人運動控制，當然也存在一些不足，這里放在實驗總結中去詳細敘述，

四、實驗總結

本次作業使用Matlab2018a與VS2013雙軟體完成，Matlab2018a實作雙向RRT程式的“外包“，當然這樣外包是在通過txt文本檔案與VS斷點實作資料傳輸通信，它通過讀入機器人設定的起始點與目標點坐標規劃出一條可行路徑，并將此路徑點序列寫入txt文本檔案，VS2013在繼續運行后讀取RRT路徑規劃點序列并通過粗糙的控制演算法實作對機器人運動控制，當然這樣方法存在一些不足之處，

4.1不足之處

4.1.1雙軟體——操作繁瑣

如果要實作上述方法，首先在VS2013中讀取RRT點列前設定斷點，然后運行VS程式，選擇好起始點與目標點后，程式將兩點資訊寫入txt檔案中后便進入斷點暫停，這時運行Matlab中的RRT_Connect程式，程式便會讀取你設定的起始點與目標點資訊，經過雙向RRT后獲取優化的路徑點序列并將點序列寫入另一個txt檔案中，完成后你便可以繼續運行VS程式，最終你會看到機器人在你控制下從起始點運動至目標點，這樣的操作步驟是比較繁瑣的，但是幸好傳遞過來的資料是非常精確的，不會出現資料傳輸錯誤問題，

4.1.2 粗糙控制方法存在弊端——決定運行速度不能過快

最終程式選用50幀引數，因為50幀意味著50次完成依次角度旋轉或位置移動，每一次旋轉角度，位移距離都是存在誤差的，并且這樣的誤差會累積，形成累計誤差，如果累計誤差過大會深深影響機器熱的運動精度，最終導致機器人偏離預想航向，如下圖十一所示，當幀數設定為5，其累計誤差較大，最終出現碰撞，

圖十一 碰撞

4.1.3 起始點與目標點不能設定靠近障礙物20范圍內

因為當初為了避免RRT演算法出現通過狹窄路徑情況并避免機器人半徑15帶來的影響，選擇在進行RRT演算法之前將障礙物膨脹20，如果你選定目標點與起始點靠近障礙物20內，RRT演算法將自動出現錯誤，因為讀入資料將首先進行與地圖比較，程式如下，看是否與地圖內障礙物沖突，

if ~feasiblePoint(source,map), error('source lies on an obstacle or outside map'); end
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map'); end
…………………………………………………………………………….
%% feasiblePoint函式 檢查無碰撞點和內部地圖
function feasible=feasiblePoint(point,map)
    feasible=true;
    if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)
    feasible=false;
    end
end

致謝
感謝石老師在機器人自主導航與環境建模這門課上的精彩講解，通過對門課的學習與實踐，使我對視覺感知、多傳感器資訊融合、運動規劃與自主定位有了初步的了解，在此了解的基礎上，我深入學習了運動規劃的內容，并利用其中的RRT演算法實作了本次個人作業的考核要求，此外還要感謝石老師認真對我們作業遇到問題進行答疑，解答我們的困惑，使我和同學們能順利完成此次個人作業，
二?二?年十二月 于南京
鏈接: 視頻演示：link.
程式代碼：link.