董方方, 楊 超, 韓 江, 張新榮

(1. 合肥工業(yè)大學(xué) 機械工程學(xué)院, 合肥 230009;2. 安徽省智能數(shù)控技術(shù)及裝備工程實驗室, 合肥 230009;3. 長安大學(xué) 陜西省高速公路施工機械重點實驗室, 西安 710064)

0 引 言

移動機械臂是通過在移動平臺上加裝一個或多個機械臂以實現(xiàn)協(xié)調(diào)控制的完整機械系統(tǒng),相比于傳統(tǒng)的固定基座機械臂有更大的工作范圍,因此在工業(yè)生產(chǎn)、物料搬用、家政服務(wù)、搶險救援等領(lǐng)域有很大的應(yīng)用空間．

移動機械臂雖然具有強大的功能,但由于機械臂和移動平臺這兩個系統(tǒng)各自的運動形式和運動特性存在較大差異,使得系統(tǒng)在作業(yè)過程中兩者會存在強烈的相互作用,對各自運動狀態(tài)產(chǎn)生相互影響,即耦合效應(yīng)．耦合效應(yīng)的建模分析較為困難,從而導(dǎo)致整體運動控制的穩(wěn)定性較差．而且,這種耦合效應(yīng)會伴隨著機械臂-移動平臺的質(zhì)量比增大而愈加明顯．因此,通常做法是盡可能將移動平臺的質(zhì)量加大來弱化這種耦合效應(yīng)．表1列出了目前一些著名的機械臂-移動平臺的質(zhì)量比,可以看出,質(zhì)量比大部分不超過0.21,少數(shù)超過0.3．但是過大的移動平臺也會使得移動機械臂過于笨重,限制了其靈活性和工作能力．因此要實現(xiàn)對高質(zhì)量比的移動機械臂的有效控制,高效而準(zhǔn)確的建模方法就成為了一種前置條件．

表1 機械臂-移動平臺質(zhì)量比[1]

目前在移動機械臂的建模方法上有多種不同思路和解決辦法．Liu等[9]將原本耦合的移動機械臂系統(tǒng)分離,將耦合效應(yīng)看作外部擾動,并利用Lagrange方法分別建立移動平臺和機械臂獨立的動力學(xué)方程．該方法雖然簡化了建模過程,但不能充分發(fā)揮移動作業(yè)機器人系統(tǒng)的動態(tài)作業(yè)能力．楊賀賀等[10]基于多體系統(tǒng)離散時間傳遞矩陣法,分別建立了車輪、車體柔性關(guān)節(jié)和機械臂的動力學(xué)方程,最后得到了移動柔性機械臂的整體動力學(xué)模型．該方法雖然得到了完整動力學(xué)方程,但還是要對每個分析單元進行細致的受力分析．陳良港等[11]基于單位對偶四元數(shù)法,針對冗余移動機械臂求取任務(wù)空間速度與廣義空間速度映射關(guān)系困難的問題,將移動機械臂看作一個整體建立其運動學(xué)模型．該方法雖然保證了運動學(xué)精度,但需要求解微分和逆運動學(xué)．魏麗君等[12]基于D-H算法將移動平臺看作成一個由2個移動關(guān)節(jié)和1個轉(zhuǎn)動組成的虛擬關(guān)節(jié),并將其納入到機械臂中進行整體建模．該方法用在運動學(xué)分析中有其獨特優(yōu)勢,但在動力學(xué)分析上就十分棘手．Zhong等[13]使用Lagrange方法和直接路徑法(DMP)的概念對移動機械臂進行了整體建模, 兼顧了移動機械臂內(nèi)在的耦合特性, 有相對較高的建模精度, 但是增加了形式和計算的復(fù)雜性, 且仍然需要求解Lagrange乘子．

