JP6851837B2 - 制御装置、ロボットシステム、制御方法、プログラム、記録媒体及び物品の製造方法 - Google Patents

Description

本発明は、ロボットの制御に関する。

ロボットは、位置制御と力制御のいずれかで制御される。位置制御は、目標とする位置に位置指令値に基づきロボットを制御するものである。力制御は、位置ベース力制御、即ちアドミッタンス制御と、トルクベース力制御とがある。位置ベース力制御は、ロボットの先端部に設けられた力センサの値に基づき、ワークに倣うような位置指令値を求め、位置指令値に基づきロボットを制御する。位置ベース力制御では、位置制御をマイナーループとして利用する。そのため、位置ベース力制御の応答性は、位置制御の応答性よりも低下する。一般的に位置制御の応答周波数は、ロボットの固有振動数に制約される。特に、接触時は安定性の問題から応答性を高めることが難しい。トルクベース力制御は、ワークに倣うようなトルク指令値を求め、トルク指令値に基づきロボットを制御する。トルクベース力制御では、ロボットの固有振動数に制約を受けずに応答性を高めることができる。トルクベース力制御は、位置ベース力制御よりも応答性が高いため、組立作業などにおいて応用範囲を広げることが可能となる。

特許文献１には、上述のロボットの力制御としてロボットの柔軟制御を行うことが記載されている。特許文献１には、ユーザが柔軟制御の複数のパラメータセットを記憶部に記憶させることが記載されている。また、特許文献１には、エンコーダ、力センサまたは時間を計測する時間計測部からの出力に基づいてロボットの動作状態を判定する動作状態監視部が記載されている。また、特許文献１には、ロボットがあるパラメータセットを用いた柔軟制御を実行しているときに、パラメータセットを切替える切替条件が成立した場合には、パラメータセットを別のパラメータセットに切替えて、柔軟制御を実行することが記載されている。

特許第５８４５３１１号公報

ところで、ロボットプログラムに従いながらロボットの動作の切り替えを指示する処理は、ロボットプログラムを解釈する必要があるため演算負荷が高い。一方、安定してトルクベースの力制御を行うには、トルクセンサの値及びエンコーダの値に基づいてフィードバック制御する処理を、速い制御周期で行うことが求められている。しかし、従来の方法では、演算負荷の高いロボットプログラムの処理を行いながらフィードバック制御を行っているため、トルクベースの力制御においてフィードバック制御の制御周期を高速化するのには限界があった。

そこで、本発明は、ロボットの様々な動作の切り替えに対応しながら、ロボットの力制御の安定化を図ることを目的とする。

本発明の一態様は、センサを有するロボットに関する複数の動作内容と、前記複数の動作内容の切替条件に関する値と、が記述されたロボットプログラムと、前記ロボットプログラムに基づき、前記複数の動作内容から第１の動作内容を前記ロボットに実行させるための第１動作指令を出力する第１制御部と、前記第１動作指令と前記センサの出力結果とに基づき前記ロボットの動作を制御し、前記センサの出力結果が前記切替条件と合致しているか判定する第２制御部と、を備え、前記第１制御部は、前記センサの出力結果が前記切替条件に合致すれば前記ロボットプログラムに基づき、第２の動作内容を前記ロボットに実行させるための第２動作指令を前記第２制御部に出力する、ことを特徴とする制御装置である。

本発明によれば、ロボットの様々な動作の切り替えに対応しながら、ロボットの力制御を安定して行うことができる。

第１実施形態に係るロボットシステムの斜視図である。 第１実施形態に係るロボット制御装置を示すブロック図である。 第１実施形態に係るロボットシステムの制御系を示す制御ブロック図である。 第１実施形態においてロボットのトルクベース力制御を説明するためのフローチャートである。 第１実施形態においてロボットの位置制御を説明するためのフローチャートである。 （ａ）、（ｂ）、（ｃ）及び（ｄ）は、トルクベース力制御を行うときの指示部と制御部の制御の流れを説明するための概念図である。 第１実施形態における指示部及び制御部の制御ブロック図である。 第１実施形態におけるロボットにより組付作業を行う直前の状態を示す模式図である。 第１実施形態におけるロボットプログラムの一例を示す説明図である。 （ａ）及び（ｂ）は、ロボットプログラムに従って動作するロボットの組付作業を説明するためのロボットの模式図である。 第２実施形態におけるロボットにより組付作業を行う直前の状態を示す模式図である。 第２実施形態におけるロボットプログラムの一例を示す説明図である。 （ａ）、（ｂ）及び（ｃ）は、ロボットプログラムに従って動作するロボットの組付作業を説明するためのロボットの模式図である。 第３実施形態における指示部及び制御部の制御ブロック図である。 （ａ）〜（ｄ）は、手先力と力目標値の時間変化を示す模式図である。 第４実施形態におけるロボットプログラムの一例を示す説明図である。 第４実施形態において切替条件に合致するか否かの判定を示す概念図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
［第１実施形態］
図１は、第１実施形態に係るロボットシステムの斜視図である。図１に示すように、ロボットシステム１００は、多関節のロボット２００と、ロボット２００の動作を制御するロボット制御装置３００と、を備えている。また、ロボットシステム１００は、ロボット制御装置３００に教示データを送信する教示装置としてのティーチングペンダント４００を備えている。ティーチングペンダント４００は、操作者が操作するものであり、ロボット２００やロボット制御装置３００の動作を指定するのに用いる。

ロボット２００は、垂直多関節のロボットアーム２５１と、ロボットアーム２５１の先端に取り付けられたエンドエフェクタの一例であるロボットハンド２５２と、を備えている。以下、エンドエフェクタがロボットハンド２５２である場合について説明するが、これに限定するものではなく、例えばドライバなどのツールであってもよい。ロボットアーム２５１の基端は、台座１５０に固定されている。ロボットハンド２５２は、部品やツール等の物体を把持するものである。

ロボット２００、即ちロボットアーム２５１は、複数の関節、例えば６つの関節Ｊ_１〜Ｊ_６を有している。ロボットアーム２５１は、各関節Ｊ_１〜Ｊ_６を各関節軸Ａ_１〜Ａ_６まわりにそれぞれ回転駆動する複数、例えば６つのサーボモータ２０１〜２０６を有している。ロボットアーム２５１は、複数のリンク２１０_０〜２１０_６が各関節Ｊ_１〜Ｊ_６で回転可能に連結されている。ロボットアーム２５１は、可動範囲の中であれば、任意の３次元位置で任意の３方向の姿勢に、ロボット２００の手先を向けることができる。

ロボット２００の手先の位置及び姿勢は、ロボットアーム２５１の基端、即ち台座１５０を基準とする座標系Ｔｏで表現される。ロボット２００の手先には、座標系Ｔｅが設定されている。ここで、ロボット２００の手先とは、第１実施形態では、ロボットハンド２５２が物体を把持していない場合には、ロボットハンド２５２のことである。ロボットハンド２５２が物体を把持している場合は、ロボットハンド２５２と把持している物体を含めてロボット２００の手先という。つまり、ロボットハンド２５２が物体を把持している状態であるか物体を把持していない状態であるかにかかわらず、ロボットアーム２５１の先端から先を手先という。

各サーボモータ２０１〜２０６は、各関節Ｊ_１〜Ｊ_６をそれぞれ駆動する電動モータであるモータ２１１〜２１６と、各モータ２１１〜２１６にそれぞれ接続されたセンサ部２２１〜２２６とを有している。各センサ部２２１〜２２６は、各関節Ｊ_１〜Ｊ_６の位置、即ち角度に応じた信号を発生する位置センサであるエンコーダを有する。また、各センサ部２２１〜２２６は、各関節Ｊ_１〜Ｊ_６のトルクに応じた信号を発生するトルクセンサを有する。また、各サーボモータ２０１〜２０６は、不図示の減速機を有し、直接、又は不図示のベルトやベアリング等の伝達機構を介して各関節Ｊ_１〜Ｊ_６で駆動されるリンクに接続されている。

