Anh Nguyễn

nvtienanh.info

Code Matlab: Khảo sát tính ổn định theo tiêu chuẩn Routh

2014-05-06 Anh Nguyễnnotes

Trong bản 2.0 này, tích hợp thêm việc tìm ẩn số K để hệ ổn định.Khuyến nghị, nên cài Matlab2012b, bởi vì qua quá trình test, máy nào dùng Matlab cũ hơn có thể bị lỗi ở một vài hàm. Cách sử dụng thì đã có kèm theo trong phần hướng dẫn code, tài liệu tham khảo là cuốn sách: Lý thuyết điều khiển tự động - Nguyễn Phương Hà - NXB ĐHQG HCM. Hình minh họa

HÌnh minh họa

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% ROUTH TABLE 2.0 UPDATE 01/05/2013                                     %%
%% AUTHOR: NHOM 5 CDT2010- DONG LUC HOC & DIEU KHIEN                     %%
%% DU LIEU VAO : TU SO (Num), MAU SO (Den) CUA HAM TRUYEN                %%
%% ==TRUONG HOP KHONG CO AN SO K:==                                      %%
%% VI DU:            S+1                                                 %%
%%        G(s) = ------------ %%
%%               S^2 + 2*S +1                                            %%
%% TieuChuanRouth([1 1],[1 2 1]);                                        %%
%% DU LIEU XUAT RA:                                                      %%
%%       KET LUAN HE ON DINH HAY KHONG ON DINH                           %%
%% ==TRUONG HOP CO AN SO K:==                                            %%
%% VI DU:               K*(S-1)                                          %%
%%        G(s) = ------------------- %%
%%                S(S-2)(S^2+3*S+10)                                     %%
%% syms k;                                                               %%
%% tu=[k -k];                                                            %%
%% mau=conv([1 0],conv([1 -2],[1 3 10]));                                %%
%% TieuChuanRouth(tu,mau);                                               %%
%% DU LIEU XUAT RA:                                                      %%
%% GIA TRI K DE HE ON DINH.                                              %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function TieuChuanRouth(Num,Den)
CoAn=false;
OnDinh=true;
K=sym('K');
e=sym('e');
% Kiem tra xem du lieu nhap vao co An K hay khong
for i=1:length(Num)
   if ~isempty(symvar(Num(i)))
      CoAn=true;
      K=symvar(Num(i));
   end
end
if ~CoAn
    for i=1:length(Den)
        if ~isempty(symvar(Den(i)))
            CoAn=true;
            K=symvar(Den(i));
        end
    end
