機器人技術(shù)第三章機器人運動學(xué)及其數(shù)學(xué)基礎(chǔ)

1、I. I. 機器人學(xué)機器人學(xué) 機器人學(xué)機器人學(xué) 機械電子工程機械電子工程 Dr. Kevin CraigDr. Kevin CraigI. I. 機器人學(xué)機器人學(xué) IEEE International Conference on Robotics and IEEE International Conference on Robotics and Automation (ICRA) 2010 Automation (ICRA) 2010 安克雷奇安克雷奇 文章：文章：856/2034856/2034 分會場：分會場：154154 國家：國家：4747 IEEE/RSJ International

2、 Conference on Intelligent IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2009 Robots and Systems (IROS) 2009 圣路易斯圣路易斯 文章：文章：936/1599936/1599 分會場：分會場：192192 國家：國家：5353I. I. 機器人學(xué)機器人學(xué) Technical SessionTechnical Session的主要內(nèi)容的主要內(nèi)容Human robot interactionMedical roboticsSensor

3、fusionLegged robotsUnderwater robotsManipulator motion planningCamera calibrationIntelligent transportation systemsSLAM: Features and landmarksHumanoid robot body motionMicrorobotsBiologically-inspired robotic devicesRehabilitation roboticsField roboticsGraspingNanorobotic manipulationFish-like robo

4、tParallel robot 第二章 機器人運動學(xué)及其數(shù)學(xué)基礎(chǔ)參考教材參考教材 美美 付京遜付京遜機器人學(xué)機器人學(xué) 中南大學(xué)中南大學(xué) 蔡自興蔡自興機器人學(xué)機器人學(xué) 美美 理查德理查德鮑爾鮑爾機器人操作手機器人操作手數(shù)數(shù)學(xué)學(xué)編程與控制編程與控制參考教材參考教材 美美 付京遜付京遜機器人學(xué)機器人學(xué)n 美籍華人n 普渡大學(xué)（Purdue University）電機工程專業(yè)著名教授n 4部著作、400多篇論文n 第一任國際模式識別學(xué)會會長，被譽為自動模式識別之父n 1985年去世參考教材參考教材 中南大學(xué)中南大學(xué) 蔡自興蔡自興n 中南大學(xué)教授，我國人工智能和機器人領(lǐng)域著名專家n 中國人工智能學(xué)會智能

5、機器人專委會理事長n 曾與付京遜教授一起工作過第一節(jié)第一節(jié) 引言引言 串聯(lián)機器人可以用一個開環(huán)關(guān)節(jié)鏈來建模串聯(lián)機器人可以用一個開環(huán)關(guān)節(jié)鏈來建模 由數(shù)個驅(qū)動器驅(qū)動的由數(shù)個驅(qū)動器驅(qū)動的轉(zhuǎn)動轉(zhuǎn)動或或移動移動關(guān)節(jié)串聯(lián)而成關(guān)節(jié)串聯(lián)而成 一端固定在基座上，另一端是自由的，安裝工具（末端一端固定在基座上，另一端是自由的，安裝工具（末端執(zhí)行器），用以操縱物體，或完成各種任務(wù)執(zhí)行器），用以操縱物體，或完成各種任務(wù)inoa 關(guān)節(jié)的相對運動導(dǎo)致桿件的運動，使末端執(zhí)行器定位于所需要的方位上 在一般機器人應(yīng)用問題中，人們感興趣的是：末端執(zhí)行器相對于固定參考坐標數(shù)的空間幾何描述，也就是機器人的運動學(xué)問題 機器人的運動學(xué)即

6、是研究機器人手臂末端執(zhí)行器位置和姿態(tài)與關(guān)節(jié)變量空間之間的關(guān)系運動學(xué)研究的問題運動學(xué)研究的問題Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!運動學(xué)運動學(xué)滾動接觸滾動接觸非完整控制非完整控制數(shù)學(xué)基礎(chǔ)數(shù)學(xué)基礎(chǔ)- -剛體運動剛體運動 參考文獻：參考文獻：機器人操作的數(shù)學(xué)導(dǎo)論機器人操作的數(shù)學(xué)導(dǎo)論 作者：理查德作者：理查德摩雷摩雷 李澤湘李澤湘 夏卡恩夏卡恩薩斯特里薩斯特里 翻譯：徐衛(wèi)良翻譯：徐衛(wèi)良 錢瑞明（東南大學(xué)）錢瑞明（東南大學(xué)）n 19

7、551955年丹納維特（年丹納維特（DenavitDenavit）和哈頓伯格（）和哈頓伯格（HartenbergHartenberg）提）提出了一種出了一種采用矩陣代數(shù)方法采用矩陣代數(shù)方法解決機器人的運動學(xué)問題解決機器人的運動學(xué)問題D-HD-H方方法，其數(shù)學(xué)基礎(chǔ)即是法，其數(shù)學(xué)基礎(chǔ)即是齊次變換齊次變換具有直觀的幾何意義具有直觀的幾何意義能表達動力學(xué)、計算機視覺和能表達動力學(xué)、計算機視覺和 比例變換問題比例變換問題為以后的比例變換、透視變換為以后的比例變換、透視變換 等打下基礎(chǔ)等打下基礎(chǔ)1000pppTzyyyxxxzzzyxwww第二節(jié) 數(shù)學(xué)基礎(chǔ)齊次坐標和齊次變換2.1 2.1 點和面的齊次坐標

8、點和面的齊次坐標2.1.1 2.1.1 點的齊次坐標點的齊次坐標 一般來說，一般來說，n n維空間的齊次坐標表示是一個（維空間的齊次坐標表示是一個（n+1n+1）維空間實體。有一）維空間實體。有一個特定的投影附加于個特定的投影附加于n n維空間，也可以把它看作一個附加于每個矢量的維空間，也可以把它看作一個附加于每個矢量的特定坐標特定坐標比例系數(shù)。比例系數(shù)。 引入齊次坐標的目的是為了表示幾何變換的旋轉(zhuǎn)、平移和縮放引入齊次坐標的目的是為了表示幾何變換的旋轉(zhuǎn)、平移和縮放kcj bi av zy x TwwzyxV式中i, j, k為x, y, z 軸上的單位矢量，a= , b= , c= ，w為比例

