空間柔性機(jī)械臂高階滑模干擾觀測(cè)器軌跡跟蹤

楊 洋， 王思明

(蘭州交通大學(xué) 電氣與自動(dòng)化工程學(xué)院，甘肅 蘭州 730070)

0 引 言

考慮到太空環(huán)境的特殊性[1～3]，為了更有效地完成軌跡跟蹤任務(wù)，本文采取二連桿的機(jī)械結(jié)構(gòu)，針對(duì)機(jī)械臂抗干擾等問(wèn)題進(jìn)行控制研究。

于瀟雁等人[4]主要采取了魯棒控制，針對(duì)振動(dòng)最優(yōu)問(wèn)題利用奇異攝動(dòng)法將漂浮基二連桿機(jī)械臂系統(tǒng)分為快慢2個(gè)子系統(tǒng)并設(shè)計(jì)了狀態(tài)觀測(cè)器。王興龍等人[5]研究了速度增益矩陣的動(dòng)態(tài)調(diào)整方法,對(duì)二連桿機(jī)械臂進(jìn)行MATLAB和ADAMS聯(lián)合仿真，有效地抑制了系統(tǒng)的外界干擾及抖動(dòng)。Muhammad Saad等人[6]運(yùn)用非線性分離原則設(shè)計(jì)了機(jī)械臂基于級(jí)聯(lián)高串聯(lián)狀態(tài)觀測(cè)器，使用高增益參數(shù)有效地跟蹤了非線性機(jī)械臂運(yùn)動(dòng)軌跡。Martino Capurso等人[7]運(yùn)用卡爾曼預(yù)估器原理進(jìn)行物理學(xué)建模，提出了一種無(wú)傳感器的LTP方法并且已實(shí)際運(yùn)用于現(xiàn)在冗余機(jī)械臂中，為軌跡跟蹤問(wèn)題提出了一種更為快速的方法。Hisashi Tamashima[8]主要針對(duì)雙關(guān)節(jié)機(jī)械手軌跡跟蹤問(wèn)題提出了一種向量不同，階級(jí)不同改進(jìn)狀態(tài)觀測(cè)器。

以上諸多問(wèn)題都是基于給定擾動(dòng)的簡(jiǎn)單方程進(jìn)行在線控制，而對(duì)于漂浮狀態(tài)下的機(jī)械臂可能產(chǎn)生的復(fù)雜擾動(dòng)方程并沒(méi)有給出具體的控制方法，且上述先進(jìn)算法計(jì)算量較大，極容易使操作者產(chǎn)生視覺(jué)生理疲勞，且需要具體研究對(duì)象的初始速度，以及需要實(shí)時(shí)監(jiān)控?cái)?shù)據(jù)，雖然實(shí)現(xiàn)了良好的跟蹤效果，但是過(guò)程較為繁瑣。

本文以漂浮基二連桿機(jī)械臂為研究對(duì)象，針對(duì)機(jī)械臂在漂浮狀態(tài)中極易產(chǎn)生內(nèi)部碰撞產(chǎn)生隨機(jī)擾動(dòng)的問(wèn)題，利用高階滑模算法設(shè)計(jì)了干擾觀測(cè)器，首先利用拉格朗日方程對(duì)系統(tǒng)進(jìn)行物理建模，結(jié)合對(duì)應(yīng)的控制律以及跟蹤軌跡方程對(duì)系統(tǒng)進(jìn)行模擬仿真實(shí)驗(yàn)，最后將控制器運(yùn)用于復(fù)雜方波軌跡中，實(shí)驗(yàn)結(jié)果表明，該方法不僅可以抑制系統(tǒng)產(chǎn)生的擾動(dòng)，還可運(yùn)用于更為復(fù)雜的軌跡中，有效地減少擾動(dòng)精確跟蹤期望軌跡。

1 二連桿機(jī)械臂建模

