JP2023149060A - スケールファクタ推定装置、スケールファクタ推定方法、及びスケールファクタ推定プログラム - Google Patents

Abstract

【課題】慣性計測装置のスケールファクタを補正する。
【解決手段】車体に搭載されたＩＭＵの出力に基づいて、車体のヨー方向の方位角変化量を算出する方位角変化量算出部４６と、車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの差分情報から車体のヨー方向の方位角変化量を算出するＧＮＳＳ方位角変化量算出部４２と、方位角変化量算出部４６で算出した方位角変化量に対し、ＧＮＳＳ方位角変化量算出部４２で算出した方位角変化量を参照することによりＩＭＵのスケールファクタを推定すると共に、推定したスケールファクタに基づいてＩＭＵの出力を補正するスケールファクタ推定部４８と、を含む。
【選択図】図３

Description

本発明は、ジャイロを利用したＩＭＵ（Inertial Measurement Unit）等の慣性計測装置のスケールファクタ推定装置、スケールファクタ推定方法、及びスケールファクタ推定プログラムに関する。

慣性計測装置に用いられるジャイロの出力には誤差が含まれるが、主要な誤差として、（１）ゼロ点誤差、又はオフセット誤差とも呼ばれるバイアス誤差 、（２）スケールファクタ誤差、（３）線形性（リニアリティ）、（４）雑音成分等が挙げられる。

図１１は、ジャイロの出力に含まれる誤差を示した概略図である。図１１には、誤差を含まないジャイロ出力を示す真値１２０と、バイアス誤差及びスケールファクタ誤差を含んだジャイロ出力１２２と、バイアス誤差を補正したものの、スケールファクタ誤差を含むバイアス誤差補正後ジャイロ出力１２４とが各々示されている。

バイアス誤差は、縦軸に対する切片１２６として示される。かかるバイアス誤差は、慣性計測装置を搭載した車体が静止状態の場合に、ジャイロ出力１２２が示す角速度が０となる様に補正すればよいため、比較的容易に検出及び補正が可能である。特に、図１１の横軸で示した角速度が小さい場合には、バイアス誤差は、ほぼ一定値のオフセット性の誤差となる。そこで、ＧＮＳＳ（Global Navigation Satellite System：全地球航法衛星システム）等を用いて取得した方位情報を参照することで、車両が略直進走行中、すなわちヨー方向の角速度であるヨーレートが小さい場合にバイアス誤差を補正することが従来から行われている。

スケールファクタ誤差（以下、「スケールファクタ」と略記）１２８は、バイアス誤差補正後ジャイロ出力１２４が示すように、バイアス誤差を補正した後で、真値１２０に対して傾きがずれる誤差である。

図１１に示したように、真値１２０を示す直線も、スケールファクタ１２８を含むバイアス誤差補正後ジャイロ出力１２４を示す直線も、共に原点Ｏを通るので、スケールファクタ１２８は、角速度が小さい場合には小さくなり、角速度が大きい場合に顕著になるという性質がある。

従って、スケールファクタ１２８は、角速度がある閾値以上となる旋回時に、外部の方位角情報、又は方位角速度情報を参照することで、効果的に検出及び補正が可能となる。

特許文献１には、旋回角センサ（ヨーレートジャイロ）による旋回角と、左右の車輪速から求めた旋回角（第１旋回角）又はＧＮＳＳを用いて算出した旋回角（第２旋回角）のいずれかを用いてジャイロ出力のスケールファクタを推定し補正する方位検出装置の発明が開示されている。

特許６４９５６０９号公報

しかしながら、特許文献１に記載の技術は、左右の車輪速から旋回角を求める場合、タイヤがスリップすると旋回角の推定に誤差が発生するという問題がある。また、ＧＮＳＳによって得られる観測量に基づいて、第２旋回角及びその精度を算出することになっているが、どの様に旋回角を算出するかについての詳細な記載がなく、結果的にスケールファクタを的確に推定し補正することが困難であるという問題がある。

本発明は、上記事情を鑑みて成されたものであり、慣性計測装置のスケールファクタを補正できるスケールファクタ推定装置、スケールファクタ推定方法、及びスケールファクタ推定プログラムを提供することを目的とする。

上記目的を達成するために、請求項１に係るスケールファクタ推定装置は、車体に搭載された慣性計測装置の出力に基づいて、前記車体のヨー方向の方位角変化量を算出する慣性方位角変化量算出部と、前記車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの差分情報から前記車体のヨー方向の方位角変化量を算出する衛星測位方位角変化量算出部と、前記慣性方位角変化量算出部で算出した方位角変化量に対し、前記衛星測位方位角変化量算出部で算出した方位角変化量を参照することにより前記慣性計測装置のスケールファクタを推定すると共に、推定したスケールファクタに基づいて前記慣性計測装置の出力を補正するスケールファクタ推定部と、を含む。

請求項２に係るスケールファクタ推定装置は、前記慣性計測装置が出力したヨー方向の方位角速度が所定の閾値以上の場合に前記車体が旋回中であると判定する旋回状態判定部をさらに含み、前記スケールファクタ推定部は、前記旋回状態判定部が、前記車体が旋回中と判定した際に前記慣性方位角変化量算出部、及び前記衛星測位方位角変化量算出部の各々で算出された前記車体のヨー方向の方位角変化量に基づいて前記慣性計測装置のスケールファクタを推定する。