9、系數(shù) wxwywz 顯然，齊次坐標表達并不是唯一的，隨w值的不同而不同。在計算機圖學(xué)中，w 作為通用比例因子，它可取任意正值，但在機器人的運動分析中，總是取w=1 。列矩陣一個點矢： 例例11：kjiV543可以表示為： V=3 4 5 1T 或 V=6 8 10 2T 或 V=-12 -16 -20 -4T n 齊次坐標與三維直角坐標的區(qū)別齊次坐標與三維直角坐標的區(qū)別 V V點在點在OOXYZXYZ坐標系中表坐標系中表示是示是唯一唯一的（的（a a、b b、c c） 而在齊次坐標中表示可而在齊次坐標中表示可以是多值的。以是多值的。不同的表不同的表示方法代表的示方法代表的V V點在空間點在空間

10、位置上不變。位置上不變。 xyzzzxV圖2-2on 幾個特定意義的齊次坐標：幾個特定意義的齊次坐標： 0 0 0 n0 0 0 nT T坐標原點矢量的齊次坐標，坐標原點矢量的齊次坐標，n n為任為任意非零比例系數(shù)意非零比例系數(shù) 1 0 0 01 0 0 0T T 指向無窮遠處的指向無窮遠處的OXOX軸軸 0 1 0 00 1 0 0T T 指向無窮遠處的指向無窮遠處的OYOY軸軸 0 0 1 00 0 1 0T T 指向無窮遠處的指向無窮遠處的OZOZ軸軸 0 0 0 00 0 0 0T T 沒有意義沒有意義n 2個常用的公式：zzyyxxbabababakbabajbabaibababbb

