基于法向約束的背靠視覺端面對接定位方法

于長志，張連新，孫鵬飛，陳東生，宋穎慧，姚云飛，李 煉，李代楊

（中國工程物理研究院機械制造工藝研究所，四川綿陽 621999）

0 概述

對接定位，尤其是端面圓特征對接定位，在產品自動化裝配［1-2］、航天交會對接［3-4］、飛機大部件對接［5-6］、電子產品貼合等工程領域應用廣泛。其通過數字化測量技術獲取零部件的空間狀態信息，并反饋給相關設備進行位姿調整，可大幅提高對接過程的柔性化。端面圓特征定位通?？捎煽臻g中的圓特征在不同坐標系下的相對旋轉量和相對位移量表示［7-8］，其檢測精度將影響對接成功率、對接效率等。實際端面圓的材質、加工精度、鏡面反射、環境光照等因素嚴重制約了端面圓的高精度檢測。

目前，對接定位方法主要分為激光跟蹤儀測量方 法［9］、激光雷達測量方法［10］、機器視覺測量方法［11-13］等。激光跟蹤儀方法屬于點位測量，可用于跟蹤目標點的位置，但需要在產品表面安裝特定標識。激光雷達測量方法屬于海量點云測量，測量效率和數據處理效率較低，檢測精度往往不高。機器視覺測量方法［12-14］屬于非接觸測量且不需要粘貼特定標識，通過視覺測量獲取目標的空間位置和姿態，具有高精度、高效率的特點，成為當前的研究熱點。文獻［15-17］采用多目視覺對圓位姿進行求解，然而由于多目視覺測量方法需采用多部設備，因此限制了視覺測量的視場范圍，導致該方法在測量范圍和測量距離上有待提高。此外，由于多目視覺測量方法需要對目標的立體匹配及測量系統進行現場校準，因此其研究算法和系統步驟均較復雜［5-7］。相比多目視覺測量方法，單目視覺測量方法［18-20］僅釆用一部相機對空間物體特征進行測量，由于結構簡單、計算速度快、測量準確度高等優點，其具有更高的研究價值?？臻g圓可以提供圓心坐標和法向量（即位姿解），然而在采用單目視覺測量方法時，圓特征的位姿解存在二義性問題［7-8］。ZHOU 等［2］為降低大型齒輪的裝配誤差，通過引入權重，提出改進的圓特征姿態計算方法。LI 等［15］分析三維物體圓形特征的投影與相應的符號距離函數之間的關系，提出一種利用水平集方法結合橢圓形狀先驗信息進行物體姿態估計的方法。WANG 等［17］針對圓位姿解的非唯一性及處理精度不高的問題，引入4 個共面特征點，并提出一種基于點特征的單目視覺位姿測量算法。ZHU［20］等提出基于連續圖像的位姿測量算法，根據連續圖像的相關性位姿得到位姿正確解。ZHANG等［21］通過增加空間特征點幾何約束，證明特征點與光心和圓平面的相對位置，并得到唯一位姿解。ZHANG 等［22］借助激光測距儀，改進圓特征的單目視覺測量算法，并進行仿真驗證。HAO 等［23］提出一種多因素融合的誤差分析方法，通過抑制相機畸變和提高相機分辨率，提升位姿測量精度。QU 等［24］在分析圓形合作目標特征點的提取精度基礎上，提出專門設計圓形和方形合作目標進行位姿測量的方法。

針對不同的應用背景，研究人員針對單目視覺圓特征位姿測量改進開展了大量研究，然而在獲取高精度、高魯棒圓特征位姿解方面仍存在一些不足，尤其是在機器人對接定位領域，由于需要通過背靠視覺對端面圓位姿進行高效、高精度解算，因此限制了其在工程領域的應用。

本文從提高測量精度和魯棒性目的出發，以排列點激光位移傳感器和背靠視覺為測量手段，提出一種基于法向約束的背靠視覺端面圓對接定位方法。針對端面圓檢測中由圖像噪聲、邊緣識別精度等因素引起的橢圓擬合中心不準確問題，引入隨機采樣一致性（Random Sample Consensus，RANSAC）算法進行橢圓擬合，降低噪聲點和無序點對擬合中心的影響。針對圓位姿解二義性問題，采用排列點激光構建法向約束，通過設定法向閾值判定圓位姿解，從而得到準確的圓位姿解。

1 相機模型坐標系

