基于傾角傳感器的液壓比例驅(qū)動機械臂位姿控制

高波，李法康

(山東科技大學(xué)電氣與自動化工程學(xué)院，山東青島 266590)

0 前言

20世紀(jì)60年代左右，使用機械手噴漿在國外開始興起[1]。針對礦井下混凝土噴射具有回彈率高、粉塵大等弊端，1996年，李貽斌等[2]設(shè)計了中國第一臺煤礦機器人——PJR-2噴漿機器人，為噴漿機器人在我國的發(fā)展奠定了基礎(chǔ)。

在長期的噴漿作業(yè)和研究過程中，總結(jié)出了一套完整的最佳噴漿工藝要求。其中針對受噴面噴料不均勻的問題，要求噴槍水平運動(調(diào)整噴槍相對待噴面距離)或垂直運動(調(diào)整噴槍相對地面高度)的同時不斷做圓周運動，并且作業(yè)面噴槍順序應(yīng)由下而上，先側(cè)幫后拱頂。為了滿足該要求，傳統(tǒng)的噴漿機械手將結(jié)構(gòu)設(shè)計為大臂升高與伸縮臂調(diào)平，通過人工操作多關(guān)節(jié)共同運動，完成噴槍在水平方向或豎直方向的直線運動[3-4]。但是機械臂各關(guān)節(jié)運動與噴頭位姿之間的關(guān)系存在耦合，并且操作困難，控制精度低，使得在調(diào)整噴槍相對待噴面距離時，其相對地面高度也會隨之改變，反之亦然。

電液比例控制相對伺服控制具有抗干擾能力強、成本低的特點，在對機械手的響應(yīng)速度要求不高、滿足基本的控制精度就算達(dá)標(biāo)的情況下，多采用電液比例控制系統(tǒng)[5]。通過在電液比例控制系統(tǒng)的油缸中安裝位移傳感器，將油缸活塞位移反饋至控制器[6-8]，形成位置閉環(huán)，進(jìn)而實現(xiàn)對關(guān)節(jié)的控制，但是該方法會使得系統(tǒng)復(fù)雜化，可靠性和安全性也會降低[9]。

傾角傳感器是一款用于測量運動載體與其初始位置所成夾角的慣性測量設(shè)備，獲得運動載體的動態(tài)參數(shù)(橫滾和俯仰)信息，由成本較低的傾角傳感器構(gòu)成的閉環(huán)控制系統(tǒng)同樣具有較好的控制性能[10-13]，性價比較高。

為了擺脫這些局限性，本文作者根據(jù)噴槍的位姿，運用幾何法求解得到關(guān)節(jié)角度，基于閥控液壓馬達(dá)系統(tǒng)以及傾角傳感器，采用傳統(tǒng)PID控制，構(gòu)成關(guān)節(jié)位置閉環(huán)控制系統(tǒng)，控制相應(yīng)關(guān)節(jié)運動實現(xiàn)噴槍的水平運動和豎直運動。通過結(jié)構(gòu)改變，使得各關(guān)節(jié)運動最大程度實現(xiàn)解耦，使得關(guān)節(jié)操作簡單，且可通過傾角傳感器實現(xiàn)控制，利用傾角傳感器測量無法解耦的大臂與小臂角度，實現(xiàn)位姿解算和關(guān)節(jié)角度閉環(huán)控制，避免了因安裝傳感器對系統(tǒng)造成的影響。

1 關(guān)節(jié)角度的求解

t時刻大、小臂當(dāng)前位置相對其初始狀態(tài)的位置關(guān)系如圖1所示，其中關(guān)節(jié)1為伸縮關(guān)節(jié)，關(guān)節(jié)2、3、4為回轉(zhuǎn)關(guān)節(jié)，傾角傳感器1、2分別用于測量大臂、小臂與各自初始狀態(tài)的夾角，j1(t)、j2(t)分別為t時刻大臂、小臂與其初始位置之間的夾角。

