DH坐標系建立

1、 坐標變換的最終目的是確定機器人各桿件之間坐標變換的最終目的是確定機器人各桿件之間 的相互位置關系，通過關節(jié)角的值計算末端操作器的相互位置關系，通過關節(jié)角的值計算末端操作器 在空間位置（正運動學）或預算出末端操作器在要在空間位置（正運動學）或預算出末端操作器在要 求位置的關節(jié)角（逆運動學）。求位置的關節(jié)角（逆運動學）。 主要包括以下內(nèi)容：主要包括以下內(nèi)容： 1) 相對桿件的坐標系的確定；相對桿件的坐標系的確定； 2) 建立各連桿的模型矩陣建立各連桿的模型矩陣A； 3) 正運動學算法；正運動學算法； 4) 逆運動學算法。逆運動學算法。 在機器人中，通常有兩類在機器人中，通常有兩類 關節(jié)：轉(zhuǎn)動關節(jié)

2、和移動關節(jié)。關節(jié)：轉(zhuǎn)動關節(jié)和移動關節(jié)。 不同于人類的關節(jié)，一般機器不同于人類的關節(jié)，一般機器 人關節(jié)為一個自由度的關節(jié)，人關節(jié)為一個自由度的關節(jié)， 其目的是為了簡化力學、運動其目的是為了簡化力學、運動 學和機器人的控制。轉(zhuǎn)動關節(jié)學和機器人的控制。轉(zhuǎn)動關節(jié) 提供了一個轉(zhuǎn)動自由度，移動提供了一個轉(zhuǎn)動自由度，移動 關節(jié)提供一個移動自由度，各關節(jié)提供一個移動自由度，各 關節(jié)間是以固定桿件相連接的。關節(jié)間是以固定桿件相連接的。 旋轉(zhuǎn)關節(jié)有兩種基本形式：旋轉(zhuǎn)關節(jié)有兩種基本形式： 鉸鏈和兩桿的相對轉(zhuǎn)動。鉸鏈和兩桿的相對轉(zhuǎn)動。 關關 節(jié)節(jié) 桿桿 件件 機器人桿件是連接兩個關節(jié)的固定物體（機機器人桿件是連接兩個

3、關節(jié)的固定物體（機 械）。機器人桿件的主要目的是用來保持該關節(jié)械）。機器人桿件的主要目的是用來保持該關節(jié) 與各相關末端關節(jié)一個固定的關系。機器人末端與各相關末端關節(jié)一個固定的關系。機器人末端 桿件只有一個關節(jié)，位于最接近末端（或機座）桿件只有一個關節(jié)，位于最接近末端（或機座） 的位置。在最遠離機座的末端，通常是附加一個的位置。在最遠離機座的末端，通常是附加一個 手爪。為了更容易、清楚地解釋一個機器人的末手爪。為了更容易、清楚地解釋一個機器人的末 端和其各關節(jié)點的關系，課程中只以有限的桿件端和其各關節(jié)點的關系，課程中只以有限的桿件 數(shù)作為研究討論對象。實際上，為了使得機器人數(shù)作為研究討論對象。實

4、際上，為了使得機器人 更容易制造，類似的限制在機器人制造中也使用。更容易制造，類似的限制在機器人制造中也使用。 下面給出了八種類型的常見桿件構形：下面給出了八種類型的常見桿件構形： 1) 兩個平行的轉(zhuǎn)動關節(jié)且在兩個平行的轉(zhuǎn)動關節(jié)且在 兩軸間沒有扭轉(zhuǎn)；連桿參數(shù)兩軸間沒有扭轉(zhuǎn)；連桿參數(shù)ln 連桿的長度；如果連桿的中心線連桿的長度；如果連桿的中心線 被認作被認作x方向并且從關節(jié)方向并且從關節(jié)n-1到關到關 節(jié)節(jié)n沿沿xn-1方向有一定距離，整個方向有一定距離，整個 桿件可以繞關節(jié)桿件可以繞關節(jié)n-1轉(zhuǎn)動轉(zhuǎn)動 n角，該角，該 角認為是兩連桿夾角并且這個角角認為是兩連桿夾角并且這個角 就是一般轉(zhuǎn)動關節(jié)的

