JP6848571B2 - 姿勢算出装置、姿勢計測システム、及び姿勢算出方法 - Google Patents
姿勢算出装置、姿勢計測システム、及び姿勢算出方法 Download PDFInfo
- Publication number
- JP6848571B2 JP6848571B2 JP2017055616A JP2017055616A JP6848571B2 JP 6848571 B2 JP6848571 B2 JP 6848571B2 JP 2017055616 A JP2017055616 A JP 2017055616A JP 2017055616 A JP2017055616 A JP 2017055616A JP 6848571 B2 JP6848571 B2 JP 6848571B2
- Authority
- JP
- Japan
- Prior art keywords
- sensor
- value
- angular velocity
- posture
- period
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Gyroscopes (AREA)
Description
本発明は、姿勢算出装置、姿勢計測システム、及び姿勢算出方法に関する。
角速度センサ、加速度センサ及び地磁気センサが搭載されたセンサデバイスを用いて、運動中の物体の姿勢を計測する方法がある。角速度センサの検出結果によれば、ある時刻から次の時刻への姿勢変化が得られる。したがって、この姿勢変化を積算することで、姿勢のトラッキングが可能である。一方で、角速度センサの出力値には、誤差が含まれているため、角速度センサの出力値を積算し続けると、算出される姿勢は徐々に真値から外れていく。このため、角速度センサの出力値を補正する必要がある。
例えば、特許文献１には、加速度センサによって検出される重力方向及び地磁気センサによって検出される地磁気方向と、角速度センサの出力値に基づいて算出される姿勢の重力方向及び地磁気方向との比較により、角速度センサの出力値を補正することについて開示されている。
例えば特許文献１に係る技術のように、加速度センサ及び地磁気センサの出力値を用いて角速度センサの出力値を補正する場合において、加速度センサ及び地磁気センサの出力にも、環境等の様々な要因で局所的なノイズが重畳することがある。角速度センサの補正にこのような局所的なノイズが影響すると、角速度センサの出力値に基づく姿勢算出が正確に行われなくなる。
本発明は、角速度を用いた正確な姿勢算出を行える、姿勢算出装置、姿勢計測システム、及び姿勢算出方法を提供することを目的とする。
本発明の一態様によれば、姿勢算出装置は、角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得する参照姿勢取得部と、前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出する方向算出部と、前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出する角度差算出部と、前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部とを備える。
本発明の一態様によれば、姿勢算出装置は、角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得する参照姿勢取得部と、前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出する方向算出部と、前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出する角度差算出部と、前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部とを備える。
本発明の一態様によれば、姿勢計測システムは、上述の姿勢算出装置と、前記センサユニットとを備える。
本発明の一態様によれば、姿勢算出方法は、角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得することと、前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出することと、前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出することと、前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することとを含む。
本発明の一態様によれば、姿勢算出方法は、角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得することと、前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出することと、前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出することと、前記参照角度差に基づいて前記角速度値を補正する補正値を算出することと、前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することとを含む。
本発明によれば、角速度を用いた正確な姿勢算出を行える、姿勢算出装置、姿勢計測システム、及び姿勢算出方法を提供できる。
［システム構成］
本発明の一実施形態について図面を参照して説明する。本実施形態は、センサユニットに設けられた角速度センサの検出値に基づいてセンサユニットの姿勢を算出する姿勢計測システムに関する。検出された角速度を時間経過に沿って積算することで、すなわち時間経過に沿って姿勢を回転させてトラッキングしていくことで、センサユニットの姿勢は高精度で求められ得る。ただし、角速度センサは誤差を有している。このため、検出結果を積算すると誤差も蓄積され、算出される姿勢は正しい値から離れていく。そこで本実施形態では、角速度センサとの位置関係が固定された加速度センサ及び地磁気センサを用いて計測された重力方向及び地磁気方向に基づいて、角速度センサの出力を補正する補正値が算出される。この際、加速度センサ及び地磁気センサの短期的な検出誤差の影響を受けないようにするために、ある程度長い期間の各センサの出力を用いて補正値が算出される。
本発明の一実施形態について図面を参照して説明する。本実施形態は、センサユニットに設けられた角速度センサの検出値に基づいてセンサユニットの姿勢を算出する姿勢計測システムに関する。検出された角速度を時間経過に沿って積算することで、すなわち時間経過に沿って姿勢を回転させてトラッキングしていくことで、センサユニットの姿勢は高精度で求められ得る。ただし、角速度センサは誤差を有している。このため、検出結果を積算すると誤差も蓄積され、算出される姿勢は正しい値から離れていく。そこで本実施形態では、角速度センサとの位置関係が固定された加速度センサ及び地磁気センサを用いて計測された重力方向及び地磁気方向に基づいて、角速度センサの出力を補正する補正値が算出される。この際、加速度センサ及び地磁気センサの短期的な検出誤差の影響を受けないようにするために、ある程度長い期間の各センサの出力を用いて補正値が算出される。
本実施形態に係る姿勢計測システム１の構成例の概略を図１に示す。姿勢計測システム１は、センサユニット１０と解析ユニット２０とを含む。本実施形態では、センサユニット１０は、角速度、加速度、地磁気方向を検出するセンサを有しており、任意の期間の検出結果を保存する。解析ユニット２０は、センサユニット１０に記録された任意の期間の角速度、加速度、地磁気方向に係る情報を読み出して、これらに基づいて解析を行いセンサユニット１０の姿勢を算出する。解析ユニット２０は、姿勢算出装置として機能する。
センサユニット１０は、バスライン１９を介して互いに接続された角速度センサ１１と、加速度センサ１２と、地磁気センサ１３と、プロセッサ１４と、random access memory（ＲＡＭ）１５と、フラッシュメモリ１６と、入力装置１７と、インターフェイス（Ｉ／Ｆ）１８とを備える。
角速度センサ１１は、例えばＭＥＭＳ角速度センサが３軸方向に設けられた構成を有し、各軸周りの角速度を検出する。加速度センサ１２は、例えばＭＥＭＳ加速度センサが３軸方向に設けられた構成を有し、各軸方向の加速度を検出する。加速度センサ１２の検出結果に基づけば、重力方向が求められる。地磁気センサ１３は、例えば３軸地磁気センサであり、地磁気方向を検出する。
プロセッサ１４は、例えばCentral Processing Unit（ＣＰＵ）、Application Specific Integrated Circuit（ＡＳＩＣ）、Field Programmable Gate Array（ＦＰＧＡ）等といった集積回路であり、各種信号処理等を行う。ＲＡＭ１５は、プロセッサ１４の主記憶装置として機能する。フラッシュメモリ１６には、角速度センサ１１、加速度センサ１２、地磁気センサ１３により検出され、プロセッサ１４で処理された角速度情報、加速度情報、地磁気情報等が記録される。また、フラッシュメモリ１６には、プロセッサ１４で用いられるプログラム、パラメータ等各種情報も記録されている。ＲＡＭ１５及びフラッシュメモリ１６は、これに限らず各種記憶装置に置換され得る。入力装置１７は、例えばスイッチ等、ユーザの入力を受け付ける装置であり、例えばセンサユニット１０の起動や計測の開始及び終了の指示が入力される。Ｉ／Ｆ１８は、センサユニット１０の外部とデータの送受信を行うためのインターフェイスである。
解析ユニット２０は、バスライン２９によって互いに接続されたプロセッサ２１と、ＲＡＭ２２と、記録装置２３と、ディスプレイ２４と、入力装置２５と、Ｉ／Ｆ２６とを備える。Ｉ／Ｆ２６は、解析ユニット２０の外部とデータの送受信を行うためのインターフェイスであり、例えばセンサユニット１０と通信を行う。センサユニット１０と解析ユニット２０との間の通信は、有線で行われても、無線で行われてもよい。
プロセッサ２１は、例えばＣＰＵ、ＡＳＩＣ、ＦＰＧＡ等といった集積回路であり、各種信号処理等を行う。プロセッサ２１は、センサユニット１０から読み出した、角速度情報、加速度情報、地磁気情報に基づいて、センサユニット１０の姿勢を算出する。ＲＡＭ２２は、プロセッサ２１の主記憶装置として機能する。
記録装置２３には、プロセッサ２１で用いられるプログラム、パラメータ等各種情報が記録されている。また、記録装置２３には、センサユニット１０から読み出された検出結果、プロセッサ２１で算出されたセンサユニット１０の姿勢等が記録される。ＲＡＭ２２及び記録装置２３は、これに限らず各種記憶装置に置換され得る。ディスプレイ２４は、これに限らないが例えば液晶ディスプレイ等であり、センサユニット１０の姿勢等は、ディスプレイ２４に表示される。入力装置２５は、例えばキーボード、マウス等である。解析ユニット２０は、例えばパーソナルコンピュータ（ＰＣ）等であり得る。
姿勢計測システム１の使用状況の一例を図２に模式的に示す。図２の例において、センサユニット１０は、計測対象である人物９０の腰などの体幹に装着される。この状態で人物９０が運動をする。その結果、センサユニット１０には、人物９０の動きに関わるデータが蓄積される。運動後、解析ユニット２０は、センサユニット１０からデータを読み出し、人物９０の運動を解析する。解析ユニット２０は、解析結果に基づいて、そのディスプレイ２４に、姿勢をアニメーション表示してもよいし、力積、速度等を表示してもよい。
センサユニット１０は、人物９０のどこにいくつ装着されてもよい。また、姿勢計測システム１は、人物の運動に関わらず、例えば各種ロボット、機械、自動車、航空機等の運動解析に用いられてもよい。
なお、ここでは、センサユニット１０は人物９０の運動に支障が出ない程度に小型のものであることを想定しているが、これに限らない。角速度センサ１１と、加速度センサ１２と、地磁気センサ１３とは、相対的な位置関係が維持されていれば、互いに離れて配置されていてもよい。
［システムの動作］
上述のとおり、姿勢計測システム１が使用される際、まずセンサユニット１０が計測対象に装着される。センサユニット１０は、計測対象の運動中に計測される角速度、加速度、地磁気方向をフラッシュメモリ１６に記録する。計測対象である運動の終了後、センサユニット１０と解析ユニット２０とは接続され、解析ユニット２０は、センサユニット１０のフラッシュメモリ１６に記録された運動に係るデータを読み出し、記録装置２３に保存する。その後、解析ユニット２０のプロセッサ２１は、記録装置２３に記録されたデータを用いて、計測期間中のセンサユニット１０の姿勢の変化を解析する。
上述のとおり、姿勢計測システム１が使用される際、まずセンサユニット１０が計測対象に装着される。センサユニット１０は、計測対象の運動中に計測される角速度、加速度、地磁気方向をフラッシュメモリ１６に記録する。計測対象である運動の終了後、センサユニット１０と解析ユニット２０とは接続され、解析ユニット２０は、センサユニット１０のフラッシュメモリ１６に記録された運動に係るデータを読み出し、記録装置２３に保存する。その後、解析ユニット２０のプロセッサ２１は、記録装置２３に記録されたデータを用いて、計測期間中のセンサユニット１０の姿勢の変化を解析する。
角速度センサ１１の出力の積算によって得られるセンサユニット１０の姿勢をセンサ姿勢と称し、センサ姿勢が示す重力方向の成分をセンサ重力方向と称し、センサ姿勢が示す地磁気方向の成分をセンサ地磁気方向と称することにする。また、加速度センサ１２の出力に基づいて得られる重力方向を参照重力方向と称し、地磁気センサ１３の出力に基づいて得られる地磁気方向を参照地磁気方向と称し、参照重力方向及び参照地磁気方向を合わせて参照姿勢と称することにする。参照重力方向の算出には、加速度センサ１２の出力の所定期間の時間平均が用いられることが好ましい。また、参照地磁気方向の算出には、地磁気センサ１３の出力の所定期間の時間平均が用いられることが好ましい。
本実施形態で取り扱われる方向等について、図３を参照して説明する。図３に示すように、センサ重力方向と参照重力方向とには、角度θ１に示すような重力方向誤差が生じることがある。同様に、センサ地磁気方向と参照地磁気方向とには、角度θ２に示すような地磁気方向誤差が生じることがある。この重力方向誤差θ１及び地磁気方向誤差θ２を含む参照角度差がなくなるように、角速度センサ１１の出力値が角速度補正値によって補正される。
本実施形態では、解析ユニット２０は、角速度センサ１１で取得された角速度履歴値と、加速度センサ１２で取得された参照重力方向履歴値と、地磁気センサ１３で取得された参照地磁気方向履歴値とを取得する。その後に、解析ユニット２０のプロセッサ２１で行われる本実施形態に係る姿勢算出処理について、図４Ａ及び図４Ｂに示すフローチャートを参照して説明する。
ステップＳ１０１において、プロセッサ２１は、運動前のセンサユニット１０が静止していた状態における加速度センサ１２及び地磁気センサ１３の出力値を用いて、センサ姿勢の重力方向及び地磁気方向を初期化する。すなわち、プロセッサ２１は、加速度センサ１２の出力の時間平均に基づいて、重力方向を決定する。プロセッサ２１は、この重力方向をセンサ重力方向の初期値とする。同様に、プロセッサ２１は、地磁気センサ１３の出力の時間平均に基づいて、地磁気方向を決定する。プロセッサ２１は、この地磁気方向をセンサ地磁気方向の初期値とする。
ステップＳ１０２において、プロセッサ２１は、角速度補正値を初期化する。角速度補正値の初期値は、例えばｘ軸、ｙ軸、ｚ軸のそれぞれについて０とする。さらに、１つの角速度補正値を求めるために用いられる期間である評価区間の開始時を初期化する。評価区間の開始時をセンサユニット１０で記録された全データの開始時（ｔ＝０）に設定する。以降、各々の評価区間の開始時を時刻ｉと表記することにする。また、時刻を順に進めながら解析を行っていくが、現在解析を行っている時刻を時刻Ｎと表記することにする。
ステップＳ１０３において、プロセッサ２１は、誤差算出処理を行う。誤差算出処理では、後述するように、時刻ｉから経過時間に沿って順次にセンサ姿勢及びその誤差を算出していく。ここで算出される誤差は、角速度補正値を求めるための評価区間を決定するために用いられる。誤差算出処理について、図５に示すフローチャートを参照して説明する。
ステップＳ２０１において、プロセッサ２１は、角速度センサ１１の出力に角速度補正値を適用して、補正後の角速度を積分することにより、センサ姿勢を取得する。すなわち、前回のセンサ姿勢を、補正した角速度センサ１１の出力に基づいて得られる回転角度だけ回転させて現在のセンサ姿勢を算出する。
各時刻のセンサユニット１０の姿勢をＰi、各時刻の回転変化をＲiとしたとき、両者の関係は、
Ｐ1 = Ｒ0・Ｐ0
Ｐ2 = Ｒ1・Ｐ1 = Ｒ1・Ｒ0・Ｐ0
Ｐ3 = Ｒ2・Ｐ2 = Ｒ2・Ｒ1・Ｒ0・Ｐ0
で表される。すなわち、
Ｐi = Ｒi-1・Ｐi-1
で表される。ここで、初期姿勢Ｐ0は既知である。Ｐi、Ｒiは３×３の回転行列でもよいし、大きさ１のクォータニオンでもよい。
Ｐ1 = Ｒ0・Ｐ0
Ｐ2 = Ｒ1・Ｐ1 = Ｒ1・Ｒ0・Ｐ0
Ｐ3 = Ｒ2・Ｐ2 = Ｒ2・Ｒ1・Ｒ0・Ｐ0
で表される。すなわち、
Ｐi = Ｒi-1・Ｐi-1
で表される。ここで、初期姿勢Ｐ0は既知である。Ｐi、Ｒiは３×３の回転行列でもよいし、大きさ１のクォータニオンでもよい。
誤差算出処理で算出されるセンサ姿勢をセンサ積算値と称し、その履歴をセンサ積算履歴値と称することにする。
ステップＳ２０２において、プロセッサ２１は、センサ積算値及び参照姿勢に基づいて、参照角度差を取得する。すなわち、センサ積算値の重力方向（センサ重力方向）と加速度センサ１２の出力に基づいて取得された重力方向（参照重力方向）とを比較して、重力方向の角度差（重力方向誤差）を算出する。同様に、センサ積算値の地磁気方向（センサ地磁気方向）と地磁気センサ１３の出力に基づいて取得された地磁気方向（参照地磁気方向）とを比較して、地磁気方向の角度差（地磁気方向誤差）を算出する。これらの重力方向の角度差と地磁気方向の角度差とを参照角度差とする。
すなわち、世界座標系の重力方向が
世界座標系の地磁気方向が
（簡単のため、地磁気の伏角を無視している）だとすると、Ｐiのときのセンサから見た重力方向（センサ重力方向）Ｇi pは、
となり、地磁気方向（センサ地磁気方向）Ｍi pは、
となる。
このとき、加速度センサ１２から得られた重力方向（参照重力方向）をＧi sとし、地磁気センサ１３から得られた地磁気方向（参照地磁気方向）をＭi sとすると、重力方向の角度差（参照角度差）Ｄi Gは、
となり、地磁気方向の角度差（参照角度差）Ｄi Mは、
となる。
ステップＳ２０３において、プロセッサ２１は、評価区間の開始時である時刻ｉから現在解析対象としている時刻Ｎまでの誤差平均を算出する。ここで、誤差平均は、時刻ｉから時刻Ｎまでの参照角度差の時間平均である。一般に、誤差平均は時間経過と共に大きくなる傾向がある。
以上で誤差算出処理を終了し、誤差平均が戻り値としてメイン処理に返される。
図４Ａに戻って説明を続ける。誤差算出処理の後、処理はステップＳ１０４に進む。ステップＳ１０４において、プロセッサ２１は、解析対象としている全期間、すなわち、センサユニット１０でデータが取得された全期間について誤差平均の算出がおこなわれたか否かを判定する。全期間について誤差平均の算出がおこなわれていないとき、処理はステップＳ１０５に進む。
ステップＳ１０５において、プロセッサ２１は、誤差平均は所定の第１の閾値ＴＨ１より大きいか否かを判定する。ここで、第１の閾値ＴＨ１は、例えば角速度センサ１１の特性などを参考にして決定する。誤差平均が第１の閾値ＴＨ１より大きくないとき、処理はステップＳ１０６に進む。
ステップＳ１０６において、プロセッサ２１は、解析対象の時刻ｔを進める。その後、処理はステップＳ１０３に戻る。すなわち、角速度センサ１１の出力に基づいてセンサ積算履歴値を取得して誤差平均の履歴値を算出することを、全期間について解析が終わるか、誤差平均が第１の閾値ＴＨ１より大きくなるまで繰り返す。
ステップＳ１０５において、誤差平均が第１の閾値ＴＨ１より大きいと判定されたとき、処理はステップＳ１０７に進む。このときまでの時刻に対する誤差平均の変化の一例を図６Ａに示す。図６Ａにおいて、横軸は時刻を示し、縦軸は誤差平均を示す。時刻ｔ０を評価区間の開始時刻ｉとする。誤差平均が第１の閾値ＴＨ１より大きいと判定された時刻を時刻ｔ１とする。すなわち、時刻ｔ１は、処理がステップＳ１０６に進むときの現在解析対象としている時刻Ｎである。図６Ａに示すように、時刻ｔ０（ｉ）から誤差平均は、概ね時間経過と共に大きくなり、現在解析対象としている時刻ｔ１（Ｎ）において、第１の閾値ＴＨ１を超えている。時刻ｔ０から時刻ｔ１までの時間は、これに限らないが、例えば１０秒程度となるように各パラメータは設定される。
ステップＳ１０７において、プロセッサ２１は、評価区間を設定する。ここで設定される評価区間は、評価区間開始時の時刻ｉから、誤差平均が第１の閾値ＴＨ１より大きいと判定された時刻Ｎまでとする。
ステップＳ１０８において、プロセッサ２１は、角速度補正値探索処理を行う。角速度補正値探索処理について、図７のフローチャート及び図８を参照して説明する。
ステップＳ３０１において、プロセッサ２１は、角速度センサ１１のｘ，ｙ，ｚ軸について、角速度補正値を算出していない軸のうち、誤差が最大となっている軸を選択する。
ステップＳ３０２において、プロセッサ２１は、ステップＳ３０１で選択された軸について、角速度補正値ごとに、その補正値を評価区間全体に適用した場合に得られる参照角度差の二乗和を算出する。すなわち、プロセッサ２１は、現在設定されている角速度補正値の近傍に角速度補正値の仮値を仮補正値として設定する。図８は、横軸に角速度補正値を示し、縦軸に各々の角速度補正値が適用された場合に評価区間全体で算出される参照角度差の二乗和を示す。例えば図８の例では、白抜きの丸印で示した５点の角速度補正値が仮補正値とされており、その各々について算出される仮参照角度差としての参照角度差の二乗和が示されている。
ステップＳ３０３において、プロセッサ２１は、ステップＳ３０２の算出結果について、２次曲線近似を行い、参照角度差の二乗和が最小となる最適値を算出する。すなわち、図８の例では、黒丸で示した角速度補正値が最適値であると算出される。
ステップＳ３０４において、プロセッサ２１は、上述のようにして算出される角速度補正値が、角速度センサ１１のｘ，ｙ，ｚ軸の全てについて算出されたか否かを判定する。全ての軸について算出されていないとき、処理はステップＳ３０１に戻り、次に誤差が大きい軸を選択し、当該軸について同様に角速度補正値が算出される。この際、既に算出した軸についての角速度補正値を適用して演算が行われる。ステップＳ３０４において、全ての軸について角速度補正値が算出されたと判断されたとき、角速度補正値探索処理は終了し、算出されたｘ，ｙ，ｚ軸のそれぞれの角速度補正値が戻り値としてメイン処理に返される。
このような方法で角速度補正値を算出することで高次元の方程式を解く必要なく、容易に最適値を算出することができる。すなわち、角速度補正値は時刻ｉから時刻Ｎまでの各回転に適用されるため、数式で表すと（Ｎ−ｉ）次方程式となり解析的に最適値を算出することは困難である。このため、前回の角速度補正値の近傍値を用いて参照角度差を再計算し、回帰推定を行う本手法は、演算量の削減に貢献する。
また、誤差が大きい軸から順に角速度補正値を決定することで、高精度に角速度補正値を決定することができる。なお、角速度補正値探索処理において、角速度補正値の算出は、誤差最大の軸から行われずに、例えば常にｘ軸、ｙ軸、ｚ軸の順に行われてもよい。
図４Ｂに戻って説明を続ける。ステップＳ１０８の角速度補正値探索処理の後、処理はステップＳ１０９に進む。ステップＳ１０９において、プロセッサ２１は、誤差平均に係る値をクリアし、解析対象とする時刻ｔを評価区間の開始の時刻ｉに再設定する。
ステップＳ１１０において、プロセッサ２１は、姿勢・誤差算出処理を実行する。すなわち、評価区間の最初から、ステップＳ１０８の角速度補正値探索処理で決定された角速度補正値を角速度センサ１１の出力に適用し、それを用いてセンサ姿勢を算出する。また、このときの誤差平均を改めて求める。
姿勢・誤差算出処理について、図９に示すフローチャートを参照して説明する。ステップＳ４０１において、プロセッサ２１は、角速度センサ１１の出力にステップＳ１０８で算出された角速度補正値を適用し、補正後の角速度を積算することにより、センサ姿勢を取得する。後述するように、ここで算出されるセンサ姿勢が、最終的にセンサユニット１０の姿勢として確定される値となる。
ステップＳ４０２において、プロセッサ２１は、センサ姿勢及び参照姿勢に基づいて、参照角度差を取得する。参照角度差の算出は、図５を参照して説明したステップＳ２０２の処理と同様である。すなわち、センサ姿勢の重力方向（センサ重力方向）と加速度センサ１２の出力に基づいて取得された重力方向（参照重力方向）とを比較して、重力方向の角度差（重力方向誤差）を算出する。同様に、センサ姿勢の地磁気方向（センサ地磁気方向）と地磁気センサ１３の出力に基づいて取得された地磁気方向（参照地磁気方向）とを比較して、地磁気方向の角度差（地磁気方向誤差）を算出する。
ステップＳ４０３において、プロセッサ２１は、評価区間の開始時である時刻ｉから現在解析対象としている時刻Ｎまでの誤差平均を算出する。誤差平均の算出は、図５を参照して説明したステップＳ２０３の処理と同様である。すなわち、誤差平均は、時刻ｉから時刻Ｎまでの参照角度差の時間平均である。
以上で姿勢・誤差算出処理を終了し、センサ姿勢と誤差平均とが戻り値としてメイン処理に返される。
図４Ｂに戻って説明を続ける。姿勢・誤差算出処理の後、ステップＳ１１１において、プロセッサ２１は、データが存在する全期間についてセンサ姿勢が決定されたか否かを判定する。全期間についてセンサ姿勢が決定されていないとき、処理はステップＳ１１２に進む。
ステップＳ１１２において、プロセッサ２１は、誤差平均が所定の第２の閾値ＴＨ２より大きいか否かを判定する。ここで、第２の閾値ＴＨ２は、第１の閾値ＴＨ１よりも小さい値であり、一般に誤差平均が第２の閾値ＴＨ２より大きくなるのは、誤差平均が第１の閾値ＴＨ１より大きくなるよりも早い。誤差平均が第２の閾値ＴＨ２よりも大きくないとき、処理はステップＳ１１３に進む。ステップＳ１１３において、プロセッサ２１は、解析対象の時刻ｔを進める。その後、処理はステップＳ１１０に戻る。すなわち、角速度センサ１１の出力に基づいてセンサ姿勢を取得すること及び平均誤差を算出することが、全期間について解析が終わるか、誤差平均が第２の閾値ＴＨ２より大きくなるまで繰り返される。
ステップＳ１１２において、誤差平均が第２の閾値ＴＨ２よりも大きいと判定されたとき、処理はステップＳ１１４に進む。
このときまでの時刻に対する誤差平均の変化の一例を図６Ｂに示す。図６Ｂにおいて、横軸は時刻を示し、縦軸は誤差平均を示す。実線はこのステップＳ１１０の姿勢・誤差算出処理で算出された誤差平均の変化を示し、破線は、図６Ａに示したステップＳ１０３の誤差算出処理で算出された誤差平均を示す。このように、ステップＳ１０８の角速度補正値探索処理で決定された角速度補正値が適用されているので、実線で示した誤差平均は、破線で示した誤差平均よりも小さい。しかしながら、誤差平均の値は、時刻の経過と共に徐々に大きくなる。時刻ｔ２（Ｎ）において、誤差平均は第２の閾値ＴＨ２を超えている。
図４Ｂに戻って説明を続ける。ステップＳ１１４において、プロセッサ２１は、時刻ｉ（ｔ０）から時刻Ｎ（ｔ２）までのセンサ姿勢の確定値をステップＳ１１０で算出されたセンサ姿勢とする。すなわち、上述した一連の処理によって、例えば時刻ｔ０から時刻ｔ２までのセンサユニット１０の姿勢が、角速度補正値で補正された角速度センサ１１の出力値の積算によって決定される。ここで、第２の閾値ＴＨ２が第１の閾値ＴＨ１よりも小さく設定されていることは、精度が高いセンサ姿勢の決定に貢献する。
ステップＳ１１５において、プロセッサ２１は、続いて行われる積算によるセンサ姿勢の算出の初期値となるセンサ姿勢を、重力センサの出力及び地磁気センサ１３の出力に基づいて改めて決定する。また、プロセッサ２１は、誤差平均に係る値をクリアし、解析対象とする評価区間の開始の時刻ｉを誤差平均が第２の閾値ＴＨ２を超えた時刻Ｎ（ｔ２）に設定する。その後、処理はステップＳ１０３に戻り、上述の処理が繰り返される。
図６Ｂの状態に続いてステップＳ１０３乃至ステップＳ１０６の処理が繰り返され、ステップＳ１０５で誤差平均が第１の閾値ＴＨ１よりも大きいと判定されたときの、時刻に対する誤差平均の値を図６Ｃに示す。図６Ｃにおいて、実線がこのときに得られた時刻と誤差平均との関係を示す。一点鎖線は、図６Ａに示した前回のステップＳ１０３乃至ステップＳ１０５の処理に係る時刻と誤差平均との関係を示し、破線は、図６Ｂに示した前回のステップＳ１１０乃至ステップＳ１１３の処理に係る時刻と誤差平均との関係を示す。時刻ｔ２である評価区間の開始時刻ｉにおいて加速度センサ１２の出力及び地磁気センサ１３の出力に基づいてセンサ姿勢が初期化されているので、実線で示すように、評価区間の開始時刻ｉ（ｔ２）の時点での誤差平均は０である。図６Ａに示した場合と同様に、評価区間の開始時刻ｉ（ｔ２）より後においては、誤差平均は時間経過と共に徐々に大きくなる。ただし、時刻ｔ０から時刻ｔ１までの評価区間に基づいて算出された角速度補正値が適用されているので、一般に実線で示したこのときの誤差平均は、一点鎖線で示した前回の誤差平均よりも大きくなるのが遅い。このため、誤差平均が第１の閾値ＴＨ１よりも大きくなる時刻を時刻ｔ３とすると、時刻ｔ２から時刻ｔ３までの時間は、時刻ｔ０から時刻ｔ１までの時間よりも長くなる。以下、同様の処理が順に繰り返されることで、誤差平均が第２の閾値ＴＨ２よりも大きくならないように、区間ごとに順にセンサ姿勢が決定されていく。
以上の処理を言い換えると次のようになる。センサユニット１０で記録された角速度値が存在する期間を第１の期間とする。時刻ｔ０を第１の時点としたときに、センサ積算履歴値は、第１の時点から一定の角速度補正値で補正した角速度値を順次に積算することで算出される。参照角度差の第１の時点からの累積値の時間平均である誤差平均が所定の第１の閾値ＴＨ１よりも大きくなる時刻ｔ１を第２の時点とする。このとき、第１の時点から第２の時点までの評価区間を第２の期間とすると、第２の期間で参照角度差が最小となる角速度補正値が算出される。第１の時点である時刻ｔ０から第３の時点としての時刻ｔ２までの期間を第３の期間とすると、算出された角速度補正値は、第３の期間の角速度値に適用されてセンサ姿勢が算出される。
ステップＳ１０３乃至ステップＳ１０６の繰り返し処理において、ステップＳ１０４で全期間についてのセンサ姿勢の算出が終了したと判定されたとき、処理はステップＳ１０７に進む。すなわち、この場合、評価区間は時刻ｉから最後のデータが存在する時刻Ｎまでに設定され、角速度補正値が決定され、この角速度補正値を用いて時刻ｉ以降について順にセンサ姿勢が決定される。
ステップＳ１１１の判定において、全期間について姿勢が決定されたと判定されたとき、決定されたセンサ姿勢は確定値とされ、姿勢算出処理は終了する。
以上のように、プロセッサ２１は、角速度値取得部、参照姿勢取得部、積算履歴値算出部、方向算出部、角度差算出部、補正値算出部、及び姿勢算出部としての機能を有する。ここで、角速度値取得部は、センサユニット１０で取得された角速度履歴値を取得する。角速度履歴値が取得される期間を第１の期間と称することにする。参照姿勢取得部は、センサユニット１０から上述の第１の期間に対応する加速度センサ１２及び地磁気センサ１３の検出値を取得し、これらの値に基づいて、第１の期間に対応する参照重力方向履歴値及び参照地磁気方向履歴値を取得する。積算履歴値算出部は、上述の角速度値取得部で取得した角速度値を順次に積算することでセンサ積算履歴値を算出する。方向算出部は、積算履歴値算出部で算出したセンサ積算履歴値に基づいて、センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値とセンサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値とを算出する。角度差算出部は、方向算出部で算出されたセンサ重力方向履歴値と参照重力方向履歴値とを比較し、センサ重力方向と参照重力方向との差である重力方向誤差を算出する。また角度差算出部は、センサ地磁気方向履歴値と参照地磁気方向履歴値とを比較し、センサ地磁気方向と参照地磁気方向との差である地磁気方向誤差を算出する。角度差算出部は、重力方向誤差及び地磁気方向誤差を含む参照角度差の履歴を算出する。補正値算出部は、角速度補正値を算出する。例えば、補正値算出部は、参照角度差が所定の期間で最小となるように角速度補正値を算出する。この角速度補正値は、角速度値を補正する値である。姿勢算出部は、角速度補正値で補正した角速度値を順次に積算することで、センサユニット１０のセンサ姿勢を算出する。特に上述の実施形態では、補正値算出部は、上述の第１の期間の一部期間である複数の第２の期間ごとに、すなわち評価区間ごとに、複数の角速度補正値を算出する。姿勢算出部は、算出された角速度補正値で補正した角速度値に基づいて、センサ姿勢を算出し、決定する。特に上述の実施形態では、姿勢算出部は、上述の第１の期間の一部期間である複数の第３の期間ごとに、複数の補正値の何れかを用いて角速度値を補正して、センサ姿勢を算出する。
［姿勢計測システムの特長］
本実施形態に係る姿勢計測システム１の解析ユニット２０は、角速度補正値を順次に算出して更新しながら、角速度センサ１１の出力を用いてセンサ姿勢を算出する。このため、本実施形態に係る姿勢算出方法を用いる解析ユニット２０は、角速度補正値を変更しなければならないような各種外乱に対してロバストに対応できる。例えば、角速度センサの出力に含まれるノイズは、直流から高周波領域まで広域にわたるため、固定のフィルタによるカットや固定値を用いたオフセットでは除去しきれず、角速度センサの出力を積算することにより得られる角度値は、穏やかに振動する傾向がある。また、角速度センサの出力特性は、温度変化などによっても変化する。本実施形態に係る解析ユニット２０によれば、このような角速度センサの特性に対してもロバストである。
本実施形態に係る姿勢計測システム１の解析ユニット２０は、角速度補正値を順次に算出して更新しながら、角速度センサ１１の出力を用いてセンサ姿勢を算出する。このため、本実施形態に係る姿勢算出方法を用いる解析ユニット２０は、角速度補正値を変更しなければならないような各種外乱に対してロバストに対応できる。例えば、角速度センサの出力に含まれるノイズは、直流から高周波領域まで広域にわたるため、固定のフィルタによるカットや固定値を用いたオフセットでは除去しきれず、角速度センサの出力を積算することにより得られる角度値は、穏やかに振動する傾向がある。また、角速度センサの出力特性は、温度変化などによっても変化する。本実施形態に係る解析ユニット２０によれば、このような角速度センサの特性に対してもロバストである。
また、解析ユニット２０は、瞬間値ではなく比較的長時間の評価区間における参照角度差を用いて角速度補正値を算出する。このため、得られるセンサ姿勢は、加速度センサ１２及び地磁気センサ１３に対する外乱に対してロバストである。また、一般的に微小である角速度の誤差を十分に長い時間蓄積させているため、解析ユニット２０は、精度よく角速度補正値を算出でき、正確なセンサ姿勢を得ることができる。
また、上述の評価区間は適応的に決定され、また、その評価区間によって得られた角速度補正値を角速度センサ１１の出力に適用する区間も適応的に決定される。このため、解析ユニット２０は、センサユニット１０の解析対象とする運動に応じた適切なセンサ姿勢の算出が行える。
［変形例］
以下、上述の姿勢計測システム１のいくつかの変形例について説明する。
以下、上述の姿勢計測システム１のいくつかの変形例について説明する。
上述の実施形態では、角速度補正値は、重力方向と地磁気方向とに基づいて決定される例を示したが、例えば、重力方向又は地磁気方向の回転方向に制限が設けられている場合、重力方向と地磁気方向とのうち一方のみに基づいて角速度補正値は決定され得る。例えば、重力方向に維持される回転軸周りにのみ回転する系においては、地磁気方向についてのみ各種データを取得し、各種演算が行われればよい。必要な軸についてのみ演算が行われることで、演算量が削減され得る。なお、上述の実施形態のように３軸について演算が行われれば、どのような姿勢であっても算出され得る。
また、上述の実施形態は、センサユニット１０と別体として設けられている解析ユニット２０がセンサ姿勢の算出を行う場合である。しかしながらこれに限らない。センサ姿勢の算出は、計測対象に取り付けられるセンサユニット１０のプロセッサ１４で行われてもよい。すなわち、検出データに基づいてセンサ姿勢を算出する姿勢算出装置は、センサユニット１０に組み込まれていてもよい。その場合、センサユニット１０で算出されたセンサ姿勢が、センサユニット１０の外部のＰＣ等に出力されてもよい。
また、上述の実施形態では、センサユニット１０を用いて、角速度、加速度、地磁気方向が検出される全期間内に複数の評価区間が設定されている。この評価区間の数は、いくつであってもよい。データが取得される全期間が１つの評価区間として取り扱われてもよい。
また、上述の実施形態では、時間経過に沿って、前の時点（過去）から後の時点（未来）へ向けて順次にセンサ姿勢が算出される場合を例に挙げた。センサ姿勢の算出は、これに限らず、例えば後の時点（未来）から前の時点（過去）へ向けて順次にセンサ姿勢が算出されてもよい。すなわち、例えば運動終了後の静止しているときを基準として、時間をさかのぼって角速度センサの出力を積算していくことで、各時点のセンサ姿勢が算出されてもよい。この場合も、上述の実施形態と同様にある程度の期間を有する評価区間のデータに基づいて角速度補正値が算出され、算出された角速度補正値がある区間について適用されることになる。
このように、解析対象とする運動が終了した後に、センサ姿勢が算出される場合、前の時点から後の時点に向けて順にセンサ姿勢を決定しても、後の時点から前の時点に向けて順にセンサ姿勢を決定してもよい。したがって、解析対象について運動前に安定した静止状態があるか、運動後に安定した静止状態があるか等に応じて、解析の手順は適宜に選択され得る。
また、センサ姿勢は前の時点（過去）から後の時点（未来）へ向けて順に算出され、評価区間を決定するためのセンサ積算履歴値は後の時点（未来）から前の時点（過去）にさかのぼって算出されることも可能である。この場合の例を図１０に示す。図１０において、横軸は時刻を示し、縦軸は誤差平均を示す。点線は、区間ごとに順にセンサ姿勢が算出されるときの誤差平均の値の変化を示す。
実線で示した時刻ｔ２からのセンサ姿勢の算出に注目して説明する。上述の実施形態と同様に、時刻ｔ２の姿勢を基準として、時刻ｔ２以前に決定された角速度補正値で補正した角速度センサ１１の出力を積算することで、順にセンサ姿勢を算出していく。このときの誤差平均を順に求めていった結果が実線で示されている。誤差平均は、時間経過と共に徐々に大きくなり、時刻ｔ３において、第２の閾値ＴＨ２より大きくなったとする。このとき、角速度補正値の再設定が行われる。
新たな角速度補正値の算出は、次のように行われる。すなわち、時刻ｔ３におけるセンサ姿勢を基準として、時刻ｔ２から時刻ｔ３までの間のセンサ姿勢の算出に用いられた角速度補正値を用いて各時刻に得られた角速度センサ１１の出力を補正し、それらを時間をさかのぼりながら積算して各時点のセンサ積算値を算出する。このようにして得られたセンサ積算値について、各々の時点での誤差平均を算出すると、図１０の破線のようになる。すなわち、ｔ３から時間をさかのぼっていくと、誤差平均は徐々に大きくなる。そして、時刻ｔ１において第１の閾値ＴＨ１よりも大きくなったとする。このとき、時刻ｔ３から時刻ｔ１までの区間が評価区間に設定される。この評価区間における参照角度差の二乗和が最小となるような角速度補正値が算出される。
このようにして求まった角速度補正値が、時刻ｔ３以降のセンサ姿勢の算出に用いられる。時刻ｔ３以降の誤差平均は、図１０の一点鎖線で示すように、時間経過と共に徐々に増加する。時刻ｔ４において誤差平均がＴＨ２よりも大きくなったとすると、上述と同様に時刻ｔ４から遡って評価区間が設定され、新たな角速度補正値が算出される。
図１０を参照して説明したようにセンサ姿勢及び角速度補正値が算出される場合、常にセンサ姿勢を算出している時点よりも過去の角速度センサ１１の出力値を用いており、未来の角速度センサ１１の出力値を用いていない。このため、本手法によれば、センサユニット１０による角速度、加速度及び地磁気の検出と共に、センサ姿勢の算出が行われ得る。すなわち、演算能力が十分にあれば、リアルタイムにセンサ姿勢が算出され得る。
同様に、センサ姿勢は後の時点から前の時点にさかのぼって算出され、角速度補正値は前の時点から後の時点へ向けて順に算出されてもよい。
また、上述の実施形態では、角速度補正値の算出において、角速度補正値を変更しながら評価区間についての複数の参照角度差の二乗和を算出し、この値に二次関数をフィッティングして角速度補正値の最適値を算出する例を示した。しかしながら、これに限らない。勾配法等を含む各種解析方法が用いられ得る。角速度補正値は、３軸の補正値の各々が順に算出されるに限らず、３軸の補正値の各々が一度に算出されてもよい。演算能力、要求される算出速度等に応じて適切な手法が選択され得る。
なお、本発明は、上記実施形態に限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で種々に変形することが可能である。また、各実施形態は適宜組み合わせて実施してもよく、その場合組み合わせた効果が得られる。更に、上記実施形態には種々の発明が含まれており、開示される複数の構成要件から選択された組み合わせにより種々の発明が抽出され得る。例えば、実施形態に示される全構成要件からいくつかの構成要件が削除されても、課題が解決でき、効果が得られる場合には、この構成要件が削除された構成が発明として抽出され得る。
以下に、本願出願の当初の特許請求の範囲に記載された発明を付記する。
［１］
角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、
前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得する参照姿勢取得部と、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出する方向算出部と、
前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出する角度差算出部と、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部と
を備える姿勢算出装置。
［２］
角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、
前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得する参照姿勢取得部と、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出する方向算出部と、
前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出する角度差算出部と、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部と
を備える姿勢算出装置。
［３］
前記センサユニットは、参照重力方向を検出する加速度センサをさらに有し、
前記参照姿勢取得部は、前記センサユニットで取得された前記第１の期間の参照重力方向履歴値をさらに取得し、
前記方向算出部は、前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値をさらに算出し、
前記角度差算出部は、前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差をさらに含む前記参照角度差を算出する、
［１］に記載の姿勢算出装置。
［４］
前記補正値算出部は、前記第１の期間の一部期間である複数の第２の期間を設定し、前記第２の期間ごとに複数の前記補正値を算出し、
前記姿勢算出部は、前記第１の期間の一部期間である複数の第３の期間を設定し、前記第３の期間ごとに前記複数の補正値の何れかで前記角速度値を補正して前記センサ姿勢を算出し、
前記第３の期間の長さは、前記第２の期間の長さ以下である、
［１］乃至［３］のうち何れか一に記載の姿勢算出装置。
［５］
前記補正値算出部は、第１の時点から第２の時点までを前記第２の期間に設定して前記補正値を算出し、
前記姿勢算出部は、前記第１の時点から前記第２の時点までを前記第２の期間として算出した前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出する、
［４］に記載の姿勢算出装置。
［６］
前記積算履歴値算出部は、第１の時点から一定の前記補正値で補正した前記角速度値を順次に積算することで前記センサ積算履歴値を算出し、
前記角度差算出部は、前記参照角度差の前記第１の時点からの累積値の時間平均が所定の閾値よりも大きくなる第２の時点を決定し、
前記補正値算出部は、前記第１の時点から前記第２の時点までを前記第２の期間に設定して前記補正値を算出し、
前記姿勢算出部は、前記第１の時点から前記第２の期間までを前記第２の期間として算出した前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出する、
［４］に記載の姿勢算出装置。
［７］
前記補正値算出部が前記第１の時点から前記第２の時点までを前記第２の期間に設定して前記補正値を算出することと、
前記姿勢算出部が前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出することと、
前記補正値算出部が前記第３の時点を前記第１の時点として前記第２の期間を再設定することと、
前記姿勢算出部が前記第３の時点を前記第１の時点として前記第３の期間を再設定することと
が繰り返されて、前記姿勢算出部は、前記第１の期間の前記センサ姿勢を順次算出する、
［５］又は［６］に記載の姿勢算出装置。
［８］
前記補正値算出部は、前記補正値として、前記参照角度差が所定の期間で最小となる前記補正値を算出する、［１］乃至［７］のうち何れか一に記載の姿勢算出装置。
［９］
前記補正値算出部は、前記補正値の仮値を仮補正値としたときに、前記仮補正値で前記角速度値を補正した場合の前記参照角度差である仮参照角度差を、複数の前記仮補正値について求め、前記仮補正値と前記仮参照角度差との関係に基づいて、前記参照角度差が最小となる前記補正値を決定する、［８］に記載の姿勢算出装置。
［１０］
［１］乃至［９］のうち何れか一に記載の姿勢算出装置と、
前記センサユニットと
を備える姿勢計測システム。
［１１］
角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、
前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得することと、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出することと、
前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出することと、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することと
を含む姿勢算出方法。
［１２］
角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、
前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得することと、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出することと、
前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出することと、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出することと、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することと
を含む姿勢算出方法。
［１］
角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、
前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得する参照姿勢取得部と、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出する方向算出部と、
前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出する角度差算出部と、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部と
を備える姿勢算出装置。
［２］
角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、
前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得する参照姿勢取得部と、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出する方向算出部と、
前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出する角度差算出部と、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部と
を備える姿勢算出装置。
［３］
前記センサユニットは、参照重力方向を検出する加速度センサをさらに有し、
前記参照姿勢取得部は、前記センサユニットで取得された前記第１の期間の参照重力方向履歴値をさらに取得し、
前記方向算出部は、前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値をさらに算出し、
前記角度差算出部は、前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差をさらに含む前記参照角度差を算出する、
［１］に記載の姿勢算出装置。
［４］
前記補正値算出部は、前記第１の期間の一部期間である複数の第２の期間を設定し、前記第２の期間ごとに複数の前記補正値を算出し、
前記姿勢算出部は、前記第１の期間の一部期間である複数の第３の期間を設定し、前記第３の期間ごとに前記複数の補正値の何れかで前記角速度値を補正して前記センサ姿勢を算出し、
前記第３の期間の長さは、前記第２の期間の長さ以下である、
［１］乃至［３］のうち何れか一に記載の姿勢算出装置。
［５］
前記補正値算出部は、第１の時点から第２の時点までを前記第２の期間に設定して前記補正値を算出し、
前記姿勢算出部は、前記第１の時点から前記第２の時点までを前記第２の期間として算出した前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出する、
［４］に記載の姿勢算出装置。
［６］
前記積算履歴値算出部は、第１の時点から一定の前記補正値で補正した前記角速度値を順次に積算することで前記センサ積算履歴値を算出し、
前記角度差算出部は、前記参照角度差の前記第１の時点からの累積値の時間平均が所定の閾値よりも大きくなる第２の時点を決定し、
前記補正値算出部は、前記第１の時点から前記第２の時点までを前記第２の期間に設定して前記補正値を算出し、
前記姿勢算出部は、前記第１の時点から前記第２の期間までを前記第２の期間として算出した前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出する、
［４］に記載の姿勢算出装置。
［７］
前記補正値算出部が前記第１の時点から前記第２の時点までを前記第２の期間に設定して前記補正値を算出することと、
前記姿勢算出部が前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出することと、
前記補正値算出部が前記第３の時点を前記第１の時点として前記第２の期間を再設定することと、
前記姿勢算出部が前記第３の時点を前記第１の時点として前記第３の期間を再設定することと
が繰り返されて、前記姿勢算出部は、前記第１の期間の前記センサ姿勢を順次算出する、
［５］又は［６］に記載の姿勢算出装置。
［８］
前記補正値算出部は、前記補正値として、前記参照角度差が所定の期間で最小となる前記補正値を算出する、［１］乃至［７］のうち何れか一に記載の姿勢算出装置。
［９］
前記補正値算出部は、前記補正値の仮値を仮補正値としたときに、前記仮補正値で前記角速度値を補正した場合の前記参照角度差である仮参照角度差を、複数の前記仮補正値について求め、前記仮補正値と前記仮参照角度差との関係に基づいて、前記参照角度差が最小となる前記補正値を決定する、［８］に記載の姿勢算出装置。
［１０］
［１］乃至［９］のうち何れか一に記載の姿勢算出装置と、
前記センサユニットと
を備える姿勢計測システム。
［１１］
角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、
前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得することと、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出することと、
前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出することと、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することと
を含む姿勢算出方法。
［１２］
角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、
前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得することと、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出することと、
前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出することと、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出することと、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することと
を含む姿勢算出方法。
１…姿勢計測システム、１０…センサユニット、１１…角速度センサ、１２…加速度センサ、１３…地磁気センサ、１４…プロセッサ、１５…ｒａｎｄｏｍ ａｃｃｅｓｓ ｍｅｍｏｒｙ（ＲＡＭ）、１６…フラッシュメモリ、１７…入力装置、１８…インターフェイス（Ｉ／Ｆ）、１９…バスライン、２０…解析ユニット、２１…プロセッサ、２２…ＲＡＭ、２３…記録装置、２４…ディスプレイ、２５…入力装置、２６…Ｉ／Ｆ、２９…バスライン。
Claims (12)
- 角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、
前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得する参照姿勢取得部と、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出する方向算出部と、
前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出する角度差算出部と、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部と
を備える姿勢算出装置。 - 角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得する角速度値取得部と、
前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得する参照姿勢取得部と、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出する積算履歴値算出部と、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出する方向算出部と、
前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出する角度差算出部と、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出する姿勢算出部と
を備える姿勢算出装置。 - 前記センサユニットは、参照重力方向を検出する加速度センサをさらに有し、
前記参照姿勢取得部は、前記センサユニットで取得された前記第１の期間の参照重力方向履歴値をさらに取得し、
前記方向算出部は、前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値をさらに算出し、
前記角度差算出部は、前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差をさらに含む前記参照角度差を算出する、
請求項１に記載の姿勢算出装置。 - 前記補正値算出部は、前記第１の期間の一部期間である複数の第２の期間を設定し、前記第２の期間ごとに複数の前記補正値を算出し、
前記姿勢算出部は、前記第１の期間の一部期間である複数の第３の期間を設定し、前記第３の期間ごとに前記複数の補正値の何れかで前記角速度値を補正して前記センサ姿勢を算出し、
前記第３の期間の長さは、前記第２の期間の長さ以下である、
請求項１乃至３のうち何れか１項に記載の姿勢算出装置。 - 前記補正値算出部は、第１の時点から第２の時点までを前記第２の期間に設定して前記補正値を算出し、
前記姿勢算出部は、前記第１の時点から前記第２の時点までを前記第２の期間として算出した前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出する、
請求項４に記載の姿勢算出装置。 - 前記積算履歴値算出部は、第１の時点から一定の前記補正値で補正した前記角速度値を順次に積算することで前記センサ積算履歴値を算出し、
前記角度差算出部は、前記参照角度差の前記第１の時点からの累積値の時間平均が所定の閾値よりも大きくなる第２の時点を決定し、
前記補正値算出部は、前記第１の時点から前記第２の時点までを前記第２の期間に設定して前記補正値を算出し、
前記姿勢算出部は、前記第１の時点から前記第２の期間までを前記第２の期間として算出した前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出する、
請求項４に記載の姿勢算出装置。 - 前記補正値算出部が前記第１の時点から前記第２の時点までを前記第２の期間に設定して前記補正値を算出することと、
前記姿勢算出部が前記補正値を用いて、前記第１の時点から第３の時点までを前記第３の期間に設定して前記センサ姿勢を算出することと、
前記補正値算出部が前記第３の時点を前記第１の時点として前記第２の期間を再設定することと、
前記姿勢算出部が前記第３の時点を前記第１の時点として前記第３の期間を再設定することと
が繰り返されて、前記姿勢算出部は、前記第１の期間の前記センサ姿勢を順次算出する、
請求項５又は６に記載の姿勢算出装置。 - 前記補正値算出部は、前記補正値として、前記参照角度差が所定の期間で最小となる前記補正値を算出する、請求項１乃至７のうち何れか１項に記載の姿勢算出装置。
- 前記補正値算出部は、前記補正値の仮値を仮補正値としたときに、前記仮補正値で前記角速度値を補正した場合の前記参照角度差である仮参照角度差を、複数の前記仮補正値について求め、前記仮補正値と前記仮参照角度差との関係に基づいて、前記参照角度差が最小となる前記補正値を決定する、請求項８に記載の姿勢算出装置。
- 請求項１乃至９のうち何れか１項に記載の姿勢算出装置と、
前記センサユニットと
を備える姿勢計測システム。 - 角速度値を検出する角速度センサと参照地磁気方向を検出する地磁気センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、
前記センサユニットで取得された前記第１の期間の参照地磁気方向履歴値を取得することと、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の地磁気方向成分であるセンサ地磁気方向履歴値を算出することと、
前記センサ地磁気方向履歴値と前記参照地磁気方向履歴値とを比較することで得られる地磁気方向誤差を含む参照角度差を算出することと、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出する補正値算出部と、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することと
を含む姿勢算出方法。 - 角速度値を検出する角速度センサと参照重力方向を検出する加速度センサとを有するセンサユニット、で取得された第１の期間の角速度履歴値を取得することと、
前記センサユニットで取得された前記第１の期間の参照重力方向履歴値を取得することと、
前記センサユニットの角速度センサより取得された前記角速度値を順次に積算することでセンサ積算履歴値を算出することと、
前記センサ積算履歴値に基づいて、前記センサ積算履歴値の重力方向成分であるセンサ重力方向履歴値を算出することと、
前記センサ重力方向履歴値と前記参照重力方向履歴値とを比較することで得られる重力方向誤差を含む参照角度差を算出することと、
前記参照角度差に基づいて前記角速度値を補正する補正値を算出することと、
前記補正値で補正した前記角速度値を順次に積算することで前記センサユニットのセンサ姿勢を算出することと
を含む姿勢算出方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017055616A JP6848571B2 (ja) | 2017-03-22 | 2017-03-22 | 姿勢算出装置、姿勢計測システム、及び姿勢算出方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017055616A JP6848571B2 (ja) | 2017-03-22 | 2017-03-22 | 姿勢算出装置、姿勢計測システム、及び姿勢算出方法 |
Related Child Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2021033722A Division JP7120361B2 (ja) | 2021-03-03 | 2021-03-03 | 補正装置、姿勢算出装置、補正方法、及び、プログラム |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2018159569A JP2018159569A (ja) | 2018-10-11 |
| JP6848571B2 true JP6848571B2 (ja) | 2021-03-24 |
Family
ID=63796435
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2017055616A Active JP6848571B2 (ja) | 2017-03-22 | 2017-03-22 | 姿勢算出装置、姿勢計測システム、及び姿勢算出方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6848571B2 (ja) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR102360306B1 (ko) * | 2018-11-23 | 2022-02-10 | 주식회사 엔텍로직 | 짐벌카메라와 지향제어장치의 캘리브레이션 검사 방법 및 장치 |
| CN111625764B (zh) * | 2020-05-21 | 2023-05-12 | 北京嘀嘀无限科技发展有限公司 | 移动数据的校准方法、装置、电子设备和存储介质 |
| CN113137983B (zh) * | 2021-04-30 | 2023-08-22 | 深圳市恒星物联科技有限公司 | 一种自学习的井盖姿态监测方法及监测系统 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3581392B2 (ja) * | 1994-04-18 | 2004-10-27 | 株式会社ザナヴィ・インフォマティクス | 積分型センシング装置 |
| JPH11248456A (ja) * | 1998-02-27 | 1999-09-17 | Olympus Optical Co Ltd | ３軸姿勢検出装置 |
| JP2013217793A (ja) * | 2012-04-10 | 2013-10-24 | Sony Corp | オフセット算出装置、オフセット算出方法、プログラム、及び情報処理装置 |
| EP2998702A4 (en) * | 2013-05-15 | 2017-01-11 | Asahi Kasei Kabushiki Kaisha | Offset estimation device, method, and program |
| US10451438B2 (en) * | 2015-02-05 | 2019-10-22 | Alpinereplay, Inc. | Systems and methods for in-motion gyroscope calibration |
| JP2017049115A (ja) * | 2015-09-02 | 2017-03-09 | カシオ計算機株式会社 | 電子機器及び電子機器のセンサ較正方法、電子機器のセンサ較正プログラム |
-
2017
- 2017-03-22 JP JP2017055616A patent/JP6848571B2/ja active Active
Also Published As
| Publication number | Publication date |
|---|---|
| JP2018159569A (ja) | 2018-10-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8913134B2 (en) | Initializing an inertial sensor using soft constraints and penalty functions | |
| JP6702543B2 (ja) | 情報処理装置、方法およびプログラム | |
| US10309983B2 (en) | Systems and methods for motion detection | |
| JP6848571B2 (ja) | 姿勢算出装置、姿勢計測システム、及び姿勢算出方法 | |
| JPWO2021019602A1 (ja) | 構造物の変位を推定するためのシステム、方法、及びプログラム | |
| CN110319851B (zh) | 传感器的校正方法、装置、设备及存储介质 | |
| CN105841695B (zh) | 信息处理装置、信息处理方法以及记录介质 | |
| EP2910954B1 (en) | Method for counting steps and electronic apparatus using the same | |
| JP2014185996A (ja) | 計測装置 | |
| TW201833557A (zh) | 空間定向的決定 | |
| JP6074897B2 (ja) | 運動解析装置及び運動解析方法 | |
| CN107632966B (zh) | 运动轨迹确定方法及电子设备 | |
| JP7120361B2 (ja) | 補正装置、姿勢算出装置、補正方法、及び、プログラム | |
| CN108225368B (zh) | 计步装置和计步方法 | |
| KR101639351B1 (ko) | 웨어러블 입력 시스템 및 모션 인식 방법 | |
| KR101870542B1 (ko) | 모션 인식 방법 및 장치 | |
| JP2006038650A (ja) | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム | |
| JP6147446B1 (ja) | ソフト制約及びペナルティ機能を使用した慣性センサの初期化 | |
| CN108413970B (zh) | 一种定位方法、云系统、电子设备及计算机程序产品 | |
| JP6972675B2 (ja) | 姿勢評価装置、姿勢算出装置、姿勢計測システム、姿勢評価方法及びプログラム | |
| CN108519100A (zh) | 用于估算步长的方法、云系统、设备和计算机程序产品 | |
| JP5958920B2 (ja) | 傾斜角度推定システム | |
| JP5424226B2 (ja) | 傾斜角度推定システム | |
| Tran et al. | Kalman filtering for wearable fitness monitoring | |
| JP2017015605A (ja) | 慣性計測システムおよびその位置出力方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20200213 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210118 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20210202 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20210215 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6848571 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |