第六讲:非线性优化
文章目录
在前面几章,我们介绍了经典 SLAM 模型的运动方程和观测方程。现在我们已经知道,方程中的位姿可以由变换矩阵来描述,然后用李代数进行优化。然而,由于噪声的存在,运动方程和观测方程的等式必定不是精确成立的。
1. 状态估计问题
1.1. 最大后验与最大似然
第二讲中介绍过经典的slam模型由一个运动方程和一个观测方程构成:
{
x
k
=
f
(
x
k
−
1
,
u
k
,
w
k
)
+
w
k
w
k
为
运
动
方
程
噪
声
z
k
,
j
=
h
(
y
j
,
x
k
,
v
k
,
j
)
−
v
k
,
j
v
k
,
j
为
观
测
方
程
噪
声
\begin{cases} x_k = f(x_{k-1},u_k,w_k)+w_k \qquad w_k 为运动方程噪声\\ z_{k,j}=h(y_j,x_k,v_{k,j})-v_{k,j} \qquad v_{k,j}为观测方程噪声 \end{cases}
{xk=f(xk−1,uk,wk)+wkwk为运动方程噪声zk,j=h(yj,xk,vk,j)−vk,jvk,j为观测方程噪声
通常假设两个噪声满足均匀高斯分布:
w
k
∼
N
(
0
,
R
k
)
,
v
k
∼
N
(
0
,
Q
k
,
j
)
w_k \sim N(0,R_k), \quad v_k \sim N(0,Q_{k,j})
wk∼N(0,Rk),vk∼N(0,Qk,j)
从概率学的角度看,在非线性优化中把所有的待估计变量放在一个状态变量中:
x
=
{
x
1
,
⋯
,
x
N
,
y
1
,
⋯
,
y
M
}
x=\{x_1,\cdots,x_N,y_1,\cdots,y_M\}
x={x1,⋯,xN,y1,⋯,yM}
现在对机器人的状态估计,就是已知输入数据
u
u
u,和观测数据
z
z
z的条件下, 求计算机状态
x
x
x的条件概率分布:
P
(
x
∣
z
,
u
)
P(x|z,u)
P(x∣z,u)
当没有测量数据,即只考虑z时,相当于估计 P ( x ∣ z ) P(x|z) P(x∣z)的条件概率分布.
如果忽略图像在时间上的联系,把他们看着一堆彼此没有关系的图片,该问题称为Sructure from Motion(sfm),即如何从许多图像中重建三维结构.
SLAM可以看做是图像具有时间先后顺序,需要实时求解一个sfm问题.
为了估计状态变量的条件分布,利用贝叶斯公式:
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(z|x) 为
似然 - P(x) 为
先验
直接求后验分布是困难的, 但是求一个状态最优估计,使得该状态下后验概率最大化,则可进行:
x M A P ∗ = arg max P ( x ∣ z ) = arg max P ( z ∣ x ) P ( x ) x^*_{MAP} = \arg\max P(x|z) = \argmax P(z|x)P(x) xMAP∗=argmaxP(x∣z)=argmaxP(z∣x)P(x)
x ∗ = arg max f ( x ) x^*=\argmax f(x) x∗=argmaxf(x)表示当x=x^*时,f(x)取得最大值.
求解最大后验概率 相当于最大化似然和先验的乘积
如果不知道机器人的位姿大概在什么位置,可以求解x的最大释然估计:
x
M
A
P
∗
=
arg max
P
(
z
∣
x
)
x^*_{MAP} = \argmax P(z|x)
xMAP∗=argmaxP(z∣x)
似然: 指在现在的位姿下可能产生怎样的观测数据.
最大似然估计: 在什么样的状态下最可能产生观测到的数据.
1.2. 最小二乘的引出
对于一次观测:
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
由于假设了噪声项
u
k
∼
N
(
0
,
Q
k
,
j
)
u_k \sim N(0,Q_{k,j})
uk∼N(0,Qk,j),所以观测数据的条件概率为:
P
(
z
j
,
k
∣
x
k
,
y
j
)
=
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
P(z_{j,k}|x_k,y_j) = N(h(y_j,x_k),Q_{k,j})
P(zj,k∣xk,yj)=N(h(yj,xk),Qk,j)
上式依然是一个高斯分布,为了计算使它最大化的 想 x k ( 位 置 ) , y i ( 路 标 ) 想x_k(位置),y_i(路标) 想xk(位置),yi(路标),使用 最小负对数来求高斯分布的最大似然.
高斯分布
x
∼
N
(
μ
,
∑
)
x \sim N(\mu, \sum)
x∼N(μ,∑),它的概率密度展开形式为:
P
(
x
)
=
1
(
2
π
)
N
det
(
∑
)
exp
(
−
(
x
−
μ
)
T
2
∑
(
x
−
μ
)
)
P(x) = \frac{1}{\sqrt {(2\pi)^N \det(\sum) }} \exp(-\frac{(x- \mu)^T}{2\sum{(x-\mu)}})
P(x)=(2π)Ndet(∑)1exp(−2∑(x−μ)(x−μ)T)
对其取负对数
− ln ( P ( x ) ) = l n ( ( 2 π ) N det ( ∑ ) ) 2 + ( x − μ ) T 2 ∑ ( x − μ ) -\ln(P(x)) = \frac{ln((2\pi)^N\det(\sum))}{2} + \frac{(x- \mu)^T}{2\sum{(x-\mu)}} −ln(P(x))=2ln((2π)Ndet(∑))+2∑(x−μ)(x−μ)T
∑ \sum ∑ 表示协方差矩阵
det \det det 表示求行列式的值
对原分布求最大化相当于对负对数求最小化. ⋯ \cdots ⋯
定义数据和估计值之间的误差.
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(x_{k-1},u_k) \newline e_{y,j,k} = z_{k,j} -h(x_k,y_j)
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}^TR_k^{-1}e_{v,k}} + \sum_k\sum_j{ e^T_{y,k,j}Q^{-1}_{k.j}e_{y,k,j} }
J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,k,jTQk.j−1ey,k,j
这样得到了总体意义下的最小二乘问题. 它的最优解等价于状态的最大似然估计.
由于噪声的存在,我们把估计的轨迹带入SLAM的运动,观测方程时,他们并不会完美成立. 我们对状态估计进行微调,使得整体误差下降一些.一般会得到一个极小值. 这就是一个典型的非线性优化过程.
1.3. 非线性最小二乘法
一个简单的最小二乘问题:
min
1
2
∣
∣
f
(
x
)
∣
∣
2
2
\min \frac{1}{2}||f(x)||_2^2
min21∣∣f(x)∣∣22
对x求导可以得到极值:
d
(
1
2
∣
∣
f
(
x
)
∣
∣
2
2
)
d
x
=
0
\frac{d(\frac{1}{2}||f(x)||^2_2)}{dx} =0
dxd(21∣∣f(x)∣∣22)=0
如果 f ( x ) f(x) f(x)比较复杂,那么用求导的方法是比较困难的.
对于不方便直接求解的最小二乘问题,可以用迭代的方式:
- 给定某个初始值 x 0 x_0 x0
- 对于第 k k k次迭代,寻找一个增量 Δ x k \Delta x_k Δxk, 使得 ∣ ∣ f ( x k + Δ x k ) ∣ ∣ 2 2 ||f(x_k+\Delta x_k)||^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,返回第二步
关键问题是增量 Δ x k \Delta x_k Δxk 如何确定.
在slam中有两类方法.下面将介绍这两种方法.
1.4. 一阶和二阶梯度法
将目标函数在
x
x
x附近进行泰勒展开:
∥
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^TH \Delta x
∥f(x+Δx)∥22≈∥f(x)∥22+J(x)Δx+21ΔxTHΔx
J J J 是 ∥ f ( x ) ∥ 2 2 \|f(x)\|^2_2 ∥f(x)∥22关于x的导数, 而H则是二阶导数.
如果保留一阶梯度,那么增量的解为:
Δ
x
∗
=
−
J
T
(
x
)
\Delta x^* = -J^T(x)
Δx∗=−JT(x)
如果保留二阶梯度信息,增量方程为:
Δ
x
∗
=
arg min
∥
f
(
x
)
∥
2
2
+
J
(
x
)
Δ
x
+
1
2
Δ
x
T
H
Δ
x
\Delta x^* = \argmin \|f(x)\|^2_2 + J(x)\Delta x + \frac{1}{2}\Delta x^TH \Delta x
Δx∗=argmin∥f(x)∥22+J(x)Δx+21ΔxTHΔx
求右侧关于
Δ
x
\Delta x
Δx的导数并令它为0, 就得到了增量的解:
H
Δ
x
=
−
J
T
H \Delta x = -J^T
HΔx=−JT
该方法又称为牛顿法. 牛顿法需要计算目标函数的H矩阵,我们通常避免H的计算,接下来的方法即解决改问题.
1.5. 牛顿高斯法
思想: 将
非
(
x
)
非(x)
非(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)关于x的导数.
当前目前:寻找下降矢量:
Δ
x
\Delta x
Δx, 使得
∥
f
(
x
+
Δ
x
)
∥
2
2
\|f(x+ \Delta x)\|^2_2
∥f(x+Δx)∥22达到最小.
Δ
x
∗
=
arg min
1
2
∥
f
(
x
)
+
J
(
x
)
Δ
x
∥
2
\Delta x^* = \argmin \frac{1}{2}\|f(x)+J(x)\Delta x\|^2
Δx∗=argmin21∥f(x)+J(x)Δx∥2
展开平方项并对
Δ
x
\Delta x
Δx 求导,得:
J
(
x
)
T
J
(
x
)
Δ
x
=
−
J
(
x
)
T
f
(
x
)
J(x)^TJ(x)\Delta x = - J(x)^Tf(x)
J(x)TJ(x)Δx=−J(x)Tf(x)
这是一个线性方程, 被称为:增量方程或高斯牛顿方程或正规方程.
定义:
- J ( x ) T J ( x ) J(x)^TJ(x) J(x)TJ(x) 为 H H H
- J(x)^Tf(x) 为 g g g
上式可记为:
H
Δ
x
=
g
H \Delta x = g
HΔx=g
求解增量方程是整个优化过程的核心所在.
高斯牛顿法的步骤:
- 给定初始值 x 0 x_0 x0
- 对于第k次迭代, 求出当前雅可比矩阵 J ( x k ) J(x_k) J(xk)和误差 f ( x k ) f(x_k) f(xk)
- 求解增量方程: H Δ x k = g H \Delta x_k = g HΔxk=g
- 若 Δ 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,返回第二步
高斯牛顿法的缺点:
- 可能出现 J T J J^TJ JTJ为奇异矩阵或者病态
- 如果求出的 Δ x \Delta x Δx过大,也会导致算法不稳定.
在非线性优化里,相当多的方法都是高斯牛顿法的变种.
1.6. 列文伯格-马夸尔特方法
高是牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的效果, 所以我们可以给
Δ
x
\Delta x
Δx一个信赖区域.这种方法被称为信赖区域法.
如何确定信赖区域的范围?
根据我们的近似模型跟实际函数之间的差异来确定: 如果差异小,扩大范围,如果差异大,缩小范围.
因此使用
ρ
\rho
ρ来判断泰勒近似是否好:
ρ
=
f
(
x
+
Δ
x
)
−
f
(
x
)
J
(
x
)
Δ
x
\rho = \frac{f(x+\Delta x)-f(x)}{J(x)\Delta x}
ρ=J(x)Δxf(x+Δx)−f(x)
ρ \rho ρ接近于1 说明近似效果好.
如果 ρ \rho ρ太小,说明近似差,需要缩小范围, 反之放大范围.
于是有了一个改良版的非线性优化框架:
- 给定初始值 x 0 x_0 x0, 以及初始优化半径 μ \mu μ
- 对于第k次迭代,求解:
min Δ x k 1 2 ∥ f ( x k ) + J ( x k ) Δ x k ∥ 2 , s . t . ∥ D Δ x k ∥ 2 ⩽ μ \min \limits_{\Delta x_k} \frac{1}{2}\|f(x_k) + J(x_k)\Delta x_k \|^2 , \quad s.t. \quad \|D \Delta x_k \|^2 \leqslant \mu Δxkmin21∥f(xk)+J(xk)Δxk∥2,s.t.∥DΔxk∥2⩽μ- μ \mu μ 是信赖区域半径
- D 将会在后面说明
- 计算 ρ \rho ρ
- 若 ρ > 3 4 \rho > \frac{3}{4} ρ>43, 则 μ = 2 μ \mu = 2 \mu μ=2μ
- 若 ρ < 1 4 \rho < \frac{1}{4} ρ<41, 则 μ = 0.5 μ \mu = 0.5 \mu μ=0.5μ
- 如果 ρ \rho ρ大于阈值,则认为近似可行. 令 x k + 1 = x k + Δ x k x_{k+1}= x_k+\Delta x_k xk+1=xk+Δxk
- 判断算法是都收敛.如果不收敛返回第二步,否则结束.
这里的近似范围和阈值都是经验值.
用拉格朗日将它转化为无约束的优化问题:
min
Δ
x
k
1
2
∥
f
(
x
k
)
+
J
(
x
k
)
Δ
x
k
∥
2
+
λ
2
∥
D
Δ
x
∥
2
\min \limits_{\Delta x_k} \frac{1}{2}\|f(x_k) + J(x_k)\Delta x_k \|^2+\frac{\lambda}{2}\|D\Delta x\|^2
Δxkmin21∥f(xk)+J(xk)Δxk∥2+2λ∥DΔx∥2
这里的
λ
\lambda
λ 为拉格朗日乘子.把它展开:
(
H
+
λ
D
T
D
)
Δ
x
=
g
(H+\lambda D^TD)\Delta x =g
(H+λDTD)Δx=g
考虑简化形式
D
=
I
D= I
D=I :
(
H
+
λ
I
)
Δ
x
=
g
(H+\lambda I)\Delta x = g
(H+λI)Δx=g
- 当 λ \lambda λ 较小,H 占据主导地位,说明二次近似模型在该范围内较好,列文伯格-马夸而特方法更接近于高斯牛顿法.
- 当 λ \lambda λ 较大时, λ I \lambda I λI占据主导地位,列文伯格-马夸尔特方法更接近于一阶梯度下降法.
在实际问题中,有有许多方法来求解函数的增量.非线性优化的框架大体上可分为两类:
- Lince search : 先固定搜索方向.在该方向上寻找步长,以最速下降和高斯牛顿法为代表.
- Trust Region: 先固定搜索区域,在该区域内搜索最优点,以列文伯格-看马夸尔特方法为代表.
2. 实践: Ceres
主要介绍两个库:
- Ceres
- g2o
2.1. Ceres简介
Ceres面向通用的最小二乘求解.
在Ceres中定义优化变量 x x x和每个代价函数 f i f_i fi,再调用Ceres进行求解.设定梯度下降条件,Ceres会在优化之后将最优结果返回.
2.2. 安装Ceres
…
2.3. 使用Ceres拟合曲线
假设有一条去下方程:
y
=
exp
(
a
x
2
+
b
x
+
c
)
+
w
y = \exp(ax^2 +bx+c)+w
y=exp(ax2+bx+c)+w
w为高斯噪声
求解下面的最小二乘问题来估计曲线的参数.
min a , b , c 1 2 ∑ i = 1 N ∥ y i − exp ( a x i 2 + b x i + c ) ∥ 2 \min \limits_{a,b,c} \frac{1}{2}\sum_{i=1}^N \| y_i -\exp(ax_i^2+bx_i+c)\|^2 a,b,cmin21i=1∑N∥yi−exp(axi2+bxi+c)∥2
要估计的是变量是a,b,c,而不是x. 首先给出真实值,然后在真实值中添加噪声作为测试数据.
代码见/code/第六讲
3. 实践:g2o
它是一个基于图优化的库.图优化是一种将非线性优化和图论结合起来的优化方式.
3.1. 图优化的简单理论
一个图由若干定点以及连接顶点的边组成. 进而用顶点表示优化变量,用边表示误差.
3.2. 使用g2o拟合曲线
代码见/code/第六讲
8615
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
