摘"要"6D位姿是目標(biāo)物體基于平面矢量和旋轉(zhuǎn)矢量的位置和姿態(tài). 基于單目RGB相機(jī)進(jìn)行6D位姿估計的傳統(tǒng)方法由于標(biāo)記點少、識別率較低等原因，易導(dǎo)致測量精度不高. 為提高物體位姿測量的精度，論文提出了一種基于特征標(biāo)記的物體6D位姿測量方法. 首先，設(shè)計一種能提供更多標(biāo)記點且識別率高的特征標(biāo)記，并通過相機(jī)識別被測物體上的特征標(biāo)記獲得3D-2D點對. 其次，基于相機(jī)成像原理設(shè)計物體6D位姿測量系統(tǒng)與方法. 然后，建立求解工件位姿變化的旋轉(zhuǎn)矩陣和位移矩陣模型. 最后，基于所建立的旋轉(zhuǎn)矩陣模型與歐拉角之間的關(guān)系求解工件6D位姿. 實驗結(jié)果表明，在0-170 mm位移范圍內(nèi)，沿X、Y、Z軸位移測量誤差小于0.5 mm;在0-30°旋轉(zhuǎn)范圍內(nèi)，其繞X、Y軸旋轉(zhuǎn)角度測量誤差小于0.8°;在0-45°旋轉(zhuǎn)范圍內(nèi)，其繞Z軸旋轉(zhuǎn)角度測量誤差小于0.8°;測量物體繞X、Y軸旋轉(zhuǎn)角度誤差耗時小于1.5 s，繞Z軸旋轉(zhuǎn)角度誤差耗時小于2 s;測量物體沿X、Z軸位移誤差耗時小于2 s，沿Z軸位移誤差耗時小于2.5 s;能夠滿足實際生產(chǎn)要求. 實驗結(jié)果檢驗了論文所提方法的有效性，可用于實際生產(chǎn)環(huán)境中.

關(guān)鍵詞"機(jī)器視覺;6D位姿;位姿估計;單目RGB相機(jī);PnP

中圖分類號"TP391.4""文獻(xiàn)標(biāo)識碼"A

0"引"言

位姿測量可提高工件的生產(chǎn)質(zhì)量及其工作效率，如實時監(jiān)測軌道交通工程中路基的結(jié)構(gòu)變形（位移）以確保列車的安全運(yùn)行[1]、在缺陷檢測中通過確定超聲波探頭的具體位置從而精準(zhǔn)定位缺陷位置[2]. 位姿測量即在知道內(nèi)部參數(shù)的前提下求解其外部參數(shù)，其核心是通過n個空間點（3D點）和相對應(yīng)的圖像點（2D點）確定被測物體在空間中的位姿，亦被稱為透視n點問題（Perspective-n-Points，PnP）. Fischler等首次提出了PnP問題，并證明了至少需要3組對應(yīng)的3D-2D點才能解出該問題[3].

自PnP問題被提出以來，PnP算法及其改進(jìn)算法在圖像視覺測量、機(jī)器人定位、增強(qiáng)現(xiàn)實、SLAM（Simultaneous Localization and Mapping）等領(lǐng)域已獲得廣泛研究和應(yīng)用[4]. 在單目視覺法中，通常以固定在工件上的標(biāo)志物為參考平面建立世界坐標(biāo)系，通過求解PnP問題獲得相機(jī)與標(biāo)志物之間的空間位置關(guān)系. 由此可見，工件6D位姿求解精度在很大程度上受限于標(biāo)志物的定位和匹配精度[5]. 目前物體6D位姿估計有非基準(zhǔn)標(biāo)記和基準(zhǔn)標(biāo)記兩種方法，其中非基準(zhǔn)標(biāo)記法通常是指由數(shù)據(jù)集提供場景中物體的6D位姿標(biāo)簽[6]并通過算法設(shè)計對物體位姿進(jìn)行估計，該方法的物體6D位姿估計的準(zhǔn)確度很大程度上依賴于算法的可靠性，且對一些較小目標(biāo)的測量誤差較大. 而基準(zhǔn)標(biāo)記法常用ArUco標(biāo)記系統(tǒng)[7]，該系統(tǒng)檢測速度快，且具有較強(qiáng)的魯棒性;若將基準(zhǔn)標(biāo)記與待測物體進(jìn)行剛性連接，即可對待測物體進(jìn)行定位和位姿估計. 但是，當(dāng)實際場景中ArUco標(biāo)記尺寸較小時，由于存在標(biāo)記邊界模糊、可用于計算的3D-2D點數(shù)目較少等問題，物體6D位姿估計的精確度仍待提高.

