JP5017392B2 - 位置推定装置および位置推定方法 - Google Patents

Description

本発明は、自動車や電車などの移動体の車載装置に関し、特に位置推定を行う機器に関する。

従来のカーナビゲーションシステムでは、測位結果を基にマップマッチングを行い、自車位置を求めていたが、特開２００８−２８７６６９号公報に示されているようなカーナビゲーションシステムから車両位置を車両運動制御機器へ出力する車両運動制御システムおいては、より正確な車両位置の測位が求められる。

正確な測位を行うため、自車位置の算出に用いる複数の数式モデルを用意し、事前に決められた一定の閾値に従い計算に用いる数式モデルを変更する技術が提案されている。例えば特開平８−１４９２３号公報には、グローバルポジショニングシステム（Global Positioning System：以下ＧＰＳ）の信号強度や衛星位置などの情報を用いて、ＧＰＳの出力する位置の誤差がある一定以上であれば、ジャイロや加速度センサなどの相対センサによる位置算出に切り替える装置が開示されている。

あるいは、カルマンフィルタなどの確率システムを用いて、自車位置を推定するシステムでは、特開平５−３３３１３２号公報のように、受信できたＧＰＳ衛星の数に対応したカルマンフィルタを複数事前に用意し、これらを切り替えて自車位置を測定する装置が提案されている。カルマンフィルタやパーティクルフィルタなどの確率モデルを用いた位置推定ロジックを用いて自車位置を推定する技術としては、拡張カルマンフィルタアルゴリズムを用いる発明が特開２００７−３２２３９１号公報に開示されている。

測位誤差に基づき位置算出手法を切り替える場合、舵角センサとジャイロセンサを搭載するカーナビゲーションシステムにおける測位誤差のモデルとしては、速度によるアッカーマンモデルやデッドレコニングモデルが知られており、また、ＧＰＳとジャイロセンサを搭載するカーナビゲーションシステムにおける、ＧＰＳを採用した場合のセンサフュージョンモデルやＧＰＳ非受信もしくは採用されなかった場合のデッドレコニングモデルが知られている。

アッカーマンモデルは、“安部正人，「自動車の運動と制御」，東京電機大学出版局，２００８年３月，ＩＳＢＮ：９７８４５０１４１７０００”に記載されている。センサフュージョンモデルと内界センサモデルは、“青野俊宏 他，「ＧＰＳと内界センサを用いた起伏地における移動機の位置計測」，計測自動制御学会論文集 Ｖol.３５，Ｎo.８，１９９９年”に記載されている。

さらに、自車位置をデジタル地図上にマッチングする際に、デジタル地図情報の中にあるリンク情報を使用する場合のセンサフュージョンモデル（以後、リンクフュージョンモデルと呼ぶ）として、特開２００８−２６２８２号公報に示されるような、デジタル地図データのリンクをカルマンフィルタの観測方程式として使う数式モデルが知られている。

特開２００８−２８７６６９号公報 特開平８−１４９２３号公報 特開平５−３３３１３２号公報 特開２００７−３２２３９１号公報 特開２００８−２６２８２号公報

"安部正人 著「自動車の運動と制御」 東京電機大学出版局 ２００８年３月刊 ＩＳＢＮ：９７８４５０１４１７０００" "青野俊宏 他著「ＧＰＳと内界センサを用いた起伏地における移動機の位置計測」 計測自動制御学会論文集 Ｖol.３５，Ｎo.８，１９９９年"

走行中の車両は、その路面状態や外気温や各部品の経年数、運転するドライバーの特性といった、前記のような誤差などの各種モデルを切り替えるための閾値の決定を事前に行うことが難しいような場合、事前に決められた不変の値では、モデル化誤差が大きくなり、自車位置の算出精度に大きな悪影響を与えかねない。またこの場合、車両運動制御に自車位置の算出精度が関わっている場合、車両制御の精度に悪影響を及ぼす可能性がある。

自車位置の推定精度をどのような場合でも高精度に保つためには、複数のモデルを切り替えるための閾値を事前に用意した不変な値とするのではなく、自車の状態、特に自車位置を算出する際に用いるために推定や学習されるパラメータ，変数や係数などを用いて算出される値を閾値としてモデルを変化させるような自車位置推定装置を用いることにより、解決される。

カーナビゲーションシステムやオートクルーズシステムなど、自身の絶対位置を高精度に推定しなければならないシステムにおいて、自車の状態を検出もしくは算出もしくは学習した値で、自身の絶対位置を推定するためのモデルを切り替えることで、どのような状態・状況でも常に高精度な位置を推定できるようになる。また、位置を高精度に推定するために必要な自身の誤差を元に、自車位置の信頼性を出力することが可能となる。

測位装置の概略構成を示す図である。 状態量と数式モデルの誤差の相関を示す図である。 閾値候補生成処理のフローチャートである。 閾値決定処理のフローチャートである。 数式モデル変数テーブルを示す図である。 数式モデル選択処理のフローチャートである。 アッカーマンモデルとデッドレコニングモデルによる数式モデルの車速に対する誤差変化を表した図である。 閾値ｖ_thの時間変化を表した図である。 アッカーマンモデルとデッドレコニングモデルの数式モデルテーブルを示す図である。 センサフュージョンモデルと内界センサモデルの数式モデルによる誤差変化を表した図である。 センサフュージョンモデルと内界センサモデルの数式モデルテーブルを示す図である。 リンクフュージョンモデルとＩＮＳモデルの数式モデルによる誤差変化を表した図である。 リンクフュージョンモデルとＩＮＳモデルの数式モデルテーブルを示す図である。

以下、本発明を用いた測位装置の詳細を説明する。

図１に本発明を用いた測位装置の概略構成を示す。測位装置は、カーナビゲーションシステムまたは専用端末上で実現され、測位装置が搭載された車両の現在の姿勢情報および位置測位情報を取得するセンサ１０１と自車位置推定装置１０２とで構成される。自車位置推定装置１０２は、カルマンフィルタなどの確率モデルに基づく数式モデルを用いて周期的に車両位置を算出する確率モデルフィルタリング処理部１０３と、この確率モデルフィルタリング処理部１０３で算出された速度などの車両の状態量やこの算出された状態量の誤差から、確率モデルフィルタリング処理部１０３で車両位置の計算に用いる数式モデルを変更するための閾値となり得る閾値候補の値を算出する閾値算出部１０４と、閾値算出部１０４にて求められた閾値候補の値および確率モデルフィルタリング処理部１０３で使用する数式モデルの特性を基に、確率モデルフィルタリング処理部１０３で計算に用いる数式モデルを変更する閾値を決定する閾値決定部１０５とを有する。

