JP5639468B2

JP5639468B2 - 取り付け角度検出装置、方法、プログラムおよび記録媒体

Info

Publication number: JP5639468B2
Application number: JP2010294344A
Authority: JP
Inventors: 真秀橋本; 一聡田中; 淑子加藤; 良樹轡
Original assignee: Pioneer Corp
Current assignee: Pioneer Corp
Priority date: 2010-12-28
Filing date: 2010-12-28
Publication date: 2014-12-10
Anticipated expiration: 2030-12-28
Also published as: JP2012141220A

Description

本発明は、ジャイロセンサ等の検出装置の取り付け角度検出装置、方法、プログラムおよび記録媒体に関する。

例えば、車両等に搭載されるナビゲーション装置には、ＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）衛星から受信する位置情報と、ジャイロセンサや加速度センサによる検出情報に基づいて走行距離および移動方位情報を取得し、現在位置と進行方向を推測し、案内表示を行っているものがある。ところが、上述のジャイロセンサや加速度センサは、車両に対して検出軸が傾いて取り付けられていると、出力値が所望の値よりも減少し、正確な値が得られない。

このため、車載装置等に取り付けられる検出装置の取り付け角度を検出することが必要である。このような車載装置の取り付け角度が異常であることを通知する車載装置が提案されている（特許文献１参照）。

この従来の車載装置は、ジャイロセンサおよびＧＰＳ受信装置を備え、車両に搭載される車載装置であって、ＧＰＳ受信装置の出力から求めた参照角速度と前記ジャイロセンサにより検出された角速度との比をとることで前記ジャイロセンサの感度を求め、求められた感度が許容範囲を超える場合に、前記車載装置の取り付け角度が異常であることを通知するようになっている。また、この車載装置は、進行方向の加速度成分がゼロの時における加速度センサの出力Ａｓを積算し、平均化させた値Ａａｖｅを算出し、この平均化させた値Ａａｖｅを重力加速度ｇで除算することでピッチ方向の取り付け角度に相当するｓｉｎ（θｐｉｔｃｈ）を求めている。

特許第４１９１４９９号

しかしながら、上述した従来の車載装置においては、センサがロール軸回りに傾いて取り付けられている場合、ヨー角の角度変化、すなわち、車両の左右旋回の影響を受けてしまう。特に、ヨー角の角度変化は０〜３６０度まで起こり得るため、角度変化が大きいとその影響は大きくなる。また、ピッチ角は大きく変化しても±５度程度と、ヨー角に比べて極めて角度変化が少ないため、ピッチ角を求めるためにヨー角の影響を受ける加速度の値を用いることは、誤差範囲が大きくなってしまうといった問題があった。

さらに、ＧＰＳからデータを受信して利用する場合においても、ヨー角に対応する方位位置情報のデータの利用は、広範囲での多少の誤差となるものの、ピッチ角に対応する高度情報のデータは、利用するデータの値の範囲が狭く、誤差の率が高くなる。このため、ピッチ角の情報から直接ピッチ軸回りの取り付け角度を検出しようとすると、信頼性が低くなってしまうといった問題があった。

本発明は、このような従来の問題を一例とした課題を解決するためになされたもので、ジャイロセンサ等の検出装置の取り付け角度の検出の信頼性を高めた取り付け角度検出装置、方法、プログラムおよび記録媒体を提供することを課題とする。

上記課題を解決するため、請求項１に記載の発明は、ＸＹＺ座標系におけるＺ軸方向の変動を伴うＸＹ平面を移動する移動体に取り付けられた検出装置の取り付け角度を検出する取り付け角度検出装置において、所定の検出開始位置から所定の検出終了位置までの検出範囲における前記移動体のＺ軸回りのＺ軸回転角変化量を取得するＺ軸回転角変化量取得手段と、前記検出装置に検出された検出値に基づいて、前記検出範囲におけるＹ軸回りのＹ軸回転角変化量を算出するＹ軸回転角変化量算出手段と、前記Ｚ軸回転角変化量取得手段により取得されたＺ軸回転角変化量と、前記Ｙ軸回転角変化量算出手段により算出されたＹ軸回転角変化量と、の比に基づいて、前記検出装置の前記移動体に対するＸ軸回りの取り付け角度を検出する取り付け角度検出手段と、を備えたことを特徴とする。
なお、上記Ｚ軸方向の変動とは、移動体が走行するＸＹ平面と略平行な走行面の傾斜や凹凸によって発生する移動体の変位のことをいう。

