陳平，雷學(xué)軍，李燦，胡義亮，陳爽

（重慶大學(xué) 機(jī)械與運(yùn)載工程學(xué)院，重慶 400044）

1 引言

隨著工業(yè)4.0時(shí)代的到來(lái)，智能化裝配受到廣泛的關(guān)注，吸引著大量的研究人員。其中裝配領(lǐng)域最為常見的便是軸與孔的裝配。隨著技術(shù)的發(fā)展，軸孔裝配的自動(dòng)化程度越來(lái)越高，但傳統(tǒng)的自動(dòng)化裝配無(wú)法滿足日益智能化、精密化的裝配要求，尤其是在軍工、光學(xué)產(chǎn)品裝配中。圓軸與沉孔是比較常見的配合零件，圓軸與沉孔零件的裝配面臨的關(guān)鍵問(wèn)題是：（1）如何估計(jì)沉孔零件的任意的空間位置和姿態(tài)；（2）如何將軸零件柔順地導(dǎo)入沉孔零件內(nèi)。

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

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

本文針對(duì)長(zhǎng)圓軸與沉孔的自動(dòng)裝配問(wèn)題，通過(guò)3D相機(jī)獲得沉孔的軸向點(diǎn)云，并針對(duì)軸向點(diǎn)云提出了沉孔空間位姿估計(jì)算法，同時(shí)融合阻抗控制的優(yōu)點(diǎn)，形成3D視覺(jué)和力覺(jué)的柔順裝配策略，實(shí)現(xiàn)圓軸與沉孔零件的柔順裝配，可提升裝配精度和裝配效率，適應(yīng)性更廣，并滿足穩(wěn)定可靠的要求。

2 系統(tǒng)組成及工作原理

2.1 自動(dòng)裝配系統(tǒng)的硬件構(gòu)成

圖1展示了機(jī)器人零件裝配平臺(tái)的組成。其中，六自由度機(jī)器人是整個(gè)系統(tǒng)的載體，3D相機(jī)和單目相機(jī)安裝于機(jī)器人末端法蘭盤，它們共同組成整個(gè)系統(tǒng)的視覺(jué)部分，可實(shí)現(xiàn)在抓取過(guò)程中對(duì)軸的識(shí)別、定位以及裝配過(guò)程中對(duì)沉孔的空間位姿估計(jì)。六維力傳感器安裝于機(jī)器人末端法蘭盤與末端執(zhí)行氣爪之間，可實(shí)時(shí)監(jiān)測(cè)裝配過(guò)程中的力/力矩變化，從而引導(dǎo)機(jī)器人的移動(dòng)，達(dá)到柔順裝配的目的。

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

圖1 機(jī)器人零件裝配平臺(tái)Fig.1 Platform of robotic assembly

2.2 自動(dòng)裝配系統(tǒng)的工作原理

應(yīng)用該系統(tǒng)進(jìn)行圓軸與沉孔裝配的主要步驟如下：

（1）機(jī)器人移動(dòng)至待裝配圓軸零件工作臺(tái)上方，利用單目相機(jī)，通過(guò)基于深度學(xué)習(xí)的目標(biāo)檢測(cè)方法完成對(duì)軸零件的識(shí)別與定位，最后完成抓取；

（2）完成抓取后，機(jī)器人移動(dòng)至沉孔零件工作臺(tái)上方，使用3D相機(jī)完成對(duì)沉孔零件上端面點(diǎn)云數(shù)據(jù)的獲取；

（3）通過(guò)沉孔位姿估計(jì)關(guān)鍵算法獲取沉孔零件的空間位姿；

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

本文將使用預(yù)訓(xùn)練的網(wǎng)絡(luò)YOLOv3來(lái)完成步驟（1）中對(duì)圓軸零件進(jìn)行檢測(cè)的任務(wù)，因?yàn)閅OLOv3已經(jīng)有較成熟的應(yīng)用技術(shù)，在本文中將不再進(jìn)行敘述。本文聚焦于步驟（3）和（4）的敘述，即沉孔位姿估計(jì)算法和阻抗控制。

