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