傾斜角度推定システム
【課題】積分演算をデータ処理に含めることなく、或いは、積分演算処理工程を最小限しか行わず、回転運動する物体の姿勢或いは速度状態を推定するシステムの提供。
【解決手段】一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、前記物体に取付けられた少なくとも２つの加速度センサと、該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過する。
【解決手段】一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、前記物体に取付けられた少なくとも２つの加速度センサと、該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、回転運動する物体の姿勢或いは速度状態を推定するシステムに関する。より詳しくは、安価に構築可能であり、積分誤差を含まない或いは積分誤差が最小限化された推定値を算出可能とする傾斜角度推定システムに関する。
【背景技術】
【０００２】
高齢化社会の到来にともなって、人間の身体運動を評価するシステムに対する需要は増大しつつある。例えば、長期にわたって入院した高齢者が退院するにあたって、日常生活に差し障りのない程度に足腰の筋力の回復が図られているか否かを見極めることは非常に重要であり、十分な筋力の回復がなされる前に退院することにより、その後、他の身体部位を痛め、再度、入院をするといった問題も現存する。
医療関連分野以外においても、人間の身体運動を評価するシステムが用いられている。代表的なものとして、スポーツ科学の分野である。スポーツ選手の身体運動特性を評価、分析を行い、そのデータの蓄積を行なうことにより、スポーツ分野における成績向上を図る試みが数多くなされている。
運動評価システムの人間以外への適用例として、ロボット、特に、人型ロボットへの応用を挙げることができる。階段や坂道を歩行している間のロボットの傾斜角度などを測定することにより、この傾斜角度をロボットの制御部にフィードバックし、ロボットが転倒しないようにロボットの姿勢を制御するなどといったことは一般的に既に行なわれている。
【０００３】
近年、このような運動評価システムの開発分野においては、従来、測定対象体の動作を分析する際に、多数の色付マーカをとりつけ、複数のカメラで撮像するという手段が採られていた（例えば、特許文献１参照）。
複数のカメラによる撮像データに基づき、測定対象体の動作を分析する手法の大きな欠点として、場所的な制約を受けること並びにシステムの構築に多大な費用を要することを挙げることができる。例えば、特許文献１において言及されているが、このようなシステムは、色差に基づきデータ分析を行なうが、時々刻々と変化する周囲の光量の影響を受けやすいという問題があり、多くの場合、強い光を測定対象体に当てるといった手法が採られている。したがって、広い空間或いは屋外空間における動作データの取得は極めて困難である。また、複数のカメラ並びに画像解析システムを必要とし、システム構築を廉価に行なうことは不可能である。
【０００４】
画像解析によらず、測定対象体の動作の分析を可能とする技術も、近年、開発されてきている。この画像解析を用いないシステムは、一般に、ジャイロセンサ、加速度センサ及び地磁気センサを組み合わせて構築される（例えば、特許文献２参照）。このようなシステムのほとんどは、ジャイロセンサから得られた３軸の角速度に対して、非線形積分を施して、角度を算出し、この算出された角度に対して、加速度計や地磁気センサからの出力を用いて補正を施すものである。
この複数種のセンサを用いた方法は、高価な撮像装置や画像解析システムを用いないため、上記した技術よりは、廉価にシステム構築を行なうことができる。また、周囲の光量の変動に影響を受けるといった問題点を解消できる点で優れている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−５０９８３号公報
【特許文献２】特開２００７−７５９６７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記した複数種のセンサを用いる技術は、画像解析装置を利用する従来の手法と比べて多くの利点を有するものではあるが、いくつかの問題点がある。
まず、ジャイロセンサから得られる角速度データに対して非線形積分を施すことにより、誤差が累積する点である。特に角速度にドリフトが存在すれば累積誤差が大きくなる。この誤差の累積の存在のため、他のセンサ（加速度センサ或いは地磁気センサ）からの出力により、補正処理を行なう必要が生ずる。結果として、複数種のセンサの利用が不可避であり、これに伴い、データ処理の煩雑さが増すこととなる。また、複数種のセンサの使用並びに煩雑なデータ処理工程は、測定対象体の動作（測定対象体の姿勢の傾斜角度）の推定値の精度の悪化を招く。高い精度の推定値を得ようとすれば、積分処理における分割数の増大等の対策をする必要を生じ、結果として、高価なデータ処理システムの使用が不可避なものとなる。
更には、ジャイロセンサは、画像処理システムと比べた場合には、廉価なものであるが、一般的な加速度センサと比べた場合には、高価である。また、地磁気センサにより補正処理を行なう場合には、地磁気センサの外乱因子に対する脆弱性故、傾斜角度推定システムの使用に対して、場所的な制約が生み出されることとなる。
【０００７】
本発明は上記実情を鑑みてなされたものであって、積分演算をデータ処理に含めることなく、或いは、積分演算処理工程を最小限しか行わず、回転運動する物体の姿勢或いは速度，加速度状態を推定するシステムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
請求項１記載の発明は、一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、π／２の奇数倍数値に対してπ／２を超えない所定の角度だけ増分した第１の上限値と、前記π／２の奇数倍数値に対して前記所定の角度だけ減分した第１の下限値の間で定義される第１の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第１の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記余弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システムである。
【０００９】
請求項２記載の発明は、一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を乗算する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、πの倍数値に対してπを超えない所定の角度だけ増分した第２の上限値と、前記πの倍数値に対して前記所定の角度だけ減分した第２の下限値の間で定義される第２の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第２の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記正弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システムである。
【発明の効果】
【００１０】
請求項１及び２記載の発明によれば、用いるセンサが加速度センサのみであるので、簡便且つ廉価に傾斜角度推定システムを構築可能となる。したがって、従来技術と比して、推定システムの設定を簡便に行なうことができる。
請求項１及び２記載の発明によれば、積分演算を行なうことなく、傾斜角度の推定を実行可能である。したがって、従来技術のように、積分誤差を含んだ推定値を出力することがなく、高い精度を確保可能である。
請求項１及び２記載の発明によれば、算出された傾斜角度の推定値に応じて、出力する推定値の算出に用いられる関数の選択が可能となる。
【図面の簡単な説明】
【００１１】
【図１】本発明に係る傾斜角度推定システムの概略的構成図である。
【図２】図１に示す傾斜角度推定システムの測定対象体の運動の座標系を説明する図である。
【図３】立体空間内で回転動作する測定対象体の運動の座標系を説明する図である。
【図４】立体空間内で回転動作する測定対象体の傾斜角度推定に用いられる座標変換の概念を説明する図である。
【図５】立体空間内で回転動作する測定対象体の傾斜角度推定に用いられる座標変換の概念を説明する図である。
【図６】軸周りに回転する測定対象体の運動の座標系を説明する図である。
【図７】軸周りに回転する測定対象体の運動の座標系を説明する図である。
【図８】軸周りに回転する測定対象体の運動の座標系を説明する図である。
【図９】本発明に係る相対角度推定システムにより相対角度を推定される測定対象体のモデル図である。
【図１０】本発明に係る角速度推定システムにより角速度を推定される測定対象体のモデル図である。
【発明を実施するための形態】
【００１２】
以下、本発明に係る傾斜角度推定システムについて、図を参照しつつ説明する。
図１は、本発明に係る傾斜角度推定システムの概略図である。
傾斜角度推定システム（１）は、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）と、加速度センサ（２）とデータ通信可能なデータ演算処理ユニット（コンピュータ）（３）からなる。
図１に示す例において、測定対象体（Ｐ）は、その一方の端部を回転可能に支持されている。測定対象体（Ｐ）は、この支持点を回転中心点（Ｒ）として、一方向に回転動作をし、その姿勢を傾斜させる。したがって、図１に示す例においては、測定対象体（Ｐ）は、一平面内で回転動作するものとなる。
加速度センサ（２）が取付けられる測定対象体（Ｐ）に対して特段の制限はないが、測定対象体の回転傾斜動作の際に生ずる慣性力により変形しない程度の剛性を有することが好ましい。傾斜角度推定のためのデータ取得時における測定対象体（Ｐ）の変形は、推定値の誤差に直結するためである。したがって、本発明が対象とする測定対象体（Ｐ）には、完全な剛体のみならず、傾斜角度推定の目的が許容する誤差を生じさせる程度に変形可能なものも含まれる。
【００１３】
図１において、測定対象体（Ｐ）上に２つの加速度センサ（２）が示されているが、加速度センサの数は、２つに限定されるものではなく、３或いはそれ以上であってもよい。
図１から明らかなように、２つの加速度センサ（２）を結ぶ線の延長線上に測定対象体（Ｐ）の回転中心点（Ｒ）が位置する。尚、本明細書或いは請求の範囲で、「加速度センサ（２）を結ぶ線が回転中心点（Ｒ）を通過する」との文言が用いられるが、この文言は、必ずしも加速度センサ（２）同士を結ぶ線が正確に回転中心点（Ｒ）を通過することのみを意味するものではなく、加速度センサ（２）同士を結ぶ線が、回転中心点（Ｒ）近傍を通過することも意味する。加速度センサ（２）同士を結ぶ線と回転中心点（Ｒ）との距離は、直接的に、傾斜角度推定システム（１）から得られる推定値の誤差に直結するが、傾斜角度推定システム（１）が許容する誤差範囲内の誤差を生み出す程度の加速度センサ（２）同士を結ぶ線と回転中心点（Ｒ）との間の距離であれば、本発明の技術的範囲に属するものと解するべきである。
【００１４】
データ演算処理ユニット（３）には、回転中心点（Ｒ）から各加速度センサ（２）までの距離が予め入力されている。
データ演算処理ユニット（３）は、加速度センサ（２）からの出力値（加速度データ）を受信し、データ演算処理ユニット（３）への入力値（回転中心点（Ｒ）から各加速度センサ（２）までの距離）と、加速度センサ（２）からの出力値（加速度データ）を用いて、測定対象体（Ｐ）の傾斜角度を算出する。
【００１５】
図２は、データ演算処理ユニット（３）の演算処理の概念図である。
図２において、２つの座標系が示されている。本明細書において、一方の座標系を全体座標系と称し、他方の座標系を瞬時静止座標系と称する。
全体座標系において、重力加速度ベクトルに平行な方向をＺ軸として定義する。また、図１に示す測定対象体（Ｐ）が回転動作をする平面内において、Ｚ軸に対して直交する方向をＸ軸とする。ここで、Ｘ軸とＺ軸の交点と、測定対象体（Ｐ）の回転中心点（Ｒ）が一致するように全体座標系を定義する。
瞬時静止座標系において、２つの加速度センサ（２）同士を結ぶ線をｘ軸として定義する。また、図１に示す測定対象体（Ｐ）が回転動作をする平面内において、ｘ軸に対して直交する方向をｚ軸とする。ここで、ｘ軸とｚ軸の交点と、測定対象体（Ｐ）の回転中心点（Ｒ）が一致するように瞬時静止座標系を定義する。
図２に示す例において、全体座標系のＸ軸と瞬時静止座標系のｘ軸との間の挟角は記号「α」で示されている。
以上の説明並びに座標系の定義から明らかであるが、図２は、測定対象体（Ｐ）がその軸（加速度センサ（２）同士を結んだ線）を全体座標系のＸ軸に置いた位置から角度αだけ回転運動した瞬間の測定対象体（Ｐ）の状態を表している。
以下に、この測定対象体（Ｐ）の傾斜角度αを算出するための演算処理工程を示す。
【００１６】
測定対象体（Ｐ）に取付けられた２つの加速度センサ（２）はそれぞれ、瞬時静止座標系ｘ軸に平行な方向の加速度データ（演算記号「ax又はbx」で表される）と、瞬時静止座標系ｚ軸に平行な方向の加速度データ（演算記号「az又はbz」で表される）を出力する。
測定対象体（Ｐ）の回転傾斜動作によって生ずる瞬時静止座標系ｘ軸に平行な加速度成分は、以下の数式１から求めることができる。また、測定対象体（Ｐ）の回転傾斜動作によって生ずる瞬時静止座標系ｚ軸に平行な加速度成分は、以下の数式２から求めることができる。
尚、数式１中、「r」は、回転中心点（Ｒ）から加速度センサ（２）までの距離を意味する。また、数式１及び数式２において、測定対象体（Ｐ）の回転運動の角速度成分、角加速度成分を演算記号「θ」の微分値として表現している。
【００１７】
【数１】
【００１８】
【数２】
【００１９】
ここで、回転中心点（Ｒ）から離れた側に取付けられた加速度センサ（２）を、符号「２Ａ」で表し、回転中心点（Ｒ）に近い側に取付けられた加速度センサ（２）を、符号「２Ｂ」で表す。また、演算処理ユニット（３）には、回転中心点（Ｒ）と加速度センサ（２Ａ）の距離として、値「rA」が入力されているものとし、回転中心点（Ｒ）と加速度センサ（２Ｂ）の距離として、値「rB」が入力されているものとする。
加速度センサ（２）の出力は、慣性力の影響を受けることから、加速度センサの出力と、測定対象体（Ｐ）の回転動作に起因する加速度成分並びに重力加速度の関係は、下記の数式３乃至数式６によって表すことができる。尚、数式３及び数式４は、加速度センサ（２Ａ）に対するものであり、瞬時静止座標系ｘ軸に平行な方向の出力加速度データを演算記号「ax」で示し、瞬時静止座標系ｚ軸に平行な方向の出力加速度データを演算記号「az」で示している。数式５及び６は加速度センサ（２Ｂ）に対するものであり、瞬時静止座標系ｘ軸に平行な方向の出力加速度データを演算記号「bx」で示し、瞬時静止座標系ｚ軸に平行な方向の出力加速度データを演算記号「bz」で示している。
【００２０】
【数３】
【００２１】
【数４】
【００２２】
【数５】
【００２３】
【数６】
【００２４】
上記数式３と数式５の関係から、以下の手順で、測定対象体の角度αを求めることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式７に示す。
【００２５】
【数７】
【００２６】
上記数式４と数式６の関係から、以下の手順で、測定対象体の角度αを求めることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角加速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式８に示す。
【００２７】
【数８】
【００２８】
上記数式７及び数式８のいずれを用いても、角度αを算出することは可能である。しかしながら、数式７を用いた場合、角度αがπ／２の奇数倍の値の近傍である場合、計測誤差の影響が大きくなって現れる。また、数式８を用いた場合、角度αがπの整数倍の値の近傍である場合、計測誤差の影響が大きくなって現れる。
したがって、演算処理ユニット（３）に対する入力値として、閾値データを入力可能とすることが好ましい。例えば、演算処理ユニット（３）に対して、第１の閾値範囲として、９０°を僅かに下回る値（例えば、８５°）及び９０°を僅かに上回る値（例えば、９５°）を入力する。また、第２の閾値範囲として、０°を僅かに下回る値（例えば、−５°）及び０°を僅かに上回る値（例えば、５°）を入力する。
数式７及び／又は数式８による角度αの演算結果が、演算処理ユニット（３）に入力された第１の閾値範囲であるならば、数式８に基づいて算出された値を演算処理ユニット（３）が出力する。一方、数式７及び／又は数式８による角度αの演算結果が、演算処理ユニット（３）に入力された第２の閾値範囲であるならば、数式７に基づいて算出された値を演算処理ユニット（３）が出力する。
他の方法として、数式７及び数式８に基づく算出結果から、正接関数値を算出し、この正接関数値に基づき、角度αを算出してもよいし、数式７及び数式８に基づき得られた値を積算し、正弦関数の半角の公式に基づき、角度αを算出してもよい。
【００２９】
尚、数式７により、角度αに対する正弦関数値が定まるので、この正弦関数値を上記の数式３及び５に代入し、測定対象体（Ｐ）の回転傾斜運動の角速度を算出することが可能である。また、数式８により、角度αに対する余弦関数値が定まるので、この余弦関数値を上記の数式４及び６に代入し、測定対象体（Ｐ）の回転傾斜運動の角加速度を算出することが可能である。
このようにして求められる測定対象体（Ｐ）の回転傾斜運動の角速度及び角加速度の算出式を下記数式９及び数式１０に示す。
【００３０】
【数９】
【００３１】
【数１０】
【００３２】
このようにして、図１及び図２に示す実施形態においては、本発明に係る傾斜角度推定システム（１）では、積分演算工程を実行することなく、傾斜角度、傾斜角速度及び傾斜角加速度を算出することが可能である。したがって、従来のジャイロセンサを利用する方式と比して、傾斜角度推定値に積分誤差が含まれることがない。このようにして、本発明に係る傾斜角度推定システム（１）は、出力される推定値に対して、高い精度を確保可能となる。
【００３３】
尚、上記数式３及び数式４或いは数式５及び数式６から明らかであるが、回転中心点（Ｒ）から加速度センサ（２）までの距離を「０」とすれば、加速度センサ（２）からの出力値は、測定対象体（Ｐ）の回転動作に係る角速度成分或いは角加速度成分を含まないものとなる。したがって、加速度センサ（２）を回転中心点（Ｒ）に一致するように取付けることで、加速度センサ（２）の出力値を重力加速度で除算することにより、傾斜角度αの正弦関数値或いは余弦関数値を求めることが可能である。尚、この場合には、測定対象体（Ｐ）の回転動作に係る角速度成分或いは角加速度成分を得るためには、算出された角度αに対して時間微分処理を行う必要がある。
【００３４】
図１及び図２に示す例において、全体座標系（Ｘ，Ｚ）が静止している場合を考えたが、本発明は、全体座標系が加速度運動している場合にも適用可能である。ここでは、便宜的に、全体座標系の原点（ＸＯ，ＺＯ）を基準点として説明するが、この基準点は、２つの加速度センサ（２）のうち一方の位置に置き換えてもよい。全体座標系（Ｘ，Ｚ）が一平面内で加速度運動している場合、全体座標系における加速度（ＤＸ，ＤＹ）は、下記の数式１１及び数式１２によって、それぞれ表される。
【００３５】
【数１１】
【００３６】
【数１２】
【００３７】
これを瞬時静止座標系（ｘ，ｚ）に座標変換し、瞬時静止座標系（ｘ，ｚ）における基準点（ＸＯ，ＺＯ）の加速度成分（Ｄｘ，Ｄｚ）を得ることができる。座標変換に係る演算を、下記数式１３に示す。
【００３８】
【数１３】
【００３９】
上記数式３乃至数式６で示すものと同様に、加速度センサ（２）からの出力（ax,az,bx,bz）を全体座標系が移動している場合について表すと、下記数式１４を得ることができる。
【００４０】
【数１４】
【００４１】
基準点（ＸＯ，ＺＯ）の両方向の加速度成分が既知の値である場合、図１及び図２に関連して説明した手順と同様に、下記手順を実行することにより、以下の数式１５及び数式１６に示す関係を得ることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
【００４２】
【数１５】
【００４３】
【数１６】
【００４４】
数式１５及び数式１６は、一変数の関数であるから、いずれか一方の式から傾斜角度αを算出することもできるし、数式１５及び数式１６を連立させて、正弦関数値或いは余弦関数値を算出してもよい。
また、図１及び図２に関連して説明したものと同様の手法を用いて、測定対象体（Ｐ）の回転運動の角速度及び角加速度を積分演算工程なしに算出することが可能である。
【００４５】
基準点（ＸＯ，ＺＯ）の加速度成分のうち一方が未知である場合においても、上記数式１５及び数式１６が、同一未知パラメータに係る２式をなすので、測定対象体（Ｐ）の傾斜角度α並びに基準点（ＸＯ，ＺＯ）の未知の加速度成分を、数式１５及び数式１６を連立させることにより算出することができる。特に、対象となる測定対象体（Ｐ）が一方向しか直線移動をしない場合に、この関係は非常に有利である。特に、人間の歩行動作を対象とするとき、多くの場合、鉛直方向の移動は存在せず、専ら、水平方向の移動が取扱われる。したがって、この場合、Ｚ方向の加速度成分を既知の値「０」として取扱うことができ、上記数式１５及び数式１６を連立して得られる下記数式１７によって、測定対象体（Ｐ）の傾斜角度を算出することができる。
【００４６】
【数１７】
【００４７】
図３は、測定対象体（Ｐ）が３次元空間内で回転傾斜動作する態様を示す。
図１及び図２に関連して説明した実施形態においては、測定対象体（Ｐ）が２次元平面内を移動していたが、本発明の傾斜角度推定システム（１）は、３次元空間内で回転傾斜運動をする測定対象体（Ｐ）に対しても適用可能である。
図３において、初期位置にある測定対象体（Ｐ）は点線で示されており、回転傾斜動作を開始した後の測定対象体（Ｐ）は実線で示されている。３次元空間の全体座標系（Ｘ，Ｙ，Ｚ）のＺ軸は、重力加速度に対して平行な方向に設定される。また、測定対象体（Ｐ）の初期位置は、本発明の理解を容易にするために、測定対象体（Ｐ）の軸、即ち、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）同士を結ぶ線がＺ軸と直交するように配されているものとする。また、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）同士を結ぶ線に一致するようにＸ軸が定められる。また、Ｙ軸は、Ｘ軸とＺ軸の交点を通過するとともにＸ軸及びＺ軸に直交する軸として定義される。
測定対象体（Ｐ）の回転中心点（Ｒ）は、Ｘ軸、Ｙ軸及びＺ軸の交点と一致するものとする。
【００４８】
次に、瞬時静止座標系の定義について説明する。瞬時静止座標系において、回転傾斜動作開始後の測定対象体（Ｐ）の軸をｘ軸として定義する。したがって、瞬時静止座標系（ｘ，ｙ，ｚ）は、全体座標系（Ｘ，Ｙ，Ｚ）をＹ軸周りに所定角度回転させ、その後、Ｚ軸周りに所定角度回転させた座標ということができる。
【００４９】
図４及び図５は、全体座標系（Ｘ，Ｙ，Ｚ）から瞬時静止座標系（ｘ，ｙ，ｚ）への座標変換を説明する図であり、図４は、Ｙ軸周りの回転をさせたときの状態を示し、図５は、図４に示す状態からＺ軸周りの回転をさせたときの状態を示す。
座標変換の一例として、全体座標（Ｘ，Ｙ，Ｚ）上の点Ｇ（０，０，ｇ）を考える。この点Ｇは、全体座標が加速度を表す座標であるとき、重力加速度を意味する点である。図４は、この点ＧをＹ軸周りに角度αγだけＸ軸に向けて回転させた状態を示す。この回転動作の後の点ＧＹの座標（ｘｇ１，ｙｇ１，ｚｇ１）は、下記の数式１８により表される。
【００５０】
【数１８】
【００５１】
図５は、図４に示す点ＧＹをｚ軸周りに角度αｚだけＹ軸に向けて回転させた状態を示す。この回転動作の後の点ＧＹＺの座標（ｘｇ，ｙｇ，ｚｇ）は、下記の数式１９により表される。
【００５２】
【数１９】
【００５３】
図３乃至図５に関連する説明により、３次元空間における全体座標系と瞬時静止座標系の関係が明らかとなった。次に、瞬時静止座標系における測定対象体（Ｐ）上の点の加速度成分について考える。図１及び図２に関連してなされた説明と同様に、測定対象体（Ｐ）上に取付けられた２つの加速度センサ（２Ａ，２Ｂ）が検知する加速度について考える。
回転動作後の測定対象体（Ｐ）上の任意の位置（ｘ，ｙ，ｚ）の加速度成分は、下記数式２０により表される。尚、瞬時静止座標系のｘ軸に沿う方向の加速度成分は、瞬時静止座標系のｙ軸周りの角速度とｚ軸周りの角速度の関数で表され、瞬時静止座標系のｙ軸に沿う方向の加速度成分は、瞬時静止座標系のｙ軸周りの角加速度の関数で表され、瞬時静止座標系のｚ軸に沿う方向の加速度成分は、瞬時静止座標系のｚ軸周りの角加速度の関数で表される。
【００５４】
【数２０】
【００５５】
図１及び図２で説明したように、加速度センサ（２）が出力する加速度は、測定対象体（Ｐ）の回転傾斜動作に起因する加速度成分と重力加速度（慣性力）に起因する加速度成分を含むので、加速度センサ（２）が出力する加速度は、下記数式２１及び数式２２により表される。尚、数式２１は、加速度センサ（２）のうち、回転中心点（Ｒ）から離れた位置（回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離ｒＡ）に配される加速度センサ（２Ａ）に対するものであり、加速度センサ（２Ａ）の瞬間静止座標中の位置座標を（ｘＡ，ｙＡ，ｚＡ）で表し、この位置座標に対応する加速度成分を（ａｘ，ａｙ，ａｚ）で表している。また、数式２２は、加速度センサ（２）のうち、回転中心点（Ｒ）から近い位置（回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離ｒＢ）に配される加速度センサ（２Ｂ）に対するものであり、加速度センサ（２Ｂ）の瞬間静止座標中の位置座標を（ｘＢ，ｙＢ，ｚＢ）で表し、この位置座標に対応する加速度成分を（ｂｘ，ｂｙ，ｂｚ）で表している。
【００５６】
【数２１】
【００５７】
【数２２】
【００５８】
傾斜角度αｙ及びαｚの算出は、図１及び図２に関連したものと同様の下記の手順により実行可能である。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式２３に示す。
【００５９】
【数２３】
【００６０】
上記数式２３から明らかであるが、数式２３に示す関係式から傾斜角度αｙ及びαｚを算出する方法は多数存在する。例えば、瞬時静止座標系ｚ軸方向の加速度成分のデータから、Ｙ軸周りの傾斜角αｙに関する余弦関数値を算出することができる。このＹ軸周りの傾斜角αｙに関する余弦関数値に基づき、Ｙ軸周りの傾斜角αｙに関する正弦関数値の算出も可能である。算出されたＹ軸周りの傾斜角αｙに関する正弦関数値を、瞬時静止座標系ｘ軸方向或いはｙ軸方向の加速度成分の上記関係式に代入することにより、Ｚ軸周りの傾斜角αｚの余弦関数値或いは正弦関数値を算出することができる。
このようにして、３次元空間内を回転する測定対象体（Ｐ）に対しても、積分演算工程を行なうことなく、傾斜角の算出を実行可能である。
【００６１】
３次元空間内の測定対象体（Ｐ）の回転傾斜動作の角加速度も、積分演算工程を行うことなしに算出可能である。
上記数式２１及び数式２２を参照する。ここで、加速度センサ（２Ａ）が出力する瞬時静止座標系のｙ軸方向の加速度成分と、加速度センサ（２Ｂ）が出力する瞬時静止座標系のｙ軸方向の加速度成分の差をとることにより、全体座標系Ｚ軸周りの角加速度を算出することができる。下記数式２４に、全体座標系Ｚ軸周りの角加速度と加速度センサ（２）の出力値との関係を示す。
同様に、加速度センサ（２Ａ）が出力する瞬時静止座標系のｚ軸方向の加速度成分と、加速度センサ（２Ｂ）が出力する瞬時静止座標系のｚ軸方向の加速度成分の差をとることにより、全体座標系Ｙ軸周りの角加速度を算出することができる。下記、数式２５に、全体座標系Ｙ軸周りの角加速度と加速度センサ（２）の出力値との関係を示す。
【００６２】
【数２４】
【００６３】
【数２５】
【００６４】
尚、角速度成分と加速度センサ（２）の出力値との関係を、加速度センサ（２）の瞬時静止座標系ｘ軸方向の加速度に関する出力値に基づき、上記と同様に加速度センサ（２Ａ）と加速度センサ（２Ｂ）の差をとることにより得ることができる。この関係式を下記数式２６に示す。数式２６から明らかであるが、角速度に関しては、積分演算或いは微分演算のデータ処理工程が必要となる。しかしながら、上記の如く、関係式は必要とされるもの以上に存在するため、微分演算処理或いは積分演算処理を行ったとしても、高い精度を確保可能である。
【００６５】
【数２６】
【００６６】
図６は、図３に示す測定対象体（Ｐ）の回転傾斜動作に対して、更に、測定対象体（Ｐ）の軸（加速度センサ（２Ａ，２Ｂ）を結ぶ線）周りに回転する動作を加えた状態を示す。
図３乃至図５に関連して説明した実施形態においては、まず全体座標系のＹ軸周りの回転傾斜動作を考え、その後、全体座標系のＺ軸周りの回転傾斜動作を考えたが、図６に示す測定対象体（Ｐ）の運動に対しては、別の手法により、角度算出を行なう必要がある。
【００６７】
図７は、図６に示す測定対象体（Ｐ）の位置変化について説明する図である。尚、全体座標系の定義、瞬時静止座標系の定義並びに測定対象体（Ｐ）の初期位置に関する定義は、図３に関連して説明したものと同様である。
図６に示す測定対象体（Ｐ）の位置は、初期位置からＹ軸周りに角度αだけ測定対象体（Ｐ）を回転させ（図７（ａ）参照）、その後、測定対象体（Ｐ）をＹ軸周りに角度γだけ回転させた位置に一致するものとする（図７（ｂ）参照）。
【００６８】
図８は、測定対象体（Ｐ）の軸周りに測定対象体（Ｐ）が回転した状態を示す。図８（ａ）は、初期位置の測定対象体（Ｐ）の横断面を示し、図８（ｂ）は、図６に示す測定対象体（Ｐ）の位置における測定対象体（Ｐ）の横断面を示す。
図８から明らかな如く、ここでは、図６に示す位置に測定対象体（Ｐ）が移動する間に、測定対象体（Ｐ）はその軸周りに角度βだけ回転したものとする。
【００６９】
まず、この実施形態においても、上記数式２０に示される関係式は成立する。
次に、図３に関連した説明したものと同様に、加速度を表す全体座標上の点Ｇ（０，０，ｇ）の座標変換を考える。尚、点Ｇは、図３に関連して説明したものと同様に重力加速度を意味する点である。
ここで、全体座標系のＹ軸周りに、Ｘ軸からＺ軸方向に向けて点Ｇ（０，０，ｇ）を回転変換させた場合を考える。この座標変換後の座標（ｘｇ１，ｙｇ１，ｚｇ１）との間の関係を、下記数式２７に示す。
【００７０】
【数２７】
【００７１】
上記数式２０で示される加速度センサ（２）が出力する加速度の各成分に、数式２７で示す座標変換後の重力加速度成分を加える。この変換後の重力加速度成分の加算式を下記数式２８に示す。
【００７２】
【数２８】
【００７３】
次に、測定対象体（Ｐ）の軸周りの回転について考える。測定対象体（Ｐ）の軸が、瞬時静止座標系のｘ軸と一致するように瞬間静止座標系が定義されているから、測定対象体（Ｐ）の軸周りにｚ軸からｙ軸に向けて角度θだけ回転させたとすると、上記数式２８は、下記数式２９に変換されることとなる。
【００７４】
【数２９】
【００７５】
回転中心点（Ｒ）から距離ｒａの位置に存する加速度センサ（２Ａ）の瞬時静止座標系の各成分を（ａｘ，ａｙ，ａｚ）で表し、回転中心点（Ｒ）から距離ｒｂの位置に存する加速度センサ（２Ｂ）の瞬時静止座標系の各成分を（ｂｘ，ｂｙ，ｂｚ）で表すと、以下の数式３０乃至数式３２に示す関係式を得ることができる。尚、下記数式３０は、瞬時静止座標系のｘ軸方向の加速度成分を示し、下記数式３１は、瞬時静止座標系のｙ軸方向の加速度成分を示し、下記数式３２は、瞬時静止座標系のｚ軸方向の加速度成分を示す。
【００７６】
【数３０】
【００７７】
【数３１】
【００７８】
【数３２】
【００７９】
数式３０乃至数式３２に示す一対の関係式に対して、上記した手順と同様の下記手順により、角速度成分或いは角加速度成分を除去する。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式３３に示す。
【００８０】
【数３３】
【００８１】
上記数式３３から明らかな如く、瞬時静止座標系ｘ軸に係るデータから、全体座標系Ｙ軸周りの回転傾斜角度の正弦関数値を算出できる。また、この正弦関数値に基づき、余弦関数値も既知のものとなる。したがって、この既知の余弦関数値と、瞬時静止座標系ｘ軸周りの回転角度βを算出することができる。
したがって、全体座標系Ｙ軸周りの回転傾斜角度並びに瞬時静止座標系ｘ軸周りの回転傾斜角度の算出は、積分演算処理を要せず、代数演算のみで実行可能である。
【００８２】
続いて、瞬時静止座標系ｚ軸周りの回転を考え、その後、これを全体座標系（Ｘ，Ｙ，Ｚ）上に座標変換し、全体座標系Ｚ軸周りの回転傾斜角度を算出する。
まず、加速度センサ（２Ａ，２Ｂ）それぞれから得られた加速度成分の差を算出する。この演算工程を下記数式３４に示す。
【００８３】
【数３４】
【００８４】
上記数式３４に示すｙ軸成分とｚ軸成分に係る関係式を連立させることにより、瞬時静止座標系のｙ軸周りの角加速度及びｚ軸周りの角加速度を算出することができる。
下記数式３５に算出された瞬時静止座標系のｙ軸周りの角加速度及びｚ軸周りの角加速度を全体座標系（Ｘ，Ｙ，Ｚ）上に変換する行列式を示す。尚、この変換において、Ｘ軸からＺ軸に向かう方向を正としている。また、瞬時静止座標系ｘ軸に沿う加速度成分は「０」である。
【００８５】
【数３５】
【００８６】
上記数式３５に基づき、全体座標系（Ｘ，Ｙ，Ｚ）の角加速度成分を得ることができ、これらを積分演算することにより、角速度成分を得ることができる。この積分演算により得られる角速度成分の関係式を下記数式３６に示す。
【００８７】
【数３６】
【００８８】
上記数式３４のうち瞬時静止座標系ｘ軸方向の加速度出力の関係式は、瞬時静止座標系ｙ軸周り及びｚ軸周りの回転に係る加速度成分の関数であるから、上記数式３６を利用して、全体座標系のＹ軸周りの回転速度とＺ軸周りの回転速度の関係式を得ることができる。この関係式を利用して、全体座標系Ｚ軸周りの角速度を積分することにより、積分誤差を抑制しつつ、全体座標系Ｚ軸周りの回転傾斜角度γを算出することができる。
【００８９】
図９は、図６乃至図８に関連して説明した傾斜角度推定システム（１）の演算方式の応用例を示す。図９に示す例において、２つの測定対象体（Ｐ１，Ｐ２）が１つの回転中心点（Ｒ）において連結している。また、２つの測定対象体（Ｐ１，Ｐ２）は、それぞれ無関係に回転中心点（Ｒ）を軸として、回転傾斜動作をしている。
測定対象体（Ｐ１）には２つの加速度センサ（２Ａ１，２Ｂ１）が取り付けられ、測定対象体（Ｐ２）には、２つの加速度センサ（２Ａ２，２Ｂ２）が取り付けられている。回転中心点（Ｒ）から加速度センサ（２Ａ１）までの距離はｒａ１で表され、回転中心点（Ｒ）から加速度センサ（２Ｂ１）までの距離はｒｂ１で表され、回転中心点（Ｒ）から加速度センサ（２Ａ２）までの距離はｒａ２で表され、回転中心点（Ｒ）から加速度センサ（２Ｂ２）までの距離はｒａ２で表される。
【００９０】
各加速度センサ（２Ａ１，２Ｂ１，２Ａ２，２Ｂ２）は、測定対象体（Ｐ１，Ｐ２）の軸方向の加速度、測定対象体（Ｐ１，Ｐ２）の軸に対して直交する方向の加速度及び測定対象体（Ｐ１，Ｐ２）の軸周りの回転に係る加速度を測定可能である。これにより、測定対象体（Ｐ１，Ｐ２）それぞれの加速度測定点における３成分からなる加速度ベクトルを得ることができる。各測定点の加速度ベクトルが加速度センサ（２Ａ１，２Ｂ１，２Ａ２，２Ｂ２）から求めることができることから、測定対象体（Ｐ１，Ｐ２）の回転中心点（Ｒ）における加速度ベクトルを算出することができる。
以下に、回転中心点（Ｒ）における加速度ベクトルの算出式を示す。下記数式３７は、測定対象体（Ｐ１）に対するものであり、下記数式３８は測定対象体（Ｐ２）に対するものである。
【００９１】
【数３７】
【００９２】
【数３８】
【００９３】
上記数式３７に示される測定対象体（Ｐ１）の回転中心点（Ｒ）における加速度ベクトルを全体座標系（Ｘ，Ｙ，Ｚ）のＹ軸周りに角度αだけ回転させ、Ｚ軸周りに角度γだけ回転させ、測定対象体（Ｐ１）軸周りに角度θだけ回転させたとき（図７及び図８参照）、他方の測定対象体（Ｐ２）の加速度ベクトルに一致する場合を考える。このような角度（α，β，γ）を数値演算により求めることにより、測定対象体（Ｐ１）に対する測定対象体（Ｐ２）の相対的角度位置を算出することができる。
【００９４】
図１０は、互いに直交するように配された３つの測定対象体（Ｐ１，Ｐ２，Ｐ３）を考えた場合の概念図である。３つの測定対象体（Ｐ１，Ｐ２，Ｐ３）は、互いに一点で交わり、この１つの交点を回転中心点（Ｒ）として回転傾斜運動をする。回転中心点（Ｒ）には、各測定対象体（Ｐ１，Ｐ２，Ｐ３）の軸方向の加速度成分を測定する加速度センサ（２）が取り付けられている。また、各測定対象体（Ｐ１，Ｐ２，Ｐ３）上には更にそれぞれ１つずつ加速度センサが取り付けられている。
測定対象体（Ｐ１，Ｐ２，Ｐ３）のうち１つをｘ方向と定めると、他の測定対象体（Ｐ１，Ｐ２，Ｐ３）の軸は、自ずからｙ方向及びｚ方向として定義されることとなる。この結果、下記数式３９に示す関係式を得ることができる。
【００９５】
【数３９】
【００９６】
上記数式３９の各軸方向成分に係る関係式を連立させることにより、３つの角速度の絶対値を算出することができる。３つの軸のうち２つの方向に関しては、角速度が計測により求められているので、符号が判明する。他のもう１つの軸については、角速度が計測により判明する軸と平行な方向の加速度を同時に測定することにより、当該他のもう１つの軸の加速度の符号も判明する。
【産業上の利用可能性】
【００９７】
本発明は、人間の歩行動作解析研究、人間のバランス機能の評価、ロボットの移動動作解析研究、ゴルフなどのスポーツのトレーニングや医療福祉機器の開発に好適に利用可能である。
【符号の説明】
【００９８】
１・・・・・傾斜角度推定システム
２・・・・・加速度センサ
３・・・・・演算処理ユニット
【技術分野】
【０００１】
本発明は、回転運動する物体の姿勢或いは速度状態を推定するシステムに関する。より詳しくは、安価に構築可能であり、積分誤差を含まない或いは積分誤差が最小限化された推定値を算出可能とする傾斜角度推定システムに関する。
【背景技術】
【０００２】
高齢化社会の到来にともなって、人間の身体運動を評価するシステムに対する需要は増大しつつある。例えば、長期にわたって入院した高齢者が退院するにあたって、日常生活に差し障りのない程度に足腰の筋力の回復が図られているか否かを見極めることは非常に重要であり、十分な筋力の回復がなされる前に退院することにより、その後、他の身体部位を痛め、再度、入院をするといった問題も現存する。
医療関連分野以外においても、人間の身体運動を評価するシステムが用いられている。代表的なものとして、スポーツ科学の分野である。スポーツ選手の身体運動特性を評価、分析を行い、そのデータの蓄積を行なうことにより、スポーツ分野における成績向上を図る試みが数多くなされている。
運動評価システムの人間以外への適用例として、ロボット、特に、人型ロボットへの応用を挙げることができる。階段や坂道を歩行している間のロボットの傾斜角度などを測定することにより、この傾斜角度をロボットの制御部にフィードバックし、ロボットが転倒しないようにロボットの姿勢を制御するなどといったことは一般的に既に行なわれている。
【０００３】
近年、このような運動評価システムの開発分野においては、従来、測定対象体の動作を分析する際に、多数の色付マーカをとりつけ、複数のカメラで撮像するという手段が採られていた（例えば、特許文献１参照）。
複数のカメラによる撮像データに基づき、測定対象体の動作を分析する手法の大きな欠点として、場所的な制約を受けること並びにシステムの構築に多大な費用を要することを挙げることができる。例えば、特許文献１において言及されているが、このようなシステムは、色差に基づきデータ分析を行なうが、時々刻々と変化する周囲の光量の影響を受けやすいという問題があり、多くの場合、強い光を測定対象体に当てるといった手法が採られている。したがって、広い空間或いは屋外空間における動作データの取得は極めて困難である。また、複数のカメラ並びに画像解析システムを必要とし、システム構築を廉価に行なうことは不可能である。
【０００４】
画像解析によらず、測定対象体の動作の分析を可能とする技術も、近年、開発されてきている。この画像解析を用いないシステムは、一般に、ジャイロセンサ、加速度センサ及び地磁気センサを組み合わせて構築される（例えば、特許文献２参照）。このようなシステムのほとんどは、ジャイロセンサから得られた３軸の角速度に対して、非線形積分を施して、角度を算出し、この算出された角度に対して、加速度計や地磁気センサからの出力を用いて補正を施すものである。
この複数種のセンサを用いた方法は、高価な撮像装置や画像解析システムを用いないため、上記した技術よりは、廉価にシステム構築を行なうことができる。また、周囲の光量の変動に影響を受けるといった問題点を解消できる点で優れている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平６−５０９８３号公報
【特許文献２】特開２００７−７５９６７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記した複数種のセンサを用いる技術は、画像解析装置を利用する従来の手法と比べて多くの利点を有するものではあるが、いくつかの問題点がある。
まず、ジャイロセンサから得られる角速度データに対して非線形積分を施すことにより、誤差が累積する点である。特に角速度にドリフトが存在すれば累積誤差が大きくなる。この誤差の累積の存在のため、他のセンサ（加速度センサ或いは地磁気センサ）からの出力により、補正処理を行なう必要が生ずる。結果として、複数種のセンサの利用が不可避であり、これに伴い、データ処理の煩雑さが増すこととなる。また、複数種のセンサの使用並びに煩雑なデータ処理工程は、測定対象体の動作（測定対象体の姿勢の傾斜角度）の推定値の精度の悪化を招く。高い精度の推定値を得ようとすれば、積分処理における分割数の増大等の対策をする必要を生じ、結果として、高価なデータ処理システムの使用が不可避なものとなる。
更には、ジャイロセンサは、画像処理システムと比べた場合には、廉価なものであるが、一般的な加速度センサと比べた場合には、高価である。また、地磁気センサにより補正処理を行なう場合には、地磁気センサの外乱因子に対する脆弱性故、傾斜角度推定システムの使用に対して、場所的な制約が生み出されることとなる。
【０００７】
本発明は上記実情を鑑みてなされたものであって、積分演算をデータ処理に含めることなく、或いは、積分演算処理工程を最小限しか行わず、回転運動する物体の姿勢或いは速度，加速度状態を推定するシステムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
請求項１記載の発明は、一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、π／２の奇数倍数値に対してπ／２を超えない所定の角度だけ増分した第１の上限値と、前記π／２の奇数倍数値に対して前記所定の角度だけ減分した第１の下限値の間で定義される第１の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第１の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記余弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システムである。
【０００９】
請求項２記載の発明は、一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を乗算する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、πの倍数値に対してπを超えない所定の角度だけ増分した第２の上限値と、前記πの倍数値に対して前記所定の角度だけ減分した第２の下限値の間で定義される第２の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第２の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記正弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システムである。
【発明の効果】
【００１０】
請求項１及び２記載の発明によれば、用いるセンサが加速度センサのみであるので、簡便且つ廉価に傾斜角度推定システムを構築可能となる。したがって、従来技術と比して、推定システムの設定を簡便に行なうことができる。
請求項１及び２記載の発明によれば、積分演算を行なうことなく、傾斜角度の推定を実行可能である。したがって、従来技術のように、積分誤差を含んだ推定値を出力することがなく、高い精度を確保可能である。
請求項１及び２記載の発明によれば、算出された傾斜角度の推定値に応じて、出力する推定値の算出に用いられる関数の選択が可能となる。
【図面の簡単な説明】
【００１１】
【図１】本発明に係る傾斜角度推定システムの概略的構成図である。
【図２】図１に示す傾斜角度推定システムの測定対象体の運動の座標系を説明する図である。
【図３】立体空間内で回転動作する測定対象体の運動の座標系を説明する図である。
【図４】立体空間内で回転動作する測定対象体の傾斜角度推定に用いられる座標変換の概念を説明する図である。
【図５】立体空間内で回転動作する測定対象体の傾斜角度推定に用いられる座標変換の概念を説明する図である。
【図６】軸周りに回転する測定対象体の運動の座標系を説明する図である。
【図７】軸周りに回転する測定対象体の運動の座標系を説明する図である。
【図８】軸周りに回転する測定対象体の運動の座標系を説明する図である。
【図９】本発明に係る相対角度推定システムにより相対角度を推定される測定対象体のモデル図である。
【図１０】本発明に係る角速度推定システムにより角速度を推定される測定対象体のモデル図である。
【発明を実施するための形態】
【００１２】
以下、本発明に係る傾斜角度推定システムについて、図を参照しつつ説明する。
図１は、本発明に係る傾斜角度推定システムの概略図である。
傾斜角度推定システム（１）は、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）と、加速度センサ（２）とデータ通信可能なデータ演算処理ユニット（コンピュータ）（３）からなる。
図１に示す例において、測定対象体（Ｐ）は、その一方の端部を回転可能に支持されている。測定対象体（Ｐ）は、この支持点を回転中心点（Ｒ）として、一方向に回転動作をし、その姿勢を傾斜させる。したがって、図１に示す例においては、測定対象体（Ｐ）は、一平面内で回転動作するものとなる。
加速度センサ（２）が取付けられる測定対象体（Ｐ）に対して特段の制限はないが、測定対象体の回転傾斜動作の際に生ずる慣性力により変形しない程度の剛性を有することが好ましい。傾斜角度推定のためのデータ取得時における測定対象体（Ｐ）の変形は、推定値の誤差に直結するためである。したがって、本発明が対象とする測定対象体（Ｐ）には、完全な剛体のみならず、傾斜角度推定の目的が許容する誤差を生じさせる程度に変形可能なものも含まれる。
【００１３】
図１において、測定対象体（Ｐ）上に２つの加速度センサ（２）が示されているが、加速度センサの数は、２つに限定されるものではなく、３或いはそれ以上であってもよい。
図１から明らかなように、２つの加速度センサ（２）を結ぶ線の延長線上に測定対象体（Ｐ）の回転中心点（Ｒ）が位置する。尚、本明細書或いは請求の範囲で、「加速度センサ（２）を結ぶ線が回転中心点（Ｒ）を通過する」との文言が用いられるが、この文言は、必ずしも加速度センサ（２）同士を結ぶ線が正確に回転中心点（Ｒ）を通過することのみを意味するものではなく、加速度センサ（２）同士を結ぶ線が、回転中心点（Ｒ）近傍を通過することも意味する。加速度センサ（２）同士を結ぶ線と回転中心点（Ｒ）との距離は、直接的に、傾斜角度推定システム（１）から得られる推定値の誤差に直結するが、傾斜角度推定システム（１）が許容する誤差範囲内の誤差を生み出す程度の加速度センサ（２）同士を結ぶ線と回転中心点（Ｒ）との間の距離であれば、本発明の技術的範囲に属するものと解するべきである。
【００１４】
データ演算処理ユニット（３）には、回転中心点（Ｒ）から各加速度センサ（２）までの距離が予め入力されている。
データ演算処理ユニット（３）は、加速度センサ（２）からの出力値（加速度データ）を受信し、データ演算処理ユニット（３）への入力値（回転中心点（Ｒ）から各加速度センサ（２）までの距離）と、加速度センサ（２）からの出力値（加速度データ）を用いて、測定対象体（Ｐ）の傾斜角度を算出する。
【００１５】
図２は、データ演算処理ユニット（３）の演算処理の概念図である。
図２において、２つの座標系が示されている。本明細書において、一方の座標系を全体座標系と称し、他方の座標系を瞬時静止座標系と称する。
全体座標系において、重力加速度ベクトルに平行な方向をＺ軸として定義する。また、図１に示す測定対象体（Ｐ）が回転動作をする平面内において、Ｚ軸に対して直交する方向をＸ軸とする。ここで、Ｘ軸とＺ軸の交点と、測定対象体（Ｐ）の回転中心点（Ｒ）が一致するように全体座標系を定義する。
瞬時静止座標系において、２つの加速度センサ（２）同士を結ぶ線をｘ軸として定義する。また、図１に示す測定対象体（Ｐ）が回転動作をする平面内において、ｘ軸に対して直交する方向をｚ軸とする。ここで、ｘ軸とｚ軸の交点と、測定対象体（Ｐ）の回転中心点（Ｒ）が一致するように瞬時静止座標系を定義する。
図２に示す例において、全体座標系のＸ軸と瞬時静止座標系のｘ軸との間の挟角は記号「α」で示されている。
以上の説明並びに座標系の定義から明らかであるが、図２は、測定対象体（Ｐ）がその軸（加速度センサ（２）同士を結んだ線）を全体座標系のＸ軸に置いた位置から角度αだけ回転運動した瞬間の測定対象体（Ｐ）の状態を表している。
以下に、この測定対象体（Ｐ）の傾斜角度αを算出するための演算処理工程を示す。
【００１６】
測定対象体（Ｐ）に取付けられた２つの加速度センサ（２）はそれぞれ、瞬時静止座標系ｘ軸に平行な方向の加速度データ（演算記号「ax又はbx」で表される）と、瞬時静止座標系ｚ軸に平行な方向の加速度データ（演算記号「az又はbz」で表される）を出力する。
測定対象体（Ｐ）の回転傾斜動作によって生ずる瞬時静止座標系ｘ軸に平行な加速度成分は、以下の数式１から求めることができる。また、測定対象体（Ｐ）の回転傾斜動作によって生ずる瞬時静止座標系ｚ軸に平行な加速度成分は、以下の数式２から求めることができる。
尚、数式１中、「r」は、回転中心点（Ｒ）から加速度センサ（２）までの距離を意味する。また、数式１及び数式２において、測定対象体（Ｐ）の回転運動の角速度成分、角加速度成分を演算記号「θ」の微分値として表現している。
【００１７】
【数１】
【００１８】
【数２】
【００１９】
ここで、回転中心点（Ｒ）から離れた側に取付けられた加速度センサ（２）を、符号「２Ａ」で表し、回転中心点（Ｒ）に近い側に取付けられた加速度センサ（２）を、符号「２Ｂ」で表す。また、演算処理ユニット（３）には、回転中心点（Ｒ）と加速度センサ（２Ａ）の距離として、値「rA」が入力されているものとし、回転中心点（Ｒ）と加速度センサ（２Ｂ）の距離として、値「rB」が入力されているものとする。
加速度センサ（２）の出力は、慣性力の影響を受けることから、加速度センサの出力と、測定対象体（Ｐ）の回転動作に起因する加速度成分並びに重力加速度の関係は、下記の数式３乃至数式６によって表すことができる。尚、数式３及び数式４は、加速度センサ（２Ａ）に対するものであり、瞬時静止座標系ｘ軸に平行な方向の出力加速度データを演算記号「ax」で示し、瞬時静止座標系ｚ軸に平行な方向の出力加速度データを演算記号「az」で示している。数式５及び６は加速度センサ（２Ｂ）に対するものであり、瞬時静止座標系ｘ軸に平行な方向の出力加速度データを演算記号「bx」で示し、瞬時静止座標系ｚ軸に平行な方向の出力加速度データを演算記号「bz」で示している。
【００２０】
【数３】
【００２１】
【数４】
【００２２】
【数５】
【００２３】
【数６】
【００２４】
上記数式３と数式５の関係から、以下の手順で、測定対象体の角度αを求めることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式７に示す。
【００２５】
【数７】
【００２６】
上記数式４と数式６の関係から、以下の手順で、測定対象体の角度αを求めることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角加速度成分を除去する。
上記手順を経て得られる角度αに対する演算式を、下記数式８に示す。
【００２７】
【数８】
【００２８】
上記数式７及び数式８のいずれを用いても、角度αを算出することは可能である。しかしながら、数式７を用いた場合、角度αがπ／２の奇数倍の値の近傍である場合、計測誤差の影響が大きくなって現れる。また、数式８を用いた場合、角度αがπの整数倍の値の近傍である場合、計測誤差の影響が大きくなって現れる。
したがって、演算処理ユニット（３）に対する入力値として、閾値データを入力可能とすることが好ましい。例えば、演算処理ユニット（３）に対して、第１の閾値範囲として、９０°を僅かに下回る値（例えば、８５°）及び９０°を僅かに上回る値（例えば、９５°）を入力する。また、第２の閾値範囲として、０°を僅かに下回る値（例えば、−５°）及び０°を僅かに上回る値（例えば、５°）を入力する。
数式７及び／又は数式８による角度αの演算結果が、演算処理ユニット（３）に入力された第１の閾値範囲であるならば、数式８に基づいて算出された値を演算処理ユニット（３）が出力する。一方、数式７及び／又は数式８による角度αの演算結果が、演算処理ユニット（３）に入力された第２の閾値範囲であるならば、数式７に基づいて算出された値を演算処理ユニット（３）が出力する。
他の方法として、数式７及び数式８に基づく算出結果から、正接関数値を算出し、この正接関数値に基づき、角度αを算出してもよいし、数式７及び数式８に基づき得られた値を積算し、正弦関数の半角の公式に基づき、角度αを算出してもよい。
【００２９】
尚、数式７により、角度αに対する正弦関数値が定まるので、この正弦関数値を上記の数式３及び５に代入し、測定対象体（Ｐ）の回転傾斜運動の角速度を算出することが可能である。また、数式８により、角度αに対する余弦関数値が定まるので、この余弦関数値を上記の数式４及び６に代入し、測定対象体（Ｐ）の回転傾斜運動の角加速度を算出することが可能である。
このようにして求められる測定対象体（Ｐ）の回転傾斜運動の角速度及び角加速度の算出式を下記数式９及び数式１０に示す。
【００３０】
【数９】
【００３１】
【数１０】
【００３２】
このようにして、図１及び図２に示す実施形態においては、本発明に係る傾斜角度推定システム（１）では、積分演算工程を実行することなく、傾斜角度、傾斜角速度及び傾斜角加速度を算出することが可能である。したがって、従来のジャイロセンサを利用する方式と比して、傾斜角度推定値に積分誤差が含まれることがない。このようにして、本発明に係る傾斜角度推定システム（１）は、出力される推定値に対して、高い精度を確保可能となる。
【００３３】
尚、上記数式３及び数式４或いは数式５及び数式６から明らかであるが、回転中心点（Ｒ）から加速度センサ（２）までの距離を「０」とすれば、加速度センサ（２）からの出力値は、測定対象体（Ｐ）の回転動作に係る角速度成分或いは角加速度成分を含まないものとなる。したがって、加速度センサ（２）を回転中心点（Ｒ）に一致するように取付けることで、加速度センサ（２）の出力値を重力加速度で除算することにより、傾斜角度αの正弦関数値或いは余弦関数値を求めることが可能である。尚、この場合には、測定対象体（Ｐ）の回転動作に係る角速度成分或いは角加速度成分を得るためには、算出された角度αに対して時間微分処理を行う必要がある。
【００３４】
図１及び図２に示す例において、全体座標系（Ｘ，Ｚ）が静止している場合を考えたが、本発明は、全体座標系が加速度運動している場合にも適用可能である。ここでは、便宜的に、全体座標系の原点（ＸＯ，ＺＯ）を基準点として説明するが、この基準点は、２つの加速度センサ（２）のうち一方の位置に置き換えてもよい。全体座標系（Ｘ，Ｚ）が一平面内で加速度運動している場合、全体座標系における加速度（ＤＸ，ＤＹ）は、下記の数式１１及び数式１２によって、それぞれ表される。
【００３５】
【数１１】
【００３６】
【数１２】
【００３７】
これを瞬時静止座標系（ｘ，ｚ）に座標変換し、瞬時静止座標系（ｘ，ｚ）における基準点（ＸＯ，ＺＯ）の加速度成分（Ｄｘ，Ｄｚ）を得ることができる。座標変換に係る演算を、下記数式１３に示す。
【００３８】
【数１３】
【００３９】
上記数式３乃至数式６で示すものと同様に、加速度センサ（２）からの出力（ax,az,bx,bz）を全体座標系が移動している場合について表すと、下記数式１４を得ることができる。
【００４０】
【数１４】
【００４１】
基準点（ＸＯ，ＺＯ）の両方向の加速度成分が既知の値である場合、図１及び図２に関連して説明した手順と同様に、下記手順を実行することにより、以下の数式１５及び数式１６に示す関係を得ることができる。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
【００４２】
【数１５】
【００４３】
【数１６】
【００４４】
数式１５及び数式１６は、一変数の関数であるから、いずれか一方の式から傾斜角度αを算出することもできるし、数式１５及び数式１６を連立させて、正弦関数値或いは余弦関数値を算出してもよい。
また、図１及び図２に関連して説明したものと同様の手法を用いて、測定対象体（Ｐ）の回転運動の角速度及び角加速度を積分演算工程なしに算出することが可能である。
【００４５】
基準点（ＸＯ，ＺＯ）の加速度成分のうち一方が未知である場合においても、上記数式１５及び数式１６が、同一未知パラメータに係る２式をなすので、測定対象体（Ｐ）の傾斜角度α並びに基準点（ＸＯ，ＺＯ）の未知の加速度成分を、数式１５及び数式１６を連立させることにより算出することができる。特に、対象となる測定対象体（Ｐ）が一方向しか直線移動をしない場合に、この関係は非常に有利である。特に、人間の歩行動作を対象とするとき、多くの場合、鉛直方向の移動は存在せず、専ら、水平方向の移動が取扱われる。したがって、この場合、Ｚ方向の加速度成分を既知の値「０」として取扱うことができ、上記数式１５及び数式１６を連立して得られる下記数式１７によって、測定対象体（Ｐ）の傾斜角度を算出することができる。
【００４６】
【数１７】
【００４７】
図３は、測定対象体（Ｐ）が３次元空間内で回転傾斜動作する態様を示す。
図１及び図２に関連して説明した実施形態においては、測定対象体（Ｐ）が２次元平面内を移動していたが、本発明の傾斜角度推定システム（１）は、３次元空間内で回転傾斜運動をする測定対象体（Ｐ）に対しても適用可能である。
図３において、初期位置にある測定対象体（Ｐ）は点線で示されており、回転傾斜動作を開始した後の測定対象体（Ｐ）は実線で示されている。３次元空間の全体座標系（Ｘ，Ｙ，Ｚ）のＺ軸は、重力加速度に対して平行な方向に設定される。また、測定対象体（Ｐ）の初期位置は、本発明の理解を容易にするために、測定対象体（Ｐ）の軸、即ち、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）同士を結ぶ線がＺ軸と直交するように配されているものとする。また、測定対象体（Ｐ）に取付けられた複数の加速度センサ（２）同士を結ぶ線に一致するようにＸ軸が定められる。また、Ｙ軸は、Ｘ軸とＺ軸の交点を通過するとともにＸ軸及びＺ軸に直交する軸として定義される。
測定対象体（Ｐ）の回転中心点（Ｒ）は、Ｘ軸、Ｙ軸及びＺ軸の交点と一致するものとする。
【００４８】
次に、瞬時静止座標系の定義について説明する。瞬時静止座標系において、回転傾斜動作開始後の測定対象体（Ｐ）の軸をｘ軸として定義する。したがって、瞬時静止座標系（ｘ，ｙ，ｚ）は、全体座標系（Ｘ，Ｙ，Ｚ）をＹ軸周りに所定角度回転させ、その後、Ｚ軸周りに所定角度回転させた座標ということができる。
【００４９】
図４及び図５は、全体座標系（Ｘ，Ｙ，Ｚ）から瞬時静止座標系（ｘ，ｙ，ｚ）への座標変換を説明する図であり、図４は、Ｙ軸周りの回転をさせたときの状態を示し、図５は、図４に示す状態からＺ軸周りの回転をさせたときの状態を示す。
座標変換の一例として、全体座標（Ｘ，Ｙ，Ｚ）上の点Ｇ（０，０，ｇ）を考える。この点Ｇは、全体座標が加速度を表す座標であるとき、重力加速度を意味する点である。図４は、この点ＧをＹ軸周りに角度αγだけＸ軸に向けて回転させた状態を示す。この回転動作の後の点ＧＹの座標（ｘｇ１，ｙｇ１，ｚｇ１）は、下記の数式１８により表される。
【００５０】
【数１８】
【００５１】
図５は、図４に示す点ＧＹをｚ軸周りに角度αｚだけＹ軸に向けて回転させた状態を示す。この回転動作の後の点ＧＹＺの座標（ｘｇ，ｙｇ，ｚｇ）は、下記の数式１９により表される。
【００５２】
【数１９】
【００５３】
図３乃至図５に関連する説明により、３次元空間における全体座標系と瞬時静止座標系の関係が明らかとなった。次に、瞬時静止座標系における測定対象体（Ｐ）上の点の加速度成分について考える。図１及び図２に関連してなされた説明と同様に、測定対象体（Ｐ）上に取付けられた２つの加速度センサ（２Ａ，２Ｂ）が検知する加速度について考える。
回転動作後の測定対象体（Ｐ）上の任意の位置（ｘ，ｙ，ｚ）の加速度成分は、下記数式２０により表される。尚、瞬時静止座標系のｘ軸に沿う方向の加速度成分は、瞬時静止座標系のｙ軸周りの角速度とｚ軸周りの角速度の関数で表され、瞬時静止座標系のｙ軸に沿う方向の加速度成分は、瞬時静止座標系のｙ軸周りの角加速度の関数で表され、瞬時静止座標系のｚ軸に沿う方向の加速度成分は、瞬時静止座標系のｚ軸周りの角加速度の関数で表される。
【００５４】
【数２０】
【００５５】
図１及び図２で説明したように、加速度センサ（２）が出力する加速度は、測定対象体（Ｐ）の回転傾斜動作に起因する加速度成分と重力加速度（慣性力）に起因する加速度成分を含むので、加速度センサ（２）が出力する加速度は、下記数式２１及び数式２２により表される。尚、数式２１は、加速度センサ（２）のうち、回転中心点（Ｒ）から離れた位置（回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離ｒＡ）に配される加速度センサ（２Ａ）に対するものであり、加速度センサ（２Ａ）の瞬間静止座標中の位置座標を（ｘＡ，ｙＡ，ｚＡ）で表し、この位置座標に対応する加速度成分を（ａｘ，ａｙ，ａｚ）で表している。また、数式２２は、加速度センサ（２）のうち、回転中心点（Ｒ）から近い位置（回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離ｒＢ）に配される加速度センサ（２Ｂ）に対するものであり、加速度センサ（２Ｂ）の瞬間静止座標中の位置座標を（ｘＢ，ｙＢ，ｚＢ）で表し、この位置座標に対応する加速度成分を（ｂｘ，ｂｙ，ｂｚ）で表している。
【００５６】
【数２１】
【００５７】
【数２２】
【００５８】
傾斜角度αｙ及びαｚの算出は、図１及び図２に関連したものと同様の下記の手順により実行可能である。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式２３に示す。
【００５９】
【数２３】
【００６０】
上記数式２３から明らかであるが、数式２３に示す関係式から傾斜角度αｙ及びαｚを算出する方法は多数存在する。例えば、瞬時静止座標系ｚ軸方向の加速度成分のデータから、Ｙ軸周りの傾斜角αｙに関する余弦関数値を算出することができる。このＹ軸周りの傾斜角αｙに関する余弦関数値に基づき、Ｙ軸周りの傾斜角αｙに関する正弦関数値の算出も可能である。算出されたＹ軸周りの傾斜角αｙに関する正弦関数値を、瞬時静止座標系ｘ軸方向或いはｙ軸方向の加速度成分の上記関係式に代入することにより、Ｚ軸周りの傾斜角αｚの余弦関数値或いは正弦関数値を算出することができる。
このようにして、３次元空間内を回転する測定対象体（Ｐ）に対しても、積分演算工程を行なうことなく、傾斜角の算出を実行可能である。
【００６１】
３次元空間内の測定対象体（Ｐ）の回転傾斜動作の角加速度も、積分演算工程を行うことなしに算出可能である。
上記数式２１及び数式２２を参照する。ここで、加速度センサ（２Ａ）が出力する瞬時静止座標系のｙ軸方向の加速度成分と、加速度センサ（２Ｂ）が出力する瞬時静止座標系のｙ軸方向の加速度成分の差をとることにより、全体座標系Ｚ軸周りの角加速度を算出することができる。下記数式２４に、全体座標系Ｚ軸周りの角加速度と加速度センサ（２）の出力値との関係を示す。
同様に、加速度センサ（２Ａ）が出力する瞬時静止座標系のｚ軸方向の加速度成分と、加速度センサ（２Ｂ）が出力する瞬時静止座標系のｚ軸方向の加速度成分の差をとることにより、全体座標系Ｙ軸周りの角加速度を算出することができる。下記、数式２５に、全体座標系Ｙ軸周りの角加速度と加速度センサ（２）の出力値との関係を示す。
【００６２】
【数２４】
【００６３】
【数２５】
【００６４】
尚、角速度成分と加速度センサ（２）の出力値との関係を、加速度センサ（２）の瞬時静止座標系ｘ軸方向の加速度に関する出力値に基づき、上記と同様に加速度センサ（２Ａ）と加速度センサ（２Ｂ）の差をとることにより得ることができる。この関係式を下記数式２６に示す。数式２６から明らかであるが、角速度に関しては、積分演算或いは微分演算のデータ処理工程が必要となる。しかしながら、上記の如く、関係式は必要とされるもの以上に存在するため、微分演算処理或いは積分演算処理を行ったとしても、高い精度を確保可能である。
【００６５】
【数２６】
【００６６】
図６は、図３に示す測定対象体（Ｐ）の回転傾斜動作に対して、更に、測定対象体（Ｐ）の軸（加速度センサ（２Ａ，２Ｂ）を結ぶ線）周りに回転する動作を加えた状態を示す。
図３乃至図５に関連して説明した実施形態においては、まず全体座標系のＹ軸周りの回転傾斜動作を考え、その後、全体座標系のＺ軸周りの回転傾斜動作を考えたが、図６に示す測定対象体（Ｐ）の運動に対しては、別の手法により、角度算出を行なう必要がある。
【００６７】
図７は、図６に示す測定対象体（Ｐ）の位置変化について説明する図である。尚、全体座標系の定義、瞬時静止座標系の定義並びに測定対象体（Ｐ）の初期位置に関する定義は、図３に関連して説明したものと同様である。
図６に示す測定対象体（Ｐ）の位置は、初期位置からＹ軸周りに角度αだけ測定対象体（Ｐ）を回転させ（図７（ａ）参照）、その後、測定対象体（Ｐ）をＹ軸周りに角度γだけ回転させた位置に一致するものとする（図７（ｂ）参照）。
【００６８】
図８は、測定対象体（Ｐ）の軸周りに測定対象体（Ｐ）が回転した状態を示す。図８（ａ）は、初期位置の測定対象体（Ｐ）の横断面を示し、図８（ｂ）は、図６に示す測定対象体（Ｐ）の位置における測定対象体（Ｐ）の横断面を示す。
図８から明らかな如く、ここでは、図６に示す位置に測定対象体（Ｐ）が移動する間に、測定対象体（Ｐ）はその軸周りに角度βだけ回転したものとする。
【００６９】
まず、この実施形態においても、上記数式２０に示される関係式は成立する。
次に、図３に関連した説明したものと同様に、加速度を表す全体座標上の点Ｇ（０，０，ｇ）の座標変換を考える。尚、点Ｇは、図３に関連して説明したものと同様に重力加速度を意味する点である。
ここで、全体座標系のＹ軸周りに、Ｘ軸からＺ軸方向に向けて点Ｇ（０，０，ｇ）を回転変換させた場合を考える。この座標変換後の座標（ｘｇ１，ｙｇ１，ｚｇ１）との間の関係を、下記数式２７に示す。
【００７０】
【数２７】
【００７１】
上記数式２０で示される加速度センサ（２）が出力する加速度の各成分に、数式２７で示す座標変換後の重力加速度成分を加える。この変換後の重力加速度成分の加算式を下記数式２８に示す。
【００７２】
【数２８】
【００７３】
次に、測定対象体（Ｐ）の軸周りの回転について考える。測定対象体（Ｐ）の軸が、瞬時静止座標系のｘ軸と一致するように瞬間静止座標系が定義されているから、測定対象体（Ｐ）の軸周りにｚ軸からｙ軸に向けて角度θだけ回転させたとすると、上記数式２８は、下記数式２９に変換されることとなる。
【００７４】
【数２９】
【００７５】
回転中心点（Ｒ）から距離ｒａの位置に存する加速度センサ（２Ａ）の瞬時静止座標系の各成分を（ａｘ，ａｙ，ａｚ）で表し、回転中心点（Ｒ）から距離ｒｂの位置に存する加速度センサ（２Ｂ）の瞬時静止座標系の各成分を（ｂｘ，ｂｙ，ｂｚ）で表すと、以下の数式３０乃至数式３２に示す関係式を得ることができる。尚、下記数式３０は、瞬時静止座標系のｘ軸方向の加速度成分を示し、下記数式３１は、瞬時静止座標系のｙ軸方向の加速度成分を示し、下記数式３２は、瞬時静止座標系のｚ軸方向の加速度成分を示す。
【００７６】
【数３０】
【００７７】
【数３１】
【００７８】
【数３２】
【００７９】
数式３０乃至数式３２に示す一対の関係式に対して、上記した手順と同様の下記手順により、角速度成分或いは角加速度成分を除去する。
（１）加速度センサ（２Ａ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ｂ）までの距離を積算する。
（２）加速度センサ（２Ｂ）の出力値に対して、回転中心点（Ｒ）から加速度センサ（２Ａ）までの距離を積算する。
（３）上記手順（１）及び（２）から得られた積算値の差分を取り、測定対象体（Ｐ）の角速度成分或いは角加速度成分を除去する。
この手順にしたがって得られる一組の関係式を下記数式３３に示す。
【００８０】
【数３３】
【００８１】
上記数式３３から明らかな如く、瞬時静止座標系ｘ軸に係るデータから、全体座標系Ｙ軸周りの回転傾斜角度の正弦関数値を算出できる。また、この正弦関数値に基づき、余弦関数値も既知のものとなる。したがって、この既知の余弦関数値と、瞬時静止座標系ｘ軸周りの回転角度βを算出することができる。
したがって、全体座標系Ｙ軸周りの回転傾斜角度並びに瞬時静止座標系ｘ軸周りの回転傾斜角度の算出は、積分演算処理を要せず、代数演算のみで実行可能である。
【００８２】
続いて、瞬時静止座標系ｚ軸周りの回転を考え、その後、これを全体座標系（Ｘ，Ｙ，Ｚ）上に座標変換し、全体座標系Ｚ軸周りの回転傾斜角度を算出する。
まず、加速度センサ（２Ａ，２Ｂ）それぞれから得られた加速度成分の差を算出する。この演算工程を下記数式３４に示す。
【００８３】
【数３４】
【００８４】
上記数式３４に示すｙ軸成分とｚ軸成分に係る関係式を連立させることにより、瞬時静止座標系のｙ軸周りの角加速度及びｚ軸周りの角加速度を算出することができる。
下記数式３５に算出された瞬時静止座標系のｙ軸周りの角加速度及びｚ軸周りの角加速度を全体座標系（Ｘ，Ｙ，Ｚ）上に変換する行列式を示す。尚、この変換において、Ｘ軸からＺ軸に向かう方向を正としている。また、瞬時静止座標系ｘ軸に沿う加速度成分は「０」である。
【００８５】
【数３５】
【００８６】
上記数式３５に基づき、全体座標系（Ｘ，Ｙ，Ｚ）の角加速度成分を得ることができ、これらを積分演算することにより、角速度成分を得ることができる。この積分演算により得られる角速度成分の関係式を下記数式３６に示す。
【００８７】
【数３６】
【００８８】
上記数式３４のうち瞬時静止座標系ｘ軸方向の加速度出力の関係式は、瞬時静止座標系ｙ軸周り及びｚ軸周りの回転に係る加速度成分の関数であるから、上記数式３６を利用して、全体座標系のＹ軸周りの回転速度とＺ軸周りの回転速度の関係式を得ることができる。この関係式を利用して、全体座標系Ｚ軸周りの角速度を積分することにより、積分誤差を抑制しつつ、全体座標系Ｚ軸周りの回転傾斜角度γを算出することができる。
【００８９】
図９は、図６乃至図８に関連して説明した傾斜角度推定システム（１）の演算方式の応用例を示す。図９に示す例において、２つの測定対象体（Ｐ１，Ｐ２）が１つの回転中心点（Ｒ）において連結している。また、２つの測定対象体（Ｐ１，Ｐ２）は、それぞれ無関係に回転中心点（Ｒ）を軸として、回転傾斜動作をしている。
測定対象体（Ｐ１）には２つの加速度センサ（２Ａ１，２Ｂ１）が取り付けられ、測定対象体（Ｐ２）には、２つの加速度センサ（２Ａ２，２Ｂ２）が取り付けられている。回転中心点（Ｒ）から加速度センサ（２Ａ１）までの距離はｒａ１で表され、回転中心点（Ｒ）から加速度センサ（２Ｂ１）までの距離はｒｂ１で表され、回転中心点（Ｒ）から加速度センサ（２Ａ２）までの距離はｒａ２で表され、回転中心点（Ｒ）から加速度センサ（２Ｂ２）までの距離はｒａ２で表される。
【００９０】
各加速度センサ（２Ａ１，２Ｂ１，２Ａ２，２Ｂ２）は、測定対象体（Ｐ１，Ｐ２）の軸方向の加速度、測定対象体（Ｐ１，Ｐ２）の軸に対して直交する方向の加速度及び測定対象体（Ｐ１，Ｐ２）の軸周りの回転に係る加速度を測定可能である。これにより、測定対象体（Ｐ１，Ｐ２）それぞれの加速度測定点における３成分からなる加速度ベクトルを得ることができる。各測定点の加速度ベクトルが加速度センサ（２Ａ１，２Ｂ１，２Ａ２，２Ｂ２）から求めることができることから、測定対象体（Ｐ１，Ｐ２）の回転中心点（Ｒ）における加速度ベクトルを算出することができる。
以下に、回転中心点（Ｒ）における加速度ベクトルの算出式を示す。下記数式３７は、測定対象体（Ｐ１）に対するものであり、下記数式３８は測定対象体（Ｐ２）に対するものである。
【００９１】
【数３７】
【００９２】
【数３８】
【００９３】
上記数式３７に示される測定対象体（Ｐ１）の回転中心点（Ｒ）における加速度ベクトルを全体座標系（Ｘ，Ｙ，Ｚ）のＹ軸周りに角度αだけ回転させ、Ｚ軸周りに角度γだけ回転させ、測定対象体（Ｐ１）軸周りに角度θだけ回転させたとき（図７及び図８参照）、他方の測定対象体（Ｐ２）の加速度ベクトルに一致する場合を考える。このような角度（α，β，γ）を数値演算により求めることにより、測定対象体（Ｐ１）に対する測定対象体（Ｐ２）の相対的角度位置を算出することができる。
【００９４】
図１０は、互いに直交するように配された３つの測定対象体（Ｐ１，Ｐ２，Ｐ３）を考えた場合の概念図である。３つの測定対象体（Ｐ１，Ｐ２，Ｐ３）は、互いに一点で交わり、この１つの交点を回転中心点（Ｒ）として回転傾斜運動をする。回転中心点（Ｒ）には、各測定対象体（Ｐ１，Ｐ２，Ｐ３）の軸方向の加速度成分を測定する加速度センサ（２）が取り付けられている。また、各測定対象体（Ｐ１，Ｐ２，Ｐ３）上には更にそれぞれ１つずつ加速度センサが取り付けられている。
測定対象体（Ｐ１，Ｐ２，Ｐ３）のうち１つをｘ方向と定めると、他の測定対象体（Ｐ１，Ｐ２，Ｐ３）の軸は、自ずからｙ方向及びｚ方向として定義されることとなる。この結果、下記数式３９に示す関係式を得ることができる。
【００９５】
【数３９】
【００９６】
上記数式３９の各軸方向成分に係る関係式を連立させることにより、３つの角速度の絶対値を算出することができる。３つの軸のうち２つの方向に関しては、角速度が計測により求められているので、符号が判明する。他のもう１つの軸については、角速度が計測により判明する軸と平行な方向の加速度を同時に測定することにより、当該他のもう１つの軸の加速度の符号も判明する。
【産業上の利用可能性】
【００９７】
本発明は、人間の歩行動作解析研究、人間のバランス機能の評価、ロボットの移動動作解析研究、ゴルフなどのスポーツのトレーニングや医療福祉機器の開発に好適に利用可能である。
【符号の説明】
【００９８】
１・・・・・傾斜角度推定システム
２・・・・・加速度センサ
３・・・・・演算処理ユニット
【特許請求の範囲】
【請求項１】
一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、π／２の奇数倍数値に対してπ／２を超えない所定の角度だけ増分した第１の上限値と、前記π／２の奇数倍数値に対して前記所定の角度だけ減分した第１の下限値の間で定義される第１の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第１の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記余弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システム。
【請求項２】
一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を乗算する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、πの倍数値に対してπを超えない所定の角度だけ増分した第２の上限値と、前記πの倍数値に対して前記所定の角度だけ減分した第２の下限値の間で定義される第２の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第２の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記正弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システム。
【請求項１】
一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、π／２の奇数倍数値に対してπ／２を超えない所定の角度だけ増分した第１の上限値と、前記π／２の奇数倍数値に対して前記所定の角度だけ減分した第１の下限値の間で定義される第１の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第１の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記余弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システム。
【請求項２】
一の回転中心点を有するとともに該回転中心点を軸に一の平面内で回転しつつ傾斜する物体の傾斜角度を推定するシステムであって、
前記物体に取付けられた少なくとも２つの加速度センサと、
該少なくとも２つの加速度センサからの出力を受信するとともに該出力に基づいて物体の傾斜角を算出する演算処理ユニットからなり、
前記少なくとも２つの加速度センサは、一線上に整列するとともに、該少なくとも２つの加速度センサを結ぶ線が前記回転中心点を通過し、
前記回転中心点からの少なくとも２つの加速度センサそれぞれの距離が互いに異なり、
前記演算処理ユニット内で、前記一の平面を定めるとともに前記回転中心点を通過する第１の基準軸が定義され、
前記少なくとも２つの加速度センサのうち、一の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第２の加速度を出力し、他の加速度センサが、前記少なくとも２つの加速度センサを結ぶ線に沿う方向の第１の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第３の加速度と、前記少なくとも２つの加速度センサを結ぶ線に対して直交するとともに前記物体の移動平面に対して平行な方向の第４の加速度を出力し、
前記演算処理ユニットが、前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第１の乗算値を算出する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に沿う方向の加速度の出力値に対して乗算し、第２の乗算値を算出する段階と、
前記回転中心点から前記一の加速度センサまでの距離を、前記他の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第３の乗算値を乗算する段階と、
前記回転中心点から前記他の加速度センサまでの距離を、前記一の加速度センサにより出力される前記少なくとも２つの加速度センサを結ぶ線に直交する方向の加速度の出力値に対して乗算し、第４の乗算値を算出する段階と、
前記第１の乗算値と前記第２の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の正弦関数値を算出する段階と、
前記第３の乗算値と前記第４の乗算値の差分値に基づき、前記第１の基準軸と前記少なくとも２つの加速度センサを結ぶ線との挟角の余弦関数値を算出する段階を実行し、
前記演算処理ユニットが、πの倍数値に対してπを超えない所定の角度だけ増分した第２の上限値と、前記πの倍数値に対して前記所定の角度だけ減分した第２の下限値の間で定義される第２の閾値領域を認識し、
前記正弦関数値と前記余弦関数値から求められる前記挟角の値が前記第２の閾値領域の範囲内にあるとき、前記演算処理ユニットが前記正弦関数値に基づき算出された挟角を出力することを特徴とする傾斜角度推定システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１２−２３３９１５（Ｐ２０１２−２３３９１５Ａ）
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願番号】特願２０１２−１６１０１８（Ｐ２０１２−１６１０１８）
【出願日】平成２４年７月１９日（２０１２．７．１９）
【分割の表示】特願２００８−２４５３５（Ｐ２００８−２４５３５）の分割
【原出願日】平成２０年２月４日（２００８．２．４）
【出願人】（５０９０９３０２６）公立大学法人高知工科大学 (95)
【Ｆターム（参考）】
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願日】平成２４年７月１９日（２０１２．７．１９）
【分割の表示】特願２００８−２４５３５（Ｐ２００８−２４５３５）の分割
【原出願日】平成２０年２月４日（２００８．２．４）
【出願人】（５０９０９３０２６）公立大学法人高知工科大学 (95)
【Ｆターム（参考）】
[ Back to top ]