JP2014077769A - センサ傾斜判定装置及びプログラム - Google Patents

Abstract

【課題】ヨーレイトセンサの軸が傾斜しているか否かを精度よく判定することができるようにする。
【解決手段】ドップラー信頼度判定部７６によって、ＨＤＯＰを算出する。方位角変化推定部２４によって、ＧＰＳドップラーを用いて自車両のＧＰＳ推定方位角の変化を推定する。角度変化取得部２６によって、ジャイロセンサ１４によって検出されたヨーレイトに基づいて、ジャイロの角度変化を算出する。ジャイロ軸傾斜判定部７８によって、算出されたＨＤＯＰが閾値未満であり、ＧＰＳの信頼度が高いと判定される場合であって、ＧＰＳ推定方位角の変化とジャイロの角度変化とに基づく最適推定における残差の分散が閾値以上であり、かつ、ヨーレイトが発生していると判定される場合に、ジャイロセンサ１６の軸が、水平面に対する垂直方向に対して傾斜していると判定する。
【選択図】図１

Description

本発明は、センサ傾斜判定装置及びプログラムに係り、特に、移動体に搭載されたヨーレイトセンサの軸が水平面に対する垂直方向に対して傾斜しているか判定するセンサ傾斜判定装置及びプログラムに関する。

従来、センサの取り付け誤差、車両ピッチ角、及びジャイロの感度誤差を、ＧＰＳを用いて補正する角速度補正装置が知られている（特許文献１）。この角速度補正装置では、ＧＰＳを用いて出力する方位角を、測位位置の時間差分に基づいて推定している。また、角速度補正装置は、ＧＰＳから得られる傾斜角および方位角と合うように、センサを補正している。

特開２００９−１３９２２７号公報

しかしながら、上記の特許文献１に記載の技術では、ＧＰＳの測位結果の差分から方位を推定しているため、時間遅れが生じるとともに、ＧＰＳの測位精度そのものが数ｍ程度であることから、方位の推定精度は高くない、という問題がある。また、傾斜角を推定する場合においても、高さ方向の精度は平面内の位置精度に比較して著しく劣化するため、傾斜角を精度よく推定することができない、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、ヨーレイトセンサの軸が傾斜しているか否かを精度よく判定することができるセンサ傾斜判定装置及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明のセンサ傾斜判定装置は、移動体に搭載され、かつ、前記移動体に発生するヨーレイトを検出するヨーレイトセンサと、複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段と、前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体の基準方向の角度の変化を算出する第１算出手段と、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の基準方向の方位角の変化を算出する第２算出手段と、前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とが対応しているか否かを判定する角度変化判定手段と、前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体にヨーレイトが発生しているか否かを判定するヨーレイト判定手段と、前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定する傾斜判定手段と、を含んで構成されている。

また、本発明のプログラムは、コンピュータを、複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段、前記移動体に搭載され、かつ、前記移動体に発生するヨーレイトを検出するヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体のヨーレイトセンサの軸まわりの角度の変化を算出する第１算出手段、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の基準方向の方位角の変化を算出する第２算出手段、前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とが対応しているか否かを判定する角度変化判定手段と、前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体にヨーレイトが発生しているか否かを判定するヨーレイト判定手段、及び前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定する傾斜判定手段として機能させるためのプログラムである。

本発明によれば、ヨーレイトセンサによって、移動体に搭載され、かつ、前記移動体に発生するヨーレイトを検出する。衛星情報取得手段によって、複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する。

そして、第１算出手段によって、前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体のヨーレイトセンサの軸まわりの角度の変化を算出する。第２算出手段によって、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の基準方向の方位角の変化を算出する。

そして、角度変化判定手段によって、前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とが対応しているか否かを判定する。ヨーレイト判定手段によって、前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体にヨーレイトが発生しているか否かを判定する。傾斜判定手段によって、前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定する。

このように、ヨーレイトセンサから算出される角度の変化と、衛星情報を用いて算出された方位角の変化とが対応していないと判定され、かつ、ヨーレイトが発生している場合に、ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定すると判定することにより、ヨーレイトセンサの軸が傾斜しているか否かを精度よく判定することができる。

本発明に係るセンサ傾斜判定装置は、前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とに基づいて、前記ヨーレイトセンサによって検出されたヨーレイトのバイアス補正を行うバイアス補正手段と、前記バイアス補正手段によってバイアス補正されたヨーレイトと、前記第２算出手段によって算出された前記方位角の変化の変化量とに基づいて、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜している傾斜角度を推定する傾斜角度推定手段と、を更に含むようにすることができる。これによって、ヨーレイトセンサの軸が傾斜している傾斜角度を精度良く推定することができる。

また、上記のセンサ傾斜判定装置は、傾斜角度推定手段によって推定された傾斜角度の変化に基づいて、前記ヨーレイトセンサの軸の傾斜が、取り付け誤差による傾斜であるか、あるいは前記移動体のロール方向の運動による傾斜であるかを判定するロール判定手段を更に含むようにすることができる。

本発明に係るセンサ傾斜判定装置は、前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とに基づいて、前記ヨーレイトセンサによって検出されたヨーレイトのバイアス補正を行うバイアス補正手段と、前記第１算出手段によって算出された前記角度の変化に基づくヨーレイト、及び前記バイアス補正手段によってバイアス補正されたヨーレイトの比と、前記移動体がロール方向に運動していないときの予め求められた前記ヨーレイトの比とに基づいて、前記ヨーレイトセンサの軸の傾斜が、取り付け誤差による傾斜であるか、あるいは前記移動体のロール方向の運動による傾斜であるかを判定するロール判定手段を更に含むようにすることができる。

本発明に係るセンサ傾斜判定装置は、前記複数のＧＰＳ衛星の各々の衛星情報から得られるＧＰＳの信頼度を示す指標を算出する信頼度算出手段を更に含み、前記傾斜判定手段は、前記信頼度算出手段によって算出された前記指標に基づいて前記ＧＰＳの信頼度が高いと判定される場合であって、前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定するようにすることができる。

本発明に係るセンサ傾斜判定装置は、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の高さ方向の変化を算出する高さ変化算出手段と、前記複数のＧＰＳ衛星の各々の衛星情報から得られる、水平面方向のＧＰＳの信頼度を示す第１指標及び高さ方向のＧＰＳの信頼度を示す第２指標を各々算出する信頼度算出手段とを更に含み、前記傾斜判定手段は、前記信頼度算出手段によって算出された前記第１指標に基づいて前記水平面方向のＧＰＳの信頼度が高いと判定される場合であって、前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面の垂直方向に対して傾斜していると判定し、前記信頼度算出手段によって算出された前記第２指標に基づいて前記高さ方向のＧＰＳの信頼度が高いと判定される場合に、前記高さ変化算出手段によって算出された前記移動体の高さ方向の変化に基づいて、前記ヨーレイトセンサの軸の傾斜が、取り付け誤差による傾斜であるか、あるいは前記移動体のロール方向の運動による傾斜であるかを判定するロール判定手段を更に含むようにすることができる。

上記のＧＰＳの信頼度を示す指標を、ＧＰＳ受信機が同一時刻に受信した複数のＧＰＳ衛星の各々の衛星情報から得られ、かつ該複数の衛星情報各々を送信した複数のＧＰＳ衛星の配置に応じた測位精度の低下率を表すＤＯＰとすることができる。

