孫亦劼,蔣文萍*,閔 軍,汪凌陽

(1. 上海應(yīng)用技術(shù)大學(xué)電氣與電子工程學(xué)院,上海 201418;2. 同濟大學(xué)電子與信息工程學(xué)院,上海 201804)

1 引言

隨著現(xiàn)代工業(yè)機器人技術(shù)的不斷發(fā)展,越來越多的高性能算法被廣泛地應(yīng)用到工業(yè)機器人的生產(chǎn)、教育與科研中,同時,也對我國的工業(yè)機器人算法領(lǐng)域的創(chuàng)新和驗證提出了更高的要求[1]。

文獻[2]提出利用極限學(xué)習(xí)機(ELM)算法對工業(yè)機器人定位誤差規(guī)律及其補償問題進行分析,得出采用ELM算法建模的補償速度快,泛化性能好的結(jié)論。文獻[3]提出了一種計算簡便,并且能夠有效地反映雙臂協(xié)調(diào)運動靈活性性能指標的方法,提高了雙臂冗余度機器人在其交互工作空間中的協(xié)調(diào)運動能力。文獻[4]采用模糊神經(jīng)PID算法對機械臂進行運動學(xué)/動力學(xué)控制,并證明了其控制策略的有效性和實用性。

上述文獻方法雖然在一定程度上提升了機器人控制效果,但是很難預(yù)測工業(yè)機器人在實際工業(yè)環(huán)境下即將面臨的問題,且無法有效地對工業(yè)場景進行真實還原?；谏鲜鰡栴},提出了一種將三重增量式模糊PID控制方法與工業(yè)機器人虛擬驗證相結(jié)合的研究方法。三重增量式模糊PID控制方法根據(jù)工業(yè)機器人在三維笛卡爾坐標系(空間三軸上的)末端實時位置、速度與期望位置、速度間的誤差,使用三個獨立的模糊控制器分別對末端三軸位置進行跟蹤,經(jīng)過機器人運動學(xué)計算,推導(dǎo)出各個關(guān)節(jié)角;而機器人虛擬仿真技術(shù)則最大程度上還原了真實工業(yè)場景,可有效避免碰撞、軸超限、奇異點等報錯情況的發(fā)生。并以工業(yè)機器人噴漆操作為例,有效驗證控制算法投入工業(yè)應(yīng)用的可行性。

2 機械臂及其運動學(xué)建模

2.1 機械臂機械模型及其連桿參數(shù)

研究對象為三自由度串聯(lián)工業(yè)機械臂,其模型的搭建由CAD軟件SOLIDWORKS 2019完成。從機械臂的基座底部,到第一個左右旋轉(zhuǎn)關(guān)節(jié)幾何中心的距離0.6m;機械臂兩個上下俯仰關(guān)節(jié)之間的機械臂稱為大臂,大臂的長度為0.6m;從機械臂的第二個俯仰關(guān)節(jié),到機械臂末端頂端的距離,即小臂加上末端的長度為0.4m[5]。機械模型如圖1所示,連桿參數(shù)如表1所示。

表1 三自由度機械臂D-H參數(shù)

圖1 機械臂機械模型

2.2 機械臂運動學(xué)建模

根據(jù)表1中的三自由度機械臂D-H參數(shù),以及機器人正運動學(xué)連桿變換,得到該機械臂的總變換矩陣[6]?？傋儞Q矩陣可以描述為

0T3=A1A2A3

(1)

式中,A1、A2、A3分別為機械臂從基座到旋轉(zhuǎn)關(guān)節(jié)、從旋轉(zhuǎn)關(guān)節(jié)到俯仰關(guān)節(jié)、從俯仰關(guān)節(jié)到機械手末端的三個變換矩陣。其中,變換矩陣為

(2)

將表1中的連桿參數(shù)代入到式(2)中,得到

(3)

(4)

(5)

