基于改進ArUco標簽的位姿估計方法

段志瑜, 羅 哉, 江文松, 楊 力, 李亞茹, 卞 點

(1.中國計量大學 計量測試工程學院，浙江 杭州 310018；2.中國計量大學 信息工程學院，浙江 杭州 310018)

1 引 言

單目視覺因成本低、標定簡單、自動化程度高等優勢而備受關注,在諸如機器人導航[1]、地圖構建[2]、定位跟蹤[3,4]和目標檢測[5,6]等場景有著廣泛的應用價值。

盡管使用關鍵點和紋理特征進行位姿估計的方法非常流行,但基準標簽的使用仍然很重要,因為它在提供點時更具有效性和精確性。例如,Garrido等[7]提出的ArUco標簽兼具良好的定位跟蹤功能,在虛擬現實、目標跟蹤和無人機降落等領域應用廣泛;Marut等[8]使用標簽作為參考點,通過姿態估計算法高精度地確定無人機著陸期間的飛行高度;Silva等[9]建立了ArUco標簽陣列,并用來進行相機標定;Hu等[10]用標簽輔助AGV小車進行室內定位,提高了定位精度;邢伯陽等[11]利用ArUco標簽陣列實現單目實時定位建圖。但這些文獻僅將ArUco標簽應用到不同的領域,而定位性能的提高還需對ArUco標簽的性能進行研究。

針對ArUco標簽問題,Kam等[12]引入線性卡爾曼濾波器來提高標簽檢測的魯棒性;Oscadal等[13]將一組ArUco標簽以一定角度放置形成3D ArUco板,開發了一個庫組合各個標簽的姿勢數據,提高了測量的穩定性;Ferrao等[14]通過利用四邊形內角和猜想,實現了檢測距離的提升,但只能保持相似的姿態估計精度。針對遠距離測量問題,丁浩峰等[15]設計了基于ArUco的多層級組合新型合作標識,通過融合不同層級標簽測量結果實現大距離跨度范圍內無人機的高精度位姿測量,但僅進行縮比仿真實驗,實際實驗結果未知;陳曼桐等[16]則使用不同的標簽組分別承擔距離測量和方位角測量任務,并將長距離分解成多段短距離進行測量,通過數據融合的方式結合直接和間接測量的優勢,實現測量準確度的提升,但中轉標簽數量的增加,會顯著增加對目標標簽測量時產生的誤差;何宇[17]采用雙目視覺系統,根據視差得到深度信息,從而高精度地進行測量,但是需要保證標簽出現在兩相機的重疊區域,限制了視野范圍;張文等[18]設計ArUco標簽角點重投影和IMU聯合優化的方法來修正位姿結果,雖然提高了位姿測量精度,但是高精度IMU成本較高。在對遠距離ArUco標簽進行測量時,標簽角點像素坐標的檢測極易出現偏差,會導致位姿測量結果的跳躍。因此,ArUco標簽在遠距離下難以提供準確的位姿信息。

本文針對遠距離ArUco標簽測量精度不高的問題,提出一種改進ArUco標簽的位姿估計方法。在ArUco標簽對角線上增加了4個呈正方形分布的陣列特征圓環,因為圓心定位更加穩定,且角點圓心共線約束可以修正角點像素坐標,進而提高位姿測量精度。

2 改進ArUco標簽位姿測量原理

為了在遠距離下獲得準確的空間位姿信息,對ArUco標簽進行改進。圓形標識點在識別和中心定位精度具有較好的穩定性,運用范圍廣泛,但圓形特征單一,而同心圓環易于分離,且當圓環內外半徑比為1:2時,中心定位精度較好[19]。因此將圓環標識放置在ArUco標簽對角線的延長線上。一方面,角點和圓心在一條直線上,根據共線的特點約束角點檢測的穩定性;另一方面,保證4個特征圓環和ArUco標簽的旋轉不變,從而在識別角點和圓心后正確分配像素點。設圓環外半徑為R,內半徑為r,且內外半徑比為1:2,相鄰圓環的圓心距為l,ArUco標簽邊長為L,改進標簽及其角點和圓心的成像,如圖1所示。

圖1 改進ArUco標簽及其成像示意圖Fig.1 Improved ArUco tag and their imaging schematics

為了更好地說明改進標簽的識別定位方法在位姿測量中的應用,其具體流程為:

