基于關(guān)節(jié)速度的液壓比例驅(qū)動工程機械臂位姿控制

高 波，王珞豪

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

引言

工程機械是我國的重要產(chǎn)業(yè)之一，廣泛應(yīng)用于道路、建筑、礦山和水利等工程領(lǐng)域，是生產(chǎn)、建設(shè)的重要工程設(shè)備[1-2]。液壓比例閥控系統(tǒng)擁有結(jié)構(gòu)簡單、輸出力矩大、可靠性較高、響應(yīng)快、傳動效率及控制精度高等優(yōu)點[3]。負載敏感系統(tǒng)具有效率高、體積小且結(jié)構(gòu)緊湊等優(yōu)點，并能對系統(tǒng)流量與壓力關(guān)系實現(xiàn)自動控制，廣泛應(yīng)用于工程機械[4-5]，因此使用負載敏感液壓比例閥控系統(tǒng)驅(qū)動機械臂關(guān)節(jié)。

由于閥控液壓缸系統(tǒng)完成的是一個直線運動，伺服系統(tǒng)中常采用精度較高的位置傳感器測出位置。但在實際工程機械應(yīng)用中，在比例閥控缸安裝位置傳感器會使得系統(tǒng)復(fù)雜，可靠性與效率降低。一些工程機械嘗試用傾角傳感器獲得部分關(guān)節(jié)的姿態(tài)并加以控制，取得了較好的效果，但局限性較大，只適用于少數(shù)應(yīng)用。

為解決這個問題，本研究基于對機器人正逆運動學(xué)、閥控液壓缸控制、陀螺儀姿態(tài)解算進行研究分析，實現(xiàn)機械臂位姿閉環(huán)控制，從而可以取代閥控液壓系統(tǒng)位置閉環(huán)伺服控制。由此基礎(chǔ)上通過MATLAB/Simulink軟件實現(xiàn)基于關(guān)節(jié)速度控制的比例液壓系統(tǒng)驅(qū)動機械臂閉環(huán)控制建模，仿真并驗證方法的合理性和可行性。

1 工程機械臂機器人學(xué)分析

1.1 正運動學(xué)分析

對液壓驅(qū)動機械臂位姿控制之前，需要進行必要的運動學(xué)分析，求得各關(guān)節(jié)連桿坐標(biāo)變換關(guān)系以及機械手的運動學(xué)正解和逆解。使用D-H法建立機器人運動學(xué)模型，該方法通過4個參數(shù)連桿長度ai、連桿扭轉(zhuǎn)角αi、連桿偏距di、關(guān)節(jié)角θi來描述機器人相鄰連桿之間的位姿關(guān)系，進而建立機器人模型。本研究以噴漿機械臂為例[6]，各關(guān)節(jié)均采用擺動關(guān)節(jié)，取代原先為便于操控而設(shè)置的伸縮關(guān)節(jié)，使其結(jié)構(gòu)更為優(yōu)化，建立連桿坐標(biāo)系如圖1所示。根據(jù)連桿坐標(biāo)系，可得各連桿坐標(biāo)參數(shù)如表1所示。

圖1 噴漿機械臂連桿坐標(biāo)系

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

其中d1=0.5，a2=2，d4=2。根據(jù)表1的各連桿參數(shù)，由連桿變換矩陣式(1)得到各連桿的變換矩陣：

Ai=

(1)

由此得出正運動學(xué)模型，機械臂末端坐標(biāo)系相對于基坐標(biāo)系的位姿變換矩陣如式(2):

(2)

使用MATLAB中的機器人工具箱構(gòu)建機械臂仿真，驗證了機械臂正運動學(xué)模型的正確性，為后面進行逆運動學(xué)分析奠定了基礎(chǔ)。

1.2 逆運動學(xué)分析

逆運動學(xué)是由已知的機械臂末端位姿，計算機械臂的各關(guān)節(jié)變量，本研究采用代數(shù)法求逆解。

由于三軸加速度計在工程機械實際應(yīng)用中受限于技術(shù)水平而導(dǎo)致累積誤差較大。超高精度的捷聯(lián)慣導(dǎo)系統(tǒng)位置累積誤差仍可達到10 cm/h，因此對于本研究的六關(guān)節(jié)機械臂，選擇使用2個陀螺儀分別裝于關(guān)節(jié)3與關(guān)節(jié)6處獲取姿態(tài)信息。關(guān)節(jié)3處的姿態(tài)信息用于對關(guān)節(jié)角θ1，θ2，θ3進行解算，關(guān)節(jié)6處的姿態(tài)信息結(jié)合前3個關(guān)節(jié)信息可以實現(xiàn)對θ4，θ5，θ6的解算，由此完成機械臂逆運動學(xué)分析。

1.3 陀螺儀姿態(tài)解算

