JP2005156185A - 速度算出装置 - Google Patents

Abstract

【課題】 より精度の良い移動速度を算出可能な速度算出装置を提供すること
【解決手段】 速度算出装置では、角速度センサ１により、移動体の３軸周りの角速度ｐ，ｑ及びｒが、加速度センサ２により、２軸方向への加速度ｇｘ及びｇｙが検出される。第１の傾斜角算出部３は、加速度ｇｘ及びｇｙから、静止中の移動体のピッチ角θ₂（０）及びロール角θ₃（０）を算出する。角速度補正部４は、角度θ₂（０）及びθ₃（０）と、第２の傾斜角算出部５からの角度θ₂及びθ₃とから、角速度ｐ、ｑ及びｒを補正して、補正値α、β及びγを算出する。第２の傾斜角度算出部５は、補正値α、β及びγを使って、移動体の現在の角度θ₂及びθ₃を算出する。加速度算出部６は、角度θ₂及びθ₃と、加速度ｇｘ及びｇｙとを使って、移動体に加わる前後方向への加速度Ａｘを算出する。速度算出部７は、加速度Ａｘを積算して、移動体の速度Ｖを算出する。
【選択図】 図１

Description

本発明は、速度算出装置に関し、より特定的には、自身が設置される移動体の速度を算出する速度算出装置に関する。

ナビゲーションシステムは、車両に代表される移動体に搭載され、多くの場合、電波航法及び自律航法の組み合わせ（いわゆるハイブリッド航法）により、車両の現在位置を算出する。電波航法において、ナビゲーションシステムは基本的に、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）のような測位システムに収容される人工衛星からの情報を使って、車両の絶対位置を算出する。しかし、電波航法の場合、例えば、地下、屋内、トンネル又は都心のビル街のように電波を受信できない場所では、ナビゲーションシステムは、正確な位置を算出することができなくなる。

ナビゲーションシステムは、人工衛星からの電波を受信できない場所で、自律航法を行う。自律航法において、ナビゲーションシステムは、車両に取り付けられた角速度センサから、車両のヨー方向の角速度情報を取得する。さらに、ナビゲーションシステムは、車速パルス発生器から、車輪の回転数に応じてその周期が変化する車速パルスを取得する。ナビゲーションシステムは、取得した角速度情報から車両の方位を算出し、取得した車速パルスから移動速度を算出する。

電波航法及び自律航法のいずれの場合においても、ナビゲーションシステムには、高精度な位置を算出できることが要求される。また、ナビゲーションシステムには、より少ない電気配線で簡易に取り付けられることが要求される。つまり、ユーザが購入したナビゲーションシステムを車両に設置するだけで、高精度な位置を算出できることがナビゲーションシステムには要求される。電波航法については、新しい人工衛星の打ち上げ及び／又はＤＧＰＳ（Ｄｉｆｆｅｒｅｎｔｉａｌ ＧＰＳ）により、測位精度の向上が図られている。さらには、人工衛星からの電波を受信するアンテナについても、車両への取り付けが容易なものが開発されている。しかしながら、自律航法を実現するには、車輪の近傍に設置される車速パルス発生器から電気配線をナビゲーションシステムの取り付け位置まで引き込む必要がある。それ故、ナビゲーションシステムは、一般の人が簡単に車両に取り付けることができない。

車両への取り付けをより簡素化するために、車速パルスを用いることなく、車両の移動速度を算出するいくつかの速度算出装置が提案されている。その中のある速度算出装置は、車両の進行方向に沿う加速度を検出する加速度センサを備えており、このような加速度センサの出力値を積分することにより移動速度を求める。このような算出方法では、傾斜している路面上を車両が走行中、加速度センサの出力値には、重力加速度成分が含まれてしまうので、出力値をそのまま単純に積分することはできない。この問題を解決するため、ピッチ方向の角速度を検出する角速度センサの出力値を積分することにより、車両のピッチ角を算出し、算出したピッチ角を用いて加速度センサの出力から重力加速度成分を除去する速度算出装置がある（例えば、特許文献１参照）。
特開平８―３２０３３１号公報

