3D位姿估計結合阻抗控制的沉孔裝配

陳平，雷學軍，李燦，胡義亮，陳爽

（重慶大學 機械與運載工程學院，重慶 400044）

1 引言

隨著工業4.0時代的到來，智能化裝配受到廣泛的關注，吸引著大量的研究人員。其中裝配領域最為常見的便是軸與孔的裝配。隨著技術的發展，軸孔裝配的自動化程度越來越高，但傳統的自動化裝配無法滿足日益智能化、精密化的裝配要求，尤其是在軍工、光學產品裝配中。圓軸與沉孔是比較常見的配合零件，圓軸與沉孔零件的裝配面臨的關鍵問題是：（1）如何估計沉孔零件的任意的空間位置和姿態；（2）如何將軸零件柔順地導入沉孔零件內。

關于孔零件的位姿（位置和姿態）估計方法，根據不同的相機配置和視覺技術，大致可以分為3種：采用單目相機［1-2］，采用雙目相機［3-6］及采用3D相機［7-9］。胡瑞欽等人［1］建立了單目相機與探針、銷釘結合的方式，利用單目相機將部件引導至輔助銷釘的導向范圍內以完成裝配。該策略易于系統工程化，但是孔位測量匹配誤差達到了2.9 mm，無法滿足更精密的裝配要求，此外，單目相機獲取的圖像丟失深度信息且易受光照影響，無法直接用于位姿估計，需要利用銷釘導向，通用性不高。Zheng等人［6］采用雙目相機拍攝空間中的圓孔，根據兩個視角中橢圓與圓孔的幾何關系，完成圓孔的空間位姿估計，但其前提是已知空間圓的半徑。Shah等人［9］利用CAD創建零件的標準化模型點云，再通過3D相機獲得零件的真實點云，將二者配準獲得零件的空間位姿，但是該方法需要預先已知待裝配零件的尺寸。Vosselman等 人［10］使用霍夫圓 柱 擬 合對圓柱點云擬合來估計其位姿。Forsman等人［11］則采用隨機采樣一致性（Random Sample Consensus，RANSAC）圓柱擬合來估計姿態。這些方法的前提是必須獲得零件的徑向點云（即側面點云）。在沉孔的裝配場景中，孔作為零件的一部分，外圓柱面形狀未知，徑向點云很難獲取，故無法采用圓柱擬合的方法估計位姿。沉孔的上端面特征較為固定，一般由平面和平面上的圓組成，且點云數據易獲得，故本文基于沉孔端面點云（軸向點云）來估計沉孔零件位姿。

零件柔順裝配的關鍵在于機器人的柔順控制，已有的方法包括阻抗控制［12-15］和力/位混合控制［16］等。力/位混合控制利用接觸力和位置的正交原理同時實現機器人末端的接觸力和位置跟蹤控制。阻抗控制通過建立機器人末端作用力與位置偏差之間的動態關系，間接地控制機器人與待裝配零件之間的接觸力，使機器人在受約束方向保持期望的接觸力。在軸與沉孔裝配過程中已完成對沉孔零件的精確定位，但由于機器人在運動過程中的位置控制誤差無法完全消除，且沉孔與軸之間的配合間隙很小，很容易產生卡阻現象，此時阻抗控制便能很好地消除卡阻，實現柔順裝配。

本文針對長圓軸與沉孔的自動裝配問題，通過3D相機獲得沉孔的軸向點云，并針對軸向點云提出了沉孔空間位姿估計算法，同時融合阻抗控制的優點，形成3D視覺和力覺的柔順裝配策略，實現圓軸與沉孔零件的柔順裝配，可提升裝配精度和裝配效率，適應性更廣，并滿足穩定可靠的要求。

2 系統組成及工作原理

2.1 自動裝配系統的硬件構成

圖1展示了機器人零件裝配平臺的組成。其中，六自由度機器人是整個系統的載體，3D相機和單目相機安裝于機器人末端法蘭盤，它們共同組成整個系統的視覺部分，可實現在抓取過程中對軸的識別、定位以及裝配過程中對沉孔的空間位姿估計。六維力傳感器安裝于機器人末端法蘭盤與末端執行氣爪之間，可實時監測裝配過程中的力/力矩變化，從而引導機器人的移動，達到柔順裝配的目的。

待裝配的零件為圓軸與沉孔零件，其基本尺寸如圖1所示，沉孔零件的內徑為20 mm，整個沉孔的深度為270 mm，軸零件的直徑為19 mm，軸的長度為270 mm，軸孔之間的配合間隙為0.5 mm±0.05 mm。

圖1 機器人零件裝配平臺Fig.1 Platform of robotic assembly

