MeArmPi で Object tracking

MeArmPi で Object tracking

対象物体を検知し追跡するロボットです。ロックオンするとグリップが動きます。

www.youtube.com

コードは GitHub に上げています。

github.com

MeArmPi とは?

MeArmPi は ラズパイで動くロボットアームキットです。ジョイスティックでの操作やブラウザからプログラミングを組んで動かすことができます。

mime.co.uk

MeArmPiには、カメラは付属していないので、カメラモジュール(PiCamera)を取り付けました。

環境

仕組み

ブラウザから見るロボットカメラの画像です。「黄色いもの」を検知/追跡しています。 追跡する物体の位置が赤枠の境界を超えると枠内に戻す方向にロボットアームを動かすという仕組みです。 下は物体の検出を表す画像(ヒストグラムの逆投影)です。物体が白く見えます。

cube

Python でサーボを動かすために

コードは以下を使わせてもらいました。

  • MeaArmPi TechnicalOverview

https://groups.google.com/group/mearm/attach/18a4eb363ddaa/MeArmPiTechnicalOverviewV0-2DRAFT.pdf?part=0.1

実際にやる際は、このドキュメントにある sample コードを実行してサーボが意図した角度に動くかどうか十分に確認することをお勧めします。

https://gist.github.com/bjpirt/9666d8c623cb98e755c92f1fbeeb6118

動く範囲を制限するために MeArm クラスで maxAngle が設定されていますが、私の MeArmPi ではこの制限では足りずサーボモーターがアームの可動域を超えて動こうとするのでモーターが過度に熱くなってしまいました。安全のためさらに±25度の制限をかけています。

mearm.py

safe_angle = 25
    def moveToAngle(self, angle):
        """prevent mearmpi from moving so far (angle: +- safe_angle)"""
        if angle > self.maxAngle - safe_angle:
            angle = self.maxAngle - safe_angle
        if angle < self.minAngle + safe_angle:
            angle = self.minAngle + safe_angle
        self.currentAngle = angle
        self.updateServo()

ロボットアームとの連携

あとは物体の動きに合わせてアームを動かす実装です。初めは動いた方向と距離からサーボを回転させる角度を計算すれば良いだろうと考えてました。 実際やってみると、それでは物体が動いた方向にアームを動かした後、ロボットのカメラにとっては物体は中央に戻ったように見えるわけで、ロボットアームは行ったり来たりと誤動作(ピンポン動作)を繰り返します。また角度を割り出す方法も簡単ではありませんでした。

そこで考えを改めてフレームに一定の境界を設定して、それを超えたらその方向に少しずつアームを動かす単純な実装にしました。結果として赤枠内(上の画像参照)に収まる動きが実現できます。赤枠はフレームの端から余白をどれくらいとるのか frame_margin というパラメーターで調整しています。

  • config.ini
[camera]
# define window boundary at which robot arm starts moving.
# margin window is drawn red line in the frame.
frame_margin = 0.12

それでもアームが動きすぎるとピンポン動作が発生しますし、角度が小さすぎすると動作が緩慢になるためちょうどいいパラメーターを調整する必要があります。 パラメータは [mearm] セクションで指定できます。

[mearm]
# define move angle per operation (min, max).
base_by = (4, 6)
upper_by = (4, 6)
lower_by = (4, 6)

その他技術的な解説

ラッキング処理時のフレームの調整

ラッキング処理時に大きい解像度のフレームを処理しようとすると ラズパイではカクツキがでました。 試行錯誤の結果、フレームの解像度は以下のように 320 * 240 (16fps)にしています。

[camera]
# deifne frame resolution and frame rate.
# (320 * 240  16fps : recommend setting)
frame_prop = (320, 240, 16)

前後のアームの動作

アームの前後の動きは、カメラに映る物体の大きさによってコントロールします。

まず基準となるウィンドウサイズ (track_area) を定義します。次にOpenCVが返す物体検知の青枠面積 (track_window_area) を求めます。 最後にそれぞれの面積の平方根の割合(track_area_ratio)を求め、その値に応じて前後にアームを動かします。

 track\_area\_ratio = \displaystyle \frac{\sqrt{track\_area}}{\sqrt{track\_window\_area}}

参考サイト opencv.blog.jp

Object tracking で camshfit を使う理由

上記と関連しますが、物体検知で使用するアルゴリズムの話です。OpenCVでは、meanshift とそれを拡張した camshift というアルゴリズムがありますが前述の奥行きを判断するためには、camshiftを使います。camshiftでは meanshift で検知した物体のウィンドウサイズを最もフィットするまで更新するため、物体の奥行きの判断に利用できるからです。

https://docs.opencv.org/3.4/db/df8/tutorial_py_meanshift.html

スレッドを使うか否か

サーボモーターにどれくらいコマンドを投げつけて良いのか判断できなかったので、初めはアームの動作はスレッドで並列処理 + wait させることを考えました。 だだ、 0.1秒, 0.05秒くらいのwait間隔でもカクカクした動作になったため、結局はスレッドは使わないことにしました。 スレッドなしだと最大で各フレーム毎にサーボを動作させる処理が投げられることになりますが、可動域を絞っていることもあり、サーボが過度に熱くなることはありませんでした。ジーっていう音はします。

ロックオンでグリップを動かす

MeArmPi には グリップが付いています。せっかくの特徴的な動作なので、検知物体が動かない時間が 2秒経過したらグリップを動かすようにしました。 これは並列処理が必要なので スレッド を使います。

  • mearmlib.py
    def _grip_mearm(self):
        self.time_now = time()
        """update time_delta
           mearm grip starts wheh time_delta exceed 2(sec)
        """
        self.time_delta = self.time_delta + (self.time_now - self.time_old)
        if self.time_delta > 2:
            logger.info(
                "!! grip close !! time_delta:{}".format(self.time_delta))
            if not self.is_test:
                self.myMeArm.moveToGrip(60)
            sleep(0.2)
            logger.info(
                "!! grip open !! time_delta:{}".format(self.time_delta))
            if not self.is_test:
                self.myMeArm.moveToGrip(30)
            sleep(0.2)
            self.time_delta = 0
        self.time_old = self.time_now

    def motion(self, track_window, track_area_ratio, move_ratio, video_prop,
               margin_window):
        """mearm "grip"s object locked on"""
        if move_ratio == (0, 0):
            if not self.grip_t.is_alive():
                self.grip_t = threading.Thread(target=self._grip_mearm)
                self.grip_t.start()
            return

まとめ

この分野は初挑戦でしたが、物体追跡できるロボットができたらと思い立ち試行錯誤の末、自分なりのゴールにたどり着きました。

Youtube などで公開されているロボットの物体追跡の動画をみるときびきびとした動作で精度も素晴らしくどういう仕組みなのか知りたいものがたくさんあります。 きっと今回作成した仕組み自体検討外れなものかもしれませんし、もっとスマートな方法があるんだろうなと思います。

それでも問題を解決する過程や、できたときの達成感を考えると有意義な時間でした。

参考

  • Video Streaming with Flask

https://blog.miguelgrinberg.com/post/video-streaming-with-flask https://github.com/miguelgrinberg/flask-video-streaming