アナログロボトレーサ「RedSpecial」

私は、マイクロマウス ロボトレース競技に結構昔から参加しておりますが、近年(大体2018年あたりから）アナログ回路によるフィードバック制御を用いたロボトレーサを出場させています。ロボット名は「RedSpecial」。ハードウェアは多分に苦労があるものの、苦手（≒嫌い）なプログラムを触らなくて良いので安定した走行実績があります。

# 見たほうが早い
https://youtu.be/wYV1I6_x9_E

# ロボトレース競技とは
マイクロマウス競技内で開催されるライントレースロボット競技。詳細はこちら[New Technology Foundation 公式HP](https://www.ntf.or.jp/?page_id=27)。
普通のライントレーサーはコースの白線を追従するだけですが、この大会では**スタートとゴールのマーカーの間で自動的に停止**する必要があります。
![rt-003.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/ba9ac469-4fa9-55ca-13ed-6da553f63ac6.jpeg)


その一方で、コースには必ず交差点が存在するので、**ゴールと交差点を区別[^1]できないとその場で停止・リタイア**になります。
![rt-001.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/c4d6433b-a329-d270-defb-83198b2aefc2.jpeg)


[^1]: 通常、ゴールやスタートの検出はマーカーを用いますが、進行方向右側だけマーカーセンサを付けた場合、そのままでは交差点と区別ができません。そのため、反対側にもマーカーセンサを設けて、同時検出したら交差点として区別するのが一般的です。ただ、交差点は直角とは限らない(±5度)し、マシンが斜めぶれている場合もあるので、ゴールマーカーだけを検出する瞬間があることを考慮する必要があります。。

また、コースの形状は本番まで非公開で毎回(規程の範囲で)変わりますが、複数回(2022年現在5回まで)リトライ可能なので、曲率やマーカー・交差点などの特徴をベースにコースを記憶して、コーナー/直線での加減速やコースアウトしない範囲でのショートカットが可能です（もちろん可能なら。ですが）。
![rt-004.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/391fa128-98d0-7ac4-c0ba-805ce35a5397.jpeg)

# Redspecialの設計
Redspecialは自分が把握している限り、初めてのアナログ制御によるロボトレーサーです(諸説あり)。
![IMG_6303.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/1e64d0a5-8ebf-86bc-5e02-f9851ec545fb.jpeg)

![2022_Redspecial_2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/3c0b1a63-9955-1f8d-ec23-5baf2eeb334c.png)


## センサ
赤外線を用いたロボットのセンサは外乱光（照明や太陽光など）の影響を受けやすいです。対策として以下のものが挙げられますが、アナログトレーサーではセンサの値が取得できない状態が存在することは好ましくないため、5以外の対策を採用しています
1. 周囲を囲い外乱光の侵入を物理的に防止
1. 受光部(フォトトランジスタ・フォトダイオード)が特定の波長以外受け取らない特性のものを採用
1. 受光回路の感度を下げる(特に太陽光などの強い外乱光によるセンサの飽和に有効）
1. 赤外線を外乱光の周波数(太陽光：直流、電球：50Hz/60Hz、インバーター式電灯：？Hz)以外の周波数で点滅させ、それ以外はフィルタする回路を設置
1. 赤外線発光時と消灯時の受光量の差分計算

### 発光部
赤外線LEDを、72kHzで点滅させるRC発振回路と定電流回路でドライブしています。
![スクリーンショット 2022-09-26 015852.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/8fd0bd2c-f846-709f-16e5-47a869bec003.png)
バッテリから直接LEDを点灯させると、バッテリ電圧の変動で光量が増減してしまいます。一方、レギュレータの下流でLEDを点滅させると、レギュレータの負荷が増えるだけでなくノイズの原因になります。それを解決するため、Q201とQ202の定電流回路を用いています。Q201はR219に流れる電流(≒LED電流)が大体 0.6[V] / R219の抵抗値 以上となると、コレクタ-エミッタ間が導通するので、Q202のベース電流が減少しLED電流が減少します。LED電流が減少するとQ201がOFFされるので再びLED電流が増加・・・といった動作で電流がフィードバックされます。

### 受光部
72kHzという比較的高速な赤外線を受光するため、赤外線フォトダイオードを逆バイアスして使用し、微弱な電流を高速オペアンプで電圧に変換しています（キーワード：少数キャリア、IV変換）。

![スクリーンショット 2022-09-26 013440.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/58a07e4c-f3a0-0cf7-e928-9268eaab9fff.png)
C204、C212、C213、C215はオペアンプの発振防止コンデンサです。

### フィルタ＋整流回路
72kHzの赤外線の点滅成分のみを抽出し、直流に変換する回路です。多重帰還バンドパスフィルタ回路という高性能なフィルタと、ショットキーダイオードを用いた半波整流回路の組み合わせになります。
![スクリーンショット 2022-09-26 020741.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/8d541fdc-2e65-11e2-248f-5d172826753c.png)
多重帰還バンドパスフィルタは[こちら](http://sim.okawa-denshi.jp/OPtazyuBakeisan.htm)を参考にパラメータを決めました。
ショットキーダイオードは検出した電圧(±2.5V以下)の電圧をロスなく直流に変換するために採用しています。また、ダイオードの向きは後々の回路の簡略化ため、極性を逆転させる役割があります。
電圧が揃うよう、調整用の可変抵抗を通して次段の回路に接続されています。


## アナログコンピュータ(PID制御)
:::note warn
このへんの回路は設計ミスがあり、色々手直ししています。考え方は間違っていなかったのですが。。。ふ～んぐらいの感じで回路図の詳細までは参考にしないでください。
:::

かなり複雑に見えますが、ただの加算増幅器の組み合わせです。コンデンサと可変抵抗を駆使して、PID演算ができるようになっているはずです。
![スクリーンショット 2022-09-26 021547.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/2008d306-a63a-e859-8323-d900aaab38d2.png)
また、通常であればPID制御には目標と現在の差分(引き算)が必要となりますが、回路の簡略化のため、加算回路を用いています。その代わりに、入力される値を-1になるように設計しています(先程の整流回路でダイオードが逆だった理由)。

```math
C = A - B \Leftrightarrow C = A + (-B)

```

### 旋回制御
U211A～Dはセンサの左右の値の差をPID制御により補正する値を計算しています。先の整流回路で得られたセンサ電圧は、片方マイナスになっているので実質引き算となります。

### 速度制御
U208A/U208BはPI制御により速度を制御します。速度はタイヤに取り付けたギヤからタコジェネレータ（要は直流発電機）により、速度に比例する電圧として取得できます。また、目標速度は別の回路(本大会ではArduinoとDAC回路）から受け取ることで指示されます。この速度指示値も、-1倍になるように予め計算されています。


## モータ制御
ここまでは、速度と旋回制御のPI制御・PID制御についてに説明しました。前段のPI制御・PID制御制御からは（細かい計算式は省きますが）加速度・旋回トルクがでてきますが、このロボットは左右の車輪の回転差で旋回するロボットのなので、左右のモータトルクに成分を分解する必要があります。[こちら](https://www.mech.tohoku-gakuin.ac.jp/rde/contents/course/robotics/wheelrobot.html)を参考にして計算式を導出しました。

```math

\begin{bmatrix}
v \\
\omega
\end{bmatrix} = 
\begin{bmatrix}
\frac{v_r + v_l}{2} \\
\frac{ v_r - v_l }{d}
\end{bmatrix}\\

v = ロボット速度[m/s],\quad \omega = ヨーレート[rad/s] \\
v_r = 右タイヤ速度[m/s],\quad v_l = 左タイヤ速度[m/s] \\
d = 左右のタイヤ間距離[m]
```
これを微分。ギヤ比やタイヤ間距離などの定数は雑にまとめてしまいます(実際はPIDのゲインにまとめられます)。

```math

\begin{bmatrix}
a \\
T_y
\end{bmatrix} = 
\begin{bmatrix}
T_r + T_l \\
T_r - T_l
\end{bmatrix}
\begin{bmatrix}
k_a k_y
\end{bmatrix}
\\
a = ロボット加速度[m/s^2],\quad T_y = ロボット角加速度[rad/s^2] \\
T_l = 左タイヤトルク[Nm],\quad T_r = 右タイヤトルク[Nm] \\
k_a = 加速定数, \quad k_y = 旋回トルク定数\\
```
加速度と旋回トルクを受け取り、左右のトルクに変換。
```math
\begin{bmatrix}
T_l \\
T_r
\end{bmatrix} = 
\begin{bmatrix}
\frac{a}{2k_a} - \frac{T_y}{2k_y} \\
\frac{a}{2k_a} + \frac{T_y}{2k_y}
\end{bmatrix}\\

\begin{bmatrix}
T_l \\
T_r
\end{bmatrix} = 
\begin{bmatrix}
aKp_{acc} - Kp_{yaw}T_y \\
aKp_{acc} + Kp_{yaw}T_y
\end{bmatrix}\\

```

### トルクの分解
上記の計算を実現するため、加算増幅器と-1倍の増幅器を組み合わせて引き算を行います。また、マイコンからの指示で走行開始許可を受け取り、後段のパワー回路への信号受け渡しをスイッチで制御します。
![スクリーンショット 2022-09-26 102618.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/2d74317a-68fa-dba4-0288-c12a6ed5d542.png)
なお、後段の定電流モータドライバが受け取る電圧が大きくなると、シャント抵抗での損失が増加するので10分の1になっています。

### 定電流モータドライバ
この回路は、シャント抵抗の端子電圧をフィードバックして、指示電圧と一致するように制御するアンプ(U502A)と、それに追従して逆の電圧を出力するアンプ(U502B)からなります。これで、フルブリッジ回路のようにモータに正逆の方向の電流が流せます。
また、オペアンプの出力ではモータを駆動できないのでMOSFETでブーストしています。このとき、MOSFETの温度上昇でゲートしきい値が下がり熱暴走する減少が発生するので、これを防止するための補正回路がQ501とQ503であり、MOSFETと熱結合させてあります。このあたりの作りは殆どオーディオアンプと同じ原理です。
![スクリーンショット 2022-09-26 103549.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/269597/13e2a152-f020-f458-b5c0-355c6ca58bac.png)
なお、MOSFETのゲートしきい値(約数ボルト)はトランジスタ(約0.6V)に比べ大きく、バッテリ電圧ギリギリの電圧が出力できずに速度の低下に繋がりますので、バイアス回路は別途設けた昇圧回路で駆動されています。
また、アナログコンピュータ周りはバッテリのマイナス端子から5Vで動作(±2.5V電源)しますが、モータドライバはGNDからバッテリ電圧(約12V程度)の間で動作します。したがって動作中央の電圧が異なるので差動増幅器で吸収しています。


## デジタル系
マーカーを検出し停止したりスタートしたりする動作や、加減速制度のための位置情報取得にはArduinoを使い、ADC・DACでアナログ系と接続します。DACで速度指示さえ行ってしまえば勝手に走り出すので、ソフト側の処理は最小限です。

# 今後
つい最近まで、SPI接続のADCがうまく動作せずArduinoからすると何が起きているかわからない状況でした。結局原因はドライバソフトのバグだったようなので、このあとは加減速制御を行い、長い直線は高速に。小さいRのコーナーは低速に制御することで、よりタイム向上を図る予定です。