ロボットアーム２５１の内部には、各サーボモータ２０１〜２０６のモータ２１１〜２１６の駆動を制御するサーボ制御部２３０が配置されている。サーボ制御部２３０は、入力を受けた各関節Ｊ_１〜Ｊ_６に対応する各トルク指令値に基づき、各関節Ｊ１〜Ｊ６のトルクがトルク指令値に追従するよう、各モータ２１１〜２１６に電流を出力し、各モータ２１１〜２１６の駆動を制御する。なお、第１実施形態では、サーボ制御部２３０は、ロボットアーム２５１の内部に配置されているが、ロボットアーム２５１の外部、例えばロボット制御装置３００の筐体内部に配置されていてもよい。

また、ロボット２００のロボットアーム２５１は、各関節Ｊ_１〜Ｊ_６をそれぞれ制動する複数のブレーキ２３１〜２３６を有する。ブレーキ２３１〜２３６は、例えばディスクブレーキである。各ブレーキ２３１〜２３６を作動させることにより、各関節Ｊ_１〜Ｊ_６が動かないように各関節Ｊ_１〜Ｊ_６を固定することができる。

次に、ロボット制御装置３００について説明する。図２は、第１実施形態に係るロボット制御装置３００を示すブロック図である。ロボット制御装置３００は、コンピュータで構成されており、ＣＰＵ（Central Processing Unit）３０１を備えている。また、ロボット制御装置３００は、記憶部として、内部記憶装置であるＲＯＭ（Read Only Memory）３０２、ＲＡＭ（Random Access Memory）３０３及びＨＤＤ（Hard Disk Drive）３０４を備えている。また、ロボット制御装置３００は、ディスクドライブ３０５及びインタフェース３０６〜３１０を備えている。ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、ディスクドライブ３０５、及びインタフェース３０６〜３１０は、互いに通信可能にバス３１１で接続されている。

ＣＰＵ３０１は、第１実施形態では複数のコア３０１Ａ，３０１Ｂを有しており、各コア３０１Ａ，３０１Ｂで独立して演算が可能である。ＲＯＭ３０２には、基本プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ３０１に、後述する演算処理を実行させるためのプログラム３３０を記録するものである。ＣＰＵ３０１は、ＨＤＤ３０４に記録されたプログラム３３０に基づいてロボット制御方法の各ステップを実行する。ディスクドライブ３０５は、ディスク３３１に記録された各種データやプログラム等を読み出すことができる。

ティーチングペンダント４００はインタフェース３０６に接続されている。ＣＰＵ３０１はインタフェース３０６及びバス３１１を介してティーチングペンダント４００からの教示データの入力を受ける。

サーボ制御部２３０は、インタフェース３０９に接続されている。ＣＰＵ３０１は、サーボ制御部２３０、インタフェース３０９及びバス３１１を介して各センサ部２２１〜２２６から信号を取得する。また、ＣＰＵ３０１は、各関節の指令値のデータを所定の時間間隔でバス３１１及びインタフェース３０９を介してサーボ制御部２３０に出力する。

インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、ＣＰＵ３０１の制御の下、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の記憶部である外部記憶装置３２２が接続可能に構成されている。

インタフェース３１０には、外部装置３２３が接続されている。ＣＰＵ３０１は、インタフェース３１０及びバス３１１を介して、外部装置３２３とデータの入出力を行う。外部装置３２３は、例えばＬＥＤを明滅する装置やＰＬＣ（Programmable Logic Controller）といった装置である。

なお、第１実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ３０４であり、ＨＤＤ３０４にプログラム３３０が格納される場合について説明するが、これに限定するものではない。プログラム３３０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム３３０を供給するための記録媒体としては、図２に示すＲＯＭ３０２，ディスク３３１、外部記憶装置３２２等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、ＤＶＤ−ＲＯＭやＣＤ−ＲＯＭ等の光ディスク、光磁気ディスク、磁気テープ、不揮発性メモリ等を用いることができる。

図３は、第１実施形態に係るロボットシステム１００の制御系を示す制御ブロック図である。ロボット制御装置３００のＣＰＵ３０１は、プログラム３３０を実行することにより、指示部５０１及び制御部５１０として機能する。第１実施形態では、ＣＰＵ３０１のコア３０１Ａが指示部５０１として機能し、ＣＰＵ３０１のコア３０１Ｂが制御部５１０として機能する。サーボ制御部２３０は、複数、即ち６関節であるので６つのモータ制御部５３１〜５３６を有する。

各センサ部２２１〜２２６は、各エンコーダ５５１〜５５６と、各トルクセンサ５４１〜５４６とを有する。エンコーダ５５１〜５５６は、ロータリエンコーダであり、モータ２１１〜２１６の回転角度に応じた計測値である角度値θ_１〜θ_６を信号としてそれぞれ出力する。各トルクセンサ５４１〜５４６は、各関節Ｊ_１〜Ｊ_６のトルクに応じた計測値であるトルク値τ_１〜τ_６を信号としてそれぞれ出力する。

ティーチングペンダント４００は、操作者の操作により、ロボット２００の手先に作用する力の目標値である力目標値Ｆ_ｒｅｆを、ロボット制御装置３００に設定、即ち外部記憶装置３２２に記憶させる。また、ロボット２００の手先の位置姿勢の目標値である位置目標値Ｐ_ｒｅｆを、ロボット制御装置３００に設定、即ち外部記憶装置３２２に設定する。力目標値Ｆ_ｒｅｆ及び位置目標値Ｐ_ｒｅｆは、組立工程に合わせて複数存在する。また、外部記憶装置３２２には、ロボット言語で記述されたロボットプログラム５０６、及びロボット２００の３次元のＣＡＤデータ等のロボットモデル５１９が記憶されている。ロボットプログラム５０６は、ロボット２００と外部装置３２３の動作を規定するものである。操作者がテキストエディタや専用のＩＤＥ（Integrated Development Environment）を用いて組立工程に対応させて作成する。ロボットプログラム５０６は、力目標値Ｆ_ｒｅｆ及び位置目標値Ｐ_ｒｅｆと関連付けて用いることが可能である。なお、力目標値Ｆ_ｒｅｆ、位置目標値Ｐ_ｒｅｆ、ロボットプログラム５０６及びロボットモデル５１９は、外部記憶装置３２２に記憶されるものとしたが、内部記憶装置に記憶されてもよい。

指示部５０１は、ロボットプログラム５０６に従って、ロボット２００の動作を制御部５１０に指示するものであり、ロボットプログラム５０６を解釈するため、演算負荷が制御部５１０よりも高い。

制御部５１０は、指示部５０１の指示に従って、第１の制御である力制御又は第２の制御である位置制御のいずれか一方で選択的にロボット２００の動作を制御する。力制御は、第１実施形態ではトルクベース力制御である。トルクベース力制御では、ロボット２００のトルクセンサ５４１〜５４６のトルク値τ_１〜τ_６及びロボット２００のエンコーダ５５１〜５５６の角度値θ_１〜θ_６に基づきロボット２００の動作をフィードバック制御する。位置制御では、ロボット２００のエンコーダ５５１〜５５６の角度値θ_１〜θ_６に基づきロボット２００の動作をフィードバック制御する。

