200字范文 > 视觉惯性单目SLAM （三）优化方法

视觉惯性单目SLAM （三）优化方法

时间：2023-08-29 09:42:05

相关推荐

视觉惯性单目SLAM （三）优化方法

1. 最小二乘法

最小二乘法：Least Squares Method （LSM）

函数： y=f(x,θ) y = f ( x , θ )

θ θ 为待估参数(可为向量或标量) x,y x , y 常用术语

目标函数：

Q=∑i=0n(yi−f(xi,θ))2 Q = ∑ i = 0 n ( y i − f ( x i , θ ) ) 2

目标：求使残差的平方和最小的待估参数

最小二乘法分类：

线性（linear or ordinary least squares(OLS)）：函数f是参数θ的线性函数，即θ都是一次的，对其求导为常数函数 f 是参数 θ 的线性函数，即 θ 都是一次的，对其求导为常数

非线性（non-linear least squares）：函数f是参数θ的非线性函数，即θ不都是一次的，对其求导不都为常数函数 f 是参数 θ 的非线性函数，即 θ 不都是一次的，对其求导不都为常数

线性最小二乘的解：是封闭形式(closed-form)的，即对于Ax=b，则有x=(ATA)−1ATb 对于 A x = b ，则有 x = ( A T A ) − 1 A T b (A不一定为方阵)

非线性最小二乘的解：不是封闭形式(closed-form)，通常用迭代法求解

超定方程组（overdetermined systems）：方程组中：方程的个数 > > 未知量的个数

残差（residuals）：观测值(实际值)−模型的预测值" role="presentation"> $观测值 (实际值) - 模型的预测值$

估计量（estimator)：是指计算系数(待估参数)的方程

估计值（estimate）：是指估计出来的系数(待估参数)的值

MSE(Mean Squared Error)：平均预测误差平方和 (评价标准)

MSE=1m∑i=0m(ysi−yαi)2 M S E = 1 m ∑ i = 0 m ( y i s − y i α ) 2

ysi：yi的预测值，yαi的实际值，m：样本数 y i s ： y i 的预测值， y i α 的实际值， m ：样本数

MAE(Mean Absolute Error):平均预测误差绝对值 (评价标准)

MAE=1m∑i=0m|ysi−yαi| M A E = 1 m ∑ i = 0 m | y i s − y i α |

2. 优化方法分类

优化方法分类优化方法分类：

对于非线性优化问题，不能象线性最小二乘法那样用求多元函数极值的办法来得到参数估计值，而需要采用复杂的优化算法来求解。常用的算法有两类：搜索算法迭代算法

搜索算法的思路搜索算法的思路：

按一定的规则选择若干组参数值，分别计算它们的目标函数值并比较大小；选出使目标函数值最小的参数值，同时舍弃其他的参数值；然后按规则补充新的参数值，再与原来留下的参数值进行比较，选出使目标函数达到最小的参数值。如此继续进行，直到选不出更好的参数值为止。以不同的规则选择参数值，即可构成不同的搜索算法。常用的方法有单纯形搜索法、复合形搜索法、随机搜索法等。

迭代算法的思路迭代算法的思路：

是从参数的某一初始猜测值 θ(0) θ ( 0 ) 出发，然后产生一系列的参数点 θ(1)、θ(2)… θ ( 1 ) 、 θ ( 2 ) … ，如果这个参数序列收敛到使目标函数极小的参数点，那么对充分大的 N N 就可用θ(N)" role="presentation"> $θ (N)$ 作为结果。迭代算法的一般步骤是：

①给出初始猜测值θ(0)，并置迭代步数 i＝1 i ＝ 1 。

②确定一个向量 v(i) v ( i ) 作为第 i i 步的迭代方向。

③用寻优的方法决定一个标量步长ρ(i)" role="presentation"> $ρ (i)$ ，使得 Q(θ(i))＜Q(θ(i−1)) Q ( θ ( i ) ) ＜ Q ( θ ( i − 1 ) ) ，其中 θ(i)＝θ(i−1)+ρ(i)v(i) θ ( i ) ＝ θ ( i − 1 ) + ρ ( i ) v ( i ) 。

④检查停机规则是否满足，如果不满足,则将i加1再从②开始重复；如果满足，则取θ(i)为结果。

