【正文】 %將各位置參數(shù)用向量儲存，便于后面繪圖，角度用度表示 ss3(i)=y(1)。th1(i)=x(10)/dr。th3(i)=y(2)/dr。th4(i)=y(3)/dr。sse(i)=y(4)。 %進行速度分析 A=[cos(theta3) s3*sin(theta3) 0 0。 sin(theta3) s3*cos(theta3) 0 0。 0 x(6)*sin(theta3) x(7)*sin(theta4) 1。 0 x(6)*cos(theta3) x(7)*cos(theta4) 0]。%A 機構從動件的位置參數(shù)矩陣 B=[x(5)*sin(x(10))。x(5)*cos(x(10))。0。0]。%B機構原動件的位置參數(shù)列陣 yy=w1*inv(A)*B。%公式 13 求解， yy 表示機構從動件速度列陣， inv(A)是 A 的西南大學工 程技術學院課程設計 (論文 ) 12 逆陣 vs3=yy(1)。w3=yy(2)。w4=yy(3)。vse=yy(4)。 %將各速度參數(shù)以向量的方式表示，以便后面繪圖 dvs3(i)=yy(1)。dw3(i)=yy(2)。dw4(i)=yy(3)。dvse(i)=yy(4)。 %dA為從動件位置參 數(shù)矩陣對時間一次求導 %進行角速度分析 dA=[w3*sin(theta3) vs3*sin(theta3)s3*w3*cos(theta3) 0 0。 w3*cos(theta3) vs3*cos(theta3)s3*w3*sin(theta3) 0 0。 0 x(6)*w3*cos(theta3) x(7)*w4*cos(theta4) 0。 0 x(6)*w3*sin(theta3) x(7)*w4*sin(theta4) 0]。 %dB 就是原動件位置參數(shù)列陣對時間一次求導 dB=[x(5)*w1*cos(x(10))。x(5)*w1*sin(x(10))。0。0]。 KK=dA*yy+w1*dB。 %KK 為公式 14右端 ya=inv(A)*KK。%公式 14求解， ya 為從動件加速度列陣 %將各加速度以向量表示 as3(i)=ya(1)。atheta3(i)=ya(2)。atheta4(i)=ya(3)。ase(i)=ya(4)。 x(10)=x(10)+dth。 %計算下一個點 x(1)=s3。 x(2)=theta3。 x(3)=theta4。 x(4)=se。 end %繪制運動參數(shù)曲線 subplot(2,2,1)。 % 選擇第 1個子窗口 plot(th1,th3,th1,th4,th1,sse*1e3)。%繪制位置線圖 subplot(2,2,2)。 plot(th1,dw3,th1,dw4,th1,dvse)。%繪制速度線圖 subplot(2,2,3)。 plot(th1,atheta3,th1,atheta4,th1,ase)。 %繪制加速度線圖 %這個函數(shù)是關于牛頭刨床位置方程求解，可得： s3,theta3,theta4,sE 四個運動變量 。 子程序： function y=ntpc(x) %x(1)—— 代表 3s 。 %x(2)—— 代表構件 3的轉角 3? ； %x(3） —— 代表構件 4的轉角 4? ； %x(4)—— 代表 E點的線位移 Es 。 %x(5)—— 代表 1l ； %x(6)—— 代表 3l 。 %x(7)—— 代表 4l 。 西南大學工 程技術學院課程設計 (論文 ) 13 %x(8)—— 代表 6l 。 %x(9） —— 代表 6l? 。 %x(10)—— 代表構件 1的轉角 1? 。 %先賦初值；這些初值來自于主程序。 s3=x(1)。 theta3=x(2)。 theta4=x(3)。 se=x(4)。 epsilon=1e6。%設置求解精度為 106 %用矩陣的形式表示位置方程組（ 4x1 的矩陣） f=[s3*cos(theta3)x(5)*cos(x(10))。s3*sin(theta3)x(5)*sin(x(10))x(8)。 x(6)*cos(theta3)+x(7)*cos(theta4)se。 x(6)*sin(theta3)+x(7)*sin(theta4)x(9)]。 %用牛頓 辛普森法求解 while norm(f)epsilon %J 位置方程組的雅可比矩陣，即從動件位置參數(shù)矩陣 J=[cos(theta3) s3*sin(theta3) 0 0。 sin(theta3) s3*cos(theta3) 0 0。 0 x(6)*sin(theta3) x(7)*sin(theta4) 1。 0 x(6)*cos(theta3) x(7)*cos(theta4) 0]。 dth=inv(J)*(*f)。 %計算增量，進行迭代， inv(J) 為 J的逆陣 s3=s3+dth(1)。 theta3=theta3+dth(2)。 theta4=theta4+dth(3)。 se=se+dth(4)。 f=[s3*cos(theta3)x(5)*cos(x(10))。s3*sin(theta3)x(5)*sin(x(10))x(8)。 x(6)*cos(theta3)+x(7)*cos(theta4)se。 x(6)*sin(theta3)+x(7)*sin(theta4)x(9)]。 norm(f)。%若未達精度，會繼續(xù)迭代。 end %輸出 4個參數(shù) y(1)=s3。 y(2)=theta3。 y(3)=theta4。 y(4)=se。 將程序輸入 matlab 中就可得到 E 點的位移、速度和加速度的運動線圖（圖 圖 圖 5）。 西南大學工 程技術學院課程設計 (論文 ) 14 圖 3 位置線圖 圖 4 速度線圖 西南大學工 程技術學院課程設計 (論文 ) 15 圖 5 加速度線圖 （作圖法） 建立直角坐標系，并標出各桿矢量及方位角。利用兩個封閉圖形 ABCA 及 CDEGC。（令 S3=BCl 1