徐建明,楊冰冰

(浙江工業(yè)大學(xué) 信息工程學(xué)院,浙江 杭州 310023)

冗余機器人是指主動關(guān)節(jié)的自由度數(shù)目大于任務(wù)所需求的維度的機器人。對于軸孔裝配實驗,在機械臂末端位姿給定的條件下,有多種關(guān)節(jié)位置和連桿位形對應(yīng)此末端位姿,使得機械臂更加靈活。利用機器人的冗余性可以改善機器人的運動學(xué)和動力學(xué)性能,使機器人在保證完成給定末端任務(wù)的同時,對系統(tǒng)的其他各項性能指標進行優(yōu)化[1-2]。運動學(xué)性能改善常見的指標有增強可操作度、避關(guān)節(jié)極限和避奇異位形等。另外,冗余度機器人動力學(xué)優(yōu)化的一個重要性能指標是為了防止機器人關(guān)節(jié)力矩的過載,而關(guān)節(jié)力矩合理分配對目標進行優(yōu)化是十分重要的。針對這方面的研究,國內(nèi)外已有較豐富的研究成果。郭立新等[3]分析冗余機器人運動學(xué)的最小關(guān)節(jié)力矩法,對空間4R冗余度機器人關(guān)節(jié)軌跡的運動規(guī)劃進行了分析仿真。張威等[4]在速度水平上利用冗余特性進行動力學(xué)優(yōu)化,同時綜合了運動學(xué)的算法,經(jīng)過仿真驗證了算法的可靠性。Hollerbach等[5]通過確定關(guān)節(jié)零空間矢量,得出具有最小關(guān)節(jié)力矩的關(guān)節(jié)表達式。Shen等[6]根據(jù)雅可比矩陣的零空間的優(yōu)化的方式,研究了避碰撞的方法。相較于單臂機器人,雙臂機器人的優(yōu)勢更加突出。在軸孔裝配任務(wù)方面,雙臂機器人比單臂機器人更加靈巧,為了使軸孔完成裝配,雙臂機器人兩個手臂共同完成軌跡的規(guī)劃,而單臂機器人僅依靠單只手臂進行軌跡規(guī)劃,由于單臂機器人在操作能力、控制等方面的局限性,雙臂機器人的研究逐漸成為熱點。Lee等[7]使用兩個單臂機器人建立軸孔裝配系統(tǒng),在線生成一種雙臂無碰撞軌跡的算法,完成軸孔裝配的任務(wù)。張溯[8]針對雙臂系統(tǒng)的協(xié)作問題,建立了雙臂機器人軸孔裝配的運動學(xué)方程,在Matlab中對軸孔裝配的任務(wù)的各個階段進行了仿真,得到兩個手臂末端協(xié)調(diào)操作的運動軌跡,在SolidWorks中建立冗余雙臂機器人模型并進行實驗驗證。蘇越[9]研究了雙臂協(xié)調(diào)操作的運動規(guī)劃和柔順控制方法。陳漫勛[10]研究了標定的方法,通過Baxter雙臂機器人進行協(xié)調(diào)控制,實現(xiàn)了軸孔裝配的實驗。周軍等[11]分析了冗余度雙臂機器人進行軸孔裝配的運動學(xué)約束關(guān)系,通過三維仿真在冗余雙臂機器人協(xié)調(diào)控制平臺上進行了實驗。Zhang等[12]針對柔順雙臂機器人定位不準確的問題,提出了一種兩階段裝配方案(孔外裝配階段和孔內(nèi)裝配階段),實驗驗證了該裝配方案的有效性和高效性。但是,在機器人軸孔裝配任務(wù)中,剛性機器人會損壞工件,而柔性機器人定位精度較低,且在目前研究軸孔裝配的尋孔階段大都采用隨機搜孔,易導(dǎo)致尋孔效率低。關(guān)于冗余性,目前的研究大都是單一地從運動學(xué)或動力學(xué)角度進行優(yōu)化,針對兩種優(yōu)化方式結(jié)合的研究較匱乏,已有的運動學(xué)和動力學(xué)結(jié)合的方式也僅是針對避關(guān)節(jié)奇異和關(guān)節(jié)力矩進行研究,較為單一。

