JP7048176B2 - ロボットの直接教示装置及び直接教示方法 - Google Patents

Description

この発明は、ロボットの直接教示を行う直接教示装置及び直接教示方法に関する。

産業用のロボットでは、ロボットに作業をさせるために、前もって教示（ティーチング）と呼ばれる作業が実施される。このロボットの教示を行う方法の１つとして、直接教示（ダイレクト教示）と呼ばれる方法がある。

例えば特許文献１では、力センサを用いたロボットの直接教示方法が開示されている。また、例えば特許文献２では、トルク検出手段を用いたロボットの直接教示方法が開示されている。これらの特許文献で開示されている直接教示装置の概略構成を図２０に示す。

図２０に示す直接教示装置１１では、まず、外力検知部１１０１が、力センサ又はトルクセンサ等を用いて、ロボットが有するアーム２に対して操作者により加えられた外力を検知する。次いで、従動制御演算部１１０２が、外力検知部１１０１により検知された外力に従うアームの動きを算出（従動制御演算）する。なお、従動制御演算部１１０２は、従動制御演算において、位置姿勢計測部（不図示）により計測されたアームの位置姿勢に関するパラメータを用いる場合もある。なお、アームの位置姿勢とは、アームの位置及びアームの姿勢のうちの少なくとも一方を意味する。また、上記パラメータとしては、アームの位置、アームの姿勢、又は、アームの関節角等が挙げられる。そして、従動制御演算部１１０２は、従動制御演算の結果に基づいて更新した従動制御指令値を駆動制御部１１０３に通知する。次いで、駆動制御部１１０３は、従動制御演算部１１０２により通知された従動制御指令値に従ってアーム２を駆動する。
この一連の動作により、図２０に示す直接教示装置１１は、操作者により加えられた外力に従ってアーム２が動くように制御できる。そして、直接教示装置１１は、制御によりアーム２の位置及び姿勢が操作者の意図する状態となった場合に、その際の上記パラメータを教示点として記録する。この直接教示装置１１により記録された教示点はロボットが作業する際に使用される。

このように、ロボットの直接教示は、操作者がアームを直接操作して位置及び姿勢を教示するため、操作者にとって直観的でわかりやすいという利点を持つ。一方で、アームが操作者により加えられた外力に従ってそのまま動くという特徴は利点ばかりではない。

例えば、操作者が、ロボットに対して、ある点Ｐの位置を教示した後に、その真下の別の点Ｑの位置を教示するというケースを考える。このケースでは、操作者は、真下方向（Ｚ軸方向）にアームを動かすことになる。しかしながら、操作者がアームに対して正確に真下方向へ外力を加えることは難しく、アームのＸ軸座標及びＹ軸座標がずれることが多い。後からアームのＸ軸座標及びＹ軸座標だけをティーチングペンダント等を用いて修正することは可能であるが、それでは直接教示の利点が減ずる。
また、例えば、操作者が、アームの先端に設けられたエンドエフェクタを真下に向けたまま教示を行うケースを考える。このケースにおいても、操作者がアームに対して正確に真下に向けた状態を維持したままアームを直接操作することは難しく、アームの姿勢がずれることが多い。
以上のように、直接教示は、直観的で分かりやすい反面、操作が難しいケースも存在する。

これに対し、例えば特許文献３，４では、上記のような課題を解決する技術が開示されている。

特許文献３では、モード切換スイッチにより、動作モードを、位置方向移動モード、位置移動モード又は方向移動モードに切換え可能とする直接教示装置が開示されている。位置移動モードでは、アームの先端の姿勢を維持したまま位置のみを移動可能としている。また、方向移動モードでは、アームの先端の位置を維持したまま姿勢のみを変更可能としている。この直接教示装置により、上記のような直接教示における操作の難しさをある程度緩和可能となる。なお、通常の直接教示は位置方向移動モードで実現可能である。

また、特許文献４では、入力装置により、動作モードを、拘束モード又は全方向移動モードに切換え可能とする直接教示装置が開示されている。拘束モードでは、エンドエフェクタの先端を特定の拘束軸又は拘束面に沿って移動可能としている。また、全方向移動モードでは、通常の直接教示を実施可能としている。この直接教示装置は、直接教示の途中で拘束モードに切換えることにより、アームの先端を正確に上下方向に動かすことが可能となり、直接教示における操作の難しさが軽減される。

特開平０５－２０４４４１号公報 特開平０５－２５００２９号公報 特開平０５－２８５８７０号公報 特開平０５－３０３４２５号公報

上記のように、特許文献３，４に開示された直接教示装置は、アームの操作に拘束を課さない通常の直接教示を行うモードと、アームの操作に拘束を課す拘束付き直接教示を行うモードとを切換える。そして、特許文献３ではモード切換スイッチによって、また、特許文献４では入力装置によって、モードの切換えが行われており、操作者が手でスイッチ等を操作することで切換え可能となっている。

しかしながら、直接教示では、操作者は手でアームを直接操作しているため、モードの切換えを手で操作するのは便利ではない。例えば、操作者が両手でアームを操作している場合、モードの切換えを操作する際にはアームの操作を一旦停止してアームを片手で保持することになる。通常時に両手で保持しているアームを片手で保持すると、アームの位置姿勢がずれることも考えられる。その結果、拘束付き直接教示の効果を減ずることにもなりかねない。

この発明は、上記のような課題を解決するためになされたもので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換える可能な直接教示装置を提供することを目的としている。

この発明に係る直接教示装置は、ロボットが有するアームに加えられた外力を検知する外力検知部と、外力検知部により検知された外力に従うアームの動きを算出する従動制御演算部と、アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、位置姿勢演算部による算出結果に基づいて、アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、拘束目標及び位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部と、位置姿勢演算部による算出結果に基づいて、目標値算出部により算出された目標値に移動するアームの動きを算出する拘束制御演算部と、拘束制御演算部による算出結果を制限する拘束制御制限部と、近接判定部により拘束目標に近づいていると判定された場合に拘束制御演算部による算出結果を有効とし、近接判定部により拘束目標に近づいていないと判定された場合に拘束制御演算部による算出結果を無効とする切換部と、従動制御演算部による算出結果と拘束制御制限部による制限結果とを合成する合成部と、合成部による合成結果に基づいてアームを駆動する駆動制御部とを備えたことを特徴とする。

この発明によれば、上記のように構成したので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。

