JP2008281508A

JP2008281508A - 傾斜角度測定装置

Info

Publication number: JP2008281508A
Application number: JP2007127792A
Authority: JP
Inventors: Toshiaki Shiotani; 敏昭塩谷
Original assignee: Mitsuba Corp
Current assignee: Mitsuba Corp
Priority date: 2007-05-14
Filing date: 2007-05-14
Publication date: 2008-11-20

Abstract

【課題】加速度センサのみで傾斜角度を高速に検出できるようにする。
【解決手段】傾斜角度測定装置２１は、移動体に取り付けられた第１の加速度センサ１１と第２の加速度センサ１２と傾斜角度算出部２２を有する。これら加速度センサ１１，１２は、直交する２方向の加速度を検出可能に配置されている。傾斜角度算出部２２は、ＨＰＦ（ハイパスフィルタ）３１，３３を通して静的な周波数成分を除去した検出値が入力される動的傾斜角度算出手段３５と、ＬＰＦ（ローパスフィルタ）３２，３４を通して動的な周波数成分を除去した検出値が入力される静的傾斜角度算出手段３６を有し、それぞれの算出結果を加算手段３７で加算して傾斜角度として出力する。
【選択図】図２

Description

本発明は、移動体の傾斜角度を算出する傾斜角度測定装置に関する。

移動体の走行制御や姿勢制御をする際には、移動体の傾斜角度を検出してフィードバック制御を行うことがある。傾斜角度を検出する手段としては、例えば、ＭＥＭＳ（Micro Electro Mechanical Systems）に利用される技術を用いて製造された２軸の加速度センサが知られている。
ここで、この種の加速度センサでは、重力と移動体自身の運動によって発生する加速度の和が検出されてしまうので、正確な傾斜角度を直接測定することが困難であった。そこで、従来の傾斜角度測定装置では、例えば特許文献１に開示されているように、２軸の加速度センサに加えて、移動体の角速度を検出する角速度センサを移動体に取り付けていた。加速度センサの出力信号から移動体自身の運動によって発生する加速度をローパスフィルタでカットし、カットした部分の情報を角速度センサで補うことで、移動体の傾斜角度を検出していた。
特開２００６−２２０４９１号公報

しかしながら、従来の構成で傾斜角度を検出するためには、加速度センサに加えて角速度センサを設ける必要があるので、装置構成が複雑化していた。
この発明は、このような事情に鑑みてなされたものであり、加速度センサのみで傾斜角度を高速に検出できるようにすることを主な目的とする。

上記の課題を解決する本発明の請求項１に係る発明は、移動体に固定され、移動体が傾斜又は回転するときの方向ベクトルを含む平面に平行な第１の方向の加速度を検出する第１の加速度センサと、移動体に固定され、移動体が傾斜又は回転するときの方向ベクトルを含む平面に平行で、第１の方向と直交する第２の方向の加速度を検出する第２の加速度センサと、前記第１、第２の加速度センサの出力信号から静的な加速度成分を除去するハイパスフィルタと、前記第１、第２の加速度センサの出力信号から動的な加速度成分を除去するローパスフィルタと、前記ハイパスフィルタを通した前記第１、第２の加速度センサの出力信号から算出した傾斜角速度を２階積分し、動的な傾斜角度を算出する動的傾斜角度算出手段と、前記ローパスフィルタを通した前記第１、第２の加速度センサの出力信号から静的な傾斜角度を算出する静的傾斜角度算出手段と、動的な傾斜角度と静的な傾斜角度を加算する加算手段と、を備えることを特徴とする傾斜角度測定装置とした。
この傾斜角度測定装置では、移動体が傾斜又は回転して加速度が発生すると、第１、第２の加速度センサから信号が出力される。動的傾斜角度算出手段は、加速度センサの出力信号からハイパスフィルタで低周波数成分をカットした信号から動的な傾斜角度を算出する。静的傾斜角度算出手段は、加速度センサの出力信号からローパスフィルタで高周波数成分をカットした信号から静的な傾斜角度を算出する。これら傾斜角度を加算手段で加算すると、動的な傾斜角度成分が静的な傾斜角度成分で補完された傾斜角度が得られる。

請求項２に係る発明は、請求項１に記載の傾斜角度測定装置において、前記動的傾斜角度算出手段は、前記第１、第２の加速度センサごとに設けられた第１の動的傾斜角度算出部及び第２の傾斜角度算出部と、前記代１、第２の傾斜角度算出部で算出される動的な傾斜角度のうち２階積分を行ったときの積分誤差が少なくなる方に重みを付けた値の平均をとる平均化手段とを備えることを特徴とする。
この傾斜角度測定装置は、２つの加速度センサのそれぞれから動的な傾斜角度を算出し、平均化手段で平均値をとることで動的な傾斜角度を算出している。さらに、動的傾斜角度算出手段が２階積分したときに発生する積分誤差の大小を２つの加速度センサのそれぞれについて予め調べておき、積分誤差が小さい方の動的な傾斜角度に重み付けをしてから平均化値をとることで積分誤差の影響が小さくなるようにしている。

