特許 5700311 角速度推定システム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5700311

(24)【登録日】2015年2月27日

(45)【発行日】2015年4月15日

(54)【発明の名称】角速度推定システム

(51)【国際特許分類】

   G01P 7/00 20060101AFI20150326BHJP

【ＦＩ】

   G01P7/00

【請求項の数】1

【全頁数】21

(21)【出願番号】特願2013-205710(P2013-205710)

(22)【出願日】2013年9月30日

(62)【分割の表示】特願2012-93393(P2012-93393)の分割

【原出願日】2008年2月4日

(65)【公開番号】特開2014-32206(P2014-32206A)

(43)【公開日】2014年2月20日

【審査請求日】2013年10月10日

(73)【特許権者】

【識別番号】509093026

【氏名又は名称】公立大学法人高知工科大学

(74)【代理人】

【識別番号】100082072

【弁理士】

【氏名又は名称】清原 義博

(72)【発明者】

【氏名】芝田 京子

(72)【発明者】

【氏名】井上 喜雄

【審査官】 岡田 卓弥

(56)【参考文献】

【文献】 特開２００７−７５９６７（ＪＰ，Ａ）

【文献】 中国特許出願公開第１０１４８７７０９（ＣＮ，Ａ）

【文献】 特開２００５−２０７９７４（ＪＰ，Ａ）

【文献】 特開２００１−１１７７１１（ＪＰ，Ａ）

【文献】 特開平１１−２９５３３５（ＪＰ，Ａ）

【文献】 特開平８−２１９８６９（ＪＰ，Ａ）

【文献】 特開平６−１７４４８７（ＪＰ，Ａ）

【文献】 特開平６−５０９８３（ＪＰ，Ａ）

【文献】 特開平３−１２６４７７（ＪＰ，Ａ）

【文献】 特開昭６０−２５２２１３（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｐ ７／００

Ｇ０１Ｐ１５／００−１５／１８

(57)【特許請求の範囲】

【請求項1】

回転動作する物体の回転中心点に配された一の加速度センサと、
該一の加速度センサから離間して配される３つの加速度センサと、
前記一の加速度センサと前記３つの加速度センサからの出力を受信する演算処理ユニットからなり、
前記一の加速度センサと前記３つの加速度センサそれぞれを結ぶ線が互いに直交し、
前記一の加速度センサが、該一の加速度センサと前記３つの加速度センサそれぞれを結ぶ線に対して平行な方向の加速度成分を出力し、
前記３つの加速度センサがそれぞれ、前記一の加速度センサに向かう直線に対して平行な方向の加速度成分を出力し、
前記３つの加速度センサのうち、少なくとも１つが他の加速度センサと前記一の加速度センサを結ぶ線に対して平行な方向の加速度成分を出力し、
前記一の加速度センサと前記３つの加速度センサそれぞれの出力間の関係に基づき、前記物体の回転動作の角速度を推定する角速度推定システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、回転運動する物体の姿勢或いは速度状態を推定するシステムに関する。より詳しくは、安価に構築可能であり、積分誤差を含まない或いは積分誤差が最小限化された推定値を算出可能とする角速度推定システムに関する。

【背景技術】

【0002】

高齢化社会の到来にともなって、人間の身体運動を評価するシステムに対する需要は増大しつつある。例えば、長期にわたって入院した高齢者が退院するにあたって、日常生活に差し障りのない程度に足腰の筋力の回復が図られているか否かを見極めることは非常に重要であり、十分な筋力の回復がなされる前に退院することにより、その後、他の身体部位を痛め、再度、入院をするといった問題も現存する。
医療関連分野以外においても、人間の身体運動を評価するシステムが用いられている。代表的なものとして、スポーツ科学の分野である。スポーツ選手の身体運動特性を評価、分析を行い、そのデータの蓄積を行なうことにより、スポーツ分野における成績向上を図る試みが数多くなされている。
運動評価システムの人間以外への適用例として、ロボット、特に、人型ロボットへの応用を挙げることができる。階段や坂道を歩行している間のロボットの傾斜角度などを測定することにより、この傾斜角度をロボットの制御部にフィードバックし、ロボットが転倒しないようにロボットの姿勢を制御するなどといったことは一般的に既に行なわれている。

【0003】

近年、このような運動評価システムの開発分野においては、従来、測定対象体の動作を分析する際に、多数の色付マーカをとりつけ、複数のカメラで撮像するという手段が採られていた（例えば、特許文献１参照）。
複数のカメラによる撮像データに基づき、測定対象体の動作を分析する手法の大きな欠点として、場所的な制約を受けること並びにシステムの構築に多大な費用を要することを挙げることができる。例えば、特許文献１において言及されているが、このようなシステムは、色差に基づきデータ分析を行なうが、時々刻々と変化する周囲の光量の影響を受けやすいという問題があり、多くの場合、強い光を測定対象体に当てるといった手法が採られている。したがって、広い空間或いは屋外空間における動作データの取得は極めて困難である。また、複数のカメラ並びに画像解析システムを必要とし、システム構築を廉価に行なうことは不可能である。

【0004】

