機器人運動分析中的矩陣變換

1、2021/8/1412021/8/1422021/8/1432021/8/14412Tnqqqqi(i=1,2,., )qniq 2021/8/145TxyzVxyz 2021/8/14612xynzxyqzqJq2021/8/147111111(,)(,)(,)(,)(,)(,)nnnxnynznx qqy qqz qqPqqqqqq TdPVPqdtq2021/8/1481212121212nnxxxtnyyynzzznxxxqqqyyyqqqPJqqqqqqqqqq2021/8/149 在機器人學中，在機器人學中，J J是一個把關節(jié)速度向量是一個把關節(jié)速度向量 變變換為手爪相對基坐標的廣

2、義速度向量的變換矩陣。換為手爪相對基坐標的廣義速度向量的變換矩陣。在三維空間運行的機器人，其在三維空間運行的機器人，其J J陣的行數(shù)恒為陣的行數(shù)恒為6(6(沿沿/ /繞基坐標系的變量共繞基坐標系的變量共6 6個個) )；列數(shù)則為機械手含有的；列數(shù)則為機械手含有的關節(jié)數(shù)目。關節(jié)數(shù)目。 對于平面運動的機器人來說，手的廣義位置向對于平面運動的機器人來說，手的廣義位置向量量 均容易確定，可采用直接微分法求均容易確定，可采用直接微分法求J J，比較方便。比較方便。( , , )Tx yiq 2021/8/1410 對于三維空間運行的機器人則不完全適用。對于三維空間運行的機器人則不完全適用。從從三維空間運

3、行的機器人運動學方程，可以獲得直角三維空間運行的機器人運動學方程，可以獲得直角坐標位置向量坐標位置向量 的顯式方程，因此，的顯式方程，因此，J J的前三的前三行可以直接微分求得，但不可能找到方位向量行可以直接微分求得，但不可能找到方位向量 的一般表達式。找不出互相獨立的、無的一般表達式。找不出互相獨立的、無順序的三個轉角來描述方位繞直角坐標軸的連續(xù)順序的三個轉角來描述方位繞直角坐標軸的連續(xù)角運動變換是不可交換的，而對角位移的微分與對角運動變換是不可交換的，而對角位移的微分與對角位移的形成順序無關，故一般不能運用直接微分角位移的形成順序無關，故一般不能運用直接微分法來獲得法來獲得J J的后三行。

4、因此，常用構造性方法求雅可的后三行。因此，常用構造性方法求雅可比比J J。( , , )Tx y z(,)Txyz 2021/8/1411xyzdd id jd kTxyzddddxyzijkTxyz2021/8/1412dD 01limtdVt 2021/8/1413()()()()()()()()()000000000TxxxyzxyzTyyxyzxyzTxyzxyzzzTxyzxxTxyzyyTxyzzzddnnnPnPnPnddoooP oP oP oaaaPaPaPaddnnnoooaaa( )0TTTTTddRR S PR 2021/8/1414.xxxyyyzzznoaRnoan

5、oa0( )0.0yzxyxppS Ppppp( ),( ).S PPS PP( )() ,( )()TTTTS PPS PP ()()()( )()()()()()()xyzTxyzxyzPnPnPnR S PP oP oP oPaPaPa2021/8/1415( )0TTTTTRR S PR ()0BABBTAAABOBBAARR SPR2021/8/1416( )VJ q q( )DJ q dq2021/8/1417112ln2111llaaanqJJJqJJJq iq LiJaiJ2021/8/1418iiqdiiqd2021/8/1419設某時刻僅此關節(jié)運動、其余的關節(jié)靜止不動，則：

