第三章 機(jī)器人運(yùn)動(dòng)學(xué) 已知關(guān)節(jié)變量 ，求末端執(zhí)行器位姿 TnqqqQ ,., 11 ?E 本章要解決的問(wèn)題： 運(yùn)動(dòng)學(xué)正問(wèn)題： 運(yùn)動(dòng)學(xué)逆問(wèn)題： 已知 ，求 ?QE 3.1 桿件、關(guān)節(jié)、標(biāo)架、桿件變換 一、基本概念（圖 3 1） 圖 3-1 1、 桿件 （ Link): 操作機(jī)器人的每個(gè)運(yùn)動(dòng)單元，稱 為 圖 3-1 0L 2L nL 1L 1J 2J nJ :0L 機(jī)座，固定不動(dòng)，稱為 0號(hào)桿件； :1L 與機(jī)座相聯(lián)的第一個(gè)桿件； :2L 與第一個(gè)桿件相聯(lián)的后一個(gè)桿件； 與一般機(jī)構(gòu)不同，在談起機(jī)器人時(shí)，常說(shuō)機(jī)器人是多少個(gè) 自由度的，而不說(shuō)是幾桿機(jī)構(gòu) ！ 開(kāi)式鏈機(jī)器人用此標(biāo)法，并聯(lián)機(jī)器人有專用方法 。 2、 關(guān)節(jié) （ Joint): 連接相鄰兩個(gè)桿件的運(yùn)動(dòng)副，稱為 習(xí)慣上， :1J 連接 與 ； 0L 1L :2J 連接 與 ； 1L 2L 關(guān)節(jié)分類： 考慮到運(yùn)動(dòng)精度以及技術(shù)實(shí)現(xiàn)等原因，操作機(jī)器人一般 只用兩種形式的關(guān)節(jié)！ 回轉(zhuǎn)關(guān)節(jié)（ Revolute): 棱柱關(guān)節(jié)（ Prismatic): 只能單自由度回轉(zhuǎn)而不能移動(dòng)的低副； 只能單自由度移動(dòng)而不能回轉(zhuǎn)的低副； 3、 標(biāo)架 （ Frame): 在機(jī)器人學(xué)中，常將 桿件坐標(biāo)系 稱為。 4、 自由度 （ DOF Degree-Of-Freedom): 機(jī)器人末端執(zhí)行器所具有的獨(dú)立運(yùn)動(dòng)能力，稱為機(jī)器人 的 5、 活動(dòng)度 （ Mobility): 機(jī)器人所具有的關(guān)節(jié)數(shù)，稱為機(jī)器人的 自由度與活動(dòng)度間的關(guān)系？（人自腰部以上到手腕為多少？） 二、基本參數(shù) 1、 桿件參數(shù) （ i=1,2,n -1 )（圖 3 2） 桿長(zhǎng) ： ，桿件 i上兩條軸線間公法線長(zhǎng)度，稱為 ia 扭角 ： ， 桿件 i上兩條軸線在垂直于公法線平面內(nèi)的 夾角 ， 稱為 （ ） i i ia 圖 3-2 i用這兩個(gè)參數(shù)就可以確定相鄰兩條軸 線的相互位臵！； 桿件結(jié)構(gòu)確定了，這 2個(gè)參數(shù)為常量； i=0 及 i=n 時(shí)的參數(shù)呢？（僅 1根軸線） 待標(biāo)架建立后才 能確定！ 若桿件的兩根軸線平行 ， 桿件參數(shù)是多少 ？ （ 扭角： 縱向左右扭 ！ ） 桿件參數(shù)可能是負(fù)值嗎 ？ 2、 關(guān)節(jié)參數(shù) （ i=1,2,n -1 )（圖 3 3） 1i 1id 圖 3-3 i 1i 2iJ 1iJ iJ 關(guān)節(jié)角 ：關(guān)節(jié) 上 相鄰兩條公法線在垂直于 軸線平面內(nèi)的夾角，稱 為 1i 1iJ 1iJ 偏距 ：關(guān)節(jié) 上相鄰兩 條公法線沿 軸線測(cè)得的距 離，稱為 1id 1iJ 1iJ （ 軸上兩條公法線間的最短距 離） 1iJ 如同桿件參數(shù)一樣，關(guān)節(jié)參數(shù)也為非負(fù)值！ 對(duì)回轉(zhuǎn)關(guān)節(jié)： 是常量， 是變量； 1id 1i 對(duì)棱柱關(guān)節(jié)： 是變量， 是常量； 1id 1i i=0 及 i=n 時(shí)的參數(shù)呢？ 待標(biāo)架建立后才能確定！ 關(guān)節(jié)參數(shù)物理含義？ 決定了相鄰兩個(gè)桿件間相互位置； 11 , ii d 若相鄰 3根軸線平行，可直接可以看出關(guān)節(jié)角即是 i+1 桿 相對(duì)于 i 桿轉(zhuǎn)過(guò)的角度； （圖 3-4） 01 id iL 1iL 圖 3-4 在機(jī)器人中常用回轉(zhuǎn)關(guān)節(jié) ， 若 意味著相鄰兩個(gè)桿件不 上下交錯(cuò) ， 將可能導(dǎo)致關(guān)節(jié)角 小于 360 。 01 id 人臂結(jié)構(gòu)如何？ 三、 Denavit-Hartenberg標(biāo)架（簡(jiǎn)稱 D-H標(biāo)架） 1、桿件上標(biāo)架的建立 桿件標(biāo)架建立方法不一樣 ， 位姿 描述含義與結(jié)果也不一樣 ！ 為統(tǒng)一起見(jiàn) ， 均采用本講義 D-H 方法 （ 其它方法 ？ ） 1.1 中間桿件標(biāo)架 （ i=1,2,n -1)（圖 3-5） (1) ：與 軸線重合，正方向按照機(jī)器人 構(gòu)型 確定 ； iZ iJ (2) ：與 桿件公法線重合 ， 由 指向 ；當(dāng) 時(shí) ， 取 iX iL iJ 1iJ 0ia iii ZZX 1 （ 3） ：按照右手法則確定； iY iO iXiZ（ 4） ： 與 交點(diǎn)。 圖 3-5 i iJ iZ 1iJ iX iO 構(gòu)型：左構(gòu) （左臂）、 右 構(gòu) （右臂）。使機(jī)器人初 始轉(zhuǎn)動(dòng)角度為正值！ 特殊情況： 當(dāng) ，即 與 相交時(shí)， 取在交點(diǎn)處； 0ia iZ 1iZ iO 當(dāng) 時(shí)， 取在使 處； 1 ii ZZ iO 01 id 桿件標(biāo)架的建立需要兩根特殊的線：關(guān)節(jié)軸線、公法線！ 桿件的標(biāo)架位于桿件前一個(gè)關(guān)節(jié)軸線上！ 基座、末桿只有 1根軸線，其標(biāo)架需按照特殊的方法建立！ 1.2 機(jī)座標(biāo)架 0 機(jī)座標(biāo)架 與機(jī)座固聯(lián) ， 用來(lái)描述機(jī)器人各個(gè)桿件及末端 執(zhí)行器的位姿； 0 的建立可隨意 ， 但為了方便起見(jiàn) ， 一般規(guī)定： 當(dāng)?shù)谝粋€(gè) 關(guān)節(jié)變量為零值時(shí) ， 與 重合 。 0 10 因此，有： 0 ,0 00 a 當(dāng)?shù)谝粋€(gè)關(guān)節(jié)為回轉(zhuǎn)關(guān)節(jié)時(shí)，還有： 01d 當(dāng)?shù)谝粋€(gè)關(guān)節(jié)為棱柱關(guān)節(jié)時(shí)，還有： 01 1.3 末桿標(biāo)架 n 機(jī)器人末桿一端與前一個(gè)桿件相聯(lián) ， 另一端是 機(jī)械接口 ， 用以連接末端工具 ， 因此只有 1根軸線 。 其標(biāo)架建立原則與機(jī)座標(biāo)架相類似： 為了便于計(jì)算 ！ (1) ：與 軸線重合 ； nZ nJ (2) ：當(dāng)其關(guān)節(jié)變量為 0時(shí) ， 與 重合 。 nX 1nXnX 機(jī)座標(biāo)架看桿 1，末桿標(biāo)架看的是 n-1桿！ n 1n二者不同之處： 與 原點(diǎn)不重合！ 具體地： nJ 0n 1nXnX當(dāng) 為回轉(zhuǎn)關(guān)節(jié)且 時(shí)，取 與 重合 ； 因此 0nd nJ 0nd 1n XnX當(dāng) 為棱柱關(guān)節(jié)且 時(shí)，取 與 重合 ； 因此 0 n 1.4 末端執(zhí)行器標(biāo)架 E 末端執(zhí)行器不同 ， 其標(biāo)架不同 ， 詳見(jiàn)教科書 。 末端執(zhí)行器標(biāo)架與末桿標(biāo)架是平移關(guān)系 ！ 同一臺(tái)機(jī)器人可以使用不同的末端執(zhí)行器 ， 為此 ， 在樣本中 一般只給出桿件標(biāo)架參數(shù) 。 末端執(zhí)行器標(biāo)架視情況而定 。 2、 D-H參數(shù) 采用 D-H標(biāo)架 ， 用來(lái)描述機(jī)器人 各桿件標(biāo)架間 相對(duì)位姿關(guān) 系的參數(shù) ， 稱為 為何要采用 D-H參數(shù) ？ 如果要確定坐標(biāo)系間相對(duì)位姿關(guān)系，需要幾個(gè)參數(shù)？ 共有 4個(gè) D-H參數(shù)（圖 3-6）！ 圖 3-6 i iX iZ iO 1iX 1iZ 1iO :ia 從 到 ，沿 的距離； iZ 1iZ iX 從 到 ，繞 的角度； :i iZ 1iZ iX 從 到 ，沿 的距離； :1id iX 1iX 1iZ 從 到 ，繞 的角度； :1i iX 1iX 1iZ D-H參數(shù)仍然借用桿件參數(shù) 、 關(guān)節(jié) 參數(shù)的 符號(hào) ， 但有正負(fù)號(hào)了 ！ ！ 僅用 4個(gè)參數(shù) ！ 3、 桿件變換矩陣 對(duì)一臺(tái)機(jī)器人講，可有如下坐標(biāo)系： :0 機(jī)座標(biāo)架（參考坐標(biāo)系）； 桿件 1標(biāo)架； :1 桿件 n標(biāo)架； :n 末端執(zhí)行器坐標(biāo)系； :E 世界坐標(biāo)系 ； :W 研究機(jī)器人運(yùn)動(dòng)學(xué)時(shí)，一般只討論機(jī)器人本身?xiàng)U件標(biāo)架！ 所涉及的 桿件變換 有： TTT n n11201 ., , , 通式為： Ti i1 習(xí)慣上， 桿件變換矩陣 寫成 形式 ! ii A1 變換過(guò)程（？嘗試一下 D-H參數(shù)?。?變換順序（？）： （ 1）繞 旋轉(zhuǎn) ，使得 ； 1iX 1i ii ZZ 1 （ 3）繞 旋轉(zhuǎn) ，使得 iZ i ii XX 1 （ 4）沿 移動(dòng) ，使得 與 重合。 iZ id 1i i 桿件變換矩陣 （通式，學(xué)生做）： 10 0 ),0,0(),()0,0,(),( 1 1 1 111 111 11 1 ii ii i iiiii iiiii ii iiiii i cd sd a cscss scccs sc dT r a n szR o taT r a n sxR o tA 僅包含一個(gè)變量（回轉(zhuǎn)關(guān)節(jié)：關(guān)節(jié)角。棱柱關(guān)節(jié)：偏距）！ （ 2）沿 移動(dòng) ；使得 與 重合 ； 1iX 1ia 1iZ iZ 僅僅需要 4次變換即可 ！ （ D-H參數(shù)的優(yōu)點(diǎn) ！ 4個(gè)參數(shù)即可確 定相對(duì)位姿 ！ 樣本 、 論文中通常只給這 4個(gè)參數(shù) ！ ） 3.2 機(jī)器人運(yùn)動(dòng)學(xué) 一、 運(yùn)動(dòng)學(xué)基本方程 機(jī)器人末桿標(biāo)架相對(duì)于機(jī)座的齊次變換矩陣為： nnn AAAAT 13221100 . 簡(jiǎn)記為： nn AAAT .21 上式即為 機(jī)器人 運(yùn)動(dòng)學(xué)基本方程 。 二、 運(yùn)動(dòng)學(xué)反解（？） 運(yùn)動(dòng)學(xué)反解是機(jī)器人控制的基礎(chǔ)！ 1、解存在域 在 工作空間 外，無(wú)解！ 在工作空間內(nèi)，？ 使用時(shí)首先 要選擇合適的 機(jī)器人，且安 裝位臵要恰當(dāng)！ 2、求解分析（以 6DOF機(jī)器人為例！？） 6216 . . . AAAT 已知： 10 0 6 6 z y x p p p R T 分析： （ 1）方程右端每個(gè)桿件矩陣中含有 1個(gè)變量，共有 6個(gè)未知數(shù)； （ 2）矩陣相等，其每個(gè)對(duì)應(yīng)元素相等，共有 12個(gè)有效等式。 如何從 12個(gè)等式中挑出 6個(gè) 獨(dú)立的方程 來(lái)？ 要求解的方程為 超越方程（ 三角函數(shù)方程），如何求解？ 3、求解方法 （ 1）數(shù)值法。任意 6自由度串聯(lián)機(jī)器人都有 迭代 解 。缺點(diǎn)： 求解時(shí)間不固定，不適合控制應(yīng)用（？）。 （ 2）解析法?？梢缘玫?閉式解 ， 適合控制應(yīng)用（？）。 4、解析解存在條件 不是任意情況的機(jī)器人都有閉式解！ 必要條件： 0i 或 90i 物理含義？ 由于運(yùn)動(dòng)學(xué)反解是控制的基礎(chǔ)，因此操作機(jī)器人一般都要 滿足此必要條件！（在結(jié)構(gòu)上進(jìn)行了限制?。?充分條件（ Pieper條件）： 機(jī)器人機(jī)構(gòu)中有 3個(gè)相鄰的關(guān)節(jié)軸線相交于一點(diǎn)或平行。 若 3根相鄰的關(guān)節(jié)軸線相交于一點(diǎn)，其結(jié)構(gòu)將很緊湊。常將 此形式的機(jī)構(gòu)稱為 手腕 （或 球腕 ），放在機(jī)器人末端使用！ 典型操作機(jī)器人形式： 操作臂手腕 實(shí)現(xiàn)位置 實(shí)現(xiàn)姿態(tài) 帶球腕的機(jī)器人還有特殊解法，后面將討論！ 解析解存在的充要條件？至今還未給出！ 5、重解問(wèn)題 對(duì)于給定的一個(gè)位姿，常常有多組關(guān)節(jié)變量相對(duì)應(yīng)，這種現(xiàn) 象稱為。 示例（圖 3-7） 圖 3-7 都可行。哪一組最好？ （必須作出選擇?。?最小能量約束，關(guān)節(jié)運(yùn)動(dòng)范圍限制， 6、 分離關(guān)節(jié)變量法 分離關(guān)節(jié)變量法 基本思想： 關(guān)節(jié)變量以三角函數(shù)形式出現(xiàn)； 若能將等式一端化為某一關(guān)節(jié)變量的三角函數(shù) ， 另一端 為常量 ， 則可以用反三角函數(shù)法求出待求變量 。 小結(jié)：反解時(shí)需解決的問(wèn)題： 求解方法問(wèn)題，解析解存在條件問(wèn)題， 6個(gè)獨(dú)立方程選 擇問(wèn)題，超越方程問(wèn)題，重解問(wèn)題， 不用數(shù)值計(jì)算法；解前驗(yàn)證條件；與下一個(gè)問(wèn)題一起解決！關(guān) 鍵是反三角函數(shù)問(wèn)題；應(yīng)用附加約束條件，例如最小能量約束 等； 求解關(guān)鍵：如何能化成上述情況 ！ 采用的是逆向思維方法進(jìn)行求解！ 基本過(guò)程： （ 2）方程兩端同時(shí)左乘以 ，得： 11A （ 1）先試探一下可否根據(jù)運(yùn)動(dòng)學(xué)基本方程解出某些變量； 若不能，則繼續(xù)步驟（ 2）； 65432611 AAAAATA 上式左端只含有桿件 1的關(guān)節(jié)變量；若在方程右端能找到一 個(gè)對(duì)應(yīng)的常數(shù)項(xiàng) ， 則可以求出桿件 1的關(guān)節(jié)變量了 。 解得后 ， 再看一下能否解出其它變量 ， 能解出 ， 解之；否則繼續(xù)左乘 下一個(gè)桿件矩陣的逆 ， 依次類推 ， 直到解得全部變量 。 用左乘的方法將變量依次隔離出來(lái)，因此，稱為 。 有些學(xué)者建議一開(kāi)始就進(jìn)行左乘分離?。逼鋵?shí)） 矩陣乘法操作小竅門?。◤挠叶碎_(kāi)始！？減少計(jì)算量） 例題： ),(),(),(),(2 zR o tyR o tzR o tE u l e r 10 0),(2 zzz yyy xxx aon aon aon E u l e r 已知： 解： 1000 0 0 0 10 0 csscs ssccscsscccs sccssccssccc aon aon aon zzz yyy xxx 解法 1： caz zaarc c o s sca x sa xarc c o s csn z sn zarc c o s 結(jié)果出來(lái)了， 可以嗎？ 存在的問(wèn)題： 例如： caz （ 1）反三角函數(shù)多值問(wèn)題。 當(dāng) 時(shí)，其反函數(shù)值至少有兩個(gè)（限定為 1轉(zhuǎn)時(shí)）： 0 za 900 090 及 （ 2）反三角函數(shù)值精度問(wèn)題：當(dāng)取一些值時(shí)，誤差較大。 例如： 9.0 za 94.0za 與 0arc c o s za 但對(duì)于一些定位精度要求較高的機(jī)器人來(lái)說(shuō) ， 可能造成較 大的位置誤差 。 不宜用反正弦或反余弦函數(shù)求關(guān)節(jié)角度值！ 解法 2： tgaa x y ),(2t an xy x y aaa a aa r ct g stgaa z y ),(2t an saaasa aa r ct g zy z y tgno z z ),(2t a n zz z z noanoa r c tg 0s 解法 2可行嗎？ 反正切 、 反余切采用了 2個(gè)參數(shù)求角度值 ， 其結(jié)果唯一 。 反正切 、 反余切靈敏度高 ， 誤差小 ！ 要采用反正切或反余切計(jì)算關(guān)節(jié)角度值！ 7、帶球腕機(jī)器人運(yùn)動(dòng)學(xué)反解 球腕運(yùn)動(dòng)學(xué)特點(diǎn)分析： 球腕： 3個(gè)關(guān)節(jié)軸相交于一點(diǎn) 。 球腕 3個(gè)桿件的標(biāo)架原點(diǎn)重合在一起 。 球腕 3個(gè)關(guān)節(jié)變量不影響末桿的位臵?。ㄖ挥绊懽藨B(tài)） 末桿位臵完全由前 3個(gè)桿件確定。 帶球腕機(jī)器人運(yùn)動(dòng)學(xué)反解方法： （ 1） 將 、 、 相乘 ， 由這三個(gè)矩陣相乘得到的位置 值即是整個(gè)機(jī)器人的位置值 。 據(jù)此 ， 可以解得 1A 2A 3A 321 , , qqq （ 2） 求出后， 變?yōu)槌Ａ?，將它們分離出，根 據(jù) 可以進(jìn)一步求出 321 AAA 321 , , qqq 65461321 )( AAATAAA 654 , , qqq 將 6個(gè)聯(lián)立方程變成 2組 3元聯(lián)立方程，求解難度自然降低了！ 帶球腕機(jī)器人運(yùn)動(dòng)特點(diǎn)： 球腕一般放在機(jī)器人末端； 前三個(gè)自由度用于實(shí)現(xiàn)位姿，后三個(gè)用于調(diào)整姿態(tài)； 反解簡(jiǎn)單； 示教方便，便于應(yīng)用！ 擬人式機(jī)器人（圖 3-8）？ 圖 3-8 擬人式機(jī)器人 3.3 機(jī)器人工作空間 一、概念： 1. 工作空間 ： 機(jī)器人反解存在的區(qū)域，稱為 為機(jī)器人可達(dá)位姿的集合。 2. 靈活工作空間 ： 在工作空間中，機(jī)器人的末端執(zhí)行器能夠從機(jī)構(gòu)允 許的各個(gè)方向到達(dá)的位姿點(diǎn)的集合，稱為 3. 可達(dá)工作空間 ： 機(jī)器人末端執(zhí)行器至少能以某一姿態(tài)到達(dá)的區(qū)域， 稱為 靈活工作空間 可達(dá)工作空間工作空間 對(duì)機(jī)器人應(yīng)用有意義的空間：靈活工作空間！ 機(jī)器人樣本中給出的工作空間：機(jī)械接口的位姿空間 二、示例： 平面 2桿 2DOF機(jī)器人（圖 3 9） 設(shè) 3600 21 圖 3-9 示例 1 2 1l 2l 半徑為 2l的圓面 原點(diǎn) 無(wú) 外徑為 ，內(nèi)徑為 的圓環(huán) 21 ll 21 ll 可達(dá)空間 靈活工作空間 21 ll lll 21 在邊界上，只有一種姿態(tài)； 在可達(dá)空間內(nèi)，只有 2種姿態(tài)； 若不可靈活地工作，則會(huì)大大降低機(jī)器人使用價(jià)值 ！ 對(duì)圖例機(jī)器人，只需在腕部再增加一個(gè)回轉(zhuǎn)關(guān)節(jié)自由度，則 可使可達(dá)空間變成靈活工作空間（見(jiàn)黑板）。 設(shè)計(jì)機(jī)器人時(shí)，應(yīng)該保證 可達(dá)空間即為靈活工作空間 ！ 鑒于此，習(xí)慣上只用“工作空間”術(shù)語(yǔ)！ 三、工作空間繪制 機(jī)器人只能在其工作空間中運(yùn)動(dòng)，因此，了解其工作空間 形狀、大小等是必須的！ 繪制方法： 1. 計(jì)算機(jī)繪圖。由計(jì)算機(jī)根據(jù)其運(yùn)動(dòng)學(xué)方程繪出位姿 點(diǎn)圖集，例如 蒙特卡羅法。 2. 手工繪制。對(duì)一些簡(jiǎn)單的情況，可以人工繪出工作 空間，關(guān)鍵是給出 工作空間邊界 。 因結(jié)構(gòu)上的限制，機(jī)器人關(guān)節(jié)變量范圍一般有所限制 ！ 習(xí)題 : Daikin s1400 SCARA型機(jī)器人 圖 3-9 s1400 l 2l1l 1 2 3 ,3 0 0 ,3 5 0 ,3 0 00 3 6 00 ,1 4 530 ,2 5 00 21 321 mmlmmlmml 問(wèn)題： 1. 什么因素影響關(guān)節(jié)運(yùn)動(dòng)范圍？ 2. 回轉(zhuǎn)關(guān)節(jié)有無(wú)必要可以整周回轉(zhuǎn)？ 3. 工作空間形狀、大小與哪些因素有關(guān)？ 4. 試討論一下 SCARA機(jī)器人大、小臂各自的作用。 5. 試討論一下搬運(yùn)、平面裝配、點(diǎn)焊、弧焊、垂直面噴 漆所需工作空間形狀。