緊耦合多機器人聯合吊運系統逆運動學求解

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

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

?

緊耦合多機器人聯合吊運系統逆運動學求解

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

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

摘要：針對多機器人共同吊運同一個物體形成閉運動鏈的協調系統，在給定系統構型的基礎上，建立了系統運動學模型，并建立了系統動力學模型。分析了系統的運動學逆解，根據被吊運物體的六個自由度期望運動，給出了逆運動學的解析表達式。利用UG/ADAMS/MATLAB虛擬控制仿真系統，搭建了多機器人吊運系統虛擬實驗平臺。在確定的參數條件下，實驗驗證了所建立模型的合理性及逆運動學解析表達式的準確性，為進一步研究系統的軌跡規劃、路徑跟蹤、穩定性等問題奠定了基礎。

關鍵詞：多機器人；吊運；逆運動學分析；仿真驗證

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

多機器人聯合吊運系統的運動學逆解是系統進行一系列研究的基礎，對整個系統的研究有著至關重要的作用。鄭亞青等[12]探討了三種構型的六自由度繩牽引門式起重機器人的運動學位置逆解。Bin Zi等[13]對三臺吊車同時吊運一個重物進行了逆解分析。可以看出，關于多機器人聯合吊運的運動學逆解，基本是對特定構型的系統進行探討，沒有形成通用的體系。

文中以三臺工業機器人為例，建立了系統運動學模型, 對系統逆運動學進行分析,建立了虛擬控制仿真系統。進行仿真計算，并對結果進行了分析。

1多機器人聯合吊運系統構型

多機器人聯合吊運系統是3臺三自由度的機器人通過繩索吊運重物構成，如圖1所示。

圖1　多機器人吊運系統空間構型布局Fig.1　The layout diagram of the multi-robot lifting system

在重物的質心建立重物體坐標系og-xyz如圖2所示，它們的初始姿態始終也相同。

圖2　連接點坐標示意圖Fig.2　The coordinate diagram of junctions

2系統運動學建模

2.1 單個機器人運動學模型

(1)

式中：ci1、ci2、ci3是機器人3個關節角的余弦值；si1、si2、si3是機器人的3個關節角的正弦值。通過坐標轉換，得到3臺機器人末端相對固定坐標系o-xyz的坐標為

(2)

2.2多機器人吊運系統運動學模型

由單個機器人的運動學模型，解得已知機器人的關節角，得到機器人末端相對于固定坐標系的坐標。建立吊運系統的運動學模型即是建立物體位姿關于機器人末端點坐標的函數關系表達式。

2.2.1吊運系統的位移約束

定義繩向量是結點坐標減去相對應的機器人末端點坐標。繩向量滿足矢量封閉原理如圖2所示，在三角形P1GJ1、P2GJ2、P3GJ3中存在關系式PiJi等于PiG與GJi之和：

(3)

式中：

(4)

(5)

在吊運系統中，繩索的長度始終不變，根據向量模的性質存在如下關系式：

(6)

2.2.2 吊運系統的速度關系

定義繩速度是結點Ji相對于Pi點的速度，由于繩長不變，繩速度Vi與繩向量PiJi的方向垂直。從圖3中可以得到結點Ji相對于Pi點的速度Vi就是繩向量PiJi的導數。可以得到下列關系式：

(7)

式中：

(8)

(9)

(10)

其中，

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

2.2.3機器人末端位移約束

在多機器人聯合吊運過程中，由于多解的存在，使多機器人的運動學逆解比較復雜，本文中限定機器人末端軌跡為直線，如圖3所示，從而使所建模型只有單一逆解。由于直線的方程為一般方程，可以具體的、系統的、全面的分析多機器人聯合吊運過程。當3臺機器人的末端的位移為直線時，可以列出機器人末端位移的方程:

(11)

式中：mi、ni、ri、ai、bi、ci分別為一般直線的參數。位移直線必須在相對應的單個機器人的可行域內，單臺機器人的可行域是機器人末端點在空間可以達到空間范圍，可以限定直線的參數范圍保證線段在所對應的可行域內。在本文的求解時，假定參數滿足以上條件。

2.2.4吊運系統動力學方程

(12)

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

(13)

(14)

3系統逆運動學分析

