机器人大作业讲解

1、一 利用前置法建立D-H坐标系如下图所示：照上图由题意列写连杆参数表如下表所示：杆号i连杆扭角连杆长度连杆间距转角变量1000.5200.5300.5400.5500.5其中初始位置时：，。2 计算各由所学知道，将上表中的各个值代入可得：4 任意设定各关节变量，计算。其中：令；代入以上各式并计算得：5 根据上面得到的，利用Paul反变换法求解各关节变量。令，求。由得到：其中由此得到：上式中： 由对应项相等可得：，令 （即 ）代入 、 并由对应式相等得到： 将式代入得到： 由 得到： 将式代入 可得： 又因为 所以 ，将、式代入 可得： 所以 ，代入式可得： 所以 。6 当（时利用机器人的位移方

2、程求解其末端轨迹（x,y,z）；由题意可令， 则 ，计算可得： 因为 其中 ，令 则上式简化为： 7 利用ADAMS建模，并测量机器人末端轨迹7.1 首先利用Matlab编程画出其末端轨迹，程序如下：t=0:0.01:1;th=sin(pi*t/4);a=sin(th);b=cos(th);x=0.5*(b.*b.*b.*b.*a-2*a.*a.*a.*b+a.*a.*a+b.*b.*a+a);y=0.5*(b.*b.*b.*a.*a-b.*b.*b+b.*b.*a.*a-b.*b-b-a.*a.*a.*a-a.*a.*a-a.*a);z=0.5*(a.*a.*b.*b+b.*b+b.*a.*

3、a+b+a.*a+1);plot3(x,y,z);xlabel(X),ylabel(Y),zlabel(Z);title(末端轨迹图);grid on;所得结果如下图所示：7.2 利用ProE仿真满足条件的轨迹如下图所示：7.3 各轴位移对比如下所示： X方向的对比： Y方向的对比： Z方向的对比：图中可以看出，matlab的计算结果与仿真吻合8 当m/s，并任意设定关节初始位形，求解机器人关节轨迹并用ADAMS模型进行仿真。已知 求 ，由所学可得： 。其中 8.1 利用MATLAB计算机器人各个关节轨迹设各关节初始位形为 。编写第一个M文件，文件名为zhaolei.mfunction dS=