この発明の実施の形態１に係る直接教示装置の構成例を示す図である。 図２Ａ～図２Ｄは、この発明の実施の形態１に係る直接教示装置による通常の直接教示と拘束付き直接教示（位置拘束の場合）の切換えの一例を示す図である。 図３Ａ～図３Ｄは、この発明の実施の形態１に係る直接教示装置による通常の直接教示と拘束付き直接教示（姿勢拘束の場合）の切換えの一例を示す図である。 この発明の実施の形態１に係る直接教示装置の動作例を示すフローチャートである。 この発明の実施の形態１における近接判定部の動作例を説明する図である（姿勢拘束の場合）。 この発明の実施の形態１における目標値算出部の動作例を説明する図である（姿勢拘束の場合）。 この発明の実施の形態１における目標値算出部の動作例を説明する図である（姿勢拘束の場合）。 この発明の実施の形態２に係る直接教示装置の動作例を示すフローチャートである。 図９Ａ、図９Ｂは、この発明の実施の形態２における従動制御演算部の動作例を示す図である。 この発明の実施の形態２における近接判定部の動作例を示すフローチャートである。 図１１Ａ、図１１Ｂ、図１１Ｃは、この発明の実施の形態２における近接判定部の動作例を説明する図である。 図１２Ａ、図１２Ｂは、この発明の実施の形態２における目標値算出部の動作例を説明する図であり、回転軸の算出例を示す図である。 図１３Ａ、図１３Ｂは、この発明の実施の形態２における目標値算出部の動作例を説明する図であり、回転方向の算出例を示す図である。 この発明の実施の形態２における目標値算出部の動作例を説明する図である。 この発明の実施の形態３に係る直接教示装置による軸拘束を説明する図である。 この発明の実施の形態３に係る直接教示装置の動作例を示すフローチャートである。 この発明の実施の形態３における近接判定部の動作例を説明する図である。 この発明の実施の形態４に係る直接教示装置による面拘束を説明する図である。 この発明の実施の形態４に係る直接教示装置の動作例を示すフローチャートである。 従来の直接教示装置の構成例を示す図である。

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る直接教示装置１の構成例を示す図である。
直接教示装置１は、ロボットの直接教示を行う。この直接教示装置１は、直接教示方法として、ロボットが有するアーム２の位置姿勢に対して拘束を課さない通常の直接教示と、アーム２の位置姿勢を拘束目標に拘束する拘束付き直接教示とを切換え可能である。
なお、アーム２の位置姿勢とは、アーム２の位置及びアーム２の姿勢のうちの少なくとも一方を意味する。また、アーム２の位置とは、アーム２の先端に設けられたエンドエフェクタ２０１の位置５０１を意味し、アーム２の姿勢とはエンドエフェクタ２０１の向き５０２を意味する。また、拘束目標とは、アーム２の位置姿勢の拘束先である。アーム２の位置に対する拘束目標としては、拘束面５０３及び拘束軸５０４がある。拘束面５０３の種別としては、平面及び曲面がある。拘束軸５０４の種別としては、直線及び曲線がある。アーム２の姿勢に対する拘束目標としては、拘束方向５０５がある。また、直接教示装置１で用いる拘束目標は１つに限らず複数でもよい。また、ロボットに対しては基準となるロボット座標系が設定され、エンドエフェクタ２０１に対してはツール座標系が設定される（図６等参照）。

この直接教示装置１は、図１に示すように、位置姿勢計測部１０１、外力検知部１０２、従動制御演算部１０３、位置姿勢演算部１０４、近接判定部１０５、切換部１０６、目標値算出部１０７、拘束制御演算部１０８、拘束制御制限部１０９、合成部１１０及び駆動制御部１１１を備えている。なお、従動制御演算部１０３、位置姿勢演算部１０４、近接判定部１０５、切換部１０６、目標値算出部１０７、拘束制御演算部１０８、拘束制御制限部１０９、合成部１１０及び駆動制御部１１１は、システムＬＳＩ（Ｌａｒｇｅ－Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等により実現される。

位置姿勢計測部１０１は、アーム２の位置姿勢に関するパラメータを計測する。なお、アーム２の位置姿勢に関するパラメータとしては、アーム２の位置、アーム２の姿勢、又は、アーム２の関節角θ等が挙げられる。

外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する。例えば、外力検知部１０２は、アーム２の先端に取付けられた力センサを用い、この力センサにより計測された力Ｆを上記外力として検知してもよい。また、例えば、外力検知部１０２は、アーム２のモータ駆動軸に取付けられたトルクセンサを用い、このトルクセンサにより計測されたトルクτを上記外力として検知してもよい。また、外力検知部１０２は、上記のようにセンサを用いて外力を直接計測するのではなく、アーム２が有するモータの電流又はアーム２の関節角θの計測値から間接的に外力を算出する外力オブザーバを用いて上記外力を検知してもよい。

従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の動き（従動制御指令値）を算出する。

なお、位置姿勢計測部１０１、外力検知部１０２及び従動制御演算部１０３は、拘束が無い通常の直接教示で用いられる構成と同様であり、公知技術である。

位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置姿勢を算出する。

なお図１では、従動制御演算部１０３が位置姿勢計測部１０１による計測結果を用い、位置姿勢演算部１０４が従動制御演算部１０３による算出結果を用いている。
しかしながら、これに限らず、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果は用いず、外力検知部１０２により検知された外力に従うアーム２の動きを演算してもよい。この場合、位置姿勢演算部１０４は、位置姿勢計測部１０１による計測結果に基づいてアーム２の位置姿勢を算出する。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方を用いてアーム２の位置姿勢を算出してもよい。

近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が拘束目標に近づいているかを判定する。

切換部１０６は、近接判定部１０５による判定結果に従い、拘束制御演算部１０８の動作を切換える。すなわち、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていると判定された場合には、拘束制御演算部１０８による処理を有効とする。一方、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていないと判定された場合には、拘束制御演算部１０８による処理を無効とする。

また、切換部１０６は、拘束目標が複数存在する場合に、近接判定部１０５による判定結果に従い、目標値算出部１０７による動作（対象とする拘束目標）の切換えも実施してもよい。

目標値算出部１０７は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束制御の目標値を算出する。なお、拘束制御の目標値とは、アーム２の目標位置及びアーム２の目標姿勢のうちの少なくとも一方を意味する。

拘束制御演算部１０８は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標値に移動するアーム２の動き（拘束制御指令値）を算出する。

拘束制御制限部１０９は、拘束制御演算部１０８による算出結果を制限する。すなわち、拘束制御制限部１０９は、従動制御演算部１０３による算出結果が拘束制御演算部１０８による算出結果より優先されるよう拘束制御演算部１０８による算出結果に制限を課す。
なお図１では、拘束制御演算部１０８と拘束制御制限部１０９を別々の構成要素として示しているが、一体として実装することも可能である。

合成部１１０は、従動制御演算部１０３による算出結果（従動制御指令値）と拘束制御制限部１０９による制限結果（拘束制御指令値）とを一つの指令値に合成する。

駆動制御部１１１は、合成部１１０による合成結果に基づいて、アーム２を駆動する。

実施の形態１に係る直接教示装置１では、通常の直接教示と拘束付きの直接教示の切換えを、操作者によるアーム２の操作により実行可能としている。この切換え方法は、以下に示す２つの仕組みによって実現される。

１つ目は、直接教示装置１が、アーム２の位置姿勢が拘束目標に近づいた際に、自動的に通常の直接教示から拘束付きの直接教示に切換える仕組みである。例えば図２Ａ，２Ｂに示すように、アーム２の位置を所定の平面５０６（拘束面５０３）に拘束させたい場合、直接教示装置１は、その平面５０６からアーム２の位置までの距離が例えば±５０［ｍｍ］以下である場合には拘束付き直接教示を行い、それ以外の場合には通常の直接教示を行う。また、例えば図３Ａ，３Ｂに示すように、アーム２の姿勢を真下方向５０７（拘束方向５０５）に拘束させたい場合、直接教示装置１は、アーム２の姿勢が真下方向５０７から例えば±０．２［ｒａｄ］以下である場合には拘束付き直接教示を行い、それ以外の場合には通常の直接教示を行う。この仕組みにより、直接教示装置１は、操作者によるアーム２の操作のみで、通常の直接教示から拘束付き直接教示への切換えを実現できる。

