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
升级会员 