姿勢角決定装置およびその決定方法
【課題】複雑な関係にある座標系間であってもその姿勢関係を容易に，精度良く求めることのできる姿勢角決定装置およびその決定方法を提供すること。
【解決手段】本発明の姿勢角決定装置は，各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得する傾斜角取得部と，第１リンクと第２リンクとの姿勢関係を固定したまま最大３通りの全体姿勢を取らせる姿勢変化指示部と，各全体姿勢における各リンクについて各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，各リンクについて全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出するリンク行列算出部とを有する。
【解決手段】本発明の姿勢角決定装置は，各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得する傾斜角取得部と，第１リンクと第２リンクとの姿勢関係を固定したまま最大３通りの全体姿勢を取らせる姿勢変化指示部と，各全体姿勢における各リンクについて各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，各リンクについて全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出するリンク行列算出部とを有する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は，リンク機構によって接続された２つの座標系間の姿勢関係を決定する姿勢角決定装置およびその決定方法に関する。さらに詳細には，２つの座標系間の姿勢関係を３つの姿勢角（ヨー角φ，ピッチ角θ，ロール角ψ）で表すための姿勢角決定装置およびその決定方法に関するものである。
【背景技術】
【０００２】
リンク機構によって接続された複数部材の姿勢制御を行う場合，一般に，各部材ごとにその位置座標を表す座標系を設け，まず部材の姿勢をその座標によって特定する。その後，座標間の姿勢関係を３つの姿勢角を用いて変換することにより，共通の座標で表している。従来，このような座標間の姿勢関係を求めるには，図面に基づいて寸法を計測したり，三次元測定器によって角度を実測することにより行っていた。
【０００３】
例えば，特許文献１には，人間の大腿部，下腿部等の各部を剛体要素（リンク）として，各剛体要素に要素座標系を設定した剛体リンクモデルを作成し，そのモデルを用いて移動体の関節モーメントを推定する方法が開示されている。本文献に記載の技術では，例えば股関節に設けられた関節変位センサや発光／受光器等の出力を基にして，座標変換のための変換テンソルを作成し，それを用いて各関節要素や各剛体要素の位置が求められるとされている。
【特許文献１】特開２００５−８１５３７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら，前記した従来の図面や実測による方法では，２つの座標系の配置が複雑になるにつれて作業が繁雑となり，姿勢角の決定が困難であった。特に，座標系同士がねじれの関係にあって，図面上でそれらの関係を明記できない場合等では，図面から姿勢の関係を読み取ることができない。また，座標系間の距離が長い場合あるいは，座標系間に障害物がある場合等では，三次元測定器による計測が困難であった。これらの条件が重なると，図面によっても実測によっても姿勢関係が求められないという問題点があった。また，座標系の設置について角度誤差がある場合では，図面上から得られる座標系間の姿勢関係では，精度が低いものとなるという問題点があった。
【０００５】
また，特許文献１に記載の技術は，例えば股関節の関節変位センサ等によりリンク間の回転角が把握されている場合の変換方法である。さらに，股関節，膝関節，足首関節の３つの関節を通る脚平面を想定してその上での２次元量を利用することにより，移動しつつある人間の脚体の関節に作用するモーメントを推定するものであり，このままではリンク間の姿勢関係の取得には利用できない。
【０００６】
本発明は，前記した従来の技術が有する問題点を解決するためになされたものである。すなわちその課題とするところは，複雑な関係にある座標系間であってもその姿勢関係を容易に，精度良く求めることのできる姿勢角決定装置およびその決定方法を提供することにある。
【課題を解決するための手段】
【０００７】
この課題の解決を目的としてなされた本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得する傾斜角取得部と，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせる姿勢変化指示部と，姿勢変化指示部の指示による各全体姿勢における各リンクについて，傾斜角取得部で取得された各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，各リンクについて，傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものである。
【０００８】
本発明の姿勢角決定装置によれば，傾斜角取得部によって，各リンクに設定された３軸直交座標系の各軸方向と重力ベクトルの方向との間の角度である傾斜角が取得される。また，姿勢変化指示部によって，リンク機構は最大３通りの全体姿勢を取るように指示される。そして各全体姿勢において傾斜ベクトル算出部によって傾斜ベクトルが求められ，続いて，傾斜行列算出部によって傾斜ベクトルより傾斜行列が求められる。さらに，求められた傾斜行列に基づいて，姿勢角を表すリンク行列が算出される。従って，複雑な関係にある座標系間であってもその姿勢関係を容易に，高価な機構を用いず，小型な装置によって精度良く求めることが出来る。
【０００９】
なお，「傾斜角取得部」，「傾斜ベクトル算出部」，「傾斜行列算出部」は，両リンクについてまとめて処理するものであっても良いし，各リンクのそれぞれに設けられているものであっても良い。また，傾斜ベクトルは傾斜角を成分とするベクトルである。傾斜行列は，傾斜ベクトルを並べたものであり，すなわち，傾斜角を軸および全体姿勢により２次元に配列した行列である。また，傾斜行列は，その座標系の傾斜角を表すものであるが，座標系から別の座標系を見たときの姿勢関係までは表現しない。
【００１０】
また，傾斜角取得部は，各座標系の軸の数（３個）×リンクの数（２個）の都合６個の傾斜角を取得する。傾斜ベクトル算出部は，全体姿勢の数（最大３種）×リンクの数（２個）の傾斜ベクトルを算出する。傾斜行列算出部は，リンクの数（２個）の傾斜行列を算出する。
【００１１】
さらに本発明では，傾斜角取得部が，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた水準器を用いて得ることが望ましい。このようにすれば，傾斜角を直接的に得ることができる。
【００１２】
また本発明では，傾斜角取得部が，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた加速度センサの出力値から求めるものであっても良い。このようにすれば，傾斜角を容易に得ることができる。
【００１３】
また本発明では，傾斜角取得部が，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めるものとしてもよい。この場合，平方根の符号は，座標系の上下関係として外部信号またはＧＰＳ，マーカー等で与えられることが好ましい。このようにすれば，必要な加速度センサの個数や全体姿勢の変化の回数を減らすことができる。
【００１４】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得する加速度値取得部と，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせる姿勢変化指示部と，姿勢変化指示部の指示による各全体姿勢における各リンクについて，加速度値取得部で取得された各軸方向の加速度値から傾斜角を求める傾斜角測定部と，姿勢変化指示部の指示による各全体姿勢における各リンクについて，傾斜角測定部で求められた各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，各リンクについて，傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであってもよい。
【００１５】
すなわち，第１リンクの座標系と第２リンクの座標系との間に，１軸が互いに平行であるという関係がある場合は，処理をやや簡略化できる。一般的には加速度値取得部は，各リンクごとに軸の数（ここでは３軸なので３個）だけ加速度を取得することが必要であるが，この条件があれば，その平行な１軸を除いた２軸の加速度値を取得するのみでよい。すなわち，本発明では，加速度値取得部は，２軸×リンク数の都合４個の加速度値を取得する。従って，例えば加速度センサの出力値から加速度値を求める場合では，このような条件が満たされる場合には，一般的な場合より加速度センサの数を減らすことができる。さらに，全体姿勢の数も減らすことができる。
【００１６】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得する加速度値取得部と，各リンクについて，加速度値取得部で取得された加速度値から傾斜角を求める傾斜角測定部と，傾斜角測定部により求められた各リンクの傾斜角の差より姿勢角を算出する姿勢角計算部とを有するものであっても良い。
【００１７】
すなわち，第１リンクの座標系と第２リンクの座標系との間に，１軸が互いに平行でありかつ水平であるという関係がある場合は，処理をさらに簡略化できる。加速度値取得部は，各リンクに１個ずつの都合２個の加速度値を取得すればよい。従って，加速度センサの数は２個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００１８】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクに設定された座標系間のヨー角を取得する第１ヨー角取得部と，加速度値取得部で取得された加速度値と，第１ヨー角取得部で取得されたヨー角とから，姿勢角を表すリンク行列を算出する第２リンク行列算出部とを有するものであっても良い。
【００１９】
すなわち，加速度値取得部は，各リンクに１個ずつの都合２個の加速度値を取得すればよい。従って，加速度センサの数は２個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。ここで，ヨー角とは，２つの座標系間の１つの軸回りの姿勢角変化である。
【００２０】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得する第２ヨー角取得部と，各リンクについて，加速度値取得部で取得された加速度値と，第２ヨー角取得部で取得されたヨー角とから，基準座標系に対する姿勢行列を求める第１姿勢行列算出部と，第１姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであっても良い。
【００２１】
すなわち，加速度値取得部は，各リンクに１個ずつの都合２個の加速度値を取得すればよい。従って，加速度センサの数は２個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００２２】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクに設けられそれぞれにおける角速度値を取得する角速度値取得部と，各リンクについて，加速度値取得部で取得された加速度値と，角速度値取得部で取得された角速度値とから，基準座標系に対する姿勢行列を求める第２姿勢行列算出部と，第２姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであっても良い。
【００２３】
すなわち，加速度値取得部は，各リンクに３個ずつの都合６個の加速度値を取得すればよい。また，角速度値取得部は，各リンクに３個ずつの都合６個の角速度値を取得すればよい。また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００２４】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクに設けられた磁気センサと，各リンクについて，加速度値取得部で取得された加速度値と，磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求める第３姿勢行列算出部と，第３姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであっても良い。
【００２５】
すなわち，加速度値取得部は，各リンクに２個ずつの都合４個の加速度値を取得すればよい。従って，加速度センサの数は４個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００２６】
さらに本発明では，各リンクに設けられた磁気センサを有し，第２姿勢行列算出部は，加速度値取得部で取得された加速度値と，角速度値取得部で取得された角速度値と，磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求めることが望ましい。
【００２７】
さらに本発明では，姿勢変化動作部が，姿勢変化装置を内蔵するロボットであり自ら全体姿勢を形成することが望ましい。
【００２８】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得し，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせ，指示による各全体姿勢における各リンクについて，取得した各軸方向の傾斜角から傾斜ベクトルを求め，各リンクについて，全体姿勢ごとに求めた傾斜ベクトルにより傾斜行列を求め，求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００２９】
また本発明では，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めることが望ましい。
【００３０】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得し，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせ，指示による各全体姿勢における各リンクについて，取得した各軸方向の加速度値から傾斜角を求め，指示による各全体姿勢における各リンクについて，求めた各軸方向の傾斜角から傾斜ベクトルを求め，各リンクについて，全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求め，求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３１】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得し，各リンクについて，取得した加速度値から傾斜角を求め，求めた各リンクの傾斜角の差より姿勢角を算出する姿勢角決定方法にも及ぶ。
【００３２】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクに設定された座標系間のヨー角を取得し，取得した加速度値と，取得したヨー角とから，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３３】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得し，各リンクについて，取得した加速度値と，取得したヨー角とから，基準座標系に対する姿勢行列を求め，求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３４】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクにおける角速度値を取得し，各リンクについて，取得した加速度値と，取得した角速度値とから，基準座標系に対する姿勢行列を求め，求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３５】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクにおける磁北方向との交差角の情報を磁気センサで取得し，各リンクについて，取得した加速度値と，取得した磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求め，求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３６】
また本発明では，姿勢変化動作部が，姿勢変化装置を内蔵するロボットであり，自ら全体姿勢を形成することが望ましい。
【発明の効果】
【００３７】
本発明の姿勢角決定装置およびその決定方法によれば，複雑な関係にある座標系間であってもその姿勢関係を容易に，精度良く求めることができる。
【発明を実施するための最良の形態】
【００３８】
以下，本発明を具体化した最良の形態について，添付図面を参照しつつ詳細に説明する。本形態は，２つの座標系間の姿勢関係を得るための姿勢角決定装置である。ここで，姿勢角とは，例えばリンク等のなす角度（ヨー角φ，ピッチ角θ，ロール角ψ）をある座標系で表したものである。そして姿勢関係とは，その姿勢角を，別の座標系を基準として見た姿勢角に変換するための関係式である。
【００３９】
まず，本形態の姿勢角決定装置によって姿勢角が決定される対象であるリンク機構の例を図１に示す。リンク機構とは，リンクと関節との２つの部位から構成されているものを指す。この図に示すリンク機構１０は，３つのリンク１１，１２，１３を有している。また，リンク１１とリンク１２とが関節１４で，リンク１２とリンク１３とが関節１５でそれぞれ接続されている。関節１４は３つの回転自由度を持つ。関節１５も同じく３つの回転自由度を持つ。
【００４０】
またリンク機構上の二箇所には，第１座標系（ｘ１，ｙ１，ｚ１）と第２座標系（ｘ２，ｙ２，ｚ２）が設置されている。図１では第１座標系はリンク１１に固定されており，第２座標系はリンク１３に固定されている。第１座標系と第２座標系は，関節１４と関節１５を固定しておけば，互いに所定の姿勢関係を有している。関節１４または関節１５を動かすと，座標系間の姿勢関係は変化する。また両座標系間の姿勢関係を固定したままリンク機構１０の全体としての姿勢を変更することも可能である。本形態の姿勢角決定装置は，このようなリンク機構１０に設けられた２つの座標系間の姿勢関係を得るものである。
【００４１】
次にリンク機構１０の具体的な例をいくつか示す。リンク自体は，剛なものと柔なものとに大きく分けることができる。また，リンク同士の関係すなわち関節の状態としては，常に固定されている関係，動作中あるいは一定期間のみ固定される関係，常に柔軟な関係がある。固定されているとは，それらのリンクの間の姿勢は変化しないことを意味する。柔軟であるとは，リンク間の姿勢が変化することを意味する。
【００４２】
まず，図２にリンクが剛であり関係が固定されている例を示す。これは，１つの剛なる物体上の異なる２面に設置された２つの座標系の関係である。すなわち，図２の例は，剛なリンクと固定した関節からなるリンク機構で表すことができる。この例では，第１面上に設置した第１座標系と第２面上に設置した第２座標系の姿勢関係は変化しない。またこの例では隣り合った面同士が直交関係になく，二つの座標系間の姿勢関係がネジレの位置にあるため，座標系間の姿勢関係を容易には求めることができない。この例で表すことができる事例としては，部品を組み立てた直後や部品を所定の場所に設置した直後で，部品内の面同士や部品と設置面との直交がでていない場合や，部品の形状に設計値からのズレがある場合などがある。
【００４３】
次に，図３にリンクが剛であり，関係が一定期間のみ固定される例を示す。これは，可動リンクを持つロボットである。図３の例では，剛なリンクと、固定および非固定を自由に可変できる関節から成り立っている。この例で表すことができる事例としては，ロボットのリンク間角度が初期状態では不定であるが，ロボットの動作中は関節がサーボ機構によってソフト的に，またはネジなどによりハード的に固定されることでリンク機構が剛な関係であると見なせる場合などがある。固定された状態では各リンク上に置かれた二つの座標系間の関係は変化しない。図３の例では，第１座標系はロボットの胴体に，また第２座標系は腕上に設置されている。ただし関節部には関節角度を計測する装置がなく，関節角度を直接測定することはできない。
【００４４】
また上記のタイプの別の例として、ロボットのリンク間角度が初期状態では不定であるが，リンク間角度の初期値が与えられるという初期化工程を経ることで、初期化後には関節部に設置された関節角度測定器により、関節角度が得られるものもある。この場合は関節が固定されていなくともよい。
【００４５】
次に，図４にリンクが柔であり，関係が柔であるものの例を示す。図４の例は，常に柔らかく変形する構造体であり，このような構造体上の異なる２点に設置された２つの座標系の関係は，常に柔な関係である。従って，図４の例は，柔軟なリンク、またはリンクは剛であるが固定されない関節を持つリンク機構で表すことができる。リンク上に設置した第１座標系と第２座標系の姿勢関係は，リンクに働く重力方向の変化によって変化する。この例で表すことのできる事例としては，リンクがゴムのように容易に変形する物質で作られていたり，リンクを構成する板の剛性が低い場合などがある。なお，以上の説明に用いたリンクおよび関節は，第１座標系と第２座標系の関係をわかりやすく求めるために用いたものであり，実際にリンクあるいは関節が第１座標系および第２座標系の間に存在しなくてもよい。
【００４６】
次に，固定リンクをもつリンク機構についての姿勢角を決定するための姿勢角決定装置２０の概略構成を，図５のブロック図に基づいて説明する。これは，図２に示した変形しないリンク機構を対象とするものである。図２における第１座標系と第２座標系が，図１に示すリンク機構１０で表されるリンク１１，リンク１２，リンク１３および関節１４，関節１５で結びつけられているものとすることができる。
【００４７】
この例では，図５に示すように，リンク機構１０には測定対象の各座標系の各軸方向に沿って，それぞれ加速度センサが設けられる。すなわち，第１座標系のリンク１１には，ｘ1軸加速度センサ２１，ｙ1軸加速度センサ２２，ｚ1軸加速度センサ２３が設けられている。また，第２座標系のリンク１３には，ｘ2軸加速度センサ２４，ｙ2軸加速度センサ２５，ｚ2軸加速度センサ２６が設けられている。
【００４８】
さらに，このリンク機構１０には，全体姿勢を変化させるためのモータ等を有する姿勢変化動作器２７が設けられている。この姿勢角決定装置２０で姿勢角を算出するために，姿勢変化動作器２７を動作させて，測定対象であるリンク機構１０の姿勢を適宜変化させることが必要となる。そのために，姿勢角決定装置２０はさらに，姿勢変化指令器２８および計測回数カウント器２９を有している。計測回数カウント器２９は，リンク機構１０の全体姿勢を変化をさせた回数をカウントする。そして，必要回数の姿勢変化が終了するまで，姿勢変化指令器２８が姿勢変化動作器２７に指令を出し，リンク機構１０の全体姿勢を変化させる。
【００４９】
また，姿勢角決定装置２０には，ｘ1軸加速度センサ２１，ｙ1軸加速度センサ２２，ｚ1軸加速度センサ２３の傾斜角出力を受けて，その値をサンプリングするサンプリング器３１と，ｘ2軸加速度センサ２４，ｙ2軸加速度センサ２５，ｚ2軸加速度センサ２６の出力を受けて，その値をサンプリングするサンプリング器３２とを有する。ここでサンプリングされる値はそれぞれ，傾斜角の各軸方向の余弦値となっている。
【００５０】
また，姿勢角決定装置２０は，サンプリング器３１の結果から傾斜ベクトルを算出する傾斜ベクトル算出器３３と，サンプリング器３２の結果から傾斜ベクトルを算出する傾斜ベクトル算出器３４とを有する。これらの傾斜ベクトルは，各座標系の重力方向に対する傾斜を表す。さらに，姿勢角決定装置２０は，傾斜ベクトル算出器３３と傾斜ベクトル算出器３４との算出結果を受けるレジスタ３５，レジスタ３５の出力を受けて各傾斜ベクトルから傾斜行列を算出する傾斜行列算出器３６を有する。
【００５１】
ここで，基準座標系から見た第１座標系への変換を姿勢行列Ｔ１，基準座標系から見た第２座標系への変換を姿勢行列Ｔ２としたとき，姿勢行列Ｔ１から姿勢行列Ｔ２への変換行列をリンク行列Ｔ１２とする。姿勢角決定装置２０は，リンク行列算出器３７，リンク行列表示器３８，姿勢角算出・表示器３９を有している。リンク行列算出器３７は，傾斜行列算出器３６の結果を受けて，リンク行列Ｔ１２を算出する。算出されたリンク行列は，リンク行列表示器３８によって表示される。さらに，姿勢角算出・表示器３９は，リンク行列Ｔ１２を用いて姿勢角を算出し，表示する。
【００５２】
この姿勢角決定装置２０を用いて姿勢角を求める手順を簡単に説明する。まず，第１座標系と第２座標系との間には，リンク１１〜１３があり，各リンクの関係の設定が終了すると２つの座標系間の姿勢関係が固定される。この固定された姿勢関係を保持したまま，これらのリンク機構１０の全体姿勢を条件に応じて最大３通りに変化させる。ここでは，まず最も一般的な例として，３通りに変化させる場合を説明する。
【００５３】
全体姿勢を変化させた後，，その全体姿勢における加速度センサ２１〜２６の出力結果を取得する。これにより，最大３種類の取りうる姿勢についての傾斜角のデータを得る。すなわち，計測回数カウント器２９によって計測回数をカウントしつつ，姿勢変化指令器２８より姿勢変化動作器２７へ姿勢を変化させる指示を送出する。そして，各全体姿勢において，計６個の加速度センサ２１〜２６によって，各軸方向の加速度を測定する。
【００５４】
次に，各全体姿勢ごとに，各加速度センサ２１〜２３の出力から，サンプリング器３１によって，第１座標系の各軸の重力方向に対する傾斜角を抽出する。また，各全体姿勢ごとに，各加速度センサ２４〜２６の出力から，サンプリング器３２によって，第２座標系の各軸の重力方向に対する傾斜角を抽出する。さらに，これらの傾斜角から，傾斜ベクトル算出器３３によって，第１座標系の傾斜ベクトルを算出する。また，同様に，傾斜ベクトル算出器３４によって，第２座標系の傾斜ベクトルを算出する。
【００５５】
これにより，各全体姿勢に対応する計３種類の傾斜ベクトルが得られるので，傾斜行列算出器３６によって傾斜ベクトルを並べた傾斜行列を求める。続いて，この傾斜行列に基づいてリンク行列算出器３７によってリンク行列Ｔ１２を算出する。さらに，リンク行列表示器３８によってリンク行列Ｔ１２を表示する。さらに，リンク行列Ｔ１２から姿勢角算出・表示器３９によって，第１座標系から見た第２座標系の姿勢角（ヨー角φ，ピッチ角θ，ロール角ψ）が求められる。
【００５６】
次に，各手順について詳しく説明する。まず，図２および図３に示した対象の例について，基準座標系（Ｘ，Ｙ，Ｚ）から見た第１座標系への姿勢行列Ｔ１，基準座標系から見た第２座標系への姿勢行列Ｔ２，第１座標系からみた第２座標系へのリンク行列Ｔ１２の関係を，図６および図７に示す。ここでは，基準座標系は時間によりその姿勢や位置が変化せず，そのＺ軸は重力ベクトルの逆方向を向いている。これら図６および図７の例は，図８に示すリンク構成として表すことができる。
【００５７】
まず，リンク行列Ｔ１２と各姿勢行列Ｔ１，Ｔ２との関係は次の式１で表される。
Ｔ２＝Ｔ１・Ｔ１２ …（式１）
この姿勢行列Ｔ１を，基準座標系から見た第１座標系の姿勢角（ヨー角φ1，ピッチ角θ1，ロール角ψ1）を用いて表すと，次の式２のようになる。
Ｔ１＝ＲＰＹ（φ1，θ1，ψ1）
＝Ｒｏｔ（Ｚ，φ1）Ｒｏｔ（Ｙ，θ1）Ｒｏｔ（Ｘ，ψ1） …（式２）
これは，まず基準座標系のＺ軸回りにφ1回転させ，次にＺ軸回りに回転した後のＹ軸回りにθ1回転させ，次にＹ軸回りに回転した後のＸ軸回りにψ1回転させることで，第１座標系が得られることを示している。
【００５８】
同様に，姿勢行列Ｔ２を，基準座標系から見た第２座標系の姿勢角（ヨー角φ2，ピッチ角θ2，ロール角ψ2）を用いて表すと，次の式３のようになる。
Ｔ２＝ＲＰＹ（φ2，θ2，ψ2）
＝Ｒｏｔ（Ｚ，φ2）Ｒｏｔ（Ｙ，θ2）Ｒｏｔ（Ｘ，ψ2） …（式３）
また，リンク行列Ｔ１２を，第１座標系から見た第２座標系の姿勢角（ヨー角φ12，ピッチ角θ12，ロール角ψ12）を用いて表すと，次の式４のようになる。
Ｔ１２＝ＲＰＹ（φ12，θ12，ψ12）
＝Ｒｏｔ（ｚ1，φ12）Ｒｏｔ（ｙ1，θ12）Ｒｏｔ（ｘ1，ψ12）…（式４）
これは，第１座標系から各軸のまわりに，φ12，θ12，ψ12の順番で回転させると，第２座標系が得られることを表している。
【００５９】
ここで，図８に示すように，リンク１１とリンク１３には，各座標系の各軸方向への加速度を検出する加速度センサ２１〜２６が設けられている。さらに基準座標系のＺ軸が重力と逆方向に設定されているので，各加速度センサ２１〜２６の出力から，第１，第２座標系の各軸の基準座標系のＺ軸とのなす角が求められる。これは，運動していないリンク機構１０では，重力の影響のみが各加速度センサ２１〜２６で測定されるからである。これらの各加速度センサ２１〜２６の出力から，第１，第２座標系の各姿勢行列Ｔ１，Ｔ２を求める方法について説明する。
【００６０】
まず，姿勢行列Ｔ１，姿勢行列Ｔ２，リンク行列Ｔ１２を一般形で要素表示すると，以下の式５，式６，式７に示すように表すことができる。
【００６１】
【数１】
【００６２】
【数２】
【００６３】
【数３】
【００６４】
これらを用いて，式１を要素表示すると，以下の式８となる。
【００６５】
【数４】
【００６６】
ここで，例えば姿勢行列Ｔ１の要素ｃ1は，第１座標系のｘ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。すなわち，図９に示すように，第１座標系のｘ1軸と基準座標系のＺ軸とのなす傾斜角度をγ1とすると，次の式９で表される。
ｃ1＝ｃｏｓγ1 …（式９）
同様に，姿勢行列Ｔ１，姿勢行列Ｔ２のうち，最下段の各要素が各座標系の各軸と基準座標系のＺ軸との間の方向余弦を表しているので，各加速度センサ２１〜２６の出力値からｃ1，ｆ1，ｉ1，ｃ2，ｆ2，ｉ2が求まる。
【００６７】
そこで，式８の要素表示のうち，ｃ1，ｆ1，ｉ1，ｃ2，ｆ2，ｉ2に着目すると，以下の式１０〜式１２が得られる。
ｃ2＝ｃ1ｐ＋ｆ1ｑ＋ｉ1ｒ …（式１０）
ｆ2＝ｃ1ｓ＋ｆ1ｔ＋ｉ1ｕ …（式１１）
ｉ2＝ｃ1ｖ＋ｆ1ｗ＋ｉ1α …（式１２）
【００６８】
従って，式８は，以下の式１３で表すことができる。この式１３の左辺の右項を第２傾斜ベクトル，右辺を第１傾斜ベクトルと呼ぶ。
【００６９】
【数５】
【００７０】
次に，第１座標系と第２座標系との姿勢関係を保持したままで，リンク機構１０の全体姿勢を３通りに変更させる。そして，各全体姿勢ごとに，各加速度センサ２１〜２６の出力値を得る。さらに，上記の手順と同様に，各全体姿勢における第１座標系の傾斜ベクトルおよび第２座標系の傾斜ベクトルを得ることができる。すなわち，各座標系についてそれぞれ３通りの傾斜ベクトルの組が得られる。ここで，リンク機構１０の全体姿勢を変化させて得られた第１座標系の傾斜ベクトルの組を第１傾斜行列Ｓ１，第２座標系の傾斜ベクトルの組を第２傾斜行列Ｓ２とおく。すなわち，それぞれ３つの傾斜ベクトルの組をまとめて傾斜行列として表すことにより，リンク行列Ｔ１２は，次の式１４で表すことができる。
【００７１】
【数６】
【００７２】
式１４において，例えばｃjkのｊは座標系の番号であり，ｋは測定回数を示している。また，式１４の右辺第２項の逆行列が存在するためには，次の式１５が成立する必要がある。
ｃ21・ｆ22・ｉ23＋ｆ21・ｉ22・ｃ23＋ｃ22・ｆ23・ｉ21−ｉ21・ｆ22・ｃ23−ｆ21・ｃ22・ｉ23−ｉ22・ｆ23・ｃ21≠０ …（式１５）
具体的には，基準座標系のＺ軸回りの回転や，同じ軸回りの回転は姿勢を変化したことにならないので避ける。
【００７３】
そして，式１４で求められたリンク行列Ｔ１２の各要素の値を使って，第１座標系から見た第２座標系の各姿勢角は，次の式１６〜式１８のように決定される。
φ12＝ａｔａｎ２（ｑ，ｐ） …（式１６）
θ12＝ａｔａｎ２（−ｒ，ｃｏｓφ・ｐ＋ｓｉｎφ・ｑ） …（式１７）
ψ12＝ａｔａｎ２（ｓｉｎφ・ｖ−ｃｏｓφ・ｗ，−ｓｉｎφ・ｓ＋ｃｏｓφ・ｔ） …（式１８）
【００７４】
従って，上記の式１６〜式１８を用いて，第２座標系は，第１座標系をまずｚ1軸回りにφ12，次に回転後のｙ2軸回りにθ12，最後に回転後のｘ2軸回りにψ12回転させることで表すことができる。これから，第１座標系と第２座標系との関係が得られた。ただし，ｘ2軸がｚ1軸に平行近くになった場合には，式１６からφ12が求まらない。このような場合には，第１座標系と第２座標系との関係を表すのに，姿勢角を用いる代わりにリンク行列Ｔ１２を用いると良い。
【００７５】
次に，式１９にこのように求められるリンク行列Ｔ１２の，具体例を示す。
【００７６】
【数７】
【００７７】
次に，この姿勢角決定方法を図１０のフローチャートと図５のブロック図に基づいて説明する。リンク機構１０の各リンク関係の設定が終了すると，まず，各加速度センサ２１〜２６による測定を開始する（Ｓ１０１）。そして，第１座標系の加速度センサ２１〜２３の傾斜角出力結果からサンプリング器３１において，第１座標系のｘ1軸，ｙ1軸，ｚ1軸に関する傾斜角を抽出する（Ｓ１０２）。さらに，傾斜ベクトル算出器３３において傾斜ベクトルを算出して，レジスタ３５に記憶する（Ｓ１０３）。
【００７８】
同様に，第２座標系の加速度センサ２４〜２６の結果からサンプリング器３２において，第２座標系の傾斜角を抽出する（Ｓ１０４）。さらに，傾斜ベクトル算出器３４において傾斜ベクトルを算出して，レジスタ３５に記憶する（Ｓ１０５）。第１，第２座標系についての算出が終了したら，姿勢変化指令器２８によって姿勢変化動作器２７に指示を出し，第１，第２座標系の姿勢を変化させる（Ｓ１０６）。
【００７９】
このとき，計測回数カウント器２９で計測回数をカウントし，姿勢変化指令器２８が姿勢変化動作器２７に指令を発する。全体姿勢を３通りに変化させ，３種類の全体姿勢について，両座標系の傾斜ベクトルの組を算出する。３回の計測を実施したら（Ｓ１０７：Ｙｅｓ），傾斜行列算出器３６によって，第１座標系の傾斜ベクトルの組から第１傾斜行列Ｓ１を，第２座標系の傾斜ベクトルの組から第２傾斜行列Ｓ２をそれぞれ生成する（Ｓ１０８）。
【００８０】
続いて，第１傾斜行列Ｓ１と第２傾斜行列Ｓ２とを用いて，リンク行列算出器３７によって，リンク行列Ｔ１２を算出する（Ｓ１０９）。さらに，リンク行列表示器３８によって，算出したリンク行列を表示する（Ｓ１１０）。そして，姿勢角算出・表示器３９によって，求めたリンク行列を基に，第１座標系から第２座標系への姿勢角（φ，θ，ψ）を算出し表示する（Ｓ１１１）。以上で，この処理を終了する。
【００８１】
次に，第１座標系と第２座標系との間に所定の関係がある場合の処理の簡略化について説明する。まず第１に，例えば，ｚ1軸とｚ2軸とが平行である場合のように，両座標系のうちの１軸が平行であることが判っている場合では，その平行な軸に関する回転のみで記述できる。ただしここでは，ｚ1軸とｚ2軸とが重力方向に平行である場合は除く。例えば，ｚ1軸とｚ2軸とが平行である場合では，θ12，ψ12は回転に用いる必要がないので，φ12のみを含む次の式２０で表記できる。
【００８２】
【数８】
【００８３】
また，この場合では，全体姿勢を変化させる回転軸をｚ1軸とすれば，ｚ1軸とｚ2軸とにはいずれも加速度センサは不要である。すなわち，加速度センサ２１，２２，２４，２５の４個でよい。さらに，２通りの全体姿勢をとらせるだけでよい。そして，２回の測定結果から次の式２１を用いてリンク行列Ｔ１２を得ることができる。
【００８４】
【数９】
【００８５】
次に，図３の例のように，動作時に剛な構造物を対象とした姿勢角決定装置の具体的な構成の例を，図１１のブロック図を参照して説明する。なお，図５に示した例と共通の部材については，同じ符号を付して説明を省略する。以下の，類似のブロック図についても同様である。この図１１に示したのは，測定対象が動作時に剛な構造物であり，両座標系のうちの１軸（ここではｚ1軸とｚ2軸）が平行であるとともに，平行な回転軸の情報（あるいは全体姿勢の上下）が外部信号として得られる場合である。このようなリンク機構１１０に適用される姿勢角決定装置では，加速度センサは各座標系のｘ軸とｙ軸に設けるのみでよい。
【００８６】
動作時に剛な構造物では，リンク機構１１０が有する関節が動作すると，動作中信号発生器１１１がリンク機構１１０が動作中であることを，リンク固定指示器１１２に送る。さらに，動作が終了するとリンク固定指示器１１２は，リンク機構１１０内にある関節の固定をリンク機構固定器１１３に指示する。これにより，リンク機構１１０の関節が固定される。
【００８７】
ここで，傾斜角を測定中に各加速度センサ２１，２２，２４，２５に対し重力加速度以外の，例えば運動による並進加速度が印加されると，傾斜角の測定結果に誤差が生ずる。このため全体姿勢計測中は傾斜ベクトル算出器３３，３４から出力された傾斜ベクトルの各要素の値が安定している必要がある。そこで静止状態判定器１１４により，タイマー１１５でカウントした時間内に傾斜ベクトル算出器３３，３４から出力された傾斜ベクトルの各要素が一定か否かを判定し，各要素の変化量が規定値以内であれば，レジスタ３５に取得した傾斜ベクトルを記録するように指示する。
【００８８】
次に静止状態判定器１１４は姿勢変化指令器２８に姿勢変化を指令してもよいことを伝える。姿勢変化指令器２８は姿勢変化動作器２７に全体姿勢を変化させるように指示する。姿勢変化動作器２７は，第１座標系と第２座標系の姿勢関係が変化しないようリンク機構１１０の全体姿勢を変更する。各加速度センサ２１，２２，２４，２５の出力はサンプリング器３１，３２で常に取り込まれ，傾斜ベクトル算出器３３，３４において傾斜ベクトルが生成される。
【００８９】
このとき，この例では外部から回転軸平行状態検知用信号１１６が得られるので，回転軸判定器１１７は，回転軸の判定を行い，傾斜ベクトル算出器３３，３４と計測回数カウント器２９に対し，回転軸の軸名と，得られた複数軸の加速度信号のうち，傾斜行列を算出するために必要な加速度信号名、および必要計測回数を伝える。さらに，レジスタ３５は，計測回数カウント器２９から得られた計測回数値と，傾斜行列の各要素値を保存する。本計測は計測回数カウント器２９が規定する回数繰り返され，必要な計測回数が完了したらリンク行列算出器３７により，第１座標系からみた第２座標系のリンク行列が算出される。これをリンク行列表示器３８により表示し，さらに，姿勢角算出・表示器３９によって姿勢角に変換する。
【００９０】
次に，測定対象が動作時に剛な構造物であり，両座標系のうちの１軸の組が平行であるが，平行な回転軸の情報（あるいは全体姿勢の上下）が外部信号としては得られない場合の例を，図１２のブロック図に示す。この場合には，回転軸の判定を内部で行わなければならない。そのため，加速度センサ２１〜２６は各軸に計６個必要となっている。
【００９１】
この例では，サンプリング器３１，３２において得られた傾斜角信号を回転軸判定器１１７に送る。回転軸判定器１１７は，第１座標系と第２座標系の同じ軸の傾斜角を比較し，それぞれの傾斜角値の変化量が特定の軸について既定値内であれば、当該軸が回転軸でありかつ当該軸が第１座標系および第２座標系において平行な軸であると判定する。回転軸判定器１１７は，このようにして判定された平行な軸の組をそれぞれの回転軸と判定する。さらに，傾斜ベクトル算出器３３，３４と計測回数カウント器２９に対し，回転軸の軸名と，得られた複数軸の傾斜角信号のうち傾斜行列を算出するために必要な軸名、および必要計測回数を伝える。以下の処理は，上記の図１１の例と同様である。
【００９２】
次の例は，上記の図１２の例に加えて，平行な１組の軸が水平面内にある場合である。例えば，上記の図１２の例において，平行な軸の組であるとともに全体姿勢を変化させる回転軸がｚ1軸とｚ2軸であり，かつこれらが重力方向を法線にもつ水平面内にある場合である。この場合には，ｘ1軸とｘ2軸，あるいは，ｙ1軸とｙ2軸のいずれかの組み合わせのみに加速度センサを設置すればよい。すなわち，加速度センサ２１と２４の組み合わせまたは加速度センサ２２と２５の組み合わせのいずれかでよい。そして，全体姿勢を１通りに設定するだけでよい。この場合には，加速度センサの出力から得られる傾斜角からｚ1軸回りの姿勢角変化であるヨー角を得ることができる。
【００９３】
また，第１の全体姿勢と第２の全体姿勢との間に，加速度センサを設置した軸が基準座標系のＺ軸に平行にならない場合は，次の式２２を式２０に代入することでリンク行列Ｔ１２を得ることができる。
ヨー角φ12＝第２座標系の傾斜角−第１座標系の傾斜角 …（式２２）
すなわち，この場合には回転軸がｚ1軸であるため，姿勢角のうちθ12，ψ12は回転に用いる必要がなく，リンク行列Ｔ１２はヨー角φ12のみで表すことができる。
【００９４】
また，第１の全体姿勢と第２の全体姿勢との間に，加速度センサを設置した軸が基準座標系のＺ軸に平行になる場合は，次の式２３と式２０とからリンク行列Ｔ１２を得ることができる。
ヨー角φ12＝第２座標系の傾斜角＋第１座標系の傾斜角 …（式２３）
【００９５】
さらに，重力加速度の大きさが既知で，座標系の上下（重力方向）が判る場合では，次のような簡略化が可能である。この場合の例を図１３のブロック図に示す。この場合には，姿勢角決定装置は，加速度算出器１１８，１１９を有し，これらが座標系の上下識別用外部信号１２０を受ける。
【００９６】
重力加速度の大きさが｜Ｇ｜であれば，例えば，ｚ1軸方向の加速度ａｚは，ｘ1軸およびｙ1軸方向の加速度ａｘ，ａｙと次の式２４のような関係がある。
｜Ｇ｜＝√（ａｘ2＋ａｙ2＋ａｚ2） …（式２４）
従って，ｚ1軸方向の加速度ａｚは，加速度算出器１１８によりｘ1軸およびｙ1軸方向の加速度ａｘ，ａｙを用いて次の式２５のように表すことができる。
ａｚ＝±√（｜Ｇ｜2−ａｘ2−ａｙ2） …（式２５）
【００９７】
さらに，座標系上下識別用外部信号１２０によりｚ1軸の上下が判ることから，ａｚの符号が与えられる。以上のことから，ｚ1軸とｚ2軸とにはいずれも加速度センサは不要であり，加速度センサ２１，２２，２４，２５の４個の測定結果からｚ1軸方向とｚ2軸方向との加速度も得ることができる。また，リンク行列Ｔ１２は，式１４から算出できる。なお，この第２の条件は，例えば地表上でおよそ直立して移動するロボット等では，一般に満たされるものである。
【００９８】
次に，図３の例のように，動作時に剛な構造物を対象とした姿勢角決定装置のより一般的な構成を図１４に示す。この例のリンク機構１１０も，図８の例と同様に，第１座標系と第２座標系が，リンク１１，リンク１２，リンク１３および関節１４，関節１５で結びつけられているものとして表される。リンク機構１１０には測定対象の各座標系の各軸方向に沿って，それぞれ加速度センサが設けられる。すなわち，第１座標系のリンク１１には，ｘ1軸加速度センサ２１，ｙ1軸加速度センサ２２，ｚ1軸加速度センサ２３が設けられている。また，第２座標系のリンク１３には，ｘ2軸加速度センサ２４，ｙ2軸加速度センサ２５，ｚ2軸加速度センサ２６が設けられている。さらに，このリンク機構１０には，全体姿勢を変化させるためのモータ等を有する姿勢変化動作器２７を含む姿勢変化部が設けられている。
【００９９】
動作時に剛な構造物では，リンク機構１１０が有する関節が動作すると，動作中信号発生器１１１がリンク機構１１０が動作中であることを，リンク固定指示器１１２に送る。さらに，動作が終了するとリンク固定指示器１１２は，リンク機構１１０内にある関節の固定をリンク機構固定器１１３に指示する。これにより，リンク機構１１０の関節が固定される。
【０１００】
ここで，傾斜角を測定中に各加速度センサ２１〜２６に対し重力加速度以外の，例えば運動による並進加速度が印加されると，傾斜角の測定結果に誤差が生ずる。このため，全体姿勢計測中は傾斜ベクトル算出器３３，３４から出力された傾斜ベクトルの各要素の値が安定している必要がある。そこで静止状態判定器１１４により，タイマー１１５でカウントした時間内に傾斜ベクトル算出器３３，３４から出力された各傾斜ベクトルの各要素が一定か否かを判定し，各要素の変化量が規定値以内であれば，レジスタ３５に取得した傾斜ベクトルを記録するように指示する。
【０１０１】
次に，静止状態判定器１１４は姿勢変化指令器２８に姿勢変化を指令してもよいことを伝える。姿勢変化指令器２８は姿勢変化動作器２７に全体姿勢を変化させるように指示する。姿勢変化動作器２７は，第１座標系と第２座標系の姿勢関係が変化しないようリンク機構１１０の全体姿勢を変更する。各加速度センサ２１〜２６の出力は，サンプリング器３１，３２で常に取り込まれ，傾斜ベクトル算出器３３，３４において傾斜ベクトルが生成される。
【０１０２】
レジスタ３５は計測回数カウント器２９から得られた計測回数値と，傾斜ベクトルの各要素値を保存する。本計測は計測回数カウント器２９が規定する回数繰り返され，必要な計測回数が完了したらリンク行列算出器３７により，第１座標系からみた第２座標系のリンク行列が算出される。これをリンク行列表示器３８により表示し，さらに姿勢角算出・表示器３９において姿勢角に変換し表示する。
【０１０３】
次に，図４に示したような，リンクが柔であり，関係が柔である構造物を対象とした姿勢角決定装置について説明する。このような構造物では，全体姿勢を変化させることで第１座標系と第２座標系の姿勢関係が変化するため，全体姿勢を変化させることによって測定誤差が増える恐れがある。そこで，全体姿勢を変化させずに姿勢角を測定することが好ましい。この例における構造物と各座標系との関係を図１５に示す。また，姿勢角決定装置の概略構成を図１６のブロック図に示す。
【０１０４】
この例では，図１５に示すように，第１座標系および第２座標系上には，座標系のそれぞれの軸方向への加速度とそれぞれの軸回りの角速度を測定するための加速度センサ２１〜２６に加えて角速度センサ２１１〜２１６が設置されている。図１６に示すように，加速度センサ２１〜２６からの出力は，サンプリング器３１，３２によってサンプリングされ，角速度センサ２１１〜２１６からの出力はサンプリング器２１７，２１８によってサンプリングされる。サンプリングされた出力は，合成器２１９，２２０に導入される。合成器２１９，２２０ではそれぞれのセンサ出力を基に、現在のもっとも確からしい姿勢を算出する。
【０１０５】
さらに，姿勢行列算出器２２１，２２２は合成器２１９，２２０の出力から姿勢行列を生成する。この結果，時間の経過とともに変化する第１座標系および第２座標系の基準座標系から見た姿勢行列Ｔ１，Ｔ２を，連続的に得ることができる。このようにして得られた姿勢行列と，次の式２６から，リンク行列Ｔ１２を得ることができる。
Ｔ１２＝Ｔ1-1・Ｔ２ …（式２６）
ただし，Ｔ１：基準座標系から見た第１座標系への姿勢行列
Ｔ２：基準座標系から見た第２座標系への姿勢行列
Ｔ１２：第１座標系から見た第２座標系へのリンク行列
この例の方法を採用すれば，全体姿勢は１通りでよく，変化させる必要はない。
【０１０６】
また，あるいは，図１５に示すような例において，第１座標系および第２座標系上に，座標系のそれぞれの軸方向への加速度を測定するための加速度センサと，磁北方向を測定するための磁気センサとが設置されているものとしてもよい。加速度センサを用いれば各座標系上の各座標軸と基準座標系Ｚ軸との間の角度として傾斜角が得られ，次に傾斜角から，ロール角ψおよびピッチ角θを求めることができる。ここで姿勢角としてロール角，ピッチ角，ヨー角を用いるときは，姿勢角表記の性質から，磁気センサをそれぞれの座標系のｘ軸に設置することで，ヨー角φの計算が容易となるため好ましい。また，ｘ軸だけでなく各座標系のｙ軸，ｚ軸に設置すれば，測定精度と信頼性が向上し，より好ましい構成となる。
【０１０７】
次に，さらに別の例を説明する。基準座標系からみた第１座標系の姿勢は，姿勢行列Ｔ１で表すことができる。姿勢行列Ｔ１の具体的な要素は上記した式２を展開して得られる次の式２７で表される。
【０１０８】
【数１０】
【０１０９】
このとき，姿勢行列Ｔ１の第３行第１列の成分は，第１座標系のｘ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。また，第３行第２列の成分は，第１座標系のｙ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。また，第３行第３列の成分は，第１座標系のｚ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。すなわち図９に示すように，第１座標系の各軸と基準座標系のＺ軸とのなす傾斜角度をγ1〜γ3とおけば，傾斜角度の方向余弦は次の式２８〜式３０で表される。
【０１１０】
−ｓｉｎθ1＝ｃｏｓγ1 …（式２８）
ｃｏｓθ1ｓｉｎψ1＝ｃｏｓγ2 …（式２９）
ｃｏｓθ1ｃｏｓψ1＝ｃｏｓγ3 …（式３０）
さらに，これらの式から，基準座標系から見た第１座標系のロール角θ1，ピッチ角ψ1は，次の式３１，式３２で表される。
θ1＝ｓｉｎ-1（−ｃｏｓγ1） …（式３１）
ψ1＝ｔａｎ-1（ｃｏｓγ2／ｃｏｓγ3） …（式３２）
【０１１１】
加速度センサの出力は座標系の姿勢が基準座標系のＺ軸回りに回転しても変化しないため，ヨー角φを得ることはできない。しかし，磁気センサを用いて磁北を求めることで，ヨー角φを得ることができる。具体的には，基準座標系からみた磁北方向と，第１座標系から見た磁北方向の差異分をヨー角φ1と置き，式３１〜３２および式２を用いて姿勢行列Ｔ１を求める。次に基準座標系からみた磁北方向と第２座標系から見た磁北方向の差異分をヨー角φ2と置き，第２座標系に関する式３１〜３２および式２から姿勢行Ｔ２を求める。姿勢行列Ｔ１および姿勢行列Ｔ２を式２６に適用することで，リンク行列Ｔ１２を得ることができる。
【０１１２】
また別の方法として，磁気センサを用いて第１座標系から見た第２座標系へのヨー角を直接求めてもよい。また本形態において磁気センサの代わりに，外部からの情報として，ＧＰＳ（Global Positioning System）や，マーカーを用いてもよい。そして，次の式３３において，磁気センサまたは外部情報で得られた第１座標系から見た第２座標系のヨー角を（φ2−φ1）に代入する。
【０１１３】
【数１１】
【０１１４】
また姿勢角θ1，θ2，ψ1，ψ2を加速度センサの出力から求める。本手法によりリンク行列Ｔ１２を求めることができる。このようにすれば，第１座標系および第２座標系に設置する加速度センサの個数はそれぞれ２個でよく，測定精度を向上することができる。また全体姿勢は１通りでよい。
【０１１５】
次に，さらに別の姿勢角決定装置の概略構成を図１７のブロック図に示す。第１座標系および第２座標系上には，座標系のそれぞれの軸方向への加速度を測定するための加速度センサ２１〜２６と，それぞれの軸回りの角速度を測定するための角速度センサ２１１〜２１６と，磁北方向を測定するための磁気センサ２３１〜２３６を設置している。加速度センサ２１〜２６と角速度センサ２１１〜２１６を用いればロール角ψ，ピッチ角θ，ヨー角φを求めることができるが，角速度センサ２１１〜２１６の出力の時間的な変化により，ヨー角φが変化する場合がある。
【０１１６】
そこで角速度センサ２１１〜２１６の出力を磁気センサ２３１〜２３６の出力と比較して統合することによってヨー角φの変化量を低減することができる。また反対に，磁気センサ２３１〜２３６によって求められたヨー角φは外部磁界の影響を受けるが，この影響を角速度センサ２１１〜２１６の出力と比較して統合することによって低減することができる。
【０１１７】
この手法により角速度センサ２１１〜２１６と磁気センサ２３１〜２３６が互いの利点によって補完するため，姿勢角から得られる姿勢行列Ｔ１およびＴ２が角速度センサ２１１〜２１６の出力変動や外部磁界の影響を受にくくなり，姿勢行列Ｔ１およびＴ２から求まるリンク行列Ｔ１２の測定精度を向上することができる。なおここでは柔軟な構造物について説明したが，常に剛な構造物や動作中のみ剛となるリンクをもつ構造物へ適用してもよい。またこの場合の全体姿勢は１通りでよい。
【０１１８】
さらに，図４に示したような，リンクが柔であり，関係が柔である構造物を対象とした別の姿勢角決定装置の例について説明する。外部から姿勢角φが与えられる場合の姿勢角決定装置の概略構成を図１８に示す。このようなものでは，第１座標系および第２座標系上に設置された加速度センサ２１，２２，２４，２５からの加速度を用い，上記の式３１，式３２からロール角ψおよびピッチ角θが求まる。またヨー角は外部姿勢角信号２４１として与えられる。これら３つの姿勢角をもとに，姿勢行列算出器２２１，２２２において姿勢行列が得られる。この結果をレジスタ３５に保存したのちリンク行列算出器３７においてリンク行列を求める。
【０１１９】
なお，以上の各例において述べた加速度センサ，角速度センサ，磁気センサは，必ずしも第１座標系および第２座標系上に常設する必要はない。初期状態ではこれらセンサが第１座標系のみに設置され，ある全体姿勢において第１座標系の姿勢を計測後，これらセンサを第２座標系に移動し，同じ全体姿勢において第２座標系の姿勢を計測するのに用いてもよい。
【０１２０】
以上のような姿勢角決定装置が適用されるリンク機構の具体例を図１９に示す。この例では，リンク１１，１２，１３と，リンク同士を接続する関節１４，１５を有している。各リンク上には各リンクの姿勢を規定するために，それぞれ対応する座標系が設けられている。リンク１１上の部位にはリンク１１と一体となって動く第１座標系が，リンク１２上の部位にはリンク１２と一体となって動く第２座標系が，またリンク１３上の部位にはリンク１３と一体となって動く第３座標系が設置されている。
【０１２１】
ここで，関節１４および関節１５にはそれぞれのリンク角度を変化させるアクチュエータとしてのモータと，関節が接続するリンクのリンク角度を測定するエンコーダが配置されている。通常エンコーダは小型化，軽量化のために，絶対角度タイプではなく相対角度タイプが使われる。相対角度タイプでは初期値が不定であるため，リンク機構を初期化する前の状態では正しいリンク角度は得られない。このためリンク機構利用前には，各関節をその最大回転可能角度まで回転させ，ピンなどの機械的な突起部に押し当てることで関節角度の初期値を規定する初期化工程が必要である。この初期化工程には時間がかかること，またリンク機構の重量バランスが大きく崩れるため体勢が不安定になることといった課題があった。
【０１２２】
これに対して，本発明を利用した初期化工程の実施例について述べる。図１９に示すように，このリンク機構には，計測中には第１座標系と第２座標系間の姿勢関係が一定となるよう，関節１４および関節１５にリンク機構固定器１６，１７が設置されている。リンク機構固定器１６，１７は，各リンク間角度を固定する機能を持つ。リンク機構固定器１６，１７としては，図２０，図２１，図２２に示す三種類の手法がある。
【０１２３】
図２０に示した手法では，リンク１２には，リンク１２とリンク１３との間の角度変化を計測するためのエンコーダ３１１と，リンク２とリンク３の間の角度を変化させることができるモータ３１２と，モータ３１２を制御するためのコントローラ３１３が設置されている。そして，各全体姿勢についてリンク間の姿勢関係を計測している間は，継続的にエンコーダ３１１の出力をモニタする。そしてリンク固定指示器１１２（図１１参照）の発する命令により，コントローラ３１３はモータ３１２に対してエンコーダ３１１の出力が常に一定となるようにサーボ方式による位置制御指令を出す。この結果，リンク１２とリンク１３の間の角度を位置制御により一定とすることができる。
【０１２４】
このサーボ方式による位置制御によるリンク間角度の固定手法には，はじめから関節にエンコーダ３１１およびモータ３１２が設置されている場合，追加装置が不要である利点がある。また本手法は電気的にリンク間の姿勢関係を固定しているため機械的な切り替え時間が不要であり，リンク間の姿勢関係測定といった初期化工程終了後，即座にリンク機構を利用することができる。このため初期化工程の効率化が可能となる。
【０１２５】
またあるいは，図２１に示すように，リンク１２から延びリンク１３に押し当てることでリンク１２に対するリンク１３の動きを止めるための摩擦を用いたクラッチ３１４と，クラッチ３１４を作動させるためのアクチュエータ３１５が設置されているものとしてもよい。各全体姿勢についてリンク間の姿勢関係を計測している間は，リンク固定指示器１１２の発する命令によりクラッチ３１４の動作を有効とすることで，リンク１２に対するリンク１３の角度関係を固定できる。クラッチ３１４による固定方法は，エンコーダ３１１やモータ３１２を使用せずリンク固定機構が簡単になるという利点がある。
【０１２６】
またあるいは，図２２に示すように，リンク１２とリンク１３の間に，ロッド＋ピン３１６が設置されているものとしてもよい。このロッド＋ピン３１６によりリンク同士を機械的に固定する。本固定手法は，他の手法にくらべ最も強固であるため，全体姿勢を変化させてもリンク間の姿勢関係を最も精度良く一定とすることができる。またロッド＋ピン３１６による固定方法は電気信号を必要とせず，もっとも容易かつ確実な手法である。本手法による固定手法は，ロッド＋ピン３１６を容易に取り外せないことから，リンク姿勢関係の計測後も，継続的にリンク間の姿勢関係を変えない用途に向いている。
【０１２７】
これらの固定手法を用いて求めた第１座標系から第２座標系へのリンク行列をＴ１，第２座標系から第３座標系へのリンク行列をＴ２とおく。このとき，第１座標系から見た第３座標系へのリンク行列Ｔ１２は，次の式３４で表される。
Ｔ１２＝Ｔ１・Ｔ２ …（式３４）
【０１２８】
このようにすれば，リンク行列Ｔ１２が求まるので，式３４の逆関数を求めることで，関節１４および関節１５における関節角度を算出できる。逆関数の求め方の例は，「ロボット・マニピュレータ コロナ社 吉川恒夫訳」に記載されている。さらに，各関節角度の初期値が求まれば，初期化工程が終了した後の，任意の時間経過後の関節角度は，関節角度の初期値と関節角度の変化分を用いて，次の式３５，式３６によって表される。このとき，各関節角度の変化分は，相対角度タイプのエンコーダによって得られる。
関節１４の角度＝関節１４の角度の初期値＋関節１４の角度の変化分 …（式３５）
関節１５の角度＝関節１５の角度の初期値＋関節１５の角度の変化分 …（式３６）
【０１２９】
本実施例によれば，リンク機構の初期化工程をリンク機構の姿勢を大きく変化させなくとも，第１座標系および第３座標系の間の姿勢関係を容易にかつ高精度に求めることができる。また得られた座標系間の姿勢関係を利用して，各関節に設置されたリンク機構の初期化工程を簡便に行うことができる。
【０１３０】
以上詳細に説明したように本形態の姿勢角決定装置２０によれば，姿勢関係を求めようとする両座標系の各軸方向にそれぞれ加速度センサ２１〜２６を備えているので，これらの測定結果から各軸の傾斜方向を算出することができる。さらに，この測定を姿勢を変化させて１〜３回行い，リンク行列算出器３７により，リンク行列Ｔ１２を算出することができる。従って，リンク行列Ｔ１２から，リンク構造に含まれる両座標系の間の姿勢角が求められる。すなわち，図面や実測によることなく，複雑な関係にある座標系間であってもその姿勢関係を容易に，精度良く求めることができる。
【０１３１】
なお，本形態は単なる例示にすぎず，本発明を何ら限定するものではない。したがって本発明は当然に，その要旨を逸脱しない範囲内で種々の改良，変形が可能である。
【図面の簡単な説明】
【０１３２】
【図１】本形態の姿勢角決定装置の対象部材の例を示す説明図である。
【図２】対象部材の具体例を示す説明図である。
【図３】対象部材の具体例を示す説明図である。
【図４】対象部材の具体例を示す説明図である。
【図５】本形態の姿勢角決定装置の概略構成を示すブロック図である。
【図６】対象部材の具体例を示す説明図である。
【図７】対象部材の具体例を示す説明図である。
【図８】対象部材のリンク構成を示す説明図である。
【図９】基準座標系のＺ軸と他の座標系の軸との関係を示す説明図である。
【図１０】本形態の姿勢角決定方法のアルゴリズムを示すフローチャートである。
【図１１】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１２】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１３】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１４】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１５】対象部材のリンク構成を示す説明図である。
【図１６】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１７】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１８】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１９】リンク機構の具体例を示す説明図である。
【図２０】リンク機構固定器の具体例を示す説明図である。
【図２１】リンク機構固定器の具体例を示す説明図である。
【図２２】リンク機構固定器の具体例を示す説明図である。
【符号の説明】
【０１３３】
１０ リンク機構
１１，１２ リンク
２０ 姿勢角決定装置
２１，２２，２３，２４，２５，２６ 加速度センサ
２８ 姿勢変化指令器
３３ 傾斜ベクトル算出器
３４ 傾斜ベクトル算出器
３６ 傾斜行列算出器
３７ リンク行列算出器
３９ 姿勢角算出・表示器
【技術分野】
【０００１】
本発明は，リンク機構によって接続された２つの座標系間の姿勢関係を決定する姿勢角決定装置およびその決定方法に関する。さらに詳細には，２つの座標系間の姿勢関係を３つの姿勢角（ヨー角φ，ピッチ角θ，ロール角ψ）で表すための姿勢角決定装置およびその決定方法に関するものである。
【背景技術】
【０００２】
リンク機構によって接続された複数部材の姿勢制御を行う場合，一般に，各部材ごとにその位置座標を表す座標系を設け，まず部材の姿勢をその座標によって特定する。その後，座標間の姿勢関係を３つの姿勢角を用いて変換することにより，共通の座標で表している。従来，このような座標間の姿勢関係を求めるには，図面に基づいて寸法を計測したり，三次元測定器によって角度を実測することにより行っていた。
【０００３】
例えば，特許文献１には，人間の大腿部，下腿部等の各部を剛体要素（リンク）として，各剛体要素に要素座標系を設定した剛体リンクモデルを作成し，そのモデルを用いて移動体の関節モーメントを推定する方法が開示されている。本文献に記載の技術では，例えば股関節に設けられた関節変位センサや発光／受光器等の出力を基にして，座標変換のための変換テンソルを作成し，それを用いて各関節要素や各剛体要素の位置が求められるとされている。
【特許文献１】特開２００５−８１５３７号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら，前記した従来の図面や実測による方法では，２つの座標系の配置が複雑になるにつれて作業が繁雑となり，姿勢角の決定が困難であった。特に，座標系同士がねじれの関係にあって，図面上でそれらの関係を明記できない場合等では，図面から姿勢の関係を読み取ることができない。また，座標系間の距離が長い場合あるいは，座標系間に障害物がある場合等では，三次元測定器による計測が困難であった。これらの条件が重なると，図面によっても実測によっても姿勢関係が求められないという問題点があった。また，座標系の設置について角度誤差がある場合では，図面上から得られる座標系間の姿勢関係では，精度が低いものとなるという問題点があった。
【０００５】
また，特許文献１に記載の技術は，例えば股関節の関節変位センサ等によりリンク間の回転角が把握されている場合の変換方法である。さらに，股関節，膝関節，足首関節の３つの関節を通る脚平面を想定してその上での２次元量を利用することにより，移動しつつある人間の脚体の関節に作用するモーメントを推定するものであり，このままではリンク間の姿勢関係の取得には利用できない。
【０００６】
本発明は，前記した従来の技術が有する問題点を解決するためになされたものである。すなわちその課題とするところは，複雑な関係にある座標系間であってもその姿勢関係を容易に，精度良く求めることのできる姿勢角決定装置およびその決定方法を提供することにある。
【課題を解決するための手段】
【０００７】
この課題の解決を目的としてなされた本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得する傾斜角取得部と，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせる姿勢変化指示部と，姿勢変化指示部の指示による各全体姿勢における各リンクについて，傾斜角取得部で取得された各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，各リンクについて，傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものである。
【０００８】
本発明の姿勢角決定装置によれば，傾斜角取得部によって，各リンクに設定された３軸直交座標系の各軸方向と重力ベクトルの方向との間の角度である傾斜角が取得される。また，姿勢変化指示部によって，リンク機構は最大３通りの全体姿勢を取るように指示される。そして各全体姿勢において傾斜ベクトル算出部によって傾斜ベクトルが求められ，続いて，傾斜行列算出部によって傾斜ベクトルより傾斜行列が求められる。さらに，求められた傾斜行列に基づいて，姿勢角を表すリンク行列が算出される。従って，複雑な関係にある座標系間であってもその姿勢関係を容易に，高価な機構を用いず，小型な装置によって精度良く求めることが出来る。
【０００９】
なお，「傾斜角取得部」，「傾斜ベクトル算出部」，「傾斜行列算出部」は，両リンクについてまとめて処理するものであっても良いし，各リンクのそれぞれに設けられているものであっても良い。また，傾斜ベクトルは傾斜角を成分とするベクトルである。傾斜行列は，傾斜ベクトルを並べたものであり，すなわち，傾斜角を軸および全体姿勢により２次元に配列した行列である。また，傾斜行列は，その座標系の傾斜角を表すものであるが，座標系から別の座標系を見たときの姿勢関係までは表現しない。
【００１０】
また，傾斜角取得部は，各座標系の軸の数（３個）×リンクの数（２個）の都合６個の傾斜角を取得する。傾斜ベクトル算出部は，全体姿勢の数（最大３種）×リンクの数（２個）の傾斜ベクトルを算出する。傾斜行列算出部は，リンクの数（２個）の傾斜行列を算出する。
【００１１】
さらに本発明では，傾斜角取得部が，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた水準器を用いて得ることが望ましい。このようにすれば，傾斜角を直接的に得ることができる。
【００１２】
また本発明では，傾斜角取得部が，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた加速度センサの出力値から求めるものであっても良い。このようにすれば，傾斜角を容易に得ることができる。
【００１３】
また本発明では，傾斜角取得部が，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めるものとしてもよい。この場合，平方根の符号は，座標系の上下関係として外部信号またはＧＰＳ，マーカー等で与えられることが好ましい。このようにすれば，必要な加速度センサの個数や全体姿勢の変化の回数を減らすことができる。
【００１４】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得する加速度値取得部と，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせる姿勢変化指示部と，姿勢変化指示部の指示による各全体姿勢における各リンクについて，加速度値取得部で取得された各軸方向の加速度値から傾斜角を求める傾斜角測定部と，姿勢変化指示部の指示による各全体姿勢における各リンクについて，傾斜角測定部で求められた各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，各リンクについて，傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであってもよい。
【００１５】
すなわち，第１リンクの座標系と第２リンクの座標系との間に，１軸が互いに平行であるという関係がある場合は，処理をやや簡略化できる。一般的には加速度値取得部は，各リンクごとに軸の数（ここでは３軸なので３個）だけ加速度を取得することが必要であるが，この条件があれば，その平行な１軸を除いた２軸の加速度値を取得するのみでよい。すなわち，本発明では，加速度値取得部は，２軸×リンク数の都合４個の加速度値を取得する。従って，例えば加速度センサの出力値から加速度値を求める場合では，このような条件が満たされる場合には，一般的な場合より加速度センサの数を減らすことができる。さらに，全体姿勢の数も減らすことができる。
【００１６】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得する加速度値取得部と，各リンクについて，加速度値取得部で取得された加速度値から傾斜角を求める傾斜角測定部と，傾斜角測定部により求められた各リンクの傾斜角の差より姿勢角を算出する姿勢角計算部とを有するものであっても良い。
【００１７】
すなわち，第１リンクの座標系と第２リンクの座標系との間に，１軸が互いに平行でありかつ水平であるという関係がある場合は，処理をさらに簡略化できる。加速度値取得部は，各リンクに１個ずつの都合２個の加速度値を取得すればよい。従って，加速度センサの数は２個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００１８】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクに設定された座標系間のヨー角を取得する第１ヨー角取得部と，加速度値取得部で取得された加速度値と，第１ヨー角取得部で取得されたヨー角とから，姿勢角を表すリンク行列を算出する第２リンク行列算出部とを有するものであっても良い。
【００１９】
すなわち，加速度値取得部は，各リンクに１個ずつの都合２個の加速度値を取得すればよい。従って，加速度センサの数は２個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。ここで，ヨー角とは，２つの座標系間の１つの軸回りの姿勢角変化である。
【００２０】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得する第２ヨー角取得部と，各リンクについて，加速度値取得部で取得された加速度値と，第２ヨー角取得部で取得されたヨー角とから，基準座標系に対する姿勢行列を求める第１姿勢行列算出部と，第１姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであっても良い。
【００２１】
すなわち，加速度値取得部は，各リンクに１個ずつの都合２個の加速度値を取得すればよい。従って，加速度センサの数は２個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００２２】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクに設けられそれぞれにおける角速度値を取得する角速度値取得部と，各リンクについて，加速度値取得部で取得された加速度値と，角速度値取得部で取得された角速度値とから，基準座標系に対する姿勢行列を求める第２姿勢行列算出部と，第２姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであっても良い。
【００２３】
すなわち，加速度値取得部は，各リンクに３個ずつの都合６個の加速度値を取得すればよい。また，角速度値取得部は，各リンクに３個ずつの都合６個の角速度値を取得すればよい。また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００２４】
また，本発明の姿勢角決定装置は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置であって，各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，各リンクに設けられた磁気センサと，各リンクについて，加速度値取得部で取得された加速度値と，磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求める第３姿勢行列算出部と，第３姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有するものであっても良い。
【００２５】
すなわち，加速度値取得部は，各リンクに２個ずつの都合４個の加速度値を取得すればよい。従って，加速度センサの数は４個で良く，また全体姿勢は１通りで良く，変化させる必要はないので，この場合は姿勢変化指示部を有する必要はない。
【００２６】
さらに本発明では，各リンクに設けられた磁気センサを有し，第２姿勢行列算出部は，加速度値取得部で取得された加速度値と，角速度値取得部で取得された角速度値と，磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求めることが望ましい。
【００２７】
さらに本発明では，姿勢変化動作部が，姿勢変化装置を内蔵するロボットであり自ら全体姿勢を形成することが望ましい。
【００２８】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得し，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせ，指示による各全体姿勢における各リンクについて，取得した各軸方向の傾斜角から傾斜ベクトルを求め，各リンクについて，全体姿勢ごとに求めた傾斜ベクトルにより傾斜行列を求め，求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００２９】
また本発明では，傾斜角を，第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めることが望ましい。
【００３０】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得し，第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせ，指示による各全体姿勢における各リンクについて，取得した各軸方向の加速度値から傾斜角を求め，指示による各全体姿勢における各リンクについて，求めた各軸方向の傾斜角から傾斜ベクトルを求め，各リンクについて，全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求め，求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３１】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得し，各リンクについて，取得した加速度値から傾斜角を求め，求めた各リンクの傾斜角の差より姿勢角を算出する姿勢角決定方法にも及ぶ。
【００３２】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクに設定された座標系間のヨー角を取得し，取得した加速度値と，取得したヨー角とから，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３３】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得し，各リンクについて，取得した加速度値と，取得したヨー角とから，基準座標系に対する姿勢行列を求め，求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３４】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクにおける角速度値を取得し，各リンクについて，取得した加速度値と，取得した角速度値とから，基準座標系に対する姿勢行列を求め，求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３５】
また本発明は，１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法であって，各リンクにおける加速度値を取得し，各リンクにおける磁北方向との交差角の情報を磁気センサで取得し，各リンクについて，取得した加速度値と，取得した磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求め，求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する姿勢角決定方法にも及ぶ。
【００３６】
また本発明では，姿勢変化動作部が，姿勢変化装置を内蔵するロボットであり，自ら全体姿勢を形成することが望ましい。
【発明の効果】
【００３７】
本発明の姿勢角決定装置およびその決定方法によれば，複雑な関係にある座標系間であってもその姿勢関係を容易に，精度良く求めることができる。
【発明を実施するための最良の形態】
【００３８】
以下，本発明を具体化した最良の形態について，添付図面を参照しつつ詳細に説明する。本形態は，２つの座標系間の姿勢関係を得るための姿勢角決定装置である。ここで，姿勢角とは，例えばリンク等のなす角度（ヨー角φ，ピッチ角θ，ロール角ψ）をある座標系で表したものである。そして姿勢関係とは，その姿勢角を，別の座標系を基準として見た姿勢角に変換するための関係式である。
【００３９】
まず，本形態の姿勢角決定装置によって姿勢角が決定される対象であるリンク機構の例を図１に示す。リンク機構とは，リンクと関節との２つの部位から構成されているものを指す。この図に示すリンク機構１０は，３つのリンク１１，１２，１３を有している。また，リンク１１とリンク１２とが関節１４で，リンク１２とリンク１３とが関節１５でそれぞれ接続されている。関節１４は３つの回転自由度を持つ。関節１５も同じく３つの回転自由度を持つ。
【００４０】
またリンク機構上の二箇所には，第１座標系（ｘ１，ｙ１，ｚ１）と第２座標系（ｘ２，ｙ２，ｚ２）が設置されている。図１では第１座標系はリンク１１に固定されており，第２座標系はリンク１３に固定されている。第１座標系と第２座標系は，関節１４と関節１５を固定しておけば，互いに所定の姿勢関係を有している。関節１４または関節１５を動かすと，座標系間の姿勢関係は変化する。また両座標系間の姿勢関係を固定したままリンク機構１０の全体としての姿勢を変更することも可能である。本形態の姿勢角決定装置は，このようなリンク機構１０に設けられた２つの座標系間の姿勢関係を得るものである。
【００４１】
次にリンク機構１０の具体的な例をいくつか示す。リンク自体は，剛なものと柔なものとに大きく分けることができる。また，リンク同士の関係すなわち関節の状態としては，常に固定されている関係，動作中あるいは一定期間のみ固定される関係，常に柔軟な関係がある。固定されているとは，それらのリンクの間の姿勢は変化しないことを意味する。柔軟であるとは，リンク間の姿勢が変化することを意味する。
【００４２】
まず，図２にリンクが剛であり関係が固定されている例を示す。これは，１つの剛なる物体上の異なる２面に設置された２つの座標系の関係である。すなわち，図２の例は，剛なリンクと固定した関節からなるリンク機構で表すことができる。この例では，第１面上に設置した第１座標系と第２面上に設置した第２座標系の姿勢関係は変化しない。またこの例では隣り合った面同士が直交関係になく，二つの座標系間の姿勢関係がネジレの位置にあるため，座標系間の姿勢関係を容易には求めることができない。この例で表すことができる事例としては，部品を組み立てた直後や部品を所定の場所に設置した直後で，部品内の面同士や部品と設置面との直交がでていない場合や，部品の形状に設計値からのズレがある場合などがある。
【００４３】
次に，図３にリンクが剛であり，関係が一定期間のみ固定される例を示す。これは，可動リンクを持つロボットである。図３の例では，剛なリンクと、固定および非固定を自由に可変できる関節から成り立っている。この例で表すことができる事例としては，ロボットのリンク間角度が初期状態では不定であるが，ロボットの動作中は関節がサーボ機構によってソフト的に，またはネジなどによりハード的に固定されることでリンク機構が剛な関係であると見なせる場合などがある。固定された状態では各リンク上に置かれた二つの座標系間の関係は変化しない。図３の例では，第１座標系はロボットの胴体に，また第２座標系は腕上に設置されている。ただし関節部には関節角度を計測する装置がなく，関節角度を直接測定することはできない。
【００４４】
また上記のタイプの別の例として、ロボットのリンク間角度が初期状態では不定であるが，リンク間角度の初期値が与えられるという初期化工程を経ることで、初期化後には関節部に設置された関節角度測定器により、関節角度が得られるものもある。この場合は関節が固定されていなくともよい。
【００４５】
次に，図４にリンクが柔であり，関係が柔であるものの例を示す。図４の例は，常に柔らかく変形する構造体であり，このような構造体上の異なる２点に設置された２つの座標系の関係は，常に柔な関係である。従って，図４の例は，柔軟なリンク、またはリンクは剛であるが固定されない関節を持つリンク機構で表すことができる。リンク上に設置した第１座標系と第２座標系の姿勢関係は，リンクに働く重力方向の変化によって変化する。この例で表すことのできる事例としては，リンクがゴムのように容易に変形する物質で作られていたり，リンクを構成する板の剛性が低い場合などがある。なお，以上の説明に用いたリンクおよび関節は，第１座標系と第２座標系の関係をわかりやすく求めるために用いたものであり，実際にリンクあるいは関節が第１座標系および第２座標系の間に存在しなくてもよい。
【００４６】
次に，固定リンクをもつリンク機構についての姿勢角を決定するための姿勢角決定装置２０の概略構成を，図５のブロック図に基づいて説明する。これは，図２に示した変形しないリンク機構を対象とするものである。図２における第１座標系と第２座標系が，図１に示すリンク機構１０で表されるリンク１１，リンク１２，リンク１３および関節１４，関節１５で結びつけられているものとすることができる。
【００４７】
この例では，図５に示すように，リンク機構１０には測定対象の各座標系の各軸方向に沿って，それぞれ加速度センサが設けられる。すなわち，第１座標系のリンク１１には，ｘ1軸加速度センサ２１，ｙ1軸加速度センサ２２，ｚ1軸加速度センサ２３が設けられている。また，第２座標系のリンク１３には，ｘ2軸加速度センサ２４，ｙ2軸加速度センサ２５，ｚ2軸加速度センサ２６が設けられている。
【００４８】
さらに，このリンク機構１０には，全体姿勢を変化させるためのモータ等を有する姿勢変化動作器２７が設けられている。この姿勢角決定装置２０で姿勢角を算出するために，姿勢変化動作器２７を動作させて，測定対象であるリンク機構１０の姿勢を適宜変化させることが必要となる。そのために，姿勢角決定装置２０はさらに，姿勢変化指令器２８および計測回数カウント器２９を有している。計測回数カウント器２９は，リンク機構１０の全体姿勢を変化をさせた回数をカウントする。そして，必要回数の姿勢変化が終了するまで，姿勢変化指令器２８が姿勢変化動作器２７に指令を出し，リンク機構１０の全体姿勢を変化させる。
【００４９】
また，姿勢角決定装置２０には，ｘ1軸加速度センサ２１，ｙ1軸加速度センサ２２，ｚ1軸加速度センサ２３の傾斜角出力を受けて，その値をサンプリングするサンプリング器３１と，ｘ2軸加速度センサ２４，ｙ2軸加速度センサ２５，ｚ2軸加速度センサ２６の出力を受けて，その値をサンプリングするサンプリング器３２とを有する。ここでサンプリングされる値はそれぞれ，傾斜角の各軸方向の余弦値となっている。
【００５０】
また，姿勢角決定装置２０は，サンプリング器３１の結果から傾斜ベクトルを算出する傾斜ベクトル算出器３３と，サンプリング器３２の結果から傾斜ベクトルを算出する傾斜ベクトル算出器３４とを有する。これらの傾斜ベクトルは，各座標系の重力方向に対する傾斜を表す。さらに，姿勢角決定装置２０は，傾斜ベクトル算出器３３と傾斜ベクトル算出器３４との算出結果を受けるレジスタ３５，レジスタ３５の出力を受けて各傾斜ベクトルから傾斜行列を算出する傾斜行列算出器３６を有する。
【００５１】
ここで，基準座標系から見た第１座標系への変換を姿勢行列Ｔ１，基準座標系から見た第２座標系への変換を姿勢行列Ｔ２としたとき，姿勢行列Ｔ１から姿勢行列Ｔ２への変換行列をリンク行列Ｔ１２とする。姿勢角決定装置２０は，リンク行列算出器３７，リンク行列表示器３８，姿勢角算出・表示器３９を有している。リンク行列算出器３７は，傾斜行列算出器３６の結果を受けて，リンク行列Ｔ１２を算出する。算出されたリンク行列は，リンク行列表示器３８によって表示される。さらに，姿勢角算出・表示器３９は，リンク行列Ｔ１２を用いて姿勢角を算出し，表示する。
【００５２】
この姿勢角決定装置２０を用いて姿勢角を求める手順を簡単に説明する。まず，第１座標系と第２座標系との間には，リンク１１〜１３があり，各リンクの関係の設定が終了すると２つの座標系間の姿勢関係が固定される。この固定された姿勢関係を保持したまま，これらのリンク機構１０の全体姿勢を条件に応じて最大３通りに変化させる。ここでは，まず最も一般的な例として，３通りに変化させる場合を説明する。
【００５３】
全体姿勢を変化させた後，，その全体姿勢における加速度センサ２１〜２６の出力結果を取得する。これにより，最大３種類の取りうる姿勢についての傾斜角のデータを得る。すなわち，計測回数カウント器２９によって計測回数をカウントしつつ，姿勢変化指令器２８より姿勢変化動作器２７へ姿勢を変化させる指示を送出する。そして，各全体姿勢において，計６個の加速度センサ２１〜２６によって，各軸方向の加速度を測定する。
【００５４】
次に，各全体姿勢ごとに，各加速度センサ２１〜２３の出力から，サンプリング器３１によって，第１座標系の各軸の重力方向に対する傾斜角を抽出する。また，各全体姿勢ごとに，各加速度センサ２４〜２６の出力から，サンプリング器３２によって，第２座標系の各軸の重力方向に対する傾斜角を抽出する。さらに，これらの傾斜角から，傾斜ベクトル算出器３３によって，第１座標系の傾斜ベクトルを算出する。また，同様に，傾斜ベクトル算出器３４によって，第２座標系の傾斜ベクトルを算出する。
【００５５】
これにより，各全体姿勢に対応する計３種類の傾斜ベクトルが得られるので，傾斜行列算出器３６によって傾斜ベクトルを並べた傾斜行列を求める。続いて，この傾斜行列に基づいてリンク行列算出器３７によってリンク行列Ｔ１２を算出する。さらに，リンク行列表示器３８によってリンク行列Ｔ１２を表示する。さらに，リンク行列Ｔ１２から姿勢角算出・表示器３９によって，第１座標系から見た第２座標系の姿勢角（ヨー角φ，ピッチ角θ，ロール角ψ）が求められる。
【００５６】
次に，各手順について詳しく説明する。まず，図２および図３に示した対象の例について，基準座標系（Ｘ，Ｙ，Ｚ）から見た第１座標系への姿勢行列Ｔ１，基準座標系から見た第２座標系への姿勢行列Ｔ２，第１座標系からみた第２座標系へのリンク行列Ｔ１２の関係を，図６および図７に示す。ここでは，基準座標系は時間によりその姿勢や位置が変化せず，そのＺ軸は重力ベクトルの逆方向を向いている。これら図６および図７の例は，図８に示すリンク構成として表すことができる。
【００５７】
まず，リンク行列Ｔ１２と各姿勢行列Ｔ１，Ｔ２との関係は次の式１で表される。
Ｔ２＝Ｔ１・Ｔ１２ …（式１）
この姿勢行列Ｔ１を，基準座標系から見た第１座標系の姿勢角（ヨー角φ1，ピッチ角θ1，ロール角ψ1）を用いて表すと，次の式２のようになる。
Ｔ１＝ＲＰＹ（φ1，θ1，ψ1）
＝Ｒｏｔ（Ｚ，φ1）Ｒｏｔ（Ｙ，θ1）Ｒｏｔ（Ｘ，ψ1） …（式２）
これは，まず基準座標系のＺ軸回りにφ1回転させ，次にＺ軸回りに回転した後のＹ軸回りにθ1回転させ，次にＹ軸回りに回転した後のＸ軸回りにψ1回転させることで，第１座標系が得られることを示している。
【００５８】
同様に，姿勢行列Ｔ２を，基準座標系から見た第２座標系の姿勢角（ヨー角φ2，ピッチ角θ2，ロール角ψ2）を用いて表すと，次の式３のようになる。
Ｔ２＝ＲＰＹ（φ2，θ2，ψ2）
＝Ｒｏｔ（Ｚ，φ2）Ｒｏｔ（Ｙ，θ2）Ｒｏｔ（Ｘ，ψ2） …（式３）
また，リンク行列Ｔ１２を，第１座標系から見た第２座標系の姿勢角（ヨー角φ12，ピッチ角θ12，ロール角ψ12）を用いて表すと，次の式４のようになる。
Ｔ１２＝ＲＰＹ（φ12，θ12，ψ12）
＝Ｒｏｔ（ｚ1，φ12）Ｒｏｔ（ｙ1，θ12）Ｒｏｔ（ｘ1，ψ12）…（式４）
これは，第１座標系から各軸のまわりに，φ12，θ12，ψ12の順番で回転させると，第２座標系が得られることを表している。
【００５９】
ここで，図８に示すように，リンク１１とリンク１３には，各座標系の各軸方向への加速度を検出する加速度センサ２１〜２６が設けられている。さらに基準座標系のＺ軸が重力と逆方向に設定されているので，各加速度センサ２１〜２６の出力から，第１，第２座標系の各軸の基準座標系のＺ軸とのなす角が求められる。これは，運動していないリンク機構１０では，重力の影響のみが各加速度センサ２１〜２６で測定されるからである。これらの各加速度センサ２１〜２６の出力から，第１，第２座標系の各姿勢行列Ｔ１，Ｔ２を求める方法について説明する。
【００６０】
まず，姿勢行列Ｔ１，姿勢行列Ｔ２，リンク行列Ｔ１２を一般形で要素表示すると，以下の式５，式６，式７に示すように表すことができる。
【００６１】
【数１】
【００６２】
【数２】
【００６３】
【数３】
【００６４】
これらを用いて，式１を要素表示すると，以下の式８となる。
【００６５】
【数４】
【００６６】
ここで，例えば姿勢行列Ｔ１の要素ｃ1は，第１座標系のｘ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。すなわち，図９に示すように，第１座標系のｘ1軸と基準座標系のＺ軸とのなす傾斜角度をγ1とすると，次の式９で表される。
ｃ1＝ｃｏｓγ1 …（式９）
同様に，姿勢行列Ｔ１，姿勢行列Ｔ２のうち，最下段の各要素が各座標系の各軸と基準座標系のＺ軸との間の方向余弦を表しているので，各加速度センサ２１〜２６の出力値からｃ1，ｆ1，ｉ1，ｃ2，ｆ2，ｉ2が求まる。
【００６７】
そこで，式８の要素表示のうち，ｃ1，ｆ1，ｉ1，ｃ2，ｆ2，ｉ2に着目すると，以下の式１０〜式１２が得られる。
ｃ2＝ｃ1ｐ＋ｆ1ｑ＋ｉ1ｒ …（式１０）
ｆ2＝ｃ1ｓ＋ｆ1ｔ＋ｉ1ｕ …（式１１）
ｉ2＝ｃ1ｖ＋ｆ1ｗ＋ｉ1α …（式１２）
【００６８】
従って，式８は，以下の式１３で表すことができる。この式１３の左辺の右項を第２傾斜ベクトル，右辺を第１傾斜ベクトルと呼ぶ。
【００６９】
【数５】
【００７０】
次に，第１座標系と第２座標系との姿勢関係を保持したままで，リンク機構１０の全体姿勢を３通りに変更させる。そして，各全体姿勢ごとに，各加速度センサ２１〜２６の出力値を得る。さらに，上記の手順と同様に，各全体姿勢における第１座標系の傾斜ベクトルおよび第２座標系の傾斜ベクトルを得ることができる。すなわち，各座標系についてそれぞれ３通りの傾斜ベクトルの組が得られる。ここで，リンク機構１０の全体姿勢を変化させて得られた第１座標系の傾斜ベクトルの組を第１傾斜行列Ｓ１，第２座標系の傾斜ベクトルの組を第２傾斜行列Ｓ２とおく。すなわち，それぞれ３つの傾斜ベクトルの組をまとめて傾斜行列として表すことにより，リンク行列Ｔ１２は，次の式１４で表すことができる。
【００７１】
【数６】
【００７２】
式１４において，例えばｃjkのｊは座標系の番号であり，ｋは測定回数を示している。また，式１４の右辺第２項の逆行列が存在するためには，次の式１５が成立する必要がある。
ｃ21・ｆ22・ｉ23＋ｆ21・ｉ22・ｃ23＋ｃ22・ｆ23・ｉ21−ｉ21・ｆ22・ｃ23−ｆ21・ｃ22・ｉ23−ｉ22・ｆ23・ｃ21≠０ …（式１５）
具体的には，基準座標系のＺ軸回りの回転や，同じ軸回りの回転は姿勢を変化したことにならないので避ける。
【００７３】
そして，式１４で求められたリンク行列Ｔ１２の各要素の値を使って，第１座標系から見た第２座標系の各姿勢角は，次の式１６〜式１８のように決定される。
φ12＝ａｔａｎ２（ｑ，ｐ） …（式１６）
θ12＝ａｔａｎ２（−ｒ，ｃｏｓφ・ｐ＋ｓｉｎφ・ｑ） …（式１７）
ψ12＝ａｔａｎ２（ｓｉｎφ・ｖ−ｃｏｓφ・ｗ，−ｓｉｎφ・ｓ＋ｃｏｓφ・ｔ） …（式１８）
【００７４】
従って，上記の式１６〜式１８を用いて，第２座標系は，第１座標系をまずｚ1軸回りにφ12，次に回転後のｙ2軸回りにθ12，最後に回転後のｘ2軸回りにψ12回転させることで表すことができる。これから，第１座標系と第２座標系との関係が得られた。ただし，ｘ2軸がｚ1軸に平行近くになった場合には，式１６からφ12が求まらない。このような場合には，第１座標系と第２座標系との関係を表すのに，姿勢角を用いる代わりにリンク行列Ｔ１２を用いると良い。
【００７５】
次に，式１９にこのように求められるリンク行列Ｔ１２の，具体例を示す。
【００７６】
【数７】
【００７７】
次に，この姿勢角決定方法を図１０のフローチャートと図５のブロック図に基づいて説明する。リンク機構１０の各リンク関係の設定が終了すると，まず，各加速度センサ２１〜２６による測定を開始する（Ｓ１０１）。そして，第１座標系の加速度センサ２１〜２３の傾斜角出力結果からサンプリング器３１において，第１座標系のｘ1軸，ｙ1軸，ｚ1軸に関する傾斜角を抽出する（Ｓ１０２）。さらに，傾斜ベクトル算出器３３において傾斜ベクトルを算出して，レジスタ３５に記憶する（Ｓ１０３）。
【００７８】
同様に，第２座標系の加速度センサ２４〜２６の結果からサンプリング器３２において，第２座標系の傾斜角を抽出する（Ｓ１０４）。さらに，傾斜ベクトル算出器３４において傾斜ベクトルを算出して，レジスタ３５に記憶する（Ｓ１０５）。第１，第２座標系についての算出が終了したら，姿勢変化指令器２８によって姿勢変化動作器２７に指示を出し，第１，第２座標系の姿勢を変化させる（Ｓ１０６）。
【００７９】
このとき，計測回数カウント器２９で計測回数をカウントし，姿勢変化指令器２８が姿勢変化動作器２７に指令を発する。全体姿勢を３通りに変化させ，３種類の全体姿勢について，両座標系の傾斜ベクトルの組を算出する。３回の計測を実施したら（Ｓ１０７：Ｙｅｓ），傾斜行列算出器３６によって，第１座標系の傾斜ベクトルの組から第１傾斜行列Ｓ１を，第２座標系の傾斜ベクトルの組から第２傾斜行列Ｓ２をそれぞれ生成する（Ｓ１０８）。
【００８０】
続いて，第１傾斜行列Ｓ１と第２傾斜行列Ｓ２とを用いて，リンク行列算出器３７によって，リンク行列Ｔ１２を算出する（Ｓ１０９）。さらに，リンク行列表示器３８によって，算出したリンク行列を表示する（Ｓ１１０）。そして，姿勢角算出・表示器３９によって，求めたリンク行列を基に，第１座標系から第２座標系への姿勢角（φ，θ，ψ）を算出し表示する（Ｓ１１１）。以上で，この処理を終了する。
【００８１】
次に，第１座標系と第２座標系との間に所定の関係がある場合の処理の簡略化について説明する。まず第１に，例えば，ｚ1軸とｚ2軸とが平行である場合のように，両座標系のうちの１軸が平行であることが判っている場合では，その平行な軸に関する回転のみで記述できる。ただしここでは，ｚ1軸とｚ2軸とが重力方向に平行である場合は除く。例えば，ｚ1軸とｚ2軸とが平行である場合では，θ12，ψ12は回転に用いる必要がないので，φ12のみを含む次の式２０で表記できる。
【００８２】
【数８】
【００８３】
また，この場合では，全体姿勢を変化させる回転軸をｚ1軸とすれば，ｚ1軸とｚ2軸とにはいずれも加速度センサは不要である。すなわち，加速度センサ２１，２２，２４，２５の４個でよい。さらに，２通りの全体姿勢をとらせるだけでよい。そして，２回の測定結果から次の式２１を用いてリンク行列Ｔ１２を得ることができる。
【００８４】
【数９】
【００８５】
次に，図３の例のように，動作時に剛な構造物を対象とした姿勢角決定装置の具体的な構成の例を，図１１のブロック図を参照して説明する。なお，図５に示した例と共通の部材については，同じ符号を付して説明を省略する。以下の，類似のブロック図についても同様である。この図１１に示したのは，測定対象が動作時に剛な構造物であり，両座標系のうちの１軸（ここではｚ1軸とｚ2軸）が平行であるとともに，平行な回転軸の情報（あるいは全体姿勢の上下）が外部信号として得られる場合である。このようなリンク機構１１０に適用される姿勢角決定装置では，加速度センサは各座標系のｘ軸とｙ軸に設けるのみでよい。
【００８６】
動作時に剛な構造物では，リンク機構１１０が有する関節が動作すると，動作中信号発生器１１１がリンク機構１１０が動作中であることを，リンク固定指示器１１２に送る。さらに，動作が終了するとリンク固定指示器１１２は，リンク機構１１０内にある関節の固定をリンク機構固定器１１３に指示する。これにより，リンク機構１１０の関節が固定される。
【００８７】
ここで，傾斜角を測定中に各加速度センサ２１，２２，２４，２５に対し重力加速度以外の，例えば運動による並進加速度が印加されると，傾斜角の測定結果に誤差が生ずる。このため全体姿勢計測中は傾斜ベクトル算出器３３，３４から出力された傾斜ベクトルの各要素の値が安定している必要がある。そこで静止状態判定器１１４により，タイマー１１５でカウントした時間内に傾斜ベクトル算出器３３，３４から出力された傾斜ベクトルの各要素が一定か否かを判定し，各要素の変化量が規定値以内であれば，レジスタ３５に取得した傾斜ベクトルを記録するように指示する。
【００８８】
次に静止状態判定器１１４は姿勢変化指令器２８に姿勢変化を指令してもよいことを伝える。姿勢変化指令器２８は姿勢変化動作器２７に全体姿勢を変化させるように指示する。姿勢変化動作器２７は，第１座標系と第２座標系の姿勢関係が変化しないようリンク機構１１０の全体姿勢を変更する。各加速度センサ２１，２２，２４，２５の出力はサンプリング器３１，３２で常に取り込まれ，傾斜ベクトル算出器３３，３４において傾斜ベクトルが生成される。
【００８９】
このとき，この例では外部から回転軸平行状態検知用信号１１６が得られるので，回転軸判定器１１７は，回転軸の判定を行い，傾斜ベクトル算出器３３，３４と計測回数カウント器２９に対し，回転軸の軸名と，得られた複数軸の加速度信号のうち，傾斜行列を算出するために必要な加速度信号名、および必要計測回数を伝える。さらに，レジスタ３５は，計測回数カウント器２９から得られた計測回数値と，傾斜行列の各要素値を保存する。本計測は計測回数カウント器２９が規定する回数繰り返され，必要な計測回数が完了したらリンク行列算出器３７により，第１座標系からみた第２座標系のリンク行列が算出される。これをリンク行列表示器３８により表示し，さらに，姿勢角算出・表示器３９によって姿勢角に変換する。
【００９０】
次に，測定対象が動作時に剛な構造物であり，両座標系のうちの１軸の組が平行であるが，平行な回転軸の情報（あるいは全体姿勢の上下）が外部信号としては得られない場合の例を，図１２のブロック図に示す。この場合には，回転軸の判定を内部で行わなければならない。そのため，加速度センサ２１〜２６は各軸に計６個必要となっている。
【００９１】
この例では，サンプリング器３１，３２において得られた傾斜角信号を回転軸判定器１１７に送る。回転軸判定器１１７は，第１座標系と第２座標系の同じ軸の傾斜角を比較し，それぞれの傾斜角値の変化量が特定の軸について既定値内であれば、当該軸が回転軸でありかつ当該軸が第１座標系および第２座標系において平行な軸であると判定する。回転軸判定器１１７は，このようにして判定された平行な軸の組をそれぞれの回転軸と判定する。さらに，傾斜ベクトル算出器３３，３４と計測回数カウント器２９に対し，回転軸の軸名と，得られた複数軸の傾斜角信号のうち傾斜行列を算出するために必要な軸名、および必要計測回数を伝える。以下の処理は，上記の図１１の例と同様である。
【００９２】
次の例は，上記の図１２の例に加えて，平行な１組の軸が水平面内にある場合である。例えば，上記の図１２の例において，平行な軸の組であるとともに全体姿勢を変化させる回転軸がｚ1軸とｚ2軸であり，かつこれらが重力方向を法線にもつ水平面内にある場合である。この場合には，ｘ1軸とｘ2軸，あるいは，ｙ1軸とｙ2軸のいずれかの組み合わせのみに加速度センサを設置すればよい。すなわち，加速度センサ２１と２４の組み合わせまたは加速度センサ２２と２５の組み合わせのいずれかでよい。そして，全体姿勢を１通りに設定するだけでよい。この場合には，加速度センサの出力から得られる傾斜角からｚ1軸回りの姿勢角変化であるヨー角を得ることができる。
【００９３】
また，第１の全体姿勢と第２の全体姿勢との間に，加速度センサを設置した軸が基準座標系のＺ軸に平行にならない場合は，次の式２２を式２０に代入することでリンク行列Ｔ１２を得ることができる。
ヨー角φ12＝第２座標系の傾斜角−第１座標系の傾斜角 …（式２２）
すなわち，この場合には回転軸がｚ1軸であるため，姿勢角のうちθ12，ψ12は回転に用いる必要がなく，リンク行列Ｔ１２はヨー角φ12のみで表すことができる。
【００９４】
また，第１の全体姿勢と第２の全体姿勢との間に，加速度センサを設置した軸が基準座標系のＺ軸に平行になる場合は，次の式２３と式２０とからリンク行列Ｔ１２を得ることができる。
ヨー角φ12＝第２座標系の傾斜角＋第１座標系の傾斜角 …（式２３）
【００９５】
さらに，重力加速度の大きさが既知で，座標系の上下（重力方向）が判る場合では，次のような簡略化が可能である。この場合の例を図１３のブロック図に示す。この場合には，姿勢角決定装置は，加速度算出器１１８，１１９を有し，これらが座標系の上下識別用外部信号１２０を受ける。
【００９６】
重力加速度の大きさが｜Ｇ｜であれば，例えば，ｚ1軸方向の加速度ａｚは，ｘ1軸およびｙ1軸方向の加速度ａｘ，ａｙと次の式２４のような関係がある。
｜Ｇ｜＝√（ａｘ2＋ａｙ2＋ａｚ2） …（式２４）
従って，ｚ1軸方向の加速度ａｚは，加速度算出器１１８によりｘ1軸およびｙ1軸方向の加速度ａｘ，ａｙを用いて次の式２５のように表すことができる。
ａｚ＝±√（｜Ｇ｜2−ａｘ2−ａｙ2） …（式２５）
【００９７】
さらに，座標系上下識別用外部信号１２０によりｚ1軸の上下が判ることから，ａｚの符号が与えられる。以上のことから，ｚ1軸とｚ2軸とにはいずれも加速度センサは不要であり，加速度センサ２１，２２，２４，２５の４個の測定結果からｚ1軸方向とｚ2軸方向との加速度も得ることができる。また，リンク行列Ｔ１２は，式１４から算出できる。なお，この第２の条件は，例えば地表上でおよそ直立して移動するロボット等では，一般に満たされるものである。
【００９８】
次に，図３の例のように，動作時に剛な構造物を対象とした姿勢角決定装置のより一般的な構成を図１４に示す。この例のリンク機構１１０も，図８の例と同様に，第１座標系と第２座標系が，リンク１１，リンク１２，リンク１３および関節１４，関節１５で結びつけられているものとして表される。リンク機構１１０には測定対象の各座標系の各軸方向に沿って，それぞれ加速度センサが設けられる。すなわち，第１座標系のリンク１１には，ｘ1軸加速度センサ２１，ｙ1軸加速度センサ２２，ｚ1軸加速度センサ２３が設けられている。また，第２座標系のリンク１３には，ｘ2軸加速度センサ２４，ｙ2軸加速度センサ２５，ｚ2軸加速度センサ２６が設けられている。さらに，このリンク機構１０には，全体姿勢を変化させるためのモータ等を有する姿勢変化動作器２７を含む姿勢変化部が設けられている。
【００９９】
動作時に剛な構造物では，リンク機構１１０が有する関節が動作すると，動作中信号発生器１１１がリンク機構１１０が動作中であることを，リンク固定指示器１１２に送る。さらに，動作が終了するとリンク固定指示器１１２は，リンク機構１１０内にある関節の固定をリンク機構固定器１１３に指示する。これにより，リンク機構１１０の関節が固定される。
【０１００】
ここで，傾斜角を測定中に各加速度センサ２１〜２６に対し重力加速度以外の，例えば運動による並進加速度が印加されると，傾斜角の測定結果に誤差が生ずる。このため，全体姿勢計測中は傾斜ベクトル算出器３３，３４から出力された傾斜ベクトルの各要素の値が安定している必要がある。そこで静止状態判定器１１４により，タイマー１１５でカウントした時間内に傾斜ベクトル算出器３３，３４から出力された各傾斜ベクトルの各要素が一定か否かを判定し，各要素の変化量が規定値以内であれば，レジスタ３５に取得した傾斜ベクトルを記録するように指示する。
【０１０１】
次に，静止状態判定器１１４は姿勢変化指令器２８に姿勢変化を指令してもよいことを伝える。姿勢変化指令器２８は姿勢変化動作器２７に全体姿勢を変化させるように指示する。姿勢変化動作器２７は，第１座標系と第２座標系の姿勢関係が変化しないようリンク機構１１０の全体姿勢を変更する。各加速度センサ２１〜２６の出力は，サンプリング器３１，３２で常に取り込まれ，傾斜ベクトル算出器３３，３４において傾斜ベクトルが生成される。
【０１０２】
レジスタ３５は計測回数カウント器２９から得られた計測回数値と，傾斜ベクトルの各要素値を保存する。本計測は計測回数カウント器２９が規定する回数繰り返され，必要な計測回数が完了したらリンク行列算出器３７により，第１座標系からみた第２座標系のリンク行列が算出される。これをリンク行列表示器３８により表示し，さらに姿勢角算出・表示器３９において姿勢角に変換し表示する。
【０１０３】
次に，図４に示したような，リンクが柔であり，関係が柔である構造物を対象とした姿勢角決定装置について説明する。このような構造物では，全体姿勢を変化させることで第１座標系と第２座標系の姿勢関係が変化するため，全体姿勢を変化させることによって測定誤差が増える恐れがある。そこで，全体姿勢を変化させずに姿勢角を測定することが好ましい。この例における構造物と各座標系との関係を図１５に示す。また，姿勢角決定装置の概略構成を図１６のブロック図に示す。
【０１０４】
この例では，図１５に示すように，第１座標系および第２座標系上には，座標系のそれぞれの軸方向への加速度とそれぞれの軸回りの角速度を測定するための加速度センサ２１〜２６に加えて角速度センサ２１１〜２１６が設置されている。図１６に示すように，加速度センサ２１〜２６からの出力は，サンプリング器３１，３２によってサンプリングされ，角速度センサ２１１〜２１６からの出力はサンプリング器２１７，２１８によってサンプリングされる。サンプリングされた出力は，合成器２１９，２２０に導入される。合成器２１９，２２０ではそれぞれのセンサ出力を基に、現在のもっとも確からしい姿勢を算出する。
【０１０５】
さらに，姿勢行列算出器２２１，２２２は合成器２１９，２２０の出力から姿勢行列を生成する。この結果，時間の経過とともに変化する第１座標系および第２座標系の基準座標系から見た姿勢行列Ｔ１，Ｔ２を，連続的に得ることができる。このようにして得られた姿勢行列と，次の式２６から，リンク行列Ｔ１２を得ることができる。
Ｔ１２＝Ｔ1-1・Ｔ２ …（式２６）
ただし，Ｔ１：基準座標系から見た第１座標系への姿勢行列
Ｔ２：基準座標系から見た第２座標系への姿勢行列
Ｔ１２：第１座標系から見た第２座標系へのリンク行列
この例の方法を採用すれば，全体姿勢は１通りでよく，変化させる必要はない。
【０１０６】
また，あるいは，図１５に示すような例において，第１座標系および第２座標系上に，座標系のそれぞれの軸方向への加速度を測定するための加速度センサと，磁北方向を測定するための磁気センサとが設置されているものとしてもよい。加速度センサを用いれば各座標系上の各座標軸と基準座標系Ｚ軸との間の角度として傾斜角が得られ，次に傾斜角から，ロール角ψおよびピッチ角θを求めることができる。ここで姿勢角としてロール角，ピッチ角，ヨー角を用いるときは，姿勢角表記の性質から，磁気センサをそれぞれの座標系のｘ軸に設置することで，ヨー角φの計算が容易となるため好ましい。また，ｘ軸だけでなく各座標系のｙ軸，ｚ軸に設置すれば，測定精度と信頼性が向上し，より好ましい構成となる。
【０１０７】
次に，さらに別の例を説明する。基準座標系からみた第１座標系の姿勢は，姿勢行列Ｔ１で表すことができる。姿勢行列Ｔ１の具体的な要素は上記した式２を展開して得られる次の式２７で表される。
【０１０８】
【数１０】
【０１０９】
このとき，姿勢行列Ｔ１の第３行第１列の成分は，第１座標系のｘ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。また，第３行第２列の成分は，第１座標系のｙ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。また，第３行第３列の成分は，第１座標系のｚ1軸と基準座標系のＺ軸とのなす傾斜角度の方向余弦である。すなわち図９に示すように，第１座標系の各軸と基準座標系のＺ軸とのなす傾斜角度をγ1〜γ3とおけば，傾斜角度の方向余弦は次の式２８〜式３０で表される。
【０１１０】
−ｓｉｎθ1＝ｃｏｓγ1 …（式２８）
ｃｏｓθ1ｓｉｎψ1＝ｃｏｓγ2 …（式２９）
ｃｏｓθ1ｃｏｓψ1＝ｃｏｓγ3 …（式３０）
さらに，これらの式から，基準座標系から見た第１座標系のロール角θ1，ピッチ角ψ1は，次の式３１，式３２で表される。
θ1＝ｓｉｎ-1（−ｃｏｓγ1） …（式３１）
ψ1＝ｔａｎ-1（ｃｏｓγ2／ｃｏｓγ3） …（式３２）
【０１１１】
加速度センサの出力は座標系の姿勢が基準座標系のＺ軸回りに回転しても変化しないため，ヨー角φを得ることはできない。しかし，磁気センサを用いて磁北を求めることで，ヨー角φを得ることができる。具体的には，基準座標系からみた磁北方向と，第１座標系から見た磁北方向の差異分をヨー角φ1と置き，式３１〜３２および式２を用いて姿勢行列Ｔ１を求める。次に基準座標系からみた磁北方向と第２座標系から見た磁北方向の差異分をヨー角φ2と置き，第２座標系に関する式３１〜３２および式２から姿勢行Ｔ２を求める。姿勢行列Ｔ１および姿勢行列Ｔ２を式２６に適用することで，リンク行列Ｔ１２を得ることができる。
【０１１２】
また別の方法として，磁気センサを用いて第１座標系から見た第２座標系へのヨー角を直接求めてもよい。また本形態において磁気センサの代わりに，外部からの情報として，ＧＰＳ（Global Positioning System）や，マーカーを用いてもよい。そして，次の式３３において，磁気センサまたは外部情報で得られた第１座標系から見た第２座標系のヨー角を（φ2−φ1）に代入する。
【０１１３】
【数１１】
【０１１４】
また姿勢角θ1，θ2，ψ1，ψ2を加速度センサの出力から求める。本手法によりリンク行列Ｔ１２を求めることができる。このようにすれば，第１座標系および第２座標系に設置する加速度センサの個数はそれぞれ２個でよく，測定精度を向上することができる。また全体姿勢は１通りでよい。
【０１１５】
次に，さらに別の姿勢角決定装置の概略構成を図１７のブロック図に示す。第１座標系および第２座標系上には，座標系のそれぞれの軸方向への加速度を測定するための加速度センサ２１〜２６と，それぞれの軸回りの角速度を測定するための角速度センサ２１１〜２１６と，磁北方向を測定するための磁気センサ２３１〜２３６を設置している。加速度センサ２１〜２６と角速度センサ２１１〜２１６を用いればロール角ψ，ピッチ角θ，ヨー角φを求めることができるが，角速度センサ２１１〜２１６の出力の時間的な変化により，ヨー角φが変化する場合がある。
【０１１６】
そこで角速度センサ２１１〜２１６の出力を磁気センサ２３１〜２３６の出力と比較して統合することによってヨー角φの変化量を低減することができる。また反対に，磁気センサ２３１〜２３６によって求められたヨー角φは外部磁界の影響を受けるが，この影響を角速度センサ２１１〜２１６の出力と比較して統合することによって低減することができる。
【０１１７】
この手法により角速度センサ２１１〜２１６と磁気センサ２３１〜２３６が互いの利点によって補完するため，姿勢角から得られる姿勢行列Ｔ１およびＴ２が角速度センサ２１１〜２１６の出力変動や外部磁界の影響を受にくくなり，姿勢行列Ｔ１およびＴ２から求まるリンク行列Ｔ１２の測定精度を向上することができる。なおここでは柔軟な構造物について説明したが，常に剛な構造物や動作中のみ剛となるリンクをもつ構造物へ適用してもよい。またこの場合の全体姿勢は１通りでよい。
【０１１８】
さらに，図４に示したような，リンクが柔であり，関係が柔である構造物を対象とした別の姿勢角決定装置の例について説明する。外部から姿勢角φが与えられる場合の姿勢角決定装置の概略構成を図１８に示す。このようなものでは，第１座標系および第２座標系上に設置された加速度センサ２１，２２，２４，２５からの加速度を用い，上記の式３１，式３２からロール角ψおよびピッチ角θが求まる。またヨー角は外部姿勢角信号２４１として与えられる。これら３つの姿勢角をもとに，姿勢行列算出器２２１，２２２において姿勢行列が得られる。この結果をレジスタ３５に保存したのちリンク行列算出器３７においてリンク行列を求める。
【０１１９】
なお，以上の各例において述べた加速度センサ，角速度センサ，磁気センサは，必ずしも第１座標系および第２座標系上に常設する必要はない。初期状態ではこれらセンサが第１座標系のみに設置され，ある全体姿勢において第１座標系の姿勢を計測後，これらセンサを第２座標系に移動し，同じ全体姿勢において第２座標系の姿勢を計測するのに用いてもよい。
【０１２０】
以上のような姿勢角決定装置が適用されるリンク機構の具体例を図１９に示す。この例では，リンク１１，１２，１３と，リンク同士を接続する関節１４，１５を有している。各リンク上には各リンクの姿勢を規定するために，それぞれ対応する座標系が設けられている。リンク１１上の部位にはリンク１１と一体となって動く第１座標系が，リンク１２上の部位にはリンク１２と一体となって動く第２座標系が，またリンク１３上の部位にはリンク１３と一体となって動く第３座標系が設置されている。
【０１２１】
ここで，関節１４および関節１５にはそれぞれのリンク角度を変化させるアクチュエータとしてのモータと，関節が接続するリンクのリンク角度を測定するエンコーダが配置されている。通常エンコーダは小型化，軽量化のために，絶対角度タイプではなく相対角度タイプが使われる。相対角度タイプでは初期値が不定であるため，リンク機構を初期化する前の状態では正しいリンク角度は得られない。このためリンク機構利用前には，各関節をその最大回転可能角度まで回転させ，ピンなどの機械的な突起部に押し当てることで関節角度の初期値を規定する初期化工程が必要である。この初期化工程には時間がかかること，またリンク機構の重量バランスが大きく崩れるため体勢が不安定になることといった課題があった。
【０１２２】
これに対して，本発明を利用した初期化工程の実施例について述べる。図１９に示すように，このリンク機構には，計測中には第１座標系と第２座標系間の姿勢関係が一定となるよう，関節１４および関節１５にリンク機構固定器１６，１７が設置されている。リンク機構固定器１６，１７は，各リンク間角度を固定する機能を持つ。リンク機構固定器１６，１７としては，図２０，図２１，図２２に示す三種類の手法がある。
【０１２３】
図２０に示した手法では，リンク１２には，リンク１２とリンク１３との間の角度変化を計測するためのエンコーダ３１１と，リンク２とリンク３の間の角度を変化させることができるモータ３１２と，モータ３１２を制御するためのコントローラ３１３が設置されている。そして，各全体姿勢についてリンク間の姿勢関係を計測している間は，継続的にエンコーダ３１１の出力をモニタする。そしてリンク固定指示器１１２（図１１参照）の発する命令により，コントローラ３１３はモータ３１２に対してエンコーダ３１１の出力が常に一定となるようにサーボ方式による位置制御指令を出す。この結果，リンク１２とリンク１３の間の角度を位置制御により一定とすることができる。
【０１２４】
このサーボ方式による位置制御によるリンク間角度の固定手法には，はじめから関節にエンコーダ３１１およびモータ３１２が設置されている場合，追加装置が不要である利点がある。また本手法は電気的にリンク間の姿勢関係を固定しているため機械的な切り替え時間が不要であり，リンク間の姿勢関係測定といった初期化工程終了後，即座にリンク機構を利用することができる。このため初期化工程の効率化が可能となる。
【０１２５】
またあるいは，図２１に示すように，リンク１２から延びリンク１３に押し当てることでリンク１２に対するリンク１３の動きを止めるための摩擦を用いたクラッチ３１４と，クラッチ３１４を作動させるためのアクチュエータ３１５が設置されているものとしてもよい。各全体姿勢についてリンク間の姿勢関係を計測している間は，リンク固定指示器１１２の発する命令によりクラッチ３１４の動作を有効とすることで，リンク１２に対するリンク１３の角度関係を固定できる。クラッチ３１４による固定方法は，エンコーダ３１１やモータ３１２を使用せずリンク固定機構が簡単になるという利点がある。
【０１２６】
またあるいは，図２２に示すように，リンク１２とリンク１３の間に，ロッド＋ピン３１６が設置されているものとしてもよい。このロッド＋ピン３１６によりリンク同士を機械的に固定する。本固定手法は，他の手法にくらべ最も強固であるため，全体姿勢を変化させてもリンク間の姿勢関係を最も精度良く一定とすることができる。またロッド＋ピン３１６による固定方法は電気信号を必要とせず，もっとも容易かつ確実な手法である。本手法による固定手法は，ロッド＋ピン３１６を容易に取り外せないことから，リンク姿勢関係の計測後も，継続的にリンク間の姿勢関係を変えない用途に向いている。
【０１２７】
これらの固定手法を用いて求めた第１座標系から第２座標系へのリンク行列をＴ１，第２座標系から第３座標系へのリンク行列をＴ２とおく。このとき，第１座標系から見た第３座標系へのリンク行列Ｔ１２は，次の式３４で表される。
Ｔ１２＝Ｔ１・Ｔ２ …（式３４）
【０１２８】
このようにすれば，リンク行列Ｔ１２が求まるので，式３４の逆関数を求めることで，関節１４および関節１５における関節角度を算出できる。逆関数の求め方の例は，「ロボット・マニピュレータ コロナ社 吉川恒夫訳」に記載されている。さらに，各関節角度の初期値が求まれば，初期化工程が終了した後の，任意の時間経過後の関節角度は，関節角度の初期値と関節角度の変化分を用いて，次の式３５，式３６によって表される。このとき，各関節角度の変化分は，相対角度タイプのエンコーダによって得られる。
関節１４の角度＝関節１４の角度の初期値＋関節１４の角度の変化分 …（式３５）
関節１５の角度＝関節１５の角度の初期値＋関節１５の角度の変化分 …（式３６）
【０１２９】
本実施例によれば，リンク機構の初期化工程をリンク機構の姿勢を大きく変化させなくとも，第１座標系および第３座標系の間の姿勢関係を容易にかつ高精度に求めることができる。また得られた座標系間の姿勢関係を利用して，各関節に設置されたリンク機構の初期化工程を簡便に行うことができる。
【０１３０】
以上詳細に説明したように本形態の姿勢角決定装置２０によれば，姿勢関係を求めようとする両座標系の各軸方向にそれぞれ加速度センサ２１〜２６を備えているので，これらの測定結果から各軸の傾斜方向を算出することができる。さらに，この測定を姿勢を変化させて１〜３回行い，リンク行列算出器３７により，リンク行列Ｔ１２を算出することができる。従って，リンク行列Ｔ１２から，リンク構造に含まれる両座標系の間の姿勢角が求められる。すなわち，図面や実測によることなく，複雑な関係にある座標系間であってもその姿勢関係を容易に，精度良く求めることができる。
【０１３１】
なお，本形態は単なる例示にすぎず，本発明を何ら限定するものではない。したがって本発明は当然に，その要旨を逸脱しない範囲内で種々の改良，変形が可能である。
【図面の簡単な説明】
【０１３２】
【図１】本形態の姿勢角決定装置の対象部材の例を示す説明図である。
【図２】対象部材の具体例を示す説明図である。
【図３】対象部材の具体例を示す説明図である。
【図４】対象部材の具体例を示す説明図である。
【図５】本形態の姿勢角決定装置の概略構成を示すブロック図である。
【図６】対象部材の具体例を示す説明図である。
【図７】対象部材の具体例を示す説明図である。
【図８】対象部材のリンク構成を示す説明図である。
【図９】基準座標系のＺ軸と他の座標系の軸との関係を示す説明図である。
【図１０】本形態の姿勢角決定方法のアルゴリズムを示すフローチャートである。
【図１１】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１２】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１３】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１４】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１５】対象部材のリンク構成を示す説明図である。
【図１６】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１７】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１８】姿勢角決定装置の別の例の概略構成を示すブロック図である。
【図１９】リンク機構の具体例を示す説明図である。
【図２０】リンク機構固定器の具体例を示す説明図である。
【図２１】リンク機構固定器の具体例を示す説明図である。
【図２２】リンク機構固定器の具体例を示す説明図である。
【符号の説明】
【０１３３】
１０ リンク機構
１１，１２ リンク
２０ 姿勢角決定装置
２１，２２，２３，２４，２５，２６ 加速度センサ
２８ 姿勢変化指令器
３３ 傾斜ベクトル算出器
３４ 傾斜ベクトル算出器
３６ 傾斜行列算出器
３７ リンク行列算出器
３９ 姿勢角算出・表示器
【特許請求の範囲】
【請求項１】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得する傾斜角取得部と，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせる姿勢変化指示部と，
前記姿勢変化指示部の指示による各全体姿勢における各リンクについて，前記傾斜角取得部で取得された各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，
各リンクについて，前記傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，
前記傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項２】
請求項１に記載の姿勢角決定装置において，前記傾斜角取得部は，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた水準器を用いて得ることを特徴とする姿勢角決定装置。
【請求項３】
請求項１に記載の姿勢角決定装置において，前記傾斜角取得部は，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた加速度センサの出力値から求めることを特徴とする姿勢角決定装置。
【請求項４】
請求項１に記載の姿勢角決定装置において，前記傾斜角取得部は，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，
各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めることを特徴とする姿勢角決定装置。
【請求項５】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得する加速度値取得部と，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせる姿勢変化指示部と，
前記姿勢変化指示部の指示による各全体姿勢における各リンクについて，前記加速度値取得部で取得された各軸方向の加速度値から傾斜角を求める傾斜角測定部と，
前記姿勢変化指示部の指示による各全体姿勢における各リンクについて，前記傾斜角測定部で求められた各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，
各リンクについて，前記傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，
前記傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項６】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得する加速度値取得部と，
各リンクについて，前記加速度値取得部で取得された加速度値から傾斜角を求める傾斜角測定部と，
前記傾斜角測定部により求められた各リンクの傾斜角の差より姿勢角を算出する姿勢角計算部とを有することを特徴とする姿勢角決定装置。
【請求項７】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクに設定された座標系間のヨー角を取得する第１ヨー角取得部と，
前記加速度値取得部で取得された加速度値と，前記第１ヨー角取得部で取得されたヨー角とから，姿勢角を表すリンク行列を算出する第２リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項８】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得する第２ヨー角取得部と，
各リンクについて，前記加速度値取得部で取得された加速度値と，前記第２ヨー角取得部で取得されたヨー角とから，基準座標系に対する姿勢行列を求める第１姿勢行列算出部と，
前記第１姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項９】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクに設けられそれぞれにおける角速度値を取得する角速度値取得部と，
各リンクについて，前記加速度値取得部で取得された加速度値と，前記角速度値取得部で取得された角速度値とから，基準座標系に対する姿勢行列を求める第２姿勢行列算出部と，
前記第２姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項１０】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクに設けられた磁気センサと，
各リンクについて，前記加速度値取得部で取得された加速度値と，前記磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求める第３姿勢行列算出部と，
前記第３姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項１１】
請求項９に記載の姿勢角決定装置において，
各リンクに設けられた磁気センサを有し，
前記第２姿勢行列算出部は，前記加速度値取得部で取得された加速度値と，前記角速度値取得部で取得された角速度値と，前記磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求めることを特徴とする姿勢角決定装置。
【請求項１２】
請求項１から請求項５までのいずれか１つに記載の姿勢角決定装置において，前記姿勢変化動作部は，
姿勢変化装置を内蔵するロボットであり自ら全体姿勢を形成することを特徴とする姿勢角決定装置。
【請求項１３】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得し，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせ，
前記指示による各全体姿勢における各リンクについて，取得した各軸方向の傾斜角から傾斜ベクトルを求め，
各リンクについて，全体姿勢ごとに求めた傾斜ベクトルにより傾斜行列を求め，
求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１４】
請求項１３に記載の姿勢角決定方法において，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，
各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めることを特徴とする姿勢角決定方法。
【請求項１５】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得し，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせ，
前記指示による各全体姿勢における各リンクについて，取得した各軸方向の加速度値から傾斜角を求め，
前記指示による各全体姿勢における各リンクについて，求めた各軸方向の傾斜角から傾斜ベクトルを求め，
各リンクについて，全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求め，
求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１６】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得し，
各リンクについて，取得した加速度値から傾斜角を求め，
求めた各リンクの傾斜角の差より姿勢角を算出することを特徴とする姿勢角決定方法。
【請求項１７】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクに設定された座標系間のヨー角を取得し，
取得した加速度値と，取得したヨー角とから，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１８】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得し，
各リンクについて，取得した加速度値と，取得したヨー角とから，基準座標系に対する姿勢行列を求め，
求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１９】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクにおける角速度値を取得し，
各リンクについて，取得した加速度値と，取得した角速度値とから，基準座標系に対する姿勢行列を求め，
求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項２０】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクにおける磁北方向との交差角の情報を磁気センサで取得し，
各リンクについて，取得した加速度値と，取得した磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求め，
求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項２１】
請求項１３から請求項１５までのいずれか１つに記載の姿勢角決定方法において，
前記姿勢変化動作部が，姿勢変化装置を内蔵するロボットであり，
自ら全体姿勢を形成することを特徴とする姿勢角決定方法。
【請求項１】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得する傾斜角取得部と，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせる姿勢変化指示部と，
前記姿勢変化指示部の指示による各全体姿勢における各リンクについて，前記傾斜角取得部で取得された各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，
各リンクについて，前記傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，
前記傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項２】
請求項１に記載の姿勢角決定装置において，前記傾斜角取得部は，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた水準器を用いて得ることを特徴とする姿勢角決定装置。
【請求項３】
請求項１に記載の姿勢角決定装置において，前記傾斜角取得部は，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する３方向に設けられた加速度センサの出力値から求めることを特徴とする姿勢角決定装置。
【請求項４】
請求項１に記載の姿勢角決定装置において，前記傾斜角取得部は，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，
各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めることを特徴とする姿勢角決定装置。
【請求項５】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得する加速度値取得部と，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせる姿勢変化指示部と，
前記姿勢変化指示部の指示による各全体姿勢における各リンクについて，前記加速度値取得部で取得された各軸方向の加速度値から傾斜角を求める傾斜角測定部と，
前記姿勢変化指示部の指示による各全体姿勢における各リンクについて，前記傾斜角測定部で求められた各軸方向の傾斜角から傾斜ベクトルを求める傾斜ベクトル算出部と，
各リンクについて，前記傾斜ベクトル算出部で全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求める傾斜行列算出部と，
前記傾斜行列算出部により求められた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項６】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得する加速度値取得部と，
各リンクについて，前記加速度値取得部で取得された加速度値から傾斜角を求める傾斜角測定部と，
前記傾斜角測定部により求められた各リンクの傾斜角の差より姿勢角を算出する姿勢角計算部とを有することを特徴とする姿勢角決定装置。
【請求項７】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクに設定された座標系間のヨー角を取得する第１ヨー角取得部と，
前記加速度値取得部で取得された加速度値と，前記第１ヨー角取得部で取得されたヨー角とから，姿勢角を表すリンク行列を算出する第２リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項８】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得する第２ヨー角取得部と，
各リンクについて，前記加速度値取得部で取得された加速度値と，前記第２ヨー角取得部で取得されたヨー角とから，基準座標系に対する姿勢行列を求める第１姿勢行列算出部と，
前記第１姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項９】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクに設けられそれぞれにおける角速度値を取得する角速度値取得部と，
各リンクについて，前記加速度値取得部で取得された加速度値と，前記角速度値取得部で取得された角速度値とから，基準座標系に対する姿勢行列を求める第２姿勢行列算出部と，
前記第２姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項１０】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定装置において，
各リンクに設けられそれぞれにおける加速度値を取得する加速度値取得部と，
各リンクに設けられた磁気センサと，
各リンクについて，前記加速度値取得部で取得された加速度値と，前記磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求める第３姿勢行列算出部と，
前記第３姿勢行列算出部により求められた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出する第１リンク行列算出部とを有することを特徴とする姿勢角決定装置。
【請求項１１】
請求項９に記載の姿勢角決定装置において，
各リンクに設けられた磁気センサを有し，
前記第２姿勢行列算出部は，前記加速度値取得部で取得された加速度値と，前記角速度値取得部で取得された角速度値と，前記磁気センサで取得された磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求めることを特徴とする姿勢角決定装置。
【請求項１２】
請求項１から請求項５までのいずれか１つに記載の姿勢角決定装置において，前記姿勢変化動作部は，
姿勢変化装置を内蔵するロボットであり自ら全体姿勢を形成することを特徴とする姿勢角決定装置。
【請求項１３】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクに設定された３軸直交座標系の各軸について，軸方向と重力ベクトルの方向との間の角度である傾斜角を取得し，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して最大３通りの全体姿勢を取らせ，
前記指示による各全体姿勢における各リンクについて，取得した各軸方向の傾斜角から傾斜ベクトルを求め，
各リンクについて，全体姿勢ごとに求めた傾斜ベクトルにより傾斜行列を求め，
求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１４】
請求項１３に記載の姿勢角決定方法において，傾斜角を，
第１リンクおよび第２リンクにそれぞれ，互いに直交する２方向に設けられた加速度センサの出力値と，
各リンクごとの各加速度センサの出力値の２乗の和をその場所の重力加速度の値の２乗から差し引いた値の平方根とから求めることを特徴とする姿勢角決定方法。
【請求項１５】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現されるとともに，姿勢変化動作部に搭載または接続されている構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクに１方向が互いに平行となるように設定された３軸直交座標系の当該１方向以外の各方向の加速度値を取得し，
第１リンクと第２リンクとの姿勢関係を固定したまま，姿勢変化動作部に構造体の姿勢変化を指示して，当該１方向に平行な軸の周りの回転による２通りの全体姿勢を取らせ，
前記指示による各全体姿勢における各リンクについて，取得した各軸方向の加速度値から傾斜角を求め，
前記指示による各全体姿勢における各リンクについて，求めた各軸方向の傾斜角から傾斜ベクトルを求め，
各リンクについて，全体姿勢ごとに求められた傾斜ベクトルにより傾斜行列を求め，
求めた各リンクの傾斜行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１６】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクに１方向が互いに平行かつ水平となるように設定された３軸直交座標系の当該１方向以外の任意の１方向の加速度値を取得し，
各リンクについて，取得した加速度値から傾斜角を求め，
求めた各リンクの傾斜角の差より姿勢角を算出することを特徴とする姿勢角決定方法。
【請求項１７】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクに設定された座標系間のヨー角を取得し，
取得した加速度値と，取得したヨー角とから，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１８】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクについて，そこに設定された座標系と基準座標系との間のヨー角を取得し，
各リンクについて，取得した加速度値と，取得したヨー角とから，基準座標系に対する姿勢行列を求め，
求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項１９】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクにおける角速度値を取得し，
各リンクについて，取得した加速度値と，取得した角速度値とから，基準座標系に対する姿勢行列を求め，
求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項２０】
１以上の関節を介して接続された第１リンクおよび第２リンクを有するリンク機構として表現される構造体における，第１リンクの座標系から見た第２リンクの座標系の姿勢角を決定する姿勢角決定方法において，
各リンクにおける加速度値を取得し，
各リンクにおける磁北方向との交差角の情報を磁気センサで取得し，
各リンクについて，取得した加速度値と，取得した磁北方向との交差角の情報とから，基準座標系に対する姿勢行列を求め，
求めた各リンクの姿勢行列に基づいて，一方と他方の逆行列との積を求めることにより，姿勢角を表すリンク行列を算出することを特徴とする姿勢角決定方法。
【請求項２１】
請求項１３から請求項１５までのいずれか１つに記載の姿勢角決定方法において，
前記姿勢変化動作部が，姿勢変化装置を内蔵するロボットであり，
自ら全体姿勢を形成することを特徴とする姿勢角決定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【公開番号】特開２００８−２８６７（Ｐ２００８−２８６７Ａ）
【公開日】平成２０年１月１０日（２００８．１．１０）
【国際特許分類】
【出願番号】特願２００６−１７０８９６（Ｐ２００６−１７０８９６）
【出願日】平成１８年６月２１日（２００６．６．２１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２０年１月１０日（２００８．１．１０）
【国際特許分類】
【出願日】平成１８年６月２１日（２００６．６．２１）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]