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

小小鲁班

%%下面是画图的函数
x0=[pi/3;pi/4;-pi/3;-pi/5;
# K* j6 ~( h) j* N; j    0;0;0;0;
* U+ J! s# Q7 a3 W: x7 w$ `    0;0;0;0;
" c& x! T' o! J. g' R% q5 d    0;0;0;0;
    0;0;0;0;
) Q4 f6 W! y* l: S$ N    0;0]; %%初始条件
[t,x]=ode45(@example180928,[0:30],x0);%求解微分函数值
figure(1)
) Y2 ?0 [3 `" Mplot(t,x(:,1),'-.b' ,t,x(:,2),'-m' ,t,x(:,3),'--g' ,t,x(:,4),':k' ,t,x(:,21),'-.r','LineWidth',2.0);
; ]6 Z& S9 A" g# ]9 q! }/ Y( o" [$ C%axis([0 20 -2 5]);
set(gca,'FontSize',10,'LineWidth',1.2); set(gcf,'Color','White');
xlabel('Time (seconds)','FontSize',16.0);
legend('x_{1}','x_{2}','x_{3}','x_{4}','x_{0}');
) u% |$ H( B5 O" A) }: S0 U1 ltitle('The trajectories of five agents','FontSize',16.0 );
%%%%下面是函数定义的程序
1 h. K5 `& H  F$ \function [ dx] = example180928( t,x )
( g! ^. h. x1 k) J7 L   A=[0 1 0 1;%拓扑
      1 0 1 0;
      0 1 0 1;
" h2 F( M# J/ Q3 I      1 0 1 0];
' x! e. W9 \9 f% ~( cH=diag([sum(A(1,) sum(A(2,) sum(A(3,) sum(A(4,:))]);
1 c! B# N" r. E) {' R& R0 {L=H-A;
B=diag([1 0 0 0]);
6 ?6 z! i9 y4 i& I%%%实际系统参数
f=zeros(4,1);
d=zeros(4,1);
g=zeros(4,1);
for i=1:4
    li0=1+0.04*i;li1=0.5*li0;mi=0.4*li0;
3 e7 K, f; M5 @/ I! L( Z    XIi=(cos(x(i))+li0/li1)^2*li1^2/li0;
    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;
5 s8 p- }$ V& U) i4 P    g(i)=0.4/(mi*li0*XIi);
9 Q! R/ _" w1 B/ @    d(i)=0.2*i*cos(t);
, V; ^5 j( H5 S) _# S( Cend
0 ~9 P4 Z( P. t% Dd=[d(1); d(2); d(3); d(4)];
3 Z0 L% q% H+ ^f=[f(1); f(2); f(3); f(4)];
' u, ^; }9 Z: y2 cgg=diag([g(1) g(2) g(3) g(4)]);
- ?: m8 K0 M3 J) ^1 h0 F! _0 ?%%%%%%%5%x1变量x,v,参考轨迹
' P2 W) Z$ ]* A1 W/ ^) |q=[x(1) x(2) x(3) x(4)]';%所有智能体的位置
8 }6 P, C- d; Y9 n8 Y+ g. o$ Ev=[x(5) x(6) x(7) x(8)]';%所有智能体的速度
9 w, D7 M8 X* A9 T, \7 z, A* ZWf=[x(9) x(10) x(11) x(12)]';%未知动态
) {& \' e3 G8 [0 a  f$ N# X- xk=[x(13) x(14) x(15) x(16)]';%nussbaum
7 {8 [6 d& w1 P/ r% T, B9 [alpha=[x(17) x(18) x(19) x(20)]';%未知外部扰动误差
vd=x(22);
+ t6 Z# W; e) q, k%dvd=x(22);
! X$ B7 N$ L2 d2 T" y%%%%%%%%%%%%%%%%跟踪误差z
xi=zeros(4,1);
8 [+ s, ?! `) M) d3 d0 O4 ^for i=1:4  
( d' K9 @+ S* ]    S=0;
: ^5 Q3 V  R: B5 @/ B: I. u# j8 |    for j=1:4
        S=A(i,j)*(x(i)-x(j))+S;
& i. Z6 u" ]& J+ _. w0 U6 E" b, T! \/ [    end
    xi(i)=S+B(i,i)*(x(i)-x(21));
end
, B, Y5 M  A& ~* b. V/ K- \9 ^  f' \xi=[xi(1); xi(2); xi(3); xi(4)];%\xi

9 k4 [' q" J# e4 \) H7 R1 ndxi=zeros(4,1);
for i=1:4
. K! T* t9 F* p4 S5 `! V* K, I5 v    T=0;
    for j=1:4
/ y: L. O* @8 w* J, ^9 S        T=A(i,j)*(x(i+4)-x(j+4))+T;
    end
) v/ u; P# M+ Y* |0 V    dxi(i)=S+B(i,i)*(x(i+4)-x(22));
end
dxi=[dxi(1); dxi(2); dxi(3); dxi(4)];%\dxi
+ \! m; b! t: l! @* Y& v1 X5 C. F%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模糊逻辑系统%%%%%%%%%
( a2 ]4 n4 M2 ~: b5 p" E& cs1=zeros(16,1);
for i=1:4
    phi1=[exp(-(x(i)-3+1)^2/16);
0 O$ m+ c6 W& z) I          exp(-(x(i)-3+2)^2/16);
          exp(-(x(i)-3+3)^2/16);
9 P* X# k8 r% J8 l% H: K+ _$ d2 b          exp(-(x(i)-3+4)^2/16)];
5 E1 a! X; O0 R' w% N& D* iph1i=phi1/sum(phi1);
s1(4*(i-1)+1:4*i)=ph1i;
end
Sf1=s1(1:4);Sf2=s1(5:8);Sf3=s1(9:12);Sf4=s1(13:16); %%\varphi_{i}
Sf=[Sf1 Sf2 Sf3 Sf4];
%%%%%%参数,
Beta=[15 0 0 0 ;
2 Y1 g, S+ v: i+ E; a3 X        0 15 0 0 ;
        0 0 15 0 ;
        0 0 0 15];
Mu=[1 0 0 0 ;
" O! o# Y/ N2 h6 `& h- ^      0 1 0 0 ;
4 ?/ c. h8 o* V! x7 G2 V      0 0 1 0 ;
      0 0 0 1];
+ Y) D& y, J0 A# [Rho=[1 0 0 0 ;
6 O/ I, A9 d/ ], P$ ?      0 1 0 0 ;
      0 0 1 0 ;
- {4 E( L  V$ K/ `2 f- a/ H  H      0 0 0 1];
  z=xi+dxi;
* O" c1 ?3 S/ l6 E) B1 ~' X3 }/ s  z=[z(1);z(2);z(3);z(4)];
  dWf=Mu*Sf*z;
2 V4 f. I$ R# S7 ~! a1 M* G  bf=Sf*Wf;
  theta=exp(-0.05*t);
, a9 l6 i  N. y- _0 z, j# D1 l  pi=[z(1)*x(17)/(z(1)^2+theta^2)^0.5;
    z(2)*x(18)/(z(2)^2+theta^2)^0.5;
6 c" F8 u3 J! x    z(3)*x(19)/(z(3)^2+theta^2)^0.5;
    z(4)*x(20)/(z(4)^2+theta^2)^0.5];
! c; [* X+ @$ w: J( S1 w9 ?9 r; Y  dalpha=Rho*[z(1)^2/((z(1)^2+theta^2)^0.5);
% C' V" u. Z# R             z(2)^2/((z(2)^2+theta^2)^0.5);
& Z2 ?1 h1 b8 q4 T% j             z(3)^2/((z(3)^2+theta^2)^0.5);
! X- Q( T- M- \, _             z(4)^2/((z(4)^2+theta^2)^0.5)];
1 u8 W' m) p7 @6 S: [& q0 {% x4 Zdk=diag([z(1) z(2) z(3) z(4)])*(Beta*z+bf+pi);
6 h9 M2 Q/ j0 x5 g' ]9 m$ MNk1=-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));
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));
$ o# {3 `5 t2 uNk=diag([Nk1 Nk2 Nk3 Nk4]);
, T* m0 \/ S( g& A0 }7 R6 Mu=Nk*(Beta*z+bf+pi);
! G6 ]  Z+ s* u8 W+ M%%%%%%%%%%%%%%%%%%动态方程的表达式
' c- ~# ^) x2 w; b" |5 g6 Kdq=v;
dv=f+gg*u+d;
dxd=vd;%%%leader
+ t$ r" F9 l; ~dvd=cos(t);
- J8 T5 r7 J3 G& a( h/ V%%%%%%%%%%%%%%%%%%解微分方程
' M, w  f/ A8 X" ^. J% wdx=[dq;dv;dWf;dk;dalpha;dxd;dvd];
x
end
" M$ d0 L* J4 J) I6 f5 ?
( l* |1 T* @( R) p图像只出坐标轴不显示图像，x的结果到一定时间之后是NAN，不知道要怎么调试参数，求助
- E0 ~  W- K* ^
' y! b  S8 f7 |$ e; ?* M

shuddkk

帮你顶一下

大小的小

来学习，蹲一个大神

zzz.dan