JP6423815B2 - 人協働型のロボットシステム - Google Patents

Description

本発明は、作業員と協働で作業を行う人協働型のロボットシステムに関する。

従来、作業員がロボットに加えた操作力を検出し、該操作力に従って動作する人協働型のロボットシステムが知られている（例えば、特許文献１）。

特開２０１５−１９９１７４号公報

上述のような人協働型のロボットシステムにおいて、作業員がロボットに操作力を加えて該ロボットを動作させているときに、ロボットまたは該ロボットが運搬しているワークが外部の物体と不意に接触する可能性がある。従来、ロボットまたはワークと外部の物体との接触を確実に検出可能とする技術が求められる。

本発明の一態様において、作業員と協働で作業を行う人協働型のロボットシステムは、操作部を有するロボットと、操作部に加えられた操作力を検出する操作力検出部と、操作力検出部が検出した操作力に従ってロボットを動作させる動作制御部とを備える。

また、ロボットシステムは、動作制御部がロボットを動作させているときに該ロボットに掛かる外力を検出する外力検出部を備える。ここで、外力は、操作力と、ロボットが外部の物体と接触したときに該物体から該ロボットに掛かる接触力との和である。

また、ロボットシステムは、外力検出部が検出した外力から、操作力検出部が検出した操作力を減算して、接触力を算出する接触力算出部を備える。接触力算出部は、操作力検出部が検出した操作力、または、外力検出部が検出した外力からノイズ成分を除去するフィルタリングを行ってもよい。

動作制御部は、接触力算出部が算出した接触力が予め定められた閾値を超えたときに、ロボットの動作を停止してもよい。動作制御部は、接触力算出部が算出した接触力に応じて、ロボットの動作速度を変化させてもよい。

動作制御部は、ロボットの動作を停止した後に操作力検出部が検出した操作力の方向が、接触力を減少させることができる方向である場合にのみ、該操作力に従ってロボットを動作させてもよい。

一実施形態に係るロボットシステムの概略図である。 図１に示すロボットシステムのブロック図である。 図１に示す第１の力センサの拡大図である。 図２に示す第２の力センサの拡大図である。 図１に示すロボットシステムの動作フローの一例を示すフローチャートである。 図５に示すステップＳ７のフローの一例を示すフローチャートである。 図１に示すロボットシステムの動作フローの他の例を示すフローチャートである。 他の実施形態に係るロボットシステムの概略図である。 図８に示すロボットシステムのブロック図である。 図８に示す第１の力センサの拡大図である。 さらに他の実施形態に係るロボットシステムの概略図である。 さらに他の実施形態に係るロボットシステムの概略図である。 図１２に示すロボットシステムのブロック図である。

以下、本発明の実施の形態を図面に基づいて詳細に説明する。まず、図１および図２を参照して、本発明の一実施形態に係るロボットシステム１０について説明する。ロボットシステム１０は、作業員Ａと協働でワークＷ_１をワークＷ_２に嵌め入れる作業を行うためのロボットシステムである。

ロボットシステム１０は、ロボット１２と、該ロボット１２を制御するロボット制御部１４とを備える。ロボット制御部１４は、ＣＰＵ（中央演算処理装置）および記憶部（ともに図示せず）等を有し、ロボット１２の各構成要素を直接的または間接的に制御する。

ロボット１２は、垂直多関節型ロボットであって、ロボットベース１８、旋回胴２０、ロボットアーム２２、ロボットハンド２４、および操作部３８を有する。ロボットベース１８は、ワークセルの床Ｂに固定された固定プレート１６に対して移動不能に設置されている。

旋回胴２０は、ロボットベース１８に垂直な軸周りに旋回可能に取り付けられている。ロボットアーム２２は、旋回胴２０に回転可能に取り付けられた上腕部２６と、該上腕部２６の先端に回動可能に取り付けられた前腕部２８とを有する。

ロボットハンド２４は、手首部３０を介して、前腕部２８の先端に取り付けられている。ロボットハンド２４は、アダプタ３４と、該アダプタ３４に開閉可能に設けられた複数の指部３６とを有する。ロボットハンド２４は、指部３６を開閉することによって、ワークＷ_１を把持したり、解放したりすることができる。

操作部３８は、作業員Ａが把持し易い形状を有するハンドルであって、アダプタ３４に固定されている。操作部３８には、スイッチ４４が設置されている。スイッチ４４は、ロボット制御部１４に電気的に接続されており、作業員Ａがスイッチ４４をＯＮにした場合、ハンドガイドＯＮ信号をロボット制御部１４に送信する。

ロボット１２は、複数のサーボモータ３２（図２）を有する。サーボモータ３２は、旋回胴２０、ロボットアーム２２、および手首部３０にそれぞれ内蔵され、ロボット制御部１４からの指令に応じて、これら要素を回動軸周りに回動させる。

ロボットシステム１０は、第１の力センサ４０および第２の力センサ４２をさらに備える。第１の力センサ４０は、固定プレート１６とロボットベース１８との間に介挿されている。第１の力センサ４０は、複数の歪みゲージを有する６軸力覚センサから構成され、第１の力センサ４０に生じた歪みに応じた出力信号を、ロボット制御部１４に送信する。

ロボット制御部１４は、第１の力センサ４０から受信した出力信号をフィルタリングし、該出力信号からノイズ成分を除去する。例えば、ロボット制御部１４は、第１の力センサ４０からの出力信号に、ローパスフィルタ、算術平均、重みづけ平均、ＦＩＲフィルタ、またはＩＩＲフィルタ等の公知の信号処理を行うことによって、該出力信号からノイズ成分を除去する。