Zt(t)=Z0+vzt

(1)

Yt(t)=Y0+vyt

(2)

式中：Z0、Y0為噴槍相對關(guān)節(jié)2在巷道坐標(biāo)系中的初始位置；vz表示噴槍在Z軸的運動速度，即垂直運動速度；vy表示噴槍在y軸的運動速度，即水平運動速度；t表示時間；Zt(t)表示t時刻噴槍在OXYZ坐標(biāo)系中Z軸方向上的位置；Yt(t)表示t時刻噴槍在OXYZ坐標(biāo)系中在Y軸方向上位置。

當(dāng)需要噴槍在水平方向上運動時，vz=0，vy≠0；當(dāng)需要噴槍在豎直方向上運動時，vz≠0，vy=0。

由幾何法可以得到：

d1sinα(t)-d2sinβ(t)=Zt(t)

(3)

d1cosα(t)+d2cosβ(t)=Yt(t)

(4)

式中：α(t)表示大臂與水平方向的夾角；β(t)表示小臂與水平方向的夾角；d1、d2分別表示大臂、小臂的長度。

聯(lián)立式(3)(4)得：

當(dāng)Zt(t)≥0時：

(5)

(6)

當(dāng)Zt(t)<0時：

(7)

(8)

t時刻α(t)、β(t)與j1(t)、j2(t)之間的位置關(guān)系如圖1所示。

圖1 機械手運動示意

2 關(guān)節(jié)位置閉環(huán)控制系統(tǒng)設(shè)計

EPEC(Extreme Performance Engine Controller)作為主控制器接收遙控器發(fā)送的速度指令，即上述噴槍在水平和豎直方向的運動速度vy、vz，根據(jù)式(3)(4)解算得到α(t)、β(t)，并根據(jù)圖1中的位置關(guān)系可以分別計算得到大臂、小臂在t時刻與其初始狀態(tài)之間的夾角j1(t)、j2(t)，其作為關(guān)節(jié)位置閉環(huán)控制系統(tǒng)的輸入。

關(guān)節(jié)位置閉環(huán)控制系統(tǒng)如圖2所示，由PID控制器、EPEC控制器的PWM輸出模塊、功率放大器、負(fù)載敏感比例多路換向閥、液壓馬達(dá)以及傾角傳感器組成。j1(t)、j2(t)與傾角傳感器實時測量值r1(t)、r2(t)的差值經(jīng)過PID控制器得到相應(yīng)的控制量，該控制量作為PWM輸出模塊的占空比，然后該PWM信號經(jīng)功率放大器放大后控制負(fù)載敏感比例多路換向閥的閥芯運動，從而控制液壓馬達(dá)驅(qū)動回轉(zhuǎn)支承，進(jìn)而驅(qū)動機械臂帶動噴槍在巷道坐標(biāo)系中運動直至到達(dá)目標(biāo)位置，最終實現(xiàn)通過遙控器控制噴槍在巷道坐標(biāo)系Y軸方向和Z軸方向上的平穩(wěn)運動。

圖2 關(guān)節(jié)位置閉環(huán)控制系統(tǒng)框圖

3 關(guān)節(jié)位置閉環(huán)控制系統(tǒng)的數(shù)學(xué)模型

對圖2所示的關(guān)節(jié)位置閉環(huán)控制系統(tǒng)的每個組成部分建立數(shù)學(xué)模型，為下面仿真驗證及分析優(yōu)化提供理論依據(jù)。

3.1 關(guān)節(jié)位置反饋傳遞函數(shù)

傾角傳感器用來實時測量機械臂的角度信息r1(t)、r2(t)，在控制系統(tǒng)中作為單位負(fù)反饋，與上述解算得到的j1(t)、j2(t)形成關(guān)節(jié)位置閉環(huán)，控制關(guān)節(jié)運動，驅(qū)動機械臂到達(dá)期望位置。

以大臂為例，建立位置閉環(huán)的數(shù)學(xué)模型：

