一種基于rgb-d的slam算法的改進(jìn)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及移動(dòng)機(jī)器人研宄領(lǐng)域，具體是一種基于RGB-D的SLAM算法的改進(jìn)方 法。
【背景技術(shù)】
[0002] 為了在未知環(huán)境中進(jìn)行導(dǎo)航，移動(dòng)機(jī)器人需要構(gòu)建環(huán)境地圖并且同時(shí)定位自 身在地圖中的位置，像這樣同時(shí)解決這兩個(gè)問(wèn)題的過(guò)程就稱為同步定位與地圖構(gòu)建 (SimultaneouslyLocalizationAndMapping，SLAM)。當(dāng)機(jī)器人處于室外環(huán)境的時(shí)候，這 個(gè)問(wèn)題可以通過(guò)高精度的GPS來(lái)解決。但是，當(dāng)機(jī)器人處于室內(nèi)環(huán)境時(shí)，或當(dāng)GPS不夠精確 無(wú)法滿足高精度的需求時(shí)，或者當(dāng)機(jī)器人所處環(huán)境涉密時(shí)，人們就必須使用其他方法來(lái)精 確估計(jì)機(jī)器人的位置并且同時(shí)構(gòu)建環(huán)境地圖。SLAM問(wèn)題就是在這樣的需求背景下提出的， 是當(dāng)前移動(dòng)機(jī)器人研宄領(lǐng)域的一個(gè)重要課題。
[0003] 移動(dòng)機(jī)器人可以通過(guò)不同類型的傳感器獲取所處環(huán)境的信息，傳感器的類型不 同，得到的環(huán)境信息的內(nèi)容也不相同。如聲吶測(cè)距儀獲取的信息為傳感器與障礙的距離信 息，激光掃描測(cè)距儀獲取的信息為傳感器與障礙的距離和角度信息，視覺(jué)傳感器獲取的信 息為環(huán)境的圖像信息等等。移動(dòng)機(jī)器人可以同時(shí)配置多類傳感器，通過(guò)融合不同傳感器所 獲取的信息，可以有效地提高地圖環(huán)境的精度。
[0004] 把視覺(jué)傳感器和紅外傳感器集成到一起并實(shí)現(xiàn)兩種傳感器信息之間的良好同步， 這樣就構(gòu)成了RGB-D傳感器?；赗GB-D傳感器的VSLAM則被稱為RGB-DSLAM。
[0005] 因此，本發(fā)明提出了一種針對(duì)RGB-DSLAM實(shí)現(xiàn)算法的各部分改進(jìn)方法。

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