画像解析によらず、測定対象体の動作の分析を可能とする技術も、近年、開発されてきている。この画像解析を用いないシステムは、一般に、ジャイロセンサ、加速度センサ及び地磁気センサを組み合わせて構築される（例えば、特許文献２参照）。このようなシステムのほとんどは、ジャイロセンサから得られた３軸の角速度に対して、非線形積分を施して、角度を算出し、この算出された角度に対して、加速度計や地磁気センサからの出力を用いて補正を施すものである。
この複数種のセンサを用いた方法は、高価な撮像装置や画像解析システムを用いないため、上記した技術よりは、廉価にシステム構築を行なうことができる。また、周囲の光量の変動に影響を受けるといった問題点を解消できる点で優れている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開平６−５０９８３号公報

【特許文献2】特開２００７−７５９６７号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

上記した複数種のセンサを用いる技術は、画像解析装置を利用する従来の手法と比べて多くの利点を有するものではあるが、いくつかの問題点がある。
まず、ジャイロセンサから得られる角速度データに対して非線形積分を施すことにより、誤差が累積する点である。特に角速度にドリフトが存在すれば累積誤差が大きくなる。この誤差の累積の存在のため、他のセンサ（加速度センサ或いは地磁気センサ）からの出力により、補正処理を行なう必要が生ずる。結果として、複数種のセンサの利用が不可避であり、これに伴い、データ処理の煩雑さが増すこととなる。また、複数種のセンサの使用並びに煩雑なデータ処理工程は、測定対象体の動作（測定対象体の姿勢の傾斜角度）の推定値の精度の悪化を招く。高い精度の推定値を得ようとすれば、積分処理における分割数の増大等の対策をする必要を生じ、結果として、高価なデータ処理システムの使用が不可避なものとなる。
更には、ジャイロセンサは、画像処理システムと比べた場合には、廉価なものであるが、一般的な加速度センサと比べた場合には、高価である。また、地磁気センサにより補正処理を行なう場合には、地磁気センサの外乱因子に対する脆弱性故、傾斜角度推定システムの使用に対して、場所的な制約が生み出されることとなる。

【0007】

本発明は上記実情を鑑みてなされたものであって、積分演算をデータ処理に含めることなく、或いは、積分演算処理工程を最小限しか行わず、回転運動する物体の姿勢或いは速度，加速度状態を推定するシステムを提供することを目的とする。

【課題を解決するための手段】

【0008】

請求項１記載の発明は、回転動作する物体の回転中心点に配された一の加速度センサと、該一の加速度センサから離間して配される３つの加速度センサと、前記一の加速度センサと前記３つの加速度センサからの出力を受信する演算処理ユニットからなり、前記一の加速度センサと前記３つの加速度センサそれぞれを結ぶ線が互いに直交し、前記一の加速度センサが、該一の加速度センサと前記３つの加速度センサそれぞれを結ぶ線に対して平行な方向の加速度成分を出力し、前記３つの加速度センサがそれぞれ、前記一の加速度センサに向かう直線に対して平行な方向の加速度成分を出力し、前記３つの加速度センサのうち、少なくとも１つが他の加速度センサと前記一の加速度センサを結ぶ線に対して平行な方向の加速度成分を出力し、前記一の加速度センサと前記３つの加速度センサそれぞれの出力間の関係に基づき、前記物体の回転動作の角速度を推定する角速度推定システムである。

【発明の効果】

【0019】

請求項１記載の発明によれば、回転動作する物体の角速度を、積分演算処理を要することなく、推定することが可能である。したがって、推定値に積分誤差が含まれず、高い精度の推定値を出力することができる。

【0020】

請求項１記載の発明によれば、積分演算を行なうことなく、傾斜角度の推定を実行可能である。したがって、従来技術のように、積分誤差を含んだ推定値を出力することがなく、高い精度を確保可能である。

【図面の簡単な説明】

【0026】

【図1】本発明に係る傾斜角度推定システムの概略的構成図である。

【図2】図１に示す傾斜角度推定システムの測定対象体の運動の座標系を説明する図である。

【図3】立体空間内で回転動作する測定対象体の運動の座標系を説明する図である。

【図4】立体空間内で回転動作する測定対象体の傾斜角度推定に用いられる座標変換の概念を説明する図である。

【図5】立体空間内で回転動作する測定対象体の傾斜角度推定に用いられる座標変換の概念を説明する図である。

【図6】軸周りに回転する測定対象体の運動の座標系を説明する図である。

【図7】軸周りに回転する測定対象体の運動の座標系を説明する図である。

【図8】軸周りに回転する測定対象体の運動の座標系を説明する図である。

【図9】本発明に係る相対角度推定システムにより相対角度を推定される測定対象体のモデル図である。

【図10】本発明に係る角速度推定システムにより角速度を推定される測定対象体のモデル図である。

【発明を実施するための形態】

【0027】

以下、本発明に係る傾斜角度推定システムについて、図を参照しつつ説明する。
図１は、本発明に係る傾斜角度推定システムの概略図である。
傾斜角度推定システム（１）は、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）と、加速度センサ（２）とデータ通信可能なデータ演算処理ユニット（コンピュータ）（３）からなる。
図１に示す例において、測定対象体（Ｐ）は、その一方の端部を回転可能に支持されている。測定対象体（Ｐ）は、この支持点を回転中心点（Ｒ）として、一方向に回転動作をし、その姿勢を傾斜させる。したがって、図１に示す例においては、測定対象体（Ｐ）は、一平面内で回転動作するものとなる。
加速度センサ（２）が取付けられる測定対象体（Ｐ）に対して特段の制限はないが、測定対象体の回転傾斜動作の際に生ずる慣性力により変形しない程度の剛性を有することが好ましい。傾斜角度推定のためのデータ取得時における測定対象体（Ｐ）の変形は、推定値の誤差に直結するためである。したがって、本発明が対象とする測定対象体（Ｐ）には、完全な剛体のみならず、傾斜角度推定の目的が許容する誤差を生じさせる程度に変形可能なものも含まれる。