end
if CoAn
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Xu ly truong hop co an K
    %% Xuat ra gia tri K de he on dinh
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    temp=[zeros(1,length(Den)-length(Num)) Num];
    PTDacTrung=temp+Den;
    row=length(PTDacTrung);
    disp('Phuong trinh dac trung: ')
    temp=0;
    syms s;
    for i=1:length(PTDacTrung)
        temp=temp+PTDacTrung(i)*s^(length(PTDacTrung)-i);
    end
    disp(temp);
    clear temp;
    %% Neu co phan tu <=0 thi khong on dinh %%
    for i=1:row
        if isempty(symvar(PTDacTrung(i)))
            if sym2poly(PTDacTrung(i))<=0
                OnDinh=false;
                break;
            end
        end
    end
    %% Nhap cac he so vao Bang Routh %%
    if OnDinh
        colum=round(row/2);
        c=sym('Table',[row colum]);
        c=PTDacTrung(1:2:row);% Hang 1
        % Nhap du lieu cho hang thu 2
        if length(PTDacTrung(2:2:row))\=0
                    c(i,j)=c(i-1,j)*Bac_Ap;
                    Bac_Ap=Bac_Ap-2;
                    j=j+1;
                end
            else
                % Truong hop phan tu cot 1 = 0
                if isempty(symvar(c(i,1)))
                    if sym2poly(c(i,1))==0
                        c(i,1)=e;
                    end
                end
            end
        end
    end
    disp('BANG ROUTH:   ')
    disp(c)
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% TINH TOAN K DE HE ON DINH
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Kiem tra co he so cot 1 <=0 thi khong on dinh
    DacBiet1=false;
    KhongCoAn=true;% Cot 1 chi toan la con so
    for i=1:row
        if isempty(symvar(c(i,1)))
            if sym2poly(c(i,1))<=0
                OnDinh=false;
                break;
            end
        else
            if length(symvar(c(i,1),2))==2
                DacBiet1=true;
                KhongCoAn=false;
            else
                if length(symvar(c(i,1),2))==1
                    temp=symvar(c(i,1),2);
                    KhongCoAn=false;
                    if  temp==e
                        DacBiet1=true;
                    end
                end
            end
        end
    end
    % Giai cac he so co an K de he on dinh
    KetQua=zeros(1,2);
    if OnDinh&&~KhongCoAn
        if DacBiet1
            % Xu ly truong hop dac biet 1 co 2 an K va e
            for i=1:row
                [Tu,Mau]=numden(c(i,1));
                f=Tu*Mau;
                if symvar(f,2)==e
                % Phan tu c(i,1) chi chua an e
                    temp=sym2poly(f);
                    for j=length(temp):1
                        if (temp(j)<0)
                            OnDinh=false;
                            break;
                        else
                            if (temp(j)==0)
                                continue;
                            else
                                break;
                            end
                        end
                    end
                else
                    if symvar(f,2)==K
                        Nghiem=sym2poly(solve(f));
                        Nghiem=sort(Nghiem)';
                        if  f(Nghiem(1)-1)>0
                            KetQua=[KetQua;-Inf Nghiem(1)];
                        end
                        for j=1:length(Nghiem)-1
                            if f((Nghiem(j)+Nghiem(j+1))/2)>0
                                KetQua=[KetQua;Nghiem(j) Nghiem(j+1)];
                            end
                        end
                         if f(Nghiem(end)+1)>0
                            KetQua=[KetQua;Nghiem(end) Inf];
                         end
                    end
                    % Phan tu c(i,1) chua an e va K
                    % Vi du c(3,1)=K-e+1;
                    [tu,mau]=numden(c(i,1));
                    f=tu*mau;
                    temp=subs(f,e,0);
                    if isempty(symvar(temp,1))
                        if temp>0
                            KetQua=[KetQua;-Inf Inf];
                        else if temp<0
                                continue;
                            else
                              temp=sym2poly(subs(f,K,1));
                              Flag1=false;
                              j=length(temp);
                              while ~Flag1
                                  if temp(j)<0
                                       Flag1=true;
                                  end
                              end
                              if ~Flag1
                                   KetQua=[KetQua;-Inf Inf];
                              end
                            end
                        end
                    else
                        % Write code here
                        Nghiem=sym2poly(solve(temp));
                        Nghiem=sort(Nghiem)';
                        if temp(Nghiem(1)-1)>0
                            KetQua=[KetQua;-Inf Nghiem(1)];
                        end
                        for j=1:length(Nghiem)-1
                            if temp((Nghiem(j)+Nghiem(j+1))/2)>0
                                KetQua=[KetQua;Nghiem(j) Nghiem(j+1)];
                            end
                        end
                        if f(Nghiem(end)+1)>0
                            KetQua=[KetQua;Nghiem(end) Inf];
                        end
                    end
                end
            end
        else
            % Xu ly truong hop chi co an K
            % Trong doan code nay giai bat
            %  phuong trinh de tim K theo
            %  dieu kien c(i,1)>0
            % Ket qua luu vao Mang KetQua
            for i=1:row
                if ~isempty(symvar(c(i,1)))
                    f(K)=c(i,1);
                    [Tu,Mau]=numden(c(i,1));
                    Nghiem=sym2poly(solve(Tu*Mau));
                    Nghiem=sort(Nghiem)';
                    if  f(Nghiem(1)-1)>0
                        KetQua=[KetQua;0 Nghiem(1)];
                    end
                    for j=1:length(Nghiem)-1
                        if f((Nghiem(j)+Nghiem(j+1))/2)>0
                            KetQua=[KetQua;Nghiem(j) Nghiem(j+1)];
                        end
                    end
                    if f(Nghiem(end)+1)>0
                        KetQua=[KetQua;Nghiem(end) Inf];
                    end
                end
            end
            KetQua(1,:)=[];
        end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   LOC NGHIEM
    i=1;j=1;
    KetQuaCuoi=zeros(1,2);
    while i\=KetQua(i+1,2)
                KetQua(i,:)=[NaN NaN];
            else
                trai=max(KetQua(i,1),KetQua(i+1,1));
                phai=min(KetQua(i,2),KetQua(i+1,2));
                KetQua(i,:)=[trai phai];
                KetQua(i+1,:)=[trai phai];
                KetQuaCuoi(j,:)=[trai phai];
                j=j+1;
            end
            i=i+1;
    end
    disp('Khoang gia tri cua K de he on dinh: ')
    disp(KetQuaCuoi);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Ket thuc truong hop K la an so
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
else
    % Xu ly Truong hop khong co an K
    temp=[zeros(1,length(Den)-length(Num)) Num];
    PTDacTrung=temp+Den;
    disp('Phuong trinh dac trung: ')
    disp(poly2sym(PTDacTrung,'s'));
    clear temp;
    % Kiem tra cac he so PT dac trung >0
    for i=1:length(PTDacTrung)
        if PTDacTrung(i)<=0
        OnDinh=false;
        break;
        end
    end
    if OnDinh
        sohang=length(PTDacTrung);
        socot=round(sohang/2);
        c=sym('BangRouth',[sohang socot]);% Khoi tao bang Routh
        % Nhap du lieu, tinh toan bang Routh
        c(1,:)=PTDacTrung(1:2:sohang);
        if mod(sohang,2)==0
            c(2,:)=PTDacTrung(2:2:sohang);
        else
            c(2,:)=[PTDacTrung(2:2:sohang) 0];
        end
        % Vong lap tinh cac he so con lai cua bang Routh
        for i=3:sohang
            for j=1:socot
                if j end
            % Xu ly Truong hop dac biet 2:
            % 1 hang bang 0 het
            if isequal(c(i,:),zeros(1,socot))
               for j=1:socot
                   c(i,j)=c(i-1,j)*(sohang-i-(j-1)*2);
               end
            else if c(i,1)==0
                    c(i,1)=e;
                 end
            end
        end
        disp(c);
        if OnDinh
            disp('He on dinh')
        else
            disp('He KHONG on dinh')
        end
    else
       % He so nhap vao <=0 thi ket luan ngay
       disp('Khong On Dinh');
    end
end
% end function
end