知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2022-06-27
(45)【発行日】2022-07-05
(54)【発明の名称】ロボットの直接教示装置及び直接教示方法
(51)【国際特許分類】
B25J 9/22 20060101AFI20220628BHJP
G05B 19/42 20060101ALI20220628BHJP
【ＦＩ】
B25J9/22 A
G05B19/42 D
【請求項の数】
9
(21)【出願番号】P 2018167821
(22)【出願日】2018-09-07
(65)【公開番号】P2020040136
(43)【公開日】2020-03-19
【審査請求日】2021-07-28
(73)【特許権者】
【識別番号】000006666
【氏名又は名称】アズビル株式会社
(74)【代理人】
【識別番号】100101133
【弁理士】
【氏名又は名称】濱田 初音
(74)【代理人】
【識別番号】100199749
【弁理士】
【氏名又は名称】中島 成
(74)【代理人】
【識別番号】100188880
【弁理士】
【氏名又は名称】坂元 辰哉
(74)【代理人】
【識別番号】100197767
【弁理士】
【氏名又は名称】辻岡 将昭
(74)【代理人】
【識別番号】100201743
【弁理士】
【氏名又は名称】井上 和真
(72)【発明者】
【氏名】田原 鉄也
【審査官】杉田 隼一
(56)【参考文献】
【文献】特開平５－２８５８７０（ＪＰ，Ａ）
【文献】特開平５－３０３４２５（ＪＰ，Ａ）
【文献】特開平９－１４１５８０（ＪＰ，Ａ）
【文献】特開平６－９５７３９（ＪＰ，Ａ）
【文献】特開２０１９－９３５２６（ＪＰ，Ａ）
【文献】特開２０１９－５５４５９（ＪＰ，Ａ）
【文献】特開昭６２－１９９３７６（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ２５Ｊ ９／２２
Ｇ０５Ｂ １９／４２
(57)【特許請求の範囲】
【請求項1】
ロボットが有するアームに加えられた外力を検知する外力検知部と、前記外力検知部により検知された外力に従う前記アームの動きを算出する従動制御演算部と、
前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、
前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、
操作者が拘束付き直接教示を解除するための解除操作を行ったかを判定する解除操作判定部と、
拘束付き直接教示へと切換え可能な状態への復帰条件を満たしたかを判定する復帰判定部と、
拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束付き直接教示における拘束制御の目標値を算出する目標値算出部と、
前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出する拘束制御演算部と、
自機が第一の状態である場合において前記近接判定部により拘束目標に近づいていると判定された場合に自機を第二の状態へと切換え、自機が第二の状態である場合において前記解除操作判定部により操作者が解除操作を行ったと判定された場合に自機を第三の状態へと切換え、自機が第三の状態である場合において前記復帰判定部により復帰条件を満たしたと判定された場合に自機を第一の状態へと切換え、自機が第二の状態にある場合に前記拘束制御演算部による処理を有効とし、自機が第一の状態又は第三の状態にある場合に前記拘束制御演算部による処理を無効とする切換部と、
前記従動制御演算部による算出結果と前記拘束制御演算部による算出結果とを合成する合成部と、
前記合成部による合成結果に基づいて前記アームを駆動する駆動制御部と
を備えた直接教示装置。
【請求項2】
前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、前記従動制御演算部は、位置姿勢計測部による計測結果に基づいて、前記外力検知部により検知された外力に従う前記アームの動きを算出し、
前記位置姿勢演算部は、前記従動制御演算部による算出結果に基づいて、前記アームの位置姿勢を算出する
ことを特徴とする請求項１記載の直接教示装置。
【請求項3】
前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、前記位置姿勢演算部は、前記位置姿勢計測部による計測結果に基づいて、前記アームの位置姿勢を算出する
ことを特徴とする請求項１記載の直接教示装置。
【請求項4】
前記解除操作判定部は、前記外力検知部による検知結果又は前記位置姿勢演算部による算出結果に基づいて、操作者が解除操作を行ったかを判定することを特徴とする請求項１記載の直接教示装置。
【請求項5】
前記復帰判定部は、操作者が解除操作を行った後一定時間が経過した場合、又は、前記位置姿勢演算部により算出された前記アームの位置姿勢が拘束目標に近づいていない場合に、復帰条件を満たしたと判定することを特徴とする請求項１記載の直接教示装置。
【請求項6】
前記アームは垂直多関節型ロボットアームであり、前記位置姿勢演算部は、前記アームの姿勢を算出し、
前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの姿勢が、拘束目標である拘束方向に近づいているかを判定する
ことを特徴とする請求項１から請求項５のうちの何れか１項記載の直接教示装置。
【請求項7】
前記アームは垂直多関節型ロボットアームであり、前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束軸に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
ことを特徴とする請求項１から請求項５のうちの何れか１項記載の直接教示装置。
【請求項8】
前記アームは垂直多関節型ロボットアームであり、前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束面に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
ことを特徴とする請求項１から請求項５のうちの何れか１項記載の直接教示装置。
【請求項9】
外力検知部が、ロボットが有するアームに加えられた外力を検知するステップと、従動制御演算部が、前記外力検知部により検知された外力に従う前記アームの動きを算出するステップと、
位置姿勢演算部が、前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出するステップと、
近接判定部が、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定するステップと、
解除操作判定部が、操作者が拘束付き直接教示を解除するための解除操作を行ったかを判定するステップと、
復帰判定部が、拘束付き直接教示へと切換え可能な状態への復帰条件を満たしたかを判定するステップと、
目標値算出部が、拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束付き直接教示における拘束制御の目標値を算出するステップと、
拘束制御演算部が、前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出するステップと、
切換部が、自機が第一の状態である場合において前記近接判定部により拘束目標に近づいていると判定された場合に自機を第二の状態へと切換え、自機が第二の状態である場合において前記解除操作判定部により操作者が解除操作を行ったと判定された場合に自機を第三の状態へと切換え、自機が第三の状態である場合において前記復帰判定部により復帰条件を満たしたと判定された場合に自機を第一の状態へと切換え、自機が第二の状態にある場合に前記拘束制御演算部による処理を有効とし、自機が第一の状態又は第三の状態にある場合に前記拘束制御演算部による処理を無効とするステップと、
合成部が、前記従動制御演算部による算出結果と前記拘束制御演算部による算出結果とを合成するステップと、
駆動制御部が、前記合成部による合成結果に基づいて前記アームを駆動するステップと
を有する直接教示方法。
【発明の詳細な説明】
【技術分野】
【0001】
この発明は、ロボットの直接教示を行う直接教示装置及び直接教示方法に関する。
【背景技術】
【0002】
産業用のロボットでは、ロボットに作業をさせるために、前もって教示（ティーチング）と呼ばれる作業が実施される。このロボットの教示を行う方法の１つとして、直接教示（ダイレクト教示）と呼ばれる方法がある。
【0003】
例えば特許文献１では、力センサを用いたロボットの直接教示方法が開示されている。また、例えば特許文献２では、トルク検出手段を用いたロボットの直接教示方法が開示されている。これらの特許文献で開示されている直接教示装置の概略構成を図２３に示す。
【0004】
図２３に示す直接教示装置１１では、まず、外力検知部１１０１が、力センサ又はトルクセンサ等を用いて、ロボットが有するアーム２に対して操作者により加えられた外力を検知する。次いで、従動制御演算部１１０２が、外力検知部１１０１により検知された外力に従うアームの動きを算出（従動制御演算）する。なお、従動制御演算部１１０２は、従動制御演算において、位置姿勢計測部（不図示）により計測されたアームの位置姿勢に関するパラメータを用いる場合もある。なお、アームの位置姿勢とは、アームの位置及びアームの姿勢のうちの少なくとも一方を意味する。また、上記パラメータとしては、アームの位置、アームの姿勢、又は、アームの関節角等が挙げられる。そして、従動制御演算部１１０２は、従動制御演算の結果に基づいて更新した従動制御指令値を駆動制御部１１０３に通知する。次いで、駆動制御部１１０３は、従動制御演算部１１０２により通知された従動制御指令値に従ってアーム２を駆動する。
この一連の動作により、図２３に示す直接教示装置１１は、操作者により加えられた外力に従ってアーム２が動くように制御できる。そして、直接教示装置１１は、制御によりアーム２の位置及び姿勢が操作者の意図する状態となった場合に、その際の上記パラメータを教示点として記録する。この直接教示装置１１により記録された教示点はロボットが作業する際に使用される。
この一連の動作により、図２３に示す直接教示装置１１は、操作者により加えられた外力に従ってアーム２が動くように制御できる。そして、直接教示装置１１は、制御によりアーム２の位置及び姿勢が操作者の意図する状態となった場合に、その際の上記パラメータを教示点として記録する。この直接教示装置１１により記録された教示点はロボットが作業する際に使用される。
【0005】
このように、ロボットの直接教示は、操作者がアームを直接操作して位置及び姿勢を教示するため、操作者にとって直観的でわかりやすいという利点を持つ。一方で、アームが操作者により加えられた外力に従ってそのまま動くという特徴は利点ばかりではない。
【0006】
例えば、操作者が、ロボットに対して、ある点Ｐの位置を教示した後に、その真下の別の点Ｑの位置を教示するというケースを考える。このケースでは、操作者は、真下方向（Ｚ軸方向）にアームを動かすことになる。しかしながら、操作者がアームに対して正確に真下方向へ外力を加えることは難しく、アームのＸ軸座標及びＹ軸座標がずれることが多い。後からアームのＸ軸座標及びＹ軸座標だけをティーチングペンダント等を用いて修正することは可能であるが、それでは直接教示の利点が減ずる。
また、例えば、操作者が、アームの先端に設けられたエンドエフェクタを真下に向けたまま教示を行うケースを考える。このケースにおいても、操作者がアームに対して正確に真下に向けた状態を維持したままアームを直接操作することは難しく、アームの姿勢がずれることが多い。
以上のように、直接教示は、直観的で分かりやすい反面、操作が難しいケースも存在する。
また、例えば、操作者が、アームの先端に設けられたエンドエフェクタを真下に向けたまま教示を行うケースを考える。このケースにおいても、操作者がアームに対して正確に真下に向けた状態を維持したままアームを直接操作することは難しく、アームの姿勢がずれることが多い。
以上のように、直接教示は、直観的で分かりやすい反面、操作が難しいケースも存在する。
【0007】
これに対し、例えば特許文献３，４では、上記のような課題を解決する技術が開示されている。
【0008】
特許文献３では、モード切換スイッチにより、動作モードを、位置方向移動モード、位置移動モード又は方向移動モードに切換え可能とする直接教示装置が開示されている。位置移動モードでは、アームの先端の姿勢を維持したまま位置のみを移動可能としている。また、方向移動モードでは、アームの先端の位置を維持したまま姿勢のみを変更可能としている。この直接教示装置により、上記のような直接教示における操作の難しさをある程度緩和可能となる。なお、通常の直接教示は位置方向移動モードで実現可能である。
【0009】
また、特許文献４では、入力装置により、動作モードを、拘束モード又は全方向移動モードに切換え可能とする直接教示装置が開示されている。拘束モードでは、エンドエフェクタの先端を特定の拘束軸又は拘束面に沿って移動可能としている。また、全方向移動モードでは、通常の直接教示を実施可能としている。この直接教示装置は、直接教示の途中で拘束モードに切換えることにより、アームの先端を正確に上下方向に動かすことが可能となり、直接教示における操作の難しさが軽減される。
【先行技術文献】
【特許文献】
【0010】
【文献】特開平０５－２０４４４１号公報
【文献】特開平０５－２５００２９号公報
【文献】特開平０５－２８５８７０号公報
【文献】特開平０５－３０３４２５号公報
【発明の概要】
【発明が解決しようとする課題】
【0011】
上記のように、特許文献３，４に開示された直接教示装置は、アームの操作に拘束を課さない通常の直接教示を行うモードと、アームの操作に拘束を課す拘束付き直接教示を行うモードとを切換える。そして、特許文献３ではモード切換スイッチによって、また、特許文献４では入力装置によって、モードの切換えが行われており、操作者が手でスイッチ等を操作することで切換え可能となっている。
【0012】
しかしながら、直接教示では、操作者は手でアームを直接操作しているため、モードの切換えを手で操作するのは便利ではない。例えば、操作者が両手でアームを操作している場合、モードの切換えを操作する際にはアームの操作を一旦停止してアームを片手で保持することになる。通常時に両手で保持しているアームを片手で保持すると、アームの位置姿勢がずれることも考えられる。その結果、拘束付き直接教示の効果を減ずることにもなりかねない。
【0013】
この発明は、上記のような課題を解決するためになされたもので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能な直接教示装置を提供することを目的としている。
【課題を解決するための手段】
【0014】
この発明に係る直接教示装置は、ロボットが有するアームに加えられた外力を検知する外力検知部と、外力検知部により検知された外力に従うアームの動きを算出する従動制御演算部と、アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、位置姿勢演算部による算出結果に基づいて、アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、操作者が拘束付き直接教示を解除するための解除操作を行ったかを判定する解除操作判定部と、拘束付き直接教示へと切換え可能な状態への復帰条件を満たしたかを判定する復帰判定部と、拘束目標及び位置姿勢演算部による算出結果に基づいて、拘束付き直接教示における拘束制御の目標値を算出する目標値算出部と、位置姿勢演算部による算出結果に基づいて、目標値算出部により算出された目標値に移動するアームの動きを算出する拘束制御演算部と、自機が第一の状態である場合において近接判定部により拘束目標に近づいていると判定された場合に自機を第二の状態へと切換え、自機が第二の状態である場合において解除操作判定部により操作者が解除操作を行ったと判定された場合に自機を第三の状態へと切換え、自機が第三の状態である場合において復帰判定部により復帰条件を満たしたと判定された場合に自機を第一の状態へと切換え、自機が第二の状態にある場合に拘束制御演算部による処理を有効とし、自機が第一の状態又は第三の状態にある場合に拘束制御演算部による処理を無効とする切換部と、従動制御演算部による算出結果と拘束制御演算部による算出結果とを合成する合成部と、合成部による合成結果に基づいてアームを駆動する駆動制御部とを備えたことを特徴とする。
【発明の効果】
【0015】
この発明によれば、上記のように構成したので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。
【図面の簡単な説明】
【0016】
【図1】この発明の実施の形態１に係る直接教示装置の構成例を示す図である。
【図4】この発明の実施の形態１に係る直接教示装置の動作例を示すフローチャートである。
【図5】この発明の実施の形態１における近接判定部の動作例を説明する図である（姿勢拘束の場合）。
【図6】この発明の実施の形態１における解除操作判定部の動作例を説明する図である（外力を用いた場合）。
【図7】この発明の実施の形態１における解除操作判定部の動作例を説明する図である（アームの位置姿勢を用いた場合）。
【図8】この発明の実施の形態１における切換部の動作例を説明する図である。
【図9】この発明の実施の形態１における目標値算出部の動作例を説明する図である（姿勢拘束の場合）。
【図10】この発明の実施の形態１における目標値算出部の動作例を説明する図である（姿勢拘束の場合）。
【図11】この発明の実施の形態２に係る直接教示装置の動作例を示すフローチャートである。
【図13】この発明の実施の形態２における近接判定部の動作例を示すフローチャートである。
【図17】この発明の実施の形態２における目標値算出部の動作例を説明する図である。
【図18】この発明の実施の形態３に係る直接教示装置による軸拘束を説明する図である。
【図19】この発明の実施の形態３に係る直接教示装置の動作例を示すフローチャートである。
【図20】この発明の実施の形態３における近接判定部の動作例を説明する図である。
【図21】この発明の実施の形態４に係る直接教示装置による面拘束を説明する図である。
【図22】この発明の実施の形態４に係る直接教示装置の動作例を示すフローチャートである。
【図23】従来の直接教示装置の構成例を示す図である。
【発明を実施するための形態】
【0017】
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る直接教示装置１の構成例を示す図である。
直接教示装置１は、ロボットの直接教示を行う。この直接教示装置１は、直接教示方法として、ロボットが有するアーム２の位置姿勢に対して拘束を課さない通常の直接教示と、アーム２の位置姿勢を拘束目標に拘束する拘束付き直接教示とを切換え可能である。
なお、アーム２の位置姿勢とは、アーム２の位置及びアーム２の姿勢のうちの少なくとも一方を意味する。また、アーム２の位置とは、アーム２の先端に設けられたエンドエフェクタ２０１の位置５０１を意味し、アーム２の姿勢とはエンドエフェクタ２０１の向き５０２を意味する。また、拘束目標とは、アーム２の位置姿勢の拘束先である。アーム２の位置に対する拘束目標としては、拘束面５０３及び拘束軸５０４がある。拘束面５０３の種別としては、平面及び曲面がある。拘束軸５０４の種別としては、直線及び曲線がある。アーム２の姿勢に対する拘束目標としては、拘束方向５０５がある。また、直接教示装置１で用いる拘束目標は１つに限らず複数でもよい。また、ロボットに対しては基準となるロボット座標系が設定され、エンドエフェクタ２０１に対してはツール座標系が設定される（図９等参照）。
実施の形態１．
図１はこの発明の実施の形態１に係る直接教示装置１の構成例を示す図である。
直接教示装置１は、ロボットの直接教示を行う。この直接教示装置１は、直接教示方法として、ロボットが有するアーム２の位置姿勢に対して拘束を課さない通常の直接教示と、アーム２の位置姿勢を拘束目標に拘束する拘束付き直接教示とを切換え可能である。
なお、アーム２の位置姿勢とは、アーム２の位置及びアーム２の姿勢のうちの少なくとも一方を意味する。また、アーム２の位置とは、アーム２の先端に設けられたエンドエフェクタ２０１の位置５０１を意味し、アーム２の姿勢とはエンドエフェクタ２０１の向き５０２を意味する。また、拘束目標とは、アーム２の位置姿勢の拘束先である。アーム２の位置に対する拘束目標としては、拘束面５０３及び拘束軸５０４がある。拘束面５０３の種別としては、平面及び曲面がある。拘束軸５０４の種別としては、直線及び曲線がある。アーム２の姿勢に対する拘束目標としては、拘束方向５０５がある。また、直接教示装置１で用いる拘束目標は１つに限らず複数でもよい。また、ロボットに対しては基準となるロボット座標系が設定され、エンドエフェクタ２０１に対してはツール座標系が設定される（図９等参照）。
【0018】
この直接教示装置１は、図１に示すように、位置姿勢計測部１０１、外力検知部１０２、従動制御演算部１０３、位置姿勢演算部１０４、近接判定部１０５、解除操作判定部１０６、復帰判定部１０７、切換部１０８、目標値算出部１０９、拘束制御演算部１１０、合成部１１１及び駆動制御部１１２を備えている。なお、従動制御演算部１０３、位置姿勢演算部１０４、近接判定部１０５、解除操作判定部１０６、復帰判定部１０７、切換部１０８、目標値算出部１０９、拘束制御演算部１１０、合成部１１１及び駆動制御部１１２は、システムＬＳＩ（Ｌａｒｇｅ－Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｉｏｎ）等の処理回路、又はメモリ等に記憶されたプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）等により実現される。
【0019】
位置姿勢計測部１０１は、アーム２の位置姿勢に関するパラメータを計測する。なお、アーム２の位置姿勢に関するパラメータとしては、アーム２の位置、アーム２の姿勢、又は、アーム２の関節角θ等が挙げられる。
【0020】
外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する。例えば、外力検知部１０２は、アーム２の先端に取付けられた力センサを用い、この力センサにより計測された力Ｆを上記外力として検知してもよい。また、例えば、外力検知部１０２は、アーム２のモータ駆動軸に取付けられたトルクセンサを用い、このトルクセンサにより計測されたトルクτを上記外力として検知してもよい。また、外力検知部１０２は、上記のようにセンサを用いて外力を直接計測するのではなく、アーム２が有するモータの電流又はアーム２の関節角θの計測値から間接的に外力を算出する外力オブザーバを用いて上記外力を検知してもよい。
【0021】
従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の動き（従動制御指令値）を算出する。
【0022】
なお、位置姿勢計測部１０１、外力検知部１０２及び従動制御演算部１０３は、拘束が無い通常の直接教示で用いられる構成と同様であり、公知技術である。
【0023】
位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置姿勢を算出する。
【0024】
なお図１では、従動制御演算部１０３が位置姿勢計測部１０１による計測結果を用い、位置姿勢演算部１０４が従動制御演算部１０３による算出結果を用いている。
しかしながら、これに限らず、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果は用いず、外力検知部１０２により検知された外力に従うアーム２の動きを演算してもよい。この場合、位置姿勢演算部１０４は、位置姿勢計測部１０１による計測結果に基づいてアーム２の位置姿勢を算出する。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方を用いてアーム２の位置姿勢を算出してもよい。
しかしながら、これに限らず、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果は用いず、外力検知部１０２により検知された外力に従うアーム２の動きを演算してもよい。この場合、位置姿勢演算部１０４は、位置姿勢計測部１０１による計測結果に基づいてアーム２の位置姿勢を算出する。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方を用いてアーム２の位置姿勢を算出してもよい。
【0025】
近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
【0026】
解除操作判定部１０６は、操作者が拘束付き直接教示を解除するための解除操作を行ったかを判定する。この際、例えば、解除操作判定部１０６は、外力検知部１０２による検知結果（外力の大きさ、外力の向き又は外力のパターン等）又は位置姿勢演算部１０４による算出結果（位置姿勢又は位置姿勢のパターン等）に基づいて、操作者が解除操作を行ったかを判定する。
【0027】
復帰判定部１０７は、拘束付き直接教示へと切換え可能な状態（後述する第一の状態）へ復帰させるための所定の復帰条件を満たしたかを判定する。この際、例えば、復帰判定部１０７は、操作者が解除操作を行った後一定時間が経過した場合、又は、位置姿勢演算部１０４により算出されたアーム２の位置姿勢が拘束目標に近づいていない場合に、復帰条件を満たしたと判定する。
【0028】
切換部１０８は、近接判定部１０５による判定結果、解除操作判定部１０６による判定結果及び復帰判定部１０７による判定結果に従い、拘束制御演算部１１０の動作を切換える。すなわち、切換部１０８は、直接教示装置１が第一の状態（近接判定中）である場合において、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていると判定された場合に、直接教示装置１を第二の状態（解除操作判定中）へと切換える。また、切換部１０８は、直接教示装置１が第二の状態である場合において、解除操作判定部１０６により操作者が解除操作を行ったと判定された場合に、直接教示装置１を第三の状態（復帰判定中）へと切換える。また、切換部１０８は、直接教示装置１が第三の状態である場合において、復帰判定部１０７により復帰条件を満たしたと判定された場合に、直接教示装置１を第一の状態へと切換える。その上で、切換部１０８は、直接教示装置１が第二の状態である場合には拘束制御演算部１１０による処理を有効とし、直接教示装置１がそれ以外の状態である場合には拘束制御演算部１１０による処理を無効とする。
【0029】
また、切換部１０８は、拘束目標が複数存在する場合に、近接判定部１０５による判定結果に従い、目標値算出部１０９による動作（対象とする拘束目標）の切換えも実施してもよい。
【0030】
目標値算出部１０９は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束付き直接教示における拘束制御の目標値を算出する。なお、拘束制御の目標値とは、アーム２の目標位置及びアーム２の目標姿勢のうちの少なくとも一方を意味する。
【0031】
拘束制御演算部１１０は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０９により算出された目標値に移動するアーム２の動き（拘束制御指令値）を算出する。
【0032】
合成部１１１は、従動制御演算部１０３による算出結果（従動制御指令値）と拘束制御演算部１１０による算出結果（拘束制御指令値）とを一つの指令値に合成する。
【0033】
駆動制御部１１２は、合成部１１１による合成結果に基づいて、アーム２を駆動する。
【0034】
実施の形態１に係る直接教示装置１では、通常の直接教示と拘束付き直接教示の切換えを、操作者によるアーム２の操作により実行可能としている。この切換え方法は、以下に示す３つの仕組みによって実現される。
【0035】
１つ目は、直接教示装置１が、アーム２の位置姿勢が拘束目標に近づいた際に、自動的に通常の直接教示から拘束付き直接教示に切換える仕組みである。例えば図２Ａ，２Ｂに示すように、直接教示装置１が、アーム２の位置を所定の平面５０６（拘束面５０３）に拘束させる場合を考える。この場合、直接教示装置１は、通常の直接教示を行っている状態（第一の状態）で、平面５０６からアーム２の位置までの距離が例えば±５０［ｍｍ］以下になった場合には拘束付き直接教示への切換えを行い、それ以外の場合には通常の直接教示のままにする。また、例えば図３Ａ，３Ｂに示すように、直接教示装置１が、アーム２の姿勢を真下方向５０７（拘束方向５０５）に拘束させる場合を考える。この場合、直接教示装置１は、通常の直接教示を行っている状態で、アーム２の姿勢が真下方向５０７から例えば±０．２［ｒａｄ］以下になった場合には拘束付き直接教示への切換えを行い、それ以外の場合には通常の直接教示のままにする。この１つ目の仕組みにより、直接教示装置１は、操作者によるアーム２の操作のみで、通常の直接教示から拘束付き直接教示への切換えを実現できる。
【0036】
２つ目は、直接教示装置１が、操作者による解除操作を検知することで、拘束付き直接教示を解除する仕組みである。例えば、直接教示装置１は、拘束付き直接教示を行っている状態（第二の状態）で、操作者がアーム２に５．０［Ｎ］以上の力を加えたことを検知した場合には解除操作があったと判定して拘束付き直接教示を中止し、５．０［Ｎ］以上の力を検知しなかった場合には拘束付き直接教示を継続する。又は、直接教示装置１は、拘束付き直接教示を行っている状態で、操作者がアーム２に１．０［Ｎｍ］以上の回転トルクを時計回りに加えたことを検知した場合には解除操作があったと判定して拘束付き直接教示を中止し、１．０［Ｎｍ］以上の時計回りの回転トルクを検知しなかった場合には拘束付き直接教示を継続する。
【0037】
また、直接教示装置１は、解除操作があったと判定した場合、最初の状態（第一の状態）に戻るのではなく、復帰判定中という第三の状態に移行する。直接教示装置１は、復帰判定中では、通常の直接教示を実行するが、アーム２の位置姿勢が拘束目標に近づいても拘束付き直接教示への切換えは実行しない。これは、直接教示装置１は、拘束付き直接教示を解除した直後はアーム２の位置姿勢は拘束目標と一致しており、その状態で拘束付き直接教示へと切替えてしまうと通常の直接教示への切換えができなくなるためである。そこで、直接教示装置１は、操作者による解除操作を検知した場合には、一時的に拘束付き直接教示への切換えを停止する。これが、第三の状態が必要となる理由である。
そして、操作者は復帰判定中の間に、図２Ｃ，３Ｃに示すように、アーム２を拘束目標から遠ざけることができる。この２つ目の仕組みにより、直接教示装置１は、拘束付き直接教示を解除可能となる。
そして、操作者は復帰判定中の間に、図２Ｃ，３Ｃに示すように、アーム２を拘束目標から遠ざけることができる。この２つ目の仕組みにより、直接教示装置１は、拘束付き直接教示を解除可能となる。
【0038】
３つ目は、直接教示装置１が、復帰判定中に所定の復帰条件を満たした場合に、拘束付き直接教示へと切換え可能な状態（第一の状態）へと復帰させる仕組みである。２つ目の仕組みにより、直接教示装置１は、拘束付き直接教示を解除可能となるが、これだけでは再度拘束付き直接教示を行うことができない。そこで、例えば、直接教示装置１は、アーム２の位置姿勢が拘束目標から十分遠ざかったら、第一の状態へと復帰させる。又は、例えば、直接教示装置１は、アーム２の拘束が解除されてから一定時間以上経過したら、第一の状態へと復帰させる。この３つ目の仕組みにより、直接教示装置１は、拘束付き直接教示を解除するだけでなく、図２Ｄ，３Ｄのように再度拘束付き直接教示へ切換え可能な状態となる。
【0039】
以上の３つの仕組みを組合わせることにより、実施の形態１に係る直接教示装置１は、操作者がアーム２を操作している状態のまま、通常の直接教示と拘束付き直接教示とを切換え可能となる。以下、図１に示す直接教示装置１の動作例について、図４を参照しながら説明する。
【0040】
【0041】
また、外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する（ステップＳＴ４０２）。なお、外力検知部１０２により検知される外力は、センサの構造等によっては、操作者によりアーム２に加えられた外力だけではなく、重力に起因する成分が重畳されている場合がある。そこで、このような場合には、外力検知部１０２は、重力に起因する成分を算出し、検知した外力から当該重力に起因する成分を差し引くことで、外力成分のみを算出するとよい。これは重力補償と呼ばれる公知技術であり、例えば特許文献５等に開示されている。
【文献】特開平０１－０６６７１５号公報
【0042】
次いで、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の動き（従動制御指令値）を算出する（ステップＳＴ４０３）。この際、従動制御演算部１０３は、まず、外力検知部１０２により検知された外力の向き及び大きさから、操作者がどのようにアーム２を動かそうとしているのかを判断する。そして、従動制御演算部１０３は、その判断結果及び位置姿勢計測部１０１により計測されたアーム２の位置姿勢に基づいて、アーム２の移動量（関節角θ等の差分）又は関節角速度θ（ドット）等を算出する。なお、従動制御演算部１０３による従動制御演算方法は、例えば特許文献２等で開示されており、様々な方式が開発されているためその詳細な記述は省略する。
【0043】
次いで、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置姿勢を算出する（ステップＳＴ４０４）。
ここで、直接教示装置１が位置拘束を行う場合（すなわち直接教示装置１がアーム２の位置を所定の拘束面５０３又は拘束軸５０４に拘束する場合）には、位置姿勢演算部１０４は少なくともアーム２の位置を算出する必要がある。
また、直接教示装置１が姿勢拘束を行う場合（すなわち直接教示装置１がアーム２の姿勢を所定の拘束方向５０５に拘束する場合）には、位置姿勢演算部１０４は少なくともアーム２の姿勢を算出する必要がある。
また、直接教示装置１が位置拘束及び姿勢拘束を行う場合（すなわち直接教示装置１がアーム２の位置及び姿勢を所定の拘束面５０３又は拘束軸５０４且つ所定の拘束方向５０５に拘束する場合）には、位置姿勢演算部１０４はアーム２の位置及び姿勢を算出する必要がある。
ここで、直接教示装置１が位置拘束を行う場合（すなわち直接教示装置１がアーム２の位置を所定の拘束面５０３又は拘束軸５０４に拘束する場合）には、位置姿勢演算部１０４は少なくともアーム２の位置を算出する必要がある。
また、直接教示装置１が姿勢拘束を行う場合（すなわち直接教示装置１がアーム２の姿勢を所定の拘束方向５０５に拘束する場合）には、位置姿勢演算部１０４は少なくともアーム２の姿勢を算出する必要がある。
また、直接教示装置１が位置拘束及び姿勢拘束を行う場合（すなわち直接教示装置１がアーム２の位置及び姿勢を所定の拘束面５０３又は拘束軸５０４且つ所定の拘束方向５０５に拘束する場合）には、位置姿勢演算部１０４はアーム２の位置及び姿勢を算出する必要がある。
【0044】
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果（最新の従動制御指令値）に基づいて上記算出を行うことが最も望ましい。これは、直接教示装置１が拘束付き直接教示を行う場合に、操作者がアーム２に対して与えた移動量のうち、拘束面５０３等から外れる成分を同時に相殺でき、拘束制御が最も有効に働くからである。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果を用いず、位置姿勢計測部１０１による計測結果に基づいて上記算出を行うことも可能である。一方、この場合には、拘束面５０３等から外れたことが計測されてから拘束制御が始まるので、拘束制御の効きが悪くなる。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方に基づいて上記算出を行ってもよい。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果を用いず、位置姿勢計測部１０１による計測結果に基づいて上記算出を行うことも可能である。一方、この場合には、拘束面５０３等から外れたことが計測されてから拘束制御が始まるので、拘束制御の効きが悪くなる。
また、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果及び位置姿勢計測部１０１による計測結果の両方に基づいて上記算出を行ってもよい。
【0045】
なお、位置姿勢計測部１０１及び従動制御演算部１０３で関節角θのみが得られ、アーム２の位置姿勢を直接得られない場合、位置姿勢演算部１０４は、順運動学演算によってアーム２の位置姿勢を算出することが可能である。
【0046】
次いで、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が拘束目標に近づいているかを判定する（ステップＳＴ４０５）。
【0047】
例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］の位置に固定することであり、また、接近判定のしきい値が０．０５［ｍ］であるとする。この場合、近接判定部１０５は、位置姿勢演算部１０４により算出されたエンドエフェクタ２０１の先端のＺ軸座標が０．０５～０．１５［ｍ］の範囲内であればアーム２の位置は拘束目標に近づいていると判定し、それ以外の場合にはアーム２の位置は拘束目標に近づいていないと判定する。
【0048】
また、拘束制御の目的がエンドエフェクタ２０１の向き５０２（Ｚ軸）を真下に向けることであり、また、接近判定のしきい値が０．１［ｒａｄ］であるとする。この場合、近接判定部１０５は、図５に示すように、真上方向（基準方向）５０８とエンドエフェクタ２０１の向き５０２との成す角度Θが（π－０．１）［ｒａｄ］以上（すなわち真下方向５０７とエンドエフェクタ２０１の向き５０２との成す角度が０．１［ｒａｄ］未満）であれば、アーム２の姿勢は拘束目標に近づいていると判定し、それ以外の場合にはアーム２の姿勢は拘束目標に近づいていないと判定する。
【0049】
また、拘束面５０３が複数存在する場合等のように拘束目標が複数存在する場合、近接判定部１０５は、拘束目標毎に上記判定を行い、各拘束目標のうちの１つでもアーム２の位置姿勢が近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。この際、更に、近接判定部１０５は、アーム２と各拘束目標との間の距離を評価し、上記判定の結果とともにアーム２に最も近い拘束目標を示す情報を切換部１０８に通知してもよい。
【0050】
また、解除操作判定部１０６は、操作者が解除操作を行ったかを判定する（ステップＳＴ４０６）。
【0051】
解除操作判定部１０６による最も好ましい判定方法は、外力を用いた判定方法である。解除操作判定部１０６による外力を用いた最も単純な判定方法の一例は、外力が所定の力（例えば５．０［Ｎ］）以上である場合に操作者が解除操作を行ったと判定するという、外力の大きさに基づいた判定方法である。また、解除操作判定部１０６は、外力の大きさに加えて時間も考慮し、外力が所定の力以上であり且つ一定時間以上継続している場合に、操作者が解除操作を行ったと判定してもよい。
【0052】
解除操作判定部１０６による外力を用いたより複雑な判定方法の一例について、図６に示す。
図６Ａに示すように、解除操作判定部１０６は、外力が拘束面５０３から遠ざかる方向且つ所定の力以上である場合に、操作者が解除操作を行ったと判定してもよい。この判定方法は、外力検知部１０２において、外力のうち、拘束面５０３と直交する成分の大きさを検知することで実現可能である。このように、解除操作判定部１０６による判定方法としては、外力の大きさだけではなく外力の向きも考慮した判定方法も実施可能である。
図６Ａに示すように、解除操作判定部１０６は、外力が拘束面５０３から遠ざかる方向且つ所定の力以上である場合に、操作者が解除操作を行ったと判定してもよい。この判定方法は、外力検知部１０２において、外力のうち、拘束面５０３と直交する成分の大きさを検知することで実現可能である。このように、解除操作判定部１０６による判定方法としては、外力の大きさだけではなく外力の向きも考慮した判定方法も実施可能である。
【0053】
また、図６Ｂに示すように、解除操作判定部１０６は、外力がエンドエフェクタ２０１の軸を中心とした回転力であり且つ所定の力以上である場合に、操作者が解除操作を行ったと判定してもよい。この判定方法は、外力検知部１０２において、エンドエフェクタ２０１の軸を中心とした回転力を検知することで実現可能である。
【0054】
また、図６Ｃに示すように、解除操作判定部１０６は、外力が左右方向に交互に加えられた力であり且つ所定の力以上である場合に、操作者が解除操作を行ったと判定してもよい。この判定方法では、解除操作判定部１０６は、外力の大きさ及び向きだけではなく時系列なパターンも考慮しているため、意図しない操作により偶然に解除操作と判定する可能性（すなわち誤判定の可能性）が低くなることが期待できる。解除操作判定部１０６が解除操作を意図するパターンであるかを判定する方法としては、公知のパターン認識、機械学習又はニューラルネットワーク等、様々な方法を利用可能である。
【0055】
また、解除操作判定部１０６は、外力による判定に限らず、アーム２の位置姿勢を用いて操作者が解除操作を行ったかを判定してもよい。但し、直接教示装置１は、拘束付き直接教示を行っている間は、アーム２の位置姿勢を拘束しているため、アーム２をその拘束範囲でしか動かすことができない。そのため、解除操作判定部１０６によるアーム２の位置姿勢を用いた判定方法はある程度制限される。解除操作判定部１０６によるアーム２の位置姿勢を用いた判定方法の一例について、図７に示す。
【0056】
図７Ａ～７Ｃに示すように、直接教示装置１が、アーム２をＺ座標が一定である平面５０６（拘束面５０３）に拘束している場合を考える。
この場合、図７Ａに示すように、解除操作判定部１０６は、アーム２のＸ座標又はＹ座標が平面５０６より外（例えば±０．５［ｍ］の範囲外）である場合に、操作者が解除操作を行ったと判定してもよい。また、解除操作判定部１０６は、ワークからエンドエフェクタ２０１の先端までの距離が例えば０．２［ｍ］より大きい場合に、操作者が解除操作を行ったと判定してもよい。
この場合、図７Ａに示すように、解除操作判定部１０６は、アーム２のＸ座標又はＹ座標が平面５０６より外（例えば±０．５［ｍ］の範囲外）である場合に、操作者が解除操作を行ったと判定してもよい。また、解除操作判定部１０６は、ワークからエンドエフェクタ２０１の先端までの距離が例えば０．２［ｍ］より大きい場合に、操作者が解除操作を行ったと判定してもよい。
【0057】
また、図７Ｂに示すように、解除操作判定部１０６は、アーム２が拘束面５０３上で所定の形（図７Ｂでは三角形の形）に動いた場合に操作者が解除操作を行ったと判定する等、エンドエフェクタ２０１の位置の時系列なパターンに基づいて判定を行ってもよい。解除操作判定部１０６が解除操作を意図するパターンであるかを判定する方法としては、公知のパターン認識、機械学習又はニューラルネットワーク等、様々な方法を利用可能である。
【0058】
また、図７Ｃに示すように、解除操作判定部１０６は、アーム２が拘束面５０３上で一回転した場合に操作者が解除操作を行ったと判定する等、エンドエフェクタ２０１の姿勢の時系列なパターンに基づいて判定を行ってもよい。但し、この判定方法は、直接教示装置１が拘束付き直接教示を行っている状態では実施できない場合がある。解除操作判定部１０６が解除操作を意図するパターンであるかを判定する方法としては、公知のパターン認識、機械学習又はニューラルネットワーク等、様々な方法を利用可能である。
【0059】
また、図７Ｄ，７Ｅに示すように、直接教示装置１が、アーム２をＺ軸に平行な軸（拘束軸５０４）に拘束している場合を考える。
この場合、図７Ｄに示すように、解除操作判定部１０６は、エンドエフェクタ２０１の先端のＺ座標が所定の位置（例えば０．３［ｍ］）より大きい場合に、操作者が解除操作を行ったと判定してもよい。また、解除操作判定部１０６は、ワークからエンドエフェクタ２０１の先端までの距離が大きい場合（すなわちアーム２がワークから遠ざかった場合）に、操作者が解除操作を行ったと判定してもよい。
この場合、図７Ｄに示すように、解除操作判定部１０６は、エンドエフェクタ２０１の先端のＺ座標が所定の位置（例えば０．３［ｍ］）より大きい場合に、操作者が解除操作を行ったと判定してもよい。また、解除操作判定部１０６は、ワークからエンドエフェクタ２０１の先端までの距離が大きい場合（すなわちアーム２がワークから遠ざかった場合）に、操作者が解除操作を行ったと判定してもよい。
【0060】
また、図７Ｅに示すように、解除操作判定部１０６は、アーム２が拘束軸５０４上で所定の周期よりも短い周期で上下に往復した場合に、操作者が解除操作を行ったと判定してもよい。
【0061】
なお、解除操作判定部１０６は、位置姿勢演算部１０４により算出されたアーム２の位置姿勢の代わりに、位置姿勢計測部１０１により計測されたアーム２の位置姿勢に関するパラメータを用いることも可能である。
【0062】
また、直接教示装置１の実施環境が操作者の音声を取得可能な環境である場合、直接教示装置１はマイク等を用いて操作者の音声を取得し、解除操作判定部１０６は音声認識によって操作者が解除操作を行ったかを判定してもよい。この判定方法では、操作者はアーム２から手を離すことなく解除操作を実施可能となる。
【0063】
また、直接教示装置１の実施環境が操作者の画像を動画として取得可能な環境である場合、直接教示装置１はカメラ等を用いて操作者の唇の画像を取得し、解除操作判定部１０６は当該画像から操作者が発している言葉を認識することで操作者が解除操作を行ったかを判定してもよい。この方法でも、操作者はアーム２から手を離すことなく解除操作を実施可能となる。
【0064】
また、復帰判定部１０７は、拘束付き直接教示へと切換え可能な状態へ復帰させるための所定の復帰条件を満たしたかを判定する（ステップＳＴ４０７）。前述した通り、直接教示装置１は、操作者により解除操作が行われた直後に第一の状態に移行してしまうと、解除操作が行われても直ぐに拘束付き直接教示へ戻ってしまうため、そうならないようにする必要がある。復帰判定部１０７はこのような不具合を解消するために存在する。
【0065】
復帰判定部１０７による最も好ましい判定方法の一つは、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいていると判定されなくなるのを待って復帰条件を満たしたと判定する方法である。例えば、復帰判定部１０７は、図２であればアーム２が拘束面５０３から±５０［ｍｍ］以上離れた場合に、図３であれば真下方向５０７とアーム２の姿勢とが成す角度が±０．２［ｒａｄ］以上離れた場合に、復帰条件を満たしたと判定してもよい。これにより、直接教示装置１は、操作者により解除操作が行われた後に操作者の意図に反して拘束付き直接教示が再度有効になることを抑制できる。
【0066】
なお、復帰判定部１０７は、近接判定のしきい値をそのまま用いるのではなく、近接判定のしきい値よりも多少余裕を持たせたしきい値を用いて判定を行ってもよく、より安定した動作になると考えられる。例えば、復帰判定部１０７は、図２であればアーム２が拘束面５０３から±５５［ｍｍ］以上離れた場合に、図３であれば真下方向５０７とアーム２の姿勢とが成す角度が±０．２２［ｒａｄ］以上離れた場合に、復帰条件を満たしたと判定してもよい。
【0067】
また、復帰判定部１０７は、操作者が解除操作を行った後一定時間が経過した場合に、復帰条件を満たしたと判定してもよい。例えば、復帰判定部１０７は、操作者が解除操作を行った後１０［秒］経過した場合に、復帰条件を満たしたと判定してもよい。この場合、操作者は、通常の直接教示に戻したい場合には、１０［秒］以内に近接判定条件を満たさない領域へとアーム２を操作すればよい。また、操作者は、拘束付き直接教示を再開したい場合には、アーム２をそのままにして時間が経過するのを待てばよい。
【0068】
また、復帰判定部１０７は、上記の二つの方法を組合わせて、アーム２の位置姿勢が拘束目標から離れた後一定時間が経過した場合に、復帰条件を満たしたと判定してもよい。
【0069】
なお、操作者は、復帰判定部１０７が拘束目標からの距離又は解除操作からの時間の何れを用いる場合でも、アーム２から手を離すことなく復帰操作が可能である。
【0070】
次いで、切換部１０８は、近接判定部１０５による判定結果、解除操作判定部１０６による判定結果及び復帰判定部１０７による判定結果に従い、拘束制御演算部１１０の動作を切換える（ステップＳＴ４０８）。
【0071】
切換部１０８は、図８に示すように、直接教示装置１を３つの状態（第一の状態～第三の状態）に分けて管理する。そして、切換部１０８は、近接判定部１０５、解除操作判定部１０６及び復帰判定部１０７によるそれぞれの判定結果に基づいて、直接教示装置１の状態を切換える。第一の状態は、近接判定部１０５による判定のみが有効となる近接判定中という状態であり、直接教示装置１はこの状態から動作を開始する。第二の状態は、解除操作判定部１０６による判定のみが有効となる解除操作判定中という状態である。第三の状態は、復帰判定部１０７による判定のみが有効となる復帰判定中という状態である。直接教示装置１はこの３つの状態の何れかの状態となる。
【0072】
切換部１０８は、直接教示装置１が第一の状態である場合には、近接判定部１０５による判定結果のみを参照する。そして、切換部１０８は、近接判定部１０５によりアーム２の位置姿勢が拘束目標に近づいたと判定された場合に、直接教示装置１を第二の状態に切換える。
【0073】
切換部１０８は、直接教示装置１が第二の状態である場合には、解除操作判定部１０６による判定結果のみを参照する。そして、切換部１０８は、解除操作判定部１０６により操作者が解除操作を行ったと判定された場合に、直接教示装置１を第三の状態に切換える。
【0074】
切換部１０８は、直接教示装置１が第三の状態である場合には、復帰判定部１０７による判定結果のみを参照する。そして、切換部１０８は、復帰判定部１０７により復帰条件を満たしたと判定された場合に、直接教示装置１を第一の状態に切換える。
【0075】
そして、切換部１０８は、直接教示装置１の状態に応じて、拘束制御演算部１１０による処理を有効又は無効に切換える。すなわち、切換部１０８は、直接教示装置１が第二の状態である場合には、拘束制御演算部１１０による処理を有効とする。また、切換部１０８は、直接教示装置１が第一の状態又は第三の状態である場合には、拘束制御演算部１１０による演算を停止するか又は算出結果がアーム２の制御に反映されないようにして、拘束制御演算部１１０による処理を無効とする。
また図８に示すように、切換部１０８は、直接教示装置１が第二の状態である場合には目標値算出部１０９による目標値の算出を実行させ、直接教示装置１が第一の状態又は第三の状態である場合には目標値算出部１０９による目標値の算出を停止させるよう切換えてもよい。
また図８に示すように、切換部１０８は、直接教示装置１が第二の状態である場合には目標値算出部１０９による目標値の算出を実行させ、直接教示装置１が第一の状態又は第三の状態である場合には目標値算出部１０９による目標値の算出を停止させるよう切換えてもよい。
【0076】
このように、直接教示装置１は、操作者によりアーム２の位置姿勢が拘束目標に近づけられると、自動的に拘束制御を開始する。また、直接教示装置１は、操作者により拘束付き直接教示の解除操作が行われると、自動的に拘束制御を終了する。また、直接教示装置１は、復帰条件を満たした後に再度操作者によりアーム２の位置姿勢が拘束目標に近づけられると、自動的に拘束制御を開始する。これにより、直接教示装置１は、操作者によりアーム２が保持された状態で拘束制御のオンオフを切換えることができる。
なお、切換部１０８は、拘束目標が複数あって近接判定部１０５からアーム２に最も近い拘束目標を示す情報が通知された場合には、後述する目標値算出部１０９にその情報を出力して当該目標値算出部１０９の動作を切換えてもよい。
なお、切換部１０８は、拘束目標が複数あって近接判定部１０５からアーム２に最も近い拘束目標を示す情報が通知された場合には、後述する目標値算出部１０９にその情報を出力して当該目標値算出部１０９の動作を切換えてもよい。
【0077】
なお、近接判定部１０５、解除操作判定部１０６及び復帰判定部１０７は、切換部１０８がその判定結果を必要としないときは処理を停止してもよい。例えば、直接教示装置１が第二の状態である場合には、切換部１０８は近接判定部１０５による判定結果は用いないため、近接判定部１０５は処理を停止しても支障はない。また、直接教示装置１が第一の状態又は第三の状態である場合には、切換部１０８は解除操作判定部１０６による判定結果は用いないため、解除操作判定部１０６は処理を停止しても支障はない。また、直接教示装置１が第一の状態又は第二の状態である場合には、切換部１０８は復帰判定部１０７による判定結果は用いないため、復帰判定部１０７は処理を停止しても支障はない。なお、近接判定部１０５は、復帰判定部１０７が復帰判定のために近接判定部１０５による判定結果を用いる場合には、直接教示装置１が第三の状態である場合にも処理を実施する必要がある。
【0078】
次いで、目標値算出部１０９は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束制御の目標値を算出する（ステップＳＴ４０９）。
【0079】
例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］の位置に固定することであり、位置姿勢演算部１０４により算出されたエンドエフェクタ２０１の先端の現在値が［ＸＰ，ＹＰ，ＺＰ］であるとする。この場合、目標値算出部１０９は、拘束制御の目標値を［ＸＰ，ＹＰ，０．１］とする。
【0080】
また、拘束制御の目的が、図９下段に示すようにエンドエフェクタ２０１の向き５０２のうちのＺ軸を真下方向５０７とした上でＸ軸をロボット座標系のＸ軸に合わせることであるとする。この場合、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式（１）のようになる。なお、回転行列による姿勢の表現は、例えば非特許文献１等に開示されており、その詳細については省略する。
【文献】Ｊｏｈｎ Ｊ． Ｃｒａｉｇ （三浦・下山訳）：ロボティクス，共立出版（１９９１）
【0081】
なお、目標値算出部１０９は、拘束制御の目標値として、１つの目標値のみを算出してもよいし、複数の目標値を算出してその中から１つの目標値を選択してもよい。例えば、拘束制御の目的がエンドエフェクタ２０１の先端のＺ軸座標を０．１［ｍ］刻みで最も近い位置に固定することである場合、目標値算出部１０９は、拘束制御の目標値として［ＸＰ，ＹＰ，０．１ｎ］（ｎは整数）の中で最も現在の位置に近いものを選択する。
【0082】
また、拘束制御の目的が図１０に示すようにエンドエフェクタ２０１の向き５０２のうちのＺ軸を真下方向５０７とし且つこのＺ軸を回転軸としてπ／２刻み（９０度刻み）に回転させた姿勢で固定することである場合、目標値算出部１０９は、拘束制御の目標値として最も現在の姿勢に近いものを選択する。このとき、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式（２）のようになる。
この式（２）は図１０に示すように、ｎによって目標姿勢が４通りに変わるが、目標値算出部１０９はその中から現在の姿勢に最も近いものを１つ選択する。目標値算出部１０９は、現在の姿勢に近いかどうかは、現在の姿勢から各目標姿勢へと回転させた場合の回転角の大小等から判断できる。また、近接判定部１０５がアーム２に最も近い拘束目標を示す情報を通知している場合には、目標値算出部１０９はその情報から拘束制御の目標値を選択してもよい。
【0083】
次いで、拘束制御演算部１１０は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０９により算出された目標値に移動するアーム２の動き（拘束制御指令値）を算出する（ステップＳＴ４１０）。この際、拘束制御演算部１１０は、目標値算出部１０９により算出された目標値から、アーム２を現在の位置姿勢から目標値に移動させるのに必要なアーム２の移動量又は単位時間当たり移動量である関節角速度θ（ドット）等を算出する。また、拘束制御演算部１１０は、関節角速度θ（ドット）の代わりに１制御周期あたりの関節角変化量Δθを演算してもよい。
【0084】
次いで、合成部１１１は、従動制御演算部１０３から出力された従動制御指令値と拘束制御演算部１１０から出力された拘束制御指令値とを一つの指令値に合成する（ステップＳＴ４１１）。
【0085】
例えば、従動制御演算部１０３が従動制御指令値として関節角θＤを出力し、拘束制御演算部１１０が拘束制御指令値として関節角変化量ΔθＣを出力した場合には、合成部１１１は、θＤ＋ΔθＣを関節角指令値として出力する。
また、例えば、従動制御演算部１０３が従動制御指令値として位置座標［ｘＤ，ｙＤ，ｚＤ］及び姿勢ＴＤ（回転行列表現）を出力し、拘束制御演算部１１０が拘束制御指令値として位置変化量［ΔｘＣ，ΔｙＣ，ΔｚＣ］及び姿勢変化量ΔＴＣ（回転行列表現）を出力した場合には、合成部１１１は、［ｘＤ＋ΔｘＣ，ｙＤ＋ΔｙＣ，ｚＤ＋ΔｚＣ］を位置指令値として、ΔＴＣＴＤ（回転行列表現）を姿勢指令値として出力する（回転行列表現された姿勢の合成は行列の積となる）。
また、例えば、従動制御演算部１０３が従動制御指令値としてトルクτＤを出力し、拘束制御演算部１１０が拘束制御指令値としてトルク変化量ΔτＣを出力した場合には、合成部１１１は、τＤ＋ΔτＣをトルク指令値として出力する。
また、例えば、従動制御演算部１０３が従動制御指令値として位置座標［ｘＤ，ｙＤ，ｚＤ］及び姿勢ＴＤ（回転行列表現）を出力し、拘束制御演算部１１０が拘束制御指令値として位置変化量［ΔｘＣ，ΔｙＣ，ΔｚＣ］及び姿勢変化量ΔＴＣ（回転行列表現）を出力した場合には、合成部１１１は、［ｘＤ＋ΔｘＣ，ｙＤ＋ΔｙＣ，ｚＤ＋ΔｚＣ］を位置指令値として、ΔＴＣＴＤ（回転行列表現）を姿勢指令値として出力する（回転行列表現された姿勢の合成は行列の積となる）。
また、例えば、従動制御演算部１０３が従動制御指令値としてトルクτＤを出力し、拘束制御演算部１１０が拘束制御指令値としてトルク変化量ΔτＣを出力した場合には、合成部１１１は、τＤ＋ΔτＣをトルク指令値として出力する。
【0086】
次いで、駆動制御部１１２は、合成部１１１による合成結果に基づいて、アーム２を駆動する（ステップＳＴ４１２）。この駆動制御部１１２によるアーム２の駆動は、通常のアーム２の制御であるため、その詳細については省略する。
【0087】
以上のように、この実施の形態１によれば、直接教示装置１は、ロボットが有するアーム２に加えられた外力を検知する外力検知部１０２と、外力検知部１０２により検知された外力に従うアーム２の動きを算出する従動制御演算部１０３と、アーム２の位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部１０４と、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置姿勢が、当該アーム２の位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部１０５と、操作者が拘束付き直接教示を解除するための解除操作を行ったかを判定する解除操作判定部１０６と、拘束付き直接教示へと切換え可能な状態への復帰条件を満たしたかを判定する復帰判定部１０７と、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、拘束付き直接教示における拘束制御の目標値を算出する目標値算出部１０９と、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０９により算出された目標値に移動するアーム２の動きを算出する拘束制御演算部１１０と、自機が第一の状態である場合において近接判定部１０５により拘束目標に近づいていると判定された場合に自機を第二の状態へと切換え、自機が第二の状態である場合において解除操作判定部１０６により操作者が解除操作を行ったと判定された場合に自機を第三の状態へと切換え、自機が第三の状態である場合において復帰判定部１０７により復帰条件を満たしたと判定された場合に自機を第一の状態へと切換え、自機が第二の状態にある場合に拘束制御演算部１１０による処理を有効とし、自機が第一の状態又は第三の状態にある場合に拘束制御演算部１１０による処理を無効とする切換部１０８と、従動制御演算部１０３による算出結果と拘束制御演算部１１０による算出結果とを合成する合成部１１１と、合成部１１１による合成結果に基づいてアーム２を駆動する駆動制御部１１２とを備えた。これにより、実施の形態１に係る直接教示装置１は、操作者がアーム２を操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。
【0088】
実施の形態２．
実施の形態２では、アーム２が垂直多関節型ロボットアームであり、直接教示装置１による拘束方法が姿勢拘束のみに限定された場合を示す。なお、実施の形態２に係る直接教示装置１の構成例は、実施の形態１に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
実施の形態２では、アーム２が垂直多関節型ロボットアームであり、直接教示装置１による拘束方法が姿勢拘束のみに限定された場合を示す。なお、実施の形態２に係る直接教示装置１の構成例は、実施の形態１に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
【0089】
なお、実施の形態２における位置姿勢演算部１０４は、アーム２の姿勢を算出する。
また、実施の形態２における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の姿勢が拘束方向５０５に近接しているかを判定する。
また、実施の形態２における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の姿勢が拘束方向５０５に近接しているかを判定する。
【0090】
次に、実施の形態２に係る直接教示装置１の動作例について、図１１を参照しながら説明する。
以下では、直接教示装置１は、エンドエフェクタ２０１の向き５０２（Ｚ軸）を真下方向５０７又は真横方向５０９に拘束するような姿勢拘束を行うものとする。また、ロボット座標系の各軸の単位ベクトルをｅＸ，ｅＹ，ｅＺで表す。また、アーム２は６軸であるとする。また、切換部１０８は、目標値算出部１０９及び拘束制御演算部１１０の動作を切換えるものとする。また、従動制御演算部１０３は従動制御指令値としてアーム２の各関節角θを算出し、拘束制御演算部１１０は拘束制御指令値としてアーム２の各関節角速度θ（ドット）を算出するものとする。
以下では、直接教示装置１は、エンドエフェクタ２０１の向き５０２（Ｚ軸）を真下方向５０７又は真横方向５０９に拘束するような姿勢拘束を行うものとする。また、ロボット座標系の各軸の単位ベクトルをｅＸ，ｅＹ，ｅＺで表す。また、アーム２は６軸であるとする。また、切換部１０８は、目標値算出部１０９及び拘束制御演算部１１０の動作を切換えるものとする。また、従動制御演算部１０３は従動制御指令値としてアーム２の各関節角θを算出し、拘束制御演算部１１０は拘束制御指令値としてアーム２の各関節角速度θ（ドット）を算出するものとする。
【0091】
実施の形態２に係る直接教示装置１の動作例では、図１１に示すように、まず、位置姿勢計測部１０１は、アーム２の各関節角θを計測する（ステップＳＴ１１０１）。例えば、位置姿勢計測部１０１は、アーム２が有するモータに取付けられたエンコーダ等を用いて、アーム２の関節毎に関節角θを計測する。
【0092】
次いで、外力検知部１０２は、操作者によりアーム２に加えられた外力を検知する（ステップＳＴ１１０２）。この外力検知部１０２による処理は実施の形態１と同じである。なお、外力検知部１０２は重力補償を行うものとする。
【0093】
次いで、従動制御演算部１０３は、位置姿勢計測部１０１による計測結果に基づいて、外力検知部１０２により検知された外力に従うアーム２の各関節角θ（従動制御指令値）を算出する（ステップＳＴ１１０３）。この従動制御演算部１０３の動作例について、図１２を参照しながら説明する。
【0094】
従動制御演算部１０３は、例えば図１２Ａに示される方法により、外力検知部１０２により検知されたトルクτからアーム２の各関節角θを算出できる。
図１２Ａでは、従動制御演算部１０３は、まず、外力検知部１０２により検知されたトルクτにゲインＫＴを掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、ゲインＫＴは、アーム２の関節毎に同一でもよいし異なっていてもよい。なお図１２Ａに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期ＴＳを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。
図１２Ａでは、従動制御演算部１０３は、まず、外力検知部１０２により検知されたトルクτにゲインＫＴを掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、ゲインＫＴは、アーム２の関節毎に同一でもよいし異なっていてもよい。なお図１２Ａに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期ＴＳを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。
【0095】
また、従動制御演算部１０３は、例えば図１２Ｂに示される方法により、外力検知部１０２により検知された力Ｆからアーム２の各関節角θを演算できる。
図１２Ｂの例では、従動制御演算部１０３は、まず、外力検知部１０２により検知された力ＦにゲインＫＦを掛けることで、アーム２の位置及び姿勢毎に移動速度を算出する。なお、ゲインＫＦは、アーム２の位置及び姿勢の各成分毎に同一でもよいし異なっていてもよい。
次に、従動制御演算部１０３は、移動速度にヤコビ行列Ｊの逆行列を掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、従動制御演算部１０３は、ヤコビ行列Ｊの逆行列が存在しない場合は疑似逆行列を掛ける。なお図１２Ｂに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期ＴＳを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。
図１２Ｂの例では、従動制御演算部１０３は、まず、外力検知部１０２により検知された力ＦにゲインＫＦを掛けることで、アーム２の位置及び姿勢毎に移動速度を算出する。なお、ゲインＫＦは、アーム２の位置及び姿勢の各成分毎に同一でもよいし異なっていてもよい。
次に、従動制御演算部１０３は、移動速度にヤコビ行列Ｊの逆行列を掛けることで、アーム２の関節毎に目標とする関節角速度θ（ドット）を算出する。なお、従動制御演算部１０３は、ヤコビ行列Ｊの逆行列が存在しない場合は疑似逆行列を掛ける。なお図１２Ｂに示すように、従動制御演算部１０３は、目標とする関節角速度θ（ドット）にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部１０３は、目標とする関節角速度θ（ドット）に制御周期ＴＳを掛けることで、アーム２の関節毎に、１制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部１０３は、アーム２の関節毎に、現時刻ｋの関節角θ（ｋ）に関節角変化量Δθを加算して次の時刻ｋ＋１の関節角θ（ｋ＋１）を算出する。
【0096】
次いで、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の姿勢を算出する（ステップＳＴ１１０４）。位置姿勢演算部１０４は、順運動学により上記算出を実行できる。以下、位置姿勢演算部１０４により算出されるアーム２の姿勢を回転行列によりＴで表す。また、Ｔを列ベクトルや要素に分解したものを次式（３）のように表す。
【0097】
次いで、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の姿勢が拘束方向５０５に近づいているかを判定する（ステップＳＴ１１０５）。ここでは、拘束方向５０５は真下方向５０７又は真横方向５０９である。また、接近判定のしきい値は０．１［ｒａｄ］であるとする。近接判定部１０５の動作例について、図１３を参照しながら説明する。
【0098】
近接判定部１０５の動作例では、図１３に示すように、近接判定部１０５は、まず、真上方向（基準方向）５０８とエンドエフェクタ２０１の向き５０２（Ｚ軸）とが成す角度Θを算出する（ステップＳＴ１３０１）。すなわち、近接判定部１０５は、次式（４）のように、ｅＺとｔＺとの内積から角度Θを算出する。
【0099】
そして、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内であるかを判定する（ステップＳＴ１３０２）。
このステップＳＴ１３０２において、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内であると判定した場合（例えば図１４Ａの場合）には、アーム２の姿勢が真横方向５０９に近づいていると判定する（ステップＳＴ１３０３）。
このステップＳＴ１３０２において、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内であると判定した場合（例えば図１４Ａの場合）には、アーム２の姿勢が真横方向５０９に近づいていると判定する（ステップＳＴ１３０３）。
【0100】
一方、ステップＳＴ１３０２において、近接判定部１０５は、角度Θが（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内ではないと判定した場合には、角度Θが（π－０．１）［ｒａｄ］以上であるかを判定する（ステップＳＴ１３０４）。
このステップＳＴ１３０４において、近接判定部１０５は、角度Θが（π－０．１）［ｒａｄ］以上であると判定した場合（例えば図１４Ｂの場合）には、アーム２の姿勢が真下方向５０７に近づいていると判定する（ステップＳＴ１３０５）。
このステップＳＴ１３０４において、近接判定部１０５は、角度Θが（π－０．１）［ｒａｄ］以上であると判定した場合（例えば図１４Ｂの場合）には、アーム２の姿勢が真下方向５０７に近づいていると判定する（ステップＳＴ１３０５）。
【0101】
一方、ステップＳＴ１３０４において、近接判定部１０５は、角度Θが（π－０．１）［ｒａｄ］以上ではないと判定した場合（例えば図１４Ｃの場合）には、アーム２の姿勢がどの拘束方向５０５にも近づいていないと判定する（ステップＳＴ１３０６）。
【0102】
また、解除操作判定部１０６は、操作者が解除操作を行ったかを判定する（ステップＳＴ１１０６）。ここで、解除操作判定部１０６は、外力を用いた判定を行う場合には、実施の形態１と同様の方法が適用可能である。一方、解除操作判定部１０６は、アーム２の位置姿勢を用いた判定を行う場合には、実施の形態２では直接教示装置１がアーム２の姿勢拘束を行っているため、適用可能な判定方法は姿勢拘束と両立可能な判定方法に限定される。例えば、実施の形態２における解除操作判定部１０６は、図７Ｃに示すように、アーム２の姿勢のパターンを用いた判定方法は適用できない。
【0103】
また、復帰判定部１０７は、拘束付き直接教示へと切換え可能な状態への復帰条件を満たしたかを判定する（ステップＳＴ１１０７）。実施の形態２では直接教示装置１がアーム２の姿勢拘束を行っているため、復帰判定部１０７は、アーム２の位置姿勢に基づいて判定を行う場合には、角度Θの値に基づいて判定可能である。例えば、復帰判定部１０７は、角度Θが、（π／２－０．１）［ｒａｄ］から（π／２＋０．１）［ｒａｄ］の範囲内ではなく、且つ、（π－０．１）［ｒａｄ］未満である場合に、アーム２の勢が拘束姿勢から離れており、復帰条件を満たしたと判定できる。
【0104】
また上記では、復帰判定部１０７は、復帰判定のしきい値として、（π／２－０．１）［ｒａｄ］、（π／２＋０．１）［ｒａｄ］及び（π―０．１）［ｒａｄ］を用いた。しかしながら、これに限らず、復帰判定部１０７は、復帰判定のしきい値として、（π／２－０．１２）［ｒａｄ］、（π／２＋０．１２）［ｒａｄ］及び（π－０．１２）［ｒａｄ］のように上記のしきい値よりも多少広げた値を用いてもよい。
【0105】
一方で、復帰判定部１０７は、復帰判定のしきい値として、近接判定部１０５が用いるしきい値よりも狭い値を用いてはいけない。これは、復帰判定部１０７が上記の狭い値を用いると、復帰判定部１０７が復帰条件を満たしたと判定した直後に近接判定部１０５がアーム２の姿勢が拘束方向５０５に近づいていると判定し、直接教示装置１が拘束付き直接教示から離脱できないためである。
【0106】
なお、復帰判定部１０７は、判定にアーム２の位置姿勢を用いず時間のみを用いる場合には、実施の形態１と同様の方法が適用可能である。
【0107】
次いで、切換部１０８は、目標値算出部１０９の動作を切換え、また、拘束制御演算部１１０の動作を切換える（ステップＳＴ１１０８）。すなわち、切換部１０８は、近接判定部１０５による判定結果に従い、目標値算出部１０９の動作を切換える。また、切換部１０８は、近接判定部１０５による判定結果、解除操作判定部１０６による判定結果及び復帰判定部１０７による判定結果に従い、拘束制御演算部１１０の動作を切換える。
【0108】
この際、切換部１０８は、近接判定部１０５によりアーム２の姿勢が真横方向５０９に近づいていると判定された場合には、目標値算出部１０９に目標値として真横方向５０９への目標姿勢を算出させる。
また、切換部１０８は、近接判定部１０５によりアーム２の姿勢が真下方向５０７に近づいていると判定された場合には、目標値算出部１０９に目標値として真下方向５０７への目標姿勢を算出させる。
また、切換部１０８は、近接判定部１０５によりアーム２の姿勢がどの拘束方向５０５にも近づいていないと判定された場合には、目標値算出部１０９による算出を不要とする。
なお、切換部１０８による拘束制御演算部１１０の動作の切換えは、実施の形態１と同様である。
また、切換部１０８は、近接判定部１０５によりアーム２の姿勢が真下方向５０７に近づいていると判定された場合には、目標値算出部１０９に目標値として真下方向５０７への目標姿勢を算出させる。
また、切換部１０８は、近接判定部１０５によりアーム２の姿勢がどの拘束方向５０５にも近づいていないと判定された場合には、目標値算出部１０９による算出を不要とする。
なお、切換部１０８による拘束制御演算部１１０の動作の切換えは、実施の形態１と同様である。
【0109】
次いで、目標値算出部１０９は、拘束目標及び位置姿勢演算部１０４による算出結果に基づいて、目標姿勢を算出する（ステップＳＴ１１０９）。例えば、目標値算出部１０９は、真横方向５０９又は真下方向５０７であり、且つ、アーム２の現在の姿勢から最も小さい回転角度ΘＲで到達可能な目標姿勢を算出する。目標値算出部１０９による目標姿勢の算出方法について以下に示す。
【0110】
まず、目標値算出部１０９は、対象となる目標姿勢毎に、ｅＺに対する目標角度Θ＊を算出する。ここで、目標姿勢が真横方向５０９である場合には目標角度Θ＊はπ／２であり、目標姿勢が真下方向５０７である場合には目標角度Θ＊はπである。
【0111】
次に、目標値算出部１０９は、アーム２の姿勢に基づいて、回転角度ΘＲが最も小さい回転軸５１０を算出する。図１５に示すように、この回転軸５１０は、ロボット座標系のＸＹ平面上にあり且つｔＺと直交する軸（すなわちｅＺ及びｔＺの両方と直交する軸）である。よって、目標値算出部１０９は、ｅＺ及びｔＺの両方と内積が０となるような単位ベクトルであるｒを１つ求め、それを回転軸５１０とする。なお、目標値算出部１０９は、ｅＺとｔＺとが平行の場合にはｒを一意に求めることはできない。しかしながら、これはアーム２の姿勢が真上方向５０８又は真下方向５０７を向いている場合であり、目標値算出部１０９は、ＸＹ平面上の任意のベクトルを回転軸５１０として算出する。
【0112】
次に、目標値算出部１０９は、算出した目標角度Θ＊及び回転軸５１０に基づいて、目標姿勢として回転角度ΘＲを算出する。ここで、エンドエフェクタ２０１の向き５０２が真上方向（基準方向）５０８から角度Θである場合、回転角度ΘＲの大きさはΘ＊－Θとなるが、図１６に示すように回転の向き（正負）を考慮する必要がある。これは、実施の形態２に係る直接教示装置１では、角度Θを求める際に内積を使っているため、エンドエフェクタ２０１の向き５０２の情報が失われているためである。回転の向きを考慮すると、回転角度ΘＲは次式（５）のようになる。すなわち、回転の向きは、ｅＺ×ｔＺとｒとが同じ向きである場合には符号の反転は不要であり、ｅＺ×ｔＺとｒとが逆の向きである場合には符号の反転が必要である。
目標値算出部１０９では、回転軸５１０を表すｒ＝［ｒｘ，ｒｙ，ｒｚ］Ｔ（Ｔは転置を表す）と回転角度ΘＲが得られれば、アーム２の姿勢から目標姿勢への回転が得られたことになるので、拘束目標が事実上得られたことになる。目標姿勢の具体的な表現が必要な場合は、目標値算出部１０９は、アーム２の姿勢に前述した回転を適用することで得られる。
【0113】
上記では、目標値算出部１０９が、エンドエフェクタ２０１の向き５０２（Ｚ軸）が真下方向５０７又は真横方向５０９を向くような目標姿勢を算出する方法について述べた。目標値算出部１０９は、ここから更に回転を加えて所望の姿勢にして、それを目標姿勢とすることも可能である。
例えば、目標値算出部１０９は、アーム２が真下方向５０７を向いた状態からＺ軸周りに回転を加えることで、図１０に示すような姿勢を目標姿勢とすることも可能である。また、目標値算出部１０９は、図１７に示すように、アーム２が真横方向５０９を向いた状態から更にＺ軸周りに回転を加え、ｔＸ又はｔＹがロボット座標系のＺ軸と平行となる姿勢を目標姿勢とすることも可能である。
例えば、目標値算出部１０９は、アーム２が真下方向５０７を向いた状態からＺ軸周りに回転を加えることで、図１０に示すような姿勢を目標姿勢とすることも可能である。また、目標値算出部１０９は、図１７に示すように、アーム２が真横方向５０９を向いた状態から更にＺ軸周りに回転を加え、ｔＸ又はｔＹがロボット座標系のＺ軸と平行となる姿勢を目標姿勢とすることも可能である。
【0114】
次いで、拘束制御演算部１１０は、位置姿勢演算部１０４による算出結果に基づいて、目標値算出部１０９により算出された目標姿勢に移動するアーム２の動き（拘束制御指令値）を算出する（ステップＳＴ１１１０）。なお、アーム２を現在の姿勢から目標姿勢へと動かすような回転は、目標値算出部１０９が目標姿勢を算出する際に既に算出しているため、拘束制御演算部１１０は、そのようにアーム２を回転させる動きを算出する。ただし、拘束制御演算部１１０の出力は関節角速度θ（ドット）とする必要があるので、拘束制御演算部１１０は、目標値算出部１０９により算出された回転軸５１０であるｒと回転角度ΘＲの組から関節角速度θ（ドット）を算出する必要がある。以下にその計算の一例を示す。
【0115】
拘束制御演算部１１０は、まず、回転の角速度の大きさ｜ω｜を決める。例えば、制限を受けなければ１秒で目標姿勢に到達するように制御するのであれば、｜ω｜＝θＲとする。拘束制御演算部１１０は、角速度が大き過ぎるのが好ましくないのであれば、角速度を適宜制限すればよい。
次に、拘束制御演算部１１０は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。ヤコビ行列Ｊについては次式（６）が成り立つものとする。
ここで、ｖＸ，ｖＹ，ｖＺはエンドエフェクタ２０１の先端のＸ，Ｙ，Ｚ軸方向の速度であり、ωＸ，ωＹ，ωＺはアーム２の姿勢がＸ軸，Ｙ軸，Ｚ軸について回転する角速度であり、θ１（ドット）～θ６（ドット）は各関節角速度である。
次に、拘束制御演算部１１０は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。ヤコビ行列Ｊについては次式（６）が成り立つものとする。
【0116】
次に、拘束制御演算部１１０は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。
次に、拘束制御演算部１１０は、次式（７）を満たすようなアーム２の関節毎の関節角速度θ１（ドット）～θ６（ドット）を算出する。なお、拘束制御演算部１１０は、ヤコビ行列Ｊに逆行列が存在しない場合は疑似逆行列等を用いて関節角速度θ１（ドット）～θ６（ドット）を算出する。
次に、拘束制御演算部１１０は、次式（７）を満たすようなアーム２の関節毎の関節角速度θ１（ドット）～θ６（ドット）を算出する。なお、拘束制御演算部１１０は、ヤコビ行列Ｊに逆行列が存在しない場合は疑似逆行列等を用いて関節角速度θ１（ドット）～θ６（ドット）を算出する。
【0117】
なお、拘束制御演算部１１０は、得られた関節角速度が過大になるのを避けるために、リミッタを適用してもよい。その場合、拘束制御演算部１１０は、全ての関節角に同じ定数（０から１の間の値）を掛けて制限し、回転の向きを変えないようにすることが好ましい。
【0118】
次いで、合成部１１１は、従動制御演算部１０３から出力された従動制御指令値（関節角θ）と拘束制御演算部１１０から出力された拘束制御指令値（関節角速度θ（ドット））とを合成する（ステップＳＴ１１１１）。
例えば、従動制御演算部１０３が関節角θ＝［θ１，・・・，θｍ］Ｔを出力し、拘束制御演算部１１０が関節角速度θ（ドット）＝［θ１（ドット），・・・，θｍ（ドット）］Ｔを出力した場合、制御周期ＴＳから、合成部１１１の出力である関節角指令値はθ＋ＴＳθ（ドット）となる。ただし、ｍは駆動制御部１１２が制御するアーム２の関節数である。
例えば、従動制御演算部１０３が関節角θ＝［θ１，・・・，θｍ］Ｔを出力し、拘束制御演算部１１０が関節角速度θ（ドット）＝［θ１（ドット），・・・，θｍ（ドット）］Ｔを出力した場合、制御周期ＴＳから、合成部１１１の出力である関節角指令値はθ＋ＴＳθ（ドット）となる。ただし、ｍは駆動制御部１１２が制御するアーム２の関節数である。
【0119】
次いで、駆動制御部１１２は、合成部１１１による合成結果（関節角指令値）に基づいて、アーム２を駆動する（ステップＳＴ１１１２）。
【0120】
なお上記では一部、６軸（関節数が６個）のアーム２を前提として説明及び例示を行ったが、６軸以外のアーム２であっても同様に実施可能である。
【0121】
実施の形態３．
実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態３に係る直接教示装置１では、拘束方法が軸拘束のみに限定された場合を示す。軸拘束とは、アーム２の位置を所定の拘束軸５０４に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態３に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図１８に示すようなロボット座標系のＺ軸に平行な直線５１１への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束軸５０４及び拘束方向５０５は任意に設定可能である。
なお、実施の形態３に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態３に係る直接教示装置１では、拘束方法が軸拘束のみに限定された場合を示す。軸拘束とは、アーム２の位置を所定の拘束軸５０４に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態３に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図１８に示すようなロボット座標系のＺ軸に平行な直線５１１への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束軸５０４及び拘束方向５０５は任意に設定可能である。
なお、実施の形態３に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
【0122】
なお、実施の形態３における位置姿勢演算部１０４は、アーム２の位置及び姿勢を算出する。
また、実施の形態３における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束軸５０４に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
また、実施の形態３における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束軸５０４に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
【0123】
図１９は、実施の形態３に係る直接教示装置１の動作例を示すフローチャートである。図１９に示す実施の形態３に係る直接教示装置１において、図１１に示す実施の形態２に係る直接教示装置１と動作が異なるのは、位置姿勢演算部１０４、近接判定部１０５、解除操作判定部１０６、復帰判定部１０７、目標値算出部１０９及び拘束制御演算部１１０であり、これらの構成の動作例についてのみ説明を行う。
【0124】
まず、位置姿勢演算部１０４は、従動制御演算部１０３による算出結果に基づいて、アーム２の位置及び姿勢を算出する（ステップＳＴ１９０４）。実施の形態３ではアーム２の位置及び姿勢の両方を拘束するため、位置姿勢演算部１０４は、アーム２の位置及び姿勢の両方を算出する必要がある。位置姿勢演算部１０４は、順運動学により上記算出を実行できる。
【0125】
また、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束軸５０４に近づいているか及びアーム２の姿勢が拘束方向５０５に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム２の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する（ステップＳＴ１９０５）。例えば、近接判定部１０５は、アーム２の位置が拘束軸５０４に近づき且つアーム２の姿勢が拘束方向５０５に近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。近接判定部１０５による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、近接判定部１０５による拘束軸５０４に対する判定について説明する。
【0126】
この際、近接判定部１０５は、例えば図２０に示すように、エンドエフェクタ２０１の位置５０１から拘束軸５０４に対して垂線５１２をおろし、その垂線５１２の長さが所定の値より小さければアーム２の位置が拘束軸５０４に近づいていると判定する。例えばアーム２の位置の座標を［ｘ，ｙ，ｚ］で表し、直線５１１のＸ座標とＹ座標をそれぞれｘ＊，ｙ＊で表す。この場合、垂線５１２の長さは次式（８）のようになる。
そして、近接判定部１０５は、式（８）に示す垂線５１２の長さが所定の値よりも小さければ、アーム２の位置が拘束軸５０４に近づいていると判定し、それ以外の場合にはアーム２の位置が拘束軸５０４には近づいていないと判定する。なお、近接判定部１０５は、拘束軸５０４がロボット座標系のＺ軸に平行な場合以外でも、同様にして拘束軸５０４に対しておろした垂線の長さから接近判定が可能である。
【0127】
次いで、解除操作判定部１０６は、操作者が解除操作を行ったかを判定する（ステップＳＴ１９０６）。ここで、解除操作判定部１０６は、外力を用いた判定を行う場合には、実施の形態１と同様の方法が適用可能である。一方、解除操作判定部１０６は、アーム２の位置姿勢を用いた判定を行う場合には、実施の形態３では直接教示装置１がアーム２の軸拘束を行っているため、適用可能な判定方法は軸拘束と両立可能な判定方法に限定される。例えば、実施の形態３における解除操作判定部１０６は、図７の例では、図７Ｄ，７Ｅに示した方法のみ適用可能である。一方、実施の形態３における解除操作判定部１０６は、図７Ａ～７Ｃに示した方法は、アーム２が軸に拘束されたままでは実施できないため、適用不可である。
【0128】
次いで、復帰判定部１０７は、拘束付き直接教示へと切換え可能な状態への復帰条件を満たしたかを判定する（ステップＳＴ１９０７）。実施の形態３では直接教示装置１がアーム２の軸拘束を行っているため、復帰判定部１０７は、アーム２の位置姿勢に基づいて判定を行う場合には、アーム２の位置と拘束軸５０４との間の距離及びアーム２の姿勢と拘束方向５０５との間の関係に基づいて判定可能である。復帰判定部１０７による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、復帰判定部１０７による拘束軸５０４に対する判定について説明する。
【0129】
復帰判定部１０７は、拘束軸５０４に対する判定では、近接判定部１０５と同様に、垂線５１２の長さに基づいて行う。すなわち、復帰判定部１０７は、垂線５１２の長さが所定の値より大きい場合に、復帰条件を満たしたと判定する。なお、上記所定の値は、近接判定部１０５が用いるしきい値と同じ、又はより大きくする必要がある。これは、復帰判定部１０７が上記しきい値より狭い値を用いると、復帰判定部１０７が復帰条件を満たしたと判定した直後に近接判定部１０５がアーム２の位置姿勢が拘束目標に近づいていると判定してしまい、直接教示装置１が拘束付き直接教示から離脱できなくなってしまうためである。
【0130】
なお、復帰判定部１０７は、判定にアーム２の位置姿勢を用いず時間のみを用いる場合には、実施の形態１と同様の方法が適用可能である。
【0131】
また、目標値算出部１０９は、実施の形態２で説明した目標姿勢の算出に加え、アーム２の位置を拘束軸５０４へと移動させる場合の目標位置の算出を行う（ステップＳＴ１９０９）。例えば、目標値算出部１０９は、エンドエフェクタ２０１の位置５０１から図２０に示すように拘束軸５０４へと垂線５１２をおろした位置５１３を目標位置とする。ここで、目標値算出部１０９は、アーム２の位置をロボット座標系のＺ軸に平行な拘束軸５０４に拘束させるのであれば、目標位置は［ｘ＊，ｙ＊，ｚ］となる。
【0132】
また、拘束制御演算部１１０は、実施の形態２と同様に、ヤコビ行列Ｊを用いてアーム２の関節毎に関節角速度θ（ドット）を算出する（ステップＳＴ１９１０）。ただし、実施の形態３における拘束制御演算部１１０では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態２に対して以下のように変わる。
【0133】
まず、拘束制御演算部１１０は、回転の角速度の大きさ｜ω｜を決める。
次に、拘束制御演算部１１０は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
次に、拘束制御演算部１１０は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。ここまでの動作は、実施の形態２と同様である。
次に、拘束制御演算部１１０は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
次に、拘束制御演算部１１０は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。ここまでの動作は、実施の形態２と同様である。
【0134】
次に、拘束制御演算部１１０は、アーム２の位置を拘束軸５０４へと移動させる速さ｜ｖ｜を決める。例えば、制限を受けなければ１秒で目標位置に到達するように制御するのであれば、拘束制御演算部１１０は、｜ｖ｜＝√｛（ｘ－ｘ＊）２＋（ｙ－ｙ＊）２｝とする。また、拘束制御演算部１１０は、ある定数ｖ＊を決めて｜ｖ｜＝ｖ＊としてもよい。
次に、拘束制御演算部１１０は、アーム２の位置に関する速度ベクトルｖ＝［ｖＸ，ｖＹ，０］Ｔを算出する。拘束制御演算部１１０は、ロボット座標系のＺ軸に平行な拘束軸５０４に拘束させる場合はｖ＝｜ｖ｜［ｘ＊－ｘ，ｙ＊－ｙ，０］／√｛（ｘ－ｘ＊）２＋（ｙ－ｙ＊）２｝より算出可能である。
次に、拘束制御演算部１１０は、アーム２の関節毎に関節角速度θ（ドット）を次式（９）より算出する。なお、拘束制御演算部１１０は、得られた関節角速度にリミッタを適用してもよい。
次に、拘束制御演算部１１０は、アーム２の位置に関する速度ベクトルｖ＝［ｖＸ，ｖＹ，０］Ｔを算出する。拘束制御演算部１１０は、ロボット座標系のＺ軸に平行な拘束軸５０４に拘束させる場合はｖ＝｜ｖ｜［ｘ＊－ｘ，ｙ＊－ｙ，０］／√｛（ｘ－ｘ＊）２＋（ｙ－ｙ＊）２｝より算出可能である。
次に、拘束制御演算部１１０は、アーム２の関節毎に関節角速度θ（ドット）を次式（９）より算出する。なお、拘束制御演算部１１０は、得られた関節角速度にリミッタを適用してもよい。
【0135】
その他の動作は実施の形態２と同じになる。
なお、実施の形態３に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された軸に拘束する制御となる。
なお、実施の形態３に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された軸に拘束する制御となる。
【0136】
実施の形態４．
実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態４に係る直接教示装置１では、拘束方法が面拘束のみに限定された場合を示す。面拘束とは、アーム２の位置を所定の拘束面５０３に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態４に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図２１に示すようなＺ軸座標がＸ軸座標及びＹ軸座標の関数ｚ＝ｆ（ｘ，ｙ）で表される曲面５１３への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束面５０３及び拘束方向５０５は任意に設定可能であり、例えば拘束面５０３が関数ｘ＝ｆ（ｙ，ｚ）又はｙ＝ｆ（ｚ，ｘ）で表される場合等についても同様の方法で実施可能である。
なお、実施の形態４に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
実施の形態２に係る直接教示装置１では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態４に係る直接教示装置１では、拘束方法が面拘束のみに限定された場合を示す。面拘束とは、アーム２の位置を所定の拘束面５０３に拘束する位置拘束とアーム２の姿勢を所定の拘束方向５０５に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態４に係る直接教示装置１は、実施の形態２で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置１が、図２１に示すようなＺ軸座標がＸ軸座標及びＹ軸座標の関数ｚ＝ｆ（ｘ，ｙ）で表される曲面５１３への位置拘束及び真下方向５０７への姿勢拘束を行う場合を示すが、拘束面５０３及び拘束方向５０５は任意に設定可能であり、例えば拘束面５０３が関数ｘ＝ｆ（ｙ，ｚ）又はｙ＝ｆ（ｚ，ｘ）で表される場合等についても同様の方法で実施可能である。
なお、実施の形態４に係る直接教示装置１の構成例は、実施の形態２に係る直接教示装置１の構成例と同様であり、以下では図１に示す構成例を用いて説明を行う。
【0137】
なお、実施の形態４における位置姿勢演算部１０４は、アーム２の位置及び姿勢を算出する。
また、実施の形態４における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束面５０３に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
また、実施の形態４における近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束目標である拘束面５０３に近づいているかを判定し且つアーム２の姿勢が拘束目標である拘束方向５０５に近づいているかを判定することで、アーム２の位置姿勢が拘束目標に近づいているかを判定する。
【0138】
図２２は、実施の形態４に係る直接教示装置１の動作例を示すフローチャートである。図２２に示す実施の形態４に係る直接教示装置１において、図１９に示す実施の形態３に係る直接教示装置１と動作が異なるのは、近接判定部１０５、解除操作判定部１０６、復帰判定部１０７、目標値算出部１０９及び拘束制御演算部１１０であり、これらの構成の動作例についてのみ説明を行う。
【0139】
まず、近接判定部１０５は、位置姿勢演算部１０４による算出結果に基づいて、アーム２の位置が拘束面５０３に近づいているか及びアーム２の姿勢が拘束方向５０５に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム２の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する（ステップＳＴ２２０５）。例えば、近接判定部１０５は、アーム２の位置が拘束面５０３に近づき且つアーム２の姿勢が拘束方向５０５に近づいていると判定した場合には、アーム２の位置姿勢は拘束目標に近づいていると判定する。近接判定部１０５による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、近接判定部１０５による拘束面５０３に対する判定について説明する。
【0140】
この際、近接判定部１０５は、エンドエフェクタ２０１の位置５０１から拘束面５０３に対してロボット座標系のＺ軸に平行な直線をおろし、その直線の長さが所定の値より小さければアーム２の位置が拘束面５０３に近づいていると判定する。例えばアーム２の位置の座標が［ｘ，ｙ，ｚ］である場合、上記直線の長さは｛ｚ－ｆ（ｘ，ｙ）｝となる。そして、近接判定部１０５は、この長さが所定の値よりも小さければ、アーム２の位置が拘束面５０３に近づいていると判定し、それ以外の場合にはアーム２の位置が拘束面５０３には近づいていないと判定する。
【0141】
次いで、解除操作判定部１０６は、操作者が解除操作を行ったかを判定する（ステップＳＴ２２０６）。ここで、解除操作判定部１０６は、外力を用いた判定を行う場合には、実施の形態１と同様の方法が適用可能である。一方、解除操作判定部１０６は、アーム２の位置姿勢を用いた判定を行う場合には、実施の形態４では直接教示装置１がアーム２の曲面拘束を行っているため、適用可能な判定方法は曲面拘束と両立可能な判定方法に限定される。例えば、実施の形態４における解除操作判定部１０６は、図７では、図７Ａ，７Ｂに示した方法のみ適用可能である。一方、実施の形態４における解除操作判定部１０６は、図７Ｃに示した方法は、アーム２の姿勢を自由に変更できないため、適用不可である。また、実施の形態４における解除操作判定部１０６は、図７Ｄ，７Ｅに示した方法は、アーム２を曲面５１３から離すことができないため、適用不可である。
【0142】
次いで、復帰判定部１０７は、拘束付き直接教示へと切換え可能な状態への復帰条件を満たしたかを判定する（ステップＳＴ２２０７）。実施の形態４では直接教示装置１がアーム２の曲面拘束を行っているため、復帰判定部１０７は、アーム２の位置姿勢に基づいて判定を行う場合には、アーム２の位置と拘束面５０３との間の距離及びアーム２の姿勢と拘束方向５０５との間の関係に基づいて判定可能である。復帰判定部１０７による拘束方向５０５に対する判定については実施の形態２で説明しているので省略し、以下では、復帰判定部１０７による拘束面５０３に対する判定について説明する。
【0143】
復帰判定部１０７は、拘束面５０３に対する判定では、近接判定部１０５と同様に、エンドエフェクタ２０１の位置５０１から拘束面５０３に対してロボット座標系のＺ軸に平行な直線をおろし、その直線の長さに基づいて行う。すなわち、復帰判定部１０７は、この長さが所定の値より大きい場合に、復帰条件を満たしたと判定する。なお、上記所定の値は、近接判定部１０５が用いる所定の値と同じ、又はより大きくする必要がある。その理由は実施の形態３と同様である。
【0144】
なお、復帰判定部１０７は、判定にアーム２の位置姿勢を用いず時間のみを用いる場合には、実施の形態１と同様の方法が適用可能である。
【0145】
また、目標値算出部１０９は、実施の形態２で説明した目標姿勢の算出に加え、アーム２の位置を拘束面５０３へと移動させる場合の目標位置の算出を行う（ステップＳＴ２２０９）。例えば、目標値算出部１０９は、アーム２の位置の座標が［ｘ，ｙ，ｚ］である場合、Ｚ軸座標だけを曲面上の座標に置き換えた［ｘ，ｙ，ｆ（ｘ，ｙ）］を目標位置とする。
【0146】
また、拘束制御演算部１１０は、実施の形態２と同様に、ヤコビ行列Ｊを用いてアーム２の関節毎に関節角速度θ（ドット）を算出する（ステップＳＴ２２１０）。ただし、実施の形態４における拘束制御演算部１１０では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態２に対して以下のように変わる。
【0147】
まず、拘束制御演算部１１０は、回転の角速度の大きさ｜ω｜を決める。
次に、拘束制御演算部１１０は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
次に、拘束制御演算部１１０は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。ここまでの動作は、実施の形態２と同様である。
次に、拘束制御演算部１１０は、アーム２の現在の位置姿勢におけるヤコビ行列Ｊを算出する。
次に、拘束制御演算部１１０は、回転軸５１０であるｒと回転速度の大きさ｜ω｜から姿勢に関する回転ベクトルω＝［ωＸ，ωＹ，ωＺ］Ｔを算出する。ここまでの動作は、実施の形態２と同様である。
【0148】
次に、拘束制御演算部１１０は、アーム２の位置を拘束軸５０４へと移動させる速さ｜ｖ｜を決める。例えば、制限を受けなければ１秒で目標位置に到達するように制御するのであれば、拘束制御演算部１１０は、｜ｖ｜＝｜ｚ－ｆ（ｘ，ｙ）｜とする。また、拘束制御演算部１１０は、速さ｜ｖ｜をある定数値にしてもよい。
次に、拘束制御演算部１１０は、アーム２の位置に関する速度ベクトルｖ＝［０，０，ｖＺ］Ｔをｖ＝［０，０，｜ｖ｜］より算出する。
次に、拘束制御演算部１１０は、アーム２の関節毎に関節角速度θ（ドット）を次式（１０）より算出する。なお、拘束制御演算部１１０は、得られた関節角速度にリミッタを適用してもよい。
次に、拘束制御演算部１１０は、アーム２の位置に関する速度ベクトルｖ＝［０，０，ｖＺ］Ｔをｖ＝［０，０，｜ｖ｜］より算出する。
次に、拘束制御演算部１１０は、アーム２の関節毎に関節角速度θ（ドット）を次式（１０）より算出する。なお、拘束制御演算部１１０は、得られた関節角速度にリミッタを適用してもよい。
【0149】
その他の動作は実施の形態２，３と同じになる。
なお、実施の形態４に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された面に拘束する制御となる。また、実施の形態４に係る直接教示装置１において、アーム２の位置から拘束面５０３へ垂線を下した方向を目標姿勢とした場合には、エンドエフェクタ２０１が拘束面５０３に対して常に垂直となるような姿勢拘束になる。
なお、実施の形態４に係る直接教示装置１において、姿勢拘束に関する演算を省略した場合には、アーム２の姿勢に関する拘束は行われず、アーム２の位置のみを指定された面に拘束する制御となる。また、実施の形態４に係る直接教示装置１において、アーム２の位置から拘束面５０３へ垂線を下した方向を目標姿勢とした場合には、エンドエフェクタ２０１が拘束面５０３に対して常に垂直となるような姿勢拘束になる。
【0150】
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【0151】
１ 直接教示装置
２ アーム
１０１ 位置姿勢計測部
１０２ 外力検知部
１０３ 従動制御演算部
１０４ 位置姿勢演算部
１０５ 近接判定部
１０６ 解除操作判定部
１０７ 復帰判定部
１０８ 切換部
１０９ 目標値算出部
１１０ 拘束制御演算部
１１１ 合成部
１１２ 駆動制御部
２０１ エンドエフェクタ
２ アーム
１０１ 位置姿勢計測部
１０２ 外力検知部
１０３ 従動制御演算部
１０４ 位置姿勢演算部
１０５ 近接判定部
１０６ 解除操作判定部
１０７ 復帰判定部
１０８ 切換部
１０９ 目標値算出部
１１０ 拘束制御演算部
１１１ 合成部
１１２ 駆動制御部
２０１ エンドエフェクタ
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図22】
【図23】