制御部５１０は、トルクベース力制御において、フィードバック指令値として、各関節のトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を求め、各トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を各モータ制御部５３１〜５３６に出力する。制御部５１０は、トルクベース力制御を、所定の制御周期で実行する。また、制御部５１０は、位置制御において、フィードバック指令値として、各関節の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を求め、各角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を各モータ制御部５３１〜５３６に出力する。

各モータ制御部５３１〜５３６において、入力を受けた指令値がトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}の場合について説明する。各モータ制御部５３１〜５３６は、各トルク値τ_１〜τ_６と各トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}との偏差が小さくなるように、各モータ２１１〜２１６に流す各電流Ｃｕｒ_１〜Ｃｕｒ_６を制御する。

各モータ制御部５３１〜５３６において、入力を受けた指令値が角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６の場合について説明する。各モータ制御部５３１〜５３６は、各角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を各モータ２１１〜２１６の角度指令値θ_ｒｅｆ１〜θ_ｒｅｆ６に変換する。そして、各モータ制御部５３１〜５３６は、各角度値θ_１〜θ_６と各角度指令値θ_ｒｅｆ１〜θ_ｒｅｆ６との偏差が小さくなるように、各モータ２１１〜２１６に流す各電流Ｃｕｒ_１〜Ｃｕｒ_６を制御する。なお、各モータ制御部５３１〜５３６は、各角度値θ_１〜θ_６を各角度値ｑ_１〜ｑ_６に変換してもよい。この場合、各モータ制御部５３１〜５３６は、各角度値ｑ_１〜ｑ_６と各角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６との偏差が小さくなるように、各モータ２１１〜２１６に流す各電流Ｃｕｒ_１〜Ｃｕｒ_６を制御する。

ロボット２００のトルクベース力制御について詳細に説明する。図４は、第１実施形態においてロボット２００のトルクベース力制御を説明するためのフローチャートである。まず、操作者が力目標値Ｆ_ｒｅｆと位置目標値Ｐ_ｒｅｆとをティーチングペンダント４００を用いて設定する（Ｓ１）。力目標値Ｆ_ｒｅｆ及び位置目標値Ｐ_ｒｅｆは、外部記憶装置３２２に記憶される。なお、力目標値Ｆ_ｒｅｆ及び位置目標値Ｐ_ｒｅｆは、ティーチングペンダント４００を使わずに、ロボットプログラム５０６に直接記述してもよい。

制御部５１０は、手先に作用した力の計測値である手先力Ｆが力目標値Ｆ_ｒｅｆに倣うよう、即ち手先力Ｆと力目標値Ｆ_ｒｅｆとの力偏差が小さくなるよう各モータ２１１〜２１６に対するトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を算出する（Ｓ２）。

各モータ制御部５３１〜５３６は、各エンコーダ５５１〜５５６の角度値θ_１〜θ_６に基づいて、各トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を実現するよう、各モータ２１１〜２１６へ電流Ｃｕｒ_１〜Ｃｕｒ_６を通電する通電制御を行う（Ｓ３）。各モータ２１１〜２１６は、通電されることによりトルクを発生する（Ｓ４）。

各エンコーダ５５１〜５５６は、各角度値θ_１〜θ_６を示す信号を制御部５１０へ出力する。各トルクセンサ５４１〜５４６は、各トルク値τ_１〜τ_６を示す信号を制御部５１０へ出力する。これにより、各エンコーダ５５１〜５５６の角度値θ_１〜θ_６と各トルクセンサ５４１〜５４６のトルク値τ_１〜τ_６は、制御部５１０にフィードバックされ、制御部５１０は、角度値θ_１〜θ_６及びトルク値τ_１〜τ_６を取得する（Ｓ５）。

制御部５１０は、ロボットモデル５１９及び角度値θ_１〜θ_６に基づいて各関節Ｊ_１〜Ｊ_６の角度値ｑ_１〜ｑ_６を算出する。また、制御部５１０は、ロボットモデル５１９及び各関節Ｊ_１〜Ｊ_６の角度値ｑ_１〜ｑ_６に基づいてロボット２００の手先の位置Ｐを算出する。制御部５１０は、ロボットモデル５１９、角度値ｑ_１〜ｑ_６及びトルク値τ_１〜τ_６に基づき、ロボット２００の手先にかかる手先力Ｆを算出する（Ｓ６）。制御部５１０は、駆動が終了したか否かを判定し（Ｓ７）、終了していない場合は（Ｓ７：Ｎｏ）、ステップＳ２〜Ｓ７を繰り返す。以上のフローチャートに従って各モータ２１１〜２１６を駆動することで、ロボット２００の手先力Ｆを力目標値Ｆ_ｒｅｆに制御することが可能である。なお、トルクベース力制御は図４に示すフローチャートの順番に限定するものではなく、他の順番でもよい。

次に、ロボット２００の位置制御について説明する。図５は、第１実施形態においてロボット２００の位置制御を説明するためのフローチャートである。まず、操作者が位置目標値Ｐ_ｒｅｆをティーチングペンダント４００を用いて設定する（Ｓ１１）。位置目標値Ｐ_ｒｅｆは、外部記憶装置３２２に記憶される。なお、位置目標値Ｐ_ｒｅｆは、ティーチングペンダント４００を使わずに、ロボットプログラム５０６に直接記述してもよい。

制御部５１０は、ロボットモデル５１９に基づき、位置目標値Ｐ_ｒｅｆを各関節Ｊ_１〜Ｊ_６の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６に変換することで、角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を算出する（Ｓ１２）。

各モータ制御部５３１〜５３６は、各角度値ｑ_１〜ｑ_６が各角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６に倣うよう各モータ２１１〜２１６へ電流Ｃｕｒ_１〜Ｃｕｒ_６を通電する通電制御を行う（Ｓ１３）。なお、関節Ｊ_１〜Ｊ_６の角度を示す信号としては、角度値ｑ_１〜ｑ_６の代わりにモータ２１１〜２１６の角度値θ_１〜θ_６を用いてもよい。各モータ２１１〜２１６は、通電されることによりトルクを発生する（Ｓ１４）。

各エンコーダ５５１〜５５６は、各角度値θ_１〜θ_６を示す信号を制御部５１０へ出力する。これにより、各エンコーダ５５１〜５５６の角度値θ_１〜θ_６は、制御部５１０にフィードバックされ、制御部５１０は、角度値θ_１〜θ_６を取得する（Ｓ１５）。制御部５１０は、ロボットモデル５１９及び角度値θ_１〜θ_６に基づいて各関節Ｊ_１〜Ｊ_６の角度値ｑ_１〜ｑ_６を求める。

制御部５１０は、駆動が終了したか否か、即ち手先の位置Ｐが位置目標値Ｐ_ｒｅｆ付近に達したか否かを判定し（Ｓ１６）、駆動が終了していない場合は（Ｓ１６：Ｎｏ）、ステップＳ１２〜Ｓ１６を繰り返す。以上のフローチャートに従って各モータ２１１〜２１６を駆動することで、ロボット２００の手先の位置Ｐを位置目標値Ｐ_ｒｅｆに制御することが可能である。

ステップＳ１〜Ｓ７のトルクベース力制御とステップＳ１１〜Ｓ１６の位置制御は、ロボットプログラム５０６に従って指示部５０１にて切り替えられ、作業に応じていずれか一方の制御が選択される。