在相機測量模型中［15，25］，主要使用世界坐標系、圖像坐標系、像平面坐標系和相機坐標系表征物體與圖像像素點之間的轉換關系，如圖1 所示。世界坐標系描述物體的空間位姿，圖像坐標系表示像素所在的列數和行數，像平面坐標系描述像素在圖像中的物理位置。相機坐標系與世界坐標系之間的關系可以用物體與相機的平移與旋轉來表示。

圖1 相機模型的坐標系描述Fig.1 Coordinate description of camera model

根據相機的理想小孔成像模型［14，24］，世界坐標系下點(xW,yW,zW)對應的相機圖像像素坐標如式（1）所示：

其中：zc表示物體在相機坐標系中的z坐標；dx和dy分別是像素的尺寸因子；s為相機像平面坐標軸不正交導致的傾斜因子；(u,v)表示像素坐標，(u0,v0)是圖像主點坐標；f是相機焦距；K稱為相機內參矩陣；C=[R H]表示相機外部參數，其中R表示旋轉矩陣，H表示平移向量。

2 本文方法

2.1 方法流程

在機器人對接裝配中，需要以下工件的端面為基準，對上工件進行姿態調節和位移調節，從而確保上下工件完成對接裝配。本文方法的流程如圖2所示。

圖2 本文方法流程Fig.2 Procedure of method in this paper

本文方法的具體流程如下：

1）進行背靠相機標定和機器人手眼標定，得到相機的內參以及相機到機器人基坐標系的轉換關系；

2）下相機采集下工件端面圖像，經由特征提取、RANSAC 算法進行特征擬合得到下工件端面的中心位置，并將其作為上下工件對接的基準位置；

3）上相機采集上工件端面圖像，經由特征提取、RANSAC 算法進行特征擬合得到圖像坐標系下的橢圓方程；

4）根據單目相機圓位姿模型，計算在相機坐標系下以相機光心為頂點、目標圓為底面的橢圓錐方程，得到二義性的特征圓位姿解；

5）下相機采集排列點激光的激光點，通過光點中心以及激光測距信息進行平面擬合，以平面法向量作為法向約束；

6）將法向約束與得到的圓位姿解進行計算，得到正確的圓位姿解；

7）判斷圓姿態偏差，若不滿足閾值（一般取閾值ε=0.01 °）則由機器人進行調整，重復步驟2～步驟7，直至滿足閾值要求；

8）計算上下工件端面中心位移偏差量，判斷位移偏差量是否在閾值（一般取η=0.01 mm）范圍內，否則由機器人進行位移調整，重復步驟2～步驟8，直至滿足閾值范圍，從而完成上下工件端面的對接定位調節。

本文方法的特點在于背靠視覺獲取上下工件端面特征圖像，采用RANSAC 算法進行圓特征擬合，并引入排列點激光作為法向約束，實現端面圓位姿參數的準確獲取。此外，通過機器人進行姿態迭代調整和位移迭代調整，完成上下工件姿態和位移的調節，從而實現準確的端面對接。

2.2 基于RANSAC 的端面圓特征擬合

相機采集的工件端面圓圖像通常表現為橢圓，需先進行特征亞像素邊緣提取［25］，然后根據邊緣提取結果進行橢圓擬合，從而得到橢圓的空間表達式。設成像橢圓在像面XY坐標系中的方程［13］如式（2）所示：

則根據最小二乘擬合原理，可通過取如式（3）所示的目標函數最小值來決定參數(a,b,c,d,e)。

其中：κnm表示邊緣點的數量。

令每個參數的偏微分算子均為0，即：

可得如式（5）所示的線性方程：

通過求解式（5），可以得到上述橢圓參數。最小二乘橢圓擬合算法屬于無偏估計，即所有點均參與擬合，因此擬合誤差大，且魯棒性較差［25］。而RANSAC 算法能夠迭代地從含有大量局外點的觀測數據集中估計設定數學模型的參數，剔除了噪聲極值、異常值等邊緣點帶來的影響，從而可以提高擬合結果的魯棒性和準確性［25］。RANSAC 算法的步驟如下：

1）從工件端面橢圓邊緣點中隨機抽取樣本點，根據式（2）～式（5）進行最小二乘橢圓擬合，得到橢圓參數；

2）計算所有樣本點與得到的橢圓參數的擬合殘差，并選擇小于容許誤差的樣本數據和樣本數目；