上記のセンサ傾斜判定装置は、前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の進行方向を含む速度ベクトル、又は前記移動体の速度及び前記移動体の進行方向を示す速度情報を算出する速度算出手段を更に含み、前記第２算出手段は、前記速度算出手段によって算出された前記速度情報に基づいて、前記移動体の基準方向の方位角を推定することにより、前記移動体の基準方向の方位角の変化を算出するようにすることができる。

上記の速度算出手段は、前記ＧＰＳ衛星の各々の位置に関する情報、及び前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記ＧＰＳ衛星の各々の方向を算出し、時系列の前記ＧＰＳ衛星の各々の位置に関する情報に基づいて、前記ＧＰＳ衛星の各々の速度を算出し、前記移動体から見た前記ＧＰＳ衛星の各々の方向、前記ＧＰＳ衛星の各々の速度、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体のＧＰＳ衛星の各々の方向の速度を算出し、複数の前記移動体のＧＰＳ衛星の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出するようにすることができる。

上記のＧＰＳ衛星の各々の位置に関する情報を、衛星軌道情報とし、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報を、擬似距離情報とし、前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を、ドップラー周波数情報とすることができる。

なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。

以上説明したように、本発明のセンサ傾斜判定装置及びプログラムによれば、ヨーレイトセンサから算出される角度の変化と、衛星情報を用いて算出された方位角の変化とが対応していないと判定され、かつ、ヨーレイトが発生している場合に、ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定すると判定することにより、ヨーレイトセンサの軸が傾斜しているか否かを精度よく判定することができる、という効果が得られる。

第１の実施の形態に係るセンサ傾斜判定装置を示すブロック図である。 第１の実施の形態に係るセンサ傾斜判定装置の角度変化推定部を示すブロック図である。 各ＧＰＳ衛星の速度ベクトル及び各ＧＰＳ衛星の方向に基づいて、ＧＰＳ衛星方向の自車両の速度を算出する様子を示すイメージ図である。 ヨーレイトセンサの出力に基づくジャイロ推定角度の変化と、ＧＰＳドップラーに基づくＧＰＳ推定方位角の変化とを示すグラフである。 車両の旋回中心に対し、ジャイロセンサの軸ズレや設置位置ズレがない場合における左旋回時及び右旋回時のジャイロセンサの出力及び軌跡を示す図である。 車両の旋回中心に対し、設置位置ズレがない場合であって、ジャイロセンサの軸ズレがある場合における左旋回時及び右旋回時のジャイロセンサの出力及び軌跡を示す図である。 車両の旋回中心に対し、ジャイロセンサの軸ズレ及び設置位置ズレがある場合における左旋回時及び右旋回時のジャイロセンサの出力及び軌跡を示す図である。 （Ａ）ＧＰＳドップラーに基づくＧＰＳ推定方位角の変化と、バイアス補正を行ったジャイロ推定角度の変化とを示すグラフ、（Ｂ）ＧＰＳドップラーに基づくＧＰＳ推定方位角の変化量と、バイアス補正を行ったヨーレイトとを示すグラフ、及び（Ｃ）自車両の高さ方向速度の変化を示すグラフである。 ジャイロセンサの軸ズレを判定するフローを示す図である。 第１の実施の形態に係るセンサ傾斜判定装置のコンピュータにおけるジャイロセンサ軸角度推定処理ルーチンの内容を示すフローチャートである。 第１の実施の形態に係るセンサ傾斜判定装置のコンピュータにおける方位角変化推定処理ルーチンの内容を示すフローチャートである。 第２の実施の形態に係るセンサ傾斜判定装置を示すブロック図である。 ジャイロセンサの軸ずれがロール運動によるものか取り付け誤差によるものかを判定する方法を説明するための図である。 第３の実施の形態に係るセンサ傾斜判定装置を示すブロック図である。 通信を用いて先行車両に追従する協調型ＡＣＣシステムを示す図である。 第４の実施の形態に係る協調型ＡＣＣシステムを示すブロック図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、ＧＰＳ衛星から発信されたＧＰＳ情報を取得してジャイロセンサの傾斜角を推定するセンサ軸角度推定装置に、本発明を適用した場合を例に説明する。

図１に示すように、第１の実施の形態に係るセンサ軸角度推定装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信部１２と、自車両のヨーレイトを検出するジャイロセンサ１４と、自車両の速度を検出する速度センサ１６と、ＧＰＳ受信部１２によって受信されたＧＰＳ衛星からの受信信号、ジャイロセンサ１４の検出値、及び速度センサ１６の検出値に基づいて、ジャイロセンサ１４の軸角度を推定すると共に、自車両の走行軌跡を推定する処理を実行するコンピュータ１８とを備えている。ジャイロセンサ１４は、ヨーレイトセンサの一例である。

ＧＰＳ受信部１２は、複数のＧＰＳ衛星からの電波を受信して、受信した全てのＧＰＳ衛星からの受信信号から、ＧＰＳ衛星の情報として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を送信した時刻、受信信号の強度、周波数などを取得し、コンピュータ１８に出力する。

コンピュータ１８は、ＣＰＵ、後述するジャイロセンサ軸角度推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置で構成されている。

コンピュータ１８を機能ブロックで表すと、図１に示すように、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得部２０と、ジャイロセンサ１４の検出値及び速度センサ１６の検出値を記憶するデータ記憶部２２と、取得したＧＰＳ衛星の情報に基づいて、自車両の方位角の変化を推定する方位角変化推定部２４と、記憶されたジャイロセンサ１４の検出値に基づいて、自車両の角度の変化を推定する角度変化取得部２６と、方位角変化推定部２４及び角度変化取得部２６によって得られた方位角の変化及び角度の変化に基づいて、ジャイロセンサ１４の軸角度を推定する姿勢推定部２８と、所定時間分の自車両の軌跡を推定する軌跡推定部３０と、を含んだ構成で表すことができる。方位角変化推定部２４は、第１算出手段の一例であり、角度変化取得部２６は、第２算出手段の一例である。

ＧＰＳ情報取得部２０は、ＧＰＳ受信部１２から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び自車両で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。また、ＧＰＳ情報取得部２０は、各ＧＰＳ衛星から送信される信号の既知の周波数と、各ＧＰＳ衛星から受信した受信信号の周波数とに基づいて、各ＧＰＳ衛星からの受信信号のドップラー周波数を各々算出する。なお、ドップラー周波数は、ＧＰＳ衛星と自車との相対速度による、搬送波周波数のドップラーシフト量を観測したものである。また、ＧＰＳ情報取得部２０は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。

方位角変化推定部２４は、図２に示すように、取得したＧＰＳ情報に基づいて、ＧＰＳ衛星の位置及び自車両の位置を算出する位置算出部５８と、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出部６０と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出部６２と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出部６６と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出部６８と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出部７０と、算出された自車両の速度ベクトルに基づいて自車両の方位角を算出して方位角の変化（時系列）を算出する自車両方位角算出部７２と、を含んだ構成で表すことができる。

位置算出部５８は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。

また、位置算出部５８は、以下のように、ＧＰＳ情報取得部２０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。

ＧＰＳを用いた測位では、既知であるＧＰＳ衛星の位置座標と、各ＧＰＳ衛星から受信した受信信号の伝播距離である擬似距離とに基づいて、三角測量の原理に従って、自車両の位置が推定される。

ここで、ＧＰＳ衛星までの真の距離ｒ_ｊは、以下の（１）式で表され、ＧＰＳで観測される擬似距離ρ_ｊは、以下の（２）式で表される。