針對移動機械臂建模的復(fù)雜性和耦合性,通過應(yīng)用分析力學(xué)中的U-K理論[14-16]可以對移動機械臂這一非線性的機械系統(tǒng)進行高效而準(zhǔn)確的建模．該理論不同于以往的力學(xué)分析方法,通過經(jīng)典建模三步法就可以完成對一個復(fù)雜機械系統(tǒng)的動力學(xué)建模工作．即先求得一個機械系統(tǒng)的無約束的動力學(xué)模型,然后將約束方程轉(zhuǎn)化為二階標(biāo)準(zhǔn)形式,之后利用UKE得到解析形式的約束力,最后可得到系統(tǒng)受約束的動力學(xué)方程．Huang等[17]通過對無約束的單個子系統(tǒng)進行聚類和級聯(lián),利用UKE計算由約束引入的約束力,得到多體系統(tǒng)運動方程的封閉形式表達式．該方法表明U-K理論具有層級嵌套屬性,多層約束可以疊加聚合,這為復(fù)雜多體機械系統(tǒng)建模提供了一種新的實踐方法．該方法被應(yīng)用到移動機械臂的建模中,不需要分析其每個子系統(tǒng)的受力情況,也不需要求解系統(tǒng)的逆解,更不需要求解Lagrange乘子,且保證了建模的準(zhǔn)確性和簡單性．董方方等[18]基于此方法構(gòu)建了雙臂機器人動力學(xué)模型,極大地減小了計算過程,并保證了模型的控制精度．韓江等[19]利用該方法高效、系統(tǒng)、快速地建立了2自由度冗余驅(qū)動并聯(lián)機器人的動力學(xué)解耦模型．雖然這種方法不區(qū)分完整約束或非完整約束,且簡單、通用性強,但當(dāng)直接應(yīng)用U-K方法時,必須保證系統(tǒng)在操作期間的每個時刻滿足約束條件．在實際工程中,由于各種原因,在初始階段很難滿足這些約束條件．因此,為了處理初始條件對約束可能存在的偏差,需要針對約束方程進行修正[20]．

本文根據(jù)移動機械臂的運動特點,將移動機械臂劃分為多個子系統(tǒng)．該方法既考慮系統(tǒng)所固有的耦合效應(yīng),又不失簡單性和可操作性．該方法可以獲得在不考慮任務(wù)要求下動力學(xué)建模的結(jié)構(gòu)約束和任務(wù)約束下跟蹤指定軌跡的性能約束,然后便可以通過求解UKE獲得控制力的顯式的解析表達式．同時,為了解決當(dāng)初始條件不滿足給定約束條件時的問題,基于Lyapunov穩(wěn)定性理論,構(gòu)造了修正的約束方程．最后通過仿真,驗證了建模的正確性和應(yīng)對初始條件偏差的有效性．

1 Udwadia-Kalaba理論簡介

1.1 Udwadia-Kalaba基本方程

U-K方法是一種求解受約束系統(tǒng)約束力解析解的方法．假設(shè)一個無約束的機械系統(tǒng)有n個狀態(tài)變量q=[q1,q2,q3,…,qn]T,該系統(tǒng)無約束條件下的動力學(xué)方程可以描述為如下形式[14]:

(1)

若該系統(tǒng)存在m(m≤n)個約束:

(2)

m個約束可以被劃分為兩類,即完整約束和非完整約束．其包括h個完整約束

φi(q,t)=0,i=1,2,…,h,

(3)

和m-h個非完整約束

(4)

基于一致性的假設(shè),我們可以將非完整約束(4)對時間t微分一次,將完整約束(3)對時間t微分兩次,以矩陣方程的形式推導(dǎo)出一組約束方程．可得到約束的二階Pfaffian標(biāo)準(zhǔn)微分形式如下[14]:

(5)

(6)

(7)

式中B(q,t)=A(q,t)M1/2(q,t),B+(q,t)為B(q,t)的Moore-Penrose廣義逆矩陣．對比式(1)和(6),可得由施加約束產(chǎn)生的約束力為

(8)

