第七章 機(jī)器人運(yùn)動(dòng)學(xué)

1、第七章 機(jī)器人運(yùn)動(dòng)學(xué) n機(jī)器人運(yùn)動(dòng)學(xué)主要是把機(jī)器人相對(duì)于固定參考系的運(yùn)動(dòng)作為時(shí)間的函數(shù)進(jìn)行分析研究，而不考慮引起這些運(yùn)動(dòng)的力和力矩n將機(jī)器人的空間位移解析地表示為時(shí)間的函數(shù)，特別是研究機(jī)器人關(guān)節(jié)變量空間和機(jī)器人末端執(zhí)行器位置和姿態(tài)之間的關(guān)系n本章將討論機(jī)器人運(yùn)動(dòng)學(xué)幾個(gè)具有實(shí)際意義的基本問題。 機(jī)器人技術(shù)及空間應(yīng)用機(jī)器人技術(shù)及空間應(yīng)用7.1 機(jī)器人運(yùn)動(dòng)學(xué)所討論的問題 7.1.1 研究的對(duì)象機(jī)器人從機(jī)構(gòu)形式上分為兩種，一種是關(guān)節(jié)式串聯(lián)機(jī)器人，另外一種是并聯(lián)機(jī)器人。PUMA560HexapodFanuc manipulator7.1.2 運(yùn)動(dòng)學(xué)研究的問題Where is my hand?Direct

2、 KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!運(yùn)動(dòng)學(xué)逆問題運(yùn)動(dòng)學(xué)正問題關(guān)節(jié)角桿件參數(shù)末端執(zhí)行器運(yùn)動(dòng)學(xué)正問題關(guān)節(jié)角桿件參數(shù)研究的兩類問題:n運(yùn)動(dòng)學(xué)正問題-已知桿件幾何參數(shù)和關(guān)節(jié)角矢量，求操作機(jī)末端執(zhí)行器相對(duì)于固定參考作標(biāo)的位置和姿態(tài)（齊次變換問題）。n運(yùn)動(dòng)學(xué)逆問題-已知操作機(jī)桿件的幾何參數(shù)，給定操作機(jī)末端執(zhí)行器相對(duì)于參考坐標(biāo)系的期望位置和姿態(tài)（位置），操作機(jī)能否使其末端執(zhí)行器達(dá)到這個(gè)預(yù)期的位姿？如能達(dá)到，那么操作機(jī)有幾種不同形態(tài)可以滿足同樣的條件？7.2 機(jī)器人桿件，關(guān)節(jié)和它們的

3、參數(shù) 7.2.1 桿件與關(guān)節(jié)n操作機(jī)由一串用轉(zhuǎn)動(dòng)或平移（棱柱形）關(guān)節(jié)連接的剛體（桿件）組成n每一對(duì)關(guān)節(jié)桿件構(gòu)成一個(gè)自由度，因此N個(gè)自由度的操作機(jī)就有N對(duì)關(guān)節(jié)桿件。n0號(hào)桿件（一般不把它當(dāng)作機(jī)器人的一部分）固聯(lián)在機(jī)座上，通常在這里建立一個(gè)固定參考坐標(biāo)系，最后一個(gè)桿件與工具相連n關(guān)節(jié)和桿件均由底座向外順序排列，每個(gè)桿件最多和另外兩個(gè)桿件相聯(lián)，不構(gòu)成閉環(huán)。 關(guān)節(jié)：n一般說來，兩個(gè)桿件間是用低副相聯(lián)的n只可能有6種低副關(guān)節(jié)：旋轉(zhuǎn)（轉(zhuǎn)動(dòng)）、棱柱（移動(dòng)）、圓柱形、球形、螺旋和平面，其中只有旋轉(zhuǎn)和棱柱形關(guān)節(jié)是串聯(lián)機(jī)器人操作機(jī)常見的，各種低副形狀如下圖所示：旋轉(zhuǎn)旋轉(zhuǎn)棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面