以下、制御部５１０が、指示部５０１の指示により、トルクベース力制御によるフィードバック制御を行う場合について説明する。図６（ａ）、図６（ｂ）、図６（ｃ）及び図６（ｄ）は、トルクベース力制御を行うときの指示部５０１と制御部５１０の制御の流れを説明するための概念図である。図６（ａ）に示すように、指示部５０１は、ロボットプログラム５０６に従って制御部５１０に切替条件を送る。制御部５１０は、トルクベース力制御中にロボット２００の状態が切替条件に合致したか否かを判定する。制御部５１０は、ロボット２００の状態が切替条件に合致した場合、図６（ｂ）に示すように、切替条件に合致したことを示す情報、即ち切替情報を、指示部５０１に送る。指示部５０１は、ロボットプログラム５０６に従って、図６（ｃ）に示すように、切替情報に対応する動作指令を制御部５１０に送る。制御部５１０は、動作指令に従って、図６（ｄ）に示すように、ロボット２００の動作を切り替える。このように、指示部５０１は、ロボットプログラム５０６を解釈する処理を行い、制御部５１０は、指示部５０１の指示に従ってトルクベース力制御によるフィードバック制御を行う。このように指示部５０１と制御部５１０と処理を分けることにより、演算負荷の高いフィードバック処理を集中的に行うことが可能になる。このため、制御部５１０において、速い制御周期でフィードバック制御を行うことができる。速い制御周期でフィードバック制御を行うため、ロボット２００に作用する力等が急激に変化するような場合であっても、迅速に対応することが可能となり、ロボット２００の制御が安定化する。即ち、ロボット２００の制御性が向上する。なお、指示部５０１と制御部５１０との間では、切替条件、切替情報及び動作指令のデータを電気信号で送受信する。

以下、指示部５０１及び制御部５１０の動作について具体的に説明する。図７は、第１実施形態における指示部５０１及び制御部５１０の制御ブロック図である。指示部５０１は、切替部５０２、力指令生成部５０３、位置指令生成部５０４及び外部指令生成部５０５として機能する。制御部５１０は、監視部５１１、時間計測部５１２、計測部５１３、パラメータ設定部５１６、力制御部５１７及び位置制御部５１８として機能する。計測部５１３は、力計測部５１４及び位置計測部５１５として機能する。

時間計測部５１２は、トルクベース力制御を開始した時点からの経過時間ｔを計測する。計測部５１３は、外部記憶装置３２２からロボットモデル５１９を読み込む。位置計測部５１５は、エンコーダ５５１〜５５６から取得した角度値θ_１〜θ_６、及びロボットモデル５１９に基づき、ロボット２００の手先の位置姿勢の計測値を示す位置Ｐを求める。また、位置計測部５１５は、各角度値θ_１〜θ_６の入力を受け、ロボットモデル５１９にて規定された不図示の減速機の減速比等に基づき、各関節Ｊ_１〜Ｊ_６の角度の計測値である角度値ｑ_１〜ｑ_６をそれぞれ求める。力計測部５１４は、エンコーダ５５１〜５５６から取得した角度値θ_１〜θ_６、トルクセンサ５４１〜５４６から取得したトルク値τ_１〜τ_６、及びロボットモデル５１９に基づき、ロボット２００の手先に作用する力の計測値である手先力Ｆを求める。

切替部５０２は、ロボットプログラム５０６と力目標値Ｆ_ｒｅｆ及び位置目標値Ｐ_ｒｅｆを読み込んで解釈処理を行い、また監視部５１１から入力を受けた切替情報に従って、ロボット２００又は外部装置３２３の動作指示を生成する。切替部５０２は、生成した動作指示がロボット２００のトルクベース力制御の場合は、力指令生成部５０３へ動作指示を送る。切替部５０２は、生成した動作指示がロボット２００の位置制御の場合は、位置指令生成部５０４へ動作指示を送る。切替部５０２は、生成した動作指示が外部装置３２３の動作の場合は、外部指令生成部５０５へ動作指示を送る。

力指令生成部５０３は、切替部５０２から入力を受けた動作指示に従い、切替条件及び動作指令としての力指令を制御部５１０へ送る。切替条件は、第１実施形態では以下の１３種類である。力指令生成部５０３は、ロボットプログラム５０６の命令文に従って、以下の切替条件を監視部５１１へ送る。
１．トルクベース力制御を開始した時点からの経過時間
２．ロボット２００の手先力
３．ロボット２００の手先力の時間微分
４．ロボット２００の手先力の時間２階微分
５．ロボット２００の手先の位置
６．ロボット２００の手先の速度
７．ロボット２００の手先の加速度
８．ロボット２００の各軸の位置
９．ロボット２００の各軸の速度
１０．ロボット２００の各軸の加速度
１１．ロボット２００の各軸のトルク
１２．ロボット２００の各軸のトルクの時間微分
１３．ロボット２００の各軸のトルクの時間２階微分
なお、切替条件を例示したが、これらに限定されるものではない。

監視部５１１は、時間計測部５１２や計測部５１３から取得した計測値が、入力を受けた上記の切替条件に合致したかどうかを判定する。即ち、監視部５１１は、トルクベース力制御中に下記の計測値を監視し、力指令生成部５０３から入力を受けた切替条件に合致した場合は、切替部５０２に切替条件に合致した旨を示す切替情報を送る。なお、以下の時間による１階微分及び２階微分は、監視部５１１で行う。
・時間計測部５１２から通知された経過時間ｔ
・力計測部５１４から通知された手先力Ｆ
・力計測部５１４から通知された各軸のトルク値τ_１〜τ_６
・位置計測部５１５から通知された手先の位置Ｐ
・位置計測部５１５から通知された各角度値ｑ_１〜ｑ_６
・手先力Ｆの時間微分
・トルク値τ_１〜τ_６の時間微分
・手先の位置Ｐの時間微分（速度）
・各角度値ｑ_１〜ｑ_６の時間微分（速度）
・手先力Ｆの時間２階微分
・各トルク値τ_１〜τ_６の時間２階微分
・手先の位置Ｐの時間２階微分（加速度）
・各角度値ｑ_１〜ｑ_６の時間２階微分（加速度）

第１実施形態では、力指令生成部５０３が出力する動作指令である力指令は、トルクベース力制御に係るパラメータである。パラメータは、第１実施形態では下記７種類である。
１．ロボット２００の手先の力目標値Ｆ_ｒｅｆ
２．ロボット２００の手先の位置目標値Ｐ_ｒｅｆ
３．ロボット２００の手先の速度目標値Ｐ_ｒｅｆ（・）
４．ロボット２００の手先の加速度目標値Ｐ_ｒｅｆ（・・）
５．ロボット２００の手先における目標の剛性係数Ｋ_ｒｅｆ
６．ロボット２００の手先における目標の粘性係数Ｄ_ｒｅｆ
７．ロボット２００の手先における目標の慣性行列Λ_ｒｅｆ
なお、（・）は時間の１階微分、（・・）は時間の２階微分を表す。以上、パラメータを例示したが、これらに限定されるものではない。また、これら全てを必須とするものでもない。上記パラメータ中、剛性係数Ｋ_ｒｅｆ、粘性係数Ｄ_ｒｅｆ及び慣性行列Λ_ｒｅｆは、トルクベース力制御のフィードバック制御において用いる下記の式（１）及び式（２）に表す演算式の係数である。