請求項３に係るスケールファクタ推定装置は、衛星測位方位角変化量算出部は、前記第１アンテナと前記第２アンテナとの間の相対測位の結果から所定の時間間隔で推定した前記車体のヨー方向の方位角に基づいて方位角変化量を算出し、前記慣性方位角変化量算出部は、前記慣性計測装置が出力した前記車体のヨー方向の方位角速度を、前記所定の時間間隔で積分することにより方位角変化量を算出する。

請求項４に係るスケールファクタ推定装置は、衛星測位方位角変化量算出部は、前記第１アンテナと前記第２アンテナとの間の相対速度ベクトルに基づいて前記車体のヨー方向の方位角変化量である方位角速度を算出し、前記慣性方位角変化量算出部は、前記慣性計測装置が出力した前記車体のヨー方向の方位角速度を方位角変化量として出力する。

請求項５に係るスケールファクタ推定装置は、前記スケールファクタ推定部は、前記旋回状態判定部が、前記車体が旋回中と判定した際に、前記衛星測位方位角変化量算出部が算出した方位角変化量に対する前記慣性方位角変化量算出部が算出した方位角変化量の比から前記スケールファクタを推定する。

請求項６に係るスケールファクタ推定装置は、前記スケールファクタ推定部は、前記旋回状態判定部が、前記車体が旋回を開始したと判定した時から、前記慣性方位角変化量算出部、及び前記衛星測位方位角変化量算出部のいずれかによる前記車体のヨー方向の方位角変化量に基づいて得られた前記車体の方位角の変化が所定の方位角変化量閾値を超えた場合に、前記慣性方位角変化量算出部、及び前記衛星測位方位角変化量算出部の各々で算出された前記車体のヨー方向の方位角変化量に基づいて前記慣性計測装置のスケールファクタを推定する。

上記目的を達成するために、請求項７に係るスケールファクタ推定方法は、車体に搭載された慣性計測装置の出力に基づいて、前記車体のヨー方向の方位角変化量を算出する慣性方位角変化量算出工程と、前記車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの間の差分情報から前記車体のヨー方向の方位角変化量を算出する衛星測位方位角変化量算出工程と、前記慣性方位角変化量算出工程で算出した方位角変化量に対し、前記衛星測位方位角変化量算出工程で算出した方位角変化量を参照することにより前記慣性計測装置のスケールファクタを推定すると共に、推定したスケールファクタに基づいて前記慣性計測装置の出力を補正するスケールファクタ推定工程と、を含む。

上記目的を達成するために、請求項８に係るスケールファクタ推定プログラムは、コンピュータを、車体に搭載された慣性計測装置の出力に基づいて、前記車体のヨー方向の方位角変化量を算出する慣性方位角変化量算出部、前記車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの間の差分情報から前記車体のヨー方向の方位角変化量を算出する衛星測位方位角変化量算出部、及び前記慣性方位角変化量算出部で算出した方位角変化量に対し、前記衛星測位方位角変化量算出部で算出した方位角変化量を参照することにより前記慣性計測装置のスケールファクタを推定すると共に、推定したスケールファクタに基づいて前記慣性計測装置の出力を補正するスケールファクタ推定部として機能させる。

本発明のスケールファクタ推定装置、スケールファクタ推定方法、及びスケールファクタ推定プログラムによれば、慣性計測装置のスケールファクタを補正できる。

本発明の第１実施形態に係るスケールファクタ推定装置の構成の一例を示したブロック図である。 本発明の第１実施形態における第１ＧＮＳＳアンテナ及び第２ＧＮＳＳアンテナの配置の一例を示した概略図である。 本発明の第１実施形態に係るスケールファクタ推定装置の演算装置の具体的な構成の一例を示すブロック図である。 （Ａ）は、右旋回中の車体において、第１ＧＮＳＳアンテナ及び第２ＧＮＳＳアンテナの各々の位置の移動方向を示した概略図であり、（Ｂ）は、車体の例えば重心位置、第１ＧＮＳＳアンテナ及び第２ＧＮＳＳアンテナの各々の方位角の変化の一例を示した概略図である。 本発明の第１実施形態に係るスケールファクタ推定装置の処理の流れの概略を示したブロック図である。 本発明の第１実施形態に係るスケールファクタ推定装置の処理の一例を示したフローチャートである。 本発明の第２実施形態に係るスケールファクタ推定装置の演算装置の具体的な構成の一例を示すブロック図である。 （Ａ）は、右旋回中の車体において、第１ＧＮＳＳアンテナ及び第２ＧＮＳＳアンテナの各々の配置と、第１ＧＮＳＳアンテナ及び第２ＧＮＳＳアンテナの各々の速度ベクトルの一例を示した概略図であり、（Ｂ）は、相対速度ベクトルとアンテナ間基線ベクトルとの関係を示した説明図である。 本発明の第２実施形態に係るスケールファクタ推定装置の処理の流れの概略を示したブロック図である。 本発明の第２実施形態に係るスケールファクタ推定装置の処理の一例を示したフローチャートである。 ジャイロの出力に含まれる誤差を示した概略図である。

