Petoi Bittle 入門 (16) - BittleとRaspberry Pi 4によるROS+SLAM

npaka

2021年6月5日 10:36

以下の記事を参考に書いてます。

・SLAM with ROS Using Bittle and Raspberry Pi 4

前回

1. はじめに

今回のテーマは、「ROS」と「SLAM」によるマッピング（地図作成）です。使用するのは、先月のKickstarterキャンペーンで大成功を収めたPetoi社の四足歩行ロボット「Bittle」です。

2. SLAM

「SLAM」(Simultaneous Localization and Mapping)は、コンピュータで空間の2Dまたは3Dの地図を作成し、その中での自分の位置を特定するための一連のアルゴリズムです。「SLAM」だけではナビゲーションできませんが、地図を持ち自分の位置を知ることは、A地点からB地点へのナビゲーションのための必須条件になります。

様々なセンサーを使って、環境に関するデータを取得し、地図作成に利用することができます。

・レーザースキャナー（1次元、2次元レーザー距離計）
・カメラ（単眼、ステレオ、RGB-D）
・ソナーセンサー
・触覚センサー
・その他

実際には、複数のセンサーを組み合わせて使用し、その後、拡張カルマンフィルターなどの融合アルゴリズムを適用して、正確な情報を取得します。

多くのアプリケーションでは、「LiDARベースのSLAM」か「Visual SLAM」のどちらかを扱うことになります。

「LiDARベースのSLAM」は設定が比較的簡単で、精度も高いです。しかし、「LiDAR」はかさばり、高価で、回転部分があるので長時間の運用にはメンテナンスが必要です。「Visual SLAM」は、RGB-Dセンサーによるアプローチも非常に堅牢ですが、単純なステレオや単眼のシステムではセットアップに手間がかかります。

「SLAM」の詳細については、以下のリンクを参照してください。

・ローカライズとマッピングの同時進行とは?
・LSD-slamとORB-slam2、文献に基づいた解説。
・RPLIDARとROSプログラミング-ロボットを作るための最良の方法。

今回は、「ORB-SLAM2」と呼ばれる「Monocular Visual SLAM」アルゴリズムと、「LiDARベースのHector SLAM」を試してみます。

3. ORB-SLAM2によるVisual SLAM

「ORB-SLAM2」は、一般的の安価なWebカメラを使用します。キャリブレーションが必要で、「ROS」に内蔵されているカメラキャリブレーションツールを利用することをお勧めします。

◎ ツールのインストール
このツールのインストール手順（Ubuntu）は、次の通りです。

sudo apt-get install ros-melodic-camera-calibration

キャリブレーション用のチェッカーボードは、ここからダウンロードして印刷してください。正方形の辺をミリ単位で測ります。

◎ キャリブレーションの実行
以下のコマンドでキャリブレーションを開始します。

roslaunch usb_cam usb_cam.launch
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.108 image:=/camera/image_raw camera:=/camera

キャリブレーションボードの正方形のサイズに合わせて、パラメータを変更します。

　・Xバー : 視野内の左右
　・Yバー : 視野内の上下
　・Size : カメラの方向、距離、傾き

適切にキャリブレーションするためには、カメラフレーム内でチェッカーボードを、次のように移動させる必要があります。

(1) チェッカーボードを上下左右に調整。
(2) 視野いっぱいにチェッカーボードを広げる。
(3) チェッカーボードの傾きを調整。

各ステップでは、キャリブレーションウィンドウに画像がハイライトされるまでチェッカーボードを静止さてください。

アプリケーションに十分なデータが集まったら、「Calibrate」ボタンを押します。キャリブレーションの処理には数分かかるので、しばらくお待ちください。キャリブレーションが成功すると、実世界の直線的なエッジが、補正後の画像ではまっすぐに表示されます。キャリブレーションに失敗すると、通常、画像が空白になったり、認識できなくなったり、直線的なエッジが維持されない画像になります。

◎ パッケージの準備
このパッケージを使って、カメラパラメータをyaml形式に変換し、「head_camera.yaml」にリネームして、「.ros/camera_info/」フォルダに配置します。

ORB-SLAM2をROSに統合したパッケージがあり、2Dの占有マップも公開されています。インストールは非常に複雑です。「Raspberry Pi」用の「Ubuntu 18.04」イメージを出発点にすると、多くの追加パッケージをコンパイルする必要がなくお勧めです。

ROSデスクトップと必要な依存パッケージのインストールは、次のとおりです。

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt update
sudo apt install ros-melodic-desktop
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source  ~/.bashrc
sudo apt-get install ros-melodic-pcl-ros ros-melodic-image-geometry ros-melodic-octomap-ros ros-melodic-usb-cam

「catkin workspace」を作成し、「catkin build tools」をインストールし、「ORB_SLAM2_ROS」リポジトリと「Bittle driver」リポジトリをcatkin_ws/srcフォルダにクローンします。

mkdir -p catkin_ws/src && cd catkin_ws/src
git clone https://github.com/rayvburn/ORB-SLAM2_ROS
git clone https://github.com/AIWintermuteAI/bittle_ROS
cd bittle_ROS && git checkout slam