假設(shè)[9]:1)二連桿機(jī)械臂為鋼體結(jié)構(gòu)；2)空間忽略微重力對(duì)系統(tǒng)的影響；3)當(dāng)系統(tǒng)在漂浮狀態(tài)下，關(guān)節(jié)鉸和關(guān)節(jié)軸均為鋼體結(jié)構(gòu)。

因此，建立的二連桿機(jī)械臂模型如圖1所示。建立慣性坐標(biāo)系∑I,基座坐標(biāo)系∑O，關(guān)節(jié)坐標(biāo)系∑i和末端執(zhí)行器坐標(biāo)系∑e，O為慣性坐標(biāo)系的原點(diǎn)，C0為基座的質(zhì)心，Ci(i=0，1，2)為連桿質(zhì)心，Ji(i=0，1，2)為連桿i到連桿i+1的關(guān)節(jié)，CM為二連桿機(jī)械臂系統(tǒng)總質(zhì)心，r0∈R2為基座質(zhì)心位置矢量，ri∈R2(i=0，1，2)為連桿質(zhì)心位置矢量，rc∈R2為質(zhì)心位置矢量，re∈R2為末端執(zhí)行器位置矢量，pi∈R2(i=0，1，2)為連桿位置矢量。

圖1 二連桿空間機(jī)械臂平面結(jié)構(gòu)模型

設(shè)機(jī)械臂各分部均在慣性坐標(biāo)上運(yùn)動(dòng)，由于忽略微重力，系統(tǒng)動(dòng)量守恒，固由Lagrange第二類(lèi)方程可得漂浮下的機(jī)械臂系統(tǒng)運(yùn)動(dòng)學(xué)方程為

(1)

式中D(q)為系統(tǒng)7×7正定慣性矩陣，系統(tǒng)軌跡期望角q=[q0q1q2]T，系統(tǒng)角度補(bǔ)償量θ=[θ11θ12θ21θ22]T,C(q,q)∈R7×7為系統(tǒng)所受哥式力，離心力7階矩陣，非線性二連桿機(jī)械臂剛性矩陣為Z∈R6×6，系統(tǒng)電機(jī)驅(qū)動(dòng)力矩矩陣為τ=[τ1τ2]T。

為了更方便地進(jìn)行控制律設(shè)計(jì)，式(1)中所描述的系統(tǒng)具有以下幾個(gè)定理：

?Ω∈R7×7

(2)

(3)

式中W0∈R7×7為關(guān)節(jié)變量回歸矩陣，I1∈R1×7為系統(tǒng)驅(qū)動(dòng)參數(shù)向量矩陣。

2 空間機(jī)械臂干擾觀測(cè)器高階滑?？刂坡稍O(shè)計(jì)

2.1 干擾觀測(cè)器數(shù)學(xué)建模

根據(jù)上述對(duì)二連桿機(jī)械臂數(shù)學(xué)建模，將式(3)寫(xiě)成對(duì)角矩陣的形式，可得到

(4)

二連桿機(jī)械臂中電機(jī)的驅(qū)動(dòng)方程為

RI1+L1+K2r-1=v(t)

(5)

式中v(t)為電機(jī)電壓矢量，L和K分別為二關(guān)節(jié)不同電機(jī)驅(qū)動(dòng)系數(shù)。將I1從式(4)代入到式(5)有

W0vR-1

(6)

此時(shí)，假設(shè)一個(gè)n維空間關(guān)節(jié)位姿向量p存在于慣性坐標(biāo)系中，則p可表示為[10]

(7)

J(q)為Jacobian矩陣，對(duì)式(7)求導(dǎo)，可得

(8)

將式(7)、式(8)代入式(6)中，可得

(9)

為了使式(9)能夠更直觀地計(jì)算出基于此模型的干擾觀測(cè)器數(shù)學(xué)模型，所以，對(duì)式(9)求逆矩陣，并寫(xiě)成狀態(tài)輸出矩陣為

