SLAM模型:
x k = f ( x k − 1 , u k ) + w k z k , j = h ( y j , x k ) + v k , j \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} xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j
运动方程与输入的具体形式有关,观测方程由针孔模型给定。
数据受噪声影响,有:
w k ∼ N ( 0 , R k ) , v k ∼ N ( 0 , Q k , j ) \boldsymbol{w}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol{R}_k),\boldsymbol{v}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol Q_{k,j}) wk∼N(0,Rk),vk∼N(0,Qk,j)
通过带噪声的数据 z \boldsymbol z z和 u \boldsymbol u u推断位姿 x \boldsymbol x x和 y \boldsymbol y y(以及它们的概率分布),这构成了一个状态估计问题。
处理状态估计问题的方法有两种:
仅对当前时刻附近的一些轨迹进行优化,称为滑动窗口估计法
定义所有时刻的机器人位姿和路标点坐标:
x = x 1 , ⋯ , x N y = y 1 , ⋯ , y M \boldsymbol x={\boldsymbol x_1,\cdots,\boldsymbol x_{N}}\\ \boldsymbol y={\boldsymbol y_1,\cdots,\boldsymbol y_{M}} x=x1,⋯,xNy=y1,⋯,yM
已知输入数据和观测数据,求 x , y \boldsymbol x,\boldsymbol y x,y的条件概率分布:
P ( x , y ∣ z , u ) P(\boldsymbol x,\boldsymbol y\vert\boldsymbol z,\boldsymbol u) P(x,y∣z,u)
利用贝叶斯法则:
P ( x , y ∣ z , u ) ⏟ 后验 = P ( z , u ∣ x , y ) P ( x , y ) P ( z , u ) ∝ P ( z , u ∣ x , y ) ⏟ 似然 P ( x , y ) ⏟ 先验 \underbrace{P(\boldsymbol x,\boldsymbol y\vert\boldsymbol z,\boldsymbol u)}_{\text{后验}}=\frac{P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)P(\boldsymbol x,\boldsymbol y)}{P(\boldsymbol z,\boldsymbol u)}\propto\underbrace{P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)}_{似然}\underbrace{P(\boldsymbol x,\boldsymbol y)}_{先验} 后验 P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x,y)∝似然 P(z,u∣x,y)先验 P(x,y)
直接求后验分布是困难的,但是可以求一个状态最优估计,使得在该状态下后验概率最大化:
( x , y ) ∗ MAP = arg max P ( x , y ∣ z , u ) = arg max P ( z , u ∣ x , y ) P ( x , y ) {(\boldsymbol x,\boldsymbol y)^{*}}_{\text{MAP}}=\arg\max P(\boldsymbol x,\boldsymbol y\vert\boldsymbol z,\boldsymbol u)=\arg\max P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)P(\boldsymbol x,\boldsymbol y) (x,y)∗MAP=argmaxP(x,y∣z,u)=argmaxP(z,u∣x,y)P(x,y)
求解最大后验概率等价于最大化似然和先验的乘积。先验未知时,可以求解最大似然估计(Maximize Likelihood Estimation, MLE):
( x , y ) ∗ MLE = arg max P ( z , u ∣ x , y ) {(\boldsymbol x,\boldsymbol y)^{*}}_{\text{MLE}}=\arg\max P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y) (x,y)∗MLE=argmaxP(z,u∣x,y)
可以理解为:在什么状态下,最可能产生现在观测到的数据
考虑任意高维高斯分布 x ∼ N ( μ , Σ ) \boldsymbol x\sim\mathcal{N}(\boldsymbol\mu,\boldsymbol\Sigma) x∼N(μ,Σ),有:
P ( x ) = 1 ( 2 π ) N det ( Σ ) exp ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) P(\boldsymbol x)=\frac{1}{\sqrt{(2\pi)^N\det(\boldsymbol\Sigma)}}\exp(-\frac{1}{2}{(\boldsymbol x-\boldsymbol\mu)}^T\boldsymbol\Sigma^{-1}{(\boldsymbol x-\boldsymbol\mu)}) 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((2\pi)^N\det(\boldsymbol\Sigma))+\frac{1}{2}{(\boldsymbol x-\boldsymbol\mu)}^T\boldsymbol\Sigma^{-1}{(\boldsymbol x-\boldsymbol\mu)} −ln(P(x))=21ln((2π)Ndet(Σ))+21(x−μ)TΣ−1(x−μ)
对原函数求最大化相当于对负对数求最小化(负对数第一项与自变量无关)。
max P ( x ) = min − ln ( P ( x ) ) = min 1 2 ( x − μ ) T Σ − 1 ( x − μ ) \max\quad P(x)=\min\quad-\ln(P(x))=\min\quad\frac{1}{2}{(\boldsymbol x-\boldsymbol\mu)}^T\boldsymbol\Sigma^{-1}{(\boldsymbol x-\boldsymbol\mu)} maxP(x)=min−ln(P(x))=min21(x−μ)TΣ−1(x−μ)
假设噪声项服从高斯分布:
w k ∼ N ( 0 , R k ) , v k ∼ N ( 0 , Q k , j ) \boldsymbol{w}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol{R}_k),\boldsymbol{v}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol Q_{k,j}) wk∼N(0,Rk),vk∼N(0,Qk,j)
观测和输入(运动传感器读数)的条件概率均服从高斯分布:
P ( u k ∣ x k − 1 , x k ) = N ( f ( x k − 1 , u k ) , R k ) P ( z k , j ∣ x k y j ) = N ( h ( y j , x k ) , Q k , j ) P(\boldsymbol u_k\vert\boldsymbol x_{k-1},\boldsymbol x_k)=\mathcal{N}(f(\boldsymbol x_{k-1},\boldsymbol u_k),\boldsymbol R_k)\\ P(\boldsymbol z_{k,j}\vert\boldsymbol x_k\boldsymbol y_j)=\mathcal{N}(h(\boldsymbol y_j,\boldsymbol x_k),\boldsymbol Q_{k,j}) P(uk∣xk−1,xk)=N(f(xk−1,uk),Rk)P(zk,j∣xkyj)=N(h(yj,xk),Qk,j)
对联合分布(似然)进行因式分解:
P ( z , u ∣ x , y ) = ∏ k P ( u k ∣ x k − 1 , x k ) ∏ k , j P ( z k , j ∣ x k y j ) P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)=\prod_kP(\boldsymbol u_k\vert\boldsymbol x_{k-1},\boldsymbol x_k)\prod_{k,j}P(\boldsymbol z_{k,j}\vert\boldsymbol x_k\boldsymbol y_j) P(z,u∣x,y)=k∏P(uk∣xk−1,xk)k,j∏P(zk,j∣xkyj)
定义输入和观测与模型之间的误差:
e u , k = x k − f ( x k − 1 , u k ) e z , j , k = z k , j − h ( y j , x k ) e_{\boldsymbol u,k}=\boldsymbol x_k-f(\boldsymbol x_{k-1},\boldsymbol u_k)\\ e_{\boldsymbol z,j,k}=\boldsymbol z_{k,j}-h(\boldsymbol y_j,\boldsymbol x_k) eu,k=xk−f(xk−1,uk)ez,j,k=zk,j−h(yj,xk)
最大似然估计等价于最小化噪声项(误差)的二次型:
min J ( x , y ) = ∑ k e u , k T R k − 1 e u , k + ∑ k ∑ j e z , j , k T Q k , j − 1 e z , j , k \min J(\boldsymbol x,\boldsymbol y)=\sum_ke_{\boldsymbol u,k}^T\boldsymbol R_k^{-1}e_{\boldsymbol u,k}+\sum_k\sum_je_{\boldsymbol z,j,k}^T\boldsymbol Q_{k,j}^{-1}e_{\boldsymbol z,j,k} minJ(x,y)=k∑eu,kTRk−1eu,k+k∑j∑ez,j,kTQk,j−1ez,j,k
误差项的二次型称为马哈拉诺比斯距离(Mahalanobis distance),又称为马氏距离。也可以看作是 R k − 1 , Q k , j − 1 \boldsymbol R_k^{-1},\boldsymbol Q_{k,j}^{-1} Rk−1,Qk,j−1加权后的欧氏距离, R k − 1 , Q k , j − 1 \boldsymbol R_k^{-1},\boldsymbol Q_{k,j}^{-1} Rk−1,Qk,j−1称为信息矩阵(高斯分布协方差矩阵的逆)
这样就得到了一个最小二乘问题,它的解等价于状态的最大似然估计。
考虑简单的最小二乘问题:
KaTeX parse error: Got function '\boldsymbol' with no arguments as subscript at position 7: \min_\̲b̲o̲l̲d̲s̲y̲m̲b̲o̲l̲ ̲xF(\boldsymbol …
f f f形式简单时,另目标函数导数为0有解析解:
d F d x = 0 \frac{dF}{d\boldsymbol x}=0 dxdF=0
形式复杂时用迭代方法,从一个初始值出发,不断更新当前的优化变量,使得目标函数下降:
将目标函数在 x k \boldsymbol 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)\approx 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ΔxkTH(xk)Δxk
其中, J ( x k ) \boldsymbol J(\boldsymbol x_k) J(xk)是 F ( x ) F(\boldsymbol x) F(x)关于 x \boldsymbol x x的一阶导数雅可比(Jacobian)矩阵, H ( x k ) \boldsymbol H(\boldsymbol x_k) H(xk)是二阶导数**海塞(Hessian)**矩阵。
保留一阶梯度,称为一阶梯度法:
Δ x ∗ = − J ( x k ) \Delta\boldsymbol x^*=-\boldsymbol J(\boldsymbol x_k) Δx∗=−J(xk)
保留二阶梯度信息,即二阶梯度法,增量方程为:
Δ x ∗ = arg min ( F ( x ) + J ( x ) T Δ x + 1 2 Δ x T H Δ x ) \Delta\boldsymbol x^*=\arg\min(F(\boldsymbol x)+\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x+\frac{1}{2}\Delta\boldsymbol x^T\boldsymbol H\Delta\boldsymbol x) Δx∗=argmin(F(x)+J(x)TΔx+21ΔxTHΔx)
另右侧关于 Δ x \Delta\boldsymbol x Δx的导数为零,有:
H Δ x = − J \boldsymbol H\Delta\boldsymbol x=-\boldsymbol J HΔx=−J
求此方程,得到增量。又称为牛顿法
高斯牛顿法的思想是将 f ( x ) f(\boldsymbol x) f(x)(不是 F ( x ) F(\boldsymbol x) F(x))进行一阶泰勒展开:
f ( x + Δ x ) ≈ f ( x ) + J ( x ) T Δ x f(\boldsymbol x+\Delta\boldsymbol x)\approx f(\boldsymbol x)+\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x f(x+Δx)≈f(x)+J(x)TΔx
这里 J ( x ) T \boldsymbol J(\boldsymbol x)^T J(x)T是 f ( x ) 关于 x f(\boldsymbol x)关于\boldsymbol x f(x)关于x的导数。目标是寻找增量 Δ x \Delta\boldsymbol x Δx使得 ∥ f ( x + Δ x ) ∥ 2 \Vert f(\boldsymbol x+\Delta\boldsymbol x)\Vert^2 ∥f(x+Δx)∥2最小,即如下最小二乘问题:
Δ x ∗ = arg min Δ x 1 2 ∥ f ( x ) + J ( x ) T Δ x ∥ 2 \Delta\boldsymbol x^*=\arg\min_{\Delta\boldsymbol x}\frac{1}{2}\Vert f(\boldsymbol x)+\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x\Vert^2 Δx∗=argΔxmin21∥f(x)+J(x)TΔx∥2
求导并使其为零,最终得到如下方程组:
J ( x ) J T ( x ) ⏟ H ( x ) Δ x = − J ( x ) f ( x ) ⏟ g ( x ) \underbrace{\boldsymbol J(\boldsymbol x)\boldsymbol J^T(\boldsymbol x)}_{\boldsymbol H(\boldsymbol x)}\Delta\boldsymbol x=\underbrace{-\boldsymbol J(\boldsymbol x)f(\boldsymbol x)}_{\boldsymbol g(\boldsymbol x)} H(x) J(x)JT(x)Δx=g(x) −J(x)f(x)
这个方程是关于变量 Δ x \Delta\boldsymbol x Δx的线性方程组,称为增量方程,也可以称为高斯牛顿方程(Gauss-Newton equation)或正规方程(Normal equation)。上式可变为:
H Δ x = g \boldsymbol H\Delta\boldsymbol x=\boldsymbol g HΔx=g
高斯牛顿法用 J J T \boldsymbol J\boldsymbol J^T JJT作为牛顿法中二阶Hessian矩阵的近似,从而省略了计算 H \boldsymbol H H的过程。求解增量方程是整个优化问题的核心所在。
高斯牛顿法的算法步骤:
在高斯牛顿法中,可能出现 J J T \boldsymbol J\boldsymbol J^T JJT为奇异矩阵或者病态的情况,此时增量的稳定性较差,导致算法不收敛,原函数在这个点的局部近似不像一个二次函数。
高斯牛顿法中的近似二阶泰勒展开只能在展开点附近有较好的近似效果。因此,想到给 Δ x \Delta\boldsymbol x Δx添加一个范围,称为信赖区域(Trust Region),这个范围定义了在什么情况下二阶近似是有效的,这类方法也称为信赖区域方法。
根据近似模型跟实际模型之间的差异来确定信赖区域的范围:如果差异小,近似效果好,扩大近似的范围;反之缩小近似范围。
定义指标来刻画近似的好坏程度:
ρ = f ( x + Δ x ) − f ( x ) J ( x ) T Δ x \rho=\frac{f(\boldsymbol x+\Delta\boldsymbol x)-f(\boldsymbol x)}{\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x} ρ=J(x)TΔxf(x+Δx)−f(x)
分子是实际函数下降值,分子是模型下降值。如果接近于1,则近似效果好。
改良版非线性优化框架:
给定初始值 x 0 \boldsymbol x_0 x0,以及初始优化半径 μ \mu μ
对于第 k k k次迭代,在高斯牛顿法的基础上加上信赖区域,求解:
min Δ x k 1 2 ∥ f ( x k ) + J ( x k ) T Δ x k ∥ 2 , s.t. ∥ D Δ x k ∥ 2 ≤ μ \min_{\Delta\boldsymbol x_k}\frac{1}{2}\Vert f(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k\Vert^2,\quad\text{s.t.}\quad\Vert\boldsymbol D\Delta\boldsymbol x_k\Vert^2\leq\mu Δxkmin21∥f(xk)+J(xk)TΔxk∥2,s.t.∥DΔxk∥2≤μ
其中, μ \mu μ是信赖区域的半径, D \boldsymbol D D是系数矩阵。列文伯格把 D \boldsymbol D D取成单位阵 I \boldsymbol I I,相当于直接把 Δ x k \Delta\boldsymbol x_k Δxk约束在一个球中;马夸尔特将 D \boldsymbol D D取成非负数对角阵,使得梯度小的维度上约束范围更大一些。
按下式计算 ρ \rho ρ:
ρ = f ( x + Δ x ) − f ( x ) J ( x ) T Δ x \rho=\frac{f(\boldsymbol x+\Delta\boldsymbol x)-f(\boldsymbol x)}{\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x} ρ=J(x)TΔxf(x+Δx)−f(x)
若 ρ > 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 \boldsymbol x_{k+1}=\boldsymbol x_k+\Delta\boldsymbol x_k xk+1=xk+Δxk
若 Δ x k \Delta\boldsymbol x_k Δxk足够小,则停止;否则,令 x k + 1 = x k + Δ x k \boldsymbol x_{k+1}=\boldsymbol x_k+\Delta\boldsymbol x_k xk+1=xk+Δxk,返回第2步
在列文伯格-马夸尔特优化中,需要求解如下带不等式约束的优化子问题:
min Δ x k 1 2 ∥ f ( x k ) + J ( x k ) T Δ x k ∥ 2 , s.t. ∥ D Δ x k ∥ 2 ≤ μ \min_{\Delta\boldsymbol x_k}\frac{1}{2}\Vert f(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k\Vert^2,\quad\text{s.t.}\quad\Vert\boldsymbol D\Delta\boldsymbol x_k\Vert^2\leq\mu Δxkmin21∥f(xk)+J(xk)TΔxk∥2,s.t.∥DΔxk∥2≤μ
用拉格朗日法求解,构成拉格朗日函数:
L ( Δ x k , λ ) = 1 2 ∥ f ( x k ) + J ( x k ) T Δ x k ∥ 2 + λ 2 ( ∥ D Δ x k ∥ 2 − μ ) \mathcal{L}(\Delta\boldsymbol x_k,\lambda)=\frac{1}{2}\Vert f(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k\Vert^2+\frac{\lambda}{2}(\Vert\boldsymbol D\Delta\boldsymbol x_k\Vert^2-\mu) L(Δxk,λ)=21∥f(xk)+J(xk)TΔxk∥2+2λ(∥DΔxk∥2−μ)
这里 λ \lambda λ为拉格朗日乘子。令拉格朗日函数关于 Δ x \Delta\boldsymbol x Δx的导数为零,其核心仍然是计算增量的线性方程:
( H + λ D T D ) Δ x k = g (\boldsymbol H+\lambda\boldsymbol D^T\boldsymbol D)\Delta\boldsymbol x_k=\boldsymbol g (H+λDTD)Δxk=g
考虑其简化形式( D = I \boldsymbol D=\boldsymbol I D=I),则相当于求解:
( H + λ I ) Δ x k = g (\boldsymbol H+\lambda\boldsymbol I)\Delta\boldsymbol x_k=\boldsymbol g (H+λI)Δxk=g
当 λ \lambda λ较小时,列文伯格-马夸尔特方法更接近于高斯牛顿法; λ \lambda λ较大时,更接近于一阶梯度下降法(最速下降)