走行状態推定システム
【課題】自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償して最終的に決定される測位データの精度を向上させること。
【解決手段】ＧＰＳ信号に基づき第一のサンプリング周期で測位データを演算して推定手段に出力するＧＰＳ測位演算手段と、第一のサンプリング周期より短い第二のサンプリング周期で観測データを出力する自律航法用センサと、第一のサンプリング周期内に自律航法用センサから出力された観測データの組を所定の一次関数に近似する近似式演算手段と有し、該近似式演算手段が、測位データと略同期したタイミングにおける一次関数の近似式の値を切片の値とし、該切片の値を該近似式の傾きの値とともに自律航法データとして推定手段に出力するように構成された走行状態推定システムを提供する。
【解決手段】ＧＰＳ信号に基づき第一のサンプリング周期で測位データを演算して推定手段に出力するＧＰＳ測位演算手段と、第一のサンプリング周期より短い第二のサンプリング周期で観測データを出力する自律航法用センサと、第一のサンプリング周期内に自律航法用センサから出力された観測データの組を所定の一次関数に近似する近似式演算手段と有し、該近似式演算手段が、測位データと略同期したタイミングにおける一次関数の近似式の値を切片の値とし、該切片の値を該近似式の傾きの値とともに自律航法データとして推定手段に出力するように構成された走行状態推定システムを提供する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、走行状態に関する各種観測を行う自律航法システムとＧＰＳ（Global Positioning System）とを複合して対象物の走行状態を推定する走行状態推定システムに関連し、詳しくは、各観測システムによって取得される観測データの同期をとり対象物の走行状態を高精度に推定する走行状態推定システムに関する。
【背景技術】
【０００２】
ＧＰＳは、地球を周回するＧＰＳ衛星から発信されるＧＰＳ信号を用いて対象物の位置情報等を測定するための測位システムであり、日常生活においても広く利用されている。
【０００３】
ＧＰＳでは、ＣＤＭＡ（Code Division Multiple Access）方式によって複数のＧＰＳ衛星が同じ周波数帯を共用してＧＰＳ信号を送信する。具体的には、ＧＰＳ信号は、クロック情報やＧＰＳ衛星の軌道情報を含む通信信号としての航法メッセージと、ＧＰＳ衛星毎に定められた拡散符号としてのＰＲＮコード（Pseudo Random Noise code）とにより、１５７５．４２ＭＨｚのキャリアをＢＰＳＫ（Binary Phase Shift Keying）変調したものである。ＧＰＳレシーバは、原理上必要とされる個数以上のＧＰＳ衛星を選択し、選択されたＧＰＳ衛星に対応するＰＲＮコードを生成し、生成されたＰＲＮコードの位相をＧＰＳ信号に同期させて乗じることにより航法データを復調する。そして、復調して得られた航法データを用いて演算を行い、対象物の位置情報、方位情報、移動速度等を生成する。以下、ＧＰＳレシーバによるＧＰＳ信号を利用した位置測位、方位測定、移動速度測定等を「ＧＰＳ測位」と記す。
【０００４】
ＧＰＳレシーバを備えた一般消費者向け製品には、例えば乗用車に搭載されるカーナビゲーションシステムがある。かかるカーナビゲーションシステムの一例が、特許文献１に開示されている。特許文献１に示されるように、一般的なカーナビゲーションシステムは、ＧＰＳ測位と並行して、ジャイロセンサ、速度センサ等の各種自律航法用センサ（以下、「ＤＲ（dead reckoning）センサ」と記す。）から出力される観測データを用いてカルマンフィルタにより位置推定、方位推定、移動速度推定等を行う。次いで、統合処理回路により、各測位方式の弱点を相互に補完するように、両方式による測位データに基づいた演算により最終的な測位データが決定される。カーナビゲーションシステムは、演算して得られた測位データに基づき、画面上に表示させる自車位置を更新する。また、ユーザによって目的地が設定されている場合には、画面上の自車位置の更新と併せて当該目的地に向けたナビゲーションを行う。
【特許文献１】特開２００８−１４８１０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ところで、特許文献１に記載のカルマンフィルタは、ＤＲセンサから観測データが入力される毎に位置推定等を逐次行い、推定データを統合処理回路に出力している。すなわち、カルマンフィルタによる推定データの出力レートは、ＤＲセンサによる観測データの出力サンプリングレート（例えば１００Ｈｚ）に応じて決定されている。一方、ユニット化された一般的なＧＰＳレシーバは、位相積算時間に関わる精度の関係から、ＤＲセンサによる観測データの出力サンプリングレートと同じレートでＧＰＳ測位データを高速に演算することができない。ＧＰＳレシーバは、カルマンフィルタによる推定演算より遙かに低速なレートでＧＰＳ測位データを演算し統合処理回路に出力している。
【０００６】
このようにＧＰＳ測位データとカルマンフィルタによる推定データのサンプリングレートが相違する場合、両データの同期を正確にとり続けることが難しく、各データ間に遅延が生じる問題がある。この場合、統合処理回路は、観測時点が相違する（つまり非同期な）データに基づき測位データを演算することになる。そのため、最終的に決定される測位データの精度低下が避けられない問題が生じる。
【０００７】
また、ＤＲセンサによる観測データの出力サンプリングレートは高速である。そのため、カルマンフィルタにおける演算処理量が膨大になるといった問題も指摘されている。特に、廉価仕様のカーナビゲーションシステムでは演算処理回路の性能が低いため、演算処理回路のプロセスがカルマンフィルタによる演算処理によって専有されることがある。この場合、システム全体の処理速度が著しく低下する弊害が生じる。
【０００８】
そこで、ＤＲセンサから出力される観測データをダウンサンプリングすることにより、カルマンフィルタにおける演算処理量を軽減させる回路設計が考えられる。ところが、この場合、カルマンフィルタによる推定データのサンプリング数が減少するため、該推定データの精度が低下する。その結果、結局は、最終的に決定される測位データの精度を低下させる弊害が生じる。
【０００９】
本発明はこのような事情に鑑みてなされたものであり、その目的とするところは、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償して、最終的に決定される測位データの精度向上を図ることができる走行状態推定システムを提供することにある。また、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償しつつもカルマンフィルタ等によるソフトウェア処理の負担を軽減させることができる走行状態推定システムを提供することにある。
【課題を解決するための手段】
【００１０】
上記の課題を解決する本発明の一形態に係る走行状態推定システムは、所定の推定手段により自律航法システムによる自律航法データとＧＰＳによる測位データとを複合して対象物の走行状態を推定するシステムであり、以下の特徴を有している。すなわち、該走行状態推定システムは、ＧＰＳ信号に基づき第一のサンプリング周期で測位データを演算して推定手段に出力するＧＰＳ測位演算手段と、第一のサンプリング周期より短い第二のサンプリング周期で観測データを出力する自律航法用センサと、第一のサンプリング周期内に自律航法用センサから出力された観測データの組を所定の一次関数に近似する近似式演算手段とを有している。
【００１１】
そして、走行状態推定システムが有する近似式演算手段は、推定手段により演算される測位データの精度を向上させるため、ＧＰＳ測位データと同期のとれた自律航法データを生成して推定手段に出力する。具体的には、近似式演算手段は、測位データと略同期したタイミングにおける一次関数の近似式の値を切片の値とし、該切片の値を該近似式の傾きの値とともに自律航法データとして推定手段に出力する。近似式演算手段による信号処理により、自律航法システムとＧＰＳの各方式間の信号の遅延時間が高精度に補償されるため、推定手段により演算される測位データが向上する。
【００１２】
近似式演算手段は、好ましくは、第一のサンプリング周期内に自律航法用センサから出力された複数の観測データを用いて所定の整数演算を行い、所定の加工データを生成して第一のサンプリング周期で出力する一次データ生成手段と、加工データ生成手段から出力された加工データに基づき近似式を浮動小数点演算により計算する二次データ生成手段とを有するように構成される。この場合、一次データ生成手段をハードウェアで構成し、二次データ生成手段をソフトウェアで構成することが望ましい。かかる構成によれば、第二のサンプリング周期から第一のサンプリング周期にダウンサンプリングされたデータを二次データ生成手段に処理させることができる。そのため、二次データ生成手段における演算処理量が軽減される。別の観点によれば、桁数の多い高精度な演算処理を二次データ生成手段に実行させることができる。
【発明の効果】
【００１３】
本発明の走行状態推定システムによれば、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償して最終的に決定される測位データの精度を向上させることができる。また、かかる効果に加えて、ソフトウェアにおける演算処理量を軽減させることができる。
【発明を実施するための最良の形態】
【００１４】
以下、図面を参照して、本発明の実施形態のナビゲーションシステムの構成および動作について説明する。
【００１５】
図１は、本実施形態のナビゲーションシステム１００の概略構成を示すブロック図である。ナビゲーションシステム１００は自動車に搭載された車載器であり、自律航法システムとＧＰＳとを複合して自動車の走行状態を推定し、推定結果に基づき搭乗者にナビゲーション情報を提供する運転支援装置である。図１に示されるように、ナビゲーションシステム１００は、ＣＰＵ（Central Processing Unit）１０、ＲＯＭ（Read-Only Memory）２０、ＲＡＭ（Random Access Memory）２２、ＧＰＳレシーバ３０、ジャイロセンサ４２、加速度センサ４４、Ａ／Ｄ変換器４６、４８Ｘ、４８Ｙ、４８Ｚ、ゲートアレイ５０、車速センサ５２、ＨＤＤ（Hard Disk Drive）５４、ディスプレイ５６、および入力部５８を有している。
【００１６】
ＣＰＵ１０は、ナビゲーションシステム１００全体を統括的に制御する。ＣＰＵ１０は、ナビゲーションシステム１００の電源投入後、必要なハードウェアにアクセスする。例えばナビゲーションシステム１００の電源投入直後、ＣＰＵ１０は、ＲＯＭ２０にアクセスして、ＲＯＭ２０に格納された地図表示プログラムをはじめとするナビゲーションに必要な各種プログラムをＲＡＭ２２にロードして起動する。ＣＰＵ１０は、各種プログラムを起動後、起動されたプログラムを実行する。
【００１７】
また、ＣＰＵ１０は、サンプリング周期Ｔｔ毎（５Ｈｚ周期）に後述する演算に使用される定数Ｎ（整数値）をＧＰＳレシーバ３０とゲートアレイ５０に同時に出力する。ＧＰＳレシーバ３０は、定数Ｎの入力タイミングに従い、複数のＧＰＳ衛星からＧＰＳ測位に必要な数以上のＧＰＳ信号を捕捉、追尾してＧＰＳ測位を行う。そして、演算されたＧＰＳ測位データを５Ｈｚ周期でＣＰＵ１０（後述するカルマンフィルタ１４）に出力する。
【００１８】
ジャイロセンサ４２および三軸加速度センサ４４は、自律航法を行うための観測データを出力する周知のＤＲセンサである。ジャイロセンサ４２は、カーナビゲーションシステム１００を搭載した自動車の水平面における方位に関する角速度を検出してＡ／Ｄ変換器４６に出力する。三軸加速度センサ１０６は、加速度の三軸（Ｘ、Ｙ、Ｚ）成分を検出し、検出されたＸ、Ｙ、Ｚの各軸成分をＡ／Ｄ変換器４８Ｘ、４８Ｙ、４８Ｚに出力する。
【００１９】
Ａ／Ｄ変換器４６、４８Ｘ、４８Ｙおよび４８Ｚは、後段のゲートアレイ５０から出力されるクロックに同期して、検出された観測データを例えば１００Ｈｚ周期でサンプリングして出力する。以下、各Ａ／Ｄ変換器がサンプリングを行う周期（本実施形態においては１００Ｈｚ）を「サンプリング周期Ｔｓ」と記す。
【００２０】
なお、ナビゲーションシステム１００は、Ａ／Ｄ変換器の個数を削減するために、マルチプレクサを有する構成であってもよい。かかる構成においては、マルチプレクサは、ジャイロセンサ４２および三軸加速度センサ４４の後段に配置される。マルチプレクサは、ジャイロセンサ４２および三軸加速度センサ４４から出力される４系統の観測データを時分割多重して一系統の信号に変換して出力する。このように、観測データは一系統の信号に多重化されて出力されるため、観測データのサンプリングに必要なＡ／Ｄ変換器は、マルチプレクサの後段に１つ配置するだけでよい。Ａ／Ｄ変換器の個数を削減することにより、Ａ／Ｄ変換器に与える高周波クロックに起因するクロストークやノイズの発生を低減させることができる。なお、ジャイロセンサ４２および三軸加速度センサ４４として観測値をシリアルデータで出力するタイプのものを使用する場合には、各種センサの後段にＡ／Ｄ変換器を配置する必要はない。
【００２１】
図２は、ゲートアレイ５０およびＣＰＵ１０の構成を模式的に示すブロック図である。図２に示されるように、ゲートアレイ５０には、Ａ／Ｄ変換器４６、４８Ｘ、４８Ｙおよび４８Ｚによりサンプリングされた各種観測データが入力される。
【００２２】
ゲートアレイ５０は、各種観測データに対応した４系統の演算回路５０Ａ〜５０Ｄを有している。具体的には、演算回路５０Ａにはジャイロセンサ４２からの観測データが、演算回路５０Ｂには三軸加速度センサ４４からのＸ軸成分の観測データが、演算回路５０Ｃには三軸加速度センサ４４からのＹ軸成分の観測データが、演算回路５０Ｄには三軸加速度センサ４４からのＺ軸成分の観測データが、それぞれ入力される。本実施形態においては、演算回路５０Ａ〜５０Ｄは、全て同じ回路構成を有している。演算回路５０Ａ〜５０Ｄは、入力された観測データを用いて整数演算を行い、一次加工データ（値Ｓ、値Ｄ）を生成する。
【００２３】
図３に、演算回路５０Ａで行われる信号処理をフローチャートを用いて概略的に示す。なお、他の演算回路５０Ｂ〜５０Ｄにおいても演算回路５０Ａと同様の信号処理が行われるため、演算回路５０Ｂ〜５０Ｄの信号処理の説明は図３のフローチャートの説明をもって省略する。また、図３のフローチャートおよび以下の説明において、ステップを「Ｓ」と略記する。
【００２４】
演算回路５０Ａには、ＣＰＵ１０から出力された上記定数Ｎがサンプリング周期Ｔｔ毎に入力される。なお、定数Ｎは、Ｔｔ／Ｔｓの整数部（商）であり、ＣＰＵ１０による１サンプリング周期Ｔｔ中に行われる各Ａ／Ｄ変換器によるサンプリング回数を示す。演算回路５０Ａは、図３に示されるように、ＣＰＵ１０から定数Ｎが入力されると（Ｓ１：ＹＥＳ）、値Ｓ、値Ｄ、カウント値ｉを０にリセットする（Ｓ２）。
【００２５】
また、演算回路５０Ａには、Ａ／Ｄ変換器４６によりサンプリングされた観測データ（整数値である観測値ｇ）がサンプリング周期Ｔｓ毎に入力される。演算回路５０Ａは、Ａ／Ｄ変換器４６から出力された観測値ｇを検出すると（Ｓ３：ＹＥＳ）、観測値ｇを値Ｓに加算するとともに、観測値ｇとカウント値ｉとの積を値Ｄに加算する（Ｓ４）。次いで、カウント値ｉを１インクリメントしてカウント値ｉと定数Ｎとを比較演算し（Ｓ５、Ｓ６）、カウント値ｉが定数Ｎに達するまでＳ３〜Ｓ６の処理を繰り返し実行する。かかる処理により計算される値Ｓおよび値Ｄは、以下の関数により定義される。
【数１】
【００２６】
演算回路５０Ａは、計算された値Ｓおよび値ＤをＣＰＵ１０に出力する（Ｓ７）。そして、次回の定数Ｎの入力をトリガーとしてＳ２〜Ｓ７の処理を再度実行する。
【００２７】
演算回路５０Ａ〜５０Ｄから出力された値Ｓおよび値Ｄは、一次近似式演算部１２に入力される。なお、一次近似式演算部１２は、実際には演算用プログラムがロードされたＲＡＭ２２とＣＰＵ１０との協働によって実現されるが、図２においては説明の便宜上、ＣＰＵ１０内の機能ブロックとして図示されている。カルマンフィルタ１４も同様の理由によりＣＰＵ１０内の機能ブロックとして図示されている。
【００２８】
一次近似式演算部１２は、各演算回路５０Ａ〜５０Ｄにより演算された値Ｓおよび値Ｄを入力パラメータとして、サンプリング周期Ｔｔ内に検出されたＮ個の観測値ｇの組についての一次関数近似式（ｙ＝ａｘ＋ｂ）を演算する。なお、一次関数近似式のｘは時間軸に対応し、ｙは観測値ｇに対応する。すなわち、一次近似式演算部１２は、ジャイロセンサ４２と三軸加速度センサ４４の各軸成分に対応する計４種類の一次関数近似式をサンプリング周期Ｔｔ毎に演算する。そして、近似演算処理により計算された傾きａおよび切片ｂを自律航法システムによる測位データとしてカルマンフィルタ１４に出力する。
【００２９】
傾きａおよび切片ｂは、各サンプリング周期Ｔｔ内の最後（Ｎ個目）の観測値ｇが検出されたタイミングをｙ軸（ｘ＝０）としたときの値である。すなわち、例えばジャイロセンサ４２の観測データに基づき計算された傾きａ、切片ｂはそれぞれ、各サンプリング周期Ｔｔ内の最後の観測値ｇが検出された時点における自動車の角加速度、角速度を表している。ＧＰＳ測位データは該時点から例えば数μｍ秒（最大でもサンプリング周期Ｔｓ）後に演算され出力されるため、傾きａおよび切片ｂは、ＧＰＳ測位データとほぼ同期のとれた位置、方位、移動速度等を表す自律航法データといえる。つまり、ナビゲーションシステム１００において、自律航法システムとＧＰＳの各方式による位置、方位、移動速度等の測位データは、ほぼ同期のとれた状態でカルマンフィルタ１４に入力されている。
【００３０】
一次近似式演算部１２は、値Ｓおよび値Ｄを用いて浮動小数点演算を行い、以下に定義される数式により傾きａおよび切片ｂを計算する。値Ｒ、値Ｑおよび値Ｐは、定数Ｎおよびカウント値ｉに従って決まる値であり、一次近似式演算部１２により予め計算されている。
【数２】
【００３１】
このようにナビゲーションシステム１００においては、単純で負荷の軽い整数演算を演算回路５０Ａ〜５０Ｄに高速レート（サンプリング周期Ｔｓ）で実行させ、複雑で負荷の重い浮動小数点演算を一次近似式演算部１２に低速レート（サンプリング周期Ｔｔ）で実行させている。つまり、最小二乗法を用いて一次関数近似式を決定するにあたり、単純であるが高速レートを必要とする演算処理部分をハードウェア（演算回路５０Ａ〜５０Ｄ）に負担させ、複雑であるが低速レートで十分な演算処理部分をソフトウェア（一次近似式演算部１２）に負担させている。そのため、ＣＰＵ１０の性能が低い場合であっても、有効桁数の多い高精度な傾きａおよび切片ｂを計算することができる。
【００３２】
また、傾きａおよび切片ｂの計算に最小二乗法を適用することにより、各ＤＲセンサの観測値が平滑化されてノイズ成分が効果的に除去される。
【００３３】
図４に、一次近似式演算部１２により計算される傾きａおよび切片ｂを説明するための図を示す。図４の横軸（ｘ軸）は時間軸であり、縦軸（ｙ軸）は観測値ｇに対応する軸である。各時間ＴＮ１〜ＴＮ４は、ＣＰＵ１０がＧＰＳレシーバ３０と各演算回路５０Ａ〜５０Ｄに定数Ｎを出力する、サンプリング周期Ｔｔに対応するタイミングを示している。ＧＰＳレシーバ３０は、例えば時間ＴＮ１に出力された定数Ｎに応答してＧＰＳ測位データを演算し、演算されたＧＰＳ測位データを１サンプリング周期Ｔｔ（０．２秒）後の時間ＴＮ２にＣＰＵ１０に出力する。つまり、各時間ＴＮ１〜ＴＮ４は、ＣＰＵ１０が定数Ｎを出力するタイミングであるとともに、ＧＰＳレシーバ３０がＧＰＳ測位データを出力するタイミングでもある。
【００３４】
図４の各プロットは、Ａ／Ｄ変換器４６（または４８Ｘ、４８Ｙ、４８Ｚ）によりサンプリングされた観測データ（観測値ｇ）を示す。そのため、各プロットの時間間隔は、各Ａ／Ｄ変換器の１サンプリング周期Ｔｓ（０．０１秒）に相当する。図４に示されるＣＰＵ１０による１サンプリング周期Ｔｔ内には２０個のプロットが打点されている。
【００３５】
例えば時間ＴＮ１からＴＮ２までの期間において、演算回路５０Ａは、時間ＴＮ１に出力された定数Ｎの入力をトリガーとして図３のフローチャートに示される信号処理を開始し、値Ｓと値Ｄを計算して各プロットのタイミングで一次近似式演算部１２に出力する。一次近似式演算部１２は、入力された値Ｓと値Ｄを用いて傾きａ１と切片ｂ１（一次関数近似式ｆ１）を計算し、カルマンフィルタ１４に出力する。
【００３６】
ところで、演算回路５０Ａが２０個目の観測値ｇｆを検出してから値Ｓと値Ｄを計算するまでにかかる処理時間、ＣＰＵ１０への値Ｓと値Ｄの伝送時間、および一次近似式演算部１２における傾きａ１および切片ｂ１の計算時間を合計するとおよそ時間Ｔｄになる。一次近似式演算部１２は、ＧＰＳ測位データと同期のとれた観測データを出力するために、実際には遅延時間ｔｄを考慮して切片ｂ１を計算している。つまり、切片ｂ１は、各サンプリング周期Ｔｔ内の最後の観測値ｇが検出されてから遅延時間ｔｄ経過したタイミング（別の表現によれば、傾きａ１および切片ｂ１が計算される時刻であって、より正確には該時刻より僅かに進んだＧＰＳ測位データが演算される時刻とほぼ同時刻）をｙ軸としたときの値である。図４においては、遅延時間ｔｄ経過後のタイミングが時間ＴＮ２とほぼ一致している。なお、上記数式（数１、数２）は、傾きａおよび切片ｂの概略的な計算方法を説明するため、遅延時間ｔｄを考慮しない簡略化された数式となっている。
【００３７】
カルマンフィルタ１４には、ＧＰＳ測位データ、および該ＧＰＳ測位データと同期のとれた自律航法データ（つまり各種観測値に対応する傾きａおよび切片ｂ）がサンプリング周期Ｔｔ毎に入力されるのに加えて、さらに、車速センサ５２から走行速度に応じたレートで車速パルスが入力される。カルマンフィルタ１４は、自律航法データおよび車速パルスと、ＧＰＳ測位データとを比較して、最終的な測位データを決定する。
【００３８】
カルマンフィルタ１４は、ＧＰＳレシーバ３０や各種ＤＲセンサによって取得された誤差を含む観測データに基づき、自動車の位置、方位、移動速度等の走行状態に関する情報を推定する。ここで、図５を用い、自動車の方位を例に挙げてカルマンフィルタ１４による推定演算処理を説明する。
【００３９】
図５に示されるように、カルマンフィルタ１４には、ＧＰＳレシーバ３０から出力された方位θｎと、該方位θｎとほぼ同期のとれた角速度（切片ｂ）とがサンプリング周期Ｔｔ毎に入力される。カルマンフィルタ１４は、切片ｂに基づき自律航法による方位を計算し、計算された方位とＧＰＳレシーバ３０から出力された方位θｎとを比較する。そして、比較結果に基づき推定誤差を計算して最終的な方位θｎ’を推定し、出力する。切片ｂと方位θｎはほぼ同期がとれたデータであるため、遅延に起因する測定誤差が実質的に０となる。そのため、カルマンフィルタ１４は、最終的な方位θｎ’を高精度に演算することができる。なお、図５においては説明の便宜上、カルマンフィルタ１４の入出力信号を方位に関する信号に限定しているが、実際には位置や移動速度等に関する他の入出力信号も存在する。
【００４０】
また、切片ｂだけでなく、傾きａをカルマンフィルタに入力してもよい。傾きａは、切片ｂの示すパラメータの時間微分であり、パラメータの直近の時間変動に関する有用な情報となる。また、傾きａと切片ｂにより、簡単な台形面積の計算式からパラメータの時間積分を求めることができる。例えば、ジャイロセンサ４２による角速度データに基づく一次近似式演算部１２の出力（切片ｂ、傾きａ）から、サンプリング周期Ｔｔにおける方位θの変化量を次式で計算することができる。このように傾きａを利用することによって、パラメータの時間微分や時間積分の計算に必要となるＣＰＵの演算量を大幅に削減することができる。
【数３】
【００４１】
ＣＰＵ１０は、カルマンフィルタ１４から出力された最終的な測位データに基づきＨＤＤ５４に格納されている地図データベースを検索し、対応する地図画像を抽出する。ＣＰＵ１０は、最後にマップマッチングを行い、抽出された地図画像に自車位置マークを重畳してディスプレイ５６に表示させる。このときユーザが入力部５８を操作して目的地を設定している場合、ＣＰＵ１０はその目的地に向けたナビゲーション処理も行う。
【００４２】
以上のように、ナビゲーションシステム１００においては、ＣＰＵ１０側のソフトウェア処理（一次近似式演算部１２およびカルマンフィルタ１４による処理）がダウンサンプリングされたレートで実行されている。そのため、一次近似式演算部１２およびカルマンフィルタ１４における演算処理量が軽減する。別の観点によれば、桁数の多い高精度な演算処理をＣＰＵ１０に実行させることができる。
【００４３】
また、ＧＰＳ測位データと自律航法データ（傾きａおよび切片ｂ）との同期をほぼ完全にとり、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償したことにより、カルマンフィルタ１４による推定演算処理の精度が向上する。そのため、正確なナビゲーション情報を搭乗者に提供することができる。
【００４４】
本発明は上記実施形態に限定されるものではなく、本発明の技術的思想の範囲において様々な変形が可能である。本発明は、例えば本実施形態のカーナビゲーションシステムに限らず、地図データベースを保持しない簡易ナビゲーションシステムであるＰＮＤ（Personal Navigation Device）や、携帯電話等のモバイル端末等にも適用することができる。
【図面の簡単な説明】
【００４５】
【図１】本発明の実施形態のナビゲーションシステムの概略構成を示すブロック図である。
【図２】本発明の実施形態のゲートアレイおよびＣＰＵの構成を模式的に示すブロック図である。
【図３】本発明の実施形態の演算回路で行われる信号処理を概略的に示したフローチャート図である。
【図４】本発明の実施形態の一次近似式演算部により計算される傾きａおよび切片ｂを説明するための図である。
【図５】本発明の実施形態のカルマンフィルタによる推定演算処理を説明するためのブロック図である。
【符号の説明】
【００４６】
１０ ＣＰＵ
１２ 一次近似式演算部
１４ カルマンフィルタ
３０ ＧＰＳレシーバ
４２ ジャイロセンサ
４４ 加速度センサ
５０ ゲートアレイ
５０Ａ〜５０Ｄ 演算回路
１００ ナビゲーションシステム
【技術分野】
【０００１】
この発明は、走行状態に関する各種観測を行う自律航法システムとＧＰＳ（Global Positioning System）とを複合して対象物の走行状態を推定する走行状態推定システムに関連し、詳しくは、各観測システムによって取得される観測データの同期をとり対象物の走行状態を高精度に推定する走行状態推定システムに関する。
【背景技術】
【０００２】
ＧＰＳは、地球を周回するＧＰＳ衛星から発信されるＧＰＳ信号を用いて対象物の位置情報等を測定するための測位システムであり、日常生活においても広く利用されている。
【０００３】
ＧＰＳでは、ＣＤＭＡ（Code Division Multiple Access）方式によって複数のＧＰＳ衛星が同じ周波数帯を共用してＧＰＳ信号を送信する。具体的には、ＧＰＳ信号は、クロック情報やＧＰＳ衛星の軌道情報を含む通信信号としての航法メッセージと、ＧＰＳ衛星毎に定められた拡散符号としてのＰＲＮコード（Pseudo Random Noise code）とにより、１５７５．４２ＭＨｚのキャリアをＢＰＳＫ（Binary Phase Shift Keying）変調したものである。ＧＰＳレシーバは、原理上必要とされる個数以上のＧＰＳ衛星を選択し、選択されたＧＰＳ衛星に対応するＰＲＮコードを生成し、生成されたＰＲＮコードの位相をＧＰＳ信号に同期させて乗じることにより航法データを復調する。そして、復調して得られた航法データを用いて演算を行い、対象物の位置情報、方位情報、移動速度等を生成する。以下、ＧＰＳレシーバによるＧＰＳ信号を利用した位置測位、方位測定、移動速度測定等を「ＧＰＳ測位」と記す。
【０００４】
ＧＰＳレシーバを備えた一般消費者向け製品には、例えば乗用車に搭載されるカーナビゲーションシステムがある。かかるカーナビゲーションシステムの一例が、特許文献１に開示されている。特許文献１に示されるように、一般的なカーナビゲーションシステムは、ＧＰＳ測位と並行して、ジャイロセンサ、速度センサ等の各種自律航法用センサ（以下、「ＤＲ（dead reckoning）センサ」と記す。）から出力される観測データを用いてカルマンフィルタにより位置推定、方位推定、移動速度推定等を行う。次いで、統合処理回路により、各測位方式の弱点を相互に補完するように、両方式による測位データに基づいた演算により最終的な測位データが決定される。カーナビゲーションシステムは、演算して得られた測位データに基づき、画面上に表示させる自車位置を更新する。また、ユーザによって目的地が設定されている場合には、画面上の自車位置の更新と併せて当該目的地に向けたナビゲーションを行う。
【特許文献１】特開２００８−１４８１０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ところで、特許文献１に記載のカルマンフィルタは、ＤＲセンサから観測データが入力される毎に位置推定等を逐次行い、推定データを統合処理回路に出力している。すなわち、カルマンフィルタによる推定データの出力レートは、ＤＲセンサによる観測データの出力サンプリングレート（例えば１００Ｈｚ）に応じて決定されている。一方、ユニット化された一般的なＧＰＳレシーバは、位相積算時間に関わる精度の関係から、ＤＲセンサによる観測データの出力サンプリングレートと同じレートでＧＰＳ測位データを高速に演算することができない。ＧＰＳレシーバは、カルマンフィルタによる推定演算より遙かに低速なレートでＧＰＳ測位データを演算し統合処理回路に出力している。
【０００６】
このようにＧＰＳ測位データとカルマンフィルタによる推定データのサンプリングレートが相違する場合、両データの同期を正確にとり続けることが難しく、各データ間に遅延が生じる問題がある。この場合、統合処理回路は、観測時点が相違する（つまり非同期な）データに基づき測位データを演算することになる。そのため、最終的に決定される測位データの精度低下が避けられない問題が生じる。
【０００７】
また、ＤＲセンサによる観測データの出力サンプリングレートは高速である。そのため、カルマンフィルタにおける演算処理量が膨大になるといった問題も指摘されている。特に、廉価仕様のカーナビゲーションシステムでは演算処理回路の性能が低いため、演算処理回路のプロセスがカルマンフィルタによる演算処理によって専有されることがある。この場合、システム全体の処理速度が著しく低下する弊害が生じる。
【０００８】
そこで、ＤＲセンサから出力される観測データをダウンサンプリングすることにより、カルマンフィルタにおける演算処理量を軽減させる回路設計が考えられる。ところが、この場合、カルマンフィルタによる推定データのサンプリング数が減少するため、該推定データの精度が低下する。その結果、結局は、最終的に決定される測位データの精度を低下させる弊害が生じる。
【０００９】
本発明はこのような事情に鑑みてなされたものであり、その目的とするところは、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償して、最終的に決定される測位データの精度向上を図ることができる走行状態推定システムを提供することにある。また、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償しつつもカルマンフィルタ等によるソフトウェア処理の負担を軽減させることができる走行状態推定システムを提供することにある。
【課題を解決するための手段】
【００１０】
上記の課題を解決する本発明の一形態に係る走行状態推定システムは、所定の推定手段により自律航法システムによる自律航法データとＧＰＳによる測位データとを複合して対象物の走行状態を推定するシステムであり、以下の特徴を有している。すなわち、該走行状態推定システムは、ＧＰＳ信号に基づき第一のサンプリング周期で測位データを演算して推定手段に出力するＧＰＳ測位演算手段と、第一のサンプリング周期より短い第二のサンプリング周期で観測データを出力する自律航法用センサと、第一のサンプリング周期内に自律航法用センサから出力された観測データの組を所定の一次関数に近似する近似式演算手段とを有している。
【００１１】
そして、走行状態推定システムが有する近似式演算手段は、推定手段により演算される測位データの精度を向上させるため、ＧＰＳ測位データと同期のとれた自律航法データを生成して推定手段に出力する。具体的には、近似式演算手段は、測位データと略同期したタイミングにおける一次関数の近似式の値を切片の値とし、該切片の値を該近似式の傾きの値とともに自律航法データとして推定手段に出力する。近似式演算手段による信号処理により、自律航法システムとＧＰＳの各方式間の信号の遅延時間が高精度に補償されるため、推定手段により演算される測位データが向上する。
【００１２】
近似式演算手段は、好ましくは、第一のサンプリング周期内に自律航法用センサから出力された複数の観測データを用いて所定の整数演算を行い、所定の加工データを生成して第一のサンプリング周期で出力する一次データ生成手段と、加工データ生成手段から出力された加工データに基づき近似式を浮動小数点演算により計算する二次データ生成手段とを有するように構成される。この場合、一次データ生成手段をハードウェアで構成し、二次データ生成手段をソフトウェアで構成することが望ましい。かかる構成によれば、第二のサンプリング周期から第一のサンプリング周期にダウンサンプリングされたデータを二次データ生成手段に処理させることができる。そのため、二次データ生成手段における演算処理量が軽減される。別の観点によれば、桁数の多い高精度な演算処理を二次データ生成手段に実行させることができる。
【発明の効果】
【００１３】
本発明の走行状態推定システムによれば、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償して最終的に決定される測位データの精度を向上させることができる。また、かかる効果に加えて、ソフトウェアにおける演算処理量を軽減させることができる。
【発明を実施するための最良の形態】
【００１４】
以下、図面を参照して、本発明の実施形態のナビゲーションシステムの構成および動作について説明する。
【００１５】
図１は、本実施形態のナビゲーションシステム１００の概略構成を示すブロック図である。ナビゲーションシステム１００は自動車に搭載された車載器であり、自律航法システムとＧＰＳとを複合して自動車の走行状態を推定し、推定結果に基づき搭乗者にナビゲーション情報を提供する運転支援装置である。図１に示されるように、ナビゲーションシステム１００は、ＣＰＵ（Central Processing Unit）１０、ＲＯＭ（Read-Only Memory）２０、ＲＡＭ（Random Access Memory）２２、ＧＰＳレシーバ３０、ジャイロセンサ４２、加速度センサ４４、Ａ／Ｄ変換器４６、４８Ｘ、４８Ｙ、４８Ｚ、ゲートアレイ５０、車速センサ５２、ＨＤＤ（Hard Disk Drive）５４、ディスプレイ５６、および入力部５８を有している。
【００１６】
ＣＰＵ１０は、ナビゲーションシステム１００全体を統括的に制御する。ＣＰＵ１０は、ナビゲーションシステム１００の電源投入後、必要なハードウェアにアクセスする。例えばナビゲーションシステム１００の電源投入直後、ＣＰＵ１０は、ＲＯＭ２０にアクセスして、ＲＯＭ２０に格納された地図表示プログラムをはじめとするナビゲーションに必要な各種プログラムをＲＡＭ２２にロードして起動する。ＣＰＵ１０は、各種プログラムを起動後、起動されたプログラムを実行する。
【００１７】
また、ＣＰＵ１０は、サンプリング周期Ｔｔ毎（５Ｈｚ周期）に後述する演算に使用される定数Ｎ（整数値）をＧＰＳレシーバ３０とゲートアレイ５０に同時に出力する。ＧＰＳレシーバ３０は、定数Ｎの入力タイミングに従い、複数のＧＰＳ衛星からＧＰＳ測位に必要な数以上のＧＰＳ信号を捕捉、追尾してＧＰＳ測位を行う。そして、演算されたＧＰＳ測位データを５Ｈｚ周期でＣＰＵ１０（後述するカルマンフィルタ１４）に出力する。
【００１８】
ジャイロセンサ４２および三軸加速度センサ４４は、自律航法を行うための観測データを出力する周知のＤＲセンサである。ジャイロセンサ４２は、カーナビゲーションシステム１００を搭載した自動車の水平面における方位に関する角速度を検出してＡ／Ｄ変換器４６に出力する。三軸加速度センサ１０６は、加速度の三軸（Ｘ、Ｙ、Ｚ）成分を検出し、検出されたＸ、Ｙ、Ｚの各軸成分をＡ／Ｄ変換器４８Ｘ、４８Ｙ、４８Ｚに出力する。
【００１９】
Ａ／Ｄ変換器４６、４８Ｘ、４８Ｙおよび４８Ｚは、後段のゲートアレイ５０から出力されるクロックに同期して、検出された観測データを例えば１００Ｈｚ周期でサンプリングして出力する。以下、各Ａ／Ｄ変換器がサンプリングを行う周期（本実施形態においては１００Ｈｚ）を「サンプリング周期Ｔｓ」と記す。
【００２０】
なお、ナビゲーションシステム１００は、Ａ／Ｄ変換器の個数を削減するために、マルチプレクサを有する構成であってもよい。かかる構成においては、マルチプレクサは、ジャイロセンサ４２および三軸加速度センサ４４の後段に配置される。マルチプレクサは、ジャイロセンサ４２および三軸加速度センサ４４から出力される４系統の観測データを時分割多重して一系統の信号に変換して出力する。このように、観測データは一系統の信号に多重化されて出力されるため、観測データのサンプリングに必要なＡ／Ｄ変換器は、マルチプレクサの後段に１つ配置するだけでよい。Ａ／Ｄ変換器の個数を削減することにより、Ａ／Ｄ変換器に与える高周波クロックに起因するクロストークやノイズの発生を低減させることができる。なお、ジャイロセンサ４２および三軸加速度センサ４４として観測値をシリアルデータで出力するタイプのものを使用する場合には、各種センサの後段にＡ／Ｄ変換器を配置する必要はない。
【００２１】
図２は、ゲートアレイ５０およびＣＰＵ１０の構成を模式的に示すブロック図である。図２に示されるように、ゲートアレイ５０には、Ａ／Ｄ変換器４６、４８Ｘ、４８Ｙおよび４８Ｚによりサンプリングされた各種観測データが入力される。
【００２２】
ゲートアレイ５０は、各種観測データに対応した４系統の演算回路５０Ａ〜５０Ｄを有している。具体的には、演算回路５０Ａにはジャイロセンサ４２からの観測データが、演算回路５０Ｂには三軸加速度センサ４４からのＸ軸成分の観測データが、演算回路５０Ｃには三軸加速度センサ４４からのＹ軸成分の観測データが、演算回路５０Ｄには三軸加速度センサ４４からのＺ軸成分の観測データが、それぞれ入力される。本実施形態においては、演算回路５０Ａ〜５０Ｄは、全て同じ回路構成を有している。演算回路５０Ａ〜５０Ｄは、入力された観測データを用いて整数演算を行い、一次加工データ（値Ｓ、値Ｄ）を生成する。
【００２３】
図３に、演算回路５０Ａで行われる信号処理をフローチャートを用いて概略的に示す。なお、他の演算回路５０Ｂ〜５０Ｄにおいても演算回路５０Ａと同様の信号処理が行われるため、演算回路５０Ｂ〜５０Ｄの信号処理の説明は図３のフローチャートの説明をもって省略する。また、図３のフローチャートおよび以下の説明において、ステップを「Ｓ」と略記する。
【００２４】
演算回路５０Ａには、ＣＰＵ１０から出力された上記定数Ｎがサンプリング周期Ｔｔ毎に入力される。なお、定数Ｎは、Ｔｔ／Ｔｓの整数部（商）であり、ＣＰＵ１０による１サンプリング周期Ｔｔ中に行われる各Ａ／Ｄ変換器によるサンプリング回数を示す。演算回路５０Ａは、図３に示されるように、ＣＰＵ１０から定数Ｎが入力されると（Ｓ１：ＹＥＳ）、値Ｓ、値Ｄ、カウント値ｉを０にリセットする（Ｓ２）。
【００２５】
また、演算回路５０Ａには、Ａ／Ｄ変換器４６によりサンプリングされた観測データ（整数値である観測値ｇ）がサンプリング周期Ｔｓ毎に入力される。演算回路５０Ａは、Ａ／Ｄ変換器４６から出力された観測値ｇを検出すると（Ｓ３：ＹＥＳ）、観測値ｇを値Ｓに加算するとともに、観測値ｇとカウント値ｉとの積を値Ｄに加算する（Ｓ４）。次いで、カウント値ｉを１インクリメントしてカウント値ｉと定数Ｎとを比較演算し（Ｓ５、Ｓ６）、カウント値ｉが定数Ｎに達するまでＳ３〜Ｓ６の処理を繰り返し実行する。かかる処理により計算される値Ｓおよび値Ｄは、以下の関数により定義される。
【数１】
【００２６】
演算回路５０Ａは、計算された値Ｓおよび値ＤをＣＰＵ１０に出力する（Ｓ７）。そして、次回の定数Ｎの入力をトリガーとしてＳ２〜Ｓ７の処理を再度実行する。
【００２７】
演算回路５０Ａ〜５０Ｄから出力された値Ｓおよび値Ｄは、一次近似式演算部１２に入力される。なお、一次近似式演算部１２は、実際には演算用プログラムがロードされたＲＡＭ２２とＣＰＵ１０との協働によって実現されるが、図２においては説明の便宜上、ＣＰＵ１０内の機能ブロックとして図示されている。カルマンフィルタ１４も同様の理由によりＣＰＵ１０内の機能ブロックとして図示されている。
【００２８】
一次近似式演算部１２は、各演算回路５０Ａ〜５０Ｄにより演算された値Ｓおよび値Ｄを入力パラメータとして、サンプリング周期Ｔｔ内に検出されたＮ個の観測値ｇの組についての一次関数近似式（ｙ＝ａｘ＋ｂ）を演算する。なお、一次関数近似式のｘは時間軸に対応し、ｙは観測値ｇに対応する。すなわち、一次近似式演算部１２は、ジャイロセンサ４２と三軸加速度センサ４４の各軸成分に対応する計４種類の一次関数近似式をサンプリング周期Ｔｔ毎に演算する。そして、近似演算処理により計算された傾きａおよび切片ｂを自律航法システムによる測位データとしてカルマンフィルタ１４に出力する。
【００２９】
傾きａおよび切片ｂは、各サンプリング周期Ｔｔ内の最後（Ｎ個目）の観測値ｇが検出されたタイミングをｙ軸（ｘ＝０）としたときの値である。すなわち、例えばジャイロセンサ４２の観測データに基づき計算された傾きａ、切片ｂはそれぞれ、各サンプリング周期Ｔｔ内の最後の観測値ｇが検出された時点における自動車の角加速度、角速度を表している。ＧＰＳ測位データは該時点から例えば数μｍ秒（最大でもサンプリング周期Ｔｓ）後に演算され出力されるため、傾きａおよび切片ｂは、ＧＰＳ測位データとほぼ同期のとれた位置、方位、移動速度等を表す自律航法データといえる。つまり、ナビゲーションシステム１００において、自律航法システムとＧＰＳの各方式による位置、方位、移動速度等の測位データは、ほぼ同期のとれた状態でカルマンフィルタ１４に入力されている。
【００３０】
一次近似式演算部１２は、値Ｓおよび値Ｄを用いて浮動小数点演算を行い、以下に定義される数式により傾きａおよび切片ｂを計算する。値Ｒ、値Ｑおよび値Ｐは、定数Ｎおよびカウント値ｉに従って決まる値であり、一次近似式演算部１２により予め計算されている。
【数２】
【００３１】
このようにナビゲーションシステム１００においては、単純で負荷の軽い整数演算を演算回路５０Ａ〜５０Ｄに高速レート（サンプリング周期Ｔｓ）で実行させ、複雑で負荷の重い浮動小数点演算を一次近似式演算部１２に低速レート（サンプリング周期Ｔｔ）で実行させている。つまり、最小二乗法を用いて一次関数近似式を決定するにあたり、単純であるが高速レートを必要とする演算処理部分をハードウェア（演算回路５０Ａ〜５０Ｄ）に負担させ、複雑であるが低速レートで十分な演算処理部分をソフトウェア（一次近似式演算部１２）に負担させている。そのため、ＣＰＵ１０の性能が低い場合であっても、有効桁数の多い高精度な傾きａおよび切片ｂを計算することができる。
【００３２】
また、傾きａおよび切片ｂの計算に最小二乗法を適用することにより、各ＤＲセンサの観測値が平滑化されてノイズ成分が効果的に除去される。
【００３３】
図４に、一次近似式演算部１２により計算される傾きａおよび切片ｂを説明するための図を示す。図４の横軸（ｘ軸）は時間軸であり、縦軸（ｙ軸）は観測値ｇに対応する軸である。各時間ＴＮ１〜ＴＮ４は、ＣＰＵ１０がＧＰＳレシーバ３０と各演算回路５０Ａ〜５０Ｄに定数Ｎを出力する、サンプリング周期Ｔｔに対応するタイミングを示している。ＧＰＳレシーバ３０は、例えば時間ＴＮ１に出力された定数Ｎに応答してＧＰＳ測位データを演算し、演算されたＧＰＳ測位データを１サンプリング周期Ｔｔ（０．２秒）後の時間ＴＮ２にＣＰＵ１０に出力する。つまり、各時間ＴＮ１〜ＴＮ４は、ＣＰＵ１０が定数Ｎを出力するタイミングであるとともに、ＧＰＳレシーバ３０がＧＰＳ測位データを出力するタイミングでもある。
【００３４】
図４の各プロットは、Ａ／Ｄ変換器４６（または４８Ｘ、４８Ｙ、４８Ｚ）によりサンプリングされた観測データ（観測値ｇ）を示す。そのため、各プロットの時間間隔は、各Ａ／Ｄ変換器の１サンプリング周期Ｔｓ（０．０１秒）に相当する。図４に示されるＣＰＵ１０による１サンプリング周期Ｔｔ内には２０個のプロットが打点されている。
【００３５】
例えば時間ＴＮ１からＴＮ２までの期間において、演算回路５０Ａは、時間ＴＮ１に出力された定数Ｎの入力をトリガーとして図３のフローチャートに示される信号処理を開始し、値Ｓと値Ｄを計算して各プロットのタイミングで一次近似式演算部１２に出力する。一次近似式演算部１２は、入力された値Ｓと値Ｄを用いて傾きａ１と切片ｂ１（一次関数近似式ｆ１）を計算し、カルマンフィルタ１４に出力する。
【００３６】
ところで、演算回路５０Ａが２０個目の観測値ｇｆを検出してから値Ｓと値Ｄを計算するまでにかかる処理時間、ＣＰＵ１０への値Ｓと値Ｄの伝送時間、および一次近似式演算部１２における傾きａ１および切片ｂ１の計算時間を合計するとおよそ時間Ｔｄになる。一次近似式演算部１２は、ＧＰＳ測位データと同期のとれた観測データを出力するために、実際には遅延時間ｔｄを考慮して切片ｂ１を計算している。つまり、切片ｂ１は、各サンプリング周期Ｔｔ内の最後の観測値ｇが検出されてから遅延時間ｔｄ経過したタイミング（別の表現によれば、傾きａ１および切片ｂ１が計算される時刻であって、より正確には該時刻より僅かに進んだＧＰＳ測位データが演算される時刻とほぼ同時刻）をｙ軸としたときの値である。図４においては、遅延時間ｔｄ経過後のタイミングが時間ＴＮ２とほぼ一致している。なお、上記数式（数１、数２）は、傾きａおよび切片ｂの概略的な計算方法を説明するため、遅延時間ｔｄを考慮しない簡略化された数式となっている。
【００３７】
カルマンフィルタ１４には、ＧＰＳ測位データ、および該ＧＰＳ測位データと同期のとれた自律航法データ（つまり各種観測値に対応する傾きａおよび切片ｂ）がサンプリング周期Ｔｔ毎に入力されるのに加えて、さらに、車速センサ５２から走行速度に応じたレートで車速パルスが入力される。カルマンフィルタ１４は、自律航法データおよび車速パルスと、ＧＰＳ測位データとを比較して、最終的な測位データを決定する。
【００３８】
カルマンフィルタ１４は、ＧＰＳレシーバ３０や各種ＤＲセンサによって取得された誤差を含む観測データに基づき、自動車の位置、方位、移動速度等の走行状態に関する情報を推定する。ここで、図５を用い、自動車の方位を例に挙げてカルマンフィルタ１４による推定演算処理を説明する。
【００３９】
図５に示されるように、カルマンフィルタ１４には、ＧＰＳレシーバ３０から出力された方位θｎと、該方位θｎとほぼ同期のとれた角速度（切片ｂ）とがサンプリング周期Ｔｔ毎に入力される。カルマンフィルタ１４は、切片ｂに基づき自律航法による方位を計算し、計算された方位とＧＰＳレシーバ３０から出力された方位θｎとを比較する。そして、比較結果に基づき推定誤差を計算して最終的な方位θｎ’を推定し、出力する。切片ｂと方位θｎはほぼ同期がとれたデータであるため、遅延に起因する測定誤差が実質的に０となる。そのため、カルマンフィルタ１４は、最終的な方位θｎ’を高精度に演算することができる。なお、図５においては説明の便宜上、カルマンフィルタ１４の入出力信号を方位に関する信号に限定しているが、実際には位置や移動速度等に関する他の入出力信号も存在する。
【００４０】
また、切片ｂだけでなく、傾きａをカルマンフィルタに入力してもよい。傾きａは、切片ｂの示すパラメータの時間微分であり、パラメータの直近の時間変動に関する有用な情報となる。また、傾きａと切片ｂにより、簡単な台形面積の計算式からパラメータの時間積分を求めることができる。例えば、ジャイロセンサ４２による角速度データに基づく一次近似式演算部１２の出力（切片ｂ、傾きａ）から、サンプリング周期Ｔｔにおける方位θの変化量を次式で計算することができる。このように傾きａを利用することによって、パラメータの時間微分や時間積分の計算に必要となるＣＰＵの演算量を大幅に削減することができる。
【数３】
【００４１】
ＣＰＵ１０は、カルマンフィルタ１４から出力された最終的な測位データに基づきＨＤＤ５４に格納されている地図データベースを検索し、対応する地図画像を抽出する。ＣＰＵ１０は、最後にマップマッチングを行い、抽出された地図画像に自車位置マークを重畳してディスプレイ５６に表示させる。このときユーザが入力部５８を操作して目的地を設定している場合、ＣＰＵ１０はその目的地に向けたナビゲーション処理も行う。
【００４２】
以上のように、ナビゲーションシステム１００においては、ＣＰＵ１０側のソフトウェア処理（一次近似式演算部１２およびカルマンフィルタ１４による処理）がダウンサンプリングされたレートで実行されている。そのため、一次近似式演算部１２およびカルマンフィルタ１４における演算処理量が軽減する。別の観点によれば、桁数の多い高精度な演算処理をＣＰＵ１０に実行させることができる。
【００４３】
また、ＧＰＳ測位データと自律航法データ（傾きａおよび切片ｂ）との同期をほぼ完全にとり、自律航法システムとＧＰＳの各方式間の信号の遅延時間を高精度に補償したことにより、カルマンフィルタ１４による推定演算処理の精度が向上する。そのため、正確なナビゲーション情報を搭乗者に提供することができる。
【００４４】
本発明は上記実施形態に限定されるものではなく、本発明の技術的思想の範囲において様々な変形が可能である。本発明は、例えば本実施形態のカーナビゲーションシステムに限らず、地図データベースを保持しない簡易ナビゲーションシステムであるＰＮＤ（Personal Navigation Device）や、携帯電話等のモバイル端末等にも適用することができる。
【図面の簡単な説明】
【００４５】
【図１】本発明の実施形態のナビゲーションシステムの概略構成を示すブロック図である。
【図２】本発明の実施形態のゲートアレイおよびＣＰＵの構成を模式的に示すブロック図である。
【図３】本発明の実施形態の演算回路で行われる信号処理を概略的に示したフローチャート図である。
【図４】本発明の実施形態の一次近似式演算部により計算される傾きａおよび切片ｂを説明するための図である。
【図５】本発明の実施形態のカルマンフィルタによる推定演算処理を説明するためのブロック図である。
【符号の説明】
【００４６】
１０ ＣＰＵ
１２ 一次近似式演算部
１４ カルマンフィルタ
３０ ＧＰＳレシーバ
４２ ジャイロセンサ
４４ 加速度センサ
５０ ゲートアレイ
５０Ａ〜５０Ｄ 演算回路
１００ ナビゲーションシステム
【特許請求の範囲】
【請求項１】
所定の推定手段により自律航法システムによる自律航法データとＧＰＳ（Global Positioning System）による測位データとを複合して対象物の走行状態を推定する走行状態推定システムにおいて、
ＧＰＳ信号に基づき第一のサンプリング周期で測位データを演算して前記推定手段に出力するＧＰＳ測位演算手段と、
前記第一のサンプリング周期より短い第二のサンプリング周期で観測データを出力する自律航法用センサと、
前記第一のサンプリング周期内に前記自律航法用センサから出力された観測データの組を所定の一次関数に近似する近似式演算手段と、
を有し、
前記近似式演算手段は、前記測位データと略同期したタイミングにおける前記一次関数の近似式の値を切片の値とし、該切片の値を該近似式の傾きの値とともに前記自律航法データとして前記推定手段に出力することを特徴とする走行状態推定システム。
【請求項２】
前記近似式演算手段は、
前記第一のサンプリング周期内に前記自律航法用センサから出力された複数の観測データを用いて所定の整数演算を行い、所定の加工データを生成して前記第一のサンプリング周期で出力する一次データ生成手段と、
前記加工データ生成手段から出力された加工データに基づき前記近似式を浮動小数点演算により計算する二次データ生成手段と、
を有することを特徴とする、請求項１に記載の走行状態推定システム。
【請求項３】
前記一次データ生成手段はハードウェアであり、前記二次データ生成手段はソフトウェアであることを特徴とする、請求項２に記載の走行状態推定システム。
【請求項１】
所定の推定手段により自律航法システムによる自律航法データとＧＰＳ（Global Positioning System）による測位データとを複合して対象物の走行状態を推定する走行状態推定システムにおいて、
ＧＰＳ信号に基づき第一のサンプリング周期で測位データを演算して前記推定手段に出力するＧＰＳ測位演算手段と、
前記第一のサンプリング周期より短い第二のサンプリング周期で観測データを出力する自律航法用センサと、
前記第一のサンプリング周期内に前記自律航法用センサから出力された観測データの組を所定の一次関数に近似する近似式演算手段と、
を有し、
前記近似式演算手段は、前記測位データと略同期したタイミングにおける前記一次関数の近似式の値を切片の値とし、該切片の値を該近似式の傾きの値とともに前記自律航法データとして前記推定手段に出力することを特徴とする走行状態推定システム。
【請求項２】
前記近似式演算手段は、
前記第一のサンプリング周期内に前記自律航法用センサから出力された複数の観測データを用いて所定の整数演算を行い、所定の加工データを生成して前記第一のサンプリング周期で出力する一次データ生成手段と、
前記加工データ生成手段から出力された加工データに基づき前記近似式を浮動小数点演算により計算する二次データ生成手段と、
を有することを特徴とする、請求項１に記載の走行状態推定システム。
【請求項３】
前記一次データ生成手段はハードウェアであり、前記二次データ生成手段はソフトウェアであることを特徴とする、請求項２に記載の走行状態推定システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１０−１９７２３（Ｐ２０１０−１９７２３Ａ）
【公開日】平成２２年１月２８日（２０１０．１．２８）
【国際特許分類】
【出願番号】特願２００８−１８１２２９（Ｐ２００８−１８１２２９）
【出願日】平成２０年７月１１日（２００８．７．１１）
【出願人】（０００００１４８７）クラリオン株式会社 (1,722)
【Ｆターム（参考）】
【公開日】平成２２年１月２８日（２０１０．１．２８）
【国際特許分類】
【出願日】平成２０年７月１１日（２００８．７．１１）
【出願人】（０００００１４８７）クラリオン株式会社 (1,722)
【Ｆターム（参考）】
[ Back to top ]