基于ArUco碼的無地圖AMR視覺輔助歸航方法*

郭驛眾，李 根，許佳斌，王衛軍，馮 偉，王 建

(1. 廣州先進技術研究所·廣州·511458；2. 中國科學院深圳先進技術研究院·深圳·518055)

0 引 言

作為自主移動機器人(Autonomous Mobile Robot，AMR)的關鍵組成部分，導航模塊已經發展出以慣性、地磁、視覺、激光雷達等傳感器為基礎的同時兼具效率和精度的導航方法。由于圖像中包含大量空間信息，對于室內環境中工作的AMR，采用視覺傳感器的導航方式在信息豐富度和環境適應性上具有較大的優勢[1]。

視覺歸航(Visual Homing，VH)是一種輕量級機器人的導航方式，只利用參考位置和當前位姿下拍攝的圖像來進行導航，不需要當前狀態之前的路徑和運動。該導航方式通常采用純方位方法實現，比如Cartwright和Collett仿照蜜蜂歸巢原理提出的快照模型(Snapshot Model，SM)[2]，通過移動不斷減小當前位姿下圖像和參考圖像的差距來達到靠近目標點的目的。Corke P使用平均地標矢量(Average Landmark Vector，ALV)原理[3]，通過檢測地標并對比當前位置和目標位置關鍵點方向矢量和的差值求出導航矢量。Robert和 Michael等人提出考慮圖像平面均勻網格上點的流向量屬性[4]。流向量指示當前圖像中局部區域必須移動的方向，通過對齊當前圖像與參考圖像提供導航控制。這些傳統的視覺歸航方法多依賴于環境中的全局地標及導航過程中地標的選擇[5]，且對于環境紋理及光照的要求較高，對于動態環境則缺乏魯棒性[4]，此外，還存在無法獲得精確的導航位置，以及無法保證收斂性等問題[5]。

通過引入ArUco碼作為地標，可以解決傳統視覺歸航方法的弊端[6]。相較于傳統的基于視覺的方式，對ArUco碼進行識別和相對定位可以直接估計位置和航向，減少了計算量，且具有較高的精度和魯棒性[7]。另外，可以通過將ArUco碼粘貼在天花板上減少可能受到的障礙物遮擋的影響[8]。ArUco碼具有成本低廉，信息獲取較為簡單，可以使用低成本攝像頭進行檢測等優點[7]。然而采用ArUco碼輔助視覺導航有時局限于單個標記，比如在預定位置采用ArUco碼標記引導自動導航小車(Automated Guided Vehicle，AGV)轉向[9]；或者至多局限于一部分標記，且這些標記的相對位姿需要已知，比如與同步定位和建圖(Simultaneous Localization and Mapping，SLAM)進行結合[10]，在使用ArUco碼輔助機器人導航之前，對ArUco碼進行全局定位并提前構建地圖，通過識別ArUco碼對比全局地圖，提供自主移動機器人的位置和航向信息。

使用SLAM對ArUco碼進行建圖，具有較高的定位精度，然而其需要預先構建精確且全局一致的地圖，當地圖丟失時，機器人立即失去定位和導航的能力。本文提出了一種在沒有構建全局地圖的情況下，通過對既往路徑中記錄的ArUco碼信息進行反向對比和搜尋的方式，實現移動機器人無地圖自主視覺返航同時實現避障功能的方法。

1 視覺導航設計

1.1 系統設計

如圖1所示，相機視界內通?？梢宰R別到多個ArUco碼，機器人的運動會導致識別的ArUco碼產生時間順序上的連續變化，本文將通過識別和記錄對應ArUco碼構建的局部坐標系下的關鍵位姿，并通過不同的ArUco碼構建的局部坐標系的連續變換實現機器人的反向導航并回到出發點。然而該方法只能適用于機器人經過和記錄的地標物，當遇到動態障礙物或者未曾經過的地方，該方法就無效了，因此，本文引入里程計，通過對車輪轉速進行積分來獲得機器人既往任意時刻的位姿。然而里程計存在累積誤差，且隨著累計時間的增加而變大，因此機器人只采用較近期位姿實現超出原有識別區域的自主返航。同時使用激光雷達提供障礙物的距離和方位信息用以避障。

圖1 相機視界與機器人軌跡Fig.1 Camera field of view and the robot trajectory

具體來說，系統有返航和非返航兩種狀態，非返航狀態下可以通過預設軌跡或手動方式操作移動機器人進行運動，全過程中相機采用固定頻率對視界中的ArUco碼進行識別，由于同一時刻相機視界內存在多個二維碼，解算當前時刻相機相對視界內所有ArUco碼的位姿，并以當前時刻相機直線距離最近的ArUco碼建立局部坐標系，如果該時刻是初次識別到ArUco碼的時刻，將機器人當前相對于該ArUco碼的位姿作為關鍵位姿進行保存，同時左右輪編碼器通過計算轉動速度積分并記錄車輛軌跡。

