机器人学基础

1、机器人学基础运动学及动力学 43第1章 机器人学的数学基础21.1 机器人运动学的矩阵表示21.1.1 空间向量的表示21.1.2 坐标系在固定参考坐标系原点的表示21.1.3 刚体的表示31.2 变换的表示31.2.1 纯平移的变换41.2.2 绕轴纯旋转变换41.2.3 复合变换61.2.4 变换矩阵的逆7第2章 机器人的正运动学解82.1 位置的正运动学方程82.1.1 直角（台架坐标）82.1.2 圆柱坐标82.1.3 球坐标82.2 姿态的正运动学方程92.2.1 滚动角、俯仰角和偏航角92.2.2 欧拉角102.3 位姿的正运动学102.4 机器人正运动学方程的D-H表示102.5

2、 微分运动的正运动学分析132.5.1 坐标系的微分运动13第3章 机器人的逆运动学解163.1 位姿的逆运动学解163.1.1 欧拉变换的逆运动学解163.1.2 RPY变换的逆运动学解183.1.3 球坐标系变换的逆运动学解193.2 逆运动学方程解的步骤203.3 微分逆运动学26第4章 机器人动力学294.1 动力学问题概述294.2 动力学建模方法介绍304.2.1 Newton-Euler方法介绍304.2.2 拉格朗日法314.2.3 虚功原理法324.3 基于拉格朗日函数的机械臂的动力学建模324.3.1动力学建模概述324.3.2 建立机器人动力学方程推导过程的步骤334.3

3、.3 机械臂连杆质点速度的计算334.3.4 机械臂连杆质点动能计算354.3.5 机械臂连杆质点势能的计算364.3.6 机械臂的动力学方程374.3.7 二自由度机械臂动力学建模实例384.4 机器人的动态特性和静态特性404.4.1 机器人的动态特性414.4.2 结论414.5 机器人的静态特性424.5.1 静力和力矩表示方法424.5.2 不同坐标系间静负荷的变换424.6 总结42参考文献（References）43 机器人位置运动学运动学主要研究机器人的正逆运动学。当所有的关节变量已知时，可用正运动学来确定机器人末端手的位姿。如果要使机器人末端手放在特定的点上并且具有特定的姿态

4、，可用逆运动学来计算出每一关节的变量的值。首先利用矩阵建立物体、位置、姿态及运动的表示方法，然后研究直角坐标型、圆柱坐标型及球坐标型等不同构型机器人的正逆运动学，最后利用Dnavir-Hartenberg表示法来推导机器人的正逆运动学方程，这种方法适用于所有可能的机器人构型，而不管关节数量的多少、关节顺序的不同及关节轴之间是否存在偏移与扭曲等。机器人动力学分析 动力学方程明确的描述了机器人力和运动之间的关系。动力学包括正逆动力学。正动力学是在给定外力的情况下，计算力所引起的关节速度及关节加速度的变化。逆动力学是在已知某一时刻机器人各关节的位置、关节速度及关节加速度，求此时施加在机器人手的驱动力

5、或力矩。动力学研究的是物体的运动和受力之间的关系。包括机构的惯性计算、受力分析、动力平衡、动力学模型的建立、计算机动态仿真、动态参数识别、弹性动力分析等方面。 第1章 机器人学的数学基础1.1 机器人运动学的矩阵表示1.1.1 空间向量的表示对于图1.1空间中任一点p的位置可用列矢量P=axbycz来表示，其中ax，by，cz是点P在坐标系中的三个坐标分量。图 1.1 向量空间的表示1.1.2 坐标系在固定参考坐标系原点的表示坐标系通常由3个互相正交的轴来表示（例如x、y、z）。因为在任意给定的时间可能有多个坐标系，因此用x、y和z轴表示固定的全局参考坐标系Fx,y,z，用n，o和a轴表示相对

6、于参考坐标系的另一个运动坐标系Fn,o,a。如图1.2所示，位于参考坐标系Fx,y,z原点的坐标系Fn,o,a，用相对参考坐标系的3个方向余弦来表示。因此坐标系的3个轴就可以用矩阵形式的3个向量表示为F=nxoxaxnyoyaynzozaz。图1.2 坐标系在参考坐标系原点的表示1.1.3 刚体的表示一个物体在空间的表示可以这样实现：通过在它上面固连一个坐标系，再将该固连的坐标系在空间表示出来。由于这个坐标系一直固连在该物体上，所以该物体相对于坐标系的位姿是已知的。因此，只要这个坐标系可以在空间表示出来，那么这个物体相对于固定坐标系的位姿就是已知的。于是Fobject=nxoxaxpxnyoy

7、aypynzozazpz0001。在一个空间中一个刚体有6个自由度，它不仅可以沿着x、y和z这三个轴移动，而且还可以绕着这三个轴旋转。1.2 变换的表示变换有以下几种形式：纯平移绕一个轴的旋转平移与旋转的结合1.2.1 纯平移的变换 如果坐标系（也可能是一个物体）在空间以不变的姿态运动，那么该变换就是纯平移。在这种情况下，它的方向单位向量保持同一方向不变，所有改变的只是坐标系原点相对于参考坐标系的变化，如图1.3所示。相对于固定参考坐标系，新的坐标系的位置可以用原来坐标系的原点位置向量加上表示位移的向量来表示。若用矩阵形式，新的坐标系可以通过坐标系左乘变换矩阵得到。因为在纯平移中方向向量不改变

8、，变换矩阵T可以简单地表示为T=100dx010dy001dz0001。其中dx、dy和dz是纯平移向量d相对于参考坐标系x、y和z轴的3个分量。矩阵的前3列表示没有旋转矩阵（等同于单位阵），而最后1列表示平移运动。新的坐标系位置为Fnew=Trans(dx,dy,dz)×Fold首先可以看到，新坐标系位置可通过在原坐标系矩阵前面左乘变换矩阵得到。图1.3 空间纯平移变换的表示1.2.2 绕轴纯旋转变换假设坐标系Fn,o,a位于参考坐标系Fx,y,z的原点，在旋转前P点在两个坐标系中的坐标是相同的，当坐标系Fn,o,a绕Fx,y,z坐标系中的X轴旋转一个角度时，点P在Fn,o,a坐标

9、系中的坐标没有改变，但在Fx,y,z坐标系的坐标却发生了改变（见图1.4）.图1.4 旋转坐标系上的点的坐标在旋转前后的变化由图1.5可知，Px不随坐标系绕x轴的转动而变化，而Py和Pz却改变了，写成矩阵形式为PxPyPz=1000cos-sin0sincosPnPoPa，即则Pxyz=Rot（x，）×Pnoa。则绕x轴旋转的旋转矩阵为用同样的方法来分析绕y和z轴的旋转矩阵，则，图1.5 相对于参考坐标系的点的坐标和从x轴上观察的旋转坐标系1.2.3 复合变换复合变换可以分解为按一定顺序的一组平移变换和旋转变换。假定坐标系相对于参考坐标系依次进行下面3个变换：1、绕x轴旋转度；2、分

10、别沿x、y、z轴平移；3、最后绕y轴旋转度。注：矩阵相乘的顺序不能改变；相对于参考坐标系的每次变换，变换矩阵都是左乘的。假定所有的变换都是相对于当前的运动坐标系的：1、绕n轴旋转度；2、分别沿n、o、a轴平移；3、最后绕o轴旋转度。注：相对于运动坐标系或当前坐标系的轴的变化，需要右乘变换矩阵；复合变换是有一系列的平移变换和旋转变换所组成的。1.2.4 变换矩阵的逆计算矩阵逆的步骤：计算矩阵的行列式；将矩阵转置；将转置矩阵的每个元素用它的子行列式代替（称为伴随矩阵）；用上述经过转换的伴随矩阵除以行列式。因此，关于x轴的旋转矩阵的逆与它的转置矩阵相同。即 具有这种性质的矩阵称为酉矩阵，也就是说所有

11、的旋转矩阵都是酉矩阵。对于4×4的变换矩阵求逆可以分为两部分。矩阵的旋转部分仍是酉矩阵，只需简单转置；矩阵的位置部分是向量p分别于向量n、o、a的点积的负值，其结果为：，第2章 机器人的正运动学解2.1 位置的正运动学方程2.1.1 直角（台架坐标）直角坐标是机器人沿着x、y和z轴做线性运动。机器人手位置的正运动学变换矩阵为2.1.2 圆柱坐标 圆柱型坐标的运动顺序为：先沿x轴移动r，再绕z轴旋转角，最后沿z轴移动l，因此这三个所产生的总变换可以通过依次左乘每个矩阵而求得：2.1.3 球坐标球坐标的运动顺序为：先沿z轴平移r，再绕y轴旋转和绕z轴旋转。这三个变换所产生的总变换通过依次

