このWebカメラを使ってな、このWebカメラを使ってな、ビジュアルフィードバックしようと思うたのじゃ

# 1. 概要
通常、汎用OS上で動作するSimulinkはリアルタイム性が保証されていません。しかしSimulinkのSimulation Pacing機能やSimulink Desktop Real-Timeを用いることで、専用機器がなくとも、100 Hz程度ならばそこそこの(ソフト)リアルタイム性が得られる(可能性がある)そうです。低速とはいえ、実機実験の際にシリアルI/FのAD/DA変換機器さえ用意すれば、I/O以外の全機能をPC上のSimulinkのみに集中して実装できマイコンへの頻繁な書き込みが不要になったり、PC用のアクセサリが使用できたりするのは魅力的ですよね。

そこで今回はSimulink Desktop Real-Timeと手持ちのWebカメラを用いて簡易的なビジュアルフィードバック制御にトライした事例をレポートします。
![rect12071.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/18f5bfe8-53a0-011e-890d-b14b6fdd2980.png)

***用語たち***
- ***Simulation Pacing***: Simulinkモデルの実行時間を実時間と同期するため、サンプリング周期に合わせたウェイトを設けるSimulinkの実行方式です。kHzオーダーの同期は厳しいですが、100 Hzぐらいまでならそこそこの性能が出ます。
- ***Simulink Desktop Real-Time***: 専用I/Oデバイスとの連携により、Simulation Pacingより高精度なリアルタイム性を提供するアドオンです。シリアルポートなどの汎用I/Oを用いた場合はSimulation Pacingと同じ性能しか発揮できませんが、通信系のブロックが便利だったりデッドライン超過エラーが出せたりするので今回はこちらを使用しました。詳細は[MATLABブログ](https://blogs.mathworks.com/japan-community/2020/04/16/simulink-desktop-real-time/?from=jp)の記事が大変参考になります。
- ***ビジュアルフィードバック制御***: フィードバック制御系のセンサとして、カメラ画像を用いた制御のことです。

# 2. 結果
![s1.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/6f071286-f862-62c3-0430-197bc8e8ee58.gif)
こんな感じのアームの位置決め制御ができました。フィードバック制御周期は40 msです。

# 3. 実装
## 3.1 ハードウェア構成
実験系のハードウェア構成は以下の写真の通り、DCモータに直結されたアームの角度をWebカメラで撮影しPCに取得、PCから送信されたPWM Dutyに従いモータドライバを駆動してアーム角度を制御します。
<img width="300" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/d940d09a-9872-677e-1b20-d211e4c85a39.png">

### 3.1.1 Webカメラ
Logicool C270nを使用しています。最高フレームレートは30 fpsです。
<img width="240" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/b702e71f-f936-a613-2cbd-72639fef4a13.png">

### 3.1.2 アーム
拾ったDCモータ(型式不明)に養生テープを巻いたアームを自称する何かを接着した、人類史に残るクソプラントです。
<img width="300" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/9369061b-db62-16f8-9c56-7e26f00a36b2.png">

### 3.1.3 モータドライバ
Arduino Uno Rev3 + Motor Shield Rev3の組み合わせです。モータ駆動用電源はDC 5V/2.3 Aを使用しています。
<img width="240" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/d5469a4e-f85a-7e39-7b36-adb4ef8031ad.png">
### 3.1.4 PC
PCのスペックは以下の通りです。

- CPU: Intel(R) Core(TM) i7-8700
- RAM: 16 GB
- OS: Windows 10 Home

## 3.2 ソフトウェア構成
### 3.2.1 Arduinoスケッチ
モータドライバたるArduino Unoには以下のスケッチを書き込んでいます。ポーリングループ内の``parseInt``関数でSimulinkからシリアル通信を待機し、受信後Motor Shieldの回転方向とPWM Dutyの設定を行います。

``` c:Arduinoスケッチ
#define PIN_DIRECTION 12
#define PIN_PWM 3
#define PIN_BRAKE 9

void setup() {
  /* for higher pwm frequency */
  TCCR2B = TCCR2B & B11111000 | B00000001;
  Serial.begin(115200);

  pinMode(PIN_DIRECTION, OUTPUT);
  pinMode(PIN_BRAKE, OUTPUT);

  /* disengage brake */
  digitalWrite(PIN_BRAKE, LOW);
}

void loop() {
  long v_out;

  /* receive control input from simulink */
  v_out = Serial.parseInt();

  if (v_out >= 0) { /* forward direction */
    digitalWrite(PIN_DIRECTION, HIGH);
    analogWrite(PIN_PWM, v_out);
  }
  
  else { /* reverse direction */
    digitalWrite(PIN_DIRECTION, LOW);
    analogWrite(PIN_PWM, -v_out);
  }

  /* flush buffer */
  while (Serial.available() > 0) {
    Serial.read();
  }
}
```

### 3.2.2 Simulinkモデル
使用したSimulinkモデルの全体は下図の通りです。以下、各部分について解説します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/0c6c8cd5-cb4e-0032-afe8-0bbd4b023968.png)
#### Image Acquisition Toolbox/From Video Deviceブロック
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/1cc56042-e11b-aab0-1bdd-337b3c44267c.png)
Webカメラなどの映像デバイスとのI/Fを行うブロックです。
<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/149ad038-19a2-ae2c-47f5-c117c62eec38.png">
主要な設定項目は以下の通りです。