11、aaakjibaxyyxzxxzyzzyzyxzyx)()()(點乘:叉乘:2.1.2 2.1.2 平面的齊次坐標平面的齊次坐標 平面齊次坐標由平面齊次坐標由行矩陣行矩陣P=a b c d P=a b c d 來表示來表示 當(dāng)點當(dāng)點v=x y z wv=x y z wT T處于平面處于平面P P內(nèi)時，矩陣乘積內(nèi)時，矩陣乘積PV=0PV=0，或記為，或記為 0dwczbyaxwzyxdcbaPV與點矢 相仿，平面 也沒有意義 T00000000n 點和平面間的位置關(guān)系點和平面間的位置關(guān)系設(shè)一個平行于x、y軸，且在z軸上的坐標為單位距離的平面P可以表示為： 或 有： PV= 1100P2200P

12、v0 v0 v0 點在平面下方點在平面上點在平面上方例如：點 V=10 20 1 1T 必定處于此平面內(nèi)，而點 V=0 0 2 1T處于平 P 的上方，點V=0 0 0 1T處于P平面下方，因為：0112010101000 0 1120011000 -110001-1002.2 2.2 旋轉(zhuǎn)矩陣及旋轉(zhuǎn)齊次變換旋轉(zhuǎn)矩陣及旋轉(zhuǎn)齊次變換2.2.1 2.2.1 旋轉(zhuǎn)矩陣旋轉(zhuǎn)矩陣 設(shè)固定參考坐標系直角坐標為Oxyz，動坐標系為Ouvw，研究旋轉(zhuǎn)變換情況。xyzwvuPo(O)圖2-3 初始位置時，動靜坐標系重合，O、O 重合，如圖。各軸對應(yīng)重合，設(shè)P點是動坐標系Ouvw中的一點，且固定不變。則P點在Ou

13、vw中可表示為： wwvvuuuvwkPjPiPP 、 、 為坐標系Ouvw的單位矢量，則P點在oxyz中可表示為： uivjwkzzyyxxxyzkPjPiPPxyzuvwPP 當(dāng)動坐標系Ouvw繞O點回轉(zhuǎn)時，求P點在固定坐標系oxyz中的位置 yzxo(O)uvwPPwPvPu圖2-4已知：P點在Ouvw中是不變的仍然成立，由于Ouvw回轉(zhuǎn)，則： wwvvuuuvwkPjPiPPxwwvvuuxuvwxikPjPiPiP)(PywwvvuuyuvwyjkPjPiPjP)(PzwwvvuuzuvwzkkPjPiPkP)(P用矩陣表示為: wvwzvzzwvyywxvxxzyxPPPkkjki

14、kkjjjijkijiiiPPPy（2-7） uvwxyzwzvzzwvyywxvxxPRpkkjkikkjjjijkijiii:R y則旋轉(zhuǎn)矩陣為：定義反過來： xyzuvwPRP1RRRdet*1T1RRRdet是正交矩陣，的行列式，為的伴隨矩陣，為RRRR2.2.2 2.2.2 旋轉(zhuǎn)齊次變換旋轉(zhuǎn)齊次變換 用齊次坐標變換來表示式（2-7） 110000001wvuzyxPPPRPPP1100000011zyxwvuPPPRPPP2.2.3 2.2.3 三個基本旋轉(zhuǎn)矩陣和合成旋轉(zhuǎn)矩陣三個基本旋轉(zhuǎn)矩陣和合成旋轉(zhuǎn)矩陣 三個基本旋轉(zhuǎn)矩陣 ),(xR即動坐標系 求 的旋轉(zhuǎn)矩陣，也就是求出坐標系 中各

15、軸單位矢量 在固定坐標系中各軸的投影分量，很容易得到在兩個坐標系重合時，有：角，軸轉(zhuǎn)動繞，XOvwOvwOwvkji,Oxyz),(xR100010001Rwzvzzwvyywxvxxkkjkikkjjjijkijiiiy)R(x,xyzouvwUVWO圖2-5ssin0sincos0001coiiux方向余弦陣同理： cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssin0sincos0001)R(x,co三個基本旋轉(zhuǎn)矩陣: xyzouvwUWOxyzouvwUWOvn 合成旋轉(zhuǎn)矩陣:例1：在動坐標中有一固定點 ，相對固定參考坐標系 做如下運動：

16、 R（x, 90）； R(z, 90)； R(y,90)。求運動后點 在固定參考坐標系 下的位置。 TuvwPo1321OxyzuvwPoOxyz解1：用畫圖的簡單方法 解2：用分步計算的方法 R（x, 90） R（z, 90） R（y, 90） 123113211000001001-000001P12131231100001000001001-0 P1312121310000001-00100100 P（2-14） （2-15） （2-16） 上述計算方法非常繁瑣，可以通過一系列計算得到上述結(jié)果。將式（2-14）（2-15）（2-16）聯(lián)寫為如下形式： 11000133wvuzyxPPPRP

17、PPR3x3為二者之間的關(guān)系矩陣，我們令： ),(),(),RR33xRzRy（定義1： 當(dāng)動坐標系 繞固定坐標系 各坐標軸順序有限次轉(zhuǎn)動時，其合成旋轉(zhuǎn)矩陣為各基本旋轉(zhuǎn)矩陣依旋轉(zhuǎn)順序左乘。注意：旋轉(zhuǎn)矩陣間不可以交換 uvwOOxyzn 平移齊次變換矩陣1000100010001c) b (a TransHcba注意：平移矩陣間可以交換， 平移和旋轉(zhuǎn)矩陣間不可以交換 zyxoowuvabc2.2.4 2.2.4 相對變換相對變換 舉例說明：例1：動坐標系0起始位置與固定參考坐標系0重合,動坐標系0做如下運動：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩陣 解1：用畫圖的

18、方法： ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) v解2：用計算的方法 根據(jù)定義1，我們有：1000701030014100 )R(Z,90 )90 R(y, 7) , 3- ,Trans(4T 以上均以固定坐標系多軸為變換基準，因此矩陣左乘。如果我們做如下變換，也可以得到相同的結(jié)果： 例2：先平移Trans (4,-3,7)；繞當(dāng)前 軸轉(zhuǎn)動90； 繞當(dāng)前 軸轉(zhuǎn)動90；求合成旋轉(zhuǎn)矩陣。 vw （2-20）解1：用畫圖的方法 zyxo(o)vwuzyxoowuvozyxowvuxyzoowuv解2：用計算的方法 1000701030014100)R(Z,90

19、)90 R(y, 7) , 3- ,Trans(4Too（2-21）式（2-20）和式（2-21）無論在形式上，還是在結(jié)果上都是一致的。因此我們有如下的結(jié)論：動坐標系在固定坐標系中的齊次變換有2種情況：定義1：如果所有的變換都是相對于固定坐標系中各坐標軸旋轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。定義2：如果動坐標系相對于自身坐標系的當(dāng)前坐標軸旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為相對變換。 結(jié)果均為動坐標系在固定坐標中的位姿（位置+姿態(tài)）。相對于固定坐標系，軸。軸相當(dāng)于軸，軸相對于軸，軸相當(dāng)于ZYXwv 也就是說，動坐標系繞自身坐標軸做齊次變換，要達到繞固定坐標系相等的結(jié)果，就應(yīng)該用相反的順序。

20、右乘的意義：右乘的意義： 機器人用到相對變換的機器人用到相對變換的時候比較多時候比較多 例如機械手抓一個杯子，例如機械手抓一個杯子，如右圖所示，手爪需要如右圖所示，手爪需要轉(zhuǎn)動一個角度才抓的牢，轉(zhuǎn)動一個角度才抓的牢，相對于固定坐標系表達相對于固定坐標系表達太麻煩，可以直接根據(jù)太麻煩，可以直接根據(jù)手爪的坐標系表示手爪的坐標系表示 但也要知道在但也要知道在O O中的位中的位姿，就用右乘的概念。姿，就用右乘的概念。 xyzoH2.2.5 2.2.5 繞通過原點的任意軸旋轉(zhuǎn)的齊次變換繞通過原點的任意軸旋轉(zhuǎn)的齊次變換 有時動坐標系有時動坐標系O O 可能繞過原點可能繞過原點O O的分量分別為的分量分別為

21、r rx x、r ry y、r rz z的的任意單位矢量任意單位矢量r r 轉(zhuǎn)動轉(zhuǎn)動角。角。研究這種轉(zhuǎn)動的好處是可用研究這種轉(zhuǎn)動的好處是可用O O 繞某軸繞某軸r r 的一次轉(zhuǎn)動代替繞的一次轉(zhuǎn)動代替繞O O各坐標軸的數(shù)次轉(zhuǎn)動各坐標軸的數(shù)次轉(zhuǎn)動為推導(dǎo)此旋轉(zhuǎn)矩陣，可作下述為推導(dǎo)此旋轉(zhuǎn)矩陣，可作下述5 5步變換：步變換：1.1.繞繞X X 軸轉(zhuǎn)軸轉(zhuǎn)角，角， 使使r r 軸處于軸處于XZXZ平面內(nèi)平面內(nèi)2.2.繞繞Y Y 軸轉(zhuǎn)軸轉(zhuǎn)- -角，使角，使r r 軸與軸與OZOZ軸重合軸重合3.3.繞繞OZOZ軸轉(zhuǎn)動軸轉(zhuǎn)動角角4.4.繞繞Y Y 軸轉(zhuǎn)軸轉(zhuǎn)角角5.5.繞繞X X 軸轉(zhuǎn)軸轉(zhuǎn)- -角角XYZrxry

22、rzABCDBO51243rA由上圖容易求出：2z2yyrrrsin2z2yzrrrcosxxrrrrOCsin2z2y2z2yrrrrrOBCBcos由定義1和定義2，上述5次旋轉(zhuǎn)的合成旋轉(zhuǎn)矩陣為：cossin0sin-cos0001cos0sin010sin-0cos1000cossin0sin-coscos0sin-010sin0coscossin0sincos0001RRRRRR,x,y, z,y,x, r（2-25）XYZrxryrzABCDBO51243rA帶入式（2-25），得cos)cos(1rsinr)cos(1rrsinr)cos(1rrsinr)cos(1rrsinr)c

