# 鲁棒跟踪逆推控制器设计

对于存在外部扰动的平面2R机器人的动力学方程： M ( q ) q ¨ + c ( q , q ˙ ) + G ( q ) + d = τ M\left( q \right) \ddot{q}+c\left( q,\dot{q} \right) +G\left( q \right) +d=\tau M ( q ) M\left( q \right) 是对称正定的质量矩阵， c ( q , q ˙ ) c\left( q,\dot{q} \right) 是包含科氏力和向心力的矢量， G ( q ) G\left( q \right) 是重力矩矢量， d d 是外部扰动， τ \tau 是关节力矩。

### 鲁棒跟踪backstepping控制器设计(matlab实现)：

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
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
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的参数来降低跟踪误差，总体鲁棒控制跟踪效果良好。

05-14

03-11
02-19
12-13 894
11-28 2万+
12-07
03-05
05-18
09-12
01-09 158
05-25 7260