基於圖的視覺slam方法

2023-06-26 17:16:51 2

基於圖的視覺slam方法
【專利摘要】本發明公開了一種基於圖的視覺SLAM方法，基於圖像的自然特徵概率向量表示可以得到圖像視覺特徵間的匹配關係，利用圖像間的空間幾何關係計算兩幀間的相對位姿。利用連續圖像之間的對應關係獲得視覺裡程計的數據關聯，由此得到圖像序列中的所有約束。將攝像頭相對位姿看作地圖中的節點，圖像幀間的空間約束關係表示為邊，構建基於攝像頭相對位姿估計的軌跡地圖。最後採用最大似然法優化地圖，通過隨機梯度下降法得到最優化位姿估計。在實驗室環境下對本發明提出的方法進行了相關實驗，並展示了機器人的運行軌跡，證明了本發明算法的有效性。
【專利說明】基於圖的視覺SLAM方法

【技術領域】
[0001] 本發明涉及一種基於圖的視覺SLAM方法。

【背景技術】
[0002] 大多數視覺SLAM算法採用特徵跟蹤的方法，將視覺特徵作為自然路標構建環境 特徵地圖，並同時通過與之前已創建的環境地圖（自然路標庫）中的路標進行匹配，對機器 人當前位姿進行估計實現機器人定位。由於地圖構建與位姿估計兩個過程都有噪聲，因此 整個SLAM過程具有不確定性。
[0003] 目前最流行的兩大算法EKF-SLAM和PF-SLAM以概率描述信息的不確定性， Siegwart等提出了一種基於單攝像機的EKF-SLAM算法，通過多特徵匹配來定位並減少定 位誤差，算法的不足之處在於需要建立運動模型和觀測模型，計算複雜度很高，並且缺乏 閉環能力，數據一旦關聯錯誤會帶入到整個SLAM狀態估計中，可能使整個預測過程發散。 Murphy和Russel等提出了一種PF-SLAM算法，它的複雜度隨特徵數目對數地增長，不像 EKF-SLAM那樣平方地增長，並且不需要運動模型線性化。


【發明內容】

