(57)【特許請求の範囲】
【請求項１】
複数の回転部、前記回転部を互いに回転可能に連結する関節、及び前記各回転部を駆動するサーボモータを含むアームと、
所定のロボット設置場所に設置され、前記アームの両端部のうち前記アームの先端部とは反対側を動作可能に支持する支持部と、を備えるロボットに適用され、ＰＴＰ制御により前記先端部の位置及び姿勢を制御するロボットの制御装置であって、
動作周期毎に前記各サーボモータを駆動させる角速度を算出する角速度算出手段と、
前記各回転部に設定された監視部の速度を算出する監視速度算出手段と、
前記監視速度算出手段により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記アームを構成する前記各関節のうち、前記最大の速度となる前記監視部よりも前記支持部側に設けられた全ての前記関節を速度低減対象軸として選択する選択手段と、
前記選択手段により選択された前記速度低減対象軸を回転させる前記サーボモータを低減対象モータとし、前記各監視部の速度が前記基準速度以下となるように、前記低減対象モータを駆動させる角速度を低下させる角速度低下手段と、
前記各サーボモータのうち前記低減対象モータ以外のモータを、前記角速度算出手段により算出された角速度で駆動させ、前記低減対象モータを、前記角速度低下手段により低下させられた角速度で駆動させる駆動手段と、を備えることを特徴とするロボットの制御装置。
【請求項２】
前記角速度算出手段により算出された前記各サーボモータの角速度に基づいて、前記各サーボモータの前記動作周期後の角度を算出する次周期角度算出手段と、
前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度のうち前記低減対象モータの動作周期後の角度と、前記低減対象モータの現在の角度とに基づいて、前記低減対象モータの動作周期あたりの角度変化量を算出する変化量算出手段と、をさらに備え、
前記角速度低下手段は、前記最大の速度が前記基準速度以下となるように、前記変化量算出手段により算出された前記低減対象モータの角度変化量を低下させ、
前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度のうち、前記低減対象モータの動作周期後の角度を、前記角速度低下手段によって低下させられた角度変化量と、前記低減対象モータの現在の角度とに基づいて算出された前記低減対象モータの動作周期後の角度で更新することにより、前記各サーボモータの動作周期後の角度を再設定する再設定手段をさらに備え、
前記駆動手段は、前記各サーボモータの現在の角度を、前記再設定手段により再設定された前記各サーボモータの角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる請求項１記載のロボットの制御装置。
【請求項３】
前記角速度低下手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記変化量算出手段により算出された角度変化量を低下させる請求項２に記載のロボットの制御装置。
【請求項４】
前記角速度低下手段は、前記角度変化量を前記比の値で割ることにより、前記角度変化量を低下させる請求項３に記載のロボットの制御装置。
【請求項５】
前記角速度低下手段は、前記最大の速度が前記基準速度以下となるように、前記角速度算出手段により算出された前記各サーボモータの角速度のうち、前記低減対象モータの角速度を低下させ、
前記角速度算出手段により算出された前記各サーボモータの角速度のうち、前記低減対象モータの角速度を、前記角速度低下手段によって低下させられた角速度で更新することにより、前記各サーボモータの角速度を再設定する再設定手段をさらに備え、
前記駆動手段は、前記再設定手段により再設定された前記各サーボモータの角速度により、前記各サーボモータを駆動させる請求項１に記載のロボットの制御装置。
【請求項６】
前記角速度低下手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記角速度算出手段により算出された前記低減対象モータの角速度を低下させる請求項５に記載のロボットの制御装置。
【請求項７】
前記角速度低下手段は、前記角速度算出手段により算出された前記低減対象モータの角速度を前記比の値で割ることにより、前記角速度を低下させる請求項６に記載のロボットの制御装置。
【請求項８】
前記各回転部を回転させる際に回転中心となる前記関節から最も離れた部分を、前記各回転部の前記監視部として設定する請求項１〜７のいずれか１項に記載のロボットの制御装置。
【請求項９】
前記角速度算出手段により算出された前記各サーボモータの角速度に基づいて、前記各サーボモータの前記動作周期後の角度を算出する次周期角度算出手段と、
前記各監視部の現在の位置を算出する現在監視位置算出手段と、
前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度に基づいて、前記各監視部の動作周期後の位置を算出する次周期監視位置算出手段と、をさらに備え、
前記監視速度算出手段は、前記現在監視位置算出手段により算出された前記各監視部の現在の位置と、前記次周期監視位置算出手段により算出された前記各監視部の動作周期後の位置とに基づいて、前記各監視部の速度を算出する請求項１〜８のいずれか１項に記載のロボットの制御装置。
【請求項１０】
前記監視速度算出手段は、前記各監視部の現在の位置と前記各監視部の動作周期後の位置との距離を前記動作周期で割ることにより前記各監視部の速度を算出する請求項９に記載のロボットの制御装置。
【請求項１１】
前記次周期監視位置算出手段は、前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度と、前記各回転部の大きさとに基づいて、前記各監視部の動作周期後の位置を算出する請求項９又は１０に記載のロボットの制御装置。
【請求項１２】
複数の回転部、前記回転部を互いに回転可能に連結する関節、及び前記各回転部を駆動するサーボモータを含むアームと、
所定のロボット設置場所に設置され、前記アームの両端部のうち前記アームの先端部とは反対側を動作可能に支持する支持部と、を備えるロボットに適用され、ＰＴＰ制御により前記先端部の位置及び姿勢を制御するロボットの制御方法であって、
動作周期毎に前記各サーボモータを駆動させる角速度を算出する角速度算出工程と、
前記各回転部に設定された監視部の速度を算出する監視速度算出工程と、
前記監視速度算出工程により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記アームを構成する前記各関節のうち、前記最大の速度となる前記監視部よりも前記支持部側に設けられた全ての前記関節を速度低減対象軸として選択する選択工程と、
前記選択工程により選択された前記速度低減対象軸を回転させる前記サーボモータを低減対象モータとし、前記各監視部の速度が前記基準速度以下となるように、前記低減対象モータを駆動させる角速度を低下させる角速度低下工程と、
前記各サーボモータのうち前記低減対象モータ以外のモータを、前記角速度算出工程により算出された角速度で駆動させ、前記低減対象モータを、前記角速度低下工程により低下させられた角速度で駆動させる駆動工程と、を備えることを特徴とするロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの制御装置及び制御方法に関する。
【背景技術】
【０００２】
従来、ロボットの手動操作時に、ロボットの制御点の移動速度が基準速度を超えた際に、移動速度が基準速度以下となるように動作目標位置を修正してロボットを動作させるものがある（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第３９９４４８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、ロボットの制御点として設定されるアームの先端部の移動速度を、基準速度以下となるように制御したとしても、アームの移動速度を十分に抑制することができない場合があることに本願発明者は着目した。
【０００５】
本発明は、こうした実情に鑑みてなされたものであり、アームの移動速度を十分に抑制することのできるロボットの制御装置及び制御方法を提供することを主たる目的とするものである。
【課題を解決するための手段】
【０００６】
第１の手段は、複数の回転部、前記回転部を互いに回転可能に連結する関節、及び前記各回転部を駆動するサーボモータを含むアームと、所定のロボット設置場所に設置され、前記アームの両端部のうち前記アームの先端部とは反対側を動作可能に支持する支持部と、を備えるロボットに適用され、ＰＴＰ制御により前記先端部の位置及び姿勢を制御するロボットの制御装置であって、動作周期毎に前記各サーボモータを駆動させる角速度を算出する角速度算出手段と、前記各回転部に設定された監視部の速度を算出する監視速度算出手段と、前記監視速度算出手段により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記アームにおいて、前記最大の速度となる前記監視部よりも前記支持部側に設けられた全ての前記関節を速度低減対象軸として選択する選択手段と、前記選択手段により選択された前記速度低減対象軸を回転させる前記サーボモータを低減対象モータとし、前記各監視部の速度が前記基準速度以下となるように、前記低減対象モータを駆動させる角速度を低下させる角速度低下手段と、前記各サーボモータのうち前記低減対象モータ以外のモータを、前記角速度算出手段により算出された角速度で駆動させ、前記低減対象モータを、前記角速度低下手段により低下させられた角速度で駆動させる駆動手段と、を備えることを特徴とする。
【０００７】
上記構成では、ロボットのアームは複数の回転部を含んでおり、回転部は関節により互いに回転可能に連結されている。そして、アームの両端部のうちアームの先端部とは反対側は、所定のロボット設置場所に設置される支持部に動作可能に支持されている。そして、ＰＴＰ制御により、ロボットの制御点として設定される先端部の位置及び姿勢が制御される。
【０００８】
ここで、制御点として設定されるアームの先端部の移動速度を、基準速度以下となるように制御したとしても、アーム（ロボット）の姿勢によっては、アームにおける制御点以外の部分の移動速度が基準速度よりも高くなる場合があることに本願発明者は着目した。
【０００９】
そこで上記構成では、各回転部に監視部が設定される。例えば、各回転部を回転させる際に回転中心となる関節から最も離れた部分が、各回転部の監視部として設定される。そして、角速度算出手段により動作周期毎に各サーボモータを駆動させる角速度が算出され、また、監視速度算出手段により各監視部の速度が算出される。そして、算出された各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、ロボットのアームにおいて、各監視部のうち最大の速度となる監視部よりも支持部側に設けられた全ての関節が速度低減対象軸として選択される。
【００１０】
続いて、角速度低下手段により、各監視部の速度が基準速度以下となるように、選択された速度低減対象軸を回転させるサーボモータ（以下、低減対象モータ）を駆動させる角速度が低下させられる。そして、各サーボモータのうち低減対象モータ以外のモータが、角速度算出手段により算出された角速度で駆動され、低減対象モータが、角速度低下手段により低下させられた角速度で駆動される。これにより、ロボットの制御点のみならず、各回転部に設定された監視部の速度を基準速度以下にすることができ、アームの移動速度を十分に抑制することができる。
【００１１】
また上記構成では、各サーボモータのうち角速度の低減対象となるモータを、上記速度低減対象軸を回転させる低減対象モータとしている。このため、各監視部の速度が基準速度以下となるように全てのサーボモータの角速度を一律に低下させる構成と比較して、角速度を低下させるサーボモータの数を少なくできる。このため、角速度を低下させる処理に要する時間を短縮でき、オペレータにとって快適なロボットの動作レスポンスを維持することもできる。さらに上記構成では、各関節のうち、上記最大の速度が基準速度を超えるに至った大きな要因となる関節がどの関節であるかを特定する必要がなく、角速度を低下させる処理を簡易にすることができる。
【００１２】
第２の発明では、前記角速度算出手段により算出された前記各サーボモータの角速度に基づいて、前記各サーボモータの前記動作周期後の角度を算出する次周期角度算出手段と、前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度のうち前記低減対象モータの動作周期後の角度と、前記低減対象モータの現在の角度とに基づいて、前記低減対象モータの動作周期あたりの角度変化量を算出する変化量算出手段と、をさらに備え、前記角速度低下手段は、前記最大の速度が前記基準速度以下となるように、前記変化量算出手段により算出された前記低減対象モータの角度変化量を低下させ、前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度のうち、前記低減対象モータの動作周期後の角度を、前記角速度低下手段によって低下させられた角度変化量と、前記低減対象モータの現在の角度とに基づいて算出された前記低減対象モータの動作周期後の角度で更新することにより、前記各サーボモータの動作周期後の角度を再設定する再設定手段をさらに備え、前記駆動手段は、前記各サーボモータの現在の角度を、前記再設定手段により再設定された前記各サーボモータの角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる。
【００１３】
上記構成では、次周期角度算出手段により、各サーボモータの動作周期後の角度が算出される。そして、低減対象モータの動作周期後の角度と、低減対象モータの現在の角度とに基づいて、低減対象モータの動作周期あたりの角度変化量が算出され、各監視部の速度のうち最大の速度が基準速度以下となるように、算出された低減対象モータの角度変化量が低下させられる。
【００１４】
続いて、次周期角度算出手段により算出された各サーボモータの動作周期後の角度のうち、低減対象モータの動作周期後の角度が、角速度低下手段によって低下させられた角度変化量と、低減対象モータの現在の角度とに基づいて算出された低減対象モータの動作周期後の角度で更新されることにより、各サーボモータの動作周期後の角度が再設定される。そして、各サーボモータの現在の角度が、再設定された各サーボモータの角度まで動作周期後に制御されるように、各サーボモータが駆動される。このため、低減対象モータの動作周期あたりに駆動される角度が小さくなり、低減対象モータの駆動される角速度を適切に低下させることができる。
【００１５】
第３の発明では、前記角速度低下手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記変化量算出手段により算出された角度変化量を低下させる。
【００１６】
上記構成では、各監視部の速度のうち最大の速度と基準速度との比の値に基づいて、低減対象モータの角度変化量が低下させられる。上記比の値を用いることにより、基準速度に対する最大の速度の超過分を反映して角度変化量を低下させることができる。なお、最大の速度と基準速度との比の値とは、最大の速度を基準速度で割った値である（比の値＝最大の速度／基準速度）。
【００１７】
第４の発明では、前記角速度低下手段は、前記角度変化量を前記比の値で割ることにより、前記角度変化量を低下させる。
【００１８】
上記構成では、角度変化量を上記比の値で割るといった簡単な演算を用いるため、上記角度変化量を容易に低下させることができる。さらに上記構成では、角度変化量を上記比の値で割るといった演算を用いるため、角度変化量を一定の法則から定まる比率で低下させることができ、角度変化量の低下分をオペレータが予測しやすくなる。
【００１９】
第５の手段では、前記角速度低下手段は、前記最大の速度が前記基準速度以下となるように、前記角速度算出手段により算出された前記各サーボモータの角速度のうち、前記低減対象モータの角速度を低下させ、前記角速度算出手段により算出された前記各サーボモータの角速度のうち、前記低減対象モータの角速度を、前記角速度低下手段によって低下させられた角速度で更新することにより、前記各サーボモータの角速度を再設定する再設定手段をさらに備え、前記駆動手段は、前記再設定手段により再設定された前記各サーボモータの角速度により、前記各サーボモータを駆動させる。
【００２０】
上記構成では、最大の速度が基準速度以下となるように、角速度算出手段により算出された各サーボモータの角速度のうち、低減対象モータの角速度が低下させられる。そして、角速度算出手段により算出された各サーボモータの角速度のうち、低減対象モータの角速度が、角速度低下手段によって低下させられた角速度で更新されることにより、各サーボモータの角速度が再設定される。そして、再設定された各サーボモータの角速度により、各サーボモータが駆動される。これにより、低減対象モータを駆動させる角速度を適切に低下させることができる。
【００２１】
第６の発明では、前記角速度低下手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記角速度算出手段により算出された前記低減対象モータの角速度を低下させる。
【００２２】
上記構成では、各監視部の速度のうち最大の速度と基準速度との比の値に基づいて、速度が最大となる監視部の速度を基準速度以下とするように、低減対象モータを駆動させる角速度を適切に低下させることができる。なお、最大の速度と基準速度との比の値とは、最大の速度を基準速度で割った値である（比の値＝最大の速度／基準速度）。
【００２３】
第７の発明では、前記角速度低下手段は、前記角速度算出手段により算出された前記低減対象モータの角速度を前記比の値で割ることにより、前記角速度を低下させる。
【００２４】
上記構成では、低減対象モータを駆動させる角速度を容易かつ適切に低下させることができる。
【００２５】
第８の発明では、前記各回転部を回転させる際に回転中心となる前記関節から最も離れた部分を、前記各回転部の前記監視部として設定する。
【００２６】
上記構成によれば、各回転部を回転させる際に回転中心となる関節から最も離れた部分が、各回転部の監視部として設定される。このため、各回転部において、最も速度が高くなる可能性の高い部分を監視部に設定することができ、アームの移動速度を十分に抑制することができる。
【００２７】
第９の発明では、前記角速度算出手段により算出された前記各サーボモータの角速度に基づいて、前記各サーボモータの前記動作周期後の角度を算出する次周期角度算出手段と、前記各監視部の現在の位置を算出する現在監視位置算出手段と、前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度に基づいて、前記各監視部の動作周期後の位置を算出する次周期監視位置算出手段と、をさらに備え、前記監視速度算出手段は、前記現在監視位置算出手段により算出された前記各監視部の現在の位置と、前記次周期監視位置算出手段により算出された前記各監視部の動作周期後の位置とに基づいて、前記各監視部の速度を算出する。
【００２８】
上記構成では、現在監視位置算出手段により算出された各監視部の現在の位置と、次周期監視位置算出手段により算出された各監視部の動作周期後の位置とに基づいて、各監視部の速度を適切に算出することができる。
【００２９】
なお、監視速度算出手段としては、具体的には、第１０の発明のように、前記各監視部の現在の位置と前記各監視部の動作周期後の位置との距離を前記動作周期で割ることにより前記各監視部の速度を算出するといった構成を採用することができる。
【００３０】
また、次周期監視位置算出手段としては、具体的には、第１１の発明のように、前記次周期角度算出手段により算出された前記各サーボモータの動作周期後の角度と、前記各回転部の大きさとに基づいて、前記各監視部の動作周期後の位置を算出するといった構成を採用することができる。
【００３１】
第１２の手段は、複数の回転部、前記回転部を互いに回転可能に連結する関節、及び前記各回転部を駆動するサーボモータを含むアームと、所定のロボット設置場所に設置され、前記アームの両端部のうち前記アームの先端部とは反対側を動作可能に支持する支持部と、を備えるロボットに適用され、ＰＴＰ制御により前記先端部の位置及び姿勢を制御するロボットの制御方法であって、動作周期毎に前記各サーボモータを駆動させる角速度を算出する角速度算出工程と、前記各回転部に設定された監視部の速度を算出する監視速度算出工程と、前記監視速度算出工程により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記アームにおいて、前記最大の速度となる前記監視部よりも前記支持部側に設けられた全ての前記関節を速度低減対象軸として選択する選択工程と、前記選択工程により選択された前記速度低減対象軸を回転させる前記サーボモータを低減対象モータとし、前記各監視部の速度が前記基準速度以下となるように、前記低減対象モータを駆動させる角速度を低下させる角速度低下工程と、前記各サーボモータのうち前記低減対象モータ以外のモータを、前記角速度算出工程により算出された角速度で駆動させ、前記低減対象モータを、前記角速度低下工程により低下させられた角速度で駆動させる駆動工程と、を備えることを特徴とする。
【００３２】
上記工程によれば、第１の手段と同様の作用効果を奏することができる。
【図面の簡単な説明】
【００３３】
【図１】ロボット、コントローラ、及びティーチングペンダントの概要を示す図。
【図２】ロボットの特定姿勢を示す正面図。
【図３】第１実施形態に係るアーム速度抑制制御の処理手順を示すフローチャート。
【図４】サーボモータの角速度パターンを示すグラフ。
【図５】第２実施形態に係るアーム速度抑制制御の処理手順を示すフローチャート。
【発明を実施するための形態】
【００３４】
（第１実施形態）
以下、垂直多関節型ロボットの制御装置に具体化した第１実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。
【００３５】
はじめに、ロボット１０の概要を図１に基づいて説明する。
【００３６】
同図に示すように、ロボット１０は、回転部を互いに連結する各関節の回転中心軸線として、第１軸線Ｊ１、第２軸線Ｊ２、第３軸線Ｊ３、第４軸線Ｊ４、第５軸線Ｊ５、及び第６軸線Ｊ６を有する６軸ロボットである。これら各軸線における各部の動作角度は、それぞれサーボモータ等からなる駆動源の駆動、及び減速機等による減速を通じて調整される。サーボモータは、いずれも正逆両方向の回転が可能であり、サーボモータの駆動により原点位置を基準として各回転部が動作（駆動）する。各サーボモータには、その出力軸を制動する電磁ブレーキと、出力軸の回転角度に応じたパルス信号を出力するエンコーダとがそれぞれ設けられている。
【００３７】
ロボット１０は、床に設置されており、第１軸線Ｊ１が鉛直方向へ延びている。ロボット１０において、基台１１は、床等に固定される固定部１２と、その固定部１２の上方に設けられる回転部１３（第１回転部）とを有している。ロボット１０のアームは、回転部１３に加え、下アーム１５、上アーム１６、手首部１７、及びハンド部１８によって構成されている。なお、本実施形態において、固定部１２が支持部に相当する。
【００３８】
回転部１３は、アームの両端部のうち、アーム先端部とは反対側の根元部に相当する。回転部１３は、第１軸線Ｊ１を回転中心として水平方向に回転可能になっている。すなわち、回転部１３は、第１軸線Ｊ１の方向に延びるとともに、固定部１２により第１軸線Ｊ１を中心として回転可能に支持されている。
【００３９】
下アーム１５（第２回転部）が、水平方向に延びる第２軸線Ｊ２を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、下アーム１５は、第１軸線Ｊ１に直交する平面に含まれる第２軸線Ｊ２から離れる方向へ延びるとともに、回転部１３により第２軸線Ｊ２を中心として回転可能に支持されている。下アーム１５は、基本姿勢として鉛直方向に延びる向きに設けられている。
【００４０】
下アーム１５の上端部には、上アーム１６が、水平方向に延びる第３軸線Ｊ３を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、上アーム１６は、第２軸線Ｊ２に平行な第３軸線Ｊ３から離れる方向へ延びるとともに、下アーム１５により第３軸線Ｊ３を中心として回転可能に支持されている。上アーム１６は、基本姿勢として水平方向に延びる向きに設けられている。
【００４１】
上アーム１６は、基端側（回転の際に第３軸線Ｊ３を回転中心とする関節側）と先端側とで２つのアーム部に分割されて構成されており、基端側は第１上アーム１６Ａ（第３回転部）、先端側は第２上アーム１６Ｂ（第４回転部）となっている。第２上アーム１６Ｂは、上アーム１６の長手方向に延びる第４軸線Ｊ４を回転中心として、第１上アーム１６Ａに対してねじり方向に回転可能になっている。すなわち、第２上アーム１６Ｂは、第３軸線Ｊ３に直交する平面に含まれる第４軸線Ｊ４の方向に延びるとともに、第１上アーム１６Ａにより第４軸線Ｊ４を中心として回転可能に支持されている。
【００４２】
上アーム１６（詳しくは第２上アーム１６Ｂ）の先端部には、手首部１７（第５回転部）が設けられている。手首部１７は、水平方向に延びる第５軸線Ｊ５を回転中心として、第２上アーム１６Ｂに対して回転可能になっている。すなわち、手首部１７は、第４軸線Ｊ４に直交する第５軸線Ｊ５から離れる方向へ延びるとともに、第２上アーム１６Ｂにより第５軸線Ｊ５を中心として回転可能に支持されている。
【００４３】
手首部１７の先端部には、ワークやツール等を取り付けるためのハンド部１８（第６回転部）が設けられている。ハンド部１８は、その中心線である第６軸線Ｊ６を回転中心として、ねじり方向に回転可能になっている。すなわち、ハンド部１８は、第５軸線Ｊ５に直交する第６軸線Ｊ６の方向に延びるとともに、手首部１７により第６軸線Ｊ６を中心として回転可能に支持されている。
【００４４】
コントローラ３０（制御装置）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、及び位置検出回路等を備えている。ＲＯＭは、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたサーボモータの回転角度を検出する。
【００４５】
ＣＰＵは、予め設定された動作プログラム（プログラム）を実行することにより、位置検出回路から入力される位置情報に基づいて、アーム先端部の制御点の位置及び姿勢を制御する。詳しくは、ＣＰＵは、ＰＴＰ（Point To Point）制御により、ロボット１０のアームにおける各関節の回転角度（アームの姿勢）を目標回転角度（目標姿勢）にフィードフォワード制御する。ＰＴＰ制御では、制御点を目標位置まで動作させる際に制御点の動作軌道（位置及び姿勢）が設定されない。本実施形態では、制御点として、アームのハンド部１８の中心点１８ａであるＴＣＰ（Tool Center Point）が設定されている。また、ＣＰＵは、ＴＣＰの位置及び姿勢に基づいて、この位置及び姿勢を実現するための各関節の角度を逆変換によって算出する機能と、各関節の角度に基づいて、ＴＣＰの位置及び姿勢を順変換によって算出する機能とを有する。
【００４６】
本実施形態では、コントローラ３０は、ロボット１０のティーチング時（手動操作時）において、ロボット１０のアームの移動速度を基準速度以下に抑制する速度抑制制御を実行する。基準速度は、ＪＩＳやＩＳＯ等の規格により、例えば２５０ｍｍ／ｓに規定されている。
【００４７】
ティーチングペンダント４０（操作機）は、ＣＰＵ、ＲＯＭ、及びＲＡＭを含むマイクロコンピュータ、各種の手動操作キー、並びにディスプレイ４２等を備えている。ペンダント４０は、コントローラ３０に接続されており、コントローラ３０と通信可能となっている。オペレータ（使用者）は、このペンダント４０を手動操作して、ロボット１０の動作プログラムの作成、修正、登録、各種パラメータの設定を行うことができる。動作プログラムの修正等を行うティーチングでは、作業において制御点であるＴＣＰが通過する教示点を教示する。そして、オペレータは、コントローラ３０を通じて、ティーチングされた動作プログラムに基づきロボット１０を動作させることができる。換言すれば、コントローラ３０は、予め設定された動作プログラム及びペンダント４０の操作に基づいて、ロボット１０のアームの動作を制御する。
【００４８】
ここで、ロボット１０のティーチング時（手動操作時）において、ＴＣＰの移動速度を、基準速度以下となるように制御したとしても、ロボット１０の姿勢によっては、アームにおけるＴＣＰ以外の部分の移動速度が基準速度よりも高くなる場合があることに本願発明者は着目した。例えば、ロボット１０が図２に示す姿勢である場合、回転部１３を回転させると、ＴＣＰ（点Ｃ５）の移動速度は基準速度よりも十分に小さくなる。しかしながら、下アーム１５の先端部（点Ｃ２）及び上アーム１６の一方の端部（点Ｃ３）の移動速度が、基準速度よりも高くなる場合がある。
【００４９】
そこで、各回転部を回転させる際に回転中心となる関節（各回転部の回転中心軸線）から最も離れた部分を、各回転部の監視部（点Ｃ１〜Ｃ５）として設定する。例えば、下アーム１５を回転させる際に回転中心となる関節（回転部１３と下アーム１５との連結部）から最も離れた点Ｃ２を、下アーム１５の監視部として設定する。同様にして、上アーム１６を回転させる際に回転中心となる関節（下アーム１５と上アーム１６との連結部）から最も離れた点Ｃ３，Ｃ４を、上アーム１６の監視部として設定する等を行う。なお、上アーム１６等の回転部に他の部品（パーツ）が取り付けられている場合には、その部品の先端部等を監視部として設定してもよい。そして、全ての監視部の移動速度が基準速度以下となるように、各サーボモータの角速度を抑制する。
【００５０】
図３は、ロボット１０のアームの移動速度を基準速度以下に抑制する速度抑制制御の処理手順を示すフローチャートである。この一連の処理は、コントローラ３０によって、アームを動作させる動作周期Ｔｒ毎に繰り返し実行される。動作周期Ｔｒ（制御周期）は、例えば８ｍｓである。
【００５１】
この一連の処理では、まず、各サーボモータの現在の角度θｋ１を検出する（Ｓ１１）。詳しくは、各サーボモータに設けられたエンコーダの検出信号に基づいて、位置検出回路により各サーボモータの現在の角度θｋ１を検出する。なお、ｋは、第１軸線Ｊ１〜第６軸線Ｊ６にそれぞれ対応する１〜６の数字である。
【００５２】
続いて、各サーボモータの現在の角度θｋ１及び各回転部の大きさに基づいて、各監視部の現在の位置Ｐｉ１を算出する（Ｓ１２）。ｉは、点Ｃ１〜Ｃ５にそれぞれ対応する１〜５の数字である。現在の位置Ｐｉ１の算出手法の具体例について説明すると、まず、各回転部の大きさと各監視部の設定された位置とに基づいて、各回転部の回転中心から監視部までの距離を算出する。そして、各サーボモータの現在の角度θｋ１、各回転部の大きさ、及び上記距離を組み合わせることにより、点Ｃ１〜Ｃ５の位置を算出する。
【００５３】
続いて、各サーボモータの角速度ωｋを算出する（Ｓ１３）。具体的には、ティーチング時において、ＴＣＰが通過する点として教示された教示点に基づいて、各サーボモータの目標角度が算出されている。そして、例えば図４に示すように、その目標角度まで各サーボモータを駆動する際の角速度ωｋのパターンが設定されている。そこで、設定された角速度ωｋのパターンに基づいて、各サーボモータの現在の角速度ωｋを算出する。なお、ｋは、第１軸線Ｊ１〜第６軸線Ｊ６にそれぞれ対応する１〜６の数字である。
【００５４】
続いて、各サーボモータの動作周期Ｔｒ後の角度θｋ２を算出する（Ｓ１４）。詳しくは、θｋ２＝θｋ１＋ωｋ×Ｔｒの式により、角度θｋ２を算出する。
【００５５】
続いて、各監視部の動作周期Ｔｒ後の位置Ｐｉ２を算出する（Ｓ１５）。ｉは、点Ｃ１〜Ｃ５にそれぞれ対応する１〜５の数字である。ここで、動作周期Ｔｒ後の位置Ｐｉ２は、Ｓ１２の処理と同様にして、各サーボモータの動作周期Ｔｒ後の角度θｋ２、各回転部の大きさ、及び各回転部の回転中心から監視部までの距離に基づいて算出すればよい。
【００５６】
続いて、各監視部の速度Ｖｉを算出する（Ｓ１６）。詳しくは、各監視部の現在の位置Ｐｉ１と動作周期Ｔｒ後の位置Ｐｉ２との距離を動作周期Ｔｒで割ることにより、速度Ｖｉを算出する。なお、ｉは、点Ｃ１〜Ｃ５にそれぞれ対応する１〜５の数字である。
【００５７】
続いて、各監視部の速度Ｖｉのうち最大の速度である最大速度Ｖｍｘを算出する（Ｓ１７）。そして、各監視部のうち、最大速度Ｖｍｘをとる監視部をコントローラ３０の記憶手段（メモリ）に記憶する（Ｓ１８）。
【００５８】
続いて、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いか否か判定する（Ｓ１９）。この判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いと判定した場合（Ｓ１９：ＹＥＳ）、最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αを算出する（Ｓ２０）。すなわち、α＝Ｖｍｘ／Ｖｌｍの式により、比の値αを算出する（α＞１）。
【００５９】
続いて、アームにおいて、メモリに記憶された最大速度Ｖｍｘをとる監視部よりも固定部１２側に設けられた全ての関節を、速度低減対象軸として選択する（Ｓ２１）。具体的には例えば、先の図２において、最大速度Ｖｍｘとなる監視部が点Ｃ２である場合、速度低減対象軸として、第１軸線Ｊ１及び第２軸線Ｊ２を選択する。本実施形態では、速度低減対象軸を回転させるサーボモータを、低減対象モータと称すこととする。
【００６０】
続いて、低減対象モータの動作周期Ｔｒ後の角度θｋ２と現在の角度θｋ１との差分（θｋ２−θｋ１）を、低減対象モータの角度変化量Δθｋとして算出する（Ｓ２２）。なお、ｋは、第１軸線Ｊ１〜第６軸線Ｊ６にそれぞれ対応する１〜６の数字である。そして、低減対象モータの角度変化量Δθｋを比の値αで割ることにより、角度変化量Δθｋを更新する（Ｓ２３）。
【００６１】
続いて、全てのサーボモータの動作周期Ｔｒ後の角度θｋ２を再設定する（Ｓ２４）。詳しくは、まず、Ｓ２３の処理で更新された角度変化量Δθｋと、低減対象モータの現在の角度θｋ１とを加算することにより、低減対象モータの動作周期Ｔｒ後の角度を新たに算出する。そして、全てのサーボモータの動作周期Ｔｒ後の角度のうち、低減対象モータの動作周期Ｔｒ後の角度θｋ２のみを、新たに算出した動作周期Ｔｒ後の角度で更新することにより、全てのサーボモータの動作周期Ｔｒ後の角度θｋ２を再設定する。そして、再設定された角度θｋ２を用いて、Ｓ１５の処理から再度実行する。
【００６２】
一方、Ｓ１９の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍ以下であると判定した場合（Ｓ１９：ＮＯ）、各サーボモータの現在の角度が、各サーボモータの角度θｋ２まで動作周期Ｔｒ後に制御されるように各サーボモータを駆動する（Ｓ２５）。ここでは、Ｓ２４の処理を経由している場合、Ｓ２４の処理で再設定された角度θｋ２まで、動作周期Ｔｒ後に各サーボモータが制御されるように各サーボモータを駆動する。そして、この一連の処理を一旦終了する（ＥＮＤ）。
【００６３】
なお、Ｓ１２の処理が現在監視位置算出手段としての処理（現在監視位置算出工程）に相当し、Ｓ１３の処理が角速度算出手段としての処理（角速度算出工程）に相当し、Ｓ１４の処理が次周期角度算出手段としての処理（次周期角度算出工程）に相当し、Ｓ１５の処理が次周期監視位置算出手段としての処理（次周期監視位置算出工程）に相当し、Ｓ１６の処理が監視速度算出手段としての処理（監視速度算出工程）に相当する。また、Ｓ２１の処理が選択手段としての処理（選択工程）に相当し、Ｓ２２の処理が変化量算出手段としての処理（変化量算出工程）に相当し、Ｓ２３の処理が角速度低下手段としての処理（角速度低下工程）に相当し、Ｓ２４の処理が再設定手段としての処理（再設定工程）に相当し、Ｓ２５の処理が駆動手段としての処理（駆動工程）に相当する。
【００６４】
以上詳述した本実施形態は、以下の利点を有する。
【００６５】
・動作周期Ｔｒ毎に各サーボモータを駆動させる角速度ωｋが算出され、また、各回転部に設定された監視部（点Ｃ１〜Ｃ５）の速度Ｖｉが算出される。そして、算出された各監視部の速度Ｖｉのうち最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いことを条件として、アームにおいて、各監視部のうち最大速度Ｖｍｘとなる監視部よりも固定部１２側に設けられた全ての関節が速度低減対象軸として選択される。
【００６６】
続いて、各監視部の速度Ｖｉが基準速度Ｖｌｍ以下となるように、速度低減対象軸を回転させる低減対象モータの動作周期Ｔｒあたりの角度変化量Δθｋが低下させられる。そして、各サーボモータの動作周期Ｔｒ後の角度θｋ２のうち、低減対象モータの動作周期Ｔｒ後の角度が、低下させられた角度変化量Δθｋと、低減対象モータの現在の角度θｋ１とに基づいて算出された低減対象モータの動作周期Ｔｒ後の角度で更新されることにより、各サーボモータの動作周期Ｔｒ後の角度θｋ２が再設定される。そして、各サーボモータの現在の角度が、再設定された各サーボモータの角度θｋ２まで動作周期Ｔｒ後に制御されるように、各サーボモータが駆動される。このため、低減対象モータを駆動させる角速度を適切に低下させることができる。これにより、制御点であるＴＣＰのみならず、各監視部の速度Ｖｉを基準速度Ｖｌｍ以下にすることができ、アームの移動速度を十分に抑制することができる。
【００６７】
また、各サーボモータのうち角度変化量Δθｋの低減対象となるモータを、速度低減対象軸を回転させる低減対象モータとしている。これにより、各監視部の速度Ｖｉが基準速度Ｖｌｍ以下となるように全てのサーボモータの角度変化量Δθｋを一律に低下させる構成と比較して、角度変化量Δθｋを低下させるサーボモータの数を少なくできる。このため、角度変化量Δθｋを低下させるための再計算に要する時間を短縮でき、ティーチング時においてオペレータにとって快適なロボットの動作レスポンスを維持することもできる。さらに、ロボットの各関節のうち、上記最大の速度Ｖｍｘが基準速度Ｖｌｍを超えるに至った大きな要因となる関節がどの関節であるかを特定する処理、及び特定された関節を回転させるサーボモータの角度変化量Δθｋを低下させる処理を行う構成と比較して、この構成と同程度の安全性を実現しつつ、角度変化量Δθｋを低下させるプログラムを簡易にすることができる。
【００６８】
・低減対象モータの角度変化量Δθｋを、最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αで割ることにより、角度変化量Δθｋが低下させられる。角度変化量Δθｋを比の値αで割るといった簡単な演算を用いるため、角度変化量Δθｋを容易に低下させることができる。さらに、角度変化量Δθｋを比の値αで割るといった演算を用いるため、角度変化量Δθｋを一定の法則から定まる比率で低下させることができ、角度変化量Δθｋの低下分をオペレータが予測しやすくなる。
【００６９】
・各回転部を回転させる際に回転中心となる関節から最も離れた部分が、各回転部の監視部として設定される。このため、各回転部において、最も速度が高くなる可能性の高い部分を監視部に設定することができ、アームの移動速度を十分に抑制することができる。
【００７０】
（第２実施形態）
第２実施形態では、低減対象モータを駆動させる角速度の低下手法を変更する。以下、上記第１実施形態との相違点を中心に説明する。
【００７１】
図５は、ロボット１０のアームの移動速度を基準速度以下に抑制する速度抑制制御の処理手順を示すフローチャートである。この一連の処理は、コントローラ３０によって、アームを動作させる動作周期Ｔｒ毎に繰り返し実行される。なお、図５において、先の図３で説明した処理と同一の処理については、便宜上、同一のステップ番号を付している。
【００７２】
この一連の処理では、Ｓ１１〜Ｓ１８の処理の後、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いか否か判定する（Ｓ１９）。Ｓ１９の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いと判定した場合（Ｓ１９：ＹＥＳ）、Ｓ２０，Ｓ２１を経由した後、全てのサーボモータのうち低減対象モータの角速度ωｋを比の値αで割ることにより、低減対象モータの角速度ωｋを更新する（Ｓ２６）。
【００７３】
続いて、全てのサーボモータの角速度ωｋを再設定する（Ｓ２７）。詳しくは、Ｓ１３で設定された全てのサーボモータの角速度ωｋのうち、低減対象モータの角速度のみを、Ｓ２６で更新された低減対象モータの角速度で更新することにより、全てのサーボモータの角速度ωｋを再設定する。そして、再設定された角速度ωｋを用いて、Ｓ１４の処理から再度実行する。
【００７４】
一方、Ｓ１９の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍ以下であると判定した場合（Ｓ１９：ＮＯ）、Ｓ１４で算出された各サーボモータの角度θｋ２まで、各サーボモータを角速度ωｋで駆動する（Ｓ２８）。ここでは、Ｓ２７の処理を経由している場合、Ｓ２７の処理で再設定された角速度ωｋで各サーボモータを駆動する。そして、この一連の処理を一旦終了する（ＥＮＤ）。
【００７５】
以上詳述した本実施形態は、以下の利点を有する。ここでは、上記第１実施形態と相違する利点のみを述べる。
【００７６】
・動作周期Ｔｒ毎に各サーボモータを駆動させる角速度ωｋが算出され、また、各回転部に設定された監視部（点Ｃ１〜Ｃ５）の速度Ｖｉが算出される。そして、算出された各監視部の速度Ｖｉのうち最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いことを条件として、アームにおいて、各監視部のうち最大速度Ｖｍｘとなる監視部よりも固定部１２側に設けられた全ての関節が速度低減対象軸として選択される。
【００７７】
そして、各監視部の速度Ｖｉが基準速度Ｖｌｍ以下となるように、選択された速度低減対象軸を回転させる低減対象モータを駆動させる角速度が低下させられる。そして、既に算出された各サーボモータの角速度ωｋのうち、低減対象モータの角速度が、低下させられた角速度で更新されることにより、各サーボモータの角速度ωｋが再設定される。そして、再設定された各サーボモータの角速度ωｋにより、各サーボモータが駆動される。これにより、制御点であるＴＣＰのみならず、各監視部の速度Ｖｉを基準速度Ｖｌｍ以下にすることができ、アームの移動速度を十分に抑制することができる。
【００７８】
さらに、各サーボモータのうち角速度ωｋの低減対象となるモータを、上記速度低減対象軸を回転させる低減対象モータとしている。これにより、各監視部の速度Ｖｉが基準速度Ｖｌｍ以下となるように全てのサーボモータの角速度ωｋを一律に低下させる構成と比較して、角速度ωｋを低下させるサーボモータの数を少なくできる。このため、角速度ωｋを低下させるための再計算に要する時間を短縮でき、ティーチング時においてオペレータにとって快適なロボットの動作レスポンスを維持することもできる。さらに、ロボットの各関節のうち、上記最大の速度Ｖｍｘが基準速度Ｖｌｍを超えるに至った大きな要因となる関節がどの関節であるかを特定する処理、及び特定された関節を回転させるサーボモータの角速度ωｋを低下させる処理を行う構成と比較して、この構成と同程度の安全性を実現しつつ、角速度ωｋを低下させるプログラムを簡易にすることができる。
【００７９】
なお、上記第１，第２実施形態を以下のように変更して、実施することもできる。
【００８０】
・図３及び図５のＳ１９において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いか否か判定したが、最大速度Ｖｍｘが、基準速度Ｖｌｍよりも若干高く設定した判定速度よりも高いか否か判定してもよい。この場合は、アームの速度抑制制御を迅速に終了することができる。
【００８１】
・図３のＳ２３において、角度変化量Δθｋを比の値αで割ることにより角度変化量Δθｋを低下させたが、角度変化量Δθｋを比の値αよりも若干大きい値で割ることにより角度変化量Δθｋを低下させてもよい。この場合も、アームの速度抑制制御を迅速に終了することができる。なお、図５のＳ２６においても同様に、角速度ωｋを比の値αよりも若干大きい値で割ることにより角速度ωｋを低下させてもよい。
【００８２】
・上記第１，第２実施形態では、基準速度Ｖｌｍとして、ＪＩＳやＩＳＯ等の規格により規定された２５０ｍｍ／ｓを用いたが、それよりも若干低い速度、例えば２３０ｍｍ／ｓを基準速度Ｖｌｍとして用いてもよい。これらの場合、アームの移動速度を、確実かつ容易に２５０ｍｍ／ｓよりも低下させることができる。
【００８３】
・上記第１，第２実施形態において、垂直多関節型のロボット１０に代えて、水平多関節型のロボット等を採用することもできる。
【符号の説明】
【００８４】
１０…ロボット、１３…回転部、１５…下アーム、１６…上アーム、１６Ａ…第１上アーム、１６Ｂ…第２上アーム、１７…手首部、１８…ハンド部、３０…コントローラ。
【図１】
0006547320.tif 000002
【図２】
0006547320.tif 000003
【図３】
0006547320.tif 000004
【図４】
0006547320.tif 000005
【図５】
0006547320.tif 000006