JP2006015430A - ロボット制御方法およびロボットシステム - Google Patents

Abstract

【課題】 自由度の高いロボットの各軸の値を短時間に求めることができるロボット制御方法を提供する。
【解決手段】 ステップＳ１では、遠隔操作装置からの作業目標、たとえば手先の移動先の指示位置を取得する。ステップＳ２では、手先、肩、および腰が、それぞれ腕部３２ａ、胴体部３３、および脚部３４ａの動作目標を満たす条件に合致する位置を、それぞれの参照位置として計算する。ステップＳ３では、計算された参照位置に基づいて、作業目標を達成するために設定された動作目標の優先度に従って、手先、肩、および腰の目標位置を計算する。この場合、動作目標の優先度は、脚部３４ａ、胴体部３３、腕部３２ａの順である。ステップＳ４では、計算された目標位置に基づいてロボットの各軸の値を計算する。ステップＳ５では、計算された各軸の値をロボットの各軸に指令して終了する。
【選択図】 図２

Description

本発明は、ヒューマノイドなどの自由度の高いロボットをリアルタイムで制御することができるロボット制御方法に関する。

昨今、研究教育分野あるいはエンターテイメント分野での人間型ロボットが脚光を浴びている。しかし、これらの人間型ロボットは、自由度が少なく、人間が行う作業をそのまま肩代わりしてくれるものではない。人間が作業を行う環境と同じ環境で、人間が行う作業をそのまま肩代わりすることができる人間型ロボットの実用化が期待されている。

図２４は、人間型ロボットの概略の構成を示す図である。ロボット１は、人間型ロボットであり、頭部２、腕部３ａ，３ｂ、胴体部４、脚部５ａ，５ｂ、手部６ａ，６ｂを含んで構成される。頭部２は、人間の頭に相当する部位であり、１つのリンクと１つの連結部からなる。腕部３ａ，３ｂは、人間の腕に相当する部位であり、それぞれ２つのリンクと３つの連結部からなり、胴体部４は、人間の胴体に相当する部位であり、２つのリンクと１つの連結部からなる。脚部５ａ，５ｂは、人間の脚に相当する部位であり、それぞれ３つのリンクと３つの連結部からなり、手部６ａ，６ｂは、人間の手に相当する部位であり、１つのリンク部と１つの連結部からなる。連結部は、１つまたは複数のジョイントから構成され、リンクとリンクとを連結する部位である。このジョイントは、１つの自由度を有する。

人間型ロボットは、さらに指などの自由度を含めると、自由度が３０自由度を超えることもあり、２０自由度以上のロボットを超多自由度ロボットと呼ぶことがある。このような超多自由度ロボットは、実際の作業を行うために必要な自由度に比べ、多くの冗長な軸に自由度を有する。ロボットを動作させるためには、ロボットの位置と姿勢をロボットに指示する必要がある。具体的には、その位置と姿勢を実現するロボットの各軸の値、たとえば角度を計算して、計算した値をロボットの各軸に指令する必要がある。軸の値が既知の軸以外の軸について、各軸の値を求める方法として、評価関数を用いて、各軸の値を変化させて評価関数が最適な値になる各軸の値を導出し、導出された各軸の値をロボットの各軸に指令する各軸の値として求める方法がある。

図２５は、評価関数を用いてロボットの各軸の値を求める方法を説明するための図である。作業目標情報（以下作業目標という）、たとえば手先の移動先の位置が指示され、ロボットの各軸の値を求めるときに本処理が行われる。ステップＡ１では、作業目標が指示される。ステップＡ２では、冗長軸に対する各軸の値に初期値を設定する。ステップＡ３では、評価関数に各軸の値を代入して評価値を計算する。ステップＡ４では、評価関数が最適な評価値になる各軸の値が求まったか否か判定する。最適な評価値になる各軸の値が求まったときは、ステップＡ５に進み、最適な評価値になる各軸の値が求まらなかったときは、ステップＡ６に進む。

ステップＡ５では、求めた各軸の値をロボットに指令して終了する。ステップＡ６では、冗長軸に対する各軸の値を変更して、ステップＡ３に戻る。このように、評価値が最適になる各軸の値を、計算を繰り返すことによって求めることができる。

人間が指示することができる自由度は、高々１２自由度であり、たとえば、マスターアームなどの操縦装置を用いて指示する場合でも１２自由度であり、残りの自由度については、計算で求める必要がある。人間型ロボットの場合、残りの自由度は１８自由度以上である。このような計算は、上述した評価関数を用いて繰り返し計算で行われるため、計算時間がかかる。そのため計算能力の高いコンピュータを用いてオフラインで計算する必要があり、実時間で計算して、つまり指示が与えられると即時に各軸の値を計算して人間型ロボットに指令することは難しい。

人間型ロボットを制御する第１の従来技術として、人間の意識動作に対応する動作を操作者によるジョイスティック操作によってロボットに指示を与え、人間の無意識動作に対応する動作をロボットの自律制御で実現する方法を提案している。この方法は、意識して動かしたい部位をスイッチで切替えて選択して指示する方法であって、作業目的に応じてロボットの自律制御を実現する方法を示したものではない（たとえば非特許文献１参照）。

人間型ロボットを制御する第２の従来技術として、人間型ロボットの手先に対して目標位置と目標外力が与えられた場合、ロボットが転倒しないように脚部を自律制御によって動作させる実時間制御方法がある。この方法では、評価関数を用いて最急降下法による繰り返し計算で肩の最適な位置を計算し、２次元モデルのロボットの最適な姿勢を求めているものであり、ロボットの自由度が増大すれば、評価するパラメータは増え、ロボット全体姿勢の評価に対する最適化計算は、非常に計算量が多くなり、実時間性を損なう可能性がある（たとえば非特許文献２参照）。

人間型ロボットを実時間で操作するときに、操作者の意図通りにロボットの動作を実現するためには、操作者からのロボットの一部に対する指示に基づいて、他の冗長度のある軸の値を短時間で計算する必要がある。特に、遠隔操作によってロボットを制御するときは、ロボットと遠隔操作装置との間の通信の実時間性だけでなく、ロボットに搭載されたコンピュータによって、ロボットの各軸の値を計算する計算周期毎にロボットへの指示を行なうことができる実時間性が重要である。人間型ロボット以外のロボットについても、ロボットの各軸の値の計算は評価関数を用いているので、自由度が２０自由度以上になると、計算時間がかかる。

Ｎｅｏ Ｅｅ Ｓｉａｎ、他４名、「ヒューマノイドロボットの全身遠隔操作-モーメンタム・コントロールを用いた全身運動生成-（Whole Body Teleoperation of a Humanoid Robot-A Method of Integrating Operator's Intention and Robot's Autonomy-）」、Proc. IEEE Int. Conf. Robotics and Automation、２００３年 井上健司、他２名、「腕で作業をしながら移動する人間型ロボットの実時間制御（Ｓａｇｉｔｔａｌ Ｐｌａｎｅ内の運動の制御）」、日本ロボット学会誌１８巻１号、２０００年１月、Ｐ７５−８６

しかしながら、上述した第１の従来技術は、人間の無意識動作に対応する動作をロボットの自律制御で実現する１つの方法を提案しているが、作業目標に応じた自律制御をどのように実現するかについては示していない。

上述した第２の従来技術は、ロボットに指令するための各軸の値を、評価関数を用いた繰り返し計算によって求めているために計算時間がかかり、実時間制御を行うことができないという課題がある。

本発明の目的は、自由度の高いロボットにおいて、作業目標に応じた自律制御を行うためのロボットの各軸の値を短時間に求めることができるロボット制御方法およびロボットシステムを提供することである。

本発明は、複数のリンクと各リンクを相互に連結する複数の連結部によって構成されるロボットにこのロボットが行うべき動作を示す作業目標情報が指示されたときに、前記ロボットを制御対象として分割した複数の制御対象領域毎に、前記作業目標情報を達成するために優先度が設定された複数の動作条件を、前記作業目標情報に応じて抽出する抽出工程と、
前記制御対象領域毎に、前記制御対象領域に含まれる部位のうちで前記制御対象領域の移動先位置が計算される部位である代表点が前記動作条件に合致する位置を、前記代表点の移動先位置の候補である参照位置として計算する参照位置計算工程と、
前記参照位置計算工程で代表点毎に計算された参照位置に基づいて、前記動作条件の優先度に従って、前記各代表点を移動させるべき位置である目標位置を計算する目標位置計算工程と、
前記目標位置計算工程で計算された目標位置に基づいて前記ロボットの各軸の値を計算する各軸値計算工程と、
前記各軸値計算工程で計算された各軸の値をロボットの各軸に指令する指令工程とを含むことを特徴とするロボット制御方法である。

本発明に従えば、まず、リンクと各リンクを連結する連結部とから構成されるロボットにこのロボットが行うべき動作を示す作業目標情報が指示されたときに、前記ロボットを制御対象として分割した複数の制御対象領域毎に、前記ロボットが行うべき動作を示す作業目標情報を達成するために、優先度が付された動作条件を抽出し、前記ロボットに作業目標情報が指示されたときに、前記制御対象領域毎に、前記制御対象領域に含まれる部位のうちで前記制御対象領域の移動先位置が計算される部位である代表点が前記動作条件に合致する位置を、前記代表点の移動先位置の候補である参照位置として計算する。次に、代表点毎に計算された参照位置に基づいて、前記動作条件の優先度に従って、前記各代表点を移動させるべき位置である目標位置を計算し、計算された目標位置に基づいて前記ロボットの各軸の値を計算し、計算された各軸の値をロボットの各軸に指令する。

このように、ロボットの各軸の値を制御対象領域毎に計算するので、ロボット全体としての繰り返し計算が不要になり、自由度の高いロボットにおいて、作業目標に応じた自律制御を行うためのロボットの各軸の値を短時間に求めることができる。

また本発明は、前記目標位置計算工程では、前記参照位置計算工程で代表点毎に計算された参照位置が、各代表点が移動することができる移動可能範囲内の所定の範囲である移動許諾範囲内にあるときは、各代表点の参照位置を各代表点の目標位置として計算し、
前記参照位置計算工程で代表点毎に計算された参照位置が対応する代表点の移動許諾範囲内にないときは、前記移動許諾範囲内の位置であって、各制御対象領域の所定の条件を満たす位置を、各代表点の目標位置として計算することを特徴とする。

本発明に従えば、制御対象領域毎に計算した参照位置が代表点の移動許諾範囲内でないときも、移動許諾範内の位置に目標位置を計算するので、許容される範囲内で動作が可能な各軸の値を短時間に求めることができる。

また本発明は、前記目標位置計算工程では、優先度の高い動作条件が設定された制御対象領域の代表点の目標位置を計算し、次に優先度の高い動作条件が設定された制御対象領域の代表点の目標位置を基準として、隣接する制御対象領域の代表点の目標位置を計算することを特徴とする。