12、左乘得到。2.2 姿态的正运动学方程在不改变机器人位置的情况下通过使其绕当前坐标系旋转可以改变机器人的姿态。考虑一下3种常见的构型配置：（a）滚动角、俯仰角、偏航角（Roll，Pitch，Yaw，RPY）（b）欧拉角（c）链式关节2.2.1 滚动角、俯仰角和偏航角RPY:如果当前运动坐标系不平行于参考坐标系，那么机器人手最终的姿态将是先前的姿态与RPY右乘的结果。RPY旋转的顺序包括以下几种：绕a轴（运动坐标系的z轴）旋转a，称为滚动；绕o轴（运动坐标系的y轴）旋转o，称为俯仰；绕n轴（运动坐标系的x轴）旋转n，称为偏航。2.2.2 欧拉角 表示欧拉角的转动如下：绕a轴（运动坐标系的z轴）旋转

13、；接着绕o轴（运动坐标系的y轴）旋转；最后再绕a轴（运动坐标系的z轴）旋转。表示欧拉角姿态变化的矩阵是该矩阵只是表示了由欧拉角所引起的姿态变化，相对参考坐标系，这个坐标系的最终姿态是由表示位置变化的矩阵和表示欧拉角的矩阵的乘积决定。2.3 位姿的正运动学机器人最终的位姿矩阵是前面矩阵的组合，该矩阵取决于所用的矩阵。假设机器人的运动是由直角坐标和RPY的组合关节组成的，那么机器人最终的位姿是两个矩阵的乘积，即=Tcart（Px，Py，Pz）×RPY（a，o，n）2.4 机器人正运动学方程的D-H表示机器人一般是由一系列的关节和连杆按任意的顺序连接而成。这些关节可能是滑动（线性）的或旋转

14、的，它们可能不在一个平面。为了对机器人进行建模和分析，我们必须对每一个关节建立相应的坐标系，将所有关节的变换矩阵结合起来就得到了机器人总的变换矩阵。为了用D-H表示法对机器人进行建模，首先要对每一个关节建立一个本地的参考坐标系。因此，对每一个关节都必须指定x轴和z轴，一般不需要指定y轴。指定本地参考系的步骤： 所有关节，无一例外地使用z轴表示。如果关节是旋转的，那么z轴位于按右手规则旋转的方向。如果关节是滑动的，那么z轴为沿直线运动的方向。对于旋转关节，绕z轴的旋转角是关节变量。对于滑动关节，沿z轴的长度d是关节变量。表示与之间的公垂线，则的方向将是沿的方向。两个关节之间的公垂线不一定相交或共

15、线，因此两个相邻坐标系原点的位置也可能不在同一个位置。在这种情况下就要考虑下面介绍的特殊情况，就可以为所有的关节定义坐标。 如果两个关节的z轴平行，它们之间就有无数条公垂线。这时可挑选与前一关节的公垂线共线的一条公垂线，这样可以简化模型。 如果两个相邻关节的z轴是相交的，那么它们就没有公垂线，这时可将垂直于两条轴线构成的平面的直线指定为x轴。图 2.1在图2.1中，表示绕z轴旋转的角度，d表示在z轴上两条公垂线之间的距离，a表示每条公垂线的长度，表示两个相邻的z轴之间的角度。根据D-H常规步骤，按照如下从一个坐标系到下一个坐标系所必须的4个变换。（1） 绕轴旋转，使和平行；（2） 沿轴平移，使

16、和共线；（3） 沿已经旋转过的轴平移的距离，使和原点重合；（4） 将轴绕轴旋转，使轴与对准。由于所有的变换都是相对当前坐标系进行的，因此所有的变换矩阵都是右乘的。在机器人的基座与手之间的总变换则为D-H参数表#0-11-22-33-44-52.5 微分运动的正运动学分析微分运动指机构的微小运动，如果在一个小的时间段内测量或计算这个运动就能得到速度关系。下式建立了机器人的关节微分运动和机器人手坐标系微分运动之间的联系：dxdydzxyz=机器人雅可比矩阵d1d2d3d4d5d6 或 D=JD式中，D中的dx、dy和dz分别表示机器人手沿x、y和z轴的微分运动，D中的x、y和z分别表示机器人手绕x

