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

テクノロジー

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

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

実行環境

・OS Ubuntu 16.04

(頂いたコメントやこちらの記事によると,Windowsでもできるみたいです.)

・言語 python3.7.4

・ドローン Raze Tech/DJI Tello

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

どんなプログラムか

コードは

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

です。

このプログラムでは、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」です。

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

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

です。

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

ATS0000/drone_pursuit
Contribute to ATS0000/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/ATS0000/drone_pursuitの「TelloPy」というファイルをどこかのディレクトリに入れておきます.

TelloのWi-FiをPCで受信し,「Tellopy」ディレクトリに入り、ターミナルで

python -m tellopy.examples.video_effect917_velo

で実行します。

直接「python video_effect917_velo.py」で実行してしまうと,飛んだ後に一部の機能が使えなくなります.ご注意ください.

Telloが飛んだ後,動画ウインドウが立ち上がります.

自動追尾モードにする

キーボードの『R』を押すと、最初に現れた動画ウインドウとは別に,ROIウインドウが立ち上がります。

ROIウインドウで人の顔の範囲をマウスでドラックして選択し、『Enter』キーを押すと、自動追尾モードになります.

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

飛行を終了する

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

ただし,Qを押すなどのキー入力操作は,ターミナルではなく,動画ウインドウを選択した状態で押してください.ターミナルを選択した状態では,効果がありません.

追記(2020/8/1)

このコードでは,キーボード操作でTelloを上下左右前後に動かすことができません.

実行中にTelloをキー操作で動かすことができれば,人が画面外に行ってしまっても,調整することができます.

そのようなコードにするには,Whileループの中(例えば,

if key == ord('q'):
    print('Q!')
    break

の後など)に,

if key == ord('u'):
     drone.up(3)

のようなものを書けばよいです.(※ただし,ROIで選択したBOXは動かないため,BOXも動かすように書く必要があると思います.)

実際の動画

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

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

コード解説

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

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で人の顔を選択する部分です。Whileループ実行中にキーボードの『R』を押すと、ROIが立ち上がります。

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

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

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

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

バッテリーを複数用意しておくと充電の待ちの時間が省け,スムーズに遊べます.

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

今後は、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で遊んでみた

話題の映画やアニメを見るならU-NEXTがおすすめです!
今なら31日間無料で多数の映画見放題!!
無料体験中に忘れずに解約すればお金はかかりません!
キャンペーン終了前にお早めの登録を!!
※筆者も利用しました( ´∀` )

テクノロジー
理系リアルタイムをフォローする
理系リアルタイム

コメント

  1. 田島 唯 より:

    コメント失礼いたします.
    ご質問よろしいでしょうか?

    私もこのプログラミング通りに実行したのですが,ROI画面で顔を囲うところまでは動作するのですが,その先がうまくいきません.

    よければアドバイスいただけませんか?
    よろしくお願いいたします.

    • ATS より:

      記事をご覧いただき、ありがとうございました。返信が遅くなってしまい、申し訳ございません。

      もうやられていないか、解決してしまったかもしれませんが、どのような症状でしょうか?RIOの画面が暗くなり、画面が落ちるなどの症状なら経験があるので、具体的なアドバイスが可能かと思います。

      改めて、記事をご覧いただき、ありがとうございました。

  2. たかし より:

    記事を大変興味深く面白かったです。
    同じようなことをwindows10やraspberrypiでもできますか???

    • ATS より:

      記事をご覧いただきありがとうございます。

      ラズパイでもWindowsでも、マシン内でUbuntu OSを用意するればそのまま使えるとおもいます!

      また、やってみたことはないので使い方はわかりませんが、Telloはwindowsでもラズパイでも動くと思うので、windowsなどに合わせてプログラムを書けば同じことは可能だと思います。使っているモジュールは、Ubuntu限定のものではないので!

  3. めたまた より:

    プログラム初心者です。
    windows環境で問題なく動いていたのですが、ある日プログラムを動かしてRを押してもROIが立ち上がらないようになりました。
    Rを押すとターミナル上では while decoding error が表示されます。
    解決策はありますでしょうか?

    • 理系リアルタイム より:

      実行してくださり、ありがとうございます。

      申し訳ございませんが、windowsで動かしたことがないので分かりません。いままで動いていたということは、どこかのソフトウェアの更新が影響したかもしれませんね。

      お力になれず、すみません。

      また、本記事を読んでくださってとても嬉しいです。

      • めたまた より:

        解決しました!!!
        ただ単にCapsLock有効になっていただけでした…お騒がせしました。
        気づかず肘があたったのかもしれないです笑

        楽しい記事ありがとうございました!
        まだ読んでいない記事も読んでみます!

        • 理系リアルタイム より:

          解決してよかったです!!

          Windowsでもできることが分かり,こちらとしても勉強になりました!!

          ありがとうございました!

    • 吉永 より:

      コメント失礼します。
      windows環境で実行したいのですがやり方を教えてもらえないでしょうか。

      • 理系リアルタイム より:

        Windows環境で実行したことがないので分かりませんが,例えば,仮想のUbuntu環境上で実行するとできるのではないでしょうか.

      • 理系リアルタイム より:

        コメントありがとうございます!

        https://qiita.com/beholder/items/158e2d29364dff392f3b

        では,windowsでtelloを動かしているようです.この記事のサンプルプログラムを本記事のような形に書き直せば,実行できるかと思います.

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