南航非线性大作业.docx

课程:非线性系统控制题目:二阶机械臂的非线性反馈设计 姓 名：刘 洋 洋 学 号：SX1203179 专 业：导航制导与控制2013年6月28日一、被控对象动力学模型被控对象为两杆机械臂，为第一级杆转角，为第二级杆相对第一级杆的转角，为负载位置坐标，。系统的动力学模型如下：输出方程：状态方程：其中：式中部分相关的参数为：二、被控对象反馈线性化取状态量，系统化为： 令， ， 可得和输入u1有关的是g1,和输入u2有关的是g2,g1、g2其与状态方程中的输入前面的矩阵没有任何关系：可知，系统有相对阶向量，构造矩阵得：则，当时，有可逆，有反馈控制律：和变量替换：将原系统转换成如下线性系统：三、控制器设计采用LQR方法设计控制器，利用Matlab的LQR工具得到一组反馈增益：搭建simulink仿真程序如下图设置系统初始状态为，目标信号，载荷在平面运动轨迹为：五、部分源代码系统动力学模型模块model.mfunction sys,x0,str,ts = model(t,x,u,flag)switch flag, 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); otherwise error(Unhandled flag = ,num2str(flag);end function sys,x0,str,ts=mdlInitializeSizessizes = simsizes;sizes.NumContStates = 4;sizes.NumDiscStates = 0;sizes.NumOutputs = 6;sizes.NumInputs = 2;sizes.DirFeedthrough = 1;sizes.NumSampleTimes = 1; % at least one sample time is neededsys = simsizes(sizes);x0 = 90*pi/180,-170*pi/180,0,0;str = ;ts = 0 0; function sys=mdlDerivatives(t,x,u)theta1 = x(1);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);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);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;d_x1 = d_theta1;d_x2 = d_theta2;M_st = DE+Du1;u2;d_x3 = 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,theta1,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=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); otherwise error(Unhandled flag = ,num2str(flag);end function sys,x0,str,ts=mdlInitializeSizessizes = simsizes;sizes.NumContStates = 0;sizes.NumDiscStates = 0;sizes.NumOutputs = 2;sizes.NumInputs = 6;sizes.DirFeedthrough = 1;sizes.NumSampleTimes = 1; % at least one sample time is neededsys = simsizes(sizes);x0 = ;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=0 theta1 = 10*pi/180; theta2 = 10*pi/180;else ;endif t = 0.01575 1; ;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*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);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_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(