MATLAB函数定义

D-3-1: routh()—由特征多项式构造劳斯阵列表

function [rtab,info]=routh(den)

info=[];

vec1=den(1:2:length(den)); nrT=length(vec1);

vec2=den(2:2:length(den)-1);

rtab=[vec1; vec2, zeros(1,nrT-length(vec2))];

for k=1:length(den)-2,

alpha(k)=vec1(1)/vec2(1);

for i=1:length(vec2),

a3(i)=rtab(k,i+1)-alpha(k)*rtab(k+1,i+1);

end

if sum(abs(a3))==0

a3=polyder(vec2);

info=[info,'All elements in row ',...

int2str(k+2) ' are zeros;'];

elseif abs(a3(1))

a3(1)=1e-6;

info=[info,'Replaced first element;'];

end

rtab=[rtab; a3, zeros(1,nrT-length(a3))];

vec1=vec2; vec2=a3;

end

D-3-2: hurwitz()—由特征多项式构造胡尔维茨矩阵

function H=hurwitz(den)

n=length(den)-1;

for i=1:n

i1=floor(i/2);

if i==i1*2

hsub1=den(1:2:n+1);

i1=i1-1;

else

hsub1=den(2:2:n+1);

end

l1=length(hsub1);

H(i,:)=[zeros(1,i1),hsub1,zeros(1,n-i1-l1)];

end

D-3-3: posdef()—判定矩阵的正定性

function [key,sdet]=posdef(A)

[nr,nc]=size(A); sdet=[];

for i=1:nr

sdet=[sdet,det(A(1:i,1:i))];

end

key=1;

if any(sdet<=0)

key=0;

end

D-4-1：

stepchar（）—计算时域响应的性能指标

function [pos,tr,ts,tp]=stepchar(g0,delta)

[y,t]=step(g0);

[mp,ind]=max(y); dimt=length(t); yss=y(dimt);

pos=100*(mp-yss)/yss;

tp=t(ind);

for i=1:dimt

if y(i)>=1

tr=t(i);

break;

end

end;

for i=1:length(y)

if y(i)<=(1-delta)*yss|y(i)>=(1+delta)*yss

ts=t(i);

end

end

function [ngc,dgc]=lead1(ng0,dg0,s1)

ngv=polyval(ng0,s1); dgv=polyval(dg0,s1);

g=ngv/dgv; theta=angle(g); phi=angle(s1);

if theta>0

phi_c=pi-theta;

end

if theta<0;

phi_c=-theta

end

theta_z=(phi+phi_c)/2; theta_p=(phi-phi_c)/2;

z_c=real(s1)-imag(s1)/tan(theta_z);

p_c=real(s1)-imag(s1)/tan(theta_p);

nk=[1 -z_c]; dgc=[1 -p_c]; kc=abs(p_c/z_c);

if theta<0

kc=-kc

end

ngc=kc*nk;

D-4-3：lead2（）—根轨迹几何法设计串联超前校正器

function [ngc,dgc]=lead2(ng0,dg0,s1)

ngv=polyval(ng0,s1); dgv=polyval(dg0,s1);

g=ngv/dgv; theta1=angle(g);

zc=real(s1)-imag(s1)/tan(theta); t=-1/zc;

ngc=[t,1];dgc=[1];

D-4-4：lead3（）—根轨迹解析法设计串联超前校正器

function [ngc,dgc]=lead3(ng0,dg0,KK,s1)

ngv=polyval(ng0,s1); dgv=polyval(dg0,s1); g=ngv/dgv;

thetag=angle(g); mg=abs(g); ms=abs(s1); thetas=angle(s1);

tz=(sin(thetas)-KK*mg*sin(thetag-thetas))/(KK*mg*ms*sin(thetag));

tp=-(KK*mg*sin(thetas)+sin(thetag+thetas))/(ms*sin(thetag));

ngc=[tz,1];dgc=[tp,1];

D-4-5：lead4（）—伯德图几何法设计串联超前校正器

function [ngc,dgc]=lead4(ng0,dg0,KK,Pm,w)

