冗長マニピュレータの冗長自由度の決定方法
【課題】冗長マニピュレータ１の手先８をＣＰ制御にて変化させるに際し、アームアングルψの連続性を担保する技術を提供する。
【解決手段】制御装置１００は、各ステップにおける手先８の位置及び姿勢状態を取得する状態取得手段２０と、各ステップにおける手先８の位置及び姿勢状態に基づいて、各ステップにおけるアームアングルψの実現可能な領域を算出する領域算出手段２１と、隣り合うステップ間で領域同士が少なくとも一部重複するように、ステップｓ０からステップｓｎに至る間の領域の組み合わせを作成する組み合わせ作成手段２２と、領域の組み合わせに基づいて、ＣＰ制御の各ステップにおけるアームアングルψを決定する冗長自由度決定手段２３と、を備える。
【解決手段】制御装置１００は、各ステップにおける手先８の位置及び姿勢状態を取得する状態取得手段２０と、各ステップにおける手先８の位置及び姿勢状態に基づいて、各ステップにおけるアームアングルψの実現可能な領域を算出する領域算出手段２１と、隣り合うステップ間で領域同士が少なくとも一部重複するように、ステップｓ０からステップｓｎに至る間の領域の組み合わせを作成する組み合わせ作成手段２２と、領域の組み合わせに基づいて、ＣＰ制御の各ステップにおけるアームアングルψを決定する冗長自由度決定手段２３と、を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御される冗長マニピュレータの各ステップにおける冗長自由度の決定方法、決定装置、及び、決定プログラムに関する。
【背景技術】
【０００２】
作業に必要な自由度より多くの自由度を有する冗長マニピュレータは、近年、ロボット分野において広く用いられるようになってきている。これは、冗長マニピュレータが有する余分な自由度を有効活用することで、例えば、ロボットの可操作性が向上したり、関節トルクの最適化が図れたり、障害物を回避できたり、所謂特異点を回避できたり、関節限界を回避できたりするからである。
【０００３】
非特許文献１は、このような冗長マニピュレータの一例として、７自由度冗長マニピュレータについて言及している。一般に、空間内におけるマニピュレータの手先の位置及び姿勢は、６つのパラメータで表現することができる。従って、７自由度マニピュレータには余分な自由度が１つ存在していることになる。
【０００４】
先ずは、この７自由度冗長マニピュレータについて図２２を参照しつつ説明する。図２２に示すように、７自由度冗長マニピュレータの関節は、すべて回転関節で構成されている。肩部の三関節（第１〜第３関節、θ１〜θ３）の関節軸は一点で交差している。同様に、手首部の三関節（第５〜第７関節、θ５〜θ７）の関節軸も一点で交差している。また、隣り合う一対の関節の軸同士は、互いに直交する。このように肩部の三関節と手首部の三関節は、単純な構造をしているため、夫々仮想的な球面機構を構成しているとみなすことができる。そこで、この種のマニピュレータは、一般に、３Ｒ−１Ｒ−３Ｒ型或いはＳ−Ｒ−Ｓ型のマニピュレータとして分類されている。
【０００５】
更に、上記の非特許文献１は、冗長自由度を表現するためのパラメータとして、非特許文献２に開示のアームアングルψを採用している。このアームアングルψは、図２３に示すようにアーム平面と参照面との成す角度として定義されるものである。アーム平面は、肩部三関節の交点Ｐｓ、肘関節位置Ｐｅ及び手首部三関節の交点Ｐｗの三点を通る平面である。もし関節の可動範囲に制限がなければ、手先の位置及び姿勢を固定したまま、アームアングルψのみを自由自在に変えることができよう。しかしながら、実在のマニピュレータでは、機構的な制約のため、関節の可動範囲には制限がある。この結果、アームアングルψの実現可能な領域は自ずと限定されることになる。これに対し、非特許文献１は、アームアングルψの実現可能な領域を代数学的に解く手法を提案している。この手法の詳細は、非特許文献１を参照されたい。要するに、非特許文献１によれば、アームアングルψの実現可能な領域は、１つ又は複数の閉じた領域から構成され得るとしている。
【先行技術文献】
【特許文献】
【０００６】
【非特許文献１】清水昌幸、他4名、「関節の可動範囲を考慮に入れた7自由度冗長マニヒ゜ュレータの解析的逆運動学解法」、日本ロホ゛ット学会誌、日本ロホ゛ット学会、2007年、第25巻、第4号、p.122-133
【非特許文献２】K.Kreutz-Delgado, M.Long and H. Seraji: "Kinematic Analysis of 7-DOF Manipulators," Int.J.Robotics Research, 1992, vol.11, no.5, pp.469-481.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
前述の通り、アームアングルψの実現可能な領域は、１つ又は複数の閉じた領域から構成される。しかし、このようなアームアングルψの実現可能な領域と、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御と、の間には未解決な問題があった。ここで、ＣＰ制御とは、図２４に示すように、（１）第１の位置及び姿勢状態（"ｓ０"を参照）から第２の位置及び姿勢状態（"ｓ６"を参照）に至るまでの、マニピュレータの手先の位置及び姿勢状態を例えば所定の時間間隔毎に細かく設定し、（２）各手先位置毎に、逆運動学を用いて各関節の角度を求め、（３）（２）で求めた各関節の角度をロボットによって再生させる、制御のことである。しかし、冗長マニピュレータは冗長な構成であるがため、上記（２）において各関節の角度が一意に定まらず、冗長分の自由度を何らかの基準に基づいて予め決定する必要がある。以下、図２４及び図２５を参照しつつ、この問題を詳細に説明する。なお、説明の便宜上、冗長自由度を表すパラメータとしてアームアングルψを継続して使用するが、冗長自由度を表すパラメータとしてアームアングルψを使用しなければならない特段の事情はなく、他のパラメータを代わりに使用してもよいことに留意されたい。
【０００８】
図２５には、図２４で示した各ステップにおけるアームアングルψの実現可能な領域を示している。図２５の横軸に並べたｓ０〜ｓ６は、図２４に示すステップｓ０〜ｓ６に夫々対応している。図２５の縦軸は、アームアングルψである。図２５において、上付添字と下付添字を有する「ψ」は、アームアングルψの実現可能な領域（範囲）を意味している。「ψ」の上付添字は、その領域が属するステップを識別するための番号を意味する。一方、「ψ」の下付添字は、その領域が属するステップにおける各領域の識別番号である。確認のため繰り返すと、「ψ」はアームアングルを意味するが、上付添字と下付添字を伴った「ψ」はアームアングルψの実現可能な領域を意味している。文章中では、このアームアングルψの実現可能な領域を、便宜上、ψ（上付添字，下付添字）のようにして表記する。
【０００９】
さて、従来では、冗長分の自由度を予め決定するための基準に関する知見は一切なかった。従って、ステップ０におけるアームアングルψを予め決定するための基準に関する知見も同様になかった。それでは、本願発明者らはどうやってステップ０におけるアームアングルψを決定していたか。本願発明者らに対するインタビューによれば、以下の通りである。即ち、本願発明者らは、先ず、ステップ０におけるψ（０，１）とψ（０，２）の大小を比較し、図２５の場合ではψ（０，１）＞ψ（０，２）であることから、この単純な大小関係に基づいてψ（０，１）を選択していたのである。そして、本願発明者らは、このψ（０，１）の中間値をステップ０におけるアームアングルψとして採用していた。また、次のステップ１におけるアームアングルψについて言えば、本願発明者らは、アームアングルψの一応の連続性を考慮してψ（０，１）とψ（１，１）との重複関係を重視することで、ψ（１，１）とψ（１，２）のうちψ（１，１）を選択していた。そして、本願発明者らは、このψ（１，１）の中間値をステップ１におけるアームアングルψとして採用していた。
【００１０】
このような場当たり的な基準は、それでも一見すると合理的であるかのようにも見える。しかしながら、この基準によると、ステップ４からステップ５へと移行するときに大きな問題が生じる。というのは、確かに、ψ（０，１）にとっては次のステップ１において自己と重複するアームアングルψの実現可能な領域が存在していた。同様に、ψ（１，１）にとっては次のステップ２において自己と重複するアームアングルψの実現可能な領域が存在していた。しかしながら、ψ（４，１）にとっては次のステップ５において自己と重複するアームアングルψの実現可能な領域が存在していない。従って、仕方なく、アームアングルψの連続性を犠牲にして、唯一存在するψ（５，１）の中から、ステップ５におけるアームアングルψを選択し採用せざるを得なかった。そして、このようにアームアングルψの値が看過できない程に大きく変化する結果、各関節の角速度が有限であることとの兼ね合いもあり、手先が意図した軌道から大きく外れたり、手先が他の物体と干渉したり、といった種々の問題が発生していた。
【００１１】
本願発明の目的は、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、冗長マニピュレータの冗長自由度の連続性を担保する技術を提供することにある。ここで、冗長自由度の連続性とは、「冗長自由度の値の変化がステップ間で緩やかである」ことを意味する。
【課題を解決するための手段】
【００１２】
本願発明の第１の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する方法は、以下の通りである。各ステップにおける前記手先の位置及び姿勢状態を取得する。各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する。隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する。前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の方法によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【００１３】
好ましくは、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上の方法によれば、最終のステップにおいて前記領域が最大化される。従って、前記第２の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第２の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、前記領域の組み合わせの作成が可能となる確率が高くなる。第２に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。
【００１４】
好ましくは、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の方法によれば、前記冗長マニピュレータの動きが滑らかになる。
【００１５】
好ましくは、前記冗長マニピュレータは、７自由度である。
【００１６】
好ましくは、前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である。
【００１７】
本願発明の第２の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する装置は、以下の通りである。決定装置は、各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、を備える。以上の構成によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【００１８】
好ましくは、前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上の構成によれば、最終のステップにおいて前記領域が最大化される。従って、前記第２の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第２の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、前記領域の組み合わせの作成が可能となる確率が高くなる。第２に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。
【００１９】
好ましくは、前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の構成によれば、前記冗長マニピュレータの動きが滑らかになる。
【００２０】
好ましくは、前記冗長マニピュレータは、７自由度である。
【００２１】
好ましくは、前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である。
【００２２】
本願発明の第３の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するためのプログラムは、コンピュータを、各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、として機能させる。以上のプログラムによれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【００２３】
好ましくは、前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上のプログラムによれば、最終のステップにおいて前記領域が最大化される。従って、前記第２の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第２の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、前記領域の組み合わせの作成が可能となる確率が高くなる。第２に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。
【００２４】
好ましくは、前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上のプログラムによれば、前記冗長マニピュレータの動きが滑らかになる。
【００２５】
好ましくは、前記冗長マニピュレータは、７自由度である。
【００２６】
好ましくは、前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である。
【発明の効果】
【００２７】
本願発明によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【図面の簡単な説明】
【００２８】
【図１】図１は、冗長マニピュレータの全体模式図である（第１実施形態）。
【図２】図２は、冗長マニピュレータの機能ブロック図である（第１実施形態）。
【図３】図３は、冗長マニピュレータの制御フローである（第１実施形態）。
【図４】図４は、冗長マニピュレータの制御フローである（第１実施形態）。
【図５】図５は、冗長自由度の実現可能な領域ψの一覧である（第１実施形態）。
【図６】図６は、冗長自由度の実現可能な領域ψの連続性を示すグラフである（第１実施形態）。
【図７】図７は、冗長自由度の実現可能な領域ψの組み合わせを示す図である（第１実施形態）。
【図８】図８は、冗長マニピュレータの全体模式図である（第２実施形態）。
【図９】図９は、冗長自由度の実現可能な領域ψを示す図である（第２実施形態）。
【図１０】図１０は、冗長自由度の実現可能な領域ψの一覧である（第２実施形態）。
【図１１】図１１は、冗長自由度の実現可能な領域ψの連続性を示すグラフである（第２実施形態）。
【図１２】図１２は、冗長自由度の実現可能な領域ψの組み合わせを示す図である（第２実施形態）。
【図１３】図１３は、各ステップにおいて決定された冗長自由度を示す図である（第２実施形態）。
【図１４】図１４は、各ステップにおいて決定された冗長自由度を示す図である（第３実施形態）。
【図１５】図１５は、冗長自由度の実現可能な領域ψを示す図である（第４実施形態）。
【図１６】図１６は、冗長自由度の実現可能な領域ψの一覧である（第４実施形態）。
【図１７】図１７は、冗長マニピュレータの制御フローである（第４実施形態）。
【図１８】図１８は、冗長自由度の実現可能な領域ψの連続性を示すグラフである（第４実施形態）。
【図１９】図１９は、冗長自由度の実現可能な領域ψの組み合わせを示す図である（第４実施形態）。
【図２０】図２０は、図１９に示す複数の組み合わせから選択された組み合わせを示す図である（第４実施形態）。
【図２１】図２１は、各ステップにおいて決定された冗長自由度を示す図である（第４実施形態）。
【図２２】図２２は、冗長マニピュレータの全体模式図である（従来技術）。
【図２３】図２３は、冗長自由度の一例としてのアームアングルの定義説明図である（従来技術）。
【図２４】図２４は、冗長マニピュレータの全体模式図である（従来技術）。
【図２５】図２５は、各ステップにおいて決定された冗長自由度を示す図である（従来技術）。
【発明を実施するための形態】
【００２９】
（第１実施形態）
以下、図１〜７を参照しつつ、本願発明の第１実施形態を説明する。以下、明細書中で「ステップ」とあるのは、ＣＰ制御の制御フローの多種多様な処理のステップを示す場合と、ＣＰ制御によるマニピュレータの動きのステップを示す場合があるので、両者を混同しないように留意されたい。
【００３０】
図１に示すように、本実施形態において冗長マニピュレータ１は、体幹部２と、肩関節部３と、上腕部４と、肘関節部５と、前腕部６と、手首関節部７と、手先８と、制御装置１００（決定装置）と、を主たる構成として備えている。体幹部２は、例えば地面などに固定されている。地面と肩関節部３は体幹部２によって連結されている。肩関節部３と肘関節部５は上腕部４によって連結されている。肘関節部５と手首関節部７は前腕部６によって連結されている。手先８は手首関節部７に支持されている。
【００３１】
肩関節部３は、第１〜第３の関節から構成される球面機構となっている。肘関節部５は、第４の関節から構成されている。手首関節部７は、第５〜第７の関節から構成される球面機構となっている。従って、冗長マニピュレータ１は、所謂Ｓ−Ｒ−Ｓ型マニピュレータに分類される。また、冗長マニピュレータ１は、余分な自由度を１つ有している。以降、冗長自由度を表現するためのパラメータとしては、非特許文献２に開示のアームアングルψを使用するものとする。繰り返すが、冗長自由度を表現するためのパラメータとしては、アームアングルψに限らず、例えば、任意の１自由度の関節角や別の表現のアームアングルでもよい。
【００３２】
図２に示すように、冗長マニピュレータ１は、更に、第１関節モータ（肩部）３０と、第２関節モータ（肩部）３１と、第３関節モータ（肩部）３２と、第４関節モータ（肘部）３３と、第５関節モータ（手首部）３４と、第６関節モータ（手首部）３５と、第７関節モータ（手首部）３６と、を備えている。第１関節モータ（肩部）３０は、肩関節部３を構成する第１関節に搭載され、第１関節を駆動する。同様に、第２関節モータ（肩部）３１は、肩関節部３を構成する第２関節に搭載され、第２関節を駆動する。第３関節モータ（肩部）３２は、肩関節部３を構成する第３関節に搭載され、第３関節を駆動する。第４関節モータ（肘部）３３は、肘関節部５を構成する第４関節に搭載され、第４関節を駆動する。第５関節モータ（手首部）３４は、手首関節部７を構成する第５関節に搭載され、第５関節を駆動する。第６関節モータ（手首部）３５は、手首関節部７を構成する第６関節に搭載され、第６関節を駆動する。第７関節モータ（手首部）３６は、手首関節部７を構成する第７関節に搭載され、第７関節を駆動する。
【００３３】
制御装置１００は、冗長マニピュレータ１の手先８の位置及び姿勢状態を、ＣＰ制御にて、第１の位置及び姿勢状態（ステップｓ０、図１参照）から第２の位置及び姿勢状態（ステップｓｎ、図１参照）へと変化させるに際し、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψの決定する。制御装置１００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１１と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２と、を備える。ROM１２には、制御プログラム（決定プログラム）が記憶されている。この制御プログラムはCPU１０によって読み出され、CPU１０上で実行される。これにより、制御プログラムは、CPU１０などのハードウェアを、状態取得手段２０、領域算出手段２１、組み合わせ作成手段２２、冗長自由度決定手段２３、関節角算出手段２４、CP制御手段２５、として機能させる。
【００３４】
状態取得手段２０は、ＣＰ制御に必要となる、各ステップにおける手先８の位置及び姿勢状態に関する情報としての状態情報を取得する。状態取得手段２０による各状態情報の取得の態様としては、有線又は無線を介した他の制御機器からの受信や、例えばキーボードなどの入力手段を介した操作人員からの入力、ROM１２からの読み込みなどが考えられる。各状態情報は、手先８の空間に対する位置を表すＸ値、Ｙ値、Ｚ値と、手先８の空間に対する姿勢を表すθ１値、θ２値、θ３値と、を有し、これら６つのパラメータから構成される。状態取得手段２０は、取得した各ステップ毎の状態情報をRAM１１に記憶させる。
【００３５】
領域算出手段２１は、RAM１１から各ステップ毎の状態情報を読み込み、各状態情報に基づいて、各ステップ毎に、冗長マニピュレータ１のアームアングルψの実現可能な領域に関する情報としての領域情報を算出する。領域情報は、すべてのステップにおいて１つのみ存在する場合と、すべてのステップにおいて複数存在する場合と、各ステップにおいて１つ又は複数存在する場合と、がある。領域算出手段２１による各領域情報の算出の方法については、非特許文献１を参照されたい。領域算出手段２１は、算出した各ステップ毎の領域情報をRAM１１に記憶させる。
【００３６】
組み合わせ作成手段２２は、RAM１１から各ステップ毎の領域情報を読み込み、隣り合うステップ間で領域同士が少なくとも一部重複するように、第１の位置及び姿勢状態から第２の位置及び姿勢状態に至る間の領域情報の組み合わせに関する情報としての組み合わせ情報を１つ、作成する。「隣り合うステップ間で領域同士が少なくとも一部重複する」とは、「隣り合うステップ間で領域同士が共通する冗長自由度を包含している」と言い換えることができる。なお、最初のステップ（ステップｓ０）における領域と、最後のステップ（ステップｓｎ）における領域と、は必ずしも重複している必要はない。組み合わせ作成手段２２は、作成した組み合わせ情報をRAM１１に記憶させる。
【００３７】
冗長自由度決定手段２３は、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する。冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００３８】
関節角算出手段２４は、RAM１１から各ステップ毎のアームアングルψを読み込み、各ステップ毎に、そのステップにおけるアームアングルψと、そのステップにおける状態情報と、に基づいて逆運動学により、そのステップにおける各関節（第１〜第７関節）の関節角を算出する。関節角算出手段２４は、算出した各ステップ毎の各関節の関節角をRAM１１に記憶させる。
【００３９】
CP制御手段２５は、RAM１１から各ステップ毎の各関節の関節角を読み込み、これらの関節角によって規定される冗長マニピュレータ１の動きを再生する。詳しくは、CP制御手段２５は、各ステップ毎に各関節（第１〜第７関節）の関節角を達成すべく、第１関節モータ（肩部）３０、第２関節モータ（肩部）３１、第３関節モータ（肩部）３２、第４関節モータ（肘部）３３、第５関節モータ（手首部）３４、第６関節モータ（手首部）３５、第７関節モータ（手首部）３６を制御する。
【００４０】
次に、図３〜図７を参照しつつ、冗長マニピュレータ１の作動を説明する。
【００４１】
先ず、有線又は無線を介して他の制御機器から制御装置１００へＣＰ制御開始信号が送信され、又は、例えばキーボードなどの入力手段を介して操作人員が制御装置１００にＣＰ制御を開始するよう命令する（Ｓ３００）。
【００４２】
すると、状態取得手段２０は、ＣＰ制御に必要となる、各ステップ毎の状態情報を取得する（Ｓ３０２）。図５〜７において、各ステップ毎の状態情報は、同次変換行列であるＴｎで示されている。添字のｎは、離散化されたＣＰ制御の軌道のステップの総数を表している。状態取得手段２０は、取得した各ステップ毎の状態情報TnをRAM１１に記憶させる。
【００４３】
次に、領域算出手段２１は、RAM１１から各ステップ毎の状態情報Tnを読み込み、各状態情報Tnに基づいて、各ステップ毎に、アームアングルψの実現可能な領域に関する情報としての領域情報を算出する（Ｓ３０４）。図５〜７において、各ステップ毎の領域情報は、ψ（ｉ，ｊ）で示している。ここで、「ｉ」は上付添字を意味し、「ｊ」は下付添字を意味している。「ｉ」は、その領域が属するステップを識別するための番号を意味する。一方、「ｊ」は、その領域が属するステップにおいて算出された１つ又は複数の閉じた領域を識別するための番号である。また、下付添字で「ｍｎ」とあるのは、そのステップにおいて算出された領域情報の個数に相当している。領域算出手段２１は、算出した各ステップ毎の領域情報ψ（ｉ，ｊ）をRAM１１に記憶させる。
【００４４】
次に、制御装置１００は、すべてのステップにおいて、上記の領域情報ψ（ｉ，ｊ）の個数が１以上であった場合は、処理をＳ３０８に進める。一方で、何れかのステップにおいて、上記の領域情報ψ（ｉ，ｊ）の個数が１以上でなかった場合は、解なしであるから、処理を終了する（Ｓ３０７）。
【００４５】
すべてのステップにおいて、上記の領域情報ψ（ｉ，ｊ）の個数が１以上であった場合は、組み合わせ作成手段２２は、RAM１１から各ステップ毎の領域情報ψ（ｉ，ｊ）を読み込み、隣り合うステップ間で領域同士が少なくとも一部重複するように、第１の位置及び姿勢状態から第２の位置及び姿勢状態に至る間の領域情報ψ（ｉ，ｊ）の組み合わせに関する情報としての組み合わせ情報を１つ、作成する。
【００４６】
具体的には、組み合わせ作成手段２２は、先ず、図６に示すような連続性グラフを作成する（Ｓ４００）。詳細には、図５において、隣り合うステップ間で領域情報ψ（ｉ，ｊ）と領域情報ψ（ｉ−１，ｋ）が下記式（１）を満たすならば少なくとも一部重複しているので、領域情報ψ（ｉ，ｊ）と領域情報ψ（ｉ−１，ｋ）は連続しており、領域情報ψ（ｉ，ｊ）と領域情報ψ（ｉ−１，ｋ）は、グラフ理論で言うところのエッジで結合される。例えば、図５において、隣り合うステップｓ０とステップｓ１間で領域情報ψ（１，１）と領域情報ψ（０，１）が下記式（１）を満たしているので、領域情報ψ（１，１）と領域情報ψ（０，１）は、図６で太線で示すようにエッジで結合される。図５において、ステップｓ０には領域情報ψ（ｉ−１，ｋ）がｍ０個属しており、ステップｓ１には領域情報ψ（ｉ，ｊ）がｍ１個属している。従って、ステップｓ０に属するすべての領域情報ψ（ｉ−１，ｋ）と、ステップｓ１に属するすべての領域情報ψ（ｉ，ｊ）と、の重複関係を漏れなく判定するには、原則として、ｍ０×ｍ１回、下記式（１）を考慮する必要があることになる。
【００４７】
【数１】
【００４８】
次に、組み合わせ作成手段２２は、ステップｓ０とステップｓｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせを図６に示す連続性グラフ内で探索する（Ｓ４０２）。なお、この探索アルゴリズムはグラフ理論の分野で種々の提案がなされ公知となっているので、その説明は割愛する。仮に、この探索の結果、組み合わせ作成手段２２が、ステップｓ０とステップｓｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせとして、図７に示すように、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、・・・、ψ（ｎ−１，１）、ψ（ｎ，１）」から成る組み合わせを特定したとする。このようにステップｓ０とステップｓｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つかった場合（S404:YES）は、組み合わせ作成手段２２は、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、・・・、ψ（ｎ−１，１）、ψ（ｎ，１）」から成る組み合わせ情報を作成する。そして、組み合わせ作成手段２２は、作成した組み合わせ情報をRAM１１に記憶させ、処理を図３の制御フローへと戻す（Ｓ４０６）。一方、ステップ０とステップｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つからなかった場合（S404:NO）は、解なしとして処理を終了する（Ｓ４０８）。
【００４９】
次に、冗長自由度決定手段２３は、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する（Ｓ３１０）。例えば、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の上限値、下限値、好ましくは中間値を採用する。冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００５０】
次に、関節角算出手段２４は、RAM１１から各ステップ毎のアームアングルψを読み込み、各ステップ毎に、そのステップにおけるアームアングルψと、そのステップにおける状態情報と、に基づいて逆運動学により、そのステップにおける各関節（第１〜第７関節）の関節角を算出する（Ｓ３１２）。関節角算出手段２４は、算出した各ステップ毎の各関節の関節角をRAM１１に記憶させる。
【００５１】
そして、CP制御手段２５は、RAM１１から各ステップ毎の各関節の関節角を読み込み、これらの関節角によって規定される冗長マニピュレータ１の動きを再生する（Ｓ３１４）。
【００５２】
以上に本願発明の好適な第１実施形態を説明した。第１実施形態は、要するに、以下の特徴を有している。
【００５３】
冗長マニピュレータ１の手先８の位置及び姿勢状態を、ＣＰ制御にて、ステップｓ０からステップｓｎへと変化させるに際し、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する制御装置１００は、以下の通りである。制御装置１００は、各ステップにおける手先８の位置及び姿勢状態を取得する状態取得手段２０と、各ステップにおける手先８の位置及び姿勢状態に基づいて、各ステップにおけるアームアングルψの実現可能な領域を算出する領域算出手段２１と、隣り合うステップ間で領域同士が少なくとも一部重複するように、ステップｓ０からステップｓｎに至る間の領域の組み合わせを作成する組み合わせ作成手段２２と、領域の組み合わせに基づいて、ＣＰ制御の各ステップにおけるアームアングルψを決定する冗長自由度決定手段２３と、を備える。以上の構成によれば、冗長マニピュレータ１の手先８の位置及び姿勢状態を、ＣＰ制御にて、ステップｓ０からステップｓｎへと変化させるに際し、冗長マニピュレータ１の冗長自由度の連続性を担保することができる。
【００５４】
また、副次的な効果として、これにより関節角レベルで起きる非連続性が克服され、機構の性能を最大限活用した手先動作が可能となる。
【００５５】
（第２実施形態）
次に、図８〜１３を参照しつつ、本願発明の第２実施形態を説明する。ここでは、本実施形態が上記第１実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第１実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。本実施形態では、上記第１実施形態を更に具現化したものである。
【００５６】
図８に示すように、本実施形態において、冗長マニピュレータ１のＣＰ制御は、ステップｓ０からステップｓ５に至るまでの６つのステップを基礎としている。図９には、状態取得手段２０が取得した各状態情報Tnと、領域算出手段２１が算出した各領域情報ψ（ｉ，ｊ）と、の関係を示している。図９の横軸に各状態情報Tnを並べている。図９の縦軸はアームアングルψである。なお、アームアングルψの原点は特に明示していないが、アームアングルψの取り得る値は−πからπとなっている。各状態情報Tnと、各領域情報ψ（ｉ，ｊ）と、の関係を図５のように一覧表示したのが図１０である。図９に示すように、領域情報ψ（０，１）と領域情報ψ（１，１）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，１）は連続的であり、図１１に示すようにエッジで結合される。同様に、図９に示すように、領域情報ψ（０，１）と領域情報ψ（１，２）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，２）は連続的であり、図１１に示すようにエッジで結合される。組み合わせ作成手段２２は、この要領で処理を進め、やがて、図１１に示すような連続性グラフを完成させる（Ｓ４００）。
【００５７】
次に、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせを図１１に示す連続性グラフ内で探索する（Ｓ４０２）。この探索の結果、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせとして、図１２に示すように、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から成る組み合わせを特定する。組み合わせ作成手段２２は、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から構成される組み合わせ情報を作成する。そして、組み合わせ作成手段２２は、作成した組み合わせ情報をRAM１１に記憶させ、処理を図３の制御フローへと戻す（Ｓ４０６）。
【００５８】
次に、冗長自由度決定手段２３は、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、図１３に示すように、ＣＰ制御の各ステップにおけるアームアングルψを決定する（Ｓ３１０）。各ステップにおける冗長マニピュレータ１の決定されたアームアングルψは、図１３において丸印で示している。本実施形態では、図１３に示すように、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の中間値を採用している。そして、冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００５９】
なお、図１３において、△ψは「△ψｐ→ｐ＋１」を意味し、「△ψｐ→ｐ＋１」は、ステップｐのアームアングルψとステップｐ＋１のアームアングルψとの差分を意味する。
【００６０】
（第３実施形態）
次に、図１４を参照しつつ、本願発明の第３実施形態を説明する。ここでは、本実施形態が上記第２実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第２実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。
【００６１】
上記第２実施形態において、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の中間値を採用するとした。これに対し、本実施形態において、冗長自由度決定手段２３は、図１４に示すように、ステップ間におけるアームアングルψの差分の二乗和が最も小さくなるように、各ステップにおけるアームアングルψを決定する。具体的には、冗長自由度決定手段２３は、下記式（２）で定義された変数Ｑの値が最小となるように、各ステップにおけるアームアングルψを決定する。
【００６２】
【数２】
【００６３】
以上に本願発明の第２実施形態を説明した。第２実施形態は、要するに、以下の特徴を有している。
【００６４】
冗長自由度決定手段２３は、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定するに際し、ステップ間におけるアームアングルψの差分の二乗和が最も小さくなるように、ＣＰ制御の各ステップにおけるアームアングルψを決定する。以上の構成によれば、冗長マニピュレータ１の動きが滑らかになる。また、省エネや手先８の動き自体が機敏になる、といったメリットもある。
【００６５】
（第４実施形態）
次に、図１５〜２１を参照しつつ、本願発明の第４実施形態を説明する。ここでは、本実施形態が上記第２実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第２実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。
【００６６】
図１５には、状態取得手段２０が取得した各状態情報Tnと、領域算出手段２１が算出した各領域情報ψ（ｉ，ｊ）と、の関係を示している。各状態情報Tnと、各領域情報ψ（ｉ，ｊ）と、の関係を図５のように一覧表示したのが図１６である。そして、本実施形態において制御装置１００は、図４の制御フローに代えて、図１７の制御フローを実行する。以下、図１７に示す制御フローに沿って、本実施形態における冗長マニピュレータ１の作動を説明する。
【００６７】
図１５に示すように、領域情報ψ（０，１）と領域情報ψ（１，１）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，１）は連続的であり、図１８に示すようにエッジで結合される。同様に、図１５に示すように、領域情報ψ（０，１）と領域情報ψ（１，２）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，２）は連続的であり、図１８に示すようにエッジで結合される。組み合わせ作成手段２２は、この要領で処理を進め、やがて、図１８に示すような連続性グラフを完成させる（Ｓ５００）。
【００６８】
次に、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせを図１８に示す連続性グラフ内で探索する（Ｓ５０２）。この探索の結果、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせとして、図１９に示すように、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から成る組み合わせと、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせと、を特定する。このようにステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つかった場合は、制御装置１００は、処理をＳ５０６へと進める。一方、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つからなかった場合は、制御装置１００は、解なしとして処理を終了する（Ｓ５０８）。
【００６９】
そして、図１９に示すように、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが複数見つかった場合（Ｓ５０６：ＹＥＳ）は、制御装置１００は、処理をＳ５１０へと進める。一方、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが１つだけ見つかった場合（Ｓ５０６：ＮＯ）は、制御装置１００は、処理を図３の制御フローへと戻す（Ｓ５１２）。
【００７０】
上述のようにステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが複数見つかった場合（Ｓ５０６：ＹＥＳ）は、組み合わせ作成手段２２は、最終のステップ（ステップｓ５）における領域情報ψ（５，ｊ）が最も大きな組み合わせを選択して採用する（Ｓ５１０）。本実施形態において、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から成る組み合わせの最終のステップ（ステップｓ５）における領域情報ψ（５，ｊ）は、領域情報ψ（５，１）である。また、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせの最終のステップ（ステップｓ５）における領域情報ψ（５，ｊ）は、領域情報ψ（５，３）である。従って、組み合わせ作成手段２２は、各連続的な領域情報ψ（ｉ，ｊ）の組み合わせの最終のステップにおける領域情報ψ（５，ｊ）としての領域情報ψ（５，１）と領域情報ψ（５，３）を比較する。本実施形態では、図１５に示すように、領域情報ψ（５，３）が領域情報ψ（５，１）よりもアームアングルψに関して広範な範囲を包含しており、もって、領域情報ψ（５，３）は領域情報ψ（５，１）よりも大きいと言うことができる。従って、組み合わせ作成手段２２は、図２０に示すように、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせを選択して採用し、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせ情報を作成する。そして、組み合わせ作成手段２２は、作成し、選択した組み合わせ情報をRAM１１に記憶させ、処理を図３の制御フローへと戻す（Ｓ５１４）。
【００７１】
次に、冗長自由度決定手段２３は、図２１に示すように、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する（Ｓ３１０）。各ステップにおける冗長マニピュレータ１の決定されたアームアングルψは、図２１において、丸印で示している。本実施形態では、図２１に示すように、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の中間値を採用している。そして、冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００７２】
以上に本願発明の好適な第４実施形態を説明した。第４実施形態は、要するに、以下の特徴を有している。
【００７３】
組み合わせ作成手段２２は、領域情報ψ（ｉ，ｊ）の組み合わせが複数作成可能であったときは、最終のステップにおける領域情報ψ（ｉ，ｊ）が最も大きな組み合わせを採用する。以上の構成によれば、最終のステップにおいて領域情報ψ（ｉ，ｊ）が最大化される。従って、ステップｓ５において、手先８の位置及び姿勢状態をそのままにしつつ、アームアングルψを大きく変化させることが可能となる。このことは、例えばステップｓ５から更に手先８の位置及び姿勢状態を変化させようとする場合、その変化の最初のステップにおけるアームアングルψの選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、領域情報ψ（ｉ，ｊ）の組み合わせの作成が可能となる確率が高くなる。第２に、領域情報ψ（ｉ，ｊ）の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、領域情報ψ（ｉ，ｊ）の組み合わせの作成可能な組数がより多くなる可能性がある。
【００７４】
以上に本願発明の好適な第１〜第４実施形態を説明したが、これらの実施形態は適宜に組み合わせることができる。
【符号の説明】
【００７５】
１ 冗長マニピュレータ
２０ 状態取得手段
２１ 領域算出手段
２２ 組み合わせ作成手段
２３ 冗長自由度決定手段
２４ 関節角算出手段
２５ CP制御手段
１００ 制御装置（決定装置）
ψ アームアングル（冗長自由度）
ψ（ｉ，ｊ） 領域情報
Tn 状態情報
【技術分野】
【０００１】
本発明は、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御される冗長マニピュレータの各ステップにおける冗長自由度の決定方法、決定装置、及び、決定プログラムに関する。
【背景技術】
【０００２】
作業に必要な自由度より多くの自由度を有する冗長マニピュレータは、近年、ロボット分野において広く用いられるようになってきている。これは、冗長マニピュレータが有する余分な自由度を有効活用することで、例えば、ロボットの可操作性が向上したり、関節トルクの最適化が図れたり、障害物を回避できたり、所謂特異点を回避できたり、関節限界を回避できたりするからである。
【０００３】
非特許文献１は、このような冗長マニピュレータの一例として、７自由度冗長マニピュレータについて言及している。一般に、空間内におけるマニピュレータの手先の位置及び姿勢は、６つのパラメータで表現することができる。従って、７自由度マニピュレータには余分な自由度が１つ存在していることになる。
【０００４】
先ずは、この７自由度冗長マニピュレータについて図２２を参照しつつ説明する。図２２に示すように、７自由度冗長マニピュレータの関節は、すべて回転関節で構成されている。肩部の三関節（第１〜第３関節、θ１〜θ３）の関節軸は一点で交差している。同様に、手首部の三関節（第５〜第７関節、θ５〜θ７）の関節軸も一点で交差している。また、隣り合う一対の関節の軸同士は、互いに直交する。このように肩部の三関節と手首部の三関節は、単純な構造をしているため、夫々仮想的な球面機構を構成しているとみなすことができる。そこで、この種のマニピュレータは、一般に、３Ｒ−１Ｒ−３Ｒ型或いはＳ−Ｒ−Ｓ型のマニピュレータとして分類されている。
【０００５】
更に、上記の非特許文献１は、冗長自由度を表現するためのパラメータとして、非特許文献２に開示のアームアングルψを採用している。このアームアングルψは、図２３に示すようにアーム平面と参照面との成す角度として定義されるものである。アーム平面は、肩部三関節の交点Ｐｓ、肘関節位置Ｐｅ及び手首部三関節の交点Ｐｗの三点を通る平面である。もし関節の可動範囲に制限がなければ、手先の位置及び姿勢を固定したまま、アームアングルψのみを自由自在に変えることができよう。しかしながら、実在のマニピュレータでは、機構的な制約のため、関節の可動範囲には制限がある。この結果、アームアングルψの実現可能な領域は自ずと限定されることになる。これに対し、非特許文献１は、アームアングルψの実現可能な領域を代数学的に解く手法を提案している。この手法の詳細は、非特許文献１を参照されたい。要するに、非特許文献１によれば、アームアングルψの実現可能な領域は、１つ又は複数の閉じた領域から構成され得るとしている。
【先行技術文献】
【特許文献】
【０００６】
【非特許文献１】清水昌幸、他4名、「関節の可動範囲を考慮に入れた7自由度冗長マニヒ゜ュレータの解析的逆運動学解法」、日本ロホ゛ット学会誌、日本ロホ゛ット学会、2007年、第25巻、第4号、p.122-133
【非特許文献２】K.Kreutz-Delgado, M.Long and H. Seraji: "Kinematic Analysis of 7-DOF Manipulators," Int.J.Robotics Research, 1992, vol.11, no.5, pp.469-481.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
前述の通り、アームアングルψの実現可能な領域は、１つ又は複数の閉じた領域から構成される。しかし、このようなアームアングルψの実現可能な領域と、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御と、の間には未解決な問題があった。ここで、ＣＰ制御とは、図２４に示すように、（１）第１の位置及び姿勢状態（"ｓ０"を参照）から第２の位置及び姿勢状態（"ｓ６"を参照）に至るまでの、マニピュレータの手先の位置及び姿勢状態を例えば所定の時間間隔毎に細かく設定し、（２）各手先位置毎に、逆運動学を用いて各関節の角度を求め、（３）（２）で求めた各関節の角度をロボットによって再生させる、制御のことである。しかし、冗長マニピュレータは冗長な構成であるがため、上記（２）において各関節の角度が一意に定まらず、冗長分の自由度を何らかの基準に基づいて予め決定する必要がある。以下、図２４及び図２５を参照しつつ、この問題を詳細に説明する。なお、説明の便宜上、冗長自由度を表すパラメータとしてアームアングルψを継続して使用するが、冗長自由度を表すパラメータとしてアームアングルψを使用しなければならない特段の事情はなく、他のパラメータを代わりに使用してもよいことに留意されたい。
【０００８】
図２５には、図２４で示した各ステップにおけるアームアングルψの実現可能な領域を示している。図２５の横軸に並べたｓ０〜ｓ６は、図２４に示すステップｓ０〜ｓ６に夫々対応している。図２５の縦軸は、アームアングルψである。図２５において、上付添字と下付添字を有する「ψ」は、アームアングルψの実現可能な領域（範囲）を意味している。「ψ」の上付添字は、その領域が属するステップを識別するための番号を意味する。一方、「ψ」の下付添字は、その領域が属するステップにおける各領域の識別番号である。確認のため繰り返すと、「ψ」はアームアングルを意味するが、上付添字と下付添字を伴った「ψ」はアームアングルψの実現可能な領域を意味している。文章中では、このアームアングルψの実現可能な領域を、便宜上、ψ（上付添字，下付添字）のようにして表記する。
【０００９】
さて、従来では、冗長分の自由度を予め決定するための基準に関する知見は一切なかった。従って、ステップ０におけるアームアングルψを予め決定するための基準に関する知見も同様になかった。それでは、本願発明者らはどうやってステップ０におけるアームアングルψを決定していたか。本願発明者らに対するインタビューによれば、以下の通りである。即ち、本願発明者らは、先ず、ステップ０におけるψ（０，１）とψ（０，２）の大小を比較し、図２５の場合ではψ（０，１）＞ψ（０，２）であることから、この単純な大小関係に基づいてψ（０，１）を選択していたのである。そして、本願発明者らは、このψ（０，１）の中間値をステップ０におけるアームアングルψとして採用していた。また、次のステップ１におけるアームアングルψについて言えば、本願発明者らは、アームアングルψの一応の連続性を考慮してψ（０，１）とψ（１，１）との重複関係を重視することで、ψ（１，１）とψ（１，２）のうちψ（１，１）を選択していた。そして、本願発明者らは、このψ（１，１）の中間値をステップ１におけるアームアングルψとして採用していた。
【００１０】
このような場当たり的な基準は、それでも一見すると合理的であるかのようにも見える。しかしながら、この基準によると、ステップ４からステップ５へと移行するときに大きな問題が生じる。というのは、確かに、ψ（０，１）にとっては次のステップ１において自己と重複するアームアングルψの実現可能な領域が存在していた。同様に、ψ（１，１）にとっては次のステップ２において自己と重複するアームアングルψの実現可能な領域が存在していた。しかしながら、ψ（４，１）にとっては次のステップ５において自己と重複するアームアングルψの実現可能な領域が存在していない。従って、仕方なく、アームアングルψの連続性を犠牲にして、唯一存在するψ（５，１）の中から、ステップ５におけるアームアングルψを選択し採用せざるを得なかった。そして、このようにアームアングルψの値が看過できない程に大きく変化する結果、各関節の角速度が有限であることとの兼ね合いもあり、手先が意図した軌道から大きく外れたり、手先が他の物体と干渉したり、といった種々の問題が発生していた。
【００１１】
本願発明の目的は、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、冗長マニピュレータの冗長自由度の連続性を担保する技術を提供することにある。ここで、冗長自由度の連続性とは、「冗長自由度の値の変化がステップ間で緩やかである」ことを意味する。
【課題を解決するための手段】
【００１２】
本願発明の第１の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する方法は、以下の通りである。各ステップにおける前記手先の位置及び姿勢状態を取得する。各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する。隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する。前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の方法によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【００１３】
好ましくは、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上の方法によれば、最終のステップにおいて前記領域が最大化される。従って、前記第２の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第２の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、前記領域の組み合わせの作成が可能となる確率が高くなる。第２に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。
【００１４】
好ましくは、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の方法によれば、前記冗長マニピュレータの動きが滑らかになる。
【００１５】
好ましくは、前記冗長マニピュレータは、７自由度である。
【００１６】
好ましくは、前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である。
【００１７】
本願発明の第２の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する装置は、以下の通りである。決定装置は、各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、を備える。以上の構成によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【００１８】
好ましくは、前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上の構成によれば、最終のステップにおいて前記領域が最大化される。従って、前記第２の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第２の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、前記領域の組み合わせの作成が可能となる確率が高くなる。第２に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。
【００１９】
好ましくは、前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上の構成によれば、前記冗長マニピュレータの動きが滑らかになる。
【００２０】
好ましくは、前記冗長マニピュレータは、７自由度である。
【００２１】
好ましくは、前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である。
【００２２】
本願発明の第３の観点によれば、冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するためのプログラムは、コンピュータを、各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、として機能させる。以上のプログラムによれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【００２３】
好ましくは、前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する。以上のプログラムによれば、最終のステップにおいて前記領域が最大化される。従って、前記第２の位置及び姿勢状態において、前記手先の位置及び姿勢状態をそのままにしつつ、前記冗長自由度を大きく変化させることが可能となる。このことは、例えば前記第２の位置及び姿勢状態から前記手先の位置及び姿勢状態を更に変化させようとする場合、その変化の最初のステップにおける前記冗長自由度の選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、前記領域の組み合わせの作成が可能となる確率が高くなる。第２に、前記領域の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、前記領域の組み合わせの作成可能な組数がより多くなる可能性がある。
【００２４】
好ましくは、前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する。以上のプログラムによれば、前記冗長マニピュレータの動きが滑らかになる。
【００２５】
好ましくは、前記冗長マニピュレータは、７自由度である。
【００２６】
好ましくは、前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である。
【発明の効果】
【００２７】
本願発明によれば、前記冗長マニピュレータの手先の位置及び姿勢状態を、前記ＣＰ制御にて、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態へと変化させるに際し、前記冗長マニピュレータの冗長自由度の連続性を担保することができる。
【図面の簡単な説明】
【００２８】
【図１】図１は、冗長マニピュレータの全体模式図である（第１実施形態）。
【図２】図２は、冗長マニピュレータの機能ブロック図である（第１実施形態）。
【図３】図３は、冗長マニピュレータの制御フローである（第１実施形態）。
【図４】図４は、冗長マニピュレータの制御フローである（第１実施形態）。
【図５】図５は、冗長自由度の実現可能な領域ψの一覧である（第１実施形態）。
【図６】図６は、冗長自由度の実現可能な領域ψの連続性を示すグラフである（第１実施形態）。
【図７】図７は、冗長自由度の実現可能な領域ψの組み合わせを示す図である（第１実施形態）。
【図８】図８は、冗長マニピュレータの全体模式図である（第２実施形態）。
【図９】図９は、冗長自由度の実現可能な領域ψを示す図である（第２実施形態）。
【図１０】図１０は、冗長自由度の実現可能な領域ψの一覧である（第２実施形態）。
【図１１】図１１は、冗長自由度の実現可能な領域ψの連続性を示すグラフである（第２実施形態）。
【図１２】図１２は、冗長自由度の実現可能な領域ψの組み合わせを示す図である（第２実施形態）。
【図１３】図１３は、各ステップにおいて決定された冗長自由度を示す図である（第２実施形態）。
【図１４】図１４は、各ステップにおいて決定された冗長自由度を示す図である（第３実施形態）。
【図１５】図１５は、冗長自由度の実現可能な領域ψを示す図である（第４実施形態）。
【図１６】図１６は、冗長自由度の実現可能な領域ψの一覧である（第４実施形態）。
【図１７】図１７は、冗長マニピュレータの制御フローである（第４実施形態）。
【図１８】図１８は、冗長自由度の実現可能な領域ψの連続性を示すグラフである（第４実施形態）。
【図１９】図１９は、冗長自由度の実現可能な領域ψの組み合わせを示す図である（第４実施形態）。
【図２０】図２０は、図１９に示す複数の組み合わせから選択された組み合わせを示す図である（第４実施形態）。
【図２１】図２１は、各ステップにおいて決定された冗長自由度を示す図である（第４実施形態）。
【図２２】図２２は、冗長マニピュレータの全体模式図である（従来技術）。
【図２３】図２３は、冗長自由度の一例としてのアームアングルの定義説明図である（従来技術）。
【図２４】図２４は、冗長マニピュレータの全体模式図である（従来技術）。
【図２５】図２５は、各ステップにおいて決定された冗長自由度を示す図である（従来技術）。
【発明を実施するための形態】
【００２９】
（第１実施形態）
以下、図１〜７を参照しつつ、本願発明の第１実施形態を説明する。以下、明細書中で「ステップ」とあるのは、ＣＰ制御の制御フローの多種多様な処理のステップを示す場合と、ＣＰ制御によるマニピュレータの動きのステップを示す場合があるので、両者を混同しないように留意されたい。
【００３０】
図１に示すように、本実施形態において冗長マニピュレータ１は、体幹部２と、肩関節部３と、上腕部４と、肘関節部５と、前腕部６と、手首関節部７と、手先８と、制御装置１００（決定装置）と、を主たる構成として備えている。体幹部２は、例えば地面などに固定されている。地面と肩関節部３は体幹部２によって連結されている。肩関節部３と肘関節部５は上腕部４によって連結されている。肘関節部５と手首関節部７は前腕部６によって連結されている。手先８は手首関節部７に支持されている。
【００３１】
肩関節部３は、第１〜第３の関節から構成される球面機構となっている。肘関節部５は、第４の関節から構成されている。手首関節部７は、第５〜第７の関節から構成される球面機構となっている。従って、冗長マニピュレータ１は、所謂Ｓ−Ｒ−Ｓ型マニピュレータに分類される。また、冗長マニピュレータ１は、余分な自由度を１つ有している。以降、冗長自由度を表現するためのパラメータとしては、非特許文献２に開示のアームアングルψを使用するものとする。繰り返すが、冗長自由度を表現するためのパラメータとしては、アームアングルψに限らず、例えば、任意の１自由度の関節角や別の表現のアームアングルでもよい。
【００３２】
図２に示すように、冗長マニピュレータ１は、更に、第１関節モータ（肩部）３０と、第２関節モータ（肩部）３１と、第３関節モータ（肩部）３２と、第４関節モータ（肘部）３３と、第５関節モータ（手首部）３４と、第６関節モータ（手首部）３５と、第７関節モータ（手首部）３６と、を備えている。第１関節モータ（肩部）３０は、肩関節部３を構成する第１関節に搭載され、第１関節を駆動する。同様に、第２関節モータ（肩部）３１は、肩関節部３を構成する第２関節に搭載され、第２関節を駆動する。第３関節モータ（肩部）３２は、肩関節部３を構成する第３関節に搭載され、第３関節を駆動する。第４関節モータ（肘部）３３は、肘関節部５を構成する第４関節に搭載され、第４関節を駆動する。第５関節モータ（手首部）３４は、手首関節部７を構成する第５関節に搭載され、第５関節を駆動する。第６関節モータ（手首部）３５は、手首関節部７を構成する第６関節に搭載され、第６関節を駆動する。第７関節モータ（手首部）３６は、手首関節部７を構成する第７関節に搭載され、第７関節を駆動する。
【００３３】
制御装置１００は、冗長マニピュレータ１の手先８の位置及び姿勢状態を、ＣＰ制御にて、第１の位置及び姿勢状態（ステップｓ０、図１参照）から第２の位置及び姿勢状態（ステップｓｎ、図１参照）へと変化させるに際し、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψの決定する。制御装置１００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１０と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１１と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２と、を備える。ROM１２には、制御プログラム（決定プログラム）が記憶されている。この制御プログラムはCPU１０によって読み出され、CPU１０上で実行される。これにより、制御プログラムは、CPU１０などのハードウェアを、状態取得手段２０、領域算出手段２１、組み合わせ作成手段２２、冗長自由度決定手段２３、関節角算出手段２４、CP制御手段２５、として機能させる。
【００３４】
状態取得手段２０は、ＣＰ制御に必要となる、各ステップにおける手先８の位置及び姿勢状態に関する情報としての状態情報を取得する。状態取得手段２０による各状態情報の取得の態様としては、有線又は無線を介した他の制御機器からの受信や、例えばキーボードなどの入力手段を介した操作人員からの入力、ROM１２からの読み込みなどが考えられる。各状態情報は、手先８の空間に対する位置を表すＸ値、Ｙ値、Ｚ値と、手先８の空間に対する姿勢を表すθ１値、θ２値、θ３値と、を有し、これら６つのパラメータから構成される。状態取得手段２０は、取得した各ステップ毎の状態情報をRAM１１に記憶させる。
【００３５】
領域算出手段２１は、RAM１１から各ステップ毎の状態情報を読み込み、各状態情報に基づいて、各ステップ毎に、冗長マニピュレータ１のアームアングルψの実現可能な領域に関する情報としての領域情報を算出する。領域情報は、すべてのステップにおいて１つのみ存在する場合と、すべてのステップにおいて複数存在する場合と、各ステップにおいて１つ又は複数存在する場合と、がある。領域算出手段２１による各領域情報の算出の方法については、非特許文献１を参照されたい。領域算出手段２１は、算出した各ステップ毎の領域情報をRAM１１に記憶させる。
【００３６】
組み合わせ作成手段２２は、RAM１１から各ステップ毎の領域情報を読み込み、隣り合うステップ間で領域同士が少なくとも一部重複するように、第１の位置及び姿勢状態から第２の位置及び姿勢状態に至る間の領域情報の組み合わせに関する情報としての組み合わせ情報を１つ、作成する。「隣り合うステップ間で領域同士が少なくとも一部重複する」とは、「隣り合うステップ間で領域同士が共通する冗長自由度を包含している」と言い換えることができる。なお、最初のステップ（ステップｓ０）における領域と、最後のステップ（ステップｓｎ）における領域と、は必ずしも重複している必要はない。組み合わせ作成手段２２は、作成した組み合わせ情報をRAM１１に記憶させる。
【００３７】
冗長自由度決定手段２３は、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する。冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００３８】
関節角算出手段２４は、RAM１１から各ステップ毎のアームアングルψを読み込み、各ステップ毎に、そのステップにおけるアームアングルψと、そのステップにおける状態情報と、に基づいて逆運動学により、そのステップにおける各関節（第１〜第７関節）の関節角を算出する。関節角算出手段２４は、算出した各ステップ毎の各関節の関節角をRAM１１に記憶させる。
【００３９】
CP制御手段２５は、RAM１１から各ステップ毎の各関節の関節角を読み込み、これらの関節角によって規定される冗長マニピュレータ１の動きを再生する。詳しくは、CP制御手段２５は、各ステップ毎に各関節（第１〜第７関節）の関節角を達成すべく、第１関節モータ（肩部）３０、第２関節モータ（肩部）３１、第３関節モータ（肩部）３２、第４関節モータ（肘部）３３、第５関節モータ（手首部）３４、第６関節モータ（手首部）３５、第７関節モータ（手首部）３６を制御する。
【００４０】
次に、図３〜図７を参照しつつ、冗長マニピュレータ１の作動を説明する。
【００４１】
先ず、有線又は無線を介して他の制御機器から制御装置１００へＣＰ制御開始信号が送信され、又は、例えばキーボードなどの入力手段を介して操作人員が制御装置１００にＣＰ制御を開始するよう命令する（Ｓ３００）。
【００４２】
すると、状態取得手段２０は、ＣＰ制御に必要となる、各ステップ毎の状態情報を取得する（Ｓ３０２）。図５〜７において、各ステップ毎の状態情報は、同次変換行列であるＴｎで示されている。添字のｎは、離散化されたＣＰ制御の軌道のステップの総数を表している。状態取得手段２０は、取得した各ステップ毎の状態情報TnをRAM１１に記憶させる。
【００４３】
次に、領域算出手段２１は、RAM１１から各ステップ毎の状態情報Tnを読み込み、各状態情報Tnに基づいて、各ステップ毎に、アームアングルψの実現可能な領域に関する情報としての領域情報を算出する（Ｓ３０４）。図５〜７において、各ステップ毎の領域情報は、ψ（ｉ，ｊ）で示している。ここで、「ｉ」は上付添字を意味し、「ｊ」は下付添字を意味している。「ｉ」は、その領域が属するステップを識別するための番号を意味する。一方、「ｊ」は、その領域が属するステップにおいて算出された１つ又は複数の閉じた領域を識別するための番号である。また、下付添字で「ｍｎ」とあるのは、そのステップにおいて算出された領域情報の個数に相当している。領域算出手段２１は、算出した各ステップ毎の領域情報ψ（ｉ，ｊ）をRAM１１に記憶させる。
【００４４】
次に、制御装置１００は、すべてのステップにおいて、上記の領域情報ψ（ｉ，ｊ）の個数が１以上であった場合は、処理をＳ３０８に進める。一方で、何れかのステップにおいて、上記の領域情報ψ（ｉ，ｊ）の個数が１以上でなかった場合は、解なしであるから、処理を終了する（Ｓ３０７）。
【００４５】
すべてのステップにおいて、上記の領域情報ψ（ｉ，ｊ）の個数が１以上であった場合は、組み合わせ作成手段２２は、RAM１１から各ステップ毎の領域情報ψ（ｉ，ｊ）を読み込み、隣り合うステップ間で領域同士が少なくとも一部重複するように、第１の位置及び姿勢状態から第２の位置及び姿勢状態に至る間の領域情報ψ（ｉ，ｊ）の組み合わせに関する情報としての組み合わせ情報を１つ、作成する。
【００４６】
具体的には、組み合わせ作成手段２２は、先ず、図６に示すような連続性グラフを作成する（Ｓ４００）。詳細には、図５において、隣り合うステップ間で領域情報ψ（ｉ，ｊ）と領域情報ψ（ｉ−１，ｋ）が下記式（１）を満たすならば少なくとも一部重複しているので、領域情報ψ（ｉ，ｊ）と領域情報ψ（ｉ−１，ｋ）は連続しており、領域情報ψ（ｉ，ｊ）と領域情報ψ（ｉ−１，ｋ）は、グラフ理論で言うところのエッジで結合される。例えば、図５において、隣り合うステップｓ０とステップｓ１間で領域情報ψ（１，１）と領域情報ψ（０，１）が下記式（１）を満たしているので、領域情報ψ（１，１）と領域情報ψ（０，１）は、図６で太線で示すようにエッジで結合される。図５において、ステップｓ０には領域情報ψ（ｉ−１，ｋ）がｍ０個属しており、ステップｓ１には領域情報ψ（ｉ，ｊ）がｍ１個属している。従って、ステップｓ０に属するすべての領域情報ψ（ｉ−１，ｋ）と、ステップｓ１に属するすべての領域情報ψ（ｉ，ｊ）と、の重複関係を漏れなく判定するには、原則として、ｍ０×ｍ１回、下記式（１）を考慮する必要があることになる。
【００４７】
【数１】
【００４８】
次に、組み合わせ作成手段２２は、ステップｓ０とステップｓｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせを図６に示す連続性グラフ内で探索する（Ｓ４０２）。なお、この探索アルゴリズムはグラフ理論の分野で種々の提案がなされ公知となっているので、その説明は割愛する。仮に、この探索の結果、組み合わせ作成手段２２が、ステップｓ０とステップｓｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせとして、図７に示すように、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、・・・、ψ（ｎ−１，１）、ψ（ｎ，１）」から成る組み合わせを特定したとする。このようにステップｓ０とステップｓｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つかった場合（S404:YES）は、組み合わせ作成手段２２は、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、・・・、ψ（ｎ−１，１）、ψ（ｎ，１）」から成る組み合わせ情報を作成する。そして、組み合わせ作成手段２２は、作成した組み合わせ情報をRAM１１に記憶させ、処理を図３の制御フローへと戻す（Ｓ４０６）。一方、ステップ０とステップｎの間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つからなかった場合（S404:NO）は、解なしとして処理を終了する（Ｓ４０８）。
【００４９】
次に、冗長自由度決定手段２３は、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する（Ｓ３１０）。例えば、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の上限値、下限値、好ましくは中間値を採用する。冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００５０】
次に、関節角算出手段２４は、RAM１１から各ステップ毎のアームアングルψを読み込み、各ステップ毎に、そのステップにおけるアームアングルψと、そのステップにおける状態情報と、に基づいて逆運動学により、そのステップにおける各関節（第１〜第７関節）の関節角を算出する（Ｓ３１２）。関節角算出手段２４は、算出した各ステップ毎の各関節の関節角をRAM１１に記憶させる。
【００５１】
そして、CP制御手段２５は、RAM１１から各ステップ毎の各関節の関節角を読み込み、これらの関節角によって規定される冗長マニピュレータ１の動きを再生する（Ｓ３１４）。
【００５２】
以上に本願発明の好適な第１実施形態を説明した。第１実施形態は、要するに、以下の特徴を有している。
【００５３】
冗長マニピュレータ１の手先８の位置及び姿勢状態を、ＣＰ制御にて、ステップｓ０からステップｓｎへと変化させるに際し、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する制御装置１００は、以下の通りである。制御装置１００は、各ステップにおける手先８の位置及び姿勢状態を取得する状態取得手段２０と、各ステップにおける手先８の位置及び姿勢状態に基づいて、各ステップにおけるアームアングルψの実現可能な領域を算出する領域算出手段２１と、隣り合うステップ間で領域同士が少なくとも一部重複するように、ステップｓ０からステップｓｎに至る間の領域の組み合わせを作成する組み合わせ作成手段２２と、領域の組み合わせに基づいて、ＣＰ制御の各ステップにおけるアームアングルψを決定する冗長自由度決定手段２３と、を備える。以上の構成によれば、冗長マニピュレータ１の手先８の位置及び姿勢状態を、ＣＰ制御にて、ステップｓ０からステップｓｎへと変化させるに際し、冗長マニピュレータ１の冗長自由度の連続性を担保することができる。
【００５４】
また、副次的な効果として、これにより関節角レベルで起きる非連続性が克服され、機構の性能を最大限活用した手先動作が可能となる。
【００５５】
（第２実施形態）
次に、図８〜１３を参照しつつ、本願発明の第２実施形態を説明する。ここでは、本実施形態が上記第１実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第１実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。本実施形態では、上記第１実施形態を更に具現化したものである。
【００５６】
図８に示すように、本実施形態において、冗長マニピュレータ１のＣＰ制御は、ステップｓ０からステップｓ５に至るまでの６つのステップを基礎としている。図９には、状態取得手段２０が取得した各状態情報Tnと、領域算出手段２１が算出した各領域情報ψ（ｉ，ｊ）と、の関係を示している。図９の横軸に各状態情報Tnを並べている。図９の縦軸はアームアングルψである。なお、アームアングルψの原点は特に明示していないが、アームアングルψの取り得る値は−πからπとなっている。各状態情報Tnと、各領域情報ψ（ｉ，ｊ）と、の関係を図５のように一覧表示したのが図１０である。図９に示すように、領域情報ψ（０，１）と領域情報ψ（１，１）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，１）は連続的であり、図１１に示すようにエッジで結合される。同様に、図９に示すように、領域情報ψ（０，１）と領域情報ψ（１，２）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，２）は連続的であり、図１１に示すようにエッジで結合される。組み合わせ作成手段２２は、この要領で処理を進め、やがて、図１１に示すような連続性グラフを完成させる（Ｓ４００）。
【００５７】
次に、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせを図１１に示す連続性グラフ内で探索する（Ｓ４０２）。この探索の結果、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせとして、図１２に示すように、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から成る組み合わせを特定する。組み合わせ作成手段２２は、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から構成される組み合わせ情報を作成する。そして、組み合わせ作成手段２２は、作成した組み合わせ情報をRAM１１に記憶させ、処理を図３の制御フローへと戻す（Ｓ４０６）。
【００５８】
次に、冗長自由度決定手段２３は、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、図１３に示すように、ＣＰ制御の各ステップにおけるアームアングルψを決定する（Ｓ３１０）。各ステップにおける冗長マニピュレータ１の決定されたアームアングルψは、図１３において丸印で示している。本実施形態では、図１３に示すように、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の中間値を採用している。そして、冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００５９】
なお、図１３において、△ψは「△ψｐ→ｐ＋１」を意味し、「△ψｐ→ｐ＋１」は、ステップｐのアームアングルψとステップｐ＋１のアームアングルψとの差分を意味する。
【００６０】
（第３実施形態）
次に、図１４を参照しつつ、本願発明の第３実施形態を説明する。ここでは、本実施形態が上記第２実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第２実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。
【００６１】
上記第２実施形態において、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の中間値を採用するとした。これに対し、本実施形態において、冗長自由度決定手段２３は、図１４に示すように、ステップ間におけるアームアングルψの差分の二乗和が最も小さくなるように、各ステップにおけるアームアングルψを決定する。具体的には、冗長自由度決定手段２３は、下記式（２）で定義された変数Ｑの値が最小となるように、各ステップにおけるアームアングルψを決定する。
【００６２】
【数２】
【００６３】
以上に本願発明の第２実施形態を説明した。第２実施形態は、要するに、以下の特徴を有している。
【００６４】
冗長自由度決定手段２３は、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定するに際し、ステップ間におけるアームアングルψの差分の二乗和が最も小さくなるように、ＣＰ制御の各ステップにおけるアームアングルψを決定する。以上の構成によれば、冗長マニピュレータ１の動きが滑らかになる。また、省エネや手先８の動き自体が機敏になる、といったメリットもある。
【００６５】
（第４実施形態）
次に、図１５〜２１を参照しつつ、本願発明の第４実施形態を説明する。ここでは、本実施形態が上記第２実施形態と異なる点を中心に説明し、重複する説明は適宜省略する。また、上記第２実施形態の各構成要素に対応する構成要素には原則として同一の符号を付すこととする。
【００６６】
図１５には、状態取得手段２０が取得した各状態情報Tnと、領域算出手段２１が算出した各領域情報ψ（ｉ，ｊ）と、の関係を示している。各状態情報Tnと、各領域情報ψ（ｉ，ｊ）と、の関係を図５のように一覧表示したのが図１６である。そして、本実施形態において制御装置１００は、図４の制御フローに代えて、図１７の制御フローを実行する。以下、図１７に示す制御フローに沿って、本実施形態における冗長マニピュレータ１の作動を説明する。
【００６７】
図１５に示すように、領域情報ψ（０，１）と領域情報ψ（１，１）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，１）は連続的であり、図１８に示すようにエッジで結合される。同様に、図１５に示すように、領域情報ψ（０，１）と領域情報ψ（１，２）との間には共通するアームアングルψが存在する。従って、領域情報ψ（０，１）と領域情報ψ（１，２）は連続的であり、図１８に示すようにエッジで結合される。組み合わせ作成手段２２は、この要領で処理を進め、やがて、図１８に示すような連続性グラフを完成させる（Ｓ５００）。
【００６８】
次に、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせを図１８に示す連続性グラフ内で探索する（Ｓ５０２）。この探索の結果、組み合わせ作成手段２２は、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせとして、図１９に示すように、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から成る組み合わせと、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせと、を特定する。このようにステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つかった場合は、制御装置１００は、処理をＳ５０６へと進める。一方、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが見つからなかった場合は、制御装置１００は、解なしとして処理を終了する（Ｓ５０８）。
【００６９】
そして、図１９に示すように、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが複数見つかった場合（Ｓ５０６：ＹＥＳ）は、制御装置１００は、処理をＳ５１０へと進める。一方、ステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが１つだけ見つかった場合（Ｓ５０６：ＮＯ）は、制御装置１００は、処理を図３の制御フローへと戻す（Ｓ５１２）。
【００７０】
上述のようにステップｓ０とステップｓ５の間における連続的な領域情報ψ（ｉ，ｊ）の組み合わせが複数見つかった場合（Ｓ５０６：ＹＥＳ）は、組み合わせ作成手段２２は、最終のステップ（ステップｓ５）における領域情報ψ（５，ｊ）が最も大きな組み合わせを選択して採用する（Ｓ５１０）。本実施形態において、「ψ（０，１）、ψ（１，１）、ψ（２，１）、ψ（３，１）、ψ（４，１）、ψ（５，１）」から成る組み合わせの最終のステップ（ステップｓ５）における領域情報ψ（５，ｊ）は、領域情報ψ（５，１）である。また、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせの最終のステップ（ステップｓ５）における領域情報ψ（５，ｊ）は、領域情報ψ（５，３）である。従って、組み合わせ作成手段２２は、各連続的な領域情報ψ（ｉ，ｊ）の組み合わせの最終のステップにおける領域情報ψ（５，ｊ）としての領域情報ψ（５，１）と領域情報ψ（５，３）を比較する。本実施形態では、図１５に示すように、領域情報ψ（５，３）が領域情報ψ（５，１）よりもアームアングルψに関して広範な範囲を包含しており、もって、領域情報ψ（５，３）は領域情報ψ（５，１）よりも大きいと言うことができる。従って、組み合わせ作成手段２２は、図２０に示すように、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせを選択して採用し、「ψ（０，１）、ψ（１，２）、ψ（２，２）、ψ（３，３）、ψ（４，３）、ψ（５，３）」から成る組み合わせ情報を作成する。そして、組み合わせ作成手段２２は、作成し、選択した組み合わせ情報をRAM１１に記憶させ、処理を図３の制御フローへと戻す（Ｓ５１４）。
【００７１】
次に、冗長自由度決定手段２３は、図２１に示すように、RAM１１から組み合わせ情報を読み込み、組み合わせ情報に基づいて、ＣＰ制御の各ステップにおける冗長マニピュレータ１のアームアングルψを決定する（Ｓ３１０）。各ステップにおける冗長マニピュレータ１の決定されたアームアングルψは、図２１において、丸印で示している。本実施形態では、図２１に示すように、冗長自由度決定手段２３は、各ステップ毎に、アームアングルψとして、組み合わせ情報に属する領域情報ψ（ｉ，ｊ）の中間値を採用している。そして、冗長自由度決定手段２３は、決定した各ステップ毎のアームアングルψをRAM１１に記憶させる。
【００７２】
以上に本願発明の好適な第４実施形態を説明した。第４実施形態は、要するに、以下の特徴を有している。
【００７３】
組み合わせ作成手段２２は、領域情報ψ（ｉ，ｊ）の組み合わせが複数作成可能であったときは、最終のステップにおける領域情報ψ（ｉ，ｊ）が最も大きな組み合わせを採用する。以上の構成によれば、最終のステップにおいて領域情報ψ（ｉ，ｊ）が最大化される。従って、ステップｓ５において、手先８の位置及び姿勢状態をそのままにしつつ、アームアングルψを大きく変化させることが可能となる。このことは、例えばステップｓ５から更に手先８の位置及び姿勢状態を変化させようとする場合、その変化の最初のステップにおけるアームアングルψの選択の柔軟性に優れることから、少なくとも以下の３つのメリットが生じる。第１に、領域情報ψ（ｉ，ｊ）の組み合わせの作成が可能となる確率が高くなる。第２に、領域情報ψ（ｉ，ｊ）の組み合わせの作成可能な組数が複数となる確率が高くなる。第３に、領域情報ψ（ｉ，ｊ）の組み合わせの作成可能な組数がより多くなる可能性がある。
【００７４】
以上に本願発明の好適な第１〜第４実施形態を説明したが、これらの実施形態は適宜に組み合わせることができる。
【符号の説明】
【００７５】
１ 冗長マニピュレータ
２０ 状態取得手段
２１ 領域算出手段
２２ 組み合わせ作成手段
２３ 冗長自由度決定手段
２４ 関節角算出手段
２５ CP制御手段
１００ 制御装置（決定装置）
ψ アームアングル（冗長自由度）
ψ（ｉ，ｊ） 領域情報
Tn 状態情報
【特許請求の範囲】
【請求項１】
冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する方法であって、
各ステップにおける前記手先の位置及び姿勢状態を取得し、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出し、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成し、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定方法。
【請求項２】
請求項１に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定方法。
【請求項３】
請求項１又は２に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定方法。
【請求項４】
請求項１〜３の何れかに記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記冗長マニピュレータは、７自由度である、
冗長マニピュレータの冗長自由度の決定方法。
【請求項５】
請求項４に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である、
冗長マニピュレータの冗長自由度の決定方法。
【請求項６】
冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する装置であって、
各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、
を備える、
冗長マニピュレータの冗長自由度の決定装置。
【請求項７】
請求項６に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定装置。
【請求項８】
請求項６又は７に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定装置。
【請求項９】
請求項６〜８の何れかに記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長マニピュレータは、７自由度である、
冗長マニピュレータの冗長自由度の決定装置。
【請求項１０】
請求項９に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である、
冗長マニピュレータの冗長自由度の決定装置。
【請求項１１】
冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するためのプログラムであって、
コンピュータを、
各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、
として機能させる、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１２】
請求項１１に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１３】
請求項１１又は１２に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１４】
請求項１１〜１３の何れかに記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長マニピュレータは、７自由度である、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１５】
請求項１４に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１】
冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する方法であって、
各ステップにおける前記手先の位置及び姿勢状態を取得し、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出し、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成し、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定方法。
【請求項２】
請求項１に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定方法。
【請求項３】
請求項１又は２に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定方法。
【請求項４】
請求項１〜３の何れかに記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記冗長マニピュレータは、７自由度である、
冗長マニピュレータの冗長自由度の決定方法。
【請求項５】
請求項４に記載の冗長マニピュレータの冗長自由度の決定方法であって、
前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である、
冗長マニピュレータの冗長自由度の決定方法。
【請求項６】
冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する装置であって、
各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、
を備える、
冗長マニピュレータの冗長自由度の決定装置。
【請求項７】
請求項６に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定装置。
【請求項８】
請求項６又は７に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定装置。
【請求項９】
請求項６〜８の何れかに記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長マニピュレータは、７自由度である、
冗長マニピュレータの冗長自由度の決定装置。
【請求項１０】
請求項９に記載の冗長マニピュレータの冗長自由度の決定装置であって、
前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である、
冗長マニピュレータの冗長自由度の決定装置。
【請求項１１】
冗長マニピュレータの手先の位置及び姿勢状態を、ＣＰ（Ｃｏｎｔｉｎｕｏｕｓ Ｐｏｉｎｔ）制御にて、第１の位置及び姿勢状態から第２の位置及び姿勢状態へと変化させるに際し、ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するためのプログラムであって、
コンピュータを、
各ステップにおける前記手先の位置及び姿勢状態を取得する状態取得手段と、
各ステップにおける前記手先の位置及び姿勢状態に基づいて、各ステップにおける前記冗長マニピュレータの冗長自由度の実現可能な領域を算出する領域算出手段と、
隣り合うステップ間で前記領域同士が少なくとも一部重複するように、前記第１の位置及び姿勢状態から前記第２の位置及び姿勢状態に至る間の前記領域の組み合わせを作成する組み合わせ作成手段と、
前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する冗長自由度決定手段と、
として機能させる、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１２】
請求項１１に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記組み合わせ作成手段は、前記領域の組み合わせが複数作成可能であったときは、最終のステップにおける前記領域が最も大きな組み合わせを採用する、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１３】
請求項１１又は１２に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長自由度決定手段は、前記領域の組み合わせに基づいて、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定するに際し、ステップ間における前記冗長自由度の差分の二乗和が最も小さくなるように、前記ＣＰ制御の各ステップにおける前記冗長マニピュレータの冗長自由度を決定する、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１４】
請求項１１〜１３の何れかに記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長マニピュレータは、７自由度である、
冗長マニピュレータの冗長自由度の決定プログラム。
【請求項１５】
請求項１４に記載の冗長マニピュレータの冗長自由度の決定プログラムであって、
前記冗長マニピュレータは、Ｓ−Ｒ−Ｓ型である、
冗長マニピュレータの冗長自由度の決定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【公開番号】特開２０１２−４０６３３（Ｐ２０１２−４０６３３Ａ）
【公開日】平成２４年３月１日（２０１２．３．１）
【国際特許分類】
【出願番号】特願２０１０−１８２８０４（Ｐ２０１０−１８２８０４）
【出願日】平成２２年８月１８日（２０１０．８．１８）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年３月１日（２０１２．３．１）
【国際特許分類】
【出願日】平成２２年８月１８日（２０１０．８．１８）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]