主被动关节柔性树形机器人系统动力学建模与仿真*

田富洋曹东董小宁李法德蔡占河

（1.山东农业大学机械与电子工程学院，山东省园艺机械与装备重点实验室，泰安 271018）（2.菏泽市产品检验检测研究院，菏泽 274000）

主被动关节柔性树形机器人系统动力学建模与仿真*

田富洋1†曹东1董小宁1李法德1蔡占河2

（1.山东农业大学机械与电子工程学院，山东省园艺机械与装备重点实验室，泰安 271018）（2.菏泽市产品检验检测研究院，菏泽 274000）

进一步完善空间算子代数理论体系，研究了带有主被动关节的树形柔性机器人系统符号动力学高效率建模的方法.利用拓扑结构和低序体阵列的方法描述了树型机器人系统.采用空间算子研究了欠驱动树型多体系统的描述方法.根据系统中铰的驱动情况分别对铰链定义为主动铰和被动铰，建立树形机器人系统的运动学，然后通过铰链的类型分别按照两次从系统的顶端到基座的顺序、一次从基座到顶端的顺序进行了系统铰接体惯量的递推、系统冗余力的递推、广义加速度和广义主动力的递推，最后将分叉处的广义力和广义加速度矢量相加得到树形机器人系统的广义动力学模型.通过上述递推过程建立动力学模型，实现了高效率O（N）（N为系统自由度）次的计算效率.最后以加拿大树形空间机械臂为例做仿真验证了本研究内容的正确性和高效性.

空间算子，树形，主被动关节，广义动力学，高效率

引言

随着机器人技术的发展，机器人系统的结构越来越复杂［1］.目前的机器人多体系统构型包括链式系统、树形系统如图1所示等结构，并且系统中既含有刚体又包括柔性体，该机器人系统的动力学建模越来越复杂［1～4］.因此研究带有刚体和柔体的树型机器人系统动力学高效率建模成为重要的课题［5～6］.

Carrera和Serna［7］、J.Znamenacek和M.Valasek［8］、Hwang Yunn-Lin［9］等人分别研究了柔性多系统动力学建模方法，但都是O（N3）的计算效率.研究O（N）次算法已经成为多体系统动力学研究领域中的重要内容［6］.美国NASA科学家G.Rodriguez［11－12，20，22］等人发展了基于空间算子代数（Spatial Operator Algebra，SOA）的多体系统动力学方法，其结果在理论上加深了对多体系统的认识，算法上提高了计算效率，但其公开发表的文献中，只研究了树形系统的运动学，没有将空间算子代数理论推广到树型柔性系统动力学建模中.国内刘云平［13］、赵大旭［14］、方喜峰［15］、孙宏丽［16］和田富洋［17～18］等系统地研究了空间算子代数理论，并且进行了程序实验.

图1 加拿大太空机械臂Fig.1 Space manipulator in Canada

在实际工程应用中，在整个机器人系统中所有的关节不一定全部都知道驱动力矩，也不一定全部明确加速度，是一部分确定关节力矩而另一部分确定关节加速度，其动力学建模不是单纯的正向动力学和反向动力学，这种系统通常被称为欠驱动多体系统［18－19］.利用递推形式的正向和反向动力学建模方法无法进行计算.G.Rodriguez［20］和Bobrow［21］分别利用空间算子代数和伴随算子研究了多刚体链式系统的混合递推动力学建模，而没有推广到树型刚-柔系统动力学建模中.田富洋［18－19］利用空间算子代数进行了链式刚体、柔体和刚柔耦合的广义动力学建模算法的研究.

本文利用空间算子描述树型柔性多体系统广义递推动力学建模过程，并且建立了一套树型柔性多体系统高效率的O（N）次混合递推动力学算法，通过定义铰链的类型（主动或被动）可以对正向、反向和混合动力学进行建模.

图2 树形机器人Fig.2 Tree Robots System

1 空间算子代数理论体系

1.1 刚柔机械臂树型系统模型

为不失一般性，本文研究的是如图1所示具有由n个铰链将n个柔性体连接成s链的一般欠驱动柔性树型多体系统.为便于编程和计算，本文对上述树型柔性结构做如下约定：

1）链信息的约定：如图1所示树型机器人系统，系统的链数与分叉数目相等，每条链中的包含从链的基座到顶端的所有体，如3链中含有1、2、7和8体.当k号铰链是主动关节时记为Ia，否则记为Ip.

2）体与铰信息的约定：体号与铰号按照链的顺序依次排序，如图1所示.k号铰链是k号机械臂的内连接铰链.本文假定所有的铰为一个自由度的旋转铰，当已知铰的旋转加速度而未知施加的力矩则称为主动铰；反之则为被动铰.