從上述約束力的求解過程可以看出,該方法既不需要先確定存在的具體約束條件,也不需要求解Lagrange乘子,其求解的約束力為解析解,免去了大量的求解過程,且形式簡單整潔．

1.2 Udwadia-Kalaba基本方程的層級屬性

依據(jù)式(8),外加約束力Qc中有包含Q的成分,即Qc是基于Q而得到的解析化結(jié)果．這就使得當(dāng)給系統(tǒng)施加多個約束時,UKE可以不斷層級化地生成對應(yīng)約束所產(chǎn)生的約束力．例如一個無約束的系統(tǒng)的動力學(xué)方程如下:

(9)

(10)

其中

(11)

(12)

其中

(13)

當(dāng)再有別的附加約束時,這種過程可以持續(xù)迭代下去．假設(shè)當(dāng)?shù)侥硨蛹墪r,該無約束動力學(xué)方程為

(14)

其中

(15)

1.3 建模示例

我們通過一個算例來對比層級聚合方法與傳統(tǒng)Lagrange方法的建模過程．假設(shè)有以下單擺系統(tǒng),如圖1所示,小球的質(zhì)量為m0,繩子長度為l0,繩子系在坐標(biāo)系O0xy原點處,小球的坐標(biāo)為(x0,y0)．小球擺動的速度為v,繩子角速度為ω,繩子與水平的夾角為β,繩子對小球施加的拉力為F．

圖1 單擺系統(tǒng)Fig. 1 A single pendulum system

1.3.1 層級聚合建模方法

現(xiàn)將單擺上的小球視為一個質(zhì)量為m0做自由落體運動的質(zhì)點,則運動方程為

(16)

將式(16)改寫為矩陣形式:

(17)

根據(jù)單擺系統(tǒng)的運動特點,小球受到的約束為

(18)

寫成二階Pfaffian標(biāo)準(zhǔn)微分形式為

(19)

其中

利用UKE求得約束力,并將約束力施加到式(17)的系統(tǒng)中,因此單擺系統(tǒng)動力學(xué)方程為

(20)

1.3.2 Lagrange方法

首先寫出該系統(tǒng)的Lagrange函數(shù):

(21)

將式(21)代入Lagrange方程:

(22)

F-m0gsinβ=mω2l0．

(23)

因此可知F為

F=m0gsinβ+mω2l0,

(24)

計算可以得到

(25)

對應(yīng)到Q0為

(26)

最后可以得到其動力學(xué)方程為

(27)

其中

從上面兩種方法的推導(dǎo)及分析過程可知,層級聚合方法直接對約束進行標(biāo)準(zhǔn)化處理后代入U-K方程可獲得解析形式的約束力,再與無約束系統(tǒng)結(jié)合即可寫出受約束系統(tǒng)的動力學(xué)方程．整個過程的分析簡單,無需推導(dǎo),易于編程實現(xiàn)．而拉氏方法不僅需要復(fù)雜的運動分析,還需要對得到的Lagrange函數(shù)方程進行求解,進而得到最終的動力學(xué)方程．因此從求解全過程可知,從數(shù)據(jù)處理和運動分析的角度來看,層級聚合方法具有簡明直觀的特點．

2 基于層級聚合建模方法的移動機械臂建模

基于U-K方法的層級屬性,將所研究的移動機械臂劃分為3個子系統(tǒng),即移動平臺系統(tǒng)和兩個機械關(guān)節(jié)系統(tǒng),如圖2所示．為了方便在移動機器人上建立坐標(biāo)系,下文將移動機器人抽象成簡略圖．對于3個子系統(tǒng)均為無約束系統(tǒng)時,可以更容易地得到其動力學(xué)方程．將移動平臺和機械臂切分開來是考慮到移動平臺和機械臂的差異性．此外將機械臂切分為第一、 二關(guān)節(jié)和其余關(guān)節(jié)系統(tǒng), 這樣劃分的目的是減少建模過程的復(fù)雜性．

