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