iSAM1論文鏈接

一.基于QR分解的增量平滑優化

A.一個SLAM的概率圖模型

????????用下圖的網絡表示SLAM問題：

其中，?是機器人在時刻i的狀態，是地標j的位置，是時刻i的控制輸入，是第k個地標測量。

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

其中是初始狀態上的先驗值，是運動學模型，由控制輸入ui參數化，是地標性測量模型。這里假設每個測量值都有已知的對應關系。

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

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

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

?對機器人的地標傳感器進行建模，其中是正態分布的零均值測量噪聲與協方差。

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

當執行平滑而不是濾波操作時，注意的是給定控制輸入u和地標測量值z時，整個軌跡x和地標l的地圖估計。軌跡和映射的映射估計，是通過最小化來自（1）的聯合概率的負對數得到的：

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

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

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

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

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

?這里，和分別是測量函數關于在線性化點上計算的和變化的雅可比矩陣:

并且?是測量預測誤差。

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

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

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

?也就是說，總是可以通過將每個項中的、和預乘來從（29）中消除Λi，同樣從測量項中消除。對于標量測量，這僅僅意味著將每一項除以測量的標準差。下面假設已經這樣做了，然后去掉馬氏符號:

最后，將雅可比矩陣收集到一個大矩陣A，將向量和收集到一個右側向量b，得到以下標準最小二乘問題:

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

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

通過將導數設為0，將該稀疏最小二乘系統轉化為普通的線性方程組，得到所謂的正規方程。該方程系統可以用的?Cholesky 分解來求解。

?C.用QR分解法求解

將標準QR矩陣分解應用于測量雅可比矩陣A來求解最小二乘問題（6）。與Cholesky分解相比，這避免了必須用矩陣條件數的相關平方來計算信息矩陣。測量值雅可比矩陣A的QR分解得到：

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

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

?求解這個方程組的大部分工作已經通過QR分解完成了，因為R是上三角的，所以可以使用簡單的反替換。結果是基于所有測量條件的完整機器人軌跡和地圖的最小二乘估計。

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。