ナビゲーションにおける自己位置推定 from エンジニア・メモ  記者:早川喜太

ナビゲーションにおける自己位置推定

目次

動作環境

項目内容
OSUbuntu20.04
ROS 2 DistroFoxy Fitzroy

はじめに

ROS2では 目的地を設定したら経路を計算して実際にロボットを目的地まで移動させることができるNavigation2の開発が進められている。

  • github: https://github.com/ros-planning/navigation2
  • ドキュメント: https://navigation.ros.org/index.html

また、ROS 2ではないが産業技術総合研究所ロボットイノベーション研究センターによるNavigationスタックの設計仕様をまとめたドキュメントも理解の助けになる。

  • https://robo-marc.github.io/navigation_documents/introduction.html

Navigation2は複数のパッケージで構成されており、それぞれのパッケージごとに設定すべきパラメータが存在する。その結果、パラメータの数が多くなっており、自作のロボットにナビゲーションを適用する場合などはパラメータのチューニングに多くの時間が割かれることとなる。

Navigation2ではこの問題を認識し、Nav2ドキュメントにTuning Guideの章を設けている。

ROS 2のDistributionについて

2024年1月現在ではUbuntu20.04をターゲットとしているFoxy Fitzroyと Galactic GeocheloneはEOLを迎えている。新規にROS 2環境を構築する場合はUbuntu22.04をターゲットとするHumble Hawksbillを使うのがおすすめだ。

我々は開発初期よりROS 2 Foxyを使っているが、バイナリで提供されているfoxyのnavigation2スタックには既知のバグが含まれている。

以下にその対策案を示す。

  • ROS 2 Galactic以降のDistributionに乗り換える
  • foxyのNavigation2スタックをsource buildする

ナビゲーションにおける自己位置推定について

ナビゲーションのメイン機能を大別すると以下のように3つに分けられる。

  • 地図とセンサ情報を照らし合わせて地図上における自身の位置を推定する
  • 目的地までのルートを算出する
  • 障害物を避けながら目的地まで運行を制御する

更に、障害物がルート上避けて通れない場所にある場合に別ルートを計算したり、 ルート再計算を含めたロボットの動作をビヘイビアツリーで定義するなど、細かな調整も可能だ。

センサ情報から地図上の自己位置を推定する機能はNav2_amclパッケージが担当している。 amclはAdaptive Monte Carlo Localizationの略で、基本的には、粒子フィルタ(もしくはKLDサンプリング)を使用したモンテカルロ法で自己位置を推定している。また、状況に応じて生成する粒子の数を増減させることで効率を上げている。

パラメータ(基本設定)

ロボットの種類やセンサの性能に合わせて指定しないといけないパラメータを表に示す。

ロボットのモデルタイプ

robot_model_typeパラメータではロボットの駆動タイプを指定する。 ROS 2 Humble以降ではフルでプラグイン名を指定する必要がある。

駆動タイプ値(Humble以降)備考
差動駆動“differential”“nav2_amcl::DifferentialMotionModel”デフォルト
オムニホイール“omnidirectional”“nav2_amcl::OmniMotionModel”

フレーム関連

フレーム名は基本はデフォルト値で問題ないと思うが、 base_frame_idは”base_footprint”ではなく”base_link”の場合もあるので要確認。 また、ロボット側でtf(odomからbase_frame)をpublishすることを忘れないこと。

パラメータ名デフォルト値備考
global_frame_id“map”パッケージごとにglobal_frameが共通とは限らないので注意。
odom_frame_id“odom”オドメトリ座標系
base_frame_id“base_footprint”ロボット座標系

TF(トランスフォーム)関連

パラメータ名デフォルト値備考
tf_broadcastTrue基本的にTrue

トピック関連

パラメータ名デフォルト値備考
scan_topic“scan”
map_topic“map”

センサ関連

以下のパラメータは基本的には2D LiDARの測定可能距離に合わせて設定する。 どちらのパラメータも、-1.0に設定すれば無効化できる。

