机器人操作的数学导论——机器人动力学1

1、机器人的动力学及控制1 拉格朗日方程2开链机器人动力学方程K拉格朗日方程1.1刚体的惯性设VUR3表示刚体的体积，p(r), rGV是刚体的密度。如果物 体是均匀的，那么p(r)= p为常量。刚体的质量可以表示为： 加=J p(r)dVv刚体的质心是密度的加权平均:r 二丄p(r)rdV如图所示刚体，在质心建立 物体坐标系，g=(p,R)eSE为 物体相对于惯性坐标系的运动轨 迹，WR3为刚体上一点相对于 物体坐标系的坐标，现求刚体的 动能。刚体的惯性点在惯性坐标系的速度为:物体的动能可用如下求得:rVip + Rr j| 2dV展开计算可得:rVip + Rr j| 2dV寺3厂 p(r)H

2、dVja)二：三血卩3少其中w为在物体坐标系中表示的刚体角速度，矩阵3 GRX3为物体坐标 系中的物体惯性张量1.1刚体的惯性物体坐标系中表示的物体惯性张量3如下:其中:J VZ.I %?V=jp(r)(2+ Odjridydh物体总动能可写为移动动能和转动动呢之和，如下式所示:寺m | 02卜*品& = 寺（徉）4r ml 010 S 护= ：*（門丁5XW1.1刚体的惯性上式中V二g-i&w(3)是物体速度矩阵 斬：称为在物体坐标系中表示的物体广义惯性矩 阵。该矩阵是对称且正定的。可以看出动能是个标量，与刚体的位置和姿态无关，由此可以 通过运动旋量的坐标变换来求得新坐标系下的刚体广义惯性矩

3、阵。T=(1/2)VT 游 V=(1 /2)(AdgV)T 咖(AdgV) 网:=(AdgT)-1 游(Adg 尸选取三个坐标轴,使刚体的广义惯性矩阵为对角阵，则这三个轴为刚体的惯性主轴。1.2拉格朗日方程定义拉格朗日函数示为：L(q.q)T(q,q) V(q)式中T和V分别表示系统的动能和势能。对于广义坐标为qWRm、拉格朗日函数为L的机械系统，其运动方 程为：d 3L 3L rdidi * dqi T：上式即为拉格朗日方程，将其写成矢量形式为:At dq 8q作用于第i个广 义坐标的外力对机器人，可将关节角作为其广义坐标，广义力就是作用于关节轮 娱上闵力矩。1.2刚体的牛顿一欧拉方程对于位

4、行为geSE(3)的刚体，利用运动螺旋和力螺旋建立动力学方程。设g=(p，R) w SE (3)表示刚体质心坐标系相对于惯性坐标系的位形,f为在惯性坐标系中表示的作用于质心的力。由牛顿定律可得移动方程:同理，可推导刚体的转动方程，相对于惯性坐标系的角动量为3妙为相对于惯性坐标系的瞬时惯性张量，WS是空间角速度。故 转动方程为I化简得:3+ 4/ X 3 V - T该式称为欧拉方程1.2刚体的牛顿一欧拉方程/二务(灯)(1)+ W = T (2)上述两式描述了刚体的动力学，力和力矩是相对于惯性坐标系表 示的。F面推导用运动螺旋和力螺旋来表示刚体动力学，将物体速度说=尺9和物体力/ =尺了代入式

5、(ZW0)二 ( mR/) Rmvb 十 Rmvb等式两边左乘RT,mvb + a X m/ = f上式就是在物体坐标系中表示的牛顿定律(NEwums Law)。1.2刚体的牛顿一欧拉方程将物体角速度4/工1?丁妙和物体力矩代人欧拉方程化简得:在物体坐标系 中为常量将物体坐标系表示的牛顿定律与上式结合，可得质心处作用有外 力螺旋F、相对于物体坐标系的物体运动方程ml-0广附X加-3JL 沖x-上式称为在物体坐标系中表示的牛顿欧拉方程，由于相对于物体坐标系的线速度和当前姿态有关，故移动和转动存在耦合联系。2、开链机器人动力学2.1开链机器人的拉格朗日函数计算n个关节的开链机器人的动能，可将其中每

6、一连杆动能求和, 定义一固连于第i杆质心的坐标系Li,则可得Li位形：制(0)和占也(0)第i杆质心的物葆速度为：必为相对于為的物体雅可比矩阵* /4r?鹏(9)=专，V 。0】j式中E；=Ad加.如g)J51为相对于第i连杆坐标系的第j个瞬时关节运动螺旋o2、开链机器人动力学2.1开链机器人的拉格朗日函数第i杆的动能为总动能为：矩阵M(O)GR-n为机器人惯性矩阵，可将其定义如下： M(0)=f(0)断涣 )/ = 1第i杆势能为：Vf(0) = mighiWHi(e)为第i杆质心高度。2.开链机器人动力学2.1开链机器人的拉格朗日函数总势能为rr?i = 1? - L将其与动能加以组合,得