5、變量；同時就是一般轉(zhuǎn)動關節(jié)的變量；同時 關節(jié)軸被認為是關節(jié)軸被認為是z方向并且繞方向并且繞zn-1 轉(zhuǎn)動；轉(zhuǎn)動；y軸由右手定則確定。軸由右手定則確定。 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)軸軸線轉(zhuǎn)軸軸線) (2) Xn-1：垂直兩連桿公垂線：垂直兩連桿公垂線xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離；的距離； (2) 兩關節(jié)軸扭角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的轉(zhuǎn)角；的轉(zhuǎn)角；

6、(3) dn：兩連桿間偏置：兩連桿間偏置：xn-1 xn沿沿zn-1距離，距離，0； (4) n：兩連桿間角度，：兩連桿間角度，Xn-1 Xn繞繞zn-1的轉(zhuǎn)角，的轉(zhuǎn)角， 變量。變量。 2) 兩個轉(zhuǎn)動關節(jié)在空間形成兩個轉(zhuǎn)動關節(jié)在空間形成 兩軸間兩軸間90的扭轉(zhuǎn)；連桿參數(shù)的扭轉(zhuǎn)；連桿參數(shù) ln連桿的長度；如果連桿的中連桿的長度；如果連桿的中 心線被認作心線被認作x方向并且從關節(jié)方向并且從關節(jié)n-1 到關節(jié)到關節(jié)n沿沿xn-1方向有一定距離，方向有一定距離， 整個桿件可以繞關節(jié)整個桿件可以繞關節(jié)n-1轉(zhuǎn)動轉(zhuǎn)動 n角，角， 該角認為是兩連桿夾角，這個角該角認為是兩連桿夾角，這個角 就是一般轉(zhuǎn)動關節(jié)的

7、變量；同時就是一般轉(zhuǎn)動關節(jié)的變量；同時 關節(jié)軸被認為是關節(jié)軸被認為是z方向并且方向并且 繞繞zn-1轉(zhuǎn)動；轉(zhuǎn)動；y軸由右手定則確定。軸由右手定則確定。 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)軸軸線轉(zhuǎn)軸軸線) (2) Xn-1：xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離；的距離； (2) 兩關節(jié)軸扭角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的轉(zhuǎn)角，的轉(zhuǎn)角， 90； (3) dn ：兩連桿間偏

8、置：兩連桿間偏置：xn-1 xn沿沿zn-1距離，距離， 0； (4) n：兩連桿間角度，：兩連桿間角度，Xn-1 Xn繞繞Zn-1的轉(zhuǎn)的轉(zhuǎn) 角，變量。角，變量。 3) 兩個轉(zhuǎn)動關節(jié)相互交兩個轉(zhuǎn)動關節(jié)相互交 叉垂直軸；兩個關節(jié)轉(zhuǎn)動軸叉垂直軸；兩個關節(jié)轉(zhuǎn)動軸 相交，連桿參數(shù)相交，連桿參數(shù)ln=0；關節(jié)；關節(jié) 軸被認為是軸被認為是z方向；方向；x方向由方向由 兩個兩個z軸確定，軸確定，y方向由右手方向由右手 定則確定；定則確定；d n為偏移值 為偏移值。 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)軸軸線、柱軸方向線轉(zhuǎn)軸軸線、柱軸

9、方向線) (2) Xn-1：xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離，兩軸相交為的距離，兩軸相交為0； (2) 兩關節(jié)軸扭角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的轉(zhuǎn)角，的轉(zhuǎn)角，-90 ； (3) dn ：兩連桿間偏置：兩連桿間偏置：xn-1 xn沿沿zn-1距離，距離，dn； (4) n：連桿繞：連桿繞zn-1的轉(zhuǎn)角，變量。的轉(zhuǎn)角，變量。 4) 兩個轉(zhuǎn)動關節(jié)相互垂兩個轉(zhuǎn)動關節(jié)相互垂 直并且重合；連桿參數(shù)直并且重合；連桿參數(shù)ln=0； 關節(jié)軸被認為是關節(jié)軸被

10、認為是z方向；方向；x方方 向由兩個向由兩個z軸確定，軸確定， y方向由方向由 右手定則；右手定則；dn為偏移值。原為偏移值。原 點 由點 由 n - 1 坐 標 系 決 定坐 標 系 決 定 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)軸軸線、柱軸方向線轉(zhuǎn)軸軸線、柱軸方向線) (2) Xn-1、 、： ：xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離，兩軸相交的距離，兩軸相交0； (2) 兩關節(jié)軸扭

