Raspberry Pi Zero + Edge TPU搭載のタミヤ自走戦車の製作

製作の動機

• ブラッドピット主演の映画「Fury」の戦車戦闘シーンを再現したい。
• 趣味のディスプレイ用プラモデルをraspberry piで動かしたい。
• ラジコン、電子回路等は未経験だけど、加工は得意なのでなんとかなりそう。
• GoogleのEdge TPUで物体検出すれば砲塔回転方向を割り出せるはず。
• 車体はとりあえずライントレースでまずは作ってみよう。

貫いたこだわり

• スペースに余裕のある1/16スケールは高価でかつ車種も少ないので、安価で車種も豊富だけどスペースに全く余裕のない1/35スケールでなんとかした。
• 今後のリアルなスケールモデル製作（塗装、ディティールアップ等）に繋げるために、Edge TPUのUSBケーブルの一部とカメラレンズ以外は全て車体内部および車体底面に配置し、「いかにも手作りラズパイマシン」な外観を徹底的に回避。
• 外観を損ねないために、航続距離を犠牲にしてでも絶対に単4電池の分散配置で動かす。
• 安全面を考慮してリチウムイオンバッテリーは使わない。

動画では、とりあえずデモとしてマウスを認識するかを見ています。
(動画中での砲塔上面の電池はただの重りで、現在はとれています。)
物体検出には、バナナロボのコードを一部改変させていただいたものを利用しています。
project-banana-robo
ライントレースは快調ですが、砲塔の指向先の精度が...。

コンセント電源で固定した戦車で視野をディスプレイ表示（バナナロボのコードのデモ）

紆余曲折の末、とりあえず1ヶ月でそれなりにはなったかな、と。今後はさらにEdge TPUのモデルを再学習し、敵のタイガー戦車を認識させ、クロマキー合成で特撮するのが究極的な目標です。

先は長い...。

外観

砲塔上部の黒い突起物がスパイカメラのレンズ部分です。

カメラはネジとナットでとりあえず留めています。

砲塔内部には、サーボモーターの動力を噛み合わせるためにコの字型に合わせたプラ板と、釣具店に売っている重りを適宜配置。

お手製の歯車を、

車体の砲塔リングと砲塔回転用サーボモーターに合わせます。

天板を外した中身の配置は以下の通り。

• 車体前面に収まるように加工したギアボックス
• ミニ四駆用の馬力のあるタイプの走行モーター（トルクチューン2）
• その直上に、大電流を要する走行モーターを制御できるDCモータードライバー（黒色基盤）
• 更にその直上に、raspberry pi電源用に5Vに昇圧できるコンバータ（青色基盤）
• 走行モーター駆動用の単4電池を車体左右、raspberry pi直下、車体外底部に直列で計4つ
• 車体後部に見える白いブレッドボードは、左右のライントレース用赤外線センサーの配線をまとめているもので、可変抵抗器（青色）で黒のテープと白の画用紙を区別するセンサー感度を調節しています。
  

raspberry piの下には、

• オレンジ色のトルクチューンモーターの後部
• 走行モーター用の単4電池のうちの一つ
• Edge TPU本体から伸びるケーブル
• それをraspberry piに繋ぐ短い自作ケーブル
• 手前に埋もれている砲塔回転用サーボモーター
• サーボモーターの後ろに、ライントレース用の電源配線をまとめているブレッドボード
  が格納されています。
  

車体底面に赤外線センサー、電池、Edge TPUを配置。車体の左右に見える小さなボックスのようなものがライントレース用の赤外線センサーです。車体前面の単4電池2つがraspberry pi本体への電源供給用で、これでEdge TPUと砲塔回転用サーボモーターへの電流もまかないます。

電子回路全体図

模式的にFritzingで作成した回路図です。
電子回路もFritzingも初めてなので、配線には間違いや無駄があるかもしれません。
実際にはブレッドボードは車体に格納するために切り詰めています。
可変抵抗は500kΩ前後に調整することで、ラインの白黒を判別できました。
なお、モータードライバーの該当アイコンがFritzingに見つからなかったので、この模式図では代用品を利用しています。

使用したパーツ（主要部品のみ）

メイン

ブラピ搭乗のM4A3E8戦車
raspberry pi zero wh スターターキット
PowerBoost 1000 Basic - 5V USB Boost（昇圧コンバータ）

車体制御系

トルクチューン2モーター
ツインモーターギアボックス
Cytron 4V～16V DCモータードライバ（2チャンネル、連続3A）
赤外線センサー（フォトリフレクタ）
100Ω抵抗
可変抵抗器（1MΩ）
電圧・電流計測用のテスタ