2.2 自動裝配系統的工作原理

應用該系統進行圓軸與沉孔裝配的主要步驟如下：

（1）機器人移動至待裝配圓軸零件工作臺上方，利用單目相機，通過基于深度學習的目標檢測方法完成對軸零件的識別與定位，最后完成抓??；

（2）完成抓取后，機器人移動至沉孔零件工作臺上方，使用3D相機完成對沉孔零件上端面點云數據的獲??；

（3）通過沉孔位姿估計關鍵算法獲取沉孔零件的空間位姿；

（4）最后根據（3）中獲取的位姿，通過阻抗控制將軸柔順的裝入沉孔中。

本文將使用預訓練的網絡YOLOv3來完成步驟（1）中對圓軸零件進行檢測的任務，因為YOLOv3已經有較成熟的應用技術，在本文中將不再進行敘述。本文聚焦于步驟（3）和（4）的敘述，即沉孔位姿估計算法和阻抗控制。

3 沉孔位姿估計關鍵算法

沉孔的空間位姿可表示為(X，n)，其中X表示沉孔上端面的圓孔圓心，n表示沉孔上端面平面法線。當獲得沉孔上端面點云之后，通過預處理去除點云數據中的離群點和噪聲。對沉孔上端面的平面擬合可獲得其平面參數，進而獲得n。而X可利用沉孔輪廓的圓擬合來估計。

3.1 點云數據預處理

3D相機獲得的沉孔上端面點云數據如圖2所 示，表 示 為 點 集P={p1，p2，...，pn}，pi∈R3，受外部環境及相機自身噪聲的影響，獲得的點云存在許多離群點和噪聲，這些數據影響裝配位姿估計的精度，本文采取超體素聚類分割［17-18］和約束平面分割（Constrained Planar Cuts，CPC）超體素聚合［19］來去除噪聲和離群點。

圖2 點云離群點和噪聲Fig.2 Outliers and noise of point cloud

超體素分割算法首先對點云數據進行體素化處理得到體素云，并且在體素空間下利用網格化來篩選出種子體素，為后續的聚類算法進行初始化。然后利用體素數據的空間位置和特征屬性，設計鄰接體素之間的距離度量。并利用基于流約束的聚類算法將體素云過分割得到超體素。最后由CPC聚類算法完成對離群點和噪聲的去除，獲得最終點云Pc={pc1，pc2，...pcnc}，pci∈R3。圖3中使用不同的顏色標注出分割的超體素（彩圖見期刊電子版），并連接相鄰的超體素，隨著體素半徑和網格半徑的增大，所分割的超體素個數也在增多，CPC聚類結果中使用不同的顏色將離群點標出，當體素半徑為0.3 mm，網格半徑為15 mm時已經可以分割出離群點，但是同時存在誤分割。圖4給出了在不同的體素半徑下超體素的個數及分割比，從圖4中可以看出在體素半徑為0.32 mm之后，點云的分割比維持在75%左右，此時可有效地去除離群點和噪聲。

圖3 超體素分割與CPC結果對比Fig.3 Results of supervoxel segmentation and CPC clustering

圖4 超體素分割結果Fig.4 Results of supervoxel segmentation

3.2 基于權重RANSAC的沉孔上端面擬合

為獲得沉孔上端面法線n，需對預處理后的點云數據進行平面擬合。預處理雖然去除了大片聚集的離群點，但是點云數據仍然存在部分分散的離群點和噪聲，且在點云數據的邊緣處明顯增多，如圖5紅色標記所示（彩圖見期刊電子版），這源于雙目結構光相機自身的噪聲和誤差。針對這種點云數據，提出基于權重的RANSAC（Weight-based Random Sample Consensus，

圖5 邊緣離群點和噪聲點Fig.5 Outliers and noise of the edge

WRANSAC）平面擬合方法來獲得沉孔的上端面。因為傳統RANSAC的平面擬合方法中，點云中的每個點被選中的概率是相同的，但是點云邊緣點誤差太大，會影響平面擬合的精度，為改進傳統的RANSAC方法，對鄰近沉孔中心的點賦予大權重，而遠離沉孔的邊緣點賦予小權重，來減小邊緣點的影響，如圖6所示，權重函數如式（1），式（2）所示：

圖6 權重函數示意圖Fig.6 Schematic diagram of the weight function

其中：di為點云中的一點pi到重心的距離，k為比例系數，η為標準差，本文取k=1/10 000，η=4。

為從擬合出的眾多平面模型中篩選出最佳平面，設計如式（3）所示的評價公式：

