【三維路徑規劃】基於RRT三維路徑規劃

對RRT演算法感興趣，是因為我對它不僅在二維平面上適用，在高維空間也能使用而感到心動，最近比較忙，下週或者什麼時候要要用程式碼親自實現一下。

　路徑規劃都有哪些方法呢？比較流行的有：圖搜尋、勢場法、RRT 等等。 

　 　RRT（快速探索隨機樹） 是一種通用的方法，不管什麼機器人型別、不管自由度是多少、不管約束有多複雜都能用。而且它的原理很簡單，這是它在機器人領域流行的主要原因之一。不過它的缺點也很明顯，它得到的路徑一般質量都不是很好，例如可能包含稜角，不夠光滑，通常也遠離最優路徑。

RRT 能在眾多的規劃方法中脫穎而出，它到底厲害在哪裡呢？ 
　　天下武功唯快不破，“快”是 RRT 的一大優點。RRT 的思想是快速擴張一群像樹一樣的路徑以探索（填充）空間的大部分割槽域，伺機找到可行的路徑。之所以選擇“樹”是因為它能夠探索空間。我們知道，陽光幾乎是樹木唯一的能量來源。為了最大程度地利用陽光，樹木要用盡量少的樹枝佔據儘量多的空間。當然了，能探索空間的不一定非得是樹，比如Peano曲線也可以做到，而且要多密有多密，如上圖左所示的例子。雖然像Peano曲線這樣的單條連續曲線也能探索空間，但是它太“確定”了。在搜尋軌跡的時候我們可不知道出路應該在哪裡，如果不在“確定”的搜尋方向上，我們怎麼找也找不到（找到的概率是0）。這時“隨機”的好處就體現出來了，雖然不知道出路在哪裡，但是通過隨機的反覆試探還是能碰對的，而且碰對的概率隨著試探次數的增多越來越大，就像買彩票一樣，買的數量越多中獎的概率越大（RRT名字中“隨機”的意思）。可是隨機試探也講究策略，如果我們從樹中隨機取一個點，然後向著隨機的方向生長，那麼結果是什麼樣的呢？見上圖右。可以看到，同樣是隨機樹，但是這棵樹並沒很好地探索空間，它一直在起點（紅點）附近打轉。這可不好，我們希望樹儘量經濟地、均勻地探索空間，不要過度探索一個地方，更不能漏掉大部分地方。這樣的一棵樹怎麼構造呢？ 

　RRT 的基本步驟是： 
　　1. 起點作為一顆種子，從它開始生長枝丫； 
　　2. 在機器人的“構型”空間中，生成一個隨機點 ； 
　　3. 在樹上找到距離  最近的那個點，記為  吧； 
　　4.  朝著 的方向生長，如果沒有碰到障礙物就把生長後的樹枝和端點新增到樹上，返回 2； 
　　隨機點一般是均勻分佈的，所以沒有障礙物時樹會近似均勻地向各個方向生長，這樣可以快速探索空間（RRT名字中“快速探索”的意思）。當然如果你事先掌握了最有可能發現路徑的區域資訊，可以集中兵力重點探索這個區域，這時就不宜用均勻分佈了。 

　　RRT 的一個弱點是難以在有狹窄通道的環境找到路徑。因為狹窄通道面積小，被碰到的概率低，找到路徑需要的時間要看運氣了。下圖展示的例子是 RRT 應對一個人為製作的很短的狹窄通道，有時RRT很快就找到了出路，有時則一直被困在障礙物裡面。 

　RRT探索空間的能力還是不錯的，例如下圖左所示的例子，障礙物多而且雜亂（藉助 Mathematica 本身具有的強大函式庫，實現這個例子所需的所有程式碼應該不會超過30行）。還有沒有環境能難住RRT呢？下圖右所示的迷宮對RRT就是個挑戰。這個時候空間被分割得非常嚴重，RRT顯得有些力不從心了，可見隨機策略不是什麼時候都有效的。 

　　“隨機”使得RRT有很強的探索能力。但是成也蕭何敗也蕭何，“隨機”也導致 RRT 很盲目，像個無頭蒼蠅一樣到處亂撞。如果機器人對環境一無所知，那麼採用隨機的策略可以接受。可實際情況是，機器人對於它的工作環境多多少少是知道一些的（即使不是完全知道）。我的部落格一直強調資訊對於機器人的重要性。這些已知的資訊就可以用來改進演算法。一個改進的辦法就是給它一雙“慧眼”：在勢場法中，勢函式攜帶了障礙物和目標的資訊，如果能把這個資訊告訴 RRT，讓它在探索空間時有傾向地沿著勢場的方向前進會更好。這樣，RRT 出色的探索能力剛好可以彌補勢場法容易陷入區域性極小值的缺點。

