机器人学第七章(机器人动力学的凯恩方法)

1、第七章 机器人动力学的凯恩方法7.1 引言机器人动力学凯恩方程方法是建立在凯恩动力学方程基础上的，因而本章首先介绍凯恩动力学方程。设一质点系具有n个质点，该质点系的动力学普遍方程为 （7-1）式中 作用于第i质点主动力矢量；质点i的质量；质点i的加速度矢量；质点i在参考坐标系中的位置矢量；质点i的微分位移；“·”数量积符号。设质点系为完全系，即它具有l个自由度和l个广义坐标，则 （7-2）式中 广义坐标；t时间变量；质点i的线速度为式中 （7-3）凯恩（kane）定义为质点I相对于广义速度的偏速度。微分可表示为（7-4）将（7-4）代入（7-1）式，得交换求和符号，得因为是独立变量，

2、故 j=1,2,，l （7-5）或 这就是质点系的凯恩动力学方程(Kane Dynamics Equation),可以改写为 （7-6）图7-1 刚体中点i的速度如图7-1所示将刚体看成是由n个质点组成的。设刚体的质心为C，以C为力的简化中心并设作用于刚体的主动力的合力为，合力矩为： （7-7） （7-8）当刚体以角速度旋转时，其中点i的速度为其中 点到质心C的位置矢量； 质心C的线速度。Z点对广义速度的偏速度为或 （7-9）式中质心C相对于的偏速度： （7-10）刚体相对于的偏角速度： （7-11）于是作用在刚体上相对于的广义力为或 （7-12）相对于的广义惯性力为而 式中动量矩用刚体的惯性

3、张量表示为 （7-13）因此 （7-14）得广义惯性力表示为 （7-15）将（7-12）和（7-15）式合并，从而得到刚体的凯恩动力学方程为 （7-16）式中 I刚体相对于质心C的惯性张量。7.2 机器人杆件速度、加速度及偏速度的递推计算公式图7-2 n自由度机器人的杆件坐标系x1x0x2xnznena2z2e2z0q1=e0z1e1d1C1a1C2如图9-2所示，杆件坐标系均设置在各杆件上编号关节处,个自由度的机器人有个关节。图中指定杆件坐标系各轴方向的单位矢量，共有i=0，1，2，n个，它们均是以杆件坐标系描述的常矢量，；以杆件坐标系i-1的原点为始点到以i系原点为终点的矢量，但它是以i系

4、描述的矢量； 以杆件坐标系i描述的第i号杆件质心的位置矢量；两相邻坐标系i-1及i中速度、加速度等的关系可用变换矩阵中的旋转子矩阵及相联系。仿照第六章处理杆件坐标系及杆件质心的速度及加速度的方法，并考虑到坐标系设置方法上的区别，不难得到如下所述的速度及加速度递推计算公式： （7-17）式中 广义坐标对时间的1阶导数，即关节轴的数量速度； 广义坐标对时间的2阶导数，即关节轴的数量加速度；关节类型识别符号； （7-18）与第六章相同，令 （7-19）式中 g重力加速度。上式是假定绝对参考系的轴垂直于地面且指向向上的。若轴垂直于地面，则其中负号表示轴指向地心（于重力场同方向）。偏速度的递推公式为：

5、（7-20）例7-1 如图7-3所示的平面包2自由度机器人，、为已知，试用（7-17）及（7-20）式计算各杆的速度、加速度及偏速度。杆件的质心均在杆件的末端。解：， ， 图7-3 平面2自由度机器人，。式中 g重力加速度。轴与重力场反向，故g取正。i=1时：i=2时：， ， （同上式）此例所得各质心的速度及加速度的计算结果与上一章的计算方法得到的结果是完全相同（例7-2将用此例的偏速度）。7.3 关节驱动力或力矩的求解以下分末端执行器有无负载的两种情形进行讨论。1-no1-nxnpnfnnnCnenonx1=ne1=nZnZ图7-4 末端杆件广义力j计算简图设末端杆件为旋转关节，相对于的广义

6、力计算简图如图7-4所示。图中作用在末端杆件坐标系中的主动力;在末端杆件坐标系中的主动力矩;、坐标系n及n-1中z轴的单位矢量，且应用凯恩方程（7-16）： j=1，2，n有 （7-21）式中 杆件n的质量，集中于质心；杆件n相对于质心的惯性张量。利用偏速度的公式（7-20），上式的左端为得 （关节n的力矩）考虑（7-21）式的右端，所以 （7-22）为了导出第n-1号杆质心的广义公式，需要第n号杆到n-1号杆讲的受力图，2-no2-nxnp1-nf2-nn1-nCncnonx1-nZ2-nZnZ图7-5 第n-1号杆件广义力的计算简图1-no1-nRnnnfR1-1-nxnfnnnnR1-n