其中：Pw是與擬合平面的距離誤差小于閾值β（β=0.1 mm）的點集，m是該點集的點數，歸一化計算可以得到平面的尺度不變分數。

WRANSAC沉孔上端面擬合的流程如下：

（1）根據公式（1）計算點云P（c經過預處理后的點云）中每個點的概率ωi；

（2）根據每個點的概率ωi從點云Pc中選擇3個點作為樣本點，估計平面模型：Ai x+Bi y+Ci z+Di=0；

（3）計算點云Pc中的每個點pci與平面模型的距離，將距離小于閾值β的點記錄在點集Pm（內點集）中；

（4）根 據 公 式（3）計 算 該 平 面 模 型 的得分Swi；

（5）重復步驟（2）～（4），達到最大迭代次數（本文設置的迭代次數k=100）則停止。

選取分數Sw最大的估計作為最終的平面擬合模型：Ax+By+Cz+D=0，擬合的平面如圖7所示的P平面。

圖7 平面擬合結果Fig.7 Results of plane fitting

3.3 沉孔圓擬合

經過3.2的平面擬合后，可獲得沉孔上端面：Ax+By+Cz+D=0，則n=(A，B，C)。而X可通過對沉孔輪廓的圓擬合來獲得。

3D相機獲得的點云如圖8所示（彩圖見期刊電子版），沉孔輪廓并非為圓（如圖中紅色實線），輪廓應為圖中紅色虛線所示，這是因為3D相機在掃描沉孔的時候，會在沉孔圓內生成一些無關的點云數據（沉孔內點），如圖8紅色實線與虛線內的點，為了提取出沉孔的輪廓，完成輪廓的圓擬合，需要將沉孔內點分割出來，然后再提取出沉孔輪廓進行圓擬合，以完成對X的估計。

圖8 點云數據Fig.8 Point cloud

點云數據在3D相機獲取的過程中會根據深度不同賦予不同的RGB值。在經過沉孔輪廓時點云數據的法線方向會發生改變，屬于上端面的點云法線應與端面法線方向一致，而沉孔內點的法線方向會因深度的不同發生偏移。據此，利用結合顏色和法線信息的權重函數，來完成對沉孔內點的分割。

權重函數如式（4）所示：

其中：Ii為 點 的RGB值，θi為點的法 線 與 上 端 面的法線n=(A，B，C)的夾角，ωr為顏色值的權重，ωn為法線夾角的權重，同時設定閾值λ，只有當φi大于閾值λ的點才會被篩選為沉孔內點，經過該權重函數的篩選之后，可完成對沉孔內點的分割，圖9中紅色點為利用公式（4）分割出的沉孔內點（彩圖見期刊電子版），其中包含部分螺紋孔的點云和邊緣點云，再次利用3.1節中介紹的超體素分割和CPC聚類分割出屬于沉孔的部分，并表示為Pe={pe1，pe2，...，pen}，pei∈R3。

圖9 沉孔內點Fig.9 Inliers of countersunk-hole

為完成對沉孔輪廓的圓擬合以確定上端面圓心X，將點集Pe中的點投影到3.2擬合的平面Ax+By+Cz+D=0上，提取出其外部輪廓，然后完成圓擬合，估計出圓心X。

令點集Pe中 一 點為pei(x0，y0，z0)，設其在 平面上的投影為pplane(xp，yp，zp)，則有：

將式（5），式（6）代入平面方程Ax+By+Cz+D=0，可以解得：

將式（7）代入式（5），式（6），可以解得：

經過平面投影后，所有的沉孔內點都在平面Ax+By+Cz+D=0上，如圖10所示（彩圖見期刊電子版），通對這些內點的輪廓點提取獲得二維點集Pcircle，如圖10中紅色點所示。通過對點集Pcircle的圓擬合便可估計出沉孔上端面圓心X。

圖10 沉孔輪廓Fig.10 Profile of countersunk-hole

在現有的圓擬合方 法中，Hyper算法［20-22］的精度和速度都領先于同類算法，且它可以完成對殘缺的圓的擬合，故本文采用Hyper算法完成對點集Pcircle的圓擬合，首先，將圓表示為代數方程形式：

其中：a，b，c，d為圓的待求參數，將其表示為向量形式為：A(a，b，c，d)，將式（10）轉換為圓參數方程的一般形式：

則圓心為：

半徑為：

將點集Pcircle表示為矩陣形式為：

其中zi=x2i+yi2，xi，yi為點集Pcircle中點的x，y坐標。定義矩陣M為：

M矩陣中的求法與公式（16）中的相同。在ATNA=1的約束下，Hyper算法將圓擬合問題變成最小化函數F(A)=ATMA問題，只需要解決廣義特征值問題MA=ηNA，選擇所有解中最小的η和對應的單位向量作為最終擬合結果，即A等于單位向量。其中矩陣N為：

