Adept机器人运动学反解验证matlab编程

nx=1; ny=0; nz=0; ox=0; oy=1; oz=0; ax=0; ay=0; az=1; a1=10; a2=10;

px=-2*pi:0.1:2*pi;%预设轨迹 py=px.*0;

pz=10.*sin(px); figure

plot3(px,py,pz),grid on

xlabel('px'),ylabel('py'),zlabel('pz') title('预设轨迹曲线')

t=(px.^2+py.^2-a1.^2-a2.^2)/(2.*a1.*a2);%反解表达式 rhu=sqrt(px.^2+py.^2);

theta1=atan2(sqrt(1-((a1+a2.*t)/rhu).^2),(a1+a2.*t)/rhu)+atan2(py,px) theta2=atan2(sqrt(1-t.^2),t)

s4=sin(theta1+theta2).*nx-cos(theta1+theta2).*ny; c4=cos(theta1+theta2).*nx+sin(theta1+theta2).*ny; theta4=atan2(s4,c4) d3=-pz

figure%反解结果的变化图像

subplot(221),plot(px,rad2deg(theta1)),grid on xlabel('x方向位移'),ylabel('\\theta1的值') title('theta1变化')

subplot(222),plot(px,rad2deg(theta2)),grid on xlabel('x方向位移'),ylabel('\\theta2的值') title('theta2变化')

subplot(223),plot(px,rad2deg(theta4)),grid on xlabel('x方向位移'),ylabel('\\theta4的值') title('theta4变化')

subplot(224),plot(px,d3),grid on

xlabel('x方向位移'),ylabel('d3的值') title('d3变化')

x=a1.*cos(theta1)+a2.*cos(theta1+theta2);%将反解结果代入正解验证 y=a1.*sin(theta1)+a2.*sin(theta1+theta2); z=-d3; figure

plot3(x,y,z),grid on

xlabel('x'),ylabel('y'),zlabel('z') title('验证结果曲线')

则由验证结果图像与预设轨迹图像一致可知，反解表达式计算正确。