stewar.m

并联机器人源程序

clear
ra=0.4;
rb=0.2;
h=0.3;
alfaA=20; 
alfaB=14;
fai=[ alfaA/2 120-alfaA/2 120+alfaA/2 -(120+alfaA/2)  -(120-alfaA/2) -alfaA/2];
beta=[alfaB/2 120-alfaB/2 120+alfaB/2 -(120+alfaB/2) -(120-alfaB/2) -alfaB/2];  %单位为度
%各球铰点矢量表示
for i=1:6;;
b(i)=beta(i)*pi/180;
i=i+1;
end
for i=1:6;;
a(i)=fai(i)*pi/180;
i=i+1;
end
B1=[rb*cos(b(1)),rb*sin(b(1)),h]';B2=[rb*cos(b(2)),rb*sin(b(2)),h]';
B3=[rb*cos(b(3)),rb*sin(b(3)),h]';B4=[rb*cos(b(4)),rb*sin(b(4)),h]';
B5=[rb*cos(b(5)),rb*sin(b(5)),h]';B6=[rb*cos(b(6)),rb*sin(b(6)),h]';
A1=[ra*cos(a(1)),ra*sin(a(1)),0]';A2=[ra*cos(a(2)),ra*sin(a(2)),0]';
A3=[ra*cos(a(3)),ra*sin(a(3)),0]';A4=[ra*cos(a(4)),ra*sin(a(4)),0]';
A5=[ra*cos(a(5)),ra*sin(a(5)),0]';A6=[ra*cos(a(6)),ra*sin(a(6)),0]';
%一阶静力影响系数矩阵
G=[(B1-A1)/norm(B1-A1),(B2-A2)/norm(B2-A2),(B3-A3)/norm(B3-A3),(B4-A4)/norm(B4-A4),(B5-A5)/norm(B5-A5),(B6-A6)/norm(B6-A6);
cross(A1,B1)/norm(B1-A1),cross(A2,B2)/norm(B2-A2),cross(A3,B3)/norm(B3-A3),cross(A4,B4)/norm(B4-A4),cross(A5,B5)/norm(B5-A5),cross(A6,B6)/norm(B6-A6)];
C=inv(G);
F=[0;0;1;0;0;0];
f=C*F;