【泡泡機器人SLAM原創專欄-滑動窗算法】: Sliding Window Filter for SLAM

這篇專題的idea主要來源於2006年Gabe Sibley的論文A Sliding Window Filter for SLAM，此論文詳細說明了SLAM後端優化的稀疏性的緣由，並提出了應用於slam中Sliding window filter算法，在進入主題之前，小編先向大家安利一波。

接下來，小編就給大家分享一下讀了論文之後的心得。老生常談地，先簡單說明一下slam這個問題的數學模型，機器人在運動過程中，通常會記錄大量pose，以及landmark,我們用x_p表達pose，用x_m表達landmark，那麼如果在運動過程中，有m個pose，n個landmark，整個向量可以寫成這樣的形式：

對於空間中的三維運動，一般pose是一個6維的向量(位置和姿態)，landmark是一個三維的向量(位置)，在這樣一次運動過程中，由於我們能獲得大量的pose和landmark，而且他們總是充滿了一些聯繫，因此，接下來的任務，就是對這樣的聯繫進行數學建模

Process Model

首先我們要建立pose之間的聯繫，一個簡單的方法自然就是使用運動學公式，我們可以通過慣性傳感器(IMU)測量到加速度(acceleration)和角速度(angular velocity),這樣通過積分以及給定的上一時刻的pose和速度(velocity)，直接就可以算到當前的pose了，當然，其實也可以建立其他很複雜的關係，但是我們總是希望這個關係是馬爾科夫性的，也就是如果知道了上一時刻的pose，那麼當前的pose和上一時刻之前的pose信息是相互獨立的，或者說可以認為上一時刻的pose已經包含了之前pose的所有信息，這樣的模型也許並不那麼完美，比如說傳感器隨著時間精度有所變化等，可能得不到那麼完整的信息，導致模型不滿足馬爾科夫性，這是完全有可能的，但是我們也這樣去近似了，就認為當前狀態僅僅之和前一時刻的狀態有關，和更前面的狀態沒有關係，由此，我們有這樣一個抽象的模型：

其中u代表輸入，對於速度加速度模型可以理解為速度，如果建立了動力學模型，完全可以理解為力和力矩等，是一個給定的具體值，w代表噪聲(均值為0，方差為Q)，如果我們把每個時刻的pose方程拼起來，那麼我們就愉快地得到了一個估計方程組(表達式)：

由此，我們可以通過這個方程組，寫出整個x_p的分布，由於噪聲是滿足高斯分布的，所以整個x_p的分布也滿足高斯分布，如下：

Sensor Model
接下來讓我們建立landmark和pose之間的聯繫，可以想像的是，一個pose會對多個landmark進行觀測，同時同一個landmark可能被不同的pose觀測到，而且由於傳感器的不同，觀測方程可能千變萬化，近年火起來的視覺slam，就是把雷射傳感器換成了相機，用像素代表觀測值，從而想方設法得到landmark以及pose的估計，一個更加general的模型是這樣的一個方程：

這個代表的是在j位置對i觀測，z是觀測值，h是觀測函數，v是滿足均值為0方差為R的噪聲，你可以想像，每個pose都有大量觀測，如此多的觀測湊在一起，拼成了一個無比龐大的函數：

同樣的，由於噪聲滿足高斯分布，我們也可以寫出這個概率密度函數：

Model

有了這些玩意兒，我們是時候建一波把他們統一在一起的模型了。

首先，我們有一些先驗信息，比如說你大概猜了一下，機器人都有第一個pose在哪個地方，沿途看到了哪些路標，同時有一個不確信度，整個信息滿足高斯分布，如下：

其中Pi表示信息矩陣，那麼概率密度如下：

有了這些個概率，我們可以寫一個聯合概率密度：

根據貝葉斯公式，可以表示後驗概率為：

注意到分母和x無關，因此，可以寫出這樣一個公式：

那麼這個時候，我們定義一個函數g:

那麼我們用最大似然估計，可以得到一個能量方程(g2o上面那個):

於是，這個時候應該愉快地高斯牛頓了。具體推導我就不做了，最後可以得到一個類似這樣的方程：

然而，這個不是重點，重要的是對hessian的認識，請看下圖：

看起來是不是很酷？左邊第一個是雅可比矩陣，有顏色部分是有數據的，其他都是0，左邊第二塊是整個hessian矩陣的長相，它正好等於傳感器給你的(觀測模型)+運動過程中建立的(運動模型)+先驗信息。
為啥會長這個樣子呢？其實很簡單，注意觀測模型，其實正好是稀疏BA的矩陣，它反映了pose和point之間的連接，第二個反映了pose和pose之間的連接(可以看到這個塊塊比第一個左上角的塊塊大，其實是把連接放進去了)，第三個反映了對地圖的未知性(看右下角，認為point之間可能是有關聯的)，以及對第一幀的預測，還有地圖的預測，具體公式推導，請自行進行，並不難。

The Sliding Window Filter

我想，最後是時候上算法了，講了這麼多，我想大家對slam問題的數學模型也有一定的了解了，但是其實有一個比較大的問題，即大規模運動和觀測總會跪，什麼意思呢，雖然slam有很好的稀疏性，有強大的g2o作為後端優化的神器，但是數據總不能沒有限制地增大增大再增大，所以，這個時候，如果我們有一個將計算複雜程度限制甚至固定的方法，這樣是不是很酷呢？

Marginalization

這個算法的核心就是邊緣化了，什麼是邊緣化呢，簡單說，邊緣化就是利用shur補減少優化參數，從而提升計算性能。shur補是啥呢？是時候上一波維基百科：

以上，均為維基百科對舒爾補的解釋，實際上，使用shur補，就是計算了餘留參數在準備remove的參數下的條件概率。所以說，每marg一次，就會代入一些交叉項信息，請看下圖，這是論文上的原圖

我不想去翻譯著些英文，就大致說明一下是怎麼回事

marg了最老的一幀，那麼你需要在首先在這一幀上看到的landmark之間建立起聯繫，並且需要建立和被marg的那一幀上看到的landmark與那一幀聯繫的所有幀之間的聯繫(由於有馬爾可夫性，所以一般就是下一幀和上一幀之間的聯繫)，最後可以直接marg掉點

下面那個網格是從動態貝葉斯網的觀點看這個原理，我不想多做說明，至於marg的具體操作，其實就是做一次舒爾補罷了。這就是小編這次想給大家分享的所有內容，希望大家喜歡！！

【版權聲明】泡泡機器人SLAM的所有文章全部由泡泡機器人的成員花費大量心血製作而成的原創內容，希望大家珍惜我們的勞動成果，轉載請務必註明出自【泡泡機器人SLAM】微信公眾號，否則侵權必究！同時，我們也歡迎各位轉載到自己的朋友圈，讓更多的人能否進入到SLAM這個領域中，讓我們共同為推進中國的SLAM事業而努力！

【注】商業轉載請聯繫劉富強（[e-mail]）進行授權。普通個人轉載，請保留版權聲明，並且在文章下方放上「泡泡機器人SLAM」微信公眾帳號的二維碼即可。