Stewart平台多能域系统动力学全解建模与实验

李永泉　刘天旭　王立捷　张立杰　程雪利

(1.燕山大学河北省并联机器人与机电系统实验室， 秦皇岛 066004；2.燕山大学先进锻压成形技术与科学教育部重点实验室， 秦皇岛 066004；3.燕山大学河北省重型机械流体动力传输与控制实验室， 秦皇岛 066004; 4.河南工学院机械工程系， 新乡 453002)

0　引言

并联机构的动力学模型是对其进行动力学分析、动力学优化以及动力学参数辨识的基础，而且准确的动力学模型是对机构高速高精度控制的前提。牛顿-欧拉法、拉格朗日方程法、虚功原理法和凯恩方程法是目前常用的并联机构动力学建模方法。这些方法在Stewart平台机构的动力学建模中得到了广泛的应用。文献[1-3]采用牛顿-欧拉法建立了该平台的动力学模型，通过对运动杆件的单独分析，列出运动杆件的牛顿方程，通过消去中间变量，得到包含关节约束力的整体动力学方程；文献[4-6]应用拉格朗日方程法，依据系统动能和势能构造拉格朗日函数，基于运动学模型计算系统动能、势能以及广义力，从而建立Stewart平台只包含驱动力而并没有关节约束力的动力学模型；文献[7-10]采用虚功原理建立了该平台动力学模型，引入虚位移和虚功的概念，基于惯性力和驱动力所做虚功之和为零这一基本思想，得到多刚体系统的动力学模型；鹿玲等[11]利用凯恩方程法对5UPS/PRPU进行动力学建模。李兵等[12]采用凯恩方程法对Stewart平台进行动力学建模，基本思想是广义惯性力和广义主动力之和为零，从而得到系统不含约束力的动力学方程。李鹭扬等[13]利用凯恩方程建立了Gough-Stewart平台的高效动力学模型。此外，FIJANY等[14]提出了一种新的算法，利用投影矩阵将平台的速度、加速度信息与关节的速度、加速度信息直接关联，建立了平台的动力学模型。以上方法都是针对机构本体建立的单一能域的动力学模型，而机器人系统本身可能包含伺服电机、液压缸、电动缸等驱动部分的一种或几种组合，本身属于多能域耦合系统。经典的动力学建模方法局限于并联机构机械部分本体的动力学建模，若要构建系统多能域全局动力学模型，需要对各个能域包含的子系统分别建模，建立中间变量将各个系统联系起来解，这就增加了动力学状态方程变量的数目，并使建模过程变得十分复杂，降低了模型的准确性。

为解决多能域子系统之间的相互作用，功率键合图(Bond graph)理论于1959年由PAYNTER[15]提出。经过KARNOPP等[16-17]的研究，键合图理论日益完善。针对Stewart并联机构，先后有DAMIC等[18]、YILDIZ等[19]采用功率键合图构建了包含驱动电机在内的键合图模型，并推导了平台的状态方程。但是由于Stewart平台分支较多且属于一个空间多自由度、多体系统，其键合图模型复杂度高、耦合性强，采用传统键合图理论建立的机构动力学模型复杂。为此，文献[20-21]在机构动力学建模过程中引入旋量理论，综合了旋量理论在描述空间构件以及键合图理论在多能域动力学建模的优势，提出旋量键合图概念。本文综合旋量理论在描述并联机构运动学方面的优势，结合键合图理论在动力学建模方面的简洁性，对并联机构的多能域动力学进行建模。

1　Stewart平台介绍

图1　Stewart平台Fig.1　Stewart platform

