基于圖的視覺slam方法
【專利摘要】本發(fā)明公開了一種基于圖的視覺SLAM方法,基于圖像的自然特征概率向量表示可以得到圖像視覺特征間的匹配關系,利用圖像間的空間幾何關系計算兩幀間的相對位姿。利用連續(xù)圖像之間的對應關系獲得視覺里程計的數(shù)據(jù)關聯(lián),由此得到圖像序列中的所有約束。將攝像頭相對位姿看作地圖中的節(jié)點,圖像幀間的空間約束關系表示為邊,構建基于攝像頭相對位姿估計的軌跡地圖。最后采用最大似然法優(yōu)化地圖,通過隨機梯度下降法得到最優(yōu)化位姿估計。在實驗室環(huán)境下對本發(fā)明提出的方法進行了相關實驗,并展示了機器人的運行軌跡,證明了本發(fā)明算法的有效性。
【專利說明】基于圖的視覺SLAM方法
【技術領域】
[0001] 本發(fā)明涉及一種基于圖的視覺SLAM方法。
【背景技術】
[0002] 大多數(shù)視覺SLAM算法采用特征跟蹤的方法,將視覺特征作為自然路標構建環(huán)境 特征地圖,并同時通過與之前已創(chuàng)建的環(huán)境地圖(自然路標庫)中的路標進行匹配,對機器 人當前位姿進行估計實現(xiàn)機器人定位。由于地圖構建與位姿估計兩個過程都有噪聲,因此 整個SLAM過程具有不確定性。
[0003] 目前最流行的兩大算法EKF-SLAM和PF-SLAM以概率描述信息的不確定性, Siegwart等提出了一種基于單攝像機的EKF-SLAM算法,通過多特征匹配來定位并減少定 位誤差,算法的不足之處在于需要建立運動模型和觀測模型,計算復雜度很高,并且缺乏 閉環(huán)能力,數(shù)據(jù)一旦關聯(lián)錯誤會帶入到整個SLAM狀態(tài)估計中,可能使整個預測過程發(fā)散。 Murphy和Russel等提出了一種PF-SLAM算法,它的復雜度隨特征數(shù)目對數(shù)地增長,不像 EKF-SLAM那樣平方地增長,并且不需要運動模型線性化。
【發(fā)明內容】
[0004] 本發(fā)明米用基于圖(graph-based),也稱為基于網(wǎng)絡(network-based)的SLAM方 法,不同于EKF-SLAM和PF-SLAM,不需要建立嚴格的運動模型和觀測模型,只需要通過所有 的觀測信息以及幀間的空間約束關系,最終構建一個完整的機器人位姿路徑地圖。地圖中 的點代表機器人位姿,邊代表位姿之間的約束。機器人不同時刻的位姿之間存在關聯(lián),關聯(lián) 其實是一種約束,約束用邊來表示形成了點的結構圖。
[0005] 本發(fā)明將利用連續(xù)圖像幀間的對應關系解決局部數(shù)據(jù)關聯(lián)問題。本發(fā)明的環(huán)境為 自然特征環(huán)境,自然特征通過對獲取的圖像序列進行視覺特征提取得到?;趫D的SLAM方 法不需要明確的攝像機運動模型,只需要批量處理順序獲取的所有圖像,利用圖像間內在 的多視角幾何約束即可同時估計特征位置和攝像機位姿。地圖創(chuàng)建部分利用觀測信息和幀 間的空間約束關系構建攝像機位姿地圖,地圖優(yōu)化部分基于極大似然法最小化約束誤差得 到最優(yōu)位姿估計,本發(fā)明通過隨機梯度下降法最小化約束誤差得到極大似然地圖。
[0006] 本發(fā)明的技術解決方案是:
[0007] -種基于圖的視覺SLAM方法,
[0008] 對攝像頭獲取的每一幀圖像進行視覺特征提取,基于自然視覺單詞的概率模型表 示每幀圖像,基于圖像的概率向量表示得到匹配的特征點對,利用同一個特征點在不同時 刻的圖像坐標得到機器人在這兩個時刻的相對位姿關系,通過立體視距技術計算每幀圖像 代表的相對位姿;
[0009] 機器人不同時刻的位姿之間存在關聯(lián),基于圖像序列中連續(xù)圖像之間的位姿轉換 得到視覺里程計的數(shù)據(jù)關聯(lián),得到圖像幀間的空間約束關系;
[0010] 將攝像頭相對位姿看作地圖中的節(jié)點,圖像幀間的空間約束關系表示為邊,構建 基于攝像機相對位姿估計的軌跡地圖;
[0011] 通過調整圖中節(jié)點的位置進行最優(yōu)位姿序列估計,通過隨機梯度下降法最小化約 束誤差得到最優(yōu)位姿地圖。
[0012] 優(yōu)選地,為得到當前時刻所有相機的位姿集合Xciim = (Xci, 每個當前位姿需 要計算相對于初始時刻k=0相機局部坐標系的運動估計;
[0013] 由于不同時刻轉換矩陣Tk(k = 1…n)之間存在連接關系,當前位姿xn = xn_Jn,xQ 為初始時刻的相機位姿,由此通過計算圖像Ilrt與Ik之間的轉換關系Tk把所有轉換關系連 接起來即可增量得到相機序列運動軌跡X 1 :n。
[0014] 優(yōu)選地,計算攝像機相對運動Tk :
[0015] 首先估計相對位姿和兩幀攝像機的運動方向,然后恢復這兩幀中觀測到的特征點 的位姿,最后通過路標位姿的匹配計算這些路標的正確尺度,進行估計相機運動。
[0016] 優(yōu)選地,由本質矩陣E進行分解可以得到旋轉矩陣R和平移矢量I, I與實際平移 矢量t相差一個比例系數(shù),最終可以得到k-1時刻攝像頭局部坐標系下,k時刻的攝像頭位 姿表示為^ 得到每個時刻的相對位姿后,將所有時刻的這些相對 > 轉換放到一個結構IL 中,以表示連續(xù)圖像節(jié)點間的關聯(lián)。
[0017] 優(yōu)選地,假設第j時刻成功檢測到當前圖像與第i時刻的圖像形成閉環(huán),則兩個圖 像間的相對位姿表示為言息矩陣為該閉環(huán)數(shù)據(jù)關聯(lián)加入到連 續(xù)幀的數(shù)據(jù)結構中,形成一個新的結構^
[0018] 優(yōu)選地,假設節(jié)點i和節(jié)點j之間已經(jīng)存在約束薄~,而某一時刻又得到兩個節(jié)點 間的另一個約束if,如此兩個約束可以融入到一個約束中,融合的標準如下:
【權利要求】
1. 一種基于圖的視覺SLAM方法,其特征在于: 對攝像頭獲取的每一幀圖像進行視覺特征提取,基于自然視覺單詞的概率模型表示每 幀圖像,基于圖像的概率向量表示得到匹配的特征點對,利用同一個特征點在不同時刻的 圖像坐標得到機器人在這兩個時刻的相對位姿關系,通過立體視距技術計算每幀圖像代表 的相對位姿; 機器人不同時刻的位姿之間存在關聯(lián),基于圖像序列中連續(xù)圖像之間的位姿轉換得到 視覺里程計的數(shù)據(jù)關聯(lián),得到圖像幀間的空間約束關系; 將攝像頭相對位姿看作地圖中的節(jié)點,圖像幀間的空間約束關系表示為邊,構建基于 攝像機相對位姿估計的軌跡地圖; 通過調整圖中節(jié)點的位置進行最優(yōu)位姿序列估計,通過隨機梯度下降法最小化約束誤 差得到最優(yōu)位姿地圖。
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相差一個比例系數(shù),最終可得到k-Ι時 刻攝像頭局部坐標系下,k時刻的攝像頭位姿表示為:X::_: =Ut: 得到 f 每個時刻的相對位姿后,將所有時刻的這些相對轉換放到一個結構=( > 中,以表示連續(xù)圖像節(jié)點間的關聯(lián)。
5. 如權利要求1所述的基于圖的視覺SLAM方法,其特征在于,假設第j時刻成 功檢測到當前圖像與第i時刻的圖像形成閉環(huán),則兩個圖像間的相對位姿表示為: V=K )-泛J信息矩陣為Ω^.,將該閉環(huán)數(shù)據(jù)關聯(lián)加入到連續(xù)幀的數(shù)據(jù)結構中,形 成一個新的結構η=?χ;:.α:: > O
6. 如權利要求5所述的基于圖的視覺SLAM方法,其特征在于,假設節(jié)點i和節(jié)點j之 間已經(jīng)存在約束左而某一時刻又得到兩個節(jié)點間的另一個約束i;:如此兩個約束可以 , v , 融入到一個約束中,融合的標準如下:
7. 如權利要求6所述的基于圖的視覺SLAM方法,其特征在于,將圖像幀看作頂點集V, 圖像幀之間的位姿關聯(lián)作為邊的集合E,連接兩幀圖像的每條路徑代表一個相關位姿估計, 由此構建攝像頭位姿軌跡地圖G = (V,E)。
8. 如權利要求1-7任一項所述的基于圖的視覺SLAM方法,其特征在于:采用隨機梯 度下降法進行地圖優(yōu)化,通過迭代地選擇約束X并移動圖中的節(jié)點來實現(xiàn),節(jié)點的更新如 下:
其中,X是圖中攝像頭的位姿變量集合;m為迭代次數(shù)Jij.是&的雅可比矩陣, Qij.是觀測值的不確定信息矩陣,為約束誤差的余;F1是一個先驗矩陣,H是系統(tǒng)的 Hessian矩陣,代表約束誤差函數(shù)的曲率,使用H的近似值
!個可逆 矩陣,
9. 如權利要求8所述的基于圖的視覺SLAM方法,其特征在于:找到一個變量X/使代價 函數(shù)Fu(X)最小,通過迭代選擇初始值X 1計算得到新的節(jié)點序列,序列在一定條件下收斂 使Fu(X)最小從而求得解f ;函數(shù)Fu(X)對變量X丨:的梯度為,VfTXp為一個負 向量,梯度方向是Fij(X)減少最快的方向;由此得到其中,將Xij定義為 λ ij = I/ O ijin),m 表示前迭代的次數(shù),參數(shù) K u = diag I (j-i) Ω u I。
10. 如權利要求1-7任一項所述的基于圖的視覺SLAM方法,其特征在于:地圖優(yōu)化過 程的迭代步驟為: 步驟1 :選取初始節(jié)點X1,給定允許約束誤差a > 0, b > 0,并令m = 0 ; 步驟2 :計算梯度下降方向向量. > 步驟3 :判斷VJYxP是否滿足約束條件Of滿足則轉向步驟8,否則繼 續(xù); 步驟4 :計算最佳學習速率λ u,得到新節(jié)點xm+1 ; 步驟5 :所有節(jié)點組成一個位姿序列X = (X1, x2,…,xk,xk+1)T ; 步驟6 :判斷代價函數(shù)是否滿足Fu(Xk+1) -Fu(Xk)彡b,滿足則轉向步驟8,否則繼續(xù); 步驟7 :迭代次數(shù)加1,令m = m+Ι,轉向步驟2 ; 步驟8 :輸出結果,結束。
【文檔編號】G01C21/32GK104374395SQ201410125956
【公開日】2015年2月25日 申請日期:2014年3月31日 優(yōu)先權日:2014年3月31日
【發(fā)明者】梁志偉, 徐小根 申請人:南京郵電大學