【0028】

図１において、測定対象体（Ｐ）上に２つの加速度センサ（２）が示されているが、加速度センサの数は、２つに限定されるものではなく、３或いはそれ以上であってもよい。
図１から明らかなように、２つの加速度センサ（２）を結ぶ線の延長線上に測定対象体（Ｐ）の回転中心点（Ｒ）が位置する。尚、本明細書或いは請求の範囲で、「加速度センサ（２）を結ぶ線が回転中心点（Ｒ）を通過する」との文言が用いられるが、この文言は、必ずしも加速度センサ（２）同士を結ぶ線が正確に回転中心点（Ｒ）を通過することのみを意味するものではなく、加速度センサ（２）同士を結ぶ線が、回転中心点（Ｒ）近傍を通過することも意味する。加速度センサ（２）同士を結ぶ線と回転中心点（Ｒ）との距離は、直接的に、傾斜角度推定システム（１）から得られる推定値の誤差に直結するが、傾斜角度推定システム（１）が許容する誤差範囲内の誤差を生み出す程度の加速度センサ（２）同士を結ぶ線と回転中心点（Ｒ）との間の距離であれば、本発明の技術的範囲に属するものと解するべきである。

【0029】

データ演算処理ユニット（３）には、回転中心点（Ｒ）から各加速度センサ（２）までの距離が予め入力されている。
データ演算処理ユニット（３）は、加速度センサ（２）からの出力値（加速度データ）を受信し、データ演算処理ユニット（３）への入力値（回転中心点（Ｒ）から各加速度センサ（２）までの距離）と、加速度センサ（２）からの出力値（加速度データ）を用いて、測定対象体（Ｐ）の傾斜角度を算出する。

【0030】

図２は、データ演算処理ユニット（３）の演算処理の概念図である。
図２において、２つの座標系が示されている。本明細書において、一方の座標系を全体座標系と称し、他方の座標系を瞬時静止座標系と称する。
全体座標系において、重力加速度ベクトルに平行な方向をＺ軸として定義する。また、図１に示す測定対象体（Ｐ）が回転動作をする平面内において、Ｚ軸に対して直交する方向をＸ軸とする。ここで、Ｘ軸とＺ軸の交点と、測定対象体（Ｐ）の回転中心点（Ｒ）が一致するように全体座標系を定義する。
瞬時静止座標系において、２つの加速度センサ（２）同士を結ぶ線をｘ軸として定義する。また、図１に示す測定対象体（Ｐ）が回転動作をする平面内において、ｘ軸に対して直交する方向をｚ軸とする。ここで、ｘ軸とｚ軸の交点と、測定対象体（Ｐ）の回転中心点（Ｒ）が一致するように瞬時静止座標系を定義する。
図２に示す例において、全体座標系のＸ軸と瞬時静止座標系のｘ軸との間の挟角は記号「α」で示されている。
以上の説明並びに座標系の定義から明らかであるが、図２は、測定対象体（Ｐ）がその軸（加速度センサ（２）同士を結んだ線）を全体座標系のＸ軸に置いた位置から角度αだけ回転運動した瞬間の測定対象体（Ｐ）の状態を表している。
以下に、この測定対象体（Ｐ）の傾斜角度αを算出するための演算処理工程を示す。

【0031】

測定対象体（Ｐ）に取付けられた２つの加速度センサ（２）はそれぞれ、瞬時静止座標系ｘ軸に平行な方向の加速度データ（演算記号「a_x又はb_x」で表される）と、瞬時静止座標系ｚ軸に平行な方向の加速度データ（演算記号「a_z又はb_z」で表される）を出力する。
測定対象体（Ｐ）の回転傾斜動作によって生ずる瞬時静止座標系ｘ軸に平行な加速度成分は、以下の数式１から求めることができる。また、測定対象体（Ｐ）の回転傾斜動作によって生ずる瞬時静止座標系ｚ軸に平行な加速度成分は、以下の数式２から求めることができる。
尚、数式１中、「r」は、回転中心点（Ｒ）から加速度センサ（２）までの距離を意味する。また、数式１及び数式２において、測定対象体（Ｐ）の回転運動の角速度成分、角加速度成分を演算記号「θ」の微分値として表現している。

【0032】

【数1】

【0033】

【数2】

【0034】

