基本杆组-运动学仿真课件

1、第第5章章 运动学仿真运动学仿真 Simulink的使用的使用 从基本杆组理论为基础建立模型jrjiri在复数坐标系中，曲柄AB复向量的模为常数、幅角为变量，通过转动副A与机架联接，转动副A的复向量的模为常量，幅角为常量。5-1 曲柄的MATLAB运动学仿真模块曲柄AB的端点B位移、速度和加速度推导如下ijiireArjjjjr erjijjjijrer eBA+ r 1曲柄的运动学矩阵表达式5-1 曲柄的MATLAB运动学仿真模块ijiireArjjjjr erjijjjijrer eBA+ r， (511)对时间分别求两次导数 (511) 22jjjjjjrerejjB(512) 写成矩阵

2、形式有 22coscosRe2Imsinsin2jjjjjjjjjjjjrrrrBB2jjjrejB(512) 编写曲柄原动件MATLAB的M函数如下 function y=crank(x)%Function to compute the accleration of crank%Input parameters%x(1)=rj%x(2)=thetaj%x(3)=dthetaj%x(4)=ddthetaj%0utput parameters%y(1)=ReddB%y(2)=ImddBddB=x(1)*x(4)*cos(x(2)+pi/2)+x(1)*x(3)2*cos(x(2)+pi); x(

3、1)* x(4)*sin(x(2)+pi/2)+x(1)*x(3)2*sin(x(2)+pi); y=ddB;5-1 曲柄的MATLAB运动学仿真模块2.曲柄MATLAB运动学仿真模块M函数也可以改写为如下 function y=crank(lgth,theta)%Function to compute the accleration of crank%Input parameters%lgth=rj%theta(1)=thetaj% theta(2)=dthetaj% theta(3)=ddthetaj%0utput parameters%y(1)=ReddB%y(2)=ImddB%ddB=

4、lgth* theta(3)*cos(theta(1)+pi/2)+lgth* theta(2)2*cos(theta(1)+pi); lgth* theta(3)*sin(theta(1)+pi/2)+lgth* theta(2)2*sin(theta(1)+pi); y=ddB;5-1 曲柄的MATLAB运动学仿真模块 5-2 RRR II级杆组MATLAB运动学仿真模块 1RRR II级杆组运动学矩阵表达式irjrij在复数坐标系中，由3个转动副（B，C，D）和2个构件（长度分别为，）组成RRR II级杆组，2个构件的幅角和为变量，则点c的加速度推导如下 jiijrer ejjCB+Dj

5、iijrer ejjDB(514) (515) 对式(215)求导数并整理得 22jiiijjrer ejjDB(516) 5-2 RRR II级杆组MATLAB运动学仿真模块 1RRR II级杆组运动学矩阵表达式22jiiijjrer ejjDB对方程式(516)求导数并整理得(516) (517) (518) 22jjiiiiijijjjrer erer e jjjjDB写成矩阵形式有22cos2cos2sin2sin2coscosReReImImsinsiniijjijiijjiijjijiijjrrrrrrrrDBD-BcosReRecosImImiiiirrCB+CBcos2ReRe

6、sin2ImImiiiiirrBC+BC2cos2cosReResin2sinImImiiiiiiiiiirrrrBC+BC将式（5.14）写成矩阵形式：式（5.19）对时间t求导数，得到点c的速度为再次对时间t求导数，得到点c的加速度为 5-2 RRR II级杆组MATLAB运动学仿真模块 1RRR II级杆组运动学矩阵表达式jiijrer ejjCB+D(514) (519) (520) (521) 根据式(518)和式(521)编写RRRII级杆组的M函数如下： 5-2 RRR II级杆组MATLAB运动学仿真模块 function y=RRRki(x)%function to comp

