【自律型ロボット製作記】#8 amclとlaser_scan_matcherで自己位置推定を試す
どうもこんにちは、
現在開発中の自律型ロボットは、SLAM（HectorSLAM)を使い、自律走行＋地図作成を同時に行っていました。
ただし、室内における実運用を考えると、以下の様な課題がありました。
・ある程度地図は固定されているのに毎回地図を作成するのが面倒。
・中間経路や最終ゴールを毎回指定するのは面倒。
ということで、今回は
目次
amclとは？
amcl(adaptive Monte Carlo localization)は、2Dで移動するロボットのための確率的な位置推定システムです。 パーティクルフィルタ（またはKLDサンプリング）を使用したモンテカルロ法を用いて、既知のマップに対するロボットの姿勢の追跡を行います。このノードは、Andrew Howardの「amcl」ドライバーから派生したものとなります。
引用:Navigation Stack-amcl-
詳細は勉強中ですが、パーティクルフィルタを使うことで、ノイズにガウス性を仮定する必要がないため、想定外のノイズに強いアルゴリズムのようです。（その代わりカルマンフィルタベースのアルゴリズムよりも計算量が多い）
※カルマンフィルタベースのHector SLAMはノイズにガウス性を仮定しないといけない。
amcl+laser_scan_matcherによる自己位置推定
このamclとlaser_scan_matcherを組み合わせることで、事前に作成したマップ上での自己位置推定が可能です。(RPLiDARのみで推定します）
TF座標変換としては、以下の様になります。
map—(amcl)—>odom—(laser_scan_matcher)—>base_link
※amclでodomの初期位置を補正され、補正されたodomに対して、laser_scan_matcherでbase_linkが推定されていくようです。
動かしてみる
動作環境
システム構成は以下です。
・センサー：RPLiDAR A1M8 レーザースキャナ
・Ubuntu18.04
・ROS melodic （最近、Ubuntu,ROSをアップデートしました）
※ROS hector_slamが動作する自律ロボットで動かす。
（詳細は、過去の自律ロボット製作記記事を参照ください）
「ロボット製作記」に関する記事
laser_scan_matcherのインストール
laser_scan_matcherをインストールします
amclのインストール
amclをインストールします
事前にHectorSLAMでマップを生成
事前にHectorSLAMでマップを作成し、マップファイルを保存しておきます。
laser_scan_matcherのlaunchファイルを作成
laser_scan_matcher用のlaunchファイル（laser_odometory.launch)を作成します。
map座標系からodomは、amcl側で座標変換されるので、laser_scan_matcherのfixed_frameは、/odomを指定します。（7行目）
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
<?xml version="1.0"?> <launch> <!-- <node pkg="tf" type="static_transform_publisher" name="tf_base_link_to_laser" args="0.0 0 0 0.0 0.0 0.0 /base_link /laser 100" /> --> <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 /laser 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="10"/> </node> <node pkg="rviz" type="rviz" name="rviz" args="-d $(find navigation2d_example)/rviz_cfg/rviz_cfg.rviz"/> </launch> |
amclのlaunchファイルを作成
amcl用のlaunchファイル(amcl.launch)を作成します。
各パラメータの詳細は調査中です。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 |
<?xml version="1.0"?> <launch> <node pkg="amcl" type="amcl" name="amcl"> <!--<remap from="scan" to="base_scan"/>--> <param name="odom_model_type" value="omni"/> <param name="odom_alpha5" value="0.1"/> <param name="transform_tolerance" value="0.2" /> <param name="gui_publish_rate" value="1.0"/> <param name="laser_max_beams" value="30"/> <param name="min_particles" value="500"/> <param name="max_particles" value="5000"/> <param name="kld_err" value="0.05"/> <param name="kld_z" value="0.99"/> <param name="odom_alpha1" value="0.2"/> <param name="odom_alpha2" value="0.2"/> <param name="odom_alpha3" value="0.8"/> <param name="odom_alpha4" value="0.2"/> <param name="laser_z_hit" value="0.5"/> <param name="laser_z_short" value="0.05"/> <param name="laser_z_max" value="0.05"/> <param name="laser_z_rand" value="0.5"/> <param name="laser_sigma_hit" value="0.2"/> <param name="laser_lambda_short" value="0.1"/> <param name="laser_lambda_short" value="0.1"/> <param name="laser_model_type" value="likelihood_field"/> <param name="laser_likelihood_max_dist" value="2.0"/> <param name="update_min_d" value="0.2"/> <param name="update_min_a" value="0.5"/> <param name="global_frame_id" value="/map"/> <param name="base_frame_id" value="/base_link"/> <param name="odom_frame_id" value="odom"/> <param name="resample_interval" value="1"/> <param name="transform_tolerance" value="0.1"/> <param name="recovery_alpha_slow" value="0.0"/> <param name="recovery_alpha_fast" value="0.0"/> </node> </launch> |
map_serverのlaunchファイルを作成
map_server用のlaunchファイル（map_server.launch）を作成します。
argsには、事前にhectorSlamで作成したマップファイルを指定します。
|
1 2 3 4 |
<?xml version="1.0"?> <launch> <node name="map_server" pkg="map_server" type="map_server" args="/home/jetson/slam_map/mymap.yaml"/> </launch> |
各launchファイルを実行
各launchファイルを実行して動かしてみます。
まず、roscoreの立ち上げ、rplidarノードを立ち上げます
今回作成したlaunchを立ち上げていきます。
$ roslaunch laser_odometry.launch
$ roslaunch amcl.launch
rvizを立ち上げ、デバッグします。
PoseArrayを追加し、Topicにparticlecloudを指定すると、パーティクルフィルタを可視化できます。
以下のようにロボットを動かしていくと、パーティクルフィルタが収束していき
位置が推定されていくことが確認できました。
参考
以下サイトを参考にさせていただきました。ありがとうございます。
LiDARと地図情報を使った自己位置推定①
これから
amcl+laser_scan_matcherである程度自己位置が可能なことがわかりました。
今後の課題としては、
AMCL+laser_scan_matcherでの自宅内での自己位置推定の精度を検証する。（hector_slamと比べてどうか？など。）
AMCLの原理理解、パラメータ調整の調査をやっていく予定です。
あと、laser_scan_matcherは、IMU（慣性センサー）を使うことで精度の向上ができるようなので、IMUなども試してみます。
スポンサーリンク