3 沉孔位姿估計(jì)關(guān)鍵算法

沉孔的空間位姿可表示為(X，n)，其中X表示沉孔上端面的圓孔圓心，n表示沉孔上端面平面法線。當(dāng)獲得沉孔上端面點(diǎn)云之后，通過(guò)預(yù)處理去除點(diǎn)云數(shù)據(jù)中的離群點(diǎn)和噪聲。對(duì)沉孔上端面的平面擬合可獲得其平面參數(shù)，進(jìn)而獲得n。而X可利用沉孔輪廓的圓擬合來(lái)估計(jì)。

3.1 點(diǎn)云數(shù)據(jù)預(yù)處理

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

圖2 點(diǎn)云離群點(diǎn)和噪聲Fig.2 Outliers and noise of point cloud

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

圖3 超體素分割與CPC結(jié)果對(duì)比Fig.3 Results of supervoxel segmentation and CPC clustering

圖4 超體素分割結(jié)果Fig.4 Results of supervoxel segmentation

3.2 基于權(quán)重RANSAC的沉孔上端面擬合

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

圖5 邊緣離群點(diǎn)和噪聲點(diǎn)Fig.5 Outliers and noise of the edge

WRANSAC）平面擬合方法來(lái)獲得沉孔的上端面。因?yàn)閭鹘y(tǒng)RANSAC的平面擬合方法中，點(diǎn)云中的每個(gè)點(diǎn)被選中的概率是相同的，但是點(diǎn)云邊緣點(diǎn)誤差太大，會(huì)影響平面擬合的精度，為改進(jìn)傳統(tǒng)的RANSAC方法，對(duì)鄰近沉孔中心的點(diǎn)賦予大權(quán)重，而遠(yuǎn)離沉孔的邊緣點(diǎn)賦予小權(quán)重，來(lái)減小邊緣點(diǎn)的影響，如圖6所示，權(quán)重函數(shù)如式（1），式（2）所示：

圖6 權(quán)重函數(shù)示意圖Fig.6 Schematic diagram of the weight function

其中：di為點(diǎn)云中的一點(diǎn)pi到重心的距離，k為比例系數(shù)，η為標(biāo)準(zhǔn)差，本文取k=1/10 000，η=4。

為從擬合出的眾多平面模型中篩選出最佳平面，設(shè)計(jì)如式（3）所示的評(píng)價(jià)公式：

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

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

（1）根據(jù)公式（1）計(jì)算點(diǎn)云P（c經(jīng)過(guò)預(yù)處理后的點(diǎn)云）中每個(gè)點(diǎn)的概率ωi；

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

（3）計(jì)算點(diǎn)云Pc中的每個(gè)點(diǎn)pci與平面模型的距離，將距離小于閾值β的點(diǎn)記錄在點(diǎn)集Pm（內(nèi)點(diǎn)集）中；

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

（5）重復(fù)步驟（2）～（4），達(dá)到最大迭代次數(shù)（本文設(shè)置的迭代次數(shù)k=100）則停止。

選取分?jǐn)?shù)Sw最大的估計(jì)作為最終的平面擬合模型：Ax+By+Cz+D=0，擬合的平面如圖7所示的P平面。

圖7 平面擬合結(jié)果Fig.7 Results of plane fitting

3.3 沉孔圓擬合

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

3D相機(jī)獲得的點(diǎn)云如圖8所示（彩圖見期刊電子版），沉孔輪廓并非為圓（如圖中紅色實(shí)線），輪廓應(yīng)為圖中紅色虛線所示，這是因?yàn)?D相機(jī)在掃描沉孔的時(shí)候，會(huì)在沉孔圓內(nèi)生成一些無(wú)關(guān)的點(diǎn)云數(shù)據(jù)（沉孔內(nèi)點(diǎn)），如圖8紅色實(shí)線與虛線內(nèi)的點(diǎn)，為了提取出沉孔的輪廓，完成輪廓的圓擬合，需要將沉孔內(nèi)點(diǎn)分割出來(lái)，然后再提取出沉孔輪廓進(jìn)行圓擬合，以完成對(duì)X的估計(jì)。

