JP5736106B2 - 移動状態検出装置 - Google Patents

Description

この発明は、移動体に設置され、当該移動体の速度やピッチ角等の移動状態を検出する移動状態検出装置に関するものである。

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

ところで、最近では、上述のような移動体に予め設置され、移動体の速度情報等を取得できるナビゲーション装置とは別に、ナビゲーション装置単体で速度情報を取得するなどしてナビゲーションを実行することができる、比較的小型で持ち運びが可能なパーソナルナビゲーション装置も各種考案されている。そして、このようなパーソナルナビゲーション装置を移動体に設置することで、元々移動体にナビゲーション装置が装着されていなくても、ナビゲーションシステムを利用することができる。

このようなパーソナルナビゲーション装置を用いる場合、移動体への取付角度すなわちパーソナルナビゲーション装置内の加速度センサの移動体への取付角度によっては、移動体の走行速度や走行方位を正確に得られず、ナビゲーションを正確に実行できないことがある。このため、特許文献１，２に示すような取付角度算出装置を設置し、加速度センサの取付角度を算出して、加速度を補正している。

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

しかしながら、上述の特許文献１，２に示した取付角度算出装置では、加速度センサの出力値のみで取付角度を検出するものではなく、ジャイロセンサや角速度センサからの角速度や、上述の移動体から得られる車速パルス等を利用している。また、加速度センサから出力される加速度は、バイアス成分やノイズ成分等の不要成分が含まれているが、これらについては考慮していない。したがって、バイアス成分やノイズ成分による誤差を含む加速度を用いて取付角度を算出するため、取付角度を精度良く算出することができない。このため、加速度センサからの加速度が精度良く補正されず、移動体の走行速度や走行方位を正確に得ることができなくなってしまう。

本発明の目的は、加速度センサから出力される加速度に含まれるバイアス成分やノイズ成分等の不要成分による影響を除去し、加速度センサの取付角度を精度良く算出し、当該加速度センサからの加速度を精度良く補正できる、移動状態検出装置を実現することにある。

この発明の移動状態検出装置は、加速度センサと加速度補正部とを備える。加速度センサは、移動体に設置され、複数軸成分の加速度を検出する。加速度補正部は、加速度センサから得られる複数軸成分における横方向の加速度から移動体の直進走行を検出し、複数軸成分の加速度を周波数帯域毎に分解し、所定周波数帯域成分からなる重力周波数成分および運動加速度周波数成分に基づいて前記加速度センサの少なくともピッチ方向の取付角度を直進走行中に推定して、該取付角度に基づいて加速度を補正する。

より具体的に、この発明の移動状態検出装置は、加速度センサと加速度補正部とを備える。加速度センサは、移動体に設置され、当該移動体の加速度を検出する。加速度補正部は、加速度センサから得られる加速度を低周波数側から順次周波数帯域毎にバイアス周波数成分、重力周波数成分、運動加速度周波数成分、および雑音周波数成分に分解し、重力周波数成分および運動加速度周波数成分に基づいて加速度センサの少なくともピッチ方向の取付角度を直進走行中に推定して、該取付角度に基づいて加速度を補正する。

この構成では、加速度センサで得られる加速度が周波数成分毎に分解される。ここで、バイアス周波数成分は、移動体の移動状態によらず略定常的に出力される成分であり、極低周波数からなる。雑音周波数成分は、移動体の移動状態によらずランダムに変動し続ける成分であり、高い周波数からなる。そして、これらとは別に、重力周波数成分および運動加速度周波数成分は移動体の移動状態に依存し、バイアス周波数成分よりも変動が大きいので、バイアス周波数成分よりも高い周波数成分となり、雑音周波数成分ほどのランダム性を有さないので、雑音周波数成分よりも低い周波数成分となる。このため、加速度を周波数成分毎に分解することで、移動体の移動状態に依存しない、バイアス周波数成分や雑音周波数成分を含む不要成分を除去することができる。これにより、移動体の移動状態に依存する重力周波数成分および運動加速度周波数成分のみから構成される正確な加速度を得ることができる。

また、この発明の移動状態検出装置の加速度補正部は、加速度をウェーブレット変換により周波数分解する。

この構成では、加速度の各周波数成分を得るためにウェーブレット変換を用いることで、周波数軸上に展開した各周波数成分を単に得られるだけでなく、各周波数成分の時間軸上における遷移状態等も簡単に得ることができる。

