基于AprilTag的視覺二次定位

楊維鑫 ，劉大鵬 ，黃政暉 ，沈方榮 ，章勇杰

（浙江農林大學光機電工程學院，浙江 杭州 311300）

隨著計算機技術及其網絡技術的飛速發展，人工智能技術在機械工業中的應用越來越廣泛[1]，其中移動機器人[2]在機械工業化中的應用也越來越廣泛，移動機器人在環境中的定位問題受到人們的廣泛關注。目前，人工地標的識別定位被廣泛運用，AprilTag就是其中代表之一，在室內或室外通過合理設置AprilTag圖標[3]，就可以方便地實現圖標的識別與定位。AprilTag是由密歇根大學Edwin Olson教授及其實驗室團隊在2011年提出的一種標記識別算法，與2D圖標（例如二維碼）類似，AprilTag加入的識別圖標的編碼，與傳統的視覺地標識別系統相比，魯棒性更好。并且在快速識別的同時能夠處理遮擋、翹曲和透鏡變形等情況。AprilTag具有良好開源特性，有許多的學者已經將AprilTag運用在室內定位和無人機定位的研究當中。長安大學的林威等[4]提出來了一種基于AprilTag的視覺定位導航方案用于分體式飛行汽車的導引對接，解決了GPS因誤差大而無法實現厘米級精度的導引對接問題，并提升了平穩性。河南理工大學的孫藝銘等[5]通過單目攝像頭識別AprilTag標記的空間信息，解算車體位置，進而實現對智能車的自動跟隨控制。臺州學院的章書琦等[6]提出了一種基于AprilTag自動跟隨控制系統的公共場所行李搬運車，將AprilTag圖標置于腰帶上，行李搬運車上的攝像頭識別和定位用戶攜帶的AprilTag，從而實現自動跟隨，減少維護成本。本研究將AprilTag置于目標點作為標記，用攝像頭識別AprilTag的信息，解算導航之后移動機器人距離目標點的距離，從而實現移動機器人的二次定位。

1 AprilTag識別定位

AprilTag識別定位程序流程如圖1所示。首先通過攝像頭獲取AprilTag圖像，再對圖像進行灰度化處理以及高斯濾波消除噪聲，然后再利用自適應閾值方法將灰度圖像轉成二值化圖像，并對圖像進行邊緣檢測，使邊緣兩側不同的黑白像素部分聚類為不同的簇。接著為聚類的無序邊緣點進行擬合，擬合成一個個四邊形，對形成的四邊形進行編碼，并與已知的編碼庫進行匹配，如果匹配不到就重新獲取圖像，如果匹配成功，即可進行兩者相對位置的計算。

圖1 AprilTag識別定位流程圖

1.1 圖像線段檢測

對圖像上每個像素上的梯度強度和方向進行計算，然后根據梯度的強度以及方向進行聚類，聚類算法是基于圖的算法，圖像上的每一個像素為圖的節點，圖像上相鄰的兩個像素皆存在一條邊，且邊的權重值為梯度方向之差，根據增加的邊權值對這些邊進行排序，并根據權值進行邊的合并[7]。給定一個集合n，邊的梯度方向范圍為D（n），邊的梯度強度范圍為M（n），D（n）的值位于0~2Π之間。給定兩個邊的集合為n和m，滿足以下條件即可合并：

1.2 四邊形檢測

檢測完圖像的所有線段之后，接下來是對線段進行分組。如果前一條邊的末端點和下一條邊的始端點之間的距離小于一個閾值，那么就將兩條線段按照逆時針方向連接。在線段分組成功后，線段會構成一個樹，樹的第一層是所有的有向線段，第二層到最后一層的節點為同一組候選線段，使用深度優先搜索遍歷整個樹，在樹的第四層時，如果后一條邊與第一條邊構成一個閉環，說明其滿足構成四邊形條件，遍歷邊的節點路徑就可以構成四邊形[8]。

1.3 編碼