17、、y和z轴的微分旋转，D表示关节的微分运动。2.5.1 坐标系的微分运动坐标系的微分运动可以分为微分平移微分旋转微分变换（平移与旋转的组合）2.5.1.1 微分平移微分平移就是坐标系平移一个微分量，因此它可以用Trans（dx，dy，dz）来表示，其含义是坐标系沿x、y和z轴做了微小量的运动。2.5.1.2 绕参考轴的微分旋转微分旋转就是坐标系的微小旋转，绕x、y和z轴的微分转动分别定义为x、y和z。因此，绕x、y和z轴的微分旋转转矩为Rotx，x=100001-x00x100001，Roty，y=10y00100-y0100001，Rotz，z=1-z00z10000100001同样，也可以

18、定义绕当前轴的微分旋转为Rotn，n=100001-n00n100001，Roto，o=10o00100-o0100001，Rota，a=1-a00a100001000012.5.1.3 绕一般轴q的微分旋转对于微分旋转，乘法的顺序并不重要，因此绕任意一般轴q轴的微分运动可以表示为 Rotq，d= Rotx，x Roty，yRotz，z=100001-x00x10000110y00100-y01000011-z00z10000100001=1-zy0z1-x0-yx1000012.5.1.4 坐标系的微分变换坐标系的微分变换就是微分平移和以任意次序进行微分旋转的合成。假设用T表示原始的坐标系，

19、dT表示由于微分变换所引起的坐标系的变化量，则有T+dT=Transdx，dy，dzRotq，dT则，dT=T,其中=Transdx，dy，dzRotq，d-I称为微分算子，=Transdx，dy，dzRotq，d-I=100dx010dy001dz00011-zy0z1-x0-yx100001-1000010000100001=0-zydxz0-xdy-yx0dz0000经微分运动后的坐标系的新位姿可以通过将这个变化加到原来坐标系上求得：Tnew=dT+Told第3章 机器人的逆运动学解3.1 位姿的逆运动学解3.1.1 欧拉变换的逆运动学解欧拉变换为，用T来表示欧拉变换的结果，即其中比较上

20、述两个式子可得从而可求得一下结果： 这里需要指出的是，这会带来如下问题：1）由于绝对值相同的正负角度的余弦相等，如，因此不能确定反余弦的结果是在那个象限；2）当接近于0时，所求出的角度和是不精确的；3）当0或±180º时，会出现无数值解。为此，我们采用前节的方法，用左乘将上式写成如下形式根据（2,3）元素得根据（2,1）（2，2）元素得根据（1,3）（3,3）得至此，我们求出了欧拉变换的逆运动学解。3.1.2 RPY变换的逆运动学解由（2,1）元素得由（3,1）（1,1）元素得由（2,2）（2,3）元素得至此，我们求出了RPY的逆运动学解。3.1.3 球坐标系变换的逆运动学

21、解球坐标变换的表达式如下用Rot1(z,)左乘上式得到将上列矩阵方程的第4列元素写出有由上式第2行元素相等有可得到或第1行和第3行元素相等有为了获得平移量，我们用R左乘式得由上式第3行元素相等得到至此，我们求出了球坐标变换的逆运动学解。3.2 逆运动学方程解的步骤逆运动学方程求解通常是按以下步骤进行：（1） 根据机械手关节坐标设置确定 为关节坐标的齐次坐标变换，由关节变量和参数确定。关节变量和参数有：连杆长度; 连杆扭转角;相邻两连杆的距离; 相邻两连杆的夹角。对于旋转关节为关节变量，而对于滑动关节为关节变量。其余为连杆参数，由机械手的几何尺寸和组合形态决定。（2） 根据任务确定机械手的位姿

22、为机械手末端在直角坐标系（参考坐标或基坐标）中的位姿，由任务确定，即D-H得出的表达式确定。它是由三个平移分量构成的平移矢量P（确定空间位置）和三个旋转矢量n，o，a（确定姿态）组成的齐次变换矩阵描述。（3） 由和（n1，2，6）求出相应的关节变量 或。根据式分别用An（n1，2，5）的逆左乘式有 根据上述五个矩阵方程对应元素相等，可得到若干个可解的代数方程，便可求出关节变量或。 以图3.1的简单6自由度机器人为例，根据D-H表示法，建立该机器人所需的坐标系、填写相应的参数表及导出它的正运动学方程。图 3.1机器人的参数#0-10901-2002-3003-40-904-50905-600通过

