劉尚爭， 朱清慧， 劉群坡

(1.南陽理工學(xué)院智能制造學(xué)院 河南 南陽 473004；2.河南理工大學(xué)電氣工程與自動化學(xué)院 河南 焦作 454000)

0 引言

裝配在機(jī)械制造工業(yè)中占有較大比重[1]，隨著工業(yè)機(jī)器人的廣泛應(yīng)用，利用機(jī)器人進(jìn)行軸孔裝配的場景越來越多，然而在軸孔裝配作業(yè)的過程中出現(xiàn)的卡阻現(xiàn)象會對裝配工件甚至機(jī)器人造成損害[2]。為解決軸孔自動裝配中的卡阻問題，確保裝配過程順利進(jìn)行，提出一種主動柔順裝配的方法進(jìn)行去卡阻，使軸孔裝配作業(yè)可以平穩(wěn)順利地進(jìn)行。

在主動柔順裝配方面，國內(nèi)學(xué)者李煥[3]提出基于阻抗控制的機(jī)械臂末端工具的柔順控制，以腕力傳感器反饋值為依據(jù)進(jìn)行相應(yīng)調(diào)整，將其控制在一定范圍內(nèi)，對精細(xì)操作具有指導(dǎo)意義。再有學(xué)者王小濤[4]設(shè)計(jì)出一種空間五指靈巧手單指力柔順控制系統(tǒng)，在對接過程中采用阻抗控制方法進(jìn)行接觸力和位置控制，可快速跟蹤手指關(guān)節(jié)位置以及精確控制腱張力的大小。

本文針對6自由度機(jī)器人軸孔裝配過程中經(jīng)常遇到的卡阻問題，設(shè)計(jì)了一種主動柔順裝配方法。為模擬機(jī)器人在實(shí)際的軸孔裝配中遇到的卡阻現(xiàn)象，在V-REP(virtual robot experimentation platform)仿真平臺下搭建仿真環(huán)境并進(jìn)行了實(shí)驗(yàn)驗(yàn)證，證明了該主動柔順裝配策略理論上的正確性與可行性[5-7]。

本文主要研究內(nèi)容包括：對機(jī)器人正逆運(yùn)動學(xué)的分析、軸孔裝配理論分析、主動柔順裝配控制系統(tǒng)的設(shè)計(jì)以及基于V-REP的主動柔順裝配仿真[8,9]。

1 軸孔裝配系統(tǒng)模型建立

1.1 仿真裝配平臺

如圖1所示，軸孔裝配的系統(tǒng)主要由UR5機(jī)器人、六維力傳感器、待裝配工件和工件孔組成。為簡化仿真，在裝配平臺中未添加夾具，將工件直接連接在六維力傳感器上。

圖1 V-REP仿真裝配平臺

1.2 UR5機(jī)器人運(yùn)動學(xué)分析

1.2.1 機(jī)器人D-H建模

機(jī)器人的每個連桿都可以用4個運(yùn)動學(xué)參數(shù)來描述，兩個參數(shù)用于描述連桿本身，兩個參數(shù)用于描述連桿之間的關(guān)系，這種用連桿參數(shù)描述機(jī)構(gòu)運(yùn)動關(guān)系的規(guī)則稱為D-H(Denavit-Hartenberg)方法[10]。并有給出連桿參數(shù)定義如下：ai為沿著Xi軸，從Zi軸移動至Zi+1軸的距離；αi為圍繞Xi軸，從Zi軸旋轉(zhuǎn)至Zi+1軸的角度；di為沿著Zi軸，從Xi-1軸移動至Xi軸的距離；θi為圍繞Zi軸，從Xi-1軸旋轉(zhuǎn)至Xi軸的角度。據(jù)此，建立如圖2所示的UR5機(jī)器臂的連桿坐標(biāo)系。

圖2 UR5機(jī)械臂連桿坐標(biāo)系

根據(jù)圖2機(jī)械臂各連桿坐標(biāo)的相對位置，可得到UR5機(jī)械臂的D-H參數(shù)表如表1所示。

