《機(jī)器人技術(shù)第三章機(jī)器人運(yùn)動學(xué)及其數(shù)學(xué)基礎(chǔ).ppt》由會員分享，可在線閱讀，更多相關(guān)《機(jī)器人技術(shù)第三章機(jī)器人運(yùn)動學(xué)及其數(shù)學(xué)基礎(chǔ).ppt（150頁珍藏版）》請?jiān)谘b配圖網(wǎng)上搜索。

1、I. 機(jī)器人學(xué) 機(jī)器人學(xué) 機(jī)械電子工程 Dr. Kevin Craig I. 機(jī)器人學(xué) IEEE International Conference on Robotics and Automation (ICRA) 2010 安克雷奇 文章： 856/2034 分會場： 154 國家： 47 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2009 圣路易斯 文章： 936/1599 分會場： 192 國家： 5

2、3 I. 機(jī)器人學(xué) Technical Session的主要內(nèi)容 Human robot interaction Medical robotics Sensor fusion Legged robots Underwater robots Manipulator motion planning Camera calibration Intelligent transportation systems SLAM: Features and landmarks Humanoid robot body motion Microrobots Biologically

3、-inspired robotic devices Rehabilitation robotics Field robotics Grasping Nanorobotic manipulation Fish-like robot Parallel robot 第二章 機(jī)器人運(yùn)動學(xué)及其數(shù)學(xué)基礎(chǔ) 參考教材 美 付京遜 機(jī)器人學(xué) 中南大學(xué) 蔡自興 機(jī)器人學(xué) 美 理查德 鮑爾 機(jī)器人操作手 數(shù) 學(xué) 編程與控制 參考教材 美 付京遜 機(jī)器人學(xué) 美籍華人 普渡大學(xué)（ Purdue University）電機(jī)工程專業(yè)著 名教授 4部

4、著作、 400多篇論文 第一任國際模式識別學(xué)會會長，被譽(yù)為自動模 式識別之父 1985年去世 參考教材 中南大學(xué) 蔡自興 中南大學(xué)教授，我國人工 智能和機(jī)器人領(lǐng)域著名專 家 中國人工智能學(xué)會智能機(jī) 器人專委會理事長 曾與付京遜教授一起工作 過 第一節(jié) 引言 串聯(lián)機(jī)器人可以用一個開環(huán)關(guān)節(jié)鏈來建模 由數(shù)個驅(qū)動器驅(qū)動的 轉(zhuǎn)動 或 移動 關(guān)節(jié)串聯(lián)而成 一端固定在基座上，另一端是自由的，安裝工具（末端 執(zhí)行器），用以操縱物體，或完成各種任務(wù) i n o a 關(guān)節(jié)的 相對運(yùn)動 導(dǎo)致桿件的運(yùn)動， 使末端執(zhí)行器定位于所需要的方 位上 在一般機(jī)器人應(yīng)用問題中，人

5、們 感興趣的是：末端執(zhí)行器相對于 固定參考坐標(biāo)數(shù)的 空間幾何描述 ， 也就是機(jī)器人的運(yùn)動學(xué)問題 機(jī)器人的運(yùn)動學(xué)即是研究機(jī)器人 手臂 末端執(zhí)行器位置和姿態(tài) 與 關(guān) 節(jié)變量空間 之間的關(guān)系 運(yùn)動學(xué)研究的問題 Where is my hand? Direct Kinematics HERE! How do I put my hand here? Inverse Kinematics: Choose these angles! 運(yùn)動學(xué)正問題 運(yùn)動學(xué)逆問題 哈佛大學(xué) Roger Brockett建立的指數(shù)積公 式 運(yùn)動學(xué) 滾動接觸 非完整控制 數(shù)學(xué)基礎(chǔ) -剛體運(yùn)動

6、 參考文獻(xiàn)： 機(jī)器人操作的數(shù)學(xué)導(dǎo)論 作者：理查德 摩雷 李澤湘 夏卡恩 薩斯特里 翻譯：徐衛(wèi)良 錢瑞明（東南大學(xué)） 研究運(yùn)動學(xué)的方法 1955年丹納維特（ Denavit）和哈頓伯格（ Hartenberg）提 出了一種 采用矩陣代數(shù)方法 解決機(jī)器人的運(yùn)動學(xué)問題 D-H 方法，其數(shù)學(xué)基礎(chǔ)即是 齊次變換 具有直觀的幾何意義 能表達(dá)動力學(xué)、計算機(jī)視覺和 比例變換問題 為以后的比例變換、透視變換 等打下基礎(chǔ) 1000 p p p T z yyy xxx zzz y

7、 x w w w 第二節(jié) 數(shù)學(xué)基礎(chǔ) 齊次坐標(biāo)和齊次變換 2.1 點(diǎn)和面的齊次坐標(biāo) 2.1.1 點(diǎn)的齊次坐標(biāo) 一般來說， n維空間的齊次坐標(biāo)表示是一個（ n+1）維空間實(shí)體。有一 個特定的投影附加于 n維空間，也可以把它看作一個附加于每個矢量的 特定坐標(biāo) 比例系數(shù)。 引入齊次坐標(biāo)的目的是為了表示幾何變換的旋轉(zhuǎn)、平移和縮放 kcjbiav zy x Tw w z y x V 式中 i, j, k為 x, y, z 軸上的單位矢量， a= , b= , c= ， w為比例系數(shù) w x w y w z

8、顯然，齊次坐標(biāo)表達(dá) 并不是唯一 的，隨 w值的不同而不同。在計算機(jī)圖學(xué)中， w 作為通用比例因子，它可取任意正值，但 在機(jī)器人的運(yùn)動分析中，總是取 w=1 。 列矩陣 一個點(diǎn)矢： 例 1： kjiV 543 可以表示為： V=3 4 5 1T 或 V=6 8 10 2T 或 V=-12 -16 -20 -4T 齊次坐標(biāo)與三維直角坐標(biāo)的區(qū)別 V點(diǎn)在 O XYZ坐標(biāo)系中表 示是 唯一 的（ a、 b、 c） 而在齊次坐標(biāo)中表示可 以是多值的。 不同的表 示方法代表的 V點(diǎn)在空間 位置上不變。 x y z z z x V 圖 2 - 2 o 幾個特定

