機器人學-第3章微分運動學(2)

1、第第3章章 微分運動學與雅可比矩陣微分運動學與雅可比矩陣12Tnqqqqi(i=1,2,., )qniq TxyzVxyz 12xynzxyqzqJq111111(,)(,)(,)(,)(,)(,)nnnxnynznx qqy qqz qqPqqqqqq TdPVPqdtq1212121212nnxxxtnyyynzzznxxxqqqyyyqqqPJqqqqqqqqqq 在機器人學中，在機器人學中，J J是一個把關(guān)節(jié)速度向量是一個把關(guān)節(jié)速度向量 變變換為手爪相對基坐標的廣義速度向量的變換矩陣。換為手爪相對基坐標的廣義速度向量的變換矩陣。在三維空間運行的機器人，其在三維空間運行的機器人，其J J

2、陣的行數(shù)恒為陣的行數(shù)恒為6(6(沿沿/ /繞基坐標系的變量共繞基坐標系的變量共6 6個個) )；列數(shù)則為機械手含有的；列數(shù)則為機械手含有的關(guān)節(jié)數(shù)目。關(guān)節(jié)數(shù)目。 對于平面運動的機器人來說，手的廣義位置向?qū)τ谄矫孢\動的機器人來說，手的廣義位置向量量 均容易確定，可采用直接微分法求均容易確定，可采用直接微分法求J J，比較方便。比較方便。( , , )Tx yiq 對于三維空間運行的機器人則不完全適用。對于三維空間運行的機器人則不完全適用。從從三維空間運行的機器人運動學方程，可以獲得直角三維空間運行的機器人運動學方程，可以獲得直角坐標位置向量坐標位置向量 的顯式方程，因此，的顯式方程，因此，J J的

3、前三的前三行可以直接微分求得，但不可能找到方位向量行可以直接微分求得，但不可能找到方位向量 的一般表達式。找不出互相獨立的、無的一般表達式。找不出互相獨立的、無順序的三個轉(zhuǎn)角來描述方位繞直角坐標軸的連續(xù)順序的三個轉(zhuǎn)角來描述方位繞直角坐標軸的連續(xù)角運動變換是不可交換的，而對角位移的微分與對角運動變換是不可交換的，而對角位移的微分與對角位移的形成順序無關(guān)，故一般不能運用直接微分角位移的形成順序無關(guān)，故一般不能運用直接微分法來獲得法來獲得J J的后三行。因此，常用構(gòu)造性方法求雅可的后三行。因此，常用構(gòu)造性方法求雅可比比J J。( , , )Tx y z(,)Txyz xyzdd id jd kTxy

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

5、( )0TTTTTRR S PR ()0BABBTAAABOBBAARR SPR( )VJ q q( )DJ q dq112ln2111llaaanqJJJqJJJq iq LiJaiJiiqdiiqd設(shè)某時刻僅此關(guān)節(jié)運動、其余的關(guān)節(jié)靜止不動，則：設(shè)某時刻僅此關(guān)節(jié)運動、其余的關(guān)節(jié)靜止不動，則：eLiivJ q 設(shè)設(shè)b bi-1i-1為為z zi-1i-1軸上的單位矢量，利用它可將局部坐標下軸上的單位矢量，利用它可將局部坐標下的平移速度的平移速度didi轉(zhuǎn)換成基礎(chǔ)坐標下的速度：轉(zhuǎn)換成基礎(chǔ)坐標下的速度：1eiivb diiqd由于由于所以所以1LiiJb (2)(2)第第i i個關(guān)節(jié)為轉(zhuǎn)動個關(guān)節(jié)為