表1 UR5機(jī)械臂D-H參數(shù)表

1.2.2 機(jī)器人正運(yùn)動學(xué)分析

在已知機(jī)械臂當(dāng)前各關(guān)節(jié)角值的情況下，則可通過對機(jī)械臂正運(yùn)動學(xué)的分析得到機(jī)械臂末端工具的位置和姿態(tài)。下面對機(jī)械臂末端坐標(biāo)系{6}到固定坐標(biāo)系{0}的變換矩陣進(jìn)行推導(dǎo)[11,12]。

rot(xi,ai)·trans(xi,αi)

代入UR5機(jī)械臂DH參數(shù)表中各參數(shù)可得到各連桿變換矩陣

故腕部坐標(biāo)系{6}相對于坐標(biāo)系{0}的變換矩陣為

(1)

1.2.3 機(jī)器人逆運(yùn)動學(xué)分析

θ2=atan2(s2,c2)

θ3=±arccos(m2+n2-a22-a32/2a2a3)

θ4=atan2(t1,t2)-θ2-θ3

θ5=±arccos(axs1-ayc1)

θ6=atan2(m/s5,n/s5)

其中

m=d6ay-py；n=axd6-px；

m2+n2-d42≥0；axs1-ayc1≤1；

s2=(a3c3+a2)n-a3s3m/a22+a32+2a2a3c3；

c2=m+a3s2s3/a3c3+a2；

s5≠0；m2+n2≤(a2+a3)2；

t1=-s6(nxc1+nys1)-c6(oxc1+oys1)；

t2=ozc6+nzs6；

2 軸孔裝配原理分析

2.1 工件裝配條件分析

軸孔裝配過程共有兩步，尋孔階段和入孔之后的姿態(tài)調(diào)整階段。理想情況下的工件入孔情況如圖3所示，工件與軸孔既沒有位置偏差也沒有姿態(tài)偏差，工件中心軸線與軸孔中心軸線完全重合，在整個裝配過程中，工件與軸孔之間沒有接觸力或接觸力在一個合理的范圍內(nèi)，則不需調(diào)整姿態(tài)。

圖3 理想情況下工件入孔示意圖

在現(xiàn)實(shí)操作環(huán)境中，由于機(jī)器人的精度誤差、測量誤差等，裝配過程中會出現(xiàn)工件中心軸線與軸孔中心軸線不完全重合的情況。若工件中心軸線與軸孔中心軸線偏差角較小(如圖4和圖5所示)，在整個裝配過程中，機(jī)械臂無需調(diào)整姿態(tài)仍可順利入孔。但當(dāng)該偏差角達(dá)到某一特定值時，若不調(diào)整工件姿態(tài)，在裝配過程中會產(chǎn)生卡阻，工件將無法順利入孔，并會對工件產(chǎn)生一定的磨損，嚴(yán)重時會損壞機(jī)械臂。

圖4 恰能入孔的極限情況

圖5 恰能無卡阻入孔的建模分析

設(shè)軸孔深度AB為h，工件直徑CE為d，軸孔直徑BE為Q，工件中心軸線與軸孔中心軸線所形成的傾斜角為θ。

由幾何關(guān)系可知

BD=h·tanθ

DE=d·cosθ

由BD+DE=BE可知

h·tanθ+d·cosθ=Q

(2)

在實(shí)際裝配環(huán)境中，軸孔深度、工件直徑及軸孔直徑均為已知條件，代入公式中即可求得工件恰能無卡阻入孔的最大偏差角θ。

圖6 工件不能入孔臨界情況

圖6為工件不能入孔的臨界情況，可以看到工件的一側(cè)底角已經(jīng)入孔，而另一側(cè)底角還卡在軸孔邊緣，此時工件與軸孔軸線的夾角為α，對該夾角進(jìn)行計(jì)算，由圖中的幾何關(guān)系可知

(3)