1) 角點檢測。首先,對圖像進行預處理,使用模板為5×5的高斯濾波去噪,根據相機的內參進行圖像去畸變;其次,使用局部自適應閾值分割標簽并提取外形輪廓,對輪廓進行四邊形逼近,得到近似于ArUco標簽的候選框;通過四邊形逼近得到的4個角點坐標獲得透視變換關系,將圖像中的標簽透視投影到正方形中;用Otsu算法進行閾值化,分離出白色位和黑色位,得到二進制編碼信息,并與字典庫進行匹配;匹配成功后,確定標簽的編號和正確朝向;最后,對標簽四邊邊緣像素進行線性回歸,獲得亞像素級角點像素坐標。

2) 橢圓中心提取。圓環位于ArUco標簽對角線的延長線上,因此改進前后的標簽大小存在比例關系,比例k為

(1)

理論上,平行投影前后比例k通常保持不變,這里將k的值進行一定程度的擴大,取k=k+0.5,將步驟1)中識別的ArUco標簽區域擴大以形成新候選框,確保整個改進后的標簽在該候選框內。在該候選框內檢測圓環,圓環在平行投影下的成像為2個橢圓,采用基于弧段檢測[20]的方法檢測橢圓,獲得橢圓集合,遍歷所有橢圓對,設2個橢圓的中心像素坐標分別為(xm,ym)、(xn,yn),長短半軸分別為Am、Bm、An、Bn,合格橢圓對需要滿足以下約束:

約束1:圓環的內外圓為同心圓,對合格橢圓對的中心像素坐標進行歐式距離約束

(2)

式中:ε1為距離閾值,一般設定ε1=2。

約束2:根據圓環的內外圓面積比h進行篩選

h=Sin/Sout

(3)

式中:Sout為圓環外圓面積;Sin為圓環內圓面積。Sout=πAmBm。若h∈(0.2,0.3),則認定為合格橢圓對。

3) 排序。由于第1)步檢測出的角點像素坐標按照圖1中abcd的順序保存,而圓環中心像素坐標無序,因此,需要進行排序,使角點和圓環中心的像素坐標按照圖1中efgh順序排列。以4個中心像素點坐標輸入,依次遍歷2個點,設e′、g′連線構造直線方程

mx+ny+q=0

(4)

式中:m、n、q為常數。通過ArUco的左上角點a′坐標判斷其是否在直線e′g′上,由于存在投影誤差,計算點a′到該直線的距離

(5)

式中:(Ua′,Va′)為點a′的像素坐標。設定距離約束范圍ε2≤1.5,若滿足該條件,且滿足向量點乘e′a′·g′a′<0,則認為2點為e′和g′,否則繼續遍歷,從而確定點e′和g′;此外,點c位于點a和g之間,投影后位置關系保持不變,故e′需要滿足條件a′c′·a′e′<0,g′則需滿足條件a′c′·a′g′>0。由此可確定出點e′的像素坐標,以此像素坐標為起點進行排序。利用向量叉乘的性質對剩余3個圓環中心像素坐標按照順時針排序,設2個二維向量x=[i1,j1]T,y=[i2,j2]T,有

x×y=[i1j1]T×[i2j2]T=i1j2-i2j1

(6)

通過向量e′f′和e′g′叉乘積正負判斷點的順序,若為正,則向量e′f′和e′g′為正角度,從e′f′轉到e′g′是逆時針,如圖2(a)所示,此時需要調換f′、h′兩點;若為負,從e′f′轉到e′g′是順時針,4點坐標呈順時針排列,如圖2(b)所示。

圖2 橢圓中心排序原理Fig.2 Center of ellipse ordering principle

4) 交比不變性。計算機視覺中射影變換,其性質包括共線的點滿足交比不變性,結合改進標簽,a,c,e,g和b,d,f,h4點分別共線,投影前后交比值分別記為

(7)

(8)

式中:|ec|、|ag|、|ac|、|eg|、|fd|、|bh|、|bd|、|fh|、|e′c′|、|a′g′|、|a′c′|、|e′g′|、|f′d′|、|b′h′|、|b′d′|、|f′h′|分別為對應兩點之間向量的模。根據射影定理的性質可得

cross(a,c,e,g)=cross(a′,c′,e′,g′)

=cross(b,d,f,h)

=cross(b′,d′,f′,h′)