しかし、上記特許文献に開示された速度算出装置では、車両の傾斜している場合には角速度センサ自体が傾斜しているため、正しいピッチ方向の角速度を出力できない。従って、角速度センサの出力値を積分することにより得られるピッチ角には、その軸の傾斜の影響に起因する誤差が含まれるという問題点がある。また、旋回時に車両に働く遠心力の影響を受けて、加速度センサの積分値がドリフトしていってしまうという問題もある。

それ故に、本発明の目的は、加速度センサの出力値を用いて、より精度の良い移動速度を算出可能な速度算出装置を提供することである。

上記目的を達成するために、本発明の一局面は、角速度センサ及び加速度センサが搭載された移動体の速度を算出する速度算出装置であって、加速度センサにより検出された加速度を用いて、前記移動体の静止時の傾斜角を第１の傾斜角として算出する第１の傾斜角算出部と、第１の傾斜角算出部で算出された第１の傾斜角を用いて、前記角速度センサにより検出された角速度を補正する角速度補正部と、前記角速度補正部で補正された角速度を用いて、前記移動体の現在の傾斜角を第２の傾斜角として算出する第２の傾斜角算出部と、前記加速度センサにより検出された加速度と、前記第２の傾斜角算出部により算出された第２の傾斜角とを用いて、移動体の進行方向に沿う加速度を算出する加速度算出部と、前記加速度算出部で算出された加速度を用いて、前記移動体の進行方向への速度を算出する速度算出部とを備える。

上記局面によれば、本速度算出装置によれば、第１の傾斜角算出部により、少なくとも、静止時の移動体の傾斜角が求められる。このような傾斜角を使って、角速度補正部により、角速度センサにより検出された角速度が補正される。速度算出部における速度算出には、第２の傾斜角が使われる。さらに、第２の傾斜角の算出には、角速度が使われる。従って、角速度補正部が角速度を補正することにより、第２の傾斜角算出部が算出する傾斜角には、角速度センサで検出された角速度の誤差が伝搬することを防止することができる。その結果、本速度算出装置により算出される速度への重力加速度の影響が少なくなる。これによって、より精度の良い速度を算出可能な速度算出装置を提供することが可能となる。

（実施形態）
図１は、本発明の一実施形態に係る速度算出装置の構成を示すブロック図である。図１において、速度算出装置は、例えば車両のような移動体に搭載され、移動体の傾斜角による重力の影響を除去した移動体の速度Ｖを算出する。かかる速度Ｖを算出するために、本速度算出装置は、角速度センサ１と、加速度センサ２と、第１の傾斜角算出部３と、角速度補正部４と、第２の傾斜角算出部５と、加速度算出部６と、速度算出部７とを備える。

角速度センサ１は、移動体の３軸周りの角速度ｐ，ｑ及びｒを定期的に検出する。ここで、角速度ｐは、移動体のロール方向への角速度である。また、角速度ｑは、移動体のピッチ方向への角速度である。さらに、角速度ｒは、移動体のヨー方向への角速度である。
加速度センサ２は、移動体の２軸方向への加速度ｇｘ及びｇｙを定期的に検出する。ここで、加速度ｇｘは、移動体の前後方向への加速度である。また、加速度ｇｙは、移動体の左右方向への加速度である。
第１の傾斜角算出部３は、加速度センサ２の出力値ｇｘ及びｇｙを用いて、移動体の静止時におけるピッチ角θ₂（０）及びロール角θ₃（０）を算出する。

角速度補正部４は、第１の傾斜角算出部３の出力値θ₂（０）及びθ₃（０）と、第２の傾斜角算出部５の出力値θ₂及びθ₃（後述）を使って、角速度センサ１の出力値ｐ、ｑ及びｒを補正して、補正値α、β及びγを算出する。ここで、補正値αは、基準平面上での移動体のロール方向への角速度である。また、角速度βは、基準平面上での移動体のピッチ方向への角速度である。さらに、角速度γは、基準平面上での移動体のヨー方向への角速度である。