從以上介紹可知,移動機器臂是由移動平臺和機械臂所構(gòu)成的復(fù)合系統(tǒng)．我們將選用由Mecanum輪驅(qū)動的全方向移動平臺和三關(guān)節(jié)機械臂構(gòu)成的移動機械臂系統(tǒng)來研究移動機械臂的建模過程．選用全向移動平臺是考慮到其靈活性和保證底盤的小巧性;選用的三關(guān)節(jié)機械臂是將常規(guī)的六關(guān)節(jié)機械臂簡化為只有旋轉(zhuǎn)、下臂和上臂的三軸機械臂,而舍去其他3個對移動機械臂平臺整體控制精度和工作能力影響較小的腕關(guān)節(jié),同時也不失研究的代表性和一般性．

圖2 移動機械臂子系統(tǒng)分割圖Fig. 2 Partition of the mobile manipulator subsystem

移動機械臂運動學(xué)分析的主要任務(wù)是給出關(guān)節(jié)空間變量與位姿空間變量之間的轉(zhuǎn)換關(guān)系,即運動學(xué)正問題與運動學(xué)逆問題．圖3中的機械系統(tǒng)的工作裝置為三關(guān)節(jié)機械臂,因此工作裝置有三自由度．對移動機器臂進行運動學(xué)分析,首先在地面和機械裝置上建立一些坐標(biāo)系．OWxWyWzW為基坐標(biāo)系,OMxMyMzM為車輛坐標(biāo)系,其原點OM被設(shè)置在車輛在地面上的投影,ORxRyRzR為固定在機械臂基座的坐標(biāo)系,O1x1y1z1為固定在第一關(guān)節(jié)的坐標(biāo)系,O2x2y2z2為固定在第二關(guān)節(jié)的坐標(biāo)系,O3x3y3z3為固定在第三關(guān)節(jié)的坐標(biāo)系,O4x4y4z4為固定在末端執(zhí)行器的坐標(biāo)系．選取機械臂的關(guān)節(jié)空間變量為:第一關(guān)節(jié)和底座之間轉(zhuǎn)角為θ1,第二關(guān)節(jié)和底座之間轉(zhuǎn)角為θ2,第三關(guān)節(jié)和第二關(guān)節(jié)之間轉(zhuǎn)角為θ3．

(28)

(29)

其中,c(θi-1)=cosθi-1,s(θi-1)=sinθi-1,c(αi-1)=cosαi-1,s(αi-1)=sinαi-1,i=1,2,3,4,θi-1為坐標(biāo)系Oixiyizi相對于坐標(biāo)系Oi-1xi-1yi-1zi-1的旋轉(zhuǎn)角,αi-1為坐標(biāo)系Oi-1xi-1yi-1zi-1旋轉(zhuǎn)軸相對于坐標(biāo)系Oixiyizi旋轉(zhuǎn)軸的旋轉(zhuǎn)角,ai-1為在xi-1方向上Oi-2xi-2yi-2zi-2和Oi-1xi-1yi-1zi-1之間的距離,di-1為在zi-1方向上Oi-2xi-2yi-2zi-2和Oi-1xi-1yi-1zi-1之間的距離．機械臂幾何參數(shù)如表2所示．

圖3 移動機械臂坐標(biāo)示意圖Fig. 3 Coordinate systems for the mobile manipulator

表2 機械臂幾何參數(shù)

因此,可得在坐標(biāo)系O4x4y4z4中的末端執(zhí)行器尖端向量p4在機械臂坐標(biāo)系O1x1y1z1中表達式p0為

(30)

(31)

其中,c(θi)=cosθi,s(θi)=sinθi,c(θi-θj)=cos(θi-θj),s(θi-θj)=sin(θi-θj),i=1,2,3,j=2,3．機器人運動學(xué)分析中,Jacobi矩陣用來表示機器人末端執(zhí)行器的線速度和角速度與各關(guān)節(jié)速度之間的轉(zhuǎn)換關(guān)系．上面求得末端位置與各個關(guān)節(jié)角度的關(guān)系后,則可得到末端執(zhí)行器速度和各個關(guān)節(jié)速度的關(guān)系為