確率モデルフィルタリング処理部１０３は、自車位置を算出するために用いる車両の挙動を表す複数の数式モデルからなる数式モデル処理部１０６と閾値決定部１０５において決定された閾値に基づいて、数式モデル処理部１０６において遷移行列の各要素の値の算出に用いる数式モデルを選択する数式モデル選択部１０７を備え、カルマンフィルタなどの確率モデルにおける状態方程式などから求められる遷移行列の各要素の値を数式モデル処理部１０６において計算する。ここで、数式モデルとは、測位アルゴリズムのモデルを表す状態方程式もしくは観測方程式を構成する方程式一つ一つの式のことであり、識別番号で管理されるものとする。複数の数式モデルに対応した各々の状態方程式または観測方程式を組み合わせることにより、数式モデル処理部１０６では状態方程式や観測方程式が表され、この状態方程式と観測方程式によりカルマンフィルタなどの確率モデルが計算される。

閾値決定部１０５では、閾値算出部１０４で算出された閾値候補の値を保存し、数式モデル選択部１０７において数式モデル処理部１０６で計算に用いる数式モデルを選択し変更するために用いる数式モデルテーブル１０８に対して閾値を設定する。また、数式モデルテーブル１０８には、センサ１０１から入力される状態量を変数として各数式モデルの状態方程式または観測方程式を表したときの係数を格納した数式モデル変数テーブル１１１が格納されている。

車両の姿勢情報および位置測位情報を取得するセンサ１０１からは、自車の姿勢や移動方向，移動速度、あるいは大局的な位置のデータが自車位置推定装置１０２に入力される。確率モデルフィルタリング処理部１０３ではこれらの入力値に基づき、数式モデル選択部１０７によって、数式モデル処理部１０６で遷移行列の計算に使用する数式モデルが選ばれ、この数式モデルの計算により求めた位置を出力する。また、数式モデル処理部１０６において数式モデルで算出された位置以外の状態量は閾値算出部１０４へ入力される。

閾値算出部１０４では数式モデル処理部１０６に用意された複数の数式モデルの特性に従い、各数式モデルにおける状態量と誤差の関係を計算して、その数式モデルについての閾値の候補を決定する。算出された閾値の候補は閾値決定部１０５へ入力され、複数の数式モデルについて適用時の誤差の大きさおよびその時の状態量に基づき閾値を決定して、数式モデル選択部１０７から参照できるようにするため数式モデルテーブル１０８に格納する。閾値決定部１０５にて決められた各状態量毎の閾値は確率モデルフィルタリング処理部１０３内の数式モデル選択部１０７から参照され、次の位置計算の周期の際に、車両の姿勢情報および位置測位情報を取得するセンサ１０１からデータが自車位置推定装置１０２に入力された際に、数式モデル処理部１０６の複数の数式モデルの中から最適な数式モデルを選択するために使用される。この流れを周期的にループ処理することによって、各状態量に対して最適な閾値が順次決定され、数式モデルテーブル１０８が更新されていく。

車両の姿勢情報および位置測位情報を取得するセンサ１０１からは、自車両の移動する方位，自車両の傾きなど車両の姿勢，自車両の絶対位置，自車両の移動する速度，自車両とその他の物体との相対的な位置および方位・姿勢の関係などの情報が出力される。

車両の絶対位置は、ＧＰＳなどのように直接位置を算出できるセンサや、絶対位置と形状が既知の物体の相対的な位置を計測するカメラやレーダセンサなどのセンサにより算出する。

車両の移動する方位は、直接絶対方位を計測できる地磁気センサや、初期方位が与えられた上で計測した角速度を積分することで方位を算出するジャイロセンサのような方位センサや、あるいは異なる時間に受信したＧＰＳの信号により算出する。

車両の移動する速度は、直接速度を算出できる車速パルスのセンサや、車両の加速度を検知して積分することで速度を算出する加速度センサ、あるいは異なる時間に受信したＧＰＳにより算出する。

車両の姿勢は、加速度センサなどのように重力の向きを検知することで車両の傾きを検出する姿勢検知センサや形状が既知の物体に対する相対的な姿勢を計測できるレーダセンサや、あるいは形状が既知の物体の自車に対する傾きを計測できるようなカメラの画像により算出する。

確率モデルフィルタリング処理部１０３では、数式モデル処理部１０６が、カルマンフィルタやパーティクルフィルタなど、確率モデルに基づく位置推定ロジックを用いた複数の数式モデルにより位置を推定する。位置推定ロジックの代表的な例としては、前述のような拡張カルマンフィルタアルゴリズムによる自車位置推定がある。確率モデルに基づく位置推定ロジックでは、センサ１０１からの出力もしくは数式モデル処理部１０６で算出された自車両の方位・位置・姿勢速度の情報を、その誤差を考慮した観測方程式として表し、ある時間から次の時間へと遷移する自車の方位・位置・姿勢・速度を状態方程式として表す。数式モデル処理部１０６が備える各数式モデルは、この観測方程式及び状態方程式を計算してフィルタリング処理を行う。

数式モデル処理部１０６では、自車両の位置と共に、方位・姿勢・速度に関する情報も推定する。これらの値を状態量と呼ぶ。算出された状態量は、閾値算出部１０４に出力される。また、数式モデル処理部１０６において、誤差や誤差の分散を算出する数式モデルを用いて計算を行う場合、その誤差も出力する。

閾値算出部１０４では、確率モデルフィルタリング処理部１０３から入力された状態量に対する数式モデル処理部１０６が備える複数の数式モデルの誤差を見積もる。図２に数式モデルにおける状態量とその誤差の相関例の模式図を示す。図２の横軸は、確率モデルフィルタリング処理部１０３から入力された状態量、縦軸はその状態量と共に確率モデルフィルタリング処理部１０３から入力された誤差の値であり、数式モデル毎の状態量に対する誤差の値を示す。曲線２０１はある数式モデル１における、状態量に対する誤差の変化を表した曲線であり、曲線２０２は数式モデル２における状態量に対する誤差の変化を表した曲線である。曲線２０１と曲線２０２の交点Ａ２０３が数式モデル１と数式モデル２の誤差が逆転する点であり、ここから右の状態量の場合には数式モデル１による計算のほうが誤差が小さく、逆にここから左の状態量の場合には数式モデル２を用いて計算したほうが誤差が小さくなることになる。そこで交点Ａ２０３の状態量の値Ｂ２０４が、フィルタリング処理に用いる数式モデルを切り替える判断を行う際の閾値候補となり、この閾値候補を閾値決定部１０５へ出力する。