２つ目は、直接教示装置１が、拘束付き直接教示の際に拘束する力を敢えて抑制することで、操作者が拘束制御に打ち勝ってアーム２を操作可能とする仕組みである。例えば、直接教示装置１は、拘束制御における関節角速度θ（ドット）を例えば０．１［ｒａｄ／ｓ］以下に抑制し、操作者がそれ以上の外力を加えることでアーム２を操作可能とする。これにより、操作者は、アーム２を、拘束された状態から少しずつ逸脱させることができ、最終的には拘束付き直接教示の実行条件から外すことができる。一般的な従来の直接教示装置では、拘束付き直接教示においてアーム２を拘束面等に強く拘束することでアーム２の位置姿勢を保持している。それに対し、実施の形態１に係る直接教示装置１では、拘束する力を敢えて弱めることで、図２Ｃ，２Ｄ，３Ｃ，３Ｄに示すように、操作者によるアーム２の操作のみで、拘束付き直接教示から通常の直接教示への切換えを実現できる。

以上の２つの仕組みを組合わせることにより、実施の形態１に係る直接教示装置１は、操作者がアーム２を操作している状態のまま、通常の直接教示と拘束付き直接教示とを切換え可能となる。以下、図１に示す直接教示装置１の動作例について、図４を参照しながら説明する。

図１に示す直接教示装置１の動作例では、図４に示すように、まず、位置姿勢計測部１０１は、アーム２の位置姿勢に関するパラメータを計測する（ステップＳＴ４０１）。

また、外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する（ステップＳＴ４０２）。なお、外力検知部１０２により検知される外力は、センサの構造等によっては、操作者によりアーム２に加えられた外力だけではなく、重力に起因する成分が重畳されている場合がある。そこで、このような場合には、外力検知部１０２は、重力に起因する成分を算出し、検知した外力から当該重力に起因する成分を差し引くことで、外力成分のみを算出するとよい。これは重力補償と呼ばれる公知技術であり、例えば特許文献５等に開示されている。
特開平０１－０６６７１５号公報

次いで、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の動き（従動制御指令値）を算出する（ステップＳＴ４０３）。この際、従動制御演算部１０３は、まず、外力検知部１０２により検知された外力の向き及び大きさから、操作者がどのようにアーム２を動かそうとしているのかを判断する。そして、従動制御演算部１０３は、その判断結果及び位置姿勢計測部１０１により計測されたアーム２の位置姿勢に基づいて、アーム２の移動量（関節角θ等の差分）又は関節角速度θ（ドット）等を算出する。なお、従動制御演算部１０３による従動制御演算方法は、例えば特許文献２等で開示されており、様々な方式が開発されているためその詳細な記述は省略する。

次いで、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置姿勢を算出する（ステップＳＴ４０４）。
ここで、直接教示装置１が位置拘束を行う場合、すなわち、直接教示装置１がアーム２の位置を所定の拘束面５０３又は拘束軸５０４に拘束する場合には、位置姿勢演算部１０４は少なくともアーム２の位置を算出する必要がある。
また、直接教示装置１が姿勢拘束を行う場合、すなわち、直接教示装置１がアーム２の姿勢を所定の拘束方向５０５に拘束する場合には、位置姿勢演算部１０４は少なくともアーム２の姿勢を算出する必要がある。
また、直接教示装置１が位置拘束及び姿勢拘束を行う場合、すなわち、直接教示装置１がアーム２の位置及び姿勢を所定の拘束面５０３又は拘束軸５０４且つ所定の拘束方向５０５に拘束する場合には、位置姿勢演算部１０４はアーム２の位置及び姿勢を算出する必要がある。

また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果（最新の従動制御指令値）に基づいて上記算出を行うことが最も望ましい。これは、直接教示装置１が拘束付き直接教示を行う場合に、操作者がアーム２に対して与えた移動量のうち、拘束面５０３等から外れる成分を同時に相殺でき、拘束制御が最も有効に働くからである。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果を用いず、位置姿勢計測部１０１による計測結果に基づいて上記算出を行うことも可能である。一方、この場合には、拘束面５０３等から外れたことが計測されてから拘束制御が始まるので、拘束制御の効きが悪くなる。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方に基づいて上記算出を行ってもよい。

なお、位置姿勢計測部１０１及び従動制御演算部１０３で関節角θのみが得られ、アーム２の位置姿勢を直接得られない場合、位置姿勢演算部１０４は、順運動学演算によってアーム２の位置姿勢を算出することが可能である。

次いで、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が拘束目標に近づいているかを判定する（ステップＳＴ４０５）。

例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］の位置に固定することであり、また、接近判定のしきい値が０．０５［ｍ］であるとする。この場合、近接判定部１０５は、位置姿勢演算部１０４により算出されたエンドエフェクタ２０１の先端のＺ軸座標が０．０５～０．１５［ｍ］の範囲内であればアーム２の位置は拘束目標に近づいていると判定し、それ以外の場合にはアーム２の位置は拘束目標に近づいていないと判定する。

また、拘束制御の目的がエンドエフェクタ２０１の向き５０２（Ｚ軸）を真下に向けることであり、また、接近判定のしきい値が０．１［ｒａｄ］であるとする。この場合、近接判定部１０５は、図５に示すように、真上方向（基準方向）５０８とエンドエフェクタ２０１の向き５０２との成す角度Θが（π－０．１）［ｒａｄ］以上、すなわち、真下方向５０７とエンドエフェクタ２０１の向き５０２との成す角度が０．１［ｒａｄ］未満であれば、アーム２の姿勢は拘束目標に近づいていると判定し、それ以外の場合にはアーム２の姿勢は拘束目標に近づいていないと判定する。

また、拘束面５０３が複数存在する場合等のように拘束目標が複数存在する場合、近接判定部１０５は、拘束目標毎に上記判定を行い、各拘束目標のうちの１つでもアーム２の位置姿勢が近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。この際、更に、近接判定部１０５は、アーム２と各拘束目標との間の距離を評価し、上記判定の結果とともにアーム２に最も近い拘束目標を示す情報を切換部１０６に通知してもよい。

次いで、切換部１０６は、近接判定部１０５による判定結果に従い、拘束制御演算部１０８の動作を切換える（ステップＳＴ４０６）。すなわち、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていると判定された場合には、拘束制御演算部１０８による処理を有効とする。一方、切換部１０６は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていないと判定された場合には、拘束制御演算部１０８による演算を停止するか又は算出結果がアーム２の制御に反映されないようにして、拘束制御演算部１０８による処理を無効とする。

このように、直接教示装置１は、操作者によりアーム２の位置姿勢が拘束目標に近づけられると、自動的に拘束制御を開始する。また、直接教示装置１は、後述する拘束制御制限部１０９との組合わせにより、操作者によりアーム２の位置姿勢が拘束目標から遠ざけられると、自動的に拘束制御を終了する。これにより、直接教示装置１は、操作者によりアーム２が保持された状態で拘束制御のオンオフを切換えることができる。
なお、切換部１０６は、拘束目標が複数あって近接判定部１０５からアーム２に最も近い拘束目標を示す情報が通知された場合には、切換部１０６は、後述する目標値算出部１０７にその情報を出力して当該目標値算出部１０７の動作を切換えてもよい。

次いで、目標値算出部１０７は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束制御の目標値を算出する（ステップＳＴ４０７）。

