ししかわです。
社員研修の一環で、マイクロマウスを自作して大会に出場しました。
全日本大会2020は終わりましたが、ブログはもうちょっとだけ続きます。
今回はドライバとコントローラ（図中赤枠）の実装です。
ドライバの実装
まずはセンサやモータを駆動するためのドライバの実装です。
個別のセンサやモータについては、マウスの基板を作る前にそれぞれ単体で動かしていました。動作確認のときのコードをベースにしつつ、別ファイルに切り出して実装しました。
- Part.16 STM32で光センサの値を読み取る
- 実装したコード：sensor.c
- Part.18 STM32でDCモータを回す
- 実装したコード：motor.c
- Part.19 STM32でロータリエンコーダを読む
- 実装したコード：encoder.c
ジャイロセンサ
ジャイロセンサは基板を作り始めてから追加することにしたので個別の動作確認をしておらず、基板が届いてからドライバを実装しました。高良さんのブログ記事を参考にしました。
- マイクロマウス研修（kora編）[8]ジャイロとエンコーダ
- 実装したコード：gyro.c
動かしてみると、静止した状態でも僅かに角速度が出力されています（このような静止状態での角速度センサの誤差をドリフト誤差というそうです）。この角速度が蓄積されると、静止状態のはずのマウスが勝手に旋回し始めてしまいます（マウスが回っていると勘違いしている状態）。
そこでキャリブレーション関数を追加で実装しました。この関数は静止した状態の角速度を計測してオフセットとします。キャリブレーション後は測定値からオフセット値を差し引くことでドリフト誤差を抑制します。
カルマンフィルタなどを使うとさらに正確な補正ができそうですが、時間切れで実装できませんでした。
壁センサ
壁センサは2つのクラスから構成しています。
- 赤外線センサ（sensor）：赤外線LEDとフォトトランジスタを使ってセンサの生値を取得する。
- 壁センサ（wall_sensor）：赤外線センサを複数持ち、前と左右の壁の有無を判定する。
赤外線センサのみ実装して壁の有無の判定は制御ループの中で直接行う手もありますが、壁の判定もセンシングの一種とみなして責務をきっちり分けたいので壁センサを実装しました。クラスを分けたのは後からセンサの部品を差し替えるなどの変更をしやすくするためです。
ピン配置
動作確認の際に使った開発ボード「Bluepill」と自作マウス基板では使っているSTM32マイコンの種類も違いますし、ピンアサインも違っています。CubeMXを使って再度設定しました。現在のピンアサインは次の画像のとおりです。CubeMXとHALライブラリの組み合わせはコード手修正の手間が省けてミスも少なくなるので良いですね。
コントローラの実装
続いてコントローラの実装です。
オドメトリ
安定した走行のために、マウスが今迷路のどこを走っているか推定させる自己位置推定（オドメトリ）を実装しました。オドメトリが正確に計算できていると、壁補正（壁との距離を使った速度補正）が無くてもある程度きれいに走行してくれます。
M5Mouseは最初はオドメトリは使わず、台形制御による直進やスラロームのみ実装していましたが、走り続けているうちに誤差が蓄積していって壁にぶつかる失敗がありました。そこで途中からオドメトリを導入することに決めました。
オドメトリの実装方法については他の方のブログ記事を参考にしました。
- マイクロマウス研修（kora編）
- MIZUHO
ざっくり次のような流れで速度指令を計算します。
- 目標位置を計算する
- エンコーダとジャイロセンサから現在のマウスの速度、角速度を計算する
- 速度、角速度の値を用いてマウスの自己位置（推定値）を更新する
- 目標位置と自己位置の差分（ズレ）をもとに、目標位置に近づけるような速度指令（速度、角速度）を計算する
Motionクラス
目標位置の計算のために、直線や超信地旋回、スラロームなどの動作を個別のクラス（Motion）として分けました。
次のようなAPIを持ちます。
このクラスは動きの種類（直進や旋回）、目標座標、最大速度などの引数を与えて初期化すると、内部で目標軌道の生成を行ってくれます。m5motion_get_nextを呼ぶたびに制御ループの周期1つ分時刻を進めて、その時刻にマウスがどの座標にいるべきかの目標座標（TrackingTarget）を返します。
速度指令の計算
速度指令の算出方法はKanayama Control Methodに従いました。koraさんのブログ記事に詳しいです。
マウスから見て目標点の前後のズレをXe、左右のずれをYe、角度のズレをθeとしたとき、ズレを反映した目標速度（V）と目標角速度（ω）を以下の式で求めます。
V = Vr * cos(θe) + Kx * Xe
ω = ωr + Vr * (Ky * Ye + Kθ * sin(θe))ただし、Kx、Ky、Kθはそれぞれ前後、左右、角度のズレに掛けるゲインです。
これを実装すると次のようになります。三角関数の演算は算術演算を高速に行うためのarm_mathライブラリを使っています。
PIDコントローラ
PID制御はフィードバック制御の一種で、現在値と目標値の差に対する比例項（Propotional）、積分項（Integral）、微分項（Derivative）の3つの要素を使う方法です。
M5MouseではPIDコントローラを複数ヶ所で使い回すためにクラスとして切り出しました。Pゲイン、Iゲイン、Dゲインの3種類を引数としてインスタンスを作成した後、m5pid_updateメソッドを用いて観測値による内部状態更新とフィードバック値の取得を行います。
以上です。次回はドライバやコントローラを使ったマウスの制御ループについて解説します。