陀螺儀能夠通過測量載體運動角速度解算得出偏航角、俯仰角與滾轉(zhuǎn)角獲取姿態(tài)信息[7]。在進行機器人姿態(tài)解算時，本研究采用四元數(shù)算法[8]對姿態(tài)矩陣進行求解，通過四元數(shù)的變化對姿態(tài)矩陣進行更新，通過式(3)求得四元數(shù)：

式中，ωbx，ωby，ωbz由陀螺儀輸出計算得到的角速度信息，利用式(3)求得的四元數(shù)表示為姿態(tài)矩陣，如式(4):

C=

(4)

由式(4)可得關(guān)節(jié)3處的姿態(tài)矩陣C3與關(guān)節(jié)6處的姿態(tài)矩陣C6：

(5)

(6)

1.4 逆運動學(xué)求解

1)求解關(guān)節(jié)角θ1，θ2，θ3

由于工程機械臂在結(jié)構(gòu)上θ2與θ3同為俯仰角且軸線在同一平面內(nèi)，故θ3處陀螺儀所測得同一姿態(tài)下θ2，θ3逆解時出現(xiàn)多解，為避免這個問題，本研究在θ2處安裝一個傾角傳感器輸出θ2的值；也可對機械臂關(guān)節(jié)設(shè)計進行優(yōu)化，避免出現(xiàn)這種沖突，如調(diào)換關(guān)節(jié)3和關(guān)節(jié)4，但如何優(yōu)化效果最佳及其數(shù)學(xué)表達仍待進一步研究。

(7)

(8)

(9)

式中,C11，C13，C21，C23，C31為式(5)中矩陣C3的各個元素，由式(8)、式(9)可解得關(guān)節(jié)1、關(guān)節(jié)3的值θ1，θ3。

2)求解關(guān)節(jié)角θ4，θ5，θ6

(10)

(11)

式中,

r13=azsin(θ2+θ3)+axcos(θ2+θ3)cosθ1+

aycos(θ2+θ3)sinθ1

(12)

r23=azcos(θ2+θ3)-axsin(θ2+θ3)cosθ1-

aysin(θ2+θ3)sinθ1

(13)

r33=axsinθ1-aycosθ1

(14)

r21=nzcos(θ2+θ3)-nxsin(θ2+θ3)cosθ1-

nysin(θ2+θ3)sinθ1

(15)

r22=ozcos(θ2+θ3)-oxsin(θ2+θ3)cosθ1-

oysin(θ2+θ3)sinθ1

(16)

上式中ni，oi，ai為式(6)矩陣C6中各個元素，令式(10)、式(11)兩矩陣的元素(1,3)、(2,1)、(2,2)、(2,3)、(3,3)對應(yīng)相等可解得：

(17)

θ4=atan 2(r33,r13)

(18)

θ6=atan 2(-r22,r21)

(19)

由此根據(jù)已知陀螺儀解算的姿態(tài)矩陣可逆解求得各個關(guān)節(jié)的角度值，且機械臂末端位置也可通過正運動學(xué)一并解出，據(jù)此可實現(xiàn)位姿閉環(huán)以獲得各關(guān)節(jié)速度給定值。

2 位姿閉環(huán)控制模型

工程機械臂的各關(guān)節(jié)通過負載敏感比例閥控液壓系統(tǒng)驅(qū)動。負載敏感液壓系統(tǒng)是一種根據(jù)系統(tǒng)壓及流量需求動態(tài)調(diào)整提供所需求的流量和壓力大小的液壓回路[10]，使執(zhí)行元件的速度僅與比例換向閥開度有關(guān)，而與負載壓力無關(guān)。電磁比例閥是液壓系統(tǒng)的流控元件，響應(yīng)速度快且控制方便[11]。從而根據(jù)本研究內(nèi)容建立電磁比例閥控液壓開環(huán)系統(tǒng)，如圖2所示。

圖2 比例閥控液壓油缸數(shù)學(xué)模型

圖2中，Ka(A/V)為PWM比例放大器比例系數(shù)；Kv為電磁比例閥流量增益，m3·A/s；Kq為閥的空載流量增益，m3·A/s；Ap為活塞有效作用面積，m2；ωh為液壓固有頻率；ζh為液壓阻尼比[12]。

通過對閥控液壓系統(tǒng)綜合設(shè)計分析，其開環(huán)傳遞函數(shù)可以近似為：

(20)

連桿與液壓缸不同的結(jié)構(gòu)組合會使活塞位移與連桿間角度變換關(guān)系發(fā)生變化，本研究中液壓油缸驅(qū)動噴漿機器人大臂、小臂關(guān)節(jié)的原理如圖3所示[13]。

圖3 液壓驅(qū)動關(guān)節(jié)原理