7、Rnn如图7-5所示。图中在n-1坐标系中表示的第n-1号杆件的主动力；在n-1坐标系中表示的第n-1号杆件的主动力矩；在n-1坐标系中表示的第n号杆件的主动力的反作用力；在n-1坐标系中表示的第n号杆件的主动力矩的反作用力矩；其余符号与图7-3相同。相对于的广义力为 （7-23）将各偏速度代入（7-23）式(1)(9)各项。利用三矢积(混合积)的性质作变换（参见（7-21）式的变换过程），可以发现：总计此式的8项之和时，(1)+(2)+(3)+(4)+(5)+(6)+(7)+(8)=0，因此（关节n-1的力矩）所以 （7-24）仿上述推导过程，可得旋转关节力矩的一般公式为 （7-25）当在第

8、i号杆件上作用有系统的外载荷时，应向质心简化，求出合力和合力矩，于是 （7-26）若n-1号关节为移动关节，则公式（7-23）式的9项之和为0，所余为第一项，即因此 （关节n-1的力） （7-27）仿前有 （7-28）公式（7-26）及（7-26）就是关节力矩（j为转动关节）及格关节力（j为移动关节）的计算公式例7-2 设如图8-3所示机器人两杆件的质量分别为，试用凯恩动力学方法求解两个关节的力矩。解：引用例7-1所计算的速度，加速度及偏速度，并考虑到，则由此得关节1、2的驱动力矩为此结果与牛顿欧拉算法的、是一致的。若将、中的、分解出来，写成矩阵形式，则得式中的M就是拉格朗日动力学方法的广义质

9、量矩阵。7.4 含有闭链机构机器人的动力学计算方法当机器人含有闭链机构时，如用牛顿欧拉算法或拉格朗日算法，通常要将闭链在某处切开，使其变成几个开链，然后求出在切开处的约束力及约束力矩，再按开链情况分别处理，但用凯恩动力学算法，则不再需要求出切开处的约束力及约束力矩，因而比较方便。设含有闭链机构的机器人具有k个从动关节，n个主动关节。从动关节变量是主动关节变量的函数：； （7-29）式中从动关节变量，i=1，2，k，共k个；主动关节变量，i=，,2，k，共n个。（7-29）式给出了机构的几何约束及运动约束条件，找出这些约束之后，再将闭链分成若干支路，每一路都按开链的凯恩动力学方法计算，求出所有杆

10、件的速度及偏速度(i=1，2，n+k；j=1，2，n)。如果关节i是主动关节，则可以用本章第二节的方法求速度及偏速度。如果关节i是从动关节，偏线速度及偏角速度可用以下方法计算： （7-30）式中应根据已求出的，共k个关系式求得，其他各项仍按本章第二节相应公式，最后求出各从动关节力或力矩。例如图7-6所示的机器人，主动关节变量为，其中闭链的平面平行连杆机构中，从动关节变量为，利用此机构的几何约束：及运动约束：q 2q4¢q4q 5图7-6 具有闭链机构的机器人q 1q 3q2¢q3¢可求出其偏速度（用7-30式）及速度。然后将闭链分成如图7-7所示的两路等效开链，就

11、可用凯恩方法计算出，最后求得主动关节力或力矩。图7-7 闭链的等效开链7.5 机器人动力学自动建模软件系统机器人实时控制和实时仿真迫切需要精确、高效的动力学模型。但由于机器人动力学模型方程的非线性性和强耦合性，其建模过程十分复杂和困难。从计算机软件设计的观点看来，机器人动力学模型算法可分为三类：数字法、符号法以及数字符号法。在数字法中，所有的变量都表示为实数，每个变量占据一个内存，这种方法计算量很大，难以在实时控制和实时仿真中应用；在符号法中，所有的变量均表示为符号，这种方法需要先进的计算机及复杂的计算机软件的支持，并需要较大内存；数字符号法将部分变量处理为实数，将部分变量处理为符号，取得了比

12、较好的效果。Vukobratovic对数字符号模型法作出了重要贡献，他提出了面向计算机的数字符号模型算法4,5。在应用数字符号法进行动力学模型研究方面已作出了不少杰出的研究610。封闭形式的机器人开链系统（见图7-2）的动力学方程的一般形式可表示为 （7-31）式中：式中，为驱动器广义驱动力向量；为惯性矩阵；为离心力和哥氏力效应矩阵；为重力效应矩阵；为机器人的广义坐标、广义速度、广义加速度向量。一、惯性矩阵现设机器人各构件的广义速度均为零，无重力作用，并设构件i的广义加速度为一个单位值，其余各构件的广义加速度均为零。即 (7-32)式中，表示非构件i的广义加速度，即除构件i以外其它构件的广义加