9、意義的齊次坐標(biāo)： 0 0 0 nT 坐標(biāo)原點(diǎn)矢量的齊次坐標(biāo)， n為任 意非零比例系數(shù) 1 0 0 0T 指向無窮遠(yuǎn)處的 OX軸 0 1 0 0T 指向無窮遠(yuǎn)處的 OY軸 0 0 1 0T 指向無窮遠(yuǎn)處的 OZ軸 0 0 0 0T 沒有意義 2個常用的公式： zzyyxx babababa kbabajbabaibaba bbb aaa kji ba xyyxzxxzyzzy zyx zyx )()()( 點(diǎn)乘 : 叉乘 : 2.1.2 平面的齊次坐標(biāo) 平面齊次坐標(biāo)由 行矩陣 P=a b c d 來表示 當(dāng)點(diǎn) v=x y z wT處于平面 P內(nèi)時，

10、矩陣乘積 PV=0，或記為 0 dwczbyax w z y x dcbaPV 與點(diǎn)矢 相仿，平面 也沒有意義 T0000 0000 點(diǎn)和平面間的位置關(guān)系 設(shè)一個平行于 x、 y軸 ， 且在 z軸上的坐標(biāo)為單位距離的平面 P可以表示為： 或 有： PV= 1100 P 2200 P v0 v0 v0 點(diǎn)在平面下方 點(diǎn)在平面上 點(diǎn)在平面上方 例如：點(diǎn) V=10 20 1 1T 必定處于此平面內(nèi) ， 而點(diǎn) V=0 0 2 1T 處于平 P

11、的上方 ， 點(diǎn) V=0 0 0 1T處于 P平面下方 ， 因?yàn)椋? 0 1 1 20 10 101000 0 1 1 2 0 0 1100 0 -1 1 0 0 0 1-100 2.2 旋轉(zhuǎn)矩陣及旋轉(zhuǎn)齊次變換 2.2.1 旋轉(zhuǎn)矩陣 設(shè)固定參考坐標(biāo)系直角坐標(biāo)為 Oxyz， 動坐標(biāo)系為 Ouvw， 研究旋轉(zhuǎn)變換情況 。 x y z w v u P o ( O ) 圖 2 - 3 初始位置時，動靜坐標(biāo)系重合， O、 O 重合，如圖。各軸對 應(yīng)重合，設(shè) P點(diǎn)是動坐標(biāo)系 Ouvw中的一點(diǎn)，且固定

12、不變。則 P 點(diǎn)在 Ouvw中可表示為： wwvvuuu v w kPjPiPP 、 、 為坐標(biāo)系 Ouvw的單位矢量 ， 則 P點(diǎn)在 oxyz中可表示為： ui vj wk zzyyxxxyz kPjPiPP xyzu v w PP 當(dāng)動坐標(biāo)系 Ouvw繞 O點(diǎn)回轉(zhuǎn)時，求 P點(diǎn)在固定坐標(biāo)系 oxyz 中的位置 y z x o ( O ) u v w P Pw Pv Pu 圖 2 - 4 已知： P點(diǎn)在 Ouvw中是不變的仍然成 立，由于 Ouvw回轉(zhuǎn)，則： wwvvuuu v w kPjPiPP xwwvvuuxu v wx ikPjPiPiP )(P ywwvv

13、uuyu v wy jkPjPiPjP )(P zwwvvuuzu v wz kkPjPiPkP )(P 用矩陣表示為 : w v wzvzz wvyy wxvxx z y x P P P kkjkik kjjjij kijiii P P P y （ 2-7） u v wxyz wzvzz wvyy wxvxx PRp kkjkik kjjjij kijiii :R y 則旋轉(zhuǎn)矩陣為：定義 反過來： x y zu v w PRP 1 R RR de t *1 T1 RR Rd

14、 e t 是正交矩陣，的行列式，為的伴隨矩陣，為 RRRR 2.2.2 旋轉(zhuǎn)齊次變換 用齊次坐標(biāo)變換來表示式（ 2-7） 11000 0 0 0 1 w v u z y x P P P R P P P 11000 0 0 0 1 1 z y x w v u P P P R P P P 2.2.3 三個基本旋轉(zhuǎn)矩陣和合成旋轉(zhuǎn)矩陣 三個基本旋轉(zhuǎn)矩陣 ),( xR 即動坐標(biāo)系 求 的旋轉(zhuǎn)矩陣 ， 也就是 求出坐標(biāo)系

15、 中各軸單位矢量 在固定坐標(biāo)系 中各軸的投影分量 ， 很容易得到在兩個坐標(biāo)系重合時 ， 有： 角，軸轉(zhuǎn)動繞， XOvwO vwO wv kji ,, Oxyz ),( xR 100 010 001 R wzvzz wvyy wxvxx kkjkik kjjjij kijiii y)R ( x , x y z o u v w U V W O 圖 2 - 5 ss in0 s inc o s0 001 co ii ux 方向余弦陣 同理： c o s0

16、s in 010 s in0c o s )y,R （ 100 0c o ss in 0s in-c o s )z,R （ ss in0 s inc o s0 001 )R ( x , co 三個基本旋轉(zhuǎn)矩陣 : x y z o u v w U W O x y z o u v w U W O v 合成旋轉(zhuǎn)矩陣 : 例 1：在動坐標(biāo)中有一固定點(diǎn) ，相對固定參 考坐標(biāo)系 做如下運(yùn)動： R（ x, 90 ）； R(z, 90 )； R(y,90 )。求運(yùn)動后點(diǎn) 在固定參考

17、坐標(biāo)系 下的位置。 Tu v wPo 1321 Oxyz uvwPo Oxyz 解 1：用畫圖的簡單方法 解 2：用分步計算的方法 R（ x, 90 ） R（ z, 90 ） R（ y, 90 ） 1 2 3 1 1 3 2 1 1000 0010 01-00 0001 P 1 2 1 3 1 2 3 1 1000 0100 0001 001-0 P

18、 1 3 1 2 1 2 1 3 1000 0001- 0010 0100 P （ 2-14） （ 2-15） （ 2-16） 上述計算方法非常繁瑣，可以通過一系列計算得到上述 結(jié)果。將式（ 2-14）（ 2-15）（ 2-16）聯(lián)寫為如下形式： 110001 33 w v u z y x P P P R P P P R3x3為二者之間的關(guān)系矩陣，我們令： ),(),(),RR 33 xRzRy（ 定義 1： 當(dāng)動坐標(biāo)系 繞固定坐標(biāo)系 各坐標(biāo)軸順序有

