1、1&第六章第六章 机器人静力学和动力学机器人静力学和动力学 静力学和动力学分析，是机器人操作机设计和动态性能分静力学和动力学分析，是机器人操作机设计和动态性能分析的基础。特别是动力学分析，它还是机器人控制器设计、析的基础。特别是动力学分析，它还是机器人控制器设计、动态仿真的基础。动态仿真的基础。机器人静力学研究机器人静止或缓慢运动式，作用在机器机器人静力学研究机器人静止或缓慢运动式，作用在机器人上的力和力矩问题。特别是当手端与环境接触时，各关节人上的力和力矩问题。特别是当手端与环境接触时，各关节力（矩）与接触力的关系。力（矩）与接触力的关系。机器人动力学研究机器人运动与关节驱动力（矩）间的动机
2、器人动力学研究机器人运动与关节驱动力（矩）间的动态关系。描述这种动态关系的微分方程称为动力学模型。由态关系。描述这种动态关系的微分方程称为动力学模型。由于机器人结构的复杂性，其动力学模型也常常很复杂，难以于机器人结构的复杂性，其动力学模型也常常很复杂，难以用于机器人实时控制。然而高质量的控制应当基于被控对象用于机器人实时控制。然而高质量的控制应当基于被控对象的动态特性，因此，如何合理简化机器人动力学模型，使其的动态特性，因此，如何合理简化机器人动力学模型，使其适合于实时控制的要求，是机器人动力学研究追求的目标。适合于实时控制的要求，是机器人动力学研究追求的目标。26.1 6.1 机器人静力学机
3、器人静力学 一、杆件之间的静力传递一、杆件之间的静力传递1iF1iM 1ix1iy 在操作机中，任取两连杆在操作机中，任取两连杆 ，。设在杆。设在杆 上的上的 点点作用有力矩作用有力矩 和力和力 ；在杆；在杆 上作用有自重力上作用有自重力 过质过质心心 ）；）；和和 分别为由分别为由 到到 和和 的向径。的向径。iL1iL1iL1iO1iM 1iFiLiGiCir Cir iO1iOiC1izixiyiOiiGm g irCir1iO1iLiz3 按静力学方法，把这些力、力矩简化到按静力学方法，把这些力、力矩简化到 的固联坐标系的固联坐标系 ，可得：，可得：iLiiiiox y z111iii
4、iiiiCiiFFGMMrFrG 1011011011110iiiiiiiiiiiiiiiiiiiiiiCiiFRFR GMRMrRFrR G 或或式中式中 (为杆为杆 的质量的质量)。0iiGm g imiL 求出求出 和和 在在 轴上的分量，就得到了关节力和扭矩，轴上的分量，就得到了关节力和扭矩，它们就是在忽略摩擦之后，驱动器为使操作机保持静力平衡它们就是在忽略摩擦之后，驱动器为使操作机保持静力平衡所应提供的关节力或关节力矩，记作所应提供的关节力或关节力矩，记作 ，其大小为，其大小为iFiM iziiiik F iik M 41111110iiiiiiiiiiiiiiiFRFMrRRM 当
5、忽略杆件自重时当忽略杆件自重时 ，上式可简记为，上式可简记为：iG5二、机器人动力学研究的问题可分为两类：二、机器人动力学研究的问题可分为两类：1、给定机器人的驱动力（矩），用动力学方程求解机器、给定机器人的驱动力（矩），用动力学方程求解机器 人（关节）的运动参数或动力学效应（即已知人（关节）的运动参数或动力学效应（即已知 ,求求 和和 ，称为动力学正问题）。，称为动力学正问题）。2、给定机器人的运动要求，求应加于机器人上的驱动力、给定机器人的运动要求，求应加于机器人上的驱动力（矩）（即已知（矩）（即已知 和和 ，求，求 ,称为动力学逆问题称为动力学逆问题）。）。一、研究目的：一、研究目的：1
6、、合理地确定各驱动单元（以下称关节）的电机功率。、合理地确定各驱动单元（以下称关节）的电机功率。2、解决对伺服驱动系统的控制问题（力控制）、解决对伺服驱动系统的控制问题（力控制）在机器人处于不同位置图形（位形）时，各关节的有在机器人处于不同位置图形（位形）时，各关节的有效惯量及耦合量都会发生变化（效惯量及耦合量都会发生变化（时变的时变的），因此，加于各），因此，加于各关节的驱动力也应是时变的，可由动力学方程给以确定。关节的驱动力也应是时变的，可由动力学方程给以确定。6-2 机器人动力学概述机器人动力学概述,6三、动力学研究方法：三、动力学研究方法：1拉格朗日方程法：拉格朗日方程法：通过通过动、
7、势能变化与广义力的关系动、势能变化与广义力的关系，建，建立机器人的动力学方程。代表人物立机器人的动力学方程。代表人物 R.P.Paul、J.J.Uicker、J.M.Hollerbach等。计算量等。计算量O(n4)，经优化，经优化O(n3)，递推，递推O(n)。2牛顿牛顿欧拉方程法：欧拉方程法：用用构件质心的平动和相对质心的转动构件质心的平动和相对质心的转动表示机器人构件的运动，利用动静法建立基于牛顿表示机器人构件的运动，利用动静法建立基于牛顿欧拉方程欧拉方程的动力学方程。代表人物的动力学方程。代表人物Orin,Luh(陆养生陆养生)等。计算量等。计算量O(n)。3高斯原理法高斯原理法:利用
8、力学中的高斯最小约束原理利用力学中的高斯最小约束原理,把机器人动把机器人动力学问题化成极值问题求解力学问题化成极值问题求解.代表人物波波夫代表人物波波夫(苏苏).用以解决第用以解决第二类问题。计算量二类问题。计算量O(n3)。4凯恩方程法：凯恩方程法：引入引入偏速度偏速度概念，应用矢量分析建立动力学概念，应用矢量分析建立动力学方程。该方法在求构件的速度、加速度及关节驱动力时，只进方程。该方法在求构件的速度、加速度及关节驱动力时，只进行一次由基础到末杆的推导，即可求出关节驱动力，其间不必行一次由基础到末杆的推导，即可求出关节驱动力，其间不必求关节的约束力，具有完整的结构，也求关节的约束力，具有完
9、整的结构，也适用于闭链机器人适用于闭链机器人。计。计算量算量O(n!)。7v 系统的动能和势能可在任何形式的坐标系（极坐标系、系统的动能和势能可在任何形式的坐标系（极坐标系、圆柱坐标系等）中表示圆柱坐标系等）中表示，不是一定在直角坐标系中，不是一定在直角坐标系中。动力学方程为：动力学方程为：广义力广义力 广义速度广义速度 广义坐标广义坐标 （力或力矩）（力或力矩）（或或 ）（或或 ）iiidLLdtqqvd6.3 二杆机器人的拉格朗日方程二杆机器人的拉格朗日方程应用质点系的应用质点系的拉格朗日拉格朗日方程来处理杆系的问题。方程来处理杆系的问题。定义：定义：L=K-P LLagrange函数；函
10、数；K系统动能之和；系统动能之和；P系统势能之和。系统势能之和。6.3.1 刚体系统拉格朗日方程刚体系统拉格朗日方程8一、动能和势能一、动能和势能 设二杆机器人臂杆长度分别为设二杆机器人臂杆长度分别为 ，质量，质量分别集中在端点为分别集中在端点为 ，坐标系选取如图。，坐标系选取如图。2,1dd2,1mm以下分别计算方程中各项：以下分别计算方程中各项：221mvK 对质点对质点 ：1m222111111111111()222km vmdm d势能：势能：动能：动能：1111cos()pm gd v（负号与坐标系建立有关）（负号与坐标系建立有关）对质点对质点 ：2m 先写出直角坐标表达式：先写出直
11、角坐标表达式：)cos()cos()sin()sin(212112212112ddyddx6.3.2 机器人拉格朗日方程机器人拉格朗日方程11Pmgh9对对 求导得速度分量：求导得速度分量：x)2121)(2cos(212)2221221(222121222222)21)(21sin(21)1sin(12)21)(21cos(21)1cos(12ddddyxvddyddx动能：动能：)(cos()2(212121212212222121222212122ddmdmdmK)21cos(22)1cos(122gdmgdmP势能：势能：二、二、LagrangeLagrange函数函数 1212()(
12、)LKPkkpp22222212112211 22212211 211()(2)cos()()22mm dm dm d d 12112212()()cos()mmg dsm gd),(2121L10三、动力学方程三、动力学方程 先求第一个关节上的力矩先求第一个关节上的力矩 222121221222221222121211)cos()cos(2)(ddmddmdmdmdmmL222122221221222221211)cos()cos(2)(ddmdmddmdmdmmLdtd222212212212)sin()sin(2ddmddm)sin()sin()(212211211gdmgdmmL222
13、12122212212221222()2cos()cos()mm dm dm d dm dm d d221221221222121122122sin()sin()()sin()sin()m d dm d dmmg dm g d （1）11111()()dLLdLLdtqqdt111同理，对同理，对 和和 微分，可求得第二关节力矩微分，可求得第二关节力矩 2212212222212222)cos(ddmdmdmL21221212212222212222)sin()cos(ddmddmdmdmLdtd)sin()(sin(2122212122122gdmddmL222)(LLdtd)sin()si
14、n()cos(2122212212222212212222gdmddmdmddmdm 以上是两杆机器人动力学模型。以上是两杆机器人动力学模型。（2）12系数系数 D 的物理意义：的物理意义：关节关节 的有效惯量（等效转动惯量的概念）。由关节的有效惯量（等效转动惯量的概念）。由关节 处的加速度处的加速度 引起的关节引起的关节 处的力矩为处的力矩为 （）iiDii iiiiD JMii 关节关节 和和 之间的耦合惯量之间的耦合惯量。由关节由关节 或或 的加速度的加速度 （或或 ）所引起的关节）所引起的关节 和和 处的力矩为处的力矩为 或或 ijDijji j jiiijD jij i 向心力项系数
15、。表示关节向心力项系数。表示关节 处的速度作用在关节处的速度作用在关节 处的处的 向心力（向心力（）ijjDji2jijjD向心力项系数。表示关节向心力项系数。表示关节 处的速度作用在本身关节处处的速度作用在本身关节处 的向心力（的向心力（）iiiDi2iiiiD四、动力学方程中各系数的物理意义四、动力学方程中各系数的物理意义 将前面结果重新写成简单的形式将前面结果重新写成简单的形式：22111 1122111 112221122 1DDDDDD 22221 1222211 12222212 1 2221 2 12DDDDDDD 13哥氏力项系数。哥氏力项系数。两项组合为关节两项组合为关节 与
16、与 处的速度作用在关节处的速度作用在关节 处的哥氏力，哥氏力是由于处的哥氏力，哥氏力是由于 牵连运动是转动造成的。牵连运动是转动造成的。ijkDjkijkkjijkDDjki关节关节 处的重力项处的重力项。重力项只与重力项只与 大小、长度大小、长度 以以 及机构的结构图形（及机构的结构图形（）有关。）有关。iDimd21,比较二杆机器人例中的系数与一般表达式中的系数得到比较二杆机器人例中的系数与一般表达式中的系数得到有效惯量系数：有效惯量系数：2211121222122()2cos()Dmm dm dm d d22222Dm d耦合惯量系数：耦合惯量系数：21221222122cos()DDm
17、 dm d d14向心力项系数：向心力项系数：0)sin()sin(22222122112212122111DddmDddmDDD哥氏力项系数：哥氏力项系数：0)sin(22212122212121112DDddmDD重力项：重力项：112112212()sin()sin()Dmmg dm g d22212sin()Dm g d156.4 机器人拉格朗日方程机器人拉格朗日方程的一般表达形式的一般表达形式 从上节容易看出从上节容易看出Lagrange方程是一个二阶耦合、非线性的微方程是一个二阶耦合、非线性的微分方程，为简化计算，未虑及传动链中的摩擦。以下方程的推分方程，为简化计算，未虑及传动链中
18、的摩擦。以下方程的推导，也是不考虑传动链带来的摩擦影响，只考虑杆件本身，然导，也是不考虑传动链带来的摩擦影响，只考虑杆件本身，然后再加入关节处驱动装置（如电机、码盘等）的影响。后再加入关节处驱动装置（如电机、码盘等）的影响。推导分五步进行：推导分五步进行：一、计算任意杆件上任意点的速度；一、计算任意杆件上任意点的速度；二、计算动能二、计算动能 ；三、计算势能三、计算势能 ；四、形成四、形成Lagrange函数；函数；五、建立动力学方程五、建立动力学方程 。212iiikm viiipm gh()iiidLLFdtqq16其速度为：其速度为：一、点的速度一、点的速度r 由于整个系统的动能都是在基
19、础系中考虑的，故需求系统由于整个系统的动能都是在基础系中考虑的，故需求系统各质点在基础坐标系中的速度各质点在基础坐标系中的速度 。r对于杆对于杆 坐标系中的一点坐标系中的一点 ，它在基础坐标系中的位置为，它在基础坐标系中的位置为 iiriirTr式中式中 变换矩阵变换矩阵iT1()iiiiijjjd TrTdrrqrdtdtq速度平方为：速度平方为：22()()()rrrTrace rr式中式中 矩阵的迹矩阵的迹，即矩阵主对角元素之和。，即矩阵主对角元素之和。()T ra c e 172()()rTrace rr11()iiiiTiijkjkjkTTTraceqrqrqq 11()iiTTii
20、iijkjkjkTTTracerrq qqq 二、动能二、动能 位于杆位于杆 上上 处质量为处质量为 的质点的动能是：的质点的动能是：iirdm111()2iiTTiiiiijkjkjkTTdkTracerrq qdmqq 111()2iiTTiiiijkjkjkTTTracer dmrq qqq 18则杆则杆 的动能（在基础坐标系中）为：的动能（在基础坐标系中）为：i11.1()2iiTTiiiiiijkjkjklink ilink iTTkdkTracerr dmq qqq 令式中令式中 称为连杆称为连杆 的伪惯量矩阵。的伪惯量矩阵。i.Tiiilink iJrr dm 1112iiTii
21、iijkjkjkTTkTraceJq qqq 则得到杆则得到杆 的动能为：的动能为：i对于杆对于杆 上任意一点的上任意一点的 （在杆（在杆 坐标系中）可以表示为：坐标系中）可以表示为：iiri(,1)iiiirxyz192.2.2.iiiiiilink ilink ilink ilink iiiiiiiTlink ilink ilink ilink iiiiiiiiiilink ilink ilink ilink ilink iiilink ilink ix dmxydmxzdmxdmxydmy dmyzdmydmJrr dmxzdmyzdmz dmzdmxdmydm .ilink ilink
22、 izdmdm根据理论力学中惯性矩、惯性积和静矩的定义，引入下列记号：根据理论力学中惯性矩、惯性积和静矩的定义，引入下列记号：222222(),(),()xxyyzzIyzdmIxzdmIxydm对坐标轴的惯性矩：对坐标轴的惯性矩：则有：则有：20对坐标轴的惯性积：对坐标轴的惯性积：,xyyzxzIxydmIyzdmIxzdm对坐标轴的静矩：对坐标轴的静矩：,xyzIxdmIydmIzdm质量之和：质量之和：mdm于是：于是：2222222111()()()222x dmyzdmxzdmxydm 2xxyyzzIIIxzyr21同理：同理：22,22xxyyzzxxyyzzIIIIIIy dm
23、z dm222ixxiyyizzixyixzixixxiyyizzixyiyziyiixxiyyizzixziyzizixiyiziIIIIIIIIIIIIJIIIIIIIIIm于是于是 能够表达为：能够表达为：iJ机器人臂杆总的动能是：机器人臂杆总的动能是：111112nniiTiiiijkjkiijkTTKkTraceJq qqq 22如果考虑到关节处驱动装置的动能：如果考虑到关节处驱动装置的动能：2.12imotor iaikJq 调换求迹与求和运算顺序，并加入关节处驱动装置的动能，调换求迹与求和运算顺序，并加入关节处驱动装置的动能，得到机器人总的动能为：得到机器人总的动能为：21111
24、11()22iniinTiiijkaijkijkiTTKTraceJq qJqqq 2.12im o to r iaikmq（对于移动关节：（对于移动关节：）式中式中 为关节为关节 处驱动装置的转动惯量。处驱动装置的转动惯量。aiJi三、势能三、势能 设杆设杆 的质心在再其自身坐标系的位置向量为的质心在再其自身坐标系的位置向量为 ，则它在，则它在基础坐标系中的位置向量基础坐标系中的位置向量 为为 iiCrCriCiCrTr(,1)TiiiiCCCCrxyz23设重力加速度设重力加速度 在基础坐标系中的齐次分量为：在基础坐标系中的齐次分量为：TTiiiCiiCpm grm gTr 于是机器人的总
25、势能为：于是机器人的总势能为：g(,1)Txyzgggg则杆在基础坐标系中的势能为：则杆在基础坐标系中的势能为：v（一般认为基础坐标系的（一般认为基础坐标系的 z 轴取向上方）轴取向上方）11nnTiiiiCiiPpm gTr 24先求拉格朗日方程中的各项：先求拉格朗日方程中的各项：111()2niTiiikppkikTTLTraceJqqqq四、拉格朗日函数四、拉格朗日函数1111()2niiTiiijkjkijkTTLKPTraceJq qqq 21112nnTiaiiiiCiiJqm gTr五、动力学方程五、动力学方程111()2pniTiiijapjpijTTTraceJqIqqq（1
26、)25由于由于 是对称矩阵，则有：是对称矩阵，则有：合并合并(a)(a)式中前两项，得到：式中前两项，得到：iJ()()TTTiiiiiipkpkTTTTTraceJTraceJqqqq()TTiiikpTTTraceJqq()TiiikpTTTraceJqq11()pniTiiikappkpikTTLTraceJqIqqqq（1)当当 时，时，中不包含中不包含 以后关节变量，即：以后关节变量，即：ipiTp0,ipTq()ip于是可得：于是可得：1()pniTiiikappkpipkTTLTraceJqIqqqq26211()niiTiiikmkmpipkmTTTraceJq qqqq（2)
27、1()()pniTiiikappkpipkTTdLTraceJqIqdtqqq211()niiTiiikmpmkipkmTTTraceJq qqqq 1()()pniTiiikappkpipkTTdLTraceJqIqdtqqq交换其中的部分亚元，得到：交换其中的部分亚元，得到：2112()niiTiiikmkmpipkmTTTraceJq qqqq 272111()2niiTiiijkpjpkipjkTTLTraceJq qqqqq 2111()2niiTiiijkkpjipjkTTTraceJq qqqq nTiiiCpipTm grq211()niiTiiijkjpkipjkTTTrac
28、eJq qqqq nTiiiCpipTm grq（3)28将以上各项带入拉格朗日公式，并用将以上各项带入拉格朗日公式，并用 和和 分别代替上式中分别代替上式中的哑元的哑元 和和 ，得到：，得到：ijip1()ijnTjjijkaikijikTTTraceJqIqqq211()jjnTjjjkmkmijikmTTTraceJq qqqq njTijCijiTm grq上式为拉格朗日方程的最后形式。这些方程与求和的次序无上式为拉格朗日方程的最后形式。这些方程与求和的次序无关，因此可将上式写为简化形式：关，因此可将上式写为简化形式：（5)111innniijjaiijkjkijjkD qIqDq q
29、D（4)29式中：式中：max(,)()nTppijpjjpi jTTDTraceJqq()npTpipCipiTDm grq2max(,)()nTppijkpjkipi j kTTDTraceJqqqv 以上的动力学方程以上的动力学方程 (5)(5)中系数中系数 D 的意义与上节所列相同，的意义与上节所列相同，即分别为有效惯量项系数即分别为有效惯量项系数 (),(),耦合惯量项系数（耦合惯量项系数（），），向心力项系数（向心力项系数（），哥氏力项系数（），哥氏力项系数（），重力项等。），重力项等。ijkjkjij30v 动力学方程中的惯量项和重力项在机器人控制重特别重要，动力学方程中的惯量项
30、和重力项在机器人控制重特别重要，将直接影响系统的稳定性和定位精度。只有当机器人高速运将直接影响系统的稳定性和定位精度。只有当机器人高速运动时，向心力项和哥氏力项才是重要的。传动装置的惯量值动时，向心力项和哥氏力项才是重要的。传动装置的惯量值往往较大，对系统动态特性的影响也不可忽略。往往较大，对系统动态特性的影响也不可忽略。在机器人动力学问题的讨论中，拉格朗日动力学方程常写在机器人动力学问题的讨论中，拉格朗日动力学方程常写作更简化的一般形式：作更简化的一般形式：()()()(),()()tD q tq th q tq tG q t式中：式中：12()(),(),(),)Tntttt12()(),
31、(),(),)Tnq tqtqtqt12()(),(),(),)Tnq tqtqtqt12()(),(),(),)Tnq tqtqtqt(),(),(),()D q th q tq tG q t的意义见（的意义见（5)式。式。（6)31乘法次数：乘法次数：432128/3512/3844/376/3nnnn6.5 机器人的牛顿机器人的牛顿欧拉方程欧拉方程 机器人的拉格朗日动力学模型为非线性二阶常微分方程，机器人的拉格朗日动力学模型为非线性二阶常微分方程，利用这些方程，由已知的每一轨迹设定点的关节位置、速度利用这些方程，由已知的每一轨迹设定点的关节位置、速度和加速度，可以计算各关节的标称力矩，但
32、拉格朗日方程利和加速度，可以计算各关节的标称力矩，但拉格朗日方程利用用44齐次变换矩阵，使得计算效率太低。齐次变换矩阵，使得计算效率太低。43298/3781/6637/3107/6nnnn加法次数：加法次数：为了实现实时控制，曾用过略去哥氏力和向心力的简化模为了实现实时控制，曾用过略去哥氏力和向心力的简化模型，但当操作机快速运动时，哥氏力和向心力在计算关节力型，但当操作机快速运动时，哥氏力和向心力在计算关节力矩中是相当重要的。因而这种简化只能用于机器人的低速运矩中是相当重要的。因而这种简化只能用于机器人的低速运动，在典型的制造业环境中，这是不合乎要求的。此外，这动，在典型的制造业环境中，这是
33、不合乎要求的。此外，这种简化所引起的关节力矩误差，不能用反馈控制校正。种简化所引起的关节力矩误差，不能用反馈控制校正。牛顿牛顿欧位法采用迭代形式方程，计算速度快，可用于实欧位法采用迭代形式方程，计算速度快，可用于实时控制，因而成为一种常用的建模方法。时控制，因而成为一种常用的建模方法。32 寻求转动坐标系和固定惯性坐标系之间必要的数学关系寻求转动坐标系和固定惯性坐标系之间必要的数学关系,再再推广到运动坐标系推广到运动坐标系(转动和平移转动和平移)和惯性坐标系之间的关系。和惯性坐标系之间的关系。如图如图,惯性坐标系惯性坐标系O-XYZ和转动坐标系和转动坐标系O-X*Y*Z*的原点重合的原点重合于
34、于O点。而点。而OX*、OY*、OZ*轴轴相对相对OX、OY、OZ轴旋转。设轴旋转。设 和和 分别为这两个坐标系沿主轴的单位矢分别为这两个坐标系沿主轴的单位矢量。转动坐标系中点量。转动坐标系中点 P 可用它在任一坐标系中的分量来表示：可用它在任一坐标系中的分量来表示：6.5.1 转动坐标系转动坐标系(,)ij k(,)ijkrxiyjzk或或rx iy jz k在惯性坐标系中的运动：在惯性坐标系中的运动：drdxdydzijkxiyjzkdtdtdtdtYXZrY*X*Z*OP在转动坐标系中的运动：在转动坐标系中的运动：d rdxdydzijkx iy jz kdtdtdtdt 33在惯性坐标
35、系中的运动：（在惯性坐标系中的运动：（）drdxdydzdidjdkijkxyzdtdtdtdtdtdtdtd rdidjdkxyzdtdtdtdt（7)需要解决转动坐标系坐标轴在惯性坐标系中的导数问题。我需要解决转动坐标系坐标轴在惯性坐标系中的导数问题。我们假定，转动坐标系绕着过原点们假定，转动坐标系绕着过原点O的某轴的某轴OQ以角速度以角速度 旋转。旋转。方向沿方向沿OQ轴，指向转动坐标系右旋方向。则可以证明转动坐轴，指向转动坐标系右旋方向。则可以证明转动坐标系中的任意固定矢量标系中的任意固定矢量 在惯性坐标系中的导数为：在惯性坐标系中的导数为：sXZYsY*X*Z*OQdssdtrx i
36、y jz k34于是由于是由（6)式可得：式可得：()()()drd rxiyjzkdtdtdrd rrdtdt这是建立转动坐标系两种时间导数之间关系的基本方程。这是建立转动坐标系两种时间导数之间关系的基本方程。22()d rdd rdrdrdtdtdtdtdt22()drd rd rdrrdtdtdtdt22222()d rdrd rdrrdtdtdtdt方程方程（9)又被称为哥氏定理。又被称为哥氏定理。（8)（9)356.5.2 运动坐标系运动坐标系如图如图,运动坐标系运动坐标系O-X*Y*Z*相对于惯性坐标系相对于惯性坐标系O-XYZ转动和平转动和平移。质量为移。质量为M 的质点的质点P
37、 分别以分别以 和和 确定相对于惯性坐标系确定相对于惯性坐标系和运动坐标系的原点的位置。原点和运动坐标系的原点的位置。原点O*相对于原点相对于原点O的位置以矢的位置以矢量量 表示。则有：表示。则有：rrhrrh()()kdrdrdhv tvvdtdtdt()d rdhv trdtdt2222()()()kdv td rd ha taadtdtdt22222()drd rdd hrrdtdtdtdtrYXZOPY*X*Z*O*r*h（10)（11)366.5.3 杆件运动学杆件运动学 根据前述运动坐标系的概念，推导一组数学方程，描述机根据前述运动坐标系的概念，推导一组数学方程，描述机器人的运动杆
38、件相对于基础坐标系的运动学关系。器人的运动杆件相对于基础坐标系的运动学关系。坐标系坐标系 是基础坐是基础坐标系，而坐标系标系，而坐标系 和和 分别固联于杆分别固联于杆件件 i-1和杆件和杆件 i 上，原点分别上，原点分别为为 Oi-1和和 Oi。原点。原点 Oi 相对于相对于原点基础坐标系原点基础坐标系O 和原点和原点 Oi-1 的位置分别用位置矢量的位置分别用位置矢量 和和 表示。原点表示。原点 Oi-1相对于基相对于基础坐标系原点础坐标系原点 O 的位置用位的位置用位置矢量置矢量 表示。表示。000(,)xyz(,)iiixyzipip1ip111(,)iiixyz3711iiiiidpv
39、pvdt1iii式中，式中，d*(.)dt 表示在表示在运动坐标系运动坐标系 的时间导数。的时间导数。111(,)iiixyz（12)（13)令令 和和 分别为坐标系分别为坐标系 相对于基础坐标相对于基础坐标系系 的线速度和角速度。令的线速度和角速度。令 和和 分别为杆件分别为杆件i坐标系坐标系 相对于基础坐标系相对于基础坐标系 和杆件和杆件 i-1坐标系坐标系 的角速度。则杆件的角速度。则杆件 i 坐标系坐标系 相对于基相对于基础坐标系的线速度础坐标系的线速度 和角速度和角速度 分别是：分别是：1iv1i111(,)iiixyz000(,)xyzii(,)iiixyz000(,)xyz111
40、(,)iiixyzivi3821111122()iiiiiiiiiid pd pvppvdtdt1iii1iiiiidddtdt11iiiiiddt（14)（15)为坐标系为坐标系 相对于相对于 的角加速度的角加速度:(,)iiixyz111(,)iiixyzi 根据机器人杆件坐标系建立的步骤和参数的定义，杆件根据机器人杆件坐标系建立的步骤和参数的定义，杆件 i 在在杆件杆件 i 1 坐标系中的运动是沿坐标系中的运动是沿 方向的平移或绕方向的平移或绕 转动。转动。因此，因此，1iz1iz式中式中 是杆件是杆件 i 相对于杆件相对于杆件 i-1 坐标系的角速度值。坐标系的角速度值。q i1iiz
41、q0若杆件若杆件 i 转动转动若杆件若杆件 i 平移平移（16)39（18)类似地类似地（17)idd t1iizq0若杆件若杆件 i 转动转动若杆件若杆件 i 平移平移由式由式（13)和和式式（16)有：有：11iiizqi1i若杆件若杆件 i 转动转动若杆件若杆件 i 平移平移1111()iiiiiizqzqi1i若杆件若杆件 i 转动转动若杆件若杆件 i 平移平移（19)22idpdt1iizq()iiiiidppdt若杆件若杆件 i 转动转动若杆件若杆件 i 平移平移由式由式（15)、式式（16)和和式式（17)有：有：idpdt1iizqiip若杆件若杆件 i 转动转动若杆件若杆件
42、i 平移平移（20)（21)由式由式（15)、式式（16)和和式式（17)有：有：40（22)iv 1iiipv11iiiiizqpv若杆件若杆件 i 转动转动若杆件若杆件 i 平移平移由式由式（12)、式式（13)和和式式（20)有：有：并根据式并根据式（14)、式式（15)和和式式（19)有：有：iv 1()iiiiiippv1112()()iiiiiiiiiz qpz qpv（23)若杆件若杆件 i 转动转动若杆件若杆件 i 平移平移41 刚体的运动可分解为随质心的移动和绕质心的转动。借助于刚体的运动可分解为随质心的移动和绕质心的转动。借助于杆件运动学知识，我们把达朗贝尔原理用于每个杆件
43、，描述机杆件运动学知识，我们把达朗贝尔原理用于每个杆件，描述机器人各杆件的运动。达朗贝尔原理可应用于任意瞬时，它实质器人各杆件的运动。达朗贝尔原理可应用于任意瞬时，它实质上是牛顿第二运动定律的一种变型，可表示为：上是牛顿第二运动定律的一种变型，可表示为：6.5.4 牛顿牛顿欧拉法基本运动方程欧拉法基本运动方程 iiiFm v牛顿定理牛顿定理:欧拉方程欧拉方程 :()iiiiiiNII式中：式中：杆杆i 质量；质量；杆杆i上所有外力合力；上所有外力合力；杆杆i上所有外力对质心的合力矩；上所有外力对质心的合力矩；杆杆i 绕其质心惯性矩阵。绕其质心惯性矩阵。imiFiNiI()iiidm vFdt(
44、)iiidINdt421,1iiii iiFffm g1,1,11,1.()()iiii ii cii iiciiiNNNrfrf 根据力（矩）平衡原理，在质心处有：根据力（矩）平衡原理，在质心处有：则有则有1,10iii iiiiffm gmv1,1,11,1.()()()0iii ii cii iiciiiiiiiiNNrfrfII （24)方程方程（24)即为牛顿即为牛顿欧拉法的基本方程。欧拉法的基本方程。,1i if,1i iN1,iif1,iiNim giz1iz,ii Crioiyix1iy1ix1io43 上面推导的牛顿上面推导的牛顿欧拉法（也简称欧拉法（也简称N-E法）方程式含
45、关节法）方程式含关节联接的约束力（矩），没有显示地表示输入联接的约束力（矩），没有显示地表示输入输出关系，不适输出关系，不适合进行动力学分析和控制器设计。如果变换成由一组完备且独合进行动力学分析和控制器设计。如果变换成由一组完备且独立的位置变量（质心位置变量通常不是相互独立的）和输入力立的位置变量（质心位置变量通常不是相互独立的）和输入力来描述，这些变量都显式地出现在动力学方程中，即得到来描述，这些变量都显式地出现在动力学方程中，即得到显式显式的输入的输入输出形式表示的动力学方程，称为封闭形式的动力输出形式表示的动力学方程，称为封闭形式的动力学方程学方程（拉格朗日方程即是封闭的）。（拉格朗日方
46、程即是封闭的）。6.5.5 递推形式的牛顿递推形式的牛顿欧拉方程欧拉方程 关节变量关节变量 是一组完备且独立的变量，关节力（矩）是一组完备且独立的变量，关节力（矩）是是一组从约束力（矩）中分解出来的独立的输入，所以用一组从约束力（矩）中分解出来的独立的输入，所以用 和和来描述方程，可以得到封闭形式的动力学方程。来描述方程，可以得到封闭形式的动力学方程。qq44 根据根据N-E法的基本方程，利用质心运动变量与关节变量及关法的基本方程，利用质心运动变量与关节变量及关节运动变量之间的关系以及约束力与关节力矩之间的关系，消节运动变量之间的关系以及约束力与关节力矩之间的关系，消去中间变量，可以得到封闭形
47、式的动力学方程。但显然不如用去中间变量，可以得到封闭形式的动力学方程。但显然不如用拉格朗日法简单，特别是当机器人自由度较多时，更是如此。拉格朗日法简单，特别是当机器人自由度较多时，更是如此。因此，对于因此，对于N-E法，常用的不是它的封闭形式方程，而是它法，常用的不是它的封闭形式方程，而是它的递推形式方程。方程（的递推形式方程。方程（2424）可直接写成如下递推形式：）可直接写成如下递推形式：1,1iii iiiiffmvm g1,1,11,1.()0iii ii cii iiciiiiiiiiNNrfrfII（25)而关节力（矩）可写成如下形式：而关节力（矩）可写成如下形式：11,11,Ti
48、iiiiiTiiiiizNbqzfbq对于旋转关节对于移动关节（26)式中，式中，为沿关节轴线为沿关节轴线 的单位矢量，的单位矢量，为关节的粘滞阻尼系数。为关节的粘滞阻尼系数。1izib1iz45 递推形式的递推形式的N-E法方程法方程与封闭形式方程比较，计算量从与封闭形式方程比较，计算量从减少到减少到 ：乘法次数：乘法次数：117117n 24 24，加法次数：加法次数：103103n-2121，从而大大加快了计算速度。自由度越多，递推形式的优势越明从而大大加快了计算速度。自由度越多，递推形式的优势越明显。对于典型显。对于典型 n=6 的情形，递推形式的计算效率几乎提高的情形，递推形式的计算
49、效率几乎提高1010倍。倍。因此，常用于实时计算。因此，常用于实时计算。递推形式递推形式方程方程的特点是其计算从机器人操作机的一个杆到另的特点是其计算从机器人操作机的一个杆到另一杆逐个顺序进行的，它充分利用了操作机的串联链特性，一杆逐个顺序进行的，它充分利用了操作机的串联链特性，常常用于求解动力学逆问题（即已知用于求解动力学逆问题（即已知 ，求，求 ）。）。求解的大致过程为：根据运动和力的不同传递方向，进行运求解的大致过程为：根据运动和力的不同传递方向，进行运动量的向前迭代和力学量的向后迭代。动量的向前迭代和力学量的向后迭代。具体步骤如下：具体步骤如下：4()O n()O n,46n1n210
50、n,1n nf,1n nN动力学动力学计算计算运动学计算运动学计算22221,111111333122,cccccncnnnvvw wq q qvvw wvvw wq q qqqq 1 1确定计算确定计算N-E方程所需的所有运动量，包括每个杆件的方程所需的所有运动量，包括每个杆件的 （）由杆）由杆1 1 杆杆n：1,1,11,1.1,2,10,1()nnn nn cin nncininnnnnnnnnNNrfrfIINNN1,11,2,10,1nnn nnn cnnnnnffm gm vfffi2.将上述运动量代入将上述运动量代入N-E方程，确定关节力（矩）。计算顺方程，确定关节力（矩）。计算