6、設某時刻僅此關節(jié)運動、其余的關節(jié)靜止不動，則：eLiivJ q 設設b bi-1i-1為為z zi-1i-1軸上的單位矢量，利用它可將局部坐標下軸上的單位矢量，利用它可將局部坐標下的平移速度的平移速度didi轉換成基礎坐標下的速度：轉換成基礎坐標下的速度：1eiivb diiqd由于由于所以所以1LiiJb2021/8/1420 (2)(2)第第i i個關節(jié)為轉動個關節(jié)為轉動關節(jié)時，關節(jié)時， 設某時刻僅此關節(jié)運設某時刻僅此關節(jié)運動，其余的關節(jié)靜止動，其余的關節(jié)靜止不動，仍然利用不動，仍然利用b bi-1i-1將將z zi-1i-1軸上的角速度軸上的角速度轉化到基礎坐標中去轉化到基礎坐標中去1i

7、iibiiq1,ier2021/8/1421矢量矢量 起于起于O Oi-1i-1，止于，止于O On n，所以由，所以由i i產(chǎn)生的線速度為產(chǎn)生的線速度為: :eLiivJ q1,eiievr1,LiiiieJ qr1,ier2021/8/14221iiib1,LiiiieJ qr11,11,()LiiiiieiieiJ qbrbr （）iiq11,LiiieJbr由于由于所以所以2021/8/1423iiqdiiqd0aiJiiq1iaiiiiJ qb1aiiJb2021/8/14241LiiJb11,LiiieJbr0aiJ1aiiJb10liiaiJbJ11,1iieliaiibrJJb

8、2021/8/14251ib1,ier001b 1 1、用、用b b表示表示z zi-1i-1軸上的單位向量軸上的單位向量把它轉換到基礎坐標系中，即為把它轉換到基礎坐標系中，即為0121112211()().()iiiibR qR qRqb2021/8/1426 如右圖所示。用如右圖所示。用O O、O Oi-1i-1、O On n分別表示基礎坐標系、分別表示基礎坐標系、i-1i-1號坐標及手部坐標系的原點。號坐標及手部坐標系的原點。用矢量用矢量x x表示在各自坐標系中表示在各自坐標系中的原點。的原點。T(0,0,0,1)x 1,1ienirOOOO1,ier把把用齊次坐標表示用齊次坐標表示1,

9、1,1ieierx2021/8/14271,T0121,xyz112211(p ,p ,p ,1)()().()1ieiieiirxA qA qAqx有上式可以確定有上式可以確定1,ier2021/8/14282021/8/14292021/8/1430機械臂末端的速度為機械臂末端的速度為2021/8/1431000 ,001idd 微分變換法微分變換法 ()(0)()TxzTyzTzziTzxTzyTzzdPndPdPadnoa對于轉動關節(jié)對于轉動關節(jié) 2021/8/1432000,010iddd 000TxzTyzTzziTxTyTzdndodadd對于移動關節(jié)對于移動關節(jié) 2021/8/

10、1433000zzzTinoaJ()(0)()zzzTizzzPnPPaJnoa對于移動關節(jié)對于移動關節(jié) 對于轉動關節(jié)對于轉動關節(jié) 2021/8/1434000105061656126015( )zPzPzPJ qJJJzzz例：例：PUMA560PUMA560的的6 6個關節(jié)都是轉動關節(jié)，其雅可比個關節(jié)都是轉動關節(jié)，其雅可比有有6 6列。此處用矢量積法計算列。此處用矢量積法計算J(q)J(q)2021/8/1435123 4 563 5634223 4 56356414 562123 4 563 5634223 4 56356414 5621()()()()()()0001s c c c s

11、 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 ddJ123 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 ddJsc2021/8/14361 23 4 5636 541 23

12、 4 5635641 23 4 5636 541 23 4 56356423 4 5636 5423 4 563564311()()()()()()0c c s c s dc d 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 sc2021/8/1437561 23 41 41 23 56

13、561 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 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 c2021/8/1438例：斯坦福六自由度機器人除第三關節(jié)為移動關節(jié)例：斯坦福六自由度機器人除第三關節(jié)為移動關節(jié)外，其余外，其余5 5個關節(jié)為轉動關節(jié)。此處用微分法計算個關