[0004] 本發明米用基於圖（graph-based)，也稱為基於網絡（network-based)的SLAM方 法，不同於EKF-SLAM和PF-SLAM，不需要建立嚴格的運動模型和觀測模型，只需要通過所有 的觀測信息以及幀間的空間約束關係，最終構建一個完整的機器人位姿路徑地圖。地圖中 的點代表機器人位姿，邊代表位姿之間的約束。機器人不同時刻的位姿之間存在關聯，關聯 其實是一種約束，約束用邊來表示形成了點的結構圖。
[0005] 本發明將利用連續圖像幀間的對應關係解決局部數據關聯問題。本發明的環境為 自然特徵環境，自然特徵通過對獲取的圖像序列進行視覺特徵提取得到。基於圖的SLAM方 法不需要明確的攝像機運動模型，只需要批量處理順序獲取的所有圖像，利用圖像間內在 的多視角幾何約束即可同時估計特徵位置和攝像機位姿。地圖創建部分利用觀測信息和幀 間的空間約束關係構建攝像機位姿地圖，地圖優化部分基於極大似然法最小化約束誤差得 到最優位姿估計，本發明通過隨機梯度下降法最小化約束誤差得到極大似然地圖。
[0006] 本發明的技術解決方案是：
[0007] -種基於圖的視覺SLAM方法，
[0008] 對攝像頭獲取的每一幀圖像進行視覺特徵提取，基於自然視覺單詞的概率模型表 示每幀圖像，基於圖像的概率向量表示得到匹配的特徵點對，利用同一個特徵點在不同時 刻的圖像坐標得到機器人在這兩個時刻的相對位姿關係，通過立體視距技術計算每幀圖像 代表的相對位姿；
[0009] 機器人不同時刻的位姿之間存在關聯，基於圖像序列中連續圖像之間的位姿轉換 得到視覺裡程計的數據關聯，得到圖像幀間的空間約束關係；
[0010] 將攝像頭相對位姿看作地圖中的節點，圖像幀間的空間約束關係表示為邊，構建 基於攝像機相對位姿估計的軌跡地圖；
[0011] 通過調整圖中節點的位置進行最優位姿序列估計，通過隨機梯度下降法最小化約 束誤差得到最優位姿地圖。
[0012] 優選地，為得到當前時刻所有相機的位姿集合Xciim = (Xci, 每個當前位姿需 要計算相對於初始時刻k=0相機局部坐標系的運動估計；
[0013] 由於不同時刻轉換矩陣Tk(k = 1…n)之間存在連接關係，當前位姿xn = xn_Jn，xQ 為初始時刻的相機位姿，由此通過計算圖像Ilrt與Ik之間的轉換關係Tk把所有轉換關係連 接起來即可增量得到相機序列運動軌跡X 1 :n。
[0014] 優選地，計算攝像機相對運動Tk :
[0015] 首先估計相對位姿和兩幀攝像機的運動方向，然後恢復這兩幀中觀測到的特徵點 的位姿，最後通過路標位姿的匹配計算這些路標的正確尺度，進行估計相機運動。
[0016] 優選地，由本質矩陣E進行分解可以得到旋轉矩陣R和平移矢量I, I與實際平移 矢量t相差一個比例係數，最終可以得到k-1時刻攝像頭局部坐標系下，k時刻的攝像頭位 姿表示為^ 得到每個時刻的相對位姿後，將所有時刻的這些相對 > 轉換放到一個結構IL 中，以表示連續圖像節點間的關聯。
[0017] 優選地，假設第j時刻成功檢測到當前圖像與第i時刻的圖像形成閉環，則兩個圖 像間的相對位姿表示為言息矩陣為該閉環數據關聯加入到連 續幀的數據結構中，形成一個新的結構^
[0018] 優選地，假設節點i和節點j之間已經存在約束薄~,而某一時刻又得到兩個節點 間的另一個約束if，如此兩個約束可以融入到一個約束中，融合的標準如下：

【權利要求】
1. 一種基於圖的視覺SLAM方法，其特徵在於： 對攝像頭獲取的每一幀圖像進行視覺特徵提取，基於自然視覺單詞的概率模型表示每 幀圖像，基於圖像的概率向量表示得到匹配的特徵點對，利用同一個特徵點在不同時刻的 圖像坐標得到機器人在這兩個時刻的相對位姿關係，通過立體視距技術計算每幀圖像代表 的相對位姿； 機器人不同時刻的位姿之間存在關聯，基於圖像序列中連續圖像之間的位姿轉換得到 視覺裡程計的數據關聯，得到圖像幀間的空間約束關係； 將攝像頭相對位姿看作地圖中的節點，圖像幀間的空間約束關係表示為邊，構建基於 攝像機相對位姿估計的軌跡地圖； 通過調整圖中節點的位置進行最優位姿序列估計，通過隨機梯度下降法最小化約束誤 差得到最優位姿地圖。
2. 如權利要求1所述的基於圖的視覺SLAM方法，其特徵在於：為得到當前時刻所有相 機的位姿集合X0 :n = {X(I，…，X1J，每個當前位姿需要計算相對於初始時刻k=0相機局部坐 標系的運動估計； 由於不同時刻轉換矩陣Tk(k = L···η)之間存在連接關係，當前位姿xn = XlriTn，Xci為 初始時刻的相機位姿，由此通過計算圖像Ilrt與Ik之間的轉換關係Tk，把所有轉換關係連接 起來即可增量得到相機序列運動軌跡X 1 :n。
3. 如權利要求2所述的基於圖的視覺SLAM方法，其特徵在於，計算攝像機相對運動 Tk = 首先估計相對位姿和兩幀攝像機的運動方向，然後恢復這兩幀中觀測到的特徵點的位 姿，最後通過路標位姿的匹配計算這些路標的正確尺度，進行估計相機運動。
4. 如權利要求1所述的基於圖的視覺SLAM方法，其特徵在於：由本質矩陣E進行分解 得到旋轉矩陣R和平移矢量Γ _實際平移矢量t相差一個比例係數，最終可得到k-Ι時 刻攝像頭局部坐標系下，k時刻的攝像頭位姿表示為：X::_: =Ut: 得到 f 每個時刻的相對位姿後，將所有時刻的這些相對轉換放到一個結構=( > 中，以表示連續圖像節點間的關聯。
5. 如權利要求1所述的基於圖的視覺SLAM方法，其特徵在於，假設第j時刻成 功檢測到當前圖像與第i時刻的圖像形成閉環，則兩個圖像間的相對位姿表示為： V=K )-泛J信息矩陣為Ω^.，將該閉環數據關聯加入到連續幀的數據結構中，形 成一個新的結構η=?χ;:.α:: > O
6. 如權利要求5所述的基於圖的視覺SLAM方法，其特徵在於，假設節點i和節點j之 間已經存在約束左而某一時刻又得到兩個節點間的另一個約束i;:如此兩個約束可以 ， v ， 融入到一個約束中，融合的標準如下：
7. 如權利要求6所述的基於圖的視覺SLAM方法，其特徵在於，將圖像幀看作頂點集V， 圖像幀之間的位姿關聯作為邊的集合E，連接兩幀圖像的每條路徑代表一個相關位姿估計， 由此構建攝像頭位姿軌跡地圖G = (V，E)。
8. 如權利要求1-7任一項所述的基於圖的視覺SLAM方法，其特徵在於：採用隨機梯 度下降法進行地圖優化，通過迭代地選擇約束X並移動圖中的節點來實現，節點的更新如 下：
其中，X是圖中攝像頭的位姿變量集合;m為迭代次數Jij.是&的雅可比矩陣， Qij.是觀測值的不確定信息矩陣，為約束誤差的餘；F1是一個先驗矩陣，H是系統的 Hessian矩陣,代表約束誤差函數的曲率,使用H的近似值
！個可逆 矩陣，
9. 如權利要求8所述的基於圖的視覺SLAM方法，其特徵在於：找到一個變量X/使代價 函數Fu(X)最小，通過迭代選擇初始值X 1計算得到新的節點序列，序列在一定條件下收斂 使Fu(X)最小從而求得解f ;函數Fu(X)對變量X丨:的梯度為，VfTXp為一個負 向量,梯度方向是Fij(X)減少最快的方向；由此得到其中，將Xij定義為 λ ij = I/ O ijin)，m 表示前迭代的次數，參數 K u = diag I (j-i) Ω u I。
10. 如權利要求1-7任一項所述的基於圖的視覺SLAM方法，其特徵在於：地圖優化過 程的迭代步驟為： 步驟1 :選取初始節點X1，給定允許約束誤差a > 0, b > 0,並令m = 0 ; 步驟2 :計算梯度下降方向向量. > 步驟3 :判斷VJYxP是否滿足約束條件Of滿足則轉向步驟8,否則繼 續； 步驟4 :計算最佳學習速率λ u，得到新節點xm+1 ; 步驟5 :所有節點組成一個位姿序列X = (X1, x2,…，xk，xk+1)T ; 步驟6 :判斷代價函數是否滿足Fu(Xk+1) -Fu(Xk)彡b，滿足則轉向步驟8,否則繼續； 步驟7 :迭代次數加1，令m = m+Ι，轉向步驟2 ; 步驟8 :輸出結果，結束。
【文檔編號】G01C21/32GK104374395SQ201410125956
【公開日】2015年2月25日 申請日期:2014年3月31日 優先權日:2014年3月31日
【發明者】梁志偉, 徐小根 申請人:南京郵電大學