function [sys,x0,str,ts] = RobustControler(t,x,u,flag)
switch flag,
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;
  case 1,
    sys=mdlDerivatives(t,x,u);
  case {2,4,9},
    sys=[];
  case 3,
    sys=mdlOutputs(t,x,u);
  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end
function [sys,x0,str,ts]=mdlInitializeSizes   
sizes = simsizes;
sizes.NumContStates  = 0; 
sizes.NumDiscStates  = 0;  
sizes.NumOutputs     = 6; 
sizes.NumInputs      = 4;  
sizes.DirFeedthrough = 1;   
sizes.NumSampleTimes = 0;   
                           
sys = simsizes(sizes);
x0  = [];            
str = [];                   
ts  = [];                  
 
function sys=mdlOutputs(t,x,u)   
th1 = u(1);
th2 = u(2);
q = [th1;th2];
dth1 = u(3);
dth2 = u(4);
dq = [dth1;dth2];

%跟踪轨迹x1d，dx1d，ddx1d
q1d = 0.5*sin(t) + 0.1*sin(3*t) - 0.2*sin(4*t);
q2d = 0.1*sin(2*t) + 0.2*sin(3*t) - 0.1*sin(4*t);
x1d = [q1d; q2d];
dq1d = 0.5*cos(t) + 0.3*cos(3*t) - 0.8*cos(4*t);
dq2d = 0.2*cos(2*t) + 0.6*cos(3*t) - 0.4*cos(4*t);
dx1d = [dq1d;dq2d];
ddq1d = -0.5*sin(t) - 0.9*sin(3*t) + 3.2*sin(4*t);
ddq2d = -0.4*sin(2*t) - 1.8*sin(3*t) + 1.6*sin(4*t);
ddx1d = [ddq1d;ddq2d];

%系统参数
g = 9.81;
m1 = 1;
m2 = 2;
l1 = 1;
l2 = 2;
d = [th1*dth1*sin(th2)+dth1; th2*dth2*cos(th1)+dth2]; %外部干扰矩阵d
%质量矩阵M
M = [l2^2*m2+2*l1*l2*m2*cos(th2)+l1^2*(m1+m2) l2^2*m2+l1*l2*m2*cos(th2);
     l2^2*m2+l1*l2*m2*cos(th2) l2^2*m2];
%离心力和哥氏力矢量
C = [-m2*l1*l2*sin(th2)*dth2^2-2*m2*l1*l2*sin(th2)*dth1*dth2; m2*l1*l2*sin(th2)*dth1^2];
%重力矢量
G = [m2*l2*g*cos(th1+th2)+(m1+m2)*l1*g*cos(th1); m2*l2*g*cos(th1+th2)];

%跟踪误差
z1 = q - x1d;
L1 = [2 0;0 2];
K1 = [3 0 ;0 3];
A1 = K1;
%取虚拟控制x2d
x2d = dx1d - L1*A1*z1;
z2 = dq - x2d;
f2 = -inv(M)*G - inv(M)*C;
xite2 = 2;
f2_k = f2 - (L1*K1*dx1d+ddx1d+(-L1*K1)*dq);
L2 = [4 0;0 4];
K2 = [5 0;0 5];
A2 = K2 + inv(L2)*(xite2*(d*d'))*inv(L2);
ut = -M*(f2_k + L2*inv(L1)*z1 + L2*A2*z2);

e1 = th1 - q1d;
e2 = th2 - q2d;
de1 = dth1 - dq1d;
de2 = dth2 - dq2d;

sys(1) = ut(1);
sys(2) = ut(2);
sys(3) = e1;
sys(4) = e2;
sys(5) = de1;
sys(6) = de2;

2R型开链机器人动力学模型建立

function [sys,x0,str,ts] = RobustPlant(t,x,u,flag)
switch flag,
  case 0, 
    [sys,x0,str,ts]=mdlInitializeSizes;
  case 1, 
    sys=mdlDerivatives(t,x,u);
  case {2,4,9}, 
    sys=[];
  case 3, 
    sys=mdlOutputs(t,x,u);
  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', 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.5 -0.5 0 0];            
str = [];                   
ts  = [];                   

function sys=mdlDerivatives(t,x,u)  
th1 = x(1);
th2 = x(2);
dth1 = x(3);
dth2 = x(4);
tao1 = u(1);
tao2 = u(2);
tao = [tao1;tao2];

%系统参数
g = 9.81;
m1 = 1;
m2 = 2;
l1 = 1;
l2 = 2;
d = [th1*dth1*sin(th2)+dth1; th2*dth2*cos(th1)+dth2]; %外部干扰矩阵d

%质量矩阵M
M = [l2^2*m2+2*l1*l2*m2*cos(th2)+l1^2*(m1+m2) l2^2*m2+l1*l2*m2*cos(th2);
     l2^2*m2+l1*l2*m2*cos(th2) l2^2*m2];
%离心力和哥氏力矢量
C = [-m2*l1*l2*sin(th2)*dth2^2-2*m2*l1*l2*sin(th2)*dth1*dth2; m2*l1*l2*sin(th2)*dth1^2];
%重力矢量
G = [m2*l2*g*cos(th1+th2)+(m1+m2)*l1*g*cos(th1); m2*l2*g*cos(th1+th2)];
some = inv(M) * (tao-d-G-C);

sys(1) = x(3);   
sys(2) = x(4);   
sys(3) = some(1);   
sys(4) = some(2);  
    
function sys=mdlOutputs(t,x,u)   %产生（传递）系统输出
sys(1)=x(1);   %q1
sys(2)=x(2);   %q2
sys(3)=x(3);   %dq1
sys(4)=x(4);   %dq2

第一、二关节角度跟踪误差：

第一、二关节角速度跟踪误差：

总结：从仿真结果可以看出角度和角速度在2s内跟踪上了期望轨迹，之后曲线存在轻微波动，后期可以通过调节矩阵L1,K1,L2,K2的参数来降低跟踪误差，总体鲁棒控制跟踪效果良好。