(32)

(33)

2.1 移動平臺子系統(tǒng)的建模

移動平臺采用了由伺服電機通過減速齒輪箱進行獨立驅(qū)動每個Mecanum輪的設(shè)置,將其置于空間坐標(biāo)中對其進行運動學(xué)和動力學(xué)建模,如圖4所示．

為了表征移動平臺的位置,使用廣義坐標(biāo)x,y,φ來描述其位置信息．因為該移動平臺為全向驅(qū)動,每個輪子對應(yīng)了一個角位移,分別為θω1,θω2,θω3,θω4．同時由圖4可知平臺的長度和寬度分別為2L,2l,車輪半徑為Rω．其運動方程可以表示為

(34)

定義移動平臺子系統(tǒng)為S11,并構(gòu)建其Lagrange函數(shù)為

(35)

圖4 移動平臺結(jié)構(gòu)示意圖Fig. 4 Schematic diagram of the mobile platform structure

將式(35)代入到Lagrange方程中可得

(36)

通過求解方程(36),可以得到S11的動力學(xué)方程:

(37)

M11和Q11的具體結(jié)構(gòu)如下:

(38)

其中,μ表示摩擦因數(shù)．

2.2 機械臂子系統(tǒng)的建模

上面對機械臂部分進行了運動學(xué)分析,下面對其進行動力學(xué)建模．根據(jù)本文前面所闡述的將機械臂劃分為第一、二關(guān)節(jié)和其余關(guān)節(jié)子系統(tǒng)的建模思路,下面對其具體建模過程進行演示．將第一、二關(guān)節(jié)子系統(tǒng)定義為系統(tǒng)S21,其余關(guān)節(jié)子系統(tǒng)定義為系統(tǒng)S31,圖5為機械臂關(guān)節(jié)子系統(tǒng)示意圖．

對于系統(tǒng)S21,選取廣義坐標(biāo)為q21=[x01y01θ01θ2]T,(x01,y01,z01)為坐標(biāo)系O1x1y1z1的原點在基座標(biāo)系OMxMyMzM中的空間位置．其中z01為定值,不作為其狀態(tài)變量．選取(α01,β01,θ01)為坐標(biāo)系O1x1y1z1的Euler角,Euler角選取為XYZ表示方法,其中α01=0,β01=0．其子系統(tǒng)的Lagrange函數(shù)為L21為

(39)

其中,xc2=x01+r2cosθ01cosθ2,yc2=y01+r2sinθ01cosθ2,zc2=z01+l1+r2sinθ2,I1和I2分別為第一關(guān)節(jié)和第二關(guān)節(jié)的轉(zhuǎn)動慣量．

(a) 第一、二關(guān)節(jié)子系統(tǒng)(子系統(tǒng)S21) (b) 其余關(guān)節(jié)子系統(tǒng)(子系統(tǒng)S31) (a) The 1st and 2nd joint subsystem (subsystem S21) (b) The remaining joint subsystem (subsystem S31)

代入Lagrange方程可得

(40)

可得系統(tǒng)S21在無約束條件下的動力學(xué)方程,其表達式為

(41)

其中

(42)

(43)

對于系統(tǒng)S31,選取廣義坐標(biāo)為q31=[x03y03z03γθ03]T,同樣(x03,y03,z03)為坐標(biāo)系O3x3y3z3的原點在基座標(biāo)系OMxMyMzM中的空間位置．選取(γ,ψ,-θ03)為坐標(biāo)系O3x3y3z3的Euler角,Euler角選取為ZXZ表示方法,其中ψ=90°．

子系統(tǒng)S31的Lagrange函數(shù)L31可以表示為

(44)

其中

xc3=x03+r3cosθ03cosγ,yc3=y03+r3cosθ03sinγ,

zc3=z03-r3sinθ03,xc4=x03+(l3+r4)cosθ03cosγ,

