一個(gè)移動(dòng)機(jī)器人要從A點(diǎn)走到B點(diǎn)中途有人走過(guò)來(lái)地面上多了臨時(shí)堆放的紙箱你要是沒(méi)給機(jī)器人配好路徑規(guī)劃它就只能面臨兩個(gè)結(jié)局要么直愣愣撞上去要么停在原地死等直到障礙自己消失。路徑規(guī)劃算法要解決的就是這個(gè)繞不開(kāi)的問(wèn)題而在這個(gè)領(lǐng)域里A*、RRT、DWA 是三個(gè)繞不開(kāi)的名字A* 負(fù)責(zé)在地圖上找出一條不撞墻的宏觀路線(xiàn)RRT 負(fù)責(zé)在沒(méi)有完整地圖時(shí)也能探出一條可達(dá)的路DWA 負(fù)責(zé)盯著眼前這一兩秒讓底盤(pán)既避障、又走得不毛躁。我是做移動(dòng)機(jī)器人導(dǎo)航的這幾年和這三個(gè)算法打了太多交道踩過(guò)的坑能裝滿(mǎn)一抽屜。這篇文章想做的只有一件事把它們的原理拆碎講透各自擅長(zhǎng)什么、死穴在哪再把“A* 全局規(guī)劃 DWA 局部避障”這個(gè)最常見(jiàn)的融合方案怎么落地、怎么調(diào)參、怎么排故從頭到尾給你捋一遍。剛接觸自主導(dǎo)航的人可以把它當(dāng)成一張入門(mén)地圖已經(jīng)在調(diào)導(dǎo)航參數(shù)的人可以直接跳到后面的速查表和排故清單。1. 三個(gè)算法先站穩(wěn)全局、采樣、局部各管哪一段1.1 A* 是地圖上的“路線(xiàn)規(guī)劃師”A* 不是某個(gè)機(jī)器人專(zhuān)用的算法它是圖搜索家族的明星也是我做全局規(guī)劃時(shí)最先上手的方案。你手里有一張柵格地圖每個(gè)格子要么可走、要么有障礙機(jī)器人要從起點(diǎn)格挪到終點(diǎn)格A* 做的就是告訴你沿哪一串格子走代價(jià)最小。它的思路樸素但好用維護(hù)兩個(gè)集合open list 和 close list。每輪從 open list 里挑一個(gè) f 值最小的格子擴(kuò)展f g h。g 是從起點(diǎn)走到當(dāng)前格已經(jīng)花掉的真實(shí)代價(jià)h 是從當(dāng)前格到終點(diǎn)的一個(gè)樂(lè)觀估計(jì)代價(jià)常用歐幾里得距離。每擴(kuò)展一個(gè)格子就把它的鄰居丟進(jìn) open list同時(shí)把父節(jié)點(diǎn)指向當(dāng)前格子循環(huán)直到終點(diǎn)被擴(kuò)展。一句話(huà)總結(jié)它是從所有候選路徑里優(yōu)先探索“看起來(lái)最有希望”的那幾條。類(lèi)比一下。你在陌生城市趕去一家飯店手里有地圖。你不會(huì)把所有路口都走一遍而是先估算哪些路口方向離飯店最近優(yōu)先走過(guò)去。A* 的 h 就是這個(gè)估算它能把搜索方向強(qiáng)烈地拽向終點(diǎn)所以比 Dijkstra 那種無(wú)差別掃描通常要快一個(gè)量級(jí)。補(bǔ)充一些實(shí)用考慮。網(wǎng)格鄰居用四鄰居還是八鄰居直接決定路徑是只能橫豎走還是可以斜穿。用八鄰居時(shí)斜向移動(dòng)的代價(jià)要按 sqrt(2) 倍計(jì)算否則地圖上會(huì)出現(xiàn)“斜著走和橫著走一樣快”的錯(cuò)覺(jué)路徑會(huì)失真。另一個(gè)常被忽略的點(diǎn)是啟發(fā)式函數(shù)的權(quán)重如果地圖很大可以給 h 乘一個(gè)大于 1 的權(quán)重來(lái)提速這叫加權(quán) A*。我試過(guò)把權(quán)重從 1.0 拉到 1.5 左右在大地圖上規(guī)劃耗時(shí)能明顯下降路徑長(zhǎng)度只多出百分之幾。但權(quán)重太大就會(huì)犧牲最優(yōu)性甚至看起來(lái)像“貪心”所以別一味求快。1.2 RRT 是“摸著石頭過(guò)河”的探險(xiǎn)家RRT 和 A* 長(zhǎng)相完全不同它不依賴(lài)柵格也不需要先建一張完整的環(huán)境圖。它的工作方式是從起點(diǎn)長(zhǎng)一棵隨機(jī)樹(shù)隨機(jī)撒一個(gè)點(diǎn)在樹(shù)上找離它最近的節(jié)點(diǎn)沿這個(gè)方向按固定步長(zhǎng)生長(zhǎng)出一個(gè)新節(jié)點(diǎn)如果新節(jié)點(diǎn)和父節(jié)點(diǎn)之間的線(xiàn)段沒(méi)有碰到障礙就把它掛上樹(shù)。這個(gè)循環(huán)重復(fù)幾百上千次樹(shù)會(huì)慢慢長(zhǎng)滿(mǎn)整個(gè)搜索空間等某個(gè)新節(jié)點(diǎn)距離終點(diǎn)足夠近就順著父節(jié)點(diǎn)回溯出一條完整路徑。這個(gè)機(jī)制讓它天然適合 A* 難以建模的問(wèn)題。比如機(jī)械臂在關(guān)節(jié)空間里找運(yùn)動(dòng)軌跡每個(gè)關(guān)節(jié)角度都是維度柵格化代價(jià)極高再比如地圖外沿有大量未知區(qū)域建圖本身就很勉強(qiáng)。這類(lèi)問(wèn)題里RRT 只需要判斷“兩點(diǎn)之間能不能走”不要求全局地圖完整往往是更現(xiàn)實(shí)的選擇。但隨機(jī)性是把雙刃劍。同一張地圖跑十次RRT 給出的路徑每次都完全不一樣而且很容易帶一堆無(wú)用拐角路徑長(zhǎng)度也可能明顯差于最優(yōu)解。工程上很少直接用裸 RRT一般會(huì)用它的兩個(gè)改進(jìn)版本。RRT* 在每次插入新節(jié)點(diǎn)后會(huì)把附近節(jié)點(diǎn)的父指針重新優(yōu)化讓解漸近收斂到最優(yōu)雙向 RRT 則從起點(diǎn)和終點(diǎn)同時(shí)向中間長(zhǎng)樹(shù)遇到狹窄通道時(shí)成功率提升明顯。從我實(shí)際調(diào)試的經(jīng)驗(yàn)看如果你急著讓一個(gè)粗糙但能用的導(dǎo)航系統(tǒng)跑起來(lái)雙向 RRT 加簡(jiǎn)單路徑裁剪性?xún)r(jià)比其實(shí)很高。1.3 DWA 盯著“下一秒怎么走”DWA 的視角和上面兩個(gè)完全不同。A* 和 RRT 都在回答“從全局看走哪條路”DWA 則在回答“下一個(gè)控制周期底盤(pán)該給多大線(xiàn)速度和角速度”。它不評(píng)估遠(yuǎn)方只看眼前一小段。算法流程是這樣的根據(jù)當(dāng)前速度、底盤(pán)最大加速度、控制周期算出一個(gè)下一周期可行的速度組合窗口對(duì)窗口里的每一組速度按恒定速度向前模擬一小段軌跡然后給每條軌跡打分主要看它會(huì)不會(huì)撞障礙、末端朝向是不是趨近目標(biāo)、速度夠不夠快最后選得分最高的速度組合發(fā)給電機(jī)。每個(gè)控制周期都重新算一遍因此對(duì)動(dòng)態(tài)障礙物的反應(yīng)很快這也是它成為局部規(guī)劃主力的原因。DWA 的“動(dòng)態(tài)窗口”這四個(gè)字指的就是每步約束出來(lái)的可行速度集合。這個(gè)集合不是憑空來(lái)的而是由當(dāng)前運(yùn)動(dòng)狀態(tài)和機(jī)器人物理極限共同決定的。明白了這一點(diǎn)就明白 DWA 為什么會(huì)出那些經(jīng)典毛病只看腳下路徑會(huì)陷入局部最優(yōu)目標(biāo)太遠(yuǎn)時(shí)容易對(duì)著墻反復(fù)試探。這也是為什么生產(chǎn)級(jí)方案幾乎不會(huì)單獨(dú)用 DWA而是把它掛在全局路徑后面。1.4 選型參考什么場(chǎng)景用誰(shuí)直接把三種算法放進(jìn)應(yīng)用場(chǎng)景里對(duì)比場(chǎng)景特征推薦組合原因地圖完整、環(huán)境基本靜止A* DWAA* 能給出最短且穩(wěn)定的全局路線(xiàn)DWA 負(fù)責(zé)沿途避障地圖未知或頻繁變化雙向 RRT DWA不依賴(lài)全局精確柵格能快速摸出可達(dá)路徑人流多、動(dòng)態(tài)障礙密集A* 強(qiáng)化局部規(guī)劃全局路線(xiàn)讓機(jī)器人不繞遠(yuǎn)局部層要留足反應(yīng)空間機(jī)械臂等關(guān)節(jié)空間規(guī)劃RRT/RRT*高維空間難以柵格化采樣法幾乎是唯一實(shí)用選項(xiàng)選型不是比誰(shuí)更高級(jí)而是看環(huán)境缺什么信息、底盤(pán)缺什么約束。對(duì)絕大多數(shù)輪式移動(dòng)機(jī)器人來(lái)說(shuō)第一行“A* DWA”就是最穩(wěn)的起點(diǎn)。先把它跑通再談別的這套組合也是后面要展開(kāi)的融合主線(xiàn)。2. A* 和 RRT 的全局規(guī)劃實(shí)戰(zhàn)原理與摳參數(shù)2.1 A* 的代價(jià)函數(shù)怎么寫(xiě)最穩(wěn)直接給你一段可以反復(fù)用的偽代碼骨架# A* 主循環(huán)偽代碼 open_list [start] closed_list [] while open_list: current open_min_f(open_list) # 每次取 f 最小的節(jié)點(diǎn) if current goal: return reconstruct_path(current) open_list.remove(current) closed_list.append(current) for neighbor in get_neighbors(current): if neighbor in closed_list: continue tentative_g current.g distance(current, neighbor) if tentative_g neighbor.g or neighbor not in open_list: neighbor.g tentative_g neighbor.h heuristic(neighbor, goal) neighbor.parent current open_list.add(neighbor)對(duì)有經(jīng)驗(yàn)的讀者來(lái)說(shuō)這段代碼太“標(biāo)準(zhǔn)”了但標(biāo)準(zhǔn)正好說(shuō)明核心邏輯簡(jiǎn)單。真正影響效果的是后面三件容易被忽略的事。啟發(fā)式函數(shù)選型八鄰居網(wǎng)格里用歐幾里得距離是自然選擇曼哈頓距離只能用在四鄰居場(chǎng)景否則會(huì)嚴(yán)重高估移動(dòng)代價(jià)。h 權(quán)重大于 1 時(shí)規(guī)劃更快但結(jié)果次優(yōu)我一般在大地圖上把權(quán)重壓在 1.2~1.5 之間。open list 的實(shí)現(xiàn)方式節(jié)點(diǎn)很多時(shí)線(xiàn)性?huà)呙枞∽钚≈禃?huì)明顯變慢。工程上建議用二叉堆數(shù)據(jù)量大再考慮更復(fù)雜的數(shù)據(jù)結(jié)構(gòu)。我見(jiàn)過(guò)有人拿著基礎(chǔ)實(shí)現(xiàn)處理 500×500 的地圖一幀規(guī)劃卡了 300 多毫秒換成堆之后降到十幾毫秒這是純數(shù)據(jù)結(jié)構(gòu)的收益不是算法本身變高級(jí)了。閉合檢查也要留意neighbor 同時(shí)由多條路徑到達(dá)時(shí)要正確比較并更新 g否則路徑可能在角落反復(fù)橫跳。細(xì)節(jié)雖小但出問(wèn)題時(shí)會(huì)表現(xiàn)為地圖上出現(xiàn)“同一點(diǎn)被反復(fù)選中”的卡頓排查起來(lái)容易懷疑人生。2.2 柵格分辨率、膨脹半徑和路徑毛刺實(shí)際導(dǎo)航中A* 的問(wèn)題通常不在搜索算法本身而在“地圖喂得對(duì)不對(duì)”。一個(gè)常見(jiàn)組合是地圖分辨率 0.05 米機(jī)器人半徑 0.3 米膨脹半徑設(shè) 0.35 米。這個(gè)配置下A* 找出來(lái)的路徑一般會(huì)離障礙物半米左右局部規(guī)劃器跟起來(lái)很舒服。分辨率如果改到 0.2 米效果就開(kāi)始變質(zhì)。窄通道在地圖上可能只剩一個(gè)格子機(jī)器人物理尺寸根本過(guò)不去但 A* 不知道路徑照樣從那里穿過(guò)去。所以地圖分辨率要和機(jī)器人尺寸掛鉤經(jīng)驗(yàn)上單個(gè)柵格邊長(zhǎng)別超過(guò)機(jī)器人外接圓半徑的一半。地圖精度也不是越高越好0.02 米和 0.05 米在大多數(shù)室內(nèi)場(chǎng)景根本沒(méi)區(qū)別但計(jì)算量會(huì)暴漲。膨脹本身也會(huì)帶來(lái)問(wèn)題。膨脹半徑設(shè)得太大A* 會(huì)認(rèn)為所有路都被堵死在地圖上找不到解設(shè)得太小路徑貼著墻走給局部規(guī)劃器留的調(diào)整空間不足。比較穩(wěn)的做法是膨脹半徑 機(jī)器人外接圓半徑 安全余量 定位誤差。安全余量通常給 5~10 厘米定位誤差按實(shí)際傳感器水平估。然后是路徑毛刺。A* 在柵格上搜出來(lái)的路徑由一串相鄰格子組成折角多斜線(xiàn)少。直接拿給 DWA 跟輪子會(huì)頻繁修正方向。后期可以做一個(gè)剪枝從起點(diǎn)開(kāi)始往前找能無(wú)碰撞直連的最遠(yuǎn)點(diǎn)刪掉中間點(diǎn)再對(duì)剩下的點(diǎn)做輕量平滑。這個(gè)步驟不用引入復(fù)雜的曲線(xiàn)優(yōu)化就能把路徑長(zhǎng)度和角度跳變降下來(lái)一大截。2.3 RRT 的關(guān)鍵參數(shù)步長(zhǎng)、碰撞檢測(cè)與迭代次數(shù)RRT 的參數(shù)比 A* 少很多但并不代表好調(diào)。步長(zhǎng)是最關(guān)鍵的一個(gè)太大新節(jié)點(diǎn)可能一步跨過(guò)窄縫漏掉可行路徑太小樹(shù)長(zhǎng)得慢到達(dá)終點(diǎn)要更多迭代規(guī)劃耗時(shí)增加。按地圖尺度來(lái)定比如地面機(jī)器人場(chǎng)景步長(zhǎng)取 0.5 米左右是一個(gè)常用起點(diǎn)再按地圖障礙密度微調(diào)。碰撞檢測(cè)也要注意。判斷“節(jié)點(diǎn)之間能否直連”時(shí)不能只檢查兩個(gè)端點(diǎn)要在線(xiàn)段上均勻采樣逐點(diǎn)看是不是處于障礙或膨脹區(qū)域。采樣太粗會(huì)漏檢路徑看著能通過(guò)實(shí)際會(huì)撞。我習(xí)慣把采樣間隔設(shè)成地圖柵格分辨率的二分之一以下。迭代次數(shù)決定成功率。迭代太少樹(shù)還沒(méi)有覆蓋到終點(diǎn)方向容易超時(shí)失敗迭代太多耗時(shí)白白浪費(fèi)。工程上可以先跑一次看第多少次迭代找到解然后把這個(gè)數(shù)字乘 1.5 作為上限。如果你用的是 RRT*還要額外關(guān)心重連半徑和優(yōu)化迭代次數(shù)。重連半徑太小節(jié)點(diǎn)優(yōu)化效果基本為零太大每次插入節(jié)點(diǎn)都掃描一堆鄰居耗時(shí)上升。我在二維平面上常用“步長(zhǎng)的 2~3 倍”作為起始值再根據(jù)實(shí)測(cè)調(diào)整。2.4 全局規(guī)劃怎么打分規(guī)劃結(jié)果好不好不能靠感覺(jué)。我自己評(píng)估全局規(guī)劃主要看四個(gè)指標(biāo)規(guī)劃耗時(shí)、路徑長(zhǎng)度、平滑性累計(jì)轉(zhuǎn)角、成功率。算法耗時(shí)路徑長(zhǎng)度平滑性適用場(chǎng)景基礎(chǔ) A*中短一般柵格完整、可重復(fù)規(guī)劃加權(quán) A*短略長(zhǎng)一般大圖、實(shí)時(shí)要求高RRT*中較長(zhǎng)差高維空間、連續(xù)空間雙向 RRT中較長(zhǎng)差狹窄通道、地圖不完整路徑長(zhǎng)度和平滑性往往是一對(duì)矛盾。過(guò)分追求最短路徑必然貼障礙、折角多追求平滑路徑會(huì)變長(zhǎng)。全局規(guī)劃層只需要求“大致合理”把大量平滑工作交給局部規(guī)劃層去處理這是分層架構(gòu)的核心思想也是后面融合方案的出發(fā)點(diǎn)。3. DWA 局部規(guī)劃器拆解速度窗口、軌跡模擬與評(píng)分3.1 速度窗口怎么算DWA 的第一步是確定“動(dòng)態(tài)窗口”。假設(shè)機(jī)器人當(dāng)前線(xiàn)速度是 0.2 米/秒最大線(xiàn)加速度是 0.5 米/秒2控制周期是 0.1 秒那么下一周期能達(dá)到的線(xiàn)速度范圍就是 0.15~0.25 米/秒。角速度也一樣由當(dāng)前角速度和最大角加速度約束。這個(gè)范圍還要和機(jī)器人允許的絕對(duì)速度上限取交集比如底盤(pán)最高線(xiàn)速度 0.8 米/秒你就不能算出一個(gè) 0.9 的采樣點(diǎn)。如果問(wèn)題只有加減速約束DWA 還算簡(jiǎn)單。真正復(fù)雜的是剎車(chē)約束就算某個(gè)速度組合在加減速范圍內(nèi)如果機(jī)器人以上這個(gè)速度沖出去在可預(yù)測(cè)的距離內(nèi)剎不住、會(huì)撞上障礙那它也是不可行的。所以窗口最終是“加速度窗口 ∩ 速度極限 ∩ 剎車(chē)安全區(qū)”。這個(gè)邏輯就是 DWA 被稱(chēng)為“動(dòng)態(tài)”的根源窗口不是固定值而是隨當(dāng)前狀態(tài)不斷變化。給個(gè)直觀例子。機(jī)器人正以 0.5 米/秒沖向一個(gè) 0.4 米外的箱子最大減速度 0.8 米/秒2那它至少需要 0.156 米的剎車(chē)距離按勻減速公式 v2/(2a) 算勉強(qiáng)來(lái)得及但任何加速選項(xiàng)都會(huì)被剔除。如果箱子在 0.2 米外能用的速度選項(xiàng)基本就只剩下“原地制動(dòng)”了。3.2 軌跡模擬與預(yù)測(cè)時(shí)間確定速度窗口后DWA 會(huì)對(duì)每個(gè)可行速度組合做一次短時(shí)模擬假設(shè)在接下來(lái)的一小段時(shí)間里機(jī)器人保持這個(gè)速度不變沿著一條圓弧或直線(xiàn)前進(jìn)生成一條模擬軌跡。模擬的預(yù)測(cè)時(shí)間一般取 0.5~1.5 秒這是個(gè)很講究的參數(shù)。預(yù)測(cè)時(shí)間太短機(jī)器人只看得見(jiàn)眼前一小段障礙稍微遠(yuǎn)一點(diǎn)就“看不見(jiàn)”會(huì)顯得反應(yīng)過(guò)激頻繁急停預(yù)測(cè)時(shí)間太長(zhǎng)它又會(huì)過(guò)度考慮遠(yuǎn)期軌跡是否撞墻在通道里容易猶豫不決明明貼著墻就可以過(guò)它卻覺(jué)得前面會(huì)撞。我用來(lái)回測(cè)試的起點(diǎn)通常是 1.0 秒障礙密集時(shí)縮短到 0.6 秒開(kāi)闊環(huán)境拉到 1.2 秒。另一個(gè)容易被忽略的點(diǎn)是采樣分辨率。線(xiàn)速度采樣 5 檔、角速度采樣 10 檔就有 50 條模擬軌跡每個(gè)控制周期都要重新算一遍計(jì)算量完全可接受。但采樣太粗時(shí)速度選擇不連續(xù)相鄰周期可能從“猛轉(zhuǎn)”跳到“直行”機(jī)器人就會(huì)抖。采樣太細(xì)則差異太小容易在幾個(gè)相似選項(xiàng)中反復(fù)橫跳。實(shí)際操作中觀察模擬軌跡的疏密再對(duì)比機(jī)器人實(shí)際路徑的平穩(wěn)度就能大致判斷該往哪邊調(diào)。3.3 評(píng)價(jià)函數(shù)的三項(xiàng)方向、距離、速度每條模擬軌跡的得分由三個(gè)量決定score α × 方向偏差 β × 障礙距離 γ × 速度方向偏差衡量軌跡末端朝向和目標(biāo)的夾角障礙距離衡量軌跡與最近障礙的間距速度項(xiàng)則獎(jiǎng)勵(lì)走得快。如果一個(gè)軌跡會(huì)撞上障礙分?jǐn)?shù)直接記負(fù)無(wú)窮或直接淘汰。權(quán)重配平是 DWA 調(diào)試的核心。α 過(guò)大機(jī)器人會(huì)特別“犟”非要正對(duì)目標(biāo)不可結(jié)果貼著障礙邊緣硬擠β 過(guò)大機(jī)器人安全第一繞遠(yuǎn)路也不在乎在窄通道里會(huì)墨跡γ 過(guò)大機(jī)器人貪快動(dòng)態(tài)障礙高發(fā)區(qū)容易剎不住。我建議的初始配平順序是先把 β 固定在一個(gè)中間值比如 0.8再調(diào) α 和 γ 的比值。一組能跑通大多數(shù)室內(nèi)場(chǎng)景的起始權(quán)重是 α0.5、β0.8、γ0.3但這不是萬(wàn)能值。有一個(gè)關(guān)鍵改動(dòng)要從融合的角度理解在純 DWA 里“目標(biāo)方向”直接指向終點(diǎn)一旦把它掛在全局路徑后面“目標(biāo)”應(yīng)該改為局部前瞻點(diǎn)方向而不是遠(yuǎn)方的最終終點(diǎn)。否則機(jī)器人會(huì)在接近終點(diǎn)前就開(kāi)始猶豫或者在岔路口做出不合理的轉(zhuǎn)向。這個(gè)改動(dòng)一句話(huà)就能說(shuō)完但很多人融合失敗就是栽在這。3.4 DWA 三個(gè)經(jīng)典故障和處理方向原地旋轉(zhuǎn)不停常見(jiàn)原因是目標(biāo)方向項(xiàng)權(quán)重 α 太大機(jī)器人還沒(méi)往前挪就急著轉(zhuǎn)向或者局部目標(biāo)點(diǎn)離機(jī)器人太近速度小、轉(zhuǎn)向要求又高。先試著降低 α或把速度項(xiàng)權(quán)重提上去也要看是不是局部目標(biāo)一直貼在機(jī)器人正前方很近處。左右抖動(dòng)相鄰控制周期選出的速度組合頻繁跳變。處理辦法通常是把預(yù)測(cè)時(shí)間拉長(zhǎng)一點(diǎn)或者把采樣分辨率調(diào)粗也可以給速度輸出加限制讓每個(gè)周期的變化量不能超過(guò)某個(gè)比例。速度全部被拒絕日志里找不到可發(fā)送的可行速度機(jī)器人直接死鎖。優(yōu)先檢查目標(biāo)點(diǎn)是不是被代價(jià)地圖膨脹成了障礙其次看當(dāng)前速度窗口是不是太小最后看剎車(chē)距離計(jì)算有沒(méi)有因?yàn)闇p速度設(shè)置太小而把所有選項(xiàng)都掐死。4. A* DWA 融合落地路徑跟隨、前瞻目標(biāo)與參數(shù)聯(lián)動(dòng)4.1 為什么不直接讓 DWA 追終點(diǎn)把終點(diǎn)直接丟給 DWA理論上也能走但實(shí)際表現(xiàn)很糟距離遠(yuǎn)時(shí)終點(diǎn)方向在機(jī)器人坐標(biāo)系里的變化很慢DWA 缺少“遠(yuǎn)方該繞哪兒”的指引遇到第一個(gè)障礙就可能被帶偏。局部規(guī)劃的每一步都只保證“這一秒最優(yōu)”多個(gè)“這一秒最優(yōu)”拼起來(lái)整體路線(xiàn)往往不是最短甚至?xí)谡系K前磨蹭。融合的基本結(jié)構(gòu)可以理解成一個(gè)“宏觀意志下放”的過(guò)程A* 在地圖上算出一條全局路徑這條路徑被切分成一串離散點(diǎn)機(jī)器人實(shí)時(shí)找回離自己最近的點(diǎn)把前方一段距離外的那個(gè)點(diǎn)當(dāng)作臨時(shí)目標(biāo)DWA 只需要負(fù)責(zé)把這個(gè)臨時(shí)目標(biāo)吃進(jìn)評(píng)價(jià)函數(shù)輸出速度。這樣全局的繞行決策和局部的避障動(dòng)作就自然分開(kāi)了。這樣的分層架構(gòu)還有個(gè)額外好處地圖更新時(shí)不需要讓局部規(guī)劃器重新思考全局。全局規(guī)劃以較低頻率重算局部規(guī)劃器每個(gè)控制周期都允許動(dòng)態(tài)障礙改變它的決策兩者節(jié)奏不一各司其職。4.2 實(shí)現(xiàn)流程路徑重采樣、目標(biāo)切換與坐標(biāo)對(duì)齊把前面說(shuō)的結(jié)構(gòu)落到代碼上可以拆成四個(gè)步驟全局路徑重采樣把 A* 產(chǎn)出的一串稠密格點(diǎn)按固定間距比如 0.2 米轉(zhuǎn)成離散點(diǎn)序列。計(jì)算機(jī)器人當(dāng)前位置到每個(gè)路徑點(diǎn)的距離找到最近點(diǎn)。從最近點(diǎn)向前取一段距離這個(gè)位置作為局部目標(biāo)。前瞻距離通常取 1~2 個(gè)車(chē)身長(zhǎng)度或者按“當(dāng)前線(xiàn)速度 × 1.5~2 秒”估算。把局部目標(biāo)點(diǎn)從全局坐標(biāo)轉(zhuǎn)換到機(jī)器人坐標(biāo)系再交給 DWA 的評(píng)價(jià)函數(shù)使用。這里最關(guān)鍵的是前瞻距離的選擇。前瞻太短機(jī)器人對(duì)全局路徑的跟隨太“粘”路徑稍微有波動(dòng)就跟著急剎走過(guò)直角彎時(shí)會(huì)一頓一頓前瞻太長(zhǎng)則過(guò)彎時(shí)機(jī)器人會(huì)切內(nèi)線(xiàn)離障礙物很近甚至把全局路徑的彎道“拉直”了。我調(diào)試時(shí)有個(gè)快速經(jīng)驗(yàn)如果機(jī)器人過(guò)彎順暢但路徑貼邊就把前瞻距離拉大一點(diǎn)如果機(jī)器人頻繁停車(chē)、轉(zhuǎn)彎遲鈍就把前瞻距離縮近一點(diǎn)。這個(gè)參數(shù)通常不需要精確公式而是根據(jù)實(shí)測(cè)路徑形態(tài)反推。4.3 更高級(jí)的融合低分辨率 A* RRT* 局部規(guī)劃A* DWA 是“柵格世界的標(biāo)準(zhǔn)答案”但當(dāng)環(huán)境里局部障礙非常復(fù)雜、柵格圖又不可靠時(shí)可以再加一層 RRT*。一種可行的混合策略是全局層用低分辨率 A*以很低的頻率更新給出一個(gè)大致方向上的粗路徑當(dāng)機(jī)器人進(jìn)入障礙密集區(qū)域時(shí)局部層用 RRT* 在這個(gè)區(qū)域快速重規(guī)劃出一條無(wú)碰撞的短線(xiàn)段替代原來(lái)那一段的全局路徑DWA 繼續(xù)負(fù)責(zé)把這條短線(xiàn)段平滑地執(zhí)行出來(lái)。背后的思路是分層降低不確定性A* 提供方向穩(wěn)定性RRT* 提供對(duì)復(fù)雜局部的“可繞過(guò)”判斷DWA 提供底盤(pán)物理約束下的平滑執(zhí)行。這種三明治結(jié)構(gòu)比兩層結(jié)構(gòu)復(fù)雜得多代價(jià)地圖也要分多層維護(hù)但它確實(shí)能處理很多“A* 認(rèn)為走得通、實(shí)際現(xiàn)場(chǎng)卻亂成一團(tuán)”的場(chǎng)景。如果你剛開(kāi)始做融合我不建議一上來(lái)就搞三層先把 A* DWA 調(diào)順再考慮往里塞 RRT*否則出問(wèn)題都不知道去哪一層找。4.4 參數(shù)聯(lián)動(dòng)控制周期、最大速度、前瞻距離怎么一起調(diào)融合系統(tǒng)的參數(shù)不是獨(dú)立存在的必須聯(lián)動(dòng)調(diào)整。我習(xí)慣按照下面順序取數(shù)控制周期由底盤(pán)接口決定通常是 0.05~0.1 秒。最大線(xiàn)速度由現(xiàn)場(chǎng)安全要求決定取值時(shí)得保證緊急剎停距離遠(yuǎn)小于傳感器可視范圍。前瞻距離按“最大線(xiàn)速度 × 1.5~2 秒”估算。比如最大線(xiàn)速度 0.5 米/秒前瞻距離約 0.8~1.0 米。最大加速度取一個(gè)讓單周期速度增量不超過(guò)最大線(xiàn)速度 10%~15% 的值這樣底盤(pán)起步、剎車(chē)都相對(duì)平穩(wěn)。舉個(gè)例子。我在一個(gè)模擬巡檢項(xiàng)目里用過(guò)這組配置控制周期 0.1 秒最大線(xiàn)速度 0.5 米/秒最大角速度 1.0 弧度/秒最大線(xiàn)加速度 0.4 米/秒2最大角加速度 2.0 弧度/秒2前瞻距離 0.9 米預(yù)測(cè)時(shí)間 1.0 秒。這個(gè)組合跑起來(lái)整體平順動(dòng)態(tài)障礙從側(cè)面插進(jìn)來(lái)時(shí)DWA 也能在 1 米左右的距離做出明顯的減速避讓。注意這只是起點(diǎn)不同底盤(pán)和傳感器下差異很大一定要按自己的機(jī)器人實(shí)測(cè)。5. 避坑經(jīng)驗(yàn)、排故速查與調(diào)試心得5.1 地圖分辨率與機(jī)器人尺寸不匹配很多人第一次翻車(chē)都翻在這地圖用 0.2 米分辨率機(jī)器人外接圓半徑卻接近 0.35 米。A* 規(guī)劃出來(lái)的路徑在地圖上看著穿過(guò)了某個(gè)窄口實(shí)際上機(jī)器人物理上根本擠不進(jìn)去由于膨脹半徑當(dāng)時(shí)沒(méi)包含機(jī)器人尺寸路徑也會(huì)貼著墻角走導(dǎo)致局部規(guī)劃器一路瘋狂修正。對(duì)策分兩層地圖分辨率盡量滿(mǎn)足單格邊長(zhǎng)小于機(jī)器人半徑的一半這是硬指標(biāo)膨脹半徑一定要含上機(jī)器人外接圓半徑、安全余量和定位誤差。否則你調(diào)再多的 DWA 權(quán)重也救不回來(lái)因?yàn)閱?wèn)題的根源在“地圖對(duì)機(jī)器人來(lái)說(shuō)壓根不可通行”。5.2 DWA 死鎖所有采樣軌跡都被判失敗現(xiàn)象比較典型機(jī)器人停在原地速度始終為零日志刷滿(mǎn)了“找不到可行軌跡”。我排查這個(gè)問(wèn)題時(shí)一般按順序做三件事。第一看局部目標(biāo)點(diǎn)。代價(jià)地圖的膨脹層可能把目標(biāo)點(diǎn)附近的區(qū)域全標(biāo)成了障礙目標(biāo)點(diǎn)本身不可達(dá)這時(shí)機(jī)器人不管怎么采樣都“撞墻”。處理辦法是讓全局規(guī)劃把終點(diǎn)或局部目標(biāo)改成最近的自由格點(diǎn)。第二看膨脹半徑是不是過(guò)大。機(jī)器人明明離障礙還有 30 厘米但代價(jià)地圖認(rèn)為已經(jīng)是禁區(qū)也會(huì)把可行速度全掐斷。第三看剎車(chē)約束計(jì)算。最大減速度設(shè)得太小會(huì)讓剎車(chē)距離變大動(dòng)態(tài)窗口被進(jìn)一步壓縮速度選項(xiàng)就少了。把減速度調(diào)大一點(diǎn)再給原地轉(zhuǎn)向留一條可選速度死鎖往往會(huì)解除。5.3 全局路徑貼墻局部規(guī)劃蛇形走位這種現(xiàn)象表現(xiàn)為機(jī)器人路線(xiàn)總體正確但輪子一直在左右修方向走起來(lái)像蛇。先別急著怪 DWA多半是全局路徑本身離障礙太近。A* 的代價(jià)函數(shù)只優(yōu)化距離不優(yōu)化“安全間距”所以如果膨脹半徑?jīng)]設(shè)置好或者加權(quán) A* 權(quán)重太大導(dǎo)致路徑靠墻抄近路就必然出現(xiàn)這種結(jié)果。處理辦法是給全局層的障礙距離加權(quán)或者干脆把膨脹半徑再加大一圈讓 A* 主動(dòng)繞開(kāi)“剛剛好能過(guò)”的區(qū)域。我還會(huì)在后處理階段對(duì)路徑做一次“向外推”的輕量操作把每個(gè)路徑點(diǎn)往遠(yuǎn)離最近障礙物的方向挪一小段距離再重新檢測(cè)碰撞。這個(gè)操作在走廊環(huán)境里尤其有用。5.4 重規(guī)劃震蕩機(jī)器人左右擺頭融合系統(tǒng)跑起來(lái)后最常見(jiàn)的動(dòng)態(tài)問(wèn)題不是撞而是“左右擺”。A* 全局路徑不變但每個(gè)控制周期算出來(lái)的局部目標(biāo)點(diǎn)都在細(xì)微變化DWA 的評(píng)分對(duì)目標(biāo)方向非常敏感于是速度指令來(lái)回跳底盤(pán)表現(xiàn)為左右擺頭。給目標(biāo)切換加一個(gè)滯回區(qū)問(wèn)題能消掉大半。具體做法是當(dāng)前局部目標(biāo)點(diǎn)與上一個(gè)目標(biāo)點(diǎn)的距離小于某個(gè)閾值比如 0.1 米時(shí)繼續(xù)沿用舊目標(biāo)不切新的。這本質(zhì)上是給離散控制加了一點(diǎn)“慣性”讓決策不要因?yàn)槲⑿_動(dòng)而來(lái)回翻轉(zhuǎn)。另一個(gè)輔助手段是略微降低角速度采樣分辨率減少相鄰周期速度指令的跳變幅度。5.5 排故速查表現(xiàn)象可能原因檢查點(diǎn)處理方向機(jī)器人懟墻地圖分辨率過(guò)低路徑穿過(guò)不可行窄縫柵格邊與機(jī)器人半徑比提高地圖精度加大膨脹原地死鎖DWA 評(píng)分全失敗目標(biāo)點(diǎn)是否被膨脹減速度設(shè)置修正目標(biāo)點(diǎn)調(diào)大減速度蛇形走位全局路徑貼障礙、折角多膨脹半徑、路徑采樣點(diǎn)加大膨脹路徑平滑左右擺頭局部目標(biāo)頻繁切換前瞻距離、目標(biāo)切換邏輯加滯回區(qū)降低采樣分辨率規(guī)劃超時(shí)卡頓A* 在大地圖上搜索過(guò)慢open list 數(shù)據(jù)結(jié)構(gòu)、h 權(quán)重?fù)Q二叉堆、用加權(quán) A*排故的核心思路是分層定位先分清是全局路徑的問(wèn)題還是局部執(zhí)行的問(wèn)題再動(dòng)參數(shù)。不要一上來(lái)就調(diào) DWA 的權(quán)重很多時(shí)候根子在前面那層。5.6 調(diào)試心得把內(nèi)部狀態(tài)畫(huà)出來(lái)比盲調(diào)參數(shù)快十倍這是我最想強(qiáng)調(diào)的一點(diǎn)。調(diào)導(dǎo)航參數(shù)時(shí)不要只看機(jī)器人最終跑得好不好要把中間狀態(tài)全部可視化全局路徑點(diǎn)、當(dāng)前局部目標(biāo)、DWA 模擬出來(lái)的幾十條軌跡、代價(jià)地圖、還有每條軌跡的評(píng)分全都畫(huà)在同一張圖上。一旦機(jī)器人出現(xiàn)怪動(dòng)作先通過(guò)畫(huà)面定位是哪一層的問(wèn)題。全局路徑在障礙物里穿說(shuō)明 A* 這層錯(cuò)了全局路線(xiàn)上局部目標(biāo)亂跳說(shuō)明前瞻邏輯有問(wèn)題全局路徑?jīng)]問(wèn)題但模擬軌跡全貼著障礙那就去調(diào) DWA 的評(píng)價(jià)函數(shù)。我見(jiàn)過(guò)很多開(kāi)發(fā)者憑感覺(jué)在參數(shù)文件里來(lái)回改改半天也不知道是哪一層導(dǎo)致就是因?yàn)闆](méi)看到中間狀態(tài)。另外一個(gè)容易出效果的小操作是把 DWA 的預(yù)測(cè)時(shí)間臨時(shí)調(diào)大比如從 1.0 秒調(diào)到 2.0 秒你就能在可視化面板里看到機(jī)器人“預(yù)判”的軌跡到底傾向哪邊這時(shí)候評(píng)分的傾向性一目了然。看完再調(diào)回實(shí)際值調(diào)參效率會(huì)高很多。我個(gè)人這幾年做下來(lái)最大的體會(huì)是路徑規(guī)劃這個(gè)領(lǐng)域真正難的從來(lái)不是某個(gè)算法本身的公式而是怎么讓不同工作節(jié)奏的算法在一臺(tái)機(jī)器人身上協(xié)同工作。A* 想的是全局要不要繞路RRT 想的是到底有沒(méi)有一條路DWA 想的是這一秒怎么走穩(wěn)。把這三層接好之后機(jī)器人才會(huì)像一個(gè)有判斷力的司機(jī)而不只是條件反射的避障器。最后再分享一個(gè)很小的習(xí)慣每次只改一個(gè)參數(shù)改完把評(píng)分日志打出來(lái)對(duì)比數(shù)據(jù)別一次動(dòng)三四個(gè)參數(shù)。路徑規(guī)劃這種系統(tǒng)牽一發(fā)動(dòng)全身盲改參數(shù)翻車(chē)之后你連復(fù)現(xiàn)問(wèn)題都困難。數(shù)據(jù)說(shuō)話(huà)比手感可靠得多。