日本无码免费高清在线|成人日本在线观看高清|A级片免费视频操逼欧美|全裸美女搞黄色大片网站|免费成人a片视频|久久无码福利成人激情久久|国产视频一二国产在线v|av女主播在线观看|五月激情影音先锋|亚洲一区天堂av

  • 手機站
  • 小程序

    汽車測試網(wǎng)

  • 公眾號

    • 汽車測試網(wǎng)

    • 在線課堂

    • 電車測試

基于關(guān)鍵幀的使用非線性優(yōu)化的視覺-慣導(dǎo)SLAM

2018-12-24 21:37:32·  來源：同濟智能汽車研究所  作者：Leutenegger.Stefan  
　
主要作者：Leutenegger.Stefan本文由同濟智能汽車研究所編譯，轉(zhuǎn)載請在文首注明出處。摘要：視覺和慣性傳感器融合因其感知方式的互補性質(zhì)在機器人技術(shù)中已經(jīng)變得
主要作者：Leutenegger.Stefan
本文由同濟智能汽車研究所編譯，轉(zhuǎn)載請在文首注明出處。
摘要：視覺和慣性傳感器融合因其感知方式的互補性質(zhì)在機器人技術(shù)中已經(jīng)變得流行起來。雖然目前大多數(shù)融合策略依賴于濾波方案，但視覺機器人社區(qū)最近已經(jīng)轉(zhuǎn)向基于非線性優(yōu)化方法的同步定位與建圖（SLAM），并發(fā)現(xiàn)這種方法在性能方面具有顯著優(yōu)勢和計算復(fù)雜性。遵循這一趨勢，我們提出一種新的方法，將視覺測量與來自慣性測量單元（IMU）的讀數(shù)在SLAM中緊耦合。IMU誤差項以完全概率的方式與特征重投影誤差緊耦合，優(yōu)化聯(lián)合非線性代價函數(shù)。我們使用“關(guān)鍵幀”的概念將舊狀態(tài)部分邊緣化，以維持有限變量的優(yōu)化窗口，從而確保系統(tǒng)實時運行。與松耦合的視覺慣性算法相比，我們的實驗證實了緊耦合在精度和魯棒性方面更有優(yōu)勢。
 
I. 引言
融合視覺和慣性傳感器長期以來一直是解決常見機器人任務(wù)，如運動估計、視覺測距和SLAM的流行手段。圖像數(shù)據(jù)中豐富的場景特性，和在IMU中陀螺儀和加速度計的精確短期估計特性被認(rèn)為彼此互補，可用于無人機[6,20]和無人車中[14] ]的導(dǎo)航。 此外，大多數(shù)智能手機中這些傳感器的普及，引起了對視覺慣性SLAM的研究的極大興趣。
 
歷史上已經(jīng)通過濾波方式處理了視覺慣性姿態(tài)估計問題，IMU測量被用于預(yù)測傳播而關(guān)鍵點檢測用于更新。Mourikis和Roumeliotis [14]提出了一種基于EKF的單目視覺的實時融合方法，Jones和Soatto [8]在長距離的室外軌跡上提供單目視覺慣性濾波估計結(jié)果，其中包括IMU到相機標(biāo)定和閉環(huán)檢測。這兩個方法表現(xiàn)出令人印象深刻的低于0.5％的距離誤差。Kelly和Sukhatme[9]提供了相機標(biāo)定和基于濾波的vision-IMU融合可觀性的研究。偏航角和絕對位置的全局不可觀性，和相對于初始參考姿態(tài)的不確定性，是視覺慣性估計的本質(zhì)問題；這對依賴于線性化的濾波方法提出了挑戰(zhàn)。
 
文獻[18]表明，相比濾波方法，基于優(yōu)化方法的純視覺SLAM在相同的計算條件下提供了更好的精度。因此，保持稀疏的關(guān)鍵幀和關(guān)聯(lián)的地標(biāo)的圖優(yōu)化方法，已經(jīng)非常受歡迎。
 