［第１実施形態］
以下、図面を参照して本発明の第１実施形態を詳細に説明する。図１は、本実施形態に係るスケールファクタ推定装置１０の構成の一例を示したブロック図である。スケールファクタ推定装置１０は、演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、撮像装置２２が取得した画像データから道路の白線等の情報を抽出する画像情報処理部２０と、画像情報処理部２０が抽出した情報、車速センサ２４が検出した車体の前後速度、３軸のジャイロと３方向の加速度計によって、３次元の角速度と加速度とが検出可能なＩＭＵ２６が検出した車体の方位角の角速度及び加速度、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で衛星から受信したＧＮＳＳの情報、並びに地図情報データベース３４に格納された地図情報が各々入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいてＩＭＵ２６が出力する角速度の誤差を補正する演算を行なうコンピュータ等で構成された演算装置１４と、車体２００の外界との無線通信が可能なＶ２Ｘ通信部３６と、演算装置１４による演算結果等を表示可能なＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。

本実施形態に係る撮像装置２２は車載カメラ等であり、一例として、撮影により取得した車両周辺の画像情報を解析して走路の白線、及び建造物等をエッジとして検出する。ＩＭＵ２６は、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能な慣性計測装置である。

図２は、本実施形態における第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の配置の一例を示した概略図である。

図２に示したように、本実施形態では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の差分情報を取得可能なように、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々を、所定以上の距離を隔てて、車体２００の前後方向であるＸ軸方向に平行して配置している。一般に２つのアンテナ間距離を長く取るほど、方位角の推定精度が向上するので、第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との間の所定以上の距離は、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の基線長を可能な限り長くするために、例えば、車体２００のルーフの前端から後端までの距離に近い値である。具体的に所定以上の距離は、少なくとも５０ｃｍ以上あることが好ましい。本実施形態では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々を車体２００の前後方向であるＸ軸方向に平行して配置することは必須ではない。従って、例えば、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々をルーフの対角線上に配置してもよい。かかる配置であれば、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々を車体２００の前後方向であるＸ軸方向に平行して配置する場合よりも、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の基線長を、より長くすることができる。後述するように、本実施形態では、ある同時刻の第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との差分情報を用いることから、車両速度、又は車体２００の進行方向に依存せずに、車体２００の方位角情報を精度よく取得することが可能となる。第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０で取得した情報に基づく方位角変化量の推定の詳細については後述する。

図３は、演算装置１４の具体的な構成の一例を示すブロック図である。演算装置１４は、一種のコンピュータであり、ＣＰＵ（Central Processing Unit）１４Ｂ、ＲＯＭ（Read Only Memory）１４Ａ、ＲＡＭ（Random Access Memory）１４Ｃ、及び入出力ポート１４Ｄを備える。

演算装置１４では、ＣＰＵ１４Ｂ、ＲＯＭ１４Ａ、ＲＡＭ１４Ｃ、及び入出力ポート１４Ｄがアドレスバス、データバス、及び制御バス等の各種バスを介して互いに接続されている。入出力ポート１４Ｄには、各種の入出力機器として、入力装置１２、ハードディスク（ＨＤＤ）等である記憶装置１８、表示装置１６、及びＶ２Ｘ通信部３６等が各々接続されている。

記憶装置１８には、スケールファクタの推定のためのスケールファクタ推定プログラムがインストールされている。本実施形態では、ＣＰＵ１４Ｂがスケールファクタ推定プログラムを実行することにより、スケールファクタ推定を実行する。また、ＣＰＵ１４Ｂは、スケールファクタ推定プログラムによる処理結果を表示装置１６に表示させる。なお、本実施形態のスケールファクタ推定プログラムを演算装置１４にインストールするには、幾つかの方法があるが、例えば、スケールファクタ推定プログラムをセットアッププログラムと共にＣＤ－ＲＯＭやＤＶＤ等に記憶しておき、入出力装置であるディスクドライブ等にディスクをセットし、ＣＰＵ１４Ｂに対してセットアッププログラムを実行することにより記憶装置１８にスケールファクタ推定プログラムをインストールする。または、公衆電話回線又はネットワークを介して演算装置１４と接続される他の情報処理機器と通信することで、記憶装置１８にスケールファクタ推定プログラムをインストールするようにしてもよい。

次に、演算装置１４のＣＰＵ１４Ｂがスケールファクタ推定プログラムを実行することで実現される各種機能について説明する。スケールファクタ推定プログラムは、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の相対測位結果から車体２００の方位角を推定するＧＮＳＳ方位角推定機能、推定した方位角の情報を所定の時間間隔で得ることで方位角変化量を算出するＧＮＳＳ方位角変化量算出機能、ＩＭＵ２６が備えるヨーレートジャイロの出力に基づいて車体２００の旋回状態を判定する旋回状態判定機能、ヨーレートジャイロの出力に基づいて車体２００の方位角変化量を算出する方位角変化量算出機能、及びＧＮＳＳを用いて算出した方位角変化量と、ヨーレートジャイロの出力に基づいて算出した方位角変化量とを比較してスケールファクタを推定するスケールファクタ推定機能として機能させる。ＣＰＵ１４Ｂは、スケールファクタ推定プログラムを実行することにより、ＧＮＳＳ方位角推定部４０、ＧＮＳＳ方位角変化量算出部４２、旋回状態判定部４４、方位角変化量算出部４６及びスケールファクタ推定部４８として機能する。

図４（Ａ）は、右旋回中の車体２００において、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の位置の移動方向を示した概略図であり、図４（Ｂ）は、車体２００の例えば重心位置、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の方位角の変化の一例を示した概略図である。図４（Ｂ）には、車体２００の方位角を示す車体方位角１１０、第１ＧＮＳＳアンテナ２８の方位角を示す第１ＧＮＳＳアンテナ方位角１１２、及び第２ＧＮＳＳアンテナ３０の方位角を示す第２ＧＮＳＳアンテナ方位角１１４の各々が示されている。