ただし、（Ｘ_ｊ，Ｙ_ｊ，Ｚ_ｊ）がＧＰＳ衛星ｊの位置座標であり、（ｘ，ｙ，ｚ）が自車両の位置座標である。ｓは、ＧＰＳ受信部１２の時計誤差による距離誤差である。

上記（１）式、（２）式より、４つ以上のＧＰＳ衛星のＧＰＳ擬似距離データから得られる以下の（３）式の連立方程式を解くことによって、自車両の位置（ｘ、ｙ、ｚ）が算出される。

相対速度算出部６０は、ドップラー周波数とＧＰＳ衛星に対する相対速度との関係を表わす以下の（４）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度を算出する。

ただし、ｖ_ｊはＧＰＳ衛星ｊに対する相対速度であり、Ｄ１_１ｊはＧＰＳ衛星ｊから得られるドップラー周波数（ドップラーシフト量）である。また、Ｃは光速であり、Ｆ_１は、ＧＰＳ衛星から送信される信号の既知のＬ１周波数である。

衛星速度算出部６２は、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（３次元速度ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）を算出する。例えば、非特許文献（Ｐｒａｔａｐ Ｍｉｓｒａ ａｎｄ Ｐｅｒ Ｅｎｇｅ原著 日本航海学会ＧＰＳ研究会訳：“精説ＧＰＳ基本概念・測位原理・信号と受信機”正陽文庫，２００４．）に記載された方法を用いて、各ＧＰＳ衛星の速度ベクトルを算出することができる。

衛星方向算出部６６は、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係（水平方向に対する仰角θ_ｊ、北方向に対する方位角φ_ｊ）を、各ＧＰＳ衛星の方向として算出する。

衛星方向自車速算出部６８は、図３に示すように、算出された各ＧＰＳ衛星に対する自車両の相対速度ｖ_ｊ、各ＧＰＳ衛星の速度ベクトル（ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）、及び各ＧＰＳ衛星の方向Ｒ_ｊ（θ_ｊ、φ_ｊ）に基づいて、以下の（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖ_ｊを算出する。

ｖ_ｊは、ＧＰＳ衛星ｊに対する自車両の相対速度（衛星方向におけるＧＰＳ衛星との相対速度）である。また、Ｖｓ_ｊは、自車方向のＧＰＳ衛星ｊの速度であり、Ｖｓ_ｊ＝Ｒ_ｊ［ＶＸ_ｊ，ＶＹ_ｊ，ＶＺ_ｊ］^Ｔにより求まる。また、Ｖｖ_ｊは、ＧＰＳ衛星ｊの方向の自車速であり、ｖＣｂは、クロックバイアス変動である。

上述したように、ＧＰＳ衛星方向の自車速は、ＧＰＳ衛星位置の三次元位置ではなく、ＧＰＳ衛星との方位関係によってのみ算出される。ＧＰＳ衛星は遥か遠方にあり、１日で地球をほぼ２周するため１分間の角度変化は０．５度である。通常、ＧＰＳ衛星とＧＰＳ受信機との時計誤差は通常１ｍｓｅｃ以下であるため、ＧＰＳ衛星との方位関係に大きな影響はない。また、同じくＧＰＳ衛星は遥か遠方にあるため、自車の位置決定に数１００ｍ程度の誤差が生じていたとしても、ＧＰＳ衛星との方位関係に大きな影響はない。このため、擬似距離に誤差が乗りやすい状況であったとしても、ＧＰＳ衛星方向の自車速は、比較的正確に算出され得る。

自車両速度算出部７０は、以下に説明するように、自車両の速度ベクトルの最適推定を行う。

まず、自車の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、ＧＰＳ衛星方向の自車両の速度Ｖｖ_ｊとの関係は以下の（６）式で表される。

各ＧＰＳ衛星ｊについて得られる上記（６）式より、Ｖｘ，Ｖｙ，Ｖｚ及びＣｂを推定値とした、以下の（７）式で表される連立方程式が得られる。

電波を受信したＧＰＳ衛星が４個以上である場合に、上記（７）式の連立方程式を解くことによって、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。

自車両方位角算出部７２は、自車両速度算出部７０により算出された自車両の速度ベクトルから三角関数を用いて自車両の前後方向の方位角を算出し、メモリ（図示省略）に記憶する。自車両方位角算出部７２は、メモリに記憶された自車両の方位角の変化(時系列)を、方位角変化の推定結果として出力する。ここで推定された方位角は、ＧＰＳ情報に基づいて推定された方位角であるので、以下、「ＧＰＳ推定方位角」という。

これに対し、角度変化取得部２６によってジャイロセンサ１４の検出値から算出される自車両の角度を「ジャイロ推定角度」という。ただし、ジャイロセンサ１４では、角度の変化分しか計測できないため、ある初期値を基準に時間経過分を積分することで、ジャイロセンサ１４の検出値からジャイロ推定角度を算出する。

角度変化取得部２６は、自車両の角度の変化(時系列)を出力する。

姿勢推定部２８は、ＧＰＳ推定方位角の変化及びジャイロの角度変化に基づいて、ジャイロセンサ１４のバイアスを推定するジャイロバイアス推定部７４と、同一時刻に取得された全てのＧＰＳ情報に基づいて、ドップラーの信頼度としてのＤＯＰ（dilution of precision）値を判定するドップラー信頼度判定部７６と、ジャイロセンサ１４のバイアスの推定結果及びＤＯＰに基づいて、ジャイロセンサ１４の軸が傾斜しているか判定するジャイロ軸傾斜判定部７８と、バイアス補正されたジャイロセンサ１４の検出値に基づいて、ジャイロセンサ１４の軸の傾斜角度を推定するジャイロ軸角度推定部８０と、推定された傾斜角度に基づいて、ジャイロセンサ１４の軸の傾斜が取り付け誤差によるものか、バンクを含むロール運動によるものかを判定するロール判定部８２とを備えている。なお、ジャイロ軸傾斜判定部７８が、方位角変化判定手段、ヨーレイト判定手段、及び傾斜判定手段の一例である。ジャイロバイアス推定部７４が、バイアス補正手段の一例である。ジャイロ軸角度推定部８０が、傾斜角度推定手段の一例である。

ジャイロバイアス推定部７４は、図４に示すように、ＧＰＳ推定方位角の変化と、ジャイロの角度変化とを比較して、初期角度とバイアスを未知数として、ＧＰＳドップラーの各種推定値とバイアス補正後のジャイロの値との誤差が最も小さくなるように最適推定を行うことにより、初期角度と、ジャイロセンサ１４の検出値のバイアスを推定する。具体的には、以下の（８）式に示す関数ｆを最小化する初期角度θ_initialGyroとバイアスＧ_yroBiasを求める。

ただし、ω_Gyroはジャイロのヨーレート計測値であり、θ_GPSは、ＧＰＳ推定方位角である。

最適推定を行う際に、ＧＰＳドップラーの残差が一定以上である場合はＧＰＳドップラーの精度が劣化したとして、推定から除去することで、精度を上げることができる。また、ジャイロバイアス推定部７４は、バイアス補正を行ったジャイロセンサ１４の検出値を求める。ジャイロバイアス推定部７４は、バイアス補正を行ったジャイロセンサ１４の検出値に基づくジャイロの角度変化と、ドップラーを用いたＧＰＳ推定方位角の変化との各時刻の残差を算出し、残差のばらつき（分散）を算出する。

