哈工大机器人大作业

一、运动学正解程序及结果

1、程序：

syms x1 x2 x3 x4 x5 x6 d1 d2 d4 a2 a3 x d a

Rx=[1 0 0 0;0 cos(x) -sin(x) 0;0 sin(x) cos(x) 0;0 0 0 1]; Rz=[cos(x) -sin(x) 0 0;sin(x) cos(x) 0 0;0 0 1 0;0 0 0 1]; Tx=[1 0 0 a;0 1 0 0;0 0 1 0;0 0 0 1]; Tz=[1 0 0 0;0 1 0 0;0 0 1 d;0 0 0 1]; t=pi/180;

y1=90;y2=-90;y3=-90;

T01=subs(Rz,x,x1)*subs(Tz,d,d1)*subs(Rx,x,y1*t); T12=subs(Rz,x,x2)*subs(Tz,d,d2)*subs(Tx,a,a2); T23=subs(Rz,x,x3)*subs(Tx,a,a3)*subs(Rx,x,y3*t); T34=subs(Rz,x,x4)*subs(Tz,d,d4)*subs(Rx,x,y4*t); T45=subs(Rz,x,x5)*subs(Rx,x,90); T=T01*T12*T23*T34*T45;

t=subs(T,{y1,y3,y4,y5},[pi/2,-pi/2,-pi/2,pi/2]); t= simplify(t);

nx=t(1,1);ny=t(2,1);nz=t(3,1); ox=t(1,2);oy=t(2,2);oz=t(3,2); ax=t(1,3);ay=t(2,3);az=t(3,3); px=t(1,4);py=t(2,4);pz=t(3,4);

结果：

Nx=sin(x2 + x3)*cos(x1)*sin(x5) - cos(x5)*sin(x1)*sin(x4)+cos(x1)*cos(x2)*cos(x3)*cos(x4)*cos(x5) - cos(x1)*cos(x4)*cos(x5)*sin(x2)*sin(x3)

Ny=cos(x1)*cos(x5)*sin(x4) + sin(x2 + x3)*sin(x1)*sin(x5)+cos(x2)*cos(x3)*cos(x4)*cos(x5)*sin(x1) - cos(x4)*cos(x5)*sin(x1)*sin(x2)*sin(x3)

Nz=sin(x2 + x3)*cos(x4)*cos(x5) - cos(x2 + x3)*sin(x5)

Ox=sin(x4)*(cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3)) - cos(x4)*sin(x1)

Oy=cos(x1)*cos(x4) - sin(x4)*(cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3))

Oz=-sin(x2 + x3)*sin(x4)

Ax=cos(x1)*cos(x2)*cos(x3)*cos(x4)*sin(x5) - sin(x2 + x3)*cos(x1)*cos(x5) - sin(x1)*sin(x4)*sin(x5) - cos(x1)*cos(x4)*sin(x2)*sin(x3)*sin(x5)

Ay=cos(x1)*sin(x4)*sin(x5) - sin(x2 + x3)*cos(x5)*sin(x1) + cos(x2)*cos(x3)*cos(x4)*sin(x1)*sin(x5) - cos(x4)*sin(x1)*sin(x2)*sin(x3)*sin(x5)

Az=cos(x2 + x3)*cos(x5) + sin(x2 + x3)*cos(x4)*sin(x5)

Px=d2*sin(x1) - d4*(cos(x1)*cos(x2)*sin(x3) + cos(x1)*cos(x3)*sin(x2)) + a2*cos(x1)*cos(x2) + a3*cos(x1)*cos(x2)*cos(x3) - a3*cos(x1)*sin(x2)*sin(x3)

Py=a2*cos(x2)*sin(x1) - d2*cos(x1) - d4*(cos(x2)*sin(x1)*sin(x3) + cos(x3)*sin(x1)*sin(x2)) + a3*cos(x2)*cos(x3)*sin(x1) - a3*sin(x1)*sin(x2)*sin(x3) Pz=d1 + d4*cos(x2 + x3) + a3*sin(x2 + x3) + a2*sin(x2)

二、绘制工作空间 （一）正解 1、程序

syms x1 x2 x3 x4 x5 x6 d1 d2 d4 a2 a3 x d a

