[發(fā)明專利]一種基于Apriltag標簽與因子圖的SLAM方法

有效

• 申請?zhí)枺?/td>: 202010017116.1
• 申請日: 2020-01-08
• 公開（公告）號: CN111242996B
• 公開（公告）日: 2021-03-16
• 發(fā)明（設計）人: 郭軒;程楠
• 申請（專利權）人: 郭軒
• 主分類號: G06T7/50
• 分類號: G06T7/50;G06T7/73;G01C21/20
• 代理公司: 新鄉(xiāng)市平原智匯知識產(chǎn)權代理事務所(普通合伙) 41139
• 代理人: 林海

本發(fā)明提供了一種基于Apriltag標簽與因子圖的SLAM方法，包括：首先讀取預設數(shù)據(jù)并構建坐標系；以預設數(shù)據(jù)為因子，初始化因子圖；使用相機采集標簽圖像；利用相機標定畸變參數(shù)對標簽圖像進行預處理；提取Apriltag標簽角點與ID；以Apriltag標簽角點的圖像坐標值為映射因子更新因子圖；判斷是否提供相機載體的里程計(Odom)；采集載體里程計數(shù)據(jù)；以載體里程計數(shù)據(jù)為相對位姿因子更新因子圖；優(yōu)化因子圖，計算物體，Apriltag標簽，載體，相機的位姿，最后更新地圖。利用本發(fā)明能夠提供高精度的載體位姿數(shù)據(jù)，并構建高精度的地圖信息。

技術領域

本發(fā)明屬于機器人自主導航技術領域，涉及一種基于Apriltag標簽與因子圖的機器人同步定位與制圖(SLAM)方法。

背景技術

SLAM技術是機器人自主導航領域的關鍵與研究熱點，是實現(xiàn)機器人環(huán)境感知的關鍵基礎技術，在工業(yè)，物流，服務等領域有廣泛的應用前景。Apriltag是一種視覺基準標定(fiducial marker)系統(tǒng)，提供標簽(Tag)與相機之間空間位姿關系以及自身ID信息。目前基于標簽定位技術大多采用PnP法實現(xiàn)，此類技術缺少整體優(yōu)化與回環(huán)檢測，位姿估算受測量誤差影響大。

目前多數(shù)視覺SLAM技術直接從圖像中提取自然特征點進行匹配來實現(xiàn)定位與制圖。傳統(tǒng)特征點提取存在數(shù)據(jù)量大，易丟失，匹配計算復雜，抗干擾能力差等缺點；同時，此類SLAM技術依賴于網(wǎng)格化生成算法來感知空間物體存在，易受點云噪聲干擾。此類技術存在計算復雜，制圖信息不完整的缺點。

發(fā)明內容

本發(fā)明的目的：本發(fā)明提供一種基于Apriltag標簽(簡稱標簽)與因子圖的SLAM方法，以解決現(xiàn)有SLAM技術計算量大，空間物體感知能力差，構建地圖不完整且誤差大的問題；通過本發(fā)明公開的方法，利用人工設定的特征點(標簽角點)，預先有限設定的空間數(shù)據(jù)以及因子圖，能夠實現(xiàn)載體的高精度定位，空間物體建模，以及獲得完整精確的地圖。

技術方案：本發(fā)明所述的基于Apriltag標簽與因子圖的SLAM方法包括：

步驟1，讀取預設數(shù)據(jù)，構建世界(World)坐標系，物體(Object)坐標系，標簽(Apriltag)坐標系，載體(Robot)坐標系，和相機(Camera)坐標系；

其中，所述的構建世界坐標系，物體坐標系，標簽坐標系，載體坐標系，和相機坐標系的具體方法為：

步驟1.1，定義李代數(shù)位姿和李群sE(3)變換矩陣其中是平移(translation)向量，是旋轉(rotation)向量；

其中，位姿ξ與變換矩陣T存在映射公式：

步驟1.2，如圖2所示，以初始時刻某點的位置為原點構建世界坐標系；以物體某頂點為原點構建物體坐標系；以標簽的第一個角點為原點，所在平面為X-O-Y平面構建標簽坐標系；以載體中心為原點構建載體坐標系；以相機焦點為原點構建相機坐標系；設物體在世界坐標系下位姿為標簽在物體坐標系下位姿為相機在載體坐標系下位姿為載體在時刻t在世界坐標系下位姿為標簽在時刻t在相機坐標系下位姿為

為對應變換矩陣；

其中，所述的預設數(shù)據(jù)為人工測量數(shù)據(jù)和設定數(shù)據(jù)，其包括：

設定為物體在世界坐標系下，標簽在物體坐標系下或相機在載體坐標系下的位姿測量值，對應變換矩陣∑為協(xié)方差矩陣；