M5StickC + RoverC + UnitV でオブジェクトを追跡させる方法

#Introduction

※M5Stackから教えていただいたコードは、どうもM5StickVをベースにしていたようです。
　その前提をもとにコードを見直し、記事を修正しました。（2019/12/30）

RoverC ing pic.twitter.com/RbYs69ep0e

— M5Stack (@M5Stack) December 6, 2019

近頃、新製品ラッシュの止まらないM5Stickシリーズですが、このTweetのメカナムホイール付きのRoverCとK210搭載のUnitVの組み合わせに興味が湧いて初めて購入してみました。

ちょうど機械学習や深層学習もやってみたいと思っていたので、ハード製作の手間を省いてソフトの学習を進めるにも楽しそうなキットです。とは言いつつも自分でイチからコードを書くのもハードルが高そうだったので、何気なく呟いてみたところ・・・・

こいつのコードってgithubとかに上がってるんだろうかhttps://t.co/EhE4L9fD0P

— AIRPOCKET＠rastaman vibration (@AirpocketRobot) December 23, 2019

即座にアップしてくれるという神対応。

https://t.co/EB1ozXFHhC

— M5Stack (@M5Stack) December 24, 2019

途中、ちょっと失礼なことも言っちゃってますが、感謝の念に堪えません。
電子工作初心者の非力な身ではありますが、M5Stick公式の心意気に応えるためにも、このガジェットをTwitterの動画の様に動かすまでのセットアップについてまとめておきます。

#構成
今回のガジェットはタイトルの通りM5StickCとRoverCとUnitVと言う機器で構成されています。
M5StickCは、UnitVから届いたオブジェクト検出データをもとに動作判断を行い、RoverCを制御します。
RoverCはM5StickCからの制御指令に従い動作するだけなので、特別なセットアップは必要ありません。
UnitVはカメラによる撮影とOpenMVのオブジェクト検出機能を用いてオブジェクトの検出データをM5StickCに送信します。

今回の記事では、M5Stackより公開されたM5StackCのArduinoIDE用スケッチとUnitVのMaixPy用ソースコードについてポイントを解説します。

記事のベースになるソースコードはこちらにあります。
https://github.com/icyqwq/RoverC_TrackBall

#M5StickCのセットアップ
M5StickCの使い方についてはたくさんの記事が有りますので要点の説明のみです。
ソースはArudinoIDE用のスケッチです。
以下の点に注意してスケッチをM5StickCに書きこんで下さい。

**M5StickC側のコードはそのまま書き込んでも問題なく動作します。スピード調整やオブジェクト探索のための行動パターンなどはそれが楽しみでもありますので皆さん工夫してみてください(^^)/

###動作方向判定の符号反転
この部分の符号が逆になっていますので修正して書きこみましょう。
loop()関数内、106行目と110行目です。

以下の動作方向の違いは、M5sticKVとUnitVのカメラ取付向きの違いに起因するものです。UnitV側のコードで修正した方がスマートですので以下の修正は不要です。

･･･略
            else if(v_data.dx >= -kThreshold)     //デフォルトでは<=、正しくは>=
            {
                state = kLeft;  // Go left
            }
            else if(v_data.dx <= kThreshold)      //デフォルトでは>=、正しくは<= 
            {
                state = kRight; // Go right
            }
･･･略

※ただし、UnitVの取付け方向にも依存するかと思います。公式Tweet動画の通りの取付け方向だとこれで正常に動きます。UnitVの端子が上側、ロゴが天地逆の向きです。

###オブジェクト検出位置のキャリブレーション
UnitVからはオブジェクトのセンター位置が、カメラ視野のセンター位置から左右どちらへどれだけずれているかと言う情報が送られてきます。しかし、この位置情報のゼロ点とRoverCの進行方向センターがずれています。おそらくカメラの取付け角度のズレの為だと思いますが、カメラ視野が比較的狭いためオブジェクトをロストする原因になります。
位置情報はV_data.dxという変数に格納されているため、キャリブレーション値を入れて補正しましょう。
私の場合は+50でセンターに合いました。

このセンター位置のずれは、M5StickVとUnitVのカメラ取付向きの違いによるものです。
この修正も、UnitV側のコードで修正した方がスマートですので、次のコード修正は不要です。