返航狀態的流程如圖2所示，首先相機對視界內所有ArUco碼進行識別；再檢索數據庫中是否有保存對應ArUco碼ID號，并以此判定是否經過該ArUco碼對應的可識別區域。如果保存有對應的ID號，則判定經過，并進行基于ArUco碼時序最小的導航偏差估計。由于ArUco碼存取是按照識別的時間順序保存，如果存有甚至有多個ArUco碼被識別和保存，則選取時序更小的ArUco碼構建局部坐標系，獲得機器人在當前ArUco碼坐標系下的位姿作為當前點，并讀取該ArUco碼被初次識別時的機器人的位姿作為目標點。如果沒有保存對應的ArUco碼，說明機器人移動到一個未曾到過的區域，則判定未經過，則以當前視界范圍內直線距離最近的ArUco碼構建局部坐標系，并獲取機器人在當前坐標系下的位姿作為當前點。通過讀取編碼器積分軌跡選取前瞻點作為目標點。

圖2 無地圖視覺導航系統返航流程圖Fig.2 Flow chart of the visual homing navigation system with no map building

隨后，通過激光雷達判定是否有障礙物及與障礙物之間的距離。當判定路徑上沒有障礙物時，利用純跟蹤(Pure Pursuit，PP)導航算法計算線速度和角速度并驅動機器人移動，直至返回出發位置。當判定路徑上存在障礙物時，采用動態窗口(Dynamic Window Approach，DWA)算法對機器人進行軌跡預測，同時通過讀取編碼器積分軌跡選取前瞻目標點，實現避障功能并返回既往軌跡。

1.2 導航原理

基于人工標記物位置固定的前提下，如ArUco碼粘貼在天花板上(圖3)，相機在任何可以拍攝到ArUco碼的位置都可以通過識別視界內的ArUco碼獲得該碼坐標系下的相對位姿，即該位姿下相機坐標系相對于ArUco碼坐標系的空間轉換矩陣。如圖3所示，M1CT為相機相對1號ArUco碼的空間轉換矩陣，M2CT為相機相對2號ArUco碼的空間轉換矩陣。同時，雷達和相機固定在機器人上，且兩個傳感器都有其特定的測定坐標系。從圖3中可知，雷達坐標系、相機坐標系及機器人坐標系的Z軸重合，且三者原點都位于機器人本體中軸線上，通過測量固定位置的距離，可以很輕易地實現從雷達到相機及從機器人到相機的坐標轉換，為了簡化說明，后文以相機坐標系替代機器人本體坐標系進行討論。

圖3 局部坐標系的連續變換Fig.3 Continuous transform of the local coordinate system

M2rR=M2CR×CrR+M2CS

(1)

CrRQ=M2CRR-1×(M2rQ-M2rR)

(2)

通過將方向矢量帶入運動學模型中即可解出機器人線速度及角速度[v，ω]。為了實現連續的導航，機器人需要在上一個ArUco碼離開相機視界范圍之前識別到下一個ArUco碼并重復構建局部坐標系、坐標轉換及計算控制量的步驟，即要求任意ArUco碼間距小于相機的視界直徑。

在非返航過程中機器人將所有識別的ArUco碼以時間順序保存，在返航過程中通過對視界內ArUco碼的識別和對比，始終選取時序最小的，可以確保在連續返航的過程中回到出發點的收斂性，同時在偏離原有航跡且非連續倒序識別ArUco碼時，對于重新識別的ArUco碼仍然可以通過時序對比返回出發點，具有一定的自適應性。

1.3 ArUco碼輔助定位

ArUco碼是一種由黑色邊框及內部白色二進制編碼組成的正方形基準標記(Squared Fiducial Marker，SFM)，相較其他人工路標具有檢測精度高及檢測速度快等優點[6]。通過基于OpenCV的圖像識別和處理，將識別出的ArUco碼解碼為二進制與字典庫進行匹配可以獲得對應的唯一ID號，并獲得ArUco碼中心坐標系的四個角點的坐標(見圖4)。

圖4 ArUco碼識別及中心坐標系下角點坐標Fig.4 Recognition of the ArUco marker and the endpoint coordinates at the corresponding ArUco marker coordinate system

(3)

(4)

(5)

將n個匹配點對全部展開，可以寫成一個2n×12的矩陣M和一個12×1的矩陣x，所有使得Mx=0的解可以表示為

(6)

其中，vi是M的零奇異值對應的右奇異矢量，通過構建MTM組成的方陣，求解特征值為0的特征矢量，求得為vi。根據剛體結構不變性，不同坐標系下的兩個點的相對距離是不變的，原式可優化為