また、請求項１２に記載の発明は、ＸＹＺ座標系におけるＺ軸方向の変動を伴うＸＹ平面を移動する移動体に取り付けられた検出装置の取り付け角度を検出する取り付け角度検出方法において、所定の検出開始位置から所定の検出終了位置までの検出範囲における前記移動体のＺ軸回りのＺ軸回転角変化量を取得するＺ軸回転角変化量取得ステップと、前記検出装置に検出された検出結果に基づいて、前記検出範囲におけるＹ軸回りのＹ軸回転角変化量を算出するＹ軸回転角変化量算出ステップと、前記Ｚ軸回転角変化量取得ステップで取得したＺ軸回転角変化量と、前記Ｙ軸回転角変化量算出ステップで算出したＹ軸回転角変化量と、の比に基づいて、前記検出装置の前記移動体に対するＸ軸回りの取り付け角度を検出する取り付け角度検出ステップと、を備えたことを特徴とする。

さらに、請求項１３に記載の発明は、ＸＹＺ座標系におけるＺ軸方向の変動を伴うＸＹ平面を移動する移動体に取り付けられた検出装置の取り付け角度を検出する取り付け角度検出プログラムにおいて、所定の検出開始位置から所定の検出終了位置までの検出範囲における前記移動体のＺ軸回りのＺ軸回転角変化量を取得するＺ軸回転角変化量取得ステップと、前記検出装置に検出された検出結果に基づいて、前記検出範囲におけるＹ軸回りのＹ軸回転角変化量を算出するＹ軸回転角変化量算出ステップと、前記Ｚ軸回転角変化量取得ステップで取得したＺ軸回転角変化量と、前記Ｙ軸回転角変化量算出ステップで算出したＹ軸回転角変化量と、の比に基づいて、前記検出装置の前記移動体に対するＸ軸回りの取り付け角度を検出する取り付け角度検出ステップと、をコンピュータに実行させることを特徴とする。

さらに、請求項１４に記載の記録媒体は、請求項１３に記載の取り付け角度検出プログラムを記録したことを特徴とする。

本実施の形態における取り付け角度検出装置の概略構成を示すブロック図である。本実施の形態における座標系の定義、および、検出装置の取り付け角度を示す図である。本実施の形態における動作原理を説明する図である。本実施の形態における取り付け角度検出処理を示すフローチャートである。本実施の形態におけるピッチ角変化量の補正処理を示すフローチャートである。本実施の形態における取り付け角度検出処理を示すフローチャートである。

以下、本発明の実施の形態について、図面を参照して説明する。
図１は、本実施の形態における取り付け角度検出装置の概略構成を示すブロック図である。図２は、本実施の形態における座標系の定義、および、検出装置の取り付け角度を示す図である。なお、本実施の形態は、移動体として車両１０を、移動体に取り付けられた検出装置としてピッチジャイロ２１を例にとり、車両１０に対するピッチジャイロ２１のＸ軸（ロール軸）回りの取り付け角度φｘを検出する取り付け角度検出装置１００について説明する。
また、車両１０は、Ｘ軸方向を進行方向とし、傾斜や凹凸により多少Ｚ軸方向に変動するものの、ＸＹ平面上での旋回および移動を基本とするものとする。

図１に示すように、取り付け角度検出装置１００は、所定の検出範囲におけるヨー角変化量θｚを取得するヨー角変化量取得部１１０と、上記所定の検出範囲におけるピッチ角変化量θｙを算出するピッチ角変化量算出部１２０と、取り付け角度φｘを検出する取り付け角度検出部１３０と、車両１０の傾斜角ψｘを算出するロール軸傾斜角算出部１４０と、ピッチジャイロ２１に検出されたピッチ角速度ωｙあるいはピッチ角速度ωｙにより算出されたピッチ角変化量θｙを取り付け角度φｘに基づいて補正する補正部１５０と、を備えている。

ここで、ヨー角変化量θｚとは、車両１０のＺ軸（ヨー軸）回りのＺ軸回転角の変化量θｚのことをいう。また、ピッチ角変化量θｙとは、Ｙ軸（ピッチ軸）回りのＹ軸回転角の変化量θｙのことをいう。さらに、取り付け角度φｘとは、ピッチジャイロ２１の車両１０に対するＸ軸回りの取り付け角度φｘのことをいう。

また、車両１０の傾斜角ψｘとは、車両１０が接地しているＸＹ平面と水平面との傾斜角ψｘのことをいう。また、ピッチ角速度ωｙとは、ピッチ角の単位時間当たりの変化量、すなわち、Ｙ軸回りのＹ軸回転角速度ωｙのことをいう。
さらに、以下では、Ｚ軸回転角速度ωｚのことをヨー角速度ωｚという。

ヨー角変化量取得部１１０は、ヨー角速度ωｚを検出するヨージャイロ１１１と、位置情報Ｐを取得するＧＰＳ受信装置１１２と、所定の検出範囲におけるヨー角変化量θｚを算出するヨー角変化量算出部１１３と、を備えたＺ軸回転角変化量取得手段である。

