特開2019-202383(P2019-202383A)IP Force 特許公報掲載プロジェクト 2015.5.11 β版

知財求人 - 知財ポータルサイト「IP Force」

▶ アズビル株式会社の特許一覧
特開2019-202383ロボットの直接教示装置及び直接教示方法
<>
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000013
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000014
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000015
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000016
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000017
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000018
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000019
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000020
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000021
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000022
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000023
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000024
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000025
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000026
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000027
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000028
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000029
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000030
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000031
  • 特開2019202383-ロボットの直接教示装置及び直接教示方法 図000032
< >
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】特開2019-202383(P2019-202383A)
(43)【公開日】2019年11月28日
(54)【発明の名称】ロボットの直接教示装置及び直接教示方法
(51)【国際特許分類】
   B25J 9/22 20060101AFI20191101BHJP
   G05B 19/42 20060101ALI20191101BHJP
【FI】
   B25J9/22 Z
   G05B19/42 D
【審査請求】未請求
【請求項の数】7
【出願形態】OL
【全頁数】27
(21)【出願番号】特願2018-98891(P2018-98891)
(22)【出願日】2018年5月23日
(71)【出願人】
【識別番号】000006666
【氏名又は名称】アズビル株式会社
(74)【代理人】
【識別番号】100123434
【弁理士】
【氏名又は名称】田澤 英昭
(74)【代理人】
【識別番号】100101133
【弁理士】
【氏名又は名称】濱田 初音
(74)【代理人】
【識別番号】100199749
【弁理士】
【氏名又は名称】中島 成
(74)【代理人】
【識別番号】100188880
【弁理士】
【氏名又は名称】坂元 辰哉
(74)【代理人】
【識別番号】100197767
【弁理士】
【氏名又は名称】辻岡 将昭
(74)【代理人】
【識別番号】100201743
【弁理士】
【氏名又は名称】井上 和真
(72)【発明者】
【氏名】田原 鉄也
(72)【発明者】
【氏名】岩下 純久
(72)【発明者】
【氏名】野村 琢磨
(72)【発明者】
【氏名】杉本 広大
【テーマコード(参考)】
3C269
3C707
【Fターム(参考)】
3C269AB33
3C269BB09
3C269SA02
3C269SA30
3C707BS10
3C707JT09
3C707KS16
3C707KS20
3C707KS33
3C707KW03
3C707LS02
3C707LS15
3C707LV19
(57)【要約】
【課題】操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能とする。
【解決手段】外力を検知する外力検知部102と、外力に従うアーム2の動きを算出する従動制御演算部103と、アーム2の位置姿勢を算出する位置姿勢演算部104と、アーム2の位置姿勢が拘束目標に近づいているかを判定する近接判定部105と、拘束制御の目標値を算出する目標値算出部107と、目標値に移動するアーム2の動きを算出する拘束制御演算部108と、拘束制御演算部108による算出結果を制限する拘束制御制限部109と、拘束目標に近づいていると判定された場合に拘束制御演算部108を有効とし、拘束目標に近づいていないと判定された場合に拘束制御演算部108を無効とする切換部106と、従動制御演算部103による算出結果と拘束制御制限部109による制限結果とを合成する合成部110と、合成結果に基づいてアーム2を駆動する駆動制御部111とを備えた。
【選択図】図1
【特許請求の範囲】
【請求項1】
ロボットが有するアームに加えられた外力を検知する外力検知部と、
前記外力検知部により検知された外力に従う前記アームの動きを算出する従動制御演算部と、
前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、
前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、
拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部と、
前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出する拘束制御演算部と、
前記拘束制御演算部による算出結果を制限する拘束制御制限部と、
前記近接判定部により拘束目標に近づいていると判定された場合に前記拘束制御演算部による算出結果を有効とし、前記近接判定部により拘束目標に近づいていないと判定された場合に前記拘束制御演算部による算出結果を無効とする切換部と、
前記従動制御演算部による算出結果と前記拘束制御制限部による制限結果とを合成する合成部と、
前記合成部による合成結果に基づいて前記アームを駆動する駆動制御部と
を備えた直接教示装置。
【請求項2】
前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、
前記従動制御演算部は、位置姿勢計測部による計測結果に基づいて、前記外力検知部により検知された外力に従う前記アームの動きを算出し、
前記位置姿勢演算部は、前記従動制御演算部による算出結果に基づいて、前記アームの位置姿勢を算出する
ことを特徴とする請求項1記載の直接教示装置。
【請求項3】
前記アームの位置姿勢に関するパラメータを計測する位置姿勢計測部を備え、
前記位置姿勢演算部は、前記位置姿勢計測部による計測結果に基づいて、前記アームの位置姿勢を算出する
ことを特徴とする請求項1記載の直接教示装置。
【請求項4】
前記アームは垂直多関節型ロボットアームであり、
前記位置姿勢演算部は、前記アームの姿勢を算出し、
前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの姿勢が、拘束目標である拘束方向に近づいているかを判定する
ことを特徴とする請求項1から請求項3のうちの何れか1項記載の直接教示装置。
【請求項5】
前記アームは垂直多関節型ロボットアームであり、
前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束軸に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
ことを特徴とする請求項1から請求項3のうちの何れか1項記載の直接教示装置。
【請求項6】
前記アームは垂直多関節型ロボットアームであり、
前記位置姿勢演算部は、前記アームの位置及び姿勢を算出し、
前記近接判定部は、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置が拘束目標である拘束面に近づいているかを判定し且つ当該アームの姿勢が拘束目標である拘束方向に近づいているかを判定することで、当該アームの位置姿勢が拘束目標に近づいているかを判定する
ことを特徴とする請求項1から請求項3のうちの何れか1項記載の直接教示装置。
【請求項7】
外力検知部が、ロボットが有するアームに加えられた外力を検知するステップと、
従動制御演算部が、前記外力検知部により検知された外力に従う前記アームの動きを算出するステップと、
位置姿勢演算部が、前記アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出するステップと、
近接判定部が、前記位置姿勢演算部による算出結果に基づいて、前記アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定するステップと、
目標値算出部が、拘束目標及び前記位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出するステップと、
拘束制御演算部が、前記位置姿勢演算部による算出結果に基づいて、前記目標値算出部により算出された目標値に移動する前記アームの動きを算出するステップと、
拘束制御制限部が、前記拘束制御演算部による算出結果を制限するステップと、
切換部が、前記近接判定部により拘束目標に近づいていると判定された場合に前記拘束制御演算部による算出結果を有効とし、前記近接判定部により拘束目標に近づいていないと判定された場合に前記拘束制御演算部による算出結果を無効とするステップと、
合成部が、前記従動制御演算部による算出結果と前記拘束制御制限部による制限結果とを合成するステップと、
駆動制御部が、前記合成部による合成結果に基づいて前記アームを駆動するステップと
を有する直接教示方法。
【発明の詳細な説明】
【技術分野】
【0001】
この発明は、ロボットの直接教示を行う直接教示装置及び直接教示方法に関する。
【背景技術】
【0002】
産業用のロボットでは、ロボットに作業をさせるために、前もって教示(ティーチング)と呼ばれる作業が実施される。このロボットの教示を行う方法の1つとして、直接教示(ダイレクト教示)と呼ばれる方法がある。
【0003】
例えば特許文献1では、力センサを用いたロボットの直接教示方法が開示されている。また、例えば特許文献2では、トルク検出手段を用いたロボットの直接教示方法が開示されている。これらの特許文献で開示されている直接教示装置の概略構成を図20に示す。
【0004】
図20に示す直接教示装置11では、まず、外力検知部1101が、力センサ又はトルクセンサ等を用いて、ロボットが有するアーム2に対して操作者により加えられた外力を検知する。次いで、従動制御演算部1102が、外力検知部1101により検知された外力に従うアームの動きを算出(従動制御演算)する。なお、従動制御演算部1102は、従動制御演算において、位置姿勢計測部(不図示)により計測されたアームの位置姿勢に関するパラメータを用いる場合もある。なお、アームの位置姿勢とは、アームの位置及びアームの姿勢のうちの少なくとも一方を意味する。また、上記パラメータとしては、アームの位置、アームの姿勢、又は、アームの関節角等が挙げられる。そして、従動制御演算部1102は、従動制御演算の結果に基づいて更新した従動制御指令値を駆動制御部1103に通知する。次いで、駆動制御部1103は、従動制御演算部1102により通知された従動制御指令値に従ってアーム2を駆動する。
この一連の動作により、図20に示す直接教示装置11は、操作者により加えられた外力に従ってアーム2が動くように制御できる。そして、直接教示装置11は、制御によりアーム2の位置及び姿勢が操作者の意図する状態となった場合に、その際の上記パラメータを教示点として記録する。この直接教示装置11により記録された教示点はロボットが作業する際に使用される。
【0005】
このように、ロボットの直接教示は、操作者がアームを直接操作して位置及び姿勢を教示するため、操作者にとって直観的でわかりやすいという利点を持つ。一方で、アームが操作者により加えられた外力に従ってそのまま動くという特徴は利点ばかりではない。
【0006】
例えば、操作者が、ロボットに対して、ある点Pの位置を教示した後に、その真下の別の点Qの位置を教示するというケースを考える。このケースでは、操作者は、真下方向(Z軸方向)にアームを動かすことになる。しかしながら、操作者がアームに対して正確に真下方向へ外力を加えることは難しく、アームのX軸座標及びY軸座標がずれることが多い。後からアームのX軸座標及びY軸座標だけをティーチングペンダント等を用いて修正することは可能であるが、それでは直接教示の利点が減ずる。
また、例えば、操作者が、アームの先端に設けられたエンドエフェクタを真下に向けたまま教示を行うケースを考える。このケースにおいても、操作者がアームに対して正確に真下に向けた状態を維持したままアームを直接操作することは難しく、アームの姿勢がずれることが多い。
以上のように、直接教示は、直観的で分かりやすい反面、操作が難しいケースも存在する。
【0007】
これに対し、例えば特許文献3,4では、上記のような課題を解決する技術が開示されている。
【0008】
特許文献3では、モード切換スイッチにより、動作モードを、位置方向移動モード、位置移動モード又は方向移動モードに切換え可能とする直接教示装置が開示されている。位置移動モードでは、アームの先端の姿勢を維持したまま位置のみを移動可能としている。また、方向移動モードでは、アームの先端の位置を維持したまま姿勢のみを変更可能としている。この直接教示装置により、上記のような直接教示における操作の難しさをある程度緩和可能となる。なお、通常の直接教示は位置方向移動モードで実現可能である。
【0009】
また、特許文献4では、入力装置により、動作モードを、拘束モード又は全方向移動モードに切換え可能とする直接教示装置が開示されている。拘束モードでは、エンドエフェクタの先端を特定の拘束軸又は拘束面に沿って移動可能としている。また、全方向移動モードでは、通常の直接教示を実施可能としている。この直接教示装置は、直接教示の途中で拘束モードに切換えることにより、アームの先端を正確に上下方向に動かすことが可能となり、直接教示における操作の難しさが軽減される。
【先行技術文献】
【特許文献】
【0010】
【特許文献1】特開平05−204441号公報
【特許文献2】特開平05−250029号公報
【特許文献3】特開平05−285870号公報
【特許文献4】特開平05−303425号公報
【発明の概要】
【発明が解決しようとする課題】
【0011】
上記のように、特許文献3,4に開示された直接教示装置は、アームの操作に拘束を課さない通常の直接教示を行うモードと、アームの操作に拘束を課す拘束付き直接教示を行うモードとを切換える。そして、特許文献3ではモード切換スイッチによって、また、特許文献4では入力装置によって、モードの切換えが行われており、操作者が手でスイッチ等を操作することで切換え可能となっている。
【0012】
しかしながら、直接教示では、操作者は手でアームを直接操作しているため、モードの切換えを手で操作するのは便利ではない。例えば、操作者が両手でアームを操作している場合、モードの切換えを操作する際にはアームの操作を一旦停止してアームを片手で保持することになる。通常時に両手で保持しているアームを片手で保持すると、アームの位置姿勢がずれることも考えられる。その結果、拘束付き直接教示の効果を減ずることにもなりかねない。
【0013】
この発明は、上記のような課題を解決するためになされたもので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換える可能な直接教示装置を提供することを目的としている。
【課題を解決するための手段】
【0014】
この発明に係る直接教示装置は、ロボットが有するアームに加えられた外力を検知する外力検知部と、外力検知部により検知された外力に従うアームの動きを算出する従動制御演算部と、アームの位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部と、位置姿勢演算部による算出結果に基づいて、アームの位置姿勢が、当該アームの位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部と、拘束目標及び位置姿勢演算部による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部と、位置姿勢演算部による算出結果に基づいて、目標値算出部により算出された目標値に移動するアームの動きを算出する拘束制御演算部と、拘束制御演算部による算出結果を制限する拘束制御制限部と、近接判定部により拘束目標に近づいていると判定された場合に拘束制御演算部による算出結果を有効とし、近接判定部により拘束目標に近づいていないと判定された場合に拘束制御演算部による算出結果を無効とする切換部と、従動制御演算部による算出結果と拘束制御制限部による制限結果とを合成する合成部と、合成部による合成結果に基づいてアームを駆動する駆動制御部とを備えたことを特徴とする。
【発明の効果】
【0015】
この発明によれば、上記のように構成したので、操作者がアームを操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。
【図面の簡単な説明】
【0016】
図1】この発明の実施の形態1に係る直接教示装置の構成例を示す図である。
図2図2A図2Dは、この発明の実施の形態1に係る直接教示装置による通常の直接教示と拘束付き直接教示(位置拘束の場合)の切換えの一例を示す図である。
図3図3A図3Dは、この発明の実施の形態1に係る直接教示装置による通常の直接教示と拘束付き直接教示(姿勢拘束の場合)の切換えの一例を示す図である。
図4】この発明の実施の形態1に係る直接教示装置の動作例を示すフローチャートである。
図5】この発明の実施の形態1における近接判定部の動作例を説明する図である(姿勢拘束の場合)。
図6】この発明の実施の形態1における目標値算出部の動作例を説明する図である(姿勢拘束の場合)。
図7】この発明の実施の形態1における目標値算出部の動作例を説明する図である(姿勢拘束の場合)。
図8】この発明の実施の形態2に係る直接教示装置の動作例を示すフローチャートである。
図9図9A図9Bは、この発明の実施の形態2における従動制御演算部の動作例を示す図である。
図10】この発明の実施の形態2における近接判定部の動作例を示すフローチャートである。
図11図11A図11B図11Cは、この発明の実施の形態2における近接判定部の動作例を説明する図である。
図12図12A図12Bは、この発明の実施の形態2における目標値算出部の動作例を説明する図であり、回転軸の算出例を示す図である。
図13図13A図13Bは、この発明の実施の形態2における目標値算出部の動作例を説明する図であり、回転方向の算出例を示す図である。
図14】この発明の実施の形態2における目標値算出部の動作例を説明する図である。
図15】この発明の実施の形態3に係る直接教示装置による軸拘束を説明する図である。
図16】この発明の実施の形態3に係る直接教示装置の動作例を示すフローチャートである。
図17】この発明の実施の形態3における近接判定部の動作例を説明する図である。
図18】この発明の実施の形態4に係る直接教示装置による面拘束を説明する図である。
図19】この発明の実施の形態4に係る直接教示装置の動作例を示すフローチャートである。
図20】従来の直接教示装置の構成例を示す図である。
【発明を実施するための形態】
【0017】
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態1.
図1はこの発明の実施の形態1に係る直接教示装置1の構成例を示す図である。
直接教示装置1は、ロボットの直接教示を行う。この直接教示装置1は、直接教示方法として、ロボットが有するアーム2の位置姿勢に対して拘束を課さない通常の直接教示と、アーム2の位置姿勢を拘束目標に拘束する拘束付き直接教示とを切換え可能である。
なお、アーム2の位置姿勢とは、アーム2の位置及びアーム2の姿勢のうちの少なくとも一方を意味する。また、アーム2の位置とは、アーム2の先端に設けられたエンドエフェクタ201の位置501を意味し、アーム2の姿勢とはエンドエフェクタ201の向き502を意味する。また、拘束目標とは、アーム2の位置姿勢の拘束先である。アーム2の位置に対する拘束目標としては、拘束面503及び拘束軸504がある。拘束面503の種別としては、平面及び曲面がある。拘束軸504の種別としては、直線及び曲線がある。アーム2の姿勢に対する拘束目標としては、拘束方向505がある。また、直接教示装置1で用いる拘束目標は1つに限らず複数でもよい。また、ロボットに対しては基準となるロボット座標系が設定され、エンドエフェクタ201に対してはツール座標系が設定される(図6等参照)。
【0018】
この直接教示装置1は、図1に示すように、位置姿勢計測部101、外力検知部102、従動制御演算部103、位置姿勢演算部104、近接判定部105、切換部106、目標値算出部107、拘束制御演算部108、拘束制御制限部109、合成部110及び駆動制御部111を備えている。なお、従動制御演算部103、位置姿勢演算部104、近接判定部105、切換部106、目標値算出部107、拘束制御演算部108、拘束制御制限部109、合成部110及び駆動制御部111は、システムLSI(Large−Scale Integration)等の処理回路、又はメモリ等に記憶されたプログラムを実行するCPU(Central Processing Unit)等により実現される。
【0019】
位置姿勢計測部101は、アーム2の位置姿勢に関するパラメータを計測する。なお、アーム2の位置姿勢に関するパラメータとしては、アーム2の位置、アーム2の姿勢、又は、アーム2の関節角θ等が挙げられる。
【0020】
外力検知部102は、操作者によりアーム2に加えられた外力を検知する。例えば、外力検知部102は、アーム2の先端に取付けられた力センサを用い、この力センサにより計測された力Fを上記外力として検知してもよい。また、例えば、外力検知部102は、アーム2のモータ駆動軸に取付けられたトルクセンサを用い、このトルクセンサにより計測されたトルクτを上記外力として検知してもよい。また、外力検知部102は、上記のようにセンサを用いて外力を直接計測するのではなく、アーム2が有するモータの電流又はアーム2の関節角θの計測値から間接的に外力を算出する外力オブザーバを用いて上記外力を検知してもよい。
【0021】
従動制御演算部103は、位置姿勢計測部101による計測結果に基づいて、外力検知部102により検知された外力に従うアーム2の動き(従動制御指令値)を算出する。
【0022】
なお、位置姿勢計測部101、外力検知部102及び従動制御演算部103は、拘束が無い通常の直接教示で用いられる構成と同様であり、公知技術である。
【0023】
位置姿勢演算部104は、従動制御演算部103による算出結果に基づいて、アーム2の位置姿勢を算出する。
【0024】
なお図1では、従動制御演算部103が位置姿勢計測部101による計測結果を用い、位置姿勢演算部104が従動制御演算部103による算出結果を用いている。
しかしながら、これに限らず、従動制御演算部103は、位置姿勢計測部101による計測結果は用いず、外力検知部102により検知された外力に従うアーム2の動きを演算してもよい。この場合、位置姿勢演算部104は、位置姿勢計測部101による計測結果に基づいてアーム2の位置姿勢を算出する。
また、位置姿勢演算部104は、従動制御演算部103による算出結果及び位置姿勢計測部101による計測結果の両方を用いてアーム2の位置姿勢を算出してもよい。
【0025】
近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の位置姿勢が拘束目標に近づいているかを判定する。
【0026】
切換部106は、近接判定部105による判定結果に従い、拘束制御演算部108の動作を切換える。すなわち、切換部106は、近接判定部105によりアーム2の位置姿勢が拘束目標に近づいていると判定された場合には、拘束制御演算部108による処理を有効とする。一方、切換部106は、近接判定部105によりアーム2の位置姿勢が拘束目標に近づいていないと判定された場合には、拘束制御演算部108による処理を無効とする。
【0027】
また、切換部106は、拘束目標が複数存在する場合に、近接判定部105による判定結果に従い、目標値算出部107による動作(対象とする拘束目標)の切換えも実施してもよい。
【0028】
目標値算出部107は、拘束目標及び位置姿勢演算部104による算出結果に基づいて、拘束制御の目標値を算出する。なお、拘束制御の目標値とは、アーム2の目標位置及びアーム2の目標姿勢のうちの少なくとも一方を意味する。
【0029】
拘束制御演算部108は、位置姿勢演算部104による算出結果に基づいて、目標値算出部107により算出された目標値に移動するアーム2の動き(拘束制御指令値)を算出する。
【0030】
拘束制御制限部109は、拘束制御演算部108による算出結果を制限する。すなわち、拘束制御制限部109は、従動制御演算部103による算出結果が拘束制御演算部108による算出結果より優先されるよう拘束制御演算部108による算出結果に制限を課す。
なお図1では、拘束制御演算部108と拘束制御制限部109を別々の構成要素として示しているが、一体として実装することも可能である。
【0031】
合成部110は、従動制御演算部103による算出結果(従動制御指令値)と拘束制御制限部109による制限結果(拘束制御指令値)とを一つの指令値に合成する。
【0032】
駆動制御部111は、合成部110による合成結果に基づいて、アーム2を駆動する。
【0033】
実施の形態1に係る直接教示装置1では、通常の直接教示と拘束付きの直接教示の切換えを、操作者によるアーム2の操作により実行可能としている。この切換え方法は、以下に示す2つの仕組みによって実現される。
【0034】
1つ目は、直接教示装置1が、アーム2の位置姿勢が拘束目標に近づいた際に、自動的に通常の直接教示から拘束付きの直接教示に切換える仕組みである。例えば図2A,2Bに示すように、アーム2の位置を所定の平面506(拘束面503)に拘束させたい場合、直接教示装置1は、その平面506からアーム2の位置までの距離が例えば±50[mm]以下である場合には拘束付き直接教示を行い、それ以外の場合には通常の直接教示を行う。また、例えば図3A,3Bに示すように、アーム2の姿勢を真下方向507(拘束方向505)に拘束させたい場合、直接教示装置1は、アーム2の姿勢が真下方向507から例えば±0.2[rad]以下である場合には拘束付き直接教示を行い、それ以外の場合には通常の直接教示を行う。この仕組みにより、直接教示装置1は、操作者によるアーム2の操作のみで、通常の直接教示から拘束付き直接教示への切換えを実現できる。
【0035】
2つ目は、直接教示装置1が、拘束付き直接教示の際に拘束する力を敢えて抑制することで、操作者が拘束制御に打ち勝ってアーム2を操作可能とする仕組みである。例えば、直接教示装置1は、拘束制御における関節角速度θ(ドット)を例えば0.1[rad/s]以下に抑制し、操作者がそれ以上の外力を加えることでアーム2を操作可能とする。これにより、操作者は、アーム2を、拘束された状態から少しずつ逸脱させることができ、最終的には拘束付き直接教示の実行条件から外すことができる。一般的な従来の直接教示装置では、拘束付き直接教示においてアーム2を拘束面等に強く拘束することでアーム2の位置姿勢を保持している。それに対し、実施の形態1に係る直接教示装置1では、拘束する力を敢えて弱めることで、図2C,2D,3C,3Dに示すように、操作者によるアーム2の操作のみで、拘束付き直接教示から通常の直接教示への切換えを実現できる。
【0036】
以上の2つの仕組みを組合わせることにより、実施の形態1に係る直接教示装置1は、操作者がアーム2を操作している状態のまま、通常の直接教示と拘束付き直接教示とを切換え可能となる。以下、図1に示す直接教示装置1の動作例について、図4を参照しながら説明する。
【0037】
図1に示す直接教示装置1の動作例では、図4に示すように、まず、位置姿勢計測部101は、アーム2の位置姿勢に関するパラメータを計測する(ステップST401)。
【0038】
また、外力検知部102は、操作者によりアーム2に加えられた外力を検知する(ステップST402)。なお、外力検知部102により検知される外力は、センサの構造等によっては、操作者によりアーム2に加えられた外力だけではなく、重力に起因する成分が重畳されている場合がある。そこで、このような場合には、外力検知部102は、重力に起因する成分を算出し、検知した外力から当該重力に起因する成分を差し引くことで、外力成分のみを算出するとよい。これは重力補償と呼ばれる公知技術であり、例えば特許文献5等に開示されている。
【特許文献5】特開平01−066715号公報
【0039】
次いで、従動制御演算部103は、位置姿勢計測部101による計測結果に基づいて、外力検知部102により検知された外力に従うアーム2の動き(従動制御指令値)を算出する(ステップST403)。この際、従動制御演算部103は、まず、外力検知部102により検知された外力の向き及び大きさから、操作者がどのようにアーム2を動かそうとしているのかを判断する。そして、従動制御演算部103は、その判断結果及び位置姿勢計測部101により計測されたアーム2の位置姿勢に基づいて、アーム2の移動量(関節角θ等の差分)又は関節角速度θ(ドット)等を算出する。なお、従動制御演算部103による従動制御演算方法は、例えば特許文献2等で開示されており、様々な方式が開発されているためその詳細な記述は省略する。
【0040】
次いで、位置姿勢演算部104は、従動制御演算部103による算出結果に基づいて、アーム2の位置姿勢を算出する(ステップST404)。
ここで、直接教示装置1が位置拘束を行う場合、すなわち、直接教示装置1がアーム2の位置を所定の拘束面503又は拘束軸504に拘束する場合には、位置姿勢演算部104は少なくともアーム2の位置を算出する必要がある。
また、直接教示装置1が姿勢拘束を行う場合、すなわち、直接教示装置1がアーム2の姿勢を所定の拘束方向505に拘束する場合には、位置姿勢演算部104は少なくともアーム2の姿勢を算出する必要がある。
また、直接教示装置1が位置拘束及び姿勢拘束を行う場合、すなわち、直接教示装置1がアーム2の位置及び姿勢を所定の拘束面503又は拘束軸504且つ所定の拘束方向505に拘束する場合には、位置姿勢演算部104はアーム2の位置及び姿勢を算出する必要がある。
【0041】
また、位置姿勢演算部104は、従動制御演算部103による算出結果(最新の従動制御指令値)に基づいて上記算出を行うことが最も望ましい。これは、直接教示装置1が拘束付き直接教示を行う場合に、操作者がアーム2に対して与えた移動量のうち、拘束面503等から外れる成分を同時に相殺でき、拘束制御が最も有効に働くからである。
また、位置姿勢演算部104は、従動制御演算部103による算出結果を用いず、位置姿勢計測部101による計測結果に基づいて上記算出を行うことも可能である。一方、この場合には、拘束面503等から外れたことが計測されてから拘束制御が始まるので、拘束制御の効きが悪くなる。
また、位置姿勢演算部104は、従動制御演算部103による算出結果及び位置姿勢計測部101による計測結果の両方に基づいて上記算出を行ってもよい。
【0042】
なお、位置姿勢計測部101及び従動制御演算部103で関節角θのみが得られ、アーム2の位置姿勢を直接得られない場合、位置姿勢演算部104は、順運動学演算によってアーム2の位置姿勢を算出することが可能である。
【0043】
次いで、近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の位置姿勢が拘束目標に近づいているかを判定する(ステップST405)。
【0044】
例えば、拘束制御の目的がエンドエフェクタ201の先端のZ軸座標を0.1[m]の位置に固定することであり、また、接近判定のしきい値が0.05[m]であるとする。この場合、近接判定部105は、位置姿勢演算部104により算出されたエンドエフェクタ201の先端のZ軸座標が0.05〜0.15[m]の範囲内であればアーム2の位置は拘束目標に近づいていると判定し、それ以外の場合にはアーム2の位置は拘束目標に近づいていないと判定する。
【0045】
また、拘束制御の目的がエンドエフェクタ201の向き502(Z軸)を真下に向けることであり、また、接近判定のしきい値が0.1[rad]であるとする。この場合、近接判定部105は、図5に示すように、真上方向(基準方向)508とエンドエフェクタ201の向き502との成す角度Θが(π−0.1)[rad]以上、すなわち、真下方向507とエンドエフェクタ201の向き502との成す角度が0.1[rad]未満であれば、アーム2の姿勢は拘束目標に近づいていると判定し、それ以外の場合にはアーム2の姿勢は拘束目標に近づいていないと判定する。
【0046】
また、拘束面503が複数存在する場合等のように拘束目標が複数存在する場合、近接判定部105は、拘束目標毎に上記判定を行い、各拘束目標のうちの1つでもアーム2の位置姿勢が近づいていると判定した場合には、アーム2の位置姿勢は拘束目標に近づいていると判定する。この際、更に、近接判定部105は、アーム2と各拘束目標との間の距離を評価し、上記判定の結果とともにアーム2に最も近い拘束目標を示す情報を切換部106に通知してもよい。
【0047】
次いで、切換部106は、近接判定部105による判定結果に従い、拘束制御演算部108の動作を切換える(ステップST406)。すなわち、切換部106は、近接判定部105によりアーム2の位置姿勢が拘束目標に近づいていると判定された場合には、拘束制御演算部108による処理を有効とする。一方、切換部106は、近接判定部105によりアーム2の位置姿勢が拘束目標に近づいていないと判定された場合には、拘束制御演算部108による演算を停止するか又は算出結果がアーム2の制御に反映されないようにして、拘束制御演算部108による処理を無効とする。
【0048】
このように、直接教示装置1は、操作者によりアーム2の位置姿勢が拘束目標に近づけられると、自動的に拘束制御を開始する。また、直接教示装置1は、後述する拘束制御制限部109との組合わせにより、操作者によりアーム2の位置姿勢が拘束目標から遠ざけられると、自動的に拘束制御を終了する。これにより、直接教示装置1は、操作者によりアーム2が保持された状態で拘束制御のオンオフを切換えることができる。
なお、切換部106は、拘束目標が複数あって近接判定部105からアーム2に最も近い拘束目標を示す情報が通知された場合には、切換部106は、後述する目標値算出部107にその情報を出力して当該目標値算出部107の動作を切換えてもよい。
【0049】
次いで、目標値算出部107は、拘束目標及び位置姿勢演算部104による算出結果に基づいて、拘束制御の目標値を算出する(ステップST407)。
【0050】
例えば、拘束制御の目的がエンドエフェクタ201の先端のZ軸座標を0.1[m]の位置に固定することであり、位置姿勢演算部104により算出されたエンドエフェクタ201の先端の現在値が[X,Y,Z]であるとする。この場合、目標値算出部107は、拘束制御の目標値を[X,Y,0.1]とする。
【0051】
また、拘束制御の目的が、図6下段に示すようにエンドエフェクタ201の向き502のうちのZ軸を真下方向507とした上でX軸をロボット座標系のX軸に合わせることであるとする。この場合、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式(1)のようになる。なお、回転行列による姿勢の表現は、例えば非特許文献1等に開示されており、その詳細については省略する。
【非特許文献1】John J. Craig (三浦・下山訳):ロボティクス,共立出版(1991)
【0052】
なお、目標値算出部107は、拘束制御の目標値として、1つの目標値のみを算出してもよいし、複数の目標値を算出してその中から1つの目標値を選択してもよい。例えば、拘束制御の目的がエンドエフェクタ201の先端のZ軸座標を0.1[m]刻みで最も近い位置に固定することである場合、目標値算出部107は、拘束制御の目標値として[X,Y,0.1n](nは整数)の中で最も現在の位置に近いものを選択する。
【0053】
また、拘束制御の目的が図7に示すようにエンドエフェクタ201の向き502のうちのZ軸を真下方向507とし且つこのZ軸を回転軸としてπ/2刻み(90度刻み)に回転させた姿勢で固定することである場合、目標値算出部107は、拘束制御の目標値として最も現在の姿勢に近いものを選択する。このとき、拘束制御の目標値である目標姿勢を回転行列表現で表したものは次式(2)のようになる。
この式(6)は図7に示すように、nによって目標姿勢が4通りに変わるが、目標値算出部107はその中から現在の姿勢に最も近いものを1つ選択する。目標値算出部107は、現在の姿勢に近いかどうかは、現在の姿勢から各目標姿勢へと回転させた場合の回転角の大小等から判断できる。また、近接判定部105がアーム2に最も近い拘束目標を示す情報を通知している場合には、目標値算出部107はその情報から拘束制御の目標値を選択してもよい。
【0054】
次いで、拘束制御演算部108は、位置姿勢演算部104による算出結果に基づいて、目標値算出部107により算出された目標値に移動するアーム2の動き(拘束制御指令値)を算出する(ステップST408)。この際、拘束制御演算部108は、目標値算出部107により算出された目標値から、アーム2を現在の位置姿勢から目標値に移動させるのに必要なアーム2の移動量又は単位時間当たり移動量である関節角速度θ(ドット)等を算出する。また、拘束制御演算部108は、関節角速度θ(ドット)の代わりに1制御周期あたりの関節角変化量Δθを演算してもよい。
【0055】
次いで、拘束制御制限部109は、拘束制御演算部108による算出結果を制限する(ステップST409)。すなわち、拘束制御制限部109は、従動制御演算部103による算出結果が拘束制御演算部108による算出結果より優先されるよう拘束制御演算部108による算出結果に制限を課す。
この拘束制御制限部109の役割は、操作者が、直接教示装置1が拘束制御を行っている状態(拘束付き直接教示中の状態)から拘束制御を行っていない状態(通常の直接教示の状態)に切換えられるようにすることである。例えば、直接教示装置1が、拘束する力を強くして、拘束から外れるような操作者の操作を常時完全に打ち消していると、一旦拘束制御が開始すると拘束目標に常時拘束してしまい、拘束制御を終了できなくなる。そこで、拘束制御の終了を可能とするため、拘束制御制限部109により拘束制御の強さを制限して、操作者が強い外力を加えれば拘束目標から外すことができるようにする。
【0056】
例えば、直接教示装置1がアーム2の位置を所定の拘束面503に拘束している場合において、拘束制御制限部109は拘束制御の速度を例えば0.05[m/s]以下に制限する。これにより、操作者が0.05[m/s]より速くアーム2を移動すれば、アーム2は拘束面503から離れていき、最終的には近接判定部105が拘束面503に近づいていないと判定するまで移動させることができる。一方、操作者が0.05[m/s]より遅くアーム2を移動しようとすれば、アーム2は拘束面503に拘束された状態を継続する。
【0057】
また、直接教示装置1がアーム2の位置を所定の拘束面503に拘束している場合において、拘束制御制限部109は拘束制御の関節角速度θ(ドット)を例えば0.1[rad/s]以下に制限する。これにより、操作者が0.1[rad/s]より速くアーム2を移動すれば、アーム2は拘束面503から離れていき、最終的には近接判定部105が拘束面503に近づいていないと判定するまで移動させることができる。一方、操作者が0.1[rad/s]より遅くアーム2を移動しようすれば、アーム2は拘束面503に拘束された状態を継続する。
【0058】
上記の他、例えば、直接教示装置1がアーム2の姿勢を真下方向507に向けている場合についても、拘束制御制限部109が姿勢を変化させる速度又は関節角速度θ(ドット)に制限を加えることで、操作者は拘束状態を解除する操作が可能となる。
【0059】
なお、拘束制御制限部109が拘束制御の強さを制限する際、何れの場合においても、拘束制御によってアーム2を動かす向きを維持することが非常に好ましい。
例えば、拘束制御制限部109が関節角速度θ(ドット)を制限する場合であって、ある関節角速度θ(ドット)を0.5倍にしないと制限を超過する場合、全ての関節角速度θ(ドット)を0.5倍にすべきであり、制限を超えた関節角速度θ(ドット)だけを修正すべきではない。拘束制御制限部109が全ての関節角速度θ(ドット)に対して同じように制限しなかった場合、拘束制御演算部108が動かそうとする向きから変わってしまうため、拘束制御の精度が劣化する。
【0060】
次いで、合成部110は、従動制御演算部103から出力された従動制御指令値と拘束制御制限部109から出力された拘束制御指令値とを一つの指令値に合成する(ステップST410)。
【0061】
例えば、従動制御演算部103が従動制御指令値として関節角θを出力し、拘束制御制限部109が拘束制御指令値として関節角変化量Δθを出力した場合には、合成部110は、θ+Δθを関節角指令値として出力する。
また、例えば、従動制御演算部103が従動制御指令値として位置座標[x,y,z]及び姿勢T(回転行列表現)を出力し、拘束制御制限部109が拘束制御指令値として位置変化量[Δx,Δy,Δz]及び姿勢変化量ΔT(回転行列表現)を出力した場合には、合成部110は、[x+Δx,y+Δy,z+Δz]を位置指令値として、ΔT(回転行列表現)を姿勢指令値として出力する(回転行列表現された姿勢の合成は行列の積となる)。
また、例えば、従動制御演算部103が従動制御指令値としてトルクτを出力し、拘束制御制限部109が拘束制御指令値としてトルク変化量Δτを出力した場合には、合成部110は、τ+Δτをトルク指令値として出力する。
【0062】
次いで、駆動制御部111は、合成部110による合成結果に基づいて、アーム2を駆動する(ステップST411)。この駆動制御部111によるアーム2の駆動は、通常のアーム2の制御であるため、その詳細については省略する。
【0063】
以上のように、この実施の形態1によれば、直接教示装置1は、ロボットが有するアーム2に加えられた外力を検知する外力検知部102と、外力検知部102により検知された外力に従うアーム2の動きを算出する従動制御演算部103と、アーム2の位置又は姿勢のうちの少なくとも一方である位置姿勢を算出する位置姿勢演算部104と、位置姿勢演算部104による算出結果に基づいて、アーム2の位置姿勢が、当該アーム2の位置姿勢の拘束先である拘束目標に近づいているかを判定する近接判定部105と、拘束目標及び位置姿勢演算部104による算出結果に基づいて、拘束制御の目標値を算出する目標値算出部107と、位置姿勢演算部104による算出結果に基づいて、目標値算出部107により算出された目標値に移動するアーム2の動きを算出する拘束制御演算部108と、拘束制御演算部108による算出結果を制限する拘束制御制限部109と、近接判定部105により拘束目標に近づいていると判定された場合に拘束制御演算部108による算出結果を有効とし、近接判定部105により拘束目標に近づいていないと判定された場合に拘束制御演算部108による算出結果を無効とする切換部106と、従動制御演算部103による算出結果と拘束制御制限部109による制限結果とを合成する合成部110と、合成部110による合成結果に基づいてアーム2を駆動する駆動制御部111とを備えた。これにより、実施の形態1に係る直接教示装置1は、操作者がアーム2を操作している状態のまま通常の直接教示と拘束付き直接教示を切換え可能となる。
【0064】
実施の形態2.
実施の形態2では、アーム2が垂直多関節型ロボットアームであり、直接教示装置1による拘束方法が姿勢拘束のみに限定された場合を示す。なお、実施の形態2に係る直接教示装置1の構成例は、実施の形態1に係る直接教示装置1の構成例と同様であり、以下では図1に示す構成例を用いて説明を行う。
【0065】
なお、実施の形態2における位置姿勢演算部104は、アーム2の姿勢を算出する。
また、実施の形態2における近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の姿勢が拘束方向505に近接しているかを判定する。
【0066】
次に、実施の形態2に係る直接教示装置1の動作例について、図8を参照しながら説明する。
以下では、直接教示装置1は、エンドエフェクタ201の向き502(Z軸)を真下方向507又は真横方向509に拘束するような姿勢拘束を行うものとする。また、ロボット座標系の各軸の単位ベクトルをe,e,eで表す。また、アーム2は6軸であるとする。また、切換部106は、目標値算出部107及び拘束制御演算部108の動作を切換えるものとする。また、従動制御演算部103は従動制御指令値としてアーム2の各関節角θを算出し、拘束制御演算部108は拘束制御指令値としてアーム2の各関節角速度θ(ドット)を算出するものとする。
【0067】
実施の形態2に係る直接教示装置1の動作例では、図8に示すように、まず、位置姿勢計測部101は、アーム2の各関節角θを計測する(ステップST801)。例えば、位置姿勢計測部101は、アーム2が有するモータに取付けられたエンコーダ等を用いて、アーム2の関節毎に関節角θを計測する。
【0068】
次いで、外力検知部102は、操作者によりアーム2に加えられた外力を検知する(ステップST802)。この外力検知部102による処理は実施の形態1と同じである。なお、外力検知部102は重力補償を行うものとする。
【0069】
次いで、従動制御演算部103は、位置姿勢計測部101による計測結果に基づいて、外力検知部102により検知された外力に従うアーム2の各関節角θ(従動制御指令値)を算出する(ステップST803)。この従動制御演算部103の動作例について、図9を参照しながら説明する。
【0070】
従動制御演算部103は、例えば図9Aに示される方法により、外力検知部102により検知されたトルクτからアーム2の各関節角θを算出できる。
図9Aでは、従動制御演算部103は、まず、外力検知部102により検知されたトルクτにゲインKを掛けることで、アーム2の関節毎に目標とする関節角速度θ(ドット)を算出する。なお、ゲインKは、アーム2の関節毎に同一でもよいし異なっていてもよい。なお図9Aに示すように、従動制御演算部103は、目標とする関節角速度θ(ドット)にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部103は、目標とする関節角速度θ(ドット)に制御周期Tを掛けることで、アーム2の関節毎に、1制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部103は、アーム2の関節毎に、現時刻kの関節角θ(k)に関節角変化量Δθを加算して次の時刻k+1の関節角θ(k+1)を算出する。
【0071】
また、従動制御演算部103は、例えば図9Bに示される方法により、外力検知部102により検知された力Fからアーム2の各関節角θを演算できる。
図9Bの例では、従動制御演算部103は、まず、外力検知部102により検知された力FにゲインKを掛けることで、アーム2の位置及び姿勢毎に移動速度を算出する。なお、ゲインKは、アーム2の位置及び姿勢の各成分毎に同一でもよいし異なっていてもよい。
次に、従動制御演算部103は、移動速度にヤコビ行列Jの逆行列を掛けることで、アーム2の関節毎に目標とする関節角速度θ(ドット)を算出する。なお、従動制御演算部103は、ヤコビ行列Jの逆行列が存在しない場合は疑似逆行列を掛ける。なお図9Bに示すように、従動制御演算部103は、目標とする関節角速度θ(ドット)にリミッタを適用し、従動制御の速さを制限してもよい。
次に、従動制御演算部103は、目標とする関節角速度θ(ドット)に制御周期Tを掛けることで、アーム2の関節毎に、1制御周期あたりの関節角変化量Δθを算出する。
次に、従動制御演算部103は、アーム2の関節毎に、現時刻kの関節角θ(k)に関節角変化量Δθを加算して次の時刻k+1の関節角θ(k+1)を算出する。
【0072】
次いで、位置姿勢演算部104は、従動制御演算部103による算出結果に基づいて、アーム2の姿勢を算出する(ステップST804)。位置姿勢演算部104は、順運動学により上記算出を実行できる。以下、位置姿勢演算部104により算出されるアーム2の姿勢を回転行列によりTで表す。また、Tを列ベクトルや要素に分解したものを次式(3)のように表す。
【0073】
次いで、近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の姿勢が拘束方向505に近づいているかを判定する(ステップST805)。ここでは、拘束方向505は真下方向507又は真横方向509である。また、接近判定のしきい値は0.1[rad]であるとする。近接判定部105の動作例について、図10,11を参照しながら説明する。
【0074】
近接判定部105の動作例では、図10に示すように、近接判定部105は、まず、真上方向(基準方向)508とエンドエフェクタ201の向き502(Z軸)とが成す角度Θを算出する(ステップST1001)。すなわち、近接判定部105は、次式(4)のように、eとtとの内積から角度Θを算出する。
【0075】
そして、近接判定部105は、角度Θが(π/2−0.1)[rad]から(π/2+0.1)[rad]の範囲内であるかを判定する(ステップST1002)。
このステップST1002において、近接判定部105は、角度Θが(π/2−0.1)[rad]から(π/2+0.1)[rad]の範囲内であると判定した場合(例えば図11Aの場合)には、アーム2の姿勢が真横方向509に近づいていると判定する(ステップST1003)。
【0076】
一方、ステップST1002において、近接判定部105は、角度Θが(π/2−0.1)[rad]から(π/2+0.1)[rad]の範囲内ではないと判定した場合には、角度Θが(π−0.1)[rad]以上であるかを判定する(ステップST1004)。
このステップST1004において、近接判定部105は、角度Θが(π−0.1)[rad]以上であると判定した場合(例えば図11Bの場合)には、アーム2の姿勢が真下方向507に近づいていると判定する(ステップST1005)。
【0077】
一方、ステップST1004において、近接判定部105は、角度Θが(π−0.1)[rad]以上ではないと判定した場合(例えば図11Cの場合)には、アーム2の姿勢がどの拘束方向505にも近づいていないと判定する(ステップST1006)。
【0078】
次いで、切換部106は、近接判定部105による判定結果に従い、目標値算出部107及び拘束制御演算部108の動作を切換える(ステップST806)。
この際、切換部106は、近接判定部105によりアーム2の姿勢が真横方向509に近づいていると判定された場合には、目標値算出部107に目標値として真横方向509への目標姿勢を算出させ、拘束制御演算部108による処理を有効とする。
また、切換部106は、近接判定部105によりアーム2の姿勢が真下方向507に近づいていると判定された場合には、目標値算出部107に目標値として真下方向507への目標姿勢を算出させ、拘束制御演算部108による処理を有効とする。
また、切換部106は、近接判定部105によりアーム2の姿勢がどの拘束方向505にも近づいていないと判定された場合には、目標値算出部107による算出を不要し、拘束制御演算部108による処理を無効とする。
【0079】
次いで、目標値算出部107は、拘束目標及び位置姿勢演算部104による算出結果に基づいて、目標姿勢を算出する(ステップST807)。例えば、目標値算出部107は、真横方向509又は真下方向507であり、且つ、アーム2の現在の姿勢から最も小さい回転角度Θで到達可能な目標姿勢を算出する。目標値算出部107による目標姿勢の算出方法について以下に示す。
【0080】
まず、目標値算出部107は、対象となる目標姿勢毎に、eに対する目標角度Θを算出する。ここで、目標姿勢が真横方向509である場合には目標角度Θはπ/2であり、目標姿勢が真下方向507である場合には目標角度Θはπである。
【0081】
次に、目標値算出部107は、アーム2の姿勢に基づいて、回転角度Θが最も小さい回転軸510を算出する。図12に示すように、この回転軸510は、ロボット座標系のXY平面上にあり且つtと直交する軸であり、すなわち、e及びtの両方と直交する軸である。よって、目標値算出部107は、e及びtの両方と内積が0となるような単位ベクトルであるrを1つ求め、それを回転軸510とする。なお、目標値算出部107は、eとtとが平行の場合にはrを一意に求めることはできない。しかしながら、これはアーム2の姿勢が真上方向508又は真下方向507を向いている場合であり、目標値算出部107は、XY平面上の任意のベクトルを回転軸510として算出する。
【0082】
次に、目標値算出部107は、算出した目標角度Θ及び回転軸510に基づいて、目標姿勢として回転角度Θを算出する。ここで、エンドエフェクタ201の向き502が真上方向(基準方向)508から角度Θである場合、回転角度Θの大きさはΘ−Θとなるが、図13に示すように回転の向き(正負)を考慮する必要がある。これは、実施の形態2に係る直接教示装置1では、角度Θを求める際に内積を使っているため、エンドエフェクタ201の向き502の情報が失われているためである。回転の向きを考慮すると、回転角度Θは次式(5)のようになる。すなわち、回転の向きは、e×tとrとが同じ向きである場合には符号の反転は不要であり、e×tとrとが逆の向きである場合には符号の反転が必要である。
目標値算出部107では、回転軸510を表すr=[r,r,r(Tは転置を表す)と回転角度Θが得られれば、アーム2の姿勢から目標姿勢への回転が得られたことになるので、拘束目標が事実上得られたことになる。目標姿勢の具体的な表現が必要な場合は、目標値算出部107は、アーム2の姿勢に前述した回転を適用することで得られる。
【0083】
上記では、目標値算出部107が、エンドエフェクタ201の向き502(Z軸)が真下方向507又は真横方向509を向くような目標姿勢を算出する方法について述べた。目標値算出部107は、ここから更に回転を加えて所望の姿勢にして、それを目標姿勢とすることも可能である。
例えば、目標値算出部107は、アーム2が真下方向507を向いた状態からZ軸周りに回転を加えることで、図7に示すような姿勢を目標姿勢とすることも可能である。また、目標値算出部107は、図14に示すように、アーム2が真横方向509を向いた状態から更にZ軸周りに回転を加え、t又はtがロボット座標系のZ軸と平行となる姿勢を目標姿勢とすることも可能である。
【0084】
次いで、拘束制御演算部108は、位置姿勢演算部104による算出結果に基づいて、目標値算出部107により算出された目標姿勢に移動するアーム2の動き(拘束制御指令値)を算出する(ステップST808)。なお、アーム2を現在の姿勢から目標姿勢へと動かすような回転は、目標値算出部107が目標姿勢を算出する際に既に算出しているため、拘束制御演算部108は、そのようにアーム2を回転させる動きを算出する。ただし、拘束制御演算部108の出力は関節角速度θ(ドット)とする必要があるので、拘束制御演算部108は、目標値算出部107により算出された回転軸510であるrと回転角度Θの組から関節角速度θ(ドット)を算出する必要がある。以下にその計算の一例を示す。
【0085】
拘束制御演算部108は、まず、回転の角速度の大きさ|ω|を決める。例えば、制限を受けなければ1秒で目標姿勢に到達するように制御するのであれば、|ω|=θとする。
次に、拘束制御演算部108は、アーム2の現在の位置姿勢におけるヤコビ行列Jを算出する。ヤコビ行列Jについては次式(6)が成り立つものとする。
ここで、v,v,vはエンドエフェクタ201の先端のX,Y,Z軸方向の速度であり、ω,ω,ωはアーム2の姿勢がX軸,Y軸,Z軸について回転する角速度であり、θ(ドット)〜θ(ドット)は各関節角速度である。
【0086】
次に、拘束制御演算部108は、回転軸510であるrと回転速度の大きさ|ω|から姿勢に関する回転ベクトルω=[ω,ω,ωを算出する。
次に、拘束制御演算部108は、次式(7)を満たすようなアーム2の関節毎の関節角速度θ(ドット)〜θ(ドット)を算出する。なお、拘束制御演算部108は、ヤコビ行列Jに逆行列が存在しない場合は疑似逆行列等を用いて関節角速度θ(ドット)〜θ(ドット)を算出する。
【0087】
次いで、拘束制御制限部109は、拘束制御演算部108による算出結果を制限する(ステップST809)。
まず、拘束制御制限部109は、拘束制御演算部108により算出されたアーム2の関節毎の関節角速度θ(ドット)に対する上限θMAX(ドット)を決める。上限θMAX(ドット)は関節毎に同一でもよいし異なっていてもよい。また、従動制御演算部103でリミッタを適用している場合は、拘束制御制限部109は、そのリミッタの上限よりも上限θMAX(ドット)を小さくする必要がある。リミッタの上限よりも上限θMAX(ドット)を小さくしないと、操作者による操作が拘束制御よりも制限され、拘束付き直接教示から脱することができなくなる。現実的には、拘束制御制限部109は、上限θMAX(ドット)を、リミッタの上限に比して半分以下にすることが好ましい。その程度に制限すれば、操作者による操作で拘束を脱することは比較的容易になる。
【0088】
次に、拘束制御制限部109は、全ての関節角速度θ(ドット)が上限θMAX(ドット)を超えないように制限する。このとき、回転の方向を変えないように制限することが好ましい。そのような演算を行う手順の一例を以下に示す。なお、i番目の関節角速度θ(ドット)の上限をθi,MAX(ドット)で表す。
拘束制御制限部109は、まず、アーム2の関節毎に、関節角速度θ(ドット)の大きさと上限θi,MAX(ドット)の比cを|θ(ドット)|/θi,MAX(ドット)で算出する。
次に、拘束制御制限部109は、この比cの最大値cを算出する。
次に、拘束制御制限部109は、アーム2の関節毎に、関節角速度θ(ドット)を最大値cで割って制限後の関節角速度θ(ドット)を算出する。すなわち制限後の関節角速度θ(ドット)はθ(ドット)/cである。
【0089】
次いで、合成部110は、従動制御演算部103から出力された従動制御指令値(関節角θ)と拘束制御制限部109から出力された拘束制御指令値(関節角速度θ(ドット))とを合成する(ステップST810)。
例えば、従動制御演算部103が関節角θ=[θ,・・・,θを出力し、拘束制御制限部109が関節角速度θ(ドット)=[θ(ドット),・・・,θ(ドット)]を出力した場合、制御周期Tから、合成部110の出力である関節角指令値はθ+Tθ(ドット)となる。ただし、mは駆動制御部111が制御するアーム2の関節数である。
【0090】
次いで、駆動制御部111は、合成部110による合成結果(関節角指令値)に基づいて、アーム2を駆動する(ステップST811)。
【0091】
なお上記では一部、6軸(関節数が6個)のアーム2を前提として説明及び例示を行ったが、6軸以外のアーム2であっても同様に実施可能である。
【0092】
実施の形態3.
実施の形態2に係る直接教示装置1では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態3に係る直接教示装置1では、拘束方法が軸拘束のみに限定された場合を示す。軸拘束とは、アーム2の位置を所定の拘束軸504に拘束する位置拘束とアーム2の姿勢を所定の拘束方向505に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態3に係る直接教示装置1は、実施の形態2で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置1が、図15に示すようなロボット座標系のZ軸に平行な直線511への位置拘束及び真下方向507への姿勢拘束を行う場合を示すが、拘束軸504及び拘束方向505は任意に設定可能である。
なお、実施の形態3に係る直接教示装置1の構成例は、実施の形態2に係る直接教示装置1の構成例と同様であり、以下では図1に示す構成例を用いて説明を行う。
【0093】
なお、実施の形態3における位置姿勢演算部104は、アーム2の位置及び姿勢を算出する。
また、実施の形態3における近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の位置が拘束目標である拘束軸504に近づいているかを判定し且つアーム2の姿勢が拘束目標である拘束方向505に近づいているかを判定することで、アーム2の位置姿勢が拘束目標に近づいているかを判定する。
【0094】
図16は、実施の形態3に係る直接教示装置1の動作例を示すフローチャートである。図16に示す実施の形態3に係る直接教示装置1において、図8に示す実施の形態2に係る直接教示装置1と動作が異なるのは、位置姿勢演算部104、近接判定部105、目標値算出部107及び拘束制御演算部108であり、これらの構成の動作例についてのみ説明を行う。
【0095】
まず、位置姿勢演算部104は、従動制御演算部103による算出結果に基づいて、アーム2の位置及び姿勢を算出する(ステップST1604)。実施の形態3ではアーム2の位置及び姿勢の両方を拘束するため、位置姿勢演算部104は、アーム2の位置及び姿勢の両方を算出する必要がある。位置姿勢演算部104は、順運動学により上記算出を実行できる。
【0096】
また、近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の位置が拘束軸504に近づいているか及びアーム2の姿勢が拘束方向505に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム2の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する(ステップST1605)。例えば、近接判定部105は、アーム2の位置が拘束軸504に近づき且つアーム2の姿勢が拘束方向505に近づいていると判定した場合には、アーム2の位置姿勢は拘束目標に近づいていると判定する。近接判定部105による拘束方向505に対する判定については実施の形態2で説明しているので省略し、以下では、近接判定部105による拘束軸504に対する判定について説明する。
【0097】
この際、近接判定部105は、例えば図17に示すように、エンドエフェクタ201の位置501から拘束軸504に対して垂線512をおろし、その垂線512の長さが所定の値より小さければアーム2の位置が拘束軸504に近づいていると判定する。例えばアーム2の位置の座標を[x,y,z]で表し、直線511のX座標とY座標をそれぞれx,yで表す。この場合、垂線512の長さは次式(8)のようになる。
そして、近接判定部105は、式(8)に示す垂線512の長さが所定の値よりも小さければ、アーム2の位置が拘束軸504に近づいていると判定し、それ以外の場合にはアーム2の位置が拘束軸504には近づいていないと判定する。なお、近接判定部105は、拘束軸504がロボット座標系のZ軸に平行な場合以外でも、同様にして拘束軸504に対しておろした垂線の長さから接近判定が可能である。
【0098】
また、目標値算出部107は、実施の形態2で説明した目標姿勢の算出に加え、アーム2の位置を拘束軸504へと移動させる場合の目標位置の算出を行う(ステップST1607)。例えば、目標値算出部107は、エンドエフェクタ201の位置501から図17に示すように拘束軸504へと垂線512をおろした位置513を目標位置とする。ここで、目標値算出部107は、アーム2の位置をロボット座標系のZ軸に平行な拘束軸504に拘束させるのであれば、目標位置は[x,y,z]となる。
【0099】
また、拘束制御演算部108は、実施の形態2と同様に、ヤコビ行列Jを用いてアーム2の関節毎に関節角速度θ(ドット)を算出する(ステップST1608)。ただし、実施の形態3における拘束制御演算部108では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態2に対して以下のように変わる。
【0100】
まず、拘束制御演算部108は、回転の角速度の大きさ|ω|を決める。
次に、拘束制御演算部108は、アーム2の現在の位置姿勢におけるヤコビ行列Jを算出する。
次に、拘束制御演算部108は、回転軸510であるrと回転速度の大きさ|ω|から姿勢に関する回転ベクトルω=[ω,ω,ωを算出する。ここまでの動作は、実施の形態2と同様である。
【0101】
次に、拘束制御演算部108は、アーム2の位置を拘束軸504へと移動させる速さ|v|を決める。例えば、制限を受けなければ1秒で目標位置に到達するように制御するのであれば、拘束制御演算部108は、|v|=√{(x−x+(y−y}とする。また、拘束制御演算部108は、ある定数vを決めて|v|=vとしてもよい。
次に、拘束制御演算部108は、アーム2の位置に関する速度ベクトルv=[v,v,0]を算出する。拘束制御演算部108は、ロボット座標系のZ軸に平行な拘束軸504に拘束させる場合はv=|v|[x−x,y−y,0]/√{(x−x+(y−y}より算出可能である。
次に、拘束制御演算部108は、アーム2の関節毎に関節角速度θ(ドット)を次式(9)より算出する。
【0102】
その他の動作は実施の形態2と同じになる。
なお、実施の形態3に係る直接教示装置1において、姿勢拘束に関する演算を省略した場合には、アーム2の姿勢に関する拘束は行われず、アーム2の位置のみを指定された軸に拘束する制御となる。
【0103】
実施の形態4.
実施の形態2に係る直接教示装置1では、拘束方法が姿勢拘束のみに限定された場合を示した。それに対し、実施の形態4に係る直接教示装置1では、拘束方法が面拘束のみに限定された場合を示す。面拘束とは、アーム2の位置を所定の拘束面503に拘束する位置拘束とアーム2の姿勢を所定の拘束方向505に拘束する姿勢拘束とを同時に行うことを意味する。すなわち、実施の形態4に係る直接教示装置1は、実施の形態2で示した姿勢拘束に加え、位置拘束も行う。以下では、直接教示装置1が、図18に示すようなZ軸座標がX軸座標及びY軸座標の関数z=f(x,y)で表される曲面513への位置拘束及び真下方向507への姿勢拘束を行う場合を示すが、拘束面503及び拘束方向505は任意に設定可能であり、例えば拘束面503が関数x=f(y,z)又はy=f(z,x)で表される場合等についても同様の方法で実施可能である。
なお、実施の形態4に係る直接教示装置1の構成例は、実施の形態2に係る直接教示装置1の構成例と同様であり、以下では図1に示す構成例を用いて説明を行う。
【0104】
なお、実施の形態4における位置姿勢演算部104は、アーム2の位置及び姿勢を算出する。
また、実施の形態4における近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の位置が拘束目標である拘束面503に近づいているかを判定し且つアーム2の姿勢が拘束目標である拘束方向505に近づいているかを判定することで、アーム2の位置姿勢が拘束目標に近づいているかを判定する。
【0105】
図19は、実施の形態4に係る直接教示装置1の動作例を示すフローチャートである。図19に示す実施の形態4に係る直接教示装置1において、図16に示す実施の形態3に係る直接教示装置1と動作が異なるのは、近接判定部105、目標値算出部107及び拘束制御演算部108であり、これらの構成の動作例についてのみ説明を行う。
【0106】
まず、近接判定部105は、位置姿勢演算部104による算出結果に基づいて、アーム2の位置が拘束面503に近づいているか及びアーム2の姿勢が拘束方向505に近づいているかをそれぞれ判定し、それらの判定結果を基にアーム2の位置姿勢が拘束目標に近づいているかどうかを総合的に判定する(ステップST1905)。例えば、近接判定部105は、アーム2の位置が拘束面503に近づき且つアーム2の姿勢が拘束方向505に近づいていると判定した場合には、アーム2の位置姿勢は拘束目標に近づいていると判定する。近接判定部105による拘束方向505に対する判定については実施の形態2で説明しているので省略し、以下では、近接判定部105による拘束面503に対する判定について説明する。
【0107】
この際、近接判定部105は、エンドエフェクタ201の位置501から拘束面503に対してロボット座標系のZ軸に平行な直線をおろし、その直線の長さが所定の値より小さければアーム2の位置が拘束面503に近づいていると判定する。例えばアーム2の位置の座標が[x,y,z]である場合、上記直線の長さは{z−f(x,y)}となる。そして、近接判定部105は、この長さが所定の値よりも小さければ、アーム2の位置が拘束面503に近づいていると判定し、それ以外の場合にはアーム2の位置が拘束面503には近づいていないと判定する。
【0108】
また、目標値算出部107は、実施の形態2で説明した目標姿勢の算出に加え、アーム2の位置を拘束面503へと移動させる場合の目標位置の算出を行う(ステップST1907)。例えば、目標値算出部107は、アーム2の位置の座標が[x,y,z]である場合、Z軸座標だけを曲面上の座標に置き換えた[x,y,f(x,y)]を目標位置とする。
【0109】
また、拘束制御演算部108は、実施の形態2と同様に、ヤコビ行列Jを用いてアーム2の関節毎に関節角速度θ(ドット)を算出する(ステップST1908)。ただし、実施の形態4における拘束制御演算部108では、姿勢拘束に加え位置拘束も行うので、演算手順が実施の形態2に対して以下のように変わる。
【0110】
まず、拘束制御演算部108は、回転の角速度の大きさ|ω|を決める。
次に、拘束制御演算部108は、アーム2の現在の位置姿勢におけるヤコビ行列Jを算出する。
次に、拘束制御演算部108は、回転軸510であるrと回転速度の大きさ|ω|から姿勢に関する回転ベクトルω=[ω,ω,ωを算出する。ここまでの動作は、実施の形態2と同様である。
【0111】
次に、拘束制御演算部108は、アーム2の位置を拘束軸504へと移動させる速さ|v|を決める。例えば、制限を受けなければ1秒で目標位置に到達するように制御するのであれば、拘束制御演算部108は、|v|=|z−f(x,y)|とする。また、拘束制御演算部108は、速さ|v|をある定数値にしてもよい。
次に、拘束制御演算部108は、アーム2の位置に関する速度ベクトルv=[0,0,vをv=[0,0,|v|]より算出する。
次に、拘束制御演算部108は、アーム2の関節毎に関節角速度θ(ドット)を次式(10)より算出する。
【0112】
その他の動作は実施の形態2,3と同じになる。
なお、実施の形態4に係る直接教示装置1において、姿勢拘束に関する演算を省略した場合には、アーム2の姿勢に関する拘束は行われず、アーム2の位置のみを指定された面に拘束する制御となる。また、実施の形態4に係る直接教示装置1において、アーム2の位置から拘束面503へ垂線を下した方向を目標姿勢とした場合には、エンドエフェクタ201が拘束面503に対して常に垂直となるような姿勢拘束になる。
【0113】
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【0114】
1 直接教示装置
2 アーム
101 位置姿勢計測部
102 外力検知部
103 従動制御演算部
104 位置姿勢演算部
105 近接判定部
106 切換部
107 目標値算出部
108 拘束制御演算部
109 拘束制御制限部
110 合成部
111 駆動制御部
201 エンドエフェクタ
図1
図2
図3
図4
図5
図6
図7
図8
図9
図10
図11
図12
図13
図14
図15
図16
図17
図18
図19
図20