(57)【要約】【課題】多関節マニピュレータの手先の位置を指定するのみでは対応が難しい環境において、適切な制御を可能とする。【解決手段】多関節マニピュレータに対して、いずれかの位置を指定点として設定する。多関節マニピュレータの運動に関する制御指令値が与えられたとき、指定点において多関節マニピュレータの少なくとも１つの運動の自由度を拘束した被拘束状態で多関節マニピュレータの運動を制御するための被拘束制御指令値を生成する。【選択図】図５
2015174184.tif 000002
【特許請求の範囲】
【請求項１】
互いに接続された複数の関節を有する多関節マニピュレータの制御装置であって、
前記多関節マニピュレータに対して、手先以外の位置を指定点として設定する指定点設定部と、
前記多関節マニピュレータの運動に関する制御指令値が与えられたとき、前記指定点において前記多関節マニピュレータの少なくとも１つの運動の自由度を拘束した被拘束状態で、前記多関節マニピュレータを制御するための被拘束制御指令値を生成する計算部と
を具備する制御装置。
【請求項２】
請求項１に記載された制御装置であって、
更に、前記指定点に対応する指定位置を設定する座標設定部を具備し、
前記被拘束状態は、前記指定点における前記多関節マニピュレータを前記指定位置に固定した状態である
制御装置。
【請求項３】
請求項２に記載された制御装置であって、
更に、前記指定点における前記多関節マニピュレータの姿勢を指定する情報である指定姿勢を設定する姿勢設定部を具備し、
前記計算部は、前記指定点における前記多関節マニピュレータを前記指定姿勢に固定した状態で前記被拘束制御指令値を生成する
制御装置。
【請求項４】
請求項１に記載された制御装置であって、
更に、前記関節のいずれかを指定関節として設定する指定関節設定部を具備し、
前記被拘束状態は、前記指定関節の動きを固定した状態である
制御装置。
【請求項５】
請求項４に記載された制御装置であって、
前記計算部は、前記多関節マニピュレータの中で、前記指定関節から手先側に位置するすべての関節の角度を固定した状態で前記被拘束制御指令値を生成する
制御装置。
【請求項６】
請求項４に記載された制御装置であって、
前記計算部は、前記多関節マニピュレータの中で、前記指定関節から根元側に位置するすべての関節の角度を固定した状態で前記被拘束制御指令値を生成する
制御装置。
【請求項７】
請求項１から６のいずれかに記載された制御装置であって、
前記指定点設定部は、前記多関節マニピュレータが支持する対象物上の位置を前記指定点として設定することが可能である
制御装置。
【請求項８】
請求項１から７のいずれかに記載された制御装置であって、
更に、前記多関節マニピュレータのシミュレーション画像を表示する表示部を具備し、
前記指定点設定部は、画面上の位置を指定するマーカを用いて前記シミュレーション画像における前記多関節マニピュレータの位置を指定する入力操作に基づいて、前記指定点を設定する
制御装置。
【請求項９】
請求項８に記載された制御装置であって、
更に、前記多関節マニピュレータが支持する対象物の形状を検出する検出装置を具備し、
前記シミュレーション画像には、前記多関節マニピュレータの画像と共に、前記検出装置が検出した対象物の形状を示す対象物画像が表示され、
前記指定点設定部は、前記対象物画像上の位置を前記指定点として設定することができる
制御装置。
【請求項１０】
請求項１から９のいずれかに記載された制御装置と、
前記多関節マニピュレータと
を具備するロボットシステム。
【請求項１１】
互いに接続された複数の関節を有する多関節マニピュレータの制御用データ生成方法であって、
前記多関節マニピュレータに対して、手先以外の位置を指定点として設定する工程と、
前記多関節マニピュレータの運動に関する制御指令値が与えられたとき、前記指定点において前記多関節マニピュレータの少なくとも１つの運動の自由度を拘束した被拘束状態で、前記多関節マニピュレータを制御するための被拘束制御指令値を生成する工程と
を具備する制御用データ生成方法。
【請求項１２】
請求項１１に記載された制御用データ生成方法であって、
更に、前記指定点に対応する指定位置を設定する工程を具備し、
前記被拘束状態は、前記指定点における前記多関節マニピュレータを前記指定位置に固定した状態である
制御用データ生成方法。
【請求項１３】
請求項１１に記載された制御用データ生成方法であって、
更に、前記関節のいずれかを指定関節として設定する工程を具備し、
前記被拘束状態は、前記指定関節の動きを固定した状態である
制御用データ生成方法。
【請求項１４】
請求項１１から１３のいずれかに記載された制御用データ生成方法をコンピュータに実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの多関節マニピュレータを制御する技術に関する。
【背景技術】
【０００２】
多関節マニピュレータ（多関節アーム）を備えたロボットが知られている。図１は、多関節マニピュレータの参考例を示す。多関節マニピュレータ１０１は、直列的に接続された複数のリンクＬ１０１〜Ｌ１０６を備える。隣接する一対のリンク（例えばリンクＬ１０１とＬ１０２）は、その間に設けられた関節（関節Ｊ１０２）によって互いに可動的に接続される。図１の例では、６つの回転関節（関節Ｊ１０１〜Ｊ１０６）を有する多関節マニピュレータ１０１が描かれている。
【０００３】
具体的には、固定された基部１０２に、支持部１０３の一端が取り付けられる。支持部１０３の他端に、第１の関節Ｊ１０１の一方側が取り付けられる。第１の関節Ｊ１０１の他方側に、第１のリンクＬ１０１の一端が取り付けられる。第１のリンクＬ１０１の他端に、第２の関節Ｊ１０２の一方側が取り付けられる。以下、同様にして、第５のリンクの他端に第６の関節Ｊ１０６の一方側が取り付けられる。第６の関節Ｊ１０６の他方側に第６のリンクＬ１０６の一端が取り付けられる。リンクＬ１０６の他端にエンドエフェクタ１０４が取り付けられる。
【０００４】
図２は、多関節マニピュレータの関節及びリンクの関係をシンボルで表した図である。この図の例では、ｎ個の関節Ｊ１０１〜Ｊ１０ｎと、ｎ個のリンクＬ１０１〜Ｌ１０ｎが描かれている。
【０００５】
作業者は、エンドエフェクタ１０４の先端などに設定される指定点１０５のワールド座標系における位置指令値を制御装置に対して指定する。制御装置は、指定点１０５が位置指令値の方向に移動するように、各関節Ｊ１０１〜Ｊ１０ｎの角度指令値を計算する。各関節Ｊ１０１〜Ｊ１０ｎは、その角度指令値に従ってモータ等により駆動される。このような制御により、多関節マニピュレータ１０１の手先（指定点１０５）を所望の位置に移動することができる。
【０００６】
非特許文献１には、このような多関節マニピュレータの制御についての一般論が記載されている。
【先行技術文献】
【非特許文献】
【０００７】
【非特許文献１】吉川恒夫『ロボット制御基礎論』、コロナ社、１９８８年１１月２５日発行
【発明の概要】
【発明が解決しようとする課題】
【０００８】
上記のように、多関節マニピュレータ１０１は、手先の位置を指定して制御することが多い。そのような制御においては、各関節Ｊ１０１〜Ｊ１０ｎは、指定された手先位置を実現するように、計算に基づいて自動制御される。
【０００９】
ところで、多関節マニピュレータ１０１は、手先の位置を指定するのみでは作業を行うことが難しい場合がある。本願の発明者は、そのような場合における多関節マニピュレータ１０１の制御について開発を進めている。図３は、その一例を示す。多関節マニピュレータ１０１が作業を行う場所が障害物１０６の裏側にある場合、障害物１０６を迂回する姿勢を維持して多関節マニピュレータ１０１の制御を行うことが望まれる。図３の例では、障害物１０６の右側から裏側に回り込むように多関節マニピュレータ１０１の姿勢が維持されている。
【００１０】
図４は、手先の位置を指定するのみでは作業を行うことが難しい場合の他の例を示す。この例では、多関節マニピュレータ１０１の基部１０２から見て、壁１０７の反対側の領域の天井１０９に対する作業が行われる。リンクＬ１０２が、壁１０７の隙間１０８に配置されている。このような場合、天井１０９付近のエンドエフェクタ１０４の位置を指定するのみでは十分ではなく、リンクＬ１０２が隙間１０８の位置を維持することが望まれる。
【００１１】
多関節マニピュレータの手先の位置を指定するのみでは対応が難しい環境において、適切な制御を可能とすることが望まれる。
【課題を解決するための手段】
【００１２】
本発明の一側面において、制御装置は、互いに接続された複数の関節を有する多関節マニピュレータの制御に用いられる。制御装置は、多関節マニピュレータに対して、手先以外の位置を指定点として設定する指定点設定部と、多関節マニピュレータの運動に関する制御指令値が与えられたとき、指定点において多関節マニピュレータの少なくとも１つの運動の自由度を拘束した被拘束状態で、多関節マニピュレータを制御するための被拘束制御指令値を生成する計算部とを備える。
【００１３】
本発明の他の側面において、制御用データ生成方法は、互いに接続された複数の関節を有する多関節マニピュレータの制御用データを生成する。制御用データ生成方法は、多関節マニピュレータに対して、手先以外の位置を指定点として設定する工程と、多関節マニピュレータの運動に関する制御指令値が与えられたとき、指定点において多関節マニピュレータの少なくとも１つの運動の自由度を拘束した被拘束状態で、多関節マニピュレータを制御するための被拘束制御指令値を生成する工程とを備える。
【００１４】
本発明の更に他の側面において、ロボットシステムは、本発明による制御装置と、多関節マニピュレータとを備える。
【００１５】
本発明の更に他の側面において、プログラムは、本発明による制御用データ生成方法をコンピュータに実行させる。
【発明の効果】
【００１６】
多関節マニピュレータの手先の位置を指定するのみでは対応が難しい環境において、適切な制御が可能となる。
【図面の簡単な説明】
【００１７】
【図１】図１は、参考例における多関節マニピュレータを示す。
【図２】図２は、多関節マニピュレータの関節とリンクの関係をシンボルで表した図である。
【図３】図３は、障害物がある場合の制御の一例を示す。
【図４】図４は、障害物がある場合の制御の他の例を示す。
【図５】図５は、本発明の一実施形態における多関節マニピュレータを示す。
【図６】図６は、関節の制御の流れを示す。
【図７】図７は、制御用コンピュータによって実現される機能ブロックを示す。
【図８】図８は、リンク上に描かれた指定点を示す。
【図９】図９は、リンク上に描かれた指定点を示す。
【図１０】図１０は、リンク上に描かれた指定点を示す。
【図１１】図１１は、対象物を把持した多関節マニピュレータを示す。
【図１２】図１２は、手先固定制御について説明するための多関節マニピュレータの図である。
【図１３】図１３は、手先固定制御と根元固定制御のフローチャートである。
【図１４】図１４は、手先固定制御の説明図である。
【図１５】図１５は、手先固定制御の説明図である。
【図１６Ａ】図１６Ａは、指定点の設定方法の説明図である。
【図１６Ｂ】図１６Ｂは、指定点の設定方法の説明図である。
【図１６Ｃ】図１６Ｃは、指定点の設定方法の説明図である。
【図１７Ａ】図１７Ａは、指定点の設定方法の説明図である。
【図１７Ｂ】図１７Ｂは、指定点の設定方法の説明図である。
【図１７Ｃ】図１７Ｃは、指定点の設定方法の説明図である。
【発明を実施するための形態】
【００１８】
以下、添付図面を参照して、本発明の実施形態を説明する。図５は、本発明の一実施形態における多関節マニピュレータ１と、コンピュータＣ１と、表示装置Ｃ２を備えたロボットシステムを示す。多関節マニピュレータ１は、床面等に固定された基部２を備える。基部２に、支持部３の一端が固定される。支持部３の他端は、関節Ｊ１の一方側に固定される。関節Ｊ２の他方側に、第１のリンクＬ１の一端が取り付けられる。第１のリンクＬ１の他端に、第２の関節Ｊ２の一方側が取り付けられる。以下、同様にして、第５のリンクの他端に第６の関節Ｊ６の一方側が取り付けられる。第６の関節Ｊ６の他方側に、第６のリンクＬ６の一端が取り付けられる。第６のリンクＬ６の他端に、エンドエフェクタ４が取り付けられる。図５の例では、６個の関節Ｊ１〜Ｊ６を備える多関節マニピュレータ１が描かれているが、これより多い、またはこれより少ないｎ個の関節Ｊ１〜Ｊｎを備えたｎ自由度の多関節マニピュレータ１を用いてもよい。
【００１９】
図１、図２を参照して説明した参考例と同様に、作業者は、多関節マニピュレータ１の手先（エンドエフェクタ４の先端など）に設定される指定点５のワールド座標系における位置及び姿勢を示す位置・姿勢指令値を制御装置に対して指定する。制御装置は、指定点５が位置・姿勢指令値に示された状態に向かうように、各関節Ｊ１〜Ｊ６の角度指令値を生成する。各関節Ｊ１〜Ｊ６は、その角度指令値に従ってモータ等により駆動される。このような制御により、多関節マニピュレータ１の手先（指定点５）を所望の位置に移動することができる。
【００２０】
多関節マニピュレータ１に、コンピュータＣ１が接続される。コンピュータＣ１は、ハードディスク等の非遷移的（ｎｏｎ−ｔｒａｎｓｉｔｏｒｙ）な記憶媒体を備える。コンピュータＣ１は、その記憶媒体に格納されたソフトウェアを実行することによって、多関節マニピュレータ１の動作をシミュレーションにより再現することができる。そのシミュレーションにより、多関節マニピュレータ１の動作は、表示装置Ｃ２の画面上で再現される。作業者は、その画面上に表示された多関節マニピュレータ画像６を見て、例えば後述するＩｎｔｅｒａｃｔｉｖｅ  Ｍａｒｋｅｒ等のグラフィカルユーザインタフェースによって画面上の指定点５を所望の場所に移動し、更にその指定点における姿勢を指定する。このような画面操作により、多関節マニピュレータ１の手先の位置・姿勢指令値を設定することができる。
【００２１】
図６は、指定点５の指令値を入力したときの関節Ｊ１〜Ｊ６の制御の一般的な流れを示す。多関節マニピュレータ１は、エンコーダなどによって、関節Ｊ１〜Ｊ６の各々の現在の姿勢を示す関節角度θを検出することができる。コンピュータＣ１は、多関節マニピュレータ１から、各関節Ｊ１〜Ｊ６の関節角度θの現在値を取得する。コンピュータＣ１は、その関節角度θに基づいて順運動学計算を行うことにより、ワールド座標系における現在の手先位置及び手先姿勢を計算する（Ａ１）。
【００２２】
一方、作業者はコンピュータＣ１を用い、表示装置Ｃ２のシミュレーション画像を見ながら、指定点５の目標位置及び目標姿勢を示す手先指令を入力する。コンピュータＣ１は、Ａ１で算出された現在の手先位置及び姿勢に対する、手先指令の手先位置及び姿勢の偏差Ｅを計算する（Ａ２）。コンピュータＣ１は更に、予め設定された位置制御用の比例ゲインＫＰを偏差Ｅに掛ける（Ａ３）。
【００２３】
各関節Ｊ１〜Ｊ６の現在の角度の検出値に基づいて、ヤコビ行列Ｊを計算する（Ａ４）。更に、そのヤコビ行列の逆行列（多関節マニピュレータ１が冗長自由度を持っている場合は疑似逆行列）を計算する（Ａ５）。この逆ヤコビ行列を用いて、ゲインを掛けた位置姿勢偏差ＫＰ・Ｅから関節速度の指令値を算出する（Ａ６）。この関節速度の指令値を時間積分することにより、関節角度の指令値が算出される（Ａ７）。コンピュータＣ１は、関節角度の指令値を多関節マニピュレータ１に送信する。多関節マニピュレータ１の制御装置は、その指令値に基づいて、各関節Ｊ１〜Ｊ６のモータ等を制御する。以上の制御により、作業者が指定した目標位置及び目標姿勢を取るようにエンドエフェクタ４を動かすことが可能である。
【００２４】
［リンク位置の固定］
以上は、多関節マニピュレータ１の手先を目標位置及び目標姿勢に動かすための制御についての説明である。それに加えて、本実施形態においては、多関節マニピュレータ１に対して、手先以外の位置を指定点１０として設定する。そして指定点１０における多関節マニピュレータ１の少なくとも１つの運動の自由度を拘束した被拘束状態で手先の位置及び姿勢を制御する部分固定制御を実行する。本実施形態における部分固定制御では、リンクＬ１〜Ｌ６のうちの指定された位置を固定した状態でエンドエフェクタ４を動かす。以下、そのような制御について説明する。
【００２５】
図７は、そのような制御を行うためにコンピュータＣ１によって実現される機能ブロックを示す。コンピュータＣ１は、指定点設定部３１、座標設定部３２、計算部３３、姿勢設定部３４、及び指定関節設定部３５として機能する。これらの各機能ブロックは、コンピュータＣ１の演算制御装置が記憶媒体に格納されたソフトウェアを読み出して実行することによって実現される。
【００２６】
指定点設定部３１は、多関節マニピュレータ１上のいずれかの位置を指定点１０として設定する。具体的には、作業者が画面上に表示されるポインタ等を操作して、多関節マニピュレータ画像６の所望の位置を指定する入力操作を行う。指定点設定部３１は、その入力操作に応じて、指定点１０を設定する。指定点１０は、ロボット座標系（多関節マニピュレータ１のローカル座標系）により指定される。後述するようにリンク番号とリンク原点からの相対位置によって、そのような指定を行うことができる。
【００２７】
指定点１０の一例として、図８では指定点１０がリンクＬ４上に描かれている。作業者は、多関節マニピュレータ画像６を見ながらコンピュータＣ１を操作することによって、所望の位置に指定点１０を設定することができる。図８の例では、第１の関節Ｊ１から手先側が可動部分であるため、多関節マニピュレータ１上の関節Ｊ１から手先側の任意の位置を指定点１０として設定することができる。
【００２８】
指定点１０を指定するための位置情報として特に重要なのは、リンクＬ１〜Ｌ６の長さ方向の位置である。従って、作業者は例えば、多関節マニピュレータ１の基部２から、各リンクＬ１〜Ｌ６の断面の中心付近を通ってエンドエフェクタ４に向かう仮想的な中心線ＣＬ（図５参照）を引いたときに、可動部（第１の関節Ｊ１より手先側）における中心線ＣＬの任意の位置を、指定点１０として指定することができる。
【００２９】
このような指定点１０は、リンク番号とリンク原点からの長さ方向の位置によって特定することができる。リンク番号は、各々のリンクを個別に特定する識別子（例えば図８ではリンクＬ１〜Ｌ６のうちの「Ｌ４」）である。リンク原点からの位置は、指定点１０が設定されたリンクＬ４において、根元側の所定位置（図８では例えば、関節Ｊ４とリンクＬ４の接続箇所の位置）から指定点１０までの長さを示す。
【００３０】
座標設定部３２は、指定点１０に対応づけて、ワールド座標系（図８ではｘｙｚ座標として示されている）における指定点１０の固定位置を示す指定位置を設定する。具体的には、既に説明した指定点１０の設定の場合と同様に、座標設定部３２は、作業者が行う入力操作に応じて指定位置を設定する。
【００３１】
計算部３３は、多関節マニピュレータ１から入力した検出値などに基づいて、指定点１０の現在位置を示す情報を生成する。計算部３３は更に、指定点１０を現在位置から指定位置に移動するための指令値を生成し、多関節マニピュレータ１に送信する。多関節マニピュレータ１は、この指令値に基づいて関節Ｊ１〜Ｊ６を駆動して、指定点１０を指定位置に移動する。
【００３２】
計算部３３は、指定点１０を指定位置に固定した状態で多関節マニピュレータ１の手先位置及び手先姿勢を制御するための指令値（被拘束制御指令値）を計算する。例えば、作業者が手先の指定点５の目標位置１５を入力すると、図８の指定点５が目標位置１５に移動して図９に示す姿勢となるように、各関節Ｊ１〜Ｊ６に対する制御指令値を計算する。その計算は、指定点１０よりも根元側の関節Ｊ１〜Ｊ４と、手先側の関節Ｊ５〜Ｊ６の二部分に分けて、各部分について独立に、図６で説明した順運動計算及び逆運動計算を用いることにより実現できる。
【００３３】
以上で、本実施の形態における多関節マニピュレータ１の制御用データの生成方法を説明した。この制御用データをコンピュータＣ１が多関節マニピュレータ１に送信することにより、指定点１０を指定位置に固定した状態で、多関節マニピュレータ１の制御が行われる。
【００３４】
指定点１０より手先側の関節（図８、図９の例では関節Ｊ５、Ｊ６）の個数が６個以上有る場合など、手先側の自由度が十分であれば、指定点５に対して自在に目標位置及び目標姿勢を設定することができる。指定点１０より手先側の自由度が十分でない場合でも、その自由度の範囲内で、作業者が目標位置及び目標姿勢を設定することができる。このような制御により、例えば周囲に図３のような障害物が存在したり図４のように隙間１０８を通すためにリンクＬ４を動かしたくない場合に、リンクＬ４を固定して手先を制御することができる。
【００３５】
［リンク位置と姿勢の固定］
部分固定制御においては更に、指定点１０の位置のみならず、指定点１０における多関節マニピュレータ１の姿勢も固定した制御を実行することができる。その際、図７の姿勢設定部３４は、作業者が多関節マニピュレータ画像６を参照して入力した角度に応じて、指定姿勢を設定する。この操作により、例えばリンクＬ４の角度が指定姿勢によって示される値に固定される。図８では、この指定姿勢が角度αとして示されている。図８は平面的に描かれているが、多関節マニピュレータ１が三次元的な動作を行う場合には、指定姿勢は三次元的な角度を示し、例えばワールド座標系に設定されたオイラー角によって指定される。
【００３６】
このような場合、計算部３３は、指定点１０において多関節マニピュレータ１の位置を指定位置に固定し、姿勢を指定姿勢に固定した状態で、多関節マニピュレータ１の運動を制御するための指令値の計算を行う。この指令値を多関節マニピュレータ１に送信することにより、図１０に示すように、指定されたリンクＬ４の指定点１０の位置のみならず、姿勢（角度α）を固定した状態で多関節マニピュレータ１の制御をすることができる。
【００３７】
［対象物の固定］
部分固定制御においては、多関節マニピュレータ１上の位置以外の位置を指定点１０として設定する対象物固定制御を行うことも可能である。図１１は、その一例を示す。この例では、エンドエフェクタ４が工具などの対象物１１を支持している。その対象物１１上に指定点１０が設定される。
【００３８】
このような位置に指定点１０を設定した場合でも、計算部３３は、指定点１０において対象物１１の位置及び姿勢を固定した状態で、被拘束制御指令値を生成することができる。但し、エンドエフェクタ４が取り付けられたリンクＬ６に対する対象物１１の位置及び姿勢は固定されているものとする。このような制御により、多関節マニピュレータ１の手先ではなく、そのエンドエフェクタ４が把持している対象物の先端の位置や姿勢を固定したい場合に、容易に設定を行うことができる。
【００３９】
この制御において、作業者は、リンクＬ１〜Ｌ６や関節Ｊ１〜Ｊ６の所望の箇所を指定する。更に、その箇所の目標位置や目標姿勢を指定する。計算部３３は、対象物１１上の指定点１０をワールド座標系中の指定された位置に固定した被拘束状態で、指定された箇所が目標位置・目標姿勢に向かうように、各関節Ｊ１〜Ｊ６の角度を計算する。
【００４０】
対象物固定制御において、指定点１０の設定は、以下のように行うことができる。リンク番号とリンク原点に対するワールド座標系における相対位置とが設定される。図１１の場合では、リンク番号としてリンクＬ６を設定し、リンク原点に対する指定点１０の目標値の相対位置を設定する。この際、リンク原点に対する相対位置がリンクＬ６やエンドエフェクタ４の先端よりも遠方側に設定されれば、エンドエフェクタ４が把持している対象物１１上に指定点１０を設定したこととなる。
【００４１】
指定点１０の設定は、以下のように行うこともできる。多関節マニピュレータ１が備えるロボットヘッド等に、エンドエフェクタ４付近における物体の形状を検出することのできる検出装置（例えばレーザスキャナ）を設ける。その検出装置により、対象物１１の位置、形状、及び姿勢を検出する。検出された対象物情報は、コンピュータＣ１に送信される。
【００４２】
コンピュータＣ１は、多関節マニピュレータ１から受信した対象物情報に基づいて、表示装置Ｃ２に対象物画像を表示する。すなわち、実空間における対象物１１を把持した状態における多関節マニピュレータ１のシミュレーション画像が表示される。作業者は、その画面上の対象物１１を見て、ポインタ等により対象物画像上に指定点１０を設定する入力操作を行う。指定点設定部３１は、その入力操作に応じて指定点１０の設定を行う。
【００４３】
［手先固定制御］
次に、本発明の実施形態として、手先固定制御と、根元固定制御を説明する。これらはいずれも、指定点において多関節マニピュレータ１の少なくとも１つの運動の自由度を拘束するという点では、図５〜図１１で説明した実施形態と同様である。しかし、関節を選択し、その選択された関節の前後（根元側と手先側）のいずれか一方の全関節を固定するという点で異なる。既述の実施形態では指定点１０のワールド座標系における位置を固定するという制御が行われたが、本実施形態では、根元側又は手先側において、関節におけるリンク相互の角度を固定する制御が行われる。
【００４４】
まず手先固定制御について説明する。この制御においては、多関節マニピュレータ１において、ある関節より手先側の部分の全関節を固定した制御が行われる。図１２を参照して、この制御について説明する。この実施形態においては、多関節マニピュレータ１の根元（例えば基部２と支持部３の接続箇所）などの固定された位置を「絶対基準座標２０」、作業者によって指定された指定関節（図１２では関節Ｊ３）の位置を「設定座標２１」、エンドエフェクタ４上の所定位置など多関節マニピュレータ１の手先の位置を「手先座標２２」と呼ぶことにする。
【００４５】
図１３は、手先固定制御と根元固定制御の処理を示すフローチャートである。まず、作業者は、コンピュータＣ１に対する入力操作により、多関節マニピュレータ１が備える複数の関節Ｊ１〜Ｊ６のうちのいずれかを指定関節として選択する。図７の指定関節設定部３５は、その入力操作に応じて指定関節を設定する。図１２の例では、関節Ｊ３が指定関節として設定される。指定関節のワールド座標系における位置が「設定座標２１」である（ステップＳ１）。
【００４６】
次に、手先と根元のどちらを固定するかを選択する。本実施形態においては、作業者のコンピュータＣ１に対する入力操作に応じて、手先固定制御が選択される。この選択により、「設定座標２１」より根元側が可動側、手先側が固定側に設定される（ステップＳ２）。
【００４７】
次に、計算部３３は、可動側の根元の位置を、基準座標として設定する。手先固定制御の場合は根元側が可動側なので、多関節マニピュレータ１全体の根元に当たる「絶対基準座標２０」を基準座標として設定する（ステップＳ３）。
【００４８】
次に、計算部３３は、固定側の長さを算出する。図１２の例では、指定関節の「設定座標２１」から、エンドエフェクタ４の「手先座標２２」までのワールド座標系における長さを算出する。この計算は、以下のようにして可能である。コンピュータＣ１は、固定側の各関節Ｊ３〜Ｊ６の関節角度の現在の検出値を読み込む。更に、コンピュータＣ１には多関節マニピュレータ１のリンクパラメータ等のシミュレーションモデルのデータが登録されているため、各リンクＬ１〜Ｌ６の長さを知ることができる。そこで、固定側の関節角度の検出値と、各リンクＬ３〜Ｌ６の長さとに基づいて、「設定座標２１」と「手先座標２２」との間の、ワールド座標系におけるｘ軸、ｙ軸、ｚ軸方向のそれぞれの距離を算出する。この計算により、固定側のｘ軸、ｙ軸、ｚ軸の各方向の長さが得られる（ステップＳ４）。
【００４９】
次に、座標設定部３２は、設定座標２１の移動先を指定座標として設定する（ステップＳ５）。計算部３３は、設定座標２１が指定座標に移動するように逆運動学計算に基づいて、可動側の各関節Ｊ１、Ｊ２の制御指令値を生成する（ステップＳ６）。図１２では可動側に２個の関節Ｊ１、Ｊ２しか描かれていないが、このような運動を可能とするために、実際にはより多数の関節が用意されていることが望ましい。次に、計算部３３は、固定側（設定座標２１から手先座標２２まで）の各関節Ｊ３〜Ｊ６の角度指令値を、一定の値に固定する。このような処理により、被拘束制御指令値を生成する（ステップＳ７）。
【００５０】
コンピュータＣ１は、以上の処理で生成された関節Ｊ１〜Ｊ６の角度の指令値を多関節マニピュレータ１に送信する。その指令値に基づいて、多関節マニピュレータ１の各関節Ｊ１〜Ｊ６が駆動される（ステップＳ８）。
【００５１】
図１４と図１５は、手元固定制御における多関節マニピュレータ１の動作の一例を示す。図１４において、関節Ｊ４が指定関節として設定されている。それより根元側２３が可動部、手先側２４が固定部である。図１５は、ステップＳ５で設定された移動後の多関節マニピュレータ１を示す。指定関節Ｊ４が指定点２５に移動している。その際、手先側２４の各関節Ｊ４〜Ｊ７の角度は固定されているため、リンクＬ３〜Ｌ７の相対的な位置及び姿勢は固定されている。すなわち、リンクＬ３からエンドエフェクタ４までの部分は、形状が固定されており、手先側２４の可動部に把持された一種のエンドエフェクタのような使い方をすることができる。
【００５２】
通常、エンドエフェクタ４の手先は何らかの作業を行うため、そのワールド座標系における位置が制御の対象となる。その位置は、指定関節Ｊ４の座標と、ステップＳ４で計算された手先側２４の長さを足すことによって知ることができる。
【００５３】
［根元固定制御］
次に、根元固定制御について説明する。この制御においては、多関節マニピュレータ１において、ある関節より根元側の部分における全ての関節を固定した制御が行われる。再び図１３を参照して、根元固定制御について説明する。ステップＳ１の指定関節の選択は、手先固定制御と同様である。ステップＳ２において、作業者のコンピュータＣ１に対する入力操作に応じて、根元固定制御が選択される。この選択により、「設定座標」より根元側が固定側、手先側が可動側に設定される（ステップＳ２）。
【００５４】
次に、計算部３３は、可動側の根元の位置を、基準座標として設定する。根元固定制御の場合は手先側が可動側なので、手先側の根元に当たる「設定座標２１」を基準座標として設定する（ステップＳ３）。
【００５５】
次に、計算部は、固定側の長さを算出する。図１２の例では、「絶対基準座標２０」から、指定関節の「設定座標２１」までの長さを算出する。算出方法は、手先固定制御の場合と同様である（ステップＳ４）。
【００５６】
次に、座標設定部３２は、設定座標２１の移動先を指定座標として設定する（ステップＳ５）。計算部３３は、設定座標２１が指定座標に移動するように逆運動学計算に基づいて、可動側の各関節Ｊ１、Ｊ２の制御指令値を生成する（ステップＳ６）。次に、計算部３３は、固定側（絶対基準座標２０から設定座標２１まで）の各関節Ｊ１、Ｊ２の角度指令値を、一定の値に固定する（ステップＳ７）。
【００５７】
コンピュータＣ１は、以上の処理で生成された関節Ｊ１〜Ｊ６の角度の指令値を多関節マニピュレータ１に送信する。その指令値に基づいて、多関節マニピュレータ１の各関節Ｊ１〜Ｊ６が駆動される（ステップＳ８）。
【００５８】
以上の処理により、図１４に示された根元側２３の各関節Ｊ１〜Ｊ３を固定した状態にして、手先側２４の各関節Ｊ４〜Ｊ７を動作させる根元固定制御が可能となる。このような制御により、例えば図３に示したように障害物１０６がある場合に、途中までの関節Ｊ１〜Ｊ３をその障害物１０６を回り込むような姿勢で固定し、その向こう側の領域に対して、手先側２４の各関節Ｊ４〜Ｊ７を動かして作業を行うことができる。
【００５９】
上記の手先固定制御と根元固定制御の応用として、一か所の指定関節のみの動きを固定することもできる。このような制御においては、指定関節の前後に接続された一対のリンクの相対位置及び相対姿勢を固定して、他の関節の制御が行われる。
【００６０】
［設定方法］
次に、指定点１０の設定方法について説明する。図１６Ａは、図５の表示装置Ｃ２に表示される画面の例を示す。図１６Ａの例では、ｘｙｚの３軸で示される直交座標のｘ軸正方向から見た多関節マニピュレータ画像６が示されている。デフォルトの状態で、エンドエフェクタ４を備える手先のリンクＬ６が選択されている。選択されているリンクＬ６は、他の部分に対して視覚的に区別して（例えば違う色で）表示される。
【００６１】
デフォルトの状態では更に、手先位置（エンドエフェクタ４上の所定位置）に、指定点１０が表示される。指定点１０の位置に更に、選択されたリンクＬ６の三次元的な姿勢を示すマーカ１３が表示される。マーカ１３として例えば、Ｗｉｌｌｏｗ  Ｇａｒａｇｅ社が開発したミドルウェアであるＲＯＳ（Ｒｏｂｏｔ  Ｏｐｅｒａｔｉｎｇ  Ｓｙｓｔｅｍ）のＩｎｔｅｒａｃｔｉｖｅ  Ｍａｒｋｅｒを使用することができる。
【００６２】
画面に、マウス等のポインティングデバイスで操作できるポインタ１４が表示される。作業者は、ポインタ１４を操作して、多関節マニピュレータ画像６のうち所望のリンクを指示し、選択操作を行う。図１６Ｂは、その選択操作が行われた画面を示す。リンクＬ５が選択され、他のリンクＬ１〜Ｌ４、Ｌ６とは異なる色で表示される。作業者は更に、ポインタ１４の先端を所望の位置に置いて指定操作を行う。その指定操作に応じて、ポインタ１４の先端の箇所が指定点１０として指定される。
【００６３】
指定点１０の付近に、選択されたリンクＬ５の姿勢を示すマーカ１３が表示される。マーカ１３は例えばｘｙｚの三軸の矢印を有し、三次元空間内でその角度が自在に設定できる。作業者は、ポインタ１４でマーカ１３を指示し、画面上で所望の角度に回転することによって、リンクＬ５の姿勢を設定する。
【００６４】
リンクＬ５の姿勢の設定に応じて、多関節マニピュレータ画像６の全体を表示し直してもよい。その場合、計算部３３は、マーカ１３を用いて設定された姿勢に応じて順運動学及び逆運動学計算を行うことにより、各関節Ｊ１〜Ｊ６の角度を計算し、リンクＬ５の姿勢を設定姿勢に変更した後の状態の多関節マニピュレータ画像６を表示する。
【００６５】
作業者は更に、多関節マニピュレータ画像６を見る仮想的な視点の位置及び角度を自在に変更することができる。図１６Ｂではｘ軸正方向から見た多関節マニピュレータ画像６が示されており、図１６Ｃでは仮想的な視点を変更してｚ軸正方向から見た多関節マニピュレータ画像６が示されている。この状態においても、作業者はポインタ１４及びマーカ１３を操作することにより、リンクＬ５の選択と、その姿勢の指定とを行うことができる。このように仮想空間中における多関節マニピュレータ画像６の姿勢を様々な視点から見ることにより、選択されたリンクＬ５の姿勢の設定を容易に行うことができる。
【００６６】
次に、図１１を参照して説明した対象物１１を固定する部分固定制御の設定方法について説明する。図１７Ａは、エンドエフェクタ４が対象物１１を把持している場合の多関節マニピュレータ画像６を示す。既述のように、レーザスキャナ等により対象物１１の形状、大きさ、及び姿勢を検出することができる。その検出結果に基づいて、多関節マニピュレータ画像６の一部として対象物１１が表示される。デフォルトで、手先のリンクＬ６が選択された状態の画像が表示される。その際、最も手先側のリンクＬ６と共に対象物１１も、その他の部分と異なる色で表示される。手先の位置に指定点１０が表示され、その付近にマーカ１３が表示される。
【００６７】
図１７Ｂに示すように、作業者は、ポインタ１４を操作して、対象物１１上の所望の場所を指定点１０として設定する。この指定点１０の位置は、図１１の場合について説明したように、リンク番号（リンクＬ６）と、リンク原点からの相対位置とを示すデータによって特定することができる。作業者は更に、指定点１０の近傍に表示されるマーカ１３を操作することにより、対象物１１の姿勢を設定する。図１７Ｃに示すように、作業者は、画面上の仮想的な視点の位置及び角度を自由に変更することにより、様々な角度から対象物１１を見てその姿勢を設定することができる。
【符号の説明】
【００６８】
１  多関節マニピュレータ
２  基部
３  支持部
４  エンドエフェクタ
５  指定点
６  多関節マニピュレータ画像
１０  指定点
１１  対象物
１２  基準位置
１３  マーカ
１４  ポインタ
１５  目標位置
２０  絶対基準座標
２１  指定関節
２２  手先座標
２３  根元側
２４  手先側
３１  指定点設定部
３２  座標設定部
３３  計算部
３４  姿勢設定部
３５  指定関節設定部
１０１  多関節マニピュレータ
１０２  基部
１０３  支持部
１０４  エンドエフェクタ
１０５  指定点
１０６  障害物
１０７  壁
１０８  隙間
１０９  天井
Ｃ１  コンピュータ
Ｃ２  表示装置
Ｊ１〜Ｊ６  関節
Ｌ１〜Ｌ５  リンク
Ｊ１０１〜Ｊ１０６  関節
Ｌ１０１〜Ｌ１０６  リンク
【図１】
2015174184.tif 000003
【図２】
2015174184.tif 000004
【図３】
2015174184.tif 000005
【図４】
2015174184.tif 000006
【図５】
2015174184.tif 000007
【図６】
2015174184.tif 000008
【図７】
2015174184.tif 000009
【図８】
2015174184.tif 000010
【図９】
2015174184.tif 000011
【図１０】
2015174184.tif 000012
【図１１】
2015174184.tif 000013
【図１２】
2015174184.tif 000014
【図１３】
2015174184.tif 000015
【図１４】
2015174184.tif 000016
【図１５】
2015174184.tif 000017
【図１６Ａ】
2015174184.tif 000018
【図１６Ｂ】
2015174184.tif 000019
【図１６Ｃ】
2015174184.tif 000020
【図１７Ａ】
2015174184.tif 000021
【図１７Ｂ】
2015174184.tif 000022
【図１７Ｃ】
2015174184.tif 000023