e(t)=j1(t)-r1(t)

(9)

式中：e(t)為目標(biāo)值與測量值之間的差值；j1(t)為t時刻的目標(biāo)角度；r1(t)為t時刻大臂上傾角傳感器的實時測量值。

對式(9)進(jìn)行拉普拉斯變換得：

E(s)=J1(s)-R1(s)

(10)

式中：E(s)為e(t)的復(fù)頻域表達(dá)式；J1(s)為j1(t)的復(fù)頻域表達(dá)式；R1(s)為r1(t)的復(fù)頻域表達(dá)式；s為復(fù)變數(shù)。

對于以流量為控制對象的PID控制器，Kd一般取零[14]，輸入為E(s)，所以PID控制器的傳遞函數(shù)為

(11)

式中：C(s)為PID控制器輸出量；Kp為PID控制器的比例系數(shù)；Ki為PID控制器的積分系數(shù)。

3.2 PWM控制器的傳遞函數(shù)

PWM控制器采用EPEC控制器的PWM輸出模塊，其最大輸出電壓為24 V。PID控制器輸出量C(s)作為該模塊的占空比，控制該模塊電壓的輸出。當(dāng)e(t)=0時，即j1=r1時，輸出電壓為0，關(guān)節(jié)停止運動，此時機械臂到達(dá)目標(biāo)位置，噴槍停止運動；當(dāng)e(t)≠0時，某時刻電壓的大小在復(fù)頻域的表達(dá)式為

U(s)=[24C(s)]/100

(12)

式中：U(s)為u(t)的復(fù)頻域表達(dá)式。

3.3 純滯后環(huán)節(jié)的傳遞函數(shù)

在實際的工程實踐中，當(dāng)控制器的控制量已經(jīng)改變，但是EPEC控制器的PWM輸出模塊不能立即響應(yīng)，會有一定的輸出延時，此處取其為0.1 s，再加上總線傳輸延遲以及比例閥本身的響應(yīng)速度較慢，使得延時時間在0.1 s附近波動。在控制系統(tǒng)中該環(huán)節(jié)相當(dāng)于一個純滯后環(huán)節(jié)[15]。

其表達(dá)式為

G2(s)=e-τs

(13)

式中：τ為滯后時間，取τ=0.1。

3.4 功率放大器的傳遞函數(shù)

功率放大器按輸入電壓的大小成比例地輸出電流[16]，用于驅(qū)動負(fù)載敏感比例多路換向閥。該處的輸入電壓為EPEC的PWM輸出模塊的電壓信號。該環(huán)節(jié)近似于比例環(huán)節(jié)，其傳遞函數(shù)為

I(s)=KaU(s)

(14)

式中：I(s)為功率放大器的輸出電流；Ka為比例放大系數(shù)。

3.5 負(fù)載敏感比例多路換向閥的傳遞函數(shù)

比例多路換向閥主要用于工程機械電液控制領(lǐng)域，控制液壓油缸或馬達(dá)的運動方向和速度。比例多路換向閥采用負(fù)載敏感技術(shù)，使泵的輸出壓力隨負(fù)載變化而變化，以及執(zhí)行元件的速度與負(fù)載變化無關(guān)，并且能夠通過計算機進(jìn)行控制[17-18]。

負(fù)載敏感比例多路換向閥中的電-機信號轉(zhuǎn)化器是比例電磁鐵，比例電磁鐵線圈起主導(dǎo)作用的是二階振蕩環(huán)節(jié)[19]。負(fù)載敏感比例多路換向閥輸入電流I(比例放大器的輸出電流)與閥芯位移的傳遞函數(shù)為

(15)

式中：Kv為負(fù)載敏感比例多路換向閥流量增益；ωv為負(fù)載敏感比例多路換向閥固有頻率；εv為負(fù)載敏感比例多路換向閥阻尼比；Xv(s)為閥芯位移。

3.6 液壓馬達(dá)的傳遞函數(shù)