圖3中，ls為關(guān)節(jié)2與基座間的距離；lr為油缸與大臂連接處A至關(guān)節(jié)2間的距離；xp為油缸活塞位移；lc為油缸長度；θ為兩連桿間的角度。根據(jù)圖3由余弦定理計算可得，液壓缸活塞位移與連桿間角度變換關(guān)系為：

(21)

通過對液壓缸模型與機器人運動學(xué)的分析，可構(gòu)建出位姿閉環(huán)控制總框圖，如圖4所示。

由圖4可知，輸入由目標(biāo)位姿T逆解得到的各關(guān)節(jié)目標(biāo)角度θT，同時通過裝于機械臂的陀螺儀測出當(dāng)前姿態(tài)，利用逆解算法解算出當(dāng)前的各關(guān)節(jié)角度值θd，與目標(biāo)關(guān)節(jié)角θT進行比較，以此偏差值進行調(diào)節(jié)控制，其輸出作為關(guān)節(jié)速度信號給定輸入到閥控液壓系統(tǒng)，實現(xiàn)對機械臂的位姿調(diào)整，最終完成位姿閉環(huán)控制。也可以直接用位姿誤差根據(jù)雅可比方程求解關(guān)節(jié)速度給定量，但是這種方法需要實時計算機器人逆雅可比矩陣，結(jié)構(gòu)復(fù)雜且計算量大，影響系統(tǒng)實時性[14]。

圖4 噴漿機械臂位姿閉環(huán)控制框圖

3 閉環(huán)控制仿真

3.1 仿真模型構(gòu)建

利用MATLAB的機器人工具箱對機器人進行運動學(xué)仿真并對建模的正確性進行驗證。利用表1所列的D-H參數(shù)建立噴漿機械手模型。

根據(jù)第2節(jié)所述的閉環(huán)控制框圖，通過 MATLAB/Simulink建立關(guān)節(jié)速度控制的液壓驅(qū)動機械臂位姿閉環(huán)控制模型，仿真框圖如圖5所示。

1.目標(biāo)關(guān)節(jié)角 2.比例增益 3.液壓缸模型 4.機械臂模型 5.逆運動學(xué)解算模塊 6.正運動學(xué)解算模塊 7.末端姿態(tài)輸出 8.末端位置輸出

傾角傳感器輸出的θ2值與陀螺儀輸出姿態(tài)矩陣C3，C6輸入到逆運動學(xué)解算函數(shù)模塊中，其輸出的關(guān)節(jié)值通過正運動學(xué)解算模塊得出當(dāng)前機械臂末端位姿，同時與輸入的目標(biāo)值進行比較，通過控制器將關(guān)節(jié)速度量輸入到液壓油缸模型中，進而輸入到由Simulink構(gòu)建的機械臂模型中對位姿進行調(diào)整，實現(xiàn)整個閉環(huán)控制仿真。

3.2 閉環(huán)控制仿真驗證

使用MATLAB/Simulink進行仿真，設(shè)置機械臂初始關(guān)節(jié)位置、目標(biāo)關(guān)節(jié)位置,由于關(guān)節(jié)3結(jié)構(gòu)原因，故設(shè)置關(guān)節(jié)3初始值小于0，具體仿真結(jié)果如表2所示。設(shè)陀螺儀漂移誤差0.01(°)/h，陀螺儀白噪聲背離標(biāo)準(zhǔn)0.01(°)/h,采樣頻率設(shè)置為100 Hz。設(shè)置θ1，θ4，θ5，θ6關(guān)節(jié)角速度不超過0.3 rad/s,大臂關(guān)節(jié)角θ2、小臂關(guān)節(jié)角θ3關(guān)節(jié)速度限定不超過0.2 rad/s。根據(jù)以上參數(shù)得到機械臂末端位姿、各關(guān)節(jié)角度值、速度變化仿真曲線以及陀螺儀測量值變化仿真曲線如圖6、圖7所示。

表2 噴漿機器人閉環(huán)仿真關(guān)節(jié)參數(shù) rad/s

圖7 陀螺儀測量值仿真

1)陀螺儀測量仿真

在引入漂移誤差與白噪聲后，對姿態(tài)角測量誤差進行仿真，結(jié)果如圖6所示。

陀螺漂移是慣導(dǎo)系統(tǒng)的主要誤差源，會激勵舒勒周期振蕩、地球周期振蕩及傅科周期振蕩[15]。從圖6結(jié)果曲線分析，陀螺儀測量誤差呈現(xiàn)振蕩特性。橫滾角、俯仰角、航向角測量誤差的標(biāo)準(zhǔn)差分別為0.0017，0.0023，0.0014 rad/s，變化趨勢穩(wěn)定。

