关于基于BP神经网络控制器的平面三级倒立摆的相关问题
这个程序执行到后面出现数据为NaN的情况，这是我用BP神经网络训练平面三级倒立摆模型的程序，执行到最后只有一两个数据有效，希望有巨佬看看，谢了。
clear all;
close all;
clc;
syms m0 m1 m2 m3 d1 d2 d3 d4 d5 d6 A1 A2 A3 J1 J2 J3 f0 f1 f2 f3 g G0 t p;
m0 = 1.32;m1 = 0.15;m2 = 0.22;m3 = 0.18;J1 = 0.0022;d1 = 0.2;J2 = 0.005;d2 = 0.3;J3 = 0.0032;d3 = 0.24;d4 = 0.33;d5 = 0.49;f0 = 22.9;f1 = 0.0071;f2 = 0.0026;f3 = 0.0026;g = 9.81;G0 = 11.88;
M = [m0+m1+m2+m3 m1*d1+m2*d4+m3*d4 m2*d2+m3*d5 m3*d3;m1*d1+m2*d4+m3*d4 J1+m1*d1^2+m2*d4^2+m3*d4^2 m2*d2*d4+m3*d4*d2 m3*d3*d4;m2*d2+m3*d5 m2*d2*d4+m3*d4*d2 J2+m2*d2^2+m3*d5^2 m3*d3*d5;m3 m3*d3*d4 m3*d3*d5 J3+m3*d3^2];
M_inv = inv(M);
%disp(M_inv);
F = [-f0 0 0 0;0 -(f1+f2) f2 0;0 f2 -(f3+f2) f3;0 0 f3 -f3];
N = [0 0 0 0;0 (m1*d1+m2*d4+m3*d4)*g 0 0;0 0 (m2*d2+m3*d5)*g 0;0 0 0 m3*d3*g];
T0 = [1 0 0 0;0 1 0 0;0 -1 1 0;0 0 -1 1];
I = eye(4);
T0_inv = inv(T0);
A21 = T0/M*N*T0_inv;
A22 = T0/M*F*T0_inv;
B0 = T0/M*[G0;0;0;0];
Ax = [zeros(4,4) I;A21 A22];
Bx = [0;0;0;0;B0];
Cx = [1 0 0 0 0 0 0 0;0 1 0 0 0 0 0 0;0 0 1 0 0 0 0 0;0 0 0 1 0 0 0 0];
Dx = zeros(4,1);
[num,den] = ss2tf(Ax,Bx,Cx,Dx);
% step(Ax,Bx,Cx,Dx);
ts=0.001;T=0.3;
[G,H] = c2d(Ax,Bx,ts);
[num_d,den_d] = ss2tf(G,H,Cx,Dx);
%三级倒立摆数学模型
%BP神经网络控制器仿真
xite=0.2;
alfa=0.01;
IN=4;H=5;Out=3;
% wi1=[-0.6394 -0.2696 -0.3756 -0.7023;
% -0.8603 -0.2013 -0.5024 -0.2596;
% -1.0749 0.5543 -1.6820 -0.5437;
% -0.3625 -0.0724 -0.6463 -0.2859;
% 0.1425 0.0279 -0.5406 -0.7660];
wi1=(2.*rand(H,IN)-1)*0.8;
wi2=wi1;wi3=wi1;wi4=wi1;
wi1_1=wi1;wi1_2=wi1;wi1_3=wi1;
wi2_1=wi2;wi2_2=wi2;wi2_3=wi2;
wi3_1=wi3;wi3_2=wi3;wi3_3=wi3;
wi4_1=wi4;wi4_2=wi4;wi4_3=wi4;
% wo1=[0.7576 0.2616 0.5820 -0.1416 -0.1325;
% -0.1146 0.2949 0.8352 0.2205 0.4508;
% 0.7201 0.4566 0.7672 0.4962 0.3632];
wo1=(2.*rand(Out,H)-1)*0.6;
wo2=wo1;wo3=wo1;wo4=wo1;
wo1_1=wo1;wo1_2=wo1;wo1_3=wo1;
wo2_1=wo2;wo2_2=wo2;wo2_3=wo2;
wo3_1=wo3;wo3_2=wo1;wo3_3=wo3;
wo4_1=wo4;wo4_2=wo4;wo4_3=wo4;
% disp(wi1);disp(wo1);
X=zeros(8,1);
u=zeros(8,1);
u_1=zeros(4,1);u_2=zeros(4,1);u_3=zeros(4,1);u_4=zeros(4,1);u_5=zeros(4,1);
y_1=[0;0;0;0];y_2=[0;0;0;0];y_3=[0;0;0;0];y_4=[0;0;0;0];yout=[0;0;0;0];
error=[0;0;0;0];
Oh1=zeros(H,1); Oh2=zeros(H,1);Oh3=zeros(H,1); Oh4=zeros(H,1);
I1=Oh1;I2=Oh2; I3=Oh3; I4=Oh4;
error_2=[0;0;0;0];
error_1=[0;0;0;0];
yout_temp=zeros(4,T/ts);u_temp=zeros(4,T/ts);
for k=1:1:500
time(k)=k*ts;
rin=[0.1;0;0;0]; %期望值x=0.1，角度1=0，角度2-角度1=0，角度3-角度2=0
dX = G * X + H * u;
yout = Cx * X;
error=rin-yout;
xi1=[rin(1,:),yout(1,:),error(1,:),1];
xi2=[rin(2,:),yout(2,:),error(2,:),1];
xi3=[rin(3,:),yout(3,:),error(3,:),1];
xi4=[rin(4,:),yout(4,:),error(4,:),1];
x1=error-error_1;
x2=error;
x3=error-2.*error_1+error_2;
epid1=[x1(1,:);x2(1,:);x3(1,:)];
epid2=[x1(2,:);x2(2,:);x3(2,:)];
epid3=[x1(3,:);x2(3,:);x3(3,:)];
epid4=[x1(4,:);x2(4,:);x3(4,:)];
I1=xi1*wi1';I2=xi2*wi2';I3=xi3*wi3'; I4=xi4*wi4';
for j=1:1:H
Oh1(j)=(exp(I1(j))-exp(-I1(j)))/(exp(I1(j))+exp(-I1(j)));
Oh2(j)=(exp(I2(j))-exp(-I2(j)))/(exp(I2(j))+exp(-I2(j)));
Oh3(j)=(exp(I3(j))-exp(-I3(j)))/(exp(I3(j))+exp(-I3(j)));
Oh4(j)=(exp(I4(j))-exp(-I4(j)))/(exp(I4(j))+exp(-I4(j)));
end
K1=wo1*Oh1; K2=wo2*Oh2; K3=wo3*Oh3; K4=wo4*Oh4;
for l=1:1:Out
K1(l)=exp(K1(l))/(exp(K1(l))+exp(-K1(l)));
K2(l)=exp(K2(l))/(exp(K2(l))+exp(-K2(l)));
K3(l)=exp(K3(l))/(exp(K3(l))+exp(-K3(l)));
K4(l)=exp(K4(l))/(exp(K4(l))+exp(-K4(l)));
end
kp1(k)=K1(1);ki1(k)=K1(2);kd1(k)=K1(3); kp2(k)=K2(1);ki2(k)=K2(2);kd2(k)=K2(3);kp3(k)=K3(1);ki3(k)=K3(2);kd3(k)=K3(3);kp4(k)=K4(1);ki4(k)=K4(2);kd4(k)=K4(3);
Kpid1=[kp1(k),ki1(k),kd1(k)]; Kpid2=[kp2(k),ki2(k),kd2(k)]; Kpid3=[kp3(k),ki3(k),kd3(k)];Kpid4=[kp4(k),ki4(k),kd4(k)];
du1=Kpid1*epid1;
du2=Kpid2*epid2;
du3=Kpid3*epid3;
du4=Kpid4*epid4;
u(1,:)=u_1(1,:)+du1; u(2,:)=u_1(2,:)+du2; u(3,:)=u_1(1,:)+du3; u(4,:)=u_1(4,:)+du4; disp(u);
for p=1:1:4
if u(p,:)>=2
u(p,:)=2;
end
if u(p,:)<=-2
u(p,:)=-2;
end
end
dyu1=sign((yout(1,:)-y_1(1,:))/(u(1,:)-u_1(1,:)+0.0000001));
dyu2=sign((yout(2,:)-y_1(2,:))/(u(2,:)-u_1(2,:)+0.0000001));
dyu3=sign((yout(3,:)-y_1(3,:))/(u(3,:)-u_1(3,:)+0.0000001));
dyu4=sign((yout(4,:)-y_1(4,:))/(u(4,:)-u_1(4,:)+0.0000001));
for j=1:1:Out
dK1(j)=2/(exp(K1(j))+exp(-K1(j)))^2;
dK2(j)=2/(exp(K2(j))+exp(-K2(j)))^2;
dK3(j)=2/(exp(K3(j))+exp(-K3(j)))^2;
dK4(j)=2/(exp(K4(j))+exp(-K4(j)))^2;
end
for l=1:1:Out
delta3_1(l)=error(1,:)*dyu1*epid1(l)*dK1(l);
delta3_2(l)=error(2,:)*dyu2*epid2(l)*dK2(l);
delta3_3(l)=error(3,:)*dyu3*epid3(l)*dK3(l);
delta3_4(l)=error(4,:)*dyu4*epid4(l)*dK4(l);
end
for l=1:1:Out
for i=1:1:H
d_wo1=xite*delta3_1(l)*Oh1(i)+alfa*(wo1_1-wo1_2);
d_wo2=xite*delta3_2(l)*Oh2(i)+alfa*(wo2_1-wo2_2);
d_wo3=xite*delta3_3(l)*Oh3(i)+alfa*(wo3_1-wo3_2);
d_wo4=xite*delta3_4(l)*Oh4(i)+alfa*(wo4_1-wo4_2);
end
end
wo1=wo1_1+d_wo1+alfa*(wo1_1-wo1_2); wo2=wo2_1+d_wo2+alfa*(wo2_1-wo2_2); wo3=wo3_1+d_wo3+alfa*(wo3_1-wo3_2); wo4=wo4_1+d_wo4+alfa*(wo4_1-wo4_2);
for i=1:1:H
dO1(i)=4/(exp(I1(i))+exp(-I1(i)))^2;dO2(i)=4/(exp(I2(i))+exp(-I2(i)))^2;
dO3(i)=4/(exp(I3(i))+exp(-I3(i)))^2;dO4(i)=4/(exp(I4(i))+exp(-I4(i)))^2;
end
segma1=delta3_1*wo1;segma2=delta3_2*wo2;segma3=delta3_3*wo3;segma4=delta3_4*wo4;
for i=1:1:H
delta2_1(i)=dO1(i)*segma1(i);
delta2_2(i)=dO2(i)*segma2(i);
delta2_3(i)=dO3(i)*segma3(i);
delta2_4(i)=dO4(i)*segma4(i);
end
d_wi1=xite*delta2_1'*xi1;
wi1=wi1_1+d_wi1+alfa*(wi1_1-wi1_2);
d_wi2=xite*delta2_2'*xi2;
wi2=wi2_1+d_wi2+alfa*(wi2_1-wi2_2);
d_wi3=xite*delta2_3'*xi3;
wi3=wi3_1+d_wi3+alfa*(wi3_1-wi3_2);
d_wi4=xite*delta2_4'*xi4;
wi4=wi4_1+d_wi4+alfa*(wi4_1-wi4_2);
%disp(yout);
for j=1:4
yout_temp(j,k)=yout(j,1);u_temp(j,k)=u(j,1);
end
u_5=u_4;u_4=u_3;u_3=u_2;u_2=u_1;u_1=u;
y_2=y_1;y_1=yout;X = dX; disp(X);
wo1_4=wo1_3;
wo1_3=wo1_2;
wo1_2=wo1_1;
wo1_1=wo1;
wi1_4=wi1_3;
wi1_3=wi1_2;
wi1_2=wi1_1;
wi1_1=wi1;
wo2_4=wo2_3;
wo2_3=wo2_2;
wo2_2=wo2_1;
wo2_1=wo2;
wi2_4=wi2_3;
wi2_3=wi2_2;
wi2_2=wi2_1;
wi2_1=wi2;
wo3_4=wo3_3;
wo3_3=wo3_2;
wo3_2=wo3_1;
wo3_1=wo3;
wi3_4=wi3_3;
wi3_3=wi3_2;
wi3_2=wi3_1;
wi3_1=wi3;
wo4_4=wo4_3;
wo4_3=wo4_2;
wo4_2=wo4_1;
wo4_1=wo4;
wi4_4=wi4_3;
wi4_3=wi4_2;
wi4_2=wi4_1;
wi4_1=wi4;
error_2=error_1;
error_1=error;
end
figure(1);
plot(time,yout_temp(1,:),'b');
xlabel('time(s)');ylabel('rin,x');
figure(2);
plot(time,yout_temp(2,:),'b');
xlabel('time(s)');ylabel('rin,$$\varphi_1$$');
figure(3);
plot(time,yout_temp(3,:),'b');
xlabel('time(s)');ylabel('rin,$$\varphi_2-\varphi_1$$');
figure(4);
plot(time,yout_temp(4,:),'b');
xlabel('time(s)');ylabel('rin,$$\varphi_3-\varphi_2$$');
运行结果只有前面几个数据，如图：
程序是我将只有1个控制器的程序改成了4个同时计算的，结果就出现了这个问题，不知道为什么
希望有巨佬指出问题所在，谢谢了。
应该是闭环系统发散了，或者出现了奇点。可能的原因是，控制器设计不合适或参数不合适，系统不稳定，是发散的。