4、平面AiAi+1Ai-1 桿件參數(shù)的定義 、 、 和n l 和 l在 A 軸 線上的交點(diǎn)之間 的距離。iidiAiAi+1iilid1iliAi-1idn l 和和 l之間的夾之間的夾 角，按右手定則角，按右手定則 由由l轉(zhuǎn)向轉(zhuǎn)向 l。 由運(yùn)動(dòng)學(xué)的觀點(diǎn)來看，桿件保持其兩端關(guān)節(jié)間的形態(tài)由運(yùn)動(dòng)學(xué)的觀點(diǎn)來看，桿件保持其兩端關(guān)節(jié)間的形態(tài)不變，這種形態(tài)由兩個(gè)參數(shù)決定：桿件長(zhǎng)度不變，這種形態(tài)由兩個(gè)參數(shù)決定：桿件長(zhǎng)度 li 和桿件扭和桿件扭轉(zhuǎn)角轉(zhuǎn)角 。桿件的相對(duì)位置關(guān)系，由另外桿件的相對(duì)位置關(guān)系，由另外兩兩個(gè)參數(shù)決定：個(gè)參數(shù)決定：桿件的距離桿件的距離 di 和桿件的回轉(zhuǎn)角和桿件的回轉(zhuǎn)角 。iiilin li

5、 ii v 上述上述4個(gè)參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相個(gè)參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對(duì)位置關(guān)系。在轉(zhuǎn)動(dòng)關(guān)節(jié)中，對(duì)位置關(guān)系。在轉(zhuǎn)動(dòng)關(guān)節(jié)中，li, i, di是固定值，是固定值，i是變量。是變量。在移動(dòng)關(guān)節(jié)中，在移動(dòng)關(guān)節(jié)中，li, i , i是固定值，是固定值， d i 是變量。是變量。7.3 機(jī)器人關(guān)節(jié)坐標(biāo)系的建立n 對(duì)于每個(gè)桿件都可以在關(guān)節(jié)軸處建立一個(gè)正規(guī)的笛卡對(duì)于每個(gè)桿件都可以在關(guān)節(jié)軸處建立一個(gè)正規(guī)的笛卡兒坐標(biāo)系（兒坐標(biāo)系（xi, yi, zi），（），（i=1, 2, , n），），n是自由度是自由度數(shù)，再加上基座坐標(biāo)系，一共有（數(shù)，再加上基座坐標(biāo)系，一共有（n+1）

6、個(gè)坐標(biāo)系。）個(gè)坐標(biāo)系。n 基座坐標(biāo)系基座坐標(biāo)系 O0定義為定義為0號(hào)坐標(biāo)系（號(hào)坐標(biāo)系（x0, y0, z0）,它也是它也是機(jī)器人的慣性坐標(biāo)系，機(jī)器人的慣性坐標(biāo)系，0號(hào)坐標(biāo)系在基座上的位置和號(hào)坐標(biāo)系在基座上的位置和方向可任選，但方向可任選，但z0軸線必須與關(guān)節(jié)軸線必須與關(guān)節(jié)1的軸線重合，位的軸線重合，位置和方向可任選；置和方向可任選；n 最后一個(gè)坐標(biāo)系（最后一個(gè)坐標(biāo)系（n關(guān)節(jié)），可以設(shè)在手的任意部位，關(guān)節(jié)），可以設(shè)在手的任意部位，但必須保證但必須保證 zn與與zn-1 垂直。垂直。 D-H關(guān)節(jié)坐標(biāo)系建立原則n機(jī)器人關(guān)節(jié)坐標(biāo)系的建立主要是為了描述機(jī)器人各桿件和終端之間的相對(duì)運(yùn)動(dòng)，對(duì)建立運(yùn)動(dòng)方程和動(dòng)