本発明に従えば、隣接する制御対象領域の代表点の目標位置の計算を、優先度の高い動作条件が設定された制御対象領域の目標位置を基準に計算しているので、すなわち、他の制御対象領域との関係を考慮する必要がないので、計算量を少なくでき、ロボットの各軸の値を短時間に求めることができる。

また本発明は、前記各代表点の移動許諾範囲を対応する移動可能範囲で除した割合が、優先度の高い動作条件が設定された制御対象領域のほうが優先度の低い動作条件が設定された制御対象領域より小さいことを特徴とする。

本発明に従えば、優先度の高い動作条件が設定された制御対象領域の代表点の移動可能範囲に対する移動許諾範囲の割合を、優先度の低い動作条件が設定された制御対象領域より小さくしているので、優先度の高い動作条件が設定された制御対象領域の代表点の目標位置の影響を少なくすることができる。

また本発明は、前記参照位置計算工程では、以前前記ロボットに指示された作業目標情報に対して前記目標位置計算工程で計算された目標位置と前記各軸値計算工程で計算された各軸の値に基づいて参照位置が計算されることを特徴とする。

本発明に従えば、以前に求めた目標位置と以前に求めた各軸の値とに基づいて新たな目標位値を求めるので、動作しているロボットの現在の状態に基づいた次の目標位置を計算できる。

また本発明は、前記目標位置計算工程では、計算された目標位置を、以前前記指令工程で各軸の値をロボットの各軸に指令したことによって生じたロボットの動作に起因する動的要素の情報に基づいて再計算することを特徴とする。

本発明に従えば、ロボットの動作に起因する動的要素、たとえば、動作しているロボットの慣性を考慮して目標位置を再計算するので、より安定した目標位置を計算でき、ロボットの安定性を向上することができる。

また本発明は、前記制御対象領域毎の代表点の参照位置を並行して計算することを特徴とする。

本発明に従えば、制御対象領域毎の代表点の参照位置を並行して計算するので、参照位置の計算時間を短時間に行うことができ、さらにロボットの各軸の値を短時間に求めることができる。

また本発明は、前記制御対象領域内の各軸の値を制御対象領域毎に並行して計算することを特徴とする。

本発明に従えば、制御対象領域内の各軸の値を制御対象領域毎に並行して計算するので、ロボット全体の各軸の値の計算時間を短時間に行うことができる。

また本発明は、前記ロボットは、２０自由度以上の自由度を有することを特徴とする。
本発明に従えば、繰り返し計算を行わないので、２０自由度以上の自由度を有するロボットについて、従来技術による計算時間に比べて、ロボットの各軸の値をより短時間に求めることができる。

また本発明は、前記制御対象領域の自由度は、６自由度以下であることを特徴とする。
本発明に従えば、制御対象領域の自由度が６自由度以下であるので、つまり制御対象領域内の自由度が少ないので、冗長な自由度を少なくでき、制御対象領域内の各軸の値の計算時間を短くでき、さらにロボットの各軸の値を短時間に求めることができる。

また本発明は、前記ロボットは、人間の腕に相当する腕部、人間の胴体に相当する胴体部、人間の脚に相当する脚部とを含む人間型ロボットであり、前記腕部、前記胴体部、および前記脚部をそれぞれ制御対象領域とすることを特徴とする。

本発明に従えば、人間型ロボットを腕部、胴体部、および脚部の制御対象領域に分割するので、３０自由度以上の自由度の人間型ロボットにも適用することができる。

また本発明は、前記人間型ロボットの制御対象領域毎の動作条件が、腕部は作業目標情報に応じた動作を実現すること、胴体部は腕部の動作範囲を広く保つこと、脚部は全体のバランスを維持することおよび胴体部の動作範囲を広く保つことであることを特徴とする。

本発明に従えば、それぞれの部位に作業目標情報を達成するための動作条件を設定しているので、ロボット全体として、ロボットのバランスを維持し、手先の動作範囲を広く保ち、さらに作業目標情報に応じた動作を実現することができる。

また本発明は、前記人間型ロボットは、人間の頭に相当する頭部を含み、頭部の動作条件が作業に必要な視野を確保すること、および前記胴体部は頭部の動作範囲も広く保つことであることを特徴とする。

本発明に従えば、人間型ロボットの頭部も制御対象領域として分割するので、頭部の動作条件を実現することができ、頭部の動作範囲を広く保つことができる。

また本発明は、前記人間型ロボットは、前記腕部の代表点が手先、前記胴体部の代表点が肩、および前記脚部の代表点が腰であり、前記制御対象領域の代表点の移動許諾範囲の移動可能範囲に対する割合が、腰の代表点の割合が肩部の代表点の割合より小さく、かつ肩の代表点の割合が手先の代表点の割合より小さいことを特徴とする。

本発明に従えば、腰の移動可能範囲に対する移動許諾範囲の割合を肩より小さくし、かつ肩の移動可能範囲に対する移動許諾範囲の割合を手先より小さくしているので、優先度の高い動作条件が設定された脚部の代表点である腰の目標位置の影響を肩および手先より、および肩の目標位置の影響を手先より少なくすることができる。

また本発明は、前記参照位置計算工程で、腰の参照位置を計算する際、腰の速度が肩の速度に比例した速度に計算することを特徴とする。

本発明に従えば、腰の移動速度を肩の移動速度に比例させているので、胴体部の前屈量を抑えることができる。

また本発明は、複数のリンクと各リンクを相互に連結する複数の連結部によって構成されるロボットに、前記ロボット制御方法を実行するコンピュータを搭載したことを特徴とするロボットシステムである。

本発明に従えば、ロボットに前記ロボット制御方法を実行するコンピュータを搭載しているので、外部の遠隔操作装置から指示する制御情報が作業目標情報に関する情報だけでよく、ロボットシステムと遠隔操作装置との通信情報量を少なくすることができ、さらにロボットの各軸の値を短時間に求めることができる。

また本発明は、前記ロボットは、作業目標情報を決定するための検知手段を有し、前記検知手段で検知された情報に基づいて作業目標情報を生成し、生成された作業目標情報を前記ロボットの作業目標情報とすることを特徴とする。

本発明に従えば、ロボットが有する検知手段からの情報に基づいてロボットシステムが生成した新たな作業目標情報をロボットへの新たな作業目標情報とすることができるので、より自律したロボットシステムを実現することができ、さらにロボットの各軸の値を短時間に求めることができる。

また本発明は、複数のリンクと各リンクを相互に連結する複数の連結部によって構成されるロボットを制御対象とする複数の制御対象領域に分割する分割工程と、
前記制御対象領域毎に、前記制御対象領域に含まれる部位のうちで前記制御対象領域の移動先位置が計算される部位である代表点を決定する決定工程と、
前記制御対象領域毎に、前記ロボットが行うべき動作を示す作業目標情報を達成するために、優先度を付けた動作条件を設定する設定工程と、
前記ロボットに作業目標情報が指示されたときに、前記制御対象領域毎に、前記代表点が前記動作条件に合致する位置を、前記代表点の移動先位置の候補である参照位置として計算する参照位置計算工程と、
前記参照位置計算工程で代表点毎に計算された参照位置に基づいて、前記動作条件の優先度に従って、前記各代表点を移動させるべき位置である目標位置を計算する目標位置計算工程と、
前記目標位置計算工程で計算された目標位置に基づいて前記ロボットの各軸の値を計算する各軸値計算工程と、
前記各軸値計算工程で計算された各軸の値をロボットの各軸に指令する指令工程とを含むことを特徴とするロボット指令決定方法である。

本発明に従えば、ロボットを複数に制御対象に分割した制御対象領域を想定し、制御対象領域毎に作業目標情報を達成するための動作条件を優先度を付けて設定し、制御対象領域を移動する位置の候補である参照位置を計算し、計算された参照位置に基づいて、優先度に従って目標位置を計算し、計算された目標位置に基づいてロボットの各軸の値を計算するので、ロボットの制御対象への分割の仕方を変えれば、種々のロボットに適用可能であり、さらにロボット全体としての繰り返し計算が不要になり、ロボットの各軸の値を短時間に求めることができる。

本発明によれば、ロボットの各軸の値を短時間に求めることができるので、ロボットを実時間で制御することができ、オフラインでの各軸の値の決定に使用すれば、オフラインでの計算時間を短くすることができる。

図１は、本発明の実施の一形態であるロボット制御方法が適用されるロボットシステム１０の構成を示す図である。ロボットシステム１０は、ロボット部１１、制御部１２、通信部１３を含んで構成される。ロボット部１１は、撮像部１４を含み、複数のリンクと各リンクを相互に連結する複数の連結部によって構成されるロボットであり、たとえば、頭部３１、腕部３２、胴体部３３、脚部３４などのサブシステムから構成される人間型ロボットである。撮像部１４は、ＣＣＤカメラなどの映像を撮影する部位であり、たとえば人間型ロボットの場合、ロボット部１１の頭部３１に搭載され、撮影した映像を制御部１２に送信する。

制御部１２は、ロボットシステム１０全体を制御する部位であり、ＯＳ（Operating
System）が稼動するコンピュータによって実現される。コンピュータは、ロボット部１１に搭載され、図２または図２１に示すロボット制御方法を実行することができる。制御部１２は、遠隔操作装置２０からロボットが行うべき動作を示す作業目標情報（以下作業目標という）作業目標を受信すると、作業目標を実現するためのロボットの各軸の値を、図２または図２１に示すロボット制御方法によって計算し、計算した各軸の値をロボット部１１に指令する。制御部１２は、通信部１３を介して、撮像部１４からの映像を遠隔操作装置２０に送信する。

通信部１３は、遠隔操作装置２０と通信を行う部位であり、遠隔操作装置２０から受信した情報を制御部１２に送信し、制御部１２からの情報を遠隔操作装置２０に送信する。

遠隔操作装置２０は、操作者が入力したロボットシステム１０への指示をロボットシステム１０に送信するための装置であり、入力部２１、制御部２２、通信部２３、および表示部２４を含んで構成される。ロボットシステム１０との通信は、有線・無線を問わず、たとえば、ネットワークを介した通信でもよく、専用の通信回線を用いてもよい。

入力部２１は、マスターアームあるいはジョイスティックなどの入力装置から構成され、キーボードおよびマウスなどの入力装置を併用してもよい。入力部２１は、これらの入力装置から入力された情報を制御部２２に送信する。

制御部２２は、遠隔操作装置２０全体を制御する部位であり、入力部２１から入力された情報のうちロボットシステム１０に送信する必要がある情報を通信部２３に送る。制御部２２は、ロボットシステム１０から通信部２３を介して受信した情報のうち、表示部２４に表示する情報を表示部２４に送信する。

