自己位置推定実験

確率を ， 時刻1から tま でのセン サ観測z_1:tと 制御入力u_1:tが与え ら れた下で求める こ と が 問題と なる ． こ の確率は， 式(5.1)に示す漸化式で与え ら れる ． セン サ観測には3軸磁気セ ン サを 用いる ． 特に， 3軸の強度に分解し た磁場地図を 保有し て いる こ と から ， セン サ観測 zを [mx, my, mz]^T と する ．

通常MCLでは， 観測モデルp(zt|xt)を セン サの性能に合わせて 設計する ． 前章で述べた よ う に， 走行経路上の磁場のみし か地図化し て いない場合には， こ の観測モデルを 設計する こ と ができ ない． 一方で， 空間すべて の磁場を 地図化でき て いる 場合には， 観測モデルの設 計が可能と なる ． 本研究では， ガウ ス過程回帰によ る 磁場分布推定を 行っ て いる ため， 各地 点の磁場強度の平均と 分散を 得る こ と ができ る ． そこ で， 状態xに対する 3軸磁場強度の観 測値の平均ベク ト ルを µ_x， その分散共分散行列を Σ_xと し ， 観測モデルを 以下のよ う に定 める ．

p(zt|xt) = 1 (√

2π)³√

|Σ_x|exp (

−1

2(z−µ_x)^TΣ⁻¹_x (z−µ_x) )

(6.17) 本研究では， 提案する 自己位置推定法を Fig. 6.4に示すロ ボッ ト に実装する ． 本ロ ボッ ト の制御方式は， 前輪左右独立2輪駆動方式である ため， 動作モデルと し て は式(5.3)に示す を モデルを 使用する ． ま た， 磁気セン サは3台搭載さ れて いる ため， 各セン サの状態を x_i， 観測値を z_i (i= 1,2,3)と し ， 使用する 観測モデルを 以下のよ う 設定する ．

p(z_t|x_t) =

3

∏

i=1

p(z_i,t|x_i,t) (6.18)

ま た， パーティ ク ルのリ サン プリ ン グを 行う 際には， 式(4.11)に示すESSを 計算し ， その 値がn/2を 下回る 場合にリ サン プリ ン グを 行う ． こ こ で， nは使用する パーティ ク ル数を 表 し ， 実装ではn= 1000と し た．

6.8 _{自己位置推定実験}

Fig. 6.8に示す環境において ， Fig. 6.4に示すロ ボッ ト を 移動さ せた際のセン サデータ を 記録し ， こ れら のデータ と 構築し た2次元磁場地図を 使用し て 自己位置推定実験を 行っ た．

Fig. 6.15には， ロ ボッ ト の走行軌跡(赤線)， 磁場地図を 用いた位置推定結果の軌跡(青線)， およ びオド メ ト リ 軌跡(黒線)を 示す． なお， ロ ボッ ト の走行軌跡(真値)には， LIDARを 用 いたMCLによ っ て 推定さ れた値を 利用し て いる ． オド メ ト リ 軌跡が累積誤差によ っ て 真値 から 逸れている のに対し て， 磁場地図を 用いた自己位置推定を 行う こ と で， 真値の追跡が実 現でき て いる ． こ の結果から ， 磁場地図を 用いた自己位置推定によ り ， オド メ ト リ の累積誤 差を 修正でき る こ と が確認でき た． さ ら に Fig. 6.16には， 真値と 推定値の差分を 示す． 赤

6.8 自己位置推定実験

線は位置に対する 誤差(√

∆x²+ ∆y²)， 青線は角度に対する 誤差(∆θ)を それぞれ表し て い る ． こ の結果から ， 位置誤差0.8-m以下， 角度誤差0.14 radの精度で自己位置推定が実現 でき る こ と が確認でき た．

Fig. 6.15: Trajectories obtained from the magnetic map-based localization (a) [39]

Fig. 6.9内に示すA地点は， Fig. 6.10から も 確認でき る よ う に， 磁場計測があま り 密に行 われて いな い地点である ． 一方でFig. 6.13から わかる よ う に， こ の地点の磁場にも 磁場の 乱れが含ま れて いる こ と が確認でき る ． こ の地点において ， 同様の自己位置推定実験を 行っ た． Fig. 6.17には， 真値， 推定軌跡， およ びオド メ ト リ 軌跡， Fig. 6.18には位置と 角度に 対する 真値と 推定値の差分を それぞれ示す． 前述の実験と 同様に， オド メ ト リ の累積誤差を 磁場地図を 用いた自己位置推定によ り 修正でき て いる こ と が確認でき る ． し かし ながら ， 前 述の実験と 比較し て ， 位置推定の誤差が増加し て いる こ と が確認でき る ． 特に， 角度成分の 誤差が増加し て いる こ と が見て 取れる ． こ れは， 前回の実験と 比較し て 旋回が多く なっ たた めである と いえ ， 磁場地図を 用いた自己位置推定の精度は前述の実験と 同等である と いえ る ． こ の結果から ， MCLベースの磁場地図を 用いた位置推定が行え る こ と ， およ び構築し た磁場地図が十分な精度で実際の磁場を 表現し ている こ と が確認でき た． ま た重要な結果と し て ， Fig. 1.2に示すよ う に， 磁場地図を 用いる こ と でオド メ ト リ よ り も 高い精度で自己位 置推定を 行う こ と が確認でき た． すなわち ， 本提案手法によ っ て 構築し た磁場地図， およ び 前章で述べた2次元幾何地図を 併用する こ と がで， よ り ロ バスト かつ精度の高い自己位置推 定を 行う こ と が可能と なる ． さ ら に， 2次元の磁場地図を 用いる こ と で， 磁気セン サの計測 モデルに基づく 融合を 実現でき る ．

6.8 自己位置推定実験

Fig. 6.16: Localization errors (a) [39]

Fig. 6.17: Trajectories obtained from the magnetic map-based localization (b) [39]