パラメータ設定部５１６は、力指令生成部５０３からの送信される力制御に係るパラメータを受け、座標変換を行い力制御部５１７へ設定する。力制御部５１７は、ロボットモデル５１９、力目標値Ｆ_ｒｅｆ、位置目標値Ｐ_ｒｅｆ、速度目標値Ｐ_ｒｅｆ（・）、加速度目標値Ｐ_ｒｅｆ（・・）、剛性係数Ｋ_ｒｅｆ、粘性係数Ｄ_ｒｅｆ、慣性行列Λ_ｒｅｆの入力を受ける。また、力制御部５１７は、計測値として、トルク値τ_１〜τ_６、角度値ｑ_１〜ｑ_６、位置Ｐ及び力Ｆの入力を受ける。そして、力制御部５１７は、これら目標値及び計測値を用いて、各関節Ｊ_１〜Ｊ_６に対するトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を求める。ここで、手先力Ｆと力目標値Ｆ_ｒｅｆとの偏差を力偏差とする。手先の位置Ｐと位置目標値Ｐ_ｒｅｆとの偏差を位置偏差とする。手先の速度Ｐ（・）と速度目標値Ｐ_ｒｅｆ（・）との偏差を速度偏差とする。手先の加速度値Ｐ（・・）と加速度目標値Ｐ_ｒｅｆ（・・）との偏差を加速度偏差とする。力制御部５１７は、力偏差、位置偏差、速度偏差及び加速度偏差が小さくなるように、トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を求める。

具体的に説明すると、力制御部５１７は、下記の変数をもとに、式（１）、式（２）により計算を行う。
・力目標値Ｆ_ｒｅｆ
・位置目標値Ｐ_ｒｅｆ
・速度目標値Ｐ_ｒｅｆ（・）
・加速度目標値Ｐ_ｒｅｆ（・・）
・剛性係数Ｋ_ｒｅｆ
・粘性係数Ｄ_ｒｅｆ
・慣性行列Λ_ｒｅｆ
・手先位置Ｐ
・手先力Ｆ
・ロボットモデル５１９及び関節速度ｑ（・）から算出されるヤコビアンＪ（ｑ）
・逆動力学計算により算出される慣性行列Ｍ（ｑ）
・逆動力学計算により算出されるコリオリ遠心行列ｃ（ｑ，ｑ（・））
・逆動力学計算により算出される重力ベクトルｇ（ｑ）
・Ｊ（ｑ）、Ｍ（ｑ）及び位置Ｐから算出される慣性行列Λ

なお、式（１）及び式（２）は、トルクベース力制御の一例であり、これに限定するものではない。

力制御部５１７は、求めた各トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を各モータ制御部５３１〜５３６に出力する。ここで、速度Ｐ（・）は、位置Ｐを時間で微分することにより求まり、加速度Ｐ（・・）は、速度Ｐ（・）を微分することにより求まる。したがって、第１実施形態では、力制御部５１７が位置計測部５１５の計測結果である手先の位置Ｐから手先の速度Ｐ（・）と手先の加速度Ｐ（・・）を求めることになる。なお、位置計測部５１５が演算を行って手先の速度Ｐ（・）や手先の加速度Ｐ（・・）のデータを出力するようにしてもよい。

位置指令生成部５０４は、切替部５０２から入力を受けた動作指示に従い、動作指令として位置目標値Ｐ_ｒｅｆを位置制御部５１８へ送る。外部指令生成部５０５は、切替部５０２から入力を受けた動作指示に従い、外部装置３２３に対応するフォーマットにデータを成形して、外部装置３２３へ指令を送信する。

位置制御部５１８は、手先の位置目標値Ｐ_ｒｅｆから逆運動学計算により各関節Ｊ_１〜Ｊ_６の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を求め、各角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を各モータ制御部５３１〜５３６に出力する。

以下、指示部５０１及び制御部５１０の動作を具体例を挙げて説明する。図８は、第１実施形態におけるロボット２００により組付作業を行う直前の状態を示す模式図である。第１実施形態では、トルクベース力制御により、ロボットハンド２５２を下方向に動かし、ロボットハンド２５２が把持した第１のワークである円柱状のワークＷ１を第２のワークである円環状のワークＷ２に組み付ける作業を行う。これにより、ワークＷ２にワークＷ１が組み付けられた物品を製造する。

図９は、第１実施形態におけるロボットプログラム５０６の一例を示す説明図である。また、図１０（ａ）及び図１０（ｂ）は、ロボットプログラム５０６に従って動作するロボット２００の組付作業を説明するためのロボット２００の模式図である。指示部５０１の切替部５０２は、第０１行目の命令文により、事前にティーチングペンダント４００で教示した目標位置Ｐ_{ｒｅｆｓｔａｒｔ１}に位置制御により動作するよう位置指令生成部５０４に動作指示を送る。位置指令生成部５０４は、動作指令として、目標位置Ｐ_{ｒｅｆｓｔａｒｔ１}を制御部５１０の位置制御部５１８に送る。これにより位置制御部５１８はロボット２００の手先を目標位置Ｐ_{ｒｅｆｓｔａｒｔ１}に移動させることになる。

次に、指示部５０１の切替部５０２は、第０３行目の命令文により、ロボット２００の手先を位置Ｐ_{ｒｅｆｓｔａｒｔ１}から図８の下方にトルクベース力制御により移動を開始するよう力指令生成部５０３に動作指示を送る。力指令生成部５０３は、動作指令として、パラメータセット１をパラメータ設定部５１６に送ると共に、切替条件Ａ，Ｂ，Ｃを監視部５１１に送る。これにより、指示部５０１は、制御部５１０にトルクベース力制御によるフィードバック制御を開始させる。

パラメータセット１は、トルクベース力制御を行うために操作者が設定するパラメータ群であり、その値は第１５行目に記述されている。パラメータセット１には、力目標値Ｆ_ｒｅｆ１、位置目標値Ｐ_ｒｅｆ１、速度目標値Ｐ_ｒｅｆ１（・）、加速度目標値Ｐ_ｒｅｆ１（・・）、剛性係数Ｋ_ｒｅｆ１、粘性係数Ｄ_ｒｅｆ１、慣性行列Λ_ｒｅｆ１が含まれている。切替条件Ａ，Ｂ，Ｃは、ロボット２００の動作を切り替えるための切替条件群であり、第１８行目〜第２０行目には個々の切替条件が記述されている。具体的には、第１８行目には、切替条件Ａとして、手先力Ｆの並進ｚ成分が閾値Ｔｈ＿ｆ＿ｚ１を超えるという条件が記述されている。第１９行目には、切替条件Ｂとして、手先の並進ｚ成分の位置Ｐｚが閾値Ｔｈ＿ｐｏｓ＿ｚ１未満であるという条件が記述されている。第２０行目には、切替条件Ｃとして、トルクベース力制御を開始した時点からの経過時間が閾値（１０［ｓ］）を超えるという条件が記述されている。なお、１つの切替条件に対して例えば『「Ｆｚ＞Ｔｈ＿ｆ＿ｚ１」かつ「経過時間＞１０」』や『「Ｆｚ＞Ｔｈ＿ｆ＿ｚ１」または「経過時間＞１０」』というように、複数の条件を併記することも可能である。

以上、指示部５０１は、ロボットプログラム５０６に従って、パラメータセット１を制御部５１０に送り、制御部５１０は、パラメータセット１を用いてトルクベース力制御によりロボット２００の動作をフィードバック制御する。

また、指示部５０１は、制御部５１０にトルクベース力制御を開始させる時、制御部５１０に複数の切替条件Ａ，Ｂ，Ｃを送る。制御部５１０の監視部５１１は、トルクベース力制御中にロボット２００の状態が切替条件Ａに合致した場合、切替条件Ａに合致したことを示す切替情報を指示部５０１に送る。また、監視部５１１は、トルクベース力制御中にロボット２００の状態が切替条件Ｂに合致した場合、切替条件Ｂに合致したことを示す切替情報を指示部５０１に送る。また、監視部５１１は、トルクベース力制御中にロボット２００の状態が切替条件Ｃに合致した場合、切替条件Ｃに合致したことを示す切替情報を指示部５０１に送る。