7、力學(xué)研究是基礎(chǔ)性的工作。n為了描述機(jī)器人各桿件和終端之間轉(zhuǎn)動(dòng)或移動(dòng)關(guān)系，Denavit和Hartenberg于1955年提出了一種為運(yùn)動(dòng)鏈中每個(gè)桿件建立附體坐標(biāo)系的矩陣方法（D-H方法） ，建立原則如下：u右手坐標(biāo)系右手坐標(biāo)系u原點(diǎn)原點(diǎn)Oi：設(shè)在：設(shè)在li與與Ai+1軸線的交點(diǎn)上軸線的交點(diǎn)上 uZi軸：軸： 與與Ai+1關(guān)節(jié)軸重合，指向任意關(guān)節(jié)軸重合，指向任意 uXi軸：軸： 與公法線與公法線Li重合，指向沿重合，指向沿Li由由Ai軸線指向軸線指向Ai+1軸線軸線 uYi軸：軸： 按右手定則按右手定則 關(guān)節(jié)坐標(biāo)系的建立方法n原點(diǎn)Oi：設(shè)在li與Ai+1軸線的交點(diǎn)上 nzi軸：與Ai+1關(guān)節(jié)軸重

8、合，指向任意 nxi軸：與公法線li重合，指向沿li由Ai軸線指向Ai+1軸線 nyi軸：按右手定則 AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiyio 沿 xi 軸， zi-1 軸與 xi 軸交點(diǎn)到 0i 的距離 繞 xi 軸，由 zi-1 轉(zhuǎn)向zi 沿 zi-1 軸，zi-1 軸和 xi 交點(diǎn)至0i 1 坐標(biāo)系原點(diǎn)的距離 繞 zi-1 軸，由 xi-1轉(zhuǎn)向 xi兩種特殊情況n 兩軸相交，怎么建立坐標(biāo)系？ Oi Ai與Ai+1關(guān)節(jié)軸線的 交點(diǎn)； zi Ai+1軸線； xi zi和zi-1構(gòu)成的平面的 法線 ； yi 右手定則；-1iizzAiAi+1zi-1zix

9、iyiOin兩軸平行，怎么建立坐標(biāo)系(Ai與Ai+1平行)？先建立 Oi-1然后建立Oi+1最后建立 OiAi-1AiAi+1Ai+2li-1oi-1xi-1yi-1zi-1ABDCoi（xi）（yi）zixiyioi+1xi+1yi+1zi+1di+1li+1di-1iO D 注意：注意： 由于由于Ai和和Ai+1平行，所以公法線平行，所以公法線 任意點(diǎn)在任意點(diǎn)在A點(diǎn)位置；點(diǎn)位置； 按照先前的定義，按照先前的定義，di為為Oi-1點(diǎn)和點(diǎn)和A點(diǎn)之間的距離，點(diǎn)之間的距離，di+1為為B點(diǎn)和點(diǎn)和C點(diǎn)間點(diǎn)間的距離，這樣設(shè)定可以的，但我們可以變更一下，將的距離，這樣設(shè)定可以的，但我們可以變更一下，將0

10、i點(diǎn)放在點(diǎn)放在C點(diǎn)，點(diǎn)，定義定義Oi在在li+1和和Ai+1軸的交點(diǎn)上，這樣使軸的交點(diǎn)上，這樣使di+1=0使計(jì)算簡(jiǎn)便，此時(shí)使計(jì)算簡(jiǎn)便，此時(shí)di= 7.4 相鄰關(guān)節(jié)坐標(biāo)系間的齊次變換過程 機(jī)器人運(yùn)動(dòng)學(xué)正解n將xi-1軸繞 zi-1 軸轉(zhuǎn) i 角度，將其與xi軸平行；n沿 zi-1軸平移距離 di ，使 xi-1 軸與 xi 軸重合；n沿 xi 軸平移距離 li，使兩坐標(biāo)系原點(diǎn)及x軸重合；n繞 xi 軸轉(zhuǎn) i 角度，兩坐標(biāo)系完全重合111A(,)(,)( , ) ( ,)iiiiiiiiiiR zTrans zd Trans x l R xAiAi+1iilid1iliAi-11iz1ix1iy

11、1ioizixiyio 機(jī)器人的運(yùn)動(dòng)學(xué)正解方程001112iiiTAAA D-H變換矩陣變換矩陣iiA1100010000100001id1000010000cossin00sincosiiii100001000010001il10000cossin00sincos00001iiii1000cossin0sincossincoscossincossinsinsincoscosiiiiiiiiiiiiiiiiidaa=例題 試求立方體中心在機(jī)座坐標(biāo)系0中的位置 該手爪從上方把物體抓起，同時(shí)手爪的開合方向與物體的Y軸同向，那么，求手爪相對(duì)于0的姿態(tài)是什么？ 在機(jī)器人工作臺(tái)上加裝一電視攝像機(jī)，攝像機(jī)