[mu,pu]=bode(KK*ng0,dg0,w);[gm,pm,wcg,wcp]=margin(mu,pu,w);

alf=ceil(Pm-pm+5);phi=(alf)*pi/180;

a=(1+sin(phi))/(1-sin(phi)),dbmu=20*log10(mu);mm=-10*log10(a);

wgc=spline(dbmu,w,mm), T=1/(wgc*sqrt(a)),

ngc=[a*T,1];gc=[T,1];

D-4-6：lead5（）—伯德图几何法设计串联超前校正器

function [ngc,dgc]=lead5(ng0,dg0,KK,Pm,wc,w)

[mu,pu]=bode(KK*ng0,dg0,w);

ngv=polyval(KK*ng0,j*wc); dgv=polyval(dg0,j*wc);

g=ngv/dgv; theta=180*angle(g)/pi;

alf=ceil(Pm-(theta+180)+5); phi=(alf)*pi/180;

a=(1+sin(phi))/(1-sin(phi)),

dbmu=20*log10(mu); mm=-10*log10(a);

wgc=spline(dbmu,w,mm), T=1/(wgc*sqrt(a)),

ngc=[a*T,1]; dgc=[T,1];

D-4-7：lead6（）—伯德图几何法设计超前校正器

function [ngc,dgc]=lead6(ng0,dg0,KK,wc)

ngv=polyval(KK*ng0,j*wc); dgv=polyval(dg0,j*wc);

g=ngv/dgv; mg0=abs(g); t=sqrt(((1/mg0)^2-1)/(wc^2)); %幅值相加为零

ngc=[t,1];dgc=[1];

D-4-8：lead7（）—伯德图解析法设计超前校正器

function [ngc,dgc]=lead7(ng0,dg0,KK,Pm,wc,w)

ngv=polyval(ng0,j*wc); dgv=polyval(dg0,j*wc); g=ngv/dgv;

thetag=angle(g); mg=abs(g); thetar=Pm*pi/180;

tz=(1+KK*mg*cos(thetar-thetag))/(-wc*KK*mg*sin(thetar-thetag));

tp=(cos(thetar-thetag)+KK*mg)/(wc*sin(thetar-thetag));

ngc=[tz,1];dgc=[tp,1];

D-4-9：lead8（）—根据

ω

c

采用伯德图法设计超前校正器

function [ngc,dgc]=lead8(ng0,dg0,wc)

ngv=polyval(ng0,j*wc);dgv=polyval(dg0,j*wc);g=ngv/dgv;

g1=abs(g);a=1/(g1.^2),T=1/(wc*sqrt(a)),

dgc=[a*T,1];ngc=[T,1];

D-4-10：lag1（）—根轨迹几何法设计串联滞后校正器

function [ngc,dgc,k]=lag1(ng0,dg0,KK,s1,a)

ngv=polyval(ng0,s1); dgv=polyval(dg0,s1);

g=dgv/ngv; k=abs(g); beta=k/KK;

[kosi1,wn1]=s2kw(s1);

zc=-wn1*sin(a*pi/180)/sin(pi-atan(sqrt(1-kosi1^2)/kosi1)-(a*pi/180))

pc=beta*zc;

ngc=beta*[1,-zc];dgc=[1,-pc];

ngc=beta*[1,-zc];dgc=[1,-pc];

D-4-11：lag2（）—伯德图几何法设计滞后校正器

function [ngc,dgc]=lag2(ng0,dg0,w,KK,Pm)

[mu,pu]=bode(KK*ng0,dg0,w); wgc=spline(pu,w,Pm+5-180),

ngv=polyval(KK*ng0,j*wgc); dgv=polyval(dg0,j*wgc); g=ngv/dgv;

alph=abs(1/g), T=10/alph*wgc,

ngc=[alph*T,1]; dgc=[T,1];

D-4-12：lag3（）—伯德图几何法设计滞后校正器

function [ngc,dgc]=lag3(ng0,dg0,w,KK,wc)

ngv=polyval(KK*ng0,j*wc); dgv=polyval(dg0,j*wc); g=ngv/dgv;

