趙志剛，滕富軍，石廣田，李勁松，季鋼

(1.蘭州交通大學(xué) 機(jī)電工程學(xué)院，甘肅 蘭州 730070；2.上海交通大學(xué) 機(jī)械與動(dòng)力工程學(xué)院， 上海 200030)

?

緊耦合多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)逆運(yùn)動(dòng)學(xué)求解

趙志剛1，滕富軍1，石廣田1，李勁松2，季鋼2

(1.蘭州交通大學(xué) 機(jī)電工程學(xué)院，甘肅 蘭州 730070；2.上海交通大學(xué) 機(jī)械與動(dòng)力工程學(xué)院， 上海 200030)

摘要：針對(duì)多機(jī)器人共同吊運(yùn)同一個(gè)物體形成閉運(yùn)動(dòng)鏈的協(xié)調(diào)系統(tǒng)，在給定系統(tǒng)構(gòu)型的基礎(chǔ)上，建立了系統(tǒng)運(yùn)動(dòng)學(xué)模型，并建立了系統(tǒng)動(dòng)力學(xué)模型。分析了系統(tǒng)的運(yùn)動(dòng)學(xué)逆解，根據(jù)被吊運(yùn)物體的六個(gè)自由度期望運(yùn)動(dòng)，給出了逆運(yùn)動(dòng)學(xué)的解析表達(dá)式。利用UG/ADAMS/MATLAB虛擬控制仿真系統(tǒng)，搭建了多機(jī)器人吊運(yùn)系統(tǒng)虛擬實(shí)驗(yàn)平臺(tái)。在確定的參數(shù)條件下，實(shí)驗(yàn)驗(yàn)證了所建立模型的合理性及逆運(yùn)動(dòng)學(xué)解析表達(dá)式的準(zhǔn)確性，為進(jìn)一步研究系統(tǒng)的軌跡規(guī)劃、路徑跟蹤、穩(wěn)定性等問題奠定了基礎(chǔ)。

關(guān)鍵詞：多機(jī)器人；吊運(yùn)；逆運(yùn)動(dòng)學(xué)分析；仿真驗(yàn)證

柔性緊耦合多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)是多機(jī)器人系統(tǒng)的重要分支，現(xiàn)階段關(guān)于柔性緊耦合多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)的研究雖然剛剛起步，但已逐漸成為研究熱點(diǎn)。在文獻(xiàn)[1]中利用 Kaman K-MAX 全尺寸無人直升機(jī)來實(shí)現(xiàn)吊運(yùn)操作。Markus Bernard等[2]利用兩架小型無人直升機(jī)做了吊運(yùn)實(shí)驗(yàn)。 I.Maza等[3-4]用三架小型無人直升機(jī)進(jìn)行了吊運(yùn)控制實(shí)驗(yàn) 。Nathan Michael等[5]考慮了被吊運(yùn)物的穩(wěn)定性及靜力學(xué)平衡，討論了多機(jī)吊運(yùn)系統(tǒng)的空間配置。隨后，Qimi Jiang等[6]建立了多機(jī)吊運(yùn)系統(tǒng)的逆運(yùn)動(dòng)學(xué)模型。Peng Cheng 等[7]探討了利用多臺(tái)地面移動(dòng)機(jī)器人協(xié)調(diào)拖曳一個(gè)物體在光滑平整室內(nèi)地面上運(yùn)動(dòng)，在考慮了干摩擦力和拉緊力約束的情況下，建立了系統(tǒng)的微分運(yùn)動(dòng)方程，討論了不同系統(tǒng)配置下的配平條件。趙志剛等在UG/ADAMS/MATLAB三個(gè)軟件聯(lián)合控制虛擬仿真平臺(tái)的基礎(chǔ)上，建立了虛擬仿真實(shí)驗(yàn)平臺(tái)，并利用三臺(tái)工業(yè)機(jī)器人/運(yùn)動(dòng)控制卡/上位機(jī)/物體位姿追蹤器搭建了實(shí)體實(shí)驗(yàn)平臺(tái)，初步探討了緊耦合多機(jī)器人聯(lián)合協(xié)同吊運(yùn)同一重物系統(tǒng)的特點(diǎn)和多機(jī)器人之間合適分配對(duì)被吊運(yùn)重物的操作力，對(duì)系統(tǒng)的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)建模進(jìn)行了初步探索[8-11]。可以看出，柔性緊耦合多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)的文獻(xiàn)只有為數(shù)不多的幾篇，所涉及的研究工作僅從硬件和軟件結(jié)構(gòu)上討論了系統(tǒng)所涉及的基本問題，雖然嘗試對(duì)系統(tǒng)進(jìn)行控制，但并沒有從根本對(duì)系統(tǒng)進(jìn)行模型和穩(wěn)定性分析，無法指導(dǎo)實(shí)際應(yīng)用。