PnP問題計算速度快、實時性好，但是對含有噪聲的圖像比較敏感，缺乏穩(wěn)定性. 許多人在此基礎(chǔ)上對PnP算法進(jìn)行了改進(jìn). 2009年，許允喜等[8]將目標(biāo)空間線段誤差納入正交迭代算法，使用點和線段特征來計算誤差函數(shù). 與OI算法相比，該算法具有更強(qiáng)的抗噪聲性能;同年，許允喜等[9]提出了一種用于多攝像機(jī)系統(tǒng)姿態(tài)估計的廣義正交迭代方法. 該方法統(tǒng)一表示所有攝像機(jī)數(shù)據(jù)，并將所有特征點的空間共線性誤差作為目標(biāo)函數(shù);2012年，苗錫奎等[10]通過建模特征點并使用Levenberg算法優(yōu)化目標(biāo)函數(shù)，提高了迭代精度;2015年，李鑫等[11]提出了加速前向迭代方法，通過轉(zhuǎn)換平移向量，減少了迭代過程中的計算負(fù)載，加快了迭代速度;2018年，周潤等[12]提出了一種加權(quán)前向重疊方法，使用權(quán)重測量測量點的可信度，提高了迭代精度. 2019年，張慧娟等[13]提出一種基于P4P矩形分布的平面標(biāo)靶和EPNP算法結(jié)合的單目視覺姿態(tài)測量方法，提高了測量精度. 2022年，楊志偉等[14]提出一種基于雙高速攝像機(jī)交匯的測量方法，進(jìn)一步地提高了PNP算法的測量精度. 然而，前人大多是基于算法上的改進(jìn)，在面對用于計算3D-2D點對數(shù)目不足的情況，如何提升特征標(biāo)記的識別精度仍是一個具有挑戰(zhàn)性問題.

針對當(dāng)前特征標(biāo)記的識別精度低且用于計算的3D-2D點數(shù)目少等問題，本論文基于棋盤格提出一種物體6D位姿測量方法，如圖1所示是該棋盤格特征標(biāo)記示意圖，該標(biāo)記共有56個待測內(nèi)角點，在不確定擾動工況下亦可確保PnP算法的精度，進(jìn)而準(zhǔn)確測量出工件的6D位姿. 實驗表明，該方法的測量精度得到了明顯的改進(jìn).

1"物體6D位姿匹配算法分析

為快速定位和匹配內(nèi)角點，論文采用OpenCV庫中的棋盤格內(nèi)角點檢測算法和亞像素精細(xì)化算法獲取56個內(nèi)角點的二維像素坐標(biāo). 然后根據(jù)相機(jī)成像原理建立成像模型，如圖2所示，并規(guī)定成像平面、物平面、相機(jī)坐標(biāo)系、圖像坐標(biāo)系、像素坐標(biāo)系和世界坐標(biāo)系等.

其中OC-XCYC ZC為相機(jī)坐標(biāo)系，Ouv-uv為像素坐標(biāo)系，Oxy-xy為圖像坐標(biāo)系，OW-XWYW ZW為世界坐標(biāo)系. 點P（XCYC ZC）為空間中任意3D點，其所在的平面為物平面. 將物平面上的任意3D點P歸一化到成像平面后得到2D像素點p（x，y）.

