移動体位置推定システム、移動体位置推定方法、及び移動体位置推定プログラム
【課題】本発明は、移動体の位置情報を検出する際に位置検出遅れが生じても移動体に対して位置情報を高精度に取得可能な移動体位置推定装置を提供する。
【解決手段】移動体位置推定装置１は、移動体が備える位置検出手段からの更新位置信号と移動体が備える加速度などの物理量検出手段からの物理量検出信号に基づき移動体の位置を予測推定した推定信号を出力する移動体位置推定手段（位置推定器１８）、位置検出手段の位置検出遅れを補正するため更新位置信号と物理量検出信号の検出時刻に関する対応を図るように移動体位置推定手段の入出力動作、処理動作を制御する補正制御手段１０を含む。
【解決手段】移動体位置推定装置１は、移動体が備える位置検出手段からの更新位置信号と移動体が備える加速度などの物理量検出手段からの物理量検出信号に基づき移動体の位置を予測推定した推定信号を出力する移動体位置推定手段（位置推定器１８）、位置検出手段の位置検出遅れを補正するため更新位置信号と物理量検出信号の検出時刻に関する対応を図るように移動体位置推定手段の入出力動作、処理動作を制御する補正制御手段１０を含む。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体位置推定システム、移動体位置推定方法、及び移動体位置推定プログラムに関する。
【背景技術】
【０００２】
移動体位置推定装置は、宇宙機や飛翔体、航空機や車両など移動体の正確な位置情報を得るためのものであり、例えば図３に示すようなものが挙げられる。図３は、関連技術の移動体位置推定装置の一例を示すブロック図である。同図において、１０１はＧＰＳ受信機、１０２は加速度センサ、１０３は測位演算器（カルマンフィルタ）、１０４は位置更新信号、１０５は加速度検出信号である。
【０００３】
図３に示すように、この関連技術の移動体位置推定装置１００では、移動体に搭載したＧＰＳ受信機１０１と加速度センサ１０２でそれぞれ検出する位置更新信号１０４と加速度検出信号１０５とを用いて、測位演算器１０３でカルマンフィルタ処理を実施することにより移動体の位置推定値を生成する。
【０００４】
このとき、図３の測位演算器１０３では、カルマンフィルタを適用しているため、ＧＰＳ受信機１０１と加速度センサ１０２のデータ検出間隔が異なる場合でも、検出信号間のリアルタイム性と検出時刻精度とがある程度確保できていれば、移動体位置の推定はある程度可能となる。
【０００５】
そこで、測位演算器１０３の一例として例えば特許文献１などに示された関連技術の移動体位置推定装置では、複数のＧＰＳ衛星毎に発生する位置検出データの伝搬遅延時間値を位置推定値と共に状態量としてカルマンフィルタを構成した上で、さらにカルマンフィルタの推定状態量と検出状態量との差分値（状態量誤差）に対する可変の重み係数を導入し、この重み係数を状態量誤差の大きさに応じ変動させながらカルマンゲインと共に状態量誤差へ乗算することにより、状態ベクトルを構成する伝搬遅延時間と移動体位置との同時推定を実現している。
【０００６】
一方、例えば特許文献２に記載された技術では、ＧＰＳ受信機のクロック誤差ダイナミクスを３次関数でモデル化し、移動体の位置および速度をクロック誤差と共に推定することで、カルマンフィルタで生成する移動体の位置・速度の精度向上を実現している。
【０００７】
また、例えば特許文献３に記載された技術では、ＩＭＵ部で検出した移動体加速度からＩＮＳ航法位置／速度を生成するＩＮＳ航法部と、ＧＰＳ受信機で検出する移動体測位情報からＧＰＳ航法位置／速度を生成するＧＰＳ航法部とを組み合わせることでカルマンフィルタ処理系を構成し、高精度の位置情報取得を実現可能としている。
【０００８】
さらに、例えば特許文献４に記載された技術では、移動体の想定移動経路を位置検出システムへ適用することで、４個以上のＧＰＳ衛星捕捉が困難な場合でも、ＧＰＳ受信機の時計誤差と移動体位置導出を実現可能としている。
【特許文献１】特開２００５−２２１３７４号公報
【特許文献２】特開２００６−１８９３２０号公報
【特許文献３】特開２００６−２０８３９２号公報
【特許文献４】特開２００６−７１２８６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００９】
ところで、図３に示すような移動体位置推定装置では、ＧＰＳ受信機で検出される位置更新信号１０４を測位演算器１０３におけるカルマンフィルタ観測値（更新値）として適用するため、検出した位置更新信号１０４に検出時間遅れが含まれる場合には、加速度センサ１０２で検出した加速度検出信号１０５との間に検出時刻ズレが発生し、結果的に位置推定結果を劣化させてしまう、という課題がある。
【００１０】
さらに、特許文献１に記載された技術では、４個の衛星利用によるＧＰＳ測位に基づいているため衛星間の時刻誤差は復調可能となるが、測位位置を出力するカルマンフィルタの観測値と予測値との間で検出時刻ズレがあった場合には、観測値と予測値の時刻同期が取れていないことから逆に設定した重み係数で測位誤差を拡大してしまう、という課題がある。
【００１１】
一方、特許文献２に記載された技術では、ＧＰＳ受信機のクロック誤差（クロックバイアス、クロックドリフト、およびクロックバイアス加速度など）は補正可能となるものの、ＧＰＳ受信機で検出する測位信号自体に検出むだ時間が含まれている場合には、測位演算部のカルマンフィルタで推定する位置および速度の精度劣化は避けられない、という課題がある。
【００１２】
また、特許文献３に記載された技術では、カルマンフィルタをＩＭＵ加速度信号に基づくＩＮＳ航法部と、ＧＰＳ測位信号に基づくＧＰＳ航法部とを組み合わせることで構成しているため、ＧＰＳ測位信号に検出時間遅れが含まれる場合、ＩＮＳ航法部とＧＰＳ航法部とから生成する時系列データ間で検出時刻がそれぞれ異なることとなるため、生成する状態推定値および誤差推定値の精度がＧＰＳ測位信号の検出時間遅れが無い場合と比較し劣化する、という課題がある。
【００１３】
さらに、特許文献４に記載された技術では、ＧＰＳ衛星からの電波が途切れた場合にはジャイロや車速センサなど移動体に搭載されている慣性航法センサの検出信号により移動体の位置推定が可能な構成となっているが、ＧＰＳ測位信号に検出時間遅れが含まれる場合には、慣性航法センサ検出信号との間で検出時刻ズレが生じてしまい、結果的に位置精度が劣化してしまう、という課題がある。
【００１４】
本発明の目的は、上述の関連技術の課題を解決することにあり、宇宙機や航空機および車両など移動体の位置情報を検出する際にＧＰＳ受信機などの位置検出手段特有の位置検出遅れが生じたとしても、高速に移動する移動体に対して位置情報を高精度に取得可能ならしめる移動体位置推定装置、移動体位置推定方法、及び移動体位置推定プログラムを提供することにある。
【課題を解決するための手段】
【００１５】
上記目的を達成するため、本発明の移動体位置推定装置は、少なくとも、移動体が備えている位置検出手段からの更新位置信号と、前記移動体が備えている加速度等の物理量を検出する物理量検出手段からの物理量検出信号と、に基づいて前記移動体の位置を予測推定した推定信号を出力する移動体位置推定手段と、前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように前記移動体位置推定手段の入出力動作及び処理動作を制御する補正制御手段と、を備えたことを特徴としている。
【００１６】
本発明の移動体位置予測方法は、移動体の位置を推定する移動体位置推定装置を用いた移動体位置推定方法であって、前記移動体が備えている位置検出手段が更新位置信号を検出し、前記移動体が備えている加速度などの物理量を検出する物理量検出手段が物理量検出信号を検出し、前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように補正制御をし、この補正制御された前記物理量検出信号と前記更新位置信号とに基づいて前記補正が加味された前記移動体の位置を予測推定した推定信号を出力する、ことを特徴としている。
【００１７】
本発明の移動体位置推定プログラムは、移動体の位置を推定する移動体位置推定装置が備えたコンピュータに諸機能を実現させることが可能な移動体位置推定プログラムであって、少なくとも、移動体が備えている位置検出手段からの更新位置データと、前記移動体が備えている加速度などの物理量を検出する物理量検出手段からの物理量検出データと、に基づいて前記移動体の位置を予測推定した推定データを出力する移動体位置推定機能と、前記位置検出手段での位置検出遅れを補正するために、前記更新位置データと前記物理量検出データとの検出時刻に関する対応を図るように前記移動体位置推定機能の入出力動作及び処理動作を制御する補正制御機能と、を前記コンピュータに実現させることを特徴としている。
【発明の効果】
【００１８】
本発明によれば、位置検出手段において検出される更新位置信号に検出時間遅れが含まれる場合でも、前記位置検出手段での位置検出遅れを補正するために、更新位置信号と物理量検出信号との検出時刻に関する対応を図るように移動体位置推定手段の入出力動作及び処理動作を制御して推定処理を実施するため、位置検出手段の検出時間遅れを補正することが可能となり、その結果、高速に移動する移動体に対しても位置推定値の精度確保が実現できる、という関連技術にない優れた移動体位置推定システム、移動体位置推定方法、及び移動体位置推定プログラムを提供することができる。
【発明を実施するための最良の形態】
【００１９】
〔移動体位置推定装置の基本的構成〕
先ず、移動体位置推定装置の基本的構成について説明する。図１に示すように、移動体位置推定装置１は、飛翔体や宇宙機や航空機や車両など移動体の位置を生成するものであり、移動体に搭載し移動体の更新位置を検出し更新位置信号Ｓ１０を出力する位置検出手段の一例又は一部であるＧＰＳ受信機１１と、移動体に搭載し移動体の物理量例えば加速度を検出し物理量検出信号の一例である加速度センサ検出信号Ｓ１９を出力する物理量検出手段の一例又は一部である加速度センサ１２とを有する構成としている。
【００２０】
更に、移動体位置推定装置１は、少なくとも、移動体が備えている位置検出手段からの更新位置信号と、前記移動体が備えている加速度などの物理量を検出する物理量検出手段からの物理量検出信号と、に基づいて前記移動体の位置を予測推定した推定信号一例又は一部であるＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を出力する移動体位置推定手段の一例又は一部である位置推定器１８を有する。
【００２１】
又、移動体位置推定装置１は、位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように前記移動体位置推定手段の入出力動作及び処理動作を制御する補正制御手段１０を有する。
【００２２】
又更に、移動体位置推定装置１は、測位衛星ベース推定信号の一例又は一部であるＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１と、物理量（加速度）ベース予測値信号の一例又は一部である加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とを選択し、いずれか一方を前記推定信号として出力する選択手段の一例又は一部である位置推定値選定器１９をさらに有してよい。
【００２３】
ここで、移動体が、一例として無人航空機（飛翔体）であり、該飛翔体にＧＰＳ受信機１１と加速度センサ１２とを搭載する場合、この飛翔体の位置を地上局で推定することができる。その場合、移動体位置推定装置１は、補正制御手段１０、位置推定器１８、位置推定値選定器１９で構成できる。
【００２４】
このような移動体位置推定装置１によれば、位置検出手段において検出される更新位置信号に検出時間遅れが含まれる場合でも、補正制御手段が前記位置検出手段での位置検出遅れを補正するために、更新位置信号と物理量検出信号との検出時刻に関する対応を図るように移動体位置推定手段の入出力動作及び処理動作を制御して推定処理を実施するため、位置検出手段の検出時間遅れを補正することが可能となり、その結果、高速に移動する移動体に対しても位置推定値の精度確保が実現できる。
【００２５】
又、移動体位置推定装置１における補正制御手段１０は、物理量検出信号（加速度センサ検出信号Ｓ１９）と前回の推定信号の一例又は一部である位置推定値／誤差共分散値信号Ｓ１６とに基づいて前記移動体の位置を予測して物理量ベース予測値信号（加速度ベースの位置予測値／誤差共分散値信号Ｓ２０）を生成する予測値信号生成部の一例又は一部である位置伝播器１６を備える。
【００２６】
更に、補正制御手段１０は、前記更新位置信号の検出時刻を検出し時刻信号の一例又は一部であるＧＰＳデータ更新時刻信号Ｓ２２として生成する時刻検出部の一例又は一部であるＧＰＳデータ更新時刻検出器１３を備える。
【００２７】
又更に、補正制御手段１０は、前記物理量検出手段（加速度センサ１２）にて検出された前記物理量検出信号（加速度センサ検出信号Ｓ１９）を時系列データとして蓄積処理し、物理量時系列データ信号の一例又は一部である加速度時系列データ信号Ｓ１７を生成する第１の蓄積処理部の一例又は一部である加速度時系列データメモリ１４を備える。
【００２８】
更に又、補正制御手段１０は、前記予測値信号生成部（位置伝播器１６）にて生成された前記物理量ベース予測値信号（加速度ベースの位置予測値／誤差共分散値信号Ｓ２０）を時系列データとして蓄積処理し、予測値時系列データ信号の一例又は一部である位置予測値／誤差共分散値時系列データ信号Ｓ１８を生成する第２の蓄積処理部の一例又は一部である位置予測値／誤差共分散値時系列データメモリ１５を備える。
【００２９】
又、補正制御手段１０は、現在時刻までの生成済みの前記予測値時系列データ信号（位置予測値／誤差共分散値時系列データ信号Ｓ１８）と前記物理量時系列データ信号（加速度時系列データ信号Ｓ１７）と前記時刻信号（ＧＰＳデータ更新時刻信号Ｓ２２）とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号の一例又は一部である抽出予測値／抽出誤差共分散値信号Ｓ１３と、物理量データ抽出信号の一例又は一部である加速度データ抽出信号Ｓ１２とをそれぞれ生成し、これらを時刻信号の一例又は一部である更新時刻信号Ｓ１１とともに前記移動体位置推定手段に入力する時系列データ抽出部の一例又は一部である位置推定用時系列データ抽出器１７を備える。
【００３０】
このような構成の補正制御手段１０の場合、前記移動体位置推定手段は、前記更新位置信号Ｓ１０と前記時刻信号（更新時刻信号Ｓ１１）と前記物理量データ抽出信号（加速度データ抽出信号Ｓ１２）と前記予測値抽出信号（抽出予測値／抽出誤差共分散値信号Ｓ１３）とに基づいて前記推定信号（ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１）を出力する機能を備えてよい。
【００３１】
更に、前記移動体位置推定手段は、前記更新位置信号の前記検出時刻に応じ予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成する機能を備えてよい。
【００３２】
このような構成の補正制御手段１０を有する移動体位置推定装置１によると、ＧＰＳ受信機１１と加速度センサ１２とを組み合わせた汎用的なカルマンフィルタの一部である移動体位置推定処理機能（移動体位置推定手段：位置推定器１８）を用いて、位置伝播計算から生成する加速度センサ検出データに基づく現在時刻の移動体位置予測値に対し、ＧＰＳ受信機が発生する移動体位置更新値の検出時刻をＧＰＳデータ更新時刻検出器１３を用いて検出した上で、現在時刻までに検出しメモリ上に確保した移動体位置予測値と加速度センサ検出信号とに基づいて現在時刻での位置予測値をカルマンフィルタ処理により生成する機能を実現している。
その結果、移動体位置推定装置１では、ＧＰＳ受信機１１が検出する位置更新データに検出時間遅れが含まれる場合でも、過去の位置予測結果から検出時刻の対応するデータどうしを組み合わせてカルマンフィルタ処理を実行するため、ＧＰＳ受信機１１の時間遅れ補正が明確に実施可能となり、位置推定値の推定精度確保が達成できる。
【００３３】
このように、ＧＰＳ受信機１１で生じる更新位置信号Ｓ１０の検出時刻誤差を陽に補正しながら飛翔体の位置推定を実施しているため、急激な位置変動が連続的に生じた場合でも、高精度な飛翔体の位置推定値を継続的に発生することが可能となり、移動体の位置情報を、高速に移動する移動体に対しても高精度に取得可能となる。
【００３４】
以下、このような本発明の「移動体位置推定装置」のさらに具体的な実施の形態の一例について、図面を参照して説明する。
【００３５】
〔第１の実施の形態〕
（移動体位置推定装置の全体構成）
先ず、本実施の形態の移動体位置推定装置の具体的構成について、全体構成から説明し、続いて各部の詳細構成について説明することとする。図１は、本発明における第１実施の形態の移動体位置推定装置の全体の概略構成の一例を示すブロック図である。
【００３６】
図１に示すように、本実施の形態の移動体位置推定装置１は、位置検出手段としてのＧＰＳ受信機１１、物理量検出手段としての加速度センサ１２、時刻検出部としてのＧＰＳデータ更新時刻検出器１３、第１の蓄積処理部又は物理量時系列データ蓄積処理部としての加速度時系列データメモリ１４、第２の蓄積処理部又は物理量ベース予測値時系列データ蓄積処理部としての位置予測値／誤差共分散値時系列データメモリ１５、（物理量ベース）予測値信号生成部としての位置伝播器１６、時系列データ抽出部としての位置推定用時系列データ抽出器１７、移動体位置推定手段としての位置推定器１８、選択手段としての位置推定値選定器１９を含んで構成される。
【００３７】
ここにおいて、更新位置信号Ｓ１０、時刻信号としての更新時刻信号Ｓ１１、物理量データ抽出信号としての加速度データ抽出信号Ｓ１２、（物理量ベース）予測値抽出信号としての抽出予測値／抽出誤差共分散値信号Ｓ１３、第１の操作指令信号としての加速度メモリ操作指令信号Ｓ１４、第２の操作指令信号としての位置メモリ操作指令信号Ｓ１５、推定信号としての位置推定値／誤差共分散値信号Ｓ１６、物理量時系列データ信号としての加速度時系列データ信号Ｓ１７、（物理量ベース）予測値時系列データ信号としての位置予測値／誤差共分散値時系列データ信号Ｓ１８、物理量検出信号としての加速度センサ検出信号Ｓ１９、物理量（加速度）ベース予測値信号としての加速度ベースの位置予測値／誤差共分散値信号Ｓ２０、推定信号ないしは測位衛星ベース推定信号としてのＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１、時刻信号としてのＧＰＳデータ更新時刻信号Ｓ２２が各部で授受される。
【００３８】
又、ＧＰＳデータ更新時刻検出器１３、加速度時系列データメモリ１４、位置予測値／誤差共分散値時系列データメモリ１５、位置伝播器１６、位置推定用時系列データ抽出器１７により補正制御手段１０の一例又は一部を構成できる。
【００３９】
ＧＰＳ受信機１１は、移動体に搭載し前記移動体の更新位置信号Ｓ１０を検出する。加速度センサ１２は、移動体に搭載し前記移動体の加速度センサ検出信号を検出する。
ＧＰＳデータ更新時刻検出器１３は、更新位置信号Ｓ１０からＧＰＳデータ更新時刻信号Ｓ２２を生成する。
【００４０】
加速度時系列データメモリ１４は、加速度センサ検出信号Ｓ１９と加速度メモリ操作指令信号Ｓ１４とから加速度時系列データ信号Ｓ１７を生成する。
位置伝播器１６は、加速度センサ検出信号Ｓ１９と位置推定値／誤差共分散値信号Ｓ１６とから加速度ベースの位置予測値／誤差共分散値信号Ｓ２０を生成する。
【００４１】
位置予測値／抽出誤差共分散値時系列データメモリ１５は、加速度ベースの位置予測値／誤差共分散値信号Ｓ２０と位置メモリ操作指令信号Ｓ１５とから位置予測値／誤差共分散値時系列データ信号Ｓ１８を生成する。
位置推定用時系列データ抽出器１７は、ＧＰＳデータ更新時刻信号Ｓ２２と加速度時系列データ信号Ｓ１７と位置予測値／誤差共分散値時系列データ信号Ｓ１８とから更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３と加速度メモリ操作指令信号Ｓ１４と位置メモリ操作指令信号Ｓ１５とを生成する。
【００４２】
位置推定器１８は、更新位置信号Ｓ１０と更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３とからＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を生成する。
位置推定値選定器１９は、ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１と加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とから位置推定値／誤差共分散値信号Ｓ１６を生成する。
【００４３】
本実施の形態に係る移動体位置推定装置１は、図１に示すように、ＧＰＳ受信機１１で検出した更新位置信号Ｓ１０からＧＰＳデータ更新時刻信号Ｓ２２をＧＰＳデータ更新時刻検出器１３において生成すると共に、加速度センサ１２で検出した加速度センサ検出信号Ｓ１９と位置伝播器１６で加速度センサ検出信号Ｓ１９から位置推定値／誤差共分散値信号Ｓ１６を初期値として生成する加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とに対し、加速度時系列データメモリ１４では加速度時系列データ信号Ｓ１７、位置予測値／誤差共分散値時系列データメモリ１５では位置予測値／誤差共分散値時系列データ信号Ｓ１８をそれぞれ生成する。
【００４４】
次に、位置推定用時系列データ抽出器１７では、ＧＰＳデータ更新時刻信号Ｓ２２と加速度時系列データ信号Ｓ１７と位置予測値／誤差共分散値時系列データ信号Ｓ１８とに基づき、位置推定処理に必要となる更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３とをそれぞれ生成する。
【００４５】
さらに、位置推定器１８では、ＧＰＳ受信機１１の検出時間遅れ分を考慮した上で、更新位置信号Ｓ１０と更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３とから、更新位置信号Ｓ１０と加速度データ抽出信号Ｓ１２との検出時刻に関する対応を図りながらカルマンフィルタ処理を実施後、ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１として位置推定値とそれに付随する誤差共分散値信号を発生する。
【００４６】
最後に、位置推定値選定器１９では、加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１とから、現在時刻において最も適切と思われる位置予測値と誤差共分散値信号とを選択し、それを位置推定値／誤差共分散値信号Ｓ１６として発生する。
【００４７】
ここで、本実施の形態の補正制御手段１０は、生成済みの予測値時系列データ信号（データ列）から検出時刻の対応するデータを抽出する機能を備えてよい。
又、補正制御手段１０は、前記物理量検出信号と前回の推定信号とから生成される現在時刻の物理量ベース予測値信号に対し、前記更新位置信号の検出時刻である時刻信号を検出した上で、現在時刻までに検出し蓄積された物理量時系列データ信号と予測値時系列データ信号とに基づいて、検出時刻の対応する予測値抽出信号、物理量データ抽出信号を抽出してこれらを前記移動体位置推定手段に入力し、補正を加味した推定処理を前記移動体位置推定手段に実行させ、前記移動体位置推定手段により現在時刻での推定データを推定するように制御する機能を備えてよい。
【００４８】
（動作処理手順について）
（全体の概略動作）
次に、上述のような構成を有する移動体位置推定における全体の動作処理手順について説明する。
【００４９】
本実施の形態に係る動作処理手順は、移動体の位置を推定する移動体位置推定装置が行うものを対象とするものである。
【００５０】
本実施の形態に係る移動体位置推定における全体の動作処理手順は、基本的手順として、前記移動体が備えている位置検出手段が更新位置信号を検出し（位置検出ステップ）、前記移動体が備えている加速度などの物理量を検出する物理量検出手段が物理量検出信号を検出し（物理量検出ステップ）、前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように補正制御をし（補正制御ステップ）、この補正制御された前記物理量検出信号と前記更新位置信号とに基づいて前記補正が加味された前記移動体の位置を予測推定した推定信号を出力する（移動体位置推定ステップ）ことを行う。
【００５１】
また、この移動体位置推定における動作処理手順では、この内、前記補正制御をするに際しては（補正制御ステップでは）、前記物理量検出信号と前回の推定信号とに基づいて前記移動体の位置を予測して物理量ベース予測値信号を生成し（物理量ベースの予測値信号生成ステップ）、前記更新位置信号の検出時刻を検出し時刻信号として生成し（時刻検出ステップ）、前記物理量検出手段にて検出された前記物理量検出信号を時系列データとして蓄積処理し、物理量時系列データ信号を生成し（第１の蓄積処理ステップ又は物理量時系列データ蓄積処理ステップ）、前記予測値信号生成ステップにて生成された前記物理量ベース予測値信号を時系列データとして蓄積処理し、予測値時系列データ信号を生成し（第２の蓄積処理ステップ又は物理量ベース予測値時系列データ蓄積処理ステップ）、現在時刻までの生成済みの前記予測値時系列データ信号と前記物理量時系列データ信号と前記時刻信号とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号と物理量データ抽出信号とを生成し、これらを前記時刻信号とともに前記移動体位置推定手段に入力する（時系列データ抽出ステップ）ことを行う。
この場合、前記推定をするに際しては（移動体位置推定ステップでは）、前記更新位置信号と前記時刻信号と前記物理量データ抽出信号と前記予測値抽出信号とに基づいて予測推定を行い前記推定信号を出力することができる。
【００５２】
また、この移動体位置推定における動作処理手順では、前記推定をするに際しては（移動体位置推定ステップでは）、前記更新位置信号の前記検出時刻に応じ加速度ベース予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することができる。
【００５３】
さらに、この移動体位置推定における動作処理手順では、前記測位衛星ベース推定信号と加速度ベース予測値信号とを選択し、いずれか一方を前記推定信号として出力する（選択ステップ）をさらに行うことができる。
以下、これらの各ステップを詳述する。
【００５４】
（詳細動作）
ここで、図１に示す本実施の形態の動作について、具体例として移動体である無人航空機（飛翔体）にＧＰＳ受信機１１と加速度センサ１２とを搭載し、この飛翔体の位置を地上局で推定する場合を説明する。
【００５５】
（位置検出ステップ、物理量検出ステップ）
先ず、移動体に搭載されたＧＰＳ受信機１１が測位衛星からの信号に基づき更新位置を検出して更新位置信号を出力する。又、移動体に搭載された加速度センサ１２が加速度を検出して加速度センサ検出信号を出力する。
【００５６】
（物理量ベースの予測値信号生成ステップ）
一方、位置伝播器１６では、位置推定値選定器１９が前回出力した位置推定値／誤差共分散値信号Ｓ１６を構成するシステム状態量と誤差共分散行列をそれぞれｘ+k、Ｐ+kと設定した上で、加速度センサ１２で検出した飛翔体の加速度センサ検出信号Ｓ１９と組み合わせることで得られる（１）式の線形確率システム（状態空間モデル）
〔数１〕
ｘk+1 ＝ Φkｘk＋Ｇkｗk＋Ｄkｕk
ｙk ＝ Ｈkｘk＋ｖk （１）
（ｘkはシステム状態量、Φkは状態遷移行列、Ｇkは駆動ノイズ行列、Ｄkは駆動入力行列、Ｈkは観測行列、ｕkは加速度センサ検出信号Ｓ１９、ｙkは観測信号）に基づき、以下の計算式（２）（３）を用いて、システム状態量予測値ｘ-k+1と誤差共分散行列予測値Ｐ-k+1とで構成する加速度ベースの位置予測値／誤差共分散値信号Ｓ２０を生成する。
【００５７】
〔数２〕
ｘ-k+1 ＝ Φkｘ+k＋Ｄkｕk （２）
〔数３〕
Ｐ-k+1 ＝ ΦkＰ+kΦTk＋ＧkＱkＧTk （３）
ただし、式中、上付きの「＋」はカルマンフィルタ処理後の推定値、上付きの「−」はカルマンフィルタ処理前の伝播予測値、上付きの「Ｔ」は転置行列、下付きの「ｋ」は観測時系列の順序を示す。
また、システムノイズｗkと観測ノイズｖkは互いに独立な白色ノイズを仮定し、さらにＱkは、システムノイズｗkの共分散行列（カルマンフィルタ設計者が飛翔体ダイナミクス特性に応じ設定する設計パラメータ）を示す。
【００５８】
（第１の蓄積処理ステップ、第２の蓄積処理ステップ）
このとき、加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とでは、位置伝播器１６に対する入力であるｕk（加速度センサ検出信号Ｓ１９）とシステム状態量予測値ｘ-k+1および誤差共分散行列予測値Ｐ-k+1（加速度ベースの位置予測値／誤差共分散値信号Ｓ２０）とを、それぞれ時系列データとして格納する。
【００５９】
（時刻検出ステップ）
次に、ＧＰＳデータ更新時刻検出器１３では、ＧＰＳ受信機１１により検出した更新位置信号Ｓ１０からＧＰＳデータが更新された時刻を検出し、それをＧＰＳデータ更新時刻信号Ｓ２２として出力する。
【００６０】
（時系列データ検出ステップ）
また、位置推定用時系列データ抽出器１７では、加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とが生成する加速度時系列データ信号Ｓ１７と位置予測値／誤差共分散値時系列データ信号Ｓ１８とから、ＧＰＳデータ更新時刻信号Ｓ２２に基づき前回のＧＰＳ更新時刻以降の時系列データをそれぞれ加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３として抽出し、これらを更新時刻信号Ｓ１１と共に生成する。
【００６１】
このとき、位置推定用時系列データ抽出器１７は、加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とに対し、加速度メモリ操作指令信号Ｓ１４と位置メモリ操作指令信号Ｓ１５とをそれぞれ加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とへ出力することで、それぞれ加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３として抽出済みの加速度センサ検出信号Ｓ１９と加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とが占有していたメモリ領域を開放するためこれらデータ列をそれぞれのメモリ域から削除する。
【００６２】
（移動体位置推定ステップ）
一方、位置推定器１８では、位置推定用時系列データ抽出器１７により抽出した加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３と更新時刻信号Ｓ１１とに加え、ＧＰＳ受信機１１によって生成された更新位置信号Ｓ１０とを組み合わせカルマンフィルタ処理を実施することで、ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を生成する。
【００６３】
ここで、観測時系列の順序を下付きの「ｎ」を用いて今回の観測を「ｎ」、次回の観測を「ｎ＋１」と記述すると、まず、位置推定器１８では、既に位置伝播器１６において生成した加速度ベースの位置予測値／誤差共分散値信号Ｓ２０からＧＰＳデータ更新時刻検出器１３によって検出したＧＰＳデータの更新時刻信号Ｓ１１に基づき、ＧＰＳ受信機１１によって得られた更新位置信号Ｓ１０に基づくカルマンフィルタ更新処理を、位置予測値／誤差共分散値時系列データメモリ１５が発生する位置予測値／誤差共分散値時系列データ信号Ｓ１８から抽出した抽出予測値／抽出誤差共分散値信号Ｓ１３に対し（４）式、（５）式、（６）式を用いて実施する。
【００６４】
〔数４〕
Ｋn ＝ Ｐ-nＨn（ＨTnＰ-nＨn＋Ｒn）-1 （４）
〔数５〕
ｘ+n ＝ ｘ-n＋Ｋn（ｙn−ＨTnｘ-n） （５）
〔数６〕
Ｐ+n ＝ Ｐ-n−ＫnＨTnＰ-n （６）
ただし、Ｒnは、観測ノイズｖnの共分散行列（カルマンフィルタ設計者がＧＰＳ特性に応じ設定する設計パラメータ）であり、また上付きの「−１」は逆行列を示す。
【００６５】
次に、カルマンフィルタ予測処理は、加速度データ抽出信号Ｓ１２をｕnで表すと、（５）式、（６）式の結果から次に示す（７）式、（８）式の適用により実施できる。
〔数７〕
ｘ-n+1 ＝ Φnｘ+n＋Ｄnｕn （７）
〔数８〕
Ｐ-n+1 ＝ ΦnＰ+nΦTn＋ＧnＱnＧTn （８）
【００６６】
この結果、位置推定器１８では（４）式、（５）式、（６）式により実施する状態量の更新後に、（７）式、（８）式の適用により得られるシステム状態量予測値ｘ-n+1と誤差共分散行列予測値Ｐ-n+1とをＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１として生成する。
【００６７】
（選択ステップ）
最後に、位置推定値選定器１９では、位置推定器１８で生成されるＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１が更新された場合には最新のＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を、それ以外の場合には加速度ベースの位置予測値／誤差共分散値信号Ｓ２０を選択し、それを位置推定値／誤差共分散値信号Ｓ１６として出力する。
【００６８】
従って本実施の形態では、ＧＰＳ受信機１１によって得られた更新位置信号Ｓ１０をそのままカルマンフィルタ更新処理に適用するのではなく、更新位置信号Ｓ１０の検出時刻に応じ位置推定値／誤差共分散値信号Ｓ１６に対するカルマンフィルタ更新処理を実施後、加速度センサ検出信号Ｓ１９に基づくカルマンフィルタ予測処理を改めて実行することで、位置推定値／誤差共分散値信号Ｓ１６を生成する構成をとる。
【００６９】
ここで、以上の予測値信号生成ステップ、時刻検出ステップ、第１の蓄積処理ステップ、第２の蓄積処理ステップ、時系列データ抽出ステップからなるステップは、補正制御ステップないしは補正制御機能の一部又は一例とすることができる。
又、前記推定ステップでは、前記更新位置信号の前記検出時刻に応じ加速度ベース予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することができる。
【００７０】
以上のように本実施の形態によれば、位置検出手段において検出される更新位置信号に検出時間遅れが含まれる場合でも、補正制御手段が前記位置検出手段での位置検出遅れを補正するために、更新位置信号と物理量検出信号との検出時刻に関する対応を図るように移動体位置推定手段の入出力動作及び処理動作を制御して推定処理を実施するため、位置検出手段の検出時間遅れを補正することが可能となり、その結果、高速に移動する移動体に対しても位置推定値の精度確保が実現できる。
【００７１】
すなわち、ＧＰＳ受信機において検出される更新位置データに検出時間遅れが含まれる場合でも、生成済みの位置予測値から時刻対応するデータ列を抽出しカルマンフィルタ処理を実施するため、ＧＰＳ受信機の検出時間遅れを補正することが可能となり、高速に移動する移動体に対してでも、それらの位置情報を高精度に取得可能とする
【００７２】
より詳細には、補正制御手段１０を備えた移動体位置推定装置１は、ＧＰＳ受信機１１と加速度センサ１２とを組み合わせた汎用的なカルマンフィルタの一部である移動体位置推定処理機能（移動体位置推定手段：位置推定器１８）を用いて、位置伝播計算から生成する加速度センサ検出データに基づく現在時刻の移動体位置予測値に対し、ＧＰＳ受信機が発生する移動体位置更新値の検出時刻をＧＰＳデータ更新時刻検出器１３を用いて検出した上で、現在時刻までに検出しメモリ上に確保した移動体位置予測値と加速度センサ検出信号とに基づいて現在時刻での位置予測値をカルマンフィルタ処理により生成する機能を実現している。
その結果、移動体位置推定装置１では、過去の位置予測結果から検出時刻の対応するデータどうしを組み合わせてカルマンフィルタ処理を実行するため、ＧＰＳ受信機１１の時間遅れ補正が明確に実施可能となり、位置推定値の推定精度確保が達成できる。
【００７３】
特に、ＧＰＳ受信機１１で生じる更新位置信号Ｓ１０の検出時刻誤差を陽に補正しながら飛翔体の位置推定を実施しているため、急激な位置変動が連続的に生じた場合でも、ＧＰＳ受信機１での検出遅れ時間有無にかかわらず、高精度な飛翔体の位置推定値を継続的に発生することが可能となる。
【００７４】
また、ここで示した位置推定用時系列データ抽出器１７と位置推定器１８との組み合わせ構成による位置推定処理は、ＧＰＳ受信機１１における検出むだ時間（検出時刻誤差）を補正するために実現した独自技術であり、特許文献１など関連技術の発明の組み合わせにより容易に類推できるものではない。
【００７５】
[その他の各種変形例]
また、本発明にかかる装置及び方法は、そのいくつかの特定の実施の形態に従って説明してきたが、本発明の主旨および範囲から逸脱することなく本発明の本文に記述した実施の形態に対して種々の変形が可能である。
【００７６】
例えば、図１に示すブロック図における一部の各ブロックは、コンピュータにより実行可能なプログラムにより機能化された状態を示すソフトウエアモジュール構成であってもよい。
【００７７】
その場合、物理的構成は例えば一又は複数のＣＰＵ（或いは一又は複数のＣＰＵと一又は複数のメモリ）等ではあるが、各部（器・回路・手段）によるソフトウエア構成は、プログラムの制御によってＣＰＵが発揮する複数の機能を、それぞれ複数の部（器・回路・手段）による構成要素として表現したものとすることができる。
【００７８】
以上に示した各部（器・回路・手段）は、プログラムにより機能化されたコンピュータをプログラムの機能と共に実現し得るように構成してもよい。この場合、各部間で授受される各種「信号」を「データ」としてもよい。
【００７９】
また、固有のハードウエアにより恒久的に機能化された複数の電子回路ブロックからなる装置で構成してもよい。したがって、これらの機能ブロックがハードウェアのみ、ソフトウェアのみ、またはそれらの組合せによっていろいろな形で実現でき、いずれかに限定されるものではない。
【００８０】
ここで、プログラム化してコンピュータに実行させる場合のコンピュータのハードウエア構成としては、種々の情報等を表示するための表示部（スクリーン）、この表示部の表示画面上（の各種入力欄等）にデータを操作入力するための操作入力部（例えばキーボード・マウス・各種ボタン・表示操作部＜画面上のボタン＞・タッチパネル等）、各種信号・データを送受信するための送受信部ないしは通信部（モデムなど）、各種プログラム・各種データを記憶しておく記憶部（例えばメモリ、ハードディスク等）、これらの制御を司る制御部（例えばＣＰＵ、ＭＰＵ、ＤＳＰ等）などを有することができる。
【００８１】
この場合のコンピュータは、デスクトップ、ラップトップコンピュータ、サーバコンピュータ、その他無線・有線通信機能を有する情報機器、またはこれに類するコンピュータなどいかなるコンピュータでもよく、移動式・固定式を問わない。
【００８２】
プログラム化して実行する場合、例えば図２に示すフローチャートのように実行することができる。以下の説明では、「信号」を「データ」として扱い、「器」を「部」として扱うことができるものとする。
すなわち、まず、コンピュータが、前記移動体が備えた位置検出手段が更新位置データを検出するように制御し（ステップＳＴ１１０：位置検出ステップないし位置検出機能）、前記移動体が備えている加速度などの物理量を検出する物理量検出手段（加速度検出手段）が物理量検出データ（加速度検出データ）を検出するように制御する処理（ステップＳＴ１１０：物理量検出ステップないし物理量検出機能）を行う。
この検出は、プログラム部分として含まなくてもよい。
【００８３】
この検出後、補正制御手段として機能する移動体位置推定装置が備えたコンピュータが、前記位置検出手段での位置検出遅れを補正するために、前記更新位置データと前記物理量検出データとの検出時刻に関する対応を図るように補正制御をする（ステップＳＴ１２０：補正制御ステップないし補正検出機能）。
より具体的には、この補正制御ステップでは、前記位置と前記加速度との検出時刻に関する対応を図るように時刻データと加速度データ抽出データと加速度ベース予測値抽出データとを位置推定部に入力する。
【００８４】
続いて、移動体位置推定手段として機能する移動体位置推定装置が備えたコンピュータが、この補正制御された前記物理量検出データと前記更新位置データとに基づいて前記補正が加味された前記移動体の位置を予測推定した推定データを出力する（ステップＳＴ１３０：移動体位置推定ステップないしは移動体位置推定機能）ことを行う。
【００８５】
さらに、選択手段として機能する移動体位置推定装置が備えたコンピュータが、測位衛星ベース推定データと加速度ベース予測値データとを選択し、いずれか一方を前記推定データとして出力する（ステップＳＴ１４０：選択ステップないしは選択機能）。
【００８６】
ここにおいて、補正制御ステップＳＴ１２０では、補正制御手段として機能する移動体位置推定装置が備えたコンピュータが、前記物理量検出データと前回の推定データとに基づいて前記移動体の位置を予測して物理量ベース予測値データを生成する（物理量ベースの予測値データ生成ステップないしは機能）。
また、前記コンピュータが、前記更新位置データの検出時刻を検出し時刻データとして生成する（時刻検出ステップないしは機能）。
さらに、前記コンピュータが、前記物理量検出手段にて検出された前記物理量検出データを時系列データとして蓄積処理し、物理量時系列データを生成する（第１の蓄積処理機能）。
またさらに、前記コンピュータが、前記物理量ベースの予測値データ生成部にて生成された前記物理量ベース予測値データを時系列データとして蓄積処理し、物理量ベースの予測値時系列データを生成する（第２の蓄積処理ステップないしは機能）。
また、前記コンピュータが、現在時刻までの生成済みの前記物理量ベースの予測値時系列データと前記物理量時系列データデータと前記時刻データとに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して物理量ベースの予測値抽出データと物理量データ抽出データとを生成し、これらを前記時刻データとともに前記移動体位置推定機能に入力する（時系列データ抽出ステップないしは機能）。
この場合、前記移動体位置推定ステップＳＴ１３０では、前記コンピュータが、前記更新位置データと前記時刻データと前記物理量抽出データと前記予測値抽出データとに基づいて前記推定データを出力することができる。
【００８７】
さらに、前記移動体位置推定ステップＳＴ１３０では、前記コンピュータが、前記更新位置データの前記検出時刻に応じ加速度ベースの予測値抽出データに対するカルマンフィルタ更新処理を実施後、前記加速度抽出データに基づくカルマンフィルタ予測処理を実行し、前記推定データとして測位衛星ベース推定データを生成することができる。
【００８８】
その他の構成およびその他のステップないしは機能並びにその作用効果については、前述した実施の形態の場合と同一となっている。以上説明した方法は、コンピュータがプログラムを記録媒体から読み込んで実行することによっても実現することが出来る。すなわち、上述のプログラムを、情報記録媒体に記録した構成であってもよい。
【００８９】
更に、位置検出手段としては、ＧＰＳの他ＧＬＯＮＡＳＳなど他の測位システムから位置情報を取得する場合であってもよい。
また、ＧＰＳ受信機は、ＧＰＳ（汎地球測位システム）により自車の現在位置を例えば緯度、経度、高度で検出すると共に、この自車の進行方向や走行速度等を検出するものであり、例えば４個のＧＰＳ人工衛星から地上に無線送信されるＧＰＳ電波をＧＰＳアンテナを介してそれぞれ受信して、これらの各電波の到達時間差から三角測量の原理で自車の現在位置を３次元で測位することができる。２次元測位の場合は、３個のＧＰＳ人工衛星からのＧＰＳ電波を受信することができる。
【００９０】
さらにまた、上記構成部材の数、位置、形状等は上記実施の形態に限定されず、本発明を実施する上で好適な数、位置、形状等にすることができる。すなわち、上記実施の形態では、ＧＰＳ受信機が１個、加速度センサが１個、の場合を示したが、本発明は、これらの個数を制限するものではない。
【００９１】
ところで、このような装置は、単独で存在する場合もあるし、ある機器（例えば電子機器など）に組み込まれた状態で利用されることもあるなど、発明の思想としてはこれに限らず、各種の態様を含むものである。
【００９２】
また、前記実施の形態において、各構成の「〜器」が同一筐体中にあるか否かは問わない。このため、複数の機器から構成されるものに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
【００９３】
また、発明の範囲は、図示例に限定されないものとする。さらに、上記各実施の形態には種々の段階が含まれており、開示される複数の構成要件における適宜な組み合わせにより種々の発明が抽出され得る。つまり、上述の各実施の形態同士、あるいはそれらのいずれかと各変形例のいずれかとの組み合わせによる例をも含む。
また、様々な移動体に対する位置情報の推定手法として容易に利用することができるものである。
【産業上の利用可能性】
【００９４】
本発明は、移動体位置推定装置ないしはそのシステム全般に利用できる。
【図面の簡単な説明】
【００９５】
【図１】本発明の第１の実施の形態による移動体位置推定装置の全体構成の一例を示すブロック図である。
【図２】本発明の他の実施の形態による移動体位置推定装置における処理手順の一例を示すフローチャートである。
【図３】関連技術の移動体位置推定装置の全体構成の一例を示すブロック図である。
【符号の説明】
【００９６】
１ 移動体位置推定装置
１０ 補正制御手段
１１ ＧＰＳ受信機（位置検出手段）
１２ 加速度センサ（物理量検出手段）
１３ ＧＰＳデータ更新時刻検出器（時刻検出部）
１４ 加速度時系列データメモリ（第１の蓄積処理部）
１５ 位置予測値／誤差共分散値時系列データメモリ（第２の蓄積処理部）
１６ 位置伝播器（予測値信号生成部）
１７ 位置推定用時系列データ抽出器（時系列データ抽出部）
１８ 位置推定器（移動体位置推定手段）
１９ 位置推定値選定器（選択手段）
Ｓ１０ 更新位置信号
Ｓ１１ 更新時刻信号（時刻信号）
Ｓ１２ 加速度データ抽出信号（物理量データ抽出信号）
Ｓ１３ 抽出予測値／抽出誤差共分散値信号（予測値抽出信号）
Ｓ１４ 加速度メモリ操作指令信号（第１操作指令信号）
Ｓ１５ 位置メモリ操作指令信号（第２操作指令信号）
Ｓ１６ 位置推定値／誤差共分散値信号（推定信号）
Ｓ１７ 加速度時系列データ信号（物理量時系列データ信号）
Ｓ１８ 位置予測値／誤差共分散値時系列データ信号（予測値抽出信号）
Ｓ１９ 加速度センサ検出信号（物理量検出信号）
Ｓ２０ 加速度ベースの位置予測値／誤差共分散値信号（物理量ベース予測値信号、加速度ベース予測値信号）
Ｓ２１ ＧＰＳベースの位置予測値／誤差共分散値信号（推定信号、測位衛星ベース推定信号）
Ｓ２２ ＧＰＳデータ更新時刻信号（時刻信号）
１００ 移動体位置推定装置
１０１ ＧＰＳ受信機
１０２ 加速度センサ
１０３ 測位演算器（カルマンフィルタ）
１０４ 位置更新信号
１０５ 加速度検出信号
【技術分野】
【０００１】
本発明は、移動体位置推定システム、移動体位置推定方法、及び移動体位置推定プログラムに関する。
【背景技術】
【０００２】
移動体位置推定装置は、宇宙機や飛翔体、航空機や車両など移動体の正確な位置情報を得るためのものであり、例えば図３に示すようなものが挙げられる。図３は、関連技術の移動体位置推定装置の一例を示すブロック図である。同図において、１０１はＧＰＳ受信機、１０２は加速度センサ、１０３は測位演算器（カルマンフィルタ）、１０４は位置更新信号、１０５は加速度検出信号である。
【０００３】
図３に示すように、この関連技術の移動体位置推定装置１００では、移動体に搭載したＧＰＳ受信機１０１と加速度センサ１０２でそれぞれ検出する位置更新信号１０４と加速度検出信号１０５とを用いて、測位演算器１０３でカルマンフィルタ処理を実施することにより移動体の位置推定値を生成する。
【０００４】
このとき、図３の測位演算器１０３では、カルマンフィルタを適用しているため、ＧＰＳ受信機１０１と加速度センサ１０２のデータ検出間隔が異なる場合でも、検出信号間のリアルタイム性と検出時刻精度とがある程度確保できていれば、移動体位置の推定はある程度可能となる。
【０００５】
そこで、測位演算器１０３の一例として例えば特許文献１などに示された関連技術の移動体位置推定装置では、複数のＧＰＳ衛星毎に発生する位置検出データの伝搬遅延時間値を位置推定値と共に状態量としてカルマンフィルタを構成した上で、さらにカルマンフィルタの推定状態量と検出状態量との差分値（状態量誤差）に対する可変の重み係数を導入し、この重み係数を状態量誤差の大きさに応じ変動させながらカルマンゲインと共に状態量誤差へ乗算することにより、状態ベクトルを構成する伝搬遅延時間と移動体位置との同時推定を実現している。
【０００６】
一方、例えば特許文献２に記載された技術では、ＧＰＳ受信機のクロック誤差ダイナミクスを３次関数でモデル化し、移動体の位置および速度をクロック誤差と共に推定することで、カルマンフィルタで生成する移動体の位置・速度の精度向上を実現している。
【０００７】
また、例えば特許文献３に記載された技術では、ＩＭＵ部で検出した移動体加速度からＩＮＳ航法位置／速度を生成するＩＮＳ航法部と、ＧＰＳ受信機で検出する移動体測位情報からＧＰＳ航法位置／速度を生成するＧＰＳ航法部とを組み合わせることでカルマンフィルタ処理系を構成し、高精度の位置情報取得を実現可能としている。
【０００８】
さらに、例えば特許文献４に記載された技術では、移動体の想定移動経路を位置検出システムへ適用することで、４個以上のＧＰＳ衛星捕捉が困難な場合でも、ＧＰＳ受信機の時計誤差と移動体位置導出を実現可能としている。
【特許文献１】特開２００５−２２１３７４号公報
【特許文献２】特開２００６−１８９３２０号公報
【特許文献３】特開２００６−２０８３９２号公報
【特許文献４】特開２００６−７１２８６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００９】
ところで、図３に示すような移動体位置推定装置では、ＧＰＳ受信機で検出される位置更新信号１０４を測位演算器１０３におけるカルマンフィルタ観測値（更新値）として適用するため、検出した位置更新信号１０４に検出時間遅れが含まれる場合には、加速度センサ１０２で検出した加速度検出信号１０５との間に検出時刻ズレが発生し、結果的に位置推定結果を劣化させてしまう、という課題がある。
【００１０】
さらに、特許文献１に記載された技術では、４個の衛星利用によるＧＰＳ測位に基づいているため衛星間の時刻誤差は復調可能となるが、測位位置を出力するカルマンフィルタの観測値と予測値との間で検出時刻ズレがあった場合には、観測値と予測値の時刻同期が取れていないことから逆に設定した重み係数で測位誤差を拡大してしまう、という課題がある。
【００１１】
一方、特許文献２に記載された技術では、ＧＰＳ受信機のクロック誤差（クロックバイアス、クロックドリフト、およびクロックバイアス加速度など）は補正可能となるものの、ＧＰＳ受信機で検出する測位信号自体に検出むだ時間が含まれている場合には、測位演算部のカルマンフィルタで推定する位置および速度の精度劣化は避けられない、という課題がある。
【００１２】
また、特許文献３に記載された技術では、カルマンフィルタをＩＭＵ加速度信号に基づくＩＮＳ航法部と、ＧＰＳ測位信号に基づくＧＰＳ航法部とを組み合わせることで構成しているため、ＧＰＳ測位信号に検出時間遅れが含まれる場合、ＩＮＳ航法部とＧＰＳ航法部とから生成する時系列データ間で検出時刻がそれぞれ異なることとなるため、生成する状態推定値および誤差推定値の精度がＧＰＳ測位信号の検出時間遅れが無い場合と比較し劣化する、という課題がある。
【００１３】
さらに、特許文献４に記載された技術では、ＧＰＳ衛星からの電波が途切れた場合にはジャイロや車速センサなど移動体に搭載されている慣性航法センサの検出信号により移動体の位置推定が可能な構成となっているが、ＧＰＳ測位信号に検出時間遅れが含まれる場合には、慣性航法センサ検出信号との間で検出時刻ズレが生じてしまい、結果的に位置精度が劣化してしまう、という課題がある。
【００１４】
本発明の目的は、上述の関連技術の課題を解決することにあり、宇宙機や航空機および車両など移動体の位置情報を検出する際にＧＰＳ受信機などの位置検出手段特有の位置検出遅れが生じたとしても、高速に移動する移動体に対して位置情報を高精度に取得可能ならしめる移動体位置推定装置、移動体位置推定方法、及び移動体位置推定プログラムを提供することにある。
【課題を解決するための手段】
【００１５】
上記目的を達成するため、本発明の移動体位置推定装置は、少なくとも、移動体が備えている位置検出手段からの更新位置信号と、前記移動体が備えている加速度等の物理量を検出する物理量検出手段からの物理量検出信号と、に基づいて前記移動体の位置を予測推定した推定信号を出力する移動体位置推定手段と、前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように前記移動体位置推定手段の入出力動作及び処理動作を制御する補正制御手段と、を備えたことを特徴としている。
【００１６】
本発明の移動体位置予測方法は、移動体の位置を推定する移動体位置推定装置を用いた移動体位置推定方法であって、前記移動体が備えている位置検出手段が更新位置信号を検出し、前記移動体が備えている加速度などの物理量を検出する物理量検出手段が物理量検出信号を検出し、前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように補正制御をし、この補正制御された前記物理量検出信号と前記更新位置信号とに基づいて前記補正が加味された前記移動体の位置を予測推定した推定信号を出力する、ことを特徴としている。
【００１７】
本発明の移動体位置推定プログラムは、移動体の位置を推定する移動体位置推定装置が備えたコンピュータに諸機能を実現させることが可能な移動体位置推定プログラムであって、少なくとも、移動体が備えている位置検出手段からの更新位置データと、前記移動体が備えている加速度などの物理量を検出する物理量検出手段からの物理量検出データと、に基づいて前記移動体の位置を予測推定した推定データを出力する移動体位置推定機能と、前記位置検出手段での位置検出遅れを補正するために、前記更新位置データと前記物理量検出データとの検出時刻に関する対応を図るように前記移動体位置推定機能の入出力動作及び処理動作を制御する補正制御機能と、を前記コンピュータに実現させることを特徴としている。
【発明の効果】
【００１８】
本発明によれば、位置検出手段において検出される更新位置信号に検出時間遅れが含まれる場合でも、前記位置検出手段での位置検出遅れを補正するために、更新位置信号と物理量検出信号との検出時刻に関する対応を図るように移動体位置推定手段の入出力動作及び処理動作を制御して推定処理を実施するため、位置検出手段の検出時間遅れを補正することが可能となり、その結果、高速に移動する移動体に対しても位置推定値の精度確保が実現できる、という関連技術にない優れた移動体位置推定システム、移動体位置推定方法、及び移動体位置推定プログラムを提供することができる。
【発明を実施するための最良の形態】
【００１９】
〔移動体位置推定装置の基本的構成〕
先ず、移動体位置推定装置の基本的構成について説明する。図１に示すように、移動体位置推定装置１は、飛翔体や宇宙機や航空機や車両など移動体の位置を生成するものであり、移動体に搭載し移動体の更新位置を検出し更新位置信号Ｓ１０を出力する位置検出手段の一例又は一部であるＧＰＳ受信機１１と、移動体に搭載し移動体の物理量例えば加速度を検出し物理量検出信号の一例である加速度センサ検出信号Ｓ１９を出力する物理量検出手段の一例又は一部である加速度センサ１２とを有する構成としている。
【００２０】
更に、移動体位置推定装置１は、少なくとも、移動体が備えている位置検出手段からの更新位置信号と、前記移動体が備えている加速度などの物理量を検出する物理量検出手段からの物理量検出信号と、に基づいて前記移動体の位置を予測推定した推定信号一例又は一部であるＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を出力する移動体位置推定手段の一例又は一部である位置推定器１８を有する。
【００２１】
又、移動体位置推定装置１は、位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように前記移動体位置推定手段の入出力動作及び処理動作を制御する補正制御手段１０を有する。
【００２２】
又更に、移動体位置推定装置１は、測位衛星ベース推定信号の一例又は一部であるＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１と、物理量（加速度）ベース予測値信号の一例又は一部である加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とを選択し、いずれか一方を前記推定信号として出力する選択手段の一例又は一部である位置推定値選定器１９をさらに有してよい。
【００２３】
ここで、移動体が、一例として無人航空機（飛翔体）であり、該飛翔体にＧＰＳ受信機１１と加速度センサ１２とを搭載する場合、この飛翔体の位置を地上局で推定することができる。その場合、移動体位置推定装置１は、補正制御手段１０、位置推定器１８、位置推定値選定器１９で構成できる。
【００２４】
このような移動体位置推定装置１によれば、位置検出手段において検出される更新位置信号に検出時間遅れが含まれる場合でも、補正制御手段が前記位置検出手段での位置検出遅れを補正するために、更新位置信号と物理量検出信号との検出時刻に関する対応を図るように移動体位置推定手段の入出力動作及び処理動作を制御して推定処理を実施するため、位置検出手段の検出時間遅れを補正することが可能となり、その結果、高速に移動する移動体に対しても位置推定値の精度確保が実現できる。
【００２５】
又、移動体位置推定装置１における補正制御手段１０は、物理量検出信号（加速度センサ検出信号Ｓ１９）と前回の推定信号の一例又は一部である位置推定値／誤差共分散値信号Ｓ１６とに基づいて前記移動体の位置を予測して物理量ベース予測値信号（加速度ベースの位置予測値／誤差共分散値信号Ｓ２０）を生成する予測値信号生成部の一例又は一部である位置伝播器１６を備える。
【００２６】
更に、補正制御手段１０は、前記更新位置信号の検出時刻を検出し時刻信号の一例又は一部であるＧＰＳデータ更新時刻信号Ｓ２２として生成する時刻検出部の一例又は一部であるＧＰＳデータ更新時刻検出器１３を備える。
【００２７】
又更に、補正制御手段１０は、前記物理量検出手段（加速度センサ１２）にて検出された前記物理量検出信号（加速度センサ検出信号Ｓ１９）を時系列データとして蓄積処理し、物理量時系列データ信号の一例又は一部である加速度時系列データ信号Ｓ１７を生成する第１の蓄積処理部の一例又は一部である加速度時系列データメモリ１４を備える。
【００２８】
更に又、補正制御手段１０は、前記予測値信号生成部（位置伝播器１６）にて生成された前記物理量ベース予測値信号（加速度ベースの位置予測値／誤差共分散値信号Ｓ２０）を時系列データとして蓄積処理し、予測値時系列データ信号の一例又は一部である位置予測値／誤差共分散値時系列データ信号Ｓ１８を生成する第２の蓄積処理部の一例又は一部である位置予測値／誤差共分散値時系列データメモリ１５を備える。
【００２９】
又、補正制御手段１０は、現在時刻までの生成済みの前記予測値時系列データ信号（位置予測値／誤差共分散値時系列データ信号Ｓ１８）と前記物理量時系列データ信号（加速度時系列データ信号Ｓ１７）と前記時刻信号（ＧＰＳデータ更新時刻信号Ｓ２２）とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号の一例又は一部である抽出予測値／抽出誤差共分散値信号Ｓ１３と、物理量データ抽出信号の一例又は一部である加速度データ抽出信号Ｓ１２とをそれぞれ生成し、これらを時刻信号の一例又は一部である更新時刻信号Ｓ１１とともに前記移動体位置推定手段に入力する時系列データ抽出部の一例又は一部である位置推定用時系列データ抽出器１７を備える。
【００３０】
このような構成の補正制御手段１０の場合、前記移動体位置推定手段は、前記更新位置信号Ｓ１０と前記時刻信号（更新時刻信号Ｓ１１）と前記物理量データ抽出信号（加速度データ抽出信号Ｓ１２）と前記予測値抽出信号（抽出予測値／抽出誤差共分散値信号Ｓ１３）とに基づいて前記推定信号（ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１）を出力する機能を備えてよい。
【００３１】
更に、前記移動体位置推定手段は、前記更新位置信号の前記検出時刻に応じ予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成する機能を備えてよい。
【００３２】
このような構成の補正制御手段１０を有する移動体位置推定装置１によると、ＧＰＳ受信機１１と加速度センサ１２とを組み合わせた汎用的なカルマンフィルタの一部である移動体位置推定処理機能（移動体位置推定手段：位置推定器１８）を用いて、位置伝播計算から生成する加速度センサ検出データに基づく現在時刻の移動体位置予測値に対し、ＧＰＳ受信機が発生する移動体位置更新値の検出時刻をＧＰＳデータ更新時刻検出器１３を用いて検出した上で、現在時刻までに検出しメモリ上に確保した移動体位置予測値と加速度センサ検出信号とに基づいて現在時刻での位置予測値をカルマンフィルタ処理により生成する機能を実現している。
その結果、移動体位置推定装置１では、ＧＰＳ受信機１１が検出する位置更新データに検出時間遅れが含まれる場合でも、過去の位置予測結果から検出時刻の対応するデータどうしを組み合わせてカルマンフィルタ処理を実行するため、ＧＰＳ受信機１１の時間遅れ補正が明確に実施可能となり、位置推定値の推定精度確保が達成できる。
【００３３】
このように、ＧＰＳ受信機１１で生じる更新位置信号Ｓ１０の検出時刻誤差を陽に補正しながら飛翔体の位置推定を実施しているため、急激な位置変動が連続的に生じた場合でも、高精度な飛翔体の位置推定値を継続的に発生することが可能となり、移動体の位置情報を、高速に移動する移動体に対しても高精度に取得可能となる。
【００３４】
以下、このような本発明の「移動体位置推定装置」のさらに具体的な実施の形態の一例について、図面を参照して説明する。
【００３５】
〔第１の実施の形態〕
（移動体位置推定装置の全体構成）
先ず、本実施の形態の移動体位置推定装置の具体的構成について、全体構成から説明し、続いて各部の詳細構成について説明することとする。図１は、本発明における第１実施の形態の移動体位置推定装置の全体の概略構成の一例を示すブロック図である。
【００３６】
図１に示すように、本実施の形態の移動体位置推定装置１は、位置検出手段としてのＧＰＳ受信機１１、物理量検出手段としての加速度センサ１２、時刻検出部としてのＧＰＳデータ更新時刻検出器１３、第１の蓄積処理部又は物理量時系列データ蓄積処理部としての加速度時系列データメモリ１４、第２の蓄積処理部又は物理量ベース予測値時系列データ蓄積処理部としての位置予測値／誤差共分散値時系列データメモリ１５、（物理量ベース）予測値信号生成部としての位置伝播器１６、時系列データ抽出部としての位置推定用時系列データ抽出器１７、移動体位置推定手段としての位置推定器１８、選択手段としての位置推定値選定器１９を含んで構成される。
【００３７】
ここにおいて、更新位置信号Ｓ１０、時刻信号としての更新時刻信号Ｓ１１、物理量データ抽出信号としての加速度データ抽出信号Ｓ１２、（物理量ベース）予測値抽出信号としての抽出予測値／抽出誤差共分散値信号Ｓ１３、第１の操作指令信号としての加速度メモリ操作指令信号Ｓ１４、第２の操作指令信号としての位置メモリ操作指令信号Ｓ１５、推定信号としての位置推定値／誤差共分散値信号Ｓ１６、物理量時系列データ信号としての加速度時系列データ信号Ｓ１７、（物理量ベース）予測値時系列データ信号としての位置予測値／誤差共分散値時系列データ信号Ｓ１８、物理量検出信号としての加速度センサ検出信号Ｓ１９、物理量（加速度）ベース予測値信号としての加速度ベースの位置予測値／誤差共分散値信号Ｓ２０、推定信号ないしは測位衛星ベース推定信号としてのＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１、時刻信号としてのＧＰＳデータ更新時刻信号Ｓ２２が各部で授受される。
【００３８】
又、ＧＰＳデータ更新時刻検出器１３、加速度時系列データメモリ１４、位置予測値／誤差共分散値時系列データメモリ１５、位置伝播器１６、位置推定用時系列データ抽出器１７により補正制御手段１０の一例又は一部を構成できる。
【００３９】
ＧＰＳ受信機１１は、移動体に搭載し前記移動体の更新位置信号Ｓ１０を検出する。加速度センサ１２は、移動体に搭載し前記移動体の加速度センサ検出信号を検出する。
ＧＰＳデータ更新時刻検出器１３は、更新位置信号Ｓ１０からＧＰＳデータ更新時刻信号Ｓ２２を生成する。
【００４０】
加速度時系列データメモリ１４は、加速度センサ検出信号Ｓ１９と加速度メモリ操作指令信号Ｓ１４とから加速度時系列データ信号Ｓ１７を生成する。
位置伝播器１６は、加速度センサ検出信号Ｓ１９と位置推定値／誤差共分散値信号Ｓ１６とから加速度ベースの位置予測値／誤差共分散値信号Ｓ２０を生成する。
【００４１】
位置予測値／抽出誤差共分散値時系列データメモリ１５は、加速度ベースの位置予測値／誤差共分散値信号Ｓ２０と位置メモリ操作指令信号Ｓ１５とから位置予測値／誤差共分散値時系列データ信号Ｓ１８を生成する。
位置推定用時系列データ抽出器１７は、ＧＰＳデータ更新時刻信号Ｓ２２と加速度時系列データ信号Ｓ１７と位置予測値／誤差共分散値時系列データ信号Ｓ１８とから更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３と加速度メモリ操作指令信号Ｓ１４と位置メモリ操作指令信号Ｓ１５とを生成する。
【００４２】
位置推定器１８は、更新位置信号Ｓ１０と更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３とからＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を生成する。
位置推定値選定器１９は、ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１と加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とから位置推定値／誤差共分散値信号Ｓ１６を生成する。
【００４３】
本実施の形態に係る移動体位置推定装置１は、図１に示すように、ＧＰＳ受信機１１で検出した更新位置信号Ｓ１０からＧＰＳデータ更新時刻信号Ｓ２２をＧＰＳデータ更新時刻検出器１３において生成すると共に、加速度センサ１２で検出した加速度センサ検出信号Ｓ１９と位置伝播器１６で加速度センサ検出信号Ｓ１９から位置推定値／誤差共分散値信号Ｓ１６を初期値として生成する加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とに対し、加速度時系列データメモリ１４では加速度時系列データ信号Ｓ１７、位置予測値／誤差共分散値時系列データメモリ１５では位置予測値／誤差共分散値時系列データ信号Ｓ１８をそれぞれ生成する。
【００４４】
次に、位置推定用時系列データ抽出器１７では、ＧＰＳデータ更新時刻信号Ｓ２２と加速度時系列データ信号Ｓ１７と位置予測値／誤差共分散値時系列データ信号Ｓ１８とに基づき、位置推定処理に必要となる更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３とをそれぞれ生成する。
【００４５】
さらに、位置推定器１８では、ＧＰＳ受信機１１の検出時間遅れ分を考慮した上で、更新位置信号Ｓ１０と更新時刻信号Ｓ１１と加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３とから、更新位置信号Ｓ１０と加速度データ抽出信号Ｓ１２との検出時刻に関する対応を図りながらカルマンフィルタ処理を実施後、ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１として位置推定値とそれに付随する誤差共分散値信号を発生する。
【００４６】
最後に、位置推定値選定器１９では、加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１とから、現在時刻において最も適切と思われる位置予測値と誤差共分散値信号とを選択し、それを位置推定値／誤差共分散値信号Ｓ１６として発生する。
【００４７】
ここで、本実施の形態の補正制御手段１０は、生成済みの予測値時系列データ信号（データ列）から検出時刻の対応するデータを抽出する機能を備えてよい。
又、補正制御手段１０は、前記物理量検出信号と前回の推定信号とから生成される現在時刻の物理量ベース予測値信号に対し、前記更新位置信号の検出時刻である時刻信号を検出した上で、現在時刻までに検出し蓄積された物理量時系列データ信号と予測値時系列データ信号とに基づいて、検出時刻の対応する予測値抽出信号、物理量データ抽出信号を抽出してこれらを前記移動体位置推定手段に入力し、補正を加味した推定処理を前記移動体位置推定手段に実行させ、前記移動体位置推定手段により現在時刻での推定データを推定するように制御する機能を備えてよい。
【００４８】
（動作処理手順について）
（全体の概略動作）
次に、上述のような構成を有する移動体位置推定における全体の動作処理手順について説明する。
【００４９】
本実施の形態に係る動作処理手順は、移動体の位置を推定する移動体位置推定装置が行うものを対象とするものである。
【００５０】
本実施の形態に係る移動体位置推定における全体の動作処理手順は、基本的手順として、前記移動体が備えている位置検出手段が更新位置信号を検出し（位置検出ステップ）、前記移動体が備えている加速度などの物理量を検出する物理量検出手段が物理量検出信号を検出し（物理量検出ステップ）、前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように補正制御をし（補正制御ステップ）、この補正制御された前記物理量検出信号と前記更新位置信号とに基づいて前記補正が加味された前記移動体の位置を予測推定した推定信号を出力する（移動体位置推定ステップ）ことを行う。
【００５１】
また、この移動体位置推定における動作処理手順では、この内、前記補正制御をするに際しては（補正制御ステップでは）、前記物理量検出信号と前回の推定信号とに基づいて前記移動体の位置を予測して物理量ベース予測値信号を生成し（物理量ベースの予測値信号生成ステップ）、前記更新位置信号の検出時刻を検出し時刻信号として生成し（時刻検出ステップ）、前記物理量検出手段にて検出された前記物理量検出信号を時系列データとして蓄積処理し、物理量時系列データ信号を生成し（第１の蓄積処理ステップ又は物理量時系列データ蓄積処理ステップ）、前記予測値信号生成ステップにて生成された前記物理量ベース予測値信号を時系列データとして蓄積処理し、予測値時系列データ信号を生成し（第２の蓄積処理ステップ又は物理量ベース予測値時系列データ蓄積処理ステップ）、現在時刻までの生成済みの前記予測値時系列データ信号と前記物理量時系列データ信号と前記時刻信号とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号と物理量データ抽出信号とを生成し、これらを前記時刻信号とともに前記移動体位置推定手段に入力する（時系列データ抽出ステップ）ことを行う。
この場合、前記推定をするに際しては（移動体位置推定ステップでは）、前記更新位置信号と前記時刻信号と前記物理量データ抽出信号と前記予測値抽出信号とに基づいて予測推定を行い前記推定信号を出力することができる。
【００５２】
また、この移動体位置推定における動作処理手順では、前記推定をするに際しては（移動体位置推定ステップでは）、前記更新位置信号の前記検出時刻に応じ加速度ベース予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することができる。
【００５３】
さらに、この移動体位置推定における動作処理手順では、前記測位衛星ベース推定信号と加速度ベース予測値信号とを選択し、いずれか一方を前記推定信号として出力する（選択ステップ）をさらに行うことができる。
以下、これらの各ステップを詳述する。
【００５４】
（詳細動作）
ここで、図１に示す本実施の形態の動作について、具体例として移動体である無人航空機（飛翔体）にＧＰＳ受信機１１と加速度センサ１２とを搭載し、この飛翔体の位置を地上局で推定する場合を説明する。
【００５５】
（位置検出ステップ、物理量検出ステップ）
先ず、移動体に搭載されたＧＰＳ受信機１１が測位衛星からの信号に基づき更新位置を検出して更新位置信号を出力する。又、移動体に搭載された加速度センサ１２が加速度を検出して加速度センサ検出信号を出力する。
【００５６】
（物理量ベースの予測値信号生成ステップ）
一方、位置伝播器１６では、位置推定値選定器１９が前回出力した位置推定値／誤差共分散値信号Ｓ１６を構成するシステム状態量と誤差共分散行列をそれぞれｘ+k、Ｐ+kと設定した上で、加速度センサ１２で検出した飛翔体の加速度センサ検出信号Ｓ１９と組み合わせることで得られる（１）式の線形確率システム（状態空間モデル）
〔数１〕
ｘk+1 ＝ Φkｘk＋Ｇkｗk＋Ｄkｕk
ｙk ＝ Ｈkｘk＋ｖk （１）
（ｘkはシステム状態量、Φkは状態遷移行列、Ｇkは駆動ノイズ行列、Ｄkは駆動入力行列、Ｈkは観測行列、ｕkは加速度センサ検出信号Ｓ１９、ｙkは観測信号）に基づき、以下の計算式（２）（３）を用いて、システム状態量予測値ｘ-k+1と誤差共分散行列予測値Ｐ-k+1とで構成する加速度ベースの位置予測値／誤差共分散値信号Ｓ２０を生成する。
【００５７】
〔数２〕
ｘ-k+1 ＝ Φkｘ+k＋Ｄkｕk （２）
〔数３〕
Ｐ-k+1 ＝ ΦkＰ+kΦTk＋ＧkＱkＧTk （３）
ただし、式中、上付きの「＋」はカルマンフィルタ処理後の推定値、上付きの「−」はカルマンフィルタ処理前の伝播予測値、上付きの「Ｔ」は転置行列、下付きの「ｋ」は観測時系列の順序を示す。
また、システムノイズｗkと観測ノイズｖkは互いに独立な白色ノイズを仮定し、さらにＱkは、システムノイズｗkの共分散行列（カルマンフィルタ設計者が飛翔体ダイナミクス特性に応じ設定する設計パラメータ）を示す。
【００５８】
（第１の蓄積処理ステップ、第２の蓄積処理ステップ）
このとき、加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とでは、位置伝播器１６に対する入力であるｕk（加速度センサ検出信号Ｓ１９）とシステム状態量予測値ｘ-k+1および誤差共分散行列予測値Ｐ-k+1（加速度ベースの位置予測値／誤差共分散値信号Ｓ２０）とを、それぞれ時系列データとして格納する。
【００５９】
（時刻検出ステップ）
次に、ＧＰＳデータ更新時刻検出器１３では、ＧＰＳ受信機１１により検出した更新位置信号Ｓ１０からＧＰＳデータが更新された時刻を検出し、それをＧＰＳデータ更新時刻信号Ｓ２２として出力する。
【００６０】
（時系列データ検出ステップ）
また、位置推定用時系列データ抽出器１７では、加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とが生成する加速度時系列データ信号Ｓ１７と位置予測値／誤差共分散値時系列データ信号Ｓ１８とから、ＧＰＳデータ更新時刻信号Ｓ２２に基づき前回のＧＰＳ更新時刻以降の時系列データをそれぞれ加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３として抽出し、これらを更新時刻信号Ｓ１１と共に生成する。
【００６１】
このとき、位置推定用時系列データ抽出器１７は、加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とに対し、加速度メモリ操作指令信号Ｓ１４と位置メモリ操作指令信号Ｓ１５とをそれぞれ加速度時系列データメモリ１４と位置予測値／誤差共分散値時系列データメモリ１５とへ出力することで、それぞれ加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３として抽出済みの加速度センサ検出信号Ｓ１９と加速度ベースの位置予測値／誤差共分散値信号Ｓ２０とが占有していたメモリ領域を開放するためこれらデータ列をそれぞれのメモリ域から削除する。
【００６２】
（移動体位置推定ステップ）
一方、位置推定器１８では、位置推定用時系列データ抽出器１７により抽出した加速度データ抽出信号Ｓ１２と抽出予測値／抽出誤差共分散値信号Ｓ１３と更新時刻信号Ｓ１１とに加え、ＧＰＳ受信機１１によって生成された更新位置信号Ｓ１０とを組み合わせカルマンフィルタ処理を実施することで、ＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を生成する。
【００６３】
ここで、観測時系列の順序を下付きの「ｎ」を用いて今回の観測を「ｎ」、次回の観測を「ｎ＋１」と記述すると、まず、位置推定器１８では、既に位置伝播器１６において生成した加速度ベースの位置予測値／誤差共分散値信号Ｓ２０からＧＰＳデータ更新時刻検出器１３によって検出したＧＰＳデータの更新時刻信号Ｓ１１に基づき、ＧＰＳ受信機１１によって得られた更新位置信号Ｓ１０に基づくカルマンフィルタ更新処理を、位置予測値／誤差共分散値時系列データメモリ１５が発生する位置予測値／誤差共分散値時系列データ信号Ｓ１８から抽出した抽出予測値／抽出誤差共分散値信号Ｓ１３に対し（４）式、（５）式、（６）式を用いて実施する。
【００６４】
〔数４〕
Ｋn ＝ Ｐ-nＨn（ＨTnＰ-nＨn＋Ｒn）-1 （４）
〔数５〕
ｘ+n ＝ ｘ-n＋Ｋn（ｙn−ＨTnｘ-n） （５）
〔数６〕
Ｐ+n ＝ Ｐ-n−ＫnＨTnＰ-n （６）
ただし、Ｒnは、観測ノイズｖnの共分散行列（カルマンフィルタ設計者がＧＰＳ特性に応じ設定する設計パラメータ）であり、また上付きの「−１」は逆行列を示す。
【００６５】
次に、カルマンフィルタ予測処理は、加速度データ抽出信号Ｓ１２をｕnで表すと、（５）式、（６）式の結果から次に示す（７）式、（８）式の適用により実施できる。
〔数７〕
ｘ-n+1 ＝ Φnｘ+n＋Ｄnｕn （７）
〔数８〕
Ｐ-n+1 ＝ ΦnＰ+nΦTn＋ＧnＱnＧTn （８）
【００６６】
この結果、位置推定器１８では（４）式、（５）式、（６）式により実施する状態量の更新後に、（７）式、（８）式の適用により得られるシステム状態量予測値ｘ-n+1と誤差共分散行列予測値Ｐ-n+1とをＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１として生成する。
【００６７】
（選択ステップ）
最後に、位置推定値選定器１９では、位置推定器１８で生成されるＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１が更新された場合には最新のＧＰＳベースの位置予測値／誤差共分散値信号Ｓ２１を、それ以外の場合には加速度ベースの位置予測値／誤差共分散値信号Ｓ２０を選択し、それを位置推定値／誤差共分散値信号Ｓ１６として出力する。
【００６８】
従って本実施の形態では、ＧＰＳ受信機１１によって得られた更新位置信号Ｓ１０をそのままカルマンフィルタ更新処理に適用するのではなく、更新位置信号Ｓ１０の検出時刻に応じ位置推定値／誤差共分散値信号Ｓ１６に対するカルマンフィルタ更新処理を実施後、加速度センサ検出信号Ｓ１９に基づくカルマンフィルタ予測処理を改めて実行することで、位置推定値／誤差共分散値信号Ｓ１６を生成する構成をとる。
【００６９】
ここで、以上の予測値信号生成ステップ、時刻検出ステップ、第１の蓄積処理ステップ、第２の蓄積処理ステップ、時系列データ抽出ステップからなるステップは、補正制御ステップないしは補正制御機能の一部又は一例とすることができる。
又、前記推定ステップでは、前記更新位置信号の前記検出時刻に応じ加速度ベース予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することができる。
【００７０】
以上のように本実施の形態によれば、位置検出手段において検出される更新位置信号に検出時間遅れが含まれる場合でも、補正制御手段が前記位置検出手段での位置検出遅れを補正するために、更新位置信号と物理量検出信号との検出時刻に関する対応を図るように移動体位置推定手段の入出力動作及び処理動作を制御して推定処理を実施するため、位置検出手段の検出時間遅れを補正することが可能となり、その結果、高速に移動する移動体に対しても位置推定値の精度確保が実現できる。
【００７１】
すなわち、ＧＰＳ受信機において検出される更新位置データに検出時間遅れが含まれる場合でも、生成済みの位置予測値から時刻対応するデータ列を抽出しカルマンフィルタ処理を実施するため、ＧＰＳ受信機の検出時間遅れを補正することが可能となり、高速に移動する移動体に対してでも、それらの位置情報を高精度に取得可能とする
【００７２】
より詳細には、補正制御手段１０を備えた移動体位置推定装置１は、ＧＰＳ受信機１１と加速度センサ１２とを組み合わせた汎用的なカルマンフィルタの一部である移動体位置推定処理機能（移動体位置推定手段：位置推定器１８）を用いて、位置伝播計算から生成する加速度センサ検出データに基づく現在時刻の移動体位置予測値に対し、ＧＰＳ受信機が発生する移動体位置更新値の検出時刻をＧＰＳデータ更新時刻検出器１３を用いて検出した上で、現在時刻までに検出しメモリ上に確保した移動体位置予測値と加速度センサ検出信号とに基づいて現在時刻での位置予測値をカルマンフィルタ処理により生成する機能を実現している。
その結果、移動体位置推定装置１では、過去の位置予測結果から検出時刻の対応するデータどうしを組み合わせてカルマンフィルタ処理を実行するため、ＧＰＳ受信機１１の時間遅れ補正が明確に実施可能となり、位置推定値の推定精度確保が達成できる。
【００７３】
特に、ＧＰＳ受信機１１で生じる更新位置信号Ｓ１０の検出時刻誤差を陽に補正しながら飛翔体の位置推定を実施しているため、急激な位置変動が連続的に生じた場合でも、ＧＰＳ受信機１での検出遅れ時間有無にかかわらず、高精度な飛翔体の位置推定値を継続的に発生することが可能となる。
【００７４】
また、ここで示した位置推定用時系列データ抽出器１７と位置推定器１８との組み合わせ構成による位置推定処理は、ＧＰＳ受信機１１における検出むだ時間（検出時刻誤差）を補正するために実現した独自技術であり、特許文献１など関連技術の発明の組み合わせにより容易に類推できるものではない。
【００７５】
[その他の各種変形例]
また、本発明にかかる装置及び方法は、そのいくつかの特定の実施の形態に従って説明してきたが、本発明の主旨および範囲から逸脱することなく本発明の本文に記述した実施の形態に対して種々の変形が可能である。
【００７６】
例えば、図１に示すブロック図における一部の各ブロックは、コンピュータにより実行可能なプログラムにより機能化された状態を示すソフトウエアモジュール構成であってもよい。
【００７７】
その場合、物理的構成は例えば一又は複数のＣＰＵ（或いは一又は複数のＣＰＵと一又は複数のメモリ）等ではあるが、各部（器・回路・手段）によるソフトウエア構成は、プログラムの制御によってＣＰＵが発揮する複数の機能を、それぞれ複数の部（器・回路・手段）による構成要素として表現したものとすることができる。
【００７８】
以上に示した各部（器・回路・手段）は、プログラムにより機能化されたコンピュータをプログラムの機能と共に実現し得るように構成してもよい。この場合、各部間で授受される各種「信号」を「データ」としてもよい。
【００７９】
また、固有のハードウエアにより恒久的に機能化された複数の電子回路ブロックからなる装置で構成してもよい。したがって、これらの機能ブロックがハードウェアのみ、ソフトウェアのみ、またはそれらの組合せによっていろいろな形で実現でき、いずれかに限定されるものではない。
【００８０】
ここで、プログラム化してコンピュータに実行させる場合のコンピュータのハードウエア構成としては、種々の情報等を表示するための表示部（スクリーン）、この表示部の表示画面上（の各種入力欄等）にデータを操作入力するための操作入力部（例えばキーボード・マウス・各種ボタン・表示操作部＜画面上のボタン＞・タッチパネル等）、各種信号・データを送受信するための送受信部ないしは通信部（モデムなど）、各種プログラム・各種データを記憶しておく記憶部（例えばメモリ、ハードディスク等）、これらの制御を司る制御部（例えばＣＰＵ、ＭＰＵ、ＤＳＰ等）などを有することができる。
【００８１】
この場合のコンピュータは、デスクトップ、ラップトップコンピュータ、サーバコンピュータ、その他無線・有線通信機能を有する情報機器、またはこれに類するコンピュータなどいかなるコンピュータでもよく、移動式・固定式を問わない。
【００８２】
プログラム化して実行する場合、例えば図２に示すフローチャートのように実行することができる。以下の説明では、「信号」を「データ」として扱い、「器」を「部」として扱うことができるものとする。
すなわち、まず、コンピュータが、前記移動体が備えた位置検出手段が更新位置データを検出するように制御し（ステップＳＴ１１０：位置検出ステップないし位置検出機能）、前記移動体が備えている加速度などの物理量を検出する物理量検出手段（加速度検出手段）が物理量検出データ（加速度検出データ）を検出するように制御する処理（ステップＳＴ１１０：物理量検出ステップないし物理量検出機能）を行う。
この検出は、プログラム部分として含まなくてもよい。
【００８３】
この検出後、補正制御手段として機能する移動体位置推定装置が備えたコンピュータが、前記位置検出手段での位置検出遅れを補正するために、前記更新位置データと前記物理量検出データとの検出時刻に関する対応を図るように補正制御をする（ステップＳＴ１２０：補正制御ステップないし補正検出機能）。
より具体的には、この補正制御ステップでは、前記位置と前記加速度との検出時刻に関する対応を図るように時刻データと加速度データ抽出データと加速度ベース予測値抽出データとを位置推定部に入力する。
【００８４】
続いて、移動体位置推定手段として機能する移動体位置推定装置が備えたコンピュータが、この補正制御された前記物理量検出データと前記更新位置データとに基づいて前記補正が加味された前記移動体の位置を予測推定した推定データを出力する（ステップＳＴ１３０：移動体位置推定ステップないしは移動体位置推定機能）ことを行う。
【００８５】
さらに、選択手段として機能する移動体位置推定装置が備えたコンピュータが、測位衛星ベース推定データと加速度ベース予測値データとを選択し、いずれか一方を前記推定データとして出力する（ステップＳＴ１４０：選択ステップないしは選択機能）。
【００８６】
ここにおいて、補正制御ステップＳＴ１２０では、補正制御手段として機能する移動体位置推定装置が備えたコンピュータが、前記物理量検出データと前回の推定データとに基づいて前記移動体の位置を予測して物理量ベース予測値データを生成する（物理量ベースの予測値データ生成ステップないしは機能）。
また、前記コンピュータが、前記更新位置データの検出時刻を検出し時刻データとして生成する（時刻検出ステップないしは機能）。
さらに、前記コンピュータが、前記物理量検出手段にて検出された前記物理量検出データを時系列データとして蓄積処理し、物理量時系列データを生成する（第１の蓄積処理機能）。
またさらに、前記コンピュータが、前記物理量ベースの予測値データ生成部にて生成された前記物理量ベース予測値データを時系列データとして蓄積処理し、物理量ベースの予測値時系列データを生成する（第２の蓄積処理ステップないしは機能）。
また、前記コンピュータが、現在時刻までの生成済みの前記物理量ベースの予測値時系列データと前記物理量時系列データデータと前記時刻データとに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して物理量ベースの予測値抽出データと物理量データ抽出データとを生成し、これらを前記時刻データとともに前記移動体位置推定機能に入力する（時系列データ抽出ステップないしは機能）。
この場合、前記移動体位置推定ステップＳＴ１３０では、前記コンピュータが、前記更新位置データと前記時刻データと前記物理量抽出データと前記予測値抽出データとに基づいて前記推定データを出力することができる。
【００８７】
さらに、前記移動体位置推定ステップＳＴ１３０では、前記コンピュータが、前記更新位置データの前記検出時刻に応じ加速度ベースの予測値抽出データに対するカルマンフィルタ更新処理を実施後、前記加速度抽出データに基づくカルマンフィルタ予測処理を実行し、前記推定データとして測位衛星ベース推定データを生成することができる。
【００８８】
その他の構成およびその他のステップないしは機能並びにその作用効果については、前述した実施の形態の場合と同一となっている。以上説明した方法は、コンピュータがプログラムを記録媒体から読み込んで実行することによっても実現することが出来る。すなわち、上述のプログラムを、情報記録媒体に記録した構成であってもよい。
【００８９】
更に、位置検出手段としては、ＧＰＳの他ＧＬＯＮＡＳＳなど他の測位システムから位置情報を取得する場合であってもよい。
また、ＧＰＳ受信機は、ＧＰＳ（汎地球測位システム）により自車の現在位置を例えば緯度、経度、高度で検出すると共に、この自車の進行方向や走行速度等を検出するものであり、例えば４個のＧＰＳ人工衛星から地上に無線送信されるＧＰＳ電波をＧＰＳアンテナを介してそれぞれ受信して、これらの各電波の到達時間差から三角測量の原理で自車の現在位置を３次元で測位することができる。２次元測位の場合は、３個のＧＰＳ人工衛星からのＧＰＳ電波を受信することができる。
【００９０】
さらにまた、上記構成部材の数、位置、形状等は上記実施の形態に限定されず、本発明を実施する上で好適な数、位置、形状等にすることができる。すなわち、上記実施の形態では、ＧＰＳ受信機が１個、加速度センサが１個、の場合を示したが、本発明は、これらの個数を制限するものではない。
【００９１】
ところで、このような装置は、単独で存在する場合もあるし、ある機器（例えば電子機器など）に組み込まれた状態で利用されることもあるなど、発明の思想としてはこれに限らず、各種の態様を含むものである。
【００９２】
また、前記実施の形態において、各構成の「〜器」が同一筐体中にあるか否かは問わない。このため、複数の機器から構成されるものに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
【００９３】
また、発明の範囲は、図示例に限定されないものとする。さらに、上記各実施の形態には種々の段階が含まれており、開示される複数の構成要件における適宜な組み合わせにより種々の発明が抽出され得る。つまり、上述の各実施の形態同士、あるいはそれらのいずれかと各変形例のいずれかとの組み合わせによる例をも含む。
また、様々な移動体に対する位置情報の推定手法として容易に利用することができるものである。
【産業上の利用可能性】
【００９４】
本発明は、移動体位置推定装置ないしはそのシステム全般に利用できる。
【図面の簡単な説明】
【００９５】
【図１】本発明の第１の実施の形態による移動体位置推定装置の全体構成の一例を示すブロック図である。
【図２】本発明の他の実施の形態による移動体位置推定装置における処理手順の一例を示すフローチャートである。
【図３】関連技術の移動体位置推定装置の全体構成の一例を示すブロック図である。
【符号の説明】
【００９６】
１ 移動体位置推定装置
１０ 補正制御手段
１１ ＧＰＳ受信機（位置検出手段）
１２ 加速度センサ（物理量検出手段）
１３ ＧＰＳデータ更新時刻検出器（時刻検出部）
１４ 加速度時系列データメモリ（第１の蓄積処理部）
１５ 位置予測値／誤差共分散値時系列データメモリ（第２の蓄積処理部）
１６ 位置伝播器（予測値信号生成部）
１７ 位置推定用時系列データ抽出器（時系列データ抽出部）
１８ 位置推定器（移動体位置推定手段）
１９ 位置推定値選定器（選択手段）
Ｓ１０ 更新位置信号
Ｓ１１ 更新時刻信号（時刻信号）
Ｓ１２ 加速度データ抽出信号（物理量データ抽出信号）
Ｓ１３ 抽出予測値／抽出誤差共分散値信号（予測値抽出信号）
Ｓ１４ 加速度メモリ操作指令信号（第１操作指令信号）
Ｓ１５ 位置メモリ操作指令信号（第２操作指令信号）
Ｓ１６ 位置推定値／誤差共分散値信号（推定信号）
Ｓ１７ 加速度時系列データ信号（物理量時系列データ信号）
Ｓ１８ 位置予測値／誤差共分散値時系列データ信号（予測値抽出信号）
Ｓ１９ 加速度センサ検出信号（物理量検出信号）
Ｓ２０ 加速度ベースの位置予測値／誤差共分散値信号（物理量ベース予測値信号、加速度ベース予測値信号）
Ｓ２１ ＧＰＳベースの位置予測値／誤差共分散値信号（推定信号、測位衛星ベース推定信号）
Ｓ２２ ＧＰＳデータ更新時刻信号（時刻信号）
１００ 移動体位置推定装置
１０１ ＧＰＳ受信機
１０２ 加速度センサ
１０３ 測位演算器（カルマンフィルタ）
１０４ 位置更新信号
１０５ 加速度検出信号
【特許請求の範囲】
【請求項１】
少なくとも、移動体が備えている位置検出手段からの更新位置信号と、前記移動体が備えている加速度等の物理量を検出する物理量検出手段からの物理量検出信号と、に基づいて前記移動体の位置を予測推定した推定信号を出力する移動体位置推定手段と、
前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように前記移動体位置推定手段の入出力動作及び処理動作を制御する補正制御手段と、
を備えたことを特徴とする移動体位置推定装置。
【請求項２】
請求項１に記載の移動体位置推定装置において、
前記補正制御手段は、
前記物理量検出信号と前回の推定信号とに基づいて前記移動体の位置を予測して物理量ベース予測値信号を生成する予測値信号生成部と、
前記更新位置信号の検出時刻を検出しこれを時刻信号とする時刻検出部と、
前記物理量検出手段にて検出された前記物理量検出信号を時系列データとして蓄積処理し、物理量時系列データ信号を生成する第１の蓄積処理部と、
前記予測値信号生成部にて生成された前記物理量ベース予測値信号を時系列データとして蓄積処理し、予測値時系列データ信号を生成する第２の蓄積処理部と、
現在時刻までの生成済みの前記予測値時系列データ信号と前記物理量時系列データ信号と前記時刻信号とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号と物理量データ抽出信号とを生成し、これらを前記時刻信号とともに前記移動体位置推定手段に入力する時系列データ抽出部と、
を備え、
前記移動体位置推定手段は、
前記更新位置信号と前記時刻信号と前記物理量データ抽出信号と前記予測値抽出信号とに基づいて前記推定信号を出力する、
ことを特徴とする移動体位置推定装置。
【請求項３】
請求項２に記載の移動体位置推定装置において、
前記物理量は、加速度を含み、
前記移動体位置推定手段は、
前記更新位置信号の前記検出時刻に応じ予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することを特徴とする移動体位置推定装置。
【請求項４】
請求項３に記載の移動体位置推定装置において、
前記測位衛星ベース推定信号と加速度ベース予測値信号とを選択し、いずれか一方を前記推定信号として出力する選択手段をさらに有することを特徴とする移動体位置推定装置。
【請求項５】
移動体の位置を推定する移動体位置推定装置を用いた移動体位置推定方法であって、
前記移動体が備えている位置検出手段が更新位置信号を検出し、
前記移動体が備えている加速度などの物理量を検出する物理量検出手段が物理量検出信号を検出し、
前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように補正制御をし、
この補正制御された前記物理量検出信号と前記更新位置信号とに基づいて前記補正が加味された前記移動体の位置を予測推定した推定信号を出力する、
ことを特徴とする移動体位置推定方法。
【請求項６】
請求項５に記載の移動体位置推定方法において、
前記補正制御をするに際しては、
前記物理量検出信号と前回の推定信号とに基づいて前記移動体の位置を予測して物理量ベース予測値信号を生成し、
前記更新位置信号の検出時刻を検出しこれを時刻信号とし、
前記物理量検出手段にて検出された前記物理量検出信号を時系列データとして蓄積処理し、物理量時系列データ信号を生成し、
前記物理量ベース予測値信号を時系列データとして蓄積処理し、予測値時系列データ信号を生成し、
現在時刻までの生成済みの前記予測値時系列データ信号と前記物理量時系列データ信号と前記時刻信号とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号と物理量データ抽出信号とを生成し、
前記推定をするに際しては、
前記更新位置信号と前記時刻信号と前記物理量データ抽出信号と前記予測値抽出信号とに基づいて前記推定信号を出力する、
ことを特徴とする移動体位置推定方法。
【請求項７】
請求項６に記載の移動体位置推定方法において、
前記物理量は、加速度を含み、
前記推定をするに際しては、
前記更新位置信号の前記検出時刻に応じ予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することを特徴とする移動体位置推定方法。
【請求項８】
請求項７記載の移動体位置推定方法において、
前記測位衛星ベース推定信号と加速度ベース予測値信号とを選択し、いずれか一方を前記推定信号として出力することを特徴とする移動体位置推定方法。
【請求項９】
移動体の位置を推定する移動体位置推定装置が備えたコンピュータに諸機能を実現させることが可能な移動体位置推定プログラムであって、
少なくとも、移動体が備えている位置検出手段からの更新位置データと、前記移動体が備えている加速度などの物理量を検出する物理量検出手段からの物理量検出データと、に基づいて前記移動体の位置を予測推定した推定データを出力する移動体位置推定機能と、
前記位置検出手段での位置検出遅れを補正するために、前記更新位置データと前記物理量検出データとの検出時刻に関する対応を図るように前記移動体位置推定機能の入出力動作及び処理動作を制御する補正制御機能と、
を前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【請求項１０】
請求項９に記載の移動体位置推定プログラムにおいて、
前記補正制御機能では、
前記物理量検出データと前回の推定データとに基づいて前記移動体の位置を予測して物理量ベース予測値データを生成する予測値データ生成機能と、
前記更新位置データの検出時刻を検出しこれを時刻データとする時刻検出機能と、
前記物理量検出手段にて検出された前記物理量検出データを時系列データとして蓄積処理し、物理量時系列データを生成する第１の蓄積処理機能と、
前記予測値データ生成機能にて生成された前記物理量ベース予測値データを時系列データとして蓄積処理し、予測値時系列データを生成する第２の蓄積処理機能と、
現在時刻までの生成済みの前記予測値時系列データと前記物理量時系列データと前記時刻データとに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出データと物理量抽出データとを生成し、これらを前記時刻データとともに前記移動体位置推定機能に入力する時系列データ抽出機能と、
をその内容とし、これらを前記コンピュータに実現させ、
前記移動体位置推定機能では、
前記更新位置データと前記時刻データと前記物理量抽出データと前記予測値抽出データとに基づいて前記推定データを出力する機能をその内容とし、これを前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【請求項１１】
請求項１０に記載の移動体位置推定プログラムにおいて、
前記物理量は、加速度を含み、
前記移動体位置推定機能では、
前記更新位置データの前記検出時刻に応じ前記予測値抽出データに対するカルマンフィルタ更新処理を実施後、前記加速度抽出データに基づくカルマンフィルタ予測処理を実行し、前記推定データとして測位衛星ベース推定データを生成する機能をその内容とし、これを前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【請求項１２】
請求項１１に記載の移動体位置推定プログラムにおいて、
前記測位衛星ベース推定データと加速度ベース予測値データとを選択し、いずれか一方を前記推定データとして出力する選択機能をさらに前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【請求項１】
少なくとも、移動体が備えている位置検出手段からの更新位置信号と、前記移動体が備えている加速度等の物理量を検出する物理量検出手段からの物理量検出信号と、に基づいて前記移動体の位置を予測推定した推定信号を出力する移動体位置推定手段と、
前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように前記移動体位置推定手段の入出力動作及び処理動作を制御する補正制御手段と、
を備えたことを特徴とする移動体位置推定装置。
【請求項２】
請求項１に記載の移動体位置推定装置において、
前記補正制御手段は、
前記物理量検出信号と前回の推定信号とに基づいて前記移動体の位置を予測して物理量ベース予測値信号を生成する予測値信号生成部と、
前記更新位置信号の検出時刻を検出しこれを時刻信号とする時刻検出部と、
前記物理量検出手段にて検出された前記物理量検出信号を時系列データとして蓄積処理し、物理量時系列データ信号を生成する第１の蓄積処理部と、
前記予測値信号生成部にて生成された前記物理量ベース予測値信号を時系列データとして蓄積処理し、予測値時系列データ信号を生成する第２の蓄積処理部と、
現在時刻までの生成済みの前記予測値時系列データ信号と前記物理量時系列データ信号と前記時刻信号とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号と物理量データ抽出信号とを生成し、これらを前記時刻信号とともに前記移動体位置推定手段に入力する時系列データ抽出部と、
を備え、
前記移動体位置推定手段は、
前記更新位置信号と前記時刻信号と前記物理量データ抽出信号と前記予測値抽出信号とに基づいて前記推定信号を出力する、
ことを特徴とする移動体位置推定装置。
【請求項３】
請求項２に記載の移動体位置推定装置において、
前記物理量は、加速度を含み、
前記移動体位置推定手段は、
前記更新位置信号の前記検出時刻に応じ予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することを特徴とする移動体位置推定装置。
【請求項４】
請求項３に記載の移動体位置推定装置において、
前記測位衛星ベース推定信号と加速度ベース予測値信号とを選択し、いずれか一方を前記推定信号として出力する選択手段をさらに有することを特徴とする移動体位置推定装置。
【請求項５】
移動体の位置を推定する移動体位置推定装置を用いた移動体位置推定方法であって、
前記移動体が備えている位置検出手段が更新位置信号を検出し、
前記移動体が備えている加速度などの物理量を検出する物理量検出手段が物理量検出信号を検出し、
前記位置検出手段での位置検出遅れを補正するために、前記更新位置信号と前記物理量検出信号との検出時刻に関する対応を図るように補正制御をし、
この補正制御された前記物理量検出信号と前記更新位置信号とに基づいて前記補正が加味された前記移動体の位置を予測推定した推定信号を出力する、
ことを特徴とする移動体位置推定方法。
【請求項６】
請求項５に記載の移動体位置推定方法において、
前記補正制御をするに際しては、
前記物理量検出信号と前回の推定信号とに基づいて前記移動体の位置を予測して物理量ベース予測値信号を生成し、
前記更新位置信号の検出時刻を検出しこれを時刻信号とし、
前記物理量検出手段にて検出された前記物理量検出信号を時系列データとして蓄積処理し、物理量時系列データ信号を生成し、
前記物理量ベース予測値信号を時系列データとして蓄積処理し、予測値時系列データ信号を生成し、
現在時刻までの生成済みの前記予測値時系列データ信号と前記物理量時系列データ信号と前記時刻信号とに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出信号と物理量データ抽出信号とを生成し、
前記推定をするに際しては、
前記更新位置信号と前記時刻信号と前記物理量データ抽出信号と前記予測値抽出信号とに基づいて前記推定信号を出力する、
ことを特徴とする移動体位置推定方法。
【請求項７】
請求項６に記載の移動体位置推定方法において、
前記物理量は、加速度を含み、
前記推定をするに際しては、
前記更新位置信号の前記検出時刻に応じ予測値抽出信号に対するカルマンフィルタ更新処理を実施後、前記加速度データ抽出信号に基づくカルマンフィルタ予測処理を実行し、前記推定信号として測位衛星ベース推定信号を生成することを特徴とする移動体位置推定方法。
【請求項８】
請求項７記載の移動体位置推定方法において、
前記測位衛星ベース推定信号と加速度ベース予測値信号とを選択し、いずれか一方を前記推定信号として出力することを特徴とする移動体位置推定方法。
【請求項９】
移動体の位置を推定する移動体位置推定装置が備えたコンピュータに諸機能を実現させることが可能な移動体位置推定プログラムであって、
少なくとも、移動体が備えている位置検出手段からの更新位置データと、前記移動体が備えている加速度などの物理量を検出する物理量検出手段からの物理量検出データと、に基づいて前記移動体の位置を予測推定した推定データを出力する移動体位置推定機能と、
前記位置検出手段での位置検出遅れを補正するために、前記更新位置データと前記物理量検出データとの検出時刻に関する対応を図るように前記移動体位置推定機能の入出力動作及び処理動作を制御する補正制御機能と、
を前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【請求項１０】
請求項９に記載の移動体位置推定プログラムにおいて、
前記補正制御機能では、
前記物理量検出データと前回の推定データとに基づいて前記移動体の位置を予測して物理量ベース予測値データを生成する予測値データ生成機能と、
前記更新位置データの検出時刻を検出しこれを時刻データとする時刻検出機能と、
前記物理量検出手段にて検出された前記物理量検出データを時系列データとして蓄積処理し、物理量時系列データを生成する第１の蓄積処理機能と、
前記予測値データ生成機能にて生成された前記物理量ベース予測値データを時系列データとして蓄積処理し、予測値時系列データを生成する第２の蓄積処理機能と、
現在時刻までの生成済みの前記予測値時系列データと前記物理量時系列データと前記時刻データとに基づいて、前記検出時刻の対応する物理量ベース予測値と物理量データとを各々抽出して予測値抽出データと物理量抽出データとを生成し、これらを前記時刻データとともに前記移動体位置推定機能に入力する時系列データ抽出機能と、
をその内容とし、これらを前記コンピュータに実現させ、
前記移動体位置推定機能では、
前記更新位置データと前記時刻データと前記物理量抽出データと前記予測値抽出データとに基づいて前記推定データを出力する機能をその内容とし、これを前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【請求項１１】
請求項１０に記載の移動体位置推定プログラムにおいて、
前記物理量は、加速度を含み、
前記移動体位置推定機能では、
前記更新位置データの前記検出時刻に応じ前記予測値抽出データに対するカルマンフィルタ更新処理を実施後、前記加速度抽出データに基づくカルマンフィルタ予測処理を実行し、前記推定データとして測位衛星ベース推定データを生成する機能をその内容とし、これを前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【請求項１２】
請求項１１に記載の移動体位置推定プログラムにおいて、
前記測位衛星ベース推定データと加速度ベース予測値データとを選択し、いずれか一方を前記推定データとして出力する選択機能をさらに前記コンピュータに実現させることを特徴とする移動体位置推定プログラム。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２０１０−６６０７３（Ｐ２０１０−６６０７３Ａ）
【公開日】平成２２年３月２５日（２０１０．３．２５）
【国際特許分類】
【出願番号】特願２００８−２３１３６０（Ｐ２００８−２３１３６０）
【出願日】平成２０年９月９日（２００８．９．９）
【出願人】（０００００４２３７）日本電気株式会社 (19,353)
【Ｆターム（参考）】
【公開日】平成２２年３月２５日（２０１０．３．２５）
【国際特許分類】
【出願日】平成２０年９月９日（２００８．９．９）
【出願人】（０００００４２３７）日本電気株式会社 (19,353)
【Ｆターム（参考）】
[ Back to top ]