0.引言

轉(zhuǎn)載自此處，修正了一點小錯誤。

1.最小二乘問題

在求解 SLAM 中的最優(yōu)狀態(tài)估計問題時，我們一般會得到兩個變量，一個是由傳感器獲得的實際觀測值 $\mathbf{z}$，一個是根據(jù)目前估計的狀態(tài)量和觀測模型計算出來的預(yù)測值 $h(\mathbf{x})$。求解最優(yōu)狀態(tài)估計問題時通常我們會嘗試最小化預(yù)測值和觀測值計算出的殘差平方（使用平方是為了統(tǒng)一正負(fù)號的影響），即求解以下最小二乘問題：

$${\mathbf{x}}^{?}=\arg \underset{\mathbf{x}}{\min }||\mathbf{z}?h(\mathbf{x})|{|}^2$$
如果觀測模型是線性模型，則上式轉(zhuǎn)化為線性最小二乘問題：

$${\mathbf{x}}^{?}=\arg \underset{\mathbf{x}}{\min }||\mathbf{z}?\mathbf{H}\mathbf{x}|{|}^2$$
對于線性最小二乘問題，我們可以直接求閉式解：${\mathbf{x}}^{?}=?({\mathbf{H}}^T\mathbf{H}{)}^{?1}{\mathbf{H}}^T\mathbf{z}$ 這里不進(jìn)行贅述。

實際的問題中，我們通常要最小化不止一個殘差，不同殘差通常會按其重要性（不確定性）分配一個相應(yīng)的權(quán)重系數(shù)，且觀測模型也通常是非線性的，即求解以下問題：

$$\begin{array}{rl}{\mathbf{e}}_i(\mathbf{x})& ={\mathbf{z}}_i?h_i(\mathbf{x})i=1,2,...,n\\ ||{\mathbf{e}}_i(\mathbf{x})|{|}_{{\Sigma }_i}^2& ={\mathbf{e}}_i^T{\mathbf{\Sigma }}_i{\mathbf{e}}_i\\ {\mathbf{x}}^{?}& =\arg \underset{\mathbf{x}}{\min }F(\mathbf{x})\\ & =\arg \underset{\mathbf{x}}{\min }\sum _i||{\mathbf{e}}_i(\mathbf{x})|{|}_{{\Sigma }_i}^2\end{array}$$
我們想要獲得一個狀態(tài)量 ${\mathbf{x}}^{?}$，使得損失函數(shù) $F(\mathbf{x})$ 取得局部最小值。

在具體求解之前，先考慮 $F(\mathbf{x})$ 的性質(zhì)，對其進(jìn)行二階泰勒展開：

$$F(\mathbf{x}+\Delta \mathbf{x})=F(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}+O(||\Delta \mathbf{x}|{|}^3)$$
忽略高階余項，對二次函數(shù)，有以下性質(zhì)：

如果在點 ${\mathbf{x}}^{?}$ 處，導(dǎo)數(shù)為 $0$，則這個點為穩(wěn)定點，根據(jù) Hessian 矩陣的正定性有不同性質(zhì)：

• 如果 $\mathbf{H}$ 為正定矩陣，則 $F({\mathbf{x}}^{?})$ 為局部最小值
• 如果 $\mathbf{H}$ 為負(fù)定矩陣，則 $F({\mathbf{x}}^{?})$ 為局部最大值
• 如果 $\mathbf{H}$ 為不定矩陣，則 $F({\mathbf{x}}^{?})$ 為鞍點

在實際過程中，一般 $F(x)$ 比較復(fù)雜，我們沒有辦法直接使其導(dǎo)數(shù)為 0 進(jìn)而求出該點，因此常用的是迭代法，即找到一個下降方向使損失函數(shù)隨 $\mathbf{x}$ 的迭代逐漸減少，直到 $\mathbf{x}$ 收斂到 ${\mathbf{x}}^{?}$。下面整理以下常用的幾種迭代方法。

2.迭代下降法

上面提到，我們需要找到一個 $\mathbf{x}$ 的迭代量使得 $F(\mathbf{x})$ 減少。這個過程分兩步：

找到 $\mathbf{F}(\mathbf{x})$ 的下降方向，構(gòu)建該方向的單位向量 $\mathbf{d}$
確定該方向的迭代步長 $\alpha$
迭代后的自變量 $\mathbf{x}+\alpha \mathbf{d}$ 對應(yīng)的函數(shù)值可以用一階泰勒展開近似（當(dāng)步長足夠小的時候）：

