はじめに

何かAIを使った動くモノを作りたい！
ということで、タミヤのリモコンロボット製作セット(クローラータイプ) とJetson Nanoを使用して、顔認識して自分の元へ動いてくる「Follow me キャタピラ」を作ります。
Githubリポジトリはこちらです。

環境

Jetson Nano上での開発です。

OS : Linux Jetson-nano 4.9.140-tegra aarch64 GNU/Linux
Python : 3.6.9
OpenCV : 4.5.2

構成

必要なもの

細かい電気部品はすべて秋月電子通商で買ってます。

Jetson Nano
- 無いと始まらない
- 一応FANつけてます。
CSIカメラ
- ラズパイ用のものがJetson Nanoでも使用できる。
  - Amazonで昔買ったもの。安い中華製だった。
  - 秋月でもラズパイ用が売ってた。
タミヤリモコンロボット制作セット(クローラータイプ)
- 前後進・左右旋回できるDCモータ2個構成。クランク機構は今回全く使用しない。タミヤさんすいません。カメラステイ・Jetson Nanoを乗せる台にクレーン部分の部品を流用
- ギヤボックスとか、がんばって工作する。
モバイルバッテリー電源 (Jetson Nano, モータドライバ基板兼用)
- エレコム DE-C18L-10000WF
  - ２つのそこそこの電流を流せる5V出力(3A:Jetson Nano, 2.4A:モータドライバ基板)があれば何でも良い。ちなみにJetson NanoはMAXN(20W)モードでうごかすには5V/4A必要。今回は5Wモードで動作させる。
ブレッドボード
- モータドライバ基板用
モータドライバ用レギュレータ
- PQ3RD23
モータドライバ
- DRV8835モジュール
  - ブレッドボードに刺すために、ちょっとだけはんだ付けが必要
USB電源入力端子キット
- USB電源の口(USBマイクロ端子)をブレッドボードに刺すためのキット
  - やっぱりちょっとだけはんだ付けが必要
ジャンパワイヤ
抵抗
コンデンサ
LEDランプ
- ブレッドボード電源ON確認用
USB WiFiモジュール
- 何でも良いけど私はPLANEX GW-USNano2を使用。古い。。。
  - Jetson Nanoに取り付けて、WiFi経由でsshできるようにするため

電気回路

回路図というほどのものではないので、各ICとJetson Nano J41ピンヘッダの結線図を示します。
各ICの使用方法はデータシートを見ると書いてあります。

キャタピラキット改造

タミヤリモコンロボット制作セット(クローラータイプ)は有線リモコン付きのモデルです。リモコンには単一乾電池2個を直列に接続してDCモータの電源としています。ツインモータボックスがキャタピラの駆動系なので、この２つのモータからリモコンへ配線されているところを切って、モータドライバの出力のAOUT1/AOUT2, BOUT1/BOUT2へ接続します。
クランクギヤボックスは今回使用しません。クレーン部分の部品はカメラステイやJetson Nanoを乗せる台として流用しました。

JetsonNano GPIO設定とモータ制御

モータドライバからの出力でDCモータを駆動します。
レギュレータからモータ電源を作り、モータドライバに入力します。
今回はモータドライバのMODE=0(Low)で使用しています。MODEの説明については以下の表に示すとおりです。

IN/INモード (MODE=0)

xIN1	xIN2	xOUT1	xOUT2	動作
0	0	HiZ	HiZ	空転
0	1	L	H	逆転
1	0	H	L	正転
1	1	L	L	ブレーキ

PHASE/ENABLEモード (MODE=1)

MODE	xENABLE	xPHASE	xOUT1	xOUT2	動作
1	0	X	L	L	ブレーキ
1	1	1	L	H	逆転
1	1	0	H	L	正転

