接手一個移動機(jī)器人的導(dǎo)航項目時我最早花了很多時間只調(diào) DWA認(rèn)為局部避障做好了車就能跑起來。后來發(fā)現(xiàn)在幾十米范圍的倉庫場景里光靠 DWA 根本走不到目標(biāo)點——它只能看到周圍一米左右的環(huán)境遇到長的障礙物帶就繞不過去常常原地左右搖擺。搭上 A* 之后全局路徑有了跑起來順了很多再后來又遇到巷道邊緣堆著臨時托盤、行人亂走的狀況A* 的柵格路徑在動態(tài)變化的環(huán)境里不夠穩(wěn)于是把 RRT 也拉進(jìn)來做全局備選。從“只會一個算法”到“三個算法分工配合”這個過程踩了不少坑也把每種算法的脾氣摸清了。這篇內(nèi)容算是自己在這個項目里的完整梳理適合正在做移動底盤導(dǎo)航、或者剛?cè)腴T路徑規(guī)劃的開發(fā)者參考。我手里的測試平臺是一臺差速底盤的輪式機(jī)器人配單線激光雷達(dá)建圖用的是常規(guī)柵格地圖地圖分辨率 0.05m整車尺寸 0.6m×0.7m最大速度 1.2m/s最大角速度 1.5rad/s。場景是一個 40m×50m 的模擬倉儲庫房有三條 1.8m 寬的巷道還有兩處直角彎和一處狹窄門洞。后面講到的參數(shù)、代碼、數(shù)據(jù)都是從這套環(huán)境里來的你可以按自己的平臺適當(dāng)縮放。1. 路徑規(guī)劃系統(tǒng)里三個算法到底各自扛什么活1.1 項目背景與“單算法”的隱患先說結(jié)論A*、RRT、DWA 這三個東西不是同一個層級上的同類算法硬拿來對比誰好誰壞沒有意義。它們解決的是不同尺度、不同約束下的問題。DWA 是局部規(guī)劃器負(fù)責(zé)在當(dāng)前時刻輸出車的線速度和角速度A* 是全局規(guī)劃器負(fù)責(zé)在已知靜態(tài)地圖里算出一條從起點到終點的大致路徑RRT 也是全局規(guī)劃器但它更適合高維空間、非結(jié)構(gòu)化環(huán)境和快速避讓動態(tài)變化區(qū)域。最初我接到這個任務(wù)時只用 DWA 做導(dǎo)航。DWA 本質(zhì)上是一個速度空間采樣算法它從當(dāng)前速度出發(fā)枚舉一小段時間內(nèi)所有可能的“線速度角速度”組合用運動模型推演出未來一段軌跡再根據(jù)評分選擇最優(yōu)的一組速度去執(zhí)行。聽起來很合理實際一跑就發(fā)現(xiàn)問題DWA 的規(guī)劃視野取決于速度采樣時的模擬時間predict_time我一開始設(shè)成 1.5s最大速度 1.2m/s也就是它最多能看到未來 1.8m 左右的軌跡。而倉庫里一堵墻可能有 8m 長車到了墻面前DWA 檢測到前方障礙橫向的候選速度又被減掉只能反復(fù)切換“左轉(zhuǎn)/右轉(zhuǎn)”在兩個方向上搖擺最后停在原地報“無可行軌跡”。這不是參數(shù)沒調(diào)好而是原理決定的局部規(guī)劃器只看得到局部永遠(yuǎn)做不出“從墻的盡頭繞過去”這種全局決策。要讓車在真實場景里從 A 點移動到 B 點必須有一個更高層的規(guī)劃器先畫出一條完整路線DWA 再沿著這條路線做局部避障。這個認(rèn)知花了兩個禮拜才徹底想明白也解釋了為什么業(yè)內(nèi)大多數(shù)導(dǎo)航框架都是“全局規(guī)劃 局部規(guī)劃”分層架構(gòu)。A* 在這個架構(gòu)里承擔(dān)全局選路的任務(wù)。它工作在柵格地圖上把地圖看成網(wǎng)格用廣度優(yōu)先搜索的思路從起點開始擴(kuò)展節(jié)點但通過一個啟發(fā)函數(shù)指導(dǎo)擴(kuò)展方向。它保證在給定柵格地圖上找到最短路徑如果啟發(fā)函數(shù)滿足一致性要求這也是它在工程中受歡迎的原因之一——路徑質(zhì)量穩(wěn)定、可重復(fù)、可解釋。RRT 則是我在后期處理復(fù)雜場景時才加進(jìn)來的。它不需要把地圖網(wǎng)格化而是直接在連續(xù)空間里隨機(jī)撒點然后從起點出發(fā)不斷把新采樣點連接到已有隨機(jī)樹上。它的實現(xiàn)簡單且非常適合高維狀態(tài)空間在地圖分區(qū)不規(guī)則、包含動態(tài)障礙物或需要快速重新規(guī)劃的場合RRT 比 A* 更快找到一條可行通道但路徑長度通常不是最優(yōu)的。它不是用來替代 A*而是作為全局規(guī)劃層的另一條候選路徑來源在某些場景下比 A* 更可靠。1.2 全局規(guī)劃與局部規(guī)劃的職責(zé)邊界工程實現(xiàn)上我會把整個導(dǎo)航邏輯拆成三層任務(wù)層接收目標(biāo)點維護(hù)當(dāng)前狀態(tài)機(jī)如待命、前往目標(biāo)、任務(wù)完成全局規(guī)劃層負(fù)責(zé)在柵格地圖上計算全局路徑每隔一段時間或檢測到地圖變化時重新規(guī)劃局部規(guī)劃層DWA以全局路徑作為參考輸出實時控制指令。全局規(guī)劃和局部規(guī)劃之間靠一個關(guān)鍵機(jī)制對接全局路徑上的“局部目標(biāo)點”。DWA 不是機(jī)械地追全局路徑上的每一個點而是從當(dāng)前機(jī)器人位置出發(fā)沿全局路徑找一個距離機(jī)器人一定距離比如 2~3m的前瞻點把這個點作為當(dāng)前局部目標(biāo)。這樣做的原因是全局路徑通常很密集A* 在 0.05m 分辨率地圖上算出的一小段路徑可能包含幾十上百個點如果 DWA 每個周期都試圖精確逼近當(dāng)前最近點會出現(xiàn)兩個問題一是路徑點之間的小角度變化會讓 DWA 頻繁加減速車走起來一頓一頓的二是如果全局路徑在某個轉(zhuǎn)角處有輕微鋸齒DWA 可能在轉(zhuǎn)角處反復(fù)對點導(dǎo)致輕微擺動。用前瞻點之后DWA 只關(guān)心“怎么在避開障礙的前提下到達(dá)前方那個點”全局路徑的鋸齒被天然過濾掉了。這個設(shè)計也符合人的開車間距意識——不是每一步都盯著眼前的車道線而是看著前方一個合適距離的參考點。這個分層結(jié)構(gòu)還有一個好處如果全局規(guī)劃算法要替換或者新增比如我要用 RRT 做備選路徑只需要改全局規(guī)劃層DWA 完全不需要動。我最初給全局規(guī)劃層設(shè)計了一個統(tǒng)一接口輸入是當(dāng)前坐標(biāo)、目標(biāo)坐標(biāo)、地圖輸出是PathPoint[]后續(xù)不管是 A* 還是 RRT 都實現(xiàn)同一個接口切換成本很低。這個項目經(jīng)驗讓我養(yǎng)成了一個習(xí)慣做算法集成時先定義好數(shù)據(jù)接口再往上堆算法否則后期每加一個算法都要重寫局部規(guī)劃器工程量會成倍增加。2. 動態(tài)窗口法 DWA本地執(zhí)行層的安全底線2.1 DWA 是怎么在速度空間里“找窗口”的DWA 全稱 Dynamic Window Approach中文常譯作“動態(tài)窗口法”核心思想非常直白每一時刻機(jī)器人能執(zhí)行的速度并不是一個無限集合而是受到自身加速度和最大速度限制的“動態(tài)窗口”約束。在這個窗口內(nèi)采一組離散速度模擬未來一段時間的軌跡然后按評分挑選最優(yōu)速度執(zhí)行。整個過程在實時控制周期內(nèi)完成通常一輪計算耗時在 10ms 級完全滿足控制頻率要求。具體來說速度空間由三個約束裁剪機(jī)器人自身速度極限線速度不能超過v_max角速度不能超過w_max電機(jī)加減速能力當(dāng)前速度v_cur在時間間隔dt內(nèi)最多能變化v_acc * dt角速度同理這形成了速度值的動態(tài)下界和上界安全制動距離以當(dāng)前候選速度模擬剎車如果在碰到障礙之前能停下來該速度才被允許否則速度組被剔除。經(jīng)過這三層約束后剩下的合法速度組就叫“動態(tài)窗口”。接下來在這個窗口里按照一定分辨率通常線速度 0.01m/s、角速度 0.05rad/s 左右均勻采樣。然后對每一組候選速度推演軌跡。對于一個差速底盤可以用下面的運動模型近似推演x_{k1} x_k v * cos(theta_k) * dt y_{k1} y_k v * sin(theta_k) * dt theta_{k1} theta_k w * dt這里v是線速度w是角速度theta是航向角。把這一步重復(fù)n次n predict_time / dt就得到一條未來軌跡。然后把軌跡上的每個點與障礙物地圖做碰撞檢測對可通行軌跡計算評分。評分函數(shù)是 DWA 最有意思的部分基本上每個工程落地時都要針對場景調(diào)整權(quán)重。最經(jīng)典的評分公式是score alpha * heading beta * clearance gamma * velocityheading軌跡末端方向與目標(biāo)方向的角度差越小分越高clearance軌跡離最近障礙物的距離距離越大分越高velocity鼓勵保持較大線速度減少到達(dá)時間。但實際工程里我很少直接套用這三個權(quán)重。比如在一個窄巷道環(huán)境里如果beta障礙距離權(quán)重設(shè)置相對過大機(jī)器人會走到巷道正中間碰到只有一側(cè)有障礙物的區(qū)域時軌跡會偏向另一側(cè)空地上導(dǎo)致大幅繞行走出來的路徑嚴(yán)重偏離全局路徑。所以我更傾向于在評分前用全局路徑做“目標(biāo)點”約束用前面提到的前瞻點替代heading的單一目標(biāo)方向。如果追求更平滑的路徑還可以在評分項里加上“與上一時刻速度變化量”的懲罰項減少急加速急減速。這個項類似 PID 里的微分項score -lambda * (abs(v_candidate - v_current) abs(w_candidate - w_current))加了這項之后機(jī)器人實際控制指令更加平滑底盤電機(jī)的電流波動也小很多。代價是到達(dá)時間會長一點但這個代價在倉儲環(huán)境下完全值得。2.2 自己實現(xiàn) DWA 時的參數(shù)調(diào)參經(jīng)驗DWA 參數(shù)算不上多但每一項都很敏感。我自己在模擬倉庫環(huán)境里調(diào)出來的參數(shù)可以供參考參數(shù)數(shù)值作用說明最大線速度1.2 m/s由底盤硬件決定最大角速度1.5 rad/s受限于轉(zhuǎn)向能力線加速度0.8 m/s2電機(jī)加減速能力角加速度1.2 rad/s2電機(jī)加減速能力predict_time2.0 s規(guī)劃視野時長模擬時間步長0.1 s軌跡推演步長線速度采樣間隔0.02 m/s候選密度角速度采樣間隔0.06 rad/s候選密度前瞻點距離2.5 m全局路徑截取距離predict_time 的選擇很關(guān)鍵。設(shè)置太小機(jī)器人視野短速度剛起來就發(fā)現(xiàn)前方障礙頻繁急剎設(shè)置太大軌跡推演的點數(shù)增多計算量變大而且遠(yuǎn)離機(jī)器人的預(yù)測軌跡可信度低地圖誤差和動力學(xué)誤差都會累積。我的經(jīng)驗是predict_time ≈ 2 * 前瞻距離 / 最大速度是一個不錯的出發(fā)點比如前瞻 2.5m、最大速度 1.2m/s算出來約 2.1s取 2.0s 正好。v_max不能想當(dāng)然地設(shè)成底盤標(biāo)稱最大速度。在一個 1.8m 寬的巷道里如果最大速度 1.2m/s直角轉(zhuǎn)彎處 DWA 為了讓軌跡安全通過會頻繁選擇低速轉(zhuǎn)向?qū)嶋H平均速度可能只有 0.4m/s。這種情況下適當(dāng)降速反而提升通過效率我后來在直角彎區(qū)域把限速降到了 0.6m/s整體任務(wù)時間反而縮短了。還有一點DWA 給出的線速度角速度指令需要低通濾波后再發(fā)給底盤否則底盤執(zhí)行器響應(yīng)很快會出現(xiàn)輕微的抖動。我在指令側(cè)加了一個一階低通濾波時間常數(shù)約 0.2s實測軌跡跟蹤平滑性提升很明顯。但要注意濾波時間常數(shù)不能太大否則會導(dǎo)致真實速度反饋滯后DWA 的碰撞檢測防線會失效這在動態(tài)障礙多的場景里很危險。3. A* 與 RRT 的選型和組合細(xì)節(jié)3.1 A* 在柵格地圖上的實現(xiàn)與性能優(yōu)化A* 在柵格地圖上的實現(xiàn)本質(zhì)上是在節(jié)點圖上做帶啟發(fā)式的最短路徑搜索。我采用的柵格地圖是 0.05m 分辨率40m×50m 的場景就是 800×1000 個柵格總共 80 萬個節(jié)點。暴力搜索全部節(jié)點不現(xiàn)實所以 A* 的性能優(yōu)化非常重要。A* 的核心公式是f(n) g(n) h(n)其中g(shù)(n)是起點走到當(dāng)前節(jié)點n的實際代價h(n)是從當(dāng)前節(jié)點到目標(biāo)點的啟發(fā)估計。啟發(fā)函數(shù)的選擇直接影響搜索速度和路徑質(zhì)量。我前前后后試過三種啟發(fā)歐幾里得距離sqrt((dx)^2 (dy)^2)計算代價略高路徑方向性弱一些曼哈頓距離|dx| |dy|要求只能橫豎移動在允許斜向移動的情況下會低估實際距離搜索范圍偏大八方向距離切比雪夫距離max(|dx|, |dy|)適合允許八鄰域移動的柵格圖我最終選了這個搜索節(jié)點數(shù)量比曼哈頓少約 30%路徑也更自然。A* 的性能瓶頸通常不在啟發(fā)函數(shù)而在開放列表的維護(hù)。我用的是二叉堆priority queue每次從開放列表取出f值最小的節(jié)點這一步是 O(logN)。還有一個關(guān)鍵優(yōu)化提前判斷節(jié)點能否進(jìn)入開放列表。常規(guī)做法是把當(dāng)前節(jié)點能到達(dá)的鄰居都先塞進(jìn)去再由堆排序去篩選。但柵格地圖里很多鄰居會重復(fù)進(jìn)入浪費大量時間和內(nèi)存。我的做法是對每個節(jié)點維護(hù)一個best_g值只有當(dāng)新的g比之前的best_g更小時才允許入堆否則直接丟棄。這個“懶刪除”思路能大幅減少無用節(jié)點數(shù)量。實測下來在 80 萬柵格的地圖里從頭到尾規(guī)劃一條橫穿全場的路徑平均耗時從 280ms 降到了 90ms 左右。還有工程上很實用的兩個小技巧。第一柵格地圖在做碰撞檢測時一定要先把機(jī)器人半徑考慮進(jìn)去對地圖做膨脹處理。機(jī)器人半徑 0.35m加上 0.1m 的安全余量就是 0.45m膨脹半徑按 9 個柵格0.45 / 0.05處理。不做膨脹的后果是 A* 算出來的路徑貼著墻走DWA 執(zhí)行時發(fā)現(xiàn)障礙物距離不足頻繁給“剎車”指令根本跑不起來。第二A* 的路徑點密度極高在 0.05m 分辨率下1m 路徑就有 20 個點。直接傳給 DWA 會讓局部規(guī)劃器對每個點都做一次目標(biāo)點匹配開銷很大而且容易發(fā)生抖動。我在 A* 輸出端做了抽稀處理保留路徑方向變化大于 15° 的點和關(guān)鍵轉(zhuǎn)折點這樣一條 30m 的全局路徑從 600 個點壓到大概 80 個點DWA 的匹配壓力小了很多。3.2 RRT 面向復(fù)雜巷道和變化環(huán)境的用法RRT 的全稱是 Rapidly-exploring Random Tree中文常叫快速擴(kuò)展隨機(jī)樹。它的原理比 A* 原始得多從起點開始建立一棵樹每次在地圖的可通行區(qū)域內(nèi)隨機(jī)采樣一個點然后從樹上找距離這個采樣點最近的節(jié)點沿著兩點連線方向擴(kuò)展一段固定距離步長如果這段路徑?jīng)]有碰到障礙就把新節(jié)點加入樹中。不斷重復(fù)這個過程直到某個節(jié)點靠近目標(biāo)點一定距離時回溯出一條從起點到目標(biāo)的路徑。我最早覺得 RRT 就是個“碰運氣”的算法路徑不可預(yù)測而且不夠最優(yōu)。直到遇到這種場景倉庫里有幾排貨架貨架之間形成窄巷道如果其中一個巷道被臨時停放的托盤堵住了A* 在靜態(tài)柵格地圖上仍然會規(guī)劃穿過這個巷道的路徑因為地圖上根本沒有托盤。而使用 RRT 時如果我們在每次傳感器發(fā)現(xiàn)新障礙后重新采樣RRT 的隨機(jī)性反而變成了優(yōu)勢——它會嘗試各種其他通道哪怕它們不在原始 A* 路徑上。RRT 的步長參數(shù)非常關(guān)鍵。步長越大每次擴(kuò)展距離越遠(yuǎn)收斂越快但路徑越粗糙甚至在窄通道采樣時新節(jié)點會直接跨過走廊、嵌入對側(cè)障礙物里。步長越小路徑越精細(xì)但節(jié)點數(shù)量爆炸收斂時間變長。在 1.8m 寬的巷道環(huán)境里我用的步長是 0.3m這個值大約是巷道寬度的 1/6既能保證通過窄通道時不會“跨欄”又能控制擴(kuò)展次數(shù)。另一個參數(shù)是提前停止的判斷當(dāng)樹中某節(jié)點與目標(biāo)點的距離小于goal_tolerance時就認(rèn)為路徑已找到。我設(shè)的goal_tolerance是 0.3m這防止了算法為了精確到達(dá)目標(biāo)點而無限擴(kuò)展。如果地圖里目標(biāo)點周圍有障礙物還要檢查最后一段路徑是否真的無碰撞。RRT 生成路徑后一般要做平滑。因為這個算法生成的路徑由大量短線段組成轉(zhuǎn)折角可能很大直接交給 DWA 跟蹤會出現(xiàn)頻繁減速。我用的平滑方式是對 RRT 路徑做“剪枝”凡是兩點之間的連線不碰障礙就跳過中間節(jié)點將這兩點直接相連。重復(fù)這個過程直到無法再縮短然后再做一次三次樣條平滑。這樣處理之后RRT 路徑的可跟蹤性跟 A* 路徑差別不大了。3.3 兩者怎么選怎么互補(bǔ)A* 和 RRT 同屬全局規(guī)劃但在架構(gòu)里承擔(dān)的角色其實可以共存。我采用的方式是雙全局路徑源A* 是默認(rèn)主路徑因為它在靜態(tài)地圖上給出的是確定性的最短路徑便于預(yù)測任務(wù)時間、避開已知大障礙。RRT 作為備選路徑源在以下場合啟用全局路徑的終點附近被新障礙物阻斷A* 重新規(guī)劃后仍然走不通地圖部分區(qū)域因為動態(tài)障礙出現(xiàn)明顯變化需要快速生成一條替代路線目標(biāo)點在人無法進(jìn)入的狹窄空間A* 因網(wǎng)格離散化可能找不到路徑RRT 的連續(xù)采樣更擅長這種場景。在實際系統(tǒng)中我保留兩條全局路徑由上層邏輯根據(jù)代價選擇。代價函數(shù)包括路徑長度、預(yù)估通行耗時、以及路徑與最近障礙物的距離。RRT 路徑通常在長度上吃虧但在“繞開變化區(qū)域”這一點上更有價值。我把 A* 看成“穩(wěn)妥的保守派”把 RRT 看成“靈活的冒險派”兩者并存時絕大多數(shù)場景都能兜住。用表格簡單總結(jié)一下它們的對比對比維度A*RRT工作空間柵格離散圖連續(xù)或任意空間最優(yōu)性柵格分辨率下最優(yōu)不保證最優(yōu)計算復(fù)雜度與柵格數(shù)量相關(guān)與迭代次數(shù)相關(guān)窄通道表現(xiàn)穩(wěn)定但耗時長可能快速通過動態(tài)變化適應(yīng)性需重新規(guī)劃天然適合快速重規(guī)劃路徑平滑度較好仍有鋸齒差需要后處理從實現(xiàn)角度看A* 是先算后走RRT 是邊走邊算快速重規(guī)劃。兩者的本質(zhì)差異在于A* 依賴一張準(zhǔn)確的全局代價圖RRT 更依賴實時采樣和碰撞檢測器的速度。這在它們與 DWA 融合時會體現(xiàn)出截然不同的工程調(diào)性。4. 三算法融合落地全局規(guī)劃 局部避障的工程方案4.1 融合框架從全局路徑到局部目標(biāo)點的傳遞現(xiàn)在說最核心的部分——怎么把三個算法安全地組合在一個系統(tǒng)里跑。一個最常見的錯誤是“主程序里先調(diào) A*再在每一步循環(huán)里調(diào) DWA完全不管中間狀態(tài)”這樣兩個算法各做各的A* 的路徑只是一根“看得見摸不著”的線DWA 走到一半發(fā)現(xiàn)路徑被動態(tài)障礙擋住也不會重新請求 A* 規(guī)劃結(jié)果就是機(jī)器人停在障礙前發(fā)呆。我的做法是引入一個“全局重規(guī)劃條件”和“局部目標(biāo)點管理模塊”。整體控制循環(huán)可以抽象成這樣的偽代碼while task_active: if need_global_replan(): global_path choose_global_path(astar_path, rrt_path) path_smooth smooth_global_path(global_path) lookahead_point get_lookahead_point(robot_pose, path_smooth, lookahead_dist) v_cmd, w_cmd dwa_planner(robot_pose, lookahead_point, local_costmap) send_velocity(v_cmd, w_cmd)choose_global_path(astar_path, rrt_path)就是融合決策模塊。我給它設(shè)定的規(guī)則很簡單優(yōu)先采用 A* 路徑如果 A* 路徑被動態(tài)障礙阻斷再重新規(guī)劃一次若重新規(guī)劃后仍然阻斷調(diào)用 RRT 生成新路徑并在下一次控制周期里評估兩條路徑的代價選擇可行且總代價較低的一條如果 RRT 找不到可行路徑則上報“全局路徑失敗”請求操作員介入。get_lookahead_point是連接全局規(guī)劃和 DWA 的紐帶。這個函數(shù)接收機(jī)器人當(dāng)前位置和完整全局路徑返回一個距離當(dāng)前位置指定距離的點。這里的距離不是歐幾里得距離而是沿路徑的累計弧長。因為如果直接用歐幾里得距離選點在經(jīng)過 U 形彎時會選到彎道對側(cè)的路徑點導(dǎo)致 DWA 目標(biāo)點穿越障礙區(qū)。我用的是沿路徑點逐個累計距離找到第一個累計弧長大于lookahead_dist的點。DWA 拿到這個局部目標(biāo)點后它的作用就變得非常單純在局部代價地圖里車載激光雷達(dá)實時掃描數(shù)據(jù)與全局地圖疊加算出避障軌跡并把車導(dǎo)向這個前瞻點。只要前瞻點選擇合理DWA 的避障就和全局方向天然一致不會出現(xiàn)在錯誤的障礙側(cè)繞行。工程上還有一個細(xì)節(jié)很值得寫DWA 的代價地圖與 A* 的全局地圖的坐標(biāo)系必須完全同步。設(shè)備啟動時我們做了一次坐標(biāo)標(biāo)定把激光雷達(dá)坐標(biāo)系、底盤坐標(biāo)系、里程計坐標(biāo)系全部轉(zhuǎn)換到地圖坐標(biāo)系。之后每次全局重規(guī)劃或 DWA 更新局部代價地圖都從同一個坐標(biāo)基準(zhǔn)讀取這省掉了很多在融合階段極其隱蔽的“坐標(biāo)系錯位”bug。早期項目里DWA 局部地圖漂移了 0.2m車在窄巷道里老是貼著墻跑排查了兩天才發(fā)現(xiàn)是坐標(biāo)變換多乘了一次車輛的偏航角。4.2 路徑平滑為目的的可操作步驟全局路徑直接給 DWA 用還是不夠。A* 的路徑有鋸齒RRT 的路徑更是瘸腿的折線。直接跟蹤這種路徑DWA 為了貼合每次的轉(zhuǎn)向點會頻繁減速嚴(yán)重時可能把平均速度從 1.0m/s 拉到 0.35m/s。所以我的融合層里有一個獨立的“路徑平滑”步驟。當(dāng)初我嘗試過很多平滑算法簡單有效的方案是“梯度下降 三次樣條”的組合。大致步驟這樣對全局路徑點做抽稀把路徑點數(shù)量減少到原來的 1/5 左右對抽稀路徑做碰撞檢測如果某段抽稀后的連線穿過障礙物保留中途的關(guān)鍵節(jié)點對所有路徑點做帶障礙物約束的平滑迭代對平滑結(jié)果做三次樣條插值得到稠密的、曲率連續(xù)的路徑。平滑迭代的核心代碼如下它其實就是一個帶約束的梯度下降# 參數(shù) iters 300 alpha 0.3 obs_cost 0.15 for it in range(iters): for i in range(1, len(path) - 1): prev_p path[i - 1] next_p path[i 1] cur_p path[i] # 向兩點中點收斂 mid (prev_p next_p) / 2 grad (mid - cur_p) * alpha # 避開障礙物 if is_obstacle(cur_p): grad (cur_p - nearest_obstacle(cur_p)) * obs_cost path[i] grad這里有一個重要細(xì)節(jié)平滑迭代前必須先把路徑抽稀否則連續(xù)相鄰的路徑點會因為彼此距離太近導(dǎo)致步長和障礙代價相互干擾平滑結(jié)果反而變得扭曲。我抽稀后相鄰點之間的最小距離保持在 0.2m 左右迭代收斂很穩(wěn)定。迭代次數(shù)不是越多越好。400 次以后路徑基本定型再多迭代也不會明顯改善反而可能因為反復(fù)拉扯而出現(xiàn)輕微過沖導(dǎo)致路徑壓到障礙物邊緣。所以在迭代過程中我會持續(xù)檢查平滑后路徑與障礙物的最近距離一旦小于安全閾值就停止迭代并回退到上一版路徑——這個保護(hù)機(jī)制在實際調(diào)試中救過我很多次。平滑完成后再用三次樣條插值把路徑均勻加密到每 0.1m 一個點。DWA 拿到的是這樣一條“順滑的、連續(xù)的、可跟蹤的”路徑。從 A*/RRT 粗糙輸出到 DWA 可用輸入質(zhì)量提升非常明顯實測平均跟蹤誤差從 0.18m 降到 0.06m。4.3 融合實測效果與關(guān)鍵對比數(shù)據(jù)我在模擬倉儲環(huán)境里做了一組對照實驗用同一臺底盤、同一張地圖、同一組任務(wù)點分別跑了以下幾種配置配置平均速度任務(wù)完成時間碰到障礙次數(shù)全局重規(guī)劃次數(shù)僅 DWA0.28 m/s未完成多次0A* DWA0.72 m/s128s08次RRT DWA0.65 m/s142s017次A* RRT DWA0.78 m/s115s05次數(shù)據(jù)說明幾個問題。僅 DWA 跑不完全程因為它在長墻前反復(fù)搖擺。A* DWA 配置下平均速度已經(jīng)可以接受但重規(guī)劃次數(shù)偏多——原因是 A* 規(guī)劃出的最短路徑要經(jīng)過一個很窄的門洞而門洞附近經(jīng)常有臨時堆放的箱子DWA 判斷無法通過于是頻繁觸發(fā)全局重規(guī)劃。這個場景正是 RRT 發(fā)揮作用的地方加進(jìn)來之后遇到障礙物時 RRT 快速生成另一條繞行路徑全局重規(guī)劃次數(shù)反而下降到 5 次而且被選中路徑并不總是 A* 的路徑——有時候是通過另一個巷道多繞了 6m 但是更穩(wěn)妥的那條。融合配置的任務(wù)時間從 128s 降到 115s主要收益來自減少“無效重規(guī)劃 原地等待”。A* 每次重規(guī)劃耗時大約 90msRRT 每次約 40ms等待本身并不算長但每次重規(guī)劃后 DWA 都要重新確定前瞻點、重新加速這個“重新建立運動狀態(tài)”的過程才是時間損耗的大頭。從這個角度看全局規(guī)劃層的冗余反而提升了系統(tǒng)整體效率。還有一個值得注意的指標(biāo)碰撞次數(shù)。三種配置下都是 0 次這說明 DWA 在面對未知障礙物時確實可靠。DWA 的職責(zé)邊界很清晰——它不負(fù)責(zé)“繞遠(yuǎn)路”它只負(fù)責(zé)“別撞上”。全局規(guī)劃層給了它一個繞行的機(jī)會DWA 只需要在局部做精細(xì)避讓兩者協(xié)作后系統(tǒng)的安全底線和通行效率都有保障。5. 實戰(zhàn)排坑與避坑經(jīng)驗5.1 常見問題速查表調(diào)這三個算法時目標(biāo)行為怪異但出錯點往往不在算法本身而在輸入數(shù)據(jù)和參數(shù)之間。我在自己的項目里總結(jié)了一個問題速查表基本覆蓋了遇到的大多數(shù)情況。現(xiàn)象最可能原因解決方案DWA 原地?fù)u擺全局路徑缺失或前瞻點太近檢查 lookahead_dist上調(diào)到 2.5~3.0mA* 規(guī)劃極慢開放列表重復(fù)節(jié)點過多加 best_g 防止重復(fù)入堆A* 路徑貼墻沒有進(jìn)行地圖膨脹按機(jī)器人半徑安全余量膨脹地圖RRT 遲遲找不到路徑步長過大窄通道內(nèi)采樣失效縮短步長至巷道寬度 1/6 左右RRT 路徑過彎太大節(jié)點回溯后未做剪枝和平滑增加剪枝和三次樣條平滑融合后車突然急剎前瞻點跨過障礙區(qū)用累計弧長選點而非歐幾里得距離局部地圖漂移坐標(biāo)變換額外乘了偏航角檢查 tf 樹和坐標(biāo)系變換DWA 在轉(zhuǎn)彎處減速明顯速度權(quán)重太低或最大速度過大適當(dāng)降低該區(qū)域限速降低速度權(quán)重A* 路徑被廢棄RRT 頻繁接管動態(tài)障礙在 A* 路徑附近檢查地圖刷新頻率加大局部代價地圖更新頻率車?yán)@大圈走遠(yuǎn)路平滑迭代把路徑扯變形抑制過沖增加碰撞檢測保護(hù)這個表格里每個問題都是我實際調(diào)過、修過的不是網(wǎng)上隨便抄來的。比如“DWA 原地?fù)u擺”這個現(xiàn)象最有迷惑性第一眼看著像參數(shù)問題實際上往往不是 DWA 的問題——你去看 DWA 日志會發(fā)現(xiàn)它每個周期都在選“當(dāng)前最好”的速度只是那個“最好”一直在左右橫跳。真正原因是全局路徑在墻前面斷了或者前瞻點選到了障礙物上。把這幾行排查邏輯記住能省下大量調(diào)參時間。5.2 我踩過三次以上的坑第一個大概率會踩的坑忽略模擬器與真機(jī)的動力學(xué)差異。我在模擬倉庫里把 DWA 的加速度設(shè)得很大因為仿真模型的響應(yīng)是完美的順利跑通。到真機(jī)上底盤電機(jī)的響應(yīng)延遲和摩擦非線性導(dǎo)致實際加速度明顯低于設(shè)定值結(jié)果 DWA 模擬出來的軌跡與真實軌跡不一致碰撞檢測形同虛設(shè)。后來我在每個速度周期里加入了真實底盤速度反饋的閉環(huán)補(bǔ)償DWA 模擬所用起始速度不再取“上次指令速度”而是取“當(dāng)前里程計反饋速度”這從原理上保證了運動模型和真實狀態(tài)同步。真機(jī)效果瞬間正常了。第二個反復(fù)踩的坑是“地圖刷新頻率與全局重規(guī)劃之間的時間競賽”。初版系統(tǒng)里激光雷達(dá)以 10Hz 更新局部代價地圖全局規(guī)劃層每 5s 才檢查一次是否需要重規(guī)劃。當(dāng)動態(tài)障礙突然出現(xiàn)在全局路徑上時DWA 第一個發(fā)現(xiàn)并避讓但全局規(guī)劃層不知道路徑已經(jīng)失效還在用舊路徑引導(dǎo)。這導(dǎo)致一個很別扭的狀態(tài)DWA 明明繞開了障礙全局路徑卻還在障礙上下次前瞻點又會把車?yán)厝ボ囅瘛爱嬋Α彼频膩砘乩@。解決辦法是把“DWA 連續(xù) N 次檢測到前方無障礙但無法沿全局路徑前進(jìn)”作為全局重規(guī)劃的觸發(fā)條件而不是單純按時間輪詢。這個改動讓整個系統(tǒng)在動態(tài)場景下的表現(xiàn)穩(wěn)定了很多。第三個坑屬于工程素養(yǎng)層面的不要試圖在一個功能里同時改三個算法。我在早期融合時每次遇到問題就順手調(diào)一下 A* 的啟發(fā)代價、再改一改 RRT 的步長、再動一下 DWA 的權(quán)重結(jié)果根本分不清是哪個改動導(dǎo)致的出了新問題也無法定位。后來我給自己定了一個死規(guī)矩每次只改一個參數(shù)改完跑一遍基準(zhǔn)測試記錄行為變化再改下一個。雖然過程慢但排查效率非常高很多之前積累的參數(shù)選擇經(jīng)驗都是靠這個習(xí)慣沉淀下來的。做路徑規(guī)劃算法融合真正的難點不在算法本身而在“把三個算法各自的狀態(tài)、觸發(fā)條件、參數(shù)影響邊界梳理清楚”這個系統(tǒng)性工程。如果讓我給新人一個直接的建議先把 DWA 單獨調(diào)到能在一個簡單的矩形場地里穩(wěn)定繞開臨時障礙再把 A* 或 RRT 單獨調(diào)好最后才考慮融合融合時也要先從最簡單的一個全局路徑源 DWA 的組合入手跑通了再疊加冗余路徑源。這個順序能避開 80% 的坑。最后再分享一個小技巧每個規(guī)劃周期里把當(dāng)前選擇的速度、軌跡、全局路徑、前方障礙物距離這些關(guān)鍵數(shù)據(jù)全部可視化出來。你不需要畫多精致的圖只要能同時看到“全局路徑”和“DWA 當(dāng)前候選軌跡”這兩條線融合邏輯里大部分詭異行為都會一目了然。我調(diào)試過程中有一半的問題都是靠盯著一張實時軌跡圖瞬間看明白的比看任何日志都快。