(9)

由于像素點的檢測存在偏差,投影后計算出的交比值一般在真實值處浮動,設定交比誤差ε3≤0.5,符合該條件則認為滿足交比不變,依次記錄4個圓環中心像素坐標,否則圓環中心像素坐標檢測存在誤差,返回步驟1)重新檢測。

5) 修正角點。若滿足交比約束,則對4個點a,c,e,g的像素坐標進行加權最小二乘直線擬合[21],其與四邊形的兩條鄰邊的交點情況如圖3所示,若3條直線相交于一點,如圖3(a)所示,將此點作為點a′的像素坐標;若3條線不交于一點,如圖3(b)所示,將三角形的內切圓的圓心作為點a′的像素坐標。同樣,對剩下三個角點像素坐標也進行校正。

圖3 角點修正方法Fig.3 Corner correction method

6) 位姿估計。獲得校正后的ArUco標簽角點后,根據4組2D/3D對應關系,使用EPnP+LM算法[22]進行位姿解算優化,通過最小化重投影誤差求得相對位姿。改進標簽的識別定位方法的流程,如圖4所示。

圖4 改進ArUco識別定位流程Fig.4 Improve ArUco identification and positioning process

3 實驗結果與分析

3.1 實驗系統設計

本文搭建了針對改進ArUco標簽的位姿測量系統,以機械臂為位姿精度測量平臺,該機械臂的重復性平移精度為0.03 mm,旋轉精度為0.003°,可通過控制平臺對6個軸進行移動,改進ArUco標簽張貼于機械臂末端,由機械臂帶動轉換位姿。實驗使用的相機分辨率xres×yres為2 048×2 048,鏡頭焦距可調,實驗前固定相機焦距。

3.2 實驗數據分析

3.2.1 重復定位精度分析

分別在小傾角和大傾角兩種情況下,相機距離標簽2.5 m至4 m范圍內進行實驗。根據相機標定結果和相機視場計算式(10),計算此范圍內相機的視場大小,至少保證在最遠距離測量時,改進ArUco標簽所占的視場比例不低于10%,可知改進標簽的外接正方形的邊長至少為12.7 cm,為了在2.5 m處也能能充分測試標簽的性能,改進ArUco標簽的尺寸設置為:ArUco的實際長度為10 cm,圓環外半徑為2 cm,內半徑為1 cm,相鄰圓環的中心距為15 cm。

(10)

式中:dx和dy分別為水平和垂直方向像元尺寸;D為標簽中心距離相機光心的距離;f為相機焦距。

小傾角實驗時,相機固定不動,調整標簽平面近似垂直相機光軸并距離相機光軸2.5 m處,以機械臂帶動標簽在其光軸方向移動,每次移動10 cm,至4 m處,在16處位置拍攝圖像,每個位置拍攝150幀。兩組實驗圖片如圖5所示。

圖5 2.5 m和4 m處小傾角和大傾角實驗圖Fig.5 Experimental diagrams of small and large dip angles at 2.5 m and 4 m

(11)

因此,以每組圖片中的第1幀圖像為初始姿態,按照式(11)求得其它幀的相對位姿,實際六自由度的真實值均為零,計算每個自由度與零的差值,取150幀圖像差值的平均值,得到每個位置六自由度重復定位的誤差。分別計算改進前后標簽的重復定位誤差,實驗結果如圖6所示。

圖6 小傾角六自由度重復定位誤差Fig.6 Small angle six degrees of freedom repeated positioning error

ArUco標簽在tx,ty,tz位置上的最大平均誤差分別為1.25,2.19,16.32 mm,rx,ry,rz最大平均誤差分別為3.89°,2.28°和0.27°,改進標簽在tx,ty,tz位置上的最大平均誤差分別為0.41,0.44,4.16 mm,rx,ry,rz最大平均誤差分別為1.64°,1.47°和0.11°。此外,隨著距離的增加,改進前后標簽的重復定位誤差均呈現出增大的趨勢,但ArUco標簽的重復定位誤差在個別地方出現較大的異常值,重復定位效果不如改進標簽。

