[發(fā)明專利]動(dòng)態(tài)環(huán)境下基于幾何與運(yùn)動(dòng)約束的機(jī)器人RGB-D SLAM方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202011381750.X
• 申請(qǐng)日: 2020-12-01
• 公開（公告）號(hào): CN112378409B
• 公開（公告）日: 2022-08-12
• 發(fā)明（設(shè)計(jì)）人: 艾青林;劉剛江
• 申請(qǐng)（專利權(quán)）人: 杭州宇芯機(jī)器人科技有限公司
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G06T7/80
• 代理公司: 杭州斯可睿專利事務(wù)所有限公司 33241
• 代理人: 王利強(qiáng)

一種動(dòng)態(tài)環(huán)境下基于幾何與運(yùn)動(dòng)約束的機(jī)器人RGB?D SLAM方法，通過RGB?D相機(jī)獲取圖像數(shù)據(jù)，圖像數(shù)據(jù)包括RGB圖像和深度圖像，使用ORB特征點(diǎn)檢測(cè)算法、RGB圖像和深度圖像，獲取特征點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)；使用g2o優(yōu)化算法計(jì)算相機(jī)位姿，把在相機(jī)坐標(biāo)系下的特征點(diǎn)變換到世界坐標(biāo)系下，通過不斷跟蹤同一點(diǎn)在世界坐標(biāo)系下的坐標(biāo)，得到該點(diǎn)多次觀測(cè)速度；通過分析多次觀測(cè)速度的規(guī)律，得知該點(diǎn)是真實(shí)運(yùn)動(dòng)物體上的還是靜止物體上的。分析得到該點(diǎn)是真實(shí)運(yùn)動(dòng)后，通過卡爾曼濾波算法對(duì)動(dòng)點(diǎn)運(yùn)動(dòng)速度、世界坐標(biāo)系下的坐標(biāo)進(jìn)行濾波；最后，把動(dòng)點(diǎn)和靜點(diǎn)一起加入g2o位姿優(yōu)化。本發(fā)明有效適用于動(dòng)態(tài)環(huán)境，得到更準(zhǔn)確的相機(jī)位姿。

技術(shù)領(lǐng)域

本發(fā)明涉及一種室內(nèi)動(dòng)態(tài)環(huán)境下機(jī)器人定位方法。

背景技術(shù)

在自主移動(dòng)機(jī)器人智能導(dǎo)航技術(shù)的相關(guān)研究中，機(jī)器人在未知環(huán)境下的同步 定位與地圖構(gòu)建(簡(jiǎn)稱SLAM，simultaneous localization and mapping)技術(shù)作為關(guān)鍵 性的技術(shù)，兼具應(yīng)用和學(xué)術(shù)上的雙重價(jià)值，已然成為近二十年來該領(lǐng)域的研究熱 點(diǎn)。在這種趨勢(shì)下，學(xué)者們提出了多種解決SLAM問題的方法，也應(yīng)用了多種傳 感器來解決SLAM中的環(huán)境感知問題。

攝像頭作為一種廉價(jià)、信息量大的傳感器，在自主移動(dòng)機(jī)器人領(lǐng)域有極好的 應(yīng)用前景。視覺SLAM主要通過對(duì)多幀圖像中相同的特征點(diǎn)進(jìn)行匹配，求解出相 機(jī)的位姿變換。但是視覺SLAM方向還有四大難題，導(dǎo)致其實(shí)際應(yīng)用場(chǎng)景較小。 包括如何處理閉環(huán)回路、如何處理大尺度場(chǎng)景、如何處理動(dòng)態(tài)場(chǎng)景、如何處理快 速運(yùn)動(dòng)和強(qiáng)旋轉(zhuǎn)。

動(dòng)態(tài)場(chǎng)景指的是攝像頭捕獲的照片中存在人、車等正在運(yùn)動(dòng)的物體。對(duì)于稀 疏地圖的視覺SLAM系統(tǒng)，動(dòng)態(tài)場(chǎng)景的挑戰(zhàn)主要來源于傳統(tǒng)視覺SLAM無法區(qū) 分出運(yùn)動(dòng)特征點(diǎn)與靜態(tài)特征點(diǎn)，在計(jì)算相機(jī)位姿時(shí)，錯(cuò)誤地把運(yùn)動(dòng)特征點(diǎn)當(dāng)成靜 態(tài)特征點(diǎn)進(jìn)行計(jì)算，導(dǎo)致計(jì)算出來的相機(jī)位姿和實(shí)際位姿有極大偏差，接著導(dǎo)致 地圖點(diǎn)計(jì)算錯(cuò)誤，進(jìn)一步擴(kuò)大視覺SLAM的定位偏差。

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

為了克服已有技術(shù)的不足，本發(fā)明提供了一種室內(nèi)動(dòng)態(tài)環(huán)境下機(jī)器人定位方 法，有效適用于動(dòng)態(tài)環(huán)境，得到更準(zhǔn)確的相機(jī)位姿。

本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是：

一種動(dòng)態(tài)環(huán)境下基于幾何與運(yùn)動(dòng)約束的機(jī)器人RGB-D SLAM方法，所述方 法包括如下步驟：

步驟1：進(jìn)行相機(jī)內(nèi)參標(biāo)定；

步驟2：在視頻流中依次對(duì)其中的圖像幀進(jìn)行獲取，首先對(duì)獲取的圖像幀建立 圖像金字塔，然后分別在圖像金字塔上進(jìn)行圖像分塊，獲取一定窗口大小的圖像 區(qū)域，然后利用現(xiàn)有的特征點(diǎn)提取算法在每個(gè)分塊里分別進(jìn)行特征點(diǎn)提取和建立 對(duì)應(yīng)特征點(diǎn)的描述子；

步驟3：判斷該幀是否是視頻流里的第一幀，如果是第一幀，則進(jìn)行初始化操 作，否則跳過這個(gè)步驟，過程如下：

步驟3.1：將RGB圖像平面的特征點(diǎn)和深度圖像中的像素點(diǎn)相互對(duì)應(yīng)；

步驟3.2：將深度圖像中的像素點(diǎn)轉(zhuǎn)換為該點(diǎn)離相機(jī)平面的距離；

步驟3.3：使用特征點(diǎn)在RGB圖像像素平面的坐標(biāo)和對(duì)應(yīng)的深度圖像像素值 轉(zhuǎn)換得到的該點(diǎn)距離相機(jī)平面的距離，算出該點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)P,像素坐 標(biāo)(u,v)和深度圖像素值d轉(zhuǎn)換為坐標(biāo)P的轉(zhuǎn)換方式為：

P＝K[u v d]^T

其中K為相機(jī)內(nèi)參；

步驟3.4：將該幀的相機(jī)坐系定為世界坐標(biāo)系，并將步驟3.3獲取的點(diǎn)加入1 號(hào)地圖，然后跳轉(zhuǎn)回步驟2；

步驟4，利用特征點(diǎn)描述子的距離匹配圖像中的特征點(diǎn)與1、2號(hào)地圖中的3D 點(diǎn)，計(jì)算相機(jī)位姿，將相機(jī)坐標(biāo)系下的坐標(biāo)P轉(zhuǎn)換到世界坐標(biāo)系下的坐標(biāo)x，并 進(jìn)行關(guān)鍵幀選取；過程如下：