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

【發(fā)明內(nèi)容】

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