閥控液壓馬達(dá)的轉(zhuǎn)動角度與負(fù)載敏感比例多路換向閥閥芯位移之間的關(guān)系[20]為

(16)

式中：ωh為液壓系統(tǒng)的無阻尼固有頻率；Kq為電磁閥的流量增益；εh為液壓系統(tǒng)的阻尼比；Dm為液壓馬達(dá)的排量；θ(s)為液壓馬達(dá)轉(zhuǎn)角。

液壓馬達(dá)轉(zhuǎn)角與機械臂回轉(zhuǎn)關(guān)節(jié)轉(zhuǎn)角之間存在一個比例關(guān)系，即

R1(s)=Kθθ(s)

(17)

式中：Kθ為液壓馬達(dá)轉(zhuǎn)角與關(guān)節(jié)角度之間的比例系數(shù)。

3.7 關(guān)節(jié)位置閉環(huán)控制系統(tǒng)傳遞函數(shù)方框圖

綜上，圖2所對應(yīng)的系統(tǒng)傳遞函數(shù)框圖如圖3所示。

圖3 系統(tǒng)傳遞函數(shù)框圖

4 關(guān)節(jié)位置閉環(huán)控制系統(tǒng)仿真驗證及系統(tǒng)優(yōu)化

在工程試驗過程中，發(fā)現(xiàn)所用負(fù)載敏感比例多路換向閥在零位附近存在位置死區(qū)，除此之外，PWM控制器輸出電壓時，存在的時間滯后對整個控制系統(tǒng)有著很大的影響。如何解決這2個非線性因素對整個控制系統(tǒng)的響應(yīng)速度及控制精度至關(guān)重要，因此基于上述所確定的數(shù)學(xué)模型，對整個控制系統(tǒng)進(jìn)行仿真研究，選用合適的方法對控制系統(tǒng)進(jìn)行優(yōu)化。

4.1 負(fù)載敏感比例多路換向閥的零位死區(qū)的影響及優(yōu)化

所用負(fù)載敏感比例多路換向閥的相關(guān)參數(shù)為：Ka=0.2 V/A；ωv=108 rad/s；Kv=3.65×10-3m3/s·A；εv=0.6。

為了解決該閥的零位死區(qū)對整個控制系統(tǒng)的影響，提高比例閥的響應(yīng)速度，以負(fù)載敏感比例多路換向閥的閥芯位置控制為例，其控制框圖如圖4所示。

圖4 負(fù)載敏感比例多路換向閥閥芯位置控制框圖

當(dāng)輸入信號為正弦信號時，其仿真結(jié)果如圖5所示。

圖5 負(fù)載敏感比例多路換向閥閥芯位置的正弦響應(yīng)

由圖5可知：閥芯零位附近存在的位置死區(qū)，使其響應(yīng)存在一定的時間延遲。

在工程試驗過程中，通過電壓階躍響應(yīng)的方式實現(xiàn)對比例閥位置死區(qū)的補償。即通過測試得到閥芯開始移動時控制器輸出的電壓，該電壓作為比例閥在零位附近的補償電壓Ub，經(jīng)比例放大器成比例地輸出相應(yīng)的電流，以此來改善比例閥的動態(tài)響應(yīng)。

電壓階躍表達(dá)式為

(18)

式中：Uv為電壓階躍模塊的輸出電壓。

加入電壓階躍響應(yīng)后，負(fù)載敏感比例多路換向閥對正弦信號的響應(yīng)如圖6所示。

圖6 加入電壓階躍信號后閥芯的正弦穩(wěn)態(tài)響應(yīng)

如圖6所示，加入階躍電壓后，在零位附近，閥芯停滯的時間明顯減少，響應(yīng)更加迅速，負(fù)載敏感比例多路換向閥的動態(tài)性能得到明顯改善。

4.2 控制系統(tǒng)中純滯后環(huán)節(jié)的影響及優(yōu)化

