JP3783061B1 - 傾斜角と並進加速度の検出方法および検出装置 - Google Patents

Abstract

【課題】物体の傾斜角と物体に働く並進の加速度とを応答性よく、かつ、誤差なく同時に検出できるようにする。
【解決手段】鉛直面内の４方向の加速度成分とそれらの幾何学的な関係から傾斜角の２階時間微分を、測定値から求められる傾斜角の２階時間微分値をデジタル的に積分して傾斜角を知る。初期誤差や近似の累積誤差を避けるため、鉛直面内に検算用の検算用加速度検出器を置き、計算で求められた傾斜角、並進加速度および角速度の値を使って出力値を合成し、検算用加速度検出器の出力値と合致するように各サンプル点での傾斜角の値に修正を加える。加速度運動中であっても傾斜角を必要な精度で知ることができるだけでなく、加速度検出器に働く鉛直方向および水平方向の加速度の値もそれぞれ分離して求めることができる。
【選択図】図１

Description

本発明は、動揺や振動を伴う物体、構造物、車輌や船舶などの移動体に設置または搭載され、その傾斜角と加速度を分離して計測する方法に関する。具体的には、複数の加速度検出器を使用して運動中に変化する傾斜角と、その物体に働く鉛直面内の並進の加速度とを同時に検出する方法および検出装置に関する。

重力加速度のように定常的に働く加速度も検出できる加速度検出装置は、静的な状況下では重力加速度の方向を検出する傾斜角検出装置として利用できるが、検出装置が移動体に設置されて加速度運動をする場合には、被検出物の運動に伴う検出装置自体の並進および回転の加速度も同時に検出してしまう。したがって、運動中も含めて正しい傾斜角を求めるには、加速度検出器の出力から重力加速度成分とそれ以外の加速度成分とを分離して取り出す必要がある。

特許文献１には、機械的な振子を使用し、本体に作用する２方向の加速度と、振子と本体の相対角度を検出し、検出された加速度を振子の運動方程式に適用して振子の角度を求め、この振子の角度から相対角度を引くことにより、運動中の本体の傾斜角を算出する方法が示されているが、信頼性と小型化という観点からは振子のような機械的な可動部をもたない構造の装置である方が望ましい。

このような観点から近年は、半導体加速度センサと加速度運動中の回転角度を推定するためのジャイロセンサとを組み合わせた装置が考案された。しかし、レートジャイロセンサや圧電振動ジャイロセンサには角速度が零のときでも出力値が緩やかに変化し続けるゼロ点ドリフトの問題があるので、これに対処しなければ実用的なシステムとはならない。

特許文献２では、観測周期ごとに圧電振動ジャイロセンサの出力値の差分をとることでゼロ点ドリフトの影響から逃れる方法を提案している。装置自体は姿勢角（３次元空間内における傾斜状態）を求めることができるが、圧電振動ジャイロセンサの出力は加速度運動中の姿勢角を求める目的で利用されるのではなく、静的な状態の姿勢角を求める目的で使われている。

圧電振動ジャイロセンサの出力を高域通過フィルタに通してドリフト成分を除去する方法も考えられている。例えば、特許文献３には、高域通過フィルタを通した後の信号を積分することを基本にした誤差の少ない回転角度算出法が開示されており、この方法を適用して回転運動中は振動ジャイロの信号から求めた傾斜角を出力し、静止あるいは殆ど静止していると見なせるときには加速度センサの信号から求めた傾斜角を出力する１軸の傾斜角検出器も製品化されている（非特許文献１参照）。
この方法では高域通過フィルタの特性上、低い周波数成分ほど角速度情報を多く捨てることになるので、加速度検出器の信号を用いた出力に切換える前の低域における傾斜角の検出精度は悪いものとなる。また、回転の角速度がある程度大きくなり、圧電振動ジャイロセンサの信号を用いた出力に切換える直前の周波数域での加速度センサ信号による姿勢角の精度も回転の加速度が重畳されている分だけ悪くなる。

