計算動力學作業 樣本

Assignment #2, 2005

作者: 四機四甲 李光君 (94 級)

目錄

一、問題描述．．．．．．．．．．．．．．．．．．．．．．．．．．．．1
二、問題分析．．．．．．．．．．．．．．．．．．．．．．．．．．．．2
2.1 接頭與質心位置之定義．．．．．．．．．．．．．．．．．．．．．2
2.2 運動方程式之推導．．．．．．．．．．．．．．．．．．．．．．．2
2.2-1 拘束方程式 Φ 之建立．．．．．．．．．．．．．．．．．．．3
2.2-2 Jacobian 矩陣，Φq．．．．．．．．．．．．． ．．．．．．．4
2.2-3 位置分析及牛頓疊代法．．．．．．．．．．．．．．．．．．．5
2.2-4 速度方程式．．．．．．．．．．．．．．．．．．．．．．．．5
2.2-5 加速度方程式．．．．．．．．．．．．．．．．．．．．．．．6
三、分析模擬程式實作．．．．．．．．．．．．．．． ．．．．．．．．．7
3.1 位置分析與牛頓疊代程序設計． ．．．．．．．．．．．．．．．7
3.2 速度分析之函式設計．．．．．．．．．．．．．．．．．．．．10
3.3 加速度分析之函式設計．．．．．．．．．．．．．．．．．．．11
四、結果與討論．．．．．．．．．．．．．．．． ．．．．．．．．．． 12

0
 一、問題描述

已知四連桿機構之桿長及初位置，如下圖所示。設若已知桿 2 之角速度ω2
及角加速度α2，試求機構各桿於整個運動循環中之位置、(角)速度及(角)加速度，
(須列印分析結果，並以電腦動畫顯示其運動)。

Table 1. Initial positions of the center of mass of links

Link no. x (mm) y (mm) φ (deg)
1 0 0 0
2 -0.05 0 0
3 -0.175 0.15sin(60°) 300°
4 -0.325 0.15sin(60°) 60°

1
 二 問題分析

2.1 接頭與質心位置之定義
參見圖一，首先定義固定座標系與各桿件的局部座標系，其中固定座標系
(OXY)與桿一的局部座標系重合。根據這些座標系，可列出各接頭在各桿件之局
部座標系的座標值如下(單位: ㎜)：

接頭 A： ξ 3A = −150 、η 3A = 0.0 、 ξ 4A = 150 、η 4A = 0.0

接頭 B： ξ 3B = 150 、η 3B = 0.0 、 ξ 2B = −50 、η 2B = 0.0

接頭 O： ξ 2O = 50 、η 2O = 0 、 ξ1O = 0.0 、η1O = 0.0

接頭 D： ξ 4D = −150 、η 4D = 0 、 ξ1D = −400 、η1D = 0.0

圖一

2.2 運動方程式之推導
對於圖一中所示之多體系統而言，由於桿件之總數 m = 4，每支桿的自由度
f=3，即系統之總自由度為 m×f=12，故需要 12 條拘束方程式(Constraint equation)
以確定各桿件之位置。系統中共有四個迴轉接頭，每個迴轉接頭將導致兩條拘束
方程式，固定桿將導致三條拘束方程式，而驅動條件則對應於一個驅動拘束，故
共有 4×2+3+1=12 條拘束方程式。

2
2.2-1 拘束方程式 Φ 之建立
四個接頭都是迴轉對，迴轉對接頭之拘束方程式的通式如式(1)所示。

 xi + ξ iP cos φi − η iP sin φi − x j − ξ jP cos φ j + η Pj sin φ j 

Φ (r ,2)
≡  (1)
 yi + ξ i sin φi + η i cos φi − y j − ξ j sin φ j − η j cos φ j 
P P P P

其中，下標 i 與 j，指第 i 桿與第 j 桿。上標 P 指接頭。

固定桿的拘束方程式：
Φ 1 ≡ x1 = 0 (2)
Φ 2 ≡ y1 = 0 (3)
Φ 3 ≡ φ1 = 0 (4)

接頭 A 的拘束方程式：
Φ 4 ≡ x4 − x3 + 150 cos φ 4 + 150 cos φ3 = 0 (5)
Φ 5 ≡ y 4 − y3 + 150 sin φ 4 + 150 sin φ3 = 0 (6)

接頭 B 的拘束方程式：
Φ 6 ≡ x3 − x2 + 150 cos φ3 + 50 cos φ2 = 0 (7)
Φ 7 ≡ y3 − y 2 + 150 sin φ3 + 50 sin φ 2 = 0 (8)

接頭 O 的拘束方程式：
Φ 8 ≡ x2 − x1 + 50 cos φ 2 = 0 (9)
Φ 9 ≡ y 2 − y1 + 50 sin φ 2 = 0 (10)