例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］の位置に固定することであり、位置姿勢演算部１０４により算出されたエンドエフェクタ２０１の先端の現在値が［Ｘ_Ｐ，Ｙ_Ｐ，Ｚ_Ｐ］であるとする。この場合、目標値算出部１０７は、拘束制御の目標値を［Ｘ_Ｐ，Ｙ_Ｐ，０．１］とする。

また、拘束制御の目的が、図６下段に示すようにエンドエフェクタ２０１の向き５０２のうちのＺ軸を真下方向５０７とした上でＸ軸をロボット座標系のＸ軸に合わせることであるとする。この場合、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式（１）のようになる。なお、回転行列による姿勢の表現は、例えば非特許文献１等に開示されており、その詳細については省略する。

Ｊｏｈｎ Ｊ． Ｃｒａｉｇ （三浦・下山訳）：ロボティクス，共立出版（１９９１）

なお、目標値算出部１０７は、拘束制御の目標値として、１つの目標値のみを算出してもよいし、複数の目標値を算出してその中から１つの目標値を選択してもよい。例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］刻みで最も近い位置に固定することである場合、目標値算出部１０７は、拘束制御の目標値として［Ｘ_Ｐ，Ｙ_Ｐ，０．１ｎ］（ｎは整数）の中で最も現在の位置に近いものを選択する。

また、拘束制御の目的が図７に示すようにエンドエフェクタ２０１の向き５０２のうちのＺ軸を真下方向５０７とし且つこのＺ軸を回転軸としてπ／２刻み（９０度刻み）に回転させた姿勢で固定することである場合、目標値算出部１０７は、拘束制御の目標値として最も現在の姿勢に近いものを選択する。このとき、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式（２）のようになる。

この式（６）は図７に示すように、ｎによって目標姿勢が４通りに変わるが、目標値算出部１０７はその中から現在の姿勢に最も近いものを１つ選択する。目標値算出部１０７は、現在の姿勢に近いかどうかは、現在の姿勢から各目標姿勢へと回転させた場合の回転角の大小等から判断できる。また、近接判定部１０５がアーム２に最も近い拘束目標を示す情報を通知している場合には、目標値算出部１０７はその情報から拘束制御の目標値を選択してもよい。

次いで、拘束制御演算部１０８は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標値に移動するアーム２の動き（拘束制御指令値）を算出する（ステップＳＴ４０８）。この際、拘束制御演算部１０８は、目標値算出部１０７により算出された目標値から、アーム２を現在の位置姿勢から目標値に移動させるのに必要なアーム２の移動量又は単位時間当たり移動量である関節角速度θ（ドット）等を算出する。また、拘束制御演算部１０８は、関節角速度θ（ドット）の代わりに１制御周期あたりの関節角変化量Δθを演算してもよい。

次いで、拘束制御制限部１０９は、拘束制御演算部１０８による算出結果を制限する（ステップＳＴ４０９）。すなわち、拘束制御制限部１０９は、従動制御演算部１０３による算出結果が拘束制御演算部１０８による算出結果より優先されるよう拘束制御演算部１０８による算出結果に制限を課す。
この拘束制御制限部１０９の役割は、操作者が、直接教示装置１が拘束制御を行っている状態（拘束付き直接教示中の状態）から拘束制御を行っていない状態（通常の直接教示の状態）に切換えられるようにすることである。例えば、直接教示装置１が、拘束する力を強くして、拘束から外れるような操作者の操作を常時完全に打ち消していると、一旦拘束制御が開始すると拘束目標に常時拘束してしまい、拘束制御を終了できなくなる。そこで、拘束制御の終了を可能とするため、拘束制御制限部１０９により拘束制御の強さを制限して、操作者が強い外力を加えれば拘束目標から外すことができるようにする。

例えば、直接教示装置１がアーム２の位置を所定の拘束面５０３に拘束している場合において、拘束制御制限部１０９は拘束制御の速度を例えば０．０５［ｍ／ｓ］以下に制限する。これにより、操作者が０．０５［ｍ／ｓ］より速くアーム２を移動すれば、アーム２は拘束面５０３から離れていき、最終的には近接判定部１０５が拘束面５０３に近づいていないと判定するまで移動させることができる。一方、操作者が０．０５［ｍ／ｓ］より遅くアーム２を移動しようとすれば、アーム２は拘束面５０３に拘束された状態を継続する。

また、直接教示装置１がアーム２の位置を所定の拘束面５０３に拘束している場合において、拘束制御制限部１０９は拘束制御の関節角速度θ（ドット）を例えば０．１［ｒａｄ／ｓ］以下に制限する。これにより、操作者が０．１［ｒａｄ／ｓ］より速くアーム２を移動すれば、アーム２は拘束面５０３から離れていき、最終的には近接判定部１０５が拘束面５０３に近づいていないと判定するまで移動させることができる。一方、操作者が０．１［ｒａｄ／ｓ］より遅くアーム２を移動しようすれば、アーム２は拘束面５０３に拘束された状態を継続する。

上記の他、例えば、直接教示装置１がアーム２の姿勢を真下方向５０７に向けている場合についても、拘束制御制限部１０９が姿勢を変化させる速度又は関節角速度θ（ドット）に制限を加えることで、操作者は拘束状態を解除する操作が可能となる。

なお、拘束制御制限部１０９が拘束制御の強さを制限する際、何れの場合においても、拘束制御によってアーム２を動かす向きを維持することが非常に好ましい。
例えば、拘束制御制限部１０９が関節角速度θ（ドット）を制限する場合であって、ある関節角速度θ（ドット）を０．５倍にしないと制限を超過する場合、全ての関節角速度θ（ドット）を０．５倍にすべきであり、制限を超えた関節角速度θ（ドット）だけを修正すべきではない。拘束制御制限部１０９が全ての関節角速度θ（ドット）に対して同じように制限しなかった場合、拘束制御演算部１０８が動かそうとする向きから変わってしまうため、拘束制御の精度が劣化する。

次いで、合成部１１０は、従動制御演算部１０３から出力された従動制御指令値と拘束制御制限部１０９から出力された拘束制御指令値とを一つの指令値に合成する（ステップＳＴ４１０）。

例えば、従動制御演算部１０３が従動制御指令値として関節角θ_Ｄを出力し、拘束制御制限部１０９が拘束制御指令値として関節角変化量Δθ_Ｃを出力した場合には、合成部１１０は、θ_Ｄ＋Δθ_Ｃを関節角指令値として出力する。
また、例えば、従動制御演算部１０３が従動制御指令値として位置座標［ｘ_Ｄ，ｙ_Ｄ，ｚ_Ｄ］及び姿勢Ｔ_Ｄ（回転行列表現）を出力し、拘束制御制限部１０９が拘束制御指令値として位置変化量［Δｘ_Ｃ，Δｙ_Ｃ，Δｚ_Ｃ］及び姿勢変化量ΔＴ_Ｃ（回転行列表現）を出力した場合には、合成部１１０は、［ｘ_Ｄ＋Δｘ_Ｃ，ｙ_Ｄ＋Δｙ_Ｃ，ｚ_Ｄ＋Δｚ_Ｃ］を位置指令値として、ΔＴ_ＣＴ_Ｄ（回転行列表現）を姿勢指令値として出力する（回転行列表現された姿勢の合成は行列の積となる）。
また、例えば、従動制御演算部１０３が従動制御指令値としてトルクτ_Ｄを出力し、拘束制御制限部１０９が拘束制御指令値としてトルク変化量Δτ_Ｃを出力した場合には、合成部１１０は、τ_Ｄ＋Δτ_Ｃをトルク指令値として出力する。

