移動ロボットにおけるブラシ付きDCモータのPID制御

この記事は「[マイクロマウス Advent Calendar 2022](https://adventar.org/calendars/8292)」ならびに、「[制御工学 Advent Calendar 2022](https://qiita.com/advent-calendar/2022/system_control)」の12日目です。
[マイクロマウス Advent Calendar 2022](https://adventar.org/calendars/8292)11日目の記事は、アライさんのマイクロマウス合宿の記事でした。マイクロマウス合宿は良い学びの場になる一方、開催のノウハウが残っていない・運営の負担が大きかったので、運営としてやるべきタスクが整理されているのは非常に大事なことだと思います。

# はじめに
この記事では、ブラシ付きDCモータ(以下DCモータ)を用いたタイヤ駆動のロボット(例：マイクロマウス、ライントレーサー)について取り扱います。
（角度制御するロボットアームなどにも応用できるはずですが、風呂敷が広がりすぎるので特に触れません。）

また、ロボットのパラメータは何度も使うので予め定義しておきます。

|項目|記号|単位|備考|
|:---:|:---:|:---:|:---:|
| 車速 |$V$| m/s | |
|ギヤ比|$G_R$|-| モータからタイヤへの減速比とする|
|タイヤ径|$R_W$|m| 直径|
|バッテリ電圧|$V_B$|$V$| |
|モータ電圧|$V_M$|$V$| モータ両端に掛かる平均的な電圧。PWMにより可変 |
|モータ電流|$I_M$|$A$| |
|モータ回転数|$N$|rpm| $N = \frac{60V}{\pi R_W} G_R$ |
|モータ回転定数|$k_n$|rpm/V|無負荷回転数/定格電圧|
|逆起電力定数|$k_E$|V/rpm|$k_E = \frac{1}{k_n}$|
|トルク定数|$k_M$|Nm/A||
|モータインダクタンス |$L_M$|$H$||
|モータ端子間抵抗 |$R_M$|$\Omega$||


# ハードウェアの設計
[こちら](https://mmk.rulez.jp/?page_id=5)の7項目目にあるように、良いロボットは良いハードの設計があってこそです。

## メカ
良いメカ設計の詳細を解説することは差し控えさせていただきますが、ポイントは以下の通りでしょう。
 - ギヤやタイヤの精度は十分か？
     - 精度が悪い場合、非線形が強くなり制御しにくくなる（静止摩擦係数の増大。回転中の摩擦変化。）
 - ギヤのバックラッシは適当か？直径と歯数の比(モジュール)は可能な限り小さくなっているか？
     - 非線形性(ヒステリシス)を持つので制御性が悪化する
 - エンコーダの精度(分解能)は十分か
     - モータ側に取り付けることで精度はギヤ比倍される

## 回路
だい～ぶ長くなったので[別記事にしました](https://qiita.com/tanutanup/items/345b59c319d511ae0a3d)。

# フィードバック(F/B)制御
今回対象にする速度のPID制御システムを以下に示します。水色の箇所はマイコン内のプログラムで実装されます。緑の部分はマイコンのハード機能と周辺回路。オレンジは電気的・メカ的要素です。
（わかりやすさのために色々省いてます。また、積分や微分はデジタルっぽい感じに表現し直してます。）
![PIDベーシック.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/22326f2f-bffa-50b5-3c52-7fab57ab5828.png)

## デジタル制御とアナログ制御
PID制御は本来線形・時不変システムを前提としています（言い換えればすべて一つの微分方程式で記述可能なシステムです）。
一方、マイコン上に実装したプログラムでPID制御する場合、PWMや計算時間・エンコーダなど、多くの非線形・時変システムな要素が含まれます。
なので、マイコンで組んだPID制御システムでは、これらの非線形・時変システムが線形・時不変システムとみなせる工夫が必要です。

- 制御周期(=無駄時間)は無視できるぐらいに短いか？
    - 最低限時定数の10分の1ぐらい（ゲイン選定にもよるので感覚にはなりますが）
    - 通常使う最低モータ回転数で出力されるエンコーダパルス周期は制御周期より短く
（目標速度が低い場合はフィードバックをやめてブレーキしてしまうなどの対策）
    - せっかく制御周期が速くてもPWM周期が長いと意味がない（[別記事も参照](https://qiita.com/tanutanup/items/345b59c319d511ae0a3d)）
- 制御周期はいつでも一定か？
    - タイマ割り込みを使う
    - 割込み禁止や割り込み中の処理負荷で割り込み時間が大きく変わっていないか？
- PWMのDutyやエンコーダで検出した速度、マイコンの変数は十分な分解能(=線形とみなせる)があるか？
    - 最低9~10bitぐらいはほしい。低分解能のF/Bは現代デジタル制御の領域
- フィードバックの有効/無効切り替え時や想定外の誤差などで、通常のF/B中より大きく誤差が発生した場合の対策はあるか？
    - 微分値・積分値のリミット
    - 制御のON/OFFを切り替えた時、積分・微分値リセット
- その他、線形とみなせる設計か？
    - ドライブ回路、メカなど”変な癖”がないか？


## カスケード制御
先程の制御ブロックでは**モータが電圧をいきなりトルクに変換してくれる**前提ですが、実際は電気的な遅れの影響を受けます。このように多数の微積分ブロックが重なった制御系を、1組のPIDゲインで安定させるのは実は難しいです。
この場合、**カスケード制御**を用いることで制御性が向上します。カスケード制御とは、１段目のフィードバック制御(メジャーループ）の出力を2・３段目のフィードバック制御(マイナーループ)の目標値として用いる制御です。

今回の例では、速度制御がメジャーループ。モータの電流制御がマイナーループに該当します。
![PIDカスケード制御.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/16a74c42-eb18-f436-30ba-c81556b88ed3.png)
（ちなみにマイナループ応答や誤差の影響度合いでP制御以外を省くことが可能です）

カスケード制御を用いるメリットとしては、以下の点が挙げられます。
- モータの発生するトルクが安定し、速度制御の誤差が減少する。
    - 理由①：速度が増加すると逆起電力も増加し、同じDutyでもトルクが変わる(≒最適なPIDゲインが速度で変動する)。電流をF/Bしていればこれを防止できる。
    - 理由②：電源電圧などの変動で電流が変動しトルクが変わることを防止できる。
- モータの発生するトルクの応答性が向上し、速度制御の応答性が改善する。
    - 理由①：モータ電流が不足している瞬間のみDutyを増やすことができ、モータ電流の応答がステップ状に近づくため
（特にモータの定格電圧より高い電源電圧を用意した場合、モータへの過電圧を防止しつつ応答を高められる）
    - 理由②：速度制御の誤差要因が減るので、メジャーループのゲインを ”_攻める_” ことができる。

ただ、制御ブロックを見てわかる通りなかなかに複雑な制御になります。モータの電流検出回路が必要ですし、モータの電流応答はメカに比べて遥かに速いのでマイナーループは制御周期の高速化が必要です（モータによっては数十μｓ程度）。
（ICを使うとか、ディスクリートで組むとか、[いっその事アナログパワーアンプにしてしまう](https://qiita.com/tanutanup/items/8cad22d386e8295a3e84#%E5%AE%9A%E9%9B%BB%E6%B5%81%E3%83%A2%E3%83%BC%E3%82%BF%E3%83%89%E3%83%A9%E3%82%A4%E3%83%90)などなど、ハードにまかせてしまえば制御周期の問題は解決ですが。。。）

# フィードフォワード(F/F)制御
フィードフォワード制御は、結果が誤差へ反映されるのを待たないので応答性が勝ります。
しかし、フィードフォワード制御単体は誤差が出ても対処出来ない欠点があるので、フィードバック制御に割り込ませることで、応答性の向上と誤差の低減を両立可能です。

## 電圧の正規化
電源電圧が変動すると同じDutyでも出力電圧が変わってしまいます。フィードバック制御はこの変動が速度変化に現れてから対処となり遅れが生じますし、適正ゲインを外れる原因になります。そこで、基準となる電源電圧を決め、電圧が高い場合はDutyを下げるようにフィードフォワード制御する方法が電圧の正規化です。
![PID_電圧補正.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/d017bc82-f19b-35eb-21eb-a9829003a7fe.png)
補正係数$K_{FFB}$は以下の用に計算されます。
$V_{BN}$は基準となるバッテリ電圧です。Dutyは1以上にはならないので、通常のバッテリ電圧より低い値である必要があります。また、あまりに低い基準電圧を選ぶと、せっかくバッテリ電圧が高くても性能が絞られてしまいます。
```math
K_{FFB} = \frac{V_B}{V_{BN}}
```
なお、当然のことながら電流F/B制御を行っている場合、このフィードフォワード制御は不要です。


## 速度のF/F
制御の目標値は速度でありモータの回転数に比例します。そしてモータの回転数は(外乱を抜きにすれば)Duty比(≒モータ電圧)に比例します。
![PID_速度FF.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/70351a11-19ec-f0a5-f1e9-53c1a8c1afae.png)
速度F/Fのゲインは、Duty100%で到達しうる最高速度から計算したり、Dutyとタイヤの実回転数の関係から実験的に求められます（タイヤに反射板を貼り、光学式回転計などで求めるといいでしょう）。

```math
K_{FFV} = \frac{V_{target}}{\frac{V_{BN}k_n}{60G_R}\pi R_W}
```
なお、係数に含まれるバッテリ電圧を実際に計測した電圧値に置き換えることも可能ですが、電圧を正規化している場合は不要です。

## 電流のF/F
先に述べたカスケード制御では、電流F/Bの実装が大変であることを説明いたしました。実際に電流をF/Bする代わりに逆起電力を推定して差し引くことである程度簡略化が可能です。
(もちろん、インダクタンスやバッテリ電圧変動の影響は取り除けません）

```math
I_{M} \fallingdotseq \frac{V_M-Nk_n}{R_M} \\
I_{M} \fallingdotseq \frac{(V_B * Duty)-Nk_n}{R_M} \\ 
Duty \fallingdotseq \frac{I_{MTGT}R_M + Nk_N}{V_B}
```

![PIDカスケード制御_電流FF.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/3bf194ec-6f81-e888-b7b9-2212532b6b02.png)

## F/F制御の注意点
F/F制御はF/B制御側から見ると”__外乱__”に見えてしまいます。誤差が減る側に作用していれば、F/B制御のしごとが減るので問題ないですが、逆だと困ったことになります。
チューニング・動作確認の際は、P・I・D制御・各種F/FをDutyに反映する箇所(図では足し算のところ)をON/OFF切り替えできるようにしたうえで、以下の値はモニタしておくと良いでしょう。
- 目標値と実際の値(あたりまえ)
- P制御、I制御、D制御単体の出力値(誤差とゲインを掛けたもの)
    - F/F制御を有効にした場合はF/B制御の出力値は小さくなるはず。
（F/F制御が妨害してしまっていることを確認できる。）
- 各F/F制御それぞれ単体の出力値
    - 計算結果が妥当か確認
    - 誤差等で過不足があればチューニング
- Duty
    - Dutyが飽和(100%)になっていないか？

# 終わりに
PID制御の理論は数多く書籍や論文が出ているとは思いますが、マイコンによる実装に特化した話は中々まとまっていない様に思えます。
より詳しくは参考の章に掲載のHPに書いてあるので参考にしてください。
（特に私はゲインチューニングが下手くそなので、調整方法についてはあえて触れていないです^^）
[マイクロマウス Advent Calendar 2022](https://adventar.org/calendars/8292)の13日目は㈱RTの中川 範晃さんによる「[STM32CubeMonitorの紹介](https://rt-net.jp/mobility/archives/22743)」です。
あまり詳しくはないですが、マイコンのRAMが自作ソフトなしにモニターできるようです。是非参考にさせてください！

# 参考

- [宮崎技術研究所](http://www.miyazaki-gijutsu.com/series/)