1、會計(jì)學(xué)1機(jī)器人運(yùn)動學(xué)正解逆解機(jī)器人運(yùn)動學(xué)正解逆解學(xué)習(xí)目標(biāo)：學(xué)習(xí)目標(biāo)：1. 理解理解D-H法原理法原理 2. 學(xué)會用學(xué)會用D-H法對機(jī)器人建模法對機(jī)器人建模學(xué)習(xí)重點(diǎn)：學(xué)習(xí)重點(diǎn)：1. 給關(guān)節(jié)指定參考坐標(biāo)系給關(guān)節(jié)指定參考坐標(biāo)系 2. 制定制定D-H參數(shù)表參數(shù)表 3. 利用參數(shù)表計(jì)算轉(zhuǎn)移矩陣?yán)脜?shù)表計(jì)算轉(zhuǎn)移矩陣第1頁/共60頁背景簡介：背景簡介： 1955年，年，Denavit和和Hartenberg(迪納維特和哈坦伯格迪納維特和哈坦伯格)提出提出了這一方法，后成為表示機(jī)器人以及對機(jī)器人建模的標(biāo)準(zhǔn)方法，了這一方法，后成為表示機(jī)器人以及對機(jī)器人建模的標(biāo)準(zhǔn)方法，應(yīng)用廣泛。應(yīng)用廣泛。 總體思想：總體思想：

2、 首先給每個(gè)關(guān)節(jié)指定坐標(biāo)系，然后確定從一個(gè)關(guān)節(jié)到下一個(gè)首先給每個(gè)關(guān)節(jié)指定坐標(biāo)系，然后確定從一個(gè)關(guān)節(jié)到下一個(gè)關(guān)節(jié)進(jìn)行變化的步驟，這體現(xiàn)在兩個(gè)相鄰參考坐標(biāo)系之間的變關(guān)節(jié)進(jìn)行變化的步驟，這體現(xiàn)在兩個(gè)相鄰參考坐標(biāo)系之間的變化，將所有變化結(jié)合起來，就確定了末端關(guān)節(jié)與基座之間的總化，將所有變化結(jié)合起來，就確定了末端關(guān)節(jié)與基座之間的總變化，從而建立運(yùn)動學(xué)方程，進(jìn)一步對其求解。變化，從而建立運(yùn)動學(xué)方程，進(jìn)一步對其求解。第2頁/共60頁1.1.第一個(gè)關(guān)節(jié)指定為關(guān)節(jié)第一個(gè)關(guān)節(jié)指定為關(guān)節(jié)n,n,第二個(gè)關(guān)節(jié)為第二個(gè)關(guān)節(jié)為n+1,n+1,其余其余關(guān)節(jié)以此類推。關(guān)節(jié)以此類推。坐標(biāo)系的確定坐標(biāo)系的確定2.Z2.Z軸確定規(guī)則

3、：軸確定規(guī)則：如果關(guān)如果關(guān)節(jié)是旋轉(zhuǎn)的，節(jié)是旋轉(zhuǎn)的，Z Z軸位于按軸位于按右手規(guī)則旋轉(zhuǎn)的方向右手規(guī)則旋轉(zhuǎn)的方向，轉(zhuǎn)角轉(zhuǎn)角 為關(guān)節(jié)變量。如為關(guān)節(jié)變量。如果關(guān)節(jié)是滑動的，果關(guān)節(jié)是滑動的，Z Z軸為軸為沿直線運(yùn)動的方向沿直線運(yùn)動的方向，連，連桿長度桿長度d d為關(guān)節(jié)變量。為關(guān)節(jié)變量。關(guān)關(guān)節(jié)節(jié)n n處處Z Z軸下標(biāo)為軸下標(biāo)為n-1n-1。 第3頁/共60頁3.X3.X軸確定規(guī)則軸確定規(guī)則情況情況1 1：兩關(guān)節(jié)：兩關(guān)節(jié)Z Z軸既不平行也不相交軸既不平行也不相交取兩取兩Z Z軸公垂線方向作為軸公垂線方向作為X X軸方向，命名規(guī)則同軸方向，命名規(guī)則同Z Z軸。軸。情況情況2 2：兩關(guān)節(jié)：兩關(guān)節(jié)Z Z軸平行軸平