$$F(\mathbf{x}+\alpha \mathbf{d})=F(\mathbf{x})+\alpha \mathbf{Jd}$$
因此，不難發(fā)現(xiàn)，要保持 $F(x)$ 是下降的，只需要保證 $\mathbf{Jd}<0$。以下幾種方法都是以不同思路在尋找一個合適的方向進(jìn)行迭代。

3.最速下降法

基于上一部分，我們知道變化量為 $\alpha \mathbf{Jd}$，其中 $\mathbf{Jd}=||\mathbf{J}||\cos \theta$，$\theta$ 為梯度$\mathbf{J}$ 和 $\mathbf{d}$ 的夾角。當(dāng) $\theta =?\pi$ 時，$\mathbf{Jd}=?||\mathbf{J}||$ 取得最小值，此時方向向量為：

$$\mathbf{d}=\frac{?{\mathbf{J}}^T}{||\mathbf{J}||}$$
因此，沿梯度 $\mathbf{J}$ 的負(fù)方向可以使 $F(\mathbf{x})$，但實際過程中，一般只會在迭代剛開始使用這種方法，當(dāng)接近最優(yōu)值時這種方法會出現(xiàn)震蕩并且收斂較慢。

4.牛頓法

對 $F(\mathbf{x})$ 進(jìn)行二階泰勒展開有：

$$F(\mathbf{x}+\Delta \mathbf{x})=F(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}$$
我們關(guān)注的是求一個 $\Delta \mathbf{x}$ 使得 $\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}$ 最小，因此可以求導(dǎo)得：

$$\begin{array}{rl}\frac{?(\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x})}{?\Delta \mathbf{x}}& ={\mathbf{J}}^T+\mathbf{H}\Delta \mathbf{x}=0\\ ?\Delta \mathbf{x}& =?{\mathbf{H}}^{?1}{\mathbf{J}}^T\end{array}$$
當(dāng) $\mathbf{H}$ 為正定矩陣且當(dāng)前 $\mathbf{x}$ 在最優(yōu)點附近時，取 $\Delta \mathbf{x}=?{\mathbf{H}}^{?1}{\mathbf{J}}^T$ 可以使函數(shù)獲得局部最小值。但缺點是殘差的 Hessian 函數(shù)通常比較難求。

5.阻尼法

在牛頓法的基礎(chǔ)上，為了控制每次迭代不要太激進(jìn)，我們可以在損失函數(shù)中增加一項懲罰項，如下所示：

$$\arg \underset{\Delta \mathbf{x}}{\min }\left\{F(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T\mathbf{H}\Delta \mathbf{x}+\frac{1}{2}\mu (\Delta \mathbf{x}{)}^T(\Delta \mathbf{x})\right\}$$
當(dāng)我們選的 $\Delta \mathbf{x}$ 太大時，損失函數(shù)也會變大，變大的幅度由 $\mu$ 決定，因此我們可以控制每次迭代量 $\Delta \mathbf{x}$ 的大小。同樣在右側(cè)部分對 $\Delta \mathbf{x}$ 求導(dǎo)有：

$$\begin{array}{rl}{\mathbf{J}}^T+\mathbf{H}\Delta \mathbf{x}+\mu \Delta \mathbf{x}& =0\\ (\mathbf{H}+\mu \mathbf{I})\Delta \mathbf{x}=?{\mathbf{J}}^T& \end{array}$$
這個思路我們后面在介紹 LM 法時也會用到。

6.高斯牛頓(GN)法

在前面的整理中實際上我們求解的是一系列殘差的和，求單個殘差的雅可比比較簡單，因此在后續(xù)的幾種方法中我們關(guān)注各個殘差的變化。將上述非線性最小二乘問題中的各個殘差寫成向量形式：

$$\mathbf{F}(\mathbf{x})=\mathbf{E}(\mathbf{x})=\left[\begin{array}{c}{\mathbf{e}}_1(\mathbf{x})\\ {\mathbf{e}}_2(\mathbf{x})\\ ...\\ {\mathbf{e}}_n(\mathbf{x})\end{array}\right]$$

對 $\mathbf{e}(\mathbf{x})$ 進(jìn)行泰勒展開，有：

$$\mathbf{e}(\mathbf{x}+\Delta \mathbf{x})=\mathbf{e}(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}$$
上式中，$\mathbf{J}$ 是殘差矩陣 $\mathbf{e}(\mathbf{x})$ 對狀態(tài)量的雅可比矩陣。

注意到在原來的線性最小二乘問題中，對每個殘差還有一個權(quán)重矩陣 $\mathbf{\Sigma }$。這種情況下，我們只需要令 ${\mathbf{e}}_i(\mathbf{x})=\sqrt{{\mathbf{\Sigma }}_i}{\mathbf{e}}_i(\mathbf{x})$ 即可。因此下式中暫不考慮 $\mathbf{\Sigma }$ 的影響。

在這種形式下對 $\mathbf{e}(\mathbf{x})$ 進(jìn)行泰勒展開，有：

$$\begin{array}{rl}||e(\mathbf{x}+\Delta \mathbf{x})|{|}^2& =\mathbf{e}(\mathbf{x}+\Delta \mathbf{x}{)}^T\mathbf{e}(\mathbf{x}+\Delta \mathbf{x})\\ & =(\mathbf{e}(\mathbf{x})+\mathbf{J}\Delta \mathbf{x}{)}^T(\mathbf{e}(\mathbf{x})+\mathbf{J}\Delta \mathbf{x})\\ & =\mathbf{e}(\mathbf{x}{)}^T\mathbf{e}(\mathbf{x})+\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{e}(\mathbf{x})+\mathbf{e}(\mathbf{x}{)}^T\mathbf{J}\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}\end{array}$$
上式中，注意到：$e(\mathbf{x})$ 是一維的，因此 $\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{e}(\mathbf{x})=\mathbf{e}(\mathbf{x}{)}^T\mathbf{J}\Delta \mathbf{x}$，因此化簡得：