　　將RRT方法用在機械臂上的效果如下圖所示（綠色表示目標狀態）。我設定了4個障礙物（其中一個是大地），這對機械臂是個小小的挑戰。由於我們生活在三維空間，沒辦法看到六維關節空間，所以我把六維關節空間拆成了兩個三維空間，分別對應前三個關節和後三個關節（嚴格來說，六維轉動關節空間不是一個歐式空間，它是個流形，不過這裡我們不必糾結這個差別）： 

function c;
clc
clear all
close all
%map1 隨機地表。
% a=10;
% b=0.2;
% c=0.1;
% d=0.6;
% e=1;
% f=0.1;
% g=0.1;
% for x=1:80
%     for y=1:80
% Z1=sin(y+a)+b*sin(x)+cos(d*(x^2+y^2)^(1/2))+e*cos(y)+f*sin(f*(x^2+y^2)^(1/2))+g*cos(y);
% % Z1=SquareDiamond(6,2,8);
%  figure(1);
%  surf(Z1); %畫出三維曲面 
%  shading flat; %各小曲面之間不要網格 
% %map2 山峰圖
tic;
% h=[20,35,25,38,20,25];
% x0=[20,40,45,60,20,20];
% y0=[10,25,50,30,45,10];
% xi=[5.5,8,5,4.5,5.5,3.5];
% yi=[5,7,6,5.5,6,4.5];
h=[20,35,25,38,20,25];
x0=[20,40,45,60,20,20];
y0=[10,25,50,30,45,10];
xi=[5.5,8,5,4.5,5.5,3.5];
yi=[5,7,6,5.5,6,4.5];
Z2=CeatHill(6,h,x0,y0,xi,yi,80); 
figure(2);
surf(Z2); %畫出三維曲面 
shading flat; %各小曲面之間不要網格 
%map3 合成圖
%  Z3=max(Z1,Z2);
%  figure(3);
%  surf(Z3); %畫出三維曲面 
%  shading flat; %各小曲面之間不要網格 
segmentLength =5;
start_node =  [10,80,5,0,0,0];
end_node   =[60,0,5,1,0,0];
hold on
plot3(start_node(:,1),start_node(:,2),start_node(:,3),'r*');
plot3(end_node(:,1),end_node(:,2),end_node(:,3),'r*');
tree = start_node;
if ( (norm(start_node(1:3)-end_node(1:3))<segmentLength )...
    &(collision(start_node,end_node)==0) )
  path = [start_node; end_node];
  else
  numPaths = 0;
  while numPaths<1,
      [tree,flag] = extendTree(tree,end_node,segmentLength,Z2);
      numPaths = numPaths + flag;
  end
end
 path = findMinimumPath(tree);
 plot3(path(:,1),path(:,2),path(:,3),'r');  
 toc;
 function [data]=CeatHill(N,h,x0,y0,xi,yi,num) 
x=1:1:num;y=1:1:num;
for m=1:num
    for n=1:num
        Sum=0;
        for k=1:N
           s=h(k)*exp(-((x(m)-x0(k))/xi(k))^2-((y(n)-y0(k))/yi(k))^2);
           Sum=Sum+s;
        end
        data(m,n)=Sum;
    end
end

完整程式碼或者代寫新增QQ1575304183

往期回顧>>>>>>

【路徑規劃】基於BBO演算法的無人機三維路徑規劃matlab原始碼

【路徑規劃】基於SSA演算法的無人機三維路徑規劃matlab 原始碼

【路徑規劃】基於A星演算法的三維路徑規劃matlab原始碼

【路徑規劃】基於蟻群演算法的無人機路徑規劃matlab原始碼

【路徑規劃】基於粒子群的三維無人機路徑規劃matlab原始碼

【路徑規劃】基於粒子群的無人機三維路徑規劃含障礙matlab原始碼

【路徑規劃】基於nsga的無人機路徑規劃

【路徑規劃】基於人工蜂群的無人機三維路徑規劃

【路徑規劃】A*演算法解決三維路徑規劃問題

【路徑規劃】考慮分配次序的多無人機協同目標分配建模與遺傳演算法求解

【路徑規劃】基於改進差分之三維多 無人機協同航跡規劃

【路徑規劃】基於人工勢場的無人機三維路徑規劃matlab原始碼

【路徑規劃】基於狼群演算法之三維路徑規劃matlab原始碼