4、行此時(shí)，兩此時(shí)，兩Z Z軸之間有無數(shù)條公垂線，可挑選與前一關(guān)節(jié)的公垂線共線的軸之間有無數(shù)條公垂線，可挑選與前一關(guān)節(jié)的公垂線共線的一條公垂線。一條公垂線。情況情況3 3：兩關(guān)節(jié)：兩關(guān)節(jié)Z Z軸相交軸相交取兩條取兩條Z Z軸的叉積方向作為軸的叉積方向作為X X軸。軸。4.Y4.Y軸確定原則軸確定原則取取X X軸、軸、Z Z軸叉積方向作為軸叉積方向作為Y Y軸方向。（右手）軸方向。（右手）5.5.變量選擇原則變量選擇原則用用n+1n+1角表示角表示XnXn到到Xn+1Xn+1繞繞ZnZn軸的旋轉(zhuǎn)角軸的旋轉(zhuǎn)角; ;d dn+1n+1表示從表示從XnXn到到Xn+1Xn+1沿沿ZnZn測量測量的距離的距

5、離; ;a an+1n+1表示關(guān)節(jié)偏移，表示關(guān)節(jié)偏移，an+1an+1是從是從ZnZn到到Zn+1Zn+1沿沿Xn+1Xn+1測量的距離測量的距離; ;角角表示關(guān)節(jié)扭轉(zhuǎn)表示關(guān)節(jié)扭轉(zhuǎn), n+1, n+1是從是從ZnZn到到Zn+1Zn+1繞繞Xn+1Xn+1旋轉(zhuǎn)的角度。旋轉(zhuǎn)的角度。 通常情況下，通常情況下，只有只有和和d d是關(guān)節(jié)變量。是關(guān)節(jié)變量。第4頁/共60頁斯坦福機(jī)器人斯坦福機(jī)器人斯坦福機(jī)器人開始的兩個(gè)關(guān)節(jié)是旋轉(zhuǎn)的，第三個(gè)關(guān)節(jié)是滑動的，最后三個(gè)腕關(guān)節(jié)全是旋轉(zhuǎn)關(guān)節(jié)第5頁/共60頁第6頁/共60頁第7頁/共60頁A1A2A3A4A5A6d1z1x1y1O1d2z2x2y2O2z3y3x3O3y

6、4z4x4O4z5y5x5O534545,0o o odd重重合合d3z6x6y6O6d6z0y0 x0O0ai沿沿 xi 軸，軸， zi-1 軸與軸與 xi 軸交點(diǎn)到軸交點(diǎn)到Oi 的距離的距離i 繞繞 xi 軸，由軸，由 zi-1 轉(zhuǎn)向轉(zhuǎn)向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關(guān)節(jié)1坐標(biāo)系0關(guān)節(jié)2坐標(biāo)系1關(guān)節(jié)3坐標(biāo)系2連桿0連桿1連桿2連桿3連桿4連桿5關(guān)節(jié)4坐標(biāo)系3關(guān)節(jié)5坐標(biāo)系4關(guān)節(jié)6坐標(biāo)系5第8頁/共60頁解：解：第9頁/共60頁第10頁/共60頁第11頁

7、/共60頁第12頁/共60頁123456關(guān)節(jié)變量都是關(guān)節(jié)變量都是第13頁/共60頁P(yáng)UMA560機(jī)器人的連桿及關(guān)節(jié)編號第14頁/共60頁為右手坐標(biāo)系，Yi軸：按右手定則 Zi軸：與Ai+1關(guān)節(jié)軸重合，指向任意 Xi軸： Zi和Zi-1構(gòu)成的面的法線， 或連桿i兩端軸線Ai 與Ai+1的公垂線(即： Zi和Zi-1的公垂線)原點(diǎn)Oi： Ai與Ai+1關(guān)節(jié)軸線的交點(diǎn)，或Zi與Xi的交點(diǎn)ai沿沿 xi 軸，軸， zi-1 軸與軸與 xi 軸交點(diǎn)到軸交點(diǎn)到Oi 的距離的距離i 繞繞 xi 軸，由軸，由 zi-1 轉(zhuǎn)向轉(zhuǎn)向zidi 沿沿 zi-1 軸，軸，zi-1 軸和軸和 xi 交點(diǎn)至交點(diǎn)至Oi 1

