(57)【要約】  物体に接触したリンクの関節軸がより柔軟に動作できるロボットシステム、ロボット及びロボット制御装置を提供する。  ロボットシステム（１０）は、複数の関節軸を介して連結された複数のリンク（２８）、各関節軸を駆動するサーボモータ、及びリンク（２８）が物体に接触したことを検出するための接触検出センサ（３０）を有するロボット（１２）と、ロボット制御装置（１４）とを備え、ロボット制御装置（１４）は、接触検出センサ（３０）の出力に基づいてリンク（２８）の接触位置を決定する接触位置決定部（４６）と、接触位置に対応するリンク（２８）の退避方向の退避方向ベクトルを計算する退避方向ベクトル計算部（４８）と、退避方向ベクトルの方向にリンク（２８）を移動させるためのアシストトルク指令を計算するアシストトルク計算部（５０）と、アシストトルク指令を各サーボモータのトルク指令に加算し各リンク（２８）を柔軟制御する柔軟制御部（５２）とを有する。
2013027250.tif 000002
【特許請求の範囲】
【請求項１】
複数の関節軸を介して連結された複数のリンク、前記各関節軸を駆動するサーボモータ、及び前記リンクが物体に接触したことを検出するための接触検出センサを有するロボットと、  前記ロボットを制御するロボット制御装置とを備え、  前記ロボット制御装置は、前記接触検出センサの出力に基づいて前記リンク上の前記物体との接触位置を決定する接触位置決定部と、  前記接触位置決定部が決定した前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する退避方向ベクトル計算部と、  前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記各リンクを柔軟制御する柔軟制御部とを有するロボットシステム。
【請求項２】
請求項１記載のロボットシステムにおいて、  前記アシストトルク計算部は、前記リンクを退避させるためのアシスト力の大きさを決定するアシスト力決定部と、  前記リンクの退避方向ベクトル及び前記アシスト力の大きさに基づいて、アシスト力ベクトルを計算するアシスト力ベクトル計算部と、  前記アシスト力ベクトルに基づいて、前記アシストトルク指令を計算するトルク計算部とを有するロボットシステム。
【請求項３】
請求項１記載のロボットシステムにおいて、  前記アシストトルク計算部は、前記リンクを退避させるためのアシスト力の大きさを決定するアシスト力決定部と、  前記リンクの退避方向ベクトル及び前記アシスト力の大きさに基づいて、アシスト力ベクトルを計算するアシスト力ベクトル計算部と、  前記各サーボモータのアシストトルク指令の大きさを調整するための重み係数行列を前記アシスト力ベクトルに乗じ、重み調整された前記アシスト力ベクトルを計算する重み計算部と、  前記重み計算部が計算した前記アシスト力ベクトルに基づいて、前記アシストトルク指令を計算するトルク計算部とを有するロボットシステム。
【請求項４】
請求項３記載のロボットシステムにおいて、  前記重み係数行列は、粘性摩擦係数がより大きな前記関節軸について、前記退避方向ベクトルの方向に前記リンクを移動させるためのアシストトルクがより大きくなる重み付けをする行列であるロボットシステム。
【請求項５】
請求項３記載のロボットシステムにおいて、  前記重み係数行列は、前記物体に接触した前記リンクの基端側に位置する前記関節軸のうち、前記リンクの基端部に位置する前記関節軸について、前記退避方向ベクトルの方向に前記リンクを移動させるためのアシストトルクが最も大きくなる重み付けをする行列であるロボットシステム。
【請求項６】
請求項５記載のロボットシステムにおいて、  前記重み係数行列は、前記物体に接触した前記リンクの基端側に位置する前記関節軸のうち、前記物体に接触した前記リンクに近い前記関節軸ほど前記アシストトルクが大きくなる重み付けをする行列であるロボットシステム。
【請求項７】
請求項２〜６のいずれか１項に記載のロボットシステムにおいて、  前記アシスト力決定部は、前記リンクが前記物体に接触してから予め決められた時間ｔｉｍが経過するまで、前記アシストトルクの大きさが第１の大きさとなるように決定し、前記時間ｔｉｍが経過した後は、前記アシストトルクの大きさが前記第１の大きさよりも小さい第２の大きさとなるように決定するロボットシステム。
【請求項８】
複数の関節軸を介して連結された複数のリンク、前記各関節軸を駆動するサーボモータ、及び前記リンクが物体に接触したことを検出するための接触検出センサを有するロボットと、  前記ロボットを制御するロボット制御装置とを備え、  前記ロボット制御装置は、前記接触検出センサの出力に基づいて前記リンク上の前記物体との接触位置を決定する接触位置決定部と、  １）前記接触位置決定部が決定した前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する接触部法線ベクトル計算部、２）前記接触位置決定部が決定した前記接触位置が実際に移動する方向を表す移動ベクトルを計算する接触点移動ベクトル計算部、及び３）前記移動ベクトルに基づいて、前記退避方向ベクトルを補正する退避方向ベクトル補正部を有する退避方向ベクトル計算部と、  前記退避方向ベクトル計算部が補正した前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記各リンクを柔軟制御する柔軟制御部とを有するロボットシステム。
【請求項９】
複数の関節軸を介して連結された複数のリンクと、  前記各関節軸を駆動するサーボモータとを備え、  前記各リンクは、該リンクの外周方向に沿って並んで設けられ、該リンクが物体に接触したことを検出するための複数の接触検出センサを有するロボット。
【請求項１０】
複数のサーボモータによってそれぞれ駆動されるロボットのリンクが物体に接触した際の前記リンク上の接触位置を決定する接触位置決定部と、  前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する退避方向ベクトル計算部と、  前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記リンクを柔軟制御する柔軟制御部とを備えたロボット制御装置。
【請求項１１】
複数のサーボモータによってそれぞれ駆動されるロボットのリンクが物体に接触した際の前記リンク上の接触位置を決定する接触位置決定部と、  １）前記接触位置決定部が決定した前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する接触部法線ベクトル計算部、２）前記接触位置決定部が決定した前記接触位置が実際に移動する方向を表す移動ベクトルを計算する接触点移動ベクトル計算部、及び３）前記移動ベクトルに基づいて、前記退避方向ベクトルを補正する退避方向ベクトル補正部を有する退避方向ベクトル計算部と、  前記退避方向ベクトル計算部が補正した前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記リンクを柔軟制御する柔軟制御部とを備えたロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】  本発明は、ロボットシステム、ロボット及びロボット制御装置に関する。
【背景技術】
【０００２】  特許文献１には、ロボットの制御方式が記載されている。このロボットの制御方式は、位置制御ループ及び速度制御ループを備える制御系で制御されるサーボモータで各軸が駆動されるロボットの移動動作中に、ロボット又はロボットに支持される物体と外部の物体との間の接触の有無を検出し、接触が検出されたならば、位置制御ループ及び速度制御ループのゲインを下方調整するようにしている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第３３００６２５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】  本発明は、本発明の構成を有しない場合に比べて、物体に接触したリンクの関節軸がより柔軟に動作できるロボットシステム、ロボット及びロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】  上記課題を解決するため、本発明の一の観点によれば、複数の関節軸を介して連結された複数のリンク、前記各関節軸を駆動するサーボモータ、及び前記リンクが物体に接触したことを検出するための接触検出センサを有するロボットと、  前記ロボットを制御するロボット制御装置とを備え、  前記ロボット制御装置は、前記接触検出センサの出力に基づいて前記リンク上の前記物体との接触位置を決定する接触位置決定部と、  前記接触位置決定部が決定した前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する退避方向ベクトル計算部と、  前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記各リンクを柔軟制御する柔軟制御部とを有するロボットシステムが適用される。【０００６】  また、他の観点によれば、複数の関節軸を介して連結された複数のリンク、前記各関節軸を駆動するサーボモータ、及び前記リンクが物体に接触したことを検出するための接触検出センサを有するロボットと、  前記ロボットを制御するロボット制御装置とを備え、  前記ロボット制御装置は、前記接触検出センサの出力に基づいて前記リンク上の前記物体との接触位置を決定する接触位置決定部と、  １）前記接触位置決定部が決定した前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する接触部法線ベクトル計算部、２）前記接触位置決定部が決定した前記接触位置が実際に移動する方向を表す移動ベクトルを計算する接触点移動ベクトル計算部、及び３）前記移動ベクトルに基づいて、前記退避方向ベクトルを補正する退避方向ベクトル補正部を有する退避方向ベクトル計算部と、  前記退避方向ベクトル計算部が補正した前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記各リンクを柔軟制御する柔軟制御部とを有するロボットシステムが適用される。【０００７】  また、他の観点によれば、複数の関節軸を介して連結された複数のリンクと、  前記各関節軸を駆動するサーボモータとを備え、  前記各リンクは、該リンクの外周方向に沿って並んで設けられ、該リンクが物体に接触したことを検出するための複数の接触検出センサを有するロボットが適用される。【０００８】  また、他の観点によれば、複数のサーボモータによってそれぞれ駆動されるロボットのリンクが物体に接触した際の前記リンク上の接触位置を決定する接触位置決定部と、  前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する退避方向ベクトル計算部と、  前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記リンクを柔軟制御する柔軟制御部とを備えたロボット制御装置が適用される。【０００９】  また、他の��点によれば、複数のサーボモータによってそれぞれ駆動されるロボットのリンクが物体に接触した際の前記リンク上の接触位置を決定する接触位置決定部と、  １）前記接触位置決定部が決定した前記接触位置に対応する前記リンクの退避方向の退避方向ベクトルを計算する接触部法線ベクトル計算部、２）前記接触位置決定部が決定した前記接触位置が実際に移動する方向を表す移動ベクトルを計算する接触点移動ベクトル計算部、及び３）前記移動ベクトルに基づいて、前記退避方向ベクトルを補正する退避方向ベクトル補正部を有する退避方向ベクトル計算部と、  前記退避方向ベクトル計算部が補正した前記退避方向ベクトルの方向に前記リンクを移動させるための前記各サーボモータのトルク指令であるアシストトルク指令を計算するアシストトルク計算部と、  前記アシストトルク指令を前記各サーボモータのトルク指令に加算し、前記リンクを柔軟制御する柔軟制御部とを備えたロボット制御装置が適用される。
【発明の効果】
【００１０】  本発明によれば、本発明の構成を有しない場合に比べて、物体に接触したリンクの関節軸がより柔軟に動作できる。
【図面の簡単な説明】
【００１１】
【図１】本発明の第１の実施例に係るロボットシステムの説明図である。
【図２】同ロボットシステムが有するロボットのリンクの説明図である。
【図３】同ロボットシステムが有するロボットのリンクの断面図である。
【図４】同ロボットシステムが有するロボットシステムのブロック図である。
【図５】同ロボットシステムが有するロボット制御装置に設けられたアシストトルク計算部のブロック図である。
【図６】同ロボットシステムが有するロボット制御装置に設けられたアシストトルク計算部の変形例を示すブロック図である。
【図７】同ロボットシステムが有するロボット制御装置に設けられた柔軟制御部のブロック図である。
【図８】本発明の第２の実施例に係るロボットシステムが有するロボット制御装置のブロック図である。
【図９】同ロボットシステムが有するロボット制御装置に設けられた第２の退避方向ベクトル計算部のブロック図である。
【図１０】同ロボットシステムが有するロボット制御装置が計算する補正された退避方向ベクトルを示す説明図である。
【発明を実施するための形態】
【００１２】  続いて、添付した図面を参照しつつ、本発明を具体化した実施例につき説明し、本発明の理解に供する。なお、各図において、説明に関連しない部分は図示を省略する場合がある。
【００１３】＜第１の実施例＞  本発明の第１の実施例に係るロボットシステム１０は、図１に示すように、ロボット１２及びロボット１２の動作を制御するロボット制御装置１４を備えている。
【００１４】  ロボット１２は、例えば、床面上を移動するための移動部２２と、移動部２２の上部に関節軸ＡＸＷを介して設けられ、この関節軸ＡＸＷを中心に前後方向に回転する胴体部２４と、胴体部２４の両側部に設けられた腕部２６とを有している。なおロボット制御装置１４はロボット１２に内蔵されていてもよい。  図２に示すように、各腕部２６は、複数の関節軸ＡＸを介して連結された複数のリンク２８を有している。各関節軸ＡＸは、エンコーダＥＮＣを有するサーボモータＳＶＭ（図４参照）により駆動される。
【００１５】  図２及び図３に示すように、ロボット１２の腕部２６のリンク２８には、例えば合計８枚の矩形状の接触センサ３０が設けられている。接触センサ３０は、リンク２８を覆う外皮３２の内側に設けられている。この外皮３２は、リンク２８が物体に接触した際の衝突力を緩和して、接触センサ３０を保護できる。  接触センサ３０は、物体との接触状態に応じて、ＯＮ／ＯＦＦ信号を出力できる。例えば、接触センサ３０は、物体が接触するとＯＮ信号を出力し、物体が離れるとＯＦＦ信号を出力できる。
【００１６】  接触センサ３０は、リンク２８の外周方向に沿って、並んで貼り付けられている。詳細には、リンク２８の外周が８つの領域に分割され、各領域に接触センサ３０が貼り付けられている。従って、物体が接触したリンク２８上の位置を、図２に示すリンク２８の長手方向の軸ＡＸＬを中心とする角度位置に関して４５度の分解能で特定できる。
【００１７】  なお、接触センサ３０は、合計８枚に限定されるものではない。また、接触センサ３０は、リンク２８の外周方向に沿って、並んで設けられていなくてもよい。  物体との接触を検出するための領域がリンク２８の周方向又は長手方向に複数分割され、各領域に接触センサ３０が貼り付けられていればよい。第１の例として、物体との接触を検出するための領域がリンク２８の周方向に１６の領域に分割され、各領域に接触センサ３０が設けられていてもよい。第２の例として、物体との接触を検出するための領域がリンク２８の周方向に８、長手方向に２の計１６の領域に分割され、各領域に接触センサ３０が設けられていてもよい。
【００１８】  また、接触センサ３０は、ＯＮ／ＯＦＦ信号を出力する接触センサに限定されるものではない。接触センサの他の例として、接触力（接触圧力）の大きさに応じて出力信号の大きさが増減するセンサが挙げられる。
【００１９】  これら複数の接触センサ３０により、リンク２８が物体に接触したことを検出することができる接触検出センサの一例が構成されている。この接触検出センサは、物体が接触したリンク２８上の位置又はその位置を含む領域を検出できればよい。
【００２０】  ロボット制御装置１４は、図４に示すように、指令生成部４２と、センサ信号入力部４４と、接触位置決定部４６と、第１の退避方向ベクトル計算部４８と、アシストトルク計算部５０と、各サーボモータＳＶＭに接続されたサーボアンプＳＶＡに対して、トルク指令τｒｅｆを出力する柔軟制御部５２とを有する。ロボット制御装置１４は、図示しないＣＰＵ及びメモリを内蔵しており、図４に示す各ブロックは、ＣＰＵにより実行されるソフトウェアプログラムやハードウェアによって実現される。  なお、図４において、複数の接触センサ３０、複数のサーボモータＳＶＭ、複数のエンコーダＥＮＣ及び複数のサーボアンプＳＶＡ等は、それぞれ１つのブロックでまとめて表されている。
【００２１】  指令生成部４２は、ロボット１２のリンク２８を回転移動させ、腕部２６の先端を移動させるためのサーボモータＳＶＭへの位置指令Ｐｒｅｆを生成できる。生成された位置指令Ｐｒｅｆは、柔軟制御部５２に入力される。
【００２２】  センサ信号入力部４４には、各接触センサ３０が接続されている。センサ信号入力部４４は、各接触センサ３０のインターフェースとして機能する。センサ信号入力部４４は、各接触センサ３０のＯＮ／ＯＦＦ信号に対応した信号を出力できる。この信号は、指令生成部４２に入力される。
【００２３】  接触位置決定部４６は、センサ信号入力部４４の出力信号に基づいて、物体が接触したリンク２８上の位置を接触位置Ｐｃとして決定できる。  接触位置決定部４６には、センサ信号入力部４４を介して、各接触センサ３０が出力したＯＮ／ＯＦＦ信号が入力される。接触位置決定部４６は、８枚の接触センサ３０のうち、いずれかの接触センサ３０が出力したＯＮ信号を検出し、物体が接触した接触センサ３０を特定する。
【００２４】  接触位置決定部４６は、物体に接触した接触センサ３０を特定した後、その接触センサ３０に対応する予め決められた点を図３に示す接触位置Ｐｃとして求めることができる。  ここで、接触位置Ｐｃは、例えば、各接触センサ３０の表面の中心位置として予め決定されている。接触位置Ｐｃは、接触センサ３０の表面の中心位置に限定されるものではなく、その接触センサ３０に対応した位置であれば、任意の位置でよい。  接触位置Ｐｃは予め決定されているので、図３に示すように、現実に物体が接触したリンク２８上の位置は、接触位置決定部４６が求めた接触位置Ｐｃとは異なる場合がある。ただし、接触センサを貼り付ける領域の分割数を増加させることによって、現実に物体が接触したリンク２８上の位置と接触位置Ｐｃとは、一致してくる。また、接触センサ３０の代わりに、接触位置も出力されるセンサを設けることによって、現実に物体が接触したリンク２８上の位置と接触位置Ｐｃとは、実質的に一致するようになる。
【００２５】  接触位置Ｐｃは、ロボット１２の各リンク２８を駆動する各サーボモータＳＶＭのエンコーダＥＮＣの信号から得られた各関節軸ＡＸの角度情報から導出される座標変換行列Ｔを用いて、例えばロボット１２の胴体部２４に固定されたロボット座標系上の位置として計算される。  接触位置決定部４６は、接触位置を複数検出するように構成されていてもよい。その場合、接触位置決定部４６は計算上の接触位置を複数出力できる。
【００２６】  図４に示す第１の退避方向ベクトル計算部４８は、接触位置Ｐｃに応じ、リンク２８が移動する方向（退避方向）を退避方向ベクトルｎとして計算できる。  退避方向ベクトルｎは、予め決められており、例えば、接触位置Ｐｃにおけるリンク２８の表面の法線方向の単位ベクトルである。従って、図３に示すように、退避方向ベクトルｎの方向は、リンク２８が接触した物体から受ける外力ｆｃの方向とは異なる場合がある。  退避方向ベクトルｎは、前述の座標変換行列Ｔを用いて、ロボット座標系におけるベクトルとして計算される。
【００２７】  図４に示すアシストトルク計算部５０は、接触位置Ｐｃと退避方向ベクトルｎに基づいて、ロボット１２の各関節軸ＡＸのアシストトルク指令τａを計算できる。  アシストトルク計算部５０は、図５に示すように、アシスト力決定部５０ａ、アシスト力ベクトル計算部５０ｂ及びトルク計算部５０ｃを有している。
【００２８】  アシスト力決定部５０ａは、リンク２８（腕部２６）を退避させるためのアシスト力ｆの大きさを決定し、スカラ量のアシスト力ｆを出力できる。このアシスト力ｆを状況に応じて変更することによって、ロボット１２が有する腕部２６の各関節軸ＡＸの柔軟性を調整できる。  例えば、アシスト力決定部５０ａは、リンク２８が物体に接触した後、予め決められた時間ｔｉｍ（例えば１〜１０ｍｓｅｃ）が経過するまではより大きな第１の大きさのアシスト力ｆを出力し、時間ｔｉｍが経過した後は、第１の大きさよりも小さい第２の大きさのアシスト力ｆを出力できる。このように、リンク２８が物体に接触した後、アシスト力決定部５０ａが、まず、より大きな第１の大きさのアシスト力ｆを出力することによって、リンク２８が退避方向へ速く動き、接触力（衝突力）が抑制される。  なお、接触センサが、前述の接触力（接触圧力）の大きさに応じて出力信号の大きさが増減するセンサである場合には、アシスト力決定部５０ａは、この信号の大きさに基づいてアシスト力ｆを決定してもよい。
【００２９】  アシスト力ベクトル計算部５０ｂは、アシスト力ｆ及び第１の退避方向ベクトル計算部４８が計算した退避方向ベクトルｎに基づいて、接触位置Ｐｃにおけるアシスト力ベクトルＦａを次式により計算できる。
【００３０】Ｆａ＝ｆ・ｎ      式（１）
【００３１】  トルク計算部５０ｃは、接触位置決定部４６で計算した計算上の接触位置から計算上の接触位置におけるヤコビ転置行列ＪＴを求め、次式によって各関節軸ＡＸを駆動するサーボモータＳＶＭのアシストトルク指令τａを計算できる。アシストトルク指令τａは、退避方向ベクトルの方向にリンク２８を移動させるための各関節軸ＡＸのトルク指令である。
【００３２】τａ＝ＪＴＦａ      式（２ａ）
【００３３】  ここで、アシストトルク計算部は、図６に示すように、アシスト力ベクトル計算部５０ｂとトルク計算部５０ｃとの間に重み計算部５０ｄを更に有するアシストトルク計算部５０ｘであってもよい。  この重み計算部５０ｄは、次式に示すように、前段のアシスト力ベクトル計算部５０ｂが出力したアシスト力ベクトルＦａに、重み係数行列ｋを乗じて、後段のトルク計算部５０ｃに出力できる。この重み係数行列ｋは、リンク２８の各関節軸ＡＸが発生するアシストトルク（退避方向ベクトルの方向にリンク２８を移動させるためのトルク）の大きさを調整するための対角行列である。
【００３４】  重み係数行列ｋは、例えば、以下のように設定される。  第１の例として、重み係数行列ｋは、各関節軸ＡＸに設けられた減速機等の粘性摩擦係数の大きさに応じて設定される。具体的には、各関節軸ＡＸの粘性摩擦係数の大きさに応じて、その関節軸ＡＸを駆動するサーボモータＳＶＭのアシストトルク指令が大きくなるように、重み係数行列ｋが設定される。即ち、重み係数行列ｋは、粘性摩擦係数がより大きな関節軸ＡＸについて、アシストトルクがより大きくなるように重み付けする。  第１の例のように重み係数行列ｋが設定されていると、各関節軸ＡＸの粘性摩擦の違いを吸収して、自然な動作でリンク２８（腕部２６）が動作できる。
【００３５】  第２の例として、重み係数行列ｋは、物体に接触したリンク２８の付け根（基端部）に位置する関節軸ＡＸのアシストトルクが最も大きくなるように重み付けする。更に詳細には、重み係数行列ｋは、接触位置Ｐｃよりも腕部２６の付け根側（基端側）に位置する全ての関節軸ＡＸのうち、物体に接触したリンク２８上の接触位置Ｐｃに近い関節軸ＡＸほどアシストトルクが大きくなる重み付けをする。  第２の例のように重み係数行列ｋが設定されていると、接触位置Ｐｃに近い軸がより速く退避動作するので、接触した物体から受ける外力の吸収性能が向上する。
【００３６】  アシストトルク計算部５０ｘのトルク計算部５０ｃは、次式で示すアシストトルク指令τａを計算する。
【００３７】τａ＝ｋ・ＪＴＦａ      式（２ｂ）
【００３８】  図４に示す柔軟制御部５２は、位置指令Ｐｒｅｆ及びアシストトルク指令τａに基づいて、サーボモータＳＶＭを駆動するためのトルク指令τｒｅｆをサーボアンプＳＶＡに出力し、ロボット１２の各関節軸ＡＸが柔軟に動作するよう制御できる。  柔軟制御部５２は、図７に示すように、位置速度制御部５２ａと、トルク制限値計算部５２ｂと、トルク制限部５２ｃと、重力補償トルク計算部５２ｄとを有している。
【００３９】  位置速度制御部５２ａにおいては、位置速度ループ（サーボループ）が構成されている。位置速度制御部５２ａは、エンコーダＥＮＣから得られた各サーボモータＳＶＭの角度フィードバック（エンコーダ値）Ｐｆｂと指令生成部４２が生成した位置指令Ｐｒｅｆとの位置偏差ｅに応じて、トルク指令τｂを出力できる。
【００４０】  トルク制限値計算部５２ｂは、トルク制限値Ｔｌｉｍを求めることができる。  トルク制限値計算部５２ｂは、例えば、リンク２８の運動に必要なサーボモータＳＶＭのトルク（リンク２８及び腕部２６の先端負荷の両者を加速するためのトルク並びにリンク２８の運動速度を維持するためのトルク）が含まれるように、トルク制限値（上限値及び下限値）Ｔｌｉｍを求めることができる。  トルク制限値計算部５２ｂは、他の例として、予め決められたサーボモータＳＶＭの最大トルクをトルク制限値Ｔｌｉｍとして求めることもできる。
【００４１】  トルク制限部５２ｃは、位置速度制御部５２ａが出力したトルク指令τbをトルク制限値計算部５２ｂが計算したトルク制限値Ｔｌｉｍにて制限し、制限されたトルク指令τｌｉｍを出力できる。
【００４２】  重力補償トルク計算部５２ｄは、各サーボモータＳＶＭのエンコーダ値Ｐｆｂから動力学演算によって各関節軸ＡＸの重力によるトルクを重力補償トルクτｇとして計算できる。  計算された重力補償トルクτｇは、アシストトルク指令τａと共にトルク制限部５２ｃが出力したトルク指令τｌｉｍに加算されるので、トルク制限部５２ｃがトルク指令を制限することによりリンク２８が落下したり、リンク２８が移動するために必要なトルクが不足したりすることが抑制される。
【００４３】  次に、ロボットシステム１０の動作について、腕部２６が物体に接触していない場合と、接触した場合とに分けて説明する。（１）腕部２６が物体に接触していない場合  柔軟制御部５２の位置速度制御部５２ａは、指令生成部４２が生成した位置指令Ｐｒｅｆに基づいて、トルク指令τｂを出力する。トルク制限部５２ｃは、このトルク指令τｂがトルク制限値Ｔｌｉｍを超える場合にはトルク指令の大きさを制限してトルク指令τｌｉｍとして出力する（図７参照）。  アシストトルク計算部５０は、各接触センサ３０がＯＮ信号を出力しないので、アシストトルクτａを出力しない。即ち、アシストトルク指令τａの大きさはゼロである。  重力補償トルク計算部５２ｄは、エンコーダ値Ｐｆｂに基づいて、重力補償トルクτｇを出力する。
【００４４】  従って、トルク指令τｌｉｍには、重力補償トルク計算部５２ｄが計算した重力補償トルクτｇが加算され、トルク指令τｒｅｆがサーボアンプＳＶＡ（図４参照）に出力される。  その結果、トルク指令τｒｅｆに従って、各サーボモータＳＶＭが駆動され、ロボット１２の腕部２６が動作する。  トルク指令τｒｅｆには、重力補償トルクτｇが加算されているので、腕部２６が自重により落下することが抑制される。
【００４５】（２）腕部２６が物体に接触した場合  腕部２６が、接触した物体から退避するか、接触した物体から受ける外力ｆｃに倣いながら移動するかによって、ロボットシステム１０の動作が異なる。以下、更に場合を分けて説明する。
【００４６】（２ａ）腕部２６が接触した物体から退避する場合  指令生成部４２は、センサ信号入力部４４が出力する信号に基づいて、位置指令Ｐｒｅｆの出力を停止する。  柔軟制御部５２の位置速度制御部５２ａは、位置偏差ｅに応じて、トルク指令τｂを出力する。  トルク制限値計算部５２ｂは、位置指令Ｐｒｅｆの出力が停止されたことを判断し、トルク制限値Ｔｌｉｍをゼロにする。すなわち、トルク制限部５２ｃは、トルク指令τｂに関わらず、トルク指令τｌｉｍをゼロとする。  アシストトルク計算部５０は、接触位置Ｐｃに応じて、アシストトルク指令τａを出力する。  重力補償トルク計算部５２ｄは、エンコーダ値Ｐｆｂに基づいて、重力補償トルクτｇを出力する。
【００４７】  従って、トルク制限部５２ｃが出力するトルク指令τｌｉｍがゼロとなっても、トルク制限部５２ｃの後段にて、アシストトルク指令τａ及び重力補償トルクτｇが加算され、トルク指令τｒｅｆが出力される。  その結果、トルク指令τｒｅｆに従って、各サーボモータＳＶＭが駆動され、リンク２８は、アシストトルクにより接触した物体から退避する。  トルク指令τｒｅｆには、重力補償トルクτｇが加算されているので、腕部２６が自重により落下することが抑制される。
【００４８】（２ｂ）腕部２６が接触した物体から受ける外力ｆｃに倣いながら移動する場合  指令生成部４２は、位置指令Ｐｒｅｆの出力を継続する。  柔軟制御部５２の位置速度制御部５２ａは、指令生成部４２が生成した位置指令Ｐｒｅｆに基づいて、トルク指令τｂを出力する。トルク制限部５２ｃは、位置速度制御部５２ａが出力するトルク指令τｂがトルク制限値Ｔｌｉｍを超える場合には、トルク指令の大きさを制限してトルク指令τｌｉｍとして出力する。  アシストトルク計算部５０は、接触位置Ｐｃに応じて、アシストトルク指令τａを出力する。  重力補償トルク計算部５２ｄは、エンコーダ値Ｐｆｂに基づいて、重力補償トルクτｇを出力する。
【００４９】  従って、トルク制限部５２ｃが出力するトルク指令τｌｉｍにアシストトルク指令τａ及び重力補償トルクτｇが加算され、トルク指令τｒｅｆが出力される。  その結果、トルク指令τｒｅｆに従って、各サーボモータＳＶＭが駆動される。リンク２８（腕部２６）は、トルク指令τｂの大きさがトルク制限部５２ｃによって制限されるので、接触した物体から受ける外力ｆｃに倣うことができる。一方、位置指令Ｐｒｅｆの出力は継続されるので、腕部２６の各関節軸ＡＸは、外力ｆｃに倣いながらできるだけ目標位置に向かって移動を続けようとする。  また、トルク指令τｒｅｆには、アシストトルクτａが加算されるので、腕部２６の各関節軸ＡＸは外力ｆｃに倣う方向に柔軟に動作する。  更に、トルク指令τｒｅｆには、重力補償トルクτｇが加算されているので、リンク２８が自重により落下することが抑制される。
【００５０】  以上説明したように、本実施例に係るロボットシステム１０によれば、アシストトルクが加算されることによって、腕部２６の各関節軸ＡＸが柔軟に動作できる。またリンク２８と接触した物体へ与える衝撃を抑えることができる。
【００５１】＜第２の実施例＞  続いて、本発明の第２の実施例に係るロボットシステムについて説明する。第１の実施例に係るロボットシステム１０と同一の構成要素同じ符号を付して詳しい説明を省略する場合がある。
【００５２】  本実施例に係るロボットシステムのロボット制御装置１１４は、図８に示すように、第１の退避方向ベクトル計算部４８に代えて、第２の退避方向ベクトル計算部１４８を有している。  この第２の退避方向ベクトル計算部１４８は、図９に示すように、退避方向ベクトルｎ（図１０参照）を計算できる接触部法線ベクトル計算部１４８ａと、移動ベクトルｍを計算できる接触点移動ベクトル計算部１４８ｂと、移動ベクトルｍに基づいて、退避方向ベクトルｎを補正できる退避方向ベクトル補正部１４８ｃとを有している。
【００５３】  接触部法線ベクトル計算部１４８ａは、接触位置Ｐｃに応じ、リンク２８が移動する方向を退避方向ベクトルｎとして計算できる。退避方向ベクトルｎは、予め決められており、例えば、接触位置におけるリンク２８の表面の法線方向の単位ベクトルである。
【００５４】  接触点移動ベクトル計算部１４８ｂは、各サーボモータＳＶＭのエンコーダ値Ｐｆｂに基づいて計算される各関節軸ＡＸの角度と、接触位置決定部４６が求めた接触位置Ｐｃとから、接触位置Ｐｃが実際に移動する方向を表すベクトルを移動ベクトルｍとして計算できる。
【００５５】  退避方向ベクトル補正部１４８ｃは、接触部法線ベクトル計算部１４８ａが計算した退避方向ベクトルｎと、接触点移動ベクトル計算部１４８ｂが計算した移動ベクトルｍとの差のベクトルｖｄを計算することができる。  退避方向ベクトル補正部１４８ｃは、更に、この差のベクトルｖｄと退避方向ベクトルｎとを合成したベクトルを新たな退避方向ベクトルｎｃとして計算できる。  即ち、第２の退避方向ベクトル計算部１４８は、図１０に示すように、リンク２８が予め決められた退避方向ベクトルｎの方向へ移動できるように、各サーボモータＳＶＭのエンコーダ値Ｐｆｂに基づいて、接触部法線ベクトル計算部１４８ａが計算した退避方向ベクトルｎを補正して、補正された退避方向ベクトルｎｃを計算することができる。
【００５６】  本実施例に係るロボットシステムによれば、本実施例の構成を有していない場合に比べ、より正確に予め決められた退避方向にリンク２８を移動させることができる。
【００５７】  本発明は、前述の実施例に限定されるものではなく、本発明の要旨を変更しない範囲での変更は可能である。例えば、前述の実施例や変形例の一部又は全部を組み合わせて発明を構成する場合も本発明の技術的範囲に含まれる。  ロボットは、移動部と、移動部の上部に設けられた胴体部と、胴体部の両側部に設けられた腕部とを有するロボットに限定されるものではない。ロボットは、例えば、関節軸により連結されたリンクを有する産業用ロボットであってもよい。  また、本発明は、前述の実施例にて示した腕部２６に関してのみ適用されるものではない。本発明は、例えば、胴体部２４と移動部２２の間の関節軸ＡＸＷに関して適用することもできる。
【符号の説明】
【００５８】１０：ロボットシステム、１２：ロボット、１４：ロボット制御装置、２２：移動部、２４：胴体部、２６：腕部、２８：リンク、３０：接触センサ、３２：外皮、４２：指令生成部、４４：センサ信号入力部、４６：接触位置決定部、４８：第１の退避方向ベクトル計算部、５０：アシストトルク計算部、５０ａ：アシスト力決定部、５０ｂ：アシスト力ベクトル計算部、５０ｃ：トルク計算部、５０ｄ：重み計算部、５０ｘ：アシストトルク計算部、５２：柔軟制御部、５２ａ：位置速度制御部、５２ｂ：トルク制限値計算部、５２ｃ：トルク制限部、５２ｄ：重力補償トルク計算部、１１４ロボット制御装置、１４８：第２の退避方向ベクトル計算部、１４８ａ：接触部法線ベクトル計算部、１４８ｂ：接触点移動ベクトル計算部、１４８ｃ：退避方向ベクトル補正部、ＥＮＣ：エンコーダ、ＳＶＡ：サーボアンプ、ＳＶＭ：サーボモータ、ＡＸ：関節軸
【図１】
2013027250.tif 000003
【図２】
2013027250.tif 000004
【図３】
2013027250.tif 000005
【図４】
2013027250.tif 000006
【図５】
2013027250.tif 000007
【図６】
2013027250.tif 000008
【図７】
2013027250.tif 000009
【図８】
2013027250.tif 000010
【図９】
2013027250.tif 000011
【図１０】
2013027250.tif 000012
【国際調査報告】
2013027250.tif 000013
2013027250.tif 000014
2013027250.tif 000015
2013027250.tif 000016
2013027250.tif 000017
2013027250.tif 000018