4、zhaolei( t,th )th1=th(1);th2=th(2);th3=th(3);th4=th(4);th5=th(5);x1=(-sin(th1)*cos(th2)*cos(th3)*cos(th4)*sin(th5)-cos(th1)*sin(th3)*cos(th4)*cos(th5)-sin(th1)*sin(th4)*sin(th5)-sin(th1)*cos(th2)*sin(th3)*cos(th5)+cos(th1)*cos(th3)*cos(th5)+cos(th1)*sin(th3)*sin(th4)+sin(th1)*cos(th2)*cos(th3)*sin(t

5、h4)-sin(th1)*cos(th4)-sin(th1)*cos(th2)*sin(th3)+cos(th1)*cos(th3)-sin(th1)+cos(th1)/2;x2=(-cos(th1)*sin(th2)*cos(th3)*cos(th4)*sin(th5)-cos(th2)*sin(th4)*sin(th5)-cos(th1)*sin(th2)*sin(th3)*cos(th5)+cos(th1)*sin(th2)*cos(th3)*sin(th4)-cos(th2)*cos(th4)-cos(th1)*sin(th2)*sin(th3)-cos(th2)/2;x3=(-cos

6、(th1)*cos(th2)*sin(th3)*cos(th4)*sin(th5)-sin(th1)*cos(th3)*cos(th4)*sin(th5)+cos(th1)*cos(th2)*cos(th3)*cos(th5)-sin(th1)*sin(th3)*cos(th5)+sin(th1)*cos(th3)*sin(th4)+cos(th1)*cos(th2)*sin(th3)*sin(th4)+cos(th1)*cos(th2)*cos(th3)-sin(th1)*sin(th3)/2;x4=(-cos(th1)*cos(th2)*cos(th3)*sin(th4)*sin(th5)

7、+sin(th1)*sin(th3)*sin(th4)*sin(th5)+cos(th1)*cos(th4)*sin(th5)-sin(th2)*cos(th4)*sin(th5)+sin(th1)*sin(th3)*cos(th4)-cos(th1)*cos(th2)*cos(th3)*cos(th4)-cos(th1)*sin(th4)+sin(th2)*sin(th4)/2;x5=(cos(th1)*cos(th2)*cos(th3)*cos(th4)*cos(th5)-sin(th1)*sin(th3)*cos(th4)*cos(th5)+cos(th1)*sin(th4)*cos(t

8、h5)-sin(th2)*sin(th4)*cos(th5)-cos(th1)*cos(th2)*sin(th3)*sin(th5)-sin(th1)*cos(th3)*sin(th5)/2;y1=(cos(th1)*cos(th2)*cos(th3)*cos(th4)*sin(th5)-sin(th1)*sin(th3)*cos(th4)*sin(th5)-cos(th1)*sin(th2)*sin(th4)*sin(th5)+cos(th1)*cos(th2)*sin(th3)*cos(th5)+sin(th1)*cos(th3)*cos(th5)+cos(th1)*cos(th2)*si

9、n(th3)+sin(th1)*cos(th3)-cos(th1)*sin(th2)+sin(th1)-cos(th1)*cos(th2)*cos(th3)*sin(th4)+sin(th1)*sin(th3)*sin(th4)-cos(th1)*sin(th2)*sin(th4)/2;y2=(-sin(th1)*sin(th2)*cos(th3)*cos(th4)*sin(th5)-sin(th1)*cos(th2)*sin(th4)*sin(th5)-sin(th1)*sin(th2)*sin(th3)*cos(th5)-sin(th1)*sin(th2)*sin(th3)-sin(th1

10、)*cos(th2)+sin(th1)*sin(th2)*cos(th3)*sin(th4)-sin(th1)*cos(th2)*sin(th4)/2;y3=(-sin(th1)*cos(th2)*sin(th3)*cos(th4)*sin(th5)+cos(th1)*cos(th3)*cos(th4)*sin(th5)+sin(th1)*cos(th2)*cos(th3)*cos(th5)+cos(th1)*sin(th3)*cos(th5)+sin(th1)*cos(th2)*cos(th3)+cos(th1)*sin(th3)+sin(th1)*cos(th2)*sin(th3)*sin

11、(th4)-cos(th1)*cos(th3)*sin(th4)/2;y4=(-sin(th1)*cos(th2)*cos(th3)*sin(th4)*sin(th5)-cos(th1)*sin(th3)*sin(th4)*sin(th5)-sin(th1)*sin(th2)*cos(th4)*sin(th5)-sin(th1)*cos(th2)*cos(th3)*cos(th4)-cos(th1)*sin(th3)*cos(th4)-sin(th1)*sin(th2)*cos(th4)/2;y5=(sin(th1)*cos(th2)*cos(th3)*cos(th4)*cos(th5)+co

12、s(th1)*sin(th3)*cos(th4)*cos(th5)-sin(th1)*sin(th2)*sin(th4)*cos(th5)-sin(th1)*cos(th2)*sin(th3)*sin(th5)+cos(th1)*cos(th3)*sin(th5)/2;z1=0;z2=(cos(th2)*cos(th3)*cos(th4)*sin(th5)-sin(th2)*sin(th4)*sin(th5)+cos(th2)*sin(th3)*cos(th5)+cos(th2)*sin(th3)-sin(th2)-cos(th2)*cos(th3)*sin(th4)-sin(th2)*cos

13、(th4)/2;z3=(-sin(th2)*sin(th3)*cos(th4)*sin(th5)+sin(th2)*cos(th3)*cos(th5)+sin(th2)*cos(th3)+sin(th2)*sin(th3)*sin(th4)/2;z4=(-sin(th2)*cos(th3)*sin(th4)*sin(th5)+cos(th2)*cos(th4)*sin(th5)-sin(th2)*cos(th3)*cos(th4)-cos(th2)*sin(th4)/2;z5=(sin(th2)*cos(th3)*cos(th4)*cos(th5)+cos(th2)*sin(th4)*cos(

14、th5)-sin(th2)*sin(th3)*sin(th5)/2;j=x1,x2,x3,x4,x5;y1,y2,y3,y4,y5;z1,z2,z3,z4,z5;j_1=pinv(j);dx=0;2*pi*0.15*sin(2*pi*t);2*pi*0.15*cos(2*pi*t);dS=j_1*dx;end求解各关节角 的程序（m文件名为fanqiuth.m）如下：t th=ode45(zhaolei,0:0.01:1, pi/2;-pi/3;0;pi/2;0);plot(t,th(:,1),-b.,t,th(:,2),-gx,t,th(:,3),:r*,t,th(:,4),-.mx,t,t

15、h(:,5),-k*);xlabel(Time);title(关节轨迹曲线);由MATLAB画出的关节轨迹曲线如下图所示：其中蓝色曲线表示 ，绿色曲线表示 ，红色曲线表示 ，品红色曲线表示 ，黑色曲线表示 。8.2 利用上面反求的各 用Matlab计算出末端轨迹 编写一个M文件，命名为yuan.m，程序如下： syms th1 th2 th3 th4 th5th1=th(:,1);th2=th(:,2);th3=th(:,3);th4=th(:,4);th5=th(:,5);px=(cos(th1).*cos(th2).*cos(th3).*cos(th4).*sin(th5)-sin(th1

16、).*sin(th3).*cos(th4).*sin(th5)+cos(th1).*sin(th4).*sin(th5)-sin(th2).*sin(th4).*sin(th5)+cos(th1).*cos(th2).*sin(th3).*cos(th5)+sin(th1).*cos(th3).*cos(th5)+sin(th1).*sin(th3).*sin(th4)-cos(th1).*cos(th2).*cos(th3).*sin(th4)+cos(th1).*cos(th4)-sin(th2).*cos(th4)+cos(th1).*cos(th2).*sin(th3)+sin(th1

17、).*cos(th3)+cos(th1)+sin(th1)-sin(th2)./2;py=(sin(th1).*cos(th2).*cos(th3).*cos(th4).*sin(th5)+cos(th1).*sin(th3).*cos(th4).*cos(th5)-sin(th1).*sin(th2).*sin(th4).*sin(th5)+sin(th1).*cos(th2).*sin(th3).*cos(th5)-cos(th1).*cos(th3).*cos(th5)+sin(th1).*cos(th2).*sin(th3)-cos(th1).*cos(th3)-sin(th1).*sin(th2)-cos(th1)-sin(th1).*cos(th2).*cos(th3).*sin(th4)-cos(th1).*sin(th3).*sin(th4)-sin(th1).*sin(th2).*sin(th4)./2