本発明によれば、２つ加速度センサの出力信号から動的な傾斜角度と静的な傾斜角度を算出し、これらを用いて移動体の傾斜角度を求めるようにしたので、加速度センサのみで移動体の傾斜角度や回転角度を計測することが可能になる。加速度センサのデータを補完する目的で他のセンサを設ける必要がなくなって装置構成を簡略化できる。

発明を実施するための最良の形態について図面を参照しながら詳細に説明する。
最初に、図１を参照して２軸の加速度センサで移動体の傾斜角度を検出する原理について説明する。
図１に示すように、移動体Ｍｂが支点Ｆｃを中心にして傾斜するときの水平方向からの傾斜角度をθとし、移動体Ｍｂと共に移動する加速度センサ１の運動加速度をαｓとする。また、重力をｇとする。２軸の加速度センサ１は、移動体Ｍｂが傾斜していない初期状態における水平方向と鉛直方向のそれぞれに相当する２軸で加速度を検出するように設置されている。これら方向Ｄ１，Ｄ２は、移動体Ｍｂが傾斜又は回転するときの移動方向を示す方向ベクトルを含む平面に平行になっている。以下、移動方向を示す方向ベクトルを含む平面上で初期状態における水平方向に相当する第１の方向Ｄ１の加速度を検出する要素を第１の加速度センサ１１とし、その検出値をｓｓ１とする。第１の方向Ｄ１に直交する第２の方向Ｄ２、つまり初期状態における水平方向の加速度を検出する要素を第２の加速度センサ１２とし、その検出値をｓｓ２とする。
いま、移動体Ｍｂが外力によって支点Ｆｃを中心にして傾斜角度θだけ傾斜したとき、第１の加速度センサ１１の出力信号ｓｓ１は、以下のように表すことができる。

同様に、第２の加速度センサ１２の出力信号ｓｓ２は以下のようになる。

また、移動体Ｍｂの回転角加速度は、支点Ｆｃから各加速度センサ１１，１２までの距離ｒｍと、各加速度センサ１１，１２の運動加速度αｓを用いると、

となる。式（３）に式（１）、（２）のそれぞれを代入すると、

と表せる。式（４）、（５）は、出力信号ｓｓ１，ｓｓ２を含むθに関する微分方程式になっているので、これらの微分方程式を解けば傾斜角度θが得られる。式（４）又は式（５）のいずれか一方を計算すれば、傾斜角度θを求めることができるが、計算の精度を向上させるため、この実施の形態では各式の計算結果の平均値をもって傾斜角度θとする。
式（４）、（５）に示す微分方程式を解く場合、重力加速度ｇが既知なので、出力信号ｓｓ１，ｓｓ２及び微小時間前の傾斜角度θを使って逐次、２階積分して傾斜角度θを算出すれば良い。

しかしながら、２階積分を行う過程では、積分誤差が発生するので静的な傾斜角度を検出することは困難である。そこで、以下に示す式（６）を使って静的な傾斜角度θ２を算出し、これを用いて積分誤差を修正する。

より詳細には、式（４）、（５）を積分して得られる傾斜角度は、動的成分のみが抽出されるようにハイパスフィルタを通した出力信号ｓｓ１，ｓｓ２から求め、これを動的な傾斜角度θ１とする。式（６）の傾斜角度θ２は、静的成分のみが抽出されるようにローパスフィルタを通した出力信号ｓｓ１，ｓｓ２から求める。そして、傾斜角度θ１と傾斜角度θ２の和をとって移動体の傾斜角度θとする。

なお、式（４）、（５）では、検出された傾斜角度θ（つまり動的な傾斜角度θ１）の値がフィードバックされるようになっている。したがって、傾斜角度θに対する感度が高いと積分誤差が大きくなる。式（４）、（５）のそれぞれにおいて、θに関する感度は、以下のようにそれぞれ表せる。

θ≒０のときは、式（８）がほぼ０になる。θ≒９０°のときは、式（７）がほぼ０になる。したがって、傾斜角度θの値に応じて式（４）、（５）の結果に重み付けて平均値を算出する積分誤差をより小さくできる。