ロボット制御部１４は、例えば図３中の直交座標系に示すように、第１の力センサ４０に対して第１のセンサ座標系を設定する。ロボット制御部１４は、第１の力センサ４０からの出力信号に基づいて、図３に示す第１のセンサ座標系のｘ軸、ｙ軸、ｚ軸の方向の力と、ｘ軸、ｙ軸、ｚ軸周りのモーメントとを、それぞれ算出する。こうして、ロボット制御部１４は、第１の力センサ４０に作用する全ての力の合力を算出できる。

その一方で、ロボット制御部１４は、ロボット１２の質量と、該ロボット１２に取り付けられた部材の質量と、ロボットハンド２４が把持するワークＷ_１等の質量と、ロボット１２が動作することよって生じる慣性力とに起因して第１の力センサ４０に作用する力（本稿では「内力」として言及する）を算出する。

この内力は、ロボット１２の各構成要素の質量、ロボット１２の姿勢、およびロボット１２の各構成要素の動作速度を、運動方程式に代入することによって、算出できる。そして、ロボット制御部１４は、第１の力センサ４０から得られた合力から、算出した内力を減算することによって、ロボット１２の任意の部位（ロボットベース１８、旋回胴２０、上腕部２６、前腕部２８、手首部３０、ロボットハンド２４、またはロボットハンド２４が把持するワークＷ_１）に掛かる外力ＥＦを算出する。

この外力ＥＦは、作業員Ａによって操作部３８に加えられた操作力ＨＦと、ロボット１２の任意の部位が外部の物体と接触したときに該物体から該ロボット１２に掛かる接触力ＣＦとの和である。

このように、本実施形態においては、第１の力センサ４０およびロボット制御部１４は、ロボット１２の任意の部位に掛かる外力ＥＦを検出する外力検出部４６を構成する。

外力検出部４６は、ロボット１２の任意の部位（ロボットベース１８、旋回胴２０、上腕部２６、前腕部２８、手首部３０、ロボットハンド２４、またはロボットハンド２４が把持するワークＷ_１）に掛かる外力ＥＦを検出できる。

例えば、ロボットハンド２４のアダプタ３４に掛けられた外力は、手首部３０、ロボットアーム２２、旋回胴２０、およびロボットベース１８を通して第１の力センサ４０に伝達し、該第１の力センサ４０によって検出される。

このように、本実施形態においては、外力検出部４６は、ロボット１２の任意の部位に掛けられた外力を、該外力に起因して固定プレート１６とロボットベース１８との間に作用する力として、検出する。

一方、第２の力センサ４２は、操作部３８とアダプタ３４との間に介挿されている。第２の力センサ４２は、６軸力覚センサから構成され、第２の力センサ４２に生じた歪みに応じた出力信号を、ロボット制御部１４に送信する。

ロボット制御部１４は、第２の力センサ４２からの出力信号を、ローパスフィルタ、算術平均、重みづけ平均、ＦＩＲフィルタ、またはＩＩＲフィルタ等の手法を用いてフィルタリングし、該出力信号からノイズ成分を除去する。

ロボット制御部１４は、例えば図４中の直交座標系に示すように、第２の力センサ４２に対して第２のセンサ座標系を設定する。この第２のセンサ座標系の位置（原点位置）および姿勢（ｘ軸、ｙ軸、ｚ軸の方向）は、ロボットハンド２４の位置および姿勢とともに変化することになる。

したがって、ロボット制御部１４は、ロボットハンド２４の位置および姿勢を変化させる毎に、第２のセンサ座標系の位置および姿勢を更新する。

ロボット制御部１４は、第２の力センサ４２からの出力信号に基づいて、図４に示す第２のセンサ座標系のｘ軸、ｙ軸、ｚ軸の方向の力と、ｘ軸、ｙ軸、ｚ軸周りのモーメントとを、それぞれ算出する。こうして、ロボット制御部１４は、操作部３８に加えられた操作力ＨＦを算出する。

このように、本実施形態においては、第２の力センサ４２およびロボット制御部１４は、操作部３８に加えられた操作力ＨＦを検出する操作力検出部４８を構成する。

次に、図５を参照して、ロボットシステム１０の動作フローの一例について説明する。図５に示すフローは、ロボット制御部１４がスイッチ４４からハンドガイドＯＮ信号を受信したときに、開始する。

ステップＳ１において、ロボット制御部１４は、第２の力センサ４２に指令を送り、操作部３８に加えられた操作力ＨＦの検出を開始する。具体的には、ロボット制御部１４は、第２の力センサ４２に検出開始指令を送る。

第２の力センサ４２は、ロボット制御部１４から検出開始指令を受信すると、該第２の力センサ４２に生じた歪みに応じた出力信号を、周期Ｔ（例えば０．５秒）でロボット制御部１４へ送信する。

ロボット制御部１４は、第２の力センサ４２から送信される出力信号から操作部３８に加えられた操作力ＨＦを算出し、ロボット制御部１４に内蔵された記憶部に記憶する。こうして、操作力ＨＦが周期Ｔで検出される。

ステップＳ２において、ロボット制御部１４は、第１の力センサ４０に指令を送り、ロボット１２に掛かる外力ＥＦの検出を開始する。具体的には、ロボット制御部１４は、第１の力センサ４０に検出開始指令を送る。

第１の力センサ４０は、ロボット制御部１４から検出開始指令を受信すると、該第１の力センサ４０に生じた歪みに応じた出力信号を、周期Ｔでロボット制御部１４へ送信する。ロボット制御部１４は、第１の力センサ４０からの出力信号に基づいて、第１の力センサ４０に作用する合力を算出する。

