《南航非线性大作业.docx》由会员分享，可在线阅读，更多相关《南航非线性大作业.docx（10页珍藏版）》请在三一文库上搜索。
1、课程 : 非线性系统控制题目 : 二阶机械臂的非线性反馈设计姓 名：刘洋 洋学 号： SX1203179专 业：导航制导与控制2013 年 6 月 28 日一、被控对象动力学模型被控对象为两杆机械臂，1 为第一级杆转角，2为第二级杆相对第一级杆的转角， PxPy 为负载位置坐标，12 。系统的动力学模型如下：输出方程：Pxl1 cos1l2 cos(12 )Pyl1 sin1l2 sin(12 )PxJ()1Py2J ()l1 sin( 1 )l 2 sin(12 )l2 sin(12 )l1 cos( 1 )l2 cos( 12 )l2 cos( 12 )状态方程：M ()N (, )G (
2、)其中：M ()a1a2 cos( 2 )a3(a2 / 2)cos(2 )a3(a2 / 2)cos(2 )a3mJT ( )J ( )N ( , )a2 sin2 (1 222/ 2)mJ T ( )J ( , )a2 sin 212 / 2G(a4 cos 1a5 cos( 12 )mJT () g)a5 cos( 12 )式中部分相关的参数为：12122, a2m2l1l2, a312a1mlm2l2m2l1m l231 1323a1 m glm gl, a1 m gl,4211215222m115.19kg, m211.36kg,l1l 20.432m, m10kg, g 9.81二
3、、被控对象反馈线性化取状态量 x11 , x22 , x31 , x42 ，D (x)M ( ) ，E (x)N ( , ) G ( ) ，u1 , 2T， yPx , PyT系统化为：x1x2x3x4y1y2x30u1x4D1( x)u2D 1( x) E( x)l1 cosx1l2 cos(x1x2 )l1 sin x1l2 sin(x1x2 )f3 ( x)D 1( x) E( x) ，令f4 ( x)g31 ( x)g32 (x)D 1( x) ，g41 ( x)g42 (x)h1 ( x)l1 cos x1l 2 cos(x1x2 )h2 ( x)l1 sin x1l 2 sin(
4、x1x2 )可得：L f h1Lg1 h1Lg2 h1L2f h1Lg1 L fLg2 L fl1 sin x1 x3l2 sin( x1x2 )( x300l1cos x x2l2cos(xx )( x13123h1l1 sin x1g31 (x)l 2 sin(x1h1l1 sin x1g32 (x)l2sin(x1x4 )x4)2 l1sin xf3(x)l2sin(xx )( f3( x)f4( x)112x2 )( g31( x)g41 (x)x2 )( g32 ( x)g42( x)L f h2 l1 cos x1x3l 2 cos(x1x2 )( x3x4 )Lg1 h20Lg2
5、 h20L2f h2l1 sin x1 x32l 2 sin(x1x2 )(x3x4 )2l1 cosx1 f3 (x) l2 cos(x1 x2 )( f 3 ( x) f 4 ( x)Lg L f h2l1 cosx1 (g31 (x)l2 cos(x1x2 )( g31( x)g41( x)1Lg2 L f h2l1 cosx1 (g32 ( x)l 2 cos(x1x2 )( g32 ( x)g42 (x)可知，系统有相对阶向量 22，构造矩阵 A(x)A( x)Lg1 Lf h1Lg2 Lf h1Lg1 Lf h2Lg2 Lf h2得：l1 sin x1l 2 sin( x1x2 )
6、l2 sin( x1x2 )g31(x)g32( x)A( x)l2 cos(x1x2 )l2 cos(x1x2 )g41(x)g42( x)l1 cosx1则，当 x20 时，有 A(x) 可逆，有反馈控制律：uA 1 (x)L2f h11 ( x)AL2f h2和变量替换：z1h1z2L f h1z3h2z4L f h2将原系统转换成如下线性系统：010000000010z00z0v0100000011000y01z00三、控制器设计采用 LQR方法设计控制器，利用Matlab 的 LQR工具得到一组反馈增益：154.7000K00154.70搭建 simulink 仿真程序如下图feed
7、back linearizationPxPxx1x1kexi1kexi1Goto8From4Goto2u1u1PyPyx2x2dkexi1dkexi1From10Goto9From5Goto3x1x1x3x3kexi2kexi2Goto10From6Goto4x2x2x4x4dkexi2dkexi2Goto11From7Goto5u2u2x3x3v1v 1u1u1Goto12From8Goto6From11x4x4v2v 2u2u2system modelGoto13From9Goto7LQR controlleruu1Pxuu1From12uu2v1v1Pyuu2GotoFrom13kexi
8、1kexi1x1FromFrom14dkexi1dkexi1x2From1From15kexi2kexi2v2v2x3From2From16Goto1dkexi2dkexi2x4From3From17Scope1f(u)1x11Fcnkexi1Px2f(u)2system_modelx2dkexi12Fcn1Py3f(u)3x3kexi2Fcn2314f(u)4x1u1modelx4Fcn3dkexi22S-Function4feedback linearizationu2x2u155x3fblfblu26v16x45v26LQR controlleruu11-K-v13Gain1kexi1d
9、kexi1Add-K-4Gain1uu2Add22kexi2-K-v25Gain22dkexi2Add3-K-6Gain3Add190 / 57.3设置系统初始状态为170 / 57.3，目标信号uu10.3sin(t )0uu20.3cos(，载荷在 xoy平面运动t)0轨迹为：1运 动 轨 迹0.8参 考 轨 迹0.60.40.2m/y0P-0.2-0.4-0.6-0.8-1-1-0.8-0.6-0.4-0.200.20.40.60.81Px/m五、部分源代码系统动力学模型模块model.mfunction sys,x0,str,ts = model(t,x,u,flag)switch f
10、lag,case 0,sys,x0,str,ts=mdlInitializeSizes;case 1,sys=mdlDerivatives(t,x,u);case 2,sys=mdlUpdate(t,x,u);case 3,sys=mdlOutputs(t,x,u);case 4,sys=mdlGetTimeOfNextVarHit(t,x,u);case 9,sys=mdlTerminate(t,x,u);otherwiseerror( Unhandled flag = ,num2str(flag);endfunction sys,x0,str,ts=mdlInitializeSizessi
11、zes = simsizes;sizes.NumContStates= 4;sizes.NumDiscStates= 0;sizes.NumOutputs= 6;sizes.NumInputs= 2;sizes.DirFeedthrough = 1;sizes.NumSampleTimes = 1;sys = simsizes(sizes);x0 = 90*pi/180,-170*pi/180,0,0;% at least one sample time is neededstr = ;ts= 0 0;function sys=mdlDerivatives(t,x,u)theta1 = x(1
12、);theta2 = x(2);d_theta1 = x(3);d_theta2 = x(4);u1 = u(1);u2 = u(2);m = 10;g = 9.81;l1 = 0.432;l2 = 0.432;m1 = 15.19;m2 = 11.36;a1 = 1/3*m1*l12+1/3*m2*l22+m2*l12;a2 = m2*l1*l2;a3 = 1/3*m2*l22;a4 = 1/2*m1*g*l1+m2*g*l1;a5 = 1/2*m2*g*l2;J = -l1*sin(theta1)-l2*sin(theta1+theta2) -l2*sin(theta1+theta2);l
13、1*cos(theta1)+l2*cos(theta1+theta2)l2*cos(theta1+theta2);dJ = -l1*d_theta1*cos(theta1)-l2*(d_theta1+d_theta2)*cos(theta1+theta2)-l2*(d_theta1+d_theta2)*cos(theta1+theta2);.-l1*d_theta1*sin(theta1)-l2*(d_theta1+d_theta2)*sin(theta1+theta2)-l2*(d_theta1+d_theta2)*sin(theta1+theta2);M = a1+a2*cos(theta
14、2) a3+(a2/2)*cos(theta2);a3+(a2/2)*cos(theta2) a3+m*J*J; N =-a2*sin(theta2)*(d_theta1*d_theta2+d_theta22/2);a2*sin(theta2)*d_theta12/2+m*J*dJ*d_theta1; d_theta2;G = a4*cos(theta1)+a5*cos(theta1+theta2);a5*cos(theta1+theta2)+m*J*g;g; D = M;E = -N-G;d_x1 = d_theta1;d_x2 = d_theta2;M_st = DE+Du1;u2;d_x
15、3 = M_st(1);d_x4 = M_st(2);sys = d_x1,d_x2,d_x3,d_x4;function sys=mdlUpdate(t,x,u)sys = ;function sys=mdlOutputs(t,x,u)theta1 = x(1);theta2 = x(2);d_theta1 = x(3);d_theta2 = x(4);l1 = 0.432;l2 = 0.432;Px = l1*cos(theta1)+l2*cos(theta1+theta2);Py = l1*sin(theta1)+l2*sin(theta1+theta2);sys = Px,Py,the
16、ta1,theta2,d_theta1,d_theta2;function sys=mdlGetTimeOfNextVarHit(t,x,u)sampleTime = 1;sys = t + sampleTime;function sys=mdlTerminate(t,x,u)sys = ;反馈线性化模块fbl.mfunction sys,x0,str,ts = fbl(t,x,u,flag)switch flag,case 0,sys,x0,str,ts=mdlInitializeSizes;case 1,sys=mdlDerivatives(t,x,u);case 2,sys=mdlUpd
17、ate(t,x,u);case 3,sys=mdlOutputs(t,x,u);case 4,sys=mdlGetTimeOfNextVarHit(t,x,u);case 9,sys=mdlTerminate(t,x,u);otherwiseerror( Unhandled flag = ,num2str(flag);endfunction sys,x0,str,ts=mdlInitializeSizessizes = simsizes; sizes.NumContStates = 0; sizes.NumDiscStates = 0; sizes.NumOutputs = 2; sizes.
18、DirFeedthrough = 1; sizes.NumSampleTimes = 1; sys = simsizes(sizes);% at least one sample time is neededx0 = ;str = ;ts= 0 0;function sys=mdlDerivatives(t,x,u)sys = ;function sys=mdlUpdate(t,x,u)sys = ;function sys=mdlOutputs(t,x,u)theta1 = u(1);theta2 = u(2);d_theta1 = u(3);d_theta2 = u(4);if t=0th
19、eta1 = 10*pi/180;theta2 = 10*pi/180;else;endif t = 0.015751;else;endv1 = u(5);v2 = u(6);m = 10;g = 9.81;l1 = 0.432;l2 = 0.432;m1 = 15.19;m2 = 11.36;a1 = 1/3*m1*l12+1/3*m2*l22+m2*l12;a2 = m2*l1*l2;a3 = 1/3*m2*l22;a4 = 1/2*m1*g*l1+m2*g*l1;a5 = 1/2*m2*g*l2;J = -l1*sin(theta1)-l2*sin(theta1+theta2) -l2*
20、sin(theta1+theta2);l1*cos(theta1)+l2*cos(theta1+theta2)l2*cos(theta1+theta2);dJ = -l1*d_theta1*cos(theta1)-l2*(d_theta1+d_theta2)*cos(theta1+theta2)-l2*(d_theta1+d_theta2)*cos(theta1+theta2);.-l1*d_theta1*sin(theta1)-l2*(d_theta1+d_theta2)*sin(theta1+theta2)-l2*(d_theta1+d_theta2)*sin(theta1+theta2)
21、;M = a1+a2*cos(theta2) a3+(a2/2)*cos(theta2);a3+(a2/2)*cos(theta2) a3+m*J*J; N =-a2*sin(theta2)*(d_theta1*d_theta2+d_theta22/2);a2*sin(theta2)*d_theta12/2+m*J*dJ*d_theta1; d_theta2;G = a4*cos(theta1)+a5*cos(theta1+theta2);a5*cos(theta1+theta2)+m*J*g;g; D = M;E = -N-G;M_st = DE;f3x = M_st(1);f4x = M_
22、st(2);M_st1 = D1 0;0 1;g31x = M_st1(1,1);g32x = M_st1(1,2);g41x = M_st1(2,1);g42x = M_st1(2,2);Lf2h1 =-l1*cos(theta1)*d_theta12-l1*sin(theta1)*f3x-l2*cos(theta1+theta2)*(d_theta1+d_theta2)2-l2*sin(theta1+theta2)*(f3x+f4x);Lf2h2 =-l1*sin(theta1)*d_theta12+l1*cos(theta1)*f3x-l2*sin(theta1+theta2)*(d_theta1+d_theta2)2+l2*cos(t heta1+theta2)*(f3x+f4x);A = J*g31x g32x;g41x g42x;out = -ALf2h1;Lf2h2+Av1;v2;u1 = out(1);u2 = out(2);sys = u1,u2;function sys=mdlGetTimeOfNextVarHit(t,x,u)sampleTime = 1;sys = t + sampleTime;function sys=mdlTerminate(t,x,u)sys = ;