具體求解過程為對矩陣PM進行奇異值分解，PM=UΣVT，如果分解出的最小特征值σ4＜10-12，則A為矩陣V的第4列，當σ4≥10-12時，令Y=VΣVT，并求解YN-1Y的特征對，選擇具有最小特征值的特征對(η，A*)，此時待擬合圓參數向量A(a，b，c，d)=Y-1A*，至此，完成對沉孔輪廓圓的擬合，將圓心點投影回三維空間便可確定X。

至此，得到了沉孔零件的空間位姿，即

(X，n)。

4 軸孔零件的阻抗控制

機器人末端氣爪沿著沉孔軸線將軸零件移入孔內。然而，考慮到機器人位置控制誤差、沉孔軸線位姿估計的誤差，軸與沉孔配合間隙較小且軸與孔的長度較長，軸零件部分插入孔內，可能會接觸沉孔零件內壁，發生卡阻現象。因此，為避免卡阻現象，在軸插入孔的過程中，采用阻抗控制［12-15］自適應地調整末端執行器的姿態，從而使軸零件柔順地裝入孔內，直至軸零件插入沉孔三分之二后，松開氣爪，軸在重力的作用下完成整個裝配。

圖11 圓軸零件與孔壁接觸時的接觸力Fig.11 Contact force generated when the cylindrical shaft part contacts the hole wall

軸零件插入沉孔的過程中，當軸與孔相互作用時，可建立二階線性［13］方程：

其 中：e=(ex，ey，ez)是 末 端 執 行 器 的 位 置 修 正量，F是來自環境的外力，Md，Bd，Kd分別為期望慣性矩陣、期望阻尼矩陣和期望剛度矩陣。阻抗控制過程中，根據式（18）可以根據力偏差計算位置修正量e，通過e對機器人末端執行器的姿態持續調整，以避免產生卡阻，實現柔順裝配。

5 實驗與結果

5.1 實驗條件

實驗系統實物如圖12所示。機器人末端安裝單目相機、3D相機、力/力矩（F/T）傳感器和氣爪。機器人為安川公司的YASKAWA MOTOMAN MCL-50型機器人，有效載荷為50 kg，位置控制精度可達0.07 mm。單目相機為大恒MER-200-20G型號的工業相機，分辨率為1 268×1 236。3D相 機 型 號 為IDS Ensenso N10，其獲取的點云位置精度可達到0.1 mm。六維力傳感器即力/力矩（F/T）傳感器，選用的型號為ATI-FT26044，其沿x軸與y軸方向的力Fx，Fy的量程為660 N，分辨率為1/8 N，沿z軸方向的力Fz的量程為1 980 N，分辨率為1/4 N；力矩Tx，Ty，Tz的 量 程 為60 N·m，分 辨 率 為10/1 333 N·m。在實驗之前，已完成了單目相機和3D相機的手眼標定，六維力傳感器已進行標定和重力補償，且在軸零件末端圓心處采用五點法建立了工具坐標系。機器人已根據單目相機對軸零件的識別與粗定位完成抓取并移動到沉孔零件的上方，用3D相機拍攝沉孔零件的上端面獲得點云，因3D相機已完成手眼標定，故相機坐標系下的點云可直接轉換到機器人基坐標系下。

圖12 沉孔零件裝配平臺Fig.12 Platform of countersunk-hole part assembly

5.2 基于權重RANSAC的沉孔上平面估計

為驗證所提出的WRANSAC沉孔端面擬合算法在噪聲和離群點的干擾下的魯棒性，使用meshlab生成平面點云，其平面方程為z=0，即平面的參數A=0，B=0，C=-1，D=0，利用MATLAB為該點云添加零均值μ=0，標準差σ2=2的高斯噪聲，分別使用M樣本估計一致性［23］（M-Estimate Sample Consensus，MSAC）、最小二乘法［24］（Least Square，LS）、霍夫變換［25］（Hough Transform，HT）、隨 機 采 樣 一 致 性（RANSAC）與本文提出WRANSAC對生成的點云數據進行平面擬合，并計算模型內點到理想平面的平均距離Δd和擬合平面法線與理想平面法線的夾角Δθ，結果如圖13和表1所示。此外為驗證算法對離群點的魯棒性，在上述點云中加入15%的離群點，實驗的結果如圖14和表2所示。

圖13 平面擬合結果對比Fig.13 Comparison of plane fitting results

表1 平面擬合結果Tab.1 Comparison of plane fitting results