図４（Ａ）に示したように、右旋回における第１ＧＮＳＳアンテナ２８は第１速度ベクトル７０で示したような、第２ＧＮＳＳアンテナ３０は第２速度ベクトル７２で示したような、各々異なる方向に進行する。その結果、図４（Ｂ）において、車体２００が右旋回を開始すると、第２ＧＮＳＳアンテナ方位角１１４は、車体方位角１１０、及び第１ＧＮＳＳアンテナ方位角１１２に先行して変化し、次いで車体方位角１１０と、第１ＧＮＳＳアンテナ方位角１１２との順で変化する。第２ＧＮＳＳアンテナ方位角１１４は、車体２００が右旋回を終了する前に変化が止まり、次いで車体方位角１１０と、第１ＧＮＳＳアンテナ方位角１１２との順で変化が止まる。

図４（Ａ）、及び図４（Ｂ）に示したように、ＧＮＳＳによる方位推定では、車体２００の旋回中に、車体方位角１１０に対して第１ＧＮＳＳアンテナ方位角１１２及び第２ＧＮＳＳアンテナ方位角１１４の各々が示す方位角がずれる現象が生じる。このような方位角のずれの大きさは、車体２００の速度や旋回半径の変化に伴って変動する。従って、ＧＮＳＳのアンテナが１本だけでは、精度よく車体２００の方位角変化（旋回角）を推定することが困難となる。

しかしながら、本実施形態のように、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々を用いた相対測位では、図４（Ａ）、及び図４（Ｂ）に示したようなアンテナの設置位置に依存して発生するアンテナの進行方向の方位ずれの影響を受けないので、車体２００の方位角を精度よく算出することができる。

図５は、本実施形態に係るスケールファクタ推定装置１０の処理の流れの概略を示したブロック図である。ブロックＢ１００では、第１ＧＮＳＳアンテナ２８で受信した情報を入力装置１２に含まれる第１ＧＮＳＳ受信機で、ブロックＢ１０２では、第２ＧＮＳＳアンテナ３０で受信した情報を入力装置１２に含まれる第２ＧＮＳＳ受信機で、各々取得する。

ブロックＢ１０４では、ＧＮＳＳ方位角推定部４０において、第１ＧＮＳＳ受信機と第２ＧＮＳＳ受信機との各々で取得した情報に基づいて車体２００の方位角を推定する。

ブロックＢ１０４では、ある同時刻における第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の間の差分情報を用いる相対測位により、車体２００の方位角を推定する。ブロックＢ１０４で相対測位結果を得るには、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で個別に同時刻の測位結果を求めて差分を取ってもよい。また、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の間の疑似距離情報の差分を用いるＤ-ＧＮＳＳ（Differential Global Navigation Satellite System）又は搬送波位相情報の差分を用いるＭＢ-ＲＴＫ（Moving Base Real Time Kinematic）等の技術を用いて、相対測位演算を行い、相対測位結果を求めてもよい。かかる相対測位結果から車体２００の方位角情報を取得することができる。

ブロックＢ１０６では、ブロックＢ１０４において、所定の時間間隔で推定した方位角の情報を得ることで方位角変化量を求める。スケールファクタは、ある程度ヨーレートの値が大きくなる旋回中でなければ精度よく検出できないので、ブロックＢ１０６での方位角変化量算出は、後述するブロックＢ１１０に示した旋回状態判定部４４での判定結果に従って実行する。

ブロックＢ１０８では、ヨーレートジャイロの出力が入力される。ブロックＢ１０８で入力されるヨーレートジャイロの出力は、バイアス誤差が予め補正されている。

ブロックＢ１１０では、旋回状態判定部４４において、ヨーレートジャイロが出力したヨーレートが所定の閾値以上であるか否かを判定する。ヨーレートの閾値は、設計段階でのシミュレーション、又は実車を用いた実験を通じて具体的に決定する。ブロックＢ１１０での判定結果は、ブロックＢ１０６とブロックＢ１１２とに出力される。

ブロックＢ１１２では、方位角変化量算出部４６において、ヨーレートジャイロが出力したヨーレートに基づいて車体２００の方位角の変化量を推定する。ヨーレートジャイロの出力が示す瞬時での方位角の変化量はヨーレートそのものであるが、当該ヨーレートを、所定の時間間隔で積分すれば、車体２００の方位角の変化量を算出できる。ブロックＢ１１２における所定の時間間隔は、ブロックＢ１０６における所定の時間間隔と同一である。

ブロックＢ１１４では、スケールファクタ推定部４８において、ヨーレートジャイロによる方位角変化量とＧＮＳＳの相対測位に基づく方位角変化量とを比較して、スケールファクタを補正する。スケールファクタは、ヨーレートの値に係数として係るため、例えば、参照値であるＧＮＳＳの相対測位に基づく方位角変化量に対するヨーレートジャイロによる方位角変化量の比を算出することで推定し、推定したスケールファクタでＩＭＵ２６の出力を補正する。

図６は、本実施形態に係るスケールファクタ推定装置１０の処理の一例を示したフローチャートである。ステップＳ１００では、ヨーレートジャイロのバイアス誤差を推定し、推定したバイアス誤差でヨーレートジャイロの出力を補正する。バイアス誤差は、ＩＭＵ２６を搭載した車体２００が静止状態の場合に、ヨーレートジャイロが示す角速度が０となる様に補正すればよい。より具体的には、ＧＮＳＳ等を用いて取得した方位情報を参照して車両が略直進走行中であると推定される場合に、ＩＭＵ２６が出力したヨーレートをバイアス誤差として検出し、検出したバイアス誤差でＩＭＵ２６の出力を補正する。