文獻中的視覺-慣性傳感器融合方法遵循兩種思路。在松耦合系統(tǒng)中，例如文獻[10]，陀螺儀數(shù)據(jù)和相對偏航測量作為獨立的IMU觀測結(jié)合到立體視覺中進行優(yōu)化。 Weiss等人[20]僅用視覺姿態(tài)估計作為IMU傳播的EKF的更新。在文獻[15,7]中，相對立體姿態(tài)估計被集成到包含慣性項和絕對GPS測量的因子圖中。這種方法降低了系統(tǒng)的復(fù)雜性，但忽略了不同傳感器的內(nèi)部狀態(tài)之間的關(guān)聯(lián)。相反，緊耦合方法聯(lián)合估計所有傳感器狀態(tài)。作為濾波方案的替代，Dong-Si和Mourikis [2]提出了一種固定滯后平滑器，其中在固定窗口中維持連續(xù)的機器人姿勢和相關(guān)狀態(tài)估計，超出范圍后邊緣化狀態(tài)[19。類似的方法，也在文獻[16]中使用。
 
出于視覺慣性SLAM系統(tǒng)的精度和魯棒性，我們提倡緊耦合方法，以最大限度地利用傳感器特性，同時使用非線性優(yōu)化方法而非濾波方法，以減少由于線性化造成的誤差。我們的方法靈感來自文獻[17]，其中建議在批量優(yōu)化的SLAM中加入IMU誤差項（僅在初始化時）。 我們的方法與[2]中提出的固定滯后平滑器密切相關(guān)，因為它在單個代價函數(shù)中結(jié)合慣性誤差項和重新投影誤差項，并且通過邊緣化舊幀以限制系統(tǒng)復(fù)雜性。
 
我們的工作主要有三個貢獻：
  • 我們在慢速運動或完全靜止時，采用關(guān)鍵幀進行無漂移估計：而不是使用時間上連續(xù)的姿勢的優(yōu)化窗口，我們保持關(guān)鍵幀在時間間隔上隨機分布，以保持視覺約束的同時仍然考慮IMU項。我們的關(guān)鍵幀的相對不確定性方程讓我們可以在不表示全局姿態(tài)不確定性的情況下建立位姿圖，這是從RSLAM[13]得到的靈感。
  • 我們用完全概率的方式推導(dǎo)了IMU誤差項，包括相關(guān)的連續(xù)圖像幀間對應(yīng)的信息矩陣，而沒有以IMU數(shù)據(jù)頻率引入精確狀態(tài)。
  • 在系統(tǒng)層面，我們開發(fā)了精確實時SLAM方案的硬件和算法，包括魯棒關(guān)鍵點匹配和通過慣性測量項進行外點剔除。
在本文的其余部分，在II-B中，我們在batch visual SLAM中引入了慣性誤差項，隨后在II-C中簡單描述了我們的實時立體聲圖像處理和關(guān)鍵幀選擇，和II-D中的邊緣化形式。 最后，在III中我們展示了立體視覺和IMU傳感器在室內(nèi)和室外獲得的結(jié)果。
 
II. 緊耦合的視覺-IMU融合
在視覺SLAM中，非線性優(yōu)化通過最小化在相機幀中觀察到地標(biāo)的重投影誤差來找到相機姿勢和地標(biāo)位置。圖1表示相應(yīng)的圖優(yōu)化拓?fù)浣Y(jié)構(gòu)：其將測量顯示為具有方框的邊，估計的量作為節(jié)點。一旦引入慣性測量，它們不僅在連續(xù)姿勢之間產(chǎn)生約束，而且在加速度計和陀螺儀的速度和IMU誤差估計之間產(chǎn)生狀態(tài)增量的約束。 在本節(jié)中，我們提出了將慣性測量結(jié)合到視覺SLAM中的方法。
圖1：在視覺SLAM（左）和視覺慣導(dǎo)SLAM（右）中涉及的狀態(tài)變量圖模型
圖2：硬件中涉及的坐標(biāo)系：兩個相機分別代表一個坐標(biāo)系
FCi，IMU數(shù)據(jù)在FS坐標(biāo)系中獲得，F(xiàn)S坐標(biāo)系通過世界坐標(biāo)系FW估計獲得。
A.符號和定義
  1. 符號：我們在整個過程中采用以下符號：FA表示參考幀A，其中向量被寫為pA或pBCA，其中B和C分別為起點和終點。幀間的變換由齊次變換矩陣TAB表示，TAB表示從FA到FB的點坐標(biāo)變換 ，其旋轉(zhuǎn)矩陣部分寫為CAB。我們采用Barfoot等人介紹的符號[1]：四元數(shù)乘法qAC=qAB⊗qBC，引入左手側(cè)復(fù)合運算符（.）+和右手側(cè)運算符（.）⊕，使得qAC = qAB+qBC=qBC⊕qAB。
  2. 坐標(biāo)系：使用圖2中描繪的立體相機IMU。在被跟蹤物體內(nèi)部，相對于世界坐標(biāo)系FW，我們區(qū)分相機坐標(biāo)系FC和IMU傳感器坐標(biāo)系FS。
  3. 狀態(tài)：要估計的變量包括圖像時間xkR和地標(biāo)xcL處的機器人狀態(tài)。xR表示在慣性幀pWSW中的機器人位置，qWS表示旋轉(zhuǎn)四元數(shù)，vWSW表示世界坐標(biāo)系下的速度，以及陀螺儀的偏置bg和加速度計的偏置ba。于是xR被寫為：
