? ? ? ? 這節在進行講解SLAM中一個重要概念,零空間,講它有啥用呢?因為SLAM中零空間的存在,才需要FEJ或固定約束存在,本節內容不屬于2D圖優化獨有,先看看什么是零空間概念;零空間是一個核心概念,它深刻揭示了SLAM問題的內在結構和優化過程中的關鍵特性。簡單來說,零空間指的是對系統狀態(機器人的位姿和地圖點的位置)進行某種整體變換時,不會改變傳感器觀測值(例如圖像特征點重投影誤差、激光點云匹配誤差)的變換方向所構成的空間。
一、核心概念解析
-
不可觀測的自由度:
? ? SLAM的目標是同時估計機器人的運動軌跡(位姿序列)和環境地圖(點云或特征點位置)。然而,僅憑機器人自身的運動傳感器(IMU、輪速計)和外部環境觀測傳感器(相機、激光雷達)的相對測量,無法確定整個系統在全局坐標系中的絕對位置和朝向。想象一下:你閉著眼睛在一個未知的房間里走動,通過觸摸墻壁和家具來構建房間地圖并確定自己的位置。你構建的地圖和你的路徑在房間內的絕對位置和朝向是未知的(你不知道地圖的“北”在哪里,也不知道起點在地球上的經緯度),但地圖本身的結構和你路徑的相對形狀是正確的。 -
數學表達:
? ? ? 大多數SLAM后端(無論是基于濾波的如EKF-SLAM,還是基于優化的如Bundle Adjustment)最終都歸結為一個最小二乘問題:最小化預測的觀測值與實際觀測值之間的誤差。目標函數通常寫作:?min Σ ||h?(X) - z?||2
,其中?X
?是整個狀態向量(包含所有位姿和地圖點),h?(X)
?是基于狀態預測的第?i
?個觀測值,z?
?是實際的第?i
?個觀測值。求解通常需要計算目標函數關于狀態?X
?的導數(雅可比矩陣?J
)和海森矩陣近似?H ≈ J?J
。零空間就體現在海森矩陣?H
?上:H
?是一個對稱半正定矩陣,但它不是滿秩的。H
?存在特征值為零(或非常接近零)的特征向量。這些特征向量對應的方向,就是系統的零空間方向。 -
零空間變換:? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??? ??假設存在一個變換?
T
(例如,對整個地圖和所有機器人位姿同時施加一個剛體變換——平移和旋轉)。如果對這個變換后的狀態?T(X)
?進行觀測預測?h?(T(X))
,其結果與對原始狀態?X
?的觀測預測?h?(X)
?完全相等,那么?T
?所對應的增量方向?ΔX
?就位于海森矩陣?H
?的零空間中。數學上:?J * ΔX = 0?
這意味著在這個方向?ΔX
?上移動狀態,觀測誤差的一階變化為零。H * ΔX = J?J * ΔX = J? * (J * ΔX) = J? * 0 = 0
-
維度:? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?在三維空間?中,一個自由漂浮的SLAM系統(沒有固定到全局坐標系的錨點)的零空間維度是?6:3個自由度:全局旋轉 (
roll
,?pitch
,?yaw
);3個自由度:全局平移 (x
,?y
,?z
)。在?二維空間?中,零空間維度是?3:1個自由度:全局旋轉 (yaw
);2個自由度:全局平移 (x
,?y
)。
零空間帶來的影響和挑戰
-
解的不唯一性:
? ? ? ?由于存在零空間,最小二乘問題有無數個解。所有這些解只相差一個全局的剛體變換(零空間變換)。從觀測誤差的角度看,它們都是等價的“最優解”。后果:?如果優化算法不加約束,理論上最終解會漂移在零空間定義的流形上。數值計算中,由于噪聲和線性化誤差,解可能會在這個流形上緩慢漂移。 -
數值不穩定性:
? ? ? ? 海森矩陣?H
?是奇異的(或病態的,條件數很大),因為它有零特征值。這使得在求解線性方程?H * ΔX = -b
?(高斯-牛頓/列文伯格-馬夸爾特算法的核心步驟) 時變得困難。后果:?直接求逆或使用不穩定的線性求解器會導致數值溢出、解振蕩或不收斂。
實踐中如何處理零空間
SLAM算法必須顯式或隱式地處理零空間問題:
-
固定先驗(錨定):最常見的方法。?固定第一個(或前幾個)機器人位姿的6個自由度(或3個在2D中)。例如,設置?
T0 = [I | 0]
?(單位旋轉矩陣,零平移向量)。作用:?這相當于給系統添加了一個絕對的參考系,移除了零空間對應的自由度,使?H
?矩陣變得滿秩正定。優化問題有唯一解(在噪聲范圍內)。實現:?在優化問題中添加一個強先驗因子(Prior Factor),將?T0
?牢牢地“釘”在原點。 -
添加偽觀測值:引入一些微弱的絕對測量(即使現實中不存在),比如一個非常弱的GPS先驗(方差很大),或者一個假設的地圖點固定在原點。作用:?這些微弱的觀測理論上消除了零空間,使?
H
?變得正定(但可能條件數仍然很大)。數值穩定性提高,解被輕微地約束在原點附近,但允許在觀測證據很強時發生輕微漂移。比固定先驗更“軟”一些。 -
使用特殊的線性求解器:使用能夠處理奇異或病態矩陣的求解器,例如:(1)QR 分解:?數值穩定性較好。(2)Cholesky 分解(帶阻尼/正則化):?列文伯格-馬夸爾特算法本身就通過添加?
λI
?項對?H
?進行正則化 (H + λI
),使矩陣正定,間接處理了零空間問題。λ
?控制阻尼強度。(3)SVD(奇異值分解):?可以顯式識別并丟棄(或阻尼)對應于零奇異值(零空間)的方向。作用:?這些方法不直接移除零空間自由度,而是在求解步驟中穩健地處理奇異性,找到一個有效的解(通常對應于某種最小范數解)。 -
流形優化:機器人位姿(旋轉)本身存在于非歐幾里得空間(流形)上(如SO(3)/SE(3))。現代優化庫(g2o, Ceres Solver, GTSAM)通常直接在流形上進行狀態更新。作用:?這確保了位姿在優化過程中始終保持在有效的流形上,提高了數值穩定性和收斂性。雖然不直接解決零空間問題,但它是處理旋轉相關數值問題的必要基礎。
-
忽略(在特定算法中):在某些基于濾波的方法(如EKF)中,初始協方差矩陣可能設置得足夠大以包含零空間不確定性。隨著過程噪聲和觀測更新,協方差矩陣中與零空間對應的部分可能會增長(如果沒有任何絕對觀測),但算法通常仍能給出相對一致的軌跡和地圖(協方差反映了這種全局不確定性)。然而,數值問題仍然需要小心處理。
為什么零空間不影響SLAM的核心功能
? ? ? ?關鍵在于?相對關系:(1)雖然整個軌跡和地圖的絕對位置和朝向(零空間)無法確定,但軌跡的形狀(機器人從一個位姿移動到下一個位姿的相對變換)和地圖的結構(點與點之間的相對位置)是唯一確定的(在觀測噪聲范圍內)。(2)SLAM的核心目標——讓機器人在自己構建的地圖中實現定位(知道自己在哪)和導航(知道怎么去目的地)——完全依賴于這些精確的相對關系。絕對的經緯度坐標對于機器人在其自身地圖中行動通常不是必需的(除非需要與外部絕對坐標系對齊)。
總結:(1)零空間是SLAM問題的固有屬性:?它源于僅依靠相對測量無法確定全局參考系(6個自由度);(2)體現為奇異性:?優化問題的海森矩陣?H
?不滿秩,存在特征值為零的方向(零空間變換方向);(3)導致解不唯一和數值困難:?解在零空間定義的流形上不唯一,數值求解不穩定。(4)必須處理:?常用方法包括固定第一個位姿(錨定)、添加弱先驗、使用魯棒的線性求解器(SVD, 帶阻尼的Cholesky)以及在流形上進行優化。(5)不影響核心功能:?SLAM提供的精確的相對軌跡和地圖結構足以支持定位和導航。
二、SLAM 中的零空間(Null Space)數學解釋
? ? ? ? 在 SLAM(Simultaneous Localization and Mapping)問題中,零空間(Null Space)描述了系統狀態(機器人位姿和地圖點)的某些變換不會影響觀測誤差(如重投影誤差、點云匹配誤差)。這些變換對應的方向在優化問題的 Hessian 矩陣中是?零特征值方向,導致系統無法唯一確定全局參考系。
1. SLAM優化的數學模型
1.1 線性化與Gauss-Newton優化
2.1 零空間的數學定義
2.1 零空間(Null Space)的定義
2.2 SLAM 零空間的物理意義
3.?Hessian矩陣的奇異性
4.?數學示例
-
零空間是 SLAM 固有的自由度,對應全局剛體變換(平移+旋轉)。
-
Hessian 矩陣奇異,導致優化問題有無窮多解(相差一個全局變換)。
-
解決方法:
-
固定先驗(Gauge Fixing)。
-
使用偽逆或阻尼(LM 方法)。
-
流形優化(確保位姿在?SE(3) 上更新)。
-
?數學上,零空間的存在反映了 SLAM 問題的?相對觀測無法確定絕對參考系,但?不影響內部一致性(軌跡和地圖的相對關系仍然正確)。
?
則
? ? ? ?在 SLAM(Simultaneous Localization and Mapping)中,觀測模型?hij(Ti,pj)hij?(Ti?,pj?)?通常描述的是?相對測量(如相機觀測到的地圖點像素坐標、激光雷達匹配的點云距離等)。這些測量僅依賴于?機器人與環境之間的相對幾何關系,而不是它們的絕對全局位置。
三、FEJ的數學解釋
? ? ? ?用數學來解釋 FEJ 的核心思想及其解決的問題;關鍵在于理解?線性化點不一致如何破壞系統的零空間(不可觀自由度)?。