alph=abs(1/g), T=10/(alph*wc),

ngc=[alph*T,1]; dgc=[T,1];

D-4-13: s2kw( )求闭环极点对应的阻尼比、无阻尼振荡频率

function [kosi,wn]=s2kw(s)

kosi=1/sqrt(1+(imag(s)/real(s)).^2);

wn=-real(s)/kosi;

D-4-14: kw2s( )求阻尼比、无阻尼振荡频率对应的闭环极点

function s=kw2s(kosi,wn)

s=-kosi*wn+j*wn*sqrt(1-kosi.^2);

D-4-15: bpts2s( )，求满足性能指标的闭环主导极点

function s=bpts2s(bp,ts,delta)

kosi=sqrt(1-(1/(1+((1/pi)*log(1/bp)).^2)));

wn=log(1/delta*sqrt(1-kosi.^2))/(kosi*ts);

s=-kosi*wn+j*wn*sqrt(1-kosi.^2);

D-5-1：bass_pp()—采用bass_pp算法进行极点配置

function K=bass_pp(A,b,p)

if rank(ctrb(A,b))~= length(b) 'No !!!',

else

n=length(b);alpha=poly(diag(p',0));

a=poly(A);aa=[a(n:-1:2),1];W=hankel(aa);M=ctrb(A,b);

K=(alpha(n+1:-1:2)-a(n+1:-1:2))*inv(W)*inv(M);end

D-5-2：bass_pp( )—采用疋田算法进行极点配置

function K=pitian(A,B,p)

[n,r]=size(B);zeta=[];

for j=1:1:n

k=ceil(j/r);

Ir=eye(r);zeta1=Ir(:,j-(k-1)*r);zeta=[zeta,zeta1];end

V=[];

for i=1:1:n

Inpi=p(i)*eye(n);

V1=(inv(A-Inpi))*B*zeta(:,i);

V=[V,V1];

end

K=zeta*(inv(V));

D-5-3：pp_sifuI( )—含有积分器的I型伺服系统的设计

function [K,x_ss,y_ss,u_ss]=pp_sifuI(A,b,c,p,v)

K=acker(A,b,p);

x_ss=-(inv(A-b*K))*b*(K(1,1))*v;

y_ss=c*x_ss;

u_ss=-K*x_ss+v*K(1,1);

D-5-4：pp_sifu0( )—含有积分器的0型伺服系统的设计

function [K,kI,x,y,t,x_ss,y_ss,u_ss,zeta_ss]=pp_sifu0(A,b,c,p,v,t)

n=length(A);

AA=[A,zeros(n,1);-c,0];bb=[b;0];

KK=acker(AA,bb,p);

K=KK(1:n);kI=-KK(n+1);

X=(inv([[A,b];[-c,0]]))*([zeros(n,1);-v]);

x_ss=X(1:n,1);

y_ss=c*x_ss;

u_ss=X(n+1,1);

zeta_ss=([K,1])*X/kI;

[y,x,t]=step([A-b*K,b*kI;-c,0],[zeros(n,1);1],[c,0],0,1,t);

D-5-5：lqr_c( )—最优跟踪系统的设计

function [P,g,K1,K2]=lqr_c(A,B,C,Q,R,yr)

P=are(A,B*(inv(R))*B',C'*Q*C);

g=(inv(P*B*(inv(R))*B'-A'))*C'*Q*yr;

K1=-(inv(R))*B'*P;

K2=(inv(R))*B'*g;

D-5-6：decoupling( )—动态解耦控制算法

function [G,K,L]=jieou(A,B,C)

[m,n]=size(C);

for i=1:m

dd=[];

for j=0:1:n-1

if (max(abs(C(i,:)*A^j*B)))>0 dd=[dd;j];

end

end

if length(dd)==0 d(i)=n-1,

else d(i)=min(dd);

end

end

E=C(1,:)*A^d(1)*B;

for i=2:m

E=[E;C(i,:)*A^d(i)*B];

end

L=inv(E);

F=C(1,:)*A^(d(1)+1);

for i=2:m

F=[F;C(i,:)*A^(d(i)+1)];

end

K=L*F;

AA=A-B*L*F;BB=B*L;CC=C;DD=zeros(m);

G=tf(ss(AA,BB,CC,DD));

D-5-7：decoupling_s( )—静态解耦控制算法

function [vv,K,L]=

decoupling_s(A,B,C,p,dd)

[m,n]=size(C);ss=[];CC=ctrb(A,B);nB=rank(CC);

if nB==n,vv=1;

else

[Ac,Bc,Cc]=ctrbf(A,B,C);Anc=Ac(1:n-nB,1:n-nB);ee=eig(Anc);

for i=1:length(ee)

if real(ee(i))>=0,ss=[ss,ee(i)];end,end

if length(ss)>0,vv=0;

else,vv=1;end,end

if (rank([A,B;C,zeros(m,m)]))~=(n+m), vv=0;end

p=[-1;-2;-3];K=place(A,B,p);d=diag(dd);

L=-(inv(C*(inv(A-B*K))*B))*d;

D-5-8：simobsv( )—全维观测器设计

function [xh,x,t]=simobsv(A,B,C,L)

G=ss(A,B,C,0);

[y,t,x]=step(G);

[y1,xh1]=step((A-L*C),B,C,0,1,t);

[y2,xh2]=lsim((A-L*C),L,C,0,y,t);

xh=xh1+xh2;

plot(t,x,'-',t,xh,':')

D-5-9：jiangweiguanceqi( )—降维观测器设计

function [L,Az,By,Bu,Cz,Dy]=jiangweiguanceqi(A,B,C,R,p)

[m,n]=size(C);

P=[C;R];Q=inv(P);

Q1=Q(:,1:m);Q2=Q(:,m+1:n);

AA=P*A*Q;BB=P*B;

AA11=AA(1:m,1:m);AA12=AA(1:m,m+1:n);

AA21=AA(m+1:n,1:m);AA22=AA(m+1:n,m+1:n);

BB1=BB(1:m,:);BB2=BB(m+1:n,:);

KK=place(AA22',AA12',p);

LL=KK';

Az=AA22-LL*AA12;

By=AA21-LL*AA11+(AA22-LL*AA12)*LL;

Bu=BB2-LL*BB1;

Cz=Q2;

Dy=Q1+Q2*LL;

L=LL;

D-6-1: ode4( )—计算定步长四阶龙格-库塔法

function [t,y]=ode4(A,B,C,D,x0,h,r,v,n,t0,tf)

Ab=a-B*v*C;B=B;C=C;x=x0';y=0;t=t0;

N=round((tf-t0)/h);

for i=1:N

k1=Ab*x+B*r;

k2=Ab*(x+h*k1/2)+B*r;

k3=Ab*(x+h*k2/2)+B*r;

k4=Ab*(x+h*k3)+B*r;

x=x+h*(k1+2*k2+2*k3+k4)/6;

y=[y,C*x];t=[t,t(i)+h];

end

D-6-2: Saturation( )饱和非线性环节的仿真

function uc=Saturation(ur,s1)

if(abs(ur)>=s1)

if (ur>0)

uc=s1;

else uc=-s1;

end

else uc=ur;

end

D-6-3: DeadZone( )死区非线性环节的仿真

function uc= DeadZone(ur,s1)

if(abs(ur)>=s1)

if (ur>0)

uc=ur-s1;

else uc=ur+s1;

end

else uc=0;

end

D-6-4: backlash( )间隙非线性环节的仿真

function [uc,uss]=backlash(urs,ur,ucs,s1)

if(ur>urs)

if((ur-s1)>=ucs)

uc=ur-s1;

else uc=ucs;

end

else if(ur

if ((ur+s1)<=ucs)

uc=ur+s1;

else uc=ucs;

end

else

uc=ucs;

end

end

uss=ur;

D-6-5: diffstate( )离散系统仿真

function [t,xx]=diffstate(G,H,x0,u0,N,T)

xk=x0;u=u0;t=0

for k=1:N

xk=G*xk+H*u;

x(:,k)=xk;

xx=[x0,x];t=[t,k*T];

end;