角速度センサの零点補正装置および補正方法
ロボット等の運動体に角速度センサ（１０）が設けられる。第１設定器（１４）は角速度の変化幅が所定幅以下であるか否かで静止状態を判定し、静止判定器（２０）は判定時間を超えて静止状態が継続しているか否かを判定する。総和平均器（３４）は、静止状態と判定された期間のｎ個のデータのうち、終了タイミング直前のｉ個のデータを除いた（ｎ−ｉ）個のデータの総和平均を算出して零点オフセットとする。零点補正器（３６）は、出力値を零点補正して出力器２８に出力する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は角速度センサ、特にロボット等の運動体に設けられる角速度センサの零点補正に関する。
【背景技術】
【０００２】
ロボット等の移動体の姿勢制御に加速度センサやヨーレートセンサが用いられている。直交する３軸をｘ軸、ｙ軸、ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りのヨーレートを３個のヨーレートセンサで検出する。軸回りの角度、あるいは姿勢角（ピッチ角、ロール角、ヨー角）は、ヨーレートセンサの出力を時間積分して得られる。
【０００３】
特開２００４−２６８７３０号公報には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。
【０００４】
しかしながら、角速度センサのオフセット及びドリフトが大きいと、角速度の積分により姿勢角を求めるため、オフセット等が徐々に蓄積して極めて大きな値となり、時間とともに増加、発散してしまう。これを防止するために、オフセットやドリフトの小さい角速度センサを用いればよいが、このような角速度センサは大型で重く、かつ高価である。
【発明の開示】
【発明が解決しようとする課題】
【０００５】
本発明の目的は、高価で高精度な角速度センサを用いることなく、簡易な構成で積分誤差の蓄積を防止し運動体の姿勢角を検出することができる装置および方法を提供することにある。
【課題を解決するための手段】
【０００６】
本発明の第１の態様は、運動体に設けられた角速度センサの零点補正装置に関する。この零点補正装置は、前記運動体の静止状態を検出する検出手段と、前記静止状態における前記角速度センサの出力値の平均値を算出する算出手段と、前記平均値を前記角速度センサの零点として補正する補正手段とを有する。
【０００７】
この零点補正装置では、角速度センサが設けられた運動体の静止状態を検出し、この静止状態における角速度センサの出力値を用いて角速度センサの零点を補正する。運動体の静止状態では、角速度センサの出力値は本来的に零であるはずであるから、静止状態における出力値は零点オフセットとなる。そこで、本発明では、静止状態における出力値の平均値をもって零点を補正し、角速度を積分して得られる姿勢角の誤差蓄積を防止する。
【０００８】
この零点補正装置では、運動体の静止状態は、角速度センサあるいは同一運動体に設けられた加速度センサの少なくともいずれかの出力の変化幅から検出できる。零点補正の精度を確保するためには静止状態が一定時間以上継続することが望しいため、所定の判定時間以上継続する静止状態での出力値の平均値から零点を補正してもよい。
【０００９】
本発明の第２の態様は、運動体に設けられた角速度センサの零点補正方法に関する。この方法は、前記運動体が静止状態かどうかを判定するステップと、前記静止状態における前記角速度センサの出力値の平均値を算出するステップと、前記平均値を前記角速度センサの零点として補正するステップとを備える。
【発明の効果】
【００１０】
本発明によれば、簡易な構成で角速度センサの零点を補正し、積分誤差の蓄積を防止して運動体の姿勢角を高精度に検出できる。
【発明を実施するための最良の形態】
【００１１】
以下、図面に基づき本発明の実施形態について説明する。
【００１２】
＜第１実施形態＞
図１に、本実施形態の構成ブロック図を示す。角速度センサ１０は、ロボット等の運動体の所定位置に設けられ、運動体の角速度を検出する。例えば、センサ座標系をｘｙｚ直交座標系とし、ｘ軸、ｙ軸、ｚ軸の各軸回りの角速度を検出する。角速度センサ１０は、検出した角速度を第１比較器１２に出力する。
【００１３】
第１比較器１２は、入力した角速度の変化幅（変動幅）を所定の範囲と大小比較する。所定の範囲は、第１設定器１４で設定される。第１設定器１４は、固定範囲を所定の範囲としてもよいが、図示のようにレジスタ１６に設定された範囲を所定範囲とし、ユーザが入力器１８を用いて所望の範囲をレジスタ１６に設定できるようにしてもよい。第１比較器１２は、角速度の変化幅と所定の範囲との大小比較結果を静止判定器２０に出力する。例えば、第１比較器１２は、角速度の変化幅が所定の範囲内である場合にＨｉ、角速度の変化幅が所定の範囲を超える場合にＬｏｗの信号を静止判定器２０に出力する。
【００１４】
静止判定器２０は、第１比較器１２からの比較結果を入力し、角速度センサ１０の出力が所定の範囲内に入った時刻（開始時刻ｔｏ）を検知する。また、角速度センサ１０の出力が所定の範囲内から逸脱した時刻（終了時刻ｔｅ）を検知する。また、開始時刻ｔｏからの経過時間（継続時間ｔｏｋ）を計測する。静止判定器２０は、継続時間ｔｏｋが所定の判定時間ｔｏｓを超えたか否かを判定し、超えたときに静止状態開始タイミング信号Ｔｓｔを出力器２８に出力する。所定の判定時間ｔｏｓは第２設定器２２から供給され、第２設定器２２の設定内容はレジスタ２４で適宜設定される。静止状態開始タイミング信号を出力した後、静止判定器２０は、角速度センサ１０の出力が所定の範囲内から逸脱した時刻（終了時刻ｔｅ）において終了タイミング信号Ｔｅｎを出力器２８に出力する。さらに、静止判定器２０は、静止状態開始タイミング信号ｔｓｔからの経過時間（静止状態継続時間ｔｓｋ）を計測する。静止状態継続時間ｔｓｋが所定の補正終了時間ｔｏｅを超えたときに、強制的に終了タイミング信号Ｔｅｎを出力器２８に出力する。
【００１５】
出力器２８は、静止状態開始タイミング信号Ｔｓｔ及び終了タイミング信号Ｔｅｎを補正時間管理器３２に出力する。補正時間管理器３２は、静止状態開始タイミング信号Ｔｓｔを受信し、ｎ−ｉ総和平均器３４での平均処理を開始させる。また、終了タイミング信号Ｔｅｎを受信し、ｎ−ｉ総和平均器３４での平均処理を終了させる。また、ｎ−ｉ総和平均器３４での−ｉ処理時間ｔｏｉをレジスタ３０を参照してｎ−ｉ総和平均器３４へ指示する。さらに、静止状態継続時間ｔｓｋが所定の補正最低時間ｔｏｈ以上の場合に零点補正値をｎ−ｉ総和平均の出力で更新して実施するようｎ−ｉ総和平均器３４を介して零点補正器３６へ命令する。静止状態継続時間ｔｓｋが所定の補正最低時間ｔｏｈ未満の場合には十分な精度が得られないと判定し、零点補正器３６における零点補正値は更新されない。
【００１６】
ｎ−ｉ総和平均器３４は、所定の時間分のｉ個のデータを保管するレジスタを有し、補正時間管理器３２からの指示でｉ個のデータ保管を開始する。ｎ−ｉ総和平均器３４は、ｉ個のデータが保管されるまで待ち、ｉ＋１個目のデータを受け取ったときに、１番目のデータから総和平均の演算を開始する。以下、１番目からｎ−ｉ番目のデータまでの総和平均の演算を繰り返す。そして、補正時間管理器３２からの終了指示でｎ−ｉ総和平均値を零点補正器３６へ出力する。零点補正器３６は、入力した平均値を新たな零点として更新することで補正する。このように１番目からｎ−ｉ番目のデータまでの総和平均を演算するのは、１番目からｎ番目のデータの総和平均を算出したのでは、角速度が実際に変化し始めてからのデータも算入してしまうため、その精度が低下するからである。ｔｅよりも所定時間ｔｏｉだけ遡った時点までのデータ１〜ｎ−ｉを用いて総和平均することで、零点補正値を正確に算出できる。
【００１７】
以下、角速度センサの出力値のタイミングチャートを用いて本実施形態の零点補正値算出演算を説明する。
【００１８】
図２に、角速度センサ１０から出力される角速度の時間変化を示す。図において、横軸は時間（ｓ）、縦軸は角速度（ｒａｄ／ｓ）である。基本演算は、開始時刻ｔｏで補正値用の総和平均演算を開始し、終了時刻ｔｅで補正値用総和平均演算を終了するものである。静止状態開始タイミング信号Ｔｓｔ及び終了タイミング信号Ｔｅｎがそれぞれ時刻ｔｏ、ｔｅで出力される。総和平均器３４からの総和平均値Ｍｅａｎ１は零点補正器３６に出力され、零点補正値を更新する。
【００１９】
図３に、改良された零点補正値算出方法を示す。図２において、補正行為の安定性を考慮し、静止状態判定開始時刻ｔｏではなく、継続時間ｔｏｋが所定の判定時間ｔｏｓを超えたときに（つまり、時刻ｔｏ＋ｔｏｓに）、静止状態開始タイミング信号Ｔｓｔを出力器２８に出力して補正値用総和平均演算を開始する。終了時刻ｔｅで終了タイミング信号Ｔｅｎを出力して補正値用総和平均演算を終了する。総和平均器３４で算出された補正値Ｍｅａｎ２で零点補正器３６の補正値を更新する。これにより、無意味な補正行為の発生を防止できる。静止開始直後の不安定なデータを平均処理に含めないため精度が向上する。
【００２０】
図４に、さらに改良された零点補正値算出方法を示す。図３において、終了時刻ｔｅより所定の時間、すなわち−ｉ処理時間ｔｏｉだけ遡ったタイミング（つまり時刻ｔｅ−ｔｏｉ）のデータに対して総和平均演算（ｎ−ｉ総和平均演算）を行い、ｎ−ｉ総和平均値Ｍｅａｎ３を零点補正器３６へ出力する。図４におけるＴｎ−ｉは、補正時間管理器３２が総和平均器３４に対して指示するタイミング信号である。これにより、終了時刻ｔｅ前の精度が極めて悪いデータ群を総和平均値から除き、精度を向上することができる。なお、総和平均器３４によって、ｔｏｉまたは、ｔｏｉに相当する出力値の数が変更されても良い。
【００２１】
図５に、さらに改良された零点補正値算出方法を示す。図２〜図４において、静止状態継続時間ｔｓｋを計測し、補正最低時間ｔｏｈ以上の場合に零点補正値を総和平均の出力で更新して実施すべく、総和平均器３４を介して零点補正器３６へ命令する。これにより、精度維持に必要なサンプル数が確保でき、精度の良い補正値更新が可能となる。
【００２２】
図６に、さらに改良された零点補正値算出方法を示す。図２〜図５において、静止状態継続時間ｔｓｋが所定の時間、すなわち補正終了時間ｔｏｅに達したときに強制的に終了タイミング信号Ｔｅｎを静止判定器２０が出力し、静止補正を終了させる。これにより、所定の時間で必要な精度を有する零点補正値が得られ、零点補正値を更新できる。以上の各算出方法において、判定時間ｔｏｓは０．１ｓ〜１ｓ程度であり、例えば０．３ｓ程度に設定してもよい。判定時間は短い方が早く静止補正に移行できるが、運動体の安定する応答性に依存する。静止まで時間のかかる運動体では長めに設定される。但し、余り長く（例えば５ｓ以上）設定すると静止補正に移行する頻度が低下するので全体精度が低下してしまう。
【００２３】
また、補正最低時間ｔｏｈは１ｓ〜１０ｓ程度でよく、８ｓ程度としてもよい。なるべく長く設定する方が補正値精度が向上するので好都合であるが、長くしすぎると静止補正を実施する頻度が低下するので全体精度が低下してしまう。従って、この時間も同様に運動体の動作に合わせて設定される。
【００２４】
−ｉ処理時間ｔｏｉは０．１ｓ〜１ｓ程度であり、０．３ｓ程度としてもよい。なるべく短い方が静止補正データ量が増大するが、運動体の起動する応答性に依存する。起動まで時間のかかる運動体では長めに設定される。余り長く（例えば５ｓ程度）設定すると静止補正に使用されるデータ量が減少し、逆に補正値精度が低下してしまうので、適当な値に設定してもよい。
【００２５】
補正終了時間ｔｏｅは１ｓ〜３００ｓ程度であり、３ｓ程度であってもよい。なるべく長い方が補正値精度が向上するが、長くしすぎると静止補正を実施する頻度が低下して全体精度が低下してしまう。運動体の動作に合わせて設定するのがよい。補正最低時間ｔｏｈよりも、少なくとも−ｉ処理時間ｔｏｉ分以上長めに設定する。
【００２６】
なお、第１比較器１２、静止判定器２０、補正時間管理器３２、総和平均器３４、零点補正器３６等は、具体的にはマイクロプロセッサで構成できる。
【００２７】
＜第２実施形態＞
図７に、本実施形態の構成ブロック図を示す。図１では、角速度センサ１０からの角速度を用いてロボットの静止状態を判定したが、本実施形態では同じロボットに設けられた加速度センサからの加速度を用いて静止状態を判定する。
【００２８】
加速度センサ４０は、ロボットのｘ軸、ｙ軸、ｚ軸各軸方向の加速度を検出して第２比較器４２に出力する。
【００２９】
第２比較器４２は、第３設定器４４に設定された所定範囲と加速度の変化幅とを大小比較し、比較結果を第３比較器４８に出力する。なお、第３設定器４４は、固定範囲を所定の範囲としてもよいが、図示のようにレジスタ４６に設定された範囲を所定範囲とし、ユーザが入力器を用いて所望の範囲をレジスタ４６に設定できるようにしてもよい。
【００３０】
第３比較器４８は、静止状態開始タイミング及び終了タイミングにおける加速度の絶対値が実質的に重力加速度（例えば９．７９７０７２ｍ／ｓ２、京都大学の国際基準点であり、「実質的」とは、重力加速度±許容誤差を意味し、許容誤差は加速度センサ４０に生じていると予想されるオフセットを考慮して設定される）であるか否か、あるいは当該期間における加速度が実質的に重力加速度であるか否かを判定する。これらの加速度が実質的に重力加速度ではない場合、静止状態ではないと判定する。一方、これらの加速度が実質的に重力加速度である場合、静止状態にあると判定し、静止判定器２０に出力する。その後の処理は第１実施形態と同様である。加速度を用いて静止状態を判定することで、並進運動や振動運動の検出が容易化され、角速度センサ１０の零点補正精度が向上する。
【００３１】
＜第３実施形態＞
図８Ａ，８Ｂに、本実施形態の構成ブロック図を示す。本実施形態では、角速度と加速度をともに用いて静止状態を判定する。
【００３２】
角速度を用いて静止状態を判定する構成は図１とほぼ同様であるが、第１比較器１２は比較結果を静止判定器２０ではなく組合せ比較器５０に出力する。
【００３３】
一方、加速度を用いて静止状態を判定する構成も図７とほぼ同様であるが、第３比較器４８は比較結果を静止判定器２０ではなく組合せ比較器５０に出力する。
【００３４】
組合せ比較器５０は、第１比較器１２からの比較結果、及び第３比較器４８からの比較結果を入力し、両者がともに範囲内にある場合を静止状態と判定し、判定結果を静止判定器２０に出力する。その後の処理は第１実施形態と同様である。
【００３５】
本実施形態では、角速度と加速度の組合せで静止状態を判定しているので、ロボットの並進及び回転の動きを監視できるのでより高精度に静止状態を判定し、角速度センサ１０の零点を補正できる。
【００３６】
以上、本発明の実施形態について説明したが、本発明はこれに限定されず種々の変更が可能である。
【００３７】
例えば、図１〜図８Ｂにおいて、有意の静止状態と判定するための判定時間ｔｏｓは入力器１８から所望の値を入力しレジスタ２４に設定することで調整できるが、ユーザの手動設定ではなく自動設定としてもよい。判定時間ではなく、図８Ｂの組合せ比較器５０における組合せの判定基準を変更することも可能である。所定時間経過しても零点補正が行われない場合、角速度あるいは加速度のいずれも静止状態と判定された場合に静止状態とするのではなく、角速度あるいは加速度のいずれかが静止状態と判定された場合に静止状態と判定して判定条件を緩和し、零点補正の早期実行を担保する等である。なお、レジスタ１６、２０、２４、３０、４６は１つの装置によって構成されていても良い。
【図面の簡単な説明】
【００３８】
【図１】実施形態の構成ブロック図である。
【図２】補正値算出方法を示すタイミングチャートである。
【図３】改良された補正値算出方法を示すタイミングチャートである。
【図４】さらに改良された補正値算出方法を示すタイミングチャートである。
【図５】さらに改良された補正値算出方法を示すタイミングチャートである。
【図６】さらに改良された補正値算出方法を示すタイミングチャートである。
【図７】他の実施形態の構成ブロック図である。
【図８Ａ】さらに他の実施形態の構成ブロック図である。
【図８Ｂ】さらに他の実施形態の構成ブロック図である。
【技術分野】
【０００１】
本発明は角速度センサ、特にロボット等の運動体に設けられる角速度センサの零点補正に関する。
【背景技術】
【０００２】
ロボット等の移動体の姿勢制御に加速度センサやヨーレートセンサが用いられている。直交する３軸をｘ軸、ｙ軸、ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りのヨーレートを３個のヨーレートセンサで検出する。軸回りの角度、あるいは姿勢角（ピッチ角、ロール角、ヨー角）は、ヨーレートセンサの出力を時間積分して得られる。
【０００３】
特開２００４−２６８７３０号公報には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。
【０００４】
しかしながら、角速度センサのオフセット及びドリフトが大きいと、角速度の積分により姿勢角を求めるため、オフセット等が徐々に蓄積して極めて大きな値となり、時間とともに増加、発散してしまう。これを防止するために、オフセットやドリフトの小さい角速度センサを用いればよいが、このような角速度センサは大型で重く、かつ高価である。
【発明の開示】
【発明が解決しようとする課題】
【０００５】
本発明の目的は、高価で高精度な角速度センサを用いることなく、簡易な構成で積分誤差の蓄積を防止し運動体の姿勢角を検出することができる装置および方法を提供することにある。
【課題を解決するための手段】
【０００６】
本発明の第１の態様は、運動体に設けられた角速度センサの零点補正装置に関する。この零点補正装置は、前記運動体の静止状態を検出する検出手段と、前記静止状態における前記角速度センサの出力値の平均値を算出する算出手段と、前記平均値を前記角速度センサの零点として補正する補正手段とを有する。
【０００７】
この零点補正装置では、角速度センサが設けられた運動体の静止状態を検出し、この静止状態における角速度センサの出力値を用いて角速度センサの零点を補正する。運動体の静止状態では、角速度センサの出力値は本来的に零であるはずであるから、静止状態における出力値は零点オフセットとなる。そこで、本発明では、静止状態における出力値の平均値をもって零点を補正し、角速度を積分して得られる姿勢角の誤差蓄積を防止する。
【０００８】
この零点補正装置では、運動体の静止状態は、角速度センサあるいは同一運動体に設けられた加速度センサの少なくともいずれかの出力の変化幅から検出できる。零点補正の精度を確保するためには静止状態が一定時間以上継続することが望しいため、所定の判定時間以上継続する静止状態での出力値の平均値から零点を補正してもよい。
【０００９】
本発明の第２の態様は、運動体に設けられた角速度センサの零点補正方法に関する。この方法は、前記運動体が静止状態かどうかを判定するステップと、前記静止状態における前記角速度センサの出力値の平均値を算出するステップと、前記平均値を前記角速度センサの零点として補正するステップとを備える。
【発明の効果】
【００１０】
本発明によれば、簡易な構成で角速度センサの零点を補正し、積分誤差の蓄積を防止して運動体の姿勢角を高精度に検出できる。
【発明を実施するための最良の形態】
【００１１】
以下、図面に基づき本発明の実施形態について説明する。
【００１２】
＜第１実施形態＞
図１に、本実施形態の構成ブロック図を示す。角速度センサ１０は、ロボット等の運動体の所定位置に設けられ、運動体の角速度を検出する。例えば、センサ座標系をｘｙｚ直交座標系とし、ｘ軸、ｙ軸、ｚ軸の各軸回りの角速度を検出する。角速度センサ１０は、検出した角速度を第１比較器１２に出力する。
【００１３】
第１比較器１２は、入力した角速度の変化幅（変動幅）を所定の範囲と大小比較する。所定の範囲は、第１設定器１４で設定される。第１設定器１４は、固定範囲を所定の範囲としてもよいが、図示のようにレジスタ１６に設定された範囲を所定範囲とし、ユーザが入力器１８を用いて所望の範囲をレジスタ１６に設定できるようにしてもよい。第１比較器１２は、角速度の変化幅と所定の範囲との大小比較結果を静止判定器２０に出力する。例えば、第１比較器１２は、角速度の変化幅が所定の範囲内である場合にＨｉ、角速度の変化幅が所定の範囲を超える場合にＬｏｗの信号を静止判定器２０に出力する。
【００１４】
静止判定器２０は、第１比較器１２からの比較結果を入力し、角速度センサ１０の出力が所定の範囲内に入った時刻（開始時刻ｔｏ）を検知する。また、角速度センサ１０の出力が所定の範囲内から逸脱した時刻（終了時刻ｔｅ）を検知する。また、開始時刻ｔｏからの経過時間（継続時間ｔｏｋ）を計測する。静止判定器２０は、継続時間ｔｏｋが所定の判定時間ｔｏｓを超えたか否かを判定し、超えたときに静止状態開始タイミング信号Ｔｓｔを出力器２８に出力する。所定の判定時間ｔｏｓは第２設定器２２から供給され、第２設定器２２の設定内容はレジスタ２４で適宜設定される。静止状態開始タイミング信号を出力した後、静止判定器２０は、角速度センサ１０の出力が所定の範囲内から逸脱した時刻（終了時刻ｔｅ）において終了タイミング信号Ｔｅｎを出力器２８に出力する。さらに、静止判定器２０は、静止状態開始タイミング信号ｔｓｔからの経過時間（静止状態継続時間ｔｓｋ）を計測する。静止状態継続時間ｔｓｋが所定の補正終了時間ｔｏｅを超えたときに、強制的に終了タイミング信号Ｔｅｎを出力器２８に出力する。
【００１５】
出力器２８は、静止状態開始タイミング信号Ｔｓｔ及び終了タイミング信号Ｔｅｎを補正時間管理器３２に出力する。補正時間管理器３２は、静止状態開始タイミング信号Ｔｓｔを受信し、ｎ−ｉ総和平均器３４での平均処理を開始させる。また、終了タイミング信号Ｔｅｎを受信し、ｎ−ｉ総和平均器３４での平均処理を終了させる。また、ｎ−ｉ総和平均器３４での−ｉ処理時間ｔｏｉをレジスタ３０を参照してｎ−ｉ総和平均器３４へ指示する。さらに、静止状態継続時間ｔｓｋが所定の補正最低時間ｔｏｈ以上の場合に零点補正値をｎ−ｉ総和平均の出力で更新して実施するようｎ−ｉ総和平均器３４を介して零点補正器３６へ命令する。静止状態継続時間ｔｓｋが所定の補正最低時間ｔｏｈ未満の場合には十分な精度が得られないと判定し、零点補正器３６における零点補正値は更新されない。
【００１６】
ｎ−ｉ総和平均器３４は、所定の時間分のｉ個のデータを保管するレジスタを有し、補正時間管理器３２からの指示でｉ個のデータ保管を開始する。ｎ−ｉ総和平均器３４は、ｉ個のデータが保管されるまで待ち、ｉ＋１個目のデータを受け取ったときに、１番目のデータから総和平均の演算を開始する。以下、１番目からｎ−ｉ番目のデータまでの総和平均の演算を繰り返す。そして、補正時間管理器３２からの終了指示でｎ−ｉ総和平均値を零点補正器３６へ出力する。零点補正器３６は、入力した平均値を新たな零点として更新することで補正する。このように１番目からｎ−ｉ番目のデータまでの総和平均を演算するのは、１番目からｎ番目のデータの総和平均を算出したのでは、角速度が実際に変化し始めてからのデータも算入してしまうため、その精度が低下するからである。ｔｅよりも所定時間ｔｏｉだけ遡った時点までのデータ１〜ｎ−ｉを用いて総和平均することで、零点補正値を正確に算出できる。
【００１７】
以下、角速度センサの出力値のタイミングチャートを用いて本実施形態の零点補正値算出演算を説明する。
【００１８】
図２に、角速度センサ１０から出力される角速度の時間変化を示す。図において、横軸は時間（ｓ）、縦軸は角速度（ｒａｄ／ｓ）である。基本演算は、開始時刻ｔｏで補正値用の総和平均演算を開始し、終了時刻ｔｅで補正値用総和平均演算を終了するものである。静止状態開始タイミング信号Ｔｓｔ及び終了タイミング信号Ｔｅｎがそれぞれ時刻ｔｏ、ｔｅで出力される。総和平均器３４からの総和平均値Ｍｅａｎ１は零点補正器３６に出力され、零点補正値を更新する。
【００１９】
図３に、改良された零点補正値算出方法を示す。図２において、補正行為の安定性を考慮し、静止状態判定開始時刻ｔｏではなく、継続時間ｔｏｋが所定の判定時間ｔｏｓを超えたときに（つまり、時刻ｔｏ＋ｔｏｓに）、静止状態開始タイミング信号Ｔｓｔを出力器２８に出力して補正値用総和平均演算を開始する。終了時刻ｔｅで終了タイミング信号Ｔｅｎを出力して補正値用総和平均演算を終了する。総和平均器３４で算出された補正値Ｍｅａｎ２で零点補正器３６の補正値を更新する。これにより、無意味な補正行為の発生を防止できる。静止開始直後の不安定なデータを平均処理に含めないため精度が向上する。
【００２０】
図４に、さらに改良された零点補正値算出方法を示す。図３において、終了時刻ｔｅより所定の時間、すなわち−ｉ処理時間ｔｏｉだけ遡ったタイミング（つまり時刻ｔｅ−ｔｏｉ）のデータに対して総和平均演算（ｎ−ｉ総和平均演算）を行い、ｎ−ｉ総和平均値Ｍｅａｎ３を零点補正器３６へ出力する。図４におけるＴｎ−ｉは、補正時間管理器３２が総和平均器３４に対して指示するタイミング信号である。これにより、終了時刻ｔｅ前の精度が極めて悪いデータ群を総和平均値から除き、精度を向上することができる。なお、総和平均器３４によって、ｔｏｉまたは、ｔｏｉに相当する出力値の数が変更されても良い。
【００２１】
図５に、さらに改良された零点補正値算出方法を示す。図２〜図４において、静止状態継続時間ｔｓｋを計測し、補正最低時間ｔｏｈ以上の場合に零点補正値を総和平均の出力で更新して実施すべく、総和平均器３４を介して零点補正器３６へ命令する。これにより、精度維持に必要なサンプル数が確保でき、精度の良い補正値更新が可能となる。
【００２２】
図６に、さらに改良された零点補正値算出方法を示す。図２〜図５において、静止状態継続時間ｔｓｋが所定の時間、すなわち補正終了時間ｔｏｅに達したときに強制的に終了タイミング信号Ｔｅｎを静止判定器２０が出力し、静止補正を終了させる。これにより、所定の時間で必要な精度を有する零点補正値が得られ、零点補正値を更新できる。以上の各算出方法において、判定時間ｔｏｓは０．１ｓ〜１ｓ程度であり、例えば０．３ｓ程度に設定してもよい。判定時間は短い方が早く静止補正に移行できるが、運動体の安定する応答性に依存する。静止まで時間のかかる運動体では長めに設定される。但し、余り長く（例えば５ｓ以上）設定すると静止補正に移行する頻度が低下するので全体精度が低下してしまう。
【００２３】
また、補正最低時間ｔｏｈは１ｓ〜１０ｓ程度でよく、８ｓ程度としてもよい。なるべく長く設定する方が補正値精度が向上するので好都合であるが、長くしすぎると静止補正を実施する頻度が低下するので全体精度が低下してしまう。従って、この時間も同様に運動体の動作に合わせて設定される。
【００２４】
−ｉ処理時間ｔｏｉは０．１ｓ〜１ｓ程度であり、０．３ｓ程度としてもよい。なるべく短い方が静止補正データ量が増大するが、運動体の起動する応答性に依存する。起動まで時間のかかる運動体では長めに設定される。余り長く（例えば５ｓ程度）設定すると静止補正に使用されるデータ量が減少し、逆に補正値精度が低下してしまうので、適当な値に設定してもよい。
【００２５】
補正終了時間ｔｏｅは１ｓ〜３００ｓ程度であり、３ｓ程度であってもよい。なるべく長い方が補正値精度が向上するが、長くしすぎると静止補正を実施する頻度が低下して全体精度が低下してしまう。運動体の動作に合わせて設定するのがよい。補正最低時間ｔｏｈよりも、少なくとも−ｉ処理時間ｔｏｉ分以上長めに設定する。
【００２６】
なお、第１比較器１２、静止判定器２０、補正時間管理器３２、総和平均器３４、零点補正器３６等は、具体的にはマイクロプロセッサで構成できる。
【００２７】
＜第２実施形態＞
図７に、本実施形態の構成ブロック図を示す。図１では、角速度センサ１０からの角速度を用いてロボットの静止状態を判定したが、本実施形態では同じロボットに設けられた加速度センサからの加速度を用いて静止状態を判定する。
【００２８】
加速度センサ４０は、ロボットのｘ軸、ｙ軸、ｚ軸各軸方向の加速度を検出して第２比較器４２に出力する。
【００２９】
第２比較器４２は、第３設定器４４に設定された所定範囲と加速度の変化幅とを大小比較し、比較結果を第３比較器４８に出力する。なお、第３設定器４４は、固定範囲を所定の範囲としてもよいが、図示のようにレジスタ４６に設定された範囲を所定範囲とし、ユーザが入力器を用いて所望の範囲をレジスタ４６に設定できるようにしてもよい。
【００３０】
第３比較器４８は、静止状態開始タイミング及び終了タイミングにおける加速度の絶対値が実質的に重力加速度（例えば９．７９７０７２ｍ／ｓ２、京都大学の国際基準点であり、「実質的」とは、重力加速度±許容誤差を意味し、許容誤差は加速度センサ４０に生じていると予想されるオフセットを考慮して設定される）であるか否か、あるいは当該期間における加速度が実質的に重力加速度であるか否かを判定する。これらの加速度が実質的に重力加速度ではない場合、静止状態ではないと判定する。一方、これらの加速度が実質的に重力加速度である場合、静止状態にあると判定し、静止判定器２０に出力する。その後の処理は第１実施形態と同様である。加速度を用いて静止状態を判定することで、並進運動や振動運動の検出が容易化され、角速度センサ１０の零点補正精度が向上する。
【００３１】
＜第３実施形態＞
図８Ａ，８Ｂに、本実施形態の構成ブロック図を示す。本実施形態では、角速度と加速度をともに用いて静止状態を判定する。
【００３２】
角速度を用いて静止状態を判定する構成は図１とほぼ同様であるが、第１比較器１２は比較結果を静止判定器２０ではなく組合せ比較器５０に出力する。
【００３３】
一方、加速度を用いて静止状態を判定する構成も図７とほぼ同様であるが、第３比較器４８は比較結果を静止判定器２０ではなく組合せ比較器５０に出力する。
【００３４】
組合せ比較器５０は、第１比較器１２からの比較結果、及び第３比較器４８からの比較結果を入力し、両者がともに範囲内にある場合を静止状態と判定し、判定結果を静止判定器２０に出力する。その後の処理は第１実施形態と同様である。
【００３５】
本実施形態では、角速度と加速度の組合せで静止状態を判定しているので、ロボットの並進及び回転の動きを監視できるのでより高精度に静止状態を判定し、角速度センサ１０の零点を補正できる。
【００３６】
以上、本発明の実施形態について説明したが、本発明はこれに限定されず種々の変更が可能である。
【００３７】
例えば、図１〜図８Ｂにおいて、有意の静止状態と判定するための判定時間ｔｏｓは入力器１８から所望の値を入力しレジスタ２４に設定することで調整できるが、ユーザの手動設定ではなく自動設定としてもよい。判定時間ではなく、図８Ｂの組合せ比較器５０における組合せの判定基準を変更することも可能である。所定時間経過しても零点補正が行われない場合、角速度あるいは加速度のいずれも静止状態と判定された場合に静止状態とするのではなく、角速度あるいは加速度のいずれかが静止状態と判定された場合に静止状態と判定して判定条件を緩和し、零点補正の早期実行を担保する等である。なお、レジスタ１６、２０、２４、３０、４６は１つの装置によって構成されていても良い。
【図面の簡単な説明】
【００３８】
【図１】実施形態の構成ブロック図である。
【図２】補正値算出方法を示すタイミングチャートである。
【図３】改良された補正値算出方法を示すタイミングチャートである。
【図４】さらに改良された補正値算出方法を示すタイミングチャートである。
【図５】さらに改良された補正値算出方法を示すタイミングチャートである。
【図６】さらに改良された補正値算出方法を示すタイミングチャートである。
【図７】他の実施形態の構成ブロック図である。
【図８Ａ】さらに他の実施形態の構成ブロック図である。
【図８Ｂ】さらに他の実施形態の構成ブロック図である。
【特許請求の範囲】
【請求項１】
運動体に設けられた角速度センサの零点補正装置であって、
前記運動体の静止状態を検出する検出手段と、
前記静止状態における前記角速度センサの出力値の平均値を算出する算出手段と、
前記平均値を前記角速度センサの零点として補正する補正手段と、
を有する零点補正装置。
【請求項２】
請求項１記載の零点補正装置において、
前記算出手段は、前記静止状態の開始タイミングから、前記静止状態の終了タイミングより所定時間だけ前までの前記角速度センサの出力値の平均値を算出する零点補正装置。
【請求項３】
請求項２記載の零点補正装置において、さらに、
前記所定時間を可変設定する手段
を有する零点補正装置。
【請求項４】
請求項１記載の零点補正装置において、
前記算出手段は、前記静止状態の開始タイミングから、前記静止状態の終了タイミングまでに出力された角速度センサの出力値のうち、終了タイミングの直前の所定個数の前記出力値を除いた出力値の平均値を算出する零点補正装置。
【請求項５】
請求項４記載の零点補正装置において、さらに、
前記所定個数を可変設定する手段
を有する零点補正装置。
【請求項６】
請求項１〜５のいずれか１に記載の零点補正装置において、
前記検出手段は、前記出力値の変化幅が第１の所定幅以下であるか否かに基づき静止状態を検出する零点補正装置。
【請求項７】
請求項１〜５のいずれか１に記載の零点補正装置において、
前記検出手段は、前記運動体に設けられた加速度センサの出力値の変化幅が第２の所定幅以下であるか否かに基づき静止状態を検出する零点補正装置。
【請求項８】
請求項１〜５のいずれか１に記載の零点補正装置において、
前記検出手段は、前記角速度センサの出力値の変化幅が第１の所定幅以下であり、かつ、前記運動体に設けられた加速度センサの出力値の変化幅が第２の所定幅以下であるか否かに基づき静止状態を検出する零点補正装置。
【請求項９】
請求項７、８のいずれかに記載の零点補正装置において、
前記検出手段は、さらに、前記加速度センサの出力値の絶対値が実質的に重力加速度であるか否かに基づき静止状態を検出する零点補正装置。
【請求項１０】
請求項６〜９のいずれか１に記載の零点補正装置において、さらに、
前記第１の所定幅と第２の所定幅とのうち少なくとも一方を可変設定する手段
を有する零点補正装置。
【請求項１１】
請求項１〜１０のいずれか１に記載の零点補正装置において、
前記検出手段は、所定の判定時間以上継続する静止状態を検出する零点補正装置。
【請求項１２】
請求項１１記載の零点補正装置において、さらに、
前記所定の判定時間を可変設定する手段
を有する零点補正装置。
【請求項１３】
運動体に設けられた角速度センサの零点補正方法であって、
前記運動体が静止状態かどうかを判定し、
前記静止状態における前記角速度センサの出力値の平均値を算出し、
前記平均値を前記角速度センサの零点として補正する
零点補正方法。
【請求項１４】
運動体に設けられた角速度センサの零点補正装置であって、
前記運動体の静止状態を検出する検出器と、
前記静止状態における前記角速度センサの出力値の平均値を算出し、前記平均値を前記角速度センサの零点として補正するコントローラ
を有する零点補正装置。
【請求項１】
運動体に設けられた角速度センサの零点補正装置であって、
前記運動体の静止状態を検出する検出手段と、
前記静止状態における前記角速度センサの出力値の平均値を算出する算出手段と、
前記平均値を前記角速度センサの零点として補正する補正手段と、
を有する零点補正装置。
【請求項２】
請求項１記載の零点補正装置において、
前記算出手段は、前記静止状態の開始タイミングから、前記静止状態の終了タイミングより所定時間だけ前までの前記角速度センサの出力値の平均値を算出する零点補正装置。
【請求項３】
請求項２記載の零点補正装置において、さらに、
前記所定時間を可変設定する手段
を有する零点補正装置。
【請求項４】
請求項１記載の零点補正装置において、
前記算出手段は、前記静止状態の開始タイミングから、前記静止状態の終了タイミングまでに出力された角速度センサの出力値のうち、終了タイミングの直前の所定個数の前記出力値を除いた出力値の平均値を算出する零点補正装置。
【請求項５】
請求項４記載の零点補正装置において、さらに、
前記所定個数を可変設定する手段
を有する零点補正装置。
【請求項６】
請求項１〜５のいずれか１に記載の零点補正装置において、
前記検出手段は、前記出力値の変化幅が第１の所定幅以下であるか否かに基づき静止状態を検出する零点補正装置。
【請求項７】
請求項１〜５のいずれか１に記載の零点補正装置において、
前記検出手段は、前記運動体に設けられた加速度センサの出力値の変化幅が第２の所定幅以下であるか否かに基づき静止状態を検出する零点補正装置。
【請求項８】
請求項１〜５のいずれか１に記載の零点補正装置において、
前記検出手段は、前記角速度センサの出力値の変化幅が第１の所定幅以下であり、かつ、前記運動体に設けられた加速度センサの出力値の変化幅が第２の所定幅以下であるか否かに基づき静止状態を検出する零点補正装置。
【請求項９】
請求項７、８のいずれかに記載の零点補正装置において、
前記検出手段は、さらに、前記加速度センサの出力値の絶対値が実質的に重力加速度であるか否かに基づき静止状態を検出する零点補正装置。
【請求項１０】
請求項６〜９のいずれか１に記載の零点補正装置において、さらに、
前記第１の所定幅と第２の所定幅とのうち少なくとも一方を可変設定する手段
を有する零点補正装置。
【請求項１１】
請求項１〜１０のいずれか１に記載の零点補正装置において、
前記検出手段は、所定の判定時間以上継続する静止状態を検出する零点補正装置。
【請求項１２】
請求項１１記載の零点補正装置において、さらに、
前記所定の判定時間を可変設定する手段
を有する零点補正装置。
【請求項１３】
運動体に設けられた角速度センサの零点補正方法であって、
前記運動体が静止状態かどうかを判定し、
前記静止状態における前記角速度センサの出力値の平均値を算出し、
前記平均値を前記角速度センサの零点として補正する
零点補正方法。
【請求項１４】
運動体に設けられた角速度センサの零点補正装置であって、
前記運動体の静止状態を検出する検出器と、
前記静止状態における前記角速度センサの出力値の平均値を算出し、前記平均値を前記角速度センサの零点として補正するコントローラ
を有する零点補正装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８Ａ】
【図８Ｂ】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８Ａ】
【図８Ｂ】
【公表番号】特表２００８−５４２７８２（Ｐ２００８−５４２７８２Ａ）
【公表日】平成２０年１１月２７日（２００８．１１．２７）
【国際特許分類】
【出願番号】特願２００８−５１５３１６（Ｐ２００８−５１５３１６）
【出願日】平成１８年８月１日（２００６．８．１）
【国際出願番号】ＰＣＴ／ＩＢ２００６／００２０８７
【国際公開番号】ＷＯ２００７／０１５１３７
【国際公開日】平成１９年２月８日（２００７．２．８）
【新規性喪失の例外の表示】特許法第３０条第３項適用申請有り
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公表日】平成２０年１１月２７日（２００８．１１．２７）
【国際特許分類】
【出願日】平成１８年８月１日（２００６．８．１）
【国際出願番号】ＰＣＴ／ＩＢ２００６／００２０８７
【国際公開番号】ＷＯ２００７／０１５１３７
【国際公開日】平成１９年２月８日（２００７．２．８）
【新規性喪失の例外の表示】特許法第３０条第３項適用申請有り
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]