ここで、回転中心点（Ｒ）から離れた側に取付けられた加速度センサ（２）を、符号「２Ａ」で表し、回転中心点（Ｒ）に近い側に取付けられた加速度センサ（２）を、符号「２Ｂ」で表す。また、演算処理ユニット（３）には、回転中心点（Ｒ）と加速度センサ（２Ａ）の距離として、値「r_A」が入力されているものとし、回転中心点（Ｒ）と加速度センサ（２Ｂ）の距離として、値「r_B」が入力されているものとする。
加速度センサ（２）の出力は、慣性力の影響を受けることから、加速度センサの出力と、測定対象体（Ｐ）の回転動作に起因する加速度成分並びに重力加速度の関係は、下記の数式３乃至数式６によって表すことができる。尚、数式３及び数式４は、加速度センサ（２Ａ）に対するものであり、瞬時静止座標系ｘ軸に平行な方向の出力加速度データを演算記号「a_x」で示し、瞬時静止座標系ｚ軸に平行な方向の出力加速度データを演算記号「a_z」で示している。数式５及び６は加速度センサ（２Ｂ）に対するものであり、瞬時静止座標系ｘ軸に平行な方向の出力加速度データを演算記号「b_x」で示し、瞬時静止座標系ｚ軸に平行な方向の出力加速度データを演算記号「b_z」で示している。

【0035】

【数3】

【0036】

【数4】

【0037】

【数5】

【0038】

【数6】

【0039】

上記数式３と数式５の関係から、以下の手順で、測定対象体の角度αを求めることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式７に示す。

【0040】

【数7】

【0041】

上記数式４と数式６の関係から、以下の手順で、測定対象体の角度αを求めることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角加速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式８に示す。

【0042】

【数8】

【0043】

上記数式７及び数式８のいずれを用いても、角度αを算出することは可能である。しかしながら、数式７を用いた場合、角度αがπ／２の奇数倍の値の近傍である場合、計測誤差の影響が大きくなって現れる。また、数式８を用いた場合、角度αがπの整数倍の値の近傍である場合、計測誤差の影響が大きくなって現れる。
したがって、演算処理ユニット（３）に対する入力値として、閾値データを入力可能とすることが好ましい。例えば、演算処理ユニット（３）に対して、第１の閾値範囲として、９０°を僅かに下回る値（例えば、８５°）及び９０°を僅かに上回る値（例えば、９５°）を入力する。また、第２の閾値範囲として、０°を僅かに下回る値（例えば、−５°）及び０°を僅かに上回る値（例えば、５°）を入力する。
数式７及び／又は数式８による角度αの演算結果が、演算処理ユニット（３）に入力された第１の閾値範囲であるならば、数式８に基づいて算出された値を演算処理ユニット（３）が出力する。一方、数式７及び／又は数式８による角度αの演算結果が、演算処理ユニット（３）に入力された第２の閾値範囲であるならば、数式７に基づいて算出された値を演算処理ユニット（３）が出力する。
他の方法として、数式７及び数式８に基づく算出結果から、正接関数値を算出し、この正接関数値に基づき、角度αを算出してもよいし、数式７及び数式８に基づき得られた値を積算し、正弦関数の半角の公式に基づき、角度αを算出してもよい。

【0044】

尚、数式７により、角度αに対する正弦関数値が定まるので、この正弦関数値を上記の数式３及び５に代入し、測定対象体（Ｐ）の回転傾斜運動の角速度を算出することが可能である。また、数式８により、角度αに対する余弦関数値が定まるので、この余弦関数値を上記の数式４及び６に代入し、測定対象体（Ｐ）の回転傾斜運動の角加速度を算出することが可能である。
このようにして求められる測定対象体（Ｐ）の回転傾斜運動の角速度及び角加速度の算出式を下記数式９及び数式１０に示す。

【0045】

【数9】

【0046】

【数10】

【0047】

このようにして、図１及び図２に示す実施形態においては、本発明に係る傾斜角度推定システム（１）では、積分演算工程を実行することなく、傾斜角度、傾斜角速度及び傾斜角加速度を算出することが可能である。したがって、従来のジャイロセンサを利用する方式と比して、傾斜角度推定値に積分誤差が含まれることがない。このようにして、本発明に係る傾斜角度推定システム（１）は、出力される推定値に対して、高い精度を確保可能となる。

【0048】

尚、上記数式３及び数式４或いは数式５及び数式６から明らかであるが、回転中心点（Ｒ）から加速度センサ（２）までの距離を「０」とすれば、加速度センサ（２）からの出力値は、測定対象体（Ｐ）の回転動作に係る角速度成分或いは角加速度成分を含まないものとなる。したがって、加速度センサ（２）を回転中心点（Ｒ）に一致するように取付けることで、加速度センサ（２）の出力値を重力加速度で除算することにより、傾斜角度αの正弦関数値或いは余弦関数値を求めることが可能である。尚、この場合には、測定対象体（Ｐ）の回転動作に係る角速度成分或いは角加速度成分を得るためには、算出された角度αに対して時間微分処理を行う必要がある。

【0049】

図１及び図２に示す例において、全体座標系（Ｘ，Ｚ）が静止している場合を考えたが、本発明は、全体座標系が加速度運動している場合にも適用可能である。ここでは、便宜的に、全体座標系の原点（Ｘ_Ｏ，Ｚ_Ｏ）を基準点として説明するが、この基準点は、２つの加速度センサ（２）のうち一方の位置に置き換えてもよい。全体座標系（Ｘ，Ｚ）が一平面内で加速度運動している場合、全体座標系における加速度（Ｄ_Ｘ，Ｄ_Ｙ）は、下記の数式１１及び数式１２によって、それぞれ表される。

【0050】

【数11】

【0051】

【数12】

【0052】

これを瞬時静止座標系（ｘ，ｚ）に座標変換し、瞬時静止座標系（ｘ，ｚ）における基準点（Ｘ_Ｏ，Ｚ_Ｏ）の加速度成分（Ｄ_ｘ，Ｄ_ｚ）を得ることができる。座標変換に係る演算を、下記数式（１３）に示す。