Stewart平台机构简图如图1a所示，其定平台上6个万向铰U的铰点用A1,A2,…,A6表示，动平台上6个球铰S的铰点用B1,B2，…，B6表示，定平台和动平台通过6个完全相同的UPS支链相连，移动副P作为驱动副，由电动缸实现支链的伸缩运动。在定平台上建立定坐标系Oxyz，坐标原点O位于万向铰所在平面的几何中心，z轴为过中心点O的竖直轴，它与定平台所在平面垂直，x轴垂直于A1A6，y轴由右手定则确定。同样建立动坐标系O1x1y1z1，即惯性系Ψ0。坐标原点O1位于球铰所在平面的几何中心,x1轴垂直于B1B6，z1轴垂直于运动平台所在平面，y1轴由右手螺旋定则确定。图1b为Stewart平台的三维模型。

2　Stewart平台机构本体旋量键合图模型

2.1　支链坐标系和动平台坐标系建立

首先建立用于旋量键合图建模的支链坐标系，如下：

杆件质心坐标系：简化驱动支链，假设支链上下连杆的质心Cui、Cdi位于支链伸缩方向上，如图2所示，在质心处构建与惯性系Ψ0各坐标轴方向平行的坐标系Ψuι、Ψdi。

图2　驱动支链结构图Fig.2　Structure diagram of drive chain

支链坐标系：如图3所示，在万向铰转动中心处，建立支链坐标系Ψ-i(i=1,2,3,4,5,6)，坐标原点为Ai，其z-轴沿支链伸缩方向由Ai指向Bi，y-轴垂直于平面OAiBi，沿z1负方向作为顺时针方向，则x-=y-z-。

图3　驱动支链分布图Fig.3　Distribution diagram of drive chain

运动副接触坐标系：首先，在万向铰转动中心Ai处建立万向铰接触坐标系ΨAi(i=1,2,3,4,5,6)，令yAi轴方向为万向铰十字轴轴线的方向，分布如图3所示，xAi轴垂直于yAi轴和支链伸缩方向si构成的平面，即xAi=yAisi，则zAi=xAiyAi；接着，分别在电动缸活塞杆和缸体接触点Ki和球铰转动中心Bi处，建立移动副接触坐标系ΨKi和球铰接触坐标系ΨBi，各坐标轴方向平行于支链坐标系Ψ-i的各坐标轴方向。

假设机构动平台受作用在E点载荷We，如图4所示。以E点为原点，建立与惯性系Ψ0平行的载荷坐标系ΨE。

图4　动平台组件结构图Fig.4　Structure diagram of moving platform

2.2　Stewart平台机构本体旋量键合图建模

Stewart平台的运动组件可视为由驱动支链组件和动平台组件两部分组成，其中驱动支链组件由移动副即伺服电缸、万向铰、球铰及测力传感器组成；动平台组件由动平台及球铰安装楔块组成。分别对这两部分进行旋量键合图建模。

根据文献[20]建立UPS支链旋量键合图，如图5所示。由于万向铰连接定平台，速度为0，因此采用0流源作为输入；而球铰连接动平台，通过键②/③/④/⑤/⑥/⑦，将驱动支链旋量键合图连接到动平台旋量键合图。键①则表示连接到电动缸驱动子系统键合图。建立动平台旋量键合图模型，如图6所示。

根据图5驱动支链键合图模型，列写驱动支链的状态方程

(1)

图6　动平台旋量键合图Fig.6　Screw bond graph of moving platform

mdi、mui——支链下、上连杆的质量

根据图6动平台键合图模型，可得动平台状态方程为

(2)

式(1)、(2)分别是驱动支链以及动平台的状态方程，即Stewart平台机构本体动力学模型，共包含78个未知量，对应的可以列出78个状态方程，可以得到该平台机构本体的动力学全解。

2.3　Stewart平台机构本体逆向动力学仿真

平台机构的逆向动力学模型是分析其动态性能及参数辨识的基础，基于机构的旋量键合图得到的状态方程，利用Matlab编程软件和ADAMS动力学仿真软件对其动力学模型进行仿真验证。

表1　Stewart平台部分参数Tab.1　Part of Stewart platform parameters

