一、RRT算法

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

?　　

　　RRT是一種多維空間中有效率的規(guī)劃方法。它以一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，通過隨機(jī)采樣增加葉子節(jié)點(diǎn)的方式，生成一個(gè)隨機(jī)擴(kuò)展樹，當(dāng)隨機(jī)樹中的葉子節(jié)點(diǎn)包含了目標(biāo)點(diǎn)或進(jìn)入了目標(biāo)區(qū)域，便可以在隨機(jī)樹中找到一條由從初始點(diǎn)到目標(biāo)點(diǎn)的路徑。基本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()

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

　　上述算法的效果是隨機(jī)采樣點(diǎn)會“拉著”樹向外生長，這樣能更快、更有效地探索空間（The effect is that the nearly uniformly distributed samples “pull” the?tree toward them, causing the tree to rapidly explore?C-Space）。隨機(jī)探索也講究策略，如果我們從樹中隨機(jī)取一個(gè)點(diǎn)，然后向著隨機(jī)的方向生長，那么結(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?）。可以看到，同樣是隨機(jī)樹，但是這棵樹并沒很好地探索空間。

二、部分代碼

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é)果?

??

?