基于二維碼的相機位姿求解

吳 飛， 介遠航， 余神志

(武漢理工大學(xué) 機電工程學(xué)院， 湖北 武漢 430070)

二維碼因其信息容量大，編碼范圍廣，已被廣泛應(yīng)用于手機支付和信息查閱，極大方便了人們的生活.在定位技術(shù)領(lǐng)域，二維碼也得到了廣泛應(yīng)用[1].在定位技術(shù)中，二維碼可以很好地存儲絕對位置信息，且信息獲取簡單易用，為機器人里程計提供的后驗信息可用于修正機器人里程計數(shù)據(jù).現(xiàn)已廣泛應(yīng)用于智能物流、機器人巡檢等場景，是重要的定位方式之一[2].目前研究人員針對機器人定位問題提出諸多解決方案，例如，利用慣性傳感器構(gòu)建電子地圖完成定位[3]，利用二維碼實現(xiàn)機器人之間的相對定位[4].但慣性傳感器累計誤差較大，而傳統(tǒng)二維碼定位過程中，機器人相機識別二維碼的位姿通常是未知的，傳統(tǒng)二維碼定位技術(shù)為機器人提供的定位信息不能實現(xiàn)機器人的精準(zhǔn)定位.因此，在二維碼為機器人提供位置信息的基礎(chǔ)上，如果通過特征點匹配反解出相機位姿，那就可以為機器人提供更為精準(zhǔn)、全面的位置信息，包括機器人的平移量和旋轉(zhuǎn)量，進而實現(xiàn)機器人的精準(zhǔn)定位.機器人位姿求解示意圖如圖1所示.

圖1 機器人位姿求解示意圖

機器人在全局坐標(biāo)系下的旋轉(zhuǎn)矩陣R和平移向量t定義如下：

R=R1×R2,

t=t1+t2，

式中：R1和t1分別為相機坐標(biāo)系相對于二維碼坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量；R2和t2分別為機器人坐標(biāo)系相對于相機坐標(biāo)系的旋轉(zhuǎn)矩陣和平移向量.因此，利用二維碼進行相機位姿求解的方法，不僅對動態(tài)相機識別二維碼有重要的指導(dǎo)意義[5]，還可以為定位問題提供一定的技術(shù)支持.

1 標(biāo)定二維碼

筆者選取戴爾720p HD攝像頭，對地面粘貼的二維碼進行拍攝.為了保證計算效率，定義圖像分辨率為480×640像素，同時選取二維碼標(biāo)準(zhǔn)位置進行標(biāo)定，以便下一步進行特征點匹配和位姿求解[6].標(biāo)準(zhǔn)位置定義如下：二維碼中心像素位于圖像中央，二維碼邊長為100像素，如圖2所示.此時相機位姿定義為標(biāo)準(zhǔn)位姿，所對應(yīng)的旋轉(zhuǎn)矩陣為Rs，即三階單位矩陣E.用激光測距儀測量相機光心至二維碼中心距離為Zs=0.58 m，故定義平移向量ts=(0,0,0.58)，此位姿下的4個特征點定義為標(biāo)準(zhǔn)位姿特征點集Ps.

圖2 二維碼標(biāo)準(zhǔn)位置

2 標(biāo)定相機

無論是在三維重建還是在機器視覺應(yīng)用中，相機參數(shù)的標(biāo)定都極為重要，標(biāo)定結(jié)果的精度將直接影響獲得圖像數(shù)據(jù)的準(zhǔn)確性，所以做好相機標(biāo)定是進行圖像處理的基礎(chǔ)[7].相機標(biāo)定的本質(zhì)是獲得世界坐標(biāo)系下三維坐標(biāo)到圖像中像素點二維坐標(biāo)的轉(zhuǎn)化矩陣K，也稱為相機內(nèi)參矩陣.標(biāo)定相機就是求解相機內(nèi)參矩陣K中的未知量.K是一個三階矩陣，未知量有4個.K的定義如下：

式中：fx為像素在x方向的縮放量；fy為像素在y方向的縮放量；cx為像素在x方向的平移量；cy為像素在y方向的平移量.世界坐標(biāo)系到像素坐標(biāo)系的坐標(biāo)變換關(guān)系為

(1)

式中：Q為世界坐標(biāo)系到相機坐標(biāo)系的變換矩陣；u、v為像素在像素坐標(biāo)系中的x、y坐標(biāo)值；x0、y0和z0為像素在世界坐標(biāo)系中的x、y、z坐標(biāo)值.

筆者使用棋盤格和OpenCV內(nèi)置API實現(xiàn)相機標(biāo)定，棋盤格采用6個×8個角點，棋盤格邊長為24 mm，通過相機標(biāo)定可以得出內(nèi)參矩陣K.