12、可見到固聯(lián)在機(jī)器人工作臺(tái)上加裝一電視攝像機(jī)，攝像機(jī)可見到固聯(lián)著著6DOF關(guān)節(jié)機(jī)器人的機(jī)座坐標(biāo)系原點(diǎn)，它也可以見到被操作關(guān)節(jié)機(jī)器人的機(jī)座坐標(biāo)系原點(diǎn)，它也可以見到被操作物體（立方體）的中心，如果在物體中心建一局部坐標(biāo)系，則物體（立方體）的中心，如果在物體中心建一局部坐標(biāo)系，則攝像機(jī)所見到的這個(gè)物體可由齊次變換矩陣攝像機(jī)所見到的這個(gè)物體可由齊次變換矩陣T1來表示，如果攝來表示，如果攝像機(jī)所見到的機(jī)座坐標(biāo)系為矩陣像機(jī)所見到的機(jī)座坐標(biāo)系為矩陣T2表示。表示。1000101-002001-010-001T100091-00100011010T21xyz解1： T T 21物機(jī)機(jī)攝物攝求，已知TTT TT

13、11 -2）（有：物攝攝機(jī)物機(jī)TTT 100091-00100011010 1000101-002001-0100011000110010001-11010 O物根據(jù)T1畫出O機(jī)根據(jù)T2畫出因此物體位于機(jī)座坐標(biāo)系的（因此物體位于機(jī)座坐標(biāo)系的（11，10，1）T處，它的處，它的X，Y，Z軸分別與機(jī)座坐標(biāo)系的軸分別與機(jī)座坐標(biāo)系的-Y，X，Z軸平行。軸平行。 解2：Osnayzx 0001xxxxyyyyxzzznsapnsapTnsap機(jī)手爪實(shí)際要求向重合手爪開合方向與物體ya:Ts001有方向相反方向物體的從上向下抓，指出手爪zab:Ta 100則有:10000010001Tijkc nsaij

14、k 1-00001010因此：姿態(tài)矩陣為重合時(shí)與物體中心當(dāng)手爪中心100011-001000111010 T物機(jī)X機(jī)例：Stanford機(jī)器人運(yùn)動(dòng)學(xué)方程d3A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y4z4x4O4z5y5x5O5z6x6y6O6d6z0y0 x0O0 為右手坐標(biāo)系 原點(diǎn)Oi： Ai與Ai+1關(guān)節(jié)軸線的交點(diǎn) zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意 xi軸： Zi和Zi-1構(gòu)成的面的法線 yi軸：按右手定則 li 沿沿 xi 軸，軸， zi-1 軸與軸與 xi 軸交點(diǎn)到軸交點(diǎn)到Oi 的距離的距離i 繞繞 xi 軸，由軸，由 zi-1 轉(zhuǎn)向轉(zhuǎn)向

15、zidi 沿沿 zi-1 軸，軸，zi-1 軸和軸和 xi 交點(diǎn)至交點(diǎn)至Oi 1 坐標(biāo)坐標(biāo) 系原點(diǎn)的距離系原點(diǎn)的距離i 繞繞 zi-1 軸，由軸，由 xi-1轉(zhuǎn)向轉(zhuǎn)向 xi解：解： 7.5 工作空間n工作空間: 末端操作手可以到達(dá)的空間位置集合。n如何獲得工作空間: 利用正運(yùn)動(dòng)學(xué)模型,改變關(guān)節(jié)變量值。n靈活空間: 末端操作手可以以任何姿態(tài)到達(dá)的空間位置集合。n可達(dá)空間: 末端操作手可以至少以一個(gè)姿態(tài)到達(dá)的空間位置 集合。n 空洞：空洞：在在 zi軸周圍，參考點(diǎn)軸周圍，參考點(diǎn)Pn沿沿z的全長(zhǎng)均不能達(dá)到的空間。的全長(zhǎng)均不能達(dá)到的空間。n 空腔：空腔：參考點(diǎn)不能達(dá)到的被完全封閉在工作空間之內(nèi)的空間。