當(dāng)工件與軸孔軸線的偏差角大于α?xí)r，工件不能入孔，此時機(jī)械臂需要重新定位軸孔入口處圓心的位置，并通過旋轉(zhuǎn)工件減小工件與軸孔軸線的偏差角，使工件可以順利入孔。

2.2 工件與軸孔各接觸狀態(tài)分析

在軸孔裝配過程中，經(jīng)常遇到的情況是圖7中(a)和(b)兩種情況，其中圖(a)為一點(diǎn)式卡阻的情形，圖(b)為兩點(diǎn)式卡阻的情形，此時必須要調(diào)整工件姿態(tài)才能保證裝配過程順利進(jìn)行，使工件順時針或逆時針旋轉(zhuǎn)，盡量使工件中心軸線與軸孔中心軸線重合，有兩種可能情況。

圖7 常見的軸孔裝配卡阻現(xiàn)象

1) 旋轉(zhuǎn)機(jī)械臂末端夾具

由圖8可知，只旋轉(zhuǎn)機(jī)械臂末端夾具的方案是行不通的，調(diào)整后工件左側(cè)會與軸孔內(nèi)壁發(fā)生碰撞，進(jìn)而損壞工件或機(jī)械臂。

圖8 僅旋轉(zhuǎn)機(jī)械臂末端夾具

2)以工件自身某一點(diǎn)為旋轉(zhuǎn)中心進(jìn)行旋轉(zhuǎn)

由圖9中可以看出，繞工件自身m點(diǎn)進(jìn)行旋轉(zhuǎn)后，工件不會與軸孔內(nèi)壁發(fā)生碰撞，并且可以實(shí)現(xiàn)工件中心軸線與軸孔中心軸線重合。調(diào)整前后機(jī)械臂末端夾具的姿態(tài)與位置均發(fā)生了變化。

2.3 卡阻姿態(tài)分析

2.3.1 一點(diǎn)式卡阻

如圖10所示，裝配過程中出現(xiàn)的一點(diǎn)式卡阻現(xiàn)象，也是在裝配過程中最容易出現(xiàn)的卡阻情況，工件在軸孔內(nèi)任意的卡阻現(xiàn)象都可以分解到力傳感器坐標(biāo)系的XOY和XOZ兩個平面，如圖11所示XOY坐標(biāo)系的力分解，XOZ平面類似。

圖9 以工件的m點(diǎn)為中心進(jìn)行旋轉(zhuǎn)

圖10 軸孔裝配卡阻圖

圖11 在XOY平面的卡阻現(xiàn)象

2.3.2 兩點(diǎn)式卡阻

當(dāng)工件裝配形成如圖12所示的兩點(diǎn)式卡阻時，軸孔對工件的反作用力F2必然大于F1。此時類似一點(diǎn)式卡阻現(xiàn)象，可將工件旋轉(zhuǎn)后再盡行裝配，最終會變?yōu)槿鐖D11所示的一點(diǎn)式卡阻現(xiàn)象。由此可以看出，兩點(diǎn)式卡阻現(xiàn)象是一點(diǎn)式卡阻現(xiàn)象的特殊情況，仍可按照一點(diǎn)式卡阻情況進(jìn)行分析。

3 主動柔順裝配系統(tǒng)設(shè)計(jì)

3.1 卡阻建模分析

3.1.1 自適應(yīng)姿態(tài)調(diào)整中心點(diǎn)建模

三維空間軸孔裝配過程中發(fā)生卡阻現(xiàn)象時的一個場景如圖12所示。對該卡阻現(xiàn)象進(jìn)行建模分析，如圖13所示。其中A、C兩點(diǎn)分別為工件中心軸線與其上下底面的交點(diǎn)，B、E兩點(diǎn)所在直線為軸孔中心軸線，M點(diǎn)為工件中心軸線與軸孔中心軸線的交點(diǎn)，力傳感器坐標(biāo)系與工具坐標(biāo)系姿態(tài)相同。

圖12 兩點(diǎn)式卡阻

圖13 工件卡阻現(xiàn)象的平面建模分析