ドップラー信頼度判定部７６は、同一時刻に取得された全てのＧＰＳ情報（以下、「ＧＰＳ情報群」ともいう）を用いて、そのＧＰＳ情報群のＤＯＰを算出する。ＤＯＰは、複数のＧＰＳ情報各々を送信した複数のＧＰＳ衛星の幾何学的配置に応じた測位精度の低下率を表す指標であり、ＤＯＰの算出方法については、例えば、「改定第二版 精鋭ＧＰＳ基本概念・測位原理・信号と受信機 Partap Misra and Per Enge著」等に記載された周知の算出方法を用いることができる。

ＧＰＳ衛星が散らばって配置されている場合には、ＤＯＰは良好に（小さく）なる。ＧＰＳ測位は、三角測量の原理に基づいて測位を行うため、ＧＰＳ衛星の散らばり具合が大きいほど、幾何学的配置に起因する誤差は小さくなる。一方、ＤＯＰが悪い（大きい）例では、天頂付近にＧＰＳ衛星が固まって配置されていたり、ある方向にだけＧＰＳ衛星が固まって配置されていたりする場合がある。このような状況では、ＧＰＳ衛星の散らばりが小さいため、ＤＯＰが大きくなり、幾何学的配置に起因する誤差も大きくなる。

ここで、ジャイロセンサ１４が検出するヨーレイトの回転軸が傾斜しているか判定する原理について説明する。

上述したジャイロバイアス推定部７４による、ＧＰＳドップラーを用いてバイアス補正をする手法は、ジャイロセンサ１４の軸が、移動体の回転平面の軸（水平面に対する垂直方向の軸）と一致している場合に成立するものであり、回転平面の軸とジャイロセンサ１４の軸がずれている場合は成立しない。

たとえば、図５に示すように、車両の旋回中心に対し、ジャイロセンサの軸ズレや設置位置ズレがない場合、ジャイロセンサのバイアスや車速が十分に正確に推定されている状況では、車両の本来の軌跡とジャイロセンサが出力する軌跡はほぼ同一の形状を描く。

但し、図６に示すように、設置位置がオフセットしているような場合、ヨーレイトセンサからは正しい値と同じ値が出力されるが、ジャイロセンサの設置位置における速度が旋回中心の速度と異なるため、推定される軌跡形状には差が生じる。この場合、ジャイロセンサのオフセット量は予め計測できる場合が多く、ヨーレイトそのものには影響を与えない。

一方、図７に示すように、ジャイロセンサの軸が、実際の車両の旋回中心の軸（水平面に対する垂直方向の軸）に対して角度があるような場合、実際のヨーレイトの絶対値に対して小さい値が出力される。設置時の角度だけではなく、バンクなどのように車体そのものが傾斜したような場合も同様の誤差が生じる。その場合、ヨーレイトセンサの出力結果をそのまま用いることには問題がある。本実施の形態では、ジャイロセンサの姿勢を推定することで、車両運動計測精度を上げることを目的としている。

常時、ＧＰＳドップラーの計測値が信用できるならばヨーレイトの代わりにＧＰＳドップラーからの推定値を用いてもいいが、ＧＰＳは衛星数が少なくなると推定精度は劣化する傾向がある。都心部などでは推定できない場合もあり、ロバスト性が低い。このため、方位角の推定はＧＰＳドップラーが受信できるところで行い、軌跡はジャイロセンサの検出値と車速から推定する手法が知られている。

実際のジャイロセンサのバイアス補正は、ジャイロバイアス推定部７４によって、上記図４のような最適推定の結果を用いて実施されている。この際、ジャイロセンサに基づく角度変化とＧＰＳドップラーに基づく方位角変化は同一であると仮定しているため、上記図７に示すようにジャイロセンサに軸ズレがある場合は正しい推定ができなくなる可能性がある。軸ズレがあるとそもそものヨーレイトセンサの出力値が間違っているため、バイアス成分を考慮してヨーレイト積算をしたとしても正しい角度変化が得られない。

取り付け誤差やバンクによる軸ズレがある場合、図８（Ａ）のような方位角の変化、及び図８（Ｂ）のようなヨーレイトの変化が予想される。

ヨーレイトが発生する状況において軸ズレがある場合、実際のヨーレイトよりも小さい値が出力されるため、その振幅は抑えられる形になる。ヨーレイトが発生する部分だけが抑えられるため、そのヨーレイトを積算した角度とＧＰＳドップラーから得られる方位角との間には誤差が生じる。通常、軸ズレがある場合は一定のバイアス誤差の推定だけでは、つじつまが合わなくなる状況（方位角の変化が対応していない状況）が生じる。

そこで、本実施の形態では、ジャイロ軸傾斜判定部７８によって、ＧＰＳドップラーの状態を判断した上で、上記のつじつまが合わなくなる状況を判定することで、ジャイロセンサ１４の軸ズレ（ジャイロセンサ１４が検出するヨーレイトの回転軸が、水平面に対する垂直方向に対して傾斜していること）があるか否かを判定する。軸ズレがあると判定された場合、ヨーレイトの比率から軸の傾きを推定することが可能になる。

図９に、ジャイロセンサ１４の軸ズレを判定するフローについて示す。

一般的にＧＰＳの測位精度は衛星の配置と擬似距離の精度に起因する。速度ベクトルの推定精度も同様に衛星の配置とドップラーの精度に起因する。そこで、衛星の配置から算出される幾何学的誤差（ＤＯＰ：Ｄｉｌｕｔｉｏｎ Ｏｆ Ｐｒｅｃｉｓｉｏｎ）が小さい場合はＧＰＳドップラーによるＧＰＳ推定方位角の変化の精度が高いものと考える。

過去Ｔ秒後において速度ベクトルの推定に用いたＧＰＳのＨＤＯＰ（水平面方向のＤＯＰ）が閾値ＴＨ_DOPh未満であり、ＧＰＳの信頼度が高いと判断される場合であって、上記ジャイロバイアス推定部７４で算出された最適推定における残差のばらつき（分散）が閾値ＴＨσ未満の場合は、ドップラーの精度が高く、ジャイロセンサ１４の軸ずれも少ないと判定する。なお、最適推定における残差のばらつき（分散）が閾値ＴＨ_σ未満であるか否かの判定が、ジャイロの角度変化とＧＰＳ推定方位角の変化とが対応しているか否かの判定の一例である。

ＧＰＳのＨＤＯＰ（水平方面のＤＯＰ）が閾値ＴＨ_DOPh未満であり、ＧＰＳの信頼度が高いと判断される場合であって、最適推定における残差のばらつき（分散）が閾値ＴＨ_σ以上である場合は、その時にヨーレイトが発生している状況（車両の旋回が考えられる場合：Ｎ回のヨーレイトの平均が閾値ＴＨ_ωより大きい）の場合（図８（Ｂ）参照）、軸ズレによって上記残差のばらつきが大きくなっている可能性があると考えて、ジャイロセンサ１４の軸ずれの可能性があると判定する。

最適推定における残差のばらつき（分散）が閾値ＴＨ_σ以上である場合であったとしても、ヨーレイトが発生していない場合（Ｎ回のヨーレイトの平均が閾値ＴＨ_ω以下の場合）、誤差要因は、ジャイロセンサ１４の軸ずれ以外であると判断し、ジャイロセンサ１４の軸の傾斜角の推定を行わない。

