PID_机器人控制_二自由度机器人PID_pid机器人_

function [sys,x0,str,ts]= Plant (t,x,u,flag) switch flag case 0 [sys,x0,str,ts]=mdlInitializeSizes; case 1 sys=mdlDerivatives(t,x,u); case 3 sys=mdlOutputs(t,x,u); case {2, 4, 9 } sys = []; otherwise error(['Unhandled flag = ',num2str(flag)]); end function [sys,x0,str,ts]=mdlInitializeSizes sizes = simsizes; sizes.NumContStates = 4; sizes.NumDiscStates = 0; sizes.NumOutputs = 4; sizes.NumInputs = 2; sizes.DirFeedthrough = 0; sizes.NumSampleTimes = 0; sys=simsizes(sizes); x0=[0 0 0 0]; str=[]; ts=[]; function sys=mdlDerivatives(t,x,u) tol1=u(1); tol2=u(2); m1=1;m2=0.7; I1=0.25; I2=0.1; l1=0.4; lc1=0.2; l2=0.4; lc2=0.2; g=9.8; q1=x(1); q2=x(2); dq1=x(3); dq2=x(4); M11=m1*lc1*lc1+I1+m2*(l1*l1+lc2*lc2+2*l1*lc2*cos(q2))+I2; M12=m2*l1*lc2*cos(q2)+m2*lc2*lc2+I2; M22=m2*lc2*lc2+I2; c=-m2*l1*lc2*sin(q2); g1=m1*lc1*g*cos(q1)+m2*g*(lc2*cos(q1+q2)+l1*cos(q1)); g2=m2*lc2*cos(q1+q2); M = [M11 M12;M12 M22]; C = [c*dq2 c*(dq1+dq2);-c*dq1 0]; G = [g1;g2]; d=[0.05*sin(0.2*t);0.05*cos(0.2*t)]; tol=[tol1;tol2]; S=-inv(M)*C*[dq1;dq2]+inv(M)*(tol-d)-inv(M)*G; sys(1)=x(3); sys(2)=x(4); sys(3)=S(1); sys(4)=S(2); function sys=mdlOutputs(t,x,u) sys(1)=x(1); sys(2)=x(2); sys(3)=x(3); sys(4)=x(4);