安価な2DレーザスキャナRPLidar A2とROSでSLAMを試す

記事の概要

本記事は、安価な2DレーザスキャナRPLidar A2の動作確認をします。

まずはwindowsで点群地図を作成できることを確認します。
次にUbuntuでROSを用いて点群地図を作成できることを確認します。
最後にHector SLAMを用いてリアルタイムに地図を作成できることを確認します。

動作確認が目的なので、アルゴリズムの解説や精度を高める調整は行いません。

RPLidar A2とは何か

RPLidar A2は安価な2次元レーザスキャナです。
2次元レーザスキャナはSLAMで必要になります。

性能をAMAZONの商品紹介ページから以下に引用します。

• 距離範囲：0.15-12m(反射率70％の白いオブジェクトに基づく)
• 角度範囲：0-360度
• 距離分解能：<0.5mm
• 角度分解能：0.45〜1.35度、通常0.9度
• サンプル期間：0.25ms
• サンプルレート：2000~8000Hz,Typical 4000Hz
• スキャンレート：5~15Hz,Typical 10Hz

SLAMの種類

ロボットや自動運転車が移動する為には、地図の作成と自分がどこにいるかの自己位置推定が必要です。これを実現する手段がSLAMです。

SLAMには以下の種類があります。

• 2D/2D型
  • ロボット位置：2次元3自由度
  • 地図：2次元
  • センサ：2Dレーザースキャナ
• 2D/3D型
  • ロボット位置：2次元3自由度
  • 地図：3次元
  • センサ：2D/3Dレーザスキャナ
• 3D/3D型
  • ロボット位置：3次元6自由度
  • 地図：3次元
  • センサ：3Dレーザスキャナ

windowsでの動作確認

準備

SDKのインストール

RPLidarA2サポートページのSDK and FirmwareのRPLIDAR SDKのDownloadをクリックします

USBデバイスドライバのインストール

USBをPCに接続し、デバイスマネージャーでPORT番号を確認してください。

USBを認識しない場合は、USBデバイスドライバがインストールされていないのかもしれません。その時はSDKのtoolフォルダ内のcp2102_driverフォルダにあるCP210xVCPInstaller_x64.exeを実行し、ドライバをインストール後に、再度試してみてください。

同じドライバはCP210x USB to UART Bridge VCP Driversからもインストールできます。

動作確認

RPLidar toolページのframe_grabber.exeをダウンロードします

frame_grabber.exeを実行し、デバイスマネージャーで確認したPORT番号に設定し、通信速度を115200にします。

起動後、以下の画面が表示されます。赤い枠で囲ったボタンをクリックするとRPLidarが回転を開始します。

正常に動作すれば、以下のように点群地図を作成します。

Ubuntuでの動作確認

次はROSを用いてRPLidarの動作確認をします。

準備

https://github.com/robopeak/rplidar_ros.git をコピーして、ビルドします。
ここではビルドにcatkin buildを使用していますが、catkin_makeでも問題ありません。

catkin buildの設定方法はiRobot Create 2でROSを勉強する（１）準備をご覧ください。

$ cd ~/catkin_ws/src
$ git clone https://github.com/robopeak/rplidar_ros.git
$ cd ~/catkin_ws
$ catkin build rplidar_ros
$ source devel/setup.bash

動作確認

ターミナルを新規に開いて以下を実行します

$ roscore

"roslaunch rplidar_ros"を入力してTABをクリックすると、launch候補の一覧が確認できます。

$ cd ~/catkin_ws
$ roslaunch rplidar_ros 
rplidar_a3.launch       test_rplidar_a3.launch  view_rplidar_a3.launch
rplidar.launch          test_rplidar.launch     view_rplidar.launch

RPLidarが点群地図を作成するのを確認したい場合は、view_rplidar.launchを実行します。

$ roslaunch rplidar_ros view_rplidar.launch

RVizが起動し、以下の画面が表示されます。

