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

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.3 沉孔圓擬合

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

則圓心為：

半徑為：

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

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

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

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

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

(X，n)。

4 軸孔零件的阻抗控制

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

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

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

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

5 實驗與結(jié)果

5.1 實驗條件

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

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

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

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

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

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

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

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

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

5.3 沉孔零件裝配實驗

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

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

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

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

圖16 裝配過程Fig.16 Assembly process

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

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

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

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

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

6 結(jié)論

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