非线性最小二乘法除可直接用于估计静态非线性模型的参数外，在时间序列建模、连续动态模型的参数估计中，也往往遇到求解非线性最小二乘问题。

二阶以上多项式的曲线拟合属于非线性最小二乘法。

迭代法迭代法：在每一步update未知量逐渐逼近解，可以用于各种各样的问题（包括最小二乘），比如求的不是误差的最小平方和而是最小立方和

梯度下降是迭代法梯度下降是迭代法：可以用于求解最小二乘问题（线性和非线性都可以）

高斯−牛顿迭代法高斯 − 牛顿迭代法：一种经常用于求解非线性最小二乘的迭代法（一定程度上可视为标准非线性最小二乘求解方法）

Levenberg−Marquardt(L−M)迭代法 L e v e n b e r g − M a r q u a r d t ( L − M ) 迭代法：用于求解非线性最小二乘问题，就结合了梯度下降和高斯-牛顿法。

3. 线性最小二乘法

3.1 简单问题求解步骤

1）列出目标函数2）对每个待估参数求偏导，并令其等于03）把第2)步的所有方程组成一个方程组，解此方程组即可得待估参数的值

3.2 通用问题求解方案

对于超定方程（overdetermined system）

yi=∑j=1nxi,jβj(i=1,2,⋯,m) y i = ∑ j = 1 n x i , j β j ( i = 1 , 2 , ⋯ , m )

1） m m ：线性方程组个数

2）n" role="presentation"> $n$ ：未知系数个数（ β1,β2,⋯,βn β 1 , β 2 , ⋯ , β n )

3） m>n，xi,1=1 m > n ， x i , 1 = 1

4）矩阵形式： Y=Xβ Y = X β

X=⎡⎣⎢⎢⎢⎢⎢x1,1x2,1⋮xm,1x1,2x2,2⋮xm,2⋯⋯⋱⋯x1,nx2,n⋮xm,n⎤⎦⎥⎥⎥⎥⎥,β=⎡⎣⎢⎢⎢⎢β1β2⋮βn⎤⎦⎥⎥⎥⎥,y=⎡⎣⎢⎢⎢⎢y1y2⋮yn⎤⎦⎥⎥⎥⎥ X = [ x 1 , 1 x 1 , 2 ⋯ x 1 , n x 2 , 1 x 2 , 2 ⋯ x 2 , n ⋮ ⋮ ⋱ ⋮ x m , 1 x m , 2 ⋯ x m , n ] , β = [ β 1 β 2 ⋮ β n ] , y = [ y 1 y 2 ⋮ y n ] 这样的超定方程组经常没有解，所以不得不找到参数 β β ，使它更好地拟合方程组定义目标函数：

S(β)=∑i=1m(yi−∑j=1nxi,jβj)2=||y−Xβ||2 S ( β ) = ∑ i = 1 m ( y i − ∑ j = 1 n x i , j β j ) 2 = | | y − X β | | 2 优化目标：

β^=argminβS(β) β ^ = a r g m i n β ⁡ S ( β ) 解的常规方程变为：

Xβ^=y⇒(XTX)β^=XTy⇒β^=(XTX)−1XTy X β ^ = y ⇒ ( X T X ) β ^ = X T y ⇒ β ^ = ( X T X ) − 1 X T y XTX X T X ：格拉姆矩阵（Gramian Matrix），是一个方阵，其性质为：

是半正定的（positive semi-definite matrix）

G=ATA=⎡⎣⎢⎢⎢⎢⎢aT1aT2⋮aTn⎤⎦⎥⎥⎥⎥⎥[a1a2⋯aTn]=⎡⎣⎢⎢⎢⎢⎢aT1a1aT2a1⋮aTna1aT1a2aT2a2⋮aTna2⋯⋯⋱⋯aT1anaT2an⋮aTnan⎤⎦⎥⎥⎥⎥⎥ G = A T A = [ a 1 T a 2 T ⋮ a n T ] [ a 1 a 2 ⋯ a n T ] = [ a 1 T a 1 a 1 T a 2 ⋯ a 1 T a n a 2 T a 1 a 2 T a 2 ⋯ a 2 T a n ⋮ ⋮ ⋱ ⋮ a n T a 1 a n T a 2 ⋯ a n T a n ]