即ち、監視部５１１は、ロボット２００の状態として、時間計測部５１２からの経過時間ｔ、力計測部５１４からの手先力Ｆ及びトルク値τ_１〜τ_６、位置計測部５１５からの手先の位置Ｐ及び角度値ｑ_１〜ｑ_６を監視している。監視部５１１は、これら計測値を監視して、いずれかの切替条件Ａ，Ｂ，Ｃが成立した場合には、その切替条件に対応する切替情報を指示部５０１に送る。

指示部５０１の切替部５０２は、切替条件Ａに合致したことを示す切替情報の入力を受けた場合、その切替情報に対応する動作指令を制御部５１０に送る。具体的には、切替条件Ａは、手先力Ｆの並進ｚ成分が閾値Ｔｈ＿ｆ＿ｚ１を超えるという条件であり、図１０（ａ）に示すように、ワークＷ１の底面がワークＷ２に組み付いたこと示している。切替条件Ａが成立しているので、切替部５０２は、第０５行目から第０６行目に記述された命令文に従い、ロボットハンド２５２を開く動作指示を力指令生成部５０３に送り、力指令生成部５０３は、制御部５１０に動作指令を送る。制御部５１０は、入力を受けた動作指令に従ってロボット２００の動作を、ロボットハンド２５２を開く動作に切り替え、組付作業を完了する。

指示部５０１の切替部５０２は、切替条件Ｂに合致したことを示す切替情報の入力を受けた場合、その切替情報に対応する動作指令を制御部５１０に送る。具体的には、切替条件Ｂは、並進ｚ方向におけるロボット２００の手先の位置Ｐｚが閾値Ｔｈ＿ｐｏｓ＿ｚ１未満となる条件であり、図１０（ｂ）に示すように、組み付けが失敗していることを示している。切替条件Ｂが成立しているので、切替部５０２は、第０７行目から第０９行目に記述された命令文に従い、ロボット２００を停止させる動作指示を力指令生成部５０３に送り、外部装置３２３に対してＬＥＤを明滅させる動作指示を外部指令生成部５０５に送る。力指令生成部５０３は、制御部５１０に動作指令を送り、制御部５１０は、入力を受けた動作指令に従ってロボット２００の動作を停止させる。また、外部指令生成部５０５は、外部装置３２３に動作指令を送り、外部装置３２３の動作を制御、即ち外部装置３２３にＬＥＤを明滅させる。

指示部５０１の切替部５０２は、切替条件Ｃに合致したことを示す切替情報の入力を受けた場合、その切替情報に対応する動作指令を制御部５１０に送る。具体的には、切替条件Ｃは、トルクベース力制御を開始した時点からの経過時間が閾値（１０［ｓ］）を超えるという条件である。切替条件Ｃが成立しているので、切替部５０２は、第１０行目及び第１１行目に記述された命令文に従い、ロボット２００の手先が位置Ｐ_{ｒｅｆｓｔａｒｔ１}へ位置制御で移動する動作指示を位置指令生成部５０４に送る。位置指令生成部５０４は、制御部５１０に動作指令を送る。制御部５１０は、入力を受けた動作指令に従ってロボット２００の手先を位置Ｐ_{ｒｅｆｓｔａｒｔ１}に位置制御で戻す。即ち、制御部５１０は、指示部５０１の指示に従い、ロボット２００の動作の切り替えとして、トルクベース力制御から位置制御に切り替える。このように、トルクベース力制御と位置制御との切り替えを、指示部５０１と制御部５１０との協働により迅速に行うことができる。

以上、第１実施形態によれば、ユーザが作成したロボットプログラム５０６に従って、状態遷移を行う指示部５０１と、指示部５０１からの指令を受けて所定の制御周期でフィードバック制御する制御部５１０の２部構成をとっている。これにより、演算負荷の高いロボットプログラム５０６を解釈する処理を、制御部５１０に行わせる必要がなくなるので、トルクベース力制御におけるフィードバック制御の制御周期を速くすることができる。よって、ロボット２００の様々な動作の切り替えに対応しながら、ロボット２００のトルクベース力制御を安定して行うことができる。特に、第１実施形態によれば、１つのプロセスでロボットプログラムの処理とセンサの値を用いたフィードバック制御処理とを行う場合に比べ、フィードバックの制御周期を速くすることが可能となり、ロボット２００の制御の安定性が向上する。

また、指示部５０１においても、センサ情報に基づくフィードバック制御を制御部５１０に行わせることで、その分の演算負荷が軽減し、コンピュータリソースを多大に消費することを防止することができる。

また、第１実施形態によれば、制御部５１０の監視部５１１は、トルクベース力制御中に切替条件に合致した場合において、切替情報を切替部５０２に通知する。切替部５０２は、通知された切替情報をもとに条件分岐することで、ロボット２００の動作を柔軟に切り替えることが可能となる。よって、トルクベース力制御を用いた組付作業の応用範囲が広がり、複雑な組付作業に対応することが可能となる。

［第２実施形態］
次に、第２実施形態に係るロボットシステムについて説明する。図１１は、第２実施形態におけるロボットにより組付作業を行う直前の状態を示す模式図である。第２実施形態では、トルクベース力制御により、ロボットハンド２５２を下方向に動かし、ロボットハンド２５２が把持した第１のワークである円柱状のワークＷ１を、第２のワークである円環状のワークＷ３に組み付ける作業を行う。これにより、ワークＷ３にワークＷ１が組み付けられた物品を製造する。その際、指示部５０１及び制御部５１０により、成立した切替条件に応じて多段に分岐する複雑な制御処理を実現する。

第２実施形態では、ロボット２００の動作の切り替えとして、力制御に係るパラメータを切り替える場合について説明する。なお、第２実施形態におけるロボットシステムの構成及び機能は、第１実施形態と同様であり、ユーザが作成するロボットプログラムの内容、及び組み付け対象となる第２のワークが異なる。第２実施形態では、ワークＷ１を組み付けるワークＷ３の凹部分がどの位置にあるか正確には設定されていない。このため、下記のシーケンスにより組付作業を行うものとする。説明の簡略化のため、正常系以外の条件に当てはまった場合は、ロボット２００の動作を停止する。

図１２は、第２実施形態におけるロボットプログラム５０６の一例を示す説明図である。また、図１３（ａ）、図１３（ｂ）及び図１３（ｃ）は、ロボットプログラム５０６に従って動作するロボット２００の組付作業を説明するためのロボット２００の模式図である。指示部５０１の切替部５０２は、第０１行目の命令文により、事前にティーチングペンダント４００で教示した目標位置Ｐ_{ｒｅｆｓｔａｒｔ２}に位置制御により動作するよう位置指令生成部５０４に動作指示を送る。位置指令生成部５０４は、動作指令として、目標位置Ｐ_{ｒｅｆｓｔａｒｔ２}を制御部５１０の位置制御部５１８に送る。これにより位置制御部５１８はロボット２００の手先の位置Ｐを目標位置Ｐ_{ｒｅｆｓｔａｒｔ２}に移動させることになる。

次に、指示部５０１の切替部５０２は、第０３行目の命令文により、ロボット２００の手先を位置Ｐ_{ｒｅｆｓｔａｒｔ2}から図１１の下方にトルクベース力制御により移動を開始するよう力指令生成部５０３に動作指示を送る。力指令生成部５０３は、動作指令として、パラメータセット１をパラメータ設定部５１６に送ると共に、切替条件Ａ，Ｂを監視部５１１に送る。これにより、指示部５０１は、制御部５１０にトルクベース力制御によるフィードバック制御を開始させる。即ち、指示部５０１は、ロボットプログラム５０６に従って、パラメータセット１を制御部５１０に送り、制御部５１０は、パラメータセット１を用いてトルクベース力制御によりロボット２００の動作をフィードバック制御する。

