ロケット自動追尾カメラを作ってみた

ソフトウェア開発

ハードができたら、次はソフトである。

ロケットの検出には、オープンソースの画像処理ライブラリ「OpenCV」を利用する。OpenCVには、単純な画像処理だけでなく、構造解析やパターン認識のような高度な機能も含まれている。こういった機能を利用すれば、画像認識に詳しくなくても、比較的簡単に画像認識アプリが作れるので非常に便利だ。

プログラミング言語は、OpenCVさえ動けば何でも良いのだが、今回は「Python」を選んだ。筆者も若い頃はコンパイル言語を好んで使っていたのだが、なんと言ってもスクリプト言語はとにかくトライ&エラーが楽だ。Pythonを使った経験は無かったものの、参考書籍を買って文法を覚えてしまえばとりあえずなんとかなる。

画像の中からロケットの位置を検出するために、今回実装した処理の流れは以下のようになる。これ以外にも、様々なアルゴリズムが考えられるが、なるべくシンプルになるように、ロケットの噴射炎を追跡する方式を採用した。

1. 必要なのは「明るさ」だけなので、入力画像をグレースケール化
2. ガウスフィルタで平滑化し、ノイズを除去
3. ある閾値で2値化する
4. 白い部分(=噴射炎)の輪郭を抽出
5. データを扱いやすくするために、輪郭を内包する矩形を計算
6. もし複数検出されていた場合、サイズが最大のものを噴射炎と判断
7. その矩形の座標を元に、サーボを動かす指示を出す
8. 以上を繰り返す

上記のうち、処理3で使う閾値はスライドバーを用意して、変更できるようにしてある。噴射炎のみを抽出させるため、なるべく値は高くしたいが、高すぎると検出できなくなる恐れがある。かといって必要以上に低いと、周辺の雲まで検出してしまう。ロケットの打ち上げの場合、本番の噴射炎で事前にテストできないのが難しいところだ。

また処理7では、サーボを動かせる方向に制約を設けた。エンジンを点火してもすぐにロケットは動かないため、打ち上げ直後の3秒までは雲台を固定。その後しばらくはほぼ垂直に上昇するので、10秒までは上にのみ動かし、10秒以降は右側にも解放する。こういった制限を付けることで、誤検出があった時でも、変な方向に動くのを避けようとしている。

Pythonのソースコードはこちら。

# coding: UTF-8
import cv2
import serial
import datetime

#トラックバー処理
def onTrackbarT(position):
    global threshold
    threshold = position

def onTrackbarH(position):
    global hour
    hour = position

def onTrackbarM(position):
    global min
    min = position

def onTrackbarS(position):
    global sec
    sec = position

#サーボ駆動
def servo_move(ser, pos1, pos2):
    str1 = str(pos1)
    str2 = str(pos2)

    ser.write(str1.zfill(5)+str2.zfill(5))

#メイン関数
def main():
    #パラメータ初期化
    global threshold, hour, min, sec
    threshold = 232
    hour, min, sec = 15, 50, 0

    #サーボ移動量(ズーム無しでは10、ズーム有りでは5に)
    step = 5

    try:
        ser = serial.Serial(6, 115200) #COM7    
    except:
        print(u"シリアルポートがオープンできません")
        exit()

    cap = cv2.VideoCapture(1)
    cap.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 640)
    cap.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT, 480)

    cv2.namedWindow("View")
    cv2.createTrackbar("Threshold", "View", threshold, 255, onTrackbarT)
    cv2.createTrackbar("hour", "View", hour, 23, onTrackbarH)
    cv2.createTrackbar("min", "View", min, 59, onTrackbarM)
    cv2.createTrackbar("sec", "View", sec, 59, onTrackbarS)

    #サーボ初期位置
    posx = 7500
    posy = 10167
    servo_move(ser, posx, posy)

    while(True):
        ret, img = cap.read()
        #グレースケール
        img_g = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        #ガウスフィルタ
        img_g = cv2.GaussianBlur(img_g, (15,15), 0)
        #2値化
        ret, img_g = cv2.threshold(img_g, threshold, 255, cv2.THRESH_BINARY)
        #カラー化
        img_out = cv2.cvtColor(img_g, cv2.COLOR_GRAY2BGR)

        #輪郭抽出
        contours, hierarchy = cv2.findContours(img_g, 1, 2)

        #矩形を求める
        mw, mh = 0, 0
        flg_detect = 0
        for cont in contours:
            x,y,w,h = cv2.boundingRect(cont)
            cv2.rectangle(img_out, (x, y), (x+w, y+h), (0, 255, 0), 1)
            if mw*mh < w*h:
                mw, mh, mx, my = w, h, x, y
                flg_detect = 1

        #最大面積の矩形を赤で
        if flg_detect == 1:
            cv2.rectangle(img_out, (mx, my), (mx+mw, my+mh), (0, 50, 255), 1)

        #現在時刻と起動時刻の比較
        t_start = datetime.time(hour, min, sec)
        t_lock = datetime.time(hour, min, sec+2)
        t_up = datetime.time(hour, min, sec+10)
        t_now = datetime.datetime.now()

        if t_start > t_now.time():
            #待機
            print("wait")
        elif t_lock > t_now.time():
            #3秒までは固定
            print("lock")
        elif t_up > t_now.time():
            #10秒までは上昇のみ
            print("go up")
            if flg_detect == 1:
                if my < 240:
                    posy -= step

                servo_move(ser, posx, posy)
        else:
            #10秒以降は上昇+右
            print("go up and right")
            if flg_detect == 1:
                if mx+mw > 320:
                    posx += step

                if my < 240:
                    posy -= step

                servo_move(ser, posx, posy)

        #ウィンドウ表示
        img_view = cv2.hconcat([img, img_out])
        cv2.imshow("View",img_view)

        if cv2.waitKey(10) > 0:
            break

    #終了処理
    cap.release()
    cv2.destroyAllWindows()

#メイン関数実行
main()