JP5643334B2 - 角速度検出装置、角速度検出方法、移動状態検出装置およびナビゲーション装置 - Google Patents

Description

この発明は、移動体に設置され、当該移動体の角速度を検出する角速度検出装置および、移動体の移動状態を検出する移動状態検出装置に関する。

現在、自動車等の移動体に取り付けられ、当該移動体の位置、走行速度、および走行方位を検出して、目的地への走行を援助する情報を表示するナビゲーション装置が各種考案されている。そして、このようなナビゲーション装置では、ＧＰＳ衛星等の測位衛星からの測位信号に基づいて自装置位置を検出するとともに、ジャイロセンサによる角速度、加速度センサによる加速度等を利用して、移動体の移動状態を検出している。

ところで、最近では、移動体に予め設置されたものではなく、着脱可能なパーソナルナビゲーション装置も多く利用されている。

このようなパーソナルナビゲーション装置を用いる場合、移動体への取り付け角度が問題になる。例えば、ジャイロセンサの取り付け角度によって、ジャイロセンサの角速度検出軸と、姿勢を演算するための絶対方位に基づく軸との間に差が生じ、正確な姿勢角を検出できない場合がある。

このため、特許文献１に示す車載用角速度検出装置では、加速度センサの検出結果から取り付け角度を検出している。特許文献１に記載の車載用角速度検出装置は、検出した取り付け角度に基づいて、ジャイロセンサから出力される角速度の補正を行っている。

特開２００２−２４３４９４号公報

しかしながら、上述の特許文献１に記載の車載用角速度検出装置では、加速度センサとジャイロセンサとの取り付け角が同じでなければ、正確なジャイロセンサの取り付け角度を検出することができない。また、加速度センサで検出した正確な加速度が無ければ、取り付け角度を検出することができない。

本発明の目的は、ジャイロセンサからの出力を用いて、正確なジャイロセンサの取り付け角度を推定検出できる角速度検出装置を実現することにある。

この発明は、移動体に装着され、該移動体の角速度を検出する角速度検出装置に関する。角速度検出装置は、ジャイロセンサ、ミスアライメント角推定部、座標変換部を備える。ジャイロセンサは、移動体に取り付けられており、取り付け角度に応じたセンサ座標系における直交三軸の角速度を計測する。ミスアライメント角推定部は、移動体が平坦地を旋回中に取得したセンサ座標系における角速度に基づいて、ジャイロセンサの直交三軸と移動体の直交三軸とのロール方向の角度差（ミスアライメント角）またはピッチ方向の角度差（ミスアライメント角）を推定する。座標変換部は、角度差に基づいてセンサ座標系における角速度から移動体座標系における角速度への座標変換演算を実行する。

この構成では、ジャイロセンサで計測された角速度のみによって、ミスアライメント角が推定される。

また、この発明の角速度検出装置は、角速度のバイアス成分を除去するバイアス成分除去部を備える。ミスアライメント角推定部および座標変換部は、バイアス成分除去後の角速度を用いて演算を行う。

この構成では、ジャイロセンサで計測された角速度に含まれるバイアス成分が除去されるので、ミスアライメント角がより高精度に推定される。

また、この発明の角速度検出装置のバイアス成分除去部は、ジャイロセンサから出力された角速度の時間平均値を用いて、該角速度を補正することで、バイアス成分を除去する。

この構成では、バイアス成分除去部の具体的処理を示しており、時間平均値を用いた例を示している。

また、この発明の角速度検出装置は、角速度を周波数解析し、バイアス成分および雑音成分の周波数成分を除去した周波数成分からなる角速度を算出する周波数解析部を備える。ミスアライメント角推定部および座標変換部は、周波数解析部から出力される角速度を用いて演算を行う。

この構成では、バイアス成分および雑音成分を除去する具体的処理を示しており、周波数解析を用いた例を示している。

また、この発明の角速度検出装置の周波数解析部は、ウェーブレット変換を用いて周波数解析を行う。

この構成では、周波数解析部のより具体的処理について示しており、ウェーブレット変換を用いた例を示している。

また、この発明の角速度検出装置では、移動体の加速度および過去に検出した角速度に基づいて旋回中であることを検出する旋回検出部を備える。

