tanq2=(L3*sinq3-L1*sinq1)/(L4+L3*cosq3-L1sinq1)
求q5、q6:
先有xe=L4+L3*cosq3+L2'*cos(q2-a)(式3
ye=L3*sinq3+L2'*sin(q2-a)
tanβ=(yg-ye)/(xg-xe)
cosγ=[(xe-xg)²+(ye-yg)²+L5²-L6]/{2*L5*sqrt[(xe-xg)²+(te-yg)²]}
则q5'=β-γ
q5=q5'
tanq6=(ye+L5*sinq5'-yg)/(xe+L5*cosq5'-xg)
2)角速度分析
分别将(式2)(式3)对时间求一次导数,可得
-L2*w2*sinq2+L3*w3*sinq3=L1*w1*sinq1
L2*w2*cosq2-L3*w3*cosq3=-L1*w1*cosq1
-w2(L2*sinq2+L2'sin(q2-a))+L5*w5*sinq5+L6*w6*sinq6=L1*w1*sinq1w2*(L2*cosq2+L2'*cos(q2-a))-L5*w5*cosq5-L6*w6*cosq6=-L1*w1*cosq1
…………………………………………………………式4
vex=-L3*w3*sinq3-L2'*w2*sin(q2-a)
vey=L3*w3*cosq3+L2'*w2*cos(q2-a)………………(式5)
解之可得w2,w3.w5,w6,vex,vey.将(式4)(式5)写成矩阵形式
-L2*sinq2L3*sinq300w2L1*sinq1
L2*cosq2-L3*cosq300w3=w1-L1*cosq1
-L2*sinq2-L2'*sin(q2-a)0L5*sinq5L6*sinq6w5L1*sinq1L2*cosq2+L2'*cos(q2-a)0L5*cosq5-L6*cosq6w6-L1*cosq1
……………………………………………………………………(式6)
E点速度
vexw3-L3*sinq3-L2*sin(q2-a)
veyw2L3*cosq3L2'cos(q2-a)(式7)
采用高斯消去法可求解(式6)可解得角速度w2,w3,w5,w6;将求解结果带入(式7)可求得vex,vey.
3)角加速度分析
分别将(式2)(式3)对时间取二次导数,可得加速度关系
-L2*sinq2L3*sinq300a2
L2*cosq2-L3*cosq300a3
-L2*sinq2-L2'*sin(q2-a)0L5*sinq5L6*sinq6a5
L2*cosq2+L2'*cos(q2-a)0-L5*cosq5L6*cosq6a6
-w2*L2*cosq2w3*L3*cosq300w2L1*w1*cosq1
=-w2*L2*sinq2w3*L3*sinq300w3+w1L1*w1*sinq1
w2*L2*cosq2+w2*L2'*cos(q2-a)0w5*L5*cosq5w6*L6*cosq6w5L1*w1*cosq1
w2*L2*sinq2+w2*L2'*sin(q2-a)0w5*L5*sinq5w6*L6*sinq6w6L1*w1*sinq1
……………………………………………………………………(式8)
E点的加速度
aex-L2'*sin(q2-a)-L3*sinq3a2L2'*cos(q2-a)L3*cosq3w2²
AeyL2'*cos(q2-a)L3*cosq3a3L2'*sin(q2-a)L3*sinq3w3²
采用高斯消去法可求解(式8)可解得角加速度α2,α3,α5,α6;将求解结果代入(式9)可求得aex,aey.
三、程序流程图
四、计算源程序
#include
#include
#include
#definePI3.1415926
#defineN4
voidSolutionangle(double[18],double);/*矢量法求角位移*/
voidSolutionspeed(double[N][N],double[N],double[18],double);/*角速度求解*/
voidSolutionacceleration(double[N][N],double[N][N],double[N],double[18]);/*角加速度求解*/
voidGaussianE(double[N][N],double[N],double[N]);/*高斯消去*/
voidFoundmatrixA(double[18],double[N][N]);/*创建系数矩阵A*/
voidFoundmatrixB(double[18],double,double[N]);/*创建系数矩阵B*/
voidFoundmatrixDA(double[18],double[N][N]);/*创建矩阵DA*/
voidFoundmatrixDB(double[18],double,double[N]);/*创建矩阵DB*/
/*定义全局变量*/
doublel1=26.5,l2=75,l3=59,l4=87.5,l5=48.4,l6=39.0;
doublel2g=65.0,xg=153.5,yg=41.7,inang=60*PI/180,as1=1.0;
/*主函数*/
voidmain()
{
inti,j;
FILE*fp;
doubleshuju[36][18];
doublepsvalue[18],a[N][N],da[N][N],b[N],db[N],ang1;
printf("PleaseentertheL3:
");
scanf("%lf",&l3);
/*建立文件,并制表头*/
if((fp=fopen("数据.txt","w"))==NULL)
{
printf("Cann'topenthisfile.\n");
exit(0);
}
fprintf(fp,"\nTheKinematicParametersofPoint5\n");
fprintf(fp,"ang2ang3ang5ang6");
fprintf(fp,"as2as3as5as6");
fprintf(fp,"aas2aas3aas5aas6");
fprintf(fp,"xeyevexveyaexaey\n");
/*计算数据并写入文件*/
for(i=0;i<36;i++)
{
ang1=i*PI/18;
Solutionangle(psvalue,ang1);
FoundmatrixB(psvalue,ang1,b);
FoundmatrixA(psvalue,a);
Solutionspeed(a,b,psvalue,ang1);
FoundmatrixDA(psvalue,da);
FoundmatrixDB(psvalue,ang1,db);
Solutionacceleration(a,da,db,psvalue);
for(j=0;j<4;j++)
{shuju[i][j]=psvalue[j]*180/PI;}
for(j=4;j<18;j++)
{shuju[i][j]=psvalue[j];}
fprintf(fp,"\n");
for(j=0;j<18;j++)
fprintf(fp,"%12.3f",shuju[i][j]);
}
fclose(fp);
/*输出数据*/
for(i=0;i<36;i++)
{
ang1=i*PI/18;
printf("\n输出ang1=%d时的求解\n",i*10);
printf("angleangspeedangacceleration:
\n");
for(j=0;j<4;j++)
printf("%lf\t",shuju[i][j]);
printf("\n");
for(j=4;j<8;j++)
printf("%lf\t",shuju[i][j]);
printf("\n");
for(j=8;j<12;j++)
printf("%lf\t",shuju[i][j]);
printf("\n");
for(j=12;j<18;j++)
printf("%lf\t",shuju[i][j]);
printf("\n");
}
}
/*矢量法求角位移*/
voidSolutionangle(doublevalue[18],doubleang1)
{
doublexe,ye,A,B,C,phi,alpha,csn,ang5g,d2,d,ang2,ang3,ang5,ang6;
A=2*l1*l3*sin(ang1);
B=2*l3*(l1*cos(ang1)-l4);
C=l2*l2-l1*l1-l3*l3-l4*l4+2*l1*l4*cos(ang1);
ang3=2*atan((A+sqrt(A*A+B*B-C*C))/(B-C));
if(ang3<0)/*限定ang3大小*/
{ang3=2*atan((A-sqrt(A*A+B*B-C*C))/(B-C));}
ang2=asin((l3*sin(ang3)-l1*sin(ang1))/l2);
xe=l4+l3*cos(ang3)+l2g*cos(ang2-inang);
ye=l3*sin(ang3)+l2g*sin(ang2-inang);
phi=atan2((yg-ye),(xg-xe));
d2=(yg-ye)*(yg-ye)+(xg-xe)*(xg-xe);
d=sqrt(d2);
csn=(l5*l5+d2-l6*l6)/(2.0*l5*d);
alpha=atan2(sqrt(1.0-csn*csn),csn);
ang5g=phi-alpha;
ang5=ang5g-PI;
ang6=atan2(ye+l5*sin(ang5g)-yg,xe+l5*cos(ang5g)-xg);
value[0]=ang2;value[1]=ang3;value[2]=ang5;value[3]=ang6;
value[12]=xe;value[13]=ye;
/*限定角度大小*/
for(inti=0;i<4;i++)
{
while(value[i]>2*PI)
value[i]-=2*PI;
while(value[i]<0)
value[i]+=2*PI;
}
}
/*角速度求解*/
voidSolutionspeed(doublea2[N][N],doubleb2[N],doublevalue[18],doubleang1)
{
doubleang2,ang3;
ang2=value[0];ang3=value[1];
doublep2[N];
GaussianE(a2,b2,p2);
value[4]=p2[0];
value[5]=p2[1];
value[6]=p2[2];
value[7]=p2[3];
value[14]=-l3*value[5]*sin(ang3)-l2g*value[4]*sin(ang2-inang);
value[15]=l3*value[5]*cos(ang3)+l2g*value[4]*cos(ang2-inang);
}
/*角加速度求解*/
voidSolutionacceleration(doublea3[N][N],doubleda3[N][N],doubledb3[N],doublevalue[18])
{
inti,j;
doubleang2,ang3;
ang2=value[0];ang3=value[1];
doublebk[N]={0};
doublep3[N];
for(i=0;i{
for(j=0;j{
bk[i]+=-da3[i][j]*value[4+j];
}
bk[i]+=db3[i]*as1;
}
GaussianE(a3,bk,p3);
value[8]=p3[0];
value[9]=p3[1];
value[10]=p3[2];
value[11]=p3[3];
value[16]=-l3*value[9]*sin(ang3)-l3*value[5]*value[5]*cos(ang3)-l2g*value[8]*sin(ang2-inang)-l2g*value[4]*value[4]*cos(ang2-inang);
value[17]=l3*value[9]*cos(ang3)-l3*value[5]*value[5]*sin(ang3)+l2g*value[8]*cos(ang2-inang)-l2g*value[4]*value[4]*sin(ang2-inang);
}
/*高斯消去法解矩阵方程*/
voidGaussianE(doublea4[N][N],doubleb4[N],doublep4[N])
{
inti,j,k;
doublea4g[N][N],b4g[N],t;
for(i=0;ifor(j=0;ja4g[i][j]=a4[i][j];
for(i=0;ib4g[i]=b4[i];
/*使主对角线上的值尽可能大*/
if(a4g[0][0]a4g[1][1])
{
for(j=0;j{t=a4g[0][j];a4g[0][j]=a4g[1][j];a4g[1][j]=t;}
t=b4g[0];b4g[0]=b4g[1];b4g[1]=t;
}
if(a4g[2][2]a4g[3][3])
{
for(j=0;j{t=a4g[2][j];a4g[2][j]=a4g[3][j];a4g[3][j]=t;}
t=b4g[2];b4g[2]=b4g[1];b4g[3]=t;
}
/*初等行变换*/
for(j=0;jfor(i=0;i{
if(i!
=j)
{
for(k=0;kif(k!
=j)
{a4g[i][k]-=a4g[i][j]/a4g[j][j]*a4g[j][k];}
b4g[i]-=b4g[j]*a4g[i][j]/a4g[j][j];
a4g[i][j]=0;
}
}
for(i=0;ib4g[i]/=a4g[i][i];
p4[0]=b4g[0];
p4[1]=b4g[1];
p4[2]=b4g[2];
p4[3]=b4g[3];
}
/*创建系数矩阵A*/
voidFoundmatrixA(doublevalue5[18],doublea5[N][N])
{
doubleang2,ang3,ang5,ang6;
ang2=value5[0];ang3=value5[1];ang5=value5[2];ang6=value5[3];
a5[0][0]=-l2*sin(ang2);a5[0][1]=l3*sin(ang3);
a5[1][0]=l2*cos(ang2);a5[1][1]=-l3*cos(ang3);
a5[2][0]=-l2*sin(ang2)-l2g*sin(ang2-inang);
a5[2][2]=l5*sin(ang5);a5[2][3]=l6*sin(ang6);
a5[3][0]=l2*cos(ang2)+l2g*cos(ang2-inang);
a5[3][2]=-l5*cos(ang5);a5[3][3]=-l6*cos(ang6);
a5[0][2]=a5[0][3]=a5[1][2]=a5[1][3]=a5[2][1]=a5[3][1]=0;
}
/*创建系数矩阵B*/
voidFoundmatrixB(doublevalue6[18],doubleang1,doubleb6[N])
{
b6[0]=b6[2]=l1*sin(ang1)*as1;
b6[1]=b6[3]=-l1*cos(ang1)*as1;
}
/*创建矩阵DA*/
voidFoundmatrixDA(doublevalue7[18],doubleda7[N][N])
{
doubleang2,ang3,ang5,ang6,as2,as3,as5,as6;
ang2=value7[0];ang3=value7[1];ang5=value7[2];ang6=value7[3];
as2=value7[4];as3=value7[5];as5=value7[6];as6=value7[7];
da7[0][0]=-l2*as2*cos(ang2);da7[0][1]=l3*as3*cos(ang3);
da7[1][0]=-l2*as2*sin(ang2);da7[1][1]=l3*as3*sin(ang3);
da7[2][0]=as2*(-l2*cos(ang2)-l2g*cos(ang2-inang));
da7[2][2]=as5*l5*cos(ang5);da7[2][3]=as6*l6*cos(ang6);
da7[3][0]=as2*(-l2*sin(ang2)-l2g*sin(ang2-inang));
da7[3][2]=as5*l5*sin(ang5);da7[3][3]=as6*l6*sin(ang6);
da7[0][2]=da7[0][3]=da7[1][2]=da7[1][3]=da7[2][1]=da7[3][1]=0;
}
/*创建矩阵DB*/
voidFoundmatrixDB(doublevalue8[18],doubleang1,doubledb8[N])
{
db8[0]=db8[2]=l1*as1*cos(ang1);
db8[1]=db8[3]=l1*as1*sin(ang1);
}
5、计算结果数据
转角
ang2
ang3
ang5
ang6
as2
as3
as5
as6
aas2
aas3
0
50.135
102.656
117.179
280.669
-0.434
-0.434
1.858
2.614
-0.14
0.52
10
45.726
98.819
143.16
313.171
-0.442
-0.328
3.141
3.628
0.05
0.685
20
41.397
96.166
169.094
340.63
-0.42
-0.201
3.132
2.454
0.189
0.758
30
37.388
94.822
175.957
343.137
-0.379
-0.069
1.098
-0.151
0.266
0.743
40
33.837
94.767
174.965
336.081
-0.33
0.055
0.73
-0.607
0.291
0.66