7、拉格朗日函数L(ere)=i(i(et&)-vi(6)= eTM(e)e-vw2.2开链机器人的运动方程拉格朗日函数将动能表示成和的形式， Mwe-vO)z j= 1将上式代入拉格朗日方程:J_3L _3L dr 吗 30Ri表示第i关节的驱动力矩和其它非保守力o2.2开链机器人的运动方程展开并整理得：也(0)玄十,口触 + 詣(8)7 r = 1,(1) 其中n 1卫帆dM/0) dM血(的心刃ddk 码 迪)从(1)式看出方程由4部分组成：与关节加速度有关的惯性力和，与 关节速度平方成正比的离心力和哥氏力，有势力和外力。其中 铀t (zVj)项称为哥氏力，将磅项称为离心力2、开链机器人动力

8、学2.2开链机器人的运动方程外力可分为两部分：设&为作用于关节处的外力矩 并定义-0）为作用于第i个广义坐标系的任意其它力，包 括有势力和摩擦力产生的保守力，卩为阻尼系数。作用于机器人上的其它力，可以通过雅克比矩阵的转置将其折 算到关节上。讥T = J F宦义矩阵c（化e） e卿4: li称矩阵C为机器人的哥氏矩阵，于是（1）式可改写为r z y I 迥十 - 2 bM（e）iJ+ce +式中T为驱动力矩矢量，N6）包括重力和作用于关节的 其它力。2.开链机器人动力学2.2开链机器人的运动方程反映机器人惯性特性的矩阵M和C具有如下重要性质o1) M (0)是对称且正定的；2) M-2CRnxn

9、是反对称矩阵。C的定义不是唯一的。F面为一个求三杆机器人的动力如右图，在每一连杆的质心处建立一坐标系Li,坐标轴方向取为与连杆惯性主轴方向一致。机器人动力学方程如下：由于坐标轴均建立在惯性主轴上，故连杆惯性矩阵有如 下的一般形式。式中mi为连杆质量，Ixi, lyi, Izi为第i连杆分别关于x，y和z轴的惯性矩。为计算机器人的惯性矩M (0),需先算出相对于每一连杆的物体雅 可比矩阵。于第i连杆坐标系的第j个 瞬时关节运动螺旋。容易得第1、2连杆对应的物体雅可比矩阵如下：0 O0 0Jj - Jt/o）二0000 00 00 00 0一=丿加）=r心000000厂100-100000.2.开

10、链机器人动力学对连杆三的物体雅可比可 按如下来求：雅可比矩阵的第一列为关 节1的运动螺在L3下的表示判断e的正负时需注意，以03为例，选择一根较容易看出来的轴（例如本例可选择与z轴平行方向的轴） 当03转过一个角度后的到一个位形，判断该选定的轴相对于坐标系L3是如何转的，本例是逆时针，故03相对于 0 0 0 P =厂2+ e迢-A-r2 -也00一人归L3为正。_ 5 23C23同样，可得雅可比矩阵的 其它两列：-/“2-厂2 “3000C23为计算机器人的惯性矩M (0) 可比矩阵。0?卩300_尸2一 100 _,需先算出相对于每一连杆的物体雅2.开链机器人动力学系统的惯性矩阵为:rMn

11、 m12M(6)= M2 M22M3 AJ32M23 = Xfanj i +Xs皿訂 3其中的元素为= ly2S2 + 棚务+ J +心2诊+人?迓3 + 炖2曲士皿3( ? 1 +厂23 FMq = 0/VTl3 = 0M2l = 0矩阵元素:M（e）+c（e+皿22二厶2十心十加3”十叫卄+胡3斥+2加3九厂2“M：3 = Lr3 十 m3r2 + ni3lr23A/引=0M32二匚3 + X厂;+血3仃厂2M33 匚3 十 m3r20Mik L 昭羽障哥氏力和离心力的计算C”（趴 6）二 1 Pijjfik it :|经复杂的计算后，不为零的项为：（心一仃2 一炉2打）C2$2+ （山一 b）c、23七3 一 皿3（工2+厂毋23）（/沙2+厂2$23）厂=（厶3 一文3）3$石f加3厂2也（？山2十厂2C3）不为零的项:厂=（y2 I辽一 m2ri）C22 + 仃，3 -）C23$23 一 m3（l2 4 r*2C23）Ui2 +戶23）厂131 =（ （v3 一作3尢23*3 -协3厂2切（八巾+厂2“3厂211 =（冬2 r 金-叫卄“$2 +（厶3 -心比23巾3 + 加 3（耳2+ 厂 23）（1$2 + 旷223223= _仃叭3心$3232 =-仃U厂3厂233 = -