3）最外虚拟体和虚拟铰链的约定：定义每条链的最外虚拟体体号和虚拟铰铰号为：最外体号加该链的链号.如：第3条链的虚拟体号和铰链号为最大体号9＋链号3＝12，则最外虚拟铰号和虚拟体号都为12.

表1 低序体阵列Table 1 Lower Body Array

根据上述定义的树型结构的模型，本文采用低序体阵列表达如表1所示的可描述树型航天器的结构表达.其中：C1、C2、C3和C4列中的数字分别表示系统中包含在该链中的柔体的体号；X所在的行表示系统中的虚拟体号；Li表示i级的体号.

图3 柔性体链接关系Fig.3 Illustration of tree flexible body system

本文假定机器人系统中所带有的柔性体是一个由有限个节点（在空间位置中定义i）组成的有限元模型.空间位置可以由连接到物体上的坐标系表示出来.k号柔性体有限元模型包括一个质量矩阵Mm和一个刚度矩阵Km，它们可由结构动力学分析得到.质量矩阵表示为：

其中，Ms（k）表示k体的结构质量矩阵；Ks（k）表示k体的结构刚度矩阵；∏（k）表示k体的模态矩阵；B（k）表示k体的坐标系到k体内节点的变换矩阵.

柔性体的质量矩阵和刚度矩阵是不随时间变化可预先计算得到的独立变量，是由计算好的振动模型和相对应的模态频率决定的.

1.2 树型系统运动学模型

定义机器人系统中柔性机械臂的刚性广义坐标和模态广义坐标分别为（假定系统中全是柔性的）：

其中，n为系统中的总体数；Nm表示柔性机械臂的模态数.

在本研究中，为提高计算效率所求解的速度和加速度都是在本体坐标系下定义的（本体坐标系建立在体的顶端，如k体的坐标系建立在第k铰链上）.按照从每条链i（i＝1，…，s）的基座到顶端的顺序递推计算机器人系统的速度模型和加速度模型，运动学递推过程可表示成如下形式（具体理论过程见参考文献［11，20－22］：

其中，Vmi（k）为第i链中柔性附件k的模态速度；Φ（k－1，k）分别为柔性附件本体坐标系的移位矩阵和k－1操作臂到k操作臂的模态移位矩阵；ηi（k）表示i链中k体的模态坐标；ami（k）为i链中的柔性体k的加速度项；bmi（k）表示i链中的柔性体的科氏加速度项，可以表示为

Ci（Lj）表示低序体阵列i链中的第Lj个柔性附件的编号，C2（L2）表示第2链中L2体，根据表1所示是2体.

1.3 反向动力学递推

根据方程（1）可获得柔性航天器树型多体系统反向动力学高效递推算法.在此基础上，从每条链i（i＝1，…，s）的顶端到基座递推计算各连杆的广义力和力矩，递推过程分为两步：首先计算各条链中的广义力和力矩；然后将分叉处的广义力和力矩线性相加.可表示如下：

（1）各链中柔性体所受的广义力递推和力矩递推模型

其中，φ（tk，k＋1）为k体在有限元离散后与k＋1铰链连接的元素到k＋1体的移位算子；cmi（k）为i链中k体离心力项，为：

fmi（k）表示i链中k体受到的广义力.

（2）整个系统中柔性体的广义力和力矩为：

1.4 一般树型系统正向动力学

按照牛顿定律，多体系统动力学线性方程可以表示为：

其中，Mä表示惯性力项；T为主动力项；z为系统的速度的一次项，冗余力项.

根据多柔体空间算子代数理论［18－19，22］，系统广义质量按照LDL*分解，可以分解为：M＝［I＋HΦK］D［I＋HΦK］*，进而可以得到系统的广义质量的逆：M－1＝［I－HΨK］*D－1［I－HΨK］.因此方程（4）用算子表达的形式得到正向动力学表达式为：

按照文献［22］的方法分别计算系统中每条链的正向动力学，由于所计算的参数（坐标、速度、加速度等）都是在本体坐标系下定义的，可将每条链的计算结果线性相加得到树型系统的正向动力学算法.

2 柔性树型多体系统广义动力学建模理论

本节主要研究柔性树型多体系统的广义递推动力学高效率建模方法.按照如图2表示，树型系统是含有主动关节和被动关节的树型欠驱动系统，按照传统的正向、反向动力学建模方法不能进行动力学计算，需要研究适合求解欠驱动系统问题的混合动力学建模方法.