高域通過フィルタを通して取り出した圧電振動ジャイロセンサの出力を使って傾斜角を求める方法は、高域では正しい再現値を与え、加速度検出器の出力を使って傾斜角を求める方法は低域ほど正しい値を示すという相補的な特徴が見られる。非特許文献２では、このような相補的特徴に着目して、液体傾斜計と圧電振動ジャイロセンサに対して、Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｆｉｌｔｅｒ（非特許文献３）を構成して傾斜角を求めるという方法を提案しており、液体傾斜計を加速度検出器に置換しても、同様の効果が得られる。

Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｆｉｌｔｅｒを構成する方法は、ジャイロセンサのゼロ点ドリフトの問題を解決するだけでなく、静止状態から必要とする帯域までの動的な傾斜角を、同一の理論を使って検出できる点で魅力的であるが、液体傾斜計、あるいは、加速度検出器の出力に重畳する重力以外の加速度の影響を定量的に取り扱うことはできないので、並進の加速度が働くような状況下では傾斜角の検出値に誤差が生じる欠点がある。
この欠点は特許文献２の方法でも特許文献３の方法でも、あるいは、Ｋａｌｍａｎフィルタを利用することによって３次元の動的な姿勢角を検出可能とした特許文献４の方法でも解決されてはいない。

特開２００４−２１９０７９号公報 特開２００４−１１７１３９号公報 特開平１０−１３２８４９号公報 米国特許第６６４７３５２号明細書 「ＳＥＮＳＯＲカタログ ＶＲセンサ／１軸角度センサ／３軸角度センサ」，株式会社データテック，２００４年，ｐ２ Ｂａｅｒｖｅｌｄｔ Ａ．Ｊ．，Ｒ．Ｋｌａｎｇ： Ａ Ｌｏｗ−ｃｏｓｔ ａｎｄ Ｌｏｗ−ｗｅｉｇｈｔ Ａｔｔｉｔｕｄｅ Ｅｓｔｉｍａｔｉｏｎ Ｓｙｓｔｅｍ ｆｏｒ ａｎ Ａｕｔｏｎｏｍｏｕｓ Ｈｅｌｉｃｏｐｔｅｒ，Ｐｒｏｃ． ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔ．Ｃｏｎｆ． ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｅｎｇｉｎｅｅｒｉｎｇ Ｓｙｓｔｅｍｓ，ＩＮＥＳ‘９７，Ｂｕｄａｐｅｓｔ，Ｈｕｎｇａｒｙ，Ｓｅｐｔｅｍｂｅｒ １９９７ Ｍ．Ｚｉｍｍｅｒｍａｎｎ ａｎｄ Ｗ．Ｓｕｌｚｅｒ：Ｈｉｇｈ Ｂａｎｄｗｉｄｔｈ Ｏｒｉｅｎｔａｔｉｏｎ Ｍｅａｓｕｒｅｍｅｎｔ ａｎｄ Ｃｏｎｔｒｏｌ Ｂａｓｅｄ ｏｎ Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｆｉｌｔｅｒｉｎｇ，Ｐｒｏｃ．ｏｆ ＳＹＲＯＣＯ‘９１，ＩＦＡＣ Ｓｙｍｐｏｓｉｕｍ ｏｎ Ｒｏｂｏｔ Ｃｏｎｔｒｏｌ，Ｖｉｅｎｎａ，Ａｕｓｔｒｉａ，Ｓｅｐｔｅｍｂｅｒ １９９１

以上のように、物体に働く並進の加速度を加速度検出器の出力から分離して取り出すことは、傾斜角の測定精度を上げることができるだけでなく、慣性航行の加速度データとして使用することも可能となる。特に、自動車や船舶、自律移動ロボットなどの慣性航行では低周波域の微小な加速度の検出精度が移動位置の計算精度を決めるので、低周波域も含めた並進の加速度を精度良く取り出す要求が大きく、本発明は、複数の加速度検出器を用いて、物体の傾斜角と物体に働く並進の加速度とを応答性よく、かつ、誤差なく同時に検出できる廉価な加速度検出装置を提供するものである。

感受軸を交差配置した加速度検出器を検出器の大きさより十分大きな間隔をあけて配置し、この二つの加速度検出器の出力から傾斜角を求め、２階時間微分値を時間で積分することにより傾斜角と並進加速度とを分離して検出するものであり、応答性よく、かつ、誤差なく傾斜角と加速度を同時に検出することができる。
感受軸を交差配置した加速度検出器を検出器の大きさより十分大きな間隔をあけて配置してあり、この二つの検出器の出力から傾斜角と並進加速度とを分離する演算装置からなる加速度検出装置である。
また、他の加速度感受軸を設けて加速度検出器の検出値の補正ができるようにし、更には、加速度検出器の温度補償装置を設けて環境温度による加速度検出装置の出力変化を補償できるようにした。