ここで、第２実施形態では、複数のパラメータセット１，２，３及び複数の切替条件Ａ，Ｂ，Ｃ，Ｄ，Ｅがある。パラメータセット１〜３は、第１７行目〜第１９行目に記述されている。パラメータセット１には、力目標値Ｆ_ｒｅｆ２、位置目標値Ｐ_ｒｅｆ２、速度目標値Ｐ_ｒｅｆ２（・）、加速度目標値Ｐ_ｒｅｆ２（・・）、剛性係数Ｋ_ｒｅｆ２、粘性係数Ｄ_ｒｅｆ２、慣性行列Λ_ｒｅｆ２が含まれている。パラメータセット２には、力目標値Ｆ_ｒｅｆ３、位置目標値Ｐ_ｒｅｆ３、速度目標値Ｐ_ｒｅｆ３（・）、加速度目標値Ｐ_ｒｅｆ３（・・）、剛性係数Ｋ_ｒｅｆ３、粘性係数Ｄ_ｒｅｆ３、慣性行列Λ_ｒｅｆ３が含まれている。パラメータセット３には、力目標値Ｆ_ｒｅｆ４、位置目標値Ｐ_ｒｅｆ４、速度目標値Ｐ_ｒｅｆ４（・）、加速度目標値Ｐ_ｒｅｆ４（・・）、剛性係数Ｋ_ｒｅｆ４、粘性係数Ｄ_ｒｅｆ４、慣性行列Λ_ｒｅｆ４が含まれている。

切替条件Ａ〜Ｅは、第２２行目〜第２６行目に記述されている。切替条件Ａは、手先力Ｆの並進ｚ成分が閾値Ｔｈ＿ｆ＿ｚ３を超えるという条件である。切替条件Ｂは、トルクベース力制御を開始した時点からの経過時間が閾値（５［ｓ］）を超えるという条件である。切替条件Ｃは、手先力Ｆの並進ｚ成分が閾値Ｔｈ＿ｆ＿ｚ４未満であるという条件である。切替条件Ｄは、トルクベース力制御を開始した時点からの経過時間が閾値（４［ｓ］）を超えるという条件である。切替条件Ｅは、トルクベース力制御を開始した時点からの経過時間が閾値（３［ｓ］）を超えるという条件である。切替条件Ｃ，Ｄは、切替条件Ａに関連付けされている。また、切替条件Ｅは、切替条件Ｃに関連付けされている。即ち、切替条件Ａ〜Ｅが多段のツリー構造状に関連付けされている。

指示部５０１は、制御部５１０にパラメータセット１に基づくトルクベース力制御を開始させる時、制御部５１０に複数の切替条件Ａ，Ｂを送る。制御部５１０の監視部５１１は、トルクベース力制御中にロボット２００の状態が切替条件Ａに合致した場合、切替条件Ａに合致したことを示す切替情報を指示部５０１に送る。また、監視部５１１は、トルクベース力制御中にロボット２００の状態が切替条件Ｂに合致した場合、切替条件Ｂに合致したことを示す切替情報を指示部５０１に送る。

指示部５０１の切替部５０２は、切替条件Ａに合致したことを示す切替情報の入力を受けた場合、その切替情報に対応する動作指令を制御部５１０に送る。具体的には、切替条件Ａは、手先力Ｆの並進ｚ成分が閾値Ｔｈ＿ｆ＿ｚ３を超えるという条件であり、図１３（ａ）に示すように、ワークＷ１の底面がワークＷ３に突き当たったこと示している。

切替条件Ａが成立しているので、切替部５０２は、第０５行目から第０６行目に記述された命令文に従い、動作指示を力指令生成部５０３に送り、力指令生成部５０３は、制御部５１０に動作指令を送る。この動作指令は、パラメータセット２であり、力制御部５１７は、新たなパラメータセット２を用いてトルクベース力制御でフィードバック制御を行う。即ち、図１３（ｂ）に示すように、紙面右方向にトルクベース力制御で探り動作を行う。ここで、指示部５０１は、動作指令であるパラメータセット２を制御部５１０に送る時、ロボット２００の状態が合致したと判定された切替条件Ａに関連付けされた新たな切替条件Ｃ，Ｄを、制御部５１０の監視部５１１に送る。

指示部５０１の切替部５０２は、切替条件Ｃに合致したことを示す切替情報の入力を受けた場合、その切替情報に対応する動作指令を制御部５１０に送る。具体的には、切替条件Ｃは、手先力Ｆの並進ｚ成分が閾値Ｔｈ＿ｆ＿ｚ４未満であるという条件であり、図１３（ｂ）に示すように、ワークＷ１の底面がワークＷ３の凹部に入ったこと示している。

切替条件Ｃが成立しているので、切替部５０２は、第０７行目から第０８行目に記述された命令文に従い、動作指示を力指令生成部５０３に送り、力指令生成部５０３は、制御部５１０に動作指令を送る。この動作指令は、パラメータセット３であり、力制御部５１７は、新たなパラメータセット３を用いてトルクベース力制御でフィードバック制御を行う。即ち、図１３（ｃ）に示すように、下方向にトルクベース力制御で嵌合動作を行う。ここで、指示部５０１は、動作指令であるパラメータセット３を制御部５１０に送る時、ロボット２００の状態が合致したと判定された切替条件Ｃに関連付けされた新たな切替条件Ｅを、制御部５１０の監視部５１１に送る。

以上、第２実施形態によれば、トルクベース力制御中に切替条件に合致した場合において、制御部５１０が切替情報を切替部５０２に通知し、切替部５０２が通知を受けた切替情報をもとに条件分岐する。指示部５０１及び制御部５１０の動作により、力制御に係るパラメータを１回以上切り替えることが可能となる。即ち切替条件Ａに合致したら初期のパラメータセット１からパラメータセット２に切り替え、更に切替条件Ｃに合致したらパラメータセット２からパラメータセット３に切り替えるといった、多段で複雑な切り替えが容易に可能となる。

［第３実施形態］
次に、第３実施形態に係るロボットシステムについて説明する。図１４は、第３実施形態における指示部５０１及び制御部５１０Ａの制御ブロック図である。第２実施形態の制御部５１０Ａでは、図７に示す制御部５１０Ａに、パラメータ記憶部５２０が追加されている。また、力指令生成部５０３からの出力には、遷移時間ｔｓの情報が追加されている。

パラメータ記憶部５２０は、切り替え前のパラメータを保持することにある。切り替え前のパラメータを保持することで、力制御に係るパラメータの切り替えが生じた場合、パラメータ設定部５１６において、切り替え前のパラメータと切り替え後のパラメータとを補間関数により補間する。これにより、急激なパラメータの変化を防止する。補間方法としては、例えば線形補間や３次多項式、シグモイド関数等の補間関数が挙げられるが、これらに限るものではない。

簡単化のため、パラメータのひとつである力目標値Ｆ_ｒｅｆのみを補間する場合について説明する。図１５（ａ）は、補間をしなかった場合を示す手先の力目標値Ｆ_ｒｅｆの時間変化を示す模式図である。図１５（ｂ）は、補間をしなかった場合を示す手先力Ｆの時間変化を示す模式図である。図１５（ｃ）は、補間をした場合を示す手先の力目標値Ｆ_ｒｅｆの時間変化を示す模式図である。図１５（ｄ）は、補間をした場合を示す手先力Ｆの時間変化を示す模式図である。

