JP2007040765A - 角速度センサの零点補正装置 - Google Patents

Abstract

【課題】角速度センサの零点を補正する。
【解決手段】ロボット等の運動体に角速度センサ１０が設けられる。変化範囲設定器１４は角速度の変化幅が所定値以下であるか否かで静止状態を判定し、静止判定器２０は判定時間を超えて静止状態が継続しているか否かを判定する。総和平均器３４は、静止状態と判定された期間のｎ個のデータのうち、終了タイミング直前のｉ個のデータを除いた（ｎ−ｉ）個のデータの総和平均を算出して零点オフセットとする。零点補正器３６は、センサ出力を零点補正して出力器２８に出力する。
【選択図】図１

Description

本発明は角速度センサ、特にロボット等の運動体に設けられる角速度センサの零点補正に関する。

ロボット等の移動体の姿勢制御に加速度センサやヨーレートセンサが用いられている。直交する３軸をｘ軸、ｙ軸、ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りのヨーレートを３個のヨーレートセンサで検出する。軸回りの角度、あるいは姿勢角は、ヨーレートセンサの出力を時間積分して得られ、ピッチ角、ロール角、ヨー角が算出される。

以下の特許文献には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。

特開２００４−２６８７３０号公報

しかしながら、角速度センサのオフセット及びドリフトが大きいと、角速度の積分により姿勢角を求めるため、オフセット等が徐々に蓄積して極めて大きな値となり、時間とともに増加、発散してしまう。これを防止するために、オフセットやドリフトの小さい角速度センサを用いればよいが、このような角速度センサは大型で重く、かつ高価である。

本発明の目的は、高価で高精度な角速度センサを用いることなく、簡易な構成で積分誤差の蓄積を防止し運動体の姿勢角を検出することができる装置を提供することにある。

本発明は、運動体に設けられた角速度センサの零点補正装置であって、前記運動体の静止状態を検出する手段と、前記静止状態における前記角速度センサ出力の平均値を算出する手段と、前記平均値を前記角速度センサの零点として補正する手段とを有する。

本発明では、角速度センサが設けられた運動体の静止状態を検出し、この静止状態における角速度センサの出力を用いて角速度センサの零点を補正する。運動体の静止状態では、角速度センサの出力は本来的に零であるはずであるから、静止状態におけるセンサ出力は零点オフセットとなる。そこで、本発明では、静止状態におけるセンサ出力の平均値をもって零点を補正し、角速度を積分して得られる姿勢角の誤差蓄積を防止する。

本発明において、運動体の静止状態は、角速度センサあるいは同一運動体に設けられた加速度センサの少なくともいずれかの出力の変化幅から検出できる。零点補正の精度を確保するためには静止状態が一定時間以上継続することが望ましく、所定の判定時間以上継続する静止状態でのセンサ出力平均値から零点を補正する。

本発明によれば、簡易な構成で角速度センサの零点を補正し、積分誤差の蓄積を防止して運動体の姿勢角を高精度に検出できる。

以下、図面に基づき本発明の実施形態について説明する。

＜第１実施形態＞
図１に、本実施形態の構成ブロック図を示す。角速度センサ１０は、ロボット等の運動体の所定位置に設けられ、運動体の角速度を検出する。例えば、センサ座標系をｘｙｚ直交座標系とし、ｘ軸、ｙ軸、ｚ軸の各軸回りの角速度を検出する。角速度センサ１０は、検出した角速度を変化範囲比較器１２に出力する。

変化範囲比較器１２は、入力した角速度の変化幅（変動幅）を所定の範囲と大小比較する。所定の範囲は、変化範囲設定器１４で設定される。変化範囲設定器１４は、固定範囲を所定の範囲としてもよいが、図示のようにレジスタ１６に設定された範囲を所定範囲とし、ユーザが入力器１８を用いて所望の範囲をレジスタ１６に設定できるように構成するのが好適である。変化範囲比較器１２は、角速度の変化幅と所定の範囲との大小比較結果を静止判定器２０に出力する。例えば、変化範囲比較器１２は、角速度の変化幅が所定の範囲内である場合にＨｉ、角速度の変化幅が所定の範囲を超える場合にＬｏｗとなる２値信号を静止判定器２０に出力する。

