取付け角度算出装置
【課題】移動体が水平面を走行している状態もしくは車両が水平面に停車している状態のみではない実走行状態において、高度変化量の算出等の機能を有する他の高価なセンサーを用いることなく電子機器の筐体の取付け角度を精度良く算出することのできる取付け角度算出装置をを提供する。
【解決手段】取付け角度算出装置１は、移動体の角速度を検出する１軸の角速度センサ１０と、車両の走行速度に応じてパルス周期が変化する車速パルスを発生する速度センサ１１と、移動体の加速度を検出する加速度センサ１２と、移動体の走行状況を判定するカーブ・直進判定手段１４とを備え、取付け角度演算手段１３は、移動体が直進しているとき取付け角度を算出し、カーブ走行時に算出した値が正確かどうか検証する。
【解決手段】取付け角度算出装置１は、移動体の角速度を検出する１軸の角速度センサ１０と、車両の走行速度に応じてパルス周期が変化する車速パルスを発生する速度センサ１１と、移動体の加速度を検出する加速度センサ１２と、移動体の走行状況を判定するカーブ・直進判定手段１４とを備え、取付け角度演算手段１３は、移動体が直進しているとき取付け角度を算出し、カーブ走行時に算出した値が正確かどうか検証する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体に搭載される電子機器の筐体の取付け角度を算出する取付け角度算出装置に関するものである。
【背景技術】
【０００２】
近年、移動体に搭載される電気機器の一例としての車両に搭載されるナビゲーションシステムは、現在位置を算出するものとして、GPS（Global Positioning System）受信機、加速度センサ及び角速度センサを含む自立センサ、及び速度センサにより構成されているのが一般的である。
【０００３】
このようなナビゲーションシステムにおいて、筐体は車両に対して所定の方向に正しく取付けることを前提に基本的な設計がなされているが、ユーザーに対してその必要性が十分に周知されておらず、また、近年のナビゲーションシステムのコンパクト化や軽量化に伴い、取付け位置の自由度が増すことで車両に対して筐体が大きく傾けられた状態で取付けられることがある。このような状態で取付けられたナビゲーションシステムでは、自立センサ（加速度センサや角速度センサ）の出力値が、その傾き角度（取付け角度）に応じて減少し、その結果、誤差を含む自車位置を算出してしまうことがあった。それらを解決するため、ナビゲーションシステムの筐体の取付け角度を算出して自立センサの各出力値を補正する装置が提案されている。
【０００４】
従来の例では、図１２に示すように車両の加速度を検出する加速度センサ１２と、車両の角速度を検出する角速度１０と、GPS受信機２１を備えており、車両の進行方向に加速度が発生しない条件（停車中および一定速度で走行時）での加速度センサ１２の出力値の平均値から取付け角度を取付け角度演算手段１３にて算出する。取付け角度演算手段１３による演算は、以下のように行う。進行方向に加速度が発生しないので、図１３に示すように、加速度センサが検出するのは車両の鉛直方向軸の重力加速度ｇのみとなる。すなわち、加速度センサの出力値の平均値をＡavg、取付け角度をθpitch、とすると、Ａavg＝ｇ×ｓｉｎ（θpitch）として表すことができ、Ａavgをｇで除算することで、取付け角度であるｓｉｎ（θpitch）を算出することができる。（例えば特許文献１参照）。
【特許文献１】特開２００４−２３９６１３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来の取付け角度算出装置は、車両が水平面を走行している状態もしくは車両が水平面に停車している状態であると仮定し、取り付け角度の算出を行う。そのため、車両が水平面を走行していないとき、路面の傾きが加速度センサの出力値に影響を及ぼし、ナビゲーションシステムの筐体の取付け角度のみを精度良く検出することができない。結果、誤差を含んだ自車位置を算出する等の問題があった。
【０００６】
また、従来は車両が水平面を走行している状態または、水平面に停車している状態を判定するには、例えば高度センサやGPS受信機により高度の変化量を算出する等の必要があり、それら外部装置の精度に判定結果が依存することや製造コスト、設置スペースの観点から車両が水平面を走行しているかどうかを判定することは難しかった。
【０００７】
本発明は、従来の問題を解決するためになされたもので、移動体が水平面を走行している状態もしくは車両が水平面に停車している状態のみではない実走行状態において、高度変化量の算出等の機能を有する他の高価なセンサーを用いることなく電子機器の筐体の取付け角度を精度良く算出することのできる取付け角度算出装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
前記従来の課題を解決するために、本発明の第１の局面は、移動体が直進しているかカーブを走行しているかを判定するカーブ直進判定手段を有し、カーブ直進判定手段により移動体が直進していると判定しているときに取付け角度候補を算出し、カーブ直進判定手段により移動体がカーブを走行していると判定しているときの加速度センサおよび／または角速度センサの出力値から前記取付け角度を確定する。
【０００９】
本構成によって、移動体が直進しているときに算出した取付け角度候補が水平面を走行していた時に算出された値かそうでないかが判定できる。
【００１０】
また、本発明の第２の局面は、カーブ直進判定手段により移動体が直進していると判定しているときに取付け角度候補演算手段により算出された取付け角度候補から移動体のヨー方向の角速度を算出し、カーブ直進判定手段によりカーブを走行していると判定している間、角速度を時間積算することにより得られる所定のカーブの角度と、カーブ情報記憶手段に記憶されているカーブの角度情報またはＧＰＳ方位から算出した角度情報が一致するとき取付け角度候補を筐体の取付け角度としたものである。
【００１１】
また、本発明の第３の局面は、算出された取付け角度を正規分布となるまで蓄積し、前記正規分布の平均値または代表値を筐体の取付け角度としたものである。
【００１２】
また、本発明の第４の局面は、筐体の取り付け角度を決定した時の移動体の加速度センサによる進行方向の加速度と、速度センサの出力値を時間微分して得られる加速度もしくは、GPS受信機から算出した加速度が一致しない場合、筐体の取付け角度を再計算することとしたものである。
【００１３】
また、本発明の第５の局面は、筐体の取り付け角度を決定した時の角速度センサによる移動体の水平面に対して直交する方向の角速度と、GPS受信機から算出した角速度が一致しない場合、筐体の取付け角度を再計算することとしたものである。
【００１４】
また、本発明の第６の局面は、算出した取付け角度を自立航法センサの補正に用いることとしたものである。
【発明の効果】
【００１５】
本発明の取付け角度算出装置によれば、移動体が水平面を走行または停車している状態、傾いた路面を走行または停車している状態等の様々な走行状況のある実際の走行状態においても、精度良く移動体に搭載された電子機器の筐体の取付け角度を算出することができる。また、高価なセンサを新たに追加する必要がないので、低コストであり、かつ筐体内に余分なスペースを必要としない。
【発明を実施するための最良の形態】
【００１６】
以下、本発明の実施の形態１、２、３による取付け角度算出装置について、図面を参照しながら具体的に説明する。本実施の形態の取付け角度算出装置としては、例えば自動車などの移動体に設置されるナビゲーション装置に適用した場合を例に挙げる。
（実施形態１）
本発明の実施の形態の取付け角度算出装置１の構成を図１に示す。図１において、取付け角度算出装置１は、移動体に搭載される電子機器の一例としてのナビゲーションシステムに実装されており、ナビゲーションシステムの筐体（以下、単に筐体と称する）が取付けられた角度を算出する。このような取付け角度の算出のために、取付け角度算出装置１は、微分器９と、角速度センサ１０と、速度センサ１１と、加速度センサ１２と、取付け角度演算手段１３と、カーブ・直進判定手段１４とを備える。
【００１７】
加速度センサ１２は、予め定められた座標系における移動体の加速度を検出する。このような加速度センサ１２が移動体に対して正しい姿勢で設置された場合、加速度センサ１２によって、移動体の進行方向への加速度と、移動体の右側方向で移動体の進行方向に水平面内で直交する方向への加速度と、移動体の水平面に対して下方向に直交する方向への加速度とが検出される。以下、図３（Ａ）、（Ｂ）に示すように、移動体の進行方向をXB軸方向と称し、移動体の右側方向で移動体の進行方向に水平面内で直交する方向をYB軸方向と称し、移動体の水平面に対して下方向に直交する方向をZB軸方向と称する。以下、これらXB軸、YB軸及びZB軸からなる座標系を車両座標系と称する（下添え字は車両座標系（Body frame）を表す）。なお、本実施形態の取付け角度算出装置１は、少なくとも２軸の加速度センサ１２を備えるとして説明する。２軸の加速度センサを備える場合、その軸の組み合わせはどの組み合わせでもよい。
【００１８】
角速度センサ１０は、予め定められた座標系における移動体の角速度を検出する。このような角速度センサ１０が移動体に対して正しい姿勢で設置されている場合、角速度センサ１０によって、車両座標系におけるそれぞれの軸回りの移動体が回転する際の角速度が検出される。車両座標系におけるXB軸回りの角速度をロールレートと称し、その角速度を時間積分して求められる角度をロール角と称し、車両座標系におけるYB軸回りの角速度をピッチレートと称し、その角速度を時間積分して求められる角度をピッチ角と称し、車両座標系におけるZB軸回りの角速度をヨーレートと称し、その角速度を時間積分して求められる角度をヨー角と称する。なお、本実施形態の取付け角度算出装置１は、上述のようなZB軸回りの角速度を検出する１軸の角速度センサ１０を備えるとして説明する。
【００１９】
速度センサ１１は、移動体の移動速度を検出する。移動体が車両の場合には、速度センサ１１は好ましくは、移動体の一例としての車両に元々設置されており、車両の走行速度に応じてパルス周期が変化する車速パルスを発生する車速パルス発生装置である。なお、本実施形態の取付け角度算出装置１は、上述のような移動速度を微分器９で時間微分して求められる加速度を使用する。
【００２０】
取付け角度演算手段１３は、典型的には、CPU、ROM及びRAMを含んでおり、角速度センサ１０、速度センサ１１、加速度センサ１２と電気的に接続される。CPUは、ROMに予め格納されるコンピュータプログラムを、RAMを使って実行する。コンピュータの実行中、取付け角度演算手段１３は、角速度センサ１０から得られる角速度、速度センサ１１から得られる移動速度を微分器９で時間微分して得られる加速度、及び加速度センサ１２から得られる各加速度を少なくとも用いて、筐体の取付け角度を算出する。
【００２１】
カーブ・直進判定手段１４は、例えばRAMの記憶領域の一部で構成されており、角速度センサ１０で検出された角速度の１秒間積算した値（移動体が１秒間に回転した角度）が所定の値、例えば0.2deg未満か以上かで移動体の走行状況を判定する。判定の方法は、これに限らず加速度センサの出力値を用いてもよい。
【００２２】
次に、取付け角度演算手段１３の詳細を示した図４を用いて詳しく説明する。取付け角度の候補演算手段１６は、移動体が直進走行している場合に移動体が水平面を走行していると仮定して、加速度センサ１２から得られる加速度を用いて筐体の取付け角度の候補を算出する。車両の走行角度算出手段１５は、移動体がカーブを走行している場合に、取付け角度の候補演算手段１６により算出した取付け角度の候補と角速度センサ１０から得られる角速度を用いて移動体が走行したカーブの角度を算出する。また、地図の角度情報１８は例えばHDDもしくはDVD、CD-ROMなどの記憶媒体に予め格納されているのが一般的である。
【００２３】
取付け角度の決定演算手段１７による筐体の取付け角度の決定は、以下のように行う。角速度センサ１０から得られる角速度を用いて計算し得られるカーブの角度と地図の角度情報１８またはGPS方位から算出した角度を比較し一致するとき、取付け角度の候補を求めたときに移動体が水平面を走行していたと確認できるので、移動体が水平面を走行している状態で取付け角度候補を算出したことになる。したがって、その取付け角度候補を筐体の取付け角度として決定する。さらには、移動体が水平面を走行していた時に算出されたと確かめられた取付け角度の候補を十分なデータ量が得られるまで蓄積し、正規分布等の統計的手法により筐体の取付け角度を決定してもよい。
【００２４】
次に、以上のような構成の取付け角度算出装置１の動作の流れについて図２を用いて説明する。具体的な算出方法については、後ほど図３（Ａ）、（Ｂ）を用いて説明する。
【００２５】
まず、カーブ・直進判定手段１４により移動体がカーブまたは直進していることを判定する（S００１）。移動体が直進していると判断した場合に加速度センサ１２から検出される加速度より、筐体が取付けられた角度の候補を算出する（S００２）。そして、移動体がカーブを走行していると判断した場合にＳ００２で求めた筐体の取付け角度を用いて車両座標系における移動体のZB軸回りの角速度を算出する（S００３）。その角速度をカーブが終了する（Ｓ００５）までの間、時間積分することで求まる移動体が走行したカーブの角度（Ｓ００４）と地図に格納された角度情報がある一定の範囲、例えば±５degの範囲で一致した時（Ｓ００６）の取付け角度の候補を蓄積する（Ｓ００７）。図２のフローチャートでは、統計的性質から筐体の取付け角度を決定する流れを示しているが、Ｓ００６において地図の角度情報と走行角度が一致したときの取付け角度候補を取付け角度として採用してもよい。また、地図に格納された角度情報の替わりにGPS方位を用いて算出したカーブの角度を使うことも可能である。
【００２６】
次に、ある程度のデータ量が蓄積された時点（Ｓ００８）でデータの正規性の検定を行い、その結果、データが正規分布である場合（Ｓ００９）に筐体の取付け角度を決定する（Ｓ０１０）。
【００２７】
次に、本実施の形態の取付け角度算出装置１の具体的な算出方法の詳細を図３（Ａ）、（Ｂ）を用いて説明する。
【００２８】
図３（Ａ）、（Ｂ）に示すように車両の進行方向をXB軸、車両の右側をYB軸、XB-YB軸平面に対して垂直下向きをZB軸とし、これらXB軸、YB軸、ZB軸からなる座標系は、前述したように車両座標系とする（下添え字は車両座標系（Body frame）を表す）。それぞれ筐体がY軸回りの回転θ及びZ軸回りの回転φで傾いている状態で取付けられた場合の座標軸の変化を示している。また、（XS, YS, ZS）（下添え字はセンサ座標系（Sensor frame）を表す。）は筐体に取付けられた加速度センサもしくは角速度センサのセンサ座標系であり、筐体が車両に対して正しい姿勢で設置された場合、車両座標系とセンサ座標系のそれぞれの軸は一致する。
【００２９】
ここで、Y軸を角度θ回転させる回転行列をRY(θ)、Z軸を角度φ回転させる回転行列をRZ(φ)とすると、2軸回転後の座標系（Xαβ, Yαβ, Zαβ）と基準となる座標系（X,Y, Z）には下式（１）の関係が成り立つ。
【００３０】
【数１】
【００３１】
したがって、筐体の取付け角度としてYB軸回りの角度であるピッチ角と、ZB軸回りの角度であるヨー角の２軸の回転がある場合に車両座標系で検出された加速度（AB,X, AB,Y, AB,Z）とセンサの座標系で検出された加速度（AS,X, AS,Y, AS,Z）には下式（２）、（３）、（４）の関係式が成り立つ。なお、AB,Xは車両座標系における進行方向への加速度を意味する（下添え字は車両座標系におけるXB軸方向の意味を表す）。また、AS,Xはセンサ座標系におけるXS軸方向への加速度を意味する。（下添え字はセンサ座標系におけるXS軸方向の意味を表す。）
【００３２】
【数２】
【００３３】
また、角速度センサ１０で検出される角速度についても同様に車両座標系で検出された角速度（WB,X, WB,Y, WB,Z）とセンサ座標系で検出された角速度センサ１０で検出された角速度（WS,X , WS,Y, WS,Z）には下式（５）、（６）、（７）の関係式が成り立つ。
【００３４】
【数３】
【００３５】
以上の関係を用いて、角速度センサ１０の出力値から車両が直進している場合の加速度と車両がカーブを走行している場合の角速度を用いて筐体が取付けられている角度を算出する方法を図４に示す取付け角度演算手段１３の構成で説明する。まず、取付け角度の候補演算手段１６において角速度センサ１０の出力値からカーブ・直進判定手段１４より車両の直進走行を判断する。車両が直進走行している場合に、車両が水平面上を加速度Aで走行しているとすると、車両座標系における加速度のXB軸方向成分はAであり、YB軸方向成分は0であり、ZB軸方向成分はG（重力加速度のみ）であるから（AB,X, AB,Y, AB,Z）=（A, 0, G）となる。AB,Xは速度センサの時間微分から求められる。したがって、（３）式において加速度センサの出力値からAS,Yは検出できるので筐体の取付け角度の候補φ’を求めることができる。また、φ’を（２）式もしくは（４）式に代入することで筐体の取付け角度θ’を求めることができる。これは上述したように車両が水平面を走行していることを前提として算出する。
【００３６】
また、車両が水平面を走行している場合に、角速度センサの出力値から車両がカーブを走行していると判断した場合、角速度は車両座標系におけるZB軸回りのみと考えられる。すなわち、車両座標系で検出される角速度は（0, 0, WB,Z）となる。
【００３７】
したがって、（５）、（６）、（７）式は下式（８）、（９）、（１０）のように簡単にできる。
【００３８】
【数４】
【００３９】
以上より、車両の走行角度算出手段１５において取付け角度の候補演算手段１６で算出した筐体の取付け角度の候補であるθ’を（１０）式に代入することでWB,Zを算出し、WB,Zをカーブ終了時まで時間積分することで車両が走行したカーブの角度γを下式（１１）により求めることができる。
【００４０】
【数５】
【００４１】
次に、取付け角度の決定演算手段１７にて行う処理について説明する。水平面を車両が走行していることを前提として導いた（１１）式で求められた角度γと地図に格納されたカーブの角度（地図の角度情報１８）またはGPS方位より算出したカーブの角度が一致するとき直進走行時、カーブ走行時に車両が水平面を走行していたことが検証できる。したがって、求められた取付け角度候補θ’は正確であるといえるのでθ’を取付け角度として決定することができる。しかしながら、実際には地図に格納されたカーブの角度が全て実際の道路形状と同じではない。したがって、精度を上げるには、例えば±２deg程度で一致したときの取付け角度θ’のデータを蓄積し、このθ’が正規分布に従うとき、例えばその平均値を取付け角度として算出する。取付け角度のヨー角φについてはθ’と対応するときの値を蓄積させ、θ’について正規性が成り立った時に例えば平均値を求めればよい。
【００４２】
このような本発明の第１の実施の形態の取付け角度算出装置１によれば、移動体が直進している場合に加速度センサ１２で検出される値から筐体が取付けられている角度の候補を算出する。そして、移動体がカーブを走行している場合にその取付け角度の候補から求めた車両座標系におけるZ軸回りの角速度を、カーブ終了時まで時間積分することで求まるカーブの角度と、地図に格納された角度情報が一致するか比較する。一致した時の取付け角度の候補を筐体の取付け角度として決定するか、その候補の十分な量の蓄積後に統計的処理により取付け角度を算出する。したがって、車両が水平面を走行していなくても筐体が取付けられている角度を算出することができる。
（実施形態２）
第１の実施形態では、地図に格納されているカーブの角度情報を用いて筐体の取付け角度を算出する際に地図に格納されたカーブの角度若しくは、GPS方位から算出した角度に誤差があるので、より精度を向上させるために、一定の範囲で一致するものを抽出し、取付け角度を統計的処理により求める構成とした。
【００４３】
第２の実施形態では、より筺体の取付け角度を正確にもとめることが可能な取付け角度算出装置を図５のブロック図を参照して説明する。
【００４４】
図５に示す取付け角度算出装置１は、図１に示す取付け角度算出装置１と比較すると、加速度を用いた取付け角度信頼性判定手段２２、車両の進行方向加速度演算手段２３及びGPS受信機２１とをさらに備える。それ以外に両取付け角度算出装置１には相違点は無いので、図５において、図１に示す構成に相当するものには同一の参照符号を付け、それぞれの説明は省略する。ただし、本実施形態の取付け角度算出装置１は、筐体の取付け角度算出の信頼度を高めるために３軸の加速度センサ１２を備えるとして説明する。
【００４５】
GPS受信機２１は、GPS衛星からの電波を受信して地球上における車両の絶対位置（緯度・経度・高度）を算出する。
【００４６】
車両の進行方向加速度演算手段２３は、加速度センサ１２から得られる各加速度及び取付け角度演算手段１３から得られる筐体の取付け角度を少なくとも用いて車両の進行方向の加速度を算出する。加速度を用いた取付け角度信頼性判定手段２２は、求められた車両の進行方向の加速度及び速度センサ１１から得られる速度を時間微分することで算出する加速度もしくはGPS受信機から得られる速度を時間微分することで算出する参照加速度を少なくとも用いて取付け角度演算手段１３より得られる筐体の取付け角度の信頼度を確認する。
【００４７】
次に、以上のような構成の取付け角度算出装置１の動作の流れについて図７を用いて説明する。
【００４８】
取付け角度演算手段１３により、算出された筐体の取付け角度から筐体の取付け角度を決定（Ｓ１０１）した時（統計的処理により取付け角度を求めた場合は、決定した取付け角度と同じ値の取付け角度候補を算出した時）の車両の進行方向の加速度を加速度センサ１２より算出（S１０２）する。本実施例では、筐体の取付け角度の算出は、実施の形態１により求めた値を使用するとして説明するが、他の方法により求めた値を使用しても構わない。また、同時に速度センサ１１で得られる速度を時間微分することにより求まる加速度もしくはGPS受信機２１から得られる速度を時間微分することにより求まる参照加速度を算出する。
【００４９】
次に、筐体の取付け角度を用いて加速度センサ１２から算出した加速度と速度センサ１１もしくはGPS受信機２１から算出した加速度と比較する（S１０３）。速度センサ１１から算出される加速度の誤差は、速度センサ１１として例えば車速パルスを用いる場合、１パルス分であり、GPSから算出される加速度の誤差はGPS速度誤差分（GPS速度によるが１σ=３km/h）であり精度が高い。したがって、加速度の差がその誤差範囲外であればＳ１０１で決定した筐体の取付け角度は、信頼度が低いことになるので、取付け角度演算手段１３を用いて筐体の取付け角度を再度計算し直す（S１０５）。これを繰り返すことで算出する筐体の取付け角度の信頼性を高めることができる。また、誤差範囲を決定するのに、速度センサ１１とＧＰＳ受信機２１の両方の値を用いてもよい
次に、本実施形態の取付け角度算出装置１の具体的な算出方法の詳細を図５及び数式を用いて説明する。尚、座標系は実施形態１で説明した図３と同じであり、符号も同じものを使用する。
【００５０】
取付け角度演算手段１３により筐体の取付け角度を決定した値を算出した時（または、決定値と同じ値の取付け角度候補を算出した時）は車両が水平面を走行していると仮定して、加速度センサの値を用いて車両の進行方向の加速度計算を行う方法を説明する。まず、水平面に対するセンサ座標系の傾きは筐体が取付けられた角度のピッチ角θとヨー角φのみと考えることができる。実施の形態１の説明で用いた（３）式について（RY(θ)RZ(φ)）は直交行列となっており、その逆行列と転置行列は等しいことから下式（１２）となる。
【００５１】
【数６】
【００５２】
なお、上添え字の−１は逆行列を表し、Tは転置行列を表す。したがって、下式（１３）の関係が成り立つ。
【００５３】
【数７】
【００５４】
取付け角度演算手段１３より求めた筐体が取付けられた角度のピッチ角θとヨー角φを（１３）式に代入すると車両の進行方向の加速度が求まる。すなわち、車両座標系のXB軸方向の加速度は下式（１４）となる。
【００５５】
【数８】
【００５６】
したがって、車両の進行方向加速度演算手段２３では、（１４）式を用いて車両座標系のXB軸方向の加速度を算出する。そして、加速度を用いた取付け角度信頼性判定手段２２では、加速度センサ１２を用いて求められた加速度と速度センサ１１から得られる速度を時間微分することより算出した加速度もしくはGPS受信機２１から得られた速度を時間微分することにより算出した参照加速度の差がある一定の範囲例えば速度センサ１１として例えば車速パルスを用いる場合、１パルス分、GPSから算出される加速度の場合、GPS速度誤差分（GPS速度によるが１σ=３km/h）の範囲内であれば算出した筐体の取付け角度の信頼性は高く、その範囲外であれば再び取付け角度演算手段１３で筐体の取付け角度を求めることで第１の実施形態よりも筐体の取付け角度の精度を高めることが可能となる。
（実施形態３）
本実施の形態の構成は、筺体の取付け角度の決定後、筺体の取付け角度の精度を高めるため角速度を用いて信頼性判定を行う。
【００５７】
以下、図６のブロック図を参照して、筐体の取付け角度の決定後、筐体の取付け角度の信頼度を確認する取付け角度算出装置１を説明する。取付け角度算出装置１は、図５に示す取付け角度算出装置１と比較すると、図５に示す車両の加速度を用いた取付け角度信頼性判定手段２２及び車両の進行方向加速度演算手段２３に代えて、角速度を用いた取付け角度信頼性判定手段３１及び車両の鉛直方向軸角速度演算手段３２とを備える。それ以外に両取付け角度算出装置１には相違点は無いので、図６において、図１及び図５に示す構成に相当するものには同一の参照符号を付け、それぞれの説明を省略する。ただし、本実施の形態の取付け角度算出装置１は、さらに筐体の取付け角度の精度を高めるためにXB軸回りの角速度とZB軸回りの角速度を検出する２軸の角速度センサ１０を備えるとして説明する。
【００５８】
車両の鉛直方向軸角速度演算手段３２は、角速度センサ１０から得られる各角速度及び取付け角度演算手段１３から得られる筐体の取付け角度を少なくとも用いて車両座標系におけるZB軸回りの角速度を算出する。角速度を用いた取付け角度信頼性判定手段３１は、求められた車両座標系におけるZB軸回りの角速度及びGPS受信機から得られる方位変化量を時間微分することで算出する参照角速度を少なくとも用いて取付け角度演算手段１３より得られる筐体の取付け角度の信頼度を確認する。
【００５９】
次に、以上のような構成の取付け角度算出装置１の動作の流れについて図８を用いて説明する。
【００６０】
これは、実施形態２の取付け角度算出装置１と同様に取付け角度演算手段１３により算出された筐体の取付け角度を決定した時（統計的処理により取付け角度を求めた場合は、決定した取付け角度と同じ値の取付け角度候補を算出した時）の車両のZB軸方向の角速度を算出（S２０２）する。また、同時にGPS受信機２１から得られる方位変化量を時間微分することで角速度を算出する。筐体の取付け角度を用いて角速度センサ１０から算出した角速度とGPS受信機２１から算出した角速度と比較する（S２０３）。GPSから算出される角速度の誤差はGPS方位誤差分であるため、角速度の差がその範囲外であれば取付け角度演算手段１３を用いて筐体の取付け角度を再度計算し直す（S２０５）。これを繰り返すことで算出する筐体の取付け角度の精度を高めることができる。
【００６１】
次に、本実施形態の取付け角度算出装置１の具体的な処理の詳細を図６と数式を用いて説明する。
【００６２】
（１３）式から筐体の取付け角度のピッチ角θ、ヨー角φを代入することで車両座標系におけるZＢ軸方向の角速度は次式より求まる。
【００６３】
【数９】
【００６４】
したがって、車両の鉛直方向軸角速度演算手段３２では、（１５）式で求まる車両座標系のZB軸回りの角速度を算出する。そして、角速度を用いた取付け角度信頼性判定手段２２では、角速度センサを用いて求められた角速度とGPS受信機から得られる方位変化量を時間微分することより算出した参照角速度差がある一定の範囲例えばGPS方位誤差内であれば算出した筐体の取付け角度の信頼性は高く、その範囲外であれば再び取付け角度演算手段で筐体の取付け角度を求める。
【００６５】
さらに、本発明の取付け角度算出装置１を備えた自立航法センサ補正装置４について、図９を用いて説明する。図９において、自立航法センサ補正装置４は、前述の実施の形態１の取付け角度算出装置１を組み込んでおり、筐体の取付け角度からセンサ座標系で得られた加速度、角速度を車両座標系の出力に補正する。このような自立航法センサの補正のために、自立航法センサ補正装置４は、取付け角度算出装置１と、自立センサ補正手段１９とを備える。
【００６６】
次に、自立センサ補正手段１９の詳細を図１０を用いて説明する。自立センサ補正手段１９は、角速度センサ１０から得られる角速度、加速度センサ１２から得られる各加速度、及び取付け角度演算手段１３より算出した筐体の取付け角度を少なくとも用いて自立航法センサを補正する。
【００６７】
方位角算出手段５は、角速度センサ１０から得られるセンサ座標系におけるZS軸回りの角速度を取付け角度演算手段１３で算出した筐体の取付け角度により、車両座標系におけるXB軸回りの角速度に補正することで車両の方位角を算出する。
【００６８】
速度算出手段６は、同様に加速度センサ１２から得られるセンサ座標系におけるXS軸方向への加速度を取付け角度演算手段１３で算出した筐体の取付け角度より、車両座標系におけるXB軸方向への加速度に補正することで車両の進行方向への速度を算出する。
【００６９】
次に、以上のような構成の自立航法センサ補正装置４の動作の流れについて図１１を用いて説明する。
【００７０】
取付け角度算出装置１で筐体の取付けられている角度を算出（Ｓ３０１）し、求めた筐体の取付け角度から加速度センサ１２、角速度センサ１０から得られたセンサ座標系におけるXS軸方向への加速度、ZS軸回りの角速度を車両座標系におけるXB軸方向への加速度、ZB軸回りの角速度を算出する（Ｓ３０２）。このように補正された出力値を時間積分することで車両の速度や方位角を求めることできる（Ｓ３０３）。また、正確な自車位置を算出することができる。
【００７１】
次に、本実施形態の自立航法センサ補正装置４の具体的な処理の詳細を図５、図１０を用いて説明する。
【００７２】
速度算出手段６では、加速度センサ１２から得られるセンサ座標系におけるXS軸方向への加速度AS,Xを取付け角度の算出装置１で算出した筐体の取付け角度を用いて（１５）式から車両座標系におけるXB軸方向への加速度AB,Xに補正する。そして、補正された加速度AB,Xを時間積分することで車両の進行方向への速度を算出する。方位角算出手段５では、角速度センサ１０から得られるセンサ座標系におけるZS軸回りの角速度WS,Zを取付け角度の算出装置１で算出した筐体の取付け角度を用いて（１６）式からZB軸回りの角速度WB,Zに補正する。そして、補正された角速度WB,Zを時間積分することで車両の方位角を算出する。
【産業上の利用可能性】
【００７３】
本発明にかかる取付け角度算出装置は、様々な走行状況が存在する実際の走行状態においても、精度良く移動体に搭載された電子機器の筐体の取付け角度を算出することが可能になるので、移動体に搭載される電子機器の筐体の取付け角度を算出する取付け角度算出装置等として有用である。
【図面の簡単な説明】
【００７４】
【図１】本発明の第１の実施の形態における取付け角度算出装置の構成図
【図２】本発明の第１の実施の形態における取付け角度算出装置の動作説明のためのフロー図
【図３】（Ａ）本発明の座標系でのピッチ方向の取付け角度を説明するための図（Ｂ）本発明の座標系でのヨー方向の取付け角度を説明するための図
【図４】本発明の第１の実施の形態における取付け角度演算手段の構成図
【図５】本発明の第２の実施の形態における取付け角度算出装置の構成図
【図６】本発明の第３の実施の形態における取付け角度算出装置の構成図
【図７】本発明の第２の実施の形態における取付け角度算出装置の動作説明のためのフロー図
【図８】本発明の第３の実施の形態における取付け角度算出装置の動作説明のためのフロー図
【図９】本発明の取付け角度算出装置を備えた自立航法センサ補正装置の構成図
【図１０】本発明の取付け角度算出装置を自立航法センサ補正装置に適用する構成図
【図１１】本発明の取付け角度算出装置を備えた自立航法センサ補正装置の動作説明のためのフロー図
【図１２】従来の取付け角度算出装置の構成図
【図１３】従来の取付け角度算出装置における算出方法を説明するための図
【符号の説明】
【００７５】
１ 取付け角度算出装置
４ 自立航法センサ補正装置
５ 方位角算出手段
６ 速度算出手段
９ 微分器
１０ 角速度センサ
１１ 速度センサ
１２ 加速度センサ
１３ 取付け角度演算手段
１４ カーブ・直進判定手段
１５ 車両の走行角度算出手段
１６ 取付け角度の候補演算手段
１７ 取付け角度の決定演算手段
１８ 地図の角度情報
１９ 自立センサ補正手段
２１ ＧＰＳ受信機
２２ 加速度を用いた取付け角度信頼性判定手段
２３ 車両の進行方向加速度演算手段
３１ 角速度を用いた取付け角度信頼性判定手段
３２ 車両の鉛直方向軸角速度演算手段
【技術分野】
【０００１】
本発明は、移動体に搭載される電子機器の筐体の取付け角度を算出する取付け角度算出装置に関するものである。
【背景技術】
【０００２】
近年、移動体に搭載される電気機器の一例としての車両に搭載されるナビゲーションシステムは、現在位置を算出するものとして、GPS（Global Positioning System）受信機、加速度センサ及び角速度センサを含む自立センサ、及び速度センサにより構成されているのが一般的である。
【０００３】
このようなナビゲーションシステムにおいて、筐体は車両に対して所定の方向に正しく取付けることを前提に基本的な設計がなされているが、ユーザーに対してその必要性が十分に周知されておらず、また、近年のナビゲーションシステムのコンパクト化や軽量化に伴い、取付け位置の自由度が増すことで車両に対して筐体が大きく傾けられた状態で取付けられることがある。このような状態で取付けられたナビゲーションシステムでは、自立センサ（加速度センサや角速度センサ）の出力値が、その傾き角度（取付け角度）に応じて減少し、その結果、誤差を含む自車位置を算出してしまうことがあった。それらを解決するため、ナビゲーションシステムの筐体の取付け角度を算出して自立センサの各出力値を補正する装置が提案されている。
【０００４】
従来の例では、図１２に示すように車両の加速度を検出する加速度センサ１２と、車両の角速度を検出する角速度１０と、GPS受信機２１を備えており、車両の進行方向に加速度が発生しない条件（停車中および一定速度で走行時）での加速度センサ１２の出力値の平均値から取付け角度を取付け角度演算手段１３にて算出する。取付け角度演算手段１３による演算は、以下のように行う。進行方向に加速度が発生しないので、図１３に示すように、加速度センサが検出するのは車両の鉛直方向軸の重力加速度ｇのみとなる。すなわち、加速度センサの出力値の平均値をＡavg、取付け角度をθpitch、とすると、Ａavg＝ｇ×ｓｉｎ（θpitch）として表すことができ、Ａavgをｇで除算することで、取付け角度であるｓｉｎ（θpitch）を算出することができる。（例えば特許文献１参照）。
【特許文献１】特開２００４−２３９６１３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来の取付け角度算出装置は、車両が水平面を走行している状態もしくは車両が水平面に停車している状態であると仮定し、取り付け角度の算出を行う。そのため、車両が水平面を走行していないとき、路面の傾きが加速度センサの出力値に影響を及ぼし、ナビゲーションシステムの筐体の取付け角度のみを精度良く検出することができない。結果、誤差を含んだ自車位置を算出する等の問題があった。
【０００６】
また、従来は車両が水平面を走行している状態または、水平面に停車している状態を判定するには、例えば高度センサやGPS受信機により高度の変化量を算出する等の必要があり、それら外部装置の精度に判定結果が依存することや製造コスト、設置スペースの観点から車両が水平面を走行しているかどうかを判定することは難しかった。
【０００７】
本発明は、従来の問題を解決するためになされたもので、移動体が水平面を走行している状態もしくは車両が水平面に停車している状態のみではない実走行状態において、高度変化量の算出等の機能を有する他の高価なセンサーを用いることなく電子機器の筐体の取付け角度を精度良く算出することのできる取付け角度算出装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
前記従来の課題を解決するために、本発明の第１の局面は、移動体が直進しているかカーブを走行しているかを判定するカーブ直進判定手段を有し、カーブ直進判定手段により移動体が直進していると判定しているときに取付け角度候補を算出し、カーブ直進判定手段により移動体がカーブを走行していると判定しているときの加速度センサおよび／または角速度センサの出力値から前記取付け角度を確定する。
【０００９】
本構成によって、移動体が直進しているときに算出した取付け角度候補が水平面を走行していた時に算出された値かそうでないかが判定できる。
【００１０】
また、本発明の第２の局面は、カーブ直進判定手段により移動体が直進していると判定しているときに取付け角度候補演算手段により算出された取付け角度候補から移動体のヨー方向の角速度を算出し、カーブ直進判定手段によりカーブを走行していると判定している間、角速度を時間積算することにより得られる所定のカーブの角度と、カーブ情報記憶手段に記憶されているカーブの角度情報またはＧＰＳ方位から算出した角度情報が一致するとき取付け角度候補を筐体の取付け角度としたものである。
【００１１】
また、本発明の第３の局面は、算出された取付け角度を正規分布となるまで蓄積し、前記正規分布の平均値または代表値を筐体の取付け角度としたものである。
【００１２】
また、本発明の第４の局面は、筐体の取り付け角度を決定した時の移動体の加速度センサによる進行方向の加速度と、速度センサの出力値を時間微分して得られる加速度もしくは、GPS受信機から算出した加速度が一致しない場合、筐体の取付け角度を再計算することとしたものである。
【００１３】
また、本発明の第５の局面は、筐体の取り付け角度を決定した時の角速度センサによる移動体の水平面に対して直交する方向の角速度と、GPS受信機から算出した角速度が一致しない場合、筐体の取付け角度を再計算することとしたものである。
【００１４】
また、本発明の第６の局面は、算出した取付け角度を自立航法センサの補正に用いることとしたものである。
【発明の効果】
【００１５】
本発明の取付け角度算出装置によれば、移動体が水平面を走行または停車している状態、傾いた路面を走行または停車している状態等の様々な走行状況のある実際の走行状態においても、精度良く移動体に搭載された電子機器の筐体の取付け角度を算出することができる。また、高価なセンサを新たに追加する必要がないので、低コストであり、かつ筐体内に余分なスペースを必要としない。
【発明を実施するための最良の形態】
【００１６】
以下、本発明の実施の形態１、２、３による取付け角度算出装置について、図面を参照しながら具体的に説明する。本実施の形態の取付け角度算出装置としては、例えば自動車などの移動体に設置されるナビゲーション装置に適用した場合を例に挙げる。
（実施形態１）
本発明の実施の形態の取付け角度算出装置１の構成を図１に示す。図１において、取付け角度算出装置１は、移動体に搭載される電子機器の一例としてのナビゲーションシステムに実装されており、ナビゲーションシステムの筐体（以下、単に筐体と称する）が取付けられた角度を算出する。このような取付け角度の算出のために、取付け角度算出装置１は、微分器９と、角速度センサ１０と、速度センサ１１と、加速度センサ１２と、取付け角度演算手段１３と、カーブ・直進判定手段１４とを備える。
【００１７】
加速度センサ１２は、予め定められた座標系における移動体の加速度を検出する。このような加速度センサ１２が移動体に対して正しい姿勢で設置された場合、加速度センサ１２によって、移動体の進行方向への加速度と、移動体の右側方向で移動体の進行方向に水平面内で直交する方向への加速度と、移動体の水平面に対して下方向に直交する方向への加速度とが検出される。以下、図３（Ａ）、（Ｂ）に示すように、移動体の進行方向をXB軸方向と称し、移動体の右側方向で移動体の進行方向に水平面内で直交する方向をYB軸方向と称し、移動体の水平面に対して下方向に直交する方向をZB軸方向と称する。以下、これらXB軸、YB軸及びZB軸からなる座標系を車両座標系と称する（下添え字は車両座標系（Body frame）を表す）。なお、本実施形態の取付け角度算出装置１は、少なくとも２軸の加速度センサ１２を備えるとして説明する。２軸の加速度センサを備える場合、その軸の組み合わせはどの組み合わせでもよい。
【００１８】
角速度センサ１０は、予め定められた座標系における移動体の角速度を検出する。このような角速度センサ１０が移動体に対して正しい姿勢で設置されている場合、角速度センサ１０によって、車両座標系におけるそれぞれの軸回りの移動体が回転する際の角速度が検出される。車両座標系におけるXB軸回りの角速度をロールレートと称し、その角速度を時間積分して求められる角度をロール角と称し、車両座標系におけるYB軸回りの角速度をピッチレートと称し、その角速度を時間積分して求められる角度をピッチ角と称し、車両座標系におけるZB軸回りの角速度をヨーレートと称し、その角速度を時間積分して求められる角度をヨー角と称する。なお、本実施形態の取付け角度算出装置１は、上述のようなZB軸回りの角速度を検出する１軸の角速度センサ１０を備えるとして説明する。
【００１９】
速度センサ１１は、移動体の移動速度を検出する。移動体が車両の場合には、速度センサ１１は好ましくは、移動体の一例としての車両に元々設置されており、車両の走行速度に応じてパルス周期が変化する車速パルスを発生する車速パルス発生装置である。なお、本実施形態の取付け角度算出装置１は、上述のような移動速度を微分器９で時間微分して求められる加速度を使用する。
【００２０】
取付け角度演算手段１３は、典型的には、CPU、ROM及びRAMを含んでおり、角速度センサ１０、速度センサ１１、加速度センサ１２と電気的に接続される。CPUは、ROMに予め格納されるコンピュータプログラムを、RAMを使って実行する。コンピュータの実行中、取付け角度演算手段１３は、角速度センサ１０から得られる角速度、速度センサ１１から得られる移動速度を微分器９で時間微分して得られる加速度、及び加速度センサ１２から得られる各加速度を少なくとも用いて、筐体の取付け角度を算出する。
【００２１】
カーブ・直進判定手段１４は、例えばRAMの記憶領域の一部で構成されており、角速度センサ１０で検出された角速度の１秒間積算した値（移動体が１秒間に回転した角度）が所定の値、例えば0.2deg未満か以上かで移動体の走行状況を判定する。判定の方法は、これに限らず加速度センサの出力値を用いてもよい。
【００２２】
次に、取付け角度演算手段１３の詳細を示した図４を用いて詳しく説明する。取付け角度の候補演算手段１６は、移動体が直進走行している場合に移動体が水平面を走行していると仮定して、加速度センサ１２から得られる加速度を用いて筐体の取付け角度の候補を算出する。車両の走行角度算出手段１５は、移動体がカーブを走行している場合に、取付け角度の候補演算手段１６により算出した取付け角度の候補と角速度センサ１０から得られる角速度を用いて移動体が走行したカーブの角度を算出する。また、地図の角度情報１８は例えばHDDもしくはDVD、CD-ROMなどの記憶媒体に予め格納されているのが一般的である。
【００２３】
取付け角度の決定演算手段１７による筐体の取付け角度の決定は、以下のように行う。角速度センサ１０から得られる角速度を用いて計算し得られるカーブの角度と地図の角度情報１８またはGPS方位から算出した角度を比較し一致するとき、取付け角度の候補を求めたときに移動体が水平面を走行していたと確認できるので、移動体が水平面を走行している状態で取付け角度候補を算出したことになる。したがって、その取付け角度候補を筐体の取付け角度として決定する。さらには、移動体が水平面を走行していた時に算出されたと確かめられた取付け角度の候補を十分なデータ量が得られるまで蓄積し、正規分布等の統計的手法により筐体の取付け角度を決定してもよい。
【００２４】
次に、以上のような構成の取付け角度算出装置１の動作の流れについて図２を用いて説明する。具体的な算出方法については、後ほど図３（Ａ）、（Ｂ）を用いて説明する。
【００２５】
まず、カーブ・直進判定手段１４により移動体がカーブまたは直進していることを判定する（S００１）。移動体が直進していると判断した場合に加速度センサ１２から検出される加速度より、筐体が取付けられた角度の候補を算出する（S００２）。そして、移動体がカーブを走行していると判断した場合にＳ００２で求めた筐体の取付け角度を用いて車両座標系における移動体のZB軸回りの角速度を算出する（S００３）。その角速度をカーブが終了する（Ｓ００５）までの間、時間積分することで求まる移動体が走行したカーブの角度（Ｓ００４）と地図に格納された角度情報がある一定の範囲、例えば±５degの範囲で一致した時（Ｓ００６）の取付け角度の候補を蓄積する（Ｓ００７）。図２のフローチャートでは、統計的性質から筐体の取付け角度を決定する流れを示しているが、Ｓ００６において地図の角度情報と走行角度が一致したときの取付け角度候補を取付け角度として採用してもよい。また、地図に格納された角度情報の替わりにGPS方位を用いて算出したカーブの角度を使うことも可能である。
【００２６】
次に、ある程度のデータ量が蓄積された時点（Ｓ００８）でデータの正規性の検定を行い、その結果、データが正規分布である場合（Ｓ００９）に筐体の取付け角度を決定する（Ｓ０１０）。
【００２７】
次に、本実施の形態の取付け角度算出装置１の具体的な算出方法の詳細を図３（Ａ）、（Ｂ）を用いて説明する。
【００２８】
図３（Ａ）、（Ｂ）に示すように車両の進行方向をXB軸、車両の右側をYB軸、XB-YB軸平面に対して垂直下向きをZB軸とし、これらXB軸、YB軸、ZB軸からなる座標系は、前述したように車両座標系とする（下添え字は車両座標系（Body frame）を表す）。それぞれ筐体がY軸回りの回転θ及びZ軸回りの回転φで傾いている状態で取付けられた場合の座標軸の変化を示している。また、（XS, YS, ZS）（下添え字はセンサ座標系（Sensor frame）を表す。）は筐体に取付けられた加速度センサもしくは角速度センサのセンサ座標系であり、筐体が車両に対して正しい姿勢で設置された場合、車両座標系とセンサ座標系のそれぞれの軸は一致する。
【００２９】
ここで、Y軸を角度θ回転させる回転行列をRY(θ)、Z軸を角度φ回転させる回転行列をRZ(φ)とすると、2軸回転後の座標系（Xαβ, Yαβ, Zαβ）と基準となる座標系（X,Y, Z）には下式（１）の関係が成り立つ。
【００３０】
【数１】
【００３１】
したがって、筐体の取付け角度としてYB軸回りの角度であるピッチ角と、ZB軸回りの角度であるヨー角の２軸の回転がある場合に車両座標系で検出された加速度（AB,X, AB,Y, AB,Z）とセンサの座標系で検出された加速度（AS,X, AS,Y, AS,Z）には下式（２）、（３）、（４）の関係式が成り立つ。なお、AB,Xは車両座標系における進行方向への加速度を意味する（下添え字は車両座標系におけるXB軸方向の意味を表す）。また、AS,Xはセンサ座標系におけるXS軸方向への加速度を意味する。（下添え字はセンサ座標系におけるXS軸方向の意味を表す。）
【００３２】
【数２】
【００３３】
また、角速度センサ１０で検出される角速度についても同様に車両座標系で検出された角速度（WB,X, WB,Y, WB,Z）とセンサ座標系で検出された角速度センサ１０で検出された角速度（WS,X , WS,Y, WS,Z）には下式（５）、（６）、（７）の関係式が成り立つ。
【００３４】
【数３】
【００３５】
以上の関係を用いて、角速度センサ１０の出力値から車両が直進している場合の加速度と車両がカーブを走行している場合の角速度を用いて筐体が取付けられている角度を算出する方法を図４に示す取付け角度演算手段１３の構成で説明する。まず、取付け角度の候補演算手段１６において角速度センサ１０の出力値からカーブ・直進判定手段１４より車両の直進走行を判断する。車両が直進走行している場合に、車両が水平面上を加速度Aで走行しているとすると、車両座標系における加速度のXB軸方向成分はAであり、YB軸方向成分は0であり、ZB軸方向成分はG（重力加速度のみ）であるから（AB,X, AB,Y, AB,Z）=（A, 0, G）となる。AB,Xは速度センサの時間微分から求められる。したがって、（３）式において加速度センサの出力値からAS,Yは検出できるので筐体の取付け角度の候補φ’を求めることができる。また、φ’を（２）式もしくは（４）式に代入することで筐体の取付け角度θ’を求めることができる。これは上述したように車両が水平面を走行していることを前提として算出する。
【００３６】
また、車両が水平面を走行している場合に、角速度センサの出力値から車両がカーブを走行していると判断した場合、角速度は車両座標系におけるZB軸回りのみと考えられる。すなわち、車両座標系で検出される角速度は（0, 0, WB,Z）となる。
【００３７】
したがって、（５）、（６）、（７）式は下式（８）、（９）、（１０）のように簡単にできる。
【００３８】
【数４】
【００３９】
以上より、車両の走行角度算出手段１５において取付け角度の候補演算手段１６で算出した筐体の取付け角度の候補であるθ’を（１０）式に代入することでWB,Zを算出し、WB,Zをカーブ終了時まで時間積分することで車両が走行したカーブの角度γを下式（１１）により求めることができる。
【００４０】
【数５】
【００４１】
次に、取付け角度の決定演算手段１７にて行う処理について説明する。水平面を車両が走行していることを前提として導いた（１１）式で求められた角度γと地図に格納されたカーブの角度（地図の角度情報１８）またはGPS方位より算出したカーブの角度が一致するとき直進走行時、カーブ走行時に車両が水平面を走行していたことが検証できる。したがって、求められた取付け角度候補θ’は正確であるといえるのでθ’を取付け角度として決定することができる。しかしながら、実際には地図に格納されたカーブの角度が全て実際の道路形状と同じではない。したがって、精度を上げるには、例えば±２deg程度で一致したときの取付け角度θ’のデータを蓄積し、このθ’が正規分布に従うとき、例えばその平均値を取付け角度として算出する。取付け角度のヨー角φについてはθ’と対応するときの値を蓄積させ、θ’について正規性が成り立った時に例えば平均値を求めればよい。
【００４２】
このような本発明の第１の実施の形態の取付け角度算出装置１によれば、移動体が直進している場合に加速度センサ１２で検出される値から筐体が取付けられている角度の候補を算出する。そして、移動体がカーブを走行している場合にその取付け角度の候補から求めた車両座標系におけるZ軸回りの角速度を、カーブ終了時まで時間積分することで求まるカーブの角度と、地図に格納された角度情報が一致するか比較する。一致した時の取付け角度の候補を筐体の取付け角度として決定するか、その候補の十分な量の蓄積後に統計的処理により取付け角度を算出する。したがって、車両が水平面を走行していなくても筐体が取付けられている角度を算出することができる。
（実施形態２）
第１の実施形態では、地図に格納されているカーブの角度情報を用いて筐体の取付け角度を算出する際に地図に格納されたカーブの角度若しくは、GPS方位から算出した角度に誤差があるので、より精度を向上させるために、一定の範囲で一致するものを抽出し、取付け角度を統計的処理により求める構成とした。
【００４３】
第２の実施形態では、より筺体の取付け角度を正確にもとめることが可能な取付け角度算出装置を図５のブロック図を参照して説明する。
【００４４】
図５に示す取付け角度算出装置１は、図１に示す取付け角度算出装置１と比較すると、加速度を用いた取付け角度信頼性判定手段２２、車両の進行方向加速度演算手段２３及びGPS受信機２１とをさらに備える。それ以外に両取付け角度算出装置１には相違点は無いので、図５において、図１に示す構成に相当するものには同一の参照符号を付け、それぞれの説明は省略する。ただし、本実施形態の取付け角度算出装置１は、筐体の取付け角度算出の信頼度を高めるために３軸の加速度センサ１２を備えるとして説明する。
【００４５】
GPS受信機２１は、GPS衛星からの電波を受信して地球上における車両の絶対位置（緯度・経度・高度）を算出する。
【００４６】
車両の進行方向加速度演算手段２３は、加速度センサ１２から得られる各加速度及び取付け角度演算手段１３から得られる筐体の取付け角度を少なくとも用いて車両の進行方向の加速度を算出する。加速度を用いた取付け角度信頼性判定手段２２は、求められた車両の進行方向の加速度及び速度センサ１１から得られる速度を時間微分することで算出する加速度もしくはGPS受信機から得られる速度を時間微分することで算出する参照加速度を少なくとも用いて取付け角度演算手段１３より得られる筐体の取付け角度の信頼度を確認する。
【００４７】
次に、以上のような構成の取付け角度算出装置１の動作の流れについて図７を用いて説明する。
【００４８】
取付け角度演算手段１３により、算出された筐体の取付け角度から筐体の取付け角度を決定（Ｓ１０１）した時（統計的処理により取付け角度を求めた場合は、決定した取付け角度と同じ値の取付け角度候補を算出した時）の車両の進行方向の加速度を加速度センサ１２より算出（S１０２）する。本実施例では、筐体の取付け角度の算出は、実施の形態１により求めた値を使用するとして説明するが、他の方法により求めた値を使用しても構わない。また、同時に速度センサ１１で得られる速度を時間微分することにより求まる加速度もしくはGPS受信機２１から得られる速度を時間微分することにより求まる参照加速度を算出する。
【００４９】
次に、筐体の取付け角度を用いて加速度センサ１２から算出した加速度と速度センサ１１もしくはGPS受信機２１から算出した加速度と比較する（S１０３）。速度センサ１１から算出される加速度の誤差は、速度センサ１１として例えば車速パルスを用いる場合、１パルス分であり、GPSから算出される加速度の誤差はGPS速度誤差分（GPS速度によるが１σ=３km/h）であり精度が高い。したがって、加速度の差がその誤差範囲外であればＳ１０１で決定した筐体の取付け角度は、信頼度が低いことになるので、取付け角度演算手段１３を用いて筐体の取付け角度を再度計算し直す（S１０５）。これを繰り返すことで算出する筐体の取付け角度の信頼性を高めることができる。また、誤差範囲を決定するのに、速度センサ１１とＧＰＳ受信機２１の両方の値を用いてもよい
次に、本実施形態の取付け角度算出装置１の具体的な算出方法の詳細を図５及び数式を用いて説明する。尚、座標系は実施形態１で説明した図３と同じであり、符号も同じものを使用する。
【００５０】
取付け角度演算手段１３により筐体の取付け角度を決定した値を算出した時（または、決定値と同じ値の取付け角度候補を算出した時）は車両が水平面を走行していると仮定して、加速度センサの値を用いて車両の進行方向の加速度計算を行う方法を説明する。まず、水平面に対するセンサ座標系の傾きは筐体が取付けられた角度のピッチ角θとヨー角φのみと考えることができる。実施の形態１の説明で用いた（３）式について（RY(θ)RZ(φ)）は直交行列となっており、その逆行列と転置行列は等しいことから下式（１２）となる。
【００５１】
【数６】
【００５２】
なお、上添え字の−１は逆行列を表し、Tは転置行列を表す。したがって、下式（１３）の関係が成り立つ。
【００５３】
【数７】
【００５４】
取付け角度演算手段１３より求めた筐体が取付けられた角度のピッチ角θとヨー角φを（１３）式に代入すると車両の進行方向の加速度が求まる。すなわち、車両座標系のXB軸方向の加速度は下式（１４）となる。
【００５５】
【数８】
【００５６】
したがって、車両の進行方向加速度演算手段２３では、（１４）式を用いて車両座標系のXB軸方向の加速度を算出する。そして、加速度を用いた取付け角度信頼性判定手段２２では、加速度センサ１２を用いて求められた加速度と速度センサ１１から得られる速度を時間微分することより算出した加速度もしくはGPS受信機２１から得られた速度を時間微分することにより算出した参照加速度の差がある一定の範囲例えば速度センサ１１として例えば車速パルスを用いる場合、１パルス分、GPSから算出される加速度の場合、GPS速度誤差分（GPS速度によるが１σ=３km/h）の範囲内であれば算出した筐体の取付け角度の信頼性は高く、その範囲外であれば再び取付け角度演算手段１３で筐体の取付け角度を求めることで第１の実施形態よりも筐体の取付け角度の精度を高めることが可能となる。
（実施形態３）
本実施の形態の構成は、筺体の取付け角度の決定後、筺体の取付け角度の精度を高めるため角速度を用いて信頼性判定を行う。
【００５７】
以下、図６のブロック図を参照して、筐体の取付け角度の決定後、筐体の取付け角度の信頼度を確認する取付け角度算出装置１を説明する。取付け角度算出装置１は、図５に示す取付け角度算出装置１と比較すると、図５に示す車両の加速度を用いた取付け角度信頼性判定手段２２及び車両の進行方向加速度演算手段２３に代えて、角速度を用いた取付け角度信頼性判定手段３１及び車両の鉛直方向軸角速度演算手段３２とを備える。それ以外に両取付け角度算出装置１には相違点は無いので、図６において、図１及び図５に示す構成に相当するものには同一の参照符号を付け、それぞれの説明を省略する。ただし、本実施の形態の取付け角度算出装置１は、さらに筐体の取付け角度の精度を高めるためにXB軸回りの角速度とZB軸回りの角速度を検出する２軸の角速度センサ１０を備えるとして説明する。
【００５８】
車両の鉛直方向軸角速度演算手段３２は、角速度センサ１０から得られる各角速度及び取付け角度演算手段１３から得られる筐体の取付け角度を少なくとも用いて車両座標系におけるZB軸回りの角速度を算出する。角速度を用いた取付け角度信頼性判定手段３１は、求められた車両座標系におけるZB軸回りの角速度及びGPS受信機から得られる方位変化量を時間微分することで算出する参照角速度を少なくとも用いて取付け角度演算手段１３より得られる筐体の取付け角度の信頼度を確認する。
【００５９】
次に、以上のような構成の取付け角度算出装置１の動作の流れについて図８を用いて説明する。
【００６０】
これは、実施形態２の取付け角度算出装置１と同様に取付け角度演算手段１３により算出された筐体の取付け角度を決定した時（統計的処理により取付け角度を求めた場合は、決定した取付け角度と同じ値の取付け角度候補を算出した時）の車両のZB軸方向の角速度を算出（S２０２）する。また、同時にGPS受信機２１から得られる方位変化量を時間微分することで角速度を算出する。筐体の取付け角度を用いて角速度センサ１０から算出した角速度とGPS受信機２１から算出した角速度と比較する（S２０３）。GPSから算出される角速度の誤差はGPS方位誤差分であるため、角速度の差がその範囲外であれば取付け角度演算手段１３を用いて筐体の取付け角度を再度計算し直す（S２０５）。これを繰り返すことで算出する筐体の取付け角度の精度を高めることができる。
【００６１】
次に、本実施形態の取付け角度算出装置１の具体的な処理の詳細を図６と数式を用いて説明する。
【００６２】
（１３）式から筐体の取付け角度のピッチ角θ、ヨー角φを代入することで車両座標系におけるZＢ軸方向の角速度は次式より求まる。
【００６３】
【数９】
【００６４】
したがって、車両の鉛直方向軸角速度演算手段３２では、（１５）式で求まる車両座標系のZB軸回りの角速度を算出する。そして、角速度を用いた取付け角度信頼性判定手段２２では、角速度センサを用いて求められた角速度とGPS受信機から得られる方位変化量を時間微分することより算出した参照角速度差がある一定の範囲例えばGPS方位誤差内であれば算出した筐体の取付け角度の信頼性は高く、その範囲外であれば再び取付け角度演算手段で筐体の取付け角度を求める。
【００６５】
さらに、本発明の取付け角度算出装置１を備えた自立航法センサ補正装置４について、図９を用いて説明する。図９において、自立航法センサ補正装置４は、前述の実施の形態１の取付け角度算出装置１を組み込んでおり、筐体の取付け角度からセンサ座標系で得られた加速度、角速度を車両座標系の出力に補正する。このような自立航法センサの補正のために、自立航法センサ補正装置４は、取付け角度算出装置１と、自立センサ補正手段１９とを備える。
【００６６】
次に、自立センサ補正手段１９の詳細を図１０を用いて説明する。自立センサ補正手段１９は、角速度センサ１０から得られる角速度、加速度センサ１２から得られる各加速度、及び取付け角度演算手段１３より算出した筐体の取付け角度を少なくとも用いて自立航法センサを補正する。
【００６７】
方位角算出手段５は、角速度センサ１０から得られるセンサ座標系におけるZS軸回りの角速度を取付け角度演算手段１３で算出した筐体の取付け角度により、車両座標系におけるXB軸回りの角速度に補正することで車両の方位角を算出する。
【００６８】
速度算出手段６は、同様に加速度センサ１２から得られるセンサ座標系におけるXS軸方向への加速度を取付け角度演算手段１３で算出した筐体の取付け角度より、車両座標系におけるXB軸方向への加速度に補正することで車両の進行方向への速度を算出する。
【００６９】
次に、以上のような構成の自立航法センサ補正装置４の動作の流れについて図１１を用いて説明する。
【００７０】
取付け角度算出装置１で筐体の取付けられている角度を算出（Ｓ３０１）し、求めた筐体の取付け角度から加速度センサ１２、角速度センサ１０から得られたセンサ座標系におけるXS軸方向への加速度、ZS軸回りの角速度を車両座標系におけるXB軸方向への加速度、ZB軸回りの角速度を算出する（Ｓ３０２）。このように補正された出力値を時間積分することで車両の速度や方位角を求めることできる（Ｓ３０３）。また、正確な自車位置を算出することができる。
【００７１】
次に、本実施形態の自立航法センサ補正装置４の具体的な処理の詳細を図５、図１０を用いて説明する。
【００７２】
速度算出手段６では、加速度センサ１２から得られるセンサ座標系におけるXS軸方向への加速度AS,Xを取付け角度の算出装置１で算出した筐体の取付け角度を用いて（１５）式から車両座標系におけるXB軸方向への加速度AB,Xに補正する。そして、補正された加速度AB,Xを時間積分することで車両の進行方向への速度を算出する。方位角算出手段５では、角速度センサ１０から得られるセンサ座標系におけるZS軸回りの角速度WS,Zを取付け角度の算出装置１で算出した筐体の取付け角度を用いて（１６）式からZB軸回りの角速度WB,Zに補正する。そして、補正された角速度WB,Zを時間積分することで車両の方位角を算出する。
【産業上の利用可能性】
【００７３】
本発明にかかる取付け角度算出装置は、様々な走行状況が存在する実際の走行状態においても、精度良く移動体に搭載された電子機器の筐体の取付け角度を算出することが可能になるので、移動体に搭載される電子機器の筐体の取付け角度を算出する取付け角度算出装置等として有用である。
【図面の簡単な説明】
【００７４】
【図１】本発明の第１の実施の形態における取付け角度算出装置の構成図
【図２】本発明の第１の実施の形態における取付け角度算出装置の動作説明のためのフロー図
【図３】（Ａ）本発明の座標系でのピッチ方向の取付け角度を説明するための図（Ｂ）本発明の座標系でのヨー方向の取付け角度を説明するための図
【図４】本発明の第１の実施の形態における取付け角度演算手段の構成図
【図５】本発明の第２の実施の形態における取付け角度算出装置の構成図
【図６】本発明の第３の実施の形態における取付け角度算出装置の構成図
【図７】本発明の第２の実施の形態における取付け角度算出装置の動作説明のためのフロー図
【図８】本発明の第３の実施の形態における取付け角度算出装置の動作説明のためのフロー図
【図９】本発明の取付け角度算出装置を備えた自立航法センサ補正装置の構成図
【図１０】本発明の取付け角度算出装置を自立航法センサ補正装置に適用する構成図
【図１１】本発明の取付け角度算出装置を備えた自立航法センサ補正装置の動作説明のためのフロー図
【図１２】従来の取付け角度算出装置の構成図
【図１３】従来の取付け角度算出装置における算出方法を説明するための図
【符号の説明】
【００７５】
１ 取付け角度算出装置
４ 自立航法センサ補正装置
５ 方位角算出手段
６ 速度算出手段
９ 微分器
１０ 角速度センサ
１１ 速度センサ
１２ 加速度センサ
１３ 取付け角度演算手段
１４ カーブ・直進判定手段
１５ 車両の走行角度算出手段
１６ 取付け角度の候補演算手段
１７ 取付け角度の決定演算手段
１８ 地図の角度情報
１９ 自立センサ補正手段
２１ ＧＰＳ受信機
２２ 加速度を用いた取付け角度信頼性判定手段
２３ 車両の進行方向加速度演算手段
３１ 角速度を用いた取付け角度信頼性判定手段
３２ 車両の鉛直方向軸角速度演算手段
【特許請求の範囲】
【請求項１】
移動体に搭載される電子機器の筐体の取付け角度算出装置であって、
移動体が直進しているかカーブを走行しているかを判定するカーブ直進判定手段を有し、前記カーブ直進判定手段により移動体が直進していると判定しているときに取付け角度候補を算出し、前記カーブ直進判定手段により移動体がカーブを走行していると判定しているときの加速度センサおよび／または角速度センサの出力値から前記取付け角度を決定する取付け角度演算手段を有することを特徴とする取付け角度算出装置。
【請求項２】
移動体に搭載される電子機器の筐体の取付け角度算出装置であって、
カーブの角度情報を記憶するカーブ情報記憶手段と、移動体が直進しているかカーブを走行しているかを判定するカーブ直進判定手段と、加速度センサの出力値から筐体の取付け角度候補を算出する取付け角度候補演算手段を有し、
前記カーブ直進判定手段により移動体が直進していると判定しているときに前記取付け角度候補演算手段により算出された取付け角度候補から移動体のヨー方向の角速度を算出し、前記カーブ直進判定手段によりカーブを走行していると判定している間、前記角速度を時間積算することにより得られる所定のカーブの角度と、前記カーブ情報記憶手段に記憶されている前記カーブの角度情報またはＧＰＳ方位から算出した角度情報が一致するとき前記取付け角度候補を筐体の取付け角度とする取付け角度演算手段を有することを特徴とする取付け角度算出装置。
【請求項３】
前記取付け角度演算手段は、算出された取付け角度を正規分布となるまで蓄積し、前記正規分布の平均値または代表値を筐体の取付け角度とすることを特徴とする請求項１または請求項２に記載の取付け角度演算装置。
【請求項４】
前記取付け角度演算手段は、決定した筐体の取り付け角度と同じ値の取付け角度候補を算出した時の移動体の加速度センサによる進行方向の加速度と、速度センサの出力値を時間微分して得られる加速度もしくはGPS受信機から算出した加速度が一致しない場合、筐体の取付け角度を再計算することを特徴とする請求項１または２に記載の取付け角度演算装置。
【請求項５】
前記取付け角度演算手段は、決定した筐体の取り付け角度と同じ値の取付け角度候補を算出した時の角速度センサによる移動体の水平面に対して直交する方向の角速度と、GPS受信機から算出した角速度が一致しない場合、筐体の取付け角度を再計算することを特徴とする請求項１または２に記載の取付け角度演算装置。
【請求項６】
算出した取付け角度を自立航法センサの補正に用いることを特徴とする請求項１または２に記載の取付け角度演算装置。
【請求項１】
移動体に搭載される電子機器の筐体の取付け角度算出装置であって、
移動体が直進しているかカーブを走行しているかを判定するカーブ直進判定手段を有し、前記カーブ直進判定手段により移動体が直進していると判定しているときに取付け角度候補を算出し、前記カーブ直進判定手段により移動体がカーブを走行していると判定しているときの加速度センサおよび／または角速度センサの出力値から前記取付け角度を決定する取付け角度演算手段を有することを特徴とする取付け角度算出装置。
【請求項２】
移動体に搭載される電子機器の筐体の取付け角度算出装置であって、
カーブの角度情報を記憶するカーブ情報記憶手段と、移動体が直進しているかカーブを走行しているかを判定するカーブ直進判定手段と、加速度センサの出力値から筐体の取付け角度候補を算出する取付け角度候補演算手段を有し、
前記カーブ直進判定手段により移動体が直進していると判定しているときに前記取付け角度候補演算手段により算出された取付け角度候補から移動体のヨー方向の角速度を算出し、前記カーブ直進判定手段によりカーブを走行していると判定している間、前記角速度を時間積算することにより得られる所定のカーブの角度と、前記カーブ情報記憶手段に記憶されている前記カーブの角度情報またはＧＰＳ方位から算出した角度情報が一致するとき前記取付け角度候補を筐体の取付け角度とする取付け角度演算手段を有することを特徴とする取付け角度算出装置。
【請求項３】
前記取付け角度演算手段は、算出された取付け角度を正規分布となるまで蓄積し、前記正規分布の平均値または代表値を筐体の取付け角度とすることを特徴とする請求項１または請求項２に記載の取付け角度演算装置。
【請求項４】
前記取付け角度演算手段は、決定した筐体の取り付け角度と同じ値の取付け角度候補を算出した時の移動体の加速度センサによる進行方向の加速度と、速度センサの出力値を時間微分して得られる加速度もしくはGPS受信機から算出した加速度が一致しない場合、筐体の取付け角度を再計算することを特徴とする請求項１または２に記載の取付け角度演算装置。
【請求項５】
前記取付け角度演算手段は、決定した筐体の取り付け角度と同じ値の取付け角度候補を算出した時の角速度センサによる移動体の水平面に対して直交する方向の角速度と、GPS受信機から算出した角速度が一致しない場合、筐体の取付け角度を再計算することを特徴とする請求項１または２に記載の取付け角度演算装置。
【請求項６】
算出した取付け角度を自立航法センサの補正に用いることを特徴とする請求項１または２に記載の取付け角度演算装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【公開番号】特開２００７−１０７９５１（Ｐ２００７−１０７９５１Ａ）
【公開日】平成１９年４月２６日（２００７．４．２６）
【国際特許分類】
【出願番号】特願２００５−２９７５０９（Ｐ２００５−２９７５０９）
【出願日】平成１７年１０月１２日（２００５．１０．１２）
【出願人】（０００００５８２１）松下電器産業株式会社 (73,050)
【Ｆターム（参考）】
【公開日】平成１９年４月２６日（２００７．４．２６）
【国際特許分類】
【出願日】平成１７年１０月１２日（２００５．１０．１２）
【出願人】（０００００５８２１）松下電器産業株式会社 (73,050)
【Ｆターム（参考）】
[ Back to top ]