通信部２３は、ロボットシステム１０と通信を行う部位であり、ロボットシステム１０から受信した情報を制御部２２に送信し、制御部２２からの情報をロボットシステム１０に送信する。

表示部２４は、液晶ディスプレイなどの表示画面を有する表示装置であり、制御部２２から送信された情報を表示画面に表示する。操作者は、表示画面に表示された映像をみながら、入力装置から作業目標を指示しロボットを操作することができる。

ロボットに前記ロボット制御方法を実行するコンピュータを搭載しているので、外部の遠隔操作装置から指示する制御情報が作業目標に関する情報だけでよく、ロボットシステムと遠隔操作装置との通信情報量を少なくすることができ、さらにロボットの各軸の値を短時間に求めることができる。

ロボットシステム１０に作業目標を決定するための検知手段、たとえば距離を測るセンサを搭載し、制御部１２は、搭載されたセンサで検出された情報に基づいて作業目標を生成し、遠隔操作装置２０から受信した作業目標の代わりに、制御部１２が生成した作業目標をロボットの作業目標としてもよい。あるいは撮像部１４からの情報を基に、制御部１２で手先位置を認識し、手先位置が目的の位置に到達していないとき、目的の位置に近づけるための位置を新たな作業目標として生成し、生成した作業目標をロボットの作業目標としてもよい。この場合、作業目標をロボットシステム１０自体で生成することができるので、より自律したロボットシステムを実現することができる。

図２は、本発明の実施の一形態であるロボット制御方法の工程を示すフローチャートである。本処理は、ジョイスティックなどの入力装置を有する遠隔操作装置から作業目標、つまりロボットが実現すべき動きが指示されたときに開始される。このロボット制御方法は、ロボットの手先、たとえば図２４に示したロボットの腕部３ａの先端を移動させるという作業目標を、ロボットに指示したときのロボットの各軸の値、たとえば、ロボットの連結部を構成するジョイントの角度を求め、求めた角度をロボットの各軸に指令する方法である。

ステップＳ１では、遠隔操作装置からの作業目標、たとえば手先の移動先の指示位置を取得する。ステップＳ２では、手先が、たとえば腕部３ａに設定された動作条件である動作目標に合致する位置を参照位置として計算し、肩、たとえば図２４に示したロボットの腕部３ａと胴体部４との連結部が、胴体部４に設定された動作目標に合致する位置を参照位置として計算し、腰、たとえば図２４に示したロボットの胴体部４と脚部５ａとの連結部が、脚部５ａの動作目標に合致する位置を、それぞれの参照位置として計算する。

動作目標は、遠隔操作装置から指示される作業目標に応じて制御対象領域毎、つまりサブシステム毎に設定される目標であり、たとえばそれぞれのサブシステムに設定された動作目標は、腕部３ａは、「作業目標に応じた動作を実現すること」、胴体部４は、「手先の到達範囲を拡大すること」、脚部５ａは、「全身のバランスを維持することと上下方向に対する手先の到達範囲を拡大すること」である。それぞれの部位に作業目標を達成するための動作目標を設定しているので、ロボット全体として、ロボットのバランスを維持し、手先の動作範囲を広く保ち、さらに作業目標に応じた動作を実現できる。

サブシステムは、１または複数のリンクが変位可能に連結部で連結されたまとまりであり、たとえば図２４に示したロボットでは、腕部３ａ、胴体部４、脚部５ａなどがサブシステムである。参照位置は、手先、肩、腰などの代表点を移動させる位置の候補である。代表点は、ロボット全体の動作を決定する各軸の値を計算するために、ロボットの部位のうちで移動させる位置が最初に計算される部位であり、作業目標によって指示が与えられる部位とサブシステムの連結部とが代表点となり、各サブシステムに１つ設定される。

ステップＳ３では、計算された参照位置に基づいて、作業目標を達成するために設定された動作目標の優先度に従って、手先、肩、および腰の目標位置を計算する。動作目標の優先度は、作業目標に応じて決定され、脚部５ａ、胴体部４、腕部３ａの順である。目標位置は、代表点を移動させる位置である。ステップＳ４では、計算された目標位置に基づいてロボットの各軸の値を計算する。ステップＳ５では、計算された各軸の値をロボットの各軸に指令して終了する。この実施例では、図２４に示した、腕部３ａ、胴体部４、脚部５ａにのみ着目して説明したが、他のサブシステム、たとえば頭部２、腕部３ｂ、脚部５ｂなどについても動作目標と優先度を設定して計算する。

図３は、図２に示すロボット制御方法が適用されるロボット３０のサブシステムへの分割の一例を示す図である。図３（ａ）は、ロボット３０の概観図である。ロボット３０は、頭部３１、腕部３２、胴体部３３、および脚部３４を含んで構成される。ロボット３０は、机６１の上にある対象物６２に対して、右腕の手先を移動するところを示している。視野３１ｓは、ロボットの頭部３１に搭載された撮像部の視野を示している。

図３（ｂ）は、ロボット３０を側方から見た図であり、ロボット３０は、腕部３２、胴体部３３、および脚部３４を含んで構成され、３つのサブシステムに分割されている。脚部３４については、左脚部は右脚部と重なった状態であり、腕部については、左腕部は右腕部に重なった状態である。手部３５と頭部３１を省略しているが、手部３５および頭部３１をそれぞれ１つのサブシステムとして含めてもよい。

腕部３２は、手先４１、肘４２、および第１のリンク５１と第２のリンク５２を含み、第１のリンク５１と第２のリンク５２の連結部が肘４２である。胴体部３３は、肩４３、胸４４、および第３のリンク５３と第４のリンク５４を含み、第２のリンク５２と第３のリンク５３の連結部が肩４３であり、第３のリンク５３と第４のリンク５４の連結部が胸４４である。脚部３４は、腰４５、膝４６、および第５のリンク５５と第６のリンク５６を含み、第４のリンク５４と第５のリンク５５の連結部が腰４５であり、第５のリンク５５と第６のリンク５６の連結部が膝４６である。足４７は、いずれにも属さない部位であり、基準点とする。連結部は、変位可能である。

腕部３２の代表点は手先４１、胴体部３３の代表点は肩４３、脚部３４の代表点は腰４５である。この代表点は、ロボット全体の動作を決定する各軸の値を計算するために、ロボットの部位のうちで移動させる位置が最初に計算される部位であり、作業目標によって指示が与えられる部位とサブシステムの連結部が代表点となり、各サブシステムに１つ設定される。この例では、作業目標によって指示が与えられる部位が手先４１であり、サブシステムの連結部が肩４３と腰４５である。

ロボット３０は、床面あるいは地面などの水平面６０上の足４７の位置でバランスが取れた状態、つまり、転倒しない状態であり、以下の説明では、「足の位置を動かさない状態での手先操作」について説明するが、足を動かす場合についても実施可能である。

図４は、人間型ロボットの各部の自由度を説明するための図である。図４（ａ）、図４（ｂ）は、図４（ｃ）〜図４（ｇ）に用いられている記号を説明するための図である。図４（ａ）は、軸方向を中心に回転する同軸ジョイント３８０を説明するための図である。同軸ジョイント３８０は、リンク部材３８１とリンク部材３８２とを連結し、リンク部材３８２がリン部材ク３８１に対して、リンク部材３８１の軸方向を中心に回転する。図４（ｂ）は、両方のリンク部材の軸を含む平面に垂直な直線でジョイントの中心を通る直線を中心に一方のリンク部材を他方のリンク部材に対して回転させる屈曲ジョイント３９０を説明するための図である。屈曲ジョイント３９０は、リンク部材３９２の中心軸を含む平面上でリンク部材３９３を屈曲ジョイント３９０の軸３９１を中心に回転させる。同軸ジョイント３８０と屈曲ジョイント３９０は、それぞれ１つの自由度を有する。

図４（ｃ）は、頭部３１の自由度を示す図であり、同軸ジョイント３１１と屈曲ジョイント３１２の２つの軸があり、合計２自由度である。同軸ジョイント３１１と屈曲ジョイント３１２が頭部３１と胴体部３３の連結部である首に相当する。図４（ｄ）は、腕部３２の自由度を示す図であり、同軸ジョイント３２１，３２３，３２５、屈曲ジョイント３２２，３２４，３２６の６つの軸があり、合計６自由度である。同軸ジョイント３２１，３２３、および屈曲ジョイント３２２が肩に相当し、屈曲ジョイント３２４が肘に相当し、同軸ジョイント３２５および屈曲ジョイント３２６が手首に相当する。

図４（ｅ）は、胴体部３３の自由度を示す図であり、屈曲ジョイント３３１と同軸ジョイント３３２の２つの軸があり、合計２自由度である。屈曲ジョイント３３１と同軸ジョイント３３２が、胸に相当する。図４（ｆ）は、脚部３４の自由度を示す図であり、同軸ジョイント３４１，３４３，３４４，３４５、同軸ジョイント３４２，３４６の６つの軸があり、合計６自由度である。同軸ジョイント３４１，３４３、および屈曲ジョイント３４２が腰、同軸ジョイント３４４が膝、同軸ジョイント３４５と屈曲ジョイント３４６が足首に相当する。図４（ｇ）は、手部３５の自由度を示す図であり、屈曲ジョイント３５１の１つの軸があり、合計１自由度である。屈曲ジョイント３５１は指の関節に相当し、指の開閉を行う。

図５は、図２に示すロボット制御方法が適用されるロボット３０への作業目標の指示とその指示によるロボットの変位の一例を示す図である。ロボット３０への作業目標は、マスターアームあるいはジョイスティックなどの入力部を有する遠隔操作装置から指示される。この例では、作業目標は、手先４１を手先の指示位置４１ａの位置に移動させることである。作業目標を実現するロボットの各軸の値を求めて、求めた各軸の値をロボットの各軸に指令することによって、手先４１のロボットの位置と姿勢から、手先が指示位置４１ａに移動したときの位置と姿勢に変化させる。

図６は、図２に示すロボット制御方法での手先４１の参照位置の計算のフローチャートである。手先４１の参照位置の計算を行うときに本処理が行われる。ステップＳ１０では、遠隔操作装置から指示された手先４１の指示位置を手先４１の参照位置として終了する。

図７は、図２に示すロボット制御方法での肩４３の参照位置の計算のフローチャートである。肩４３の参照位置の計算を行うときに本処理が行われる。胴体部３３の動作目標は、「手先の到達範囲を拡大すること」であり、手先４１の参照位置と現在位置および肩４３の現在位置から肩４３の参照位置を求める。

