3自由度机械臂正逆解matlab.zip

%% 单腿逆运动学模型——肘式 function [q1,q2,q3]=invar_kinamic_z(a,b,c,x,y,z) if z==0 if y>0 q3=pi/2; elseif y<0 q3=-pi/2; else q3=0; end else q3 = atan(y/(-z)); end l=sqrt(x^2+(y-a*sin(q3))^2+(z+a*cos(q3))^2); q2=-acos((b^2+l^2-c^2)/(2*b*l))+asin(x/l); q1=pi/2-acos((b^2+c^2-l^2)/(2*b*c)); end