この構成では、移動体の旋回中を検出する具体的構成を示している。

また、この発明は、移動状態検出装置に関する。移動状態検出装置は、上述の角速度検出装置、姿勢角検出部、加速度センサ、測位部、および移動状態算出部を備える。姿勢角検出部は、角速度検出装置で検出した角速度を用いて、移動体の姿勢角を検出する。加速度センサは、移動体の加速度を検出する。測位部は、測位用信号を受信して、移動体の位置、速度、方位を測定する。移動状態算出部は、姿勢角検出部で得られた姿勢角、加速度センサで得られた加速度、測位部で得られた位置、速度、方位を用いて、移動体の位置、速度、姿勢角を含む移動状態を算出する。

この構成では、上述の角速度検出装置を含む移動状態検出装置の構成について示している。上述の角速度検出装置を備えることで、高精度な角速度が得られるため、高精度な姿勢角を検出することができる。これにより、移動体の移動状態を高精度に検出することができる。

また、この発明はナビゲーション装置に関する。このナビゲーション装置は、上述の移動状態検出装置と、移動状態を含むナビゲーション情報を通知するユーザ通知部と、を備える。

この構成では、上述の移動状態検出装置を備えることで、移動体の移動状態を高精度に検出できるので、正確なナビゲーションが可能になる。

この発明によれば、ジャイロセンサの出力のみを用いて、当該ジャイロセンサの取り付け角度の誤差を正確に検出することができる。これにより、移動体の姿勢を検出する軸と、ジャイロセンサによる角速度の計測の軸とで角度差に影響されることなく、移動体の姿勢角を正確に検出することができる。

第１の実施形態に係る角速度検出装置１の構成を示すブロック図である。 ジャイロセンサ２０の移動体に対する取り付け角度を説明する図である。 第１の実施形態に係る角速度検出装置１を含む移動状態検出装置１００の構成を示すブロック図である。 第２の実施形態に係る角速度検出装置１Ａの構成を示すブロック図である。

本発明の第１の実施形態に係る角速度検出装置、および当該角速度検出装置を含む移動状態検出装置について、図を参照して説明する。本実施形態に係る移動状態検出装置は、車載用ナビゲーション装置やＰＮＤ（Personal Navigation Device）等の各種ナビゲーション装置に利用されるものである。

図１は本実施形態の角速度検出装置１の主要構成を示すブロック図である。図２は移動体に装着された状態でのジャイロセンサ２０の検出軸を説明する図である。

角速度検出装置１は、角速度算出部１０、およびジャイロセンサ２０を備える。

ジャイロセンサ２０は、図２に示すように、自身の座標系（センサ座標系）において、センサ座標系角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］を計測する。

角速度ω_ｘ ^ｓは移動体の前後方向であるｘ軸を回転軸とするロール角（φ）方向の角速度である。この際、角速度ω_ｘ ^ｓは移動体を正面視した状態で時計回り方向を正方向として検出される。

角速度ω_ｙ ^ｓは移動体の横方向であるｙ軸を回転軸とするピッチ角（θ）方向の角速度である。この際、角速度ω_ｙ ^ｓは移動体を右舷側から見た状態で時計回り方向を正方向として検出される。

角速度ω_ｚ ^ｓは移動体の上下方向であるｚ軸を回転軸とする方位角（ψ）方向の角速度である。この際、角速度ω_ｚ ^ｓは移動体を上面から見た状態で反時計回り方向を正方向として検出される。

ここで、ジャイロセンサ２０は、ロール方向の取り付け角度誤差Δφ、ピッチ方向の取り付け角度誤差Δθ、および方位方向の取り付け角度誤差Δψからなるミスアライメント角［Δφ，Δθ，Δψ］で、移動体に取り付けられているものとする。

