ssk tech blog

自律移動ロボットの話を書きます

Google Cartographerのパラメータチューニングに関するIssuesまとめ

Robot SLAM Cartographer

目次

はじめに

Google Cartographerのパラメータチューニングに関するIssuesをまとめました．

github.com

ちなみに公式ドキュメントにおけるチューニング・ガイドはこちらです．
cartographer_ros/tuning.rst at master · googlecartographer/cartographer_ros · GitHub

問題

主にIMU・オドメトリに関する問題

Parameter tuning issues for hallways · Issue #48 · googlecartographer/cartographer_turtlebot · GitHub

  • レーザーのURDFが正しくなかった．

  • IMUの性能が悪い．
    対策：IMUを無効にして、代わりに相関スキャンマッチャーに切り替える．

TRAJECTORY_BUILDER_2D.use_imu_data  =  false  
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching  =  true
  • オドメトリの性能が良いのに，SLAMに反映できていない．
    対策：オドメトリをより信頼できるように以下のパラメータを調整する．
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight  =  70  
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight  =  300

誤ったループ閉じ込みのせいで地図が歪む問題

about the global loop-closure optimization between sub-maps · Issue #127 · googlecartographer/cartographer · GitHub

対策：誤ったループ閉じ込みを行わないようにするためにもっと高い最小得点とHuberスケールに設定する．

POSE_GRAPH.optimization_problem.huber_scale = 1e2 --<--1e1
POSE_GRAPH.constraint_builder.min_score = 0.8 --<--0.5

レーザースキャナに関する問題

Cartographer worse than Gmapping · Issue #202 · googlecartographer/cartographer · GitHub

  • レーザーの検出レンジが大きいのに，Cartographerが読み取るレーザーレンジがあっていない．
    対策：Cartographerの設定のレーザーレンジを上げる．
TRAJECTORY_BUILDER_2D.laser_max_range = 50 --<--30
  • レーザーの性能が悪い．
    対策：サブマップの分解能を下げる．
TRAJECTORY_BUILDER_2D.submaps.resolution = 0.15 --<--0.05

LiDAR-SLAM資料まとめ

Robot ROS SLAM

目次

はじめに

こんにちは．ササキ(@saitosasaki)です．
今回はLiDAR-SLAMに関わるSLAMの資料をまとめました。

資料

書籍

  • 確率ロボティクス
    言わずもがな名著です．

下のリンクは原本に関するサイトです。説明スライドが置いてあります。
http://www.probabilistic-robotics.org/

  • SLAM入門
    『確率ロボティクス』は2005年に発売された本なので、カルマンフィルタやパーティクルフィルタのようなベイズフィルタ系のアプローチのSLAMがメインになっています（語弊があるかも）。
    最近（少なくとも屋外では）主流のスキャンマッチングやグラフベースSLAMといった最適化によるアプローチは以下の解説論文が詳しいです。
    移動ロボットの環境認識 —地図構築と自己位置推定
    より詳しくは上の解説論文を書いた方が書いた『SLAM入門』があります。

記事

  • Crafty_as_a_FoxさんのQiita記事

SLAM(Simultaneous Localization and Mapping)と環境計測センサについて - Qiita

Graph-Based SLAMを用いた軌跡推定シミュレーション - Qiita

  • sakai atushiさんのブログmy enigmaの記事

Simultaneous Localization And Mapping (SLAM)について - MyEnigma

Simultaneous Localization And Mapping (SLAM) 技術がもたらす利点 - MyEnigma

EKF SLAMのためのMATLAB, Pythonサンプルプログラム - MyEnigma

ちなみにこの方、趣味で作ったOSS(Open Source Software)がGithubで4000starを超えるという凄い人です。
GitHub - AtsushiSakai/PythonRobotics: Python sample codes for robotics algorithms.

PythonRoboticsはロボットでよく使われるアルゴリズムのPythonでのサンプルコード集で、 ロボティクス初学者におすすめのOSSです。
SLAMにおいてもEKF(Extended Kalman Filter)SLAM、FastSLAM、GraphBasedSLAMが実装されています。