以世界坐標(biāo)系為參考坐標(biāo)系,已知A點(diǎn)坐標(biāo)為(Xa,Ya,Za)，E點(diǎn)坐標(biāo)為(Xe,Ye,Ze)，C點(diǎn)坐標(biāo)為(Xc,Yc,Zc)，工件長度AC=L，AB為A點(diǎn)到軸孔中心軸線的垂線距離，DC為C點(diǎn)到軸孔中心軸線的垂線距離。

根據(jù)上圖可推導(dǎo)出B點(diǎn)坐標(biāo)為(Xa,Ye,Ze)，D點(diǎn)坐標(biāo)為(Xc,Ye,Ze)

則根據(jù)以上點(diǎn)坐標(biāo)可推導(dǎo)出

BD=Xa-Xc

由幾何關(guān)系可知，△ABM相似于△CDM，故可得到如下關(guān)系式

AB/DC=BM/MD=SM/MC=k1

BM+MD=BD

得

BM=(k1/1+k1)(Xa-Xc)

(4)

AM=L(k1/1+k1)

(5)

M點(diǎn)在參考坐標(biāo)系下坐標(biāo)為

(Xa-(k1/1+k1)(Xa-Xc)，Ye,Ze)

其中

3.1.2 工具坐標(biāo)系與目標(biāo)坐標(biāo)系各軸夾角計(jì)算

圖14為工件卡阻現(xiàn)象的空間建模分析，其中AM所在直線為工件中心軸線，BM所在直線為軸孔中心軸線，A′M為AM在A′BMM′平面上的投影，A″M為AM在A″BM平面上的投影。工具坐標(biāo)系{0}與目標(biāo)坐標(biāo)系{7}均位于M點(diǎn)，若要工件中心軸線與軸孔中心軸線完全重合，則需要將旋轉(zhuǎn)中心M點(diǎn)坐標(biāo)系{7}的姿態(tài)調(diào)整到與目標(biāo)坐標(biāo)系{0}的姿態(tài)完全一致。

圖14 工件卡阻現(xiàn)象的空間建模分析

設(shè)工具坐標(biāo)系與目標(biāo)坐標(biāo)系的Z軸夾角為β角，由圖14可以看出

β=∠AMA″

在直角△AA″M中

sinβ=AA″/AM

進(jìn)而得

β=arcsin(|Za-Ze|·L·(1+k1)/k1)

設(shè)工具坐標(biāo)系與目標(biāo)坐標(biāo)系的Y軸夾角為α角，由圖14可以看出

α=∠AMA′

在直角△AA′M中，

sinα=AA′/AM

進(jìn)而得

α=arcsin(|Ya-Ye|·L·(1+k1)/k1)

(6)

3.2 基于動態(tài)自適應(yīng)M點(diǎn)的運(yùn)動學(xué)分析

工件入孔出現(xiàn)卡阻情況時，應(yīng)調(diào)整機(jī)械臂使工件分別繞著旋轉(zhuǎn)中心M點(diǎn)的Y軸和Z軸旋轉(zhuǎn)一定角度。因此需要根據(jù)工件旋轉(zhuǎn)中心M點(diǎn)的期望位置和當(dāng)前姿態(tài)信息，通過逆運(yùn)動學(xué)解析出機(jī)械臂調(diào)整后的各關(guān)節(jié)角度大小。

設(shè)工具坐標(biāo)系位于M點(diǎn)處，坐標(biāo)系{6}與旋轉(zhuǎn)點(diǎn)M之間的關(guān)系如圖15所示。機(jī)械臂關(guān)節(jié)6到工件上底面圓心點(diǎn)A點(diǎn)的距離可測，設(shè)該距離已知為d，則坐標(biāo)系{6}與工件上旋轉(zhuǎn)中心點(diǎn) M點(diǎn)的距離為

d7=d+AM

圖15 坐標(biāo)系{6}與工具坐標(biāo)系{7}之間的關(guān)系

由公式5得

d7=d+L(k1/1+k1)

其中L為工件長度。