閾値算出部１０４における閾値候補の生成処理を図３に示す。この処理は、数式モデル処理部１０６が管理する数式モデルの内、２つの数式モデルについてその誤差を比較して、各２つの数式モデル間についての閾値候補を求める処理である。まず、確率モデルフィルタリング処理部１０３から、数式モデル処理部１０６で計算された状態量を取得する（ステップ３０１）。取得した状態量は、それぞれ計算に用いられた数式モデル毎に最新の値から多くとも所定数分まで記憶しておく。次に、数式モデル処理部１０６が有する複数の数式モデルの中から、ステップ３０１で取得した状態量を用いている複数の数式モデルを探してリストを作り、リストの中の各数式モデルについて数式モデル変数テーブル１１１を参照して状態量に対応した係数を取得（以下、状態量に対応した数式モデルの係数を取得することを、数式モデルを取得する、と呼ぶ）して状態方程式または観測方程式を構成した上で、取得した状態量に基づき計算を行い、各数式モデルについてその誤差を取得する（ステップ３０２）。数式モデル変数テーブル１１１には、予め閾値候補を決定するために必要な数式モデルのみが設定されているものとする。また、取得した数式モデルの誤差はそれぞれ計算に用いられた数式モデル毎に、最新の値から多くとも所定数分まで閾値算出部１０４で記憶しておくものとする。

そして、ステップ３０１で取得した状態量の中から、数式モデルにおける誤差比較に用いる状態量（これを、支配的な変数となる状態量と呼ぶ）としている数式モデルがステップ３０２で作成したリストに存在する未処理の状態量が残っているか調べる（ステップ３０３）。支配的な変数となる状態量は、数式モデルテーブル１０８に格納されている数式モデル変数テーブル１１１に予め定義されているものとする。数式モデル変数テーブルは図５に示すように、数式モデル処理部１０６が備える各数式モデルについて、各々の数式モデルが処理する状態量に対応した変数の変数名と、その変数が支配的であるか否かを表すフラグが記憶されている。数式モデル変数テーブル１１１は、予め閾値候補を決定するために必要な数式モデルのみが設定されており、このテーブルの順番で選択の対象となる数式モデルが存在するかどうかを数式モデルテーブルの番号順に検索する。未処理の状態量があれば、その支配的な変数となる状態量を選択する（ステップ３０４）。

次に、選択した状態量を支配的な変数とする数式モデルをステップ３０２で作成したリストを先頭から順に探し、最初に見つかった未処理の数式モデルの次から、同じ状態量を支配的な変数とする数式モデルを探すことにより、同じ状態量を支配的な変数とする数式モデルの対を選択する（ステップ３０５）。対象となる数式モデルの対が一つでも見つかった場合、数式モデル変数テーブルの番号と数式モデルテーブルの番号を一時保存し、ステップ３０６，３０７へ進む。

ここでは、対処となる数式モデルが少なくとも１つは存在するものとして、次にステップ３０６，ステップ３０７にて、誤差比較する２つの数式モデルに対して、ステップ３０４で選択した状態量の変化に対する誤差を、ステップ３０２で取得した誤差を設定し、図２における曲線２０１および曲線２０２を決定していく。そして、ステップ３０６およびステップ３０７で算出もしくは推定した誤差に基づき決定した曲線同士の交点を算出し、その交点における状態量の値を算出する（ステップ３０８）。算出された状態量は、閾値候補として閾値決定部１０５へ出力もしくは、出力待機としてバッファリングされる（ステップ３０９）。

そして、ステップ３１０にてステップ３０２で取得した各数式モデルのリストに対して、選択した状態量を支配的な変数とする数式モデルを先頭方向から順に探し、見つかった未処理の数式モデルの次から、また同じ状態量を支配的な変数とする数式モデルを探すことを繰り返して、支配的である状態量が重なる数式モデルの全ての組み合わせに対して、ステップ３０６からステップ３０９の処理を完了したかどうかを判断し、全て計算が完了していた場合（ステップ３１０：ｙｅｓ）、即ちステップ３０４で選択した状態量を支配的な変数とする数式モデルの対を全て処理し終えていた場合には、ステップ３０３に戻る。またこの時、ステップ３０９にて出力待機としてバッファリングしたデータが存在する場合は、このタイミングで閾値決定部１０５へ出力しても良い。一方、全ての数式モデルの対について計算が完了していない場合は、まだその他の選択対象となる数式モデルが存在する可能性があるためステップ３０５へ戻り、上記の計算を繰り返す（ステップ３１０：ｎｏ）。

また、ステップ３０３の判定で、ステップ３０２で作成したリストの数式モデルが支配的な変数としている状態量の内、未処理の状態量が残っていない場合（ステップ３０３：不可）、閾値算出部１０４における閾値候補の生成処理を終了する。

閾値決定部１０５では、閾値算出部１０４から入力された閾値候補を用いて、各状態量の閾値を決定する。決定された閾値は数式モデルテーブル１０８に出力される。なお、閾値決定部１０５は、閾値算出部１０４からの閾値候補を記憶しておく記憶領域（図示せず）を備えている。

閾値算出部１０４から入力された閾値候補から各状態量の閾値を決定する閾値決定処理を図４のフローチャートに示す。まず、数式モデルテーブル１０８の数式モデル変数テーブル１１１から、数式モデル処理部１０６が有する複数の数式モデルを取得する（ステップ４０１）。次に、閾値算出部１０４から出力された閾値候補を取得して（ステップ４０２）、これまでの閾値候補を全て保存している領域に更に今回の閾値候補の値を保存する（ステップ４０３）。これまで保存されていた閾値候補は、メモリ量の上限が設定されている場合には、平均値などのような代表点に置き換えても良い。

次にステップ４０４にて、ある閾値候補の状態量を選び、選んだ閾値候補の状態量が誤差に影響する数式モデルが存在するかを判断する。そして、閾値候補の状態量が変動すると誤差が変動するような数式モデルが存在する場合、ステップ４０５にて、閾値候補の状態量の特性が時変な閾値候補（時間変動する状態量）と時不変な閾値候補（時間変動しないはずの状態量）とで処理を分ける。状態量が時変か時不変であるかは、後述する数式モデルテーブル１０８で分類されて設定されているため、これを参照することになる。閾値候補が時変な状態量であった場合、ステップ４０６へ移り、これまでの閾値候補の値を踏まえた逐次学習法、例えば、カルマンフィルタや最小２乗法などを用いて時変な閾値の変動に追従できるよう逐次学習法のゲインを設定する。そして、ステップ４０７にて、ステップ４０６からの逐次学習法による追従の最終出力値を閾値として決定する。次に、ステップ４０８にて数式モデルテーブル１０８の対象行もしくは対象列に対して、ステップ４０７にて決定した閾値よりも大きい場合に誤差が小さくなるような数式モデルおよび、ステップ４０７にて決定した閾値よりも小さい場合に誤差が小さくなるような数式モデルを数式モデルテーブル１０８の対象となる要素を選択し、数式モデルを表す番号を設定する。図２を例にとると、閾値の対象となる状態量の値がＢより小さい場合は数式モデル２の方が誤差が小さいので、数式モデルテーブルの要素に「２」を設定し、閾値の対象となる状態量の値がＢより大きい場合は数式モデル１の方が誤差が小さいので、数式モデルテーブルの要素に「１」を設定する。