第２の傾斜角度算出部５は、角速度補正部４の出力値α、β及びγを使って、移動体の現在の傾斜角度θ₂及びθ₃を算出する。より具体的には、傾斜角度θ₂は、基準平面に対する現在の移動体のピッチ角である。また、傾斜角度θ₃は、基準平面に対する現在の移動体のロール角である。
加速度算出部６は、第２の傾斜角算出部５の出力値θ₂及びθ₃と、加速度センサ２の出力値ｇｘ及びｇｙとを使って、移動体に加わる前後方向への加速度Ａｘを算出する。
速度算出部７は、加速度算出部６の出力値Ａｘを使って、移動体の速度Ｖを算出する。

次に、以上のような構成の速度算出装置について、構成各部の動作及び各値の算出方法をより具体的に説明する。今、図２Ａに示すように、重力加速度Ｇの方向をＺ₀軸とし、Ｚ₀軸に直交する平面（Ｘ₀Ｙ₀平面）を基準平面とする。なお、以下の説明では、Ｘ₀軸、Ｙ₀軸及びＺ₀軸からなる座標系を基準座標系と称する。

まず、第１の傾斜角算出部３は、現在移動体が静止しているか否かを判断する。第１の傾斜角算出部３は典型的には、加速度センサ２の出力値ｇｘ及び／又はｇｙの経時変化から、移動体が静止しているか否かを判断する。ここで、移動体の静止時には、加速度は経時変化しないので、加速度センサ２の出力値ｇｘ及び／又はｇｙのばらつき度合い、又は平均値変動をチェックすることにより、第１の傾斜角算出部３は移動体が現在静止しているか否かを判断することができる。なお、移動体の静止時には角速度ｐ、ｑ及びｒもまた経時変化しないので、第１の傾斜角算出部３は、角速度センサ１の出力値ｐ、ｑ及び／又はｒからも、移動体が現在静止しているか否かを判断することができる。

移動体が静止していると判断すると、第１の傾斜角算出部３は、加速度センサ２の出力値ｇｘ及びｇｙを、初期値ｇｘ（０）及びｇｙ（０）として取得する。その後、第１の傾斜角算出部３は、次式（１）にｇｘ（０）を代入することにより、静止時における移動体のピッチ角θ₂（０）を求める。
θ₂（０）＝ｓｉｎ^-1（ｇｘ（０）／Ｇ）…（１）

さらに、第１の傾斜角算出部３は、今回求めたθ₂（０）及び今回得られたｇｙ（０）を次式（２）に代入することにより、静止時における移動体のロール角θ₃（０）を求める。
θ₃（０）＝ｓｉｎ^-1（ｇｙ（０）／Ｇｃｏｓθ₂（０））…（２）

次に、角速度補正部４の動作並びに補正値α、β及びγの算出方法について説明する。まず、図２Ａに示すように、基準座標系におけるＸ₀軸、Ｙ₀軸及びＺ₀軸を、Ｚ₀軸周りに角度θ₁だけ回転させたものを、Ｘ₁軸、Ｙ₁軸及びＺ₁軸と称する。以上のＸ₁軸、Ｙ₁軸及びＺ₁軸からなる座標系を、以下では、第１の回転座標系と称する。また、図２Ｂに示すように、第１の回転座標系におけるＸ₁軸、Ｙ₁軸及びＺ₁軸を、Ｙ₁軸周りに角度θ₂だけ回転させたものを、Ｘ₂軸、Ｙ₂軸及びＺ₂軸と称する。以上のＸ₂軸、Ｙ₂軸及びＺ₂軸からなる座標系を、以下では、第２の回転座標系と称する。また、図２Ｃに示すように、第２の回転座標系におけるＸ₂軸、Ｙ₂軸及びＺ₂軸を、Ｘ₂軸周りに角度θ₃だけ回転させたものを、Ｘ₃軸、Ｙ₃軸及びＺ₃軸と称する。以上のＸ₃軸、Ｙ₃軸及びＺ₃軸からなる座標系を、以下では、第３の回転座標系と称する。このような第３の回転座標系内に移動体は存在する。そのため、出力値ｐ、ｑ及びｒは、第３の回転座標で検出された値となる。