最初は、MODE=1を使用し、GPIOをtimerでH/Lを切り替えてPWMパルスを作り出し、それをxENABLEに入力して、PWM制御しようと思っていました。しかし、テストしてみたところ、正転はうまく動作したのですが、逆転がうまく動作せず、深く追う時間が無かったので断念しました。
MODE=0ではシンプルに与えたモータ電源電圧で回転動作をさせます。xIN1, xIN2にJetson NanoのGPIO Outを入力することで、正転、逆転、ブレーキ制御します。今回は空転（ブレーキや電圧をかけない状態にする。ゆるやかに止まる。）は使用していません。

Jetson NanoのGPIOは使用するGPIOポートを選択、IN/OUTの選択をすることで使用可能になります。今回はGPIO12, 13, 14, 15をOutとして使用します。

sample.py

import os
import subprocess
import time

def _setup_gpio(self) -> bool:
    # Map GPIO12, GPIO13, GPIO14, GPIO15 (pin 37, 22, 13, 18) 
    with open("/sys/class/gpio/export", "w") as dev_file:
        if os.path.exists("/sys/class/gpio/gpio12") is False:
            ret = subprocess.run(["echo", "12"], stdout=dev_file)
            if ret.returncode != 0:
                return False
        if os.path.exists("/sys/class/gpio/gpio13") is False:
            ret = subprocess.run(["echo", "13"], stdout=dev_file)
            if ret.returncode != 0:
                return False
        if os.path.exists("/sys/class/gpio/gpio14") is False:
            ret = subprocess.run(["echo", "14"], stdout=dev_file)
            if ret.returncode != 0:
                return False
        if os.path.exists("/sys/class/gpio/gpio15") is False:
            ret = subprocess.run(["echo", "15"], stdout=dev_file)
            if ret.returncode != 0:
                return False

    # Wait for mapping GPIO
    time.sleep(3)

    # Set direction as out on GPIO12 (pin 37) 
    with open("/sys/class/gpio/gpio12/direction", "w") as dev_file:
        ret = subprocess.run(["echo", "out"], stdout=dev_file)
        if ret.returncode != 0:
            return False

    # Set direction as out on GPIO13 (pin 22) 
    with open("/sys/class/gpio/gpio13/direction", "w") as dev_file:
        ret = subprocess.run(["echo", "out"], stdout=dev_file)
        if ret.returncode != 0:
            return False

    # Set direction as out on GPIO14 (pin 13) 
    with open("/sys/class/gpio/gpio14/direction", "w") as dev_file:
        ret = subprocess.run(["echo", "out"], stdout=dev_file)
        if ret.returncode != 0:
            return False

    # Set direction as out on GPIO15 (pin 18) 
    with open("/sys/class/gpio/gpio15/direction", "w") as dev_file:
        ret = subprocess.run(["echo", "out"], stdout=dev_file)
        if ret.returncode != 0:
            return False

    return True

実際にGPIOに値(1:High or 2:Low)を出力するには、valueに1 or 0をWriteするだけです。

sample.py

import subproces

def _set_gpio(self, addr: int, value: int) -> bool:
    with open(f"/sys/class/gpio/gpio{addr}/value", "w") as dev_file:
        ret = subprocess.run(["echo", f"{value}"], stdout=dev_file)
        if ret.returncode != 0:
            return False
    return True

AI/キャタピラ制御アプリケーション

Githubで公開しています。
https://github.com/kanlkan/FollowMeTank

Build OpenCV (opencv-python)

今回はRaspberry Pi用カメラを使用して顔認識しますので、Gstreamerオプションと物体(顔)検出・顔認識のためのライブラリ(dnn, face)を有効にしてopencvをビルドします。
いつもビルドオプションを忘れるので、メモがてらcmakeオプションを掲載します。
なお、ビルドにはopencv-contribも必要なので、opencvとともにgit cloneしておきます。
opencv 4.5.5でビルドしたら、なぜかdnnライブラリのビルドでエラーが出たので、4.5.2でビルドしました。

