知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024111358
(43)【公開日】2024-08-19
(54)【発明の名称】直接教示装置及び直接教示方法
(51)【国際特許分類】
B25J 9/22 20060101AFI20240809BHJP
B25J 13/08 20060101ALI20240809BHJP
【ＦＩ】
B25J9/22 Z
B25J13/08 Z
【審査請求】未請求
【請求項の数】9
【出願形態】ＯＬ
(21)【出願番号】P 2023015784
(22)【出願日】2023-02-06
(71)【出願人】
【識別番号】000006666
【氏名又は名称】アズビル株式会社
(74)【代理人】
【識別番号】110003166
【氏名又は名称】弁理士法人山王内外特許事務所
(72)【発明者】
【氏名】田原 鉄也
【テーマコード（参考）】
3C707
【Ｆターム（参考）】
3C707BS10
3C707HS27
3C707KS21
3C707KS35
3C707KS36
3C707KV01
3C707KV12
3C707KW05
3C707KX10
3C707LS03
3C707LU07
3C707LV19
3C707LV24
(57)【要約】
【課題】従来に対し、操作者がロボットアーム１における手先以外を操作しても手先の姿勢の挙動が分かりやすくなる。
【解決手段】複数の関節に加えられた外力トルクを計測する外力トルク計測部２０２と、外力トルクに基づいて、従動制御を行う従動制御部２０３と、操作位置を判定する操作位置判定部２０４と、複数の関節の関節角を計測する位置姿勢計測部２０５と、関節角に基づいて、手先の姿勢を演算する姿勢演算部２０６と、姿勢に基づいて、手先の姿勢制御を行う姿勢制御部２０７と、従動制御部２０３による制御結果及び姿勢制御部２０７による制御結果に基づいて、ロボットアーム１を駆動する駆動制御部２０８とを備え、姿勢制御部２０７は、操作位置がロボットアーム１における手先部分であると判定された場合に、姿勢制御を無効とする。
【選択図】図２
【解決手段】複数の関節に加えられた外力トルクを計測する外力トルク計測部２０２と、外力トルクに基づいて、従動制御を行う従動制御部２０３と、操作位置を判定する操作位置判定部２０４と、複数の関節の関節角を計測する位置姿勢計測部２０５と、関節角に基づいて、手先の姿勢を演算する姿勢演算部２０６と、姿勢に基づいて、手先の姿勢制御を行う姿勢制御部２０７と、従動制御部２０３による制御結果及び姿勢制御部２０７による制御結果に基づいて、ロボットアーム１を駆動する駆動制御部２０８とを備え、姿勢制御部２０７は、操作位置がロボットアーム１における手先部分であると判定された場合に、姿勢制御を無効とする。
【選択図】図２
【特許請求の範囲】
【請求項1】
ロボットアームが有する複数の関節に加えられた外力トルクを計測する外力トルク計測部と、
前記外力トルク計測部により計測された外力トルクに基づいて、前記ロボットアームの従動制御を行う従動制御部と、
前記ロボットアームの操作を行う操作者が操作を行っている当該ロボットアームにおける操作位置を判定する操作位置判定部と、
前記ロボットアームが有する複数の関節の関節角を計測する位置姿勢計測部と、
前記位置姿勢計測部により計測された関節角に基づいて、前記ロボットアームにおける手先の姿勢を演算する姿勢演算部と、
前記姿勢演算部により演算された姿勢に基づいて、前記ロボットアームにおける手先の姿勢制御を行う姿勢制御部と、
前記従動制御部による制御結果及び前記姿勢制御部による制御結果に基づいて、前記ロボットアームを駆動する駆動制御部とを備え、
前記姿勢制御部は、前記操作位置判定部により操作者の操作位置が前記ロボットアームにおける手先部分であると判定された場合に、姿勢制御を無効とする
ことを特徴とする直接教示装置。
前記外力トルク計測部により計測された外力トルクに基づいて、前記ロボットアームの従動制御を行う従動制御部と、
前記ロボットアームの操作を行う操作者が操作を行っている当該ロボットアームにおける操作位置を判定する操作位置判定部と、
前記ロボットアームが有する複数の関節の関節角を計測する位置姿勢計測部と、
前記位置姿勢計測部により計測された関節角に基づいて、前記ロボットアームにおける手先の姿勢を演算する姿勢演算部と、
前記姿勢演算部により演算された姿勢に基づいて、前記ロボットアームにおける手先の姿勢制御を行う姿勢制御部と、
前記従動制御部による制御結果及び前記姿勢制御部による制御結果に基づいて、前記ロボットアームを駆動する駆動制御部とを備え、
前記姿勢制御部は、前記操作位置判定部により操作者の操作位置が前記ロボットアームにおける手先部分であると判定された場合に、姿勢制御を無効とする
ことを特徴とする直接教示装置。
【請求項2】
前記姿勢制御部は、前記操作位置判定部により操作者が前記ロボットアームにおける手先部分以外を操作していると判定される直前での、当該ロボットアームにおける手先の姿勢に基づいて、当該ロボットアームにおける手先の姿勢制御を行う
ことを特徴とする請求項１記載の直接教示装置。
ことを特徴とする請求項１記載の直接教示装置。
【請求項3】
前記姿勢制御部は、前記ロボットアームにおける手先部分の関節のみを制御することで、当該ロボットアームにおける手先の姿勢制御を行う
ことを特徴とする請求項２記載の直接教示装置。
ことを特徴とする請求項２記載の直接教示装置。
【請求項4】
前記姿勢制御部は、所定の座標系における所定の回転軸回りの回転を抑制するように、前記ロボットアームにおける手先の姿勢制御を行う
ことを特徴とする請求項２記載の直接教示装置。
ことを特徴とする請求項２記載の直接教示装置。
【請求項5】
前記姿勢制御部は、所定の座標系における所定の回転軸回りの回転を抑制するように、前記ロボットアームにおける手先の姿勢制御を行う
ことを特徴とする請求項３記載の直接教示装置。
ことを特徴とする請求項３記載の直接教示装置。
【請求項6】
前記操作位置判定部は、前記ロボットアームにおける手先と手首とを接続する関節及び当該関節よりも手先側にある関節のうちの何れか１つ以上で、前記外力トルク計測部により計測された外力トルクの大きさが所定の値よりも大きい場合に、操作者の操作位置が当該ロボットアームにおける手先部分であると判定する
ことを特徴とする請求項１記載の直接教示装置。
ことを特徴とする請求項１記載の直接教示装置。
【請求項7】
前記操作位置判定部は、前記ロボットアームにおける手先部分に設けられて当該手先部分における人の接触有無を検知する接触センサによる検知結果に基づいて、操作者の操作位置が当該ロボットアームにおける手先部分であるか否かを判定する
ことを特徴とする請求項１記載の直接教示装置。
ことを特徴とする請求項１記載の直接教示装置。
【請求項8】
前記操作位置判定部は、前記ロボットアームに設けられて光を検知する光電センサによる検知結果に基づいて、操作者の操作位置が当該ロボットアームにおける手先部分であるか否かを判定する
ことを特徴とする請求項１記載の直接教示装置。
ことを特徴とする請求項１記載の直接教示装置。
【請求項9】
外力トルク計測部が、ロボットアームが有する複数の関節に加えられた外力トルクを計測するステップと、
従動制御部が、前記外力トルク計測部により計測された外力トルクに基づいて、前記ロボットアームの従動制御を行うステップと、
操作位置判定部が、前記ロボットアームの操作を行う操作者が操作を行っている当該ロボットアームにおける操作位置を判定するステップと、
位置姿勢計測部が、前記ロボットアームが有する複数の関節の関節角を計測するステップと、
姿勢演算部が、前記位置姿勢計測部により計測された関節角に基づいて、前記ロボットアームにおける手先の姿勢を演算するステップと、
姿勢制御部が、前記姿勢演算部により演算された姿勢に基づいて、前記ロボットアームにおける手先の姿勢制御を行うステップと、
駆動制御部が、前記従動制御部による制御結果及び前記姿勢制御部による制御結果に基づいて、前記ロボットアームを駆動するステップとを有し、
前記姿勢制御部は、前記操作位置判定部により操作者の操作位置が前記ロボットアームにおける手先部分であると判定された場合に、姿勢制御を無効とする
ことを特徴とする直接教示方法。
従動制御部が、前記外力トルク計測部により計測された外力トルクに基づいて、前記ロボットアームの従動制御を行うステップと、
操作位置判定部が、前記ロボットアームの操作を行う操作者が操作を行っている当該ロボットアームにおける操作位置を判定するステップと、
位置姿勢計測部が、前記ロボットアームが有する複数の関節の関節角を計測するステップと、
姿勢演算部が、前記位置姿勢計測部により計測された関節角に基づいて、前記ロボットアームにおける手先の姿勢を演算するステップと、
姿勢制御部が、前記姿勢演算部により演算された姿勢に基づいて、前記ロボットアームにおける手先の姿勢制御を行うステップと、
駆動制御部が、前記従動制御部による制御結果及び前記姿勢制御部による制御結果に基づいて、前記ロボットアームを駆動するステップとを有し、
前記姿勢制御部は、前記操作位置判定部により操作者の操作位置が前記ロボットアームにおける手先部分であると判定された場合に、姿勢制御を無効とする
ことを特徴とする直接教示方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、ロボットアームに対して直接教示を行う直接教示装置及び直接教示方法に関する。
【背景技術】
【0002】
産業用ロボットでは、ロボットアームに作業をさせるために、前もって教示と呼ばれる作業を行う。この教示は、ティーチングとも呼ばれる。このロボットアームに対して教示を行う方法の中に、直接教示と呼ばれる方法がある。この直接教示は、ダイレクト教示とも呼ばれる。
例えば特許文献１では、トルク検出手段を用いたロボットアームに対する直接教示方法が開示されている。
例えば特許文献１では、トルク検出手段を用いたロボットアームに対する直接教示方法が開示されている。
【0003】
【0004】
ロボットアーム１には、各関節に、トルクセンサ１０１が設けられている。
トルクセンサ１０１は、自身が設けられた関節に加えられたトルクを検知する。
トルクセンサ１０１は、自身が設けられた関節に加えられたトルクを検知する。
【0005】
ロボットコントローラ２ｂは、従動制御部２０１ｂ及び駆動制御部２０２ｂを有している。
従動制御部２０１ｂは、各トルクセンサ１０１により検知されたトルクに基づいて、操作者が加えた外力トルクの方向にロボットアーム１を動かす制御を行う。
駆動制御部２０２ｂは、従動制御部２０１ｂによる制御に従ってロボットアーム１を駆動する。
従動制御部２０１ｂは、各トルクセンサ１０１により検知されたトルクに基づいて、操作者が加えた外力トルクの方向にロボットアーム１を動かす制御を行う。
駆動制御部２０２ｂは、従動制御部２０１ｂによる制御に従ってロボットアーム１を駆動する。
【0006】
これにより、従来の直接教示方法では、操作者がロボットアーム１の任意の部分に力を加えてロボットアーム１を直接操作でき、所望の姿勢になったところで手先の位置等を記録することで、直接教示が実現できる。
【先行技術文献】
【特許文献】
【0007】
【特許文献1】特開平０５－２５００２９号公報
【発明の概要】
【発明が解決しようとする課題】
【0008】
このように、特許文献１に開示された従来の直接教示方法では、ロボットアーム１における手先だけでなく、人の手首又は腕にあたる部分に力を加えることでもロボットアーム１を操作できる。
しかしながら、操作者がロボットアーム１における手先以外の部分を操作した場合、ロボットアーム１における手先の姿勢が意図しない方向になる場合があり、直感的に分かりにくいという課題がある。
しかしながら、操作者がロボットアーム１における手先以外の部分を操作した場合、ロボットアーム１における手先の姿勢が意図しない方向になる場合があり、直感的に分かりにくいという課題がある。
【0009】
例として、図１４に示すロボットアーム１において、操作者が、手先が鉛直下向きの状態から、部位Ａ又は部位Ｂを上方に操作した場合で説明する。部位Ａはロボットアーム１における手先である。また、部位Ｂは人手首に相当する部分である。
【0010】
操作者が部位Ａを上方に操作した場合には、図１５Ａに示すように、ロボットアーム１における手先の姿勢は概ね鉛直下向きのままになる。なお、手先の姿勢は、手先の向きともいえる。
すなわち、操作者が部位Ａを上方に操作した場合には、当該操作によってロボットアーム１における関節Ｊａ，Ｊｂ，Ｊｃにトルクが発生して、当該関節Ｊａ，Ｊｂ，Ｊｃが全て動く。その結果、ロボットアーム１における手先の姿勢を維持したまま手先の位置を変えることができる。
すなわち、操作者が部位Ａを上方に操作した場合には、当該操作によってロボットアーム１における関節Ｊａ，Ｊｂ，Ｊｃにトルクが発生して、当該関節Ｊａ，Ｊｂ，Ｊｃが全て動く。その結果、ロボットアーム１における手先の姿勢を維持したまま手先の位置を変えることができる。
【0011】
一方、操作者が部位Ｂを上方に操作した場合には、図１５Ｂに示すように、ロボットアーム１における手先の姿勢が変わる。
すなわち、操作者が部位Ｂを上方に操作した場合には、当該操作によってロボットアーム１における関節Ｊａ，Ｊｂにはトルクが発生するが、人が力を加えた点である力点よりも手先側にある関節Ｊｃにはトルクが発生せず、当該関節Ｊｃは動かない。その結果、関節Ｊｃが動かないことによって、ロボットアーム１における手先の位置に連動して当該手先の姿勢が変わる。
すなわち、操作者が部位Ｂを上方に操作した場合には、当該操作によってロボットアーム１における関節Ｊａ，Ｊｂにはトルクが発生するが、人が力を加えた点である力点よりも手先側にある関節Ｊｃにはトルクが発生せず、当該関節Ｊｃは動かない。その結果、関節Ｊｃが動かないことによって、ロボットアーム１における手先の位置に連動して当該手先の姿勢が変わる。
【0012】
この違いは、ロボットアームの構造又は力学を理解している者には自明であるが、一般の使用者は必ずしも理解できるとは限らない。
特に初心者は、手首である部位Ｂを操作した場合でも手先である部位Ａを操作した場合と同じように動くことを期待する可能性がある。
また、ロボットアームの操作に慣れた者であっても、部位Ｂを操作してしまうとロボットアームにおける手先の姿勢が変わるため、その手先の姿勢を元に戻すために再度操作が必要であり、ひと手間増えることになる。
特に初心者は、手首である部位Ｂを操作した場合でも手先である部位Ａを操作した場合と同じように動くことを期待する可能性がある。
また、ロボットアームの操作に慣れた者であっても、部位Ｂを操作してしまうとロボットアームにおける手先の姿勢が変わるため、その手先の姿勢を元に戻すために再度操作が必要であり、ひと手間増えることになる。
【0013】
本開示は、上記のような課題を解決するためになされたもので、従来に対し、操作者がロボットアームにおける手先以外を操作しても手先の姿勢の挙動が分かりやすくなる直接教示装置を提供することを目的としている。
【課題を解決するための手段】
【0014】
本開示に係る直接教示装置は、ロボットアームが有する複数の関節に加えられた外力トルクを計測する外力トルク計測部と、外力トルク計測部により計測された外力トルクに基づいて、ロボットアームの従動制御を行う従動制御部と、ロボットアームの操作を行う操作者が操作を行っている当該ロボットアームにおける操作位置を判定する操作位置判定部と、ロボットアームが有する複数の関節の関節角を計測する位置姿勢計測部と、位置姿勢計測部により計測された関節角に基づいて、ロボットアームにおける手先の姿勢を演算する姿勢演算部と、姿勢演算部により演算された姿勢に基づいて、ロボットアームにおける手先の姿勢制御を行う姿勢制御部と、従動制御部による制御結果及び姿勢制御部による制御結果に基づいて、ロボットアームを駆動する駆動制御部とを備え、姿勢制御部は、操作位置判定部により操作者の操作位置がロボットアームにおける手先部分であると判定された場合に、姿勢制御を無効とすることを特徴とする。
【発明の効果】
【0015】
本開示によれば、上記のように構成したので、従来に対し、操作者がロボットアームにおける手先以外を操作しても手先の姿勢の挙動が分かりやすくなる。
【図面の簡単な説明】
【0016】
【図1】実施の形態１に係るロボットシステムの構成例を示す図である。
【図2】実施の形態１に係るロボットシステムの構成例を示す図である。
【図3】実施の形態１における姿勢制御部の構成例を示す図である。
【図4】実施の形態１に係るロボットコントローラの動作例を示すフローチャートである。
【図5】実施の形態１における操作位置判定部による判定例を示す図である。
【図6】実施の形態１に係るロボットシステムの別の構成例を示す図である。
【図7】実施の形態１に係るロボットシステムの別の構成例を示す図である。
【図8】実施の形態１における姿勢制御部の動作例を示すフローチャートである。
【図9】実施の形態３に係るロボットコントローラで解決する課題の背景を説明するための図である。
【図10】実施の形態３における姿勢制御部の構成例を示す図である。
【図13】従来のロボットシステムの構成例を示す図である。
【図14】従来のロボットシステムにおける課題を説明するための図である。
【発明を実施するための形態】
【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】
また、ロボットアーム１が垂直多関節型ロボットアームである場合には、通常、各関節に重力トルクが加えられるため、前述の方法で得られた各関節に加えられたトルクには重力トルクが含まれる。そのため、外力トルク計測部２０２は、操作者が加えた外力トルクのみを得るために、各関節に加えられたトルクから重力トルクを引いて外力トルクを推定する必要がある。このような技術は例えば特許文献２等で開示されており公知である。
なお、重力トルクは、前述の重力補償制御部２０１で算出されるため、外力トルク計測部２０２はその値を利用できる。
なお、重力トルクは、前述の重力補償制御部２０１で算出されるため、外力トルク計測部２０２はその値を利用できる。
【特許文献2】特開平０１－０６６７１５号公報
【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】
次いで、座標変換部２０７４は、ゲイン乗算器２０７３により得られた速度指令値にヤコビ行列の逆行列を乗算することで、当該速度指令値をロボットアーム１が有する各関節の速度指令値に変換する（ステップＳＴ８０３）。この座標変換部２０７４による変換後の速度指令値を示す信号は、速度制御部２０７６に出力される。
各関節の速度指令値は下式（４）のように表される。式（４）において、ωは各関節の速度指令値を示し、J（θ）はヤコビ行列を示し、θは各関節の角度の計測値を示し、Ｋｐはゲイン乗算器２０７３のゲインを示し、ｅは姿勢の偏差を示す。なお、ωは関節数×１のベクトルであり、J（θ）は６×関節数の行列であり、θは関節数×１のベクトルであり、Ｋｐは３×３の行列である。
各関節の速度指令値は下式（４）のように表される。式（４）において、ωは各関節の速度指令値を示し、J（θ）はヤコビ行列を示し、θは各関節の角度の計測値を示し、Ｋｐはゲイン乗算器２０７３のゲインを示し、ｅは姿勢の偏差を示す。なお、ωは関節数×１のベクトルであり、J（θ）は６×関節数の行列であり、θは関節数×１のベクトルであり、Ｋｐは３×３の行列である。
【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】
１ ロボットアーム
２ ロボットコントローラ（直接教示装置）
１１ａ～１１ｆ 関節
１１ｇ 手先
１０１ トルクセンサ
１０２ モータ
１０３ エンコーダ
１０４ 接触センサ
１０５ 光電センサ
２０１ 重力補償制御部
２０２ 外力トルク計測部
２０３ 従動制御部
２０４ 操作位置判定部
２０５ 位置姿勢計測部
２０６ 姿勢演算部
２０７ 姿勢制御部
２０８ 駆動制御部
１０５１ 投光部
１０５２ 受光部
２０７１ 姿勢記憶部
２０７２ 姿勢偏差演算器
２０７３ ゲイン乗算器
２０７４ 座標変換部
２０７５ 速度演算部
２０７６ 速度制御部
２０７７ 指令値出力制御部
２０７８ 修正姿勢偏差演算部
２ ロボットコントローラ（直接教示装置）
１１ａ～１１ｆ 関節
１１ｇ 手先
１０１ トルクセンサ
１０２ モータ
１０３ エンコーダ
１０４ 接触センサ
１０５ 光電センサ
２０１ 重力補償制御部
２０２ 外力トルク計測部
２０３ 従動制御部
２０４ 操作位置判定部
２０５ 位置姿勢計測部
２０６ 姿勢演算部
２０７ 姿勢制御部
２０８ 駆動制御部
１０５１ 投光部
１０５２ 受光部
２０７１ 姿勢記憶部
２０７２ 姿勢偏差演算器
２０７３ ゲイン乗算器
２０７４ 座標変換部
２０７５ 速度演算部
２０７６ 速度制御部
２０７７ 指令値出力制御部
２０７８ 修正姿勢偏差演算部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】