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