为使机构工作空间最大，令机构初始姿态为ψ=0°、φ=0°、θ=0°。采用组合正弦函数[22]对末端运动轨迹进行轨迹规划，末端运动轨迹如图7所示，通过反解计算得到电动缸的运动学信息，如图8所示。

图7　末端P的运动轨迹Fig.7　Trajectory of end P

图8　驱动位移Fig.8　Drive displacement

根据机构的运动学信息和状态方程，利用Matlab软件计算得到机构驱动力，如图9所示；同时，建立机构三维模型，利用ADAMS软件对其进行仿真，仿真验证模型如图10所示，得到机构驱动力如图11所示。对比图9和图11，由于在理论计算中忽略了球铰及万向铰等构件的惯量，从而使得理论值与仿真值产生了微小偏差。但是，误差相对于驱动力数值很小，因此可以验证基于旋量键合图得到的动力学模型的准确性。

图9　理论计算驱动力Fig.9　Theory value of driving force

图10　ADAMS仿真模型Fig.10　Simulation mode in ADAMS

图11　仿真驱动力Fig.11　Driving force of simulation

基于旋量键合图模型得到的状态方程，动力学全解除了6条支链的驱动力之外，还包括任意时刻万向铰、移动副和球面副处的约束力。驱动支链各个关节约束力变化如图12～14所示。

图12　万向铰处约束力Fig.12　Constraint force of universal joints

图13　球铰处约束力Fig.13　Constraint force of spherical joints

图14　移动副处约束力Fig.14　Constraint force of translational joints

3　Stewart平台多能域系统动力学模型

3.1　伺服电动缸键合图模型

Stewart平台采用电动缸作为执行机构。电动缸是将伺服电机与丝杠一体化设计的模块化产品，伺服电机转子的旋转运动通过滚珠丝杠机构转化为推杆的直线运动。其工作原理如图15所示。伺服电机及滚珠丝杠机构采用传统键合图建模，键合图模型如图16所示。

图15　电动缸物理模型Fig.15　Physical model of electric cylinder

图16　电动缸键合图模型Fig.16　Bonding graph model of electric cylinder

分别选取p和q作为状态变量，则状态方程为

(3)

式中La——电机电枢电感

Ra——电机电枢电阻

Ja——电机转子以及同步带轮惯量

Ta——电机电磁转矩常量

Tf——滚珠丝杠导程

K——滚珠丝杠扭矩刚度，为无穷大

3.2　Stewart平台多能域系统动力学模型

将Stewart平台机构本体旋量键合图与伺服电动缸驱动系统键合图结合，建立完整Stewart平台机、电耦合的多能域系统动力学全解模型，如图17所示。

依据式(1)～(3)，运行图7所示轨迹，可理论计算电机电流变换情况，如图18所示。对比图9与图18可以看出，电机电流和驱动力呈一定的比例关系，这符合伺服电动缸的特点，此外，电机电流的变化曲线的峰值并未超过所选电机的额定电流，可以验证所选伺服电机在此工况下满足要求。

4　实验

在加工完成Stewart平台机构样机的基础上，选取辅助的实验设备，完成平台机构本体动力学验证实验。实验样机和控制系统如图19所示，实验设备结构如图20所示。

图17　Stewart平台机、电耦合的多能域系统动力学全局模型Fig.17　Model of multi-energy domain dynamics of Stewart platform, which was mechanical and electronic coupling systems

图18　电机电流理论计算值Fig.18　Theory value of motor current

图19　实验系统Fig.19　System of experiment

图20　实验装置Fig.20　Device of experiment

其中，控制器选用倍福公司生产的C6920-0050型控制器，伺服电机选用的是中达电通公司的ECMA-C11010SS型伺服电机，电动缸选用上海涟恒公司伺服电动缸，型号为AH75-S500-B-T-10-M3-C1-2，物理参数如表2所示，力传感器的型号为LH-S50。

