Go to Qiita Advent Calendar 2024 Top
LoginSignup
15
17

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 3 years have passed since last update.

@hsgucci

ライントレースプログラム

Last updated at Posted at 2020-03-23

#はじめに

このページは，

公式SDK「Tello-Python」を試そう

の１ページです．
全体を見たい場合は上記ページへお戻りください．

#概要
本ページでは，以下の動画の様に，Telloで床に置いたトラロープを検出してライントレースさせます．

使っているのは，画像処理の入門である「色検出」です． 　[色検出プログラム](https://qiita.com/hsgucci/items/e9a65d4fa3d279e4219e) を少し書き換えるだけで，これが実現できます． 　

トラロープを使った理由は「20m程度の長さを持つ色付きロープのなかで，一番安かったから」です(^^;
　googleショッピング検索'トラロープ 20m'
画像処理を行うためには，**単色で太い**ロープが望ましいのですが，十分な長さのある太いロープがなかなか見つからず，あったとしても数千円もしたので諦めました．

「将来は工事現場でドローンで荷物搬送をする」とか言い訳しておきます(-_-;

##ライントレースロボットの歴史
ロボットのプログラミングの入門でよくあるのは，ラインからはみ出さないように走り続けるという「ライントレース」ですね．
「ライントレースロボット」「ライントレースカー」「ライントレーサー」などと呼ばれ，専用の商品も多数販売されています．
LEGO MINDSTORMSでもライトセンサーの練習で作ります．

ライントレースのロボコンで有名なものは，やはり『ジャパンマイコンカーラリー』でしょう．
歴史も古く，スピードも強烈です．
　参考URL：　Youtube検索"マイコンカーラリー"

工場内の搬送ロボットでもライントレース技術は使われており，ラインを追いかけることは，もはやロボットプログラミングの基本中の基本と言っても良いでしょう．

ライントレースロボットでは一般的に，検出距離が数センチ以下の反射型フォトインタラプタ(ラインセンサーと呼称)を使ってラインの位置を検出し，駆動用モータやステアリングサーボへの出力を調整する制御プログラムを書きます．

##カメラでライントレース
しかし，近年のCPU技術の発達に伴って，カメラを用いた画像処理技術も発展したため，
「いつまで地面のラインを検出する専用センサを使ってんだよ？」
と言い出す輩も出てくる様になりました．
（マイコンカーラリーもそうですが，多くのロボコンは中・高生の教育のためにあるので，「入門は誰しも簡単な物から」「ルールを高度化できない」なんですけどね．．．）

最近話題の『DonkeyCar』『AWS DeepRacer』『JetRacer』では，
Raspberry PiやJetson Nanoを使ってディープラーニングでコースを学習し，
カメラ映像のみでコースを自動走行するレースが普通に行われています．
以下参考動画

ジャパンマイコンカーラリーでも，2020年からCamera Classが正式競技になりました．

以上の様に，カメラでライントレースが徐々に「当たり前」になりつつあります．

##ドローンでライントレース?

本記事では，地上の車ではなく，空飛ぶドローンでライントレースをさせます．
幸い，Telloはカメラ映像が取得でき，簡単なコマンドで移動ができるので，練習台には最適です．

きっと将来は「マイコンドローンラリー」とか「自律ドローンレース」とかが行われると信じて，時代を先取りしてみましょう！(^^

#前提条件

ホームフォルダにTello-Pythonがインストールされているという前提で話を進めます．

Linuxマシンであれば　/home/(ユーザー名)/　に，Tello-Pythonというフォルダがあることになります．

詳しくは　Tello-Pythonのダウンロード　を御覧ください．

#今回の作業内容

以前の記事　色検出プログラム　のプログラムをコピー＆改変して，ライントレースに対応させます．

#作業ディレクトリの作成

まずは，色検出のプログラムTello-CV-colorをコピーして，新しいプロジェクト(ディレクトリ)**Tello-CV-linetrace**を作ります．

Tello-CV-colorをディレクトリごとコピー
$ cd ~/Tello-Python/
$ cp -R Tello-CV-color Tello_CV_linetrace
$ cd Tello-CV-linetrace

tello.pyとlibh264decoder.soのコピーの手間など考えると，フォルダごとコピーが一番楽ですね．

次に，色検出のプログラムで最後に作ったmain_control.pyをコピーして，新しいファイル**main_linetrace.py**を作ります．

コントロールプログラムを別名にコピー
$ cp main_control.py ./main_linetrace.py

以下，このmain_linetrace.pyを書き換えていきます．

#ラインを検出してTelloが追いかけるプログラム

##main_linetrace.py

プログラム本体であるmain_linetrace.pyは，以下のコードを参考に書き換えてください．
書き加えの手間を省くのであれば，ここ　を右クリックして[名前を付けて保存]機能でファイル保存してください．

main_linetrace.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import tello		# tello.pyをインポート
import time			# time.sleepを使いたいので
import cv2			# OpenCVを使うため
import numpy as np

# メイン関数
def main():
	# Telloクラスを使って，droneというインスタンス(実体)を作る
	drone = tello.Tello('', 8889, command_timeout=.01)  

	current_time = time.time()	# 現在時刻の保存変数
	pre_time = current_time		# 5秒ごとの'command'送信のための時刻変数

	time.sleep(0.5)		# 通信が安定するまでちょっと待つ

	# トラックバーを作るため，まず最初にウィンドウを生成
	cv2.namedWindow("OpenCV Window")

	# トラックバーのコールバック関数は何もしない空の関数
	def nothing(x):
		pass

	# トラックバーの生成
	cv2.createTrackbar("H_min", "OpenCV Window", 0, 179, nothing)
	cv2.createTrackbar("H_max", "OpenCV Window", 128, 179, nothing)		# Hueの最大値は179
	cv2.createTrackbar("S_min", "OpenCV Window", 128, 255, nothing)
	cv2.createTrackbar("S_max", "OpenCV Window", 255, 255, nothing)
	cv2.createTrackbar("V_min", "OpenCV Window", 128, 255, nothing)
	cv2.createTrackbar("V_max", "OpenCV Window", 255, 255, nothing)

	a = b = c = d = 0	# rcコマンドの初期値を入力
	b = 40				# 前進の値を40に設定
	flag = 0
	#Ctrl+cが押されるまでループ
	try:
		while True:

			# (A)画像取得
			frame = drone.read()	# 映像を1フレーム取得
			if frame is None or frame.size == 0:	# 中身がおかしかったら無視
				continue 

			# (B)ここから画像処理
			image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)		# OpenCV用のカラー並びに変換する
			small_image = cv2.resize(image, dsize=(480,360) )	# 画像サイズを半分に変更
			bgr_image = small_image[250:359,0:479]				# 注目する領域(ROI)を(0,250)-(479,359)で切り取る
			hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)	# BGR画像 -> HSV画像

			# トラックバーの値を取る
			h_min = cv2.getTrackbarPos("H_min", "OpenCV Window")
			h_max = cv2.getTrackbarPos("H_max", "OpenCV Window")
			s_min = cv2.getTrackbarPos("S_min", "OpenCV Window")
			s_max = cv2.getTrackbarPos("S_max", "OpenCV Window")
			v_min = cv2.getTrackbarPos("V_min", "OpenCV Window")
			v_max = cv2.getTrackbarPos("V_max", "OpenCV Window")

			# inRange関数で範囲指定２値化
			bin_image = cv2.inRange(hsv_image, (h_min, s_min, v_min), (h_max, s_max, v_max)) # HSV画像なのでタプルもHSV並び

			kernel = np.ones((15,15),np.uint8)	# 15x15で膨張させる
			dilation_image = cv2.dilate(bin_image,kernel,iterations = 1)	# 膨張して虎ロープをつなげる
			#erosion_image = cv2.erode(dilation_image,kernel,iterations = 1)	# 収縮

			# bitwise_andで元画像にマスクをかける -> マスクされた部分の色だけ残る
			masked_image = cv2.bitwise_and(hsv_image, hsv_image, mask=dilation_image)

			# ラベリング結果書き出し用に画像を準備
			out_image = masked_image

			# 面積・重心計算付きのラベリング処理を行う
			num_labels, label_image, stats, center = cv2.connectedComponentsWithStats(dilation_image)

			# 最大のラベルは画面全体を覆う黒なので不要．データを削除
			num_labels = num_labels - 1
			stats = np.delete(stats, 0, 0)
			center = np.delete(center, 0, 0)


			if num_labels >= 1:
				# 面積最大のインデックスを取得
				max_index = np.argmax(stats[:,4])
				#print max_index

				# 面積最大のラベルのx,y,w,h,面積s,重心位置mx,myを得る
				x = stats[max_index][0]
				y = stats[max_index][1]
				w = stats[max_index][2]
				h = stats[max_index][3]
				s = stats[max_index][4]
				mx = int(center[max_index][0])
				my = int(center[max_index][1])
				#print("(x,y)=%d,%d (w,h)=%d,%d s=%d (mx,my)=%d,%d"%(x, y, w, h, s, mx, my) )

				# ラベルを囲うバウンディングボックスを描画
				cv2.rectangle(out_image, (x, y), (x+w, y+h), (255, 0, 255))

				# 重心位置の座標を表示
				#cv2.putText(out_image, "%d,%d"%(mx,my), (x-15, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))
				cv2.putText(out_image, "%d"%(s), (x, y+h+15), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 0))

				if flag == 1:
					# a=c=d=0，　b=40が基本．
					# 左右旋回のdだけが変化する．
					# 前進速度のbはキー入力で変える．

					dx = 1.0 * (240 - mx)		# 画面中心との差分

					# 旋回方向の不感帯を設定
					d = 0.0 if abs(dx) < 50.0 else dx   # ±50未満ならゼロにする

					d = -d
					# 旋回方向のソフトウェアリミッタ(±100を超えないように)
					d =  100 if d >  100.0 else d
					d = -100 if d < -100.0 else d

					print('dx=%f'%(dx) )
					drone.send_command('rc %s %s %s %s'%(int(a), int(b), int(c), int(d)) )


			# (X)ウィンドウに表示
			cv2.imshow('OpenCV Window', out_image)	# ウィンドウに表示するイメージを変えれば色々表示できる

			# (Y)OpenCVウィンドウでキー入力を1ms待つ
			key = cv2.waitKey(1)
			if key == 27:					# k が27(ESC)だったらwhileループを脱出，プログラム終了
				break
			elif key == ord('t'):
				drone.takeoff()				# 離陸
			elif key == ord('l'):
				drone.land()				# 着陸
			elif key == ord('w'):
				drone.move_forward(0.3)		# 前進
			elif key == ord('s'):
				drone.move_backward(0.3)	# 後進
			elif key == ord('a'):
				drone.move_left(0.3)		# 左移動
			elif key == ord('d'):
				drone.move_right(0.3)		# 右移動
			elif key == ord('q'):
				drone.rotate_ccw(20)		# 左旋回
			elif key == ord('e'):
				drone.rotate_cw(20)			# 右旋回
			elif key == ord('r'):
				drone.move_up(0.3)			# 上昇
			elif key == ord('f'):
				drone.move_down(0.3)		# 下降
			elif key == ord('1'):
				flag = 1					# 追跡モードON
			elif key == ord('2'):
				flag = 0					# 追跡モードOFF
				drone.send_command('rc 0 0 0 0')
			elif key == ord('y'):			# 前進速度をキー入力で可変
				b = b + 10
				if b > 100:
					b = 100
			elif key == ord('h'):
				b = b - 10
				if b < 0:
					b = 0

			# (Z)5秒おきに'command'を送って、死活チェックを通す
			current_time = time.time()	# 現在時刻を取得
			if current_time - pre_time > 5.0 :	# 前回時刻から5秒以上経過しているか？
				drone.send_command('command')	# 'command'送信
				pre_time = current_time			# 前回時刻を更新

	except( KeyboardInterrupt, SystemExit):    # Ctrl+cが押されたら離脱
		print( "SIGINTを検知" )

	drone.send_command('streamoff')
	# telloクラスを削除
	del drone


# "python main.py"として実行された時だけ動く様にするおまじない処理
if __name__ == "__main__":		# importされると"__main__"は入らないので，実行かimportかを判断できる．
	main()    # メイン関数を実行

##プログラム解説

色検出のプログラムは，オレンジ色のカラーコーンを追いかけて左右旋回するプログラムでした．
これと異なる点は，大きく分けて３つです．

  1. 画像処理を行う領域を，画面下側480x110だけに絞った
  2. トラロープのまだら模様を検出するため，膨張処理を行った
  3. 前進/後進を行うrcコマンドが0だったものを40にした
  4. キー入力で前進/後進の速度を増減できるようにした

###1. ROIの設定
色検出でも使ったmain_bgr.pyでTelloの画像を見てみると，
下の写真の様に遠くまでロープが見えています．
linetrace_roi.png

あまり遠くまで色検出しても意味がないので，今回は画像処理領域を狭くします．
これを，注目領域，すなわちRegion Of Interest(ROI)を切り出す，と言います．

　参考URL：Python/OpenCVのROI抽出!領域の切り出しとコピー

実際にはこの様に記述します．

画像の
# (B)ここから画像処理
image = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)		# OpenCV用のカラー並びに変換する
small_image = cv2.resize(image, dsize=(480,360) )	# 画像サイズを半分に変更
bgr_image = small_image[250:359,0:479]				# 注目する領域(ROI)を(0,250)-(479,359)で切り取る
hsv_image = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2HSV)	# BGR画像 -> HSV画像

