(57)【要約】【課題】  ヤコビ行列を用いた単純な逆運動学であっても、例えば片足６自由度の歩行ロボットで膝関節を伸ばした姿勢を実現可能な機構制御装置等を提案する。【解決手段】  片足６自由度の歩行ロボットにおいて、回転関節２９（膝関節）とこの回転関節２９につながっている２つの支持部材を、１つの仮想直動関節と１つの仮想リンクに置換した仮想的なリンク構造に基づき、ヤコビ行列を用いた逆運動学により、仮想の関節角度と関節変位を求め、最後に余弦定理を用いて実際の関節角度を求める。この仮想的なリンク構造には、膝関節に対応する回転関節がないため、膝特異姿勢になることがない。そのため、安定した歩行制御を実現することが可能になる。【選択図】図３
2014014920.tif 000002
【特許請求の範囲】
【請求項１】
第１回転軸を中心に回転可能な第１支持部材と、前記第１回転軸に平行な第２回転軸を中心に回転可能な第２支持部材とが、前記第１回転軸及び前記第２回転軸に平行な第３回転軸を中心に回転可能に連結された機構に対して、前記第１回転軸、前記第２回転軸及び前記第３回転軸の各回転角を制御する機構制御装置であって、
前記第１回転軸及び前記第２回転軸を２つの回転関節とし、前記第１回転軸と前記第２回転軸との間を１つの直動関節として、前記第１回転軸と前記第２回転軸との長さ並びに前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算する仮想リンク構造演算手段と、
前記第１回転軸と前記第３回転軸との固定長及び前記第２回転軸と前記第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記第１回転軸と前記第２回転軸との長さにより、前記第１回転軸と前記第２回転軸と前記第３回転軸とが平面に射影した際に互いになす角度を求める角度演算手段と、
前記第１回転軸及び前記第２回転軸の回転角を、それぞれ、前記仮想リンク構造演算手段により演算された前記第１回転軸及び前記第２回転軸の仮想回転角度、並びに、前記角度演算手段により演算された前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度及び前記第２回転軸が前記第３回転軸及び前記第１回転軸に対して平面に射影した際になす角度により演算し、前記第３回転軸の回転角を、前記角度演算手段により演算された前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算する実リンク構造演算手段と、
前記第１回転軸、前記第２回転軸及び前記第３回転軸を、それぞれ、前記実リンク構造演算手段により演算された角度に回転させる回転制御手段を備える機構制御装置。
【請求項２】
前記第３回転軸は、歩行ロボットにおける足の膝関節であり、
前記第１回転軸及び前記第２回転軸は、それぞれ、前記第１支持部材及び前記第２支持部材により前記膝関節につながれた関節であり、
前記歩行ロボットの歩様には、前記第１回転軸、前記第２回転軸及び前記第３回転軸が同一平面上に存在するものが含まれる、請求項１記載の機構制御装置。
【請求項３】
前記仮想リンク構造演算手段は、前記第１回転軸及び前記第２回転軸を２つの回転関節とし、前記第１回転軸と前記第２回転軸との間を１つの直動関節として、ヤコビ行列を構成して前記第１回転軸と前記第２回転軸との長さ並びに前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算し、又は、前記第１回転軸と前記第２回転軸との長さを決定してヤコビ行列を構成して前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算し、
前記角度演算手段は、前記第１回転軸と前記第３回転軸との固定長及び前記第２回転軸と前記第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記第１回転軸と前記第２回転軸との長さにより、前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度、前記第２回転軸が前記第１回転軸及び前記第３回転軸に対して平面に射影した際になす角度、及び、前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度を求め、
前記実リンク構造演算手段は、
前記第１回転軸の回転角を、前記第１回転軸の仮想回転角度及び前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度から演算し、
前記第２回転軸の回転角を、前記第２回転軸の仮想回転角度及び記第２回転軸が前記第１回転軸及び前記第３回転軸に対して平面に射影した際になす角度から演算し、
前記第３回転軸の回転角を、前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度から演算する、請求項１又は２に記載の機構制御装置。
【請求項４】
隣り合う３つの平行な回転軸の周りで回転可能な回転関節を含む片足６自由度の歩行ロボットを制御するコンピュータを、
ヤコビ行列を用いて前記隣り合う３つの並行な回転軸のそれぞれの回転量を制御して、前記歩行ロボットの歩様において、前記隣り合う３つの平行な回転軸を同一平面上に存在させるものを含ませる回転制御手段として機能させるためのプログラム。
【請求項５】
第１回転軸を中心に回転可能な第１支持部材と、前記第１回転軸に平行な第２回転軸を中心に回転可能な第２支持部材とが、前記第１回転軸及び前記第２回転軸に平行な第３回転軸を中心に回転可能に連結された機構に対して、前記第１回転軸、前記第２回転軸及び前記第３回転軸の各回転角を制御する機構制御方法であって、
仮想リンク構造演算手段が、前記第１回転軸及び前記第２回転軸を２つの回転関節とし、前記第１回転軸と前記第２回転軸との間を１つの直動関節として、ヤコビ行列を構成して前記第１回転軸と前記第２回転軸との長さ並びに前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算し、又は、前記第１回転軸と前記第２回転軸との長さを決定してヤコビ行列を構成して前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算する仮想リンク構造演算ステップと、
角度演算手段が、前記第１回転軸と前記第３回転軸との固定長及び前記第２回転軸と前記第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記第１回転軸と前記第２回転軸との長さにより、前記第１回転軸と前記第２回転軸と前記第３回転軸とが平面に射影した際に互いになす角度を求める角度演算ステップと、
実リンク構造演算手段が、前記第１回転軸及び前記第２回転軸の回転角を、それぞれ、前記仮想リンク構造演算手段により演算された前記第１回転軸及び前記第２回転軸の仮想回転角度、並びに、前記角度演算手段により演算された前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度及び前記第２回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算し、前記第３回転軸の回転角を、前記角度演算手段により演算された前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算する実リンク構造演算ステップと、
回転制御手段が、前記第１回転軸、前記第２回転軸及び前記第３回転軸を中心に、それぞれ、前記実リンク構造演算手段により演算された角度に回転させる回転制御ステップを含む機構制御方法。
【請求項６】
第１支持部材と第２支持部材とが連結された機構を制御する機構制御装置であって、
前記第１支持部材は、第１回転軸を中心とした回転を実現可能なものであり、
前記第２支持部材は、第２回転軸を中心とした回転を実現可能なものであり、
前記第１支持部材及び前記第２支持部材は、一方が、一つの関節により回転を実現可能なものであり、他方は、一つ又は複数の関節により回転を実現可能なものであり、
前記第１支持部材と前記第２支持部材は、第３回転軸を中心に回転可能に連結されたものであり、
前記第１回転軸と前記第２回転軸と前記第３回転軸は、平行であり、
前記第１回転軸と前記第２回転軸と前記第３回転軸の垂直面に射影し、当該垂直面上において、前記第１回転軸、前記第２回転軸及び前記第３回転軸並びに前記第１支持部材及び前記第２支持部材にそれぞれ対応する、仮想第１回転軸、仮想第２回転軸及び仮想第３回転軸並びに仮想第１支持部材及び仮想第２支持部材に対して、前記仮想第１回転軸及び前記仮想第２回転軸を２つの回転関節とし、前記仮想第１回転軸と前記仮想第２回転軸との間を１つの直動関節として、前記仮想第１回転軸と前記仮想第２回転軸との長さ並びに前記仮想第１回転軸の仮想回転角度及び前記仮想第２回転軸の仮想回転角度を演算する仮想リンク構造演算手段と、
前記仮想第１回転軸と前記仮想第３回転軸との固定長及び前記仮想第２回転軸と前記仮想第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記仮想第１回転軸と前記仮想第２回転軸との長さにより、前記仮想第１回転軸と前記仮想第２回転軸と前記仮想第３回転軸とが平面に射影した際に互いになす角度を求める角度演算手段と、
前記第１回転軸及び前記第２回転軸の回転角を、それぞれ、前記仮想リンク構造演算手段により演算された前記仮想第１回転軸及び前記仮想第２回転軸の仮想回転角度、並びに、前記角度演算手段により演算された前記仮想第１回転軸が前記仮想第２回転軸及び前記仮想第３回転軸に対して平面に射影した際になす角度及び前記仮想第２回転軸が前記仮想第３回転軸及び前記仮想第１回転軸に対して平面に射影した際になす角度により演算し、前記第３回転軸の回転角を、前記角度演算手段により演算された前記仮想第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算する実リンク構造演算手段と、
前記第１回転軸、前記第２回転軸及び前記第３回転軸を、それぞれ、前記実リンク構造演算手段により演算された角度に回転させる回転制御手段を備える機構制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本願発明は、機構制御装置、プログラム及び機構制御方法に関し、特に、例えば片足６自由度の歩行ロボットにおける膝関節とこれにリンクする２つの回転関節のそれぞれの回転角を制御する機構制御装置等に関する。
【背景技術】
【０００２】
始点と終点の間に関節を持った物体を考える。３次元空間内で、終点が届く範囲で自由に位置と姿勢を決めるためには、６個の関節が必要であることが知られている。このため、市販のロボットアームや歩行ロボットの足は、６個の回転関節で実現されている場合が多い。与えられた位置・姿勢から６個の関節角度を求めることは、逆運動学と呼ばれる。通常、ヤコビ行列（ヤコビアン）を用いた逆運動学によって、位置・姿勢制御が行われている。すなわち、ロボットを歩行させる際、ヤコビ行列を用いる逆運動学に基づいて足の関節角度を決定し、関節モータの角度を指定することが一般的である。
【０００３】
このヤコビ行列を用いる逆運動学には、特異点と呼ばれる姿勢が存在する。特異点付近では、ヤコビ行列から実現可能な関節角度を求めることはできない。この特異点は、ロボットアームがまっすぐな状態にあるような姿勢である。２足歩行では、特異点は、膝が伸びた状態に相当する。このため、片足を６個の回転関節で構成する２足歩行ロボットにおいて、単純なヤコビ行列を用いた関節角度指定では、膝を伸ばした歩行が実現できない。そのため、例えば、ホンダ製のＡＳＩＭＯをはじめとする２足歩行ロボットの多くでは、歩行は、膝を曲げた状態で行っている。
【０００４】
なお、早稲田大学で開発されたＷＡＢＩＡＮ−２Ｒ（非特許文献１参照）は、腰に２自由度を持たせることで特異点問題を回避している。産業技術総合研究所のＨＲＰ−４Ｃ（非特許文献２参照）も、膝伸ばし２足歩行を、冗長自由度を用いる際に特徴的な手法を用いて実現している。また、ロボガレージ製のクロイノで実現されているＳＨＩＮ−Ｗａｌｋ（特許文献１参照）は、片足６自由度ロボットによる膝伸ばし２足歩行を、ヤコビ行列を用いずに実現している。また、山形大学の妻木氏のグループは、片足６自由度ロボットに対して、特異点付近で床下支点球面振子モデルから本来とは異なる軌道を用意することで、特異点問題を回避している（非特許文献３参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第３７２９４５９号公報
【非特許文献】
【０００６】
【非特許文献１】“Biped  Humanoid  Robot  WABIAN-2R”、[online]、[平成２４年５月２９日検索]、インターネット<URL：http://www.takanishi.mech.waseda.ac.jp/top/research/wabian/index_j.htm>
【非特許文献２】K. Miura、外５名著，Human-like  Walking  with  Toe  Supporting  for  Humanoids，Proceedings  of  The  2011  IEEE/RSJ  International  Conference  on  Intelligent  Robots  and  Systems，pp.4428-4435，2011/09．
【非特許文献３】“Humanoid  robot”、[online]、[平成２４年５月２９日検索]、インターネット<URL：http://telerobotics.yz.yamagata-u.ac.jp/research/humanoid/humanoid.html>
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、早稲田大学で開発されたＷＡＢＩＡＮ−２Ｒは、腰に２自由度を持たせるため、余分に関節が必要となり、コストの増大やアルゴリズムの複雑化などの問題がある。産業技術総合研究所のＨＲＰ−４Ｃも同様である。
【０００８】
また、ロボガレージ製のクロイノで実現されているＳＨＩＮ−Ｗａｌｋは、ヤコビ行列を用いる逆運動学に基づいておらず、汎用性に乏しいという問題がある。
【０００９】
また、山形大学の妻木氏のグループによる研究は、別の軌道を用意する必要があり、さらに、アルゴリズムが複雑であるという問題がある。
【００１０】
そこで、本願発明は、ヤコビ行列を用いた単純な逆運動学であっても、例えば片足６自由度の歩行ロボットで膝関節を伸ばした姿勢を実現可能な機構制御装置等を提案することを目的とする。
【課題を解決するための手段】
【００１１】
本願発明の第１の観点は、第１回転軸を中心に回転可能な第１支持部材と、前記第１回転軸に平行な第２回転軸を中心に回転可能な第２支持部材とが、前記第１回転軸及び前記第２回転軸に平行な第３回転軸を中心に回転可能に連結された機構に対して、前記第１回転軸、前記第２回転軸及び前記第３回転軸の各回転角を制御する機構制御装置であって、前記第１回転軸及び前記第２回転軸を２つの回転関節とし、前記第１回転軸と前記第２回転軸との間を１つの直動関節として、前記第１回転軸と前記第２回転軸との長さ並びに前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算する仮想リンク構造演算手段と、前記第１回転軸と前記第３回転軸との固定長及び前記第２回転軸と前記第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記第１回転軸と前記第２回転軸との長さにより、前記第１回転軸と前記第２回転軸と前記第３回転軸とが平面に射影した際に互いになす角度を求める角度演算手段と、前記第１回転軸及び前記第２回転軸の回転角を、それぞれ、前記仮想リンク構造演算手段により演算された前記第１回転軸及び前記第２回転軸の仮想回転角度、並びに、前記角度演算手段により演算された前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度及び前記第２回転軸が前記第３回転軸及び前記第１回転軸に対して平面に射影した際になす角度により演算し、前記第３回転軸の回転角を、前記角度演算手段により演算された前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算する実リンク構造演算手段と、前記第１回転軸、前記第２回転軸及び前記第３回転軸を、それぞれ、前記実リンク構造演算手段により演算された角度に回転させる回転制御手段を備えるものである。
【００１２】
本願発明の第２の観点は、第１の観点の機構制御装置であって、前記第３回転軸は、歩行ロボットにおける足の膝関節であり、前記第１回転軸及び前記第２回転軸は、それぞれ、前記第１支持部材及び前記第２支持部材により前記膝関節につながれた関節であり、前記歩行ロボットの歩様には、前記第１回転軸、前記第２回転軸及び前記第３回転軸が同一平面上に存在するものが含まれるものである。
【００１３】
本願発明の第３の観点は、第１又は第２の観点の機構制御装置であって、前記仮想リンク構造演算手段は、前記第１回転軸及び前記第２回転軸を２つの回転関節とし、前記第１回転軸と前記第２回転軸との間を１つの直動関節として、ヤコビ行列を構成して前記第１回転軸と前記第２回転軸との長さ並びに前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算し、又は、前記第１回転軸と前記第２回転軸との長さを決定してヤコビ行列を構成して前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算し、前記角度演算手段は、前記第１回転軸と前記第３回転軸との固定長及び前記第２回転軸と前記第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記第１回転軸と前記第２回転軸との長さにより、前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度、前記第２回転軸が前記第１回転軸及び前記第３回転軸に対して平面に射影した際になす角度、及び、前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度を求め、前記実リンク構造演算手段は、前記第１回転軸の回転角を、前記第１回転軸の仮想回転角度及び前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度から演算し、前記第２回転軸の回転角を、前記第２回転軸の仮想回転角度及び記第２回転軸が前記第１回転軸及び前記第３回転軸に対して平面に射影した際になす角度から演算し、前記第３回転軸の回転角を、前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度から演算するものである。
【００１４】
本願発明の第４の観点は、隣り合う３つの平行な回転軸の周りで回転可能な回転関節を含む片足６自由度の歩行ロボットを制御するコンピュータを、ヤコビ行列を用いて前記隣り合う３つの並行な回転軸のそれぞれの回転量を制御して、前記歩行ロボットの歩様において、前記隣り合う３つの平行な回転軸を同一平面上に存在させるものを含ませる回転制御手段として機能させるためのプログラムである。
【００１５】
本願発明の第５の観点は、第１回転軸を中心に回転可能な第１支持部材と、前記第１回転軸に平行な第２回転軸を中心に回転可能な第２支持部材とが、前記第１回転軸及び前記第２回転軸に平行な第３回転軸を中心に回転可能に連結された機構に対して、前記第１回転軸、前記第２回転軸及び前記第３回転軸の各回転角を制御する機構制御方法であって、仮想リンク構造演算手段が、前記第１回転軸及び前記第２回転軸を２つの回転関節とし、前記第１回転軸と前記第２回転軸との間を１つの直動関節として、ヤコビ行列を構成して前記第１回転軸と前記第２回転軸との長さ並びに前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算し、又は、前記第１回転軸と前記第２回転軸との長さを決定してヤコビ行列を構成して前記第１回転軸の仮想回転角度及び前記第２回転軸の仮想回転角度を演算する仮想リンク構造演算ステップと、角度演算手段が、前記第１回転軸と前記第３回転軸との固定長及び前記第２回転軸と前記第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記第１回転軸と前記第２回転軸との長さにより、前記第１回転軸と前記第２回転軸と前記第３回転軸とが平面に射影した際に互いになす角度を求める角度演算ステップと、実リンク構造演算手段が、前記第１回転軸及び前記第２回転軸の回転角を、それぞれ、前記仮想リンク構造演算手段により演算された前記第１回転軸及び前記第２回転軸の仮想回転角度、並びに、前記角度演算手段により演算された前記第１回転軸が前記第２回転軸及び前記第３回転軸に対して平面に射影した際になす角度及び前記第２回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算し、前記第３回転軸の回転角を、前記角度演算手段により演算された前記第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算する実リンク構造演算ステップと、回転制御手段が、前記第１回転軸、前記第２回転軸及び前記第３回転軸を中心に、それぞれ、前記実リンク構造演算手段により演算された角度に回転させる回転制御ステップを含むものである。
【００１６】
本願発明の第６の観点は、第１支持部材と第２支持部材とが連結された機構を制御する機構制御装置であって、前記第１支持部材は、第１回転軸を中心とした回転を実現可能なものであり、前記第２支持部材は、第２回転軸を中心とした回転を実現可能なものであり、前記第１支持部材及び前記第２支持部材は、一方が、一つの関節により回転を実現可能なものであり、他方は、一つ又は複数の関節により回転を実現可能なものであり、前記第１支持部材と前記第２支持部材は、第３回転軸を中心に回転可能に連結されたものであり、前記第１回転軸と前記第２回転軸と前記第３回転軸は、平行であり、前記第１回転軸と前記第２回転軸と前記第３回転軸の垂直面に射影し、当該垂直面上において、前記第１回転軸、前記第２回転軸及び前記第３回転軸並びに前記第１支持部材及び前記第２支持部材にそれぞれ対応する、仮想第１回転軸、仮想第２回転軸及び仮想第３回転軸並びに仮想第１支持部材及び仮想第２支持部材に対して、前記仮想第１回転軸及び前記仮想第２回転軸を２つの回転関節とし、前記仮想第１回転軸と前記仮想第２回転軸との間を１つの直動関節として、前記仮想第１回転軸と前記仮想第２回転軸との長さ並びに前記仮想第１回転軸の仮想回転角度及び前記仮想第２回転軸の仮想回転角度を演算する仮想リンク構造演算手段と、前記仮想第１回転軸と前記仮想第３回転軸との固定長及び前記仮想第２回転軸と前記仮想第３回転軸との固定長、並びに、前記仮想リンク構造演算手段により求められた前記仮想第１回転軸と前記仮想第２回転軸との長さにより、前記仮想第１回転軸と前記仮想第２回転軸と前記仮想第３回転軸とが平面に射影した際に互いになす角度を求める角度演算手段と、前記第１回転軸及び前記第２回転軸の回転角を、それぞれ、前記仮想リンク構造演算手段により演算された前記仮想第１回転軸及び前記仮想第２回転軸の仮想回転角度、並びに、前記角度演算手段により演算された前記仮想第１回転軸が前記仮想第２回転軸及び前記仮想第３回転軸に対して平面に射影した際になす角度及び前記仮想第２回転軸が前記仮想第３回転軸及び前記仮想第１回転軸に対して平面に射影した際になす角度により演算し、前記第３回転軸の回転角を、前記角度演算手段により演算された前記仮想第３回転軸が前記第１回転軸及び前記第２回転軸に対して平面に射影した際になす角度により演算する実リンク構造演算手段と、前記第１回転軸、前記第２回転軸及び前記第３回転軸を、それぞれ、前記実リンク構造演算手段により演算された角度に回転させる回転制御手段を備えるものである。
【００１７】
本願発明において、例えば「回転軸Ａが回転軸Ｂ及び回転軸Ｃに対して平面に射影した際になす角」という表現は、回転軸Ａ、Ｂ及びＣの回転関節を平面に投影した際に、各回転関節を繋げるリンクが互いになす角を意味する。例えば「仮想第１、第２及び第３回転軸が平面に射影した際に互いになす角度」という表現は、仮想第１、第２及び第３回転軸に対応する仮想第１、第２、第３回転関節を平面に投影した際に、各仮想回転関節を繋げるリンクが互いになす角度を意味する。
【００１８】
なお、本願発明は、例えば第１、第３、第５及び第６の観点について、歩行動作の制御のためのものに限定されず、例えばアーム等のような機構の動作制御のためのものであってもよい。
【発明の効果】
【００１９】
本願発明によれば、実際にリンクされた、第１回転軸、第２回転軸及び第３回転軸のそれぞれの３つの回転関節に代えて、第１回転軸及び第２回転軸のそれぞれの２つの回転関節及び第１回転軸と第２回転軸との間の１つの直動関節の構成として仮想的なリンク機構を構成することにより、第３回転軸が伸びた状態でも特異点が発生することなく、回転関節の角度と直動関節の変位とを計算することが可能になる。第１回転軸と第３回転軸との距離及び第２回転軸と第３回転軸との距離は一定であるため、余弦定理を用いて、簡単に、第１回転軸、第２回転軸及び第３回転軸の実際の関節角度を計算することができる。よって、たとえ片足６自由度の回転関節をもった歩行ロボットであっても、逆運動学によって、容易に膝が伸びた状態を含む歩様を実現することが可能になる。特に、ヤコビ行列を用いた逆運動学によって歩様が実現可能になり、汎用性が高まる。この仮想リンクに基づく片足６回転関節を持った歩行ロボットの動歩行や静歩行は、既存の歩行パターンジェネレータを組み合わせて実現することができるものである。
【００２０】
さらに、本願発明は、余弦定理を用いて回転角度を計算できればよい。そのため、一つの回転関節で実現するだけでなく、複数の回転関節を組み合わせることにより、同一平面上で、平行な第１回転軸・第２回転軸・第３回転軸を中心にした回転が実現可能なものであれば実現が可能である。さらに、例えば第１回転軸（第２回転軸）と第３回転軸が平行でなく実リンク構造としては同一平面上での回転角を求められなくても、少なくとも連続する２つの関節が平行であれば６自由度の範囲内で構成することが可能であり、例えば第６の観点にあるように、これらの軸の垂直面に実リンク構造を射影して仮想リンク構造を形成して、実リンク構造の角度を求めることができる。
【図面の簡単な説明】
【００２１】
【図１】本願発明の実施の形態の一例である機構制御装置の概略ブロック図である。
【図２】図１の二足歩行ロボット３の下半身における回転関節とリンクの関係を示す図である。
【図３】図１の回転関節２９（膝関節）とこの回転関節２９につながっている２つの支持部材３３及び３５を、１つの仮想直動関節と１つの仮想リンクに置換した場合の関係を示す図である。
【図４】図１の機構制御装置１の動作の一例を示すフロー図である。
【図５】PUMA型ロボットアームの一例を示す図である。
【図６】図５のオイラー角をピッチ・ロール・ヨーに入れ替えて、仮想の回転軸を構成する概要を示す図である。
【発明を実施するための形態】
【００２２】
以下では、図面を参照して、本願発明の実施例について説明する。なお、本願発明は、この実施例に限定されるものではない。
【実施例】
【００２３】
図１は、本願発明の実施の形態の一例である機構制御装置の概略ブロック図である。
【００２４】
機構制御装置１が制御する２足歩行ロボット３は、片足６自由度のものである。すなわち、２足歩行ロボット３は、片足に、胴体２１から足先２３にかけて、回転関節２５、２７、２９及び３１を備える。
【００２５】
図１において、左向きをｘ軸、裏から表への向きをｙ軸、上向きをｚ軸とし、ｘ軸周りの回転をロール（roll）、ｙ軸周りの回転をピッチ（pitch）、ｚ軸周りの回転をヨー（yaw）とする。回転関節２５はヨーとロール、回転関節２７（本願発明の各観点の「第１回転軸」の回転関節の一例）はピッチ、回転関節２９（膝関節）（本願発明の各観点の「第３回転軸」の回転関節の一例）はピッチ、回転関節３１（ピッチを実現する場合が、本願発明の各観点の「第２回転軸」の回転関節の一例）はピッチとロールを実現するものである。
【００２６】
回転関節２７及び２９並びにピッチを実現する回転関節３１は、回転軸が平行である。回転関節２５のヨーの回転角度をｑ1、ロールの回転角度をｑ2、回転関節２７のピッチの回転角度をｑ3、回転関節２９のピッチの回転角度をｑ4、回転関節３１のピッチの回転角度をｑ5、ロールの回転角度をｑ6とする。
【００２７】
回転関節２７と回転関節２９は、支持部材３３（本願発明の各観点の「第１支持部材」の一例）によりつながれている。回転関節２７と回転関節２９の間の長さをｌaとする。回転関節２９と回転関節３１は、支持部材３５（本願発明の各観点の「第２支持部材」の一例）によりつながれている。回転関節２９と回転関節３１の間の長さをｌbとする。
【００２８】
機構制御装置１は、仮想リンク構造演算部１１（本願発明の各観点の「仮想リンク構造演算手段」の一例）と、角度計算部１３（本願発明の各観点の「角度計算手段」の一例）と、実リンク構造演算部１５（本願発明の各観点の「実リンク構造演算手段」の一例）と、回転制御部１７（本願発明の各観点の「回転制御手段」の一例）を備える。
【００２９】
仮想リンク構造演算部１１は、回転関節２９を直動関節に置換した仮想的なリンク機構により導出されるヤコビ行列を演算して、仮想的なリンク構造における微小変位を求めるものである。
【００３０】
角度計算部１３は、回転関節２７と回転関節２９との間、及び、回転関節２９と回転関節３１との間の距離が固定長であることを利用して、余弦定理により、回転関節２７と回転関節２９と回転関節３１とを頂点とする三角形の各頂点の内角を求めるものである。
【００３１】
実リンク構造演算部１５は、仮想リンク構造演算部１１が計算した仮想的なリンク機構における各関節の回転角及び角度計算部１３が計算した回転関節２７と回転関節２９と回転関節３１とを頂点とする三角形の各頂点の内角により、回転関節２５、２７、２９及び３１の回転角を演算するものである。
【００３２】
回転制御部１７は、実リンク構造演算部１５により演算された回転角を用いて、回転関節２５、２７、２９及び３１の回転角を制御するものである。
【００３３】
図２は、二足歩行ロボット３の下半身を、ロボットに対して左から見たときの、回転関節とリンクの関係を示す図である。逆運動学は、エンドエフェクタの位置及び姿勢を満たす関節角度（回転関節の場合）、関節変位（直動関節の場合）を求める計算である。従来よく用いられてきたヤコビ行列を利用した逆運動学では、ヤコビ行列Ｊと、関節角度の微小変位δｑ及びエンドエフェクタの微小変位、微小回転［δｐ  δω］Tを用いて、式(1)のように解く。
【００３４】
【数１】
2014014920.tif 000003
【００３５】
しかし、片足６自由度のロボットが膝を伸ばした姿勢をとっている場合、ｄｅｔＪ＝０となるため、Ｊ-1が発散してしまい、ｑが無限大として得られ、計算が破綻してしまう。このように、ｄｅｔＪ＝０となるような姿勢を特異姿勢という。以上のように、従来の逆運動学計算手法では、特異姿勢における計算が破綻してしまうため、特異姿勢を利用した動作の制御ができないという問題がある。その問題の一端として、片足６自由度の二足歩行ロボットは、膝を伸ばした姿勢（膝特異姿勢）を利用することができず、膝を曲げたままの不自然な歩行動作を強いられてきた。
【００３６】
図３は、回転関節２９（膝関節）とこの回転関節２９につながっている２つの支持部材３３及び３５を、１つの仮想直動関節と１つの仮想リンク（virtual  link）に置換したものを示す。図３の仮想的なリンク構造には、膝関節に対応する回転関節がないため、膝特異姿勢になることがない。図４は、図１の機構制御装置１の動作の一例を示すフロー図である。図３及び図４を参照して、図１の機構制御装置１の動作の一例について説明する。
【００３７】
図３の仮想的なリンク構造も、現実のリンク構造と同じく６自由度である。従来と同様に、ヤコビ行列を用いた逆運動学計算を行う。ただし、その際、膝関節が、回転関節（ｑ4）から直動関節（＾ｑ4）に置換しており、それに伴い、回転関節２７及び３１の回転角度は、直動関節との角度＾ｑ3及び＾ｑ5として扱う。
【００３８】
具体的なヤコビ行列の一例について説明する。なお、本願発明におけるヤコビ行列については、以下に説明するものに限定されない。６自由度に対するヤコビ行列は、６×６行列である。ｉ番目の関節が回転関節である場合、ヤコビ行列のｉ列目は、式(2)である。ｉ番目の関節が直動関節である場合、ヤコビ行列のｉ列目は、式(3)である。ここで、座標系Σwを、ロボットの足の付け根（図１の胴体２１）のローカル座標系として、ｗaiは、座標系Σwで表した関節ｉの関節軸方向の単位ベクトル、ｐiは、Σwで表された関節ｉのローカル座標系原点を表すベクトルである。
【００３９】
【数２】
2014014920.tif 000004
【００４０】
膝関節が腰からｊ番目の関節である場合、ヤコビ行列のｊ列に式(3)のベクトルを用いる。それ以外の列は、回転関節という前提であるため、式(2)のベクトルを用いる。また、ｐiを得るために、順運動学によって各関節の座標を求め、そこから仮想リンクの長さｌvを計算する。以上のようにして、ヤコビ行列が得られる。図１の仮想リンク構造演算部１１は、このヤコビ行列を用いて演算して、図３の仮想的リンク機構における微小関節変位を求め、関節角度を求める（図４のステップＳＴ１）。
【００４１】
図３において、回転関節２７と回転関節２９と回転関節３１とを頂点とする三角形に着目する。この三角形の各辺は、実リンクと仮想リンクから構成されている。そして、その辺の長さｌa、ｌb及びｌvは、いずれも長さが分かっている。角度計算部１３は、余弦定理により、各頂点の内角を求める（図４のステップＳＴ２）。回転関節２７を頂点とする内角を∠Ａ、回転関節２９を頂点とする内角を∠Ｂ、回転関節３１を頂点とする内角を∠Ｃとする。
【００４２】
回転関節２５の回転角ｑ1及びｑ2、回転関節３１のロールの回転角ｑ6は、仮想リンク構造演算部１１により演算されている。実リンク構造演算部１５は、式(4)、(5)及び(6)により、それぞれ、回転関節２７の現実の回転角ｑ3、回転関節２９の現実の回転角ｑ4及び回転関節３１のピッチの現実の回転角ｑ5を演算する（図４のステップＳＴ３）。なお、式(4)、(5)及び(6)は、概念的な式であり、実際計算する際には関節の回転方向の正負を考慮する必要がある。
【００４３】
回転制御部１７は、各回転関節の回転角を用いて、回転関節２５、２７、２９及び３１の回転角を制御する（図４のステップＳＴ４）。
【００４４】
【数３】
2014014920.tif 000005
【００４５】
なお、本実施例では、仮想リンク構造演算部１１が、６個の連立方程式を逆行列によって一度に解く場合（以下、「６自由度の場合」という。）について、具体的に説明した。その他、仮想リンク構造演算部１１は、例えば、仮想直動リンクの長さを先に求め、５個の連立方程式を解くこと（以下、「５自由度の場合」という。）により、近似解を求めるものであってもよい。すなわち、例えば、関節ｑ1〜ｑ6の位置座標ｐ1〜ｐ6を使って説明すると、ｐ3の現在座標とｐ3の目標座標（次の計算結果から予測される座標）に大きな差が生じない構造又は動作の場合（動作によってｐ3に及ぼすｐ1、ｐ2の影響が異なるため、その影響が小さい場合には近似してよい）には５自由度を使用してもよく、それが保証できない場合は６自由度を使用するようにしたものである。
【００４６】
６自由度の場合の一例について具体的にまとめると、各関節の現在座標（ｐ1〜ｐ6）を、順運動学計算により求める。実リンク構造を仮想リンク構造に置換し、ｐ5とｐ3の現在座標から、座標間の距離を計算して仮想直動リンクの現在の長さ（ｌv）を得る。この仮想リンク構造に対し、６自由度のヤコビ行列を用い、６個の連立方程式を解く。そして、得られた直動関節の微小変位を、仮想直動リンクの現在の長さｌvに足して、目標の長さｌvを求める。そして、ｌa、ｌb、ｌvで構成される三角形に対し、余弦定理を用いて、仮想関節角度を現実の関節角度に戻す。
【００４７】
他方、５自由度の場合の一例について具体的に説明する。関節の現在座標ｐ5及びｐ6が同一座標とみなせる場合に、各関節の現在座標（ｐ1〜ｐ6）を順運動学計算により求める。実リンク構造を仮想リンク構造に置換し、目標足先座標（ｐ6の目標座標）とｐ3の現在座標から座標間の距離を計算して仮想直動リンクの目標の長さ（ｌv）を近似的に得る。直動関節を固定（仮想直動リンクの長さをｌvで固定）した仮想リンク構造に対し、５自由度のヤコビ行列を用い、５個の連立方程式を解く。仮想直動リンクの目標の長さｌvは近似的に求めたものを使用して、ｌa、ｌb、ｌvで構成される三角形に対し、余弦定理を用いて、仮想関節角度を現実の関節角度に戻す。
【００４８】
すなわち、５自由度の場合は、ｌvに関係する変数を先に近似的に求めて、そのあと、５変数に対する６個の連立方程式を考えていることになる。上記のように、その１つの方程式を除いてヤコビ行列を正方にしてから逆行列を解くものでもよく、また、６個の方程式から最小二乗法等により、疑似逆行列を用いて計算するようにしてもよい。
【００４９】
なお、本願発明は、歩行動作の制御のためのものに限定されず、例えばアーム等であっても、平行な第１回転軸と第２回転軸と第３回転軸を中心とした回転が実現可能なものであればよい。
【００５０】
続いて、回転軸の垂直面への射影を用いた手法について説明する。３つの回転関節の回転軸（第１回転軸，第２回転軸，第３回転軸）が平行な場合には、これらの軸に垂直な平面に射影して、図２及び図３と同様の状態にすることができる。
【００５１】
他方、隣り合う３つの回転関節の回転軸（第１回転軸、第２回転軸及び第３回転軸）が平行でない場合は、単なる射影では解決することができない。ただし、以下のように、関節の回転順番を入れ替える（図６を参照）ことで、仮想的に、第１回転軸、第２回転軸及び第３回転軸が平行であるような関節構成にすることができる。この場合、仮想直動関節を用いた逆運動学が適用可能となる。
【００５２】
隣り合う３つの回転関節の回転軸が平行でない場合について検討する。これは、第３回転軸と第１回転軸が平行でなく、第１回転軸は、第３回転軸が存在しない側に、２つの回転関節（第４回転軸を中心とする回転関節及び第５回転軸を中心とする回転関節）を有し、第１回転軸、第４回転軸及び第５回転軸が１点で交差する場合である。具体的には、図５のPUMA型ロボットアームを例にして説明する。
【００５３】
この条件を満足することは、回転軸と平行でない側が、その先の２関節を合わせて３次元空間の姿勢を表現できること（オイラー角又はロール・ピッチ・ヨー角（ロール，ピッチ，ヨーの順番は問わないが，先頭に来るいずれか一つは，平行に３つ連続で並んでいる場合に相当するので除外できる））を意味する。その場合、関節の入れ替えが可能である。オイラー角とロール・ピッチ・ヨー角は、互いに変換可能である。よって、仮想的に入れ替えても、元に戻すことができる。
【００５４】
図５は、PUMA型ロボットアームにおける関節の構造の一例を示す概略図である。PUMA型ロボットアームは、市販のロボットアームで多く採用されている構造である。図５では、J1軸とJ2軸が肩、J3軸が肘に相当する。J4〜J6軸の３軸で、肘より先を実現している。このPUMA型ロボットアームは、J2軸（ピッチ軸）とJ3軸（ピッチ軸）は平行で、J4軸（ヨー軸）は平行ではなく、３つ連続していない。J4〜J6軸は、オイラー角と呼ばれる構成をしており、３軸が１点で交わる。よって、上記条件に当てはまる。そこで、図６にあるように、オイラー角をピッチ・ロール・ヨーに入れ替えて、仮想のJ^4，J^5，J^6軸を構成する。すなわち、図６を参照して、まず、３軸が一点で交差している（図６（ａ）参照）。そのため、回転関節の軸の交点に関節をスライドする（図６（ｂ）参照）。そして、球体関節とする（図６（ｃ）参照）。そして、平行な回転軸を含む仮想ピッチ・ロール・ヨー角に置き換える（図６（ｄ）参照）。これによって、J2，J3，J^4が平行になり、これまでの方法が使え、肘に相当するJ3軸の特異点問題を解消することができる。
【００５５】
このPUMA型ロボットアームの関節角度を求める手順をまとめると、次のようになる。
１．オイラー角（ヨー・ピッチ・ヨー角）を仮想ピッチ・ロール・ヨー角に置き換える。
２．３つ連続するピッチ角を仮想直動関節と両端仮想ピッチ角構成に置き換える。
３．２回置き換えた仮想リンク構造に対する逆運動学問題を解く。
４．仮想直動関節と両端仮想ピッチ角を３つの連続するピッチ角に戻す。
５．ピッチ・ロール・ヨー角をオイラー角に戻す。
【符号の説明】
【００５６】
１  機構制御装置、３  歩行ロボット、１１  仮想リンク構造演算部、１３  角度計算部、１５  実リンク構造演算部、１７  回転制御部、２１  胴体、２３  足先、２５，２７，２９，３１  回転関節、３３，３５  支持部材
【図１】
2014014920.tif 000006
【図２】
2014014920.tif 000007
【図３】
2014014920.tif 000008
【図４】
2014014920.tif 000009
【図５】
2014014920.tif 000010
【図６】
2014014920.tif 000011