ここで、所定の検出範囲とは、ヨー角速度ωｚが所定の角速度しきい値ωｔｈ、例えば、３［°／ｓ］となった位置を検出開始位置とし、所定の角速度しきい値ωｔｈ未満となった位置を検出終了位置とする。
このように、本実施の形態における取り付け角度検出装置１００は、ヨー角速度ωｚが所定の角速度しきい値ωｔｈ以上である場合を検出範囲として、取り付け角度φｘを検出することにより、不安定な値を除去し、検出誤差を小さくすることができ、取り付け角度φｘの検出の信頼性を高めることができる。

なお、検出開始位置を決定する角速度しきい値ωｔｈと、検出終了位置を決定する角速度しきい値ωｔｈと、を異なる値としても良い。
また、ＧＰＳ受信装置１１２が取得する位置情報Ｐとは、車両１０の東経、北緯、標高等を随時、車両１０の位置ごとに取得することができるものである。

ヨージャイロ１１１は、車両１０のヨー角速度ωｚを検出するＺ軸回転角速度検出手段である。ＧＰＳ受信装置１１２は、車両１０の東経、北緯等の位置情報Ｐを取得する位置情報取得手段である。ヨー角変化量算出部１１３は、ヨージャイロ１１が取得したヨー角速度ωｚまたはＧＰＳ受信装置１１２が取得した位置情報Ｐの推移から所定の検出範囲における車両１０のヨー角変化量θｚを算出するＺ軸回転角変化量算出手段である。

このように、本実施の形態における取り付け角度検出装置１００は、ヨージャイロ１１１に検出されたヨー角速度ωｚ、または、ＧＰＳ受信装置１１２に取得された位置情報Ｐに基づいて、車両１０のヨー角変化量θｚを取得することができ、取り付け角度φｘを検出するので、簡単な構成部品で検出の信頼性を高めることができる。

ピッチ角変化量算出部１２０は、ピッチジャイロ２１から車両１０のピッチ角速度ωｙを取得し、取得したピッチ角速度ωｙの推移から所定の検出範囲における車両１０のピッチ角変化量θｙを算出するＹ軸回転角変化量算出手段である。

取り付け角度検出部１３０は、ヨー角変化量取得部１１０により取得されたヨー角変化量θｚと、ピッチ角変化量算出部１２０により算出されたピッチ角変化量θｙと、の比に基づいて、取り付け角度φｘを検出する取り付け角度検出手段である。
これにより、本実施の形態における取り付け角度検出装置１００は、ピッチジャイロの車両１０に対するロール軸回りの取り付け角度φｘの検出の信頼性を高めることができる。

また、取り付け角度検出部１３０は、ヨー角変化量取得部１１０により取得されたヨー角変化量θｚが、所定の回転角変化量しきい値θｚｔｈ、例えば、６０［°］以上である場合に、取り付け角度φｘを検出するようになっている。
このように、本実施の形態における取り付け角度検出装置１００は、ヨー角変化量θｚが所定の回転角変化量しきい値θｚｔｈ以上である場合に、取り付け角度φｘを検出することにより、変位が大きいときのみの検出によって検出誤差を小さくすることができ、取り付け角度φｘの検出の信頼性を高めることができる。

さらに、取り付け角度検出部１３０は、ヨー角変化量θｚとピッチ角変化量θｙとを、複数の検出範囲においてそれぞれ取得し、各検出範囲の取り付け角度の検出値φｘｎ（ｎ＝１，２，３・・・）を算出し、取り付け角度の検出値φｘｎを平均化することにより、取り付け角度φｘを検出するようになっている。
これにより、本実施の形態における取り付け角度検出装置１００は、検出誤差を小さくすることができ、取り付け角度φｘの検出の信頼性を高めることができる。

さらに、取り付け角度検出部１３０は、ロール軸傾斜角算出部１４０に算出された傾斜角ψｘに基づいて、ヨー角変化量θｚおよびピッチ角変化量θｙを補正して、取り付け角度φｘを検出することもできるようになっている。
これにより、本実施の形態における取り付け角度検出装置１００は、検出の信頼性を高めることができる。

ロール軸傾斜角算出部１４０は、車両１０の傾斜角ψｘを算出するＸ軸傾斜角算出手段である。例えば、ロール軸傾斜角算出部１４０は、ロールジャイロから車両１０のロール角速度を取得し、取得したロール角速度の値に基づいて、車両１０の傾斜角ψｘを算出するようになっている。