このように、ジャイロセンサ２０が移動体に対してミスアライメント角［Δφ，Δθ，Δψ］で取り付けられているので、ジャイロセンサ２０から出力されるセンサ座標系角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］の各成分と移動体座標系加速度［ω_ｘ ^ｂ，ω_ｙ ^ｂ，ω_ｚ ^ｂ］の各成分との間には、ミスアライメント角［Δφ，Δθ，Δψ］に基づく立体角に応じた差が生じてしまう。また、ジャイロセンサ２０は、出力される角速度にバイアス成分を定常的に含んでおり、出力される角速度に対して誤差要因となる。したがって、次に示す角速度算出部１０により、これらの誤差要因を除去する。

角速度算出部１０は、バイアス成分除去部１１、ミスアライメント角推定部１２、座標変換部１３を備える。

バイアス成分除去部１１は、センサ座標系における角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］を、それぞれ時間平均処理し、平均値［Ｅ_ａｖ（ω_ｘ ^ｓ），Ｅ_ａｖ（ω_ｙ ^ｓ），Ｅ_ａｖ（ω_ｚ ^ｓ）］を算出する。バイアス成分除去部１１は、センサ座標系における角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］を平均値［Ｅ_ａｖ（ω_ｘ ^ｓ），Ｅ_ａｖ（ω_ｙ ^ｓ），Ｅ_ａｖ（ω_ｚ ^ｓ）］で減算する。

すなわち、バイアス成分除去処理後の角速度を［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］とすると、
［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］＝［ω_ｘ ^ｓ−Ｅ_ａｖ（ω_ｘ ^ｓ），ω_ｙ ^ｓ−Ｅ_ａｖ（ω_ｙ ^ｓ），ω_ｚ ^ｓ−Ｅ_ａｖ（ω_ｚ ^ｓ）］
の演算を実行する。バイアス成分除去処理には、時間平均に類似した処理を用いても良い。

これにより、角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］に含まれるバイアス成分が除去される。バイアス成分除去後の角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］は、ミスアライメント角推定部１２および座標変換部１３へ出力される。

ミスアライメント角推定部１２には、バイアス成分除去後の角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］とともに、旋回検出結果が入力される。旋回検出結果とは、移動体が旋回中であるかどうかを識別した結果を示す情報である。旋回検出結果は、例えば、後述するように加速度センサによって計測された加速度および前回検出した角速度に基づいて設定されたり、移動体が自動車であればハンドルの回転を検出することで設定される。

ミスアライメント角推定部１２は、旋回検出結果に基づいて、旋回中であることを検出している期間に所得したバイアス成分除去後の角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］を用いてミスアライメント角［Δφ，Δθ，Δψ］を推定する。このミスアライメント角の推定は、旋回中と検出されている期間中であれば、上述のセンサ座標系における角速度の取得タイミングに応じて、例えば１秒毎に行ってもよく、また、センサ座標系における角速度をバッファリングしながら、適宜設定したタイミング毎に行ってもよい。

ここで、ミスアライメント角［Δφ，Δθ，Δψ］の推定算出原理について説明する。バイアス成分除去後のセンサ座標系における角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］、移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］、ミスアライメント角［Δφ，Δθ，Δψ］とした場合、次式が成り立つ。

Ｃ_ｂ ^ｓは、移動体座標系を角速度センサ座標系に変換する回転行列であり、ミスアライメント角［Δφ，Δθ，Δψ］を用いて、次式で表すことができる。

ここで、ミスアライメント角［Δφ，Δθ，Δψ］のそれぞれが、Δφ≪１［ｒａｄ］、Δθ≪１［ｒａｄ］、Δψ≪１［ｒａｄ］である場合、回転行列Ｃ_ｂ ^ｓは、次式のように近似することができる。

したがって、式（１）は次式で表すことができる。

ところで、移動体が平坦地を旋回している場合、移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］のロール角φ方向成分ω_ｘ ^ｂｅ、ピッチ角θ方向成分ω_ｙ ^ｂｅは「０」と見なすことができる。すなわち、ω_ｘ ^ｂｅ＝０，ω_ｙ ^ｂｅ＝０となる。

したがって、式（４）は、次式のように表される。

これにより、方位角ψ方向におけるセンサ座標系における角速度ω_ｚ ^ｓｅが「０」でない場合に、ロール方向ミスアライメント角Δφ、ピッチ方向ミスアライメント角Δθは、次式から算出される。