13、速度。将上式代入式(7-31)，则有惯性矩阵H中的第i列等于等于驱动力矢量，即 (7-33)式中，表示惯性矩阵的第i列。为了求得驱动器的广义驱动力向量，可将式（7-33）代入一组动力学递推公式，动力学递推公式既可采用基于牛顿-欧拉方程的动力学递推公式又可采用基于凯恩方程的动力学递推公式。 (7-34)式中，为构件i的角速度向量；为构件i的角加速度向量；为构件i坐标原点的加速度向量；为构件i质心加速度向量；为构件i-1作用在构件i上的关节反力矩；为构件i-1作用在构件i上的关节反力；为构件i质心所受惯性力矩；为构件i质心所受惯性力；为第i坐标系到i+1坐标系的变换矩阵；为关节类型识别符号。在这里

14、和以下，利用递推公式（7-34）求驱动器的广义驱动力向量时，广义坐标被处理为符号量，因而得到的模型矩阵各元素均为广义坐标的函数。广义坐标是由递推公式中的变换矩阵中引入的，为了便于输出模型矩阵各元素的实时代码，引入符号 （7-35）例如：，则变换矩阵可改写为式（7-32）中，将i的值从1变化到n，重复上述过程，可求得惯性矩阵H的所有n列元素。二、离心力效应矩阵现设机器人各构件的广义加速度均为零，无重力作用，并设构件i的广义速度为一个单位值，其余各构件的广义速度均为零。即 (7-36)将上式代入式(7-31)，则有 (7-37)式中，表示矩阵C中所有的第i行、第i列元素组成的列向量。 将i的值从1

15、变化到n，重复上述过程，可求得矩阵C中的所有对角元素，即对应于离心力效应的所有元素。三、哥氏力效应矩阵现设机器人各构件的广义加速度均为零，无重力作用，并设构件i和构件j的广义速度为一个单位值，其余各构件的广义速度均为零。即 (7-38)将上式代入式(7-31)，则有 (7-39)由于，于是有 (7-40) 上式中，已由式（7-37）求得。改变i和j的值，；重复上述过程，可求得矩阵C中的所有的，即对应于哥氏力效应的所有元素。四、重力效应矩阵设机器人各构件的广义速度和广义加速度均为零，并设基础以重力加速度g向上加速，即 (7-41)此时对应于基础坐标系Z0轴垂直向上的情况。将式（7-41）代入式（

16、7-31），可得 (7-42)于是可得重力效应矩阵。五、雅可比矩阵当机器人手部末端夹持器有外力作用时，机器人动力学方程变为： （7-43）式中：，分别为作用于机器人末端夹持器的外力、外力矩向量；J为雅可比矩阵。由于 （7-44）可设机器人中构件i的广义速度为一个单位值，其余各构件的广义速度为零。即 （7-45）将上式代入式（7-44），则有 （7-46）为了求得机器人手部末端夹持器的线速度和角速度，可将式（7-45）代入一组运动学递推公式 （7-47）利用递推公式（7-47）求末端夹持器的线速度和角速度时，广义坐标被处理为符号量，因此得到的雅可比矩阵各元素均为广义坐标的函数。式（7-45）中，

17、将i的值从1变化到n，重复上述过程，可得雅可比矩阵的所有n列元素。将机器人所有运动学和动力学参数处理为数字量，将广义坐标处理为符号量，由运动学和动力学递推公式不难得出：任一运动学、动力学变量可表示为 （7-48）式中 项数系数对应于正弦、余弦、广义坐标函数的整数幂指数 为了便于对多项式进行数字符号混合运算，引入幂指数矩阵 (7-49) 于是式(7-48)可表示为 (7-50) 可定义其如下的基本乘加运算法则由上述的数值符号处理方法，可以得到数值符号表示的运动学、动力学变量，进而可得到数值符号表示的动力学模型矩阵的各个矩阵元素。定义如下的结构Struct Model_elementdouble

18、u;int m3n;Struct Model_element *Link;由此结构可构成一首尾相连的线性链表，通过指针Link可将一个个结构串联起来，链表的终结标志是将最后一个结构的Link指针指向NULL(空指针)。由此可形成链表长度可以动态变化的动态数据结构，运动学和动力学变量都可以用上述动态数据结构表示和存储。在链表中增加、删除链表元素可以通过修改指针方便实现。动力学模型方程式(7-31)中的模型矩阵有如下性质4(1) H矩阵对称正定，即：(2) C矩阵 a.对称性， 即： b.反对称性，即： 当i，jk 当i=jk利用上述性质能较大地减少所需计算矩阵元素的数目。所需计算H矩阵元素个数减