【0053】

【数13】

【0054】

上記数式３乃至数式６で示すものと同様に、加速度センサ（２）からの出力（a_x,a_z,b_x,b_z）を全体座標系が移動している場合について表すと、下記数式１３を得ることができる。

【0055】

【数14】

【0056】

基準点（Ｘ_Ｏ，Ｚ_Ｏ）の両方向の加速度成分が既知の値である場合、図１及び図２に関連して説明した手順と同様に、下記手順を実行することにより、以下の数式１４及び数式１５に示す関係を得ることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。

【0057】

【数15】

【0058】

【数16】

【0059】

数式１４及び数式１５は、一変数の関数であるから、いずれか一方の式から傾斜角度αを算出することもできるし、数式１４及び数式１５を連立させて、正弦関数値或いは余弦関数値を算出してもよい。
また、図１及び図２に関連して説明したものと同様の手法を用いて、測定対象体（Ｐ）の回転運動の角速度及び角加速度を積分演算工程なしに算出することが可能である。

【0060】

基準点（Ｘ_Ｏ，Ｚ_Ｏ）の加速度成分のうち一方が未知である場合においても、上記数式１４及び数式１５が、同一未知パラメータに係る２式をなすので、測定対象体（Ｐ）の傾斜角度α並びに基準点（Ｘ_Ｏ，Ｚ_Ｏ）の未知の加速度成分を、数式１４及び数式１５を連立させることにより算出することができる。特に、対象となる測定対象体（Ｐ）が一方向しか直線移動をしない場合に、この関係は非常に有利である。特に、人間の歩行動作を対象とするとき、多くの場合、鉛直方向の移動は存在せず、専ら、水平方向の移動が取扱われる。したがって、この場合、Ｚ方向の加速度成分を既知の値「０」として取扱うことができ、上記数式１４及び数式１５を連立して得られる下記数式１６によって、測定対象体（Ｐ）の傾斜角度を算出することができる。

【0061】

【数17】

【0062】

図３は、測定対象体（Ｐ）が３次元空間内で回転傾斜動作する態様を示す。
図１及び図２に関連して説明した実施形態においては、測定対象体（Ｐ）が２次元平面内を移動していたが、本発明の傾斜角度推定システム（１）は、３次元空間内で回転傾斜運動をする測定対象体（Ｐ）に対しても適用可能である。
図３において、初期位置にある測定対象体（Ｐ）は点線で示されており、回転傾斜動作を開始した後の測定対象体（Ｐ）は実線で示されている。３次元空間の全体座標系（Ｘ，Ｙ，Ｚ）のＺ軸は、重力加速度に対して平行な方向に設定される。また、測定対象体（Ｐ）の初期位置は、本発明の理解を容易にするために、測定対象体（Ｐ）の軸、即ち、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）同士を結ぶ線がＺ軸と直交するように配されているものとする。また、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）同士を結ぶ線に一致するようにＸ軸が定められる。また、Ｙ軸は、Ｘ軸とＺ軸の交点を通過するとともにＸ軸及びＺ軸に直交する軸として定義される。
測定対象体（Ｐ）の回転中心点（Ｒ）は、Ｘ軸、Ｙ軸及びＺ軸の交点と一致するものとする。

【0063】

次に、瞬時静止座標系の定義について説明する。瞬時静止座標系において、回転傾斜動作開始後の測定対象体（Ｐ）の軸をｘ軸として定義する。したがって、瞬時静止座標系（ｘ，ｙ，ｚ）は、全体座標系（Ｘ，Ｙ，Ｚ）をＹ軸周りに所定角度回転させ、その後、Ｚ軸周りに所定角度回転させた座標ということができる。

【0064】

図４及び図５は、全体座標系（Ｘ，Ｙ，Ｚ）から瞬時静止座標系（ｘ，ｙ，ｚ）への座標変換を説明する図であり、図４は、Ｙ軸周りの回転をさせたときの状態を示し、図５は、図４に示す状態からＺ軸周りの回転をさせたときの状態を示す。
座標変換の一例として、全体座標（Ｘ，Ｙ，Ｚ）上の点Ｇ（０，０，ｇ）を考える。この点Ｇは、全体座標が加速度を表す座標であるとき、重力加速度を意味する点である。図４は、この点ＧをＹ軸周りに角度α_ＹだけＸ軸に向けて回転させた状態を示す。この回転動作の後の点Ｇ_Ｙの座標（ｘ_ｇ１，ｙ_ｇ１，ｚ_ｇ１）は、下記の数式１７により表される。

【0065】

【数18】

【0066】

図５は、図４に示す点Ｇ_Ｙをｚ軸周りに角度α_ｚだけＹ軸に向けて回転させた状態を示す。この回転動作の後の点Ｇ_ＹＺの座標（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）は、下記の数式１８により表される。

【0067】

【数19】

【0068】