その一方で、ロボット制御部１４は、合力の算出と同期して、そのときの内力を算出する。そして、ロボット制御部１４は、合力から内力を減算することによってロボット１２に掛かる外力ＥＦを算出し、記憶部に記憶する。こうして、外力ＥＦが周期Ｔで検出される。

ステップＳ３において、ロボット制御部１４は、直近に検出した操作力ＨＦの大きさが、予め定められた閾値α以上（｜ＨＦ｜≧α）であるか否かを判定する。この閾値αは、操作力ＨＦに対して予め定められ、記憶部に記憶される。

ロボット制御部１４は、操作力ＨＦの大きさが閾値α以上である（すなわち、ＹＥＳ）と判定した場合、ステップＳ４へ進む。一方、ロボット制御部１４は、操作力ＨＦの大きさが閾値αよりも小さい（すなわち、ＮＯ）と判定した場合、ステップＳ８へ進む。

ステップＳ４において、ロボット制御部１４は、ハンドガイド動作を実行する。具体的には、ロボット制御部１４は、直近に検出した操作力ＨＦを構成する、第２のセンサ座標系のｘ軸、ｙ軸、ｚ軸の方向の力と、ｘ軸、ｙ軸、ｚ軸周りのモーメントとに応じて、ロボット１２を動作させるための速度指令を生成する。

そして、ロボット制御部１４は、生成した速度指令を各サーボモータ３２へ送信し、ロボットハンド２４を、作業員Ａが与えた操作力ＨＦの方向へ動作させる。その結果、ロボット１２は、作業員Ａが操作部３８に加えた操作力ＨＦに従って動作することになる。

このように、本実施形態においては、ロボット制御部１４は、操作力ＨＦに従ってロボット１２を動作させる動作制御部５０（図２）としての機能を有する。

ステップＳ５において、ロボット制御部１４は、ロボット１２に掛かる接触力ＣＦを算出する。まず、ロボット制御部１４は、直近に検出した操作力ＨＦから、該操作力ＨＦに起因して第１の力センサ４０に作用する力ＨＦ’を算出する。

一例として、ロボット制御部１４は、直近に検出した操作力ＨＦを構成する、第２のセンサ座標系のｘ軸、ｙ軸、ｚ軸の方向の力に、第２のセンサ座標系から第１のセンサ座標系への座標変換行列を乗算し、第１のセンサ座標系の力に変換する。これにより、操作力ＨＦに起因して第１の力センサ４０に作用する力ＨＦ’の、 第１のセンサ座標系での値を算出できる。

次いで、ロボット制御部１４は、直近に検出した外力ＥＦから、上述のように算出した力ＨＦ’を減算する。これにより、第１の力センサ４０にて検出された外力ＥＦから、操作力ＨＦの成分が除去されて、ロボット１２の任意の部位が外部の物体と接触したときに該物体から該ロボット１２に掛かる接触力ＣＦを算出することができる。

このように、本実施形態においては、ロボット制御部１４は、接触力ＣＦを算出する接触力算出部５２（図２）としての機能を有する。

ステップＳ６において、ロボット制御部１４は、ステップＳ５にて算出した接触力ＣＦが、予め定められた閾値β以上（すなわち、ＣＦ≧β）であるか否かを判定する。この閾値βは、接触力ＣＦに対して予め定められ、記憶部に記憶される。

ロボット制御部１４は、接触力ＣＦが閾値β以上である（すなわちＹＥＳ）と判定した場合、ステップＳ７へ進む。一方、ロボット制御部１４は、接触力ＣＦが閾値βよりも小さい（すなわちＮＯ）と判定した場合、ステップＳ８へ進む。

ステップＳ７において、ロボット制御部１４は、緊急停止スキームを実行する。このステップＳ７について、図６を参照して説明する。

ステップＳ１１において、ロボット制御部１４は、ロボット１２の動作を停止する。具体的には、ロボット制御部１４は、動作制御部５０として機能して、各サーボモータ３２へ指令を送り、ロボット１２を停止する。

ステップＳ１２において、ロボット制御部１４は、第１の警告を生成する。一例として、ロボット制御部１４は、音声データまたは文字・画像データの形態で、「ロボットが外部の物体と干渉しています」との警告信号を生成する。そして、ロボット制御部１４は、スピーカまたはディスプレイ（図示せず）を通して、作業員Ａに対して警告を出力する。

ステップＳ１３において、ロボット制御部１４は、ロボット１２の許容動作方向を決定する。ここで、許容動作方向とは、ロボット１２を該許容動作方向に移動させた場合に、ステップＳ５で算出した接触力ＣＦを減少させることができる（すなわち、外部の物体と接触したロボット１２またはワークＷ_１が該物体から離れることになる）動作方向を意味する。

一例として、ロボット制御部１４は、ステップＳ４の実行中にロボット１２の各可動要素（旋回胴２０、上腕部２６、前腕部２８、手首部３０）の位置指令値を記憶部に順次記憶していく。

そして、ロボット制御部１４は、ステップＳ６にてＹＥＳと判定した時点の直前（または直後）に記憶された位置指令値Ｐ_ｎと、該位置指令値Ｐ_ｎの直前に記憶された位置指令値Ｐ_ｎ−１を記憶部から順次読み出し、ロボット１２の各可動要素を位置指令値Ｐ_ｎから位置指令値Ｐ_ｎ−１へ移動させる動作方向を、許容動作方向として決定する。

また、他の例として、ロボット制御部１４は、外部の物体がロボット１２の特定の部位（例えば、ロボットハンド２４）に接触したものと仮定して、ステップＳ５にて算出した接触力ＣＦ（または、第１の力センサ４０の出力信号）に基づいて、該物体が該特定の部位に当接した方向Ｄ_１を算出し、該方向Ｄ_１を許容動作方向として決定する。