yc4=y03+(l3+r4)cosθ03sinγ,zc4=z03-(l3+r4)sinθ03,

I3和I4分別為第三關(guān)節(jié)和末端執(zhí)行器的轉(zhuǎn)動慣量．

將式(44)代入Lagrange方程,得

(45)

求解式(45),可得系統(tǒng)S31的無約束動力學(xué)方程為

(46)

式中

(47)

(48)

其中

c(θ03)=cosθ03,s(θ03)=sinθ03,c(γ)=cosγ,s(γ)=sinγ．

2.3 構(gòu)建約束和系統(tǒng)整合

移動機械臂可以通過堆聚子系統(tǒng)S11,S21和S31再輔以物理上的結(jié)構(gòu)約束將其重構(gòu)成一個有機的完整系統(tǒng)．結(jié)合空間的位置關(guān)系,可以很容易得到其約束關(guān)系如下:

(49)

由式(49)可得其約束方程為

(50)

(51)

(52)

其中

q12=[xyφx01y01θ01θ2x03y03z03γθ03]T．

同時可以將子系統(tǒng)S11,S21和S31的矩陣M和Q集中寫成另外兩個矩陣M12和Q12,其具體構(gòu)造如下:

(53)

Q12=[Q11Q21Q31]T．

(54)

利用UKE可以得到結(jié)構(gòu)約束力,并獲得完整的受約束的動力學(xué)方程:

(55)

(56)

因為式(55)不是最簡形式,所以需要將其化簡．這就需要找到狀態(tài)變量q12與系統(tǒng)真正需要的狀態(tài)變量q=[xyφθ1θ2θ3]T之間的變換關(guān)系．除了知道式(49)所得到的關(guān)系之外,還知道θ01=γ=φ+θ1和θ03=θ3-θ2,綜合這些條件,可以得到以下關(guān)系式:

(57)

其中

(58)

(59)

則對式(55)進行化簡可得

(60)

當(dāng)系統(tǒng)受到外部運動約束時,會對系統(tǒng)施加外部約束力τ,從而最后可得式(60)的最簡形式如下:

(61)

其中

τ為運動約束力．

3 初始條件偏差

在仿真中使用的系統(tǒng)初始條件必須滿足所需的軌跡跟蹤約束．但是,由于各種因素的存在,這種情況未必真會發(fā)生．它可能只在初始時刻被近似滿足．如果初始條件不相容,則模擬結(jié)果會出現(xiàn)發(fā)散．處理初始條件問題的一種方法是使用Lyapunov穩(wěn)定性理論,如文獻[20]所示．

所期望的軌跡由式(2)描述．基于軌跡穩(wěn)定方法,我們現(xiàn)在修改約束方程為

(62)

式中f(φ,t,α)是包含向量p、參數(shù)α的m維向量．式(62)必須滿足以下要求:

注1 特別地,如果約束方程是完整的,如式(3)所示,那么方程可以修改為

(63)

現(xiàn)在用修正式(3)作為期望的軌跡要求,通過微分過程,得到矩陣方程的形式為

(64)

經(jīng)過修改,控制力為

(65)

4 仿 真

為了驗證本文所提出的層級建模方法的準(zhǔn)確性,以及UK方法對于移動機械臂的協(xié)調(diào)控制的可實現(xiàn)性和適應(yīng)性,將通過以下仿真過程予以說明和闡述．模擬的對象是圖2所示的移動機械臂,該機械裝置主要包括由4個Mecanum輪驅(qū)動的全向移動平臺和由3個關(guān)節(jié)和末端夾爪構(gòu)成的機械臂,其具體參數(shù)如表3所示．

表3 系統(tǒng)動力學(xué)參數(shù)表

為了貼近實際使用場景,便于直觀地理解仿真結(jié)果,本文采用了常見的機械臂-移動平臺質(zhì)量比參數(shù)．質(zhì)量比參數(shù)并不影響本文的建模和仿真過程,只影響穩(wěn)定性控制的難易程度．

