基于自適應(yīng)降階滑模算法的受約束多機(jī)械臂力/位混合控制

朱志浩，李蔚，高直，郭毓

(1.鹽城工學(xué)院電氣工程學(xué)院，江蘇鹽城 224051；2.鹽城工學(xué)院信息工程學(xué)院，江蘇鹽城 224051；3.南京理工大學(xué)自動(dòng)化學(xué)院，江蘇南京 210094 )

0 前言

近年來，隨著我國工業(yè)的快速發(fā)展，機(jī)器人的應(yīng)用得到了極大的發(fā)展，并逐步運(yùn)用到了制造業(yè)、農(nóng)業(yè)等領(lǐng)域。同時(shí)國內(nèi)對于機(jī)器人的研究也逐年深入，但這些應(yīng)用和研究大多集中于單機(jī)械臂領(lǐng)域[1-2]，對于多機(jī)械臂[3]的研究和應(yīng)用仍有所缺乏。在實(shí)際應(yīng)用方面，單機(jī)械臂有著效率低下、易受干擾、無法適應(yīng)復(fù)雜的工作環(huán)境、面對大而重的物體力不從心等諸多局限性，因此相較于單機(jī)械臂系統(tǒng)，多機(jī)械臂系統(tǒng)有很大的優(yōu)勢。

雖然相比于單機(jī)械臂系統(tǒng)，多機(jī)械臂在實(shí)際運(yùn)用中有著諸多的優(yōu)勢，但這也對其提出了更高的控制要求。當(dāng)多機(jī)械臂控制物體在約束環(huán)境上運(yùn)動(dòng)時(shí)，多機(jī)械臂與物體形成一個(gè)整體，所以不僅要考慮控制物體按照期望的位置指令運(yùn)動(dòng)，控制物體與約束環(huán)境間約束力，還需要考慮控制機(jī)械臂對于物體的夾持內(nèi)力[4]，保證機(jī)械臂穩(wěn)固控制物體的同時(shí)不對物體造成損傷。因此，針對多機(jī)械臂系統(tǒng)的力/位混合控制問題[5-8]，國內(nèi)外專家們提出了諸多的控制方法。文獻(xiàn)[9-13]將命令濾波與反步法相結(jié)合，同時(shí)基于誤差補(bǔ)償系統(tǒng)來消除濾波誤差。文獻(xiàn)[14]設(shè)計(jì)了一種滑?？刂扑惴?，解決了協(xié)調(diào)機(jī)械臂不確定性問題。文獻(xiàn)[15]設(shè)計(jì)了一種神經(jīng)網(wǎng)絡(luò)自適應(yīng)算法，克服模型建模以及時(shí)間滯后對系統(tǒng)的影響。然而這些方法控制器的設(shè)計(jì)都比較復(fù)雜，并且只單獨(dú)考慮位置和內(nèi)力的問題，忽略了約束力的影響。本文作者提出一種基于自適應(yīng)降階滑模算法的受約束多機(jī)械臂力/位混合控制方法，首先將位置控制、約束力控制、內(nèi)力控制引入同一控制器；其次設(shè)計(jì)自適應(yīng)律補(bǔ)償未知擾動(dòng)提高控制精度，利用降階方法提高響應(yīng)速度；最終經(jīng)過李雅普諾夫方法，證明系統(tǒng)的穩(wěn)定性，并經(jīng)過仿真證明控制策略的有效性。

1 問題描述

建立由k個(gè)n自由度機(jī)械臂及被控物體所組成的多機(jī)械臂系統(tǒng)。多機(jī)械臂系統(tǒng)坐標(biāo)系由三部分組成，如圖1所示：任務(wù)空間坐標(biāo)系{O}，以物體的重心為原點(diǎn)的坐標(biāo)系{G}，以機(jī)械臂末端執(zhí)行器為原點(diǎn)的坐標(biāo)系{Ei}(1≤i≤k)。為方便研究，滿足如下幾點(diǎn)假設(shè)[16]：