基于以上分析,利用Baxter柔性雙臂機器人末端受力分析軸孔接觸的狀態(tài),進而研究裝配策略,結(jié)合關(guān)節(jié)限制和最小關(guān)節(jié)力矩優(yōu)化冗余逆解,開展對雙臂機器人軸孔裝配任務(wù)的研究。首先,針對剛性機器人會損壞工件問題,采用柔性機器人進行軸孔裝配研究;其次,針對尋孔效率低、柔性機器人定位精度低的問題,設(shè)計兩階段裝配策略使Baxter雙臂機器人完成軸孔裝配任務(wù),根據(jù)左右臂末端受力矩情況選擇左臂運動、右臂運動或者雙臂協(xié)調(diào);再次,針對冗余性研究較為單一問題,將運動性能和動力學(xué)性能優(yōu)化相結(jié)合,使用投影梯度法對逆解進行優(yōu)化,在限制大關(guān)節(jié)運動幅度的基礎(chǔ)上進行關(guān)節(jié)最小力矩控制,使用Newton-Raphson迭代方法減小投影梯度法帶來的誤差;最后,通過裝配實驗進行驗證裝配策略和逆解的有效性。采用柔性雙臂冗余機器人進行軸孔裝配來減小對工件的損壞,根據(jù)力的反饋軸孔裝配策略的研究可以提高軸孔尋插孔階段的效率,將運動學(xué)和動力學(xué)優(yōu)化項相結(jié)合的方式對冗余逆解問題的研究也有一定的指導(dǎo)意義。

1 問題描述

利用Baxter雙臂機器人作為研究平臺,它的每個手臂上有7個自由度(7-DOF),具有運動學(xué)的冗余性。Baxter機器人有軟件開發(fā)包(SDK),使用ROS環(huán)境的操作系統(tǒng)可以和對Baxter機器人進行控制和編程。Baxter機器人所有的關(guān)節(jié)是串聯(lián)彈性驅(qū)動器(Series-elastic actuator,SEA)關(guān)節(jié),使得機器人在穩(wěn)定性和抗沖擊方面有所提高,因此使用這種高柔性機器人裝配在保護機器人自身和裝配零件方面具有一定的意義。系統(tǒng)示意圖如圖1所示。軸連接在機器人右手臂,孔連接在機器人的左手臂,右手臂末端的力與力矩由Baxter機器人自身內(nèi)部計算后輸出。{B}表示基坐標系;{Wr}表示右手臂末端建立的坐標系;{Tr}表示以軸端面圓心為原點建立的坐標系;{Wl}表示左手臂末端建立的坐標系;{Tl}表示以孔端面圓心為原點建立的坐標系。

圖1 系統(tǒng)示意圖Fig.1 Schematic diagram of the system

針對Baxter機器人柔順性和冗余性,主要研究內(nèi)容為

1) 軸孔裝配操作空間的軌跡規(guī)劃。在軸孔裝配任務(wù)中,由于Baxter機器人自身定位有誤差,需要設(shè)計軸孔裝配策略補償。針對這個問題采用一種兩階段的軸孔裝配策略,根據(jù)右臂末端受力與力矩情況調(diào)整軸或孔。在尋孔階段通過對位置和姿態(tài)兩方面調(diào)整使軸入孔,在孔內(nèi)階段調(diào)整軸孔姿態(tài)使軸孔對齊,最后根據(jù)左右手臂末端的力矩情況規(guī)劃雙臂運動,完成軸孔裝配任務(wù)。