由于相機(jī)坐標(biāo)系到圖像坐標(biāo)系之間是透視關(guān)系，則其歸一化過程可轉(zhuǎn)換為如下的三角形相似關(guān)系：

式中：f代表焦距，即相機(jī)坐標(biāo)系和圖像坐標(biāo)系在Z軸上的差.

在此基礎(chǔ)上，可將整個相機(jī)坐標(biāo)系與圖像坐標(biāo)系之間的轉(zhuǎn)換關(guān)系和歸一化過程轉(zhuǎn)換為如下齊次坐標(biāo)形式：

其中：像素坐標(biāo)系定義于成像平面的左上角，其單位是像素（pixel）. 而圖像坐標(biāo)系的原點Oxy即為相機(jī)光軸與成像平面的交點，通常位于成像平面的中點Oxy（u0，v0），其單位通常為毫米（mm）. 為探索上述兩種坐標(biāo)系之間的映射關(guān)系，論文將引入未知量dx與dy，其物理意義為每一列或每一行像素點代表的毫米量[15]. 即1 pixel=dx mm或1 pixel=dy mm.

由此，兩坐標(biāo)系之間的映射關(guān)系可表示為：

為進(jìn)一步規(guī)范化計算過程，將式（3）用齊次坐標(biāo)表示為：

若將（4）式代入（2）式，可得：

由于世界坐標(biāo)系和相機(jī)坐標(biāo)系之間是剛體變換，其映射關(guān)系主要因外參：旋轉(zhuǎn)矩陣R和位移矩陣t決定[16]. 該過程可表示為：

若將式（6）代入式（5），可得：

若將式（7）寫成方程組的形式，有：

ZCu=q11XW+q12YW+q13ZW+q14" " ①

ZCv=q21XW+q22YW+q23ZW+q24" " ②

ZC=q31XW+q32YW+q33ZW+q34" " " ③

將③式代入①式和②式中，可得：

q11XW+q12YW+q13ZW+q14-q31XWu-q32YWu-q33ZWu-q34u=0

q21XW+q22YW+q23ZW+q24- q31XWv- q32YWv- q33ZWv-q34v=0

由于每組3D-2D匹配點對應(yīng)兩個方程，一共有12個未知數(shù)，所以至少需要已知6組匹配的3D-2D點對方可求解[17]. 假設(shè)共有N組匹配點，則有：

將上式矩陣方程簡化為：

AF=0（9）

式中，矩陣F為未知，共計12個未知參數(shù)，矩陣A取決于3D-2D點對數(shù)量. 此時存在兩種情況[18]：

①當(dāng)N=6時，可直接求解該線性方程組.

②當(dāng)Ngt;6時，此式是一個超定方程，無精確解. 需在約束

F=1的條件下求該方程的最小二乘解. 進(jìn)而可確定此超定方程的解即為對A進(jìn)行SVD分解后的最小特征值對應(yīng)的特征向量[19].

上述分析表明特征標(biāo)記共有56組待檢測的3D-2D點對，符合第二種情況求解其旋轉(zhuǎn)矩陣R與位移矩陣t.

2"位姿測量系統(tǒng)的建立

如圖3所示的位姿測量系統(tǒng)，其檢測過程由視頻流輸入、尋找特征角點、匹配特征點和計算6D位姿四個部分組成.

通過單目RGB攝像頭采集含有特征標(biāo)記的圖像，并根據(jù)物體第一幀6D位姿建立其世界坐標(biāo)系OW-XWYW ZW，實現(xiàn)被測物體6D位姿測量.

由于在位姿變化的過程中，相機(jī)的位姿始終固定，因此，相機(jī)與特征標(biāo)記之間的位姿關(guān)系可表示為：

R0·VW+t0=VC=R1·VP+t1

式中R0、t0和R1、t1分別為初始時刻和第二時刻特征標(biāo)記到單目RGB相機(jī)之間的旋轉(zhuǎn)矩陣和位移矩陣. VW、VP、VC分別為初始時刻、第二時刻和單目RGB相機(jī)的位姿向量，其中相機(jī)的位姿向量VC等價于向量OCVC [20].