為了驗證本文層級聚合建模方法的建模精度和準(zhǔn)確性,對拉氏方法和層級聚合建模方法得到的模型同時施加約束(66)(約束滿足初始條件):

(66)

并比較在施加約束后每個狀態(tài)變量的數(shù)值是否一致．

圖6表示在受約束條件下兩種建模方法獲得的移動平臺軌跡隨時間的變化情況,圖7(a)和7(b)分別為層級聚合建模方法和Lagrange方法所得的機械臂各關(guān)節(jié)軌跡隨時間的變化情況,通過對比可以發(fā)現(xiàn)各自的運動軌跡是一致的．圖8為兩種模型下各個狀態(tài)變量的數(shù)值誤差,圖8(a)為移動平臺x方向和y方向的軌跡誤差,圖8(b)為機械臂各關(guān)節(jié)的軌跡誤差．可以發(fā)現(xiàn)所有結(jié)果始終為零,這說明兩種建模方法得到的計算結(jié)果完全一致,充分說明了本文建模方法與拉氏方法在建模精度和準(zhǔn)確性上是一致的．

圖6 移動平臺軌跡

為對比算法的效率,采用MATLAB記錄了兩種方法的計算耗時,如圖9所示．從圖9中可以看出,層級聚合方法比拉氏方法所獲得的模型在計算效率上略有提高,但提升的幅度有限．然而,本文提出的層級聚合建模方法對于不同類型約束(完整、非完整約束)處理具有更好的一致性,都是將約束轉(zhuǎn)化為二階微分形式,再利用UKE寫出約束力的解析解．整個建模過程簡潔明確,分析步驟較少．而拉氏方法需要先求解Lagrange乘子,無法獲得解析形式的解．此外,利用層級疊加的屬性,當(dāng)有新的約束增加后,本文方法只需將新的約束轉(zhuǎn)化為二階微分形式后代入UKE,不影響其他分析步驟．

圖8 移動平臺與機械臂各自軌跡誤差Fig. 8 The respective trajectory errors of the mobile platform and the manipulator

圖9 層級聚合方法與拉氏方法計算效率對比Fig. 9 Comparison of computation efficiency between the hierarchical aggregation method and the Lagrange method

同時為了證明本文算法對于處理初始條件偏差的有效性,為移動機械臂選定一組軌跡約束,讓移動平臺走正弦曲線,并且機械臂進行相對于移動平臺的畫斜圓運動,約束條件如表4所示(xmp(t),ymp(t)和zmp(t)分別為末端執(zhí)行器相對于移動平臺在坐標(biāo)系ORxRyRzR中三個方向上的軌跡約束時間函數(shù))．

表4 約束條件參數(shù)

圖10—13為移動平臺和移動機械臂各自設(shè)定軌跡約束并附加任意初始條件的情況下對于理想軌跡的跟蹤情況．通過觀察可知我們選定的約束均為完整約束,所以選擇式(63)這種修正方案．表4所有約束均可寫為式(63)這種形式,這樣便可以將式子Fi(i=1,2,…,6)改寫為6個修正的約束等式．其中兩參數(shù)設(shè)置為,κm=2.5(m=1,2,3對應(yīng)F1,F2,F3的修正方程),κn=2(n=4,5,6對應(yīng)F4,F5,F6的修正方程),εi=1.5 (i=1,2,…,6對應(yīng)F1,F2,…,F6的修正方程)．圖10(b)為機械臂相對于移動平臺的畫圓運動,可以發(fā)現(xiàn)在宏觀上末端執(zhí)行器從較大范圍的初始條件偏差下逐漸回歸到離線軌跡上．圖10(a)為移動平臺和機械臂相對于移動平臺畫圓展開后的空間軌跡,符合一般移動機械臂協(xié)調(diào)運動模式(p1和p3分別為末端執(zhí)行器初始位置和終止位置;p2和p4分別為移動平臺初始位置和終止位置)．

