外乱オブザーバと繰り返し制御を併用した単軸アームの学習制御

## 制御性能の向上を目指す

https://qiita.com/Manao/items/b97ac54fad986556ee5b
High Power Gear Boxを用いたDCサーボの制御設計 - 外乱オブザーバ

上の記事の続きとなります。

前回では, 制御系の設計, 外乱オブザーバの設計を行い, 外乱が発生してもアームが安定化する制御を行いました。

しかし定常的には安定する制御ではあったものの, 過渡の部分でオーバーシュートが発生していました。

今回は制御性能を上げることを目的に, 繰り返し制御を用いて過渡応答を向上させる制御を実装したいと思います。今回の"学習"は繰り返し制御の特長である, 「一定時間前の試行を利用して制御を向上させること」, と定義させていただきます。

この記事は制御工学 Advent Calender 2020 の 4日目の記事 "繰り返し制御の紹介" と対になります。

では, よろしくお願いします。

## 制御ブロック
制御ブロックについて説明します。前回では外乱オブザーバと PD 制御器を併用した構成となっておりました。オレンジ色の部分がオブザーバとなっております。
![observer6.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/c88d2094-d987-951e-3032-d52ffa1162ac.jpeg)

今回は, 繰り返し制御を追加したため制御ブロックは以下のようになります。

![repeteobserve.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/ba53be09-e010-1cf4-6dc9-d20e73a7f27d.jpeg)


緑色で囲った部分が追加した繰り返し制御部分となります。 
この制御を加えることによって, 制御性能が向上すると望ましいのですが.. 

以下にSimulink モデルの図（デジタル制御）を示します。
![repetitive_simulink.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/97f0ae71-d5b4-5eac-4d72-5f97d3b45504.jpeg)

## 繰り返し制御の実装

自分で試行錯誤で構成した繰り返し制御系を作ってみました。おそらく修正繰り返し制御だと思います。
![repeteblock.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/43c03205-2ed4-e480-c904-98d8f61c75e4.jpeg)


今回はマイコンを使用したデジタル制御にて実装を行うので, 離散時間系列で考えるため, 
$$
e^{-sT} = z^{-N}
$$
といった変換を行います。$T$ は周期（今回は4秒）であり, $N$ はサンプリング数の周期なので, 今回は,
$$
N = T/T_{samp} = 4 / 0.02 = 200
$$
となります。

偏差 $e(t)$ の部分に一周期前の偏差 $e(t) z^{-N} $ を追加するような制御で補正しつつ, 前向き伝達関数の比例ゲイン $K_l$ (学習係数) で補正量を調整できると解釈しています。

システム同定にてパラメータを取得しておきます。

$ K = 196.5 $

$ T = 0.2232 $

以下に実装したプログラムを示します。

```matlab
% pos_pd_mbd.m
s = tf('s');
N = 200;

% Initialize & load data
close all
clear all
load sim_param
load model_data

% PD design by pole placement
omega_n = 12;
zeta    = 0.8;
p1 = (-zeta + j*sqrt(1-zeta^2))*omega_n;
p2 = (-zeta - j*sqrt(1-zeta^2))*omega_n;

% Set PD parameters
Kp = p1*p2*T/K;
Kd = -((p1+p2)*T + 1)/K;
Ki = 0;

% Set learning gain
Kl = 0.15;

% Display PI parameters
disp('>>> PI parameters <<<')
fprintf('Kp  = %f\n',Kp);
fprintf('Ki  = %f\n',Ki);
fprintf('Kd  = %f\n',Kd);

% observer setting
omega_c = 7.5;        % cutoff frequency
Fs = omega_c^2 / (s^2 + sqrt(2)*omega_c*s+omega_c^2);
Fsd = c2d(Fs,ts,'zoh');
[numfsd,denfsd] = tfdata(Fsd,'v');  % discrete filter
invP = s*(T*s + 1)/K;   % inverse model
Est = Fs * invP         % estimation
Estd = c2d(Est,ts,'zoh');
[numestd,denestd]  =tfdata(Estd,'v'); % discrete model

% Experiment
r      = 60;
r_cyc  = 4;
dist   = 2; % 0 or 2 for dist test
% dist = 0;
Ncyc   = 8;
tfinal = r_cyc*Ncyc;
open_system('pos_pid_mbd_sl_observer_learning')
open_system('pos_pid_mbd_sl_observer_learning/Scope')
sim('pos_pid_mbd_sl_observer_learning')

% EOF of pos_pd_mbd.m
```

## 実験結果