表2　电动缸的物理参数Tab.2　Physical parameters of electric cylinder

4.1　Stewart平台机构本体动力学验证实验

图21　机构位姿Fig.21　Position of mechanism

令机构运行图7所示轨迹，机构不同位姿如图21所示，通过力传感器采集数据，并进行滤波处理，得到驱动力数据的实测值，和理论计算值进行对比，结果如图22所示。对比图22、图9、图11，三者符合度较高，从而证明基于旋量键合图所得机构本体动力学模型的正确性。

图22　驱动力理论值与实测值对比Fig.22　Comparisons of driving force between theory and measured values

4.2　Stewart平台机构多能域动力学模型验证实验

将数据线一端连接驱动器，一端连接装有与所选台达电机配套的ASDA-SOFT软件的计算机，通过软件中的示波器，读取实验电机电流，并将数据保存为.TXT格式的文件，和理论计算值进行对比，结果如图23所示，两者误差较小，从而验证了Stewart平台多能域系统动力学全解模型的正确性。

图23　电机电流实测值与理论值对比Fig.23　Comparisons of motor current between measured and theory values

5　结束语

采用旋量键合图建立了电动缸驱动Stewart平台机构本体动力学模型，采用传统键合图建立了伺服电动缸键合图模型，得到了该平台完整的机、电耦合的多能域系统动力学全解模型；并通过仿真分析与设计实验两方面验证了平台机电耦合多能域系统动力学全解的正确性，为后续包含驱动单元惯性参数和摩擦参数在内的动力学参数辨识以及平台控制研究奠定了基础。

1DASGUPTA B, MRUTHYUNJAYA T S. A Newton-Euler formulation for the inverse dynamics of the Stewart platform manipulator[J]. Mechanism & Machine Theory, 1998, 33(8):1135-1152.

2KHALIL W, GUEGAN S. A novel solution for the dynamic modeling of Gough-Stewart manipulators[C]∥Proceedings of the 2002 IEEE International Conference on Robotics and Automation, 2002, 1:817-822.

3KHALIL W, IBRAHIM O. General solution for the dynamic modeling of parallel robots[J]. Journal of Intelligent & Robotic Systems, 2007, 49(1):19-37.

4WANG J, GOSSELIN C M. A new approach for the dynamic analysis of parallel manipulators[J]. Multibody System Dynamics, 1998, 2(3):317-334.

5ABDELLATIF H, HEIMANN B. Computational efficient inverse dynamics of 6-DOF fully parallel manipulators by using the Lagrangian formalism[J]. Mechanism & Machine Theory, 2009, 44(1):192-207.

6吕帮俊, 彭利坤, 邢继峰,等. Gough-Stewart并联机器人刚体动力学模型[J]. 华中科技大学学报：自然科学版, 2011, 39(10):14-18.

LÜ Bangjun, PENG Likun, XING Jifeng, et al. Rigid dynamic model of Gough-Stewart parellel robot[J]. Journal of Huazhong University of Science and Technology:Natural Science Edition, 2011, 39(10):14-18. (in Chinese)

7TSAI L W. Solving the inverse dynamics of a Stewart-Gough manipulator by the principle of virtual work[J]. Journal of Mechanical Design, 2000,122(1):3-9.

8STAICU S. Dynamics of the 6-6 Stewart parallel manipulator[J]. Robotics and Computer-Integrated Manufacturing, 2011, 27(1):212-220.

9吕帮俊, 朱石坚, 邢继峰,等. 基于虚功原理的Stewart机构逆动力学模型修正[J]. 机械设计与研究, 2010, 26(4):42-44.

LÜ Bangjun, ZHU Shijian, XING Jifeng, et al. Modification of the invers dynamics model of Stewart robot by the principle of virtual work[J]. Machine Design & Research, 2010, 26(4):42-44. (in Chinese)