次に、このような原理を利用して移動体の傾斜角度を測定する傾斜角度測定装置の具体的な構成について図２を参照して説明する。
傾斜角度測定装置２１は、２軸の加速度センサ１を構成する第１の加速度センサ１１及び第２の加速度センサ１２と、傾斜角度演算部２２とを有する。
第１の加速度センサ１１及び第２の加速度センサ１２は、共に図１に示すように移動体Ｍｂに取り付けられており、第１の加速度センサ１１で第１の方向Ｄ１の加速度が検出され、第２の加速度センサ１２で第１の方向に直交する第２の方向Ｄ２の加速度が検出されるようになっている。これら加速度センサ１１，１２は、動的な加速度だけでなく静的な加速度を検出可能な構成を有し、例えば、ＭＥＭＳに使用される技術を利用して製造され、静電容量の変化を利用して直交する２つの方向Ｄ１，Ｄ２のそれぞれの加速度を検出する。

傾斜角度演算部２２は、第１の加速度センサ１１の出力信号ｓｓ１が入力される第１のハイパスフィルタ（ＨＰＦ）３１及び第１のローパスフィルタ（ＬＰＦ）３２と、第２の加速度センサ１２の出力信号ｓｓ２が入力される第２のハイパスフィルタ（ＨＰＦ）３３及び第２のローパスフィルタ（ＬＰＦ）３４を有する。２つのハイパスフィルタ３１，３３の出力は、動的傾斜角度算出手段３５に接続されている。２つのローパスフィルタ３２，３４の出力は、式（６）を使って静的な傾斜角度θ２を算出する静的傾斜角度算出手段３６に接続されている。
動的傾斜角度算出手段３５は、第１のハイパスフィルタ３１が接続される第１の動的傾斜角度算出部４１と、第２のハイパスフィルタ３３が接続される第２の動的傾斜角度算出部４２と、各動的傾斜角度算出部４１，４２の出力が接続される平均化手段４３とを備えている。
平均化手段４３の出力と、静的傾斜角度算出手段３６の出力は、加算手段３７に接続されており、加算手段３７から傾斜角度θの算出結果が出力されるようになっている。

第１の動的傾斜角度算出部４１は、式（４）を演算するように構成され、第２の動的傾斜角度算出部４２は、式（５）を演算するように構成されている。各動的傾斜角度算出部４１，４２は、傾斜角度θの前回値を記憶するメモリを有し、前回値を使って積分処理を行って動的な傾斜角度θ１の今回値を逐次的に演算する。また、重み計算を行うことで、積分誤差の影響を抑えるようになっている。重み計算は、傾斜角度θに関してリニアに重みを変えるもので、式（７）、（８）を用いて傾斜角度０°から９０°の間で傾斜角度θ１に対する感度の評価を予め実施しておき、より積分誤差が少なくなる方の傾斜角度θ１に重みを付ける。

平均化手段４３は、第１の動的傾斜角度算出部４１から出力される重み付けされた動的な傾斜角度θ１（以下、傾斜角度θ１１とする）と、第２の動的傾斜角度算出部４２から出力される重み付けされた動的な傾斜角度θ１（以下、傾斜角度θ１２とする）の平均値を算出する。
加算手段３７は、各フィルタ３１〜３４を使って周波数的に分担させた動的傾斜角度θ１と静的傾斜角度θ２を加算して傾斜角度θを算出する。

この傾斜角度測定装置２１の動作を説明する。移動体Ｍｂが傾斜すると、各加速度センサ１１，１２が出力信号ｓｓ１，ｓｓ２を出力する。第１の加速度センサ１１の出力信号ｓｓ１は、第１のハイパスフィルタ３１を通って静的な傾斜角度に相当する低周波成分がカットされた後、第１の動的傾斜角度算出部４１に入力される。傾斜角度θの前回値を用いた逐次積分で式（４）を２階積分し、積分値に傾斜角度θの今回値で定める重み付け処理を行う。同様に、第２の加速度センサ１２の出力信号ｓｓ２は、第２のハイパスフィルタ３３を通って静的な傾斜角度に相当する低周波成分がカットされた後、第２の動的傾斜角度算出部４２に入力される。傾斜角度θの前回値を用いた逐次積分で式（５）を２階積分し、積分値に傾斜角度θの今回値で定める重み付け処理を行う。平均化手段４３は、これら２つの動的な傾斜角度θ１１，１２の今回の平均値を算出する。例えば、傾斜角度θが０°付近では式（７）に示す第１の加速度センサ１１の出力の感度がほぼゼロになるので、動的な傾斜角度θ１１に重みを付ける。これに対して、式（８）で感度が算出される動的な傾斜角度θ１２は、この角度域では感度が相対的に大きくなって積分誤差が大きいことが予想されるので、重み付けは行わない。又は十分に小さい値で重み付けを行う。したがって、平均値を求める際には、動的な傾斜角度θ１１を主とする傾斜角度θ１が算出される。

