位置推定装置および位置推定方法
【課題】走行中の車両は、その路面状態や外気温や各部品の経年数、運転するドライバーの特性といった、前記のような誤差などの各種モデルを切り替えるための閾値の決定を事前に行うことが難しいような場合、事前に決められた不変の値では、モデル化誤差が大きくなり、自車位置の算出精度に大きな悪影響を与えかねない。
【解決手段】自車位置の推定精度を高精度に保つために、複数のモデルを予め用意しておき、測位に使用するモデルを切り替えるための閾値を、自車の状態、特に自車位置を算出する際に用いるために推定されるパラメータや学習されるパラメータおよび変数や係数を用いて算出し、算出された閾値に基づいて測位に使用するモデルを決定する自車位置推定装置。
【解決手段】自車位置の推定精度を高精度に保つために、複数のモデルを予め用意しておき、測位に使用するモデルを切り替えるための閾値を、自車の状態、特に自車位置を算出する際に用いるために推定されるパラメータや学習されるパラメータおよび変数や係数を用いて算出し、算出された閾値に基づいて測位に使用するモデルを決定する自車位置推定装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、自動車や電車などの移動体の車載装置に関し、特に位置推定を行う機器に関する。
【背景技術】
【0002】
従来のカーナビゲーションシステムでは、測位結果を基にマップマッチングを行い、自車位置を求めていたが、特開2008−287669号公報に示されているようなカーナビゲーションシステムから車両位置を車両運動制御機器へ出力する車両運動制御システムおいては、より正確な車両位置の測位が求められる。
【0003】
正確な測位を行うため、自車位置の算出に用いる複数の数式モデルを用意し、事前に決められた一定の閾値に従い計算に用いる数式モデルを変更する技術が提案されている。例えば特開平8−14923号公報には、グローバルポジショニングシステム(Global Positioning System:以下GPS)の信号強度や衛星位置などの情報を用いて、GPSの出力する位置の誤差がある一定以上であれば、ジャイロや加速度センサなどの相対センサによる位置算出に切り替える装置が開示されている。
【0004】
あるいは、カルマンフィルタなどの確率システムを用いて、自車位置を推定するシステムでは、特開平5−333132号公報のように、受信できたGPS衛星の数に対応したカルマンフィルタを複数事前に用意し、これらを切り替えて自車位置を測定する装置が提案されている。カルマンフィルタやパーティクルフィルタなどの確率モデルを用いた位置推定ロジックを用いて自車位置を推定する技術としては、拡張カルマンフィルタアルゴリズムを用いる発明が特開2007−322391号公報に開示されている。
【0005】
測位誤差に基づき位置算出手法を切り替える場合、舵角センサとジャイロセンサを搭載するカーナビゲーションシステムにおける測位誤差のモデルとしては、速度によるアッカーマンモデルやデッドレコニングモデルが知られており、また、GPSとジャイロセンサを搭載するカーナビゲーションシステムにおける、GPSを採用した場合のセンサフュージョンモデルやGPS非受信もしくは採用されなかった場合のデッドレコニングモデルが知られている。
【0006】
アッカーマンモデルは、“安部正人,「自動車の運動と制御」,東京電機大学出版局,2008年3月,ISBN:9784501417000”に記載されている。センサフュージョンモデルと内界センサモデルは、“青野俊宏 他,「GPSと内界センサを用いた起伏地における移動機の位置計測」,計測自動制御学会論文集 Vol.35,No.8,1999年”に記載されている。
【0007】
さらに、自車位置をデジタル地図上にマッチングする際に、デジタル地図情報の中にあるリンク情報を使用する場合のセンサフュージョンモデル(以後、リンクフュージョンモデルと呼ぶ)として、特開2008−26282号公報に示されるような、デジタル地図データのリンクをカルマンフィルタの観測方程式として使う数式モデルが知られている。
【先行技術文献】
【特許文献】
【0008】
【特許文献1】特開2008−287669号公報
【特許文献2】特開平8−14923号公報
【特許文献3】特開平5−333132号公報
【特許文献4】特開2007−322391号公報
【特許文献5】特開2008−26282号公報
【非特許文献】
【0009】
【非特許文献1】“安部正人 著「自動車の運動と制御」 東京電機大学出版局 2008年3月刊 ISBN:9784501417000”
【非特許文献2】“青野俊宏 他著「GPSと内界センサを用いた起伏地における移動機の位置計測」 計測自動制御学会論文集 Vol.35,No.8,1999年”
【発明の概要】
【発明が解決しようとする課題】
【0010】
走行中の車両は、その路面状態や外気温や各部品の経年数、運転するドライバーの特性といった、前記のような誤差などの各種モデルを切り替えるための閾値の決定を事前に行うことが難しいような場合、事前に決められた不変の値では、モデル化誤差が大きくなり、自車位置の算出精度に大きな悪影響を与えかねない。またこの場合、車両運動制御に自車位置の算出精度が関わっている場合、車両制御の精度に悪影響を及ぼす可能性がある。
【課題を解決するための手段】
【0011】
自車位置の推定精度をどのような場合でも高精度に保つためには、複数のモデルを切り替えるための閾値を事前に用意した不変な値とするのではなく、自車の状態、特に自車位置を算出する際に用いるために推定や学習されるパラメータ,変数や係数などを用いて算出される値を閾値としてモデルを変化させるような自車位置推定装置を用いることにより、解決される。
【発明の効果】
【0012】
カーナビゲーションシステムやオートクルーズシステムなど、自身の絶対位置を高精度に推定しなければならないシステムにおいて、自車の状態を検出もしくは算出もしくは学習した値で、自身の絶対位置を推定するためのモデルを切り替えることで、どのような状態・状況でも常に高精度な位置を推定できるようになる。また、位置を高精度に推定するために必要な自身の誤差を元に、自車位置の信頼性を出力することが可能となる。
【図面の簡単な説明】
【0013】
【図1】測位装置の概略構成を示す図である。
【図2】状態量と数式モデルの誤差の相関を示す図である。
【図3】閾値候補生成処理のフローチャートである。
【図4】閾値決定処理のフローチャートである。
【図5】数式モデル変数テーブルを示す図である。
【図6】数式モデル選択処理のフローチャートである。
【図7】アッカーマンモデルとデッドレコニングモデルによる数式モデルの車速に対する誤差変化を表した図である。
【図8】閾値vthの時間変化を表した図である。
【図9】アッカーマンモデルとデッドレコニングモデルの数式モデルテーブルを示す図である。
【図10】センサフュージョンモデルと内界センサモデルの数式モデルによる誤差変化を表した図である。
【図11】センサフュージョンモデルと内界センサモデルの数式モデルテーブルを示す図である。
【図12】リンクフュージョンモデルとINSモデルの数式モデルによる誤差変化を表した図である。
【図13】リンクフュージョンモデルとINSモデルの数式モデルテーブルを示す図である。
【発明を実施するための形態】
【0014】
以下、本発明を用いた測位装置の詳細を説明する。
【実施例1】
【0015】
図1に本発明を用いた測位装置の概略構成を示す。測位装置は、カーナビゲーションシステムまたは専用端末上で実現され、測位装置が搭載された車両の現在の姿勢情報および位置測位情報を取得するセンサ101と自車位置推定装置102とで構成される。自車位置推定装置102は、カルマンフィルタなどの確率モデルに基づく数式モデルを用いて周期的に車両位置を算出する確率モデルフィルタリング処理部103と、この確率モデルフィルタリング処理部103で算出された速度などの車両の状態量やこの算出された状態量の誤差から、確率モデルフィルタリング処理部103で車両位置の計算に用いる数式モデルを変更するための閾値となり得る閾値候補の値を算出する閾値算出部104と、閾値算出部104にて求められた閾値候補の値および確率モデルフィルタリング処理部103で使用する数式モデルの特性を基に、確率モデルフィルタリング処理部103で計算に用いる数式モデルを変更する閾値を決定する閾値決定部105とを有する。
【0016】
確率モデルフィルタリング処理部103は、自車位置を算出するために用いる車両の挙動を表す複数の数式モデルからなる数式モデル処理部106と閾値決定部105において決定された閾値に基づいて、数式モデル処理部106において遷移行列の各要素の値の算出に用いる数式モデルを選択する数式モデル選択部107を備え、カルマンフィルタなどの確率モデルにおける状態方程式などから求められる遷移行列の各要素の値を数式モデル処理部106において計算する。ここで、数式モデルとは、測位アルゴリズムのモデルを表す状態方程式もしくは観測方程式を構成する方程式一つ一つの式のことであり、識別番号で管理されるものとする。複数の数式モデルに対応した各々の状態方程式または観測方程式を組み合わせることにより、数式モデル処理部106では状態方程式や観測方程式が表され、この状態方程式と観測方程式によりカルマンフィルタなどの確率モデルが計算される。
【0017】
閾値決定部105では、閾値算出部104で算出された閾値候補の値を保存し、数式モデル選択部107において数式モデル処理部106で計算に用いる数式モデルを選択し変更するために用いる数式モデルテーブル108に対して閾値を設定する。また、数式モデルテーブル108には、センサ101から入力される状態量を変数として各数式モデルの状態方程式または観測方程式を表したときの係数を格納した数式モデル変数テーブル111が格納されている。
【0018】
車両の姿勢情報および位置測位情報を取得するセンサ101からは、自車の姿勢や移動方向,移動速度、あるいは大局的な位置のデータが自車位置推定装置102に入力される。確率モデルフィルタリング処理部103ではこれらの入力値に基づき、数式モデル選択部107によって、数式モデル処理部106で遷移行列の計算に使用する数式モデルが選ばれ、この数式モデルの計算により求めた位置を出力する。また、数式モデル処理部106において数式モデルで算出された位置以外の状態量は閾値算出部104へ入力される。
【0019】
閾値算出部104では数式モデル処理部106に用意された複数の数式モデルの特性に従い、各数式モデルにおける状態量と誤差の関係を計算して、その数式モデルについての閾値の候補を決定する。算出された閾値の候補は閾値決定部105へ入力され、複数の数式モデルについて適用時の誤差の大きさおよびその時の状態量に基づき閾値を決定して、数式モデル選択部107から参照できるようにするため数式モデルテーブル108に格納する。閾値決定部105にて決められた各状態量毎の閾値は確率モデルフィルタリング処理部103内の数式モデル選択部107から参照され、次の位置計算の周期の際に、車両の姿勢情報および位置測位情報を取得するセンサ101からデータが自車位置推定装置102に入力された際に、数式モデル処理部106の複数の数式モデルの中から最適な数式モデルを選択するために使用される。この流れを周期的にループ処理することによって、各状態量に対して最適な閾値が順次決定され、数式モデルテーブル108が更新されていく。
【0020】
車両の姿勢情報および位置測位情報を取得するセンサ101からは、自車両の移動する方位,自車両の傾きなど車両の姿勢,自車両の絶対位置,自車両の移動する速度,自車両とその他の物体との相対的な位置および方位・姿勢の関係などの情報が出力される。
【0021】
車両の絶対位置は、GPSなどのように直接位置を算出できるセンサや、絶対位置と形状が既知の物体の相対的な位置を計測するカメラやレーダセンサなどのセンサにより算出する。
【0022】
車両の移動する方位は、直接絶対方位を計測できる地磁気センサや、初期方位が与えられた上で計測した角速度を積分することで方位を算出するジャイロセンサのような方位センサや、あるいは異なる時間に受信したGPSの信号により算出する。
【0023】
車両の移動する速度は、直接速度を算出できる車速パルスのセンサや、車両の加速度を検知して積分することで速度を算出する加速度センサ、あるいは異なる時間に受信したGPSにより算出する。
【0024】
車両の姿勢は、加速度センサなどのように重力の向きを検知することで車両の傾きを検出する姿勢検知センサや形状が既知の物体に対する相対的な姿勢を計測できるレーダセンサや、あるいは形状が既知の物体の自車に対する傾きを計測できるようなカメラの画像により算出する。
【0025】
確率モデルフィルタリング処理部103では、数式モデル処理部106が、カルマンフィルタやパーティクルフィルタなど、確率モデルに基づく位置推定ロジックを用いた複数の数式モデルにより位置を推定する。位置推定ロジックの代表的な例としては、前述のような拡張カルマンフィルタアルゴリズムによる自車位置推定がある。確率モデルに基づく位置推定ロジックでは、センサ101からの出力もしくは数式モデル処理部106で算出された自車両の方位・位置・姿勢速度の情報を、その誤差を考慮した観測方程式として表し、ある時間から次の時間へと遷移する自車の方位・位置・姿勢・速度を状態方程式として表す。数式モデル処理部106が備える各数式モデルは、この観測方程式及び状態方程式を計算してフィルタリング処理を行う。
【0026】
数式モデル処理部106では、自車両の位置と共に、方位・姿勢・速度に関する情報も推定する。これらの値を状態量と呼ぶ。算出された状態量は、閾値算出部104に出力される。また、数式モデル処理部106において、誤差や誤差の分散を算出する数式モデルを用いて計算を行う場合、その誤差も出力する。
【0027】
閾値算出部104では、確率モデルフィルタリング処理部103から入力された状態量に対する数式モデル処理部106が備える複数の数式モデルの誤差を見積もる。図2に数式モデルにおける状態量とその誤差の相関例の模式図を示す。図2の横軸は、確率モデルフィルタリング処理部103から入力された状態量、縦軸はその状態量と共に確率モデルフィルタリング処理部103から入力された誤差の値であり、数式モデル毎の状態量に対する誤差の値を示す。曲線201はある数式モデル1における、状態量に対する誤差の変化を表した曲線であり、曲線202は数式モデル2における状態量に対する誤差の変化を表した曲線である。曲線201と曲線202の交点A203が数式モデル1と数式モデル2の誤差が逆転する点であり、ここから右の状態量の場合には数式モデル1による計算のほうが誤差が小さく、逆にここから左の状態量の場合には数式モデル2を用いて計算したほうが誤差が小さくなることになる。そこで交点A203の状態量の値B204が、フィルタリング処理に用いる数式モデルを切り替える判断を行う際の閾値候補となり、この閾値候補を閾値決定部105へ出力する。
【0028】
閾値算出部104における閾値候補の生成処理を図3に示す。この処理は、数式モデル処理部106が管理する数式モデルの内、2つの数式モデルについてその誤差を比較して、各2つの数式モデル間についての閾値候補を求める処理である。まず、確率モデルフィルタリング処理部103から、数式モデル処理部106で計算された状態量を取得する(ステップ301)。取得した状態量は、それぞれ計算に用いられた数式モデル毎に最新の値から多くとも所定数分まで記憶しておく。次に、数式モデル処理部106が有する複数の数式モデルの中から、ステップ301で取得した状態量を用いている複数の数式モデルを探してリストを作り、リストの中の各数式モデルについて数式モデル変数テーブル111を参照して状態量に対応した係数を取得(以下、状態量に対応した数式モデルの係数を取得することを、数式モデルを取得する、と呼ぶ)して状態方程式または観測方程式を構成した上で、取得した状態量に基づき計算を行い、各数式モデルについてその誤差を取得する(ステップ302)。数式モデル変数テーブル111には、予め閾値候補を決定するために必要な数式モデルのみが設定されているものとする。また、取得した数式モデルの誤差はそれぞれ計算に用いられた数式モデル毎に、最新の値から多くとも所定数分まで閾値算出部104で記憶しておくものとする。
【0029】
そして、ステップ301で取得した状態量の中から、数式モデルにおける誤差比較に用いる状態量(これを、支配的な変数となる状態量と呼ぶ)としている数式モデルがステップ302で作成したリストに存在する未処理の状態量が残っているか調べる(ステップ303)。支配的な変数となる状態量は、数式モデルテーブル108に格納されている数式モデル変数テーブル111に予め定義されているものとする。数式モデル変数テーブルは図5に示すように、数式モデル処理部106が備える各数式モデルについて、各々の数式モデルが処理する状態量に対応した変数の変数名と、その変数が支配的であるか否かを表すフラグが記憶されている。数式モデル変数テーブル111は、予め閾値候補を決定するために必要な数式モデルのみが設定されており、このテーブルの順番で選択の対象となる数式モデルが存在するかどうかを数式モデルテーブルの番号順に検索する。未処理の状態量があれば、その支配的な変数となる状態量を選択する(ステップ304)。
【0030】
次に、選択した状態量を支配的な変数とする数式モデルをステップ302で作成したリストを先頭から順に探し、最初に見つかった未処理の数式モデルの次から、同じ状態量を支配的な変数とする数式モデルを探すことにより、同じ状態量を支配的な変数とする数式モデルの対を選択する(ステップ305)。対象となる数式モデルの対が一つでも見つかった場合、数式モデル変数テーブルの番号と数式モデルテーブルの番号を一時保存し、ステップ306,307へ進む。
【0031】
ここでは、対処となる数式モデルが少なくとも1つは存在するものとして、次にステップ306,ステップ307にて、誤差比較する2つの数式モデルに対して、ステップ304で選択した状態量の変化に対する誤差を、ステップ302で取得した誤差を設定し、図2における曲線201および曲線202を決定していく。そして、ステップ306およびステップ307で算出もしくは推定した誤差に基づき決定した曲線同士の交点を算出し、その交点における状態量の値を算出する(ステップ308)。算出された状態量は、閾値候補として閾値決定部105へ出力もしくは、出力待機としてバッファリングされる(ステップ309)。
【0032】
そして、ステップ310にてステップ302で取得した各数式モデルのリストに対して、選択した状態量を支配的な変数とする数式モデルを先頭方向から順に探し、見つかった未処理の数式モデルの次から、また同じ状態量を支配的な変数とする数式モデルを探すことを繰り返して、支配的である状態量が重なる数式モデルの全ての組み合わせに対して、ステップ306からステップ309の処理を完了したかどうかを判断し、全て計算が完了していた場合(ステップ310:yes)、即ちステップ304で選択した状態量を支配的な変数とする数式モデルの対を全て処理し終えていた場合には、ステップ303に戻る。またこの時、ステップ309にて出力待機としてバッファリングしたデータが存在する場合は、このタイミングで閾値決定部105へ出力しても良い。一方、全ての数式モデルの対について計算が完了していない場合は、まだその他の選択対象となる数式モデルが存在する可能性があるためステップ305へ戻り、上記の計算を繰り返す(ステップ310:no)。
【0033】
また、ステップ303の判定で、ステップ302で作成したリストの数式モデルが支配的な変数としている状態量の内、未処理の状態量が残っていない場合(ステップ303:不可)、閾値算出部104における閾値候補の生成処理を終了する。
【0034】
閾値決定部105では、閾値算出部104から入力された閾値候補を用いて、各状態量の閾値を決定する。決定された閾値は数式モデルテーブル108に出力される。なお、閾値決定部105は、閾値算出部104からの閾値候補を記憶しておく記憶領域(図示せず)を備えている。
【0035】
閾値算出部104から入力された閾値候補から各状態量の閾値を決定する閾値決定処理を図4のフローチャートに示す。まず、数式モデルテーブル108の数式モデル変数テーブル111から、数式モデル処理部106が有する複数の数式モデルを取得する(ステップ401)。次に、閾値算出部104から出力された閾値候補を取得して(ステップ402)、これまでの閾値候補を全て保存している領域に更に今回の閾値候補の値を保存する(ステップ403)。これまで保存されていた閾値候補は、メモリ量の上限が設定されている場合には、平均値などのような代表点に置き換えても良い。
【0036】
次にステップ404にて、ある閾値候補の状態量を選び、選んだ閾値候補の状態量が誤差に影響する数式モデルが存在するかを判断する。そして、閾値候補の状態量が変動すると誤差が変動するような数式モデルが存在する場合、ステップ405にて、閾値候補の状態量の特性が時変な閾値候補(時間変動する状態量)と時不変な閾値候補(時間変動しないはずの状態量)とで処理を分ける。状態量が時変か時不変であるかは、後述する数式モデルテーブル108で分類されて設定されているため、これを参照することになる。閾値候補が時変な状態量であった場合、ステップ406へ移り、これまでの閾値候補の値を踏まえた逐次学習法、例えば、カルマンフィルタや最小2乗法などを用いて時変な閾値の変動に追従できるよう逐次学習法のゲインを設定する。そして、ステップ407にて、ステップ406からの逐次学習法による追従の最終出力値を閾値として決定する。次に、ステップ408にて数式モデルテーブル108の対象行もしくは対象列に対して、ステップ407にて決定した閾値よりも大きい場合に誤差が小さくなるような数式モデルおよび、ステップ407にて決定した閾値よりも小さい場合に誤差が小さくなるような数式モデルを数式モデルテーブル108の対象となる要素を選択し、数式モデルを表す番号を設定する。図2を例にとると、閾値の対象となる状態量の値がBより小さい場合は数式モデル2の方が誤差が小さいので、数式モデルテーブルの要素に「2」を設定し、閾値の対象となる状態量の値がBより大きい場合は数式モデル1の方が誤差が小さいので、数式モデルテーブルの要素に「1」を設定する。
【0037】
また、閾値候補が時不変な状態量であった場合、ステップ409へ移り、閾値候補の平均値をとる手法や、これまでの閾値候補の値を2つに分類するクラスタリングの手法、例えば、“Christopher M. Bishop 著「パターン認識と機械学習」下 シュプリンガー・ジャパン 2008年7月1日刊 ISBN:9784431100317”に記されているようなサポートベクターマシンなどを用いて閾値の決定を行う。そして、ステップ410にて、ステップ409からの最終出力値を閾値として決定する。次に、ステップ411にて数式モデルテーブル108の対象行もしくは対象列に対して、ステップ410にて決定した閾値よりも大きい場合に誤差が小さくなるような数式モデルおよび、ステップ410にて決定した閾値よりも小さい場合に誤差が小さくなるような数式モデルを数式モデルテーブル108の対象となる要素を選択し、数式モデルを表す識別番号を設定する。
【0038】
次に、ステップ412にて、ステップ407もしくはステップ410によって決定された閾値が全ての数式モデルに対して決定されていることを確認する。これは、数式モデル中に複数の閾値(状態量)が存在することを許容するため、唯一の閾値(状態量)の決定だけでは不足するためである。もし、全ての数式モデルに対して閾値が決定されていない場合、即ち、閾値が決定されていない数式モデルが存在する場合にはステップ404に戻り、さらにその他の閾値の決定を行う。全ての数式モデルに対して閾値が決定されている場合、ステップ413に進み、これまでの閾値として決定した値を削除して新たにステップ407もしくはステップ410にて算出された値を閾値として更新する。最後にステップ414にて、新たに決定した閾値を数式モデルテーブル108に設定する。以上の処理により、閾値候補が算出されていた状態量に対して閾値が1つ決定され、数式モデル処理部106にある全ての数式モデルに対して少なくとも1つの閾値が決定されることになる。
【0039】
次に、図9に数式モデルテーブル108の構成を示す。このテーブルの縦方向には、誤差の大きさを決定するために必要な数式モデルの状態量が並ぶ。この状態量は確率モデルフィルタリング処理部103にて計算される状態量であり、時変な状態量とする。この誤差の大きさを決定するために必要な数式モデルとは、例えば、確率モデルフィルタリング処理部103でカルマンフィルタが用いられていた場合は、観測方程式で用いられる。
【0040】
誤差の大きさを決定するために必要な数式モデル内に低速度状態ではモデル化誤差が大きいようなモデルがある場合、誤差の大小により低速度走行時と高速度走行時の二つの状態が存在することになり、どちらの状態かによってモデルの誤差の大小が分かれる。また、横方向には、確率モデルの内部状態を表す状態量を置く。これは主に、確率モデルフィルタリング処理部103にて計算される状態量から計算される状態量であり、時不変な状態量とする。縦方向の数式モデル分類用に置いた状態量であっても、確率モデルの内部状態を表す状態量であれば時間変動が小さいものと見なして、横方向の状態量に置いてよい。
【0041】
時不変な状態量として、例えば、確率モデルフィルタリング処理部103にて計算される状態量の中にセンサの取付角の誤差が存在していた場合、この状態量は横方向に並べた状態量の一つに含まれ、学習が完了と未完の二つの状態が存在することになる。このような学習の完了・未完の判断は、確率モデルフィルタリング処理部103の中で、フィルタリング処理とは別に計算することになる。上記の例では、予め定められた分散値をある一定時間内のセンサ取付角の誤差の計算結果の分散値で割ることで、どれほどセンサの取付角の誤差が揺れているか、つまり学習が進んでいるかを数値化できる。この数値を、学習度と呼び、ある値を閾値として学習の完了・未完を判断する。このような内部状態を表す状態量としては、その他にジャイロセンサのバイアスの学習やスケールファクタの学習、舵角特性の学習などがある。学習度は1以上の値となった状態を、理論上学習が完全に終わった状態とする。
【0042】
数式モデルテーブル108の内部には、縦軸と横軸で選ばれた状態量の状況の組み合わせで最適なモデルが設定される。この最適なモデルとは、ステップ408もしくはステップ411にて設定される数式モデルである。数式モデル処理部106の中には排他ではない複数の数式モデルが存在しても良い。このため、数式モデルテーブルの内部の縦軸と横軸で選ばれた状況に対しては、複数の数式モデルが設定されうる。
【0043】
ステップ413にて保存された閾値は、ステップ414にて、縦軸もしくは横軸の閾値として設定される。ステップ414にて設定された閾値が、速度10km/hおよびセンサ取付角学習度0.1であるとすれば、縦軸内の低速度走行は10km/h未満を示し、高速度走行は10km/h以上を示すことになる。また同様に、横軸において、センサ取付角学習度が0.1以下の場合を学習未完、0.1以上を学習完了として設定することになる。
【0044】
数式モデル選択部107では、閾値決定部105で作成された数式モデルテーブル108に従い、現在の車両状態に最も適した数式モデルを複数選択する。図6に数式モデル選択部107における複数の数式モデル処理部106から最適なモデルを複数選択する処理のフローチャートを示す。まず、ステップ601にて、数式モデルテーブル108を読み込む。次に数式モデル処理部106で前回計算された車両の挙動や姿勢・状態を表す状態量を読み込む(ステップ602)。次に、読み込んだ各状態量が数式モデルテーブル108に設定された閾値の縦軸側と横軸側のどちら側に属するかを判定し、数式モデルテーブル108から縦軸と横軸について計算された車両の状況(状態量)の値と条件が合致する行・列を選択する(ステップ603)。ここで選択する行および列は複数であって良い。そしてステップ604にて、選択した行・列内で設定されている対応する状況に最適な数式モデルを読み込む。
【0045】
次にステップ605にて、ステップ604で選択された全ての数式モデルについて重複の有無を確認し、重複するモデルがある場合、ステップ606に移行して、重複する数式モデルが無いようにしてから数式モデル処理部106における計算対象の数式モデルとして設定する。これは、数式モデル処理部106にて、同じ方程式を重複して使用することを避けるためである。また、ステップ605にて、選択した数式モデルに重複する数式モデルが存在しない場合、ステップ607に移行して、選択した全ての数式モデルを数式モデル処理部106に設定する。
【0046】
数式モデル処理部106では、数式モデル選択部107で選択された数式モデルのみを用いて位置やその他の状態量を算出する。数式モデル処理部106は全体でN個の数式モデルを管理しているとすると、数式モデル選択部107ではその内のn個が選択されることになる。(ただし、nはN以下の数。)数式モデル選択部107では、選択したn個の数式モデルに対応した、確率モデルの状態方程式や観測方程式を構成し、数式モデル処理部106ではカルマンフィルタなどの確率モデルフィルタから、位置やその他の状態量を算出する。位置やその他の状態量は、予め決められた数が算出され、数式モデル選択部107で選択された数式モデルの数に依存しない。誤差の少ない数式モデルを選択して確率モデルフィルタを計算することで、誤差の大きい情報を含む全ての数式モデルを使用して状態量を推定するよりも精度の高い推定が可能となる。数式モデル処理部106において、数式モデルの設定は、例えば以下のようにして実現される。
【0047】
数式モデル処理部106が所持するN個の数式モデルの内、数式モデル選択部で、数式モデル1,数式モデル2,数式モデル3が選択されたとする。数式モデル1が(式1)のように表され、また、数式モデル2が(式2)のように表され、数式モデル3が(式3)のように表されるとする。ここで、Ciは数式モデルiの観測値、(x,y,z)は確率モデルフィルタリング処理部103から出力される全ての状態量、Ai1〜Ai3は数式モデルiにおけるそれぞれの状態量の係数である。
【0048】
C1=A11x+A12y+A13z …(式1)
C2=A21x+A22y+A23z …(式2)
C3=A31x+A32y+A33z …(式3)
これらの数式モデルが、カルマンフィルタにおける観測方程式として用いられる場合、ステップ607にて全ての数式モデルが重複しなければ、観測方程式は(式4)のように表される。
【0049】
【数1】
【0050】
そして、観測行列Aの各要素(A11〜A33)は、数式モデル変数テーブル111から、各数式モデルで用いられる状態量(x,y,z)に対応して設定されている係数が読み込まれて設定される。なお、数式モデル変数テーブル111に状態量に対応した係数が設定されていない要素については0が設定されるものとする。実際のプログラムでは、選択された数式モデルの状態量の係数がまとめられた観測行列Aの各要素(A11〜A33)がif文などにより設定され、カルマンフィルタに用いられる。
【0051】
また、数式モデル処理部106が所持するN個の数式モデルの内、数式モデル選択部で、数式モデル1,数式モデル2が選択されたとする場合、同様にして観測方程式は(式5)のように表され、観測行列A′の各要素(A′11〜A′23)が同様に設定される。
【0052】
【数2】
【0053】
そして(式4)の場合と同様に、観測行列A′の各要素(A′11〜A′23)は、数式モデル変数テーブル111から、各数式モデルで用いられる状態量(x,y,z)に対応して設定されている係数が読み込まれ、設定される。
【0054】
次に、本測位装置が、舵角センサとジャイロセンサを搭載するカーナビゲーションシステムに適用され、数式モデルとしては速度によるアッカーマンモデルとデッドレコニングモデルを用い、確率モデルフィルタリング処理部103では拡張カルマンフィルタを用いている場合を例に、本測位装置の具体的な動作を説明する。
【0055】
アッカーマンモデルは低速域で成り立つモデルであり、その角速度ωは、車両速度をν、実舵角をδ、前車軸から後車軸までの距離をlとして、以下の(式6)のように表される。
【0056】
ω=ν・δ/l …(式6)
一方、デッドレコニングモデルは、ジャイロセンサからの出力値ωgyroを用いて角速度を算出するため、角速度ωは、ジャイロセンサの感度係数をS、バイアスをbiasとして、以下の(式7)のように表される。
【0057】
ω=S・ωgyro+bias …(式7)
また、ある時刻をt、単位時間をΔtとして、2次元平面内の車両の位置および方位(x,y,θ)は、速度をvとした次の式で表される。
【0058】
【数3】
【0059】
よって、これら2つの数式モデルについては、支配的な変数となる状態量を速度vとすることができる。前述のようにアッカーマンモデルは低速度で成り立つモデルであるが、どの速度まで成り立つかは車両やその時の道路特性によって変わる。速度の閾値をアッカーマンモデルが成り立つ限界速度として、本装置ではこの閾値を上記の方法で決定することになる。
【0060】
ここで、(式8)で表される方程式は拡張カルマンフィルタの状態方程式の一部、(式6)や(式7)で表される方程式は観測方程式の一部となる。ある時刻tの位置x,yと方位θが(x(t),y(t),θ(t))であったとして、確率モデルフィルタリング処理部103から出力される状態量が(位置(2次元),方位,速度,角速度)であるとする。
【0061】
ある時刻tで(式7)のデッドレコニングモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部103から算出された状態量が(x(t),y(t),θ(t),ν(t),ω(t))であったとする。この算出された状態量のうち、アッカーマンモデルとデッドレコニングモデルの切り替えに必要な速度ν(t)が閾値算出部104へ出力される。また、確率モデルフィルタリング処理部103の拡張カルマンフィルタから算出される角速度の値と(式6)や(式7)で表されるそれぞれのモデルでの観測値との差を、アッカーマンモデルの誤差,デッドレコニングモデルの誤差として、閾値算出部104へ出力する。
【0062】
次に閾値算出部104では、確率モデルフィルタリング処理部103から入力された時刻tまでの速度ν(t)とそれぞれの数式モデルにおける誤差e(t)の値を用いて、各数式モデルにおける車速vに対する誤差の値の関係を曲線近似する。これにより縦軸が誤差を示し、横軸が車速vを示すようにすると図7のようなグラフが生成できる。図7の例では曲線701がデッドレコニングモデルの誤差、曲線702がアッカーマンモデルの誤差をそれぞれ表したグラフである。これらのグラフは、時刻tまでの速度に対するデッドレコニングモデルにおける誤差を示すバツ点703と、時刻tまでの速度に対するアッカーマンモデルにおける誤差を示す丸点704をそれぞれ近似する曲線を求めることで作成される。
【0063】
そして、これらの曲線701と曲線702が交わった点705の速度の値を閾値候補vthとして、閾値決定部105へ出力する。算出された閾値候補の値vthはタイヤの摩耗や道路特性の変化などにより時変な特性を持つため、ステップ405で逐次学習による変化追従型となる。
【0064】
時刻tまでの閾値候補の値の変化が図8の丸点801のようになったとする。閾値の変化特性として時定数τと規定すると、閾値vth(t)の変化は次の(式9)(式10)の2式で表される。
【0065】
vth(t+1)=(τ−Δt)/τ・vth(t) …(式9)
vth(t)=vth …(式10)
ただし、(式10)の右辺vthは閾値算出部104から入力された今回の閾値候補の値である。
【0066】
これら(式9)を状態方程式、(式10)を観測方程式としてカルマンフィルタを構成すれば、今回の閾値vth(t)802が決定される。こうして時々刻々の決定された閾値の値を図8の曲線803で示す。そして、決定された閾値vth(t)802は、数式モデルテーブル108に設定される。アッカーマンモデルを数式モデルの識別番号1、デッドレコニングモデルを数式モデルの識別番号2とした数式モデルテーブル108を図9に示す。そして、速度がvth(t)802よりも小さい速度の場合を低速度走行、大きい速度の場合を高速度走行と定義できる。図中の「1」「2」…はそれぞれ、数式モデル処理部106の数式モデルを特定する識別番号を示しており、「/」はいずれかの数式モデルが適用可能であることを示している。次に、数式モデル選択部107で、数式モデルテーブル108を参照して、前回の確率モデルフィルタリング処理部103にて計算された状態量から、数式モデルテーブル108の当てはまる行・列を選択する。前回の確率モデルフィルタリング処理部103から出力された状態量が、仮にジャイロバイアス,ジャイロスケールファクタ,センサ取付角,舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図9の枠901が選択された列である。ここで、前回の確率モデルフィルタリング処理部103にて計算された速度がν(t)であった場合、次の関係が成り立ったとする。
【0067】
ν(t)<vth(t) …(式11)
この時、前回は低速度走行していることが判るので、図9の枠902で囲われる行が選択された行となる。従って、枠901と枠902で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、いずれの要素も識別番号が1であることから、数式モデル1のアッカーマンモデルのみが選択され、時刻t+1の時の確率モデルフィルタリング処理部103においては、拡張カルマンフィルタの観測方程式としてアッカーマンモデルの観測方程式のみが用いられる。
【0068】
アッカーマンモデルとデッドレコニングモデルとの間で数式モデル処理部106が計算に用いる数式モデルを変更する場合には、使用するセンサが舵角センサとジャイロセンサとで異なるため、数式モデル選択部107では、数式モデルを変更すると同時に、使用するセンサを切り替えて、数式モデル処理部106で計算を行う。
【実施例2】
【0069】
本測位装置の動作として、GPSとジャイロセンサを搭載するカーナビゲーションシステムにおいて、GPSを採用した場合のセンサフュージョンモデルとGPS非受信もしくは採用されなかった場合のデッドレコニングモデルの間で数式モデルを切り替える例を説明する。
【0070】
センサフュージョンモデルは、前述のようにGPS位置とジャイロ等の内界センサによるデッドレコニング位置とを拡張カルマンフィルタを用いて融合し、最尤な位置推定を行うモデルである。また、内界センサモデルは、前述のようにジャイロ等の内界センサによるデッドレコニング位置を出力するモデルである。ここでの拡張カルマンフィルタの状態方程式は(式8)と同じであるとする。センサフュージョンモデルと内界センサモデルは観測方程式の数式モデルであり、センサフュージョンモデルは(式12)、内界センサモデルは(式13)のように表される。
【0071】
【数4】
【0072】
【数5】
【0073】
前述のようにセンサフュージョンモデルはGPSが採用された場合に成り立つモデルである。どのような場合にGPSが採用されるかを「現在の推定位置とGPS位置との距離が近いこと」と定義すると、内界センサモデルとの間で一般的に時不変な閾値が存在することになる。よって、支配的な変数としての状態量を、現在の推定位置とGPS位置との限界距離とし、本装置ではセンサフュージョンモデルが成り立つための閾値を上記の方法で決定することになる。
【0074】
まず、拡張カルマンフィルタが用いられる確率モデルフィルタリング処理部103を考える。上述のように、(式8)で表される方程式は拡張カルマンフィルタの状態方程式の一部、(式12)や(式13)で表される方程式は観測方程式の一部である。ある時刻tの位置、方位が(x(t),y(t),θ(t))あったとして、確率モデルフィルタリング処理部103から出力される状態量が(位置(2次元),方位)であるとする。
【0075】
ある時刻tで(式13)の内界センサモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部103から算出された状態量が(x(t),y(t),θ(t))であったとする。この算出された状態量から、センサフュージョンモデルと内界センサモデルの切り替えに必要な現在の推定位置とGPS位置との距離Lが以下の(式14)により算出される。
【0076】
【数6】
【0077】
この(式14)のLが閾値算出部104へ出力される。また、確率モデルフィルタリング処理部103の拡張カルマンフィルタから算出される位置推定値と(式12)や(式13)で表されるそれぞれのモデルでの観測値の誤差を、センサフュージョンモデルの誤差、内界センサモデルの誤差として、閾値算出部104へ出力する。
【0078】
次に閾値算出部104では、時刻tまでの確率モデルフィルタリング処理部103からの入力である現在の推定位置とGPS位置との距離L(t)および、それぞれのモデルの誤差e(t)を用いて、図10のような分布グラフが生成できる。縦軸が誤差を示し、横軸が現在の推定位置とGPS位置との距離Lを示している。図10における丸点1001が確率モデルフィルタリング処理部103にて推定された位置に対するセンサフュージョンモデルの誤差であり、バツ点1002が確率モデルフィルタリング処理部103にて推定された位置における内界センサモデルの誤差である。これらの点を前述のサポートベクターマシンを用いて、ある値Lthで2分割にする。この時の値を閾値候補Lthとして、閾値決定部105へ出力し、閾値決定部105で保存する。
【0079】
閾値決定部105では、時刻tにおける閾値候補の値および時刻tまでの閾値候補の値の平均値を今回の閾値Lth(t)として決定する。そして、決定された閾値Lth(t)は、数式モデルテーブル108に設定される。
【0080】
センサフュージョンモデルを数式モデルの識別番号5、内界センサモデルを数式モデルの識別番号6とした数式モデルテーブル108を図11に示す。この設定によって、現在の推定位置とGPS位置との距離が閾値Lth(t)よりも小さい距離となった場合を、GPS位置を融合するためGPS受信、逆に大きい距離が出力された場合はGPS位置を使用しないため、GPS非受信と定義する。
【0081】
次に、数式モデル選択部107で、数式モデルテーブル108を参照して、前回の確率モデルフィルタリング処理部103にて計算された状態量から、数式モデルテーブル108の該当する行・列を選択する。前回の確率モデルフィルタリング処理部103にて、出力された状態量が、仮にジャイロバイアス,ジャイロスケールファクタ,センサ取付角,舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図11の枠1101が選択された列である。ここで、前回の確率モデルフィルタリング処理部103にて計算された推定位置とGPS位置との距離がL(t)であった場合、次の関係が成り立ったとする。
【0082】
L(t)<Lth(t) …(式15)
この時、現在はGPS受信であることが判るので、図11の枠1102で囲われる行が選択された行となる。枠1101と枠1102で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、いずれの要素も識別番号が5であることから、数式モデル5のセンサフュージョンモデルのみが選択され、時刻t+1の時の確率モデルフィルタリング処理部103にて、(式12)が拡張カルマンフィルタの観測方程式として用いられることになる。
【実施例3】
【0083】
本装置の動作手順として、デジタル地図情報とGPSとジャイロセンサや加速度センサなどの姿勢センサを搭載し、自車位置をデジタル地図上にマッチングすることができるカーナビゲーションシステムにおいて、デジタル地図データの中にある、リンク情報を使用する場合のセンサフュージョンモデル(以後、リンクフュージョンモデルと呼ぶ)と、デジタル地図データの中のリンク情報を使用しない場合のセンサフュージョンモデル(以後、INSモデルと呼ぶ)の間で数式モデルを切り替える例にとって説明する。
【0084】
デジタル地図データは、所定の広さを持つメッシュ単位に管理されているものとする。地図の各メッシュでは、座標を持つ点(ノード)が道の形状を表すように置かれており、それぞれのノードには番号(ノード点番号)が割り当てられているものとする。そして各ノードのX座標は経度、Y座標は緯度をそれぞれ示している。また、隣接する二つのノード間の距離をノード間リンク長、道幅をノード間リンク幅と呼び、それぞれ[m]単位で表しているものとする。
【0085】
リンクフュージョンモデルは、前述のようにデジタル地図データのリンクをカルマンフィルタの観測方程式として使う数式モデルであり、αをリンクの傾き、cをリンクの切片、rをリンクからの距離、φをリンクからの傾きとして、次の(式16)のように表される。
【0086】
【数7】
【0087】
この(式16)の左辺が0であることからも判るように、リンクフュージョンモデルはリンク上に車両が存在することを前提としているため、地図情報に誤りがある場合、確率モデルフィルタリング処理部103から出力される位置も誤差を多分に含んだものとなる。
【0088】
また、INSモデルは、ジャイロセンサから角速度を加速度センサから加速度を取得し、その値を積分して、それぞれ方位・速度に変換する。よって、角速度の算出は(式7)と等しく、加速度ACCの算出は、下の(式17)のように表される。
【0089】
ACC=Sacc・GGsns−g・sinψ+bias …(式17)
ただし、Saccが加速度センサの感度係数、GGsnsが加速度センサの生出力値、gは重力加速度、ψはセンサの取付角である。
【0090】
そして、上記リンクフュージョンモデルとINSモデルの状態方程式は、(式8)と同じである。
【0091】
前述のようにリンクフュージョンモデルは地図情報に誤りがある場合には、誤差が大きくなるモデルであるので、誤った地図情報は使用しない方が良い。そこで、GPS位置は大まかには正しい位置を出力しているとして、地図情報が使用される時の定義を「GPS位置とリンクとの距離が近いこと」とすると、INSモデルとの間で一般的に時不変な閾値が存在することになる。さらに、GPS位置自体も誤差を持っており、前述の実施例2のように、GPS位置を使用しない方が良い場合がある。よって、GPS位置とリンクとの距離を地図誤差量と定義し、これを支配的な変数としての状態量として用い、この地図誤差量についてリンクフュージョンモデルが成り立つための閾値を決定し、さらに第二の実施例と同様GPS受信位置も含めての閾値を決定することになる。
【0092】
まず、拡張カルマンフィルタを用いる確率モデルフィルタリング処理部103を考える。上述のように、(式8)で表される方程式は拡張カルマンフィルタの状態方程式の一部であり、(式16)や(式7),(式17)で表される方程式は観測方程式の一部である。ある時刻tの位置、方位が(x(t),y(t),θ(t))であったとして、確率モデルフィルタリング処理部103から出力される状態量が(位置(2次元),方位)であるとする。
【0093】
ある時刻tで(式13)の内界センサモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部103から算出された状態量が(x(t),y(t),θ(t))であったとする。この時、自車位置推定装置102で前述のようなマップマッチングを行った結果、マッチングする先のリンクの両端ノードのX座標,Y座標から、リンクの方程式が(式18)のように求められる。
【0094】
xsinα−ycosα=c …(式18)
リンクフュージョンモデルとINSモデルの切り替えに必要な現在(時刻tとする)の推定位置とリンクの距離rGPSは(式18)に表されるリンク方程式とGPS位置との点と直線の距離から、以下のように算出される。
【0095】
rGPS=xgps(t)sinα−ygps(t)cosα−c …(式19)
この(式19)のrGPSが閾値算出部104へ出力される。また、確率モデルフィルタリング処理部103の拡張カルマンフィルタから算出される位置推定値と(式16)や(式7)(式17)で表されるそれぞれのモデルでの観測値の誤差を、リンクフュージョンモデルの誤差、INSモデルの誤差として、閾値算出部104へ出力する。
【0096】
次に閾値算出部104では、確率モデルフィルタリング処理部103からの入力である時刻tまでの現在の推定位置とリンクとの距離rGPS(t)および、それぞれのモデルの誤差e(t)を用いて、図12のような分布グラフが生成する。縦軸が誤差を示し、横軸が現在の推定位置とGPS位置とリンクとの距離rGPSを示している。図12における丸点1201がGPS位置とリンクとの誤差である。これらの点を最小2乗法を用いて、直線近似する。その直線が1202である。また、第二の実施例と同様の方法で、GPS受信時の閾値Lthを求め、その時のINS誤差の値1203を求める。この誤差の値1203よりも小さい誤差の時、GPS位置は大まかに正しいと言えるので、直線1202が値1203を取る時のGPS位置とリンクとの距離rth1204がこの時の閾値候補となり、閾値決定部105へ出力され、閾値決定部105で保存される。そして、閾値決定部105において、時刻tにおける閾値候補の値および時刻tまでの閾値候補の値の平均値を今回の閾値rth(t)として決定する。そして、決定された閾値rth(t)は、数式モデルテーブル108に設定される。リンクフュージョンモデルをモデル7、INSモデルをモデル5とした数式モデルテーブル108を図13に示す。この設定によって、閾値rth(t)よりも小さいGPS位置とリンクとの距離が出力された場合は地図誤差小、閾値rth(t)よりも大きいGPS位置とリンクとの距離が出力された場合は地図誤差大と定義する。
【0097】
次に、数式モデル選択部107で、数式モデルテーブル108を参照して、前回の確率モデルフィルタリング処理部103にて計算された状態量から、数式モデルテーブル108の当てはまる行・列を選択する。前回の確率モデルフィルタリング処理部103にて、出力された状態量が、仮にジャイロバイアス,ジャイロスケールファクタ,センサ取付角,舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図13の枠1301が選択された列である。ここで、第二の実施例と同様の方法で算出されたGPS受信の閾値Lth(t)に対し、前回の確率モデルフィルタリング処理部103にて計算された推定位置とGPS位置との距離がL(t)であった場合、次の関係が成り立ったとする。
【0098】
L(t)<Lth(t) …(式20)
この時、GPS受信であることが判るので、図13の枠1302で囲われる行が選択された行となる。また、GPS位置とリンクとの距離の間に次の関係が成り立ったとする。
【0099】
rGPS(t)<rth(t) …(式21)
この時、地図誤差小であることが判るので、図13の枠1303で囲われる行が選択された行となる。枠1301と枠1302および枠1303で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、数式モデルの識別番号5のINSモデルおよび数式モデルの識別番号7のリンクフュージョンモデルの両方が選択されている。このため、時刻t+1の時の確率モデルフィルタリング処理部103にて、拡張カルマンフィルタの観測方程式としては、(式7)(式16)(式17)が用いられることになる。
【符号の説明】
【0100】
101,1501 センサ
102,1502 自車位置推定装置
103,1503 確率モデルフィルタリング処理部
104,1504 閾値算出部
105,1505 閾値決定部
106,1506 数式モデル処理部
107,1507 数式モデル選択部
108,1508 数式モデルテーブル
【技術分野】
【0001】
本発明は、自動車や電車などの移動体の車載装置に関し、特に位置推定を行う機器に関する。
【背景技術】
【0002】
従来のカーナビゲーションシステムでは、測位結果を基にマップマッチングを行い、自車位置を求めていたが、特開2008−287669号公報に示されているようなカーナビゲーションシステムから車両位置を車両運動制御機器へ出力する車両運動制御システムおいては、より正確な車両位置の測位が求められる。
【0003】
正確な測位を行うため、自車位置の算出に用いる複数の数式モデルを用意し、事前に決められた一定の閾値に従い計算に用いる数式モデルを変更する技術が提案されている。例えば特開平8−14923号公報には、グローバルポジショニングシステム(Global Positioning System:以下GPS)の信号強度や衛星位置などの情報を用いて、GPSの出力する位置の誤差がある一定以上であれば、ジャイロや加速度センサなどの相対センサによる位置算出に切り替える装置が開示されている。
【0004】
あるいは、カルマンフィルタなどの確率システムを用いて、自車位置を推定するシステムでは、特開平5−333132号公報のように、受信できたGPS衛星の数に対応したカルマンフィルタを複数事前に用意し、これらを切り替えて自車位置を測定する装置が提案されている。カルマンフィルタやパーティクルフィルタなどの確率モデルを用いた位置推定ロジックを用いて自車位置を推定する技術としては、拡張カルマンフィルタアルゴリズムを用いる発明が特開2007−322391号公報に開示されている。
【0005】
測位誤差に基づき位置算出手法を切り替える場合、舵角センサとジャイロセンサを搭載するカーナビゲーションシステムにおける測位誤差のモデルとしては、速度によるアッカーマンモデルやデッドレコニングモデルが知られており、また、GPSとジャイロセンサを搭載するカーナビゲーションシステムにおける、GPSを採用した場合のセンサフュージョンモデルやGPS非受信もしくは採用されなかった場合のデッドレコニングモデルが知られている。
【0006】
アッカーマンモデルは、“安部正人,「自動車の運動と制御」,東京電機大学出版局,2008年3月,ISBN:9784501417000”に記載されている。センサフュージョンモデルと内界センサモデルは、“青野俊宏 他,「GPSと内界センサを用いた起伏地における移動機の位置計測」,計測自動制御学会論文集 Vol.35,No.8,1999年”に記載されている。
【0007】
さらに、自車位置をデジタル地図上にマッチングする際に、デジタル地図情報の中にあるリンク情報を使用する場合のセンサフュージョンモデル(以後、リンクフュージョンモデルと呼ぶ)として、特開2008−26282号公報に示されるような、デジタル地図データのリンクをカルマンフィルタの観測方程式として使う数式モデルが知られている。
【先行技術文献】
【特許文献】
【0008】
【特許文献1】特開2008−287669号公報
【特許文献2】特開平8−14923号公報
【特許文献3】特開平5−333132号公報
【特許文献4】特開2007−322391号公報
【特許文献5】特開2008−26282号公報
【非特許文献】
【0009】
【非特許文献1】“安部正人 著「自動車の運動と制御」 東京電機大学出版局 2008年3月刊 ISBN:9784501417000”
【非特許文献2】“青野俊宏 他著「GPSと内界センサを用いた起伏地における移動機の位置計測」 計測自動制御学会論文集 Vol.35,No.8,1999年”
【発明の概要】
【発明が解決しようとする課題】
【0010】
走行中の車両は、その路面状態や外気温や各部品の経年数、運転するドライバーの特性といった、前記のような誤差などの各種モデルを切り替えるための閾値の決定を事前に行うことが難しいような場合、事前に決められた不変の値では、モデル化誤差が大きくなり、自車位置の算出精度に大きな悪影響を与えかねない。またこの場合、車両運動制御に自車位置の算出精度が関わっている場合、車両制御の精度に悪影響を及ぼす可能性がある。
【課題を解決するための手段】
【0011】
自車位置の推定精度をどのような場合でも高精度に保つためには、複数のモデルを切り替えるための閾値を事前に用意した不変な値とするのではなく、自車の状態、特に自車位置を算出する際に用いるために推定や学習されるパラメータ,変数や係数などを用いて算出される値を閾値としてモデルを変化させるような自車位置推定装置を用いることにより、解決される。
【発明の効果】
【0012】
カーナビゲーションシステムやオートクルーズシステムなど、自身の絶対位置を高精度に推定しなければならないシステムにおいて、自車の状態を検出もしくは算出もしくは学習した値で、自身の絶対位置を推定するためのモデルを切り替えることで、どのような状態・状況でも常に高精度な位置を推定できるようになる。また、位置を高精度に推定するために必要な自身の誤差を元に、自車位置の信頼性を出力することが可能となる。
【図面の簡単な説明】
【0013】
【図1】測位装置の概略構成を示す図である。
【図2】状態量と数式モデルの誤差の相関を示す図である。
【図3】閾値候補生成処理のフローチャートである。
【図4】閾値決定処理のフローチャートである。
【図5】数式モデル変数テーブルを示す図である。
【図6】数式モデル選択処理のフローチャートである。
【図7】アッカーマンモデルとデッドレコニングモデルによる数式モデルの車速に対する誤差変化を表した図である。
【図8】閾値vthの時間変化を表した図である。
【図9】アッカーマンモデルとデッドレコニングモデルの数式モデルテーブルを示す図である。
【図10】センサフュージョンモデルと内界センサモデルの数式モデルによる誤差変化を表した図である。
【図11】センサフュージョンモデルと内界センサモデルの数式モデルテーブルを示す図である。
【図12】リンクフュージョンモデルとINSモデルの数式モデルによる誤差変化を表した図である。
【図13】リンクフュージョンモデルとINSモデルの数式モデルテーブルを示す図である。
【発明を実施するための形態】
【0014】
以下、本発明を用いた測位装置の詳細を説明する。
【実施例1】
【0015】
図1に本発明を用いた測位装置の概略構成を示す。測位装置は、カーナビゲーションシステムまたは専用端末上で実現され、測位装置が搭載された車両の現在の姿勢情報および位置測位情報を取得するセンサ101と自車位置推定装置102とで構成される。自車位置推定装置102は、カルマンフィルタなどの確率モデルに基づく数式モデルを用いて周期的に車両位置を算出する確率モデルフィルタリング処理部103と、この確率モデルフィルタリング処理部103で算出された速度などの車両の状態量やこの算出された状態量の誤差から、確率モデルフィルタリング処理部103で車両位置の計算に用いる数式モデルを変更するための閾値となり得る閾値候補の値を算出する閾値算出部104と、閾値算出部104にて求められた閾値候補の値および確率モデルフィルタリング処理部103で使用する数式モデルの特性を基に、確率モデルフィルタリング処理部103で計算に用いる数式モデルを変更する閾値を決定する閾値決定部105とを有する。
【0016】
確率モデルフィルタリング処理部103は、自車位置を算出するために用いる車両の挙動を表す複数の数式モデルからなる数式モデル処理部106と閾値決定部105において決定された閾値に基づいて、数式モデル処理部106において遷移行列の各要素の値の算出に用いる数式モデルを選択する数式モデル選択部107を備え、カルマンフィルタなどの確率モデルにおける状態方程式などから求められる遷移行列の各要素の値を数式モデル処理部106において計算する。ここで、数式モデルとは、測位アルゴリズムのモデルを表す状態方程式もしくは観測方程式を構成する方程式一つ一つの式のことであり、識別番号で管理されるものとする。複数の数式モデルに対応した各々の状態方程式または観測方程式を組み合わせることにより、数式モデル処理部106では状態方程式や観測方程式が表され、この状態方程式と観測方程式によりカルマンフィルタなどの確率モデルが計算される。
【0017】
閾値決定部105では、閾値算出部104で算出された閾値候補の値を保存し、数式モデル選択部107において数式モデル処理部106で計算に用いる数式モデルを選択し変更するために用いる数式モデルテーブル108に対して閾値を設定する。また、数式モデルテーブル108には、センサ101から入力される状態量を変数として各数式モデルの状態方程式または観測方程式を表したときの係数を格納した数式モデル変数テーブル111が格納されている。
【0018】
車両の姿勢情報および位置測位情報を取得するセンサ101からは、自車の姿勢や移動方向,移動速度、あるいは大局的な位置のデータが自車位置推定装置102に入力される。確率モデルフィルタリング処理部103ではこれらの入力値に基づき、数式モデル選択部107によって、数式モデル処理部106で遷移行列の計算に使用する数式モデルが選ばれ、この数式モデルの計算により求めた位置を出力する。また、数式モデル処理部106において数式モデルで算出された位置以外の状態量は閾値算出部104へ入力される。
【0019】
閾値算出部104では数式モデル処理部106に用意された複数の数式モデルの特性に従い、各数式モデルにおける状態量と誤差の関係を計算して、その数式モデルについての閾値の候補を決定する。算出された閾値の候補は閾値決定部105へ入力され、複数の数式モデルについて適用時の誤差の大きさおよびその時の状態量に基づき閾値を決定して、数式モデル選択部107から参照できるようにするため数式モデルテーブル108に格納する。閾値決定部105にて決められた各状態量毎の閾値は確率モデルフィルタリング処理部103内の数式モデル選択部107から参照され、次の位置計算の周期の際に、車両の姿勢情報および位置測位情報を取得するセンサ101からデータが自車位置推定装置102に入力された際に、数式モデル処理部106の複数の数式モデルの中から最適な数式モデルを選択するために使用される。この流れを周期的にループ処理することによって、各状態量に対して最適な閾値が順次決定され、数式モデルテーブル108が更新されていく。
【0020】
車両の姿勢情報および位置測位情報を取得するセンサ101からは、自車両の移動する方位,自車両の傾きなど車両の姿勢,自車両の絶対位置,自車両の移動する速度,自車両とその他の物体との相対的な位置および方位・姿勢の関係などの情報が出力される。
【0021】
車両の絶対位置は、GPSなどのように直接位置を算出できるセンサや、絶対位置と形状が既知の物体の相対的な位置を計測するカメラやレーダセンサなどのセンサにより算出する。
【0022】
車両の移動する方位は、直接絶対方位を計測できる地磁気センサや、初期方位が与えられた上で計測した角速度を積分することで方位を算出するジャイロセンサのような方位センサや、あるいは異なる時間に受信したGPSの信号により算出する。
【0023】
車両の移動する速度は、直接速度を算出できる車速パルスのセンサや、車両の加速度を検知して積分することで速度を算出する加速度センサ、あるいは異なる時間に受信したGPSにより算出する。
【0024】
車両の姿勢は、加速度センサなどのように重力の向きを検知することで車両の傾きを検出する姿勢検知センサや形状が既知の物体に対する相対的な姿勢を計測できるレーダセンサや、あるいは形状が既知の物体の自車に対する傾きを計測できるようなカメラの画像により算出する。
【0025】
確率モデルフィルタリング処理部103では、数式モデル処理部106が、カルマンフィルタやパーティクルフィルタなど、確率モデルに基づく位置推定ロジックを用いた複数の数式モデルにより位置を推定する。位置推定ロジックの代表的な例としては、前述のような拡張カルマンフィルタアルゴリズムによる自車位置推定がある。確率モデルに基づく位置推定ロジックでは、センサ101からの出力もしくは数式モデル処理部106で算出された自車両の方位・位置・姿勢速度の情報を、その誤差を考慮した観測方程式として表し、ある時間から次の時間へと遷移する自車の方位・位置・姿勢・速度を状態方程式として表す。数式モデル処理部106が備える各数式モデルは、この観測方程式及び状態方程式を計算してフィルタリング処理を行う。
【0026】
数式モデル処理部106では、自車両の位置と共に、方位・姿勢・速度に関する情報も推定する。これらの値を状態量と呼ぶ。算出された状態量は、閾値算出部104に出力される。また、数式モデル処理部106において、誤差や誤差の分散を算出する数式モデルを用いて計算を行う場合、その誤差も出力する。
【0027】
閾値算出部104では、確率モデルフィルタリング処理部103から入力された状態量に対する数式モデル処理部106が備える複数の数式モデルの誤差を見積もる。図2に数式モデルにおける状態量とその誤差の相関例の模式図を示す。図2の横軸は、確率モデルフィルタリング処理部103から入力された状態量、縦軸はその状態量と共に確率モデルフィルタリング処理部103から入力された誤差の値であり、数式モデル毎の状態量に対する誤差の値を示す。曲線201はある数式モデル1における、状態量に対する誤差の変化を表した曲線であり、曲線202は数式モデル2における状態量に対する誤差の変化を表した曲線である。曲線201と曲線202の交点A203が数式モデル1と数式モデル2の誤差が逆転する点であり、ここから右の状態量の場合には数式モデル1による計算のほうが誤差が小さく、逆にここから左の状態量の場合には数式モデル2を用いて計算したほうが誤差が小さくなることになる。そこで交点A203の状態量の値B204が、フィルタリング処理に用いる数式モデルを切り替える判断を行う際の閾値候補となり、この閾値候補を閾値決定部105へ出力する。
【0028】
閾値算出部104における閾値候補の生成処理を図3に示す。この処理は、数式モデル処理部106が管理する数式モデルの内、2つの数式モデルについてその誤差を比較して、各2つの数式モデル間についての閾値候補を求める処理である。まず、確率モデルフィルタリング処理部103から、数式モデル処理部106で計算された状態量を取得する(ステップ301)。取得した状態量は、それぞれ計算に用いられた数式モデル毎に最新の値から多くとも所定数分まで記憶しておく。次に、数式モデル処理部106が有する複数の数式モデルの中から、ステップ301で取得した状態量を用いている複数の数式モデルを探してリストを作り、リストの中の各数式モデルについて数式モデル変数テーブル111を参照して状態量に対応した係数を取得(以下、状態量に対応した数式モデルの係数を取得することを、数式モデルを取得する、と呼ぶ)して状態方程式または観測方程式を構成した上で、取得した状態量に基づき計算を行い、各数式モデルについてその誤差を取得する(ステップ302)。数式モデル変数テーブル111には、予め閾値候補を決定するために必要な数式モデルのみが設定されているものとする。また、取得した数式モデルの誤差はそれぞれ計算に用いられた数式モデル毎に、最新の値から多くとも所定数分まで閾値算出部104で記憶しておくものとする。
【0029】
そして、ステップ301で取得した状態量の中から、数式モデルにおける誤差比較に用いる状態量(これを、支配的な変数となる状態量と呼ぶ)としている数式モデルがステップ302で作成したリストに存在する未処理の状態量が残っているか調べる(ステップ303)。支配的な変数となる状態量は、数式モデルテーブル108に格納されている数式モデル変数テーブル111に予め定義されているものとする。数式モデル変数テーブルは図5に示すように、数式モデル処理部106が備える各数式モデルについて、各々の数式モデルが処理する状態量に対応した変数の変数名と、その変数が支配的であるか否かを表すフラグが記憶されている。数式モデル変数テーブル111は、予め閾値候補を決定するために必要な数式モデルのみが設定されており、このテーブルの順番で選択の対象となる数式モデルが存在するかどうかを数式モデルテーブルの番号順に検索する。未処理の状態量があれば、その支配的な変数となる状態量を選択する(ステップ304)。
【0030】
次に、選択した状態量を支配的な変数とする数式モデルをステップ302で作成したリストを先頭から順に探し、最初に見つかった未処理の数式モデルの次から、同じ状態量を支配的な変数とする数式モデルを探すことにより、同じ状態量を支配的な変数とする数式モデルの対を選択する(ステップ305)。対象となる数式モデルの対が一つでも見つかった場合、数式モデル変数テーブルの番号と数式モデルテーブルの番号を一時保存し、ステップ306,307へ進む。
【0031】
ここでは、対処となる数式モデルが少なくとも1つは存在するものとして、次にステップ306,ステップ307にて、誤差比較する2つの数式モデルに対して、ステップ304で選択した状態量の変化に対する誤差を、ステップ302で取得した誤差を設定し、図2における曲線201および曲線202を決定していく。そして、ステップ306およびステップ307で算出もしくは推定した誤差に基づき決定した曲線同士の交点を算出し、その交点における状態量の値を算出する(ステップ308)。算出された状態量は、閾値候補として閾値決定部105へ出力もしくは、出力待機としてバッファリングされる(ステップ309)。
【0032】
そして、ステップ310にてステップ302で取得した各数式モデルのリストに対して、選択した状態量を支配的な変数とする数式モデルを先頭方向から順に探し、見つかった未処理の数式モデルの次から、また同じ状態量を支配的な変数とする数式モデルを探すことを繰り返して、支配的である状態量が重なる数式モデルの全ての組み合わせに対して、ステップ306からステップ309の処理を完了したかどうかを判断し、全て計算が完了していた場合(ステップ310:yes)、即ちステップ304で選択した状態量を支配的な変数とする数式モデルの対を全て処理し終えていた場合には、ステップ303に戻る。またこの時、ステップ309にて出力待機としてバッファリングしたデータが存在する場合は、このタイミングで閾値決定部105へ出力しても良い。一方、全ての数式モデルの対について計算が完了していない場合は、まだその他の選択対象となる数式モデルが存在する可能性があるためステップ305へ戻り、上記の計算を繰り返す(ステップ310:no)。
【0033】
また、ステップ303の判定で、ステップ302で作成したリストの数式モデルが支配的な変数としている状態量の内、未処理の状態量が残っていない場合(ステップ303:不可)、閾値算出部104における閾値候補の生成処理を終了する。
【0034】
閾値決定部105では、閾値算出部104から入力された閾値候補を用いて、各状態量の閾値を決定する。決定された閾値は数式モデルテーブル108に出力される。なお、閾値決定部105は、閾値算出部104からの閾値候補を記憶しておく記憶領域(図示せず)を備えている。
【0035】
閾値算出部104から入力された閾値候補から各状態量の閾値を決定する閾値決定処理を図4のフローチャートに示す。まず、数式モデルテーブル108の数式モデル変数テーブル111から、数式モデル処理部106が有する複数の数式モデルを取得する(ステップ401)。次に、閾値算出部104から出力された閾値候補を取得して(ステップ402)、これまでの閾値候補を全て保存している領域に更に今回の閾値候補の値を保存する(ステップ403)。これまで保存されていた閾値候補は、メモリ量の上限が設定されている場合には、平均値などのような代表点に置き換えても良い。
【0036】
次にステップ404にて、ある閾値候補の状態量を選び、選んだ閾値候補の状態量が誤差に影響する数式モデルが存在するかを判断する。そして、閾値候補の状態量が変動すると誤差が変動するような数式モデルが存在する場合、ステップ405にて、閾値候補の状態量の特性が時変な閾値候補(時間変動する状態量)と時不変な閾値候補(時間変動しないはずの状態量)とで処理を分ける。状態量が時変か時不変であるかは、後述する数式モデルテーブル108で分類されて設定されているため、これを参照することになる。閾値候補が時変な状態量であった場合、ステップ406へ移り、これまでの閾値候補の値を踏まえた逐次学習法、例えば、カルマンフィルタや最小2乗法などを用いて時変な閾値の変動に追従できるよう逐次学習法のゲインを設定する。そして、ステップ407にて、ステップ406からの逐次学習法による追従の最終出力値を閾値として決定する。次に、ステップ408にて数式モデルテーブル108の対象行もしくは対象列に対して、ステップ407にて決定した閾値よりも大きい場合に誤差が小さくなるような数式モデルおよび、ステップ407にて決定した閾値よりも小さい場合に誤差が小さくなるような数式モデルを数式モデルテーブル108の対象となる要素を選択し、数式モデルを表す番号を設定する。図2を例にとると、閾値の対象となる状態量の値がBより小さい場合は数式モデル2の方が誤差が小さいので、数式モデルテーブルの要素に「2」を設定し、閾値の対象となる状態量の値がBより大きい場合は数式モデル1の方が誤差が小さいので、数式モデルテーブルの要素に「1」を設定する。
【0037】
また、閾値候補が時不変な状態量であった場合、ステップ409へ移り、閾値候補の平均値をとる手法や、これまでの閾値候補の値を2つに分類するクラスタリングの手法、例えば、“Christopher M. Bishop 著「パターン認識と機械学習」下 シュプリンガー・ジャパン 2008年7月1日刊 ISBN:9784431100317”に記されているようなサポートベクターマシンなどを用いて閾値の決定を行う。そして、ステップ410にて、ステップ409からの最終出力値を閾値として決定する。次に、ステップ411にて数式モデルテーブル108の対象行もしくは対象列に対して、ステップ410にて決定した閾値よりも大きい場合に誤差が小さくなるような数式モデルおよび、ステップ410にて決定した閾値よりも小さい場合に誤差が小さくなるような数式モデルを数式モデルテーブル108の対象となる要素を選択し、数式モデルを表す識別番号を設定する。
【0038】
次に、ステップ412にて、ステップ407もしくはステップ410によって決定された閾値が全ての数式モデルに対して決定されていることを確認する。これは、数式モデル中に複数の閾値(状態量)が存在することを許容するため、唯一の閾値(状態量)の決定だけでは不足するためである。もし、全ての数式モデルに対して閾値が決定されていない場合、即ち、閾値が決定されていない数式モデルが存在する場合にはステップ404に戻り、さらにその他の閾値の決定を行う。全ての数式モデルに対して閾値が決定されている場合、ステップ413に進み、これまでの閾値として決定した値を削除して新たにステップ407もしくはステップ410にて算出された値を閾値として更新する。最後にステップ414にて、新たに決定した閾値を数式モデルテーブル108に設定する。以上の処理により、閾値候補が算出されていた状態量に対して閾値が1つ決定され、数式モデル処理部106にある全ての数式モデルに対して少なくとも1つの閾値が決定されることになる。
【0039】
次に、図9に数式モデルテーブル108の構成を示す。このテーブルの縦方向には、誤差の大きさを決定するために必要な数式モデルの状態量が並ぶ。この状態量は確率モデルフィルタリング処理部103にて計算される状態量であり、時変な状態量とする。この誤差の大きさを決定するために必要な数式モデルとは、例えば、確率モデルフィルタリング処理部103でカルマンフィルタが用いられていた場合は、観測方程式で用いられる。
【0040】
誤差の大きさを決定するために必要な数式モデル内に低速度状態ではモデル化誤差が大きいようなモデルがある場合、誤差の大小により低速度走行時と高速度走行時の二つの状態が存在することになり、どちらの状態かによってモデルの誤差の大小が分かれる。また、横方向には、確率モデルの内部状態を表す状態量を置く。これは主に、確率モデルフィルタリング処理部103にて計算される状態量から計算される状態量であり、時不変な状態量とする。縦方向の数式モデル分類用に置いた状態量であっても、確率モデルの内部状態を表す状態量であれば時間変動が小さいものと見なして、横方向の状態量に置いてよい。
【0041】
時不変な状態量として、例えば、確率モデルフィルタリング処理部103にて計算される状態量の中にセンサの取付角の誤差が存在していた場合、この状態量は横方向に並べた状態量の一つに含まれ、学習が完了と未完の二つの状態が存在することになる。このような学習の完了・未完の判断は、確率モデルフィルタリング処理部103の中で、フィルタリング処理とは別に計算することになる。上記の例では、予め定められた分散値をある一定時間内のセンサ取付角の誤差の計算結果の分散値で割ることで、どれほどセンサの取付角の誤差が揺れているか、つまり学習が進んでいるかを数値化できる。この数値を、学習度と呼び、ある値を閾値として学習の完了・未完を判断する。このような内部状態を表す状態量としては、その他にジャイロセンサのバイアスの学習やスケールファクタの学習、舵角特性の学習などがある。学習度は1以上の値となった状態を、理論上学習が完全に終わった状態とする。
【0042】
数式モデルテーブル108の内部には、縦軸と横軸で選ばれた状態量の状況の組み合わせで最適なモデルが設定される。この最適なモデルとは、ステップ408もしくはステップ411にて設定される数式モデルである。数式モデル処理部106の中には排他ではない複数の数式モデルが存在しても良い。このため、数式モデルテーブルの内部の縦軸と横軸で選ばれた状況に対しては、複数の数式モデルが設定されうる。
【0043】
ステップ413にて保存された閾値は、ステップ414にて、縦軸もしくは横軸の閾値として設定される。ステップ414にて設定された閾値が、速度10km/hおよびセンサ取付角学習度0.1であるとすれば、縦軸内の低速度走行は10km/h未満を示し、高速度走行は10km/h以上を示すことになる。また同様に、横軸において、センサ取付角学習度が0.1以下の場合を学習未完、0.1以上を学習完了として設定することになる。
【0044】
数式モデル選択部107では、閾値決定部105で作成された数式モデルテーブル108に従い、現在の車両状態に最も適した数式モデルを複数選択する。図6に数式モデル選択部107における複数の数式モデル処理部106から最適なモデルを複数選択する処理のフローチャートを示す。まず、ステップ601にて、数式モデルテーブル108を読み込む。次に数式モデル処理部106で前回計算された車両の挙動や姿勢・状態を表す状態量を読み込む(ステップ602)。次に、読み込んだ各状態量が数式モデルテーブル108に設定された閾値の縦軸側と横軸側のどちら側に属するかを判定し、数式モデルテーブル108から縦軸と横軸について計算された車両の状況(状態量)の値と条件が合致する行・列を選択する(ステップ603)。ここで選択する行および列は複数であって良い。そしてステップ604にて、選択した行・列内で設定されている対応する状況に最適な数式モデルを読み込む。
【0045】
次にステップ605にて、ステップ604で選択された全ての数式モデルについて重複の有無を確認し、重複するモデルがある場合、ステップ606に移行して、重複する数式モデルが無いようにしてから数式モデル処理部106における計算対象の数式モデルとして設定する。これは、数式モデル処理部106にて、同じ方程式を重複して使用することを避けるためである。また、ステップ605にて、選択した数式モデルに重複する数式モデルが存在しない場合、ステップ607に移行して、選択した全ての数式モデルを数式モデル処理部106に設定する。
【0046】
数式モデル処理部106では、数式モデル選択部107で選択された数式モデルのみを用いて位置やその他の状態量を算出する。数式モデル処理部106は全体でN個の数式モデルを管理しているとすると、数式モデル選択部107ではその内のn個が選択されることになる。(ただし、nはN以下の数。)数式モデル選択部107では、選択したn個の数式モデルに対応した、確率モデルの状態方程式や観測方程式を構成し、数式モデル処理部106ではカルマンフィルタなどの確率モデルフィルタから、位置やその他の状態量を算出する。位置やその他の状態量は、予め決められた数が算出され、数式モデル選択部107で選択された数式モデルの数に依存しない。誤差の少ない数式モデルを選択して確率モデルフィルタを計算することで、誤差の大きい情報を含む全ての数式モデルを使用して状態量を推定するよりも精度の高い推定が可能となる。数式モデル処理部106において、数式モデルの設定は、例えば以下のようにして実現される。
【0047】
数式モデル処理部106が所持するN個の数式モデルの内、数式モデル選択部で、数式モデル1,数式モデル2,数式モデル3が選択されたとする。数式モデル1が(式1)のように表され、また、数式モデル2が(式2)のように表され、数式モデル3が(式3)のように表されるとする。ここで、Ciは数式モデルiの観測値、(x,y,z)は確率モデルフィルタリング処理部103から出力される全ての状態量、Ai1〜Ai3は数式モデルiにおけるそれぞれの状態量の係数である。
【0048】
C1=A11x+A12y+A13z …(式1)
C2=A21x+A22y+A23z …(式2)
C3=A31x+A32y+A33z …(式3)
これらの数式モデルが、カルマンフィルタにおける観測方程式として用いられる場合、ステップ607にて全ての数式モデルが重複しなければ、観測方程式は(式4)のように表される。
【0049】
【数1】
【0050】
そして、観測行列Aの各要素(A11〜A33)は、数式モデル変数テーブル111から、各数式モデルで用いられる状態量(x,y,z)に対応して設定されている係数が読み込まれて設定される。なお、数式モデル変数テーブル111に状態量に対応した係数が設定されていない要素については0が設定されるものとする。実際のプログラムでは、選択された数式モデルの状態量の係数がまとめられた観測行列Aの各要素(A11〜A33)がif文などにより設定され、カルマンフィルタに用いられる。
【0051】
また、数式モデル処理部106が所持するN個の数式モデルの内、数式モデル選択部で、数式モデル1,数式モデル2が選択されたとする場合、同様にして観測方程式は(式5)のように表され、観測行列A′の各要素(A′11〜A′23)が同様に設定される。
【0052】
【数2】
【0053】
そして(式4)の場合と同様に、観測行列A′の各要素(A′11〜A′23)は、数式モデル変数テーブル111から、各数式モデルで用いられる状態量(x,y,z)に対応して設定されている係数が読み込まれ、設定される。
【0054】
次に、本測位装置が、舵角センサとジャイロセンサを搭載するカーナビゲーションシステムに適用され、数式モデルとしては速度によるアッカーマンモデルとデッドレコニングモデルを用い、確率モデルフィルタリング処理部103では拡張カルマンフィルタを用いている場合を例に、本測位装置の具体的な動作を説明する。
【0055】
アッカーマンモデルは低速域で成り立つモデルであり、その角速度ωは、車両速度をν、実舵角をδ、前車軸から後車軸までの距離をlとして、以下の(式6)のように表される。
【0056】
ω=ν・δ/l …(式6)
一方、デッドレコニングモデルは、ジャイロセンサからの出力値ωgyroを用いて角速度を算出するため、角速度ωは、ジャイロセンサの感度係数をS、バイアスをbiasとして、以下の(式7)のように表される。
【0057】
ω=S・ωgyro+bias …(式7)
また、ある時刻をt、単位時間をΔtとして、2次元平面内の車両の位置および方位(x,y,θ)は、速度をvとした次の式で表される。
【0058】
【数3】
【0059】
よって、これら2つの数式モデルについては、支配的な変数となる状態量を速度vとすることができる。前述のようにアッカーマンモデルは低速度で成り立つモデルであるが、どの速度まで成り立つかは車両やその時の道路特性によって変わる。速度の閾値をアッカーマンモデルが成り立つ限界速度として、本装置ではこの閾値を上記の方法で決定することになる。
【0060】
ここで、(式8)で表される方程式は拡張カルマンフィルタの状態方程式の一部、(式6)や(式7)で表される方程式は観測方程式の一部となる。ある時刻tの位置x,yと方位θが(x(t),y(t),θ(t))であったとして、確率モデルフィルタリング処理部103から出力される状態量が(位置(2次元),方位,速度,角速度)であるとする。
【0061】
ある時刻tで(式7)のデッドレコニングモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部103から算出された状態量が(x(t),y(t),θ(t),ν(t),ω(t))であったとする。この算出された状態量のうち、アッカーマンモデルとデッドレコニングモデルの切り替えに必要な速度ν(t)が閾値算出部104へ出力される。また、確率モデルフィルタリング処理部103の拡張カルマンフィルタから算出される角速度の値と(式6)や(式7)で表されるそれぞれのモデルでの観測値との差を、アッカーマンモデルの誤差,デッドレコニングモデルの誤差として、閾値算出部104へ出力する。
【0062】
次に閾値算出部104では、確率モデルフィルタリング処理部103から入力された時刻tまでの速度ν(t)とそれぞれの数式モデルにおける誤差e(t)の値を用いて、各数式モデルにおける車速vに対する誤差の値の関係を曲線近似する。これにより縦軸が誤差を示し、横軸が車速vを示すようにすると図7のようなグラフが生成できる。図7の例では曲線701がデッドレコニングモデルの誤差、曲線702がアッカーマンモデルの誤差をそれぞれ表したグラフである。これらのグラフは、時刻tまでの速度に対するデッドレコニングモデルにおける誤差を示すバツ点703と、時刻tまでの速度に対するアッカーマンモデルにおける誤差を示す丸点704をそれぞれ近似する曲線を求めることで作成される。
【0063】
そして、これらの曲線701と曲線702が交わった点705の速度の値を閾値候補vthとして、閾値決定部105へ出力する。算出された閾値候補の値vthはタイヤの摩耗や道路特性の変化などにより時変な特性を持つため、ステップ405で逐次学習による変化追従型となる。
【0064】
時刻tまでの閾値候補の値の変化が図8の丸点801のようになったとする。閾値の変化特性として時定数τと規定すると、閾値vth(t)の変化は次の(式9)(式10)の2式で表される。
【0065】
vth(t+1)=(τ−Δt)/τ・vth(t) …(式9)
vth(t)=vth …(式10)
ただし、(式10)の右辺vthは閾値算出部104から入力された今回の閾値候補の値である。
【0066】
これら(式9)を状態方程式、(式10)を観測方程式としてカルマンフィルタを構成すれば、今回の閾値vth(t)802が決定される。こうして時々刻々の決定された閾値の値を図8の曲線803で示す。そして、決定された閾値vth(t)802は、数式モデルテーブル108に設定される。アッカーマンモデルを数式モデルの識別番号1、デッドレコニングモデルを数式モデルの識別番号2とした数式モデルテーブル108を図9に示す。そして、速度がvth(t)802よりも小さい速度の場合を低速度走行、大きい速度の場合を高速度走行と定義できる。図中の「1」「2」…はそれぞれ、数式モデル処理部106の数式モデルを特定する識別番号を示しており、「/」はいずれかの数式モデルが適用可能であることを示している。次に、数式モデル選択部107で、数式モデルテーブル108を参照して、前回の確率モデルフィルタリング処理部103にて計算された状態量から、数式モデルテーブル108の当てはまる行・列を選択する。前回の確率モデルフィルタリング処理部103から出力された状態量が、仮にジャイロバイアス,ジャイロスケールファクタ,センサ取付角,舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図9の枠901が選択された列である。ここで、前回の確率モデルフィルタリング処理部103にて計算された速度がν(t)であった場合、次の関係が成り立ったとする。
【0067】
ν(t)<vth(t) …(式11)
この時、前回は低速度走行していることが判るので、図9の枠902で囲われる行が選択された行となる。従って、枠901と枠902で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、いずれの要素も識別番号が1であることから、数式モデル1のアッカーマンモデルのみが選択され、時刻t+1の時の確率モデルフィルタリング処理部103においては、拡張カルマンフィルタの観測方程式としてアッカーマンモデルの観測方程式のみが用いられる。
【0068】
アッカーマンモデルとデッドレコニングモデルとの間で数式モデル処理部106が計算に用いる数式モデルを変更する場合には、使用するセンサが舵角センサとジャイロセンサとで異なるため、数式モデル選択部107では、数式モデルを変更すると同時に、使用するセンサを切り替えて、数式モデル処理部106で計算を行う。
【実施例2】
【0069】
本測位装置の動作として、GPSとジャイロセンサを搭載するカーナビゲーションシステムにおいて、GPSを採用した場合のセンサフュージョンモデルとGPS非受信もしくは採用されなかった場合のデッドレコニングモデルの間で数式モデルを切り替える例を説明する。
【0070】
センサフュージョンモデルは、前述のようにGPS位置とジャイロ等の内界センサによるデッドレコニング位置とを拡張カルマンフィルタを用いて融合し、最尤な位置推定を行うモデルである。また、内界センサモデルは、前述のようにジャイロ等の内界センサによるデッドレコニング位置を出力するモデルである。ここでの拡張カルマンフィルタの状態方程式は(式8)と同じであるとする。センサフュージョンモデルと内界センサモデルは観測方程式の数式モデルであり、センサフュージョンモデルは(式12)、内界センサモデルは(式13)のように表される。
【0071】
【数4】
【0072】
【数5】
【0073】
前述のようにセンサフュージョンモデルはGPSが採用された場合に成り立つモデルである。どのような場合にGPSが採用されるかを「現在の推定位置とGPS位置との距離が近いこと」と定義すると、内界センサモデルとの間で一般的に時不変な閾値が存在することになる。よって、支配的な変数としての状態量を、現在の推定位置とGPS位置との限界距離とし、本装置ではセンサフュージョンモデルが成り立つための閾値を上記の方法で決定することになる。
【0074】
まず、拡張カルマンフィルタが用いられる確率モデルフィルタリング処理部103を考える。上述のように、(式8)で表される方程式は拡張カルマンフィルタの状態方程式の一部、(式12)や(式13)で表される方程式は観測方程式の一部である。ある時刻tの位置、方位が(x(t),y(t),θ(t))あったとして、確率モデルフィルタリング処理部103から出力される状態量が(位置(2次元),方位)であるとする。
【0075】
ある時刻tで(式13)の内界センサモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部103から算出された状態量が(x(t),y(t),θ(t))であったとする。この算出された状態量から、センサフュージョンモデルと内界センサモデルの切り替えに必要な現在の推定位置とGPS位置との距離Lが以下の(式14)により算出される。
【0076】
【数6】
【0077】
この(式14)のLが閾値算出部104へ出力される。また、確率モデルフィルタリング処理部103の拡張カルマンフィルタから算出される位置推定値と(式12)や(式13)で表されるそれぞれのモデルでの観測値の誤差を、センサフュージョンモデルの誤差、内界センサモデルの誤差として、閾値算出部104へ出力する。
【0078】
次に閾値算出部104では、時刻tまでの確率モデルフィルタリング処理部103からの入力である現在の推定位置とGPS位置との距離L(t)および、それぞれのモデルの誤差e(t)を用いて、図10のような分布グラフが生成できる。縦軸が誤差を示し、横軸が現在の推定位置とGPS位置との距離Lを示している。図10における丸点1001が確率モデルフィルタリング処理部103にて推定された位置に対するセンサフュージョンモデルの誤差であり、バツ点1002が確率モデルフィルタリング処理部103にて推定された位置における内界センサモデルの誤差である。これらの点を前述のサポートベクターマシンを用いて、ある値Lthで2分割にする。この時の値を閾値候補Lthとして、閾値決定部105へ出力し、閾値決定部105で保存する。
【0079】
閾値決定部105では、時刻tにおける閾値候補の値および時刻tまでの閾値候補の値の平均値を今回の閾値Lth(t)として決定する。そして、決定された閾値Lth(t)は、数式モデルテーブル108に設定される。
【0080】
センサフュージョンモデルを数式モデルの識別番号5、内界センサモデルを数式モデルの識別番号6とした数式モデルテーブル108を図11に示す。この設定によって、現在の推定位置とGPS位置との距離が閾値Lth(t)よりも小さい距離となった場合を、GPS位置を融合するためGPS受信、逆に大きい距離が出力された場合はGPS位置を使用しないため、GPS非受信と定義する。
【0081】
次に、数式モデル選択部107で、数式モデルテーブル108を参照して、前回の確率モデルフィルタリング処理部103にて計算された状態量から、数式モデルテーブル108の該当する行・列を選択する。前回の確率モデルフィルタリング処理部103にて、出力された状態量が、仮にジャイロバイアス,ジャイロスケールファクタ,センサ取付角,舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図11の枠1101が選択された列である。ここで、前回の確率モデルフィルタリング処理部103にて計算された推定位置とGPS位置との距離がL(t)であった場合、次の関係が成り立ったとする。
【0082】
L(t)<Lth(t) …(式15)
この時、現在はGPS受信であることが判るので、図11の枠1102で囲われる行が選択された行となる。枠1101と枠1102で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、いずれの要素も識別番号が5であることから、数式モデル5のセンサフュージョンモデルのみが選択され、時刻t+1の時の確率モデルフィルタリング処理部103にて、(式12)が拡張カルマンフィルタの観測方程式として用いられることになる。
【実施例3】
【0083】
本装置の動作手順として、デジタル地図情報とGPSとジャイロセンサや加速度センサなどの姿勢センサを搭載し、自車位置をデジタル地図上にマッチングすることができるカーナビゲーションシステムにおいて、デジタル地図データの中にある、リンク情報を使用する場合のセンサフュージョンモデル(以後、リンクフュージョンモデルと呼ぶ)と、デジタル地図データの中のリンク情報を使用しない場合のセンサフュージョンモデル(以後、INSモデルと呼ぶ)の間で数式モデルを切り替える例にとって説明する。
【0084】
デジタル地図データは、所定の広さを持つメッシュ単位に管理されているものとする。地図の各メッシュでは、座標を持つ点(ノード)が道の形状を表すように置かれており、それぞれのノードには番号(ノード点番号)が割り当てられているものとする。そして各ノードのX座標は経度、Y座標は緯度をそれぞれ示している。また、隣接する二つのノード間の距離をノード間リンク長、道幅をノード間リンク幅と呼び、それぞれ[m]単位で表しているものとする。
【0085】
リンクフュージョンモデルは、前述のようにデジタル地図データのリンクをカルマンフィルタの観測方程式として使う数式モデルであり、αをリンクの傾き、cをリンクの切片、rをリンクからの距離、φをリンクからの傾きとして、次の(式16)のように表される。
【0086】
【数7】
【0087】
この(式16)の左辺が0であることからも判るように、リンクフュージョンモデルはリンク上に車両が存在することを前提としているため、地図情報に誤りがある場合、確率モデルフィルタリング処理部103から出力される位置も誤差を多分に含んだものとなる。
【0088】
また、INSモデルは、ジャイロセンサから角速度を加速度センサから加速度を取得し、その値を積分して、それぞれ方位・速度に変換する。よって、角速度の算出は(式7)と等しく、加速度ACCの算出は、下の(式17)のように表される。
【0089】
ACC=Sacc・GGsns−g・sinψ+bias …(式17)
ただし、Saccが加速度センサの感度係数、GGsnsが加速度センサの生出力値、gは重力加速度、ψはセンサの取付角である。
【0090】
そして、上記リンクフュージョンモデルとINSモデルの状態方程式は、(式8)と同じである。
【0091】
前述のようにリンクフュージョンモデルは地図情報に誤りがある場合には、誤差が大きくなるモデルであるので、誤った地図情報は使用しない方が良い。そこで、GPS位置は大まかには正しい位置を出力しているとして、地図情報が使用される時の定義を「GPS位置とリンクとの距離が近いこと」とすると、INSモデルとの間で一般的に時不変な閾値が存在することになる。さらに、GPS位置自体も誤差を持っており、前述の実施例2のように、GPS位置を使用しない方が良い場合がある。よって、GPS位置とリンクとの距離を地図誤差量と定義し、これを支配的な変数としての状態量として用い、この地図誤差量についてリンクフュージョンモデルが成り立つための閾値を決定し、さらに第二の実施例と同様GPS受信位置も含めての閾値を決定することになる。
【0092】
まず、拡張カルマンフィルタを用いる確率モデルフィルタリング処理部103を考える。上述のように、(式8)で表される方程式は拡張カルマンフィルタの状態方程式の一部であり、(式16)や(式7),(式17)で表される方程式は観測方程式の一部である。ある時刻tの位置、方位が(x(t),y(t),θ(t))であったとして、確率モデルフィルタリング処理部103から出力される状態量が(位置(2次元),方位)であるとする。
【0093】
ある時刻tで(式13)の内界センサモデルを用いた拡張カルマンフィルタにより、確率モデルフィルタリング処理部103から算出された状態量が(x(t),y(t),θ(t))であったとする。この時、自車位置推定装置102で前述のようなマップマッチングを行った結果、マッチングする先のリンクの両端ノードのX座標,Y座標から、リンクの方程式が(式18)のように求められる。
【0094】
xsinα−ycosα=c …(式18)
リンクフュージョンモデルとINSモデルの切り替えに必要な現在(時刻tとする)の推定位置とリンクの距離rGPSは(式18)に表されるリンク方程式とGPS位置との点と直線の距離から、以下のように算出される。
【0095】
rGPS=xgps(t)sinα−ygps(t)cosα−c …(式19)
この(式19)のrGPSが閾値算出部104へ出力される。また、確率モデルフィルタリング処理部103の拡張カルマンフィルタから算出される位置推定値と(式16)や(式7)(式17)で表されるそれぞれのモデルでの観測値の誤差を、リンクフュージョンモデルの誤差、INSモデルの誤差として、閾値算出部104へ出力する。
【0096】
次に閾値算出部104では、確率モデルフィルタリング処理部103からの入力である時刻tまでの現在の推定位置とリンクとの距離rGPS(t)および、それぞれのモデルの誤差e(t)を用いて、図12のような分布グラフが生成する。縦軸が誤差を示し、横軸が現在の推定位置とGPS位置とリンクとの距離rGPSを示している。図12における丸点1201がGPS位置とリンクとの誤差である。これらの点を最小2乗法を用いて、直線近似する。その直線が1202である。また、第二の実施例と同様の方法で、GPS受信時の閾値Lthを求め、その時のINS誤差の値1203を求める。この誤差の値1203よりも小さい誤差の時、GPS位置は大まかに正しいと言えるので、直線1202が値1203を取る時のGPS位置とリンクとの距離rth1204がこの時の閾値候補となり、閾値決定部105へ出力され、閾値決定部105で保存される。そして、閾値決定部105において、時刻tにおける閾値候補の値および時刻tまでの閾値候補の値の平均値を今回の閾値rth(t)として決定する。そして、決定された閾値rth(t)は、数式モデルテーブル108に設定される。リンクフュージョンモデルをモデル7、INSモデルをモデル5とした数式モデルテーブル108を図13に示す。この設定によって、閾値rth(t)よりも小さいGPS位置とリンクとの距離が出力された場合は地図誤差小、閾値rth(t)よりも大きいGPS位置とリンクとの距離が出力された場合は地図誤差大と定義する。
【0097】
次に、数式モデル選択部107で、数式モデルテーブル108を参照して、前回の確率モデルフィルタリング処理部103にて計算された状態量から、数式モデルテーブル108の当てはまる行・列を選択する。前回の確率モデルフィルタリング処理部103にて、出力された状態量が、仮にジャイロバイアス,ジャイロスケールファクタ,センサ取付角,舵角特性であり、それぞれの学習度の計算結果が、ジャイロバイアスの学習度が“学習完了”、ジャイロスケールファクタの学習度が“学習完了”、センサ取付角の学習度が“学習未完”、舵角特性の学習度が“学習完了”であったとする。この場合、図13の枠1301が選択された列である。ここで、第二の実施例と同様の方法で算出されたGPS受信の閾値Lth(t)に対し、前回の確率モデルフィルタリング処理部103にて計算された推定位置とGPS位置との距離がL(t)であった場合、次の関係が成り立ったとする。
【0098】
L(t)<Lth(t) …(式20)
この時、GPS受信であることが判るので、図13の枠1302で囲われる行が選択された行となる。また、GPS位置とリンクとの距離の間に次の関係が成り立ったとする。
【0099】
rGPS(t)<rth(t) …(式21)
この時、地図誤差小であることが判るので、図13の枠1303で囲われる行が選択された行となる。枠1301と枠1302および枠1303で同時に囲われたテーブル要素が今回選択されるべき数式モデルであり、数式モデルの識別番号5のINSモデルおよび数式モデルの識別番号7のリンクフュージョンモデルの両方が選択されている。このため、時刻t+1の時の確率モデルフィルタリング処理部103にて、拡張カルマンフィルタの観測方程式としては、(式7)(式16)(式17)が用いられることになる。
【符号の説明】
【0100】
101,1501 センサ
102,1502 自車位置推定装置
103,1503 確率モデルフィルタリング処理部
104,1504 閾値算出部
105,1505 閾値決定部
106,1506 数式モデル処理部
107,1507 数式モデル選択部
108,1508 数式モデルテーブル
【特許請求の範囲】
【請求項1】
移動体の姿勢情報および位置測位情報をセンサから取得し、取得した姿勢情報および位置測位情報に基づき移動体の位置を、予め備えられた移動体の挙動を表す数式モデルに基づき算出する数式モデル処理部を備えた位置算出装置において、
前記数式モデル処理部は複数の数式モデルを備え、
位置算出装置は、
位置の算出を行う前記数式モデル処理部における確率モデルを使ったフィルタリング処理により算出した誤差を用いて閾値の候補値を計算する閾値算出部と、
数式モデルを選択するための前記閾値を前記候補値に基づき決定する閾値決定部と、
数式モデル処理部で位置算出に用いる数式モデルを前記閾値決定部で決定された閾値に基づき選択する数式モデル選択部と、
を有することを特徴とする位置推定装置。
【請求項2】
請求項1に記載の位置推定装置において、移動体の挙動を表す前記複数の数式モデルは、外部から観測可能な情報もしくは数式モデル処理部で算出した値から決められる値に基づく、移動体の挙動に対する適応範囲の制限を有し、
前記閾値決定部は、各数式モデルにより計算される誤差の大きさに基づいて前記閾値を決定し、
前記数式モデル選択部は、数式モデルの制限に応じて数式モデルを選択する、ことを特徴とする位置推定装置。
【請求項3】
請求項2に記載の位置推定装置において、前記数式モデルの適応範囲の制限と前記閾値を記憶するデータベースを有することを特徴とする位置推定装置。
【請求項4】
移動体の姿勢情報および位置測位情報をセンサから取得し、取得した姿勢情報および位置測位情報に基づき移動体の位置を、予め備えられた移動体の挙動を表す数式モデルに基づき算出する位置推定方法において、
数式モデルに従って位置の算出を行う確率モデルを使ったフィルタリング処理により算出した誤差を用いて閾値の候補値を計算する処理と、
数式モデルを選択するための前記閾値を前記候補値に基づき決定する処理と、
予め用意されている複数の数式モデルの中から位置算出に用いる数式モデルを前記閾値に基づき選択する処理と、
を行うことを特徴とする位置推定方法。
【請求項5】
請求項4に記載の位置推定方法において、移動体の挙動を表す前記複数の数式モデルは、外部から観測可能な情報もしくは数式モデル処理部で算出した値から決められる値に基づく、移動体の挙動に対する適応範囲の制限を有し、
前記閾値を決定する処理では、各数式モデルにより計算される誤差の大きさに基づいて前記閾値を決定し、
前記数式モデルを選択する処理では、数式モデルの制限に応じて数式モデルを選択する、ことを特徴とする位置推定方法。
【請求項1】
移動体の姿勢情報および位置測位情報をセンサから取得し、取得した姿勢情報および位置測位情報に基づき移動体の位置を、予め備えられた移動体の挙動を表す数式モデルに基づき算出する数式モデル処理部を備えた位置算出装置において、
前記数式モデル処理部は複数の数式モデルを備え、
位置算出装置は、
位置の算出を行う前記数式モデル処理部における確率モデルを使ったフィルタリング処理により算出した誤差を用いて閾値の候補値を計算する閾値算出部と、
数式モデルを選択するための前記閾値を前記候補値に基づき決定する閾値決定部と、
数式モデル処理部で位置算出に用いる数式モデルを前記閾値決定部で決定された閾値に基づき選択する数式モデル選択部と、
を有することを特徴とする位置推定装置。
【請求項2】
請求項1に記載の位置推定装置において、移動体の挙動を表す前記複数の数式モデルは、外部から観測可能な情報もしくは数式モデル処理部で算出した値から決められる値に基づく、移動体の挙動に対する適応範囲の制限を有し、
前記閾値決定部は、各数式モデルにより計算される誤差の大きさに基づいて前記閾値を決定し、
前記数式モデル選択部は、数式モデルの制限に応じて数式モデルを選択する、ことを特徴とする位置推定装置。
【請求項3】
請求項2に記載の位置推定装置において、前記数式モデルの適応範囲の制限と前記閾値を記憶するデータベースを有することを特徴とする位置推定装置。
【請求項4】
移動体の姿勢情報および位置測位情報をセンサから取得し、取得した姿勢情報および位置測位情報に基づき移動体の位置を、予め備えられた移動体の挙動を表す数式モデルに基づき算出する位置推定方法において、
数式モデルに従って位置の算出を行う確率モデルを使ったフィルタリング処理により算出した誤差を用いて閾値の候補値を計算する処理と、
数式モデルを選択するための前記閾値を前記候補値に基づき決定する処理と、
予め用意されている複数の数式モデルの中から位置算出に用いる数式モデルを前記閾値に基づき選択する処理と、
を行うことを特徴とする位置推定方法。
【請求項5】
請求項4に記載の位置推定方法において、移動体の挙動を表す前記複数の数式モデルは、外部から観測可能な情報もしくは数式モデル処理部で算出した値から決められる値に基づく、移動体の挙動に対する適応範囲の制限を有し、
前記閾値を決定する処理では、各数式モデルにより計算される誤差の大きさに基づいて前記閾値を決定し、
前記数式モデルを選択する処理では、数式モデルの制限に応じて数式モデルを選択する、ことを特徴とする位置推定方法。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【公開番号】特開2011−174771(P2011−174771A)
【公開日】平成23年9月8日(2011.9.8)
【国際特許分類】
【出願番号】特願2010−38104(P2010−38104)
【出願日】平成22年2月24日(2010.2.24)
【出願人】(000001487)クラリオン株式会社 (1,722)
【Fターム(参考)】
【公開日】平成23年9月8日(2011.9.8)
【国際特許分類】
【出願日】平成22年2月24日(2010.2.24)
【出願人】(000001487)クラリオン株式会社 (1,722)
【Fターム(参考)】
[ Back to top ]