11、角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的轉(zhuǎn)角，的轉(zhuǎn)角，90 ； (3) dn ：兩連桿間距離：兩連桿間距離：xn-1 xn沿沿zn-1移動距離，移動距離，0； (4) n：連桿繞：連桿繞zn-1的轉(zhuǎn)角，變量。的轉(zhuǎn)角，變量。 5) 兩個移動柱關節(jié)相互兩個移動柱關節(jié)相互 垂直并相交；連桿參數(shù)垂直并相交；連桿參數(shù)ln=0； 關節(jié)軸線方向是關節(jié)軸線方向是z向；向；x向由向由z 軸確定，軸確定， y方向由右手定則方向由右手定則 確定；確定；dn 與與dn+1為柱關節(jié)變量。為柱關節(jié)變量。 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)

12、軸軸線、柱軸方向線轉(zhuǎn)軸軸線、柱軸方向線) (2) Xn-1、 、： ：xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離，兩軸相交的距離，兩軸相交0； (2) 兩關節(jié)軸扭角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的轉(zhuǎn)角，的轉(zhuǎn)角，-90 ； (3) dn ：移動關節(jié)移動變量；：移動關節(jié)移動變量； (4) n：移動關節(jié)轉(zhuǎn)角為：移動關節(jié)轉(zhuǎn)角為0。 6) 一個轉(zhuǎn)動關節(jié)和一個一個轉(zhuǎn)動關節(jié)和一個 移動柱關節(jié)相互垂直并相交；移動柱關節(jié)相互垂直并相交； 連桿參數(shù)連桿參數(shù)ln=0；轉(zhuǎn)動關

13、節(jié)軸；轉(zhuǎn)動關節(jié)軸 線方向和移動關節(jié)移動方向線方向和移動關節(jié)移動方向 是是z向；向；x方向由方向由z軸確定，軸確定，y 方向由右手定則確定；方向由右手定則確定；n為轉(zhuǎn)為轉(zhuǎn) 動關節(jié)變量、動關節(jié)變量、dn+1為移動關節(jié)為移動關節(jié) 變量。變量。 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)軸軸線、柱軸方向線轉(zhuǎn)軸軸線、柱軸方向線) (2) Xn-1、 、： ：xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離，兩軸

14、相交的距離，兩軸相交0； (2) 兩關節(jié)軸扭角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的轉(zhuǎn)角，的轉(zhuǎn)角，-90 ； (3) dn+1 ：移動關節(jié)移動變量，：移動關節(jié)移動變量，dn=偏置量；偏置量； (4) n：轉(zhuǎn)動關節(jié)變量。：轉(zhuǎn)動關節(jié)變量。 7) 一個移動柱關節(jié)和一一個移動柱關節(jié)和一 個轉(zhuǎn)動關節(jié)相互平行；連桿個轉(zhuǎn)動關節(jié)相互平行；連桿 參數(shù)參數(shù)ln=偏置量；關節(jié)軸線方偏置量；關節(jié)軸線方 向是向是z向；向；x方向由方向由z軸確定，軸確定， y方向由右手定則；轉(zhuǎn)動和方向由右手定則；轉(zhuǎn)動和 移動關節(jié)變量為移動關節(jié)變量為 n和 和d n + 1 。 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、

15、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)軸軸線、柱軸方向線轉(zhuǎn)軸軸線、柱軸方向線) (2) Xn-1、 、： ：xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離；的距離； (2) 兩關節(jié)軸扭角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的轉(zhuǎn)角，的轉(zhuǎn)角，0 ； (3) dn+1 ：移動關節(jié)移動變量；：移動關節(jié)移動變量； (4) n：轉(zhuǎn)動關節(jié)變量。：轉(zhuǎn)動關節(jié)變量。 8) 一個移動柱關節(jié)和一一個移動柱關節(jié)和一 個轉(zhuǎn)動關節(jié)相互垂直；連桿個轉(zhuǎn)動關節(jié)相互垂直；連桿 參

16、數(shù)參數(shù)ln=0；關節(jié)軸線方向是；關節(jié)軸線方向是z 向；向；x向由向由z軸確定，軸確定，y方向方向 由右手定則；由右手定則；dn為偏移值。為偏移值。 坐標系及其相關參數(shù)確定：坐標系及其相關參數(shù)確定： 一、坐標系確定一、坐標系確定 (1) Zn-1的確定的確定(轉(zhuǎn)軸軸線、柱軸方向線轉(zhuǎn)軸軸線、柱軸方向線) (2) Xn-1、 、： ：xn -1=zn-1 zn (3) Yn-1 ：根據(jù)右手定則確定 ：根據(jù)右手定則確定 二、參數(shù)確定二、參數(shù)確定 (1) 連桿長度連桿長度ln：zn-1 zn沿沿xn-1的距離，兩軸相交的距離，兩軸相交0； (2) 兩關節(jié)軸扭角兩關節(jié)軸扭角 n： zn-1 zn繞繞xn的