圖8 點(diǎn)云數(shù)據(jù)Fig.8 Point cloud

點(diǎn)云數(shù)據(jù)在3D相機(jī)獲取的過(guò)程中會(huì)根據(jù)深度不同賦予不同的RGB值。在經(jīng)過(guò)沉孔輪廓時(shí)點(diǎn)云數(shù)據(jù)的法線方向會(huì)發(fā)生改變，屬于上端面的點(diǎn)云法線應(yīng)與端面法線方向一致，而沉孔內(nèi)點(diǎn)的法線方向會(huì)因深度的不同發(fā)生偏移。據(jù)此，利用結(jié)合顏色和法線信息的權(quán)重函數(shù)，來(lái)完成對(duì)沉孔內(nèi)點(diǎn)的分割。

權(quán)重函數(shù)如式（4）所示：

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

圖9 沉孔內(nèi)點(diǎn)Fig.9 Inliers of countersunk-hole

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

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

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

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

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

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

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

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

則圓心為：

半徑為：

將點(diǎn)集Pcircle表示為矩陣形式為：

其中zi=x2i+yi2，xi，yi為點(diǎn)集Pcircle中點(diǎn)的x，y坐標(biāo)。定義矩陣M為：

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

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

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

(X，n)。

4 軸孔零件的阻抗控制

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

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

軸零件插入沉孔的過(guò)程中，當(dāng)軸與孔相互作用時(shí)，可建立二階線性［13］方程：

其 中：e=(ex，ey，ez)是 末 端 執(zhí) 行 器 的 位 置 修 正量，F(xiàn)是來(lái)自環(huán)境的外力，Md，Bd，Kd分別為期望慣性矩陣、期望阻尼矩陣和期望剛度矩陣。阻抗控制過(guò)程中，根據(jù)式（18）可以根據(jù)力偏差計(jì)算位置修正量e，通過(guò)e對(duì)機(jī)器人末端執(zhí)行器的姿態(tài)持續(xù)調(diào)整，以避免產(chǎn)生卡阻，實(shí)現(xiàn)柔順裝配。

5 實(shí)驗(yàn)與結(jié)果

5.1 實(shí)驗(yàn)條件

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

圖12 沉孔零件裝配平臺(tái)Fig.12 Platform of countersunk-hole part assembly

5.2 基于權(quán)重RANSAC的沉孔上平面估計(jì)

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

圖13 平面擬合結(jié)果對(duì)比Fig.13 Comparison of plane fitting results

表1 平面擬合結(jié)果Tab.1 Comparison of plane fitting results

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

圖14 離群點(diǎn)下平面擬合結(jié)果對(duì)比Fig.14 Comparison of plane fitting results under outliers

表2 離群點(diǎn)下平面擬合結(jié)果Tab.2 Comparison of plane fitting results under outliers

5.3 沉孔零件裝配實(shí)驗(yàn)

使用圖12所示的機(jī)器人平臺(tái)，驗(yàn)證所提出的3D視覺(jué)引導(dǎo)定位和阻抗控制下的柔順裝配策略在裝配中的有效性和可靠性。實(shí)驗(yàn)中對(duì)4種不同位姿的沉孔進(jìn)行位姿估計(jì)，每一種姿態(tài)下的沉孔進(jìn)行10次實(shí)驗(yàn)，取10次實(shí)驗(yàn)的平均值作為最終估計(jì)值。

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

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

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

圖16 裝配過(guò)程Fig.16 Assembly process

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

表3 不同姿態(tài)的誤差結(jié)果Tab.3 Error of different postures

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

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

至此，整個(gè)裝配過(guò)程完成。

6 結(jié)論

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