補正部１５０は、取り付け角度φｘに基づいてピッチジャイロ２１に検出されたピッチ角速度ωｙあるいはピッチ角変化量θｙを補正する補正手段である。
このように、本実施の形態における取り付け角度検出装置１００は、検出した取り付け角度φｘに基づいてピッチジャイロ２１に検出されたピッチ角速度ωｙあるいはピッチ角変化量θｙを補正するので、信頼性の高いピッチ角速度ωｙあるいはピッチ角変化量θｙを出力することができる。

なお、上記構成において、取り付け角度φｘを検出する検出装置として、ピッチジャイロ２１の代わりにロールジャイロの取り付け角度φｘを検出することもできる。このロールジャイロの取り付け角度φｘを検出する場合には、Ｘ軸をピッチ軸、Ｙ軸をロール軸として、上記と同様な構成で、ピッチ軸回りの取り付け角度φｘを検出することができる。

すなわち、ピッチ角変化量算出部１２０は、Ｙ軸をロール軸としたロール角変化量算出部として機能し、ロールジャイロから車両１０のロール角速度ωｙを取得し、取得したロール角速度ωｙの推移から所定の検出範囲における車両１０のロール角変化量θｙを算出する。

取り付け角度検出部１３０は、ヨー角変化量取得部１１０により取得されたヨー角変化量θｚと、ピッチ角変化量算出部１２０により算出されたロール角変化量θｙと、の比に基づいて、取り付け角度φｘを検出するものである。

ロール軸傾斜角算出部１４０は、車両１０の傾斜角ψｘを算出するものであるが、上記実施の形態と異なり、ピッチ軸傾斜角算出部として機能し、例えば、ピッチジャイロから車両１０のピッチ角速度を取得し、取得したピッチ角速度の値に基づいて、車両１０の傾斜角ψｘを算出するようになっている。

補正部１５０は、取り付け角度φｘに基づいてロールジャイロに検出されたロール角速度ωｙあるいはロール角変化量θｙを補正するようになっている。
これにより、本実施の形態における取り付け角度検出装置１００は、ロールジャイロの車両１０に対するピッチ軸回りの取り付け角度φｘの検出の信頼性を高めることができる。

次に、本実施の形態における取り付け角度φｘを検出するための動作原理について、説明する。
図２、図３は、本実施の形態の取り付け角度検出装置１００における取り付け角度φｘを検出する原理を説明する図である。

ピッチジャイロ２１は、車両１０に対して水平に取り付けられていると、車両１０がヨー軸の周りに旋回しても、ピッチ角θｙは、変化するものではない。
一方、図２（ｂ）に示すように、ピッチジャイロ２１が車両１０に対してロール軸回りに傾いて取り付けられている場合には、車両１０がヨー軸の周りに旋回すると、ピッチ角θｙが変化する。また、このヨー軸の周りの旋回に伴って変化するピッチ角θｙは、車両１０がヨー軸の周りに旋回した大きさに応じた値となる。

そこで、本実施の形態の取り付け角度検出装置１００は、車両１０のヨー角変化量θｚにともなって変化するピッチ角変化量θｙを検出し、ヨー角変化量θｚとピッチ角変化量θｙとの比に基づいて、ピッチジャイロ２１のロール軸回りの取り付け角度φｘを検出する。

具体的には、以下の式（１）により、
φｘ＝ｓｉｎ^−１｛（θｙ／θｚ）＊（１８０／π）｝・・・（１）
取り付け角度φｘを算出する。
また、ヨー角変化量θｚおよびピッチ角変化量θｙは、以下の方法による。すなわち、図３に示すように、ヨー角速度ωｚが角速度しきい値ωｔｈ以上である範囲を検出範囲として、検出範囲内のヨー角速度ωｚを積分することにより、ヨー角変化量θｚを算出する。そして、同一の検出範囲内のピッチ角速度ωｙを積分することにより、ピッチ角変化量θｙを算出する。

図４に、本取り付け角度検出装置１００における取り付け角度φｘの検出処理のフローチャートを示し、説明する。
なお、本取り付け角度検出装置１００は、例えば、ＣＰＵ、ＲＯＭ、ＲＡＭを有し、取り付け角度検出処理が記録された取り付け角度検出プログラムをＲＯＭに記憶し、ＣＰＵが、ＲＡＭをワーク領域として、取り付け角度検出プログラムを実行するようになっている。

図４に示すように、取り付け角度検出装置１００は、取り付け角度検出処理が開始されると、初期化処理を行う（ステップＳ１１）。具体的には、取り付け角度検出装置１００は、算出回数Ｎと、取り付け角度平均値φｘａｖと、の値をクリア（＝０）する。
次いで、取り付け角度検出装置１００は、角度変化量初期化処理を行う（ステップＳ１２）。具体的には、取り付け角度検出装置１００は、ヨー角変化量θｚと、ピッチ角変化量θｙと、の値をクリア（＝０に）する。