多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)的運(yùn)動(dòng)學(xué)逆解是系統(tǒng)進(jìn)行一系列研究的基礎(chǔ)，對(duì)整個(gè)系統(tǒng)的研究有著至關(guān)重要的作用。鄭亞青等[12]探討了三種構(gòu)型的六自由度繩牽引門式起重機(jī)器人的運(yùn)動(dòng)學(xué)位置逆解。Bin Zi等[13]對(duì)三臺(tái)吊車同時(shí)吊運(yùn)一個(gè)重物進(jìn)行了逆解分析?？梢钥闯?，關(guān)于多機(jī)器人聯(lián)合吊運(yùn)的運(yùn)動(dòng)學(xué)逆解，基本是對(duì)特定構(gòu)型的系統(tǒng)進(jìn)行探討，沒有形成通用的體系。

文中以三臺(tái)工業(yè)機(jī)器人為例，建立了系統(tǒng)運(yùn)動(dòng)學(xué)模型, 對(duì)系統(tǒng)逆運(yùn)動(dòng)學(xué)進(jìn)行分析,建立了虛擬控制仿真系統(tǒng)。進(jìn)行仿真計(jì)算，并對(duì)結(jié)果進(jìn)行了分析。

1多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)構(gòu)型

多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)是3臺(tái)三自由度的機(jī)器人通過繩索吊運(yùn)重物構(gòu)成，如圖1所示。

圖1　多機(jī)器人吊運(yùn)系統(tǒng)空間構(gòu)型布局Fig.1　The layout diagram of the multi-robot lifting system

在重物的質(zhì)心建立重物體坐標(biāo)系og-xyz如圖2所示，它們的初始姿態(tài)始終也相同。

圖2　連接點(diǎn)坐標(biāo)示意圖Fig.2　The coordinate diagram of junctions

2系統(tǒng)運(yùn)動(dòng)學(xué)建模

2.1 單個(gè)機(jī)器人運(yùn)動(dòng)學(xué)模型

(1)

式中：ci1、ci2、ci3是機(jī)器人3個(gè)關(guān)節(jié)角的余弦值；si1、si2、si3是機(jī)器人的3個(gè)關(guān)節(jié)角的正弦值。通過坐標(biāo)轉(zhuǎn)換，得到3臺(tái)機(jī)器人末端相對(duì)固定坐標(biāo)系o-xyz的坐標(biāo)為

(2)

2.2多機(jī)器人吊運(yùn)系統(tǒng)運(yùn)動(dòng)學(xué)模型

由單個(gè)機(jī)器人的運(yùn)動(dòng)學(xué)模型，解得已知機(jī)器人的關(guān)節(jié)角，得到機(jī)器人末端相對(duì)于固定坐標(biāo)系的坐標(biāo)。建立吊運(yùn)系統(tǒng)的運(yùn)動(dòng)學(xué)模型即是建立物體位姿關(guān)于機(jī)器人末端點(diǎn)坐標(biāo)的函數(shù)關(guān)系表達(dá)式。

