[發明專利]一種頂標地圖構建及機器人定位方法

在審

• 申請號: 201811618066.1
• 申請日: 2018-12-28
• 公開（公告）號: CN109612468A
• 公開（公告）日: 2019-04-12
• 發明（設計）人: 陳智君;李超;曹雛清;高云峰
• 申請（專利權）人: 蕪湖哈特機器人產業技術研究院有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 蕪湖安匯知識產權代理有限公司 34107
• 代理人: 馬榮

本發明適用于自動控制技術領域，提供了一種頂標地圖的構建及機器人定位方法，包括：S1、判斷當前是否進入定位模式，若判斷結果為是，進入定位模式，執行步驟S2、步驟S3、及步驟S4，若判斷結果為否，進入建圖模式，執行步驟S2至步驟S6；S2、提取當前圖像中的頂標，獲取頂標ID，計算頂標在圖像中的位置；S3、提取當前圖像中的已知頂標和未知頂標；S4、基于至少3個已知頂標來計算機器人當前在路標地圖中的位姿；S5、計算從頂標坐標系到地圖坐標系的轉換矩陣R′、及平移向量t′；S6、基于未知頂標在頂標坐標系中的坐標x_map，計算未知頂標在路標地圖中的坐標x_w，更新路標地圖。基于當前頂標坐標系到地圖坐標系的轉換矩陣及平移向量來計算為未知頂標在路標地圖中的位置，即構建路標地圖，可以減弱誤差的累積。

技術領域

本發明屬于自動控制技術領域，提供了一種頂標地圖構建及機器人定位方法。

背景技術

隨著社會的發展和技術的進步，移動機器人越來越深的介入到人類的日常生活中，例如家庭中的清潔機器人、工廠中的搬運機器人以及餐館中的送餐機器人等。移動機器人想要實現上述所有功能就必須要準確的知道自身所在的位置，即實時定位。而實現機器人實時定位的先決條件是建立地圖，這是實現機器人導航和其它智能行為的關鍵。移動機器人建圖常用的傳感器有攝像機和激光雷達等。其中激光雷達硬件成本較高，不利于移動機器人的大范圍普及推廣。而利用攝像機進行定位硬件成本低、定位精度高，在室內定位中，基于視覺的定位方法被廣泛的應用。

基于視覺的室內定位首先要構建精準的室內地圖，用于絕對坐標系下的攝像機姿態的計算以及機器人移動路徑的規劃。可以通過人工路標構建精確的幾何地圖，也可以利用環境地標來構建地圖。其中基于環境地標的方法雖然普適性較好，不需要人工布設額外的標志，但是計算復雜，實用性較差。人工路標往往在顏色、形狀等視覺特征上具有明顯的唯一性，通過計算機視覺方法能夠很容易的將人工路標識別出來。其中有一種在屋頂布設人工路標的方法，屋頂環境單一易于提取，且攝像機的視野不容易受到干擾，在室內定位中被廣泛應用。但是現有的基于頂標的地圖構建方法是根據已知路標依次計算未知路標在地圖中的位置，計算過程中的誤差會累積向后傳播，在頂標數目較多時地圖構建結果不準確。

發明內容

本發明實施例提供一種頂標地圖構建及機器人定位方法，旨在提高地圖構建的精準性。

為了實現上述目的，本發明提供了頂標地圖構建及機器人定位方法，該方法包括如下步驟：

S1、判斷當前是否進入定位模式，若判斷結果為是，進入定位模式，執行步驟S2、步驟S3、及步驟S4，若判斷結果為否，進入建圖模式，執行步驟S2至步驟S6；

S2、提取當前圖像中的頂標，獲取頂標ID，計算頂標在圖像中的位置x_i；

S3、將提取到的頂標ID與路標地圖中的路標ID進行對比，得到已知頂標和未知頂標；

S4、基于至少3個已知頂標來計算機器人當前在路標地圖中的位姿P(x,y,θ)；

S5、計算頂標坐標系到地圖坐標系的轉換矩陣R′、及平移向量t′；

S6、基于未知頂標在頂標坐標系中的坐標x_map，計算未知頂標在路標地圖中的坐標x_w，更新路標地圖。

進一步的，所述步驟S4具體包括如下步驟：

S41、基于頂標在圖像中的坐標x_i及相機的內參矩陣M_cam計算

S42、基于仿射變換方程計算出旋轉矩陣及平移向量旋轉矩陣及平移向量去除攝影深度因子得到相機坐標系到地圖坐標系的旋轉矩陣R、及平移向量t；