機器人運動學與動力學

1、運動學方程的模型：運動學方程的模型： T=f(qi)， i=1，n T機器人手在空間的位姿機器人手在空間的位姿 qi機器人各個關(guān)節(jié)變量機器人各個關(guān)節(jié)變量1 1、運動學方程建立步驟、運動學方程建立步驟 （1 1）建立坐標系）建立坐標系 （2 2）確定參數(shù)）確定參數(shù) （3 3）相鄰桿件的位姿矩陣）相鄰桿件的位姿矩陣 （4 4）建立方程）建立方程1 1、運動學方程建立步驟、運動學方程建立步驟（1 1）建立坐標系）建立坐標系 機座坐標系機座坐標系00 桿件坐標系桿件坐標系ii i=1 i=1，2 2，n n 手部坐標系手部坐標系hh1 1、運動學方程建立步驟、運動學方程建立步驟（1 1）建立坐標系）建

2、立坐標系 機座坐標系機座坐標系00 建立原則：建立原則：z z0 0軸垂直，軸垂直， x x0 0軸水軸水平，平，x x0 0方向指向手部所在方向指向手部所在平面。平面。x0z0o01 1、運動學方程建立步驟、運動學方程建立步驟（1 1）建立坐標系）建立坐標系桿件坐標系桿件坐標系ii，i=1i=1，2 2，n n建立坐標系的總原則：是使桿件建立坐標系的總原則：是使桿件的單步坐標變換簡單的單步坐標變換簡單建立三維運動坐標系的三原則：建立三維運動坐標系的三原則：轉(zhuǎn)動關(guān)節(jié)的轉(zhuǎn)動關(guān)節(jié)的D-HD-H坐標系建立如圖坐標系建立如圖1.161.16所示。所示。連桿連桿i i的坐標系的的坐標系的ZiZi軸：軸：

3、沿著沿著i i+ +1 1的轉(zhuǎn)動關(guān)節(jié)軸線；的轉(zhuǎn)動關(guān)節(jié)軸線； XiXi軸：軸：沿著沿著ZiZi1 1和和ZiZi的公垂線，指向離開的公垂線，指向離開ZiZi1 1軸的方向；軸的方向；坐標系的坐標系的YiYi軸由軸由XiXi和和ZiZi確定。確定。至此，連桿至此，連桿i i的坐標的坐標系確立。系確立。1 1、運動學方程建立步驟、運動學方程建立步驟（1 1）建立坐標系）建立坐標系桿件坐標系桿件坐標系ii 第一種坐標系第一種坐標系： ii坐標系建立坐標系建立在第在第i+1i+1關(guān)節(jié)上。關(guān)節(jié)上。x0z0o00123關(guān)節(jié)關(guān)節(jié)1關(guān)節(jié)關(guān)節(jié)2關(guān)節(jié)關(guān)節(jié)3z2x2o2x1y1o1z3x3o3x0z0o00123關(guān)節(jié)

4、關(guān)節(jié)1關(guān)節(jié)關(guān)節(jié)2關(guān)節(jié)關(guān)節(jié)3x2y2o2z3x3o3x1z1o11 1、運動學方程建立步驟、運動學方程建立步驟（1 1）建立坐標系）建立坐標系桿件坐標系桿件坐標系ii 第二種坐標系第二種坐標系： ii坐標系建立坐標系建立在第在第i i關(guān)節(jié)上。關(guān)節(jié)上。1 1、運動學方程建立步驟、運動學方程建立步驟（1 1）建立坐標）建立坐標系系手部坐標系手部坐標系hh 在第一種桿在第一種桿件坐標系下，件坐標系下，hh與與nn坐標系坐標系重合。重合。x0z0o00123關(guān)節(jié)關(guān)節(jié)1關(guān)節(jié)關(guān)節(jié)2關(guān)節(jié)關(guān)節(jié)3z2x2o2x1y1o1Z3hx3ho3h1 1、運動學方程建立步驟、運動學方程建立步驟（1 1）建立坐標系）建立坐標

5、系手部坐標系手部坐標系hh 在第二種桿在第二種桿件坐標系下，件坐標系下，hh與與nn坐標系的方坐標系的方向保持一致。向保持一致。ohx0z0o00123關(guān)節(jié)關(guān)節(jié)1關(guān)節(jié)關(guān)節(jié)2關(guān)節(jié)關(guān)節(jié)3x2y2o2z3x3o3x1z1o1Zhxh1 1、運動學方程建立步驟、運動學方程建立步驟（2 2）確定參數(shù)）確定參數(shù)桿件幾何參數(shù)（不桿件幾何參數(shù)（不變）變） I I、桿件長度、桿件長度ai：兩關(guān)節(jié)軸線的距離。兩關(guān)節(jié)軸線的距離。 IIII、桿件扭角、桿件扭角i：兩關(guān)節(jié)軸線的夾角。兩關(guān)節(jié)軸線的夾角。iaii1 1、運動學方程建立步驟、運動學方程建立步驟（2 2）確定參數(shù)）確定參數(shù)關(guān)節(jié)運動參數(shù)關(guān)節(jié)運動參數(shù) I I、關(guān)節(jié)