10张东胜, 许允斗, 姚建涛,等. 五自由度混联机器人逆动力学分析[J/OL]. 农业机械学报, 2017，48(9):384-391. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20170949&journal_id=jcsam.DOI：10.6041/j.issn.1000-1298.2017.09.049.

ZHANG Dongsheng, XU Yundou, YAO Jiantao, et al. Inverse dynamics analysis of novel 5-DOF hybrid robot[J/OL]. Transactions of the Chinese Society for Agricultural Machinery，2017，48(9):384-391.(in Chinese)

11鹿玲, 姚建涛, 顾伟栋,等. 基于Kane方程的冗余驱动5UPS/PRPU并联机床动力学分析[J/OL]. 农业机械学报, 2016, 47(6):366-372.http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20160648&journal_id=jcsam.DOI：10.6041/j.issn.1000-1298.2016.06.048.

LU Ling, YAO Jiantao, GU Weidong, et al. Dynamics analysis of 5UPS/PRPU parallel machine tool with redundant actuation based on Kane equation[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(6):366-372. (in Chinese)

12李兵, 王知行, 李建生. 基于凯恩方法的新型并联机床动力学研究[J]. 机械科学与技术, 1999, 18(1):41-43.

LI Bing, WANG Zhixing, LI Jiansheng. Dynamic study of new parallel machine tool based on Kane method[J]. Mechanical Science and Technology for Aerospace Engineering, 1999, 18(1):41-43. (in Chinese)

13李鹭扬, 吴洪涛, 朱剑英,等. Gough-Stewart平台高效动力学建模研究[J]. 机械科学与技术, 2005, 24(8):887-889.

LI Luyang, WU Hongtao, ZHU Jianying, et al. Study on the dynamic modeling of Gough-Stewart platform[J]. Mechanical Science and Technology for Aerospace Engineering, 2005, 24(8):887-889. (in Chinese)

14FIJANY A, FRIED G. Novel algorithms for computation of inverse kinematics and inverse dynamics of Gough-Stewart platform[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2012:1573-1580.

15PAYNTER H M. Analysis and design of engineering systems[M]. Cambrige: M.I.T. Press, 1961.

16KARNOPP D C, MARGOLIS D L, ROSENBERG R C. System dynamics: modeling, simulation and control of mechatronic Systems[M]. New York: John Wiley & Sons, Inc., 2012:12-20, 53-58, 142-149, 256-259.

17KARNOPP D C, MARGOLIS D L, ROSENBERG R C. System dynamics: modeling and simulation of mechatronic systems[M]. New York: John Wiley & Sons, Inc., 2006.

18DAMIC V, COHODAR M. Dynamic analysis of Stewart platform by bond graphs[J]. Procedia Engineering, 2015, 100:226-233.

19YILDIZ I, OMURLU V E, SAGIRLI A. A novel visualization technique in bond-graph method for modeling of a generalized Stewart platform[C]∥2008 IEEE International Conference on Robotics and Biomimetics, 2008:780-785.

20郭菲, 李永泉, 宋肇经,等. 旋量键合图在并联机构动力学建模中的应用[J]. 机械工程学报, 2015, 51(23):12-20.

GUO Fei, LI Yongquan, SONG Zhaojing et al. Screw bond graph in the application of the parallel mechanism dynamics modeling[J]. Journal of Mechanical Engineering, 2015, 51(23):12-20. (in Chinese)

21BONDERSON L S. Vector bond graphs applied to one-dimensional distributed systems[J]. Journal of Dynamic Systems Measurement & Control, 1975, 97(1):75-82.

22李永泉. 球面2-DOF并联机构的理论分析与实验研究[D]. 秦皇岛：燕山大学, 2012:75-76.

LI Yongquan. Theory analysis and experiment research of spherical 2-DOF parallel manipulator[D]. Qinhuangdao:Yanshan University, 2012:75-76. (in Chinese)