過去Ｔ秒分において、ＧＰＳのＨＤＯＰの平均が閾値ＴＨ_DOPh以上の場合、ＧＰＳの信頼度が高くなく、ドップラーによる速度ベクトルの推定精度もあまり高くないと判断する。この時、傾斜角の判定情報としての確度が足りないため、後述するジャイロセンサ１４の軸の傾斜角の推定を行わない。

ジャイロ軸角度推定部８０は、ジャイロセンサ１４の軸ずれがあると判定された場合において、バイアス補正したジャイロセンサ１４の検出値（ヨーレイト）ΔＨ_ｇｙｒｏとＧＰＳから算出したＧＰＳ推定方位角の変化量（ヨーレイト）ΔＨ_ｇｐｓとを比較する。ジャイロセンサ１４の軸が旋回中心（水平面に対する垂直方向）に対してθ傾いているとすると、ヨーレイトは本来のｃｏｓθ倍の値となる。すると、ΔＨ_ｇｙｒｏ＝ΔＨ_ｇｐｓ×ｃｏｓθの関係式が得られることから、当該関係式に従って、ヨーレイトの比をジャイロセンサ１４の軸の傾斜角θとして推定すると共に、推定したジャイロセンサ１４の軸の傾斜角θをメモリ（図示省略）に記憶しておく。

ロール判定部８２は、ジャイロセンサ１４の軸ずれがあると判定された場合、ジャイロ軸角度推定部８０によって推定されたジャイロセンサ１４の軸の傾斜角θの変化に基づいて、ジャイロセンサ１４の軸ずれが、取り付け誤差によるものか、自車両のロール運動（例えば、バンク走行）によるものかを判定する。例えば、推定されたジャイロセンサ１４の軸の傾斜角θがほぼ一定である場合（ジャイロセンサ１４の軸の傾斜角θの平均値を含む所定範囲内に収まっている場合）には、ジャイロセンサ１４の軸ずれが、取り付け誤差によるものと判定し、推定されたジャイロセンサ１４の軸の傾斜角θが変動している場合（ジャイロセンサ１４の軸の傾斜角θの平均値を含む所定範囲内に収まっていない場合）には、ジャイロセンサ１４の軸ずれが、自車両のロール運動（例えば、バンク走行）によるものと判定する。

また、軌跡推定部３０は、ドップラー信頼度判定部７６によって算出されたＨＤＯＰが、閾値未満である場合、ＧＰＳの信頼度が高いと判断し、ジャイロセンサ１４の検出値を用いることなく、取得した各ＧＰＳ衛星のドップラー周波数を用いた、自車両軌跡を推定する。この場合には、自車両速度算出部７０により算出された自車両の速度ベクトルを所定時間分積算することにより自車両の軌跡を算出する。所定時間は、所望の精度が得られる適切な時間を定めておく。擬似距離に比較して正確に算出されるＧＰＳ衛星方向の自車速を用いて自車両の速度ベクトルを算出し、これを用いて自車両の軌跡を算出することで、精度の高い形状及び方位の自車両の軌跡を算出することができる。

一方、軌跡推定部３０は、ドップラー信頼度判定部７６によって算出されたＨＤＯＰが、閾値以上である場合、ＧＰＳの信頼度が低いと判断し、ジャイロバイアス推定部７４によりバイアス補正されたジャイロセンサ１４の検出値と、データ記憶部２２に記憶された速度センサ１６の検出値とを用いて、自車両軌跡を推定する。なお、自車両軌跡の推定方法については、従来既知の手法を用いればよく、詳細な説明を省略する。

次に、第１の実施の形態に係るセンサ軸角度推定装置１０の作用について説明する。

ジャイロセンサ１４及び速度センサ１６によってヨーレイト及び速度を検出すると共に、ＧＰＳ受信部１２によって、複数のＧＰＳ衛星から電波を受信しているときに、コンピュータ１８において、図１０に示すジャイロセンサ軸角度推定処理ルーチンが繰り返し実行される。

ステップ１００で、ＧＰＳ受信部１２から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。同一時刻に取得された複数のＧＰＳ衛星分のＧＰＳ情報を、ＧＰＳ情報群として取得する。

次に、ステップ１０２で、ジャイロセンサ１４及び速度センサ１６から各検出値を取得し、ヨーレイト及び速度をデータ記憶部２２に記憶する。

次に、ステップ１０４で、後述するＧＰＳ情報に基づく方位角変化推定処理を実行して、所定時間分のＧＰＳ推定方位角の変化を算出し、次に、ステップ１０６で、データ記憶部２２に記憶された所定時間分のヨーレイトに基づいて、所定時間分のジャイロの角度変化を算出する。

そして、ステップ１０８において、上記ステップ１０４で算出したＧＰＳ推定方位角の変化と、上記ステップ１０６で算出したジャイロの角度変化とに基づいて、初期方位角及びジャイロバイアスの最適推定を行う。ステップ１１０では、上記ステップ１０８で推定されたジャイロバイアスを用いて、上記ステップ１０２で得られたジャイロセンサ１４の検出値を補して、メモリ（図示省略）に記憶する。

次のステップ１１２では、上記ステップ１０８における最適推定の結果に基づいて、残差の分散を算出してメモリに記憶する。ステップ１１４では、上記ステップ１００で取得したＧＰＳ情報群のＨＤＯＰを算出してメモリに記憶する

そして、ステップ１１６において、上記ステップ１１２で算出された過去Ｔ秒間分の残差の分散の平均値と、上記ステップ１１４で算出された過去Ｔ秒間分のＨＤＯＰの平均値と、上記ステップ１０２で取得した過去Ｔ秒間分のヨーレイトの平均とに基づいて、上記図９の判定フローに従って、ジャイロセンサ１４の軸ずれの有無を判定する。

ステップ１１８では、上記ステップ１１６の判定結果に基づいて、ジャイロセンサ１４の軸ずれがあるか否かを判定し、軸ずれがない場合には、上記ステップ１００へ戻る。一方、軸ずれがある場合には、ステップ１２０において、上記ステップ１１０で補正されたジャイロセンサ１４の検出値（ヨーレイト）ΔＨ_ｇｙｒｏと、上記ステップ１０４で推定されたＧＰＳ推定方位角の変化量（ヨーレイト）ΔＨ_ｇｐｓとに基づいて、ジャイロセンサ１４の軸の傾斜角θを算出し、メモリに記憶する。

そして、ステップ１２２において、上記ステップ１２０でメモリに記憶されたジャイロセンサ１４の軸の傾斜角θの変化に基づいて、ジャイロセンサ１４の軸ずれが、取り付け誤差によるものか、自車両のロール運動（例えば、バンク走行）によるものかを判定して、判定結果を出力し、上記ステップ１００へ戻る。

次に、図１１を参照して、方位角変化推定処理ルーチンについて説明する。

ステップ１３０で、上記（１）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度ｖ_ｊを算出する。

次に、ステップ１３２で、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）を算出する。

次に、ステップ１３４で、各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、上記（２）〜（４）式に従って、自車両の位置を算出する。なお、ここでは、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しており、自車両の位置として大まかな位置が決定できればよく、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。

次に、ステップ１３６で、上記ステップ１３４で算出された自車両の位置及び取得された各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係Ｒ_ｊ（水平方向に対する仰角θ_ｊ、北方向に対する方位角φ_ｊ）を、各ＧＰＳ衛星の方向として算出する。

