如何處理個(gè)別傳感器的退化

為了解決復(fù)雜環(huán)境中的挑戰(zhàn)并提高定位的魯棒性和環(huán)境感知能力，越來越多的研究集中于融合多種傳感器的SLAM系統(tǒng)。這些系統(tǒng)通常集成了多種傳感器，如LiDAR、視覺、RGB-D相機(jī)和IMU，代表性的系統(tǒng)包括R3LIVE、FAST-LIVO、LVIO-Fusion、RIVER等。

現(xiàn)有關(guān)于卡爾曼濾波器的研究主要集中在多傳感器信息融合方面，這些研究提出了使用擴(kuò)展卡爾曼濾波器（EKF）和自適應(yīng)模糊邏輯系統(tǒng)來融合里程計(jì)和聲納信號(hào)的方案，其核心思想是自適應(yīng)調(diào)整增益。然而，這些工作主要集中于如何利用和融合來自多傳感器的信息，而未考慮是否有必要融合或是否需要融合所有可用數(shù)據(jù)。在非退化條件下，當(dāng)高精度數(shù)據(jù)與中等精度數(shù)據(jù)融合時(shí)，精度的提升往往是微小的，甚至可能是負(fù)面的，同時(shí)還會(huì)增加額外的計(jì)算開銷。我們將這種對(duì)所有測(cè)量數(shù)據(jù)不加區(qū)分進(jìn)行融合的方法稱為“全包含”（“all-in”）方法。這種方法面臨以下兩個(gè)主要問題：

1. 首先，“全包含”方法處理的信息量比單傳感器SLAM大得多，通常導(dǎo)致計(jì)算負(fù)擔(dān)顯著增加，影響實(shí)時(shí)性能。
2. 其次，盡管該方法結(jié)合了各種傳感器的優(yōu)勢(shì)，但也引入了其劣勢(shì)。例如，視覺SLAM易受運(yùn)動(dòng)模糊、劇烈光照變化和動(dòng)態(tài)物體的影響，從而產(chǎn)生較大的誤差，甚至導(dǎo)致系統(tǒng)失效。

因此，通過探討何時(shí)以及如何選擇性地融合多傳感器信息，以LiDAR與視覺融合SLAM系統(tǒng)為例，解決上述兩個(gè)問題。在多傳感器SLAM融合中，單一傳感器易出現(xiàn)定位退化，而整合不同類型的傳感器可以提供額外的約束，防止SLAM退化。

• 對(duì)于“何時(shí)”融合，我們的解決方案是僅在更為精確和魯棒的LiDAR SLAM子系統(tǒng)表現(xiàn)出退化時(shí)，才引入視覺信息，否則僅依賴LiDAR子系統(tǒng)進(jìn)行位姿估計(jì)。
• 關(guān)于“如何”融合，如果直接融合所有信息，視覺子系統(tǒng)的較低精度可能會(huì)降低LiDAR系統(tǒng)的整體精度。因此，我們的解決方案是僅在LiDAR子系統(tǒng)退化的方向上融合視覺信息。
• 解決“何時(shí)”問題需要準(zhǔn)確判斷LiDAR SLAM是否發(fā)生退化?，F(xiàn)有方法未考慮點(diǎn)面約束中旋轉(zhuǎn)與平移的高耦合性，導(dǎo)致退化方向的評(píng)估不準(zhǔn)確。

Selective Kalman Filter(選擇性卡爾曼濾波器)[1]的解決方案通過準(zhǔn)確判斷LiDAR SLAM退化的方向，僅在這些退化方向上引入視覺觀測(cè)數(shù)據(jù)。相比之下，LION的融合方法在LiDAR退化時(shí)停止使用LiDAR里程計(jì)，轉(zhuǎn)而依賴其他里程計(jì)信息。這種方法較為粗糙，因?yàn)樵诜峭嘶较蛏弦蕾囕^低精度和魯棒性的傳感器信息可能會(huì)降低系統(tǒng)性能。Zhang、Hinduja和X-ICP的方法主張僅在退化方向融合其他傳感器信息，這被我們認(rèn)為是優(yōu)化的策略。然而，這些優(yōu)化方法基于優(yōu)化框架，未能在濾波框架中精確識(shí)別退化的具體方向，可能導(dǎo)致實(shí)際退化方向未被充分約束。

具體貢獻(xiàn)包括：

