Google Cartographer を使ったマッピング　〜パラメータチューニング編〜

ということで，今年のつくばチャレンジで習得したスキルを書き残しておきたいと思います．

０．マップ作成対象

今年のつくばチャレンジのコースは下記の全長２．Xkmのコースだったんですが，全部のコースマップを一括で作るとなかなか大変だったので，自分は下記の６つに分割しました．

f:id:rkoichi2001:20191115034319p:plain
つくばチャレンジ２０１９のコース

コース１．確認走行区間

f:id:rkoichi2001:20191117074725p:plain
確認走行区間

コース２．交差点まで（行き）

f:id:rkoichi2001:20191117074755p:plain
交差点まで

コース３．研究学園駅

f:id:rkoichi2001:20191117074817p:plain
研究学園駅

コース４．研究学園駅前公園

f:id:rkoichi2001:20191117074850p:plain
研究学園駅前公園

コース５．交差点まで

f:id:rkoichi2001:20191117074934p:plain
交差点まで

コース６．交差点からゴール（帰り）

f:id:rkoichi2001:20191117075003p:plain
交差点からゴールまで

で，いつものように gmapping で地図を作ればいいかと思っていたのですが，コース４（研究学園駅前公園）が結構長く，なかなか安定してマップが作成できなかったために Cartographer を使うことにしました．ただ，この Cartographer，なかなかパラメータチューニングが大変でうまく使いこなすまでに時間がかかったので，このエントリではそのあたりを書き残せればと思います．時間があれば理論編も書きたいですが，，，いつものことながら理論系のエントリは時間がかかるので，気が向いたら書きます．

１．参考文献＆ブログ

パラメータチューニングのやり方は， Cartographer の作者の方が作ってくれている下記ドキュメントやブログを見ながら実施すればある程度は理解できると思います．

Cartographer ROS Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer-ros/latest/google-cartographer-ros.pdf

Cartographer Documentation

https://buildmedia.readthedocs.org/media/pdf/google-cartographer/latest/google-cartographer.pdf

あと，下記はお世話になったブログです．
ssk0109.hatenablog.com

ssk0109.hatenablog.com

２．前提・システム構成

このエントリでは，下記の前提でマップ作っています．

2D Lidar が一つ，Odometry（x, y, yaw）有り，IMU無し

f:id:rkoichi2001:20191117093646j:plain
今回のマップ制作に使用したロボット，改め愛犬．前から

f:id:rkoichi2001:20191117093752j:plain
今回のマップ制作に使用したロボット，改め愛犬．横から

主要パラメータファイル一覧

変更が必要となるパラメータファイルは下記のとおりです．

backpack_2d.urdf

　各センサーの搭載位置情報が記載された urdf ファイル．今回のエントリでは horizontal_lidar のみ使用．

backpack_2d.lua

　map 作成時の構成情報が記載された lua ファイル．

trajectory_builder2d.lua

　Local SLAM のパラメータが記載された lua ファイル．　　

pose_graph.lua

　Global SLAM のパラメータ（大域的最適化）のパラメータが記載された lua ファイル．

手順

で，実際にマップを生成するにあたっては，下記の手順で設定・パラメータを変更していきます．

１．センサーの搭載位置情報を backpack_2d.urdf に記載する．
２．Topic 名や構成情報を backpack2d.lua に記載する．
３．Odometry がある程度正しくなるようにパラメータ調整．
４．調整された Odometry を使って Local SLAM のパラメータ調整（trajectory_builder2d.lua）．
５．狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整（pose_graph.lua）．

ステップ１　センサーの搭載位置情報を backpack_2d.urdf に記載

ここの設定はとても大事です！これを忘れるとうまく行きません！

f:id:rkoichi2001:20191115061647p:plain
horizontal scanner の位置を設定

ステップ２　Topic 名や構成情報を backpack2d.lua に記載

Topic 名や構成情報を backpack2d.lua に記載します．

f:id:rkoichi2001:20191115054802p:plain
backpack_2d.lua の差分

変更したパラメータですが，
published_frame：今回はオドメトリを使用しているので，オドメトリフレームである odom を指定します．cartographer が生成する map フレームは，ここで指定したフレームを子供に持つ事になります．
provide_odom_frame：true にすると，Local SLAM で生成された自己位置情報が odom_frame として供給されます．自分の場合は別のノードが odom_frame の tf を供給していたので， false にしました．
use_odometry：マップ生成にオドメトリを使うか否か．今回は true です．
num_laser_scans：レーザスキャンセンサーの数．今回は１つです．
num_multi_echo_laser_scans：Echo Laser Scan を生成するレーザスキャンセンサの数．今回は０です．

