chap3_9ctrl.m

机器人控制仿真程序一书的所有源代码

%From robot2_rbfnn_sim.mdl
function [sys,x0,str,ts] = spacemodel(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
global node c b Fai M
M=4;
node=7;
c=0.1*[-1.5 -1 -0.5 0 0.5 1 1.5;
       -1.5 -1 -0.5 0 0.5 1 1.5;
       -1.5 -1 -0.5 0 0.5 1 1.5;
       -1.5 -1 -0.5 0 0.5 1 1.5;
       -1.5 -1 -0.5 0 0.5 1 1.5];
b=10;
Fai=5*eye(2);

sizes = simsizes;
sizes.NumContStates  = 2*node;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 3;
sizes.NumInputs      = 11;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0  = 0.1*ones(1,2*node);
str = [];
ts  = [];
function sys=mdlDerivatives(t,x,u)
global node c b Fai M
qd1=u(1);
d_qd1=u(2);
dd_qd1=u(3);
qd2=u(4);
d_qd2=u(5);
dd_qd2=u(6);

q1=u(7);
d_q1=u(8);
q2=u(9);
d_q2=u(10);

q=[q1;q2];

e1=qd1-q1;
e2=qd2-q2;
de1=d_qd1-d_q1;
de2=d_qd2-d_q2;
e=[e1;e2];
de=[de1;de2];
r=de+Fai*e;

qd=[qd1;qd2];
dqd=[d_qd1;d_qd2];
dqr=dqd+Fai*e;
ddqd=[dd_qd1;dd_qd2];
ddqr=ddqd+Fai*de;

c=[-1.5 -1 -0.5 0 0.5 1 1.5;
   -1.5 -1 -0.5 0 0.5 1 1.5;
   -1.5 -1 -0.5 0 0.5 1 1.5;
   -1.5 -1 -0.5 0 0.5 1 1.5;
   -1.5 -1 -0.5 0 0.5 1 1.5];

z1=[e(1);de(1);qd(1);dqd(1);ddqd(1)];
z2=[e(2);de(2);qd(2);dqd(2);ddqd(2)];
for j=1:1:node
    h1(j)=exp(-norm(z1-c(:,j))^2/(b*b));
    h2(j)=exp(-norm(z2-c(:,j))^2/(b*b));
end

F=15*eye(node);

if M==1|M==3|M==4
    for i=1:1:node
        sys(i)=15*h1(i)*r(1);
        sys(i+node)=15*h2(i)*r(2);
    end
elseif M==2
    k=0.01;
    for i=1:1:node
        sys(i)=15*h1(i)*r(1)-k*15*norm(r)*x(i);
        sys(i+node)=15*h2(i)*r(2)-k*15*norm(r)*x(i+node);
    end
end

function sys=mdlOutputs(t,x,u)
global node c b Fai M

qd1=u(1);
d_qd1=u(2);
dd_qd1=u(3);
qd2=u(4);
d_qd2=u(5);
dd_qd2=u(6);

q1=u(7);
d_q1=u(8);
q2=u(9);
d_q2=u(10);

q=[q1;q2];

e1=qd1-q1;
e2=qd2-q2;
de1=d_qd1-d_q1;
de2=d_qd2-d_q2;
e=[e1;e2];
de=[de1;de2];
r=de+Fai*e;

qd=[qd1;qd2];
dqd=[d_qd1;d_qd2];
dqr=dqd+Fai*e;
ddqd=[dd_qd1;dd_qd2];
ddqr=ddqd+Fai*de;

z=[e;de;qd;dqd;ddqd];
W_f1=[x(1:node)]';
W_f2=[x(node+1:node*2)]';

z1=[e(1);de(1);qd(1);dqd(1);ddqd(1)];
z2=[e(2);de(2);qd(2);dqd(2);ddqd(2)];
for j=1:1:node
    h1(j)=exp(-norm(z1-c(:,j))^2/(b*b));
    h2(j)=exp(-norm(z2-c(:,j))^2/(b*b));
end

fn=[W_f1*h1';
    W_f2*h2'];
Kv=20*eye(2);

if M==1|M==2
    tol=fn+Kv*r;
elseif M==3
    epN=0.20;
    bd=0.1;
    gama=0.5;
    if r>=0
        satr=1-exp(-r/gama);
    else
        satr=-(1-exp(r/gama));
    end
    v=-(epN+bd)*satr;
    tol=fn+Kv*r-v;    
elseif M==4
    epN=0.20;
    bd=0.1;
    v=-(epN+bd)*sign(r);
    tol=fn+Kv*r-v;    
elseif M==5
    tol=Kv*r;   % Only PD Control
end
fn_norm=norm(fn);

sys(1)=tol(1);
sys(2)=tol(2);
sys(3)=fn_norm;