19、少，所需计算C矩阵元素个数减少。通过上述的转化，可以将复杂的数字-符号混合处理过程转化为简单的矩阵运算过程，降低了建模过程的复杂性，容易通过计算机程序实现自动建模，并可同时进行代码优化。基于前述的方法，我们开发了机器人动力学自动建模软件系统DMAGS。该软件系统采用C语言编程，并采用了面向对象的程序设计技术，可通过人机对话方式输入机器人运动学和动力学参数，该软件系统可按照Fortran语言、Pascal语言、C语言三种格式输出动力学模型矩阵各元素的实时代码。实时代码的输出需要遍历整个线性链表，将一个个结构串联表示的多项式转换为对应的实时代码形式输出。软件系统的算法流程框图如图7-8所示。一3自

20、由度机器人运动学参数和动力学参数如下：m i经我们编制的机器人动力学模型程序DMRNS运行后，自动生成模型中各矩阵元素，并以实时代码的形式输出，其结果如下：GG1 = -147.000Y1Y2Y2+ 58.800X1X1Y2+ -88.200X2X2Y1+ -58.800X1X2X3X3Y2+-58.800X2X2X3X3Y1+ -58.800X2X2Y1Y3Y3+ -58.800X1X2Y2Y3Y3+ -19.600Y1Y2Y3+ 19.600X1X2Y3+ 137.200X1X2+ -137.200Y1Y2+ -78.400Y1GG2 = 19.600X2X3Y1+ 19.600X1X3Y

21、2GG3 = 19.600X1X3Y2+ 19.600X2X3Y1HH1,1 = -45.00Y2Y2+ -27.00X2X2+ -18.000X2X2X3X3+ -18.000X2X2Y3Y3+ -67.000+ -2.000X3X3+ -4.000Y3Y3+ -16.000Y3+ -12.000Y2Y3+ -84.000Y2HH2,1 = 6.000X2X3HH3,1 = 6.000X2X3HH2,2 = -32.000X3X3+ -32.000Y3Y3+ -16.000Y3+ -19.000HH3,2 = -8.000Y3+ -4.000HH3,3 = -4.000CC1,1,1 = 0

22、.000CC2,1,1 = -6.000X3Y2+ -8.000X3+ -2.000X3Y3CC3,1,1 = -2.000X3Y3+ -8.000X3+ -6.000X3Y2CC1,2,2 = 18.000X2+ 24.000X2X3X3+ 24.000X2Y3Y3+ 6.000X2Y3CC2,2,2 = 0.000CC3,2,2 = -8.000X3CC1,3,3 = 6.000X2Y3CC2,3,3 = 8.000X3CC3,3,3 = 0.000CC1,2,1 = 6.000X3Y2+ 8.000X3+ 2.000X3Y3CC2,2,1 = 0.000CC3,2,1 = 0.000CC

23、1,3,1 = 6.000X3Y2+ 8.000X3+ 2.000X3Y3CC2,3,1 = 0.000CC3,3,1 = 0.000CC1,3,2 = 6.000X2Y3CC2,3,2 = 8.000X3CC3,3,2 = 0.000结论文中从采用递推的计算结构，减少中间变换及动力学解耦的思想出发，提出了用数字-符号法建立机器人动力学模型的新方法。该方法可适用于任意自由度带有移动或转动关节的机器人动力学建模，该方法对硬件要求低，也不需要复杂的符号处理软件支持。由于建模和代码优化过程是离线进行的，因而大大减少了在线计算量，提高了计算效率。该方法在机器人的实时控制和实时仿真中有着广阔的应用前景。

24、本章参考文献1.2. 范守文,徐礼钜.机器人运动学的数字符号法研究.吉林工业大学学报，1992.8，P5-83. 范守文,徐礼钜.机器人动力学自动建模软件系统.电子科技大学学报,1995.24(6):249-2544. Vukobratovic M, Kircanski N. Real-time dynamics of manipulation robots. New York: Spinger-Verlag, 19845. Vukobratovic M, Kircanski N. Computer aided procedure of forming of robot motion equations in analytical forms. In: Proceedings of the Sixth World Congress on the Theory of Machines and Mechanisms, New Delhi, India, 1984: 9659696. 陈永，李立，刘朝晖. 带闭链机器人动力学建模的符号数值技术. 高技术