此外，我們使用分割成姿態(tài)狀態(tài)和速度/偏置狀態(tài)。 地標(biāo)坐標(biāo)以齊次形式表示，如[3]中所示，以便允許接近和非常遠的地標(biāo)的無縫集成。
我們在流形上的切空間g中的擾動采用組運算符，指數(shù)映射exp和對數(shù)映射log。我們使用最小坐標(biāo)表示法。雙射映射Φ從最小坐標(biāo)變換到切線空間：
具體來說，我們使用軸角擾動旋轉(zhuǎn)，可通過指數(shù)轉(zhuǎn)換成其等效四元數(shù)δq：
因此，使用算子⊗，我們獲得最小機器人誤差狀態(tài)向量：
我們使用姿態(tài)誤差狀態(tài)和速度/偏置的誤差狀態(tài)。我們將齊次地圖坐標(biāo)視為具有最小擾動的四元數(shù)δβ，因此：
B.具有慣性條件的批視覺SLAM
我們嘗試公式化視覺慣性定位和建圖問題，將它表示成包含（加權(quán)）重投影誤差er和IMU的誤差項es的代價函數(shù)J(x)的聯(lián)合優(yōu)化：
其中i是組件的相機索引，k表示相機幀索引，j表示地標(biāo)索引。在第k幀和第i個相機中可見的標(biāo)志的索引被寫為集合J(i,k)。此外，W表示相應(yīng)地標(biāo)測量的信息矩陣。
純視覺SLAM需要在優(yōu)化期間保持固定的6自由度（DoF），即絕對姿勢。組合的視覺慣性問題僅具有4個自由度，因為重力使得兩個旋轉(zhuǎn)自由度可觀察到。我們想要固定圍繞重力方向（世界z軸）的偏航角，以及第一幀的位置。因此，除了將位置變化設(shè)置為零，我們還假設(shè)：
在下面，我們將列出重投影誤差公式。之后，給出了IMU運動學(xué)與偏置模型，基于此我們得到IMU誤差項。
1）重投影誤差公式
我們使用標(biāo)準(zhǔn)的重投影誤差公式：
這里，hi(·)表示相機投影模型，zi,j,k表示圖像坐標(biāo)系下特征點坐標(biāo)。
2）IMU運動學(xué)
在地球旋轉(zhuǎn)的測量效應(yīng)小于陀螺儀精度的假設(shè)下，我們可以將IMU運動學(xué)與動態(tài)偏置模型結(jié)合起來：
其中都是不相關(guān)的零均值高斯白噪聲過程。與隨機游走的陀螺偏差相反，我們使用時間常數(shù)τ>0來將加速度計偏置建模成有界隨機游走。矩陣Ω由估計的角速率形成，用陀螺儀測量數(shù)據(jù)：
線性化誤差動力學(xué)采取形式：
其中G是直接導(dǎo)出和：
（.）×表示與矢量關(guān)聯(lián)的斜對稱叉乘矩陣。
注意到可以采用在經(jīng)典EKF濾波中用于計算均值和協(xié)方差的相同方式。實際實現(xiàn)中，需要這些方程的離散化，其中索引p表示第p個IMU測量。出于計算復(fù)雜性考慮，我們選擇使用簡單的Euler-Forward方法在時間差Δt上積分。類似地，我們獲得離散形式的誤差狀態(tài)傳播矩陣：
協(xié)方差傳播方程：
其中包含各個過程的噪聲。
3）IMU測量誤差項的公式
圖3示出了在時間步驟k和k+1進行的相機測量的測量速率的差異，較快的IMU測量通常不與相機數(shù)據(jù)同步。
圖3：IMU和相機的采樣的不同頻率：一個IMU測量項使用了圖像幀間的所有陀螺儀和加速度數(shù)據(jù)。
我們需要將IMU誤差項表示為步驟k和k+1處的機器人狀態(tài)的函數(shù)。因此，我們需要假設(shè)在相機測量在k和k + 1的給定機器人狀態(tài)下的條件概率密度f為正態(tài)分布：
考慮包含狀態(tài)傳播的協(xié)方差矩陣，IMU預(yù)測誤差項現(xiàn)在可寫為：
上述公式基于先驗的狀態(tài)的預(yù)測和實際狀態(tài)之間的差值，除了旋轉(zhuǎn)狀態(tài)量，其中我們使用簡單的最小二乘誤差。
接下來，在應(yīng)用誤差傳播定律時，得到相關(guān)的信息矩陣：
Jacobian矩陣可以直接獲得但有非零解，因為旋轉(zhuǎn)誤差一般是非零的。
最后，在優(yōu)化過程中，k到k+1幀狀態(tài)的Jacobian矩陣需要被計算?；厮萸叭说姆椒?，IMU誤差項通過迭代方式來計算（。因此應(yīng)用鏈?zhǔn)椒▌t計算相對于第k幀狀態(tài)的微分：
C.關(guān)鍵點匹配和關(guān)鍵幀選擇
我們的過程pipeline采用本地化的多尺度SSE優(yōu)化的Harris角點檢測結(jié)合BRISK描述子提取[12]。檢測器通過抑制具有較弱權(quán)重的角點來使圖像中的關(guān)鍵點均勻分布，因為它們在到較強角點的小距離處被檢測到。描述子沿著重力方向被提?。ㄍ队暗綀D像中），其由于與IMU緊耦合而可觀的。
最初，關(guān)鍵點被立體三角化并插入到局部映射中。我們對所有地圖地標(biāo)執(zhí)行暴力匹配;通過使用通過IMU積分獲得的（不確定）姿態(tài)預(yù)測，通過在圖像坐標(biāo)中應(yīng)用卡方檢驗簡單地執(zhí)行外點剔除。沒有RANSAC步驟，這是緊耦合IMU的另一個優(yōu)點。對于后續(xù)優(yōu)化，保持相機幀的有界集合，即具有在該時刻拍攝的相關(guān)圖像的姿態(tài)；在這些圖像中可見的所有地標(biāo)都保存在本地地圖中。如圖4所示，我們區(qū)分兩種類型的幀：我們引入包括當(dāng)前幀的S個最近幀的時間窗口;并且我們使用可能在過去遠的N個關(guān)鍵幀。對于關(guān)鍵幀選擇，我們使用一個簡單的啟發(fā)式：如果匹配點跨越的圖像區(qū)域與所有檢測到的點跨越的區(qū)域之間的比率低于50到60％，則幀被標(biāo)記為關(guān)鍵幀。
圖4：圖像幀被保留用以匹配和后續(xù)的優(yōu)化
D.部分邊緣化
非線性時間約束可以駐留在的有界優(yōu)化窗口中是不明顯的，窗口中包含可能在時間上任意間隔開的關(guān)鍵幀。下文中我們首先提供邊緣化的數(shù)學(xué)基礎(chǔ)，即消除非線性優(yōu)化中的狀態(tài)，并將其應(yīng)用于視覺慣性SLAM。
1）非線性優(yōu)化邊際化的數(shù)學(xué)公式
高斯-牛頓方程組由所有的誤差項，雅可比和信息構(gòu)成：形式Hδx = b。 讓我們考慮要被邊緣化的一組狀態(tài)，與誤差項和剩余狀態(tài)集合相關(guān)的所有狀態(tài)的集合。 由于條件獨立性，我們可以簡化邊緣化步驟，只將其應(yīng)用于子問題：
Schur-Complement運算的得到：
文獻[18]中的方程描述了邊緣化的單一步驟。在我們基于關(guān)鍵幀的方法中，必須重復(fù)應(yīng)用邊緣化步驟，并將結(jié)果信息作為先驗，因為我們的狀態(tài)估計會持續(xù)改變。因此，我們固定x0周圍的線性化點，x0為初始邊緣化時x的值。換句話說x組成是：
這個通用的公式允許我們將先驗信息應(yīng)用到任何狀態(tài)變量-包括單位四元數(shù)。引入Δx項：
現(xiàn)在我們可以表示高斯-牛頓系統(tǒng)為：
在該形式中，右側(cè)變?yōu)椋?/div>
邊緣化節(jié)點包括無限遠（或足夠接近無限遠）的地標(biāo)，或來自單個姿勢的僅在一個攝像機中可見的地標(biāo)，因此與那些地標(biāo)相關(guān)聯(lián)的Hessian矩陣塊有可能不是滿秩矩陣。因此，我們采用偽逆的形式。
上述公式為邊緣化xμ以及剩余狀態(tài)xλ的狀態(tài)引入了固定線性化點。這也將被用作所有未來線性化的參考點。接著，我們可以去除消耗的非線性項，并將邊緣化的和作為加數(shù)，以構(gòu)建整個高斯-牛頓系統(tǒng)。對最小二乘誤差的貢獻可以寫為：
2）邊緣化應(yīng)用于基于關(guān)鍵幀的視覺慣性SLAM
最初邊緣化誤差項由前N + 1幀構(gòu)成，如圖5中以圖形方式可視化.N個第一幀將全部被解釋為關(guān)鍵幀，并且邊緣化步驟包括消除相應(yīng)的速度和偏置狀態(tài)。
圖5：圖模型展示了在最初N+1幀時的初始邊緣化過程
當(dāng)將新幀插入到優(yōu)化窗口中時，我們應(yīng)用邊緣化操作。在時間窗口中最舊的幀不是關(guān)鍵幀的情況下，我們將丟棄其所有的界標(biāo)測量，然后將其與最舊的速度和偏置狀態(tài)一起邊緣化。圖6示出了該過程。下降的地標(biāo)測量是次優(yōu)的。
圖6：圖模型中有N=3個關(guān)鍵幀和一個IMU臨時節(jié)點，一個普通幀從滑動窗口中被丟棄。
然而，系統(tǒng)需要保持矩陣稀疏以快速求解。具有關(guān)鍵幀的視覺SLAM并行處理，丟棄具有其地標(biāo)測量的整個幀。
在是關(guān)鍵幀的情況下，簡單地丟棄所有關(guān)鍵點測量的信息丟失將更顯著：在共同界標(biāo)觀測中編碼的最早的兩個關(guān)鍵幀之間的所有相對姿勢信息將丟失。因此，我們另外將在k1幀中可見但在最近的關(guān)鍵幀中不可見的地標(biāo)邊緣化。圖7描繪了該過程。于是矩陣的稀疏性再次得以保留。
圖7：xTc-S被判斷為關(guān)鍵幀的邊緣化過程：最老的關(guān)鍵幀xTk1被舍棄。
III. 結(jié)論
本文提出了一種將慣性測量與基于關(guān)鍵幀的視覺SLAM緊耦合的方法。非線性優(yōu)化中的誤差項組合由特征點檢測和IMU誤差得到，因此取代了對任何調(diào)諧參數(shù)的需要。使用文章提出的方法，我們可以得到重力方向的全局一致性和在IMU運動學(xué)模型下更魯棒的外點剔除。同時，保留了基于關(guān)鍵幀非線性優(yōu)化的所有優(yōu)點，例如靜止姿態(tài)保持。使用立體相機和IMU傳感器獲得的結(jié)果證明了所提出的框架的可以實時操作，同時對比純視覺SLAM或松散耦合方法表現(xiàn)出更好的精度和魯棒性。
參考文獻
[1] T. Barfoot, J. R. Forbes, and P. T.Furgale. Pose estimation using linearized rotations and quaternion algebra.Acta Astronautica, 68(12):101 – 112, 2011.
[2] T-C. Dong-Si and A. I. Mourikis. Motiontracking with fixed-lag smoothing: Algorithm and consistency analysis. InProceedings of the IEEE International Conference on Robotics and Automation(ICRA), 2011.
[3] P. T. Furgale. Extensions to the VisualOdometry Pipeline for the Exploration of Planetary Surfaces. PhD thesis,University of Toronto, 2011.
[4] P. T.Furgale, J. Rehder, and R. Siegwart. Unified temporaland spatial calibration for multi-sensor systems. In Proc. of the IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS), 2013. Toappear.
[5] A. Geiger, P. Lenz, and R. Urtasun. Arewe ready for autonomous driving? the KITTI vision benchmark suite. In Proc. ofthe IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2012.
[6] J. A. Hesch, D. G. Kottas, S. L.Bowman, and S. I. Roumeliotis. Towards consistent vision-aided inertialnavigation. In Proc. of the Int’l Workshop on the Algorithmic Foundations ofRobotics (WAFR), 2012.
[7] V. Indelman, S. Williams, M. Kaess, andF. Dellaert. Factor graph based incremental smoothing in inertial navigationsystems. In Information Fusion (FUSION), International Conference on, 2012.
[8] E. S. Jones and S. Soatto.Visual-inertial navigation, mapping and localization: A scalable real-timecausal approach. International Journal of Robotics Research (IJRR),30(4):407–430, 2011.
[9] J. Kelly and G. S. Sukhatme.Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensorselfcalibration. International Journal of Robotics Research (IJRR),30(1):56–79, 2011.
[10] K. Konolige, M. Agrawal, and J. Sola.Large-scale visual odometry for rough terrain. In Robotics Research, pages201–212. Springer, 2011.
[11] R. Kummerle, G. Grisetti, H. Strasdat,K. Konolige, ¨ and W. Burgard. g 2o: A general framework for graphoptimization. In Proceedings of the IEEE International Conference on Roboticsand Automation (ICRA), 2011.
[12] S.Leutenegger, M. Chli, and R.Y. Siegwart. BRISK: Binaryrobust invariant scalable keypoints. In Proceedings of the IEEE InternationalConference on Computer Vision (ICCV), 2011.
[13] C. Mei, G. Sibley, M. Cummins, P. M.Newman, and I. D. Reid. Rslam: A system for large-scale mapping inconstant-time using stereo. International Journal of Computer Vision, pages198–214, 2011.
[14] A. I. Mourikis and S. I. Roumeliotis.A multistate constraint Kalman filter for vision-aided inertial navigation. InProceedings of the IEEE International Conference on Robotics and Automation(ICRA), 2007.
[15] A. Ranganathan, M. Kaess, and F.Dellaert. Fast 3d pose estimation with out-of-sequence measurements. In Proc.of the IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2007.
[16] G. Sibley, L. Matthies, and G.Sukhatme. Sliding window filter with application to planetary landing. Journalof Field Robotics, 27(5):587–608, 2010.
[17] D. Sterlow and S. Singh. Motionestimation from image and intertial measurements. International Journal ofRobotics Research (IJRR), 23(12):1157–1195, 2004.
[18] H. Strasdat, J. M. M. Montiel, and A.J. Davison. Realtime monocular SLAM: Why filter? In Proceedings of the IEEEInternational Conference on Robotics and Automation (ICRA), 2010.
[19] B. Triggs, P. Mclauchlan, R. Hartley,and A. Fitzgibbon. Bundle adjustment – a modern synthesis. In VisionAlgorithms: Theory and Practice, September, 1999, pages 298–372.Springer-Verlag, 1999.
[20] S. Weiss,M.W. Achtelik, S. Lynen, M. Chli, and R. Siegwart. Real-timeonboard visual-inertial state estimation and self-calibration of MAVs inunknown environments. In Proc. of the IEEE International Conference on Roboticsand Automation (ICRA), 2012.
 
 
分享到：
 
反對 0 舉報 0 收藏 0 評論 0
滬ICP備11026917號-25