(7)

2 運動學模型的建立

2.1 兩輪差速模型

自主移動機器人采用兩輪差速模型[14]，通過控制左右兩個驅動輪的轉動速度可以實現機器人的直行和轉向。僅考慮機器人在二維水平面的運動情況，運動學模型如圖5。

圖5 差速自主移動機器人運動學模型Fig.5 Dynamic model of differential autonomous mobile robot

通過驅動輪上的編碼器可以得到左右輪的轉動速度vr，vl，已知驅動輪輪距為b，從而解算出機器人中心的線速度及角速度v，ω。

(8)

在驅動輪純滾動，無側向滑動的情況下，機器人受到非完整約束[15]，那么n時刻機器人在當前ArUco碼坐標系下的位姿[x，y，θ]T存在以下關系

(9)

同時，機器人軌跡可以用圓弧進行表示，且n時刻圓弧半徑為Ln=vn/ωn，其中vn，ωn為n時刻機器人的線速度和角速度。通過對左右輪轉速記錄并進行積分，可以得到小車從開始到結束任意時刻相對于出發點位置的坐標朝向和速度，盡管使用速度積分的方式會引入累積誤差。

2.2 局部導航設計

DWA動態窗口法是基于運動學模型，在有限速度和加速度及短暫時間間隔的約束下的一種速度控制方法[15]，多用于障礙物躲避的局部路徑規劃。DWA可以概括為三步[16]：一是根據機器人自身限制和環境制約將速度的采樣空間約束在一定范圍內；二是根據機器人運動學對采樣后的速度進行模擬得到預軌跡；三是設定評價函數對預軌跡進行評分以獲取最優軌跡對應的執行速度。

將機器人當前速度設為[vc，ωc]，根據機器人自身速度及加速度的限制，速度采樣空間應滿足

(10)

通過設定的采樣分辨率獲得所有[v，ω]的速度組合，并設定預測時間窗口的長度，根據運動學模型進行模擬獲得預測的軌跡和終點位置。為了獲取最優的執行速度組合，采用代價函數對每條軌跡進行評價。代價函數定義為

C(v,ω)=αCdist(v,ω)+βCtarg(v,ω)

(11)

式中，使得C(v，ω)值最小的速度組合為最優速度；函數Cdist(v，ω)用來評價機器人在當前軌跡上與最近的障礙物之間的距離，距離越遠，Cdist(v，ω)的值越??；Ctarg(v，ω)為當前設定的采樣速度下，達到模擬軌跡終點的機器人與當前ArUco碼記錄的機器人起始點的歐幾里得距離，即返航過程始終將當前相機視野范圍內索引值最小的ArUco碼對應的初次識別到該ArUco碼時刻的位置作為返航目標點；α，β為每一個函數的權重。

3 實驗驗證與分析

3.1 實驗平臺搭建

本文搭建了如圖6所示的兩輪差速機器人實驗平臺進行測試，主要由顯示器、工控機和運動底盤組成。攝像頭使用的是索尼IMX317，分辨率為1920×1080，幀率為30Hz。實驗環境為如圖7的6m×12m的實驗室，天花板上均勻等距粘貼ArUco碼，攝像頭與天花板的距離為2.5m；地面采用紙箱作為障礙物。采用激光雷達構建了全局地圖及Marker的特征地圖為機器人在測試過程中提供定位。

圖6 機器人實驗平臺Fig.6 Robot platform

圖7 實驗室環境及地圖Fig.7 Testing site and the map

3.2 導航原理驗證

將相機視界以時間順序進行拼接。由于相機視角是從下向上，而Rviz建圖視角是從上向下，將相機視界翻轉后可以得到如圖8的視界圖和實際軌跡的對比，兩者具有高度的相似度，同時相機對ArUco碼的識別時序與導航路徑點具有同步性，每一個ArUco碼的關鍵位姿與實際機器人軌跡之間存在對應關系，因此，找尋時序更小的ArUco碼意味著找尋時序更小的路徑點，即更加靠近出發點的位置，直至返回出發點。

圖8 相機視界(翻轉)與導航軌跡Fig.8 Camera view (reversed)and the corresponding trajectory

3.3 重復定位測試

通過對同一條長度為3m的直線路徑進行20次的重復測試，以測定僅靠相機定位及導航的重復定位精度。在機器人停止時，通過讀取最小時序ArUco碼(即初始ArUco碼)獲得機器人當前位置的坐標，同時讀取出發時刻機器人起始位置坐標，并將這些坐標顯示在圖9中。停止點距離目標點的平均距離誤差為0.0169m(即1.69cm)，方差為0.000887m2(即8.87cm2)，20個停止點的平均停止點坐標為(-0.099788，0.79)cm，距目標點的誤差為0.8cm，該距離小于平均距離誤差的原因是平均停止點反映了空間上的分布，減小了極端數據對于均值的影響，具有更高的可信度。