次に、ステップ１３８で、上記ステップ１３０で算出された各ＧＰＳ衛星に対する自車両の相対速度ｖ_ｊ、上記ステップ１３２で算出された各ＧＰＳ衛星の速度ベクトルＶ_ｊ（ＶＸ_ｊ、ＶＹ_ｊ、ＶＺ_ｊ）、及び上記ステップ１３６で算出された各ＧＰＳ衛星の方向Ｒ_ｊ（θ_ｊ、φ_ｊ）に基づいて、上記（５）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖ_ｊを算出する。（５）式における自車方向のＧＰＳ衛星ｊの速度Ｖｓ_ｊは、Ｖｓ_ｊ＝Ｒ_ｊ［ＶＸ_ｊ，ＶＹ_ｊ，ＶＺ_ｊ］^Ｔにより算出する。

次に、ステップ１４０で、上記（６）式、及び（７）式に従って、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。

次に、ステップ１４２で、上記ステップ１２０で算出された自車両の速度ベクトルから、自車両のＧＰＳ推定方位角を算出してメモリに記憶する。そして、ステップ１４４において、メモリに記憶された自車両のＧＰＳ推定方位角に基づいて、ＧＰＳ推定方位角の変化を算出して、リターンする。

以上説明したように、第１の実施の形態に係るセンサ軸傾斜判定装置によれば、ＨＤＯＰが閾値未満であって、ジャイロセンサから算出されるジャイロの角度変化と、ＧＰＳ情報を用いて算出されたＧＰＳ推定方位角の変化とのつじつまが合わないと判断され、かつ、ヨーレイトが発生している場合に、ジャイロセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定すると判定することにより、ジャイロセンサの軸ズレを精度よく判定することができる。

ジャイロセンサの軸が本来の回転平面内に対してズレている場合、実際よりも小さい値が出力されるため、旋回の軌跡が膨らむ傾向がある。設置角度だけでなく、バンクなどにより車体そのものが傾斜する場合も同様である。本実施の形態では、ナビや車両に搭載されるジャイロセンサの軸ズレを簡易な構成で判定することが可能であり、ジャイロセンサの軸の傾斜角度を推定することにより、その軸ズレによるセンサ誤差の修正が可能であり、方位角変化の推定精度が向上する。また、軌跡の推定精度も向上する。

また、誤差が大きいＧＰＳの位置ではなく、精度の高いＧＰＳドップラーを用いることで高精度な補正を可能にする。ドップラーを用いることで、その時点における地球座標系での速度を推定することができるため、自車両の方位角を１エポックで算出可能であり、より高精度な補正が可能になる。

ジャイロセンサの軸が旋回中心の軸に対して傾いている場合、実際のヨーレイトに比較して小さい値が出力される。既存のGPS・INS統合による軌跡の推定では、GPSの測位結果を用いてジャイロセンサの補正をしているため、時間遅れが大きく、瞬時の姿勢判定が困難であった。本実施の形態では、ＧＰＳドップラーを用いることで、各エポックごとの方位角の推定が可能であるため、ＧＰＳ推定方位角とヨーレイトに基づくジャイロ推定角度とを直接比較して、その出力結果の差を観測することで、ジャイロセンサの軸ずれを判定することが可能になる。GPSとジャイロセンサの出力値の差から、ジャイロセンサの軸の傾斜角度についても算出可能であり、走行中に、ジャイロセンサの軸の傾斜角度の補正を行うことが可能になる。

また、取り付け誤差があったとしても、ジャイロセンサの軸の傾斜角度の推定が可能であることから、ジャイロセンサの取り付け時のコストが低減する。ジャイロセンサの軸ズレによる誤差の修正が可能であるため、軌跡の推定精度が向上する。

次に、第２の実施の形態について説明する。なお、第２の実施の形態のセンサ軸角度推定装置について、第１の実施の形態のセンサ軸角度推定装置１０と同一の構成については、同一の符号を付して説明を省略する。

第２の実施の形態では、ＧＰＳ推定方位角の変化量とジャイロセンサの検出値との比を用いて、ジャイロセンサ１４の軸ずれが、取り付け誤差によるものか、あるいはロール運動によるものかを判定する点が、第１の実施の形態と主に異なっている。

図１２に示すように、第２の実施の形態に係るセンサ軸角度推定装置２１０のコンピュータ２１８では、姿勢推定部２２８が、ジャイロバイアス推定部７４、ドップラー信頼度判定部７６、ジャイロ軸傾斜判定部７８、ジャイロ軸角度推定部８０、及びロール判定部２８２を備えて構成されている。

ここで、ジャイロセンサ１４の軸ずれがロール運動によるものか取り付け誤差によるものかを判定する原理について説明する。

取り付け誤差によるジャイロセンサ１４の軸ズレは常時ずれているため、上記傾斜角θは一定であるが、バンクなどの走行環境によって生じるジャイロセンサ１４の軸ズレは、状況ごとにθが変化することから、ヨーレイトの変動を学習することで、ジャイロセンサ１４の軸ズレが、取り付け誤差によるものか、バンク走行などの車両のロール運動によるものかを判定可能である。

そこで、本実施の形態では、ロール判定部２８２は、ジャイロ軸傾斜判定部７８によってジャイロセンサ１４の軸ずれがあると判定された場合、方位角変化推定部２４によって推定されたＧＰＳ推定方位角の変化に基づく方位角変化量（ヨーレイト）と、ジャイロバイアス推定部７４によりバイアス補正されたヨーレイトとの比、及び自車両がロール方向に運動していないときの予め求められたヨーレイトの比とに基づいて、ジャイロセンサ１４の軸ズレが、取り付け誤差による傾斜であるか、あるいは自車両のロール運動による傾斜であるかを判定する。

図１３に示すように、まず、方位角変化推定部２４によって推定されたＧＰＳ推定方位角の変化に基づく方位角変化量（ヨーレイト）と、ジャイロバイアス推定部７４によって補正されたヨーレイトとの比が一定となっている場合に、ジャイロセンサ１４の傾斜角が一定であると判断し、ＧＰＳ推定方位角度の変化量ω_GPSと、バイアス補正されたジャイロセンサ１４のヨーレイト出力ω_gyroとの比を保存する。

そして、方位角変化推定部２４によって推定されたＧＰＳ推定方位角の変化に基づく方位角変化量（ヨーレイト）と、バイアス補正されたジャイロセンサ１４のヨーレイト出力との比が、保存された比と一致する場合には、ジャイロセンサ１４の軸ズレが、取り付け誤差によるものであると判定する。

一方、方位角変化推定部２４によって推定されたＧＰＳ推定方位角の変化に基づく方位角変化量（ヨーレイト）と、バイアス補正されたジャイロセンサ１４のヨーレイト出力との比が、保存された比と一致しない場合には、ジャイロセンサ１４の軸ズレが、自車両のロール運動によるものであると判定する。

なお、第２の実施の形態に係るセンサ軸角度推定装置２１０の他の構成及び作用については、第１の実施の形態に係るセンサ軸角度推定装置１０と同様であるため、説明を省略する。

以上説明したように、第２の実施の形態に係るセンサ軸角度推定装置によれば、ジャイロセンサの軸ズレを精度よく判定することができると共に、ジャイロセンサの軸が傾斜している傾斜角度を精度良く推定することができる。

次に、第３の実施の形態について説明する。なお、第３の実施の形態のセンサ軸角度推定装置について、第１の実施の形態のセンサ軸角度推定装置１０と同一の構成については、同一の符号を付して説明を省略する。