スライド

  • ICRA 2016 Tutorial on SLAM

www.dis.uniroma1.it

https://www.doc.ic.ac.uk/~ajd/Robotics/

  • 確率ロボティクスの翻訳者で千葉工大の上田先生の授業スライド
    『確率ロボティクス』の解説．「ロボットフロンティア（中部大学にて）」は出張授業のようで短くまとまっています．
    https://lab.ueda.tech/?page_id=166

また，上田さんは『確率ロボティクス』のアルゴリズムプログラムをgithubに挙げています． 『確率ロボティクス』の解説書(?)もそのうち出すらしいです（楽しみ）．
GitHub - ryuichiueda/probrobo_practice: 確率ロボティクスのアルゴリズム解説（こちらに最新・もっと正確なバージョンがあります->）

PDF

  • State Estimation

State Estimation for Robotics (399 pages)(リンク先のPDF) http://asrl.utias.utoronto.ca/~tdb/

  • リー群
    A tutorial on SE(3) transformation parameterizations and on-manifold optimization

https://www.researchgate.net/publication/235412302_A_tutorial_on_SE3_transformation_parameterizations_and_on-manifold_optimization

Lie Groups for 2D and 3D Transformations

http://ethaneade.com/lie.pdf

オンライン授業

  • Artificial Intelligence for Robotics | Udacity
    無料で『確率ロボティクス』の著者であるセバスチャン・スラン先生が講義を聞けます。
    初学者でない方にも、各レッスンの最後に行われるQ&Aは非常に知見が得られるのでお勧めです。 SLAMはGraphSLAMの説明だけなのですが、非常にわかりやすいです。

ROS実装のある有名なOSSまとめ

以下ROS実装がある最近有名なLidarベースのSLAMオープンソースソフトウェアとその解説記事・スライドをまとめました。

まとめ表

名前 2D/3D ループ閉じ込み オドメトリ IMU 補足
gmapping 2D 有るが非明示的 必須 必須 ベイスフィルタによるSLAM
LOAM 3D 無し 不必要 可 リアルタイム性がウリ
Cartographer 2D/3D 有り 可 2Dが可。3Dは必須 ループ閉じ込みがウリ
Autoware ndt-mapping 3D 無し 可 可 NDTマッチング(非PCL実装のNDTやICPもあり)
hdl_graph_slam 3D 有り 可 可 NDT/ICP/GICP&ループ閉じ込み
blam 3D 有り 不可 不可
A-LOAM 3D 無し 不可 可
LeGO-LOAM 3D 有り 不可 可
LIO-mapping 3D
interactive_slam 3D GUIによるMap Correctionツール

gmapping

ROSのnavigationパッケージにもあって一番有名だと思います。
Rao-Blackwellised Particle FilterによるSLAMです．
2D。ループ閉じ込みはあるが非明示的。
Github
github.com
論文
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filter

論文著者による解説スライド(英語)

http://www2.informatik.uni-freiburg.de/~stachnis/pdf/rbpf-slam-tutorial-2007.pdf

日本語による解説記事。 qiita.com

LOAM(Lidar Odometry and Mapping in Real-time)

ライダーオドメトリとマッピングを分割したことによるリアルタイム性をウリにしたSLAMです。
3D。リアルタイム性が売り。ループ閉じ込み無し。 オドメトリ・IMU必要なし。IMU複合可。
Github
github.com 論文
LOAM: Lidar Odometry and Mapping in Real-time

解説スライド

www.slideshare.net

Google Cartographer

2D/3D。ループ閉じ込み有り。 2Dはオドメトリ・IMU必要なし（どちらも複合可）。3DはIMU必須。GPSも複合可。ランドマーク複合可．
逐次SLAMがいまいち。
論文では2Dに関してのみですが、実装は3Dもあります。ただし、3Dでは処理が重すぎてリアルタイムにループ閉じ込みできないです。
Github
github.com 論文
Real-Time Loop Closure in 2D LIDAR SLAM
詳しく解説したものは(論文以外)なさげでしたが、概要は以下のブログ記事にあります。
ちなみに下の記事にはHector SLAMの概要もあります。
daily-tech.hatenablog.com