また、この発明の移動状態検出装置の加速度センサは、それぞれに直交する前後方向加速度成分、横方向加速度成分、および上下方向加速度成分で加速度成分を検出する。加速度補正部は、前後方向加速度成分と上下方向加速度成分とによるピッチ方向取付角度の直進走行中での推定を実行する。

また、この発明の移動状態検出装置の加速度センサは、それぞれに直交する前後方向加速度成分、横方向加速度成分、および上下方向加速度成分で加速度成分を検出する。加速度補正部は、前後方向加速度成分と横方向加速度成分とにより方位方向取付角度を直進走行中に推定し、前後方向加速度成分と上下方向加速度成分とによりピッチ方向取付角度を直進走行中に推定し、前後方向加速度成分、横方向加速度成分、および上下方向加速度成分とピッチ方向取付角度とによりロール方向取付角度を旋回走行中に推定する。

これらの構成では、具体的に直交する３軸の加速度成分を得て、加速度センサの立体的な取付角度を推定算出する場合を示している。

また、この発明の移動状態検出装置の加速度補正部は、横方向加速度成分が所定閾値を未満の場合に、ピッチ方向取付角度を推定する。

また、この発明の移動状態検出装置の加速度補正部は、横方向加速度成分が所定閾値を未満の場合に方位方向取付角度を推定する。

また、この発明の移動状態検出装置の加速度補正部は、横方向加速度成分が所定閾値以上の場合に、ロール方向取付角度を推定する。

これらの構成では、取付角度の算出の具体的な方法を示すものであり、横方向加速度成分に基づく移動体の移動状態の遷移に応じて、方位方向取付角度、ピッチ方向取付角度、ロール方向取付角度を推定する。

また、この発明の移動状態検出装置の加速度補正部は、推定した取付角度を順次記憶するとともに取付角度を時間平均処理により算出する。この際、加速度補正部は、今回の推定した取付角度が直前に推定した取付角度に対して所定角度以上変化した場合、時間平均処理における過去の取付角度の重みを低くする。

この構成では、時間平均処理を行うことで、推定した取付角度の誤差成分が抑圧される。例えば、通常の市街地等を走行する車両であれば、走行経路の殆どが平坦地であることが多いので、登り勾配や下り勾配時に推定された勾配角の影響を受けた取付角度による誤差成分が、時間平均処理により抑圧される。これにより、取付角度をより高精度に算出することができる。この上で、ユーザが強制的に取付角度を変更した場合等により、取付角度が大きく変化すると、過去に推定算出した取付角度の影響を抑圧することができる。

また、この発明の移動状態検出装置は、加速度補正部で補正された加速度に基づいて、移動体の速度、上下方向傾き角である走行ピッチ角の少なくとも一方を算出する速度・走行角算出部を備えている。

この構成では、移動状態検出装置の具体的構成例を示すものであり、上述のように精度良く得られた移動体の加速度から、移動体の速度や走行角を精度良く算出することができる。

また、この発明の移動状態検出装置の速度・走行角算出部は、走行ピッチ角が算出される場合に、重力周波数成分の重力加速度による除算値を算出し、該除算値により、算出した走行ピッチ角を補正する。

この構成では、上述の時間平均処理を用いた場合、当該時間平均処理による積算誤差が生じてしまう可能性がある。そのため、加速度センサによる加速度の重力周波数成分と重力加速度とだけから得られる走行ピッチ角によって、時間平均処理から得られる走行ピッチ角を補正することで、上述の積算誤差を抑圧し、継続的に高精度な走行ピッチ角を算出することができる。

また、この発明の移動状態検出装置は、雑音周波数成分が算出される場合に、運動加速度周波数成分および雑音周波数成分の和が所定閾値未満であることを検出して移動体の停止を検出する停止検出部を備えている。

この構成では、上述の移動体の移動状態の一態様として停止状態を正確に検出することができる。

また、この発明のナビゲーション装置は、上述の移動状態検出装置を備え、該移動状態検出装置が算出した移動体の移動に関する情報に基づいて自身の位置算出やナビゲーション処理を実行する。

この構成では、上述のように移動状態検出装置で高精度な加速度や速度、走行ピッチ角が得られるので、これらを用いて、正確なナビゲーション処理を実行することができる。

この発明によれば、周波数成分毎に分解することで高精度に加速度を得ることができるので、加速度センサの取付角度を精度良く算出し、加速度を高精度に補正することができる。これにより、移動体の速度や走行角を高精度の検出することができる。