8、坐標(biāo)坐標(biāo) 系原點(diǎn)的距離系原點(diǎn)的距離i 繞繞 zi-1 軸，由軸，由 xi-1轉(zhuǎn)向轉(zhuǎn)向 xiA1A2A3A4A5A6O1O0第15頁/共60頁第16頁/共60頁第17頁/共60頁 對下圖所示簡單機(jī)器人，根據(jù)對下圖所示簡單機(jī)器人，根據(jù)D-H法，建立必要坐標(biāo)系及法，建立必要坐標(biāo)系及參數(shù)表。參數(shù)表。第18頁/共60頁第一步：根據(jù)第一步：根據(jù)D-H法建立坐標(biāo)系的規(guī)則建立坐標(biāo)系法建立坐標(biāo)系的規(guī)則建立坐標(biāo)系 第19頁/共60頁第二步：將做好的坐標(biāo)系簡化為我們熟悉的線圖形式第二步：將做好的坐標(biāo)系簡化為我們熟悉的線圖形式第20頁/共60頁第三步：根據(jù)建立好的坐標(biāo)系，確定各參數(shù)，并第三步：根據(jù)建立好的坐標(biāo)系，確定

9、各參數(shù)，并寫入寫入D-H參數(shù)表參數(shù)表#da1009020030040-905009060001234562a3a4a第21頁/共60頁 10000),()0 , 0 ,(), 0 , 0(111111111111111111111),(111nnnnnnnnnnnnnnnnnnnnnznnndCSSaSCCCSCaSSCSCAxRotaTransdTransRotATn #da1009020030040-905009060001234562a3a4a 10000010000011111CSSCA第22頁/共60頁第四步：將參數(shù)代入第四步：將參數(shù)代入A矩陣，可得到矩陣，可得到 100000100

