圖 2-1 機器人姿態(tài)和觀測結(jié)果圖

圖形 SLAM 將 SLAM 問題視為圖形結(jié)構(gòu)的非線性優(yōu)化，其中節(jié)點表示機器人的姿態(tài)和不同時間的觀測結(jié)果，邊表示姿態(tài)之間的約束條件。圖 2-1 展示了一個玩具示例，說明了如何借助機器人移動時的姿態(tài)和觀測結(jié)果來構(gòu)建圖形。x_i、x_j 和 x_k 是機器人的姿態(tài)，P₀ 和 P₁ 是攝像頭觀測到的地標(biāo)、物體或點。此處假設(shè)通過 x_i 和 x_j 觀測 P₀，通過 x_j 和 x_k 觀測 P₁。p_nt（其中 n = 0、1，t = i、j、k）定義為通過 x_t 觀測的 P_n 的位置。圖形 SLAM 的目標(biāo)是確定機器人姿態(tài)之間的變換，即 z’_ij 和 z’_jk，使由不同姿態(tài)產(chǎn)生的相同觀測結(jié)果盡可能地重疊。因此，需要確定 x_t（其中 t = i、j、k），使 (p_0i – p_0j)² + (p_1j – p_1k)² 最小。

圖 2-2 圖形 SLAM 流程

圖 2-2 顯示了圖形 SLAM 流程，主要由兩部分組成，即前端和后端。前端處理輸入傳感器數(shù)據(jù)，以估計移動機器人的姿態(tài)以及機器人周圍的明顯特征。前端處理包括以下步驟：

• 數(shù)據(jù)采集：從攝像頭和激光雷達等傳感器連續(xù)捕獲數(shù)據(jù)。捕獲的傳感器數(shù)據(jù)在此處進行預(yù)處理。例如，在數(shù)據(jù)采集期間執(zhí)行 ISP、鏡頭失真校正、立體圖像校正和激光雷達點云運動補償?shù)炔僮鳌Ｔ诠潭〞r間間隔內(nèi)捕獲的數(shù)據(jù)構(gòu)成一幀。
• 特征提取：從每一幀中提取獨特的描述性特征。這些特征可以是圖像中的關(guān)鍵點（視覺 SLAM），也可以是幾何特征（例如邊緣、平面以及掃描的 2D 或 3D 點本身）（激光雷達 SLAM）。
• 特征關(guān)聯(lián)：移動機器人將從不同幀中提取的特征進行關(guān)聯(lián)，以確定哪些特征與環(huán)境中的相同特征相對應(yīng)。
• 姿態(tài)估計：根據(jù)相關(guān)特征，移動機器人估計其位置和方向在兩幀之間的變化，從而估計移動機器人的新姿態(tài)。估計完姿態(tài)，就可以使用相關(guān)特征來更新地圖。

在前端估計的移動機器人姿態(tài)是有誤差的，并且誤差會隨著移動機器人的移動而累積，從而導(dǎo)致巨大的漂移誤差。后端負責(zé)細化估計的姿態(tài)并更新地圖。此細化包括以下步驟：

• 閉環(huán)檢測：此過程可確定以前是否訪問過當(dāng)前位置。要通過圖形優(yōu)化減少漂移誤差，確定訪問過的位置至關(guān)重要。
• 圖形優(yōu)化：圖形保留在圖形 SLAM 中，如圖 2-1 所示。目標(biāo)是更新先前估計的姿態(tài)，使從一個姿態(tài)觀測到的結(jié)果（如特征）與從其他姿態(tài)觀測到的相同結(jié)果盡可能重疊。牛頓法、高斯牛頓法和列文伯格-馬夸爾特 (LM) 優(yōu)化等非線性優(yōu)化技術(shù)均用于此用途。

一般來說，在視覺 SLAM 中，創(chuàng)建的地圖是一組提取的特征及其位置和描述符；在激光雷達 SLAM 中，創(chuàng)建的地圖是一組幾何特征或點本身。在許多用例中，地圖在經(jīng)過后處理后還會以占據(jù)柵格地圖的形式保存，以便于障礙物檢測和路徑規(guī)劃。