Simulinkで始める線形システムのモデル規範適応制御(MRAC)

## はじめに
本記事で対象にしている読者は下記のような悩みを持った制御設計者の方(というか最近の筆者)です。

* PID制御や状態フィードバック制御の特性をある程度理解して，感覚的であれ，安定余裕や極配置などの定量指標に基づいてであれ，制御器のパラメータチューニングができる。
* ところが，制御対象の特性が(制御動作に比べて緩やかながらも)時間変化することが分かり，想定されるどんな状況でも要求仕様を満たせるロバストなパラメータを置いたり，状態に応じてパラメータを切り替えたり(ゲインスケジューリング)する必要に迫られている。
* しかし，ロバストなパラメータが極端に応答の遅い保守的なものになってしまったり，パラメータ切り替えのために何通りものパラメータ設定と切り替え条件の検討に工数を割かなければいけなかったり，パラメータ切り替え条件でチャタリング起こすんじゃないかと心配したり，で途方に暮れている。

そういった方向けに，今回は代表的な適応制御手法とSimulinkでの実装例を紹介します。

本記事では，一般的なテキストのように理論の積み上げから実装方法を説明するのではなく，ひとまず動かせる適応制御のSimulinkモデルとその動作例を示すことにウェイトを置いています。

また，本記事では下記ツールを使用しております。

* Matlab 2021b
* Simulink
* Control System Toolbox

なお，世の中にある様々な制御手法の中での適応制御の位置付けに関しては，こちらの記事が分かりやすかったです。
[適応制御の基礎](
https://qiita.com/MENDY/items/1ca1409d365b66d7ea6d)

## 目標
先に述べたようなロバストパラメータ探しやゲインスケジューリングをしなくてもいいように，基本的な適応制御手法の1つであるモデル規範適応制御(MRAC)をとりあえず動かせるところを目指します。制御対象は1入力1出力の線形システムとします。また，安定性にも気を使ったパラメータ設計も行えるようにします。

ただし，ベースとなる制御はフィードフォワード制御＋状態フィードバック制御 ($u(t) = \theta_r(t) r(t) + \boldsymbol{\theta_x}(t)^\top \boldsymbol{x}(t)$) に限定します。状態$\boldsymbol{x}(t)$が$[x_1(t), \dot{x}_1(t)]^\top$のように定義されていればPD制御のような制御になります。
「定常偏差を消したいからI制御も含めて適応制御を使いたい！」という気持ちもあるかと思いますが，今回I制御に相当する仕組みは割愛します。

## SimulinkでMRAC

線形システムをモデル規範適応制御(MRAC)で制御するモデルをSimulink上で構成しました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/f6a8c6fd-82b5-6aad-f91a-53030d6801f6.png)

大きく，制御対象(Linear Plant)とその状態方程式の係数を変動させる(Plant Varying)ブロック，そして適応制御ブロック(Adaptive Controller)に分かれています。
簡単のために制御器が状態を直接取得していますが，現実的にはセンサやオブザーバが挟まっていると思ってください。

### 制御対象

今回は状態方程式

```math
\begin{align}
\dot{\boldsymbol{x}}(t) &= A \boldsymbol{x}(t) + \boldsymbol{b} u(t) \\
y(t) &= \boldsymbol{c}^\top \boldsymbol{x}(t)
\end{align}
```
で表される線形システムを対象にします。

なお，適応制御器構成のためにこれらの係数の値が既知である必要はありませんが，
$b_0 = \boldsymbol{c}^\top A^{(n^*-1)}\boldsymbol{b}$ ($n^{\*}$は相対次数) の符号 $\mathrm{sign}(b_0)$ だけは知っている必要があります。

具体例として下記のような係数と初期状態を持った制御対象を扱うことにしましょう。

```matlab
A = [0, 1; -2, 3];
b = [0; 1];
c = [1; 0];
initialX = [0; 0];
```
```matlab
eig(A) %実部が正の固有値があれば不安定

ans =

    0.5616
   -3.5616
```
2階の線形微分方程式で表せる制御対象であり，自由応答は不安定です。

上記と同じ構造で表せる制御対象としては，シングルアームロボット(https://okasho-engineer.com/one-link-arm-robot-equation/) を倒立状態にして，$\sin \theta \approx \theta$ と線形化したものが挙げられます。この場合，2行1列の2は重力による係数，2行2列の-3は粘性抵抗による係数に相当します。

これらの係数行列は後ほど時間変化させてみます。

今回の制御対象を伝達関数表記すると，分子分母係数は次のようになります。

```matlab
[num, den] = ss2tf(A,b,c',0)

num =

     0     0     1


den =

     1     3    -2
```
分母次数2，分子次数1であり，システムの相対次数は$n^* = 2$と分かります。


### 適応制御器

適応制御器の中身は次のようになっています。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/daea24a8-3e75-f0a2-3f6b-f5d1fbc4a04e.png)