16、參考點(diǎn)不能達(dá)到的被完全封閉在工作空間之內(nèi)的空間。 空洞空洞空腔空腔如何確定可達(dá)空間如何確定可達(dá)空間? ?首先，首先，令令 3 3變化變化 示例: 平面 3連桿機(jī)器人123112123123112123123123123123coscoscossinsinsin , 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

17、(v)z (w)ouvwu?v?W?),(ZR),(R),(wR N0T100000000110000cssccssccsscccssssccccssscccsssccsscscscc類型類型1：表示法通常用于陀螺運(yùn)動(dòng)：表示法通常用于陀螺運(yùn)動(dòng) 類型類型2：所得的轉(zhuǎn)動(dòng)矩陣為右乘所得的轉(zhuǎn)動(dòng)矩陣為右乘 10000c0s-010s0c 10000),(),v(),(RcssccsscwRRZR1000pzpyRpxTccsssssccscsscccssccssccssccc類型3：一般稱此轉(zhuǎn)動(dòng)的歐拉角為橫滾、俯仰和偏航角，這種形 式主要用于航空工程中分析飛行器的運(yùn)動(dòng)，其旋轉(zhuǎn)矩陣為（這種方法也叫做橫滾、

18、俯仰和偏航角表示方法） ccscssccssccssscssscsccsssccccssccssccsscxRyRz000010010010000),(),(),RR（ZYX偏航偏航俯仰俯仰橫滾橫滾7.7 運(yùn)動(dòng)學(xué)逆問題: 已知關(guān)節(jié)角度或位移，計(jì)算末端操作手的對(duì)應(yīng)位姿.: 已知末端操作手的位姿，求解對(duì)應(yīng)的關(guān)節(jié)變量. 可能存在多解或無解 通常需多次求解非線性超越方程 解的存在性存在雙解存在雙解! 求解方法封閉形式解數(shù)值解n 方法n 我們對(duì)封閉形式的解法更感興趣 代數(shù)方法 幾何方法n 可解性的重要結(jié)論是： 所有具有轉(zhuǎn)動(dòng)和移動(dòng)關(guān)節(jié)的系統(tǒng)，在一個(gè)單一串聯(lián)中總共有6個(gè)（或小于6個(gè)）自由度時(shí)，是可解的，其通

19、解一般是數(shù)值解，它不是解析表達(dá)式，而是利用數(shù)值迭代原理求解，它的計(jì)算量要比解析解大。 但在某些特殊情況下，如若干個(gè)關(guān)節(jié)軸線相交和或多個(gè)關(guān)節(jié)軸線等于 0 或 90的情況下，具有6個(gè)自由度的機(jī)器人可得到解析解。 為使機(jī)器人有解析解，一般設(shè)計(jì)時(shí)，使工業(yè)機(jī)器人足夠簡(jiǎn)單，盡量滿足這些特殊條件。 n 對(duì)于給定的機(jī)器人，能否求得它的運(yùn)動(dòng)學(xué)逆解的解析式（也叫封閉解）。 運(yùn)動(dòng)學(xué)逆問題的可解性 運(yùn)動(dòng)學(xué)逆問題的多解性n 機(jī)器人運(yùn)動(dòng)問題為解三角方程，解反三角函數(shù)方程時(shí)會(huì)產(chǎn)生多解.顯然對(duì)于真實(shí)的機(jī)器人，只有一組解與實(shí)際情況相對(duì)應(yīng)，因此必須作出判斷，以選擇合適的解。n 通常采用如下方法剔除多余解：0140i0002220