欠驱动机器人系统动力学方程可以表述如下：

其中，Maa为主动关节系统的质量矩阵；Mpp为被动关节质量矩阵；Map为耦合质量矩阵.

对于研究的欠驱动机器人系统对象，需要求解主动关节的外力矩T以及被动关节的加速度项，因此对公式进行分解可以得到：

结合公式可以得到：

对公式（6）做递推，步骤如下：（1）以从顶端到基座的顺序依次判断铰链的类型，然后按照铰接体惯量方法递推计算每一链中柔性体的惯量算子；（2）以顶端到基体的顺序根据铰链的类型递推计算每一链中柔性体的冗余力项；（3）以从基体到顶端的顺序根据铰链的类型递推计算每一链中柔性体的加速度项；（4）以从基体到顶端的顺序，将分叉处柔体的加速度项进行线性相加.由于该算法中的算子是柔性和刚性元素混合计算的，矩阵庞大，出现一些不必要的矩阵运算，降低了计算效率，因此利用块划分和标入上标f和r表示柔性和刚性元素，将递推算法中的算子进行柔性元素和刚性元素分块计算，将会提高计算效率，参考文献［18－22］.

（1）以从系统顶端到基座的顺序按照铰接体惯量方法递推计算系统中每一条链中的惯量算子.该递推过程为计算每条链的柔性附件的惯量矩阵.柔性附件惯量矩阵计算如方程所示：

（2）按照从顶端到基体的顺序递推计算系统的速度一次项的冗余力项.该递推过程与计算惯量矩阵类似为计算每条链的柔性附件的冗余力项.柔性体冗余力计算如方程（9）所示：

（3）按照从基体到顶端的顺序递推计算柔性树型系统各个体的加速度项.该递推计算过程与计算惯量矩阵类似，为计算柔性体中每条链的柔性体的加速度项和力矩项.

柔性附件冗余力计算如方程所示：

（4）将每条链中柔性体的广义力、力矩和模态加速度线性相加：

上述方程为欠驱动柔性树型多体系统广义递推动力学建模算法.方程中出现的空间算子和滤波算子可在文献［16，18－19］中查阅.本算法是一般柔性树型多体系统广义动力学的建模算法.它的主要特征是同时包含正向动力学、反向动力学和混合动力学建模三种算法.当系统中的铰链全是被动关节时，则算法变成正向动力学算法；当系统中的铰链全是主动关节时，则算法变成反向动力学算法；其他情况下则为混合动力学算法.另外，该算法具有高效率O（N）阶的动力学建模的特点，又兼有编程简单，实用性广泛等优点，进一步完善了空间算子代数理论体系，对于目前多数实际工程尤其是大型柔性航天器的动力学建模具有重要的理论与应用价值.

3 混合动力学软件编制与验证

3.1 软件编制

为验证上述理论的正确性和高效性，采用Mathematica符号建模平台，编制了一般多柔体系统的广义动力学建模分析软件HybridDynamics.通过给定树形多体系统的拓扑结构、物理和几何参数以及输入条件，计算机自动进行多体系统运动学和广义动力学的递推计算，并输出数值与符号共存体系的结果.如图4所示，软件流程包括：

（1）定义柔性体和铰链的数据结构；

（2）根据柔性体和铰链信息定义关键空间算子；

（3）输入机器人系统拓扑结构和低序体阵列；

（4）根据低序体阵列计算所定义柔性系统的每个体的速度以及计算每个柔体的科氏力加速度项a（k）以及科氏力b（k）；

（5）根据铰链类型，进行系统的广义动力学建模；

（6）结果输出，进行数值分析求解.

图4 软件流程图Fig.4 Flow chat of software

3.2 动力学建模

为验证本文提出的广义递推动力学建模算法的准确性，作者根据Lagrange理论，编制了利用该方法建立机械多体系统动力学方程的相关软件来验证空间算子代数理论.通过验证，对于链式、树型等自由度小于7的刚、柔性多体系统，利用两种理论得到的动力学模型一致，因此可以判断空间算子代数的正确性.本文进一步在ADAMS中建立以加拿大树形机械臂为对象进行了动力学建模对比.所研究的对象为如图5所示的模型在本体上安装三条机械臂链，共包括10个柔性机械臂、1个本体.假定柔性体有6个模态坐标，系统包括16个刚性自由度和60个柔性自由度，共76个自由度.各关节为理想刚性关节，各个体的参数为：质量均为1kg，绕内关节的转动惯量均为diag［0.1，0.13，0.13］，连杆的长度均为0.4m，各关节的初始速度为0，在第2、3、4、7、8、9、10关节上施加的力矩为10*sin［t］Nm，第1、5、6关节施加运动为3*sin［t］degree.仿真结果和Mathematica仿真如图6～10所示，结果基本一致.需要说明的是本文提出的动力学高效率建模算法对动力学仿真即微分-代数方程的求解算法的影响，作者没有进一步验证和分析.