ステップ３　Odometry がある程度正しくなるようにパラメータ調整

このステップはちょっと詐欺かもですが，，，，正しいマップを手っ取り早く作成したかったので，自分のオドメトリで軌跡がつながるようにヨーレートのオフセットを事前に計算して使うことにしました．

オフセット調整前

例えば，こんな感じで全く繋がらないオドメトリを．．．

f:id:rkoichi2001:20191115064643p:plain
オフセット調整前オドメトリ

オフセット調整してそこそこつながるようにします．

f:id:rkoichi2001:20191115064445p:plain
オフセット調整後オドメトリ

ステップ４　調整された Odometry を使って Local SLAM のパラメータ調整

Local SLAM の調整です．Cartographer のパラメータ調整マニュアルにも有りますが，下記のパラメータを変更して Global SLAM を OFF にして，Local SLAM だけで性能出しします．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90
【After】
POSE_GRAPH::optimize_every_n_nodes = 0

※それ以外はデフォルトパラメータのままで動かしてみます．

f:id:rkoichi2001:20191115070810p:plain
Local SLAM の結果（デフォルトパラメータ）

上図を見てわかるように，デフォルトパラメータだとヨー角の誤差が積算してしまい，一周して元の地点に戻ってきてもマップが繋がらなくなってしまいました．ということで，ヨー角に関しては Local Scan Matcher の結果よりももう少しオドメトリを信じたほうが良さそうです．ひとまず効果代を見るために，パラメータを目一杯変えてみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

f:id:rkoichi2001:20191115073656p:plain
オドメトリのヨー角を強く信じる設定．

だいぶいい感じになりました．というか，オドメトリしか使ってない感じになりました（笑）で，図中の吹き出しにも書いたんですが，ヨー角に関してはオドメトリ生成値の重みを強くしたものの，並進に関しては設定をいじらなかったため，２つのオドメトリ間でまだ差分が見られます．ということで，並進もオドメトリを強く信じてみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 40
【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000

f:id:rkoichi2001:20191115075034p:plain
オドメトリのヨー角，並進を強く信じる設定．

完全にオドメトリを信じて作ったマップになりました！マップもきれいになったし，めでたいめでたし．．．．だったらSLAMはいらないわけで．．．大域的にも局所的にもオドメトリには誤差があるので，細かいとこを見ていくとマップが変になってます．例えば，一つの平面をLIDARで取っているにもかかわらず，MAP上には２つの線として現れています．ここからは，，，おそらく微調整が必要になるのかと思います．ということで，もうちょっとスキャンマッチングの結果を信用してみます．

【Before】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 1000000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000000

【After】
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000

f:id:rkoichi2001:20191115083542p:plain
スキャンマッチングの結果をもうちょっと信用

うーん．あんまり変わらないですね．．．．ということで，次に Global SLAM の調整をしてみます．

ステップ５　狙った箇所で正しく Loop が閉じるように Global SLAM のパラメータ調整．

まず，ステップ４でOFFにした Global SLAM を ON にします．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 0
【After】
POSE_GRAPH::optimize_every_n_nodes = 90

f:id:rkoichi2001:20191117080829p:plain
Global SLAM を ON にしてマップ生成

再訪問したところで Loop Close してほしいのですが，デフォルトパラメータのままではそれほどループが閉じませんでした．ということで，パラメータを変更してループが閉じやすくします．

【Before】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e4
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e5

【After】
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10

f:id:rkoichi2001:20191117084435p:plain
Loop Close しやすくした結果

８の字走行した部分（図の左側）に関してはループが閉じました．が，走行の最後の部分に関してはまだループが閉じていません．おそらく自分のデータのとり方が良くなかったんですが，一周回って同じところに帰ってきた時点でデータ取得を止めてしまったので，再訪箇所のオーバーラップが少なく，ループが閉じきれてないのかと思われます．ということで，Loop Close をもっと頻繁に発生させて，最後のループが閉じるかどうか実験してみます．

【Before】
POSE_GRAPH::optimize_every_n_nodes = 90

【After】
POSE_GRAPH::optimize_every_n_nodes = 5

うーん．なんかまだ微妙です．

f:id:rkoichi2001:20191117085541p:plain
頻繁に Loop Close 走らせた結果

