トイドローンTelloで顔認識・トラッキング、自動追尾してみた(動画あり)【tellopy, Camshift使用】

テクノロジー

DJI社のトイドローン『Tello』を使って、何かしようということで、とりあえず人の顔を認識して自動でTelloが追いかけるようにしました。

(注意!)再生すると音が出ます!!

実行環境

・OS Ubuntu 16.04

・言語 python3.7.4

・ドローン Raze Tech/DJI Tello

Telloについては、以下の記事をご覧ください。

created by Rinker
¥12,980 (2019/10/15 17:10:32時点 楽天市場調べ-詳細)

どんなプログラムか

コードは

https://github.com/haizuka0000/drone_pursuit/blob/master/TelloPy/tellopy/examples/video_effect917_velo.py

です。

このプログラムでは、Telloが人の顔と一定の距離を保つように自動追尾します。

① PCとTelloをWifiで繋げた状態で実行すると、Telloが離陸します。

② カメラが自動で立ち上がり、PCに動画が表示されます。

③ キーボードの『R』を押すと、ROI(region of images)が立ち上がります(静止画)。

④ ROI内で人の顔の範囲をマウスでドラックして選択し、『Enter』キーを押すと、自動追尾モードになります(ROI画面は消えないので、最初に立ち上がった元の動画(Original)と被っている場合があります。その場合はROIをどかしましょう)。

操作イメージ
左のウインドウがOriginalで、右のウインドウがROI。

⑤ Original画面の緑色の枠がROIで指定した枠の位置(画面上で固定)で、青色の枠がトラッキングしている枠です。この2つの枠が一致するようにTelloが自ら動くことで、追尾します。

⑥ 飛行中は、Original画面をクリックした後(操作ウインドウがOriginal画面)であれば、『Q』キーでいつでも着陸できます。

他の記事でも同じようなものはありましたが、本コードでは人の動きの速さに応じてドローンの対応速度も速くするようにしました。また、追尾がうまくいくように、画面の中心付近と外側でドローンの動きを変えてあります(ゲインは各自で調整してください)。Camshiftを用いることで、トラッキング枠の大きさも変わるようにしたので、前後の動きにも対応しています。

ゲイン調整などがうまくいっていないと、トラッキングが外れたり、ドローンが暴れだしたりします。安全には十分気を付け、自己責任でよろしくお願いいたします。

コード概要

大元のコード

参考にさせていただいた大元のコードは

https://github.com/hanyazou/TelloPy/blob/develop-0.7.0/tellopy/examples/video_effect.py

の「videl_effect.py」です。

そして、今回のプログラムは

haizuka0000/drone_pursuit
Contribute to haizuka0000/drone_pursuit development by creating an account on GitHub.

です。

なお、このプログラムを実行するには

haizuka0000/drone_pursuit
Contribute to haizuka0000/drone_pursuit development by creating an account on GitHub.

の「TelloPy」というファイルをを丸ごと入れておく必要があります。

実行方法は、後述します。

インストールするもの

コードのimportを見てもらえば分かると思いますが、

・OpenCV

・av

・tellopy

などが必要です。

中でも、avのインストールにそこそこ苦戦した気がします。tellopyのインストールは、 https://github.com/hanyazou/TelloPy を参考にしてください。

当然、pythonなどもインストールしておく必要があります。

実行方法

https://github.com/haizuka0000/drone_pursuitの「TelloPy」というファイルをどこかのディレクトリに入れ、この「Tellopy」に入り、

python -m tellopy.examples.video_effect917_velo

で実行します。

実際の動画

(再生すると音が出ます)

小さくて見えにくいかもしれませんが、白いドローンが人間に合わせて移動しているのが分かるかと思います。

コード解説

コードの一部について解説します。

def tracking(drone,d,dx,dy,vx,vy,L0):

    gain_vx = 3
    gain_vy = 3

    gain_bf = 0.0022


    gain_x_1 = 0.15
    gain_y_1 = 0.1

    gain_x_2 = 0.0003
    gain_y_2 = 0.00005

   

    if d > 17:
	print('back')
        drone.set_pitch(- gain_bf*d)   
        
    if d < -17:
	print('forward')
        drone.set_pitch(- gain_bf*d)   
        

    if 5 < dx <= 150:
	print('right')
        drone.right(gain_x_1*dx + gain_vx*vx)

    if -150 <= dx < -5:
	print('left')
        drone.left(- gain_x_1*dx - gain_vx*vx)

        
    if dx > 150:
	print('right')
        drone.right(gain_x_2*dx*dx + gain_vx*vx/2)
	
    if dx < -150:
	print('left')
        drone.left(gain_x_2*dx*dx - gain_vx*vx/2)    
        
    if dy > 5:
	print('down')
        drone.down(gain_y_2*dy*dy + gain_vy*vy/2)    

    if dy < 5:
	print('up')
        drone.up(gain_y_2*dy*dy + gain_vy*vy/2) 

自動追尾モードに入った後に、この関数に従ってドローンが動きます。