ステップＳ１０２では、車体２００が旋回を開始したか否かを判定する。車体２００が旋回を開始すると、ＩＭＵ２６のヨーレートジャイロが出力したヨーレートが変化するので、ヨーレートジャイロが出力したヨーレートが所定の閾値以上の場合に、車体２００が旋回を開始したと判定する。判定に用いるヨーレートは、ＧＮＳＳ等を用いて取得した車体２００の方位角の微分値でもよい。ステップＳ１０２で、車体２００が旋回を開始したと判定した場合は手順をステップＳ１０４に移行し、車体２００が旋回を開始していないと判定した場合は車体２００が旋回を開始したか否かの判定を継続する。

ステップＳ１０４では、ヨーレートジャイロの出力に基づいて、車体２００の方位角変化量を推定するヨーレートジャイロ方位角変化量推定を行う。ステップＳ１０４では、ヨーレートジャイロが出力したヨーレートを、所定の時間間隔で積分することにより、車体２００の方位角の変化量を算出する。

ステップＳ１０６では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で取得した情報に基づいて車体２００の方位角を推定すると共に、当該方位角の変化量を算出する。具体的には、ある同時刻における第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の間の差分情報を用いる相対測位により、車体２００の方位角を推定する。相対測位結果は、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で個別に同時刻の測位結果を求めて差分を取ってもよい。さらには、第１ＧＮＳＳアンテナ２８で取得した情報、及び第２ＧＮＳＳアンテナ３０で取得した情報の差分を取ることで、両者で共通する誤差を相殺することにより高精度な測位を行うＤ-ＧＮＳＳ、又は第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との間における搬送波の位相差から算出した搬送波の行路差に基づいて測位を行うＭＢ-ＲＴＫ等の技術を用いて、相対測位演算を行い、相対測位結果を求めてもよい。第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の間の距離は変化しないので、当該距離を拘束条件とすることで、方位角の推定精度を向上させることができる。ステップＳ１０６では、かかる相対測位結果から車体２００の方位角を算出すると共に、所定の時間間隔で推定した方位角の差分から車体２００の方位角変化量を求める。

ステップＳ１０８では、ヨーレートジャイロによる方位角変化量とＧＮＳＳの相対測位に基づく方位角変化量とを比較して、スケールファクタを補正する。前述のように、スケールファクタは、ヨーレートの値に係数として係るため、ＧＮＳＳの相対測位に基づく方位角変化量とヨーレートジャイロによる方位角変化量との比を算出することで推定できる。そして、推定したスケールファクタでＩＭＵ２６の出力を補正する。

ステップＳ１１０では、車体２００が旋回を終了したか否かを判定する。具体的には、ヨーレートジャイロが出力したヨーレートが前述の所定の閾値未満の場合に、車体２００が旋回を終了したと判定する。判定に用いるヨーレートは、ＧＮＳＳ等を用いて取得した車体２００の方位角の微分値でもよい。ステップＳ１１０で、車体２００の旋回状態が継続していると判定した場合は、手順をステップＳ１０４に移行し、再度、方位角変化量の推定およびスケールファクタの補正を継続する。ステップＳ１１０で、車体２００の旋回状態が終了したと判定した場合は、スケールファクタ誤差の補正の処理を終了する。

以上説明したように、本実施形態に係るスケールファクタ推定装置１０によれば、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の２つのＧＮＳＳアンテナと、当該アンテナに対応した受信機とを用いることにより、相対測位が可能となり、スケールファクタ補正のために参照する方位角変化量の推定精度が向上する。

ＧＮＳＳの相対測位による方位角の推定は、急旋回等による車輪のスリップの影響、又は車体２００に対する横滑り角の影響を受けない点によっても、方位角の推定精度が高水準で担保される。

本実施形態に係るスケールファクタ推定装置１０によれば、スケールファクタ補正のために参照する方位角変化量を高精度で推定できることにより、ＩＭＵ２６が含むヨーレートジャイロのスケールファクタをより精度良く推定し、補正することができ、車体２００の方位推定精度を改善することができる。

［第２実施形態］
続いて本発明の第２実施形態について説明する。本実施形態に係るスケールファクタ推定装置の構成は、第１実施形態のスケールファクタ推定装置１０と同様なので、第１実施形態と同一の構成については同一の符号を付して詳細な説明は省略する。

前述のように、第１実施形態では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の２つのＧＮＳＳアンテナを用いた相対測位によって推定した方位角変化量でヨーレートジャイロのスケールファクタを補正した。

本実施形態では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の相対速度ベクトルに基づいて算出した方位角速度を用いてヨーレートジャイロのスケールファクタを補正する。