图5 实验模型Fig.5 Test Model

图6 关节3角加速度Fig.6 Angular acceleration of Joint3

图7 关节5角加速度Fig.7 Angular acceleration of Joint5

图8 关节8角加速度Fig.8 Angular acceleration of Joint8

3.3 高效率验证

在保证准确性的情况下，本文对该算法的计算效率同其他算法进行了比较研究.假定上述研究目标中所有柔性附件的模态为5阶，则整个系统的自由度为60.在同等计算条件下（计算机配置为：CPU：Pentium（R）Dual-Core（TM）E6500 2.8GHz；内存：2.0GB），利用该算法对上述研究目标建立动力学模型的时间为582.21 s，并且在整个建模过程中利用内存较少，不影响电脑的使用；而利用Lagrange动力学建模方法对研究目标进行符号建模，计算1小时后电脑出现“死机”现象，无法得到动力学模型.需要说明一点，虽然整个系统的动力学建模比较高效率，但是动力学模型是大型的微分-代数方程组，求解过程非常困难，因此研究高效高精度微分-代数方程是十分重要的.

图9 关节5力矩曲线Fig.9 Torque of Joint5

图10 关节6力矩曲线Fig.10 Torque of Joint6

4 结论

本文利用拓扑结构和低序体阵列的方法描述了树型机器人系统；采用空间算子研究了欠驱动树型多体系统的描述方法；研究了树型柔性多体系统广义动力学高效率O（N）阶建模的算法；通过判断系统中铰链的类型，可快速、高效、准确的建立柔性航天器多体系统广义动力学模型.本算法可以应用于一般多柔体系统、欠驱动系统，为大型柔性多体系统动力学高效率建模打下坚实的基础.

1 Jang-Soo C，Taw-Won P，Kim J.Dynamic analysis of a flexible multibody system.International Journal of Precision Engineering and Manufacturing，2005，6（4）：21～25

2 Featherstone R.Rigidbody dynamics algorithms.Springer，New York，2008

3 Yamane K，Nakamura Y.Comparative study on serial and parallel forward dynamics algorithms for kinematic chains.International Journal of Robotics Research，2009，28（5）：622～629

4 Moussa S，Botouoni A.Dynamics of a rotating flexible and symmetric spacecraft using impedance matrix in terms of the flexible appendages cantilever modes.Multibody System Dynamics，2008，19（4）：345～364

5 Pascal M.Some open problems in dynamic analysis of flexible multibody systems.Multibody System Dynamics，2001，5（4）：315～334

6 Subir K S.Recursive dynamics algorithms for serial，parallel，and closed-chain multibody systems.Indo-US Workshop on Protein Kinematics＆Protein Conformations，IISc，Bangalore，2007，（10-11）：1～34

7 Carrera E.Serna MA.Inverse dynamics of flexible robots.Mathematics and Computers in Simulation，1996，41（5）：485～508

8 Znamenáĉek J，Valášek M.An efficient implementation of the recursive approach to flexible multibody dynamics.Multibody System Dynamics，1998，2（3）：227～252

9 Hwang Y L.Recursive Newton-Euler formulation for flexible dynamic manufacturing analysis of open-loop robotic systems.International Journal of Advanced Manufacturing Technology，2006，29（5）：598～604

10 Amir F，Featherstone R.A new factorization of the mass matrix for optimal serial and parallel calculation of multibody dynamics.Multibody System Dynamics，2013，29（2）：169～187

11 Featherstone R.Efficient factorization of the joint space inertia matrix for branched kinematic trees.The International Journal of Robotics Research，2005，24（24）：487～500

12 Patrick W，Featherstone R，David E.A reduced-order recursive algorithm for the computation of the operationalspace inertia matrix.In：2012 IEEE International Conference on Robotics and Automation.RiverCentre，Saint Paul，Minnesota，USA.2012：14～18

13 刘云平，顾和军，吴洪涛.空间操作臂的高效率动力学建模方法.华中科技大学学报（自然科学版），2011，39（6）：75～78（Liu Y P，Gu H J，Wu H T.High effective dynamics modeling for space manipulator.Journal of Huazhong University of Science and Technology（Natural Science Edition），2011，39（6）：75～78（in Chinese））