ステップＳ１４において、ロボット制御部１４は、直近に検出した操作力ＨＦの大きさが閾値α以上（｜ＨＦ｜≧α）であるか否かを判定する。具体的には、操作力検出部４８は、ステップＳ１１の終了後も継続して、操作力ＨＦを周期Ｔで検出する。

ロボット制御部１４は、ステップＳ１１の終了後、直近に検出した操作力ＨＦの大きさが閾値α以上である（すなわち、ＹＥＳ）と判定した場合、ステップＳ１５へ進む。一方、ロボット制御部１４は、直近に検出した操作力ＨＦの大きさが閾値αよりも小さい（すなわち、ＮＯ）と判定した場合、ステップＳ１７へ進む。

ステップＳ１５において、ロボット制御部１４は、ステップＳ１４にて閾値α以上であると判定した操作力ＨＦの方向が、ステップＳ１３にて決定した許容動作方向と一致しているか否かを判定する。

例えば、ロボット制御部１４は、操作力ＨＦの方向が、ステップＳ１３にて決定した許容動作方向の±５°の範囲に収まる場合に、操作力ＨＦの方向が許容動作方向と一致していると判定する。

ロボット制御部１４は、操作力ＨＦの方向が許容動作方向と一致している（すなわちＹＥＳ）と判定した場合、図５のステップＳ４へ進み、該操作力ＨＦに従って、ハンドガイド動作を再度実行する。一方、ロボット制御部１４は、操作力ＨＦの方向が許容動作方向と一致していない（すなわちＮＯ）と判定した場合、ステップＳ１６へ進む。

ステップＳ１６において、ロボット制御部１４は、第２の警告を生成する。一例として、ロボット制御部１４は、音声データまたは文字・画像データの形態で、「その方向へは移動できません」との警告信号を生成する。そして、ロボット制御部１４は、スピーカまたはディスプレイを通して、作業員Ａに警告を出力する。

ステップＳ１７において、ロボット制御部１４は、スイッチ４４がＯＦＦにされたか否かを判定する。具体的には、ロボット制御部１４は、スイッチ４４からのハンドガイドＯＮ信号がゼロとなったか否かを判定する。

ロボット制御部１４は、スイッチ４４がＯＦＦにされた（すなわちＹＥＳ）と判定した場合、図６に示すステップＳ７を終了し、以って図５に示すフローを終了する。一方、ロボット制御部１４は、スイッチ４４がＯＮである（すなわちＮＯ）と判定した場合、ステップＳ１４へ戻る。

再度、図５を参照して、ステップＳ８において、ロボット制御部１４は、上述のステップＳ１７と同様に、スイッチ４４がＯＦＦにされたか否かを判定する。ロボット制御部１４は、スイッチ４４がＯＦＦにされた（すなわちＹＥＳ）と判定した場合、ステップＳ９へ進む。一方、ロボット制御部１４は、スイッチ４４がＯＮである（すなわちＮＯ）と判定した場合、ステップＳ３へ戻る。

ステップＳ９において、ロボット制御部１４は、上述のステップＳ１１と同様に、ロボット１２の動作を停止する。そして、ロボット制御部１４は、図５に示すフローを終了する。

上述したように、本実施形態においては、ロボット制御部１４は、ステップＳ４にてハンドガイド動作を実行しているときに、外力検出部４６が検出した外力ＥＦから、作業員Ａによって与えられた操作力ＨＦの成分を除去し、ロボット１２（またはワークＷ_１）と外部の物体との接触に起因する接触力ＣＦを算出している。

この構成によれば、ハンドガイド動作を実行しているときに接触力ＣＦをリアルタイムで監視することができるので、ロボット１２（またはワークＷ_１）と外部の物体との接触を確実に検出することができる。

また、本実施形態においては、ステップＳ６にてＹＥＳと判定した場合、ロボット制御部１４は、ロボット１２の動作を停止している（ステップＳ１１）。この構成によれば、ハンドガイド動作を実行しているときにロボット１２またはワークＷ_１が外部の物体と接触した場合に、ロボット１２またはワークＷ_１に過大な力が掛かるのを防止することができるとともに、作業員Ａの安全も確保することができる。

また、本実施形態においては、ロボット制御部１４は、ロボット１２の動作を停止した後、操作力ＨＦの方向が許容動作方向に一致している場合にのみ、ステップＳ４のハンドガイド動作を再開する。

この構成によれば、ロボット１２（またはワークＷ_１）が外部の物体と接触した場合に、作業員Ａによるハンドガイド動作によって、ロボット１２（またはワークＷ_１）を外部の物体にさらに強い力で押し付けてしまう動作を、確実に防止することができる。

また、本実施形態においては、ロボット制御部１４は、ロボット１２の動作を停止したときに、その旨を示す第１の警告を出力している（ステップＳ１２）。この構成によれば、作業員Ａは、ロボット１２（またはワークＷ_１）が外部の物体と接触したことを、直観的に認識することができる。

また、ロボット制御部１４は、ロボット１２の動作を停止した後に、許容動作方向と異なる方向へ操作力ＨＦが加えられたことを検出（ステップＳ１５にてＮＯと判定）した場合に、その旨を示す第２の警告を出力している。

この構成によれば、作業員Ａは、ロボット１２（またはワークＷ_１）を外部の物体に押し付けるようにロボット１２を操作しようとしていることを、直観的に認識することができる。