(a) 移動平臺、末端執(zhí)行器空間軌跡 (b) 末端執(zhí)行器相對移動平臺軌跡 (a) Spatial trajectories of the mobile platform and the end-effector (b) End-effector trajectories relative to the mobile platform

圖11為移動平臺的軌跡跟蹤情況,可以發(fā)現(xiàn)移動平臺在從在較大的初始條件偏差的情況下,在修正后的約束方程的約束下漸進地使得偏離的軌跡收斂于理想軌跡上．圖11(b)、(c)和(d)分別是移動軌跡在x,y方向和航向角上的跟蹤情況,也都吻合圖11(a)宏觀上所顯現(xiàn)的收斂情況,都在一定的時間段后達到了收斂．圖12(a)和(b)分別為移動平臺和末端執(zhí)行器兩部分軌跡跟蹤誤差變化情況,可以發(fā)現(xiàn)移動平臺在5 s后逐漸收斂,機械臂在8 s后逐漸收斂．其中移動平臺的軌跡跟蹤誤差為4×10-4m,末端執(zhí)行器移動平臺的軌跡跟蹤誤差為5×10-4m,符合軌跡跟蹤精度要求(圖12顯示的軌跡誤差均為綜合誤差)．初始時刻之所以會出現(xiàn)跟蹤誤差較大情況,首先因為本文特意將初始條件選取在理想軌跡之外的一點,所以使得初始時刻軌跡誤差較大;其次也是利用修正方程對理想軌跡的跟蹤使得初始時刻出現(xiàn)了較大的超調(diào)量,目的是使系統(tǒng)更快的收斂．圖13(a)中Fx和Fy分別為關(guān)節(jié)1施加在移動平臺x,y方向上的約束力, 圖13(b)中T1,T2和T3分別為關(guān)節(jié)1,2,3的內(nèi)部約束力．觀察圖中數(shù)值可知,約束力沒有出現(xiàn)奇大情況,貼合現(xiàn)實的使用需求．

(a) 移動平臺x,y軌跡 (b) 移動平臺x方向軌跡 (a) Mobile platform x,y trajectories (b) Mobile platform x-direction trajectories

(c) 移動平臺y方向軌跡 (d) 移動平臺航向角 (c) Mobile platform y-direction trajectories (d) Mobile platform heading angle

(a) 移動平臺軌跡誤差 (b) 末端執(zhí)行器相對移動平臺軌跡誤差 (a) Mobile platform trajectory errors (b) End-effector trajectory errors relative to the mobile platform

(a) 關(guān)節(jié)1施加在移動平臺x,y方向上的約束力 (b) 關(guān)節(jié)1,2,3內(nèi)部約束力 (a) The forces applied to joint 1 in the x,y-directions (b) Internal constraints of joints 1,2,3 of the moving platform

5 結(jié) 論

本文首先應(yīng)用了不同以往的建模方法,依據(jù)移動機械臂本身的運動特點,將移動機械臂劃分為多個子系統(tǒng)．該方法既考慮系統(tǒng)所固有的耦合效應(yīng),又不失簡單性和可操作性．該方法包含在不考慮任務(wù)要求下動力學(xué)建模的結(jié)構(gòu)約束和跟蹤指定軌跡的性能約束條件下,通過求解UKE可以獲得控制力的顯式、閉合形式的解析表達式．對于機械系統(tǒng)在一般條件中初始條件不滿足的情況下,通過基于Lyapunov穩(wěn)定性理論,將原來設(shè)定的軌跡約束規(guī)約化為修正的約束方程,以得到新的約束矩陣,并將其施加在所建立的動力學(xué)模型上達到補償初始條件偏差的目的．最后仿真結(jié)果驗證了移動平臺和機械臂在同時進行運動時,均可以滿足收斂到理想軌跡的性能需求,并實現(xiàn)了較高的精度要求．

致謝本文作者衷心感謝陜西省高速公路施工機械重點實驗室(長安大學(xué))開放基金(300102252505)對本文的資助．