• 提出了一種新型的多傳感器融合方法——選擇性卡爾曼濾波器，設(shè)計(jì)用于解決數(shù)據(jù)“何時(shí)”與“如何”選擇性融合的問題。這種方法僅在LiDAR數(shù)據(jù)退化時(shí)引入必要的視覺信息，大幅提升實(shí)時(shí)性能，同時(shí)確保魯棒性和精度。
• 提出了一種創(chuàng)新高效的LiDAR里程計(jì)退化檢測(cè)方法，可同時(shí)確定退化方向。該方法充分考慮了旋轉(zhuǎn)與平移約束之間的耦合關(guān)系。
• 通過實(shí)驗(yàn)驗(yàn)證了所提退化檢測(cè)方法的精確性以及選擇性卡爾曼濾波器的效率。
• 開源了代碼以促進(jìn)社區(qū)參與和廣泛應(yīng)用。

系統(tǒng)概述

首先介紹LiDAR-慣性-視覺SLAM的“全包含”方法，該方法由FAST-LIVO和R3LIVE等方法所代表。在沒有視覺或LiDAR測(cè)量數(shù)據(jù)的情況下，類似于FAST-LIO2，系統(tǒng)利用IMU的加速度和角速度進(jìn)行狀態(tài)傳播，作為卡爾曼濾波的預(yù)測(cè)步驟。當(dāng)LiDAR或視覺測(cè)量數(shù)據(jù)到達(dá)時(shí)，系統(tǒng)在卡爾曼濾波中執(zhí)行數(shù)據(jù)融合更新。

據(jù)我們所知，在沒有退化的情況下，與視覺-慣性SLAM（VIO）相比，LiDAR-慣性里程計(jì)（LIO）系統(tǒng)通常表現(xiàn)出更高的精度和更強(qiáng)的魯棒性。在將選擇性卡爾曼濾波器集成到LiDAR-慣性-視覺里程計(jì)（LIVO）系統(tǒng)中時(shí)，我們優(yōu)先將LIO子系統(tǒng)作為核心，將VIO子系統(tǒng)作為輔助。系統(tǒng)的工作流程如圖2所示。與“全包含”方法不同，我們的方法基于LiDAR測(cè)量進(jìn)行退化檢測(cè)與分析：

• 如果LIO子系統(tǒng)未發(fā)生退化，我們選擇不融合視覺測(cè)量數(shù)據(jù)。這種方法有助于避免因視覺問題（如運(yùn)動(dòng)模糊、過度曝光和錯(cuò)誤特征匹配）引入的誤差。此外，它還減少了計(jì)算負(fù)擔(dān)，從而提高了實(shí)時(shí)性能。

• 如果LIO子系統(tǒng)發(fā)生退化，為了盡量減少引入的視覺信息并防止?fàn)顟B(tài)估計(jì)惡化，我們分析LiDAR的退化方向，然后選擇性地融合與此退化方向相關(guān)的視覺數(shù)據(jù)，并舍棄與非退化方向相關(guān)的數(shù)據(jù)。這有助于防止LiDAR的非退化維度的精度下降。

我們將這種選擇性更新信息融合的方法稱為“選擇性卡爾曼濾波器”（Selective Kalman Filter）。

LiDAR SLAM中的退化檢測(cè)

首先，我們通過加權(quán)線性最小二乘法對(duì)LiDAR測(cè)量方程(2)進(jìn)行變換，得到以下形式：

其中，為狀態(tài)的估計(jì)值。為了簡化表示，我們做出以下定義：

因此，上述方程可以簡化為：

由于實(shí)際場(chǎng)景中的噪聲影響，矩陣通常為滿秩矩陣，因此是一個(gè)6×6的對(duì)稱正定矩陣。

Hinduja方法通過比較的最小特征值與預(yù)設(shè)閾值來檢測(cè)退化。然而需要注意的是，由于的前三維表示旋轉(zhuǎn)，后三維表示平移，其特征值是旋轉(zhuǎn)和平移的耦合值，缺乏明確的物理意義，且單位同時(shí)與旋轉(zhuǎn)和平移相關(guān)。同一特征值分解中的特征值單位不同，因此難以設(shè)定合理的退化檢測(cè)閾值。此外，與這些特征值對(duì)應(yīng)的特征向量也無法代表實(shí)際方向，這使得該方法在實(shí)際部署中往往不切實(shí)際。

為了解決這一問題，LION提出通過分析的對(duì)角元素來確定退化及其方向。首先，將矩陣表示為分塊矩陣的形式：

LION分別對(duì)和進(jìn)行特征值分解：

