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