2.2.1吊運(yùn)系統(tǒng)的位移約束

定義繩向量是結(jié)點(diǎn)坐標(biāo)減去相對(duì)應(yīng)的機(jī)器人末端點(diǎn)坐標(biāo)。繩向量滿足矢量封閉原理如圖2所示，在三角形P1GJ1、P2GJ2、P3GJ3中存在關(guān)系式PiJi等于PiG與GJi之和：

(3)

式中：

(4)

(5)

在吊運(yùn)系統(tǒng)中，繩索的長(zhǎng)度始終不變，根據(jù)向量模的性質(zhì)存在如下關(guān)系式：

(6)

2.2.2 吊運(yùn)系統(tǒng)的速度關(guān)系

定義繩速度是結(jié)點(diǎn)Ji相對(duì)于Pi點(diǎn)的速度，由于繩長(zhǎng)不變，繩速度Vi與繩向量PiJi的方向垂直。從圖3中可以得到結(jié)點(diǎn)Ji相對(duì)于Pi點(diǎn)的速度Vi就是繩向量PiJi的導(dǎo)數(shù)?？梢缘玫较铝嘘P(guān)系式：

(7)

式中：

(8)

(9)

(10)

其中，

圖3　繩索速度與繩索向量的關(guān)系Fig.3　The relations between ropes speed and ropes vector

2.2.3機(jī)器人末端位移約束

在多機(jī)器人聯(lián)合吊運(yùn)過程中，由于多解的存在，使多機(jī)器人的運(yùn)動(dòng)學(xué)逆解比較復(fù)雜，本文中限定機(jī)器人末端軌跡為直線，如圖3所示，從而使所建模型只有單一逆解。由于直線的方程為一般方程，可以具體的、系統(tǒng)的、全面的分析多機(jī)器人聯(lián)合吊運(yùn)過程。當(dāng)3臺(tái)機(jī)器人的末端的位移為直線時(shí)，可以列出機(jī)器人末端位移的方程:

(11)

式中：mi、ni、ri、ai、bi、ci分別為一般直線的參數(shù)。位移直線必須在相對(duì)應(yīng)的單個(gè)機(jī)器人的可行域內(nèi)，單臺(tái)機(jī)器人的可行域是機(jī)器人末端點(diǎn)在空間可以達(dá)到空間范圍，可以限定直線的參數(shù)范圍保證線段在所對(duì)應(yīng)的可行域內(nèi)。在本文的求解時(shí)，假定參數(shù)滿足以上條件。

2.2.4吊運(yùn)系統(tǒng)動(dòng)力學(xué)方程

(12)

式(12)可得出了三根繩索的豎直分力大小。已知繩索受力的方向和豎直分力大小，根據(jù)相似三角形原理，得到繩索的受力大小。根據(jù)力的合成與分解，得到水平分力的大小與方向：

(13)

(14)

3系統(tǒng)逆運(yùn)動(dòng)學(xué)分析

吊運(yùn)系統(tǒng)物體有六個(gè)自由度的運(yùn)動(dòng)，包括三個(gè)平移運(yùn)動(dòng)和三個(gè)旋轉(zhuǎn)運(yùn)動(dòng)。從任務(wù)出發(fā)，并考慮分析的方便性，限定機(jī)器人末端軌跡為直線，這樣逆運(yùn)動(dòng)學(xué)求解過程不再有耦合性，可將系統(tǒng)運(yùn)動(dòng)直接分解為六個(gè)子運(yùn)動(dòng)。假設(shè)被吊運(yùn)物體在歐拉角為零的初始點(diǎn)開始運(yùn)動(dòng)，可以得到以下的結(jié)果。由于單個(gè)機(jī)器人的逆運(yùn)動(dòng)學(xué)相對(duì)比較成熟，這里不再進(jìn)行討論，重點(diǎn)探討機(jī)器人末端位置和被吊運(yùn)物體位姿之間的關(guān)系式。