控制系統(tǒng)相關(guān)元器件參數(shù)分別為：ωh=230 rad/s；εh=11.6；Kq=0.009 m2/s；Dm=3.9×10-6m3/rad；取Kp=2.0，Ki=0.02，Kθ=3.0。

對負(fù)載敏感比例多路換向閥的控制進(jìn)行優(yōu)化后，關(guān)節(jié)位置閉環(huán)控制系統(tǒng)對單位階躍信號的響應(yīng)如圖7所示。

發(fā)出速度信號給噴槍后，希望噴槍能快速平穩(wěn)地到達(dá)指定位置，且不應(yīng)出現(xiàn)較大超調(diào)避免出現(xiàn)危險，但是從圖7可以看出：由于控制系統(tǒng)存在純滯后環(huán)節(jié)，使得控制系統(tǒng)的單位階躍響應(yīng)存在明顯超調(diào)，且響應(yīng)速度較慢。故需要采取必要的措施，優(yōu)化該系統(tǒng)控制性能，補償純滯后對系統(tǒng)的影響。

圖7 關(guān)節(jié)位置閉環(huán)控制系統(tǒng)的單位階躍響應(yīng)

針對純滯后系統(tǒng)的傳統(tǒng)控制策略包括最優(yōu)PID控制、Smith預(yù)估控制、大林算法等。Smith預(yù)估控制的優(yōu)點在于采用過程模型，使被控過程的延遲輸出超前時間τ反映到控制器，從而抵消滯后環(huán)節(jié)對控制過程的影響[21-23]。傳統(tǒng)Smith預(yù)估器結(jié)構(gòu)框圖如圖8所示。

圖8 傳統(tǒng)Smith預(yù)估器結(jié)構(gòu)框圖

其中：G(s)為閥控液壓馬達(dá)系統(tǒng)的開環(huán)控制函數(shù)；e-τs為純滯后環(huán)節(jié)；τm為預(yù)估器的延遲系數(shù)。

在未加入Smith預(yù)估器之前，整個閥控液壓馬達(dá)系統(tǒng)的閉環(huán)傳遞函數(shù)為

(19)

在加入傳統(tǒng)Smith預(yù)估器之后，當(dāng)τm=τ時，整個控制系統(tǒng)的閉環(huán)傳遞函數(shù)為

(20)

對比上述兩式可得，加入傳統(tǒng)Smith預(yù)估器之后，排除了控制系統(tǒng)的分母多項式的滯后項，因此，將具有滯后的控制系統(tǒng)模型轉(zhuǎn)換為具有純滯后項的無滯后系統(tǒng)模型，消去了純滯后環(huán)節(jié)對控制系統(tǒng)的控制品質(zhì)的影響。

在加入傳統(tǒng)Smith預(yù)估器后，控制系統(tǒng)的傳遞函數(shù)框圖如圖9所示。

圖9 加入傳統(tǒng)Smith預(yù)估器后控制系統(tǒng)的傳遞函數(shù)框圖

整個控制系統(tǒng)的單位階躍仿真結(jié)果如圖10所示。

圖10 加入傳統(tǒng)Smith預(yù)估器后控制系統(tǒng)的單位階躍響應(yīng)

由圖10可以看出：加入傳統(tǒng)Smith預(yù)估器之后，系統(tǒng)基本沒有超調(diào)量，誤差減小，系統(tǒng)的響應(yīng)速度明顯加快，消除了純滯后環(huán)節(jié)的影響。

傳統(tǒng)的Smith預(yù)估器的良好控制性能是基于受控裝置的精確模型，但是對于整個控制系統(tǒng)，τ是一個可變量，會有一些偏差。當(dāng)τm≠τ時，控制系統(tǒng)的控制性能會有一定的減弱。例如，當(dāng)τ=0.1，τm=0.08時，在加入傳統(tǒng)Smith預(yù)估器之后，控制系統(tǒng)的單位階躍仿真結(jié)果如圖11所示。

圖11 控制系統(tǒng)加入非精確傳統(tǒng)Smith預(yù)估器時的單位階躍響應(yīng)

