puma560_六自由度机械臂正逆运动学验证（通过解析法得八组逆解）

%% 逆运动学公式 clear; clc; %初始化参数 % syms r11 r12 r13 ; % syms r21 r22 r23 ; % syms r31 r32 r33 ; % syms px py pz ; r11=0.172334957055045; r12=-0.022147364656887; r13=0.984789397188856; r21=-0.760677744837175; r22=-0.638172768441326; r23=0.118763993404418; r31=0.625835466465641; r32=-0.769574565496216; r33=-0.126826484044322; px=27.336927792554377; py=55.621199040016270; pz=4.999999999999993; % T=[ r11 r12 r13 px; % r21 r22 r23 py; % r31 r32 r33 pz; % 0 0 0 1 ]; a2=50; a3=10; d3=20; d4=50; % 1关节 q11 = atan2(py , px)-atan2(d3 , sqrt(px^2+py^2-d3^2) ); q12 = atan2(py , px)-atan2(d3 , -sqrt(px^2+py^2-d3^2) ); % 3关节 K = ( px^2 + py^2 + pz^2 - a2^2 -a3^2 -d3^2 -d4^2 )/(2*a2); q31 = atan2(a3,d4)-atan2(K,sqrt(a3^2+d4^2-K^2)); q32 = atan2(a3,d4)-atan2(K,-sqrt(a3^2+d4^2-K^2)); % 2关节 % q11 q31 s2301 = ((-a3-a2*cos(q31))*pz-(cos(q11)*px+sin(q11)*py)*(d4-a2*sin(q31)))/(pz^2+(cos(q11)*px+sin(q11)*py)^2); c2301 = ((a2*sin(q31)-d4)*pz+(a3+a2*cos(q31))*(cos(q11)*px+sin(q11)*py)) /(pz^2+(cos(q11)*px+sin(q11)*py)^2); m1 = atan2(s2301,c2301); % q11 q32 s2302 = ((-a3-a2*cos(q32))*pz-(cos(q11)*px+sin(q11)*py)*(d4-a2*sin(q32)))/(pz^2+(cos(q11)*px+sin(q11)*py)^2); c2302 = ((a2*sin(q32)-d4)*pz+(a3+a2*cos(q32))*(cos(q11)*px+sin(q11)*py))/(pz^2+(cos(q11)*px+sin(q11)*py)^2); m2 = atan2(s2302,c2302); % q12 q31 s2303 = ((-a3-a2*cos(q31))*pz-(cos(q12)*px+sin(q12)*py)*(d4-a2*sin(q31)))/(pz^2+(cos(q12)*px+sin(q12)*py)^2); c2303 = ((a2*sin(q31)-d4)*pz+(a3+a2*cos(q31))*(cos(q12)*px+sin(q12)*py))/(pz^2+(cos(q12)*px+sin(q12)*py)^2); m3 = atan2(s2303,c2303); % q12 q32 s2304 = ((-a3-a2*cos(q32))*pz-(cos(q12)*px+sin(q12)*py)*(d4-a2*sin(q32)))/(pz^2+(cos(q12)*px+sin(q12)*py)^2); c2304 = ((a2*sin(q32)-d4)*pz+(a3+a2*cos(q32))*(cos(q12)*px+sin(q12)*py))/(pz^2+(cos(q12)*px+sin(q12)*py)^2); m4 = atan2(s2304,c2304); q21 = m1 - q31; q22 = m2 - q32; q23 = m3 - q31; q24 = m4 - q32; % 4关节 q41 = atan2( -r13*sin(q11)+r23*cos(q11) , -r13*cos(q11)*c2301-r23*sin(q11)*c2301+r33*s2301 ); q42 = atan2( -r13*sin(q11)+r23*cos(q11) , -r13*cos(q11)*c2302-r23*sin(q11)*c2302+r33*s2302 ); q43 = atan2( -r13*sin(q12)+r23*cos(q12) , -r13*cos(q12)*c2303-r23*sin(q12)*c2303+r33*s2303 ); q44 = atan2( -r13*sin(q12)+r23*cos(q12) , -r13*cos(q12)*c2304-r23*sin(q12)*c2304+r33*s2304 ); % 5关节 sin(q5)不等于0 q51 = atan2( -r13*(cos(q11)*c2301*cos(q41)+sin(q11)*sin(q41))-r23*(sin(q11)*c2301*cos(q41)-cos(q11)*sin(q41))+r33*(s2301*cos(q41)) , -r13*cos(q11)*s2301-r23*sin(q11)*s2301-r33*c2301 ); q52 = atan2( -r13*(cos(q11)*c2302*cos(q42)+sin(q11)*sin(q42))-r23*(sin(q11)*c2302*cos(q42)-cos(q11)*sin(q42))+r33*(s2302*cos(q42)) , -r13*cos(q11)*s2302-r23*sin(q11)*s2302-r33*c2302 ); q53 = atan2( -r13*(cos(q12)*c2303*cos(q43)+sin(q12)*sin(q43))-r23*(sin(q12)*c2303*cos(q43)-cos(q12)*sin(q43))+r33*(s2303*cos(q43)) , -r13*cos(q12)*s2303-r23*sin(q12)*s2303-r33*c2303 ); q54 = atan2( -r13*(cos(q12)*c2304*cos(q44)+sin(q12)*sin(q44))-r23*(sin(q12)*c2304*cos(q44)-cos(q12)*sin(q42))+r33*(s2304*cos(q44)) , -r13*cos(q12)*s2304-r23*sin(q12)*s2304-r33*c2304 ); % 6关节 q61 = atan2( -r11*(cos(q11)*c2301*sin(q41)-sin(q11)*cos(q41))-r21*(sin(q11)*c2301*sin(q41)+cos(q11)*cos(q41))+r31*s2301*sin(q41) , r11*( (cos(q11)*c2301*cos(q41)+sin(q11)*sin(q41))*cos(q51)-cos(q11)*s2301*sin(q51) )+r21*( (sin(q11)*c2301*cos(q41)-cos(q11)*sin(q41))*cos(q51)-sin(q11)*s2301*sin(q51) )-r31*(s2301*cos(q41)*cos(q51)+c2301*sin(q51)) ); q62 = atan2( -r11*(cos(q11)*c2302*sin(q42)-sin(q11)*cos(q42))-r21*(sin(q11)*c2302*sin(q42)+cos(q11)*cos(q42))+r31*s2302*sin(q42) , r11*( (cos(q11)*c2302*cos(q42)+sin(q11)*sin(q42))*cos(q52)-cos(q11)*s2302*sin(q52) )+r21*( (sin(q11)*c2302*cos(q42)-cos(q11)*sin(q42))*cos(q52)-sin(q11)*s2302*sin(q52) )-r31*(s2302*cos(q42)*cos(q52)+c2302*sin(q52)) ); q63 = atan2( -r11*(cos(q12)*c2303*sin(q43)-sin(q12)*cos(q43))-r21*(sin(q12)*c2303*sin(q43)+cos(q12)*cos(q43))+r31*s2303*sin(q43) , r11*( (cos(q12)*c2303*cos(q43)+sin(q12)*sin(q43))*cos(q53)-cos(q12)*s2303*sin(q53) )+r21*( (sin(q12)*c2303*cos(q43)-cos(q12)*sin(q43))*cos(q53)-sin(q12)*s2303*sin(q53) )-r31*(s2303*cos(q43)*cos(q53)+c2303*sin(q53)) ); q64 = atan2( -r11*(cos(q12)*c2304*sin(q44)-sin(q12)*cos(q44))-r21*(sin(q12)*c2304*sin(q44)+cos(q12)*cos(q44))+r31*s2304*sin(q44) , r11*( (cos(q12)*c2304*cos(q44)+sin(q12)*sin(q44))*cos(q54)-cos(q12)*s2304*sin(q54) )+r21*( (sin(q12)*c2304*cos(q44)-cos(q12)*sin(q44))*cos(q54)-sin(q12)*s2304*sin(q54) )-r31*(s2304*cos(q44)*cos(q54)+c2304*sin(q54)) ); Q1 = [ q11 q21 q31 q41 q51 q61 ] Q2 = [ q11 q22 q32 q42 q52 q62 ] Q3 = [ q12 q23 q31 q43 q53 q63 ] Q4 = [ q12 q24 q32 q44 q54 q64 ] Q5 = [ q11 q21 q31 q41+pi -q51 q61+pi ] Q6 = [ q11 q22 q32 q42+pi -q52 q62+pi ] Q7 = [ q12 q23 q31 q43+pi -q53 q63+pi ] Q8 = [ q12 q24 q32 q44+pi -q54 q64+pi ]