パラメータ名デフォルト値備考
laser_max_range100.0LiDARの測定できる最大距離を設定。小数点を付けてdouble型だと認識させること
laser_min_range-1.0LiDARの測定できる最短距離を設定。

初期姿勢の設定

ナビゲーションを起動した後には、ナビゲーションを動作させるために必要な初期姿勢を設定する必要がある。 基本的にはrviz2上で「2D Pose Estimate」メニューで図のように緑の矢印で 地図上でのロボットの位置を指定するという方法をとる。

この操作については、以下のパラメータで自動化できる。

パラメータ名デフォルト値備考
set_initial_poseFalseFalseの場合はrviz2上で設定(もしくは/initial_poseトピック)
Trueの場合は、ナビ起動時に下のinitial_poseパラメータの設定内容で初期化される。
initial_pose“{x: 0.0, y: 0.0, z: 0.0, yaw: 0.0}”ナビ起動時に送信される初期姿勢を設定

関連するパラメータとして、リセット(異常時や地図変更時など)の初期姿勢についてのパラメータがある。

パラメータ名デフォルト値備考
always_reset_initial_poseFalseリセット時にinitial_poseで初期化する場合はTrue
最後に推定された姿勢を使うのであればFalse

パラメータ(機能別)

レーザーによる尤度計算モデル

レーザーによる尤度計算をする際に、3つのモデルが用意されている。 laser_model_typeパラメータで指定する。

モデル名説明
ビームモデル“beam”各レーザーごとに計算するため、計算処理に時間がかかる。
だが障害物が多い場合は尤度モデルよりビームモデルが良い。
尤度モデル“likelihood_field”デフォルト設定。
尤度モデル(+beam skip)” likelihood_field_prob”尤度モデルでかつbeam skipも利用する場合

本稿ではデフォルトで無効化されているbeam skip機能については説明を割愛する。

オドメトリのノイズ関連

オドメトリは、ホイールエンコーダやIMU(慣性計測ユニット)などの内部センサからえられた移動情報を元に現在の位置や姿勢を推定する手法の総称である。こうして推定された座標はセンサの精度や路面状況などにより、実際の位置との間にノイズが生じる可能性がある。モンテカルロ法を適用する際にオドメトリのノイズを考慮するパラメータを以下に示す。

パラメータ名デフォルト値備考
alpha10.2ロボットの動きの回転成分から、オドメトリの回転推定で予想されるノイズ
alpha20.2ロボットの動きの並進成分から、オドメトリの回転推定で予想されるノイズ
alpha30.2ロボットの動きの並進成分から、オドメトリの並進推定で予想されるノイズ
alpha40.2ロボットの動きの回転成分から、オドメトリの並進推定で予想されるノイズ
alpha50.2平行移動関連のノイズ(オムニホイールの場合のみ有効)

本パラメータを調整しても精度が上がるわけではない。 しかし、オドメトリ座標で利用されるホイールエンコーダやセンサの性能が悪い、もしくは路面状況が悪い場合は、 モンテカルロ法でのパーティクルの探索効率が上がるかもしれない。

パーティクルフィルタの動作設定

amclの動作や精度に直結するパラメータが多い。

パラメータ名デフォルト値備考
max_beams60フィルタ更新時の各スキャンにおけるビーム数
max_particles2000最大パーティクル数
min_particles500最小パーティクル数
pf_err0.005パーティクルフィルタの誤差の閾値。
pf_z0.2パーティクルフィルタにおける集団密度
resampling_interval1リサンプリング周期。1の場合はフィルタ更新時に毎回リサンプリングする
update_min_a0.2粒子フィルタ更新に必要な回転運動量[rad]。0.2[rad]≒12[度]
update_min_d0.25粒子フィルタ更新に必要な並進回転量[m]

尤度モデルに関する設定

amclでは、基本的には確率モデルを使っている。 ただし、単純なモデルではなく、地図にない障害物に当たる可能性やそもそもレーザーが物体を検出しない可能性など 様々な場合を考慮し、以下のパラメータで設定できる重みの割合で合成した確率分布を使用している。

