学士学位论文--在matlab环境下开发平面连杆机构运动分析系统.doc

在MATLAB环境下开发平面连杆机构运动分析系统

在MATLAB环境下开发平面连杆机构运动分析系统

摘 要
　 建立了铰链四杆机构运动分析的数学模型 ,以MATLAB程序设计语言为平台 ,将参数化设计与交互式相结合 ,设计了铰链四杆机构分析软件 ,该软件具有方便用户的良好界面 ,并给出界面设计程序 ,从而使机构分析更加方便、快捷、直观和形象.设计者只需输入参数就可得到分析结果 ,再将运行结果与设计要求相比较 ,对怎样修改设计做出决策.它为四杆机构设计提供了一种实用的软件与方法.

关键词：平面四杆机构，MATLAB软件，运动分析，分析

THE DEVELOPMENT OF SYSTEM FOR ANALYSIS OF MOTION IN PLANE FOUR BAR MECHANISM BASED ON MATLAB SOFTWARE
Abstract
A mathematical model of motion analysis was established in plane four - linkage , and analytical software was developed. The software adopted Matlab as a design language. It combined parametric design with interactive design and had good interface for user. Thus, it was faster and more convenient to analyse linkage. The analytical result was obtained as soon as input parameters was imported and the devisers can make decision - making of modification by the comparing analytical result with design demand. It provides an applied software and method for linkage.

Key words: Plane Four Bar Mechanism, MATLAB, Analysis of Motion, Analyze

目 录

1 绪论 1
2 平面连杆机构的设计分析 3
2.1 平面四连杆机构的运动分析 3
2.2 机构的数学模型的建立 3
2.2.1 建立机构的闭环矢量位置方程 3
2.2.2 求解方法 5
3 基于MATLAB程序设计 6
3.1 程序流程 6
3.2 M文件编写 8
3.3 程序运行结果输出 10
4 基于MATLAB图形界面设计 14
4.1 界面设计 14
4.2 代码设计 15
5 结论 21
致谢 21
参考文献 22