$$\begin{array}{rl}F(\mathbf{x}+\Delta \mathbf{x})& =\mathbf{e}(\mathbf{x}{)}^T\mathbf{e}(\mathbf{x})+2\mathbf{e}(\mathbf{x}{)}^T\mathbf{J}\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}\\ & =F(\mathbf{x})+2\mathbf{e}(\mathbf{x}{)}^T\mathbf{J}\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}\end{array}$$
通過這種方式，我們同樣將其近似一個二次函數(shù)，并且和我們之前展開的結(jié)果比較，不難發(fā)現(xiàn)在這里我們實際上是用 ${\mathbf{J}}^T\mathbf{e}$ 來近似 Jacobian 矩陣，用 ${\mathbf{J}}^T\mathbf{J}$ 來近似 Hessian 矩陣。因此，當(dāng) $\mathbf{J}$ 滿秩時，我們可以保證在上式導(dǎo)數(shù)為 0 的地方可以確保函數(shù)取得局部最小值。同樣，在上式右側(cè)部分對 $\Delta \mathbf{x}$ 求導(dǎo)并使其為 0 有：

$$\begin{array}{rl}{\mathbf{J}}^T\mathbf{e}(\mathbf{x})+{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}=0& \\ ?{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}& =?{\mathbf{J}}^T\mathbf{e}(\mathbf{x})\\ ?\mathbf{H}\Delta \mathbf{x}& =\mathbf{b}\end{array}$$
上式中，我們令 $\mathbf{H}={\mathbf{J}}^T\mathbf{J},\mathbf{b}=?{\mathbf{J}}^T\mathbf{e}$。這樣我們獲得了高斯牛頓法的求解過程：

• 計算殘差矩陣關(guān)于狀態(tài)值的雅可比矩陣 $\mathbf{J}$
• 利用雅可比矩陣和殘差構(gòu)建信息矩陣和信息向量 $\mathbf{H},\mathbf{b}$
• 計算當(dāng)次迭代量：$\Delta \mathbf{x}={\mathbf{H}}^{?1}\mathbf{b}$
• 如果迭代量足夠小則結(jié)束迭代，否則進(jìn)入下一次迭代

7.萊文貝格馬夸特(LM)法

LM 法是在高斯牛頓法的基礎(chǔ)上按照阻尼法的思路加入阻尼因子，即求解以下方程：

$$(\mathbf{H}+\mu \mathbf{I})\Delta \mathbf{x}=\mathbf{b}$$
上式中，阻尼因子的作用有：

• 添加進(jìn) $\mathbf{H}$ 保證 $\mathbf{H}$ 是正定的

• 當(dāng) $\mu$ 很大時，$\Delta \mathbf{x}=?(\mathbf{H}+\mu \mathbf{I}{)}^{?1}\mathbf{b}\approx ?\frac{1}{\mu }\mathbf{b}=?\frac{1}{\mu }{\mathbf{J}}^T\mathbf{E}(\mathbf{x})$，接近最速下降法

