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

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

0 前言

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

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

1 問題描述

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

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

假設1，每個機械臂自由度相同；

假設2，被控物體為剛性物體；

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

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

1.1 多機械臂動力學模型

第i個機械臂動力學模型[17]為

(i=1,2,…,k)

(1)

由式(1)可得k個機械臂所組成的多機械臂動力學模型：

(2)

其中：

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

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

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

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

(3)

(4)

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

(5)

1.2 被控物體動力學模型

被控物體的動力學方程為

(6)

(7)

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

由式(7)可得：

(8)

1.3 多機械臂系統(tǒng)動力學模型

xe=φ(q)

(9)

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

對式(9)左右同時求關于時間的導數(shù)，得：

(10)

又有：

(11)

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

由式(10)(11)可推導出：

由假設4可知Je可逆，則有：

(12)

對式(12)左右同時求關于時間的導數(shù)，得：

(13)

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

(14)

(15)

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

1.4 受約束多機械臂系統(tǒng)動力學模型

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

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

(16)

由式(16)左右兩邊同時關于時間t求導得：

(17)

由式(6)可得受約束物體得動力學方程為

(18)

(19)

(20)

(21)

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

τa+Fc

(22)

則受約束多機械臂系統(tǒng)動力學方程表示為

(23)

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

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

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

(24)

2 控制器設計

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

(25)

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

滑模函數(shù)為

(26)

力誤差：

eλ=λ-λd

(27)

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

設計控制器為

(28)

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

(29)

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

用于控制力的項：

λr=λd-Kλeλ

(30)

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

(31)

(32)

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

對等號兩邊關于時間t求導得：

(33)

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

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

(34)

其中：

CoL

設用于控制內(nèi)力的項為

CoL

CoL

FIr=FId-KIeFI

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

(35)

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

(36)

3 仿真分析

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

圖2 受約束雙機械臂模型

其機械臂的動力學模型參數(shù)如下：

Ci(qi)=

Gi(qi)=

Jei(qi)=

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

圖3 位置跟蹤

圖4 約束力跟蹤誤差

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

4 結(jié)束語

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