次いで、取り付け角度検出装置１００は、前回のヨー角速度ωｚの取得から検出単位時間が経過したか否かの判定を行う（ステップＳ１３）。前回のヨー角速度ωｚの取得から検出単位時間が経過していなければ、検出単位時間が経過するまで待機し、検出単位時間が経過した場合には、ステップＳ１４に移行する。

次いで、取り付け角度検出装置１００は、ヨージャイロ１１１によりヨー角速度ωｚを取得する（ステップＳ１４）。
次いで、取り付け角度検出装置１００は、取得したヨー角速度ωｚがヨー角速度しきい値ωｔｈ以上であるか否かを判定し、取得したヨー角速度ωｚがヨー角速度しきい値ωｔｈ以上である場合には、ステップＳ１６に移行し、取得したヨー角速度ωｚがヨー角速度しきい値ωｔｈ以上でない場合には、ステップＳ２１に移行する（ステップＳ１５）。

取り付け角度検出装置１００は、取得したヨー角速度ωｚがヨー角速度しきい値ωｔｈ以上である場合には、ヨー角速度ωｚに基づいて、ヨー角変化量θｚの値を更新する（ステップＳ１６）。具体的には、前回までのヨー角変化量θｚに今回取得したヨー角速度ωｚに検出単位時間を乗算した値を加算する。
また、取り付け角度検出装置１００は、ピッチジャイロ２１によりピッチ角速度ωｙを取得する（ステップＳ１７）。

次いで、取り付け角度検出装置１００は、取得したピッチ角速度ωｙに基づいて、ピッチ角変化量θｙの値を更新して、ステップＳ１３に移行する（ステップＳ１８）。具体的には、前回までのピッチ角変化量θｙに今回取得したピッチ角速度ωｙに検出単位時間を乗算した値を加算する。

一方、取り付け角度検出装置１００は、取得したヨー角速度ωｚがヨー角速度しきい値ωｔｈ以上でない場合には（ステップＳ１５でＮＯと判定）、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上であるか否かを判定し、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上である場合には、ステップＳ２２に移行し、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上でない場合には、ステップＳ１２に移行する（ステップＳ２１）。

取り付け角度検出装置１００は、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上である場合には、ヨー角変化量θｚとピッチ角変化量θｙとの比に基づいて、取り付け角度φｘを検出する（ステップＳ２２）。具体的には、上記式（１）により、ｓｉｎ^−１｛（θｙ／θｚ）＊（１８０／π）｝を算出し、取り付け角度φｘを算出する。

次いで、取り付け角度検出装置１００は、算出回数Ｎに“１”加算する（ステップＳ２３）。次いで、取り付け角度検出装置１００は、取り付け角度φｘの平均化処理を行う。（ステップＳ２４）。具体的には、前回までの取り付け角度平均値φｘａｖに、今回算出した取り付け角度φｘと前回までの取り付け角度平均値φｘａｖとの差分値を、算出回数Ｎで除算した値を加算する。上記取り付け角度平均値φｘａｖの算出式を、以下の式（２）に示す。
φｘａｖ_（ｎ）＝φｘａｖ_{（ｎ−１）}＋（φｘ−φｘａｖ_{（ｎ−１）}）／Ｎ・・・（２）

次いで、取り付け角度検出装置１００は、取り付け角度φｘの検出処理の終了条件が成立したか否かを判定して、終了条件が成立した場合には、本取り付け角度検出処理を終了し、終了条件が成立していない場合には、ステップＳ１２に移行する（ステップＳ２５）。例えば、算出回数Ｎが所定の検出処理終了回数に達した場合に、本取り付け角度検出処理を終了する。

以上に処理により、本取り付け角度検出装置１００により、ピッチジャイロ２１の車両１０に対する取り付け角度φｘを検出することができる。

次に、本取り付け角度検出装置１００において検出した取り付け角度φｘを用いて、ピッチ角変化量θｙの補正処理について、説明する。
図５に、本取り付け角度検出装置１００におけるピッチ角変化量θｙの補正処理のフローチャートを示し、説明する。

図５に示すように、取り付け角度検出装置１００は、ピッチ角変化量の補正処理が開始されると、取り付け角度φｘが検出済みであるか否かを判定する（ステップＳ３１）。取り付け角度φｘが検出済みであれば、ステップＳ３２に移行し、取り付け角度φｘが検出済みでなければ、本ピッチ角変化量の補正処理を終了する。