ここでtopicは/scanになっています。
これは以下のコマンドでリスト一覧を表示させて確認することもできます。

$ rostopic list
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/scan
/tf
/tf_static

RPLidarが取得しているデータは以下のコマンドで確認できます。

$ rostopic echo /scan

以下のデータが取得できます。
データの詳細はhttp://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html をご覧ください。

もしくは「RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_008日目_SLAM_GMapping_LiDAR(A1M8)のLiDARスキャンデータの意味を調べる」をご参照ください。

header: 
  seq: 996
  stamp: 
    secs: 1576877613
    nsecs: 741686250
  frame_id: "laser"
angle_min: -3.12413907051
angle_max: 3.14159274101
angle_increment: 0.0174532923847
time_increment: 0.000267035182333
scan_time: 0.0958656296134
range_min: 0.15000000596
range_max: 12.0
ranges: [inf, 0.3140000104904175, inf, 0.3109999895095825, 0.30399999022483826, inf, 0.3019999861717224, 0.3059999942779541, 0.3409999907016754, 0.3199999928474426, 0.30799999833106995, 0.3019999861717224, 0.31200000643730164, 0.30799999833106995, 0.296999990940094, 0.2879999876022339, 0.28700000047683716, 0.289000004529953, 0.289000004529953, 0.2879999876022339, inf, 0.2879999876022339, 0.2930000126361847, inf, 0.30399999022483826, 0.3149999976158142, 0.3140000104904175, inf, 0.3100000023841858, inf, 0.3149999976158142, inf, 0.3149999976158142, 0.31299999356269836, inf, 0.3140000104904175, inf, inf, 0.18700000643730164, 0.18400000035762787, 0.18000000715255737, 0.1770000010728836, 0.17399999499320984, 0.17100000381469727, 0.16899999976158142, 0.16599999368190765, 0.164000004529953, 0.16200000047683716, 0.16200000047683716, 0.16500000655651093, 0.16899999976158142, inf, inf, inf, inf, inf, inf, 0.4650000035762787, 0.4180000126361847, 0.4169999957084656, 0.4230000078678131, 0.414000004529953, 0.41100001335144043, 0.41200000047683716, 0.41499999165534973, 0.4180000126361847, 0.42100000381469727, 0.4259999990463257, 0.4309999942779541, 0.4429999887943268, 0.49300000071525574, 0.5099999904632568, 0.5080000162124634, 0.5040000081062317, 0.5009999871253967, 0.5, 0.5, 0.5009999871253967, 0.5070000290870667, 0.5370000004768372, 0.5429999828338623, inf, 0.5680000185966492, 0.5830000042915344, inf, inf, 0.33000001311302185, 0.3240000009536743, 0.3140000104904175, 0.3100000023841858, 0.3059999942779541, 0.3009999990463257, 0.2930000126361847, inf, 0.289000004529953, 0.2849999964237213, 0.28299999237060547, 0.2750000059604645, 0.27300000190734863, 0.26899999380111694, 0.2669999897480011, 0.2619999945163727, inf, 0.25600001215934753, inf, 0.2549999952316284, 0.2529999911785126, 0.24799999594688416, inf, 0.24699999392032623, 0.24199999868869781, 0.24300000071525574, inf, 0.2409999966621399, 0.23999999463558197, inf, 0.26499998569488525, 0.2840000092983246, 0.30000001192092896, inf, inf, inf, inf, 0.3790000081062317, inf, inf, inf, inf, inf, inf, inf, inf, 0.8989999890327454, 1.0420000553131104, inf, 0.609000027179718, 0.6290000081062317, inf, inf, 0.6589999794960022, 0.6359999775886536, 0.5789999961853027, 0.5759999752044678, 0.5830000042915344, inf, 0.5870000123977661, 0.593999981880188, 0.6230000257492065, 0.6650000214576721, inf, 0.4659999907016754, 0.4519999921321869, 0.4480000138282776, inf, 0.38999998569488525, 0.38499999046325684, 0.3869999945163727, 0.3919999897480011, 0.4020000100135803, 0.4090000092983246, 0.4230000078678131, 0.4339999854564667, 0.44200000166893005, 0.4390000104904175, 0.44999998807907104, inf, 0.5509999990463257, 0.5559999942779541, 0.5580000281333923, 0.5519999861717224, 0.5509999990463257, 0.5640000104904175, 0.5680000185966492, 0.5609999895095825, 0.5509999990463257, 0.5450000166893005, 0.5419999957084656, 0.5440000295639038, 0.5429999828338623, 0.5440000295639038, 0.5460000038146973, 0.5509999990463257, 0.5550000071525574, 0.5720000267028809, 0.5849999785423279, inf, inf, inf, 2.565999984741211, 2.553999900817871, 2.549999952316284, 2.5260000228881836, 2.503999948501587, 2.500999927520752, 2.493000030517578, 2.4649999141693115, 2.4779999256134033, 2.446000099182129, 2.259999990463257, 2.1510000228881836, 2.128000020980835, 2.1410000324249268, 2.1579999923706055, 2.138000011444092, 2.1510000228881836, 2.1549999713897705, 2.1530001163482666, 2.186000108718872, 2.3259999752044678, 2.361999988555908, 2.4179999828338623, 2.433000087738037, 2.443000078201294, 2.4519999027252197, 2.4590001106262207, 2.4590001106262207, 2.4679999351501465, 2.4719998836517334, inf, 0.925000011920929, 0.921999990940094, 0.9520000219345093, 0.9539999961853027, 0.9890000224113464, 0.9990000128746033, 1.0010000467300415, 0.9980000257492065, 0.9819999933242798, 0.9369999766349792, 0.9350000023841858, 0.9929999709129333, 0.9929999709129333, 1.069000005722046, 1.0700000524520874, 1.0709999799728394, 1.0670000314712524, 1.062999963760376, 1.0759999752044678, 1.0839999914169312, 1.0880000591278076, 1.2120000123977661, 1.2009999752044678, inf, inf, 1.1929999589920044, inf, 3.1110000610351562, 3.046999931335449, 3.0160000324249268, 2.9509999752044678, 2.8980000019073486, 2.7880001068115234, 2.7279999256134033, 2.693000078201294, 2.630000114440918, 2.6059999465942383, 2.572000026702881, 2.565999984741211, 2.5450000762939453, 2.5, 2.4670000076293945, 2.4489998817443848, 2.4140000343322754, 2.375999927520752, 2.3550000190734863, 2.3399999141693115, 2.322000026702881, 2.306999921798706, 2.2820000648498535, 2.259999990463257, 2.244999885559082, 2.2219998836517334, 2.2019999027252197, 2.190000057220459, 2.1760001182556152, 2.1570000648498535, 2.1440000534057617, 2.174999952316284, inf, inf, 0.5090000033378601, 0.5059999823570251, 0.503000020980835, 0.5, 0.49900001287460327, 0.4950000047683716, 0.49399998784065247, 0.492000013589859, 0.47699999809265137, 0.4749999940395355, 0.48399999737739563, inf, 0.48100000619888306, inf, 0.6389999985694885, inf, 0.4830000102519989, 0.4819999933242798, 0.5080000162124634, 0.48100000619888306, 0.4880000054836273, 0.4950000047683716, 0.5139999985694885, 0.48399999737739563, 0.5049999952316284, inf, inf, 0.5709999799728394, inf, 0.5, 0.6159999966621399, 0.6110000014305115, 0.6159999966621399, 0.6079999804496765, 0.6060000061988831, 0.6069999933242798, inf, inf, inf, 0.8220000267028809, inf, inf, inf, inf, 0.5230000019073486, 0.5139999985694885, 0.5239999890327454, 0.5220000147819519, inf, inf, inf, inf, inf, inf, 0.6489999890327454, 0.6359999775886536, 0.6230000257492065, 0.6119999885559082, 0.6000000238418579, 0.5889999866485596, inf, inf, 0.3919999897480011, 0.3970000147819519, 0.37700000405311584, 0.3720000088214874, 0.37700000405311584, 0.3659999966621399, 0.3659999966621399, 0.3630000054836273, inf, inf, inf, inf, inf, inf, inf, inf, 0.3140000104904175, 0.3149999976158142]
intensities: [0.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 0.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 0.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 47.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 47.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 47.0, 47.0]
---