また、本実施形態においては、第１の力センサ４０および第２の力センサ４２からの出力信号をフィルタリングし、ノイズ成分を除去している。この構成によれば、第１の力センサ４０および第２の力センサ４２からの出力信号の瞬時的変動による影響を受けることなく、外力ＥＦおよび操作力ＨＦを安定して検出できるので、接触力ＣＦの検出精度を向上させることができる。

次に、図７を参照して、ロボットシステム１０の動作フローの他の例について説明する。なお、図７に示すフローにおいて、図５に示すフローと同様のプロセスには同じステップ番号を付し、詳細な説明を省略する。

本実施形態に係る動作フローの開始時においては、ステップＳ４にてハンドガイド動作を実行するときに各サーボモータ３２へ送信される速度指令は、初期設定として第１の速度指令Ｖ_１に設定されている。この第１の速度指令Ｖ_１は、ロボット１２を、第１の速度で動作させるための速度指令である。

ステップＳ５の後、ステップＳ２１において、ロボット制御部１４は、ステップＳ５にて算出した接触力ＣＦが、予め定められた閾値β_１以上であるか否かを判定する。この閾値β_１は、接触力ＣＦに対して予め定められ、記憶部に記憶される。

ロボット制御部１４は、接触力ＣＦが閾値β_１以上である（すなわちＹＥＳ）と判定した場合、ステップＳ２３へ進む。一方、ロボット制御部１４は、接触力ＣＦが閾値β_１よりも小さい（すなわちＮＯ）と判定した場合、ステップＳ２２へ進む。

ステップＳ２２において、ロボット制御部１４は、ハンドガイド動作を実行するときに各サーボモータ３２に送信される速度指令を第１の速度指令Ｖ_１に設定する。仮に、ステップＳ２２の開始時に、速度指令が既に第１の速度指令Ｖ_１に設定されていた場合、ロボット制御部１４は、速度指令の設定を第１の速度指令Ｖ_１に維持する。

ステップＳ２３において、ロボット制御部１４は、ステップＳ５にて算出した接触力ＣＦが、予め定められた閾値β_２以上であるか否かを判定する。この閾値β_２は、上述の閾値β_１よりも大きな値（すなわち、β_２＞β_１）として予め定められ、記憶部に記憶される。

ロボット制御部１４は、接触力ＣＦが閾値β_２以上である（すなわちＹＥＳ）と判定した場合、ステップＳ２５へ進む。一方、ロボット制御部１４は、接触力ＣＦが閾値β_２よりも小さい（すなわちＮＯ）と判定した場合、ステップＳ２４へ進む。

ステップＳ２４において、ロボット制御部１４は、ハンドガイド動作を実行するときに各サーボモータ３２に送信される速度指令を、第２の速度指令Ｖ_２に設定する。この第２の速度指令Ｖ_２は、ロボット１２を、第１の速度よりも遅い第２の速度で動作させるための速度指令である。

ステップＳ２５において、ロボット制御部１４は、ステップＳ５にて算出した接触力ＣＦが、予め定められた閾値β_３以上であるか否かを判定する。この閾値β_３は、上述の閾値β_２よりも大きな値（すなわち、β_３＞β_２）として予め定められ、記憶部に記憶される。

ロボット制御部１４は、接触力ＣＦが閾値β_３以上である（すなわちＹＥＳ）と判定した場合、ステップＳ７へ進む。一方、ロボット制御部１４は、接触力ＣＦが閾値β_３よりも小さい（すなわちＮＯ）と判定した場合、ステップＳ２６へ進む。

ステップＳ２６において、ロボット制御部１４は、ハンドガイド動作を実行するときに各サーボモータ３２に送信される速度指令を、第３の速度指令Ｖ_３に設定する。この第３の速度指令Ｖ_３は、ロボット１２を、第２の速度よりも遅い第３の速度で動作させるための速度指令である。

このように、本実施形態においては、ロボット制御部１４は、ステップＳ５にて算出した接触力ＣＦに応じて、ロボット１２の動作速度を変化させている。具体的には、ロボット制御部１４は、算出した接触力ＣＦが大きい程、ロボット１２の動作速度を遅くするように、速度指令の設定を変更している。

この構成によれば、接触力ＣＦが比較的に小さい場合は、ロボット１２の動作を停止させずに、その動作速度を減速することによって、ロボット１２（またはワークＷ_１）や作業員Ａが外部の物体と接触することによって過大な力が掛かってしまう虞を低減できる。したがって、作業効率の低下を防止しつつ、ロボット１２（またはワークＷ_１）や作業員Ａの安全を確保できる。

次に、図８および図９を参照して、他の実施形態に係るロボットシステム６０について説明する。なお、以下に説明する種々の実施形態において、既述の実施形態と同様の要素には同じ符号を付し、詳細な説明を省略する。

ロボットシステム６０は、ロボット１２、ロボット制御部６２、第１の力センサ６４、および第２の力センサ４２を備える。ロボット制御部６２は、ＣＰＵおよび記憶部（ともに図示せず）等を有し、ロボット１２の各構成要素を直接的または間接的に制御する。

第１の力センサ６４は、上述の第１の力センサ４０と同様に、複数の歪みゲージを有する６軸力覚センサから構成され、第１の力センサ６４に生じた歪みに応じた出力信号を、ロボット制御部６２に送信する。第１の力センサ６４は、上腕部２６に組み込まれている。

ロボット制御部６２は、例えば図１０中の直交座標系に示すように、第１の力センサ６４に対して第１のセンサ座標系を設定する。ロボット制御部６２は、第１の力センサ６４からの出力信号に基づいて、図１０に示す第１のセンサ座標系のｘ軸、ｙ軸、ｚ軸の方向の力と、ｘ軸、ｙ軸、ｚ軸周りのモーメントとを、それぞれ算出する。こうして、ロボット制御部６２は、第１の力センサ６４に作用する合力を算出する。