1 绪论
连杆机构的应用十分广泛，它不仅在众多工农业机械和工程机械中得到广泛应用[1]。铰链四杆机构是由转动副将各构件的头尾联接起的封闭四杆系统，并使其中一个构件固定而组成。被固定件称为机架，与机架直接铰接的两个构件和称为连架杆，不直接与机架铰接的构件称为连杆。连架杆如果能作整圈运动就称为曲柄，否则就称为摇杆,连杆机构的最基本形式是平面四杆机构，它是其它连杆机构的基础。所以，对平面四杆机构进行研究可以概括连杆机构内在的基本原理，从而用以连杆机构的设计。
机构运动学综合是按照给定的运动特性对机构进行系统的设计的过程，包括型综合和尺度综合两大主要内容，主要综合方法有解析法、作图法和实验法。作图法和实验法 工作量大，设计精度低，仅适用于对机构精度要求不高的场合。近几十年来，随着工业技术的高速发展，人们对机构的复杂程度和精度要求越来越高，作图法和实验法已不能满足要求，而基于计算机辅助设计（例如MATLAB软件）的解析法得到了广泛的应用。
此课题的主要内容是系统地对平面四杆机构连杆曲线进行研究，从而来获得连杆机构基本的原理和综合方法，以便在实际中得到应用；主要特色是在各个设计进度中将会大量应用计算机高级语言MATLAB[2]编程来辅助设计和仿真平面四杆机构。
铰链四杆机构的运动学分析是机构学中典型的机构运动分析之一 ,如果设计铰链四杆机构时能及时图示其运动轨迹和速度分析 ,从而将图示结果与设计要求相比较 ,可以及时修改设计中的偏差.目前 ,MALTAB已经不再是“矩阵实验室” ,而成为国际上最流行的科学与工程计算的软件工具 ,以及一种具有广泛应用前景的全新的计算机高级编程语言 ,它在国内外高校和科研部门正扮演着越来越重要的角色 ,功能也越来越大 ,不断适应新的要求提出新的解决办法.可以预见 ,在科学运算与科学绘图领域 ,MATLAB语言将长期保持其独一无二的地位.然而 ,国内至今尚未见到采用 MATLAB 开发的有关机构学的软件 ,以 MATLAB 的科学运算与绘图的强大功能开发了铰链机构运动仿真软件.
机构的运动分析是机构设计中必不可少的工作 ,通常可使用图解法和解析法来进行 ,图解法因其作图、计算工作量大、精度差的缺点 ,在实际工程设计应用中有很大的局限性。解析法的计算工作量很大 ,但随着计算机在工程设计领域的广泛应用 , 优化方法和计算机辅助设计的应用已成为研究连杆机构的重要方法，一些软件平台为解决复杂的工程计算提供了强有力的武器。，并已相应地编制出大量的、适用范围广、计算机时少、使用方便的通用软件。
MATLAB 是 Mathworks 公司于 1982 年推出的一套功能强大的工程计算软件, 广泛应用于自动控制、 机械设计、 流体力学和数理统计等工程领域, 被誉为巨人肩上的工具。它集数值分析、 矩阵运算、 信号处理和图形显示于一体, 构成了一个方便的、 界面友好的用户环境。
所谓图形用户界面[3], 简称为GUI(Graphic User Interface) , 是指包含了各种图形控制对象, 如图形窗口、菜单、对话框以及文本等内容的用户界面。利用这些用户界面, 用户可以和计算机之间进行信息交流。用户可以通过某种方式来选择或者激活这些图形对象, 来运行一些特性的M 文件。最常见的激活方式是利用鼠标或者其它设备来点击这些对象。对于一个用户来说, 图形用户界面就是他所面对的应用程序, 对图形界面的操作直接影响应用程序的应用前途。对于以往专门用于科学计算的语言, 如FORTRAN 语言等, 编写图形界面的功能较弱, 因而用其开发的程序, 其界面往往不够友好, 用户使用起来很不方便。而目前流行的可视化语言, 对科学计算的功能又相对弱一些。MATLAB提供了非常强大的编写图形用户界面的功能。用户只和前台界面下的控件发生交互，而所有运算、绘图等内部操作都封装在内部，终端用户不需要区追究这些复杂过程的代码。图形用户界面大大提高用户使用MATLAB程序的易用性。因此，学习MATLAB图形用户界面编程，即GUI程序的创建，是MATLAB编程用户应该掌握的重要一环。
对于一个MATLAB 的图形用户界面, 它的设计过程可以分为两个部分:
(1)用户界面的外观设计。在这里, 主要是通过不同的对话框、按钮、文本框等许多工具的使用, 设计出一个图形用户界面。同时也应搞清楚这个图形界面的功能是什么, 也即在图形界面上的操作会引发什么样的结果。
(2)图形界面的完成。在这里, 用户将根据在外观设计阶段所确定的图形界面的功能, 针对各个不同的图形对象来编写出能够实现该功能的函数代码, 确保这个图形界面能够完成所预定的功能。
工程人员通过使用 MATLAB提供的工具箱, 可以高效求解复杂的工程问题, 并可以对系统进行动态仿真, 用强大的图形功能对数值计算结果进行显示。笔者在MATLAB环境下开发了一个平面四杆机构运动分析系统 ,只要在系统图形界面的文本编辑框中交互地输入四杆机构各构件的参数和原动件的角速度 ,就能迅速地输出当原动件转动1周时从动件的位移、速度、加速度的变化规律曲线 ,具有融计算与绘图为一体 ,操作简便、界面友好 ,计算速度快 ,准确性高的特点。本文以曲柄摇杆机构为分析对象，借助MATLAB软件进行运动分析。曲柄摇杆机构是平面四连杆机构中最基本的结构，它可以用来实现转动和摆动之间运动形式的转换或传递动力。对曲柄摇杆机构进行运动分析的主要内容是：在机构尺寸参数已知的情况下，假定曲柄做匀速转动，从运动几何关系上分析连杆和摇杆的角位移、角速度、角加速度等运动参数的变化情况。并根据机构闭环矢量方程计算从动件的位移偏差。
本课题的主要内容是平面四杆机构的连杆曲线及轨迹综合分析系统的开发，其意义在于：
一、深入研究计算机在设计和仿真连杆机构曲线方面的应用，从而指导实践；
二、总结出四杆机构轨迹综合的理论基础，从而指导多杆或复杂的低副平面机构的综合。
三、综合运用MATLAB软件强大的计算功能与图形界面开发系统，设计出可以方便快捷实现机构分析的应用系统
2 平面连杆机构的设计分析
2.1 平面四连杆机构的运动分析[4]
在平面四杆机构中，其具有曲柄的条件为：
a.各杆的长度应满足杆长条件，即：
最短杆长度+最长杆长度≤其余两杆长度之和。
b.组成该周转副的两杆中必有一杆为最短杆，且其最短杆为连架杆或机架（当最短杆为连架杆时，四杆机构为曲柄摇杆机构；当最短杆为机架时，则为双曲柄机构）。
在如下图1所示的曲柄摇杆机构中，构件AB为曲柄，则B点应能通过曲柄与连杆两次共线的位置。
2.2 机构的数学模型的建立[6]
2.2.1 建立机构的闭环矢量位置方程