Hector SLAMの動作確認

Hector SLAMはダルムシュタット工科大学が開発した、Scan-Matching +拡張カルマンフィルタのアルゴリズを採用したSLAMです。loop closure機能は無く、オドメトリも使用しません。
オドメトリ情報が不要なので、RPLidar単体での動作確認をするのに適しています。

作業手順は主に以下の動画を参照しました。

RPLidar and Hector SLAM for Beginners | ROS Tutorial #8

準備

https://github.com/tu-darmstadt-ros-pkg/hector_slam.git をコピーして、ビルドします。

$ cd ~/catkin_ws/src
$ git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git
$ cd ~/catkin_ws
$ catkin build hector_slam
$ source devel/setup.bash

Ubuntu PCでは正常にビルドできたのですが、Jetson Nanoではビルドエラーになってしまいました。
現在、原因を調査中です。

ファイルの修正

Special mention to NickL77に従い、以下の修正を行います。

mapping_default.launchの修正

catkin_ws/src/rplidar_hector_slam/hector_slam/hector_mapping/launch/mapping_default.launchを修正します。

54行目の修正

tf broadcasterについてmap navをbase_link laserに変更します。

【修正前】

<!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->

【修正後】

<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100" />

5行目、6行目の修正

base_frameとodom_frameをbase_linkに変更します。