静止判定器２０は、変化範囲比較器１２からの比較結果を入力し、角速度センサ１０の出力が所定の範囲内に入った時刻（静止判定開始時刻ｔｏ）を検知する。また、角速度センサ１０の出力が所定の範囲内から逸脱した時刻（静止判定終了時刻ｔｅ）を検知する。また、静止判定開始時刻ｔｏからの経過時間（静止判定継続時間ｔｏｋ）を計測する。静止判定器２０は、静止判定継続時間ｔｏｋが所定の判定時間ｔｏｓを超えたか否かを判定し、超えたときに静止状態開始タイミング信号Ｔｓｔを出力器２８に出力する。所定の判定時間ｔｏｓは判定時間設定器２２から供給され、判定時間設定器２２の設定内容はレジスタ２４で適宜設定される。静止状態開始タイミング信号を出力した後、静止判定器２０は、角速度センサ１０の出力が所定の範囲内から逸脱した時刻（静止判定終了時刻ｔｅ）において静止状態終了タイミング信号Ｔｅｎを出力器２８に出力する。さらに、静止判定器２０は、静止状態開始タイミング信号ｔｓｔからの経過時間（静止状態継続時間ｔｓｋ）を計測する。静止状態継続時間ｔｓｋが所定の補正終了時間ｔｏｅを超えたときに、強制的に静止状態終了タイミング信号Ｔｅｎを出力器２８に出力する。

出力器２８は、静止状態開始タイミング信号Ｔｓｔ及び静止状態終了タイミング信号Ｔｅｎを補正時間管理器３２に出力する。補正時間管理器３２は、静止状態開始タイミング信号Ｔｓｔを受信し、ｎ−ｉ総和平均器３４での平均処理を開始させる。また、静止状態終了タイミング信号Ｔｅｎを受信し、ｎ−ｉ総和平均器３４での平均処理を終了させる。また、ｎ−ｉ総和平均器３４での−ｉ処理時間ｔｏｉをレジスタ３０を参照してｎ−ｉ総和平均器３４へ指示する。さらに、静止状態継続時間ｔｓｋが所定の補正最低時間ｔｏｈ以上の場合に零点補正値をｎ−ｉ総和平均の出力で更新して実施するようｎ−ｉ総和平均器３４を介して零点補正器３６へ命令する。静止状態継続時間ｔｓｋが所定の補正最低時間ｔｏｈ未満の場合には十分な精度が得られないと判定し、零点補正器３６における零点補正値は更新されない。

ｎ−ｉ総和平均器３４は、所定の時間分のｉ個のデータを保管するレジスタを有し、補正時間管理器３２からの指示でｉ個のデータ保管を開始する。ｎ−ｉ総和平均器３４は、ｉ個のデータが保管されるまで待ち、−ｉ＋１個目のデータを受け取ったときに、１番目のデータから総和平均の演算を開始する。以下、１番目からｎ−ｉ番目のデータまでの総和平均の演算を繰り返す。そして、補正時間管理器３２からの終了指示でｎ−ｉ総和平均値を零点補正器３６へ出力する。零点補正器３６は、入力した平均値を新たな零点として更新することで補正する。このように１番目からｎ−ｉ番目のデータまでの総和平均を演算するのは、１番目からｎ番目のデータの総和平均を算出したのでは、角速度が実際に変化し始めてからのデータも算入してしまうため、その精度が低下するからである。ｔｅよりも所定時間ｔｏｉだけ遡った時点までのデータ１〜ｎ−ｉを用いて総和平均することで、零点補正値を正確に算出できる。

以下、角速度センサ出力のタイミングチャートを用いて本実施形態の零点補正値算出演算を説明する。

図２に、角速度センサ１０から出力される角速度の時間変化を示す。図において、横軸は時間（ｓ）、縦軸は角速度（ｒａｄ／ｓ）である。基本演算は、静止判定開始時刻ｔｏで補正値用の総和平均演算を開始し、静止判定終了時刻ｔｅで補正値用総和平均演算を終了するものである。静止状態開始タイミング信号Ｔｓｔ及び静止状態終了タイミング信号Ｔｅｎがそれぞれ時刻ｔｏ、ｔｅで出力される。総和平均値Ｍｅａｎ１は零点補正器３６に出力され、零点補正値を更新する。

図３に、改良された零点補正値算出方法を示す。図２において、補正行為の安定性を考慮し、静止状態判定開始時刻ｔｏではなく、静止判定継続時間ｔｏｋが所定の判定時間ｔｏｓを超えたときに（つまり、時刻ｔｏ＋ｔｏｓに）、静止状態開始タイミング信号Ｔｓｔを出力器２８に出力して補正値用総和平均演算を開始する。静止判定終了時刻ｔｅで静止状態終了タイミング信号Ｔｅｎを出力して補正値用総和平均演算を終了する。算出された補正値Ｍｅａｎ２で零点補正器３６の補正値を更新する。これにより、無意味な補正行為の発生を防止できる。静止開始直後の不安定なデータを平均処理に含めないため精度が向上する。

