360度深度カメラのPALを使ってROSで自己位置推定してみる

本記事では、360度で深度データが取得可能なPALというカメラとROSを用いて、自己位置推定をやっていきます。
自己位置推定とは、ロボットなどの移動物体が、与えられた地図情報の中で、自身がどこにいるかを認識するための技術です。
IntelのRealsenseという深度カメラはありますが、今回使用するPALカメラは周囲360度での深度が取得可能であり、カメラの死角が少ないため、移動ロボットの周囲認識の用途などに用いることができます。

1.360度の深度が取得可能なPALセンサ

PALはDreamVu社が開発しているカメラで、360度のステレオ画像により深度の計測を行っています。
どのように360度のステレオ画像を取得し、深度計算を行っているかは以下の記事で詳しく説明されています。
https://nanoxeed.co.jp/product/dreamvupal/
本記事では、このセンサを使って実際に部屋の地図を作成して、自己位置推定を行っていきます。

2.PALのシステム構成

PALカメラを使用する際のシステム構成を以下の図に示します。
PALはセンサとなるカメラ本体と、カメラからUSBを経由して取得されたステレオ画像を用いて、Jetsonと呼ばれる計算機上で深度を計算します。
Jetsonは画像処理を行うためのGPUという計算機を搭載した小型のパソコンです。Jetson上に既にUbuntuやROSがインストールされているため、PALを使用する際はJetsonにインストールされたUbuntuからアプリやROSノードの立ち上げを行います。

3.Rvizでセンサデータを見てみる

まずはROSのRvizを使用してPALでどのようなどのようなデータが取得できるか確認してみます。
Jetsonの中でターミナルを開いて、roscoreを立ち上げます。

$ roscore

次にカメラのパラメタ設定を行います。別のターミナルで以下を実行します。

$ source ~/catkin_ws/devel/setup.bash
$ rosparam load ~/catkin_ws/src/dreamvu_pal_navigation/laser_filter.yml

最後にROSのアプリケーションを実行します。

$ cd DreamVu/PAL
$ ./ros_demo.sh

PALのセットアップ状態によっては上記ディレクトリが存在しない可能性があるので、その場合は適宜ファイルの検索などを行って実行して下さい。
実行すると以下のような表示がされ、入力を求められるので”1”を入力し、Enterを押して下さい。
すると以下のようにRvizのウィンドウが立ち上がり、3Dで可視化されたカメラデータが表示されます。

ウィンドウの右側が深度画像から作られた点群を3D上に描画したもので、マウスを使って3D操作しながらデータを見ることができます。
左上の画像が取得された360度画像で、床面の認識を行っており、床面が青で色付けされているのが分かります。
その下の画像が360度の深度画像になっており、赤くなるにつれ近いところ、青くなるにつれ遠いところを示しています。
次の章以降では、ros_demoは落としてもらって、roscoreは立ち上げたままの状態で進めて下さい。

４.Gmappingで地図を作ってみる

自己位置推定は、例えば家の中を移動するロボットなどにおいて、ロボットが家の中のどの位置にいるかを認識する技術です。ロボットがある部屋から別の部屋に移動をする際に、どの方向に進むかを決めるためには、その家の形状とロボット自身の位置を知る必要があります。
ここでは自己位置推定を行うために、まず最初に部屋の形状の計測(地図生成)を行います。
PALのJetsonの中には、既に地図生成のためのgmappingというROSのアプリケーションが用意されており、これを使って地図生成を行います。
gmappingを使うためにカメラから取得される距離情報以外にオドメトリという情報が必要になります。オドメトリとはカメラ本体の相対的な移動量のことを言い、移動ロボットなどであれば、車輪の回転量を計測してそこから移動量の推定を行います。
今回はカメラ単体でオドメトリを取得するためにlaser_scan_matcherというROSのパッケージを使用します。
このパッケージはカメラ等から取得したスキャンデータの時間的な差分からオドメトリを推定するモジュールになります。

$ sudo apt get install ros-melodic-laser-scan-matcher

まずはパッケージのインストールを行います。
$ sudo apt get install ros-melodic-laser-scan-matcher
laser_scan_matcher用のlaunchファイルを作成します。以下の用にlaser_odometry.launch

$ cd ~/catkin_ws/src/dreamvu_pal_navigation
$ touch laser_odometry.launch

ファイルを作成します。
$ cd ~/catkin_ws/src/dreamvu_pal_navigation
$ touch laser_odometry.launch
laser_odometry.launchをnanoエディタなどで開き、以下のように内容を編集します。