本速度算出装置の最終目標は、Ｘ₀軸方向に沿う移動体の速度Ｖを算出することであるから、出力値ｐ、ｑ及びｒは、基準座標系の値α、β及びγに補正される必要がある。ここで、θ₁、θ₂及びθ₃の回転変換を表す行列をそれぞれＴ₁、Ｔ₂及びＴ₃とおく。また、基準座標系におけるＺ₀軸周りの回転角速度ベクトルを（０，０，α）とおく。また、第１の回転座標系におけるＹ₁軸周りの回転角速度ベクトルを（０，β，０）とおく。さらに、第２の回転座標系におけるＸ₂軸周りの回転角速度ベクトルを（γ，０，０）とおく。以上のように各値をおくと、次式（３）が成り立つ。

（ｐ，ｑ，ｒ）^T＝Ｔ₃Ｔ₂Ｔ₁（０，０，α）^T＋Ｔ₃Ｔ₂（０，β，０）^T
＋Ｔ₃（γ，０，０）^T …（３）
ここで、Ｔは転置行列である。角速度補正部４は上述のようにα、β及びγを算出するので、上式（３）を変形すると、次式（４）−（６）を得ることができる。
α＝（ｑｓｉｎθ₃＋ｒｃｏｓθ₃）／ｃｏｓθ₂ …（４）
β＝ｑｃｏｓθ₃−ｒｓｉｎθ₃ …（５）
γ＝ｐ＋（ｑｓｉｎθ₃＋ｒｃｏｓθ₃）ｔａｎθ₂ …（６）

角速度補正部４は、移動体が静止中、以下のように動作する。つまり、角速度補正部４は、今回得られた出力値ｑ及びｒと、今回得られたピッチ角θ₂（０）及びロール角θ₃（０）をθ₂及びθ₃として上式（４）に代入して、補正値αを算出する。また、角速度補正部４は、今回得られた出力値ｑ及びｒと、今回得られたロール角θ₃（０）をθ₃として上式（５）に代入して、補正値βを算出する。さらに、角速度補正部４は、今回得られた出力値ｐ、ｑ及びｒと、今回得られたピッチ角θ₂（０）及びロール角θ₃（０）をθ₂及びθ₃として上式（６）に代入して、補正値γを算出する。

また、角速度補正部４は、移動体が移動中、以下のように動作する。つまり、角速度補正部４は、今回得られた出力値ｑ及びｒと、第２の傾斜角算出部５から今回フィードバックされたピッチ角θ₂及びロール角θ₃を上式（４）に代入して、補正値αを算出する。また、角速度補正部４は、今回得られた出力値ｑ及びｒと、今回フィードバックされたロール角θ₃を上式（５）に代入して、補正値βを算出する。さらに、角速度補正部４は、今回得られた出力値ｐ、ｑ及びｒと、今回フィードバックされたピッチ角θ₂及びロール角θ₃を上式（６）に代入して、補正値γを算出する。それぞれの算出後、角速度補正部４は、今回算出した補正値α、β及びγを第２の傾斜角算出部５に渡す。

次に、第２の傾斜角算出部５の動作並びにピッチ角θ₂及びロール角θ₃の算出方法について説明する。ピッチ角θ₂及びロール角θ₃は、前式（５）及び（６）を積分することにより求められ、次式（７）及び（８）で表される。
θ₂＝∫βｄｔ …（７）
θ₃＝∫γｄｔ …（８）

よって、第２の傾斜角算出部５は、角速度補正部４から定期的に出力される補正値βを積算して、ピッチ角θ₂を算出する。また、第２の傾斜角算出部５は、定期的に出力される補正値γを積算して、ロール角θ₃を算出する。第２の傾斜角算出部５は、積算後、今回算出したピッチ角θ₂及びロール角θ₃を角速度補正部４及び加速度算出部６に渡す。