14、節(jié)為轉動關節(jié)。此處用微分法計算T TJ(q)J(q)2021/8/1439224 5 64 62 5 6234 5 64 6224 5 64 62 5 6234 5 64 522 4 52 523 4 5122 4 64 62 5 622 4 64 62 5 62 4 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

15、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 scJ2021/8/144045 65 65000TJs cs sc5660000TJsc6000001TJ 2021/8/1441 若給定機器人終端手抓的廣義速度向量若給定機器人終端手抓的廣義速度向量V，則可由下式解出相應的關節(jié)速度：則可由下式解出相應的關節(jié)速度：逆雅可比矩陣逆雅可比矩陣1qJ V112ln2111llaaanqJJJqJJJq 上式中，上式中， 稱為逆雅可比矩陣，稱為逆雅可比矩陣， 為加為加給對應關節(jié)伺服系統(tǒng)的速度輸入變量。

16、給對應關節(jié)伺服系統(tǒng)的速度輸入變量。1Jiq 2021/8/1442雅可比矩陣的應用雅可比矩陣的應用1 1、分離速度控制、分離速度控制1qJ V 由上式可見，當已知手端速度向量由上式可見，當已知手端速度向量V V，可通過左，可通過左乘雅可比逆矩陣計算出機器人的關節(jié)速度向量，所以乘雅可比逆矩陣計算出機器人的關節(jié)速度向量，所以上式為運動學逆問題的速度關系式，是對機器人進行上式為運動學逆問題的速度關系式，是對機器人進行速度控制的基本關系式。速度控制的基本關系式。2021/8/1443 采用計算機控制時，把速度表示位置增量的采用計算機控制時，把速度表示位置增量的形式，故將上式寫為：形式，故將上式寫為：1

17、qJ V11nqqJvq式中式中,v,v為手為手部在基礎坐標部在基礎坐標下一個采樣周下一個采樣周期的位移期的位移( (線位線位移、角位移移、角位移) )； qq為在同一周為在同一周期內關節(jié)變量期內關節(jié)變量的增量。的增量。2021/8/1444 當要求機器人沿某軌跡運動時，當要求機器人沿某軌跡運動時，v為已知，將為已知，將它代入上式中求得關節(jié)變量增量它代入上式中求得關節(jié)變量增量q ，于是可確定各，于是可確定各關節(jié)變量值，由伺服系統(tǒng)實現(xiàn)位置控制，這就是分關節(jié)變量值，由伺服系統(tǒng)實現(xiàn)位置控制，這就是分離速度控制原理，如下圖所示。離速度控制原理，如下圖所示。v要求要求1Jv1iiv實際實際分離速度控制原

18、理分離速度控制原理2021/8/1445雅可比矩陣的應用雅可比矩陣的應用2 2、在靜力分析中的應用、在靜力分析中的應用 有些機器人的工作需要與環(huán)有些機器人的工作需要與環(huán)境接觸，并保持一定的接觸力，境接觸，并保持一定的接觸力，如右圖所示。接觸力如右圖所示。接觸力F F可表示為可表示為一個六維力向量：一個六維力向量：TxyzxyzFfff n n n12.TnT 設一個驅動器只驅動一個關節(jié)，則設一個驅動器只驅動一個關節(jié)，則n n個關節(jié)需求個關節(jié)需求n n個驅動力，可組成一個個驅動力，可組成一個n n維關節(jié)力向量維關節(jié)力向量: :2021/8/1446TTJ FT T與與F F的關系可以表示為：的關系可以表示為：2-562021/8/1447TTJ F 式中，式中， 稱為機器人稱為機器人力雅可比力雅可比，它表示在，它表示在靜止平衡狀態(tài)下，末端廣義力向關節(jié)力映射的線靜止平衡狀態(tài)下，末端廣義力向關節(jié)