23、os(1rrcos)cos(1rsinr)cos(1rrsinr)cos(1rrcos)cos(1rR2zxzyyzxxzyyzx2yzyxzyx2x, r由該式可以推出3個基本旋轉(zhuǎn)矩陣2.2.6 2.2.6 齊次變換矩陣的幾何意義齊次變換矩陣的幾何意義 設(shè)，有一個手爪，即動坐標系O，已知， 初始位置重合，那么O在O中的齊次坐標變換為： ，如果手爪轉(zhuǎn)了一個角度， 則：111cbao1000100010001T 1111cba1000pppTzyyyxxxzzzyxwwwT反映了O在O中的位置和姿態(tài)，即表示了該坐標系原點和各坐標軸單位矢量在固定坐標系中的位置和姿態(tài)。該矩陣可以由4個子矩陣組成，寫

24、成如下形式：比例系數(shù)透視矩陣位置矢量旋轉(zhuǎn)矩陣11311333wfPRTzzzyyyxxxwww33R為姿態(tài)矩陣（旋轉(zhuǎn)矩陣），表示動坐標系O在固定參考坐標系O中的姿態(tài)，即表示O各坐標軸單位矢量在O各軸上的投影 為位置矢量矩陣，代表動坐標系O坐標原點在固定參考坐標系O中的位置 TzyxpppP13為透視變換矩陣，在視覺中進行圖像計算，一般置為0 00031f為比例系數(shù) 1 11w如果需要求解O在O中的位置和姿態(tài)，此時的齊次變換矩陣為 ，即求逆矩陣： 1T1000-R-TTT1 -33T1pwpvp）（）（）（ kpjpippzyxkjizyxkvjvivvzyxkwjwiwwzyx其中：這些式子以

25、后經(jīng)常遇到，在機器人計算中，所要求的就是齊次變換矩陣 2.2.7 2.2.7 透鏡成像的齊次變換透鏡成像的齊次變換 pp: Px1PP x1 1()1ppppTTy zy zypzpzpfypzpzpypfypfzpxpypfzpxpypypfypfypfypypypfypypffxpxpy 以光心為原點O，光軸與y軸重合，P為物點，用齊次坐標表示求 的齊次坐標，即求根據(jù)三角形相似原理：注意是負值， 是正值，所以實際上為相減關(guān)系又有設(shè)111000010000101110001ypzpypzppypzpfffxpxpxpypypypTfzpzpzpypf用矩陣表示：zyPypfozpfpzPpy

26、pp: Px1PP x1 1()1ppppTTy zy zypzpzpfypzpzpypfypfzpxpypfzpxpypypfypfypfypypypfypypffxpxpy 以光心為原點O，光軸與y軸重合，P為物點，用齊次坐標表示求的齊次坐標，即求根據(jù)三角形相似原理：注意是負值， 是正值，所以實際上為相減關(guān)系又有設(shè)111000010000101110001ypzpypzppypzpfffxpxpxpypypypTfzpzpzpypf用矩陣表示： 因此，進行機器人運動學(xué)計算時，不能省略透視矩陣，有因此，進行機器人運動學(xué)計算時，不能省略透視矩陣，有攝像頭時，透視矩陣為攝像頭時，透視矩陣為 0

27、- 00 - 0，沒有攝像頭時為，沒有攝像頭時為0 0 0 0 0 0 。f11010010000100001T1T11pfzyxfyzyxzyxfpppfpppppp用矩陣表示：知識點：知識點： 1.1. 點和面的齊次坐標和齊次變換點和面的齊次坐標和齊次變換2.2. 三個基本旋轉(zhuǎn)矩陣三個基本旋轉(zhuǎn)矩陣3.3. 絕對變換：如果所有的變換都是相對于固定坐標系中各坐絕對變換：如果所有的變換都是相對于固定坐標系中各坐標軸旋轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。標軸旋轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。4.4. 相對變換：如果動坐標系相對于自身坐標系的當(dāng)前坐標軸相對變換：如果動坐標系相對于自身坐標系的當(dāng)前

28、坐標軸旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為相對變換。旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為相對變換。5.5. 繞任意軸旋轉(zhuǎn)：繞任意軸旋轉(zhuǎn)：5 5步順序步順序6.6. 透視變換透視變換知識點：知識點： 三 個 基 本 旋三 個 基 本 旋轉(zhuǎn)矩陣轉(zhuǎn)矩陣cos0sin010sin0cos)y,R（1000cossin0sin-cos)z,R （ssin0sincos0001)R(x,co例題1：O與O初始重合，O作如下運動：繞Z軸轉(zhuǎn)動30 ；繞X軸轉(zhuǎn)動60 ；繞Y軸轉(zhuǎn)動90 。求T。 100001000030cos30sin0030sin30cosR11000060cos60sin0060sin6

29、0cos000012R1000090cos090sin0010090sin090cos3R1000002/12/302/34/34/102/14/34/3123RRRT例題2：O與O初始重合，O作如下運動：繞X軸轉(zhuǎn)動90；繞w軸轉(zhuǎn)動90；繞Y軸轉(zhuǎn)動90。求 T；改變旋轉(zhuǎn)順序，如何旋轉(zhuǎn)才能獲得相同的結(jié)果。 1000090cos90sin0090sin-09cos00001R1100001000090cos90sin0090sin90cos2R1000090cos090sin0010090sin090cos3R1000001001000001213RRRT解： 解： 繞Z（w）軸轉(zhuǎn)動90； 繞X軸

30、轉(zhuǎn)動90； 繞Y軸轉(zhuǎn)動90。 例題3： 矢量 在O中表示為 ，O相對于O的奇次變換為： Pkjip2230100011002000110010T oo中的矢量在求此時，軸平移的沿軸轉(zhuǎn)的繞當(dāng)中的矢量在求的位置和姿態(tài)在畫出：求0 20X0 90Y0 0)3 0 2) 00 1) O00ppp解：1） zxyuwvoo解：2） 13238T00ppo解：3） ， ， 1000090cos090sin0010090sin090cosR110000100001020001T 1r1000100102000121100TRT11orT1823231223100010010200012110100pTp例題

31、4： 如圖所示，1）寫出 、 、 、 ；2）求 1T21T32T43T 40 T 100011-00301-03.5-001T11000101-03001-0100T211000000155354-0054530 T32100001-0000103.5001- T 43解：1） o0 x0y0z433.511o1x1y1z2o2x2y2z3o3x3y3z4o4x4y4z解2）：根據(jù)定義2，繞自身旋轉(zhuǎn)，右乘100050.6-0.8-000.8-0.600001- T T T TT 433221140 習(xí)題1：O與O初始重合，O作如下運動：繞z軸轉(zhuǎn)動90；繞v軸轉(zhuǎn)動90；繞x軸轉(zhuǎn)動90。求 T；改

