3關(guān)節(jié)平面機(jī)械臂運(yùn)動(dòng)學(xué)方程

機(jī)械手臂的運(yùn)動(dòng)學(xué)公式推導(dǎo) 圖1 3關(guān)節(jié)平面機(jī)械臂 1. 3關(guān)節(jié)平面機(jī)械臂 3關(guān)節(jié)平面機(jī)械臂有3個(gè)自由度，關(guān)節(jié)1有1個(gè)自由度，關(guān)節(jié)2有1個(gè) 自由度，關(guān)節(jié)3有1個(gè)自由度 機(jī)器人手臂的幾何尺寸（mm）: 關(guān)節(jié)1長(zhǎng)度：L1 關(guān)節(jié)2長(zhǎng)度：L2 關(guān)節(jié)3長(zhǎng)度：L3 關(guān)節(jié)的運(yùn)動(dòng)范圍（右手）：如表1所示。 表1關(guān)節(jié)運(yùn)動(dòng)范圍 關(guān)節(jié) 1 2 3 最大值 Max1 <0Max2 <0Max3 最小值 Min1 Min2 Min3 2.機(jī)器人手臂的坐標(biāo)系建立 參考坐標(biāo)系 （1）為了對(duì)3關(guān)節(jié)平面機(jī)械臂進(jìn)行控制，同時(shí)也便于描述機(jī)器人的動(dòng)作狀態(tài)， 必須建立適當(dāng)?shù)某跏甲鴺?biāo)系。我們?cè)O(shè)定機(jī)械臂的初始姿態(tài)：關(guān)節(jié) 1、關(guān)節(jié)2和關(guān) 節(jié)3均處于水平姿態(tài)，與世界坐標(biāo)系（xo,yo）的xo軸的夾角為0度。 參考坐標(biāo)系（實(shí)驗(yàn)室坐標(biāo)系）的設(shè)定如圖 1所示： X軸：從關(guān)節(jié)i到關(guān)節(jié)i+1的方向定義為X軸，即沿連桿方向 y軸：根據(jù)X軸和Z軸的方向，以右手螺旋法則確定 Z軸：沿關(guān)節(jié)軸方向，即垂直紙面，從里向外為 Z軸正方向 (2)連桿參數(shù) 連桿參數(shù)列表如表2所示 C1 -S1 0 0 0T = lS 0 0! 丨A I 0 1 0 [0 0 1 一 ■C2 1 _S2 0 LJ 1 ；T = IS2 1 C2 0 0 1 10 1 0 1 0 .0 0 0 1 一 C3 _S3 0 呵 2t = IS3 C3 0 0 31 I0 0 1 0 0 0 0 1 一 —(si C2C3 C1S2C3 C1C2S^ _ S1S2S3) qC2C3 1 S1S2C3 -sqq 1 qC2S3 0 0 C1C2C^ _ SIS2C^ _C1S2S^ _ S1C2S3 SIC2C^ "*"C1S2C^*"C1C2S3 _ S1S2S3 0 I 0 0 0 1 0 L1c^ +L^c1c^s1s2 ) Ll5 + L2 ( sc2 +5% ) 0 1 (1) C123 S123 0 I -0 _S123 L1C1 C123 L2c12 L2SI2 表2連桿參數(shù) 連桿i ai-1 a i-1 di Oi 關(guān)節(jié)變量范圍 1 0 0 0 01 0Min1 ?@Max1 2 L1 0 0 02 0Min2 ?0Max2 3 L2 0 0 O3 OMin3 ?0Max3 正解： 連桿之間的齊次變換矩陣為:  「C 0 xl 1 0 yl 1 0 1 0 1 0 .0 0 0 1 一 其中 C1： cos (5) (6) c2： cos2 s1: sinG^ s1: sinG (7) C - C123 (8) s = si23 (9) x = L)C| L2q2 (10) ~ Lisi L2s12 (11) 式(1)為3關(guān)節(jié)平面機(jī)械臂的變換矩陣，式(2)為采用三角函數(shù)和差角公式化 簡(jiǎn)得到的，式(3)為式⑵的簡(jiǎn)化表示，式(4)-式(11)為簡(jiǎn)化符號(hào)的詳細(xì)表示。 反解： 幾何解： * y0 圖2 3關(guān)節(jié)平面機(jī)械臂的平面幾何關(guān)系 圖2給出了 3關(guān)節(jié)平面機(jī)械臂的幾何關(guān)系，可以看出，在世界坐標(biāo)系(xo,yo)下,由連桿Li,L2以及關(guān)節(jié)1和關(guān)節(jié)3的連線(xiàn)構(gòu)成三角形。圖中虛線(xiàn)所示為構(gòu)成三 角形的另一種情況，對(duì)于實(shí)線(xiàn)構(gòu)成的三角形，采用余弦定理可得 X2 y2 =『 L22 -2IhL2COS(180 二2) (12) 由于 cos(180+62)=-cos62,所以 L12 L2： 2L丄2 (13) 5 三角形成立的條件為 2邊之和大于第三邊，因此 L1+L2必須大于 ?,x2—y2?？衫蒙鲜綑z驗(yàn)反解是否存在，當(dāng)上述條件不成立時(shí)，反 解不存在。當(dāng)反解存在時(shí)，即可由(13)式得出G2的值。 為求⑨，可先求出B和収根據(jù)三角函數(shù)與三角形各邊的關(guān)系， (14) (15) (16) 應(yīng)用2幅角反正切公式得： :=Ata n 2( y, x) cos?= X2 y2 _L12 _L22 2L1 ,x2 y2 再利用余弦定理求出 式中，+-號(hào)根據(jù)G2的符號(hào)取，當(dāng)62<0,取正號(hào)，反之取負(fù)號(hào)。 平面內(nèi)旋轉(zhuǎn)角度可加和，因此3個(gè)連桿的旋轉(zhuǎn)角度之和即為末端連桿 的姿態(tài)，也即機(jī)械臂末端的姿態(tài)。 玉八 (17) 由以上式(1)-式(17)，可反解出所有連桿在世界坐標(biāo)系的變換矩陣， 即姿態(tài)。 #