7、ute the acceleration for RRR bar group%Input parameters%x(1)=ri%x(2)=rj%x(3)=theta-i%x(4)=theta-j%x(5)=dtheta-i%x(6)=dtheta-j%x(7)=ReddB%x(8)=ImddB%x(9)=ReddD%x(10)=ImddD%Output parameters%y(1)=ddtheta-i%y(2)=ddtheta-j%y(3)=ReddC%y(4)=ImddCa=x(1)*cos(x(3)+pi/2) -x(2)*cos(x(4)+pi/2); x(1)*sin(x(3)+pi

8、/2) -x(2)*sin(x(4)+pi/2);b=-x(1)*cos(x(3)+pi) x(2)*cos(x(4)+pi) -x(1)*sin(x(3)+pi) x(2)*sin(x(4)+pi)*x(5)2;x(6)2+x(9)-x(7);x(10)-x(8);ddth=inv(a)*b;y(1)=ddth(1);y(2)=ddth(2);y(3)=x(7)+x(1)*ddth(1)*cos(x(3)+pi/2)+x(1)*x(5)2*cos(x(3)+pi);y(4)=x(8)+x(1)*ddth(1)*sin(x(3)+pi/2)+x(1)*x(5)2*sin(x(3)+pi);(5

9、18) 2cos2cosReResin2sinImImiiiiiiiiiirrrrBC+BC(521) 5-2 RRR II级杆组MATLAB运动学仿真模块 22cos2cos2sin2sin2coscosReReImImsinsiniijjijiijjiijjijiijjrrrrrrrrDBD-B图210所示的铰链四杆机构，它由原动件(曲柄1)和1个RRR杆组构成。各构件的尺寸为 示例1 铰链四杆机构的运动学仿真 1r2r3r4r=400mm，=1000mm，700mm, 建立如图210所示复数向量坐标，构件1以等角速度10 rad/s逆时针方向回转，试求点C的加速度、构件2，3的角加速度。

10、 =1200 mm。1、求解初始位置各个构件的幅角、角速度。（详细方法见前面所分析）2、利用前面编制的曲柄（crank.m）和RRR II级杆组（RRRki.m）的M函数，用simulink建立四杆机构模型。3、设置各模块参数以及仿真参数。 示例1 铰链四杆机构的运动学仿真 simulink环境下所建立的铰链四杆机构的仿真模型 theta -31stheta -21stheta -11sr30.7r21r10.4dtheta -31sdtheta -21sdtheta -11sddtheta -10crank.mMATLABFunctionTo WorkspacesimoutReddD0RRR

11、ki.mMATLABFunctionImddD 0ImddDReddDImddBReddBImddCReddCddtheta-3ddthta-2dtheta-2dtheta-2dtheta-2theta-2theta-2theta-2dtheat-3dtheat-3dtheat-3theta-3theta-3theta-3simulink环境下所建立的铰链四杆机构的仿真模型 示例1 铰链四杆机构的运动学仿真 各个积分模块的初始值设置：曲柄1的幅角为0。角速度为10(rad/s)。其它构件的初值为前面所分析。 示例1 铰链四杆机构的运动学仿真 也可以考虑用MATLAB实现，将曲柄原动件的M函数文

12、件以及RRRII级杆组的M函数文件改写实现。做为本次课程的第一次大作业滑块C相对固定点K的位移 为变量， 和滑块C组成RRP II级杆组，构件 的幅角 5-3 RRP II级杆组MATLAB运动学仿真模块 1RRP II级杆组运动学矩阵表达式iririjs在复数坐标系中，由2个转动副（B，C），1个移动副C和构件BC）为变量，则点c的加速度推导如下 (522) (523) 对式(523)求导数并整理得 (524) （长度为滑块C的导路方向的幅角j为常量， jiiresejjCB+KjiiresejjK - B2jiiireesjjKB式(524)对时间t求导数并整理，得 220jiiiiiir

