紧耦合多机器人联合吊运系统逆运动学求解

赵志刚，滕富军，石广田，李劲松，季钢

(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