次に、加速度算出部６の動作及び加速度Ａｘの算出方法について説明する。図３は、移動体の一例である車両が傾斜面上を走行している時の様子を表す模式図である。図３において、移動体は、基準平面に対してピッチ方向に傾斜角θ₂を有する傾斜面上を走行している。この場合、加速度センサ２からは、移動体の進行方向に沿う加速度ｇｘが出力される。しかしながら、移動体には、重力加速度Ｇがかかるので、実際の進行方向への加速度Ａｘには、加速度ｇｘに加え、重力の進行方向成分Ｇｓｉｎθ₂が含まれる。従って、実際の加速度Ａｘは次式（９）で表されることになる。
Ａｘ＝ｇｘ＋Ｇｓｉｎθ₂ …（９）

よって、加速度算出部６は、加速度センサ２から今回得られた出力値ｇｘと、第２の傾斜角算出部５から今回得られたピッチ角θ₂とを、上式（９）に代入して、加速度Ａｘを算出する。その後、加速度算出部６は、今回算出した加速度Ａｘを速度算出部７に渡す。

周知のように、移動体の移動速度Ｖは、次式（１０）で表されるように、加速度Ａｘを積分することにより求められる。
Ｖ＝∫Ａｘｄｔ＝∫（ｇｘ＋Ｇｓｉｎθ₂）ｄｔ ・・・（１０）
よって、速度算出部７は、加速度算出部６から加速度Ａｘを受け取る度に、それらの積算する。

以上説明したように、本速度算出装置によれば、第１の傾斜角算出部５により、少なくとも、静止時の移動体の傾斜角度θ₂（０）が求められる。このような傾斜角度θ２（０）を使って、角速度補正部４により、角速度センサ１の出力値の内、少なくとも出力値ｑ及びｒが補正される。ここで、出力値ｑ及びｒは、速度Ｖの算出にはピッチ角θ₂の算出に使われる（前式（５）及び（７）参照）。従って、角速度補正部４が出力値ｑ及びｒを補正することにより、第２の傾斜角算出部５が算出するピッチ角θ₂に角速度センサ１の出力値ｑ及びｒの誤差が伝搬することを防止することができる。その結果、本速度算出装置により算出される速度Ｖへの重力加速度の影響が少なくなる。これによって、より精度の良い速度を算出可能な速度算出装置を提供することが可能となる。

また、図１からも明らかなように、本速度算出装置は、車速パルス無しで、速度Ｖを算出することが可能となる。つまり、本速度算出装置が移動体の一例としての車両に搭載された場合には、本速度算出装置と車速パルス発生装置とを電気配線で接続する必要がなくなる。

また、本速度算出装置がナビゲーションシステムに組み込まれた場合には、ナビゲーションシステムは、精度の高い速度を積分することにより、移動体の正確な移動距離を算出することが可能となる。これによって、ナビゲーションシステムは、移動体の現在位置を正しく表示することも可能となる。さらに、電波航法の不可能な場所でも、ナビゲーションシステムは、本速度算出装置と協働することで、正確な移動距離に基づいた自律航法が可能となる、その結果、マップマッチング等の精度を上げることができる。

（第１の変型例）
図４は、本発明の第１の変型例に係る速度算出装置の構成を示すブロック図である。図４において、速度算出装置は、図１に示すものと比較すると、旋回速度算出部８及び速度切替部９をさらに備える点で相違する。それ以外に、両速度算出装置の間に相違点は無いので、図４において、図１に示す構成に相当するものには同一の参照符号を付け、それぞれの説明を省略する。
旋回速度算出部８は、加速度センサ２の出力値ｇｙと、角速度補正部４の出力値γと、第２の傾斜角算出部５の出力値θ₂及びθ₃を用いて、移動体の旋回速度Ｖｃを算出して、速度切替部９に渡す。

速度切替部９は、移動体が旋回中か否かを判断し、移動体が旋回中でないと判断した場合には、速度算出部７から受け取る速度Ｖを選択して、例えばナビゲーションシステムに渡す。逆に移動体が旋回中であると判断した場合、速度切替部９は、旋回速度算出部８から受け取る旋回速度Ｖｃを、例えばナビゲーションシステムに渡す。ここで、速度切替部９は、例えば、移動体に働く遠心加速度の大きさＶｃ・ｒ（後述）、又は加速度センサ２から得られる左右方向への加速度ｇｙを参照して、移動体が現在旋回しているか否かを判断する。