【修正前】

<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="nav"/>

【修正後】

<arg name="base_frame" default="base_link"/>
<arg name="odom_frame" default="base_link"/>

tutorial.launchの修正

catkin_ws/src/rplidar_hector_slam/hector_slam/hector_slam_launch/launch/tutorial.launchを修正します

7行目の修正

use_sim_timeをfalseに設定することで、リアルタイム動作にします。

【修正前】

<param name="/use_sim_time" value="true"/>

【修正後】

<param name="/use_sim_time" value="false"/>

動作確認

ターミナルを新規に開いて以下を実行します。

$ roscore

ターミナルを新規に開き、パッケージrplidar_rosのrplidar.launchを実行し、RPLidarを起動させます。

$ cd ~/catkin_ws
$ roslaunch rplidar_ros rplidar.launch

ターミナルを新規に開き、パッケージhector_slamのtutorial.launchを実行します。

$ cd ~/catkin_ws
$ roslaunch hector_slam tutorial.launch

RVizが起動して、以下の画面が表示されます。
RPLidarを移動させるとリアルタイムに地図を更新します。
緑の線は移動の軌道ですが、上下に動かしたせいか、異常な跳躍をしています。

今後の予定

• Gmappingを試す
• SLAMのアルゴリズムを理解する
  • データだけを取得して、ROSを用いずにSLAMを実装する
• iRobot Create2のオドメトリを併用して精度の高い地図を作成する
• Jetson Nanoで同様のことを行う
  • RPLidar起動中に電源が落ちてしまう問題を解決しないといけない
  • Hector SLAMのビルドに失敗する

参考

• RPLidar and Hector SLAM for Beginners | ROS Tutorial #8
• ネコとロボティクス「低価格 Lidar RPLIDAR A2 を使ってみる」
• ROSのLidarSLAMまとめ
• rplider A2M8をROSで動かす
• RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_008日目_SLAM_GMapping_LiDAR(A1M8)