第１の実施形態の移動状態検出装置１の主要構成を示すブロック図である。 加速度センサ２０の移動体に対する取付角度を説明する図である。 本実施形態の構成と従来の構成による上下方向の加速度の遷移を示すグラフである。 第２の実施形態の移動状態検出装置１’の主要構成を示すブロック図である。 第３の実施形態の移動状態検出装置１００の主要構成を示すブロック図である。

本発明の第１の実施形態に係る移動状態検出装置について、図を参照して説明する。本実施形態に係る移動状態検出装置は、車載用ナビゲーション装置やＰＮＤ（Personal Navigation Device）等の各種ナビゲーション装置に利用されるものである。
図１は、本実施形態の移動状態検出装置１の主要構成を示すブロック図である。
図１に示すように、移動状態検出装置１は、加速度センサ２０、加速度補正部１０、速度・走行角算出部３０を備え、車両等の移動体に固定的に設置される。なお、本実施形態の移動状態検出装置１は、移動体の速度および走行角（走行ピッチ角、走行ロール角、走行方位角（ヨー角））を検出するものであるが、例えば、移動体の加速度のみを出力するものであれば、速度・走行角算出部３０を省略しても良い。

加速度センサ２０は、自身の座標系（センサ座標系）において、移動体の前後方向であるｘ軸方向に対応する加速度成分ａ_ｘ ^ｓと、移動体の横方向であるｙ軸方向に対応する加速度成分ａ_ｙ ^ｓと、移動体の上下方向であるｚ軸方向に対応する加速度成分ａ_ｚ ^ｓとからなる、センサ座標系加速度［ａ _ｘ ^ｓ ，ａ _ｙ ^ｓ ，ａ _ｚ ^ｓ ］を検出する。

ここで、加速度センサ２０は、移動体に対して、方位方向取付角度Δψ、ピッチ方向取付角度Δθ、ロール方向取付角度Δφからなる取付角度［Δψ，Δθ，Δφ］で取り付けられているものとする。そして、取付角度［Δψ，Δθ，Δφ］は、図２に示す座標系に準じたものである。図２は移動体の座標系を説明する図である。

図２に示すように、移動体の座標系は、移動体の前後方向を軸方向とするｘ軸と、移動体の横方向を軸方向とするｙ軸と、移動体の上下方向を軸方向とするｚ軸とからなる直交３軸の座標系である。そして、ｘ軸を中心軸として回転する方向をロール角（φ）方向とし、ｙ軸を中心軸として回転する方向をピッチ角（θ）方向とし、ｚ軸を中心軸として回転する方向を方位角（ψ）方向としている。

このように、加速度センサ２０が移動体に対して取付角度［Δψ，Δθ，Δφ］で取り付けられているので、加速度センサ２０から出力されるセンサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］の各成分と移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］の各成分との間には、取付角度［Δψ，Δθ，Δφ］に基づく立体角に応じた差が生じる。

加速度補正部１０は、周波数解析部１１、取付角度推定部１２、補正演算部１３を備える。

周波数解析部１１は、センサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］を、ウェーブレット変換により周波数軸上の加速度成分群に変換する。より具体的には、周波数解析部１１は、センサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］を、例えば、１秒毎に取得して、６４秒分記憶し、これら６４秒分のデータに基づいてウェーブレット変換を実行する。そして、周波数解析部１１は、６４秒のサンプリング期間に対応する略定常成分（ＤＣ成分）、３２秒のサンプリング期間に対応する変動成分（ＡＣ成分）、１６秒のサンプリング期間に対応する変動成分（ＡＣ成分）、８秒のサンプリング期間に対応する変動成分（ＡＣ成分）、４秒のサンプリング期間に対応する変動成分（ＡＣ成分）、２秒のサンプリング期間に対応する変動成分（ＡＣ成分）、１秒のサンプリング期間に対応する変動成分（ＡＣ成分）を取得する。

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

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

周波数解析部１１は、ウェーブレット変換により得られた重力周波数成分と運動加速度周波数成分との和を検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］として、取付角度推定部１２および補正演算部１３へ出力する。

取付角度推定部１２は、検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］から取付角度［Δψ，Δθ，Δφ］を推定算出する。この取付角度の推定は、上述のセンサ座標系加速度の取得タイミングに応じて、例えば１秒毎に行ってもよく、また、センサ座標系加速度の各周波数成分をバッファリングしながら、適宜設定したタイミング毎に行ってもよい。