則由變換矩陣定義可知，位于M點(diǎn)的坐標(biāo)系{7}相對于坐標(biāo)系{6}的變換矩陣

(7)

3.2.2 M點(diǎn)坐標(biāo)系旋轉(zhuǎn)前后的變換矩陣

工件旋轉(zhuǎn)前后，M點(diǎn)的位置不變，僅姿態(tài)發(fā)生了變化，設(shè)M點(diǎn)旋轉(zhuǎn)前的坐標(biāo)系為{7}，旋轉(zhuǎn)后的坐標(biāo)系為{7 ′}。若力矩值My>0,工件應(yīng)繞旋轉(zhuǎn)中心點(diǎn)M的Y軸逆時針旋轉(zhuǎn)ε角,則坐標(biāo)系為{7 ′}由坐標(biāo)系{7}繞自身Y軸旋轉(zhuǎn)-ε角得到,則變換矩陣為

(8)

若力矩值My<0，工件應(yīng)繞旋轉(zhuǎn)中心點(diǎn)M的Y軸順時針旋轉(zhuǎn)ε角，則坐標(biāo)系為{7 ′}即為坐標(biāo)系{7}繞自身Y軸旋轉(zhuǎn)ε角，則變換矩陣為

(9)

ε角有兩種選擇方案：(1)ε角每次選定為某一固定角；(2)ε每次以工具坐標(biāo)系與目標(biāo)坐標(biāo)系的Z軸夾角β角為參考標(biāo)準(zhǔn)進(jìn)行取值

ε=kβ=k·arcsin(|Za-Ze|·L·(1+k1)/k1)

(10)

若力矩值Mz>0,工件應(yīng)繞旋轉(zhuǎn)中心點(diǎn)M的Z軸順時針旋轉(zhuǎn)η角,則坐標(biāo)系{7 ′}即為坐標(biāo)系{7}繞自身Y軸旋轉(zhuǎn)η角,則變換矩陣為

(11)

若力矩值Mz<0,工件應(yīng)繞旋轉(zhuǎn)中心點(diǎn)M的Z軸逆時針旋轉(zhuǎn)η角,則坐標(biāo)系{7 ′}即為坐標(biāo)系{7}繞自身Y軸旋轉(zhuǎn)-η角,則變換矩陣為

(12)

η角有兩種選擇方案：(1)η角每次選定為某一固定角；(2)η每次以工具坐標(biāo)系與目標(biāo)坐標(biāo)系的Y軸夾角α角為參考標(biāo)準(zhǔn)進(jìn)行取值

η=kα=k·arcsin(|Ya-Ye|·L·(1+k1)/k1)

(13)

如圖16所示，工件旋轉(zhuǎn)前機(jī)械臂末端連桿坐標(biāo)系及M點(diǎn)坐標(biāo)系分別為{6}和{7}，旋轉(zhuǎn)后機(jī)械臂末端連桿坐標(biāo)系及M點(diǎn)坐標(biāo)系分別為{6′}和{7′}。根據(jù)機(jī)械臂正運(yùn)動學(xué)分析可知

圖16 工件旋轉(zhuǎn)前后機(jī)械臂末端連桿坐標(biāo)系{6}及M點(diǎn)坐標(biāo)系{7}的變換

由此即可計(jì)算出機(jī)械臂調(diào)整后的坐標(biāo)系{6′}到坐標(biāo)系{0}之間的變換矩陣為

(14)

根據(jù)機(jī)械臂逆運(yùn)動學(xué)解析公式即可求出機(jī)械臂調(diào)整后的各關(guān)節(jié)角度。

當(dāng)力矩反饋值My小于0時，工件應(yīng)繞旋轉(zhuǎn)中心點(diǎn)M的Y軸順時針旋轉(zhuǎn)ε角。假設(shè)當(dāng)前機(jī)械臂各關(guān)節(jié)角分別為θ1、θ2、θ3、θ4、θ5、θ6，機(jī)械臂關(guān)節(jié)6到工件上底面圓心點(diǎn)的距離為d，工件長度為L，則根據(jù)公式(14)可得