#### 基本構造
大きく，規範モデル，ベースとなる制御部，制御パラメータ更新部に分かれています。

規範モデルは制御目標値$r(t)$に対する理想的な応答$y_M(t)$を作るモデルです。この理想的な応答と実際の応答をぴったり一致させるのが"モデル規範"適応制御の目的です。参照モデル自体は伝達関数

```math
\frac{1}{W(s)}=\frac{1}{w_0 s^{n^*} + w_1 s^{n^*-1}+ \cdots + w_{n^*-1} s + 1}
```
やその状態方程式表現で記述します。
(参考文献『[適応制御](https://www.coronasha.co.jp/np/isbn/9784339033106/)』では最高次の$s^{n^*}$の係数を1に正規化していましたが，規範モデルのDCゲインは1である($y_M(\infty)= r(\infty)$)べきと考え，定数項の係数を1に正規化しました。)

ベースとなる制御はシンプルであり，フィードフォワード制御と状態フィードバック制御の組み合わせ，
``` math
u(t) = \theta_r(t) r(t) + \boldsymbol{\theta_x}(t)^\top \boldsymbol{x}(t)
```
です。Simulinkモデル内では制御パラメータを$\Theta(t) = [\theta_r(t), \boldsymbol{\theta_x}(t)^\top]^\top$，目標値と状態を$\boldsymbol{\omega}(t)=[r(t), \boldsymbol{x}(t)^\top]^\top$とまとめています。

そして，その制御パラメータ$\Theta$を適応的に調整するのがパラメータ更新部であり，まさに"適応"制御のコアとなる部分です。
パラメータの更新は規範モデルの理想的な応答$y_M(t)$と実際の応答$y(t)$の誤差$e(t) = y_M(t)-y(t)$を0に近づけるように行われます。


#### 規範モデル設計

実際に規範モデルを作ってみます。制御仕様や要求スペックに基づいて思い思いの規範モデルを設計しましょう。

制御対象の相対次数が2次なので，今回は次のような二次遅れ系を規範モデルとします。
$$\frac{1}{W(s)} = \frac{1}{\frac{s^2}{\omega_n^2} + \frac{2\delta}{\omega_n}s + 1}$$

$\delta$は減衰率であり，ステップ応答において，1未満だとオーバーシュート，1以上だと強制減衰します。
![delta.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/fff69827-d198-1436-c1dd-365334e3ed89.png)

$\omega_n$は固有角周波数であり，ステップ応答において，大きいほど立ち上がりが早くなります。
![omega_n.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/7c37e099-35d4-08da-6450-82e379731ea2.png)

今回は$\delta = 0.8$, $\omega_{n} = 7.5$ としておきます。
少しだけオーバーシュートして，±2%正定時間が0.5sとなるような規範モデルになります。
```matlab
% 規範モデル
deltaM = 0.8; % ちょっとだけオーバーシュート
omegaNM = 7.5; % ±2%正定時間が0.5s程度
WsNum = [1/omegaNM^2, 2*deltaM/omegaNM, 1]; % それぞれ s^2, s, 1の係数
figure
step(tf(1,WsNum)) % 規範モデル 1/W(s)のステップ応答
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/6a723f84-38b2-c9e3-0772-5f597ef50572.png)

後程，モデル化誤差がない場合の最適制御パラメータと，適応則のハイパーパラメータを決めるために，規範モデルの状態方程式表現を用います。

各種係数行列をtf2ssでパパっと求めたいところですが，状態$\boldsymbol{x}(t) = [x_1(t), x_2(t)]^\top$としたとき，$x_2(t) = \dot{x}_1(t)$となるように，また，$\boldsymbol{c}=[1, 0]^\top$とするため，手動で求めます。後述の誤差ベクトルを$\boldsymbol{e}(t) = [e(t), \dot{e}(t)]^\top$と置くことに対応させるためです。


```matlab
% 規範モデルの状態空間表現
% ただしx2 = dx1/dt とする
AM = [0, 1; -WsNum(3)/WsNum(1), -WsNum(2)/WsNum(1)];
bM = [0; 1/WsNum(1)];
cM = [1; 0];
```

#### 適応則設計

適応制御のコアの部分，パラメータの更新則は次のようにします。
$$\dot{\Theta} = \mathrm{sign}(b_0)\Lambda \boldsymbol{\omega}(t) \boldsymbol{h}^\top \boldsymbol{e}(t)$$

各ハイパーパラメータを適切に設定すれば制御誤差$e(t)$を0に収束させることが理論的に保障されている方法です。

$b_0$は$b_0 = \boldsymbol{c}^\top A^{(n^*-1)}\boldsymbol{b}$で表される値です ($n^{\*}$は相対次数)。$A, \boldsymbol{b}, \boldsymbol{c}$が未知な場合計算できませんが，$b_0$の符号さえ分かればよいので大雑把な値が分かっていればよいと思います。$b_0$絶対値自体は次の更新係数$\Lambda$に吸収させることができるので気にする必要はないです。

更新係数$\Lambda$ は正定対称行列です。今回は$\boldsymbol{\omega}(t)$が3次なので，3×3行列です。機械学習の学習率に相当するものなので，値が大きいほどパラメータ更新速度が速くなります。実はいくら大きくしてもMRACの安定論的には問題ありません。（おそらく制御器を離散時間化した場合に更新幅が大き過ぎてパラメータが暴れるという問題が起きると思います。）

$\boldsymbol{h}$は誤差ベクトル$\boldsymbol{e}(t) = [e(t), \dot{e}(t)]^\top$に乗じてスカラーにする係数です。今回は2×1ベクトルです。パラメータ更新を含めた制御を安定化させるために，$\boldsymbol{h}^\top (sI-A_M)^{-1}\boldsymbol{b}_M$が強正実になるように決めます。
「強正実」についてはこちらで定義と必要十分条件をご確認ください。
[鈴木隆. "正実性 (III) 正実性と適応制御." 計測と制御 34.11 (1995): 897-904.](https://www.jstage.jst.go.jp/article/sicejl1962/34/11/34_11_897/_article/-char/ja/)

必要十分条件に従えば，$\boldsymbol{h}=[h_1, h_2]^\top$として，$10h_1 > h_2, h_2 > 0$ を満たせばよいことが分かります。



適応則に関する各種パラメータは次のように設定しました。

```matlab
% 適応パラメータ
Lambda = 10*eye(n+1);
h = [1; 1]; % h'*(sI-A_M)^-1*b_Mが強正実となるように選ぶ
```

## 制御結果

ステップ入力に対する応答を見て，MRACの効果を確認します。
具体的には，目標値$r(t)$を2秒おきに1→0→1→0...とステップ上に変化させたときの応答を確認します。

以下の5つのシチュエーションについて動作例を見てみましょう。
* 制御対象モデル化誤差なし
* 制御対象モデル化誤差あり
    * 適応制御なし
    * MRAC適用
* 制御対象特性変動あり
    * 適応制御なし
    * MRAC適用

### 制御対象モデル化誤差なし

まずは制御対象の変動がない場合について，適応制御を使わず，しかしパラメータを適切に設定した場合について動作を確認します。
制御応答が規範応答に一致する，すなわち$y_M(t)=y(t)$を実現する制御則は，
制御対象モデル$(A, \boldsymbol{b}, \boldsymbol{c})$と規範モデル$1/W(s)$から逆算することができます。

逆算の結果，その制御則は

``` math
u(t) = \theta_r r(t) + \boldsymbol{\theta_x}^{\top} \boldsymbol{x} = \frac{1}{w_0 b_0} r(t) -\frac{\boldsymbol{c}^\top W(A)}{w_0 b_0}\boldsymbol{x}(t)
```
と導出できます。

もちろん，設計時に制御対象モデルに誤差がある場合，すなわち$\boldsymbol{c}$や$A$や$b_0$が真の制御対象と異なる場合には，上記の制御則も誤差を持ったものとなるので，$y_M(t)=y(t)$を実現できません。

何はともあれ，まずは誤差がない前提で制御パラメータ(の初期値)を設定してみましょう。

``` matlab
% 想定制御対象＝真の制御対象
AHat = A;
bHat = b;
cHat = c;
nRelativeHat = nRelative;

% 想定制御対象と規範モデルに基づき制御パラメータを決める
% 想定制御対象＝真の制御対象なら，実応答＝規範応答となる
b0 = cHat'*AHat^(nRelateveHat-1)*bHat;
thetaR = 1/b0/WsNum(1);
thetaX = -cHat'*(WsNum(1)*AHat^2 + WsNum(2)*AHat^1 + WsNum(3)*AHat^0)/b0/WsNum(1);
initialTheta = [thetaR; thetaX'];
```

``` matlab
initialTheta =

   56.2500
  -58.2500
   -9.0000
```

このような制御パラメータになりました。

それでは動かしてみましょう。
![nomodelerror.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/92c16c7e-9fb2-9be1-acfd-9a2843394556.gif)
規範応答の青線が隠れるほど制御応答(赤線)がぴったり一致していることが分かります。

### 制御対象モデル化誤差あり

制御対象モデルの想定パラメータを次のように設定し，設計時に想定していた制御対象と現在の制御対象のふるまいがかけ離れてしまった場合について動作を見てみましょう。
``` matlab
A = [0, 1; 2, -3];
AHat = A .* [1, 1; 0.25, 5];
```
「いつの間にか2行1列目のパラメータが1/0.25=4倍，2行2列目のパラメータが1/5=0.2倍になってしまいました！」という，もはや別物の対象を制御し続けているイメージです。

MRACが無い場合の応答は次のようになります。
![modelerror.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/7d5efe62-9910-99ca-8256-d3a78c05a55c.gif)
規範応答に追従する様子もなく，不安定な応答になってしまいました。
正確なモデル同定＆チューニングが重要であることが分かります。

しかしMRACを適用した場合にはそのような心配は無用です。
応答は次のようになります。
![modelerror_mrac.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/b6212f08-9dcf-4dc8-ba51-0ad627841423.gif)
2，3回の動作で規範モデルの応答にピッタリ沿う応答を実現できていることが分かります。
また，1回目の動作においても，MRAC未適用時のように応答が発散する様子もありません。

**強化学習などの学習制御においては，数百～数万回の試行を通してやっとまともな動きをするということも珍しくないですが，
それと比べてMRACの適応時間のスケールは，非常に短いものだと思いました。**
(解いている制御課題の複雑さが違うので単純な比較はできませんが。)

### 制御対象特性変動あり

想定制御対象モデルと実際の制御対象のギャップは，設計時のモデル同定誤差だけではなく，
稼働時の特性変動によっても発生することがあります。

制御対象システムの係数$A$に以下のA_diffを加算することで，制御対象の特性を緩やかに変動させてみます。

```matlab
function A_diff = gen_diff(t)
A_diff = [0, 0; 6*sin(2*pi*t/400), 7*sin(2*pi*t/500)];
```

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/3451853d-d074-1906-e3fd-34d99f14e33a.png)

$A$の要素は2000秒間 (r=1,0に追従する制御500回分) かけて次のように変化します。

![param_varying.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/2b754ec7-d993-90df-2264-aeebaa28fdd6.png)

パラメータは符号が反転する程度に変動しています。

なお，想定制御対象モデルは，真の制御対象モデルの初期値に合わせています。

MRACを導入していない場合についての応答を見てみましょう。
![modelvarying.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/e42f835b-e88b-b6af-c0b7-ebb6b073758f.gif)
発散しないとはいえ，応答速度，定常誤差，オーバーシュート量，何もかもが制御対象の変動によって変わってしまいます。
ある時点で適切なパラメータを設定できたとしても，制御対象が変わればとても規範モデルに追従できる様子はありません。

次にMRACを導入した場合の応答を見てみましょう。
![modelvarying_mrac.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2339483/da5bd893-c266-a684-a686-f0c934c07f77.gif)
予想通り，制御対象の変動に対して頑健な制御ができていることが分かります。


制御対象の特性が想定とずれていようが，変動しようが，適応制御(MRAC)があれば規範モデルに近い応答を実現できます。
つまりもうしんどいゲインスケジューリングをしなくても済むんだ！
めでたしめでたし。

## おまけ
適応制御，2021年10月リリースのMatlab2021bからSimulink Control DesignにModel Reference Adaptive Control(MRAC)ブロックが追加されたようで，MRACがお手軽にできるようになりました。今回紹介したFF+FB方式に加え，外乱抑制項も加えられますのでより実用的な制御が可能になりそうです。
適用例を置いておきますのでよかったら触ってみてください。

* Model Reference Adaptive Control of Aircraft Undergoing Wing Rock<br>(https://jp.mathworks.com/help/slcontrol/ug/model-reference-adaptive-control-of-aircraft-undergoing-wing-rock.html)
* Model Reference Adaptive Control of Satellite Spin<br>(https://jp.mathworks.com/help/slcontrol/ug/mode-rReference-adaptive-control-of-satellite-spin.html)

## 参考文献
* 鈴木隆. "正実性 (III) 正実性と適応制御." 計測と制御 34.11 (1995): 897-904.
* 宮里 義彦. "適応制御." コロナ社 (2018).