一種可提高機器人位姿一致性的orb關鍵幀閉環(huán)檢測slam方法
【專利摘要】一種可提高機器人位姿一致性的ORB關鍵幀閉環(huán)檢測SLAM方法，包括如下步驟：首先，采用RGB?D傳感器獲取環(huán)境的彩色信息與深度信息，使用ORB特征提取圖像特征；接著，通過基于RANSAC?ICP幀間配準算法實現(xiàn)了機器人位姿估計，構建初始位姿圖；最后，通過提取KeyFrame關鍵幀中的ORB特征構建BoVW(視覺詞袋)，當前關鍵幀與BoVW中的單詞進行相似性比較，實現(xiàn)閉環(huán)關鍵幀檢測，通過關鍵幀幀間配準檢測添加位姿圖約束，得到全局最優(yōu)的機器人位姿。本發(fā)明提供一種可提高機器人位姿一致性、環(huán)境地圖構建質量較高以及優(yōu)化效率高的ORB關鍵幀閉環(huán)檢測SLAM方法。
【專利說明】
一種可提高機器人位姿一致性的ORB關鍵幀閉環(huán)檢測SLAM 方法
技術領域
[0001 ] 本發(fā)明涉及機器人的同時定位與地圖構建（Simultaneously Localization and Mapping，SLAM)領域，尤其是一種可提高機器人位姿一致性的ORB關鍵幀閉環(huán)檢測SLAM方 法。
【背景技術】
[0002] 為了在未知室內環(huán)境下實現(xiàn)自主移動與導航，智能機器人應具有構建室內環(huán)境地 圖，并同時具有在全局地圖中定位的能力，上述過程同時實現(xiàn)稱為機器人的同時定位與地 圖構建（Simultaneously Localization and Mapping，SLAM)。當機器人處于室外環(huán)境時， 可通過高精度的GPS和先驗地圖實現(xiàn)全局地圖下的定位。但當機器人處于室內環(huán)境，或處于 GPS不可用的環(huán)境(水下、礦洞)時，就必須采取其他方式實現(xiàn)機器人SLAM。
[0003] 現(xiàn)有的室內SLAM方法存在的缺陷：定位一致性差，環(huán)境地圖構建質量不高以及優(yōu) 化效率低。

【發(fā)明內容】