次いで、駆動制御部１１１は、合成部１１０による合成結果に基づいて、アーム２を駆動する（ステップＳＴ４１１）。この駆動制御部１１１によるアーム２の駆動は、通常のアーム２の制御であるため、その詳細については省略する。

以上のように、この実施の形態１によれば、直接教示装置１は、ロボットが有するアーム２に加えられた外力を検知する外力検知部１０２と、外力検知部１０２により検知された外力に従うアーム２の動きを算出する従動制御演算部１０３と、アーム２の位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部１０４と、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が、当該アーム２の位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部１０５と、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部１０７と、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標値に移動するアーム２の動きを算出する拘束制御演算部１０８と、拘束制御演算部１０８による算出結果を制限する拘束制御制限部１０９と、近接判定部１０５により拘束目標に近づいていると判定された場合に拘束制御演算部１０８による算出結果を有効とし、近接判定部１０５により拘束目標に近づいていないと判定された場合に拘束制御演算部１０８による算出結果を無効とする切換部１０６と、従動制御演算部１０３による算出結果と拘束制御制限部１０９による制限結果とを合成する合成部１１０と、合成部１１０による合成結果に基づいてアーム２を駆動する駆動制御部１１１とを備えた。これにより、実施の形態１に係る直接教示装置１は、操作者がアーム２を操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。

実施の形態２．
実施の形態２では、アーム２が垂直多関節型ロボットアームであり、直接教示装置１による拘束方法が姿勢拘束のみに限定された場合を示す。なお、実施の形態２に係る直接教示装置１の構成例は、実施の形態１に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。

なお、実施の形態２における位置姿勢演算部１０４は、アーム２の姿勢を算出する。
また、実施の形態２における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の姿勢が拘束方向５０５に近接しているかを判定する。

次に、実施の形態２に係る直接教示装置１の動作例について、図８を参照しながら説明する。
以下では、直接教示装置１は、エンドエフェクタ２０１の向き５０２（Ｚ軸）を真下方向５０７又は真横方向５０９に拘束するような姿勢拘束を行うものとする。また、ロボット座標系の各軸の単位ベクトルをｅ_Ｘ，e_Ｙ，e_Ｚで表す。また、アーム２は６軸であるとする。また、切換部１０６は、目標値算出部１０７及び拘束制御演算部１０８の動作を切換えるものとする。また、従動制御演算部１０３は従動制御指令値としてアーム２の各関節角θを算出し、拘束制御演算部１０８は拘束制御指令値としてアーム２の各関節角速度θ（ドット）を算出するものとする。

実施の形態２に係る直接教示装置１の動作例では、図８に示すように、まず、位置姿勢計測部１０１は、アーム２の各関節角θを計測する（ステップＳＴ８０１）。例えば、位置姿勢計測部１０１は、アーム２が有するモータに取付けられたエンコーダ等を用いて、アーム２の関節毎に関節角θを計測する。

次いで、外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する（ステップＳＴ８０２）。この外力検知部１０２による処理は実施の形態１と同じである。なお、外力検知部１０２は重力補償を行うものとする。

次いで、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の各関節角θ（従動制御指令値）を算出する（ステップＳＴ８０３）。この従動制御演算部１０３の動作例について、図９を参照しながら説明する。

従動制御演算部１０３は、例えば図９Ａに示される方法により、外力検知部１０２により検知されたトルクτからアーム２の各関節角θを算出できる。
図９Ａでは、従動制御演算部１０３は、まず、外力検知部１０２により検知されたトルクτにゲインＫ_Ｔを掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、ゲインＫ_Ｔは、アーム２の関節毎に同一でもよいし異なっていてもよい。なお図９Ａに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期Ｔ_Ｓを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。

また、従動制御演算部１０３は、例えば図９Ｂに示される方法により、外力検知部１０２により検知された力Ｆからアーム２の各関節角θを演算できる。
図９Ｂの例では、従動制御演算部１０３は、まず、外力検知部１０２により検知された力ＦにゲインＫ_Ｆを掛けることで、アーム２の位置及び姿勢毎に移動速度を算出する。なお、ゲインＫ_Ｆは、アーム２の位置及び姿勢の各成分毎に同一でもよいし異なっていてもよい。
次に、従動制御演算部１０３は、移動速度にヤコビ行列Ｊの逆行列を掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、従動制御演算部１０３は、ヤコビ行列Ｊの逆行列が存在しない場合は疑似逆行列を掛ける。なお図９Ｂに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期Ｔ_Ｓを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。

次いで、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の姿勢を算出する（ステップＳＴ８０４）。位置姿勢演算部１０４は、順運動学により上記算出を実行できる。以下、位置姿勢演算部１０４により算出されるアーム２の姿勢を回転行列によりＴで表す。また、Ｔを列ベクトルや要素に分解したものを次式（３）のように表す。

次いで、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の姿勢が拘束方向５０５に近づいているかを判定する（ステップＳＴ８０５）。ここでは、拘束方向５０５は真下方向５０７又は真横方向５０９である。また、接近判定のしきい値は０．１［ｒａｄ］であるとする。近接判定部１０５の動作例について、図１０，１１を参照しながら説明する。

近接判定部１０５の動作例では、図１０に示すように、近接判定部１０５は、まず、真上方向（基準方向）５０８とエンドエフェクタ２０１の向き５０２（Ｚ軸）とが成す角度Θを算出する（ステップＳＴ１００１）。すなわち、近接判定部１０５は、次式（４）のように、ｅ_Ｚとｔ_Ｚとの内積から角度Θを算出する。

そして、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内であるかを判定する（ステップＳＴ１００２）。
このステップＳＴ１００２において、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内であると判定した場合（例えば図１１Ａの場合）には、アーム２の姿勢が真横方向５０９に近づいていると判定する（ステップＳＴ１００３）。

一方、ステップＳＴ１００２において、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内ではないと判定した場合には、角度Θが（π－０．１）［ｒａｄ］以上であるかを判定する（ステップＳＴ１００４）。
このステップＳＴ１００４において、近接判定部１０５は、角度Θが（π－０．１）［ｒａｄ］以上であると判定した場合（例えば図１１Ｂの場合）には、アーム２の姿勢が真下方向５０７に近づいていると判定する（ステップＳＴ１００５）。

一方、ステップＳＴ１００４において、近接判定部１０５は、角度Θが（π－０．１）［ｒａｄ］以上ではないと判定した場合（例えば図１１Ｃの場合）には、アーム２の姿勢がどの拘束方向５０５にも近づいていないと判定する（ステップＳＴ１００６）。

次いで、切換部１０６は、近接判定部１０５による判定結果に従い、目標値算出部１０７及び拘束制御演算部１０８の動作を切換える（ステップＳＴ８０６）。
この際、切換部１０６は、近接判定部１０５によりアーム２の姿勢が真横方向５０９に近づいていると判定された場合には、目標値算出部１０７に目標値として真横方向５０９への目標姿勢を算出させ、拘束制御演算部１０８による処理を有効とする。
また、切換部１０６は、近接判定部１０５によりアーム２の姿勢が真下方向５０７に近づいていると判定された場合には、目標値算出部１０７に目標値として真下方向５０７への目標姿勢を算出させ、拘束制御演算部１０８による処理を有効とする。
また、切換部１０６は、近接判定部１０５によりアーム２の姿勢がどの拘束方向５０５にも近づいていないと判定された場合には、目標値算出部１０７による算出を不要し、拘束制御演算部１０８による処理を無効とする。