図１５（ａ）に示すように、補間しない場合、ステップ状に力目標値Ｆ_ｒｅｆが切り替わる。このため、図１５（ｂ）のように、力目標値Ｆ_ｒｅｆに対して手先力Ｆがオーバーシュートする。一方、第３実施形態では、図１５（ｃ）に示すように、遷移時間ｔｓ内で力目標値Ｆ_ｒｅｆを補間関数を用いて補間する。図１５（ｃ）の例では、遷移時間ｔｓ内で線形補間している。このため、力制御部５１７で計算するトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}が急激に変化することが防止できる。結果として、図１５（ｄ）のように、手先力Ｆがオーバーシュートするのを防止できる。以上、第３実施形態によれば、手先力Ｆのオーバーシュートを防止できるので、ロボット２００により安定して作業を行うことができる。

［第４実施形態］
次に、第４実施形態に係るロボットシステムについて説明する。図１６は、第４実施形態におけるロボットプログラム５０６の一例を示す説明図である。第４実施形態では、監視部５１１に遅延時間機能が追加され、力指令生成部５０３から監視部５１１への出力に遅延時間の情報が追加されている。

ロボットプログラム５０６の第１２行目の切替条件Ａには、遅延時間が記述されている。具体的には監視部５１１において、トルクベース力制御の開始後、所定時間０．４［ｓ］の間は手先力Ｆの並進ｚ成分の判定を行わないことを意味する。

図１７は、第４実施形態において切替条件に合致するか否かの判定を示す概念図である。図１７には、切替条件が、手先力Ｆの６要素のうち一成分が閾値Ｔｈ＿ｆ＿ｚ２未満となる条件である場合について図示している。

位置制御後にトルクベース力制御を行う場合、図１７に示すように、トルクベース力制御の開始直後は、制御変更により手先力Ｆの変動が大きくなることがある。制御部５１０の監視部５１１は、指示部５０１により切替条件の入力を受けた時点から所定時間ｔａが経過した後にロボット２００の状態が切替条件に合致した場合に、指示部５０１に切替情報を送る。図１６及び図１７の例では、所定時間ｔａが遅延時間０．４［ｓ］である。所定時間ｔａ、例えば０．４［ｓ］の判定をしないマスク期間を設けることで、制御部５１０の監視部５１１の判定動作が安定する。

以上、第４実施形態によれば、制御部５１０の監視部５１１が遅延機能を有することで、操作者がロボットプログラム５０６に命令文を記述することにより、切替条件の判定の開始時刻を制御できるようになる。このため、ロボット２００の動き始めなどに生じる手先力Ｆや手先位置Ｐの大きな変動を制御部５１０の監視部５１１が無視することになり、ロボット２００の制御の安定性が向上する。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。

上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

また、上述の実施形態では、ＣＰＵ３０１が複数のコアを有し、複数のコアで指示部及び制御部の機能を実現する場合について説明したが、複数のＣＰＵ又は複数のコンピュータで指示部及び制御部の機能を実現する場合であってもよい。また、１つのコアのみを有する１つのＣＰＵで指示部及び制御部の機能を実現する場合であっても、複数のプロセス、即ち複数のアプリケーションソフトウェアを独立して実行できるように構成すればよい。即ち、ＣＰＵが第１のプロセスを実行して指示部として機能し、かつ第２のプロセスを実行して制御部として機能するようにしてもよい。演算負荷という点では、１つのコアを有する１つのＣＰＵで指示部及び制御部の機能を実現するよりも、複数のコア、複数のＣＰＵ又は複数のコンピュータで指示部及び制御部の機能を実現するほうがより好ましい。

また、上述の実施形態では、ロボットアームが垂直多関節のロボットアームの場合について説明したが、これに限定するものではない。ロボットアームが、例えば水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等であってもよい。

１００…ロボットシステム、２００…ロボット、３００…ロボット制御装置、５０１…指示部、５０６…ロボットプログラム、５１０…制御部、５４１…トルクセンサ、５５１…エンコーダ

Claims (16)

1. センサを有するロボットに関する複数の動作内容と、前記複数の動作内容の切替条件に関する値と、が記述されたロボットプログラムと、
   前記ロボットプログラムに基づき、前記複数の動作内容から第１の動作内容を前記ロボットに実行させるための第１動作指令を出力する第１制御部と、
   前記第１動作指令と前記センサの出力結果とに基づき前記ロボットの動作を制御し、前記センサの出力結果が前記切替条件と合致しているか判定する第２制御部と、を備え、
   前記第１制御部は、
   前記センサの出力結果が前記切替条件に合致すれば前記ロボットプログラムに基づき、第２の動作内容を前記ロボットに実行させるための第２動作指令を前記第２制御部に出力する、
   ことを特徴とする制御装置。
2. 前記第１制御部は、前記第２動作指令を前記第２制御部に出力する際、新たな切替条件も前記第２制御部に出力する、ことを特徴とする請求項１に記載の制御装置。
3. 前記第２制御部は、前記第１動作指令または前記第２動作指令と、前記センサの出力結果と、に基づく前記ロボットの動作の制御が開始されてから所定時間が経過した後に前記センサの出力結果が前記切替条件に合致したと判定すれば、前記センサの出力結果が前記切替条件に合致したという情報を前記第１制御部に出力する、ことを特徴とする請求項２に記載の制御装置。
4. 前記第１制御部は、更に外部装置の動作を制御することを特徴とする請求項１乃至３のいずれか１項に記載の制御装置。
5. 前記第１制御部は、前記第１動作指令および前記第２動作指令として、前記第２制御部による制御に係るパラメータを出力することを特徴とする請求項１乃至４のいずれか１項に記載の制御装置。
6. 前記パラメータには、前記ロボットの所定部位における力目標値及び位置目標値の少なくともいずれか一方が含まれることを特徴とする請求項５に記載の制御装置。
7. 前記パラメータには、前記第２制御部による制御において用いる演算式の係数が含まれていることを特徴とする請求項５又は６に記載の制御装置。
8. 前記第２制御部は、前記パラメータを切り替える際、補間関数によって補間することを特徴とする請求項５乃至７のいずれか１項に記載の制御装置。
9. 前記第２制御部は、前記センサの出力結果をフィードバックすることで前記ロボットの制御を行う、ことを特徴とする請求項１から８のいずれか１項に記載の制御装置。
10. 前記第１制御部と前記第２制御部は、ＣＰＵに設けられ、それぞれに割り当てられたコアにより、独立して演算できるように設けられている、ことを特徴とする請求項１乃至９のいずれか１項に記載の制御装置。
11. 前記第１制御部と前記第２制御部は、それぞれ、別々のＣＰＵに設けられ、独立して演算できるように設けられている、ことを特徴とする請求項１乃至９のいずれか１項に記載の制御装置。
12. 請求項１乃至１１のいずれか１項に記載の制御装置と、
    前記ロボットと、を備えたロボットシステム。
13. 請求項１２に記載のロボットシステムを用いて物品の製造を行うことを特徴とする物品の製造方法。
14. センサを有するロボットに関する複数の動作内容と、前記動作内容の切替条件に関する値と、が記述されたロボットプログラムと、
    前記ロボットプログラムに基づき、前記動作内容から第１の動作内容を前記ロボットに実行させるための第１動作指令を出力する第１制御部と、
    前記第１動作指令と前記センサの出力結果とに基づき前記ロボットの動作を制御し、前記センサの出力結果が前記切替条件と合致しているか判定する第２制御部と、により前記ロボットを制御する制御方法であって、
    前記第１制御部は、
    前記センサの出力結果が前記切替条件に合致すれば前記ロボットプログラムに基づき、第２の動作内容を前記ロボットに実行させるための第２動作指令を前記第２制御部に出力する、
    ことを特徴とする制御方法。
15. コンピュータに請求項１４に記載の制御方法を実行させるための制御プログラム。
16. 請求項１５に記載の制御プログラムを記録した、コンピュータにより読み取り可能な記録媒体。

Images