做迷宮類路徑規(guī)劃項(xiàng)目很多人的第一反應(yīng)都是上A*或者把地圖柵格化之后用Dijkstra。在規(guī)整的網(wǎng)格環(huán)境里這些確定性搜索算法確實(shí)好用可一旦迷宮變得極其困難——通道比車輛自身寬度大不了多少、U形死角一個(gè)接一個(gè)、障礙物還是隨機(jī)生成的——傳統(tǒng)方法的效率就會(huì)斷崖式下跌。后來(lái)我把RRTRapidly-exploring Random Tree快速擴(kuò)展隨機(jī)樹算法用到這套場(chǎng)景里配合Matlab做了完整Demo實(shí)測(cè)下來(lái)無(wú)論迷宮怎么隨機(jī)變化只要參數(shù)調(diào)對(duì)基本都能穩(wěn)定找到可行路徑。這篇文章就把整體思路、Matlab代碼、調(diào)參經(jīng)驗(yàn)和踩過(guò)的坑一次性講清楚適合正在做路徑規(guī)劃課設(shè)、機(jī)器人導(dǎo)航仿真或者想入門采樣類規(guī)劃算法的讀者。1. 為什么傳統(tǒng)路徑規(guī)劃方法會(huì)在隨機(jī)迷宮里“翻車”1.1 迷宮問(wèn)題的結(jié)構(gòu)特點(diǎn)先說(shuō)清楚“極其困難的隨機(jī)迷宮”到底難在哪。我做的測(cè)試環(huán)境是一個(gè)二維柵格地圖尺寸在100×100左右墻的密度很高通道寬度通常只有2到3個(gè)柵格。更關(guān)鍵的是迷宮由算法隨機(jī)生成沒(méi)有什么“主路”和“支路”之分大量的狹窄走廊、死胡同、回字形陷阱隨機(jī)堆疊在一起這種結(jié)構(gòu)對(duì)路徑規(guī)劃算法極不友好。這類環(huán)境的本質(zhì)問(wèn)題是可行通道在狀態(tài)空間中占比極低而且拓?fù)潢P(guān)系高度非結(jié)構(gòu)化。通道之間互相纏繞沒(méi)有任何先驗(yàn)規(guī)則可循。你想讓算法“動(dòng)腦筋”判斷哪個(gè)方向更有可能通向終點(diǎn)它根本無(wú)從下手因?yàn)閴竺媸鞘裁赐耆粗?。柵格法在這里也容易踩坑。如果柵格粒度設(shè)得太大那些兩格寬的通道直接就消失了物理上可行的路徑在數(shù)學(xué)表示里就不存在粒度設(shè)小一點(diǎn)地圖規(guī)模又會(huì)以平方級(jí)別膨脹搜索耗時(shí)迅速失控。我在85×85的迷宮上試過(guò)5像素粒度的柵格化光是構(gòu)建鄰接表就耗掉大半內(nèi)存路徑搜索時(shí)間更是無(wú)法接受。1.2 A*、Dijkstra等確定性方法的局限A*的搜索效率高度依賴啟發(fā)函數(shù)的質(zhì)量。在開闊地圖里歐氏距離或曼哈頓距離能提供很有效的方向指引但在迷宮場(chǎng)景下啟發(fā)值經(jīng)常嚴(yán)重“失真”——明明終點(diǎn)就在墻對(duì)面實(shí)際繞過(guò)去卻要幾千個(gè)格子啟發(fā)函數(shù)給出的距離估計(jì)和真實(shí)代價(jià)相差懸殊算法就會(huì)大量擴(kuò)展低價(jià)值節(jié)點(diǎn)表現(xiàn)和一個(gè)盲目廣搜沒(méi)有本質(zhì)區(qū)別。Dijkstra更不用說(shuō)了在迷宮環(huán)境里它基本是在做全圖遍歷時(shí)間復(fù)雜度和地圖面積直接掛鉤。我拿30×30的小迷宮讓Dijkstra跑路徑倒是能找到耗時(shí)已經(jīng)需要好幾秒換成60×60的隨機(jī)迷宮耗時(shí)直接變成分鐘級(jí)而且這個(gè)問(wèn)題和硬件性能關(guān)系不大純粹是算法復(fù)雜度決定的。這引出一個(gè)關(guān)鍵結(jié)論當(dāng)環(huán)境的拓?fù)浣Y(jié)構(gòu)極度復(fù)雜時(shí)任何基于“確定性枚舉”的搜索策略都會(huì)遇到組合爆炸問(wèn)題。這也是為什么我會(huì)轉(zhuǎn)向采樣類算法——它們不試圖枚舉所有可能性而是通過(guò)隨機(jī)采樣的方式在狀態(tài)空間里“抽獎(jiǎng)”用概率換時(shí)間。2. RRT快速擴(kuò)展隨機(jī)樹以隨機(jī)性對(duì)抗復(fù)雜性的核心思路2.1 RRT的核心流程拆解RRT的思想可以用一個(gè)生活化類比來(lái)理解想象你在一片完全黑暗、遍布障礙的森林里找人你手里只有一條能自動(dòng)伸縮的繩子。你每走一步就往隨機(jī)方向甩一小段繩子只要繩子沒(méi)被障礙擋住就沿著這個(gè)方向前進(jìn)一點(diǎn)然后把繩子另一頭固定住。重復(fù)無(wú)數(shù)次之后繩子的網(wǎng)絡(luò)會(huì)逐漸覆蓋整個(gè)森林只要存在一條通路這根繩子大概率最終會(huì)碰到目標(biāo)。算法落到代碼層面每輪迭代就做四件事隨機(jī)采樣一個(gè)點(diǎn)、在現(xiàn)有樹上找到離它最近的節(jié)點(diǎn)、從最近節(jié)點(diǎn)朝采樣點(diǎn)方向延伸一個(gè)固定步長(zhǎng)、檢測(cè)這段新路徑是否撞障礙。如果沒(méi)撞新節(jié)點(diǎn)就加入樹中如果撞了這一輪就放棄重新采樣。上面說(shuō)的是最基礎(chǔ)的RRT。實(shí)際工程里我會(huì)額外引入一個(gè)目標(biāo)偏置策略每次采樣時(shí)以一定概率比如10%直接把目標(biāo)點(diǎn)作為采樣點(diǎn)其余90%才做全圖隨機(jī)采樣。這樣做的目的是給樹的生長(zhǎng)施加一個(gè)“方向拉力”避免樹完全漫無(wú)目的地亂長(zhǎng)同時(shí)又保留了隨機(jī)性去探索復(fù)雜通道。2.2 為什么RRT天生適合復(fù)雜迷宮RRT最迷人的地方在于它的概率完備性只要迭代次數(shù)足夠多找到一條可行路徑的概率趨近于1。這個(gè)性質(zhì)不需要對(duì)地圖做任何假設(shè)管它是規(guī)整網(wǎng)格還是隨機(jī)迷宮。原因很簡(jiǎn)單——隨機(jī)采樣點(diǎn)在整個(gè)自由空間里均勻分布樹的擴(kuò)展方向又不受任何啟發(fā)函數(shù)的誤導(dǎo)所以隨著采樣點(diǎn)數(shù)量的增加樹對(duì)自由空間的覆蓋會(huì)越來(lái)越密最終“必然”觸及目標(biāo)所在的連通分量。實(shí)際測(cè)試也驗(yàn)證了這一點(diǎn)。在通道寬度只有3個(gè)柵格的85×85隨機(jī)迷宮里RRT通常只需要幾千次迭代就能找到路徑耗時(shí)在2到5秒之間這個(gè)表現(xiàn)和A*在高密度迷宮里的表現(xiàn)形成了鮮明對(duì)比。另外RRT還有一個(gè)隱性優(yōu)勢(shì)它對(duì)車輛的運(yùn)動(dòng)約束天然友好。在這類算法里車輛的運(yùn)動(dòng)模型是作為“擴(kuò)展規(guī)則”的一個(gè)輸入存在的——每次步進(jìn)的方向不是隨便定的而是車輛在當(dāng)前狀態(tài)下能夠執(zhí)行的方向。這意味著從仿真到實(shí)車時(shí)只需要替換掉步進(jìn)函數(shù)算法骨架完全不用動(dòng)。傳統(tǒng)A*規(guī)劃出來(lái)的路徑常常是一堆直角折線到了實(shí)車上還得再做一次運(yùn)動(dòng)約束校驗(yàn)這部分工作RRT在結(jié)構(gòu)上就替你省了。3. Matlab環(huán)境下的迷宮生成與參數(shù)設(shè)定3.1 用遞歸回溯算法生成隨機(jī)迷宮測(cè)試迷宮我用的是經(jīng)典的遞歸回溯算法Recursive Backtracker生成。這個(gè)算法的思路很直觀把每個(gè)格子視為一個(gè)房間從起點(diǎn)出發(fā)隨機(jī)選擇一個(gè)尚未訪問(wèn)的相鄰房間打通中間的墻走過(guò)去繼續(xù)這個(gè)過(guò)程如果當(dāng)前房間周圍沒(méi)有未訪問(wèn)的鄰居就沿來(lái)路回溯。全部房間都訪問(wèn)過(guò)之后迷宮就生成了。Matlab實(shí)現(xiàn)如下function maze generateMaze(rows, cols) maze ones(rows*21, cols*21); visited false(rows, cols); stack [1, 1]; visited(1,1) true; % 起點(diǎn)房間打通 maze(2, 2) 0; while ~isempty(stack) current stack(end,:); r current(1); c current(2); % 隨機(jī)選擇未訪問(wèn)鄰居 neighbors []; if r 1 ~visited(r-1, c) neighbors(end1,:) [r-1, c, 1]; end if r rows ~visited(r1, c) neighbors(end1,:) [r1, c, 2]; end if c 1 ~visited(r, c-1) neighbors(end1,:) [r, c-1, 3]; end if c cols ~visited(r, c1) neighbors(end1,:) [r, c1, 4]; end if ~isempty(neighbors) idx randi(size(neighbors,1)); nr neighbors(idx,1); nc neighbors(idx,2); dir neighbors(idx,3); % 打通當(dāng)前房間與鄰居之間的墻 if dir 1 % 上 maze(2*r-1, 2*c) 0; elseif dir 2 % 下 maze(2*r1, 2*c) 0; elseif dir 3 % 左 maze(2*r, 2*c-1) 0; elseif dir 4 % 右 maze(2*r, 2*c1) 0; end maze(2*nr, 2*nc) 0; visited(nr, nc) true; stack(end1,:) [nr, nc]; else stack(end,:) []; % 回溯 end end end生成出來(lái)的迷宮是一個(gè)(2rows1)×(2cols1)的邏輯地圖值為1表示墻0表示可通行的走廊。也就是說(shuō)如果你想讓迷宮主體是85×85實(shí)際的地圖矩陣就是171×171。測(cè)試的時(shí)候我建議先從25×25的小迷宮開始跑通之后再逐步挑戰(zhàn)更大的尺寸。3.2 地圖表示、碰撞檢測(cè)與車輛模型簡(jiǎn)化路徑規(guī)劃代碼里地圖統(tǒng)一用二值矩陣表示車輛被簡(jiǎn)化為一個(gè)質(zhì)點(diǎn)實(shí)際車身寬度通過(guò)膨脹障礙物來(lái)處理。具體做法是在生成地圖之后對(duì)所有障礙格執(zhí)行一次形態(tài)學(xué)膨脹把墻邊界向外擴(kuò)展若干個(gè)柵格等效于車輛的“安全距離”。這樣規(guī)劃出來(lái)的路徑即使在真實(shí)車輛執(zhí)行時(shí)也不會(huì)擦墻。% 障礙物膨脹確保車輛安全距離 se strel(square, 7); % 7個(gè)柵格的膨脹核相當(dāng)于3格車身寬2格安全余量 expandedMap imdilate(maze, se);碰撞檢測(cè)邏輯也依賴膨脹后的地圖在判斷某個(gè)節(jié)點(diǎn)或某條路徑線段是否可行時(shí)只要線段經(jīng)過(guò)的任意柵格在地圖上的值為1就判定為碰撞。線段檢測(cè)用的是經(jīng)典的Bresenham算法遍歷線段經(jīng)過(guò)的所有柵格速度夠快實(shí)現(xiàn)也簡(jiǎn)單。提示膨脹核尺寸必須和你設(shè)定的小車半徑強(qiáng)相關(guān)。我在測(cè)試中發(fā)現(xiàn)如果膨脹尺寸過(guò)小生成的路徑雖然在地圖上是可行的但實(shí)際執(zhí)行時(shí)車輛很容易蹭到墻邊膨脹過(guò)大又會(huì)把原本兩格寬的通道完全堵死導(dǎo)致算法找不到任何路徑。具體尺寸需要針對(duì)你的車型反復(fù)試驗(yàn)。4. RRT路徑規(guī)劃核心模塊的Matlab實(shí)現(xiàn)詳解4.1 采樣、最近鄰與步進(jìn)增長(zhǎng)RRT的三板斧就是采樣、找最近鄰、步進(jìn)擴(kuò)展。先說(shuō)采樣模塊核心邏輯是目標(biāo)偏置每次生成隨機(jī)數(shù)如果小于偏置概率就直接返回目標(biāo)點(diǎn)否則在自由空間內(nèi)均勻采樣。自由空間的范圍直接取地圖矩陣的尺寸簡(jiǎn)單高效。function [samplePoint] samplePoint(map, goal, biasProb) if rand() biasProb samplePoint goal; return; end [rows, cols] size(map); while true p [randi(rows), randi(cols)]; if map(p(1), p(2)) 0 samplePoint p; return; end end end最近鄰查找的邏輯也很直接遍歷樹中所有節(jié)點(diǎn)計(jì)算它們到采樣點(diǎn)的歐氏距離取最小值對(duì)應(yīng)的那個(gè)節(jié)點(diǎn)。在Matlab的向量化加持下這一步很快。但隨著樹節(jié)點(diǎn)的增多遍歷開銷會(huì)線性增長(zhǎng)這就是后面要優(yōu)化的重點(diǎn)。步進(jìn)擴(kuò)展是RRT的靈魂。給定最近鄰節(jié)點(diǎn)和采樣點(diǎn)實(shí)際的步進(jìn)方向是從最近鄰節(jié)點(diǎn)指向采樣點(diǎn)的單位向量乘以步長(zhǎng)stepSize。注意這里有個(gè)關(guān)鍵細(xì)節(jié)不能直接把采樣點(diǎn)作為新節(jié)點(diǎn)加入樹中而是要走一小步。因?yàn)椴蓸狱c(diǎn)可能離得很遠(yuǎn)直接一步跨過(guò)去中間路徑很可能穿越障礙。步長(zhǎng)是RRT最重要的參數(shù)之一它直接決定了樹的“伸展能力”具體怎么調(diào)我會(huì)在下一節(jié)詳細(xì)講。function [newNode] extendTree(map, nearestNode, samplePoint, stepSize) dist norm(samplePoint - nearestNode); if dist stepSize newNode samplePoint; % 離得近就直接點(diǎn)對(duì)點(diǎn) else direction (samplePoint - nearestNode) / dist; newNode round(nearestNode direction * stepSize); end % 碰撞檢測(cè) if ~isCollisionFree(map, nearestNode, newNode) newNode []; end end4.2 碰撞檢測(cè)與到達(dá)目標(biāo)的判斷邏輯碰撞檢測(cè)用Bresenham算法遍歷兩個(gè)節(jié)點(diǎn)之間的所有柵格只要有一個(gè)柵格是墻就返回碰撞。這里有一個(gè)重要的工程細(xì)節(jié)檢測(cè)半徑不能是0。因?yàn)檐囕v模型是有寬度的即使兩個(gè)節(jié)點(diǎn)之間的連線在地圖上恰好穿過(guò)了空格但在狹窄通道里連線距離墻壁太近也會(huì)導(dǎo)致實(shí)際車輛過(guò)不去。所以我實(shí)際用的是“寬線檢測(cè)”把Bresenham遍歷的每個(gè)點(diǎn)都檢查一個(gè)3×3鄰域鄰域內(nèi)有墻即判為碰撞。function flag isCollisionFree(map, node1, node2) flag true; [xPoints, yPoints] bresenham(node1(1), node1(2), node2(1), node2(2)); for i 1:length(xPoints) x xPoints(i); y yPoints(i); % 檢查3x3鄰域 for dx -1:1 for dy -1:1 nx x dx; ny y dy; if nx 1 || ny 1 || nx size(map,1) || ny size(map,2) flag false; return; end if map(nx, ny) 1 flag false; return; end end end end end到達(dá)目標(biāo)的判斷條件我做了兩重冗余。最常用的方式是當(dāng)樹中新增的節(jié)點(diǎn)距離目標(biāo)點(diǎn)小于一個(gè)閾值比如stepSize或者再小一點(diǎn)時(shí)就直接把目標(biāo)點(diǎn)作為終結(jié)點(diǎn)加入樹然后回溯生成路徑。但如果迷宮通道特別詭異新增節(jié)點(diǎn)離目標(biāo)很近卻隔著墻這招就會(huì)失效。所以我額外加了一條規(guī)則新節(jié)點(diǎn)加入樹后立即檢測(cè)新節(jié)點(diǎn)到目標(biāo)點(diǎn)的連線是否無(wú)碰撞。如果無(wú)碰撞哪怕距離遠(yuǎn)一點(diǎn)也可以直接把目標(biāo)點(diǎn)連進(jìn)樹里立刻終止搜索。function [pathFound, path] checkGoal(map, tree, newNode, goal, goalThreshold) pathFound false; path []; if norm(newNode - goal) goalThreshold || isCollisionFree(map, newNode, goal) tree(end1) newNode; pathFound true; path [newNode, goal]; end end4.3 完整可運(yùn)行的RRT主程序把上面的模塊拼起來(lái)就是一個(gè)完整的RRT迷宮路徑規(guī)劃程序。下面是主循環(huán)的關(guān)鍵部分完整代碼我建議自己整理成一個(gè)函數(shù)function [path, tree] rrtPlan(map, start, goal, maxIter) % 參數(shù)配置 stepSize 8; goalThreshold 5; biasProb 0.15; % 初始化樹 tree start; parent -1; pathFound false; for i 1:maxIter sp samplePoint(map, goal, biasProb); [nearestIdx, nearestNode] findNearest(tree, sp); newNode extendTree(map, nearestNode, sp, stepSize); if isempty(newNode) continue; end tree(end1) newNode; parent(end1) nearestIdx; % 檢查是否到達(dá)目標(biāo) if norm(newNode - goal) goalThreshold pathFound true; tree(end1) goal; parent(end1) length(tree) - 1; break; end end % 從目標(biāo)回溯生成路徑 if pathFound path []; idx length(tree); while idx 0 path [tree(idx); path]; idx parent(idx); end else path []; warning(未能在最大迭代次數(shù)內(nèi)找到路徑); end end注意這段代碼里的一個(gè)細(xì)節(jié)我在每一步擴(kuò)展后沒(méi)有做“當(dāng)前新節(jié)點(diǎn)到目標(biāo)點(diǎn)連線”的二次檢測(cè)。如果你希望提高成功率可以把4.2節(jié)中的checkGoal函數(shù)加進(jìn)來(lái)在每次成功加入新節(jié)點(diǎn)后額外調(diào)用一次。兩者配合成功率會(huì)有可感知的提升。5. 從能跑到跑好迷宮路徑規(guī)劃調(diào)試踩坑記錄5.1 步長(zhǎng)與迭代次數(shù)的博弈步長(zhǎng)是最容易出問(wèn)題的參數(shù)。極端情況下步長(zhǎng)設(shè)得太小樹長(zhǎng)得極其密集一條長(zhǎng)廊從頭到尾可能要幾千上千個(gè)節(jié)點(diǎn)迭代次數(shù)很快耗盡步長(zhǎng)設(shè)得太大又會(huì)出現(xiàn)“一步跨過(guò)窄通道”的尷尬局面——明明通道就挨著但步長(zhǎng)太長(zhǎng)導(dǎo)致步進(jìn)終點(diǎn)落在墻的另一邊碰撞檢測(cè)直接否決。我的經(jīng)驗(yàn)是迷宮通道寬度為2到3個(gè)柵格時(shí)步長(zhǎng)建議取1.5到2倍通道寬度也就是4到6個(gè)柵格。如果迷宮更大、通道更寬可以按比例放大。另外最大迭代次數(shù)不要舍不得給隨機(jī)迷宮路徑往往繞很遠(yuǎn)我的測(cè)試配置是maxIter10000對(duì)于85×85的迷宮實(shí)際找到路徑通常只要2000到4000次迭代這個(gè)余量是給極端隨機(jī)情況保底的。5.2 狹窄通道下的采樣策略問(wèn)題迷宮場(chǎng)景里最常見的失敗模式是樹沿著通道走了一段但隨機(jī)采樣點(diǎn)總是落在墻外導(dǎo)致樹的擴(kuò)展方向長(zhǎng)期被“卡死”在某一條通道里無(wú)法轉(zhuǎn)入其他通道。這是純粹隨機(jī)采樣在面對(duì)狹窄通道時(shí)的一個(gè)天然弱點(diǎn)——因?yàn)橥ǖ赖膶挾仍谡麄€(gè)地圖面積里占的比例太小隨機(jī)點(diǎn)落到通道內(nèi)的概率本來(lái)就很低。一個(gè)有效的補(bǔ)救方案是雙向RRT從起點(diǎn)和終點(diǎn)各長(zhǎng)一棵樹兩棵樹交替擴(kuò)展互相把對(duì)方當(dāng)作目標(biāo)。這樣做的好處是兩棵樹可以同時(shí)探索不同區(qū)域一旦它們的距離小于某個(gè)閾值就認(rèn)為路徑連通了。我在單向RRT需要4000次迭代才能找到的迷宮里雙向RRT通常只需要1500次左右。代價(jià)是代碼復(fù)雜度翻倍但對(duì)迷宮場(chǎng)景來(lái)說(shuō)完全值得。5.3 目標(biāo)偏置參數(shù)的經(jīng)驗(yàn)取值區(qū)間目標(biāo)偏置是一把雙刃劍。偏置概率設(shè)得太小比如小于0.02樹基本失去了方向感純粹靠運(yùn)氣漫游偏置概率設(shè)得太大比如大于0.3樹的擴(kuò)展會(huì)被強(qiáng)烈吸引向目標(biāo)方向反而容易在遇到墻體阻礙時(shí)反復(fù)試探同一條死路陷入局部振蕩。調(diào)試中發(fā)現(xiàn)偏置概率在0.08到0.15之間比較穩(wěn)。在這個(gè)區(qū)間內(nèi)樹的擴(kuò)展既有方向性又保留了全局探索的能力。另外還有一個(gè)心法偏置概率可以隨迭代次數(shù)動(dòng)態(tài)變化——迭代初期用小偏置讓樹先充分鋪開探索迭代中期加大偏置集中力量向目標(biāo)推進(jìn)。這算是一個(gè)性價(jià)比很高的“小魔改”。5.4 隨機(jī)數(shù)種子的真隨機(jī)陷阱RRT的隨機(jī)性會(huì)帶來(lái)一個(gè)很扎心的現(xiàn)象同一份代碼這次運(yùn)行能找到路徑下次運(yùn)行可能就找不到。很多人在調(diào)試時(shí)會(huì)把失敗歸咎于算法不行其實(shí)只是隨機(jī)采樣的運(yùn)氣問(wèn)題。解決思路很簡(jiǎn)單在程序啟動(dòng)時(shí)固定隨機(jī)數(shù)種子rng(42)保證調(diào)試過(guò)程可復(fù)現(xiàn)等算法穩(wěn)定之后再隨機(jī)化種子去做統(tǒng)計(jì)測(cè)試。我實(shí)際統(tǒng)計(jì)過(guò)一組參數(shù)在某迷宮上的表現(xiàn)固定種子下100次運(yùn)行成功95次不固定種子則成功率會(huì)在88%到97%之間波動(dòng)。這個(gè)數(shù)字說(shuō)明兩點(diǎn)第一參數(shù)沒(méi)問(wèn)題成功率已經(jīng)足夠高第二任何RRT的單次運(yùn)行結(jié)果都不能代表算法性能要做多次實(shí)驗(yàn)取平均值才有說(shuō)服力。6. RRT的進(jìn)階優(yōu)化方向與工程化建議6.1 RRT*的代價(jià)函數(shù)與漸進(jìn)最優(yōu)特性基礎(chǔ)RRT找到的路徑通常彎彎曲曲完全談不上“最優(yōu)”因?yàn)樗槐ＷC能通不保證路徑短。RRT*的改進(jìn)在于引入了重新布線機(jī)制每次新增節(jié)點(diǎn)后候選父節(jié)點(diǎn)不再局限于“最近鄰”而是在附近一定半徑內(nèi)尋找所有潛在父節(jié)點(diǎn)選擇使該節(jié)點(diǎn)代價(jià)最小的那個(gè)同時(shí)對(duì)新節(jié)點(diǎn)附近的其他節(jié)點(diǎn)進(jìn)行重連看能否通過(guò)新節(jié)點(diǎn)縮短它們到起點(diǎn)的代價(jià)。代價(jià)被我定義為路徑長(zhǎng)度累計(jì)步長(zhǎng)之和。RRT會(huì)隨著迭代次數(shù)的增加不斷優(yōu)化樹中各節(jié)點(diǎn)的代價(jià)因此路徑質(zhì)量會(huì)逐漸提升。代價(jià)是計(jì)算量顯著提高而且**如果迭代次數(shù)不夠RRT甚至可能比基礎(chǔ)RRT更慢**——因?yàn)槎嘧隽撕芏酂o(wú)效的重連計(jì)算。迷宮場(chǎng)景下我的建議是先用基礎(chǔ)RRT快速找出一條路徑然后再用RRT去優(yōu)化它而不是一上來(lái)就上RRT。6.2 雙向樹、動(dòng)態(tài)步長(zhǎng)與平滑后處理雙向RRT帶來(lái)的成功率提升前面已經(jīng)說(shuō)了這里補(bǔ)充一個(gè)動(dòng)態(tài)步長(zhǎng)的實(shí)現(xiàn)思路在靠近目標(biāo)時(shí)自動(dòng)將步長(zhǎng)縮小一半遠(yuǎn)離目標(biāo)時(shí)恢復(fù)默認(rèn)步長(zhǎng)。這個(gè)策略能有效減少到達(dá)目標(biāo)后的過(guò)沖問(wèn)題。路徑平滑也值得做。RRT產(chǎn)生的路徑是多段折線在機(jī)器人上執(zhí)行會(huì)讓電機(jī)頻繁啟停。最實(shí)用的后處理方法是路徑剪枝從起點(diǎn)開始嘗試直接連接更遠(yuǎn)的節(jié)點(diǎn)如果這段連線無(wú)碰撞就刪掉中間的所有節(jié)點(diǎn)。反復(fù)剪枝幾輪路徑通常會(huì)變得非常利落。如果需要曲率連續(xù)還可以用三次樣條擬合但要注意擬合后的路徑不能偏移到障礙物里必須再做一次碰撞校驗(yàn)。6.3 從仿真到實(shí)車的注意事項(xiàng)仿真代碼移植到實(shí)車時(shí)有幾個(gè)坑要提前規(guī)避。第一仿真中車輛是理想點(diǎn)模型實(shí)車則要重新計(jì)算質(zhì)心位置和轉(zhuǎn)彎半徑建議規(guī)劃層的安全距離至少再加大20%第二仿真地圖是靜態(tài)的實(shí)車環(huán)境里可能有動(dòng)態(tài)障礙RRT的隨機(jī)采樣特性可以較快適應(yīng)局部地圖變化但如果障礙物運(yùn)動(dòng)太快規(guī)劃層只能負(fù)責(zé)生成參考路徑避障還得靠局部控制器第三路徑重規(guī)劃頻率不要太高否則車輛會(huì)頻繁急轉(zhuǎn)彎反而增加執(zhí)行風(fēng)險(xiǎn)。我在某項(xiàng)目里的做法是規(guī)劃層以0.5到1秒的周期調(diào)用RRT進(jìn)行全局重規(guī)劃執(zhí)行層用PID跟蹤路徑。測(cè)試下來(lái)即使局部障礙突變車輛也能在1到2秒內(nèi)修正軌跡整體表現(xiàn)比用A*時(shí)平滑不少。6.4 新方向的探索心得最近我在嘗試把RRT與深度強(qiáng)化學(xué)習(xí)結(jié)合訓(xùn)練一個(gè)小型策略網(wǎng)絡(luò)學(xué)習(xí)“哪些區(qū)域更容易產(chǎn)生優(yōu)質(zhì)采樣點(diǎn)”再用這個(gè)策略網(wǎng)絡(luò)替代均勻隨機(jī)采樣。本質(zhì)上是在保持RRT概率完備性的同時(shí)把“探索”變得更聰明。目前效果還在驗(yàn)證中但在有些迷宮上確實(shí)能把找到路徑的平均迭代次數(shù)再降30%到40%。如果你有基礎(chǔ)這個(gè)方向值得跟一跟。寫在最后從最初在迷宮里頻繁“翻車”到最終能穩(wěn)定跑通各類隨機(jī)迷宮RRT給我的最大感受是它不講究精巧講究的是概率上的“磨”。在極其復(fù)雜的環(huán)境里探索本身往往比聰明更重要。如果你也正在被迷宮路徑規(guī)劃的問(wèn)題困擾建議先把基礎(chǔ)RRT跑通再去折騰優(yōu)化版本——先讓路徑出現(xiàn)再讓路徑變得更好。這八個(gè)字大概是我在這個(gè)項(xiàng)目里最想分享的一句話。