Autowareのndt mapping

3D。ループ閉じ込み無し。オドメトリ・IMU必要なし（どちらも複合可）。
他にもAutowareにはICPや色々な逐次SLAMの実装があります。
Github
github.com

実装者本人による解説スライド。ダウンロードしないと正常に見れないようです。
NDTスキャンマッチング 第1回3D勉強会@PFN 2018年5月27日

hdl_graph_slam

3D．NDT/ICP/GICPから選んだ逐次SLAMとグラフベースSLAM．ループ閉じ込み有り．GPSも複合可。
github.com 論文
A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement

BLAM(Berkeley Localization And Mapping)

Berkeley Localization And Mapping．更新が2016年以降ありません． 3D．ループ閉じ込みあり。　

github.com

A-LOAM

LOAMの派生で、スキャンマッチングをceres-solverで解く等の改良が加えられています。ループとじ込みなし。

github.com

LeGO-LOAM

LOAMの派生です。論文はIROS2018採択。ループ閉じ込みあり。IMU複合可。オドメトリ複合不可。
GitHub - RobustFieldAutonomyLab/LeGO-LOAM: LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

rosgraph f:id:ssk0109:20191221153349p:plain

LIO-mapping

論文の『A Tightly Coupled 3D Lidar and Inertial Odometry and Mapping Approach』はICRA 2019採択。

sites.google.com

github.com

rosgraph

f:id:ssk0109:20191221153252p:plain

interactive_slam

hdl_graph_slamの作者によるGUIでGraph SLAMを修正(ループ箇所を手動or自動で指定して最適化/ Plane-basedで地図の歪みを補正/複数地図の合成)するOSS。 github.com

その他

重複もありますが、他のROS実装のあるSLAMは以下にまとまっています。 ubuntuのバージョンが14.04以降では色々しないと動かない古いものが多く、自分は触ってません。　　

ROSのLidarSLAMまとめ - Qiita

Arduinoを用いてIMUで姿勢推定 on ROS

Arduino ROS Robot

初めに

こんにちは．ササキ(@saitosasaki)です．
ROSにおいてIMUから姿勢推定できるようにしました． IMUの値を読み取るためにArduinoを用いました．
以下、姿勢を取得するまでの道のりです．

IMU選定 -InvenSense社のIMU-

安いimuといったらInvenSense社のイメージでした．1000円くらいで買えます．
以下，InvenSense社のIMUのまとめです．

品番 説明
mpu6000,6500 もう生産してない．6軸（角速度+加速度）
mpu9150 9軸（６軸+地磁気）
mpu9250 9150の改良品で小型化し、磁気センサの分解能も高くなった．
icm20602 9250より性能が良いらしい．6軸．マイクロマウス界での最近の流行り．

今回は9軸取れ，amazonで売っているMPU9250を買いました．

f:id:ssk0109:20181215151714j:plain
MPU9250

勉強資料

今回，ついでに姿勢の勉強もしました．
以下はスタンフォードのIMUの資料です．
オイラー角やクォータニオンのことが非常にわかりやすくまとまっています．

https://stanford.edu/class/ee267/lectures/lecture9.pdf
https://stanford.edu/class/ee267/lectures/lecture10.pdf

すること

Arduino周りは下を参考にしてやっていきます．

ArduinoでMPU9250（加速度センサ、磁気センサ）を使う方法 : 試行錯誤な日々

ちなみに上に合わせてArduino買ってROSのコード入れたら，RAMが足りなくて一回詰みました． ArduinoでROSを使うときはnodehandleの宣言だけで1400bもってかれるので，
2kbしかないnano等ではコンパイルが通っても正常に動きません．Arduinoを選ぶ時は要注意です．

