JetbotでCollision avoidance（衝突回避）を動かす

## はじめに

Jetson nanoは発売後すぐに買っていたのですが、今一つ活用できていませんでした。いつもの積み基板というやつです・・ 
Jetbotには興味があったのですが、[JetbotのWiki](https://github.com/NVIDIA-AI-IOT/jetbot/wiki/bill-of-materials)にあるように、シャーシーを3Dプリンターで形成するなどの敷居の高さがありました。

そうした中、とある勉強会でJetbotのハンズオンがあり、初めてJetbotを動かしてみたら目からウロコな点がいくつかあり、これはもう少し探究してみねば！！と思ったのです。何が目ウロコなのかと言うと、私はロボットのプログラムはやったことがないのですが、AIを使うとロボットプログラムのパラダイムが変わるんじゃないかと思ったことです。

Jetbotの衝突回避では、Deep Learningによる画像認識を使って制御を行います。具体的には、カメラで撮影した画像に「進め」（fee）か「よけろ」（blocked）のラベルをつけ、これを教師データとして学習を行います。すると、障害物（今回の例では黒線）を見つけると自動的にターンして障害物をよける、つまり衝突回避の動作をするようになります。

ハンズオンでは、黒線で囲った枠からはみ出すことなくJetbotを走行させるお題で各参加者が個々に教師データを作ったのですが、どこまで黒線が近づいたら「bloked」のラベル付けをするかが参加者によって異なり、これによってJetbotの走行特性が変わっていました。つまり、教師データの作り方次第で、黒線からはみ出すギリギリまで攻めるJetbotと、安全運転で早めに回避するJetbotが生まれていました。

なるほど、「データで制御特性も作ってしまう」のがAIなのかと思いました。自動運転でも、（当然従来の制御理論も併用されるのだと思いますが）データを使って動作を作り込むことによって、制御パラメータを一つ一つ記述していくやり方に対して、より多くの運転シーン適合できるのだなと思いました。

## JetbotのKitを入手

<font color="Red">**注意：**これからご紹介するKitは保護回路が入っていない18650サイズの生リチウムイオンバッテリーを使います。Kitの基板側に保護回路があり、バッテリーの過充電や過放電からの保護を行なっていますが、万が一過充電や過放電が発生すると発火の危険性があります。そのため、リチウムイオンバッテリーの使用は自己責任でお願いします。</font>

<font color="Red">Amazonで18650サイズのリチウムイオンバッテリーを検索すると"IMR"と表記されたタイプが出てきますが、**IMR（リチウム・マンガン電池）は直列接続で使ってはいけないため**、IMRではない通常の（リチウムコバルトなど）のリチウムイオンバッテリーでかつ保護回路なしのものを使います。（保護回路付きのバッテリーは長さが69mmあり電池ケースに入らないようです）。</font>

<font color="Red">このように、バッテリー関係は色々注意点がありますので、使用される場合はご自身でよく調べてから購入して下さい。</font>

ハンズオンでは、出来合いのJetbot Kitを使っていたため、私もKitを購入することにしました。NVIDIAのサイトでも、[NVIDIA パートナー JETBOT AIロボットキット](https://www.nvidia.com/ja-jp/autonomous-machines/embedded-systems/jetbot-ai-robot-kit/)がいくつか紹介されています。Jetbot Kitでググってみると、NVIDIAパートナーサイトに掲載されている「Waveshare JetBot AIキット」がAmazonで販売されています。しかしお値段が32,000円でちとお高いです。もう少し探してみると、[AliExpressという中華ECサイト](https://ja.aliexpress.com/i/4000007884768.html)で同じ製品が11,000円ちょっとで買えることを発見!（送料込みで12,000円ちょっとでした）

本当に届くか？という心配もありましたが、AliExpressはアリババグループのECサイトで、淘宝網(Taobao)が主に中国国内の消費者を対象としているのに対して、こちらは国外の顧客を対象としているとのことで、素性の怪しいところでもなさそうなので思い切ってポチってみました。すると注文から１週間程度で無事到着。この製品、18650サイズのバッテリーは別売のため、Amazonで到着前に買っておきました。組み立ての説明書は中国語と英語表記で、必要最小限の記載ですが、なんとか組み立て完了。実は組み立ての問題はあったのですが、それは末尾に書きます。

シャーシは金属製で作りはしっかりしています。写真に映っている基板やタイヤの他に、モーター（オリジナルと同じ[AdafruitのTTモーター](https://www.adafruit.com/product/3777)）、Wi-Fiアダプタ（Intel AC-8265）、Raspberry Pi Camera V2と同等のSONY IMX219を搭載した広角レンズ付きカメラや充電用のACアダプターもセットになっています。使っていませんが、リモコン操作用のゲームパットとCPU FANもついています。電池は別売で4000円ほどかかりましたが、これだけセットで送料込み12,000円程度なのは安いと思います。

組み立てが終わったJetbotの写真は以下のとおりです。
![P1254663.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/162338/1de0406a-a002-b9c9-80f5-1b4271516b82.jpeg)

基板を眺めて見つかった部品はこんな感じです。

* NXP PAC9685 PWM Servo Driver → オリジナルと同じ
* TB6612モータードライバ → オリジナルと同じ
* ABLIC S-8245AA → 過充電と過放電の監視。過放電になる前にバッテリーを切り離す
* AO4407A MOS FET → バッテリーの充電制御用
* APW7313 → バッテリー電圧から5V電源を生成する電圧コンバーター
* TI INA219 → バッテリー電圧と充電電流のモニター（代わりにADS1015の場合もある）

## Jetbotを動かす
### インストール
動かし方は、[オリジナルのJetbotと同じ](https://github.com/NVIDIA-AI-IOT/jetbot/wiki/software-setup)ですが、いくつか注意する点があります。オリジナルのJetbotは5Vのモバイルバッテリーを使いますが、Waveshare JetBot AIキットは4.2Vのバッテリー３セルを直列で使うため、12.6Vの出力があります。モーターに加える電圧も12.6Vとなるため、この違いに注意する必要があります。詳細は別途。

Jetbot専用のOSイメージやドライバ類がありますので、[WaveshareのWiki](https://www.waveshare.com/wiki/JetBot_AI_Kit)に従ってインストールします。注意しなければならないのは、ドライバ類はNVIDIAオリジナルのものではなく、[Waveshareのgithub](https://github.com/waveshare/jetbot)にあるものを使用する必要があることです。

<font color="Red">20200128追記：上記のWaveshare WikiでリンクしているJetbotイメージはv0p3p0(0.3.0)で版数が古くOLEDが動かないというコメントをいただきました。Jetbotイメージは、[NVIDIA本家のWiki](https://github.com/NVIDIA-AI-IOT/jetbot/wiki/software-setup)に移掲載してあるv0p4p0（0.4.0)を使う必要があります。
o-maguroさん、ご指摘ありがとうございました。</font>

NVIDIAオリジナルドライバではモーターが回りませんでした。電圧監視もWaveshare提供のドライバを入れないと動きません。Waveshareのドライバをインストールしてシステムを起動すると以下の写真のように、バッテリー電圧と充電電流が表示されます。これはオリジナルのJetbotにはない便利機能です。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/162338/cb24e519-5bc9-731c-12ce-6fb4e2e02046.jpeg">

電流値は充電用のACアダプターを抜くと0.0Aになってしまうため、ACアダプタから入ってくる充電電流を測っており、放電電流ではないようです。

### 動かしてみる - 真っすぐに走らない！
準備ができたらさっそく動かしてみます。Jetbotの操作はJupyterLab（Notbookの後継）から行います。Jetbotを起動するとJupyterLabがサーバーとして立ち上がっているため、Webブラウザから、http://&lt;Jetbotに表示されているIPアドレス&gt;:8888でJupyterLabにアクセスします。

JupyterLabを開くとNVIDIAが提供したNotebookがありますので、その中から「basic_motion」を選択します。このNotebookでは、Jetbotを旋回させることでモーターが動くことを確認できます。上からセルを実行して行き、以下のrobot.left(0.3)以下でJetbotを0.5秒間左回転することができます。

```python
from jetbot import Robot
robot = Robot()

import time

robot.left(0.3)
time.sleep(0.5)
robot.stop()
```
ここまではOKでした。次に、NVIDIA提供のNotebookにはなかったのですが、直進動作を確かめようと以下のコードを実行しました。

```python
robot.forward(0.3)
time.sleep(2.0)
robot.stop()
```
すると、大きく左にカーブしなら進んでしまい、真っすぐに走ってくれません・・
どうも左モーターの回転が遅いようです。それもかなり。使っているモーターは2.95ドルで買える安価なもので、かつ回転角の検出もしていませんので、左右の回転が同期しないのはいた仕方ないところです。調べてみると、モーターの制御コードに、PWMパルス生成の補正パラメーターがあったため、以下のように設定するとほぼ真っすぐ進むようになりました。

```python
robot = Robot()
# calibrate each motor
# y = alpha * x + beta, where y.max = 255 
robot.left_motor.beta  = 0.05
robot.right_motor.beta = 0.0

import time

robot.left(0.3)
time.sleep(0.5)
robot.stop()
```

## 衝突回避を動かす
ようやくAIらしいタスクに到達できました。衝突回避では、Notebookの「collision_avoidance」を使います。YouTubeビデオでこのNotebookを動かした方の投稿を見ると、部屋の中で壁などの障害物をよけながらルンバのように自律走行させていますが、今回はハンズオンと同じく、黒線で囲った枠からはみ出さないように走行させてみます。つまり、黒線が障害物ということです。
![IMG_1931.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/162338/4b84d3e3-bc39-dc31-1475-2ef5e75e4cdb.jpeg)

### 教師データの作成
教師データの作成は「data_collection」Notebookを使って行います。
Notebookを動かしてJetbotをコースに配置すると、こんな感じでカメラの画像が表示されます。カメラのレンズが広角のため画像が結構歪みますが、歪んだ状態で学習するため大丈夫でしょう。
<img width="764" alt="スクリーンショット 2020-01-26 9.14.24.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/162338/9c99996f-a570-d4b5-bce1-8f14740e223b.png">

画像を見ながら、黒線との距離関係を加味して、この位置なら「進め」か「曲がれ」なのかを自分が判断し、進めなら「add free」曲がれなら「add blocked」のボタンを押します。この判断は人に依存するところですが、この操作を繰り返して、ラベル付けされた訓練データを作成します。
<img width="285" alt="スクリーンショット 2020-01-26 9.26.08.png" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/162338/d81e102e-4c28-294b-cd2a-b10076547feb.png">

データはそれぞれ100以上作ります。撮影した画像を訓練データとテストデータに分割する際に、後半50個をテストデータとしているため、データ件数が100より少ない場合は訓練データが不足して学習精度が上がらない可能性があります。

### 学習の実行
次に学習を行います。学習はJetson nanoのGPUでもできるのですが、私はGTX1080を持っているので、PCに教師データを転送してPC上で学習を行いました。

学習には「train_model」Notebookを使います。Deep LearningのフレームワークはPyTorchが使われています。モデルにはAlexNetの学習済みモデルを使い、出力層を2クラスに書き換えて転移学習を行なっています。

```python
model = models.alexnet(pretrained=True)

# 出力層のクラス数を2に変更（オリジナルのAlexNetは1000クラスに分類）
model.classifier[6] = torch.nn.Linear(model.classifier[6].in_features, 2)
```

これで、入力画像に対してfree or blokcedの判定結果を出力するようになります。

今回合計1000枚の画像を使っていますが、GTX1080クラスのGPUなら学習は1分程度で終わります。Jetson nanoのGPUだとそこそこ時間がかかるかもしれません。1000枚画像を集めると認識精度(test_accuracy)は100%になりました。200枚の画像でも98%程度になるため十分だと思います。

### いよいよ走行 ー コースをはみ出してオーバランしてしまう！
学習を行うと"best_model.pth"というモデルデータが生成されるため、このファイルをJetson nanoに転送します。走行には「live_demo」というNotebookを使います。

live_demoのrobot.stop()があるセルの手前までを実行するとJetbotが走り出します。が、見事に黒線をオーバランして壁に激突してしまいました・・
画像認識が動いていないのか？訓練データを作り直したり、Jetson nanoを10Wフルパワーモードで動かしたりしても変わりません（Wikiではバッテリーの電流供給不足が起きないように、5Wモードで動かすと書いてあります）。

初日はここで時間切れ。一晩寝て原因を考えてみると、ひょっとすると、WaveshareのJetbotは12Vのバッテリを使っているので、5Vバッテリーを使うオリジナルのJetbotよりスピードが早いのではないかという点に思い至りました。ハンズオンで使った5VバッテリーのJetbotに比べて、ウチの12V版は確かに動きが早いような。

live_demoの走行部分のコードは以下の通りです。

```python

def update(change):
    global blocked_slider, robot
    x = change['new'] 
    x = preprocess(x)
    y = model(x)
    
    # we apply the `softmax` function to normalize the output vector so it sums to 1 (which makes it a probability distribution)
    y = F.softmax(y, dim=1)
    
    prob_blocked = float(y.flatten()[0])
    
    blocked_slider.value = prob_blocked
    
    if prob_blocked < 0.5:
        robot.forward(0.4)
    else:
        robot.left(0.4)
    
    time.sleep(0.001)
        
update({'new': camera.value})  # we call the function once to intialize
```

カメラ画像が更新されると上記の関数が呼び出され、回避動作が必要な確率（prob_blocked）をニューラルネットワークで計算し、0.5以上の場合に左ターン、それ以外は直進します。
ここで、robot.forward(0.4)が直進速度ですが、同じ0.4でもバッテリー電圧によって実際の速度が変わります。そのため、ウチのJetbotはハンズオンで使ったJetbotより速度が速く、画像認識が追いついていなかった可能性があります。

そこでパラメーターをいじって、robot.forward(0.26)にしてみました。
すると、見事に衝突回避走行に成功！！

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">Jetbotのデモ画像。途中で搭載カメラのライブ映像が出ます。障害物（このデモでは黒線）を検出するとスライダーが上に上がって左ターンします。 <a href="https://t.co/KWAPOgFjUk">pic.twitter.com/KWAPOgFjUk</a></p>&mdash; Todotani (@todotani) <a href="https://twitter.com/todotani/status/1221032484390879232?ref_src=twsrc%5Etfw">January 25, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

Jetson nanoでの画像認識速度を調べたくなり、以下のfps表示コードを入れてみると、10Wモードでは20fps程度、5Wモードで16fps程度になりました。しかし、このコードを入れると黒線をオーバーランしてちゃんと走ってくれません。恐らく、widgetを更新する際に発生するJetson〜PC間の通信遅延のために衝突判定のタイミングが狂うのだと思います。リアルタイム制御なのでタイミングには結構シビアなようです。

```python
import torch.nn.functional as F
import time

t0 = time.time()
t1 = time.time()
fps_widget = widgets.FloatText(description='fps')
display(fps_widget)

def update(change):
    global blocked_slider, robot, t0, t1
    x = change['new'] 
    x = preprocess(x)
    y = model(x)
    
    # we apply the `softmax` function to normalize the output vector 
    # so it sums to 1 (which makes it a probability distribution)
    y = F.softmax(y, dim=1)
    
    prob_blocked = float(y.flatten()[0])
    
    blocked_slider.value = prob_blocked
    
    if prob_blocked < 0.5:
        robot.forward(0.26)
    else:
        robot.left(0.35)
    
    # update FPS widget
    t1 = time.time()
    fps_widget.value = 1.0 / (t1 - t0)
    t0 = t1
    
    time.sleep(0.001)
        
update({'new': camera.value})  # we call the function once to intialize

```

## 次の課題
現状以下の問題があり改善が必要です。

WikiではJetson nanoを5Wモードで動かせとしていますが、私の環境では5Wモードではオーバーランが発生してしまい、安定して動かすためには10Wモードが必要です。まだ速度が早すぎるのかもしれませんが、PWMのパラメータをこれ以上小さくするとモーターが起動しなくなるため、別のチューニングが必要です。そこで、TensorRTでモデル圧縮を行い、5Wモードでも動作するかをチェックしたいと考えています。

実は10Wモードでも最初の走行はなぜかオーバーランが発生し、一度モーターを止めてから再度走行するとうまく動きます。なぜ一回目の動作が安定しないのかは謎で、もう少し調べたいと思っています。

## 余談 ー 組み立ての苦労話
Waveshare Jetbotを組み立てる際に苦労した点です。

AC-8265へのアンテナケーブルの取り付け：
PCで、M.2スロットに装着するWi-Fiアダプターの交換をしたことがある方は分かると思いますが、アンテナケールブルについているコネクターが小さくかつデリケートです。デリケートなのは今回コネクターを壊して初めて気がつきました。アンテナケーブルを装着する際に、最初はWi-FiアダプターをM.2スロットに固定した状態で行ったのですが、ケーブルの余長が短くかつケーブルが硬く曲げづらいため、コネクターへの挿入に悪戦苦闘している間に、アダプター側のコネクターを壊しました（写真のAUX側コネクタを破損）
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/162338/bda7e415-911f-a382-786e-e2b393f53a74.jpeg" width=50%>

MAIN側が生きていたため受信感度は十分あったのですが、悔しいのでAmazonで新品を買い直して装着しました。受信感度は結局のところ同じだったのですが・・

アンテナケーブルを取り付ける際は、モジュールをM.2スロットから外した状態で行った方がうまく行きます。

カメラケーブルの取り付け：
カメラ用のCSIケーブルも同様に接続の際に注意が必要です。こちらもコネクターが弱く壊しやすいです。というか、Jetbotを作る前に既に壊していました。この場合もJetson nanoをシャーシに固定した後で取り付けようとすると、コネクターがある位置に手が入りづらいため、固定する前に行った方がやりやすいです。

基板の電源SW：
こちらは工作上の問題ではないのですが、電源SWが剥き出しのため、誤ってSWを切ってしまうことが多々発生。例えば、Jetbotがオーバランして壁にぶつかり、慌てて拾い上げようとした際にSWに触って電源を落としてしまうことが何度も発生しました。プラ板などを使ってSWの保護カバーを作る必要があります。
<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/162338/bc08f76d-0378-5594-a9a3-d49500505e6d.jpeg" width=60%>