一、RRT算法

傳統(tǒng)的路徑規(guī)劃算法有人工勢場法、模糊規(guī)則法、遺傳算法、神經(jīng)網(wǎng)絡(luò)、模擬退火算法、蟻群優(yōu)化算法等。但這些方法都需要在一個確定的空間內(nèi)對障礙物進行建模，計算復雜度與機器人自由度呈指數(shù)關(guān)系，不適合解決多自由度機器人在復雜環(huán)境中的規(guī)劃?；诳焖贁U展隨機樹（RRT /?rapidly exploring random tree）的路徑規(guī)劃算法，通過對狀態(tài)空間中的采樣點進行碰撞檢測，避免了對空間的建模，能夠有效地解決高維空間和復雜約束的路徑規(guī)劃問題。該方法的特點是能夠快速有效地搜索高維空間，通過狀態(tài)空間的隨機采樣點，把搜索導向空白區(qū)域，從而尋找到一條從起始點到目標點的規(guī)劃路徑，適合解決多自由度機器人在復雜環(huán)境下和動態(tài)環(huán)境中的路徑規(guī)劃。與PRM類似，該方法是概率完備且不最優(yōu)的。

?　　

　　RRT是一種多維空間中有效率的規(guī)劃方法。它以一個初始點作為根節(jié)點，通過隨機采樣增加葉子節(jié)點的方式，生成一個隨機擴展樹，當隨機樹中的葉子節(jié)點包含了目標點或進入了目標區(qū)域，便可以在隨機樹中找到一條由從初始點到目標點的路徑。基本RRT算法如下面?zhèn)未a所示：

1 function BuildRRT(qinit, K, Δq) 2 ? ? T.init(qinit) 3 ? ? for k = 1 to K 4 ? ? ? ? qrand = Sample() ?-- chooses a random configuration 5 ? ? ? ? qnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrand 6 ? ? ? ? if ?Distance(qnearest, qgoal) < Threshold then 7 ? ? ? ? ? ? return true 8 ? ? ? ? qnew = Extend(qnearest, qrand, Δq) ?-- moving from qnearest an incremental distance in the direction of qrand 9 ? ? ? ? if qnew ≠ NULL then 10 ? ? ? ? ? ? T.AddNode(qnew) 11 ? ? return false 12 13 14 function Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstacle 15 ? ? p = Random(0, 1.0) 16 ? ? if 0 < p < Prob then 17 ? ? ? ? return qgoal 18 ? ? elseif Prob < p < 1.0 then 19 ? ? ? ? return RandomNode()

　　初始化時隨機樹T只包含一個節(jié)點：根節(jié)點qinit。首先Sample函數(shù)從狀態(tài)空間中隨機選擇一個采樣點qrand（4行）；然后Nearest函數(shù)從隨機樹中選擇一個距離qrand最近的節(jié)點qnearest（5行）；最后Extend函數(shù)通過從qnearest向qrand擴展一段距離，得到一個新的節(jié)點qnew（8行）。如果qnew與障礙物發(fā)生碰撞，則Extend函數(shù)返回空，放棄這次生長，否則將qnew加入到隨機樹中。重復上述步驟直到qnearest和目標點qgaol距離小于一個閾值，則代表隨機樹到達了目標點，算法返回成功（6~7行）。為了使算法可控，可以設(shè)定運行時間上限或搜索次數(shù)上限（3行）。如果在限制次數(shù)內(nèi)無法到達目標點，則算法返回失敗。
　　為了加快隨機樹到達目標點的速度，簡單的改進方法是：在隨機樹每次的生長過程中，根據(jù)隨機概率來決定qrand是目標點還是隨機點。在Sample函數(shù)中設(shè)定參數(shù)Prob，每次得到一個0到1.0的隨機值p，當0<p<Prob的時候，隨機樹朝目標點生長行；當Prob<p<1.0時，隨機樹朝一個隨機方向生長。

　　上述算法的效果是隨機采樣點會“拉著”樹向外生長，這樣能更快、更有效地探索空間（The effect is that the nearly uniformly distributed samples “pull” the?tree toward them, causing the tree to rapidly explore?C-Space）。隨機探索也講究策略，如果我們從樹中隨機取一個點，然后向著隨機的方向生長，那么結(jié)果是什么樣的呢？見下圖（Left:?A tree generated by applying a uniformly-distributed random?motion from a randomly chosen tree node does not explore very far. Right:?A tree?generated by the RRT algorithm using samples drawn randomly from a uniform distribution. Both trees have 2000 nodes?）?？梢钥吹?，同樣是隨機樹，但是這棵樹并沒很好地探索空間。

二、部分代碼

clearvars close all rand('state',1); %% env env.x_max = 500; env.y_max = 500; x_max=500; y_max=500; width_c=50; width_b=100; l_block=0.5*(x_max-width_c); h_block=(y_max-2*width_b); obstacle1 = [0,width_b,l_block,h_block]; obstacle2 = [x_max-l_block,width_b,l_block,h_block]; ob_vec=[obstacle1;obstacle2]; env.ob_vec=ob_vec; %% constant definition EPS = 10; rho=40; ? ?BETA=EPS/rho; numNodes=1000; %% Agent1 agent1.q_start.coord = [20 20]; agent1.q_start.dir=pi/4; agent1.q_start.cost = 0; agent1.q_start.parent = 0; agent1.q_goal.coord = [x_max-20 y_max-20]; agent1.q_goal.cost = 0; agent1.q_goal.dir=0; % agent1.nodes(1) =agent1.q_start; % agent1.q_ci=agent1.q_start.coord; % agent1.phi_ci=agent1.q_start.dir; % agent1.i=1; % agent1.confliction.state=zeros(num_agent,1); % agent1.confliction.agents=[]; %% Agent2 agent2.q_start.coord = [x_max-20 20]; agent2.q_start.dir=-3*pi/4; agent2.q_start.cost = 0; agent2.q_start.parent = 0; agent2.q_goal.coord = [x_max-20 y_max-20]; agent2.q_goal.cost = 0; agent2.q_goal.dir=0; plot_env(env,1); hold on plot(agent1.q_goal.coord(1),agent1.q_goal.coord(2),'ro','LineWidth',2) plot(agent2.q_goal.coord(1),agent2.q_goal.coord(2),'ro','LineWidth',2) path1=RRTStar_FindPath(agent1,EPS,rho,numNodes,env); path2=RRTStar_FindPath(agent2,EPS,rho,numNodes,env); newpath1=fixPath(path1,rho,7); newpath2=fixPath(path2,rho,7); save('newpath_data2','newpath1','newpath2') plot(newpath1(:,1),newpath1(:,2),'y.') ? plot(newpath2(:,1),newpath2(:,2),'b.') plot(path1(:,1),path1(:,2),'g--') ? plot(path2(:,1),path2(:,2),'m--') plot(newpath1(:,1),newpath1(:,2),'y--') ? plot(newpath2(:,1),newpath2(:,2),'p--')

三、仿真結(jié)果?

??

?