其中，和為對(duì)角矩陣，包含旋轉(zhuǎn)和平移部分的特征值。然后通過設(shè)定兩個(gè)閾值，評(píng)估旋轉(zhuǎn)和平移是否退化。這種方法有效解決了Hinduja方法中閾值缺乏物理意義的問題，但其忽略了耦合項(xiàng)和，從而忽視了旋轉(zhuǎn)和平移約束之間的相互影響。這種忽略導(dǎo)致在大多數(shù)情況下，退化程度及方向的估計(jì)不準(zhǔn)確。

例如，如圖3(a)所示，根據(jù)LION的觀點(diǎn)，y方向的估計(jì)僅依賴于點(diǎn)A在y方向的點(diǎn)面殘差雅可比，導(dǎo)致y方向估計(jì)較差（如所示）。但實(shí)際上，點(diǎn)B的點(diǎn)面殘差可以改善旋轉(zhuǎn)的估計(jì)，從而增強(qiáng)y方向的估計(jì)（如所示）?？梢酝茢?，這種考慮了平移和旋轉(zhuǎn)耦合的后者方法，對(duì)于不確定的估計(jì)具有更高的準(zhǔn)確性。

為了解決上述問題，我們提出了一種基于協(xié)方差信息的新型、簡單且具有物理意義的退化檢測(cè)方法。我們首先對(duì)矩陣求逆，得到狀態(tài)的協(xié)方差矩陣：

需要注意的是：

從以上公式可以看出，我們的方法與LION使用局部Hessian矩陣的方法不同。我們的方法考慮了旋轉(zhuǎn)與平移之間的耦合關(guān)系。此外，該方法具有實(shí)際的物理意義：協(xié)方差矩陣的對(duì)角塊表示狀態(tài)的方差，而非對(duì)角塊表示不同狀態(tài)之間的相關(guān)性。方差可以作為退化程度的直觀量度，例如，高方差表示該方向上的估計(jì)存在較大的不確定性，從而表明退化。

我們對(duì)和進(jìn)行特征值分解：

其中，和的特征值分別具有物理單位，旋轉(zhuǎn)部分為，平移部分為。對(duì)應(yīng)的特征向量代表退化方向，此時(shí)，和分別為旋轉(zhuǎn)矩陣，能夠?qū)⒃甲鴺?biāo)系旋轉(zhuǎn)到這些特征方向。

最后，我們基于實(shí)際需求和經(jīng)驗(yàn)設(shè)置兩個(gè)方差閾值和。如果，則表示發(fā)生旋轉(zhuǎn)退化，對(duì)應(yīng)的特征向量表示退化方向；平移部分同理。

綜上所述，本節(jié)提出了一種具有實(shí)際物理意義并考慮耦合約束的退化檢測(cè)方法，理論上能夠更準(zhǔn)確地判斷退化程度及方向。

視覺測(cè)量的選擇

我們首先對(duì)視覺測(cè)量方程 (3) 應(yīng)用加權(quán)線性最小二乘法，得到以下形式：

其中， 為狀態(tài) 的估計(jì)值。為了簡化表示，我們定義如下：

因此，方程可以簡化為：

需要注意的是，根據(jù)實(shí)際場(chǎng)景中的噪聲，矩陣 總是可逆的。因此，可以得到估計(jì)狀態(tài) 為：

利用協(xié)方差矩陣的特征向量矩陣 和 ，我們定義一個(gè)新的旋轉(zhuǎn)矩陣 ：

該旋轉(zhuǎn)矩陣能夠?qū)? 的坐標(biāo)系旋轉(zhuǎn)至旋轉(zhuǎn)和平移的主分量方向上：

在已知 LiDAR 子系統(tǒng)退化維度的情況下，我們定義一個(gè)選擇矩陣 ，以選擇視覺測(cè)量方程中與退化方向相關(guān)的信息，從而濾除無關(guān)數(shù)據(jù)：

例如，如果 LiDAR 子系統(tǒng)在一個(gè)平移方向和一個(gè)旋轉(zhuǎn)方向上發(fā)生退化，例如 和 ，我們可以將選擇矩陣 設(shè)置為：

在該矩陣中，設(shè)置為1的對(duì)角元素對(duì)應(yīng)退化狀態(tài)的維度，而設(shè)置為0的對(duì)角元素對(duì)應(yīng)非退化維度。我們將狀態(tài)恢復(fù)到原始坐標(biāo)系，并同時(shí)恢復(fù)原始測(cè)量方程：

為了保持信息矩陣的對(duì)稱性，我們進(jìn)一步處理，得到：

可以簡化為：

其中：

此時(shí)，我們已完成對(duì)視覺數(shù)據(jù)的選擇，僅保留與 LiDAR 退化方向相關(guān)的信息。

選擇性卡爾曼濾波器