19、限次 轉(zhuǎn)動時，其合成旋轉(zhuǎn)矩陣為各基本旋轉(zhuǎn)矩陣依旋轉(zhuǎn)順序 左乘 。 注意： 旋轉(zhuǎn)矩陣間 不可以交換 uvwO Oxyz 平移齊次變換矩陣 1000 100 010 001 c) b (a T r a n sH c b a 注意： 平移矩陣間可以交換， 平移和旋轉(zhuǎn)矩陣間不可以交換 z y x o o w u v a b c 2.2.4 相對變換 舉例說明： 例 1： 動坐標(biāo)系 0起始位置與固定參考坐標(biāo)系 0重合 ,動坐標(biāo)系 0做如下運(yùn)動： R(Z,90) R（ y,90） Trans(4， -3, 7)， 求合成矩陣 解 1：用畫圖的方

20、法： o z y x 7 4 - 3 o w u v v u w z y x o o(o ) x y z u v w z y x u w o ( o ) v 解 2：用計算的方法 根據(jù)定義 1， 我們有： 1000 7010 3001 4100 )R ( Z ,90 )90 R ( y , 7) ,3- ,T r a n s ( 4T 以上均以固定坐標(biāo)系多軸為變換基準(zhǔn) ， 因此矩陣左乘 。 如果我們做如下變換，也可以得到相同的結(jié)果： 例 2： 先平移 Trans (4,-3,7)； 繞當(dāng)前 軸轉(zhuǎn)動 90；

21、繞當(dāng)前 軸轉(zhuǎn)動 90；求合成旋轉(zhuǎn)矩陣 。 v w （ 2-20） 解 1：用畫圖的方法 z y x o( o ) v w u z y x o o w u v o z y x o w v u x y z o o w u v 解 2：用計算的方法 1000 7010 3001 4100 )R ( Z ,9 0 )90 R ( y , 7) ,3- ,T r a n s ( 4T oo （ 2-21） 式 （ 2-20） 和式 （ 2-21） 無論在形式上 ， 還是在結(jié)果上都是 一致的 。 因此我們有如下的結(jié)論： 動坐標(biāo)系在固定坐標(biāo)系中的齊

22、次變換有 2種情況： 定義 1： 如果所有的變換都是 相對于固定坐標(biāo)系 中各坐標(biāo)軸旋 轉(zhuǎn)或平移 ， 則依次 左乘 ， 稱為 絕對變換 。 定義 2： 如果動坐標(biāo)系 相對于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸 旋轉(zhuǎn)或 平移，則齊次變換為依次 右乘 ，稱為 相對變換 。 結(jié)果均為動坐標(biāo)系在固定坐標(biāo)中的位姿 （ 位置 +姿態(tài) ） 。 相 對于固定坐標(biāo)系 ， 軸。軸相當(dāng)于軸，軸相對于軸，軸相當(dāng)于 ZYX wv 也就是說，動坐標(biāo)系繞自身坐標(biāo)軸做齊次變換， 要達(dá)到繞固 定坐標(biāo)系相等的結(jié)果，就應(yīng)該用相反的順序。 右乘的意義： 機(jī)器人用到相對變換的 時候比較多 例如機(jī)械手抓一個杯子， 如右圖所示，手爪

23、需要 轉(zhuǎn)動一個角度才抓的牢， 相對于固定坐標(biāo)系表達(dá) 太麻煩，可以直接根據(jù) 手爪的坐標(biāo)系表示 但也要知道在 O中的位 姿，就用右乘的概念。 x y z o H 2.2.5 繞通過原點(diǎn)的任意軸旋轉(zhuǎn)的齊次變換 有時動坐標(biāo)系 O可能繞過原點(diǎn) O的分量分別為 rx、 ry、 rz的 任意單位矢量 r 轉(zhuǎn)動 角。 研究這種轉(zhuǎn)動的好處是可用 O繞某軸 r 的一次轉(zhuǎn)動代替繞 O各坐標(biāo)軸的數(shù)次轉(zhuǎn)動 為推導(dǎo)此旋轉(zhuǎn)矩陣，可作下述 5步變換： 1. 繞 X 軸轉(zhuǎn) 角， 使 r 軸處于 XZ平面內(nèi) 2. 繞 Y 軸轉(zhuǎn) - 角，使 r 軸與 OZ軸重合 3. 繞 OZ軸

24、轉(zhuǎn)動 角 4. 繞 Y 軸轉(zhuǎn) 角 5. 繞 X 軸轉(zhuǎn) - 角 X Y Z r x r y r z A B C D B O 5 1 2 43 r A 由上圖容易求出： 2 z 2 y y rr rs in 2 z 2 y z rr rc o s x x r r r r OCs in 2 z 2 y 2 z 2 y rr r rr OB CBc o s 由定義 1和定義 2，上述 5次旋轉(zhuǎn)的合成旋轉(zhuǎn)矩陣為： c o ss i n0 s

25、i n-c o s0 001 c o s0s i n 010 s i n-0c o s 100 0c o ss i n 0s i n-c o s c o s0s i n- 010 s i n0c o s c o ss i n0 s i nc o s0 001 RRRRRR ,x,y,z,y,x,r （ 2-25） X Y Z r x r y r z A B C D B O 5 1 2 43 r A 帶入式 （ 2-25），得 c os)c os(1r s i nr)c os(1rr s i nr)c os(1rr s i nr)c o