32、變旋轉(zhuǎn)順序，如何旋轉(zhuǎn)才能獲得相同的結(jié)果。 習(xí)題2： 已知齊次變換矩陣 要求R（f,), 求f和值1000000101000010H第三章第三章 機器人運動學(xué)機器人運動學(xué) n 機器人運動學(xué)主要是把機器人機器人運動學(xué)主要是把機器人相對于固定參考相對于固定參考系系的運動作為的運動作為時間的函數(shù)時間的函數(shù)進行分析研究，而不進行分析研究，而不考慮引起這些運動的力和力矩考慮引起這些運動的力和力矩n 也就是要把機器人的也就是要把機器人的空間位移空間位移解析地表示為解析地表示為時時間的函數(shù)間的函數(shù)，特別是研究機器人，特別是研究機器人關(guān)節(jié)變量空間和關(guān)節(jié)變量空間和機器人末端執(zhí)行器位置和姿態(tài)之間機器人末端執(zhí)行器位置

33、和姿態(tài)之間的關(guān)系的關(guān)系n 本章將討論機器人運動學(xué)幾個具有實際意義的本章將討論機器人運動學(xué)幾個具有實際意義的基本問題?；締栴}。 3.1 3.1 機器人運動學(xué)所討論的問題機器人運動學(xué)所討論的問題 3.1.1 3.1.1 研究的對象研究的對象 機器人在基本機構(gòu)形式上分為兩種，一種是關(guān)節(jié)式串機器人在基本機構(gòu)形式上分為兩種，一種是關(guān)節(jié)式串聯(lián)機器人，另外一種是并聯(lián)機器人，如圖：聯(lián)機器人，另外一種是并聯(lián)機器人，如圖： PUMA560HexapodFanuc manipulator1972 Victor Scheinman在Unimation公司為通用；1980Westinghouse收購；1988Stub

34、li收購；Nokia Robotics在80年代賣出1500余臺PUMA系統(tǒng)； Nokia的 Robotics division1990年賣出。運動學(xué)研究的問題運動學(xué)研究的問題Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!研究的問題研究的問題: :n 運動學(xué)正問題運動學(xué)正問題-已知桿件幾何參數(shù)和關(guān)節(jié)角矢量，求操已知桿件幾何參數(shù)和關(guān)節(jié)角矢量，求操作機末端執(zhí)行器相對于固定參考作標的位置和姿態(tài)（作機末端執(zhí)行器相對于固定參考作標的位置和姿態(tài)

35、（齊齊次變換問題次變換問題）。）。n 運動學(xué)逆問題運動學(xué)逆問題-已知操作機桿件的幾何參數(shù)，給定操作已知操作機桿件的幾何參數(shù)，給定操作機末端執(zhí)行器相對于參考坐標系的期望位置和姿態(tài)（位機末端執(zhí)行器相對于參考坐標系的期望位置和姿態(tài)（位姿），操作機能否使其末端執(zhí)行器達到這個預(yù)期的位姿？姿），操作機能否使其末端執(zhí)行器達到這個預(yù)期的位姿？如能達到，那么操作機有幾種不同形態(tài)可以滿足同樣的如能達到，那么操作機有幾種不同形態(tài)可以滿足同樣的條件？條件？運 動 學(xué) 正 問 題關(guān) 節(jié) 角桿 件 參 數(shù)末 端 執(zhí) 行 器運 動 學(xué) 正 問 題關(guān) 節(jié) 角桿 件 參 數(shù)逆3.2 3.2 機器人桿件，關(guān)節(jié)和它們的參機器人桿件

36、，關(guān)節(jié)和它們的參數(shù)數(shù) 3.2.1 3.2.1 桿件，關(guān)節(jié)桿件，關(guān)節(jié)n操作機由一串用轉(zhuǎn)動或平移（棱操作機由一串用轉(zhuǎn)動或平移（棱柱形）關(guān)節(jié)連接的剛體（桿件）柱形）關(guān)節(jié)連接的剛體（桿件）組成組成n每一對關(guān)節(jié)桿件構(gòu)成一個每一對關(guān)節(jié)桿件構(gòu)成一個關(guān)節(jié)關(guān)節(jié)自由度自由度，因此，因此N N個自由度的操作個自由度的操作機就有機就有N N對關(guān)節(jié)對關(guān)節(jié)- -桿件。桿件。n0 0號桿件（一般不把它當(dāng)作機器號桿件（一般不把它當(dāng)作機器人的一部分）固聯(lián)在機座上，通人的一部分）固聯(lián)在機座上，通常在這里建立一個固定參考坐標常在這里建立一個固定參考坐標系，最后一個桿件與工具相連系，最后一個桿件與工具相連n關(guān)節(jié)和桿件均由底座向外順序

37、排關(guān)節(jié)和桿件均由底座向外順序排列，每個桿件最多和另外兩個桿列，每個桿件最多和另外兩個桿件相聯(lián)，不構(gòu)成閉環(huán)。件相聯(lián)，不構(gòu)成閉環(huán)。 關(guān)節(jié)：關(guān)節(jié)：n一般說來，兩個桿件間是用一般說來，兩個桿件間是用低付低付相聯(lián)的相聯(lián)的n只可能有只可能有6 6種低付關(guān)節(jié)：種低付關(guān)節(jié)：旋轉(zhuǎn)旋轉(zhuǎn)（轉(zhuǎn)動）、（轉(zhuǎn)動）、棱柱棱柱（移動）、（移動）、圓柱形圓柱形、球形球形、螺旋螺旋和和平面平面，其中只有，其中只有旋轉(zhuǎn)和棱柱形旋轉(zhuǎn)和棱柱形關(guān)關(guān)節(jié)是串聯(lián)機器人操作機常見的，各種低副形狀如下圖所節(jié)是串聯(lián)機器人操作機常見的，各種低副形狀如下圖所示：示：旋轉(zhuǎn)旋轉(zhuǎn)棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面平面AiAi+1Ai-1 桿件參數(shù)的定

38、義桿件參數(shù)的定義 和和n li AA，由運動學(xué)的觀點來看，桿件的作用僅在于它能保由運動學(xué)的觀點來看，桿件的作用僅在于它能保持其兩端關(guān)節(jié)間的持其兩端關(guān)節(jié)間的結(jié)構(gòu)形態(tài)結(jié)構(gòu)形態(tài)不變。這種形態(tài)由兩不變。這種形態(tài)由兩個參數(shù)決定，一是桿件的長度個參數(shù)決定，一是桿件的長度 li，一個是桿件的，一個是桿件的扭轉(zhuǎn)角扭轉(zhuǎn)角 iAiAi+1iiliili 桿件參數(shù)的定義桿件參數(shù)的定義 和和n L和和L 在在A軸線上軸線上的交點之間的距離的交點之間的距離n L和和L 之間的夾角，之間的夾角，由由L 轉(zhuǎn)向轉(zhuǎn)向L，由右手，由右手定則決定正負，對于定則決定正負，對于旋轉(zhuǎn)關(guān)節(jié)它是個變量旋轉(zhuǎn)關(guān)節(jié)它是個變量 確定桿件確定桿件相對