2) 關(guān)節(jié)空間的運動規(guī)劃,即冗余度機器人的逆運動學(xué)求解。由于Baxter機器人有多種逆解,機械臂會存在運動幅度大甚至出現(xiàn)關(guān)節(jié)極限等問題。針對這一問題使用投影梯度法求解機械臂的逆運動學(xué)優(yōu)化解,采用關(guān)節(jié)限制和最小關(guān)節(jié)力矩相結(jié)合的方式對Baxter機器人的關(guān)節(jié)空間進行運動規(guī)劃,使機器人運動量最小。由于投影梯度法求解過程中會產(chǎn)生誤差,使用Newton-Raphson迭代法減小誤差,精確優(yōu)化解。

2 軸孔裝配的軌跡規(guī)劃

2.1 軸孔裝配的模型分析

接觸狀態(tài)主要從孔外接觸和孔內(nèi)接觸這兩個方面分析?？淄饨佑|狀態(tài)主要有“單點接觸”“兩點接觸”“三點接觸”3種狀態(tài),在軸孔裝配的過程中“三點接觸”基本不會出現(xiàn),由于Baxter機器人關(guān)節(jié)的柔順性,“單點接觸”會轉(zhuǎn)變成“兩點接觸”。在ZOX平面對接觸狀態(tài)進行分析。Fx,Fz和My分別表示軸孔接觸點所受力作用在機器人右臂末端的沿x軸力、沿z軸力和繞y軸力矩值?？淄狻皟牲c接觸”模型如圖2所示。N1為沿x方向上的受力;F1為軸孔在接觸時引起的沿著z方向的力;r為軸半徑;l為{Tr}坐標系到{Wr}坐標系之間的距離;φ為接觸點與{Tr}圓心的連線和其y軸之間的夾角。

圖2 孔外兩點接觸Fig.2 Two-point contact outside the hole

任意點的接觸等效成準靜態(tài)的過程,根據(jù)受到的力與力矩平衡條件,其對應(yīng)分析式為

(1)

孔內(nèi)接觸有“單點接觸”和“兩點接觸”,對于雙臂的軸孔裝配來說,當孔內(nèi)“單點接觸”時,由于z軸會向前運動,所以會直接轉(zhuǎn)變成“兩點接觸”,具體如圖3(a)所示,因此孔內(nèi)接觸主要以“兩點接觸”為主,孔內(nèi)“兩點接觸”在ZOX平面接觸模型如圖3(b)所示。N1,N2分別為不同接觸點在軸孔傾斜時引起的沿x方向上的受力;F1,F2分別為軸孔在接觸時引起的沿著z方向的摩擦力;s為軸插入孔沿著z方向上的深度;α為軸孔軸線之間的夾角。由于軸孔之間的夾角較小,也可以不考慮。

圖3 孔內(nèi)接觸模型Fig.3 Contact model in the hole

受力與力矩關(guān)系式為

(2)

2.2 操作空間的軌跡規(guī)劃

機器人末端和軸孔零件的坐標系關(guān)系如圖1所示。裝配過程中,機器人控制軸孔沿著各自坐標系的z軸向前移動,當軸孔有接觸時開始裝配,機械臂根據(jù)當前右臂末端所受的力與力矩信息規(guī)劃下一個目標點,直到裝配成功。裝配的過程分成孔外階段和孔內(nèi)階段兩個階段。Fz的絕對值被視為軸孔裝配階段的判斷標準[14]。當Fz的絕對值超過閾值A(chǔ)時,認為軸接觸孔。

(3)

A—實驗值1; B—實驗值2; C—實驗值3; E—實驗值4; F—實驗值5; G—實驗值6; H—實驗值7。圖4 裝配流程圖Fig.4 Assembly flow chart

圖5 位置調(diào)整模型Fig.5 Position adjustment model

表1 雙臂各自位置調(diào)整機制