圖1 多機(jī)械臂系統(tǒng)

假設(shè)1，每個(gè)機(jī)械臂自由度相同；

假設(shè)2，被控物體為剛性物體；

假設(shè)3，整個(gè)系統(tǒng)為非冗余的；

假設(shè)4，所有轉(zhuǎn)換矩陣均滿秩且有界。

1.1 多機(jī)械臂動(dòng)力學(xué)模型

第i個(gè)機(jī)械臂動(dòng)力學(xué)模型[17]為

(i=1,2,…,k)

(1)

由式(1)可得k個(gè)機(jī)械臂所組成的多機(jī)械臂動(dòng)力學(xué)模型：

(2)

其中：

D(q)=blockdiag[D1(q1),D2(q2),…,Dk(qk)]∈Rkn×kn；

Je=blockdiag[Je1,Je2,…,Jek]∈Rkn×kn；

式(2)所描述的動(dòng)力學(xué)模型具有如下性質(zhì)：

性質(zhì)1：存在正數(shù)k1、k2，滿足下式：

(3)

(4)

性質(zhì)3：存在參數(shù)線性化關(guān)系：

(5)

1.2 被控物體動(dòng)力學(xué)模型

被控物體的動(dòng)力學(xué)方程為

(6)

(7)

其中：Joi(x)∈Rn×n為從機(jī)械臂末端位置向量到x的Jacobian矩陣；Fei∈Rn為機(jī)械臂末端對物體的力。Fe可分為內(nèi)力FI∈Rk×n和外力Fo∈Rk×n兩部分。

由式(7)可得：

(8)

1.3 多機(jī)械臂系統(tǒng)動(dòng)力學(xué)模型

xe=φ(q)

(9)

其中：φ(q)∈Rn×n為從q到位置向量xe的變換矩陣。

對式(9)左右同時(shí)求關(guān)于時(shí)間的導(dǎo)數(shù)，得：

(10)

又有：

(11)

其中：Jo(x)∈Rn×n。

由式(10)(11)可推導(dǎo)出：

由假設(shè)4可知Je可逆，則有：

(12)

對式(12)左右同時(shí)求關(guān)于時(shí)間的導(dǎo)數(shù)，得：

(13)

把式(12)(13)(8)(6)代入式(2)，可得：

(14)

(15)

式(15)描述的動(dòng)力學(xué)方程具備與式(2)相似的性質(zhì)。

1.4 受約束多機(jī)械臂系統(tǒng)動(dòng)力學(xué)模型

設(shè)多機(jī)械臂系統(tǒng)在m維約束環(huán)境運(yùn)行。取xc為物體與約束面的接觸點(diǎn)，則對于物體重心位置向量x，有xc=h(x)，因此約束環(huán)境可表示為

ψ(x)=Φ(h(x))=Φ(xc)=0

(16)

由式(16)左右兩邊同時(shí)關(guān)于時(shí)間t求導(dǎo)得：

(17)

由式(6)可得受約束物體得動(dòng)力學(xué)方程為

(18)

(19)

(20)

(21)

將式(20)(21)代入式(19)，得：

τa+Fc

(22)

則受約束多機(jī)械臂系統(tǒng)動(dòng)力學(xué)方程表示為

(23)

式(23)所描述的動(dòng)力學(xué)模型具有如下性質(zhì)：

性質(zhì)2：具備與式(2)性質(zhì)3相似的性質(zhì)，即滿足參數(shù)線性化關(guān)系。

將式(23)兩邊同左乘LT(x1)，推導(dǎo)可得：

(24)

2 控制器設(shè)計(jì)

設(shè)xd為期望的物體重心位置向量，定義以下變量：

(25)

其中：x1e為位置誤差；x1d為x1的期望位置；x1r為系統(tǒng)誤差向量；Λ>0。

滑模函數(shù)為

(26)

力誤差：

eλ=λ-λd

(27)

其中：λd為期望接觸力。