39、位置關(guān)系相對位置關(guān)系，由另外，由另外2個參數(shù)決定，一個是桿個參數(shù)決定，一個是桿件的偏移量件的偏移量 ，一個是桿件的回轉(zhuǎn)角，一個是桿件的回轉(zhuǎn)角 iidiidiAiAi+1iilid1iliAi-1id移動關(guān)節(jié)桿件參數(shù)的定義移動關(guān)節(jié)桿件參數(shù)的定義n 確定桿件的結(jié)構(gòu)形態(tài)的確定桿件的結(jié)構(gòu)形態(tài)的2個參數(shù)個參數(shù)Li與與i與旋轉(zhuǎn)關(guān)節(jié)是一樣的。與旋轉(zhuǎn)關(guān)節(jié)是一樣的。確定桿件相對位置關(guān)系的確定桿件相對位置關(guān)系的2個參數(shù)則相反。這里個參數(shù)則相反。這里i為常數(shù)，為常數(shù)，di為變量。為變量。n 上述上述4個參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位個參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位置關(guān)系，在轉(zhuǎn)動關(guān)節(jié)中，置

40、關(guān)系，在轉(zhuǎn)動關(guān)節(jié)中，Li, i, di是固定值，是固定值，i是變量。在是變量。在移動關(guān)節(jié)中，移動關(guān)節(jié)中，Li, i, i是固定值，是固定值， di 是變量。是變量。3.3 機器人關(guān)節(jié)坐標系的建立機器人關(guān)節(jié)坐標系的建立o oO Oo on n- -1 1nXn 對于每個桿件都可以在關(guān)節(jié)軸處建立一個正規(guī)的笛卡兒坐對于每個桿件都可以在關(guān)節(jié)軸處建立一個正規(guī)的笛卡兒坐標系（標系（xi, yi, zi），（），（i=1, 2, , n），），n是自由度數(shù)，再加是自由度數(shù)，再加上基座坐標系，一共有（上基座坐標系，一共有（n+1）個坐標系。）個坐標系。n 基座坐標系基座坐標系 定義為定義為0號坐標系（號坐標系

41、（x0, y0, z0）,它也是機它也是機器人的慣性坐標系，器人的慣性坐標系，0號坐標系在基座上的位置和方向可號坐標系在基座上的位置和方向可任選，但任選，但 軸線必須與關(guān)節(jié)軸線必須與關(guān)節(jié)1的軸線重合，位置和方向可的軸線重合，位置和方向可任選；任選；n 最后一個坐標系（最后一個坐標系（n關(guān)節(jié)），可以設(shè)在手的任意部位，但關(guān)節(jié)），可以設(shè)在手的任意部位，但必須保證必須保證 與與 垂直。垂直。n 機器人關(guān)節(jié)坐標系的建立主要是為了描述機器人各桿件和終機器人關(guān)節(jié)坐標系的建立主要是為了描述機器人各桿件和終端之間的相對運動，對建立運動方程和動力學(xué)研究是基礎(chǔ)性端之間的相對運動，對建立運動方程和動力學(xué)研究是基礎(chǔ)性的

42、工作。的工作。n 為了描述機器人各桿件和終端之間轉(zhuǎn)動或移動關(guān)系，為了描述機器人各桿件和終端之間轉(zhuǎn)動或移動關(guān)系，Denavit和和Hartenberg于于1955年提出了一種為運動鏈中每個桿件建立年提出了一種為運動鏈中每個桿件建立附體坐標系的矩陣方法附體坐標系的矩陣方法（D-H方法）方法） ，建立原則如下：，建立原則如下： D-H關(guān)節(jié)坐標系建立原則關(guān)節(jié)坐標系建立原則u右手坐標系右手坐標系u原點原點Oi：設(shè)在：設(shè)在Li與與Ai+1軸線的交點上軸線的交點上 uZi軸軸： 與與Ai+1關(guān)節(jié)軸重合，指向任意關(guān)節(jié)軸重合，指向任意 uXi軸軸： 與公法線與公法線Li重合，指向沿重合，指向沿Li由由Ai軸線指

43、向軸線指向Ai+1軸線軸線 uYi軸軸： 按右手定則按右手定則 關(guān)節(jié)坐標系的建立原則關(guān)節(jié)坐標系的建立原則AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiyion 原點原點Oi：設(shè)在：設(shè)在Li與與Ai+1軸線的交點上軸線的交點上 n Zi軸：與軸：與Ai+1關(guān)節(jié)軸關(guān)節(jié)軸重合，指向任意重合，指向任意 n Xi軸：與公法線軸：與公法線Li重合，指向沿重合，指向沿Li由由Ai軸線指向軸線指向Ai+1軸線軸線 n Yi軸：按右手定則軸：按右手定則 沿 xi 軸， zi-1 軸與 xi 軸交點到 0i 的距離 繞 xi 軸，由 zi-1 轉(zhuǎn)向zi 沿 zi-1 軸，zi-1 軸和

44、xi 交點至0i 1 坐標系原點的距離 繞 zi-1 軸，由 xi-1轉(zhuǎn)向 xi 兩種特殊情況兩種特殊情況n 兩軸相交，怎么建立坐兩軸相交，怎么建立坐標系？標系？ 0iAi與與Ai+1關(guān)節(jié)軸線的交關(guān)節(jié)軸線的交點；點； ZiAi+1軸線；軸線； XiZi和和Zi-1構(gòu)成的平面的構(gòu)成的平面的法線法線 ； Yi右手定則；右手定則； i-1ii-1i AiA Ai i+ +1 1o oi iz zi i- -1 1z zi ix xi iy yi in兩軸平行，怎么建立坐標系兩軸平行，怎么建立坐標系(Ai與與Ai+1平行平行)？先建立先建立 0i-1然后建立然后建立0i+1最后建立最后建立 0i i-