ステップＳ２０では、肩４３の現在位置、つまり前回ロボットの各軸への指令によって移動した位置と手先４１の参照位置との距離Ｌ_ａｒｍを計算する。ステップＳ２１では、計算した距離Ｌ_ａｒｍが所定の最大距離Ｌ_ｍａｘより長いか否か判定する。計算した距離Ｌ_ａｒｍが所定の最大距離Ｌ_ｍａｘより長いときは、ステップＳ２４に進み、計算した距離Ｌ_ａｒｍが所定の最大距離Ｌ_ｍａｘより長くないときは、ステップＳ２２に進む。

ステップＳ２２では、計算した距離Ｌ_ａｒｍが所定の最小距離Ｌ_ｍｉｎより短いか否か判定する。計算した距離Ｌ_ａｒｍが所定の最小距離Ｌ_ｍｉｎより短いときは、ステップＳ２３に進み、計算した距離Ｌ_ａｒｍが所定の最小距離Ｌ_ｍｉｎより短くないときは、ステップＳ２５に進む。ステップＳ２３では、図７に示すように、肩４３の参照位置を、肩４３と手先４１とを結ぶ直線上で手先４１から肩４３の方向と反対方向のベクトルと、腰４５と肩４３とを結ぶ方向と直角方向であって手先４１と反対方向のベクトルとの合成ベクトルの方向に肩４３を移動させる位置として終了する。

ステップＳ２４では、肩４３の参照位置を、肩４３を手先４１の方向に一定速度で移動させる位置として終了する。この場合、一定速度で移動させる位置とは、前回計算した目標位置に移動した際の速度を維持することができる位置のことである。ステップＳ２５では、肩４３の現在位置を肩４３の参照位置として終了する。

このように、手先４１が腕部３２の伸びる方向に移動したときは、肩４３も追随するので、さらに腕部３２を伸ばすことができ、逆に手先４１が肩４３に近づくときは、胴体部３３が手先４１の現在位置から離れる方向に移動して、腕部３２と胴体部３３との干渉、つまり接触または衝突を回避することができる。

図８は、図２に示すロボット制御方法での肩４３の参照位置の計算を説明するための図である。肩４３の現在位置と手先４１の参照位置との距離Ｌ_ａｒｍが所定の距離Ｌ_ｍａｘより長いとき、肩４３の移動方向は、手先４１から肩４３への方向ベクトル８０と、腰４５と肩４３を結ぶ直線８１に直角の方向ベクトル８２との合成ベクトル８３の方向である。

図９は、図２に示すロボット制御方法での腰４５の参照位置の計算のフローチャートである。腰４５の参照位置の計算を行うときに本処理が行われる。脚部３４の動作目標は、「人間型ロボットの全身のバランスを維持すること」と「上下方向に対する手先の到達範囲を拡大すること」の２点であり、ロボットの重心位置、手先４１の指示位置、および手先４１と肩４３と腰４５の現在位置から腰４５の参照位置を求める。

ステップＳ３０では、現在の重心位置を計算する。重心位置の計算は、胴体部３３と脚部３４の動作がロボット動作の動的な動きに大きく影響しない程度の速度で動作を行うと仮定して、つまり、ロボットの動作に伴う慣性を考慮しなくてもよい程度の速度で動作を行うと仮定して行う。全身の重心位置を床面に投影した位置である床面投影点（床面重心点ともいう）が、両足裏で構成される多角形である支持多角形内の所定の範囲内にあるか否かで、腰４５の参照位置の水平成分の位置を求める。ロボットの重心位置の求め方については、後述する。

ステップＳ３１では、計算された重心位置、つまり床面投影点が、支持多角形の中心点から所定以上の距離か否か判定する。所定以上の距離のときは、ステップＳ３６に進む。所定以上の距離でないときは、ステップＳ３２に進む。ステップＳ３２では、肩４３の移動方向が下方向か否か判定する。肩４３の移動方向が下方向のときは、ステップＳ３７に進み、肩４３の移動方向が下方向でないときは、ステップＳ３３に進む。

ステップＳ３３では、肩４３の移動方向が上方向か否か判定する。肩４３の移動方向が上方向のときは、ステップＳ３４に進み、肩４３の移動方向が上方向でないときは、ステップＳ３９に進む。ステップＳ３４では、腰４５の現在位置と肩４３の現在位置との距離Ｌ_ｂｏｄｙが所定の胴体距離Ｌ_ａ以上か否かを判定する。距離Ｌ_ｂｏｄｙが所定の胴体距離Ｌ_ａ以上のときは、ステップＳ３５に進み、距離Ｌ_ｂｏｄｙが所定の胴体距離Ｌ_ａ以上でないときは、ステップＳ３９に進む。ステップＳ３５では、腰４５の参照位置の垂直成分を肩４３の移動速度の垂直成分に比例した速度で上方向に移動する位置として終了する。

ステップＳ３６では、腰４５の参照位置の水平成分を、支持多角形の中心方向に、中心からの距離に比例した速度で移動する位置としてステップＳ３２に進む。ステップＳ３７では、後述する胴体部３３の傾きθ_ｗが鉛直方向から所定の角度θ_ａ以上傾いているか否か判定する。所定の角度θ_ａ以上傾いているときは、ステップＳ３８に進み、所定の角度θ_ａ以上傾いていないときは、ステップＳ３９に進む。

ステップＳ３８では、腰４５の参照位置の垂直成分を肩４３の移動速度の垂直成分に比例した速度で下方向に移動する位置とする。手先４１が下方に移動するとき、腰４５の位置を動かさずに、胴体部３３を深く曲げる動作も考えられるが、脚部関節の動作範囲制限があって実現は難しく、腰４５を肩４５の動きに連動させて移動させる。ステップＳ３９では、腰の現在位置を腰の参照位置として終了する。

図１０は、腰４５の移動を肩４３の移動と連動させることについて説明するための図である。図１０（ａ）は、胴体部３３の傾きθ_ｗを説明するための図である。胴体部３３の傾きθ_ｗは、鉛直方向８４に対する腰４５と肩４３とを結ぶ直線８１の傾きである。この傾きθ_ｗが所定の角度θ_ａ以上のときに、肩４３に連動させて腰４５の参照位置を下げる。図１０（ｂ）は、腰４５を移動しない場合の胴体部３３の動き、図１０（ｃ）は、腰４５を肩４３の移動速度に比例した速度で移動した場合の胴体部３３の動き、図１０（ｄ）は、腰４５を一定速度で移動した場合の胴体部３３の動きを説明するための図である。

図１０（ｂ）は、腰４５を移動しないで、肩４３を肩４３ａの位置まで矢印８６ａの方向に移動したとき、胸４４と肩４３とのリンク５３の傾きがθ_ｂ変化している。これに対して、図１０（ｃ）は、肩４３が肩４３ａの位置まで矢印８６ａの方向に移動したときに、腰４５は、腰４５ａの位置まで矢印８７ａの方向に移動するので、リンク５３の角度は、θ_ｃだけしか変化せず、図１０(ｂ)の腰を固定したときのリンクの傾きの変化θ_ｂより小さくすることができる。つまり、胴体部３３が傾く速度を抑えることができ、胴体部の前屈量も抑えることができる。特に、胴体部３３が伸びきっているときは、胸４４を中心とした肩の回転角度は、腰４５を中心とした肩４３の回転角度より大きくなる。胴体部３３が大きく傾いているとき、たとえば胴体部３３の傾きθ_ｗが４５度以上傾いているとき、ロボット全体の質量に占める割合の多い胴体部３３の回転速度が大きくなるので、動的な動作の影響、つまり慣性の影響が大きくなり、ロボットが転倒しやすくなる。転倒を防止するために、胴体部３３の回転速度を抑えることは、人間型ロボットの安定な制御動作を実現するために効果がある。

図１０（ｄ）は、腰４５を肩４３の動きとは独立して一定速度で移動する場合を示しており、この場合は、胴体部３３が傾く速度を必ずしも抑えることができるとは限らない。肩４３が方向８６ｂの方向、つまり、前方方向に移動したときは、肩４３は肩４３ｂの位置に移動するが、腰４５が方向８７ｂに移動して腰４５ｂの位置に来ると、肩４３は４３ｃの位置に移動することになり、肩の参照位置を実現できなくなることがある。従って、肩４３が下方に移動するときは、腰４５を肩４３の移動速度に比例した速度で移動させる必要がある。

ロボットの重心位置は、ロボットの各軸の値、つまり角度と、リンクの重心情報から計算する。たとえばロボット３０の前後方向をｘ軸方向とすると、ｘ軸方向の重心位置Ｘは、式（１）で求めることができる。
Ｘ＝（Ｍ_１Ｘ_１＋Ｍ_２Ｘ_２＋…＋Ｍ_ｎＸ_ｎ）／（Ｍ_１＋Ｍ_２＋…＋Ｍ_ｎ）
…（１）
ここに、Ｍ_ｉは、リンクｉの重量であり、Ｘ_ｉは、リンクｉの重心座標である。ｉは、１〜ｎの自然数であり、Ｘ_ｉは、ロボットが直立状態のときのロボットのｘ座標を基準としたときのリンクｉの重心座標の値である。

水平面上でロボットのｘ軸方向と直角の方向であるｙ軸方向の重心位置Ｙについても同様に、式（２）で求めることができる。
Ｙ＝（Ｍ_１Ｙ_１＋Ｍ_２Ｙ_２＋…＋Ｍ_ｎＹ_ｎ）／（Ｍ_１＋Ｍ_２＋…＋Ｍ_ｎ）
…（２）
ここに、Ｙ_ｉは、リンクｉのｙ軸方向の重心座標であり、ロボットが直立状態のときのロボットのｙ座標を基準としたときのリンクｉの重心座標の値である。

図１１は、リンクの重心位置の求め方を説明するための図である。リンクＡ９０とリンクＢ９１は、ジョイント９２によって連結されている。ジョイント９２は、リンクＢ９１と一体であり、ジョイント９２を構成する回転軸９３は、リンクＡ９０と一体である。この場合、回転軸９３の重量は、リンクＡ９０に含まれる。すなわち、ジョイント部分の重量は、一体であるほうのリンクに含めて計算する。それぞれのリンクの重量と重心位置は、３次元ＣＡＤの設計データなどから求めることができる。

図１２は、ロボットの座標系を説明するための図である。人間型ロボットについての、参照位置と目標位置は、ロボットの絶対座標系Σ_Ｗからみた座標系で表す。ロボットの絶対座標系Σ_Ｗは、ロボットを初期設定する際に、ロボットの腰座標系Σ_ｋから鉛直下方向の床面に固定された座標系とする。手先の位置および姿勢の指示は、ロボットの絶対座標系Σ_Ｗからみた手先座標系Σ_Ｔの位置姿勢を指示する。３次元でみると、ロボットの腰座標系Σ_ｋは、図２４に示すように、左右脚の股関節の中間点８である。