接頭 D 的拘束方程式：
Φ 10 ≡ x1 − 400 − x 4 + 150 cos φ 4 = 0 (11)
Φ11 ≡ y1 − y 4 + 150 sin φ4 = 0 (12)

驅動拘束方程式：
(
Φ 12 ≡ φ 2 − φ 20 + ω 2 t + 0.5α 2 t 2 = 0 ) (13)

3
將式(2)~(13)表示成矩陣形式，如式(14)所示，即為所得之拘束方程式。

 Φ1 
Φ 
 2
 Φ3 
 
 Φ4 
 Φ5 
  (14)
 Φ6 
Φ≡ =0
Φ7 
 
 Φ8 
Φ 
 9
Φ10 
 
 Φ11 
Φ12 

2.2-2 Jacobian 矩陣，Φq

Φq 是 Φ 對 q 作偏微分之 Jacobian 矩陣，其求法如下所示：

∂Φ
QΦq = , 而 q = [ x1 y1 φ1 x2 y 2 φ2 x3 y 3 φ3 x4 y 4 φ4 ]T
∂q

 ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ ∂Φ 
∴Φ q =  
 ∂x1 ∂y1 ∂ φ1 ∂x 2 ∂y 2 ∂φ 2 ∂x 3 ∂y 3 ∂φ 3 ∂x 4 ∂y 4 ∂φ 4 

 a1 0 0 0 0 0 0 0 0 0 0 0
0 b1 0 0 0 0 0 0 0 0 0 0 

0 0 c1 0 0 0 0 0 0 0 0 0
 
0 0 0 0 0 0 d1 0 d2 d3 0 d4 
0 0 0 0 0 0 0 e1 e2 0 e3 e4 
  (15)
0 0 0 f1 0 f2 f3 0 f4 0 0 0
⇒ Φq = 
0 0 0 0 g1 g2 0 g3 g4 0 0 0
 
 h1 0 0 h2 0 h3 0 0 0 0 0 0
0 i1 0 0 i2 i3 0 0 0 0 0 0
 
 j1 0 0 0 0 0 0 0 0 j2 0 j3 
 
0 k1 0 0 0 0 0 0 0 0 k2 k3 
 0 0 0 0 0 l1 0 0 0 0 0 0 

其中，a1 = 1、b1 = 1、c1 = 1、d1 = -1、d2 = -150sin φ3 、d3 = 1、d4= -150sin φ 4 、

e1 = -1、e2 = 150cos φ3 、e3 = 1、e4 = 150cos φ 4 、f1 = -1、f2 = 50sin φ 2 、f3 = 1、
f4 = 150sin φ3 、g1 = -1、g2 = -50cos φ 2 、g3 = 1、g4 = -150cos φ 2 、h1 = -1、h2 = 2、
h3 = 50sin φ 2 、i1 = -1、i2 = 1、i3 = -50cos φ 2 、j1 = 1、j2 = -1、j3 = -150sin φ 4 、
k1 = 1、k2 = -1、k3 = 150cos φ 4 、l1=1。

4
2.2-3 位置分析與牛頓疊代法
將 Φ 取一階之泰勒級數展開，以 q0 為展開之中心，如式(16)所示。

∂Φ ( q )
Φ (q ) ≅ Φ (q ) q = q0 q = q0 ( q − q 0 )
+
∂q
∂Φ ( q )
⇒ Φ ( q ) − Φ ( q ) q = q0 ≅ q = q0 ( q − q 0 ) (16)
∂q

令 q-q0=Δq，且 Φ(q) =0，重新整理如式(17)。

∂Φ ( q )
q = q0 ∆q = −Φ (q ) q = q0
∂q (17)

其中，q0 是初始的猜測值，Δq 是修正值。

將式(17)所求出之 Δq，加到 q0 得到新的 q 值，如式(18)所示。

q new = q0 + ∆q (18)

將式(18)求出的 qnew 作為式(18)的初始猜測值 q0，直到 Δq<10-6，便可求出

準確的位置值 q。

2.2-4 速度方程式
將拘束方程式 Φ 對 t 取微分。
Φ q q& + Φ t = 0
⇒ Φ q q& = − Φ t (19)
其中：
 0   x&1   x1 
 0   y&  y 
   1  1
 0   φ&1   φ1 
     
 0   x& 2   x2 
 0   y& 2   y2 
  &   
 0  φ  φ 
Φt =   q& =  2  q =  2
0 x& x
   3  3
 0   y& 3   y3 
 0  φ&  φ 
   3  3
 0   x& 4   x4 
     
 0   y& 4   y4 
 − (ω 2 + α 2t )  φ&4  φ4 

