基于V-REP的主動柔順式機器人軸孔裝配仿真

劉尚爭， 朱清慧， 劉群坡

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

0 引言

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

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

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

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

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

1.1 仿真裝配平臺

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

圖1 V-REP仿真裝配平臺

1.2 UR5機器人運動學分析

1.2.1 機器人D-H建模

機器人的每個連桿都可以用4個運動學參數(shù)來描述，兩個參數(shù)用于描述連桿本身，兩個參數(shù)用于描述連桿之間的關(guān)系，這種用連桿參數(shù)描述機構(gòu)運動關(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機器臂的連桿坐標系。

圖2 UR5機械臂連桿坐標系

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

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

1.2.2 機器人正運動學分析

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

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

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

故腕部坐標系{6}相對于坐標系{0}的變換矩陣為

(1)

1.2.3 機器人逆運動學分析

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

圖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)

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

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

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

(3)

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

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

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

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

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

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

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

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

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

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

2.3.1 一點式卡阻

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

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

圖10 軸孔裝配卡阻圖

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

2.3.2 兩點式卡阻

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

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

3.1 卡阻建模分析

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

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

圖12 兩點式卡阻

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

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

根據(jù)上圖可推導出B點坐標為(Xa,Ye,Ze)，D點坐標為(Xc,Ye,Ze)

則根據(jù)以上點坐標可推導出

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點在參考坐標系下坐標為

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

其中

3.1.2 工具坐標系與目標坐標系各軸夾角計算

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

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

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

β=∠AMA″

在直角△AA″M中

sinβ=AA″/AM

進而得

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

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

α=∠AMA′

在直角△AA′M中，

sinα=AA′/AM

進而得

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

(6)

3.2 基于動態(tài)自適應M點的運動學分析

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

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

d7=d+AM

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

由公式5得

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

其中L為工件長度。

則由變換矩陣定義可知，位于M點的坐標系{7}相對于坐標系{6}的變換矩陣

(7)

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

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

(8)

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

(9)

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

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

(10)

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

(11)

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

(12)

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

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

(13)

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

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

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

(14)

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

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

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

3.3 基于力反饋的自適應裝配流程

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

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

圖17 裝配流程圖

4 實驗仿真

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

圖18 工件入孔起始偏差角

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

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

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

圖21 裝配完成

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