10、00011111CSSCA 1000010000222222222aSCSaCSCA 1000010000333333333aSCSaCSCA 1000001000444444444aSCSaCSCA 10000010000055555CSSCA 10000100000066666CSSCA第23頁/共60頁第第5步步 求出總變化矩陣求出總變化矩陣 1000)()()()()()()()(22323423452346234652346523422323423415152341651623465234165162346523412232342341515234165162346523416516

11、234652341654321aSaSaSSSCCCCSCCSaCaCaCSCCSCSSSCCSCCCSSSCSSCCCSaCaCaCCCSSCCSSSCSCCCCCSSSSCCCCAAAAAATHR第24頁/共60頁 依次寫出從基坐標(biāo)系到手爪坐標(biāo)系之間相鄰兩坐標(biāo)系的依次寫出從基坐標(biāo)系到手爪坐標(biāo)系之間相鄰兩坐標(biāo)系的齊次齊次變換矩陣變換矩陣，它們依次連乘的結(jié)果就是末端執(zhí)行器（手爪）在基坐，它們依次連乘的結(jié)果就是末端執(zhí)行器（手爪）在基坐標(biāo)系中的空間描述，即標(biāo)系中的空間描述，即已知已知q q1 1,q,q2 2, ,q,qn n，求，求 ，稱為運(yùn)動學(xué)正解；，稱為運(yùn)動學(xué)正解；已知已知 ，求，求q q1

12、 1,q,q2 2, ,q,qn n，稱為運(yùn)動學(xué)反解。，稱為運(yùn)動學(xué)反解。上式稱為上式稱為運(yùn)動方程運(yùn)動方程。101000)()(0n0n1 -n221110nOPRpaonTqTqT綜上：綜上：第25頁/共60頁正解正解反解反解第26頁/共60頁 1000zzzzyyyyxxxxHRpaonpaonpaonT 1000)()()()()()()()(22323423452346234652346523422323423415152341651623465234165162346523412232342341515234165162346523416516234652341654321aSaSaS

13、SSCCCCSCCSaCaCaCSCCSCSSSCCSCCCSSSCSSCCCSaCaCaCCCSSCCSSSCSCCCCCSSSSCCCCAAAAAATHR 給定機(jī)器人終端位姿，求各關(guān)節(jié)變量，給定機(jī)器人終端位姿，求各關(guān)節(jié)變量，稱求機(jī)器人運(yùn)動學(xué)逆解稱求機(jī)器人運(yùn)動學(xué)逆解。讓我們通過下面這道例題來了解一下機(jī)器人逆運(yùn)動學(xué)求解的一般步讓我們通過下面這道例題來了解一下機(jī)器人逆運(yùn)動學(xué)求解的一般步驟。前面例子最后方程為：驟。前面例子最后方程為：求逆運(yùn)動學(xué)方程的解求逆運(yùn)動學(xué)方程的解第27頁/共60頁 根據(jù)第根據(jù)第3行第行第4列元素對應(yīng)相等可得到列元素對應(yīng)相等可得到依次用依次用 左乘上面兩個(gè)矩陣，得到：左乘上面

14、兩個(gè)矩陣，得到：11A 10000100056565223234234523462346523462346523422323423452346234652346234652341111111111111111CSSCSaSaSaSSSCCCCSSCCCSaCaCaCSCCSCCCSSCCCCPSPCaSaCoSoCnSnpaonSPCPSaCaSoCoSnCnyxyxyxyxzzZzyxyxyxyx180)arctan(111 和和xypp第28頁/共60頁根據(jù)根據(jù)1，4元素和元素和2，4元素，可得到：元素，可得到：將上面兩個(gè)方程兩邊平方相加，并利用和差化積公式得到將上面兩個(gè)方程兩邊平方相加，

15、并利用和差化積公式得到322322242342423411332322322)()(cosaaaaaSpaCSpCpCCCSSzyx 于是有：于是有： 22323423422323423411aSaSaSpaCaCaCSpCpzyx 第29頁/共60頁已知已知2331CS 于是可得到：于是可得到：333arctanCS 依次類推，分別在方程依次類推，分別在方程2.19兩邊左乘兩邊左乘A1A4的逆，可得到的逆，可得到 100000001000)()()()(0)()()()(6656565565653423423411234234112342341123423411234111111434234

16、23411234234112342341123423411234CSCSSCSSSCCCaSaSpCPSPCSaCaSaCSoCoSoCSnCnSnCSaSaCoSoCnSnCaaCaCpSpSpCCaSaSaCCoSoSoCCnSnSnCCzyxzyxzyxzyxxyxyxyzyxxyxzyxzyxzyxyxzaaSaaSaCa)CSC180)arctan(1123423423423411234 （和和 第30頁/共60頁接下來再一次利用式接下來再一次利用式由于由于C12=C1C2-S1S2以及以及S12=S1C2+C1S2，最后得到：，最后得到：yxzyxxyzyxzyxyxzaCaSaS

17、aSaCCaSaCCaSaSaaSpaSaCSpCpaaCaCSpCpaSaSpaaC112341123451152341123453223444234334234112334234113342342332)(arctan)CCS)()()()(arctan （，可以得到，可以得到再根據(jù)對應(yīng)項(xiàng)元素相等再根據(jù)對應(yīng)項(xiàng)元素相等進(jìn)而可得：進(jìn)而可得：22323423422323423411aSaSaSpaCaCaCSpCpzyx 第31頁/共60頁最后用最后用A5的逆左乘式的逆左乘式2.67，再利用，再利用2,1元素和元素和2,2元素，得到：元素，得到：zyxzyxoCoSoCSnCnSnCS23411

18、234234112346)()(arctan 123456關(guān)節(jié)變量都是關(guān)節(jié)變量都是第32頁/共60頁 在實(shí)際應(yīng)用中，對運(yùn)動學(xué)的求解是相當(dāng)繁瑣和耗時(shí)的，因此在實(shí)際應(yīng)用中，對運(yùn)動學(xué)的求解是相當(dāng)繁瑣和耗時(shí)的，因此需要用計(jì)算機(jī)編程來實(shí)現(xiàn)。并且應(yīng)盡量避免使用矩陣求逆或高斯需要用計(jì)算機(jī)編程來實(shí)現(xiàn)。并且應(yīng)盡量避免使用矩陣求逆或高斯消去法等相對繁瑣的算法。正確的算法是：消去法等相對繁瑣的算法。正確的算法是：333arctanCS yxzyxzyxyxzaCaSaSaSaCCaSpaSaCSpCpaaCaCSpCpaSaSpaaC11234112345322344423433423411233423411334

19、2342332)(arctan)()()()(arctan zyxzyxoCoSoCSnCnSnCS23411234234112346)()(arctan )arctan(1xypp 第33頁/共60頁 利用本書中所介紹的四自由度機(jī)器人，結(jié)合本章所學(xué)的知識利用本書中所介紹的四自由度機(jī)器人，結(jié)合本章所學(xué)的知識進(jìn)行四自由度機(jī)器人的正逆運(yùn)動學(xué)分析。進(jìn)行四自由度機(jī)器人的正逆運(yùn)動學(xué)分析。 SCARASCARA型機(jī)器人的運(yùn)動學(xué)模型的建立，包括機(jī)器人運(yùn)動學(xué)方型機(jī)器人的運(yùn)動學(xué)模型的建立，包括機(jī)器人運(yùn)動學(xué)方程的表示，以及運(yùn)動學(xué)正解、逆解等，這些是研究機(jī)器人控制的程的表示，以及運(yùn)動學(xué)正解、逆解等，這些是研究機(jī)器人

20、控制的重要基礎(chǔ)，也是開放式機(jī)器人系統(tǒng)軌跡規(guī)劃的重要基礎(chǔ)。為了描重要基礎(chǔ)，也是開放式機(jī)器人系統(tǒng)軌跡規(guī)劃的重要基礎(chǔ)。為了描述述SCARASCARA型機(jī)器人各連桿之間的數(shù)學(xué)關(guān)系，采用型機(jī)器人各連桿之間的數(shù)學(xué)關(guān)系，采用D-HD-H法。法。SCARASCARA型型機(jī)器人操作臂可以看作是一個(gè)開式運(yùn)動鏈。它是由一系列連桿通機(jī)器人操作臂可以看作是一個(gè)開式運(yùn)動鏈。它是由一系列連桿通過轉(zhuǎn)動或移動關(guān)節(jié)串聯(lián)而成的。為了研究操作臂各連桿之間的位過轉(zhuǎn)動或移動關(guān)節(jié)串聯(lián)而成的。為了研究操作臂各連桿之間的位移關(guān)系，可在每個(gè)連桿上固接一個(gè)坐標(biāo)系，然后描述這些坐標(biāo)系移關(guān)系，可在每個(gè)連桿上固接一個(gè)坐標(biāo)系，然后描述這些坐標(biāo)系之間的關(guān)系

21、。之間的關(guān)系。第34頁/共60頁SCARASCARA（Selective Compliance Assembly Robot ArmSelective Compliance Assembly Robot Arm裝配機(jī)器人臂）機(jī)器人坐標(biāo)系的建立裝配機(jī)器人臂）機(jī)器人坐標(biāo)系的建立 1.SCARA機(jī)器人坐標(biāo)系建立原則根據(jù)機(jī)器人坐標(biāo)系建立原則根據(jù)D-H坐標(biāo)系建立方法，坐標(biāo)系建立方法，SCARA機(jī)器人的每個(gè)關(guān)節(jié)坐標(biāo)系的建立可參照以下的三原則機(jī)器人的每個(gè)關(guān)節(jié)坐標(biāo)系的建立可參照以下的三原則(1) 軸沿著第軸沿著第n個(gè)關(guān)節(jié)的運(yùn)動軸個(gè)關(guān)節(jié)的運(yùn)動軸;基坐標(biāo)系的選擇為基坐標(biāo)系的選擇為:當(dāng)?shù)谝魂P(guān)當(dāng)?shù)谝魂P(guān)節(jié)變量為零時(shí)，零