Vocabularyファイルをダウンロードして、「ORB_SLAM2/orb_slam2_lib/Vocabulary」フォルダに入れる

wget https://github.com/raulmur/ORB_SLAM2/raw/master/Vocabulary/ORBvoc.txt.tar.gz

そして、「catkin workspace」フォルダで、次のコマンドを入力します。

cd src/ORB-SLAM2_ROS/ORB_SLAM2
sudo chmod +x build*
./build_catkin.sh
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source  ~/.bashrc

コンパイルがフリーズする場合は、スワップサイズを2GBに増やしてください。

sudo swapoff -a
sudo fallocate -l 2G /swapfile
sudo chmod 600 /swapfile
sudo mkswap /swapfile
sudo swapon /swapfile
grep SwapTotal /proc/meminfo

スワップファイルが不要な場合は、後で削除することができます。インストールが成功したら、サンプルを実行して、動作することを確認してください。

roslaunch orb_slam2_ros raspicam_mono.launch

「Raspberry Pi」をヘッドレスモードで動作させている場合、スクリーンやキーボードを使用していない可能性が高いため、追加のステップが必要です。そのため、ROSを複数のマシンで動作するように設定する必要があります。このプロセスの詳細については、BIttleの前回の記事を参照してください。

Bittleのドライバは「Python 3」で書かれており、ROSはデフォルトで「Python 2.7」を使用しているので、Python 3用の「rospkg」をインストールして、両者を連携させる必要があります。

pip3 install rospkg

「ORB-SLAM2」とBittleのパッケージ、Webカメラのドライバがインストールされたら、次のように実行できます。

roslaunch bittle_driver bittle_vslam_robot.launch

これにより、ロボットドライバー、Webカメラノード、「ORB-SLAM2」といったシステム全体が立ち上がりましす。「ORB-SLAM2」は初期化するために環境に関する十分な情報を必要としますので、手動でロボットを動かして、移動や向きが大きく変化しないようにします。「ORB-SLAM2」は初期化後、Octomapの公開を開始します。コントロールを使ってロボットを動かすことができます。

残念ながら、Bittleのカメラは旋回中に高速で動くため、キーポイントを見失いがちで、前の位置に戻る必要があることがわかりました。

安定させるためには、いくつかの改善点があります。

・ステレオカメラの使用。
・「ORB-SLAM3」では、IMUデータを統合することで、より正確な位置決めが可能。

4. Hector SLAMによるLiDARベースのSLAM

「Visual SLAM」がうまくいかなかったら、「LiDAR」を導入して、レーザースキャナーベースのアルゴリズムを試してみてはどうでしょうか? ここでの良いニュースは、「LiDAR」には処理速度は必要ないので、古い「Raspberry Pi 3」でも大丈夫だということです。悪い点は、小型の「LiDAR」でもサイズが大きいことです。手元にあった「RPLIDAR A1M8」は190gもあり、これを足の生えたロボットの上に取り付けると、重心がずれて歩行に影響が出てしまいました。

お腹の下に重りを追加してバランスを取った後は、急に止まらないように気をつけながらも、這ったり歩いたりすることができました。

「Hector SLAM」のインストールは、「Ubuntu 18.04」で簡単にできました。まだ「ROS Desktop」をインストールしていない場合は、以下のコマンドでインストールしてください。

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt update
sudo apt install ros-melodic-desktop
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
source  ~/.bashrc
sudo apt-get install ros-melodic-hector-slam

catkinワークスペースを作成し、catkinビルドツールをインストールし、RPLIDARリポジトリとBittleドライバリポジトリをcatkin_ws/srcフォルダにクローンします。

mkdir -p catkin_ws/src && cd catkin_ws/src
git clone https://github.com/Slamtec/rplidar_ros.git
git clone https://github.com/AIWintermuteAI/bittle_ROS
cd bittle_ROS && git checkout slam

Bittleドライバパッケージをビルドし、catkinワークスペースをソース化します。

catkin build
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source  ~/.bashrc

Bittleドライバは「Python 3」で書かれており、ROSはデフォルトで「Python 2.7」を使用しているので、両者を連携させるためには「Python 3」用のrospkgをインストールする必要があります。

pip3 install rospkg

これらが全てインストールされたら、ROSが複数のマシンで動作するように設定します。。

roslaunch bittle_driver bittle_lslam_robot.launch

「LiDAR」「ロボットコントロール」「Hector SLAM」のノードを立ち上げます。全体的なマッピング結果は「ORB-SLAM2」よりもはるかに優れており、「Hector SLAM」はオドメトリとパスメッセージをパブリッシュすることもできるので、ROSナビゲーションスタックを使った自律的なナビゲーションの実行への道が開けました。

「Bittle」で「LiDAR」を使用する際の改善点は、次のとおりです。

・IMUのデータも統合可能。
・歩行と直立のアルゴリズムを調整して、ロボット上部の重量増加に対応。
・よりコンパクトなLiDARを使用。

5. おわりに

以上、Petoi社のロボット犬「Bittle」についてご紹介しました。Kickstarterキャンペーンは終了しましたので、「Bittle」の購入をご希望の方は、Seeed studioの子会社であるTinkerGen社のオンラインショップとAmazonでの販売を予定しているので、そちらをご利用ください。