Raspberry Piで自動運転やってみる 2.自動ブレーキ

前回のつづきです。　
今回は障害物に近づくと自動で停止する機能をつくります。

画像処理の概要

カメラは一個だけなので、モーションステレオ方式で空間情報を取得します。

計算量の観点から、勾配ベースのオプティカルフローを利用してモーションステレオを行います。勾配ベースのオプティカルフロー抽出には、いわゆるアパーチャー問題があり、制約条件を追加する必要があります。典型的にはコーナー検出などでこれを回避しますが、今回は違う方法でやってみます。
主な車体の動きは前後移動と回転ですが、回転はジャイロセンサーを利用してキャンセルするとすれば、前後方向の動きのみ考えればよいことになります。また自車以外動いているものがないと仮定すれば、オプティカルフローは画像中心から放射状の方向に制約されることになります。こうすることで、コーナー検出をしなくてもオプティカルフローの計算が成り立ちます。この手法は、エッジ情報も使えて情報量が増える・キーポイント抽出にかかる計算量が削減できる、といった利点があります。
キャプチャする映像のフレームレートと解像度は精度と計算量とレイテンシに影響しますので、バランスを考えて設定します。

障害物があったら止まる

まずは簡単な機能から試してみます。
意外に感じるかもしれませんが、自車速度や焦点距離がわからなくても衝突までの時間を知ることができます。
まずは１次元で定式化してみましょう。
ピンホールカメラモデルは、画像座標$u$、焦点距離$f$、物体の大きさ$x$、物体までの距離$z$とすれば、

\frac{u}{f} = \frac{x}{z} \\

衝突までの時間 $T_{contact}$は、

T_{contact} := -\frac{z}{\frac{dz}{dt}}

$x$と$f$は時間変化しないと考えられるので、

\begin{align}
\frac{dz}{dt} &= \frac{d}{dt}(\frac{fx}{u}) \\
&=-\frac{fx}{u^2}\frac{du}{dt} \\
&=-\frac{z}{u}\frac{du}{dt}
\end{align}

一旦まとめると、

T_{contact} = \frac{u}{\frac{du}{dt}}

となり、自車速度や焦点距離によらないことがわかります。
オプティカルフローの計算式は、画像を$I=I(u,t)$として、

\frac{du}{dt} = -\frac{\frac{\partial I}{\partial t}}{\frac{\partial I}{\partial u}}

だったので、結局$T_{contact}$は、

T_{contact} = -\frac{u\frac{\partial I}{\partial u}}{\frac{\partial I}{\partial t}}

となります。これらの値は連続する２枚の画像から推定できます。
ここまで１次元で考えていますが、前記のとおりオプティカルフローが放射方向に制約されますので、２次元になった場合でも単に$x$を画像中心からの距離で置き換えれば成立します。

定式化ではピンホールカメラモデルを仮定していますので、魚眼レンズを使う場合キャリブレーションをしたほうがよいですが、ひとまずそのまま書いてみましょう。

image_processing.py

import cv2
import numpy as np
import math

class NaiveTTC(object):    
    """
    歪なしカメラを想定したTimeToContact推定アルゴリズム
    """
    def __init__(self,initial_bg,framerate=90,m0=5,r0=0.6):
        self.m0 = m0
        self.bg = initial_bg
        self.height,self.width = initial_bg.shape[:2]
        self.ex, self.ey = np.meshgrid(np.linspace(-self.width/2,self.width/2,self.width),np.linspace(-self.height/2,self.height/2,self.height))
        self.ex, self.ey = self.ex*0.5/framerate, self.ey*0.5/framerate # dx,dy,dtの補正係数を先にかけておく

        mask = (self.ex**2+self.ey**2)**0.5<r0*self.height/2
        self.ex *= mask
        self.ey *= mask

    def update(self,img):
        img = cv2.GaussianBlur(img,(5,5),2.0).astype(np.float32)

        dt = cv2.subtract(img,self.bg)
        dx = cv2.Sobel(img,cv2.CV_32F,1,0,ksize=1)
        dy = cv2.Sobel(img,cv2.CV_32F,0,1,ksize=1)

        # next background
        self.bg = img

        # remove noise
        idx0 = np.abs(dt)>self.m0

        ## calc time-to-contact
        ttc = (-(dx*self.ex+dy*self.ey)/dt)[idx0]
        mttc = np.mean(ttc)
        if math.isnan(mttc):
            mttc = 0.0
        return mttc

制御は推定した衝突までの時間が一定以下になったらモーター出力を止めるようにするだけです。

go_stop.py

import cv2
import numpy as np

from image_processing import NaiveTTC
from car_controller import CarController

if __name__=="__main__":
    WIDTH = 64
    HEIGHT = 48
    FRAMERATE = 30

    INITIAL = 0.5*FRAMERATE # スタート後に動作を継続するステップ数
    KEEP_BRAKE = 0.2*FRAMERATE # 逆方向の出力を継続するステップ数
    THRESHOLD = 0.45 # 推定TTCがこの値を下回ったらブレーキ
    R = 0.4 # 推定TTCの更新比率

    ctrl = CarController()

    cap = cv2.VideoCapture("rpicamsrc rotation=180 ! video/x-raw,format=I420,width={},height={},framerate={}/1 ! appsink".format(WIDTH,HEIGHT,FRAMERATE))

    #init
    ret,raw = cap.read()
    if not ret:
        print("cannot connect to camera ")
        exit()

    img = raw[:HEIGHT].astype(np.float32) # I420->Y
    ettc = NaiveTTC(img,FRAMERATE,m0=5,r0=0.60)

    ctrl.slottle(1.0)
    brake = 0
    mttc = 0.50
    for i in range(10*FRAMERATE):
        ret,raw = cap.read()
        img = raw[:HEIGHT].astype(np.float32) # I420->Y

        ttc = ettc.update(img)
        if ttc>0:
            mttc = min(1.0,(1-R)*mttc+R*ttc)

        print("{0:+03.6f}".format(mttc))

        if INITIAL<i and brake==0 and mttc<THRESHOLD:
            brake = 1
        if brake>0:
            if brake<KEEP_BRAKE:
                ctrl.slottle(-1.0)
                brake += 1
            else:
                break
        
    ctrl.brake()

動かしてみた様子です：

Raspberypiで自律運転実験中https://t.co/K0Ubb311zF

— Satoshi Tanaka (@stnk20) 2018年5月4日

動画ではバッチリにみえますが、実際のところはそこまででもないです。
全速で衝突する、ということはないですが、ブレーキが早すぎたり遅すぎたりすることも多いです。

つづき

障害物回避の記事につづきます。