由逆運(yùn)動學(xué)解析公式即可求出機(jī)械臂調(diào)整后的各關(guān)節(jié)角度。其中：s234表示sin(θ2+θ3+θ4)，c234表示cos(θ2+θ3+θ4)，其余以此類推。

3.3 基于力反饋的自適應(yīng)裝配流程

在裝配過程中，實(shí)時獲取機(jī)械臂末端的六維力傳感器各個方向的力矩值。正常裝配過程中，六維力傳感器的3個方向的力矩值會有一個最大值，則設(shè)置力矩閾值稍大于正常裝配過程的最大力矩值，若3個方向的力矩值均不超過該閾值，則工件繼續(xù)沿著軸孔前進(jìn)；若力矩值超過閾值，證明此時出現(xiàn)了卡阻現(xiàn)象，則轉(zhuǎn)去執(zhí)行去卡阻方案[13,14]。

卡阻現(xiàn)象發(fā)生時，首先計(jì)算出工具旋轉(zhuǎn)中心M點(diǎn)，然后判斷Y軸力矩是否超限，若超限則繼續(xù)判斷力矩的方向。若力矩大于零則應(yīng)使工件繞M點(diǎn)Y軸處順時針旋轉(zhuǎn)一定角度，若力矩小于零則應(yīng)使工件繞M點(diǎn)Y軸處逆時針旋轉(zhuǎn)一定角度。接下來判斷Z軸力矩是否超限，超限則使工件在M點(diǎn)處沿自身坐標(biāo)系Z軸旋轉(zhuǎn)一定角度，旋轉(zhuǎn)方向也由力矩的正負(fù)決定。若力矩為正則逆時針旋轉(zhuǎn)，反之則順時針旋轉(zhuǎn)。此時讀取傳感器的力矩反饋值，若已成功消除卡阻則工件繼續(xù)執(zhí)行入孔作業(yè)，否則繼續(xù)調(diào)整至無卡阻狀態(tài)再進(jìn)行裝配。通過將工件與軸孔接觸時六維力傳感器的力矩反饋值控制在一個合理的范圍內(nèi)，就可以保證機(jī)械臂平穩(wěn)順利地完成裝配任務(wù)，裝配流程如圖17所示。其中調(diào)整角度的計(jì)算以工件當(dāng)前工具坐標(biāo)系與目標(biāo)坐標(biāo)系之間的偏差角為參考標(biāo)準(zhǔn)來動態(tài)調(diào)整工件旋轉(zhuǎn)角度。當(dāng)偏差角較大時使工件以一個較大的角度旋轉(zhuǎn)，從而減少調(diào)節(jié)次數(shù)。當(dāng)偏差角較小時就適當(dāng)減小工件旋轉(zhuǎn)角度，從而避免出現(xiàn)超調(diào)現(xiàn)象。

圖17 裝配流程圖

4 實(shí)驗(yàn)仿真

在仿真中首先設(shè)定起始工作環(huán)境，使在裝配過程中出現(xiàn)卡阻，如圖18所示設(shè)定工件入孔夾角為30°時。

圖18 工件入孔起始偏差角

機(jī)械臂根據(jù)力傳感器反饋力矩值大小自適應(yīng)調(diào)節(jié)裝配過程，如圖19所示，在裝配過程中，機(jī)械臂調(diào)整兩次即可令工件與軸孔中心軸線重合。由圖20所示的機(jī)器人各個關(guān)節(jié)角值易可看出兩次調(diào)整平滑且流暢。最終實(shí)現(xiàn)了工件的裝配如圖21所示。

圖19 力傳感器X、Y、Z方向受力變化

圖20 各關(guān)節(jié)角度變化

圖21 裝配完成

由仿真可以看出，該主動柔順式裝配方案在裝配過程中機(jī)械臂的調(diào)節(jié)次數(shù)較少，縮短了機(jī)器人的軸孔裝配時間，且裝配過程柔順。