將式(3)、(4)、(5)分別代入到式(1)中,得到三自由度機械臂的總變換矩陣。

設(shè)a1為機器人的旋轉(zhuǎn)機械臂,a2和a3為機器人的俯仰機械臂。過機器人末端點P(X,Y,Z)作XY平面的法線,過基座與第一旋轉(zhuǎn)軸幾何中心點作法線的垂線。L1、L2、L3分別為第一旋轉(zhuǎn)軸到法線、末端到第一旋轉(zhuǎn)軸所在平面、第一旋轉(zhuǎn)軸到末端的距離[7],則有

(6)

L1、L2、L3的長度已知,可以計算出q、w、e三個角度值

(7)

根據(jù)機器人逆運動學(xué),θ1、θ2、θ3角度值可由式(6)(7)進行描述

(8)

3 機械臂控制器設(shè)計

機械臂三重增量式模糊PID控制器由機械臂運動控制器、機械臂三重增量式模糊PID控制器、機械臂被控系統(tǒng)三個模塊組成。其中:機械臂運動控制器負責(zé)由期望位置Rx、Ry、Rz,末端期望速度Vx、Vy、Vz以及實際位置RX、RY、RZ,實際末端速度VX、VY、VZ,計算得到三維矩陣[8]。三重增量式模糊PID控制器將軌跡誤差e以及速度誤差de作為輸入,結(jié)合隸屬度函數(shù),輸出對應(yīng)的三組Kp、Ki、Kd參數(shù),分別控制機械臂末端在三維環(huán)境下X、Y、Z軸上的位置和速度;機械臂被控對象模塊負責(zé)將三組Kp、Ki、Kd參數(shù)以及軌跡誤差e通過積分環(huán)節(jié)后的累計誤差E,利用雅可比矩陣以及機器人運動學(xué),計算末端的實際位置位置RX、RY、RZ以及末端實時速度VX、VY、VZ。

3.1 機械臂運動控制器

首先,輸入機械臂末端X、Y、Z軸上關(guān)于時間的期望運動軌跡

(9)

對期望軌跡求導(dǎo),得到機械臂末端在X、Y、Z軸上關(guān)于時間的期望運動速度

(10)

根據(jù)在2.1節(jié)中定義的機械臂臂長,由機器人運動學(xué),得到機械臂三關(guān)節(jié)的期望角度qd1、qd2、qd3

(11)

式中,c代表cos,s代表sin。

根據(jù)運動控制器的輸入和被控系統(tǒng)的反饋輸出得到三個矩陣,分別是實際軌跡與期望軌跡的誤差矩陣e,位置變化量(速度)誤差矩陣de,以及軌跡誤差矩陣經(jīng)過積分后得到的累積誤差矩陣E

(12)

(13)

(14)

3.2 三重增量式模糊PID控制器設(shè)計

三重增量式模糊PID控制器由3個獨立的模糊PID控制器組成,每個模糊PID控制器分別負責(zé)對每個空間維度上的位置和速度進行控制[9,10]。輸入中,負大和正大的隸屬度函數(shù)是論域為[-3,3]的高斯隸屬度函數(shù),分別為

(15)

輸入中負中,負小,零,正小,正中的隸屬度函數(shù)是論域為[-3,3]的三角形隸屬度函數(shù),其參數(shù)分別為

(16)

輸出Kp的隸屬度函數(shù)可以描述為

(17)

輸出Ki的隸屬度函數(shù)可以描述為

(18)

輸出Kd的隸屬度函數(shù)可以描述為

(19)

3.3 機械臂被控系統(tǒng)

根據(jù)上文提到的隸屬度函數(shù),得到實際機械臂被控系統(tǒng)中三關(guān)節(jié)的KP、KI、KD參數(shù)[11],可用三維矩陣描述為

(20)

(21)

(22)

聯(lián)合式(12)、(13)、(14)和式(20)、(21)、(22),可以得到一個三維矩陣