3 圖像預(yù)處理

為了獲得特征點，需要對獲得的源圖像進行預(yù)處理，筆者首先利用OpenCV庫中的API算法接口對圖像進行預(yù)處理，最終獲得二維碼邊緣輪廓，以便下一步對邊緣輪廓進行特征點篩選.

3.1 初步預(yù)處理

初步預(yù)處理包括對圖像采用灰度轉(zhuǎn)化、圖像平滑和增強對比度.其目的如下：降低計算量[8]和顏色干擾；在保持良好邊緣的情況下，對噪聲進行消除和抑制；提高圖像對比度，使目標(biāo)區(qū)域更容易區(qū)分.初步預(yù)處理后的圖像如圖3所示.

圖3 初步預(yù)處理后的圖像

3.2 分組閾值分割

通過閾值分割可以使圖像只保留二維碼像素點.傳統(tǒng)閾值分割是反復(fù)嘗試，直至得到合適的閾值.但不同的拍攝角度、光照條件使得每個圖像的最佳閾值不同，此時若仍采用傳統(tǒng)閾值分割，需進行大量的嘗試.由于本研究中的研究對象均為單色背景的二維碼圖像，隨著閾值增大，二維碼像素將逐漸顯現(xiàn)，隨著閾值的繼續(xù)增大，最終趨于穩(wěn)定.故可將閾值進行分組，選擇方差最小的組為最佳組，再從最佳組中選擇最接近平均數(shù)的閾值為最佳閾值.算法步驟如下：

1) 將[0,255]閾值分為16組，每組16個閾值，i為組數(shù)，可取[0,15],j為序數(shù)，可取[0,15]，由此可知第i組的第j個閾值為15×i+j.

2) 計算每個閾值所對應(yīng)的二維碼像素個數(shù)，設(shè)第i組的第j個閾值的二維碼像素個數(shù)為Nij.

3) 設(shè)第i組的像素平均值為ni，方差為σi，計算公式為

(2)

(3)

4) 設(shè)最佳組的組數(shù)為m，得到

(4)

5)m組中距離像素平均值最小的閾值為最佳閾值.設(shè)最佳閾值為e,可得

(5)

綜上步驟可求出該圖像的最佳閾值為15m+e.分組閾值分割后得到的二值化圖像如圖4所示.

圖4 分組閾值分割后的二值化圖像

3.3 邊緣求解

在進行圖像邊緣提取之前，采用形態(tài)學(xué)閉運算，對二維碼內(nèi)部進行黑色填充，以免在邊緣檢測時得到較多無用的輪廓信息，增加算法的計算量.圖像邊緣就是灰度值發(fā)生劇烈變化的地方，采用Canny算子可以較好地得到二維碼圖像的邊緣輪廓[9].邊緣求解后的圖像如圖5所示.

圖5 邊緣求解后的圖像

4 特征點集求解方法

圖像預(yù)處理得到了二維碼邊緣輪廓.邊緣輪廓構(gòu)成的點集稱為邊緣點集合.可從邊緣點集合中通過循環(huán)遍歷等算法提取二維碼特征點.設(shè)4個二維碼特征點構(gòu)成的集合為待求位姿下特征點集T，用于與標(biāo)準(zhǔn)位姿下的特征點集Ps進行匹配，以求解位姿.

4.1 循環(huán)遍歷算法

傳統(tǒng)提取二維碼特征點的方法是采用霍夫直線檢測算法[10]，該算法的基本原理是首先檢測出二維碼的4條邊界直線，然后求解出這些直線的交點，即特征點集T.但是由于霍夫直線檢測算法需要對圖像進行離散化處理，離散化程度較大，會造成圖像失真、邊界點的坐標(biāo)誤差變大，進而導(dǎo)致所匹配的直線較多，不能精確地獲得4條邊界直線；若離散化程度較小，會得到較為密集的離散點，降低算法的效率.同時，由于霍夫直線的篩選是采用投票制，閾值過大或過小依然會導(dǎo)致所篩選出的直線偏少或偏多，如圖6所示.最佳投票閾值的參數(shù)設(shè)置需要大量試驗，故采用霍夫直線檢測算法去提取二維碼特征點的魯棒性較差.

圖6 霍夫直線篩選情況

可針對二維碼特征點提出循環(huán)遍歷算法，步驟如下：通過對像素點從上到下逐行遍歷，從左到右逐列遍歷，總能找到第1個特征點，定義為T1；同理，對像素點從下到上逐行遍歷，從右到左逐列遍歷，能找到第2個特征點，定義為T2. 同時，正方形的二維碼邊緣經(jīng)過透視變換后得到的圖形是凸四邊形.由幾何關(guān)系可知，通過循環(huán)遍歷算法識別出的2個特征點正好是原二維碼對角方向的一對特征點，如圖7所示.

