【鲁棒控制】平面2R型机器人的鲁棒逆推跟踪控制(matlab实现)

鲁棒跟踪逆推控制器设计

前言:对于模型准确的对象,我们可以设计控制器直接进行控制,但通常实际情况下总是存在着种种不确定因素,如参数变化,未建模动态变化等,鲁棒控制就是在模型不精确和其他变化因素的条件下,使系统仍能保持预期的性能。在这里我只针对未建模动态变化即仅针对存在外部扰动的情况进行讨论,在这种情况下对2R型平面机器人进行控制器的设计。
  对于存在外部扰动的平面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)q¨+c(q,q˙)+G(q)+d=τ M ( q ) M\left( q \right) M(q) 是对称正定的质量矩阵, c ( q , q ˙ ) c\left( q,\dot{q} \right) c(q,q˙) 是包含科氏力和向心力的矢量, G ( q ) G\left( q \right) G(q) 是重力矩矢量, d 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
    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的参数来降低跟踪误差,总体鲁棒控制跟踪效果良好。

相关推荐
©️2020 CSDN 皮肤主题: 技术工厂 设计师:CSDN官方博客 返回首页