ode45求解倒立摆模型一致控制问题

小小鲁班

%%下面是画图的函数
; j) J, S: [1 B0 _7 R: g, L+ @3 ix0=[pi/3;pi/4;-pi/3;-pi/5;
  l0 }5 k3 k0 H    0;0;0;0;
    0;0;0;0;
    0;0;0;0;
5 _9 w* P3 l) C, o) |    0;0;0;0;
    0;0]; %%初始条件
% e9 @9 Z7 V1 ~" {  f3 }1 R4 F[t,x]=ode45(@example180928,[0:30],x0);%求解微分函数值
2 r1 g% _( }; C: k/ Mfigure(1)
plot(t,x(:,1),'-.b' ,t,x(:,2),'-m' ,t,x(:,3),'--g' ,t,x(:,4),':k' ,t,x(:,21),'-.r','LineWidth',2.0);
, I3 j" Q0 l/ B%axis([0 20 -2 5]);
8 T0 D  V$ Y: oset(gca,'FontSize',10,'LineWidth',1.2); set(gcf,'Color','White');
" S# }4 n) Q9 Hxlabel('Time (seconds)','FontSize',16.0);
9 E' b& W' n6 d+ x6 A3 `) H5 Alegend('x_{1}','x_{2}','x_{3}','x_{4}','x_{0}');
" F! k+ s0 a3 @" M0 mtitle('The trajectories of five agents','FontSize',16.0 );
4 ?, d: ~, b, V8 T%%%%下面是函数定义的程序
0 R5 K- _. w8 A2 r3 E1 t6 d* o0 tfunction [ dx] = example180928( t,x )
  c7 X6 X# d, B1 g   A=[0 1 0 1;%拓扑
      1 0 1 0;
7 L* B# p& t& Z7 }, r" k3 U      0 1 0 1;
: z& }* _( R# Z: @      1 0 1 0];
7 L% j) [+ i6 T: |H=diag([sum(A(1,) sum(A(2,) sum(A(3,) sum(A(4,:))]);
# C- x8 G# t( R$ QL=H-A;
B=diag([1 0 0 0]);
; C* }: e* u: \1 I2 c) `) z4 A/ z- s%%%实际系统参数
3 j8 ]6 q3 b: B% [$ [8 Qf=zeros(4,1);
  N9 H) {/ k# W; b/ Kd=zeros(4,1);
- _# }' Y6 E) `, @( Sg=zeros(4,1);
for i=1:4
. G0 V6 n6 q+ j) R9 W# {, z    li0=1+0.04*i;li1=0.5*li0;mi=0.4*li0;
    XIi=(cos(x(i))+li0/li1)^2*li1^2/li0;
+ {) }4 S5 k! v' `    f(i)=li1*sin(x(i))*(1+li1/li0*cos(x(i)))*x(i+4)^2/XIi-9.8*sin(x(i))*(1+cos(x(i)))/XIi;
4 k. s# @4 L- M. C    g(i)=0.4/(mi*li0*XIi);
9 r$ k1 b* G' F8 ^' o% I    d(i)=0.2*i*cos(t);
end
d=[d(1); d(2); d(3); d(4)];
f=[f(1); f(2); f(3); f(4)];
gg=diag([g(1) g(2) g(3) g(4)]);
%%%%%%%5%x1变量x,v,参考轨迹
q=[x(1) x(2) x(3) x(4)]';%所有智能体的位置
# i2 \2 C- H/ ?) ~0 E# v5 c& `v=[x(5) x(6) x(7) x(8)]';%所有智能体的速度
Wf=[x(9) x(10) x(11) x(12)]';%未知动态
4 i( X% J6 s6 Q) j& s' a9 Ek=[x(13) x(14) x(15) x(16)]';%nussbaum
alpha=[x(17) x(18) x(19) x(20)]';%未知外部扰动误差
. [% H+ @1 t  v$ Y- U# Cvd=x(22);
4 ?  N; I8 m1 }! k- r, J" K! n8 r%dvd=x(22);
%%%%%%%%%%%%%%%%跟踪误差z
8 n4 _# J8 o! d  ^xi=zeros(4,1);
for i=1:4  
9 Q9 Y0 N' z3 Z) L3 H- z    S=0;
  [4 C$ ~1 p" z+ `9 ^    for j=1:4
        S=A(i,j)*(x(i)-x(j))+S;
    end
    xi(i)=S+B(i,i)*(x(i)-x(21));
end
) f: L( M  M: D# Xxi=[xi(1); xi(2); xi(3); xi(4)];%\xi

dxi=zeros(4,1);
1 _) j7 Z# z* ^) T% bfor i=1:4
    T=0;