23、参数表得到：为了简化最后的解，利用下列三角函数关系式：机器人基座和机械手之间的总变换为将上面矩阵表示为。将机器人的期望姿态表示为为了求解这些角度，选择左乘方程两边的矩阵，首先用左乘两边的矩阵得根据式（2.01）的（3,4）元素，有和根据（2.01）的（1,4）和（2,4）元素，可得整理两个表达式，两边平方，再相加得由于因此在该式中，除了和以外，每个变量都是已知的。和将在下面求出。已知于是可得由于关节2,3,4是平行的，左乘和得不到有用的结果。下一步左乘到，结果为 根据式（2.2）的（3,3）元素，可得由此可计算和。如前面讨论的，它们可以用来算现在，再次参考上述过程，并重复列出该式就可以计算出的

24、正弦和余弦，如下过程。因为可得将上式作为具有两个方程和两个未知量的联立方程处理，求解和可得可以计算得到既然和都已知，进而可得根据式（2.2）的（1,3）和（2,3）元素，可得因为没有解耦方程，所以用到左乘，从而得到(2.03)根据式（2.03）的（2.1）和（2.2）元素，可得综上所述，对于该六自由度的机器人解耦结果为：3.3 微分逆运动学期望的可表示为如下形式:有前面可知，和可以通过对该关系式进行微分计算求取类似的，可得到求的关系式如下：对上式求微分得：上式所有元素都是已知的，因此可以计算出。接下来通过对下式求微分求得。从上式可以计算出。因为式中其他元素都是已知的，通过、的关系求出。对下式求

25、微分，求：从上式可求得。最后对下式求微分计算 根据这六个微分方程可求得6个关节微分值，由此可计算出速度。注意：（1）在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，此时应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。（2）由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关，因此当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以逆运动方程的解不是唯一的，我们应该根据机械手的组合形态和各关节的运动范围，经过多次反覆计

26、算，从中选择一组合理解。第4章 机器人动力学4.1 动力学问题概述动力学研究的是物体的运动和受力之间的关系。包括机构的惯性计算、受力分析、动力平衡、动力学模型的建立、计算机动态仿真、动态参数识别、弹性动力分析等方面。机械臂是作为主动机械装置来操作的，原则上讲它的每个自由度都可以进行单独传动。从控制观点来看，机械臂是冗余的、多变量的和非线性的自动控制系统，也是个很复杂的动力学耦合系统。对于机械臂而言，每一个控制问题的解决当中都包含着动力学任务的完成。因此，研究机械臂的动力学问题是进一步讨论控制问题的基础。机械臂的动力学问题是研究关节的运动与作用在关节上的力和力矩的关系，它对各关节变量的协调运动与

27、作用在关节上的相互耦合的力与力矩的变化进行定量的描述。分析的结果是一组运动学方程，当给定关节上的力和力矩时，方程便规定了机械臂模型的运动；当给定运动规律时候，方程可确定实现这样的运动而必须施加的力或力矩。机械臂的动力学问题包含两个相反的方面：一方面是已知机械臂上各关节的作用力或力矩，求得它们的运动轨迹，也即是它们的位移、速度和加速度；另一方面是已知机械臂上各关节的运动轨迹，即它们的位移、速度和加速度，求它们的作用力或力矩。其中，在动力学上前者是正问题，后者是逆问题。一般来讲，为了获得控制所需要的信息，我们需要求得矩阵形式的动态方程。在解决控制问题时，我们往往还对具体控制提出假设，从而简化动态方

28、程。与机械臂的运动学不同的是，机械臂的动力学不仅与机械臂各关节的几何结构相关，还与质量有关。在进行机械臂的设计与应用时，建立该机械臂的动力学模型是十分重要的。一方面，动力学模型是进行动力学分析与综合的理论基础，是进行机械臂动力学性能评价及优化设计的必要前提；动力学的逆解模型是机械臂伺服电机选配和动力学参数辨识的理论基础；动力学正解模型是动力学仿真与控制器设计的理论基础。机械臂是个复杂的动力学系统，具有多个输入和多个输出，存在着复杂的耦合关系和非线性，如何从机械臂自身的特点出发，构造形式简洁而完备的动力学模型，提高动力学模型的计算效率，对于实施机械臂动力学实时控制和仿真具有很大的工程实践价值。4

