前回の「Pythonを使いTello（ドローン）でOpenCV（顔認識）から自動追尾をしてみる!☺」では、少しうまくいきましたが、今回は　もうちょっとハードルを上げた方法で自動追尾をしてみました。まずは　YouTubeに投稿した　自動追尾の映像からご覧ください。すでに御覧になった方は、あしからず。

自動追尾映像

自動追尾についての説明です。

今回までのおおまかなあらすじは、
Tello の画をOpenCVでPCに写してみる！！！！OpenCVの導入方法

OpenＣＶで遊んでみる。ＰＣ内蔵カメラから映像の表示と　ドローンからの映像を表示をする
などが基礎になります。YouTubeの概要欄やほかにも参考になる投稿もありますので是非訪れてみてください！！

今回、スムーズな動きにできたのは、主に１．rcコマンドの使用が一番と思います。それと　２．PID制御というものにはじめて触れ、トライしてみました。3.カメラ映像のデーターを表示
以上　３点が改善できたポイントです。

１：　Telloへのコマンド送信を　SDKの一覧より　RCコマンドを使いました。

１．RCコマンドについて：

SDKに記載されている内容は次の通りです。Tello SDKリンクはこちら！

command	command	Response
rc a b c d	Set remoto controller Control via four channels 　　　　”a” = {roll} left/right (-100,100) 　　　　”b” = {pitch} forward/backward (-100,100) 　　　　”c” = {throttle} up/down (-100,100) 　　　　”d” = {yaw} (旋回） (-100,100)	ok/error

そこで　次のように考えました。　

　　　　”ｂ”（pitch)は、前後の移動位置が出れば　使えます。⇒　カメラに映る顔面積の大小で距離を判断

　　　　”ｃ”(throttle)は、上下の移動位置が出れば　使えます。⇒　顔中心の移動距離で判断

　　　　”ｄ”（yaw) と”a”（roll)は, 左右の移動位置が出れば　使えます。しかし、同じ数値情報から　２つは同時に使えない。　clockwiseで角度を出し前後で横幅をだせば、左右距離（斜め距離）ができる、”ｄ”（yaw) を適用することにしました。

次に　コマンド　rc a b c d　の送信は　a b c d の欄に数値を入れた場合はうまくいきますが　a b c d という文字の変数を使いプログラムするには　いろいろ模索し考えましたが力及ばず、いろいろインターネットで検索しても答えが出てこないので、どうしても自分の力では　どうにもなりません。

そこで助け舟を　そうだ！　今、はやりの　「OpenAIの”CahtGTP”」に聞いてみようと思い聞いてみた！チャットやり取りは、次の通りです。（抜粋）
# a, b, c, dの値をmsgに反映する
rc_command = “rc {} {} {} {}”.format(a, b, c, d)
# 文字列をバイト列に変換してから送信する
sent = sock.sendto(msg.encode(‘utf-8’), tello_address)
“CahtGTP”で答えを導いて、よくよく、本で調べてみると、formatと｛｝を使うことで文字列中に数値や他の文字列を挿入できることがわかりました。
例えば、’he is {}’.format(‘hachi-suke’)　　なら　’he is hachi-suke ‘ となります。

これは凄い！早速やってみた。確かに作動しました。　そこでそのままコード使い以下をｒｃコマンドの送信に最終的に使いました。
rc_command = “rc {} {} {} {}”.format(a, b, c, d)
sent = sock.sendto(rc_command.encode(‘utf-8’), tello_address)
ｒｃコマンドはジョイステックを使いラジコンのように操作もできるみたいですので、動きがスムーズになりました。

結果、本で調べてみると、formatと｛｝を使うことで文字列中に数値や他の文字列を挿入できることがわかりました。
例えば、’he is {}’.format(‘hachi-suke’)　　なら　’he is hachi-suke ‘ となります。
“CahtGTP”を使うことで早く正解にたどりつきました。

２：“b” = {pitch} forward/backward (-100,100)　について：

次に　”ｂ”（pitch)は、前後の移動位置が出れば　使えます。これは、前回同様に顔面積の大＞小を基準値にし、考えました。OpenCVで顔認識し、rectangleで囲いましたので、そこから中心値を求めます。次のように求めます。face_area =int(w * h) #面積

“c” = {throttle} up/down (-100,100)　　について：

”ｃ”(throttle)は、上下の移動位置が出れば　使えますので、顔の中心位置の座標軸”ｙ軸”の移動座標の位置から上下の動きに連動させました。

まずは顔のセンター位置の算出です。OpenCVで顔認識し、rectangleで囲いましたので、そこから中心値を求めます。次のように求めます。

for (x, y, w, h) in faces:

cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)

face_center_x = int(x + (w / 2)) #顔の中心x軸

face_center_y = int(y + (h) / 2)　　#顔の中心ｙ軸

センターの印は次のコードを使います。

cv2.circle(img,(face_center_x, face_center_y), 10, (0,0,225), cv2.FILLED) #中心point

条件の数値を作成する

次のステップは、　顔面積、y軸座標位置　の　移動数値の把握です。　これは、動画を見ていただければわかると思いますが、どのぐらい移動したら　数値はいくらになるか確認していきます。

この時、Telloはカメラだけ作動させています。