ArduinoからIMUデータをpublishする

Arduinoは以下のライブラリを用いてコードを書きました． スケッチ例も参考にしています．

github.com

Arduino-PC間の通信にはrosserialを用いました．
謎遮断するし、あんま評判良くないんですけどね！

github.com

以下書いたArduinoソースコードです．

クリックすると展開されます

ReadIMU.ico
#include <MPU9250_asukiaaa.h>

#ifdef _ESP32_HAL_I2C_H_
#define SDA_PIN 26
#define SCL_PIN 25
#endif

MPU9250 mySensor;

#include <ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>

ros::NodeHandle nh;

sensor_msgs::Imu imu;
sensor_msgs::MagneticField mag;
ros::Publisher pubimu("imu/data_raw", &imu);
ros::Publisher pubmag("imu/mag", &mag);

void setup() {
  while(!Serial);
  Serial.begin(115200);
  Serial.println("started");

#ifdef _ESP32_HAL_I2C_H_ // For ESP32
  Wire.begin(SDA_PIN, SCL_PIN); // SDA, SCL
#else
  Wire.begin();
#endif

  mySensor.setWire(&Wire);
  mySensor.beginAccel();
  mySensor.beginGyro();
  mySensor.beginMag();

  // You can set your own offset for mag values
  // mySensor.magXOffset = -50;
  // mySensor.magYOffset = -55;
  // mySensor.magZOffset = -10;

  sensorId = mySensor.readId();
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(pubimu);
  nh.advertise(pubmag);
}
float gXOffset= 2.50, gYOffset=-1.40, gZOffset= 1.47;

void loop() {
  Serial.println("sensorId: " + String(sensorId));
  float aX, aY, aZ, aSqrt, gX, gY, gZ, mDirection, mX, mY, mZ;
  mySensor.accelUpdate();
  aX = mySensor.accelX() ;
  aY = mySensor.accelY() ;
  aZ = mySensor.accelZ() ;
  //aSqrt = mySensor.accelSqrt();
  Serial.println("accelX: " + String(aX));
  Serial.println("accelY: " + String(aY));
  Serial.println("accelZ: " + String(aZ));
  //Serial.println("accelSqrt: " + String(aSqrt));

  mySensor.gyroUpdate();
  gX = mySensor.gyroX()-gXOffset;
  gY = mySensor.gyroY()-gYOffset;
  gZ = mySensor.gyroZ()-gZOffset;
  Serial.println("gyroX: " + String(gX));
  Serial.println("gyroY: " + String(gY));
  Serial.println("gyroZ: " + String(gZ));
 
  mySensor.magUpdate();
  mX = mySensor.magX();
  mY = mySensor.magY();
  mZ = mySensor.magZ();
  mDirection = mySensor.magHorizDirection();
  Serial.println("magX: " + String(mX));
  Serial.println("maxY: " + String(mY));
  Serial.println("magZ: " + String(mZ));
  Serial.println("horizontal direction: " + String(mDirection));
  
  Serial.println("at " + String(millis()) + "ms");
  Serial.println(""); // Add an empty line
  
  imu.header.frame_id = "imu_link";
  imu.header.stamp = nh.now();
  imu.angular_velocity.x = gX;
  imu.angular_velocity.y = gY;
  imu.angular_velocity.z = gZ; // [rad/sec]
  imu.linear_acceleration.x = aX;      
  imu.linear_acceleration.y = aY;  
  imu.linear_acceleration.z = aZ; 
  pubimu.publish(&imu);
  
  mag.header.frame_id = "imu_link";
  mag.header.stamp = nh.now();
  mag.magnetic_field.x = mX;
  mag.magnetic_field.y = mY;
  mag.magnetic_field.z = mZ; // [μT]
  pubmag.publish(&mag);

  nh.spinOnce();
  delay(1);
}

後はPC側でrosserialを起動すればデータをPCに送れます