[0004] 為了克服已有室內機器人定位與環(huán)境地圖構建中定位一致性差，環(huán)境地圖構建質 量不高以及優(yōu)化效率低的不足，本發(fā)明提供一種可提高機器人位姿一致性、環(huán)境地圖構建 質量較高以及優(yōu)化效率高的0RB關鍵幀閉環(huán)檢測SLAM方法。
[0005] 本發(fā)明解決其技術問題所采用的技術方案是：
[0006] -種可提高機器人位姿一致性的0RB關鍵幀閉環(huán)檢測SLAM方法，包括如下步驟：
[0007] 首先，采用RGB-D傳感器獲取環(huán)境的彩色信息與深度信息，使用0RB特征提取圖像 特征；
[0008] 接著，通過基于RANSAC-ICP幀間配準算法實現(xiàn)了機器人位姿估計，構建初始位姿 圖；
[0009] 最后，通過提取KeyFrame關鍵幀中的0RB特征構建BoVW(視覺詞袋），當前關鍵幀與 BoVW中的單詞進行相似性比較，實現(xiàn)閉環(huán)關鍵幀檢測，通過關鍵幀幀間配準檢測添加位姿 圖約束，得到全局最優(yōu)的機器人位姿。
[0010] 進一步，所述基于RANSAC-ICP幀間配準算法包括基于RANSAC算法的初始粗配準與 基于ICP算法的精確配準，過程如下：
[0011] 首先通過RANSAC算法對特征集進行初始配準，找到兩幀中的最佳平移旋轉變換， 隨后通過重投影誤差RANSAC優(yōu)化目標函數(shù)：
[0012]
(1-10)
[0013]式中，Af包含了特征內點；乂//分別表示場景幀和參考幀上關聯(lián)點對的空間坐標； T(p)=R · p+t表示平移旋轉變換矩陣;3I(P)為參考幀攝像頭上的投影變換函數(shù)。
[0014] 優(yōu)選的，式（1-10)通過幀間稀疏BA方法進行優(yōu)化；ICP迭代過程中，首先通過K-D樹 檢索最近點實現(xiàn)點云關聯(lián)，通過優(yōu)化關聯(lián)特征點與點云的聯(lián)合誤差函數(shù)實現(xiàn)全局位姿變換 矩陣的優(yōu)化，權重因子W表示關聯(lián)點對在誤差函數(shù)中的重要性；
[0015]
(Ml)
[0016] 式中，α為各投影變換函數(shù)件的權重因子。
[0017] 再進一步，所述閉環(huán)檢測方法中，通過KeyFrame選取算法，選取過程為：當前幀與 上一關鍵幀通過RANSAC進行配準，如果配準內點值小于規(guī)定閾值，則認定其為關鍵幀;采用 關鍵幀視覺詞袋模型，該模型將關鍵幀的0RB特征聚類，利用視覺詞典將特征描述子映射為 視覺單詞，通過對比關鍵幀中的"單詞"，可判斷兩個關鍵幀是否形成閉環(huán)。
[0018] 所述關鍵幀選取過程描述為:在完成RANSAC-ICP精確估計后，利用當前幀與上一 關鍵幀再次進行RANSAC-ICP配準。如果當前幀與關鍵幀之間的配準誤差大于閾值，則認為 該幀所在場景為新場景，該幀設為關鍵幀，反之，該幀為普通幀，用于位姿圖構建，選取下一 幀進行關鍵幀比對，其數(shù)學過程可描述為：
[0019]
(1-12)
[0020] 其中，為離j時刻最近的關鍵幀;Fj為當前幀。
[0021 ]更進一步，所述閉環(huán)檢測方法的流程如下：
[0022] 1)首先從所有圖像中檢測并提取關鍵幀，假設機器人位于地圖場景中的某一關鍵 幀位置，將生成的關鍵幀圖像使用0RB特征描述符進行描述，關鍵幀圖像I可表示為描述符 集合 DzI-icU，···，^};
[0023] 2)提取每個關鍵幀場景的ORB特征并投影到BoVW中，將關鍵幀ORB特征點與視覺詞 典中的單詞進行關聯(lián)，視覺詞典可表示為Γ = ,.//J ,通過BoVW相似描述符聚類可構建 視覺詞典V，通過映射形成最鄰近視覺字典，每一個視覺單詞的0RB描述向量都是一個關聯(lián) 的視覺詞表；
[0024] 3)采用單詞相似度的方式計算"單詞"間距，形成關鍵幀場景的相似度，為獲得場 景相似度，計算關鍵幀圖像I u和Iv之間的余弦距離可得到二者BoVW的相似度，其過程為：
[0025] 每一關鍵幀由不同權重的詞匯聚類而成，每個詞匯在全部圖像N中出現(xiàn)的頻率 形成權重Wi:
[0026] wi = logio(N/ru) (1-12)
[0027] 式中m表示cU中包含圖像的數(shù)量，若BoVW中包含I V|個不同的視覺單詞，則圖像矢 量可描述為=[%，....〃，Γ，其中關鍵幀中的單詞權重為：
[0028]
(1-13)
[0029] 得到每個單詞權重后，即可求出其所在圖像的權重，通過相似函數(shù)S可計算鍵幀圖 像Iu和Iv之間的相似度：
[0030]
(1-14)
[0031] 通過上式可得相似矩陣進而判斷圖像間的相似度，圖像i和圖像j通過相似函數(shù)得 到相似度矩陣M1>J;
[0032] 通過視覺詞袋方法，關鍵幀閉環(huán)檢測問題可轉化為從圖像序列 至幌個相似度較高的子序列A= [ai，a2，…]和B= [h，b2，…]，從而實現(xiàn)閉環(huán)檢測。
[0033] 本發(fā)明中，隨機采樣一致性(RANSAC)是一種可用于提出數(shù)據(jù)離群點的迭代算法。 SLAM方法使用RANSAC算法可減少幀間配準過程中的圖像特征匹配離群點，實現(xiàn)一種初始一 致性較好的位姿估計，提高幀間匹配質量。RANSAC通過對輸入數(shù)據(jù)進行多次隨機采樣，每次 抽取具有代表性的少量數(shù)據(jù)對估計模型進行評估，根據(jù)已有模型對所有數(shù)據(jù)進行篩選，保 留誤差范圍內的數(shù)據(jù)稱為有效數(shù)據(jù)點或內點（inliers)，篩除誤差較大的數(shù)據(jù)離群點或外 點(outliers)。離群點一般是噪聲數(shù)據(jù)或異常數(shù)據(jù)，內點比例較高的幀間配準，所獲得的位 姿估計質量越高，越接近真實情況。
[0034] 假設前后幀間共有η對匹配的三維點，變換參數(shù)可用旋轉矩陣R和平移向量t表示， 則變換參數(shù)與三維點集之間的關系可表示為：
[0035] pci = rpPi+t (1-1)
[0036] 式中口^與?^分別代表當前幀和上一幀匹配的第i對三維點坐標，? = 1，2，···，η。設 識、Θ和φ分別為繞y軸、X軸和ζ軸旋轉的角度，稱為偏航角、俯仰角及翻滾角，統(tǒng)稱為姿態(tài)角， 則r與三個姿態(tài)角的關系為：
[0037]
[0038] 代表姿態(tài)增量的旋轉參數(shù)也可用四元數(shù)表示。二維平面中單位圓上任意一點對應 一個旋轉角度，可表示二維空間的點姿態(tài);在三維空間中，單位球上任意一點應繞兩軸旋轉 的角度，可描述某點的三維空間位姿；可推，在四維空間中，三維空間中任意的旋轉可由四 維單位球上的點q = [ qo，qi，q2，q3 ]T表示，q = [ qo，qi，q2，q3 ]T稱為四元數(shù)，即四維空間單位球 面上任意一點可描述繞三個軸的旋轉角度。三維旋轉通過單位四元數(shù)表示有兩種表示方 式:q、_q，但給定一個四元數(shù)，有且僅有一個三維旋轉可與之對應。使用四元數(shù)表示三維空 間的旋轉運動，可有效減少中間變量;與歐拉角相比，四元具有更穩(wěn)定的數(shù)值解。旋轉變換 矩陣r可由四元數(shù)q表示：
[0039]
(1 3)
[0040]
(卜 4)
[0041] 對方程（1-4)求解，若忽略三維點重建誤差，可以使用直接線性變換方程(Direct Linear transformation，DLT)，但該方法精度較差，對輸入數(shù)據(jù)比較敏感?？紤]到三維點重 建誤差，運動參數(shù)的估計可轉化為非線性最小化誤差和的優(yōu)化問題：
[0042]
(1-5)
[0043] 式（1-5)是一個最小二乘估計問題，通過最大似然法可實現(xiàn)非線性參數(shù)估計。本發(fā) 明通過RANSAC去除數(shù)據(jù)中的離群點，以提高位姿估計的準確性。
[0044] 迭代最近點（ICP)是一種可用于計算具有深度和彩色信息的立體點云數(shù)據(jù)間匹配 關系的算法，可篩選出滿足匹配關系的有效數(shù)據(jù)，實現(xiàn)位姿間的精確估計。RANSAC與ICP算 法相結合，可解決機器人初始配準不確定條件下的位姿準確估計，防止位姿估計陷入局部 最優(yōu)。其基本思想是先通過0RB特征匹配與RANSAC算法為ICP配準提供初始對齊，隨后通過 優(yōu)化特征點與深度點云的聯(lián)合誤差的ICP誤差來實現(xiàn)精確化。
[0045] 設存在兩組對應點云+,n，且存在 | |ai_T · bi| |〈dmax，其 中存在集合，假設點云A和B滿足正態(tài)分布： %~/V(K)，勿~W(K)，其中C/、Cf分別為A、B的協(xié)方差矩陣。如果集合2，身完 全對應，且所求協(xié)方差矩陣具有極小值，則有：
[0046] 爲二 T* 彖 (1-6)
[0047] 定義=6, -7〇,，T為任意剛性變換。已知4和4均服從高斯分布，的概率分布 可表示為：
[0048] d= MiSlCf f) (1-7)
[0049] 使用最大似然估計可迭代計算出轉換矩陣T:
[0050] (U)
[0051]
[0052] (1-9)。
[0053] 本發(fā)明的技術構思為:室內SLAM首先應通過傳感器獲取環(huán)境信息，如采用聲吶測 距傳感器獲取傳感器與障礙物之間的距離信息;二維激光測距儀使用線掃描的方式獲取本 體與障礙物之間的距離和角度信息;RGB-D視覺傳感器通過深度相機獲取外界環(huán)境距離信 息(Depth image)，同時可獲得與之對應的彩色信息（RGB image)，是一種新型視覺傳感器。 RGB-D信息經(jīng)過處理后可直接用于機器人定位、地圖創(chuàng)建與導航。與傳統(tǒng)視覺傳感器相比， RGB-D傳感器可直接獲得環(huán)境深度信息，數(shù)據(jù)處理過程簡單，傳感器數(shù)據(jù)適合三維地圖重 建，實時性高，地圖精度好。
[0054]通過0RB算子提取室內環(huán)境圖像數(shù)據(jù)幀中魯棒性較好的特征點，通過RANSAC-ICP 算法實現(xiàn)了初始位姿估計一致性不高情況下的幀間配準;通過RANSAC算法篩選符合內點閾 值條件的關鍵幀，以關鍵幀作為校驗約束，最終實現(xiàn)了一種全局一致性較高的位姿估計，保 證了機器人在室內環(huán)境下的定位精度與自建地圖模型的準確性。
[0055]通過RANSAC算法減少0RB特征離群點，提高初始位姿配準精度，與ICP算法相結合 實現(xiàn)了機器人在初始配準不確定的條件下精確位姿估計;基于0RB特征的關鍵幀詞袋閉環(huán) 檢測算法，將關鍵幀特征點聚類成為字典，根據(jù)字典單詞間的相似度判斷是否形成場景閉 環(huán)，修正配準錯誤，減少地圖冗余結構，生成一致性最優(yōu)的地圖。
[0056]本發(fā)明的有益效果主要表現(xiàn)在：
[0057] 1)位姿估計一致性高:本發(fā)明基于RANSAC-ICP幀間配準算法篩除了圖像特征離群 點，利用內點最多的特征匹配點實現(xiàn)了六自由度位姿估計，提高幀間配準生成的位姿一致 性;采用0RB關鍵幀閉環(huán)檢測算法添加額外全局位姿約束。由上述方法生成的機器人位姿一 致性高，定位與地圖較為精確。
[0058] 2)魯棒性:RANSAC算法可剔除圖像特征匹配過程中產(chǎn)生的錯誤匹配點，基于0RB關 鍵幀的閉環(huán)檢測算法可校正由傳感器測量及幀間配準導致的位姿估計錯誤。通過不斷校正 錯誤，使SLAM系統(tǒng)更健壯。
[0059] 3)運行效率:本發(fā)明中通過0RB算子提高了特征點檢測速度與特征點匹配速度，通 過改進RANSAC算法，減少了數(shù)據(jù)離群點，提高了圖像配準重合率，較少了錯誤匹配，增強了 SLAM系統(tǒng)前端的時效性;基于視覺詞袋模型的關鍵幀0RB特征增強了位姿約束，減少了位姿 估計中的幾余結構，提尚SLAM系統(tǒng)后端的時效性。因此，基于0RB關鍵幀閉環(huán)檢測的SLAM方 法的運行效率較高。
【附圖說明】
[0060] 圖1是可提高機器人位姿一致性的0RB關鍵幀閉環(huán)檢測SLAM方法系統(tǒng)框圖；
[0061 ]圖2是0RB關鍵幀閉環(huán)檢測SLAM方法的功能_吳塊關系圖；
[0062] 圖3是0RB關鍵幀閉環(huán)檢測SLAM方法的RANSAC-ICP方法流程圖；
[0063] 圖4是0RB關鍵幀閉環(huán)檢測SLAM方法的關鍵幀與閉環(huán)檢測關系圖；
[0064] 圖5是0RB關鍵幀閉環(huán)檢測SLAM方法中基于視覺詞袋的關鍵幀場景描述方法的流 程圖。
【具體實施方式】
[0065] 下面結合附圖對本發(fā)明作進一步描述。
[0066] 參照圖1~圖5，一種可提高機器人位姿一致性的0RB關鍵幀閉環(huán)檢測SLAM方法，包 括如下步驟：
[0067] 首先，采用RGB-D傳感器獲取環(huán)境的彩色信息與深度信息，使用0RB(〇riented FAST and rotated BRIEF)特征提取圖像特征；
[0068]接著，通過基于RANSAC-ICP幀間配準算法實現(xiàn)了機器人位姿估計，構建初始位姿 圖；
[0069]最后，通過提取KeyFrame關鍵幀中的0RB特征構建BoVW(視覺詞袋），當前關鍵幀與 BoVW中的單詞進行相似性比較，實現(xiàn)閉環(huán)關鍵幀檢測，通過關鍵幀幀間配準檢測添加位姿 圖約束，得到全局最優(yōu)的機器人位姿。
[0070]圖1所示一種可提高機器人位姿一致性的0RB關鍵幀閉環(huán)檢測SLAM系統(tǒng)框架，其中 提出使用基于RANSAC-ICP幀間配準的位姿估計算法和基于ORB關鍵幀(KeyFrame)的閉環(huán)檢 測方法。通過Asus Xtion或同類型的RGB-D傳感器獲取環(huán)境色彩信息與深度信息，通過ORB 算法加快圖像特征點檢測與描述子建立速度;結合相機小孔模型與深度信息，將二維圖像 轉換為三維彩色點云，并進行存儲;通過基于改進RANSAC-ICP幀間配準的位姿估計算法實 現(xiàn)機器人在初始配準不確定的條件下精確位姿估計，提高了機器人位姿估計一致性。基于 ORB特征的關鍵幀詞袋閉環(huán)檢測算法將關鍵幀特征點聚類成為字典，根據(jù)字典單詞間的相 似度判斷是否形成場景閉環(huán)，修正配準錯誤。通過閉環(huán)檢測可減少地圖冗余結構，生成具有 一致性的位姿估計與地圖模型。
[0071]圖2所示為基于0RB關鍵幀閉環(huán)檢測SLAM方法中各功能模塊之間的關系。0RB關鍵 幀閉環(huán)檢測SLAM方法以RGB-D三維點云序列{F}(其中，I為RGB圖像，D為深度圖像，p為圖像 匹配矩陣)為輸入，輸出為場景地圖的網(wǎng)格模型與位姿估計軌跡，模型中的網(wǎng)格頂點為點云 模型的數(shù)據(jù)點。
[0072] 圖3所示為0RB關鍵幀閉環(huán)檢測SLAM方法的RANSAC-ICP算法流程圖。通過RANSAC粗 配準可得到相對變換T(p)將作為RANSAC-ICP的輸入。T(p)函數(shù)表示對點p進行剛性變換，則 T(p) iRp+TAANSAC-ICP最終輸出為最優(yōu)相對變換(ρ)。
[0073] Extract_RGB_Point_Featrures函數(shù)分別可從待配準幀與參考幀中提取稀疏的 0RB特征點云Ps、Pt。
[0074] 隨后使用RANSAC算法從兩組特征點云中找到初始的剛性變換T(p)，通過在兩幀之 間找到匹配的特征完成剛性變換初始化。隨后，通過重復采樣，可以確定特征點的內點數(shù)， 變換后內點數(shù)最大的變換即為最優(yōu)變換。Perform_RANSAC_Alignment函數(shù)首先使用RANSAC 算法對特征集進行初始配準，找到兩幀中的最佳平移旋轉變換。隨后通過重投影誤差 RANSAC優(yōu)化目標函數(shù)：
[0075]
(1-10)
[0076] 式中，Af包含了內點；分別表示場景幀和參考幀上關聯(lián)點對的空間坐標;T(p) =R · p+t表示平移旋轉變換矩陣;3?(ρ)為參考幀攝像頭上的投影變換函數(shù)。上式可通過幀 間稀疏ΒΑ方法進行優(yōu)化
[0077] 步驟4-6為基于最小二乘法的ICP配準算法。該算法選擇對應點對進行迭代計算， 計算最優(yōu)剛體變換估計，直到滿足配準的收斂精度。其具體步驟描述如下：
[0078] 第一步，計算匕中的每一個點在Pt點集的最近對應點；
[0079]第二步，求得使上述對應點對的平均距離最小的剛體變換T*
[0080]
(1-11)
[0081 ]第三步，對pt使用上一步求得的平移和旋轉參數(shù)，得到新的變換點集；
[0082]第四步，如果新的變換點集與參考點集滿足平均距離小于閾值時，則停止迭代計 算;否則新的變換點集作為新的Pt繼續(xù)迭代，直到滿足目標函數(shù)的要求為止。
[0083]圖4所示為ORB關鍵幀閉環(huán)檢測SLAM方法的關鍵幀與閉環(huán)檢測關系圖。可描述為： 關鍵幀閉環(huán)檢測實現(xiàn)過程為:輸入連續(xù)標記的圖像，根據(jù)RANSAC內點作為篩選條件選取關 鍵幀；以關鍵幀的0RB特征建立詞典，比對詞典相似性實現(xiàn)全局閉環(huán)檢測，經(jīng)過關鍵幀位姿 變換，輸出閉環(huán)校正后的位姿圖，實現(xiàn)機器人位姿估計校正。關鍵幀閉環(huán)檢測算法可在保證 幀間配準的有效性的同時提高了檢測實時性，實現(xiàn)了一種健壯、實時的SLAM后端優(yōu)化算法。 [0084]圖5所示為0RB關鍵幀閉環(huán)檢測SLAM方法中基于視覺詞袋的關鍵幀場景描述算法。 閉環(huán)檢測首先通過將關鍵幀的0RB特征進行聚類，采用視覺詞典(BoVW)將特征描述子映射 為視覺單詞，通過對比關鍵幀視覺詞典中的"單詞"即可判斷是否形成閉環(huán)。BoVW通過結構 化描述關鍵幀，可使圖像特征具有不變性，采取離散的單詞表示方法可提高閉環(huán)檢測效率。
[0085] 對于本領域技術人員而言，顯然本發(fā)明不限于示例性實施例的細節(jié)，而且在不背 離本發(fā)明的精神或基本特征的情況下，能夠以其他的具體形式實現(xiàn)本發(fā)明。因此，無論從哪 一點來看，均應將實施例看作是示范性的，而且是非限制性的，本發(fā)明額范圍由所附權力要 求而不是上述說明先定，因為旨在將落在權力要求的等同要件的含義和范圍內的所有變化 囊括在本發(fā)明內。
[0086] 此外，應當理解，雖然本說明書按照實施方式加以描述，但并非每個實施方式僅包 含一個獨立的技術方案，說明書的這種敘述方式僅僅是為了清除期間，本領域技術人員應 當將說明書作為一個整體，各實施例中的技術方案也可以經(jīng)適當組合，形成本領域技術人 員可以理解的其他實施方式。
【主權項】
1. 一種可提高機器人位姿一致性的ORB關鍵幀閉環(huán)檢測SLAM方法，其特征在于:所述方 法包括如下步驟： 首先，采用RGB-D傳感器獲取環(huán)境的彩色信息與深度信息，使用ORB特征提取圖像特征； 接著，通過基于RANSAC-ICP幀間配準算法實現(xiàn)了機器人位姿估計，構建初始位姿圖； 最后，通過提取KeyFrame關鍵幀中的ORB特征構建視覺詞袋BoVW，當前關鍵幀與BoVW中 的單詞進行相似性比較，實現(xiàn)閉環(huán)關鍵幀檢測，通過關鍵幀幀間配準檢測添加位姿圖約束， 得到全局最優(yōu)的機器人位姿。2. 如權利要求1所述的一種可提高機器人位姿一致性的ORB關鍵幀閉環(huán)檢測SLAM方法， 其特征在于:所述基于RANSAC-ICP幀間配準算法包括基于RANSAC算法的初始粗配準與基于 ICP算法的精確配準，過程如下： 首先通過RANSAC算法對特征集進行初始配準，找到兩幀中的最佳平移旋轉變換，隨后 通過重投影誤差RANSAC優(yōu)化目標函數(shù)：(1-10) 式中，Af包含了特征內點；//,尤分別表示場景幀和參考幀上關聯(lián)點對的空間坐標;T(p) =R · p+t表示平移旋轉變換矩陣;3?(ρ)為參考幀攝像頭上的投影變換函數(shù)。3. 如權利要求2所述的一種可提高機器人位姿一致性的0RB關鍵幀閉環(huán)檢測SLAM方法， 其特征在于:式(1-10)通過幀間稀疏BA方法進行優(yōu)化;ICP迭代過程中，首先通過K-D樹檢索 最近點實現(xiàn)點云關聯(lián)，通過優(yōu)化關聯(lián)特征點與點云的聯(lián)合誤差函數(shù)實現(xiàn)全局位姿變換矩陣 的優(yōu)化，權重因子W表示關聯(lián)點對在誤差函數(shù)中的重要性；(1-11) 式中，α為各投影變換函數(shù)件的權重因子。4. 如權利要求1~3之一所述的一種可提高機器人位姿一致性的0RB關鍵幀閉環(huán)檢測 SLAM方法，其特征在于:所述閉環(huán)檢測方法中，通過KeyFrame選取算法，選取過程為：當前幀 與上一關鍵幀通過RANSAC進行配準，如果配準內點值小于規(guī)定閾值，則認定其為關鍵幀;采 用關鍵幀視覺詞袋模型，該模型將關鍵幀的0RB特征聚類，利用視覺詞典將特征描述子映射 為視覺單詞，通過對比關鍵幀中的"單詞"，可判斷兩個關鍵幀是否形成閉環(huán)。5. 如權利要求4所述的一種可提高機器人位姿一致性的0RB關鍵幀閉環(huán)檢測SLAM方法， 其特征在于:所述關鍵幀選取過程描述為:在完成RANSAC-ICP精確估計后，利用當前幀與上 一關鍵幀再次進行RANSAC-ICP配準。如果當前幀與關鍵幀之間的配準誤差大于閾值，則認 為該幀所在場景為新場景，該幀設為關鍵幀，反之，該幀為普通幀，用于位姿圖構建，選取下 一幀進行關鍵幀比對，其數(shù)學過程可描述為：(1-12) 其中，F(xiàn)?為離j時刻最近的關鍵幀;Fj為當前幀。6.如權利要求4所述的一種可提高機器人位姿一致性的ORB關鍵幀閉環(huán)檢測SLAM方法， 其特征在于:所述閉環(huán)檢測方法的流程如下： 1) 首先從所有圖像中檢測并提取關鍵幀，假設機器人位于地圖場景中的某一關鍵幀位 置，將生成的關鍵幀圖像使用ORB特征描述符進行描述，關鍵幀圖像I可表示為描述符集合 D: I^{di, ···,dn}； 2) 提取每個關鍵幀場景的ORB特征并投影到BoVW中，將關鍵幀ORB特征點與視覺詞典中 的單詞進行關聯(lián)，視覺詞典可表示為，通過BoVW相似描述符聚類可構建視覺 詞典V，通過映射形成最鄰近視覺字典，每一個視覺單詞的ORB描述向量都是一個關聯(lián)的視 覺詞表； 3) 采用單詞相似度的方式計算"單詞"間距，形成關鍵幀場景的相似度，為獲得場景相 似度，計算關鍵幀圖像Iu和Iv之間的余弦距離可得到二者BoVW的相似度，其過程為： 每一關鍵幀由不同權重的詞匯聚類而成，每個詞匯&在全部圖像N中出現(xiàn)的頻率形成權 重Wi: Wi = logio(N/ru) (1-12) 式中m表示cU中包含圖像的數(shù)量，若BoVW中包含I V|個不同的視覺單詞，則圖像矢量可 描述為：其中關鍵幀中的單詞權重為：(1-13) 得到每個單詞權重后，即可求出其所在圖像的權重，通過相似函數(shù)s可計算鍵幀圖像iu 和Iv之間的相似度：(1-14) 通過上式可得相似矩陣進而判斷圖像間的相似度，圖像i和圖像j通過相似函數(shù)得到相 似度矩陣Mi, j ; 通過視覺詞袋方法，關鍵幀閉環(huán)檢測問題可轉化為從圖像序列L= [ h，I2，…]中找到兩 個相似度較高的子序列A= [ai，a2，…]和B= [h，b2，…]，從而實現(xiàn)閉環(huán)檢測。
【文檔編號】B25J9/16GK105856230SQ201610298075
【公開日】2016年8月17日
【申請日】2016年5月6日
【發(fā)明人】簡燕梅, 艾青林, 余杰, 鄭凱, 劉賽
【申請人】簡燕梅