如果你在SLAM相關(guān)的技術(shù)社區(qū)里逛過應(yīng)該早就見過FAST-LIO 這個名字。這個系列開源之后幾乎成了激光慣性里程計LIO方向最常被拿來當基線的方案跑數(shù)據(jù)集、做畢業(yè)設(shè)計、換傳感器、做真機調(diào)試大家都是先把這套代碼拉起來跑通再去談別的算法。它到底強在哪里為什么能在一眾LIO方案里脫穎而出這篇文章不打算停留在“框架很好用”這種層面而是把FAST-LIO 系列背后幾個真正關(guān)鍵的原理拆開講清楚緊耦合為什么比松耦合穩(wěn)迭代卡爾曼濾波和普通卡爾曼濾波差在哪反向傳播怎么做運動補償以及FAST-LIO2換上的ikd-Tree 到底解決了什么問題。適合誰來讀已經(jīng)會啟動一個LIO節(jié)點、但還沒把內(nèi)部公式和代碼對應(yīng)起來的人正在做相關(guān)方向、想快速吃透核心邏輯的人。也可以作為你調(diào)參之前的一份“避坑地圖”幫你少走很多彎路。1. 為什么 FAST-LIO 系列成了 LIO 圈子的“默認起點”1.1 LIO 到底在解什么題激光慣性里程計簡單說就是同時拿激光雷達點云和IMU數(shù)據(jù)估計傳感器在三維空間里的位置和姿態(tài)并在這個過程中拼出環(huán)境地圖。聽起來和純激光里程計沒什么區(qū)別但加一個IMU事情的性質(zhì)就變了。純激光方案在大多數(shù)時候也能跑但它有三個繞不過去的痛點一是角速度太快或者轉(zhuǎn)彎太猛的時候一幀點云里每個點的采集時間不同點云會“糊”掉二是環(huán)境里如果出現(xiàn)長走廊、空曠廣場這類幾何退化場景激光匹配本身給不出約束軌跡就會慢慢漂走三是初始化階段位姿完全未知純激光面臨“雞生蛋”問題。IMU恰好能從這三個方向上補位它提供高頻的旋轉(zhuǎn)和加速度信息可以把一幀點云按時間戳做運動補償在退化方向上雖然激光約束缺失但IMU的重力矢量和慣性積分能兜住一部分漂移對初始化來說IMU能快速把重力方向找回來給激光一個可用的初始姿態(tài)。但IMU也有自己的毛病它本身帶零偏和噪聲純積分只是幾秒就會漂得離譜。所以LIO的核心任務(wù)不是“用好激光”或者“用好IMU”而是把兩者放在一個狀態(tài)估計框架里讓它們互相糾正。這就是FAST-LIO系列的核心出發(fā)點。1.2 系列升級除了算法還給工程留了什么FAST-LIO系列并不是單個算法而是一條演進線。大致可以分成兩個階段第一階段把“激光配準”和“IMU傳播”放進一個統(tǒng)一的迭代卡爾曼濾波框架用緊耦合的方式解決此前松耦合方案在快速運動下精度下降的問題同時把計算量壓到一個實時可跑的水平第二階段換掉整個地圖管理結(jié)構(gòu)不再依賴提取特征點而是用增量式kd-tree來管理地圖直接拿點云配準。這一步讓系統(tǒng)在保持精度的同時適應(yīng)更多傳感器類型也更容易在嵌入式平臺上長時間運行。很多同學看代碼時會把注意力放在各種濾波公式上實際上換個角度把這套系統(tǒng)看成“傳感器數(shù)據(jù)處理流水線”更容易理解點云進來先做運動補償IMU積分給出預(yù)測激光匹配給出修正修正量再反饋給狀態(tài)地圖增量更新。后面幾章我會按照這條流水線把原理一節(jié)一節(jié)講通。2. 緊耦合核心FAST-LIO 的 IEKF 到底怎么工作2.1 狀態(tài)向量為什么要把外參也塞進去先看一個最容易被忽略的問題狀態(tài)向量里到底裝了什么。FAST-LIO的狀態(tài)量不只是位置、姿態(tài)、速度這三樣它還把陀螺儀零偏、加速度計零偏以及“激光雷達相對IMU的外參旋轉(zhuǎn)和平移”一起放進估計狀態(tài)里。為什么要把外參也塞進去因為在實際安裝中外參很難量得絕對準。哪怕差個一兩度在遠距離點云上造成的匹配誤差也會被放大接著IMU和激光之間的約束關(guān)系就會出現(xiàn)系統(tǒng)性偏差。讓外參作為狀態(tài)變量參與在線估計等于給系統(tǒng)一個“自動校準”的能力。代價是狀態(tài)維度上升常見的實現(xiàn)里會來到18維左右如果加上完整外參估計可能到21維上下。這里有個概念要拎清估計器里的狀態(tài)不是“真值”而是帶協(xié)方差的隨機變量。所以濾波過程本質(zhì)上是在維護“對狀態(tài)的信念”IMU傳播會讓這個信念的方差變大激光觀測會把方差重新壓小。緊耦合的含義也體現(xiàn)在這里——不是先用激光單獨算一次位姿再用IMU積分結(jié)果去融合而是在同一個更新步驟里讓激光殘差直接去修正IMU預(yù)測出來的狀態(tài)。2.2 一次配準迭代里發(fā)生了什么把一次完整的IEKF迭代拆開看大致是以下幾步。第一步是IMU傳播。用陀螺儀和加速度計的讀數(shù)從上一個狀態(tài)預(yù)測下一個狀態(tài)同時更新協(xié)方差。因為IMU頻率通常比激光高很多這一傳播過程會跨多個IMU采樣周期。第二步是殘差構(gòu)建。把一幀點云里的每個有效點通過當前估計出的位姿變換到世界坐標系然后在已有地圖中查找最近鄰點。FAST-LIO里一般不是簡單找單個最近點而是取周圍幾個點擬合一個局部平面然后計算該點到平面的距離作為殘差。第三步是迭代修正。理論上激光殘差和狀態(tài)之間是非線性關(guān)系。普通擴展卡爾曼濾波只做一次線性化如果初值偏差大就容易發(fā)散。FAST-LIO的做法是循環(huán)執(zhí)行“線性化-計算增益-更新狀態(tài)”每次更新后重新計算殘差直到收斂或達到最大迭代次數(shù)。這就是“迭代”兩個字的意思。這個流程和常見的“先ICP配準、再松耦合融合”有本質(zhì)區(qū)別。松耦合方案通常先用ICP求出一個位姿再把它當作觀測值送入濾波器而FAST-LIO讓配準本身直接參與卡爾曼增益的計算相當于“匹配即估計、估計即匹配”。代碼里能看到這樣的設(shè)計迭代更新的過程中點云配準使用的變換矩陣也在同步更新兩者之間沒有明顯邊界。關(guān)鍵還是理解卡爾曼增益在干什么。普通卡爾曼濾波里增益決定“預(yù)測”和“觀測”各自相信多少。IEKF里觀測其實是“當前狀態(tài)預(yù)測下點云到地圖應(yīng)該對齊”這個約束。于是選什么樣的地圖點做殘差、殘差模型夠不夠準直接決定系統(tǒng)能修正到什么程度。2.3 計算效率的關(guān)鍵把求逆從測量空間“挪”到狀態(tài)空間很多剛接觸FAST-LIO的人都會問一個問題一幀點云有幾千個點測量維度幾千起步這個卡爾曼更新怎么算得動這正是FAST-LIO最巧妙的地方。如果直接用常規(guī)公式卡爾曼增益的計算要面對一個“測量維度×測量維度”的矩陣求逆。幾千點就是幾千維矩陣求逆的復(fù)雜度是維度的三次方實時性基本沒戲。FAST-LIO把更新公式改寫了一下核心是下面這個等價形式K (P^{-1} H^T R^{-1} H)^{-1} H^T R^{-1}這里P是預(yù)測協(xié)方差H是觀測模型R是測量噪聲。注意看要逆的矩陣變成了P^{-1} H^T R^{-1} H它的維度等于狀態(tài)維度通常只是十幾或二十幾和測量點數(shù)量無關(guān)。于是計算量從“幾千維矩陣求逆”降到了“二十來維矩陣求逆”一下子變得實時可跑。這個細節(jié)就是FAST-LIO能跑在嵌入式平臺上的關(guān)鍵轉(zhuǎn)折點。也提醒我們一個道理在看這類算法時不要只盯著“精度高不高”計算圖結(jié)構(gòu)往往決定了這套系統(tǒng)能不能落地。工程上真正讓算法可用的往往不是新概念而是把老概念算得足夠便宜。2.4 運動補償把一幀點云按時間“拉直”再往下挖一層就是運動補償。雖然激光雷達每秒出很多點但一幀掃描其實有一個時間跨度。載體在采集期間一直在動如果把這幾十毫秒內(nèi)的所有點都認為是在同一時刻采的點云就會出現(xiàn)拖影。FAST-LIO里補償用的工具就是IMU狀態(tài)。具體做法是在一幀掃描的起點和終點各保留一個狀態(tài)對掃描區(qū)間內(nèi)每一個點根據(jù)它的相對時間戳反推出“從該點采集時刻到幀參考時刻”的位姿變換然后把點云變換過去相當于把一幀“彎”了的點云拉直。偽代碼層面大致是這樣for each point p in scan: t p.time # 該點在掃描內(nèi)的相對時間戳 T_t_to_ref 反向積分(t, 幀參考時刻, IMU狀態(tài)序列) p_corrected T_t_to_ref * p_raw這里最需要注意的問題是“幀參考時刻”到底選在哪個點。有的代碼用掃描開始時刻有的用結(jié)束時刻細節(jié)不同會影響IMU狀態(tài)序列的插值方式。實際調(diào)參時如果發(fā)現(xiàn)點云邊緣總是虛的或者在靜止環(huán)境下都出現(xiàn)“微小抖動”優(yōu)先懷疑運動補償和時間戳參考沒對齊。運動補償做得好不好直接決定配準殘差里有多少是“真誤差”多少是“時間拖影帶來的假誤差”。假誤差會騙過濾波器讓它把噪聲當成運動去修正表現(xiàn)出來就是軌跡漂移和建圖模糊。3. FAST-LIO2 憑什么敢不做特征提取3.1 直接配準每個點都能當“特征”早期很多LIO和LOAM類方案會先做特征提取把點云分成邊緣點和平面點然后只拿這些“有代表性”的點去配準。這樣做的理由是減少計算量、剔除不可靠點。但特征提取本身是個信息損失過程如果場景里紋理少、結(jié)構(gòu)弱提取出來的特征點稀疏匹配質(zhì)量就直線下降。FAST-LIO2走的是另一條路不做顯式特征提取直接對原始點云做配準。每個點只要在有效范圍內(nèi)都可以參與殘差計算。你可能會想幾千個點全參與計算這不是更慢嗎這里的關(guān)鍵在于配準也不是每個點都獨立去最近鄰搜索而是借助地圖組織結(jié)構(gòu)和濾波計算技巧把總體開銷壓了下來。直接配準帶來的收益很有意思弱紋理環(huán)境下比如一面白墻、一條長廊傳統(tǒng)特征提取可能只能找到零星幾個點而直接法依然能從整面墻的表面獲取足夠的約束信息。這解釋了不少用戶的實際體驗——FAST-LIO2在“看起來沒什么特征”的室內(nèi)環(huán)境下反而比需要特征點的方法更穩(wěn)。3.2 ikd-Tree增量地圖查詢的關(guān)鍵既然不做特征提取地圖的“組織方式”就成了頭等大事。配準的本質(zhì)是不斷查詢“當前點附近的已建圖區(qū)域在哪里”。如果地圖里點的規(guī)模一味增長每次查詢都遍歷全圖系統(tǒng)遲早卡死。最樸素的想法是用一個經(jīng)過下采樣的體素地圖先刪掉太密的點再靠哈希去索引。FAST-LIO1就是這么做的體素地圖在中等規(guī)模下表現(xiàn)不錯。但到了更大場景、長時間運行體素地圖的更新代價會逐漸抬高。FAST-LIO2換上的核心結(jié)構(gòu)是增量kd-tree。普通kd-tree是一種靜態(tài)結(jié)構(gòu)建好之后如果要插入新點往往得把整棵樹重建一遍這在實時跑圖時不可接受。ikd-tree的思路是“動態(tài)維護平衡樹”先按常規(guī)kd-tree的規(guī)則建樹插點時只影響局部路徑刪除點時不完全物理刪掉而是先打個“刪除標記”等某個子樹里標記過期的點達到一定比例再觸發(fā)一次局部重建Rebuild。這個過程可以用一個容易理解的類比廚房里不會做完一道菜就把所有碗碟重新擺一遍而是先把要扔的東西放在一個待處理區(qū)等垃圾堆到一定量再集中倒掉并整理臺面。ikd-tree的“增量刪除、延后重建”就是這個邏輯的分層版。由于地圖點總規(guī)模被限制在一個可控范圍內(nèi)配準查詢的耗時能得到很好的保證。這也是FAST-LIO2能長時間建圖跑大場景的底氣之一。3.3 退化場景下的特別價值很多人實測下來有個感覺FAST-LIO2在隧道、走廊這類“退化環(huán)境”里比純激光算法穩(wěn)。這還真不是玄學。退化環(huán)境的意思是激光在某個方向上沒有提供足夠約束。比如一條筆直長廊沿著寬度方向還有點約束但沿著長度方向環(huán)境看起來都一樣激光基本說不出“我走了多遠”。純激光里程計在這個方向上的誤差會越積越大。IMU在這里起了什么作用一方面加速度計感受到的重力方向始終是一個穩(wěn)定約束能防止姿態(tài)在小尺度上隨意漂移另一方面IMU的短時積分在幾秒內(nèi)相對可信相當于在激光約束缺失的方向上“墊”了一塊信息。卡爾曼濾波會按協(xié)方差自動地權(quán)衡激光約束強的方向多信激光激光約束弱的方向多信IMU。這就是緊耦合在退化場景下的核心價值。但別把它神話了。IMU本身有偏置誤差時間一長也會漂。FAST-LIO2能扛住的退化場景周期一般在幾十秒量級如果一條隧道幾公里長中間沒有轉(zhuǎn)角、沒有特殊特征光靠IMU撐還是很吃力。實用做法是外部疊加閉環(huán)檢測或者輪速器等傳感器。4. 把原理落成可跑的系統(tǒng)配置、標定與調(diào)參經(jīng)驗4.1 外參、IMU 參數(shù)和時間軸啟動前先做三件事代碼跑不跑得動頭三件事決定成敗外參對不對、IMU噪聲參數(shù)對不對、時間戳對不對。先說外參。FAST-LIO系列雖然支持在線估計外參但初值必須在合理范圍內(nèi)。如果你把激光和IMU之間的安裝角度寫錯了幾十度在線估計大概率直接發(fā)散。我的建議是先用尺子和簡單標定工具把旋轉(zhuǎn)和平移初值量到誤差不超過幾厘米、幾度的程度再讓系統(tǒng)在線微調(diào)。不要指望在線估計能從零猜出外參。然后是IMU噪聲參數(shù)。這里最容易踩的坑是拿著別人配置文件里的數(shù)字直接抄。不同IMU的白噪聲和隨機游走差別很大哪怕同一型號不同個體、不同采樣率也會有差別。如果噪聲參數(shù)設(shè)得過小濾波器會過于相信IMU實驗結(jié)果就是軌跡跟著IMU漂設(shè)得過大濾波器又說不敢用IMU退化場景下很快完蛋。正規(guī)做法是用Allan方差分析出陀螺儀和加速度計的白噪聲、零偏隨機游走再把單位換算成代碼里的量綱。最后是時間軸。激光點和IMU數(shù)據(jù)必須換算到同一時間基準系。常見傳感器在驅(qū)動里會給點云時間戳但點云輸出時刻和真實采集時刻之間可能有固定延時這個延時差個幾毫秒在高速旋轉(zhuǎn)下就可能讓配準殘差明顯變大。啟動前先看驅(qū)動文檔或者跑靜置實驗觀察點云在靜止時是否有規(guī)律變形來估計時延。4.2 參數(shù)文件里容易被低估的幾個數(shù)字FAST-LIO系列參數(shù)文件里藏著幾個看起來不起眼、實際影響很大的參數(shù)。盲區(qū)距離很多雷達近處一堆雜點特別是墻角、地面反射直接參與配準會污染殘差。設(shè)一個合適的盲區(qū)把 0.5米或更近的點濾掉通常能立刻改善穩(wěn)定性。最大測距范圍遠點誤差大角度誤差在遠處會被放大。設(shè)一個合理的最大范圍比如幾十米比無腦用滿量程更穩(wěn)。地圖下采樣分辨率地圖點太密ikd-tree節(jié)點多查詢變慢太疏配準精度下降。建議從 0.2到0.5米之間試起看實時運行單圈耗時和建圖厚度再定。迭代最大次數(shù)一般2到3次就能收斂過度增加迭代次數(shù)對精度幫助很有限反而拉高CPU。如果殘差一直不降優(yōu)先懷疑殘差模型和外參而不是盲調(diào)迭代次數(shù)。最大迭代間隔/scan間隔這個和雷達幀率相關(guān)不同傳感器差異很大不要照抄默認值。調(diào)參順序上我習慣先固定外參和時間同步再調(diào)濾波噪聲參數(shù)最后才碰地圖相關(guān)的閾值。順序反了很容易出現(xiàn)“改一個參數(shù)好一點改另一個又壞回去”的振蕩局面。4.3 從仿真到實車的工作流一個比較穩(wěn)妥的落地流程是先在公開仿真環(huán)境里跑通再遷移到真實設(shè)備。仿真環(huán)境的好處是外參、真實位姿、噪聲參數(shù)全都是“已知的”你可以用它驗證自己對參數(shù)文件的理解。比如把外參故意寫偏一點觀察軌跡怎么變把IMU噪聲寫大一點觀察退化場景下系統(tǒng)是不是更早發(fā)散。這些實驗?zāi)軒湍憬ⅰ皡?shù)-現(xiàn)象”之間的直覺比直接拿真機瞎試安全得多。遷移到真機之后首次實驗原則只有一條讓運動溫柔一點。先慢速走直線再轉(zhuǎn)小圈再加速轉(zhuǎn)彎。不要一上來就做激烈動作否則系統(tǒng)一旦崩潰你根本看不出是外參問題、時間同步問題還是參數(shù)過激。我見過不少開發(fā)者的調(diào)試過程最后發(fā)現(xiàn)是IMU安裝方向?qū)懛?、點云時間戳固定延遲相差十幾毫秒這類小問題而這在溫柔運動下很容易暴露。真機調(diào)試時建議導(dǎo)出每幀的配準殘差均值、狀態(tài)估計方差、點云匹配內(nèi)點數(shù)量這幾個指標能非常直觀地告訴你系統(tǒng)是不是健康。殘差突然抬升而內(nèi)點數(shù)驟降通常意味著地圖匹配到了錯誤地方方差長期不收斂大概率是IMU參數(shù)或外參沒設(shè)對。5. 常見問題與排查技巧實錄5.1 里程計剛開始正常幾十秒后開始飄這是提問率最高的問題。最常見的原因有四種重力方向初始化沒做好。啟動瞬間如果包含較大加速度估計出的重力方向就偏了之后軌跡會在重力方向上慢慢漂。解決方法是讓系統(tǒng)在靜止狀態(tài)下靜置幾秒等IMU初始化穩(wěn)定再開始運動。加速度計零偏估計被污染。如果初始化期間車身沒靜止濾波會把運動加速度當成零偏吸收后面越跑越偏。外參平移或旋轉(zhuǎn)初值差太多。在線估計雖然能修正一部分但修正范圍有限初值太離譜只會引發(fā)發(fā)散。角速度飽和或者IMU量程不夠。激烈旋轉(zhuǎn)時IMU數(shù)據(jù)削頂積分結(jié)果失真系統(tǒng)會把假運動當成真殘差去配準。排查順序建議先看啟動時IMU加速度模長是不是接近當?shù)刂亓χ翟倏赐鈪⒊踔翟倏催\行軌跡是否在某個方向上有單調(diào)漂移最后再懷疑代碼邏輯。5.2 地圖重影、軌跡跳變多半和時間軸有關(guān)地圖重影和軌跡跳變的問題重點懷疑兩個方向?qū)娱g時間同步和運動畸變補償。如果你用的是支持點級時間戳的雷達要確認點的時間戳單位、起點、方向?qū)Σ粚?。有的雷達把第一個點當作0有的把最后一個點當作0有的時間戳是浮點秒有的是整型微秒。單位差著千倍補償就是災(zāi)難。另外要檢查IMU時間戳和激光時間戳是不是同一個時鐘源。如果雷達有自己的時鐘而未同步毫秒級偏差在高速運動時非常致命。一個非常簡單的判斷方法讓設(shè)備靜止不動如果靜止時點云都出現(xiàn)重復(fù)邊緣那就大概率不是配準問題而是時間戳或運動補償問題。5.3 空曠與長走廊退化場景怎么判斷和繞開在線判斷退化最直接的辦法是看狀態(tài)協(xié)方差在激光更新后的變化。如果某個方向上的方差一直降不下來或者殘差在某個方向上對位姿變化不敏感基本可以判定這個方向退化。實際操作中可以同時觀察點云匹配殘差和IMU置信度。如果殘差均值不高但軌跡在橫向漂移很可能是退化方向缺少約束而不是參數(shù)沒調(diào)好。這時候再調(diào)濾波器參數(shù)意義不大更值得做的是加入輪速約束、引入閉環(huán)檢測、或者調(diào)整運動策略讓載體盡量多轉(zhuǎn)身、多制造一些非退化運動。5.4 問題速查表啟動問題的常見歸因下面這張表是我在幾次調(diào)試過程中整理出來的遇到問題可以先對著表排查一輪?，F(xiàn)象優(yōu)先排查項常見處理手段啟動后立刻飛掉外參初值、IMU方向、時間戳量外參檢查IMU安裝矩陣固定延時補償正常跑一會兒慢慢漂重力初始化、IMU零偏、加速度噪聲靜止啟動重新標定IMU參數(shù)地圖重影運動補償、時間戳單位校正時間戳基準檢查掃描參考時刻高速轉(zhuǎn)向時發(fā)散IMU量程、角速度飽和、外參旋轉(zhuǎn)檢查數(shù)據(jù)是否削頂降低運動速度驗證長走廊橫向漂移退化方向、IMU隨機游走加回環(huán)或輪速調(diào)低IMU噪聲CPU占用異常高ikd-tree規(guī)模、地圖下采樣增大下采樣分辨率檢查刪除區(qū)域設(shè)置這只是一張起點地圖真正排查時還是要結(jié)合殘差曲線和協(xié)方差數(shù)據(jù)去聯(lián)動判斷。時序問題、參數(shù)問題和外參問題常常表現(xiàn)得非常像草率下結(jié)論反而容易浪費時間。最后再分享一個我自己的體會FAST-LIO 系列最大的門檻其實不是公式而是把狀態(tài)變量、時間戳、地圖結(jié)構(gòu)這三條線從代碼里拎出來。你不需要背出每一步矩陣推導(dǎo)但一定要知道IMU傳播之后誰在負責“給預(yù)測糾偏”點云補償用的是哪個時間參考系地圖點又是怎么被動態(tài)維護的。把這幾個問題在代碼里逐個對應(yīng)上絕大多數(shù)調(diào)試問題都能從現(xiàn)象反推回原因。之后再調(diào)參就不是靠碰運氣而是每條參數(shù)都有明確的物理意義了。