17、轉(zhuǎn)角，的轉(zhuǎn)角，90 ； (3) dn ：移動關節(jié)移動變量；：移動關節(jié)移動變量； (4) n+1：轉(zhuǎn)動關節(jié)變量。：轉(zhuǎn)動關節(jié)變量。 學習目標：學習目標：1 理解理解D-H法原理法原理 2 學會用學會用D-H法對機器人建模法對機器人建模 學習重點：學習重點：1 給關節(jié)指定參考坐標系給關節(jié)指定參考坐標系 2 制定制定D-H參數(shù)表參數(shù)表 3 利用參數(shù)表計算轉(zhuǎn)移矩陣利用參數(shù)表計算轉(zhuǎn)移矩陣 一一 背景簡介：背景簡介： 1955年，年，Denavit和和Hartenberg提出了這一方提出了這一方 法，后成為表示機器人以及對機器人建模的標準方法，后成為表示機器人以及對機器人建模的標準方 法，應用廣泛。法，應用

18、廣泛。 二二 總體思想總體思想 首先給每個關節(jié)指定坐標系，然后確定從一個首先給每個關節(jié)指定坐標系，然后確定從一個 關節(jié)到下一個關節(jié)進行變化的步驟，這體現(xiàn)在兩個關節(jié)到下一個關節(jié)進行變化的步驟，這體現(xiàn)在兩個 相鄰參考坐標系之間的變化，將所有變化結合起來，相鄰參考坐標系之間的變化，將所有變化結合起來， 就確定了末端關節(jié)與基座之間的總變化，從而建立就確定了末端關節(jié)與基座之間的總變化，從而建立 運動學方程，進一步對其求解。運動學方程，進一步對其求解。 1. 坐標系的確定坐標系的確定 規(guī)則規(guī)則 一一 關節(jié)，連桿命關節(jié)，連桿命 名規(guī)則：第一個名規(guī)則：第一個 關節(jié)指定為關節(jié)關節(jié)指定為關節(jié)n, 第二個關節(jié)為第二

19、個關節(jié)為n+1, 其余關節(jié)以此類其余關節(jié)以此類 推。連桿命名規(guī)推。連桿命名規(guī) 則與關節(jié)相同。則與關節(jié)相同。 坐標系的確定坐標系的確定 坐標系的確定規(guī)則坐標系的確定規(guī)則 2. Z軸確定規(guī)則軸確定規(guī)則 如果關節(jié)是旋轉(zhuǎn)的，如果關節(jié)是旋轉(zhuǎn)的，Z軸位于按右手規(guī)則旋轉(zhuǎn)的方軸位于按右手規(guī)則旋轉(zhuǎn)的方 向，轉(zhuǎn)角向，轉(zhuǎn)角 為關節(jié)變量。如果關節(jié)是滑動的，為關節(jié)變量。如果關節(jié)是滑動的，Z軸軸 為沿直線運動的方向，連桿長度為沿直線運動的方向，連桿長度d為關節(jié)變量。關為關節(jié)變量。關 節(jié)節(jié)n處處Z軸下標為軸下標為n-1。 3. X軸確定規(guī)則軸確定規(guī)則 情況情況1 兩關節(jié)兩關節(jié)Z軸既不平行也不相交軸既不平行也不相交 取兩取兩

20、Z軸公垂線方向作為軸公垂線方向作為X軸方向，命名規(guī)則同軸方向，命名規(guī)則同Z軸軸 X軸確定規(guī)則軸確定規(guī)則 情況情況2：兩關節(jié)：兩關節(jié)Z軸平行軸平行 此時，兩此時，兩Z軸之間有無數(shù)條公垂線，可挑選與前一軸之間有無數(shù)條公垂線，可挑選與前一 關節(jié)的公垂線共線的一條公垂線關節(jié)的公垂線共線的一條公垂線 情況情況3：兩關節(jié)：兩關節(jié)Z軸相交軸相交 取兩條取兩條Z軸的叉積方向作為軸的叉積方向作為X軸。軸。 Y軸及變量確定規(guī)則軸及變量確定規(guī)則 4. Y軸確定原則軸確定原則 取取X軸，軸，Z軸差積方向作為軸差積方向作為Y軸方向。（右手）軸方向。（右手） 5. 變量選擇原則變量選擇原則 用用 角表示繞角表示繞Z軸的旋