• 當(dāng) $\mu$ 很小時，則接近高斯牛頓法
  因此，合理的設(shè)置阻尼因子，能夠達(dá)到動態(tài)對迭代速度進(jìn)行調(diào)節(jié)。阻尼因子的設(shè)置分為兩部分：

• 初始值的選取

• 隨迭代量變化的更新策略

先來看初始值選取方法，阻尼因子的大小應(yīng)該根據(jù) ${\mathbf{J}}^T\mathbf{J}$ 的大小來選取，對 ${\mathbf{J}}^T\mathbf{J}$ 進(jìn)行特征值分解，有：${\mathbf{J}}^T\mathbf{J}=\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^T$，$\mathbf{\Lambda }=\text{diag}({\lambda }_1,{\lambda }_2,...,{\lambda }_n),\mathbf{V}=[{\mathbf{v}}_1,...,{\mathbf{v}}_n]$，因此，迭代公式化簡為：

$$\begin{array}{rl}(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^T+\mu \mathbf{I})\Delta \mathbf{x}& =\mathbf{b}\\ \Delta \mathbf{x}& =(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^T+\mu \mathbf{I}{)}^{?1}\mathbf{b}\\ & =?\sum _i\frac{{\mathbf{v}}_i^T\mathbf{b}}{{\lambda }_i+\mu }{\mathbf{v}}_i\end{array}$$
因此，將 $\mu$ 選為 ${\lambda }_i$ 接近即可，一個簡單的思路是設(shè) ${\mu }_0=\tau \max ({\mathbf{J}}^T\mathbf{J}{)}_{ii}$，實際中一般設(shè) $\tau \approx [10^{?8},1]$

接下來看 $\mu$ 的更新策略，先定性分析應(yīng)該怎么更新阻尼因子：

• 當(dāng) $\Delta \mathbf{x}$ 令 $F(\mathbf{x})$ 增加時，應(yīng)該提高 $\mu$ 來降低 $\Delta \mathbf{x}$ 即通過減少步長來降低本次迭代帶來的影響
• 當(dāng) $\Delta \mathbf{x}$ 令 $F(\mathbf{x})$ 減少時，應(yīng)該降低 $\mu$ 來提高 $\Delta \mathbf{x}$ 即通過增加步長來提高這次迭代的影響

下面進(jìn)行定量分析，令 $L(\Delta \mathbf{x})=F(\mathbf{x})+2\mathbf{E}(\mathbf{x}{)}^T\mathbf{J}\Delta \mathbf{x}+\frac{1}{2}\Delta {\mathbf{x}}^T{\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}$考慮以下比例因子：

$$\rho =\frac{F(\mathbf{x})?F(\mathbf{x}+\Delta \mathbf{x})}{L(0)?F(\Delta \mathbf{x})}$$
Marquardt 提出一個策略：

• 當(dāng) $\rho <0$，表示當(dāng)前 $\Delta \mathbf{x}$ 使 $F(\mathbf{x})$ 增大，表示離最優(yōu)值還很遠(yuǎn)，應(yīng)該提高 $\mu$ 使其接近最速下降法進(jìn)行較大幅度的更新
• 當(dāng) $\rho >0$ 且比較大，表示當(dāng)前 $\Delta \mathbf{x}$ 使 $F(\mathbf{x})$ 減小，應(yīng)該降低 $\mu$ 使其接近高斯牛頓法，降低速度更新至最優(yōu)點
• 如果 $\rho >0$ 但比較小，表示已經(jīng)到最優(yōu)點附近，則增大阻尼 $\mu$，縮小迭代步長

Marquardt 的具體策略如下：

if rho < 0.25: mu = mu * 2
else if rho > 0.75: mu = mu /3

一個使用 Marquardt 策略進(jìn)行更新的過程如下：

可以發(fā)現(xiàn)，效果并不算很好，隨著迭代次數(shù)的增加，$\mu$ 開始進(jìn)行震蕩，表示迭代量周期性的使 $F(\mathbf{x})$ 增加又下降。

因此，Nielsen 提出了另一個策略，也是 G2O 和 Ceres 中使用的策略：

if rho > 0:mu = mu * max(1/3, 1 - (2 * rho - 1)^3)v = 2
else:mu = mu * vv = 2 * v

使用這種策略進(jìn)行優(yōu)化的例子如下：

可以看到，$\mu$ 隨著迭代的進(jìn)行可以比較平滑的持續(xù)下降直至達(dá)到收斂。