リンクのジョイント部分の位置は、ロボットの各軸の値、つまり角度から順変換計算式によって求める。順変換計算式は、ロボット各部のリンク構造に依存した幾何学的関係を利用して作成される。

図１３は、脚部３４の各軸の値から位置姿勢を求める際の各軸の座標系を説明するための図である。ジョイント３４１〜ジョイント３４６の角度と座標系を、それぞれθ_１〜θ_６、Σ_１〜Σ_６とし、足首の座標系をΣ_ｆとする。図１３では、ロボットの基準座標系Σ_０は、同軸ジョイント３４１，３４３、および屈曲ジョイント３４２の回転軸の交点であり、図２４に示す連結部７の位置である。

距離Ｌ_１〜距離Ｌ_３は、それぞれ屈曲ジョイント３４２から屈曲点３５０まで長さ、屈曲点３５０から同軸ジョイント３４４までの長さ、同軸ジョイント３４４から同軸ジョイント３４５までの長さを表している。同軸ジョイント３４５から屈曲ジョイント３４６までの長さは０である。

順変換は、各軸、つまりジョイント３４１〜ジョイント３４６の角度が与えられたときの基準座標系Σ_０からみた足首の座標系Σ_ｆの位置姿勢^０Ｔ_ｆを求めるものである。位置姿勢^０Ｔ_ｆを行列で表すと、式（３）として表せる。

ただし、
Ｒ_１１＝Ｃ_１Ｃ_３４５−Ｓ_１Ｓ_２Ｓ_３４５
Ｒ_１２＝Ｓ_１Ｓ_２Ｃ_３４５Ｓ_６＋Ｃ_１Ｓ_３４５Ｓ_６−Ｓ_１Ｃ_２Ｃ_６
Ｒ_１３＝Ｓ_１Ｓ_２Ｃ_３４５Ｃ_６＋Ｃ_１Ｓ_３４５Ｃ_６＋Ｓ_１Ｃ_２Ｓ_６
Ｒ_２１＝Ｓ_１Ｃ_３４５＋Ｃ_１Ｓ_２Ｓ_３４５
Ｒ_２２＝−Ｃ_１Ｓ_２Ｃ_３４５Ｓ_６＋Ｓ_１Ｓ_３４５Ｓ_６＋Ｃ_１Ｃ_２Ｃ_６
Ｒ_２３＝−Ｃ_１Ｓ_２Ｃ_３４５Ｃ_６＋Ｓ_１Ｓ_３４５Ｃ_６−Ｃ_１Ｃ_２Ｓ_６
Ｒ_３１＝−Ｃ_２Ｓ_３４５
Ｒ_３２＝Ｃ_２Ｃ_３４５Ｓ_６＋Ｓ_２Ｃ_６
Ｒ_３３＝Ｃ_２Ｃ_３４５Ｃ_６−Ｓ_２Ｓ_６
Ｘ＝−Ｌ_２（Ｓ_１Ｓ_２Ｃ_３＋Ｃ_１Ｓ_３）−Ｌ_３Ｓ_１Ｓ_２Ｃ_３４−Ｌ_３Ｃ_１Ｓ_３４
＋Ｌ_１Ｓ_１Ｃ_２
Ｙ＝Ｌ_２（Ｃ_１Ｓ_２Ｃ_３−Ｓ_１Ｓ_３）＋Ｌ_３Ｃ_１Ｓ_２Ｃ_３４−Ｌ_３Ｓ_１Ｓ_３４
−Ｌ_１Ｃ_１Ｃ_２
Ｚ＝−Ｌ_３Ｃ_２Ｃ_３−Ｌ_２Ｃ_２Ｃ_３４−Ｌ_１Ｓ_２
Ｓ_ｎ＝ｓｉｎ（θ_ｎ）、Ｃ_ｎ＝ｃｏｓ（θ_ｎ）、ｎは自然数、
Ｓ_３４５＝ｓｉｎ（θ_３＋θ_４＋θ_５）、Ｃ_３４５＝ｃｏｓ（θ_３＋θ_４＋θ_５）
である。

肩４３、手先４１の座標についても、順変換計算式を用いて計算することができる。
図１４は、支持多角形１００と床面投影点１０３との関係を説明するための図である。床面投影点１０３は、上述したｘ、ｙ座標を用いることによって、全身の重心位置を床面に投影した位置（ｘ、ｙ）として表すことができる。支持多角形１００は、両足裏で構成される多角形であり、床面投影点１０３が支持多角形内の所定の範囲内にあるか否かで、腰４５の参照位置の水平成分の位置を求める。

ロボットの重心位置、つまり床面投影点１０３が、支持多角形１００の中心点１０２から所定以上の距離、つまり範囲１０１外か否か判定する。範囲１０１外のときは、腰４５の参照位置の水平成分を、支持多角形１００の中心方向１０５に、中心からの距離Ｌ_Ｍに比例した速度で移動する位置とする。範囲１０１外でないときは、腰４５の現在位置の水平成分を腰４５の参照位置の水平成分とする。

図１５は、図２に示すロボット制御方法での腰４５の目標位置の計算のフローチャートである。腰４５の目標位置の計算を行うときに本処理が行われる。ステップＳ４０では、腰４５の参照位置が腰４５の後述する移動許諾範囲内か否か判定する。移動許諾範囲内のときは、ステップＳ４１に進み、移動許諾範囲内にないときは、ステップＳ４２に進む。ステップＳ４１では、腰４５の参照位置を腰４５の目標位置として終了する。ステップＳ４２では、腰４５の移動許諾範囲内であって、腰４５の現在位置から水平方向で、腰４５の参照位置に最も近い位置を、腰４５の目標位置として終了する。

図１６は、代表点の移動許諾範囲を説明するための図である。図１６（ａ）は、腰４５の移動許諾範囲を説明するための図である。腰４５の移動許諾範囲１１０は、腰４５が移動することができる移動可能範囲内の所定の範囲である。この場合、腰４５の参照位置１１１が移動許諾範囲１１０内にないので、腰４５の目標位置は、腰４５の水平方向１１３の方向で、かつ移動許諾範囲１１０内で、参照位置１１１に最も近い位置１１２を目標位置とする。腰４５の参照位置が移動許諾範囲１１０内のときは、参照位置がそのまま目標位置になる。移動許諾範囲１１０は、移動可能範囲１１４の所定の範囲である。

図１６（ｂ）は、肩４３の動作許諾範囲を説明するための図である。肩４３の移動許諾範囲１２０は、肩４３が移動することができる移動可能範囲内の所定の範囲である。この場合、肩４３の参照位置１２１が移動許諾範囲１２０内にないので、肩４３の目標位置は、移動許諾範囲１２０内の参照位置１２１に最も近い位置１２２を目標位置とする。肩４３の参照位置が移動許諾範囲１２０内のときは、参照位置がそのまま目標位置になる。移動許諾範囲１２０は、移動可能範囲１２３の所定の範囲である。

図１７は、図２に示すロボット制御方法での肩４３の目標位置の計算のフローチャートである。肩４３の目標位置の計算を行うときに本処理が行われる。ステップＳ５０では、肩４３の参照位置が肩４３の移動許諾範囲内か否か判定する。移動許諾範囲内のときは、ステップＳ５１に進み、移動許諾範囲内にないときは、ステップＳ５２に進む。ステップＳ５１では、肩４３の参照位置を肩４３の目標位置として終了する。ステップＳ５２では、肩４３の移動許諾範囲内で、肩４３の参照位置に最も近い位置を、肩４３の目標位置として終了する。

図１８は、図２に示すロボット制御方法での手先４１の目標位置の計算のフローチャートである。手先４１の目標位置の計算を行うときに本処理が行われる。手先４１の移動許諾範囲は、手先４１が移動することができる移動可能範囲内の所定の範囲である。ステップＳ６０では、手先４１の参照位置が手先４１の移動許諾範囲内か否か判定する。移動許諾範囲内のときは、ステップＳ６１に進み、移動許諾範囲内にないときは、ステップＳ６２に進む。ステップＳ６１では、手先４１の参照位置を手先４１の目標位置として終了する。ステップＳ６２では、手先４１の移動許諾範囲内で、手先４１の参照位置に最も近い位置を、手先４１の目標位置として終了する。

隣接するサブシステムの代表点の目標位置の計算を、優先度の高い動作目標が設定されたサブシステムの目標位置を基準に計算しているので、すなわち、他のサブシステムとの関係を考慮する必要がないので、計算量を少なくでき、ロボットの各軸の値を短時間に求めることができる。

さらに、以前に求めた目標位置と以前に求めた各軸の値とに基づいて新たな目標位値を求めるので、動作しているロボットの現在の状態に基づいた次の目標位置を計算できる。

さらに、優先度の高い動作目標が設定されたサブシステムの代表点の移動可能範囲に対する移動許諾範囲の割合を、優先度の低い動作目標が設定されたサブシステムより小さくしているので、優先度の高い動作目標が設定されたサブシステムの代表点の目標位置の影響を少なくすることができる。

代表点の移動可能範囲に対する移動許諾範囲の割合は、腰４５の割合は肩４３の割合より小さく、肩４３の割合は手先４１の割合より小さくする。たとえば、腰４５、肩４３、手先４１の割合が、それぞれ７０％、８０％、９０％という割合である。これは、優先度の高い動作目標が設定されたサブシステムから目標位置を決めるので、腰４５の影響が、肩４３の影響より大きく、肩４３の影響が手先４１の影響より大きくなるためであり、影響の大きいサブシステムの移動許諾範囲を厳しくしている。

腰の移動可能範囲に対する移動許諾範囲の割合を肩より小さくし、かつ肩の移動可能範囲に対する移動許諾範囲の割合を手先より小さくしているので、優先度の高い動作目標が設定された脚部の代表点である腰の目標位置の影響を肩および手先より、および肩の目標位置の影響を手先より少なくすることができる。

このように、目標位置の計算は、参照位置が移動許諾範囲内にあれば、その参照位置を目標値とするので、計算量を少なくすることができる。サブシステム毎に計算した参照位置が代表点の移動許諾範囲内でないときも、移動許諾範内の位置に目標位置を計算するので、許容される範囲内で動作が可能な各軸の値を短時間に求めることができる。つまり、従来技術のように最適な値を求めるのではなく、一定の評価基準の範囲内にあればよいとして計算するので、ロボットの実時間制御を行うことができる。

手先４１、肩４３、および腰４５の目標位置が計算されると、これらの目標位置と足の位置との情報に基づいて、ロボットの各軸の値、たとえば角度が、逆運動学計算によって求められる。逆運動学計算は、ロボット各部のリンク構造に依存した幾何学的関係式を利用して、変換式を作成して計算する。具体的には、上述した順変換計算式（３）と、リンク構造に依存して導かれるリンクの位置関係式から、各軸の角度を一意に計算することができる式を解くことによって、ロボットの各軸の値を求める。