取り付け角度検出装置１００は、取り付け角度φｘが検出済みである場合には、ピッチ角変化量θｙに影響を与えるヨー角変化量θｚの影響を算出する（ステップＳ３２）。具体的には、上記影響度は、θｚ＊ｓｉｎ｛φｘ＊（π／１８０）｝によって算出される。

次いで、取り付け角度検出装置１００は、算出されたピッチ角変化量θｙから上記影響度を減算して、補正されたピッチ角変化量θｙを算出して（ステップＳ３３）、本ピッチ角変化量の補正処理を終了する。

次に、図６に、ＧＰＳ受信装置１１２を利用した場合の本取り付け角度検出装置１００における取り付け角度φｘの検出処理のフローチャートを示し、説明する。

図６に示すように、取り付け角度検出装置１００は、取り付け角度検出処理が開始されると、初期化処理を行う（ステップＳ４１）。具体的には、取り付け角度検出装置１００は、算出回数Ｎと、取り付け角度平均値φｘａｖと、の値をクリア（＝０）する。
次いで、取り付け角度検出装置１００は、ＧＰＳ受信装置１１２により現在位置を取得し、前回位置Ｐｏとして記憶する（ステップＳ４２）。

次いで、取り付け角度検出装置１００は、前回の現在位置の取得から検出単位時間が経過したか否かの判定を行う（ステップＳ４３）。前回の現在位置の取得から検出単位時間が経過していなければ、検出単位時間が経過するまで待機し、検出単位時間が経過した場合には、ステップＳ４４に移行する。

次いで、取り付け角度検出装置１００は、ＧＰＳ受信装置１１２により現在位置を取得し、今回位置Ｐｎとして記憶する（ステップＳ４４）。
次いで、取り付け角度検出装置１００は、今回位置Ｐｎと前回位置Ｐｏとに基づいて、今回移動方位Ａｎを算出する（ステップＳ４５）。

次いで、取り付け角度検出装置１００は、角度変化量初期化処理を行う（ステップＳ５１）。具体的には、取り付け角度検出装置１００は、ヨー角変化量θｚと、ピッチ角変化量θｙと、の値をクリア（＝０に）する。

次いで、取り付け角度検出装置１００は、前回の現在位置の取得から検出単位時間が経過したか否かの判定を行う（ステップＳ５２）。前回の現在位置の取得から検出単位時間が経過していなければ、検出単位時間が経過するまで待機し、検出単位時間が経過した場合には、ステップＳ５３に移行する。

次いで、取り付け角度検出装置１００は、今回位置Ｐｎを前回位置Ｐｏに代入し（ステップＳ５３）、今回移動方位Ａｎを前回移動方位Ａｏに代入する（ステップＳ５４）。
次いで、取り付け角度検出装置１００は、ＧＰＳ受信装置１１２により現在位置を取得し、今回位置Ｐｎとして記憶する（ステップＳ５５）。

次いで、取り付け角度検出装置１００は、今回位置Ｐｎと前回位置Ｐｏとに基づいて、今回移動方位Ａｎを算出する（ステップＳ５６）。
次いで、取り付け角度検出装置１００は、今回移動方位Ａｎと前回移動方位Ａｏとに基づいて、方位差分値θｚｄｅｆを算出する（ステップＳ５７）。

次いで、取り付け角度検出装置１００は、算出した方位差分値θｚｄｅｆが所定の方位差分値しきい値θｚｄｅｆｔｈ以上であるか否かを判定し、算出した方位差分値θｚｄｅｆが方位差分値しきい値θｚｄｅｆｔｈ以上である場合には、ステップＳ６１に移行し、算出した方位差分値θｚｄｅｆが方位差分値しきい値θｚｄｅｆｔｈ以上でない場合には、ステップＳ７１に移行する（ステップＳ５８）。

取り付け角度検出装置１００は、算出した方位差分値θｚｄｅｆが方位差分値しきい値θｚｄｅｆｔｈ以上である場合には、方位差分値θｚｄｅｆに基づいて、ヨー角変化量θｚの値を更新する（ステップＳ６１）。具体的には、前回までのヨー角変化量θｚに今回算出した方位差分値θｚｄｅｆを加算する。
また、取り付け角度検出装置１００は、ピッチジャイロ２１によりピッチ角速度ωｙを取得する（ステップＳ６２）。

次いで、取り付け角度検出装置１００は、取得したピッチ角速度ωｙに基づいて、ピッチ角変化量θｙの値を更新して、ステップＳ５２に移行する（ステップＳ６３）。具体的には、前回までのピッチ角変化量θｙに今回取得したピッチ角速度ωｙに検出単位時間を乗算した値を加算する。