首先，我們描述傳統(tǒng)卡爾曼濾波器的形式。對(duì)于視覺子系統(tǒng)，假設(shè)已存在先驗(yàn)估計(jì)，記為 和協(xié)方差 。按照FAST-LIO的方法，卡爾曼增益 的計(jì)算公式為：

得到更新方程為：

將卡爾曼增益 代入更新方程并進(jìn)行簡化，可以得到：

根據(jù)式 (17)，可以進(jìn)一步簡化為：

其中， 是后驗(yàn)估計(jì)狀態(tài)。

對(duì)于后驗(yàn)協(xié)方差 ，其公式為：

將 的表達(dá)式代入并簡化，可以得到：

通過觀察視覺信息的測(cè)量方程在選擇前（式 (18)）和選擇后（式 (26)）的變化，我們只需將 替換為 ，將 替換為 ，即可得到新的選擇性卡爾曼濾波器公式：

備注：在本文中，所有進(jìn)行特征值分解的矩陣均為實(shí)對(duì)稱矩陣，因此其特征值分解和奇異值分解的結(jié)果是等價(jià)的。我們的主要目標(biāo)是確定這些矩陣的主方向，這本質(zhì)上與奇異值相關(guān)的物理含義一致。為了簡化表達(dá)并減少變量的數(shù)量，本文統(tǒng)一采用特征值分解描述整個(gè)過程。

LiDAR-慣性-視覺里程計(jì)

基于出色的LIVO框架R3LIVE，我們集成了選擇性卡爾曼濾波器，構(gòu)建了SKF-Fusion，其系統(tǒng)框架如圖4所示。原始R3LIVE框架包含兩個(gè)并行子系統(tǒng)：LiDAR-慣性里程計(jì)（LIO）和視覺-慣性里程計(jì)（VIO）。本文未重復(fù)描述R3LIVE中的相同內(nèi)容。在此基礎(chǔ)上，我們對(duì)LIO子系統(tǒng)進(jìn)行退化檢測(cè)。如果未檢測(cè)到退化，僅將視覺信息用于上色，同時(shí)保存地圖的顏色信息，以便在退化狀態(tài)下使用。如果檢測(cè)到退化，則在幀與幀之間以及幀與地圖之間進(jìn)行序列更新，專注于退化狀態(tài)的相關(guān)成分。在隨后的實(shí)驗(yàn)中，我們使用R3LIVE數(shù)據(jù)集對(duì)SKF-Fusion進(jìn)行了評(píng)估。

系統(tǒng)工作流程：

1. 未檢測(cè)到退化時(shí)：

• 系統(tǒng)僅使用LiDAR子系統(tǒng)進(jìn)行狀態(tài)估計(jì)和更新。
• 視覺信息僅用于地圖的顏色渲染，無需進(jìn)行視覺狀態(tài)更新。
• 這種方式避免了視覺子系統(tǒng)帶來的計(jì)算開銷，并減少了因視覺特性（如運(yùn)動(dòng)模糊、過度曝光等）引起的誤差。

2. 檢測(cè)到退化時(shí)：

• 系統(tǒng)分析LiDAR的退化方向。
• 針對(duì)退化方向，選擇性地融合視覺觀測(cè)數(shù)據(jù)，僅對(duì)退化狀態(tài)進(jìn)行更新。
• 非退化方向的數(shù)據(jù)被舍棄，從而防止引入不必要的誤差。

通過引入選擇性卡爾曼濾波器，SKF-Fusion實(shí)現(xiàn)了高效的退化處理，顯著提升了系統(tǒng)的魯棒性和實(shí)時(shí)性能。

實(shí)驗(yàn)效果

總結(jié)一下

基于傳統(tǒng)的“全包含”融合SLAM（涉及LiDAR、視覺和IMU），本研究引入了一種基于協(xié)方差信息的創(chuàng)新退化檢測(cè)模塊，以及用于退化狀態(tài)選擇性更新的選擇性卡爾曼濾波器。與現(xiàn)有退化檢測(cè)方法相比，實(shí)驗(yàn)結(jié)果表明，該方法顯著提升了退化檢測(cè)的準(zhǔn)確性，從而提高了定位的精度和魯棒性。此外，與“全包含”方法相比，方法顯著提升了視覺子系統(tǒng)的實(shí)時(shí)性能。需要注意的是，當(dāng)前的閾值選擇未能適應(yīng)不同體素分辨率下的LIO，這為未來的研究提供了方向。

參考

[1] Selective Kalman Filter: When and How to Fuse Multi-Sensor Information to Overcome Degeneracy in SLAM