由于世界坐標(biāo)系建立在初始位姿的特征標(biāo)記上，所以假設(shè)位姿向量VW=[0" "0" "0]T，則有：

此時xP、yP、zP分別為第二時刻物體沿世界坐標(biāo)系X軸、Y軸、Z軸的位移距離. 至此，物體沿世界坐標(biāo)系X、Y、Z軸的三個方向的位移均可通過（10）式解出. 接下來需通過旋轉(zhuǎn)矩陣R來求出物體沿世界坐標(biāo)系X、Y、Z軸的旋轉(zhuǎn)角度.

如圖4所示，若被測物體初始位姿與相機(jī)位姿之間的旋轉(zhuǎn)角度用旋轉(zhuǎn)矩陣R1表示，被測物體第二時刻位姿與相機(jī)位姿之間的旋轉(zhuǎn)角度用旋轉(zhuǎn)矩陣R2表示，且其初始位姿與第二時刻位姿之間的旋轉(zhuǎn)角度用旋轉(zhuǎn)矩陣R01表示.

根據(jù)旋轉(zhuǎn)矩陣特性可得：

R01=R-1

1" " ·R0

結(jié)合旋轉(zhuǎn)矩陣與歐拉角之間的關(guān)系[21]，若假設(shè)：

R01=r11" r12" r13

r21" r22" r22

r31" r32" r33

則第二時刻被測物體繞世界坐標(biāo)系OW-XWYW ZW的Z軸的旋轉(zhuǎn)角為：

θz=atan2（r21，r11）

其繞Y軸的旋轉(zhuǎn)角為：

θy=atan2-r31，

而繞X軸的旋轉(zhuǎn)角為：

θx=atan2（r32，r33）

至此，xP、yP、zP、θx、θy、θz即為被測物體的6D位姿. 在實際生產(chǎn)線中，利用上述得出的數(shù)據(jù)即可找出流水線上存在高度有缺陷或者是姿態(tài)不正確的工件. 如圖5所示是物體6D位姿檢測系統(tǒng)示意圖.

3"實驗結(jié)果及分析

為檢驗論文所提方法的有效性，基于棋盤格標(biāo)記的工件進(jìn)行6D位姿測量實驗. 實驗采用前文所述的位姿測量系統(tǒng)，其相機(jī)分辨率為2 560×1 440，軟件為MATLAB R2021a和Visual Studio 2022.

實驗過程中，將棋盤格標(biāo)記置于實驗平臺的任意位置，然后分別沿X、Y、Z方向移動，其起點均為10 mm，之后每隔5 mm拍攝一幅圖片. 實驗中的棋盤格標(biāo)記分別沿X、Y、Z方向移動170 mm，各方向拍攝33幅圖像，共99幅圖像. 圖6列出的是各方向測試圖像中選取的9幅圖像.

表1所示為圖6中所展示圖像位移誤差的絕對值，其結(jié)果顯示：三個方向最大絕對誤差均小于0.5 mm.

參考文獻(xiàn)[22]所提出的一種基于EPNP加權(quán)迭代算法的位移測量實驗結(jié)果如表2所示.

通過對比可知，論文所提方法相較于基于EPNP加權(quán)迭代算法在測量X、Y方向位移精度都能達(dá)到較好的效果，但本方法在測量Z方向位移提高大約83.5%的精度. 此外，其量程相較于參考文獻(xiàn)[22]所提出的方法更大，意味著此方法能應(yīng)用于更廣泛的場景.

如表3所示是圖6中展示圖像旋轉(zhuǎn)誤差的絕對值，三個方向的最大絕對誤差均不超過0.8°.

為檢驗論文所提方法在實際運(yùn)用中的實時性，實驗記錄了每幅圖像從預(yù)處理到得出6D位姿的耗時，結(jié)果如圖7所示（使用電腦主要參數(shù)為處理器：6核AMD Ryzen 5 5600H CPU @3.30GHz;內(nèi)存：16GB;系統(tǒng)類型：64位操作系統(tǒng)），采用基于特征標(biāo)記的物體6D位姿測量方法的耗時均小于2.5 s. 實驗結(jié)果表明，論文所提方法具有較快的檢測速度，實時性強(qiáng).

