chap4_8ctrl.m

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

function [sys,x0,str,ts] = MIMO_Tong_s(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 nmn1 nmn2 Fai
nmn1=5;nmn2=5;
Fai=[nmn1 0;0 nmn2];
sizes = simsizes;
sizes.NumContStates  = 2*5^4;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 4;
sizes.NumInputs      = 8;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0  = [0.1*ones(2*5^4,1)];
str = [];
ts  = [];
function sys=mdlDerivatives(t,x,u)
global nmn1 nmn2 Fai
qd1=u(1);
qd2=u(2);
dqd1=0.3*cos(t);
dqd2=0.3*cos(t);
dqd=[dqd1 dqd2]';

ddqd1=-0.3*sin(t);
ddqd2=-0.3*sin(t);
ddqd=[ddqd1 ddqd2]';

q1=u(3);dq1=u(4);
q2=u(5);dq2=u(6);

for l1=1:1:5
    gs1=-[(q1+pi/6-(l1-1)*pi/12)/(pi/24)]^2;
	u1(l1)=exp(gs1);
end
for l2=1:1:5
    gs2=-[(dq1+pi/6-(l2-1)*pi/12)/(pi/24)]^2;
    u2(l2)=exp(gs2);
end
for l3=1:1:5
    gs3=-[(q2+pi/6-(l3-1)*pi/12)/(pi/24)]^2;
	u3(l3)=exp(gs3);
end
for l4=1:1:5
    gs4=-[(dq2+pi/6-(l4-1)*pi/12)/(pi/24)]^2;
    u4(l4)=exp(gs4);
end

fsd=0;
fsu=zeros(5^4,1);
for l1=1:5
    for l2=1:5
        for l3=1:5
            for l4=1:5
                fsu(5^3*(l1-1)+5^2*(l2-1)+5*(l3-1)+l4)=u1(l1)*u2(l2)*u3(l3)*u4(l4);
                fsd=fsd+u1(l1)*u2(l2)*u3(l3)*u4(l4);
             end
        end
    end
end
fs=fsu/(fsd+0.001);

e1=q1-qd1;
e2=q2-qd2;
e=[e1 e2]';
de1=dq1-dqd1;
de2=dq2-dqd2;
de=[de1 de2]';

s=de+Fai*e;
Gama1=0.0001;
Gama2=0.0001;

S1=-1/Gama1*s(1)*fs;
S2=-1/Gama2*s(2)*fs;
for i=1:1:5^4
    sys(i)=S1(i);
end
for j=5^4+1:1:2*5^4
    sys(j)=S2(j-5^4);
end

function sys=mdlOutputs(t,x,u)
global nmn1 nmn2 Fai

q1=u(3);dq1=u(4);
q2=u(5);dq2=u(6);

r1=1;r2=0.8;
m1=1;m2=1.5;

D11=(m1+m2)*r1^2+m2*r2^2+2*m2*r1*r2*cos(q2);
D22=m2*r2^2;
D21=m2*r2^2+m2*r1*r2*cos(q2);
D12=D21;
D=[D11 D12;D21 D22];

C12=m2*r1*sin(q2);
C=[-C12*dq2 -C12*(dq1+dq2);C12*q1 0];

g1=(m1+m2)*r1*cos(q2)+m2*r2*cos(q1+q2);
g2=m2*r2*cos(q1+q2);
G=[g1;g2];

qd1=u(1);
qd2=u(2);
dqd1=0.3*cos(t);
dqd2=0.3*cos(t);
dqd=[dqd1 dqd2]';

ddqd1=-0.3*sin(t);
ddqd2=-0.3*sin(t);
ddqd=[ddqd1 ddqd2]';

e1=q1-qd1;
e2=q2-qd2;
e=[e1 e2]';
de1=dq1-dqd1;
de2=dq2-dqd2;
de=[de1 de2]';

s=de+Fai*e;
dqr=dqd-Fai*e;
ddqr=ddqd-Fai*de;

for i=1:1:5^4
    thta1(i,1)=x(i);
end
for i=1:1:5^4
    thta2(i,1)=x(i+5^4);
end

for l1=1:1:5
    gs1=-[(q1+pi/6-(l1-1)*pi/12)/(pi/24)]^2;
	u1(l1)=exp(gs1);
end
for l2=1:1:5
    gs2=-[(dq1+pi/6-(l2-1)*pi/12)/(pi/24)]^2;
    u2(l2)=exp(gs2);
end
for l3=1:1:5
    gs3=-[(q2+pi/6-(l3-1)*pi/12)/(pi/24)]^2;
	u3(l3)=exp(gs3);
end
for l4=1:1:5
    gs4=-[(dq2+pi/6-(l4-1)*pi/12)/(pi/24)]^2;
    u4(l4)=exp(gs4);
end

fsd=0;
fsu=zeros(5^4,1);
for l1=1:5
    for l2=1:5
        for l3=1:5
            for l4=1:5
                fsu(5^3*(l1-1)+5^2*(l2-1)+5*(l3-1)+l4)=u1(l1)*u2(l2)*u3(l3)*u4(l4);
                fsd=fsd+u1(l1)*u2(l2)*u3(l3)*u4(l4);
            end
        end
    end
end
fs=fsu/(fsd+0.001);

Fp(1)=thta1'*fs;
Fp(2)=thta2'*fs;

KD=10*eye(2);
W=[0.2 0;0 0.2];

M=1;
if M==1
    tol=D*ddqr+C*dqr+G+1*Fp'-KD*s;           % (4.136)
elseif M==2
    tol=D*ddqr+C*dqr+G+Fp'-KD*s-W*sign(s);   % (4.137)
end
sys(1)=tol(1);
sys(2)=tol(2);
sys(3)=Fp(1);
sys(4)=Fp(2);