第３の実施の形態では、ＧＰＳドップラーを用いて求められる高さ方向速度の変化に基づいて、ジャイロセンサ１４の軸ずれが、取り付け誤差によるものか、あるいはロール運動によるものかを判定する点が、第１の実施の形態と主に異なっている。

図１４に示すように、第３の実施の形態に係るセンサ軸角度推定装置３１０のコンピュータ３１８では、姿勢推定部３２８が、ジャイロバイアス推定部７４、ドップラー信頼度判定部７６、ジャイロ軸傾斜判定部７８、ジャイロ軸角度推定部８０、及びロール判定部３８２を備えて構成されている。

ここで、ジャイロセンサ１４の軸ずれがロール運動によるものか取り付け誤差によるものかを判定する原理について説明する。

ＧＰＳアンテナが車軸から離れた位置に設置されている場合、バンクによって車体が傾くと、アンテナ位置の高さに変化が生じる。ＧＰＳドップラーの高さ方向速度（図８（Ｃ）参照）の推定精度は水平面に対してあまり高くないが、その速度積分によって得られる高さ変化を見つけることで、ジャイロセンサ１４の軸ずれが、取り付け誤差によるものではなく、ロール運動によるものである可能性が高いと判断することができる。

そこで、本実施の形態では、ＧＰＳ受信部１２を、自車両の車軸から離れた位置に設置する。また、ドップラー信頼度判定部７６によって、同一時刻に取得されたＧＰＳ情報群を用いて、そのＧＰＳ情報群のＨＤＯＰ（水平面方向のＤＯＰ）及びＶＤＯＰ（高さ方向のＤＯＰ）を算出する。方位角変化推定部２４は、更に、算出される速度ベクトルの高さ成分を積分して、自車両位置の高さ方向の変化を算出する。なお、ＨＤＯＰが、第１指標の一例であり、ＶＤＯＰが第２指標の一例である。

ロール判定部３８２は、図８（Ｃ）に示すように、算出されたＶＤＯＰが閾値ＴＨ_DOPv未満であり高さ方向のＧＰＳドップラーの信頼度が高いと判定される場合に、算出された自車両位置の高さ方向の変化に基づいて、ジャイロセンサ１４の軸ズレが、取り付け誤差による傾斜であるか、あるいは車両のロール運動による傾斜であるかを判定する。例えば、自車両位置の高さ方向が、一時的に変動した場合（自車両位置の高さの平均値を含む所定範囲内に収まっていない場合）には、ジャイロセンサ１４の軸ズレが、車両のロール運動による傾斜であると判定する。一方、自車両位置の高さ方向がほぼ一定である場合（自車両位置の高さの平均値を含む所定範囲内に収まっている場合）には、ジャイロセンサ１４の軸ずれが、取り付け誤差によるものと判定する。

なお、第３の実施の形態に係るセンサ軸角度推定装置３１０の他の構成及び作用については、第１の実施の形態に係るセンサ軸角度推定装置１０と同様であるため、説明を省略する。

以上説明したように、第３の実施の形態に係るセンサ軸角度推定装置によれば、ジャイロセンサの軸ズレを精度よく判定することができると共に、ジャイロセンサの軸が傾斜している傾斜角度を精度良く推定することができる。

次に、第４の実施の形態について説明する。なお、第１の実施の形態のセンサ軸角度推定装置１０と同一の構成については、同一の符号を付して説明を省略する。

第４の実施の形態では、図１５に示すような、通信を用いて先行車両に追従する協調型ＡＣＣ（オートクルーズコントロール）システムに本発明を適用した場合を例に説明する。

図１６に示すように、協調型ＡＣＣシステム４１０は、先行車両側車載器４１０Ａ及び追従車両側車載器４１０Ｂを備えている。

先行車両側車載器４１０は、ＧＰＳ受信部１２と、ジャイロセンサ１４と、速度センサ１６と、コンピュータ１８と、送信部４２０とを備えている。

送信部４２０は、自車両位置、速度軌跡、方位角などの情報と共に、ジャイロセンサ１４の軸の傾斜角度及びジャイロセンサ１４の軸ズレが取り付け誤差によるものか、ロール運動によるものかの判定結果を含む送信情報を、追従車両側車載器４１０Ｂに対して送信する。

追従車両側車載器４１０Ｂは、受信部４２２及びコンピュータ４２４を備えている。

受信部４２２は、先行車両側車載器４１０から送信された送信情報を受信して、コンピュータ４２４へ出力する。

コンピュータ４２４は、先行車両情報判断部４２６及び車両制御部４２８を備えている。先行車両情報判断部４２６は、先行車両側車載器４１０から送信された送信情報に含まれるジャイロセンサ１４の軸の傾斜角度に基づいて、先行車両側車載器４１０からの送信情報に含まれる軌跡や方位角の誤差可能性を判断する。また、先行車両情報判断部４２６は、先行車両側車載器４１０から送信された送信情報に含まれる上記判定結果に基づいて、ロール運動（バンク走行）の可能性を判断する。

車両制御部４２８は、先行車両側車載器４１０からの送信情報に含まれる軌跡や方位角の誤差可能性に基づいて、軌跡や方位角、ヨーレイトを補正し、補正された軌跡や方位角、ヨーレイトを用いて、先行車両に追従するように車両制御を行う。また、車両制御部４２８は、ロール運動（バンク走行）の可能性に基づいて、前方のロール予測を行い、車両制御を行う。

車両制御部４２８では、先行車両から通信によって取得した先行車両の状態（車速、位置、ヨーレイトなど）を用いて、車両の運動を適切に制御することで、エネルギー効率の向上及び運転負荷の低減を実現する。このとき、送信する情報に予め誤差が含まれていることがわかっている場合、追従車両はその情報の信頼度を下げるなどによって制御の方法を変化させることが出来るようになる。

なお、上記の第４の実施の形態では、協調型ＡＣＣシステムに、第１の実施の形態で説明したジャイロセンサの軸の傾斜角の推定方法を適用した場合を例に説明したが、これに限定されるものではない。第２の実施の形態、第３の実施の形態で説明したジャイロセンサの軸の傾斜角の推定方法を、協調型ＡＣＣシステムに適用してもよい。

また、上記の第２の実施の形態において、ロール判定部の判定結果に基づくバンクなどの情報を位置情報とともに、自車両あるいはセンターに蓄えておくことで、そこを通る車両が、当該バンク情報を利用できるようにしてもよい。

また、上記第１〜第４の実施の形態では、速度ベクトルを算出する場合を例に説明したが、これに限定されるものではなく、自車両の方位角を算出すると共に、自車両の速度を検出し、自車両の方位角と速度からなる速度情報を用いてもよい。

また、上記第１〜第４の実施の形態では、車両に搭載されるセンサ傾斜判定装置について説明したが、本発明のセンサ傾斜判定装置が搭載される移動体は車両に限定されない。例えば、センサ傾斜判定装置をロボットに搭載してもよい。

また、上記第１〜第４の実施の形態では、ＧＰＳの信頼度が高いと判定された場合であって、ジャイロセンサから算出されるジャイロの角度変化と、ＧＰＳ情報を用いて算出されたＧＰＳ推定方位角の変化とのつじつまが合わないと判断され、かつ、ヨーレイトが発生している場合に、ジャイロセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定する場合を例に説明したが、これに限定されるものではなく、ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定される場合が、ＧＰＳの信頼度が高いと判定される場合に限らない。例えば、ジャイロセンサから算出されるジャイロの角度変化と、ＧＰＳ情報を用いて算出されたＧＰＳ推定方位角の変化とのつじつまが合わないと判断され、かつ、ヨーレイトが発生している場合に、ジャイロセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定するようにしてもよい。