画像を取り込んで480x360に縮小した画面から，更に下側480x110だけ切り出して，以降はそれを使います．
Pythonだと新規image = 元image[ 上端y:下端y , 左端x:右端x ]の様に書くだけで部分画像を取り出せるので簡単です．

この480x110だけの画像で二値化・ラベリング・重心計算を行い，最大面積を持つオブジェクトを追従しています．

###2. 二値化画像の膨張

上で示した画像のように，トラロープは**オレンジと黒**が交互に並んでいます．
これを単純にinRange関数で範囲指定二値化を行うと，下図の様に一本のロープではなく切断された断片として認識してしまいます．
Screenshot at 2019-12-09 18:46:33.png

『二値化画像が分断してしまう』時に便利な手法が，膨張/収縮処理です．
　参考URL：モルフォロジー変換

・膨張処理で二値画像を一回り太らせる（天ぷらの粉を着ける様に）
・収縮処理で二値画像を一回り削る（芋の皮むきをする様に）

と思えばよいです．
膨張はcv2.dilate，収縮はcv2.erodeで行います．
膨張収縮処理を行う範囲は，kernel配列の中を1で埋めることで決めます．

# inRange関数で範囲指定二値化
bin_image = cv2.inRange(hsv_image, (h_min, s_min, v_min), (h_max, s_max, v_max)) # HSV画像なのでタプルもHSV並び