孔外的姿態(tài)調(diào)整主要取決于力矩條件[12]。根據(jù)機器人雙臂和軸孔之間的位置關(guān)系,分析可得:當Fx<0時,My<0,模型如圖6(a)所示,此時右臂繞著{Tr}坐標系的y軸正方向旋轉(zhuǎn);左臂沿著{Tl}坐標系y軸正方向旋轉(zhuǎn)。圖6(a)表示調(diào)整前的軸孔的狀態(tài);圖6(b)表示調(diào)整以后的軸孔狀態(tài)。同理可得ZOY平面的軸孔裝配模型的調(diào)整方向。

圖6 方向調(diào)整模型Fig.6 Orientation adjustment model

雙臂在不同的受力情況下的姿態(tài)調(diào)整規(guī)則如表2所示。軸孔調(diào)整方向基于各自的工具坐標系{Tr},{Tl}進行調(diào)整。表2中：Lr(0,-1,0)表示左臂繞{Tl}坐標系的y軸負方向旋轉(zhuǎn)βl角度；Rr(0,+1,0)表示右臂繞{Tr}坐標系的y軸正方向旋轉(zhuǎn)βr角度；Lr(+1,0,0)表示左臂繞{Tl}坐標系的x軸正方向旋轉(zhuǎn)βl角度；Rr(-1,0,0)表示右臂繞{Tr}坐標系的x軸負方向旋轉(zhuǎn)βr角度。

表2 雙臂各自姿態(tài)調(diào)整機制

孔內(nèi)階段的方向調(diào)整機制同孔外。當Fz的絕對值超過C時,軸完全插入孔中,最終裝配成功。雙臂的調(diào)整方向都是朝著力矩減小的方向調(diào)整,MRy為右手臂末端繞y軸力矩;MRx為右手臂末端繞x軸力矩;MLy為左手臂末端繞y軸力矩;MLx為左手臂末端繞x軸力矩。為了使力矩更好地平衡,左右臂協(xié)調(diào)表達式為

(4)

當|MR-ML|ML+K時,選擇右臂調(diào)整;當ML>MR+K時,選擇左臂調(diào)整。其中,雙臂協(xié)調(diào)運動時,S為左右臂的位置移動步長之和,各自移動步長由MR,ML決定;Sr為右臂移動的步長;Sl為左臂移動的步長。表達式為

(5)

β為左右臂轉(zhuǎn)動的角度之和,各自旋轉(zhuǎn)角度由MR,ML決定;βr為右臂旋轉(zhuǎn)的角度;βl為左臂旋轉(zhuǎn)的角度。表達式為

(6)

3 關(guān)節(jié)空間的規(guī)劃

規(guī)劃操作空間的軌跡,確定下一個目標點后,機器人要根據(jù)目標位姿求解各個關(guān)節(jié)的大小,也就是機器人逆運動學(xué)求解[13]。因為Baxter機器人有7個關(guān)節(jié),具有冗余性,所以在求逆運動學(xué)解上會存在多解的情況。對于該問題使用投影梯度法解決,在雅可比矩陣的零空間中考慮子任務(wù),使用優(yōu)化項約束逆運動學(xué)的解。關(guān)節(jié)的運動速度與機械臂末端的運動速度之間的關(guān)系式為

(7)

(8)

式中:J+為雅可比矩陣的偽逆,可由雅可比矩陣J計算得到,計算式為

J+=JT(JJT)-1

(9)

對于冗余度機器人而言,式(8)的解表示的是一種特解,也稱為最小范數(shù)解,即所解出的關(guān)節(jié)角速度組成的向量,其范數(shù)最小,代表了整體運動量最小的情況,當需要優(yōu)化其他性能指標時,可以采用投影梯度法進行逆運動學(xué)求解[14-15],即

(10)

式中:(I-J+J)為零空間的投影矩陣;φ為雅可比矩陣的零空間向量,可以取任意向量,通過適當選取φ對機器人各項性能指標進行優(yōu)化;(I-J+J)φ為方程的齊次解,該齊次解與最小范數(shù)正交,齊次解對應(yīng)于機械臂的自運動,即不影響機械臂末端姿態(tài)的關(guān)節(jié)運動,該自運動可以在不改變期望末端位姿的情況下,通過選取φ優(yōu)化機器人各項性能指標。優(yōu)化項從運動學(xué)和動力學(xué)兩個角度展開分析,φ1和φ2分別是運動學(xué)優(yōu)化項和動力學(xué)優(yōu)化項。帶入式(10)后為