26、s(1rrs i nr)c os(1rr c os)c os(1rs i nr)c os(1rr s i nr)c os(1rrc os)c os(1r R 2 z xzy yzx xzyyzx 2 yzyx zyx 2 x ,r 由該式可以推出 3個基本旋轉(zhuǎn)矩陣 2.2.6 齊次變換矩陣的幾何意義 設(shè)，有一個手爪，即動坐標(biāo)系 O，已知， 初始位置 重合，那么 O在 O中的齊次坐標(biāo)變換為： ，如果手爪轉(zhuǎn)了一個角度， 則： 111 cbao 1000 100 010 001 T 1 1 1 1

27、 c b a 1000 p p p T z yyy xxx zzz y x w w w T反映了 O在 O中的位置和姿態(tài) ， 即表示了該坐標(biāo)系原點(diǎn) 和各坐標(biāo)軸單位矢量在固定坐標(biāo)系中的位置和姿態(tài) 。 該矩陣可以由 4個子矩陣組成 ， 寫成如下形式： 比例系數(shù)透視矩陣 位置矢量旋轉(zhuǎn)矩陣 1131 1333 wf PR T zzz yyy xxx w w w 33R 為 姿態(tài)矩陣（旋轉(zhuǎn)矩陣） ，表示動坐標(biāo)系 O在固定參考坐標(biāo)系 O中的姿態(tài)，即表示 O各坐標(biāo)軸單位矢量在 O各軸上的投

28、影 為 位置矢量矩陣 ，代表動坐標(biāo)系 O坐標(biāo)原點(diǎn) 在固定參考坐標(biāo)系 O中的位置 Tzyx ppp P 13 為 透視變換矩陣 ，在視覺中進(jìn)行圖像計算，一 般置為 0 00031 f 為 比例系數(shù) 1 11 w 如果需要求解 O在 O中的位置和姿態(tài)，此時的齊次變換矩 陣為 ，即求逆矩陣： 1T 1000 - -R - T T T1- 33 T 1 pw pv p ）（ ）（ ）（ kpjpipp zyx kji zyx kvjvivv zyx kwjwiww zyx 其中： 這些式子以后經(jīng)常遇到， 在機(jī)器人計算

29、中，所要 求的就是齊次變換矩陣 2.2.7 透鏡成像的齊次變換 p p : P x 1 P P x 1 1 () 1 pp pp T T yz yz y p zp zp f y p zp zp y p f y p f zp x p y p f zp x p y p y p f y p f y p f y p y p y p f y p y p f f xp xp y 以光心為原點(diǎn)O，光軸與y軸重合，P 為物點(diǎn)， 用齊次坐標(biāo)表示 求 的齊次坐標(biāo)，即求 根據(jù)三角形相似原理： 注意 是負(fù)值， 是正值，所以實(shí)際上為相減

30、關(guān)系 又有 設(shè) 11 1 0 0 0 0 1 0 0 0 0 1 0 11 1 0 0 0 1 y p zp y p zp p y p zp f f f xp x p x p yp y p y p Tfzp zp zp yp f 用矩陣表示： z y P y p f o z p f p z P p y p p : P x 1 P P x 1 1 () 1 pp pp T T yz yz y p zp zp f y p zp zp y p f y p f zp x p y p f

31、zp x p y p y p f y p f y p f y p y p y p f y p y p f f xp xp y 以光心為原點(diǎn)O，光軸與y軸重合，P 為物點(diǎn)， 用齊次坐標(biāo)表示 求 的齊次坐標(biāo)，即求 根據(jù)三角形相似原理： 注意 是負(fù)值， 是正值，所以實(shí)際上為相減關(guān)系 又有 設(shè) 11 1 0 0 0 0 1 0 0 0 0 1 0 11 1 0 0 0 1 y p zp y p zp p y p zp f f f xp x p x p yp y p y p Tfzp zp zp yp f

32、 用矩陣表示： 因此，進(jìn)行機(jī)器人運(yùn)動學(xué)計算時，不能省略透視矩陣，有 攝像頭時，透視矩陣為 0 - 0，沒有攝像頭時為 0 0 0 。 f 1 10 1 0 0100 0010 0001 T 1 T 11 p f z y x f y z y x z y x f p p p f p p p p p p 用矩陣表示： 知識點(diǎn)： 1. 點(diǎn)和面的齊次坐標(biāo)和齊次變換 2. 三個基本旋轉(zhuǎn)矩陣 3.

33、 絕對變換：如果所有的變換都是相對于固定坐標(biāo)系中各坐 標(biāo)軸旋轉(zhuǎn)或平移 ， 則依次左乘 ， 稱為絕對變換 。 4. 相對變換：如果動坐標(biāo)系相對于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸 旋轉(zhuǎn)或平移 ， 則齊次變換為依次右乘 ， 稱為相對變換 。 5. 繞任意軸旋轉(zhuǎn)： 5步順序 6. 透視變換 知識點(diǎn)： 三個基本旋 轉(zhuǎn)矩陣 c o s0s in 010 s in0c o s )y,R （ 100 0c o ss in 0s in-c o s )z,R （ ss in0 s inc o s0 001 )R ( x , co

34、例題 1： O與 O初始重合， O作如下運(yùn)動：繞 Z軸轉(zhuǎn)動 30 ； 繞 X軸轉(zhuǎn)動 60 ；繞 Y軸轉(zhuǎn)動 90 。求 T。 1000 0100 0030c o s30s in 0030s in30c o s R 1 1000 060c os60s in0 060s in60c os0 0001 2 R 1000 090c o s090s in 0010 090s in090c o s 3 R 1000 002/12/3 02/34/34

35、/1 02/14/34/3 123 RRRT 例題 2： O與 O初始重合， O作如下運(yùn)動：繞 X軸轉(zhuǎn)動 90；繞 w軸轉(zhuǎn)動 90；繞 Y軸轉(zhuǎn)動 90。求 T；改變旋轉(zhuǎn)順序，如何 旋轉(zhuǎn)才能獲得相同的結(jié)果。 1000 090c os90s in0 090s in-09c os0 0001 R 1 1000 0100 0090c o s90s in 0090s in90c o s 2 R 1000 090c o s090s in 0010 090s in090c o s

36、 3 R 1000 0010 0100 0001 213 RRRT 解： 解 ： 繞 Z（ w）軸轉(zhuǎn)動 90； 繞 X軸轉(zhuǎn)動 90； 繞 Y軸轉(zhuǎn)動 90。 例題 3： 矢量 在 O中表示為 ， O相對于 O的 奇次變換為： P kjip 2230 1000 1100 20001 10010 T oo 中的矢量在求此時 ，軸平移的沿軸轉(zhuǎn)的繞當(dāng) 中的矢量在求 的位置和姿態(tài)在畫出：求 0 20X0 90Y0 0)3 0 2) 00 1) O

