第六講機(jī)器人運(yùn)動(dòng)學(xué)逆解VIP

1、山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.前置模式：前置模式： i-1坐標(biāo)系i 。 僅涉及i桿件的參數(shù)， 1、桿長:沿xi軸從zi-1到zi的距離。 2、扭角：繞xi從zi-1轉(zhuǎn)到zi的角度。 3、平移量：沿zi-1軸從xi-1軸量至xi軸的距離。 4、轉(zhuǎn)角：繞zi-1軸從xi-1軸到xi的轉(zhuǎn)角。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.第第3 3章章 機(jī)器人運(yùn)動(dòng)學(xué)機(jī)器人運(yùn)動(dòng)學(xué)3.1 3.1 機(jī)器人的位姿描述機(jī)器人的位姿描述3.2 3.2 齊次變換及運(yùn)算齊次變換及運(yùn)算3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程3.4 3.4 機(jī)器人微分運(yùn)動(dòng)機(jī)器人微分運(yùn)動(dòng)山東大學(xué)

2、機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3.2小節(jié)運(yùn) 動(dòng) 學(xué) 方 程 的 逆解3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程 機(jī)器人運(yùn)動(dòng)學(xué)方程的逆解，也稱機(jī)器人的逆運(yùn)動(dòng)學(xué)問題，或間接位置求解。 逆運(yùn)動(dòng)學(xué)問題：對(duì)某個(gè)機(jī)器人，當(dāng)給出機(jī)器人手部在基座標(biāo)系中所處的位置和姿態(tài)時(shí)（即M0h中各元素給定），求出其對(duì)應(yīng)的關(guān)節(jié)變量值qi。 山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.2、運(yùn)動(dòng)學(xué)方程的逆解65236465423z646541652364654231646541652364654231)()()()()

3、(cscsscccsnscccsccssssccccsnscccsscsssscccccnyx逆運(yùn)動(dòng)學(xué)問題的可解性： 下面以六自由度機(jī)器人PUMA為例，研究其可解性。65236465423z646541652364654231646541652364654231)()()()()(ssccscccsoccscscssscsscccsoccscssssscssccccoyx其中：山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.2、運(yùn)動(dòng)學(xué)方程的逆解5235423z5415235423154152354231)()(ccccsassccscccsassscsccccayx)sin(cossi

4、nsincos)cos(sinsincoscosjijijiijjijijiijsc其中：2223454235236z25461222345235423612546122234523542361)()()()()(slcdccsccdpdssdcclsdcssccdspdssdsclsdcssccdcpyx山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.2、運(yùn)動(dòng)學(xué)方程的逆解 可見，我們有12個(gè)方程及6個(gè)未知數(shù)。 上述12個(gè)方程關(guān)系如何？ 我們先看看轉(zhuǎn)動(dòng)部分，它是3X3子矩陣，共有9個(gè)元素；我們知道，轉(zhuǎn)動(dòng)矩陣的每列都是單位矢量，并且每列之間都兩兩正交；因此，9個(gè)元素中僅三個(gè)是獨(dú)立的，或則

5、說，12個(gè)方程中僅有6個(gè)是獨(dú)立，對(duì)應(yīng)6個(gè)未知數(shù)。 因此，一般情況下，單從數(shù)學(xué)的角度看，方程組應(yīng)該是有解的。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.2、運(yùn)動(dòng)學(xué)方程的逆解 上述方程組是由一些非線性的、超越、難解的方程組成。為了降低求解難度，機(jī)器人的桿件參數(shù)應(yīng)僅可能地取為0，如常見的PUMA機(jī)器人那樣。對(duì)于任何非線性方程組，必須關(guān)心其解的存在性、多解性和求解方法。 山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.2、運(yùn)動(dòng)學(xué)方程的逆解解得存在性： 解是否存在與機(jī)器人的工作空間密切相關(guān)，工作空間又取決于機(jī)器人的結(jié)構(gòu)、桿件參數(shù)，或手部（工具）的位姿。 一般情況下，如果手部坐標(biāo)系的位

6、置和姿態(tài)都位于工作空間內(nèi)，則至少存在一個(gè)解；相反，若手部坐標(biāo)系的位置和姿態(tài)都位于工作空間外，則無解。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.2、運(yùn)動(dòng)學(xué)方程的逆解多解性問題： 解得數(shù)量不僅與機(jī)器人的關(guān)節(jié)數(shù)有關(guān)，還與它的桿件參數(shù)、關(guān)節(jié)活動(dòng)范圍等相關(guān)。一般說，連桿的非零參數(shù)越多，解的數(shù)量就越多，即到達(dá)某個(gè)位置的路經(jīng)就越多。多個(gè)解的存在使我們面臨選擇。 如何選擇？如：路徑最短、最近原則。 多解的應(yīng)用：躲避障礙物等。 山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程運(yùn)動(dòng)學(xué)逆解的求解方法 不像線性方程，不存在求解非線性方程組的通用算法。 非線性方程組的算法應(yīng)

7、能求出它的所有解；因此，某些數(shù)值遞推方法不適用。逆解的形式： 1)閉式解(Close-form solution):用解析函數(shù)式表示解。特點(diǎn)：求解速度快。 存在閉式解是機(jī)器人設(shè)計(jì)的目標(biāo)，僅僅在一些特殊情況下，機(jī)器人存在解析的閉式解，如：相鄰的多個(gè)關(guān)節(jié)軸交與一點(diǎn)，桿件扭角等于0或90度等。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程 2)數(shù)值解(Numerical solution): 特點(diǎn)：遞推求解。求解方法分類： 代數(shù)法、幾何法以及數(shù)值法，前兩種用于求閉式解，后一種用于數(shù)值解。 下面我們結(jié)合幾個(gè)實(shí)例，介紹機(jī)器人閉式解析解的求解方法。山東大學(xué)機(jī)械工程學(xué)院機(jī)電

