(57)【特許請求の範囲】
【請求項１】
移動物体が受信する電波が有するドップラシフトを含む信号をフィルタリングする２次の第１ローパスフィルタと、
フィルタリングされた信号に含まれる前記ドップラシフトに基づいて、第１速度を求める速度計算部と、
前記移動物体の加速度を計測する加速度計測部と、
前記加速度の時間微分を求める微分部と、
前記加速度の時間微分を時間について２回積分して求めた速度を含む第２速度信号を出力する積分部と、
前記第１ローパスフィルタと同じカットオフ周波数を有し、前記第２速度信号をフィルタリングする２次の第２ローパスフィルタと、
前記第２速度信号により決定される速度と、前記第２速度信号をフィルタリングした信号により決定される速度との速度差を求める差分部と、
前記速度差を用いて、前記第１速度を補正する補正部と、
を備える速度計測装置。
【請求項２】
前記第１速度及び前記速度差を周期的に求めており、
前記速度差を求める周期は、前記第１速度を求める周期よりも短い請求項１に記載の速度計測装置。
【請求項３】
前記電波を受信して、前記ドップラシフトを含む信号を出力する周波数計測部を備える請求項１又は２に記載の速度計測装置。
【請求項４】
前記周波数計測部は、前記電波としてＧＰＳ電波を受信する請求項３に記載の速度計測装置。
【請求項５】
前記加速度計測部は、直交する３軸方向それぞれの加速度成分を計測する請求項１〜３の何れか一項に記載の速度計測装置。
【請求項６】
請求項１〜４の何れか一項に記載の速度計測装置を有する移動物体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、速度計測装置及び移動物体に関する。
【背景技術】
【０００２】
従来、移動物体の制御において、移動物体の速度を計測しながら目的とする位置へ移動するように移動物体の運動を制御することが行われている。
【０００３】
移動物体の速度を計測する方法として、例えば、電波又はそのドップラ周波数を用いて、非接触的に速度を計測することがある。
【０００４】
航法センサを有する飛翔体は、例えば、ドップラシフトを有するＧＰＳ電波を受信してドップラ周波数を求め、ドップラ周波数に基づいて速度を計算して、自己の推進力及び姿勢を制御して、目標に到達するように運動が制御される。
【０００５】
ドップラ周波数を求める周波数計測回路では、受信したＧＰＳ電波と一致するレプリカ信号を生成し、ＧＰＳ衛星が送信している基本周波数とレプリカ信号の周波数との偏差から、ドップラ周波数を求める。
【０００６】
周波数計測回路は、レプリカ信号を生成するために、通常、周波数同期回路が用いられる。周波数同期回路を用いることにより、飛翔体の運動に対して高い速度追従性が得られる。
【０００７】
周波数同期回路は、受信したＧＰＳ電波とレプリカ信号との差信号を２次のローパスフィルタによりフィルタリングして雑音を除去し、フィルタリング後の差信号に基づいてレプリカ信号を繰り返して生成するフィードバック処理を行う。
【０００８】
２次のローパスフィルタのフィルタ特性はフィルタ定数によって設定されるが、飛翔体の場合、計測される速度の加速度への追従性を高めるために、加速度への除去効果が低く設定される。一方、フィルタ定数は、加速度の時間微分（いわゆるジャーク）に対しては、除去効果が高く設定される。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２０１２−２２０２７９号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
ロケット等の飛翔体は、推進力又は姿勢の変化により大きな加速度変化が生じる場合がある。大きな加速度変化によって、大きな加速度の時間微分が生じ得る。この時、ＧＰＳ電波のドップラ周波数には、飛翔体の大きな加速度変化にともなって、急激な変化が生じる。
【００１１】
しかし、上述したフィルタ特性を有する２次のローパスフィルタは、加速度の時間微分に対する除去効果が高いので、２次のローパスフィルタによりフィルタリングされたドップラ周波数に基づいて計算された速度には、現実の飛翔体の速度に対する遅れが生じるおそれがある。
【００１２】
そこで、加速度の時間微分への追従性を高めるために、２次のローパスフィルタの加速度の時間微分への除去効果を低く設定すると、計測される速度の加速度への追従性が低下する問題が生じる。
【００１３】
本明細書では、上述した問題を解決し得る速度計測装置を提供することを課題とする。
【００１４】
また、本明細書では、上述した問題を解決し得る速度計測装置を備える移動物体を提供することを課題とする。
【課題を解決するための手段】
【００１５】
本明細書に開示する速度計測装置によれば、ドップラシフトを含む信号をフィルタリングする２次の第１ローパスフィルタと、フィルタリングされた信号に含まれる上記ドップラシフトに基づいて、第１速度を求める速度計算部と、加速度を計測する加速度計測部と、上記加速度の時間微分を求める微分部と、上記加速度の時間微分を時間について２回積分して求めた速度を含む第２速度信号を出力する積分部と、上記第１ローパスフィルタと同じカットオフ周波数を有し、上記第２速度信号をフィルタリングする２次の第２ローパスフィルタと、上記第２速度信号により決定される速度と、上記第２速度信号をフィルタリングした信号により決定される速度との速度差を求める差分部と、上記速度差を用いて、上記第１速度を補正する補正部と、を備える。
【００１６】
また、本明細書に開示する移動物体によれば、上述した速度計測装置を備える。
【発明の効果】
【００１７】
上述した本明細書に開示する速度計測装置によれば、速度の計測精度が高い。
【００１８】
また、上述した本明細書に開示する移動物体によれば、速度の計測精度が高い。
【図面の簡単な説明】
【００１９】
【図１】本明細書に開示する速度計測装置の一実施形態を示す図である。
【図２】第３速度と、第１速度の関係を説明する図である。
【図３】本明細書に開示する飛翔体の一実施形態を示す図である。
【発明を実施するための形態】
【００２０】
以下、本明細書で開示する速度計測装置の好ましい一実施形態を、図を参照して説明する。但し、本発明の技術範囲はそれらの実施形態に限定されず、特許請求の範囲に記載された発明とその均等物に及ぶものである。
【００２１】
図１は、本明細書に開示する速度計測装置の一実施形態を示す図である。
【００２２】
本実施形態の速度計測装置１０は、飛翔体等の移動物体に搭載され、移動しながらＧＰＳ電波を受信してドップラ周波数を求め、ドップラ周波数に基づいて、移動物体の速度を計測する。
【００２３】
速度計測装置１０は、ドップラ周波数に基づいて得られた速度に対して、別途計測した移動物体の加速度に基づいて得られた速度補正値を用いて補正することにより、速度の計測精度を向上している。そのため、速度計測装置１０は、大きな加速度変化によって、大きな加速度の時間微分（いわゆるジャーク）が生じ得るような場合でも、精度良く速度を計測することができる。
【００２４】
まず、ドップラ周波数に基づいて速度を求める構成を以下に説明する。
【００２５】
速度計測装置１０は、周波数計測部１１と、２次の第１ローパスフィルタ１２と、速度計測部１３を備える。
【００２６】
周波数計測部１１は、ＧＰＳ電波を受信して、信号Ｒを出力する。周波数計測部１１は、第１ローパスフィルタ１２をローパスフィルタとして使用する周波数同期回路を有しており、受信したＧＰＳ電波の周波数と同期するように、信号Ｒを生成する。移動物体に搭載された速度計測装置１０が受信するＧＰＳ電波は、ドップラシフトを有しているので、信号Ｒは、ドップラシフトを含んでいる。
【００２７】
第１ローパスフィルタ１２は、入力した信号Ｒから所定以上の大きさの周波数成分を除去し、フィルタリングされノイズが低減したドップラシフトを含む信号Ｒｆを出力する。第１ローパスフィルタ１２は、信号Ｒｆを、速度計算部１３及び周波数計測部１１に出力する。
【００２８】
２次の第１ローパスフィルタ１２の伝達関数は、入力信号の２次の項を有する。第１ローパスフィルタ１２の伝達関数のフィルタ定数は、カットオフ周波数及び計算周期に応じて決定される。第１ローパスフィルタ１２の加速度への追従性は、主にフィルタ定数により決定される。
【００２９】
周波数計測部１１は、第１ローパスフィルタ１２から信号Ｒｆをフィードバック入力し、受信したＧＰＳ電波の基本周波数と同期するように信号Ｒを生成することを、所定の周期で繰り返す。周波数計測部１１では、信号Ｒを第１ローパスフィルタ１２によりフィルタリングすることにより、無用な発振が生じることを防止する。信号Ｒｆを生成する周期として、例えば、０．００１秒とすることができる。
【００３０】
速度計算部１３は、フィルタリングされた信号Ｒｆに含まれるドップラシフトに基づいて、第１速度Ｖ１を周期的に求める。ここで、ＧＰＳ電波の基本周波数は、既知であるので、ＧＰＳ電波の基本周波数と信号Ｒｆの周波数との差がドップラ周波数となる。速度計算部１３は、このドップラ周波数に基づいて、第１速度Ｖ１を得る。第１速度Ｖ１を求める周期として、例えば、１０Ｈｚとすることができる。速度計算部１３は、求めた第１速度Ｖ１（Ｖ１ｘ、Ｖ１ｙ、Ｖ１ｚ）を、補正部１９に出力する。第１速度Ｖ１は、ＥＣＥＦ座標系の速度成分を有するベクトル量である。
【００３１】
速度計測装置１０は、４つ以上のＧＰＳ衛星からのＧＰＳ電波を受信してドップラ周波数を求めており、ドップラ周波数に基づいて、移動物体の速度を計測し、計測した複数の速度の平均として、第１速度Ｖ１の大きさを得る。
【００３２】
また、速度計測装置１０は、４つ以上のＧＰＳ衛星からのＧＰＳ電波の信号に含まれるエフェメリスデータに基づいて、第１速度Ｖ１と共に、第１速度Ｖ１のＥＣＥＦ座標系における成分（Ｖ１ｘ、Ｖ１ｙ、Ｖ１ｚ）を得る。
【００３３】
ロケット等の移動物体は、推進力又は姿勢の変化により大きな加速度変化が生じる場合がある。大きな加速度変化によって、大きな加速度の時間微分が生じ得る。この時、ＧＰＳ電波のドップラ周波数には、移動物体の大きな加速度変化にともなって、急激な変化が生じる。
【００３４】
２次の第１ローパスフィルタ１２のフィルタ定数は、加速度への追従性を高めるために、加速度の時間微分に対しては、除去効果が高く設定されている。そのため、移動物体に大きな加速度の時間変化が生じるような運動が生じた時には、第１ローパスフィルタ１２によりフィルタリングされた信号Ｒｆに基づいて得られた第１速度Ｖ１には、現実の移動物体の速度に対して遅れが生じるおそれがある。
【００３５】
そこで、速度計測装置１０は、第１速度に対する補正を行う。
【００３６】
次に、第１速度に対する速度補正値を求める構成を以下に説明する。
【００３７】
速度計測装置１０は、慣性計測部１４と、微分部１５と、積分部１６と、２次の第２ローパスフィルタ１７と、差分部１８を備える。
【００３８】
慣性計測部１４は、直交する３軸方向それぞれの加速度成分を計測する加速度計測部１４ａと、直交する３軸方向それぞれの回りの角速度成分を計測する角速度計測部１４ｂを有する。
【００３９】
慣性計測部１４は、加速度計測部１４ａが計測した加速度成分と、角速度計測部１４ｂが計測した角速度成分に基づいて、ＥＣＥＦ座標系における３軸の加速度成分を有する加速度Ａ（Ａｘ，Ａｙ，Ａｚ）を求めて、微分部１５へ出力する。
【００４０】
微分部１５は、加速度Ａ（Ａｘ，Ａｙ，Ａｚ）の時間微分を求め、求めた加速度の時間微分を積分部１６へ出力する。加速度Ａ（Ａｘ，Ａｙ，Ａｚ）の時間微分は、位置の３階微分であり、ジャークと呼ばれる。加速度計測部１４ａが計測した加速度成分には、通常、一定のバイアスが含まれている。そこで、微分部１５は、加速度Ａ（Ａｘ，Ａｙ，Ａｚ）の時間微分を行って、加速度成分に含まれるバイアスを除去する。
【００４１】
積分部１６は、加速度Ａ（Ａｘ，Ａｙ，Ａｚ）の時間微分を時間について２回積分して求めた第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）を含む第２速度信号Ｑを生成する。積分部１６は、第２速度信号Ｑを第２ローパスフィルタ１７及び差分部１８に出力する。第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）は、ＥＣＥＦ座標系における３軸の速度成分を有する。
【００４２】
ここで、第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）を含む第２速度信号Ｑの信号強度は、時間に対して変化しており、この信号強度の時間変化は、慣性計測部１４が計測した加速度及び角速度の時間変化に基づいている。
【００４３】
第２ローパスフィルタ１７は、第１ローパスフィルタ１２と同じカットオフ周波数を有し、第２速度信号Ｑをフィルタリングする。第２ローパスフィルタ１７は、第１ローパスフィルタ１２と同じ応答特性を有し、入力した信号Ｑから所定以上の大きさの周波数成分を除去し、フィルタリングされノイズが低減したドップラシフトを含む信号Ｑｆを生成する。第２ローパスフィルタ１７は、生成した信号Ｑｆを、差分部１８に出力する。
【００４４】
２次の第２ローパスフィルタ１７の伝達関数は、入力信号の２次の項を有する。第２ローパスフィルタ１７の伝達関数のフィルタ定数は、カットオフ周波数及び計算周期に応じて決定される。第２ローパスフィルタ１７の加速度への追従性は、第１ローパスフィルタ１２と同じになるように決定される。
【００４５】
ここで、第１ローパスフィルタ１２はドップラシフトを含む信号Ｒをフィルタリングするのに対して、第２ローパスフィルタ１７は、第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）を含む第２速度信号Ｑをフィルタリングしている点が異なるが、速度とドップラ周波数とは一意的に対応する物理量である。従って、第２ローパスフィルタ１７が第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）を含む第２速度信号Ｑをフィルタリングすることは、第１ローパスフィルタ１２はドップラシフトを含む信号Ｒをフィルタリングすることに対して、解析的に同じ意味を有する。
【００４６】
第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）を含む第２速度信号Ｑがフィルタリングされた信号Ｑｆにより、速度Ｖ２ｆ（Ｖ２ｆｘ，Ｖ２ｆｙ，Ｖ２ｆｚ）が決定される。第２速度信号Ｑにより決定される速度Ｖ２ｆ（Ｖ２ｆｘ，Ｖ２ｆｙ，Ｖ２ｆｚ））は、ＥＣＥＦ座標系における３軸の速度成分を有する。
【００４７】
差分部１８は、第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）と、第２速度信号Ｑをフィルタリングした信号Ｑｆにより決定される速度Ｖ２ｆ（Ｖ２ｆｘ，Ｖ２ｆｙ，Ｖ２ｆｚ）との速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を求める。第２速度信号Ｑは信号Ｒに対応し、第２速度信号をフィルタリングした信号Ｑｆは信号Ｒｆに対応する。従って、速度差εＶは、第１ローパスフィルタ１２により信号Ｒから除去された信号成分に対応する。差分部１８は、求めた速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を、補正部１９へ出力する。速度差εＶ（εＶｘ，εＶｙ，εＶｚ）は、ＥＣＥＦ座標系における３軸の速度成分を有する。
【００４８】
本実施形態では、差分部１８は、第２速度信号をフィルタリングした信号Ｑｆにより決定される速度Ｖ２ｆ（Ｖ２ｆｘ，Ｖ２ｆｙ，Ｖ２ｆｚ）から、第２速度Ｖ２（Ｖ２ｘ，Ｖ２ｙ，Ｖ２ｚ）を減じて、速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を求める。
【００４９】
速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を求める処理の周期は、第１速度Ｖ１を求める周期よりも短いことが、第１速度Ｖ１の補正を精度良く行う観点から好ましい。速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を求める処理の周期は、例えば、第１速度Ｖ１を求める周期である０．１秒よりも短い０．０１秒とすることができる。
【００５０】
補正部１９は、速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を、第１速度Ｖ１に対する速度補正値として用いる。補正部１９は、速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を用いて、第１速度Ｖ１（Ｖ１ｘ、Ｖ１ｙ、Ｖ１ｚ）を補正する。
【００５１】
本実施形態では、補正部１９は、第１速度Ｖ１（Ｖ１ｘ、Ｖ１ｙ、Ｖ１ｚ）から補正係数ｋを乗じた速度差εＶ（εＶｘ，εＶｙ，εＶｚ）を減じて、第１速度Ｖ１（Ｖ１ｘ、Ｖ１ｙ、Ｖ１ｚ）が補正された第３速度Ｖ３（Ｖ３ｘ、Ｖ３ｙ、Ｖ３ｚ）を求める。ここで、補正係数ｋは、ＥＣＥＦ座標系における３軸の速度成分（ｋｘ、ｋｙ、ｋｚ）を有する。
【００５２】
図２は、第３速度と、第１速度の関係を説明する図である。
【００５３】
大きな加速度の時間微分が生じるような運動を行うロケット等の移動物体では、ＧＰＳ電波を受信し、第１ローパスフィルタ１２によりフィルタリングされた信号Ｒｆに基づいて得られた第１速度Ｖ１には、現実の移動物体の速度に対する遅れが生じる。
【００５４】
第２速度Ｖ２と、第２速度信号Ｑをフィルタリングした信号Ｑｆにより決定される速度Ｖ２ｆとの速度差εＶは、第１ローパスフィルタ１２によって信号Ｒから除かれた信号成分に対応する速度の遅れを、慣性計測部１４が計測した加速度等に基づいて推定したものである。
【００５５】
そこで、図２に示すように、速度差εＶを用いて、第１速度Ｖ１を補正することにより、現実の移動物体の速度が、第３速度Ｖ３として推定される。
【００５６】
上述した本明細書の速度計測装置１０によれば、速度の計測精度が向上する。
【００５７】
上述した実施形態では、差分部１８は、第２速度信号Ｑをフィルタリングした信号Ｑｆにより決定される速度Ｖ２ｆから第２速度Ｖ２を減じて、速度差εＶを求めていたが、第２速度Ｖ２から第２速度信号Ｑをフィルタリングした信号Ｑｆにより決定される速度Ｖ２ｆを減じて、速度差εＶを求めてもよい。この場合、補正部１９は、第１速度Ｖ１に補正係数ｋを乗じた速度差εＶを加えて、第１速度Ｖ１が補正された第３速度Ｖ３を求める。
【００５８】
上述した速度計測装置は、例えば、飛翔体等の移動物体に搭載されて、移動物体の速度を計測するために使用される。
【００５９】
図３は、本明細書に開示する飛翔体の一実施形態を示す図である。
【００６０】
本実施形態の飛翔体２０は、上述した速度計測装置１０を備えている。
【００６１】
飛翔体２０は、ロケットエンジン等の推進部（図示せず）及び制御翼を有しており、ＧＰＳ電波を受信してドップラ周波数を求め、ドップラ周波数に基づいて速度を計算して、推進力及び姿勢を制御して、目標に到達するように運動が制御される。
【００６２】
上述した本実施形態の飛翔体２０によれば、推進力又は姿勢の変化により大きな加速度変化が生じる場合でも、精度の高い速度を計測して、目標に到達するように運動が制御される。
【００６３】
本発明では、上述した実施形態の速度計測装置及び飛翔体は、本発明の趣旨を逸脱しない限り適宜変更が可能である。
【符号の説明】
【００６４】
１０    速度計測装置
１１    ドップラ周波数計測部
１２    第１ローパスフィルタ
１３    速度計測部
１４    慣性計測部
１４ａ    加速度計測部
１４ｂ    角速度計測部
１５    微分部
１６    積分部
１７    第２ローパスフィルタ
１８    差分部
１９    補正部
２０    飛翔体
【図１】
0006504822.tif 000002
【図２】
0006504822.tif 000003
【図３】
0006504822.tif 000004