37、00 p pp 解： 1） z x y u w v o o 解： 2） 1 3 23 8 T 00 pp o 解： 3） ， ， 1000 090c o s090s in 0010 090s in090c o s R 1 1000 0100 0010 20001 T 1r 1000 10010 20001 21100 TRT 1 1 or T 1 8 23 23

38、 1 2 2 3 1000 10010 20001 21101 00 pTp 例題 4： 如圖所示， 1）寫出 、 、 、 ； 2）求 1T 21T 32T 43T 40 T 1000 11-00 301-0 3.5-001 T 1 1000 101-0 3001- 0100 T 21 1000 0001 5 5 3 5 4 -0 0 5 4 5 3 0 T 32 1000 01-00 0010 3.5001- T 43 解： 1） o 0 x 0 y 0 z

39、4 3 3 . 5 1 1 o 1 x 1 y 1 z 2 o 2 x 2 y 2 z 3 o 3 x 3 y 3 z 4 o 4 x 4 y 4 z 解 2） ：根據(jù)定義 2， 繞自身旋轉(zhuǎn) ， 右乘 1000 50.6-0.8-0 00.8-0.60 0001- T T T TT 433221140 習(xí)題 1： O與 O初始重合， O作如下運(yùn)動：繞 z軸轉(zhuǎn)動 90；繞 v軸轉(zhuǎn)動 90；繞 x軸轉(zhuǎn)動 90。求 T；改變旋轉(zhuǎn)順序，如 何旋轉(zhuǎn)才能獲得相同的結(jié)果。 習(xí)題 2： 已知齊次變換矩陣 要求 R（ f,) , 求

40、 f和 值 1000 0001 0100 0010 H 第三章 機(jī)器人運(yùn)動學(xué) 機(jī)器人運(yùn)動學(xué)主要是把機(jī)器人 相對于固定參考 系 的運(yùn)動作為 時間的函數(shù) 進(jìn)行分析研究，而不 考慮引起這些運(yùn)動的力和力矩 也就是要把機(jī)器人的 空間位移 解析地表示為 時 間的函數(shù) ，特別是研究機(jī)器人 關(guān)節(jié)變量空間和 機(jī)器人末端執(zhí)行器位置和姿態(tài)之間 的關(guān)系 本章將討論機(jī)器人運(yùn)動學(xué)幾個具有實(shí)際意義的 基本問題。 3.1 機(jī)器人運(yùn)動學(xué)所討論的問題 3.1.1 研究的對象 機(jī)器人在基本機(jī)構(gòu)形式上分為兩種，一種是關(guān)節(jié)式串 聯(lián)機(jī)器人，另外一種是并聯(lián)機(jī)器人，如圖： PUMA56

41、0 Hexapod Fanuc manipulator 1972 Victor Scheinman在 Unimation公司為通用； 1980Westinghouse收購； 1988Stubli收購； Nokia Robotics在 80年代賣出 1500余臺 PUMA系統(tǒng)； Nokia的 Robotics division1990年賣出。 運(yùn)動學(xué)研究的問題 Where is my hand? Direct Kinematics HERE! How do I put my hand here? Inverse Kinematics: Choose these angles! 運(yùn)動學(xué)正

42、問題 運(yùn)動學(xué)逆問題 研究的問題 : 運(yùn)動學(xué)正問題 ---已知桿件幾何參數(shù)和關(guān)節(jié)角矢量 ， 求操 作機(jī)末端執(zhí)行器相對于固定參考作標(biāo)的位置和姿態(tài) （ 齊 次變換問題 ） 。 運(yùn)動學(xué)逆問題 ---已知操作機(jī)桿件的幾何參數(shù) ， 給定操作 機(jī)末端執(zhí)行器相對于參考坐標(biāo)系的期望位置和姿態(tài) （ 位 姿 ） ， 操作機(jī)能否使其末端執(zhí)行器達(dá)到這個預(yù)期的位姿 ？ 如能達(dá)到 ， 那么操作機(jī)有幾種不同形態(tài)可以滿足同樣的 條件 ？ 運(yùn)動學(xué)正問題關(guān)節(jié)角 桿件參數(shù) 末端執(zhí)行器 運(yùn)動學(xué)正問題關(guān)節(jié)角 桿件參數(shù) 逆 3.2 機(jī)器人桿件，關(guān)節(jié)和它們的參數(shù) 3.2.1 桿件 ， 關(guān)節(jié) 操作機(jī)由一串用轉(zhuǎn)動或平移（棱

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

44、各種低副形狀如下圖所 示： 旋轉(zhuǎn) 棱柱形 柱形 球形 螺旋形 平面 3.2.2 桿件參數(shù)的設(shè)定 條件 關(guān)節(jié)串聯(lián) 每個桿件 最多 與 2個桿件相連 ， 如 Ai與 Ai-1和 Ai+1相連 。 第 i 關(guān)節(jié)的關(guān)節(jié)軸 Ai 位于 2個桿 件相連接處 ， 如圖所示 ， i-1關(guān)節(jié)和 i+1關(guān)節(jié) 也各有一個關(guān)節(jié)軸 Ai-1 和 Ai+1。 Ai Ai+1 Ai-1 桿件參數(shù)的定義 和 li 關(guān)節(jié) Ai軸和 Ai+1軸線 公法線的長度 關(guān)節(jié) i軸線與 i+1軸線 在垂直于 li 平面內(nèi)的夾 角，有方向性 ,由 Ai轉(zhuǎn) 向 Ai+1， 由右手定則決 定正負(fù)