吊運系統物體有六個自由度的運動，包括三個平移運動和三個旋轉運動。從任務出發，并考慮分析的方便性，限定機器人末端軌跡為直線，這樣逆運動學求解過程不再有耦合性，可將系統運動直接分解為六個子運動。假設被吊運物體在歐拉角為零的初始點開始運動，可以得到以下的結果。由于單個機器人的逆運動學相對比較成熟，這里不再進行討論，重點探討機器人末端位置和被吊運物體位姿之間的關系式。

3.1平動時逆運動學分析

被吊運物體從歐拉角都為零的初始點開始運動，假設當其沿一個軸平動時，在其他軸上的運動為零，同時三個歐拉角始終為零。vx為沿x軸的速度，vy為沿y軸的速度，vz為沿z軸的速度。可以分別得到物體單向平移運動時機器人末端位置關于物體運動期望值的關系式。

3.2旋轉時逆運動學分析

當被吊運物體繞某一軸轉動時，假定繞其他軸的角速度、角度都為零，同時三個軸向位移始終為零。ω1為沿x軸的角速度，ω2為沿y軸的角速度，ω3為沿z軸的角速度。可以分別得到物體單向旋轉運動時機器人末端位置關于物體運動期望值的關系式。

4仿真實驗

由于纜系式緊耦合多機器人系統比較復雜，出于安全和成本的考慮，利用UG/ADAMS/MATLAB建立虛擬控制仿真系統。系統運動控制虛擬仿真結構如圖 4 所示。

圖4　虛擬仿真系統結構圖Fig.4　The structure of virtual simulation system

首先在UG中繪制系統三維圖形，并導入到ADAMS中，其中繩索建模時，應用100段小圓柱剛體，每段小圓柱剛體之間由球鉸連接，實現虛擬系統中繩索的建模，則建立的虛擬系統如圖5所示。然后利用MATLAB/Simulink設計控制系統，最后利用ADAMS和MATLAB/Simulink之間的接口構成一個閉環虛擬仿真系統。

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

在控制仿真過程中，ADAMS中多機虛擬模型的輸出變量為各機器人位姿測量值，是進入MATLAB控制系統的輸入變量；而MATLAB控制系統的輸出變量為各機器人控制輸入，也是返回到ADAMS中的輸入變量，可控制各機器人的位姿，進而調整被操作物的位姿到期望值，至此，就完成了一個閉環控制仿真循環。

根據表1中的參數，給定被吊運物體的期望運動，利用前文所述的逆運動學方程求解得到機器人末端坐標，并把機器人末端坐標加載到虛擬系統平臺上，然后測量得到被吊運物體的實際運動。對比期望值與實測值，可以看出建立運動學模型的正確性與UG/ADAMS/MATLAB建立虛擬控制仿真系統的合理性。

表1　系統參數

圖6(a)、(b)、(c)分別是物體沿x、y、z平動的期望值與實測值對比；圖6(d)、(e)、(f)分別是物體繞x、y、z轉動的期望值與實測值對比。經過對物體位姿的三個平動和三個轉動的期望值與實測值對比，驗證運動學模型的正確性以及UG/ADAMS/MATLAB 建立虛擬控制仿真系統在精度允許的范圍內的合理性。

由圖可看出，平動時期望值與實測值擬合度很高，但轉動時兩者有較大的偏差，這可能是由于虛擬仿真系統建模時的簡化導致的，接下來搭建實體實驗平臺進一步對所建模型進行驗證。

圖6　運動實驗結果比對圖Fig.6　The comparison of experimental results

5結論

通過對緊耦合多機器人聯合吊運系統逆運動學求解，完成了以下工作：

1) 建立了多機器人聯合吊運系統的運動學模型及動力學模型，分析了逆運動學，得到了顯示表達式。

2) 利用UG/ADAMS/MATLAB 建立了虛擬控制仿真系統實驗平臺并驗證了模型的準確性。

逆運動學的研究是多機器人聯合吊運系統的軌跡規劃、路徑跟蹤等方面研究的基礎，這是接下來要進一步研究的方向。

參考文獻：

[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]. 系統仿真學報, 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]. 系統仿真學報, 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]. 機器人, 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種構型的6自由度繩牽引門式起重機器人的運動學位置逆解分析[J]. 華僑大學學報: 自然科學版, 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

中圖分類號：TP242.2

文獻標志碼：A

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

doi:10.11990/jheu.201409072

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

基金項目：國家自然科學基金資助項目(51265021)；高等學校博士學科點專項科研基金新教師類資助項目(20126204120004)；教育部科學技術研究重點資助項目(212184).

收稿日期：2014-09-28.網絡出版日期：2015-12-15.

網絡出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20151215.1141.028.html