ドローンが人の顔を中心付近にとらえているときはゆっくりと動き、外側に行くにつれて動きが早くなるようにしています。

最初に実行するときは、gainをこれよりも小さめにして試してみると良いかと思います。

if 0 < frame_skip:
    frame_skip = frame_skip - 1
    continue

動画が止まったり、時間遅れしないようにするため、要らないフレームは捨てる、という部分です。

メインforループ終わりの

if frame.time_base < 1.0/60:
    time_base = 1.0/60
else:
    time_base = frame.time_base
    frame_skip = int((time.time() - start_time)/time_base)

とセットです。

if ok == True:	
    x_mae = x
    y_mae = y
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) #cmsf
    dst = cv2.calcBackProject([hsv],[0],roi_hist,[0,180],1) #cmsf

    ret, track_window = cv2.CamShift(dst, track_window, term_crit) #cmsf
    (x,y,w,h) = track_window #cmsf

    S = w*h
                    
    p1 = (x, y)
    p2 = (x + w, y + h)
    cv2.rectangle(image, p1, p2, (255,0,0), 2, 1)
    p10 = (x0, y0)
    p20 = (x0 + w0, y0 + h0)
    cv2.rectangle(image, p10, p20, (0,255,0), 2, 1)

    d = round(L0 * m.sqrt(float(S) / (w0*h0))) - L0
    dx = x + w/2 - CX0
    dy = y + h/2 - CY0

    vx = x - x_mae
    vy = y - y_mae
    print("CX,CY,S,x,y,S0 =",int(x+0.5*w),int(y+0.5*h),S,x,y,w0*h0)
    print(d,dx,dy)
    print("vx,vy =",vx,vy)
		    
    tracking(drone,d,dx,dy,vx,vy,L0)

ROIで顔を選択してEnterが押された後に、毎フレームで実行される部分です。つまり、「ok == True」で自動追尾モードです。

この中でCamshiftでトラッキングしている枠の座標や枠の中心を求め、そこからドローンとの距離や枠の移動速度を計算して、tracking関数に渡しています。

key = cv2.waitKey(1)&0xff
if key == ord('q'):
    print('Q!')
    break

飛行中はいつでもキーボードの『Q』を押せばforループから抜け出し、着陸します。

if key == ord('r'):
    roi_time = time.time()
    bbox = cv2.selectROI(image, False)
    print(bbox)
    (x0,y0,w0,h0) = (int(bbox[0]),int(bbox[1]),int(bbox[2]),int(bbox[3]))

    CX0=int(x0+0.5*w0) #Center of X
    CY0=int(y0+0.5*h0)

    #camshif--ref_https://qiita.com/MuAuan/items/a6e4aace2a6c0a7cb03d-----------------------------

    track_window = (int(bbox[0]),int(bbox[1]),int(bbox[2]),int(bbox[3]))
    roi = image[y0:y0+h0, x0:x0+w0]

    hsv_roi =  cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
    img_mask = cv2.inRange(hsv_roi, numpy.array((0., 60.,32.)), numpy.array((180.,255.,255.)))

    roi_hist = cv2.calcHist([hsv_roi], [0], img_mask, [180], [0,180])
    cv2.normalize(roi_hist, roi_hist, 0, 255, cv2.NORM_MINMAX)
    term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 1 )
		    
    ret,image = cap.read()
    ok = True

    #camshif_end--------------------------------------

    x = x0
    y = y0 

ROIで人の顔を選択する部分です。forループ実行中にキーボードの『R』を押すと、ROIが立ち上がります。

ROI内で人の顔の範囲をマウスでドラックして選択し、『Enter』キーを押すと、自動追尾モードになります(ok = Tureが自動追尾モードになるという意味)。

ROI画面は消えないので、最初に立ち上がった元の動画(Original)と被っている場合があります。その場合はROIをどかしましょう。

ここで選択した枠の座標、幅は(x0, y0, w0, h0)として固定され、Original画面では緑色の枠として表示されます。

created by Rinker
¥12,980 (2019/10/15 17:10:32時点 楽天市場調べ-詳細)

ゲイン調整などがうまくいっていないと、ドローンが暴れだします。安全には十分気を付け、自己責任でよろしくお願いいたします。

分かりにくかったかもしれませんが、お役に立てればと思います。

今後は、Telloを使って、Visual SLAMをやってみようと思います。

【参考】

【Tello】トイ・ドローンで遊んでみた♪~キー制御とマジ・トラッキングでハマった編

トイドローン Tello をプログラミングで機能拡張!顔認識と自動追尾を実装してみた

https://github.com/hanyazou/TelloPy

https://github.com/hanyazou/TelloPy/blob/develop-0.7.0/tellopy/examples/video_effect.py

Meanshift と Camshift

【Python】OpenCVのMeanShiftとCamShiftによる物体の発見・追跡

Opencv: meanshift&CamShiftで遊んでみた

コメント

タイトルとURLをコピーしました