本發(fā)明涉及機(jī)器人，尤其涉及機(jī)器人的快速定位與姿態(tài)估計(jì)。

背景技術(shù)：

機(jī)器人定位與姿態(tài)估計(jì)是機(jī)器人應(yīng)用的基本問題，定位能夠確定機(jī)器人在工作環(huán)境中的位置信息，姿態(tài)能夠獲取機(jī)器人移動(dòng)方向，準(zhǔn)確快速獲取機(jī)器人的位置及姿態(tài)，能夠提高機(jī)器人作業(yè)的效率及安全性。

基于彩色攝像機(jī)的機(jī)器人定位及姿態(tài)估計(jì)常常受工作環(huán)境的光照影響，使得機(jī)器人定位及姿態(tài)估計(jì)誤差較大。目前有些專利通過圖像預(yù)處理的技術(shù)能夠克服光照亮暗的影響，但對(duì)于一部分有陰影的情況，不能起到優(yōu)化效果。

此外，目前雖然已經(jīng)有一些技術(shù)可以針對(duì)攝像機(jī)拍攝的圖像進(jìn)行一系列處理，從而獲得機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)姿態(tài)與平移距離，但是存在運(yùn)行過程復(fù)雜，計(jì)算速度緩慢，還會(huì)出現(xiàn)一定的誤差等問題。

為此本領(lǐng)域迫切需要一種能夠基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)的方法。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明旨在克服以上技術(shù)問題，提出一種準(zhǔn)確率高、能夠有效降低圖像處理時(shí)間且處理速度較快的基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法。

為解決上述技術(shù)問題，本發(fā)明提供一種基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法，所述深度攝像頭固定地安裝在機(jī)器人主體上，并且保持相對(duì)于機(jī)器人主體的位置不變，所述方法包括以下步驟：

(1)、深度攝像頭獲取機(jī)器人在運(yùn)動(dòng)過程環(huán)境中的深度圖像P1、P2；

其中，深度圖像P1為時(shí)刻t采集的數(shù)據(jù)信息，深度圖像P2為時(shí)刻t之后采集的數(shù)據(jù)信息；

(2)、利用高斯金字塔方法對(duì)(1)中獲取的深度圖像P1、P2進(jìn)行縮放，其中，

將深度圖像P1縮小為p1，縮放系數(shù)為k1；

將深度圖像P2縮小為p2，縮放系數(shù)為k2；

(3)、利用Fast角點(diǎn)檢測算法對(duì)(2)中縮放后的圖像p1、p2進(jìn)行角點(diǎn)檢測；

(4)、利用Sift算法對(duì)檢測到的角點(diǎn)進(jìn)行特征提??；

(5)、利用Knn算法對(duì)相似特征進(jìn)行匹配；

(6)、利用(5)中的匹配結(jié)果，通過最小二乘法獲取機(jī)器人在智能移動(dòng)過程中的旋轉(zhuǎn)矩陣R與平移矩陣T，R與T的矩陣形式如下：

其中，r₁₁，r₁₂，…，r₃₃這9個(gè)參數(shù)表示深度攝像頭隨著機(jī)器人旋轉(zhuǎn)導(dǎo)致圖像的旋轉(zhuǎn)及縮放；

其中，T_x，T_y，T_z分別表示在x、y、z方向的平移，機(jī)器人的位置為機(jī)器人從原點(diǎn)出發(fā)時(shí)每次位置變化T的累計(jì)。

優(yōu)選地，對(duì)于某一個(gè)點(diǎn)X(x，y，z)，該點(diǎn)變換后的位置X’利用R、T矩陣來計(jì)算，計(jì)算方法如下：

X＝[x，y，z，1]^T，

X'＝RX+T。

優(yōu)選地，所述深度攝像頭為Kinect深度相機(jī)。

優(yōu)選地，所述深度攝像頭計(jì)算旋轉(zhuǎn)或平移的頻率根據(jù)不同環(huán)境設(shè)置為不同參數(shù)。

本發(fā)明的基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法相比于現(xiàn)有技術(shù)具有如下優(yōu)點(diǎn):該方法能夠提高機(jī)器人定位及姿態(tài)估計(jì)中的準(zhǔn)確性，同時(shí)能夠有效降低圖像處理時(shí)間，提升處理圖像的速度。

附圖說明

為了更清楚地說明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1為本發(fā)明的基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法的流程結(jié)構(gòu)示意圖。

具體實(shí)施方式

為使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明了，下面結(jié)合具體實(shí)施方式并參照附圖，對(duì)本發(fā)明進(jìn)一步詳細(xì)說明。應(yīng)該理解，這些描述只是示例性的，而并非要限制本發(fā)明的范圍。此外，在以下說明中，省略了對(duì)公知結(jié)構(gòu)及技術(shù)的描述，以避免不必要地混淆本發(fā)明的概念。

本發(fā)明提供了一種基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法，在使用該方法之前需要將深度攝像頭固定安裝在機(jī)器人主體上，保持深度攝像頭相對(duì)于機(jī)器人主體的位置不變。需要說明的是，本申請中深度攝像頭的旋轉(zhuǎn)、平移就是機(jī)器人主體的旋轉(zhuǎn)、平移。