如圖11所示，當(dāng)τm≠τ時，控制系統(tǒng)的單位階躍響應(yīng)出現(xiàn)了超調(diào)現(xiàn)象。為了很好地克服傳統(tǒng)Smith預(yù)估器對模型誤差敏感的缺點，文獻(xiàn)[24]介紹了一種改進(jìn)型的Smith預(yù)估器。改進(jìn)型Smith預(yù)估器和傳統(tǒng)型Smith預(yù)估器的區(qū)別在于：在系統(tǒng)的主反饋回路中引入了作為低通濾波器的慣性項，誤差e經(jīng)過一階低通濾波器處理后反饋給控制器，減少了Smith預(yù)估模型中誤差的影響。改進(jìn)型Smith預(yù)估器結(jié)構(gòu)框圖如圖12所示。

圖12 改進(jìn)型Smith預(yù)估器結(jié)構(gòu)框圖

其中：e為R1(s)和Rm(s)的差值；Kf為低通濾波器增益，取Kf=1；Tf為低通濾波器時間常數(shù)，取Tf=0.01。

當(dāng)τ=0.1、τm=0.08時，控制系統(tǒng)在分別加入傳統(tǒng)型與改進(jìn)型Smith預(yù)估器非精確模型時的單位階躍響應(yīng)如圖13所示。

圖13 分別加入傳統(tǒng)型與改進(jìn)型Smith預(yù)估器非精確模型時的單位階躍響應(yīng)(τ=0.1，τm=0.08)

顯然，相比傳統(tǒng)Smith預(yù)估器，控制系統(tǒng)引入改進(jìn)型Smith預(yù)估器之后，超調(diào)現(xiàn)象消失。當(dāng)τ=0.08、τm=0.1時，其單位階躍響應(yīng)如圖14所示。

圖14 分別加入傳統(tǒng)型與改進(jìn)型Smith預(yù)估器非精確模型時的單位階躍響應(yīng)(τ=0.08，τm=0.1)

從圖14可以看出：相比傳統(tǒng)Smith預(yù)估器，改進(jìn)型Smith預(yù)估器提高了系統(tǒng)的響應(yīng)速度，降低了系統(tǒng)對延時常數(shù)的敏感性。

綜上，控制系統(tǒng)在引入改進(jìn)型Smith預(yù)估器之后，噴槍可以平穩(wěn)快速地到達(dá)目標(biāo)位置。

5 結(jié)論

針對在調(diào)整噴槍相對待噴面和地面距離時機械臂操作困難等問題，運用傾角傳感器和液壓比例驅(qū)動裝置實現(xiàn)了對機械臂的位姿控制。根據(jù)噴槍位姿，通過幾何法解算得到關(guān)節(jié)位置；在負(fù)載敏感比例多路換向閥的應(yīng)用中，通過增加電壓階躍信號，解決了比例閥零位死區(qū)對比例閥響應(yīng)速度的影響；采用改進(jìn)型Smith預(yù)估器，降低了純滯后環(huán)節(jié)對控制系統(tǒng)的干擾以及控制系統(tǒng)對延時常數(shù)的敏感性。最終通過角度解算及閉環(huán)控制，在理論方面證實了通過遙控器控制噴槍在水平方向和豎直方向上平穩(wěn)運動的可行性。

在實際的工程實驗當(dāng)中，該方法已經(jīng)運用到有關(guān)科研項目中，并驗證了該方法對關(guān)節(jié)位置控制有很好的實際控制效果，驅(qū)動機械臂帶動噴槍可以在巷道坐標(biāo)系的2個坐標(biāo)軸方向上平穩(wěn)運動，控制效果符合理論預(yù)期，滿足噴漿工藝對機械臂的工作要求。相對傳統(tǒng)的控制方法，該方法在保證控制精度的前提下，可以更方便地調(diào)整噴槍相對待噴面的距離和噴槍離地高度，具有很好的實用性。