8、工程研究所2010/09/02.3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程例例1 1：已知四軸平面關(guān)節(jié)已知四軸平面關(guān)節(jié)SCARASCARA機(jī)器機(jī)器人如圖所示，試計(jì)算：人如圖所示，試計(jì)算：（1 1）機(jī)器人的運(yùn)動(dòng)學(xué)方程；）機(jī)器人的運(yùn)動(dòng)學(xué)方程；（2 2）當(dāng)關(guān)節(jié)變量?。┊?dāng)關(guān)節(jié)變量取qi=30=30，-60-60，120120，9090 T T時(shí)，機(jī)器人手部的位置和姿態(tài)；時(shí)，機(jī)器人手部的位置和姿態(tài)；（3 3）機(jī)器人運(yùn)動(dòng)學(xué)逆解的數(shù)學(xué)）機(jī)器人運(yùn)動(dòng)學(xué)逆解的數(shù)學(xué)表達(dá)式。表達(dá)式。 800400300200山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解：解：1 1）運(yùn)動(dòng)學(xué)方

9、程）運(yùn)動(dòng)學(xué)方程a a、建立坐標(biāo)系（前置模式）、建立坐標(biāo)系（前置模式） 機(jī)座坐標(biāo)系機(jī)座坐標(biāo)系0 0 桿件坐標(biāo)系桿件坐標(biāo)系i i 手部坐標(biāo)系手部坐標(biāo)系h hx0z0 x1z1x4hz4h800400300200 x2z2x3z31023山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（1 1）運(yùn)動(dòng)學(xué)方程）運(yùn)動(dòng)學(xué)方程b b、確定參數(shù)、確定參數(shù) i dii lii qi 1 800 1400 01 2 02300 02 3 d3 0 0 0 d3 4 -200 4 0 04800400300200 x0z0 x1z1x2z2x3z3x4hz