圖7 特征點T1和T2位置示意圖

4.2 最大距離算法

通過循環(huán)遍歷算法，可以得到一對在對角方向的特征點，其坐標(biāo)分別為T1(x1,y1)和T2(x2,y2)，而剩余2個特征點坐標(biāo)為T3(x3,y3)和T4(x4,y4)，它們?yōu)榫嚯xT1和T2兩點間直線L的最遠點.根據(jù)已知2個特征點的坐標(biāo)分別為T1(x1,y1)和T2(x2,y2)，采用式(6)可得出通過這2個特征點的直線L的方程，即

(y2-y1)x-(x2-x1)y+x2y1-x1y2=0.

(6)

然后，對圖像邊緣點進行遍歷，設(shè)第i個邊緣點坐標(biāo)為Pi(Xi,Yi).遍歷到第i個邊緣點Pi時，點Pi到直線L的距離di定義如下：

(7)

若點Pi在L上方，則di>0；若點Pi在L下方，則di<0；若點Pi在L上，則di=0.

di=(y2-y1)Xi-(x2-x1)Yi+x2y1-x1y2.

(8)

由于特征點T3和T4距離直線L最遠，所以di的最大值dmax和最小值dmin所對應(yīng)的特征點即為T3和T4.至此，可得到二維碼的特征點T3和T4.特征點集T示意圖如圖8所示.

圖8 特征點集T示意圖

4.3 邊緣失真優(yōu)化算法

二維碼邊緣傾斜程度較大時，總能篩選出正確的特征點集T.但如果二維碼邊緣與圖像邊緣平行,且出現(xiàn)失真現(xiàn)象時，就會導(dǎo)致誤匹配，如圖9所示.失真現(xiàn)象是由于像素過低，導(dǎo)致二維碼邊界直線輪廓出現(xiàn)偏移.

圖9 失真現(xiàn)象示意圖

誤匹配現(xiàn)象如圖10所示.由于失真現(xiàn)象，通過循環(huán)遍歷算法會首先遍歷到T1的誤匹配位置，造成誤匹配.

圖10 誤匹配現(xiàn)象示意圖

對于邊緣失真問題，提出“像素突變”算法進行優(yōu)化，算法的基本原理是對于失真圖像進行逐行遍歷，得到每行的首個邊緣點，定義為行首邊緣點.圖11為行首邊緣點集和局部視圖.行首邊緣點構(gòu)成的集合定義為F，從上到下的第i個行首邊緣點定義為Fi，通過“像素突變”算法，首先得到5個行首邊緣點F1、F2、F3、F4和F5，如圖11a所示.為方便觀察，每個行首邊緣點均配有局部視圖，如圖11b-f所示.經(jīng)過預(yù)處理后的圖像是二值化圖像，即只有白色像素和黑色像素，邊緣輪廓為白色像素，像素值為255，設(shè)(rowi,coli)為Fi在像素坐標(biāo)系下的坐標(biāo)，M[x][y]表示在像素坐標(biāo)系下坐標(biāo)為(x,y)的像素值.因此，F(xiàn)i的坐標(biāo)表示為(rowi,coli)，M[rowi][coli]表示Fi的像素值.通過局部視圖可以看出，行首邊緣點Fi的像素值M[rowi][coli]=255(白色).求解特征點的方法如下：判斷下一個點像素值M[rowi][coli+1]是否突變?yōu)?(黑色)，若突變?yōu)?，特征點即為上一個行首邊緣點，若不突變?yōu)?，則繼續(xù)遍歷行首邊緣點，直至突變情況的出現(xiàn).如圖11所示，F(xiàn)2、F3、F4和F5的下一個像素點均沒有發(fā)生突變，行首邊緣點F1的下一個像素突變?yōu)楹谏?，故可得特征點為點F2.

圖11 行首邊緣點集和局部視圖

通過“像素突變”算法的優(yōu)化，利用循環(huán)遍歷算法與最大距離算法，均能夠正確找到特征點，以證明算法的準(zhǔn)確性和可行性.圖12為特征點求解試驗中特征點提取前后對比.圖12a是8次隨機拍攝的圖像進行的特征點提取，試驗結(jié)果圖12b所示，均能正確篩選出特征點.

圖12 特征點集求解試驗

5 位姿求解與修正