ここで、取付角度［Δψ，Δθ，Δφ］の推定算出原理について説明する。センサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］、移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］、取付角度［Δψ，Δθ，Δφ］とした場合、次式が成り立つ。

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

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

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

ここで、周波数解析部１１によってバイアス周波数成分および雑音周波数成分を除去した検出用加速度を用いると、上記式（４）は、次式となる。

ところで、移動体が平坦地を直進走行している場合、移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］の横方向成分ａ_ｙ ^ｂ、上下方向成分ａ_ｚ ^ｂは「０」と見なすことができる。すなわち、ａ_ｙ ^ｂ＝０，ａ_ｚ ^ｂ＝０となる。

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

これにより、前後方向成分ａ_{ｘ（ＡＧ）} ^ｓが「０」でない場合に、方位方向取付角度Δψ、ピッチ方向取付角度Δθは、次式から算出される。

また、移動体が平坦地を旋回走行している場合、移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］の上下方向成分ａ_ｚ ^ｂは「０」と見なすことができ、前後方向成分ａ_ｘ ^ｂと横方向成分ａ_ｙ ^ｂとは、略同じ大きさに見なすことができる。そして、上述の近似条件のΔψ≪１［ｒａｄ］を用いることで、式（５）は、次式のように表される。

これにより、ロール方向取付角度Δφは、上述の式（７）で算出したピッチ方向取付角度Δθも用いて、次式から算出される。

以上のように、検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］のみから取付角度［Δψ，Δθ，Δφ］を推定算出することができる。

ところで、上述のように平坦地であることが算出処理の前提となっているが、次に示す時間平均処理を行うことにより、平坦地であるという条件を解除することができる。これは、通常の市街地走行においては、登り勾配もしくは下り勾配を有する傾斜地を走行する期間よりも、平坦地を走行する期間の方が十分に長いからである。この設定に従い、方位方向取付角度Δψ、ピッチ方向取付角度Δθ、およびロール方向取付角度Δφは、次式から算出される。なお、次式において、Ｅ［演算式］は、時間平均処理を示す演算子である。

また、上述の算出処理では、直進走行か旋回走行かの選択条件が設定されているが、これは、横方向加速度ａ_{ｙ（ＡＧ）} ^ｓに対して予め旋回検出用の閾値を設定し、当該横方向加速度ａ_{ｙ（ＡＧ）} ^ｓが当該閾値以上である場合に旋回走行と判定し、閾値未満である場合に直進走行であると判定すればよい。もしくは、後述する補正演算部１３によって算出される移動体座標系における横方向加速度ａ_ｙ ^ｂに対して予め旋回検出用の閾値を設定し、当該横方向加速度ａ_ｙ ^ｂが当該閾値以上である場合に旋回走行と判定し、閾値未満である場合に直進走行であると判定すればよい。

上述のような原理に基づいて、取付角度推定部１２は、上述の式（７Ａ）、式（７Ｂ）、式（１０）から、所定タイミング毎に取付角度［Δψ，Δθ，Δφ］を算出し、次に示すカルマンフィルタを用いて時間平均処理に相当する処理を行った上で、推定算出した取付角度［Δψ，Δθ，Δφ］を補正演算部１３へ出力する。

ここでは、方位方向取付角度Δψに対して、１次のＬＰＦを用いてカルマンフィルタ処理を行う場合を例に示すが、他のピッチ方向取付角度Δθおよびロール方向取付角度Δφについても同様にカルマンフィルタ処理を行う。

あるタイミングｔでの方位方向取付角度ΔψをΔψ［ｔ］とし、この次のタイミングの方位方向取付角度ΔψをΔψ［ｔ＋１］とすることで、次式を設定することができる。

ただし、ａｘ（ＡＧ）ｓ＝０もしくは旋回時には、Δψ［ｔ＋１］＝Δψ［ｔ］とする。

このような演算式を用いることで、方位方向取付角度Δψや、ピッチ方向取付角度Δθおよびロール方向取付角度Δφが時間平均処理され、平坦地走行の条件を解除して、取付角度［Δψ，Δθ，Δφ］を推定算出することができる。この際、上述のように、取付角度推定部１２には、バイアス周波数成分および雑音周波数成分が除去された加速度が与えられているので、高精度に取付角度［Δψ，Δθ，Δφ］を推定算出することができる。

なお、ここで、αはウェイト値であり、適宜設定することができるが、次に示す条件に基づいてウェイト値αを設定すると、更なる効果を得ることができる。ここでも、方位方向取付角度Δψのみについて説明するが、ピッチ方向取付角度Δθおよびロール方向取付角度Δφについても同様に効果を得ることができる。