また、閾値候補が時不変な状態量であった場合、ステップ４０９へ移り、閾値候補の平均値をとる手法や、これまでの閾値候補の値を２つに分類するクラスタリングの手法、例えば、“Christopher M. Bishop 著「パターン認識と機械学習」下 シュプリンガー・ジャパン ２００８年７月１日刊 ＩＳＢＮ：９７８４４３１１００３１７”に記されているようなサポートベクターマシンなどを用いて閾値の決定を行う。そして、ステップ４１０にて、ステップ４０９からの最終出力値を閾値として決定する。次に、ステップ４１１にて数式モデルテーブル１０８の対象行もしくは対象列に対して、ステップ４１０にて決定した閾値よりも大きい場合に誤差が小さくなるような数式モデルおよび、ステップ４１０にて決定した閾値よりも小さい場合に誤差が小さくなるような数式モデルを数式モデルテーブル１０８の対象となる要素を選択し、数式モデルを表す識別番号を設定する。

次に、ステップ４１２にて、ステップ４０７もしくはステップ４１０によって決定された閾値が全ての数式モデルに対して決定されていることを確認する。これは、数式モデル中に複数の閾値（状態量）が存在することを許容するため、唯一の閾値（状態量）の決定だけでは不足するためである。もし、全ての数式モデルに対して閾値が決定されていない場合、即ち、閾値が決定されていない数式モデルが存在する場合にはステップ４０４に戻り、さらにその他の閾値の決定を行う。全ての数式モデルに対して閾値が決定されている場合、ステップ４１３に進み、これまでの閾値として決定した値を削除して新たにステップ４０７もしくはステップ４１０にて算出された値を閾値として更新する。最後にステップ４１４にて、新たに決定した閾値を数式モデルテーブル１０８に設定する。以上の処理により、閾値候補が算出されていた状態量に対して閾値が１つ決定され、数式モデル処理部１０６にある全ての数式モデルに対して少なくとも１つの閾値が決定されることになる。

次に、図９に数式モデルテーブル１０８の構成を示す。このテーブルの縦方向には、誤差の大きさを決定するために必要な数式モデルの状態量が並ぶ。この状態量は確率モデルフィルタリング処理部１０３にて計算される状態量であり、時変な状態量とする。この誤差の大きさを決定するために必要な数式モデルとは、例えば、確率モデルフィルタリング処理部１０３でカルマンフィルタが用いられていた場合は、観測方程式で用いられる。

誤差の大きさを決定するために必要な数式モデル内に低速度状態ではモデル化誤差が大きいようなモデルがある場合、誤差の大小により低速度走行時と高速度走行時の二つの状態が存在することになり、どちらの状態かによってモデルの誤差の大小が分かれる。また、横方向には、確率モデルの内部状態を表す状態量を置く。これは主に、確率モデルフィルタリング処理部１０３にて計算される状態量から計算される状態量であり、時不変な状態量とする。縦方向の数式モデル分類用に置いた状態量であっても、確率モデルの内部状態を表す状態量であれば時間変動が小さいものと見なして、横方向の状態量に置いてよい。

時不変な状態量として、例えば、確率モデルフィルタリング処理部１０３にて計算される状態量の中にセンサの取付角の誤差が存在していた場合、この状態量は横方向に並べた状態量の一つに含まれ、学習が完了と未完の二つの状態が存在することになる。このような学習の完了・未完の判断は、確率モデルフィルタリング処理部１０３の中で、フィルタリング処理とは別に計算することになる。上記の例では、予め定められた分散値をある一定時間内のセンサ取付角の誤差の計算結果の分散値で割ることで、どれほどセンサの取付角の誤差が揺れているか、つまり学習が進んでいるかを数値化できる。この数値を、学習度と呼び、ある値を閾値として学習の完了・未完を判断する。このような内部状態を表す状態量としては、その他にジャイロセンサのバイアスの学習やスケールファクタの学習、舵角特性の学習などがある。学習度は１以上の値となった状態を、理論上学習が完全に終わった状態とする。

数式モデルテーブル１０８の内部には、縦軸と横軸で選ばれた状態量の状況の組み合わせで最適なモデルが設定される。この最適なモデルとは、ステップ４０８もしくはステップ４１１にて設定される数式モデルである。数式モデル処理部１０６の中には排他ではない複数の数式モデルが存在しても良い。このため、数式モデルテーブルの内部の縦軸と横軸で選ばれた状況に対しては、複数の数式モデルが設定されうる。

ステップ４１３にて保存された閾値は、ステップ４１４にて、縦軸もしくは横軸の閾値として設定される。ステップ４１４にて設定された閾値が、速度１０km／ｈおよびセンサ取付角学習度０.１であるとすれば、縦軸内の低速度走行は１０km／ｈ未満を示し、高速度走行は１０km／ｈ以上を示すことになる。また同様に、横軸において、センサ取付角学習度が０.１以下の場合を学習未完、０.１以上を学習完了として設定することになる。

数式モデル選択部１０７では、閾値決定部１０５で作成された数式モデルテーブル１０８に従い、現在の車両状態に最も適した数式モデルを複数選択する。図６に数式モデル選択部１０７における複数の数式モデル処理部１０６から最適なモデルを複数選択する処理のフローチャートを示す。まず、ステップ６０１にて、数式モデルテーブル１０８を読み込む。次に数式モデル処理部１０６で前回計算された車両の挙動や姿勢・状態を表す状態量を読み込む（ステップ６０２）。次に、読み込んだ各状態量が数式モデルテーブル１０８に設定された閾値の縦軸側と横軸側のどちら側に属するかを判定し、数式モデルテーブル１０８から縦軸と横軸について計算された車両の状況（状態量）の値と条件が合致する行・列を選択する（ステップ６０３）。ここで選択する行および列は複数であって良い。そしてステップ６０４にて、選択した行・列内で設定されている対応する状況に最適な数式モデルを読み込む。

次にステップ６０５にて、ステップ６０４で選択された全ての数式モデルについて重複の有無を確認し、重複するモデルがある場合、ステップ６０６に移行して、重複する数式モデルが無いようにしてから数式モデル処理部１０６における計算対象の数式モデルとして設定する。これは、数式モデル処理部１０６にて、同じ方程式を重複して使用することを避けるためである。また、ステップ６０５にて、選択した数式モデルに重複する数式モデルが存在しない場合、ステップ６０７に移行して、選択した全ての数式モデルを数式モデル処理部１０６に設定する。

