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

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

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

自動追尾映像

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

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

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

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

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

1.RCコマンドについて:

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

   command    commandResponse
rc a b c dSet 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

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

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

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

    ”d”(yaw) と”a”(roll)は, 左右の移動位置が出れば 使えます。しかし、同じ数値情報から 2つは同時に使えない。 clockwiseで角度を出し前後で横幅をだせば、左右距離(斜め距離)ができる、”d”(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コマンドの送信に最終的に使いました。
rc_command = “rc {} {} {} {}”.format(a, b, c, d)
sent = sock.sendto(rc_command.encode(‘utf-8’), tello_address)

rcコマンドはジョイステックを使いラジコンのように操作もできるみたいですので、動きがスムーズになりました。

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

“CahtGTP”を使うことで早く正解にたどりつきました。

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

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

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

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

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

facecenter

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)  #顔の中心y軸

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

 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 顔の下への移動は”y座標が大きくなる”

                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制御の考え方を基本にして構成します。

2.PID制御について:

pid

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

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

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

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

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

 

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

マッキー
神戸

三角関数 復習(MEMO)

san
san1
san2
san3
san4

角速度について: 

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

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

PI制御の実装:

とうとう、PI制御(但し、積分制御のような誤差の累積ではなく、前回の誤差と今回の誤差の差分(すなわち、誤差の変化量)を使っています。の実装にきました

pid = [0.4, 0.4, 0]

P制御(0.4): 比例ゲインを高く設定すると、誤差に対してより敏感に反応しますが、過剰な振動や不安定さを引き起こす可能性があるため、適度な値が選ばれています。
I制御(0.4): 積分制御の役割として、誤差が長期間存在する場合にそれを解消する動きが期待されますが、誤差の変化量に基づく近似的なI制御を行っているため、ここでも適度な値が選ばれています。

pError = TrackFace(info, w, pid, pError): 前回の誤差(pError)を更新しながら次のフレームで使用

pError = 0

pError前回の誤差を記録し、誤差の変化量を計算するために使用されます。
制御ループの中で、前回の誤差と現在の誤差を比較し、システムがどれだけ改善したか(または悪化したか)を測定します。
各ループでpErrorを更新し、次の制御ループで再利用します。
pError = error # 次のループで今回の誤差を前回の誤差として使用

  error = x – w/2

顔のx座標の中心と画像の中心のズレを計算

         d = pid[0] * error + pid[1] * (error – pError)    #PID制御のうちPI(微分制御(D制御)に近いもの)制御

pid[0] * error:比例制御(P制御)画像の中央と顔のx座標のズレ(error)に比例して、制御量を計算します。
pid[1] * (error - pError):積分制御(I制御に近い)前回の誤差(pError)との差分に基づいて、制御量を計算します。ただし、微分制御(D制御)はプログラムには実装されていないので、正確にはPI制御です。

このプログラムでは、積分制御のような誤差の累積ではなく、前回の誤差と今回の誤差の差分(すなわち、誤差の変化量)を使っています。プログラム内で行っている pid[1] * (error - pError) は、次の動作をしています。
error: 現在の誤差(顔のx座標のズレ)
pError: 前回の誤差
error - pError: 誤差の変化量
この式は、誤差の変化量を計算しており、時間を積算するわけではないため、厳密には積分制御とは異なります。しかし、誤差が変化し続けることに反応して制御量を増減するという動作が、積分制御の動きに似ているため、「I制御に近い」

ここでのPID制御は、目標値として画面の中心を設定しており、それに対して顔の中心位置の誤差(error)を算出しています。pErrorは前回の誤差を記憶するための変数で、現在の誤差と前回の誤差の差を使って目標値と実際の値の差から制御量を決定します。本来の積分制御は、過去の誤差をすべて合計してその累積に基づいて制御を加えますが、ここでは誤差の累積は行わず、前回と今回の誤差の差分のみを利用しています。このため、積分制御に似た形で誤差に修正を加えるものの、正確には微分制御に近いものです。
プログラム中の、error = x – w/2は、画面中央から顔の中心までの距離を計算するためのコードです。次に、d = pid[0] * error + pid[1] * (error – pError)は、PI制御の式で、P制御とI制御(微分制御(D制御)に近いもの)を合わせたものです。

pid = [0.4, 0.4, 0] は調整されたゲイン[比例定数、微分定数、積分定数」です。私は、調整(設定)が難しかったので、いろいろなサイトから皆さんが共通して使用されている数値を適用させていただきました

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

3.最後にデーター表示手段

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(ドローン)で自動追尾プログラミング」改良版を紹介します!これはいける!

I


コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です