4R机构矩阵法求解运动学

1、1 建立D-H坐标系。DH坐标（后置）杆号转角变量连杆扭角连杆间距连杆长度9idiai1q0.502e2%0.5030.504q%0.505000.502.计算i-；TCOj -SOjCaj SajSOj-i-：R = S0j C&Cai -COiSaj.0Soq CoqS0jCctjCOjCaj Sa： 0SttiSOjCOjSotjCetiajCOfajS0j di 1 -即:C0!SOiCcXjSa】 SB】a±CQiS0!COiCttiCOjSeXiaOi0Sa】Cgdi.0001 -?T =cex o?T =S0J 00 -1LO 0-S0! O'C0i 0

2、 00.50 1 .0C020S02O'C030-S030-2T =S02001C02000.5；!T =S9300-1CO3000.5.0001 .0001 .C040S04O'rl000!t =S04001C04000.500100100.5.0001 .0001 -3.任意设定各关节变量，计算;了（解运动学正问题）;设定 = 02 = 04 = K/3 '则：= 0T . 1T . 2T . 3T .和-0.6875-0.3247-0.64950-0.6495-0.12500.75000-0.32470.9374-0.12500-0.703611.03121.06

3、251.00000.50000-0.8660o-0.500000.8660o-0.50000-0.8660o-0.860000.500000.86000-0.500000.860000.500000-100.50100.50-100.500000010000.500000.86600-110000.86000-0.5000001000100.50010.50001-0001 -4.利用Paul反变换法求解各关节变量. ：T=和畀孑T:T-fT在式的两边同时乘:T“得：T“ ；T =扑 h-4T-|TC0!S0X00 '-0.6875-0.6495-0.3247-0.7036-100-1

4、0.5-0.3247-0.12500.93741.0312S0iC0i00-0.64950.7500-0.12501.06250001 .0001.0000 -:厂1¥t二由;T 得;T-1 二C0!0SB】.0 -S0!0CE00-10000.501 .-0.6875C1 -0.3247S10.64950.6875S - 0.3247C0(1)式右边为： 0.6495C - 0.1250S1-0.75000.6495S1 -0.1250C10 0.3247C1 + 0.9374S0.12500.3247S1 + 0.9374C00.7036C + 1.0312Si-1.06250.

5、7036S1 + 1.0312C1r«xOxPxny°yayPynzOzazPzLo001J1T . 2T . 3T . 二其中:；nynx = C02C03C04 S02S04=S02C03®4 + C02S04 ； nz = S03C04Oz = C03ox = S03C02 ； Oy S03S02 ax = C02C03S04 + C04S02 ； 3y = S02C63S04 C02C04 ; az = S03S04px = o.5co2co3se4 + o.5co4se2 - o.5se3co2 + o.sso2 ；Py = O.5S02C03S04 O

6、.5C04C02 O.5S03S02 O.5C02 ；pz = O.5S03S04 + O.5C03 + 0.5III式两边对应元素相等得Se3S04 = 0.3247Sx + 0.9374Ci O.5se3se4 + O.5C03 + 0.5 = 0.7036Si + 1.0312CiC03 = 0.6495S - 0.1250Ci解得°严%，将°严兀沧代入得0 3 = %将0, 0 3代入得°4 = 73乂丁 -Se3S02=-O.75OO解得0 2 = "/33山机器人的位移方程求其末端轨迹（x,y,z）0T = 0T . 1T . 2T . 3T

7、 . 4T 二C0!0SBO'ce20S02O'ce30-S&3O'C040S04(TS0!0C0i0S020C020se30CO30S040C0400-100.50100.50-100.50100.5.0001 ._o001 .0001.0001 .1000 -nxOxax px-0100ny°yay Py苴中0010.5nz°z5 Pz0001 -Lo00 1px = o.5ce1c02c03s04 -o.ssecece -+O.5S02C0i - O.5S0!py = O.5S01C02C03S04 + O.5S04C01S03 +-

8、O.SSECESE +O.5S02S0! + O.5C0!pz = 0.5Ce2C04 - O.5CO3SO2S04 + O.5S03SO2 + O.5C02 + 0.5当Oi =sm(7rt/4) (0<t<l)利用MATLAB对末端轨迹进行仿真，其MATLAB程序如下：t二0:0. 001:1;a=sin(pi/4*t);x二0. 5. *cos (a) *cos (a) *cos (a) *sin(a)一0 5 *sin(a) *sin(a) *sin(a) +0. 5 *sin(a) *co s (a) *cos (a)一0 3 *cos (a) *cos (a) *sin

9、(a)一0. 5 *sin(a) *cos (a) +0 3 *sin(a) *cos (a)一 0. 5. *sin(a);y二0. 5. *sin(a) *cos (a) *cos (a) *sin(a) +0. 5. *sin(a) *cos (a) *sin(a) +0. 5 *sin(a) *si n (a) *cos (a) 一0 5 *sin (a) *cos (a) *sin (a) +0. 5 *cos (a) *cos (a) +0 5 *sin (a) *sin (a) + 0. 5. *cos (a);z二0. 5. *cos (a) *cos (a)一0 5 *sin