29、.2 动力学建模方法介绍机械臂动力学建模的主要目的是研究机械臂关节所受驱动力与关节运动轨迹之间的关系，得出描述机械臂驱动力和运动轨迹的动力学方程，从而表达出机械臂的动力学特性。动力学建模是进行动力学分析与综合的基础，是驱动和控制系统设计的基础，是动力学性能评价和动力学优化设计和的必要条件，也是实时控制的前提。建立在关节空间和工作空间的机械臂动力学模型一般是一个多自由度、多变量、高度非线性、多参数稱合的复杂系统，数学模型的复杂性是由机械臂动力学内在的、本质的复杂性所决定。不同的建模方法所导出动力学模型的最终形式和计算繁简程度有较大差异。4.2.1 Newton-Euler方法介绍Newton-E

30、uler法是以理论力学的两个基本方程牛顿方程和欧拉方程为出发点，结合操作机的速度和加速度分析而得出的一种操作机动力学算法。它常以递推的形式出现，具有较高的计算速度，但形成最终的动力学完整方程却比较麻烦。它的特点之一是要计算关节之间的约束力，所以在用于含闭链的操作机动力学比较困难。但也正由于该算法可算出关节处的约束力，从而为操作机机构设计提供力分析的原始条件。牛顿动力学方程：其中、分别表示刚体质心加速度和速度。欧拉动力学方程：其中表示刚体绕定点转动时对该点的动力矩；分别表示组成刚体的质点的向径、质量和速度；表不刚体的惯量张量；应用动量矩定理： 其中表示外力对于定点的合力矩。Newton-Eule

31、r法采用递推算法，由基座前推，逐次求出各连杆的角速度角加速度和质心加速度，再由末连杆的末关节向第一关节后退，从而求出各关节力矩，其计算公式分为速度和惯性力前推、约束力和关节力矩后推两部分。总之，该法需从运动学出发求得加速度，并消去各内应力，对于较复杂的系统，此种方法十分复杂。4.2.2 拉格朗日法在拉格朗日法当中，拉格朗日方程是建立在能量平衡基础上的。拉格朗日函数一般要求选择以驱动铰链作为独立的广义坐标，并且还要将其它铰链以及末端执行器位置表示为广义坐标的函数。对于机械系统，其拉格朗日函数为系统动能和系统势能之差，即：式中：为拉格朗日函数；为动能；为势能；为动能与势能的广义坐标；动能与势能的广

32、义速度。从系统的动势能角度出发，拉格朗日法建立了机构的动力学方程。用拉格朗日方法建立的动力学模型能清楚表示关节之间的耦合关系。采用此法建模的关键是建立位形空间的参数化表达式。拉格朗日法建模特点主要有以下几点：能以最简单的形式推导非常复杂的系统动力学方程，且具有显式结构；状态方程简单便于设计补偿所有非线性因素的控制规律，可实现闭环控制；釆用这种具有显式结构，可用来分析和设计关节变量空间的高级控制策略；设计反馈控制器时，用动力学系数使反作用力的非线性影响最小；解决正、逆动力学问题时，均必须计算动力学参数，故在解决简单问题时，较之Newton-Euler法更繁琐，然而随着系统复杂程度的增加，采用此法

33、反而使得问题简单。4.2.3 虚功原理法 基于虚功原理的建模方法已经成功运用于机械臂的相关机构动力学建模，许多学者针对一些实例对这种方法进行验证分析，实验结果验证了这种方法建立的动力学算法的有效性。虚功原理又称为虚位移原理，其表达式为：式中为作用于质点系的主动力；为虚位移。虚功原理方程较为简洁，对于处理动力学的逆向问题效率较高，但在计算动力学响应时，同样无法直接得到约束力。由于操作空间和关节空间速度和加速度映射关系的解析表达式极易从雅可比矩阵和海赛矩阵直接导出，基于动力学普遍方程的虚功原理法被认为是机械臂特别是比较复杂的并联机构动力学建模的一种有效方法。4.3 基于拉格朗日函数的机械臂的动力学