傾斜角度θの増加と共に、動的な傾斜角度θ１１の重み付けが小さくなる一方で、動的な傾斜角度θ１２の重み付けが大きくなる。第１の動的傾斜角度算出部４１における重み付け処理は、傾斜角度θが０°のときが最も大きく、傾斜角度θの増加に伴ってリニアに減少し、傾斜角度θが９０°のときに最も小さくなる。逆に第２の動的傾斜角度算出部４２における重み付け処理は、傾斜角度θが０°のときが最も小さく、傾斜角度θの増加に伴ってリニアに増加し、傾斜角度θが９０°のときに最も大きくなる。

一方、２つの加速度センサ１１，１２の出力信号ｓｓ１，ｓｓ２は、各ローパスフィルタ３２，３４にも入力される。さらに、各ローパスフィルタ３２，３４において動的な傾斜角度に相当する高周波数成分がカットされた後、静的傾斜角度算出手段３６に入力される。式（６）に基づいて静的な傾斜角度θ２が算出され、加算手段３７に出力される。
加算手段３７では、同時期に出力された出力信号ｓｓ１，ｓｓ２に基づいて計算された動的な傾斜角度θ１と静的な傾斜角度θ２を加算し、傾斜角度θとして出力する。

ここで、傾斜角度測定装置２１で図１に示す移動体Ｍｂの傾斜角度θを検出した結果について以下に説明する。
不図示の移動機構を使って移動体Ｍｂを傾斜角度０°から転がらせ、傾斜角度４５°付近で一度止め、さらに転がしてみた。移動体Ｍｂの傾斜角度の実際の値（＝真値）と、傾斜角度測定装置２１で検出した傾斜角度θを図３に示す。図３は、横軸が時間の経過を示し、縦軸が傾斜角度になっている。実線で示すラインＬ１は傾斜角度の真値を示し、破線で示すラインＬ２は傾斜角度測定装置１の計測結果として出力された傾斜角度θを示す。一点鎖線で示すラインＬ３は式（６）の計算結果を示す。ラインＬ３は、ローパスフィルタ３２，３４を通さずに検出値ｓｓ１，ｓｓ２から直接に静的な傾斜角度を算出した結果を示している。

ラインＬ１及びラインＬ２からわかるように、傾斜角度θは真値とよく一致した。しかしながら、傾斜角度９０°のときは、瞬間的に大きな誤差が発生し、その後振動的に収束している。これは、移動体Ｍｂの頂点Ｆｃが接地した衝撃によって大きな加速度を各加速度センサ１１，１２が検出したためである。また、ラインＬ３に示す静的な傾斜角度は、各加速センサ１１，１２自身の運動によって生じる加速度により、動き始めと傾斜角度９０°で急峻に立ち上がる大きい誤差が発生した。動き始めの誤差は、回転トルクによって支点Ｆｃの接地部が沈み込んだためと考えられる。また、４５°においても静的な傾斜角度に誤差が生じていた。つまり、検出値ｓｓ１，ｓｓ２から直接に静的な傾斜角度を算出しただけでは、誤差が大きくなることがわかる。

図４に、傾斜角度測定装置２１で算出した傾斜角度θ、θ１、θ２を示す。ラインＬ４が傾斜角度θを示し、ラインＬ５が動的な傾斜角度θ１を示す。ラインＬ６が静的な傾斜角度θ２を示す。
ラインＬ５に示すように、動的成分に相当する傾斜角度θ１は、各ハイパスフィルタ３１，３３を通した出力信号ｓｓ１，ｓｓ２を使用して算出され、平均化手段４３から出力される値に相当する。この動的な傾斜角度θ１は、傾斜開始時では傾斜角度θに略一致する。そして、時間の経過と共にラインＬ４に示す傾斜角度θから乖離し、ゼロに収束する。移動体が４５°から再び動き出したときには、これに追従して変化している。また、ラインＬ６に示す傾斜角度θ２は、各ローパスフィルタ３２，３４を通した出力信号ｓｓ１，ｓｓ２から算出され、静的傾斜角度算出手段３６から出力される値に相当する。この静的な傾斜角度θ２は、図３のラインＬ３に示すような自身の運動による誤差がフィルタ処理によって除去されている。
このように、フィルタ３１〜３４を通した出力信号ｓｓ１，ｓｓ２から傾斜角度θ１，θ２を算出し、これら傾斜角度θ１，θ２を加算して傾斜角度θを算出することで、図３のラインＬ２に示すように、真値に略一致し時間遅れの少ないデータが得られた。