4"結(jié)"論

針對產(chǎn)線工件位姿的測量需求，論文提出了一種基于特征標(biāo)記的工件6D位姿測量方法. 該方法采用棋盤格代替?zhèn)鹘y(tǒng)特征標(biāo)記與待測物體剛性連接，增加了待測內(nèi)角點數(shù)，解決了特征標(biāo)記識別精度低和可用于計算的3D-2D點數(shù)目少等問題，提高了6D位姿測量精度.

實驗結(jié)果表明，該方法在保證測量X、Y方向位移精度不降低的情況下，將測量Z方向位移精度提高了83.5%;且測量繞X、Y、Z三個方向的旋轉(zhuǎn)誤差絕對值均小于0.8°，比傳統(tǒng)的基準(zhǔn)標(biāo)記法更具優(yōu)越性，檢驗了所提方法的有效性;并在進(jìn)一步實驗中，所提方法的測量耗時均小于2.5 s，驗證了其位姿測量的實時性.

參考文獻(xiàn)

[1]" 閔永智，陶佳，任維卓. 基于特征點位置校正的靶標(biāo)位姿測量方法[J]. 自動化學(xué)報，2020，46（3）：509-517.

[2]" 吳福培，魏亞輝，李慶華，等. 高速軌道超聲成像傷損檢測及其參數(shù)學(xué)習(xí)方法[J]. 計算機(jī)集成制造系統(tǒng)，2021，27（3）：747-756.

[3]" FISCHLER M A，BOLLES R C. Random sample consensus：a paradigm for model fitting with applications

to image analysis and automated cartography[J]. Communications of the ACM，1981，24（6）：381-395.

[4]" LU C P，HAGER G D，MJOLSNESS E. Fast and globally convergent pose estimation from video

images[J]. IEEE Computer Society，2000，22（6）：610-622.

[5]" LI S，XU C. A robust O （n） solution to the perspective-n-point problem[J]. IEEE Transactions on Pattern

Analysis and Machine Intelligence. 2012，34（7）：1444-1450.

[6]" NAIMARK L，F(xiàn)OXLIN E. Circular data matrix fiducial system and robust image processing for a wearable

vision-inertial self-tracker[C]. Darmstadt：Proceedings International Symposium on Mixed and Augmented

Reality，2002：27-36.

[7]" GARRIDO-JURADO S，MU?OZ-SALINAS R，MADRID-CUEVAS F J，et al. Automatic generation and detection of highly reliable fiducial markers under occlusion[J]. Pattern Recognition，2014，47（6）：2280-2292.

[8]" 許允喜，蔣云良，陳方. 基于點和直線段對應(yīng)的擴(kuò)展正交迭代位姿估計算法[J]. 光學(xué)學(xué)報，2009，29（11）：3129-3135.

[9]" 許允喜，蔣云良，陳方. 多攝像機(jī)系統(tǒng)位姿估計的廣義正交迭代算法[J]. 光學(xué)學(xué)報，2009，29（1）：72-77.

[10]" 苗錫奎，朱楓，郝穎明，等. 基于特征點不確定性加權(quán)誤差的位姿估計新方法[J]. 光電子.激光，2012，23（7）：1348-1355.

[11]" 李鑫，龍古燦，劉進(jìn)博，等. 相機(jī)位姿估計的加速正交迭代算法[J]. 光學(xué)學(xué)報，2015，35（1）：266-273.

[12]" 周潤，張征宇，黃敘輝. 相機(jī)位姿估計的加權(quán)正交迭代算法[J]. 光學(xué)學(xué)報，2018，38（5）：193-199.

[13]" 張慧娟，熊芝，勞達(dá)寶，等. 基于EPNP算法的單目視覺測量系統(tǒng)研究[J]. 紅外與激光工程，2019，48（5）：190-195.