(11)

(12)

(13)

(14)

其中

τm=0.5(τU+τL)

(15)

(16)

將φ1和φ2帶入式(11)可以求得運動學(xué)和動力學(xué)約束后的優(yōu)化解。使用Newton-Raphson迭代方法減小投影梯度法帶來的誤差,逼近優(yōu)化解。Newton-Raphson迭代方法涉及微分運動和雅可比矩陣,一段很短時間內(nèi)的微分運動可以用來描述物體的運動速度。迭代求解的流程如圖7所示。首先將初始位姿、目標位姿、最大迭代次數(shù)i_max和容許誤差efs輸入;然后將初始位置通過正運動學(xué)求解出初始末端位姿,根據(jù)初始末端位姿和輸入的目標位姿可求得末端誤差deltaQ,使用矢量積構(gòu)造方法構(gòu)造雅可比矩陣,接著根據(jù)式(11～16)中描述的投影梯度法求得關(guān)節(jié)角,根據(jù)求得的關(guān)節(jié)角計算末端位姿Tc,由Tc和目標位姿得出微分運動向量,根據(jù)微分運動向量和雅可比矩陣更新迭代所得的關(guān)節(jié)角,不斷迭代直到誤差在容許范圍內(nèi),輸出更新的角度。如果迭代次數(shù)超出設(shè)置的最大迭代次數(shù),則輸出錯誤。

圖7 迭代求解流程Fig.7 Iterative solution process

4 實 驗

實驗平臺如圖8所示,主要由Baxter機器人、軸孔零件和用戶控制系統(tǒng)組成,軸的外徑為39.5 mm,孔的內(nèi)徑為40 mm,軸孔之間的間隙為0.5 mm,軸孔的尺寸如表3所示。手臂的關(guān)節(jié)由SEA驅(qū)動,SEA特性的關(guān)節(jié)具有較好的柔順性,可以降低手臂的碰撞風險,較為安全。通過控制各個關(guān)節(jié)的角度來控制Baxter機器人末端的位姿。

圖8 實驗平臺Fig.8 Experimental platform

表3 軸孔尺寸表

工具末端({Tr}與{Tl})位姿為(x,y,z,θx,θy,θz),其中x,y,z表示在基坐標系的位置信息,θx,θy,θz分別表示繞x、繞y、繞z的角度。初始裝配位姿左右手臂分別為(0.59,0.015,0.2,90,0,-180),(0.59,0.001,0.2,-90,0,180)。根據(jù)逆解算法可求得初始裝配位姿對應(yīng)的關(guān)節(jié)角度。左右臂關(guān)節(jié)角度分別為[0.170 248 49,0.015 644 25,-1.110 366 319,1.705 184,-0.286 599 27,0.902 048 707,0.604 895 67],[-0.170 742 71,0.051 376 314,1.129 841 03,1.698 502,0.245 239 54,0.880 363,-0.566 031],對應(yīng)機器人姿態(tài)如圖9所示。

圖9 初始裝配姿態(tài)Fig.9 Initial assembly posture