まず前回の繰り返し制御を入れていない, PD制御と外乱オブザーバの波形を示します。
![observertest.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/223a26b0-a940-d745-3f89-9a423dfc595d.jpeg)
最近動かしてなかったとはいえ少し汚い波形でちょっと笑ってしまいました。（ギアもガタガタでグリスを塗りなおした記憶があります）

繰り返し制御を入れた時の波形を示します。
![learning.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/8be690a3-2035-a4ab-cc07-9f1d98ae9292.jpeg)
過渡応答の部分を見ると, 繰り返し制御なしの場合は大きなオーバーシュートが発生していることが確認できますが, 繰り返し制御ありの場合は外乱が抑制されオーバーシュートがなくなり, 応答が向上していることがお分かりかと思います。

比較用に, 外乱オブザーバも繰り返し制御もない場合の波形も示します。
![nonobserver.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/555e03d1-79b5-5bef-c73c-3a0844afb269.jpeg)
この場合は定常的な外乱の影響で指令値に追従できていない状況となります。
再び, 繰り返し制御つきの応答を見てみましょう。
![learning.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/8be690a3-2035-a4ab-cc07-9f1d98ae9292.jpeg)
定常外乱が発生してもオーバーシュートなく指令値に追従している波形であることが分かります。

この波形は8試行回した時の波形なので分かりにくいのですが, 応答自体が漸近的に性能向上していく動きになります。(ループをかなり回してみたら結構面白かったです) ループの回数を増やせば増やすほど性能は向上しますが, ゲインや試行回数によっては発振していきます。
時間の都合上, 今回は安定性の考察など設計法を説明することができませんが, 必要ではあると感じています。（次回あればやらせていただきます）

ちなみに僕は学習しすぎると頭が痛くなっちゃうのですが, システムの脳みそであるコントローラもそうなっちゃうのかなと解釈しています。

## 補足 
以下に応答波形のみを載せます。少し時間をおいて取り直してみました。

w/o 外乱オブザーバ, w/o 繰り返し制御
![dist12.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/e487b69a-f1d9-0c59-be80-eae34659d9b9.jpeg)
w 外乱オブザーバ, w/o 繰り返し制御
![dist22.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/8b0ae128-70f9-fc98-c77e-89372da974ef.jpeg)
w 外乱オブザーバ, w 繰り返し制御
![distbest2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/493799/7b027a03-9f54-7eec-953c-f76cf736c2b9.jpeg)

最終的に1次遅れ系の応答のように落ち着きました。
## 補足2
動画を撮ってみたのでその様子を載せておきます。

モーション制御 (繰り返し制御)
https://www.youtube.com/watch?v=1SiAKOex_-w&feature=youtu.be

以前外乱オブザーバのみの制御で撮影したものも載せておきます。

モーション制御（外乱オブザーバ）
https://www.youtube.com/watch?v=6yTACA7ZFrU&feature=youtu.be

過渡応答の動きのオーバーシュートが抑えられているかと思います。

## 感想
”学習”という言葉に関しては少し議論が必要かなと思っていますが, 
AIのようなものに時間を使いすぎるのもなぁとか,
 僕という一個人がそういうワードを使いたくてなんとなく組み込んでみたかっただけなので, あんまり突っかかってこないでいただけると幸いです。(いろいろ言われたら修正する必要はあるとは思いますけど)

<b>そもそもフィードバック制御そのものに上がりすぎたら下げる, 下がりすぎたら上げるといった簡単な学習的な機構があるよね, といった気持ちはあります。</b>

機械学習とか深層学習とか（僕はやったことないんですけど）は, 制御工学の背景知識も含めることによって高度な自動化が達成されるのではないかなと。
大電力を扱っていたり, 人が載ってるわけではないので, 今回の条件なら学習はこんなかんじでいいだろうと思ってます。あいまいさを許容するという意味合いで。



短いですが, 今回はここまでです。今回はあまり準備する時間がなく, 複雑なこともしたくなかったのですが, この制御入れてみたらどうなるだろうかなと思ったらすぐに試せたうえに, 自分の推察を結構簡単に試行錯誤できたりするので, Matlab/Simulink はいいツールじゃないのかなと思っています。

繰り返し制御について, 検索しただけで基礎理論や応用に関する論文がヒットするくらい扱いの多いアルゴリズムのようです。個人的に制御工学 Adc 4日目のほうにも少し書いてみましたので, ご興味がありましたらご覧ください。

<b>繰り返し制御の紹介</b>
https://qiita.com/Manao/items/0bdf01a3fc309a13d721

以上, Matlab/Simulink Adc 4日目でした。