その一方で、ロボット制御部６２は、ロボット１２の構成要素のうち、第１の力センサ６４の先端側に位置するもの（すなわち、上腕部２６の先端側の部分２６ａ、前腕部２８、手首部３０、およびロボットハンド２４。以下、「先端側要素」として言及する）の質量と、ロボットハンド２４が把持するワークＷ_１等の質量と、ロボット１２が動作することよって生じる慣性力とに起因して第１の力センサ６４に作用する内力を算出する。

この内力は、ロボット１２の先端側要素の質量、ロボット１２の姿勢、およびロボット１２の各構成要素の動作速度を、公知の運動方程式に代入することによって、算出できる。

そして、ロボット制御部６２は、第１の力センサ６４から得られた合力から、算出した内力を減算することによって、ロボット１２の先端側要素に掛かる外力ＥＦを算出する。この外力ＥＦは、作業員Ａによって操作部３８に加えられた操作力ＨＦと、ロボット１２の先端側要素が外部の物体と接触したときに該物体から該先端側要素に掛かる接触力ＣＦとの和である。

このように、本実施形態においては、第１の力センサ６４およびロボット制御部６２は、ロボット１２の先端側要素に掛かる外力ＥＦを検出する外力検出部６６を構成する。

外力検出部６６は、先端側要素の任意の部位（上腕部２６の部分２６ａ、前腕部２８、手首部３０、またはロボットハンド２４）に掛かけられた外力を、該外力に起因して第１の力センサ６４に作用する力として検出する。

一方、第２の力センサ４２およびロボット制御部６２は、操作部３８に加えられた操作力ＨＦを検出する操作力検出部６８を構成する。

次に、図５〜図７を参照して、ロボットシステム６０の動作フローについて説明する。ロボット制御部６２は、図５または図７に示す動作フローを実行する。ここで、ロボットシステム６０の動作は、上述のロボットシステム１０と、ステップＳ２およびＳ５において相違する。

具体的には、ステップＳ２において、ロボット制御部６２は、第１の力センサ６４に指令を送り、ロボット１２の先端側要素に掛かる外力ＥＦの検出を開始する。具体的には、ロボット制御部６２は、第１の力センサ６４に検出開始指令を送る。

第１の力センサ６４は、ロボット制御部６２から検出開始指令を受信すると、該第１の力センサ６４に生じた歪みに応じた出力信号を、周期Ｔでロボット制御部６２へ送信する。ロボット制御部６２は、第１の力センサ６４からの出力信号から、該第１の力センサ６４に作用する合力を算出する。

その一方で、ロボット制御部６２は、合力の算出と同期して、そのときの内力を算出する。そして、ロボット制御部６２は、合力から内力を減算することによってロボット１２の先端要素に掛かる外力ＥＦを算出し、記憶部に記憶する。

ステップＳ５において、ロボット制御部６２は、ロボット１２の先端側要素に掛かる接触力ＣＦを算出する。まず、ロボット制御部６２は、直近に検出した操作力ＨＦから、該操作力ＨＦに起因して第１の力センサ６４に作用する力ＨＦ’を算出する。

例えば、ロボット制御部６２は、直近に検出した操作力ＨＦを構成する、第２のセンサ座標系のｘ軸、ｙ軸、ｚ軸の方向の力に、第２のセンサ座標系から第１のセンサ座標系への座標変換行列を乗算し、第１のセンサ座標系の力に変換する。これにより、操作力ＨＦに起因して第１の力センサ６４に作用する力ＨＦ’ の、 第１のセンサ座標系での値を算出できる。

次いで、ロボット制御部６２は、直近に検出した外力ＥＦから、上述のように算出した力ＨＦ’を減算する。これにより、第１の力センサ６４にて検出された外力ＥＦから、操作力ＨＦの成分が除去されて、ロボット１２の先端側要素が外部の物体と接触したときに該物体から該先端側要素に掛かる接触力ＣＦを算出することができる。

このように、本実施形態においては、ロボット制御部６２は、先端側要素に掛かる接触力ＣＦを算出する接触力算出部６９（図９）としての機能を有する。

本実施形態によれば、ハンドガイド動作を実行しているときに、ロボット１２の先端側要素と外部の物体との接触に起因する接触力ＣＦをリアルタイムで監視することができるので、ロボット１２の先端側要素と外部の物体との接触を確実に検出することができる。

次に、図１１を参照して、さらに他の実施形態に係るロボットシステム７０について説明する。このロボットシステム７０は、上述のロボットシステム６０と、以下の点で相違する。すなわち、ロボットシステム７０においては、第１の力センサ７４が手首部３０に取り付けられている。この実施形態においては、ロボット制御部７２および第１の力センサ７４は、ロボットハンド２４に掛かる外力ＥＦを検出する外力検出部７６を構成する。

なお、上述の実施形態においては、第１の力センサ４０、６４、７４が６軸力覚センサから構成されている場合について述べた。しかしながら、これに限らず、第１の力センサは、トルクセンサから構成されてもよい。

以下、図１２および図１３を参照して、このような実施形態に係るロボットシステム８０について説明する。ロボットシステム８０は、ロボット１２、ロボット制御部８２、第１の力センサ８４、および第２の力センサ４２を備える。ロボット制御部８２は、ＣＰＵおよび記憶部（ともに図示せず）等を有し、ロボット１２の各構成要素を直接的または間接的に制御する。

第１の力センサ８４は、第１のトルクセンサ８６、第２のトルクセンサ８８、第３のトルクセンサ９０、第４のトルクセンサ９２、第５のトルクセンサ９４、および第６のトルクセンサ９６を有する。