圖1為本發(fā)明的基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法的一種結(jié)構(gòu)示意圖，如圖1所示，基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法采用以下步驟：首先是獲取一幅深度圖像P1，再獲取另一幅深度圖像P2，然后將圖像P1縮小為p1，將圖像P2縮小為p2，再對(duì)p1和p2分別進(jìn)行角點(diǎn)測測，對(duì)p1和p2檢測到的角點(diǎn)分別提取特征，之后進(jìn)行特征匹配，最后計(jì)算深度攝像頭的旋轉(zhuǎn)和平移，以此來獲取機(jī)器人的旋轉(zhuǎn)和平移矩陣。

需要說明的是，圖1中深度圖像P1、P2的獲取可以通過深度相機(jī)廠家提供的SDK獲取，優(yōu)選地，本發(fā)明中的深度攝像頭采用Kinect深度相機(jī)。

具體地，上述基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法包括以下步驟：

(1)、深度攝像頭獲取機(jī)器人在運(yùn)動(dòng)過程環(huán)境中的深度圖像P1、P2，

其中，深度圖像P1為時(shí)刻t采集的數(shù)據(jù)信息，深度圖像P2為時(shí)刻t之后采集的數(shù)據(jù)信息；

(2)、利用高斯金字塔方法對(duì)(1)中獲取的深度圖像P1、P2進(jìn)行縮放，其中，將深度圖像P1縮小為p1，縮放系數(shù)為k1；將深度圖像P2縮小為p2，縮放系數(shù)為k2；

(3)、利用Fast角點(diǎn)檢測算法對(duì)(2)中縮放后的圖像p1、p2進(jìn)行角點(diǎn)檢測；

(4)、利用Sift算法對(duì)檢測到的角點(diǎn)進(jìn)行特征提??；

(5)、利用Knn算法對(duì)相似特征進(jìn)行匹配；

(6)、利用(5)中的匹配結(jié)果，通過最小二乘法獲取機(jī)器人在智能移動(dòng)過程中的旋轉(zhuǎn)矩陣R與平移矩陣T，R與T的矩陣形式如下：

其中，r11，r12，…，r33這9個(gè)參數(shù)表示深度攝像頭隨著機(jī)器人旋轉(zhuǎn)導(dǎo)致圖像的旋轉(zhuǎn)及縮放；

其中，Tx，Ty，Tz分別表示在x、y、z方向的平移，機(jī)器人的位置為機(jī)器人從原點(diǎn)出發(fā)時(shí)每次位置變化T的累計(jì)。

本發(fā)明的上述步驟(1)中，深度圖像P1、P2的獲取順序不可以變換，因?yàn)樵谟?jì)算深度攝像頭的旋轉(zhuǎn)與平移時(shí)，用到了拍攝圖像的先后順序，并且深度圖像P1為時(shí)刻t采集的數(shù)據(jù)信息，深度圖像P2為時(shí)刻t之后采集的數(shù)據(jù)信息。

本發(fā)明的上述步驟(2)中，將深度圖像P1、P2進(jìn)行縮放的系數(shù)k1與k2可相同或不同，其具體值需要根據(jù)圖像的大小決定。

另外，本發(fā)明的基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法的上述步驟中，可以先獲取深度圖像P1，然后對(duì)深度圖像P1進(jìn)行縮小、角點(diǎn)檢測、角點(diǎn)特征提取，這幾步操作步驟在執(zhí)行時(shí)順序不可以變換。此外，再獲取深度圖像P2，然后對(duì)深度圖像P2進(jìn)行縮小、角點(diǎn)檢測、角點(diǎn)特征提取，這幾步操作步驟在執(zhí)行時(shí)順序也不可以變換。但是，對(duì)深度圖像P1、P2的處理順序可以變換，即本發(fā)明的基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法中可以先對(duì)深度圖像P1進(jìn)行處理，也可以先對(duì)深度圖像P2進(jìn)行處理。

本發(fā)明的基于深度攝像頭的機(jī)器人快速定位與姿態(tài)估計(jì)方法的上述步驟中，步驟(5)與(6)的順序不可以交換，因?yàn)椴襟E(5)的結(jié)果要用于步驟(6)中，從而計(jì)算出深度攝像頭的旋轉(zhuǎn)矩陣R與平移矩陣T。

在本發(fā)明的進(jìn)一步實(shí)施例中，對(duì)于某一個(gè)點(diǎn)X(x，y，z)，該點(diǎn)變換后的位置X’，也可以利用R、T矩陣來計(jì)算，計(jì)算方法如下：

X＝[x，y，z，1]^T，

X′＝RX+T。

在本發(fā)明的進(jìn)一步實(shí)施例中，根據(jù)不同環(huán)境或具體場景可以將深度攝像頭計(jì)算旋轉(zhuǎn)或平移的頻率設(shè)置為不同參數(shù)。其中，機(jī)器人實(shí)時(shí)的位姿就是深度攝像頭實(shí)時(shí)獲得的旋轉(zhuǎn)參數(shù)R，機(jī)器人的位置為機(jī)器人從原點(diǎn)出發(fā)時(shí)每次位置變化T的累計(jì)。

應(yīng)當(dāng)理解的是，本發(fā)明的上述具體實(shí)施方式僅僅用于示例性說明或解釋本發(fā)明的原理，而不構(gòu)成對(duì)本發(fā)明的限制。因此，在不偏離本發(fā)明的精神及范圍的情況下所做的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。此外，本發(fā)明所附權(quán)利要求旨在涵蓋落入所附權(quán)利要求范圍及邊界、或者這種范圍及邊界的等同形式內(nèi)的全部變化及修改例。