$ cd opencv
$ mkdir build
$ cd build
$ cmake \
-DBUILD_opencv_python3=ON \
-DBUILD_opencv_python2=OFF \
-DWITH_GSTREAMER=ON \
-DWITH_CUDA=ON \
-DCUDA_FAST_MATH=ON \
-DWITH_CUBLAS=ON \
-DWITH_NVCUVID=ON \
-DMAKE_BUILD_TYPE=RELEASE \
-DOPENCV_EXTRA_MODULES_PATH=~/work/repo/opencv_contrib/modules \
-DOPENCV_FORCE_PYTHON_LIBS=ON \
-DCMAKE_INSTALL_PREFIX=/usr/local \
-DWITH_OPENCL=ON \
-DENABLE_FAST_MATH=ON \
-DWITH_LIBV4L=ON \
-DWITH_V4L=ON \
-DINSTALL_C_EXAMPLES=OFF \
-DWITH_DC1394=OFF \
-DENABLE_NEON=OFF \
-DOPENCV_ENABLE_NONFREE=OFF \
-DWITH_PROTOBUF=ON \
-DINSTALL_PYTHON_EXAMPLES=ON \
-DPYTHON3_EXECUTABLE=$(which python3) \
-DPYTHON_INCLUDE_DIR=$(python3 -c "from distutils.sysconfig import get_python_inc; print(get_python_inc())") \
-DPYTHON_INCLUDE_DIR2=$(python3 -c "from os.path import dirname; from distutils.sysconfig import get_config_h_filename; print(dirname(get_config_h_filename()))") \
-DPYTHON_LIBRARY=$(python3 -c "from distutils.sysconfig import get_config_var;from os.path import dirname,join ; print(join(dirname(get_config_var('LIBPC')),get_config_var('LDLIBRARY')))") \
-DPYTHON3_NUMPY_INCLUDE_DIRS=$(python3 -c "import numpy; print(numpy.get_include())") \
-DPYTHON3_PACKAGES_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib())") \
-DBUILD_opencv_face=ON \
-DBUILD_opencv_dnn=ON ..
$ make -j3
$ sudo make install

顔検出と顔認識

当初、以下のモデルを使用していました。

顔検出 : OpenCV Cascade classifier
顔認識 : keras_facenet

しかし、OpenCV Cascade Classifierでは顔の誤検出が多く見られたため、使用を断念。
また、keras_facenetはJetsonNano上で動作させたところ、モデルの読み込み中にOOM発生。JetsonNanoのリソースでは十分に動かせないようです。
顔検出にMTCNNも考えましたが、多分リソース的に厳しいだろうと思い、こちらも断念。
さてどうするか、と思っていたところ、OpenCVの比較的新しいバージョンではDNNの使用や軽量な顔認識モデルの使用ができるとわかり、結果的にこちらを使用しました。

顔検出 : Face detection using OpenCV and Deep Neural Network
顔認識 : FisherFaceRecognizer

顔検出

ResNetを使用した物体検出です。
OpenCV DNNライブラリを使用して、モデル(caffemodel, prototxt)を読み込み、使用します。モデルはこちらからダウンロードして使用しました。
使用方法は簡単です。モデルを読み込んで、detect関数を呼ぶだけ。

sample.py

import cv2

detector = cv2.dnn_DetectionModel(
    "./assets/model/res10_300x300_ssd_iter_140000.caffemodel", "./assets/model/deploy.prototxt"
)
detector.setInputSize(300, 300)
detector.setInputMean((104.0, 177.0, 123.0))
class_ids, confidences, boxes = detector.detect(image)

顔認識

OpenCV faceライブラリのFisherFaceRecognizerを使用しました。
とりあえず、いくつかの画像で学習が必要です。私は自分の画像22枚と、ネットからクローリングして集めた有名人２名の画像を各10枚程度を用意しました。
FisherFaceRecognizerから返されるlabelはint型ですので、識別のためにEnumを定義しておきます。confidenceは0に近いほど学習結果と近いことを表します。

from enum import Enum

class FaceLabel(Enum):
    abe_hiroshi = 0 # 阿部 寛
    hirose_suzu = 1 # 広瀬すず
    me = 2          # 私

sample.py

import cv2

recognizer = cv2.face_FisherFaceRecognizer.create()
label, confidence = recognizer.predict(image)