１０、２１０、３１０ センサ軸角度推定装置
１２ 受信部
１４ ジャイロセンサ
１６ 速度センサ
１８、２１８、３１８ コンピュータ
２０ 情報取得部
２２ データ記憶部
２４ 方位角変化推定部
２６ 角度変化取得部
２８、２２８、３２８ 姿勢推定部
７４ ジャイロバイアス推定部
７６ ドップラー信頼度判定部
７８ ジャイロ軸傾斜判定部
８０ ジャイロ軸角度推定部
８２、２８２、３８２ ロール判定部

Claims (11)

1. 移動体に搭載され、かつ、前記移動体に発生するヨーレイトを検出するヨーレイトセンサと、
   複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段と、
   前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体の角度の変化を算出する第１算出手段と、
   前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の基準方向の方位角の変化を算出する第２算出手段と、
   前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とが対応しているか否かを判定する角度変化判定手段と、
   前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体にヨーレイトが発生しているか否かを判定するヨーレイト判定手段と、
   前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定する傾斜判定手段と、
   を含むセンサ傾斜判定装置。
2. 前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とに基づいて、前記ヨーレイトセンサによって検出されたヨーレイトのバイアス補正を行うバイアス補正手段と、
   前記バイアス補正手段によってバイアス補正されたヨーレイトと、前記第２算出手段によって算出された前記方位角の変化の変化量とに基づいて、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜している傾斜角度を推定する傾斜角度推定手段と、
   を更に含む請求項１記載のセンサ傾斜判定装置。
3. 前記傾斜角度推定手段によって推定された傾斜角度の変化に基づいて、前記ヨーレイトセンサの軸の傾斜が、取り付け誤差による傾斜であるか、あるいは前記移動体のロール方向の運動による傾斜であるかを判定するロール判定手段を更に含む請求項２記載のセンサ傾斜判定装置。
4. 前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とに基づいて、前記ヨーレイトセンサによって検出されたヨーレイトのバイアス補正を行うバイアス補正手段と、
   前記第１算出手段によって算出された前記角度の変化に基づくヨーレイト、及び前記バイアス補正手段によってバイアス補正されたヨーレイトの比と、前記移動体がロール方向に運動していないときの予め求められた前記ヨーレイトの比とに基づいて、前記ヨーレイトセンサの軸の傾斜が、取り付け誤差による傾斜であるか、あるいは前記移動体のロール方向の運動による傾斜であるかを判定するロール判定手段を更に含む請求項１又は２記載のセンサ傾斜判定装置。
5. 前記複数のＧＰＳ衛星の各々の衛星情報から得られるＧＰＳの信頼度を示す指標を算出する信頼度算出手段を更に含み、
   前記傾斜判定手段は、前記信頼度算出手段によって算出された前記指標に基づいて前記ＧＰＳの信頼度が高いと判定される場合であって、前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定する請求項１〜請求項４の何れか１項記載のセンサ傾斜判定装置。
6. 前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の高さ方向の変化を算出する高さ変化算出手段と、
   前記複数のＧＰＳ衛星の各々の衛星情報から得られる、水平面方向のＧＰＳの信頼度を示す第１指標及び高さ方向のＧＰＳの信頼度を示す第２指標を各々算出する信頼度算出手段とを更に含み、
   前記傾斜判定手段は、前記信頼度算出手段によって算出された前記第１指標に基づいて前記水平面方向のＧＰＳの信頼度が高いと判定される場合であって、前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面の垂直方向に対して傾斜していると判定し、
   前記信頼度算出手段によって算出された前記第２指標に基づいて前記高さ方向のＧＰＳの信頼度が高いと判定される場合に、前記高さ変化算出手段によって算出された前記移動体の高さ方向の変化に基づいて、前記ヨーレイトセンサの軸の傾斜が、取り付け誤差による傾斜であるか、あるいは前記移動体のロール方向の運動による傾斜であるかを判定するロール判定手段を更に含む請求項１又は２記載のセンサ傾斜判定装置。
7. 前記ＧＰＳの信頼度を示す指標を、ＧＰＳ受信機が同一時刻に受信した複数のＧＰＳ衛星の各々の衛星情報から得られ、かつ該複数の衛星情報各々を送信した複数のＧＰＳ衛星の配置に応じた測位精度の低下率を表すＤＯＰとした請求項５又は６記載のセンサ傾斜判定装置。
8. 前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の進行方向を含む速度ベクトル、又は前記移動体の速度及び前記移動体の進行方向を示す速度情報を算出する速度算出手段を更に含み、
   前記第２算出手段は、前記速度算出手段によって算出された前記速度情報に基づいて、前記移動体の基準方向の方位角を推定することにより、前記移動体の基準方向の方位角の変化を算出する請求項１〜請求項７の何れか１項記載のセンサ傾斜判定装置。
9. 前記速度算出手段は、前記ＧＰＳ衛星の各々の位置に関する情報、及び前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報から得られる前記移動体の位置に基づいて、前記移動体から見た前記ＧＰＳ衛星の各々の方向を算出し、時系列の前記ＧＰＳ衛星の各々の位置に関する情報に基づいて、前記ＧＰＳ衛星の各々の速度を算出し、前記移動体から見た前記ＧＰＳ衛星の各々の方向、前記ＧＰＳ衛星の各々の速度、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報に基づいて、前記移動体のＧＰＳ衛星の各々の方向の速度を算出し、複数の前記移動体のＧＰＳ衛星の各々の方向の速度に基づいて、前記移動体の速度ベクトルを算出する請求項８記載のセンサ傾斜判定装置。
10. 前記ＧＰＳ衛星の各々の位置に関する情報を、衛星軌道情報とし、前記ＧＰＳ衛星の各々と前記移動体との距離に関する情報を、擬似距離情報とし、前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を、ドップラー周波数情報とした請求項１〜請求項９の何れか１項記載のセンサ傾斜判定装置。
11. コンピュータを、
    複数のＧＰＳ衛星の各々から送信された前記ＧＰＳ衛星の各々の位置に関する情報、前記ＧＰＳ衛星の各々と移動体との距離に関する情報、及び前記ＧＰＳ衛星の各々に対する前記移動体の相対速度に関する情報を含む衛星情報を取得する衛星情報取得手段、
    前記移動体に搭載され、かつ、前記移動体に発生するヨーレイトを検出するヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体の角度の変化を算出する第１算出手段、
    前記衛星情報取得手段により取得された前記衛星情報に基づいて、前記移動体の基準方向の方位角の変化を算出する第２算出手段、
    前記第１算出手段によって算出された前記角度の変化と、前記第２算出手段によって算出された前記方位角の変化とが対応しているか否かを判定する角度変化判定手段と、
    前記ヨーレイトセンサによって検出されたヨーレイトに基づいて、前記移動体にヨーレイトが発生しているか否かを判定するヨーレイト判定手段、及び
    前記角度変化判定手段によって前記角度の変化が対応していないと判定され、かつ、前記ヨーレイト判定手段によってヨーレイトが発生していると判定された場合に、前記ヨーレイトセンサの軸が、水平面に対する垂直方向に対して傾斜していると判定する傾斜判定手段
    として機能させるためのプログラム。