このように、ジャイロセンサ計測した角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］のみからロール方向ミスアライメント角Δφ、ピッチ方向ミスアライメント角Δθを推定算出することができる。

この際、算出するロール方向ミスアライメント角Δφ、およびピッチ方向ミスアライメント角Δθに対しては、時間平均処理を行うとよりよい。ここで示す時間平均処理とは、各サンプリングタイミングの角速度に基づいて算出したミスアライメント角の瞬時値を合計してサンプリング数で除算する、一般的な時間平均処理を示す。具体的には、時間平均置処理の記号をＥ［（数式）］とすると、

の演算を行うとよい。ただし、この時間平均処理も、旋回中である期間に対してのみ実行する。

なお、上述の説明では、単純な時間平均処理を用いて、ロール方向ミスアライメント角Δφ、およびピッチ方向ミスアライメント角Δθに含まれる誤差を除去した例を示したが、次に示すような一次ローパスフィルタを用いて時間平均処理に相当する処理を行うこともできる。

ここでは、ロール方向ミスアライメント角Δφに対して、一次ローパスフィルタ（ＬＰＦ）処理を行う場合を例に示すが、他のピッチ方向ミスアライメント角Δθについても同様に一次ローパスフィルタ処理を行う。

あるサンプリングタイミングｔでのロール方向ミスアライメント角Δφを、Δφ［ｔ］とし、この次のサンプリングタイミングのロール方向ミスアライメント角ΔφをΔφ［ｔ＋１］とすることで、次式を設定することができる。

ただし、ω_ｚ ^ｓｅ＝０もしくは直進時には、Δφ［ｔ＋１］＝Δφ［ｔ］とする。

また、αは、ＬＰＦのウェイトであり、次の条件によって値を変更する。

（ｉ）ω_ｚ ^ｓｅ≠０であり、平坦な道を旋回中で、（ω_ｙ ^ｓｅ／ω_ｚ ^ｓｅ−Δφ［ｔ］）の値が閾値β未満の場合
この場合は、ミスアライメント角Δφが小さい場合であり、α＝α_１とする。

（ｉｉ）ω_ｚ ^ｓｅ≠０であり、平坦な道を旋回中で、（ω_ｙ ^ｓｅ／ω_ｚ ^ｓｅ−Δφ［ｔ］）の値が閾値β未満の場合
この場合は、ミスアライメント角Δφが大きい場合であり、α＝α_２とする。

ここで、ウェイトα_１、α_２は、０＜α_１≦α_２＜１で設定される。また、閾値βは実験的に設定された値である。

このようなウェイトを変化させるフィルタ処理を用いた場合、例えばユーザが装置の向きを変更した場合、すなわちミスアライメント角が大きな場合には、今回算出されたミスアライメント角に重いウェイトが置かれる。一方、ミスアライメント角が小さな場合には、前回までの算出によるミスアライメント角に重いウェイトが置かれる。したがって、装置の向きの変更によりミスアライメント角が大きく変化するような場合には、変化後のミスアライメント角に素早く収束させ、装置の向きの変更が無いようなミスアライメント角が殆ど変化しない状況では、角速度の算出誤差による影響を抑圧することができる。

なお、このようなローパスフィルタ処理や上述の時間平均処理に替えて、カルマンフィルタ処理を用いてもよい。

このように算出されたミスアライメント角は、座標変換部１３へ出力される。この際、陸上車であれば、ロール角φ、ピッチ角θは方位角ψに対して変動が少なく、ロール角φとピッチ角θとが方位角ψに与える影響は殆ど無い。したがって、方位方向ミスアライメント角Δψは「０」に近似することができる。

座標変換部１３は、ミスアライメント角推定部１２で推定算出されたミスアライメント角［Δφ，Δθ，Δψ］に基づいて、バイアス成分除去部１１から出力されるセンサ座標系における角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］を座標変換することで、移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］を算出する。

具体的には、この補正は次の原理に基づくものである。センサ座標系における角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］、移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］とした場合、上述のように次式が成り立つ。

Ｃ_ｓ ^ｂは、移動体座標系を角速度センサ座標系に変換する回転行列であり、推定算出したミスアライメント角［Δφ，Δθ，Δψ］を用いて、次式で表すことができる。

