台車の姿勢制御手引き MATLABプログラム説明

10 

Loading....

Loading....

Loading....

Loading....

Loading....

全文

(1)

台車の姿勢制御 手引き

 mbedマイコンの使い方

1. mbedマイコンのアカウントyagibedからパスワードkosakaでログインイン

2. 姿勢制御のプログラムはPID1のプログラムを用いる

3. mbedマイコンを動作させる場合、PID1内のmain.cppのプログラムをコンパイル

し、mbed本体にプログラムを保存

4. プログラムを本体に保存後、mbedマイコンのリセットボタンを押せばプログラムを

実行する

 プログラム(PID1)の主に使用する場所

1. 19~21行目でPID制御のゲインの値を調整(赤字の値を変更し調整)

float _Kp4th=70; // P gain for PID from motor volt. to angle.

float _Ki4th=25; // I gain for PID from motor volt. to angle.

float _Kd4th=0; // D gain for PID from motor volt. to angle.

姿勢制御の回路図の例 加速度

センサ1

加速度 センサ2

mbedマイコン

DC モータ

モータドライバ

バッテリ

(2)

2. 128行~411行でPID制御を行う

3. 479行~487行で指定した値をCSVファイルで保存

void data2mbedUSB(){ // store data to save to mbedUSB after experiment is over

if( _count_data<1000 ){

data[_count_data][0]=debug[5]; data[_count_data][1]=debug[4];

data[_count_data][2]=debug[3]; data[_count_data][3]=_time;

data[_count_data][4]=_u;

_count_data++; }

4. 488行~493行でTera Termソフトで指定した値を表示させる

pc.printf("%8.1f[s]\t%8.2f\t%8.2f\t%8.2f\t%8.2f\r\n", _time, debug[0], debug[1],

debug[3], debug[4]); // print to tera term

debug[]はPID制御内で値を表示または保存させたい場合に使用

(3)

MATLAB

・新規作成→mfileからプログラム作成を行う。実行はF5でできる。

・CTRL+R コメント化

・CTRL+T コメント化解除

・既存のmfileを実行したい場合、カレントディレクトリを実行したいmfileがある場所に設

定しなければ実行できない

・図はfigで保存しておけばいつでも開けるので必要そうな実験データなどはfigで保存してお

いたほうがよい

simulink

・matlabのコマンドウインドウでsimulinkと打つことで起動

(4)

安全な限界振動の計測のシミュレーション例

s=tf('s');      %連続時間のsを定義

t=0:0.001:20;       %時間を0.001秒刻みで20秒まで

G=1/(s^3+3*s^2+2*s);        %伝達関数1/(s3+3s2+2s)を指定

[Gg,Gp,w]=bode(G);      %周波数ωのときのゲイン余裕Gg、位相余

裕Gp

if Gp(length(Gp))>0, Gp=Gp-360;end    %位相余裕Gpが正の時-360度する

[tmp,i]=min(abs(Gp-(-180)));       %位相交差周波数を求める

y=Gg(i)*sin(w(i)*t+Gp(i)/180*pi);     %出力y=|G(jwu)|sin(G(jwu)t-φ) ※ここで

φ=-180

u=sin(w(i)*t); %入力u=sin(G(jwu)t)

plot(t,[u' y']); %横軸tとしu,yをプロットしfigに示す

grid       %figにgrid表示

(5)

ループ整形のシミュレーション

function[kp,ki,kd]=cont(G,PM,ratio_wgc,ratio_ki)

PM=60;ratio_wgc=0.9;ratio_ki=0.9; %位相余裕の指定

s=tf('s'); t=0:0.001:20;

G=1/(s^3+3*s^2+2*s);

[Gg,Gp,wu]=margin(G); %伝達関数G(s)のゲイン余裕Gg,位相余裕Gp,位相

交差周波数wuを求める

wgc=wu*ratio_wgc %ゲイン交差周波数wgcをwuの1倍弱に設定

ki=(wgc/10)*ratio_ki; %周波数kiをwgcの1/10以下に設定

C=2*pi/360; %かけるとradを度に変換する定数

kd=tan((PM-180)*C-angle(freqresp(G*(1+ki/s),wgc)) )/wgc; %位相余裕PMが望ましい

値になるようkdを設定

kp=1/abs(freqresp(G*(1+ki/s)*(1+kd*s),wgc));       %G(j wgc)K(j wgc)のゲ

インを1にするようにkpを設定

%K(s)=kp(1+ki/s)(1+Kd*s)→kp+ki/s+kd s の形式

k=[kp*(1+ki*kd) (kp*ki) (kp*kd)];       %[a b c]はベクトル

kp=k(1) ki=k(2)

kd=k(3)     %k(1)はkの第1要素

K=kp+ki/s+kd*s; %制御器

bode(G/(1+G),G*K/(1+G*K)) %閉ループ系の制御器なしの場合とありの場合の比較

(6)

リミットサイクルのシミュレーション

s=tf('s');

ts=0.001;       %サンプル時間の指定

z=tf('z',ts);       %離散時間のzを定義

E=tf([0.1],[0.1 1],'InputDelay',0.1); %(0.1*e-0.1s)/(0.1s+0.1)

X=c2d(E,ts); %離散化

G=X*(z-1)/ts        %積分器をオイラー法により離散化

[n,d]=tfdata(G,'v');          %伝達関数Gから分子n,分母dを読み取る

n=[zeros(1,0.1/ts) n];         %nにむだ時間要素をかけ遅らせる

U=zeros(max(size((n))),1); Y=zeros(max(size(d))-1,1); u=1; y=0; for i=1:30000,%;//100/ts

y=n*U - d(2:length(d))*Y; U=[u;U(1:length(U)-1)]; Y=[y;Y(1:length(Y)-1)]; u=1*sign(-y);

umem(i,1)=u; ymem(i,1)=y;

end      %リミットサイクル

[gm,pm,wp180,wg1]=margin(G); clf,

allmargin(G) %ゲイン余裕、位相余裕、および対応する交差周波数を計算.stable1なら安

定、それ以外なら0

disp(1/gm)   %1/gmを表示

disp(gm)    %gmを表示

disp(wp180),  %位相交差周波数表示

Tu=2*pi/wp180;  %T=2*pi/wの関係から限界周期を求める

disp(Tu)    %限界周期表示

disp('1/gm')

disp((max(ymem)-min(ymem))/2 /(4/pi)) %リミットサイクルで求めたゲイン余裕表示

f=0;

(7)

if ymem(i)>0&ymem(i-1)<=0 if f==0, iend=i; f=1; elseif f==1, ibegin=i; f=2; end

end end

wp180est=2*pi/((iend-ibegin)*ts);  %リミットサイクルで求めた限界周期から位相交差周

波数を求める

disp(wp180est) %表示

LTu=(iend-ibegin)*ts;       %データ数にサンプル時間をかけ、限界周期を求める

disp(LTu)           %表示

figure(1),plot([umem ymem]),grid %figure1にumem,ymemをgridつきで表示

axis([iend-612 iend min(umem)*1.2 max(umem)*1.2]) %横軸縦軸の指定

figure(2),bode(G),grid       %figure(2)にボード線図を示す

disp('freqresp of G(j wp180est)'),

disp(abs(freqresp(G,wp180est))),        %伝達関数Gの周波数wp180est時の

周波数応答

disp(angle(freqresp(G,wp180est))/pi*180)    %角度に治し位相を見る

disp('FFT後')

[uu,amp_phi1]=get_Nord(umem(iend-5000:iend),500);  %iend-5000からiendま

でをデータ数500個でフーリエ変換

[yy,amp_phi2]=get_Nord(ymem(iend-5000:iend),500);

[yyy,amp_phi3]=get_Nord(-ymem(iend-5000:iend),500);  %フーリエ変換

max(yy)

max(uu)        %uu,yyの最大値表示

amp=max(uu)/max(yy); %限界ゲイン

disp('amp') disp(amp)

p=amp_phi1-amp_phi2; %入力と出力の位相差

figure(3),plot([uu yy yyy]),grid

figure(4),plot([amp_phi1(:,1),amp_phi2(:,1),amp_phi3(:,1)]),grid FFT=[amp_phi2 amp_phi3 (amp_phi2-amp_phi3)/pi*180-360];

(8)

表示

FFT

指定した周波数で一致させるオイラー法のシミュレーション

s=tf('s'); %連続時間sの定義

Ts=0.1;       %サンプル時間

z=tf('z',Ts);        %離散時間zの定義

[Gm,Gp,wu,wpm]=margin(G);  %Gm,Gp,wu,wpmを伝達関数Gから求める

exp(j*wu*Ts);       %ejwT

R=real(exp(j*wu*Ts)); %ejwT実部

cos(wu*Ts);

J=imag(exp(j*wu*Ts));   %ejwT虚部

A=(wu*Ts)/J B=A*R

X=(A*z-B)/Ts;       %新規法の定義

Y=(z-1)/Ts;       %オイラー法

Z=1/((X+1)*(X+2)*(X+3)); %新規法

O=1/((Y+1)*(Y+2)*(Y+3)); %オイラー法

[Zg,Zp,zwu,zwpm]=margin(Z); [og,op,owu,owpm]=margin(O);

disp('wu zwu owu')

Wu=[wu zwu owu];    %それぞれの位相交差周波数の表示

Wu

100-zwu*100/wu

100-owu*100/wu     %誤差

Ggdb=20*log10(Gm); Zgdb=20*log10(Zg);

ogdb=20*log10(og);   %ゲイン余裕を求める

Gu=[Ggdb Zgdb ogdb];

Gu %それぞれのゲイン余裕を表示

disp('wu owu zwu')

(9)

100-ogdb*100/Ggdb    %誤差

figure(1),bode(G,Z,O),grid;

Getdata+FFTのプログラム例

M=csvread('data.csv'); %data.csvからデータを読み取る ※scilabの場合csvRead

r= M(:,1);       %一列目を目標値r

debug0= M(:,2);    %

y= M(:,3); %出力y

t= M(:,4); %時間t

u= M(:,5); %入力u

clear length

figure(1),clf, plot(t,[u y],'-'), grid, %1枚目に横軸tに対しu yをプロット

xlabel('time [s]'); %x軸のラベル指定

axis([1 1.2 -3.5 3.5])         %グラフを横軸1~1.2縦軸-3.5~3.5の範囲で表

示する

fy=fft(y); %yをFFT

fu=fft(u); %uをFFT

figure(2),plot([abs(fu) abs(fy)]),grid %FFTしたu,yをplot

figure(2)で調波成分を調べ、その基本波成分をXとしそれぞれ位相を見る

X=100; %基本波成分

A=atan2(imag(fu(X)),real(fu(X)))/pi*180 %入力の位相

B=atan2(imag(fy(X)),real(fy(X)))/pi*180 %出力の位相

AとBから位相差を見ることで位相を求められる

(10)

0 100 200 300 400 500 600 700 800 900 1000 0 50 100 150 200 250 300 350 400 450 500

・フーリエ変換をすると赤で囲った部分のように鏡写しになるが無視してよい ・横軸の周波数刻みの求め方は1/(サンプル時間×データ数)[Hz]

例:サンプル時間0.1[s]とし100番目の周波数を求めるには

(1/(0.1×1000))×100=1[Hz]

・黒で囲った部分が基本波成分である

simulinkの一例

Transfer Fcn2 1 s +3s +2s3 2 Transfer Fcn1

1 s +3s +2s3 2 Transfer Fcn

1 s +3s +2s3 2

To Workspace simout Step Scope Gain2 10 Gain1 6 Gain 2

伝達関数G=1/(s3+3s2+2s)に対するstep応答をみている。この場合ゲインを2,6,10と変え、

目標値を含めた4つの応答を表示させている。Scopeでその応答を見ることができる。Scope

では図を保存することができないので、simoutでMATLABのworkspaceにデータを移し

plotすることで保存できる。

ワークスペースsimout→signals→valuesでplotすることができる。しかしこの場合横軸が データ数になっている。その場合は

(11)

plot(t,simout.signals.values);

とすればplot可能である

PID制御をsimulinkで示す。上から比例ゲイン、積分ゲイン、微分ゲインを示している。

Updating...

参照

Updating...

関連した話題 :