圖9 20次重復定位測試Fig.9 Result of 20-times repeat positioning test

3.4 繞障軌跡歸航測試

通過使用鍵盤控制的方式控制移動機器人出發，形成一條特定的軌跡，比如實驗中采用繞障軌跡，分別對比了繞開單個(圖10(a))、兩個(圖10(b))、三個障礙物(圖10(c))及全覆蓋路徑(圖11)的軌跡。

(a)

(b)

(c)圖10 移動機器人繞障返航軌跡Fig.10 Homing trajectory of the AMR with obstacle avoidance

圖11 全覆蓋路徑及返航軌跡Fig.11 Full coverage path and the corresponding homing trajectory

在機器人移動的同時，相機對視界范圍內的ArUco碼標記物進行識別，僅將5次以上關鍵幀識別的ArUco碼ID進行保存，以避免運動過程中相機視界短時間掃過的ArUco碼造成的干擾。僅保留長時間穩定識別的ArUco碼，同時以此時距離最近的ArUco碼的位姿信息作為該ArUco碼對應的關鍵位姿信息，在到達預定目標點后觸發機器人返航功能。機器人識別視界內的ArUco碼標記物并查找存儲的ID號，通過存儲的時序選擇時序更小的ArUco碼生成局部坐標系，提取該ArUco碼的關鍵位姿作為當前坐標系下目標點，并生成控制量以控制機器人移動。在手動操作和返航的同時，采用激光雷達對移動機器進行定位并記錄軌跡，采用Rviz可視化工具顯示機器人運動軌跡信息(圖10)，可以清晰地對比返航前和返航后的軌跡。

在該實驗中，機器人對不同的繞障軌跡的返航展示了返航的能力，在僅依靠視覺信息及激光雷達信息進行返航時實現了對靜態障礙物的避讓，同時返回了出發位置。對于單個障礙物的軌跡(圖10(a))，返航軌跡的路程與出發軌跡的路程幾乎接近，同時由于僅保存對應ArUco碼的關鍵位姿，稀疏的導航點使得機器人在移動過程中產生一定的震蕩；對于兩個障礙物的軌跡(圖10(b))，返航軌跡相比出發軌跡節約了9.76%的路程；對于三個障礙物的軌跡(圖10(c))，返航軌跡相比出發軌跡節約了57.33%的路程。

對于出發軌跡，尤其是在復雜軌跡的情況下，本方法所具有的抄近路的能力更加顯著，如圖11所示全覆蓋路徑，返航路程相比出發路程節約了79.16%。

在復雜軌跡相交部或者頸部，由于相機視界范圍的覆蓋使得有重復ArUco碼出現，基于時序最小的導航偏差估計，此時機器人展現出跳過部分路徑抄近路的能力。當軌跡越接近直線時，返航路程則接近出發路程。由于機器人導航僅依靠視覺信息，返航的穩定性受到光照的嚴重影響，當無法識別ArUco碼或者嚴重偏離已識別區域時，機器人會失去導航能力。

4 結 論

提出了一種簡單的基于視覺的實現自主移動機器人無地圖返航及避障的方法，該方法采用ArUco碼作為視覺標定物。機器人在出發狀態中識別并以時序記錄相對于視界內的ArUco碼的關鍵位姿，使得在返航狀態中通過選擇視界內時序更小的ArUco碼并構建局部坐標系從而實現局部定位與導航，并通過局部坐標系的連續變化，實現無全局地圖情況下返回出發點的目的?；诓煌霭l軌跡，該方法展現出一定的跳過既往路徑，以更近的路線返回出發點的能力。當出發軌跡接近直線，返航軌跡接近出發軌跡；對于復雜軌跡的相交部或頸部，由于相機視界識別到時序更早的ArUco碼，使得機器人可以以更短的路徑返回出發點。實驗結果表明：

1)基于相機和ArUco碼標記物的情況下，20次重復定位測試中實現8mm的平均定位誤差；

2)基于激光雷達和里程計，在無地圖的情況下，機器人通過動態窗口算法實現自主返航過程中對于靜態障礙物的避障；

3)在全覆蓋路徑測試中，返航路程相比出發路程節約了79.16%。

本次結果只是初步的，未能實現設計的全部功能，如超出識別范圍后基于里程計的偏航矯正等，因此還需要進一步的測試和改進。這項工作為后續工作提供了一些有趣的方向，如基于編碼器的航跡推算、優化激光雷達信息實現動態避障返航，以及使用自然特征而非人工標記等。