知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024041086
(43)【公開日】2024-03-27
(54)【発明の名称】ロボット制御装置及びロボット制御方法
(51)【国際特許分類】
B25J 9/22 20060101AFI20240319BHJP
G05B 19/416 20060101ALI20240319BHJP
【ＦＩ】
B25J9/22 Z
G05B19/416 Q
【審査請求】未請求
【請求項の数】9
【出願形態】ＯＬ
(21)【出願番号】P 2022145708
(22)【出願日】2022-09-14
(71)【出願人】
【識別番号】000006666
【氏名又は名称】アズビル株式会社
(74)【代理人】
【識別番号】110003166
【氏名又は名称】弁理士法人山王内外特許事務所
(72)【発明者】
【氏名】杉本 広大
(72)【発明者】
【氏名】田原 鉄也
(72)【発明者】
【氏名】大川 陽一
【テーマコード（参考）】
3C269
3C707
【Ｆターム（参考）】
3C269AB33
3C269BB11
3C269EF02
3C269MN29
3C269PP15
3C269SA02
3C707BS12
3C707HS27
3C707KS21
3C707KS33
3C707KX10
3C707LS02
3C707LU01
3C707LU03
3C707LU07
(57)【要約】 （修正有）
【課題】ロボットを手動操作する際の移動速度が大きくなりすぎることを抑制するロボット制御装置及びロボット制御方法を提供する。
【解決手段】アームに加えられた外力に関する情報を取得する外力情報取得部と、アームの移動速度に関する情報を取得する速度情報取得部と、外力情報取得部が取得した情報に基づいて、アームに加えられた外力に従ってアームを移動させるための駆動源の駆動力を算出する従動駆動力算出部と、速度情報取得部が取得した情報に基づいて、アームの移動速度が予め設定された閾値以下になるように、駆動源の駆動力の減少量を算出する減速駆動力算出部と、アームに加えられた外力に関する情報と、従動駆動力算出部の算出結果と、減速駆動力算出部の算出結果と、に基づいて、駆動源の駆動力を制御する駆動制御部と、を備え、減速駆動力算出部は、駆動源の駆動力の減少量が連続的に変化するように、駆動源の駆動力の減少量を算出する。
【選択図】図２
【解決手段】アームに加えられた外力に関する情報を取得する外力情報取得部と、アームの移動速度に関する情報を取得する速度情報取得部と、外力情報取得部が取得した情報に基づいて、アームに加えられた外力に従ってアームを移動させるための駆動源の駆動力を算出する従動駆動力算出部と、速度情報取得部が取得した情報に基づいて、アームの移動速度が予め設定された閾値以下になるように、駆動源の駆動力の減少量を算出する減速駆動力算出部と、アームに加えられた外力に関する情報と、従動駆動力算出部の算出結果と、減速駆動力算出部の算出結果と、に基づいて、駆動源の駆動力を制御する駆動制御部と、を備え、減速駆動力算出部は、駆動源の駆動力の減少量が連続的に変化するように、駆動源の駆動力の減少量を算出する。
【選択図】図２
【特許請求の範囲】
【請求項1】
駆動源からの駆動力によって移動するアームを有するロボットの、前記アームに加えられた外力に関する情報を取得する外力情報取得部と、
前記アームの移動速度に関する情報を取得する速度情報取得部と、
前記外力情報取得部が取得した情報に基づいて、前記アームに加えられた外力に従って前記アームを移動させるための前記駆動源の駆動力を算出する従動駆動力算出部と、
前記速度情報取得部が取得した情報に基づいて、前記アームの移動速度が予め設定された閾値以下になるように、前記駆動源の駆動力の減少量を算出する減速駆動力算出部と、
前記従動駆動力算出部の算出結果と、前記減速駆動力算出部の算出結果と、に基づいて、前記駆動源の駆動力を制御する駆動制御部と、を備え、
前記減速駆動力算出部は、前記駆動源の駆動力の減少量が連続的に変化するように、前記駆動源の駆動力の減少量を算出する
ことを特徴とするロボット制御装置。
前記アームの移動速度に関する情報を取得する速度情報取得部と、
前記外力情報取得部が取得した情報に基づいて、前記アームに加えられた外力に従って前記アームを移動させるための前記駆動源の駆動力を算出する従動駆動力算出部と、
前記速度情報取得部が取得した情報に基づいて、前記アームの移動速度が予め設定された閾値以下になるように、前記駆動源の駆動力の減少量を算出する減速駆動力算出部と、
前記従動駆動力算出部の算出結果と、前記減速駆動力算出部の算出結果と、に基づいて、前記駆動源の駆動力を制御する駆動制御部と、を備え、
前記減速駆動力算出部は、前記駆動源の駆動力の減少量が連続的に変化するように、前記駆動源の駆動力の減少量を算出する
ことを特徴とするロボット制御装置。
【請求項2】
前記駆動制御部は、前記従動駆動力算出部が算出した前記駆動源の駆動力と、前記減速駆動力算出部が算出した前記駆動源の駆動力の減少量と、を合算した値を算出する合算部を備え、
前記駆動制御部は、前記合算部が算出した値と、に基づいて、前記駆動源の駆動力を制御する
ことを特徴とする請求項１記載のロボット制御装置。
前記駆動制御部は、前記合算部が算出した値と、に基づいて、前記駆動源の駆動力を制御する
ことを特徴とする請求項１記載のロボット制御装置。
【請求項3】
前記減速駆動力算出部は、前記アームの移動速度が前記閾値を超えた場合、前記アームが停止または前記閾値以下の移動速度で移動するように、前記駆動源の駆動力の減少量を算出する
ことを特徴とする請求項１記載のロボット制御装置。
ことを特徴とする請求項１記載のロボット制御装置。
【請求項4】
前記速度情報取得部は、前記アームの移動速度に関する情報として、前記アームの所定部位の並進速度及び回転速度の少なくとも一方に関する情報を取得し、
前記減速駆動力算出部は、前記所定部位の並進速度及び回転速度の少なくとも一方が前記閾値以下となるように、前記駆動源の駆動力の減少量を算出する
ことを特徴とする請求項１記載のロボット制御装置。
前記減速駆動力算出部は、前記所定部位の並進速度及び回転速度の少なくとも一方が前記閾値以下となるように、前記駆動源の駆動力の減少量を算出する
ことを特徴とする請求項１記載のロボット制御装置。
【請求項5】
前記減速駆動力算出部は、前記アームの移動速度を、所定の目標値に追従させるフィードバック制御によって前記駆動源の駆動力の減少量を算出する
ことを特徴とする請求項１記載のロボット制御装置。
ことを特徴とする請求項１記載のロボット制御装置。
【請求項6】
前記減速駆動力算出部は、前記アームの移動速度を、所定の目標値に向けて減少させるフィードフォワード制御によって前記駆動源の駆動力の減少量を算出する
ことを特徴とする請求項１記載のロボット制御装置。
ことを特徴とする請求項１記載のロボット制御装置。
【請求項7】
前記アームの移動速度が前記閾値を超える前に、前記減速駆動力算出部によって前記駆動源の駆動力を減少させるか否かを前記アームの移動速度の変化に基づいて判定する判定部を備えた
ことを特徴とする請求項１記載のロボット制御装置。
ことを特徴とする請求項１記載のロボット制御装置。
【請求項8】
前記閾値は、第１閾値であり、
前記減速駆動力算出部は、前記速度情報取得部が取得した情報に基づいて、前記アームに外力が加えられている状態における前記アームの移動速度が上昇している場合、前記第１閾値以下になるように、前記駆動源の駆動力の減少量を算出し、前記アームに外力が加えられている状態における前記アームの移動速度が減少している場合、予め設定された前記第１閾値よりも小さい第２閾値以下になるように、前記駆動源の駆動力の減少量を算出する
ことを特徴とする請求項１記載のロボット制御装置。
前記減速駆動力算出部は、前記速度情報取得部が取得した情報に基づいて、前記アームに外力が加えられている状態における前記アームの移動速度が上昇している場合、前記第１閾値以下になるように、前記駆動源の駆動力の減少量を算出し、前記アームに外力が加えられている状態における前記アームの移動速度が減少している場合、予め設定された前記第１閾値よりも小さい第２閾値以下になるように、前記駆動源の駆動力の減少量を算出する
ことを特徴とする請求項１記載のロボット制御装置。
【請求項9】
外力情報取得部と、速度情報取得部と、従動駆動力算出部と、減速駆動力算出部と、駆動制御部と、を備えた装置が行うロボット制御方法であって、
前記外力情報取得部が、駆動源からの駆動力によって移動するアームを有するロボットの、前記アームに加えられた外力に関する情報を取得するステップと、
前記速度情報取得部が、前記アームの移動速度に関する情報を取得するステップと、
前記従動駆動力算出部が、前記外力情報取得部が取得した情報に基づいて、前記アームに加えられた外力に従って前記アームを移動させるための前記駆動源の駆動力を算出するステップと、
前記減速駆動力算出部が、前記速度情報取得部が取得した情報に基づいて、前記アームに外力が加えられている状態における前記アームの移動速度が予め設定された閾値以下になるように、前記駆動源の駆動力の減少量を算出するステップと、
前記駆動制御部が、前記アームに加えられた外力に関する情報と、前記従動駆動力算出部の算出結果と、前記減速駆動力算出部の算出結果と、に基づいて、前記駆動源の駆動力を制御するステップと、を備え、
前記減速駆動力算出部は、前記駆動源の駆動力の減少量が連続的に変化するように、前記駆動源の駆動力の減少量を算出する
ことを特徴とするロボット制御方法。
前記外力情報取得部が、駆動源からの駆動力によって移動するアームを有するロボットの、前記アームに加えられた外力に関する情報を取得するステップと、
前記速度情報取得部が、前記アームの移動速度に関する情報を取得するステップと、
前記従動駆動力算出部が、前記外力情報取得部が取得した情報に基づいて、前記アームに加えられた外力に従って前記アームを移動させるための前記駆動源の駆動力を算出するステップと、
前記減速駆動力算出部が、前記速度情報取得部が取得した情報に基づいて、前記アームに外力が加えられている状態における前記アームの移動速度が予め設定された閾値以下になるように、前記駆動源の駆動力の減少量を算出するステップと、
前記駆動制御部が、前記アームに加えられた外力に関する情報と、前記従動駆動力算出部の算出結果と、前記減速駆動力算出部の算出結果と、に基づいて、前記駆動源の駆動力を制御するステップと、を備え、
前記減速駆動力算出部は、前記駆動源の駆動力の減少量が連続的に変化するように、前記駆動源の駆動力の減少量を算出する
ことを特徴とするロボット制御方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、ロボット制御装置及びロボット制御方法に関する。
【背景技術】
【0002】
従来、操作者がロボットのアームに加えた外力に従ってアームを駆動することで、ロボットの直接教示を可能とするロボット制御装置が開示されている（特許文献１参照）。このロボット制御装置は、アームの速度の検知結果に基づいてアームの速度制御を行うことにより、操作者によってアームが操作されていない状態であることを判定した場合に、アームを短時間で静止させるように構成されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２０２０－２０３３３０号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかしながら、特許文献１に開示されているロボット制御装置は、操作の有無によらずロボットの所定部位の速度の大きさを制限することはできない。なぜなら、ロボットの所定部位速度の大きさが制限値を上回ったとき、あるいはその可能性があるとき、それらを制限値以下まで減速させる制御を実施しないためである。
【0005】
本開示は、上記課題を解決するものであって、ロボットを手動操作する際の移動速度が大きくなりすぎることを抑制するロボット制御装置及びロボット制御方法を提供することを目的としている。
【課題を解決するための手段】
【0006】
本開示に係るロボット制御装置は、駆動源からの駆動力によって移動するアームを有するロボットの、アームに加えられた外力に関する情報を取得する外力情報取得部と、アームの移動速度に関する情報を取得する速度情報取得部と、外力情報取得部が取得した情報に基づいて、アームに加えられた外力に従ってアームを移動させるための駆動源の駆動力を算出する従動駆動力算出部と、速度情報取得部が取得した情報に基づいて、アームの移動速度が予め設定された閾値以下になるように、駆動源の駆動力の減少量を算出する減速駆動力算出部と、従動駆動力算出部の算出結果と、減速駆動力算出部の算出結果と、に基づいて、駆動源の駆動力を制御する駆動制御部と、を備え、減速駆動力算出部は、駆動源の駆動力の減少量が連続的に変化するように、駆動源の駆動力の減少量を算出することを特徴とする。
【発明の効果】
【0007】
本開示によれば、上記のように構成したので、ロボットのアームの移動速度が予め設定された閾値以下となるように駆動源の駆動力を制限することによって、ロボットを手動操作する際の移動速度が大きくなりすぎることを抑制することが可能になる。
【図面の簡単な説明】
【0008】
【図1】実施の形態１に係るロボットの概略構成を示す斜視図。
【図2】実施の形態１に係るロボット制御システムの構成を示すブロック図。
【図3】実施の形態１に係るロボット制御装置のハードウェア構成の例を示す図。
【図4】実施の形態１に係るロボット制御装置のハードウェア構成の例を示す図。
【図5】実施の形態１に係る駆動制御部の構成を示すブロック図。
【図6】実施の形態１に係る減速制御部の構成を示すブロック図。
【図7】実施の形態１に係る減速制御部が行う処理を示すフローチャート。
【図8】実施の形態１に係る判定部が複数の速度制限値に基づいて行う処理を説明する概念図。
【図9】実施の形態１に係る減速制御部が行う処理を説明する表。
【図10】実施の形態１に係る変形例としての駆動制御部の構成を示すブロック図。
【図11】実施の形態２に係る減速制御部の構成を示すブロック図。
【図12】実施の形態２に係る減速制御指令値演算部が算出する並進速度指令値の変化を示すグラフ。
【図13】実施の形態３に係る減速制御部が行う処理を説明する表。
【図14】実施の形態３に係る変形例としての減速制御指令値演算部が算出する並進速度指令値の変化を示すグラフ。
【発明を実施するための形態】
【0009】
以下、本開示に係る実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
まず、図１を参照して、実施の形態１に係るロボットＲ１の概略構成について説明する。図１は、実施の形態１に係るロボットＲ１を示す斜視図である。図１に示すように、ロボットＲ１は、互いに相対移動可能な複数のリンクを有するアームＡ１を備えている。アームＡ１の複数のリンク間には、それぞれ隣接するリンク間を接続する関節が形成されている。具体的には、ロボットＲ１は、リンクＬ１と、リンクＬ１に対して回動可能に支持されているリンクＬ２と、リンクＬ２に対して回動可能に支持されているリンクＬ３と、リンクＬ３に対して回動可能に支持されているリンクＬ４と、リンクＬ４に対して回動可能に支持されているリンクＬ５と、リンクＬ５に対して回動可能に支持されているリンクＬ６と、リンクＬ６に対して回動可能に支持されているリンクとしてのエンドエフェクタＥ１と、を備えている。
実施の形態１．
まず、図１を参照して、実施の形態１に係るロボットＲ１の概略構成について説明する。図１は、実施の形態１に係るロボットＲ１を示す斜視図である。図１に示すように、ロボットＲ１は、互いに相対移動可能な複数のリンクを有するアームＡ１を備えている。アームＡ１の複数のリンク間には、それぞれ隣接するリンク間を接続する関節が形成されている。具体的には、ロボットＲ１は、リンクＬ１と、リンクＬ１に対して回動可能に支持されているリンクＬ２と、リンクＬ２に対して回動可能に支持されているリンクＬ３と、リンクＬ３に対して回動可能に支持されているリンクＬ４と、リンクＬ４に対して回動可能に支持されているリンクＬ５と、リンクＬ５に対して回動可能に支持されているリンクＬ６と、リンクＬ６に対して回動可能に支持されているリンクとしてのエンドエフェクタＥ１と、を備えている。
【0010】
また、例えば、ロボットＲ１は、リンクＬ１とリンクＬ２との間に関節Ｊ１が、リンクＬ２とリンクＬ３との間に関節Ｊ２が、リンクＬ３とリンクＬ４との間に関節Ｊ３が、リンクＬ４とリンクＬ５との間に関節Ｊ４が、リンクＬ５とリンクＬ６との間に関節Ｊ５が、リンクＬ６とエンドエフェクタＥ１との間に関節Ｊ６が、それぞれ形成されて、６軸のロボットアームとして構成されている。
【0011】
次に、図２を参照して、実施の形態１に係るロボット制御システム１００について説明する。図２は、実施の形態１に係るロボット制御システムの構成を示すブロック図である。実施の形態１に係るロボット制御システム１００は、予めロボットＲ１に行わせる動作を教示（ティーチング）することで、ロボットＲ１に所定の作業をさせることが可能に構成されている。実施の形態１に係るロボット制御システム１００においては、予め操作者が直接ロボットＲ１を手動で操作することで動作を教示する直接教示が行われる。
【0012】
図２に示すように、ロボット制御システム１００は、ロボットＲ１と、複数のトルク検知部２０と、ロボット制御装置としての直接教示装置１０と、を備えている。ロボットＲ１は、ロボットＲ１の各関節に対応して配置されている複数のモータＭ１と、ロボットＲ１の各関節に対応して配置されている複数のエンコーダＥＮ１と、を有している。複数のモータＭ１は、電力の供給を受けて対応する各関節を駆動する。エンコーダＥＮ１は、各関節において隣接するリンク同士が互いに相対的に回動する際の回動角度に応じた信号を出力する。なお、複数のトルク検知部２０、複数のモータＭ１及び複数のエンコーダＥＮ１は互いに同様の構成であるため、それぞれ１つのトルク検知部２０、モータＭ１及び１つのエンコーダＥＮ１についてのみ図示し、他のトルク検知部２０、モータＭ１及び他のエンコーダＥＮ１については重複する説明及び図示を省略する。また、実施の形態１において、各関節において隣接するリンク同士が互いに相対的に回動する際の回動角度を、関節の回動角度ともいう。
【0013】
複数のトルク検知部２０は、それぞれロボットＲ１の各関節に対応して設けられており、各関節に加わるトルクを検知して、検知したトルクに応じた信号を出力する。例えば、トルク検知部２０は、モータＭ１の駆動軸に取付けられたトルクセンサである。トルク検知部２０が検知するトルクには、操作者がロボットＲ１を操作した際に加えられる操作トルク、並びに重力及びセンサ固有のオフセットに起因する静的な外乱が含まれる。
【0014】
直接教示装置１０は、直接教示によってロボットＲ１に動作を教示すると共に、直接教示を行う際のロボットＲ１のアームＡ１の移動速度を制御するための装置である。直接教示装置１０は、各トルク検知部２０、各エンコーダＥＮ１及び各モータＭ１と電気的に接続されており、各トルク検知部２０及び各エンコーダＥＮ１から取得した信号に基づいて各モータＭ１に電力を供給し、各モータＭ１を駆動する。直接教示装置１０は、メイン制御部４０と、複数の関節制御部３０と、を備えている。なお、複数の関節制御部３０は互いに同様の構成であるため、それぞれ１つの関節制御部３０についてのみ図示し、他の関節制御部３０については重複する説明及び図示を省略する。
【0015】
メイン制御部４０は、複数のトルク検知部２０及び複数のエンコーダＥＮ１から取得した信号に基づいて、複数のモータＭ１を駆動するための信号を複数の関節制御部３０に出力する。例えば、メイン制御部４０は、関節制御部３０を介してエンコーダＥＮ１と接続されており、関節制御部３０を介してエンコーダＥＮ１から情報を取得する。メイン制御部４０は、従動制御指令値演算部４１、速度演算部４３及び減速制御部４２を備えている。従動制御指令値演算部４１は、複数のトルク検知部２０から取得した信号に基づいて、各関節に加わるトルクに従う関節の動きを実現するための従動制御指令値を算出する。従動制御指令値は、トルク検知部２０によって検知されたトルクの現在値に含まれる重力及びセンサのオフセットに起因する静的な外乱の和を示すトルク値である。重力は、動力学モデルおよびエンコーダから取得した信号に基づいて算出される。また、従動制御指令値は、アームＡ１に加えられた外力に従ってアームＡ１を移動させるために算出される駆動源としてのモータＭ１の駆動力であるといえる。なお、実施の形態１において、トルク検知部２０によって検知されたトルクの現在値を、トルク現在値ともいう。また、実施の形態１において、従動制御指令値演算部４１は、外力情報取得部及び従動駆動力算出部を構成する。
【0016】
速度情報取得部としての速度演算部４３は、複数のエンコーダＥＮ１から取得した信号に基づいて、アームＡ１の所定部位（速度制限部位）の移動速度を算出することで、当該所定部位の移動速度の情報を取得する。例えば、速度演算部４３は、複数のエンコーダＥＮ１から取得した信号に基づいて、アームＡ１の手先であるエンドエフェクタＥ１の移動速度を算出する。なお、実施の形態１において、移動速度とは、対象となる所定部位が並進移動している際には並進速度（線速度）、回動している際には回転速度（周速度）を意味し、回動しながら並進移動している際には、並進速度と回転速度との少なくとも一方を意味する。また、速度演算部４３が算出する移動速度は、エンドエフェクタＥ１の移動速度に限らず、アームＡ１の他の部位の移動速度であってもよい。例えば、速度演算部４３は、以下の数式（１）及び（２）によって並進速度及び回転速度を算出する。
（並進速度の大きさ）＝√｛（ｘ方向並進速度ｖｘ）２＋（ｙ方向並進速度ｖｙ）２＋（ｚ方向並進速度ｖｚ）２｝ ・・・（１）
（回転速度の大きさ）＝√｛（ｘ方向回転速度ｗｘ）２＋（ｙ方向回転速度ｗｙ）２＋（ｚ方向回転速度ｗｚ）２｝ ・・・（２）
（並進速度の大きさ）＝√｛（ｘ方向並進速度ｖｘ）２＋（ｙ方向並進速度ｖｙ）２＋（ｚ方向並進速度ｖｚ）２｝ ・・・（１）
（回転速度の大きさ）＝√｛（ｘ方向回転速度ｗｘ）２＋（ｙ方向回転速度ｗｙ）２＋（ｚ方向回転速度ｗｚ）２｝ ・・・（２）
【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】
１０ ：直接教示装置（ロボット制御装置）
２０ ：トルク検知部
３０ ：関節制御部
３１，３２：駆動制御部
４０ ：メイン制御部
４１ ：従動制御指令値演算部（外力情報取得部、従動駆動力算出部）
４２ ：減速制御部（減速駆動力算出部）
４３ ：速度演算部（速度情報取得部）
４４ ：減速制御部
１００ ：ロボット制御システム
３１１ ：トルク制御部（従動駆動力算出部）
３１３ ：変換部
３２１ ：トルク制御部
３１２，３２２：合算部
４２１ ：判定部
４２２ ：速度制御部
４２２Ａ ：減速制御指令値演算部
４２２Ｂ ：速度ＰＩ制御器
４４２ ：速度制御部
４４２Ａ ：ＦＦ制御器
４４３ ：減速制御指令値演算部
Ａ１ ：アーム
Ｅ１ ：エンドエフェクタ（リンク）
ＥＮ１ ：エンコーダ
Ｊ１～Ｊ６，ＪＮ：関節
Ｌ１～Ｌ６：リンク
Ｍ１ ：モータ（駆動源）
Ｒ１ ：ロボット
Ｖ１ ：第１速度制限値（第１閾値）
Ｖ２ ：第２速度制限値（第２閾値）
２０ ：トルク検知部
３０ ：関節制御部
３１，３２：駆動制御部
４０ ：メイン制御部
４１ ：従動制御指令値演算部（外力情報取得部、従動駆動力算出部）
４２ ：減速制御部（減速駆動力算出部）
４３ ：速度演算部（速度情報取得部）
４４ ：減速制御部
１００ ：ロボット制御システム
３１１ ：トルク制御部（従動駆動力算出部）
３１３ ：変換部
３２１ ：トルク制御部
３１２，３２２：合算部
４２１ ：判定部
４２２ ：速度制御部
４２２Ａ ：減速制御指令値演算部
４２２Ｂ ：速度ＰＩ制御器
４４２ ：速度制御部
４４２Ａ ：ＦＦ制御器
４４３ ：減速制御指令値演算部
Ａ１ ：アーム
Ｅ１ ：エンドエフェクタ（リンク）
ＥＮ１ ：エンコーダ
Ｊ１～Ｊ６，ＪＮ：関節
Ｌ１～Ｌ６：リンク
Ｍ１ ：モータ（駆動源）
Ｒ１ ：ロボット
Ｖ１ ：第１速度制限値（第１閾値）
Ｖ２ ：第２速度制限値（第２閾値）
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】