3.1平動(dòng)時(shí)逆運(yùn)動(dòng)學(xué)分析

被吊運(yùn)物體從歐拉角都為零的初始點(diǎn)開始運(yùn)動(dòng)，假設(shè)當(dāng)其沿一個(gè)軸平動(dòng)時(shí)，在其他軸上的運(yùn)動(dòng)為零，同時(shí)三個(gè)歐拉角始終為零。vx為沿x軸的速度，vy為沿y軸的速度，vz為沿z軸的速度。可以分別得到物體單向平移運(yùn)動(dòng)時(shí)機(jī)器人末端位置關(guān)于物體運(yùn)動(dòng)期望值的關(guān)系式。

3.2旋轉(zhuǎn)時(shí)逆運(yùn)動(dòng)學(xué)分析

當(dāng)被吊運(yùn)物體繞某一軸轉(zhuǎn)動(dòng)時(shí)，假定繞其他軸的角速度、角度都為零，同時(shí)三個(gè)軸向位移始終為零。ω1為沿x軸的角速度，ω2為沿y軸的角速度，ω3為沿z軸的角速度?？梢苑謩e得到物體單向旋轉(zhuǎn)運(yùn)動(dòng)時(shí)機(jī)器人末端位置關(guān)于物體運(yùn)動(dòng)期望值的關(guān)系式。

4仿真實(shí)驗(yàn)

由于纜系式緊耦合多機(jī)器人系統(tǒng)比較復(fù)雜，出于安全和成本的考慮，利用UG/ADAMS/MATLAB建立虛擬控制仿真系統(tǒng)。系統(tǒng)運(yùn)動(dòng)控制虛擬仿真結(jié)構(gòu)如圖 4 所示。

圖4　虛擬仿真系統(tǒng)結(jié)構(gòu)圖Fig.4　The structure of virtual simulation system

首先在UG中繪制系統(tǒng)三維圖形，并導(dǎo)入到ADAMS中，其中繩索建模時(shí)，應(yīng)用100段小圓柱剛體，每段小圓柱剛體之間由球鉸連接，實(shí)現(xiàn)虛擬系統(tǒng)中繩索的建模，則建立的虛擬系統(tǒng)如圖5所示。然后利用MATLAB/Simulink設(shè)計(jì)控制系統(tǒng)，最后利用ADAMS和MATLAB/Simulink之間的接口構(gòu)成一個(gè)閉環(huán)虛擬仿真系統(tǒng)。

圖5　ADAMS界面圖Fig.5　The interface diagram in ADAMS

在控制仿真過程中，ADAMS中多機(jī)虛擬模型的輸出變量為各機(jī)器人位姿測(cè)量值，是進(jìn)入MATLAB控制系統(tǒng)的輸入變量；而MATLAB控制系統(tǒng)的輸出變量為各機(jī)器人控制輸入，也是返回到ADAMS中的輸入變量，可控制各機(jī)器人的位姿，進(jìn)而調(diào)整被操作物的位姿到期望值，至此，就完成了一個(gè)閉環(huán)控制仿真循環(huán)。

根據(jù)表1中的參數(shù)，給定被吊運(yùn)物體的期望運(yùn)動(dòng)，利用前文所述的逆運(yùn)動(dòng)學(xué)方程求解得到機(jī)器人末端坐標(biāo)，并把機(jī)器人末端坐標(biāo)加載到虛擬系統(tǒng)平臺(tái)上，然后測(cè)量得到被吊運(yùn)物體的實(shí)際運(yùn)動(dòng)。對(duì)比期望值與實(shí)測(cè)值，可以看出建立運(yùn)動(dòng)學(xué)模型的正確性與UG/ADAMS/MATLAB建立虛擬控制仿真系統(tǒng)的合理性。

表1　系統(tǒng)參數(shù)