6、平移量、關(guān)節(jié)平移量di： IIII、關(guān)節(jié)回轉(zhuǎn)量、關(guān)節(jié)回轉(zhuǎn)量i：關(guān)節(jié)變量：關(guān)節(jié)變量：di平移關(guān)節(jié)；平移關(guān)節(jié)；i回轉(zhuǎn)回轉(zhuǎn)關(guān)節(jié)。關(guān)節(jié)。1 1、運動學方程建立步驟、運動學方程建立步驟（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣（4 4）建立方程）建立方程用表示機器人連桿用表示機器人連桿n n坐標系的坐標變換成連桿坐標系的坐標變換成連桿n n11坐標系的坐標的齊次坐標變換矩陣，通常把坐標系的坐標的齊次坐標變換矩陣，通常把上標省略，寫成上標省略，寫成AnAn。對于。對于n n個關(guān)節(jié)的機器人，前個關(guān)節(jié)的機器人，前一個關(guān)節(jié)向后一個關(guān)節(jié)的坐標齊次變換矩陣分別一個關(guān)節(jié)向后一個關(guān)節(jié)的坐標齊次變換矩陣分別為為也就是也

7、就是其中，其中，A1A1表示桿件表示桿件1 1上的上的1 1號坐標系到機座的號坐標系到機座的0 0號號坐標系的齊次坐標變換矩陣。坐標系的齊次坐標變換矩陣。iAnnnRHRAAAATTTTT3211322111 1、運動學方程建立步驟、運動學方程建立步驟例：例：已知三自由度平面關(guān)節(jié)機器人如圖所示，已知三自由度平面關(guān)節(jié)機器人如圖所示，設(shè)機器人桿件設(shè)機器人桿件1 1、2 2、3 3的長度為的長度為l1，l2，l3。建立。建立機器人的運動學方程。機器人的運動學方程。 l1l3l21 1、運動學方程建立步驟、運動學方程建立步驟解解：（1 1）建立坐標系）建立坐標系( (第一種第一種) ) a a、機座坐

8、標系、機座坐標系0 0 b b、桿件坐標系、桿件坐標系i i c c、手部坐標系、手部坐標系h h （與末端桿件坐標系（與末端桿件坐標系 n n重合）重合） l1l3l2x0y0y1x1y2x2y3hx3h1 1、運動學方程建立步驟、運動學方程建立步驟解解：（2 2）確定參數(shù)）確定參數(shù)l1l3l2x0y0y1x1y2x2y3hx3h3211 1、運動學方程建立步驟、運動學方程建立步驟解解：（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3hx3h32110000100sin0cossincos0sincos1000010000100011000010000co

9、ssin00sincos)0 , 0 ,(),(11111111111111101llllTranszRotM1 1、運動學方程建立步驟、運動學方程建立步驟解解：（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3hx3h32110000100sin0cossincos0sincos)0 , 0 ,(),(222222222212lllTranszRotM同理可得：1 1、運動學方程建立步驟、運動學方程建立步驟解解：（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣l1l3l2x0y0y1x1y2x2y3hx3h32110000100sin0cossincos0sin

10、cos)0 , 0 ,(),(3333333333)(23lllTranszRotMh同理可得：1 1、運動學方程建立步驟、運動學方程建立步驟解解：（4 4）建立方程）建立方程將相鄰桿件位姿矩陣依次相乘，則有：將相鄰桿件位姿矩陣依次相乘，則有： 1000010000123312211123123123312211123123)(2312010 slslslcsclclclscMMMMhh)sin(),cos(321123321123 sc式式中中：)sin(),cos(21122112 sc1 1、運動學方程建立步驟、運動學方程建立步驟解解：（4 4）建立方程）建立方程若用矩陣形式表示，則為：

11、若用矩陣形式表示，則為： 10000100001000123312211123123123312211123123slslslcsclclclscpCBApCBApCBAzzzzyyyyxxxx1 1、運動學方程建立步驟、運動學方程建立步驟解解：（4 4）建立方程）建立方程若用方程組形式表示，則為：若用方程組形式表示，則為： 123312211123312211123123123123slslslpclclclpcBsBsAcAyxyxyx1 1、運動學方程建立步驟、運動學方程建立步驟解解：（1 1）建立坐標系）建立坐標系( (第二種第二種) ) a a、機座坐標系、機座坐標系0 0 b b、

12、桿件坐標系、桿件坐標系i i c c、手部坐標系、手部坐標系h h （與末端桿件坐標系（與末端桿件坐標系 n n方向一致）方向一致） l1l3l2x0y0y1x1y2x2y3x3yhxh1 1、運動學方程建立步驟、運動學方程建立步驟解解：（2 2）確定參數(shù)）確定參數(shù)l1l3l2x0y0y1x1y2x2y3x3yhxh3211 1、運動學方程建立步驟、運動學方程建立步驟解解：（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣1000010000cossin00sincos),(1111101zRotMl1l3l2x0y0y1x1y2x2y3x3yhxh3211 1、運動學方程建立步驟、運動學方程建立步

13、驟解解：（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣1000010000cossin0sincos),()0 , 0 ,(221222112lzRotlTransMl1l3l2x0y0y1x1y2x2y3x3yhxh3211 1、運動學方程建立步驟、運動學方程建立步驟解解：（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣1000010000cossin0sincos),()0 , 0 ,(332333223lzRotlTransMl1l3l2x0y0y1x1y2x2y3x3yhxh3211 1、運動學方程建立步驟、運動學方程建立步驟解解：（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣 100001000010001)0 , 0 ,(333hllTransMl1l3l2x0y0y1x1y2x2y3x3yhxh3211 1、運動學方程建立步驟、運動學方程建立步驟解解：（4 4）建立方程）建立方程將相鄰桿件位姿矩陣依次相乘，則有：將相鄰桿件位姿矩陣依次相乘，則有： 100001000012331221112312312331221112312332312010 slslslcscl