工業(yè)機器人三重增量式模糊控制及其虛擬驗證

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

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

1 引言

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

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

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

2 機械臂及其運動學建模

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

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

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

圖1 機械臂機械模型

2.2 機械臂運動學建模

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

0T3=A1A2A3

(1)

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

(2)

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

(3)

(4)

(5)

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

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

(6)

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

(7)

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

(8)

3 機械臂控制器設計

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

3.1 機械臂運動控制器

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

(9)

對期望軌跡求導,得到機械臂末端在X、Y、Z軸上關于時間的期望運動速度

(10)

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

(11)

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

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

(12)

(13)

(14)

3.2 三重增量式模糊PID控制器設計

三重增量式模糊PID控制器由3個獨立的模糊PID控制器組成,每個模糊PID控制器分別負責對每個空間維度上的位置和速度進行控制[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)中三關節(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),得出機械臂關節(jié)實際角度為機械臂關節(jié)的期望角度與關節(jié)控制量之和[12,13]

(24)

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

(25)

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

(26)

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

(27)

4 仿真與實驗

4.1 算法仿真

根據(jù)機械臂控制器,通過計算機械臂三關節(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è)機器人控制算法虛擬仿真驗證

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

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

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

5 結束語

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

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

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

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