45、 由運(yùn)動學(xué)的觀點(diǎn)來看 ， 桿件的作用僅在于它能保 持其兩端關(guān)節(jié)間的 結(jié)構(gòu)形態(tài) 不變 。 這種形態(tài)由兩 個參數(shù)決定 ， 一是桿件的長度 li， 一個是桿件的 扭轉(zhuǎn)角 i Ai Ai+1 i il i il i 桿件參數(shù)的定義 和 Li和 Li-1在 Ai軸線上 的交點(diǎn)之間的距離 Li和 Li-1之間的夾 角，由 Li-1轉(zhuǎn)向 Li，由 右手定則決定正負(fù)， 對于旋轉(zhuǎn)關(guān)節(jié)它是個 變量 確定桿件 相對位置關(guān)系 ， 由另外 2個參數(shù)決定 ， 一個是桿 件的偏移量 ， 一個是桿件的回轉(zhuǎn)角 iid i id i Ai Ai+1 i il id 1il i Ai-1 id

46、移動關(guān)節(jié)桿件參數(shù)的定義 確定桿件的結(jié)構(gòu)形態(tài)的 2個參數(shù) Li與 i與旋轉(zhuǎn)關(guān)節(jié)是一樣的。 確定桿件相對位置關(guān)系的 2個參數(shù)則相反。這里 i為常數(shù)， di為變量。 上述 4個參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位 置關(guān)系，在轉(zhuǎn)動關(guān)節(jié)中， Li, i, di是固定值， i是變量。在 移動關(guān)節(jié)中， Li, i, i是固定值， di 是變量。 3.3 機(jī)器人關(guān)節(jié)坐標(biāo)系的建立 oO o n-1nX 對于每個桿件都可以在關(guān)節(jié)軸處建立一個正規(guī)的笛卡兒 坐標(biāo)系（ xi, yi, zi），（ i=1, 2, , n ）， n是自由度數(shù)，再 加上基座坐標(biāo)系，一共有（ n+1）個坐標(biāo)系

47、。 基座坐標(biāo)系 定義為 0號坐標(biāo)系（ x0, y0, z0） ,它也是機(jī) 器人的慣性坐標(biāo)系， 0號坐標(biāo)系在基座上的位置和方向可 任選，但 軸線必須與關(guān)節(jié) 1的軸線重合，位置和方向可 任選； 最后一個坐標(biāo)系（ n關(guān)節(jié)），可以設(shè)在手的任意部位，但 必須保證 與 垂直。 機(jī)器人關(guān)節(jié)坐標(biāo)系的建立主要是為了描述機(jī)器人各桿件和終 端之間的相對運(yùn)動，對建立運(yùn)動方程和動力學(xué)研究是基礎(chǔ)性 的工作。 為了描述機(jī)器人各桿件和終端之間轉(zhuǎn)動或移動關(guān)系， Denavit 和 Hartenberg于 1955年提出了一種為運(yùn)動鏈中每個桿件建立 附體坐標(biāo)系的矩陣方法 （ D-H方法）

48、 ，建立原則如下： D-H關(guān)節(jié)坐標(biāo)系建立原則 右手坐標(biāo)系 原點(diǎn) Oi：設(shè)在 Li與 Ai+1軸線的交點(diǎn)上 Zi軸 ： 與 Ai+1關(guān)節(jié)軸重合，指向任意 Xi軸 ： 與公法線 Li重合，指向沿 Li由 Ai軸線指向 Ai+1軸線 Yi軸 ： 按右手定則 關(guān)節(jié)坐標(biāo)系的建立原則 Ai Ai+1 i il id 1il i Ai-1 1iz 1ix 1iy 1io iz ix iy io 原點(diǎn) Oi：設(shè)在 Li與 Ai+1軸線的交點(diǎn)上 Zi軸：與 Ai+1關(guān)節(jié)軸 重合，指向任意 Xi軸：與公法線 Li 重合，指向沿 Li由 Ai軸線指向 Ai+1軸線 Yi軸

49、：按右手定則 桿件長度 Li 沿 xi 軸 ， zi-1 軸與 xi 軸交點(diǎn)到 0i 的距離 桿件扭轉(zhuǎn)角 i 繞 xi 軸 ， 由 zi-1 轉(zhuǎn)向 zi 桿件偏移量 di 沿 zi-1 軸 ， zi-1 軸和 xi 交點(diǎn)至 0i 1 坐標(biāo)系原點(diǎn)的距離 桿件回轉(zhuǎn)角 i 繞 zi-1 軸 ， 由 xi-1轉(zhuǎn)向 xi 兩種特殊情況 兩軸相交，怎么建立坐 標(biāo)系？ 0iAi與 Ai+1關(guān)節(jié)軸線的交 點(diǎn)； ZiAi+1軸線； XiZi和 Zi-1構(gòu)成的平面的 法線 ； Yi右手定則； i-1 i A i A i + 1 o i z i -

50、 1 z i x i y i 兩軸平行，怎么建立坐標(biāo)系 (Ai與 Ai+1平行 )？ 先建立 0i-1 然后建立 0i+1 最后建立 0i i-1O D 注意： 由于 Ai和 Ai+1平行 ， 所以公法線任意點(diǎn) 在 A點(diǎn)位置； 按照先前的定義 ， di為 Oi-1點(diǎn)和 A點(diǎn)之間的距離 ， di+1為 B點(diǎn)和 C點(diǎn)間 的距離 ， 這樣設(shè)定可以的 ， 但我們可以變更一下 ， 將 0i點(diǎn)放在 C點(diǎn) ， 定義 Oi在 Li+1和 Ai+1軸的交點(diǎn)上 ， 這樣使 di+1=0使計算簡便 ， 此時 di= A i - 1 A i A i + 1 A i + 2 l i -

51、 1 o i - 1 x i - 1 y i - 1 z i - 1 A B D C o i （ x i ） （ y i ） z i x i y i o i + 1 x i + 1 y i + 1 z i + 1 d i + 1 l i + 1 d i 相鄰 關(guān)節(jié)坐標(biāo)系間的齊次變換過程 機(jī)器人運(yùn)動學(xué)正解 1. 將 xi-1軸繞 zi-1 軸轉(zhuǎn) i 角度，將其與 xi軸平行； 2. 沿 zi-1軸平移距離 di ， 使 xi-1 軸與 xi 軸重合； 3. 沿 xi 軸平移距離 Li， 使兩坐標(biāo)系原點(diǎn)及 x軸 重合； 4. 繞 xi 軸轉(zhuǎn) i 角度，兩 坐標(biāo)系完全重合

