移動体高精度速度計測装置及び方法
【課題】ＧＰＳ受信機から得られるドップラー速度情報の良否を判定し、高精度及び高信頼の速度をリアルタイムに出力する装置及び方法を提供すること。
【解決手段】移動体高精度速度計測装置１０は、移動体の加速度及び角速度からストラップダウン演算により速度を算出し、ＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定し、ドップラー速度から良否係数を算出する。次に、遅延させたドップラー速度と、フィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量を算出し、算出した誤差量に良否係数を乗算し、良否係数を乗算した誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。そして、ストラップダウン演算により算出した速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出し、出力する。
【解決手段】移動体高精度速度計測装置１０は、移動体の加速度及び角速度からストラップダウン演算により速度を算出し、ＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定し、ドップラー速度から良否係数を算出する。次に、遅延させたドップラー速度と、フィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量を算出し、算出した誤差量に良否係数を乗算し、良否係数を乗算した誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。そして、ストラップダウン演算により算出した速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出し、出力する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体高精度速度計測装置及び方法に関する。
【背景技術】
【０００２】
近年、地球上の位置を測定するシステムとして、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）と呼ばれる全地球測位システムが知られている。ＧＰＳシステムでは、ＧＰＳ受信機が地球の上空にある人工衛星から電波を受信し、受信した電波の時刻差から現在位置を即座に測定する。このようなＧＰＳ受信機を使用すれば、移動体の速度を計測することができる。
【０００３】
ＧＰＳ受信機を使用した移動体の速度を計測する方法には、（１）ＧＰＳ受信機が算出した位置情報を微分することで速度を得る方法や、（２）衛星の移動と移動体の移動とによるＧＰＳ搬送波のドップラーシフト量からドップラー速度を算出する方法等がある。通常、（１）の方法は、単独測位時の位置精度は十数ｍ、ＤＧＰＳ（Ｄｉｆｆｅｒｅｎｔｉａｌ ＧＰＳ）使用時で数十ｃｍ程度であることから、良質な精度がでない。
【０００４】
（２）の方法は、高精度な速度計測が可能であるが、計測のための条件が限定されている。例えば、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスの影響を受けた際、受信機内部のＰＬＬ（ＰＬＬ：Ｐｈａｓｅ−ｌｏｃｋｅｄ ｌｏｏｐ 位相同期回路）のロックが外れてしまうことにより、速度出力に大きなノイズが重畳するので、マルチパスの影響が大きい場所では高精度な速度計測が望めない。また、（２）の方法は、衛星信号の瞬断時は速度計測をすることができない。
【０００５】
例えば、図７は、公道において移動する車の速度を、ＧＰＳ受信機からのドップラー速度によって示す図である。図７に示すように木立の影響により速度出力はノイズ量が増し、さらに高架橋等を通過するとＧＰＳからの衛星信号が途絶えてしまうため、速度出力はゼロとなってしまう。以上から公道のような様々な電波障害が起こる状況下において、ドップラー速度のみを利用して高精度に速度を計測することは、難しかった。
【０００６】
このような問題に対して、例えば、特許文献１に開示された速度計測装置は、ＧＰＳによるドップラー速度と、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）による速度とをカルマンフィルタで融合し、ＤＯＰ（衛星精度指標）を使用することでＧＰＳ信号の良否を判定し、ドップラー速度を出力するか、ＩＭＵによる速度を出力するかを切り替えることで高精度な速度を出力している。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００８−１７５７３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、ＤＯＰ値は、あくまで衛星の配置状況（幾何学的配置）による精度指標であり、マルチパスの影響を判定しうる指標ではない。また、ＤＯＰとドップラー速度との相関性は小さく、ドップラー速度の精度指標としての信頼性は小さい。さらに、ＤＯＰ値は衛星配置状況から算出されるので、ＤＯＰの変化は、瞬時ではない。このため、特許文献１に開示された速度計測装置は、ビル群や木立等によるマルチパスの影響が多大に考えられる場所において、マルチパス等によるドップラー速度の瞬時ノイズを判別することはできない。
【０００９】
そこで、ＧＰＳ受信機から得られるドップラー速度情報の良否を判定し、高精度及び高信頼の速度をリアルタイムに出力する装置及び方法が求められている。
【００１０】
本発明は、ＧＰＳ受信機から得られるドップラー速度情報の良否を判定し、高精度及び高信頼の速度をリアルタイムに出力する移動体高精度速度計測装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明では、以下のような解決手段を提供する。
【００１２】
（１） 移動体の速度を計測する移動体高精度速度計測装置であって、ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測手段と、前記加速度計測手段によって計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータ手段と、ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定手段と、前記速度測定手段によって測定された前記ドップラー速度から算出した加速度と、前記加速度計測手段によって計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出手段と、前記良否係数算出手段によって前記係数が算出されるための時間だけ、前記速度測定手段によって測定された前記ドップラー速度を演算に用いる時間を遅延させる遅延処理手段と、前記遅延処理手段によって遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータ手段からフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理手段と、前記同期化処理手段によって算出された前記誤差量に、前記良否係数算出手段によって算出された前記係数を乗算する良否係数乗算手段と、前記良否係数乗算手段によって前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタ手段と、を備え、前記ストラップダウンナビゲータ手段は、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタ手段によって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、移動体高精度速度計測装置。
【００１３】
（１）の構成によれば、本発明に係る移動体高精度速度計測装置は、ＩＭＵを使用して移動体の加速度及び角速度を計測し、計測した移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出する。次に、移動体高精度速度計測装置は、ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定し、測定したドップラー速度から算出した加速度と、計測した加速度との差分を算出し、算出した差分に基づいて係数を算出する。次に、移動体高精度速度計測装置は、係数が算出されるための時間だけ、測定したドップラー速度を演算に用いる時間を遅延させ、遅延させたドップラー速度と、フィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量を算出する。次に、移動体高精度速度計測装置は、算出した誤差量に、算出した係数を乗算し、係数を乗算した誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。そして、移動体高精度速度計測装置は、ストラップダウン演算により算出された速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出する。
【００１４】
すなわち、移動体高精度速度計測装置は、ＧＰＳ受信機を使用して得られたドップラー速度による加速度を、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）によって計測された加速度によって良否判定して良否判定係数を算出し、ストラップダウン演算によって算出された速度に基づいて算出されたリアルタイム補間速度をフィードバックさせて、算出されたドップラー速度とフィードバックさせたリアルタイム補間速度との誤差量に良否判定係数を乗算した調整された誤差量を算出し、調整された誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算し、ストラップダウン演算により算出された速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出する。よって、移動体高精度速度計測装置は、カルマンフィルタに入力する誤差量を調整することで、ＧＰＳ受信機から算出されるドップラー速度のノイズ量を小さくし、リアルタイムに速度を算出することができる。したがって、移動体高精度速度計測装置は、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する状況下であっても、高精度及び高信頼の速度をリアルタイムに出力することができる。
【００１５】
（２） ＩＭＵとＧＰＳ受信機と備え、移動体の速度を計測する移動体高精度速度計測装置が実行する方法であって、前記ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測ステップと、計測された前記移動体の加速度及び角速度からストラップダウン演算によりリアルタイム補間速度を算出するストラップダウンナビゲータステップと、前記ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定ステップと、測定された前記ドップラー速度から算出した加速度と、計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出ステップと、前記係数が算出されるための時間だけ、前記ドップラー速度を演算に用いる時間を遅延させる遅延処理ステップと、遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータステップからフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理ステップと、算出された前記誤差量に、算出された前記係数を乗算する良否係数乗算ステップと、前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタステップと、を含み、前記ストラップダウンナビゲータステップは、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタステップによって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、方法。
【００１６】
したがって、（２）に係る方法は、（１）と同様に、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する状況下であっても、高精度及び高信頼の速度をリアルタイムに出力することができる。
【発明の効果】
【００１７】
本発明に係る移動体高精度速度計測装置は、ＧＰＳ受信機から得られるドップラー速度情報の良否を判定し、リアルタイムに高精度な速度を出力することができる。特に、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生するようなところであっても、移動体高精度速度計測装置は、ＧＰＳ受信機から得られるドップラー速度からノイズの影響を小さくして、移動体の高精度な速度をリアルタイムに出力することができる。その結果、移動体高精度速度計測装置は、公道であっても移動体の高精度な速度を安定して出力することができる。さらに、衛星信号瞬断時であっても、移動体高精度速度計測装置は、ＩＭＵを使用して補間して、高精度な速度を安定して出力することができる。また、移動体高精度速度計測装置は、リアルタイム出力が可能なため、他センサとの同時計測時に同期処理が不要であり、また出力遅れが問題となる車両制動試験等においても２次処理を行う必要がなく、移動体高精度速度計測装置の原信号を利用可能である。
【図面の簡単な説明】
【００１８】
【図１】本発明の一実施形態である移動体高精度速度計測装置の構成を示すブロック図である。
【図２】本発明の一実施形態に係る移動体高精度速度計測装置のハードウェア構成の一例を示す図である。
【図３】本発明の一実施形態に係る移動体高精度速度計測装置における処理内容を示すフローチャートである。
【図４】本発明の一実施形態に係る移動体高精度速度計測装置によって出力されるリアルタイム補間速度と、ＧＰＳ受信機によるドップラー速度とを示す図である。
【図５】本発明の一実施形態に係る移動体高精度速度計測装置によって出力されるリアルタイム補間速度の立ち上がりと、ＧＰＳ受信機から出力されるドップラー速度の立ち上がりとを示す図である。
【図６】本発明の一実施形態に係る移動体高精度速度計測装置によって出力されるリアルタイム補間速度と、ＧＰＳ受信機から出力されるドップラー速度と、光学式速度計測装置から出力される速度とを示す図である。
【図７】公道において移動する車の速度を、ＧＰＳ受信機からのドップラー速度によって示す図である。
【発明を実施するための形態】
【００１９】
以下、本発明の実施形態について図を参照しながら説明する。
【００２０】
図１は、本発明の一実施形態である移動体高精度速度計測装置１０の構成を示すブロック図である。移動体高精度速度計測装置１０は、加速度計測部１１と、ストラップダウンナビゲータ部１２と、速度測定部１３と、良否係数算出部１４と、遅延処理部１５と、同期化処理部１６と、良否係数乗算部１７と、カルマンフィルタ部１８とを備える。
【００２１】
加速度計測部１１は、ＩＭＵ１０２を使用して移動体の加速度及び角速度を計測する。ＩＭＵ１０２は、３軸のジャイロと３方向の加速度計から構成され、３次元の角速度と加速度とを求める。なお、ＩＭＵ１０２は、計測の信頼性向上のために、さらに複数のセンサを搭載してもよい。
【００２２】
ストラップダウンナビゲータ部１２（自律航法アルゴリズム）は、加速度計測部１１によって計測された移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に、後述するカルマンフィルタ部１８によって推定演算された調整量を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共に、カルマンフィルタ部１８へフィードバックさせる。
【００２３】
速度測定部１３は、ＧＰＳ受信機１０１を使用してＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定する。例えば、ＧＰＳ受信機１０１とＧＰＳ衛星との距離が、遠ざかる又は近づくと、ＧＰＳ受信機１０１が受信する搬送波の位相は、連続的に変化し、周波数が低くなったり高くなったりする。速度測定部１３は、この周波数の変化量からＧＰＳ受信機１０１が出力するドップラー速度を取得する。
【００２４】
良否係数算出部１４は、速度測定部１３によって測定されたドップラー速度から算出した加速度と、加速度計測部１１によって計測された加速度との差分を算出し、算出した差分に基づいて係数（図１におけるβ）を算出する。例えば、良否係数算出部１４は、座標変換処理部（図示せず）によって、ＩＭＵ１０２によって計測された加速度を、ストラップダウンナビゲータ部１２により演算された現在の姿勢角に基づき座標変換し、ＧＰＳ受信機１０１が出力したドップラー速度と同座標系の加速度に変換する。そして、良否係数算出部１４は、ドップラー速度を微分した加速度と、座標変換した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。次に、良否係数算出部１４は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、係数を算出する。すなわち、良否係数算出部１４は、求めた逆関数に基づいて、ドップラー速度がノイズを含んでいないと判断した場合に良判定を行い、ノイズを含んでいると判断した場合に否判定を行い、判定を数値化した良否係数を算出する。例えば、良否係数＝１−速度変動判定−α×ドップラー精度指標、により良否係数を算出してもよい。ここで、速度変動判定はドップラー速度がノイズを含むと判定された割合、αは所定の値、ドップラー速度精度指標はＧＰＳ衛星の配置に関する指標や、観測衛星数等のＧＰＳ受信機から得られる情報に基づいて統計的に算出した指標である。このように算出された良否係数は、カルマンフィルタ部１８に入力する誤差量を調整する係数である。
【００２５】
遅延処理部１５は、良否係数算出部１４によって係数が算出されるための時間だけ、速度測定部１３によって測定されたドップラー速度を演算に用いる時間を遅延させる。すなわち、遅延処理部１５は、後述する、ドップラー速度による誤差量を算出するための演算を、良否係数算出部１４によって係数が算出される時間だけ遅延させる。
【００２６】
同期化処理部１６は、遅延処理部１５によって遅延させたドップラー速度と、ストラップダウンナビゲータ部１２からフィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量（図１におけるδｘ）を算出する。
【００２７】
良否係数乗算部１７は、同期化処理部１６によって算出された誤差量に、良否係数算出部１４によって算出された係数を乗算する。
【００２８】
カルマンフィルタ部１８は、良否係数乗算部１７によって係数が乗算された誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。例えば、カルマンフィルタ部１８は、入力された誤差量と、１ステップ前の推定演算し記憶した調整量とから、今回の調整量（ろ波推定値）を算出する反復推定器（反復推定型フィルタ）である。
【００２９】
このように、移動体高精度速度計測装置１０は、速度測定部１３によって測定されたドップラー速度とフィードバックされたリアルタイム補間速度とを同期させて算出した誤差量にドップラー速度の良否係数を乗算し、乗算して調整した誤差量からカルマンフィルタ部１８によって推定演算された調整量と、加速度計測部１１によって計測された加速度及び角速度からストラップダウン演算により算出された速度と、を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共にフィードバックさせる。
【００３０】
図２は、本発明の一実施形態に係る移動体高精度速度計測装置１０のハードウェア構成の一例を示す図である。移動体高精度速度計測装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１０、バスライン１００５、通信Ｉ／Ｆ１０４０、メモリ１０５０、表示器１０２２、ＧＰＳ受信機１０１及びＩＭＵ１０２を備える。
【００３１】
ＣＰＵ１０１０は、移動体高精度速度計測装置１０を統括的に制御する部分であり、メモリ１０５０に記憶された各種プログラムを適宜読み出して実行することにより、上述したハードウェアと協働し、本発明に係る各種機能を実現している。
【００３２】
メモリ１０５０は、適宜読み出して実行されるプログラムを記憶し、プログラムの実行によって作成される種々の情報を記憶する。例えば、メモリ１０５０は、ストラップダウンナビゲータ部１２によってフィードバックされるリアルタイム補間速度や、カルマンフィルタ部１８によって推定演算される調整量等を記憶する。
【００３３】
ここで、表示器１０２２は、移動体高精度速度計測装置１０による演算処理結果、例えば、算出したリアルタイム補間速度や、算出過程のドップラー速度等を表示したりするものであり、液晶表示装置（ＬＣＤ）等のディスプレイ装置を含む。
【００３４】
また、通信Ｉ／Ｆ１０４０は、移動体高精度速度計測装置１０を専用ネットワークを介してホストコンピュータ等と接続できるようにするためのネットワーク・アダプタである。通信Ｉ／Ｆ１０４０は、ホストコンピュータ等に、算出されたリアルタイム補間速度を出力するための接続インターフェースである。
【００３５】
ＧＰＳ受信機１０１は、無線通信に必要なアンテナ及びアンテナ信号処理回路等を含んで構成される。ＧＰＳ受信機１０１は、複数のＧＰＳ衛星から電波を受信し、受信したＧＰＳ搬送波のドップラーシフト量からドップラー速度を算出し、出力する。
【００３６】
ＩＭＵ１０２は、移動体の加速度及び角速度を計測し出力する。ＩＭＵ１０２は、慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）であって、３軸のジャイロと３方向の加速度計によって、運動を司る３軸の角度（又は角速度）と加速度とを検出する。
【００３７】
図３は、本発明の一実施形態に係る移動体高精度速度計測装置１０における処理内容を示すフローチャートである。なお、本処理は、例えば、プログラム開始指令を受け付けて開始し、プログラム終了指令により終了する。
【００３８】
ステップＳ１０１において、ＣＰＵ１０１０は、ストラップダウンナビゲータ部１２の初期設定（初期速度、初期位置、初期姿勢角）、カルマンフィルタ部１８の初期設定（誤差共分散行列の初期化）を行う。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００３９】
ステップＳ１０２において、ＣＰＵ１０１０（加速度計測部１１）は、ＩＭＵ１０２によって加速度及び角速度を計測する。その後、ＣＰＵ１０１０は、処理をステップＳ１０３に移す。
【００４０】
ステップＳ１０３において、ＣＰＵ１０１０（ストラップダウンナビゲータ部１２）は、ステップＳ１０２において計測された加速度及び角速度からストラップダウン演算により速度を算出し、カルマンフィルタ部１８からの調整量を融合させて、リアルタイム補間速度を算出し、出力すると共にフィードバックさせる。その後、ＣＰＵ１０１０は、処理をステップＳ１０４に移す。
【００４１】
ステップＳ１０４において、ＣＰＵ１０１０（速度測定部１３）は、ＧＰＳ受信機１０１が出力したドップラー速度を取得する。より具体的には、ＣＰＵ１０１０は、ＧＰＳ受信機１０１が受信したＧＰＳ搬送波のドップラーシフト量から算出したドップラー速度を取得する。その後、ＣＰＵ１０１０は、処理をステップＳ１０５に移す。
【００４２】
ステップＳ１０５において、ＣＰＵ１０１０（良否係数算出部１４）は、ドップラー速度から良否係数を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０３において算出したドップラー速度を微分した加速度と、ＩＭＵ１０２が計測した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。そして、ＣＰＵ１０１０は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、ドップラー速度の信頼性（ドップラー速度がノイズを含んでいないと判定した場合に良、ノイズを含んでいると判定した場合に否）に対する判定の結果を示す良否係数（β）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０６に移す。
【００４３】
ステップＳ１０６において、ＣＰＵ１０１０（遅延処理部１５、同期化処理部１６）は、フィードバックされたリアルタイム補間速度と、ドップラー速度とを同期させ、誤差量を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０４において算出されたドップラー速度による誤差量を算出するための演算を、ステップＳ１０５において良否係数が算出される時間だけ遅延させ、遅延させたドップラー速度とフィードバックされたリアルタイム補間速度とを同期させて誤差量（δｘ）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０７に移す。
【００４４】
ステップＳ１０７において、ＣＰＵ１０１０（良否係数乗算部１７）は、誤差量に良否係数を乗算する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０６において算出した誤差量（δｘ）に、ステップＳ１０５において算出した良否係数（β）を乗算して、調整された誤差量（βδｘ）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００４５】
ステップＳ１０８において、ＣＰＵ１０１０（カルマンフィルタ部１８）は、調整された誤差量（βδｘ）からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００４６】
図４は、本発明の一実施形態に係る移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度と、ＧＰＳ受信機１０１によるドップラー速度とを示す図である。図４（ａ）は、縦軸を速度とし横軸を時間としたグラフに、ＧＰＳ受信機１０１によるドップラー速度を表した図である。同様に、図４（ｂ）は、縦軸を速度とし横軸を時間としたグラフに、移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度を表した図である。
【００４７】
図４（ａ）に示すように、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する場合、ＧＰＳ受信機１０１によるドップラー速度は、マルチパスによるノイズによる速度を含んで出力される。一方、図４（ｂ）に示すように、ＧＰＳ受信機１０１によるドップラー速度がマルチパスによるノイズを含んでいても、移動体高精度速度計測装置１０は、ノイズの影響を小さくし、非常に滑らかな波形によって移動体の速度を出力する。
【００４８】
図５は、本発明の一実施形態に係る移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度の立ち上がりと、ＧＰＳ受信機１０１から出力されるドップラー速度の立ち上がりとを示す図である。図５（ａ）は、移動体高精度速度計測装置１０が出力した速度と、ＧＰＳ受信機１０１によるドップラー速度と、ドップラー速度を１Ｈｚのローパス処理をした速度との比較を示す図である。図５（ｂ）は、図５（ａ）の一部（破線による円によって囲まれた部分）を拡大した図である。
【００４９】
図５（ｂ）に示すように、マルチパスによるノイズを含むドップラー速度と、マルチパスによるノイズを含むドップラー速度をローパス処理によってノイズ除去した速度とに比較して、移動体高精度速度計測装置１０が出力する速度は、ノイズ除去のための演算による出力遅れを小さくして、リアルタイム性を実現している。
【００５０】
図６は、本発明の一実施形態に係る移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度と、ＧＰＳ受信機１０１から出力されるドップラー速度と、光学式速度計測装置から出力される速度とを示す図である。ここで、光学式速度計測装置は、路面の不規則な模様から、特定の間隔（例えば、２．３ｍｍ）によるクシ型構造の特殊受光素子によって特定の間隔の反射ムラだけを抽出し、抽出した反射ムラの計数値に特定の間隔をかけて算出した高精度の速度を出力する。
【００５１】
図６が示すように、移動体高精度速度計測装置１０は、マルチパスによるノイズを含むドップラー速度におけるノイズの影響を小さくし、リアルタイムに移動体の速度を出力し、出力した速度は、光学式速度計測装置による高精度の速度と同様である。
【００５２】
本実施形態によれば、移動体高精度速度計測装置１０は、ＩＭＵ１０２を使用して移動体の加速度及び角速度を計測し、計測した移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出する。次に、移動体高精度速度計測装置１０は、ＧＰＳ受信機１０１を使用してＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定し、測定したドップラー速度から算出した加速度と、計測した加速度との差分を算出し、算出した差分に基づいて係数を算出する。次に、移動体高精度速度計測装置１０は、係数が算出されるための時間だけ、測定したドップラー速度を演算に用いる時間を遅延させ、遅延させたドップラー速度と、フィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量を算出する。次に、移動体高精度速度計測装置１０は、算出した誤差量に、算出した係数を乗算し、係数を乗算した誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。そして、移動体高精度速度計測装置１０は、ストラップダウン演算により算出した速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共にフィードバックさせる。したがって、移動体高精度速度計測装置１０は、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する状況下であっても、高精度及び高信頼の速度をリアルタイムに出力することができる。
【００５３】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。
【符号の説明】
【００５４】
１０ 移動体高精度速度計測装置
１１ 加速度計測部
１２ ストラップダウンナビゲータ部
１３ 速度測定部
１４ 良否係数算出部
１５ 遅延処理部
１６ 同期化処理部
１７ 良否係数乗算部
１８ カルマンフィルタ部
１０１ ＧＰＳ受信機
１０２ ＩＭＵ
【技術分野】
【０００１】
本発明は、移動体高精度速度計測装置及び方法に関する。
【背景技術】
【０００２】
近年、地球上の位置を測定するシステムとして、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）と呼ばれる全地球測位システムが知られている。ＧＰＳシステムでは、ＧＰＳ受信機が地球の上空にある人工衛星から電波を受信し、受信した電波の時刻差から現在位置を即座に測定する。このようなＧＰＳ受信機を使用すれば、移動体の速度を計測することができる。
【０００３】
ＧＰＳ受信機を使用した移動体の速度を計測する方法には、（１）ＧＰＳ受信機が算出した位置情報を微分することで速度を得る方法や、（２）衛星の移動と移動体の移動とによるＧＰＳ搬送波のドップラーシフト量からドップラー速度を算出する方法等がある。通常、（１）の方法は、単独測位時の位置精度は十数ｍ、ＤＧＰＳ（Ｄｉｆｆｅｒｅｎｔｉａｌ ＧＰＳ）使用時で数十ｃｍ程度であることから、良質な精度がでない。
【０００４】
（２）の方法は、高精度な速度計測が可能であるが、計測のための条件が限定されている。例えば、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスの影響を受けた際、受信機内部のＰＬＬ（ＰＬＬ：Ｐｈａｓｅ−ｌｏｃｋｅｄ ｌｏｏｐ 位相同期回路）のロックが外れてしまうことにより、速度出力に大きなノイズが重畳するので、マルチパスの影響が大きい場所では高精度な速度計測が望めない。また、（２）の方法は、衛星信号の瞬断時は速度計測をすることができない。
【０００５】
例えば、図７は、公道において移動する車の速度を、ＧＰＳ受信機からのドップラー速度によって示す図である。図７に示すように木立の影響により速度出力はノイズ量が増し、さらに高架橋等を通過するとＧＰＳからの衛星信号が途絶えてしまうため、速度出力はゼロとなってしまう。以上から公道のような様々な電波障害が起こる状況下において、ドップラー速度のみを利用して高精度に速度を計測することは、難しかった。
【０００６】
このような問題に対して、例えば、特許文献１に開示された速度計測装置は、ＧＰＳによるドップラー速度と、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）による速度とをカルマンフィルタで融合し、ＤＯＰ（衛星精度指標）を使用することでＧＰＳ信号の良否を判定し、ドップラー速度を出力するか、ＩＭＵによる速度を出力するかを切り替えることで高精度な速度を出力している。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００８−１７５７３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、ＤＯＰ値は、あくまで衛星の配置状況（幾何学的配置）による精度指標であり、マルチパスの影響を判定しうる指標ではない。また、ＤＯＰとドップラー速度との相関性は小さく、ドップラー速度の精度指標としての信頼性は小さい。さらに、ＤＯＰ値は衛星配置状況から算出されるので、ＤＯＰの変化は、瞬時ではない。このため、特許文献１に開示された速度計測装置は、ビル群や木立等によるマルチパスの影響が多大に考えられる場所において、マルチパス等によるドップラー速度の瞬時ノイズを判別することはできない。
【０００９】
そこで、ＧＰＳ受信機から得られるドップラー速度情報の良否を判定し、高精度及び高信頼の速度をリアルタイムに出力する装置及び方法が求められている。
【００１０】
本発明は、ＧＰＳ受信機から得られるドップラー速度情報の良否を判定し、高精度及び高信頼の速度をリアルタイムに出力する移動体高精度速度計測装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明では、以下のような解決手段を提供する。
【００１２】
（１） 移動体の速度を計測する移動体高精度速度計測装置であって、ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測手段と、前記加速度計測手段によって計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータ手段と、ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定手段と、前記速度測定手段によって測定された前記ドップラー速度から算出した加速度と、前記加速度計測手段によって計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出手段と、前記良否係数算出手段によって前記係数が算出されるための時間だけ、前記速度測定手段によって測定された前記ドップラー速度を演算に用いる時間を遅延させる遅延処理手段と、前記遅延処理手段によって遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータ手段からフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理手段と、前記同期化処理手段によって算出された前記誤差量に、前記良否係数算出手段によって算出された前記係数を乗算する良否係数乗算手段と、前記良否係数乗算手段によって前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタ手段と、を備え、前記ストラップダウンナビゲータ手段は、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタ手段によって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、移動体高精度速度計測装置。
【００１３】
（１）の構成によれば、本発明に係る移動体高精度速度計測装置は、ＩＭＵを使用して移動体の加速度及び角速度を計測し、計測した移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出する。次に、移動体高精度速度計測装置は、ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定し、測定したドップラー速度から算出した加速度と、計測した加速度との差分を算出し、算出した差分に基づいて係数を算出する。次に、移動体高精度速度計測装置は、係数が算出されるための時間だけ、測定したドップラー速度を演算に用いる時間を遅延させ、遅延させたドップラー速度と、フィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量を算出する。次に、移動体高精度速度計測装置は、算出した誤差量に、算出した係数を乗算し、係数を乗算した誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。そして、移動体高精度速度計測装置は、ストラップダウン演算により算出された速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出する。
【００１４】
すなわち、移動体高精度速度計測装置は、ＧＰＳ受信機を使用して得られたドップラー速度による加速度を、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）によって計測された加速度によって良否判定して良否判定係数を算出し、ストラップダウン演算によって算出された速度に基づいて算出されたリアルタイム補間速度をフィードバックさせて、算出されたドップラー速度とフィードバックさせたリアルタイム補間速度との誤差量に良否判定係数を乗算した調整された誤差量を算出し、調整された誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算し、ストラップダウン演算により算出された速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出する。よって、移動体高精度速度計測装置は、カルマンフィルタに入力する誤差量を調整することで、ＧＰＳ受信機から算出されるドップラー速度のノイズ量を小さくし、リアルタイムに速度を算出することができる。したがって、移動体高精度速度計測装置は、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する状況下であっても、高精度及び高信頼の速度をリアルタイムに出力することができる。
【００１５】
（２） ＩＭＵとＧＰＳ受信機と備え、移動体の速度を計測する移動体高精度速度計測装置が実行する方法であって、前記ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測ステップと、計測された前記移動体の加速度及び角速度からストラップダウン演算によりリアルタイム補間速度を算出するストラップダウンナビゲータステップと、前記ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定ステップと、測定された前記ドップラー速度から算出した加速度と、計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出ステップと、前記係数が算出されるための時間だけ、前記ドップラー速度を演算に用いる時間を遅延させる遅延処理ステップと、遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータステップからフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理ステップと、算出された前記誤差量に、算出された前記係数を乗算する良否係数乗算ステップと、前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタステップと、を含み、前記ストラップダウンナビゲータステップは、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタステップによって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、方法。
【００１６】
したがって、（２）に係る方法は、（１）と同様に、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する状況下であっても、高精度及び高信頼の速度をリアルタイムに出力することができる。
【発明の効果】
【００１７】
本発明に係る移動体高精度速度計測装置は、ＧＰＳ受信機から得られるドップラー速度情報の良否を判定し、リアルタイムに高精度な速度を出力することができる。特に、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生するようなところであっても、移動体高精度速度計測装置は、ＧＰＳ受信機から得られるドップラー速度からノイズの影響を小さくして、移動体の高精度な速度をリアルタイムに出力することができる。その結果、移動体高精度速度計測装置は、公道であっても移動体の高精度な速度を安定して出力することができる。さらに、衛星信号瞬断時であっても、移動体高精度速度計測装置は、ＩＭＵを使用して補間して、高精度な速度を安定して出力することができる。また、移動体高精度速度計測装置は、リアルタイム出力が可能なため、他センサとの同時計測時に同期処理が不要であり、また出力遅れが問題となる車両制動試験等においても２次処理を行う必要がなく、移動体高精度速度計測装置の原信号を利用可能である。
【図面の簡単な説明】
【００１８】
【図１】本発明の一実施形態である移動体高精度速度計測装置の構成を示すブロック図である。
【図２】本発明の一実施形態に係る移動体高精度速度計測装置のハードウェア構成の一例を示す図である。
【図３】本発明の一実施形態に係る移動体高精度速度計測装置における処理内容を示すフローチャートである。
【図４】本発明の一実施形態に係る移動体高精度速度計測装置によって出力されるリアルタイム補間速度と、ＧＰＳ受信機によるドップラー速度とを示す図である。
【図５】本発明の一実施形態に係る移動体高精度速度計測装置によって出力されるリアルタイム補間速度の立ち上がりと、ＧＰＳ受信機から出力されるドップラー速度の立ち上がりとを示す図である。
【図６】本発明の一実施形態に係る移動体高精度速度計測装置によって出力されるリアルタイム補間速度と、ＧＰＳ受信機から出力されるドップラー速度と、光学式速度計測装置から出力される速度とを示す図である。
【図７】公道において移動する車の速度を、ＧＰＳ受信機からのドップラー速度によって示す図である。
【発明を実施するための形態】
【００１９】
以下、本発明の実施形態について図を参照しながら説明する。
【００２０】
図１は、本発明の一実施形態である移動体高精度速度計測装置１０の構成を示すブロック図である。移動体高精度速度計測装置１０は、加速度計測部１１と、ストラップダウンナビゲータ部１２と、速度測定部１３と、良否係数算出部１４と、遅延処理部１５と、同期化処理部１６と、良否係数乗算部１７と、カルマンフィルタ部１８とを備える。
【００２１】
加速度計測部１１は、ＩＭＵ１０２を使用して移動体の加速度及び角速度を計測する。ＩＭＵ１０２は、３軸のジャイロと３方向の加速度計から構成され、３次元の角速度と加速度とを求める。なお、ＩＭＵ１０２は、計測の信頼性向上のために、さらに複数のセンサを搭載してもよい。
【００２２】
ストラップダウンナビゲータ部１２（自律航法アルゴリズム）は、加速度計測部１１によって計測された移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に、後述するカルマンフィルタ部１８によって推定演算された調整量を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共に、カルマンフィルタ部１８へフィードバックさせる。
【００２３】
速度測定部１３は、ＧＰＳ受信機１０１を使用してＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定する。例えば、ＧＰＳ受信機１０１とＧＰＳ衛星との距離が、遠ざかる又は近づくと、ＧＰＳ受信機１０１が受信する搬送波の位相は、連続的に変化し、周波数が低くなったり高くなったりする。速度測定部１３は、この周波数の変化量からＧＰＳ受信機１０１が出力するドップラー速度を取得する。
【００２４】
良否係数算出部１４は、速度測定部１３によって測定されたドップラー速度から算出した加速度と、加速度計測部１１によって計測された加速度との差分を算出し、算出した差分に基づいて係数（図１におけるβ）を算出する。例えば、良否係数算出部１４は、座標変換処理部（図示せず）によって、ＩＭＵ１０２によって計測された加速度を、ストラップダウンナビゲータ部１２により演算された現在の姿勢角に基づき座標変換し、ＧＰＳ受信機１０１が出力したドップラー速度と同座標系の加速度に変換する。そして、良否係数算出部１４は、ドップラー速度を微分した加速度と、座標変換した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。次に、良否係数算出部１４は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、係数を算出する。すなわち、良否係数算出部１４は、求めた逆関数に基づいて、ドップラー速度がノイズを含んでいないと判断した場合に良判定を行い、ノイズを含んでいると判断した場合に否判定を行い、判定を数値化した良否係数を算出する。例えば、良否係数＝１−速度変動判定−α×ドップラー精度指標、により良否係数を算出してもよい。ここで、速度変動判定はドップラー速度がノイズを含むと判定された割合、αは所定の値、ドップラー速度精度指標はＧＰＳ衛星の配置に関する指標や、観測衛星数等のＧＰＳ受信機から得られる情報に基づいて統計的に算出した指標である。このように算出された良否係数は、カルマンフィルタ部１８に入力する誤差量を調整する係数である。
【００２５】
遅延処理部１５は、良否係数算出部１４によって係数が算出されるための時間だけ、速度測定部１３によって測定されたドップラー速度を演算に用いる時間を遅延させる。すなわち、遅延処理部１５は、後述する、ドップラー速度による誤差量を算出するための演算を、良否係数算出部１４によって係数が算出される時間だけ遅延させる。
【００２６】
同期化処理部１６は、遅延処理部１５によって遅延させたドップラー速度と、ストラップダウンナビゲータ部１２からフィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量（図１におけるδｘ）を算出する。
【００２７】
良否係数乗算部１７は、同期化処理部１６によって算出された誤差量に、良否係数算出部１４によって算出された係数を乗算する。
【００２８】
カルマンフィルタ部１８は、良否係数乗算部１７によって係数が乗算された誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。例えば、カルマンフィルタ部１８は、入力された誤差量と、１ステップ前の推定演算し記憶した調整量とから、今回の調整量（ろ波推定値）を算出する反復推定器（反復推定型フィルタ）である。
【００２９】
このように、移動体高精度速度計測装置１０は、速度測定部１３によって測定されたドップラー速度とフィードバックされたリアルタイム補間速度とを同期させて算出した誤差量にドップラー速度の良否係数を乗算し、乗算して調整した誤差量からカルマンフィルタ部１８によって推定演算された調整量と、加速度計測部１１によって計測された加速度及び角速度からストラップダウン演算により算出された速度と、を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共にフィードバックさせる。
【００３０】
図２は、本発明の一実施形態に係る移動体高精度速度計測装置１０のハードウェア構成の一例を示す図である。移動体高精度速度計測装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０１０、バスライン１００５、通信Ｉ／Ｆ１０４０、メモリ１０５０、表示器１０２２、ＧＰＳ受信機１０１及びＩＭＵ１０２を備える。
【００３１】
ＣＰＵ１０１０は、移動体高精度速度計測装置１０を統括的に制御する部分であり、メモリ１０５０に記憶された各種プログラムを適宜読み出して実行することにより、上述したハードウェアと協働し、本発明に係る各種機能を実現している。
【００３２】
メモリ１０５０は、適宜読み出して実行されるプログラムを記憶し、プログラムの実行によって作成される種々の情報を記憶する。例えば、メモリ１０５０は、ストラップダウンナビゲータ部１２によってフィードバックされるリアルタイム補間速度や、カルマンフィルタ部１８によって推定演算される調整量等を記憶する。
【００３３】
ここで、表示器１０２２は、移動体高精度速度計測装置１０による演算処理結果、例えば、算出したリアルタイム補間速度や、算出過程のドップラー速度等を表示したりするものであり、液晶表示装置（ＬＣＤ）等のディスプレイ装置を含む。
【００３４】
また、通信Ｉ／Ｆ１０４０は、移動体高精度速度計測装置１０を専用ネットワークを介してホストコンピュータ等と接続できるようにするためのネットワーク・アダプタである。通信Ｉ／Ｆ１０４０は、ホストコンピュータ等に、算出されたリアルタイム補間速度を出力するための接続インターフェースである。
【００３５】
ＧＰＳ受信機１０１は、無線通信に必要なアンテナ及びアンテナ信号処理回路等を含んで構成される。ＧＰＳ受信機１０１は、複数のＧＰＳ衛星から電波を受信し、受信したＧＰＳ搬送波のドップラーシフト量からドップラー速度を算出し、出力する。
【００３６】
ＩＭＵ１０２は、移動体の加速度及び角速度を計測し出力する。ＩＭＵ１０２は、慣性計測装置（ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）であって、３軸のジャイロと３方向の加速度計によって、運動を司る３軸の角度（又は角速度）と加速度とを検出する。
【００３７】
図３は、本発明の一実施形態に係る移動体高精度速度計測装置１０における処理内容を示すフローチャートである。なお、本処理は、例えば、プログラム開始指令を受け付けて開始し、プログラム終了指令により終了する。
【００３８】
ステップＳ１０１において、ＣＰＵ１０１０は、ストラップダウンナビゲータ部１２の初期設定（初期速度、初期位置、初期姿勢角）、カルマンフィルタ部１８の初期設定（誤差共分散行列の初期化）を行う。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００３９】
ステップＳ１０２において、ＣＰＵ１０１０（加速度計測部１１）は、ＩＭＵ１０２によって加速度及び角速度を計測する。その後、ＣＰＵ１０１０は、処理をステップＳ１０３に移す。
【００４０】
ステップＳ１０３において、ＣＰＵ１０１０（ストラップダウンナビゲータ部１２）は、ステップＳ１０２において計測された加速度及び角速度からストラップダウン演算により速度を算出し、カルマンフィルタ部１８からの調整量を融合させて、リアルタイム補間速度を算出し、出力すると共にフィードバックさせる。その後、ＣＰＵ１０１０は、処理をステップＳ１０４に移す。
【００４１】
ステップＳ１０４において、ＣＰＵ１０１０（速度測定部１３）は、ＧＰＳ受信機１０１が出力したドップラー速度を取得する。より具体的には、ＣＰＵ１０１０は、ＧＰＳ受信機１０１が受信したＧＰＳ搬送波のドップラーシフト量から算出したドップラー速度を取得する。その後、ＣＰＵ１０１０は、処理をステップＳ１０５に移す。
【００４２】
ステップＳ１０５において、ＣＰＵ１０１０（良否係数算出部１４）は、ドップラー速度から良否係数を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０３において算出したドップラー速度を微分した加速度と、ＩＭＵ１０２が計測した加速度との差分を算出し、算出した差分を二乗し、二乗した差分についてエンベロープ処理を行う。そして、ＣＰＵ１０１０は、エンベロープ処理を行った後の差分を示す関数の逆関数を求め、求めた逆関数に基づいて、ドップラー速度の信頼性（ドップラー速度がノイズを含んでいないと判定した場合に良、ノイズを含んでいると判定した場合に否）に対する判定の結果を示す良否係数（β）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０６に移す。
【００４３】
ステップＳ１０６において、ＣＰＵ１０１０（遅延処理部１５、同期化処理部１６）は、フィードバックされたリアルタイム補間速度と、ドップラー速度とを同期させ、誤差量を算出する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０４において算出されたドップラー速度による誤差量を算出するための演算を、ステップＳ１０５において良否係数が算出される時間だけ遅延させ、遅延させたドップラー速度とフィードバックされたリアルタイム補間速度とを同期させて誤差量（δｘ）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０７に移す。
【００４４】
ステップＳ１０７において、ＣＰＵ１０１０（良否係数乗算部１７）は、誤差量に良否係数を乗算する。より具体的には、ＣＰＵ１０１０は、ステップＳ１０６において算出した誤差量（δｘ）に、ステップＳ１０５において算出した良否係数（β）を乗算して、調整された誤差量（βδｘ）を算出する。その後、ＣＰＵ１０１０は、処理をステップＳ１０８に移す。
【００４５】
ステップＳ１０８において、ＣＰＵ１０１０（カルマンフィルタ部１８）は、調整された誤差量（βδｘ）からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。その後、ＣＰＵ１０１０は、処理をステップＳ１０２に移す。
【００４６】
図４は、本発明の一実施形態に係る移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度と、ＧＰＳ受信機１０１によるドップラー速度とを示す図である。図４（ａ）は、縦軸を速度とし横軸を時間としたグラフに、ＧＰＳ受信機１０１によるドップラー速度を表した図である。同様に、図４（ｂ）は、縦軸を速度とし横軸を時間としたグラフに、移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度を表した図である。
【００４７】
図４（ａ）に示すように、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する場合、ＧＰＳ受信機１０１によるドップラー速度は、マルチパスによるノイズによる速度を含んで出力される。一方、図４（ｂ）に示すように、ＧＰＳ受信機１０１によるドップラー速度がマルチパスによるノイズを含んでいても、移動体高精度速度計測装置１０は、ノイズの影響を小さくし、非常に滑らかな波形によって移動体の速度を出力する。
【００４８】
図５は、本発明の一実施形態に係る移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度の立ち上がりと、ＧＰＳ受信機１０１から出力されるドップラー速度の立ち上がりとを示す図である。図５（ａ）は、移動体高精度速度計測装置１０が出力した速度と、ＧＰＳ受信機１０１によるドップラー速度と、ドップラー速度を１Ｈｚのローパス処理をした速度との比較を示す図である。図５（ｂ）は、図５（ａ）の一部（破線による円によって囲まれた部分）を拡大した図である。
【００４９】
図５（ｂ）に示すように、マルチパスによるノイズを含むドップラー速度と、マルチパスによるノイズを含むドップラー速度をローパス処理によってノイズ除去した速度とに比較して、移動体高精度速度計測装置１０が出力する速度は、ノイズ除去のための演算による出力遅れを小さくして、リアルタイム性を実現している。
【００５０】
図６は、本発明の一実施形態に係る移動体高精度速度計測装置１０によって出力されるリアルタイム補間速度と、ＧＰＳ受信機１０１から出力されるドップラー速度と、光学式速度計測装置から出力される速度とを示す図である。ここで、光学式速度計測装置は、路面の不規則な模様から、特定の間隔（例えば、２．３ｍｍ）によるクシ型構造の特殊受光素子によって特定の間隔の反射ムラだけを抽出し、抽出した反射ムラの計数値に特定の間隔をかけて算出した高精度の速度を出力する。
【００５１】
図６が示すように、移動体高精度速度計測装置１０は、マルチパスによるノイズを含むドップラー速度におけるノイズの影響を小さくし、リアルタイムに移動体の速度を出力し、出力した速度は、光学式速度計測装置による高精度の速度と同様である。
【００５２】
本実施形態によれば、移動体高精度速度計測装置１０は、ＩＭＵ１０２を使用して移動体の加速度及び角速度を計測し、計測した移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出する。次に、移動体高精度速度計測装置１０は、ＧＰＳ受信機１０１を使用してＧＰＳ搬送波のドップラーシフト量から移動体のドップラー速度を測定し、測定したドップラー速度から算出した加速度と、計測した加速度との差分を算出し、算出した差分に基づいて係数を算出する。次に、移動体高精度速度計測装置１０は、係数が算出されるための時間だけ、測定したドップラー速度を演算に用いる時間を遅延させ、遅延させたドップラー速度と、フィードバックさせたリアルタイム補間速度との同期化を行い、同期化されたドップラー速度とリアルタイム補間速度との誤差量を算出する。次に、移動体高精度速度計測装置１０は、算出した誤差量に、算出した係数を乗算し、係数を乗算した誤差量からリアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算する。そして、移動体高精度速度計測装置１０は、ストラップダウン演算により算出した速度に、カルマンフィルタによって推定演算された調整量を融合してリアルタイム補間速度を算出し、算出したリアルタイム補間速度を出力すると共にフィードバックさせる。したがって、移動体高精度速度計測装置１０は、木々やビル群等によってＧＰＳ衛星からの信号が反射されてマルチパスが発生する状況下であっても、高精度及び高信頼の速度をリアルタイムに出力することができる。
【００５３】
以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限るものではない。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されるものではない。
【符号の説明】
【００５４】
１０ 移動体高精度速度計測装置
１１ 加速度計測部
１２ ストラップダウンナビゲータ部
１３ 速度測定部
１４ 良否係数算出部
１５ 遅延処理部
１６ 同期化処理部
１７ 良否係数乗算部
１８ カルマンフィルタ部
１０１ ＧＰＳ受信機
１０２ ＩＭＵ
【特許請求の範囲】
【請求項１】
移動体の速度を計測する移動体高精度速度計測装置であって、
ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測手段と、
前記加速度計測手段によって計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータ手段と、
ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定手段と、
前記速度測定手段によって測定された前記ドップラー速度から算出した加速度と、前記加速度計測手段によって計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出手段と、
前記良否係数算出手段によって前記係数が算出されるための時間だけ、前記速度測定手段によって測定された前記ドップラー速度を演算に用いる時間を遅延させる遅延処理手段と、
前記遅延処理手段によって遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータ手段からフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理手段と、
前記同期化処理手段によって算出された前記誤差量に、前記良否係数算出手段によって算出された前記係数を乗算する良否係数乗算手段と、
前記良否係数乗算手段によって前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタ手段と、
を備え、
前記ストラップダウンナビゲータ手段は、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタ手段によって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、移動体高精度速度計測装置。
【請求項２】
ＩＭＵとＧＰＳ受信機と備え、移動体の速度を計測する移動体高精度速度計測装置が実行する方法であって、
前記ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測ステップと、
計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータステップと、
前記ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定ステップと、
測定された前記ドップラー速度から算出した加速度と、計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出ステップと、
前記係数が算出されるための時間だけ、前記ドップラー速度を演算に用いる時間を遅延させる遅延処理ステップと、
遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータステップからフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理ステップと、
算出された前記誤差量に、算出された前記係数を乗算する良否係数乗算ステップと、
前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタステップと、を含み、
前記ストラップダウンナビゲータステップは、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタステップによって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、方法。
【請求項１】
移動体の速度を計測する移動体高精度速度計測装置であって、
ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測手段と、
前記加速度計測手段によって計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータ手段と、
ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定手段と、
前記速度測定手段によって測定された前記ドップラー速度から算出した加速度と、前記加速度計測手段によって計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出手段と、
前記良否係数算出手段によって前記係数が算出されるための時間だけ、前記速度測定手段によって測定された前記ドップラー速度を演算に用いる時間を遅延させる遅延処理手段と、
前記遅延処理手段によって遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータ手段からフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理手段と、
前記同期化処理手段によって算出された前記誤差量に、前記良否係数算出手段によって算出された前記係数を乗算する良否係数乗算手段と、
前記良否係数乗算手段によって前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタ手段と、
を備え、
前記ストラップダウンナビゲータ手段は、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタ手段によって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、移動体高精度速度計測装置。
【請求項２】
ＩＭＵとＧＰＳ受信機と備え、移動体の速度を計測する移動体高精度速度計測装置が実行する方法であって、
前記ＩＭＵを使用して前記移動体の加速度及び角速度を計測する加速度計測ステップと、
計測された前記移動体の加速度及び角速度からストラップダウン演算により速度を算出し、算出した速度に基づいてリアルタイム補間速度を算出するストラップダウンナビゲータステップと、
前記ＧＰＳ受信機を使用してＧＰＳ搬送波のドップラーシフト量から前記移動体のドップラー速度を測定する速度測定ステップと、
測定された前記ドップラー速度から算出した加速度と、計測された加速度との差分を算出し、算出した差分に基づいて係数を算出する良否係数算出ステップと、
前記係数が算出されるための時間だけ、前記ドップラー速度を演算に用いる時間を遅延させる遅延処理ステップと、
遅延させた前記ドップラー速度と、前記ストラップダウンナビゲータステップからフィードバックさせた前記リアルタイム補間速度との同期化を行い、同期化された前記ドップラー速度と前記リアルタイム補間速度との誤差量を算出する同期化処理ステップと、
算出された前記誤差量に、算出された前記係数を乗算する良否係数乗算ステップと、
前記係数が乗算された誤差量から前記リアルタイム補間速度に対する調整量を、カルマンフィルタによって推定演算するカルマンフィルタステップと、を含み、
前記ストラップダウンナビゲータステップは、前記ストラップダウン演算により算出された速度に、前記カルマンフィルタステップによって推定演算された前記調整量を融合して前記リアルタイム補間速度を算出する、方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１２−４２３１８（Ｐ２０１２−４２３１８Ａ）
【公開日】平成２４年３月１日（２０１２．３．１）
【国際特許分類】
【出願番号】特願２０１０−１８３４８９（Ｐ２０１０−１８３４８９）
【出願日】平成２２年８月１８日（２０１０．８．１８）
【出願人】（０００１４５８０６）株式会社小野測器 (230)
【Ｆターム（参考）】
【公開日】平成２４年３月１日（２０１２．３．１）
【国際特許分類】
【出願日】平成２２年８月１８日（２０１０．８．１８）
【出願人】（０００１４５８０６）株式会社小野測器 (230)
【Ｆターム（参考）】
[ Back to top ]