図３乃至図５に関連する説明により、３次元空間における全体座標系と瞬時静止座標系の関係が明らかとなった。次に、瞬時静止座標系における測定対象体（Ｐ）上の点の加速度成分について考える。図１及び図２に関連してなされた説明と同様に、測定対象体（Ｐ）上に取付けられた２つの加速度センサ（２Ａ，２Ｂ）が検知する加速度について考える。
回転動作後の測定対象体（Ｐ）上の任意の位置（ｘ，ｙ，ｚ）の加速度成分は、下記数式１９により表される。尚、瞬時静止座標系のｘ軸に沿う方向の加速度成分は、瞬時静止座標系のｙ軸周りの角速度とｚ軸周りの角速度の関数で表され、瞬時静止座標系のｙ軸に沿う方向の加速度成分は、瞬時静止座標系のｙ軸周りの角加速度の関数で表され、瞬時静止座標系のｚ軸に沿う方向の加速度成分は、瞬時静止座標系のｚ軸周りの角加速度の関数で表される。

【0069】

【数20】

【0070】

図１及び図２で説明したように、加速度センサ（２）が出力する加速度は、測定対象体（Ｐ）の回転傾斜動作に起因する加速度成分と重力加速度（慣性力）に起因する加速度成分を含むので、加速度センサ（２）が出力する加速度は、下記数式２０及び数式２１により表される。尚、数式２０は、加速度センサ（２）のうち、回転中心点（Ｒ）から離れた位置（回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離ｒ_Ａ）に配される加速度センサ（２Ａ）に対するものであり、加速度センサ（２Ａ）の瞬間静止座標中の位置座標を（ｘ_Ａ，ｙ_Ａ，ｚ_Ａ）で表し、この位置座標に対応する加速度成分を（ａ_ｘ，ａ_ｙ，ａ_ｚ）で表している。また、数式２１は、加速度センサ（２）のうち、回転中心点（Ｒ）から近い位置（回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離ｒ_Ｂ）に配される加速度センサ（２Ｂ）に対するものであり、加速度センサ（２Ｂ）の瞬間静止座標中の位置座標を（ｘ_Ｂ，ｙ_Ｂ，ｚ_Ｂ）で表し、この位置座標に対応する加速度成分を（ｂ_ｘ，ｂ_ｙ，ｂ_ｚ）で表している。

【0071】

【数21】

【0072】

【数22】

【0073】

傾斜角度α_ｙ及びα_ｚの算出は、図１及び図２に関連したものと同様の下記の手順により実行可能である。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式２２に示す。

【0074】

【数23】

【0075】

上記数式２２から明らかであるが、数式２２に示す関係式から傾斜角度α_ｙ及びα_ｚを算出する方法は多数存在する。例えば、瞬時静止座標系ｚ軸方向の加速度成分のデータから、Ｙ軸周りの傾斜角α_ｙに関する余弦関数値を算出することができる。このＹ軸周りの傾斜角α_ｙに関する余弦関数値に基づき、Ｙ軸周りの傾斜角α_ｙに関する正弦関数値の算出も可能である。算出されたＹ軸周りの傾斜角α_ｙに関する正弦関数値を、瞬時静止座標系ｘ軸方向或いはｙ軸方向の加速度成分の上記関係式に代入することにより、Ｚ軸周りの傾斜角α_ｚの余弦関数値或いは正弦関数値を算出することができる。
このようにして、３次元空間内を回転する測定対象体（Ｐ）に対しても、積分演算工程を行なうことなく、傾斜角の算出を実行可能である。

【0076】

３次元空間内の測定対象体（Ｐ）の回転傾斜動作の角加速度も、積分演算工程を行うことなしに算出可能である。
上記数式２０及び数式２１を参照する。ここで、加速度センサ（２Ａ）が出力する瞬時静止座標系のｙ軸方向の加速度成分と、加速度センサ（２Ｂ）が出力する瞬時静止座標系のｙ軸方向の加速度成分の差をとることにより、全体座標系Ｚ軸周りの角加速度を算出することができる。下記、数式２３に、全体座標系Ｚ軸周りの角加速度と加速度センサ（２）の出力値との関係を示す。
同様に、加速度センサ（２Ａ）が出力する瞬時静止座標系のｚ軸方向の加速度成分と、加速度センサ（２Ｂ）が出力する瞬時静止座標系のｚ軸方向の加速度成分の差をとることにより、全体座標系Ｙ軸周りの角加速度を算出することができる。下記、数式２４に、全体座標系Ｙ軸周りの角加速度と加速度センサ（２）の出力値との関係を示す。

【0077】

【数24】

【0078】

【数25】

【0079】

尚、角速度成分と加速度センサ（２）の出力値との関係を、加速度センサ（２）の瞬時静止座標系ｘ軸方向の加速度に関する出力値に基づき、上記と同様に加速度センサ（２Ａ）と加速度センサ（２Ｂ）の差をとることにより得ることができる。この関係式を下記数式２５に示す。数式２５から明らかであるが、角速度に関しては、積分演算或いは微分演算のデータ処理工程が必要となる。しかしながら、上記の如く、関係式は必要とされるもの以上に存在するため、微分演算処理或いは積分演算処理を行ったとしても、高い精度を確保可能である。

【0080】

【数26】

【0081】

図６は、図３に示す測定対象体（Ｐ）の回転傾斜動作に対して、更に、測定対象体（Ｐ）の軸（加速度センサ（２Ａ，２Ｂ）を結ぶ線）周りに回転する動作を加えた状態を示す。
図３乃至図５に関連して説明した実施形態においては、まず全体座標系のＹ軸周りの回転傾斜動作を考え、その後、全体座標系のＺ軸周りの回転傾斜動作を考えたが、図６に示す測定対象体（Ｐ）の運動に対しては、別の手法により、角度算出を行なう必要がある。