図１に示すように、加速度検出器は、二つの加速度感受軸１、２または３、４を同一平面内で交差させて配置したものであり、加速度感受軸を含む面内で距離Ｌだけ平行移動した位置Ｐ、Ｑにおいて剛体に取り付けてある。
あるいは、平行な二平面上に別方向を向けた加速度感受軸を交差させたものでもよい。

二組の加速度感受軸１、２および３、４の取付位置である点Ｐ、Ｑを結ぶ線が取付軸６であり、取付軸６と加速度感受軸１、２とのなす角度は、φ１、φ２である。また、２方向の速度検出軸をそれぞれＸ軸、Ｙ軸とする。Ｘ軸とＹ軸に平行な面でかつ取付軸６を含む平面を取付面と呼ぶ。なお、加速度検出部の寸法は、距離Ｌに比較して十分小さいものであり、点Ｐ、Ｑにおける加速度感受軸であるＸ軸方向およびＹ軸方向の加速度がそれぞれ検出できるように配置したものである。

図２に示すように、鉛直軸７が取付面に含まれるときの鉛直軸７と取付軸６とのなす傾斜角をθ、取付面内のｘ−ｙ直交座標系８のｙ軸を鉛直軸７と平行なものとし、点Ｐの座標を（ｘ，ｙ）で表すと、点Ｑの座標は（ｘ＋Ｌｓｉｎθ，ｙ＋Ｌｃｏｓθ）となる。
したがって、この基本ユニットが重力加速度（ｇ）の下で取付面に沿って加速度運動をする場合は、点Ｐ、Ｑに作用する加速度はそれぞれ式（１）、および式（２）で表すことができる（図３参照）。

これらの加速度のＸ軸射影、Ｙ軸射影が加速度検出器の出力になるので、点ＰでのＸ軸方向及びＹ軸方向の加速度はそれぞれ式（３）、式（４）で表される。

同様に、点ＱにおけるＸ軸方向、Ｙ軸方向の加速度は、それぞれ式（５）、式（６）で求められる。

式（３）〜式（６）より、傾斜角θに関する式（７）および式（８）の２本の式が導かれる。

更に、式（３）と式（４）より、水平方向の加速度と鉛直方向の加速度は式（９）および式（１０）によってそれぞれ求めることができる。

水平方向の加速度と鉛直方向の加速度は、各加速度検出軸の出力値の観測と傾斜角から計算で求めることができる量であり、傾斜角は加速度検出軸の出力値できまる式（７）を時間で積分することによって求めることができるので、傾斜角と並進の加速度が共に得られるのである。

以上のように、本発明によれば、並進加速度と傾斜角を分離して求めることができるが、式（７）の時間積分をそのまま実行すると、実世界での初期条件と積分で使用する初期条件が異なった場合、傾斜角に誤差が生じる。更に、積分を離散積分公式で置き換えて実施するような場合には初期値誤差の他に積分の近似誤差も累積することになり、正確な傾斜角を得ることができない。したがって、式（７）を積分して正確な傾斜角を得るには、実世界の観測値と比較して絶えず真の傾斜角を与えるような修正ループが必要になる。

図３に示すように、ＰＱの中央の位置Ｒに加速度感受軸が取付軸に垂直でかつ取付面内に含まれる加速度検出器５を固定し、加速度検出器５の出力値を真の傾斜角を求めるための修正ループを形成するのに利用する。
Ｒ点に働く加速度から加速度検出器５における出力値を求めると、式（１１）を得る。

式（１１）に式（７）〜（１０）を代入して整理すると式（１２）が導かれるので、両辺の差をとり、２乗したものを式（１３）と定義する。

積分により求められた傾斜角θが真の値であるときには、式（１３）はゼロの値をとり、それ以外では正の値となる。したがって、傾斜角を求める積分を行う場合には、適切な初期傾斜角とその時間微分の初期値を与えて式（７）の積分を行い、求められた傾斜角θを式（１３）に代入して要求精度を満たすほどゼロに近いかどうかを判定する。要求精度を満たしていない場合は、積分で求められた傾斜角θを初期値として要求精度を満たすまで式（１３）を最小化するθを探索し、最終的に求められたθの値を傾斜角とする。つぎの傾斜角を求める積分の初期値にも式（１３）を最小化するこの傾斜角θの値を使い、同様のプロセスを繰り返すことにより、精度の保証された傾斜角を得ることができる。なお、この繰り返しプロセスではθの増分からθの時間微分値（角速度）符号を知ることができるので、θの時間微分値の初期値には式（８）で求めた値を使用する。