次いで、目標値算出部１０７は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、目標姿勢を算出する（ステップＳＴ８０７）。例えば、目標値算出部１０７は、真横方向５０９又は真下方向５０７であり、且つ、アーム２の現在の姿勢から最も小さい回転角度Θ_Ｒで到達可能な目標姿勢を算出する。目標値算出部１０７による目標姿勢の算出方法について以下に示す。

まず、目標値算出部１０７は、対象となる目標姿勢毎に、ｅ_Ｚに対する目標角度Θ_＊を算出する。ここで、目標姿勢が真横方向５０９である場合には目標角度Θ_＊はπ／２であり、目標姿勢が真下方向５０７である場合には目標角度Θ_＊はπである。

次に、目標値算出部１０７は、アーム２の姿勢に基づいて、回転角度Θ_Ｒが最も小さい回転軸５１０を算出する。図１２に示すように、この回転軸５１０は、ロボット座標系のＸＹ平面上にあり且つｔ_Ｚと直交する軸であり、すなわち、ｅ_Ｚ及びｔ_Ｚの両方と直交する軸である。よって、目標値算出部１０７は、ｅ_Ｚ及びｔ_Ｚの両方と内積が０となるような単位ベクトルであるｒを１つ求め、それを回転軸５１０とする。なお、目標値算出部１０７は、ｅ_Ｚとｔ_Ｚとが平行の場合にはｒを一意に求めることはできない。しかしながら、これはアーム２の姿勢が真上方向５０８又は真下方向５０７を向いている場合であり、目標値算出部１０７は、ＸＹ平面上の任意のベクトルを回転軸５１０として算出する。

次に、目標値算出部１０７は、算出した目標角度Θ_＊及び回転軸５１０に基づいて、目標姿勢として回転角度Θ_Ｒを算出する。ここで、エンドエフェクタ２０１の向き５０２が真上方向（基準方向）５０８から角度Θである場合、回転角度Θ_Ｒの大きさはΘ_＊－Θとなるが、図１３に示すように回転の向き（正負）を考慮する必要がある。これは、実施の形態２に係る直接教示装置１では、角度Θを求める際に内積を使っているため、エンドエフェクタ２０１の向き５０２の情報が失われているためである。回転の向きを考慮すると、回転角度Θ_Ｒは次式（５）のようになる。すなわち、回転の向きは、ｅ_Ｚ×ｔ_Ｚとｒとが同じ向きである場合には符号の反転は不要であり、ｅ_Ｚ×ｔ_Ｚとｒとが逆の向きである場合には符号の反転が必要である。

目標値算出部１０７では、回転軸５１０を表すｒ＝［ｒ_ｘ，ｒ_ｙ，ｒ_ｚ］^Ｔ（Ｔは転置を表す）と回転角度Θ_Ｒが得られれば、アーム２の姿勢から目標姿勢への回転が得られたことになるので、拘束目標が事実上得られたことになる。目標姿勢の具体的な表現が必要な場合は、目標値算出部１０７は、アーム２の姿勢に前述した回転を適用することで得られる。

上記では、目標値算出部１０７が、エンドエフェクタ２０１の向き５０２（Ｚ軸）が真下方向５０７又は真横方向５０９を向くような目標姿勢を算出する方法について述べた。目標値算出部１０７は、ここから更に回転を加えて所望の姿勢にして、それを目標姿勢とすることも可能である。
例えば、目標値算出部１０７は、アーム２が真下方向５０７を向いた状態からＺ軸周りに回転を加えることで、図７に示すような姿勢を目標姿勢とすることも可能である。また、目標値算出部１０７は、図１４に示すように、アーム２が真横方向５０９を向いた状態から更にＺ軸周りに回転を加え、ｔ_Ｘ又はｔ_Ｙがロボット座標系のＺ軸と平行となる姿勢を目標姿勢とすることも可能である。

次いで、拘束制御演算部１０８は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０７により算出された目標姿勢に移動するアーム２の動き（拘束制御指令値）を算出する（ステップＳＴ８０８）。なお、アーム２を現在の姿勢から目標姿勢へと動かすような回転は、目標値算出部１０７が目標姿勢を算出する際に既に算出しているため、拘束制御演算部１０８は、そのようにアーム２を回転させる動きを算出する。ただし、拘束制御演算部１０８の出力は関節角速度θ（ドット）とする必要があるので、拘束制御演算部１０８は、目標値算出部１０７により算出された回転軸５１０であるｒと回転角度Θ_Ｒの組から関節角速度θ（ドット）を算出する必要がある。以下にその計算の一例を示す。

拘束制御演算部１０８は、まず、回転の角速度の大きさ｜ω｜を決める。例えば、制限を受けなければ１秒で目標姿勢に到達するように制御するのであれば、｜ω｜＝θ_Ｒとする。
次に、拘束制御演算部１０８は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。ヤコビ行列Ｊについては次式（６）が成り立つものとする。

ここで、ｖ_Ｘ，ｖ_Ｙ，ｖ_Ｚはエンドエフェクタ２０１の先端のＸ，Ｙ，Ｚ軸方向の速度であり、ω_Ｘ，ω_Ｙ，ω_Ｚはアーム２の姿勢がＸ軸，Ｙ軸，Ｚ軸について回転する角速度であり、θ_１（ドット）～θ_６（ドット）は各関節角速度である。

次に、拘束制御演算部１０８は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ω_Ｘ，ω_Ｙ，ω_Ｚ］^Ｔを算出する。
次に、拘束制御演算部１０８は、次式（７）を満たすようなアーム２の関節毎の関節角速度θ_１（ドット）～θ_６（ドット）を算出する。なお、拘束制御演算部１０８は、ヤコビ行列Ｊに逆行列が存在しない場合は疑似逆行列等を用いて関節角速度θ_１（ドット）～θ_６（ドット）を算出する。

次いで、拘束制御制限部１０９は、拘束制御演算部１０８による算出結果を制限する（ステップＳＴ８０９）。
まず、拘束制御制限部１０９は、拘束制御演算部１０８により算出されたアーム２の関節毎の関節角速度θ（ドット）に対する上限θ_ＭＡＸ（ドット）を決める。上限θ_ＭＡＸ（ドット）は関節毎に同一でもよいし異なっていてもよい。また、従動制御演算部１０３でリミッタを適用している場合は、拘束制御制限部１０９は、そのリミッタの上限よりも上限θ_ＭＡＸ（ドット）を小さくする必要がある。リミッタの上限よりも上限θ_ＭＡＸ（ドット）を小さくしないと、操作者による操作が拘束制御よりも制限され、拘束付き直接教示から脱することができなくなる。現実的には、拘束制御制限部１０９は、上限θ_ＭＡＸ（ドット）を、リミッタの上限に比して半分以下にすることが好ましい。その程度に制限すれば、操作者による操作で拘束を脱することは比較的容易になる。

次に、拘束制御制限部１０９は、全ての関節角速度θ（ドット）が上限θ_ＭＡＸ（ドット）を超えないように制限する。このとき、回転の方向を変えないように制限することが好ましい。そのような演算を行う手順の一例を以下に示す。なお、ｉ番目の関節角速度θ_ｉ（ドット）の上限をθ_{ｉ，ＭＡＸ}（ドット）で表す。
拘束制御制限部１０９は、まず、アーム２の関節毎に、関節角速度θ_ｉ（ドット）の大きさと上限θ_{ｉ，ＭＡＸ}（ドット）の比ｃ_ｉを｜θ_ｉ（ドット）｜／θ_{ｉ，ＭＡＸ}（ドット）で算出する。
次に、拘束制御制限部１０９は、この比ｃ_ｉの最大値ｃを算出する。
次に、拘束制御制限部１０９は、アーム２の関節毎に、関節角速度θ_ｉ（ドット）を最大値ｃで割って制限後の関節角速度θ_ｉ ^＊（ドット）を算出する。すなわち制限後の関節角速度θ_ｉ ^＊（ドット）はθ_ｉ（ドット）／ｃである。

次いで、合成部１１０は、従動制御演算部１０３から出力された従動制御指令値（関節角θ）と拘束制御制限部１０９から出力された拘束制御指令値（関節角速度θ（ドット））とを合成する（ステップＳＴ８１０）。
例えば、従動制御演算部１０３が関節角θ＝［θ_１，・・・，θ_ｍ］^Ｔを出力し、拘束制御制限部１０９が関節角速度θ（ドット）＝［θ_１（ドット），・・・，θ_ｍ（ドット）］^Ｔを出力した場合、制御周期Ｔ_Ｓから、合成部１１０の出力である関節角指令値はθ＋Ｔ_Ｓθ（ドット）となる。ただし、ｍは駆動制御部１１１が制御するアーム２の関節数である。

次いで、駆動制御部１１１は、合成部１１０による合成結果（関節角指令値）に基づいて、アーム２を駆動する（ステップＳＴ８１１）。

なお上記では一部、６軸（関節数が６個）のアーム２を前提として説明及び例示を行ったが、６軸以外のアーム２であっても同様に実施可能である。

実施の形態３．
実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態３に係る直接教示装置１では、拘束方法が軸拘束のみに限定された場合を示す。軸拘束とは、アーム２の位置を所定の拘束軸５０４に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態３に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図１５に示すようなロボット座標系のＺ軸に平行な直線５１１への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束軸５０４及び拘束方向５０５は任意に設定可能である。
なお、実施の形態３に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。

なお、実施の形態３における位置姿勢演算部１０４は、アーム２の位置及び姿勢を算出する。
また、実施の形態３における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束軸５０４に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。

図１６は、実施の形態３に係る直接教示装置１の動作例を示すフローチャートである。図１６に示す実施の形態３に係る直接教示装置１において、図８に示す実施の形態２に係る直接教示装置１と動作が異なるのは、位置姿勢演算部１０４、近接判定部１０５、目標値算出部１０７及び拘束制御演算部１０８であり、これらの構成の動作例についてのみ説明を行う。

まず、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置及び姿勢を算出する（ステップＳＴ１６０４）。実施の形態３ではアーム２の位置及び姿勢の両方を拘束するため、位置姿勢演算部１０４は、アーム２の位置及び姿勢の両方を算出する必要がある。位置姿勢演算部１０４は、順運動学により上記算出を実行できる。

また、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束軸５０４に近づいているか及びアーム２の姿勢が拘束方向５０５に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム２の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する（ステップＳＴ１６０５）。例えば、近接判定部１０５は、アーム２の位置が拘束軸５０４に近づき且つアーム２の姿勢が拘束方向５０５に近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。近接判定部１０５による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、近接判定部１０５による拘束軸５０４に対する判定について説明する。

この際、近接判定部１０５は、例えば図１７に示すように、エンドエフェクタ２０１の位置５０１から拘束軸５０４に対して垂線５１２をおろし、その垂線５１２の長さが所定の値より小さければアーム２の位置が拘束軸５０４に近づいていると判定する。例えばアーム２の位置の座標を［ｘ，ｙ，ｚ］で表し、直線５１１のＸ座標とＹ座標をそれぞれｘ^＊，ｙ^＊で表す。この場合、垂線５１２の長さは次式（８）のようになる。

そして、近接判定部１０５は、式（８）に示す垂線５１２の長さが所定の値よりも小さければ、アーム２の位置が拘束軸５０４に近づいていると判定し、それ以外の場合にはアーム２の位置が拘束軸５０４には近づいていないと判定する。なお、近接判定部１０５は、拘束軸５０４がロボット座標系のＺ軸に平行な場合以外でも、同様にして拘束軸５０４に対しておろした垂線の長さから接近判定が可能である。

また、目標値算出部１０７は、実施の形態２で説明した目標姿勢の算出に加え、アーム２の位置を拘束軸５０４へと移動させる場合の目標位置の算出を行う（ステップＳＴ１６０７）。例えば、目標値算出部１０７は、エンドエフェクタ２０１の位置５０１から図１７に示すように拘束軸５０４へと垂線５１２をおろした位置５１３を目標位置とする。ここで、目標値算出部１０７は、アーム２の位置をロボット座標系のＺ軸に平行な拘束軸５０４に拘束させるのであれば、目標位置は［ｘ^＊，ｙ^＊，ｚ］となる。

また、拘束制御演算部１０８は、実施の形態２と同様に、ヤコビ行列Ｊを用いてアーム２の関節毎に関節角速度θ（ドット）を算出する（ステップＳＴ１６０８）。ただし、実施の形態３における拘束制御演算部１０８では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態２に対して以下のように変わる。

まず、拘束制御演算部１０８は、回転の角速度の大きさ｜ω｜を決める。
次に、拘束制御演算部１０８は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
次に、拘束制御演算部１０８は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ω_Ｘ，ω_Ｙ，ω_Ｚ］^Ｔを算出する。ここまでの動作は、実施の形態２と同様である。

次に、拘束制御演算部１０８は、アーム２の位置を拘束軸５０４へと移動させる速さ｜ｖ｜を決める。例えば、制限を受けなければ１秒で目標位置に到達するように制御するのであれば、拘束制御演算部１０８は、｜ｖ｜＝√｛（ｘ－ｘ^＊）^２＋（ｙ－ｙ^＊）^２｝とする。また、拘束制御演算部１０８は、ある定数ｖ^＊を決めて｜ｖ｜＝ｖ^＊としてもよい。
次に、拘束制御演算部１０８は、アーム２の位置に関する速度ベクトルｖ＝［ｖ_Ｘ，ｖ_Ｙ，０］^Ｔを算出する。拘束制御演算部１０８は、ロボット座標系のＺ軸に平行な拘束軸５０４に拘束させる場合はｖ＝｜ｖ｜［ｘ^＊－ｘ，ｙ^＊－ｙ，０］／√｛（ｘ－ｘ^＊）^２＋（ｙ－ｙ^＊）^２｝より算出可能である。
次に、拘束制御演算部１０８は、アーム２の関節毎に関節角速度θ（ドット）を次式（９）より算出する。

その他の動作は実施の形態２と同じになる。
なお、実施の形態３に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された軸に拘束する制御となる。

実施の形態４．
実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態４に係る直接教示装置１では、拘束方法が面拘束のみに限定された場合を示す。面拘束とは、アーム２の位置を所定の拘束面５０３に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態４に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図１８に示すようなＺ軸座標がＸ軸座標及びＹ軸座標の関数ｚ＝ｆ（ｘ，ｙ）で表される曲面５１３への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束面５０３及び拘束方向５０５は任意に設定可能であり、例えば拘束面５０３が関数ｘ＝ｆ（ｙ，ｚ）又はｙ＝ｆ（ｚ，ｘ）で表される場合等についても同様の方法で実施可能である。
なお、実施の形態４に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。

