一、優(yōu)化的“本質(zhì)”

先從濾波講起~

SLAM首先是一個狀態(tài)估計問題：在前端中，我們需要從k-1時刻的狀態(tài)量 出發(fā)，推斷出k時刻的狀態(tài) ，最簡單的情況下，狀態(tài)僅包含位姿，如果用四元數(shù)表達旋轉(zhuǎn)的話，狀態(tài)向量就是7維的（position 3 + orientation 4)。我們用來完成狀態(tài)估計的信息有：k-1時刻到k時刻之間的運動測量 和k時刻的環(huán)境觀測 。。 上面部分，我們構(gòu)建了兩個時刻間的優(yōu)化問題，并可以用各種優(yōu)化策略取去求解。但相比于濾波方法，這并沒有什么優(yōu)越性，其精度和效率都是等價的。

(資料圖片)

優(yōu)化方法真正的優(yōu)勢在于它可以考慮更多時刻的信息，從而大大提升精度。這里得提一下貝葉斯濾波框架的局限：貝葉斯濾波方法依賴馬爾可夫性，也即每一時刻的狀態(tài)僅與上一時刻有關(guān)，這就決定了上一時刻之前的測量信息將被丟掉。 優(yōu)化方法則可以考慮所有時刻的測量信息，上面部分中，我們僅考慮了兩個時刻間的誤差項，現(xiàn)在，我們把所有過往時刻（1, 2, ... k-1, k）的誤差項都添加到目標函數(shù)中，我們以所有時刻的狀態(tài)為自變量，求目標函數(shù)（也即總體誤差）的最小值，這樣就得到了整體的最優(yōu)狀態(tài)估計！精度大大提升。當(dāng)然，由于我們考慮了所有時刻的信息，此時的維度會非常大，并且隨著時間的推移維度將越來越大！但所幸，目標函數(shù)中的單個誤差項并不是與所有時刻的狀態(tài)都相關(guān)，所以求解過程中的相關(guān)矩陣會呈現(xiàn)出稀疏性，這就允許我們對高維問題實時求解。 我們也可以只取相近若干時刻的狀態(tài)進行優(yōu)化（而非所有時刻），也即構(gòu)建一個“滑窗”。

在此基礎(chǔ)上，更高級的做法是，對滑窗邊緣的那個時刻，我們對其進行邊緣化處理以使其能夠融合更早時刻的信息（而不是把更早時刻的信息直接丟掉），這樣，我們就構(gòu)建了“滑窗優(yōu)化”，這方面的代表作如VINS。 優(yōu)化作為一種方法，既可以用于前端，也可以用于后端，甚至你想用的任何地方。用于后端時，通常是全局的優(yōu)化，也即“考慮所有時刻的狀態(tài)和所有測量誤差項”的優(yōu)化。發(fā)生閉環(huán)時，意味著當(dāng)前時刻的狀態(tài) 和歷史上某一個時刻的狀態(tài) 之間構(gòu)建了誤差項，由于位姿漂移，這個誤差項往往很大！當(dāng)我們再去對全局誤差進行最小化時，我們就需要“微調(diào)”全局各個時刻的狀態(tài)量，以把這個誤差“分散”到各個時刻去，從而實現(xiàn)全局誤差的最小化。這個“微調(diào)”的過程，同樣由優(yōu)化來完成。 因此，從以上各個角度來看，一個現(xiàn)代的SLAM系統(tǒng)，本質(zhì)上就是一個優(yōu)化的系統(tǒng)，SLAM本質(zhì)上就是一個優(yōu)化問題。而所有的視覺里程計，激光里程計，輪式，imu等等，它們的作用就是構(gòu)建出一個個的誤差項。

一個誤差項，也即一個約束。

二、線性化與非線性化優(yōu)化問題

優(yōu)化問題本質(zhì)來說都是最小二乘問題，其關(guān)鍵的地方在于：如何設(shè)計與建立SLAM的problem structure：

除非理想情況下，實際中都是非線性化問題，但是我們不知道非線性模型，無法求解實際問題，于是我們通過對非線性問題進行線性化分析，忽略一些因素，構(gòu)建線性化方程求解非線性化問題。

非線性化：1、通俗來說，非線性優(yōu)化就是求函數(shù)的極值，2、再多說一句，在非線性優(yōu)化里面通常求最小值。

1、我們想求一個 函數(shù)的極值問題的時候，線性函數(shù)是最簡單的，因為是線性的嘛，單調(diào)增或者單調(diào)減，那么找到邊界就可以求到極值。例如 f(x)=ax+b。

2、簡單的非線性函數(shù)也是很容易求得極值的，例如f(x)=x*x.可以通過求導(dǎo)得到極值點，然后求得其極值。

3、但是對于復(fù)雜的非線性函數(shù)，或者復(fù)雜的數(shù)學(xué)模型，求導(dǎo)很困難或者無法求導(dǎo)的時候怎么求極值呢？那么就出現(xiàn)了很多非線性優(yōu)化的算法。來解決對于復(fù)雜數(shù)學(xué)模型的求極值的問題。

三、全局優(yōu)化與局部優(yōu)化

全局優(yōu)化的英文是（global optimization） 1、全局優(yōu)化是找到在整個可行區(qū)域內(nèi)使目標函數(shù) 最小的 可行點x的問題。通俗來說就是從所有可能的x值里面找到最小值 2、通常，可能是一個非常困難的問題，隨著參數(shù)數(shù)量的增加，難度將成倍增加。比如上面的例子是x1和x2兩個參數(shù)，有著不同的組合方式。那么如何有x1~x100個參數(shù)呢，會有多少種組合方式呢？