通過以上算法可以得到二維碼的4個特征點T1(x1,y1)、T2(x2,y2)、T3(x3,y3)和T4(x4,y4).由于特征點均位于同一個平面上，利用特征點的單應(yīng)性[11]，對4個特征點進行位姿求解.平面方程定義如下：

(x,y,z)nT+d=0.

式中：n為平面方程的法向量；d為平面方程的常數(shù)項.

根據(jù)式(1),進行坐標(biāo)變換得到

T=K(RPs+t).

代入平面方程與特征點，可得

(9)

式中：R為標(biāo)準(zhǔn)位姿到當(dāng)前位姿的旋轉(zhuǎn)矩陣；t為標(biāo)準(zhǔn)位姿到當(dāng)前位姿的平移向量.

T=HPs.

(10)

代入求得的4個特征點，與標(biāo)準(zhǔn)位姿特征點集Ps進行匹配[12]，可求解出當(dāng)前相機位姿相對于標(biāo)準(zhǔn)位姿的旋轉(zhuǎn)矩陣R和平移向量t.求解位姿后，通過透視變換，可將非標(biāo)準(zhǔn)位姿二維碼進行矯正.與標(biāo)準(zhǔn)位姿二維碼對比，若定位標(biāo)志位置相同，則不必對旋轉(zhuǎn)矩陣R進行修正.若不同，則需對旋轉(zhuǎn)矩陣R進行修正.修正步驟如下：

1) 圖13為區(qū)域定義示意圖.圖13中，標(biāo)準(zhǔn)位姿二維碼的定位標(biāo)志在區(qū)域1、2、3處.設(shè)i區(qū)域存在的黑色像素個數(shù)為bi，對于標(biāo)準(zhǔn)位姿二維碼，min(bi)=b4.

圖13 區(qū)域定義示意圖

2) 對于矯正后的二維碼：

若min(bi)=b1，則矯正角angle=180°；

若min(bi)=b2，則矯正角angle=90°；

若min(bi)=b3，則矯正角angle=270°；

若min(bi)=b4，則矯正角angle=0°.

修正矩陣Rf定義如下：

Rf=RangleR，

(11)

式中：Rangle為旋轉(zhuǎn)角度為angle時的旋轉(zhuǎn)矩陣.

修正平移向量tf定義如下：

tf=t+ts.

(12)

通過修正計算可以得出修正矩陣Rf和修正平移向量tf，通過Rf求解旋轉(zhuǎn)軸A和旋轉(zhuǎn)角度θ，其計算公式如下：

A=RfA,

(13)

(14)

在旋轉(zhuǎn)上，當(dāng)前相機位姿相對于二維碼中心的旋轉(zhuǎn)軸為A，旋轉(zhuǎn)角度為θ；在平移上，當(dāng)前相機位姿相對于二維碼中心的平移向量為tf.

6 誤差分析

為證明算法的準(zhǔn)確、有效，拍攝50張Rf與tf已知的圖像進行位姿求解，現(xiàn)對求解結(jié)果的誤差進行分析.誤差在x、y和z方向均有分布，定義總誤差為在x、y和z方向上誤差的內(nèi)積，總誤差分布曲線如圖14所示.

圖14 總誤差分布曲線

由圖14可知：旋轉(zhuǎn)軸總誤差約為±0.18 m；旋轉(zhuǎn)角誤差約為±2°；平移量總誤差約為±0.02 m.故該算法能夠為機器人提供較準(zhǔn)確的位姿信息，包括機器人相對于世界坐標(biāo)系的旋轉(zhuǎn)軸、旋轉(zhuǎn)角和平移量，可以幫助其實現(xiàn)精準(zhǔn)定位.

7 結(jié) 論

1) 提出一種針對二維碼的分組閾值分割算法，與傳統(tǒng)閾值分割算法相比，該算法能自動求解適合提取二維碼區(qū)域的閾值，針對不同光照、不同角度拍攝得到的二維碼圖像，均能有效地完成二維碼區(qū)域的二值化.

2) 提出運用循環(huán)遍歷算法和最大距離算法求解特征點集，并采用像素突變優(yōu)化出現(xiàn)的失真現(xiàn)象，通過多次試驗驗證了特征點集求解方法的可行性和有效性.

3) 提出基于二維碼的相機位姿求解算法，通過對相機和二維碼進行標(biāo)定、圖像預(yù)處理、求解特征點集、位姿求解與修正，完成相機位姿的求解.測試數(shù)據(jù)集中的誤差表明，相較于傳統(tǒng)的定位算法，提出的基于二維碼的相機位姿求解算法可以為機器人提供準(zhǔn)確的定位信息，包括機器人的旋轉(zhuǎn)量和位移量，實現(xiàn)對機器人的精準(zhǔn)定位.