6 m) h, a$ A: ?( B/ R0 w2 t/ {% u    for j=1:4
+ Q* S  g7 u3 M        T=A(i,j)*(x(i+4)-x(j+4))+T;
    end
8 n/ _/ T' ~; I/ z2 }# I; Z1 C    dxi(i)=S+B(i,i)*(x(i+4)-x(22));
, t5 L6 @. Y4 N3 uend
( L$ X7 `6 t* J7 O' J* m) cdxi=[dxi(1); dxi(2); dxi(3); dxi(4)];%\dxi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模糊逻辑系统%%%%%%%%%
s1=zeros(16,1);
for i=1:4
! l9 }/ Y0 [$ D6 q8 U( C4 a. u: _    phi1=[exp(-(x(i)-3+1)^2/16);
          exp(-(x(i)-3+2)^2/16);
          exp(-(x(i)-3+3)^2/16);
4 {9 G1 ]. X& @          exp(-(x(i)-3+4)^2/16)];
ph1i=phi1/sum(phi1);
s1(4*(i-1)+1:4*i)=ph1i;
/ f! M/ U/ p% ^5 Q9 Oend
Sf1=s1(1:4);Sf2=s1(5:8);Sf3=s1(9:12);Sf4=s1(13:16); %%\varphi_{i}
Sf=[Sf1 Sf2 Sf3 Sf4];
% |6 B, o9 @" D' o/ y3 L& H%%%%%%参数,
Beta=[15 0 0 0 ;
( S  ~  S4 w. q% D4 S- Y: T3 K. Q        0 15 0 0 ;
+ n& k6 d- k4 g        0 0 15 0 ;
        0 0 0 15];
Mu=[1 0 0 0 ;
8 i5 h) j3 ]' [: j" C$ f6 M      0 1 0 0 ;
      0 0 1 0 ;
: p4 d( i8 G3 N      0 0 0 1];
4 z3 W1 e2 g8 B+ v; z) i4 ~, z, BRho=[1 0 0 0 ;
      0 1 0 0 ;
      0 0 1 0 ;
$ k3 m6 X2 M1 }& b' h      0 0 0 1];
8 Y) r- o1 U/ Y+ J0 ^  z=xi+dxi;
, D0 C6 I2 _7 l  \5 [% x7 {3 t  z=[z(1);z(2);z(3);z(4)];
  dWf=Mu*Sf*z;
  bf=Sf*Wf;
4 a: c5 D8 L9 }! L7 u# ~  theta=exp(-0.05*t);
1 Z3 ~! c1 o8 T5 C. M% T  pi=[z(1)*x(17)/(z(1)^2+theta^2)^0.5;
! t/ b1 j* x, z& t    z(2)*x(18)/(z(2)^2+theta^2)^0.5;
% ], P2 n. ?# z6 o. @1 a    z(3)*x(19)/(z(3)^2+theta^2)^0.5;
    z(4)*x(20)/(z(4)^2+theta^2)^0.5];
/ B, |3 v  @3 m' {6 p  dalpha=Rho*[z(1)^2/((z(1)^2+theta^2)^0.5);
             z(2)^2/((z(2)^2+theta^2)^0.5);
             z(3)^2/((z(3)^2+theta^2)^0.5);
4 B" B* V$ z" o! \* q1 q             z(4)^2/((z(4)^2+theta^2)^0.5)];
/ G5 L" f" W' L0 y  Y3 idk=diag([z(1) z(2) z(3) z(4)])*(Beta*z+bf+pi);
Nk1=-exp(0.5*x(13)^2)*(x(13)^2+2)*sin(x(13));Nk2=-exp(0.5*x(14)^2)*(x(14)^2+2)*sin(x(14));
2 C; `# Q" L/ x% `: L/ E$ ^Nk3=-exp(0.5*x(15)^2)*(x(15)^2+2)*sin(x(15));Nk4=-exp(0.5*x(16)^2)*(x(16)^2+2)*sin(x(16));
Nk=diag([Nk1 Nk2 Nk3 Nk4]);
u=Nk*(Beta*z+bf+pi);
; o4 F* x# x4 S! r/ x& m%%%%%%%%%%%%%%%%%%动态方程的表达式
dq=v;
dv=f+gg*u+d;
1 O- h7 w9 e+ X# H& Kdxd=vd;%%%leader
7 T  z* J: i0 a2 kdvd=cos(t);
6 ]. p4 K3 W' D%%%%%%%%%%%%%%%%%%解微分方程
dx=[dq;dv;dWf;dk;dalpha;dxd;dvd];
x
end
7 _( b. D  v+ F; g8 h2 G9 p$ ]& [
8 z6 d* N2 q/ ]0 e7 ~. l0 u+ l7 h图像只出坐标轴不显示图像，x的结果到一定时间之后是NAN，不知道要怎么调试参数，求助
1 k( x- S" e5 R! t, l
$ o, G3 c0 u  I# O1 p; z

shuddkk

帮你顶一下

大小的小

来学习，蹲一个大神

zzz.dan