第１のトルクセンサ８６は、旋回胴２０を第１の軸Ｏ_１周りに旋回させるサーボモータ３２に内蔵され、第１の軸Ｏ_１周りのトルクτ_１を検出し、ロボット制御部８２へ送信する。なお、第１の軸Ｏ_１は、鉛直方向と平行である。

第２のトルクセンサ８８は、上腕部２６を第２の軸Ｏ_２周りに回動させるサーボモータ３２に内蔵され、第２の軸Ｏ_２周りのトルクτ_２を検出し、ロボット制御部８２へ送信する。

第３のトルクセンサ９０は、前腕部２８を第３の軸Ｏ_３周りに回動させるサーボモータ３２に内蔵され、第３の軸Ｏ_３周りのトルクτ_３を検出し、ロボット制御部８２へ送信する。

第４のトルクセンサ９２は、前腕部２８を第４の軸Ｏ_４周りに回動させるサーボモータ３２に内蔵され、第４の軸Ｏ_４周りのトルクτ_４を検出し、ロボット制御部８２へ送信する。

第５のトルクセンサ９４は、手首部３０を第５の軸Ｏ_５周りに回動させるサーボモータ３２に内蔵され、第５の軸Ｏ_５周りのトルクτ_５を検出し、ロボット制御部８２へ送信する。

第６のトルクセンサ９６は、手首部３０を第６の軸Ｏ_６周りに回動させるサーボモータ３２に内蔵され、第６の軸Ｏ_６周りのトルクτ_６を検出し、ロボット制御部８２へ送信する。

一方、ロボット制御部８２は、ロボット１２の各構成要素の質量と、ロボットハンド２４が把持するワークＷ_１等の質量と、ロボット１２の各構成要素が動作することよって生じる慣性力とに起因して第１〜第６の軸Ｏ_１、Ｏ_２、Ｏ_３、Ｏ_４、Ｏ_５およびＯ_６の周りに作用するトルク（すなわち、「内力」）Ｉτ_１、Ｉτ_２、Ｉτ_３、Ｉτ_４、Ｉτ_５およびＩτ_６、をそれぞれ算出する。

これら内力Ｉτ_１、Ｉτ_２、Ｉτ_３、Ｉτ_４、Ｉτ_５およびＩτ_６は、ロボット１２の各構成要素の質量、ロボット１２の姿勢、およびロボット１２の各構成要素の動作速度を、公知の運動方程式に代入することによって、算出できる。

ロボット制御部８２は、第１〜第６のトルクセンサ８６、８８、９０、９２、９４および９６から得られたトルクτ_１、τ_２、τ_３、τ_４、τ_５およびτ_６から、算出した内力Ｉτ_１、Ｉτ_２、Ｉτ_３、Ｉτ_４、Ｉτ_５およびＩτ_６をそれぞれ減算することによって、ロボット１２に掛かる外力に起因して第１〜第６の軸Ｏ_１、Ｏ_２、Ｏ_３、Ｏ_４、Ｏ_５およびＯ_６の周りに作用するトルク（以下、外力トルクとして言及する）Ｅτ_１、Ｅτ_２、Ｅτ_３、Ｅτ_４、Ｅτ_５およびＥτ_６をそれぞれ算出する。

ロボット制御部８２は、これら外力トルクＥτ_１、Ｅτ_２、Ｅτ_３、Ｅτ_４、Ｅτ_５およびＥτ_６に基づいて、ロボットハンド２４に掛かる外力ＥＦを算出する。この外力ＥＦは、作業員Ａによって操作部３８に加えられた操作力ＨＦと、ロボットハンド２４が外部の物体と接触したときに該物体から該ロボットハンド２４に掛かる接触力ＣＦとの和である。

このように、本実施形態においては、第１の力センサ８４およびロボット制御部８２は、ロボットハンド２４に掛かる外力ＥＦを検出する外力検出部９８を構成する。一方、第２の力センサ４２およびロボット制御部８２は、操作部３８に加えられた操作力ＨＦを検出する操作力検出部１００を構成する。

次に、図５〜図７を参照して、ロボットシステム８０の動作フローについて説明する。ロボット制御部８２は、図５または図７に示す動作フローを実行する。ここで、ロボットシステム８０の動作は、上述のロボットシステム１０と、ステップＳ２およびＳ５において相違する。

具体的には、ステップＳ２において、ロボット制御部８２は、第１の力センサ８４に指令を送り、ロボットハンド２４に掛かる外力ＥＦの検出を開始する。具体的には、ロボット制御部８２は、第１〜第６のトルクセンサ８６、８８、９０、９２、９４および９６に検出開始指令を送る。

第１〜第６のトルクセンサ８６、８８、９０、９２、９４および９６は、ロボット制御部８２から検出開始指令を受信すると、周期Ｔでトルクτ_１、τ_２、τ_３、τ_４、τ_５およびτ_６を検出し、ロボット制御部８２へそれぞれ送信する。

その一方で、ロボット制御部８２は、トルクτ_１、τ_２、τ_３、τ_４、τ_５およびτ_６の取得と同期して、そのときの内力Ｉτ_１、Ｉτ_２、Ｉτ_３、Ｉτ_４、Ｉτ_５およびＩτ_６をそれぞれ算出する。