- ***Video Format***: 処理の高速化のため、フレームサイズを最小の160x120に変更しています。
- ***Block sample time***: フレームレートは25 fpsで設定しています。

<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/14db8639-52e1-3af8-b49d-774a21a0b764.png">
また、***Edit properties***ボタンからカメラの撮影設定を行います。撮影の高速化のため、***Backlight Compensation***、***Exposure Mode***、***White Balance Mode***はすべてManualに設定し、さらに***Exposure***をやや下げています。

#### アーム領域のバイナリマスク作成
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/fb38dc29-2b60-facf-0249-3740f0f97726.png)
カメラ画像からのアーム角度の取得に先立ち、画像中のアーム領域を抽出します。まずANDブロック以前の部分でアーム(というか養生テープ)の蛍光グリーンを抽出するため、HSV色空間に変換してから閾値処理を行っています。この閾値の決定のため、1枚画像を撮影し、「色のしきい値」アプリを使用しました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/d16d88e5-907d-1363-4ae2-e7f28f552a2a.png)
続いてノイズ除去とアーム領域を細くして角度推定精度を向上するため、Erodeブロックで収縮処理を実施しています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/fb653e01-a7ad-a68e-9915-75cb51e8d319.png)
パラメータは以下の通りです。
<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/50278689-1f2d-705f-3c8b-27379651f4cc.png">

#### Hough変換によるライン角度検出
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/50451378-2827-1461-228c-899114913388.png)
前処理されたバイナリマスクをもとに、Hough変換を用いてラインの角度を検出します。Hough変換のパラメータは以下の通りです。
<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/2bfb0c9f-3745-9a9c-57a4-a3a0e63d2921.png">
通常、このあとは出力``Hough``をHough Peaksブロックに入力し、検出されたライン角度``Theta``に対応する局所ピークのインデックスを取得します。しかし今回は唯一のラインを検出すればよいため、単にMaximumブロックで全体最大インデックスを取得しています。

画像処理全体の様子を以下の動画に示します。
![feawg.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/46208544-d83f-4e36-d0d2-169c8b655556.gif)

#### コントローラ
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/a514aadb-0301-0d79-c97a-fc8cb1bd4874.png)
コントローラはPD制御と不感帯補償のためのディザで構成し、目標値は0°、つまり画像上下軸として設定しました。ただしモータの配線の都合により、極性を反転させています。PD制御器のパラメータは以下の通りで、制御周期はカメラサンプリング周期を引き継ぎ40 msで駆動されます。
<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/f787860a-a3b6-1120-7955-a02a3f027a8c.png">
一方ディザのパラメータは以下の通りで、こちらはさらに短いサンプリング周期10 msで駆動しています。
<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/e0eb5746-a3f9-cecc-04dc-ff845face37e.png">

#### 出力リミット
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/15c9d8fe-7c29-411c-4e5a-20980d0f1e59.png)
Saturationブロックを用いて制御入力の大きさをArduino UnoのPWM範囲0~255(+符号)に制限します。
<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/5289480c-e8f3-3a82-8bab-5b2e6e40c641.png">

#### Simulink Desktop Real-Timeによる同期I/O
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/1bdf97ec-6c2c-9e8b-83f3-06e2a8afa661.png)

最後に決定された制御入力を、制御周期10 msごとにシリアルI/F経由でArduino Unoに送信します。設定は以下の通りです。
<img width="320" alt="image.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/807cfa63-a6ea-57a5-70b9-283f6f22f6be.png">
- ***Data acquisition board***: Arduino Unoを接続したシリアルポートを指定します。
- ***Sample time***: 10 msに設定します。
- ***Maximum missed ticks***: リアルタイム処理のデッドライン超過が指定数を超えたときにエラー終了させる設定です。長すぎるとリアルタイム処理の失敗に気づきませんが、短すぎるとパラメータ調整のためにブロックパラメータを開いたりシリアル通信の開始時のディレイなどで容易にエラー終了してしまいます。Show "Missed Ticks" portにチェックを行えばデッドライン超過数を出力できるため、Maximum missed ticksはやや大きめに取り、Missed Ticks portの出力を監視するのも手です。
- ***Format String***: ``printf``関数のフォーマット指定と同じ要領です。Arduino Unoの``parseInt``関数は"\r\n"を終端子とするためこれを付与しています。
- ***Initial string/Final string***: 初回及び終了時の送信内容です。両者とも入力0を指定しています。