手先４１、肩４３、および腰４５の目標位置は、ロボットの基準座標系Σ_０からみた位置姿勢であるので、たとえば腕部３２の逆変換を行なう場合は、肩４３の座標系から見た手先４１の座標系の位置姿勢を計算して、これを腕の逆変換計算式に用いることで、腕の各軸の角度を計算する。具体例として、図１３を用いて順変換について説明した脚部３４の逆変換について説明する。基準座標系Σ_０からみた足首の座標系Σ_ｆの位置姿勢^０Ｔ_ｆが与えられたときのジョイント３４１〜ジョイント３４６の角度θ_１〜角度θ_６を求める。

基準座標系Σ_０から基準座標系Σ_０からみた足首の座標系Σ_ｆの位置姿勢^０Ｔ_ｆは、式（３）で与えられ、行列Ｒを式（４）で表すと、式(５)の関係式が成立する。

このとき、足首の座標（ｘ、ｙ、ｚ）には、次の関係がある。
ｘ＝−Ｌ_３Ｓ_５−Ｌ_２Ｓ_４５ …（６）
ｙ＝（Ｌ_３Ｃ_５＋Ｌ_２Ｃ_４５）Ｓ_６＋Ｌ_１Ｃ_６ …（７）
ｚ＝（Ｌ_３Ｃ_５＋Ｌ_２Ｃ_４５）Ｃ_６−Ｌ_１Ｓ_６ …（８）

式（７）と式（８）から次式が成立し、ジョイント３４６の角度θ_６が求まる。
Ｃ_６ｙ−Ｓ_６ｚ＝Ｌ_１

式（６）〜式（８）から次式が成立し、ジョイント３４４の角度θ_４が求まる。
ｘ^２＋ｙ^２＋ｚ^２＝Ｌ_１ ^２＋Ｌ_２ ^２＋Ｌ_３ ^２＋２Ｌ_２Ｌ_３Ｃ_４

式（６）を変形すると、次式が成り立ち、同様に式（８）を変形し、角度θ_４と角度θ_６を代入してＳ_５とＣ_５を計算することができる。
ｘ＝（−Ｌ_３−Ｌ_２Ｃ_４）Ｓ_５−Ｌ_２Ｓ_４Ｃ_５

Ｓ_５とＣ_５からジョイント３４５の角度θ_５を計算することができる。
θ_５＝−ａｔａｎ２（Ｓ_５，Ｃ_５）

次に、上述した順変換の関係式から、次式が成り立ち、ジョイント３４２の角度θ_２を計算することができる。

Ｓ_２＝Ｒ_３２Ｃ_６−Ｒ_３３Ｓ_６
同様に上述した順変換の関係式から、次式が成り立ち、Ｓ_３４５とＣ_３４５を計算することができる。
Ｓ_３４５＝−Ｒ_３１／Ｃ_２
Ｃ_３４５＝（Ｒ_３３＋Ｓ_２Ｓ_６）／（Ｃ_２Ｃ_６）

Ｓ_３４５とＣ_３４５から計算したθ_３４５にθ_４とθ_５を代入して、ジョイント３４３の角度θ_３を計算することができる。
θ_３４５＝ａｔａｎ２（Ｓ_３４５，Ｃ_３４５）＝θ_３＋θ_４＋θ_５

上述した順変換の関係式から、次式が成り立ち、Ｓ_１とＣ_１を計算することができる。
Ｓ_１＝（−Ｓ_２Ｓ_３４５Ｒ_１１＋Ｃ_３４５Ｒ_２１）
／（Ｃ_３４５ ^２＋Ｓ_２ ^２Ｓ_３４５ ^２）
Ｃ_１＝（Ｃ_３４５Ｒ_１１＋Ｓ_２Ｓ_３４５Ｒ_２１）
／（Ｃ_３４５ ^２＋Ｓ_２ ^２Ｓ_３４５ ^２）

Ｓ_１とＣ_１から計算してジョイント３４１の角度θ_１を計算することができる。
θ_１＝ａｔａｎ２（Ｓ_１，Ｃ_１）

図１９は、図２に示すロボット制御方法での参照位置の計算から各軸の値の計算までのアルゴリズムを示す図である。第１の参照位置計算工程７０は、手先４１の参照位置の計算を行う工程であり、遠隔操作装置から取得した作業目標である手先４１の指示位置が入力として与えられると、与えられた手先４１の指示位置を手先４１の参照位置として出力する。第２の参照位置計算工程７１は、肩４３の参照位置の計算を行う工程であり、手先４１の指示位置と目標位置保持工程７６で保持されている手先４１の現在位置と肩４３の現在位置が入力として与えられると、それらの情報に基づいて肩４３の参照位置を計算して出力する。

第３の参照位置計算工程７２は、腰４５の参照位置の計算を行う工程であり、目標位置保持工程７６で保持されている肩４３の現在位置と腰４５の現在位置、および重心位置計算工程７８で計算された現在の重心位置が入力として与えられると、それらの情報に基づいて腰４５の参照位置を計算して出力する。先４１、肩４３、および腰４５の参照位置の計算は、並行して計算される。

サブシステム毎の代表点の参照位置を並行して計算するので、参照位置の計算時間を短時間に行うことができ、さらにロボットの各軸の値を短時間に求めることができる。

第１の目標位置計算工程７３は、腰４５の目標位置の計算を行う工程であり、第３の参照位置計算工程７２の出力である腰４５の参照位置と、基準の位置である固定されている足の位置とが入力として与えられると、腰４５の目標位置を計算して出力する。第２の目標位置計算工程７４は、肩４３の目標位置の計算を行う工程であり、第２の参照位置計算工程７１の出力である肩４３の参照位置と、第１の目標位置計算工程７３の出力である腰４５の目標位置とが入力として与えられると、肩４３の目標位置を計算して出力する。

第３の目標位置計算工程７５は、手先４１の目標位置の計算を行う工程であり、第１の参照位置計算工程７０の出力である手先４１の参照位置と、第２の目標位置計算工程７４の出力である肩４３の目標位置とが入力として与えられると、手先４１の目標位置を計算して出力する。

目標位置保持工程７６では、第３の目標位置計算工程７５の出力である手先４１の目標位置、第２の目標位置計算工程７４の出力である肩４３の目標位置、および第１の目標位置計算工程７３の出力である腰４５の目標位置が入力として与えられると、それらの情報を保持するとともに出力する。各軸値計算工程７７は、腕部３２、胴体部３３、脚部３４の各軸の値を計算する工程であり、目標位置保持工程７６の出力である手先４１の目標位置、肩４３の目標位置、腰４５の目標位置、および足の位置が入力として与えられると、逆運動学計算を行ってロボットの各軸の値を求めて、求めた各軸の値をロボットの各軸に指令する。各軸値計算工程７７の出力は、重心位置計算工程７８に入力される。重心位置計算工程７８では、入力されたロボットの各軸の値に基づいてロボットの重心位置を計算する。各軸値計算工程７７での各軸の値の計算は、サブシステム毎に並行して行う。サブシステム内の各軸の値をサブシステム毎に並行して計算するので、ロボット全体の各軸の値の計算時間を短時間に行うことができる。

上述した実施の形態は、ロボット動作の動的な動きに大きく影響しない程度の速度で動作を行うと仮定した場合、つまり、ロボットの動作に伴う慣性を考慮しなくてもよい程度の速度で動作を行うと仮定した場合について、ロボットの全体のバランスを維持することができる。ロボットの動作を速く行うためには、ロボットの動作に伴う動的要素、つまり慣性を考慮する必要がある。目標位置を計算する際に、計算した目標位置を、ロボットの動作に伴う重心移動の慣性に基づいて、目標位置を再計算することによって、動的要素を考慮したロボットの各軸の値を計算することができる。

ロボットの動作に起因する動的要素、たとえば、動作しているロボットの慣性を考慮して目標位置を再計算するので、より安定した目標位置を計算でき、ロボットの安定性を向上することができる。

上述した実施の形態では、頭部３１をサブシステムとして含めていないが、頭部３１をサブシステムとして含める場合は、優先度を低くした動作目標を「頭部の動作目標が作業に必要な視野を確保すること」とし、この場合は、胴体部３３は「頭部の動作範囲も広く保つこと」も動作目標とする。人間型ロボットの頭部もサブシステムとして分割すれば、頭部の動作目標を実現することができ、頭部の動作範囲を広く保つことができる。

図２０は、本発明の実施の他の形態であるロボット制御方法が適用されるロボット２００のサブシステムへの分割の一例を示す図である。ロボット２００は、サブシステムＡ２０１、サブシステムＢ２０２、およびサブシステムＣ２０３を含んで構成される。サブシステムＡ２０１は、リンク２２０と連結部２１０、サブシステムＢ２０２は、リンク２２１と連結部２１１、およびサブシステムＣ２０３は、リンク２２２と連結部２１２を含み、連結部２１３によって床面などの水平面２３０に固定されている。これらのサブシステムの代表点は、それぞれ連結部２１０、２１１、２１２である。この例では、１つのリンクには、複数のリンクを代表して表現している。

これらのサブシステムは、サブシステムＣ２０３を最下層とする階層構造を形成している。たとえば人間型ロボットであれば、サブシステムＡ２０１が腕部３２、サブシステムＢ２０２が胴体部３３、およびサブシステムＣ２０３が脚部３４に対応し、それぞれの代表点は、手先４１、肩４３、および腰４５に対応する。

代表点を移動させる位置である参照位置は、「作業目標に応じて評価がよくなるような位置」である。たとえば、人間型ロボットであれば、サブシステムＣ２０３に対応する脚部３４の実現すべき動作目標を「全身のバランスをとること」とした場合、代表点である連結部２１２に対応する腰４５の参照位置は、全体の重心が前方に移動したときは、腰４５の位置を後方に引くように、腰４５の現在位置より後方の位置を腰４５の参照位置とする。このとき、各サブシステムの代表点の参照位置は、独立に計算する。

これらのサブシステムは、お互いに連結部を有するが、それぞれの参照位置を独立に計算しているので、各サブシステムの制約条件、具体的には、隣接するサブシステムの代表点の位置、リンクの構造、代表点の移動許諾範囲、周囲との干渉回避などの制約条件によって、お互いの連結部が計算した参照位置で実現できない場合がある。

たとえば人間型ロボットの場合、代表点である連結部２１０に対応する手先４１が、前方に移動しようとするとき、サブシステムＢ２０２に対応する胴体部３３は手先４１の動作範囲を拡大することができるように、代表点である連結部２１１に対応する肩４３の参照位置を手先４１が移動する方向に変更する。サブシステムＣ２０３に対応する脚部３４は、全体のバランスを維持するために、前方に移動している全体の重心位置を基準位置２１３に戻すために、代表点である連結部２１２に対応する腰４５の参照位置を後方に変更する。