ウエイト値αに対して、センサ座標系の前後方向加速度ａ_{ｘ（ＡＧ）} ^ｓが「０」でなく、直進状態で、且つ（−ａ_{ｙ（ＡＧ）} ^ｓ／ａ_{ｘ（ＡＧ）} ^ｓ−Δψ［ｔ］）が取付角度変更検出用の閾値β未満である場合には、α_１を設定する。また、ウエイト値αに対して、センサ座標系の前後方向加速度ａ_{ｘ（ＡＧ）} ^ｓが「０」でなく、直進状態で、且つ（−ａ_{ｙ（ＡＧ）} ^ｓ／ａ_{ｘ（ＡＧ）} ^ｓ−Δψ［ｔ］）が取付角度変更検出用の閾値β以上である場合には、α_２を設定する。ここで、α_１，α_２は、０＜α_１≦α_２＜１となるように設定されている。

このようなウエイト値αの設定を行うことで、前回推定算出して時間平均処理した取付角度に対して、今回算出した取付角度が大きく変化していなければ、過去の算出結果に基づく取付角度の影響を大きく受けるように時間平均処理が行われる。一方で、前回推定算出して時間平均処理した取付角度に対して、今回算出した取付角度が大きく変化していれば、過去の算出結果に基づく取付角度の影響を受け難くするように時間平均処理が行われる。これにより、移動状態検出装置１すなわち加速度センサ２０の取付角度が変更されなければ、過去の安定した取付角度をも用いて、より高精度な取付角度の推定算出を行うことができ、取付角度がユーザ等により強制的に変更されたような場合でも、この変更による影響を抑圧しながら、取付角度の推定算出を継続的に行うことができる。

補正演算部１３は、取付角度推定部１２で推定算出された取付角度［Δψ，Δθ，Δφ］に基づいて、周波数解析部１１から出力される検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］を補正することで、移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］を算出して出力する。

具体的には、この補正は次の原理に基づくものである。検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］、移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］とした場合、次式が成り立つ。

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

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

補正演算部１３は、このような回転行列Ｃ_ｓ ^ｂを用いて、検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］を、移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］に変換して出力する。

このような補正処理を行う際、周波数解析部１１からの検出用加速度はバイアス周波数成分および雑音成分が除去されており、取付角度は高精度に推定算出されているので、算出される移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］も高精度な値とすることができる。

速度・走行角算出部３０は、次に示す原理に基づいて、移動体の走行速度および走行角を算出する。なお、以下の説明では、移動体の前後方向の走行速度ｖ_ｘおよび上下方向の走行角である走行ピッチ角θのみを算出する例を示すが、他の方向の走行速度や走行角についても同様の原理を用いることで算出することができる。

移動体座標系の加速度の前後方向成分ａ_ｘ ^ｂ、上下方向成分ａ_ｚ ^ｂと、前後方向の走行速度ｖ_ｘと、走行ピッチ角θとは、重力加速度をｇとした場合に、以下の関係となる。

ここで、移動体座標系の加速度の上下方向成分ａ_ｚ ^ｂは、上述のようにウェーブレット変換によりバイアス周波数成分が除去された検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］により算出されているので、式（１５Ｂ）は、次式で表すことができる。

また、同様に、移動体座標系の加速度の前後方向成分ａ_ｘ ^ｂも、上述のようにウェーブレット変換によりバイアス周波数成分が除去された検出用加速度［ａ_{ｘ（ＡＧ）} ^ｓ，ａ_{ｙ（ＡＧ）} ^ｓ，ａ_{ｚ（ＡＧ）} ^ｓ］により算出されているので、式（１５Ａ）は、次式で表すことができる。

したがって、式（１６）、式（１７）からなる連立微分方程式を演算することで、前後方向の走行速度ｖ_ｘと、走行ピッチ角θを算出することができる。

これを実現するため、速度・走行角算出部３０は、次式に示すカルマンフィルタ処理を実行する。なお、次式におけるｔは時刻（演算タイミング）であり、Δｔは更新時間間隔を示す。

このような算出処理を行うことで、移動体の走行速度および走行角を算出することができる。この際、上述のように高精度に得られた移動体座標系の加速度を用いることで、走行速度および走行角も高精度に算出することができる。