3）當樣本數目大于局內點個數閾值時，停止局內點搜索，保存此樣本數據；

4）設置迭代次數并重復步驟1～步驟3，如果局內點個數小于局內點個數閾值，則停止局內點搜索，保存最大個數的局內點集合和此樣本數據；

5）根據式（2）～式（5）對步驟4 得到的樣本數據進行最小二乘橢圓擬合，得到橢圓表達式。

通過RANSAC 算法得到的橢圓表達式，由于剔除了異常值并降低了噪聲值的影響，因此可以得到更高魯棒性、更高準確性的橢圓擬合結果。

2.3 基于排列點激光法向約束的圓位姿求解

在單目相機圓位姿模型［18-20］中，相機光心到工件端面形成一個橢圓錐面。求解端面圓位姿即尋找一截平面，使該平面與橢圓錐相交形成一個已知半徑為r的圓，其圓心坐標和法向量即為端面圓的空間位置和姿態。在單目視覺下空間端面圓描述一般需要圓心、半徑、俯仰角及傾斜角［17］參數，空間圓的姿態可用俯仰角和傾斜角表示。如圖3 所示，其中：(x0,y0,z0)為圓心位置；向量n為空間圓所在平面的法向量；向量np為n在平面OXY上的投影。俯仰角β為n與平面OXY的夾角，取值范圍為；傾斜角α為np與正向軸OX的夾角，取值范圍為[0,2π]。

圖3 圓位姿示意圖Fig.3 Schematic diagram of circular pose

相機光心到端面形成一個橢圓錐面，在相機坐標系XcYcZc中的方程如式（7）所示：

式（7）可重寫為如式（8）所示：

相機坐標系下的點pi=(Xi,Yi,Zi)與對應圖像坐標系下點(ui,vi)的坐標轉換關系可表示為式（9）所示：

式（10）為相機光心與端面圓形成的橢圓錐，圖像平面上的橢圓投影為橢圓錐的一個截平面。設Cc=PC′c為從相機坐標系到主軸坐標系的正交變換，其中，P3×3為正交矩陣。則公式（10）可轉化為主軸坐標系下的橢圓錐，如式（11）所示：

若取P1、P2、P3分別為矩陣Q的特征向量，對應的特征值分別為λ1、λ2、λ3。其中λ1＞λ2＞0,λ3＜0。設正交矩陣P-1QP=PTQP=diag(λ1,λ2,λ3)，則 式（11）的橢圓錐可寫為式（12）所示：

根據式（12），端面圓在主軸坐標系下的圓心和法向量可分別描述為式（13）所示：

將式（13）轉換到相機坐標系下，從而得到端面圓的圓心坐標及法向量，如式（14）所示：

由式（14）得到端面圓的圓心坐標和法向量，其中包括了一個虛假解和一個正確解，如圖4 所示。端面圓在橢圓錐中可以找到2 個截圓平面，分別表示一個虛假解和正確解。從式（14）中可以看出，虛假解和正確解的法向量均可以得到，為此，引入排列點激光作為法向約束，消除圓位姿解的二義性問題。

圖4 端面圓的二義性位姿解示意圖Fig.4 Schematic diagram of ambiguous pose solution of end-face circle

背靠相機分別采集點激光在工件端面圓的圖像，以激光點輪廓的質心作為激光點對應的圖像坐標。根據相機的光心和各點激光的距離，并由點激光測距結果，可得到激光點在相機坐標系下的坐標Li=(XUi,YUi,ZUi),i=1,2,3,4。由于空間上至少3 點可擬合一個平面，則上工件端面中的4 個激光點可通過最小二乘擬合方法得到激光平面，即：

利用拉格朗日乘數法求函數極值，如式（17）所示：

根據式（18）可以得到實對稱矩陣，如式（19）所示：

由式（20）可以求解E的特征值，則最小特征值ηmin對應的特征向量即為激光平面方程的法向量nU。

對式（14）得到的二義性圓法向量根據式（21）分別計算其與對應平面法向量的夾角，即：

取對應最小夾角的圓法向量為端面圓圓法向量，從而通過增加排列點激光法向約束的方式得到端面圓法向量的正確解。

2.4 基于機器人的對接位姿調整