なお、ミスアライメント角［Δφ，Δθ，Δψ］のそれぞれが、Δφ≪１［ｒａｄ］、Δθ≪１［ｒａｄ］、Δψ≪１［ｒａｄ］である場合には、回転行列Ｃ_ｓ ^ｂは、次式のように近似することができる。

座標変換部１３は、このような回転行列Ｃ_ｓ ^ｂを用いて、バイアス成分除去後のセンサ座標系における角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］を、移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］に変換する。

このように、ミスアライメント角による補正が行われるので、算出される移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］は高精度な値となる。また、上述のようにバイアス成分が除去されているので、角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］は、さらに高精度な値となる。

このような構成の角速度検出装置１は、図３に示すような移動状態検出装置１００に搭載させる。図３は本実施形態に係る角速度検出装置１を含む移動状態検出装置１００の構成を示すブロック図である。

移動状態検出装置１００は、上述の角速度検出装置１とともに、ＧＰＳ受信機１０１、加速度センサ１０２、加速度補正部１０３、旋回検出部１０４、位置演算部３０１、速度演算部３０２、姿勢角演算部３０３を備える。

ＧＰＳ受信機１０１は、ＧＰＳ衛星からのＧＰＳ信号を受信して、受信したＧＰＳ信号から既知の方法で測位を行い、ＧＰＳ位置データ、ＧＰＳ速度データ、およびＧＰＳ方位データを算出する。ＧＰＳ受信機１０１は、ＧＰＳ位置データを位置演算部３０１へ出力し、ＧＰＳ速度データを速度演算部３０２へ出力し、ＧＰＳ方位データを姿勢角演算部３０３へ出力する。

加速度センサ１０２は、ジャイロセンサ２０と同様に、移動体に対して所定の態様で設置されており、自身の座標系（加速度センサ座標系）において、センサ座標系の加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］を計測する。

加速度ａ_ｘ ^ｓは移動体の前後方向であるｘ軸の加速度である。加速度ａ_ｙ ^ｓは移動体の横方向であるｙ軸の加速度である。加速度ａ_ｚ ^ｓは移動体の上下方向の加速度である。

加速度センサ１０２は、計測したセンサ座標系における加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］を加速度補正部１０３へ出力する。

加速度補正部１０３は、センサ座標系における加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］に対して、加速度センサのミスアライメント角による誤差の補正、バイアス成分および雑音成分を除去する補正を行う。加速度補正部１０３は、補正後の加速度を移動体の座標系に変換し、移動体座標系における加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］を生成する。移動体座標系における加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］は速度演算部３０２へ出力される。また、移動体座標系におけるｚ軸方向（上下方向）の加速度ａ_ｚ ^ｂｅは、旋回検出部１０４に対しても出力される。

旋回検出部１０４は、ｙ軸方向（横方向）の加速度ａ _ｙ ^ｂｅの値が所定閾値以下であれば、移動体が平坦地に存在すると判断する。さらに、旋回検出部１０４は、直前に検出された方位角ψ方向の角速度ω_ｚ ^ｂｅの値が所定閾値以上であれば、移動体が旋回中であることを検出する。このような二つの条件を満たすものと判断すると、旋回検出部１０４は、移動体が旋回中であることを示す旋回検出結果を角速度検出装置１のミスアライメント角推定部１２へ出力する。

角速度検出装置１は、上述のように、ジャイロセンサ２０で計測したセンサ座標系における角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］に対して座標変換を行い、移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］を生成する。移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］は、姿勢角演算部３０３へ出力される。

速度演算部３０２は、ＧＰＳ速度データが入力されている間は、当該ＧＰＳ速度データを出力する。速度演算部３０２は、ＧＰＳ速度データが入力されなければ、入力された最終のＧＰＳ速度データを初期値として、加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］を積分することで、速度データを算出して出力する。なお、ＧＰＳ速度データが入力されている間であっても、加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］を用いて速度データを生成することもできる。この場合、速度演算部３０２は、前回出力した速度データに対して加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］を積分した値と今回のＧＰＳ速度データの値とを重み付け加算して、今回の速度データを算出すればよい。

姿勢角演算部３０３は、ＧＰＳ方位データが入力されている間は、当該ＧＰＳ方位データを出力する。姿勢角演算部３０３は、ＧＰＳ方位データが入力されなければ、入力された最終のＧＰＳ方位データを初期値として、角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］を積分することで、方位データを算出して出力する。なお、ＧＰＳ方位データが入力されている間であっても、角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］を用いて方位データを生成することもできる。この場合、方位演算部３０３は、前回出力した方位データに対して角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］を積分した値と今回のＧＰＳ方位データの値とを重み付け加算して、今回の方位データを算出すればよい。

位置演算部３０１は、ＧＰＳ位置データが入力されている間は、当該ＧＰＳ位置データを出力する。位置演算部３０１は、ＧＰＳ位置データが入力されなければ、入力された最終のＧＰＳ位置データを初期値として、加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］および角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］から得られる速度ベクトルを積分することで、位置データを算出して出力する。なお、ＧＰＳ位置データが入力されている間であっても、加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］および角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］を用いて位置データを生成することもできる。この場合、位置演算部３０１は、前回出力した位置データに対して加速度［ａ_ｘ ^ｂｅ，ａ_ｙ ^ｂｅ，ａ_ｚ ^ｂｅ］および角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］から得られる速度ベクトルを積分した値と今回のＧＰＳ位置データの値とを重み付け加算して、今回の位置データを算出すればよい。

以上のように、図３の構成を用いることで、移動体の位置、速度、姿勢角（方位）を算出することができる。そして、上述の角速度検出装置１を備えることで、姿勢角を高精度に検出でき、同様の方法で速度も高精度に検出できる。したがって、ＧＰＳ信号を受信できない状態になっても、移動体の位置、速度、姿勢角（方位）を、高精度に算出することができる。

このように高精度に算出された移動体の各種移動情報は、移動状態検出装置１が実装されるナビゲーション装置におけるナビゲーション処理等に利用される。このナビゲーション装置は、ルートナビゲーション処理を実行するナビゲーション処理部と、表示部と、当該表示部でも兼用可能な操作部とを少なくとも備え、例えば、操作部からの操作入力にしたがってナビゲーション部で、移動体の現在位置と目的位置から最適なルートを算出し、表示部上に当該ルートを表示する。そして、上述のように、移動体の移動情報を高精度に得ることができるので、ナビゲーション装置は、正確なナビゲーション処理を実現することができる。

次に、第２の実施形態に係る角速度検出装置について、図を参照して説明する。図４は実施形態に係る角速度検出装置１Ａの構成を示すブロック図である。本実施形態の角速度検出装置１Ａは、第１の実施形態に示した角速度検出装置１に対して、バイアス成分除去部１１が周波数解析部１１Ａに置き換わったものであり、他の構成は同じである。したがって、異なる点のみを詳細に説明する。

周波数解析部１１Ａは、センサ座標系における角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］を、ウェーブレット変換により周波数軸上の角速度成分群に変換する。より具体的には、周波数解析部１１Ａは、センサ座標系における角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］を、例えば、１秒毎に取得して、６４秒分記憶し、これら６４秒分のデータに基づいてウェーブレット変換を実行する。

周波数解析部１１Ａは、６４秒のサンプリング期間に対応する略定常成分（ＤＣ成分）、３２秒のサンプリング期間に対応する変動成分（ＡＣ成分）、１６秒のサンプリング期間に対応する変動成分（ＡＣ成分）、８秒のサンプリング期間に対応する変動成分（ＡＣ成分）、４秒のサンプリング期間に対応する変動成分（ＡＣ成分）、２秒のサンプリング期間に対応する変動成分（ＡＣ成分）、１秒のサンプリング期間に対応する変動成分（ＡＣ成分）を取得する。

周波数解析部１１Ａは、取得した極低域周波数の６４秒幅ＤＣ成分をバイアス周波数成分とし、中域周波数の８秒幅ＡＣ成分、４秒幅ＡＣ成分、および２秒幅ＡＣ成分を運動角速度周波数成分とし、１秒幅ＡＣ成分を雑音周波数成分と設定する。これら周波数帯の区分は、次に示す原理により設定される。