22、坐標(biāo)系與一坐標(biāo)系重合。節(jié)變量為零時(shí)，零坐標(biāo)系與一坐標(biāo)系重合。(2) 軸垂直于軸垂直于 軸并指向離開軸并指向離開 軸的方向。軸的方向。(3) 軸的方向按右手定則確定。軸的方向按右手定則確定。 2.構(gòu)件參數(shù)的確定根據(jù)構(gòu)件參數(shù)的確定根據(jù)D-H構(gòu)件坐標(biāo)系表示法，構(gòu)件本身構(gòu)件坐標(biāo)系表示法，構(gòu)件本身的結(jié)構(gòu)參數(shù)的結(jié)構(gòu)參數(shù) 、 和相對位置參數(shù)和相對位置參數(shù) 、 可由以下的方法確可由以下的方法確定定:(1) 為繞為繞 軸軸(按右手定則按右手定則)由由 軸到軸到 軸的關(guān)節(jié)角。軸的關(guān)節(jié)角。(2) 為沿為沿 軸，將軸，將 軸平移至軸平移至 軸的距離。軸的距離。(3) 為沿為沿 軸從軸從 量至量至 軸的距離。軸的距離。

23、(4) 為繞為繞 軸軸(按右手定則按右手定則)由由 軸到軸到 軸的偏轉(zhuǎn)角。軸的偏轉(zhuǎn)角。nznxnznynz1na1nndnnnz1nxnxndnz1nxnx1na1nx1nznz1n1nx1nznz第35頁/共60頁 3.變換矩陣的建立全部的連桿規(guī)定坐標(biāo)系之后，就可以按變換矩陣的建立全部的連桿規(guī)定坐標(biāo)系之后，就可以按照下列的順序來建立相鄰兩連桿照下列的順序來建立相鄰兩連桿n-1和和n之間的相對關(guān)系之間的相對關(guān)系:(1)繞繞 軸轉(zhuǎn)軸轉(zhuǎn) 角。角。(2)沿沿 軸移動軸移動 。(3)繞繞 軸轉(zhuǎn)軸轉(zhuǎn) 角。角。(4)沿沿 軸移動軸移動 。這種關(guān)系可由表示連桿這種關(guān)系可由表示連桿n對連桿對連桿n-1相對位置

