基于特征標記的物體6D位姿測量方法

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

關鍵詞"機器視覺;6D位姿;位姿估計;單目RGB相機;PnP

中圖分類號"TP391.4""文獻標識碼"A

0"引"言

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

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

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

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

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

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

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

由于相機坐標系到圖像坐標系之間是透視關系，則其歸一化過程可轉換為如下的三角形相似關系：

式中：f代表焦距，即相機坐標系和圖像坐標系在Z軸上的差.

在此基礎上，可將整個相機坐標系與圖像坐標系之間的轉換關系和歸一化過程轉換為如下齊次坐標形式：

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

由此，兩坐標系之間的映射關系可表示為：

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

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

由于世界坐標系和相機坐標系之間是剛體變換，其映射關系主要因外參：旋轉矩陣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匹配點對應兩個方程，一共有12個未知數，所以至少需要已知6組匹配的3D-2D點對方可求解[17]. 假設共有N組匹配點，則有：

將上式矩陣方程簡化為：

AF=0（9）

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

①當N=6時，可直接求解該線性方程組.

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

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

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

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

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

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

由于在位姿變化的過程中，相機的位姿始終固定，因此，相機與特征標記之間的位姿關系可表示為：

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

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

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

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

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

根據旋轉矩陣特性可得：

R01=R-1

1" " ·R0

結合旋轉矩陣與歐拉角之間的關系[21]，若假設：

R01=r11" r12" r13

r21" r22" r22

r31" r32" r33

則第二時刻被測物體繞世界坐標系OW-XWYW ZW的Z軸的旋轉角為：

θz=atan2（r21，r11）

其繞Y軸的旋轉角為：

θy=atan2-r31，

而繞X軸的旋轉角為：

θx=atan2（r32，r33）

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

3"實驗結果及分析

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

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

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

參考文獻[22]所提出的一種基于EPNP加權迭代算法的位移測量實驗結果如表2所示.

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

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

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

4"結"論

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

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

參考文獻

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

[2]" 吳福培，魏亞輝，李慶華，等. 高速軌道超聲成像傷損檢測及其參數學習方法[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，FOXLIN 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]" 許允喜，蔣云良，陳方. 基于點和直線段對應的擴展正交迭代位姿估計算法[J]. 光學學報，2009，29（11）：3129-3135.

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

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

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

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

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

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

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

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

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

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

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

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

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

[22]" 汪佳寶，張世榮，周清雅. 基于視覺EPnP加權迭代算法的三維位移實時測量[J]. 儀器儀表學報，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