一、前置知识点

1.1 SLAM模型

x k \boldsymbol x_k xk​是指 k k k时刻的机器人位姿。比如相机位姿，可以使用 S E ( 3 ) SE(3) SE(3)来描述。
z k , j \boldsymbol z_{k,j} zk,j​是指 k k k时刻对第 j j j个路标点的观测值

{ x k = f ( x k − 1 , u k ) + w k           ⋅ ⋅ ⋅ 运 动 方 程 z k , j = h ( y j , x k ) + v k , j ⋅ ⋅ ⋅ 观 测 方 程 \left \{ \begin{aligned} \boldsymbol x_k&=f(\boldsymbol x_{k-1},\boldsymbol u_k)+\boldsymbol w_k ~~~~~~~~~&···运动方程\\ \boldsymbol z_{k,j}&=h(\boldsymbol y_j,\boldsymbol x_k)+\boldsymbol v_{k,j} &···观测方程 \end{aligned} \right. {xk​zk,j​​=f(xk−1​,uk​)+wk​         =h(yj​,xk​)+vk,j​​⋅⋅⋅运动方程⋅⋅⋅观测方程​

其中 u k \boldsymbol u_k uk​是指 k k k时刻的运动传感器的读数或者输入
　　 w k \boldsymbol w_k wk​是指 k k k时刻的运动噪声
　　 y j \boldsymbol y_j yj​表示第 j j j个路标点
　　 v k , j \boldsymbol v_{k,j} vk,j​表示 k k k时刻第 j j j个路标点的观测噪声

相机位姿 x \boldsymbol x x可以使用 S E ( 3 ) SE(3) SE(3)来描述

x k = T k = e x p ( ξ k ^ ) \boldsymbol x_k=\boldsymbol T_k=exp(\boldsymbol \xi_k^{\hat{}}) xk​=Tk​=exp(ξk^​)

路标点 z \boldsymbol z z一般指拍照得出图片中的像素位置。

s z k , j = K T k y j s\boldsymbol z_{k,j}=\boldsymbol K\boldsymbol T_k\boldsymbol y_j szk,j​=KTk​yj​
那么我们如何通过输入量 u \boldsymbol u u和观测量 z \boldsymbol z z来优化机器人位姿 x \boldsymbol x x和路标点信息 y \boldsymbol y y呢？
若 w k \boldsymbol w_k wk​与 v k , j \boldsymbol v_{k,j} vk,j​都是高斯噪声，且观测方程 h ( ⋅ ) \boldsymbol h(·) h(⋅)是线性的，那么可以直接使用滤波器来进行优化；
否则，需要使用非线性优化。

1.2 最小二乘项

多维高斯分布概率密度: x ∼ N ( μ , Σ ) \boldsymbol x\sim N(\boldsymbol \mu,\bold \Sigma) x∼N(μ,Σ)，其中 x , μ ∈ R n , Σ ∈ R n × n \boldsymbol x,\boldsymbol \mu\in \mathbb{R}^n, \bold \Sigma\in \mathbb{R}^{n\times n} x,μ∈Rn,Σ∈Rn×n

f ( x ) = 1 ( 2 π ) n d e t ( Σ ) e x p ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) f(\boldsymbol x)=\frac{1}{\sqrt{(2\pi)^ndet(\bold \Sigma)}}\mathrm{exp}\Big(-\frac{1}{2}(\boldsymbol x-\boldsymbol \mu)^T\mathbf{\Sigma}^{-1}(\boldsymbol x-\boldsymbol \mu)\Big) f(x)=(2π)ndet(Σ) ​1​exp(−21​(x−μ)TΣ−1(x−μ))

在优化中一般使用其负对数形式：

− l n [ f ( x ) ] = C + 1 2 ( x − μ ) T Σ − 1 ( x − μ ) -ln[f(\boldsymbol x)]=\boldsymbol C+\frac{1}{2}(\boldsymbol x-\boldsymbol \mu)^T\mathbf{\Sigma}^{-1}(\boldsymbol x-\boldsymbol \mu) −ln[f(x)]=C+21​(x−μ)TΣ−1(x−μ)
其中二次型 1 2 ( x − μ ) T Σ − 1 ( x − μ ) \frac{1}{2}(\boldsymbol x-\boldsymbol \mu)^T\mathbf{\Sigma}^{-1}(\boldsymbol x-\boldsymbol \mu) 21​(x−μ)TΣ−1(x−μ)称为马氏距离，就是一种最小二乘项。
要求 f ( x ) f(\boldsymbol{x}) f(x)的最大值，即是要求马氏距离的最小值。

1.3 牛顿迭代法

牛顿法是一种在实数域和复数域上近似求解方程的方法，原理大致如下

以最简单的一元函数为例，若要求一元方程 f ( x ) = 0 f(x)=0 f(x)=0的解，可先随机选取一个初值值 x 0 x_0 x0​，但显然 f ( x 0 ) ≠ 0 f(x_0)≠0 f(x0​)​=0，那么我们对函数 f ( x ) f(x) f(x)关于 x 0 x_0 x0​进行一阶泰勒展开：

f ( x 1 ) ≈ f ( x 0 ) + f ′ ( x 0 ) ( x 1 − x 0 ) f(x_1)≈f(x_0)+f'(x_0)(x_1-x_0) f(x1​)≈f(x0​)+f′(x0​)(x1​−x0​)

我们希望找到 f ( x 1 ) = 0 f(x_1)=0 f(x1​)=0，即

f ( x 0 ) + f ′ ( x 0 ) ( x 1 − x 0 ) = 0 f(x_0)+f'(x_0)(x_1-x_0)=0 f(x0​)+f′(x0​)(x1​−x0​)=0
所以有
x 1 = x 0 − f ( x 0 ) f ′ ( x 0 ) x_1=x_0-\frac{f(x_0)}{f'(x_0)} x1​=x0​−f′(x0​)f(x0​)​
此为一轮迭代
之后可以重复以上方法往下找 x 2 , x 3 , ⋅ ⋅ ⋅ x_2,x_3,··· x2​,x3​,⋅⋅⋅，直到误差允许的范围内为止
过程大致如下图所示

牛顿迭代法 我的思考:

假如所求方程为 x 2 − 1 = 0 x^2-1=0 x2−1=0，那么此时的迭代公式为

x 1 = x 0 − f ( x 0 ) f ′ ( x 0 ) = x 0 − x 0 2 − 1 2 x 0 = x 0 2 + 1 2 x 0 x_1=x_0-\frac{f(x_0)}{f'(x_0)}=x_0-\frac{x_0^2-1}{2x_0}=\frac{x_0}{2}+\frac{1}{2x_0} x1​=x0​−f′(x0​)f(x0​)​=x0​−2x0​x02​−1​=2x0​​+2x0​1​

但若我取初值 x 0 = 0 x_0=0 x0​=0，会导迭代时 x 1 → ∞ x_1\rightarrow\infty x1​→∞，这样牛顿法是不是就失效了？

二、状态估计问题

回顾滤波器中的贝叶斯法则：

P ( x │ z ) = P ( z ∣ x ) P ( x ) P ( z ) ∝ P ( z ∣ x ) P ( x ) P(x│z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x) P(x│z)=P(z)P(z∣x)P(x)​∝P(z∣x)P(x)

现在目标是要去求解状态变量(这里将待求的位姿信息和路标信息全部整合到一个变量中) x = { x 1 , ⋅ ⋅ ⋅ , x n , y 1 , ⋅ ⋅ ⋅ , y m } \boldsymbol x={\{x_1,···,x_n,y_1,···,y_m\}} x={x1​,⋅⋅⋅,xn​,y1​,⋅⋅⋅,ym​}的值

由于 P ( x │ z ) P(x│z) P(x│z)比较难直接表示，所以要借助两种办法

1. 最大后验估计 (Maximize a Posterior)

x M A P ∗ = arg ⁡ max ⁡ P ( x ∣ z ) = arg ⁡ max ⁡ P ( z ∣ x ) P ( x ) \boldsymbol x_{MAP}^*=\arg\max P(x|z)=\arg\max P(z|x)P(x) xMAP∗​=argmaxP(x∣z)=argmaxP(z∣x)P(x)
2) 最大似然估计 (Maximize Likelhood Estimation)

x M L E ∗ = arg ⁡ max ⁡ P ( z ∣ x ) \boldsymbol x_{MLE}^*=\arg\max P(z|x) xMLE∗​=argmaxP(z∣x)

来求出 x \boldsymbol x x的状况

先以最简单的形式来举例，假设噪声都是高斯噪声：
v k , j ∼ N ( 0 , Q k , j ) \boldsymbol v_{k,j}\sim N(\boldsymbol 0,\boldsymbol Q_{k,j}) vk,j​∼N(0,Qk,j​)
根据观测方程
z k , j = h ( y j , x k ) + v k , j \boldsymbol z_{k,j}=h(\boldsymbol y_j,\boldsymbol x_k)+\boldsymbol v_{k,j} zk,j​=h(yj​,xk​)+vk,j​
于是
z − h ( x ) ∼ N ( 0 , Q ) o r z ∼ N ( h ( x ) , Q ) o r P ( z ∣ x ) = N ( h ( x ) , Q ) \boldsymbol z-h(\boldsymbol x)\sim N(0,\boldsymbol Q) \\ or \\ \boldsymbol z\sim N(h(\boldsymbol x),\boldsymbol Q) \\ or \\ \mathrm{P}(\boldsymbol z|\boldsymbol x)=N(h(\boldsymbol x),\boldsymbol Q) z−h(x)∼N(0,Q)orz∼N(h(x),Q)orP(z∣x)=N(h(x),Q)

求解最大似然估计：

x ∗ = arg ⁡ max ⁡ P ( z ∣ x ) = arg ⁡ min ⁡ ( ( z − h ( x ) ) T Q − 1 ( z − h ( x ) ) ) = arg ⁡ min ⁡ ( v T Q − 1 v ) \begin{aligned} \boldsymbol x^*&=\arg\max P(z|x) \\ &=\arg\min\Bigg(\Big(\boldsymbol z-h(\boldsymbol x)\Big)^T\bold Q^{-1}\Big(\boldsymbol z-h(\boldsymbol x)\Big)\Bigg) \\ &=\arg\min(\boldsymbol v^T\bold Q^{-1}\boldsymbol v) \end{aligned} x∗​=argmaxP(z∣x)=argmin((z−h(x))TQ−1(z−h(x)))=argmin(vTQ−1v)​
说明求后验概率最大化等价于求误差的最小二乘

对于slam模型的两个方程的噪声项(之后记为误差项)

e u , k = x k − f ( x k − 1 , u k ) e z , k , j = z k , j − h ( y j , x k ) \begin{aligned} \boldsymbol e_{\boldsymbol u,k}&=\boldsymbol x_k-f(\boldsymbol x_{k-1},\boldsymbol u_k) \\ \boldsymbol e_{\boldsymbol z,k,j}&=\boldsymbol z_{k,j}-h(\boldsymbol y_j,\boldsymbol x_k) \end{aligned} eu,k​ez,k,j​​=xk​−f(xk−1​,uk​)=zk,j​−h(yj​,xk​)​

对所有误差的马氏距离求和(构造惩罚函数)：

min ⁡ J ( x , y ) = ∑ k e u , k T R k − 1 e u , k + ∑ k ∑ j e z , k , j T Q k , j − 1 e z , k , j \min J(\boldsymbol x, \boldsymbol y)=\sum_k\boldsymbol e_{\boldsymbol u,k}^T\boldsymbol R_k^{-1}\boldsymbol e_{\boldsymbol u,k}+\sum_k\sum_j\boldsymbol e_{\boldsymbol z,k,j}^T\boldsymbol Q_{k,j}^{-1}\boldsymbol e_{\boldsymbol z,k,j} minJ(x,y)=k∑​eu,kT​Rk−1​eu,k​+k∑​j∑​ez,k,jT​Qk,j−1​ez,k,j​

三、非线性最小二乘

对于求解最小二乘问题的极值相当于也是利用迭代的思想去逼近最优解

min ⁡ x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 \min \limits_{\boldsymbol x} F(\boldsymbol x)=\frac{1}{2}\|f(\boldsymbol x)\|^2_2 xmin​F(x)=21​∥f(x)∥22​

3.1 一阶梯度法（最速下降法）

将目标函数 F ( x ) F(\boldsymbol x) F(x)在 x k x_k xk​附近作一阶泰勒展开

F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k F(\boldsymbol x_k+\Delta\boldsymbol x_k)≈F(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k F(xk​+Δxk​)≈F(xk​)+J(xk​)TΔxk​
其中

J ( x ) = ∇ x F ( x ) = ∂ F ( x ) ∂ x \boldsymbol J(\boldsymbol x)=\nabla_{\boldsymbol x}F(\boldsymbol x)=\frac{\partial F(\boldsymbol x)}{\partial \boldsymbol x} J(x)=∇x​F(x)=∂x∂F(x)​

取梯度反方向，做梯度下降

Δ x k ∗ = − J ( x k ) \Delta\boldsymbol x^*_k=-\boldsymbol J(\boldsymbol x_k) Δxk∗​=−J(xk​)

方法缺点：
容易产生ZigZag现象（走锯齿路线），导致迭代次数增加

3.2 二阶梯度法（牛顿法）

将目标函数 F ( x ) F(\boldsymbol x) F(x)在 x k x_k xk​附近作二阶泰勒展开

F ( x k + Δ x k ) ≈ F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) Δ x k F(\boldsymbol x_k+\Delta\boldsymbol x_k)≈F(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k+\frac{1}{2}\Delta\boldsymbol x_k^T\boldsymbol H(\boldsymbol x_k)\Delta\boldsymbol x_k F(xk​+Δxk​)≈F(xk​)+J(xk​)TΔxk​+21​ΔxkT​H(xk​)Δxk​

其中

H ( x ) = ∇ x 2 F ( x ) = ∂ 2 F ( x ) ∂ x ∂ x T \boldsymbol H(\boldsymbol x)=\nabla^2_{\boldsymbol x}F(\boldsymbol x)=\frac{\partial^2 F(\boldsymbol x)}{\partial\boldsymbol x\partial\boldsymbol x^T} H(x)=∇x2​F(x)=∂x∂xT∂2F(x)​

所以

Δ x k ∗ = arg ⁡ min ⁡ F ( x k + Δ x k ) ≈ arg ⁡ min ⁡ ( F ( x k ) + J ( x k ) T Δ x k + 1 2 Δ x k T H ( x k ) Δ x k ) \begin{aligned} \Delta\boldsymbol x^*_k&=\arg\min F(\boldsymbol x_k+\Delta\boldsymbol x_k) \\ &≈\arg\min\Big(F(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k+\frac{1}{2}\Delta\boldsymbol x_k^T\boldsymbol H(\boldsymbol x_k)\Delta\boldsymbol x_k\Big) \end{aligned} Δxk∗​​=argminF(xk​+Δxk​)≈argmin(F(xk​)+J(xk​)TΔxk​+21​ΔxkT​H(xk​)Δxk​)​

将二阶泰勒展开式对 Δ x k \Delta\boldsymbol x_k Δxk​求导，并令其等于0，得

J ( x k ) + H ( x k ) Δ x k = 0 ⇒ H Δ x = − J \boldsymbol J(\boldsymbol x_k)+\boldsymbol H(\boldsymbol x_k)\Delta\boldsymbol x_k=0 \\ \Rightarrow\boldsymbol H\Delta\boldsymbol x=-\boldsymbol J J(xk​)+H(xk​)Δxk​=0⇒HΔx=−J

方法缺点：
求解Hessian矩阵 H \boldsymbol H H过于复杂

3.3 高斯-牛顿法

…未完待续