･･･略
void loop()
{
    VSerial.write(0xAF);

    if(VSerial.available())
    {
        uint8_t buffer[5];
        VSerial.readBytes(buffer, 5);
        v_data.dx = (buffer[0] << 8) | buffer[1];
        v_data.dx += //ここにキャリブレーション値を入れましょう。
        v_data.area = (buffer[2] << 16) | (buffer[3] << 8) | buffer[4];
･･･略

v_data.dxの値はSerial.Printされています。StickC、RoverC、UnitVを正しく取り付けたうえでRoverCの電源を切りStickCとPCを接続し、ArduinoIDEのシリアルモニタでdx値をモニタリングしましょう。オブジェクトをRoverCの正面に置いた時にv_data.dxがゼロになる様に補正値を定めます。私の場合は+50くらいでセンタリングできました。

ただし！！！
UnitVのソースにバグがあり、初期のままだとdx（横方向の変位）データにdy（縦方向の変位）データが送られています。キャリブレーションの前にUnitVのソースを修正して下さい。

###速度調整
速度調整はお好みのチューニングで良いかと思います。loop()内の後半にSetspeed()関数で設定しています。デフォルトで20となっている引数を調整しましょう。

#UnitVのセットアップ

セットアップだけであれば不要ですが、キャリブレーションや動作確認にはMaixPy IDEがあると便利です。0.2.3以前のバージョンではUnitVを正しく認識しないため、v0.2.4が必要です。

MaixPy IDE v0.2.4はこちら。
http://dl.sipeed.com/MAIX/MaixPy/ide/_/v0.2.4

早速セットアップに進みたいところですが、ソースに間違いが有りますので37行目のtarget[6]をtarget[5]に変更してからセットアップに進みましょう。

※追記１：この間違いもM5StickVとUnitVのカメラ取付向きの違いによるものです。
これにより、カメラ画像が左右反転していますので、次の一文を追記しましょう

sensor.set_hmirror(0)

※追記２：このコードではターゲットオブジェクトの色を検出します。カメラのゲインとホワイトバランスが自動で変化するとオブジェクトの色の見え方が変わってしまうためロストの原因となります。次の2行を加えて自動調整機能をオフにした方がよさそうです。

sensor.set_auto_gain(0)                     #検出安定化のためオートゲインオフ
sensor.set_auto_whitebal(0) 

以上の修正を加味したUnitV用コードは以下の通りです。

import sensor
import image
import lcd
import time
import utime
from machine import UART
from Maix import GPIO
from fpioa_manager import *

fm.register(34,fm.fpioa.UART1_TX)
fm.register(35,fm.fpioa.UART1_RX)
uart_out = UART(UART.UART1, 115200, 8, None, 1, timeout=1000, read_buf_len=4096)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_hmirror(0)                       #カメラ左右反転
sensor.set_auto_gain(0)                     #検出安定化のためオートゲインオフ
sensor.set_auto_whitebal(0)                 #検出安定化のためオートホワイトバランスオフ
sensor.run(1)

while False:
    uart_out.write('TEST\n')
    utime.sleep_ms(100)

target_lab_threshold =(45,   70,  -60,   -30,   0,   40)   #ターゲットに合わせてRGB(565)LABカラーベースで閾値調整が必要

while True:
    img=sensor.snapshot()

    blobs = img.find_blobs([target_lab_threshold], x_stride = 2, y_stride = 2, pixels_threshold = 100, merge = True, margin = 20)
    if blobs:
        max_area = 0
        target = blobs[0]
        for b in blobs:
            if b.area() > max_area:
                max_area = b.area()
                target = b
        if uart_out.read(4096):
            area = target.area()
            dx = 160 - target[5]                #UnitVとM5stackVではカメラ取付向きが違うためｘセンター位置を320pix/2,ｘ軸パラメータをtarget[6]→[5]へ変更
            hexlist = [(dx >> 8) & 0xFF, dx & 0xFF, (area >> 16) & 0xFF, (area >> 8) & 0xFF, area & 0xFF]
            uart_out.write(bytes(hexlist))
        else:
            pass
        print(target.area())
        tmp=img.draw_rectangle(target[0:4])
        tmp=img.draw_cross(target[5], target[6])
        c=img.get_pixel(target[5], target[6])

    else:
        if uart_out.read(4096):
            hexlist = [0x80, 0x00, 0x00, 0x00, 0x00]
            uart_out.write(bytes(hexlist))
        else:
            pass

UnitVの取り扱いはM5StickCに準じます。詳細は他サイトに譲りますが、ソースの修正が出来たら次の手順でセットアップします。

###①ファームウェアをUnitVに書きこむ
ファームウェアはこちら。
https://docs.m5stack.com/#/en/quick_start/unitv/unitv_quick_start?id=download

書きこみツールはこちら。
https://github.com/sipeed/kflash_gui/releases/tag/v1.5.3

###②MicroSDカードにソースを書きこむ
ソースはこちら
https://github.com/icyqwq/RoverC_TrackBall
ファイル名をboot.pyに変更してからmicroSDに書きこみ、UnitVに挿入します。
またmicroSDカードには相性問題があります。公式発表による使えるmicroSDカードはこちらです。
https://docs.m5stack.com/#/en/unit/unitv?id=sd-card-test

以上の手順でUnitVのセットアップは終了です。デフォルト設定での動作ができる様になりました。

#UnitVのモニタリングと閾値変更

セットアップが出来たのでRoverCを動かしてみたいところですが、おそらくこのままではオブジェクトを認識する事が出来ません。これは、初期設定でさだめられた色（緑色）にしか反応出来ないためです。また、緑にもいろいろあるので、閾値にマッチするオブジェクトを用意するのは困難です。
UnitVをモニタリングしながら動かし、追いかけさせたいオブジェクトに合わせた閾値設定を行いましょう。

###UnitVのモニタリング方法
UnitVにはディスプレイがついていないため、オブジェクトの検出状況が良くわかりません。設定がうまく出来るまでは、UnitV単体をPCと接続し、MaixPy上で動作確認をするとはかどります。
ボードの種類はM5StickVを選択すれば正常に接続できます。
MaixPyIDE上では、このようにカメラ画像をモニタリングできます。

かなり見にくいですが、 今日アップされたrover c用のunit vのソースをMaixPy IDEで走らせてみました。内容は理解できてないけど、緑のボールを認識してて、シリアル通信でその面積を送ってるっぽいです。
面積を最大化する＝接近できてるってことかな？ pic.twitter.com/VPU36un7n7

— AIRPOCKET＠rastaman vibration (@AirpocketRobot) December 24, 2019

###対象の閾値変更
このソースでは、23行目のtarget_lab_threshold = (45, 70, -60, -30, 0, 40)で指定した閾値内の色を検出しています。ターゲットとする対象に合わせて閾値を変更しましょう。6つの数字はRGB(565)のLABカラースペースです。MaixPy IDEの[ツール]→[マシンビジョン]→[しきい値エディタ]を使うと、簡単に閾値を見つけられます。

#RoverCの注意点
RoverCについては特にする事はありませんが、２点ほど注意点が有ります。
###①充電はM5StickC経由で行う
RoverCには充電用端子が有りません。RoverCにM5StickCを差してM5StickCを充電するとRoverCも同時に充電されます。
###②メカナムホイールと車体が干渉して止まる
メカナムホイールはDカットされた軸にささっているだけですが、奥までさすとホイールとシャシーが干渉します。ホイールが止まったり動きが遅い場合は少し抜きましょう。

#まとめ
以上の手順でRoverCが次の動画の通りオブジェクトを追いかけ始めてくれます。

動作確認出来た!!!
チューニングの余地はまだまだあるのでしばらく楽しめそう。 pic.twitter.com/1Lrf7RXQV2

— AIRPOCKET＠rastaman vibration (@AirpocketRobot) December 25, 2019

ソースを読み込んで知ったのですが、このオブジェクト検出は次の一行のコマンドで実行されています。

    blobs = img.find_blobs([target_lab_threshold], x_stride = 2, y_stride = 2, pixels_threshold = 100, merge = True, margin = 20)

find_blobs()の参考資料
https://maixpy.sipeed.com/en/libs/machine_vision/image.html#imagefindblobsthresholds-invertfalse-roi-xstride2-ystride1-areathreshold10-pixelsthreshold10-mergefalse-margin0-thresholdcb-none-mergecbnone

これはOpenMVのライブラリにある画像内の色を検出するコマンドの様です。画像のドットの色が閾値無いであるか否かを判定しているだけでいわゆる”AI”によるオブジェクト検出ではありませんでした。少し残念ではありますが、ベースとなる車両が手に入っているので”頭脳”を入れ替えてやればイメージしているAIカーに近いものが出来るはずです。K210のパワーでどの程度の認識ができるかはわかりませんが、次は機械学習画像認識によるオブジェクト追跡にトライしたいと思います。

#おまけ
動作確認の動画では、ターゲットとなるオブジェクトとしてLEDライトに色紙を貼り付けたものを使用しました。
色によるオブジェクト認識を行う現行システムでは、外乱光によってカメラに映るオブジェクトの色が大きく変化しロストや誤検知が多くなります。特に夜間の屋内は照明の照度が低くバラツキも大きいため、自発光するオブジェクトの方が認識しやすくなります。
プレイステーションムーブのコントローラーがこんな形していた理由が良くわかりますね。
https://www.playstation.com/ja-jp/explore/accessories/vr-accessories/playstation-move/