查找完所有四邊形之后，需要對四邊形進行編碼[9]，根據四邊形內部的點陣來確定編碼方式（例如8*8，7*7，6*6），將點陣的最外層黑色點陣去除，留下內部的點陣進行編碼。并計算AprilTag最外層和次外層兩層色塊的灰度均值作為點陣中每個點判別黑白的閾值，大于該值編為1，小于該值編為0，從而得到二進制編碼。對于二維碼的編碼，必須對向旋轉魯棒，因此需要得出每次旋轉90°之后方向上的編碼，一共為4組。

1.4 與已知編碼庫進行匹配

完成編碼后，計算四個編碼與預設家族的編碼庫中每個ID編碼之間的漢明距離，漢明距離是兩個二進制數據之間，位數不相同的個數。當找到漢明距離小于2的庫編碼時，則該四邊形即為相應家族對應ID的AprilTag。如果沒有滿足條件的漢明距離，則該四邊形不是目標家族的AprilTag，將其舍棄。

1.5 相機位姿解算

根據已知的AprilTag坐標系中四個角點的坐標和圖像二維碼四個角點的坐標，可以利用DLT直接線性變換算法計算單應性矩陣H[10]，其用來映射物體在Tag坐標系和圖像像素坐標系之間的位置。矩陣H表示為相機內參矩陣P與外參矩陣E的乘積，外參矩陣E通常為4×4階，因為Tag上的每個點在Tag坐標系中Z坐標都為零。因此，移除矩陣E的第三列，使用旋轉參數Rij和平移參數Tk形成截取后的外參矩陣。hij是單應性矩陣H中的元素，fx和fy為相機焦距參數，Rij為位置變換參數矩陣E中的旋轉分量元素，Ti為平移分量元素，未知尺度因子為s，則單應性矩陣H可表示為：

將上式展開得到：

由于旋轉矩陣為單位正交矩陣，矩陣中列向量的模為1，利用該約束能夠計算得到s。通過計算R矩陣中兩個列向量的模長均值，得到s的大小。將s作為前兩列旋轉矩陣大小的幾何平均值，再結合旋轉矩陣每一列都為單位矩陣的性質求出前兩列元素的值，然后利用正交矩陣的性質進行叉積運算得出旋轉矩陣第三列元素的大小，但是通過DLT得到的旋轉矩陣不一定具有單位正交特性，所以需要對R矩陣做極分解，最終獲得Tag坐標系到相機坐標系的變換矩陣

得到相機坐標系[11]相對于Tag坐標系的變換矩陣之后，還需要做如下轉換，得到作用于相機的位姿估計的R、t矩陣。

2 實驗結果與分析

為了驗證基于AprilTag的視覺移動機器人二次定位方法的有效性，搭建了基于四輪驅動的移動機器人平臺，如圖2所示。該平臺由底盤、電機、電源、驅動器（下位機）、工控機、激光雷達以及Kinetic相機組成。

圖2 移動機器人平臺

實驗環境取自實驗室，并將AprilTag碼貼在地面，如圖3所示，其中心為原點，垂直于地面為Z軸，圖標正向右側為Y軸，圖標正向為X軸，大小為20 cm×20 cm。

圖3 AprilTag碼

打開攝像頭，通過前后移動車的位置，并與通過ROS訂閱移動機器人平臺相對于AprilTag位置信息，如圖4所示。

圖4 移動機器人平臺相對于AprilTag位置

移動機器人平臺在導航結束之后啟動視覺二次定位的實測情況如圖5所示，可以看出當定位前移動機器人平臺正對AprilTag碼時，定位后AprilTag碼的中心與小車的中心偏差較小。當定位前移動機器人平臺與AprilTag碼有一定的角度誤差時，定位后AprilTag碼的中心與小車的中心偏差較大，但小車基本停在了AprilTag碼的區域內。

圖5 移動機器人平臺導航結束二次定位前后對比

3 總結

本文通過視覺二次定位的方法，基于AprilTag基準標記系統獲得移動機器人平臺的位置信息，并控制激光SLAM導航結束后的移動機器人平臺朝標記點前進，用于彌補移動機器人導航之后的位置離真正導航點的位置誤差，通過實驗可以得出移動機器人通過該方法可以到達指定地方，基本完成了任務。但是移動機器人平臺與AprilTag產生一定的角度誤差，視覺二次定位的精確度還有待提升，后期需展開進一步研究，提高定位的精確度。