この実施の形態によれば、２軸の加速度センサ１を用いて移動体Ｍｂの傾斜角度θを算出することが可能になる。移動体Ｍｂが支点Ｆｃを中心にして回転する場合にも同様にして傾斜角度θを算出することができる。例えば、移動体Ｍｂが回転軸回りに回転するリンクである場合、リンクに固定された２軸の加速度センサ１を用い、回転軸と加速度センサ１の位置関係が変化しないことを利用することで、リンクの傾斜角度や回転角度を算出することが可能になる。
２軸の加速度センサ１のみで傾斜角度を算出するあたり、周波数フィルタ（フィルタ３１〜３４）を使用して動的な傾斜角度θ１と静的な傾斜角度θ２に分けて計算を行うことで、動的な傾斜角度θ１の算出時に静的な成分の影響をカットできると共に、静的な傾斜角度θ２の算出時に動的な成分の影響をカットできる。そして、後にこれら傾斜角度θ１，θ２を加算することで傾斜角度θを算出するようにしたので、誤差の発生を抑制することができ、測定精度を向上できる。
加速度の検出方向が直交する２つの動的な傾斜角度θ１のそれぞれについて傾斜角度θに対する感度の評価を予め行って重み付けを決定しておき、傾斜角度θ１１，θ１２の算出結果に重み付けを行ってから平均化処理を行うことで２階積分による積分誤差の影響を小さくしたので、傾斜角度θの測定精度を向上できる。

なお、本発明は前記の実施の形態に限定されずに広く応用することができる。
例えば、第１の加速度センサ１１と第２の加速度センサ１２は、一体でも良いし、別体でも良い。さらに、各加速度センサ１１，１２を移動体Ｍｂの異なる位置に固定しても良い。２つの加速度センサ１１，１２は、移動体Ｍｂが傾斜又は回転するときの方向ベクトルを含む平面内で検出方向が直交する２方向であれば良く、鉛直方向や水平方向に限定されない。
動的な傾斜角度θ１を算出する際の重み付けは、曲線的に変化させても良い。

本発明の実施の形態に係る移動体の傾斜角度を算出する際の原理を説明する図である。傾斜角度測定装置のブロック図である。傾斜角度測定装置で傾斜角度を測定した結果を示すグラフである。傾斜角度と動的な傾斜角度、静的な傾斜角度のグラフである。

符号の説明

１２軸の加速度センサ
１１第１の加速度センサ
１２第２の加速度センサ
２１傾斜角度測定装置
２２傾斜角度算出部
３１第１のＨＰＦ（第１のハイパスフィルタ）
３２第１のＬＰＦ（第１のローパスフィルタ）
３３第２のＨＰＦ（第２のハイパスフィルタ）
３４第２のＬＰＦ（第２のローパスフィルタ）
３５動的傾斜角度算出手段
３６静的傾斜角度算出手段
３７加算手段
４１第１の動的傾斜角度算出部
４２第２の動的傾斜角度算出部
４３平均化手段
Ｍｂ移動体
Ｆｃ支点

Claims

移動体に固定され、移動体が傾斜又は回転するときの方向ベクトルを含む平面に平行な第１の方向の加速度を検出する第１の加速度センサと、
移動体に固定され、移動体が傾斜又は回転するときの方向ベクトルを含む平面に平行で、第１の方向と直交する第２の方向の加速度を検出する第２の加速度センサと、
前記第１、第２の加速度センサの出力信号から静的な加速度成分を除去するハイパスフィルタと、
前記第１、第２の加速度センサの出力信号から動的な加速度成分を除去するローパスフィルタと、
前記ハイパスフィルタを通した前記第１、第２の加速度センサの出力信号から算出した傾斜角速度を２階積分し、動的な傾斜角度を算出する動的傾斜角度算出手段と、
前記ローパスフィルタを通した前記第１、第２の加速度センサの出力信号から静的な傾斜角度を算出する静的傾斜角度算出手段と、
動的な傾斜角度と静的な傾斜角度を加算する加算手段と、
を備えることを特徴とする傾斜角度測定装置。
前記動的傾斜角度算出手段は、前記第１、第２の加速度センサごとに設けられた第１の動的傾斜角度算出部及び第２の傾斜角度算出部と、前記代１、第２の傾斜角度算出部で算出される動的な傾斜角度のうち２階積分を行ったときの積分誤差が少なくなる方に重みを付けた値の平均をとる平均化手段とを備えることを特徴とする請求項１に記載の傾斜角度測定装置。