kernel = np.ones((15,15),np.uint8)	# 15x15で膨張させる
dilation_image = cv2.dilate(bin_image,kernel,iterations = 1)	# 膨張して虎ロープをつなげる

一般的な画像処理では，4近傍や8近傍で膨張収縮を行うのですが，
今回は力技，なんと15x15画素で膨張処理を行っています！
つまり，ある画素が白(255)だったとき，その周囲15x15画素全てを白に塗り替えるのです．
ちょっと広すぎ(-_-;;

こうすることで，トラロープの切断された二値化画像を無理やり太らせ１つに繋げてしまおう，という作戦です．

###3. 前進のコマンド入力

色検出のプログラムでは左右に旋回を行うだけなので，以下の初期化でa,b,c,dを0にし，制御プログラムで旋回のdの値を変化させていました．

rcコマンドの数値初期化部
a = b = c = d = 0	# rcコマンドの初期値を入力

今回は，旋回しながら前進させたいので，以下の初期化も記述しています．

前進のrcコマンドに一定値を入力
b = 40			# 前進の値を40に設定

40という値は，何回か実験したうえで決めた移動速度です．
これ以上速いとコースアウトしてしまいました．

###4. 前進速度の可変

OpenCVウィンドウのキー入力部分には，以下の様に追記してあります．

OpenCVウィンドウのキー入力部分
elif key == ord('y'):			# 前進速度をキー入力で可変
	b = b + 10
	if b > 100:
		b = 100
elif key == ord('h'):
	b = b - 10
	if b < 0:
		b = 0

yキーを押すことでbの値を+10，
hキーを押すことでbの値を-10，
できるようにしてあります．

30ぐらいが適正値だと思いますが，色々変えて試せるようにしてあります．

##プログラムの実行

プログラム本体はmain_linetrace.pyです．

プログラムの実行
$ python main_linetrace.py

今までと同様にctrl+cを押すことで，プログラムを終了することもできますが，
OpenCVが作ったウィンドウでESCキーを押して終了するのが良いでしょう．

###操作系

操作系は以下の様になっています．
1キーで色追従のフィードバック制御がON(有効)になり，
2キーでOFF(無効)になります．

yキーで前進速度を10増加させ，
hキーで10減少させます．
linetrace_keyboard.png

フィードバック制御がONになると，Telloは初期速度(40)で前進しながら左右旋回を行います．

###操作手順

  1. tキーで離陸させる．
  2. 上下前後左右の移動キーで，顔が連続して認識できる位置(安全な位置)までTelloを手動操作する．
  3. 1キーを押してフィードバック制御を開始させる．
  4. Telloが一定速度で前進し始める．
  5. もしTelloの前進速度が速すぎる場合，hキーで速度を落とす．逆にyキーで増速もできる．
  6. 2キーを押して制御を終了させ，移動キーでTelloを止める．
  7. lキーで着陸させる．

Tello SDKのrcコマンドを使って操作しているので，機体が流れ始めた際に止めるのは手動操作だけです．（移動コマンドは応答が遅いので使っていません）

##実行結果

プログラムを実行すると，以下の様に最大面積を持つラベルが表示されます．
15x15画素の膨張を行っているので，検出している領域が元々のロープよりも太くなっていることが分かりますね．

OpenCV Window_003.png

このオブジェクトの重心位置が，画面中心のX座標240に移動するように旋回制御がかかります．

上図の様に，綺麗にロープが見えるようになるには，離陸後の高度から2回程度sキー(下降)を押すと良いでしょう．
あまり下げ過ぎると自動着陸してしまうので注意が必要です．

ロープを検出していることを確認したら，1キーを押して自動制御させます．
うまくいけば，冒頭で紹介した動画の様にTelloがライントレースします．

#おわりに

今回は，色検出の応用として，トラロープを検出してトレースさせてみました．

個人的に残念に思う点を以下に挙げてみます．

  • 前進/旋回だけの二次元平面の動きでしかない
  • ディープラーニングなどの所謂AI処理ではなく，単なる色検出

まだまだ，発展の余地がありますね．
個人的な 妄想 希望では，ドローンレースのリングをAIで通り抜けてみたいんですよ〜．

15
17
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
15
17

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?