【0082】

図７は、図６に示す測定対象体（Ｐ）の位置変化について説明する図である。尚、全体座標系の定義、瞬時静止座標系の定義並びに測定対象体（Ｐ）の初期位置に関する定義は、図３に関連して説明したものと同様である。
図６に示す測定対象体（Ｐ）の位置は、初期位置からＹ軸周りに角度αだけ測定対象体（Ｐ）を回転させ（図７（ａ）参照）、その後、測定対象体（Ｐ）をＹ軸周りに角度γだけ回転させた位置に一致するものとする（図７（ｂ）参照）。

【0083】

図８は、測定対象体（Ｐ）の軸周りに測定対象体（Ｐ）が回転した状態を示す。図８（ａ）は、初期位置の測定対象体（Ｐ）の横断面を示し、図８（ｂ）は、図６に示す測定対象体（Ｐ）の位置における測定対象体（Ｐ）の横断面を示す。
図８から明らかな如く、ここでは、図６に示す位置に測定対象体（Ｐ）が移動する間に、測定対象体（Ｐ）はその軸周りに角度βだけ回転したものとする。

【0084】

まず、この実施形態においても、上記数式１９に示される関係式は成立する。
次に、図３に関連した説明したものと同様に、加速度を表す全体座標上の点Ｇ（０，０，ｇ）の座標変換を考える。尚、点Ｇは、図３に関連して説明したものと同様に重力加速度を意味する点である。
ここで、全体座標系のＹ軸周りに、Ｘ軸からＺ軸方向に向けて点Ｇ（０，０，ｇ）を回転変換させた場合を考える。この座標変換後の座標（ｘ_ｇ１，ｙ_ｇ１，ｚ_ｇ１）との間の関係を、下記数式２６に示す。

【0085】

【数27】

【0086】

上記数式１９で示される加速度センサ（２）が出力する加速度の各成分に、数式２６で示す座標変換後の重力加速度成分を加える。この変換後の重力加速度成分の加算式を下記数式２７に示す。

【0087】

【数28】

【0088】

次に、測定対象体（Ｐ）の軸周りの回転について考える。測定対象体（Ｐ）の軸が、瞬時静止座標系のｘ軸と一致するように瞬間静止座標系が定義されているから、測定対象体（Ｐ）の軸周りにｚ軸からｙ軸に向けて角度βだけ回転させたとすると、上記数式２７は、下記数式２８に変換されることとなる。

【0089】

【数29】

【0090】

回転中心点（Ｒ）から距離ｒ_ａの位置に存する加速度センサ（２Ａ）の瞬時静止座標系の各成分を（ａ_ｘ，ａ_ｙ，ａ_ｚ）で表し、回転中心点（Ｒ）から距離ｒ_ｂの位置に存する加速度センサ（２Ｂ）の瞬時静止座標系の各成分を（ｂ_ｘ，ｂ_ｙ，ｂ_ｚ）で表すと、以下の数式２９乃至数式３１に示す関係式を得ることができる。尚、下記数式２９は、瞬時静止座標系のｘ軸方向の加速度成分を示し、下記数式３０は、瞬時静止座標系のｙ軸方向の加速度成分を示し、下記数式３１は、瞬時静止座標系のｚ軸方向の加速度成分を示す。

【0091】

【数30】

【0092】

【数31】

【0093】

【数32】

【0094】

数式２９乃至数式３１に示す一対の関係式に対して、上記した手順と同様の下記手順により、角速度成分或いは角加速度成分を除去する。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式３２に示す。

【0095】

【数33】

【0096】

上記数式３２から明らかな如く、瞬時静止座標系ｘ軸に係るデータから、全体座標系Ｙ軸周りの回転傾斜角度の正弦関数値を算出できる。また、この正弦関数値に基づき、余弦関数値も既知のものとなる。したがって、この既知の余弦関数値と、瞬時静止座標系ｘ軸周りの回転角度βを算出することができる。
したがって、全体座標系Ｙ軸周りの回転傾斜角度並びに瞬時静止座標系ｘ軸周りの回転傾斜角度の算出は、積分演算処理を要せず、代数演算のみで実行可能である。

【0097】

続いて、瞬時静止座標系ｚ軸周りの回転を考え、その後、これを全体座標系（Ｘ，Ｙ，Ｚ）上に座標変換し、全体座標系Ｚ軸周りの回転傾斜角度を算出する。
まず、加速度センサ（２Ａ，２Ｂ）それぞれから得られた加速度成分の差を算出する。この演算工程を下記数式（３３）に示す。

【0098】

【数34】

【0099】

上記数式３３に示すｙ軸成分とｚ軸成分に係る関係式を連立させることにより、瞬時静止座標系のｙ軸周りの角加速度及びｚ軸周りの角加速度を算出することができる。
下記数式３４に算出された瞬時静止座標系のｙ軸周りの角加速度及びｚ軸周りの角加速度を全体座標系（Ｘ，Ｙ，Ｚ）上に変換する行列式を示す。尚、この変換において、Ｘ軸からＺ軸に向かう方向を正としている。また、瞬時静止座標系ｘ軸に沿う加速度成分は「０」である。

【0100】

【数35】

【0101】