まず、６４秒幅で得られるＤＣ成分は、移動体の移動状態に依らず、角速度センサ２０から略定常的に出力されるものであると見なすことができるからである。次に、８秒幅、４秒幅および２秒幅で得られるＡＣ成分は、移動体の移動状態に大きく影響されるものであり、移動体の運動角速度に依存し易い成分であると見なすことができるからである。次に、１秒幅で得られるＡＣ成分は、移動体の運動角速度よりもよりランダム性を多く含むものであると見なすことができるからである。

周波数解析部１１は、ウェーブレット変換により得られた運動角速度周波数成分を誤差要因除去後の角速度［ω_ｘ ^ｓｅｆ，ω_ｙ ^ｓｅｆ，ω_ｚ ^ｓｅｆ］として、ミスアライメント角推定部１２および座標変換部１３へ出力する。

このような構成を用いることで、ジャイロセンサ２０で計測した角速度［ω_ｘ ^ｓ，ω_ｙ ^ｓ，ω_ｚ ^ｓ］からバイアス成分および雑音成分を除去して得られる角速度で、ミスアライメント角が推定されて補正される。したがって、より高精度な移動体座標系における角速度を算出することができる。

なお、上述の説明では、周波数解析部１１Ａでウェーブレット変換を行う例を示した。望ましくは、ウェーブレット変換を用いることがよいが、他の周波数変換処理、例えばフーリエ変換処理等を行ってもよく、さらには、通過周波数帯域の異なる複数のフィルタにより各周波数成分へ分解するようにしてもよい。

また、上述の説明では、角速度算出部１０を、バイアス成分除去部１１もしくは周波数解析部１１Ａと、ミスアライメント角推定部１２と、座標変換部１３とに機能的に分割した例を示したが、これらを一つの演算素子と実行プログラムにより実現してもよい。

また、上述の説明では、方位方向ミスアライメント角Δψを略「０」に設定し、推定を行っていないが、次に示す方法により、推定算出することができる。

方位方向ミスアライメント角Δψの推定は、勾配のある道路を直進する期間で行う。移動体が勾配のある道路を直進している場合、移動体座標系における角速度［ω_ｘ ^ｂｅ，ω_ｙ ^ｂｅ，ω_ｚ ^ｂｅ］のロール角φ方向成分ω_ｘ ^ｂｅ、方位角ψ方向成分ω_ｚ ^ｂｅは「０」と見なすことができる。すなわち、ω_ｘ ^ｂｅ＝０，ω_ｚ ^ｂｅ＝０となる。

したがって、式（４）は、次式のように表される。

この式により、勾配のある道路を走行中である場合、すなわち、ピッチ角θ方向におけるセンサ座標系における角速度ω_ｙ ^ｓｅが「０」でない場合に、方位方向ミスアライメント角Δψは、次式から算出される。

このように、ジャイロセンサ計測した角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］のみから方位方向ミスアライメント角Δψも推定算出することができる。

このように、方位方向ミスアライメント角Δψを算出できれば、すべての方向のミスアライメント角が推定算出できるので、ジャイロセンサ２０で検出した角速度［ω_ｘ ^ｓｅ，ω_ｙ ^ｓｅ，ω_ｚ ^ｓｅ］を、さらに高精度に補正することができる。

１，１Ａ−角速度検出装置、１０，１０Ａ−角速度算出部、１１−バイアス成分除去部、１１Ａ−周波数解析部、１２−ミスアライメント角推定部、１３−座標変換部、２０−ジャイロセンサ、１００−移動状態検出装置、１０１−ＧＰＳ受信機、１０２−加速度センサ、１０３−加速度補正部、１０４−旋回検出部、３０１−位置演算部、３０２−速度演算部、３０３−姿勢角演算部

Claims (14)

1. 移動体に装着され、該移動体の角速度を検出する角速度検出装置であって、
   センサ座標系における直交三軸の角速度を計測するジャイロセンサと、
   前記移動体が平坦地を旋回中に取得した前記センサ座標系における角速度のみを用いた演算によって、前記ジャイロセンサの直交三軸と前記移動体の直交三軸とのロール方向の角度差またはピッチ方向の角度差を推定するミスアライメント角推定部と、
   前記角度差に基づいて前記センサ座標系における角速度から移動体座標系における角速度へ座標変換する座標変換部と、
   を備えた角速度検出装置。