このような処理を行うことで、次の実験結果に示すように、移動体が平坦地および勾配を有する傾斜地を走行した場合に、平坦地を走行中であるか傾斜地を走行中であるかを正確に識別することができる。図３は、上下方向の加速度の遷移を示すグラフであり、図３（Ａ）は本実施形態の構成を用いた場合の上下方向加速度の遷移を示し、図３（Ｂ）は従来の構成を用いた場合の上下方向加速度の遷移を示す。なお、本グラフにおいて、約２４０秒〜約３５０秒の区間および約４４０秒〜約５５０秒の区間が傾斜地を走行した時間帯である。

従来の構成では、図３（Ｂ）に示すように傾斜地の走行を明確に検出することができないが、本実施形態の構成を用いることで、図３（Ａ）に示すように、傾斜地の走行期間を従来よりも明確に識別することができる。

なお、上述の説明では、前後方向の走行速度ｖ_ｘと、走行ピッチ角θを式（１８Ａ）、（１８Ｂ）から算出する例を示したが、この方法では積算誤差が生じて、精度が劣化する可能性がある。このため、速度・走行角算出部３０は、周波数解析部１１からセンサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］の前後方向加速度ａ_ｘ ^ｓにおける重力周波数成分ａ_ｘ（Ｇ） ^ｓを取得する。そして、速度・走行角算出部３０は、重力周波数成分を重力加速度ｇで除算することで、補正用走行ピッチ角θｃを算出する。

速度・走行角算出部３０は、上述の式（１８Ａ）、（１８Ｂ）で得られる走行ピッチ角θと、補正用走行ピッチ角θｃとを重み付け平均処理することで、出力する走行ピッチ角を算出する。このような処理を行うことで、積算誤差が蓄積されることを防止でき、長期的な演算処理による精度の劣化を抑制することができる。なお、この重み付け平均処理を行うタイミングは、走行ピッチ角θの算出タイミング毎でもよいが、より長い所定タイミング間隔に適宜設定すればよい。

次に、第２の実施形態に係る移動状態検出装置について図を参照して説明する。
図４は本実施形態の移動状態検出装置１’の主要構成を示すブロック図である。
本実施形態の移動状態検出装置１’は、第１の実施形態に示した移動状態検出装置１に対して停止検出部４０が加えられたものであり、以下では、当該停止検出部４０およびこれに関連する箇所のみを説明する。

停止検出部４０は、周波数解析部１１から、センサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］における運動加速度周波数成分と雑音周波数成分との和からなる停止検出用加速度［ａ_ｘ（AN) ^ｓ，ａ_ｙ（AN) ^ｓ，ａ_ｚ（AN) ^ｓ］を取得する。停止検出部４０は、停止検出用加速度［ａ_ｘ（AN) ^ｓ，ａ_ｙ（AN) ^ｓ，ａ_ｚ（AN) ^ｓ］の前後方向成分ａ_ｘ（AN) ^ｓおよび横方向成分ａ_ｙ（AN) ^ｓに対して予め停止検出用の閾値（≠０）を記憶している。停止検出部４０は、停止検出用加速度の前後方向成分ａ_ｘ（AN) ^ｓおよび横方向成分ａ_ｙ（AN) ^ｓがともに停止検出用の閾値以下であることを検出すると、移動体が停止しているものと判断し、停止検出データを速度・走行角算出部３０へ出力する。速度・走行角算出部３０は、停止検出データを取得すると、前後方向の走行速度ｖ_ｘ＝０に設定するとともに、当該設定に応じて上述の式（１８Ｂ）に基づいて走行ピッチ角θを算出する。

このような構成および処理を用いることで、移動体の停止を簡素な処理で識別することができる。

次に、第３の実施形態に係る移動状態検出装置について図を参照して説明する。
図５は、本実施形態の移動状態検出装置１００の主要構成を示すブロック図である。
本実施形態の移動状態検出装置１００は、第１の実施形態に示した移動状態検出装置１の構成による速度や走行角のみでなく、ＧＰＳ受信機１０２からの各種データや、ジャイロセンサ１０１からの角速度データに基づいて、移動体の位置および移動方位を算出するものである。したがって、以下の説明では、第１の実施形態に示した移動状態検出装置１の構成と同じ箇所については説明を省略し、異なる箇所のみを説明する。

移動状態検出装置１００は、移動状態検出装置１の構成とともに、方位算出部５０と位置算出部６０とを備え、ジャイロセンサ１０１とＧＰＳ受信機１０２とに接続している。

ジャイロセンサ１０１は、少なくとも方位方向の回転を検出可能な構成からなり、方位方向の角速度を検出し、方位検出部５０へ与える。

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