一方、取り付け角度検出装置１００は、算出した方位差分値θｚｄｅｆが方位差分値しきい値θｚｄｅｆｔｈ以上でない場合には（ステップＳ５８でＮＯと判定）、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上であるか否かを判定し、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上である場合には、ステップＳ７２に移行し、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上でない場合には、ステップＳ５１に移行する（ステップＳ７１）。

取り付け角度検出装置１００は、ヨー角変化量θｚがヨー角変化量しきい値θｚｔｈ以上である場合には、ヨー角変化量θｚとピッチ角変化量θｙとの比に基づいて、取り付け角度φｘを検出する（ステップＳ７２）。具体的には、上記式（１）により、ｓｉｎ^−１｛（θｙ／θｚ）＊（１８０／π）｝を算出し、取り付け角度φｘを算出する。

次いで、取り付け角度検出装置１００は、算出回数Ｎに“１”加算する（ステップＳ７３）。次いで、取り付け角度検出装置１００は、取り付け角度φｘの平均化処理を行う。（ステップＳ７４）。具体的には、前回までの取り付け角度平均値φｘａｖに、今回算出した取り付け角度φｘと前回までの取り付け角度平均値φｘａｖとの差分値を、算出回数Ｎで除算した値を加算する。取り付け角度平均値φｘａｖの算出式は、上記した式（２）と同様である。

次いで、取り付け角度検出装置１００は、取り付け角度φｘの検出処理の終了条件が成立したか否かを判定して、終了条件が成立した場合には、本取り付け角度検出処理を終了し、終了条件が成立していない場合には、ステップＳ５１に移行する（ステップＳ７５）。
以上の処理により、本取り付け角度検出装置１００により、ピッチジャイロ２１の車両１０に対する取り付け角度φｘを検出することができる。

以上のように、本実施の形態における取り付け角度検出装置、取り付け角度検出方法、取り付け角度検出プログラムおよび記録媒体によれば、所定の検出範囲におけるヨー角変化量θｚを取得するヨー角変化量取得部１１０と、上記所定の検出範囲におけるピッチ角変化量θｙを算出するピッチ角変化量算出部１２０と、取得されたヨー角変化量θｚと算出されたピッチ角変化量θｙとの比に基づいて取り付け角度φｘを検出する取り付け角度検出部１３０と、を備えたので、ピッチジャイロ２１等の検出装置の取り付け角度φｘの検出の信頼性を高めることができる。

また、本取り付け角度検出方法、取り付け角度検出プログラムおよび取り付け角度検出プログラムを記録した記録媒体においても同様に、ヨー角変化量θｚとピッチ角変化量θｙとの比に基づいて取り付け角度φｘを検出するので、ピッチジャイロ２１等の検出装置の取り付け角度φｘの検出の信頼性を高めることができる。

１０車両
１００取り付け角度検出装置
１１０ヨー角変化量取得部
１１１ヨージャイロ
１１２ＧＰＳ受信装置
１１３ヨー角変化量算出部
１２０ピッチ角変化量算出部
１３０取り付け角度検出部
１４０ロール軸傾斜角算出部
１５０補正手段

Claims