24、齊次變換相對位置齊次變換 來表來表征。即：征。即：展開上式得展開上式得 1nx1n1nx1nanznnzndnnT1),(),(),(),(11111nntnnrnntnnrnndzTzTaxTxTT111111111cossin0sincoscoscossinsinsinsincossincoscos0001nnnnnnnnnnnnnnnnnnnadTd第36頁/共60頁由于由于 描述第描述第n個(gè)連桿相對于第個(gè)連桿相對于第n-1連桿的位姿，對于連桿的位姿，對于SCARA教學(xué)機(jī)器人教學(xué)機(jī)器人(四個(gè)自由度四個(gè)自由度)，機(jī)器人的末端裝置即為連桿，機(jī)器人的末端裝置即為連桿4的坐標(biāo)系，它與基座的關(guān)系為

25、的坐標(biāo)系，它與基座的關(guān)系為: nnT10012341234TT T T T第37頁/共60頁如上圖坐標(biāo)系，可寫出連桿如上圖坐標(biāo)系，可寫出連桿n相對于相對于n-1變換矩陣變換矩陣 : 其中：其中： 以下相同。以下相同。相應(yīng)連桿初始位置及參數(shù)列于表相應(yīng)連桿初始位置及參數(shù)列于表2.4，表中，表中 、 為關(guān)節(jié)變量。為關(guān)節(jié)變量。 nnT1111101000000100001csscT221221200000100001cslscT223310001000010001lTd444434000000100001csscTcos,sinnnnncsnnd構(gòu)件10001020010300104000101na1

26、nndn1cosn1sinn1l2l3d124第38頁/共60頁 各連桿變換矩陣相乘，可得到各連桿變換矩陣相乘，可得到SCARA機(jī)器人末端執(zhí)行器的位機(jī)器人末端執(zhí)行器的位姿方程姿方程(正運(yùn)動學(xué)方程正運(yùn)動學(xué)方程)為下為下 式它表示了式它表示了SCARA手臂變換矩陣手臂變換矩陣 ，它，它描述了末端連桿坐標(biāo)系描述了末端連桿坐標(biāo)系4相對基坐標(biāo)系相對基坐標(biāo)系0的位姿的位姿 。SCARA機(jī)器人的正運(yùn)動學(xué)分析機(jī)器人的正運(yùn)動學(xué)分析 40T 001234112233441 2 41 2 41 2 41 2 41 2 41 2 41 2 41 2 41 221 22111 2 41 2 41 2 41 2 41 2

27、 41 2 41 2 41 2 41 20 0 0 100 xxxxyyyyzzzzn o apn o apTTTT d Tn o apccc sss css scsccs sss csc sccccl ssl clscc csc sss ccsscs css ssc cccsc21 221130010000l csl sld第39頁/共60頁 SCARA機(jī)器人的逆運(yùn)動學(xué)分析機(jī)器人的逆運(yùn)動學(xué)分析 1.求關(guān)節(jié)變量求關(guān)節(jié)變量 為了分離變量，對方程的兩邊同時(shí)為了分離變量，對方程的兩邊同時(shí)左乘左乘 ,得得:即：即：1 0111T 010123114223344TTTTdT112 42 42 42 42

28、 21112 42 42 42 42 230 000 0000 1 000100 0 100010001xxxxyyyyzzzzcsnoapccs sc ssccllscnoapscc ss sccslnoapd第40頁/共60頁 左右矩陣中的第一行第四個(gè)元素左右矩陣中的第一行第四個(gè)元素(1.4)，第二行第四個(gè)元，第二行第四個(gè)元素素(2.4)分別相等。即分別相等。即: 由以上兩式聯(lián)立可得由以上兩式聯(lián)立可得: 式中：式中： 112211122cossincossincossinxyxyppllppl211arctanAA222212221;arctan2xyyxxyllpppAplpp第41頁/

29、共60頁2 求關(guān)節(jié)變量求關(guān)節(jié)變量 由式由式(2.87)可得可得: 式中：式中：21211sinarctancosrrl22;arctanyxyxprppp第42頁/共60頁3 求關(guān)節(jié)變量求關(guān)節(jié)變量 再令左右矩陣中的第三行第四個(gè)元再令左右矩陣中的第三行第四個(gè)元素素(3.4)相等，可得相等，可得: 4 求關(guān)節(jié)變量求關(guān)節(jié)變量 再令左右矩陣中的第一行第一個(gè)元再令左右矩陣中的第一行第一個(gè)元素、第二行第一個(gè)元素素、第二行第一個(gè)元素(1.1，2.1)分別相等，即：分別相等，即：由上兩式可求得由上兩式可求得: 3d3zdp 4112424112424cossincoscossinsinsincossincos

30、cossinxyxynnnn114211sincosarctancossinxyxynnnn第43頁/共60頁 至此，機(jī)器人的所有運(yùn)動學(xué)逆解都已求出。在逆解至此，機(jī)器人的所有運(yùn)動學(xué)逆解都已求出。在逆解的求解過程中只進(jìn)行了一次矩陣逆乘，從而使計(jì)算過程大的求解過程中只進(jìn)行了一次矩陣逆乘，從而使計(jì)算過程大為簡化，從為簡化，從 的表達(dá)式中可以看出它有兩個(gè)解，所以的表達(dá)式中可以看出它有兩個(gè)解，所以SCARA機(jī)器人應(yīng)該存在兩組解。運(yùn)動學(xué)分析提供了機(jī)器機(jī)器人應(yīng)該存在兩組解。運(yùn)動學(xué)分析提供了機(jī)器人運(yùn)動規(guī)劃和軌跡控制的理論基礎(chǔ)。人運(yùn)動規(guī)劃和軌跡控制的理論基礎(chǔ)。1 第44頁/共60頁對機(jī)器人相關(guān)概念的補(bǔ)充對機(jī)器人

31、相關(guān)概念的補(bǔ)充退化：退化：當(dāng)機(jī)器人失去一個(gè)自由度，并因此不按所期望的狀當(dāng)機(jī)器人失去一個(gè)自由度，并因此不按所期望的狀態(tài)運(yùn)動時(shí)即稱為退化。態(tài)運(yùn)動時(shí)即稱為退化。退化發(fā)生條件：退化發(fā)生條件：1.機(jī)器人達(dá)到物理極限，不能進(jìn)一步運(yùn)動機(jī)器人達(dá)到物理極限，不能進(jìn)一步運(yùn)動2.兩個(gè)相似關(guān)節(jié)共線兩個(gè)相似關(guān)節(jié)共線不靈巧區(qū)域：不靈巧區(qū)域：能對機(jī)器人定位不定姿的區(qū)域稱為不靈巧區(qū)能對機(jī)器人定位不定姿的區(qū)域稱為不靈巧區(qū)域。域。D-H法的局限性：法的局限性：無法表示關(guān)于無法表示關(guān)于y軸的運(yùn)動。軸的運(yùn)動。第45頁/共60頁退化狀態(tài)下的機(jī)器人退化狀態(tài)下的機(jī)器人第46頁/共60頁1 用矩陣表示點(diǎn)，向量，坐標(biāo)系及變換的方法用矩陣表示點(diǎn)

32、，向量，坐標(biāo)系及變換的方法2 正逆運(yùn)動學(xué)方程的建立正逆運(yùn)動學(xué)方程的建立3 用用D-H法建立坐標(biāo)系及變化方程法建立坐標(biāo)系及變化方程4 正逆運(yùn)動學(xué)方程的求解正逆運(yùn)動學(xué)方程的求解第47頁/共60頁第48頁/共60頁旋轉(zhuǎn)旋轉(zhuǎn)棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面平面第49頁/共60頁AiAi+1Ai-1第50頁/共60頁iidiAiAi+1iilid1iliAi-1idn l 和和 l之間的之間的夾夾 角，按右手定則角，按右手定則 由由l轉(zhuǎn)向轉(zhuǎn)向 l。 由運(yùn)動學(xué)的觀點(diǎn)來看，桿件保持其兩端關(guān)節(jié)間的形態(tài)由運(yùn)動學(xué)的觀點(diǎn)來看，桿件保持其兩端關(guān)節(jié)間的形態(tài)不變，這種形態(tài)由兩個(gè)參數(shù)決定：桿件長度不變，這種形態(tài)

33、由兩個(gè)參數(shù)決定：桿件長度 li 和桿件扭和桿件扭轉(zhuǎn)角轉(zhuǎn)角 。桿件的相對位置關(guān)系，由另外桿件的相對位置關(guān)系，由另外兩兩個(gè)參數(shù)決定：個(gè)參數(shù)決定：桿件的距離桿件的距離 di 和桿件的回轉(zhuǎn)角和桿件的回轉(zhuǎn)角 。iiilin li ii 第51頁/共60頁v 上述上述4個(gè)參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相個(gè)參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位置關(guān)系。在轉(zhuǎn)動關(guān)節(jié)中，對位置關(guān)系。在轉(zhuǎn)動關(guān)節(jié)中，li, i, di是固定值，是固定值，i是變量。是變量。在移動關(guān)節(jié)中，在移動關(guān)節(jié)中，li, i , i是固定值，是固定值， d i 是變量。是變量。第52頁/共60頁n 對于每個(gè)桿件都可以在關(guān)節(jié)軸處建立一個(gè)正規(guī)的笛對于每個(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）個(gè)坐標(biāo)）個(gè)坐標(biāo)系。系。n 基座坐標(biāo)系基座坐標(biāo)系 O0定義為定義為0號坐標(biāo)系（號坐標(biāo)系（x0, y0, z0）,它也它也是機(jī)器人的慣性坐標(biāo)系，是機(jī)器人的慣性坐標(biāo)系，0號坐標(biāo)系在基座上的位置號坐標(biāo)系在基座上的位置