大傾角實驗時,調整標簽平面與相機光軸夾角為近似50°,同樣測量不同距離下改進前后標簽的重復定位誤差,實驗和數據處理過程與小傾角實驗相同,實驗結果如圖7所示。ArUco標簽在tx,ty,tz位置上的最大平均誤差分別為36.43,18.84,47.38 mm,rx,ry,rz最大平均誤差分別為3.64°,2.11°和1.54°,改進標簽在tx,ty,tz位置上的最大平均誤差分別為15.21,11.43,17.88 mm,rx,ry,rz最大平均誤差分別為1.37°,1.32°和1.01°。與小傾角的重復定位精度相比,ArUco標簽在大傾角的重復定位精度明顯下降,改進標簽在重復定位精度方面更加突出。

圖7 大傾角六自由度重復定位誤差Fig.7 Large angle six degrees of freedom repeated positioning error

3.2.2 旋轉平移精度分析

調整標簽平面與相機相距2.5 m并垂直于相機光軸,拍攝10幀圖像,以重復測量結果的均值作為初始位姿,此時姿態記為rx=0°,隨后用機械臂帶動標簽繞標簽坐標系的x軸旋轉10°,共6次至rx=60°,每個位置拍攝10幀圖像。計算每個位置上10幀圖像相對rx=0°時的rx角度的平均值,rx的真值依次為10°至60°,以平均值與真實值的差值作為誤差。同樣,將標簽繞標簽坐標系的y軸和z軸旋轉,每次間隔10°拍攝10幀圖像,計算ry和rz的誤差。分別在3,3.5,4 m處的位置,按照上述方法計算改進前后標簽的旋轉測量誤差,實驗結果如圖8所示,rx=0°處的誤差以其重復定位誤差來計算。

圖8 不同距離各軸的旋轉精度Fig.8 Rotation accuracy of each axis at different distances

圖8給出了改進前后標簽在不同距離下各軸的旋轉誤差,可見,在2.5～4 m測量范圍內,改進前rx和ry的誤差小于4.65°,rz的誤差小于1.47°,改進后rx和ry的誤差均小于1.78°,rz的誤差小于0.8°。

針對沿相機坐標系的x軸的平移精度,實驗方案與旋轉精度的測量方案類似,同樣在2.5 m處調整好標簽平面與相機光軸垂直,拍攝10幀圖像,此時姿態記為tx=0,以機械臂帶動標簽在左右方向上平移,間隔50 mm平移1次,各移動至tx=-200 mm與tx=+200 mm。取tx=0處10次測量結果的平均值作為初始位姿,計算每個位置的10幀圖像相對初始位置tx=0的相對位姿平均值,然后將平均值與真實值的差值絕對值作為tx的誤差。分別在3,3.5,4 m處對改進前后標簽的平移誤差進行測量,其中3.5 m和4 m處測量范圍為tx=-400 mm至tx=+400 mm,測量步長為100 mm。

同樣,以機械臂帶動標簽在上下方向平移,計算ty誤差。實驗結果如圖9所示,在2.5～4 m測量范圍內,改進前tx誤差小于14.69 mm,改進后tx誤差則小于4.86 mm;改進前ty誤差小于13.95 mm,改進后ty誤差則小于4.52 mm。

圖9 不同距離的平移精度Fig.9 Translation accuracy at different distances

針對沿相機坐標系的z軸的平移精度,同樣在2.5 m處拍攝10幀圖像,用機械臂帶動標簽遠離相機方向移動至tz=4 m處,測量步長設置為10 cm,真值依次為10,20至150 cm,得到tz誤差,實驗結果如圖10所示。改進前tz誤差小于36.32 mm,改進后tz誤差小于8.72 mm;且改進標簽在4 m處相對測量誤差為0.22%,較改進前的相對測量誤差降低了0.69%。

圖10 沿Z軸的平移精度Fig.10 Translation accuracy along the Z axis

4 結 論

ArUco標簽在遠距離的位姿估計精度不高,本文在ArUco標簽的對角線上增加特征圓環,結合圓心和角點共線的特點,進行直線擬合,對角點像素坐標進行修正。采用EPnP+LM算法對3D-2D點進行位姿解算優化,通過最小化重投影誤差求得相機位姿。

2組實驗得出以下結論:

1)通過在小傾角和大傾角2種情況對改進前后的標簽進行重復性實驗,證明改進后的標簽的重復定位精度更高。

2)提出的改進ArUco標簽在2.5～4 m的距離下進行位姿測量,旋轉誤差不大于1.78°,平移誤差不大于8.72 mm,提高了測量精度。