TelloとOpenCVを組み合わせた自動制御入門（2. 制御プログラム編）
こんにちは、ドローン研究グループの飯島です。今回はTelloとOpenCVによる画像処理を組み合わせて、Telloのカメラを使った自動制御をやってみたいと思います。
やりたいこと
手に持った赤い布をTelloが追従するプログラムを作ります。用いたTelloは通常のTelloですが、Tello EDUでも動作すると思います。
実際の動きを見てもらった方が分かりやすいので、プログラム実行時の動画をご覧ください。
前提条件
先に本家のGithubよりTello Videoをクローンします。Tello Videoは、Telloのカメラで撮影しているビデオストリームを受信して、Tcl/TKで画面表示するPythonのサンプルプログラムです。今回必要なのは、このうち「tello.py」というTello制御用のプログラムです。
環境構築方法
環境構築方法は「TelloとOpenCVを組み合わせた自動制御入門（1. 環境構築編）」にて詳しく解説しているので、そちらを参考にしてください。
フォルダ構成
フォルダは以下のような構成とします。chase_telloフォルダを作成し、ファイルを配置していきます。「main.py」は新規作成するファイルですので、サクラエディタなどお好きなエディタで編集していきます。
chase_tello/
|– main.py (今回作成するプログラム)
|– tello.py (TelloVideoより拝借)
|– libh264decoder.so (h264decoderをインストールすると生成される)
制御の流れ
大まかな制御の流れは、以下のように行います。
- 離陸する
- Telloから受信したストリームをフレーム単位で取得する
- フレームを画像処理して、赤色のオブジェクトを抽出する
- 赤色のオブジェクトがフレームの中心から左右方向にずれていれば、旋回する
（例えば、左方向にずれていれば反時計回りに旋回する） - 赤色のオブジェクトがフレームの中心から上下方向にずれていれば、上昇／下降する
（例えば、上方向にずれていれば上昇する） - フレームに対してオブジェクトの面積が小さい場合は、前進する
- 2~6を無限に繰り返す
プログラムの作成
それではTelloを追跡するプログラム（main.py）を作っていきます。
main.pyの内容について、解説していきます。
Telloとの接続
Telloとの接続はTelloクラスのコンストラクタを呼び出すことで実行されます。コンストラクタの引数として、PC側のIPアドレス（省略可）とポート番号を引数に入力します。今回は、Telloからのストリームを用いるので、with_video=Trueとしておきます。
drone = tello.Tello('', 8889, with_video=True)離陸する
ドローンに指令を与える場合は、drone.xxxx()のようにTelloの制御関数を実行します。最初に離陸する必要がありますので、takeoff()を実行します。実際には、UDP通信で’takeoff’の文字列をTelloに送信しています。
drone.takeoff()フレームの読み取り
drone.read()にてTelloから送られてくるストリームをフレーム単位で取得します。frameが空だった場合やサイズが0だった場合は、フレーム画像を処理できないのでスキップします。
while True:
frame = drone.read()
if frame is None or frame.size == 0:
continueOpenCVで画像処理
いよいよOpenCVで画像処理を行います。ここでの目標はフレーム画像から赤い色のオブジェクトを捕捉して、中心位置や面積を計算することです。
BGR変換
OpenCVで画像処理を行う前処理としてBGR変換を行います。元のフレーム画像はRGB画像ですが、OpenCVはBGR画像として処理を行います。そのため、OpenCVのcvtColor関数を使ってBGR変換します。
image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)ガウシアンぼかし
ノイズ除去のため、フレーム画像にガウシアンぼかしをかけます。ぼかしの強さは、ksize=(5,5)という引数で決まります。強くぼかしをかける必要はありません。
img_blur = cv2.GaussianBlur(image, (5,5), 0)HSV変換
次の識別色の抽出を容易にするため、BGR画像を今度はHSV画像に変換します。HSV画像とは、色相(Hue)、彩度(Saturation)、明度(Value)の3要素で表現される画像です。黄、赤、緑といった色の特徴を「色相」という1つの要素で表現できるため、画像処理の分野ではよく使われます。一般には、色相は0~359度の値を取ることが多いですが、OpenCVのHSV画像では、0~179の値で表現されるため注意が必要です。
hsv = cv2.cvtColor(img_blur, cv2.COLOR_BGR2HSV)識別色の抽出
識別したい色（本記事では識別色と呼ぶ）の範囲を決めて、識別色で画像を2値化します。今回識別したい赤色は、色相=0付近なので、「170 <= 色相 または 色相 <= 10」という範囲で識別します。また、彩度や明度が多少低い画像でも認識できるように、「100 <= 彩度 <= 255」、「100 <= 明度 <= 255」という範囲で識別します。識別色の範囲を決めたら、HSVの最小値、最大値をcolor_min、color_maxに代入し、OpenCVのinRange関数で2値化します。今回は、色相の値が0を挟んで2つの領域に跨っているため、「170 <= 色相」の2値画像(mask_color1)と、「色相 <= 10」の2値画像(mask_color2)を作り、加算して1つの2値画像(mask_color)に合成しています。
color_min = np.array([0, 100, 100])
color_max = np.array([10, 255, 255])
mask_color1 = cv2.inRange(hsv, color_min, color_max)
color_min = np.array([170, 100, 100])
color_max = np.array([180, 255, 255])
mask_color2 = cv2.inRange(hsv, color_min, color_max)
mask_color = mask_color1 + mask_color2識別色のオブジェクト抽出
識別色の2値画像(mask_color)をブロブ解析することで、赤色のオブジェクトを抽出します。
blob = analysis_blob(mask_color)ブロブ解析を行うanalysis_blob関数は自作します。ブロブ解析とは、同一の値で連続した領域を1つの塊(=ブロブ)とみなし、その特徴(面積、中心位置等)を解析することです。ブロブ解析にはOpenCVのconnectedComponentsWithStats関数を使います。ブロブは複数個存在する可能性がありますが、今回は、面積最大のブロブのみ抽出しています。詳細はこちらのページ等で紹介されてますので、割愛します。
def analysis_blob(image):
nlabel, label, stats, center = cv2.connectedComponentsWithStats(image)
data = np.delete(stats, 0, 0)
center = np.delete(center, 0, 0)
max_blob = {}
max_blob["upper_left"]=(0,0)
max_blob["width"]=0
max_blob["height"]=0
max_blob["area"]=0
max_blob["center"]=(0,0)
if (nlabel <= 1):
return max_blob
max_index = np.argmax(data[:, 4])
max_blob["upper_left"]=(data[:,0][max_index], data[:,1][max_index])
max_blob["width"]=data[:,2][max_index]
max_blob["height"]=data[:,3][max_index]
max_blob["area"]=data[:,4][max_index]
max_blob["center"]=(int(center[max_index][0]), int(center[max_index][1]))
return max_blobTelloと赤い布の位置関係による制御
Telloと赤い布の位置関係から、赤い布を追従するように制御します。OpenCVによる画像処理によって、赤い布はフレーム画像上のオブジェクトとして抽出されていますので、それを利用します。制御の優先順位は以下の通りです。
- オブジェクトの面積が小さ過ぎる場合、その場に留まります。これは正しく赤い布を捉えられていないケースを想定しています。判定基準は面積2,000pxとしています。
- オブジェクトの中心が画像の中心から見て左側に位置していた場合、反時計回りに旋回します。これによりTelloがオブジェクトの方向を向くことができます。同様にオブジェクトが画像の右側に位置していた場合、時計回りに旋回します。判定基準は左右100pxとしています。
- オブジェクトの中心が画像の中心から見て上側に位置していた場合、上昇します。これによりTelloがオブジェクトの中心と同じ高さに向かいます。同様にオブジェクトが画像の下側に位置していた場合、下降します。判定基準は上下50pxとしています。
- オブジェクトの面積が小さい場合、Telloと赤い布との距離が遠いとみなして、前進します。判定基準は面積200,000pxとしています。
以上の条件をプログラムすると以下のようになります。
if c_area > 2000:
# オブジェクトが画像の左側に位置していたら、反時計回りに旋回する
if c_center[0] < f_width / 2 - 100:
drone.rotate_ccw(20)
# オブジェクトが画像の右側に位置していたら、時計回りに旋回する
elif c_center[0] > f_width / 2 + 100:
drone.rotate_cw(20)
# オブジェクトが画像の上側に位置していたら、上昇する
elif c_center[1] < f_height / 2 - 50:
drone.move_up(0.2)
# オブジェクトが画像の下側に位置していたら、下降する
elif c_center[1] > f_height / 2 + 50:
drone.move_down(0.2)
# オブジェクトの面積が小さい場合、前進する
elif c_area < 200000:
drone.move_forward(20)識別色でマスキングされたフレームの表示
識別色でマスキングされたフレーム画像を表示します。制御には必須ではありませんが、画像表示した方が実際の動作を見ながらデバッグしやすいです。
マスキング前と後の画像は以下のようになります。赤色以外の色が黒く塗りつぶされていることが分かります。尚、手の部分がマスキングされておらず残っていますが、赤い布を持っている箇所なので、今回はこのまま処理します。
識別色によるマスキング
元の画像(img_blur)と識別色の2値画像(mask_color)でAND処理を行い、識別色以外の色をマスクします。OpenCVのbitwise_and関数を使います。
img_masked = cv2.bitwise_and(img_blur, img_blur, mask = mask_color)マスキング画像の表示
OpenCVのimshow関数を使ってマスキング画像を表示します
cv2.imshow("Image", img_masked)マスキング前と後の画像は以下のようになります。赤色以外の色が黒く塗りつぶされていることが分かります。
「Ctrl + C」押下時に着陸
以上で自動制御の部分は終わりですが、停止する手段が無いので、「Ctrl+C」押下でいつでも緊急着陸できるようにしておきます。
try:
while True:
(自動制御のプログラム)
except(KeyboardInterrupt, SystemExit):
print ("Detect Ctrl+C")
drone.land()完成版
main.pyの完成形は以下のようになります。
import tello
import time
import cv2
import numpy as np
F_WIDTH = 960
F_HEIGHT = 720
def analysis_blob(image):
nlabel, label, stats, center = cv2.connectedComponentsWithStats(image)
data = np.delete(stats, 0, 0)
center = np.delete(center, 0, 0)
max_blob = {}
max_blob["upper_left"]=(0,0)
max_blob["width"]=0
max_blob["height"]=0
max_blob["area"]=0
max_blob["center"]=(0,0)
if (nlabel <= 1):
return max_blob
max_index = np.argmax(data[:, 4])
max_blob["upper_left"]=(data[:,0][max_index], data[:,1][max_index])
max_blob["width"]=data[:,2][max_index]
max_blob["height"]=data[:,3][max_index]
max_blob["area"]=data[:,4][max_index]
max_blob["center"]=(int(center[max_index][0]), int(center[max_index][1]))
return max_blob
def main():
current_time = time.time()
pre_time = current_time
# Telloとの通信を開始する
drone = tello.Tello('', 8889, with_video=True)
time.sleep(1)
# 離陸する
drone.takeoff()
time.sleep(2)
try:
while True:
# 0.5秒間停止する
time.sleep(0.5)
# 動画のフレーム画像を読み取る
frame = drone.read()
if frame is None or frame.size == 0:
continue
# BGR変換をかける
image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
# ガウシアンぼかしをかける
img_blur = cv2.GaussianBlur(image, (5,5), 0)
# HSV変換をかける
hsv = cv2.cvtColor(img_blur, cv2.COLOR_BGR2HSV)
# 識別したい色の範囲1を設定する
color_min = np.array([0, 100, 100])
color_max = np.array([10, 255, 255])
mask_color1 = cv2.inRange(hsv, color_min, color_max)
# 識別したい色の範囲2を設定する
color_min = np.array([170, 100, 100])
color_max = np.array([180, 255, 255])
mask_color2 = cv2.inRange(hsv, color_min, color_max)
# 識別したい色の範囲を合成する
mask_color = mask_color1 + mask_color2
# 識別色のオブジェクトを抽出し、面積最大のオブジェクトを取り出す
blob = analysis_blob(mask_color)
# 取り出したオブジェクトの中心位置を取得する
c_center = blob["center"]
# 取り出したオブジェクトの面積を取得する
c_area = blob["area"]
# ドローンの現在高度を取得する
height = drone.get_height()
# オブジェクトが十分な面積を持ち、
if c_area > 2000:
# オブジェクトが画像の左側に位置していたら、反時計回りに旋回する
if c_center[0] < F_WIDTH / 2 - 100:
drone.rotate_ccw(20)
# オブジェクトが画像の右側に位置していたら、時計回りに旋回する
elif c_center[0] > F_WIDTH / 2 + 100:
drone.rotate_cw(20)
# オブジェクトが画像の上側に位置していたら、上昇する
elif c_center[1] < F_HEIGHT / 2 - 50:
drone.move_up(0.2)
# オブジェクトが画像の下側に位置していたら、下降する
elif c_center[1] > F_HEIGHT / 2 + 50:
drone.move_down(0.2)
# オブジェクトの面積が小さい場合、前進する
elif c_area < 200000:
drone.move_forward(20)
# フレーム画像から識別色以外をマスキングする
img_masked = cv2.bitwise_and(img_blur, img_blur, mask = mask_color)
# マスキング画像を表示する
cv2.imshow("Image", img_masked)
# 5秒毎に'command'を送信する
current_time = time.time()
if current_time - pre_time > 5.0:
drone.send_command('command')
pre_time = current_time
# Ctrl + Cを検出したら、着陸する
except(KeyboardInterrupt, SystemExit):
print ("Detect Ctrl+C")
drone.set_command_timeout(10.0)
drone.land()
del drone
if __name__ == "__main__":
main()
プログラムの実行
Telloを起動し、PCからTelloに対してWi-Fi接続します。コマンドプロンプトを起動し、「chase_tello」フォルダに移動し、下記コマンドを実行します。
python main.py実行するとTelloが離陸して赤い布を自動で追従します。終了したい場合は、コマンドプロンプトにて「Ctrl+C」を押すと、Telloが着陸してプログラムが終了します。