34、建模4.3.1动力学建模概述建模的方法有很多种，除了上面我们介绍的几种常用建模方法，还有一些适用于其它特定工业对象的建模方法。拉格朗日函数法只需要速度而不需要求内作用力，因而它是一种直截了当和简便的方法。在这里选择此法来分析求解二关节机械臂的动力学问题。拉格朗日函数L被规定为系统的动能K和势能P之差，即其中，和可以用任意的坐标系来表示。在系统动力学方程的基础上我们得到的拉格朗日方程如下：式中，表示动能和势能的坐标，为他们坐标的相应速度，而是作用在第个坐标系上的力或是力矩。是力或力矩，是由为直线或角坐标决定的。这些力、力矩、广义力矩和广义坐标，为关节数目。4.3.2 建立机器人动力学方程推导过程

35、的步骤（1） 计算任一关节上任一点的速度；（2） 计算各关节的动能和机械臂的总动能；（3） 计算各关节的势能和机械臂的总势能；（4） 建立机械臂动力系统的拉格朗日函数；（5） 对拉格朗日函数求导，求得到动力学方程式。4.3.3 机械臂连杆质点速度的计算图1中关节2上点B的位置为。式中，为基坐标系的位置矢量；为局部坐标系的位置矢量；为变换矩阵,包括旋转和平移变换。图1因此点B的速度为: B点的加速度:）B点速度的平方： （4.1）式中，Trace表示矩阵的迹。对于一个n阶矩阵来说，其迹即为它的主对角线上个元素之和。同理，对于任一连杆i上的一点，其位置为： （4.2）对于关节i任一点速度为：（4.

36、3）对于旋转关节，广义坐标系为关节转角，因而得导数为： （4.4）对于滑动关节，广义坐标为滑动关节位移，因而的导数为： （4.5）在式（4.4）和（4.5）中为常数矩阵，因此有对于转动关节，对于滑动关节 （4.6）依据式（4.4）、（4.5）和（4.6），给出如下定义： （4.7）使用上述符号代替式（4.3）中相应部分可得： （4.8）对于任何关节上一点的速度平方为： （4.9）依据式（4.4）、（4.5）和（4.6），还可以给出如下定义： （4.10）4.3.4 机械臂连杆质点动能计算在获得连杆上各点的速度后，就可以计算连杆的动能。假设是连杆相对于基坐标系表示的动能，而是连杆上微元质量的动能

37、，则 （4.11）所以连杆的动能可以通过对方程（4.11）的积分获得 （4.12）式中，积分称为机械臂的伪惯量矩阵。并记为： （4.13）具有个连杆的机械臂动力系统的总动能为： （4.14）此外，关节的传动装置动能为： （4.15）式中，为连杆传动装置动能；为传动装置的等效转动惯量或等效质量；为关节的速度。式（4.14）中忽略了连杆传动装置的动能，若计入各连杆传动装置的动能，机械臂动力系统总动能则为： （4.16）4.3.5 机械臂连杆质点势能的计算机械臂各连杆的势能为，则： （4.17）式中，为坐标系下重力的各分量；为连杆的质心在连杆为基准的坐标系下的位置。由式（4.17）可计算机械臂的总势

38、能： （4.18）传动装置因重力而产生的势能一般很小，可以忽略不计。4.3.6 机械臂的动力学方程依据式（4.16）和（4.18）可以建立机械臂的拉格朗日函数： （4.19）对拉格朗日函数求导得： 整理得机械臂动力学方程为： （4.20）式中。可以将式（4.20）化简为矩阵符号形式 （4.21）其中,在式（4.21）中，第一部分死角加速度惯量项，第二部分是驱动器惯量项，第三部分是科里奥利力和向心力项，最后一项是重力项。惯量项和重力项对于机械臂动力系统的稳定性和定位精度至关重要。而向心力和科里奥利力在机械臂低速运动时可以忽略，但在机械臂高速运动时其作用非常重要。4.3.7 二自由度机械臂动力学建模实例下面以一个二自由度机械臂为例说明如何利用前面推倒得到的公式建立机械臂的动力学方程，机械臂的相关参数如图1所示。依据图1，机械臂关节和连杆的参数为：，所以有：由于都是转动关节，所以取 依据式（4.7）有, ,机械臂的伪惯量矩阵分别为： 依据式（3.32）各连杆动力学方程为：其中对于，连杆的重力为：把二自由度机械臂动力学方程写成矩阵形式为：4.4 机器人的动态特性和静态特性机器人动力学问题的研究目的在于控制和保证机器人保持优良的动态特性和静态特性。4.4.1 机器人的动态特性（1）