10、4h0123山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（1 1）運(yùn)動(dòng)學(xué)方程）運(yùn)動(dòng)學(xué)方程c c、相鄰桿件位姿矩陣、相鄰桿件位姿矩陣 10008001004000400010000100001040000110008001000000)0 ,0 ,400(),()800,0 ,0(1111111111101 scscsccsscTranszRotTransM800400300200 x0z0 x1z1x2z2x3z3x4hz4h山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程

11、解解：（1 1）運(yùn)動(dòng)學(xué)方程）運(yùn)動(dòng)學(xué)方程c c、相鄰桿件位姿矩陣、相鄰桿件位姿矩陣 1000010030003000100001000010300001100001000000)0,0,300(),(2222222222212 scscsccsscTranszRotM800400300200 x0z0 x1z1x2z2x3z3x4hz4h山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（1 1）運(yùn)動(dòng)學(xué)方程）運(yùn)動(dòng)學(xué)方程c c、相鄰桿件位姿矩陣、相鄰桿件位姿矩陣 100010000100001),0 ,0(3323ddTransM8004

12、00300200 x0z0 x1z1x2z2x3z3x4hz4h山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（1 1）運(yùn)動(dòng)學(xué)方程）運(yùn)動(dòng)學(xué)方程c c、相鄰桿件位姿矩陣、相鄰桿件位姿矩陣 10002001000000),()200, 0 , 0(44444)(34 cssczRotTransMh800400300200 x0z0 x1z1x2z2x3z3x4hz4h山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（1 1）運(yùn)動(dòng)學(xué)方程）運(yùn)動(dòng)學(xué)方程d d、建立方程、建立方程

13、1000600100300400030040003121124124121124124)(342312010dsscsccscMMMMMhh )sin(),cos(421124421124 sc式式中中：)sin(),cos(21122112 sc山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（2 2）已知）已知qi=30，-60，120，90T，則：，則： 100048010050021233350023210hM山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（3

14、3）逆解數(shù)學(xué)表達(dá)式）逆解數(shù)學(xué)表達(dá)式已知運(yùn)動(dòng)學(xué)方程，用通式表示為：已知運(yùn)動(dòng)學(xué)方程，用通式表示為： 10001000010004311221112412412211124124dddslslcsclclscpaonpaonpaonzzzzyyyyxxxx 已知關(guān)系分析：上述矩陣方程有4個(gè)未知量，由于第一行第一列元素與第二行第二列元素相等，第一行第二列元素與第二行第一列元素大小相等、符號(hào)相反；因此，僅4個(gè)元素相互獨(dú)立，與變量數(shù)相同。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（3 3）逆解數(shù)學(xué)表達(dá)式）逆解數(shù)學(xué)表達(dá)式聯(lián)立方程：聯(lián)立方程：

15、)()()()()(4311221112211124124epddddpslslcpclclbonsaonczyxxyyx 其中，其中，n nx x、n ny y、p px x、p py y和和p pz z是已知的手的位姿，是已知的手的位姿，1 1、2 2、4 4及及d d3 3是待求的未知量。是待求的未知量。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（3 3）逆解數(shù)學(xué)表達(dá)式）逆解數(shù)學(xué)表達(dá)式由上面（由上面（a a）、（）、（b b）兩式可得）兩式可得 ： xynn1421tan yxnsnc)sin()cos(42112442

16、1124山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程221222211211221221222211211221sin)sin(sin2sincos)cos(cos2cosyxpll llpll ll）（）（yxpllpll）（）（2121121211sinsincoscos由上面（由上面（c c）、（）、（d d）兩式：）兩式：2122212222cosl lllppyx21222122122cosllllppyx則：兩邊平方可得兩邊平方可得 ：將兩式相加得：將兩式相加得：211212211212ssccsssccc山東大學(xué)機(jī)械工程

17、學(xué)院機(jī)電工程研究所2010/09/02.這時(shí) 已經(jīng)求出。3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程yxpccslslpsscclcl）（）（21212112121211s2解解：（3 3）逆解數(shù)學(xué)表達(dá)式）逆解數(shù)學(xué)表達(dá)式 為了求為了求1 1，由上面（，由上面（c c）、（）、（d d）兩式展開）兩式展開可得可得 ：yxpscllcslpsslccll12211221221221)()()()(化簡(jiǎn)，得：化簡(jiǎn)，得：)()()()()(4311221112211124124epddddpslslcpclclbonsaonczyxxyyx 山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.

18、3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（3 3）逆解數(shù)學(xué)表達(dá)式）逆解數(shù)學(xué)表達(dá)式由上面兩式可得由上面兩式可得 ：2221222222211)()()( cllslpslpcllcyx 2222221222211)()()( slcllpslpcllsxy yxxypslpcllpslpcll222212222111111)()(tancossintan ）（可得可得 ：山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 xynn1421tan 解解：（3 3）逆解數(shù)學(xué)表達(dá)式）逆解數(shù)學(xué)表達(dá)式已知已知1，2后，由后，由2114ta

19、n xynn可得：可得：最后由（最后由（e)e)式可得式可得 ：413ddpdz 山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解解：（3）逆解數(shù)學(xué)表達(dá)式逆解數(shù)學(xué)表達(dá)式為：413ddpdz yxxypslpcllpslpcll222212222111)()(tan 21222122122cosllllppyx 2114tan xynn 可見，四軸平面關(guān)節(jié)SCARA機(jī)器存在封閉式逆解表達(dá)式。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 結(jié)合實(shí)例介紹另一種求逆解的代數(shù)法。例2：已知

20、PUMA機(jī)器人，如圖所示，試用遞推逆變換法計(jì)算其運(yùn)動(dòng)學(xué)逆解（后置模式）。 與教材有些不同 D2=0,d3不等0.山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解：結(jié)構(gòu)參數(shù)和關(guān)節(jié)變量表31山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 接下來寫出一些桿件間齊次變換陣，并注意其中的一些元素。山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程其中：山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.手部相對(duì)基座坐標(biāo)系的位姿矩陣：

21、2、運(yùn)動(dòng)學(xué)方程的逆解1000r3332312322211312116zyxoprrrprrrprrT已知山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程、由：1606101561120106TTTTTTTTii311dpcpsyx1633323123222113121111111000100001000000Tprrrprrrprrrcssczyx 注意到，T16的（2,4）元素為d3。讓上式中等號(hào)兩邊的（2,4）元素相等，得：（1）（2）已知一個(gè)未知量1越靠近基座越簡(jiǎn)單山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.

22、3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程令：代入（2）式有：根據(jù)和差公式，得：最后：求出了1山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 、 求出1后，(1)式的左邊矩陣就已知，如果我們?cè)倭?1)式等號(hào)兩邊(1,4)和(3,4)元素相等，可得：（3） 將以上兩式平方后相加，可得：其中：（4）兩個(gè)未知量2， ,3山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 （4）式與（2）式有相同的形式，可得： 、我們注意到，T36中的（1,4）和（2,4）元素為常數(shù)，由：3606103TTT（5

23、） 令(5)式等號(hào)兩邊(1,4)和(2,4)元素相等，可得：（6）1、2已求出山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 （6）式中，僅c23和s23兩個(gè)未知數(shù)，聯(lián)立可解得：山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 這樣，式（5）左邊矩陣中的所有元素都已知了。 、為了求4和5，令(5)式等號(hào)兩邊(1,3)和(3,3)元素相等，可得：設(shè)s5不等于零，得： 5等于零對(duì)應(yīng)4軸與6軸共線的奇異結(jié)構(gòu)，這時(shí) 的轉(zhuǎn)動(dòng)效果相同，可任取 。464與山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程 、為了求5和6，我們應(yīng)用T46：4606104TTT其中： 令(7)式等號(hào)兩邊(1,3)和(3,3)元素相等，可得：（7）山東大學(xué)機(jī)械工程學(xué)院機(jī)電工程研究所2010/09/02.3.3 3.3 機(jī)器人運(yùn)動(dòng)學(xué)方程機(jī)器人運(yùn)動(dòng)學(xué)方程解得： 同樣，令(7)式等號(hào)兩邊(3,1)和(1,1)元素相等，可得：