# 4. 結果
## 4.1 初期値応答
冒頭のデモ映像(再掲)と、PD制御入力とアーム角の時系列を示します。
![s1.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/89ac1bca-700e-b95f-067d-4270c5d702ba.gif)
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/e151ff8a-3537-ffd0-4e89-874c2f5c2bd2.png)

## 4.2 インパルス応答
> 位置決め制御系を見たらしばけ、古事記にもそう書かれている。

ということで下図のインパルス応答試験装置を用いた応答試験を実施しました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/6ec9f224-5a32-0337-be72-318a1acd3c5a.png)
(クソエイムに伴う20回ほどのリテイクののち)以下のアームの挙動が得られました。
![imp3.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/7a948903-63dd-d644-a548-ebbab2ba44df.gif)
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/d11824a3-45ac-6f61-edb5-1db538c41648.png)

# 5. まとめ
ということで、Simulink Desktop Real-Timeを用いた簡易的なリアルタイム制御のデモでした。前述の通り、あまり高速には対応できず、同期を取ってなおOSの機嫌次第で数ティック落ちるなどの問題がありますが、低速用途であればなかなかに使いでがありそうです。

これ以上の高速化を求める場合、Simulink Desktop Real-Timeの専用のI/Oボードを用いるほか、今回のようにArduinoをI/Oボードとして機能させる構成だと[@motorcontrolman](https://qiita.com/motorcontrolman)さんの[記事](https://speakerdeck.com/motorcontrolman/matlab-home-prison-break)がとても参考になります。

# おまけ
## カメラ映像のリアルタイム性について
Webカメラの映像が所定の周期でサンプルされているかについては情報がなかったので、簡易的に検証を行いました。マイコンの多色LEDをサンプリング周期で赤青に点滅させ、各取得画像で点滅を検出できるかを調べます。以下が各フレームの画像ですが、少なくともこの速度帯では、正常に取得できているようです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/211913/e2e06fa6-2e09-36f6-4d2d-5830af7c62fd.png)

## その他の例
初めてこの機能を知った時にテンションが上がりすぎていろいろ制御していたので、そのログをば。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">Simulink Desktop Real-Time + Arduino DueでFRITできた！<br><br>tunedの初期値があってないのはコンデンサの初期チャージのせいだな。 <a href="https://t.co/yIIYw4SWX0">pic.twitter.com/yIIYw4SWX0</a></p>&mdash; 🦓幸福🦓 (@HppyCtrlEngnrng) <a href="https://twitter.com/HppyCtrlEngnrng/status/1434931045472763904?ref_src=twsrc%5Etfw">September 6, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">繰り返し制御 <a href="https://t.co/4utMxdJSeL">pic.twitter.com/4utMxdJSeL</a></p>&mdash; 🦓幸福🦓 (@HppyCtrlEngnrng) <a href="https://twitter.com/HppyCtrlEngnrng/status/1435252156215402500?ref_src=twsrc%5Etfw">September 7, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

<blockquote class="twitter-tweet"><p lang="pt" dir="ltr">PFC！PFC！ <a href="https://t.co/ow4vXSgzmp">pic.twitter.com/ow4vXSgzmp</a></p>&mdash; 🦓幸福🦓 (@HppyCtrlEngnrng) <a href="https://twitter.com/HppyCtrlEngnrng/status/1435636199868157957?ref_src=twsrc%5Etfw">September 8, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">予見制御+LQサーボ <a href="https://t.co/QKKPL77zgJ">pic.twitter.com/QKKPL77zgJ</a></p>&mdash; 🦓幸福🦓 (@HppyCtrlEngnrng) <a href="https://twitter.com/HppyCtrlEngnrng/status/1435984998457229316?ref_src=twsrc%5Etfw">September 9, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

プラントはすべて、以下で作成した電気回路です。
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">制御工学 Advent Calendar 2018 1日目の記事です(大嘘)<br><br>Arduino Nanoで作る小さな制御理論実験環境 <a href="https://t.co/o00ERDt4t5">https://t.co/o00ERDt4t5</a></p>&mdash; 🦓幸福🦓 (@HppyCtrlEngnrng) <a href="https://twitter.com/HppyCtrlEngnrng/status/1073782041894764544?ref_src=twsrc%5Etfw">December 15, 2018</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>