rosrun rosserial_python serial_node.py /dev/ttyACM0 _baud:=115200

デバイスのpathは/dev/ttyUSB0になる場合もあります．
ちなみに，ここで例えばSRAMいっぱいに書き込んでrosserialを起動すると

software version mismatch such as hydro rosserial_python with groovy Arduino

とエラーが出るんですがバージョンは全く関係ないです． 他にも別の原因なのにこのエラーは良く出ます．

姿勢推定

姿勢推定にはimu_toolsのimu_filter_madgwickを使いました．
github.com Madgwick Filter（マッジウィック・フィルターと読むそう）は有名なKalman Filterと比べて，モデルが不必要で，高速(数百から数千Hzで回せるっぽいです！)なのに，同程度以上の精度のフィルターだそうです．
次コマンドで実行します．

rosrun imu_filter_madgwick imu_filter_node

ノードは以下のようになります． f:id:ssk0109:20181213000434p:plain

比較

imu_filter_madgwickはinputに6軸(角速度と加速度)か9軸(6軸+地磁気)を選べます．
折角なので比較してみました．
6軸のみの場合，誤差が累積していっています．安いIMUだとこんなもんなんですかね？
もう少し調査したいです．
追記：加速度からでは絶対的な方位角を推定できないので，こんなもんですかね．
角速度のキャリブレーションを精密にやればもうちょいよくなるのかな？ f:id:ssk0109:20181215180801g:plain

9軸では，ブルブルはしてますがドリフトはないです． 積分して相対的に計測していくのって大変だなぁと感じました．

参考にしたもの

コード置き場

github.com

Google Cartographerで作った地図でmcl_3dlによる自己位置推定

ROS Robot SLAM Cartographer

はじめに

こんにちは．ササキ(@saitosasaki)です．
今回はGoogle Cartographerで作った三次元地図で自己位置推定（ローカリゼーション）パッケージmcl_3dlを動かしました．
mcl_3dlはnavigationパッケージにあるamcl(adaptive Monte Carlo localization)の三次元版でトピックもできるだけ共通化しているそうです．なので代替できるかもと思い，実際に動かしてみました．

https://github.com/at-wat/mcl_3dlgithub.com
mcl_3dlの解説スライド

mcl_3dl: amcl並に軽量な3-D/6-DoFローカリゼーションパッケージ

mcl_3dlの解説スライドの解説動画
ROSCon JP 2018: 5. mcl_3dl amcl並に軽量な3-D 6-DoFローカリゼーションパッケージ on Vimeo

今回データにodomがなかったのですが、オドメトリフリーで動かす場合は同じくat-watさんが作っているneonavigationパッケージが必要なようです。

GitHub - at-wat/neonavigation: A 2-D/3-DOF seamless global/local mobile robot motion planner package for ROS

データはカートグラファーのデモ用のbagファイルを使用．
コンフィグとlaunch弄ったりコマンドポチポチしてるだけでいけました．

実行フロー

1. デモデータのダウンロード　

カートグラファーの三次元自己位置推定用デモファイルをダウンロードします　　

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag

2. bagファイルの切り取り

これをしないと自分のPCだと点群データが重すぎて，ダウンサンプリング時にコアダンプしました()．(繋げるときはpclやCloudCompare等で)