このとき、全体のバランスを維持することがロボットの最重要の動作目標であるので、まず、代表点である連結部２１２に対応する腰４５の参照位置が実現可能か否か判定する。腰４５の参照位置が腰４５の移動許諾範囲内にあるときは、腰４５の参照位置は実現可能であるので、腰４５の参照位置を腰４５の目標位置とする。

次に、代表点である連結部２１１に対応する肩４３の参照位置が実現可能か否か判定する。肩４３の参照位置が肩４３の移動許諾範囲内にあるときは、肩４３の参照位置は実現可能であるので、肩４３の参照位置を肩４３の目標位置とする。腰４５の参照位置あるいは肩４３の参照位置が、それぞれの移動許諾範囲内にないときは、それぞれの参照位置に最も近い移動許諾範囲内の位置を目標位置とする。

このように、階層構造の下位のサブシステムの代表点を基準に１つ上のサブシステムの制約条件を考慮して、代表点の目標位置を決めていく。すべての代表点の目標位置が決まれば、逆運動計算を行い、サブシステムの各軸の値を計算する。

図２１は、本発明の実施の他の形態であるロボット制御方法の工程を示すフローチャートである。本処理は、ジョイスティックなどの入力装置である遠隔操作装置などから作業目標が与えられたときに開始される。ステップＳ７０では、指示された作業目標を取得する。ステップＳ７１では、それぞれのサブシステムに設定された動作目標を満たす条件に合致する位置を、それぞれの参照位置として計算する。

ステップＳ７２では、計算された参照位置に基づいて、作業目標を達成するために設定された動作目標の優先度に従って、各サブシステムの代表点の目標位置を計算する。ステップＳ７３では、計算された目標位置に基づいてロボットの各軸の値を計算する。ステップＳ７４では、計算された各軸の値をロボットの各軸に指令して終了する。

図２２は、図２１に示すロボット制御方法での参照位置の計算から各軸の値の計算までのアルゴリズムを示す図である。遠隔操作指示入力工程２５０では、遠隔操作装置などから与えられた作業目標を取得すると、取得した作業目標をサブシステムＡ参照位置計算工程２５２に入力する。サブシステムＡ参照位置計算工程２５２では、入力された作業目標に基づいて、サブシステムＡに設定された動作目標を満たす条件に合致する位置を、サブシステムＡの代表点の参照位置として計算する。

各代表点現在位置工程２５１では、各サブシステムの代表点の現在位置を前回計算した目標位置、つまり、サブシステムＡ目標位置計算工程２５７、サブシステムＢ目標位置計算工程２５６、およびサブシステムＣ目標位置計算工程２５５の出力である目標位置を取得して、取得した各サブシステムの代表点の現在位置をサブシステムＢ参照位置計算工程２５３とサブシステムＣ参照位置計算工程２５４に入力する。サブシステムＢ参照位置計算工程２５３とサブシステムＣ参照位置計算工程２５４では、それぞれ、入力された各サブシステムの代表点の現在位置に基づいて、各サブシステムに設定された動作目標を満たす条件に合致する位置を、各サブシステムの代表点の参照位置として計算する。サブシステムＡ参照位置計算工程２５２、サブシステムＢ参照位置計算工程２５３、およびサブシステムＣ参照位置計算工程２５４での計算は、並行して実行される。

サブシステムＣ目標位置計算工程２５５では、基準位置と、サブシステムＣ参照位置計算工程２５４から出力された参照位置とに基づいて、参照位置が実現可能か否か、つまり参照位置がサブシステムＣの代表点の移動許諾範囲に入っているか否か判定する。参照位置が実現可能、つまり参照位置が移動許諾範囲に入っているときは、その参照位置をサブシステムＣの代表点の目標位置とする。参照位置が実現可能ではない、つまり参照位置が移動許諾範囲に入っていないときは、移動許諾範囲の中で、参照位置にもっとも近い位置を目標位置とする。

サブシステムＢ目標位置計算工程２５では、サブシステムＣ目標位置計算工程２５５から出力された目標位置と、サブシステムＢ参照位置計算工程２５３から出力された参照位置とに基づいて、参照位置が実現可能か否か、つまり参照位置がサブシステムＢの代表点の移動許諾範囲に入っているか否か判定する。参照位置が実現可能、つまり参照位置が移動許諾範囲に入っているときは、その参照位置をサブシステムＢの代表点の目標位置とする。参照位置が実現可能ではない、つまり参照位置が移動許諾範囲に入っていないときは、移動許諾範囲の中で、参照位置にもっとも近い位置を目標位置とする。

サブシステムＡ目標位置計算工程２５７では、サブシステムＢ目標位置計算工程２５６から出力された目標位置と、サブシステムＡ参照位置計算工程２５２から出力された参照位置とに基づいて、参照位置が実現可能か否か、つまり参照位置がサブシステムＡの代表点の移動許諾範囲に入っているか否か判定する。参照位置が実現可能、つまり参照位置が移動許諾範囲に入っているときは、その参照位置をサブシステムＡの代表点の目標位置とする。参照位置が実現可能ではない、つまり参照位置が移動許諾範囲に入っていないときは、移動許諾範囲の中で、参照位置にもっとも近い位置を目標位置とする。この場合、各サブシステムに設定された動作目標の優先度が、サブシステムＣ、サブシステムＢ、サブシステムＡの順序であるので、目標位置の計算は、その順序で行われる。

各サブシステム各軸指令値計算工程２５８では、サブシステムＡ目標位置計算工程２５７、サブシステムＢ目標位置計算工程２５６、およびサブシステムＣ目標位置計算工程２５５から出力されたそれぞれのサブシステムの目標位置を基に、ロボットの各軸の値を計算する。計算した各軸の値は、ロボットの各軸に指令される。

このように、本発明の実施の形態によれば、人間型ロボットをサブシステムに分割することによって、作業目標に対する計算を並行処理することができ、さらに最適値を検索するような繰り返し計算を行うことがないので、ロボットを制御するための計算量を少なくすることができる。サブシステム毎に用意する参照位置の計算アルゴリズムを、作業目標に応じて切替えて組み合わせることによって、多様な作業目標に対応することが可能である。

上述したいずれのロボット制御方法も、それぞれのロボット制御方法をコンピュータに実行させるプログラムとして実現される。コンピュータは、種々の制御を行う図示していないＣＰＵ（Central Processing Unit）と、ＣＰＵで処理が行われるために用いられる図示していないメモリ、たとえば、ＲＯＭ（Read Only Memory）またはＲＡＭ（Random
Access Memory）を含んで構成され、ロボット制御方法をコンピュータに実行させるプログラムは、このメモリに格納される。

ロボット制御方法をコンピュータに実行させるプログラムは、コンピュータで読取り可能な記録媒体に格納されておればよく、上述した実施の形態では、記録媒体として、メモリを用いたが、外部記憶装置としてプログラム読取装置が設けられ、そこに記録媒体を挿入することによって読取り可能となるような記録媒体であってもよい。記録媒体に格納されているプログラムは、ＣＰＵがアクセスして実行可能な構成であればよい。あるいは、ロボット制御方法をコンピュータに実行させるプログラムは、ネットワークに接続されているコンピュータの記憶装置などに格納されておればよく、記録媒体あるいは記憶装置などに格納されているプログラムを読出し、読出したプログラムを図示されていないプログラム記録エリアにダウンロードして、プログラムを実行する構成であればよい。このダウンロード用のプログラムは、予めＲＯＭに格納しておく。

記録媒体がコンピュータと分離可能に構成される場合、記録媒体は、磁気テープ／カセットテープなどのテープ系、フレキシブルディスク／ハードディスクなどの磁気ディスクまたはＣＤ−ＲＯＭ（Compact Disk-Read Only Memory）／ＭＯ（Magneto Optical Disk）／ＭＤ（Mini Disk）／ＤＶＤ（Digital Versatile Disk）などの光ディスクのディスク系、ＩＣ（Integrated Circuit）カード（メモリカードを含む）／光カードなどのカード系、あるいはマスクＲＯＭ／ＥＰＲＯＭ（Erasable Programmable Read Only Memory）／フラッシュＲＯＭなどによる半導体メモリを含め、固定的にプログラムを保持する記録媒体であればよい。

上述した実施の形態は、人間型ロボットについて説明したが、多くの自由度を有するロボットについても同様に適用できる。特に、２０自由度以上のロボットについては、計算時間を短縮できる効果が大きい。繰り返し計算を行わないので、２０自由度以上の自由度を有するロボットについて、従来技術による計算時間に比べて、ロボットの各軸の値をより短時間に求めることができる。３０自由度以上の人間型ロボットに適用しても、サブシステムの自由度を６自由度以下とし、サブシステム内の自由度が少ないので、冗長な自由度が少なくなり、サブシステム内の各軸の値の計算時間を短くでき、さらにロボットの各軸の値を短時間に求めることができる。

上述した人間型ロボットでは、各軸の値は、角度であったが、直線方向に変位する軸を有するロボットについては、角度の代わりに、直線方向の変位量を用いれば適用可能である。

図２３は、本発明を適用することができる他のロボットシステム１５０の構成の一例を示す図である。ロボットシステム１５０は、門構形移動装置１５１と門構形移動装置１５１に搭載された６軸垂直多関節型ロボット１５２から構成される。門構形移動装置１５１は、レール１５３とレール１５４に沿って移動することができる。６軸垂直多関節型ロボット１５２は、溶接などを行うツールが手首に取り付けられ、ライン１５５上を移動するワーク１６０に対する加工を行うことができる。

門構形移動装置１５１のリンクは、可動部１５１ａ〜可動部１５１ｃであり、連結部は、レール１５３，１５４と可動部ａとの連結部、可動部１５１ａと可動部１５１ｂとの連結部、可動部１５１ｂと可動部１５１ｃとの連結部、および可動部１５１ｃと６軸垂直多関節型ロボット１５２との連結部である。門構形移動装置１５１は、走行軸１７０、横行軸１７１、昇降軸１７２、および旋回軸１７３の４軸を有し、６軸垂直多関節型ロボット１５２は、６軸を有するので、ロボットシステム１５０は、自由度１０のロボットである。この場合、サブシステムとして、門構形移動装置１５１と６軸垂直多関節型ロボット１５２との２つに分割し、つまり自由度４と自由度６の２つのサブシステムに分割する。

ロボットシステム１５０への作業目標は、たとえばツールをワーク１６０の加工位置に移動することである。動作目標として、たとえば「手先位置の作業範囲を拡大すること」と「干渉を回避すること」とし、優先度は、「干渉を回避すること」、「手先位置の作業範囲を拡大すること」の順序として設定することによって、本発明を適用することができる。