52、 ),(),(),(),(A 111 iiiir a n siir a n siiii xRlxTdZTZR 根據(jù)上述坐標(biāo)系建立原則，用下列旋轉(zhuǎn)和位移我們 可以建立相鄰的 Oi-1 和 Oi 坐標(biāo)系之間的關(guān)系 Ai Ai+1 i il id 1il i Ai-1 1iz 1ix 1iy 1io iz ix iy io 機(jī)器人的運(yùn)動學(xué)正解方程 0 0 1 1 12 i iiT A A A D-H變換矩陣 ii A1 1000 100 0010 0001 id 100 0100 00c oss in 0s inc os ii i

53、i 1000 0100 0010 001 il 1000 0c oss in0 0s inc os0 0001 ii ii 1000 c oss i n0 s i nc oss i nc osc oss i n c oss i ns i ns i nc osc os iii iiiiiii iiiiiii d l l = = 機(jī)械手的坐標(biāo)變換圖如圖所示，機(jī)械手的末端（即連桿坐標(biāo)系 i） 相對于基座坐標(biāo)系 0的描述用 oTi 表示，即： 0 z A1 A2 A3

54、A4 A5 A6 0 E X 0T6 1T6 2T6 3T6 4T6 5T6 機(jī)械手的坐標(biāo)變換圖 機(jī)器人的運(yùn)動學(xué)正解方程 0 0 1 1 12 i iiT A A A 舉例： Stanford機(jī)器人 A1 A2 A3 A4 A5 A6 d1 z1 x1 y 1 O1 d2 z2 x2 y2 O2 z3 y3 x3 O3 y4 z4 x4 O4 z5 y5 x5 O5 345 45 ,, 0 o o o dd 重 合 d3 z6 x6 y6 O6 d6 z0 y0 x0 O0 為右手坐標(biāo)系 原點(diǎn) Oi： Ai與 Ai+1 關(guān)節(jié)軸線的交點(diǎn) Zi軸：與 Ai+1關(guān)節(jié)軸 重

55、合，指向任意 Xi軸： Zi和 Zi-1構(gòu)成 的面的法線 Yi軸：按右手定則 Li 沿 xi 軸 ， zi-1 軸與 xi 軸交點(diǎn)到 0i 的距離 i 繞 xi 軸 ， 由 zi-1 轉(zhuǎn)向 zi di 沿 zi-1 軸 ， zi-1 軸和 xi 交點(diǎn)至 0i 1 坐標(biāo)系原 點(diǎn)的距離 i 繞 zi-1 軸 ， 由 xi-1轉(zhuǎn)向 xi 解： 3.4 例題 試求立方體中心在機(jī)座坐標(biāo)系 0中的位置 該手爪從上方把物體抓起，同時手爪的開合方向與物體的 Y軸同向， 那么，求手爪相對于 0的姿態(tài)是什么？ 在機(jī)器人工作臺上加裝一電視攝像機(jī)，攝像機(jī)可見

56、到固聯(lián) 著 6DOF關(guān)節(jié)機(jī)器人的機(jī)座坐標(biāo)系原點(diǎn)，它也可以見到被操作 物體（立方體）的中心，如果在物體中心建一局部坐標(biāo)系，則 攝像機(jī)所見到的這個物體可由齊次變換矩陣 T1來表示，如果攝 像機(jī)所見到的機(jī)座坐標(biāo)系為矩陣 T2表示。 1000 101-00 2001-0 10-001 T 1000 91-00 10001 1010 T 21 x y z 解 1： T T 21 物機(jī)機(jī)攝物攝 求，，已知 TTT TT 11-2 ）（有： 物攝攝機(jī)物機(jī) TTT 1000 91-00 10001 1010 1000 101-00

57、2001-0 10001 1000 1100 10001- 11010 O 物 根據(jù) T 1 畫出 O 機(jī) 根據(jù) T 2 畫出 因此物體位于機(jī)座坐標(biāo)系的（ 11， 10， 1） T 處，它的 X， Y， Z軸分別與機(jī)座坐標(biāo)系的 -Y， X， Z軸平行。 x y z y 機(jī) z 物 y 物 x 物 z 機(jī) o O 機(jī) O 物 解 2： 向重合手爪開合方向與物體 ya : Ts 001有 方向相反方向物體的從上向下抓，指出手爪 zab : Ta 100 則有 Tkji kji asnc 01000 100

58、 001: 1-00 001 010 因此：姿態(tài)矩陣為 重合時 與物體中心 當(dāng)手爪中心 1000 11-00 10001 11010 T 物 機(jī) O s n a y z x X機(jī) 手爪 機(jī)實(shí)際要求 T pzazsznz pyaysyny pxaxsxnx 1000 工作空間 工作空間 : 末端操作手可以到達(dá)的空間位置集合 如何獲得工作空間 : 利用正運(yùn)動學(xué)模型 ,改變關(guān)節(jié) 變量值 可達(dá)空間 : 末端操作手可以至少以一個姿態(tài)到達(dá)的 空間位置集合 靈活空間 : 末端

59、操作手可以以任何姿態(tài)到達(dá)的空間 位置集合 如何確定可達(dá)空間 ?首先， 令 3變 化 示例 : 平面 3連桿機(jī)器人 l2 l3 l1 1 2 3 1 1 2 1 2 3 1 2 3 1 1 2 1 2 3 1 2 3 1 2 3 1 2 3 1 2 3 c os c os c os si n si n si n , x l l l y l l l l l l l l l 然后 2變化 最終， 變化 1 3.5 機(jī)器人末端操作器位姿的其它 描述方法 用矩陣表示剛性體的轉(zhuǎn)動簡化了許多

60、運(yùn)算， 但它需要 9個元素來完全描述旋轉(zhuǎn)剛體的姿 態(tài)，因此矩陣并不直接得出一組完備的廣 義坐標(biāo)。 一組廣義坐標(biāo)應(yīng)能描述轉(zhuǎn)動剛體相對于參 考坐標(biāo)的方向，被稱為歐拉角的三個角度， 、 、 就是這種廣義坐標(biāo)。 有幾種不同的歐拉角表示方法，它們均可 描述剛體相對于固定參考系的姿態(tài)。三種 最常見的歐拉角類型列在表中 3種最常見的歐拉角類型 步 1 步 2 步 3 類型 1 繞 OZ軸轉(zhuǎn) 角 繞當(dāng)前 OU 軸轉(zhuǎn) 角 繞當(dāng)前 OW軸轉(zhuǎn) 角 類型 2 繞 OZ軸轉(zhuǎn) 角 繞當(dāng)前 OV 軸轉(zhuǎn) 角 繞當(dāng)前 OW軸轉(zhuǎn) 角 類型 3 繞 OX軸轉(zhuǎn) 角 繞 OY軸轉(zhuǎn) 角 繞 OZ軸轉(zhuǎn)

61、角 u v w x(u) y (v) z (w) o u v w u v W ),( ZR ),( R ),( wR N0 T 100 0 0 0 0 001 100 0 0 cs sc cs sccs sc ccsss sccccssscccs ssccsscscscc 類型 1：表示法通常用于陀螺運(yùn)動 類型 2： 所得的轉(zhuǎn)動矩陣為右乘 100 0 0 c0s- 010 s

62、0c 100 0 0 ),(),v(),(R cs sc cs sc wRRZR 1000 pz pyR px T ccsss ssccscsscccs sccssccssccc 類型 2 繞 OZ軸轉(zhuǎn) 角 繞當(dāng)前 OV 軸轉(zhuǎn) 角 繞當(dāng)前 OW軸轉(zhuǎn) 角 類型 3：一般稱此轉(zhuǎn)動的歐拉角 為偏航角、俯仰和橫滾， （這 種方法也叫做 偏航、俯仰和橫滾 角表示方法）這種形 式主要用 于航空工程中分析飛行器的運(yùn)動， 其旋轉(zhuǎn)矩陣為

63、 ccscs sccssccssscs sscsccsssccc cs sc cs sc cs sc xRyRz 0 0 001 0 010 0 100 0 0 ),(),(),RR （ 類型 3 繞 OX軸轉(zhuǎn) 角 繞 OY軸轉(zhuǎn) 角 繞 OZ軸轉(zhuǎn) 角 正運(yùn)動學(xué)問題 : 已知關(guān)節(jié)角度或位移，計算 末端操作手的對應(yīng)位姿 . 逆運(yùn)動學(xué)問題 : 已知 末端操作手的位姿，求 解對應(yīng)的關(guān)節(jié)變量 . 為 什么逆運(yùn)動學(xué)問題更困難 ? 可能存在多解或無解 通常需多次求解非線性超越方程 3.6 運(yùn)動學(xué)逆問題 解的存在性 目標(biāo) 點(diǎn)應(yīng)位于工作空間內(nèi) 可能存在多解，如何選擇最

