ロボット自身が自律的に移動したり、アームのようなもので物を掴む場合に、どのような動きをする必要があるでしょうか？
まず当たり前に必要になるのは設定されたゴールに向かっている動きであることです。

このときに、ゴールまでの真っ直ぐな線を進むだけならとても簡単ですが、実際は経路が曲がっていたり、経路上に物が置かれていたりするので、それらを「避けて」動く必要があります。

このような経路の端の壁や経路上の物体はロボットにとって障害物であり、何らかの方法で避ける仕組みを入れておかなければなりません。このような障害物を避けてロボットが動作する経路を計算する仕組みを軌道生成と言います。
経路上の壁や物は静止物なので、一度経路を作ってそれに沿って動けば問題ありません。
しかし、人のような動くものを避ける場合、人の動きに合わせて経路の修正を行いながら動く必要があります。

人のように動くものに対しては移動中、絶えずセンサによって計測し続ける必要があります。このように移動ロボットにおいては、動く障害物を避けるために、センサでの人の検出がとても重要な機能になります。

早速PALを用いた人検出を行っていきます。
システム構成は前回同様、以下のようになっています。
PALのカメラ本体をJetsonというGPUが搭載された小型パソコンに接続することで、Jetson上でGPUを用いた人検出を行う計算が実行できるようになっています。

まずは既存のサンプルとして用意されている、ROSで動く、人検出のサンプルを動かしてみます。
PALの人検出には、People TrackingとPeople Followingという２つのモードがあります。前者はカメラ上に写った人物を検出し、追跡(トラッキング)するモードで、後者は検出した人物の中で、特定の人物のみを追跡し続けるモードです。
今回はPeople Trackingモードで動かします。
Jetson内にログインし、ターミナルを起動して中に用意されているシェルスクリプトを実行します。

以下のような選択肢が表示されるので、4番のpeope_tracking_rvizを選択します。

少し待つと、以下のようなROSのRvizウィンドウが立ち上がります。

Rvizのウィンドウには上から順番に、ステレオの左画像、右画像、人検出結果の画像、深度画像の順に表示されます。
人検出結果の画像を見ると、検出された人物が四角で囲われており、文字が小さいですが、IDが振られているのが分かります。以下のように人物が動いても同じIDで追跡されており、同一人物を検出しているのが理解できています。

3章ではカメラの画像上で人検出の結果を確認しました。実際にロボットなどでの使用を考えた場合、3次元的に人がどこにいるかという情報が重要になってきます。
PALは深度や点群が取得できるところからも分かるようにステレオを用いて3次元での情報を取得することができ、検出した人物に対しても3次元上の位置を取得することができます。
既存のサンプルには検出した人物の3次元的な位置を確認できるようなコードが用意されていないため、3章で使用したPeople TrackingのサンプルをベースにRvizの3D表示上で人物の位置を描画するためのコードを追加しました。
実際のコードはかなり長くなってしまうので、コードの概要を以下に載せます。MarkerArrayというデータに検出結果を格納することでRviz上で表示できるようにします。

これを実行させ、Rviz上でMarkerArrayを表示させてみました。

左側のパネルには3章と同様、ステレオの左右画像と深度画像、検出結果の画像が乗っており、右側に3D空間上の検出した人の位置が赤い点で示されています。
人物が動くと以下のように、人物の軌跡が赤い線で表示されます。

軌跡を見ると少しギザギザしたような軌跡になっているのが分かります。
これは実際は部屋の中をかなり直線的に動いていたのですが、計測では数十cmくらいのブレが出ているように見えました。
ただ、追跡の精度自体は非常に正確で、見失ったり、IDを間違えたりすることはありませんでした。

360度で深度が取得できる3Dカメラ、PALを用いて、人検出結果をRviz上で3次元的に表示し、実際に人が動いたときの軌跡を描画してみました。
360度を一度に見渡しつつ、周囲の人の検出を行うことができるため、移動ロボットなどへの活用が期待できるカメラです。
この記事を読んで、このカメラ面白いなと感じていただけた方はぜひ下記リンクよりお問い合わせ下さい。

最後までお読み頂きありがとうございました。