機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)

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

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

3、沿著沿著i i+ +1 1的轉(zhuǎn)動(dòng)關(guān)節(jié)軸線；的轉(zhuǎn)動(dòng)關(guān)節(jié)軸線； XiXi軸：軸：沿著沿著ZiZi1 1和和ZiZi的公垂線，指向離開的公垂線，指向離開ZiZi1 1軸的方向；軸的方向；坐標(biāo)系的坐標(biāo)系的YiYi軸由軸由XiXi和和ZiZi確定。確定。至此，連桿至此，連桿i i的坐標(biāo)的坐標(biāo)系確立。系確立。1 1、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟（1 1）建立坐標(biāo)系）建立坐標(biāo)系桿件坐標(biāo)系桿件坐標(biāo)系ii 第一種坐標(biāo)系第一種坐標(biāo)系： ii坐標(biāo)系建立坐標(biāo)系建立在第在第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、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟（1 1）建立坐標(biāo)系）建立坐標(biāo)系桿件坐標(biāo)系桿件坐標(biāo)系ii 第二種坐標(biāo)系第二種坐標(biāo)系： ii坐標(biāo)系建立坐標(biāo)系建立在第在第i i關(guān)節(jié)上。關(guān)節(jié)上。1 1、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟（1 1）建立坐標(biāo)）建立坐標(biāo)系系手部坐標(biāo)系手部坐標(biāo)系hh 在第一種桿在第一種桿件坐標(biāo)系下，件坐標(biāo)系下，hh與與nn坐標(biāo)系坐標(biāo)系重合。重合。x0z0o00123關(guān)節(jié)關(guān)節(jié)1關(guān)節(jié)關(guān)節(jié)2關(guān)節(jié)關(guān)節(jié)3z2x2o2x1y1o1Z3hx3ho3h1 1、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟（1 1）建立坐標(biāo)系）建立坐標(biāo)

5、系手部坐標(biāo)系手部坐標(biāo)系hh 在第二種桿在第二種桿件坐標(biāo)系下，件坐標(biāo)系下，hh與與nn坐標(biāo)系的方坐標(biāo)系的方向保持一致。向保持一致。ohx0z0o00123關(guān)節(jié)關(guān)節(jié)1關(guān)節(jié)關(guān)節(jié)2關(guān)節(jié)關(guān)節(jié)3x2y2o2z3x3o3x1z1o1Zhxh1 1、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟（2 2）確定參數(shù)）確定參數(shù)桿件幾何參數(shù)（不桿件幾何參數(shù)（不變）變） I I、桿件長(zhǎng)度、桿件長(zhǎng)度ai：兩關(guān)節(jié)軸線的距離。兩關(guān)節(jié)軸線的距離。 IIII、桿件扭角、桿件扭角i：兩關(guān)節(jié)軸線的夾角。兩關(guān)節(jié)軸線的夾角。iaii1 1、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟（2 2）確定參數(shù)）確定參數(shù)關(guān)節(jié)運(yùn)動(dòng)參數(shù)關(guān)節(jié)運(yùn)動(dòng)參數(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、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟（3 3）相鄰桿件位姿矩陣）相鄰桿件位姿矩陣（4 4）建立方程）建立方程用表示機(jī)器人連桿用表示機(jī)器人連桿n n坐標(biāo)系的坐標(biāo)變換成連桿坐標(biāo)系的坐標(biāo)變換成連桿n n11坐標(biāo)系的坐標(biāo)的齊次坐標(biāo)變換矩陣，通常把坐標(biāo)系的坐標(biāo)的齊次坐標(biāo)變換矩陣，通常把上標(biāo)省略，寫成上標(biāo)省略，寫成AnAn。對(duì)于。對(duì)于n n個(gè)關(guān)節(jié)的機(jī)器人，前個(gè)關(guān)節(jié)的機(jī)器人，前一個(gè)關(guān)節(jié)向后一個(gè)關(guān)節(jié)的坐標(biāo)齊次變換矩陣分別一個(gè)關(guān)節(jié)向后一個(gè)關(guān)節(jié)的坐標(biāo)齊次變換矩陣分別為為也就是也

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

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

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

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

11、若用矩陣形式表示，則為： 10000100001000123312211123123123312211123123slslslcsclclclscpCBApCBApCBAzzzzyyyyxxxx1 1、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟解解：（4 4）建立方程）建立方程若用方程組形式表示，則為：若用方程組形式表示，則為： 123312211123312211123123123123slslslpclclclpcBsBsAcAyxyxyx1 1、運(yùn)動(dòng)學(xué)方程建立步驟、運(yùn)動(dòng)學(xué)方程建立步驟解解：（1 1）建立坐標(biāo)系）建立坐標(biāo)系( (第二種第二種) ) a a、機(jī)座坐標(biāo)系、機(jī)座坐標(biāo)系0 0 b b、

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

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