rosbag filter ${HOME}/Downloads/b3-2016-04-05-13-54-42.bag ${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag "t.secs <=1459864582.67"

3. bagファイルからbag.pbstreamファイルを作る

bag.pbstreamはカートグラファーで自己位置推定する時に使う地図や軌道情報形式が入っている専用の形式です．

roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag

4. bagとbag.pbgstreamファイルからpcdファイルを作る

pcdファイルは点群において一般的なファイル形式です ．
まずpcdファイルを出力するように設定ファイルを弄ります ．

cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_3d.lua
options = {
  tracking_frame = "base_link",
  pipeline = {
    {
      action = "min_max_range_filter",
      min_range = 1.,
      max_range = 60.,
    },
    {
      action = "dump_num_points",
    },
    ・・・
    {               -- add
      action = "write_pcd", -- add
      filename = "points.pcd",  -- add
    },              -- add
  }
}

編集したらcatkin_make忘れずに．次コマンドでpcdファイル出力．

roslaunch cartographer_ros assets_writer_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag.pbstream

ダウンサンプリング．

pcl_voxel_grid ${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag_points.pcd ${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag_points_down.pcd  -leaf 0.1,0.1,0.1

5. 自己位置推定

mcl_3dlのtest.launchを弄ります。
rosbagがpublishするtopicのリマップの追加。

<node pkg="rosbag" type="play" name="playback" args="--clock $(arg bag_file)"
      if="$(arg use_bag_file)">
<remap from="odom" to="odom_unused" if="$(arg without_odom)" />
<remap from="imu" to="imu/data"/> 　　　　   <!-- add -->
<remap from="horizontal_laser_3d" to="cloud"/><!-- add -->
</node>

同じくlaunchにtf関係の追加。

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

最後にlaunch。

roslaunch mcl_3dl test.launch use_neonavigation:=true without_odom:=true use_pointcloud_map:=true map_pcd:=${HOME}/Downloads/b3-2016-04-05-13-54-42_short.bag_points_down.pcd use_cad_map:=false use_bag_file:=true bag_file:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag

結果

作られた地図 f:id:ssk0109:20181209135240p:plain 自己位置推定の様子 f:id:ssk0109:20181209133909p:plain f:id:ssk0109:20181209212420g:plain 色々あんま弄ってないので荒ぶっています。
が，とりあえず今回はここまで。
追記：下gifのようにCartographerのデモデータの点群が不連続なのが良くなさそう？ f:id:ssk0109:20190101021026g:plain
追記2：Autowareのpoints_concat_filterで複数のpointcloudを統合すると良いっぽい？

参考にしたもの

google-cartographer-ros.readthedocs.io

関連記事

ssk0109.hatenablog.com

2DレーザーのみでGoogle Cartographer ros

ROS Robot SLAM Cartographer

目次

はじめに

こんにちは．ササキ(@saitosasaki)です．
今回は二次元のCartographer_rosをレーザのみで動かし、地図を作りました。

Cartographer_rosはgoogleが公開しているオープンソースのSLAM(Simultaneous Localization and Mapping)パッケージのrosラッパーです。

github.com

SLAMでループクロージャーができてるパッケージは貴重で、有名なgmappingは明示的にループが閉じないので屋外などの大規模環境で地図がずれるって人はこちらを使ってみたらよいと思います。使い方も簡単ですし。 (なお、パラメータチューニング)
2DではIMU・オドメトリが不要なのもポイント高いです。ただし、3DはIMUが必須です。

編集したところ 

1.レーザースキャンの型の設定

デフォルトではMultiEchoLaserScan型を読み込む設定なので、使用機器にあったLaserScan型に変更しました。ちなみにPointCloud2型も設定かえれば読み込めます。

cartographer_ros/cartographer_ros/configuration_files/backpack_2d.lua

options = {
・・・
num_laser_scans = 1,-- <--default:0
num_multi_echo_laser_scans = 0,-- <--default:1
num_subdivisions_per_laser_scan = 10,
num_point_clouds = 0,--<--if pc2 is used,set 1
・・・
}

2.IMUの設定

デフォルトではimuの入力が必要だったので、imuの設定を切りました。

cartographer/configuration_files/trajectory_builder_2d.lua

TRAJECTORY_BUILDER_2D = {
use_imu_data = false,-- <--default:true
・・・
}

実行

 レーザ用のnodeとcartographerのbackpack_2d.launchを立ち上げ

できた地図

20181206032203.png map_serverのmap_saverでpgm形式で保存もできます。

参考にしたもの

google-cartographer.readthedocs.io

google-cartographer-ros.readthedocs.io

関連記事

ssk0109.hatenablog.com