数式モデル処理部１０６では、数式モデル選択部１０７で選択された数式モデルのみを用いて位置やその他の状態量を算出する。数式モデル処理部１０６は全体でＮ個の数式モデルを管理しているとすると、数式モデル選択部１０７ではその内のｎ個が選択されることになる。（ただし、ｎはＮ以下の数。）数式モデル選択部１０７では、選択したｎ個の数式モデルに対応した、確率モデルの状態方程式や観測方程式を構成し、数式モデル処理部１０６ではカルマンフィルタなどの確率モデルフィルタから、位置やその他の状態量を算出する。位置やその他の状態量は、予め決められた数が算出され、数式モデル選択部１０７で選択された数式モデルの数に依存しない。誤差の少ない数式モデルを選択して確率モデルフィルタを計算することで、誤差の大きい情報を含む全ての数式モデルを使用して状態量を推定するよりも精度の高い推定が可能となる。数式モデル処理部１０６において、数式モデルの設定は、例えば以下のようにして実現される。

数式モデル処理部１０６が所持するＮ個の数式モデルの内、数式モデル選択部で、数式モデル１，数式モデル２，数式モデル３が選択されたとする。数式モデル１が（式１）のように表され、また、数式モデル２が（式２）のように表され、数式モデル３が（式３）のように表されるとする。ここで、Ｃ_iは数式モデルｉの観測値、（ｘ，ｙ，ｚ）は確率モデルフィルタリング処理部１０３から出力される全ての状態量、Ａ_i1〜Ａ_i3は数式モデルｉにおけるそれぞれの状態量の係数である。

Ｃ₁＝Ａ₁₁ｘ＋Ａ₁₂ｙ＋Ａ₁₃ｚ …（式１）
Ｃ₂＝Ａ₂₁ｘ＋Ａ₂₂ｙ＋Ａ₂₃ｚ …（式２）
Ｃ₃＝Ａ₃₁ｘ＋Ａ₃₂ｙ＋Ａ₃₃ｚ …（式３）
これらの数式モデルが、カルマンフィルタにおける観測方程式として用いられる場合、ステップ６０７にて全ての数式モデルが重複しなければ、観測方程式は（式４）のように表される。

そして、観測行列Ａの各要素（Ａ₁₁〜Ａ₃₃）は、数式モデル変数テーブル１１１から、各数式モデルで用いられる状態量（ｘ，ｙ，ｚ）に対応して設定されている係数が読み込まれて設定される。なお、数式モデル変数テーブル１１１に状態量に対応した係数が設定されていない要素については０が設定されるものとする。実際のプログラムでは、選択された数式モデルの状態量の係数がまとめられた観測行列Ａの各要素（Ａ₁₁〜Ａ₃₃）がｉｆ文などにより設定され、カルマンフィルタに用いられる。

また、数式モデル処理部１０６が所持するＮ個の数式モデルの内、数式モデル選択部で、数式モデル１，数式モデル２が選択されたとする場合、同様にして観測方程式は（式５）のように表され、観測行列Ａ′の各要素（Ａ′₁₁〜Ａ′₂₃）が同様に設定される。

そして（式４）の場合と同様に、観測行列Ａ′の各要素（Ａ′₁₁〜Ａ′₂₃）は、数式モデル変数テーブル１１１から、各数式モデルで用いられる状態量（ｘ，ｙ，ｚ）に対応して設定されている係数が読み込まれ、設定される。

次に、本測位装置が、舵角センサとジャイロセンサを搭載するカーナビゲーションシステムに適用され、数式モデルとしては速度によるアッカーマンモデルとデッドレコニングモデルを用い、確率モデルフィルタリング処理部１０３では拡張カルマンフィルタを用いている場合を例に、本測位装置の具体的な動作を説明する。

アッカーマンモデルは低速域で成り立つモデルであり、その角速度ωは、車両速度をν、実舵角をδ、前車軸から後車軸までの距離をｌとして、以下の（式６）のように表される。

ω＝ν・δ／ｌ …（式６）
一方、デッドレコニングモデルは、ジャイロセンサからの出力値ω_gyroを用いて角速度を算出するため、角速度ωは、ジャイロセンサの感度係数をＳ、バイアスをbiasとして、以下の（式７）のように表される。

ω＝Ｓ・ω_gyro＋bias …（式７）
また、ある時刻をｔ、単位時間をΔｔとして、２次元平面内の車両の位置および方位（ｘ，ｙ，θ）は、速度をｖとした次の式で表される。

よって、これら２つの数式モデルについては、支配的な変数となる状態量を速度ｖとすることができる。前述のようにアッカーマンモデルは低速度で成り立つモデルであるが、どの速度まで成り立つかは車両やその時の道路特性によって変わる。速度の閾値をアッカーマンモデルが成り立つ限界速度として、本装置ではこの閾値を上記の方法で決定することになる。

ここで、（式８）で表される方程式は拡張カルマンフィルタの状態方程式の一部、（式６）や（式７）で表される方程式は観測方程式の一部となる。ある時刻ｔの位置ｘ，ｙと方位θが（ｘ(ｔ)，ｙ(ｔ)，θ(ｔ)）であったとして、確率モデルフィルタリング処理部１０３から出力される状態量が（位置（２次元），方位，速度，角速度）であるとする。

ある時刻ｔで（式７）のデッドレコニングモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部１０３から算出された状態量が（ｘ(ｔ)，ｙ(ｔ)，θ(ｔ)，ν(ｔ)，ω(ｔ)）であったとする。この算出された状態量のうち、アッカーマンモデルとデッドレコニングモデルの切り替えに必要な速度ν(ｔ)が閾値算出部１０４へ出力される。また、確率モデルフィルタリング処理部１０３の拡張カルマンフィルタから算出される角速度の値と（式６）や（式７）で表されるそれぞれのモデルでの観測値との差を、アッカーマンモデルの誤差，デッドレコニングモデルの誤差として、閾値算出部１０４へ出力する。

次に閾値算出部１０４では、確率モデルフィルタリング処理部１０３から入力された時刻ｔまでの速度ν(ｔ)とそれぞれの数式モデルにおける誤差ｅ(ｔ)の値を用いて、各数式モデルにおける車速ｖに対する誤差の値の関係を曲線近似する。これにより縦軸が誤差を示し、横軸が車速ｖを示すようにすると図７のようなグラフが生成できる。図７の例では曲線７０１がデッドレコニングモデルの誤差、曲線７０２がアッカーマンモデルの誤差をそれぞれ表したグラフである。これらのグラフは、時刻ｔまでの速度に対するデッドレコニングモデルにおける誤差を示すバツ点７０３と、時刻ｔまでの速度に対するアッカーマンモデルにおける誤差を示す丸点７０４をそれぞれ近似する曲線を求めることで作成される。

