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

在審

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

1.一種頂標地圖構建及機器人定位方法，其特征在于，所述方法包括如下步驟：

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

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

S3、提取當前圖像中的已知頂標和未知頂標；

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

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

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

2.如權利要求1所述頂標地圖構建及機器人定位方法，其特征在于，所述步驟S4具體包括如下步驟：

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

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

S43、基于公式X＝-R^-1*t求得相機中心在路標地圖中的坐標，相機的旋轉角為θ＝actan(R_(1,0)/R_(0,0))，即獲取相機中心在路標地圖中的位姿。

3.如權利要求1或2所述頂標地圖構建及機器人定位方法，其特征在于，在步驟S4之后還包括：

S7、將機器人當前在路標地圖中的位姿P(x,y,θ)作為擴展卡爾曼濾波模型的輸入，對機器人的當前位姿進行優化。

4.如權利要求1所述頂標地圖構建及機器人定位方法，其特征在于，所述步驟S5具體包括如下步驟：

S51、基于已知頂標在圖像中的坐標x_i及路標地圖中的坐標x_w，計算相機坐標系在地圖坐標系中的位姿rMatU，tMatU；

S52、基于公式(1)及公式(2)計算頂標坐標系到地圖坐標系的轉換矩陣R′、及平移向量t′，公式(1)及公式(2)表示如下：

R′＝rMatU^-1*rMat (1)

t′＝rMatU^-1*(tMat-tMatU) (2)

其中，rMat為相機坐標系到地圖坐標系的旋轉矩陣，tMat為相機坐標系到地圖坐標系的平移向量。

5.如權利要求1或4所述頂標地圖構建及機器人定位方法，其特征在于，在步驟S6之后還包括：

S8、將更新后的路標地圖作為擴展卡爾曼濾波模型的輸入，對路標地圖進行優化。