3、實際上，除非知道有關(guān) f 的特殊信息，否則甚至無法確定是否找到了真正的全局最優(yōu)值，因為可能會出現(xiàn) f 值的突然下降，且這個值隱藏在您尚未查找的參數(shù)空間中。 局部優(yōu)化的英文是（local optimization） 1、局部優(yōu)化是一個容易得多的問題。它的目標是找到一個僅是局部最小值的可行點x：f（x）小于或等于所有附近可行點的f值 2、通常，非線性優(yōu)化問題可能有很多局部最小值，算法確定的最小值位置通常取決于用戶提供給算法的起點。3、另一方面，即使在非常高維的問題中（尤其是使用基于梯度的算法），局部優(yōu)化算法通常也可以快速定位局部最小值。

四、優(yōu)化使用的方法

具體的優(yōu)化求解策略包括最速下降法、牛頓法、高斯-牛頓法（G-N），基于信任區(qū)域的L-M法等。

1、梯度下降法

函數(shù)的下降方向?qū)⒂肋h為函數(shù)的負梯度方向 梯度下降法是一個一階最優(yōu)化算法，通常也稱為最速下降法。要使用梯度下降法找到一個函數(shù)的局部極小值，必須向函數(shù)上當(dāng)前點對應(yīng)梯度（或者是近似梯度）的反方向的規(guī)定步長距離點進行迭代搜索。因此指保留一階梯度信息。缺點是過于貪心，容易走出鋸齒路線。

2、牛頓法牛頓法是一個二階最優(yōu)化算法，基本思想是利用迭代點處的一階導(dǎo)數(shù)(梯度)和二階導(dǎo)數(shù)(Hessen矩陣)對目標函數(shù)進行二次函數(shù)近似。因此保留二階梯度信息。缺點是需要計算H矩陣，計算量太大。

3、高斯牛頓法

其實其就是使用上式，對牛頓法的H矩陣進行替換

有可能為奇異矩陣或變態(tài)，Δx也會造成結(jié)果不穩(wěn)定，因此穩(wěn)定性差

4、LM信任區(qū)域法

但是這個近似假設(shè)的成立是有一定程度限制的，我們可以設(shè)立一個正值變量△使得模型在一個以x為圓心為半徑的圓中被視為可以精確近似。這個圓就是我們所說的信任區(qū)域。然后我們就可以基于以下公式求解變化值h。 LM算法大致與高斯牛頓算法理論相同，但是不同于高斯牛頓算法，LM算法是基于信賴區(qū)域理論(Trust Region Method)進行計算的。這是因為高斯牛頓法中的泰勒展開只有在展開點附近才會有比較好的效果，因此為了確保近似的準確性我們需要設(shè)定一個具有一定半徑的區(qū)域作為信賴區(qū)域。

基于信賴區(qū)域我們能夠重新構(gòu)建一個更有效的優(yōu)化框架 采用信賴區(qū)域法我們就需要明確該區(qū)域該怎么確定。在LM算法中信賴區(qū)大小的確定也是運用增益比例來進行判定的。

四、非線性優(yōu)化解退化問題

對于SLAM而言，視覺傳感器需要紋理信息決定特征點，距離傳感器需要空間幾何結(jié)構(gòu)信息決定特征點，在特征點缺乏時，狀態(tài)估計方法會進行退化。定位的退化主要是因為約束的減少，比如NDT需要三個正交方向的約束才能很好的匹配，但若在狹長的走廊上或者隧道環(huán)境，條件單一，即使人肉眼觀看激光雷達數(shù)據(jù)，也很難判斷機器人所處的位置。因為激光“看到”的環(huán)境都是一樣的，在隧道方向是沒有約束的；并且在隧道中GPS信號是沒的，只能依靠高精度的姿態(tài)傳感器和輪速機的融合來做，當(dāng)然隨著運行時間的增加，誤差會慢慢增加。 處理退化的常用方法有: 1）當(dāng)出現(xiàn)退化時換一種方法;這要求在設(shè)計時需要一個備用方法可用 2）在狀態(tài)估計過程中加入人工約束，如恒速模型的約束。

即使問題本身是可解決的情況下也會帶來不必要的誤差。我們的方法是在原始問題的部分子空間添加約束。原始問題的解可以分為非退化方向和退化方向，在處理問題時，首先確定退化方向，然后將求出原始問題非退化方向的解，對于退化方向的解使用猜測值。在最后的實踐算法中，只使用非線性優(yōu)化解在非退化方向的分量，不考慮退化方向的分量。 正常情況下，約束應(yīng)該是分布在空間中的多個方向，從各個角度約束解，如下面所示，綠色點表示問題的解，被非退化約束限制在了一個小區(qū)域。這樣的解就比較準確，約束發(fā)生小變化時，解的變化也很小，被限制在一個局部區(qū)域，這樣的解就是一個比較理想的解。

如果解的約束大多近似平行，那么他們就是退化的方向（藍色箭頭表示的方向），這個時候解在退化方向收到的約束就很差。考慮絕對平行時，解的約束也是一個平行的方向，那么這種情況下的解是需要避免的，如果其中一個約束發(fā)生了小的偏移，那么解所在的局部區(qū)域會發(fā)生較大的變化，這樣的解是比較糟糕的解。

每次求解時 優(yōu)化解=原始解在非退化方向投影+估計值在退化方向投影（實際算法中可以將退化方向解丟棄，只考慮非退化方向）

編輯：黃飛