由2.3 節可以得到端面圓特征的空間姿態和位置。由于下工件端面固定不動，機器人抓取上工件端面需在進行相對位置和姿態調整之后才可完成對接。因此，需將由背靠相機和排列點激光得到的工件端面圓特征的空間姿態和位置，轉換為在機器人基座標系下得到的姿態調整量和位置調整量。

在對接裝置中，測量單元安裝在固定裝置上，與機器人的標定屬于手眼標定［4-6］，如圖5 所示。

圖5 機器人手眼標定Fig.5 Robot hand-eye calibration

因此需事先對機器人和背靠相機進行手眼標定，由于相機相對機器人基坐標系固定不動，則相機在機器人基坐標系下的位姿可描述為式（22）所示：

通過對背靠相機進行的標定，可以分別得到背靠相機的內部參數和外部參數。為了將下相機的坐標系轉換到上相機坐標系中，設上下相機相對機器人基座標系的外參分別為CU和CD，則下相機坐標系到上相機坐標系的旋轉和平移矩陣可表示為

已知下工件端面為對接裝配的基準，設下工件端面的法向量nD=[0 0 1]，根據上下工件端面的法向量，在上相機坐標系中可得上下工件端面的法向量夾角如式（23）所示：

則上工件端面相對下工件端面的位置和姿態調整量可描述為如式（24）所示：

在機器人基座標系下的位置和姿態調整量如式（25）所示：

由于在機器人位姿調整過程中，姿態的偏移會影響到位置信息，因此先通過機器人進行姿態迭代調整，當滿足姿態調整的閾值后，再進行位移迭代調整，直至滿足位移偏差閾值，完成機器人對上工件端面的位姿調整，實現上下工件端面的對接。

3 實驗結果與分析

圖6所示為機器人對接裝配裝置，主要由機器人、測量裝置、裝配臺等構成。圖7為背靠視覺和排列點激光形成的測量裝置。機器人型號為KR600（KUKA，最大負載600 kg）。點激光的型號為ACR-LDS510-10（上海達拉斯光電科技有限公司，測量范圍為10 mm，分辨率為0.5 μm，重復精度為2 μm）。相機的型號為MV-CH650-90TM-M58S-NF（海康威視，分辨率為9 344×7 000 像素），鏡頭的型號為LSF2528，焦距為25 mm。軟件工具為Halcon 20.05，運行平臺為win10 X64、內存32 GB、硬盤1 TB、處理器i9-9900K。在圖6中，上下工件的端面尺寸直徑為204 mm、端面止口雙向間隙為0.08 mm。

圖6 機器人對接裝配裝置Fig.6 Robot docking assembly device

圖7 背靠視覺和排列點激光測量裝置Fig.7 Measuring device with back against vision and arrangement point lasers

圖8 和圖9 分別為背靠下相機和上相機的部分標定圖像，由此可得到背靠相機的標定參數，以及上相機和下相機相對機器人基坐標系的外參變換關系。

圖8 不同位姿下的下相機標定圖像Fig.8 Calibration images captured by the down camera under different positions and attitudes

圖9 不同位姿下的上相機標定圖像Fig.9 Calibration images captured by the up camera under different positions and attitudes

為更好地判斷本文方法的精度，將標定板安裝在上工件端面，以標定板的中心點作為特征點，從而得到機器人的位移調整精度，如圖10 所示。

圖10 標定板位移驗證Fig.10 Displacement verification with calibration plate

分別移動機器人示教器的X軸和Y軸，并固定步長，進行標定板位移偏差的重復測驗，結果如表1 所示。從表1 可以看出，機器人的位置調整精度在X方向和Y方向均小于0.01 mm，產生誤差的原因主要在于大負載機器人存在一定抖動、機器人定位精度誤差等因素。圖11 所示為下相機采集的下工件端面圖像，圖中顯示了特征邊緣提取及擬合中心。圖12 為上相機采集的不同位姿下的上工件端面圖像，圖中顯示了邊緣提取及擬合中心。

圖11 下工件端面Fig.11 End-face of down workpiece

圖12 上工件端面的迭代調整圖像Fig.12 Iterative adjustment images of the end-face of up workpiece

為證明本文方法的有效性，將本文方法與方法1［4-5］、方法2［7，26］和方法3［27］進行比較分析。由于物體的空間姿態會影響空間位置，因此首先對物體進行姿態調節，然后再進行位移調整。表2 為端面姿態測量驗證結果。可以看出，端面姿態的測量結果相對機器人的姿態有約0.03°的偏差，相比表1、表2 的位移偏差明顯增大，這是由于標定板圖像中心點的邊緣識別和提取精度更高，受視場和光照等因素影響，端面特征更難提取，導致位移偏差增大。

