(57)【特許請求の範囲】
【請求項１】
１）活動領域内で移動する目標に対して実時間測位を行うステップと、
２）前記移動する目標に対する測位に基づいて、目標が位置する活動領域の地磁気地図を実時間更新するステップと、
３）前記更新された地磁気地図に基づいて、前記目標が移動する次の位置に対して測位するステップと、
を備え、
前記目標の移動が止まるまで、ステップ２）と３）を繰り返して実行する、
目標の実時間測位及び地図構築の方法であって、
前記活動領域内で移動する目標に対して実時間測位を行うステップは、
活動領域内における、目標の走行距離と目標の回転角度値と磁界値を測定するためのセンサが設置される目標の位置と活動領域の全域地磁気地図を初期化するステップと、
空間補間法を用いて、目標の初期位置における地磁気値を、前記初期化を経過した前記全域地磁気地図に補間するステップと、
前記目標の走行における制御変量と前記目標が位置する現在位置の地磁気値を測定するステップと、
粒子フィルター算法を用いてランダムに移動する目標に対して位置推定を行うステップと、
を含んでいる、目標の実時間測位及び地図構築の方法。
【請求項２】
前記目標に対する測位に基づいて、目標が位置する活動領域の地磁気地図を実時間更新するステップは、
目標に対する位置推定が確定された後、前記地磁気のグリッド地図からその位置における目標点の周辺の予定領域のグリッド点データを抽出して、前記測定された地磁気値で補間し、そして、得られた予定領域のグリッド点を、予定領域のグリッド地図と統合させるステップを含んでいる、請求項１に記載の方法。
【請求項３】
前記活動領域内における目標の位置と活動領域の地磁気地図を初期化するステップは、
目標が活動する室内領域をグリッド座標として抽象化し、前記グリッド座標の大きさは、活動室の実際の大きさに比例して設置するステップを含んでいる、請求項１に記載の方法。
【請求項４】
クリギング補間法を用いて目標の初期位置の地磁気値を初期の地磁気地図に補間し、このステップは、
前記目標周辺の測定値に対して、下式（数１）で重み付けを行うことにより、測定しなかった位置の予測を得るステップを含み、
ここで、λiは第ｉ個位置における測定値の位置重みであり、Ｓoは予測位置であり、Nは測定値の数であり、前記重みλiは、測定点と、予測位置の距離と、予測位置周辺の測定値との間の空間関係を表すマッチングモデルによって決定される、請求項１から３のいずれか一項に記載の方法。
（数１）
0006400128.tif 000016
【請求項５】
前記マッチングモデルは円形であり、そのバリオグラムの表現式は、下式（数１２）であり、
ここで、Ｃ０はナゲット値であり、Ｃはシル値であり、αはレンジである、請求項４に記載の方法。
（数１２）
0006400128.tif 000017
【請求項６】
前記粒子フィルター算法を用いてランダムに移動する目標に対して位置推定を行うステップは、
目標が前の時刻から現在の時刻まで移動した距離とオフセットした角度を測定するステップと、
粒子の初期分布を均一分布として設定し、目標の移動につれて、前記粒子は、前記目標と同一な運動モデルに従って移動し、非線形動的モデル
0006400128.tif 000018
に基づいて、粒子の位置平均値から前記目標の位置を推定するステップと、
を含む、請求項１に記載の方法。
【請求項７】
前記運動モデルは、下式（数２）であり、
ここで、ltはｔ-１時刻からｔ時刻までの目標の移動距離であり、
θtはｔ-１時刻の目標に対するｔ時刻の目標の回転した角度であり、且つ回転は、真北方向を基准として、反時計回りに回転するものであり、
ｒａｎｄｎ（１,２）は、１×２のベクトルを生成する乱数であり、
ｗｇｎ（１,２,０.０５）は、０.０５ｄＢｗの１×２のガウスホワイトノイズを生成することを表示する、請求項６に記載の方法。
（数２）
0006400128.tif 000019
【請求項８】
測位モジュールと地図更新モジュールを備えている目標の実時間測位と地図構築装置であって、
前記測位モジュールは、活動領域内における目標の移動に対して実時間測位を行い、
前記地図更新モジュールは、目標に対する測位に基づいて、目標が位置する活動領域の地磁気地図を実時間更新し、
さらに、前記測位モジュールは、前記更新された地磁気地図に基づいて、目標の移動が止まるまで、目標の次の移動位置を測位し、
前記測位モジュールは、
目標の初期位置と地磁気のグリッド地図を初期化するように構成される初期化モジュールと、
粒子フィルター算法と運動学的モデルを用いて、目標の初期位置と運動過程における位置を推定するように構成される位置推定モジュールと、を含み、
前記地図更新モジュールは、
空間補間算法を用いて、測定された前記目標の活動領域の地磁気値を補間するように構成される補間モジュールと、
局所地図を全域地図と統合させる地図統合モジュールと、
を含む、
目標の実時間測位と地図構築の装置。
【請求項９】
前記位置推定モジュールは、
目標の位置推定に対して補正を行うように構成される粒子フィルター算法モジュールと、
予め決められた数学モデルによって活動領域内における目標の移動を分析するように構成される運動モデルモジュールと、
を含む、請求項８に記載の装置。
【請求項１０】
前記粒子フィルター算法モジュールは、
後の粒子集合の移動に原始参照を提供するように構成される初期化粒子分布モジュールと、
目標の移動過程における粒子に基づいて推定集合を生成するように構成される重要度再サンプリングモジュールと、
目標の特性に近づくほど重みが大きくなり、逆に、離れるほど小さくなるように、前記粒子に相関重みの大きさを与えるように構成される重み計算モジュールと、
前記粒子の重みに対して正規化処理を行うように構成される正規化重みモジュールと、
目標の特性に接近している粒子数を見出すように構成される有効粒子数確定モジュールと、
目標の移動位置に対する推定を補正するように構成される位置推定補正モジュールと、
を含む、請求項９に記載の装置。
【請求項１１】
前記重み計算モジュールにおいて採用するデータベースは、磁界の方向と強度に基づいてベクトル<Hx,Hy,Hz>で表す三つのグリッド地図データであり、各前記ベクトルはそれぞれ<Hx,Hy,Hz>の分布を表し、前記三つのグリッド地図のグリッドの大きさは同じである、請求項１０に記載の装置。
【請求項１２】
前記補間モジュールは、目標に対する位置推定が確定された後、地磁気のグリッド地図からその位置における前記目標の周辺の予定領域のグリッド点データを抽出して、前記測定された地磁気値を補間するよう構成されており、
前記補間された予定領域のグリッド点を予定領域のグリッド地図と統合する統合モジュールをさらに備える、請求項１１に記載の装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、室内測位及び地図構築の分野に関し、特に、室内目標の測位と地図構築を組み合わせた方法及び装置に関する。
【背景技術】
【０００２】
同時測位及び地図構築が解決しようとする課題は、目標によりノイズのあるセンサデータを用いて、環境における物体の分布状況を構築するとともに、自己位置を確定するものである。なお、測位及び地図構築に関するこの課題を解決することにより、目標は本格的な自律ナビゲーションを実現できる。過去十数年間、同時測位と地図構築の課題は、移動ロボットに関する分野において、多くの注目を集めている。通常の手法としては、ＥＫＦ(Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ：拡張カルマンフィルター)を用いて、ロボットの位置と地標を同時に推定するものである。
【発明の概要】
【発明が解決しようとする課題】
【０００３】
しかし、地標の位置が増えていき、ロボットの移動領域が広くなった場合、対応する上記方法の計算量も多くなり、実時間性が損なわれる。さらに、上記方法は、環境を事前に配置する必要があり、例えば、道路標識の位置を事前に配置する必要があり、それにより人的資源と物的資源が費やされている。
【課題を解決するための手段】
【０００４】
本発明は、従来技術における上記欠点に鑑みて、目標の測位及び地図構築の方法を提供する。上記方法は、運動モデルを用いて、現在の地磁気を測定することにより、現在の位置に対して補正を行い、また空間補間算法を用いて、局所地図におけるデータを更新して、全域地図と統合させることにより、目標の測位と地図構築を同時に実現するとともに、計算量を大幅に減らし、環境を事前に配置する必要もないことから、資源の節約を達成した。
【０００５】
本発明の一態様によれば、目標の測位及び地図構築の方法を提供する。上記方法は、１）活動領域内で移動する目標に対して実時間測位を行うステップと、２）移動する目標に対する測位に基づいて、目標が位置する活動領域の地磁気地図を実時間更新するステップと、３）更新された地磁気地図に基づいて、目標が移動する次の位置に対して測位するステップと、を備え、目標の移動が止まるまで、ステップ２）と３）を繰り返して実行する。
【０００６】
上記方法において、ステップ１）は、活動領域内における、目標の走行距離と目標の回転角度値と磁界値を測定するセンサが設置される目標の位置と活動領域の全域地磁気地図を初期化するステップと、空間補間法を用いて目標の初期位置における地磁気値を、初期化された全域地磁気地図に補間するステップと、目標の走行における制御変量と目標が位置する現在位置の地磁気値を測定するステップと、粒子フィルター算法を用いてランダムに移動する目標に対して位置推定を行うステップと、を含んでもよい。
【０００７】
活動領域内における目標の位置と活動領域の地磁気地図を初期化するステップは、目標が活動する室内領域をグリッド座標として抽象化し、グリッド座標の大きさを活動室の実際の大きさに比例して設置するステップを含んでもよい。活動室内における目標の初期位置は、目標の内部センサによって確定される。
【０００８】
空間補間法、例えばクリギング補間法を用いて測定された地磁気値を初期地図に補間し、目標周辺の測定値に対して下式（数１）で重み付けを行うことにより、測定しなかった位置の予測を得る。
【０００９】
【数１】
0006400128.tif 000002
【００１０】
ここで、数１に表された各パラメータの意味として、λiは第i個位置における測定値の位置重みであり、S0は予測位置であり、Ｎは測定値の数である。クリギング(Ｋｒｉｇｉｎｇ)手法を使用するとき、重みλiは、測定点と、予測位置の距離と、予測位置周辺の測定値との間の空間関係を表すフィッティングモデルによって決定される。
【００１１】
補間算法を用いて地磁気値を地図に補間するステップは、目標に対する位置推定が確定された後、地磁気のグリッド地図から、その位置における目標点の周辺の第１予定領域のグリッド点データを抽出して、測定された地磁気値を補間し、そして、補間し得られた第１予定領域のグリッド点を、第２予定領域のグリッド地図と統合させるステップを含んでもよい。
【００１２】
本発明のもう１つの態様によれば、さらに目標の測位と地図更新の装置を提供する。上記装置は、測位モジュールと地図更新モジュールを備え、そのうち、測位モジュールは、活動領域内における目標の移動に対して実時間測位を行い、地図更新モジュールは、目標に対する測位に基づいて、目標が位置する活動領域の地磁気地図を実時間更新し、さらに測位モジュールは、更新された地磁気地図に基づいて、目標の移動が止まるまで、目標の次の位置を測位し補正する。
【００１３】
測位モジュールは、目標の初期位置と地磁気のグリッド地図を初期化する初期化モジュールと、粒子フィルター算法と運動学的モデルを用いて目標の初期位置と運動過程における位置を推定する位置推定モジュールと、を含んでもよく、地図更新モジュールは、空間補間算法を用いて、測定された目標の活動領域の地磁気値を補間させるように構成される補間モジュールと、局所地図を全域地図と統合させる地図統合モジュールと、を含んでもよい。
【００１４】
位置推定モジュールは、粒子フィルターにより具現化され、目標の位置推定に対して補正を行うように構成される粒子フィルター算法モジュールと、予め決められた数学モデルによって活動領域内における目標の移動を分析するように構成される運動モデルモジュールと、を含んでもよい。
【００１５】
重みの確定において、磁界の方向と強度のベクトル〈Hx,Hy,Hz〉に対応するグリッド地図を用いてデータベースを構成して、対応するベクトルの分布をそれぞれ表してもよい。これにより、地図の特徴を増やすことができ、測位に有利である。
【００１６】
地図構築モジュールは、目標に対する位置推定が確定された後、地磁気のグリッド地図から、その位置における目標点の周辺の予定領域のグリッド点データを抽出して、測定された地磁気値を補間する補間モジュールと、補間された予定領域のグリッド点を予定領域のグリッド地図と統合させる統合モジュールと、を含んでもよい。
【発明の効果】
【００１７】
従来技術に比べて、上記された手段は、目標に対する実時間測位と実時間測位の結果に基づいて磁界地図を更新することを採用し、また更新された磁界地図に基づいて目標の移動をさらに測位することにより、測位と地図の更新を組み合わせて目標に対する高精度ですばやい測位と連続的に一致する地図の構築を実現する。
【００１８】
さらに、本発明は、広範で突き込んだ研究に踏まえて、数多くの手法から、生じる誤差を補正する粒子フィルター手法を選んで、室内の地磁気地図の更新を行い、さらに空間共分散を用いた最適補間法であるクリギング法を採用して補間を行うことから、結果の信頼性が高く、両手法の組み合わせにより、ロボットに対して、より高精度ですばやい測位と連続的に一致する地図の構築を実現できる。
【００１９】
現在、ほとんどの建物内には、鉄筋コンクリート構造で、家具などの磁界の波動をもたらす要素が存在するため、室内では磁界の波動が終始存在している。そして、本発明は、まさに室内の磁界波動を利用して測位を実現し、さらに実時間測位と地図構築を実現するものである。したがって、本発明を実現することにより、測位に要する参照点を別に配置することなく、費用の削減を実現できる。
【図面の簡単な説明】
【００２０】
【図１】本発明の一実施形態に係る目標測位方法のステップを示す模式図である。
【図２】本発明の一実施形態に係る活動領域内における目標の初期位置を初期化する模式図である。
【図３】本発明の一実施形態に係る活動領域の地磁気グリッド初期地図の模式図である。
【図４】本発明の一実施形態に係るクリギング補間法により目標の初期位置の地磁気値を初期地図に補間する模式図である。
【図５】本発明の一実施形態に係る移動目標に対して位置補正を行う模式図である。
【図６】本発明の一実施形態に係る目標の現在位置の地磁気値を測定する模式図である。
【図７】本発明の一実施形態に係る補間法により地磁気値を局所地図に補間する模式図である。
【図８】局所地図を全域地図と統合させる模式図である。
【図９】本発明の一実施形態に係る地磁気による目標の実時間測位と地図構築装置の構成を示す模式図である。
【発明を実施するための形態】
【００２１】
以下、図面を参照して本発明をより詳細に説明する。
【００２２】
図１は、本発明の一実施形態に係る地磁気による目標の実時間測位と地図構築の方法のステップを示す図である。図１に示すように、本発明の一実施形態に係る目標の実時間測位と地図構築の方法は以下に示すステップを含んでいる。
【００２３】
ステップＳ１０１、測位すべき目標が位置する活動区域（本実施形態では、1つの活動室を例とする）に対して実地測定を行うことにより、室内の平面図を取り、平面図に対して模擬グリッド地図の初期化を行う。測位すべき目標に対しては、目標の内部センサからのデータ（li,θi）を参照して、初期化された模擬グリッド地図におけるその位置を確定する。ここで、liは第i回の移動距離であり、θiは第i回の移動偏角である。図２は本発明の一実施形態に係る活動領域内における目標の初期位置を初期化する模式図である。図２に示すように、ここで、Ａは目標の初期位置を指しており、傍らの黒い点は、粒子フィルター算法における重要度サンプリング算法による位置空間でのサンプリング点であり、これらのサンプリング点の平均値は、目標の位置に対する推定を示している。
【００２４】
ステップＳ１０２、目標が初期位置に位置する場合、粒子はＡの周辺に均一に分布する。Ａの位置は、目標の内部センサによって測定される。目標の移動につれて、Xkは、運動モデルXk=f(Xk-1,VK-1,Wk)から、つまり、Xk-1と制御入力量VK-1から求められる。ここで、Xkはk時刻における状態であり、VK-1はk-１時刻における制御入力量（すなわち、目標の走行距離と回転角度）であり、Wkはk-１時刻からｋ時刻までの相関誤差である。センサ自身及び周辺の環境によって、測定するときにある程度の誤差があるはずであって、これらの不適切な制御情報に基づいて、高精度の測位を行おうとすることは、ロボットにとって難しいものである。しかしながら、本発明における誤差は、ランダムノイズとガウスホワイトノイズの累積和である。また、本発明では、以下に示す運動モデル（数２）を採用する。
【００２５】
【数２】
0006400128.tif 000003
【００２６】
ここで、ltはt-１時刻からt時刻までの目標の移動距離であり、θtはt-１時刻の目標に対するt時刻の目標の回転した角度であるが、回転は、真北方向を基準として、反時計回りに回転するものである。ｒａｎｄｎ（１,２）は、１×２のベクトルを生成する乱数であり、ｗｇｎ（１,２,０.０５）は０.０５ｄＢｗの１×２のガウスホワイトノイズを生成する。設定されたすべての粒子は、運動モデルによって移動し（図５に示す）、これにより、粒子の新しい位置に係るサンプリング値を得る。図５では、室内の平面図におけるサンプリング点の移動過程を示している。そして、目標が携帯している外部センサにより検出された磁界値に基づいて目標の現在位置を推定する。パラメータの入力を制御することにより、目標の大体の位置は推定可能であるけど、偏差があり、この偏差は蓄積されるものである。したがって、本発明の一実施例では、測定された位置が実際の位置と一致するように、目標の位置に対して更なる補正を行うことにより、偏差の蓄積をできるだけ解消する。
【００２７】
粒子フィルター算法の具現化は、以下に示す通りである。
【００２８】
初期粒子分布を設置することについて、目標（ロボット）の開始位置の周辺にある均一な分布を粒子の初期分布とし、生成される粒子X0(i)をＮｕｍ=１００個、粒子の重みをω0(i)とする。この仮定によれば、粒子数は１００となる。粒子フィルター算法は、計算量が多く、実時間性に乏しいのが一大欠点であって、そのうち、計算量の大きさを決めるキーポイントは粒子数である。したがって、本実施形態では、精度を確保できる範囲内で、粒子数を減らすことにより、計算量を削減し、実行可能性を確保することができる。これにより、粒子フィルターは、すばやく収束され、実時間的な運行を達成できる。
【００２９】
重要度再サンプリングについて、毎粒子に対して、状態をXt-1(i)からXt(i)に更新し、条件付き確率分布P(Xt(i)|Xt-1(i),Ut)によると、１００個の粒子の新しい位置を取得できる。ここで、Xt-1(i)はt-１時刻における第i個の粒子の状態であり、Utはt時刻における制御入力である。粒子は、目標が位置する位置空間からサンプリングしたものであり、これらの粒子の分布は、空間上の目標位置に対する確率分布を示している。
【００３０】
重み計算について、重みの計算理論は、下式（数３）である。
【００３１】
【数３】
0006400128.tif 000004
【００３２】
ここで、p(Xt(i)|zl:t,u0:t)は真分布であり、q(Xt(i)|zl:t,u0:t)は望ましい分布である。磁界の方向と強度は、ベクトル〈Hx,Hy,Hz〉で示す。
【００３３】
これにより、本例において採用するデータベースは、同じ大きさの３個のグリッド地図から構成され、Hx,Hy,Hzの分布をそれぞれ示している。この形態によれば、地図の特徴を増やすことができ、測位に有利である。重みの計算は指数分布に基づくものである。指数分布の計算複雑度（通常は時間複雑度で示す）はO(N)であり、ガウス分布の計算複雑度はO(N2)であり、Ｎは粒子数を示している。このことからわかるように、本例において、指数分布に基づいて重みを計算する手法は、計算量を減らし、収束速度を上げるとともに、測位の精度を高めることができる。上記指数分布の関数は、下式（数４）である。
【００３４】
【数４】
0006400128.tif 000005
【００３５】
ここで、R.magxは目標が現在位置点で測定された磁界のHx方向成分の値であり、mapxは現在の位置グリッド地図における現在位置の値であり、R.wは粒子の重みである。
【００３６】
グリッド地図における磁界値は、補間算法によって求められる可能性もあり、この前に目標がここを経過して直接に補間されたものである可能性もあり、R.magy、mapy、R.magz、mapzは、類似している。λの値は、実験から得たデータに基づいて適正な調整を行うことができるが、本実施形態では、２である。
【００３７】
正規化重みについては、下式（数５）で表される。
【００３８】
【数５】
0006400128.tif 000006
【００３９】
重みにより有効粒子を確定する場合には有効粒子数Neffを下式（数６）を用いて計算して、１つの閾値δを設定する。
【００４０】
【数６】
0006400128.tif 000007
【００４１】
例えば経験値に基づいて、粒子数の比率によって設定すると、総粒子数の７５%に設定することができる。有効粒子数がこの閾値より小さいなら、粒子の重み値ωk(i)に基づいて、粒子Xkの再サンプリングを行って、重み値の小さい粒子を取り除き、取り除かれた重み値の小さい粒子の数量によって、重み値の大きい粒子をコピーするから、粒子の総数は変えない。
【００４２】
目標の位置推定について、上記のサンプリングされた粒子に基づいて、粒子の空間上の位置分布の平均値を算出し、詳しくは、粒子の重み値を位置座標と掛け合わせてから、重ね合わせて得た乗積から、現在の目標位置を推定する。
【００４３】
ステップＳ１０３において、ステップＳ１０１で得られた室内の模擬グリッド図（図３に示す）において、クリギング補間算法を用いて、目標の外部センサから測定された値を模擬グリッド図に補間する。補間された効果図は図４に示す。図４は本発明の一実施形態に係るクリギング補間法により目標の初期位置の地磁気値を初期地図に補間する模式図であり、ここで、図の右側にある部分は、左側にある補間部分を一部拡大したものである。
【００４４】
シミュレーション実験において、１０ｍ×１０ｍの活動室の範囲内で、０.５メートルおきに１組の真の磁界値を測定して、合計２１×２１組を取得し、そして、この４４１組のデータに対して空間補間計算を行って、結果は、８１×８１組のデータを得ることにより、本発明による地磁気データベースを取得する。室内地磁気の波動の安定性に鑑みて、本実施形態では、この８１×８１組のデータを地磁気データベースと見なし、シミュレーションにおいて、目標が測定された実際値として用いる。
【００４５】
ステップＳ１０４において、ステップＳ１０３で得られた地磁気値は、まず局所地磁気に挿入される。局所地磁気は全域地磁気から抽出されるが、抽出規則は、推定による位置点のグリッド地図から、推定点を中心とした１０×１０のグリッドデータを抽出して補間を行う一方、もし推定点を中心として、１０×１０の局所グリッドが抽出されなかった場合、推定点を含むグリッドの片隅から１０×１０の地図を抽出するものである。例えば、推定点の座標（ｘ，ｙ）が（ａｂｓ(ｙ-１)＜５&&ａｂｓ(ｘ-１)＜５）に満足するとき、全域地図から、横座標点が１から１０まで、縦座標が１から１０までの区域を抽出する。なお、補間された局所地図（図７に示す）を全域地図と統合させる過程は、抽出の逆過程である（図８に示す）。これによれば、磁界データと推定点周辺の磁界値との関連性を配慮することができ、補間の計算量を減らすこともできる。
【００４６】
ステップＳ１０５において、活動領域内で、目標をランダムに移動させることにより、目標の内部センサが獲得した関連データを収集して、粒子の次の更新に原始参照を提供する。
【００４７】
ステップＳ１０６において、目標の運動が停止しているか否かを検知し、いいえなら、ステップＳ１０２に戻して、目標の運動が停止するまで、操作を繰り返す（反復）。
【００４８】
図９に示すように、本発明の一実施形態では、さらに目標の実時間測位と地図構築の装置を提供しており、上記装置は、
目標の初期位置と地磁気のグリッド地図を初期化するように構成される初期化モジュール２０１と、
粒子フィルター算法と運動学的モデルを用いて目標の初期位置と運動過程における位置を推定するように構成される位置推定モジュール２０２と、
空間補間算法を用いてセンサから測定された地磁気値に対して補間を行い、さらに局所地図を全域地図と統合させるように構成される地図更新モジュール２０３と、を備える。
【００４９】
ここで、位置推定モジュール２０２は、粒子フィルター算法モジュール２０２１を含み、粒子フィルター算法モジュール２０２１は、
後の粒子集合の移動に原始参照を提供するように構成される初期化粒子分布モジュール２０２１１と、
目標の移動過程における粒子に基づいて推定集合を生成するように構成される重要度再サンプリングモジュール２０２１２と、
目標の特性に近づくほど重みが大きくなり、逆に、離れるほど小さくなるように、粒子に相関重みの大きさを与えるように構成される重み計算モジュール２０２１３と、
粒子の重みに対して正規化処理を行って、すべての粒子の重みの和が１となるようにするように構成される正規化重みモジュール２０２１４と、
目標の特性に接近している粒子数を見出すように構成される有効粒子数確定モジュール２０２１５と、
目標の移動位置に対する推定を補正するように構成される位置推定補正モジュール２０２１６と、を含む。
【００５０】
上記モジュールの操作により、上記粒子フィルター方法を用いて、初期粒子分布、重要度再サンプリング、重み計算、正規化重み、有効粒子数計算に関する操作を行い、目標の（ロボット）現在位置を推定する。
【００５１】
運動モデルユニット２０２２は、予め決められた数学モデルによって活動領域内における目標の移動を分析するように構成されている。本実施形態では、非線形動的モデル、即ち、下式（数７）を用いる。
【００５２】
【数７】
0006400128.tif 000008
【００５３】
上記モデルでは、主に事前情報によって観測値を予測する。観測値を得た後は、上記式によりパラメータに関する事後情報を得て、それによって事前情報を修正する。さらに、修正された情報を事前情報として予測を行う。このように操作を循環することにより、絶えず観測値を得て目標位置の事前情報（前の位置）を修正する。本発明において、Xt=f(xt-1,vt-1)は、ステップ１０２における運動モデルと同一ものであるが、yt=h(xt,nt)によると、粒子は補間により生成された地図に基づいて、位置情報を修正する。
【００５４】
ここで、地図更新モジュール２０３には、補間モジュール２０３１が含まれている。
【００５５】
補間モジュール２０３１は、例えば、クリギング法により周辺の測定値に対して重み付けを行うことにより、測定しなかった位置の予測を得て、下式（数８）を用いてデータの重み付けの和から予測を得る。
【００５６】
【数８】
0006400128.tif 000009
【００５７】
ここで、λiは第i個位置における測定値の位置重みであり、S0は予測位置であり、Siは予測位置周辺の乱数点であり、Z(Si)は位置Siにおける磁界値であり、Nは測定値の数であるが、本実施形態では、１００となる。
【００５８】
ここで、グリッドにおいて、データ点は、座標点Si(ai,bi)（ｉ=１,２，...,ｎ）で表し、補間すべき点の座標はS0(a0,b0)で表す。さらに、散在しているデータ点の間の距離は、下式（数９）から求められる。
【００５９】
【数９】
0006400128.tif 000010
【００６０】
算出された距離値は、小から大への順番に従って並べられた後、いくつかの組に分けられ、毎組に特定数量の距離値を含む。これらの距離値は下式（数１０）に示す距離群（{h'm}で示す）を構成する。
【００６１】
【数１０】
0006400128.tif 000011
【００６２】
ここで、NHは距離群の個数を表す。算出された{ h'm }に基づいて、下式（数１１）から分布の形状を求めることができる。
【００６３】
【数１１】
0006400128.tif 000012
【００６４】
ここで、横座標はh'mであり、縦座標はγ*(h'm)であるが、連続的な曲線を生成して、γ*(h'm)の分布を表すためには、似合わしいモデルを選んでフィッティングする必要がある。
【００６５】
本実施形態において採用するセミバリオグラム（Ｓｅｍｉ-ｖａｒｉｏｇｒａｍ）によるフィッティングモデルは円形で、下式（数１２）である。
【００６６】
【数１２】
0006400128.tif 000013
【００６７】
ここで、C0はナゲット値(Ｎｕｇｇｅｔ)であり、Cはシル値(Ｓｉｌｌ)であり、αはレンジ(Ｒａｎｇｅ)である。
【００６８】
関数γ*(h'm) (離散的な分布である)の分布をフィッティングすることにより、モデルのパラメータ（例えばC0，C及びαなど）が求められ、したがって、本試験におけるサンプルにふさわしい分布、すなわち、バリオグラム（Ｖａｒｉｏｇｒａｍ）γ*(h)を得ることができる。
【００６９】
そして、方程式系（数１３）を用いて、ここで、μはＬａｇｒａｎｇｅ乗数であって、方程式系を解くことにより、重み値λi及びＬａｇｒａｎｇｅ乗数μを求めることができる。
【００７０】
【数１３】
0006400128.tif 000014
【００７１】
最後に、下式（数１４）から、S0(a0,b0)における予測値を求めることができる。
【００７２】
【数１４】
0006400128.tif 000015
【００７３】
地図統合モジュール２０３２は、全域地磁気から局所地磁気を抽出するよう構成されてもよい。抽出規則は、例えば、推定された位置点の全域グリッド地図(８１×８１)から、推定点を中心とした１０×１０の局所グリッドデータを抽出して補間を行う一方は、もし、推定点がグリッド地図の周縁に位置すると、推定点を含むグリッド片隅が１０×１０の地図を抽出するものである。補間された局所地図（図７に示す）の大きさは補間される前の地図と同じであるから、全域地図と統合させる過程（図８に示す）は、全域地図から抽出された局所地図を、抽出した位置に従って再び覆う（或いは嵌めこむ）ものである。上記の形態によれば、実質的には、局所地図と同じ大きさの１つの小さい地図（即ち、グリッド大きさとグリッドに分布するデータが同じである）が作成されている。そのうえ、補間された小さい地図は、大きさは変えずに、様式上の分布のデータを変えている。この補間された小さい地図を全域地図に覆う。このようにして、全域地図に対して補間することにより計算量が大きくなることを防止することができる。
【図１】
0006400128.tif 000020
【図２】
0006400128.tif 000021
【図３】
0006400128.tif 000022
【図４】
0006400128.tif 000023
【図５】
0006400128.tif 000024
【図６】
0006400128.tif 000025
【図７】
0006400128.tif 000026
【図８】
0006400128.tif 000027
【図９】
0006400128.tif 000028