裝配過程中,以右臂末端反饋的力與力矩作為接觸狀態(tài)的判斷依據(jù),左右手臂都以2.5 mm的步長沿著z軸向前移動,位置控制步長S的選取小于等于軸孔的配合公差,此處設(shè)定為0.5 mm,由于Baxter自身的定位,軸孔軸線之間的角度偏差不會太大,所以姿態(tài)調(diào)整旋轉(zhuǎn)角度β設(shè)定0.5°。對于裝配流程圖的參數(shù),設(shè)置A=2 N,B=2 N,C=4 N,E=F=100 N·mm,G=H=200 N·mm,K=20 N·mm。根據(jù)筆者所提裝配算法進行實驗。在實驗過程中,分別使用機器人自帶逆解算法、筆者逆解算法進行關(guān)節(jié)空間的逆運動學(xué)求解。Baxter機器人自帶的逆解算法軸孔裝配總時長為31.6 s,逆解算法進行的軸孔裝配總時長為24.2 s。這兩種算法在軸孔裝配過程中,右臂末端所受力與力矩隨時間變化情況如圖10所示。圖10(a)和圖10(b)表示逆解算法求解的軸孔裝配力與力矩圖;圖10(c)和圖10(d)表示Baxter機器人自帶的逆解算法求解的軸孔裝配力與力矩圖。圖中a階段表示孔外調(diào)整階段;b階段表示孔內(nèi)插孔時孔內(nèi)調(diào)整階段;c階段表示退軸出孔時的孔內(nèi)調(diào)整階段;d階段表示軸孔分離階段;x曲線表右臂末端沿x方向的受力與力矩;y曲線表示右臂末端沿y方向的受力與力矩;z曲線表示右臂末端沿著z軸方向的受力與力矩。兩個算法軸孔裝配的力與力矩變化曲線規(guī)律一致,根據(jù)力與力矩曲線可以驗證軸孔裝配算法的有效性。

圖10 軸孔裝配受力/力矩圖Fig.10 Force/torque diagram of shaft hole assembly

右手臂各個關(guān)節(jié)角度隨時間變化角度如圖11所示。圖11中:right1表示逆解算法對應(yīng)的曲線;right2表示Baxter機器人自帶的逆解算法對應(yīng)的曲線。在整個裝配過程中,相較于機器人自帶算法,使用筆者逆解算法求解,s0,s1,e0,e1,w1關(guān)節(jié)角度變化幅度更小。

圖11 關(guān)節(jié)角度變化Fig.11 Changes in joint angle

在軸孔裝配過程中兩者算法的力矩對比結(jié)果如圖12所示。圖12中:right1表示筆者提出的逆解算法算法;right2表示Baxter機器人自帶的算法。在裝配過程中s0,s1,w0,w1,w2這5個關(guān)節(jié)力矩變化幅度較小,驗證了筆者逆解算法的有效性。

圖12 關(guān)節(jié)力矩變化Fig.12 Changes in joint torque

軸孔裝配實驗結(jié)果表明:筆者提出的逆解算法相較于Baxter機器人自帶的逆運動學(xué)求解的效果更好,在使用逆解算法裝配的過程中,整體上關(guān)節(jié)角度變化更小,力矩變化幅度較小,時間更短。

5 結(jié) 論

以Baxter冗余雙臂機器人為研究平臺,研究了軸孔裝配策略和冗余優(yōu)化逆解。首先,對尋孔效率和定位精度較低的問題進行分析并提出位置控制和姿態(tài)控制的裝配策略;然后,對冗余逆解較單一問題提出了關(guān)節(jié)限制和最小關(guān)節(jié)力矩優(yōu)化方式;最后,通過軸孔裝配實驗對該裝配策略和冗余逆解算法進行實驗驗證,分析軸孔裝實驗過程中右臂末端受力/力矩情況和軸孔裝配過程中機器人關(guān)節(jié)角度及力矩變化,實驗結(jié)果驗證了軸孔裝配策略的有效性。相較于Baxter機器人自帶的逆解算法,筆者逆解算法下的關(guān)節(jié)角度和力矩變化范圍得到了控制,機器人完成相同任務(wù)的時間更短。但是,在軸孔裝配位置調(diào)整機制和姿態(tài)調(diào)整機制中,將步長和角度設(shè)置為定值仍不夠靈活,后續(xù)研究會進一步考慮動態(tài)設(shè)置步長和角度,使位姿控制更加精確。

本文得到了浙江工業(yè)大學(xué)教學(xué)改革項目(JG201918)的資助。