速度・走行角算出部３０’は、ＧＰＳ速度データを取得している期間では、ＧＰＳ速度データの微分値を用いて、上述の式（１８Ａ），（１８Ｂ）から走行角を算出する。もしくは、速度・走行角算出部３０’は、ＧＰＳ速度データの微分値と、加速度補正部１０から与えられる移動体座標系加速度［ａ_ｘ ^ｂ，ａ_ｙ ^ｂ，ａ_ｚ ^ｂ］との重み付け平均値等を用いて、上述の式（１８Ａ），（１８Ｂ）から走行角を算出する。一方、速度・走行角算出部３０’は、ＧＰＳ速度データが取得できなくなると、ＧＰＳ速度データを用いて最後に出力した速度データを初期値にして、上述の式（１８Ａ），（１８Ｂ）から速度および走行角を算出する。

方位算出部５０は、ＧＰＳ方位データを取得している期間では、ＧＰＳ方位データをそのまま出力する。もしくは、方位算出部５０は、ジャイロセンサ１０１からの角速度データを積分して前回の出力した方位データに積算した値と、ＧＰＳ方位データとの重み付け平均値を算出して出力してもよい。一方、方位算出部５０は、ＧＰＳ方位データが取得できなくなると、ＧＰＳ方位データを用いて最後に出力した方位データを初期値にして、角速度データを積分して積算することで方位データを算出する。

位置算出部６０は、ＧＰＳ位置データを取得している期間では、ＧＰＳ位置データをそのまま出力する。もしくは、位置算出部６０は、速度・走行角算出部３０による速度データ、走行角データ、および方位算出部５０による方位データから演算した速度ベクトルを積分して前回の出力した位置データに加算した値と、ＧＰＳ位置データとの重み付け平均値を算出して出力してもよい。

一方、位置算出部６０は、ＧＰＳ位置データが取得できなくなると、ＧＰＳ位置データを用いて最後に出力した位置データを初期値にして、速度・走行角算出部３０による速度データ、走行角データ、および方位算出部５０による方位データから演算した速度ベクトルを積分して積算することで位置データを算出する。

このような構成とすることで、移動体の位置、速度、走行角、方位を高精度に算出することができる。

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

なお、本実施形態では、第１の実施形態の構成に本実施形態の構成を適用した例を示したが、第２の実施形態の構成に対しても、同様に本実施形態の構成を適用することができる。

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

また、上述の説明では、加速度補正部１０を、周波数解析部１１、取付角度推定部１２、補正演算部１３に機能的に分割した例を示したが、これらを一つの演算素子と実行プログラムにより実現してもよい。さらには、加速度補正部１０と速度・走行角算出部３０，３０’を一つの演算素子と実行プログラムで実現しても、これらに加え、停止検出部４０をも含むように、一つの演算素子と実行プログラムで実現してもよい。

また、上述の説明では、三次元の取付角度［Δψ，Δθ，Δφ］を推定算出する例を示したが、必要に応じて、これら取付角度［Δψ，Δθ，Δφ］の各要素（方位方向取付角度Δψ、ピッチ方向取付角度Δθ、ロール方向取付角度Δφ）の少なくとも１つを推定算出するようにしてもよい。

また、上述の説明では、走行速度ｖ_ｘと、走行ピッチ角θを算出する例を示したが、これらについても、必要に応じて少なくとも一方を算出するようにしてもよい。

また、上述の説明では、センサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］を、バイアス周波数成分、重力周波数成分、運動加速度周波数成分、および雑音周波数成分の四成分に分解するして、推定演算に利用する例を示したが、取付角度の推定算出には、センサ座標系加速度［ａ_ｘ ^ｓ，ａ_ｙ ^ｓ，ａ_ｚ ^ｓ］における少なくとも重力周波数成分と運動加速度周波数成分とを抽出して利用するようにすればよい。この場合、例えば重力周波数成分と運動加速度周波数成分とに対応する周波数帯域を通過帯域とするバンドパスフィルタを用いればよい。そして、このバンドパスフィルタを用いる場合には、走行速度等の移動体に移動状態に応じて通過帯域を可変する等の処理を行ってもよい。

１，１’，１００−移動状態検出装置、１０−加速度補正部、１１−周波数解析部、１２−取付角度推定部、１３−補正演算部、２０−加速度センサ、３０−速度・走行角算出部、４０−停止検出部、５０−方位算出部、６０−位置算出部、１０１−ジャイロセンサ、１０２−ＧＰＳ受信機