[0006] 本發(fā)明的目的在于提供一種增強(qiáng)特征匹配、匹配點(diǎn)精確的基于RGB-D的SLAM算法 的改進(jìn)方法，針對(duì)現(xiàn)有RGB-DSLAM算法中存在的效率低與誤差大的問(wèn)題做出改進(jìn)。
[0007] 為實(shí)現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：
[0008] -種基于RGB-D的SLAM算法的改進(jìn)方法，分為前端和后端兩部分，前端的任務(wù)為 提取不同觀察結(jié)果之間的空間關(guān)系，分為特征檢測(cè)與描述符提取、特征匹配、運(yùn)動(dòng)變換估計(jì) 和運(yùn)動(dòng)變換優(yōu)化；后端的任務(wù)為使用非線性誤差函數(shù)優(yōu)化位姿圖中相機(jī)的位姿信息；使用 基于圖的位姿優(yōu)化方法，利用前端得到的6D運(yùn)動(dòng)變換關(guān)系初始化位姿圖，然后進(jìn)行閉環(huán)檢 測(cè)添加閉環(huán)約束條件，再利用非線性誤差函數(shù)優(yōu)化方法進(jìn)行位姿圖優(yōu)化，最終得到全局最 優(yōu)相機(jī)位姿和相機(jī)運(yùn)動(dòng)軌跡，并進(jìn)行三維環(huán)境重建；其中，特征檢測(cè)與描述符提取、特征匹 配、運(yùn)動(dòng)變換估計(jì)和運(yùn)動(dòng)變換優(yōu)化的具體方法如下所述：
[0009] 1)基于0RB的特征檢測(cè)與描述符提取方法
[0010] 0RB算法是一種快速特征檢測(cè)與描述符提取算法，包含OFAST特征點(diǎn)檢測(cè)和 rBRIEF描述符提取兩個(gè)步驟；該算法的具體內(nèi)容如下：
[0011] (1)在oFAST特征點(diǎn)檢測(cè)步驟中，首先對(duì)原圖像構(gòu)建尺度空間金字塔，然后使用 FAST特征點(diǎn)檢測(cè)方法檢測(cè)出足夠多的特征點(diǎn)并使用Harris角點(diǎn)評(píng)估方法對(duì)檢測(cè)到的特征 點(diǎn)進(jìn)行篩選，再根據(jù)強(qiáng)度矩心求出特征點(diǎn)的方向；
[0012] (2)在rBRIEF描述符提取步驟中，首先對(duì)oFAST特征點(diǎn)生成BRIEF特征描述符，然 后利用oFAST特征點(diǎn)的方向矢量對(duì)BRIEF特征描述符進(jìn)行轉(zhuǎn)向生成steeredBRIEF特征描 述符，再使用貪心學(xué)習(xí)算法從訓(xùn)練集中篩選出一組具有較大方差值、高度不相關(guān)且位均值 位于0. 5附近的二元點(diǎn)集，然后使用這組二元點(diǎn)集生成BRIEF特征描述符并進(jìn)行轉(zhuǎn)向，得到 具有旋轉(zhuǎn)不變性的rBRIEF特征描述子；
[0013] 2)基于FLANN的增強(qiáng)特征匹配方法
[0014] 基于FLANN的增強(qiáng)特征匹配方法是對(duì)查找?guī)陀?xùn)練幀特征描述符進(jìn)行雙向的基 于FLANN的KNN匹配，然后使用單應(yīng)性矩陣變換對(duì)匹配結(jié)果進(jìn)行優(yōu)化；
[0015] 3)改進(jìn)RANSAC的運(yùn)動(dòng)變換估計(jì)方法
[0016] 使用RANSAC的方法進(jìn)行運(yùn)動(dòng)變換估計(jì)總體上是一個(gè)迭代的過(guò)程；在每一次迭代 過(guò)程中從三維坐標(biāo)匹配點(diǎn)對(duì)集合中隨機(jī)選擇20個(gè)三維坐標(biāo)匹配點(diǎn)對(duì)，根據(jù)這些三維坐標(biāo) 匹配點(diǎn)對(duì)求出一個(gè)運(yùn)動(dòng)變換模型；對(duì)于三維坐標(biāo)匹配點(diǎn)對(duì)集合中的每一個(gè)元素，使用所求 出的運(yùn)動(dòng)變換模型將元素中的原坐標(biāo)進(jìn)行投影以得到投影坐標(biāo)，計(jì)算目標(biāo)坐標(biāo)與投影坐標(biāo) 之間的誤差，如果誤差小于閾值，則該三維坐標(biāo)匹配點(diǎn)對(duì)是局內(nèi)點(diǎn)，將其加入局內(nèi)點(diǎn)集合； 否則認(rèn)為它是局外點(diǎn)，不作處理；統(tǒng)計(jì)局內(nèi)點(diǎn)的數(shù)目，并計(jì)算該模型下的平均誤差；如果局 內(nèi)點(diǎn)數(shù)目高于閾值并且平均誤差低于閾值，則利用所有的局內(nèi)點(diǎn)重新計(jì)算新的運(yùn)動(dòng)變換模 型；然后再次對(duì)新的運(yùn)動(dòng)變換模型進(jìn)行評(píng)估；
[0017] (4)基于GICP的運(yùn)動(dòng)變換優(yōu)化方法
[0018] GICP算法在對(duì)標(biāo)準(zhǔn)ICP算法和點(diǎn)到面ICP算法進(jìn)行歸納與綜合的基礎(chǔ)上，更進(jìn)一 步使用了面到面的思想進(jìn)行改進(jìn)；GICP算法的抽象程度更高，而且在一定條件下退化成標(biāo) 準(zhǔn)ICP算法或者點(diǎn)到面ICP算法；GICP算法點(diǎn)云配準(zhǔn)所需的時(shí)間高于標(biāo)準(zhǔn)ICP算法，但是 GICP的精度遠(yuǎn)遠(yuǎn)高于標(biāo)準(zhǔn)ICP，使用GICP算法替換標(biāo)準(zhǔn)ICP算法進(jìn)行RGB-DSLAM算法中 的運(yùn)動(dòng)變換優(yōu)化，以使得求解出的運(yùn)動(dòng)變換誤差更小。
[0019] 作為本發(fā)明進(jìn)一步的方案：〇FAST特征點(diǎn)檢測(cè)的具體步驟如下所述，F(xiàn)AST是一種 非常快速的特征點(diǎn)檢測(cè)算法，根據(jù)候選點(diǎn)與其圓形鄰域內(nèi)相鄰像素點(diǎn)的灰度值差異來(lái)判斷 候選點(diǎn)是否為特征點(diǎn)，具體判斷方法如式（1-1)所示：
【主權(quán)項(xiàng)】
1. 一種基于RGB-D的SLAM算法的改進(jìn)方法，分為前端和后端兩部分，前端的任務(wù)為提 取不同觀察結(jié)果之間的空間關(guān)系，分為特征檢測(cè)與描述符提取、特征匹配、運(yùn)動(dòng)變換估計(jì)和 運(yùn)動(dòng)變換優(yōu)化；后端的任務(wù)為使用非線性誤差函數(shù)優(yōu)化位姿圖中相機(jī)的位姿信息；使用基 于圖的位姿優(yōu)化方法，利用前端得到的6D運(yùn)動(dòng)變換關(guān)系初始化位姿圖，然后進(jìn)行閉環(huán)檢測(cè) 添加閉環(huán)約束條件，再利用非線性誤差函數(shù)優(yōu)化方法進(jìn)行位姿圖優(yōu)化，最終得到全局最優(yōu) 相機(jī)位姿和相機(jī)運(yùn)動(dòng)軌跡，并進(jìn)行=維環(huán)境重建；其特征在于，其中，特征檢測(cè)與描述符提 取、特征匹配、運(yùn)動(dòng)變換估計(jì)和運(yùn)動(dòng)變換優(yōu)化的具體方法如下所述： 1) 基于ORB的特征檢測(cè)與描述符提取方法 ORB算法是一種快速特征檢測(cè)與描述符提取算法，包含oFAST特征點(diǎn)檢測(cè)和巧RIEF描 述符提取兩個(gè)步驟；該算法的具體內(nèi)容如下： (1) 在oFAST特征點(diǎn)檢測(cè)步驟中，首先對(duì)原圖像構(gòu)建尺度空間金字塔，然后使用FAST特 征點(diǎn)檢測(cè)方法檢測(cè)出足夠多的特征點(diǎn)并使用Harris角點(diǎn)評(píng)估方法對(duì)檢測(cè)到的特征點(diǎn)進(jìn)行 篩選，再根據(jù)強(qiáng)度矩屯、求出特征點(diǎn)的方向； (2) 在巧RIEF描述符提取步驟中，首先對(duì)oFAST特征點(diǎn)生成BRIEF特征描述符，然后 利用oFAST特征點(diǎn)的方向矢量對(duì)BRIEF特征描述符進(jìn)行轉(zhuǎn)向生成steeredBRIEF特征描述 符，再使用貪屯、學(xué)習(xí)算法從訓(xùn)練集中篩選出一組具有較大方差值、高度不相關(guān)且位均值位 于0. 5附近的二元點(diǎn)集，然后使用該組二元點(diǎn)集生成BRIEF特征描述符并進(jìn)行轉(zhuǎn)向，得到具 有旋轉(zhuǎn)不變性的巧RIEF特征描述子； 2) 基于FLAW^的增強(qiáng)特征匹配方法 基于FLAW^的增強(qiáng)特征匹配方法是對(duì)查找帖和訓(xùn)練帖特征描述符進(jìn)行雙向的基于FLAW^的腳W匹配，然后使用單應(yīng)性矩陣變換對(duì)匹配結(jié)果進(jìn)行優(yōu)化； 3) 改進(jìn)RANSAC的運(yùn)動(dòng)變換估計(jì)方法 使用RANSAC的方法進(jìn)行運(yùn)動(dòng)變換估計(jì)總體上是一個(gè)迭代的過(guò)程；在每一次迭代過(guò)程 中從=維坐標(biāo)匹配點(diǎn)對(duì)集合中隨機(jī)選擇20個(gè)=維坐標(biāo)匹配點(diǎn)對(duì)，根據(jù)該些=維坐標(biāo)匹配 點(diǎn)對(duì)求出一個(gè)運(yùn)動(dòng)變換模型；對(duì)于=維坐標(biāo)匹配點(diǎn)對(duì)集合中的每一個(gè)元素，使用所求出的 運(yùn)動(dòng)變換模型將元素中的原坐標(biāo)進(jìn)行投影W得到投影坐標(biāo)，計(jì)算目標(biāo)坐標(biāo)與投影坐標(biāo)之間 的誤差，如果誤差小于闊值，則該S維坐標(biāo)匹配點(diǎn)對(duì)是局內(nèi)點(diǎn)，將其加入局內(nèi)點(diǎn)集合；否則 認(rèn)為它是局外點(diǎn)，不作處理；統(tǒng)計(jì)局內(nèi)點(diǎn)的數(shù)目，并計(jì)算該模型下的平均誤差；如果局內(nèi)點(diǎn) 數(shù)目高于闊值并且平均誤差低于闊值，則利用所有的局內(nèi)點(diǎn)重新計(jì)算新的運(yùn)動(dòng)變換模型； 然后再次對(duì)新的運(yùn)動(dòng)變換模型進(jìn)行評(píng)估； (4)基于GICP的運(yùn)動(dòng)變換優(yōu)化方法 GICP算法在對(duì)標(biāo)準(zhǔn)ICP算法和點(diǎn)到面ICP算法進(jìn)行歸納與綜合的基礎(chǔ)上，更進(jìn)一步 使用了面到面的思想進(jìn)行改進(jìn)；GICP算法的抽象程度更高，而且在一定條件下退化成標(biāo)準(zhǔn) ICP算法或者點(diǎn)到面ICP算法；GICP算法點(diǎn)云配準(zhǔn)所需的時(shí)間高于標(biāo)準(zhǔn)ICP算法，但是GICP 的精度遠(yuǎn)遠(yuǎn)高于標(biāo)準(zhǔn)ICP，使用GICP算法替換標(biāo)準(zhǔn)ICP算法進(jìn)行RGB-DSLAM算法中的運(yùn)動(dòng) 變換優(yōu)化，W使得求解出的運(yùn)動(dòng)變換誤差更小。
2. 根據(jù)權(quán)利要求1所述的基于RGB-D的SLAM算法的改進(jìn)方法，其特征在于，所述oFAST 特征點(diǎn)檢測(cè)的具體步驟如下所述，F(xiàn)AST是一種非?？焖俚奶卣鼽c(diǎn)檢測(cè)算法，根據(jù)候選點(diǎn)與 其圓形鄰域內(nèi)相鄰像素點(diǎn)的灰度值差異來(lái)判斷候選點(diǎn)是否為特征點(diǎn)，具體判斷方法如式 (1-1)所示：
(1-1) 式中，P為候選點(diǎn)，circle(p)為p的圓形鄰域，X為候選點(diǎn)圓形鄰域內(nèi)任意一像素點(diǎn)，I(P)為候選點(diǎn)的圖像灰度值，I(x)為X處的圖像灰度值諾N超出闊值條件￡d，則該候選 點(diǎn)為特征點(diǎn)； 在FAST特征點(diǎn)檢測(cè)結(jié)束后，使用Harris角點(diǎn)評(píng)估方法對(duì)特征點(diǎn)進(jìn)行篩選；如果想要得 到的目標(biāo)特征點(diǎn)數(shù)目為N，則首先設(shè)置一個(gè)足夠低的闊值W獲得超過(guò)N個(gè)特征點(diǎn)，然后使用 Harris評(píng)估方法對(duì)該些特征點(diǎn)進(jìn)行排序，然后選取前N個(gè)作為所得到的目標(biāo)特征點(diǎn)；