なお、実施の形態４における位置姿勢演算部１０４は、アーム２の位置及び姿勢を算出する。
また、実施の形態４における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束面５０３に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。

図１９は、実施の形態４に係る直接教示装置１の動作例を示すフローチャートである。図１９に示す実施の形態４に係る直接教示装置１において、図１６に示す実施の形態３に係る直接教示装置１と動作が異なるのは、近接判定部１０５、目標値算出部１０７及び拘束制御演算部１０８であり、これらの構成の動作例についてのみ説明を行う。

まず、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束面５０３に近づいているか及びアーム２の姿勢が拘束方向５０５に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム２の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する（ステップＳＴ１９０５）。例えば、近接判定部１０５は、アーム２の位置が拘束面５０３に近づき且つアーム２の姿勢が拘束方向５０５に近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。近接判定部１０５による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、近接判定部１０５による拘束面５０３に対する判定について説明する。

この際、近接判定部１０５は、エンドエフェクタ２０１の位置５０１から拘束面５０３に対してロボット座標系のＺ軸に平行な直線をおろし、その直線の長さが所定の値より小さければアーム２の位置が拘束面５０３に近づいていると判定する。例えばアーム２の位置の座標が［ｘ，ｙ，ｚ］である場合、上記直線の長さは｛ｚ－ｆ（ｘ，ｙ）｝となる。そして、近接判定部１０５は、この長さが所定の値よりも小さければ、アーム２の位置が拘束面５０３に近づいていると判定し、それ以外の場合にはアーム２の位置が拘束面５０３には近づいていないと判定する。

また、目標値算出部１０７は、実施の形態２で説明した目標姿勢の算出に加え、アーム２の位置を拘束面５０３へと移動させる場合の目標位置の算出を行う（ステップＳＴ１９０７）。例えば、目標値算出部１０７は、アーム２の位置の座標が［ｘ，ｙ，ｚ］である場合、Ｚ軸座標だけを曲面上の座標に置き換えた［ｘ，ｙ，ｆ（ｘ，ｙ）］を目標位置とする。

また、拘束制御演算部１０８は、実施の形態２と同様に、ヤコビ行列Ｊを用いてアーム２の関節毎に関節角速度θ（ドット）を算出する（ステップＳＴ１９０８）。ただし、実施の形態４における拘束制御演算部１０８では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態２に対して以下のように変わる。

まず、拘束制御演算部１０８は、回転の角速度の大きさ｜ω｜を決める。
次に、拘束制御演算部１０８は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
次に、拘束制御演算部１０８は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ω_Ｘ，ω_Ｙ，ω_Ｚ］^Ｔを算出する。ここまでの動作は、実施の形態２と同様である。

次に、拘束制御演算部１０８は、アーム２の位置を拘束軸５０４へと移動させる速さ｜ｖ｜を決める。例えば、制限を受けなければ１秒で目標位置に到達するように制御するのであれば、拘束制御演算部１０８は、｜ｖ｜＝｜ｚ－ｆ（ｘ，ｙ）｜とする。また、拘束制御演算部１０８は、速さ｜ｖ｜をある定数値にしてもよい。
次に、拘束制御演算部１０８は、アーム２の位置に関する速度ベクトルｖ＝［０，０，ｖ_Ｚ］^Ｔをｖ＝［０，０，｜ｖ｜］より算出する。
次に、拘束制御演算部１０８は、アーム２の関節毎に関節角速度θ（ドット）を次式（１０）より算出する。

その他の動作は実施の形態２，３と同じになる。
なお、実施の形態４に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された面に拘束する制御となる。また、実施の形態４に係る直接教示装置１において、アーム２の位置から拘束面５０３へ垂線を下した方向を目標姿勢とした場合には、エンドエフェクタ２０１が拘束面５０３に対して常に垂直となるような姿勢拘束になる。

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

１ 直接教示装置
２ アーム
１０１ 位置姿勢計測部
１０２ 外力検知部
１０３ 従動制御演算部
１０４ 位置姿勢演算部
１０５ 近接判定部
１０６ 切換部
１０７ 目標値算出部
１０８ 拘束制御演算部
１０９ 拘束制御制限部
１１０ 合成部
１１１ 駆動制御部
２０１ エンドエフェクタ

Claims (7)

1. ロボットが有するアームに加えられた外力を検知する外力検知部と、
   前記外力検知部により検知された外力に従う前記アームの動きを算出する従動制御演算部と、
   前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、
   前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、
   拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部と、
   前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出する拘束制御演算部と、
   前記拘束制御演算部による算出結果を制限する拘束制御制限部と、
   前記近接判定部により拘束目標に近づいていると判定された場合に前記拘束制御演算部による算出結果を有効とし、前記近接判定部により拘束目標に近づいていないと判定された場合に前記拘束制御演算部による算出結果を無効とする切換部と、
   前記従動制御演算部による算出結果と前記拘束制御制限部による制限結果とを合成する合成部と、
   前記合成部による合成結果に基づいて前記アームを駆動する駆動制御部と
   を備えた直接教示装置。
2. 前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、
   前記従動制御演算部は、位置姿勢計測部による計測結果に基づいて、前記外力検知部により検知された外力に従う前記アームの動きを算出し、
   前記位置姿勢演算部は、前記従動制御演算部による算出結果に基づいて、前記アームの位置姿勢を算出する
   ことを特徴とする請求項１記載の直接教示装置。
3. 前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、
   前記位置姿勢演算部は、前記位置姿勢計測部による計測結果に基づいて、前記アームの位置姿勢を算出する
   ことを特徴とする請求項１記載の直接教示装置。
4. 前記アームは垂直多関節型ロボットアームであり、
   前記位置姿勢演算部は、前記アームの姿勢を算出し、
   前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの姿勢が、拘束目標である拘束方向に近づいているかを判定する
   ことを特徴とする請求項１から請求項３のうちの何れか１項記載の直接教示装置。
5. 前記アームは垂直多関節型ロボットアームであり、
   前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
   前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束軸に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
   ことを特徴とする請求項１から請求項３のうちの何れか１項記載の直接教示装置。
6. 前記アームは垂直多関節型ロボットアームであり、
   前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
   前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束面に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
   ことを特徴とする請求項１から請求項３のうちの何れか１項記載の直接教示装置。
7. 外力検知部が、ロボットが有するアームに加えられた外力を検知するステップと、
   従動制御演算部が、前記外力検知部により検知された外力に従う前記アームの動きを算出するステップと、
   位置姿勢演算部が、前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出するステップと、
   近接判定部が、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定するステップと、
   目標値算出部が、拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出するステップと、
   拘束制御演算部が、前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出するステップと、
   拘束制御制限部が、前記拘束制御演算部による算出結果を制限するステップと、
   切換部が、前記近接判定部により拘束目標に近づいていると判定された場合に前記拘束制御演算部による算出結果を有効とし、前記近接判定部により拘束目標に近づいていないと判定された場合に前記拘束制御演算部による算出結果を無効とするステップと、
   合成部が、前記従動制御演算部による算出結果と前記拘束制御制限部による制限結果とを合成するステップと、
   駆動制御部が、前記合成部による合成結果に基づいて前記アームを駆動するステップと
   を有する直接教示方法。

Images