強化学習で実機ライントレーサ制御

# 概要
今回の記事は，自分が大学の講義で作成したライントレーサの紹介記事のようなものなので，普遍的に役立つような内容ではないという事を先に断っておきます。

さて，ライントレーサと言えばフォトリフレクタ等のセンサを取り付けた差動二輪が真っ先に思い浮かぶと思いますが，定義的には平面上に引かれた線の上を自動追尾する移動ロボットであれば，ライントレーサと呼ぶことができます。
今回自分は，オリジナリティを優先するために，ハード面では全方向移動，ソフト面では強化学習という２つのコンセプトを定めました。
全方向移動が可能であることは強化学習を行う際に大きなメリットがあります。例えば対向二輪をベースに強化学習を行う場合，機体が向いている方向を何らかの方法で強化学習モデルに認識させる必要があります。一方で全方向移動が可能であればその必要はなくなり，学習難易度が低下すると考えられます。
また，強化学習を利用するメリットを生かすためにセンサーとしてWebカメラを利用しています。カメラを利用することでセンサー数を抑えつつ，高い情報量を持ったデータが獲得できます。そして強化学習を利用することで画像データを効率的に処理し，制御信号へ変換できると考えられます。
一方でカメラを利用する都合上，被写体である地面とカメラの間に十分な距離を設ける必要があります。この問題に対処しつつ，全方向移動を達成するために，本体を球の内部に納めることにしました。

この記事では，主に強化学習周りの実装に着目しつつ，製作したライントレーサについて説明していきます。

ソースコードはGitHubにあるので適宜参照してください。

https://github.com/Azuma413/rl_linetrace

# ハードウェア
ライントレーサの実機の外観は以下の画像ようになっています。球殻の内部にオムニホイールを備えた本体が格納されており，球の重心の移動によって転がりながら動くような仕組みになっています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/5cac340d-76b5-34ec-d54d-9f989426f9bb.png)

上の画像だと分かりにくいので，CAD上の画像で各パーツの名称を示します。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/bacffe69-d0cf-c3b8-c97f-967830b85ae1.png)

