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

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

圖 2-2 圖形 SLAM 流程

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

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

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

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

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