在激光SLAM的發展歷程中LOAMLidar Odometry and Mapping無疑是一座里程碑。Ji Zhang博士于2014年提出的這一算法創造性地將復雜的SLAM問題解耦為高頻里程計線程與低頻建圖線程通過提取邊緣點和平面點進行點到線、點到面的掃描匹配在KITTI數據集上取得了當時最佳的精度。然而LOAM的光環之下掩藏著一個根本性的結構缺陷它缺乏全局回環檢測能力難以整合GPS等絕對測量進行位姿校正且采用全局體素地圖進行匹配的方式在大規模場景中計算效率驟降累積漂移問題始終揮之不去。2020年Tixiao Shan在LeGO-LOAM基礎上推出的LIO-SAM以因子圖Factor Graph架構為核心徹底改變了這一局面。一、因子圖從“鏈式遞推”到“網狀優化”理解LIO-SAM的革命性首先要理解因子圖是什么。因子圖是一種用于表示復雜優化問題的概率圖模型由兩類節點構成變量節點和因子節點。變量節點是需要估計的狀態例如機器人各時刻的位姿位置姿態、速度、IMU零偏等因子節點則代表傳感器測量對狀態施加的約束——可以是相鄰幀之間的相對約束也可以是全局絕對觀測。LIO-SAM的優化目標就是找到一組最可能的狀態序列使得所有因子殘差的加權平方和最小。LIO-SAM的因子圖中主要包含四類因子IMU預積分因子約束相鄰兩個關鍵幀之間的狀態變化。與LOAM僅用IMU做點云去畸變的松耦合不同LIO-SAM將IMU作為緊耦合因子納入全局優化其協方差矩陣是一個15×15的矩陣反映了陀螺儀和加速度計白噪聲在積分過程中傳播導致的不確定性由于IMU置信度高其因子權重也相應較高。激光里程計因子通過當前幀點云與局部子關鍵幀地圖的匹配提供相鄰位姿間的相對變換約束殘差定義為測量值與預測值之間的李代數差異。GPS因子提供絕對位置觀測其協方差矩陣根據環境動態調整——開闊環境下協方差小、權重大城市峽谷或樹林中協方差大、權重降低。回環因子在檢測到回環時提供閉環約束其數學形式與激光里程計因子相同但跨越的時間跨度更大是消除長期漂移的關鍵。這四種因子共同構成了一個全局聯合優化的網絡。當新的關鍵幀加入時系統使用iSAM2增量優化算法更新整個因子圖而非重新求解全部變量。二、IMU預積分橋接異頻傳感器的數學巧思IMU預積分是LIO-SAM實現高效緊耦合的核心技術之一。IMU頻率高達100-1000Hz激光雷達僅10-20Hz若在每個IMU時刻都引入狀態變量優化問題規模將急劇膨脹。預積分的精妙之處在于將兩個關鍵幀之間的所有IMU測量值積分成一個相對運動增量ΔR, Δv, Δp這個增量只與零偏有關而與兩幀之間的絕對狀態無關。因此當優化過程中IMU零偏被微調時無需重新積分所有原始IMU數據只需根據零偏變化對預積分量做一階近似修正即可。其離散形式的預積分計算公式為ΔR_ij Π Exp((ω_meas - b_g) × Δt)Δv_ij Σ ΔR_ik × (a_meas - b_a) × ΔtΔp_ij Σ [Δv_ik × Δt 0.5 × ΔR_ik × (a_meas - b_a) × Δt2]其中b_g和b_a分別是陀螺儀和加速度計的零偏。LIO-SAM通過imuPreintegration.cpp中的gtsam::PreintegratedImuMeasurements類實現這一功能每次新的IMU數據到達時調用integrateMeasurement()更新預積分狀態為激光雷達里程計優化提供初始位姿估計。預積分后的IMU因子進入因子圖與激光里程計因子形成交替優化閉環IMU預積分為激光里程計提供初值激光里程計的解反過來用于估計IMU的零偏。三、滑動窗口與邊緣化實時性與全局一致性的平衡術LIO-SAM對LOAM的另一個關鍵改進在于放棄了全局體素地圖匹配轉而采用局部滑動窗口策略。系統不維護整個全局地圖而是只保留固定數量的最新子關鍵幀構成局部點云地圖新關鍵幀僅與這些先驗子關鍵幀集合進行配準。滑動窗口需要妥善處理“邊緣化”問題——當舊關鍵幀被移出窗口時不能簡單地丟棄其信息而是將其轉化為先驗約束保留在優化問題中。LIO-SAM使用GTSAM庫實現的貝葉斯樹增量優化通過邊緣化操作將被移出窗口的變量折疊為先驗因子既控制了計算復雜度又保留了歷史信息對當前狀態的影響。這一設計的工程價值體現在計算負載的顯著降低LOAM在大規模場景中因全局體素地圖稠密化而效率驟降LIO-SAM的局部滑動窗口則保證了計算開銷始終穩定在可接受范圍內。四、從LIO-SAM到LVI-SAM因子圖架構的可擴展性LIO-SAM之所以能夠成為更復雜融合系統的基礎根源在于其因子圖架構的天然可擴展性。LVI-SAM正是這一能力的直接體現。該框架構建在因子圖之上由兩個子系統組成視覺慣導系統VIS和激光慣導系統LIS兩者以緊耦合方式協同運作。VIS以VINS-Mono為基線LIS則基于LIO-SAM構建。VIS利用激光雷達測量為視覺特征提供深度信息來提升精度LIS則利用VIS的估計值作為幀匹配的初值。兩個子系統最核心的協同設計在于互初始化VIS的初始化需要求解高度非線性問題對初始運動和IMU參數敏感LIS先對系統狀態和IMU零偏進行初始化再插值到圖像關鍵幀作為VIS初始化的初值大幅提升了初始化的魯棒性。視覺回環檢測的結果也被傳遞給LIS進行全局優化。這種雙系統互備份的架構使得LVI-SAM在視覺或激光任一傳感器失效時仍能繼續工作——極大提升了系統在無紋理場景或光照劇烈變化環境中的魯棒性。LVI-SAM的整個框架構建在因子圖之上正是LIO-SAM所奠定的圖優化基礎使得這種多源傳感器的靈活融合成為可能。結語從LOAM到LIO-SAM再到LVI-SAM這條技術演進脈絡清晰地展示了因子圖架構如何改變了激光SLAM的游戲規則。LOAM以“分治”策略奠定了實時性與精度的平衡基礎但其鏈式遞推結構注定難以整合多源異構信息。LIO-SAM通過因子圖將激光雷達、IMU、GPS、回環檢測統一為全局優化問題以IMU預積分實現了異頻傳感器的深度融合以滑動窗口和邊緣化策略兼顧了實時性與全局一致性。當LVI-SAM在此基礎上引入視覺傳感器時因子圖架構的可擴展性再次得到驗證——多源融合不再是“打補丁”式的疊加而是同一數學框架下的自然延伸。對于SLAM研究者而言LOAM與LIO-SAM的分野不僅是算法效率的躍升更是一種思維范式的轉變從逐幀遞推的“鏈式思維”走向全局一致的“網狀思維”這一轉變正在被越來越多的大規模場景定位、多傳感器融合和具身智能導航應用所驗證