《RPP平面连杆机构的动态仿真.doc》由会员分享，可在线阅读，更多相关《RPP平面连杆机构的动态仿真.doc（42页珍藏版）》请在三一办公上搜索。
1、RPP平面连杆机构的动态仿真 摘要：利用力矩、复数推导了曲柄、RPPII级杆组的运动学和动力学数学模型，对其编制相应的M函数，并给于一定的初值，将适用于MATLAB仿真的矩阵数学模型，对给定的RPPII级机构、RPRRPP六杆机构为例说明如何使用这二个仿真模块建立MAllAB仿真模型，并对其仿真结果的正确性加以判别其主要目的是以组成机构的杆组为仿真模块，搭建各种平面连杆机构MATLAB仿真模型，可以对各种机构进行运动、动力学仿真。关键词：运动学 动力学 机构 仿真 杆组 MATLAB指导老师签名：Kinematic simulation & dynamic simulation of RPP
2、II degree of freedom mechanism with MATLAB Abstract: Use of torque, the plural is derived crank, RPPII class bar groups kinematic and dynamic mathematical model, its preparation of the corresponding M function, and to the initial value at a certain, will apply to the MATLAB simulation of the matrix
3、model, given The RPPII level institutions, RPR-RPP 6 as examples of how to use these two simulation module to establish MAllAB simulation model and its simulation results to determine the accuracy. Its main purpose is to bar group constituted bodies simulation module, build a variety of planar linka
4、ge MATLAB simulation model, can exercise a variety of institutions, dynamics simulation. Key words：kinematics mechanism simulation bar group MATLABSignature of Supervisor: 目录1.引言11.1 平面连杆机构概述11.2 杆组11.3 机构的组成原理22 曲柄原动件、RPP平面连杆运动学数学模型的建立22.1 曲柄原动件运动学分析22.11 曲柄原动件运动学数学模型的建立22.12 曲柄MATLAB运动学仿真模块M函数22.2
5、 RPP四杆运动学分析32.21 RPP四杆运动学数学模型的建立32.3 RPP四杆机构MATLAB运动仿真52.31 RPP四杆机构MATLAB仿真模型52.32 用MATLAB实现牛顿-辛普森求解方法72.4 RPP四杆机构MATLAB仿真结果93 RPRRPP六杆机构MATLAB运动学仿真123.1 RPRII级杆组MATLAB运动学仿真模块123.2 RPRRPP六杆机构143.21 RPRRPP六杆机构MATLAB仿真模型143.22 RPRRPP杆组M函数为：163.23 RPRRPP六杆机构MATLAB仿真结果174 曲柄、RPP杆组的MATLAB动力学分析204.1 曲柄MAT
6、LAB动力学仿真模块204.11曲柄的动力学矩阵表达式204.2 RPRII级杆组的动力学仿真模块224.21 RPRII级杆组动力学矩阵表达式224.22 RPRII级杆组MATLAB动力学仿真模块M函数234.3 RPPII级杆组的动力学仿真模块254.31 RPPII级杆组动力学矩阵表达式254.32 RPPII级杆组MATLAB动力学仿真模块M函数264.4 RPRRPP六杆机构MATLAB动力学仿真274.41 RPRRPP六杆机构MATLAB仿真模型304.42 RPRRPP六杆机构MATLAB仿真结果33 总结36 参考文献37 致 谢38RPP平面连杆机构的运动仿真1引言在大学
7、四年学习，通过老师的讲解和自己的学习，收获了很多，深深的喜欢上了机械这个行业，对机械设计方面很是喜欢，我所研究的课题就是通过MATLAB做平面连杆RPP仿真，进一步加深了对平面连杆的研究。此次毕业设计，要求我对MATLAB软件有一定的认识，在加上我学完了机械原理，进行了生产实习之后，进行的一个重要的实践性环节。这要求我们把所学的机械原理和实践知识相结合，在实际的设计中综合地加以运用，这有助与提高了我们分析和解决实际问题的能力，为以后从事相关的技术工作奠定的基础。1.1平面连杆机构概述平面连杆机构是将各构件用转动副或移动副联接而成的平面机构。最简单的平面连杆机构是由四个构件组成的，简称平面四杆机
8、构。它的应用非常广泛，而且是组成多杆机构的基础。全部用回转副组成的平面四杆机构称为铰链四杆机构。仅能在某一角度摆动的连架杆，称为摇杆。对于铰链四杆机构来说，机架和连杆总是存在的，因此可按照连架杆是曲柄还是摇杆，将铰链四杆机构分为三种基本型式：曲柄摇杆机构、双曲柄机构和双摇杆机构。在实际机械中，平面连杆机构的型式是多种多样的，但其中绝大多数是在铰链四杆机构的基础上发展和演化而成。如曲柄滑块机构、导杆机构等。1.2杆组机构的从动件系统一般还可以进一步分解成若干个不可再分的自由度为零的构件组合，这种组合称为基本杆组，简称杆组。对于只含低副的平面连杆机构，若杆组中有N个活动构件、个低副，因杆组自由度为
9、零，故有：为保证n和 均为整数，n只能取偶数。根据n的取值不同，杆组可以分为以下几种情况。（1）n=2，的II级杆组II级杆组为最简单，也是应用最多的基本杆组。根据3个运动副（转动副用R表示，运动副用P表示）的不同组合，II级杆组分为5种，RRRII级杆组、 RRPII级杆组、 RPR II级杆组、PRPII级杆组和RPPII级杆组 。（2）n=4， 的III级杆组III级杆组特别是III级以上杆组早实际应用中较少，故在这里不再介绍。1.3机构的组成原理任何机构都可以看做是由若干个基本杆组依次联接于原动件和机构而构成的，者就是所谓机构的组成原理。把由最高级别为II级杆组的基本杆组构成的机构称为
10、II级机构，把最高级别为III级杆组的基本杆组构成的机构称为III级机构。其余类推。2 曲柄原动件、RPP平面连杆运动学数学模型的建立2.1曲柄原动件运动学分析2.11 曲柄原动件运动学数学模型的建立 如图2.1所示，在复数坐标系中，曲柄AB复向量的模rj为常数、幅角j为变量，通过转动副A与机架连接，转动副A的复向量的模ri为常量、幅角i为常量，曲柄AB端点B的位移、速度和加速度的推导如下：图2.1 曲柄的复数坐标系 (1.1)将方程(1.1)两边对时间t求两次导数得： （1.2）由式(1.2)写成矩阵形式有： (1.3)2.12曲柄MATLAB运动学仿真模块M函数根据式编写曲柄原动件MATL
11、AB的M函数如下：function y=crank(x)%Function to compute the acceleration of crank%Input parameters%x(1)=r1 （r1的杆长）%x(2)=theta-1 （r1的角位移）%x(3)=dtheta-1 （r1的角速度）%x(4)=ddtheta-1 （r1的角加速度）%Output parameters%y(1)=ReddB （转动副B的水平分量)%y(2)=ImddB (转动副B的垂直分量)%ddB=x(1)*x(4)*cos(x(2)+pi/2)+x(1)*x(3)2*cos(x(2)+pi); x(1)
12、*x(4)*sin(x(2)+pi/2)+x(1)*x(3)2*sin(x(2)+pi); y=ddB; 各构件的初值为：r1=0.4, theta-1=0, dtheta-1=10,ddtheta-1=0 。2.2 RPP四杆运动学分析2.21 RPP四杆运动学数学模型的建立如图2.2所示，在复数坐标系中，由1个转动副(B)、2个移动副(C,D)和2个滑块(C,D)组成RPPII级杆组，滑块C的滑动方向与滑块D的滑动方向的夹角j为常量，滑块D的幅度j也为常量，滑块C相对滑块D位移S i为变量，滑块D相对固定点K的位移也为变量，则滑块C相对滑块D的加速度和滑块D相对固定点K的加速度推导如下页图
13、：C=B=K+ （1.4）整理(1.4)得： (1.5) 图2.2 RPPII级杆组的位置参数式(1.5)对t求导并整理得到： (1.6)式(1.6)对时间t求导并整理得： (1.7)由(1.7)写成矩阵形式有 (1.8)RPPII级杆组MATLAB运动学仿真M函数function y =RPPki(x)% compute veloccity of RPP bar group% Input parameters% x(1) = theta - i (构件2的移动方向)% x(2) = theta - j (构件3的移动方向)% x(3) = ReddB （转动副B的水平加速度）% x(4) =
14、 ImddB （转动副B的垂直加速度）% x(5) = ReddK （转动副3的水平加速度）% x(6) = ImddK （转动副B的垂直加速度）% Output parameters% y(1) = dds - 2 （构件2的加速度）% y(2) = dds - 3 （构件3的加速度）%a = cos(x(1)+x(2) cos(x(2);sin(x(1)+x(2) sin(x(2);b = x(3)-x(5);x(4)-x(6);ddsij = inv(a) * b; y(1) = ddsij(1);y(2) = ddsij(2);各构件的初值为：theta-i=1.5708，theta-
15、j=0,ReddK=0,ImddK=0 。2.3 RPP四杆机构MATLAB运动仿真 1、 如图2.3所示为RPP四杆机构，它由原动件(曲柄1)和一个RPP杆组构成。构件的尺寸为r1=400mm，转动副A到移动副DE的距离r4=800mm，复数向量坐标见图上，构件1以等角速度10rad/s逆时针方向回转，试求构件2和构件3的速度和加速度?图2.3 RPP四杆机构2.31RPP四杆机构MATLAB仿真模型图中的各积分模块的初值是以曲柄1的幅度为0和角速度等于10rad/s逆时针方向回转，相应各个构件的位移和速度的瞬时值。theta-1=0；dtheta-1=10；theta-i=1.5708；s
16、-2=0；ds-2=0.8；s-3=0.89；ds-3=0.4。二个MATLAB函数模块分别为crank.m和rppki.m图2.4 RPP四杆机构Simunlink2.32 用MATLAB实现牛顿-辛普森求解方法图2.5 RPPII级杆组的位置参数 如图2.5所示，以复数形式表示为整理上式得：按欧拉公式展开得：上式展开整理得：将上式求出雅克比矩阵为：function y = RPPposi(x)% Script used to implement Newton - Raphson mechod for% solving nonlinear position of RPP bar group
17、% Input parameters% x(1) = theta-1 guess value （构件1的角位移）% x(2) = theta-2 guess value （构件2的角位移）% x(3) = theta-3 guess value （构件3的角位移）% x(4) = theta-4 guess value （构件4的角位移）% x(5) = s1 guess value （构件1的相对位移）% x(6) = s2 （ 构件2的相对位移）% x(7) = s3 guess value （构件3的相对位移）% x(8) = s4 guess value （构件4的相对位移）% Out
18、put parameters% y(1) = theta - 1 （构件1的角位移）% y(2) = theta - 2 （构件2的角位移）%theta1 = x(1);theta2 = x(2);%epsilon = 1.0E-6;%f=x(6)*cos(x(2)+x(5)*cos(x(1)+x(2)+x(7)*cos(x(3)-x(8)*cos(x(4);x(6)*sin(x(2)+x(5)*sin(x(1)+x(2)+x(7)*sin(x(3)-x(8)*sin(x(4);%while norm(f)epsilon J=-x(5)*sin(theta1+theta2) -x(6)*sin
19、(theta2)-x(5)*sin(theta1+theta2);x(5)*cos(theta1+theta2) x(6)*cos(theta2)+x(5)*cos(theta1+theta2); dth = inv(J)*(-1.0*f); theta1=theta1+dth(1); theta2=theta2+dth(2); f=x(6)*cos(theta2)+x(5)*cos(theta1+theta2)+x(7)*cos(x(3)-x(8)*cos(x(4);x(6)*sin(theta2)+x(5)*sin(theta1+theta2)+x(7)*sin(x(3)-x(8)*sin
20、(x(4); norm(f);endy(1)=theta1;y(2)=theta2;估计杆1和杆2的角位移为，则输入参数x=90*pi/180 0 0 63.435*pi/180 0.8 0.4 0 0.89，带入上面的函数，得到构件1和构件2的角位移分别为。function y = RPPvel(x)% Input parameters% x(1)= theta-1 （构件1的角位移）% x(2) =theta-2 （构件2的角位移）% x(3)= theta-3 （构件3的角位移）% x(4)= dtheta-1 （构件1的角速度）% x(5)= r1 （构件1的杆长）% x(6)= r2
21、 （构件2的杆长）% x(7)= r3 （构件3的杆长）% Output parameters% y(1)=dtheta-2% y(2)=dtheta-3%A = cos(x(1)+x(2) cos(x(2);sin(x(1)+x(2) sin(x(2);B = x(7)*cos(x(3);x(7)*sin(x(3);y = inv(A)*B;由位移分析计算出的和曲柄1的角速度及各个构件长度，则输入参数为 x=1.5831 -0.0099 0 10 0.8 0.4 0带入上面的函数得到构件1和构件2的角速度分别为。 2.4 RPP四杆机构MATLAB仿真结果曲柄1的幅度为0和角速度等于10ra
22、d/s逆时针方向回转，相应各个构件的位移和速度的瞬时值。theta-1=0；dtheta-1=10；theta-i=1.5708；s-2=0；ds-2=0.8；s-3=0.89；ds-3=0.4。由于曲柄转速为10rad/s，因此每转动1周的时间是0.628s,用绘画命令plot(tout,simout(:,3)，plot(tout,simout(:,4) ，plot(tout,simout(:,5)和plot(tout,simout(:,6)分别绘画出构件2和构件3的速度和加速度。见下图(a)构件2的速度(纵坐标表示构件2的速度,单位为；横坐标表示时间，单位为s)(b)构件3的速度(纵坐标表
23、示构件3的速度,单位为；横坐标表示时间，单位为s)(c)构件2的加速度(纵坐标表示构件2的加速度,单位为；横坐标表示时间，单位为s)(d)构件3的加速度(纵坐标表示构件3的加速度,单位为；横坐标表示时间，单位为s)3 RPRRPP六杆机构MATLAB运动学仿真3.1 RPRII级杆组MATLAB运动学仿真模块 如下图RPRII级杆组由2个转动副、一个移动副和导杆、滑块组成RPRII级杆组，导杆幅角为变量，滑块C相对D的移动S也为变量，则导杆角速度、滑块C相对D的加速度和导杆上点E的加速度推导如下 RPRII级杆组的位置参数 移项整理得 上式对时间t求导并整理得： 上式对时间t求导并整理得：把上
24、式写成矩阵的形式：同样可以推出点E的加速度矩阵方程形式： RPRII级杆组MATLAB运动学仿真模块M函数 function y = RPRki(x) % function to compute the acceleration for RPR bar group% Input parameters% x(1) = r-3 （构件3的杆长）% x(2) = theta-3 （构件3的角位移）% x(3) = s-2 （构件2的相对位移）% x(4) = dtheta-3 （构件3的角速度）% x(5) = ds-2 （构件2的相对速度）% x(6) = ReddB （转动副B的水平加速度）%
25、x(7) = ImddB （转动副B的垂直加速度）% x(8) = ReddD （转动副D的水平加速度）% x(9) = ImddD （转动副D的垂直加速度）% Output parameters% y(1) = ddtheta -3 （构件3的角加速度） % y(2) = dds-2 （构件2的相对加速度）% y(3) = ReddE （转动副E的水平加速度）% y(4) = ImddE （转动副E的垂直加速度)%a=x(3)*cos(x(2)+pi/2) cos(x(2);x(3)*sin(x(2)+pi/2) sin(x(2);b=-x(5)*cos(x(2)+pi/2)+x(3)*x(
26、4)*cos(x(2)+pi) x(4)*cos(x(2)+pi/2); x(5)*sin(x(2)+pi/2)+x(3)*x(4)*sin(x(2)+pi) x(4)*sin(x(2)+pi/2);b = b*x(4);x(5)+x(6)-x(8);x(7)-x(9);ddths = inv(a)*b;dde = x(8);x(9)+x(1)*ddths(1)*cos(x(2)+pi/2);x(1)*ddths(1)*sin(x(2)+pi/2)+x(1)*x(4)2*cos(x(2)+pi);x(1)*x(4)2*sin(x(2)+pi);y(1) = ddths(1);y(2) = dd
27、ths(2);y(3) = dde(1);y(4) = dde(2);各构件的初值为：r-3=1.6,theta-3 =1.5708,s-2 =1.4,dtheta-3 =2.8571,ds-2 =0,ReddD =0,ImddD=0 。3.2 RPRRPP六杆机构图右所示是由原动件(曲柄1)和 一个RPR杆组、RPP杆组所组成的RPRRPP六杆机构，各构件的尺寸为r1=400mm，r3=1600mm，AD=1000，复数向量坐标见图所示，转动副A到滑块5的滑道的垂直距离为800mm,构件1以等角速度10rad/s逆时针方向回转，试求构件4相对构件5的位移、速度、加速度和构件5的位移、速度、加
28、速度。图 RPRRPP六杆机构3.21 RPRRPP六杆机构MATLAB仿真模型RPRRPP六杆机构的MATLAB仿真模型如下图所示，在图中各积分模块的初值是以曲柄1的幅角为90和角速度等于10rad/s逆时针方向回转，相应各构件的位移、速度的瞬时值分别为dtheta-1=10；theta-1=1.5708；ds-2=0;s-2=1.4；dtheta-3=2.8571；theta-3=1.5708；ds-4=0；s-4=0.4。ds-5=-4.5714；s-5=0 。3个MATLAB函数模块分别为crank.m、rprki.m和rppki.m。RPRRPP六机构Simulink仿真模型3.22
29、 RPRRPP杆组M函数为：function y = crank(x)% Function to compute the acceleration of crank% Input parameters% x(1) = r-1 （构件1的杆长）% x(2) = theta-1 （构件1的角位移）% x(3) = dtheta-1 （构件1的角速度）% x(3) = ddtheta-1 （构件1的角加速度）% Output parameters% y(1) = ReddB （转动副B加速度的水平分量）% y(2) = ImddB （转动副B加速度的垂直分量）%ddB=x(1)*x(4)*cos(x
30、(2)+pi/2)+x(1)*x(3)2*cos(x(2)+pi);x(1)*x(4)*sin(x(2)+pi/2)+x(1)*x(3)2*sin(x(2)+pi);y = ddB;function y =RPPki(x)% compute veloccity of RPP bar group% Input parameters% x(1) = theta - s4 （构件4的角位移）% x(2) = theta - s5 （构件5的角位移）% x(3) = ReddE （转动副E加速度的水平分量）% x(4) = ImddE （转动副E加速度的垂直分量）% x(5) = ReddK （转动副
31、K加速度的水平分量）% x(6) = ImddK （转动副K加速度的水平分量）% Output parameters% y(1) = dds - 4 （构件4的加速度）% y(2) = dds - 5 （构件5的加速度）%a = cos(x(1)+x(2) cos(x(2);sin(x(1)+x(2) sin(x(2);b = x(3)-x(5);x(4)-x(6);ddsij = inv(a) * b; y(1) = ddsij(1);y(2) = ddsij(2);3.23 RPRRPP六杆机构MATLAB仿真结果曲柄1的幅角为和角速度等于10rad/s逆时针方向回转，相应各构件的位移、速
32、度的瞬时值分别为dtheta-1=10；theta-1=1.5708；ds-2=0;s-2=1.4；dtheta-3=2.8571；theta-3=1.5708；ds-4=0；s-4=0.4。ds-5=-4.5714；s-5=0 由于曲柄转速为10rad/s，因此每转动1周的时间是0.628s，用绘画命令plot(tout,simout(:,1)，plot(tout,simout(:,2)，plot(tout,simout(:,3)，plot(tout,simout(:,4)plot(tout,simout(:,5)，plot(tout,simout(:6)绘制构件4相对构件5的位移、速度、加
33、速度和构件5的位移、速度、加速度，如下图：（a）构件4相对构件5的位移(纵坐标表示构件4相对构件5的位移,单位为m；横坐标表示时间，单位为s)（b）构件5的位移(纵坐标表示构件5的位移,单位为m；横坐标表示时间，单位为s)（c）构件4相对构件5的速度(纵坐标表示构件4相对构件5的速度,单位为m/s；横坐标表示时间，单位为s)（d）构件5的速度(纵坐标表示构件5的速度,单位为m/s；横坐标表示时间，单位为s)（e）构件4相对构件5的加速度(纵坐标表示构件4相对构件5的加速度,单位为；横坐标表示时间，单位为s)（f）构件5的加速度(纵坐标表示构件5的加速度,单位为m/s2；横坐标表示时间，单位为s
34、)4 曲柄、RPP杆组的MATLAB动力学分析4.1 曲柄MATLAB动力学仿真模块4.11曲柄的动力学矩阵表达式如图4.1,已知曲柄AB向量的模为常量，幅角为常量，质点到转动副A的距离为，质量为，绕质点的转动惯量为，作用于质点上的外力为和，外力矩为，曲柄与机架联接，转动副A的约束反力为和，驱动力矩为。图4.1由理论力学可得：由运动学知识可推出得：将上面的式子合并整理得： 曲柄MATLAB动力学仿真模块M函数：根据上式编写曲柄原动件MATLAB的M函数如下：function = crankdy_3(x)% Dynamic analysis for arank% Input parameters
35、% x(1) = theta - 1 （构件1的角位移）% x(2) = dtheta - 1 （构件1的角速度）% x(3) = ddtheta - 1 （构件1的角加速度）% x(4) = -RxB （转动副B反作用力的水平分量）% x(5) = -RyB （转动副B反作用力的垂直分量）% % Output parameters% y(1) = RxA （转动副A反作用力的水平分量） % y(2) = RxB （转动副A反作用力的垂直分量）% y(3) = M1 （曲柄1上的驱动力矩）%g = 9.8;ri = 0.4; rci = 0.2;mi = 1.2; Ji = 0.016;Fxi
36、 = 0; Fyi = 0; Mi = 0;Redda = 0; Imdda = 0;y(1) = mi * ReddA + mi * rci * x(3) * cos(x(1) + pi/2) + mi * rci * (2)2*cos(x(1) + pi)-Fxi + x(4);y(2) = mi * ImddA + mi * rci * x(3) * sin(x(1) + pi/2) + mi * rci * (2)2*sin(x(1) + pi)-Fxi + x(5) + mi * g;y(3) = Ji * x(3) - y(1) * rci * sin(x(1)+y(2) * rc
37、i * cos(x(1) - x(4) * (ri-rci) * sin(x(1) + x(5) * (ri -rci) * cos(x(1)-Mi;各构件的初值为：theta - 1=1.5708,dtheta - 1=10, ddtheta - 1=0 。4.2 RPRII级杆组的动力学仿真模块4.21 RPRII级杆组动力学矩阵表达式RPRII级杆组由2个构件滑块和导杆组成。滑块的质量为，导杆的质量为， 转动惯量为。滑块和导杆的受力分析图如下所示，则转动副B和E及移动副C的约束反力推导如下。 由图b受力分析得：由图c受力分析得：由动力学可推出得：将上式分别代入得：将上式整理成矩阵为：4.
38、22 RPRII级杆组MATLAB动力学仿真模块M函数根据上式编写RPRII级杆组MATLAB的M函数如下：function y =RPRdy_2(x)% Dynamic analysis of RPR bar group% Input parameters% x(1) = theta-3 （构件3 的角位移）% x(2) = s-2 （构件2的相对位移）% x(3) = dtheta-3 （构件3 的角速度）% x(4) = ddtheta-3 （构件3 的角加速度）% x(5) = ReddB （转动副B加速度的水平分量）% x(6) = ImddB （转动副B加速度的垂直分量）% x(7
39、) = -RxE （转动副E的约束反力的水平分量）% x(8) = -RyE （转动副E的约束反力的垂直分量）% x(9) = M3 （构件3上的力矩）% % Output parameters% y(1) = RxB （运动副B的约束反力的水平分量）% y(2) = RyB （运动副B的约束反力的垂直分量）% y(3) = RC （移动副C的约束反力）% y(4) = RxD （移动副D的约束反力的水平分量）% y(5) = RyD （移动副D的约束反力的垂直分量）%g = 9.8;rcj = 0.8;mi = 1.5; mj = 10;Jj = 2.2;ReddD=0;ImddD=0;Fx
40、i = 0; Fyi = 0;a = zeros(5);a(1,1) = 1; a(1,3) = -sin(x(1);a(2,2) = 1; a(2,3) = cos(x(1);a(3,3) = sin(x(1); a(3,4) = 1;a(4,3) = -cos(x(1); a(4,5) = 1;a(5,3) = - (x(2) - rcj);a(5,4) = -rcj*sin(x(1);a(5,5) = -rcj*cos(x(1);b = zeros(5,1);b(1,1) = mi*x(5) - Fxi;b(2,1) = mi*x(6) + mi*g-Fyi;b(3,1) =mj*(ReddD+rcj*x(4)*cos(x(1)+pi/2)+rcj*x(3)2*cos(x(1)+pi)+x(7);b(4,1) =mj*(ImddD+rcj*x(4)*sin(x(1)+pi/2)+rcj*x(3)2*sin(x(1)+pi)+x(8)+mj*gb(5,1) = Jj*x(4)-x(9);y = inv(a) * b;各构件的初值为：theta-3=1.5708,s-2 =1.4,dtheta-3=2.8571 。 4.3 RPPII级杆组的动力学仿真模块4.31 RPPI