そして、これらの曲線７０１と曲線７０２が交わった点７０５の速度の値を閾値候補ｖ_thとして、閾値決定部１０５へ出力する。算出された閾値候補の値ｖ_thはタイヤの摩耗や道路特性の変化などにより時変な特性を持つため、ステップ４０５で逐次学習による変化追従型となる。

時刻ｔまでの閾値候補の値の変化が図８の丸点８０１のようになったとする。閾値の変化特性として時定数τと規定すると、閾値ｖ_th(ｔ)の変化は次の（式９）（式１０）の２式で表される。

ｖ_th(ｔ＋１)＝（τ−Δｔ）／τ・ｖ_th(ｔ) …（式９）
ｖ_th(ｔ)＝ｖ_th …（式１０）
ただし、（式１０）の右辺ｖ_thは閾値算出部１０４から入力された今回の閾値候補の値である。

これら（式９）を状態方程式、（式１０）を観測方程式としてカルマンフィルタを構成すれば、今回の閾値ｖ_th(ｔ)８０２が決定される。こうして時々刻々の決定された閾値の値を図８の曲線８０３で示す。そして、決定された閾値ｖ_th(ｔ)８０２は、数式モデルテーブル１０８に設定される。アッカーマンモデルを数式モデルの識別番号１、デッドレコニングモデルを数式モデルの識別番号２とした数式モデルテーブル１０８を図９に示す。そして、速度がｖ_th(ｔ)８０２よりも小さい速度の場合を低速度走行、大きい速度の場合を高速度走行と定義できる。図中の「１」「２」…はそれぞれ、数式モデル処理部１０６の数式モデルを特定する識別番号を示しており、「／」はいずれかの数式モデルが適用可能であることを示している。次に、数式モデル選択部１０７で、数式モデルテーブル１０８を参照して、前回の確率モデルフィルタリング処理部１０３にて計算された状態量から、数式モデルテーブル１０８の当てはまる行・列を選択する。前回の確率モデルフィルタリング処理部１０３から出力された状態量が、仮にジャイロバイアス，ジャイロスケールファクタ，センサ取付角，舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図９の枠９０１が選択された列である。ここで、前回の確率モデルフィルタリング処理部１０３にて計算された速度がν(ｔ)であった場合、次の関係が成り立ったとする。

ν(ｔ)＜ｖ_th(ｔ) …（式１１）
この時、前回は低速度走行していることが判るので、図９の枠９０２で囲われる行が選択された行となる。従って、枠９０１と枠９０２で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、いずれの要素も識別番号が１であることから、数式モデル１のアッカーマンモデルのみが選択され、時刻ｔ＋１の時の確率モデルフィルタリング処理部１０３においては、拡張カルマンフィルタの観測方程式としてアッカーマンモデルの観測方程式のみが用いられる。

アッカーマンモデルとデッドレコニングモデルとの間で数式モデル処理部１０６が計算に用いる数式モデルを変更する場合には、使用するセンサが舵角センサとジャイロセンサとで異なるため、数式モデル選択部１０７では、数式モデルを変更すると同時に、使用するセンサを切り替えて、数式モデル処理部１０６で計算を行う。

本測位装置の動作として、ＧＰＳとジャイロセンサを搭載するカーナビゲーションシステムにおいて、ＧＰＳを採用した場合のセンサフュージョンモデルとＧＰＳ非受信もしくは採用されなかった場合のデッドレコニングモデルの間で数式モデルを切り替える例を説明する。

センサフュージョンモデルは、前述のようにＧＰＳ位置とジャイロ等の内界センサによるデッドレコニング位置とを拡張カルマンフィルタを用いて融合し、最尤な位置推定を行うモデルである。また、内界センサモデルは、前述のようにジャイロ等の内界センサによるデッドレコニング位置を出力するモデルである。ここでの拡張カルマンフィルタの状態方程式は（式８）と同じであるとする。センサフュージョンモデルと内界センサモデルは観測方程式の数式モデルであり、センサフュージョンモデルは（式１２）、内界センサモデルは（式１３）のように表される。

前述のようにセンサフュージョンモデルはＧＰＳが採用された場合に成り立つモデルである。どのような場合にＧＰＳが採用されるかを「現在の推定位置とＧＰＳ位置との距離が近いこと」と定義すると、内界センサモデルとの間で一般的に時不変な閾値が存在することになる。よって、支配的な変数としての状態量を、現在の推定位置とＧＰＳ位置との限界距離とし、本装置ではセンサフュージョンモデルが成り立つための閾値を上記の方法で決定することになる。

まず、拡張カルマンフィルタが用いられる確率モデルフィルタリング処理部１０３を考える。上述のように、（式８）で表される方程式は拡張カルマンフィルタの状態方程式の一部、（式１２）や（式１３）で表される方程式は観測方程式の一部である。ある時刻ｔの位置、方位が（ｘ(ｔ)，ｙ(ｔ)，θ(ｔ)）あったとして、確率モデルフィルタリング処理部１０３から出力される状態量が（位置（２次元），方位）であるとする。

ある時刻ｔで（式１３）の内界センサモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部１０３から算出された状態量が（ｘ(ｔ)，ｙ(ｔ)，θ(ｔ)）であったとする。この算出された状態量から、センサフュージョンモデルと内界センサモデルの切り替えに必要な現在の推定位置とＧＰＳ位置との距離Ｌが以下の（式１４）により算出される。

この（式１４）のＬが閾値算出部１０４へ出力される。また、確率モデルフィルタリング処理部１０３の拡張カルマンフィルタから算出される位置推定値と（式１２）や（式１３）で表されるそれぞれのモデルでの観測値の誤差を、センサフュージョンモデルの誤差、内界センサモデルの誤差として、閾値算出部１０４へ出力する。

次に閾値算出部１０４では、時刻ｔまでの確率モデルフィルタリング処理部１０３からの入力である現在の推定位置とＧＰＳ位置との距離Ｌ(ｔ)および、それぞれのモデルの誤差ｅ(ｔ)を用いて、図１０のような分布グラフが生成できる。縦軸が誤差を示し、横軸が現在の推定位置とＧＰＳ位置との距離Ｌを示している。図１０における丸点１００１が確率モデルフィルタリング処理部１０３にて推定された位置に対するセンサフュージョンモデルの誤差であり、バツ点１００２が確率モデルフィルタリング処理部１０３にて推定された位置における内界センサモデルの誤差である。これらの点を前述のサポートベクターマシンを用いて、ある値Ｌ_thで２分割にする。この時の値を閾値候補Ｌ_thとして、閾値決定部１０５へ出力し、閾値決定部１０５で保存する。