Githubのコードでは、assets/images/*.jpg の画像ファイルから顔認識モデルの作成を行っています。画像ファイル名のフォーマットは face_{label}_{num}.jpgです。

Follow meロジック

動作シーケンス

カメラからframeを取得します
frameを入力し、顔検出結果を取得します(face_recognizer.detect())
顔が見つかった場合と見つからなかった場合に分岐します
顔検知が毎フレーム完璧に動作するとは限らない（検出漏れがある）ので顔が見つからなくても一定回数(ALLOW_NO_FACES)は次のフレームで顔検知を試します
no_face_counter += 1 します
顔が見つからない状況が一定回数続いた場合には、no_face_counter=0, follow_me_mode(後述)=Falseとします
キャタピラを小さく右回転し、異なる方向で顔検知を行います
顔検知で顔が見つかった場合には、見つかった顔の分類を行います(face_recognizer.recognize())
顔認識結果が"me"と判定されれば、キャタピラはfollow_me動作します
キャタピラはfollow_me動作します(follow_me_mode=Trueにします)

顔のBBOXの面積がFACE_AREA_MAXを超えている場合
* その場で一回転します
* follow_me_mode = Falseにします
顔が画面の中央に認識されている場合
* 前進します
顔が画面の右側に認識されている場合
* わずかに左回転します
顔が画面の左側に認識されている場合
* わずかに右回転します

キャタピラを小さくに右回転し、異なる方向で顔検知を行います

follow_meで一度follow_me_modeに入ると、できるだけ検知されている顔を追うように動きます。

Jetson Nano起動時に動作させる

起動時に自動的に動作するように、systemdのserviceとして登録します。

/usr/local/bin/FolloMeTankフォルダを作成し、GitHubのsrc以下をコピーします。
その後、systemd serviceの設定ファイルを書きます。

/etc/systemd/system/follow_me_tank.service

[Unit]
Description=FollowMeTank Application
Documentation=https://github.com/kanlkan/FollowMeTank

[Service]
Type=simple
ExecStart=/usr/local/bin/FollowMeTank/startup.sh
restart=always
CPUSchedulingPolicy=fifo
CPUSchedulingPriority=51

[Install]
WantedBy=multi-user.target

serviceとして認識されているかを確認

$ sudo systemctl list-unit-files --type=service |grep follow_me_tank
follow_me_tank.service                     disabled

enableする

$ sudo systemctl enable follow_me_tank

あとは再起動するか、sudo systemctl start follow_me_tank で動作します。
今回はHWの動作を安定させるため、CPU優先度をかなり高くしました。このため他のserviceに影響が出ていたと思われます。実際、ssh接続に完了までに時間がかかるようになりました。

完成

外観

ワイヤリングが汚いですが、コードなどが履帯に触れないようにしています。また、ブレッドボード上のレギュレータは熱くなることもありますので、導線につけたタグ（メンディングテープ）などが触れないように注意します。

動作動画

動作開始時にその場で1回転、私のところまで来たところ(顔のBBOXの面積が設定値以上になったら)で1回転します。
動作開始後の1回転のあと、少しずつ回転しながら私の顔を探して見つけ、ジリジリと自分のほうに近づきます。しかし、近づきすぎたため、いったん顔を見失って再度Face Searchに移行。その後、カメラが私を正面に捉えたところで1回転しています。

今回の製作でイマイチなところがあります。
私の使用したモバイルバッテリーには、一定以上の電流が流れないと、電源供給がオートでOFFになる機能が入っています。モータの動作が行われないと、レギュレータへの給電が一定時間でOFFになってしまうのです…
レギュレータへの給電確認のため、LEDランプをつけて確認しながら動作させて対応していましたが、給電がOFFになったら、バッテリーの物理スイッチを押す必要があるので少々面倒でした。常時給電されるような機構を回路に盛り込んだほうが良かったと思います。

参考資料

Jetson Nano J41 Pinヘッダ

OpenCV公式ドキュメント(4.5.2)

systemd service登録

systemd service優先度