6、轉(zhuǎn)動關(guān)節(jié)時，關(guān)節(jié)時， 設(shè)某時刻僅此關(guān)節(jié)運設(shè)某時刻僅此關(guān)節(jié)運動，其余的關(guān)節(jié)靜止動，其余的關(guān)節(jié)靜止不動，仍然利用不動，仍然利用b bi-1i-1將將z zi-1i-1軸上的角速度軸上的角速度轉(zhuǎn)化到基礎(chǔ)坐標中去轉(zhuǎn)化到基礎(chǔ)坐標中去1iiibiiq1,ier矢量矢量 起于起于O Oi-1i-1，止于，止于O On n，所以由，所以由i i產(chǎn)生的線速度為產(chǎn)生的線速度為: :eLiivJ q1,eiievr1,LiiiieJ qr1,ier1iiib1,LiiiieJ qr11,11,()LiiiiieiieiJ qbrbr（）iiq11,LiiieJbr由于由于所以所以iiqdiiqd0aiJiiq1ia

7、iiiiJ qb1aiiJb1LiiJb11,LiiieJbr0aiJ1aiiJb10liiaiJbJ11,1iieliaiibrJJb1ib1,ier001b 1 1、用、用b b表示表示z zi-1i-1軸上的單位向量軸上的單位向量把它轉(zhuǎn)換到基礎(chǔ)坐標系中，即為把它轉(zhuǎn)換到基礎(chǔ)坐標系中，即為0121112211()().()iiiibR qR qRqb 如右圖所示。用如右圖所示。用O O、O Oi-1i-1、O On n分別表示基礎(chǔ)坐標系、分別表示基礎(chǔ)坐標系、i-1i-1號坐標及手部坐標系的原點。號坐標及手部坐標系的原點。用矢量用矢量x x表示在各自坐標系中表示在各自坐標系中的原點。的原點。T

8、(0,0,0,1)x 1,1ienirOOOO1,ier把把用齊次坐標表示用齊次坐標表示1,1,1ieierx1,T0121,xyz112211(p ,p ,p ,1)()().()1ieiieiirxA qA qAqx有上式可以確定有上式可以確定1,ier機械臂末端的速度為機械臂末端的速度為000 ,001idd 微分變換法微分變換法 ()(0)()TxzTyzTzziTzxTzyTzzdPndPdPadnoa對于轉(zhuǎn)動關(guān)節(jié)對于轉(zhuǎn)動關(guān)節(jié) 000,010iddd 000TxzTyzTzziTxTyTzdndodadd對于移動關(guān)節(jié)對于移動關(guān)節(jié) 000zzzTinoaJ()(0)()zzzTizzz

9、PnPPaJnoa對于移動關(guān)節(jié)對于移動關(guān)節(jié) 對于轉(zhuǎn)動關(guān)節(jié)對于轉(zhuǎn)動關(guān)節(jié) 000105061656126015( )zPzPzPJ qJJJzzz例：例：PUMA560PUMA560的的6 6個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)，其雅可比個關(guān)節(jié)都是轉(zhuǎn)動關(guān)節(jié)，其雅可比有有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

10、 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 ddJsc1 23 4 5636 541 23 4 5635641 23 4 5636 541 23 4 56356423 4 5636 5423 4 563564311

11、()()()()()()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 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

12、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 c例：斯坦福六自由度機器人除第三關(guān)節(jié)為移動關(guān)節(jié)例：斯坦福六自由度機器人除第三關(guān)節(jié)為移動關(guān)節(jié)外，其余外，其余5 5個關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)。此處用微分法計算個關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)。此處用微分法計算T TJ(q)J(q)224 5 64 62 5 6234 5 64 6224 5 6462 5 6234 5 64 5224 52 52

13、3 4 51224 64 62 5 6224 6462 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 sc56

14、60000TJsc6000001TJ 若給定機器人終端手抓的廣義速度向量若給定機器人終端手抓的廣義速度向量V，則可由下式解出相應(yīng)的關(guān)節(jié)速度：則可由下式解出相應(yīng)的關(guān)節(jié)速度：逆雅可比矩陣逆雅可比矩陣1qJ V112ln2111llaaanqJJJqJJJq 上式中，上式中， 稱為逆雅可比矩陣，稱為逆雅可比矩陣， 為加為加給對應(yīng)關(guān)節(jié)伺服系統(tǒng)的速度輸入變量。給對應(yīng)關(guān)節(jié)伺服系統(tǒng)的速度輸入變量。1Jiq 雅可比矩陣的應(yīng)用雅可比矩陣的應(yīng)用1 1、分離速度控制、分離速度控制1qJ V 由上式可見，當已知手端速度向量由上式可見，當已知手端速度向量V V，可通過左，可通過左乘雅可比逆矩陣計算出機器人的關(guān)節(jié)速度向量