そして、ロボット制御部８２は、トルクτ_１、τ_２、τ_３、τ_４、τ_５およびτ_６から内力Ｉτ_１、Ｉτ_２、Ｉτ_３、Ｉτ_４、Ｉτ_５およびＩτ_６をそれぞれ減算し、外力トルクＥτ_１、Ｅτ_２、Ｅτ_３、Ｅτ_４、Ｅτ_５およびＥτ_６をそれぞれ算出する。そして、ロボット制御部８２は、外力トルクＥτ_１、Ｅτ_２、Ｅτ_３、Ｅτ_４、Ｅτ_５およびＥτ_６から、ロボットハンド２４に掛かる外力ＥＦを算出し、記憶部に記憶する。

ステップＳ５において、ロボット制御部８２は、ロボットハンド２４に掛かる接触力ＣＦを算出する。具体的には、ロボット制御部８２は、直近に検出した外力ＥＦから、直近に検出した操作力ＨＦを減算する。これにより、第１の力センサ８４によって検出された外力ＥＦから操作力ＨＦが除去されて、ロボットハンド２４が外部の物体と接触したときに該物体から該ロボットハンド２４に掛かる接触力ＣＦを算出することができる。

このように、本実施形態においては、ロボット制御部８２は、ロボットハンド２４に掛かる接触力ＣＦを算出する接触力算出部１０２（図１３）としての機能を有する。

本実施形態によれば、ハンドガイド動作を実行しているときに、ロボットハンド２４（またはワークＷ_１）と外部の物体との接触に起因する接触力ＣＦをリアルタイムで監視することができるので、ロボットハンド２４（またはワークＷ_１）と外部の物体との接触を確実に検出することができる。

なお、上述の実施形態においては、ロボット１２が垂直多関節型ロボットである場合について述べた。しかしながら、これに限らず、ロボット１２は、例えばスカラ型またはパラレルリンク型といった、如何なる型のロボットであってもよい。

また、図６に示す動作フローにおいては、ステップＳ１１においてロボット１２を停止させる場合について述べた。しかしながら、これに限らず、例えば、ロボット制御部１４、６２、７２、８２は、ステップＳ１１を実行することなく、ステップＳ１２で作業員Ａに警告を発するのみであってもよい。

また、ロボット制御部１４、６２、７２、８２は、ステップＳ５にて算出した接触力ＣＦを、リアルタイムでディスプレイに表示させてもよい。この場合において、ロボット制御部１４、６２、７２、８２は、図６に示すステップＳ１１を実行せずに、算出した接触力ＣＦをディスプレイに表示して接触力ＣＦを作業員Ａに報知するだけでもよい。

また、上述の実施形態においては、力センサ４０、４２、６４および７４が歪みゲージを有する６軸力覚センサから構成された場合について述べた。しかしながら、これに限らず、力センサ４０、４２、６４および７４は、例えば静電容量の変化に基づいて力を検出するセンサ、または光学的に歪みを検出するセンサであってもよい。

また、ロボットシステム１０にて用いた、第１の力センサ４０および第２の力センサ４２の出力信号からノイズ成分を除去するフィルタリングは、ロボットシステム６０、７０、および８０にも同様に適用できる。

以上、発明の実施形態を通じて本発明を説明したが、上述の実施形態は、特許請求の範囲に係る発明を限定するものではない。また、本発明の実施形態の中で説明されている特徴を組み合わせた形態も本発明の技術的範囲に含まれ得るが、これら特徴の組み合わせの全てが、発明の解決手段に必須であるとは限らない。さらに、上述の実施形態に、多様な変更または改良を加えることが可能であることも当業者に明らかである。

また、特許請求の範囲、明細書、および図面中において示した装置、システム、プログラム、および方法における動作、手順、ステップ、工程、および段階等の各処理の実行順序は、特段「より前に」、「先立って」等と明示しておらず、また、前の処理の出力を後の処理で用いるのでない限り、任意の順序で実現しうることに留意すべきである。特許請求の範囲、明細書、および図面中の動作フローに関して、便宜上「まず、」、「次に、」、「次いで」等を用いて説明したとしても、この順で実施することが必須であることを意味するものではない。

１０，６０，７０，８０ ロボットシステム
１２ ロボット
１４，６２，７２，８２ ロボット制御部
４０，４２，６４，７４，８４ 力センサ

Claims (5)

1. 作業員と協働で作業を行う人協働型のロボットシステムであって、
   操作部を有するロボットと、
   前記操作部に加えられた操作力を検出する操作力検出部と、
   前記操作力検出部が検出した前記操作力に従って前記ロボットを動作させる動作制御部と、
   前記動作制御部が前記ロボットを動作させているときに該ロボットに掛かる外力を検出する外力検出部であって、前記外力は、前記操作力と、前記ロボットが外部の物体と接触したときに該物体から該ロボットに掛かる接触力との和である、外力検出部と、
   前記外力検出部が検出した前記外力から、前記操作力検出部が検出した前記操作力を減算して、前記接触力を算出する接触力算出部と、を備える、ロボットシステム。
2. 前記接触力算出部は、前記操作力検出部が検出した前記操作力、または、前記外力検出部が検出した前記外力からノイズ成分を除去するフィルタリングを行う、請求項１に記載のロボットシステム。
3. 前記動作制御部は、前記接触力算出部が算出した前記接触力が予め定められた閾値を超えたときに、前記ロボットの動作を停止する、請求項１または２に記載のロボットシステム。
4. 前記動作制御部は、前記ロボットの動作を停止した後に前記操作力検出部が検出した前記操作力の方向が、前記接触力を減少させることができる方向である場合にのみ、該操作力に従って前記ロボットを動作させる、請求項３に記載のロボットシステム。
5. 前記動作制御部は、前記接触力算出部が算出した前記接触力に応じて、前記ロボットの動作速度を変化させる、請求項１〜４のいずれか１項に記載のロボットシステム。

Images