本發明涉及移動機器人同步定位與地圖構建(SLAM)技術領域，尤其是適用于動態環境的基于魚眼圖像與深度圖像的視覺里程計方法。

背景技術：

定位技術是移動機器人實現各項復雜任務的技術基礎。里程計便是一類簡單有效的定位方法，目前主要的里程計方法有輪式里程計和視覺里程計等。輪式里程計通過安裝在車輪上的編碼器，對各個車輪的行駛距離進行采集，根據輪式機器人運動學模型解算出機器人相對于初始位置的位姿。顯然這種方法只適用于平面運動的輪式移動機器人，無法應用于腿式、輪腿式、履帶式等結構形式的移動機器人。另外，由于輪子打滑、傳動誤差等因素的影響，造成輪式里程計的累計誤差增大。視覺里程計采用視覺傳感器，利用特征匹配、攝影幾何、最優化等技術估計機器人的位姿，可以實現三維空間六自由度的運動估計，且不受輪子打滑的影響。鑒于上述優點，視覺里程計已經在水下機器人、無人機、腿式機器人、野外輪式機器人等領域得到了廣泛的應用。

自從美國學者Nister于2004年提出視覺里程計的概念以來，涌現出了大量的視覺里程計算法。但是，大部分主流算法均具有一個很強的假設：機器人的運行環境是靜態不變的。然而，實際環境中充滿了各種各樣運動的物體，如辦公室內行走的職員、公路上運動的車輛等。這些運動物體，阻礙了傳統算法在實際環境中的應用。

視覺里程計采用的相機有單目、雙目、多目、RGB-D相機等。相比之下，RGB-D相機不僅能夠采集彩色圖像，還能夠提供每個像素的深度信息，可以有效減少算法的復雜性，提高運動估計的精度。但是，大部分RGB-D相機的視場范圍小。當相機進行快速旋轉時，由于幀間圖像重疊率低和運動模糊等原因，極易發生運動估計失敗。

技術實現要素：

本發明的目的在于克服傳統視覺里程計無法適應動態環境的缺點，結合魚眼圖像與深度圖像的信息實現一種動態環境下的視覺里程計。

本發明所提出的一種融合魚眼圖像與深度圖像的動態環境視覺里程計方法，同時使用魚眼圖像和深度圖像的信息，由跟蹤線程、地圖構建線程和地圖點濾波線程組成，三個線程并行運行。

本發明所提出的一種融合魚眼圖像與深度圖像的動態環境視覺里程計方法，首先對地圖進行初始化，將地圖劃分為關鍵幀和地圖點兩部分，每個地圖點具有一個靜態指數si，初始化為0；每個地圖點具有一個用于標識該地圖點為靜態、動態或者未知的地圖點屬性。

所述跟蹤線程包括以下步驟：

步驟1，同步采集魚眼圖像Ifn和深度圖像Idn，構建成圖像幀Fn；n＝2,3,4，……。

步驟2，對圖像幀Fn中的魚眼圖像Ifn提取ORB特征集kn；

步驟3，將圖像幀Fn中的特征集kn中所有特征點與前一圖像幀Fn-1的特征集中指向靜態地圖點的特征點進行匹配，形成3D靜態地圖點到圖像幀2D特征的匹配點對集合，采用RANSAC框架下的PnP算法解算出圖像幀Fn的初始位姿

步驟4，將地圖中所有的靜態地圖點投影到圖像幀Fn，與圖像幀Fn中的特征集kn進行匹配，形成3D靜態地圖點到2D圖像特征ui之間的匹配點對集合{pw u}，利用魚眼相機投影模型π，構建優化方程如下：

其中，i＝1,2,3,4……，表示匹配點對的個數，為圖像幀Fn的位姿，為優化后的圖像幀Fn的最優位姿。以步驟3得到的初始位姿為初值，利Levenberg-Marquardt方法得到圖像幀Fn的最優位姿

步驟5，如果步驟4中匹配點對集合{pw u}中點對數量少于閾值Thm，或圖像幀Fn與前一圖像幀Fn-1的距離大于閾值Thd，則將圖像幀Fn構建為關鍵幀，并加入關鍵幀隊列。

所述地圖構建線程包括以下步驟：

步驟A，從關鍵幀隊列中取出關鍵幀KFn，由于深度相機的視場只占魚眼相機視場的一部分，魚眼圖像中只有中間部分的像素能在深度圖像中找到對應的深度，因此對魚眼圖像Ifn中間大小w×h像素區域的圖像提取ORB特征；

步驟B，對步驟A中提取到的每個ORB特征，在深度圖像Idn中尋找深度，計算對應的3D世界坐標，構建成地圖點，初始化地圖點的靜態指數si＝0，屬性為未知；

步驟C，對魚眼圖像Ifn中間w×h像素區域外的特征點，利用極線搜索的方法在其他關鍵幀上搜索配對特征，利用三角方法構建地圖點的3D世界坐標，構建成地圖點，初始化地圖點的靜態指數si＝0，屬性為未知；

步驟D，將步驟B和步驟C中新構建的地圖點，反投影到地圖中其他的關鍵幀KFo的魚眼圖像上，計算KFo上投影點的ORB描述子計算描述子與關鍵幀KFn對應特征的描述子的漢明距離，j＝1,2,3,……，表示描述子個數。如果漢明距離少于閾值Thh，則靜態指數加1，否則靜態指數減1，如果靜態指數大于閾值Ths，該地圖點的屬性變為靜態，如果靜態指數小于閾值Thdy，則地圖點的屬性變為動態；

步驟E，遍歷地圖中所有的關鍵幀，剔除與關鍵幀KFn共視特征點少于閾值Thc的關鍵幀；

步驟F，對地圖中所有的靜態地圖點和關鍵幀，利用光束平差法進行全局優化。

所述地圖點濾波線程包括以下步驟：

步驟(I)，將地圖中每個屬性為未知的地圖點投影到圖像幀Fn的魚眼圖像Ifn上，形成投影點

步驟(II)，對每個投影點計算ORB描述子計算描述子與地圖點對應的描述子的漢明距離

步驟(III)，如果步驟(II)中漢明距離小于閾值Thh，則地圖點的靜態指數加1，否則靜態指數減1，如果靜態指數大于閾值Ths，則地圖點的屬性變為靜態，如果靜態指數小于閾值Thdy，則地圖點的屬性變為動態。

本發明的優點在于：

1.能夠有效排除動態物體對視覺里程計的干擾，可在實際動態環境下運行。

2.相比于傳統視覺里程計，在動態環境下的運動估計精度更高。

3.對快速運動、純旋轉等運動形式具有更強的穩健性。

4.利用魚眼相機+深度相機的硬件配置方式，圖像處理工作量小，效率高。

附圖說明

圖1為本發明的一種融合魚眼圖像與深度圖像的動態環境視覺里程計方法流程圖。

圖2為本發明所采用的地圖中地圖點的構成示意圖。

具體實施方式

下面將結合附圖對本發明做進一步的詳細說明。

如圖1所示，本發明同時使用魚眼圖像和深度圖像的信息，由跟蹤線程、地圖構建線程和地圖點濾波線程組成，三個線程并行運行。

本發明采用的相機為Intel Realsense ZR300，該相機同時提供魚眼圖像和深度圖像，魚眼圖像和深度圖像的大小均為640×480像素。

本發明提供的一種融合魚眼圖像與深度圖像的動態環境視覺里程計方法，包括地圖初始化、跟蹤線程、地圖構建線程和地圖點濾波線程四個部分，其中三個線程并行運行。本發明將地圖劃分為關鍵幀和地圖點兩部分，如圖2所示，每個地圖點具有一個靜態指數si，以及一個用于標識該地圖點為靜態、動態或者未知的地圖點屬性。

系統的運行依賴于地圖，地圖初始化具體步驟如下：

步驟1，同步采集魚眼圖像If1和深度圖像Id1，構建成圖像幀F1；

步驟2，由于深度相機的視場只占魚眼相機視場的一部分，魚眼圖像中只有中間部分的像素能在深度圖像中找到對應的深度，因此對魚眼圖像If1中間大小w×h(如320×240像素)區域的圖像提取ORB特征；

步驟3，對步驟2中提取到的每個ORB特征，在深度圖Id1中尋找深度，計算對應的3D世界坐標，構建成地圖點，初始化所述的地圖點的靜態指數si＝0，屬性為靜態。

所述跟蹤線程的具體步驟如下：

步驟1，同步采集魚眼圖像Ifn和深度圖像Idn，構建成圖像幀Fn；n＝2,3,4,…；

步驟2，對圖像幀Fn中的魚眼圖像Ifn提取ORB特征集kn；

步驟3，將圖像幀Fn中的特征集kn中所有特征點與前一幀Fn-1的特征集中指向屬性為靜態的地圖點的特征點進行匹配，形成3D靜態地圖點到圖像幀2D特征的匹配點對集合，采用RANSAC框架下的PnP算法(常用方法有EPnP、P3P、UPnP等)解算出圖像幀Fn的初始位姿

步驟4，將地圖中所有的屬性為靜態的地圖點投影到圖像幀Fn，與圖像幀Fn中的特征集kn進行匹配，形成3D靜態地圖點到2D圖像特征ui之間的匹配點對集合{pw u}，利用魚眼相機投影模型π，構建優化方程：

為圖像幀Fn的位姿，以步驟3得到的初始位姿為初值，利Levenberg-Marquardt方法得到圖像幀Fn的最優位姿

步驟5，如果步驟4中匹配點對集合{pw u}中點對數量少于閾值Thm(如100)，或圖像幀Fn與前一圖像幀Fn-1的距離大于閾值Thd(如0.2m)，則將圖像幀Fn構建為關鍵幀，并加入關鍵幀隊列。

所述地圖構建線程包括以下步驟：

步驟A，從關鍵幀隊列中取出關鍵幀KFn，對魚眼圖像Ifn中間大小w×h(如320×240像素)區域的圖像提取ORB特征；

步驟B，對步驟A中提取到的每個ORB特征，在深度圖像Idn中尋找深度，計算對應的3D世界坐標，構建成地圖點，初始化地圖點的靜態指數si＝0，屬性為未知；

步驟C，對魚眼圖像Ifn中間w×h像素(如320×240像素)區域外的特征點，利用極線搜索的方法在其他關鍵幀(關鍵幀隊列中除關鍵幀KFn以外的所有關鍵幀)上搜索配對特征，利用三角方法構建地圖點的3D世界坐標，構建成地圖點，初始化地圖點的靜態指數si＝0，屬性為未知；

步驟D，將步驟B和步驟C中新構建的地圖點，反投影到地圖中其他的關鍵幀的魚眼圖像上獲得投影點，計算所述投影點的ORB描述子計算每一個描述子與關鍵幀KFn對應特征的描述子的漢明距離，如果漢明距離少于閾值Thh(如30)，則靜態指數加1，否則靜態指數減1。如果累加后的靜態指數大于閾值Ths(如10)，該地圖點的屬性變為靜態，如果靜態指數小于閾值Thdy(如-3)，則地圖點的屬性變為動態；

步驟E，遍歷地圖中所有的關鍵幀，剔除與關鍵幀KFn共視特征點少于閾值Thc(如20)的關鍵幀；

步驟F，對地圖中所有的靜態地圖點和關鍵幀，利用光束平差法進行全局優化。

所述地圖點濾波線程包括以下步驟：

步驟I，將地圖中每個屬性為未知的地圖點投影到圖像幀Fn的魚眼圖像Ifn上，行成投影點

步驟II，對每個投影點計算ORB描述子計算描述子與地圖點對應的描述子的漢明距離

步驟III，如果步驟II中漢明距離小于閾值Thh(如30)，則地圖點的靜態指數加1，否則靜態指數減1，如果靜態指數大于閾值Ths(如10)，則地圖點的屬性變為靜態，如果靜態指數小于閾值Thdy(如-3)，則地圖點的屬性變為動態。

總結

以上是生活随笔為你收集整理的python鱼眼图像识别_一种融合鱼眼图像与深度图像的动态环境视觉里程计方法与流程...的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。