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

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

0 引 言

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

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

表1 機(jī)械臂-移動(dòng)平臺(tái)質(zhì)量比[1]

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

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

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

1 Udwadia-Kalaba理論簡(jiǎn)介

1.1 Udwadia-Kalaba基本方程

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

(1)

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

(2)

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

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

(3)

和m-h個(gè)非完整約束

(4)

基于一致性的假設(shè),我們可以將非完整約束(4)對(duì)時(shí)間t微分一次,將完整約束(3)對(duì)時(shí)間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廣義逆矩陣．對(duì)比式(1)和(6),可得由施加約束產(chǎn)生的約束力為

(8)

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

1.2 Udwadia-Kalaba基本方程的層級(jí)屬性

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

(9)

(10)

其中

(11)

(12)

其中

(13)

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

(14)

其中

(15)

1.3 建模示例

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

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

1.3.1 層級(jí)聚合建模方法

現(xiàn)將單擺上的小球視為一個(gè)質(zhì)量為m0做自由落體運(yùn)動(dòng)的質(zhì)點(diǎn),則運(yùn)動(dòng)方程為

(16)

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

(17)

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

(18)

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

(19)

其中

利用UKE求得約束力,并將約束力施加到式(17)的系統(tǒng)中,因此單擺系統(tǒng)動(dò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)

計(jì)算可以得到

(25)

對(duì)應(yīng)到Q0為

(26)

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

(27)

其中

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

2 基于層級(jí)聚合建模方法的移動(dòng)機(jī)械臂建模

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

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

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

移動(dòng)機(jī)械臂運(yùn)動(dòng)學(xué)分析的主要任務(wù)是給出關(guān)節(jié)空間變量與位姿空間變量之間的轉(zhuǎn)換關(guān)系,即運(yùn)動(dòng)學(xué)正問題與運(yùn)動(dòng)學(xué)逆問題．圖3中的機(jī)械系統(tǒng)的工作裝置為三關(guān)節(jié)機(jī)械臂,因此工作裝置有三自由度．對(duì)移動(dòng)機(jī)器臂進(jìn)行運(yùn)動(dòng)學(xué)分析,首先在地面和機(jī)械裝置上建立一些坐標(biāo)系．OWxWyWzW為基坐標(biāo)系,OMxMyMzM為車輛坐標(biāo)系,其原點(diǎn)OM被設(shè)置在車輛在地面上的投影,ORxRyRzR為固定在機(jī)械臂基座的坐標(biāo)系,O1x1y1z1為固定在第一關(guān)節(jié)的坐標(biāo)系,O2x2y2z2為固定在第二關(guān)節(jié)的坐標(biāo)系,O3x3y3z3為固定在第三關(guān)節(jié)的坐標(biāo)系,O4x4y4z4為固定在末端執(zhí)行器的坐標(biāo)系．選取機(jī)械臂的關(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相對(duì)于坐標(biāo)系Oi-1xi-1yi-1zi-1的旋轉(zhuǎn)角,αi-1為坐標(biāo)系Oi-1xi-1yi-1zi-1旋轉(zhuǎn)軸相對(duì)于坐標(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之間的距離．機(jī)械臂幾何參數(shù)如表2所示．

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

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

因此,可得在坐標(biāo)系O4x4y4z4中的末端執(zhí)行器尖端向量p4在機(jī)械臂坐標(biāo)系O1x1y1z1中表達(dá)式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．機(jī)器人運(yùn)動(dòng)學(xué)分析中,Jacobi矩陣用來表示機(jī)器人末端執(zhí)行器的線速度和角速度與各關(guān)節(jié)速度之間的轉(zhuǎn)換關(guān)系．上面求得末端位置與各個(gè)關(guān)節(jié)角度的關(guān)系后,則可得到末端執(zhí)行器速度和各個(gè)關(guān)節(jié)速度的關(guān)系為

(32)

(33)

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

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

為了表征移動(dòng)平臺(tái)的位置,使用廣義坐標(biāo)x,y,φ來描述其位置信息．因?yàn)樵撘苿?dòng)平臺(tái)為全向驅(qū)動(dòng),每個(gè)輪子對(duì)應(yīng)了一個(gè)角位移,分別為θω1,θω2,θω3,θω4．同時(shí)由圖4可知平臺(tái)的長(zhǎng)度和寬度分別為2L,2l,車輪半徑為Rω．其運(yùn)動(dòng)方程可以表示為

(34)

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

(35)

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

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

(36)

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

(37)

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

(38)

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

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

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

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

(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)動(dòng)慣量．

(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在無約束條件下的動(dòng)力學(xué)方程,其表達(dá)式為

(41)

其中

(42)

(43)

對(duì)于系統(tǒng)S31,選取廣義坐標(biāo)為q31=[x03y03z03γθ03]T,同樣(x03,y03,z03)為坐標(biāo)系O3x3y3z3的原點(diǎn)在基座標(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)動(dòng)慣量．

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

(45)

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

(46)

式中

(47)

(48)

其中

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

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

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

(49)

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

(50)

(51)

(52)

其中

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

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

(53)

Q12=[Q11Q21Q31]T．

(54)

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

(55)

(56)

因?yàn)槭?55)不是最簡(jiǎn)形式,所以需要將其化簡(jiǎn)．這就需要找到狀態(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)

則對(duì)式(55)進(jìn)行化簡(jiǎn)可得

(60)

當(dāng)系統(tǒng)受到外部運(yùn)動(dòng)約束時(shí),會(huì)對(duì)系統(tǒng)施加外部約束力τ,從而最后可得式(60)的最簡(jiǎn)形式如下:

(61)

其中

τ為運(yùn)動(dòng)約束力．

3 初始條件偏差

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

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

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

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

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

(66)

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

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

圖6 移動(dòng)平臺(tái)軌跡

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

圖8 移動(dòng)平臺(tái)與機(jī)械臂各自軌跡誤差Fig. 8 The respective trajectory errors of the mobile platform and the manipulator

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

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

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

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

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

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

(a) 移動(dòng)平臺(tái)x,y軌跡 (b) 移動(dòng)平臺(tái)x方向軌跡 (a) Mobile platform x,y trajectories (b) Mobile platform x-direction trajectories

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

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

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

致謝本文作者衷心感謝陜西省高速公路施工機(jī)械重點(diǎn)實(shí)驗(yàn)室(長(zhǎng)安大學(xué))開放基金(300102252505)對(duì)本文的資助．