图1 四杆机构简图[8]
在用矢量法建立机构的位置方程时，需将构件用矢量来表示，并作出机构的封闭矢量多边形。如图1所示，先建立一直角坐标系。设各构件的长度分别为、、、，其方位角为、、、。以各杆矢量组成一个封闭矢
量多边形，即ABCDA。其个矢量之和必等于零。即：
式1
式1 为图1所示四杆机构的封闭矢量位置方程式。对于一个特定的四杆机构，其各构件的长度和原动件2的运动规律，即为已知，而=0，故由此矢量方程可求得未知方位角、。
角位移方程的分量形式为：
式2
闭环矢量方程分量形式对时间求解一阶导数,就能够得到角速度方程，如下所示：
式3
其矩阵形式为：
式4
联立式3两公式可求得：
式5
式6
闭环矢量方程分量形式对时间求解二阶导数，得到角加速度方程矩阵形式[11]为：
式7
由式7可求得加速度：
式8
式9
注：式1--式9中，（i=1,2,3,4）分别表示机架1、曲柄2、连杆3、摇杆4的长度；（i=1,2,3,4）是各杆与x轴的正向夹角，逆时针为正，顺时针为负，单位为; 是各杆的角速度，，单位为; 为各杆的角加速度，，单位为。
2.2.2 求解方法
应用数学方法对以上各式进行求解，方法如下：
（1）求导中应用了下列公式：
式10
（2）在角位移方程分量形式（式2）中，由于假定机架为参考系，矢量1与x轴重合，=0，则有非线性方程组：
式11
可以借助MATLAB自带的fsolve函数求出连杆3的角位移和摇杆4的角位移。
（3）求解具有n个未知量（i=1,2,…,n）的线性方程组：
式12
上式可以表示为系列矩阵，是一个阶方阵，可表示如下，并运用逆矩阵知识：
式13
的逆矩阵为;
常数项b是一个n维矢量：
式14
因此，线性方程组解的矢量为：
式15
式11是求解连杆3和摇杆4角速度和角加速度的依据。
以上各式既为求解平面连杆机构运动规律的数学模型。
3 基于MATLAB程序设计[12]
程序设计是编程者实现算法，用编程语言表达数学模型的过程，或者说是现实问题求解的思维活动代码化的过程。也就是解决如何组织MATLAB的结构. 根据以上数学模型，选择优化算法，创建程序流程图，运用MATLAB语言，编写MATLAB程序，并在MATLAB6.5中调试运行，将这个实际问题转化为编程语言。
3.1 程序流程
按照现代程序设计的观点，算法（Algorithm）是一系列解决问题的清晰指令. 算法是一个程序的灵魂，一个准确无误的算法是程序设计成功的关键。任何算法功能都能够通过三种基本结构：顺序结构，选择结构，循环结构的组合来实现。为了更加方便清楚准确的编写程序，我们首先把算法用流程图的方式来表示出来。
程序流程图是人们对解决问题的方法、思路或算法的一种描述。采用简单规范的符号，画法简单, 结构清晰，逻辑性强, 便于描述，容易理解综合运用三种逻辑结构，并且仔细推算，以保证算法精确,节省空间和时间，以及达到高效率。
程序流程图如图2所示。
新步长输入
给定已知参数：L1,L2,L3,L4,W2
设置th2的范围及步长
调用fsolve函数求解th3,th4
求解连杆3两端点坐标值
绘制从动件角位移线图
绘制连杆3的几个位置点
利用矩阵公式求W3,W4
输出一个周期内角位移，角速度，角加速度数据
绘制从动件角加速度线图
利用矩阵公式求a3,a4
绘制从动件角速度线图
程序开始
程序结束
重新设置步长
图2 程序流程图