図７は、本実施形態における演算装置１４の具体的な構成の一例を示すブロック図である。本実施形態では、ＣＰＵ１４Ｂが第１実施形態と異なるスケールファクタ推定プログラムを実行することにより、第１実施形態と異なる態様でヨーレートジャイロのスケールファクタを補正するので、以下、ＣＰＵ１４Ｂが本実施形態に係るスケールファクタ推定プログラムを実行することで実現される各種機能について説明する。本実施形態に係るスケールファクタ推定プログラムは、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々相対速度ベクトルに基づいて車体２００の方位角速度を推定するＧＮＳＳ方位角速度推定機能、ＩＭＵ２６が備えるヨーレートジャイロの出力に基づいて車体２００の旋回状態を判定する旋回状態判定機能、及びＧＮＳＳを用いて算出した方位角速度と、ヨーレートジャイロの出力とを比較してスケールファクタを推定するスケールファクタ推定機能として機能させる。ＣＰＵ１４Ｂは、スケールファクタ推定プログラムを実行することにより、ＧＮＳＳ方位角速度推定部１４０、旋回状態判定部１４２及びスケールファクタ推定部１４４として機能する。

図８（Ａ）は、右旋回中の車体２００において、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の配置と、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の速度ベクトルの一例を示した概略図であり、図８（Ｂ）は、相対速度ベクトルとアンテナ間基線ベクトルとの関係を示した説明図である。

２つのＧＮＳＳアンテナを使用した方位角速度（ヨーレート）の推定では、過去には、ＧＮＳＳのドップラーシフトを用いてＧＮＳＳアンテナの速度ベクトルを求めることが試みられてきた。

本実施形態では、２つのＧＮＳＳアンテナ位置における速度ベクトルの差分、又はドップラーシフトの差分から推定した相対速度ベクトルから車体２００のヨーレートを推定する。

本実施形態では、簡略化のために、水平面内での旋回運動を考え、以下に示すベクトルが東西南北の座標系で求められるとする。

図８（Ａ）に示すように、車体２００が右旋回している場合に、第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との間の相対位置である基線ベクトル７４、第１ＧＮＳＳアンテナ２８の速度ベクトルである第１速度ベクトル７０、第２ＧＮＳＳアンテナ３０の速度ベクトルである第２速度ベクトル７２、及び相対速度ベクトル７６の各々を下記のように設定する。

ここで、車体２００を剛体とすると方位角速度ωの大きさは、下記の式（１）のようになる。
・・・（１）

また、相対速度ベクトル７６の向きは、基線ベクトル７４、及び回転軸ベクトルと互いに直交する方向となる。本実施形態では、水平面内での旋回を考えているため回転軸ベクトルは、鉛直方向ベクトルとなる。

本実施形態では、上記の式（１）で算出した方位角速度ωを参照して、ＩＭＵ２６が含むヨーレートジャイロのスケールファクタを補正する。

図８（Ａ）では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の配置が車体２００の長手方向に対して平行な場合を示したが、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０は、車体２００の長手方向に対して斜め方向、又は車体２００の長手方向に直交する左右方向に各々配置してもよい。かかる態様で第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々を配置しても、アンテナ間の基線ベクトル７４の方向が変わるだけで、上述した各々のベクトル、及び方位角速度ωの関係は成立するからである。

本実施形態では、簡略化のため、水平面内での旋回運動を説明したが、路面又は車体２００が傾いている場合、旋回中の姿勢変化に対応する傾いた回転軸ベクトルを考慮すれば、上述した各々のベクトル、及び方位角速度ωの関係は同様に成立する。

図９は、本実施形態に係るスケールファクタ推定装置１０の処理の流れの概略を示したブロック図である。ブロックＢ２００では、第１ＧＮＳＳアンテナ２８で受信した情報を入力装置１２に含まれる第１ＧＮＳＳ受信機で、ブロックＢ２０２では、第２ＧＮＳＳアンテナ３０で受信した情報を入力装置１２に含まれる第２ＧＮＳＳ受信機で、各々取得する。

ブロックＢ２０４では、ＧＮＳＳ方位角速度推定部１４０において、第１ＧＮＳＳ受信機と第２ＧＮＳＳ受信機との各々で取得した情報に基づいて車体２００の方位角速度ωを推定する。前述のように、スケールファクタは、ある程度ヨーレートの値が大きくなる旋回中でなければ精度よく検出できないので、後述する図１０のように、ブロックＢ２０４での方位角速度ωの推定は、ブロックＢ２０８に示した旋回状態判定部１４２での判定結果に従って実行するようにしてもよい。

ブロックＢ２０６では、ヨーレートジャイロの出力が後続するブロックＢ２０８とブロックＢ２１０とに各々入力される。ブロックＢ２０６で入力されるヨーレートジャイロの出力は、バイアス誤差が予め補正されている。

ブロックＢ２０８では、旋回状態判定部１４２において、ヨーレートジャイロが出力したヨーレートが所定の閾値以上であるか否かを判定する。ヨーレートの閾値は、設計段階でのシミュレーション、又は実車を用いた実験を通じて具体的に決定する。ブロックＢ２０８での判定結果は、ブロックＢ２１０に出力される。

ブロックＢ２１０では、スケールファクタ推定部１４４において、ヨーレートジャイロが出力したヨーレートとＧＮＳＳを用いて推定した方位角速度ωの相対測位に基づく方位角変化量とを比較して、スケールファクタを補正する。スケールファクタは、ヨーレートの値に係数として係るため、例えば、参照値であるＧＮＳＳを用いて推定した方位角速度ωに対するヨーレートジャイロが出力したヨーレートの比を算出することで推定し、推定したスケールファクタでＩＭＵ２６の出力を補正する。

図１０は、本実施形態に係るスケールファクタ推定装置１０の処理の一例を示したフローチャートである。ステップＳ２００では、ヨーレートジャイロのバイアス誤差を推定し、推定したバイアス誤差でヨーレートジャイロの出力を補正する。バイアス誤差は、ＩＭＵ２６を搭載した車体２００が静止状態の場合に、第１実施形態で説明したように、ヨーレートジャイロが示す角速度が０となる様に補正すればよい。