上記数式３４に基づき、全体座標系（Ｘ，Ｙ，Ｚ）の角加速度成分を得ることができ、これらを積分演算することにより、角速度成分を得ることができる。この積分演算により得られる角速度成分の関係式を下記数式３５に示す。

【0102】

【数36】

【0103】

上記数式３３のうち瞬時静止座標系ｘ軸方向の加速度出力の関係式は、瞬時静止座標系ｙ軸周り及びｚ軸周りの回転に係る加速度成分の関数であるから、上記数式３５を利用して、全体座標系のＹ軸周りの回転速度とＺ軸周りの回転速度の関係式を得ることができる。この関係式を利用して、全体座標系Ｚ軸周りの角速度を積分することにより、積分誤差を抑制しつつ、全体座標系Ｚ軸周りの回転傾斜角度γを算出することができる。

【0104】

図９は、図６乃至図８に関連して説明した傾斜角度推定システム（１）の演算方式の応用例を示す。図９に示す例において、２つの測定対象体（Ｐ１，Ｐ２）が１つの回転中心点（Ｒ）において連結している。また、２つの測定対象体（Ｐ１，Ｐ２）は、それぞれ無関係に回転中心点（Ｒ）を軸として、回転傾斜動作をしている。
測定対象体（Ｐ１）には２つの加速度センサ（２Ａ１，２Ｂ１）が取り付けられ、測定対象体（Ｐ２）には、２つの加速度センサ（２Ａ２，２Ｂ２）が取り付けられている。回転中心点（Ｒ）から加速度センサ（２Ａ１）までの距離はｒ_ａ１で表され、回転中心点（Ｒ）から加速度センサ（２Ｂ１）までの距離はｒ_ｂ１で表され、回転中心点（Ｒ）から加速度センサ（２Ａ２）までの距離はｒ_ａ２で表され、回転中心点（Ｒ）から加速度センサ（２Ｂ２）までの距離はｒ_ａ２で表される。

【0105】

各加速度センサ（２Ａ１，２Ｂ１，２Ａ２，２Ｂ２）は、測定対象体（Ｐ１，Ｐ２）の軸方向の加速度、測定対象体（Ｐ１，Ｐ２）の軸に対して直交する方向の加速度及び測定対象体（Ｐ１，Ｐ２）の軸周りの回転に係る加速度を測定可能である。これにより、測定対象体（Ｐ１，Ｐ２）それぞれの加速度測定点における３成分からなる加速度ベクトルを得ることができる。各測定点の加速度ベクトルが加速度センサ（２Ａ１，２Ｂ１，２Ａ２，２Ｂ２）から求めることができることから、測定対象体（Ｐ１，Ｐ２）の回転中心点（Ｒ）における加速度ベクトルを算出することができる。
以下に、回転中心点（Ｒ）における加速度ベクトルの算出式を示す。下記数式３６は、測定対象体（Ｐ１）に対するものであり、下記数式３７は測定対象体（Ｐ２）に対するものである。

【0106】

【数37】

【0107】

【数38】

【0108】

上記数式３６に示される測定対象体（Ｐ１）の回転中心点（Ｒ）における加速度ベクトルを全体座標系（Ｘ，Ｙ，Ｚ）のＹ軸周りに角度αだけ回転させ、Ｚ軸周りに角度γだけ回転させ、測定対象体（Ｐ１）軸周りに角度βだけ回転させたとき（図７及び図８参照）、他方の測定対象体（Ｐ２）の加速度ベクトルに一致する場合を考える。このような角度（α，β，γ）を数値演算により求めることにより、測定対象体（Ｐ１）に対する測定対象体（Ｐ２）の相対的角度位置を算出することができる。

【0109】

図１０は、互いに直交するように配された３つの測定対象体（Ｐ１，Ｐ２，Ｐ３）を考えた場合の概念図である。３つの測定対象体（Ｐ１，Ｐ２，Ｐ３）は、互いに一点で交わり、この１つの交点を回転中心点（Ｒ）として回転傾斜運動をする。回転中心点（Ｒ）には、各測定対象体（Ｐ１，Ｐ２，Ｐ３）の軸方向の加速度成分を測定する加速度センサ（２）が取り付けられている。また、各測定対象体（Ｐ１，Ｐ２，Ｐ３）上には更にそれぞれ１つずつ加速度センサが取り付けられている。
測定対象体（Ｐ１，Ｐ２，Ｐ３）のうち１つをｘ方向と定めると、他の測定対象体（Ｐ１，Ｐ２，Ｐ３）の軸は、自ずからｙ方向及びｚ方向として定義されることとなる。この結果、下記数式３８に示す関係式を得ることができる。

【0110】

【数39】

【0111】

上記数式３８の各軸方向成分に係る関係式を連立させることにより、３つの角速度の絶対値を算出することができる。３つの軸のうち２つの方向に関しては、角速度が計測により求められているので、符号が判明する。他のもう１つの軸については、角速度が計測により判明する軸と平行な方向の加速度を同時に測定することにより、当該他のもう１つの軸の加速度の符号も判明する。

【産業上の利用可能性】

【0112】

本発明は、人間の歩行動作解析研究、人間のバランス機能の評価、ロボットの移動動作解析研究、ゴルフなどのスポーツのトレーニングや医療福祉機器の開発に好適に利用可能である。

【符号の説明】

【0113】

１・・・・・傾斜角度推定システム
２・・・・・加速度センサ
３・・・・・演算処理ユニット

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】