15、，所以乘雅可比逆矩陣計算出機器人的關(guān)節(jié)速度向量，所以上式為運動學逆問題的速度關(guān)系式，是對機器人進行上式為運動學逆問題的速度關(guān)系式，是對機器人進行速度控制的基本關(guān)系式。速度控制的基本關(guān)系式。 采用計算機控制時，把速度表示位置增量的采用計算機控制時，把速度表示位置增量的形式，故將上式寫為：形式，故將上式寫為：1qJ V11nqqJvq式中式中,v,v為手為手部在基礎(chǔ)坐標部在基礎(chǔ)坐標下一個采樣周下一個采樣周期的位移期的位移( (線位線位移、角位移移、角位移) )； qq為在同一周為在同一周期內(nèi)關(guān)節(jié)變量期內(nèi)關(guān)節(jié)變量的增量。的增量。 當要求機器人沿某軌跡運動時，當要求機器人沿某軌跡運動時，v為已知，將為

16、已知，將它代入上式中求得關(guān)節(jié)變量增量它代入上式中求得關(guān)節(jié)變量增量q ，于是可確定各，于是可確定各關(guān)節(jié)變量值，由伺服系統(tǒng)實現(xiàn)位置控制，這就是分關(guān)節(jié)變量值，由伺服系統(tǒng)實現(xiàn)位置控制，這就是分離速度控制原理，如下圖所示。離速度控制原理，如下圖所示。v要求要求1Jv1iiv實際實際分離速度控制原理分離速度控制原理雅可比矩陣的應(yīng)用雅可比矩陣的應(yīng)用2 2、在靜力分析中的應(yīng)用、在靜力分析中的應(yīng)用 有些機器人的工作需要與環(huán)有些機器人的工作需要與環(huán)境接觸，并保持一定的接觸力，境接觸，并保持一定的接觸力，如右圖所示。接觸力如右圖所示。接觸力F F可表示為可表示為一個六維力向量：一個六維力向量：TxyzxyzFfff

17、 n n n12.TnT 設(shè)一個驅(qū)動器只驅(qū)動一個關(guān)節(jié)，則設(shè)一個驅(qū)動器只驅(qū)動一個關(guān)節(jié)，則n n個關(guān)節(jié)需求個關(guān)節(jié)需求n n個驅(qū)動力，可組成一個個驅(qū)動力，可組成一個n n維關(guān)節(jié)力向量維關(guān)節(jié)力向量: :TTJ FT T與與F F的關(guān)系可以表示為：的關(guān)系可以表示為：2-56TTJ F 式中，式中， 稱為機器人稱為機器人力雅可比力雅可比，它表示在，它表示在靜止平衡狀態(tài)下，末端廣義力向關(guān)節(jié)力映射的線靜止平衡狀態(tài)下，末端廣義力向關(guān)節(jié)力映射的線性關(guān)系。顯然，力雅可比是機器人速度雅可比的性關(guān)系。顯然，力雅可比是機器人速度雅可比的轉(zhuǎn)置。因此，機器人靜力學傳遞關(guān)系和速度傳遞轉(zhuǎn)置。因此，機器人靜力學傳遞關(guān)系和速度傳遞關(guān)系緊密相關(guān)。關(guān)系緊密相關(guān)。TJ由構(gòu)型和例由構(gòu)型和例2-62-6可得：可得：1 12 122 12221 12 122 12101111l sl sl sllJl cl cl cl思考題思考題1 1：右圖為三自由度機械手右圖為三自由度機械手(1)(1)用用D-HD-H方法建立各附體坐方法建立各附體坐標系