閾値決定部１０５では、時刻ｔにおける閾値候補の値および時刻ｔまでの閾値候補の値の平均値を今回の閾値Ｌ_th(ｔ)として決定する。そして、決定された閾値Ｌ_th(ｔ)は、数式モデルテーブル１０８に設定される。

センサフュージョンモデルを数式モデルの識別番号５、内界センサモデルを数式モデルの識別番号６とした数式モデルテーブル１０８を図１１に示す。この設定によって、現在の推定位置とＧＰＳ位置との距離が閾値Ｌ_th(ｔ)よりも小さい距離となった場合を、ＧＰＳ位置を融合するためＧＰＳ受信、逆に大きい距離が出力された場合はＧＰＳ位置を使用しないため、ＧＰＳ非受信と定義する。

次に、数式モデル選択部１０７で、数式モデルテーブル１０８を参照して、前回の確率モデルフィルタリング処理部１０３にて計算された状態量から、数式モデルテーブル１０８の該当する行・列を選択する。前回の確率モデルフィルタリング処理部１０３にて、出力された状態量が、仮にジャイロバイアス，ジャイロスケールファクタ，センサ取付角，舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図１１の枠１１０１が選択された列である。ここで、前回の確率モデルフィルタリング処理部１０３にて計算された推定位置とＧＰＳ位置との距離がＬ(ｔ)であった場合、次の関係が成り立ったとする。

Ｌ(ｔ)＜Ｌ_th(ｔ) …（式１５）
この時、現在はＧＰＳ受信であることが判るので、図１１の枠１１０２で囲われる行が選択された行となる。枠１１０１と枠１１０２で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、いずれの要素も識別番号が５であることから、数式モデル５のセンサフュージョンモデルのみが選択され、時刻ｔ＋１の時の確率モデルフィルタリング処理部１０３にて、（式１２）が拡張カルマンフィルタの観測方程式として用いられることになる。

本装置の動作手順として、デジタル地図情報とＧＰＳとジャイロセンサや加速度センサなどの姿勢センサを搭載し、自車位置をデジタル地図上にマッチングすることができるカーナビゲーションシステムにおいて、デジタル地図データの中にある、リンク情報を使用する場合のセンサフュージョンモデル（以後、リンクフュージョンモデルと呼ぶ）と、デジタル地図データの中のリンク情報を使用しない場合のセンサフュージョンモデル（以後、ＩＮＳモデルと呼ぶ）の間で数式モデルを切り替える例にとって説明する。

デジタル地図データは、所定の広さを持つメッシュ単位に管理されているものとする。地図の各メッシュでは、座標を持つ点（ノード）が道の形状を表すように置かれており、それぞれのノードには番号（ノード点番号）が割り当てられているものとする。そして各ノードのＸ座標は経度、Ｙ座標は緯度をそれぞれ示している。また、隣接する二つのノード間の距離をノード間リンク長、道幅をノード間リンク幅と呼び、それぞれ［ｍ］単位で表しているものとする。

リンクフュージョンモデルは、前述のようにデジタル地図データのリンクをカルマンフィルタの観測方程式として使う数式モデルであり、αをリンクの傾き、ｃをリンクの切片、ｒをリンクからの距離、φをリンクからの傾きとして、次の（式１６）のように表される。

この（式１６）の左辺が０であることからも判るように、リンクフュージョンモデルはリンク上に車両が存在することを前提としているため、地図情報に誤りがある場合、確率モデルフィルタリング処理部１０３から出力される位置も誤差を多分に含んだものとなる。

また、ＩＮＳモデルは、ジャイロセンサから角速度を加速度センサから加速度を取得し、その値を積分して、それぞれ方位・速度に変換する。よって、角速度の算出は（式７）と等しく、加速度ＡＣＣの算出は、下の（式１７）のように表される。

ＡＣＣ＝Ｓ_acc・Ｇ_Gsns−ｇ・sinψ＋bias …（式１７）
ただし、Ｓ_accが加速度センサの感度係数、Ｇ_Gsnsが加速度センサの生出力値、ｇは重力加速度、ψはセンサの取付角である。

そして、上記リンクフュージョンモデルとＩＮＳモデルの状態方程式は、（式８）と同じである。

前述のようにリンクフュージョンモデルは地図情報に誤りがある場合には、誤差が大きくなるモデルであるので、誤った地図情報は使用しない方が良い。そこで、ＧＰＳ位置は大まかには正しい位置を出力しているとして、地図情報が使用される時の定義を「ＧＰＳ位置とリンクとの距離が近いこと」とすると、ＩＮＳモデルとの間で一般的に時不変な閾値が存在することになる。さらに、ＧＰＳ位置自体も誤差を持っており、前述の実施例２のように、ＧＰＳ位置を使用しない方が良い場合がある。よって、ＧＰＳ位置とリンクとの距離を地図誤差量と定義し、これを支配的な変数としての状態量として用い、この地図誤差量についてリンクフュージョンモデルが成り立つための閾値を決定し、さらに第二の実施例と同様ＧＰＳ受信位置も含めての閾値を決定することになる。

まず、拡張カルマンフィルタを用いる確率モデルフィルタリング処理部１０３を考える。上述のように、（式８）で表される方程式は拡張カルマンフィルタの状態方程式の一部であり、（式１６）や（式７），（式１７）で表される方程式は観測方程式の一部である。ある時刻ｔの位置、方位が（ｘ(ｔ)，ｙ(ｔ)，θ(ｔ)）であったとして、確率モデルフィルタリング処理部１０３から出力される状態量が（位置（２次元），方位）であるとする。

ある時刻ｔで（式１３）の内界センサモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部１０３から算出された状態量が（ｘ(ｔ)，ｙ(ｔ)，θ(ｔ)）であったとする。この時、自車位置推定装置１０２で前述のようなマップマッチングを行った結果、マッチングする先のリンクの両端ノードのＸ座標，Ｙ座標から、リンクの方程式が（式１８）のように求められる。

ｘsinα−ｙcosα＝ｃ …（式１８）
リンクフュージョンモデルとＩＮＳモデルの切り替えに必要な現在（時刻ｔとする）の推定位置とリンクの距離ｒ_GPSは（式１８）に表されるリンク方程式とＧＰＳ位置との点と直線の距離から、以下のように算出される。