21、轉(zhuǎn)角，軸的旋轉(zhuǎn)角，d表示在表示在Z軸上兩條相軸上兩條相 鄰的公垂線之間的距離，鄰的公垂線之間的距離，a表示每一條公垂線的長表示每一條公垂線的長 度度(關節(jié)偏移關節(jié)偏移)，角，角 表示兩個相鄰表示兩個相鄰Z軸之間的角度軸之間的角度 也叫關節(jié)扭轉(zhuǎn)。通常情況下，只有也叫關節(jié)扭轉(zhuǎn)。通常情況下，只有 和和d是關節(jié)變是關節(jié)變 量。量。 到達下一坐標系的標準運動到達下一坐標系的標準運動 我們可以通過以下幾個運動，將一個參考坐標系變我們可以通過以下幾個運動，將一個參考坐標系變 換到下一個參考坐標系。換到下一個參考坐標系。 1 繞繞Zn軸旋轉(zhuǎn)軸旋轉(zhuǎn) ， 它使得它使得Xn+1和和Xn互互 相平行相平行 1n 到達

22、下一坐標系的標準運動到達下一坐標系的標準運動 2 沿沿Zn軸平移軸平移dn+1距離，距離， 使得使得Xn和和Xn+1共線共線 3 沿沿Xn軸平移軸平移an+1的距離，的距離， 使使Xn和和Xn+1的原點重合。的原點重合。 到達下一坐標系的標準運動到達下一坐標系的標準運動 4 將將Zn軸繞軸繞Xn+1軸旋轉(zhuǎn)軸旋轉(zhuǎn) ，使得，使得Zn軸與軸與 Zn+1軸對準。軸對準。 1n 這樣就實現(xiàn)了從一個坐標系變換下一個坐標系這樣就實現(xiàn)了從一個坐標系變換下一個坐標系 列出變換矩陣列出變換矩陣 由于所有的運動都是相對于當前坐標系而言的。由于所有的運動都是相對于當前坐標系而言的。 因此，總的變換矩陣因此，總的變換矩

23、陣A等于各變換矩陣右乘。等于各變換矩陣右乘。 從而得到的結果如下：從而得到的結果如下： 1000 0 ),()0 , 0 ,(), 0 , 0( 111 1111111 1111111 1 111),(11 1 nnn nnnnnnn nnnnnnn n nnnznn n dCS SSCCCS CaSSCSC A xRotaTransdTransRotAT n 以此類推，總的變換矩陣為：以此類推，總的變換矩陣為： nH R AAAAT 321 D-H參數(shù)表參數(shù)表 通過原理圖確定各參數(shù)，制定通過原理圖確定各參數(shù)，制定D-H參數(shù)表如下：參數(shù)表如下： 將各參數(shù)帶入矩陣方程即可得到運動學方程，進一將各

24、參數(shù)帶入矩陣方程即可得到運動學方程，進一 步求解。步求解。 例2.11 如圖，已知一兩桿 機構，兩桿長分別為l1、l2， 轉(zhuǎn)角變量為 、 ， 1)確定零位置；2) 建立連桿坐標系 3)確定參數(shù)； 4) 確定工作空間 1 2 例2.12 如圖，已知一兩桿 機構，一桿偏置為d1、二 桿長為l2，轉(zhuǎn)角變量為 ， ： 1) 建立連桿坐標系； 2) 確定參數(shù)； 3) 確定工作空間 1 2 例2.13 如圖，已知一兩桿 機構，一桿偏置為d1、二 桿偏置為d2，變量為 ， d2 ： 1) 建立連桿坐標系； 2) 確定參數(shù)； 3) 確定工作空間 1 例2.14 如圖，已知一兩 桿機構： 1) 建立連桿坐標系；

25、 2) 確定參數(shù)； 3) 確定工作空間 例2.15 如圖，已知一兩 桿機構： 1) 建立連桿坐標系； 2) 確定參數(shù)； 3) 確定工作空間 例例 題題 2.19 對下圖所示簡單機器人，根據(jù)對下圖所示簡單機器人，根據(jù)D-H法，建立必要坐法，建立必要坐 標系及參數(shù)表。標系及參數(shù)表。 第一步：根據(jù)第一步：根據(jù)D-H法建立坐標系的規(guī)則建立坐標系法建立坐標系的規(guī)則建立坐標系 第二步：將做好的坐標系簡化為我們熟悉的線圖形式第二步：將做好的坐標系簡化為我們熟悉的線圖形式 第三步：根據(jù)建立好的坐標系，確定各參數(shù)，并寫第三步：根據(jù)建立好的坐標系，確定各參數(shù)，并寫 入入D-H參數(shù)表參數(shù)表 1 2 3 4 5 6