設(shè)計(jì)控制器為

(28)

由式(23)及其參數(shù)線性化關(guān)系可得：

(29)

其中：Ya為回歸矩陣；Pa為系統(tǒng)未知定常向量。

用于控制力的項(xiàng)：

λr=λd-Kλeλ

(30)

則可得位置和力的控制器為

(31)

(32)

設(shè)計(jì)如下李雅普諾夫函數(shù)為

對等號兩邊關(guān)于時(shí)間t求導(dǎo)得：

(33)

式中：λmin為矩陣LTL的最小特征值。

為了使得機(jī)械臂有效控制物體，則在受約束多機(jī)械臂系統(tǒng)控制器中加入內(nèi)力控制，與式(14)相似，將式(12)(13)(8)(18)(20)(21)代入式(2)得：

(34)

其中：

CoL

設(shè)用于控制內(nèi)力的項(xiàng)為

CoL

CoL

FIr=FId-KIeFI

其中：KI>0；eFI=FI-FId，F(xiàn)Id為期望內(nèi)力。

(35)

則最終的受約束多機(jī)械臂系統(tǒng)控制器為

(36)

3 仿真分析

針對受約束雙二自由度機(jī)械臂系統(tǒng)模型進(jìn)行仿真驗(yàn)證，進(jìn)而證明文中所設(shè)計(jì)控制方案的有效性。受約束雙二自由度機(jī)械臂系統(tǒng)模型如圖2所示。

圖2 受約束雙機(jī)械臂模型

其機(jī)械臂的動(dòng)力學(xué)模型參數(shù)如下：

Ci(qi)=

Gi(qi)=

Jei(qi)=

物體的動(dòng)力學(xué)模型參數(shù)如下：

取參數(shù)：

控制器參數(shù)如下：

li1=li2=1.0 m，mi1=mi2=1.0 kg，mo=2.0 kg，

g=9.8 m/s2，R=0.2 m，

x1d(0)=0.6，x2d(0)=1.117 2，

x1(t)=0.8+0.1cost，

FId1=FId2=7 N，F(xiàn)Id3=FId4=-7 N，

Λ=11I2，Kd=25I2，w=2I2，

Kλ=10，KI=7I2，Γ=7I2。

仿真結(jié)果如圖3—6所示。由圖3可以看出：與未改進(jìn)的多機(jī)械臂自適應(yīng)位置控制相比，文中所使用的基于自適應(yīng)降階滑模的力/位混合控制算法能夠有效提高控制器的控制精度和系統(tǒng)的響應(yīng)速度；在位置指令跟蹤上，文中所使用的方法雖然在起初有較大波動(dòng)，但很快便在1.6 s時(shí)跟蹤上，而反觀對比方法在2.4 s才跟蹤上，響應(yīng)速度有了顯著提升，并且對比方法在之后的跟蹤過程中仍有一定的波動(dòng)產(chǎn)生，而所設(shè)計(jì)控制方案就能夠很好地貼合期望軌跡。從圖4—6來看，所設(shè)計(jì)的控制方案的約束力誤差、內(nèi)力誤差雖然起初有一定的波動(dòng)，但很快趨于平穩(wěn)，且將誤差限定在極小的范圍內(nèi)，說明了所設(shè)計(jì)控制器的有效性。

圖3 位置跟蹤

圖4 約束力跟蹤誤差

圖5 機(jī)械臂1內(nèi)力誤差

4 結(jié)束語

針對受約束多機(jī)械臂系統(tǒng)模型，將自適應(yīng)滑模算法與降階方法相結(jié)合，實(shí)現(xiàn)了受約束多機(jī)械臂的力/位混合控制。仿真結(jié)果表明：所設(shè)計(jì)的控制方案能夠有效提高控制器的控制精度和系統(tǒng)的響應(yīng)速度，且多機(jī)械臂系統(tǒng)的內(nèi)力和約束力可以限定在理想的范圍內(nèi)，驗(yàn)證了控制方案的有效性。