机器人运动分析中的矩阵变换

1、112Tnqqqqi(i=1,2,., )qniq TxyzVxyz 12xynzxyqzqJq111111(,)(,)(,)(,)(,)(,)nnnxnynznx qqy qqz qqPqqqqqq TdPVPqdtq1212121212nnxxxtnyyynzzznxxxqqqyyyqqqPJqqqqqqqqqq 在机器人学中，在机器人学中，J J是一个把关节速度向量是一个把关节速度向量 变变换为手爪相对基坐标的广义速度向量的变换矩阵。换为手爪相对基坐标的广义速度向量的变换矩阵。在三维空间运行的机器人，其在三维空间运行的机器人，其J J阵的行数恒为阵的行数恒为6(6(沿沿/ /绕基坐标系的

2、变量共绕基坐标系的变量共6 6个个) )；列数则为机械手含有的；列数则为机械手含有的关节数目。关节数目。 对于平面运动的机器人来说，手的广义位置向对于平面运动的机器人来说，手的广义位置向量量 均容易确定，可采用直接微分法求均容易确定，可采用直接微分法求J J，比较方便。比较方便。( , , )Tx yiq 对于三维空间运行的机器人则不完全适用。对于三维空间运行的机器人则不完全适用。从从三维空间运行的机器人运动学方程，可以获得直角三维空间运行的机器人运动学方程，可以获得直角坐标位置向量坐标位置向量 的显式方程，因此，的显式方程，因此，J J的前三的前三行可以直接微分求得，但不可能找到方位向量行可

3、以直接微分求得，但不可能找到方位向量 的一般表达式。找不出互相独立的、无的一般表达式。找不出互相独立的、无顺序的三个转角来描述方位绕直角坐标轴的连续顺序的三个转角来描述方位绕直角坐标轴的连续角运动变换是不可交换的，而对角位移的微分与对角运动变换是不可交换的，而对角位移的微分与对角位移的形成顺序无关，故一般不能运用直接微分角位移的形成顺序无关，故一般不能运用直接微分法来获得法来获得J J的后三行。因此，常用构造性方法求雅可的后三行。因此，常用构造性方法求雅可比比J J。( , , )Tx y z(,)Txyz xyzdd id jd kTxyzddddxyzijkTxyzdD 01limtdVt

4、 ()()()()()()()()()000000000TxxxyzxyzTyyxyzxyzTxyzxyzzzTxyzxxTxyzyyTxyzzzddnnnPnPnPnddoooP oP oP oaaaPaPaPaddnnnoooaaa( )0TTTTTddRR S PR .xxxyyyzzznoaRnoanoa0( )0.0yzxyxppS Ppppp( ),( ).S PPS PP( )() ,( )()TTTTS PPS PP ()()()( )()()()()()()xyzTxyzxyzPnPnPnR S PPoPoPoPaPaPa( )0TTTTTRR S PR ()0BABBTAA

5、ABOBBAARR SPR( )VJ q q( )DJ q dq112ln2111llaaanqJJJqJJJq iq LiJaiJiiqdiiqd设某时刻仅此关节运动、其余的关节静止不动，则：设某时刻仅此关节运动、其余的关节静止不动，则：eLiivJ q 设设b bi-1i-1为为z zi-1i-1轴上的单位矢量，利用它可将局部坐标下轴上的单位矢量，利用它可将局部坐标下的平移速度的平移速度didi转换成基础坐标下的速度：转换成基础坐标下的速度：1eiivb diiqd由于由于所以所以1LiiJb (2)(2)第第i i个关节为转动个关节为转动关节时，关节时， 设某时刻仅此关节运设某时刻仅此关