10、(a) *cos (a) *sin(a) +0. 5 *sin(a) *sin(a) +0 5 *cos (a) +0.5;plot3(x, y, z,' r');xlabel (' x');ylabel (' y');zlabel (' z');titleC机器人末端轨迹')；grid on;得到的图形为下图,机器人末端轨迹机器人末端轨迹图利用ADAMS仿真图像如下所示:7.当 X = 0, 2*0.15 *sin(2*t),2*0.15 *cos(2* t)rm/s, °G<1,并任意设定关 节初始位形

11、，求解机器人关节轨迹(利用速度方程求解运动学逆问)，并用ADAMS模型进行仿 真。机器人正运动解方程为：X=J0则其逆运动解方程为：e=jx其中丿为雅戈比方程且8xdx6rnxOx axPxlde±ny°y亏Py4nzoz azPz&804Lo0 01其中个元素如下所示：nx = O.5S016020304 O.5S04C01S03 O.5C04S01S02 + 0.58030291 O.5C01C03 O.5S02S0! - O.5C0!;ox = -O.5C0XS02C03S04 + 05“4(：久(：&2 + O.5S63se2C0i + O.5C02

12、C0i ;ax = -O.5C01C62SO3se4 - O.5S04S01C03 - O.SCCEC% + 0.58003;px = O.5C01C92Ce3ce4 - O.5C04SOiS03 - O.5S04ceiS02;ny = O.5C01C02C03S04 - OESSBiS% + 05“4(：&»&2 - OSBiC% +O.5S02C0X- O.5S0!;oy = -O.5S01S02C03S04 + O.5C04S01C02 + O.SSSES% + OECESE;ay = -0.5S01Ce2S63S04 + O.5S04C01C03 一 Q.SC

13、ezS 一 OECES%;py = O.5S01C02C03C04 + O.5C04COJS03 - OESSBiSE；Oz = -O.5S02C04 O.5C03S04C02 + O.5C02S03 O.5S02 ；az = O.5S04S02S03 + O.5S02C03 ;Pz = O.5C02S04 O.5C04S02C03;利用MATLAB程序仿真的关节轨迹程序如下：function dS=M(t, x)nl二-0. 5*sin(x(1)*cos(x(2)*cos(x(3)*sin(x(4)-0. 5*sin(x(4)*cos(x(l)*sin(x(3) -0. 5*sin(x(2)

14、*sin(x(l)*cos(x(4) +0. 5*sin(x(l)*cos(x(2)*sin(x(3)-0 5*cos(x(1) )*cos (x (3)-0 5*sin(x (2) *sin(x (1)-0 5*cos (x (1);ol二-0. 5*cos (x(1) *sin(x (2) *cos (x (3) *sin(x (4) +0. 5*cos (x (2) *cos (x (1) *cos (x(4) +0. 5*cos(x(l)*sin(x(2)*sin(x(3) +0. 5*cos(x(2)*cos(x(1);al=0. 5*cos(x(1)*cos(x(2)*sin(x(

15、3)*sin(x(4)-0. 5*cos(x(3)*sin(x(l)*sin(x(4) -0. 5*cos (x (1) *cos (x (2) *cos (x (3) +0. 5*sin(x (3) *sin(x (1);pl二0. 5*cos(x(l)*cos(x(2)*cos(x(3)*cos(x(4)-0. 5*sin(x(3)*sin(x(l)*cos(x(4)- 0. 5*cos(x(l)*sin(x(2)*sin(x(4);n2=0. o*cos(x(1)*cos(x(2)*cos(x(3)*sin(x(4)-0. 5*sin(x(4)*sin(x(l)*sin(x(3) +0.

16、 5*sin(x(2)*cos(x(l)*cos(x(4)-0. 5*cos(x(l)*cos(x(2)*sin(x(3)-0. 5*sin(x(1) *cos(x(3) +0. 5*sin(x(2)*cos(x(1)-0. 5*sin(x(l);o2=0. 5*sin(x(1) *sin(x(2) *cos (x (3) *sin(x (4) +0. 5*cos (x(2) *sin(x (1) *cos (x (4) +0. 5*sin(x(l) *sin (x (2) *sin (x (3) +0. 5*cos (x (2)*sin(x(l);a2=0. o*sin(x(1)*cos(x

17、(2)*sin(x(3)*sin(x(4)+0. 5*cos(x(3)*cos(x(1)*sin(x(4) -0. 5*sin (x (1) *cos (x (2) *cos (x (3) -0. 5*sin (x (3) *cos (x (1);p2=0. o*sin(x(1)*cos(x(2)*cos(x(3)*cos(x(4)+0. 5*sin(x(3)*cos(x(l)*cos(x(4)- 0. 5*sin(x(1)*sin(x(2)*sin(x (4);n3二 0;o3=0. 5*sin(x(2)*cos(x(4)-0. 5*cos(x(2)*cos(x(3)*sin(x(4)+0. 5*cos(x(2)*sin(x (3)-0. 5*sin(x(2);a3=0. o*sin(x(2)*sin(x(3)*sin(x(4)+0. 5*sin(x(2)*cos (x(3);p3二-0. 5*cos (x (2) *sin(x(4)-0. 5*sin(x (2) *cos (x(3) *cos (x (4);J=nl ol al pl;n2 o2 a2 p2;n3 o3 a3 p3;J_1二pinv(J);dy=0;2*pi*0. 15*sin(2*pi*t);2*pi*0. 15*cos(2*pi*t);dS