我的CSDN：https://blog.csdn.net/liu2015302026/article/details/107997270
Github代碼地址:
https://github.com/liuzhenboo/2D-SLAM-By-Nonlinear-Optimization
1. SLAM問題概率模型
1.1 最大后驗到最小二乘
SLAM問題其實就是一個狀態估計的問題，就是要根據一系列觀測來推測狀態量；一般情況下，我們把SLAM問題建立在概率論的框架下。
說白了，SLAM就是要是解決這樣一類后驗概率問題：
\[ p(x|z) \]
其中\(x\)是系統當前狀態量，\(z\)是與狀態量相關的觀測量；在求出這個條件概率之后，把觀測值\(Z\)帶入，就可以獲得\(p(x|z=Z)\)，這也就是我們需要的分布。
根據概率公式展開：
\[p(x|z=Z)=\frac{p(z=Z|x)p(x)}{p(z=Z)} \]
其中分母是常量，當作是歸一化因子\(\eta\)即可，所以也可寫為：
\[p(x|z=Z)=\eta{p(z=Z|x)p(x)} \]
\(p(z|x)\)表示在\(x\)已知的情況下，\(z\)的概率分布；在求出\(p(z|x)\)之后，把\(z=Z\)帶入，得到\(p(z=Z|x)\)，它也被稱為似然項，它表示在\(x\)取不同值的情況下，\(z=Z\)的概率。
\(p(x)\)也被稱為先驗，也就是在觀測之前\(x\)服從什么分布。這可以通過其他信息源得到，比如GPS，慣導等。如果事前不知道任何先驗分布信息，那么可以將其設為1，表示不相信先驗信息，只根據系統使用的量測來估計。
\[ x_{MAP}= \mathop{argmax}\limits_{x}p(z=Z|x)p(x) \]
因為每次觀測相互獨立，所以上式可以寫為：
\[ x_{MAP}= \mathop{argmax}\limits_{x}\prod_{i}p(z_i=Z_i|x)p(x) \]
取負對數得到：
\[ x_{MAP}=\mathop{argmin}\limits_{x}\left[-\sum_ilogp(z_i=Z_i|x)-logp(x)\right] \]
如果假設觀測\(p(z_i|x)\)服從高斯分布\(N(h_i(x)，\sum_i)\)；先驗\(p(x)\)服從\(N(x_p，\sum_{x})\)那么有：
\[ x_{MAP}=\mathop{argmin}\limits_{x}\sum_{i}||z_i-h_i(x)||^2_{\sum_i}+||x-x_p||^2_{\sum_x} \]
如果沒有先驗知識，那么可以寫為：
\[ x_{MAP}=\mathop{argmin}\limits_{x}\sum_{i}||z_i-h_i(x)||^2_{\sum_i} \]
顯而易見，在SLAM的過程中，不斷有新的殘差項加入到系統，上述最小乘問題不斷擴大。
1.2 非線性優化求解
設當前時刻狀態向量為x，維度為n，我們將殘差表示成向量的形式：
\[ e(x) = \left[\begin{matrix} e_1(x) \\ ... \\ e_m(x) \end{matrix}\right] \]
這里我們使用馬氏范數，損失函數定義為：
\[ E(x) =\frac{1}{2}||e(x)||^2_{\Sigma_e}= \frac{1}{2}{e(x)}^T\Sigma_e^{-1}e(x) \]
記\(J_i(x)=\frac {\partial e_i(x)} {\partial x}\)，那么可以得到：
\[ \frac {\partial e(x)}{\partial x}= J = \left[\begin{matrix} J_1(x) \\ ... \\ J_m(x) \end{matrix}\right] \]
\[ J_i(x) =\left[\begin{matrix} \frac {\partial e_i(x)} {\partial x_1} ... \frac {\partial e_i(x)} {\partial x_n} \end{matrix}\right] \]
對殘差函數泰勒展開，得到一階近似：
\[ e(x+\delta x) \approx e(x)+J\delta x \]
那么帶入損失函數可以近似得到：
\[ E(x+\delta x) = \frac{1}{2}{e(x+\delta x)}^T\Sigma_e^{-1}e(x+\delta x) \\ \approx \frac{1}{2}{e(x)}^T\Sigma _e^{-1}e(x)+{\delta x}^T J^T \Sigma_e^{-1}e(x)+\frac{1}{2}{\delta x}^TJ^T\Sigma_e^{-1}J\delta x \]
所以損失函數就轉換為關於\(\delta x\)的二次函數，並且如果\(J^T\Sigma _e^{-1}J\)正定，那么損失函數有最小值。
對上式關於\(\delta x\)求一階導數，並另其為零，得：
\[ J^T\Sigma_e^{-1}J\delta x = -J^T\Sigma_e^{-1}e(x) \]
也常常寫為
\[ H\delta x = b \]
這樣就可以得到增量\(\delta x\)，這樣一直迭代，就可以不斷優化當前解，直到誤差小於閾值。這種方法被稱為高斯牛頓法。
不過在實際中，求解上述方程需要\(H\)矩陣可逆，而我在編程中，經常遇到\(H\)不可逆的情況，所以實際代碼中我使用了Levenberg-Marquardt法。其是對高斯牛頓法進行了改進，增加了阻尼因子一項，如下所示：
\[ (H+\mu I)\delta x = b \]
這里我並沒有深究迭代中\(\mu\)的動態選取策略，只是單純地給了一個定值0.1。
2. 問題設定
一個機器人佩戴某種雷達傳感器在2D平面內運動，如何通過帶有噪聲的傳感器數據來得到自身的定位？
假設：
（1）2D平面內有若干帶有不同id號的路標點（landmark）
（2）傳感器用於觀測路標點的信息，觀測范圍是一個以自身位置為原點，r為半徑的圓。其觀測量有三個，第一個觀測量是探測范圍內路標點在傳感器坐標系中的x值，滿足正態分布；第二個觀測量是探測范圍內路標點在傳感器坐標系中的y值，滿足正態分布；第三個觀測為路標點的id號。
如下圖所示：
2.1 坐標系
傳感器坐標系用\(S\)表示，全局坐標系用\(G\)表示。
設地圖中有一路標點\(L=(l_x,l_y)\)，當前傳感器位姿\(P=(p_x,p_y,\theta)\)，那么有:
\[ L^G=R_{GS}L^S + P^G \]
其中:
\[ R_{GS} = \left[\begin{matrix} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{matrix} \right] \]
2.2 運動模型
設輸入量為\(u=(u_1,u_2,u_3)\)，將當前時刻雷達局部坐標系中的（\({u}_{1}\),\(u_2\)）點作為下一時刻雷達的位置：
\[ \left[\begin{array}{cc} {x} \\ {y} \end{array} \right] = \left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right] \left[\begin{array}{cc} {u}_{1} \\ {u}_{2} \end{array}\right] + \left[\begin{array}{cc} {x} \\ {y} \end{array} \right] \]
其方位變化為:
\[ {\theta} = {\theta} + {u}_{3} \]
2.3 觀測模型
設觀測到的路標點L的狀態為(\({{l}_{x}}^{G}\)，\({{l}_{y}}^{G}\))，當前時刻傳感器位姿狀態為\((p_x^G,p_y^G)\)，觀測方程如下：
\[ \left[\begin{array}{cc} z_1 \\ z_2 \end{array} \right] = {\left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]}^{-1} \left(\left[\begin{array}{cc} {{l}_{x}^{G}} \\ {{l}_{y}^{G}} \end{array} \right] - \left[\begin{array}{cc} {p_x^G} \\ {p_y^G} \end{array} \right]\right)+ \left[\begin{matrix} v_1 \\ v_2 \end{matrix}\right] \]
\[ v_1 = N(0,\sigma_1) \]
\[ v_2 = N(0,\sigma_2) \]
3. 技術方案
3.1 前端跟蹤
前端跟蹤的主要目的主要有兩個：
（1）狀態數據關聯
（2）求解新狀態的初始值。
這里的數據關聯可以根據觀測信息中的路標點id號來建立；這里主要討論狀態初始值估計。
算法思路：先固定舊狀態的估計值，然后通過數據關聯的結果來建立關於機器人新位姿狀態的誤差函數，得到新位姿的初始估計；之后根據新位姿，根據觀測方程，直接得到新地圖點狀態的初始估計值。
% 假設L_i是舊狀態中的一個路標點的狀態，其估計值已知為\((l_{ix}^G,l_{iy}^G)\)，設當前新位姿狀態為\(P_j = (p_{jx},p_{jy},\theta_j)\)，前觀測值已知為\(m_{ij}=(m_{ijx},m_{ijy})\)那么有如下關系：
設當前新增機器人位姿P狀態為\((p_x^G,p_y^G,\theta)\)，在當前時刻可以觀測到的一個舊路標點L，其狀態為\(({{l}_{x}}^{G}\)，\({{l}_{y}}^{G})\)，對其的觀測為\(M=(z_1,z_2)\)則可以構建下面粗糙的優化函數：
\[ || \left[\begin{array}{cc} z_1 \\ z_2 \end{array} \right] - {\left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]}^{-1} \left(\left[\begin{array}{cc} {{l}_{x}^{G}} \\ {{l}_{y}^{G}} \end{array} \right] - \left[\begin{array}{cc} {p_x^G} \\ {p_y^G} \end{array} \right]\right) ||_2 \]
進而可以轉換為下面線性最小二乘問題：
\[ \left[\begin{matrix} 1 & 0 & l_x^G & -l_y^G \\ 0 & 1 & l_y^G & l_x^G \end{matrix}\right] \left[\begin{matrix} p_x^G \\ p_y^G \\ cos\theta \\ sin\theta \end{matrix}\right] = \left[\begin{matrix} z_1 \\ z_2 \end{matrix}\right] \]
我們的目標就是估計\((p_x^G,p_y^G,cos\theta,sin\theta)\)，成功跟蹤一個點就可以增加兩個約束，跟蹤的點越多，越精確，但也越耗時，代碼中使用五點法，左側矩陣為\(10\times4\)，並采用LM算法。
上述這樣做法其實不是太好，因為建立的誤差方程不能將測量誤差的協方差引入。會導致不能“平等地”優化每一個變量。當然最一般地做法是和后端優化一樣，建立形如\(||z-h(\xi)||_{\Sigma_z}\)，這樣的誤差函數。
前段是提供一個初始值，所以夠用就行了，況且這里的數據關聯是確定的，沒有任何外點，后端優化會充分彌補前端的不足。
3.2 滑動窗口優化
3.2.1 圖結構
SLAM狀態之間的關系可以用一個圖來表示，如下圖所示：
隨着機器人不斷探索新的環境，這個圖也就不斷擴大，我們這里把這個圖稱作一個狀態窗口。
設窗口內的一次觀測\(z_i=(\alpha_i,s_i)\)，全部狀態量為\(\xi\)，\(n\)維，那么殘差項\(e_i(\xi)=h_i(\xi)-z_i\)：
用前面介紹的LM法計算，有：
\[ J^T\Sigma_e^{-1}J\delta \xi = -J^T\Sigma_e^{-1}e(\xi) \]
假設有\(m\)次觀測，那么上式可以展開為：
\[ \left(J_1^T\Sigma_{e_1}^{-1}J_1+J_2^T\Sigma_{e_2}^{-1}J_2\dots+J_m^T\Sigma_{e_m}^{-1}J_m\right)\delta \xi =-J_1^T\Sigma_{e_1}^{-1}e_1(\xi)-J_2^T\Sigma_{e_2}^{-1}e_2(\xi)\dots-J_m^T\Sigma_{e_m}^{-1}e_m(\xi) \]
\[ J_i(\xi) =\left[\begin{matrix} \frac {\partial e_i(\xi)} {\partial \xi_1} \dots \frac {\partial e_i(\xi)} {\partial \xi_n} \end{matrix}\right] \]
就這樣，在不斷得到新的觀測時，我們不斷將新觀測引入的\(J_i^T\Sigma^{-1}_{e_i}J_i\)和\(-J_i^T\Sigma_{e_i}^{-1}e_i(\xi)\)加入到求解方程兩側中，進而迭代優化。
\subsubsection{觀測模型線性化}
這一節主要內容是求\(J_i(\xi)\):
\[ J_i(\xi) =\left[\begin{matrix} \frac {\partial e_i(\xi)} {\partial \xi_1} \dots \frac {\partial e_i(\xi)} {\partial \xi_n} \end{matrix}\right] =\left[\begin{matrix} 0 & \dots & \frac {\partial e_i(\xi)} {\partial P^G} & \dots & \frac {\partial e_i(\xi)} {\partial L^G} & \dots & 0 \end{matrix}\right]_{2\times n} \]
對機器人位置狀態的子雅克比矩陣：
\[ \frac{\partial e_i(\xi)}{\partial P^G} = \left[\begin{matrix} \frac {\partial e_i^{\alpha}(\xi)} {\partial p_x^G} & \frac {\partial e_i^{\alpha}(\xi)} {\partial p_y^G} & \frac {\partial e_i^{\alpha}(\xi)} {\partial \theta} \\ \frac {\partial e_i^{s}(\xi)} {\partial p_x^G} & \frac {\partial e_i^{s}(\xi)} {\partial p_y^G} & \frac {\partial e_i^{s}(\xi)} {\partial \theta} \end{matrix}\right]_{2\times 3} \]
對地圖點位置狀態的子雅克比矩陣：
\[ \frac{\partial e_i(\xi)}{\partial L^G} = \left[\begin{matrix} \frac {\partial e_i^{\alpha}(\xi)} {\partial l_x^G} & \frac {\partial e_i^{\alpha}(\xi)} {\partial l_y^G} \\ \frac {\partial e_i^{s}(\xi)} {\partial l_x^G} & \frac {\partial e_i^{s}(\xi)} {\partial l_y^G} \end{matrix}\right]_{2\times 2} \]
關於上述兩個子雅克比矩陣在\(J_i(\xi)\)中的擺放位置是根據狀態向量\(\xi\)里的子狀態擺放次序決定的。這里需要先規定一下各個狀態量在\(\xi\)中的擺放次序。
假設滑動窗口中存在連續m個時刻的機器人位置狀態\((P_1^G,P_2^G,\dots P_m^G)\)，每個時刻\(i\)觀測到的新地圖點的狀態（注意是新的地圖點）為\((L^G_1(i),L^G_2(i),\dots,L^G_{n_i}(i))\)，那么這里把狀態量的次序規定如下，當然這樣規定更是為了以后Marginalization的時候方便對矩陣操作：
\[ \xi = \left[\begin{matrix} P_1^G \\ L_1^G(1) \\ L_2^G(1) \\ \vdots \\ L_{n_1}^G(1) \\ P_2^G \\ L_1^G(2) \\ L_2^G(2) \\ \vdots \\ L_{n_2}^G(2) \\ \vdots \\ P_m^G \\ L_1^G(m) \\ L_2^G(m) \\ \vdots \\ L_{n_m}^G(m) \\ \end{matrix} \right]= \left[\begin{matrix} \xi_1 \\ \xi_2 \\ \vdots \\ \xi_m \end{matrix}\right] \]
下面等式關系定義了觀測函數\((\alpha,s)^T=h(P^G,L^G)\)：
\[ \left[\begin{array}{cc} {{l}_{x}^{S}} \\ {{l}_{y}^{S}} \end{array} \right] = {\left[\begin{array}{cc} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]}^{-1} \left(\left[\begin{array}{cc} {{l}_{x}^{G}} \\ {{l}_{y}^{G}} \end{array} \right] - \left[\begin{array}{cc} {p_x^G} \\ {p_y^G} \end{array} \right]\right)+ \left[\begin{matrix} v_1 \\ v_2 \end{matrix}\right] \]
下面具體求\(\frac{\partial e_i(\xi)}{\partial P^G}\)和\(\frac{\partial e_i(\xi)}{\partial L^G}\)：
\[ \frac{\partial e_i(\xi)}{\partial P^G} = \frac{\partial h_i(\xi)}{\partial P^G} \]
\[ \frac{\partial e_i(\xi)}{\partial L^G} = \frac{\partial h_i(\xi)}{\partial L^G} \]
\[ \frac{\partial h_i(\xi)}{\partial P^G} = \left[\begin{matrix} \frac{\partial h_{ix}}{\partial p^G_x} & \frac{\partial h_{ix}}{\partial p^G_y} & \frac{\partial h_{ix}}{\partial \theta} \\ \frac{\partial h_{iy}}{\partial p^G_x} & \frac{\partial h_{iy}}{\partial p^G_y} & \frac{\partial h_{iy}}{\partial \theta} \end{matrix}\right]=\left[\begin{matrix} -cos\theta & -sin\theta & {(p_x^G-l_x^G)sin\theta+(l_y^G-p_y^G)cos\theta} \\ sin\theta & -cos\theta & {(p_x^G-l_x^G)cos\theta+(p_y^G-l_y^G)sin\theta} \end{matrix}\right] \]
\[ \frac{\partial h_i}{\partial L^G}=\left[\begin{matrix} \frac{\partial h_{ix}}{\partial l^G_x} & \frac{\partial h_{ix}}{\partial l^G_y} \\ \frac{\partial h_{iy}}{\partial l^G_x} & \frac{\partial h_{iy}}{\partial l^G_y} \end{matrix}\right]= \left[\begin{matrix} cos\theta & sin\theta \\ -sin\theta & cos\theta \end{matrix}\right] \]
3.2.2 舒爾補與邊緣化
顯然，隨着時間的推移，系統的狀態越來越多，\(H\)陣越來越大，使得計算量越來越大；所以這里采用滑動窗口法來控制優化規模在一個確定的區間內。
滑動窗口法的一個關鍵是如何處理丟棄的觀測，是把它直接丟掉，還是轉化為對剩余狀態量的約束。
很直觀的想法是把最近\(n\)幀的機器人位姿變量以及與其有關聯的地圖點狀態留下，其余都扔了，重新建立新的誤差方程。很明顯，這里相當於把一些約束條件扔了，使得信息丟失一部分，所以這里使用邊緣化技術來得到丟掉的觀測構成的先驗。
這里采用的策略是：對於剛剛優化完成的滑動窗口，如果\(n\)超過規定的最大位姿數量，那么就把最早的一個位姿狀態以及和其有觀測關系的所有地圖點狀態都邊緣化掉，記為\(\xi_m\)，其余剩下的狀態記為\(\xi_r\)。
\(\xi_m\)對應的觀測與\(\xi\)構成的最小二乘問題為：
\[ (\lambda I + H) \delta\xi = b \]
也即為：
\[ (\lambda I + \left[\begin{matrix} H_{mm} & H_{mr} \\ H_{rm} & H_{rr} \end{matrix}\right]) \left[\begin{matrix} \delta \xi_m \\ \delta \xi_r \end{matrix}\right] = \left[\begin{matrix} b_m \\ b_r \end{matrix}\right] \]
利用舒爾補，那么可以得到：
\[ (\lambda I_r + H_{rr} - H_{rm}*(\lambda I_m + H_{mm})^{-1}*H_{mr} )\delta\xi_r = b_r - H_{rm}*(\lambda I_m + H_{mm})^{-1}*b_m \]
這樣我們就得到了丟棄觀測中的先驗信息：
\[ H_{prior} = \lambda I_r + H_{rr} - H_{rm}*(\lambda I_m + H_{mm})^{-1}*H_{mr} \]
\[ b_{prior} = b_r - H_{rm}*(\lambda I_m + H_{mm})^{-1}*b_m \]
在下一次滑窗優化時候，其滑窗內的觀測與\(\xi\)構成新的最小二乘問題為：
\[ (\lambda I + \left[\begin{matrix} H_{rr} & H_{r,new} \\ H_{new,r} & H_{new,new} \end{matrix}\right]) \left[\begin{matrix} \delta \xi_r \\ \delta \xi_{new} \end{matrix}\right] = \left[\begin{matrix} b_r \\ b_{new} \end{matrix}\right] \]
加上先驗即為：
\[ (\lambda I + \left[\begin{matrix} H_{rr} & H_{r,new} \\ H_{new,r} & H_{new,new} \end{matrix}\right] + H_{prior}) \left[\begin{matrix} \delta \xi_r \\ \delta \xi_{new} \end{matrix}\right] = \left[\begin{matrix} b_r \\ b_{new} \end{matrix}\right] + b_{prior} \]
3.2.3 FEJ保證系統一致性
其實我們上面邊緣化的策略已經保證了\(FEJ\)，因為我們把最早的一個位姿狀態以及和其有觀測關系的所有地圖點狀態都邊緣化掉，這保證了誤差函數不會在兩個不同的點處線性化，也就保證了先驗項\(H_{prior}\)與當前\(H\)不會沖突。
4. 系統設計
4.1 數據類型
Landmark類：用於仿真實際2D環境，存放環境各種信息。
Movemodel類：用於仿真機器人運動，存放機器人運動的真實位置，為提供給觀測類。
Measure類：存放測量信息，獲取測量信息。
Frame類：這是為位姿態節點設計的抽象類，其主要目的有兩個，一個是存放當前位姿，另一個是建立與地圖點的聯系。
Mappoint類：為landmark設計的抽象類，其主要目的也有兩個，一個存放估計的地圖點位置，一個是建立與frame聯系
Gaussnewton類：這個類是一個線性最小二乘求解器。
Draw類：里面存放主要抽象類的地址，顯示想要的數據。
Slidewindowgraph類：滑動窗口類，是最重要的數據成員，其利用上述數據類型，完成前端數據關聯，后端滑窗優化。
4.2 代碼邏輯
系統整個過程的核心就是維護Slidewindowgraph這個類,具體從main.py開始看，代碼清晰，模塊分明。
5. 仿真結果
僅僅前端跟蹤，后端優化去掉。打開slidewindow_graph.py，注釋掉函數def Update(self, measure)中的self.Optimize_graph()，其運行結果為：
使用后端優化，但是不使用滑動窗口，狀態量都保存下來。打開slidewindow_graph.py，注釋掉函數def Optimize_graph(self)中的 self.Get_prior()和self.Cut_window()，其運行結果為：
使用后端優化，使用滑動窗口，但是不使用先驗信息。打開slidewindow_graph.py，注釋掉函數def Optimize_graph(self)中的 self.Get_prior()，其運行結果為：
使用后端優化，使用滑動窗口，使用先驗信息。其運行結果為：
圖中各元素說明：紅色星形為環境中固定的landmark，藍色點是當前狀態窗口中landmark位置的估計值，紅色方形是當前狀態窗口中機器人位姿估計值，藍色方形是前后幀之間跟蹤用的五個狀態點。
從仿真結果看，不使用滑動窗口的全局優化效果最好；使用滑動窗口時，用不用先驗效果不太明顯；僅僅使用前端五點法跟蹤，效果非常差。
