做了幾年機(jī)器人導(dǎo)航我發(fā)現(xiàn)自己反復(fù)回到一個(gè)最基礎(chǔ)的問(wèn)題程序里的環(huán)境到底長(zhǎng)什么樣。有人用幾何特征有人用拓?fù)鋱D但真正到了避障、路徑規(guī)劃、多傳感器融合落地的時(shí)候絕大多數(shù)項(xiàng)目最終的落腳點(diǎn)都是一張柵格占用地圖。它把一個(gè)連續(xù)世界切成一格一格的判斷每一格回答一個(gè)問(wèn)題——“這里能不能過(guò)”。聽(tīng)起來(lái)簡(jiǎn)單想把它畫(huà)準(zhǔn)、存得住、實(shí)時(shí)更新里面藏著非常多容易低估的坑。這篇文章就把我在實(shí)際項(xiàng)目里對(duì)柵格占用地圖的理解、踩過(guò)的坑和可復(fù)用的做法一次性講透。1. 柵格占用地圖把連續(xù)世界拆成一格一格的判斷題1.1 為什么幾乎所有導(dǎo)航系統(tǒng)都繞不開(kāi)柵格先聊一個(gè)最根本的問(wèn)題我們明明有厘米級(jí)精度的激光雷達(dá)為什么不用連續(xù)坐標(biāo)去描述環(huán)境反而要把它離散成網(wǎng)格我的理解是連續(xù)坐標(biāo)對(duì)“定位精確”很友好但對(duì)“推理決策”非常不友好。機(jī)器人看到一個(gè)桌子它不需要記住桌角的精確坐標(biāo)它真正關(guān)心的是桌子占住的這一塊區(qū)域我的輪子能不能碾過(guò)去我的機(jī)械臂能不能伸進(jìn)去。這是一個(gè)區(qū)域性的問(wèn)題而不是一個(gè)點(diǎn)的問(wèn)題。柵格占用地圖正好把環(huán)境表達(dá)成“區(qū)域?qū)傩浴?。每一格代表地面上的一小塊面積存儲(chǔ)的是這一小塊被占據(jù)的概率?；蛘哒f(shuō)是這塊區(qū)域?qū)C(jī)器人而言的可通行程度。這種離散化帶來(lái)三個(gè)實(shí)打?qū)嵉暮锰巶鞲衅髟肼暠惶烊蝗萑?。激光打在同一個(gè)位置不可能每次都返回完全相同的坐標(biāo)但打在同一個(gè)柵格里結(jié)果就穩(wěn)定得多。更新邏輯統(tǒng)一。不管傳感器是激光、超聲波還是毫米波都能變成“某個(gè)網(wǎng)格狀態(tài)變化”的形式融合起來(lái)非常簡(jiǎn)單。路徑規(guī)劃算法可以直接作用。A*、Dijkstra、DWA這類算法都在離散網(wǎng)格上工作它們不需要理解“墻在幾何上是什么形狀”只需要知道“哪些柵格是占據(jù)的哪些是空閑的”。當(dāng)然柵格地圖也有缺點(diǎn)最明顯的就是“固定分辨率”和“內(nèi)存增長(zhǎng)”。如果你把分辨率定成2厘米那么一個(gè)100米乘100米的區(qū)域就是5000乘5000個(gè)格兩千萬(wàn)五百萬(wàn)個(gè)格這還是在二維。如果做成三維體素地圖內(nèi)存問(wèn)題會(huì)更突出。所以柵格占用地圖從來(lái)不是一個(gè)“拍了腦袋就選”的方案而是一個(gè)在表達(dá)能力和計(jì)算資源之間做權(quán)衡的方案。1.2 一張柵格地圖里到底存了什么柵格地圖的核心數(shù)據(jù)就是一個(gè)二維矩陣矩陣的每個(gè)元素對(duì)應(yīng)一塊物理區(qū)域。這個(gè)元素不只是0和1通常是一個(gè)概率值。常見(jiàn)的約定是0表示完全空閑1表示完全占據(jù)0.5表示未知。但實(shí)際存儲(chǔ)很少直接用0到1的浮點(diǎn)數(shù)而是用對(duì)數(shù)賠率log-odds來(lái)保存后面會(huì)細(xì)說(shuō)。我自己做項(xiàng)目時(shí)第一版最容易犯的錯(cuò)就是“用0和1畫(huà)地圖”。激光打在墻上就把對(duì)應(yīng)格子設(shè)成1沒(méi)打到的設(shè)成0。看起來(lái)沒(méi)毛病但實(shí)際一跑就出問(wèn)題激光打在玻璃上會(huì)偶爾穿透返回導(dǎo)致同一面墻這一幀是占據(jù)、下一幀是空閑人走過(guò)來(lái)的時(shí)候一部分點(diǎn)落在人身上另一部分穿過(guò)人腿之間的縫隙造成一個(gè)區(qū)域被反復(fù)翻轉(zhuǎn)。最終地圖上一片麻子。所以柵格地圖的工程實(shí)現(xiàn)必須帶“置信度”。一個(gè)網(wǎng)格被觀測(cè)到一次就判定為墻和被觀測(cè)到30次之后判定為墻顯然可信度不一樣。于是占據(jù)柵格地圖的底層邏輯變成我要記錄的是“根據(jù)到目前為止的所有觀測(cè)這個(gè)格子被占據(jù)的概率有多大”而不是“這一幀激光說(shuō)了什么”。1.3 分辨率的直覺(jué)從地面一塊磚說(shuō)起理解分辨率最簡(jiǎn)單的方式是把它當(dāng)成地面鋪磚。你把機(jī)器人活動(dòng)區(qū)域鋪成瓷磚每塊瓷磚記錄“站人了嗎”。瓷磚越大整張圖越省內(nèi)存但也有兩個(gè)代價(jià)一是小障礙物直接消失比如2厘米的線纜在10厘米分辨率下基本不存在二是邊界模糊機(jī)器人靠近墻的時(shí)候它不知道自己能不能再往前蹭3厘米因?yàn)槟且桓褚呀?jīng)標(biāo)記為占據(jù)。那到底怎么選分辨率我個(gè)人的經(jīng)驗(yàn)公式是分辨率至少要達(dá)到機(jī)器人底盤直徑的十分之一到五分之一。比如一個(gè)直徑40厘米的掃地機(jī)器人2到4厘米是一個(gè)合理的起步值。室內(nèi)環(huán)境0.05米足夠用室外無(wú)人車一般用0.1到0.2米因?yàn)槭彝夥秶?.05米會(huì)讓計(jì)算量和存儲(chǔ)直接爆掉而且室外激光噪聲遠(yuǎn)大于室內(nèi)高分辨率反而容易把噪聲也一塊塊畫(huà)進(jìn)去。關(guān)鍵經(jīng)驗(yàn)分辨率不是越小越好是“讓地圖能表達(dá)出你關(guān)心的最小障礙物”就好。2. 柵格背后的概率更新機(jī)制為什么不是“碰到就標(biāo)記”2.1 一次激光測(cè)量到底攜帶了哪些信息激光雷達(dá)一幀掃描有幾百甚至幾千個(gè)點(diǎn)每個(gè)點(diǎn)不只是“有一個(gè)點(diǎn)打到東西了”。如果我們樸素地理解每個(gè)點(diǎn)會(huì)告訴我們一個(gè)占據(jù)的位置。但如果只看這些點(diǎn)地圖會(huì)非常稀疏因?yàn)榧す馐?jīng)過(guò)的空曠區(qū)域沒(méi)有被表達(dá)出來(lái)。這也是很多新人在第一次自己寫(xiě)柵格地圖更新時(shí)都會(huì)漏掉的東西一條激光束從傳感器出發(fā)在命中一個(gè)障礙物之前它所有經(jīng)過(guò)的格子都應(yīng)該被標(biāo)記為“空閑”。只有最后那一段被擊中的位置才應(yīng)該標(biāo)記為“占據(jù)”。這個(gè)過(guò)程在SLAM里叫做射線投射ray casting。想象你手電筒照到一面墻你能說(shuō)“只有墻上那個(gè)亮點(diǎn)是存在的”嗎當(dāng)然不能。光束經(jīng)過(guò)的空間是空的所以機(jī)器人能看見(jiàn)墻這本身就是信息。如果把這些途經(jīng)點(diǎn)忽略地圖會(huì)變成一堆互不相連的“亮點(diǎn)”路徑規(guī)劃算法只能看見(jiàn)稀疏的幾個(gè)障礙點(diǎn)其他地方全是未知。實(shí)際結(jié)果就是機(jī)器人規(guī)劃出一條“穿墻”路線。所以一次有效的激光測(cè)量應(yīng)該拆成兩部分中間的空閑柵格 終點(diǎn)的占據(jù)柵格。而終點(diǎn)那個(gè)柵格也不應(yīng)該直接設(shè)成1而是把它的占據(jù)概率往上抬一點(diǎn)。每多觀測(cè)到一次就往“占據(jù)”方向再移動(dòng)一點(diǎn)。這就是概率更新的雛形。2.2 貝葉斯更新與log-odds一次講透既然每個(gè)網(wǎng)格維護(hù)的是一個(gè)概率那么新的觀測(cè)來(lái)了之后怎么把這個(gè)概率和舊的概率合并這里直接用貝葉斯公式是最自然的思路。假設(shè)某個(gè)網(wǎng)格被占據(jù)的概率是P(occ)我們想根據(jù)新的傳感器測(cè)量z更新它。那么更新后的概率可以寫(xiě)成P(occ | z) P(z | occ) * P(occ) / P(z)其中P(z | occ)是傳感器模型——也就是“如果這個(gè)格子真的被占據(jù)傳感器返回當(dāng)前這個(gè)測(cè)量的概率”。這個(gè)模型不需要特別精確通常只是一個(gè)常量。但真正的工程問(wèn)題在于如果直接用概率值做多次乘法數(shù)值會(huì)快速逼近0或1而且浮點(diǎn)運(yùn)算是先乘后歸一化長(zhǎng)時(shí)間運(yùn)行后精度容易出問(wèn)題。所以實(shí)際代碼里幾乎都用對(duì)數(shù)賠率log-odds也叫l(wèi)ogit表示。賠率定義為L(zhǎng)(x) log( P(x) / (1 - P(x)) )好處是新的測(cè)量可以用加法更新L_new L_old L_measurement其中L_measurement 0 表示“這一幀告訴我這個(gè)格子被占據(jù)了”L_measurement 0 表示“這一幀告訴我這個(gè)格子是空閑的”0表示未知、無(wú)信息這個(gè)加法更新推導(dǎo)很簡(jiǎn)單本質(zhì)是把貝葉斯公式里的乘積通過(guò)取對(duì)數(shù)變成加法。工程上還有一個(gè)額外好處就是不會(huì)因?yàn)檫B續(xù)累乘導(dǎo)致下溢出。我常用的初始化是 L_old 0對(duì)應(yīng)P0.5代表未知。一次“占據(jù)”觀測(cè)的增量大概設(shè)為0.5到0.9一次“空閑”觀測(cè)的增量設(shè)為-0.4到-0.7。數(shù)值大小取決于傳感器噪聲的嚴(yán)重程度噪聲大增量小一點(diǎn)噪聲小增量可以大一點(diǎn)。這里必須注意一個(gè)問(wèn)題不考慮“阻尼”會(huì)讓概率飽和。如果某格已經(jīng)被占據(jù)觀測(cè)了100次L值越來(lái)越大這時(shí)候有一幀因?yàn)閭鞲衅髡`檢而標(biāo)記它為空閑這一個(gè)-0.7根本拉不動(dòng)它。所以更新公式后面通常會(huì)夾一個(gè)上下限L_new clamp(L_old L_measurement, L_min, L_max)我一般取L_max10L_min-10。換算成概率大約是 P0.99995和P0.000045。這樣既保證了對(duì)噪聲的魯棒性又不會(huì)讓狀態(tài)徹底“焊死”。2.3 多傳感器融合超聲波寬波束怎么處理很多人一開(kāi)始只用激光雷達(dá)后來(lái)想加入超聲波或毫米波。這時(shí)候要注意不同傳感器的“射線模型”不一樣。激光可以近似成一條極細(xì)的射線而超聲波是一個(gè)錐形波束它告訴你“這個(gè)方向上某個(gè)距離內(nèi)有物體”但它無(wú)法精確到具體是光束中心還是邊緣返回的。處理寬波束有個(gè)常用技巧不要把整個(gè)錐形區(qū)域都認(rèn)為空閑也不要把返回點(diǎn)所在那一格直接標(biāo)為占據(jù)。而是按角度分成多個(gè)子射線中間角度視為真實(shí)的射線邊緣角度降低置信度。簡(jiǎn)單做法是對(duì)同一個(gè)網(wǎng)格如果被多個(gè)傳感器同時(shí)認(rèn)為是占據(jù)那么占據(jù)概率疊加得更高如果激光認(rèn)為是占據(jù)而超聲波認(rèn)為是空閑則要設(shè)置傳感器可信度權(quán)重。比如激光權(quán)重0.8超聲波權(quán)重0.3最終更新量就是加權(quán)和。我自己實(shí)際項(xiàng)目里超聲波主要是用來(lái)補(bǔ)盲區(qū)的所以很少讓它直接參與柵格地圖的更新更多是用它做緊急剎車。真正要融合時(shí)記住一個(gè)原則無(wú)論如何用一個(gè)統(tǒng)一的log-odds增量表別把各家傳感器的輸出直接疊加成一個(gè)概率值再存。柵格地圖內(nèi)部永遠(yuǎn)是log-odds概率值只是給人看的。3. 真實(shí)建圖流程從一幀激光到一張完整地圖3.1 坐標(biāo)系與傳感器模型先對(duì)齊再談建圖建圖的輸入通常不只是激光數(shù)據(jù)還包括機(jī)器人當(dāng)前位姿。位姿可能來(lái)自里程計(jì)、IMU、或者SLAM估計(jì)出來(lái)的定位結(jié)果。最關(guān)鍵的預(yù)處理是將激光點(diǎn)從激光坐標(biāo)系轉(zhuǎn)換到機(jī)器人坐標(biāo)系再轉(zhuǎn)換到世界系。這個(gè)環(huán)節(jié)如果外參標(biāo)定不準(zhǔn)地圖會(huì)出現(xiàn)“重影”——比如一面墻激光在前面幾幀看到它在x1.0的位置后來(lái)機(jī)器人轉(zhuǎn)了一圈再回來(lái)因?yàn)橥鈪⑵?.1米墻畫(huà)在x1.1的位置。這幾乎是新手翻車率最高的點(diǎn)。我見(jiàn)過(guò)很多項(xiàng)目建圖算法沒(méi)選錯(cuò)、參數(shù)沒(méi)調(diào)錯(cuò)地圖就是糊最后發(fā)現(xiàn)是激光雷達(dá)安裝支架歪了兩毫米在5米外被放大成幾厘米的重影。所以我自己在建圖之前必做三件事靜態(tài)外參標(biāo)定確認(rèn)激光相對(duì)機(jī)器人中心的平移和旋轉(zhuǎn)。時(shí)間同步檢查如果里程計(jì)和激光時(shí)間戳差超過(guò)幾十毫秒在機(jī)器人高速移動(dòng)時(shí)就會(huì)產(chǎn)生偏移。地面點(diǎn)過(guò)濾對(duì)于三維激光或雷達(dá)建二維柵格地圖前要把高于機(jī)器人底盤高度的點(diǎn)濾掉只保留“能擋住機(jī)器人”的障礙物層面的點(diǎn)。3.2 我自己常用的柵格更新偽代碼做了這么多概念鋪墊上一段最直觀的代碼。下面這個(gè)流程可以用在任何語(yǔ)言里核心邏輯完全一樣def update_grid_map(grid_values, grid_logodds, robot_pose, laser_points, params): # 把激光點(diǎn)轉(zhuǎn)到世界坐標(biāo) for point in laser_points: world_point robot_to_world(robot_pose, point) # 射線投射從機(jī)器人位置到激光點(diǎn)末端先清空沿途格子 cells bresenham(robot_pose.xy, world_point) for cell in cells[:-1]: idx grid_index(cell, params.resolution, params.map_origin) grid_logodds[idx] params.free_increment # 終點(diǎn)是占據(jù)但也需要做范圍檢查 last cells[-1] idx grid_index(last, params.resolution, params.map_origin) grid_logodds[idx] params.hit_increment # 可選將logodds轉(zhuǎn)成概率用于可視化/發(fā)布 for idx in updated_indices: grid_values[idx] sigmoid(grid_logodds[idx])這個(gè)代碼里有幾個(gè)細(xì)節(jié)值得提bresenham用畫(huà)線算法找出激光穿越了哪些柵格。注意這個(gè)遍歷順序是從傳感器位置出發(fā)到命中點(diǎn)結(jié)束不包括命中點(diǎn)本身。free_increment和hit_increment的值不一定要選成絕對(duì)值相等的相反數(shù)。因?yàn)橐粠す饫锟臻e觀測(cè)的數(shù)量遠(yuǎn)多于占據(jù)觀測(cè)如果兩者等幅最后地圖會(huì)整體偏“空閑”。所以通常hit_increment絕對(duì)值比f(wàn)ree_increment大一些比如hit0.8free-0.4。每次更新只修改“本幀激光穿過(guò)的格子”不需要把整張地圖重算一遍這對(duì)性能非常重要。對(duì)一個(gè)400個(gè)點(diǎn)的一幀激光用上面的邏輯更新一次在普通CPU上大約需要幾十微秒到幾百微秒完全來(lái)得及實(shí)時(shí)跑。3.3 從“局部圖”到“全局圖”的拼接思路實(shí)際建圖不是單幀更新而是邊移動(dòng)邊更新。最簡(jiǎn)單的全局建圖方法就是把每一幀激光點(diǎn)通過(guò)當(dāng)前位姿變換到全局坐標(biāo)系然后執(zhí)行上面的柵格更新。這里的核心問(wèn)題是當(dāng)前位姿從哪來(lái)如果你的機(jī)器人只靠輪式里程計(jì)那么在平整地面上短距離還是可以的但一打滑就會(huì)出現(xiàn)地圖漂移。所以工業(yè)級(jí)做法是疊加SLAM實(shí)時(shí)估計(jì)位姿并回環(huán)檢測(cè)。地圖本身和SLAM是相互依賴的關(guān)系SLAM用地圖來(lái)校正位姿地圖又依賴位姿來(lái)更新。這也是為什么Cartographer這類系統(tǒng)會(huì)把柵格地圖作為核心后端。我在實(shí)際項(xiàng)目中很少自己?jiǎn)为?dú)寫(xiě)完整的SLAM大部分時(shí)候是用現(xiàn)成的SLAM框架構(gòu)建柵格地圖然后導(dǎo)出地圖文件給導(dǎo)航用。構(gòu)建完成之后的關(guān)鍵工作是“地圖后期處理”清理噪聲散點(diǎn)、填充小空洞、把地圖轉(zhuǎn)成導(dǎo)航用的代價(jià)地圖。4. 地圖數(shù)據(jù)結(jié)構(gòu)與內(nèi)存優(yōu)化從大數(shù)組到稀疏存儲(chǔ)4.1 一個(gè)樸素二維數(shù)組何時(shí)會(huì)撐爆內(nèi)存先算一筆賬。假設(shè)環(huán)境是200米乘200米分辨率0.05米那就是4000乘4000總共1600萬(wàn)個(gè)格子。如果每個(gè)格子存一個(gè)float4字節(jié)這張圖就要64MB存一個(gè)double8字節(jié)就要128MB。看起來(lái)還能接受但真實(shí)項(xiàng)目里不會(huì)只有一層地圖。導(dǎo)航系統(tǒng)通常需要多個(gè)圖層靜態(tài)占據(jù)層、代價(jià)層、膨脹層、障礙物層、機(jī)器人足跡層。單是5層地圖疊在一起64MB就變成了320MB這還沒(méi)算地圖以多分辨率保存的占用。對(duì)于載具、無(wú)人機(jī)、室外機(jī)器人來(lái)說(shuō)連續(xù)建圖幾小時(shí)地圖范圍可能超過(guò)1公里這時(shí)候直接使用二維數(shù)組存儲(chǔ)會(huì)直接爆掉。4.2 實(shí)際項(xiàng)目里我用的幾種存儲(chǔ)方案我在實(shí)踐中慢慢形成了這樣一個(gè)優(yōu)先級(jí)稀疏哈希表優(yōu)先只存儲(chǔ)被更新過(guò)的格子沒(méi)有觀測(cè)到的格子不占用空間。一個(gè)房間建圖完成后真正被更新過(guò)的格子可能只占理論總格數(shù)的20%到40%走廊場(chǎng)景甚至?xí)陀?%。用unordered_map或Python的dict存儲(chǔ)時(shí)key是柵格坐標(biāo)value是log-odds值。缺點(diǎn)是查詢速度比連續(xù)數(shù)組慢但實(shí)際跑下來(lái)性能差別在毫秒級(jí)別完全可以接受。四叉樹(shù)/八叉樹(shù)四叉樹(shù)適合二維柵格八叉樹(shù)適合三維體素柵格。核心思路是如果一個(gè)區(qū)域內(nèi)所有子格子的狀態(tài)都相同那么在父節(jié)點(diǎn)直接存一個(gè)統(tǒng)一值不再往下細(xì)分。這在地圖有大片空閑區(qū)域時(shí)非常高效。缺點(diǎn)是更新局部格子時(shí)需要從根節(jié)點(diǎn)向下遍歷代碼復(fù)雜度高一些調(diào)試起來(lái)也麻煩。分層金字塔地圖對(duì)同一張地圖保存0.05米、0.1米、0.2米等多層級(jí)。路徑規(guī)劃時(shí)全局規(guī)劃用粗分辨率快速估算局部規(guī)劃用精分辨率細(xì)致搜索。這種方案更偏應(yīng)用層但不少商業(yè)導(dǎo)航產(chǎn)品就是這么干的。4.3 數(shù)據(jù)類型選擇也能省一半內(nèi)存很多人在柵格地圖里存儲(chǔ)概率值都用float甚至double其實(shí)完全沒(méi)有必要。log-odds值的范圍通常限制在[-10, 10]這個(gè)精度用int8_t就足夠表達(dá)。把log-odds乘以一個(gè)縮放系數(shù)比如10范圍[-100, 100]用單字節(jié)表示。整張1600萬(wàn)格子的地圖內(nèi)存直接從64MB降到16MB。我曾經(jīng)在一個(gè)項(xiàng)目里為了優(yōu)化把所有柵格從float改成了int16_t內(nèi)存降了一半更新速度反而略快因?yàn)榫彺婷新矢吡?。這里有一個(gè)工程折中的點(diǎn)如果你在嵌入式設(shè)備上跑這個(gè)優(yōu)化幾乎是必選項(xiàng)。提醒一句別只盯著“地圖本身”的內(nèi)存還要考慮后續(xù)的膨脹層、發(fā)布用ROS消息時(shí)的拷貝、可視化圖像編碼。這些中間副本也會(huì)吃不少內(nèi)存處理不好會(huì)導(dǎo)致系統(tǒng)卡頓。5. 柵格地圖使用中的高頻翻車點(diǎn)與對(duì)應(yīng)避坑方案5.1 分辨率選錯(cuò)不是細(xì)就好也不是粗就好前面提過(guò)網(wǎng)格分辨率的選擇公式但實(shí)際項(xiàng)目里還有兩個(gè)隱藏問(wèn)題。太細(xì)0.02米分辨率下激光束經(jīng)過(guò)的格子數(shù)量暴增射線投射計(jì)算量跟著漲同時(shí)每一個(gè)細(xì)小噪聲都會(huì)被獨(dú)立畫(huà)成一個(gè)“障礙物”。很多清掃機(jī)器人在0.02米分辨率下建圖地圖上全是飛點(diǎn)。太粗0.2米分辨率下一個(gè)直徑30厘米的障礙物只占一兩格機(jī)器人路徑規(guī)劃時(shí)哪怕距離膨脹層做得好也容易貼得太近。我調(diào)試時(shí)的習(xí)慣是先用0.1米分辨率快速驗(yàn)證整個(gè)建圖鏈路是否通暢確認(rèn)沒(méi)有問(wèn)題后再降到0.05米。這樣可以隔離參數(shù)問(wèn)題鏈路沒(méi)通你調(diào)到0.01米也沒(méi)用。5.2 動(dòng)態(tài)障礙物在靜態(tài)地圖里留下的“幽靈”柵格占用地圖默認(rèn)假設(shè)環(huán)境是靜態(tài)的。但在真實(shí)世界里人、車輛、開(kāi)門、移動(dòng)的箱子都會(huì)出現(xiàn)。如果這些動(dòng)態(tài)物體被寫(xiě)進(jìn)長(zhǎng)期更新地圖最后地圖上會(huì)出現(xiàn)“鬼影”明明人已經(jīng)走了那面區(qū)域還是顯示占據(jù)。解決這個(gè)問(wèn)題有三個(gè)層次前端過(guò)濾檢測(cè)動(dòng)態(tài)點(diǎn)不和靜態(tài)地圖更新。判斷方法可以很簡(jiǎn)單比如連續(xù)多幀同一個(gè)位置被觀測(cè)為占據(jù)但位姿一致性偏差很大就判定為動(dòng)態(tài)點(diǎn)。后端衰減對(duì)長(zhǎng)期地圖里的占據(jù)概率做時(shí)間衰減每隔一段時(shí)間把log-odds往0方向拉一點(diǎn)動(dòng)態(tài)物體的“占據(jù)”特征會(huì)慢慢消失。語(yǔ)義分離把動(dòng)態(tài)障礙物單獨(dú)放到一個(gè)實(shí)時(shí)層靜態(tài)地圖層只保留過(guò)濾后的靜態(tài)點(diǎn)。這也是自動(dòng)駕駛里costmap的標(biāo)準(zhǔn)做法。我用得最多的還是第三種。建圖階段關(guān)閉一些動(dòng)態(tài)層導(dǎo)航階段實(shí)時(shí)維護(hù)障礙物層?xùn)鸥竦貓D本身只保存環(huán)境結(jié)構(gòu)。5.3 閾值設(shè)置占據(jù)判定為何總是誤觸柵格地圖最終要從概率或log-odds轉(zhuǎn)換成一個(gè)“能否通行”的布爾值這就需要兩個(gè)閾值占據(jù)閾值和空閑閾值。比如一個(gè)格子log-odds大于0.5判定為占據(jù)小于-0.5判定為空閑介于兩者之間判定為未知。如果閾值設(shè)得太高動(dòng)態(tài)噪聲不會(huì)觸發(fā)占據(jù)但是真實(shí)的細(xì)小障礙物也會(huì)被漏掉設(shè)得太低一片區(qū)域會(huì)被誤判為墻。我在實(shí)際項(xiàng)目里一般把占據(jù)閾值設(shè)為0.7對(duì)應(yīng)的log-odds約0.85空閑閾值設(shè)為-0.4對(duì)應(yīng)的log-odds約-0.4。這組參數(shù)跑起來(lái)誤判比較少。但是注意這些值不是萬(wàn)能的。傳感器噪聲低時(shí)可以把閾值調(diào)低讓地圖更敏感噪聲高時(shí)必須調(diào)高閾值否則會(huì)滿地都是障礙。5.4 傳感器外參標(biāo)定誤差地圖“重影”的根子前面提到了外參這里再深入一點(diǎn)。如果激光雷達(dá)相對(duì)機(jī)器人中心的安裝角度偏了0.5度那么在5米外的障礙物位置就會(huì)偏移約4.4厘米。在一張0.05米分辨率的柵格地圖里這幾乎等于讓同一堵墻被畫(huà)到兩個(gè)相鄰格子上。我建議在建圖之前做一個(gè)簡(jiǎn)單驗(yàn)證讓機(jī)器人原地旋轉(zhuǎn)觀察同一面墻在地圖上的投影是否穩(wěn)定。如果墻的輪廓在旋轉(zhuǎn)過(guò)程中來(lái)回?cái)[動(dòng)那很大概率是外參問(wèn)題而不是建圖算法問(wèn)題。很多人在算法參數(shù)里折騰了一天最后發(fā)現(xiàn)是支架歪了。6. 從“畫(huà)地圖”到“用地圖”柵格地圖如何真正支撐導(dǎo)航?jīng)Q策6.1 占用柵格地圖到代價(jià)地圖的轉(zhuǎn)化柵格地圖本身不直接參與規(guī)劃。路徑規(guī)劃器需要的是一張“代價(jià)地圖”每個(gè)網(wǎng)格除了已知是否占據(jù)外還要知道“離障礙物多近算危險(xiǎn)”。這個(gè)危險(xiǎn)程度就是膨脹。膨脹的過(guò)程很簡(jiǎn)單對(duì)每個(gè)占據(jù)柵格以它為中心按機(jī)器人半徑加上安全距離把周圍一圈柵格的代價(jià)值從0平滑上升到一個(gè)最大值。路徑規(guī)劃搜索時(shí)代價(jià)高的柵格會(huì)被A*算法主動(dòng)繞開(kāi)。我在項(xiàng)目里一直強(qiáng)調(diào)不要直接在原始占用柵格地圖上做路徑規(guī)劃。因?yàn)樵嫉貓D只有“0/1/未知”沒(méi)有考慮機(jī)器人半徑和姿態(tài)。就算機(jī)器人中心在一格空閑區(qū)域它的機(jī)身在旋轉(zhuǎn)時(shí)也可能掃到旁邊的障礙物。只有膨脹后的代價(jià)地圖才能安全指導(dǎo)導(dǎo)航。6.2 地圖的存儲(chǔ)與傳輸一張地圖最后到底存成什么建圖完成后地圖通常要打包保存下來(lái)。工業(yè)界最常見(jiàn)的格式是PGM圖像加YAML配置。PGM里面每個(gè)像素對(duì)應(yīng)一個(gè)柵格灰度值代表狀態(tài)0表示占據(jù)黑色。254表示空閑白色。205表示未知灰色。YAML配置里記錄分辨率、原點(diǎn)坐標(biāo)、柵格長(zhǎng)度等元數(shù)據(jù)。加載地圖時(shí)解析圖像矩陣恢復(fù)成log-odds或代價(jià)地圖。這里推薦一個(gè)保存前的處理對(duì)地圖做一次形態(tài)學(xué)閉運(yùn)算把小空洞和空洞填補(bǔ)掉同時(shí)清理孤立噪聲點(diǎn)。否則把建圖結(jié)果直接扔給導(dǎo)航會(huì)出現(xiàn)很多莫名其妙的“小島障礙物”路徑規(guī)劃器為了繞一個(gè)只有一兩格的噪聲點(diǎn)繞路繞大半天。6.3 多機(jī)器人共建一張地圖需要注意什么最后聊一個(gè)常在多機(jī)器人協(xié)同、工廠物流場(chǎng)景出現(xiàn)的問(wèn)題多個(gè)機(jī)器人各建各的圖怎么合并成一張統(tǒng)一地圖。理論上只要知道兩個(gè)地圖原點(diǎn)之間的相對(duì)位姿變換就可以直接疊加。但實(shí)際上不同機(jī)器人建出的地圖可能存在尺度漂移、角度漂移更不用說(shuō)各自局部地圖的誤差累積。我做的做法是先提取地圖特征點(diǎn)做粗匹配再用柵格對(duì)齊做精配準(zhǔn)。坐標(biāo)變換算出后對(duì)重疊區(qū)域做加權(quán)平均——概率值取權(quán)重平均權(quán)重取決于每個(gè)機(jī)器人局部地圖的置信度。多機(jī)器人地圖合并沒(méi)有銀彈框架只是輔助真正麻煩的是各機(jī)器人定位誤差的統(tǒng)計(jì)規(guī)律不一樣這一點(diǎn)必須在合并前就估計(jì)出來(lái)。7. 我個(gè)人調(diào)試柵格地圖的幾條心得寫(xiě)到這里分享幾個(gè)在項(xiàng)目里反復(fù)驗(yàn)證過(guò)的經(jīng)驗(yàn)排序分先后第一先畫(huà)一張“夠糙”的地圖再慢工出細(xì)活。我第一版建圖永遠(yuǎn)用0.2米分辨率、低閾值先看整體結(jié)構(gòu)對(duì)不對(duì)。一旦整體結(jié)構(gòu)都是歪的調(diào)什么0.05米、調(diào)什么概率增量都是白費(fèi)功夫。第二把傳感器外參查三遍再動(dòng)算法參數(shù)。地圖重影、模糊、錯(cuò)位八成是外參或時(shí)間同步問(wèn)題不是柵格更新公式問(wèn)題。拿一個(gè)固定障礙物做原地旋轉(zhuǎn)測(cè)試是效率最高的排查手段。第三地圖更新里別貪心。free和hit增量不要設(shè)得太大。我見(jiàn)過(guò)有人為了讓地圖快速收斂把hit增量設(shè)成5結(jié)果機(jī)器人面前十厘米的紙箱被當(dāng)成墻導(dǎo)航完全卡死。寧可讓地圖收斂慢一點(diǎn)也要保證傳感器誤檢時(shí)不會(huì)造成災(zāi)難性后果。第四動(dòng)態(tài)物體永遠(yuǎn)別和靜態(tài)結(jié)構(gòu)混在一個(gè)圖層。即使你現(xiàn)在只做靜態(tài)巡檢你的工作場(chǎng)地里也一定會(huì)有門、車輛、人。不要等到地圖被“幽靈”弄亂了才后悔一開(kāi)始就把圖層分開(kāi)。柵格占用地圖是一個(gè)看起來(lái)特別基礎(chǔ)、實(shí)際又特別深的主題。它跨越了傳感器、概率估計(jì)、數(shù)據(jù)結(jié)構(gòu)、數(shù)據(jù)存儲(chǔ)、路徑規(guī)劃好幾個(gè)領(lǐng)域。把上面這套邏輯吃透不管以后是用現(xiàn)成導(dǎo)航框架還是自己寫(xiě)核心模塊都會(huì)非常順暢。希望這篇經(jīng)驗(yàn)總結(jié)能幫你少走一些我當(dāng)年走過(guò)的彎路。