Cartographer では，マップの小領域（Submapと呼ぶみたいです）に対して Scan 結果から得られる尤度を蓄積していき，更新が一定回数以上になったときにその Submap を大域的最適化対象に登録します．この「一定回数以上」のパラメータが下記になるのですが，最適化対象にするタイミングをもっと早めてみます．

【Before】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 90

【After】
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

f:id:rkoichi2001:20191117090401p:plain
間違った Loop Closure を検出

うーん．ループを誤検知してしまいました．もう少しオドメトリを信じるべきか．．．
Cartographer のポーズ最適化では， Local SLAM のときと同様に，オドメトリ，Local SLAM の結果に対して重みを設定することができるのですが，もう少しオドメトリを信じる設定にしてみます．

【Before】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e5
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e5

【After】
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7

f:id:rkoichi2001:20191117094137p:plain
一応完成！

ということで，上記のパラメータ調整を経て，結構いい感じになったのでこれで良しとします．エンコーダとIMUを使ったヨーレート（赤線）は完全には正確でなく，一周回ってきたときには繋がってませんが，SLAMから生成されるオドメトリ（青色）は一周回った時点できれいにつながりました．

結果

ということで，全マップです！うーん．やっぱり，ループを綴じ込むのはかなり難しいですね．オドメトリを合わせればそこそこまでは行くものの，完全ではないのでループ綴じ込む程ではなく，逆に Local SLAM, Glocal SLAM の寄与を上げるとループじゃないところで誤マッチングが起こり．．．というシーソーゲームをひたすら繰り返してました．

コース１．確認走行区間

f:id:rkoichi2001:20191117074725p:plain
確認走行区間

f:id:rkoichi2001:20191117190041p:plain
コース１のマップ

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e9
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e10
POSE_GRAPH::optimize_every_n_nodes = 5
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 500
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 1000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

コース２．交差点まで（この区間は難しく，，，完璧にはできませんでした．）

f:id:rkoichi2001:20191117074755p:plain
交差点まで

f:id:rkoichi2001:20191117190556p:plain
コース２のマップ．

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

コース３．研究学園駅

f:id:rkoichi2001:20191117074817p:plain
研究学園駅

f:id:rkoichi2001:20191117190751p:plain
コース３のマップ．

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e9
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 100
POSE_GRAPH::constraint_builder::ceres_scan_matcher::rotation_weight = 100
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e6
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e7
POSE_GRAPH::optimization_problem::global_sampling_ratio = 0.3
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::translation_weight = 100000
TRAJECTORY_BUILDER_2D::ceres_scan_matcher::rotation_weight = 100000
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 10

コース４．研究学園駅前公園（この区間は難しく，，，完璧にはできませんでした．）

f:id:rkoichi2001:20191117074850p:plain
研究学園駅前公園

f:id:rkoichi2001:20191117191016p:plain
コース４のマップ

デフォルトからの変更パラメータ
POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8

コース５．交差点まで

f:id:rkoichi2001:20191117074934p:plain
交差点まで

f:id:rkoichi2001:20191117191218p:plain
コース５のマップ．

POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8
TRAJECTORY_BUILDER_2D::submaps::num_range_data = 40

コース６．交差点からゴール

f:id:rkoichi2001:20191117075003p:plain
交差点からゴールまで

f:id:rkoichi2001:20191117191340p:plain
コース６のマップ．

POSE_GRAPH::constraint_builder::constraint_builder::min_score = 0.60
POSE_GRAPH::constraint_builder::loop_closure_translation_weight = 1.1e5
POSE_GRAPH::constraint_builder::loop_closure_rotation_weight = 1e6
POSE_GRAPH::constraint_builder::fast_correlative_scan_matcher::linear_search_window = 20
POSE_GRAPH::constraint_builder::ceres_scan_matcher::translation_weight = 1
POSE_GRAPH::optimization_problem::huber_scale = 0.1e1
POSE_GRAPH::optimization_problem::local_slam_pose_translation_weight = 1e3
POSE_GRAPH::optimization_problem::local_slam_pose_rotation_weight = 1e4
POSE_GRAPH::optimization_problem::odometry_translation_weight = 1e8
POSE_GRAPH::optimization_problem::odometry_rotation_weight = 1e8

あともう一つ，英語圏（≒アメリカ，ドイツ）からのアクセスをもっと増やせないかな〜という思いもあって，アクセス数が稼げそうなエントリに限って英語版を作ってみることにしました．アクセスが劇的に増えたら，また報告します（笑）

http://daily-tech.hatenablog.com/entry/2019/11/25/062304daily-tech.hatenablog.com