带平动的三连杆运动学计算

1、带平动的三连杆运动学计算一、正向运动学：对于正向运动学，就是通过给定的转动副转动角度01,62,63求出末端机械手的位姿变化，因为有pF=TpJM，其中向F：点P在固定坐标系F中的坐标表示；pM:点p在局部坐标系M中的坐标表示；T:包含了局部坐标系M相对于固定坐标系F的全部位姿信息，即T矩阵实际上包括了M坐标系相对于F坐标系的全部转动与平动的变换信息，只要在局部坐标系M中的坐标左乘T矩阵就可以直接得到点P在固定坐标系F中的坐标表示。对于带平动的三连杆其正向运动学的目标是通过给定的转动关节的转动角度&风以及平动关节的长度b2求出末端机械手的位姿，即对应的T矩阵。方法如下：(1)根据建立DH坐标的

2、原则，在连杆上建立DH坐标，如下图图1(2)定义的DH参数如下表所示:Linkbai%100-22b2-31023003a30(3)分别列写转换矩阵通过求DH坐标间的转换求连杆i的转换矩阵T，即坐标系i+1相对于坐标系i的齐次变换矩阵，其中对于连杆i,有:3-seiCaiSiSttiiC0i1-ri+_i-SHCeiCai-CQiSotiaSi0SotjC四b0001-(1-1)针对图1,根据如上所列的DH参数表，将其三个连杆的DH参数分别代入矩阵丁出(式1-1)中有:C0S902_sq0-CO10一0100一0001注：1T2是坐标1-100030010,2=010b20001(2相对于坐标

3、系一%-S3丁4=双巡00J001的转换矩阵；0a3C”10a3s310012T3是坐标系3相对于坐标系2的转换矩阵;3丁4是坐标系4相对于坐标系3的转换矩阵。(4)求出矩阵1T小，矩阵1T+为末端坐标系i+1相对于固定坐标系1的齐次变换矩阵，其中1T=1T22T3nTnL对于图1而言，1T4=1T22T33T4,即iT4-T22T丁=-C。1-3)-S(01+63)I0I0SR飞)-C(i飞)000010b2S-a3C(1i为)bzCaia3s(J+03)01这里C=cos,S=sin到这里，正向运动学结束，根据给出的机力3以及平动关节的长度b2,求出转换矩阵T,得到末端机械手的位置姿态信息

4、。二、逆向运动学与正向运动学相反，逆向运动学是根据末端机械手的位置姿态信息求解目凤以及平动关节的长度b2O也就是说矩阵T是已知的，我们可以通过矩阵T来求解运动过程中的求解4,%以及平动关节的长度b2o针对图1,我们已经得到了对应的T矩阵为一-C(4+a)1-S(01+03)0I0SR4)-C(飞)000010bzS)-bzg-a3s(3+%)01为了表示方便，我们设由于矩阵T中的元素都是已知的，所以11，12r33和Px，Py，Pz均是已知的，所以我们可以得到9Pb2,%以下算法:一111213Pxl2122r23Py313233Pz-0001-T=设两个变量wx,wy/MWx=Px-(a3r

5、11)Wy二-Py-a3211贝Ux=atan2(Wx,Wy)/0设变量FiFi=atan2(ri2,fi)在实际编程中要特别注意反正切的表示范围，超过表示范围的角度，需要酌情加一个角度以保证正确。飞uFi-/b2b2=(wx/sin用)注意：不同的运动副以不同的方式组成的运动链形成的T矩阵均不同，所以逆向运动学求解可以根据该思路具体案例具体求解。三、雅各比矩阵求解目的：根据已知末端的速度和加速度求解中间运动副环节的速度和加速度，以便控制运动链的运动使末端以一定的速度和角速度达到期望的位置。雅各比矩阵直接连接了末端执行机构运动速度与关节变量角速度之间的关系。当终端角速度、线速度和各个关节变量速

6、度为:%exVexII|-2.e-eyVe=Vey二；一ez_Vez?n有：厂eVeT=Je,此矩阵J称为雅可比矩阵。te=jej=j11JV一需要注意的是对于带平动的连杆，它的4还应包括平动关节的移动速度V。且雅各比矩阵的每一行代表一个自由度，前三行代表转动的三个方向Wx，Wy,Wz,后三行代表平动的三个方向Vx,Vy,Vz,但是对于二维平面，则只有三个自由度，也就是说平面三连杆的雅各比矩阵只有三行，第一行表示转动Wz,后两行表示两个方向的平动Vx,Vy,而其他行实际是零，不写出。对于带一个平动关节的三连杆其雅各比矩阵为：e10aJ=L_(3-1)其中giaiee3a3e实际上，雅各比矩阵的

7、每一列都对应一个运动副，也就是说每一个运动副对应于雅各比矩阵的一列。并且转动关节对应的列为一31”，-一，.ji=-L其中ei为平行于转轴的单位向量，其中的aie向量表示从“aiei,e连杆i的原点Oj指向末端坐标系原点oe的向量。平动关节对应的列为0，,、一ji=J,e为平行平动方向的单位向量。61对于图1则有0sinze=0,=costal,备=其中的ei司分别表征的是与转动关节1,3轴向平行的单位向量,的表征与平动关节运动方向平行的单位向量。如下图所示:图中的e,鸟为垂直纸面向外的单位向量，可为方向与z2轴相同的单位向量。b2sin工-a3cos(%.=2)a1e=-b2cos&ia3s

8、in(4+%)ae=a1+a2表示从坐标系1的原点指向末端坐标系的原点Oe的向量;b2cosa3sin(92)而e1父a1e=b2sin61a3cos(4+62)-0J一a3cos(4+日2)1a3e=-a3sin(4+4)i0ia3e=a3表示从坐标系3的原点指向末端坐标系的原点oe的向a3sin（“二2）而e3ma3e=-a3cos（61+%）:.0J带入到式（3-1）可得出对应带平动的三连杆的雅各比矩阵：101J=32cos+a3sin（十日2）sin61a3sin（01+02）b2sin81-a3cos（d+日2）-cos61-a3cos（日1+%）注意，雅各比矩阵应该有六行，这里在二维空间，前边两行表示W，Wy和第六行表示Vz均为零，没有写出四、用雅各比矩阵进行加速度分析对原表达式对时间t求导可得：t：=jJ；,其中e=巴，enT,另外，其中还应当包括平动关节的关节移动的加速度1s对于雅各比矩阵中的转动关节有Ji=|-.ei*条*ei*aie_对于雅各比矩阵中的平动关节有；=t1目而对于这个实例有日1cos4e=0,电=91sin91,q=o；0dsinq+b201cos01+a3(+02)sin(01+02)a1eI-b2cos61+b201sin4-a3(+92)cos(91+2)*1a3(e1+仇)sin(61+优)a3e=|-83(61+62)cos(6i+62