3.2 M文件编写[16]
首先创建函数aa，函数fsolve通过他确定。
function t=aa(th1,th2,L2,L3,L4,L1)
t=[L2*cos(th2)+L3*cos(th(1))-L4*cos(th(2))-L1;L2*sin(th2)+L3*sin(th(1))-L4*sin(th(2))];
主程序如下：
disp ' * * * * * * 平面四杆机构的运动分析 * * * * * *'
L1=input('输入机架长度 L1=');
L2=input('输入曲柄长度(主动件) L2=');
L3=input('输入连杆长度 L3=');
L4=input('输入输入从动杆长度 L4=');
rs=[L1 L2 L3 L4];
minimum=min(rs);
maximum=max(rs);
if (minimum+maximum)>(sum(rs)-minimum-maximum)
disp '此机构不满足杆长条件,程序终止',break;
end
if L1<=L2&L1<=L3&L1<=L4
disp '此机构为双曲柄机构'
elseif L2<=L1&L2<=L3&L2<=L4
disp '此机构为曲柄摇杆机构'
else disp '此机构为双摇杆机构,本例不予考虑',break;
end
w2=input('输入主动件角速度 w2=');
th2=[0:1/6:2]*pi;
th34=zeros(length(th2),2);
options=optimset('display','off');
for m=1:length(th2)
th34(m,:)=fsolve('aa',[1 1],options,th2(m),L2,L3,L4,L1);
end
y=L2*sin(th2)+L3*sin(th34(:,1)');
x=L2*cos(th2)+L3*cos(th34(:,1)');
xx=[L2*cos(th2)];
yy=[L2*sin(th2)];
figure(1)
plot([x;xx],[y;yy],'k',[0 L1],[0 0],'k--^',x,y,'ko',xx,yy,'ks')
title('连杆3的几个位置点')
xlabel('水平方向')
ylabel('垂直方向')
axis equal
th2=[0:2/72:2]*pi;
for m=1:length(th2)
th34(m,:)=fsolve('aa',[1 1],options,th2(m),L2,L3,L4,L1);
end
figure(2)
plot(th2*180/pi,th34(:,1),th2*180/pi,th34(:,2))
plot(th2*180/pi,th34(:,1)*180/pi,th2*180/pi,th34(:,2)*180/pi)
grid
xlabel('主动件转角\theta_2(度)')
ylabel('从动件角位移(度)')
title('角位移线图')
w3=(L2*w2*sin(th34(:,2)-th2'))./(L3*sin(th34(:,1)-th34(:,2)));
w4=(L2*w2*sin(th2'-th34(:,1)))./(L4*sin(th34(:,2)-th34(:,1)));
figure(3)
plot(th2*180/pi,w3,th2*180/pi,w4);
grid
xlabel('主动件转角\theta_2(度)')
ylabel('从动件角速度(rad\cdot s^{-1})')
title('角速度线图')
s32=th34(:,1)-th2';
s43=th34(:,2)-th34(:,1);
s24=th2'-th34(:,2);
a3=(L2*w2^2*cos(s24)+L3*w3.^2.*cos(s43)-L4*w4.^2)./(L4*sin(s43));
a4=(L2*w2^2*cos(s32)-L4*w4.^2.*cos(s43)+L3*w3.^2)./(L4*sin(s43));
figure(4)
plot(th2*180/pi,a3,th2*180/pi,a4);
grid
xlabel('从动件角加速度')
ylabel('从动件角加速度(rad\cdot s^{-2})')
title('角加速度线图')
disp ' 曲柄转角 连杆转角-从动杆4转角-连杆角速度-从动杆4角速度-连杆加速度-从动杆4加速度'
ydcs=[th2'*180/pi,th34(:,1)*180/pi,th34(:,2)*180/pi,w3,w4,a3,a4];
disp(ydcs)
ex=L2*cos(th2')+L3*cos(th34(:,1))-L4*cos(th34(:,2))-L1;
ey=L2*sin(th2')+L3*sin(th34(:,1))-L4*sin(th34(:,2));
ee=norm([ex ey]);
disp ' 曲柄转角 X向偏差 Y向偏差'
wc=[th2'*180/pi,ex,ey]
disp(wc)
fprintf(1,' 偏差矢量矩阵的模ee=%3.4f\n',ee)
figure(5)
plot(th2*180/pi,ex,th2*180/pi,ey)
grid
title('均方根偏差曲线')
xlabel('主动件转角\theta_2(度)')
ylabel('均方根偏差')
3.3 程序运行结果输出
* * * * * * 平面四杆机构的运动分析 * * * * * *
输入机架长度 L1=90
输入曲柄长度(主动件) L2=30
输入连杆长度 L3=60
输入输入从动杆长度 L4=70
此机构为曲柄摇杆机构
输入主动件角速度 w2=20
曲柄转角 连杆转角-从动杆4转角-连杆角速度-从动杆4角速度-连杆加速度-从动杆4加速度
Columns 1 through 6
0 71.3707 125.6853 -10.0000 -10.0000 -184.6761
5.0000 68.7629 123.2500 -10.8220 -9.4453 -137.6470
10.0000 65.9802 120.9809 -11.3983 -8.6728 -88.7805
15.0000 63.0843 118.9297 -11.7289 -7.7076 -41.6608
20.0000 60.1349 117.1400 -11.8305 -6.5875 0.7471
25.0000 57.1857 115.6451 -11.7326 -5.3571 36.4813
30.0000 54.2822 114.4668 -11.4719 -4.0626 64.6726
35.0000 51.4601 113.6156 -11.0869 -2.7467 85.3915
40.0000 48.7460 113.0922 -10.6140 -1.4458 99.3671
45.0000 46.1578 112.8890 -10.0848 -0.1885 107.6877
50.0000 43.7061 112.9925 -9.5252 1.0046 111.5547
55.0000 41.3961 113.3848 -8.9547 2.1200 112.1168
60.0000 39.2285 114.0454 -8.3876 3.1497 110.3787
65.0000 37.2012 114.9523 -7.8334 4.0906 107.1673
70.0000 35.3102 116.0832 -7.2979 4.9422 103.1339
75.0000 33.5504 117.4161 -6.7839 5.7066 98.7752
80.0000 31.9164 118.9295 -6.2922 6.3869 94.4624
85.0000 30.4026 120.6028 -5.8217 6.9866 90.4698
90.0000 29.0040 122.4164 -5.3702 7.5098 87.0018
95.0000 27.7161 124.3516 -4.9347 7.9599 84.2148
100.0000 26.5356 126.3905 -4.5114 8.3400 82.2350
105.0000 25.4598 128.5160 -4.0959 8.6527 81.1722
110.0000 24.4874 130.7114 -3.6833 8.8996 81.1294
115.0000 23.6183 132.9604 -3.2680 9.0815 82.2090
120.0000 22.8540 135.2467 -2.8442 9.1985 84.5143
125.0000 22.1975 137.5541 -2.4053 9.2494 88.1462
130.0000 21.6532 139.8658 -1.9444 9.2326 93.1925
135.0000 21.2277 142.1645 -1.4541 9.1452 99.7082
140.0000 20.9293 144.4323 -0.9268 8.9843 107.6830
145.0000 20.7680 146.6503 -0.3554 8.7464 116.9971
150.0000 20.7557 148.7989 0.2662 8.4289 127.3655
155.0000 20.9056 150.8580 0.9422 8.0304 138.2825
160.0000 21.2315 152.8074 1.6737 7.5520 148.9874
165.0000 21.7468 154.6277 2.4570 6.9983 158.4813
170.0000 22.4636 156.3010 3.2832 6.3785 165.6267
175.0000 23.3908 157.8125 4.1374 5.7064 169.3344
180.0000 24.5330 159.1513 5.0000 5.0000 168.8014
185.0000 25.8896 160.3113 5.8483 4.2793 163.7155
190.0000 27.4540 161.2915 6.6595 3.5642 154.3353
195.0000 29.2145 162.0954 7.4135 2.8723 141.4095
200.0000 31.1547 162.7307 8.0949 2.2166 125.9711
205.0000 33.2551 163.2074 8.6936 1.6054 109.0995
210.0000 35.4943 163.5374 9.2048 1.0421 91.7321
215.0000 37.8502 163.7324 9.6279 0.5260 74.5640
220.0000 40.3011 163.8041 9.9651 0.0540 58.0308
225.0000 42.8259 163.7627 10.2202 -0.3791 42.3417
230.0000 45.4047 163.6173 10.3977 -0.7793 27.5346
235.0000 48.0186 163.3753 10.5019 -1.1529 13.5312
240.0000 50.6498 163.0425 10.5365 -1.5062 0.1811
245.0000 53.2813 162.6234 10.5045 -1.8451 -12.7055
250.0000 55.8967 162.1207 10.4076 -2.1752 -25.3371
255.0000 58.4798 161.5360 10.2466 -2.5020 -37.9287
260.0000 61.0146 160.8696 10.0212 -2.8303 -50.6938
265.0000 63.4849 160.1204 9.7299 -3.1647 -63.8408
270.0000 65.8738 159.2863 9.3702 -3.5098 -77.5696
275.0000 68.1640 158.3643 8.9388 -3.8695 -92.0672
280.0000 70.3369 157.3500 8.4313 -4.2478 -107.5012
285.0000 72.3729 156.2385 7.8425 -4.6481 -124.0090
290.0000 74.2509 155.0239 7.1668 -5.0734 -141.6810
295.0000 75.9485 153.6996 6.3980 -5.5260 -160.5356
300.0000 77.4417 152.2586 5.5305 -6.0069 -180.4846
305.0000 78.7051 150.6938 4.5590 -6.5156 -201.2871
310.0000 79.7123 148.9987 3.4804 -7.0494 -222.4931
315.0000 80.4363 147.1676 2.2943 -7.6020 -243.3816
320.0000 80.8508 145.1969 1.0048 -8.1633 -262.9044
325.0000 80.9310 143.0864 -0.3776 -8.7178 -279.6556
330.0000 80.6557 140.8404 -1.8347 -9.2440 -291.8984
335.0000 80.0097 138.4691 -3.3385 -9.7140 -297.6852
340.0000 78.9857 135.9908 -4.8512 -10.0942 -295.0989
345.0000 77.5872 133.4326 -6.3261 -10.3473 -282.6143
350.0000 75.8302 130.8308 -7.7105 -10.4360 -259.5197
355.0000 73.7440 128.2310 -8.9510 -10.3277 -226.2782
360.0000 71.3707 125.6853 -10.0000 -10.0000 -184.6761

程序运行图形输出：

图3 运行界面

图4 连杆3的几个位置点

图5 角位移线图

图6 角速度线图

图7 角加速度线图
4 基于MATLAB图形界面设计
4.1 界面设计

图8 新建GUI文件
首先我们新建一个GUI文件，如上图所示，选择Blank GUI(Default)：
进入GUI开发环境以后添加5个编辑文本框，9个静态文本框，和1个下拉菜单。利用菜单编辑器，创建Open、Print、Close三个菜单。创建好GUI界面需要的各交互控件并调整好位置后，设置这些控件的属性。最后的界面如图示：
图9 界面效果
4.2 代码设计
(1)打开M文件编辑器(M-file Edit),点击向下的三角图标，可以看到各个对象的回调函数（Callback）,某些对象的创建函数或打开函数等。通过选中相应项就可以跳动对应函数位置进行程序编辑。
选中’edit_callback’选项，光标跳到’ function edit1_Callback(hObject, eventdata, handles)’下面空白处，添加以下代码：
user_entry=str2double(get(hObject,'String'));
if isnan(user_entry)
errordlg('请输入数值！','Bad Input')
end
该语句严格限制编辑框内必须输入数值，否则出现错误对话框（如下图所示）。同理在其他四个编辑框的回调函数下输入相同的代码。

图10 错误对话框
(2)打开M文件编辑器(M-file Edit),点击向下的三角图标，设置下拉菜单返回函数，光标跳到’ function popupmenu1_Callback(hObject, eventdata, handles)’下面空白处。由于下拉菜单是本界面设计关键控件，与本设计相关的程序都放在这个返回函数下。添加代码如下：L1=str2double(get(handles.edit1,'String'));
L2=str2double(get(handles.edit2,'String'));
L3=str2double(get(handles.edit3,'String'));
L4=str2double(get(handles.edit4,'String'));
w2=str2double(get(handles.edit5,'String'));
th2=[0:2/72:2]*pi;
th34=zeros(length(th2),2);
options=optimset('display','off');
for m=1:length(th2)
th34(m,:)=fsolve('aa',[1 1],options,th2(m),L2,L3,L4,L1);
end
w2=250;
for i=1:length(th2)
A=[-L3*sin(th34(i,1)) L4*sin(th34(i,2)); L3*cos(th34(i,1)) -L4*cos(th34(i,2))];
B=[w2*L2*sin(th2(i)); -w2*L2*cos(th2(i))];
w=inv(A)*B;
w3(i)=w(1);
w4(i)=w(2);
end
for i=1:length(th2)
C=[-L3*sin(th34(i,1)) L4*sin(th34(i,2));L3*cos(th34(i,1)) -L4*cos(th34(i,2))];
D=[w2^2*L2*cos(th