ステップＳ２０２では、車体２００が旋回を開始したか否かを判定する。車体２００が旋回を開始すると、ＩＭＵ２６のヨーレートジャイロが出力したヨーレートが変化するので、ヨーレートジャイロが出力したヨーレートが所定の閾値以上の場合に、車体２００が旋回を開始したと判定する。判定に用いるヨーレートは、ＧＮＳＳ等を用いて推定した方位角速度ωでもよい。ステップＳ２０２で、車体２００が旋回を開始したと判定した場合は手順をステップＳ２０４に移行し、車体２００が旋回を開始していないと判定した場合は車体２００が旋回を開始したか否かの判定を継続する。

ステップＳ２０４では、ヨーレートジャイロの出力であるヨーレートの情報を取得する。

ステップＳ２０６では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で取得した情報に基づいて車体２００の方位角速度ωを推定する。

ステップＳ２０８では、ヨーレートジャイロが出力したヨーレートとＧＮＳＳを用いて推定した方位角速度ωとを比較して、スケールファクタを補正する。前述のように、スケールファクタは、ヨーレートの値に係数として係るため、ＧＮＳＳを用いて推定した方位角速度ωとヨーレートジャイロが出力したヨーレートとの比を算出することで推定できる。そして、推定したスケールファクタでＩＭＵ２６の出力を補正する。

ステップＳ２１０では、車体２００が旋回を終了したか否かを判定する。具体的には、ヨーレートジャイロが出力したヨーレートが前述の所定の閾値未満の場合に、車体２００が旋回を終了したと判定する。判定に用いるヨーレートは、ＧＮＳＳ等を用いて推定した方位角速度ωでもよい。ステップＳ２１０で、車体２００の旋回状態が継続していると判定した場合は、手順をステップＳ２０４に移行し、再度、方位角速度ωの推定およびスケールファクタの補正を継続する。ステップＳ２１０で、車体２００の旋回状態が終了したと判定した場合は、スケールファクタ誤差の補正の処理を終了する。

以上説明したように、本実施形態に係るスケールファクタ推定装置１０によれば、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の相対速度ベクトルに基づいて算出した方位角速度ωを用いてヨーレートジャイロのスケールファクタを補正することができる。

本実施形態では、ヨーレートジャイロの出力であるヨーレートと直接比較が可能な方位角速度ωを第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の相対速度ベクトルに基づいて推定できるので、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の２つのＧＮＳＳアンテナを用いた相対測位によって推定した車体２００の方位角から方位角変化量を算出する第１実施形態よりも処理は簡易であり、より迅速な演算処理が期待できる。

上記各実施形態では、ヨーレートジャイロが出力したヨーレートが所定の閾値以上の場合に、車体２００が旋回を開始したと判定した場合に、方位角変化量を算出してスケールファクタの推定を行ったが、これに限定されない。ＧＮＳＳ及びＩＭＵ２６の出力に基づいて常時、方位角変化量を算出し、車体２００が旋回を開始したと判定した場合に算出した方位角変化量に基づいてスケールファクタの推定を行ってもよい。

また、ヨーレートジャイロの出力に対するスケールファクタの影響は、ヨーレートのわずかな誤差となるため、車体２００の方位角変化量が小さい場合は、ヨーレートジャイロの出力に含まれる雑音成分に埋没してしまうおそれがある。かかる雑音成分とスケールファクタとを明確に識別すべく、車体２００が旋回を開始してから、ＧＮＳＳ又はＩＭＵ２６の出力に基づいて算出された車体２００の方位角の変化が、所定の方位角変化量閾値以上になった場合に、ヨーレートジャイロのスケールファクタを推定してもよい。所定の方位角変化量閾値は、設計段階でのシミュレーション、又は実車を用いての実験を通じて具体的に決定するが、一例として、３０度程度である。

なお、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行したスケールファクタ推定処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、スケールファクタ推定処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

また、上記各実施形態では、スケールファクタ推定プログラムがＲＯＭまたはストレージに予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の非一時的（ｎｏｎ－ｔｒａｎｓｉｔｏｒｙ）記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

なお、特許請求の範囲に記載の「慣性計測装置」は、発明の詳細な説明に記載の「ＩＭＵ２６に、同「慣性方位角変化量算出部」は、発明の詳細な説明に記載の「方位角変化量算出部４６」に、同「衛星測位方位角変化量算出部」は、発明の詳細な説明に記載の「ＧＮＳＳ方位角変化量算出部４２、１４０」に、同「旋回状態判定部」は、発明の詳細な説明に記載の「旋回状態判定部４６、１４２」に、同「スケールファクタ推定部」は、発明の詳細な説明に記載の「スケールファクタ推定部４８、１４４」に、各々該当する。

（付記項１）
メモリと、
前記メモリに接続された少なくとも１つのプロセッサと、
を含み、
前記プロセッサは、
車体に搭載された慣性計測装置の出力に基づいて、前記車体のヨー方向の方位角変化量を算出し、前記車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの差分情報から前記車体のヨー方向の方位角変化量を算出し、前記慣性方位角変化量算出部で算出した方位角変化量に対し、前記衛星測位方位角変化量算出部で算出した方位角変化量を参照することにより前記慣性計測装置のスケールファクタを推定すると共に、推定したスケールファクタに基づいて前記慣性計測装置の出力を補正する、
ように構成されているスケールファクタ推定装置。