|番号|名称|型番|メーカー|URL|仕様など|
|---|---|---|---|---|---|
1|オムニホイール|4571398312991|Nexus robot|[URL](https://www.vstone.co.jp/robotshop/index.php?main_page=product_info&products_id=5180)|直径48mm
2|Webカメラ|V-U0040|Logicool|[URL](https://support.logi.com/hc/ja/articles/360023462913-BRIO-4K-Pro-%E3%82%A6%E3%82%A7%E3%83%96%E3%82%AB%E3%83%A1%E3%83%A9%E6%8A%80%E8%A1%93%E4%BB%95%E6%A7%98)|水平視野角82.1度
3|ボールキャスター|S8P|TRUSCO|[URL](https://www.amazon.co.jp/gp/product/B002NBES4S/ref=ox_sc_saved_title_3?smid=AN1VRQENFRJN5&th=1)|許容荷重30kg
4|ギヤボックス|ウォームギヤーボックス HE|タミヤ|[URL](https://d7z22c0gz59ng.cloudfront.net/cms/japan/download/pdf/robot/72004.pdf)|ギヤ比336:1
5|ルーター|TL-WR902AC|TP-Link|[URL](https://www.amazon.co.jp/dp/B078GPSRSG?ref=ppx_yo2ov_dt_b_product_details&th=1)|11ac AC750。USB給電。
6|Raspberry Pi 5|RPI-SC1112|Raspberry Pi Foundation|[URL](https://akizukidenshi.com/catalog/g/g129326/)|5V5A入力推奨
7|モバイルバッテリー|-|Xuttrem|[URL](https://www.amazon.co.jp/dp/B0D1CJZBJ6?ref=ppx_yo2ov_dt_b_product_details&th=1)|480g,最大4.5A出力
8|モータードライバ基盤|TB6612FNG|東芝|[URL](https://toshiba.semicon-storage.com/info/docget.jsp?did=10661&prodName=TB6612FNG)|出力電流1.2 A(ave)/3.2 A (peak)
9|3Dプリンターパーツ|-|-|-|PLA
10|球殻|-|XYW|[URL](https://www.amazon.co.jp/gp/aw/d/B08LSHSX28/ref=ox_sc_saved_image_2?smid=A1A8BY3UILEMMB&psc=1)|直径300mm
-|モータ|RE-260|MABUTCHI MOTOR|[URL](https://www.mabuchi-motor.co.jp/motorize/branch/motor/pdf/re_260ra.pdf)|3Vを適正電圧としてトルクは0.97mN・m
-|電池ボックス|-|-|-|-

機体を下から見ると画像のようになります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/96b62234-a226-b29c-7632-20ac3bc37f44.png)
ボールキャスターを用いることで，2つのオムニホイールで全方向移動を達成しています。なお，この構成ではz軸周りの回転を拘束できませんが，それは強化学習によってカバーしています。

機体を横から見ると下の画像のようになっており，2層の3Dプリンターパーツにそれぞれのパーツが取り付けられている事が分かります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/a9f7166a-6f93-350e-be49-5e50a0ab54cd.png)

なお，全体のシステム・電源構成は以下のようになっています。
![システム電源.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/22dd63c8-4bf0-12e5-2f88-337a13b00aee.png)



# Gym環境の作成
次にソフトウェアについて説明します。

世の中のほとんどの強化学習モデルは，OpenAIが提供するGym環境という枠組みを継承していれば，どんな環境であっても実行できるようなっています。そのため今回は大きく分けて，シミュレータそのものを実装したクラスと，シミュレータをGym環境用にラップするクラスの２つに分けて学習環境を実装しました。

https://github.com/Azuma413/rl_linetrace/blob/3bbe5458c40501e8a9d823bca40e85a340abe867/drqv2/my_simulator_v2.py

まず，シミュレータそのものについてですが，今回はライントレーサのトレーニングを行いたいので，２次元画像で白地に黒い線を引いたマップを表現し，その中で仮想的にロボットが移動するようにしました。
そのうえで，シミュレータの機能を大きく分けると３つあり，１つ目はランダムな学習用コースの生成，２つ目は報酬の計算と観測の作成，３つ目はアクションの適用です。

### ランダムマップ生成
最初にランダムマップ生成について説明します。ランダムマップ生成というと，そもそも，なぜそんな事をしなければならないのかという疑問が生じると思います。例えば，実機動作時にトレースさせたいマップを用いて訓練を行えばいいと思うかもしれません。
しかし１種類のマップでライントレースを学習させた場合，エージェントはそのマップに特化してしまうため，普遍的なライントレースの動作は学習できていません。このようなモデルは，例え学習時のマップと実機動作時のマップが同じだとしても，Sim2Realの変化に耐える事ができません。
そのため，学習段階ではできるだけ多様なマップで学習を行い，エージェントの汎化性能を高めるようにしなければならないのです。

ランダム生成については，まずマップを4×4のグリッド上に区切った上で，幅優先探索アルゴリズムによって，スタートマスから出発して，また戻ってくるようなマスの配列を求めます。
|幅優先探索によるルート生成のイメージ|生成されたマップ|
|---|---|
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/2bf4c3d2-c92b-ccba-3566-8a37c7d932c8.png)|![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/99e65263-82fb-5950-8ca9-78691e561718.png)

そのようにしてルートを生成したうえで，マスとマスの境界のランダムな位置にラインが通るべき点を定め，OpenCVのline関数やellipse関数を用いてそれらの点と点を繋ぐように黒線を描画しています。


### アクションの適用
次にエージェントから出力されたアクションの，シミュレータへの適用について説明します。
初期のモデルでは，アクションを単純にサイズが２のベクトルとして，機体のxy平面上の移動速度を表現していましたが，最終的なモデルでは速度を固定して一自由度を削減することで，スカラーのアクションで移動方向を表現するようにしています。

プログラム中には`action_limit`, `action_discount`, `action`, `action_average`の4つの変数が登場しますが，それぞれ
- `action_limit`は移動方向が進行方向に対してどれだけ変化できるかを決定
- `action_discount`はアクションによって進行方向がどれだけ変化するかを決定
- `action`はエージェントから出力されたアクションであり，-1から1のスカラー
- `action_average`は基準方向に対する機体の進行方向を示しており，-1から1のスカラー

となっており，最終的な移動方向θは以下のように計算されます。
```py
theta = (self.action_average + self.action*self.action_limit)*np.pi
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/5ba80153-f0d8-2fdc-d40b-abbe4e1c277d.png)

このような移動方向の決定方法には，単純にベクトルで機体の移動方向を表す方法と比較して３つのメリットが存在します。

１つ目は前述のとおり，アクションのサイズを減らせることです。これによって強化学習の学習難易度を多少なりとも低減できると考えられます。

２つ目はアクションと基準方向を分離できることです。例えば，ベクトルで機体の移動方向を表現する場合，アクションはある基準方向に対して紐づけられているため，同じカーブでも右下のカーブと左上のカーブでは，まったく異なるアクションを出力しなければなりません。一方で今回のような計算方法の場合，アクションは進行方向に対して紐づけられているため，曲率が同じであればカーブの位置に関係なく，ほぼ同じアクションを出力すればいいことになります。これによって強化学習の学習難易度を低減できていると考えています。

３つ目は移動方向を進行方向に対して制限できることです。単純にベクトルで機体の移動方向を表現する場合，途中で進行方向を見失って逆走することが多かったのですが，このようにある程度，恣意的に進行方向を制限することで，逆走する可能性を低減することに成功しました。

### 観測の作成
次にエージェントに与える観測の作成について述べます。コードを見ると下の画像のような3チャンネルの画像を作成していますが，後述するようにこれは便宜的なものであり，実質的にはロボット周辺のマップを切り取った64×64のグレースケール画像と，進行方向を示すスカラー値，前回のアクションを示すスカラー値の３つの情報を観測としています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/75ed6d06-6185-d963-37f3-5c01ce09ebfd.png)

### 報酬の計算
最後に報酬の計算について説明します。報酬設計のポイントは，できるだけ観測に含まれる情報を使うようにする事と，期待する動きから逆算して報酬を与える事だと考えます。
今回の場合は以下のような２つの報酬を定め，その和を最終的な報酬として与えました。
1. 前回のアクションとのL1誤差
2. 観測の中心座標と，マップの観測の黒ピクセルの座標の平均座標の距離

１番目の報酬は，実験段階においてトレーニングされたエージェントの出力を見ていると，アクションの値の振動が激しかったため，それを防ぐために与えています。
```py
diff = np.abs(self.prior_action - self.action)
reward += 0.5 - diff/2
```
２番目の報酬は，ロボットを黒線の上に乗せるために与えています。
```py
# 黒ピクセルの座標を取得
black_pixels = np.where(self.obs[:,:,0] == 0)
# 黒ピクセルの平均座標
black_center = np.array([np.mean(black_pixels[1]), np.mean(black_pixels[0])])
# 画像の中心座標
image_center = np.array([self.obs_size[0]//2, self.obs_size[1]//2])
# 黒ピクセルの中心座標と画像の中心座標の距離を罰則とする
reward += (0.5 - np.linalg.norm(black_center - image_center)/np.linalg.norm(image_center))
```

これだけだと，ロボットは黒線の上に載った状態で静止してしまうと思うかもしれませんが，前述の通り，そもそもロボットの移動速度は固定されているため，いずれかの方向には必ず動くという事になります。


# エージェント
次に強化学習モデル自体の改良について述べます。
今回エージェントとして用いるDrQ-v2(Data regularized Q v2)は，Metaが開発したQ学習をベースにした強化学習アルゴリズムであり，論文によると以下のような特徴があります。
- モデルフリー手法で世界モデルベースのDreamerV2に匹敵する性能を達成
- 画像を観測として連続値制御が可能
- シンプルなモデルで処理が軽量

今回はWebカメラから取得した画像を使ってライントレースするため，画像を観測として利用できる必要があります。また，評価段階ではRaspberry Pi上で実行できなければならないので，メモリ消費量や計算量が軽量なモデルでなければなりませんが，DrQ-v2はこれらの要件を満たしていると言えます。
一方でこのモデルが発表されたのは2021年と比較的古く，またライントレーサよりも複雑なタスクを想定したパラメータ設定となっているため，モデル自体に改善の余地があると考えました。
そんなわけで今回，自分がDrQ-v2に対して手を加えた箇所は大きく分けて以下の３つに分けられます。
- 活性化関数及び最適化手法の変更
- 画像とスカラーの同時入力への対応
- パラメータ数の削減

https://github.com/Azuma413/rl_linetrace/blob/3bbe5458c40501e8a9d823bca40e85a340abe867/drqv2/drqv2.py

### 活性化関数及び最適化手法の変更
まず，活性化関数の変更について述べます。DrQ-v2ではすべての活性化関数がReLUになっていますが，近年の機械学習モデルでReLU関数が使われることは少なくなっており，代わりに改良型のGELU関数やSilU関数が用いられていると認識しています。
今回は画像の畳み込み処理に関わるエンコーダ部分の活性化関数をSiLUに変更し，それ以外の部分をGELUに変更しました。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/2d628db7-f6d1-34e5-1ad5-13916b6988a3.png)

赤がReLU，青がGELU，緑がSiLU

また，最適化手法についてもDrQ-v2ではAdam(Adaptive Moment Estimation)が利用されていたので，これをAdamW(Adam with Weight Decay)に変更しました。


### 画像とスカラーの同時入力への対応
次に画像とスカラーの同時入力への対応について説明します。前述のとおり，今回のタスクではGym環境からエージェントに対して３チャンネルの画像が観測として渡されます。しかし，３チャンネルのうち２チャンネルは実質的にスカラーの情報量しか持っていないため，これをそのまま画像として処理するのは非効率的と言えます。
そこで，DrQ-v2のエンコーダ部分を変更し，２チャンネルをスカラーとして扱えるようにしました。
```py
class EncoderV3(nn.Module):
    def __init__(self, obs_shape):
        super().__init__()
        feature_dim = 8
        convnet_output_dim = feature_dim * 5 * 5
        self.repr_dim = convnet_output_dim + 2
        self.convnet = nn.Sequential(nn.Conv2d(1, feature_dim, 6, stride=2),
                                     nn.SiLU(), nn.Conv2d(feature_dim, feature_dim, 4, stride=2),
                                     nn.SiLU(), nn.Conv2d(feature_dim, feature_dim, 4, stride=2),
                                     nn.SiLU(), nn.Conv2d(feature_dim, feature_dim, 2, stride=1),
                                     nn.SiLU())
        self.apply(utils.weight_init)

    def forward(self, obs):
        obs = obs - 0.5
        image = obs[:,0,:,:].unsqueeze(1)
        obs1 = obs[:,1,0,0].unsqueeze(1)
        obs2 = obs[:,2,0,0].unsqueeze(1)
        h = self.convnet(image)
        h = h.view(h.shape[0], -1)
        h = torch.cat([h, obs1, obs2], dim=-1)
        return h
```
観測データはforward関数のobs引数に渡されるので，これをimage, obs1, obs2の三つの変数に分割しています。この時，imageは64×64のグレースケール画像であり，obs1, obs2はスカラーになります。その後，畳み込み層を通じて１次元のテンソルに変換された画像の潜在表現に対して，obs1とobs2をconcatしています。

### パラメータ数の削減
最後にパラメータ数の削減について述べます。詳細は省きますが，エンコーダやアクタークリティックのパラメータ数を削減し，最終的に重みのデータサイズを75158kBから3278kBまで削減しました。
変更前のモデルと，変更後のモデルの学習時の報酬の推移は以下の画像のようになっています。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/4c06cb91-94af-2046-5b06-1f1cd380bd30.png)

黄色: 変更前のモデル, 青: 変更後のモデル

グラフを見ると，最終的な性能は若干下がっているものの，データ数を削減したモデルでも十分な性能を達成している事が分かります。
また，Raspberry Pi上での強化学習モデルの制御周期は1Hz前後から3Hz前後まで向上しました。制御周期が短くなることは，エージェント視点では機体の速度が遅くなることと同じであり，即ち，より繊細な制御ができるようになると期待されます。


<iframe width="560" height="315" src="https://www.youtube.com/embed/6cOpKQIKIt4?si=BANgxLPvAA3tilNB" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>

# Sim2Real
最後にSim2Realについて説明します。シミュレータ上でトレーニングしたモデルを現実世界に持ってくるにあたっては，その差異をどのように減らすのかであったり，埋め合わせるのかということが重要になります。そのため今回のライントレーサは，できるだけシミュレータ上でエージェントの汎化性能を高めたうえで，現実世界の観測をシミュレータの観測に近づけるという方針でコードを実装しました。
実機動作用のコードについてもGym環境として実装されており，ここには大きく分けて２つの機能を実装しました。
１つ目はモータの制御，２つ目は観測データの作成です。

処理の流れ
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/948fff5f-ebc7-6fd3-cac7-fdf15b454611.png)

https://github.com/Azuma413/rl_linetrace/blob/3bbe5458c40501e8a9d823bca40e85a340abe867/drqv2/my_controller.py

なお，実機での動作時にはすべての処理をRaspberry Pi 5上で行っています。ラズパイのOSはUbuntu24.04です。
### モータの制御
まず，モータの制御について説明します。
強化学習モデルからGym環境に渡されたアクションは，前述のシミュレータと同じ方法で移動方向θに変換されます。この移動方向の情報から２つのモータの回転方向とdutyを計算し，モータードライバの制御テーブルに則り，GPIOを制御します。

今回使用したモータードライバTB6612FNGの制御テーブル
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3231080/8bd1615f-b4c6-3b8e-c064-6988010078af.png)


GPIOの制御には，ラズパイ用のGPIOライブラリであるgpiozeroを利用しました。

### 観測データの作成
次に観測データの作成について述べます。
```py
def make_obs(self):
    """
    画像を取得して観測を作成する関数
    """
    while True:
        for _ in range(10): # 最新の画像を取得するために10回読み込む
            ret, frame = self.cap.read()
        if not ret:
            continue
        # frameを正方形にクリップ
        h, w = frame.shape[:2]
        if h > w:
            frame = frame[(h-w)//2:(h+w)//2, :]
        else:
            frame = frame[:, (w-h)//2:(w+h)//2]
        frame = cv2.resize(frame, (64, 64)) # frameを64x64にリサイズ
        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # frameをモノクロに変換
        mean = np.mean(frame) # frameの平均値を取得
        print(f"mean: {mean}")
        var = np.var(frame) # frameの分散を取得
        print(f"var: {var}")
        if var < 0.01: # 分散が小さい場合は線を検出していないと判断。観測を白で埋める
            frame = np.ones_like(frame)
        else:
            thereshold = mean*self.thresh # 平均値に応じて2値化の閾値を変更
            _, frame = cv2.threshold(frame, thereshold, 255, cv2.THRESH_BINARY) # frameを2値化
            frame = frame.astype(np.float32)/255 # 観測をfloat32に変換して正規化
        frame = np.dstack([frame, np.zeros_like(frame), np.zeros_like(frame)])
        frame[:,:,1] = (self.action_average + 1)/2
        frame[:,:,2] = (self.prior_action + 1)/2
        return frame
```
処理の流れとしては，まずWebカメラから取得した1920×1080×3のカラー画像を正方形にクリップして1080×1080×3にした後，OpenCVのresize関数で64×64×3に圧縮しています。
次に，圧縮したカラー画像を64×64×1のグレースケール画像に変換し，その画像のピクセルの値の平均と分散を求めています。
その後，求めた平均値から，動的に閾値を計算してグレースケール画像を二値化し，観測としています。
```py
mean = np.mean(frame)
thereshold = mean*self.thresh
_, frame = cv2.threshold(frame, thereshold, 255, cv2.THRESH_BINARY)
```
こうすることで，周辺環境が多少変化したとしても自動的に閾値を調整して，安定的にラインを検出できるようになります。
一方で，機体がライン上にない場合を想定すると，カメラ全体に白いフィールドが映っているような状況でも閾値を調整して無理やりラインを検出しようとすると観測がおかしくなってしまいます。
そのため，画像の分散がある程度小さいときは，ラインを検出していないと判断し，観測全体を白で埋めています。
その後，前述のシミュレータの処理と同様に，進行方向と，前回のアクションの情報を持つチャンネルを観測に加えています。
```py
frame[:,:,1] = (self.action_average + 1)/2
frame[:,:,2] = (self.prior_action + 1)/2
```

# 実走
<iframe width="560" height="315" src="https://www.youtube.com/embed/tTh6BYUjfMs?si=gQxMvGpT_n0bAc3R" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture; web-share" referrerpolicy="strict-origin-when-cross-origin" allowfullscreen></iframe>

# 参考文献
- D.Yarats, R.Fergus, A.Lazaric, L.Pinto, ”Mastering Visual Continuous Control: Improved Data-Augmented Reinforcement Learning”, https://arxiv.org/abs/2107.09645
- TB6612FNG
https://toshiba.semicon-storage.com/info/docget.jsp?did=10661&prodName=TB6612FNG