障害物(黒色)自動回避キャタピラ作ってみた。

作ったもの

ラズパイ3b+とPiカメラを使って、撮った写真をSVMでリアルタイム「前進」「右折」「左折」「後退」に分類して進むキャタピラ。

百聞は一見に如かずということでこちらをご覧ください。
YouTube: ラズパイを使った障害物回避キャタピラ試走

参考図書

カラー図解 最新 Raspberry Piで学ぶ電子工作 作って動かしてしくみがわかる (ブルーバックス)

ハードは10章参照＋Piカメラつけただけ。
ソフトは8章参照＋前処理+svm(Scikit-learn)+再学習のための画像保存

工夫点

ハード：カメラは目の前のみ撮影するようにした。
背景が映り込むと分類できなさそうなので、キャタピラの目の前のみの画像を入力。

ソフト①：svmへの入力を28x28ピクセル/写真(グレースケール)
Piカメラの撮影を64x64ピクセル(カラー)、前処理で28x28ピクセル(グレースケール)に落としている。
前処理に時間がかかると、モーターまでの伝達にタイムラグが発生するので。このくらいの解像度で遅れはほとんど気にならなかった。
ちなみに64x64(カラー)の画像はこんな感じ

28x28(グレースケール)は

ソフト②：再学習用データ集めながら走るモード作った
SavePicsを1とすれば、撮影画像を再学習用に保存しながら進む。
保存画像には「前進」「右折」「左折」「後退」のどれと判断したのかを、ファイル名にラベルを付けながら前進する。
走行後、間違っているラベルがついた画像を、人がフォルダ分けし、camera_car_preprocess.pyを動かせば、簡単に再学習可能。
だいたい200枚ｘ4分類で前処理に3分くらい、学習は一瞬で終わるため、現地での調整をしやすい。

ソフト③：後退時右向く
まっすぐ後退すると、無限ループに入るので、方向を右に向くようにしている。

回路図

コード

camera_car_preprocess.py

from PIL import Image
import numpy as np
import glob
from sklearn import svm, metrics
from sklearn.model_selection import train_test_split
import pickle

ModelName = "model_20191102_9.pickle"#As you wish.

def main():
    Path = '/home/pi/ドキュメント/camera_car/Train/'
    Left_L = glob.glob(Path + '1_Left/*.jpg')
    Right_L = glob.glob(Path + '2_Right/*.jpg')
    Center_L = glob.glob(Path + '0_Center/*.jpg')
    Back_L = glob.glob(Path + '3_Back/*.jpg')
    X_L = Preprocess(Left_L)
    Y_L = np.ones(int(len(X_L)/784))
    X_R = Preprocess(Right_L)
    Y_R = np.full(int(len(X_R)/784),2)
    X_C = Preprocess(Center_L)
    Y_C = np.zeros(int(len(X_C)/784))
    X_B = Preprocess(Back_L)
    Y_B = np.full(int(len(X_B)/784),3)

    X = np.r_[X_L, X_R, X_C, X_B]#concatinate all preprocessed pics.全前処理写真結合
    X = X.reshape([int(len(X)/784),784])#make array.行列化
    y = np.r_[Y_L, Y_R, Y_C, Y_B]#teacher data addition.教師データ付加
    print(X.shape)
    print(y.shape)
    X_train, X_test, y_train, y_test = train_test_split(X, y, test_size=0.1, random_state=42)

    clf = svm.SVC(kernel='linear')
    clf.fit(X_train, y_train)
    pre = clf.predict(X_test)    
    ac_score = metrics.accuracy_score(y_test, pre)
    print(ac_score)#print score.精度表示

    with open(ModelName, mode='wb') as fp:
        pickle.dump(clf, fp)#save model.モデル出力


def Preprocess(files):
    size = [28,28]
    array = np.empty([size[0]*size[1],0],int)
    print(array.shape)
    print(files)
    for i, file in enumerate(files):
        img = Image.open(file).convert('L')
        img = img.resize(size, Image.ANTIALIAS)
        print(img.format, img.size, img.mode,img.getextrema())
        img_arr = np.asarray(img)
        print("OD"+str(img_arr.ravel().shape))
        array = np.append(array,img_arr.ravel())
        print(array.shape)
    return array

if __name__ == '__main__':
    main()

camera_car_main.py

# -*- coding: utf-8 -*-
from picamera import PiCamera
import RPi.GPIO as GPIO
from time import sleep
from PIL import Image
import numpy as np
import os
import glob
import pickle
import shutil
import random

