姿勢角検出装置及び姿勢角検出用プログラム
【課題】 測定対象部分の動きに拘らず、その姿勢角を正確に測定可能にすること。
【解決手段】 測定対象部分の回転角速度を検出するジャイロセンサ１２と、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサ１３と、ジャイロセンサ１２及び加速度センサ１３からの検出値に基づいて、前記測定対象部分の姿勢角を演算する演算手段１５とを備えて姿勢角検出装置１０が構成されている。演算手段１５は、ジャイロセンサ１２の検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求める角速度処理部１７と、加速度センサ１３の検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求める加速度処理部１８と、これら角速度処理部１７及び加速度処理部１８で求めた各回転角度を加算して前記姿勢角を求める姿勢角決定部２０とを備えている。
【解決手段】 測定対象部分の回転角速度を検出するジャイロセンサ１２と、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサ１３と、ジャイロセンサ１２及び加速度センサ１３からの検出値に基づいて、前記測定対象部分の姿勢角を演算する演算手段１５とを備えて姿勢角検出装置１０が構成されている。演算手段１５は、ジャイロセンサ１２の検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求める角速度処理部１７と、加速度センサ１３の検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求める加速度処理部１８と、これら角速度処理部１７及び加速度処理部１８で求めた各回転角度を加算して前記姿勢角を求める姿勢角決定部２０とを備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、姿勢角検出装置及び姿勢角検出用プログラムに係り、更に詳しくは、測定対象部分の姿勢角をより高精度に検出することのできる姿勢角検出装置及び姿勢角検出用プログラムに関する。
【背景技術】
【０００２】
物体の姿勢角を検出する手法としては、ジャイロセンサで検出された角速度を積分することにより姿勢角を求める方法や、複数の加速度センサで検出された加速度から幾何学的に姿勢角を求める方法が知られている。
【０００３】
ところが、前記ジャイロセンサを使った方法では、所定周波数以下の低周波成分において、温度ドリフトや積分誤差等による測定誤差が発生するという問題があり、遅い動きを伴う物体の姿勢角の検出には不向きである。
【０００４】
一方、前記加速度センサを使った方法では、所定周波数以上の高周波成分において、加速運動による慣性の影響に起因した測定誤差が発生するという問題があり、早い動きを伴う物体の姿勢角の検出には不向きである。
【０００５】
ところで、特開平９−２２９６６７号に開示されている動作計測装置は、測定対象となる可動部材に取り付けられるジャイロセンサ及び加速度センサと、前記ジャイロセンサからの角速度信号及び前記加速度センサからの加速度信号に基づいて前記可動部材の姿勢角を計算する計算手段とを備えている。この計算手段は、角速度信号及び加速度信号をパラメータとする加速度方程式を使って、周波数成分と無関係に前記姿勢角を求めるようになっている。
【特許文献１】特開平９−２２９６６７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、前記動作計測装置にあっては、積分計算を経ないことから積分誤差は生じないものの、周波数成分を考慮していないため、前述した温度ドリフトによるジャイロセンサの測定誤差や加速度センサによる測定誤差が依然発生し、前記姿勢角の測定を正確に行うのには不十分である。
【０００７】
本発明は、このような不都合に着目して案出されたものであり、その目的は、測定対象部分の動きに拘らず、当該測定対象部分の姿勢角を正確に測定することができる姿勢角検出装置及び姿勢角検出用プログラムを提供することにある。
【課題を解決するための手段】
【０００８】
（１）前記目的を達成するため、本発明は、測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサと、前記ジャイロセンサ及び前記加速度センサからの検出値に基づいて、前記測定対象部分の姿勢角を演算する演算手段とを備えた姿勢角検出装置において、
前記演算手段は、前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求める角速度処理部と、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求める加速度処理部と、これら角速度処理部及び加速度処理部で求めた各回転角度を加算して前記姿勢角を求める姿勢角決定部とを備える、という構成を採っている。
【０００９】
（２）また、本発明は、測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサとの検出値に基づいて、コンピュータに前記測定対象部分の姿勢角を演算させるためのプログラムであって、
前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求めるステップと、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求めるステップと、これら各ステップで求めた各回転角度を加算して前記姿勢角を求めるステップとを前記コンピュータに実行させる、という構成を採っている。
【発明の効果】
【００１０】
本発明によれば、加速度センサを使った場合に測定誤差が大きくなる高周波側では、主としてジャイロセンサの検出値を使って姿勢角を求める一方、ジャイロセンサを使った場合に測定誤差が大きくなる低周波側では、主として加速度センサの検出値を使って姿勢角を求めるため、ジャイロセンサ及び加速度センサによる測定誤差の影響を殆ど受けることなく、測定対象部分の早い動きに対しても遅い動きに対しても、姿勢角を正確に検出することができる。
【発明を実施するための最良の形態】
【００１１】
以下、本発明の実施例について図面を参照しながら説明する。
【実施例】
【００１２】
図１には、本実施例に係る姿勢角検出装置の全体構成を表すブロック図が示されている。この図において、姿勢角検出装置１０は、測定対象部分における対象方向の回転角速度を検出するジャイロセンサ１２と、測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサ１３と、これらジャイロセンサ１２及び加速度センサ１３からの検出値に基づいて、前記対象方向の姿勢角を演算する演算手段１５とを備えて構成されている。
【００１３】
前記ジャイロセンサ１２及び加速度センサ１３は、公知の構造のものが用いられており、ここではその詳細な説明を省略するが、後述する方向の回転角速度及び加速度が検出できる限りにおいて、種々のものを採用することができる。前記ジャイロセンサ１２は、図２に示された軸線Ｌ回りとなる前記対象方向の回転角速度ωを検出可能に設けられている。一方、前記加速度センサは、図２に示されるように、軸線Ｌにそれぞれ直交する水平方向の水平加速度ａｈ及び垂直加速度ａｎを検出可能に設けられている。ここで、水平加速度ａｈ、垂直加速度ａｎは、重力方向の加速度に対して、測定対象部分Ｐにおける水平方向、垂直方向の分力を意味する。
【００１４】
前記演算手段１５は、前記ジャイロセンサ１２及び加速度センサ１３の検出値から後述する演算を実行可能なプログラムがインストールされたコンピュータによって構成されている。この演算手段１５は、ジャイロセンサ１２の検出値から、所定周波数以上の高周波成分における前記対象方向の回転角度を演算する角速度処理部１７と、加速度センサ１３の検出値から、所定周波数未満の低周波数成における前記対象方向の回転角度を演算する加速度処理部１８と、これら角速度処理部１７及び加速度処理部１８で求めた各回転角度を加算して、前記対象方向の姿勢角を求める姿勢角決定部２０とを備えている。
【００１５】
前記角速度処理部１７は、図３に示されるように、ジャイロセンサ１２からの入力値である回転角速度ωを積分して求められた回転角度に対し、数Ｈｚ〜数十Ｈｚの周波数成分以上の高周波成分のみを取り出すよう二重積分型のローパスフィルタで得られた値をフィードバックしたハイパスフィルタが設定されている。
具体的には、次式で求められる。
【数１】
【００１６】
前記加速度処理部１８は、図３に示されるように、加速度センサ１３からの入力値である各加速度ａｈ，ａｎに対し、数Ｈｚ〜数十Ｈｚの周波数成分以下の低周波成分のみを取り出すカスケード積分くし型のローパスフィルタが設定されており、当該ローパスフィルタで取り出された低周波成分に対し、各加速度ａｈ，ａｎのａｒｃｔａｎを取ることにより、静的な回転角度θを求めるようになっている。
具体的には、次式で求められる。ここで、次式（３）〜（５）にあっては、水平加速度ａｈ、垂直加速度ａｎの場合で同一の演算となるため、これら加速度ａｈ，ａｎを総称して、ａｉと表している。
【数２】
これら（３）〜（５）で求められた低周波成分の水平加速度ａｈ及び垂直加速度ａｎから、ａｒｃｔａｎを取って回転角度θが求められる。
【００１７】
前記姿勢角決定部２０では、図３に示されるように、角速度処理部１７で求められた回転角度θと、加速度処理部１８で求められた回転角度θとを、それぞれ所定のゲインを乗じた上で加算され、姿勢角θ＊（推定角度）が求められることになる。具体的には、次式（６）で求められる。
【数３】
【００１８】
従って、このような実施例によれば、ジャイロセンサ１２の測定誤差が大きくなる低周波領域では、加速度センサ１３による検出値を主として使う一方、加速度センサ１３の測定誤差が大きくなる高周波領域では、ジャイロセンサ１２による検出値を主として使うように機能するため、各センサ１２，１３の弱点が相互に補完され、当該各センサ１２，１３による測定誤差の影響を受け難くなり、測定対象部分の姿勢角を正確に測定できるという効果を得る。
【００１９】
なお、本発明における装置各部の構成は図示構成例に限定されるものではなく、実質的に同様の作用を奏する限りにおいて、種々の変更が可能である。
【図面の簡単な説明】
【００２０】
【図１】本実施例に係る姿勢角検出装置の全体構成を表すブロック図。
【図２】ジャイロセンサ及び加速度センサによる測定方向を説明するための図。
【図３】演算手段における演算内容を説明するためのブロック線図。
【符号の説明】
【００２１】
１０ 姿勢角検出装置
１２ ジャイロセンサ
１３ 加速度センサ
１５ 演算手段
１７ 角速度処理部
１８ 加速度処理部
２０ 姿勢角決定部
【技術分野】
【０００１】
本発明は、姿勢角検出装置及び姿勢角検出用プログラムに係り、更に詳しくは、測定対象部分の姿勢角をより高精度に検出することのできる姿勢角検出装置及び姿勢角検出用プログラムに関する。
【背景技術】
【０００２】
物体の姿勢角を検出する手法としては、ジャイロセンサで検出された角速度を積分することにより姿勢角を求める方法や、複数の加速度センサで検出された加速度から幾何学的に姿勢角を求める方法が知られている。
【０００３】
ところが、前記ジャイロセンサを使った方法では、所定周波数以下の低周波成分において、温度ドリフトや積分誤差等による測定誤差が発生するという問題があり、遅い動きを伴う物体の姿勢角の検出には不向きである。
【０００４】
一方、前記加速度センサを使った方法では、所定周波数以上の高周波成分において、加速運動による慣性の影響に起因した測定誤差が発生するという問題があり、早い動きを伴う物体の姿勢角の検出には不向きである。
【０００５】
ところで、特開平９−２２９６６７号に開示されている動作計測装置は、測定対象となる可動部材に取り付けられるジャイロセンサ及び加速度センサと、前記ジャイロセンサからの角速度信号及び前記加速度センサからの加速度信号に基づいて前記可動部材の姿勢角を計算する計算手段とを備えている。この計算手段は、角速度信号及び加速度信号をパラメータとする加速度方程式を使って、周波数成分と無関係に前記姿勢角を求めるようになっている。
【特許文献１】特開平９−２２９６６７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、前記動作計測装置にあっては、積分計算を経ないことから積分誤差は生じないものの、周波数成分を考慮していないため、前述した温度ドリフトによるジャイロセンサの測定誤差や加速度センサによる測定誤差が依然発生し、前記姿勢角の測定を正確に行うのには不十分である。
【０００７】
本発明は、このような不都合に着目して案出されたものであり、その目的は、測定対象部分の動きに拘らず、当該測定対象部分の姿勢角を正確に測定することができる姿勢角検出装置及び姿勢角検出用プログラムを提供することにある。
【課題を解決するための手段】
【０００８】
（１）前記目的を達成するため、本発明は、測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサと、前記ジャイロセンサ及び前記加速度センサからの検出値に基づいて、前記測定対象部分の姿勢角を演算する演算手段とを備えた姿勢角検出装置において、
前記演算手段は、前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求める角速度処理部と、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求める加速度処理部と、これら角速度処理部及び加速度処理部で求めた各回転角度を加算して前記姿勢角を求める姿勢角決定部とを備える、という構成を採っている。
【０００９】
（２）また、本発明は、測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサとの検出値に基づいて、コンピュータに前記測定対象部分の姿勢角を演算させるためのプログラムであって、
前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求めるステップと、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求めるステップと、これら各ステップで求めた各回転角度を加算して前記姿勢角を求めるステップとを前記コンピュータに実行させる、という構成を採っている。
【発明の効果】
【００１０】
本発明によれば、加速度センサを使った場合に測定誤差が大きくなる高周波側では、主としてジャイロセンサの検出値を使って姿勢角を求める一方、ジャイロセンサを使った場合に測定誤差が大きくなる低周波側では、主として加速度センサの検出値を使って姿勢角を求めるため、ジャイロセンサ及び加速度センサによる測定誤差の影響を殆ど受けることなく、測定対象部分の早い動きに対しても遅い動きに対しても、姿勢角を正確に検出することができる。
【発明を実施するための最良の形態】
【００１１】
以下、本発明の実施例について図面を参照しながら説明する。
【実施例】
【００１２】
図１には、本実施例に係る姿勢角検出装置の全体構成を表すブロック図が示されている。この図において、姿勢角検出装置１０は、測定対象部分における対象方向の回転角速度を検出するジャイロセンサ１２と、測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサ１３と、これらジャイロセンサ１２及び加速度センサ１３からの検出値に基づいて、前記対象方向の姿勢角を演算する演算手段１５とを備えて構成されている。
【００１３】
前記ジャイロセンサ１２及び加速度センサ１３は、公知の構造のものが用いられており、ここではその詳細な説明を省略するが、後述する方向の回転角速度及び加速度が検出できる限りにおいて、種々のものを採用することができる。前記ジャイロセンサ１２は、図２に示された軸線Ｌ回りとなる前記対象方向の回転角速度ωを検出可能に設けられている。一方、前記加速度センサは、図２に示されるように、軸線Ｌにそれぞれ直交する水平方向の水平加速度ａｈ及び垂直加速度ａｎを検出可能に設けられている。ここで、水平加速度ａｈ、垂直加速度ａｎは、重力方向の加速度に対して、測定対象部分Ｐにおける水平方向、垂直方向の分力を意味する。
【００１４】
前記演算手段１５は、前記ジャイロセンサ１２及び加速度センサ１３の検出値から後述する演算を実行可能なプログラムがインストールされたコンピュータによって構成されている。この演算手段１５は、ジャイロセンサ１２の検出値から、所定周波数以上の高周波成分における前記対象方向の回転角度を演算する角速度処理部１７と、加速度センサ１３の検出値から、所定周波数未満の低周波数成における前記対象方向の回転角度を演算する加速度処理部１８と、これら角速度処理部１７及び加速度処理部１８で求めた各回転角度を加算して、前記対象方向の姿勢角を求める姿勢角決定部２０とを備えている。
【００１５】
前記角速度処理部１７は、図３に示されるように、ジャイロセンサ１２からの入力値である回転角速度ωを積分して求められた回転角度に対し、数Ｈｚ〜数十Ｈｚの周波数成分以上の高周波成分のみを取り出すよう二重積分型のローパスフィルタで得られた値をフィードバックしたハイパスフィルタが設定されている。
具体的には、次式で求められる。
【数１】
【００１６】
前記加速度処理部１８は、図３に示されるように、加速度センサ１３からの入力値である各加速度ａｈ，ａｎに対し、数Ｈｚ〜数十Ｈｚの周波数成分以下の低周波成分のみを取り出すカスケード積分くし型のローパスフィルタが設定されており、当該ローパスフィルタで取り出された低周波成分に対し、各加速度ａｈ，ａｎのａｒｃｔａｎを取ることにより、静的な回転角度θを求めるようになっている。
具体的には、次式で求められる。ここで、次式（３）〜（５）にあっては、水平加速度ａｈ、垂直加速度ａｎの場合で同一の演算となるため、これら加速度ａｈ，ａｎを総称して、ａｉと表している。
【数２】
これら（３）〜（５）で求められた低周波成分の水平加速度ａｈ及び垂直加速度ａｎから、ａｒｃｔａｎを取って回転角度θが求められる。
【００１７】
前記姿勢角決定部２０では、図３に示されるように、角速度処理部１７で求められた回転角度θと、加速度処理部１８で求められた回転角度θとを、それぞれ所定のゲインを乗じた上で加算され、姿勢角θ＊（推定角度）が求められることになる。具体的には、次式（６）で求められる。
【数３】
【００１８】
従って、このような実施例によれば、ジャイロセンサ１２の測定誤差が大きくなる低周波領域では、加速度センサ１３による検出値を主として使う一方、加速度センサ１３の測定誤差が大きくなる高周波領域では、ジャイロセンサ１２による検出値を主として使うように機能するため、各センサ１２，１３の弱点が相互に補完され、当該各センサ１２，１３による測定誤差の影響を受け難くなり、測定対象部分の姿勢角を正確に測定できるという効果を得る。
【００１９】
なお、本発明における装置各部の構成は図示構成例に限定されるものではなく、実質的に同様の作用を奏する限りにおいて、種々の変更が可能である。
【図面の簡単な説明】
【００２０】
【図１】本実施例に係る姿勢角検出装置の全体構成を表すブロック図。
【図２】ジャイロセンサ及び加速度センサによる測定方向を説明するための図。
【図３】演算手段における演算内容を説明するためのブロック線図。
【符号の説明】
【００２１】
１０ 姿勢角検出装置
１２ ジャイロセンサ
１３ 加速度センサ
１５ 演算手段
１７ 角速度処理部
１８ 加速度処理部
２０ 姿勢角決定部
【特許請求の範囲】
【請求項１】
測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサと、前記ジャイロセンサ及び前記加速度センサからの検出値に基づいて、前記測定対象部分の姿勢角を演算する演算手段とを備えた姿勢角検出装置において、
前記演算手段は、前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求める角速度処理部と、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求める加速度処理部と、これら角速度処理部及び加速度処理部で求めた各回転角度を加算して前記姿勢角を求める姿勢角決定部とを備えたことを特徴とする姿勢角検出装置。
【請求項２】
測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサとの検出値に基づいて、コンピュータに前記測定対象部分の姿勢角を演算させるためのプログラムであって、
前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求めるステップと、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求めるステップと、これら各ステップで求めた各回転角度を加算して前記姿勢角を求めるステップとを前記コンピュータに実行させることを特徴とする姿勢角検出用プログラム。
【請求項１】
測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサと、前記ジャイロセンサ及び前記加速度センサからの検出値に基づいて、前記測定対象部分の姿勢角を演算する演算手段とを備えた姿勢角検出装置において、
前記演算手段は、前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求める角速度処理部と、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求める加速度処理部と、これら角速度処理部及び加速度処理部で求めた各回転角度を加算して前記姿勢角を求める姿勢角決定部とを備えたことを特徴とする姿勢角検出装置。
【請求項２】
測定対象部分の回転角速度を検出するジャイロセンサと、前記測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサとの検出値に基づいて、コンピュータに前記測定対象部分の姿勢角を演算させるためのプログラムであって、
前記ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求めるステップと、前記加速度センサの検出値から、所定周波数未満の低周波成分における前記対象方向の回転角度を求めるステップと、これら各ステップで求めた各回転角度を加算して前記姿勢角を求めるステップとを前記コンピュータに実行させることを特徴とする姿勢角検出用プログラム。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２００７−６４８５４（Ｐ２００７−６４８５４Ａ）
【公開日】平成１９年３月１５日（２００７．３．１５）
【国際特許分類】
【出願番号】特願２００５−２５２８９６（Ｐ２００５−２５２８９６）
【出願日】平成１７年８月３１日（２００５．８．３１）
【出願人】（８９９００００６８）学校法人早稲田大学 (602)
【出願人】（５０１０５５１４５）株式会社ゼットエムピー (5)
【Ｆターム（参考）】
【公開日】平成１９年３月１５日（２００７．３．１５）
【国際特許分類】
【出願日】平成１７年８月３１日（２００５．８．３１）
【出願人】（８９９００００６８）学校法人早稲田大学 (602)
【出願人】（５０１０５５１４５）株式会社ゼットエムピー (5)
【Ｆターム（参考）】
[ Back to top ]