4. 非线性最小二乘法

基本思想基本思想：用线性函数来近似非线性函数，再模仿线性最小二乘法求解

非线性最小二乘法通用描述非线性最小二乘法通用描述

x∗=argminx(12∑i=1m(fi(x))2)=argminx(12||f(x)||2)=argminx(12f(x)Tf(x)) x ∗ = a r g m i n x ⁡ ( 1 2 ∑ i = 1 m ( f i ( x ) ) 2 ) = a r g m i n x ⁡ ( 1 2 | | f ( x ) | | 2 ) = a r g m i n x ⁡ ( 1 2 f ( x ) T f ( x ) ) f(x) f ( x ) ：为向量函数（即此向量中至少有一个元素是自变量 x x 的函数），它是一个向量fi(x)：Rn→R,i=1,2,⋯,m" role="presentation"> $f_{i} (x) ： R^{n} \to R, i = 1, 2, \dots, m$ ，它为给定的 m个残差函数(residualfunction) m 个残差函数 ( r e s i d u a l f u n c t i o n ) x=[x1x2⋯xn]T x = [ x 1 x 2 ⋯ x n ] T m≥n m ≥ n

代价函数F可微(其值为标量)，其泰勒展开式为：代价函数 F 可微 ( 其值为标量 ) ，其泰勒展开式为：

F(x+h)=F(x)+hTg+12hTHh+O(||h||3) F ( x + h ) = F ( x ) + h T g + 1 2 h T H h + O ( | | h | | 3 )

g为梯度(或叫做只有一行的JacobianMatrix)，即一阶偏导： g 为梯度 ( 或叫做只有一行的 J a c o b i a n M a t r i x ) ，即一阶偏导：

g=F′(x)=⎡⎣⎢⎢⎢⎢⎢⎢⎢∂F∂x1(x)∂F∂x2(x)⋮∂F∂xn(x)⎤⎦⎥⎥⎥⎥⎥⎥⎥ g = F ′ ( x ) = [ ∂ F ∂ x 1 ( x ) ∂ F ∂ x 2 ( x ) ⋮ ∂ F ∂ x n ( x ) ]

h向量的每个元素为无穷小，即x+h在x的邻域内 h 向量的每个元素为无穷小，即 x + h 在 x 的邻域内

H为海森矩阵（HessianMatrix），即二阶偏导 H 为海森矩阵（ H e s s i a n M a t r i x ），即二阶偏导

H=F′′(x)=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢∂2F∂x1x1∂2F∂x2x1⋮∂2F∂xnx1∂2F∂x1x2∂2F∂x2x2⋮∂2F∂xnx2⋯⋯⋱⋯∂2F∂x1xn∂2F∂x2xn⋮∂2F∂xnxn⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥ H = F ″ ( x ) = [ ∂ 2 F ∂ x 1 x 1 ∂ 2 F ∂ x 1 x 2 ⋯ ∂ 2 F ∂ x 1 x n ∂ 2 F ∂ x 2 x 1 ∂ 2 F ∂ x 2 x 2 ⋯ ∂ 2 F ∂ x 2 x n ⋮ ⋮ ⋱ ⋮ ∂ 2 F ∂ x n x 1 ∂ 2 F ∂ x n x 2 ⋯ ∂ 2 F ∂ x n x n ]

注：具体推导见视觉惯性单目SLAM （四）-泰勒展开式

4.1 梯度下降法（Gradient Descent）

一阶优化算法一阶优化算法用于计算函数的局部最小值用于计算函数的局部最小值迭代增量：在当前点的梯度的负值，再乘以一个比例值λ 迭代增量：在当前点的梯度的负值，再乘以一个比例值 λ

迭代步骤：迭代步骤：

1）初始化： k=0,x=x0 k = 0 , x = x 0

2）当 k<kmax k < k m a x

xk+1=xk−λF′(xk)=xk−λg(xk)=xk−λ∇F(xk) x k + 1 = x k − λ F ′ ( x k ) = x k − λ g ( x k ) = x k − λ ∇ F ( x k )

4.2 牛顿法（Newton Method )

二阶近似优化算法二阶近似优化算法代价函数的二阶泰勒展开代价函数的二阶泰勒展开：

F(x+h)≈F(x)+F′(x)h+12F′′(x)h2 F ( x + h ) ≈ F ( x ) + F ′ ( x ) h + 1 2 F ″ ( x ) h 2

x为一维时，优化方向h为(对h求一阶导，并令其为0，求解h即可)： x 为一维时，优化方向 h 为 ( 对 h 求一阶导，并令其为 0 ，求解 h 即可 ) ：

h=−F′(x)F′′(x) h = − F ′ ( x ) F ″ ( x )

x为高维时，优化方向h为： x 为高维时，优化方向 h 为：

F(x+h)≈F(x)+hT∇F(x)+12hTH(x)h F ( x + h ) ≈ F ( x ) + h T ∇ F ( x ) + 1 2 h T H ( x ) h

h≈−H(x)−1∇F(x)=−H(x)−1g h ≈ − H ( x ) − 1 ∇ F ( x ) = − H ( x ) − 1 g 迭代步骤：迭代步骤：

1）初始化： k=0,x=x0 k = 0 , x = x 0

2）当 k<kmax k < k m a x

xk+1=xk−λH(xk)−1∇F(xk)=xk−λH(xk)−1g(xk) x k + 1 = x k − λ H ( x k ) − 1 ∇ F ( x k ) = x k − λ H ( x k ) − 1 g ( x k ) 发现海森矩阵的逆是非常昂贵的，近似的方法被使用，近似的方法有：发现海森矩阵的逆是非常昂贵的，近似的方法被使用，近似的方法有：

共轭梯度下降法（conjugate gradient method）拟牛顿法（quasi-newton method）牛顿法与梯度下降法的比较，红色的为牛顿法牛顿法与梯度下降法的比较，红色的为牛顿法

4.3 高斯-牛顿法（Gauss-Newton Methods）

经常用于求解非线性最小二乘问题经常用于求解非线性最小二乘问题原理: 原理 :

高斯—牛顿迭代法的基本思想是使用泰勒级数展开式去近似地代替非线性回归模型，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小。

4.3.1 方法描述

方法描述: 方法描述 :

给定 m m 个函数r=(r1,⋯,rm)" role="presentation"> $r = (r_{1}, \dots, r_{m})$ (经常叫做残差， r r 为向量值函数），每个函数ri(β)" role="presentation"> $r_{i} (β)$ 有 n n 个变量β=(β1,⋯,βn)" role="presentation"> $β = (β_{1}, \dots, β_{n})$ ，且 m≥n m ≥ n ，

高斯−牛顿迭代法的目标高斯 − 牛顿迭代法的目标：找到 β β 的最优解，使得残差的平方和最小：：

S(β)=∑i=1mr2i(β)=r(β)Tr(β)=||r(β)||2 S ( β ) = ∑ i = 1 m r i 2 ( β ) = r ( β ) T r ( β ) = | | r ( β ) | | 2

以初始值 β(0) β ( 0 ) 开始迭代，则迭代方法如下：

β(s+1)=β(s)−(JTrJr)−1JTrr(β(s)) β ( s + 1 ) = β ( s ) − ( J r T J r ) − 1 J r T r ( β ( s ) ) 若 r和β r 和 β 都为列向量，则雅可比矩阵（Jacobianmatrix）雅可比矩阵（ J a c o b i a n m a t r i x ）的项为：

(Jr)i,j=∂ri(β(s))∂βj ( J r ) i , j = ∂ r i ( β ( s ) ) ∂ β j

注：Jr是向量函数r对β向量的雅可比矩阵注： J r 是向量函数 r 对 β 向量的雅可比矩阵如果 m==n m == n ，则迭代简化为：

β(s+1)=β(s)−(Jr)−1r(β(s)) β ( s + 1 ) = β ( s ) − ( J r ) − 1 r ( β ( s ) ) 在数据拟合中，需要回归的函数为： y=f(x,β) y = f ( x , β ) ，此函数拟合数据点 (xi,yi) ( x i , y i ) ，则 ri r i 为：

ri(β)=yi−f(xi,β) r i ( β ) = y i − f ( x i , β )

则高斯−牛顿迭代法可用函数f的JacobianMatrix表示：则高斯 − 牛顿迭代法可用函数 f 的 J a c o b i a n M a t r i x 表示：

β(s+1)=β(s)+(JTfJf)−1JTfr(β(s)) β ( s + 1 ) = β ( s ) + ( J f T J f ) − 1 J f T r ( β ( s ) )

(Jf)i,j=∂f(xi,β(s))∂βj ( J f ) i , j = ∂ f ( x i , β ( s ) ) ∂ β j

注：Jf是向量函数f对β向量的雅可比矩阵注： J f 是向量函数 f 对 β 向量的雅可比矩阵

4.3.2 方法推导

从牛顿法出发进行推导从牛顿法出发进行推导根据牛顿法使用S(β)或S最小的β，其递归关系为根据牛顿法使用 S ( β ) 或 S 最小的 β ，其递归关系为：

S=∑i=1mr2i(S是多元标量函数，其值为标量) S = ∑ i = 1 m r i 2 ( S 是多元标量函数，其值为标量 )

βs+1=βs−H−1g,(g为S的梯度，H为S的海森矩阵) β s + 1 = β s − H − 1 g , ( g 为 S 的梯度， H 为 S 的海森矩阵 ) 则梯度为：则梯度为：

gj=2∑i=1mri∂ri∂βj g j = 2 ∑ i = 1 m r i ∂ r i ∂ β j 海森矩阵的元素由梯度元素对每个自变量 βk β k 求偏导组成：

Hjk=∂gj∂βk=2∑i=1m(∂ri∂βk∂ri∂βj+ri∂2ri∂βj∂βk) H j k = ∂ g j ∂ β k = 2 ∑ i = 1 m ( ∂ r i ∂ β k ∂ r i ∂ β j + r i ∂ 2 r i ∂ β j ∂ β k ) 高斯-牛顿法忽略了二阶偏导数，即上式中的第二项，因为其值较小，则Hessian矩阵近似表示为：

Hjk≈2∑i=1m(∂ri∂βk∂ri∂βj)=2∑i=1m(JikJij)=2∑i=1m(JijJik) H j k ≈ 2 ∑ i = 1 m ( ∂ r i ∂ β k ∂ r i ∂ β j ) = 2 ∑ i = 1 m ( J i k J i j ) = 2 ∑ i = 1 m ( J i j J i k ) Jij：是雅可比矩阵Jr的元素（i行，j列） J i j ：是雅可比矩阵 J r 的元素（ i 行， j 列）梯度和海森矩阵的矩阵表示为：（即使用雅可比矩阵来表示梯度和海森矩阵）

g=2JTrr=2JTr(βs)r(βs) g = 2 J r T r = 2 J r T ( β s ) r ( β s )

H≈2JTrJr=2Jr(βs)TJr(βs) H ≈ 2 J r T J r = 2 J r ( β s ) T J r ( β s ) 则高斯-牛顿法的迭代公式为：

βs+1=βs−H−1g=βs−(JTrJr)−1JTrr(βs) β s + 1 = β s − H − 1 g = β s − ( J r T J r ) − 1 J r T r ( β s )

4.4 列文伯格-马夸尔特法(Levenberg–Marquardt algorithm)

Levenberg–Marquardt algorithm：简记为：LMA或LMLM是一个著名的阻尼最小二乘法（Damped Least-Squares < DLS>），用于求解非线性最小二乘问题，LMA特别擅长最小二乘曲线拟合问题 LMA与GNA(Gauss−NewtonAlgorithm)和GD(GradientDescent)的比较 L M A 与 G N A ( G a u s s − N e w t o n A l g o r i t h m ) 和 G D ( G r a d i e n t D e s c e n t ) 的比较

LMA介于GNA与GD之间LMA比GNA比加健壮：在许多情况，即使从远离最后的极小值点开始，它也能找到解对于性质优良的函数（如凸函数）和合理的开始参数，LMA比GNA慢一些LMA也可以看作是使用信赖域方法的高斯–牛顿法LM法就是在高斯-牛顿法的基础上加入了一个变量因子LMA的迭代公式为：

βs+1=βs−H−1g=βs−(JTrJr+λI)−1JTrr(βs)=βs−[JTrJr+λdiag(JTrJr)]−1JTrr(βs) β s + 1 = β s − H − 1 g = β s − ( J r T J r + λ I ) − 1 J r T r ( β s ) = β s − [ J r T J r + λ d i a g ( J r T J r ) ] − 1 J r T r ( β s )