45、1i-1O OD D注意：注意： 由于由于Ai和和Ai+1平行，平行， 所以公法線任意點所以公法線任意點 在在A點位置；點位置； 按照先前的定義，按照先前的定義，di為為Oi-1點和點和A點之間的距離，點之間的距離，di+1為為B點和點和C點間點間的距離，這樣設(shè)定可以的，但我們可以變更一下，將的距離，這樣設(shè)定可以的，但我們可以變更一下，將0i點放在點放在C點，點，定義定義Oi在在Li+1和和Ai+1軸的交點上，這樣使軸的交點上，這樣使di+1=0使計算簡便，此時使計算簡便，此時di=Ai-1AiAi+1Ai+2li-1oi-1xi-1yi-1zi-1ABDCoi（ xi）（ yi）zixiyi

46、oi+1xi+1yi+1zi+1di+1li+1di 相鄰相鄰關(guān)節(jié)坐標系間的齊次變換過程關(guān)節(jié)坐標系間的齊次變換過程 機器人運動學(xué)正解機器人運動學(xué)正解1. 將將xi-1軸繞軸繞 zi-1 軸轉(zhuǎn)軸轉(zhuǎn) i 角角度，將其與度，將其與xi軸平行；軸平行；2. 沿沿 zi-1軸平移距離軸平移距離 di ，使使 xi-1 軸與軸與 xi 軸重合；軸重合；3. 沿沿 xi 軸平移距離軸平移距離 Li，使兩坐標系原點及使兩坐標系原點及x軸軸重合；重合；4. 繞繞 xi 軸轉(zhuǎn)軸轉(zhuǎn) i 角度，兩角度，兩坐標系完全重合坐標系完全重合),(),(),(),(A111iiiiransiiransiiiixRlxTdZTZ

47、R 根據(jù)上述坐標系建立原則，用下列旋轉(zhuǎn)和位移我們根據(jù)上述坐標系建立原則，用下列旋轉(zhuǎn)和位移我們可以建立相鄰的可以建立相鄰的 Oi-1 和和 Oi 坐標系之間的關(guān)系坐標系之間的關(guān)系A(chǔ)iAi+1iilid1iliAi-11iz1ix1iy1ioizixiyio 機器人的運動學(xué)正解方程機器人的運動學(xué)正解方程001112iiiTAAA D-H變換矩陣變換矩陣iiA1100010000100001id1000010000cossin00sincosiiii100001000010001il10000cossin00sincos00001iiii1000cossin0sincossincoscossinco

48、ssinsinsincoscosiiiiiiiiiiiiiiiiidll=機械手的坐標變換圖如圖所示，機械手的末端（即連桿坐標系機械手的坐標變換圖如圖所示，機械手的末端（即連桿坐標系i）相對于基座坐標系相對于基座坐標系0的描述用的描述用 oTi 表示，即：表示，即： 0zA1A2A3A4A5A60EX0T61T62T63T64T65T6 機械手的坐標變換圖 機器人的運動學(xué)正解方程機器人的運動學(xué)正解方程001112iiiTAAA舉例：舉例：StanfordStanford機器人機器人A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y4z4x4O4z5y5x5O53

49、4545,0o o odd重重合合d3z6x6y6O6d6z0y0 x0O0n 為右手坐標系為右手坐標系n 原點原點Oi： Ai與與Ai+1關(guān)節(jié)軸線的交點關(guān)節(jié)軸線的交點n Zi軸：與軸：與Ai+1關(guān)節(jié)軸關(guān)節(jié)軸重合，指向任意重合，指向任意 n Xi軸：軸： Zi和和Zi-1構(gòu)成構(gòu)成的面的法線的面的法線n Yi軸：按右手定則軸：按右手定則 Li 沿沿 xi 軸，軸， zi-1 軸與軸與 xi 軸交點到軸交點到 0i 的距離的距離i 繞繞 xi 軸，由軸，由 zi-1 轉(zhuǎn)向轉(zhuǎn)向zidi 沿沿 zi-1 軸，軸，zi-1 軸和軸和 xi 交點至交點至0i 1 坐標系原坐標系原 點的距離點的距離i 繞繞

50、 zi-1 軸，由軸，由 xi-1轉(zhuǎn)向轉(zhuǎn)向 xi解：解：3.4 3.4 例題例題試求立方體中心在機座坐標系試求立方體中心在機座坐標系0 0中的位置中的位置該手爪從上方把物體抓起，同時手爪的開合方向與物體的該手爪從上方把物體抓起，同時手爪的開合方向與物體的Y Y軸同向，軸同向，那么，求手爪相對于那么，求手爪相對于0 0的姿態(tài)是什么？的姿態(tài)是什么？ 在機器人工作臺上加裝一電視攝像機，攝像機可見到固聯(lián)在機器人工作臺上加裝一電視攝像機，攝像機可見到固聯(lián)著著6DOF關(guān)節(jié)機器人的機座坐標系原點，它也可以見到被操作關(guān)節(jié)機器人的機座坐標系原點，它也可以見到被操作物體（立方體）的中心，如果在物體中心建一局部坐標

51、系，則物體（立方體）的中心，如果在物體中心建一局部坐標系，則攝像機所見到的這個物體可由齊次變換矩陣攝像機所見到的這個物體可由齊次變換矩陣T1來表示，如果攝來表示，如果攝像機所見到的機座坐標系為矩陣像機所見到的機座坐標系為矩陣T2表示。表示。1000101-002001-010-001T100091-00100011010T21xyz解解1 1： T T 21物機機攝物攝求，已知TTT TT 11 -2）（有：物攝攝機物機TTT 100091-00100011010 1000101-002001-0100011000110010001-11010 O物根據(jù)T1畫出O機根據(jù)T2畫出因此物體位于機座

52、坐標系的（因此物體位于機座坐標系的（11，10，1）T處，它的處，它的X，Y，Z軸分別與機座坐標系的軸分別與機座坐標系的-Y，X，Z軸平行。軸平行。 xyzy機z物y物x物z機oO機O物解解2 2：向重合手爪開合方向與物體ya:Ts001有方向相反方向物體的從上向下抓，指出手爪zab:Ta 100則有Tkjikjiasnc01000100001:1-00001010因此：姿態(tài)矩陣為重合時與物體中心當(dāng)手爪中心100011-001000111010 T物機OsnayzxX機手爪機實際要求Tpzazsznzpyaysynypxaxsxnx1000 工作空間工作空間n 工作空間工作空間: : 末端操作

53、手可以到達的空間位置集合末端操作手可以到達的空間位置集合n 如何獲得工作空間如何獲得工作空間: : 利用正運動學(xué)模型利用正運動學(xué)模型, ,改變關(guān)節(jié)改變關(guān)節(jié)變量值變量值n 可達空間可達空間: : 末端操作手可以至少以一個姿態(tài)到達的末端操作手可以至少以一個姿態(tài)到達的空間位置集合空間位置集合n 靈活空間靈活空間: : 末端操作手可以以任何姿態(tài)到達的空間末端操作手可以以任何姿態(tài)到達的空間位置集合位置集合如何確定可達空間如何確定可達空間? ?首先，首先，令令 3 3變變化化 示例示例: : 平面平面 3 3連桿機器人連桿機器人123112123123112123123123123123coscoscos

54、sinsinsin , xlllylllllllll 3種最常見的歐拉角類型種最常見的歐拉角類型步步1步步2步步3類型類型1繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OU 軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OW軸轉(zhuǎn)軸轉(zhuǎn)角角類型類型2繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OV 軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OW軸轉(zhuǎn)軸轉(zhuǎn)角角類型類型3繞繞OX軸轉(zhuǎn)軸轉(zhuǎn)角角繞繞OY軸轉(zhuǎn)軸轉(zhuǎn)角角繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角uvwx(u)y (v)z (w)ouvwu?v?W?),(ZR),(R),(wR N0T100000000110000cssccssccsscccssssccccssscccsssccsscscscc類型類型1：表示法通常用于陀螺運動：表

55、示法通常用于陀螺運動 類型類型2：所得的轉(zhuǎn)動矩陣為右乘所得的轉(zhuǎn)動矩陣為右乘 10000c0s-010s0c 10000),(),v(),(RcssccsscwRRZR1000pzpyRpxTccsssssccscsscccssccssccssccc類型類型2繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OV 軸轉(zhuǎn)軸轉(zhuǎn)角角繞當(dāng)前繞當(dāng)前OW軸轉(zhuǎn)軸轉(zhuǎn)角角類型類型3 3：一般稱此轉(zhuǎn)動的歐拉角：一般稱此轉(zhuǎn)動的歐拉角為偏航角、俯仰和橫滾，為偏航角、俯仰和橫滾， （這（這種方法也叫做種方法也叫做偏航、俯仰和橫滾偏航、俯仰和橫滾角表示方法）這種形角表示方法）這種形 式主要用式主要用于航空工程中分析飛行器的運動，于航空工程中

56、分析飛行器的運動，其旋轉(zhuǎn)矩陣為其旋轉(zhuǎn)矩陣為ccscssccssccssscssscsccsssccccssccssccsscxRyRz000010010010000),(),(),RR（類型類型3繞繞OX軸轉(zhuǎn)軸轉(zhuǎn) 角角繞繞OY軸轉(zhuǎn)軸轉(zhuǎn)角角繞繞OZ軸轉(zhuǎn)軸轉(zhuǎn)角角: : 已知關(guān)節(jié)角度或位移，計算已知關(guān)節(jié)角度或位移，計算末端操作手的對應(yīng)位姿末端操作手的對應(yīng)位姿. .: : 已知已知末端操作手的位姿，求末端操作手的位姿，求解對應(yīng)的關(guān)節(jié)變量解對應(yīng)的關(guān)節(jié)變量. .可能存在多解或無解可能存在多解或無解通常需多次求解非線性超越方程通常需多次求解非線性超越方程3.6 3.6 運動學(xué)逆問題運動學(xué)逆問題 解的存在性解