26、2 a 3 a 4 a 第四步：將參數(shù)代入第四步：將參數(shù)代入A矩陣，可得到矩陣，可得到 1000 0010 00 00 11 11 1 CS SC A 1000 0100 0 0 2222 2222 2 aSCS aCSC A 1000 0100 0 0 3333 3333 3 aSCS aCSC A 1000 0010 0 0 4444 4444 4 aSCS aCSC A 1000 0010 00 00 55 55 5 CS SC A 1000 0100 00 00 66 66 6 CS SC A 第第5步步 求出總變化矩陣求出總變化矩陣 1000 ) ()()()( ) ()()()(

27、223234234523462346523465234 22323 42341 51 52341 651 6234652341 651 6234652341 22323 42341 51 52341 651 6234652341 651 6234652341 654321 aSaSaSSSCCCCSCCS aCaC aCS CC SCS SSC CSCCCS SSC SSCCCS aCaC aCC CS SCC SSS CSCCCC CSS SSCCCC AAAAAATH R 思考題思考題 654321 AAAAAAT H R 讓我們通過下面這道例題來了解一下機器人逆運動學讓我們通過下面這道例

28、題來了解一下機器人逆運動學 求解的一般步驟。例求解的一般步驟。例2.19最后方程為最后方程為 1000 zzzz yyyy xxxx H R paon paon paon T 求逆運動學方程的解求逆運動學方程的解 根據(jù)第根據(jù)第3行第行第4列元素對應相等可得到列元素對應相等可得到 依次用依次用 左乘上面兩個矩陣，得到：左乘上面兩個矩陣，得到： 1 1 A 1000 0 1000 56565 2232342345234623465234623465234 2232342345234623465234623465234 11111111 11111111 CSSCS aSaSaSSSCCCCSSCC

29、CS aCaCaCSCCSCCCSSCCC CPSPCaSaCoSoCnSn paon SPCPSaCaSoCoSnCn yxyxyxyx zzZz yxyxyxyx 180)arctan( 111 和 x y p p 根據(jù)根據(jù)1，4元素和元素和2，4元素，可得到：元素，可得到： 將上面兩個方程兩邊平方相加，并利用和差將上面兩個方程兩邊平方相加，并利用和差 化積公式得到化積公式得到 32 2 3 2 2 2 4234 2 423411 3 3232232 2 )()( cos aa aaaSpaCSpCp C CCSS zyx 于是有： 223234234 22323423411 aSaSaS

30、p aCaCaCSpCp z yx 已知已知 2 33 1 CS 于是可得到：于是可得到： 3 3 3 arctan C S 依次類推，分別在方程依次類推，分別在方程2.19兩邊左乘兩邊左乘A1A4的的 逆，可得到逆，可得到 1000 00 0 0 1000 )()()()( 0 )( ) ()()( 66 56565 56565 34234 23411234 234 11234 234 11234 234 11234 111111 434234 23411234 2341 1234 234 11234 234 11234 CS CSSCS SSCCC aSaS pCPSPCS aC aSaC

31、S oC oSoCS nC nSnCS aSaCoSoCnSnC aaCaC pSpSpCC aSaS aCC oS oSoCC nS nSnCC zyx z yx z yx z yx xyxyxy zyx xy x z yx z yx 機器人的逆運動學解 z yx yx z a aSa aSaC a )CS C 180)arctan( 11234 234 234234 11 234 （ 和 這樣，這樣， 就可以計算出來了，接下來再一次利用式就可以計算出來了，接下來再一次利用式 由于由于C12=C1C2-S1S2以及以及S12=S1C2+C1S2，最后得到：，最后得到： 3 機器人的逆運動學解 yx zyx xy zyx zyx yxz aCaS aSaSaCC aSaCC aSaSa aSpaSaCSpCpaaC aCSpCpaSaSpaaC 11 23411234 5 115 234112345 322344 423433423411233 423411334234233 2 )( arctan )CCS )()( )()( arctan （ ，可以得到再根據(jù)對應項元素相等 進而可得： 223234234 22323423411