式(19)即速度方程式，求解此式即可得廣義速度 q& ，。

5
2.2-5 加速度方程式
將式(19)對時間 t 微分，可得式(20)所示之加速度方程式。

[ ]
Φ q q&& = − (Φ q q& )q q& + 2 Φ qt q& + Φ tt ≡ γ (20)
其中，

 0 
 0 
 
 0 
 & & 
 − 150 cos φ3φ3 − 150 cos φ 4φ 4 
2 2

 − 150 sin φ3φ&32 − 150 sin φ 4φ&42 

 
− 50 cos φ 2φ&22 − 150 cos φ3φ&32 
(Φ q q& )q q& =  − 50 sin φ φ& 2 − 150 sin φ φ& 2 
 2Φ qt q& = 0
 2 2 3 3

 − 50 cos φ 2φ 2& 2

 − 50 sin φ2φ2& 2 
 
 − 150 cos φ4φ&42 
 & 
 − 150 sin φ 4φ 4 2

 0 

Φ tt = [0 0 0 0 0 0 0 0 0 0 0 − α 2 ]
T

則，
 0 
 0 
 
 0 
 &2 &2 
150 cos φ 3φ 3 + 150 cos φ 4φ 4 
 150 sin φ 3φ&32 + 150 sin φ 4φ&42 
 
 50 cos φ 2φ&22 + 150 cos φ 3φ&32 
γ =
50 sin φ 2φ&22 + 150 sin φ 3φ&32 
 
 50 cos φ 2φ&22 
 50 sin φ 2φ&22 
 
 150 cos φ 4φ&42 
 
 150 sin φ 4φ&4 2

 α 

6
 三 分析模擬程式實作
3.1 位置分析及牛頓疊代程序之設計
四連桿機構各桿的位置，須使用牛頓疊代法的方式計算出來，故需設計相
關的函式如 2.2-2 節所示。

Jacbian 矩陣的函式 Φq：

void Jacob(double **A, double *x) {
int n=12,i,j;
for(i=0; i<n; i++){
for(j=0; j<n; j++)
A[i][j]=0.0;
}
A[0][0]=1;//1
A[1][1]=1;//2
A[2][2]=1;//3
A[3][9]=1;//6
A[4][10]=1;//10
A[5][6]=1;//14
A[6][7]=1;//18
A[7][3]=1;//21
A[8][4]=1;//24
A[9][0]=1;//26
A[10][1]=1;//29
A[11][5]=1;//32
//=-1
A[3][6]=-1;//4
A[4][7]=-1;//8
A[5][3]=-1;//12
A[6][4]=-1;//16
A[7][0]=-1;//20
A[8][1]=-1;//23
A[9][9]=-1;//27
A[10][10]=-1;//30
A[3][8]=-150*sin(x[8]);//5=-150*sin(phi3)
A[3][11]=-150*sin(x[11]);//7=-150*sin(phi4)
A[4][8]=150*cos(x[8]);//9=150*cos(phi3)

7
 A[4][11]=150*cos(x[11]);//11=150*cos(phi4)
A[5][5]=-50*sin(x[5]);//13=-50*sin(phi2)
A[5][8]=-150*sin(x[8]);//15=-150*sin(phi3)
A[6][5]=50*cos(x[5]);//17=50*cos(phi2)
A[6][8]=150*cos(x[8]);//19=150*cos(phi3)
A[7][5]=-50*sin(x[5]);//22=-50*sin(phi2)
A[8][5]=50*cos(x[5]);//25=50*cos(phi2)
A[9][11]=-150*sin(x[11]);//28=-150*sin(phi4)
A[10][11]=150*cos(x[11]);//31=150*cos(phi4)
}

牛頓疊代等號右側向之函式 –Φt：

void fun(double *C, double *x) {

C[0]=-x[0];//x1
C[1]=-x[1];//y1
C[2]=-x[2];//phi1

C[3]=-(x[9]-x[6]+150*cos(x[11])+150*cos(x[8]));//x4-x3+150*cos(phi4)+150*cos(phi3)
C[4]=-(x[10]-x[7]+150*sin(x[11])+150*sin(x[8]));//y4-y3+150*sin(phi4)+150*sin(phi3)

C[5]=-(x[6]-x[3]+150*cos(x[8])+50*cos(x[5]));//x3-x2+150*cos(phi3)+50*cos(phi2)
C[6]=-(x[7]-x[4]+150*sin(x[8])+50*sin(x[5]));//y3-y2_150*sin(phi3)+50*sin(phi2)

C[7]=-(x[3]-x[0]+50*cos(x[5]));//x2-x1+50*cos(phi2)
C[8]=-(x[4]-x[1]+50*sin(x[5]));//y2-y1+50*sin(phi2)

C[9]=-(x[0]-x[9]-400+150*cos(x[11]));//x1-x4-400+150*cos(phi4)
C[10]=-(x[1]-x[10]+150*sin(x[11]));//y1-y4-0+150*sin(phi4)
C[11]=-(x[5]-0.0-velocity*start_time-0.5*acceler*start_time*start_time);

//printf("%f\n",C[11]);
}

