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