実施例
加速度検出器は、アナログデバイセス社ＡＤＸＬ３１１ＥＢなどの小型で廉価な静電容量型２軸半導体加速度センサを使用した。
図４に示すように、点Ｐと点Ｑの位置に加速度検出器を１個ずつ配置し、それぞれの加速度感受軸（１、２および３、４）が取付軸６と軸対称（φ１＝π／４、φ２＝π／４）になるように向きを揃えて固定してある。また、点ＰＱの中点Ｒの位置に、検算用の加速度検出器５をその加速度感受軸が取付軸６と直交する向きに固定してある。
更に、半導体加速度センサの出力は環境温度の影響を受けやすいので、環境温度補償モニタ用として温度センサ９を加速度検出器を設置した点Ｐ，Ｑの近くに固定してある。

アナログ信号で出力される加速度検出器のデータと温度センサ９のデータをサンプリングし、傾斜角、水平方向加速度、鉛直方向加速度を演算装置でそれぞれ求めて結果を出力する。
演算手段は、Ａ／Ｄ変換器、ＰＷＭタイマカウンタモジュール、およびシリアルデータ通信モジュールを内蔵するワンチップマイコン（ルネサスのＨ８−３６６４Ｆ）を用いる。

式（７）に基づいて傾斜角の近似値を求める近似積分計算の一例として、観測周期をΔＴにとる式（１４）を示す。

ここに、ｋは時系列のｋステージを表す。

Ｐ点、Ｑ点に固定した加速度検出器の出力から求めた各軸のｋステージ加速度式（１５）とｋステージ用に作成された初期値式（１６）を使い、式（１４）の計算を実行して傾斜角とその時間変化式（１７）を予測する。初期値式（１８）には、ｋ−１ステージで求められた予測値式（１９）を探索初期値として使い、式（２０）を最小化する値を用いる。最小化する値を見つけるには、例えば２分法などが利用できる。式（２０）を最小化する式（１９）、すなわち、式（１８）はこの時点での傾斜角である。

式（２１）の初期値には、ｋ−１ステージで求めた式（２２）による予測値符号とｋ−２ステージ、ｋ−１ステージでの式（１８）の増分の符号を調べ、共に符号が一致しているときには式（２３）の符号としてその符号をもつ式（２３）の値を採用する。符号に不一致が生じた場合には、θ（ｋ）の時間微分の値が小さいものとして式（２１）をゼロとおく。

この初期値の設定法では、ｋ＝０およびｋ＝１での式（２１）の値とｋ＝０での式（１９）が定まらず、したがって、これらの値をゼロとおいて上に述べた繰り返しアルゴリズムを実行せざるを得ないが、このような条件からスタートしても、式（２０）の最小化で正しい傾斜角が求められていくので、実用上は問題が生じない。

図５は、この近似積分を使用した傾斜角と並進の加速度の算出アルゴリズムを示したもので、Ｂと表されているブロックは式（１４）の右辺第二項目の行列を指す。右側の矢印の上に描かれている式（１８）が傾斜角の値であり、水平方向加速度と鉛直方向加速度はそれぞれ式（２４）、式（２５）の計算を実行して求める。

前述のように、加速度検出器の出力値は、環境温度の影響を受ける。また、加速度検出器の取付誤差は傾斜角や並進加速度に誤った結果を与える。図６はこのこのような誤差の原因を避けるために導入する加速度信号の前置演算部のブロック図である。センサアレイ１０から出力される全てのアナログ信号は、高周波ノイズを除去するため、高域遮断フィルタ１１を通して取り出し、スケール調整部１２でスケール校正用データテーブル１３を参照してスケール変換を行う。スケール校正用データテーブル１３には単位換算データだけでなく、加速度検出器の取付誤差を補償する校正データもテーブルとして持つ。このようにしてスケール調整されたデータは、温度補正部１４で温度補正用データテーブル１５のデータを使って温度補正され、傾斜角と並進加速度を求めるブロック１６に入る。ブロック１６の処理は図５に示した処理である。

