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