iSAM1論文鏈接

一.基于QR分解的增量平滑優(yōu)化

A.一個(gè)SLAM的概率圖模型

????????用下圖的網(wǎng)絡(luò)表示SLAM問題：

其中，?是機(jī)器人在時(shí)刻i的狀態(tài)，是地標(biāo)j的位置，是時(shí)刻i的控制輸入，是第k個(gè)地標(biāo)測量。

????????所有變量和測量值的聯(lián)合概率由以下公式表示：

其中是初始狀態(tài)上的先驗(yàn)值，是運(yùn)動(dòng)學(xué)模型，由控制輸入ui參數(shù)化，是地標(biāo)性測量模型。這里假設(shè)每個(gè)測量值都有已知的對應(yīng)關(guān)系。

????????假設(shè)為高斯測量模型。則“過程模型”如下：

?該方程描述了測程傳感器或掃描匹配的過程，其中是正態(tài)分布的零均值過程噪聲與協(xié)方差矩陣。

????????而高斯測量模型如下:

?對機(jī)器人的地標(biāo)傳感器進(jìn)行建模，其中是正態(tài)分布的零均值測量噪聲與協(xié)方差。

?B.把SLAM化為最小二乘問題

當(dāng)執(zhí)行平滑而不是濾波操作時(shí)，注意的是給定控制輸入u和地標(biāo)測量值z時(shí)，整個(gè)軌跡x和地標(biāo)l的地圖估計(jì)。軌跡和映射的映射估計(jì)，是通過最小化來自（1）的聯(lián)合概率的負(fù)對數(shù)得到的：

?結(jié)合過程和測量模型，則有以下非線性最小二乘問題：

?其中，這里使用符號代替平方馬氏距離與協(xié)方差矩陣Σ

這里附錄回顧了如何將測量函數(shù)線性化，并將非線性最小二乘目標(biāo)函數(shù)（5）的所有分量收集成一個(gè)一般的最小二乘公式，遵循。通過泰勒展開式將（5）中的測量函數(shù)線性化，假設(shè)要么有一個(gè)很好的線性化點(diǎn)可用，或者正在進(jìn)行一個(gè)非線性優(yōu)化方法的一次迭代。在以上任何一種情況下，（5）中過程模型的一階線性化給出為：

?其中，是過程模型在線性化點(diǎn)處的雅可比矩陣，這里定義為：

?并且，是測程預(yù)測誤差(注意這里的ui是給出的，因此是常數(shù))。而方程（5）中測量模型的一階線性化如下：

?這里，和分別是測量函數(shù)關(guān)于在線性化點(diǎn)上計(jì)算的和變化的雅可比矩陣:

并且?是測量預(yù)測誤差。

分別使用線性化過程和測量模型（24）和（26），則非線性最小二乘問題（5）變成：

也就是說，這里得到了δθ中一個(gè)需要有效求解的線性最小二乘問題。為了避免以一種特殊的方式處理，我們引入了矩陣,

?通過簡單地改變變量，可以去掉協(xié)方差矩陣和。用作為Λ的矩陣平方根，可以將馬氏范數(shù)重寫如下:

?也就是說，總是可以通過將每個(gè)項(xiàng)中的、和預(yù)乘來從（29）中消除Λi，同樣從測量項(xiàng)中消除。對于標(biāo)量測量，這僅僅意味著將每一項(xiàng)除以測量的標(biāo)準(zhǔn)差。下面假設(shè)已經(jīng)這樣做了，然后去掉馬氏符號:

最后，將雅可比矩陣收集到一個(gè)大矩陣A，將向量和收集到一個(gè)右側(cè)向量b，得到以下標(biāo)準(zhǔn)最小二乘問題:

為了簡單起見，iSAM在附錄之外去掉了δ·符號,則上式變成：

?其中，向量θ∈包含所有的姿態(tài)和地標(biāo)變量，矩陣A∈是一個(gè)大但稀疏的測量雅可比矩陣，b∈是右側(cè)(RHS)向量。

通過將導(dǎo)數(shù)設(shè)為0，將該稀疏最小二乘系統(tǒng)轉(zhuǎn)化為普通的線性方程組，得到所謂的正規(guī)方程。該方程系統(tǒng)可以用的?Cholesky 分解來求解。

?C.用QR分解法求解

將標(biāo)準(zhǔn)QR矩陣分解應(yīng)用于測量雅可比矩陣A來求解最小二乘問題（6）。與Cholesky分解相比，這避免了必須用矩陣條件數(shù)的相關(guān)平方來計(jì)算信息矩陣。測量值雅可比矩陣A的QR分解得到：

?其中，R∈為上三角平方根信息矩陣(注意，信息矩陣由給出)，Q∈為正交矩陣。我們將這個(gè)因子分解應(yīng)用于最小二乘問題（6）：

?其中，定義與d∈和e∈.（8）當(dāng)且僅當(dāng)Rθ=d時(shí)成為最小，留下第二項(xiàng)作為最小二乘問題的殘差。因此，QR分解將最小二乘問題簡化為具有單一唯一解的線性系統(tǒng)：

?求解這個(gè)方程組的大部分工作已經(jīng)通過QR分解完成了，因?yàn)镽是上三角的，所以可以使用簡單的反替換。結(jié)果是基于所有測量條件的完整機(jī)器人軌跡和地圖的最小二乘估計(jì)。

總結(jié)

以上是生活随笔為你收集整理的iSAM1论文推导学习--第二节QR部分的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。