视觉SLAM笔记(三)-非线性优化
SLAM问题中，机器人自身的状态估计主要有两种方式: 基于滤波器的方法(Kalman filter, Particle filter等)和基于非线性优化的方法，目前主流的SLAM方案均使用了非线性优化方法
状态估计
经典SLAM模型主要由运动模型和观测模型组成，其方程如下
x k = f ( x k − 1 , u k ) + w k z k , j = h ( y j , x k ) + v k , j {x}_{k}=f\left({x}_{k-1}, {u}_{k}\right)+{w}_{k}\\ {z}_{k, j}=h\left({y}_{j}, {x}_{k}\right)+{v}_{k, j} xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j
在传统的基于滤波器的方案中其状态估计分为两个步骤: 运动更新与观测更新，从概率学的角度看，是在已知输入数据 u u u和观测数据 z z z的条件下，计算状态 x x x的条件概率分布， x = { x 1 , . . . , x N , y 1 , . . . , y M } x=\lbrace x_1,...,x_N,y_1,...,y_M \rbrace x={x1,...,xN,y1,...,yM}，包括了机器人自身位姿以及环境中的路标点，利用贝叶斯法则:
P ( x ∣ z ) = P ( z ∣ x ) P ( x ) P ( z ) P(x|z)=\frac{P(z|x)P(x)}{P(z)} P(x∣z)=P(z)P(z∣x)P(x)
其中 P ( x ∣ z ) P(x|z) P(x∣z)为后验概率， P ( z ∣ x ) P(z|x) P(z∣x)为似然， P ( x ) P(x) P(x)为先验，此时的目标为求解一个 x x x使得后验概率最大
x M A P ∗ = arg max P ( x ∣ z ) = arg max P ( z ∣ x ) P ( x ) {x}^{*}_{M A P}=\arg \max P({x} | {z})=\arg \max P({z} | {x}) P({x}) xMAP∗=argmaxP(x∣z)=argmaxP(z∣x)P(x)
未知先验的情况下，求解最大似然问题，即在什么状态下最可能产生当前的观测数据
x M L E ∗ = arg max P ( z ∣ x ) x^*_{MLE}=\arg \max P(z|x) xMLE∗=argmaxP(z∣x)
对上述的观测模型 z k , j = h ( y j , x k ) + v k , j z_{k,j}=h(y_j,x_k)+v_{k,j} zk,j=h(yj,xk)+vk,j，假设 v k , j ∼ N ( 0 , Q k , j ) v_{k,j} \sim N(0,Q_{k,j}) vk,j∼N(0,Qk,j)，则似然如下
P ( z j , k ∣ x k , y j ) = N ( h ( y j , x k ) , Q k , j ) P\left(z_{j, k} | {x}_{k}, {y}_{j}\right)=N\left(h\left({y}_{j},{x}_{k}\right), {Q}_{k, j}\right) P(zj,k∣xk,yj)=N(h(yj,xk),Qk,j)
其概率密度展开形式为(这是一个高斯分布):
P ( x ) = 1 ( 2 π ) N det ( Σ ) exp ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) P({x})=\frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det}({\Sigma})}} \exp \left(-\frac{1}{2}({x}-{\mu})^{T} {\Sigma}^{-1}({x}-{\mu})\right) P(x)=(2π)Ndet(Σ)1exp(−21(x−μ)TΣ−1(x−μ))
对原函数最大化即对其负对数最小化
− ln ( P ( x ) ) = 1 2 ln ( ( 2 π ) N det ( Σ ) ) + 1 2 ( x − μ ) T Σ − 1 ( x − μ ) -\ln (P({x}))=\frac{1}{2} \ln \left((2 \pi)^{N} \operatorname{det}({\Sigma})\right)+\frac{1}{2}({x}-{\mu})^{T} {\Sigma}^{-1}({x}-{\mu}) −ln(P(x))=21ln((2π)Ndet(Σ))+21(x−μ)TΣ−1(x−μ)
省去第一项，得
x ∗ = arg min ( ( z k , j − h ( x k , y j ) ) T Q k , j − 1 ( z k , j − h ( x k , y j ) ) ) {x}^{*}=\arg \min \left(\left({z}_{k, j}-h\left({x}_{k}, {y}_{j}\right)\right)^{T} {Q}_{k, j}^{-1}\left({z}_{k, j}-h\left({x}_{k},{y}_{j}\right)\right)\right) x∗=argmin((zk,j−h(xk,yj))TQk,j−1(zk,j−h(xk,yj)))
这也是一个广义上得最小二乘问题，定义两个误差项，分别对应于运动模型和观测模型
e v , k = x k − f ( x k − 1 , u k ) e y , j , k = z k , j − h ( x k , y j ) e_{v, k}=x_{k}-f\left(x_{k-1}, u_{k}\right)\\ e_{y, j, k}=z_{k, j}-h\left(x_{k}, y_{j}\right) ev,k=xk−f(xk−1,uk)ey,j,k=zk,j−h(xk,yj)
最终误差函数如下:
J ( x ) = ∑ k e v , k T R k − 1 e v , k + ∑ k ∑ j e y , k , j T Q k , j − 1 e y , k , j J({x})=\sum_{k}{e}_{v, k}^{T}{R}_{k}^{-1}{e}_{v, k}+\sum_{k} \sum_{j} {e}_{y, k, j}^{T} {Q}_{k, j}^{-1} {e}_{y, k, j} J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,k,jTQk,j−1ey,k,j
这里即将状态估计问题转换为了一个最小二乘问题
非线性优化
如上所述，状态估计问题为一个非线性最小二乘问题，不方便直接用导数为零的方式求解，故通常用迭代的方式求解，其步骤如下
- 给定初值 x 0 x_0 x0
- 对于第 k k k次迭代，寻找一个增量 Δ x k \Delta x_k Δxk，使得 ∥ f ( x k + Δ x k ) ∥ 2 2 \left\|f\left({x}_{k}+\Delta {x}_{k}\right)\right\|_{2}^{2} ∥f(xk+Δxk)∥22达到极小值
- 若 Δ x k \Delta x_k Δxk足够小则停止
- 否则 x k + 1 = x k + Δ x k x_{k+1}=x_{k}+\Delta x_{k} xk+1=xk+Δxk，返回2
步骤内的主要问题在于如何寻找 Δ x \Delta x Δx
一阶和二阶梯度法
将原目标函数 ∥ f ( x + Δ x ) ∥ 2 2 \left\|f\left({x}+\Delta {x}\right)\right\|_{2}^{2} ∥f(x+Δx)∥22泰勒展开:
∥ f ( x + Δ x ) ∥ 2 2 ≈ ∥ f ( x ) ∥ 2 2 + J ( x ) Δ x + 1 2 Δ x T H Δ x \|f({x}+\Delta {x})\|_{2}^{2} \approx\|f({x})\|_{2}^{2}+{J}({x}) \Delta {x}+\frac{1}{2} \Delta {x}^{T} {H} \Delta {x} ∥f(x+Δx)∥22≈∥f(x)∥22+J(x)Δx+21ΔxTHΔx
一阶梯度法
一阶梯度法即最速下降法，保留目标函数的一阶梯度，如下:
∥ f ( x + Δ x ) ∥ 2 2 − ∥ f ( x ) ∥ 2 2 = J ( x ) Δ x \|f({x}+\Delta {x})\|_{2}^{2} -\|f({x})\|_{2}^{2}={J}({x}) \Delta {x} ∥f(x+Δx)∥22−∥f(x)∥22=J(x)Δx
由于 J ( x ) J(x) J(x)和 Δ x \Delta x Δx均为向量，故取 Δ x = − J T ( x ) \Delta x = -J^T(x) Δx=−JT(x)时下降最快
但最速下降法实际上并不一定时“最速”，因为其优化路线很可能时锯齿形(即所谓的zigzag现象)，反而增加了迭代次数。
二阶梯度法
二阶梯度法即牛顿法，求目标函数的泰勒展开关于 Δ x \Delta x Δx的导数并使其为0，可得:
H Δ x = − J T ( x ) H \Delta x = -J^T(x) HΔx=−JT(x)
牛顿法的缺陷主要在于需要计算Hessian矩阵，对于较大规模的SLAM问题求解起来比较困难，通常倾向于避免H的计算
Gauss-Newton
高斯牛顿法并非对 ∥ f ( x ) ∥ 2 2 \|f({x})\|_{2}^{2} ∥f(x)∥22做泰勒展开，而是对 f ( x ) f(x) f(x)做泰勒展开
f ( x + Δ x ) ≈ f ( x ) + J ( x ) Δ x f(x+\Delta x) \approx f(x)+J(x) \Delta x f(x+Δx)≈f(x)+J(x)Δx
注意这里的 J ( x ) J(x) J(x)为 f ( x ) f(x) f(x)的雅可比矩阵，而最速下降法中 J ( x ) J(x) J(x)为 ∥ f ( x ) ∥ 2 2 \|f({x})\|_{2}^{2} ∥f(x)∥22的雅可比矩阵，现在展开目标函数的平方项:
∥ f ( x ) + J ( x ) Δ x ∥ 2 = ( f ( x ) + J ( x ) Δ x ) T ( f ( x ) + J ( x ) Δ x ) = ( ∥ f ( x ) ∥ 2 2 + 2 f ( x ) T J ( x ) Δ x + Δ x T J ( x ) T J ( x ) Δ x ) \begin{aligned} \|f({x})+{J}({x}) \Delta {x}\|^{2} &=(f({x})+{J}({x}) \Delta {x})^{T}(f({x})+{J}({x}) \Delta {x}) \\ &=\left(\|f({x})\|_{2}^{2}+2 f({x})^{T}{J}({x}) \Delta {x}+\Delta {x}^{T} {J}({x})^{T} {J}({x}) \Delta {x}\right) \end{aligned} ∥f(x)+J(x)Δx∥2=(f(x)+J(x)Δx)T(f(x)+J(x)Δx)=(∥f(x)∥22+2f(x)TJ(x)Δx+ΔxTJ(x)TJ(x)Δx)
求上式关于 Δ x \Delta x Δx的导数，并令其为0，得:
J ( x ) T J ( x ) Δ x = − J ( x ) T f ( x ) J(x)^{T} J(x) \Delta x=-J(x)^{T} f(x) J(x)TJ(x)Δx=−J(x)Tf(x)
实际上高斯牛顿法是以 J T J J^TJ JTJ作为了Hessian矩阵的近似，避免了直接计算二阶导数
由于 J T J J^TJ JTJ是一个半正定矩阵，即在实际中可能行列式为0或接近0，这使得计算出的 Δ x \Delta x Δx可能过大，导致稳定性不好，难以收敛
Levenberg-Marquardt Method
Levenberg-Marquardt方法主要是解决高斯牛顿法的缺点，其增量计算方程如下:
( J ( x ) T J ( x ) + λ D T D ) Δ x = − J ( x ) T f ( x ) (J(x)^{T} J(x)+\lambda D^TD) \Delta x=-J(x)^{T} f(x) (J(x)TJ(x)+λDTD)Δx=−J(x)Tf(x)
Levenberg将 D D D取为单位阵 I I I，将 Δ x \Delta x Δx约束在一个球形范围内，而Marquardt将 D D D取为非负数对角阵(通常用 J T J J^TJ JTJ的对角元素平方根表示)，使得在梯度小的维度上约束范围大
当 λ \lambda λ较小时，该方法更类似于高斯牛顿法，而 λ \lambda λ较大时则更类似于最速下降法。
参考文献
[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲：从理论到实践, 电子工业出版社, 2017
[2] https://vision.in.tum.de/teaching/ss2019/mvg2019