dq=KP·e+KI·E+KD·de

(23)

根據(jù)式(11)和式(23),得出機械臂關(guān)節(jié)實際角度為機械臂關(guān)節(jié)的期望角度與關(guān)節(jié)控制量之和[12,13]

(24)

由式(24)以及機械臂長度,得到機器人的雅可比矩陣J

(25)

由式(23)和(25),得到機械臂末端線速度為機器人的雅可比矩陣點乘機械臂三關(guān)節(jié)控制量

(26)

由式(24)、機械臂三臂長度以及機器人運動學(xué),可得到機械臂末端隨時間的實際運動軌跡[14,15]：

(27)

4 仿真與實驗

4.1 算法仿真

根據(jù)機械臂控制器,通過計算機械臂三關(guān)節(jié)的實時角度,以及雅可比矩陣,可以得到機械臂末端的位置以及線速度,從而計算出機械臂的軌跡誤差以及位置變化量誤差,結(jié)合三重增量式模糊控制器,得到機械臂三個坐標下的KP、KI以及KD。將這個過程如此循環(huán),得到定步長為0.1s下的機械臂跟蹤曲線。

由式(9)和式(27),得到機械臂末端的期望軌跡及由機械臂控制器得到的實際跟蹤軌跡,并將其文獻[2]方法與文獻[3]方法得到的跟蹤軌跡進行了比較,如圖2所示。

圖2 軌跡跟蹤性能比較

如圖2所示:三重增量式模糊PID控制器在X、Y、Z軸上的跟蹤表現(xiàn)良好,其跟蹤軌跡與期望軌跡間的平均誤差絕對值分別為6.421×10-5m、2.942×10-4m、1.300×10-3m,其性能遠優(yōu)于文獻[2]和文獻[3]方法的跟蹤誤差。

4.2 工業(yè)機器人控制算法虛擬仿真驗證

驗證可以分為四個階段,依次是導(dǎo)入模型、定義機構(gòu)、導(dǎo)入軌跡以及虛擬仿真。其中,導(dǎo)入模型階段即將CAD中繪制的機械臂模型,導(dǎo)入到機器人虛擬仿真軟件中;定義機構(gòu)階段包括定義機械臂自由度、定義機械臂關(guān)節(jié)以及定義機械臂限位。

導(dǎo)入軌跡階段即將由三重增量式模糊PID算法輸出的軌跡,導(dǎo)入到機器人虛擬仿真軟件中,對算法輸出軌跡進行驗證。將控制算法得到的每一個步長點,依次輸入至機器人虛擬仿真軟件中(從0s-10s,步長為0.1s)。在導(dǎo)入軌跡工作完成后,根據(jù)輸入每個步長的軌跡點,虛擬仿真軟件會根據(jù)其機器人逆運動學(xué),推導(dǎo)出對應(yīng)位置時其各關(guān)節(jié)的角度。以對螺旋管進行噴漆為例,通過虛擬仿真,實時觀察機械臂在虛擬環(huán)境下的運動情況,如圖3所示。

圖3 對螺旋管進行虛擬仿真噴漆

5 結(jié)束語

對三自由度機械臂進行運動學(xué)建模,設(shè)計三自由度機械臂三重增量式模糊PID控制器,并將輸出的機械臂運動軌跡導(dǎo)入到虛擬仿真軟件中對控制算法進行驗證。實驗得出以下結(jié)論:

1)三重增量式模糊PID控制器對機械臂的期望軌跡有很好的跟蹤效果,且相比于傳統(tǒng)方法,運算速率更快,跟蹤性能更優(yōu),其平均誤差僅為1.300×10-3m。

2)在虛擬仿真驗證算法的過程中,所有的步長點均可達,各個關(guān)節(jié)角均在軸限范圍內(nèi),順利對螺旋管完成噴漆操作。

綜上,驗證了該算法在實際工業(yè)情況下的可行性。