I. 機器人學 機器人學 機械電子工程 Dr. Kevin Craig I. 機器人學 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 國家： 53 I. 機器人學 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-inspired robotic devices Rehabilitation robotics Field robotics Grasping Nanorobotic manipulation Fish-like robot Parallel robot 第二章 機器人運動學及其數(shù)學基礎 參考教材 美 付京遜 機器人學 中南大學 蔡自興 機器人學 美 理查德 鮑爾 機器人操作手 數(shù) 學 編程與控制 參考教材 美 付京遜 機器人學 美籍華人 普渡大學（ Purdue University）電機工程專業(yè)著 名教授 4部著作、 400多篇論文 第一任國際模式識別學會會長，被譽為自動模 式識別之父 1985年去世 參考教材 中南大學 蔡自興 中南大學教授，我國人工 智能和機器人領域著名專 家 中國人工智能學會智能機 器人專委會理事長 曾與付京遜教授一起工作 過 第一節(jié) 引言 串聯(lián)機器人可以用一個開環(huán)關節(jié)鏈來建模 由數(shù)個驅(qū)動器驅(qū)動的 轉(zhuǎn)動 或 移動 關節(jié)串聯(lián)而成 一端固定在基座上，另一端是自由的，安裝工具（末端 執(zhí)行器），用以操縱物體，或完成各種任務 i n o a 關節(jié)的 相對運動 導致桿件的運動， 使末端執(zhí)行器定位于所需要的方 位上 在一般機器人應用問題中，人們 感興趣的是：末端執(zhí)行器相對于 固定參考坐標數(shù)的 空間幾何描述 ， 也就是機器人的運動學問題 機器人的運動學即是研究機器人 手臂 末端執(zhí)行器位置和姿態(tài) 與 關 節(jié)變量空間 之間的關系 運動學研究的問題 Where is my hand? Direct Kinematics HERE! How do I put my hand here? Inverse Kinematics: Choose these angles! 運動學正問題 運動學逆問題 哈佛大學 Roger Brockett建立的指數(shù)積公 式 運動學 滾動接觸 非完整控制 數(shù)學基礎 -剛體運動 參考文獻： 機器人操作的數(shù)學導論 作者：理查德 摩雷 李澤湘 夏卡恩 薩斯特里 翻譯：徐衛(wèi)良 錢瑞明（東南大學） 研究運動學的方法 1955年丹納維特（ Denavit）和哈頓伯格（ Hartenberg）提 出了一種 采用矩陣代數(shù)方法 解決機器人的運動學問題 D-H 方法，其數(shù)學基礎即是 齊次變換 具有直觀的幾何意義 能表達動力學、計算機視覺和 比例變換問題 為以后的比例變換、透視變換 等打下基礎 1000 p p p T z yyy xxx zzz y x w w w 第二節(jié) 數(shù)學基礎 齊次坐標和齊次變換 2.1 點和面的齊次坐標 2.1.1 點的齊次坐標 一般來說， n維空間的齊次坐標表示是一個（ n+1）維空間實體。有一 個特定的投影附加于 n維空間，也可以把它看作一個附加于每個矢量的 特定坐標 比例系數(shù)。 引入齊次坐標的目的是為了表示幾何變換的旋轉(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 顯然，齊次坐標表達 并不是唯一 的，隨 w值的不同而不同。在計算機圖學中， w 作為通用比例因子，它可取任意正值，但 在機器人的運動分析中，總是取 w=1 。 列矩陣 一個點矢： 例 1： kjiV 543 可以表示為： V=3 4 5 1T 或 V=6 8 10 2T 或 V=-12 -16 -20 -4T 齊次坐標與三維直角坐標的區(qū)別 V點在 O XYZ坐標系中表 示是 唯一 的（ a、 b、 c） 而在齊次坐標中表示可 以是多值的。 不同的表 示方法代表的 V點在空間 位置上不變。 x y z z z x V 圖 2 - 2 o 幾個特定意義的齊次坐標： 0 0 0 nT 坐標原點矢量的齊次坐標， n為任 意非零比例系數(shù) 1 0 0 0T 指向無窮遠處的 OX軸 0 1 0 0T 指向無窮遠處的 OY軸 0 0 1 0T 指向無窮遠處的 OZ軸 0 0 0 0T 沒有意義 2個常用的公式： zzyyxx babababa kbabajbabaibaba bbb aaa kji ba xyyxzxxzyzzy zyx zyx )()()( 點乘 : 叉乘 : 2.1.2 平面的齊次坐標 平面齊次坐標由 行矩陣 P=a b c d 來表示 當點 v=x y z wT處于平面 P內(nèi)時，矩陣乘積 PV=0，或記為 0 dwczbyax w z y x dcbaPV 與點矢 相仿，平面 也沒有意義 T0000 0000 點和平面間的位置關系 設一個平行于 x、 y軸 ， 且在 z軸上的坐標為單位距離的平面 P可以表示為： 或 有： PV= 1100 P 2200 P v0 v0 v0 點在平面下方 點在平面上 點在平面上方 例如：點 V=10 20 1 1T 必定處于此平面內(nèi) ， 而點 V=0 0 2 1T 處于平 P 的上方 ， 點 V=0 0 0 1T處于 P平面下方 ， 因為： 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)矩陣 設固定參考坐標系直角坐標為 Oxyz， 動坐標系為 Ouvw， 研究旋轉(zhuǎn)變換情況 。 x y z w v u P o ( O ) 圖 2 - 3 初始位置時，動靜坐標系重合， O、 O 重合，如圖。各軸對 應重合，設 P點是動坐標系 Ouvw中的一點，且固定不變。則 P 點在 Ouvw中可表示為： wwvvuuu v w kPjPiPP 、 、 為坐標系 Ouvw的單位矢量 ， 則 P點在 oxyz中可表示為： ui vj wk zzyyxxxyz kPjPiPP xyzu v w PP 當動坐標系 Ouvw繞 O點回轉(zhuǎn)時，求 P點在固定坐標系 oxyz 中的位置 y z x o ( O ) u v w P Pw Pv Pu 圖 2 - 4 已知： P點在 Ouvw中是不變的仍然成 立，由于 Ouvw回轉(zhuǎn)，則： wwvvuuu v w kPjPiPP xwwvvuuxu v wx ikPjPiPiP )(P ywwvvuuyu 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 e t 是正交矩陣，的行列式，為的伴隨矩陣，為 RRRR 2.2.2 旋轉(zhuǎn)齊次變換 用齊次坐標變換來表示式（ 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 即動坐標系 求 的旋轉(zhuǎn)矩陣 ， 也就是 求出坐標系 中各軸單位矢量 在固定坐標系 中各軸的投影分量 ， 很容易得到在兩個坐標系重合時 ， 有： 角，軸轉(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 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 三個基本旋轉(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：在動坐標中有一固定點 ，相對固定參 考坐標系 做如下運動： R（ x, 90 ）； R(z, 90 )； R(y,90 )。求運動后點 在固定參考坐標系 下的位置。 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 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為二者之間的關系矩陣，我們令： ),(),(),RR 33 xRzRy（ 定義 1： 當動坐標系 繞固定坐標系 各坐標軸順序有限次 轉(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： 動坐標系 0起始位置與固定參考坐標系 0重合 ,動坐標系 0做如下運動： R(Z,90) R（ y,90） Trans(4， -3, 7)， 求合成矩陣 解 1：用畫圖的方法： 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 以上均以固定坐標系多軸為變換基準 ， 因此矩陣左乘 。 如果我們做如下變換，也可以得到相同的結(jié)果： 例 2： 先平移 Trans (4,-3,7)； 繞當前 軸轉(zhuǎn)動 90； 繞當前 軸轉(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é)論： 動坐標系在固定坐標系中的齊次變換有 2種情況： 定義 1： 如果所有的變換都是 相對于固定坐標系 中各坐標軸旋 轉(zhuǎn)或平移 ， 則依次 左乘 ， 稱為 絕對變換 。 定義 2： 如果動坐標系 相對于自身坐標系的當前坐標軸 旋轉(zhuǎn)或 平移，則齊次變換為依次 右乘 ，稱為 相對變換 。 結(jié)果均為動坐標系在固定坐標中的位姿 （ 位置 +姿態(tài) ） 。 相 對于固定坐標系 ， 軸。軸相當于軸，軸相對于軸，軸相當于 ZYX wv 也就是說，動坐標系繞自身坐標軸做齊次變換， 要達到繞固 定坐標系相等的結(jié)果，就應該用相反的順序。 右乘的意義： 機器人用到相對變換的 時候比較多 例如機械手抓一個杯子， 如右圖所示，手爪需要 轉(zhuǎn)動一個角度才抓的牢， 相對于固定坐標系表達 太麻煩，可以直接根據(jù) 手爪的坐標系表示 但也要知道在 O中的位 姿，就用右乘的概念。 x y z o H 2.2.5 繞通過原點的任意軸旋轉(zhuǎn)的齊次變換 有時動坐標系 O可能繞過原點 O的分量分別為 rx、 ry、 rz的 任意單位矢量 r 轉(zhuǎn)動 角。 研究這種轉(zhuǎn)動的好處是可用 O繞某軸 r 的一次轉(zhuǎn)動代替繞 O各坐標軸的數(shù)次轉(zhuǎn)動 為推導此旋轉(zhuǎn)矩陣，可作下述 5步變換： 1. 繞 X 軸轉(zhuǎn) 角， 使 r 軸處于 XZ平面內(nèi) 2. 繞 Y 軸轉(zhuǎn) - 角，使 r 軸與 OZ軸重合 3. 繞 OZ軸轉(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 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 os(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 齊次變換矩陣的幾何意義 設，有一個手爪，即動坐標系 O，已知， 初始位置 重合，那么 O在 O中的齊次坐標變換為： ，如果手爪轉(zhuǎn)了一個角度， 則： 111 cbao 1000 100 010 001 T 1 1 1 1 c b a 1000 p p p T z yyy xxx zzz y x w w w T反映了 O在 O中的位置和姿態(tài) ， 即表示了該坐標系原點 和各坐標軸單位矢量在固定坐標系中的位置和姿態(tài) 。 該矩陣可以由 4個子矩陣組成 ， 寫成如下形式： 比例系數(shù)透視矩陣 位置矢量旋轉(zhuǎn)矩陣 1131 1333 wf PR T zzz yyy xxx w w w 33R 為 姿態(tài)矩陣（旋轉(zhuǎn)矩陣） ，表示動坐標系 O在固定參考坐標系 O中的姿態(tài)，即表示 O各坐標軸單位矢量在 O各軸上的投影 為 位置矢量矩陣 ，代表動坐標系 O坐標原點 在固定參考坐標系 O中的位置 Tzyx ppp P 13 為 透視變換矩陣 ，在視覺中進行圖像計算，一 般置為 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)常遇到， 在機器人計算中，所要 求的就是齊次變換矩陣 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 以光心為原點O，光軸與y軸重合，P 為物點， 用齊次坐標表示 求 的齊次坐標，即求 根據(jù)三角形相似原理： 注意 是負值， 是正值，所以實際上為相減關系 又有 設 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 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 以光心為原點O，光軸與y軸重合，P 為物點， 用齊次坐標表示 求 的齊次坐標，即求 根據(jù)三角形相似原理： 注意 是負值， 是正值，所以實際上為相減關系 又有 設 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 用矩陣表示： 因此，進行機器人運動學計算時，不能省略透視矩陣，有 攝像頭時，透視矩陣為 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 用矩陣表示： 知識點： 1. 點和面的齊次坐標和齊次變換 2. 三個基本旋轉(zhuǎn)矩陣 3. 絕對變換：如果所有的變換都是相對于固定坐標系中各坐 標軸旋轉(zhuǎn)或平移 ， 則依次左乘 ， 稱為絕對變換 。 4. 相對變換：如果動坐標系相對于自身坐標系的當前坐標軸 旋轉(zhuǎn)或平移 ， 則齊次變換為依次右乘 ， 稱為相對變換 。 5. 繞任意軸旋轉(zhuǎn)： 5步順序 6. 透視變換 知識點： 三個基本旋 轉(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 例題 1： O與 O初始重合， O作如下運動：繞 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/1 02/14/34/3 123 RRRT 例題 2： O與 O初始重合， O作如下運動：繞 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 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)的繞當 中的矢量在求 的位置和姿態(tài)在畫出：求 0 20X0 90Y0 0)3 0 2) 00 1) O 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 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 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 習題 1： O與 O初始重合， O作如下運動：繞 z軸轉(zhuǎn)動 90；繞 v軸轉(zhuǎn)動 90；繞 x軸轉(zhuǎn)動 90。求 T；改變旋轉(zhuǎn)順序，如 何旋轉(zhuǎn)才能獲得相同的結(jié)果。 習題 2： 已知齊次變換矩陣 要求 R（ f,) , 求 f和 值 1000 0001 0100 0010 H 第三章 機器人運動學 機器人運動學主要是把機器人 相對于固定參考 系 的運動作為 時間的函數(shù) 進行分析研究，而不 考慮引起這些運動的力和力矩 也就是要把機器人的 空間位移 解析地表示為 時 間的函數(shù) ，特別是研究機器人 關節(jié)變量空間和 機器人末端執(zhí)行器位置和姿態(tài)之間 的關系 本章將討論機器人運動學幾個具有實際意義的 基本問題。 3.1 機器人運動學所討論的問題 3.1.1 研究的對象 機器人在基本機構(gòu)形式上分為兩種，一種是關節(jié)式串 聯(lián)機器人，另外一種是并聯(lián)機器人，如圖： PUMA560 Hexapod Fanuc manipulator 1972 Victor Scheinman在 Unimation公司為通用； 1980Westinghouse收購； 1988Stubli收購； Nokia Robotics在 80年代賣出 1500余臺 PUMA系統(tǒng)； Nokia的 Robotics division1990年賣出。 運動學研究的問題 Where is my hand? Direct Kinematics HERE! How do I put my hand here? Inverse Kinematics: Choose these angles! 運動學正問題 運動學逆問題 研究的問題 : 運動學正問題 -已知桿件幾何參數(shù)和關節(jié)角矢量 ， 求操 作機末端執(zhí)行器相對于固定參考作標的位置和姿態(tài) （ 齊 次變換問題 ） 。 運動學逆問題 -已知操作機桿件的幾何參數(shù) ， 給定操作 機末端執(zhí)行器相對于參考坐標系的期望位置和姿態(tài) （ 位 姿 ） ， 操作機能否使其末端執(zhí)行器達到這個預期的位姿 ？ 如能達到 ， 那么操作機有幾種不同形態(tài)可以滿足同樣的 條件 ？ 運動學正問題關節(jié)角 桿件參數(shù) 末端執(zhí)行器 運動學正問題關節(jié)角 桿件參數(shù) 逆 3.2 機器人桿件，關節(jié)和它們的參數(shù) 3.2.1 桿件 ， 關節(jié) 操作機由一串用轉(zhuǎn)動或平移（棱 柱形）關節(jié)連接的剛體（桿件） 組成 每一對關節(jié)桿件構(gòu)成一個 關節(jié) 自由度 ，因此 N個自由度的操作 機就有 N對關節(jié) -桿件。 0號桿件（一般不把它當作機器 人的一部分）固聯(lián)在機座上，通 常在這里建立一個固定參考坐標 系，最后一個桿件與工具相連 關節(jié)和桿件均由底座向外順序排 列，每個桿件最多和另外兩個桿 件相聯(lián)，不構(gòu)成閉環(huán)。 關 節(jié) 桿 件 末端操作手 機座 兩自 由度 關節(jié)： 一般說來，兩個桿件間是用 低付 相聯(lián)的 只可能有 6種低付關節(jié)： 旋轉(zhuǎn) （轉(zhuǎn)動）、 棱柱 （移動）、 圓柱形 、 球形 、 螺旋 和 平面 ，其中只有 旋轉(zhuǎn)和棱柱形 關 節(jié)是串聯(lián)機器人操作機常見的，各種低副形狀如下圖所 示： 旋轉(zhuǎn) 棱柱形 柱形 球形 螺旋形 平面 3.2.2 桿件參數(shù)的設定 條件 關節(jié)串聯(lián) 每個桿件 最多 與 2個桿件相連 ， 如 Ai與 Ai-1和 Ai+1相連 。 第 i 關節(jié)的關節(jié)軸 Ai 位于 2個桿 件相連接處 ， 如圖所示 ， i-1關節(jié)和 i+1關節(jié) 也各有一個關節(jié)軸 Ai-1 和 Ai+1。 Ai Ai+1 Ai-1 桿件參數(shù)的定義 和 li 關節(jié) Ai軸和 Ai+1軸線 公法線的長度 關節(jié) i軸線與 i+1軸線 在垂直于 li 平面內(nèi)的夾 角，有方向性 ,由 Ai轉(zhuǎn) 向 Ai+1， 由右手定則決 定正負 由運動學的觀點來看 ， 桿件的作用僅在于它能保 持其兩端關節(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軸線上 的交點之間的距離 Li和 Li-1之間的夾 角，由 Li-1轉(zhuǎn)向 Li，由 右手定則決定正負， 對于旋轉(zhuǎn)關節(jié)它是個 變量 確定桿件 相對位置關系 ， 由另外 2個參數(shù)決定 ， 一個是桿 件的偏移量 ， 一個是桿件的回轉(zhuǎn)角 iid i id i Ai Ai+1 i il id 1il i Ai-1 id 移動關節(jié)桿件參數(shù)的定義 確定桿件的結(jié)構(gòu)形態(tài)的 2個參數(shù) Li與 i與旋轉(zhuǎn)關節(jié)是一樣的。 確定桿件相對位置關系的 2個參數(shù)則相反。這里 i為常數(shù)， di為變量。 上述 4個參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位 置關系，在轉(zhuǎn)動關節(jié)中， Li, i, di是固定值， i是變量。在 移動關節(jié)中， Li, i, i是固定值， di 是變量。 3.3 機器人關節(jié)坐標系的建立 oO o n-1nX 對于每個桿件都可以在關節(jié)軸處建立一個正規(guī)的笛卡兒 坐標系（ xi, yi, zi），（ i=1, 2, , n ）， n是自由度數(shù)，再 加上基座坐標系，一共有（ n+1）個坐標系。 基座坐標系 定義為 0號坐標系（ x0, y0, z0） ,它也是機 器人的慣性坐標系， 0號坐標系在基座上的位置和方向可 任選，但 軸線必須與關節(jié) 1的軸線重合，位置和方向可 任選； 最后一個坐標系（ n關節(jié)），可以設在手的任意部位，但 必須保證 與 垂直。 機器人關節(jié)坐標系的建立主要是為了描述機器人各桿件和終 端之間的相對運動，對建立運動方程和動力學研究是基礎性 的工作。 為了描述機器人各桿件和終端之間轉(zhuǎn)動或移動關系， Denavit 和 Hartenberg于 1955年提出了一種為運動鏈中每個桿件建立 附體坐標系的矩陣方法 （ D-H方法） ，建立原則如下： D-H關節(jié)坐標系建立原則 右手坐標系 原點 Oi：設在 Li與 Ai+1軸線的交點上 Zi軸 ： 與 Ai+1關節(jié)軸重合，指向任意 Xi軸 ： 與公法線 Li重合，指向沿 Li由 Ai軸線指向 Ai+1軸線 Yi軸 ： 按右手定則 關節(jié)坐標系的建立原則 Ai Ai+1 i il id 1il i Ai-1 1iz 1ix 1iy 1io iz ix iy io 原點 Oi：設在 Li與 Ai+1軸線的交點上 Zi軸：與 Ai+1關節(jié)軸 重合，指向任意 Xi軸：與公法線 Li 重合，指向沿 Li由 Ai軸線指向 Ai+1軸線 Yi軸：按右手定則 桿件長度 Li 沿 xi 軸 ， zi-1 軸與 xi 軸交點到 0i 的距離 桿件扭轉(zhuǎn)角 i 繞 xi 軸 ， 由 zi-1 轉(zhuǎn)向 zi 桿件偏移量 di 沿 zi-1 軸 ， zi-1 軸和 xi 交點至 0i 1 坐標系原點的距離 桿件回轉(zhuǎn)角 i 繞 zi-1 軸 ， 由 xi-1轉(zhuǎn)向 xi 兩種特殊情況 兩軸相交，怎么建立坐 標系？ 0iAi與 Ai+1關節(jié)軸線的交 點； ZiAi+1軸線； XiZi和 Zi-1構(gòu)成的平面的 法線 ； Yi右手定則； i-1 i A i A i + 1 o i z i - 1 z i x i y i 兩軸平行，怎么建立坐標系 (Ai與 Ai+1平行 )？ 先建立 0i-1 然后建立 0i+1 最后建立 0i i-1O D 注意： 由于 Ai和 Ai+1平行 ， 所以公法線任意點 在 A點位置； 按照先前的定義 ， di為 Oi-1點和 A點之間的距離 ， di+1為 B點和 C點間 的距離 ， 這樣設定可以的 ， 但我們可以變更一下 ， 將 0i點放在 C點 ， 定義 Oi在 Li+1和 Ai+1軸的交點上 ， 這樣使 di+1=0使計算簡便 ， 此時 di= A i - 1 A i A i + 1 A i + 2 l i - 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 相鄰 關節(jié)坐標系間的齊次變換過程 機器人運動學正解 1. 將 xi-1軸繞 zi-1 軸轉(zhuǎn) i 角度，將其與 xi軸平行； 2. 沿 zi-1軸平移距離 di ， 使 xi-1 軸與 xi 軸重合； 3. 沿 xi 軸平移距離 Li， 使兩坐標系原點及 x軸 重合； 4. 繞 xi 軸轉(zhuǎn) i 角度，兩 坐標系完全重合 ),(),(),(),(A 111 iiiir a n siir a n siiii xRlxTdZTZR 根據(jù)上述坐標系建立原則，用下列旋轉(zhuǎn)和位移我們 可以建立相鄰的 Oi-1 和 Oi 坐標系之間的關系 Ai Ai+1 i il id 1il i Ai-1 1iz 1ix 1iy 1io iz ix iy io 機器人的運動學正解方程 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 ii 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 = = 機械手的坐標變換圖如圖所示，機械手的末端（即連桿坐標系 i） 相對于基座坐標系 0的描述用 oTi 表示，即： 0 z A1 A2 A3 A4 A5 A6 0 E X 0T6 1T6 2T6 3T6 4T6 5T6 機械手的坐標變換圖 機器人的運動學正解方程 0 0 1 1 12 i iiT A A A 舉例： Stanford機器人 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 為右手坐標系 原點 Oi： Ai與 Ai+1 關節(jié)軸線的交點 Zi軸：與 Ai+1關節(jié)軸 重合，指向任意 Xi軸： Zi和 Zi-1構(gòu)成 的面的法線 Yi軸：按右手定則 Li 沿 xi 軸 ， zi-1 軸與 xi 軸交點到 0i 的距離 i 繞 xi 軸 ， 由 zi-1 轉(zhuǎn)向 zi di 沿 zi-1 軸 ， zi-1 軸和 xi 交點至 0i 1 坐標系原 點的距離 i 繞 zi-1 軸 ， 由 xi-1轉(zhuǎn)向 xi 解： 3.4 例題 試求立方體中心在機座坐標系 0中的位置 該手爪從上方把物體抓起，同時手爪的開合方向與物體的 Y軸同向， 那么，求手爪相對于 0的姿態(tài)是什么？ 在機器人工作臺上加裝一電視攝像機，攝像機可見到固聯(lián) 著 6DOF關節(jié)機器人的機座坐標系原點，它也可以見到被操作 物體（立方體）的中心，如果在物體中心建一局部坐標系，則 攝像機所見到的這個物體可由齊次變換矩陣 T1來表示，如果攝 像機所見到的機座坐標系為矩陣 T2表示。 1000 101-00 2001-0 10-001 T 1000 91-00 10001 1010 T 21 x y z 解 1： T T 21 物機機攝物攝 求，已知 TTT TT 11-2 ）（有： 物攝攝機物機 TTT 1000 91-00 10001 1010 1000 101-00 2001-0 10001 1000 1100 10001- 11010 O 物 根據(jù) T 1 畫出 O 機 根據(jù) T 2 畫出 因此物體位于機座坐標系的（ 11， 10， 1） T 處，它的 X， Y， Z軸分別與機座坐標系的 -Y， X， Z軸平行。 x y z y 機 z 物 y 物 x 物 z 機 o O 機 O 物 解 2： 向重合手爪開合方向與物體 ya : Ts 001有 方向相反方向物體的從上向下抓，指出手爪 zab : Ta 100 則有 Tkji kji asnc 01000 100 001: 1-00 001 010 因此：姿態(tài)矩陣為 重合時 與物體中心 當手爪中心 1000 11-00 10001 11010 T 物 機 O s n a y z x X機 手爪 機實際要求 T pzazsznz pyaysyny pxaxsxnx 1000 工作空間 工作空間 : 末端操作手可以到達的空間位置集合 如何獲得工作空間 : 利用正運動學模型 ,改變關節(jié) 變量值 可達空間 : 末端操作手可以至少以一個姿態(tài)到達的 空間位置集合 靈活空間 : 末端操作手可以以任何姿態(tài)到達的空間 位置集合 如何確定可達空間 ?首先， 令 3變 化 示例 : 平面 3連桿機器人 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 機器人末端操作器位姿的其它 描述方法 用矩陣表示剛性體的轉(zhuǎn)動簡化了許多運算， 但它需要 9個元素來完全描述旋轉(zhuǎn)剛體的姿 態(tài)，因此矩陣并不直接得出一組完備的廣 義坐標。 一組廣義坐標應能描述轉(zhuǎn)動剛體相對于參 考坐標的方向，被稱為歐拉角的三個角度， 、 、 就是這種廣義坐標。 有幾種不同的歐拉角表示方法，它們均可 描述剛體相對于固定參考系的姿態(tài)。三種 最常見的歐拉角類型列在表中 3種最常見的歐拉角類型 步 1 步 2 步 3 類型 1 繞 OZ軸轉(zhuǎn) 角 繞當前 OU 軸轉(zhuǎn) 角 繞當前 OW軸轉(zhuǎn) 角 類型 2 繞 OZ軸轉(zhuǎn) 角 繞當前 OV 軸轉(zhuǎn) 角 繞當前 OW軸轉(zhuǎn) 角 類型 3 繞 OX軸轉(zhuǎn) 角 繞 OY軸轉(zhuǎn) 角 繞 OZ軸轉(zhuǎn) 角 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：表示法通常用于陀螺運動 類型 2： 所得的轉(zhuǎn)動矩陣為右乘 100 0 0 c0s- 010 s0c 100 0 0 ),(),v(),(R cs sc cs sc wRRZR 1000 pz pyR px T ccsss ssccscsscccs sccssccssccc 類型 2 繞 OZ軸轉(zhuǎn) 角 繞當前 OV 軸轉(zhuǎn) 角 繞當前 OW軸轉(zhuǎn) 角 類型 3：一般稱此轉(zhuǎn)動的歐拉角 為偏航角、俯仰和橫滾， （這 種方法也叫做 偏航、俯仰和橫滾 角表示方法）這種形 式主要用 于航空工程中分析飛行器的運動， 其旋轉(zhuǎn)矩陣為 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) 角 正運動學問題 : 已知關節(jié)角度或位移，計算 末端操作手的對應位姿 . 逆運動學問題 : 已知 末端操作手的位姿，求 解對應的關節(jié)變量 . 為 什么逆運動學問題更困難 ? 可能存在多解或無解 通常需多次求解非線性超越方程 3.6 運動學逆問題 解的存在性 目標 點應位于工作空間內(nèi) 可能存在多解，如何選擇最合適的解？ 存在雙解 ! 求解方法 如果各關節(jié)可用某算法獲得，一個機械手是 有解的 . 算法應包含所有可能解 . 封閉形式解（解析解） 數(shù)值解 方法 我們對封閉形式的解法更感興趣 代數(shù)方法 幾何方法 可解性的重要結(jié)論是： 所有具有轉(zhuǎn)動和移動關節(jié)的系統(tǒng)，在一個單一串聯(lián)中 總共有 6個（或小于 6個）自由度時，是可解的，其通 解一般是數(shù)值解，它不是解析表達式，而是利用數(shù)值 迭代原理求解，它的計算量要比解析解大。 但在某些特殊情況下，如若干個關節(jié)軸線相交和或多 個關節(jié)軸線等于 0 或 90 的情況下，具有 6個自由度 的機器人可得到解析解。 為使機器人有解析解，一般設計時，使工業(yè)機器人足 夠簡單，盡量滿足這些特殊條件。 對于給定的機器人 ， 能否求得它的運動學逆解的解析式 （ 也叫封閉解 ） 。 運動學逆問題的可解性 運動學逆問題的多解性 機器人運動問題為解三角方程 ， 解反三角函數(shù)方程時會 產(chǎn)生多解 .顯然對于真實的機器人 ， 只有一組解與實際情 況最相對應 ， 因此必須作出判斷 ， 以選擇合適的解 。 通常采用如下方法剔除多余解： 01 40i 0002 2 2 01 8 040 i 若該關節(jié)運動空間為 ，則應選 。 100 040i 1 根據(jù)關節(jié)運動空間選取合適的解 。 例如求得機器 人某關節(jié)角的兩個解為 2 選擇一個與前一采樣時間最接近的解 ， 例如： 01 40i 0002 2 2 01 8 040 i 若該關節(jié)運動空間為 ， 且 ， 則應選 250 01 160i 0220i 3 根據(jù)避障要求 ， 選擇合適的解 4 逐級剔除多余解 對于具有 n個關節(jié)的機器人 ， 其全部解將構(gòu)成樹形結(jié)構(gòu) 。 為簡化起見 ， 應逐級剔除多余解 。 這樣可以避免在樹形解中 選擇合適的解 。 迭代法 計算量大 幾何法 適用于自由度較少的情況 反變換法 運動學逆問題解法 用未知的逆變換逐次左乘，由乘得的矩陣方程 的元素決定未知數(shù)，即用逆變換把一個未知數(shù) 由矩陣方程的右邊移到左邊 考察方程式左、右兩端對應元素相等，以產(chǎn)生 一個有效方程式，理論上可得到 12個方程。 然后求這個三角函數(shù)方程式，以求解未知數(shù) 把下一個未知數(shù)移到左邊 重復上述過程，直到解出所有解 缺點：無法由數(shù)種可能的解中直接得出合適的解， 需要通過人為的選擇 運動學逆問題解法 反變換法 Paul 等人提出的方法 (1981年，也叫求逆的方 法，是解析解