１０ スケールファクタ推定装置
１２ 入力装置
１４ 演算装置
１４Ａ ＲＯＭ
１４Ｂ ＣＰＵ
１４Ｃ ＲＡＭ
１４Ｄ 入出力ポート
１６ 表示装置
１８ 記憶装置
２６ ＩＭＵ
２８ 第１ＧＮＳＳアンテナ
３０ 第２ＧＮＳＳアンテナ
３４ 地図情報データベース
４０ 方位角推定部
４２ 方位角変化量算出部
４４ 旋回状態判定部
４６ 方位角変化量算出部
４８ スケールファクタ推定部
７０ 第１速度ベクトル
７２ 第２速度ベクトル
７４ 基線ベクトル
７６ 相対速度ベクトル
１４０ 方位角速度推定部
１４２ 旋回状態判定部
１４４ スケールファクタ推定部
２００ 車体

Claims (8)

1. 車体に搭載された慣性計測装置の出力に基づいて、前記車体のヨー方向の方位角変化量を算出する慣性方位角変化量算出部と、
   前記車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの差分情報から前記車体のヨー方向の方位角変化量を算出する衛星測位方位角変化量算出部と、
   前記慣性方位角変化量算出部で算出した方位角変化量に対し、前記衛星測位方位角変化量算出部で算出した方位角変化量を参照することにより前記慣性計測装置のスケールファクタを推定すると共に、推定したスケールファクタに基づいて前記慣性計測装置の出力を補正するスケールファクタ推定部と、
   を含むスケールファクタ推定装置。
2. 前記慣性計測装置が出力したヨー方向の方位角速度が所定の閾値以上の場合に前記車体が旋回中であると判定する旋回状態判定部をさらに含み、
   前記スケールファクタ推定部は、前記旋回状態判定部が、前記車体が旋回中と判定した際に前記慣性方位角変化量算出部、及び前記衛星測位方位角変化量算出部の各々で算出された前記車体のヨー方向の方位角変化量に基づいて前記慣性計測装置のスケールファクタを推定する請求項１に記載のスケールファクタ推定装置。
3. 衛星測位方位角変化量算出部は、前記第１アンテナと前記第２アンテナとの間の相対測位の結果から所定の時間間隔で推定した前記車体のヨー方向の方位角に基づいて方位角変化量を算出し、
   前記慣性方位角変化量算出部は、前記慣性計測装置が出力した前記車体のヨー方向の方位角速度を、前記所定の時間間隔で積分することにより方位角変化量を算出する請求項２に記載のスケールファクタ推定装置。
4. 衛星測位方位角変化量算出部は、前記第１アンテナと前記第２アンテナとの間の相対速度ベクトルに基づいて前記車体のヨー方向の方位角変化量である方位角速度を算出し、
   前記慣性方位角変化量算出部は、前記慣性計測装置が出力した前記車体のヨー方向の方位角速度を方位角変化量として出力する請求項２に記載のスケールファクタ推定装置。
5. 前記スケールファクタ推定部は、前記旋回状態判定部が、前記車体が旋回中と判定した際に、前記衛星測位方位角変化量算出部が算出した方位角変化量に対する前記慣性方位角変化量算出部が算出した方位角変化量の比から前記スケールファクタを推定する請求項２～４のいずれか１項に記載のスケールファクタ推定装置。
6. 前記スケールファクタ推定部は、前記旋回状態判定部が、前記車体が旋回を開始したと判定した時から、前記慣性方位角変化量算出部、及び前記衛星測位方位角変化量算出部のいずれかによる前記車体のヨー方向の方位角変化量に基づいて得られた前記車体の方位角の変化が所定の方位角変化量閾値を超えた場合に、前記慣性方位角変化量算出部、及び前記衛星測位方位角変化量算出部の各々で算出された前記車体のヨー方向の方位角変化量に基づいて前記慣性計測装置のスケールファクタを推定する請求項２～５のいずれか１項に記載のスケールファクタ推定装置。
7. 車体に搭載された慣性計測装置の出力に基づいて、前記車体のヨー方向の方位角変化量を算出する慣性方位角変化量算出工程と、
   前記車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの間の差分情報から前記車体のヨー方向の方位角変化量を算出する衛星測位方位角変化量算出工程と、
   前記慣性方位角変化量算出工程で算出した方位角変化量に対し、前記衛星測位方位角変化量算出工程で算出した方位角変化量を参照することにより前記慣性計測装置のスケールファクタを推定すると共に、推定したスケールファクタに基づいて前記慣性計測装置の出力を補正するスケールファクタ推定工程と、
   を含むスケールファクタ推定方法。
8. コンピュータを、
   車体に搭載された慣性計測装置の出力に基づいて、前記車体のヨー方向の方位角変化量を算出する慣性方位角変化量算出部、前記車体に所定以上の距離を隔てて設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナの間の差分情報から前記車体のヨー方向の方位角変化量を算出する衛星測位方位角変化量算出部、及び前記慣性方位角変化量算出部で算出した方位角変化量に対し、前記衛星測位方位角変化量算出部で算出した方位角変化量を参照することにより前記慣性計測装置のスケールファクタを推定すると共に、推定したスケールファクタに基づいて前記慣性計測装置の出力を補正するスケールファクタ推定部として機能させるスケールファクタ推定プログラム。