matlab程序-运动学.zip_3自由度并联机器人运动学反解_matlab运动反解_并联机器人_运动学_运动学 Matlab

syms Fc Fg Fa1 Fa2 Fa3 Fa4 F11 F4 F13 F21 F5 F23 F31 F6 F33 Fcm R Q M q1 q2 q3 me a ax ay az Fz1 Fz2 Fz3; Fa1=[F11;F4;-F13]; Fa2=[-sin(pi/6)*F21+cos(pi/6)*F5;-cos(pi/6)*F21-sin(pi/6)*F5;-F23]; Fa3=[-sin(pi/6)*F31-cos(pi/6)*F6;cos(pi/6)*F31-sin(pi/6)*F6;-F33]; Fcm=Fa1+Fa2+Fa3 Fcm = F11 - F21/2 - F31/2 + (3^(1/2)*F5)/2 - (3^(1/2)*F6)/2 F4 - F5/2 - F6/2 - (3^(1/2)*F21)/2 + (3^(1/2)*F31)/2 - F13 - F23 - F33 R=[cos(q2)*cos(q3),sin(q1)*sin(q2)*sin(q3)-cos(q1)*sin(q3),sin(q1)*sin(q3)+cos(q1)*sin(q2)*cos(q3);cos(q2)*sin(q3),cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3),cos(q1)*sin(q2)*sin(q3)-sin(q1)*cos(q3);-sin(q2),sin(q1)*cos(q2),cos(q1)*cos(q2)]; g=9.80665; Q=-M*g*[0;0;1]; Fc=R*Fcm+Q Fc = (cos(q1)*sin(q3) - sin(q1)*sin(q2)*sin(q3))*(F5/2 - F4 + F6/2 + (3^(1/2)*F21)/2 - (3^(1/2)*F31)/2) - (sin(q1)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(F13 + F23 + F33) - cos(q2)*cos(q3)*(F21/2 - F11 + F31/2 - (3^(1/2)*F5)/2 + (3^(1/2)*F6)/2) (cos(q3)*sin(q1) - cos(q1)*sin(q2)*sin(q3))*(F13 + F23 + F33) - (cos(q1)*cos(q3) + sin(q1)*sin(q2)*sin(q3))*(F5/2 - F4 + F6/2 + (3^(1/2)*F21)/2 - (3^(1/2)*F31)/2) - cos(q2)*sin(q3)*(F21/2 - F11 + F31/2 - (3^(1/2)*F5)/2 + (3^(1/2)*F6)/2) sin(q2)*(F21/2 - F11 + F31/2 - (3^(1/2)*F5)/2 + (3^(1/2)*F6)/2) - (5520653160719109*M)/562949953421312 - cos(q1)*cos(q2)*(F13 + F23 + F33) - cos(q2)*sin(q1)*(F5/2 - F4 + F6/2 + (3^(1/2)*F21)/2 - (3^(1/2)*F31)/2) Fg=me*a; me=-M*[1,0,0;0,1,0;0,0,1]; a=[ax;ay;az] a = ax ay az Fg Fg = a*me Fcm = F11 - F21/2 - F31/2 + (3^(1/2)*F5)/2 - (3^(1/2)*F6)/2 F4 - F5/2 - F6/2 - (3^(1/2)*F21)/2 + (3^(1/2)*F31)/2 - F13 - F23 - F33 syms Fc Fg Fa1 Fa2 Fa3 Fa4 F11 F4 F13 F21 F5 F23 F31 F6 F33 Fcm R Q M q1 q2 q3 me a ax ay az Fz1 Fz2 Fz3; Fa1=[F11;F4;-F13]; Fa2=[-sin(pi/6)*F21+cos(pi/6)*F5;-cos(pi/6)*F21-sin(pi/6)*F5;-F23]; Fa3=[-sin(pi/6)*F31-cos(pi/6)*F6;cos(pi/6)*F31-sin(pi/6)*F6;-F33]; Fa4=[Fz1;Fz2;-Fz3]; Fcm=Fa1+Fa2+Fa3+Fa4; R=[cos(q2)*cos(q3),sin(q1)*sin(q2)*sin(q3)-cos(q1)*sin(q3),sin(q1)*sin(q3)+cos(q1)*sin(q2)*cos(q3);cos(q2)*sin(q3),cos(q1)*cos(q3)+sin(q1)*sin(q2)*sin(q3),cos(q1)*sin(q2)*sin(q3)-sin(q1)*cos(q3);-sin(q2),sin(q1)*cos(q2),cos(q1)*cos(q2)]; g=9.80665; a=[ax;ay;az+g]; M=R*Fcm*pinv(a) >>>> syms Ma1 Ma2 Ma3 Ma4 Mc Mt M11 M21 M31 M12 M22 M32 M13 M23 M33 ra1 ra2 ra3 ra1x ra1y ra1z ra2x ra2y ra2z ra3x ra3y ra3z Fa1 Fa2 Fa3 Fa4 F11 F4 F13 F21 F5 F23 F31 F6 F33 Fz1 Fz2 Fz3 I Ixx Ixy Ixz Iyx Iyy Iyz Izx Izy Izz Wm Wm1 Wm2 Wm3 Em Emx Emy Emz; ra1=[ra1x;ra1y;ra1z]; ra2=[ra2x;ra2y;ra2z]; ra3=[ra3x;ra3y;ra3z]; Fa1=[F11;F4;-F13]; Fa2=[-sin(pi/6)*F21+cos(pi/6)*F5;-cos(pi/6)*F21-sin(pi/6)*F5;-F23]; Fa3=[-sin(pi/6)*F31-cos(pi/6)*F6;cos(pi/6)*F31-sin(pi/6)*F6;-F33]; Ma1=ra1.*Fa1 Ma2=ra2.*Fa2 Ma3=ra3.*Fa3 Ma1 = F11*ra1x F4*ra1y -F13*ra1z Ma2 = -ra2x*(F21/2 - (3^(1/2)*F5)/2) -ra2y*(F5/2 + (3^(1/2)*F21)/2) -F23*ra2z Ma3 = -ra3x*(F31/2 + (3^(1/2)*F6)/2) -ra3y*(F6/2 - (3^(1/2)*F31)/2) -F33*ra3z >> Mc=Ma1+Ma2+Ma3 Mc = F11*ra1x - ra2x*(F21/2 - (3^(1/2)*F5)/2) - ra3x*(F31/2 + (3^(1/2)*F6)/2) F4*ra1y - ra2y*(F5/2 + (3^(1/2)*F21)/2) - ra3y*(F6/2 - (3^(1/2)*F31)/2) - F13*ra1z - F23*ra2z - F33*ra3z >> syms I Ixx Ixy Ixz Iyx Iyy Iyz Izx Izy Izz Wm Wm1 Wm2 Wm3 Em Emx Emy Emz H; I=[Ixx Ixy Ixz;Iyx Iyy Iyz;Izx Izy Izz] Wm=[Wm1;Wm2;Wm3] Em=[Emx;Emy;Emz] Mt=I*Wm.*Wm-I*Em H=Wm.*Wm-Em I = [ Ixx, Ixy, Ixz] [ Iyx, Iyy, Iyz] [ Izx, Izy, Izz] Wm = Wm1 Wm2 Wm3 Em = Emx Emy Emz Mt = Wm1*(Ixx*Wm1 + Ixy*Wm2 + Ixz*Wm3) - Emx*Ixx - Emy*Ixy - Emz*Ixz Wm2*(Iyx*Wm1 + Iyy*Wm2 + Iyz*Wm3) - Emx*Iyx - Emy*Iyy - Emz*Iyz Wm3*(Izx*Wm1 + Izy*Wm2 + Izz*Wm3) - Emx*Izx - Emy*Izy - Emz*Izz H = Wm1^2 - Emx Wm2^2 - Emy Wm3^2 - Emz >> Mt=I*H Mt = - Ixx*(- Wm1^2 + Emx) - Ixy*(- Wm2^2 + Emy) - Ixz*(- Wm3^2 + Emz) - Iyx*(- Wm1^2 + Emx) - Iyy*(- Wm2^2 + Emy) - Iyz*(- Wm3^2 + Emz) - Izx*(- Wm1^2 + Emx) - Izy*(- Wm2^2 + Emy) - Izz*(- Wm3^2 + Emz) >> Wm1*(Ixx*Wm1 + Ixy*Wm2 + Ixz*Wm3) - Emx*Ixx - Emy*Ixy - Emz*Ixz Wm2*(Iyx*Wm1 + Iyy*Wm2 + Iyz*Wm3) - Emx*Iyx - Emy*Iyy - Emz*Iyz Wm3*(Izx*Wm1 + Izy*Wm2 + Izz*Wm3) - Emx*Izx - Emy*Izy - Emz*Izz ans = Wm1*(Ixx*Wm1 + Ixy*Wm2 + Ixz*Wm3) - Emx*Ixx - Emy*Ixy - Emz*Ixz ans = Wm2*(Iyx*Wm1 + Iyy*Wm2 + Iyz*Wm3) - Emx*Iyx - Emy*Iyy - Emz*Iyz ans = Wm3*(Izx*Wm1 + Izy*Wm2 + Izz*Wm3) - Emx*Izx - Emy*Izy - Emz*Izz >> Wm1*Wm1 ans = Wm1^2 syms Ma1 Ma2 Ma3 Ma4 Mc Mt M11 M21 M31 M12 M22 M32 M13 M23 M33 ra1 ra2 ra3 ra1x ra1y ra1z ra2x ra2y ra2z ra3x ra3y ra3z Fa1 Fa2 Fa3 Fa4 F11 F4 F13 F21 F5 F23 F31 F6 F33 Fz1 Fz2 Fz3 I Ixx Ixy Ixz Iyx Iyy Iyz Izx Izy Izz Wm Wm1 Wm2 Wm3 Em Emx Emy Emz; ra1=[ra1x;ra1y;ra1z]; ra2=[ra2x;ra2y;ra2z]; ra3=[ra3x;ra3y;ra3z]; Fa1=[F11;F4;-F13]; Fa2=[-sin(pi/6)*F21+cos(pi/6)*F5;-cos(pi/6)*F21-sin(pi/6)*F5;-F23]; Fa3=[-sin(pi/6)*F31-cos(pi/6)*F6;cos(pi/6)*F31-sin(pi/6)*F6;-F33]; Ma1=ra1.*Fa1 Ma2=ra2.*Fa2 Ma3=ra3.*Fa3 Mc=Ma1+Ma2+Ma3 syms I Ixx Ixy Ixz Iyx Iyy Iyz Izx Izy Izz Wm Wm1 Wm2 Wm3 Em Emx Emy Emz H; I=[Ixx Ixy Ixz;Iyx Iyy Iyz;Izx Izy Izz] Wm=[Wm1;Wm2;Wm3] Em=[Emx;Emy;Emz] H=Wm.*Wm-Em Ma1 = F11*ra1x F4*ra1y -F13*ra1z Ma2 = -ra2x*(F21/2 - (3^(1/2)*F5)/2) -ra2y*(F5/2 + (3^(1/2)*F21)/2) -F23*ra2z Ma3 = -ra3x*(F31/2 + (3^(1/2)*F6)/2) -ra3y*(F6/2 - (3^(1/2)*F31)/2) -F33*ra3z Mc = F11*ra1x - ra2x*(F21/2 - (3^(1/2)*F5)/2) - ra3x*(F31/2 + (3^(1/2)*F6)/2) F4*ra1y - ra2y*(F5/2 + (3^(1/2)*F21)/2) - ra3y*(F6/2 - (3^(1/2)*F31)/2) - F13*ra1z - F23*ra2z - F33*ra3z I = [ Ixx, Ixy, Ixz] [ Iyx, Iyy, Iyz] [ Izx, Izy, Izz] Wm = Wm1 Wm2 Wm3 Em = Emx Emy Emz H = Wm1^2 - Emx Wm2^2 - Emy Wm3^2 - Emz >> I=(-Mc)*pinv(H) I = [ -((conj(Emx) - conj(Wm1)^2)*(ra2x*(F21/2 - (3^(1/2)*F5)/2) - F11*ra1x + ra3x*(F31/2 + (3^(1/2)*F6)/2)))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(Emy) - conj(Wm2)^2)*(- Wm2^2 + Emy) + (conj(Emz) - conj(Wm3)^2)*(- Wm3^2 + Emz)), -((conj(Emy) - conj(Wm2)^2)*(ra2x*(F21/2 - (3^(1/2)*F5)/2) - F11*ra1x + ra3x*(F31/2 + (3^(1/2)*F6)/2)))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(Emy) - conj(Wm2)^2)*(- Wm2^2 + Emy) + (conj(Emz) - conj(Wm3)^2)*(- Wm3^2 + Emz)), -((conj(Emz) - conj(Wm3)^2)*(ra2x*(F21/2 - (3^(1/2)*F5)/2) - F11*ra1x + ra3x*(F31/2 + (3^(1/2)*F6)/2)))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(Emy) - conj(Wm2)^2)*(- Wm2^2 + Emy) + (conj(Emz) - conj(Wm3)^2)*(- Wm3^2 + Emz))] [ -((conj(Emx) - conj(Wm1)^2)*(ra2y*(F5/2 + (3^(1/2)*F21)/2) - F4*ra1y + ra3y*(F6/2 - (3^(1/2)*F31)/2)))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(Emy) - conj(Wm2)^2)*(- Wm2^2 + Emy) + (conj(Emz) - conj(Wm3)^2)*(- Wm3^2 + Emz)), -((conj(Emy) - conj(Wm2)^2)*(ra2y*(F5/2 + (3^(1/2)*F21)/2) - F4*ra1y + ra3y*(F6/2 - (3^(1/2)*F31)/2)))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(Emy) - conj(Wm2)^2)*(- Wm2^2 + Emy) + (conj(Emz) - conj(Wm3)^2)*(- Wm3^2 + Emz)), -((conj(Emz) - conj(Wm3)^2)*(ra2y*(F5/2 + (3^(1/2)*F21)/2) - F4*ra1y + ra3y*(F6/2 - (3^(1/2)*F31)/2)))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(Emy) - conj(Wm2)^2)*(- Wm2^2 + Emy) + (conj(Emz) - conj(Wm3)^2)*(- Wm3^2 + Emz))] [ -((conj(Emx) - conj(Wm1)^2)*(F13*ra1z + F23*ra2z + F33*ra3z))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(Emy) - conj(Wm2)^2)*(- Wm2^2 + Emy) + (conj(Emz) - conj(Wm3)^2)*(- Wm3^2 + Emz)), -((conj(Emy) - conj(Wm2)^2)*(F13*ra1z + F23*ra2z + F33*ra3z))/((conj(Emx) - conj(Wm1)^2)*(- Wm1^2 + Emx) + (conj(