数値確認は、ある程度　式を作っておき　数値だけを変更して調整していきます。

上下運動のコマンド用式の場合：

if face_center_y > 180 and face_center_y < 40:　　#何も反応しない’遊び’範囲を作る

c = 0

elif 280 > face_center_y > 180 + 10 : #frame h:360の1/2=180 顔の下への移動は”ｙ座標が大きくなる”

c = -20

elif 50 < face_center_y < 110 :　#frame h:360の1/2=180 顔の上への移動は”座標が小さくなる”

c = 20

同様に顔面積の数値をみて、　それぞれコマンド用式の面積数値を調整していきます。

それぞれの箇所で　print(face_center_x, face_center_y, face_area) と　print等して確認していくといいです。そして　画面に数値を表示させるとわかりやすいです。

“d” = {yaw} (旋回） (-100,100)　　について：

”旋回”yaw については、PID制御の考え方を基本にして構成します。

２．PID制御について:

電気ポットを例にしてみるとわかりやすいです。

簡単にいうと目標温度設定と　実際の温度をセンサーの確認し、その差（偏差）を　比例・微分・積分を使って差をなくして目標値に近づけていく手法です。

まず自分で温度を調整する場合は、どうでしょうか？実際の温度を見ながら、目標設定温度になるようにヒーターの温度を上下させていきます。この人の頭の中の動作をマイクロコンピューターで自動で計算させたものがPID制御です。

PはProportional（比例）、IはIntegral（積分）、DはDerivative（微分）を意味し、比例制御に微分制御と積分制御を加えたものをPID制御といいます。

今回は、PI制御をしていきます。

これで何となく　理解できたと思います。
　余談ですが、微分積分も高校では文系だったため詳しく力を入れて勉強しませんでした。
こんなところで使えるとは！という思いです。　授業で数式だけの勉強でなく　このように実際の利用例を挙げ、勉強したらもっと楽に理解できたのにと、思いました。
そこで基礎の三角関数から復習しました。
　これが　今になるととても新鮮で面白かったです。皆さんも少し休憩で思い出してみてください。
復習した内容を以下にまとめみました。

三角関数　復習（MEMO)

角速度について：　

　これは斜め移動でのコマンドに使えそうです。

次回　これを使用して検討してみようと思います。

PI制御の実装：

とうとう、PI制御の実装にきました

pid = [0.4, 0.4, 0]

pError = 0

error = x – w/2

d = pid[0] * error + pid[1] * (error – pError) #PID制御のうちPI制御

ここでのPID制御は、目標値として画面の中心を設定しており、それに対して顔の中心位置の誤差（error）を算出しています。pErrorは前回の誤差を記憶するための変数で、現在の誤差と前回の誤差の差を使って目標値と実際の値の差から制御量を決定します。

プログラム中の、error = x – w/2は、画面中央から顔の中心までの距離を計算するためのコードです。次に、d = pid[0] * error + pid[1] * (error – pError)は、PI制御の式で、P制御とI制御を合わせたものです。
pid = [0.4, 0.4, 0] は調整されたゲイン[比例定数、積分定数、微分定数」です。私は、調整（設定）が難しかったので、いろいろなサイトから皆さんが共通して使用されている数値を適用させていただきました。

P制御は、目標値と実際の値の差を用いて、制御量を決定する制御要素であり、pid[0]は比例定数です。I制御は、目標値と実際の値の差を積分した値を用いて制御量を決定する制御要素であり、pid[1]は積分定数です。また、pErrorは、前回の誤差を表しており、I制御に使用されます。
Pythonでは、単体で変数を参照することで、その変数の値を取得することができます。そのため、pError変数には、以前に計算された値が格納されています。つまり、こののようにpError変数だけを使用して制御信号を計算することができます。

３．最後にデーター表示手段：

Telloからの画像のデーターの表示方法　　顔中心ポイント、顔面積、　顔中心座標、　rcコマンドの各入力データー　について

顔中心ポイント

cv2.circle(img,(face_center_x, face_center_y), 10, (0,0,225), cv2.FILLED)

顔中心座標

cv2.putText(img, f’facecenter:({face_center_x}, {face_center_y})’, org=(50, 50), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(204, 0, 255), thickness=1, lineType=cv2.LINE_4)

顔面積

cv2.putText(img, f’facearea: ‘+ str(face_area), org=(30, 30), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(204, 0, 255), thickness=1, lineType=cv2.LINE_4)

rcコマンドの入力データー

cv2.putText(img, f’ roll: ‘+ str(a) + f’ pitch:’ + str(b) + f’ throttle: ‘ + str(c) + f’ yaw: ‘ + str(d), org=(100, 100), fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.5, color=(204, 0, 255), thickness=1, lineType=cv2.LINE_4)

以上が　Telloの顔認識による自動追尾　プログラミング　になります。
これで皆さんも楽しんでください
最後まで読んでいただきありがとうございました。(^^♪

まだまだ粗削りですが　ご参考になれば幸いです　コードを添付しときます。←ここをクリック( ;∀;)

ホームへ戻る

最新の投稿はこちら☟

ホームへ戻る

Pythonで「Tello(ドローン）で自動追尾プログラミング」改良版を紹介します！これはいける！

自動追尾映像