圖6(a)、(b)、(c)分別是物體沿x、y、z平動(dòng)的期望值與實(shí)測(cè)值對(duì)比；圖6(d)、(e)、(f)分別是物體繞x、y、z轉(zhuǎn)動(dòng)的期望值與實(shí)測(cè)值對(duì)比。經(jīng)過對(duì)物體位姿的三個(gè)平動(dòng)和三個(gè)轉(zhuǎn)動(dòng)的期望值與實(shí)測(cè)值對(duì)比，驗(yàn)證運(yùn)動(dòng)學(xué)模型的正確性以及UG/ADAMS/MATLAB 建立虛擬控制仿真系統(tǒng)在精度允許的范圍內(nèi)的合理性。

由圖可看出，平動(dòng)時(shí)期望值與實(shí)測(cè)值擬合度很高，但轉(zhuǎn)動(dòng)時(shí)兩者有較大的偏差，這可能是由于虛擬仿真系統(tǒng)建模時(shí)的簡(jiǎn)化導(dǎo)致的，接下來搭建實(shí)體實(shí)驗(yàn)平臺(tái)進(jìn)一步對(duì)所建模型進(jìn)行驗(yàn)證。

圖6　運(yùn)動(dòng)實(shí)驗(yàn)結(jié)果比對(duì)圖Fig.6　The comparison of experimental results

5結(jié)論

通過對(duì)緊耦合多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)逆運(yùn)動(dòng)學(xué)求解，完成了以下工作：

1) 建立了多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)的運(yùn)動(dòng)學(xué)模型及動(dòng)力學(xué)模型，分析了逆運(yùn)動(dòng)學(xué)，得到了顯示表達(dá)式。

2) 利用UG/ADAMS/MATLAB 建立了虛擬控制仿真系統(tǒng)實(shí)驗(yàn)平臺(tái)并驗(yàn)證了模型的準(zhǔn)確性。

逆運(yùn)動(dòng)學(xué)的研究是多機(jī)器人聯(lián)合吊運(yùn)系統(tǒng)的軌跡規(guī)劃、路徑跟蹤等方面研究的基礎(chǔ)，這是接下來要進(jìn)一步研究的方向。

參考文獻(xiàn)：

[1]THEODORE C R, TISCHLER M B, COLBOURNE J D. Rapid frequency-domain modeling methods for unmanned aerial vehicle flight control applications[J]. Journal of aircraft, 2004, 41(4): 735-743.

[2]BERNARD M, KONDAK K, HOMMEL G. A slung load transportation system based on small size helicopters[M]//MAHR B, HUANYE S. Autonomous systems - self-organization, management, and control. [S.l.]: Springer, 2008: 49-61.

[3]MAZA I, KONDAK K, BERNARD M, et al. Multi-UAV cooperation and control for load transportation and deployment[J]. Journal of intelligent and robotic systems, 2010, 57(1-4): 417-449.

[4]KONDAK K, BERNARD M, CABALLERO F, et al. Cooperative autonomous helicopters for load transportation and environment perception[M]//KRüGER T, WAHL F M. Advances in robotics research. Berlin Heidelberg: Springer, 2009: 299-310.

[5]MICHAEL N, FINK J, KUMAR V. Cooperative manipulation and transportation with aerial robots[J]. Autonomous robots, 2011, 30(1): 73-86.

[6]JIANG Qimi, KUMAR V. The inverse kinematics of 3-D towing[M]//LENARCIC J, STANISIC M M. Advances in robot kinematics: motion in man and machine. [S.l.]: Springer, 2010: 321-328.

[7]CHENG Peng, FINK J, KUMAR V. Cooperative towing with multiple robots[J]. Journal of mechanisms and robotics, 2008, 1(1): 011008.

[8]ZHAO Zhigang, LV Tiansheng. Dynamics analysis of close-coupling multiple helicopters system[J]. Chinese journal of aeronautics, 2008, 21(1): 71-78.