64、合適的解？ 存在雙解 ! 求解方法 如果各關(guān)節(jié)可用某算法獲得，一個機(jī)械手是 有解的 . 算法應(yīng)包含所有可能解 . 封閉形式解（解析解） 數(shù)值解 方法 我們對封閉形式的解法更感興趣 代數(shù)方法 幾何方法 可解性的重要結(jié)論是： 所有具有轉(zhuǎn)動和移動關(guān)節(jié)的系統(tǒng)，在一個單一串聯(lián)中 總共有 6個（或小于 6個）自由度時，是可解的，其通 解一般是數(shù)值解，它不是解析表達(dá)式，而是利用數(shù)值 迭代原理求解，它的計算量要比解析解大。 但在某些特殊情況下，如若干個關(guān)節(jié)軸線相交和或多 個關(guān)節(jié)軸線等于 0 或 90 的情況下，具有 6個自由度 的機(jī)器人可得到解析解。 為使機(jī)器人有解析

65、解，一般設(shè)計時，使工業(yè)機(jī)器人足 夠簡單，盡量滿足這些特殊條件。 對于給定的機(jī)器人 ， 能否求得它的運(yùn)動學(xué)逆解的解析式 （ 也叫封閉解 ） 。 運(yùn)動學(xué)逆問題的可解性 運(yùn)動學(xué)逆問題的多解性 機(jī)器人運(yùn)動問題為解三角方程 ， 解反三角函數(shù)方程時會 產(chǎn)生多解 .顯然對于真實(shí)的機(jī)器人 ， 只有一組解與實(shí)際情 況最相對應(yīng) ， 因此必須作出判斷 ， 以選擇合適的解 。 通常采用如下方法剔除多余解： 01 40i 0002 2 2 01 8 040 i 若該關(guān)節(jié)運(yùn)動空間為 ，則應(yīng)選 。 100 040i 1 根據(jù)關(guān)節(jié)運(yùn)動空間選取合適的解 。 例如求得機(jī)器 人某

66、關(guān)節(jié)角的兩個解為 2 選擇一個與前一采樣時間最接近的解 ， 例如： 01 40i 0002 2 2 01 8 040 i 若該關(guān)節(jié)運(yùn)動空間為 ， 且 ， 則應(yīng)選 250 01 160i 0220i 3 根據(jù)避障要求 ， 選擇合適的解 4 逐級剔除多余解 對于具有 n個關(guān)節(jié)的機(jī)器人 ， 其全部解將構(gòu)成樹形結(jié)構(gòu) 。 為簡化起見 ， 應(yīng)逐級剔除多余解 。 這樣可以避免在樹形解中 選擇合適的解 。 迭代法 計算量大 幾何法 適用于自由度較少的情況 反變換法 運(yùn)動學(xué)逆問題解法 用未知的逆變換逐次左乘，由乘得的矩陣方程 的元素決定未知數(shù)，即用逆變換把一個未知數(shù) 由矩陣方程的右邊移到左邊 考察方程式左、右兩端對應(yīng)元素相等，以產(chǎn)生 一個有效方程式，理論上可得到 12個方程。 然后求這個三角函數(shù)方程式，以求解未知數(shù) 把下一個未知數(shù)移到左邊 重復(fù)上述過程，直到解出所有解 缺點(diǎn)：無法由數(shù)種可能的解中直接得出合適的解， 需要通過人為的選擇 運(yùn)動學(xué)逆問題解法 反變換法 Paul 等人提出的方法 (1981年，也叫求逆的方 法，是解析解