砲塔制御系

サーボモーター(SG90)
Raspberry Pi Zero用スパイカメラ
Google Coral Edge TPU USB Accelerator
20cmのUSB type A - type C 通信ケーブル（型番は失念）
USB(microB)コネクタ
USB2.0コネクタ
シールドスリムロボットケーブル
ダイソーのグルーガンとグルースティック

その他

秋月電子通商、千石電商、Amazon、SWITCH SCIENCE、タミヤには大変お世話になりました。上記以外にも数多の電子パーツ、ホビー用品、工具等を用いています。

制御の流れ

1. ライントレースの要領で走行用DCモーターを制御
2. 走行中に砲塔上面のカメラとEdge TPUで対象物を物体検出
3. 検出した対象物の方向を計算
4. 2.の計算の間に車体が進んだ記録を元に、対象物の方向のズレを粗く予測
5. 3.と4.を加味した方向に砲塔旋回用サーボモーターを制御
6. 1.と並行して2.から5.の繰り返し

コード

物体検出には、バナナロボのコードを一部改変させていただいたものを利用しています。
project-banana-robo
冒頭のGIF動画の通り、砲塔の回転先の精度が甘く、まだまだ改良の余地があります。
物体検出には2-3s/frameほどかかるので、その間の車体の動きを"forward", "right", "left"の変数に記録しておき、その補正も加味して砲塔を動かそうとしていますがなかなか...。
何か良い案がありましたらお願いします。

concurrent_motor.py

from edgetpu.detection.engine import DetectionEngine
from PIL import Image
from PIL import ImageDraw
from PIL import ImageFont
import time, io, picamera, pigpio, threading
import numpy as np
import wiringpi as pi

forward = 0
right = 0
left = 0

# https://github.com/waveform80/picamera/issues/383
def _monkey_patch_picamera():
    original_send_buffer = picamera.mmalobj.MMALPortPool.send_buffer

    def silent_send_buffer(zelf, *args, **kwargs):
        try:
            original_send_buffer(zelf, *args, **kwargs)
        except picamera.exc.PiCameraMMALError as error:
            if error.status != 14:
                raise error

    picamera.mmalobj.MMALPortPool.send_buffer = silent_send_buffer


# Read labels.txt file provided by Coral website
def _read_label_file(file_path):
    with open(file_path, 'r', encoding="utf-8") as f:
        lines = f.readlines()
    ret = {}
    for line in lines:
        pair = line.strip().split(maxsplit=1)
        ret[int(pair[0])] = pair[1].strip()
    return ret