[9]趙志剛, 呂恬生. 多無人直升機(jī)吊運(yùn)系統(tǒng)運(yùn)動(dòng)學(xué)與穩(wěn)定性的仿真[J]. 系統(tǒng)仿真學(xué)報(bào), 2013, 25(4): 790-794, 799.

ZHAO Zhigang, LYU Tiansheng. Simulation on kinematics and stability of multi-helicopters hoist system[J]. Journal of system simulation, 2013, 25(4): 790-794, 799.

[10]趙志剛, 呂恬生. 緊耦合多飛行機(jī)器人懸停配平計(jì)算分析[J]. 系統(tǒng)仿真學(xué)報(bào), 2007, 19(1): 153-155.

ZHAO Zhigang, LYU Tiansheng. Equilibrium computation of close-coupling multiple helicopters in hover condition[J]. Journal of system simulation, 2007, 19(1): 153-155.

[11]趙志剛, 呂恬生. 多機(jī)器人協(xié)同吊運(yùn)系統(tǒng)的協(xié)調(diào)動(dòng)態(tài)載荷分配[J]. 機(jī)器人, 2012, 34(1): 114-119.

ZHAO Zhigang, LYU Tiansheng. Coordinated dynamic load distribution for multi-robot collaborative towing system[J]. Robot, 2012, 34(1): 114-119.

[12]于蘭, 鄭亞青. 3種構(gòu)型的6自由度繩牽引門式起重機(jī)器人的運(yùn)動(dòng)學(xué)位置逆解分析[J]. 華僑大學(xué)學(xué)報(bào): 自然科學(xué)版, 2012, 33(2): 125-128.

YU Lan, ZHENG Yaqing. Analysis of inverse pose kinematics for 3 6-DOF wire driven crane robots with different mechanism configurations[J]. Journal of Huaqiao University: natural science, 2012, 33(2): 125-128.

[13]ZI Bin, QIAN Sen, DING Huafeng, et al. Design and analysis of cooperative cable parallel manipulators for multiple mobile cranes[J]. International journal of advanced robotic systems, 2012, 9: 1-10.

The inverse kinematics analysis of multi-robot combined lifting system

ZHAO Zhigang1, TENG Fujun1, SHI Guangtian1，LI Jinsong2, JI Gang2

(1.School of Mechatronic Engineering, Lanzhou Jiaotong University, Lanzhou 730070 China; 2.School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai 200030, China)

Abstract：Regarding the coordination system of closed kinematic chain formed by multi-robot jointly lifting the same body, a systematic kinematic model was established on the basis of specified system configuration. By analyzing the systematic inverse solution of kinematics, and according to the expected movement of six degrees of freedom of lifted object, analytic expression of reverse kinematics was given. According to the UG/ADAMS/MATLAB virtual control simulation system, the virtual experimental platform of multi-robot lifting system was established. Under confirmed parameter conditions, the test has verified the reasonability of the established model and the accuracy of reverse kinematics analytic expression, which provideds a foundation for further research on the trajectory planning, path tracing, stability and other issues of the system.

Keywords：multi-robot; lifting system; inverse kinematics analysis; simulation verification

中圖分類號(hào)：TP242.2

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1006-7043(2016)02-0254-07

doi:10.11990/jheu.201409072

作者簡(jiǎn)介：趙志剛(1975-), 男, 教授,博士.通信作者：趙志剛，E-mail:zhaozhg@mail.lzjtu.cn.

基金項(xiàng)目：國(guó)家自然科學(xué)基金資助項(xiàng)目(51265021)；高等學(xué)校博士學(xué)科點(diǎn)專項(xiàng)科研基金新教師類資助項(xiàng)目(20126204120004)；教育部科學(xué)技術(shù)研究重點(diǎn)資助項(xiàng)目(212184).

收稿日期：2014-09-28.網(wǎng)絡(luò)出版日期：2015-12-15.

網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20151215.1141.028.html