自動駕駛中建圖定位的線性優化問題解析

一、優化的“本質”

先從濾波講起~

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

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

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

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

二、線性化與非線性化優化問題

優化問題本質來說都是最小二乘問題，其關鍵的地方在于：如何設計與建立SLAM的problem structure：

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

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

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

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

3、但是對于復雜的非線性函數，或者復雜的數學模型，求導很困難或者無法求導的時候怎么求極值呢？那么就出現了很多非線性優化的算法。來解決對于復雜數學模型的求極值的問題。

三、全局優化與局部優化

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

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

四、優化使用的方法

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

1、梯度下降法

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

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

3、高斯牛頓法

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

有可能為奇異矩陣或變態，Δx也會造成結果不穩定，因此穩定性差

4、LM信任區域法

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

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

四、非線性優化解退化問題

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

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