本発明のハードウェアの構成の一例を図７に示す。高域遮断フィルタ１１は、アナログ回路で構成したフィルタで、ノイズの軽減と同時にアンチエリアスフィルタの役目を果たす。各種のアナログ信号は観測周期に同期させたＡ／Ｄ変換器１８で読み取り、マイクロプロセッサ１９で、スケール調整、温度補正、傾斜角の計算および並進加速度の計算を行う。様々な利用形態を考慮し、傾斜角と並進加速度の計算値はアナログ量で出力する他、ＲＳ−２３２Ｃ等のシリアルデータ出力の機能も持たせる。ルネサス社のＨ８−３６６４Ｆマイクロプロセッサは、Ａ／Ｄ変換器８チャンネル、ＰＷＭ出力３チャンネル、シリアルインターフェースモジュール１を内蔵しているので、図７のＤ／Ａ変換器２０による機能をＰＷＭのデューティ比を使ったＤ／Ａ変換に置き換えると、破線で囲ったブロック１７の部分がＨ８−３６６４Ｆ１個で実現できることになり、小型化が図れる。更に、Ｈ８−３６６４Ｆ内蔵のＥＥＰＲＯＭは、装置固有の校正データを記録保存するものである。
なお、加速度感受軸を同一平面内で交差させて配置したものに基づいて本発明を具体的に説明したが、それ以外の配列を行っても同様に傾斜角と加速度を分離することが可能であり、二軸、三軸などの複数軸の加速度の測定に適用できることはいうまでもない。

本発明は可動部を有しない小型の検出装置であって傾斜角と並進加速度を分離して検出することができるので、建設機械・クレーン等の揺れ検出、または、吊り荷の傾斜角測定、車輌搭載の各種プラットフォームの角度制御、ロボットの姿勢制御など、応答性のよい傾斜角センサが必要とされている分野、並びに自律移動車等の慣性航行制御やＧＰＳ信号が受信できない場所でのカーナビゲーション、水中ロボットの慣性航行制御など、傾斜角と加速度の計測が同時に必要とされる技術分野に適用することができる。

本発明の基本概念図。 傾斜角と座標の定義の説明図。 検算用加速度検出軸の配置の説明図。 加速度検出器の配置の実施例の説明図。 デジタル計算アルゴリズムの説明図。 前置処理のブロック図。 実施例のハードウェア構成図。

符号の説明

Ｐ 計測用加速度検出器の取付位置
Ｑ 計測用加速度検出器の取付位置
Ｒ 検算用加速度検出器の取付位置
１ 加速度検出器のＸ軸方向加速度感受軸
２ 加速度検出器のＹ軸方向加速度感受軸
３ 加速度検出器のＸ軸方向加速度感受軸
４ 加速度検出器のＹ軸方向加速度感受軸
５ 検算用加速度検出器
６ 加速度検出器の取付軸
７ 鉛直軸
８ ｘ−ｙ直交座標系
９ 温度センサ
１０ センサアレイ
１１ 高域遮断フィルタ
１２ スケール調整ブロック
１３ スケール校正用データテーブル
１４ 温度補正ブロック
１５ 温度補正用データテーブル
１６ 傾斜角および並進加速度演算ブロック
１７ ワンチップマイクロコンピュータ Ｈ８／３６６４Ｆ
１８ Ａ／Ｄ変換器
１９ マイクロコンピュータの演算処理部
２０ Ｄ／Ａ変換器
２１ シリアル通信インターフェース

Claims (5)

1. 二つの感受軸を同一平面内で交差配置した加速度検出器を間隔をあけて配置し、この二つの加速度検出器のそれぞれの方向の加速度計測値から傾斜角の２階時間微分を求め、傾斜角の２階時間微分を時間で積分することにより傾斜角と並進加速度とを分離して検出する加速度検出方法。
2. 二つの感受軸を同一平面内で交差配置した加速度検出器が間隔をあけて配置してあり、この二つの加速度検出器のそれぞれの方向の加速度計測値から傾斜角の２階時間微分を求める演算装置を有する加速度検出装置。
3. 請求項２において、二つの加速度検出器の間隔は、加速度検出器の大きさより十分大きい加速度検出装置。
4. 請求項２または３のいずれかにおいて、二つの加速度検出器を結ぶ取付軸に交差する感受軸を有する検算用の加速度検出器を設けた加速度検出装置。
5. 請求項２〜４のいずれかにおいて、加速度検出器の温度補償装置が設けてある加速度検出装置。

Images