冗長ロボットの関節目標値決定方法及び冗長ロボットの制御装置
【課題】関節数がｎ、先端目標値ベクトルＸの次数がｍ（ｎ＞ｍ）である冗長ロボットの関節目標値ベクトルθを少ない計算負荷で決定する。
【解決手段】ベクトルＸを複数の部分ベクトルに分割する。各部分ベクトルに優先度を付与する。各部分ベクトルとベクトルθの間の線形関係式を求める。一の優先度ｋを選択し、「選択された優先度ｋよりも高い優先度を有する全ての部分ベクトルが線形関係式と不等式関係式の拘束条件の下で実行可能である領域が存在するか否か」を判定する（Ｓ１４、Ｓ１５）。その処理を異なる優先度の部分ベクトルに対して実行し、上記領域が存在すると判定された部分ベクトルのうち最も低い優先度を有する部分ベクトルについて、上記拘束条件の下で目的関数Ｃ２を最小にする凸二次計画法の解を求める（Ｓ２１）。求めた解をロボットベクトルθとして決定する（Ｓ２２）。
【解決手段】ベクトルＸを複数の部分ベクトルに分割する。各部分ベクトルに優先度を付与する。各部分ベクトルとベクトルθの間の線形関係式を求める。一の優先度ｋを選択し、「選択された優先度ｋよりも高い優先度を有する全ての部分ベクトルが線形関係式と不等式関係式の拘束条件の下で実行可能である領域が存在するか否か」を判定する（Ｓ１４、Ｓ１５）。その処理を異なる優先度の部分ベクトルに対して実行し、上記領域が存在すると判定された部分ベクトルのうち最も低い優先度を有する部分ベクトルについて、上記拘束条件の下で目的関数Ｃ２を最小にする凸二次計画法の解を求める（Ｓ２１）。求めた解をロボットベクトルθとして決定する（Ｓ２２）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、制御可能な関節の数ｎがロボット先端に与えられる先端目標値ベクトルの次数ｍよりも多い冗長ロボットの関節目標値ベクトルを決定する技術に関する。
【背景技術】
【０００２】
典型的な冗長ロボットは、手先の直交３軸各軸方向位置と各軸回りの回転角の合計６個の先端目標値が与えられる７関節マニピュレータである。ロボット先端に与えられる先端目標値ベクトルの次数ｍよりも制御可能な関節の数ｎが大きいロボットを冗長ロボットという。関節の数ｎと先端目標値ベクトルの次数ｍの差は冗長度と呼ばれる。
【０００３】
近年、人型ロボットの開発が盛んになり、冗長度が極めて大きいロボットを扱うケースが増えている。例えば、６関節の脚を２本と、６関節の腕を２本有するロボットで水を満たしたコップを運ぶ場合を想定する。このロボットは合計２４自由度を有する。このロボットには、安定して歩行するために、夫々の足先位置（脚リンク機構の先端）に対して位置と角度の６自由度の先端目標値ベクトルが与えられる。さらにコップの水をこぼさないためには、コップを把持した手（腕リンク機構の先端）に対して水平２軸周りの回転角の２自由度の先端目標値ベクトルが与えられる。手先の水平面内２軸周りの角度、即ちコップの角度が一定であれば、コップの位置と垂直軸周りの回転角は任意でよい。また、他方の腕は、歩行バランスを維持するという条件の元で従属的にその手先位置と姿勢が定まる。即ち、このロボットは、制御可能な２４自由度を有しながら、ロボット先端（足先と手先）に与えられる先端目標値ベクトルの次数が１４である冗長ロボットである。このロボットの冗長度は、２４（関節自由度）−１４（先端目標値ベクトルの次数）＝１０であり、７自由度マニピュレータよりもはるかに大きな冗長度を有している。なお、ロボットには、制御周期毎に異なる先端目標値ベクトルが与えられる。先端目標値ベクトルの時系列データは「目標軌道」と呼ばれることがある。先端目標値ベクトルをどのように生成するかは、本発明の主眼ではないので説明は省略する。
【０００４】
脚と腕を有する人型ロボットに典型的な問題（典型的ではあるが人型ロボットに限られるものではない）は、大きな冗長度を有することに加えて、先端目標値ベクトルが、夫々別の条件を満たさねばならない部分ベクトルの組み合わせとなっている点である。例えば上記のコップを運ぶロボットの場合、先端目標値ベクトルが、安定歩行という条件を満たすべき足先目標値ベクトルと、コップを水平に保つという条件を満たすべき手先目標値ベクトルの組み合わせになっている。
【０００５】
さらに問題を複雑にするのが、先端目標値ベクトルを達成できる関節目標値が求まらない状況が起こり得ることである。関節目標値が求まらない場合とはどのような場合であるかは後述する。そのような場合、次善の関節目標値を求める必要がある。例えば上記したコップ運びロボットの場合、コップの水がこぼれても、即ち、手先の水平が維持できずとも、安定した歩行を維持することが優先される。
【０００６】
このように、近年の多関節ロボット、典型的には人型ロボットでは、冗長自由度を含む関節目標値を合理的に決定するための技術が望まれている。さらには、先端目標値ベクトルを満足する関節目標値ベクトルが得られない場合、次善の先端目標値ベクトルを決める技術が望まれている。
【０００７】
上記課題に対して一つの解決策を与える技術が非特許文献１で提案されている。非特許文献１の技術は、本発明を説明する上で重要であるので、ここで概説する。
【０００８】
説明に先立って、本明細書で用いる技術用語、及びいくつかの記号を説明する。本明細書では、ベクトルと行列を扱う。ベクトルはかっこ書き｛・｝で表し、行列はかっこ書き［・］で表す。また上付き文字Ｔはベクトル又は行列の転置を表す。
【０００９】
さらに、説明を理解しやすくするために、先端の目標値が速度（角速度）で与えられ、関節目標値は関節速度（角速度）で与えられるケースを想定する。なお、ロボットの技術分野では並進速度と回転角速度を混在して使うことが頻繁にある。そのため、本明細書では、並進速度と角速度の両者を「速度」という用語で総称する。そのような呼称により、回転関節と直動関節を統一して扱うことができる。速度の単位を有する関節目標値ベクトルを｛ｄθ｝で表し、速度の単位を有する先端目標値ベクトルを｛ｄｘ｝で表す。関節の数をｎで表し、先端目標値ベクトルの次数をｍで表す。即ち、ベクトル｛ｄθ」はｎ次元であり、ベクトル｛ｄｘ｝はｍ次元である。ロボットの技術分野では、関節目標値ベクトル｛ｄθ｝と先端目標値ベクトル｛ｄｘ｝の間には、次の（数１）の関係があることは良く知られている。
【００１０】
【数１】
【００１１】
ここで、［Ｊ］はヤコビアンと呼ばれる行列である。ヤコビアン［Ｊ］の次数は、ｍ行ｎ列である。（数１）は手先速度が関節速度で表されることを意味する。手先の目標速度（即ち、先端目標値ベクトル）を実現する関節目標速度（即ち関節目標値ベクトル）を求めるには、ヤコビアン［Ｊ］の逆行列を求めればよい。冗長ロボットの場合、ヤコビアン［Ｊ］は、ｍ行ｎ列（ｍ＜ｎ）であるから非正方行列である。その場合、先端目標値ベクトル｛ｄｘ｝を実現する関節目標値ベクトル｛ｄθ｝は、次の（数２）で求めることができる。
【００１２】
【数２】
【００１３】
（数２）における［Ｊ＋］は、［Ｊ］の擬似逆行列を表し、｛ｋ｝はｎ次元の任意のベクトルである。また、｛ｋ｝＝｛０｝（全ての要素がゼロであるベクトル）の場合、（数２）の解は、二乗ノルムが最小の解となる。
【００１４】
先端目標値ベクトル｛ｄｘ｝が与えられたとき、［Ｊ＋］が求まれば、（数２）から関節目標値ベクトルを求めることができる。しかしながら、擬似逆行列［Ｊ＋］が常に求まるとは限らない。すなわち、（数２）を満たす［Ｊ＋］の解が求まらない場合が、与えられた先端目標値ベクトル｛ｄｘ｝を実現できる関節目標値｛ｄθ｝が求まらない場合に相当する。なお、（数２）に基づいて関節角（｛ｄθ｝は微小時間Δｔ当たりの回転角に等しい）を逐次的に制御する方法は分解速度制御（或いは分解運動速度制御）として良く知られている（例えば、「ロボット制御基礎論」、吉川恒夫著、コロナ社、ISBN:978-4-339-04130-9）。
【００１５】
｛ｄｘ｝は、先端目標値ベクトルであり、ロボットに行わせたい作業から定められる。先のコップ運びロボットの場合、｛ｄｘ｝は、安定して歩行するための各足先６個の速度と手先の２方向速度の合計１４個の要素を有するベクトルとなる。従って冗長ロボットの問題は、与えられる先端目標速度ベクトル｛ｄｘ｝を実現する目標関節速度ベクトル｛ｄθ｝を求めることであり、即ち、（数２）を満足する解を求めることである。
【００１６】
しかし、実際のロボットにおいては、関節目標値｛ｄθ｝はどんな値でも取り得るわけではない。例えば、ロボットのモータの性能によって出せる最大速度が存在する。また、関節には通常可動範囲が存在するが、可動範囲外へ向かうような関節目標値をとることはできない。つまり、（数１）の等式で示される条件に加えて、次のような、関節目標値の最大値と最小値で表される不等式の制約条件を考慮する必要がある。
【００１７】
【数３】
【００１８】
そこで、非特許文献１においては（数３）の不等式条件も考慮したアルゴリズムが提案されている。非特許文献１に開示された関節目標速度ベクトル（関節目標値ベクトル）の決定アルゴリズムを説明する。まず、目的（タスク）別に、先端目標値ベクトル｛ｄｘ｝を複数の部分ベクトル｛ｄｘｉ｝に分割する。ここで、下付き文字「ｉ」は各部分ベクトルを区別するための指標を表す。「目的（タスク）」とは、ロボットに実行させたい作業の一つひとつを意味する。そして、複数の部分ベクトル｛ｄｘｉ｝の夫々に対して、ロボットが達成すべき順に優先度を付与する。非特許文献１の技術は、優先度の高い順に、できるだけ多くの部分ベクトルを同時に実現できる関節目標値ベクトルを求めるアルゴリズムである。
【００１９】
先のコップ運びロボットでは、安定に歩行することと、コップの水をこぼさないことの２つの目的（タスク）がある。先端目標値ベクトルの観点からみれば、「目的（タスク）」とは、先端目的値ベクトルの要素群のうち、同時に満たさねば意味を成さない要素の集まりに相当する。先のコップ運びロボットの場合、２つの目的（タスク）に分解できる。夫々の目的（タスク）に対応する部分ベクトルは、両脚足先の目標速度を指定する１２次元の速度ベクトル｛ｄｘ１｝と、手先の水平面内２軸周りの目標速度を指定する２次元の速度ベクトル｛ｄｘ２｝である。安定に歩行するための足先目標速度を指定する部分ベクトル｛ｄｘ１｝に、手先の水平面内２軸周りの目標速度を指定する部分ベクトル｛ｄｘ２｝よりも高い優先度が与えられる。非特許文献１の技術は、部分ベクトル｛ｄｘ１｝と｛ｄｘ２｝の双方を同時に実現する関節目標速度｛ｄθ｝が存在しない場合、優先度の高い部分ベクトル｛ｄｘ１｝を実現する関節目標速度｛ｄθ｝を決定する。
【００２０】
説明を簡単にするために、複数の部分ベクトルを｛ｄｘｉ｝で表し、各部分ベクトルを区別する指標ｉが小さいほど優先度が高いことを表すものとする。同時に符号「ｉ」は、目的（タスク）を区別するための指標を表す。即ち、第１番目の部分ベクトル｛ｄｘｉ｝に対応する目的（タスク）を第１番目のタスクと呼ぶことにする。なお、下付き文字「ｉ」の最大値は、ロボットが同時に実行することが要求されているタスクの数に相当する。
【００２１】
次に、夫々の部分ベクトル｛ｄｘｉ｝に対して、部分ベクトルと関節目標速度ベクトル｛ｄθ｝の間の線形関係式｛ｄｘｉ｝＝［Ｊｉ］｛ｄθ｝を求める。ここで、［Ｊｉ］は、部分ベクトル｛ｄｘｉ｝と関節目標速度｛ｄθ｝の関係を示すヤコビアンであり、ロボットの運動学的特性から定まる。なお、［Ｊｉ］は、｛ｄｘｉ｝の次数と同数の行と、関節数ｎと同数の列を有する行列となる。
【００２２】
（数３）を満たしつつ、できるだけ高い優先度のタスクの部分ベクトル｛ｄｘｉ｝を同時に実現できる関節目標速度｛ｄθ｝を求めることが、非特許文献１の目的である。非特許文献１の技術は、優先度の高い順にできるだけ上位までのタスクを同時に満たすことができる解を、次の手順で決定する。
【００２３】
付与された優先度から一の優先度を選択し（選択された優先度をｋ、即ち、ｉ＝ｋとする）、選択された優先度ｋに対応する部分ベクトル｛ｄｘｋ｝について、次の（数４）、（数５）で表される凸二次計画問題を解く。
【００２４】
【数４】
【数５】
【００２５】
ここでいう「凸二次計画問題」とは、（数５）の拘束条件の元で目的関数Ｃ２を最小化する｛ｄθ｝を凸二次計画法に基づいて求める問題を意味する。なお、所定の拘束条件の元で凸二次計画法に基づいて所定の目的関数を最小にする凸二次計画問題は、数理物理の技術分野では良く知られているので詳しい説明は省略する。
【００２６】
ここで、目的関数Ｃ２を最小にする｛ｄθ｝は、（数５）の拘束条件を満足する。（数５）の第１式は、選択された優先度ｋよりも高い優先度（ｉ＜ｋ）の全ての部分ベクトル｛ｄｘｉ｝が、線形関係式｛ｄｘｉ｝＝［Ｊｉ］｛ｄθ｝を満たすことを拘束条件とすることを表している。即ち、（数５）の第１式の拘束条件を課すことによって、（数４）の凸二次計画問題の解｛ｄθ｝は、ｋより優先度の高い部分ベクトル｛ｄｘｉ｝を同時に満足しつつ、最上位の優先度ｋの部分ベクトル｛ｄｘｋ｝の二乗誤差をできるだけ小さくするような解となる。
【００２７】
また、（数５）の第２式は、各関節に課せられた動作制限条件に関する拘束条件である。現実のロボットでは、関節の動作に物理的な制約条件が課される。（数４）が関節速度｛ｄθ｝の解を求める問題であるので、動作制限条件も、関節速度の次元で表される。すなわち、ここでは、目的関数Ｃ２を最小にする関節目標速度ベクトル｛ｄθ｝は｛ｄθｍｉｎ｝以上｛ｄθｍａｘ｝以下であることが要求されている。凸二次計画問題は、（数５）の拘束条件を満足することを要求するから、その解｛ｄθ｝は拘束条件で与えられる動作制限条件を満足する。
【００２８】
非特許文献１の技術をまとめると次の通りである。（数４）と（数５）で表される凸二次計画問題は、各関節の目標速度が動作制限条件内であり、かつ、ｋより高い優先度の部分ベクトルの実現を保証するという条件の下で、優先度ｋの部分ベクトルをできる限り実現することができる関節目標速度ベクトル｛ｄθ｝を求める問題である。即ち、凸二次計画問題の解｛ｄθ｝は、動作制限条件を満足した上で、第ｋ番目のタスクの誤差を最小化する（もしくは、第ｋ番目のタスクをできるだけ満足する）。以上のアルゴリズムは、非特許文献１、及び、非特許文献２に詳しいのでそちらを参照されたい。
【００２９】
最低の優先度が与えられた部分ベクトル｛ｄｘｉ｝について、（数４）と（数５）で与えられる凸二次計画問題の解が求まれば、その解が、全ての部分ベクトル｛ｄｘｉ｝を同時に達成できる関節目標速度｛ｄθ｝に相当する。即ち、全てのタスクを同時に実現する関節目標値ベクトルが求まることになる。しかしながら、場合によってはそのような解が存在しないこともあり得る。そこで、非特許文献１では、（数４）と（数５）で与えられる凸二次計画問題を複数の優先度に対して実行し、最も高い優先度に相当する解を最終的に関節目標速度ベクトルとして採用する。具体的には、最も高い優先度から低位の優先度へ向けて順に（数４）と（数５）の凸二次計画問題を解く。解が得られなかった場合、その１つ高位の優先度に相当する解を関節目標速度ベクトルとして採用する。以上のアルゴリズムにより、最上位から順に、最も多くのタスクを同時に実現する関節目標速度ベクトル（関節目標値ベクトル）が決定できる。
【先行技術文献】
【非特許文献】
【００３０】
【非特許文献１】Oussama Kanoun, Florent Lamiraux, Pierre-Brice Wieber, Fumio Kanehiro, Eiichi Yoshida, and Jean-Paul Laumond, “Prioritizing Linear equality and inequality systems: application to local motion planning for redundant robots”, Proc. of IEEE International Conference on Robotics and Automation, pp. 2939-2944, 2009
【非特許文献２】Bernard Faverjon, Pierre Tournassoud, “A local Based Approach for Path Planning of Manipulators With a High Number of Degrees of Freedom”, In Proc. of IEEE International Conference on Robotics and Automation, pp. 1152-1159, 1987
【発明の概要】
【発明が解決しようとする課題】
【００３１】
非特許文献１の手法は、冗長ロボットの関節目標値ベクトルの決定に極めて有効な手法であるが、凸二次計画法は計算負荷が高いという課題がある。上記したように非特許文献１の技術は、複数のタスクの夫々について凸二次計画法を解かねばならない。タスクの数が多くなるほど、即ち、部分ベクトルの数が多くなるほど、解かねばならない凸二次計画問題の数も多くなる。他方、冗長ロボットは、動作中に時々刻々変化する状況に応じて関節目標値ベクトルを決めねばならない。典型的にはサンプリング時間毎に関節目標値ベクトルを決定しなければならない。そのため、関節目標値ベクトルを求めるための計算負荷は小さいことが望ましい。本発明は、非特許文献１が提唱する関節目標値ベクトルの決定アルゴリズムを改良するものであり、その計算負荷を低減することを目的とする。
【課題を解決するための手段】
【００３２】
非特許文献１に開示されたアルゴリズムでは、凸二次計画問題を複数回解いている。しかしながら、最終的に関節目標値ベクトルとして用いる解は、解が存在するタスクのうち、最も高い優先度についての凸二次計画問題の解である。それ以外の優先度のタスクについては、凸二次計画問題を解くがその解は使われない。発明者らはこの点に着目した。即ち、優先度が定められた複数のタスクのそれぞれに対しては、凸二次計画問題の具体的な解を得ることが目的ではなく、凸二次計画問題の解が存在するか否かが判定できればよい。凸二次計画問題の解が存在することが判明したタスクのなかから、最も優先度が高いタスクの部分ベクトルに対して、凸二次計画問題の具体的な解を求めればよい。発明者らは、「拘束条件が実行可能領域であるか否か」を確認することで、同じ拘束条件の下で凸二次計画問題の解の存在を判定でき、計算負荷を低減することを思いついた。この、「拘束条件が実行可能領域である」とは、（数５）のうちの等式条件による連立方程式の解が、（数５）の上下限制約内に存在する、ということと等価である。もし「拘束条件が実行可能領域である」場合、凸二次計画問題は必ず解を持つ。なぜなら、凸二次関数の目的関数は、その実行可能領域内の任意の点について計算でき、そのうちの最小値を持つものが解となるからである。またもし、「拘束条件が実行可能領域でない」場合、凸二次計画問題は解を持たない。拘束条件を満たす実行可能な点が存在しないからである。この「拘束条件が実行可能領域である」かどうかを判定するには、線形計画問題の効率的な解法の一つである「２段階シンプレックス法」の一部を用いることができる。なお、「線形計画問題」とは、「凸二次計画問題」と同様の形で設定された拘束条件の下、与えられた線形な目的関数を最小にする解を線形計画法に基づいて解くことを意味する。「２段階シンプレックス法（単体法）」とは、まず線形計画問題の拘束条件を満たす解（「実行可能基底解」と呼ばれる）を１つ発見し（第１段階）、つぎにその「実行可能基底解」から出発して目的関数を最小化する解を探索する（第２段階）手法である。手法の詳細については、後段もしくは文献（例えば、茨木俊秀、福島雅夫、「FORTRAN77最適化プログラミング」、岩波書店、１９９１）を参照されたい。つまり、この２段階シンプレックス法の第１段階で求められる「実行可能基底解」が存在すれば、当然「拘束条件は実行可能である」と言える。また、「実行可能基底解」が存在しなければ、「拘束条件は実行可能ではない」と言える。
【００３３】
「拘束条件の実行可能領域の存在を確認する」操作は、同じ拘束条件の下での凸二次計画問題を解く操作よりも計算負荷がはるかに小さい。従って、ある優先度までのタスク（複数の部分ベクトル）に対してまず実行可能領域の存在を確認することで解の存在を判定し、解が存在する優先度までのタスクのうち、最も優先度が高いタスクについて凸二次計画問題として扱って解を求めれば、上記した非特許文献１のアルゴリズムと同じ結果が得られる。即ち、非特許文献１のアルゴリズムと同じ結果を少ない計算負荷で求めることができる。
【００３４】
非特許文献１のアルゴリズムと本明細書が開示する新規なアルゴリズムの相違を明確にするために、まず、非特許文献１のアルゴリズムをまとめる。なお、以下では、一般的な表現とするために、先に用いた先端目標速度ベクトル｛ｄｘ｝、関節目標速度ベクトル｛ｄθ｝、及び、ヤコビアン［Ｊ］を、先端目標値ベクトル｛ｄｘ｝、関節目標値ベクトル｛ｄθ｝、及び、行列［Ａ］と表現する。
【００３５】
ステップ１：ｍ次元の先端目標値ベクトル｛ｄｘ｝を、複数の部分ベクトル｛ｄｘｉ｝に分解する。ただし、下付きｉは各部分ベクトルを区別する指標である。そして、複数の部分ベクトル｛ｄｘｉ｝の夫々に対して、ロボットが達成すべき順に優先度を付与する。ここで、各部分ベクトルを区別する指標ｉが小さいほど優先度が高いことを表すとする。
【００３６】
ステップ２：夫々の部分ベクトル｛ｄｘｉ｝に対して、部分ベクトルと関節目標値ベクトル｛θ｝の間の線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝を求める。ここで、［Ａｉ］は、部分ベクトルごとにロボットの運動学的特性から定まる行列であり、｛ｄｘｉ｝の次数と同数の行と、関節の数ｎと同数の列を有する行列を表す。
【００３７】
ステップ３：複数の優先度から一の優先度ｋ（ｉ＝ｋ）を選択し、選択された優先度ｋの部分ベクトル｛ｄｘｋ｝に対して、次の凸二次計画問題の解を求める。凸二次計画問題：選択された優先度ｋよりも高い優先度を有する全ての部分ベクトル｛ｄｘｉ｝が線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝（ただしｉ＜ｋ）、及び、不等式関係式｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝（ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限）を満足することを拘束条件として、目的関数Ｃ２＝（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）Ｔ（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）を最小にする凸二次計画法の解｛ｄθ｝を求める。
【００３８】
ステップ４：ステップ３を優先度が異なる複数の部分ベクトル｛ｄｘｉ｝に対して実行する。そして、凸二次計画問題の解が求まった部分ベクトルのうち最も高い優先度を有する部分ベクトルに対する解をロボット関節目標値として決定する。
【００３９】
本発明のアルゴリズムは、上記ステップ３とステップ４を次のステップ３と４に置き換える。
【００４０】
ステップ３：複数の優先度から一の優先度ｋ（ｉ＝ｋ）を選択し、選択された優先度ｋの部分ベクトル｛ｄｘｋ｝に対して、その拘束条件の実行可能領域の存在を判定する。具体的には、線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝（ただし、ｉ＜ｋ）、及び、不等式関係式｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝（ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限）の実行可能基底解を、線形計画法の一種である２段階シンプレックス法の第１段階の操作によって求める。
【００４１】
ステップ４：ステップ３を優先度が異なる複数の部分ベクトル｛ｄｘｉ｝に対して実行する。そして、与えられた拘束条件の実行可能領域が存在すると判定された部分ベクトルのうち最も高い優先度を有する部分ベクトルについて拘束条件とし、目的関数｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝を最小にする凸二次計画法の解｛ｄθ｝を求める。
【００４２】
ステップ５：ステップ４で求められた解｛ｄθ｝をロボット関節目標値ベクトルに設定する。
【００４３】
上記のアルゴルムによれば、概ね、「（凸二次計画法の計算負荷−実行可能領域の存在確認の計算負荷）×従来のアルゴリズムにおいて凸二次計画問題を実施した回数」で評価される計算負荷が低減できる。また、典型的な事例としては、｛ｄｘ｝はロボット先端の目標速度ベクトルであり、｛ｄθ｝は関節目標速度ベクトルであり、［Ａｉ］はロボットのヤコビアンである。
【００４４】
本明細書が開示する技術は、上記のアルゴリズムを実行する冗長ロボット制御装置に具現化することも好適である。なお、そのような制御装置は、上記ステップ１の複数の部分ベクトル｛ｄｘｉ｝の夫々について、線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝と優先度を記憶している。そして制御装置は、ロボットの動作中にリアルタイムに、即ち、予め定められた周期毎に、上記ステップ３から５を実行し、得られたロボット関節目標値ベクトルに追従するように各関節を制御する。そのような制御装置を使うことによって、冗長ロボットを効率的に制御することができる。
【図面の簡単な説明】
【００４５】
【図１】脚式ロボットの模式図である。
【図２】目標関節角ベクトル決定処理のフローチャート図である。
【実施例】
【００４６】
以下、図面を参照しながら、冗長ロボットの関節目標値決定プロセスを具体的に説明する。ここでは、前述した例と同じ、冗長自由度を有する人型ロボットに水の入ったコップを運ばせる作業を想定し、両方の足先とコップを保持した手先が目標軌道に追従するための関節目標値を決定するプロセスを説明する。
【００４７】
図１に、脚式ロボット１００の模式図を示す。以下、脚式ロボット１００を単にロボット１００と称する。ロボット１００は、左右の脚１２Ｒ、１２Ｌ、左右の腕１４Ｌ、１４Ｒを有する人型ロボットである。頭部にはコントローラ１０が内蔵されている。夫々の脚は、６個の関節を備える多リンク多関節構造を有する。即ち、夫々の脚は６自由度を有する。夫々の腕も、６個の関節を備える多リンク多関節構造を有する。即ち、夫々の腕も６自由度を有する。各関節にはリンクを揺動させるモータが内蔵されており、コントローラ１０が各モータを制御する。以上の通り、ロボット１００は、合計で２４自由度を有する。
【００４８】
ロボット１００は、脚の各関節のモータを適宜に制御することによって歩行することができる。歩行動作のための脚先や重心の軌道の生成アルゴリズムは、本発明の主眼ではないので説明は省略する。また、ロボット１００は、腕の各関節のモータを適宜に制御することによって、物体を把持・移動することができる。
【００４９】
今、ロボット１００に与えられた作業は、水をこぼさないように腕でコップＰを保持しながら所定の経路に沿って歩行することである。この作業は、「安定に歩行する」というタスク（第１タスク）と、「水をこぼさないようにコップを保持する」というタスク（第２タスク）に分解できる。
【００５０】
第１タスクは、右脚１２Ｒの足先の軌道と、左脚１２Ｌの足先の軌道で表される。なお、安定に歩行するための軌道を生成するアルゴリズムについては説明を省略する。ここでは、安定に歩行するための左右の足先軌道が与えられているとする。即ち、左右の足先の位置ベクトルの時系列データは与えられる。ここで、「位置」には、いわゆるロール角、ピッチ角、ヨー角を含む。一方の足先の位置ベクトルは６個の変数要素（３軸各軸方向の位置と３軸各軸周りの角度）を含む。従って、左右の足先で合計１２要素の位置ベクトルとなる。ここでは、安定に歩行するための足先軌道は、各時刻における足先の速度ベクトルの時系列データで表されていると仮定する。即ち、「安定して歩行する」という第１タスクは、ロボット１００の両足先が追従すべき、１２要素を有する足先目標速度ベクトル（先端目標値ベクトル）で表現される。いま、第１タスクの足先目標速度ベクトルを｛ｄｘ１｝で表す。｛ｄｘ１｝は１２次元のベクトルである。なお、より正確には、「安定して歩行する」タスクにはここで述べた両足先位置姿勢の第１タスクに加えて、ロボットの重心速度や角運動量に対するタスクも加える必要があることが知られているが、ここでは例を簡単にするために考慮しない（必要であればそれらのタスクも加えて考慮できる）。
【００５１】
同様に、第２タスクは、コップＰを保持する手先のピッチ軸周りの速度（角速度）とロール軸周りの速度（角速度）の時系列データで表される。即ち、第２タスクは手先のロール軸周りの速度とピッチ軸周りの速度の合計２要素の手先目標速度ベクトル（先端目標値ベクトル）で表される。なお、コップＰの水をこぼさないための手先目標速度ベクトルは、手先が水平である場合は速度＝ゼロであり、手先が水平でない場合は水平（ロール角＝ゼロ、ピッチ角＝ゼロ）に保とうとする方向の速度が（その時点の手先姿勢に応じてフィードバック的に）与えられる。第２タスクの目標速度ベクトルを｛ｄｘ２｝で表す。｛ｄｘ２｝は２次元のベクトルである。
【００５２】
コントローラ１０の使命は、２４個の関節を使って、両足先の速度（１２次元）と、コップＰを把持した腕の手先の速度（２次元）を、合計１４次元の先端目標速度ベクトル（｛ｄｘ１｝，｛ｄｘ２｝）に追従させることである。別言すると、コントローラ１０の使命は、制御可能な関節の数ｎ＝２４がロボット先端に与えられる先端目標速度ベクトル（先端目標値ベクトル）の次数ｍ＝１４よりも多い冗長ロボット１００の関節目標速度ベクトル（関節目標値ベクトル）を決定することである。ここで、先端目標速度ベクトルは、第１タスクに対応する１２次元の先端目標速度ベクトル｛ｄｘ１｝と、第２タスクに対応する２次元の先端目標速度ベクトル｛ｄｘ２｝に分解される。
【００５３】
コントローラ１０が実行する処理を説明する前に、コントローラ１０が予め記憶しておくべき数式について説明する。
【００５４】
まず、２つのタスクの夫々に優先度を付与する。コップの水をこぼさないことより、安定に歩行できることが重要であるから、第１タスクの優先度を最高位に定め、第２タスクの優先度を次の順位に定める。即ち、第ｉタスクについて、タスクを区別する指標ｉが小さいほど高位のタスクであることを示す。
【００５５】
今、２４個の関節の速度をベクトル｛ｄθ｝（｛ｄθ｝は２４次元ベクトル）で表す。ロボットの先端（足先、手先）の速度と関節角速度にはヤコビアン［Ｊ］を使った関係が成立する。本実施例の場合、次の（数６）が成立する。
【００５６】
【数６】
【００５７】
［Ｊ１］は、関節目標速度ベクトル｛ｄθ｝と両方の足先の速度ベクトル｛ｄｘ１｝との間の線形関係式であり、［Ｊ１］はロボット１００の幾何学的構成から定まるヤコビアンである。［Ｊ２］は、関節速度ベクトル｛ｄθ｝とコップを保持した腕の手先の速度ベクトル｛ｄｘ２｝との間の線形関係式であり、［Ｊ２］はロボット１００の幾何学的構成から定まるヤコビアンである。（数６）の第１式を、第１タスクの線形関係式と称し、（数６）の第２式を第２タスクの線形関係式と称する。コントローラ１０は、それら２つの線形関係式を記憶している。
【００５８】
また、関節ベクトル｛ｄθ｝は、実機ロボットにおいては制約が存在する。関節ベクトルの制約を｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝のように不等式関係を設定する。ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限速度制約である。コントローラ１０は、この不等式関係式も記憶している。なお、ロボットの関節角度可動範囲及び関節角加速度の制約をこの関節速度の不等式関係に変形する方法については後述する。
【００５９】
コントローラ１０が目標関節速度ベクトルを決定する処理のフローチャートを図２に示す。概説すると、コントローラ１０は、上位の優先度から低位の優先度へ向けて、できるだけ多くのタスクを同時に実現できる関節目標速度｛ｄθ｝を決定する。以下、図２のフローチャートの各ステップについて説明する。なお、以下の説明では、議論を一般化するためにタスクの数をｔ個とする。即ち、（数６）で表される線形関係式はタスクの数ｔに等しい数だけ存在する。
【００６０】
初期化（ステップＳ１２）
図２における記号「ｉ」は、タスクを識別する指標であるとともに、タスクの優先度を表す指標でもある。記号「ｉ」が小さいほど、優先度が高いことを示す。コントローラ１０は、優先度の高い方から順にステップＳ１３、Ｓ１４、Ｓ１５、及びＳ１６の処理を繰り返す。そのために、変数ｉを「１」で初期化する。
【００６１】
関節角実行可能領域の存在のチェック（ステップＳ１４、Ｓ１５）
ｉ番目のタスクまでの等式関係（数６）および不等式関係｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝を連立させた条件は（数７）となる。この（数７）のすべての等式および不等式条件を満たす｛ｄθ｝の集合を、「実行可能領域」と呼ぶ。（数７）には、実行可能領域が存在する場合と、存在しない場合の２つがありうる。（数７）の実行可能領域が存在する場合、（数７）を条件とした凸二次計画問題の解は、必ず存在する。また、（数７）の実行可能領域が存在しない場合、当然ながら（数７）を条件とした凸二次計画問題の解は存在しない。つまり、凸二次計画問題の解自体を求めなくても、その解の存在を判定することができる。
【００６２】
【数７】
【００６３】
なお、本実施例では、（数７）の実行可能領域が存在するか否かを判定するために、線形計画問題の解法の一つである、２段階シンプレックス法の第１段階にあたる操作を行う。２段階のシンプレックス法は既知であるので説明は省略する。図２のステップＳ１５の分岐判定は、（数７）の実行可能領域が存在する場合に「ＹＥＳ」となり、（数７）の実行可能領域が存在しない場合に「ＮＯ」となる。
【００６４】
コントローラ１０の処理は、（数７）の実行可能領域が存在する場合、ステップＳ１６に進む。他方、（数７）の実行可能領域が存在しない場合、ステップＳ２０に進む。
【００６５】
条件を満たす優先度ｋの特定（ステップＳ１３、Ｓ１５、Ｓ２０）：
コントローラ１０は、上記した（数７）の実行可能領域の存在のチェックを、優先度が最も高いタスクから低位の優先度のタスクへと順次行う（ステップＳ１３：ＮＯ、ステップＳ１４）。優先度が第ｉ番目のタスクにおいて（数７）の実行可能領域が存在しなかった場合（ステップＳ１５：ＮＯ）、第ｉ−１番目のタスクが、自身よりも高位の優先度を有する全てのタスクを満足する｛ｄθ｝が存在するタスクのうち、最も優先度が高いタスクとなる。最も低位のタスクにおいても（数７）の実行可能領域が存在する場合（ステップＳ１５：ＹＥＳ）、全てのタスクを満足する｛ｄθ｝が存在することが判明する。ステップＳ２０は、自身を含めないで自身よりも高位の全てのタスクを満足する｛ｄθ｝が存在するタスクの中で最も優先度が低いタスクの番号を変数ｋに代入する処理である。即ち、ステップＳ２０において、第ｋ番目のタスクが、自身を含めないで自身よりも高位の優先度を有する全てのタスクを満足する｛ｄθ｝が存在するタスクの中で最も優先度が低いタスクに相当する。なお、第ｋ番目のタスクは、ｉ＜ｋのタスクが実行可能であるようなもののうち、最も優先度の低いものが選ばれることに留意されたい。
【００６６】
凸二次計画問題（ステップＳ２１）：
コントローラ１０は、第ｋ番目のタスクをできるだけ満足する、タスクの誤差を最小化する、関節目標速度ベクトル｛ｄθ｝を求める。コントローラ１０は、（数７）の制約条件の元で、次の（数８）で与えられる目的関数Ｃ２を最小化する凸二次計画法の解を求める。なお、上述したように、第ｋ番目のタスクについては凸二次計画問題の解が存在することは保証されている。
【００６７】
【数８】
【００６８】
コントローラ１０は、（数８）で与えられる目的関数Ｃ２を最小にする凸二次計画法の解｛ｄθ｝を、関節目標速度ベクトル（関節目標値ベクトル）に設定する（ステップＳ２２）。そしてコントローラ１０は、関節速度の現在値が関節目標速度ベクトルに一致するように関節のモータを制御する。コントローラ１０は、図２の処理を、サーボ周期毎に実行する。ロボットのサーボ周期は通常数msecである。
【００６９】
（数８）の目的関数Ｃ２を最小にする凸二次計画法の解｛ｄθ｝は、（数７）で与えられる制約条件を満足するから、第ｋ番目のタスクよりも優先度が高いタスクは全て満たす。こうしてコントローラ１０は、優先度が与えられた複数のタスクの中から、優先度が高い順に最も多くのタスクを満足する解を関節目標値ベクトルとして決定する。上記のアルゴリズムは、上位からできるだけ多くのタスクを満足する関節目標値ベクトルを決定するのに、凸二次計画問題を１回だけ解けばよい。従来は、凸二次計画問題を複数回解く必要があり、最大ではタスクの数と同数回も解く必要があった。本実施例で説明したアルゴリズムは、従来のアルゴリズムよりも顕著に計算負荷を低減することができる。なお、凸二次計画問題を高速に解くには、例えばGoldfarb-Idnani法（有効制約法：Active Set Method）を使うとよい。
【００７０】
図２におけるステップＳ１３からＳ１６のループを繰り返す処理が、特許請求の範囲の記載おけるステップ３の一例に相当する。図２におけるステップＳ２１の処理が、特許請求の範囲の記載におけるステップ４の一例に相当する。図２のステップＳ２２の処理が、特許請求の範囲の記載におけるステップ５の一例に相当する。
【００７１】
上記した実施例の留意点について述べる。図２のステップＳ１３からＳ１６のループは、必ずしもｉ＝１から始める必要はない。例えば、第ｒ番目のタスクについてはそれより高位の優先度のタスクを満足することが分かっている場合は、第ｒ＋１番目のタスクから始めればよい。
【００７２】
（数７）の第２式は、関節目標速度ベクトル｛ｄθ｝が制限範囲内となることを保証するための制約条件であった。速度の代わりに（或いは速度とともに）、加速度（角加速度）が関節の動作制限として加えられることがある。上記したアルゴリズムでは、加速度の制約条件を関節速度の制約条件に変形することもできる。以下、関節加速度の制限範囲を考慮する手法について説明する。
【００７３】
現在の関節速度ベクトルを｛ｄθ０｝とすると、Δｔ秒後の関節速度ベクトル｛ｄθ１｝は現在の関節加速度ベクトル｛ａθ０｝を使って次の（数９）で与えられる。
【００７４】
【数９】
【００７５】
（数９）に基づけば、関節加速度についての制限条件は、Δｔ秒後の関節速度ベクトル｛ｄθ１｝に対する制限として、次の（数１０）で与えることができる。
【００７６】
【数１０】
【００７７】
（数７）の制約条件に（数１０）を加えることで、加速度に関する制限条件を（数７）の実行可能領域の存在判定、及び（数８）の凸二次計画問題に加えることができる。
【００７８】
次に、関節角度の制限について説明する。なお、以下では説明を簡単にするため、ベクトル量のかわりにスカラ量で説明する。即ち、ベクトルを表す｛・｝を外して説明する。今、ある関節の角度の上限値をθｍａｘ、下限値をθｍｉｎで表す。この関節が速度ｄθｍｉｎとｄθｍａｘのどちらかの速度で動いているとする。このとき、最大速度で減速し始めたと仮定して、角度上下限値において速度をゼロにできる猶予角度ψｍｉｎ、ψｍａｘが、以下の（数１１）で定めることができる。
【００７９】
【数１１】
【００８０】
現在の関節角度θがψｍａｘ以上である場合、これらの猶予角度を用いて関節速度の上限値を次の（数１２）で与えると、関節角度の上限値を超えないように動作させることができる。なお、θ＝θｍａｘで速度がゼロとなることに留意されたい。
【００８１】
【数１２】
【００８２】
同様に、下限についてもθがψｍｉｎ以下である場合に関節速度の下限値を次の（数１３）で与えればよい。
【００８３】
【数１３】
【００８４】
上記説明したように、関節の角度制限、加速度制限も、速度制限の形式で制約条件に盛り込むこができる。
【００８５】
次に、タスク自体が不等式で与えられる場合であっても、上記したアルゴリズムを適用できることを説明する。繰り返すが、先の実施例では、第ｉ番目のタスクは、次の（数１４）で与えられた。
【００８６】
【数１４】
【００８７】
ここでは、先端目標値ベクトル｛ｄｘｉ｝が｛ｄｘｉ｝ｍｉｎから｛ｄｘｉ｝ｍａｘの範囲にあればよい場合を想定する。即ち、第１タスクが次の（数１５）で与えられるケースを想定する。
【００８８】
【数１５】
【００８９】
｛ｄｘｉ｝ｍｉｎと｛ｄｘｉ｝ｍａｘは夫々先端目標速度ベクトルの下限値と上限値の制約に相当する。［Ｊｉ］は上記実施例と同様にヤコビアンである。上下限のいずれか一方しか存在しない場合は、他方の制約の大きさを極端に大きくすればよい。（数１５）を不等式条件と称する。
【００９０】
（数１５）の不等式条件を採用する場合、先に示した凸二次計画問題の形式（標準形と称する）とは適合しない。凸二次計画問題の標準形は、制約条件として等式条件と変数の上下限の形であるとされているからである。
【００９１】
しかし、スラック変数と呼ばれる変数を導入することで、不等式条件を等式条件に変形することができる。つまり、不等式条件式（数１５）において、次の（数１６）で与えられるスラック変数｛ｄθｓ｝を導入する。
【００９２】
【数１６】
【００９３】
上記したスラック変数｛ｄθｓ｝を導入することで、（数１５）で与えられるタスクｉは次の（数１７）のとおり、等式条件と各変数の上下限に変形することができる。
【００９４】
【数１７】
【００９５】
（数１７）の形式は、実行可能領域の存在判定と凸二次計画法の双方に適用できる。以上のようにしてタスクとして不等式の制約を考慮する方法は、多自由度マニピュレータで障害物や自己との干渉回避を行うために提案された。その詳細は、Bernard Faverjon and Pierre Tournassoud, “A local based approach for path planning of manipulators with a high number of degrees of freedom”, In ICRA, pp.1152-1159, 1987 に詳しい。また、そのような手法を人間型ロボットに適用した例が、Oussama Kanoun, Florent Lamirauxm, Pierre-Brice Wieber, Fumio Kanehiro, Eiichi Yoshida, and Jean-Paul Laumond, “Prioritizing linear equality and inequality systems: application to local motion planning for redundant robots”, In Proc. of the IEEE International Conference on Robotics and Automation, pp. 2939-2944, 2009 に開示されている。
【００９６】
凸二次計画問題と実行可能領域についての補足事項を説明する。なお、以下では、ベクトルを表す記号｛｝、行列を表す記号［］は省略する。以下では、小文字はベクトルを表し、大文字は行列を表す。次の（数２５）で表される凸二次計画問題を考える。
【００９７】
【数１８】
【００９８】
（数１８）の凸二次計画問題の解の存在を知るには、実際に最小値を求める必要はない。もし（数１８）の第２式以降で表された等式及び不等式に実行可能領域が存在しない場合、実行可能な解が無いため、当然ながら最小値は存在しない。また実行可能領域が存在すれば、領域内のそれぞれの点について（数１８）の第１式の目的関数値が計算でき、どこかに最小値を見つけられることは明らかである。この（数１８）の条件に実行可能領域が存在するか否かは、同一の条件を設定した線形計画問題において、その効率的な解法の一つである、２段階シンプレックス法の第１段階を行うことで判定できる（例えば、茨木俊秀、福島雅夫、「FORTRAN77最適化プログラミング」、岩波書店、１９９１）。
【００９９】
二段階シンプレックス法では、第一段階目として、まず初期解として実行可能基底解を一つ見つけ出す作業を行う。例えば、次の（数１９）で与えられる線形計画問題（ＬＰ−Ｉ）を想定する。
【０１００】
【数１９】
【０１０１】
（数１９）の線形計画問題（ＬＰ−Ｉ）について、人為変数ｘｎ＋ｉ＊（ｉ＝１・・・ｍ）を導入し、（数２０）で与えられる新たな線形計画問題（ＬＰ−II）を考える。
【０１０２】
【数２０】
【０１０３】
この線形計画問題（ＬＰ−II）は通常のシンプレックス表の操作で最小解を求めることができる。このとき、もとの線形計画問題（ＬＰ−Ｉ）が解を持つためには線形計画問題（ＬＰ−II）の解のうち、人為変数ｘｎ＋ｉ＊（ｉ＝１・・・ｍ）が全てゼロとならなければならないため、これを確かめればよい。つまり、解の存在判定は線形計画問題の解法自体よりもさらに容易に行うことができる。上記の手法は、実施例で示した、複数のタスクをできるだけ満たす関節目標値ベクトル決定手法に適用することができる。解の存在判定に用いる計算は、条件の複雑さにもよるが凸二次計画問題の求解に比べれば小さい。そして、タスクの数が増えても凸二次計画問題の求解は１回で済むため、大幅な計算量の縮小が期待できる。
【０１０４】
次に凸二次計画問題の設定方法についての補足事項を説明する。優先度ｋのタスクが等式制約であった場合、その二乗誤差は、次の（数２１）で与えられる。
【０１０５】
【数２１】
【０１０６】
従って、次の（数２２）で与えられる凸二次計画問題の目的関数として用いることができる。
【０１０７】
【数２２】
【０１０８】
一般にＡｋＴＡｋは正定でないため、微小な対角項δＩを加えることで正定行列になるようにしている。
【０１０９】
優先度ｋのタスクが不等式制約であった場合、その上下限制約の中央値を先端目標値ベクトルの指令速度とする。つまり、先端指令速度は次の（数２３）で与えられる。
【０１１０】
【数２３】
【０１１１】
なお、（数２３）のｂｋは、実施例における先端速度ベクトル｛ｄｘｋ｝に対応する。
【０１１２】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【０１１３】
１０：コントローラ
１２Ｒ、１２Ｌ：脚
１４Ｒ、１４Ｌ：腕
１００：脚式ロボット
【技術分野】
【０００１】
本発明は、制御可能な関節の数ｎがロボット先端に与えられる先端目標値ベクトルの次数ｍよりも多い冗長ロボットの関節目標値ベクトルを決定する技術に関する。
【背景技術】
【０００２】
典型的な冗長ロボットは、手先の直交３軸各軸方向位置と各軸回りの回転角の合計６個の先端目標値が与えられる７関節マニピュレータである。ロボット先端に与えられる先端目標値ベクトルの次数ｍよりも制御可能な関節の数ｎが大きいロボットを冗長ロボットという。関節の数ｎと先端目標値ベクトルの次数ｍの差は冗長度と呼ばれる。
【０００３】
近年、人型ロボットの開発が盛んになり、冗長度が極めて大きいロボットを扱うケースが増えている。例えば、６関節の脚を２本と、６関節の腕を２本有するロボットで水を満たしたコップを運ぶ場合を想定する。このロボットは合計２４自由度を有する。このロボットには、安定して歩行するために、夫々の足先位置（脚リンク機構の先端）に対して位置と角度の６自由度の先端目標値ベクトルが与えられる。さらにコップの水をこぼさないためには、コップを把持した手（腕リンク機構の先端）に対して水平２軸周りの回転角の２自由度の先端目標値ベクトルが与えられる。手先の水平面内２軸周りの角度、即ちコップの角度が一定であれば、コップの位置と垂直軸周りの回転角は任意でよい。また、他方の腕は、歩行バランスを維持するという条件の元で従属的にその手先位置と姿勢が定まる。即ち、このロボットは、制御可能な２４自由度を有しながら、ロボット先端（足先と手先）に与えられる先端目標値ベクトルの次数が１４である冗長ロボットである。このロボットの冗長度は、２４（関節自由度）−１４（先端目標値ベクトルの次数）＝１０であり、７自由度マニピュレータよりもはるかに大きな冗長度を有している。なお、ロボットには、制御周期毎に異なる先端目標値ベクトルが与えられる。先端目標値ベクトルの時系列データは「目標軌道」と呼ばれることがある。先端目標値ベクトルをどのように生成するかは、本発明の主眼ではないので説明は省略する。
【０００４】
脚と腕を有する人型ロボットに典型的な問題（典型的ではあるが人型ロボットに限られるものではない）は、大きな冗長度を有することに加えて、先端目標値ベクトルが、夫々別の条件を満たさねばならない部分ベクトルの組み合わせとなっている点である。例えば上記のコップを運ぶロボットの場合、先端目標値ベクトルが、安定歩行という条件を満たすべき足先目標値ベクトルと、コップを水平に保つという条件を満たすべき手先目標値ベクトルの組み合わせになっている。
【０００５】
さらに問題を複雑にするのが、先端目標値ベクトルを達成できる関節目標値が求まらない状況が起こり得ることである。関節目標値が求まらない場合とはどのような場合であるかは後述する。そのような場合、次善の関節目標値を求める必要がある。例えば上記したコップ運びロボットの場合、コップの水がこぼれても、即ち、手先の水平が維持できずとも、安定した歩行を維持することが優先される。
【０００６】
このように、近年の多関節ロボット、典型的には人型ロボットでは、冗長自由度を含む関節目標値を合理的に決定するための技術が望まれている。さらには、先端目標値ベクトルを満足する関節目標値ベクトルが得られない場合、次善の先端目標値ベクトルを決める技術が望まれている。
【０００７】
上記課題に対して一つの解決策を与える技術が非特許文献１で提案されている。非特許文献１の技術は、本発明を説明する上で重要であるので、ここで概説する。
【０００８】
説明に先立って、本明細書で用いる技術用語、及びいくつかの記号を説明する。本明細書では、ベクトルと行列を扱う。ベクトルはかっこ書き｛・｝で表し、行列はかっこ書き［・］で表す。また上付き文字Ｔはベクトル又は行列の転置を表す。
【０００９】
さらに、説明を理解しやすくするために、先端の目標値が速度（角速度）で与えられ、関節目標値は関節速度（角速度）で与えられるケースを想定する。なお、ロボットの技術分野では並進速度と回転角速度を混在して使うことが頻繁にある。そのため、本明細書では、並進速度と角速度の両者を「速度」という用語で総称する。そのような呼称により、回転関節と直動関節を統一して扱うことができる。速度の単位を有する関節目標値ベクトルを｛ｄθ｝で表し、速度の単位を有する先端目標値ベクトルを｛ｄｘ｝で表す。関節の数をｎで表し、先端目標値ベクトルの次数をｍで表す。即ち、ベクトル｛ｄθ」はｎ次元であり、ベクトル｛ｄｘ｝はｍ次元である。ロボットの技術分野では、関節目標値ベクトル｛ｄθ｝と先端目標値ベクトル｛ｄｘ｝の間には、次の（数１）の関係があることは良く知られている。
【００１０】
【数１】
【００１１】
ここで、［Ｊ］はヤコビアンと呼ばれる行列である。ヤコビアン［Ｊ］の次数は、ｍ行ｎ列である。（数１）は手先速度が関節速度で表されることを意味する。手先の目標速度（即ち、先端目標値ベクトル）を実現する関節目標速度（即ち関節目標値ベクトル）を求めるには、ヤコビアン［Ｊ］の逆行列を求めればよい。冗長ロボットの場合、ヤコビアン［Ｊ］は、ｍ行ｎ列（ｍ＜ｎ）であるから非正方行列である。その場合、先端目標値ベクトル｛ｄｘ｝を実現する関節目標値ベクトル｛ｄθ｝は、次の（数２）で求めることができる。
【００１２】
【数２】
【００１３】
（数２）における［Ｊ＋］は、［Ｊ］の擬似逆行列を表し、｛ｋ｝はｎ次元の任意のベクトルである。また、｛ｋ｝＝｛０｝（全ての要素がゼロであるベクトル）の場合、（数２）の解は、二乗ノルムが最小の解となる。
【００１４】
先端目標値ベクトル｛ｄｘ｝が与えられたとき、［Ｊ＋］が求まれば、（数２）から関節目標値ベクトルを求めることができる。しかしながら、擬似逆行列［Ｊ＋］が常に求まるとは限らない。すなわち、（数２）を満たす［Ｊ＋］の解が求まらない場合が、与えられた先端目標値ベクトル｛ｄｘ｝を実現できる関節目標値｛ｄθ｝が求まらない場合に相当する。なお、（数２）に基づいて関節角（｛ｄθ｝は微小時間Δｔ当たりの回転角に等しい）を逐次的に制御する方法は分解速度制御（或いは分解運動速度制御）として良く知られている（例えば、「ロボット制御基礎論」、吉川恒夫著、コロナ社、ISBN:978-4-339-04130-9）。
【００１５】
｛ｄｘ｝は、先端目標値ベクトルであり、ロボットに行わせたい作業から定められる。先のコップ運びロボットの場合、｛ｄｘ｝は、安定して歩行するための各足先６個の速度と手先の２方向速度の合計１４個の要素を有するベクトルとなる。従って冗長ロボットの問題は、与えられる先端目標速度ベクトル｛ｄｘ｝を実現する目標関節速度ベクトル｛ｄθ｝を求めることであり、即ち、（数２）を満足する解を求めることである。
【００１６】
しかし、実際のロボットにおいては、関節目標値｛ｄθ｝はどんな値でも取り得るわけではない。例えば、ロボットのモータの性能によって出せる最大速度が存在する。また、関節には通常可動範囲が存在するが、可動範囲外へ向かうような関節目標値をとることはできない。つまり、（数１）の等式で示される条件に加えて、次のような、関節目標値の最大値と最小値で表される不等式の制約条件を考慮する必要がある。
【００１７】
【数３】
【００１８】
そこで、非特許文献１においては（数３）の不等式条件も考慮したアルゴリズムが提案されている。非特許文献１に開示された関節目標速度ベクトル（関節目標値ベクトル）の決定アルゴリズムを説明する。まず、目的（タスク）別に、先端目標値ベクトル｛ｄｘ｝を複数の部分ベクトル｛ｄｘｉ｝に分割する。ここで、下付き文字「ｉ」は各部分ベクトルを区別するための指標を表す。「目的（タスク）」とは、ロボットに実行させたい作業の一つひとつを意味する。そして、複数の部分ベクトル｛ｄｘｉ｝の夫々に対して、ロボットが達成すべき順に優先度を付与する。非特許文献１の技術は、優先度の高い順に、できるだけ多くの部分ベクトルを同時に実現できる関節目標値ベクトルを求めるアルゴリズムである。
【００１９】
先のコップ運びロボットでは、安定に歩行することと、コップの水をこぼさないことの２つの目的（タスク）がある。先端目標値ベクトルの観点からみれば、「目的（タスク）」とは、先端目的値ベクトルの要素群のうち、同時に満たさねば意味を成さない要素の集まりに相当する。先のコップ運びロボットの場合、２つの目的（タスク）に分解できる。夫々の目的（タスク）に対応する部分ベクトルは、両脚足先の目標速度を指定する１２次元の速度ベクトル｛ｄｘ１｝と、手先の水平面内２軸周りの目標速度を指定する２次元の速度ベクトル｛ｄｘ２｝である。安定に歩行するための足先目標速度を指定する部分ベクトル｛ｄｘ１｝に、手先の水平面内２軸周りの目標速度を指定する部分ベクトル｛ｄｘ２｝よりも高い優先度が与えられる。非特許文献１の技術は、部分ベクトル｛ｄｘ１｝と｛ｄｘ２｝の双方を同時に実現する関節目標速度｛ｄθ｝が存在しない場合、優先度の高い部分ベクトル｛ｄｘ１｝を実現する関節目標速度｛ｄθ｝を決定する。
【００２０】
説明を簡単にするために、複数の部分ベクトルを｛ｄｘｉ｝で表し、各部分ベクトルを区別する指標ｉが小さいほど優先度が高いことを表すものとする。同時に符号「ｉ」は、目的（タスク）を区別するための指標を表す。即ち、第１番目の部分ベクトル｛ｄｘｉ｝に対応する目的（タスク）を第１番目のタスクと呼ぶことにする。なお、下付き文字「ｉ」の最大値は、ロボットが同時に実行することが要求されているタスクの数に相当する。
【００２１】
次に、夫々の部分ベクトル｛ｄｘｉ｝に対して、部分ベクトルと関節目標速度ベクトル｛ｄθ｝の間の線形関係式｛ｄｘｉ｝＝［Ｊｉ］｛ｄθ｝を求める。ここで、［Ｊｉ］は、部分ベクトル｛ｄｘｉ｝と関節目標速度｛ｄθ｝の関係を示すヤコビアンであり、ロボットの運動学的特性から定まる。なお、［Ｊｉ］は、｛ｄｘｉ｝の次数と同数の行と、関節数ｎと同数の列を有する行列となる。
【００２２】
（数３）を満たしつつ、できるだけ高い優先度のタスクの部分ベクトル｛ｄｘｉ｝を同時に実現できる関節目標速度｛ｄθ｝を求めることが、非特許文献１の目的である。非特許文献１の技術は、優先度の高い順にできるだけ上位までのタスクを同時に満たすことができる解を、次の手順で決定する。
【００２３】
付与された優先度から一の優先度を選択し（選択された優先度をｋ、即ち、ｉ＝ｋとする）、選択された優先度ｋに対応する部分ベクトル｛ｄｘｋ｝について、次の（数４）、（数５）で表される凸二次計画問題を解く。
【００２４】
【数４】
【数５】
【００２５】
ここでいう「凸二次計画問題」とは、（数５）の拘束条件の元で目的関数Ｃ２を最小化する｛ｄθ｝を凸二次計画法に基づいて求める問題を意味する。なお、所定の拘束条件の元で凸二次計画法に基づいて所定の目的関数を最小にする凸二次計画問題は、数理物理の技術分野では良く知られているので詳しい説明は省略する。
【００２６】
ここで、目的関数Ｃ２を最小にする｛ｄθ｝は、（数５）の拘束条件を満足する。（数５）の第１式は、選択された優先度ｋよりも高い優先度（ｉ＜ｋ）の全ての部分ベクトル｛ｄｘｉ｝が、線形関係式｛ｄｘｉ｝＝［Ｊｉ］｛ｄθ｝を満たすことを拘束条件とすることを表している。即ち、（数５）の第１式の拘束条件を課すことによって、（数４）の凸二次計画問題の解｛ｄθ｝は、ｋより優先度の高い部分ベクトル｛ｄｘｉ｝を同時に満足しつつ、最上位の優先度ｋの部分ベクトル｛ｄｘｋ｝の二乗誤差をできるだけ小さくするような解となる。
【００２７】
また、（数５）の第２式は、各関節に課せられた動作制限条件に関する拘束条件である。現実のロボットでは、関節の動作に物理的な制約条件が課される。（数４）が関節速度｛ｄθ｝の解を求める問題であるので、動作制限条件も、関節速度の次元で表される。すなわち、ここでは、目的関数Ｃ２を最小にする関節目標速度ベクトル｛ｄθ｝は｛ｄθｍｉｎ｝以上｛ｄθｍａｘ｝以下であることが要求されている。凸二次計画問題は、（数５）の拘束条件を満足することを要求するから、その解｛ｄθ｝は拘束条件で与えられる動作制限条件を満足する。
【００２８】
非特許文献１の技術をまとめると次の通りである。（数４）と（数５）で表される凸二次計画問題は、各関節の目標速度が動作制限条件内であり、かつ、ｋより高い優先度の部分ベクトルの実現を保証するという条件の下で、優先度ｋの部分ベクトルをできる限り実現することができる関節目標速度ベクトル｛ｄθ｝を求める問題である。即ち、凸二次計画問題の解｛ｄθ｝は、動作制限条件を満足した上で、第ｋ番目のタスクの誤差を最小化する（もしくは、第ｋ番目のタスクをできるだけ満足する）。以上のアルゴリズムは、非特許文献１、及び、非特許文献２に詳しいのでそちらを参照されたい。
【００２９】
最低の優先度が与えられた部分ベクトル｛ｄｘｉ｝について、（数４）と（数５）で与えられる凸二次計画問題の解が求まれば、その解が、全ての部分ベクトル｛ｄｘｉ｝を同時に達成できる関節目標速度｛ｄθ｝に相当する。即ち、全てのタスクを同時に実現する関節目標値ベクトルが求まることになる。しかしながら、場合によってはそのような解が存在しないこともあり得る。そこで、非特許文献１では、（数４）と（数５）で与えられる凸二次計画問題を複数の優先度に対して実行し、最も高い優先度に相当する解を最終的に関節目標速度ベクトルとして採用する。具体的には、最も高い優先度から低位の優先度へ向けて順に（数４）と（数５）の凸二次計画問題を解く。解が得られなかった場合、その１つ高位の優先度に相当する解を関節目標速度ベクトルとして採用する。以上のアルゴリズムにより、最上位から順に、最も多くのタスクを同時に実現する関節目標速度ベクトル（関節目標値ベクトル）が決定できる。
【先行技術文献】
【非特許文献】
【００３０】
【非特許文献１】Oussama Kanoun, Florent Lamiraux, Pierre-Brice Wieber, Fumio Kanehiro, Eiichi Yoshida, and Jean-Paul Laumond, “Prioritizing Linear equality and inequality systems: application to local motion planning for redundant robots”, Proc. of IEEE International Conference on Robotics and Automation, pp. 2939-2944, 2009
【非特許文献２】Bernard Faverjon, Pierre Tournassoud, “A local Based Approach for Path Planning of Manipulators With a High Number of Degrees of Freedom”, In Proc. of IEEE International Conference on Robotics and Automation, pp. 1152-1159, 1987
【発明の概要】
【発明が解決しようとする課題】
【００３１】
非特許文献１の手法は、冗長ロボットの関節目標値ベクトルの決定に極めて有効な手法であるが、凸二次計画法は計算負荷が高いという課題がある。上記したように非特許文献１の技術は、複数のタスクの夫々について凸二次計画法を解かねばならない。タスクの数が多くなるほど、即ち、部分ベクトルの数が多くなるほど、解かねばならない凸二次計画問題の数も多くなる。他方、冗長ロボットは、動作中に時々刻々変化する状況に応じて関節目標値ベクトルを決めねばならない。典型的にはサンプリング時間毎に関節目標値ベクトルを決定しなければならない。そのため、関節目標値ベクトルを求めるための計算負荷は小さいことが望ましい。本発明は、非特許文献１が提唱する関節目標値ベクトルの決定アルゴリズムを改良するものであり、その計算負荷を低減することを目的とする。
【課題を解決するための手段】
【００３２】
非特許文献１に開示されたアルゴリズムでは、凸二次計画問題を複数回解いている。しかしながら、最終的に関節目標値ベクトルとして用いる解は、解が存在するタスクのうち、最も高い優先度についての凸二次計画問題の解である。それ以外の優先度のタスクについては、凸二次計画問題を解くがその解は使われない。発明者らはこの点に着目した。即ち、優先度が定められた複数のタスクのそれぞれに対しては、凸二次計画問題の具体的な解を得ることが目的ではなく、凸二次計画問題の解が存在するか否かが判定できればよい。凸二次計画問題の解が存在することが判明したタスクのなかから、最も優先度が高いタスクの部分ベクトルに対して、凸二次計画問題の具体的な解を求めればよい。発明者らは、「拘束条件が実行可能領域であるか否か」を確認することで、同じ拘束条件の下で凸二次計画問題の解の存在を判定でき、計算負荷を低減することを思いついた。この、「拘束条件が実行可能領域である」とは、（数５）のうちの等式条件による連立方程式の解が、（数５）の上下限制約内に存在する、ということと等価である。もし「拘束条件が実行可能領域である」場合、凸二次計画問題は必ず解を持つ。なぜなら、凸二次関数の目的関数は、その実行可能領域内の任意の点について計算でき、そのうちの最小値を持つものが解となるからである。またもし、「拘束条件が実行可能領域でない」場合、凸二次計画問題は解を持たない。拘束条件を満たす実行可能な点が存在しないからである。この「拘束条件が実行可能領域である」かどうかを判定するには、線形計画問題の効率的な解法の一つである「２段階シンプレックス法」の一部を用いることができる。なお、「線形計画問題」とは、「凸二次計画問題」と同様の形で設定された拘束条件の下、与えられた線形な目的関数を最小にする解を線形計画法に基づいて解くことを意味する。「２段階シンプレックス法（単体法）」とは、まず線形計画問題の拘束条件を満たす解（「実行可能基底解」と呼ばれる）を１つ発見し（第１段階）、つぎにその「実行可能基底解」から出発して目的関数を最小化する解を探索する（第２段階）手法である。手法の詳細については、後段もしくは文献（例えば、茨木俊秀、福島雅夫、「FORTRAN77最適化プログラミング」、岩波書店、１９９１）を参照されたい。つまり、この２段階シンプレックス法の第１段階で求められる「実行可能基底解」が存在すれば、当然「拘束条件は実行可能である」と言える。また、「実行可能基底解」が存在しなければ、「拘束条件は実行可能ではない」と言える。
【００３３】
「拘束条件の実行可能領域の存在を確認する」操作は、同じ拘束条件の下での凸二次計画問題を解く操作よりも計算負荷がはるかに小さい。従って、ある優先度までのタスク（複数の部分ベクトル）に対してまず実行可能領域の存在を確認することで解の存在を判定し、解が存在する優先度までのタスクのうち、最も優先度が高いタスクについて凸二次計画問題として扱って解を求めれば、上記した非特許文献１のアルゴリズムと同じ結果が得られる。即ち、非特許文献１のアルゴリズムと同じ結果を少ない計算負荷で求めることができる。
【００３４】
非特許文献１のアルゴリズムと本明細書が開示する新規なアルゴリズムの相違を明確にするために、まず、非特許文献１のアルゴリズムをまとめる。なお、以下では、一般的な表現とするために、先に用いた先端目標速度ベクトル｛ｄｘ｝、関節目標速度ベクトル｛ｄθ｝、及び、ヤコビアン［Ｊ］を、先端目標値ベクトル｛ｄｘ｝、関節目標値ベクトル｛ｄθ｝、及び、行列［Ａ］と表現する。
【００３５】
ステップ１：ｍ次元の先端目標値ベクトル｛ｄｘ｝を、複数の部分ベクトル｛ｄｘｉ｝に分解する。ただし、下付きｉは各部分ベクトルを区別する指標である。そして、複数の部分ベクトル｛ｄｘｉ｝の夫々に対して、ロボットが達成すべき順に優先度を付与する。ここで、各部分ベクトルを区別する指標ｉが小さいほど優先度が高いことを表すとする。
【００３６】
ステップ２：夫々の部分ベクトル｛ｄｘｉ｝に対して、部分ベクトルと関節目標値ベクトル｛θ｝の間の線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝を求める。ここで、［Ａｉ］は、部分ベクトルごとにロボットの運動学的特性から定まる行列であり、｛ｄｘｉ｝の次数と同数の行と、関節の数ｎと同数の列を有する行列を表す。
【００３７】
ステップ３：複数の優先度から一の優先度ｋ（ｉ＝ｋ）を選択し、選択された優先度ｋの部分ベクトル｛ｄｘｋ｝に対して、次の凸二次計画問題の解を求める。凸二次計画問題：選択された優先度ｋよりも高い優先度を有する全ての部分ベクトル｛ｄｘｉ｝が線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝（ただしｉ＜ｋ）、及び、不等式関係式｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝（ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限）を満足することを拘束条件として、目的関数Ｃ２＝（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）Ｔ（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）を最小にする凸二次計画法の解｛ｄθ｝を求める。
【００３８】
ステップ４：ステップ３を優先度が異なる複数の部分ベクトル｛ｄｘｉ｝に対して実行する。そして、凸二次計画問題の解が求まった部分ベクトルのうち最も高い優先度を有する部分ベクトルに対する解をロボット関節目標値として決定する。
【００３９】
本発明のアルゴリズムは、上記ステップ３とステップ４を次のステップ３と４に置き換える。
【００４０】
ステップ３：複数の優先度から一の優先度ｋ（ｉ＝ｋ）を選択し、選択された優先度ｋの部分ベクトル｛ｄｘｋ｝に対して、その拘束条件の実行可能領域の存在を判定する。具体的には、線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝（ただし、ｉ＜ｋ）、及び、不等式関係式｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝（ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限）の実行可能基底解を、線形計画法の一種である２段階シンプレックス法の第１段階の操作によって求める。
【００４１】
ステップ４：ステップ３を優先度が異なる複数の部分ベクトル｛ｄｘｉ｝に対して実行する。そして、与えられた拘束条件の実行可能領域が存在すると判定された部分ベクトルのうち最も高い優先度を有する部分ベクトルについて拘束条件とし、目的関数｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝を最小にする凸二次計画法の解｛ｄθ｝を求める。
【００４２】
ステップ５：ステップ４で求められた解｛ｄθ｝をロボット関節目標値ベクトルに設定する。
【００４３】
上記のアルゴルムによれば、概ね、「（凸二次計画法の計算負荷−実行可能領域の存在確認の計算負荷）×従来のアルゴリズムにおいて凸二次計画問題を実施した回数」で評価される計算負荷が低減できる。また、典型的な事例としては、｛ｄｘ｝はロボット先端の目標速度ベクトルであり、｛ｄθ｝は関節目標速度ベクトルであり、［Ａｉ］はロボットのヤコビアンである。
【００４４】
本明細書が開示する技術は、上記のアルゴリズムを実行する冗長ロボット制御装置に具現化することも好適である。なお、そのような制御装置は、上記ステップ１の複数の部分ベクトル｛ｄｘｉ｝の夫々について、線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝と優先度を記憶している。そして制御装置は、ロボットの動作中にリアルタイムに、即ち、予め定められた周期毎に、上記ステップ３から５を実行し、得られたロボット関節目標値ベクトルに追従するように各関節を制御する。そのような制御装置を使うことによって、冗長ロボットを効率的に制御することができる。
【図面の簡単な説明】
【００４５】
【図１】脚式ロボットの模式図である。
【図２】目標関節角ベクトル決定処理のフローチャート図である。
【実施例】
【００４６】
以下、図面を参照しながら、冗長ロボットの関節目標値決定プロセスを具体的に説明する。ここでは、前述した例と同じ、冗長自由度を有する人型ロボットに水の入ったコップを運ばせる作業を想定し、両方の足先とコップを保持した手先が目標軌道に追従するための関節目標値を決定するプロセスを説明する。
【００４７】
図１に、脚式ロボット１００の模式図を示す。以下、脚式ロボット１００を単にロボット１００と称する。ロボット１００は、左右の脚１２Ｒ、１２Ｌ、左右の腕１４Ｌ、１４Ｒを有する人型ロボットである。頭部にはコントローラ１０が内蔵されている。夫々の脚は、６個の関節を備える多リンク多関節構造を有する。即ち、夫々の脚は６自由度を有する。夫々の腕も、６個の関節を備える多リンク多関節構造を有する。即ち、夫々の腕も６自由度を有する。各関節にはリンクを揺動させるモータが内蔵されており、コントローラ１０が各モータを制御する。以上の通り、ロボット１００は、合計で２４自由度を有する。
【００４８】
ロボット１００は、脚の各関節のモータを適宜に制御することによって歩行することができる。歩行動作のための脚先や重心の軌道の生成アルゴリズムは、本発明の主眼ではないので説明は省略する。また、ロボット１００は、腕の各関節のモータを適宜に制御することによって、物体を把持・移動することができる。
【００４９】
今、ロボット１００に与えられた作業は、水をこぼさないように腕でコップＰを保持しながら所定の経路に沿って歩行することである。この作業は、「安定に歩行する」というタスク（第１タスク）と、「水をこぼさないようにコップを保持する」というタスク（第２タスク）に分解できる。
【００５０】
第１タスクは、右脚１２Ｒの足先の軌道と、左脚１２Ｌの足先の軌道で表される。なお、安定に歩行するための軌道を生成するアルゴリズムについては説明を省略する。ここでは、安定に歩行するための左右の足先軌道が与えられているとする。即ち、左右の足先の位置ベクトルの時系列データは与えられる。ここで、「位置」には、いわゆるロール角、ピッチ角、ヨー角を含む。一方の足先の位置ベクトルは６個の変数要素（３軸各軸方向の位置と３軸各軸周りの角度）を含む。従って、左右の足先で合計１２要素の位置ベクトルとなる。ここでは、安定に歩行するための足先軌道は、各時刻における足先の速度ベクトルの時系列データで表されていると仮定する。即ち、「安定して歩行する」という第１タスクは、ロボット１００の両足先が追従すべき、１２要素を有する足先目標速度ベクトル（先端目標値ベクトル）で表現される。いま、第１タスクの足先目標速度ベクトルを｛ｄｘ１｝で表す。｛ｄｘ１｝は１２次元のベクトルである。なお、より正確には、「安定して歩行する」タスクにはここで述べた両足先位置姿勢の第１タスクに加えて、ロボットの重心速度や角運動量に対するタスクも加える必要があることが知られているが、ここでは例を簡単にするために考慮しない（必要であればそれらのタスクも加えて考慮できる）。
【００５１】
同様に、第２タスクは、コップＰを保持する手先のピッチ軸周りの速度（角速度）とロール軸周りの速度（角速度）の時系列データで表される。即ち、第２タスクは手先のロール軸周りの速度とピッチ軸周りの速度の合計２要素の手先目標速度ベクトル（先端目標値ベクトル）で表される。なお、コップＰの水をこぼさないための手先目標速度ベクトルは、手先が水平である場合は速度＝ゼロであり、手先が水平でない場合は水平（ロール角＝ゼロ、ピッチ角＝ゼロ）に保とうとする方向の速度が（その時点の手先姿勢に応じてフィードバック的に）与えられる。第２タスクの目標速度ベクトルを｛ｄｘ２｝で表す。｛ｄｘ２｝は２次元のベクトルである。
【００５２】
コントローラ１０の使命は、２４個の関節を使って、両足先の速度（１２次元）と、コップＰを把持した腕の手先の速度（２次元）を、合計１４次元の先端目標速度ベクトル（｛ｄｘ１｝，｛ｄｘ２｝）に追従させることである。別言すると、コントローラ１０の使命は、制御可能な関節の数ｎ＝２４がロボット先端に与えられる先端目標速度ベクトル（先端目標値ベクトル）の次数ｍ＝１４よりも多い冗長ロボット１００の関節目標速度ベクトル（関節目標値ベクトル）を決定することである。ここで、先端目標速度ベクトルは、第１タスクに対応する１２次元の先端目標速度ベクトル｛ｄｘ１｝と、第２タスクに対応する２次元の先端目標速度ベクトル｛ｄｘ２｝に分解される。
【００５３】
コントローラ１０が実行する処理を説明する前に、コントローラ１０が予め記憶しておくべき数式について説明する。
【００５４】
まず、２つのタスクの夫々に優先度を付与する。コップの水をこぼさないことより、安定に歩行できることが重要であるから、第１タスクの優先度を最高位に定め、第２タスクの優先度を次の順位に定める。即ち、第ｉタスクについて、タスクを区別する指標ｉが小さいほど高位のタスクであることを示す。
【００５５】
今、２４個の関節の速度をベクトル｛ｄθ｝（｛ｄθ｝は２４次元ベクトル）で表す。ロボットの先端（足先、手先）の速度と関節角速度にはヤコビアン［Ｊ］を使った関係が成立する。本実施例の場合、次の（数６）が成立する。
【００５６】
【数６】
【００５７】
［Ｊ１］は、関節目標速度ベクトル｛ｄθ｝と両方の足先の速度ベクトル｛ｄｘ１｝との間の線形関係式であり、［Ｊ１］はロボット１００の幾何学的構成から定まるヤコビアンである。［Ｊ２］は、関節速度ベクトル｛ｄθ｝とコップを保持した腕の手先の速度ベクトル｛ｄｘ２｝との間の線形関係式であり、［Ｊ２］はロボット１００の幾何学的構成から定まるヤコビアンである。（数６）の第１式を、第１タスクの線形関係式と称し、（数６）の第２式を第２タスクの線形関係式と称する。コントローラ１０は、それら２つの線形関係式を記憶している。
【００５８】
また、関節ベクトル｛ｄθ｝は、実機ロボットにおいては制約が存在する。関節ベクトルの制約を｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝のように不等式関係を設定する。ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限速度制約である。コントローラ１０は、この不等式関係式も記憶している。なお、ロボットの関節角度可動範囲及び関節角加速度の制約をこの関節速度の不等式関係に変形する方法については後述する。
【００５９】
コントローラ１０が目標関節速度ベクトルを決定する処理のフローチャートを図２に示す。概説すると、コントローラ１０は、上位の優先度から低位の優先度へ向けて、できるだけ多くのタスクを同時に実現できる関節目標速度｛ｄθ｝を決定する。以下、図２のフローチャートの各ステップについて説明する。なお、以下の説明では、議論を一般化するためにタスクの数をｔ個とする。即ち、（数６）で表される線形関係式はタスクの数ｔに等しい数だけ存在する。
【００６０】
初期化（ステップＳ１２）
図２における記号「ｉ」は、タスクを識別する指標であるとともに、タスクの優先度を表す指標でもある。記号「ｉ」が小さいほど、優先度が高いことを示す。コントローラ１０は、優先度の高い方から順にステップＳ１３、Ｓ１４、Ｓ１５、及びＳ１６の処理を繰り返す。そのために、変数ｉを「１」で初期化する。
【００６１】
関節角実行可能領域の存在のチェック（ステップＳ１４、Ｓ１５）
ｉ番目のタスクまでの等式関係（数６）および不等式関係｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝を連立させた条件は（数７）となる。この（数７）のすべての等式および不等式条件を満たす｛ｄθ｝の集合を、「実行可能領域」と呼ぶ。（数７）には、実行可能領域が存在する場合と、存在しない場合の２つがありうる。（数７）の実行可能領域が存在する場合、（数７）を条件とした凸二次計画問題の解は、必ず存在する。また、（数７）の実行可能領域が存在しない場合、当然ながら（数７）を条件とした凸二次計画問題の解は存在しない。つまり、凸二次計画問題の解自体を求めなくても、その解の存在を判定することができる。
【００６２】
【数７】
【００６３】
なお、本実施例では、（数７）の実行可能領域が存在するか否かを判定するために、線形計画問題の解法の一つである、２段階シンプレックス法の第１段階にあたる操作を行う。２段階のシンプレックス法は既知であるので説明は省略する。図２のステップＳ１５の分岐判定は、（数７）の実行可能領域が存在する場合に「ＹＥＳ」となり、（数７）の実行可能領域が存在しない場合に「ＮＯ」となる。
【００６４】
コントローラ１０の処理は、（数７）の実行可能領域が存在する場合、ステップＳ１６に進む。他方、（数７）の実行可能領域が存在しない場合、ステップＳ２０に進む。
【００６５】
条件を満たす優先度ｋの特定（ステップＳ１３、Ｓ１５、Ｓ２０）：
コントローラ１０は、上記した（数７）の実行可能領域の存在のチェックを、優先度が最も高いタスクから低位の優先度のタスクへと順次行う（ステップＳ１３：ＮＯ、ステップＳ１４）。優先度が第ｉ番目のタスクにおいて（数７）の実行可能領域が存在しなかった場合（ステップＳ１５：ＮＯ）、第ｉ−１番目のタスクが、自身よりも高位の優先度を有する全てのタスクを満足する｛ｄθ｝が存在するタスクのうち、最も優先度が高いタスクとなる。最も低位のタスクにおいても（数７）の実行可能領域が存在する場合（ステップＳ１５：ＹＥＳ）、全てのタスクを満足する｛ｄθ｝が存在することが判明する。ステップＳ２０は、自身を含めないで自身よりも高位の全てのタスクを満足する｛ｄθ｝が存在するタスクの中で最も優先度が低いタスクの番号を変数ｋに代入する処理である。即ち、ステップＳ２０において、第ｋ番目のタスクが、自身を含めないで自身よりも高位の優先度を有する全てのタスクを満足する｛ｄθ｝が存在するタスクの中で最も優先度が低いタスクに相当する。なお、第ｋ番目のタスクは、ｉ＜ｋのタスクが実行可能であるようなもののうち、最も優先度の低いものが選ばれることに留意されたい。
【００６６】
凸二次計画問題（ステップＳ２１）：
コントローラ１０は、第ｋ番目のタスクをできるだけ満足する、タスクの誤差を最小化する、関節目標速度ベクトル｛ｄθ｝を求める。コントローラ１０は、（数７）の制約条件の元で、次の（数８）で与えられる目的関数Ｃ２を最小化する凸二次計画法の解を求める。なお、上述したように、第ｋ番目のタスクについては凸二次計画問題の解が存在することは保証されている。
【００６７】
【数８】
【００６８】
コントローラ１０は、（数８）で与えられる目的関数Ｃ２を最小にする凸二次計画法の解｛ｄθ｝を、関節目標速度ベクトル（関節目標値ベクトル）に設定する（ステップＳ２２）。そしてコントローラ１０は、関節速度の現在値が関節目標速度ベクトルに一致するように関節のモータを制御する。コントローラ１０は、図２の処理を、サーボ周期毎に実行する。ロボットのサーボ周期は通常数msecである。
【００６９】
（数８）の目的関数Ｃ２を最小にする凸二次計画法の解｛ｄθ｝は、（数７）で与えられる制約条件を満足するから、第ｋ番目のタスクよりも優先度が高いタスクは全て満たす。こうしてコントローラ１０は、優先度が与えられた複数のタスクの中から、優先度が高い順に最も多くのタスクを満足する解を関節目標値ベクトルとして決定する。上記のアルゴリズムは、上位からできるだけ多くのタスクを満足する関節目標値ベクトルを決定するのに、凸二次計画問題を１回だけ解けばよい。従来は、凸二次計画問題を複数回解く必要があり、最大ではタスクの数と同数回も解く必要があった。本実施例で説明したアルゴリズムは、従来のアルゴリズムよりも顕著に計算負荷を低減することができる。なお、凸二次計画問題を高速に解くには、例えばGoldfarb-Idnani法（有効制約法：Active Set Method）を使うとよい。
【００７０】
図２におけるステップＳ１３からＳ１６のループを繰り返す処理が、特許請求の範囲の記載おけるステップ３の一例に相当する。図２におけるステップＳ２１の処理が、特許請求の範囲の記載におけるステップ４の一例に相当する。図２のステップＳ２２の処理が、特許請求の範囲の記載におけるステップ５の一例に相当する。
【００７１】
上記した実施例の留意点について述べる。図２のステップＳ１３からＳ１６のループは、必ずしもｉ＝１から始める必要はない。例えば、第ｒ番目のタスクについてはそれより高位の優先度のタスクを満足することが分かっている場合は、第ｒ＋１番目のタスクから始めればよい。
【００７２】
（数７）の第２式は、関節目標速度ベクトル｛ｄθ｝が制限範囲内となることを保証するための制約条件であった。速度の代わりに（或いは速度とともに）、加速度（角加速度）が関節の動作制限として加えられることがある。上記したアルゴリズムでは、加速度の制約条件を関節速度の制約条件に変形することもできる。以下、関節加速度の制限範囲を考慮する手法について説明する。
【００７３】
現在の関節速度ベクトルを｛ｄθ０｝とすると、Δｔ秒後の関節速度ベクトル｛ｄθ１｝は現在の関節加速度ベクトル｛ａθ０｝を使って次の（数９）で与えられる。
【００７４】
【数９】
【００７５】
（数９）に基づけば、関節加速度についての制限条件は、Δｔ秒後の関節速度ベクトル｛ｄθ１｝に対する制限として、次の（数１０）で与えることができる。
【００７６】
【数１０】
【００７７】
（数７）の制約条件に（数１０）を加えることで、加速度に関する制限条件を（数７）の実行可能領域の存在判定、及び（数８）の凸二次計画問題に加えることができる。
【００７８】
次に、関節角度の制限について説明する。なお、以下では説明を簡単にするため、ベクトル量のかわりにスカラ量で説明する。即ち、ベクトルを表す｛・｝を外して説明する。今、ある関節の角度の上限値をθｍａｘ、下限値をθｍｉｎで表す。この関節が速度ｄθｍｉｎとｄθｍａｘのどちらかの速度で動いているとする。このとき、最大速度で減速し始めたと仮定して、角度上下限値において速度をゼロにできる猶予角度ψｍｉｎ、ψｍａｘが、以下の（数１１）で定めることができる。
【００７９】
【数１１】
【００８０】
現在の関節角度θがψｍａｘ以上である場合、これらの猶予角度を用いて関節速度の上限値を次の（数１２）で与えると、関節角度の上限値を超えないように動作させることができる。なお、θ＝θｍａｘで速度がゼロとなることに留意されたい。
【００８１】
【数１２】
【００８２】
同様に、下限についてもθがψｍｉｎ以下である場合に関節速度の下限値を次の（数１３）で与えればよい。
【００８３】
【数１３】
【００８４】
上記説明したように、関節の角度制限、加速度制限も、速度制限の形式で制約条件に盛り込むこができる。
【００８５】
次に、タスク自体が不等式で与えられる場合であっても、上記したアルゴリズムを適用できることを説明する。繰り返すが、先の実施例では、第ｉ番目のタスクは、次の（数１４）で与えられた。
【００８６】
【数１４】
【００８７】
ここでは、先端目標値ベクトル｛ｄｘｉ｝が｛ｄｘｉ｝ｍｉｎから｛ｄｘｉ｝ｍａｘの範囲にあればよい場合を想定する。即ち、第１タスクが次の（数１５）で与えられるケースを想定する。
【００８８】
【数１５】
【００８９】
｛ｄｘｉ｝ｍｉｎと｛ｄｘｉ｝ｍａｘは夫々先端目標速度ベクトルの下限値と上限値の制約に相当する。［Ｊｉ］は上記実施例と同様にヤコビアンである。上下限のいずれか一方しか存在しない場合は、他方の制約の大きさを極端に大きくすればよい。（数１５）を不等式条件と称する。
【００９０】
（数１５）の不等式条件を採用する場合、先に示した凸二次計画問題の形式（標準形と称する）とは適合しない。凸二次計画問題の標準形は、制約条件として等式条件と変数の上下限の形であるとされているからである。
【００９１】
しかし、スラック変数と呼ばれる変数を導入することで、不等式条件を等式条件に変形することができる。つまり、不等式条件式（数１５）において、次の（数１６）で与えられるスラック変数｛ｄθｓ｝を導入する。
【００９２】
【数１６】
【００９３】
上記したスラック変数｛ｄθｓ｝を導入することで、（数１５）で与えられるタスクｉは次の（数１７）のとおり、等式条件と各変数の上下限に変形することができる。
【００９４】
【数１７】
【００９５】
（数１７）の形式は、実行可能領域の存在判定と凸二次計画法の双方に適用できる。以上のようにしてタスクとして不等式の制約を考慮する方法は、多自由度マニピュレータで障害物や自己との干渉回避を行うために提案された。その詳細は、Bernard Faverjon and Pierre Tournassoud, “A local based approach for path planning of manipulators with a high number of degrees of freedom”, In ICRA, pp.1152-1159, 1987 に詳しい。また、そのような手法を人間型ロボットに適用した例が、Oussama Kanoun, Florent Lamirauxm, Pierre-Brice Wieber, Fumio Kanehiro, Eiichi Yoshida, and Jean-Paul Laumond, “Prioritizing linear equality and inequality systems: application to local motion planning for redundant robots”, In Proc. of the IEEE International Conference on Robotics and Automation, pp. 2939-2944, 2009 に開示されている。
【００９６】
凸二次計画問題と実行可能領域についての補足事項を説明する。なお、以下では、ベクトルを表す記号｛｝、行列を表す記号［］は省略する。以下では、小文字はベクトルを表し、大文字は行列を表す。次の（数２５）で表される凸二次計画問題を考える。
【００９７】
【数１８】
【００９８】
（数１８）の凸二次計画問題の解の存在を知るには、実際に最小値を求める必要はない。もし（数１８）の第２式以降で表された等式及び不等式に実行可能領域が存在しない場合、実行可能な解が無いため、当然ながら最小値は存在しない。また実行可能領域が存在すれば、領域内のそれぞれの点について（数１８）の第１式の目的関数値が計算でき、どこかに最小値を見つけられることは明らかである。この（数１８）の条件に実行可能領域が存在するか否かは、同一の条件を設定した線形計画問題において、その効率的な解法の一つである、２段階シンプレックス法の第１段階を行うことで判定できる（例えば、茨木俊秀、福島雅夫、「FORTRAN77最適化プログラミング」、岩波書店、１９９１）。
【００９９】
二段階シンプレックス法では、第一段階目として、まず初期解として実行可能基底解を一つ見つけ出す作業を行う。例えば、次の（数１９）で与えられる線形計画問題（ＬＰ−Ｉ）を想定する。
【０１００】
【数１９】
【０１０１】
（数１９）の線形計画問題（ＬＰ−Ｉ）について、人為変数ｘｎ＋ｉ＊（ｉ＝１・・・ｍ）を導入し、（数２０）で与えられる新たな線形計画問題（ＬＰ−II）を考える。
【０１０２】
【数２０】
【０１０３】
この線形計画問題（ＬＰ−II）は通常のシンプレックス表の操作で最小解を求めることができる。このとき、もとの線形計画問題（ＬＰ−Ｉ）が解を持つためには線形計画問題（ＬＰ−II）の解のうち、人為変数ｘｎ＋ｉ＊（ｉ＝１・・・ｍ）が全てゼロとならなければならないため、これを確かめればよい。つまり、解の存在判定は線形計画問題の解法自体よりもさらに容易に行うことができる。上記の手法は、実施例で示した、複数のタスクをできるだけ満たす関節目標値ベクトル決定手法に適用することができる。解の存在判定に用いる計算は、条件の複雑さにもよるが凸二次計画問題の求解に比べれば小さい。そして、タスクの数が増えても凸二次計画問題の求解は１回で済むため、大幅な計算量の縮小が期待できる。
【０１０４】
次に凸二次計画問題の設定方法についての補足事項を説明する。優先度ｋのタスクが等式制約であった場合、その二乗誤差は、次の（数２１）で与えられる。
【０１０５】
【数２１】
【０１０６】
従って、次の（数２２）で与えられる凸二次計画問題の目的関数として用いることができる。
【０１０７】
【数２２】
【０１０８】
一般にＡｋＴＡｋは正定でないため、微小な対角項δＩを加えることで正定行列になるようにしている。
【０１０９】
優先度ｋのタスクが不等式制約であった場合、その上下限制約の中央値を先端目標値ベクトルの指令速度とする。つまり、先端指令速度は次の（数２３）で与えられる。
【０１１０】
【数２３】
【０１１１】
なお、（数２３）のｂｋは、実施例における先端速度ベクトル｛ｄｘｋ｝に対応する。
【０１１２】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【０１１３】
１０：コントローラ
１２Ｒ、１２Ｌ：脚
１４Ｒ、１４Ｌ：腕
１００：脚式ロボット
【特許請求の範囲】
【請求項１】
制御可能な関節の数ｎがロボット先端に与えられる先端目標値ベクトル｛ｄｘ｝の次数ｍよりも多い冗長ロボットの関節目標値ベクトル｛ｄθ｝の決定方法であり、
ステップ１：ｍ次元の先端目標値ベクトル｛ｄｘ｝を、複数の部分ベクトル｛ｄｘｉ｝（ただし、下付きｉは各部分ベクトルを区別する指標）に分解するとともに、複数の部分ベクトル｛ｄｘｉ｝の夫々に対して、ロボットが達成すべき順に優先度を付与し（各部分ベクトルを区別する指標ｉが小さいほど優先度が高いことを表す）、
ステップ２：夫々の部分ベクトル｛ｄｘｉ｝に対して、部分ベクトルと関節目標値ベクトル｛ｄθ｝の間の線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝を求め（［Ａｉ］は、部分ベクトルごとにロボットの運動学的特性から定まる行列であり、｛ｄｘｉ｝の次数と同数の行数と、ｎ次元の列を有する行列を表す）、
ステップ３：複数の優先度から一の優先度ｋ（ｉ＝ｋ）を選択し、選択された優先度ｋの部分ベクトル｛ｄｘｋ｝に対して、次の実現可能領域存在問題、即ち、「選択された優先度ｋよりも高い優先度を有する全ての部分ベクトル｛ｄｘｉ｝に対して、線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝（ただしｉ＜ｋ）と不等式関係式｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝（ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限）で与えられる拘束条件の実行可能領域が存在するか否か」を判定し、
ステップ４：ステップ３を異なる優先度の複数の部分ベクトル｛ｄｘｉ｝に対して実行し、上記拘束条件の実行可能領域が存在すると判定された部分ベクトルのうち最も低い優先度を有する部分ベクトルについて、ステップ３の上記拘束条件の下で目的関数Ｃ２＝（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）Ｔ（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）を最小にする凸二次計画法の解｛ｄθ｝を求め、
ステップ５：ステップ４で求められた｛ｄθ｝をロボット関節目標値ベクトルとして決定する、
ことを特徴とする、冗長ロボットの関節目的値ベクトルの決定方法。
【請求項２】
ステップ３において、各関節に課せられた動作制限条件をさらに上記拘束条件に含むことを特徴とする請求項１に記載の決定方法。
【請求項３】
｛ｄｘ｝はロボット先端の目標速度ベクトルであり、｛ｄθ｝は関節目標速度ベクトルであり、［Ａｉ］はロボットのヤコビアンであることを特徴とする請求項１又は２に記載の決定方法。
【請求項４】
冗長ロボットの制御装置であり、
請求項１のステップ１の複数の部分ベクトル｛ｄｘｉ｝の夫々について、前記線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝と優先度を記憶しているとともに、前記目的関数Ｃ２を記憶しており、
予め定められた周期毎に請求項１のステップ３から５を実行し、得られたロボット関節目標値に追従するように各関節を制御することを特徴とする冗長ロボットの制御装置。
【請求項１】
制御可能な関節の数ｎがロボット先端に与えられる先端目標値ベクトル｛ｄｘ｝の次数ｍよりも多い冗長ロボットの関節目標値ベクトル｛ｄθ｝の決定方法であり、
ステップ１：ｍ次元の先端目標値ベクトル｛ｄｘ｝を、複数の部分ベクトル｛ｄｘｉ｝（ただし、下付きｉは各部分ベクトルを区別する指標）に分解するとともに、複数の部分ベクトル｛ｄｘｉ｝の夫々に対して、ロボットが達成すべき順に優先度を付与し（各部分ベクトルを区別する指標ｉが小さいほど優先度が高いことを表す）、
ステップ２：夫々の部分ベクトル｛ｄｘｉ｝に対して、部分ベクトルと関節目標値ベクトル｛ｄθ｝の間の線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝を求め（［Ａｉ］は、部分ベクトルごとにロボットの運動学的特性から定まる行列であり、｛ｄｘｉ｝の次数と同数の行数と、ｎ次元の列を有する行列を表す）、
ステップ３：複数の優先度から一の優先度ｋ（ｉ＝ｋ）を選択し、選択された優先度ｋの部分ベクトル｛ｄｘｋ｝に対して、次の実現可能領域存在問題、即ち、「選択された優先度ｋよりも高い優先度を有する全ての部分ベクトル｛ｄｘｉ｝に対して、線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝（ただしｉ＜ｋ）と不等式関係式｛ｄθｍｉｎ｝≦｛ｄθ｝≦｛ｄθｍａｘ｝（ただし、ｄθｍｉｎ、ｄθｍａｘは関節目標値ベクトルの上下限）で与えられる拘束条件の実行可能領域が存在するか否か」を判定し、
ステップ４：ステップ３を異なる優先度の複数の部分ベクトル｛ｄｘｉ｝に対して実行し、上記拘束条件の実行可能領域が存在すると判定された部分ベクトルのうち最も低い優先度を有する部分ベクトルについて、ステップ３の上記拘束条件の下で目的関数Ｃ２＝（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）Ｔ（｛ｄｘｋ｝−［Ａｋ］｛ｄθ｝）を最小にする凸二次計画法の解｛ｄθ｝を求め、
ステップ５：ステップ４で求められた｛ｄθ｝をロボット関節目標値ベクトルとして決定する、
ことを特徴とする、冗長ロボットの関節目的値ベクトルの決定方法。
【請求項２】
ステップ３において、各関節に課せられた動作制限条件をさらに上記拘束条件に含むことを特徴とする請求項１に記載の決定方法。
【請求項３】
｛ｄｘ｝はロボット先端の目標速度ベクトルであり、｛ｄθ｝は関節目標速度ベクトルであり、［Ａｉ］はロボットのヤコビアンであることを特徴とする請求項１又は２に記載の決定方法。
【請求項４】
冗長ロボットの制御装置であり、
請求項１のステップ１の複数の部分ベクトル｛ｄｘｉ｝の夫々について、前記線形関係式｛ｄｘｉ｝＝［Ａｉ］｛ｄθ｝と優先度を記憶しているとともに、前記目的関数Ｃ２を記憶しており、
予め定められた周期毎に請求項１のステップ３から５を実行し、得られたロボット関節目標値に追従するように各関節を制御することを特徴とする冗長ロボットの制御装置。
【図１】
【図２】
【図２】
【公開番号】特開２０１１−１８３５２７（Ｐ２０１１−１８３５２７Ａ）
【公開日】平成２３年９月２２日（２０１１．９．２２）
【国際特許分類】
【出願番号】特願２０１０−５３０７１（Ｐ２０１０−５３０７１）
【出願日】平成２２年３月１０日（２０１０．３．１０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２３年９月２２日（２０１１．９．２２）
【国際特許分類】
【出願日】平成２２年３月１０日（２０１０．３．１０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]