ロックオンせよ
bowreso
輪郭追跡処理をやってみよう！シリーズの最終回（また続くかも）です。 2つのサーボモーターにより上下左右に首を振れるユニットに装着した小型カメラで目標方向を撮像し、背景から目標画像を抽出して、その目標の動きにカメラを追従させます。目標の検出には先人が選りすぐれたアルゴリズムを開発、提供してくれていますが、ここでは、これまでに作った輪郭追跡処理の応用と言うことで追尾処理の一部として実装したので紹介します。
輪郭追跡処理をやってみよう！シリーズ
輪郭追跡処理をやってみよう！
輪郭追跡処理をやってみよう！（その２）
Python C言語による拡張モジュールで高速化
輪郭追跡処理をやってみよう！（その３）
何を追従しようか
前回の輪郭追跡処理をやってみよう！（その3）で目標画像としていた、このシカがウロウロするアニメーションGIFをカメラで撮像し、カメラに追従させることにします。
システム構成
システムというほどのものではありませんが、以下の構成です。
- Raspberry Pi4 B 4GB
- USB Camera （USBカメラモジュール（30万画素、OV7725センサー）、M12 Fixedレンズ（角度：90度）
- Pan/Tilt 機構作成キット（スイッチサイエンスで購入しました。）製造販売元はこちらです。
Raspberry Pi4 の設定/環境
Raspberry PiのGPIOを制御するためのライブラリ：pigpioをインストールします。これを実行できるようにする“pigpiod”デーモンは、Raspbianにプリインストールされているもの、常駐していないので、インポートする前にあらかじめ起動します。
$ sudo apt install pigpio
$ sudo service pigpiod start
$ sudo systemctl enable pigpiod
この後で、pigpiodの状態を確認し、avtive(running)となっていれば、Pythonのコードで、pigpioのimportが正常に行えます。
$ sudo systemctl stutus pigpiod
・・・・・略・・・・・ avtive(running)
配線の接続
サーボモーターへの制御信号は、GPIO18とGPIO19を用いました。PWM信号を出力するのですが、全てのGPIO端子がPWM信号を出力できるわけではないらしいので、Raspberry Piの仕様を確認し、都合の良いピンに割り当てましょう。
サーボモーターへ供給する電源は3.3v～6vの範囲が入力仕様ですが、ここでは5vとしました。ただし、Raspberry Piが動作不安定になるのを避けるためにRaspberry Piの5v端子からは得ずに、USBアダプタにUSB配線を接続してそこから5vを得るようにしました。サーボモーター１つに付きUSBからの電源１つの割り当てです。
サーボモーター（SG90の使い方）
20ms（50Hz）のPWMピリオドで、Dutyサイクルを指定することにより、それに対応した絶対角度指定を行ってサーボモーターを回します。
こちらを参照しました。”Zenn ラズベリーパイのレシピ Chapter32 サーボモータSG90に角度指定して制御する。”
今回は目標物を追従するための相対角度指定がやりたかったので、下記Servo.pyでは、相対指定を絶対指定に変換する関数を作っています。
プログラムの仕様概要
プログラムを起動するとUSBカメラが接続されていればOpenCVを使ってキャプチャーを行います。
キー入力を待ち、所定のキー入力があればそれに応じた動きをします。
キーによりカメラを上下、左右、ゼロポジションに移動させるコマンドと、背景より明るい目標を抽出して輪郭を赤で示す処理を行うコマンドと、その目標を追尾するコマンドを用意しました。
プログラムの構成
画像処理を行うクラス（IP.py）と、サーボモーターを制御するクラス（Servo.py）と、コマンド受付と全体の処理を担うmain.pyで構成されます。
IP.pyは、これまでの輪郭追跡処理シリーズで作成したCライブラリを使っています。
追尾の原理
目標画像（この場合はシカ）を抽出し、その画像の中心座標を求めます。このカメラは640×480画素です。左端上の座標（ｙ、ｘ）を（０，０）と置いたときの座標です。目標の中心座標を(y_tc, x_tc）とします。そして、シカの中心座標が、カメラのフレーム全体の中心座標（239、319）からどれだけズレているか（error_y, error_x）を計算します。 カメラのフレームの中心にシカを常に持ってることができれば、シカを追従していることになります。先に求めたズレ量を元にカメラを上下左右に動かします。ズレ量からサーボモーターの制御量を決定するゲインは理論的に求まるはずですが、ここでは横着してカットアンドトライで決めました。ちなみにX方向（yawing）のゲイン：0.02、Y方向（pitching）のゲイン：-0.015で、最終テストしています。最初にゲインを双方とも0.1にして動かしたらカメラが大暴れして、直ちにロックオンが外れ、画像処理が失敗しエラーで終了しました。テキトーにやってはいけません。
動作結果（ただのキャプチャー中、命令（目標抽出）待ち）
動作結果（目標抽出中有、命令（ロックオン）待ち）
シカはウロウロしていますが、無事シカの輪郭を抽出できている（赤で輪郭を着色しています）ことが分かります。
動作結果（ロックオン中。 カメラが鹿を追いかける）
シカは動いていますが、カメラの中央にシカを捉えていることがわかります。
追尾中のカメラの様子
手前のカメラユニットにピントを合わせているので、奥のシカの画像はピンぼけに写っています。 カメラはしっかりシカを追従できました。
ソースコード一式
補足
シカの動画を見れば明らかですが、輪郭抽出処理を楽にするために、背景画像と目標画像は明確に明るさの違う動画データを作っています。背景と目標の区別を行うために2値化処理を行っているため、目標と似たような、あるいは更に明るい輝度を持った障害物が映り込めば、どちらを追尾すべきか分からなくなります。今回の例では、最も大きな目標物を追いかける仕様としましたが、追尾妨害策としては不十分です。せっかく”輪郭追跡処理シリーズ”で、画像処理プログラムを作ったのでその応用編と言うことで自作ライブラリをの活用を選択しましたが、OpenCVに用意されちている”動体検出”関数を使うのがスマートではないかと思っていますが試していません。それから、今後、性能アップのやる気が盛り上がれば、カメラやチルト機構、サーボモーターの種類も色々試してみたいですね。