[14]" 楊志偉，王良明，張喜峰，等. 雙高速攝像機(jī)交匯的高旋彈丸位姿估計算法及誤差分析[J]. 國防科技大學(xué)學(xué)報，2022，44（2）：71-79.

[15]" 尚硯娜，石晶欣，趙巖，等. 大型結(jié)構(gòu)體裂縫檢測中的定位方法[J]. 儀器儀表學(xué)報，2017，38（3）：681-688.

[16]" 李成龍，鐘凡，秦學(xué)英. 基于3維模型的單視圖不規(guī)則物體定位[J]. 計算機(jī)輔助設(shè)計與圖形學(xué)學(xué)報，2015，27（1）：68-75.

[17]" 劉進(jìn)博，郭鵬宇，李鑫，等. 基于點對應(yīng)的相機(jī)姿態(tài)估計算法性能評價[J]. 光學(xué)學(xué)報，2016，36（5）：121-130.

[18]" 李航宇，張志龍，李楚為，等. 基于視覺的目標(biāo)位姿估計綜述[C]. 高分辨率對地觀測學(xué)術(shù)聯(lián)盟. 第七屆高分辨率對地觀測學(xué)術(shù)年會論文集，2020：14.

[19]" 劉晨陽，鄭龍江，侯培國. 一種快速加權(quán)迭代位姿估計法[J]. 激光與光電子進(jìn)展，2022，59（18）：1815019.

[20]" 陶孟衛(wèi)， 姚宇威，元海文，等. 無人機(jī)自主降落視覺標(biāo)識設(shè)計及位姿測量方法[J]. 儀器儀表學(xué)報，2023，43（5）：155-164.

[21]" 邢加偉，田海峰，王芳. 單目相機(jī)物體位姿估計方法研究[J]. 導(dǎo)航定位與授時，2019，6（4）：71-77.

[22]" 汪佳寶，張世榮，周清雅. 基于視覺EPnP加權(quán)迭代算法的三維位移實時測量[J]. 儀器儀表學(xué)報，2020，41（2）：166-175.

6D Pose Measurement Method Based on Feature Mark

ZHOU Xu， WU Fupei*， LU Xiaohui， LV Liwei， LI Shengping

（Department of Mechanical Engineering， College of Engineering， Shantou University， Shantou 515063， Guangdong， China）

Abstract" The 6D pose of an object is its position and attitude based on planar vectors and rotation vectors. Traditional methods for estimating the 6D pose of an object using a monocular RGB camera often result in low measurement accuracy due to reasons such as a lack of markers and low recognition rates. To improve the accuracy of object pose measurement， a feature marker-based method for measuring the 6D pose of an object is proposed. First， a feature marker that can provide more markers and has a high recognition rate is designed. 3D-2D point pairs are obtained by recognizing the feature markers on the object to be measured with the camera. Second， a method for measuring the 6D pose of an object is designed based on the principle of camera imaging. Then， models of rotation matrices and displacement matrices for solving the pose changes of the workpiece are established. Finally， the 6D pose of the workpiece is solved based on the relationship between the established rotation matrix model and Euler angles. Experimental results show that within a displacement range of 0-170 mm， the measurement errors of displacement along the X， Y， and Z axes are less than 0.5 mm; within a rotation range of 0-30°， the measurement errors of rotation angles around the X and Y axes are less than 0.8°; within a rotation range of 0-45°， the measurement error of rotation angle around the Z axis is less than 0.8°; the time consumed for measuring the rotation angle error of the object around the X and Y axes is less than 1.5 s， and the time consumed for measuring the rotation angle error around the Z axis is less than 2 s; the time consumed for measuring the displacement error of the object along the X and Z axes is less than 2 s， and the time consumed for measuring the displacement error along the Z axis is less than 2.5 s; it can meet the actual production requirements. The experimental results verify the effectiveness of the proposed method， which can be used in actual production environments.

Keywords" machine vision; 6D pose; pose estimation; monocular RGB camera; PnP