図４に、さらに改良された零点補正値算出方法を示す。図３において、静止判定終了時刻ｔｅより所定の時間、すなわち−ｉ処理時間ｔｏｉだけ遡ったタイミング（つまり時刻ｔｅ−ｔｏｉ）のデータに対して総和平均演算（ｎ−ｉ総和平均演算）を行い、ｎ−ｉ総和平均値Ｍｅａｎ３を零点補正器３６へ出力する。図におけるＴｎ−ｉは、補正時間管理器３２が総和平均器３４に対して指示するタイミング信号である。これにより、静止判定終了時刻ｔｅ前の精度が極めて悪いデータ群を総和平均値から除き、精度を向上することができる。

図５に、さらに改良された零点補正値算出方法を示す。図２〜図４において、静止状態継続時間ｔｓｋを計測し、補正最低時間ｔｏｈ以上の場合に零点補正値を総和平均の出力で更新して実施すべく、総和平均器３４を介して零点補正器３６へ命令する。これにより、精度維持に必要なサンプル数が確保でき、精度の良い補正値更新が可能となる。

図６に、さらに改良された零点補正値算出方法を示す。図２〜図５において、静止状態継続時間ｔｓｋが所定の時間、すなわち補正終了時間ｔｏｅに達したときに強制的に静止状態終了タイミング信号Ｔｅｎを静止判定器２０が出力し、静止補正を終了させる。これにより、所定の時間で必要な精度を有する零点補正値が得られ、零点補正値を更新できる。以上の各算出方法において、判定時間ｔｏｓは０．１ｓ〜１ｓ程度であり、例えば０．３ｓ程度に設定するのが好適である。判定時間は短い方が静止補正に移行できるので好都合であるが、運動体の安定する応答性に依存する。静止まで時間のかかる運動体では長めに設定される。但し、余り長く（例えば５ｓ以上）設定すると静止補正に移行する頻度が低下するので全体精度が低下してしまう。

また、補正最低時間ｔｏｈは１ｓ〜１０ｓ程度でよく、８ｓ程度が好適である。なるべく長く設定する方が補正値精度が向上するので好都合であるが、長くしすぎると静止補正を実施する頻度が低下するので全体精度が低下してしまう。従って、この時間も同様に運動体の動作に合わせて設定される。

−ｉ処理時間ｔｏｉは０．１ｓ〜１ｓ程度であり、０．３ｓ程度が好適である。なるべく短い方が静止補正データ量が増大するので好都合であるが、運動体の起動する応答性に依存する。起動まで時間のかかる運動体では長めに設定される。余り長く（例えば５ｓ程度）設定すると静止補正に使用されるデータ量が減少し、逆に補正値精度が低下してしまうので、適当な値に設定することが好ましい。

補正終了時間ｔｏｅは１ｓ〜３００ｓ程度であり、３ｓ程度が好適である。なるべく長い方が補正値精度が向上するので好都合であるが、長くしすぎると静止補正を実施する頻度が低下して全体精度が低下してしまう。運動体の動作に合わせて設定するのがよい。補正最低時間ｔｏｈよりも、−ｉ処理時間ｔｏｉ分だけ長めに設定する。

なお、変化範囲比較器１２、静止判定器２０、補正時間管理器３２、総和平均器２４、零点補正器３６等は、具体的にはマイクロプロセッサで構成できる。

＜第２実施形態＞
図７に、本実施形態の構成ブロック図を示す。図１では、角速度センサ１０からの角速度を用いてロボットの静止状態を判定したが、本実施形態では同じロボットに設けられた加速度センサからの加速度を用いて静止状態を判定する。

加速度センサ４０は、ロボットのｘ軸、ｙ軸、ｚ軸各軸方向の加速度を検出して変化範囲比較器４２に出力する。

変化範囲比較器４２は、変化範囲設定器４４に設定された所定範囲と加速度の変化幅とを大小比較し、比較結果を絶対値比較器４８に出力する。

絶対値比較器４８は、静止状態開始タイミング及び静止状態終了タイミングにおける加速度の絶対値が実質的に重力加速度（例えば９．７９７０７２ｍ／ｓ２、京都大学の国際基準点であり、「実質的」とは、重力加速度±許容誤差を意味し、許容誤差は加速度センサ４０に生じていると予想されるオフセットを考慮して設定される）であるか否か、あるいは当該期間における加速度が実質的に重力加速度であるか否かを判定する。これらの加速度が実質的に重力加速度ではない場合、静止状態ではないと判定する。一方、これらの加速度が実質的に重力加速度である場合、静止状態にあると判定し、静止判定器２０に出力する。その後の処理は第１実施形態と同様である。加速度を用いて静止状態を判定することで、並進運動や振動運動の検出が容易化され、角速度センサ１０の零点補正精度が向上する。

＜第３実施形態＞
図８に、本実施形態の構成ブロック図を示す。本実施形態では、角速度と加速度をともに用いて静止状態を判定する。

角速度を用いて静止状態を判定する構成は図１とほぼ同様であるが、変化範囲比較器１２は比較結果を静止判定器２０ではなく組合せ比較器５０に出力する。