13、eeress jjjKB2cos2coscos0ReResin2sin0 sin0ImImiijiiiiiijjiirrrrrss KBK -B写成矩阵形式 2jiiireesjjKB(524) (526) 根据式(526)编写的RRP II级杆组M函数如下： 5-3 RRP II级杆组MATLAB运动学仿真模块 function y=RRPki(x)%function to compute the acceleration for RRP bar group%Input parameters%x(1)=ri%x(2)=theta-i%x(3)=theta-j%x(4)=dtheta-i%x(

14、5)=ReddB%x(6)=ImddB%x(7)=ReddK%x(8)=ImddK%x(9)=ds%Output parameters%y(1)=ddtheta-i%y(2)=dds%a=x(1)*cos(x(2)+pi/2) -cos(x(3);x(1)*sin(x(2)+pi/2) -sin(x(3);b=-x(1)*cos(x(2)+pi) 0; x(1)*sin(x(2)+pi) 0*x(4)2;x(9)+x(7)-x(5);x(8)-x(6);y=inv(a)*b; 5-3 RRP II级杆组MATLAB运动学仿真模块 图示曲柄滑块机构，它由原动件(曲柄1)和1个RRP杆组构成。各构

15、件的尺寸为 示例2曲柄滑块机构的运动学仿真 1r2r=400mm，构件1以等角速度10 rad/s逆时针方向回转，试求构件2的角速度和角加速度以及点C的速度和加速度。 =1200mm，复数向量坐标如图所示， 示例2曲柄滑块机构的运动学仿真 theta -s0theta -21stheta -11ss1sr21.2r10.4dtheta -21sdtheta -11sds1sddtheta -10crank.mMATLABFunctionTo WorkspacesimoutReddD0RRPki.mMATLABFunctionImddK0ImddKReddKImddBReddBddthta-2d

16、theta-2dtheta-2dtheta-2theta-2theta-2theta-2ddssdsdsds曲柄滑块机构的simulink仿真模型 用plot(tout,simout(:,7)，plot(tout,simout(:,8)，plot(tout,simout(:,5)和plot(tout,simout(:,6)，分别绘制出构件2的角速度和角加速度以及点c的速度和加速度，如图所示。 示例2曲柄滑块机构的运动学仿真 请同学们将这些结果与先前的分析结果进行比较。 示例3曲柄-RRR-RRR机构的运动学仿真 图中机构是由原动件(曲柄1)和2个RRR杆组组成的六杆机构。各构件尺寸为r1=26

17、5mm，r2105.6mm，r3675mm，r4875mm，r5=65mm，r648mm,复数向量坐标如图所示，构件l以等角速度10 rad/s逆时针方向回转，试求点c和点E的加速度以及构件5，6的角速度和角加速度。 示例3曲柄-RRR-RRR机构的运动学仿真 1、仿真模型的建立将机构进行拆杆组分解。曲柄AB+第一个RRR杆组（BCD）+第二个RRR杆组（CEF）使用3个MATLAB函数模块，分别为crankm，rrrki_1m和rrrki_2m. crank.m函数模块的输入参数为曲柄AB的角位移、角速度和角加速度，输出参数曲柄端部(转动副B)的加速度的水平分量和垂直分量。rrrki_1m函

18、数模块的输入参数为构件2（BC）和构件3（CD）的角位移、角速度和转动副B的加速度。rrrki_2m函数模块的输入参数为构件5和构件6的角位移、角速度和转动副c的加速度。 示例3曲柄-RRR-RRR机构的运动学仿真 ddtheta -4ddtheta -2theta -51stheta -41stheta -31stheta -21stheta -11sr648e-3r565e-3r3-C-r2-C-r1-C-dtheta -51sdtheta -41sdtheta -31sdtheta -21sdtheta -11sddtheta -10crank.mMATLABFunctionTo Wor

19、kspace3Ro_ETo Workspace2Ro_CTo Workspace1simout1To WorkspacefirstReal -Imag toComplex 1ReImReal -Imag toComplexReImReddF0ReddD0RRRki.m1MATLABFunctionRRRki.mMATLABFunctionImddF0ImddD0ClockImddDReddDImddBReddBImddCImddCImddCReddCReddCReddCddtheta-3dtheta-2dtheta-2dtheta-2theta-2theta-2theta-2dtheat-3d

20、theat-3dtheat-3theta-3theta-3theta-3dtheta-4dtheta-4dtheta-4dtheta-5dtheta-5dtheta-5theta-5theta-5theta-5theta-5ReddFImddFtheta-4theta-4theta-4theta-4ddtheta-5ddtheta-5ImddEImddEReddEReddE 示例3曲柄-RRR-RRR机构的运动学仿真 示例3曲柄-RRR-RRR机构的运动学仿真 2、确定初始条件 format long x1=60*pi/180 25*pi/180 80*pi/180 26.5 105.6 67

21、.5 87.5 ; y1=rrrposi(x1)y1 = 0.392526704397405 1.218065937697697首先对ABCD部分进行初始初始位移分析：在MATLAB中执行 x2=60*pi/180 y1(1) y1(2) 10 26.5 105.6 67.5 87.5; y2=rrrvel(x2)y2 = -0.580620100031004 3.252748641369780 在此基础上，结合曲柄1的角速度10 rad/s及各个构件长度，求解ABCD部分初始速度，则输入参数为接着，对DCEF部分进行初始位移分析： 示例3曲柄-RRR-RRP机构的运动学仿真 2、确定初始条件

22、 注意到，DF与x轴的夹角，为=atan(70/(150-87.5)=0.841941600342266 x3=y1(2)-atan(70/(150-87.5) -38*pi/180-atan(70/(150-87.5) -75*pi/180-atan(70/(150-87.5) 67.5 65 48 sqrt(150-87.5)2+702) ; y3=rrrposi(x3)y3 = -2.004993648749959 -2.548510072268813 在此基础上，结合摇杆CD的角速度3.2527rad/s及各个构件长度，求解DCEF部分个构件的初始速度，则输入参数为： x4=y1(2)

23、-atan(70/(150-87.5) y3(1) y3(2) y2(2) 87.5 65 48 sqrt(150-87.5)2+702) y4=rrrvel(x4)y4 = -3.180454556295345 -7.163740667553483 3RRR-RRR六杆机构MATLAB仿真结果 RRRRRP六杆机构的Simulink运动学仿真 图2.25是由原动件(曲柄1)和1个RRR杆组、BBP杆组所组成的RRRRRP六杆机构，各构件的尺寸为r1=400mm，r2=1000mm，r3=700mm，r4=1200mm，r5=1200mm，复数向量坐标如图225所示，构件l以等角速度10 ra

24、ds逆时针方向回转，试求点c的加速度、构件3的角加速度、构件6的速度、加速度及构件5的角速度和角加速度。 占期末成绩的15%，于下周5之前递交纸质文档(doc文档打印），并将电子文档打包，用学号命名。发送.和滑块（C）组成RPR II级杆组，导杆的幅角 5-4 RPR II级杆组MATLAB运动学仿真模块 1RPR II级杆组运动学矩阵表达式 jrjsj s如图26所示，在复数坐标系中，由2个转动副（B，D），1个移动副（C）；导杆DE（长度为 ）为变量，滑块C也是变量，、滑块C的；和导杆上点 相对固定点D的位移则导杆角加速度相对点D的加速度E的加速度推导如下： jsejCDBjsejBD对时

25、间t求导数并整理 2jjjseesjjBD 5-4 RPR II级杆组MATLAB运动学仿真模块 222jjjjjjjjjseeseseess jjjjjBD写成矩阵形式，有cos2cossinsin2cos2coscos2ReReImImsin2sinsin2jjjjjjjjjjjjjjjjssssssssBDBD再次求导数并整理 2cos2cosReReImImsin2sinjjjjjjjjjjrrrrED+ED同样可得：(530) (531) 5-4 RPR II级杆组MATLAB运动学仿真模块 2RPR II级杆组MATLAB运动学仿真模块M函数 function y=RPRki(x)

26、%function to compute the acceleration for RPR bar group%Input parameters%x(1)=rj%x(2)=theta-j%x(3)=s%x(4)=dtheta-j%x(5)=ds%x(6)=ReddB%x(7)=ImddB%x(8)=ReddD%x(9)=ImddD%Output parameters%y(1)=ddtheta-j%y(2)=dds%y(3)=ReddE%y(4)=ImddE 5-4 RPR II级杆组MATLAB运动学仿真模块 a=x(3)*cos(x(2)+pi/2) cos(x(2); x(3)*sin(x

27、(2)+pi/2) sin(x(2);b=-x(5)*cos(x(2)+pi/2)+x(3)*x(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)*cos(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)*

28、x(4)2*sin(x(2)+pi);y(1)=ddths(1);y(2)=ddths(2);y(3)=dde(1);y(4)=dde(2);2RPR II级杆组MATLAB运动学仿真模块M函数 示例4摆动导杆机构MATLAB运动学仿真 图216所示为摆动导杆机构，它由原动件(曲柄1)和1个RPR杆组构成。各构件的尺寸为r1=400mm，r3=1600mm，r4=1000mm，复数向量坐标如图216所示构件1以等角速度10rads逆时针方向回转，试求构件3的角速度和角加速度以及滑块的相对速度和相对加速度。 1、仿真模型的建立 示例4摆动导杆机构MATLAB运动学仿真2个MATLAB函数模块分别

29、为crankm和rrpkim，其中crankm函数模块的输人参数为曲柄的长度、曲柄的角位移、曲柄的角速度和角加速度、输出参数曲柄端部（转动副B）的加速度的水平分量和垂直分量；rppki.m函数模块的输入参数为导杆的长度、导杆的角位移、导杆的角速度、滑块的相对位移、滑块的相对速度、转动副B的加速度和转动副D的加速度。各个参数都标注在相应数据线上,各个构件的尺寸长度单位为m，角度单位为rad。仿真结果输出到工作空间变量simout，设置仿真时间为1s，求解器选用ode45，步长选用变步长。 各积分模块的初值是以曲柄1的幅角为0和角速度等于10rads逆时针方向回转时，相应各个构件的位移和速度的瞬时

30、值，ddtheta -3ddtheta 3theta -31stheta -11ss1sr31.6r10.4dtheta -31sdtheta -11sds1sddtheta -10crank.mMATLABFunctionTo WorkspacesimoutReddD 0RPRki.mMATLABFunctionImddD0ImddDImddBReddBddsddsdtheta3dtheta3dtheta3dsdsdssssReddDReddDReddEImddE 示例4摆动导杆机构MATLAB运动学仿真 示例4摆动导杆机构MATLAB运动学仿真2MATLAB仿真结果 5-5 RPP II级

31、杆组MATLAB运动学仿真模块 isjsij如图28所示，在复数坐标系中，由1个转动副(B)、2个移动副(C，D)和2个滑块(C，D)组成RPP II级杆组，滑块C的滑动方向与滑块D的滑动方向的夹角均为变量，为常量， 滑块D的幅角也为常量 。滑块C相对滑块D位移 ，滑块D相对固定点K的位移 则滑块C相对滑块D的加速度和滑块D相对固定点K的加速度推导如下 ijjjis esejjCBKijjjis es ejjBK写成矩阵形式，有求两次导数并整理 (541) 5-5 PRP II级杆组MATLAB运动学仿真模块 ijjiijseesjj= BKcoscosReReImImsinsinijjiji

32、jjssBK=BK2RPP II级杆组MATLAB运动学仿真模块M函数 function y=RPPki(x)a=cos(x(1)+x(2)*cos(x(2) cos(x(2); sin(x(1)+x(2) sin(x(2);b=-x(3)-x(5);x(4)-x(6);b=b*x(4);x(5)+x(6)-x(8);x(7)-x(9);ddsij=inv(a)*b;y(1)=ddsij(1);y(2)=ddsij(2); 2RPP II级杆组MATLAB运动学仿真模块M函数 5-5 RPP II级杆组MATLAB运动学仿真模块 function y=RPPki(x)%function to

33、compute the acceleration for RPP bar group%Input parameters%x(1)=theta-i%x(2)=theta-j%x(3)=ReddB%x(4)=ImddB%x(5)=ReddK%x(6)=ImddK%Output parameters%y(1)=dds-i%y(2)=dds-j%y(3)=ReddD%y(4)=ImddD 示例5RPP机构（正弦机构）Matlab运动学仿真图219所示为RPP四杆机构，它由原动件(曲柄1)和1个RPP杆组构成。构件的尺寸为r1=400mm，转动副A到移动副DE的距离r4800mm，复数向量坐标如图219

34、所示。构件1以等角速度10rads逆时针方向回转，试求构件2和构件3的速度和加速度。 1、仿真模型的建立2个Matlab函数模块分别为crank.m和rppkim，其中crankm函数模块的输入参数为曲柄的长度、曲柄的角位移、曲柄的角速度和曲柄的角加速度、输出参数曲柄端部(转动副B)的加速度的水平分量和垂直分量；rppkim函数模块的输入参数为构件2和构件3移动方向、转动副B的加速度和构件3的加速度参考值。每个数据线上标注了相应变量，常量模块放置了各个构件的尺寸，长度单位为m，角度单位为rad,设置仿真时间为1s，仿真结果输出到工作空间变量simout，求解器选用ode45，步长选用变步长。

35、各积分模块的初值是以曲柄1的幅角为0和角速度等于10rad/s：逆时针方向回转时，相应各个构件的位移、速度的瞬时值。 示例5RPP机构（正弦机构）Matlab运动学仿真s-5dds-5theta -11ss-51ss-41sr40r2-C-r10.4dtheta -11sds-51sds-41sddtheta -10crank.mMATLABFunctionTo Workspace1simoutReddK 0RPPki.m1MATLABFunctionImddK0ReddBReddBImddBImddBdds-4dds-4ds-5ds-5s-4ds-4ds-4 示例5RPP机构（正弦机构）Ma

36、tlab运动学仿真 示例5RPP机构（正弦机构）Matlab运动学仿真2MATLAB仿真结果 图228所示是由原动件(曲柄1)和1个RPR杆组、RPP杆组所组成的RPRRPP六杆机构，各构件的尺寸为r1400mm，r31600，AD1000mm，复数向量坐标如图 228所示。转动副A到滑块5的滑道的垂直距离为800mm，构件1以等角速度10 rad/s逆时针力向回转，试求构件4相对构件5的位移、速度、加速度和构件5的位移、速度、加速度。 示例6RPRRPP六杆机构Matlab运动学仿真RPRRPP六杆II级机构的MATLAB仿真模型中,3个MATLAB函数模块分别为crankm, rprkim

37、，和rppki.m，其中crankm函数模块的输人参数为曲柄的长度、曲柄的角位移、曲柄的角速度和曲柄的角加速度、输出参数曲柄端部(转动副B)的加速度的水平分量和垂直分量。rprkim函数模块的输入参数为构件3的杆长、角位移和角速度，构件2的相对位移和相对速度以及转动副B和转动副D的加速度。输出参数为构件3的角加速度、构件2的相对加速度和转动副E的加速度。rppkim函数模块的输入参数为构件4和构件5的运动方向、转动副E的加速度和构件5(滑块)的加速度参考值，输出参数为构件4的相对加速度和构件5的加速度。每个数据线上标注了相应变量，常量模块放置了各个构件的尺寸，长度单位为m，角度单位为rad。设

38、置仿真时间为1s仿真结果输出到工作空间变量simout*中，求解器选用od45，步长选用变步长。 示例6RPRRPP六杆机构Matlab运动学仿真ddtheta -3ddtheta 3s-5dds-5theta -31stheta -11ss-51ss-41ss1sr40r31.6r2-C-r10.4dtheta -31sdtheta -11sds-51sds-41sds1sddtheta -10crank.mMATLABFunctionTo Workspace2Ro_ETo Workspace1simoutTo Workspacesimout1Real -Imag toComplexReIm

39、ReddK0ReddD0RPRki.mMATLABFunctionRPPki.m1MATLABFunctionImddK0ImddD0ImddDImddBReddBddsddsdtheta3dtheta3dtheta3dsdsdssssReddDReddDReddEReddEImddEImddEdds-4dds-4ds-5ds-5s-4ds-4ds-4各积分模块的初值是以曲柄1的幅角为90和角速度等于10 rads逆时针方向回转时，相应各个构件的位移、速度的瞬时值，分别为dthet-110;theta-115708;ds-20；s-214；dtheta-3=28571;theta-315708

40、; ds-40; S-40.4。 示例6RPRRPP六杆机构Matlab运动学仿真 示例6RPRRPP六杆机构Matlab运动学仿真移动副B滑动方向的幅角 、 5-6 PRP II级杆组MATLAB运动学仿真模块 irjriKisjKjsij如图27所示，在复数坐标系中，由2个移动副（B，D），1个转动副（C）和2个导杆（长度为 ）和组成PRP II级杆组，的位移 、移动副D相对动点的位移 、移动副D滑动方向幅角则2个移动副(B，D)滑动的加速度推导如下： 移动副B相对动点均为变量，22ijjiiiijjjseres er ejjjjCKK22ijjiiiijjjsreeeesr jjjj=KK写成矩阵形式，有求两次导数并整理