Claims (13)

1. 移動体に設置された複数軸成分の加速度を検出する加速度センサと、
   前記加速度センサから得られる前記複数軸成分における横方向の加速度から前記移動体の直進走行を検出し、前記複数軸成分の加速度を周波数帯域毎に分解し、所定周波数帯域成分からなる重力周波数成分および運動加速度周波数成分に基づいて前記加速度センサの少なくともピッチ方向の取付角度を直進走行中に推定して、該取付角度に基づいて前記加速度を補正する加速度補正部と、
   を備えた移動状態検出装置。
2. 前記加速度補正部は、
   前記加速度センサから得られる前記複数軸成分の加速度を、周波数帯域毎にバイアス周波数成分、前記重力周波数成分、前記運動加速度周波数成分、および雑音周波数成分に分解し、前記重力周波数成分および前記運動加速度周波数成分に基づいて前記加速度センサの少なくともピッチ方向の取付角度を直進走行中に推定して、該取付角度に基づいて前記加速度を補正する、
   請求項１に記載の移動状態検出装置。
3. 前記加速度補正部は、前記加速度をウェーブレット変換により周波数分解する、請求項１または請求項２に記載の移動状態検出装置。
4. 前記加速度センサは、それぞれに直交する前後方向加速度成分および上下方向加速度成分で加速度成分を検出し、
   前記加速度補正部は、
   前記前後方向加速度成分と前記横方向加速度成分とにより方位方向取付角度を推定し、
   前記前後方向加速度成分と前記上下方向加速度成分とによりピッチ方向取付角度を推定し、
   前記前後方向加速度成分、前記横方向加速度成分、および前記上下方向加速度成分と前記ピッチ方向取付角度を直進走行中に推定する、請求項１または請求項３に記載の移動状態検出装置。
5. 前記加速度センサは、それぞれに直交する前後方向加速度成分、横方向加速度成分、および上下方向加速度成分で加速度成分を検出し、
   前記加速度補正部は、
   前記前後方向加速度成分と前記横方向加速度成分とにより方位方向取付角度を直進走行中に推定し、
   前記前後方向加速度成分と前記上下方向加速度成分とによりピッチ方向取付角度を直進走行中に推定し、
   前記前後方向加速度成分、前記横方向加速度成分、および前記上下方向加速度成分と前記ピッチ方向取付角度とによりロール方向取付角度を旋回走行中に推定する、請求項１〜請求項３のいずれかに記載の移動状態検出装置。
6. 前記加速度補正部は、
   前記横方向加速度成分が所定閾値を未満の場合に、前記ピッチ方向取付角度を推定する請求項４に記載の移動状態検出装置。
7. 前記加速度補正部は、
   前記横方向加速度成分が所定閾値を未満の場合に、前記方位方向取付角度を推定する請求項５または請求項６に記載の移動状態検出装置。
8. 前記加速度補正部は、
   前記横方向加速度成分が前記所定閾値以上の場合に、前記ロール方向取付角度を推定する、請求項５〜請求項７のいずれかに記載の移動状態検出装置。
9. 前記加速度補正部は、
   推定した前記取付角度を順次記憶するとともに、前記取付角度を時間平均処理により算出し、
   今回の推定した取付角度が直前に推定した取付角度に対して所定角度以上変化した場合、前記時間平均処理における過去の取付角度の重みを低くする、請求項１〜請求項８のいずれかに記載の移動状態検出装置。
10. 前記加速度補正部で補正された加速度に基づいて、前記移動体の速度、上下方向傾き角である走行ピッチ角の少なくとも一方を算出する速度・走行角算出部を備えた、請求項１〜請求項９のいずれかに記載の移動状態検出装置。
11. 前記速度・走行角算出部は、前記走行ピッチ角が算出される場合に、前記重力周波数成分の重力加速度による除算値を算出し、該除算値により、算出した走行ピッチ角を補正する、請求項１０に記載の移動状態検出装置。
12. 前記雑音周波数成分が算出される場合に、前記運動加速度周波数成分および前記雑音周波数成分の和が所定閾値未満であることを検出して前記移動体の停止を検出する停止検出部を備えた、請求項２〜請求項１１のいずれかに記載の移動状態検出装置。
13. 請求項１〜請求項１２のいずれかに記載の移動状態検出装置を備え、
    該移動状態検出装置が算出した前記移動体の移動に関する情報に基づいて自身の位置やナビゲーション処理を実行するナビゲーション装置。

Images