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

1..建立D-H坐标系。D-H坐标（后置）杆号转角变量θi连杆扭角αi连杆间距di连杆长度ai11-20.502220.5033-20.504420.505000.502.计算Tii−1Tii−1=[Rii−1P000i];R=[CθiSθi−SθiCαiSαiSθiCθiCαi−CθiSαi0SαiCαi]ii−1;P=(aiCθiaiSθidi);T=[Cθi−SθiCαiSθiCθiCαiSαiSθiaiCθi−CθiSαiaiSθi0Sαi00Cαidi01]𝑖𝑖−1.即：T=[Cθ1−Sθ1Cα1Sθ1Cθ1Cα1Sα1Sθ1a1Cθ1−Cθ1Sα1a1Sθ10Sα100Cα1d101]10即：T=[Cθ10Sθ10−Sθ10Cθ100−10000.501]10∴T=[Cθ20Sθ20Sθ20−Cθ20010000.501]21；T=[Cθ30Sθ30−Sθ30Cθ300−10000.501]32；T=[Cθ40Sθ40Sθ40−Cθ40010000.501]43；T=[10010000000010.501]𝑡43.任意设定各关节变量，计算TT0（解运动学正问题）；设定θ1=θ2=θ3=θ4=π3⁄,则：T=T10∙T21∙T32∙T43∙Tt4t0=[0.500000.86000−0.866000.500000−10000.501]∙[0.500000.860000.86600−0.50000010000.501]∙[0.500000.86000−0.866000.500000−10000.501]∙[0.500000.860000.86600−0.50000010000.501].[10010000000010.501]=[−0.6875−0.6495−0.3247−0.1250−0.3247−0.70360.93741.0312−0.64950.750000−0.12501.062501.0000]4.利用Paul反变换法求解各关节变量.∵T=T10∙T21∙T32∙T43∙Tt4t0在式的两边同时乘T−110得：T−1∙10T=T21∙T32∙T43∙Tt4t0………………………………………⑴由T10得T−110=[Cθ1Sθ10000−10.5−Sθ1Cθ1000001]∴⑴式左边为T−1∙10Tt0=[Cθ1Sθ10000−10.5−Sθ1Cθ1000001]∙[−0.6875−0.6495−0.3247−0.1250−0.3247−0.70360.93741.0312−0.64950.750000−0.12501.062501.0000]=[−0.6875C1−0.3247S1−0.6495C1−0.1250S10.6495−0.7500−0.3247C1+0.9374S1−0.7036C1+1.0312S10.1250−1.06250.6875S1−0.3247C10.6495S1−0.1250C1000.3247S1+0.9374C10.7036S1+1.0312C101]⑴式右边为：T21∙T32∙T43∙Tt4=[nxoxnyoyaxpxaypynzoz00azpz01]其中：nx=Cθ2Cθ3Cθ4−Sθ2Sθ4;ny=Sθ2Cθ3Cθ4+Cθ2Sθ4;nz=Sθ3Cθ4ox=−Sθ3Cθ2;oy=−Sθ3Sθ2;oz=Cθ3ax=Cθ2Cθ3Sθ4+Cθ4Sθ2;ay=Sθ2Cθ3Sθ4−Cθ2Cθ4;az=Sθ3Sθ4px=0.5Cθ2Cθ3Sθ4+0.5Cθ4Sθ2−0.5Sθ3Cθ2+0.5Sθ2;py=0.5Sθ2Cθ3Sθ4−0.5Cθ4Cθ2−0.5Sθ3Sθ2−0.5Cθ2;pz=0.5Sθ3Sθ4+0.5Cθ3+0.5由⑴式两边对应元素相等得Sθ3Sθ4=0.3247S1+0.9374C1…………………………………………………………………⑵0.5Sθ3Sθ4+0.5Cθ3+0.5=0.7036S1+1.0312C1……………………………………………⑶Cθ3=0.6495S1−0.1250C1……………………………………………………………………⑷解得θ1≈π3⁄，将θ1≈π3⁄代入⑷得θ3=π3⁄将θ1，θ3代入⑵得θ4=π3⁄又∵−Sθ3Sθ2=−0.7500…………………………………………………………………………⑸解得θ2=π3⁄5.由机器人的位移方程求其末端轨迹（x,y,z）T=T10∙T21∙T32∙T43∙Tt4t0=[Cθ10Sθ10−Sθ10Cθ100−10000.501]∙[Cθ20Sθ20Sθ20−Cθ20010000.501]∙[Cθ30Sθ30−Sθ30Cθ300−10000.501]∙[Cθ40Sθ40Sθ40−Cθ40010000.501]∙[10010000000010.501]=[nxoxnyoyaxpxaypynzoz00azpz01]其中px=0.5Cθ1Cθ2Cθ3Sθ4−0.5Sθ4Sθ1Sθ3+0.5Cθ4Cθ1Sθ2−0.5Sθ3Cθ2Cθ1−0.5Sθ1Cθ3+0.5Sθ2Cθ1−0.5Sθ1py=0.5Sθ1Cθ2Cθ3Sθ4+0.5Sθ4Cθ1Sθ3+0.5Cθ4Sθ1Sθ2−0.5Sθ3Cθ2Sθ1+0.5Cθ1Cθ3+0.5Sθ2Sθ1+0.5Cθ1pz=0.5Cθ2Cθ4−0.5Cθ3Sθ2Sθ4+0.5Sθ3Sθ2+0.5Cθ2+0.5当)/tsin(i4（)t10利用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).*cos(a).*cos(a)-0.5.*cos(a).*cos(a).*sin(a)-0.5.*sin(a).*cos(a)+0.5.*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).*sin(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(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');title('机器人末端轨迹');gridon;得到的图形为下图，机器人末端轨迹图利用ADAMS仿真图像如下所示:7.当Tt)]**cos(2*0.15**2,t)**sin(2*0.15**2[0,Xm/s，10t，并任意设定关节初始位形，求解机器人关节轨迹（利用速度方程求解运动学逆问）,并用ADAMS模型进行仿真。机器人正运动解方程为：JX则其逆运动解方程为：1JX其中J为雅戈比方程且123412341234xxxxyyyyJzzzz[nxoxnyoyaxpxaypynzoz00azpz01]其中个元素如下所示:nx=−0.5Sθ1Cθ2Cθ3Sθ4−0.5Sθ4Cθ1Sθ3−0.5Cθ4Sθ1Sθ2+0.5Sθ3Cθ2Sθ1−0.5Cθ1Cθ3−0.5Sθ2Sθ1−0.5Cθ1;ox=−0.5Cθ1Sθ2Cθ3Sθ4+0.5Cθ4Cθ1Cθ2+0.5Sθ3Sθ2Cθ1+0.5Cθ2Cθ1;ax=−0.5Cθ1Cθ2Sθ3Sθ4−0.5Sθ4Sθ1Cθ3−0.5Cθ3Cθ2Cθ1+0.5Sθ1Sθ3;px=0.5Cθ1Cθ2Cθ3Cθ4−0.5Cθ4Sθ1Sθ3−0.5Sθ4Cθ1Sθ2;ny=0.5Cθ1Cθ2Cθ3Sθ4−0.5Sθ4Sθ1Sθ3+0.5Cθ4Cθ1Sθ2−0.5Sθ3Cθ2Cθ1−0.5Sθ1Cθ3+0.5Sθ2Cθ1−0.5Sθ1;oy=−0.5Sθ1Sθ2Cθ3Sθ4+0.5Cθ4Sθ1Cθ2+0.5Sθ3Sθ2Sθ1+0.5Cθ2Sθ1;ay=−0.5Sθ1Cθ2Sθ3Sθ4+0.5Sθ4Cθ1Cθ3−0.5Cθ3Cθ2Sθ1−0.5Cθ1Sθ3;py=0.5Sθ1Cθ2Cθ3Cθ4+0.5Cθ4Cθ1Sθ3−0.5Sθ4Sθ1Sθ2;nZ=0;oZ=−0.5Sθ2Cθ4−0.5Cθ3Sθ4Cθ2+0.5Cθ2Sθ3−0.5Sθ2;aZ=0.5Sθ4Sθ2Sθ3+0.5Sθ2Cθ3;pZ=−0.5Cθ2Sθ4−0.5Cθ4Sθ2Cθ3;利用MATLAB程序仿真的关节轨迹程序如下:functiondS=M(t,x)n1=-0.5*sin(x(1))*cos(x(2))*cos(x(3))*sin(x(4))-0.5*sin(x(4))*cos(x(1))*sin(x(3))-0.5*sin(x(2))*sin(x(1))*cos(x(4))+0.5*sin(x(1))*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));o1=-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(1))*sin(x(2))*sin(x(3))+0.5*cos(x(2))*cos(x(1));a1=-0.5*cos(x(1))*cos(x(2))*sin(x(3))*sin(x(4))-0.5*cos(x(3))*sin(x(1))*sin(x(4))-0.5*cos(x(1))*cos(x(2))*cos(x(3))+0.5*sin(x(3))*sin(x(1));p1=0.5*cos(x(1))*cos(x(2))*cos(x(3))*cos(x(4))-0.5*sin(x(3))*sin(x(1))*cos(x(4))-0.5*cos(x(1))*sin(x(2))*sin(x(4));n2=0.5*cos(x(1))*cos(x(2))*cos(x(3))*sin(x(4))-0.5*sin(x(4))*sin(x(1))*sin(x(3))+0.5*sin(x(2))*cos(x(1))*cos(x(4))-0.5*cos(x(1))*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(1));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(1))*sin(x(2))*sin(x(3))+0.5*cos(x(2))*sin(x(1));a2=-0.5*sin(x(1))*cos(x(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.5*sin(x(1))*cos(x(2))*cos(x(3))*cos(x(4))+0.5*sin(x(3))*cos(x(1))*cos(x(4))-0.5*sin(x(1))*sin(x(2))*