2. 請求項１に記載の角速度検出装置であって、
   前記角速度のバイアス成分を除去するバイアス成分除去部を備え、
   前記ミスアライメント角推定部および前記座標変換部は、バイアス成分除去後の角速度を用いて演算を行う、角速度検出装置。
3. 請求項２に記載の角速度検出装置であって、
   前記バイアス成分除去部は、
   前記ジャイロセンサから出力された角速度の時間平均値を用いて、該角速度を補正することで、バイアス成分を除去する、角速度検出装置。
4. 請求項１に記載の角速度検出装置であって、
   前記角速度を周波数解析し、バイアス成分および雑音成分の周波数成分を除去した周波数成分からなる角速度を算出する周波数解析部を備え、
   前記ミスアライメント角推定部および前記座標変換部は、前記周波数解析部から出力される角速度を用いて演算を行う、角速度検出装置。
5. 請求項４に記載の角速度検出装置であって、
   前記周波数解析部は、ウェーブレット変換を用いて周波数解析を行う、角速度検出装置。
6. 請求項１乃至請求項５のいずれかに記載の角速度検出装置であって、
   前記移動体の加速度および過去に検出した角速度に基づいて前記旋回中であることを検出する旋回検出部を備える角速度検出装置。
7. 請求項１乃至請求項６のいずれかに記載の角速度検出装置と、
   該角速度検出装置で検出した角速度を用いて、前記移動体の姿勢角を検出する姿勢角検出部と、
   前記移動体の加速度を検出する加速度センサと、
   前記測位用信号を受信して、前記移動体の位置、速度、方位を測定する測位部と、
   前記姿勢角検出部で得られた姿勢角、前記加速度センサで得られた加速度、前記測位部で得られた位置、速度、方位を用いて、前記移動体の位置、速度、姿勢角を含む移動状態を算出する移動状態算出部と、
   を備えた移動状態検出装置。
8. 請求項７に記載の移動状態検出装置と、
   前記移動状態を含むナビゲーション情報を通知するユーザ通知部と、を備えたナビゲーション装置。
9. 移動体の角速度を検出する角速度検出方法であって、
   前記移動体に装着されたジャイロセンサによって直交三軸の角速度を計測する計測工程と、
   前記移動体が平坦地を旋回中に取得した前記直交三軸の角速度のみを用いた演算によって、前記ジャイロセンサの直交三軸と前記移動体の直交三軸とのロール方向の角度差およびピッチ方向の角度差を推定するミスアライメント角推定工程と、
   前記角度差に基づいてセンサ座標系における角速度から移動体座標系における角速度へ座標変換する座標変換工程と、
   を有する角速度検出方法。
10. 請求項９に記載の角速度検出方法であって、
    前記角速度のバイアス成分を除去するバイアス成分除去工程を有し、
    前記ミスアライメント角推定工程および前記座標変換工程では、バイアス成分除去後の角速度を用いて演算を行う、角速度検出方法。
11. 請求項１０に記載の角速度検出方法であって、
    前記バイアス成分除去工程では、
    前記計測された角速度の時間平均値を用いて、該角速度を補正することで、バイアス成分を除去する、角速度検出方法。
12. 請求項９に記載の角速度検出方法であって、
    前記角速度を周波数解析し、バイアス成分および雑音成分の周波数成分を除去した周波数成分からなる角速度を算出する周波数解析工程を有し、
    前記ミスアライメント角推定工程および前記座標変換工程は、前記周波数解析工程で算出された角速度を用いて演算を行う、角速度検出方法。
13. 請求項１２に記載の角速度検出装置であって、
    前記周波数解析工程では、ウェーブレット変換を用いて周波数解析を行う、角速度検出方法。
14. 請求項９乃至請求項１３のいずれかに記載の角速度検出方法であって、
    前記移動体の加速度に基づいて前記旋回中であることを検出する旋回検出工程を有する角速度検出方法。

Images