20、18040i若該關(guān)節(jié)運(yùn)動(dòng)空間為 ，則應(yīng)選 。 100040i 1根據(jù)關(guān)節(jié)運(yùn)動(dòng)空間合適的解。例如求得機(jī)器人某關(guān)節(jié)角的兩個(gè)解為 2選擇一個(gè)與前一采樣時(shí)間最接近的解，例如：0140i000222018040i 若該關(guān)節(jié)運(yùn)動(dòng)空間為 ，且 ，則應(yīng)選 25001160i0220i3根據(jù)避障要求得選擇合適的解4逐級(jí)剔除多余解 對(duì)于具有n個(gè)關(guān)節(jié)的機(jī)器人，其全部解將構(gòu)成樹形結(jié)構(gòu)。為簡(jiǎn)化起見，應(yīng)逐級(jí)剔除多余解。這樣可以避免在樹形解中選擇合適的解。 n 逆運(yùn)動(dòng)學(xué)的定義n 逆運(yùn)動(dòng)學(xué)的存在性n 逆運(yùn)動(dòng)學(xué)的可解性n 逆運(yùn)動(dòng)學(xué)的多解性（剔除辦法）n 逆運(yùn)動(dòng)學(xué)解法（數(shù)值解、解析解） 運(yùn)動(dòng)學(xué)逆問題How do I put my

21、 hand here?Inverse Kinematics: Choose these angles!運(yùn)動(dòng)學(xué)逆問題Paul 等人提出的方法(1981年，解析解): Paul 等人提出的方法65544332211060TTTTTTT TTTTTT T 6554433221601 -10）（1 q65544332601 -101 -21TTTTTTT）（）（2q65601 -101 -21132143154TTTT)T()T()T(）（）（5 qE601 -101 -65TT) T( ）（6 q),(2xyarctg)/(xyarctg 因此，通常用反正切函數(shù) 來確定 值，它可把 校正到適當(dāng)?shù)南笙?/p>

22、，其定義為： 不能用反余弦 來求解關(guān)節(jié)角，因?yàn)檫@樣求解不僅關(guān)節(jié)角的符號(hào)不確定（ ），而且角的精度也難以保證（ ）。 (arccos)cos(cos0/ )(cos180, 0dd為負(fù)為正均為負(fù)為正為負(fù)均為正yxyxyxyxxytg,090901801809090),(200000001 例：歐拉角第一種類型，求逆步步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)角角),(ZR),(R),(wR N0T100000000110000cssccssccsscccssssccccssscccsssccsscscscc類型類型1：表示法通常用于陀螺運(yùn)

23、動(dòng)：表示法通常用于陀螺運(yùn)動(dòng) ccssssccscscazsznzaycaxssycsxsnycnxsaysaxcsyssxcnysnxccssccsscazsznzaysynyaxsxnxcssczcssccssccsscazsznzaysynyaxsxnx010000 00001 1000-01,R 1 100000000110000 1或，可得：而另兩個(gè)未知數(shù)在右邊在矩陣方程的左邊，未知數(shù)）左右兩邊，可使一個(gè)）左乘式（用）（解：),(2tan2111),(2tan01 -11 -1nysnxcsyssxctgnysnxcsyssxcsyssxcsnysnxccayaxtgayaxaysax

24、c）元素分別對(duì)應(yīng)相等，）元素和（，使（所在象限。按照前面的定義，確定具體分析辦法靠結(jié)構(gòu)結(jié)束條件、剔除確定象限靠分子，分母的符號(hào)來多值解逆運(yùn)動(dòng)唯一解正運(yùn)動(dòng)總體來講于使用者的直覺用左乘還是右乘，取決解也可以用右乘的方法求）元素對(duì)應(yīng)相等，）元素和（，（,),(2)(tan-333211azaycaxstgazaycaxsazcaycaxss 斯坦福機(jī)器人運(yùn)動(dòng)學(xué)逆問題解斯坦福機(jī)器人運(yùn)動(dòng)學(xué)逆問題解6533211060AAAAT61T653321AAA式中：式中： yxyxpCpSpfzpfpSpCpf1113121111)()()(由兩端矩陣對(duì)應(yīng)元素相等可得：由兩端矩陣對(duì)應(yīng)元素相等可得： 作三角變換：作三角變換： 式中：式中： 得到：得到： 即有：即有： 由由1, 4和和2, 4元素對(duì)應(yīng)相等，得：元素對(duì)應(yīng)相等，得： 6261121TTA636213