8
牛頓疊代程序之函式：
void NewtonV(double *x_old, void Jacob(double**, double*), void fun(double*, double*)) {
int n=12;
double x_new[12],deltax[12],C[12];

int index=1;
double **A=new double*[n];
for(int i=0;i<n;i++){
A[i]=new double[n];
}
fun(C, x_old);
Jacob(A, x_old);
gaussh(n,A,deltax,C);
for(i=0; i<n; i++){
x_new[i]=x_old[i]+deltax[i];
}
while( ( (fabs(deltax[0])>1.0e-6)||(fabs(deltax[1])>1.0e-6)||(fabs(deltax[2])>1.0e-6)
||(fabs(deltax[3])>1.0e-6)||(fabs(deltax[4])>1.0e-6)||(fabs(deltax[5])>1.0e-6)
||(fabs(deltax[6])>1.0e-6)||(fabs(deltax[7])>1.0e-6)||(fabs(deltax[8])>1.0e-6)
||(fabs(deltax[9])>1.0e-6)||(fabs(deltax[10])>1.0e-6)||(fabs(deltax[11])>1.0e-6))
&&(index<100000)){
for(i=0; i<n; i++){
x_old[i]=x_new[i];
}
fun(C, x_old);//get the violation of constraint eqs, C
Jacob(A, x_old);//get Jacob matrix A
gaussh(n,A,deltax,C);//solve A(dx)=C for dx
for(i=0; i<n; i++){
x_new[i]=x_old[i]+deltax[i];
}
index+=1;
}
for(i=0;i<n;i++)
delete []A[i];
delete []A;
}
經由這三個函式及理想的初始猜測值便可成功的計算出正確的數值。

9
3.2 速度分析之函式設計
•
根據式(19)求出 q 便可求出速度，據此設計一函式

計算速度的函式：
void Ang_velo_analy(double *posi,double *velo){
int n=12;
double **A=new double*[n];
for(int i=0;i<n;i++){
A[i]=new double[n];
}
double C[12];

//速度的右側項
for(i=0;i<n;i++)
C[i]=0.0;
C[11]=velocity+acceler*start_time;

//係數矩陣
Jacob(A, posi);

//儲存計算出的加速度值
double temp[12];
gaussh(n,A,temp,C);
for(i=0; i<n; i++)
velo[i]=temp[i];
}

10
3.3 加速度分析之函式設計
根據式(20)求出 q&& ，便可求出加速度，據此來設計一個函式。

計算加速度的函式：
void Ang_accele_analy(double *posi,double *velo,double *accele){
int n=12;
double **A=new double*[n];
for(int i=0;i<n;i++){
A[i]=new double[n];
}
double r[12];

//等號右側項
r[0]=0.0;
r[1]=0.0;
r[2]=0.0;
r[3]=150*cos(posi[8])*pow(velo[8],2)+150*cos(posi[11])*pow(velo[11],2);
r[4]=150*sin(posi[8])*pow(velo[8],2)+150*sin(posi[11])*pow(velo[11],2);
r[5]=50*cos(posi[5])*pow(velo[5],2)+150*cos(posi[8])*pow(velo[8],2);
r[6]=50*sin(posi[5])*pow(velo[5],2)+150*sin(posi[8])*pow(velo[8],2);
r[7]=50*cos(posi[5])*pow(velo[5],2);
r[8]=50*sin(posi[5])*pow(velo[5],2);
r[9]=150*cos(posi[11])*pow(velo[11],2);
r[10]=150*sin(posi[11])*pow(velo[11],2);
r[11]=acceler;

//係數矩陣
Jacob(A, posi);

//儲存加速度值
double temp[12];
gaussh(n,A,temp,r);
for(i=0; i<n; i++)
accele[i]=temp[i];
}

11
 四 結果與討論
程式執行的介面如圖二、三所示，圖三是說明。在視窗中按下 s 便會啟動
動態模擬；按下 r 則是重新啟動。

圖二

圖三

12
按下 1 顯示角位移(圖四)：

圖四

按下 2 顯示角速度(圖五)：

圖五

按下 3 顯示角加速度(圖六)：

圖六

13
按下 4 顯示質心速度(圖七)：

圖七

按下 5 顯示質心速度(圖七)：

圖七

14