さらに、宇宙ステーションなどで作業を行うロボットにも適用可能である。この場合、ロボットは、宇宙ステーションを構成するトラス構造の構造物などにつかまって作業を行う。つまり、トラスにつかまった腕で体を支えて、他の腕によって作業を行う。このロボットは、人間型ロボットの変形であり、自由度も多く、本発明を適用することができる。この場合、体を支える腕を１つのサブシステムとし、作業を行う腕を１つのサブシステムとする。作業目標は、作業を行う腕の手先を作業位置に移動することであり、動作目標としては、たとえば、「トラスをつかむこと」、「干渉を回避すること」、および「手先位置の作業範囲を拡大すること」を設定する。優先度は、「干渉を回避すること」を優先する。

海中で作業を行うロボットについても、海中に浮いているロボットの体を移動しない状態に保ちながら、作業を行うことになり、本発明を適用することができる。この場合、動作目標としては、たとえば、「海流に流されないことおよび腕などの動作の反動で体が移動しないこと」と「手先位置の作業範囲を拡大すること」を設定する。

本発明の実施の一形態であるロボット制御方法が適用されるロボットシステム１０の構成を示す図である。 本発明の実施の一形態であるロボット制御方法の工程を示すフローチャートである。 図２に示すロボット制御方法が適用されるロボット３０のサブシステムへの分割の一例を示す図である。 人間型ロボットの各部の自由度を説明するための図である。 図２に示すロボット制御方法が適用されるロボット３０への作業目標の指示とその指示によるロボットの変位の一例を示す図である。 図２に示すロボット制御方法での手先４１の参照位置の計算のフローチャートである。 図２に示すロボット制御方法での肩４３の参照位置の計算のフローチャートである。 図２に示すロボット制御方法での肩４３の参照位置の計算を説明するための図である。 図２に示すロボット制御方法での腰４５の参照位置の計算のフローチャートである。 腰４５の移動を肩４３の移動と連動させることについて説明するための図である。 リンクの重心位置の求め方を説明するための図である。 ロボットの座標系を説明するための図である。 脚部３４の各軸の値から位置姿勢を求める際の各軸の座標系を説明するための図である。 支持多角形１００と床面投影点１０３との関係を説明するための図である。 図２に示すロボット制御方法での腰４５の目標位置の計算のフローチャートである。 代表点の移動許諾範囲を説明するための図である。 図２に示すロボット制御方法での肩４３の目標位置の計算のフローチャートである。 図２に示すロボット制御方法での手先４１の目標位置の計算のフローチャートである。 図２に示すロボット制御方法での参照位置の計算から各軸の値の計算までのアルゴリズムを示す図である。 本発明の実施の他の形態であるロボット制御方法が適用されるロボット２００のサブシステムへの分割の一例を示す図である。

本発明の実施の他の形態であるロボット制御方法の工程を示すフローチャートである。 図２１に示すロボット制御方法での参照位置の計算から各軸の値の計算までのアルゴリズムを示す図である。 本発明を適用することができる他のロボットシステム１５０の構成の一例を示す図である。 人間型ロボットの概略の構成を示す図である。 評価関数を用いてロボットの各軸の値を求める方法を説明するための図である。

符号の説明

１，３０ ロボット
２，３１ 頭部
３，３２ 腕部
４，３３ 胴体部
５，３４ 脚部
６ 手部
１０ ロボットシステム
１１ ロボット部
１２，２２ 制御部
１３，２３ 通信部
１４ 撮像部
２０ 遠隔操作装置
２１ 入力部
２４ 表示部
４１ 手先
４２ 肘
４３ 肩
４４ 胸
４５ 腰
４６ 膝
４７ 足
５１〜５６ リンク
１５０ ロボットシステム
１５１ 門構形移動装置
１５２ ６軸垂直多関節型ロボット
１５３，１５４ レール
１５５ ライン
１６０ ワーク
１７０ 走行軸
１７１ 横行軸
１７２ 昇降軸
１７３ 旋回軸

Claims (18)

1. 複数のリンクと各リンクを相互に連結する複数の連結部によって構成されるロボットにこのロボットが行うべき動作を示す作業目標情報が指示されたときに、前記ロボットを制御対象として分割した複数の制御対象領域毎に、前記作業目標情報を達成するために優先度が設定された複数の動作条件を、前記作業目標情報に応じて抽出する抽出工程と、
   前記制御対象領域毎に、前記制御対象領域に含まれる部位のうちで前記制御対象領域の移動先位置が計算される部位である代表点が前記動作条件に合致する位置を、前記代表点の移動先位置の候補である参照位置として計算する参照位置計算工程と、
   前記参照位置計算工程で代表点毎に計算された参照位置に基づいて、前記動作条件の優先度に従って、前記各代表点を移動させるべき位置である目標位置を計算する目標位置計算工程と、
   前記目標位置計算工程で計算された目標位置に基づいて前記ロボットの各軸の値を計算する各軸値計算工程と、
   前記各軸値計算工程で計算された各軸の値をロボットの各軸に指令する指令工程とを含むことを特徴とするロボット制御方法。
2. 前記目標位置計算工程では、前記参照位置計算工程で代表点毎に計算された参照位置が、各代表点が移動することができる移動可能範囲内の所定の範囲である移動許諾範囲内にあるときは、各代表点の参照位置を各代表点の目標位置として計算し、
   前記参照位置計算工程で代表点毎に計算された参照位置が対応する代表点の移動許諾範囲内にないときは、前記移動許諾範囲内の位置であって、各制御対象領域の所定の条件を満たす位置を、各代表点の目標位置として計算することを特徴とする請求項１に記載のロボット制御方法。
3. 前記目標位置計算工程では、優先度の高い動作条件が設定された制御対象領域の代表点の目標位置を計算し、次に優先度の高い動作条件が設定された制御対象領域の代表点の目標位置を基準として、隣接する制御対象領域の代表点の目標位置を計算することを特徴とする請求項１または２に記載のロボット制御方法。
4. 前記各代表点の移動許諾範囲を対応する移動可能範囲で除した割合が、優先度の高い動作条件が設定された制御対象領域のほうが優先度の低い動作条件が設定された制御対象領域より小さいことを特徴とする請求項１〜３のいずれか１つに記載のロボット制御方法。
5. 前記参照位置計算工程では、以前前記ロボットに指示された作業目標情報に対して前記目標位置計算工程で計算された目標位置と前記各軸値計算工程で計算された各軸の値に基づいて参照位置が計算されることを特徴とする請求項１〜４のいずれか１つに記載のロボット制御方法。
6. 前記目標位置計算工程では、計算された目標位置を、以前前記指令工程で各軸の値をロボットの各軸に指令したことによって生じたロボットの動作に起因する動的要素の情報に基づいて再計算することを特徴とする請求項５に記載のロボット制御方法。
7. 前記参照位置計算工程では、前記制御対象領域毎の代表点の参照位置を並行して計算することを特徴とする請求項１〜６のいずれか１つに記載のロボット制御方法。
8. 前記各軸値計算工程では、前記制御対象領域内の各軸の値を制御対象領域毎に並行して計算することを特徴とする請求項１〜７のいずれか１つに記載のロボット制御方法。
9. 前記ロボットは、２０自由度以上の自由度を有することを特徴とする請求項１〜８のいずれか１つに記載のロボット制御方法。
10. 前記制御対象領域の自由度は、６自由度以下であることを特徴とする請求項１〜９のいずれか１つに記載のロボット制御方法。
11. 前記ロボットは、人間の腕に相当する腕部、人間の胴体に相当する胴体部、人間の脚に相当する脚部とを含む人間型ロボットであり、前記腕部、前記胴体部、および前記脚部をそれぞれ制御対象領域とすることを特徴とする請求項１〜１０のいずれか１つに記載のロボット制御方法。
12. 前記人間型ロボットの制御対象領域毎の動作条件が、腕部は作業目標情報に応じた動作を実現すること、胴体部は腕部の動作範囲を広く保つこと、脚部は全体のバランスを維持することおよび胴体部の動作範囲を広く保つことであることを特徴とする請求項１１に記載のロボット制御方法。
13. 前記人間型ロボットは、人間の頭に相当する頭部を含み、頭部の動作条件が作業に必要な視野を確保すること、および前記胴体部は頭部の動作範囲も広く保つことであることを特徴とする請求項１１または１２に記載のロボット制御方法。
14. 前記人間型ロボットは、前記腕部の代表点が手先、前記胴体部の代表点が肩、および前記脚部の代表点が腰であり、前記制御対象領域の代表点の移動許諾範囲の移動可能範囲に対する割合が、腰の代表点の割合が肩部の代表点の割合より小さく、かつ肩の代表点の割合が手先の代表点の割合より小さいことを特徴とする請求項１１〜１３のいずれか１つに記載のロボット制御方法。
15. 前記参照位置計算工程で、腰の参照位置を計算する際、腰の速度が肩の速度に比例した速度に計算することを特徴とする請求項１４に記載のロボット制御方法。
16. 複数のリンクと各リンクを相互に連結する複数の連結部によって構成されるロボットに、請求項１〜１５のいずれか１つに記載のロボット制御方法を実行するコンピュータを搭載したことを特徴とするロボットシステム。
17. 前記ロボットは、作業目標情報を決定するための検知手段を有し、前記検知手段で検知された情報に基づいて作業目標情報を生成し、生成された作業目標情報を前記ロボットの作業目標情報とすることを特徴とする請求項１６に記載のロボットシステム。
18. 複数のリンクと各リンクを相互に連結する複数の連結部によって構成されるロボットを、制御対象とする複数の制御対象領域に分割する分割工程と、
    前記制御対象領域毎に、前記制御対象領域に含まれる部位のうちで前記制御対象領域の移動先位置が計算される部位である代表点を決定する決定工程と、
    前記制御対象領域毎に、前記ロボットが行うべき動作を示す作業目標情報を達成するために、優先度を付けた動作条件を設定する設定工程と、
    前記ロボットに作業目標情報が指示されたときに、前記制御対象領域毎に、前記代表点が前記動作条件に合致する位置を、前記代表点の移動先位置の候補である参照位置として計算する参照位置計算工程と、
    前記参照位置計算工程で代表点毎に計算された参照位置に基づいて、前記動作条件の優先度に従って、前記各代表点を移動させるべき位置である目標位置を計算する目標位置計算工程と、
    前記目標位置計算工程で計算された目標位置に基づいて前記ロボットの各軸の値を計算する各軸値計算工程と、
    前記各軸値計算工程で計算された各軸の値をロボットの各軸に指令する指令工程とを含むことを特徴とするロボット指令決定方法。

Images