做激光雷達(dá) SLAM 的工程師基本繞不開(kāi) LOAM。LeGO-LOAM 是 LOAM 家族里一個(gè)非常能打的改進(jìn)版論文全稱(chēng)Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain核心就一句話(huà)輕量化、面向地面優(yōu)化、適配變地形。我第一次完整讀它的時(shí)候最強(qiáng)的感受是作者沒(méi)有在數(shù)學(xué)上搞花活而是非常務(wù)實(shí)地利用了一個(gè)前提——我的機(jī)器人是貼著地面跑的。整條算法鏈路的每一步都在回答同一個(gè)問(wèn)題哪些點(diǎn)可以信哪些點(diǎn)是噪聲哪些約束值得花算力去優(yōu)化。這篇文章把我讀論文時(shí)收獲的設(shè)計(jì)思路、關(guān)鍵細(xì)節(jié)、復(fù)現(xiàn)經(jīng)驗(yàn)和踩坑心得完整寫(xiě)出來(lái)適合剛接觸 LiDAR SLAM 的入門(mén)者也適合準(zhǔn)備把算法往小車(chē)、巡檢機(jī)器人上部署的工程師。1. 論文在回答什么問(wèn)題1.1 原版 LOAM 的痛點(diǎn)在哪里原版 LOAM 是激光雷達(dá)里程計(jì)與建圖領(lǐng)域的一個(gè)里程碑。它把點(diǎn)云按曲率分成邊緣點(diǎn)和平面點(diǎn)先做相鄰幀的運(yùn)動(dòng)估計(jì)再做當(dāng)前幀到局部地圖的精配準(zhǔn)。這個(gè)框架到今天仍然在不少產(chǎn)品里服役但它有幾個(gè)在實(shí)際工程里非常別扭的地方。第一它一視同仁地處理所有點(diǎn)。對(duì)一臺(tái)地面車(chē)輛來(lái)說(shuō)一幀 16 線雷達(dá)點(diǎn)云里真正有幾何價(jià)值的點(diǎn)大部分來(lái)自地面而地面點(diǎn)之間變化平緩、特征高度重復(fù)把它們?nèi)磕萌プ銮视?jì)算和特征匹配算力基本上白費(fèi)了。低線束雷達(dá)的垂直視場(chǎng)角只有 30 度左右分配給地面的線束占比非常高這個(gè)問(wèn)題會(huì)被進(jìn)一步放大。第二它對(duì)植被幾乎無(wú)能為力。16 線雷達(dá)打到草叢、樹(shù)冠、灌木上的時(shí)候會(huì)得到大量離散、無(wú)規(guī)則的點(diǎn)。這些點(diǎn)不僅沒(méi)有穩(wěn)定的幾何結(jié)構(gòu)反而會(huì)作為高曲率點(diǎn)混進(jìn)特征集合把后續(xù)的點(diǎn)到直線、點(diǎn)到平面匹配帶偏。做過(guò)戶(hù)外實(shí)驗(yàn)的人應(yīng)該都有體會(huì)草地一多軌跡就容易飄。第三6 自由度優(yōu)化一鍋端。相鄰兩幀之間明明有些方向的約束很強(qiáng)、有些方向約束很弱原版 LOAM 在每次迭代里對(duì)所有六個(gè)參數(shù)一起調(diào)整。當(dāng)某個(gè)方向缺乏約束時(shí)整個(gè)法方程就會(huì)出現(xiàn)病態(tài)既拖慢收斂速度也容易陷入局部極小值。第四全局一致性偏弱。原版 LOAM 雖然有建圖模塊但缺少顯式的回環(huán)檢測(cè)和位姿圖優(yōu)化跑大場(chǎng)景時(shí)會(huì)緩慢累積漂移最后地圖出現(xiàn)明顯的左右分層。LeGO-LOAM 的出發(fā)點(diǎn)就是把這四個(gè)痛點(diǎn)逐個(gè)解決掉。1.2 三個(gè)關(guān)鍵詞拆開(kāi)看論文標(biāo)題里藏著三個(gè)關(guān)鍵詞值得一個(gè)一個(gè)拆開(kāi)看。**Lightweight輕量化**不是單純的跑得快而是一種系統(tǒng)性的減法。它通過(guò)地面分割和聚類(lèi)把一幀三萬(wàn)個(gè)點(diǎn)左右的數(shù)據(jù)砍到只剩幾千個(gè)候選特征再把 6 自由度優(yōu)化拆成兩步每一步只解 3 個(gè)自由度。這樣做的意義在于算法可以跑在嵌入式設(shè)備上而不是非得配一臺(tái)高性能工控機(jī)。**Ground-Optimized地面優(yōu)化**是全文最有價(jià)值的設(shè)計(jì)哲學(xué)。在 LeGO-LOAM 里地面不再是被動(dòng)處理的對(duì)象而是主動(dòng)利用的約束來(lái)源。地面平面被當(dāng)作一把天然的標(biāo)尺用來(lái)限制車(chē)輛的 roll、pitch 和 z 方向位移。這個(gè)思路對(duì)我后來(lái)的很多項(xiàng)目都有啟發(fā)環(huán)境里最穩(wěn)定、最不起眼的結(jié)構(gòu)往往就是最好的參考系。**Variable Terrain變地形**是它的目標(biāo)場(chǎng)景。論文面向的不是平整的停車(chē)場(chǎng)而是草地、土坡、林間小路這種滿(mǎn)地是噪聲點(diǎn)的環(huán)境。植被點(diǎn)被聚類(lèi)過(guò)濾掉坡道通過(guò)地面分割的延伸邏輯被適配這讓它在真實(shí)戶(hù)外場(chǎng)景里比原版 LOAM 更可靠。2. 系統(tǒng)設(shè)計(jì)與整體流程2.1 四模塊架構(gòu)總覽LeGO-LOAM 的完整鏈路可以分成四個(gè)模塊我習(xí)慣用一張表把它們的關(guān)系記下來(lái)模塊輸入輸出核心作用分割模塊原始 3D 點(diǎn)云地面點(diǎn)/物體點(diǎn)/噪聲標(biāo)記投影成范圍圖判地面聚類(lèi)去噪特征提取分割后的點(diǎn)云邊緣特征點(diǎn)、平面特征點(diǎn)大幅壓縮特征數(shù)量保證特征類(lèi)型純凈里程計(jì)模塊相鄰幀特征6DOF 位姿兩步 L-M 優(yōu)化高頻粗配準(zhǔn)建圖模塊特征與位姿全局地圖與優(yōu)化軌跡低頻精配準(zhǔn)因子圖與回環(huán)優(yōu)化整個(gè)數(shù)據(jù)流可以概括為原始點(diǎn)云先進(jìn)分割得到帶標(biāo)簽的點(diǎn)云特征提取根據(jù)標(biāo)簽分別挑選邊緣點(diǎn)和平面點(diǎn)里程計(jì)利用前后兩幀特征做運(yùn)動(dòng)估計(jì)輸出位姿建圖模塊把位姿作為約束加入因子圖并持續(xù)構(gòu)建和更新全局地圖。一句話(huà)總結(jié)就是先回答哪些點(diǎn)是可信的再回答可信點(diǎn)里哪些是特征最后才談怎么配準(zhǔn)。2.2 為什么先分割再做特征很多人會(huì)問(wèn)原版 LOAM 不做分割也能跑為什么非要加這一層答案是在低線束雷達(dá)上不做分割的特征提取質(zhì)量會(huì)明顯下降。16 線雷達(dá)每線之間的俯仰角只有約 2 度同一掃描環(huán)上的點(diǎn)分布本來(lái)就不夠密如果地面點(diǎn)、墻面點(diǎn)、樹(shù)葉點(diǎn)混在一起算曲率特征類(lèi)型的判斷會(huì)經(jīng)常出錯(cuò)。一個(gè)本來(lái)很平的草地表面因?yàn)閵A雜著雜草點(diǎn)曲率會(huì)忽高忽低結(jié)果被錯(cuò)誤地選成邊緣特征。分割模塊相當(dāng)于給后續(xù)所有處理加了一個(gè)語(yǔ)義過(guò)濾層。有了地面和物體的標(biāo)簽之后特征提取就不再是純幾何運(yùn)算而是帶有語(yǔ)義的幾何運(yùn)算地面點(diǎn)的曲率再高也永遠(yuǎn)不會(huì)成為邊緣特征物體點(diǎn)的曲率再低也不會(huì)被拿去當(dāng)平面特征。這個(gè)設(shè)計(jì)不僅減少了計(jì)算量更重要的是減少了錯(cuò)誤約束。在我看來(lái)這是 LeGO-LOAM 對(duì) LOAM 家族最重要的結(jié)構(gòu)性改進(jìn)。3. 關(guān)鍵環(huán)節(jié)逐層拆解3.1 地面分割一張范圍圖加一個(gè)角度閾值LeGO-LOAM 的第一步是把 3D 點(diǎn)云投影成一張二維范圍圖。行是雷達(dá)線束編號(hào)列是水平掃描角像素值就是該點(diǎn)的距離。投影本身不復(fù)雜但它把不規(guī)則的 3D 點(diǎn)云變成了規(guī)則網(wǎng)格為后面的鄰域搜索和聚類(lèi)提供了極大的便利。地面分割的核心邏輯是逐列掃描從圖像底部向上比較相鄰兩行點(diǎn)之間的垂直夾角verticalAngle atan2(Δz, sqrt(Δx2 Δy2))如果夾角小于某個(gè)閾值論文和代碼里常用約 10 度說(shuō)明相鄰兩條掃描線之間的點(diǎn)在垂直方向上幾乎沒(méi)升高這段表面基本貼著地面于是把當(dāng)前點(diǎn)標(biāo)記為地面點(diǎn)。為什么這個(gè)判據(jù)成立因?yàn)?16 線雷達(dá)相鄰線的俯仰角是固定的在平坦地面上越靠下的線束打到的距離越近相鄰兩點(diǎn)的 Δz 很小一旦遇到臺(tái)階、路沿、低矮植被Δz 會(huì)突然變大夾角一下子就超過(guò)閾值。處理坡道時(shí)LeGO-LOAM 還有一個(gè)向上延伸的技巧。它并不是只判斷最底下那幾行而是從已判定為地面的行繼續(xù)向上掃描只要當(dāng)前點(diǎn)的下鄰點(diǎn)是地面點(diǎn)并且當(dāng)前點(diǎn)與下鄰點(diǎn)之間的垂直夾角依然小于閾值就把當(dāng)前點(diǎn)也標(biāo)記為地面。這樣斜坡上那些看起來(lái)比平地更高的點(diǎn)也能被連續(xù)地劃進(jìn)地面集合不會(huì)出現(xiàn)坡道中段被攔腰截?cái)嗟那闆r。注意10 度這個(gè)閾值是經(jīng)驗(yàn)值對(duì)應(yīng)的是雷達(dá)基本水平安裝的工況。如果你的雷達(dá)有較大的仰角安裝或者車(chē)輛經(jīng)常行駛在陡坡上必須先做外參補(bǔ)償再重新標(biāo)定這個(gè)閾值。調(diào)得太大墻根和低矮灌木會(huì)被劃成地面調(diào)得太小坡道上的地面點(diǎn)會(huì)漏檢后面平面特征就會(huì)不足。3.2 點(diǎn)云聚類(lèi)怎么把植被噪聲一次濾掉地面分割之后剩下的非地面點(diǎn)還要做一次聚類(lèi)。LeGO-LOAM 用的是在范圍圖上的廣度優(yōu)先搜索通過(guò)相鄰像素在三維空間中的距離和夾角判斷它們是否屬于同一個(gè)物體。這里的思想也很直觀同一個(gè)物體表面上的點(diǎn)在空間上是連續(xù)且平滑的而兩個(gè)物體之間的邊界處從雷達(dá)原點(diǎn)看過(guò)去方向和距離都會(huì)產(chǎn)生明顯跳變。利用這個(gè)跳變信息就能把點(diǎn)云聚成一個(gè)個(gè)連通區(qū)域。聚類(lèi)完成之后只保留點(diǎn)數(shù)超過(guò)閾值的簇論文常用 30 個(gè)點(diǎn)作為下限。那些只有十幾二十個(gè)點(diǎn)的零散點(diǎn)云往往來(lái)自樹(shù)葉、細(xì)枝、電線桿的稀疏回波直接被丟掉。這一步就是 LeGO-LOAM 在變地形環(huán)境里表現(xiàn)好的關(guān)鍵。草地和灌木產(chǎn)生的點(diǎn)云在范圍圖上呈現(xiàn)為一個(gè)個(gè)細(xì)小碎片聚類(lèi)后大多不滿(mǎn)足最小簇大小丟掉它們之后后續(xù)的特征提取和匹配面對(duì)的是一個(gè)干凈得多的點(diǎn)云。我實(shí)際跑實(shí)驗(yàn)的時(shí)候?qū)Ρ冗^(guò)開(kāi)與不開(kāi)聚類(lèi)過(guò)濾的效果開(kāi)了之后軌跡的抖動(dòng)明顯減小地圖上的毛刺也少了很多。當(dāng)然這個(gè)閾值不是越大越好。如果環(huán)境里存在小型障礙物比如路肩、小石塊、單個(gè)行人它們的點(diǎn)云簇也可能很小閾值太大會(huì)把有用的物體直接濾掉。這一點(diǎn)在做避障類(lèi)項(xiàng)目時(shí)要格外小心。3.3 特征選取地面點(diǎn)出平面特征物體點(diǎn)出角點(diǎn)特征特征提取沿用 LOAM 的曲率平滑度定義取同一條掃描線上當(dāng)前點(diǎn)前后若干個(gè)點(diǎn)計(jì)算它們?cè)诶走_(dá)坐標(biāo)系下的距離差平方和作為曲面變化量 c。c 越大說(shuō)明當(dāng)前點(diǎn)越像邊緣c 越小說(shuō)明它附近越平坦。LeGO-LOAM 的關(guān)鍵變化在于加上了分割標(biāo)簽的約束。只有地面點(diǎn)才有資格被選為平面特征只有分割后的物體點(diǎn)才有資格被選為邊緣特征。同時(shí)它把每條掃描線劃分成若干子區(qū)域每個(gè)子區(qū)域只取曲率最大或最小的若干點(diǎn)。這樣做的好處有兩個(gè)一是特征在空間中分布更均勻不會(huì)全部擠在某個(gè)方向二是從根源上限制了特征總數(shù)。實(shí)際實(shí)現(xiàn)里還有一些剔除邏輯比如丟棄被遮擋區(qū)域邊緣的點(diǎn)、丟棄與激光束方向近似平行的點(diǎn)。這些點(diǎn)和真實(shí)角點(diǎn)類(lèi)似但匹配時(shí)極不穩(wěn)定一點(diǎn)小噪聲就會(huì)引起很大的位姿誤差。剔除它們之后特征匹配的魯棒性會(huì)明顯提升。這樣選出來(lái)的特征集合數(shù)量會(huì)從原版 LOAM 的幾千個(gè)下降到幾百個(gè)的量級(jí)。別小看這個(gè)數(shù)字變化它直接決定了后續(xù) L-M 優(yōu)化里雅可比矩陣的規(guī)模也決定了單幀計(jì)算耗時(shí)。3.4 兩步優(yōu)化把 6 自由度拆成 33LeGO-LOAM 里程計(jì)最核心的設(shè)計(jì)是兩步優(yōu)化這也是它和原版 LOAM 拉開(kāi)差距的地方。先說(shuō)平面特征的約束能力。平面特征對(duì)應(yīng)的殘差是點(diǎn)到平面距離一個(gè)平面點(diǎn)與它匹配到的局部平面之間的距離應(yīng)該為零。這個(gè)距離對(duì)沿平面法線方向的平移極其敏感但對(duì)平面內(nèi)的平移幾乎不敏感。地面平面的法線方向基本是垂直的所以地面平面特征能夠強(qiáng)有力地約束 roll、pitch 和 z 方向的平移。再說(shuō)邊緣特征的約束能力。邊緣特征對(duì)應(yīng)的殘差是點(diǎn)到直線距離一個(gè)邊緣點(diǎn)與它匹配到的空間直線之間的距離應(yīng)該為零。這條殘差對(duì)沿直線方向的平移不敏感但能很好地約束垂直直線方向的兩個(gè)平移自由度以及繞直線方向以外的旋轉(zhuǎn)。地面車(chē)輛在水平面上運(yùn)動(dòng)時(shí)邊緣特征主要來(lái)自路邊的立桿、樹(shù)干、建筑墻角它們天然能約束 x、y 和 yaw。于是優(yōu)化被拆成兩步第一步只使用地面平面特征求解 roll、pitch、z 三個(gè)參數(shù)第二步固定這三個(gè)參數(shù)只使用邊緣特征求解 x、y、yaw 三個(gè)參數(shù) 最后合并成一個(gè)完整的 6DOF 位姿。為什么這樣拆是對(duì)的從優(yōu)化理論角度看如果同時(shí)優(yōu)化 6 個(gè)參數(shù)地面平面特征在水平方向上的梯度接近于零整個(gè)法方程處于病態(tài)會(huì)讓收斂速度變得極慢還容易掉進(jìn)局部極小值。拆開(kāi)之后每一步的殘差都是同構(gòu)的、約束方向一致的優(yōu)化問(wèn)題從病態(tài)變成良態(tài)。更重要的是地面平面特征對(duì) x、y、yaw 幾乎沒(méi)有約束能力如果把它們混在一起優(yōu)化地面的微小噪聲反而會(huì)污染水平方向和偏航角的估計(jì)。分開(kāi)處理實(shí)現(xiàn)了每種特征只干自己最擅長(zhǎng)的事。代碼層面這一步通過(guò)兩次獨(dú)立的 L-M 求解完成。第一次優(yōu)化只構(gòu)建平面特征對(duì)應(yīng)的殘差項(xiàng)第二次優(yōu)化在第一次結(jié)果的基礎(chǔ)上構(gòu)建邊緣特征對(duì)應(yīng)的殘差項(xiàng)。兩次優(yōu)化的計(jì)算量都很小這也是 LeGO-LOAM 能實(shí)時(shí)運(yùn)行的關(guān)鍵原因之一。3.5 回環(huán)檢測(cè)與圖優(yōu)化讓地圖保持一致里程計(jì)負(fù)責(zé)高頻粗配準(zhǔn)建圖模塊負(fù)責(zé)低頻精配準(zhǔn)。LeGO-LOAM 的建圖模塊做兩件事第一用當(dāng)前幀特征與全局地圖配準(zhǔn)得到更精確的位姿并更新地圖第二把位姿作為節(jié)點(diǎn)、位姿間的約束作為因子構(gòu)建因子圖用增量式平滑與建圖方法做全局優(yōu)化并在檢測(cè)到回環(huán)時(shí)加入回環(huán)因子。回環(huán)檢測(cè)的主要思路并不復(fù)雜。當(dāng)機(jī)器人回到曾經(jīng)到過(guò)的位置附近時(shí)系統(tǒng)會(huì)在當(dāng)前位姿周?chē)欢ò霃絻?nèi)搜索歷史關(guān)鍵幀取出這些關(guān)鍵幀附近的局部地圖與當(dāng)前幀做配準(zhǔn)再用配準(zhǔn)分值判斷是不是真的回到了同一位置。如果通過(guò)了判斷就把當(dāng)前位姿應(yīng)約等于歷史位姿作為一個(gè)新因子加進(jìn)圖優(yōu)化。這一步讓 LeGO-LOAM 在大范圍場(chǎng)景里能顯著拉回累積漂移比原版 LOAM 只有前向遞推要完整得多。值得一提的還有地圖的數(shù)據(jù)結(jié)構(gòu)。LeGO-LOAM 會(huì)對(duì)加入全局地圖的特征點(diǎn)做體素濾波下采樣避免地圖點(diǎn)云無(wú)限膨脹。這個(gè)細(xì)節(jié)在實(shí)際工程里非常重要因?yàn)?16 線雷達(dá)跑上十幾分鐘如果不對(duì)地圖做降采樣內(nèi)存和匹配耗時(shí)都會(huì)失控。4. 實(shí)驗(yàn)設(shè)計(jì)與效果復(fù)盤(pán)4.1 測(cè)試平臺(tái)與場(chǎng)景論文使用的傳感器是 16 線激光雷達(dá)頻率 10Hz搭載在一臺(tái)小型地面無(wú)人車(chē)上。測(cè)試場(chǎng)景不是干凈的停車(chē)場(chǎng)而是校園里那種植被茂密、有坡道、有建筑物和圍欄的混合地形。這類(lèi)場(chǎng)景對(duì)低線束雷達(dá)相當(dāng)不友好草叢貢獻(xiàn)大量離散點(diǎn)樹(shù)冠制造陰影和遮擋坡道又讓雷達(dá)水平安裝、地面平整的默認(rèn)假設(shè)失效。選擇這樣的場(chǎng)景是很刻意的。如果只在平整路面上測(cè)試地面優(yōu)化帶來(lái)的收益很難體現(xiàn)出來(lái)只有在滿(mǎn)地是噪聲點(diǎn)、地形起伏的環(huán)境里分割、聚類(lèi)、兩步優(yōu)化的價(jià)值才會(huì)被充分放大。這也提醒我們?cè)u(píng)價(jià)一個(gè) SLAM 算法測(cè)試場(chǎng)景的選擇往往和算法本身一樣重要。4.2 關(guān)鍵結(jié)論更輕、不更差、甚至更穩(wěn)論文的定量對(duì)比是在相同數(shù)據(jù)上把 LeGO-LOAM 和原版 LOAM 放在一起比較的。核心結(jié)論可以概括成三點(diǎn)。第一特征點(diǎn)大幅減少。由于分割和聚類(lèi)先濾掉了大量無(wú)效點(diǎn)參與配準(zhǔn)的特征點(diǎn)數(shù)量下降非常明顯論文報(bào)告大約在 40% 的量級(jí)。這個(gè)數(shù)字在不同場(chǎng)景會(huì)有波動(dòng)但數(shù)量顯著下降這個(gè)方向是確定的。第二單幀處理時(shí)間顯著下降。更少的特征點(diǎn)加上兩步優(yōu)化讓每幀點(diǎn)云從接收到輸出位姿的耗時(shí)大幅縮短。作者在論文中給出的運(yùn)行時(shí)間是遠(yuǎn)優(yōu)于原版 LOAM 的這也是它能跑在嵌入式平臺(tái)上的底氣。第三精度不降反升。在帶植被的戶(hù)外測(cè)試數(shù)據(jù)上LeGO-LOAM 的軌跡漂移與原版 LOAM 相當(dāng)甚至更小。原因在前面的設(shè)計(jì)里已經(jīng)埋下地面約束提供了穩(wěn)定的垂直方向約束聚類(lèi)又過(guò)濾掉了會(huì)誤導(dǎo)匹配的植被點(diǎn)少了錯(cuò)誤約束配準(zhǔn)自然更干凈。需要提醒的是40%這類(lèi)數(shù)字是在特定數(shù)據(jù)、特定參數(shù)下得到的換到你的傳感器和場(chǎng)景里不一定能復(fù)現(xiàn)。但分割 聚類(lèi) 兩類(lèi)特征分治帶來(lái)的收益方向是明確的這個(gè)思路本身比具體數(shù)字更有價(jià)值。4.3 對(duì)實(shí)際工程的啟示LeGO-LOAM 給我最大的啟發(fā)是永遠(yuǎn)不要把你的傳感器數(shù)據(jù)當(dāng)成一個(gè)沒(méi)有結(jié)構(gòu)的點(diǎn)包。點(diǎn)云里天然帶有哪些點(diǎn)在哪面哪些點(diǎn)屬于哪棵樹(shù)的信息只是需要花一點(diǎn)成本去提取。對(duì)地面車(chē)輛來(lái)說(shuō)地面不是要繞開(kāi)的障礙而是最穩(wěn)定的標(biāo)定物對(duì)無(wú)人機(jī)來(lái)說(shuō)地面加上頂面可能是類(lèi)似的組合。把語(yǔ)義信息注入幾何配準(zhǔn)是讓算法在復(fù)雜地形上依然可靠的關(guān)鍵思路。這個(gè)思想后來(lái)影響了我的很多項(xiàng)目。哪怕不直接用 LeGO-LOAM我也會(huì)在點(diǎn)云預(yù)處理階段先做一次快速的地面分割和聚類(lèi)去噪再來(lái)做配準(zhǔn)。它帶來(lái)的穩(wěn)定性提升往往比換一個(gè)更復(fù)雜的優(yōu)化器來(lái)得更直接。5. 復(fù)現(xiàn)與調(diào)參實(shí)錄5.1 環(huán)境與數(shù)據(jù)準(zhǔn)備復(fù)現(xiàn) LeGO-LOAM 比想象中容易因?yàn)樗且粋€(gè)標(biāo)準(zhǔn)的 ROS 工程主流程用 PCL 做點(diǎn)云處理用 Ceres 做優(yōu)化用因子圖庫(kù)做圖優(yōu)化?；A(chǔ)步驟大概是準(zhǔn)備一臺(tái)裝好 ROS 的電腦可以是實(shí)體機(jī)也可以是虛擬機(jī)安裝 PCL、Ceres、GTSAM 等依賴(lài)庫(kù)編譯工程準(zhǔn)備一份包含點(diǎn)云話(huà)題的 rosbag或者直接接上真實(shí)雷達(dá)啟動(dòng)雷達(dá)驅(qū)動(dòng)和 LeGO-LOAM 節(jié)點(diǎn)在 rviz 里觀察全局地圖和里程計(jì)軌跡。如果你是第一次跑建議先用默認(rèn)參數(shù)跑通再逐步調(diào)整。默認(rèn)情況下點(diǎn)云話(huà)題通常是/velodyne_points類(lèi)型是sensor_msgs/PointCloud2如果你的雷達(dá)是其他品牌需要先改話(huà)題名有時(shí)還要改點(diǎn)云類(lèi)型。如果你用的不是 16 線雷達(dá)那么有幾個(gè)宏定義必須改線束數(shù)量、水平掃描分辨率、初始地面掃描行數(shù)。這些參數(shù)直接決定范圍圖的行列數(shù)改錯(cuò)會(huì)導(dǎo)致地面分割和特征提取全部錯(cuò)亂。我見(jiàn)過(guò)不少同學(xué)在這上面卡了很久最后發(fā)現(xiàn)只是線束數(shù)量沒(méi)改。5.2 關(guān)鍵參數(shù)的調(diào)整方法這里整理一份我實(shí)際調(diào)參時(shí)最常用的參數(shù)速查表參數(shù)項(xiàng)作用調(diào)整建議地面分割角度閾值約 10 度判斷相鄰掃描線是否屬于地面雷達(dá)有仰角時(shí)先做外參補(bǔ)償坡道多可以從 10 度往 12 度試最小聚類(lèi)點(diǎn)數(shù)約 30過(guò)濾小物體和植被噪聲植被密可以調(diào)大需要保留小障礙物就調(diào)小回環(huán)搜索半徑與歷史關(guān)鍵幀數(shù)量決定回環(huán)觸發(fā)的窗口漏報(bào)就加大半徑和數(shù)量誤報(bào)就調(diào)嚴(yán)配準(zhǔn)分?jǐn)?shù)地圖下采樣體素大小控制地圖內(nèi)存和匹配精度16 線雷達(dá)常用 0.2 米量級(jí)可據(jù)場(chǎng)景上下浮動(dòng)建圖頻率決定后臺(tái)地圖更新節(jié)奏嵌入式平臺(tái)可降低建圖頻率保證里程計(jì)實(shí)時(shí)性調(diào)參的順序也很重要。我一般先調(diào)地面分割確認(rèn)范圍圖上的地面標(biāo)簽穩(wěn)定再調(diào)聚類(lèi)和特征提取確認(rèn)特征數(shù)量不過(guò)多也不過(guò)少最后才動(dòng)回環(huán)和建圖的參數(shù)。前兩步?jīng)]調(diào)好后面所有參數(shù)都沒(méi)有意義。經(jīng)驗(yàn)之談如果你發(fā)現(xiàn)軌跡在 z 方向持續(xù)震蕩十有八九是地面分割漏檢或者閾值太松導(dǎo)致非地面點(diǎn)混進(jìn)了平面特征。如果你發(fā)現(xiàn)水平方向緩慢漂移先檢查邊緣特征數(shù)量是否太少再考慮回環(huán)參數(shù)。5.3 常見(jiàn)問(wèn)題與排查下面是我和身邊同事實(shí)際踩過(guò)的高頻問(wèn)題整理成一張排查表現(xiàn)象可能原因排查思路有里程計(jì)輸出但全局地圖一直是空建圖頻率低關(guān)鍵幀條件未滿(mǎn)足等待幾秒檢查關(guān)鍵幀計(jì)數(shù)看 rviz 里節(jié)點(diǎn)是否在發(fā)布軌跡發(fā)散地圖明顯撕裂外參不對(duì)、點(diǎn)云時(shí)間戳不同步用標(biāo)定工具重新標(biāo)定檢查各話(huà)題時(shí)間戳是否一致回環(huán)從來(lái)不被觸發(fā)搜索半徑太小、關(guān)鍵幀太少、配準(zhǔn)分閾值太嚴(yán)調(diào)大歷史關(guān)鍵幀搜索數(shù)量放寬配準(zhǔn)分?jǐn)?shù)閾值長(zhǎng)走廊、空曠停車(chē)場(chǎng)漂移明顯退化場(chǎng)景邊緣特征不足加輪式里程計(jì)或 IMU 因子兜底不要指望單體雷達(dá)硬扛地面分割把墻面底部劃成地面角度閾值過(guò)大或雷達(dá)安裝角度不對(duì)重新檢查外參適當(dāng)調(diào)小角度閾值排查這類(lèi)問(wèn)題最忌諱的就是盲調(diào)參數(shù)。我每次都會(huì)先把可視化打開(kāi)把地面點(diǎn)、物體點(diǎn)、邊緣特征、平面特征用不同顏色顯示出來(lái)一眼就能看出問(wèn)題出在哪個(gè)環(huán)節(jié)。可視化永遠(yuǎn)比盲調(diào)高效。6. 我個(gè)人的實(shí)踐體會(huì)6.1 先跑通再回來(lái)看代碼讀這類(lèi)論文不要把注意力只放在公式上。我的習(xí)慣是先拿到開(kāi)源實(shí)現(xiàn)把默認(rèn)參數(shù)跑通再回頭看代碼里哪一行實(shí)現(xiàn)了論文里的哪句話(huà)。LeGO-LOAM 的代碼結(jié)構(gòu)非常清晰模塊劃分和論文的章節(jié)幾乎是一一對(duì)應(yīng)的非常適合這種邊看代碼邊讀論文的學(xué)習(xí)方式。我把地面分割那段逐行讀完之后才真正理解了 10 度閾值到底在保護(hù)什么也才知道坡道延伸邏輯為什么寫(xiě)得那么啰嗦——那每一行都是在為真實(shí)場(chǎng)景兜底。6.2 認(rèn)清適用邊界LeGO-LOAM 不是萬(wàn)金油。它默認(rèn)傳感器下方一定存在一個(gè)可用的地面平面并且這個(gè)平面在雷達(dá)視場(chǎng)里是連續(xù)的。如果你的機(jī)器人是足式的或者雷達(dá)裝在無(wú)人機(jī)上或者作業(yè)場(chǎng)景是密集的室內(nèi)環(huán)境這個(gè)前提就不成立效果會(huì)明顯打折。這類(lèi)場(chǎng)景下更適合借鑒它的思路——分割、聚類(lèi)、分治優(yōu)化——而不是直接套用它的參數(shù)和結(jié)構(gòu)。說(shuō)到底算法是服務(wù)于假設(shè)的先搞清楚假設(shè)再選算法是我在這個(gè)項(xiàng)目里收獲最大的一點(diǎn)。6.3 調(diào)參之前先看數(shù)據(jù)這是我想強(qiáng)調(diào)很多次的一條經(jīng)驗(yàn)。LeGO-LOAM 的輸出對(duì)參數(shù)其實(shí)比較敏感尤其是地面分割閾值和聚類(lèi)點(diǎn)數(shù)。但絕大多數(shù)問(wèn)題在 rviz 里都能一眼看出來(lái)地面點(diǎn)是不是把墻根包進(jìn)去了、物體點(diǎn)聚類(lèi)是不是碎成渣、邊緣特征是不是全部擠在同一個(gè)方向。先看數(shù)據(jù)再?zèng)Q定調(diào)哪個(gè)參數(shù)一般十分鐘內(nèi)能定位到問(wèn)題。盲調(diào)參數(shù)的話(huà)可能調(diào)一下午都找不到方向。6.4 部署時(shí)的融合兜底如果你準(zhǔn)備把它部署到量產(chǎn)平臺(tái)上我強(qiáng)烈建議加上退化檢測(cè)和多傳感器融合。LeGO-LOAM 在普通場(chǎng)景表現(xiàn)穩(wěn)定但在長(zhǎng)走廊、空曠停車(chē)場(chǎng)、高速運(yùn)動(dòng)這類(lèi)工況下單靠 16 線雷達(dá)總有極限。工程上寧可多一個(gè)輪速計(jì)因子也不要讓系統(tǒng)在退化場(chǎng)景里裸奔。我之后的幾個(gè)項(xiàng)目里都是把 LeGO-LOAM 輸出的里程計(jì)作為主因子再疊加輪速計(jì)或 IMU 的輔助因子整體魯棒性立刻上了一個(gè)臺(tái)階。最后說(shuō)一個(gè)小技巧復(fù)現(xiàn) LeGO-LOAM 的時(shí)候最值得反復(fù)看的是地面分割的可視化結(jié)果。把分割輸出打開(kāi)逐幀觀察它把哪些點(diǎn)判成了地面哪些點(diǎn)被聚類(lèi)丟掉會(huì)讓你對(duì)地面優(yōu)化這四個(gè)字有非常直觀的理解。看完之后你對(duì)這篇論文的理解會(huì)比單純讀十遍公式都深。