23_PI制御
以下の内容の実施には，シミュレーションバージョン1.1.0以上が必要になります
聞いたことありますか？PID制御。
22でP制御（比例制御，Proportional control）については勉強して頂きました。
基本的な考え方は，目標値と計測値の差を取り，それに定数倍して入力に反映させるものでした。
このP制御，便利なのですが若干物足りない部分があります。
それを修正するために，偏差を積分（Integral）及び微分（Derivative）します。I制御とD制御と呼ばれるものです。
これを好きに組み合わせて，PI制御，PD制御，やがてはPID制御と呼ぶのです。
このPID制御は，ある程度の所までなら高校レベルの数学+αで説明できますので，ここで取り扱います。
ちょっと難しい話になるかもしれませんが，ゆっくり手を動かしながら見ていきましょう。
質問はSlackでいつでも歓迎します！
簡単にP制御の復習をします。分からなくなったら，22を読み返して見てください。
繰り返しですが，P制御は目標値と計測値の偏差を定数倍し，入力に反映させるものでした。
数式でみてみましょう。
目標値をr，計測値をz，その差をe，定数倍のゲインをKp，入力をuとすると
となりますね。
図で示すとこんな感じになります。
ざっくりとした意味は，「目標値から計測値引いて，Kpかけてシステム（今回はシミュレーション）に突っ込み，返ってくる値をセンサで読む」となります。
分からなくていいですが，見覚えがある状態にしてくれるとありがたいです。
ちょっとシミュレーションしてみましょうか
今回，諸々の都合によりセンサを大量に使います。その数なんと15個！
それを5㎜間隔で配置します。
多分制御の人がラインセンサ5mm間隔で15個つけろといったら，回路の人と一悶着あるでしょう。
ここでは，回路の人を何とかねじ伏せたとして，robot.mをいじってセンサを設置します。普通に書いてもいいのですが，ここではちょっとまとめて書きます。いつものlist_light_sensorをこんな感じにします。
list_light_sensor = zeros(15,2);
for i = 1:15
list_light_sensor(i,:) = [100 35-(i-1)*5];
end
list_range_sensor = [100 0 0;];一旦実行してみましょうか。simulationを走らせてみてください。
みたいな感じになりましたか？
もし描画が重くなった人は，robot.mを
light_sensor_visible = "off";とすれば何とかなるかもしれません．それでも駄目ならこのままいきます．ごめんなさい．
さて，もう少し色々いじります。
まずフィールド番号は，01，初期状態は[200; 500; -pi/10]としてください。
さらに，robot.mをいじり
system_lebel = 2;とします。
system_lebel変数ではシステムに加える外乱や誤差を変更できます。
system_lebelが1や0の時は，この後のP制御の問題点が発生しませんので，ここでシステムレベルを変更します。
システムレベルを変更すると，他の課題等で，今まで上手くいっていた制御が使い物にならなくなる可能性があるので，必要に応じて元に戻すようにしましょう。（課題等においては，特に指定が無ければレベル1でOKです。）
system_lebelという変数がrobot.mに書かれていない（コメントアウトすら存在しない）場合，アップデートを行ってください。ver1.1.0以上が必要です。
最後に，controllerをいじります。次のようにしてみましょう。
persistent tr_sensor
Kp = 0.01;
r = 0;
if isempty(tr_sensor)
tr_sensor = -7:1:7;
end
z = 0;
Nz = ones(1,15)*( value_light_sensor(:,1)<50 );
if Nz > 0
z = ( tr_sensor*( value_light_sensor(:,1) < 50 ) ) / Nz;
end
e = r-z;
u = Kp * [1;-1] * e + [0.3; 0.3];長いわ！！と思った人はコピペしてください。
軽くコードの説明をしますが，読み飛ばしてもいいです。
まず，persistentというのは，matlabでの永続変数というものを現します。
persistent tr_sensorMATLABでは，関数が呼び出される度に変数はリセットされます。リセットされないようにするのが永続変数です。
例えば，時刻t = 1.0[s]の時，u = [0.5; 0.5];としておいても，次に関数が呼び出された，例えばt = 1.04[s]の時には，uにその値は残っていません。再度，u = [0.5; 0.5];とする必要があります。
persistentを付ければ，その値が保持されるのです。
ここでは，tr_sensorというベクトルはずっと同じ値をとるので，最初の一回のみ計算して，後はその値を取っておくという意味で，persistentを付けています。
Kp = 0.01;
r = 0;Kpは比例制御の定数，ゲインと呼ばれるものです。ここでは0.01にしています。
続いて，目標値rを0とします。つまり，機体はラインの中央に行けと，ここでは言っていることになります。
これら二つは永続変数にすることも考えられますが，まあ，一つの値を代入するだけなので，一般変数でもいいでしょう。
if isempty(tr_sensor)
tr_sensor = -7:1:7;
endif(isempty(~))みたいな部分がありますね，これは，永続変数の初期化をしています。controllerの初めての呼び出しの時に実行され，tr_sensorに中身を与えます。
2回目以降はtr_sensorは空ではないので，呼び出されません。
tr_sensor = -7:1:7;としてありますが，これはMATLABの記法で，tr_sensor = [-7,-6,-5,...,5,6,7];という1行15列のベクトルを生成できます。
雰囲気は，－7を起点として，1刻みで7までよろしく，という感じです。
ここからはセンサの読み取りに入りますが，
Nz = ones(1,15)*( value_light_sensor(:,1)<50);ここでは，黒線を検知したセンサの個数を計算しています。なんでこれで出来るのかが分からない場合はそういうもんだと思っておいてください
if Nz > 0
z = ( tr_sensor*( value_light_sensor(:,1) < 50 ) ) / Nz;
endラインを読んだセンサが存在していた場合，そのセンサ値の平均値を取ります。
例えば，0と－1のセンサがラインを読んでいた場合，計測値は－0.5となるわけです。
のこりの場所ではP制御を実施しています。
ふう…読み飛ばし領域はここまでです。次からはゆっくり読んでいきましょう。
ようやく本題に入ります。
simulation.mの下のほうをいじり，ちょっとグラフを出すことにします。シミュレーションカウントを横軸，縦軸にロボットのy座標を取ります。
figure
j = 1:k;
plot(j, q(1:k,2));
grid onそれでは，実行してみましょう。
グラフ，出てきましたか？よく見て下さね
いいですか，よく思い出してください。ロボットはラインの中心に向かうように設定したはずです．つまりロボットのy座標は500にならねばいけません。
ですが収束値は505の少し下位ですね。
そう。これがP制御の問題点の一つ目，定常偏差になります。
定常偏差は目標値の関数系や外乱といった要因により生じます。
この定常偏差，特に精密な作業を必要とする場合大問題になります。
定常偏差は，ゲインを上げる事で小さくすることが可能です。
試しにやってみましょうか
controller.mを
Kp = 0.1;にして実行してみましょう。
こちらで取得したグラフはこんな感じになりました。
確かに定常偏差は小さくなりました（501より下位の値を目指している）が，ぶれすぎです。
これではちょっと格好良い制御とは言えないでしょう。
実際，定常偏差が十分小さかったり，ゲインをある程度上げてもいいなら，定常偏差は気にならない場合もあります。その場合はP制御のみでもいいでしょう。
ですが，ここではもっと格好良く制御し，定常偏差をなくすことを試みます。
その為に使うのが，I制御です。
余談ですが，先ほどKpを0.01から0.1に変えました。
パラメータを調整するときは，まずオーダー（桁数）をいじって様子を見てみましょう。
つまり，Kpを0.01から下げるならば0.001を試して様子を見ます。
以上，余談でした
I制御（積分制御，Integral Control）はその名の通り，偏差を積分します。
P制御だけだと，ずっと偏差が残りっぱなしでも，偏差に定数倍するだけなので定常偏差を補正できません。偏差の履歴を足し合わせた量を保持しておけば，定常偏差があるとその値はどんどん大きくなります。
それに対して定数倍し，入力に加えるので，定常偏差を補正できるというわけです。
数式で見てみましょう。
P制御と合わせると
となります。新しく，偏差を積分する項が追加されました。
KiはI制御のゲインで，積分ゲイン，Iゲインなんかとも呼びます。
連続時間の積分は，ロボットの制御では使えないので，離散時間に落とし込みます。
区分求積法みたく，積分をシグマで表現し直すのです。
ちょっと雑ですが，添え字とかを気にしなければこんな感じです。
ここまで分かると実装したくなってきますね。それでは，controller.mを編集します。
persistent tr_sensor sum_e
Kp = 0.01;
Ki = 0.02;
r = 0;
if isempty(tr_sensor)
tr_sensor = -7:1:7;
end
if isempty(sum_e)
sum_e = zeros(2,1);
end
z = 0;
Nz = ones(1,15)*( value_light_sensor(:,1)<50 );
if Nz > 0
z = ( tr_sensor*( value_light_sensor(:,1) < 50 ) ) / Nz;
end
e = r-z;
u = [1;-1] .* ( Kp * e + Ki * sum_e ) + [0.3; 0.3];
sum_e = sum_e + e*delta_t;P制御のときと比較してみましょう。
persistent tr_sensor sum_e
Kp = 0.01;
Ki = 0.01;まず，永続変数sum_eと，積分ゲインKiが追加されています。
比例ゲインKpは先ほどの最初の例と同じ0.01，KiもとりあえずKpと同じにしてあります。
偏差の履歴の合計は，関数呼び出し毎にリセットされては困るので，永続変数でとってあります。
続いて，
if isempty(sum_e)
sum_e = 0;
endで永続変数sum_eの初期化を行います。動き始めの時点では偏差の履歴の合計は0でないと困ります。
このsum_eについて，初期化を忘れて事故る馬鹿を。私は何度かやっています。注意しましょう。
次に，最も重要な入力を決定する部分ですが
u = [1;-1] .* ( Kp * e + Ki * sum_e ) + [0.3; 0.3];P制御に加え．偏差の足し合わせを定数倍した項があるだけです。
最後に，
sum_e = sum_e + e*delta_t;にて積分（足し合わせ）を行います。偏差にdelta_tを掛けるのを忘れないようにしましょう。delta_tには，シミュレーション本体より，シミュレーション刻み時間が与えられています。
今回のシミュレーションでは，例え偏差にdelta_tを掛けなくてもKiを調整すれば結果的には問題ありません。
ですが，ものによってはシミュレーション間隔が一定でない場合があります。その際，偏差にdelta_tを掛けておかないと．大きな積分誤差が乗ることになります。注意しましょう。
さて，長らく修正お疲れさまでした。
それでは実行してみましょう。
どんな感じになりましたか？
積分項の威力がわかりますか…？
私の方で取得したグラフは次のようになりました。
どうです！
最初505位の値になりそうだったのが
段々修正されてきて
見事500近い値になりました！
ちょっと残ってしまっているのは，計測値が離散になっているためだと推測されますので，センサの間隔を密にする必要があります。
ほら，回路班が怖い顔してるので，これはしょうがないとして次にいきますよ。
もう少し分かりやすいように，比例制御のみの場合と比較します。
積分項の頑張りがよく分かります。
ですが，収束にちょっと時間がかかりすぎなので，Iゲインを調整しましょう。
慣例に従い，オーダーを変えてKi = 0.1とします。皆さんも実行してみましょう
振動的になり，ふっとんでしましましたね…。積分ゲインを上げすぎると，不安定になるようです。
こうなったらその間位の値を試してみたくなりますね。数通りやってみましょう
次に示すのは，Ki = 0.02とした場合です。
うーん…ちょっと振動的すぎますね。積分ゲインだけでなく，比例ゲインも少し大きすぎる気がします。
こんな感じで，ちゃんと収束するけど，振動的にしすぎないゲインの値を探すのは骨が折れます。
この作業，パラメータチューニングと呼びますが．PID制御を実装することそのものより大変な場合がほとんどかと思います。
さて，本当はこの1ページにPI，PD，PID全部押し込もうかと思ったのですが，書いている私が疲れたのでここまでにします。
次はPD制御，最終的にはPID制御まで持っていきます。
それではここまで，お疲れ様でした～