第 5 章 幾何ラ ン ド マーク ベースの位置推定法における 磁場の併用 55
5.2 磁場およ び幾何地図を 併用し た Monte Carlo Localization
何地図ベースの位置推定法と 融合を 実現し た例は報告さ れて いない． Bentoら が提案し て い る よ う な磁気セン サの観測値の確率融合を 行う ためには， ロ ボッ ト が通過する 可能性のある 区間すべて の磁場を 地図化し なければなら ず， その地図化が難し い課題と なる ． 本提案手法 では， 厳密な 確率融合を 行う のではな く ， ヒ ュ ーリ スティッ ク な 方法によ り 融合を 行う ．
前章ま でに示し てき たよ う に， 走行経路の磁場し か地図化し て いない場合においても ， そ れを 利用する こ と で実用的なナビゲーショ ン は実現可能である ． そこ で， 走行経路上の磁場 地図を 併用する こ と で， 幾何ラ ン ド マ ーク ベース の位置推定法に おけ る 問題点の解決を 目 指す． 上述の通り ， 幾何ラ ン ド マ ーク ベースの位置推定法における 問題は， ラ ン ド マ ーク が 存在し な い地点に おいて 位置推定が行え な いこ と と ， 動的障害物の影響を 受け て 地図照合 に 失敗する こ と である ． 一方で磁場地図を 用いた位置推定法はこ れら の影響を 受け な い半 面， 位置推定精度を 高く 保つこ と が難し いと いう 欠点がある ． 本章において提案する 手法は，
Fig. 1.2に示すアイ ディ アを 基にする こ と で上記問題点を 解決し ， 磁場およ び幾何地図を 用 いて 位置推定法の利点を 併用する 手法である ．
5.2 磁場およ び幾何地図を 併用し た Monte Carlo Localization
提案手法は， Dellaertら によ っ て 提案さ れたMCL[12]を ベースと し た手法であり ， その 位置推定の大ま かな 流れは以下のよ う にな る ．
1. パーティ ク ルを ロ ボッ ト の動作モデルに従い更新 2. 磁場地図を 用いて ロ ボッ ト の姿勢を 推定
3. 上記姿勢推定結果に基づき パーティ ク ルの姿勢を 更新 4. 幾何地図を 用いて パーティ ク ルの尤度を 算出
5. 算出さ れた尤度を 基に自己位置を 更新
6. 尤度の高いパーティ ク ルの複製， およ び尤度の低いパーティ ク ルの削除
Fig. 5.1は， 上記の処理を 図示し たも のである ． 上記2， 3の処理が提案手法において 追加 さ れた新たな 処理である ， 既存のMCLには存在し な い処理である ．
MCLでは， 時刻1から tにおける セン サ観測値z1:t， 制御入力u1:tが与えら れた下で， 時 刻tにおけ る ロ ボッ ト の状態xtを 表す条件付き 確率分布を 求める こ と が問題と な る ． こ の 確率分布は， 以下に示すベイ ズフ ィ ルタ を 用いて 再帰的に計算可能である ．
p(xt|z1:t,u1:t) =ηp(zt|xt)
∫
p(xt|xt−1,ut)p(xt−1|z1:t−1,u1:t−1)dxt−1 (5.1)
5.2 磁場およ び幾何地図を 併用し たMonte Carlo Localization
Fig. 5.1: Processing image of the proposed method
こ こ で， ηは正規化係数， p(zt|xt)はセン サの観測モデル， p(xt|xt−1,ut)はロ ボッ ト の動作 モデルを それぞれ表す． 通常の幾何地図ベースの位置推定法において は， セン サ観測モデル はカメ ラ やLIDARのためのモデルが適用さ れる ． 対し て 本提案手法では， 幾何地図ベース の位置推定法と 磁場地図ベースの位置推定法を 組み合わせる ． そのため， セン サの観測は2 種類存在する こ と と な り ， 幾何情報に対する 観測gzと ， 磁場情報に対する 観測mzを 取得 する こ と ができ る ． こ れら の観測は独立である ため， 提案手法において は， 上式は以下のよ う に書き 換え ら れる ．
p(xt|z1:t,u1:t) =ηp(gzt|xt)p(mzt|xt)
∫
p(xt|xt−1,ut)p(xt−1|z1:t−1,u1:t−1)dxt−1 (5.2) こ こ で， p(gzt|xt)は幾何情報に対する 観測モデル， p(mzt|xt)は磁場情報に対する 観測モデ ルを それぞれ表す． し かし ながら ， 上述し た通り ， 磁場情報に対する 観測モデルを 構築する こ と は難し い． そこ で本提案手法では， 経験則に基づく ヒ ュ ーリ スティッ ク なモデルを 適用 する こ と で， こ の融合を 実現する ．
5.2.1 動作モデルによ る 状態更新
本研究では， Fig. 3.5に示すよ う な左右独立2輪駆動のロ ボッ ト を 使用する こ と と し ， か つロ ボッ ト は2次元(xy)平面を 移動する も のと する ． ロ ボッ ト の状態xは位置x, yと 姿勢 θで表すこ と と し ， その動作モデルを 以下のよ う に表す．
xt yt θt
=
xt−1 yt−1 θt−1
+
∆dtcosθt−1
∆dtsinθt−1
∆θt
(5.3)
こ こ でut = (∆dt, ∆θt)T はエン コ ーダに よ り 測定さ れた時刻t−1から tの間の移動距離 およ び角度変化量である ． ロ ボッ ト の状態は上式に従い更新し ， パーティ ク ルの状態はエン
5.2 磁場およ び幾何地図を 併用し たMonte Carlo Localization
コ ーダの観測値に正規乱数を 加え る こ と で更新する ． なお， エン コ ーダの値に従っ て 更新さ れたパーティ ク ルの分布を 提案分布(proposal distribution)と 呼ぶ．
5.2.2 磁場地図を 用いた姿勢推定
幾何地図ベースの位置推定は占有格子地図(occupancy grid map) [42]を 用いて 行う も の と し ， 磁場地図も 2次元グリ ッ ド マッ プで表現する こ と と する ． 提案手法では， 磁場地図を 用いて 姿勢推定を 行い， こ の結果を MCLへ反映さ せる ． な おグリ ッ ド のサイ ズは， 幾何・
磁場地図のど ち ら も 10 cmと する ．
姿勢推定法は， 前章4.4.2章において 述べた内容と 類似する が， 前章で述べた手法と はロ ボッ ト の状態が異なる (前章ではロ ボッ ト の状態を 走行距離のみで表現し たが， 本手法では ロ ボッ ト の状態は位置と 姿勢である)ため， 再度こ の内容を 記述する ． 磁気セン サは前章と 同様に， 3軸の磁場強度と その磁場方位を 観測でき る も のと する ． 提案手法における 姿勢推 定法では， z軸方向の磁場強度mzと xy平面におけ る 磁場方位θmを 用いる ． 磁場方位mθ
は， ロ ボッ ト の姿勢を θ， 磁気セン サの観測値を θsと し たと き ， 次式で計算さ れる ．
θm =θ−mθ (5.4)
なお， こ の関係を 示し た図がFig. 4.4である ． 磁場地図には， 地図構築を 行っ た際のこ れら の値が格納さ れて いる も のと する が， 値が格納さ れる のは， 地図構築の際にロ ボッ ト が通過 し た経路のみである ．
姿勢推定を 行う にあたり ， 時刻tにおける オド メ ト リ によ っ て 推定さ れたロ ボッ ト の状態 を xˆt， 磁気セン サの観測値を mtと する ． こ のと き ， ま ず式(4.5)， (4.13)に示すrz， rθの 値を 求める ． こ れら の値は， 磁場の時間変動を 評価する ための指標と なり ， 値が1に近い程，
観測値と 磁場地図を 構築し た際の変動が少ないこ と を 意味する ． そのため， こ れら の値に適 当な閾値を 設定し ， 磁場の変動がないと 判断さ れた場合に， 姿勢推定を 行う こ と と する ． 実 装では， こ れら の値は実験的にrz = 0.992， rθ = 0.990と 定めた． 磁場地図を 用いた姿勢推 定によ り ， ロ ボッ ト の姿勢θは以下のよ う に修正さ れる ．
θ=θm(ˆxt,yˆt) +mθ,t (5.5)
すな わち ， 磁場地図に 記録さ れて いる 磁場方位と 現在の磁気セ ン サの観測値を 用いて ， ロ ボッ ト の姿勢を 計算する 処理である ．
上記姿勢推定の処理は， 1ステッ プ間のオド メ ト リ によ る 位置更新には， ほと んど 誤差が 含ま れて いないこ と を 前提と し て いる ． こ の前提に基づき 推定さ れた結果は， ほぼ真値に近