次に、以上のような構成の速度算出装置について、構成各部の動作及び各値の算出方法を、より具体的に説明する。図５は、移動体の一例である車両が傾斜面で旋回している時の様子を表す模式図である。図５において、移動体は、基準平面に対してロール方向に角度θ₃で、ピッチ方向に角度θ₂で傾いた状態で旋回している。この場合、加速度センサ２からは、移動体の左右方向に沿う加速度ｇｙが出力される。この加速度ｇｙには、移動体には遠心加速度と、重力の左右方向成分とが含まれる。従って、加速度ｇｙは、次式（１１）で表されることになる。
ｇｙ＝Ｖｃ・ｒ＋Ｇｃｏｓθ₂ｓｉｎθ₃ …（１１）
ここで、Ｖｃは旋回速度であり、ｒは、移動体のヨー方向周りの実際の角速度である。
従って、旋回速度Ｖｃは、次式（１２）により求められる。
Ｖｃ＝（ｇｙ−Ｇｃｏｓθ₂ｓｉｎθ₃）／ｒ …（１２）

旋回速度算出部８は、加速度センサ２の出力値ｇｙと、第２の傾斜角算出部５の出力値θ₂及びθ₃とを、上式（１２）に代入する。さらに、旋回速度算出部８は、上式（１２）のｒとして、角速度補正部４の出力値γを代入する。これによって、旋回速度算出部８は、旋回速度Ｖｃを算出し、その後、速度切替部９に渡す。

以上説明したように、本速度算出装置によれば、移動体が旋回している場合でも、速度切替部９により、速度Ｖ又は旋回速度Ｖｃを選択的に出力することができる。それによって、本速度算出装置は、より精度の高い速度を出力することができる。

（第２の変型例）
図６は、本発明の第２の変型例に係る速度算出装置の構成を示すブロック図である。図６において、速度算出装置は、図１に示すものと比較すると、非接触速度算出部１０と、速度切替部１１と、リセット部１２とをさらに備える点で相違する。それ以外に、両速度算出装置の間には相違点は無いので、図６において、図１に示す構成に相当するものには同一の参照符号を付け、それぞれの説明を省略する。

非接触速度算出部１０は、速度算出部７とは異なる方法に従って、移動体の速度Ｖｎを非接触で算出する。以上のような非接触速度算出部１０の具体例としては、第１の変型例で説明した旋回速度算出部８、又は測位システムからの情報を使って移動体の速度を算出するナビゲーションシステムがある。旋回速度算出部８は、移動体が旋回しているという条件下では、速度算出部７よりも精度の高い速度を算出する。また、ナビゲーションシステムは、人工衛星からの情報を受信可能な状態では、速度算出部７よりも精度の高い速度を算出する。

速度切替部１１は、移動体の状態に応じて、速度算出部７及び非接触算出部１０のいずれかを選択して、選択した側から受け取る速度Ｖ又はＶｎを、例えばナビゲーションシステムに渡す。

また、速度切替部１１は、非接触算出部１０から速度算出部７へと切り換える際、非接触算出部１０から得られる速度Ｖｎをリセット部１２に渡す。具体的には、非接触速度算出部１０が旋回速度算出部８である場合には、速度切替部１１は、移動体が旋回中か否かを判断する。移動体が旋回中でないと判断した場合に、その直前に非接触算出部１０からの速度Ｖｎを速度切替部１１はリセット部１２に渡す。また、非接触速度算出部１０がナビゲーションシステムの場合には、速度切替部１１は、ナビゲーションシステムが人工衛星からの情報を受信不可能になった場合に、その直前にナビゲーションシステムから受け取った速度Ｖｎを速度切替部１１はリセット部１２に渡す。

リセット部１２は、受け取った速度Ｖｎを初期値として、速度算出部７に設定する。これによって、速度算出部７は、初期値Ｖｎに、加速度算出部６から受け取った加速度Ａｘを積算していく。
以上の初期値Ｖｎは、非接触速度算出部１０が好条件で算出したものであるから、相対的に精度が高い。このような初期値Ｖｎを使って、速度算出部７の積算をリセットすることにより、加速度Ａｘの積算時における誤差蓄積が抑制され、本速度算出装置は、常に安定した速度を算出することが可能となる。