表1中，在沒有離群點的干擾時，本文方法WRANSAC估計的平面參數A=-0.023 3，B=-0.021 7，D=0.007 2，角 度 誤 差 達 到Δθ=1.823 6°，平均距離誤差Δd=0.093 4 mm，與MSAC，HT的估計結果相差不大，優于RANSAC和LS方法，其中HT的估計結果精度雖然也很高，但是HT的計算量太大，嚴重影響算法的速度。表2中，在加入15%的離群點后，本文方法估計的平面參數A=-0.024 1，B=-0.022 1，D=-0.136 6，估計結果受離群點的影響很小，角度誤差為Δθ=1.872 0°，平均距離誤差Δd=0.092 6 mm，而其他幾種方法的結果都受到一定的影響，LS和RANSAC所受影響最大，LS估計的平面模型參數B已經達到1.474 2，從圖14中可以看到LS估計的平面已經嚴重傾斜。通過以上仿真結果，可以看出本文方法對噪聲點和離群點具有良好的魯棒性。

圖14 離群點下平面擬合結果對比Fig.14 Comparison of plane fitting results under outliers

表2 離群點下平面擬合結果Tab.2 Comparison of plane fitting results under outliers

5.3 沉孔零件裝配實驗

使用圖12所示的機器人平臺，驗證所提出的3D視覺引導定位和阻抗控制下的柔順裝配策略在裝配中的有效性和可靠性。實驗中對4種不同位姿的沉孔進行位姿估計，每一種姿態下的沉孔進行10次實驗，取10次實驗的平均值作為最終估計值。

在裝配實驗時，機器人根據單目相機對軸零件識別與粗定位后抓取并移動到沉孔零件的上方，使用3D相機對沉孔的上端面進行拍攝獲得點云，并根據該點云對沉孔位姿進行估計。估計過程如圖15所示（彩圖見期刊電子版），沉孔位姿表示為(X，n)，①為原始點云P，②為去除部分離群點和噪聲后的點云Pc，③中紅色平面為WRANSAC平面擬合結果，即獲得n，④為沉孔內點篩選結果，⑤為沉孔輪廓圓擬合結果，即獲得X，至此完成整個沉孔的位姿(X，n)估計，將其表示成沉孔軸線與沉孔上端面的形式，如⑥所示。

圖15 沉孔位姿估計流程Fig.15 Process of countersunk hole pose estimation

由于空間中沉孔的真實姿態(X，n)無法獲得，為此本文提出，在軸孔阻抗控制完成時，使用最終軸的姿態作為理想沉孔位姿(X*，n*)，來計算4種不同位姿下的位姿估計均方根誤差（Root Mean Square Error，RMSE）：

圖16 裝配過程Fig.16 Assembly process

其中：θi為n與n*的角度差(°)，di為X與X*的距離差（mm），n為10，即進行了10組實驗。比較結果如表3所示，可以看出，4種姿態下，本文方法估計的沉孔位姿的距離均方根誤差小于0.5 mm，角度均方根誤差小于0.7°，估計結果穩定。

表3 不同姿態的誤差結果Tab.3 Error of different postures

根據估計的沉孔位姿，進行裝配，其中一次裝配過程中的力與力矩的變化如圖17所示，開始時刻，力與力矩幾乎為零，此時力和力矩未達到觸發阻抗控制的閾值，表明軸與孔無激烈的碰撞，軸可以準確平穩的裝入沉孔中，也從側面反映出本文提出的3D位姿估計算法位姿估計準確。在軸零件達到一定的深度之后，軸與沉孔內表面產生接觸，導致力和力矩發生突變，當達到閾值之后，阻抗控制開始作用，機器人自適應的調整末端執行器的姿態，避免產生過大的力和力矩，從圖17中可以看到，中間時刻，力和力矩突增，后在阻抗控制的調整下力和力矩逐漸減小。最后，當裝配完成時，力和力矩再次接近零。其中力反饋控制中的力響應閾值為10 N，力矩響應為1 N·m。

圖17 力和力矩Fig.17 Force and torque

至此，整個裝配過程完成。

6 結論

本文針對圓軸與沉孔的裝配問題，提出了一種3D視覺引導定位和阻抗控制下的柔順裝配策略。介紹了基于WRANSAC的沉孔上端面擬合的基本原理并與經典的平面擬合方法進行了對比實驗，實驗表明本文方法擬合出的平面的平均距離誤差Δd可以達到0.09 mm，角度誤差Δθ可以達到1.8°。同時也介紹了沉孔空間位姿獲取算法流程。實驗表明：本文提出的算法策略可以完成配合間隙為0.5 mm的沉孔裝配，滿足工程實施的要求。