6、节运动，其余的关节静止动，其余的关节静止不动，仍然利用不动，仍然利用b bi-1i-1将将z zi-1i-1轴上的角速度轴上的角速度转化到基础坐标中去转化到基础坐标中去1iiibiiq1,ier矢量矢量 起于起于O Oi-1i-1，止于，止于O On n，所以由，所以由i i产生的线速度为产生的线速度为: :eLiivJ q1,eiievr1,LiiiieJ qr1,ier1iiib1,LiiiieJ qr11,11,()LiiiiieiieiJ qbrbr（）iiq11,LiiieJbr由于由于所以所以iiqdiiqd0aiJiiq1iaiiiiJ qb1aiiJb1LiiJb11,Liii

7、eJbr0aiJ1aiiJb10liiaiJbJ11,1iieliaiibrJJb1ib1,ier001b 1 1、用、用b b表示表示z zi-1i-1轴上的单位向量轴上的单位向量把它转换到基础坐标系中，即为把它转换到基础坐标系中，即为0121112211()().()iiiibR qR qRqb 如右图所示。用如右图所示。用O O、O Oi-1i-1、O On n分别表示基础坐标系、分别表示基础坐标系、i-1i-1号坐标及手部坐标系的原点。号坐标及手部坐标系的原点。用矢量用矢量x x表示在各自坐标系中表示在各自坐标系中的原点。的原点。T(0,0,0,1)x 1,1ienirOOOO1,ie

8、r把把用齐次坐标表示用齐次坐标表示1,1,1ieierx1,T0121,xyz112211(p ,p ,p ,1)()().()1ieiieiirxA qA qAqx有上式可以确定有上式可以确定1,ier机械臂末端的速度为机械臂末端的速度为000 ,001idd 微分变换法微分变换法 ()(0)()TxzTyzTzziTzxTzyTzzdPndPdPadnoa对于转动关节对于转动关节 000,010iddd 000TxzTyzTzziTxTyTzdndodadd对于移动关节对于移动关节 000zzzTinoaJ()(0)()zzzTizzzPnPPaJnoa对于移动关节对于移动关节 对于转动关

9、节对于转动关节 000105061656126015( )zPzPzPJ qJJJzzz例：例：PUMA560PUMA560的的6 6个关节都是转动关节，其雅可比个关节都是转动关节，其雅可比有有6 6列。此处用矢量积法计算列。此处用矢量积法计算J(q)J(q)123 4 563 5634223 4 56356414 562123 4 563 5634223 4 56356414 5621 ()()() ()()()0001s c c c s ds c ds das s c s dc c ddc s s ddc c c c s ds c ds das s c s dc c dds s s ddJ

10、123 4 5636 54223 4 563564123 4 5636 54223 4 56356423 4 5636 54223 4 563564211 ()() ()() ()()0c s c c s ds d cdac s c s dc c dds s c c s ds d cdac s c s dc c ddc c c s ds d cdas s c s dc c ddJsc1 23 4 5636 541 23 4 5635641 23 4 5636 541 23 4 56356423 4 5636 5423 4 563564311()()()()()()0c c s c s dc d

11、 cdc s c c s ds c dds c s c s dc d cds s c c s ds c dds s c s dc d cdc c c s ds c ddJsc1 23 4 561 4 561 23 4 561 4 5623 4 5641 231 2323c c s s ds c s ds c s s dc c s ds s s dJc ss sc561 23 41 41 23 56561 23 41 41 23 5623 4 5623 5651 23 41 41 23 41 423 4()()c d c c cs sc s s dc ds c cc ss s s ds c c

12、dc s dJc c ss cs c sc cs s651 23 41 41 23 551 23 41 41 23 523 4 523 5000()()Js c c cs sc s cs s c cc ss s cs c sc c例：斯坦福六自由度机器人除第三关节为移动关节例：斯坦福六自由度机器人除第三关节为移动关节外，其余外，其余5 5个关节为转动关节。此处用微分法计算个关节为转动关节。此处用微分法计算T TJ(q)J(q)224 5 64 62 5 6234 5 64 6224 5 6462 5 6234 5 64 5224 52 523 4 51224 64 62 5 6224 6462