一方、加速度を用いて静止状態を判定する構成も図７とほぼ同様であるが、絶対値比較器４８は比較結果を静止判定器２０ではなく組合せ比較器５０に出力する。

組合せ比較器５０は、変化範囲比較器１２からの比較結果、及び絶対値比較器４８からの比較結果を入力し、両者がともに範囲内にある場合を静止状態と判定し、判定結果を静止判定器２０に出力する。その後の処理は第１実施形態と同様である。

本実施形態では、角速度と加速度の組合せで静止状態を判定しているので、ロボットの並進及び回転の動きを監視できるのでより高精度に静止状態を判定し、角速度センサ１０の零点を補正できる。

以上、本発明の実施形態について説明したが、本発明はこれに限定されず種々の変更が可能である。

例えば、図１〜図８において、有意の静止状態と判定するための判定時間ｔｏｓは入力器１８から所望の値を入力しレジスタ２４に設定することで調整できるが、ユーザの手動設定ではなく自動設定としてもよい。判定時間ではなく、図８の組合せ比較器５０における組合せの判定基準を変更することも可能である。所定時間経過しても零点補正が行われない場合、角速度あるいは加速度のいずれも静止状態と判定された場合に静止状態とするのではなく、角速度あるいは加速度のいずれかが静止状態と判定された場合に静止状態と判定して判定条件を緩和し、零点補正の早期実行を担保する等である。

実施形態の構成ブロック図である。 補正値算出方法を示すタイミングチャートである。 改良された補正値算出方法を示すタイミングチャートである。 さらに改良された補正値算出方法を示すタイミングチャートである。 さらに改良された補正値算出方法を示すタイミングチャートである。 さらに改良された補正値算出方法を示すタイミングチャートである。 他の実施形態の構成ブロック図である。 さらに他の実施形態の構成ブロック図である。

符号の説明

１０ 角速度センサ、１２ 変化範囲比較器、１４ 変化範囲設定器、１６ レジスタ、１８ 入力器、２０ 静止判定器、２２ 判定時間設定器、２４ レジスタ、２６ タイマ、２８ 出力器、３０ レジスタ、３２ 補正時間管理器、３４ 総和平均器、３６ 零点補正器。

Claims (10)

1. 運動体に設けられた角速度センサの零点補正装置であって、
   前記運動体の静止状態を検出する手段と、
   前記静止状態における前記角速度センサ出力の平均値を算出する手段と、
   前記平均値を前記角速度センサの零点として補正する手段と、
   を有することを特徴とする角速度センサの零点補正装置。
2. 請求項１記載の装置において、
   前記平均値を算出する手段は、前記静止状態の開始タイミングから、前記静止状態の終了タイミングより所定時間だけ前あるいは所定個数だけ遡るタイミングまでの前記角速度センサ出力の平均値を算出する
   ことを特徴とする角速度センサの零点補正装置。
3. 請求項１、２のいずれかに記載の装置において、
   前記静止状態を検出する手段は、前記角速度センサ出力の変化幅が所定幅以下であるか否かに基づき静止状態を検出することを特徴とする角速度センサの零点補正装置。
4. 請求項１、２のいずれかに記載の装置において、
   前記静止状態を検出する手段は、前記運動体に設けられた加速度センサのセンサ出力の変化幅が所定値以下であるか否かに基づき静止状態を検出することを特徴とする角速度センサの零点補正装置。
5. 請求項１、２のいずれかに記載の装置において、
   前記静止状態を検出する手段は、前記角速度センサ出力の変化幅が第１の所定幅以下であり、かつ、前記運動体に設けられた加速度センサのセンサ出力の変化幅が第２の所定幅以下であるか否かに基づき静止状態を検出することを特徴とする角速度センサの零点補正装置。
6. 請求項４、５のいずれかに記載の装置において、
   前記静止状態を検出する手段は、さらに、前記加速度センサのセンサ出力の絶対値が実質的に重力加速度であるか否かに基づき静止状態を検出することを特徴とする角速度センサの零点補正装置。
7. 請求項１〜６のいずれかに記載の装置において、
   前記静止状態を検出する手段は、所定の判定時間以上継続する静止状態を検出することを特徴とする角速度センサの零点補正装置。
8. 請求項２記載の装置において、さらに、
   前記所定時間あるいは前記所定個数を可変設定する手段
   を有することを特徴とする角速度センサの零点補正装置。
9. 請求項３〜５のいずれかに記載の装置において、さらに、
   前記所定幅を可変設定する手段
   を有することを特徴とする角速度センサの零点補正装置。
10. 請求項７記載の装置において、さらに、
    前記所定の判定時間を可変設定する手段
    を有することを特徴とする角速度センサの零点補正装置。
    

Images