# Main loop
def detect_motor():
    global forward, right, left
    model_filename = "mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite"
    label_filename = "coco_labels.txt"
    engine = DetectionEngine(model_filename)
    labels = _read_label_file(label_filename)
    CAMERA_WIDTH = 640
    CAMERA_HEIGHT = 480

    fnt = ImageFont.load_default()

    # set up for servo motor
    servo_pin = 18
    pigp = pigpio.pi()
    pulsewidth = 1450
    pigp.set_servo_pulsewidth(servo_pin, pulsewidth)

    # To view preview on VNC,
    # https://raspberrypi.stackexchange.com/a/74390
    with picamera.PiCamera() as camera:
        _monkey_patch_picamera()
        camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT)
        camera.framerate = 15
        camera.rotation = 180
        _, width, height, channels = engine.get_input_tensor_shape()
        print("{}, {}".format(width, height))
        overlay_renderer = None
        camera.vflip = True
        camera.hflip = True
        camera.start_preview()
        try:
            stream = io.BytesIO()
            for foo in camera.capture_continuous(stream,
                                                 format='rgb',
                                                 use_video_port=True):
                # Make Image object from camera stream
                stream.truncate()
                stream.seek(0)
                input = np.frombuffer(stream.getvalue(), dtype=np.uint8)
                input = input.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 3))
                image = Image.fromarray(input)
                # image.save("out.jpg")

                # Make overlay image plane
                img = Image.new('RGBA',
                                (CAMERA_WIDTH, CAMERA_HEIGHT),
                                (255, 0, 0, 0))
                draw = ImageDraw.Draw(img)
                draw.line((CAMERA_WIDTH//2, 0, CAMERA_WIDTH//2, CAMERA_HEIGHT), width=1)
                draw.line((3*CAMERA_WIDTH//8, 0, 3*CAMERA_WIDTH//8, CAMERA_HEIGHT), width=1)
                draw.line((5*CAMERA_WIDTH//8, 0, 5*CAMERA_WIDTH//8, CAMERA_HEIGHT), width=1)

                # Run detection
                forward = 0
                right = 0
                left = 0
                start_ms = time.time()
                results = engine.DetectWithImage(image,
                                                 threshold=0.2, top_k=5)
                elapsed_ms = (time.time() - start_ms)*1000.0
                obj = None
                if results:
                    obj = next((x for x in results if labels[x.label_id] == "mouse"), None)

                if obj:
                    box = obj.bounding_box.flatten().tolist()
                    box[0] *= CAMERA_WIDTH
                    box[1] *= CAMERA_HEIGHT
                    box[2] *= CAMERA_WIDTH
                    box[3] *= CAMERA_HEIGHT
                    draw.rectangle(box, outline='red')
                    draw.text((box[0], box[1]-10), labels[obj.label_id],
                              font=fnt, fill="red")
                    obj_width = box[2] - box[0]
                    obj_center = box[0] + obj_width // 2
                    draw.point((obj_center, box[1] + (box[3] - box[1])//2))
                    # if object on the right side of the sight
                    if (obj_center - CAMERA_WIDTH // 2) > 0:
                        print("TURN R")
                        mv_deg = 54 * ((obj_center - CAMERA_WIDTH // 2) / CAMERA_WIDTH)
                        # if gunport in the right direction of the vehicle head
                        if pulsewidth <= 1450:
                            mv_deg += int(54 * (forward / 105 + (right-left)/35) * np.sin(((1450-pulsewidth)/950) * (np.pi/2)))
                        # if gunport in the left direction of the vehicle head
                        elif pulsewidth > 1450:
                            mv_deg += int(54 * (-forward / 105 + (right-left)/35) * np.sin(((pulsewidth-1450)/900) * (np.pi/2)))
                        pulsewidth -= mv_deg * 10
                        print("mv_deg {}, pulsewidth {}".format(mv_deg, pulsewidth))
                        pigp.set_servo_pulsewidth(servo_pin, pulsewidth)
                    # if object on the left side of the sight
                    elif (obj_center - CAMERA_WIDTH // 2) < 0:
                        print("TURN L")
                        mv_deg = 54 * ((obj_center - CAMERA_WIDTH // 2) / CAMERA_WIDTH)
                        # if gunport in the right direction of the vehicle head
                        if pulsewidth <= 1450:
                            mv_deg += int(54 * (forward / 105 + (right-left)/35) * np.sin(((1450-pulsewidth)/950) * (np.pi/2)))
                        # if gunport in the left direction of the vehicle head
                        elif pulsewidth > 1450:
                            mv_deg += int(54 * (-forward / 105 + (right-left)/35) * np.sin(((pulsewidth-1450)/900) * (np.pi/2)))
                        pulsewidth -= mv_deg * 10
                        print("mv_deg {}, pulsewidth {}".format(mv_deg, pulsewidth))
                        pigp.set_servo_pulsewidth(servo_pin, pulsewidth)
                    camera.annotate_text = "{0:.2f}ms".format(elapsed_ms)
                
                if not overlay_renderer:
                    overlay_renderer = camera.add_overlay(
                        img.tobytes(),
                        size=(CAMERA_WIDTH, CAMERA_HEIGHT), layer=4, alpha=255)
                else:
                    overlay_renderer.update(img.tobytes())
        finally:
            if overlay_renderer:
                camera.remove_overlay(overlay_renderer)
            camera.stop_preview()

def sensor_motor():
    # set for IR sensors
    PIR_PIN1 = 23
    PIR_PIN2 = 24

    pi.wiringPiSetupGpio()
    pi.pinMode(PIR_PIN1, pi.INPUT)
    pi.pinMode(PIR_PIN2, pi.INPUT)

    # set for driving motors
    motor_right_pin1 = 16
    motor_right_pin2 = 20
    motor_left_pin1 = 5
    motor_left_pin2 = 6

    pi.pinMode(motor_right_pin1, pi.OUTPUT)
    pi.pinMode(motor_right_pin2, pi.OUTPUT)
    pi.pinMode(motor_left_pin1, pi.OUTPUT)
    pi.pinMode(motor_left_pin2, pi.OUTPUT)

    # set the speed ranges for motors
    pi.softPwmCreate(motor_right_pin1, 0, 100)
    pi.softPwmCreate(motor_right_pin2, 0, 100)
    pi.softPwmCreate(motor_left_pin1, 0, 100)
    pi.softPwmCreate(motor_left_pin2, 0, 100)

    # get the motors speed 0
    pi.softPwmWrite(motor_right_pin1, 0)
    pi.softPwmWrite(motor_right_pin2, 0)
    pi.softPwmWrite(motor_left_pin1, 0)
    pi.softPwmWrite(motor_left_pin2, 0)

    # raise up the speed
    speed = 0
    while ( speed <= 35 ):
        pi.softPwmWrite(motor_right_pin1, speed)
        pi.softPwmWrite(motor_right_pin2, 0)
        pi.softPwmWrite(motor_left_pin1, speed)
        pi.softPwmWrite(motor_left_pin2, 0)
        
        time.sleep(0.1)
        speed += 5

    # Black line trace
    global forward, right, left
    while True:
        if (pi.digitalRead(PIR_PIN1) == pi.HIGH) & (pi.digitalRead(PIR_PIN2) == pi.HIGH):
            pi.softPwmWrite(motor_right_pin1, speed)
            pi.softPwmWrite(motor_right_pin2, 0)
            pi.softPwmWrite(motor_left_pin1, speed)
            pi.softPwmWrite(motor_left_pin2, 0)
            forward += 1
        elif (pi.digitalRead(PIR_PIN1) == pi.HIGH) & (pi.digitalRead(PIR_PIN2) == pi.LOW):
            pi.softPwmWrite(motor_right_pin1, speed)
            pi.softPwmWrite(motor_right_pin2, 0)
            pi.softPwmWrite(motor_left_pin1, 100)
            pi.softPwmWrite(motor_left_pin2, 100)
            right += 1
        elif (pi.digitalRead(PIR_PIN1) == pi.LOW) & (pi.digitalRead(PIR_PIN2) == pi.HIGH):
            pi.softPwmWrite(motor_right_pin1, 100)
            pi.softPwmWrite(motor_right_pin2, 100)
            pi.softPwmWrite(motor_left_pin1, speed)
            pi.softPwmWrite(motor_left_pin2, 0)
            left += 1
        else:
            while ( speed >= 5 ):
                pi.softPwmWrite(motor_right_pin1, speed)
                pi.softPwmWrite(motor_right_pin2, 0)
                pi.softPwmWrite(motor_left_pin1, speed)
                pi.softPwmWrite(motor_left_pin2, 0)

                time.sleep(0.1)
                speed -= 5
            break

        time.sleep(0.1)

    # Get back
    time.sleep(3)

    while ( speed <= 35 ):
        pi.softPwmWrite(motor_right_pin1, 0)
        pi.softPwmWrite(motor_right_pin2, speed)
        pi.softPwmWrite(motor_left_pin1, 0)
        pi.softPwmWrite(motor_left_pin2, speed)
        
        time.sleep(0.1)
        speed += 5
        
    pi.softPwmWrite(motor_right_pin1, 0)
    pi.softPwmWrite(motor_right_pin2, speed)
    pi.softPwmWrite(motor_left_pin1, 0)
    pi.softPwmWrite(motor_left_pin2, speed)
    time.sleep(2)

    # Stop

    while ( speed >= 5 ):
        pi.softPwmWrite(motor_right_pin1, 0)
        pi.softPwmWrite(motor_right_pin2, speed)
        pi.softPwmWrite(motor_left_pin1, 0)
        pi.softPwmWrite(motor_left_pin2, speed)
        
        time.sleep(0.1)
        speed -= 5

    pi.softPwmWrite(motor_right_pin1, 100)
    pi.softPwmWrite(motor_right_pin2, 100)
    pi.softPwmWrite(motor_left_pin1, 100)
    pi.softPwmWrite(motor_left_pin2, 100)
    time.sleep(3)


if __name__ == "__main__":
    thread_1 = threading.Thread(target=detect_motor)
    thread_1.start()
    
    time.sleep(20)
    
    thread_2 = threading.Thread(target=sensor_motor)
    thread_2.start()

直近の今後の課題

• 砲塔回転の精度向上
• 目的の対象物の学習データ生成（webスクレイピングまたは観賞用回転ディスプレイを利用）
• 上記データを用いたモデルの再学習

作ったのは米軍の戦車なので、色々な独軍の戦車を認識するモデルをできれば作りたいなと思っています。ただ、スクレイピングで十分にデータが集まるかどうか。
砲塔天蓋のカメラからは自車の砲身が写るので、そのカメラで回転ディスプレイ上の対象物の連続コマ撮りの方がいいのか。でもそれで強いモデルができるのか。背景はどうしよ。などなど課題が山積していますが地道に続けていきます。

参考文献・リンク

電子部品ごとの制御を学べる! Raspberry Pi 電子工作 実践講座 改訂第2版
1/35 M1スーパーシャーマン RC化 ① : 朴念仁の艱苦
Raspberry Pi Zero用の小型電源とUSB WiFiモジュール用USBコネクタを自作する
project-banana-robo