8.魯棒核函數(shù)

在進(jìn)行最小二乘問題中，我們會遇到一些異常觀測值使得觀測殘差特別大，如果不對這些異常點做處理會影響在優(yōu)化過程中，優(yōu)化器會嘗試最小化異常的殘差項，最后影響狀態(tài)估計的精度，魯棒核函數(shù)就是用來降低這些異常觀測值造成的影響。

將魯棒核函數(shù)直接作用在每個殘差項上，將最小二乘問題變成如下形式：

$$F(\mathbf{x})=\sum _i\rho (||e_i(\mathbf{x})|{|}^2)$$
使用魯棒核函數(shù)時求解非線性最小二乘的過程
在這個形式下對帶有魯棒核函數(shù)的殘差進(jìn)行二階泰勒展開：

$$\rho (s+\Delta s)=\rho (x)+{\rho }^{\prime }(x)\Delta s+\frac{1}{2}{\rho }^{\prime \prime }(x){\Delta }^{2}s$$
上式中，變化量 $\Delta s$ 的計算方式如下：

$$\begin{array}{rl}\Delta s_k& =||e_i(\mathbf{x}+\Delta \mathbf{x})|{|}^2?||e_i(\mathbf{x})|{|}^2\\ & =||e_i(\mathbf{x})+{\mathbf{J}}_i\Delta \mathbf{x}|{|}^2?||e_i(\mathbf{x})|{|}^2\\ & =2e_i(\mathbf{x}{)}^T{\mathbf{J}}_i\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{J}}_i^T{\mathbf{J}}_i\Delta \mathbf{x}\end{array}$$
將 $\Delta s$ 代入 $\rho (s+\Delta s)$ 可得：

$$\begin{array}{rl}\rho (s+\Delta s)=& \rho (s)+{\rho }^{\prime }(s)(2e_i(\mathbf{x}{)}^T{\mathbf{J}}_i\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{J}}_i^T{\mathbf{J}}_i\Delta \mathbf{x})\\ & +\frac{1}{2}{\rho }^{\prime \prime }(s)(2e_i(\mathbf{x}{)}^T{\mathbf{J}}_i\Delta \mathbf{x}+\Delta {\mathbf{x}}^T{\mathbf{J}}_i^T{\mathbf{J}}_i\Delta \mathbf{x}{)}^2\\ \approx & \rho (s)+2{\rho }^{\prime }(s)e_i(\mathbf{x}{)}^T{\mathbf{J}}_i\Delta \mathbf{x}+{\rho }^{\prime }(s)\Delta {\mathbf{x}}^T{\mathbf{J}}_i^T{\mathbf{J}}_i\Delta \mathbf{x}\\ & +2{\rho }^{\prime \prime }(s)\Delta {\mathbf{x}}^T{\mathbf{J}}_i^T{e}_i(\mathbf{x})e_i(\mathbf{x}{)}^T{\mathbf{J}}_i\Delta \mathbf{x}\end{array}$$
按照之前的思路，對上式求導(dǎo)并使其為 0，可得：

$$\sum _i{\mathbf{J}}_i^T({\rho }^{\prime }(s)+2{\rho }^{\prime \prime }(s)e_i(\mathbf{x})e_i(\mathbf{x}{)}^T)\mathbf{J}\Delta \mathbf{x}=?\sum _i{\rho }^{\prime }(s){\mathbf{J}}_i^T{e}_i(\mathbf{x})$$
對比之前的矩陣 ${\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}=?{\mathbf{J}}_i^T\mathbf{e}(\mathbf{x})$ 可得，當(dāng)我們使用了魯棒核函數(shù)之后，只需要將各項殘差的核函數(shù)的一階二階導(dǎo)數(shù)值計算出，再按照以上形式對信息矩陣和信息向量進(jìn)行更新即可。

常用的魯棒核函數(shù)
柯西魯棒核函數(shù)：

$$\begin{array}{rl}\rho (s)& =c^2\log (1+\frac{s}{c^2})\\ {\rho }^{\prime }(s)& =\frac{1}{1+\frac{s}{c^2}}\\ {\rho }^{\prime \prime }(s)& =?\frac{1}{c^2}({\rho }^{\prime }(s){)}^2\end{array}$$
其中，$c$ 為控制參數(shù)。當(dāng)殘差是正態(tài)分布，Huber c 選為 1.345，Cauchy c 選為 2.3849。不同魯棒核函數(shù)的效果如下圖所示：