ｒ_GPS＝ｘ_gps(ｔ)sinα−ｙ_gps(ｔ)cosα−ｃ …（式１９）
この（式１９）のｒ_GPSが閾値算出部１０４へ出力される。また、確率モデルフィルタリング処理部１０３の拡張カルマンフィルタから算出される位置推定値と（式１６）や（式７）（式１７）で表されるそれぞれのモデルでの観測値の誤差を、リンクフュージョンモデルの誤差、ＩＮＳモデルの誤差として、閾値算出部１０４へ出力する。

次に閾値算出部１０４では、確率モデルフィルタリング処理部１０３からの入力である時刻ｔまでの現在の推定位置とリンクとの距離ｒ_GPS(ｔ)および、それぞれのモデルの誤差ｅ(ｔ)を用いて、図１２のような分布グラフが生成する。縦軸が誤差を示し、横軸が現在の推定位置とＧＰＳ位置とリンクとの距離ｒ_GPSを示している。図１２における丸点１２０１がＧＰＳ位置とリンクとの誤差である。これらの点を最小２乗法を用いて、直線近似する。その直線が１２０２である。また、第二の実施例と同様の方法で、ＧＰＳ受信時の閾値Ｌ_thを求め、その時のＩＮＳ誤差の値１２０３を求める。この誤差の値１２０３よりも小さい誤差の時、ＧＰＳ位置は大まかに正しいと言えるので、直線１２０２が値１２０３を取る時のＧＰＳ位置とリンクとの距離ｒ_th１２０４がこの時の閾値候補となり、閾値決定部１０５へ出力され、閾値決定部１０５で保存される。そして、閾値決定部１０５において、時刻ｔにおける閾値候補の値および時刻ｔまでの閾値候補の値の平均値を今回の閾値ｒ_th(ｔ)として決定する。そして、決定された閾値ｒ_th(ｔ)は、数式モデルテーブル１０８に設定される。リンクフュージョンモデルをモデル７、ＩＮＳモデルをモデル５とした数式モデルテーブル１０８を図１３に示す。この設定によって、閾値ｒ_th(ｔ)よりも小さいＧＰＳ位置とリンクとの距離が出力された場合は地図誤差小、閾値ｒ_th(ｔ)よりも大きいＧＰＳ位置とリンクとの距離が出力された場合は地図誤差大と定義する。

次に、数式モデル選択部１０７で、数式モデルテーブル１０８を参照して、前回の確率モデルフィルタリング処理部１０３にて計算された状態量から、数式モデルテーブル１０８の当てはまる行・列を選択する。前回の確率モデルフィルタリング処理部１０３にて、出力された状態量が、仮にジャイロバイアス，ジャイロスケールファクタ，センサ取付角，舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図１３の枠１３０１が選択された列である。ここで、第二の実施例と同様の方法で算出されたＧＰＳ受信の閾値Ｌ_th(ｔ)に対し、前回の確率モデルフィルタリング処理部１０３にて計算された推定位置とＧＰＳ位置との距離がＬ(ｔ)であった場合、次の関係が成り立ったとする。

Ｌ(ｔ)＜Ｌ_th(ｔ) …（式２０）
この時、ＧＰＳ受信であることが判るので、図１３の枠１３０２で囲われる行が選択された行となる。また、ＧＰＳ位置とリンクとの距離の間に次の関係が成り立ったとする。

ｒ_GPS(ｔ)＜ｒ_th(ｔ) …（式２１）
この時、地図誤差小であることが判るので、図１３の枠１３０３で囲われる行が選択された行となる。枠１３０１と枠１３０２および枠１３０３で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、数式モデルの識別番号５のＩＮＳモデルおよび数式モデルの識別番号７のリンクフュージョンモデルの両方が選択されている。このため、時刻ｔ＋１の時の確率モデルフィルタリング処理部１０３にて、拡張カルマンフィルタの観測方程式としては、（式７）（式１６）（式１７）が用いられることになる。

１０１，１５０１ センサ
１０２，１５０２ 自車位置推定装置
１０３，１５０３ 確率モデルフィルタリング処理部
１０４，１５０４ 閾値算出部
１０５，１５０５ 閾値決定部
１０６，１５０６ 数式モデル処理部
１０７，１５０７ 数式モデル選択部
１０８，１５０８ 数式モデルテーブル

Claims (5)

1. 移動体の姿勢情報および位置測位情報をセンサから取得し、取得した姿勢情報および位置測位情報に基づき移動体の位置を、予め備えられた移動体の挙動を表す数式モデルに基づき算出する数式モデル処理部を備えた位置算出装置において、
   前記数式モデル処理部は複数の数式モデルを備え、
   位置算出装置は、
   位置の算出を行う前記数式モデル処理部における確率モデルを使ったフィルタリング処理により算出した誤差を用いて閾値の候補値を計算する閾値算出部と、
   数式モデルを選択するための前記閾値を前記候補値に基づき決定する閾値決定部と、
   数式モデル処理部で位置算出に用いる数式モデルを前記閾値決定部で決定された閾値に基づき選択する数式モデル選択部と、
   を有することを特徴とする位置推定装置。
2. 請求項１に記載の位置推定装置において、移動体の挙動を表す前記複数の数式モデルは、外部から観測可能な情報もしくは数式モデル処理部で算出した値から決められる値に基づく、移動体の挙動に対する適応範囲の制限を有し、
   前記閾値決定部は、各数式モデルにより計算される誤差の大きさに基づいて前記閾値を決定し、
   前記数式モデル選択部は、数式モデルの制限に応じて数式モデルを選択する、ことを特徴とする位置推定装置。
3. 請求項２に記載の位置推定装置において、前記数式モデルの適応範囲の制限と前記閾値を記憶するデータベースを有することを特徴とする位置推定装置。
4. 移動体の姿勢情報および位置測位情報をセンサから取得し、取得した姿勢情報および位置測位情報に基づき移動体の位置を、予め備えられた移動体の挙動を表す数式モデルに基づき算出する位置推定方法において、
   数式モデルに従って位置の算出を行う確率モデルを使ったフィルタリング処理により算出した誤差を用いて閾値の候補値を計算する処理と、
   数式モデルを選択するための前記閾値を前記候補値に基づき決定する処理と、
   予め用意されている複数の数式モデルの中から位置算出に用いる数式モデルを前記閾値に基づき選択する処理と、
   を行うことを特徴とする位置推定方法。
5. 請求項４に記載の位置推定方法において、移動体の挙動を表す前記複数の数式モデルは、外部から観測可能な情報もしくは数式モデル処理部で算出した値から決められる値に基づく、移動体の挙動に対する適応範囲の制限を有し、
   前記閾値を決定する処理では、各数式モデルにより計算される誤差の大きさに基づいて前記閾値を決定し、
   前記数式モデルを選択する処理では、数式モデルの制限に応じて数式モデルを選択する、ことを特徴とする位置推定方法。

Images