Rx=[1 0 0 0;0 cos(x) -sin(x) 0;0 sin(x) cos(x) 0;0 0 0 1]; Rz=[cos(x) -sin(x) 0 0;sin(x) cos(x) 0 0;0 0 1 0;0 0 0 1]; Tx=[1 0 0 a;0 1 0 0;0 0 1 0;0 0 0 1]; Tz=[1 0 0 0;0 1 0 0;0 0 1 d;0 0 0 1]; t=pi/180;

T01=subs(Rz,x,x1)*subs(Tz,d,d1)*subs(Rx,x,90*t); T12=subs(Rz,x,x2)*subs(Tz,d,d2)*subs(Tx,a,a2); T23=subs(Rz,x,x3)*subs(Tx,a,a3)*subs(Rx,x,-90*t); T34=subs(Rz,x,x4)*subs(Tz,d,d4)*subs(Rx,x,-90*t); T45=subs(Rz,x,x5)*subs(Rx,x,90); T=T01*T12*T23*T34*T45; Px=T(1,4); Py=T(2,4); Pz=T(3,4); k=1;

for th1=-160*t:10*t:160*t for th2=-225*t:10*t:45*t for th3=-45*t:10*t:225*t

px(k)=subs(Px,{x1,x2,x3,a2,a3,d1,d2,d4},[th1,th2,th3,431.8,20.32,0,149.09,433.07]);

py(k)=subs(Py,{x1,x2,x3,a2,a3,d1,d2,d4},[th1,th2,th3,431.8,20.32,0,149.09,433.07]);

pz(k)=subs(Pz,{x1,x2,x3,a2,a3,d1,d2,d4},[th1,th2,th3,431.8,20.32,0,149.09,433.07]);

k=k+1; end end

end

plot3(px,py,pz,'.'); grid on

2、结果

俯视图

3、总结：

1).从图形看出，PUMA工作空间是一个中间为圆柱空心的椭球区域。 2).编程运算过程中，程序效率非常低，步长取10°的时候，计算23328个点，计算时间是一晚。后来发现计算效率不仅与取点的多少有关系，更关键的是程序用的是符号运算，每一次循环都是一次符号运算对数值与运算的转换，这个过程占用的时间时是主要的。如果要优化程序，应该先用符号运算得出表达式，再将表达式转为数值计算，其速度将得到大幅度提升。

（二）逆解

此处逆解程序是直接运用课下推出的角度表达式(没有用程序推导表达式的过程)，用程序找出运动空间边界点得出，所以程序复杂程度与正解差不多。 1、程序： clear

d2=149.09; d4=433.07; a2=431.8; a3=-20.32; i=1;

for Px=-1000:50:1000 for Py=-1000:50:1000 for Pz=-1000:50:1000 i=i+1;

k=(Px^2+Py^2+Pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2); th1(i)=atan(Py/Px)-atan(d2/sqrt(Px^2+Py^2-d2^2));

th3(i)=atan(a3/d4)-atan(k/sqrt(a3^2+d4^2-k^2))； th23(i)=atan(-(a3+a2*cos(th3(i)))*Pz+(cos(th1(i))*Px+sin(th1(i))*Py)*(a2*sin(th3(i))-d4))/((-d4+a2*sin(th3(i)))*Pz+(cos(th1(i))*Px+sin(th1(i))*Py)*(a2*cos(th3(i))+a3));

%此处为程序关键，用抑或逻辑运算找到空间边界点 If

xor(isreal(th1(i))&&isreal(th23(i))&&isreal(th3(i)),isreal(th1(i-1))&&isreal(th23(i-1))&&isreal(th3(i-1))) x(i)=Px; y(i)=Py; z(i)=Pz; end

end end end

plot3(x,y,z,'.'); hold on; grid on;

2、结果

逆解俯视图

3、总结

从运行过程可以看出，逆解求解需要计算的点数比正解多很多，所以求解速度“应该”比正解慢很多。但是由于此处是直接引用公式（没有公式推导过程），所以可以直接用数值计算，这样实际计算速度就高了很多。

从结果可以看出，逆解得出的图像点更均匀，但是从俯视图也能发现，中间本来的圆柱形空心，在此类似正方形，这是因为逆解循环取点是对称的、有间隔的，在有限的取点数条件下，不能很好的表现圆形的曲线。

本文来源：https://www.bwwdw.com/article/5n06.html