表2 端面圓姿態測量結果Table 2 Attitude measurement results of the end-face of up workpiece （°）

表3 為采用本文方法時機器人的位置調整情況，下工件端面基準為手控器中X軸和Y軸的數據，在3 次不同基準下，通過上工件端面的重復位移調整驗證，可以看出X方向最大偏差為0.01 mm，Y方向的最大偏差為0.03 mm。

表3 不同基準下端面位置調整驗證Table 3 Position adjustment verification of up workpiece under different position benchmarks

表4 為不同方法下的姿態測量結果，從表2 和表4 中可以看出：方法1 的姿態偏差約為0.1°，這是姿態解算精度不高引起的；方法2 的姿態偏差約為0.03°，方法3 的姿態偏差約為0.04°，雖然方法2 和方法3 的姿態精度非常接近本文方法，但本文方法具有更優的一致性，且收斂速度更快。

表4 不同方法下端面姿態調整比較Table 4 Comparison of end-face attitude deviation with different methods （°）

表5 為在不同基準下不同方法的端面位置偏差情況，圖13 為在同一基準下進行多次重復實驗時，不同方法的端面位置偏差情況。由圖13 可以看出，方法1 的位移偏差最大，方法2 與方法3 的位移偏差接近，但方法3 的收斂性更快，本文方法具有最小的位移偏差和最多的迭代次數。從表2 和圖13 可以看出，本文方法的姿態和位移需3～5 次迭代調整。

圖13 重復實驗下不同方法位移偏差和迭代次數Fig.13 Displacement deviation and iteration times of different methods in repeated test

表5 不同方法下端面位置調整比較Table 5 Comparison of end-face position adjustment with different methods

由表4、表5 和圖13 可以看出，方法1 的姿態計算偏差和位移偏差最大，原因在于采用最小二乘法進行橢圓中心提取屬于無偏差估計方法，受圖像噪聲、邊緣提取精度影響較大，尤其在大視場下，圖像噪聲更易受光照影響。方法2 和方法3 的偏差優于方法1 的偏差，這是因為方法2 和方法3 對單目相機測量姿態產生的二義性解進行了正確解的判斷和提取，仍存在較大偏差的主要原因是該方法受端面圖像特征邊緣的提取精度影響較大。方法2 和方法3均具有較大的姿態偏差和位移偏差，其位移計算與姿態計算相互影響，姿態的調整偏差影響了位移的調整。相比方法1、方法2 和方法3，本文方法獲得了更小的姿態計算偏差和位移調整偏差。這是因為本文方法采用了RANSAC 算法進行橢圓擬合，可以減少噪聲的影響，具有較高的魯棒性。此外，本文方法采用點激光位移傳感器建立法向約束，對單目相機測得的姿態進行正確解提取。由表4、表5 和圖13 可以看出，相比方法1、方法2 和方法3，本文方法具有姿態調整準確、位移調整收斂快的特點，姿態對接角度最大偏差為0.03°，中心位移X方向的最大偏差為0.01 mm、Y方向的最大偏差為0.03 mm，證明本文方法的有效性和準確性。

圖14 所示為機器人完成的對接裝配，通過對上下工件的端面尺寸直徑為204 mm、端面止口雙向間隙為0.08 mm 的驗證實驗可知，姿態和位移需3 次～5 次迭代調整可完成對接裝配。這證明了本文方法的有效性、魯棒性和實用性。

圖14 機器人完成的對接裝配Fig.14 Docking assembly completed by the robot

4 結束語

本文從機器人對接裝配工程應用出發，并考慮圓位姿解的二義性問題，提出一種背靠視覺端面對接定位方法。采用RANSAC 算法進行橢圓擬合，以避免在采用單目視覺進行測量時，姿態和中心位置圖受圖像噪聲、邊緣提取等因素影響。通過排列點激光建立法向約束，得到更準確的圓姿態解。實驗結果表明，該方法的姿態對接角度最大偏差為0.03°，中心位移X方向最大偏差為0.01 mm、Y方向的最大偏差為0.03 mm，具有姿態調整準確、位移調整收斂快的特點。下一步將引入深度學習等人工智能技術，以減少該定位方法的位姿調整次數，提高對接效率。