13、 5 624 52 5()()()()()()()Td c c c cs ss s cs d s c cc sdc c c ss cs s ss ds c sc cd c c ss cs d s sJs c c ss sc s cs c c ss cc s ss c sc c34 5 64 634 5 64 63 4 524 5 64 64 5 64 64 5()()Td c c cs sd c c cs cd c sJs c cc ss c sc cs s5 65 653000Ts cs scJ45 65 65000TJs cs sc5660000TJsc6000001TJ 若给定机器人终端

14、手抓的广义速度向量若给定机器人终端手抓的广义速度向量V，则可由下式解出相应的关节速度：则可由下式解出相应的关节速度：逆雅可比矩阵逆雅可比矩阵1qJ V112ln2111llaaanqJJJqJJJq 上式中，上式中， 称为逆雅可比矩阵，称为逆雅可比矩阵， 为加为加给对应关节伺服系统的速度输入变量。给对应关节伺服系统的速度输入变量。1Jiq 雅可比矩阵的应用雅可比矩阵的应用1 1、分离速度控制、分离速度控制1qJ V 由上式可见，当已知手端速度向量由上式可见，当已知手端速度向量V V，可通过左，可通过左乘雅可比逆矩阵计算出机器人的关节速度向量，所以乘雅可比逆矩阵计算出机器人的关节速度向量，所以上

15、式为运动学逆问题的速度关系式，是对机器人进行上式为运动学逆问题的速度关系式，是对机器人进行速度控制的基本关系式。速度控制的基本关系式。 采用计算机控制时，把速度表示位置增量的采用计算机控制时，把速度表示位置增量的形式，故将上式写为：形式，故将上式写为：1qJ V11nqqJvq式中式中,v,v为手为手部在基础坐标部在基础坐标下一个采样周下一个采样周期的位移期的位移( (线位线位移、角位移移、角位移) )； qq为在同一周为在同一周期内关节变量期内关节变量的增量。的增量。 当要求机器人沿某轨迹运动时，当要求机器人沿某轨迹运动时，v为已知，将为已知，将它代入上式中求得关节变量增量它代入上式中求得关

16、节变量增量q ，于是可确定各，于是可确定各关节变量值，由伺服系统实现位置控制，这就是分关节变量值，由伺服系统实现位置控制，这就是分离速度控制原理，如下图所示。离速度控制原理，如下图所示。v要求要求1Jv1iiv实际实际分离速度控制原理分离速度控制原理雅可比矩阵的应用雅可比矩阵的应用2 2、在静力分析中的应用、在静力分析中的应用 有些机器人的工作需要与环有些机器人的工作需要与环境接触，并保持一定的接触力，境接触，并保持一定的接触力，如右图所示。接触力如右图所示。接触力F F可表示为可表示为一个六维力向量：一个六维力向量：TxyzxyzFfff n n n12.TnT 设一个驱动器只驱动一个关节，

17、则设一个驱动器只驱动一个关节，则n n个关节需求个关节需求n n个驱动力，可组成一个个驱动力，可组成一个n n维关节力向量维关节力向量: :TTJ FT T与与F F的关系可以表示为：的关系可以表示为：2-56TTJ F 式中，式中， 称为机器人称为机器人力雅可比力雅可比，它表示在，它表示在静止平衡状态下，末端广义力向关节力映射的线静止平衡状态下，末端广义力向关节力映射的线性关系。显然，力雅可比是机器人速度雅可比的性关系。显然，力雅可比是机器人速度雅可比的转置。因此，机器人静力学传递关系和速度传递转置。因此，机器人静力学传递关系和速度传递关系紧密相关。关系紧密相关。TJ由构型和例由构型和例2-62-6可得：可得：1 12 122 12221 12 122 12101111l sl sl sllJl cl cl cl思考题思考题1 1：右图为三自由度机械手右图为三自由度机械手(1)(1)用用D-HD-H方法建立各附体坐方法建立各附体坐标系；