Probabilistic Robotics - Chap.8 Mobile Robot Localization: Grid And Monte Carlo
8.1 Introduction
8章では下記2つのローカライゼーションアルゴリズムについて述べる。
- グリッドローカライゼーション
beliefの計算にヒストグラムフィルタを使う。グリッドローカライゼーションは、グリッドを細かくすると、計算負荷が高くなり処理が遅くなるという問題がある。
- モンテカルロローカライゼーション(MCL:Monte Carlo Localization)
パーティクルフィルタを使ってローカライゼーションを行う、有名なアルゴリズム。欠点は色々とあるものの、誘拐ロボット問題や動的環境下への対応が可能。
これら2つのアルゴリズムは、下記の共通する特徴がある。
- センサ生値を処理するため、特徴量の抽出といった加工処理が不要
- ノンパラメトリック
- グローバルローカライゼーションのための手法であり、誘拐ロボット問題に対応可能
以降では、上記2つのアルゴリズムの詳細を述べる。
8.2 Grid Localization
8.2.1 Basic Algorithm
グリッドローカライゼーションは、ヒストグラムフィルタを利用してグリッドマップ上でのロボットの事後beliefを近似する。つまり事後beliefは、下記の様に各グリッドでの存在確率の集合
として表せる。また各グリッドの集合であるグリッドマップは、
とできる。
グリッドローカライゼーションの擬似コードを下記に示す。グリッドローカライゼーションは、入力として各グリッドでの存在確率と最新の観測結果、制御量、マップ情報を必要とする。そして処理結果として、最新の事後beliefを出力する。疑似コード中のとは、Chapter5、6で紹介したどの方法を適用してもよい。
8.2.2 Grid Resolution
グリッドローカライゼーションにおける重要な変数は、グリッドの解像度である。グリッドの決め方には、トポロジカルな方法とメトリックな方法がある。トポロジカルな方法では、環境を特徴的な領域に分割する。ここで言う特徴的な領域とは、ドアや窓といったランドマークが存在する領域を指す。メトリックな方法では、環境の特性を考慮せずに均一なグリッドに分割する。分割数は、特徴的な領域は少ない場合が多いので、メトリックな方法の方が多い。
分割数を多くすると計算負荷が高まるため、解像度は粗くなりがちである。粗さへの対応は、センサモデルと運動モデルの工夫が必要である。例えばメトリックな方法でグリッドを切った場合、グリッドがで、ロボットの速度がとすると、運動モデルが頻繁に更新してもグリッドの遷移を検出できない。そのため、運動モデルの更新頻度を下げるといった対応が必要である。
8.2.3 Comuputational Consideration
ローカライゼーション時の計算負荷を下げる方法は、沢山検討されている。
- Model pre-caching
予め特定の計測結果を保存しておき、現在の値とその値を比較することで、瞬時に状態を特定可能にする。
- Sensor Subsampling
センサデータを全て処理するのではなく、重要な一部のデータのみを処理する。
- Delayed Motion Update
制御や観測を行うよりも低い頻度でbeliefの更新を行う。
- Selective Updating
8.3 Monte Carlo Localization(MCL)
8.3.2 The MCL Algorithm
ベーシックなMCLアルゴリズムでは、beliefはパーティクルのセットで表現される。beliefの更新方法は下記の擬似コードの通り。
初期beliefは、事前分布を元にランダムに生成した個のパーティクルとして得る。この時各パーティクルにはimportance factor \ を割り当てる。なお上記擬似コード中のとは、グリッドローカライゼーションと同様に、5章、6章で紹介された方法を摘要できる。
8.3.4 Properties of MCL
MCLは殆ど全ての実用上重要な分布を近似できる。一般的なパーティクル数をセットするための戦略は、とが更新されるまでリサンプリングしないことである。またMCLの利点として、ノンパラメトリックな近似ができる点がある。
8.3.5 Random Particle MCL: Recovery form Failures
今までに述べたMCLのアルゴリズムでは、グローバルローカライゼーションは可能だが失敗時に復帰できない、誘拐ロボット問題には対処できない、といった問題がある。それは、パーティクルが一度現在の姿勢と異なる場所に集まると、正しい姿勢へ回復できないためである。
これら問題へのシンプルな対応策として、ランダムパーティクルの注入する方法がある。これにより、運動モデルの中にランダムな部分空間が出来る。ではどうやってランダムにパーティクルを注入するべきか。ある種の推論を使って行うのがよい。
1つの方法として、観測モデルを監視し、モデルと平均確率を関連付ける方法がある。(ToDo:よくわからん)平均値は、観測モデルを近似できる。通常数ステップにわたった平均を求めるのがよい。
観測確率が低い理由は、ローカライゼーションの失敗以外にも、センサノイズが異常に高い、パーティクルが空間上に分散しすぎている(集まっていない)、などがある。
パーティクスル数を決める時、数ステップの観測尤度の平均を維持し、より多くのステップの平均に関連付けるとよい。
どの分布を使ってサンプリングするかについての2つ目の問題は、2つの方法で言える。姿勢空間全体を対象とした一様分布を元にパーティクスルをばらまき、現在の観測値を使ってそれらを重み付けする。
例えばランドマーク検出モデルなどのセンサモデルでは、観測モデルの分布に沿ってパーティクルをばらまくことができる。この場合は尤度の沿ってパーティクルをばらまく。
下記にランダムランプルのばらまきを考慮したMCLのアルゴリズムを述べる。
このアルゴリズムは、短期・長期それぞれの平均尤度をトラッキングする点で適応的である。短期・長期の平均尤度は10、11行目で求めている。なお上記、は、短期・長期の平均を求める指数フィルタの減衰率であり、を満たすものとする。
このアルゴリズムの肝であるランダムサンプルの追加は、13行目で行っている。13行目に示すように、短期の平均が長期の平均を下回る、つまり[tex:{1.0 - w_{fast}/w_{slow} \rt 0}}の時、ランダムサンプルが追加される。これにより、観測尤度が急に減少すると、その文ランダムサンプルの沢山追加される。
8.3.6 Modifying the Proposal Distribution
MCLの提案分布は、MCLを非効率なものにする原因の1つである。もしも提案分布と目標分布の差が大きくなれば、精度向上のためにサンプル数を増やす必要がある。提案分布が非効率なことで、ノイズのないセンサの観測結果からロボットの姿勢が寸分の狂いもなくわかるとすると、MCLが失敗することになる。そのためMCLを使う場合、精度の良いセンサよりも悪いセンサを使うことが望ましい。別の方法として、観測モデルに人工的なノイズを乗せるようにてもよい。
その他の方法として、サンプリングプロセスを改良も考えられる。例えば動作モデルと観測モデルの役割を入れ替える、混合モデルが考えられる。混合モデルでは、観測モデルがパーティクルの生成、重みの更新を行う。混合モデルは、誤差の低減や誘拐ロボット問題へ対応が可能だが、実装には困難が伴うという問題もある。
8.4 Localization in Dynamic Environment
これまでに述べたローカライゼーション手法の重要な制約は、静的な環境を仮定している。そのため、人などの動体を考慮していない。ただし確率的なアプローチでは、ノイズを考慮しているため、結果としてある程度動体を考慮している。一方で、センサノイズは各ステップ毎に独立していると仮定しているが、実際には数ステップにまたがって影響している。
こうした動的な環境に対応する方法として、状態拡張(state augmentation)と外れ値の除外(outlier rejection)という2つの基本技術がある。前者を適用することで、ロボットの姿勢だけでなく人の特性(位置、速度など)を推定できるフィルタを定義できる。ただし計算が複雑になると言った問題がある。後者は、センサデータがなぜこの値になったかの原因追求と、モデル化していない動体の影響を受けたと思われる高尤度なデータを排除する。
この方法は、本質的にはEMアルゴリズムと同じことを行う。ここで、観測モデルとして6.3節の式(6.12)を考え、新たに変数を導入する。
ここでは、を示すものとする。
アルゴリズムは上記の通り。入力はbeliefの代替であるパーティクルの集合とレンジファインダの観測データ、地図である。出力は入力したの"reject"か"accept"のどちらか一方である。
reject/acceptの判断は、観測結果が想定よりも驚くほど短いかどうかで行う。地図上に人などの動体が存在する場合、壁よりも近い場所に何かを検出することになる。そのため、観測結果が想定よりも顕著に短い場合はrejectする。
外れ値のrejectは、一般的には良い考え方である。測距計以外のセンサでも同様の考え方を使って観測モデルを更新できる。
Probabilistic Robotics - Chap.5 Robot Motion
5.1 Introduction
モーションモデルはベイズフィルタの予測ステップにおいて肝となる状態遷移確率を構成する。本章では、ロボットの運動はある平面上での移動のみを取り上げる。
5.2 Preliminaries
5.2.1 Kinematic Configuration
ロボットの姿勢は下記の式で表せるものとする。
ロボットの向きはよく、やと呼ばれる。また姿勢から向きを取ったものはと呼ぶ。
5.2.2 Probabilistic Kinematics
状態遷移確率はとして表される。はそれぞれ、時刻でのロボット状態を、は時刻でのモーションコマンドを、それぞれ表すものとする。モーションモデルにおいて、はオドメトリを指す場合もある。
本章では、下記2種類の確率的モーションモデルを取り上げる。
- 速度モーションモデル
なモデル。ただし速度を指令値通り制御することは、ノイズ等の都合上難しいことため、精度は良くないことが多い。
- オドメトリモーションモデル
なモデル。速度モデルよりも精度が高いことが多い。
ただしオドメトリは動いた結果わかる情報である。そのため、動作の予測には使えない。そのため、速度モデルはモーションプランニングに、オドメトリモデルは状態推定に、それぞれ使うことが一般的である。以降ではそれぞれについて詳細を述べる。
5.3 Velocity Motion Model
本モデルは、ロボットの速度は並行運動と回転運動の2種類で構成されると過程する。なおとは、それぞれ1次元のスカラ値である。
5.3.1 Closed Form Calculation
速度モデルは、下記のアルゴリズムで表現される。
ここでからは、ロボット固有の誤差パラメータでる。または、モーションエラーをモデル化した関数を表す。この固有パラメータの決定方法について、特に本書には記載がないです。恐らく指令値と実測値の組み合わせを記録していき、その結果が良い感じにの結果とフィットするようにパラメータチューンが必要と思われます。
5.3.2 Sampling Algorithm
5.3.1では計算で求めていましたが、本節ではパーティクルフィルタの様にサンプリングを介して解析に状態遷移確率を求める手法を紹介します。サンプル点から求める際の下記の手法を使う。
2~4行目は、速度パラメータにノイズを加えます。その結果を使い、5~7行目で姿勢を計算しています。サンプリング手法を使う場合、運動方程式等の物理モデルを考慮せず、サンプル点の位置からのみ姿勢が求まるという利点がある。そのため実装の観点からすると、サンプリング手法を利用ことが楽である。
5.4 Odometry Motion Model
オドメトリは、ドリフトやスリップによってエラーは生じるものの、速度に比べて計測誤差が小さい。また実際に動いた後でないと計測できないため、モーションプランニングには使えない。
5.4.1 Closed Form Calculation
オドメトリをベースとしたモーションモデルは、今日の確率ロボティクスベースのシステムで中心的な役割を果たしている。
オドメトリは、ロボット内部の情報量である。そのため、実世界とリンクするためには座標変換が必要である。ただしドリフトやスリップが生じるため、変換行列は一定ではない。なおこの変換行列を求めることは、自己位置推定を行うことと等価である。
オドメトリモデルは相対動作情報を使用する。相対動作情報とは、時刻からの間で、どの程度動いたかを表す情報である。ロボットの時刻との時の姿勢のペアをとすると、
となる。
この動作情報から相対動作情報を取り出すと、回転・直進・回転の3ステップに分割できる。つまり
と表せる。この情報が分かれば、ある時刻からの間の移動を再現できる。
確率的モーションモデルでは、上記3パラメータはノイズが加わっているものと仮定する。
以上を踏まえた、オドメトリモデルでの状態遷移確率を求めるアルゴリズムは下記の通りである。
なおは、平均で分散の誤差分布におけるの発生確率を返す関数とする。また、からは、ロボットの動作を特定するロボット固有のパラメータである。
上記アルゴリズムにおいて、各行が行っている内容は下記の通り
- 2~4行目:オドメトリ情報から相対動作情報を抽出。
- 5~7行目:入力した姿勢とから理想とする相対動作情報を算出。
- 8~10行目:相対動作情報の各要素に対するエラーを確率的に計算
- 11行目:相対動作情報の各要素に対するエラー発生確率をかけ合わせ、目的姿勢であるからのずれを返す。
5.5 Motion And Maps
今まで扱っていた状態遷移確率では、周囲環境を考慮していなかった。そこで状態遷移確率のパラメータとして、新たに地図を導入する。地図を知ることで、壁などの到達不可能な場所にいるのか、そうでない到達可能な場所にいるのかが判別できる。これにより、ロボットの姿勢をより精度良く求める事が可能となる。
地図の導入により、状態遷移確率はとなる。地図を知ることによって、今まで議論してきた状態遷移確率では「遷移可能」としてきた場所に遷移出来なくなるので、
となる。よって新規なモデルとなることから、マップベースモーションモデルと呼ぶことにする。
マープベースモデルを閉形式で計算することは難しい。一方で、都合の良いことに計算効率のよい近似方法がある。近似式は下記の通り。
ここでは正規化係数である。通常は一様分布で表される。なお周囲環境は動的に変化しないことから、は定数として考える。もしもの時は、ロボットは地図上に存在する障害物上に存在することを意味する。
なお上記近似には問題がある。モデルでは地図を考慮しているため、ロボットが存在できる姿勢は全て遷移対象となる。しかし、例えば壁の向こう側に一瞬で行くのは難しいが、そうした物理的に遷移が可能かは考慮していない。そのため、遷移先の姿勢への移動コスト加味するなどの工夫が必要である。
Probabilistic Robotics - Chap.3 Gaussian Filters
3.2 The Kalman Filter
カルマンフィルタは線形ガウスシステムでフィルタリング(つまり余計なものを省くプロセス[1])と予測を行うために開発された。カルマンフィルタは、連続状態空間でのbeliefを実装する。離散状態空間やハイブリッドな空間には対応しない。
時刻でのbeliefは、ベイズフィルタがマルコフ過程に加えて下記3点を満たす時、平均と共分散のガウス分布で表せる。
【1点目】
状態遷移確率が線形関数で表わせ、状態がガウスノイズを加味した下記の式で表せる。
ここで、は状態ベクトル、は制御ベクトルを、それぞれ表す。またはの正方行列で、は状態ベクトルの次元を示す。そしてはの行列で、は制御ベクトルの次元を示す。このように行列を導入することで、カルマンフィルタは入力である状態と制御の線形関数として状態遷移を表現する。
なおは、状態遷移の不確かさを表現するためのガウスノイズで、平均は、共分散は、次元は状態ベクトルと同じとする。
式を多変量ガウス分布の式に当てはめると、平均で共分散である
として表せる。
【2点目】
計測確率も、状態遷移確率と同様で入力に対して線形とする。また計測データは、ガウスノイズを加味した下記の式で表せる。
ここではの行列で、は計測ベクトルの次元とする。は、平均で共分散の計測ノイズである。計測確率は、と同様に多変量ガウス分布の形で、
として表せる。
【3点目】
最後に、初期belief は、平均で共分散のガウス分布
として表せる。
これら3つの仮定は、がガウス分布で表せることを確認するのに十分である。
3.2.2 The Kalman Filter Algorithm
カルマンフィルタの入力と出力は
入力:と制御ベクトル、観測ベクトル
出力:
である。アルゴリズムは下記の数式で表せる。
Table3.1 対象が線形な場合のカルマンフィルタアルゴリズム
なおはカルマンゲインと呼ばれ、状態遷移時に観測結果をどの程度考慮するかを調節する値である。
Table3.1の2,3行目は制御を加えた結果の予測過程を、4~6行目は観測結果を元に状態を更新する過程を、それぞれ示している。
それぞれの行に対する私の解釈は、
- 2行目:制御を加えたことによる平均の変化を予測
- 3行目:共分散は分散の2乗であるから、に対する二次形式になっている
- 5行目:予測した平均に対し、予測からどれだけずれたか、を考慮して平均を更新
- 6行目:共分散を求める式なので、の部分は必ず正になる。よってとわかる。このことから、共分散を抑えにかかっていると言える。
下記に私がPythonで書いたカルマンフィルタのコードを載せました。参考になれば。
3.3 The Extended Kalman Filter
3.4 The Unscented Kalman Filter(UKF)
UKFも非線形なシステムに対応するカルマンフィルタの1手法。線形回帰を使うことで、統計的に線形化する。回帰に使うサンプル点として、Sigma Point[3]を設定する。Sigma Pointは、平均を中心とし、かつ共分散行列の主軸に対して線対称になるように配置する。平均・共分散な次元のガウス分布については、個のSigma Pointを配置する。配置時のルールは下記の通り。
ここで、で、とはからどの程度離れるかを決めるスケールパラメータである。
また、各は2種類の重みとを持つ。は平均を、はガウス分布の共分散を、それぞれ計算する際に用いる。なおとは下記の式で求める。
ここでは、分布に関する追加情報をエンコードするためのパラメータであり、分布がガウス分布であればが最良となる。
Sigma Pointは、関数を使って写像される。写像後の平均と共分散はそれぞれ、
として表せる。
3.4.2 The UKF Alogorithm
UKFとEKFの入出力は同じだが、計算負荷はUFKの方が少し高い。また線形システムでは、UFKとLKFは同じ結果を示す。UKFの利点は、ヤコビアンの計算が不要な点である。ヤコビアンの計算には微分が必要であり、時には計算が難しい。そのためヤコビアンの計算を回避できることは計算上好ましい。なおUKFはパーティクルフィルタに似ているが、パーティクルフィルタはサンプル点をランダムに取るのに対し、UKFではサンプル点の取り方が固定という違いがある。
3.5 The Information Filter(IF)
LKFと同様に線形なシステムを仮定している。LKFとの主な違いは、ガウス分布の表現方法である。LKFとその派生は、ガウス分布をモーメント(平均と共分散)を使って表している。それに対しIFは、カノニカルな表現方法を使用する(*注意:僕、カノニカルとはどういうことかよくわかっていません!調べている途中です。。)。
IFとLKFは、状態遷移確率と観測確率それぞれを、LKFと同じ式で求める。またLKFと同様に、予測と更新の2ステップで構成される。IFの入力は、カノニカルなパラメータである。そして出力はである。なおの定義は下記の通り。
IFとLKF・その派生を比較した時、下記のようなメリット・デメリットがある。
- メリット
LKF・その派生よりも数値的に安定している(安定が曖昧だが、他のことばへの言い換えがわからない。。)
LKF・その派生よりもロボットの実問題へ当てはめやすい
- デメリット
更新時に状態推定をリカバーする必要がある
状態空間の次元が大きくなると計算コストが膨大になる
参考
[0]Probabilistic Robotics
[1]Filter (signal processing) - Wikipedia
[2]テイラー展開 - Wikipedia
[3]http://lab.cntl.kyutech.ac.jp/~nishida/lecture/psc/no8.pdf
Numpyでの行列の扱い方
Numpyで行列を作る場合、下記の3種類の方法がある。
①numpy.array([1, 2], [3, 4])
②numpy.matrix(numpy.array([1, 2], [3, 4]))
③numpy.mat(numpy.array([1, 2], [3, 4]))
上記の挙動について調べたことを下記に示す。
足し算・引き算
import numpy as np a = np.array([2, 2]) a.shape -> (2, ) b = np.array([[1, 2], [3, 4]]) b.shape -> (2, 2) a - b -> array([[1 , 0], [-1, -2]]) # なんと計算できちゃいます。aの1行目が2行目にもコピーされ、 # 2行2列の行列として扱われます。 c = np.matrix([[1, 2, 3], [4, 5, 6]]) c.shape -> (2, 3) a - c -> error! # 相手のデータタイプがmatrixだと、aとcのデータタイプが異なるからなのか、aは2行2列とは扱わないです。
掛け算
①の方法を採用する時、行列aとbの掛け算はnumpy.dot(a, b)とする必要がある。もしもa * bとすると、それは各要素毎に掛け算を行うことを意味する。②、③の時は、a * bでもnumpy.dot(a, b)のどちらでもよい。以上をコードで表すと下記の様になる。
import numpy as np a = np.array([[1, 2], [3, 4]]) b = np.array([[4, 3], [2, 1]]) a * b -> array([[4 , 6] [6 , 4]]) # a * b[0][0] = a[0] * b[0] となっている np.dot(a, b) -> array([[8 , 5], [20, 13]]) c = np.matrix(np.array([[1, 2], [3, 4]])) d = np.matrix(np.array([[4, 3], [2, 1]])) c * d -> matrix([[8 , 5], [20, 13]]) np.dot(c * d)-> matrix([[8 , 5], [20, 13]]) e = np.mat(np.array([[1, 2], [3, 4]])) f = np.mat(np.array([[4, 3], [2, 1]])) e * f -> matrix([[8 , 5], [20, 13]]) np.dot(e * f)-> matrix([[8 , 5], [20, 13]]) g = np.array([1, 2]) g * a -> error! np.dot(a, a) -> error! # 掛け算の場合、足し算・引き算と異なり、1行目は2行目にコピーされない
RTAB-Mapについてのメモ
最近3DSLAMに興味があり、ROSにも対応したRTAB-Mapをいじってみました。その過程でわかったことなどをメモします。ある程度纏まった量が出てくるまでは箇条書きになりますのでご容赦を(笑)
・Eigen3.3.3には対応せず。3.2.0はOK(RTAB-Map 0.12.5で)
Huawei P9 Liteでアプリによる通知を有効化する方法
先日Huawei P9 Liteに機種変しました。その後Wuderlist等のアプリをインストール・使ったところ、リマインダーの通知が一向に上がってこないという事態に陥りました。調べたところ、P9 Liteのバッテリーマネージャが原因だということがわかりました。本記事では、どうやって通知を有効化すればよいか、設定方法を記載します。
(少なくともP9 Liteでは)、一部のアプリを除き、画面オフ状態やそのアプリが表示されていない時(バックグラウンドでの実行状態)になるとアプリの動作が無効化されるみたいです。そのため、通知を有効化するためには、アプリの動作有効化が必要となります。有効化の方法は単純で、
「設定」→「詳細設定」→「バッテリーマネージャ」→「保護されたアプリ」
で有効/無効を、タップで切り替えるだけです。
上記「保護されたアプリ」の一覧でwunderlistを有効化することで、リマインダの通知が上がってくるようになりました。参考になれば幸いです。