JP5700311B2 - 角速度推定システム - Google Patents
角速度推定システム Download PDFInfo
- Publication number
- JP5700311B2 JP5700311B2 JP2013205710A JP2013205710A JP5700311B2 JP 5700311 B2 JP5700311 B2 JP 5700311B2 JP 2013205710 A JP2013205710 A JP 2013205710A JP 2013205710 A JP2013205710 A JP 2013205710A JP 5700311 B2 JP5700311 B2 JP 5700311B2
- Authority
- JP
- Japan
- Prior art keywords
- acceleration
- axis
- acceleration sensor
- measurement object
- coordinate system
- 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
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Description
医療関連分野以外においても、人間の身体運動を評価するシステムが用いられている。代表的なものとして、スポーツ科学の分野である。スポーツ選手の身体運動特性を評価、分析を行い、そのデータの蓄積を行なうことにより、スポーツ分野における成績向上を図る試みが数多くなされている。
運動評価システムの人間以外への適用例として、ロボット、特に、人型ロボットへの応用を挙げることができる。階段や坂道を歩行している間のロボットの傾斜角度などを測定することにより、この傾斜角度をロボットの制御部にフィードバックし、ロボットが転倒しないようにロボットの姿勢を制御するなどといったことは一般的に既に行なわれている。
複数のカメラによる撮像データに基づき、測定対象体の動作を分析する手法の大きな欠点として、場所的な制約を受けること並びにシステムの構築に多大な費用を要することを挙げることができる。例えば、特許文献1において言及されているが、このようなシステムは、色差に基づきデータ分析を行なうが、時々刻々と変化する周囲の光量の影響を受けやすいという問題があり、多くの場合、強い光を測定対象体に当てるといった手法が採られている。したがって、広い空間或いは屋外空間における動作データの取得は極めて困難である。また、複数のカメラ並びに画像解析システムを必要とし、システム構築を廉価に行なうことは不可能である。
この複数種のセンサを用いた方法は、高価な撮像装置や画像解析システムを用いないため、上記した技術よりは、廉価にシステム構築を行なうことができる。また、周囲の光量の変動に影響を受けるといった問題点を解消できる点で優れている。
まず、ジャイロセンサから得られる角速度データに対して非線形積分を施すことにより、誤差が累積する点である。特に角速度にドリフトが存在すれば累積誤差が大きくなる。この誤差の累積の存在のため、他のセンサ(加速度センサ或いは地磁気センサ)からの出力により、補正処理を行なう必要が生ずる。結果として、複数種のセンサの利用が不可避であり、これに伴い、データ処理の煩雑さが増すこととなる。また、複数種のセンサの使用並びに煩雑なデータ処理工程は、測定対象体の動作(測定対象体の姿勢の傾斜角度)の推定値の精度の悪化を招く。高い精度の推定値を得ようとすれば、積分処理における分割数の増大等の対策をする必要を生じ、結果として、高価なデータ処理システムの使用が不可避なものとなる。
更には、ジャイロセンサは、画像処理システムと比べた場合には、廉価なものであるが、一般的な加速度センサと比べた場合には、高価である。また、地磁気センサにより補正処理を行なう場合には、地磁気センサの外乱因子に対する脆弱性故、傾斜角度推定システムの使用に対して、場所的な制約が生み出されることとなる。
図1は、本発明に係る傾斜角度推定システムの概略図である。
傾斜角度推定システム(1)は、測定対象体(P)に取付けられた複数の加速度センサ(2)と、加速度センサ(2)とデータ通信可能なデータ演算処理ユニット(コンピュータ)(3)からなる。
図1に示す例において、測定対象体(P)は、その一方の端部を回転可能に支持されている。測定対象体(P)は、この支持点を回転中心点(R)として、一方向に回転動作をし、その姿勢を傾斜させる。したがって、図1に示す例においては、測定対象体(P)は、一平面内で回転動作するものとなる。
加速度センサ(2)が取付けられる測定対象体(P)に対して特段の制限はないが、測定対象体の回転傾斜動作の際に生ずる慣性力により変形しない程度の剛性を有することが好ましい。傾斜角度推定のためのデータ取得時における測定対象体(P)の変形は、推定値の誤差に直結するためである。したがって、本発明が対象とする測定対象体(P)には、完全な剛体のみならず、傾斜角度推定の目的が許容する誤差を生じさせる程度に変形可能なものも含まれる。
図1から明らかなように、2つの加速度センサ(2)を結ぶ線の延長線上に測定対象体(P)の回転中心点(R)が位置する。尚、本明細書或いは請求の範囲で、「加速度センサ(2)を結ぶ線が回転中心点(R)を通過する」との文言が用いられるが、この文言は、必ずしも加速度センサ(2)同士を結ぶ線が正確に回転中心点(R)を通過することのみを意味するものではなく、加速度センサ(2)同士を結ぶ線が、回転中心点(R)近傍を通過することも意味する。加速度センサ(2)同士を結ぶ線と回転中心点(R)との距離は、直接的に、傾斜角度推定システム(1)から得られる推定値の誤差に直結するが、傾斜角度推定システム(1)が許容する誤差範囲内の誤差を生み出す程度の加速度センサ(2)同士を結ぶ線と回転中心点(R)との間の距離であれば、本発明の技術的範囲に属するものと解するべきである。
データ演算処理ユニット(3)は、加速度センサ(2)からの出力値(加速度データ)を受信し、データ演算処理ユニット(3)への入力値(回転中心点(R)から各加速度センサ(2)までの距離)と、加速度センサ(2)からの出力値(加速度データ)を用いて、測定対象体(P)の傾斜角度を算出する。
図2において、2つの座標系が示されている。本明細書において、一方の座標系を全体座標系と称し、他方の座標系を瞬時静止座標系と称する。
全体座標系において、重力加速度ベクトルに平行な方向をZ軸として定義する。また、図1に示す測定対象体(P)が回転動作をする平面内において、Z軸に対して直交する方向をX軸とする。ここで、X軸とZ軸の交点と、測定対象体(P)の回転中心点(R)が一致するように全体座標系を定義する。
瞬時静止座標系において、2つの加速度センサ(2)同士を結ぶ線をx軸として定義する。また、図1に示す測定対象体(P)が回転動作をする平面内において、x軸に対して直交する方向をz軸とする。ここで、x軸とz軸の交点と、測定対象体(P)の回転中心点(R)が一致するように瞬時静止座標系を定義する。
図2に示す例において、全体座標系のX軸と瞬時静止座標系のx軸との間の挟角は記号「α」で示されている。
以上の説明並びに座標系の定義から明らかであるが、図2は、測定対象体(P)がその軸(加速度センサ(2)同士を結んだ線)を全体座標系のX軸に置いた位置から角度αだけ回転運動した瞬間の測定対象体(P)の状態を表している。
以下に、この測定対象体(P)の傾斜角度αを算出するための演算処理工程を示す。
測定対象体(P)の回転傾斜動作によって生ずる瞬時静止座標系x軸に平行な加速度成分は、以下の数式1から求めることができる。また、測定対象体(P)の回転傾斜動作によって生ずる瞬時静止座標系z軸に平行な加速度成分は、以下の数式2から求めることができる。
尚、数式1中、「r」は、回転中心点(R)から加速度センサ(2)までの距離を意味する。また、数式1及び数式2において、測定対象体(P)の回転運動の角速度成分、角加速度成分を演算記号「θ」の微分値として表現している。
加速度センサ(2)の出力は、慣性力の影響を受けることから、加速度センサの出力と、測定対象体(P)の回転動作に起因する加速度成分並びに重力加速度の関係は、下記の数式3乃至数式6によって表すことができる。尚、数式3及び数式4は、加速度センサ(2A)に対するものであり、瞬時静止座標系x軸に平行な方向の出力加速度データを演算記号「ax」で示し、瞬時静止座標系z軸に平行な方向の出力加速度データを演算記号「az」で示している。数式5及び6は加速度センサ(2B)に対するものであり、瞬時静止座標系x軸に平行な方向の出力加速度データを演算記号「bx」で示し、瞬時静止座標系z軸に平行な方向の出力加速度データを演算記号「bz」で示している。
(1)加速度センサ(2A)の出力値に対して、回転中心点(R)から加速度センサ(2B)までの距離を積算する。
(2)加速度センサ(2B)の出力値に対して、回転中心点(R)から加速度センサ(2A)までの距離を積算する。
(3)上記手順(1)及び(2)から得られた積算値の差分を取り、測定対象体(P)の角速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式7に示す。
(1)加速度センサ(2A)の出力値に対して、回転中心点(R)から加速度センサ(2B)までの距離を積算する。
(2)加速度センサ(2B)の出力値に対して、回転中心点(R)から加速度センサ(2A)までの距離を積算する。
(3)上記手順(1)及び(2)から得られた積算値の差分を取り、測定対象体(P)の角加速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式8に示す。
したがって、演算処理ユニット(3)に対する入力値として、閾値データを入力可能とすることが好ましい。例えば、演算処理ユニット(3)に対して、第1の閾値範囲として、90°を僅かに下回る値(例えば、85°)及び90°を僅かに上回る値(例えば、95°)を入力する。また、第2の閾値範囲として、0°を僅かに下回る値(例えば、−5°)及び0°を僅かに上回る値(例えば、5°)を入力する。
数式7及び/又は数式8による角度αの演算結果が、演算処理ユニット(3)に入力された第1の閾値範囲であるならば、数式8に基づいて算出された値を演算処理ユニット(3)が出力する。一方、数式7及び/又は数式8による角度αの演算結果が、演算処理ユニット(3)に入力された第2の閾値範囲であるならば、数式7に基づいて算出された値を演算処理ユニット(3)が出力する。
他の方法として、数式7及び数式8に基づく算出結果から、正接関数値を算出し、この正接関数値に基づき、角度αを算出してもよいし、数式7及び数式8に基づき得られた値を積算し、正弦関数の半角の公式に基づき、角度αを算出してもよい。
このようにして求められる測定対象体(P)の回転傾斜運動の角速度及び角加速度の算出式を下記数式9及び数式10に示す。
(1)加速度センサ(2A)の出力値に対して、回転中心点(R)から加速度センサ(2B)までの距離を積算する。
(2)加速度センサ(2B)の出力値に対して、回転中心点(R)から加速度センサ(2A)までの距離を積算する。
(3)上記手順(1)及び(2)から得られた積算値の差分を取り、測定対象体(P)の角速度成分或いは角加速度成分を除去する。
また、図1及び図2に関連して説明したものと同様の手法を用いて、測定対象体(P)の回転運動の角速度及び角加速度を積分演算工程なしに算出することが可能である。
図1及び図2に関連して説明した実施形態においては、測定対象体(P)が2次元平面内を移動していたが、本発明の傾斜角度推定システム(1)は、3次元空間内で回転傾斜運動をする測定対象体(P)に対しても適用可能である。
図3において、初期位置にある測定対象体(P)は点線で示されており、回転傾斜動作を開始した後の測定対象体(P)は実線で示されている。3次元空間の全体座標系(X,Y,Z)のZ軸は、重力加速度に対して平行な方向に設定される。また、測定対象体(P)の初期位置は、本発明の理解を容易にするために、測定対象体(P)の軸、即ち、測定対象体(P)に取付けられた複数の加速度センサ(2)同士を結ぶ線がZ軸と直交するように配されているものとする。また、測定対象体(P)に取付けられた複数の加速度センサ(2)同士を結ぶ線に一致するようにX軸が定められる。また、Y軸は、X軸とZ軸の交点を通過するとともにX軸及びZ軸に直交する軸として定義される。
測定対象体(P)の回転中心点(R)は、X軸、Y軸及びZ軸の交点と一致するものとする。
座標変換の一例として、全体座標(X,Y,Z)上の点G(0,0,g)を考える。この点Gは、全体座標が加速度を表す座標であるとき、重力加速度を意味する点である。図4は、この点GをY軸周りに角度αYだけX軸に向けて回転させた状態を示す。この回転動作の後の点GYの座標(xg1,yg1,zg1)は、下記の数式17により表される。
回転動作後の測定対象体(P)上の任意の位置(x,y,z)の加速度成分は、下記数式19により表される。尚、瞬時静止座標系のx軸に沿う方向の加速度成分は、瞬時静止座標系のy軸周りの角速度とz軸周りの角速度の関数で表され、瞬時静止座標系のy軸に沿う方向の加速度成分は、瞬時静止座標系のy軸周りの角加速度の関数で表され、瞬時静止座標系のz軸に沿う方向の加速度成分は、瞬時静止座標系のz軸周りの角加速度の関数で表される。
(1)加速度センサ(2A)の出力値に対して、回転中心点(R)から加速度センサ(2B)までの距離を積算する。
(2)加速度センサ(2B)の出力値に対して、回転中心点(R)から加速度センサ(2A)までの距離を積算する。
(3)上記手順(1)及び(2)から得られた積算値の差分を取り、測定対象体(P)の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式22に示す。
このようにして、3次元空間内を回転する測定対象体(P)に対しても、積分演算工程を行なうことなく、傾斜角の算出を実行可能である。
上記数式20及び数式21を参照する。ここで、加速度センサ(2A)が出力する瞬時静止座標系のy軸方向の加速度成分と、加速度センサ(2B)が出力する瞬時静止座標系のy軸方向の加速度成分の差をとることにより、全体座標系Z軸周りの角加速度を算出することができる。下記、数式23に、全体座標系Z軸周りの角加速度と加速度センサ(2)の出力値との関係を示す。
同様に、加速度センサ(2A)が出力する瞬時静止座標系のz軸方向の加速度成分と、加速度センサ(2B)が出力する瞬時静止座標系のz軸方向の加速度成分の差をとることにより、全体座標系Y軸周りの角加速度を算出することができる。下記、数式24に、全体座標系Y軸周りの角加速度と加速度センサ(2)の出力値との関係を示す。
図3乃至図5に関連して説明した実施形態においては、まず全体座標系のY軸周りの回転傾斜動作を考え、その後、全体座標系のZ軸周りの回転傾斜動作を考えたが、図6に示す測定対象体(P)の運動に対しては、別の手法により、角度算出を行なう必要がある。
図6に示す測定対象体(P)の位置は、初期位置からY軸周りに角度αだけ測定対象体(P)を回転させ(図7(a)参照)、その後、測定対象体(P)をY軸周りに角度γだけ回転させた位置に一致するものとする(図7(b)参照)。
図8から明らかな如く、ここでは、図6に示す位置に測定対象体(P)が移動する間に、測定対象体(P)はその軸周りに角度βだけ回転したものとする。
次に、図3に関連した説明したものと同様に、加速度を表す全体座標上の点G(0,0,g)の座標変換を考える。尚、点Gは、図3に関連して説明したものと同様に重力加速度を意味する点である。
ここで、全体座標系のY軸周りに、X軸からZ軸方向に向けて点G(0,0,g)を回転変換させた場合を考える。この座標変換後の座標(xg1,yg1,zg1)との間の関係を、下記数式26に示す。
(1)加速度センサ(2A)の出力値に対して、回転中心点(R)から加速度センサ(2B)までの距離を積算する。
(2)加速度センサ(2B)の出力値に対して、回転中心点(R)から加速度センサ(2A)までの距離を積算する。
(3)上記手順(1)及び(2)から得られた積算値の差分を取り、測定対象体(P)の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式32に示す。
したがって、全体座標系Y軸周りの回転傾斜角度並びに瞬時静止座標系x軸周りの回転傾斜角度の算出は、積分演算処理を要せず、代数演算のみで実行可能である。
まず、加速度センサ(2A,2B)それぞれから得られた加速度成分の差を算出する。この演算工程を下記数式(33)に示す。
下記数式34に算出された瞬時静止座標系のy軸周りの角加速度及びz軸周りの角加速度を全体座標系(X,Y,Z)上に変換する行列式を示す。尚、この変換において、X軸からZ軸に向かう方向を正としている。また、瞬時静止座標系x軸に沿う加速度成分は「0」である。
測定対象体(P1)には2つの加速度センサ(2A1,2B1)が取り付けられ、測定対象体(P2)には、2つの加速度センサ(2A2,2B2)が取り付けられている。回転中心点(R)から加速度センサ(2A1)までの距離はra1で表され、回転中心点(R)から加速度センサ(2B1)までの距離はrb1で表され、回転中心点(R)から加速度センサ(2A2)までの距離はra2で表され、回転中心点(R)から加速度センサ(2B2)までの距離はra2で表される。
以下に、回転中心点(R)における加速度ベクトルの算出式を示す。下記数式36は、測定対象体(P1)に対するものであり、下記数式37は測定対象体(P2)に対するものである。
測定対象体(P1,P2,P3)のうち1つをx方向と定めると、他の測定対象体(P1,P2,P3)の軸は、自ずからy方向及びz方向として定義されることとなる。この結果、下記数式38に示す関係式を得ることができる。
2・・・・・加速度センサ
3・・・・・演算処理ユニット
Claims (1)
- 回転動作する物体の回転中心点に配された一の加速度センサと、
該一の加速度センサから離間して配される3つの加速度センサと、
前記一の加速度センサと前記3つの加速度センサからの出力を受信する演算処理ユニットからなり、
前記一の加速度センサと前記3つの加速度センサそれぞれを結ぶ線が互いに直交し、
前記一の加速度センサが、該一の加速度センサと前記3つの加速度センサそれぞれを結ぶ線に対して平行な方向の加速度成分を出力し、
前記3つの加速度センサがそれぞれ、前記一の加速度センサに向かう直線に対して平行な方向の加速度成分を出力し、
前記3つの加速度センサのうち、少なくとも1つが他の加速度センサと前記一の加速度センサを結ぶ線に対して平行な方向の加速度成分を出力し、
前記一の加速度センサと前記3つの加速度センサそれぞれの出力間の関係に基づき、前記物体の回転動作の角速度を推定する角速度推定システム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2013205710A JP5700311B2 (ja) | 2013-09-30 | 2013-09-30 | 角速度推定システム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2013205710A JP5700311B2 (ja) | 2013-09-30 | 2013-09-30 | 角速度推定システム |
Related Parent Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2012093393A Division JP5424224B2 (ja) | 2012-04-16 | 2012-04-16 | 相対角度推定システム |
Related Child Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2014193135A Division JP5958920B2 (ja) | 2014-09-22 | 2014-09-22 | 傾斜角度推定システム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2014032206A JP2014032206A (ja) | 2014-02-20 |
JP5700311B2 true JP5700311B2 (ja) | 2015-04-15 |
Family
ID=50282093
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2013205710A Active JP5700311B2 (ja) | 2013-09-30 | 2013-09-30 | 角速度推定システム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5700311B2 (ja) |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP4753068B2 (ja) * | 2005-09-15 | 2011-08-17 | 独立行政法人産業技術総合研究所 | 移動体の姿勢検出装置、及び、移動ロボット |
CN101487709B (zh) * | 2009-02-27 | 2011-01-26 | 哈尔滨工程大学 | 一种微小型惯性测量装置 |
-
2013
- 2013-09-30 JP JP2013205710A patent/JP5700311B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2014032206A (ja) | 2014-02-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4876204B2 (ja) | 小型姿勢センサ | |
US9599634B2 (en) | System and method for calibrating inertial measurement units | |
CN106662443B (zh) | 用于垂直轨迹确定的方法和系统 | |
JP6019504B2 (ja) | 移動体の進行方向推定装置及び進行方向推定方法 | |
KR20190041315A (ko) | 관성 기반 항법 장치 및 상대사전적분에 따른 관성 기반 항법 방법 | |
JP5706576B2 (ja) | オフセット推定装置、オフセット推定方法、オフセット推定プログラムおよび情報処理装置 | |
JP6191145B2 (ja) | オフセット推定装置及びプログラム | |
CN103487011B (zh) | 一种数据手套的姿态角检测方法 | |
CN107941212B (zh) | 一种视觉与惯性联合定位方法 | |
JP2009186244A (ja) | 傾斜角度推定システム、相対角度推定システム及び角速度推定システム | |
WO2021218731A1 (zh) | Imu与刚体的位姿融合方法、装置、设备及存储介质 | |
CN106709222A (zh) | 基于单目视觉的imu漂移补偿方法 | |
JP2020201125A (ja) | 姿勢推定装置、姿勢推定方法、および姿勢推定プログラム | |
Madrigal et al. | 3D motion tracking of the shoulder joint with respect to the thorax using MARG sensors and data fusion algorithm | |
JP5511088B2 (ja) | 自律測位に用いる重力ベクトルを補正する携帯装置、プログラム及び方法 | |
KR20190022198A (ko) | 착용형 센서를 이용한 자세정보 보정 방법 및 이를 수행하는 기록매체 | |
CN103105166A (zh) | 一种运动练习拍的运动数据处理方法及系统 | |
KR101301462B1 (ko) | 저가형 관성 센서를 이용한 보행자 관성 항법 장치 및 그 항법 | |
JP5424224B2 (ja) | 相対角度推定システム | |
KR101059748B1 (ko) | 특징점 패턴을 이용한 헤드 트랙커에서 특징점 배치 방법 및 헬멧의 위치 추정 방법 | |
Qian et al. | Optical flow based step length estimation for indoor pedestrian navigation on a smartphone | |
JP5424226B2 (ja) | 傾斜角度推定システム | |
JP5958920B2 (ja) | 傾斜角度推定システム | |
KR101745864B1 (ko) | 신체운동 측정장치 | |
JP5700311B2 (ja) | 角速度推定システム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20140528 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20140804 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20140922 |
|
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: 20150107 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20150205 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5700311 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
S111 | Request for change of ownership or part of ownership |
Free format text: JAPANESE INTERMEDIATE CODE: R313111 |
|
R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |