基于深度攝像頭的機器人快速定位與姿態估計方法與流程
本發明涉及機器人，尤其涉及機器人的快速定位與姿態估計。
背景技術：
機器人定位與姿態估計是機器人應用的基本問題，定位能夠確定機器人在工作環境中的位置信息，姿態能夠獲取機器人移動方向，準確快速獲取機器人的位置及姿態，能夠提高機器人作業的效率及安全性。
基于彩色攝像機的機器人定位及姿態估計常常受工作環境的光照影響，使得機器人定位及姿態估計誤差較大。目前有些專利通過圖像預處理的技術能夠克服光照亮暗的影響，但對于一部分有陰影的情況，不能起到優化效果。
此外，目前雖然已經有一些技術可以針對攝像機拍攝的圖像進行一系列處理，從而獲得機器人的實時運動姿態與平移距離，但是存在運行過程復雜，計算速度緩慢，還會出現一定的誤差等問題。
為此本領域迫切需要一種能夠基于深度攝像頭的機器人快速定位與姿態估計的方法。
技術實現要素：
本發明旨在克服以上技術問題，提出一種準確率高、能夠有效降低圖像處理時間且處理速度較快的基于深度攝像頭的機器人快速定位與姿態估計方法。
為解決上述技術問題，本發明提供一種基于深度攝像頭的機器人快速定位與姿態估計方法，所述深度攝像頭固定地安裝在機器人主體上，并且保持相對于機器人主體的位置不變，所述方法包括以下步驟：
(1)、深度攝像頭獲取機器人在運動過程環境中的深度圖像P1、P2；
其中，深度圖像P1為時刻t采集的數據信息，深度圖像P2為時刻t之后采集的數據信息；
(2)、利用高斯金字塔方法對(1)中獲取的深度圖像P1、P2進行縮放，其中，
將深度圖像P1縮小為p1，縮放系數為k1；
將深度圖像P2縮小為p2，縮放系數為k2；
(3)、利用Fast角點檢測算法對(2)中縮放后的圖像p1、p2進行角點檢測；
(4)、利用Sift算法對檢測到的角點進行特征提取；
(5)、利用Knn算法對相似特征進行匹配；
(6)、利用(5)中的匹配結果，通過最小二乘法獲取機器人在智能移動過程中的旋轉矩陣R與平移矩陣T，R與T的矩陣形式如下：
其中，r11，r12，…，r33這9個參數表示深度攝像頭隨著機器人旋轉導致圖像的旋轉及縮放；
其中，Tx，Ty，Tz分別表示在x、y、z方向的平移，機器人的位置為機器人從原點出發時每次位置變化T的累計。
優選地，對于某一個點X(x，y，z)，該點變換后的位置X’利用R、T矩陣來計算，計算方法如下：
X＝[x，y，z，1]T，
X'＝RX+T。
優選地，所述深度攝像頭為Kinect深度相機。
優選地，所述深度攝像頭計算旋轉或平移的頻率根據不同環境設置為不同參數。
本發明的基于深度攝像頭的機器人快速定位與姿態估計方法相比于現有技術具有如下優點:該方法能夠提高機器人定位及姿態估計中的準確性，同時能夠有效降低圖像處理時間，提升處理圖像的速度。
附圖說明
為了更清楚地說明本發明實施例或現有技術中的技術方案，下面將對實施例或現有技術描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發明的一些實施例，對于本領域普通技術人員來講，在不付出創造性勞動的前提下，還可以根據這些附圖獲得其他的附圖。
圖1為本發明的基于深度攝像頭的機器人快速定位與姿態估計方法的流程結構示意圖。
具體實施方式
為使本發明的目的、技術方案及優點更加清楚明了，下面結合具體實施方式并參照附圖，對本發明進一步詳細說明。應該理解，這些描述只是示例性的，而并非要限制本發明的范圍。此外，在以下說明中，省略了對公知結構及技術的描述，以避免不必要地混淆本發明的概念。
本發明提供了一種基于深度攝像頭的機器人快速定位與姿態估計方法，在使用該方法之前需要將深度攝像頭固定安裝在機器人主體上，保持深度攝像頭相對于機器人主體的位置不變。需要說明的是，本申請中深度攝像頭的旋轉、平移就是機器人主體的旋轉、平移。
圖1為本發明的基于深度攝像頭的機器人快速定位與姿態估計方法的一種結構示意圖，如圖1所示，基于深度攝像頭的機器人快速定位與姿態估計方法采用以下步驟：首先是獲取一幅深度圖像P1，再獲取另一幅深度圖像P2，然后將圖像P1縮小為p1，將圖像P2縮小為p2，再對p1和p2分別進行角點測測，對p1和p2檢測到的角點分別提取特征，之后進行特征匹配，最后計算深度攝像頭的旋轉和平移，以此來獲取機器人的旋轉和平移矩陣。
需要說明的是，圖1中深度圖像P1、P2的獲取可以通過深度相機廠家提供的SDK獲取，優選地，本發明中的深度攝像頭采用Kinect深度相機。
具體地，上述基于深度攝像頭的機器人快速定位與姿態估計方法包括以下步驟：
(1)、深度攝像頭獲取機器人在運動過程環境中的深度圖像P1、P2，
其中，深度圖像P1為時刻t采集的數據信息，深度圖像P2為時刻t之后采集的數據信息；
(2)、利用高斯金字塔方法對(1)中獲取的深度圖像P1、P2進行縮放，其中，將深度圖像P1縮小為p1，縮放系數為k1；將深度圖像P2縮小為p2，縮放系數為k2；
(3)、利用Fast角點檢測算法對(2)中縮放后的圖像p1、p2進行角點檢測；
(4)、利用Sift算法對檢測到的角點進行特征提取；
(5)、利用Knn算法對相似特征進行匹配；
(6)、利用(5)中的匹配結果，通過最小二乘法獲取機器人在智能移動過程中的旋轉矩陣R與平移矩陣T，R與T的矩陣形式如下：
其中，r11，r12，…，r33這9個參數表示深度攝像頭隨著機器人旋轉導致圖像的旋轉及縮放；
其中，Tx，Ty，Tz分別表示在x、y、z方向的平移，機器人的位置為機器人從原點出發時每次位置變化T的累計。
本發明的上述步驟(1)中，深度圖像P1、P2的獲取順序不可以變換，因為在計算深度攝像頭的旋轉與平移時，用到了拍攝圖像的先后順序，并且深度圖像P1為時刻t采集的數據信息，深度圖像P2為時刻t之后采集的數據信息。
本發明的上述步驟(2)中，將深度圖像P1、P2進行縮放的系數k1與k2可相同或不同，其具體值需要根據圖像的大小決定。
另外，本發明的基于深度攝像頭的機器人快速定位與姿態估計方法的上述步驟中，可以先獲取深度圖像P1，然后對深度圖像P1進行縮小、角點檢測、角點特征提取，這幾步操作步驟在執行時順序不可以變換。此外，再獲取深度圖像P2，然后對深度圖像P2進行縮小、角點檢測、角點特征提取，這幾步操作步驟在執行時順序也不可以變換。但是，對深度圖像P1、P2的處理順序可以變換，即本發明的基于深度攝像頭的機器人快速定位與姿態估計方法中可以先對深度圖像P1進行處理，也可以先對深度圖像P2進行處理。
本發明的基于深度攝像頭的機器人快速定位與姿態估計方法的上述步驟中，步驟(5)與(6)的順序不可以交換，因為步驟(5)的結果要用于步驟(6)中，從而計算出深度攝像頭的旋轉矩陣R與平移矩陣T。
在本發明的進一步實施例中，對于某一個點X(x，y，z)，該點變換后的位置X’，也可以利用R、T矩陣來計算，計算方法如下：
X＝[x，y，z，1]T，
X′＝RX+T。
在本發明的進一步實施例中，根據不同環境或具體場景可以將深度攝像頭計算旋轉或平移的頻率設置為不同參數。其中，機器人實時的位姿就是深度攝像頭實時獲得的旋轉參數R，機器人的位置為機器人從原點出發時每次位置變化T的累計。
應當理解的是，本發明的上述具體實施方式僅僅用于示例性說明或解釋本發明的原理，而不構成對本發明的限制。因此，在不偏離本發明的精神及范圍的情況下所做的任何修改、等同替換、改進等，均應包含在本發明的保護范圍之內。此外，本發明所附權利要求旨在涵蓋落入所附權利要求范圍及邊界、或者這種范圍及邊界的等同形式內的全部變化及修改例。
- 還沒有人留言評論。精彩留言會獲得點贊！