本発明に係る速度算出装置は、高精度な速度を算出するという技術的効果が要求されるナビゲーションシステム等の用途に適用できる。

本発明の一実施形態に係る速度算出装置の全体構成を示すブロック図 図１の速度算出装置の動作説明に必要な基準座標系をＺ₀軸周りに回転させた第１の回転座標系を示す模式図 図２Ａに示す第１の回転座標系を、Ｙ₁軸周りに回転させた第２の回転座標系を示す模式図 図２Ｂに示す第２の回転座標系を、Ｘ₂軸周りに回転させた第３の回転座標系を示す模式図 車両が傾斜面上を走行している時の様子を表す模式図 本発明の第１の変型例に係る速度算出装置の構成を示すブロック図 車両が傾斜面で旋回している時の様子を表す模式図 本発明の第２の変型例に係る速度算出装置の構成を示すブロック図

符号の説明

１ 角速度センサ
２ 加速度センサ
３ 第１の傾斜角算出部
４ 角速度補正部
５ 第２の傾斜角算出部
６ 加速度算出部
７ 速度算出部
８ 旋回速度算出部
９，１１ 速度切替部
１０ 非接触速度算出部
１２ リセット部

Claims (4)

1. 角速度センサ及び加速度センサが搭載された移動体の速度を算出する速度算出装置であって、
   前記加速度センサにより検出された加速度を用いて、前記移動体の静止時の傾斜角を第１の傾斜角として算出する第１の傾斜角算出部と、
   前記第１の傾斜角算出部で算出された第１の傾斜角を用いて、前記角速度センサにより検出された角速度を補正する角速度補正部と、
   前記角速度補正部で補正された角速度を用いて、前記移動体の現在の傾斜角を第２の傾斜角として算出する第２の傾斜角算出部と、
   前記加速度センサにより検出された加速度と、前記第２の傾斜角算出部により算出された第２の傾斜角とを用いて、移動体の進行方向に沿う加速度を算出する加速度算出部と、
   前記加速度算出部で算出された加速度を用いて、前記移動体の進行方向への速度を算出する速度算出部とを備える、速度算出装置。
2. 前記加速度センサにより検出された加速度と、前記角速度補正部で補正された角速度と、前記第２の傾斜角算出部で算出された第２の傾斜角とを用いて、前記移動体の旋回速度を算出する旋回速度算出部と、
   前記速度算出部で算出された速度と、前記旋回速度算出部で算出された旋回速度とを選択的に出力する速度切替部とをさらに備える、請求項１に記載の速度算出装置。
3. 前記速度算出部とは異なる方法で、前記移動体の速度を算出する非接触速度算出部と、
   前記移動体の移動状態に応じて、前記速度算出部及び前記非接触速度算出部のいずれかを選択する速度切替部と、
   前記速度切替部が前記速度算出部を選択した場合に、前記非接触速度算出部により算出された速度を、前記速度算出部に初期値として与えるリセット部とをさらに備える、請求項１又は２に記載の速度算出装置。
4. 角速度センサ及び加速度センサが搭載された移動体の速度を算出する速度算出方法であって、
   前記加速度センサにより検出された加速度を用いて、前記移動体の静止時の傾斜角を第１の傾斜角として算出する第１の傾斜角算出ステップと、
   前記第１の傾斜角算出ステップで算出された第１の傾斜角を用いて、前記角速度センサにより検出された角速度を補正する角速度補正ステップと、
   前記角速度補正ステップで補正された角速度を用いて、前記移動体の現在の傾斜角を第２の傾斜角として算出する第２の傾斜角算出ステップと、
   前記加速度センサにより検出された加速度と、前記第２の傾斜角算出ステップにより算出された第２の傾斜角とを用いて、移動体の進行方向に沿う加速度を算出する加速度算出ステップと、
   前記加速度算出ステップで算出された加速度を用いて、前記移動体の進行方向への速度を算出する速度算出ステップとを備える、速度算出方法。
   
   
   

Images