视觉SLAM笔记(三)-非线性优化

SLAM问题中,机器人自身的状态估计主要有两种方式: 基于滤波器的方法(Kalman filter, Particle filter等)和基于非线性优化的方法,目前主流的SLAM方案均使用了非线性优化方法

状态估计

经典SLAM模型主要由运动模型和观测模型组成,其方程如下
xk=f(xk1,uk)+wkzk,j=h(yj,xk)+vk,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}
在传统的基于滤波器的方案中其状态估计分为两个步骤: 运动更新与观测更新,从概率学的角度看,是在已知输入数据uu和观测数据zz的条件下,计算状态xx的条件概率分布,x={x1,...,xN,y1,...,yM}x=\lbrace x_1,...,x_N,y_1,...,y_M \rbrace,包括了机器人自身位姿以及环境中的路标点,利用贝叶斯法则:
P(xz)=P(zx)P(x)P(z) P(x|z)=\frac{P(z|x)P(x)}{P(z)}
其中P(xz)P(x|z)为后验概率,P(zx)P(z|x)为似然,P(x)P(x)为先验,此时的目标为求解一个xx使得后验概率最大
xMAP=argmaxP(xz)=argmaxP(zx)P(x) {x}^{*}_{M A P}=\arg \max P({x} | {z})=\arg \max P({z} | {x}) P({x})
未知先验的情况下,求解最大似然问题,即在什么状态下最可能产生当前的观测数据
xMLE=argmaxP(zx) x^*_{MLE}=\arg \max P(z|x)
对上述的观测模型zk,j=h(yj,xk)+vk,jz_{k,j}=h(y_j,x_k)+v_{k,j},假设vk,jN(0,Qk,j)v_{k,j} \sim N(0,Q_{k,j}),则似然如下
P(zj,kxk,yj)=N(h(yj,xk),Qk,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(x)=1(2π)Ndet(Σ)exp(12(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)
对原函数最大化即对其负对数最小化
ln(P(x))=12ln((2π)Ndet(Σ))+12(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})
省去第一项,得
x=argmin((zk,jh(xk,yj))TQk,j1(zk,jh(xk,yj))) {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)
这也是一个广义上得最小二乘问题,定义两个误差项,分别对应于运动模型和观测模型
ev,k=xkf(xk1,uk)ey,j,k=zk,jh(xk,yj) 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)
最终误差函数如下:
J(x)=kev,kTRk1ev,k+kjey,k,jTQk,j1ey,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}
这里即将状态估计问题转换为了一个最小二乘问题

非线性优化

如上所述,状态估计问题为一个非线性最小二乘问题,不方便直接用导数为零的方式求解,故通常用迭代的方式求解,其步骤如下

  1. 给定初值x0x_0
  2. 对于第kk次迭代,寻找一个增量Δxk\Delta x_k,使得f(xk+Δxk)22\left\|f\left({x}_{k}+\Delta {x}_{k}\right)\right\|_{2}^{2}达到极小值
  3. Δxk\Delta x_k足够小则停止
  4. 否则xk+1=xk+Δxkx_{k+1}=x_{k}+\Delta x_{k},返回2
    步骤内的主要问题在于如何寻找Δx\Delta x

一阶和二阶梯度法

将原目标函数f(x+Δx)22\left\|f\left({x}+\Delta {x}\right)\right\|_{2}^{2}泰勒展开:
f(x+Δx)22f(x)22+J(x)Δx+12ΔxTHΔ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)22f(x)22=J(x)Δx \|f({x}+\Delta {x})\|_{2}^{2} -\|f({x})\|_{2}^{2}={J}({x}) \Delta {x}
由于J(x)J(x)Δx\Delta x均为向量,故取Δx=JT(x)\Delta x = -J^T(x)时下降最快
但最速下降法实际上并不一定时“最速”,因为其优化路线很可能时锯齿形(即所谓的zigzag现象),反而增加了迭代次数。

二阶梯度法

二阶梯度法即牛顿法,求目标函数的泰勒展开关于Δx\Delta x的导数并使其为0,可得:
HΔx=JT(x) H \Delta x = -J^T(x)
牛顿法的缺陷主要在于需要计算Hessian矩阵,对于较大规模的SLAM问题求解起来比较困难,通常倾向于避免H的计算

Gauss-Newton

高斯牛顿法并非对f(x)22\|f({x})\|_{2}^{2}做泰勒展开,而是对f(x)f(x)做泰勒展开
f(x+Δx)f(x)+J(x)Δx f(x+\Delta x) \approx f(x)+J(x) \Delta x
注意这里的J(x)J(x)f(x)f(x)的雅可比矩阵,而最速下降法中J(x)J(x)f(x)22\|f({x})\|_{2}^{2}的雅可比矩阵,现在展开目标函数的平方项:
f(x)+J(x)Δx2=(f(x)+J(x)Δx)T(f(x)+J(x)Δx)=(f(x)22+2f(x)TJ(x)Δx+ΔxTJ(x)TJ(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}
求上式关于Δx\Delta x的导数,并令其为0,得:
J(x)TJ(x)Δx=J(x)Tf(x) J(x)^{T} J(x) \Delta x=-J(x)^{T} f(x)
实际上高斯牛顿法是以JTJJ^TJ作为了Hessian矩阵的近似,避免了直接计算二阶导数
由于JTJJ^TJ是一个半正定矩阵,即在实际中可能行列式为0或接近0,这使得计算出的Δx\Delta x可能过大,导致稳定性不好,难以收敛

Levenberg-Marquardt Method

Levenberg-Marquardt方法主要是解决高斯牛顿法的缺点,其增量计算方程如下:
(J(x)TJ(x)+λDTD)Δx=J(x)Tf(x) (J(x)^{T} J(x)+\lambda D^TD) \Delta x=-J(x)^{T} f(x)
Levenberg将DD取为单位阵II,将Δx\Delta x约束在一个球形范围内,而Marquardt将DD取为非负数对角阵(通常用JTJJ^TJ的对角元素平方根表示),使得在梯度小的维度上约束范围大
λ\lambda较小时,该方法更类似于高斯牛顿法,而λ\lambda较大时则更类似于最速下降法。

参考文献

[1] 高翔, 张涛, 颜沁睿, 刘毅, 视觉SLAM十四讲:从理论到实践, 电子工业出版社, 2017
[2] https://vision.in.tum.de/teaching/ss2019/mvg2019

发布了7 篇原创文章 · 获赞 4 · 访问量 430
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 编程工作室 设计师: CSDN官方博客

分享到微信朋友圈

×

扫一扫,手机浏览