14 赵大旭，陈柏，吴洪涛，田富洋.潜游式管道机器人动力学分析.华南理工大学学报，2010，38（8）：65～71（Zhao D X，Chen B，Wu H T，Tian FY.Dynamic analysis of submersible in－pipe robot.Journal of South China University of Technology，2010，38（8）：65～71（in Chinese））

15 方喜峰，吴洪涛，刘云平等.基于空间算子代数理论计算多体系统动力学建模.机械工程学报，2009，45（1）：228～234（Fang X F，Wu H T，Liu Y P et al.Computational dynamics of multi-body system modeling based on spatial operator algebra theory.Chinese Journal of Mechanical Engineering，2009，45（1）：228～234（in Chinese））

16 孙宏丽，吴洪涛，田富洋，邵兵.空间算子代数刚柔耦合系统动力学软件流程.华中科技大学学报，2009，37（12）：107～111（Sun H L，Wu H T，Tian F Y，Shao B.Dynamic software flow of the rigid-flexible coupling system based on spatial operator algebra theory.Journal of Huazhong University of Science and Technology，2009，37（12）：107～111（in Chinese））

17 Tian FY，Wu H T，Sun H L.The dynamics of open loop flexible robots system base on efficient recursive method.International Conference on Measuring Technolog＆Mechatronics Automation，2009，2：148～151

18 田富洋，吴洪涛，赵大旭，邵兵，王超群.一类柔性宏刚性微空间机器人广义动力学建模研究.宇航学报，2010，31（3）：687～694（Tian F Y，Wu H T，Zhao D X，Shao B，Wang C Q.Research on generalized efficient recursive dynamics of flexible macro-micro space robots system.Journal of Astronautics，2010，31（3）：687～694（in Chinese））

19 田富洋，吴洪涛，邵兵，赵大旭，王超群，朱剑英.柔性多体系统混合递推动力学建模及实时仿真研究.中国机械工程，2010，21（1）：6～12（Tian F Y，Wu H T，Shao B，Zhao D X，Wang C Q，Zhu JY.Hybrid dynamics of flexible multibody system and real time simulation.China Mechanical Engineering，2010，21（1）：6～12（inChinese））

20 Jain A，Rodriguez G.An analysis of the kinematics and dynamics of underactuated manipulators.IEEE Transactions on Robotics and Automation，1993，9（4）：411～422

21 Sohl GA，Bobrow JE.A recursive multibody dynamics and sensitivity algorithm for branched kinematic chains. Journal of Dynamic Systems.Measurement，and Control，2000，123（3）：391～399

22 Jain A，Rodriguez G.Recursive flexible multibody system dynamics using spatial operators.Journal of Guidance Control and Dynamics，1993，16（6）：345～358

Received 13 July 2015，revised 16 March 2016.

*The project supported by the National Natural Science Foundation of China（51205238）

†Corresponding author E-mail：sdautfy＠163.com

THE DYNAMIC MODELING AND SIMULATION FOR FLEXIBLE TREE ROBOTS SYSTEM W ITH ACTIVE－PASSIVE JOINT*

Tian Fuyang1†Cao Dong1Dong Xiaoning1Li Fade1Cai Zhanhe2
（1.College of Mechanical＆Electronic Engineering，Shandong Agriculture University，Shandong Provincial Key Laboratory of Horticultural Machineries and Equipments，Tai′an 271018，China）
（2.Product Inspection and Research Institute of Heze，Heze 274000，China）

The generalized high-efficient recursive dynamics model of a flexible tree robots system with activepassive joint was developed in this paper，where the tree robots system was described by the topology method and the lower body array.Meanwhile，the method to describe the under-actuated rigid-flexible multibody system based on Spatial Operator Algebra（SOA）theory was proposed.The hinges were defined as active or passive joints according to the driving conditions of the joint in the system.The dynamics model was then constructed.Subsequently，the generalized articulated inertia-matrix，the residual forces and the generalized acceleration and torque were computed through twice tip-to-base recursive and once base-to-tip recursive.The generalized torque and the generalized acceleration was linear added at the branch.At last，the generalized dynamics model of the flexible tree robots system was generated.The high-efficient O（N）th（N is degrees of freedom of robots system）computation efficiency was realized through the developed dynamics model in above recursive process.Simulation results show that the dynamic modeling and fast integration techniques proposed in this paper are correct and efficient.

spatial operators，tree，active-passive joint，generalized dynamics，high efficient

10.6052／1672-6553-2016-018

2015-07-13收到第1稿，2016-03-16收到修改稿.

*国家自然科学基金资助项目（51205238）

†通讯作者E-mail：sdautfy＠163.com