(10)

求得系統(tǒng)輸出u(t)得

u(t)=J-T(q)W0R-1v(t)

(11)

將觀測(cè)器與系統(tǒng)實(shí)際輸出相聯(lián)系，求解觀測(cè)器模型為

L(t)=(D-1(q)-I)u(t)-D-1(p+

C(q,)

(12)

2.2 高階滑模控制律設(shè)計(jì)

基于上述觀測(cè)器數(shù)學(xué)模型，定義x1=q,x2=， 將系統(tǒng)寫(xiě)成狀態(tài)方程為

(13)

式中k為系統(tǒng)補(bǔ)償矩陣，為了使得到的滑?？刂坡赡苁瓜到y(tǒng)能觀能控，假設(shè)：

假設(shè)1存在正定矩陣P，Q使矩陣K滿足如下方程式

(Ai-BiKi)Pi+Pi(Ai-BiKi)=-Qi,i=1,2

(14)

假設(shè)2式(13)中補(bǔ)償矩陣k滿足

|ki|≤u(t),i=1,2

(15)

首先將系統(tǒng)控制律寫(xiě)成

(16)

定義

(17)

將式(12)、式(13)代入式(17)中，并對(duì)式(17)求導(dǎo)，得到

(18)

根據(jù)上式，首先定義滑模面為

Φ=x1-x1d

(19)

對(duì)滑模面求二階導(dǎo)數(shù)

(20)

由式(20)可知，滑模面已出現(xiàn)控制量，為了更有效抑制擾動(dòng)得到控制量，則對(duì)式(20)求出三階導(dǎo)數(shù)，得

(21)

由式(21)可知，各個(gè)參數(shù)階次相加，系統(tǒng)共6階，而此方程中有關(guān)于驅(qū)動(dòng)電機(jī)的電流、電壓參數(shù)，這兩個(gè)參數(shù)在實(shí)際運(yùn)行中較為波動(dòng)，不容易檢測(cè)到，固本文在此基礎(chǔ)上重新添加一個(gè)補(bǔ)償正切函數(shù)，重新表示函數(shù)如下

(22)

(23)

式中λ1與λ2分別為對(duì)角矩陣，且‖λ1‖>‖λ2‖；系統(tǒng)在關(guān)節(jié)空間運(yùn)行過(guò)程中，ε1與ε2都為7階矩陣，且分別為兩個(gè)電機(jī)驅(qū)動(dòng)實(shí)時(shí)預(yù)估值。

得到預(yù)估值后，定義新的變量Γ1∈R7×7，Γ2∈R7×7，Γ3∈R7×7，將式(20)、式(21)重新表示為

(24)

本文研究的目的轉(zhuǎn)化為式(24)的零點(diǎn)收斂問(wèn)題，則可初步設(shè)置控制律為

(25)

式中f(t)為待設(shè)計(jì)階矩陣，其通用公式一般寫(xiě)為

f(t)=-d1|Γ1|a1tanh(|Γ1|)-d2|Γ2|a2tanh(|Γ2|)-

d3|Γ3|a3tanh(Γ3|)

(26)

式中d1，d2，d3為3個(gè)7階對(duì)角矩陣，并且滿足Hurwitz多項(xiàng)式定理[11]，a1,a2,a3為3個(gè)參數(shù)滿足

(27)

由于系統(tǒng)在運(yùn)行過(guò)程中，不可避免地存在干擾，則在滑模函數(shù)一般形式上增加一個(gè)積分函數(shù)來(lái)抑制干擾，積分滑模函數(shù)為

(28)

對(duì)式(24)所得的系統(tǒng)，重新分配參數(shù)設(shè)計(jì)控制器如下

(29)

2.3 系統(tǒng)穩(wěn)定性證明

定義Lyapunov函數(shù)為[12]

V=1/2sTs

(30)

求導(dǎo)可得

(31)

將式(24)、式(26)、式(29)代入式(31)中，化簡(jiǎn)并將已知參數(shù)代入可得

=-sT{D-1(x1)·

(32)

由李雅普諾夫定理可知，系統(tǒng)在有效時(shí)間內(nèi)收斂，固系統(tǒng)穩(wěn)定性得證，此高階滑?？刂破骺捎糜谀Ｐ椭小?/p>

3 高階滑模干擾觀測(cè)器仿真研究

在MATLAB中對(duì)圖1進(jìn)行模擬仿真實(shí)驗(yàn)，采用文獻(xiàn)[13]的系統(tǒng)參數(shù)，式(12)觀測(cè)器模型和式(29)的高階滑?？刂坡蛇M(jìn)行編程仿真，具體參數(shù)體現(xiàn)如下：載體B0:m0=40 kg，L0=1.5 m，J0=34.17 kg·m2；剛性臂B1:m1=2 kg,L1=3.0 m，J1=1.50 kg·m2；剛性臂B2:m2=1 kg，L2=3.0 m，J2=0.75 kg·m2?？刂破鲄?shù)選擇為

仿真過(guò)程中加入外界干擾和粘性摩擦為

F()=sgn()+k

(33)

設(shè)關(guān)節(jié)一和關(guān)節(jié)二的理想軌跡分別為：x1d=15×sin(0.2πt)，x2d=8×sin(0.6πt)仿真時(shí)間為10 s，仿真結(jié)果如圖2所示。

圖2 仿真結(jié)果

圖2(a)為關(guān)節(jié)一軌跡跟蹤情況曲線，黑色實(shí)線為期望軌跡，紅色虛線為系統(tǒng)實(shí)際運(yùn)行曲線，下圖為關(guān)節(jié)一速度跟蹤曲線，圖2(b)跟圖2(a)類(lèi)似，由于滑模算法本身具有抖動(dòng)效果，所以,系統(tǒng)在剛運(yùn)行過(guò)程中跟蹤效果并不是很好，但關(guān)節(jié)一與關(guān)節(jié)二在大約1 s內(nèi)逐漸跟蹤上期望軌跡，并在接下來(lái)9 s的運(yùn)行中，并未出現(xiàn)較為明顯的抖動(dòng)和誤差，跟蹤效果較為良好。

在關(guān)節(jié)一和關(guān)節(jié)二上的加摩擦力曲線，在干擾觀測(cè)器的觀測(cè)下，效果如圖3所示。

圖3 關(guān)節(jié)一與關(guān)節(jié)二干擾觀測(cè)效果

由圖3可知，系統(tǒng)剛開(kāi)始運(yùn)行觀測(cè)效果較不明顯，但是關(guān)節(jié)一與關(guān)節(jié)二大約在1 s之后，觀測(cè)器能精確觀測(cè)到干擾曲線，而并未出現(xiàn)較為明顯的誤差。

為了更進(jìn)一步證明此算法能運(yùn)用于更為復(fù)雜的情況中，將機(jī)械臂理想曲線改為方波曲線進(jìn)行進(jìn)一步調(diào)試，結(jié)果表明，將此算法運(yùn)用于方波軌跡中，系統(tǒng)在1 s后也與理想軌跡想重合，在后期工作中系統(tǒng)并為出現(xiàn)過(guò)大的波動(dòng)。

4 結(jié) 論

實(shí)際仿真結(jié)果表明，在短時(shí)間內(nèi)能夠?qū)崿F(xiàn)跟蹤理想軌跡的任務(wù)，能夠?qū)崟r(shí)監(jiān)控系統(tǒng)所受到的干擾摩擦等因素，不僅能夠應(yīng)用于正弦或者余弦曲線，亦能應(yīng)用于方波曲線中。該研究對(duì)空間機(jī)械臂提供了一個(gè)新型的算法進(jìn)行參考，同時(shí)也為該控制領(lǐng)域提供了一個(gè)新的方向。