0 說點兒什么
半個月前利用TUM的測試數(shù)據(jù)集實現(xiàn)了一個簡單的SLAM前段視覺里程計，對前端算法的實現(xiàn)過程理解得比較清楚旷痕，于是想嘗試一下SLAM后端的算法。經(jīng)過一周的學(xué)習(xí)和編程顽冶，感覺對后端的算法可以理解欺抗，但沒有一個比較清晰的思路邏輯，因此在這兒做一個思路的整理强重，內(nèi)容會隨著理解的深入逐漸填充绞呈。
1 SLAM后端
SLAM前端完成了一個短時間內(nèi)機器人移動和位姿估計的任務(wù)。但是這種估計是基于短時間內(nèi)甚至是僅僅兩幀之間的估計间景，對于累積的誤差和較長時間尺度下的較優(yōu)位置缺少處理能力佃声。因此，缺少后端的SLAM會在精度上出現(xiàn)比較大的問題倘要。
如下圖所示圾亏，每一次的位姿估計和路標點的估計都會存在些許誤差，應(yīng)該也需要存在一些算法封拧，能夠通過估計和預(yù)測來消減這種誤差志鹃，使對運動狀態(tài)和路標點的估計在即使是有噪聲的情況下可以更加準確。這便是后端在SLAM算法中的作用泽西，即如何使用獲得的數(shù)據(jù)去更好的估計運動和狀態(tài)曹铃。
下面通過不同的算法的演進過程來整理針對復(fù)雜運動和狀態(tài)估計的思路：
2 系統(tǒng)的狀態(tài)估計
2.1 運動方程與觀測方程
在SLAM的過程中，系統(tǒng)可以由運動方程和觀測方程來進行描述捧杉。對于
到
的時間段內(nèi)陕见，有位姿
到
并且有路標
上式的意義可以簡單解釋一下味抖，對于運動方程來說：這一時刻的位姿可由前一時刻的位姿與輸入量按運動模型fx來估計淳玩，當(dāng)然還需要加上運動的輸入噪聲的影響；對于觀測方程來說非竿，觀測數(shù)據(jù)由路標和機器人的位姿按觀測模型hx來決定蜕着，此時也需要加入觀測噪聲的影響。
2.2 優(yōu)化問題的提出
當(dāng)然，對于上式來說承匣，針對于具體的系統(tǒng)和傳感器蓖乘，上式是可以給出不同的具體形式的。而且對于SLAM系統(tǒng)尤其是vSLAM來說韧骗，因為特征點眾多嘉抒，在實際中觀測方程會明顯多于運動方程的數(shù)量。當(dāng)理解了上式后袍暴，我們的目標就很明確了：設(shè)計一種方式些侍，從已知的運動輸入數(shù)據(jù)u和觀測數(shù)據(jù)z來確定狀態(tài)量x和y的分布。其也可以說政模，當(dāng)我們存在一些運動數(shù)據(jù)和觀測數(shù)據(jù)的時候岗宣，我們?nèi)绾稳ス烙嫚顟B(tài)量的分布。
針對于具體的問題淋样，解決方案就有了不同的分支：根據(jù)fx與hx的形式可分為線性與非線性問題耗式；根據(jù)噪聲w和v的分布可分為高斯和非高斯問題。為了解決不同的問題趁猴，就有了題目中所說的不同方法：卡爾曼濾波刊咳、擴展卡爾曼濾波、粒子濾波儡司、非線性優(yōu)化等等娱挨。為了理清解決問題的思路，下面從最簡單的線性高斯問題開始一步一步來解決這個問題捕犬。
