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

(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 制御内で値を表示または保存させたい場合に使用例 debug[1]=y;

(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/(s³^+3s²^+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 表示

w(i)　　　　　　　%位相交差角周波数の確認

(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)) %閉ループ系の制御器なしの場合とありの場合の比較 grid

(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,分母ｄを読み取る 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;

for i=length(ymem):-1:2

(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];

disp('amp_phi2 amp_phi3 (amp_phi2-amp_phi3)/pi*180-360'),　%y,-y,y-(-y)の位相の

(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);_%e^jwT R=real(exp(j*wu*Ts)); %e^jwT_実部 cos(wu*Ts);

J=imag(exp(j*wu*Ts));_%e^jwT_虚部 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')

100-Zgdb*100/Ggdb

(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 +2s³ ² Transfer Fcn1

1 s +3s +2s³ ² Transfer Fcn

1 s +3s +2s³ ²

To Workspace simout Step

Scope

Gain2 10 Gain1

6 Gain 2

伝達関数 G=1/(s³^+3s²^+2s)に対する step 応答をみている。この場合ゲインを 2,6,10 と変え、目標値を含めた 4 つの応答を表示させている。Scope でその応答を見ることができる。Scope では図を保存することができないので、 simout で MATLAB の workspace にデータを移し plotすることで保存できる。

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

Matlabコマンド欄で t=0:0.1:20　などと横軸の指定をし

(11)

plot(t,simout.signals.values); とすれば plot 可能である

PID制御を simulink で示す。上から比例ゲイン、積分ゲイン、微分ゲインを示している。 Gainの数値を変えることでパラメータ調整できる。図の場合比例ゲインのみである。