關(guān)節(jié)3處與關(guān)節(jié)6處陀螺儀測量值仿真如圖7所示。陀螺儀測量信號為角速度量，由圖7可知關(guān)節(jié)3與關(guān)節(jié)6處陀螺儀測量值在仿真時間內(nèi)趨近于0達到穩(wěn)定狀態(tài)。

2)位姿精度分析

由于陀螺儀輸出包含了隨機誤差、噪聲誤差、漂移誤差等，在實際應(yīng)用中會對姿態(tài)角的解算造成影響。根據(jù)仿真過程中陀螺儀的輸出數(shù)據(jù)，解算得到位姿測量軌跡，將其與位姿標(biāo)準(zhǔn)軌跡進行對比分析。姿態(tài)角變化及誤差如圖8所示。

從圖8的姿態(tài)角誤差變化可知，橫滾角α、俯仰角β、航向角γ誤差Eα，Eβ，Eγ最大值為0.167，0.028，0.103 rad，仿真時間內(nèi)最終姿態(tài)相對誤差角分別為-0.083，0.034，-0.028 rad，曲線基本呈穩(wěn)定趨勢，且滿足精度要求。

圖8 姿態(tài)角及其誤差變化

末端位置變化及誤差如圖9所示。將仿真解算的末端位置分量與標(biāo)準(zhǔn)值進行對比分析，X，Y，Z定位誤差最大值為0.026，0.036，0.096 m，仿真結(jié)果誤差Ex,Ey,Ez分別為0.014，-0.027，0.056 m，滿足設(shè)計精度要求，并能夠較穩(wěn)定的跟蹤標(biāo)準(zhǔn)軌跡。綜合對位姿誤差分析可知，陀螺儀測量誤差會對位姿解算精度造成影響，但誤差值在精度要求范圍內(nèi)，誤差對系統(tǒng)穩(wěn)定性的影響由以下仿真給出。

圖9 末端位置及其誤差變化

3)機械臂參數(shù)仿真

機械臂關(guān)節(jié)速度及誤差變化仿真控制結(jié)果如圖10所示。由關(guān)節(jié)速度曲線圖10a、圖10b可知液壓驅(qū)動機械臂各關(guān)節(jié)值變化曲線平滑，各關(guān)節(jié)在速度限定范圍內(nèi)運動并最終收斂到0達到穩(wěn)定狀態(tài)。由誤差曲線圖10c、圖10d可知各關(guān)節(jié)變化與目標(biāo)值誤差在仿真時間內(nèi)不斷減小并趨于穩(wěn)定，角度參數(shù)的跟蹤誤差如表2所示，其控制精度滿足要求。

圖10 各關(guān)節(jié)速度、誤差變化曲線

末端位姿速度變化曲線如圖11所示。液壓驅(qū)動機械臂位姿閉環(huán)控制系統(tǒng)響應(yīng)時間為20 s，機械臂末端在仿真運動過程中位姿速度變化曲線逐漸趨向于零保持穩(wěn)定。在2個陀螺儀與1個傾角傳感器聯(lián)合定位下，結(jié)合機器人正逆運動學(xué)，所建立的液壓驅(qū)動機械臂位姿閉環(huán)控制系統(tǒng)能相對穩(wěn)定的達到仿真目標(biāo)要求，輸出結(jié)果與目標(biāo)基本一致，驗證了方法的可行性。

圖11 機械臂末端位姿速度變化曲線

4 結(jié)論

針對在工程機械上液壓系統(tǒng)因加裝位置傳感器而致使系統(tǒng)復(fù)雜的問題，本研究用機器人位姿閉環(huán)控制取代了液壓系統(tǒng)中的位置閉環(huán)，并對于工程作業(yè)下慣導(dǎo)系統(tǒng)的加速度計誤差較大的問題，使用誤差較小陀螺儀解決機械手并采用激光雷達對其累積誤差進行修正。運用機器人正逆運動學(xué)知識，分析了工程機械臂關(guān)節(jié)變量和末端執(zhí)行器位姿之間的坐標(biāo)變換關(guān)系，解決了機械臂運動學(xué)方程的建立以及由末端姿態(tài)逆解出各關(guān)節(jié)角的問題并建立了比例控制液壓缸數(shù)學(xué)模型。利用MATLAB/Simulink進行建模仿真，得到各參數(shù)的曲線和數(shù)據(jù)，仿真結(jié)果表明了該控制系統(tǒng)的可行性。本研究對于液壓驅(qū)動工程機械臂位姿閉環(huán)控制具有一定參考意義。此外，課題還存在待解決的問題需要進一步研究：

(1)高精度光纖陀螺儀的微型化；

(2)機器人關(guān)節(jié)布置的優(yōu)化以及位姿解算的進一步數(shù)學(xué)分析。；

(3)位姿閉環(huán)處控制方法的研究。