57、的存在性存在雙解存在雙解! 求解方法求解方法0140i000222018040i100040i 2 2選擇一個與前一采樣時間最接近的解，例如：選擇一個與前一采樣時間最接近的解，例如：0140i000222018040i 若該關(guān)節(jié)運動空間為若該關(guān)節(jié)運動空間為 ，且，且 ，則應(yīng)選，則應(yīng)選 25001160i0220i3 3根據(jù)避障要求，選擇合適的解根據(jù)避障要求，選擇合適的解4 4逐級剔除多余解逐級剔除多余解 對于具有對于具有n n個關(guān)節(jié)的機器人，其全部解將構(gòu)成樹形結(jié)構(gòu)。個關(guān)節(jié)的機器人，其全部解將構(gòu)成樹形結(jié)構(gòu)。為簡化起見，應(yīng)逐級剔除多余解。這樣可以避免在樹形解中為簡化起見，應(yīng)逐級剔除多余解。這樣可以

58、避免在樹形解中選擇合適的解。選擇合適的解。 Paul 等人提出的方法等人提出的方法65544332211060AAAAAAT 61655443322160-110 TAAAAATA）（1 q626554433260-110-121TAAAATAA）（）（2q6560-110121132143154)()()()(TTAAAAA-）（5 qETAA60-110-165) ( ）（6 q100060pzazsznzpyaysynypxaxsxnxT),(2xyarctg(arccos)cos(cos0/ )(cos180, 0dd為負為正均為負為正為負均為正yxyxyxyxyxtg,0909018

59、018090900),(20000001ccssssccscscazsznzaycaxssycsxsnycnxsaysaxcsyssxcnysnxccssccsscazsznzaysynyaxsxnxcssczcssccssccsscazsznzaysynyaxsxnx010000 00001 1000-01,R 1 100000000110000 1或，可得：而另兩個未知數(shù)在右邊在矩陣方程的左邊，未知數(shù)）左右兩邊，可使一個）左乘式（用）（例例1 1：歐拉角表示的逆運動學(xué)求解：歐拉角表示的逆運動學(xué)求解：),(2tan2111),(2tan01 -11 -1nysnxcsyssxctgnysnx

60、csyssxcsyssxcsnysnxccayaxtgayaxaysaxc）元素分別對應(yīng)相等，）元素和（，使（所在象限。按照前面的定義，確定具體分析辦法靠結(jié)構(gòu)結(jié)束條件、剔除確定象限靠分子，分母的符號來多值解逆運動唯一解正運動總體來講于使用者的直覺用左乘還是右乘，取決解也可以用右乘的方法求）元素對應(yīng)相等，）元素和（，（,),(2)(tan-333211azaycaxstgazaycaxsazcaycaxss例例2：斯坦福機器人運動學(xué)逆問題解：斯坦福機器人運動學(xué)逆問題解6532211060AAAAT61T653321AAA式中：式中： yxyxpCpSpfppfpSpCpf1134z241114)