<?xml version="1.0"?>
<launch>
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.0 0 0 0.0 0.0 0.0 /base_link /pal 100" />
  <node pkg="laser_scan_matcher" type="laser_scan_matcher_node" name="laser_scan_matcher_node" output="screen">
    <param name="fixed_frame" value = "/odom"/>
    <param name="base_frame" value = "/base_link"/>
    <param name="use_cloud_input" value="false"/>
    <param name="publish_tf" value="true"/>
    <param name="publish_odom" value="true"/>
    <param name="use_odom" value="false"/>
    <param name="use_imu" value="false"/>
    <param name="use_alpha_beta" value="true"/>
    <param name="max_iterations" value="20"/>
    <param name=”max_angular_correction_deg” value=”30”/>
   </node>
 </launch>

これでgmappingを実行する準備ができました。ターミナルを2つ立ち上げ、それぞれ、以下のコマンドを実行します。

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch dreamvu_pal_navigation laser_odometry.launch

ターミナル1

$ source ~/catkin_ws/devel/setup.bash
$ roslaunch dreamvu_pal_navigation gmapping.launch

ターミナル2
$ source ~/catkin_ws/devel/setup.bash
$ roslaunch dreamvu_pal_navigation gmapping.launch
立ち上がると以下のようなRvizのウィンドウが表示されます。
Rvizの下の画像が部屋の中を上から見たときの絵になっており、赤いラインが壁の位置、黒いラインが推定された地図を示しています。

カメラをなるべく水平の状態でゆっくり前後左右に動かしてみてください。

動きに合わせて黒いラインで示された地図が更新されていくのが分かるかと思います。

ここで、今回立ち上げたRvizの設定だとカメラの動きと同時に地図が動いてしまうので、以下のようにメニューバーから「Panels」→「Displays」を選択し、新しく左下に出てきた「Displays」の”Global Options”→“Fixed Frame”の値を”map”に変更してください。

概ね部屋の部屋の形が撮れてそうであれば、別ターミナルで以下のコマンドを打つことで、地図をファイルに保存します。

$ source ~/catkin_ws/devel/setup.bash
$ rosrun map_server map_saver -f mymap

実行するとホームディレクトリにmymap.pgmとmymap.yamlファイルが保存されます。

5.AMCLで自己位置推定してみる

Gmappingで作成した地図(mymap.pgmおよびmymap.yaml)を用いて自己位置推定を行っていきます。
自己位置推定で用いるROSアプリケーションはAMCLというモジュールになります。
地図生成でも使用したlaser_scan_matcherをここでも使用します。ターミナルを2つ立ち上げ、一方はlaser_scan_matcherを立ち上げ、もう一方は以下のコマンドでAMCLを立ち上げます。アプリケーションの引数に先ほど作成した地図のファイルのパスを指定します。

$ roslaunch dreamvu_pal_navigation amcl.launch map_file:=”/home/dreamvu/mymap.yaml”

Rvizのウィンドウが表示されたら、地図とカメラから取得されるスキャンデータが以下のように表示されます。

この画像を見ると、地図とスキャンデータが合っていないことが分かります。自己位置推定を行う場合、カメラの初期位置に対しては人が与えてあげる必要があります。カメラの初期位置の設定はRvizを使って次の手順で行うことができます。

1. まず先程の地図生成でも行った”Fixed Frame”を”map”にする設定を行う。
2. さらにメニューバーにある「2D Pose Estimate」をクリックし、マップ上で以下のように現在のカメラ位置と向きを教える。
3. カメラ位置が移動し、移動した位置からスキャンデータが描画される。
4. 地図とスキャンデータが合うまで2、3を繰り返す。

「2D Pose Estimate」の選択

現在のカメラ位置と向きの指示

これでAMCLのセットアップが完了したので、カメラをなるべく水平を維持させながら動かしてみます。

すると以下のように、地図上でカメラの位置(base_link)が動いている様子が表示されます。

6.まとめ

PALという360度方向で深度が取得できるカメラを用いて、ROSを使った自己位置推定を行ってみました。

通常はLiDARで周囲の距離をスキャンして自己位置推定を行うことが多いのですが、PALはLiDARと同様に360度のスキャンが可能であり、加えて360度の画像や3次元的な深度データも同時に取得できるので、LiDARに比べ多くの情報を取得できます。

この記事を読んで、このカメラ面白いなと感じていただけた方はぜひ下記リンクよりお問い合わせ下さい。

360度3Dカメラ　PALシリーズ