パラメータ名デフォルト値備考
z_hit0.5ガウシアンノイズ(マップ上で壁に当たる距離周辺で尤度が高くなる確率分布)の重み
z_max0.05(ビームモデルのみ)センサ最大距離ノイズの重み
z_rand0.5ランダムノイズ(一様分布の尤度を持つ確率分布)の重み
z_short0.005(ビームモデルのみ)障害物対策ノイズの重み
sigma_hit0.2モデルの z_hit 部分で使用されるガウス・モデルの標準偏差。
lambda_short0.1z_shortによる尤度加算の分布を調整するパラメータ
laser_likelihood_max_dist2.0尤度モデルにおいて障害物から広げる最大距離

その他

上記以外のパラメータを列挙する。基本的にデフォルトで機能が無効化されている。

パラメータ名デフォルト値備考
recovery_alpha_fast0.0
recovery_alpha_slow0.0
save_pose_rate0.5ロボットの位置推定の結果を保存する頻度。-1.0で無効
do_beamskipFalsebeamskip機能の有効/無効を設定。
ロボットのモデルタイプにlikelihood_field_prob指定する必要がある
beam_skip_distance0.5
beam_skip_error_threshold0.9
beam_skip_threshold0.3
first_map_only_False複数地図に対応するか。Humble以降は”first_map_only”

パラメータ調整に関して

基本的なtfやトピック名、2DLiDARセンサの最大測定距離/最小測定距離を設定できていることを前提とする。 どういった状況の時はどのパラメータを調整すべきかに関する一般論を示す。

尤度計算モデル

人混みの中であったり、地図には存在しない障害物が多い状況が原因で 推定される自己位置が安定しない場合はlaser_model_typeをbeamに変えると良いかもしれない。 デフォルトのlikelihood_fieldモデルでは地図上にない障害物を考慮しないが、beamモデルではそれらを考慮する仕組みになっている。

likelihood_fieldモデルの場合はzパラメータ中、z_hitとz_randの2つだけを使う(合計が1になるように設定)。 対してbeamモデルを使う場合はz_hit,z_max,z_rand,z_shortの4パラメータとも使うことに留意する。 4パラメータの合計が1.0になるように設定し直す必要がある

自己位置推定頻度

自己位置の推定が実行されるタイミングは、

  • オドメトリ座標が update_d_min[m]以上変化
  • オドメトリ座標が update_a_min[rad]以上向きが変化

のいずれかを満たした時である。

基本的には、車体の平均速度とマップ上での自己位置を補正したい頻度から決めるのがよい。高頻度であればあるほど自己位置を細かく微調整させることができるが、自己位置推定の計算負荷も高くなるので注意が必要だ。

一つの例として、路面状況が悪い、濡れていてスリップしやすい、ホイールエンコーダ分解能が低い場合などは、オドメトリ座標に誤差が累積してしまう。 この場合はupdate_min_aとupdate_min_dの値を小さく設定し、自己位置を高頻度で補正することで、オドメトリの誤差が累積する前に処理してしまう方がいいだろう。

比較動画は以下となっている。

走行ルートを再生した後に、自己位置推定を高頻度で行う場合(左側)と低頻度で行う場合(右側)の比較をしている。赤で表示されているのは2DLiDARで検出された障害物の情報で、高頻度の場合に障害物情報と地図のズレが抑えられていることがわかる。

設計の見直し

パラメータの調整だけではどうにもならない場合もある。 この場合は、センサの選定やセンサの配置場所など、設計自体を見直すのも一つの手だ。

  • 精度/分解能が優れているホイールエンコーダの導入
  • スキャン周波数や分解能が優れた2DLiDARに変更する
  • 2D LiDARを2つ用意して、障害物を検知する範囲を広げる

ナビゲーション動作事例

以下にナビゲーションの動作事例を示す。

目次