def Preprocess(i,SaveP):
    size = [28,28]
    array = np.empty([size[0]*size[1],0],int)
    print(array.shape)
    FullPath = glob.glob('/home/pi/ドキュメント/camera_car/Predict/*.jpg')
    print(FullPath)
    #Preprocessing
    img = Image.open(FullPath[0]).convert('L')
    img = img.resize(size, Image.ANTIALIAS)
    print(img.format, img.size, img.mode,img.getextrema())
    #Make one dimention array
    img_arr = np.asarray(img)
    print("OneDimention"+str(img_arr.ravel().shape))
    if SaveP:
        os.remove(FullPath[0])
    else:#saving pics for re-training the model.才が羽州のために写真保存
        shutil.move(FullPath[0],'/home/pi/ドキュメント/camera_car/Predict/done/%s.jpg' % i)
    return img_arr.ravel(), img


PickleName = "model_20191102_9.pickle"#indicate trained model(pickle)
SavePics = 0
with open(PickleName, mode='rb') as fp:
    clf = pickle.load(fp)

camera = PiCamera()
#camera.rotation = 180#if camera is upside down カメラが上下反対の場合
camera.resolution = (64,64)

GPIO.setmode(GPIO.BCM)
GPIO.setup(25, GPIO.OUT)
GPIO.setup(24, GPIO.OUT)
GPIO.setup(23, GPIO.OUT)
GPIO.setup(22, GPIO.OUT)

p0 = GPIO.PWM(25, 50)#RH
p1 = GPIO.PWM(24, 50)#RH
p2 = GPIO.PWM(23, 50)#LH
p3 = GPIO.PWM(22, 50)#LH

p0.start(0)
p1.start(0)
p2.start(0)
p3.start(0)
print("start moving...")
sleep(10)
i = 0
duty = 70


#At first go forward
p0.ChangeDutyCycle(20)
p1.ChangeDutyCycle(0)
p2.ChangeDutyCycle(20)
p3.ChangeDutyCycle(0)
try:
    while True:
        #Take a pic and save to indicated folder.写真取って指定フォルダに保存
        i += 1
        camera.capture('/home/pi/ドキュメント/camera_car/Predict/%s.jpg' % i)
        #preprocessing at indicated folder.指定フォルダの写真を前処理
        X_pred, img = Preprocess(i, SavePics)
        #predict 推定
        pred = clf.predict(X_pred)
        #change duty ratio.デューティー比変更
        if pred[0] == 0:#前進
            print("Forward")
            p0.ChangeDutyCycle(duty)
            p1.ChangeDutyCycle(0)
            p2.ChangeDutyCycle(duty)
            p3.ChangeDutyCycle(0)
            sleep(0.8)
        elif pred[0] == 1:#left 左折
            print("Left")
            p0.ChangeDutyCycle(duty-20)
            p1.ChangeDutyCycle(0)
            p2.ChangeDutyCycle(0)
            p3.ChangeDutyCycle(20)
            sleep(0.3)
        elif pred[0] == 2:#right 右折
            print("Right")
            p0.ChangeDutyCycle(0)
            p1.ChangeDutyCycle(20)
            p2.ChangeDutyCycle(duty-20)
            p3.ChangeDutyCycle(0)
            sleep(0.3)
        elif pred[0]  == 3:#backword 後退
            p0.ChangeDutyCycle(0)
            p1.ChangeDutyCycle(duty-10)
            p2.ChangeDutyCycle(0)
            p3.ChangeDutyCycle(duty-40)
            print("Backing...")
            sleep(1)
            print("finish backing")
        #save preprocessed pic前処理後写真を保存
        if SavePics:
            pass
        else:#collect preprocessed pics with pred number.再学習用写真（推定値付き）保存
            rand = random.randint(0,100000)
            img.save('/home/pi/ドキュメント/camera_car/Train/'
                     +str(i)+'_'+str(int(pred[0]))+
                     '_'+str(rand)+'.jpg')

except KeyboardInterrupt:
    pass

p0.stop()
p1.stop()
GPIO.cleanup()

フォルダ構成は

│ camera_car_main.py
│ camera_car_preprocess.py
│ model_20191102_8_best.pickle
│ model_20191102_9.pickle
├─Predict
│ └─done
└─Train
├─0_Center
├─1_Left
├─2_Right
└─3_Back

としてください。ちなみにpickleは2つありますが、8が一番良かったです。9は8の再学習版ですが、実走行していないので、実力未知数。

感想

ハードからロジックからいろいろ考えて実装できたので面白かったし、再学習まで考えてロジック考えられて勉強になった。
改善点としては、再学習するために、人がアノテーションしなきゃなので、大きめのディスプレイがないとその場で再学習できないのが難点かなー。