ＸＹＺ座標系におけるＺ軸方向の変動を伴うＸＹ平面を移動する移動体に取り付けられた検出装置の取り付け角度を検出する取り付け角度検出装置において、
所定の検出開始位置から所定の検出終了位置までの検出範囲における前記移動体のＺ軸回りのＺ軸回転角変化量を取得するＺ軸回転角変化量取得手段と、
前記検出装置に検出された検出値に基づいて、前記検出範囲におけるＹ軸回りのＹ軸回転角変化量を算出するＹ軸回転角変化量算出手段と、
前記Ｚ軸回転角変化量取得手段により取得されたＺ軸回転角変化量と、前記Ｙ軸回転角変化量算出手段により算出されたＹ軸回転角変化量と、の比に基づいて、前記検出装置の前記移動体に対するＸ軸回りの取り付け角度を検出する取り付け角度検出手段と、
を備えたことを特徴とする取り付け角度検出装置。
前記取り付け角度検出手段は、前記Ｚ軸回転角変化量と前記Ｙ軸回転角変化量とを、複数の前記検出範囲においてそれぞれ取得し、各検出範囲の取り付け角度の検出値を算出し、前記取り付け角度の検出値を平均化することにより、前記取り付け角度を検出することを特徴とする請求項１に記載の取り付け角度検出装置。
前記移動体のＸ軸回りの傾斜角を算出するＸ軸傾斜角算出手段を備え、
前記取り付け角度検出手段は、前記Ｘ軸傾斜角算出手段に算出された前記傾斜角に基づいて、前記Ｚ軸回転角変化量および前記Ｙ軸回転角変化量を補正して、前記取り付け角度を検出することを特徴とする請求項１または請求項２に記載の取り付け角度検出装置。
前記Ｚ軸回転角変化量取得手段は、Ｚ軸回りの回転角速度が所定の角速度以上となった位置を、前記検出開始位置とすることを特徴とする請求項１から請求項３のいずれか１項に記載の取り付け角度検出装置。
前記Ｚ軸回転角変化量取得手段は、Ｚ軸回りの回転角速度が所定の角速度未満となった位置を、前記検出終了位置とすることを特徴とする請求項１から請求項４のいずれか１項に記載の取り付け角度検出装置。
前記取り付け角度検出手段は、前記Ｚ軸回転角変化量取得手段により取得されたＺ軸回転角変化量が、所定の回転角変化量以上である場合に、前記取り付け角度を検出することを特徴とする請求項１から請求項５のいずれか１項に記載の取り付け角度検出装置。
前記取り付け角度検出手段に検出された前記取り付け角度に基づいて、前記検出装置に検出された検出値を補正する補正手段を備えたことを特徴とする請求項１から請求項６のいずれか１項に記載の取り付け角度検出装置。
前記Ｚ軸回転角変化量取得手段は、
前記検出範囲におけるＺ軸回転角速度を複数検出するＺ軸回転角速度検出手段と、
前記Ｚ軸回転角速度検出手段により検出されたＺ軸回転角速度に基づいて、前記検出範囲における前記移動体のＺ軸回転角変化量を算出するＺ軸回転角変化量算出手段と、
を備えたことを特徴とする請求項１から請求項７のいずれか１項に記載の取り付け角度検出装置。
前記Ｚ軸回転角変化量取得手段は、
前記検出範囲において前記移動体の位置情報を複数取得する位置情報取得手段と、
前記位置情報取得手段により取得された位置情報の推移から、前記検出範囲における前記移動体のＺ軸回転角変化量を算出するＺ軸回転角変化量算出手段と、
を備えたことを特徴とする請求項１から請求項７のいずれか１項に記載の取り付け角度検出装置。
前記検出装置が、前記移動体の進行方向に直交するピッチ軸回りのピッチ角速度を検出するピッチジャイロであって、
前記取り付け角度検出手段は、前記ピッチジャイロの前記移動体に対するロール軸回りの取り付け角度を検出することを特徴とする請求項１から請求項９のいずれか１項に記載の取り付け角度検出装置。
前記検出装置が、前記移動体の進行方向に平行なロール軸回りのロール角速度を検出するロールジャイロであって、
前記取り付け角度検出手段は、前記ロールジャイロの前記移動体に対するピッチ軸回りの取り付け角度を検出することを特徴とする請求項１から請求項９のいずれか１項に記載の取り付け角度検出装置。
ＸＹＺ座標系におけるＺ軸方向の変動を伴うＸＹ平面を移動する移動体に取り付けられた検出装置の取り付け角度を検出する取り付け角度検出方法において、
所定の検出開始位置から所定の検出終了位置までの検出範囲における前記移動体のＺ軸回りのＺ軸回転角変化量を取得するＺ軸回転角変化量取得ステップと、
前記検出装置に検出された検出結果に基づいて、前記検出範囲におけるＹ軸回りのＹ軸回転角変化量を算出するＹ軸回転角変化量算出ステップと、
前記Ｚ軸回転角変化量取得ステップで取得したＺ軸回転角変化量と、前記Ｙ軸回転角変化量算出ステップで算出したＹ軸回転角変化量と、の比に基づいて、前記検出装置の前記移動体に対するＸ軸回りの取り付け角度を検出する取り付け角度検出ステップと、
を備えたことを特徴とする取り付け角度検出方法。
ＸＹＺ座標系におけるＺ軸方向の変動を伴うＸＹ平面を移動する移動体に取り付けられた検出装置の取り付け角度を検出する取り付け角度検出プログラムにおいて、
所定の検出開始位置から所定の検出終了位置までの検出範囲における前記移動体のＺ軸回りのＺ軸回転角変化量を取得するＺ軸回転角変化量取得ステップと、
前記検出装置に検出された検出結果に基づいて、前記検出範囲におけるＹ軸回りのＹ軸回転角変化量を算出するＹ軸回転角変化量算出ステップと、
前記Ｚ軸回転角変化量取得ステップで取得したＺ軸回転角変化量と、前記Ｙ軸回転角変化量算出ステップで算出したＹ軸回転角変化量と、の比に基づいて、前記検出装置の前記移動体に対するＸ軸回りの取り付け角度を検出する取り付け角度検出ステップと、
をコンピュータに実行させることを特徴とする取り付け角度検出プログラム。
請求項１３に記載の取り付け角度検出プログラムを記録したことを特徴とする記録媒体。