JP2019104091A - ロボット装置、ロボット装置の制御方法 - Google Patents

Abstract

【課題】多関節ロボットにおいては、前の動作に関する各関節の駆動を終了しても、例えばロボットアームの振動等により手先部の位置が変動している時間がある。この間に次の動作を開始させてしまうと、次の動作に関する手先部の位置姿勢の精度に影響が生ずる可能性がある。次の動作を開始させても精度が保証可能な範囲内に変動が収まった状態、即ち位置決め完了の判定にマージンを与えすぎると、ロボットの動作速度が低下してしまう。【解決手段】各関節軸にかかる力を検出するトルクセンサが検出した力を用いて多関節ロボットの先端にかかる力および／または多関節ロボットの先端の加速度を求め、力、および／または加速度が、所定範囲内に収束したか否かを判定することで位置決めが完了したかを判定する。【選択図】図３

Description

本発明は、複数の関節を有するロボット装置と、その制御方法に関する。特に、関節を駆動してロボットの位置姿勢を変更した際の位置決め完了を検知する技術に関する。

近年、様々なロボット装置が普及しているが、工場や作業現場等においては、複雑な作業を行わせるために複数の関節を備えた多関節ロボットの導入が盛んである。
例えば工場等において生産装置として多関節ロボットを用いる場合には、生産効率を高めるために、ロボットを高速に動作させる必要がある。生産工程中の一つの動作から次の動作に移行する際には、作業を行う手先部の位置姿勢の精度を保証するため、前の動作に関する各関節の駆動を確実に終了させてから次の動作に関する関節の駆動を開始させる。その際には、前の動作に関する各関節の駆動を終了したとしても、例えばロボットアームの振動等により手先部の位置が変動している間に次の動作を開始させてしまうと、次の動作に関する手先部の位置姿勢の精度に影響が生ずる可能性がある。

以後の説明では、一つの動作に関する各関節の駆動を終了させた後に、ロボットの作業部である手先部の位置姿勢の変動が、次の動作を開始させても精度を保証可能な所定範囲内に収まった状態になることを、位置決め完了と呼ぶ。

従来は、ロボットの操作者が経験則等により位置決め完了に要する時間を設定し、前の動作の各関節の駆動を終了した後、設定した時間の経過後に、次の動作の各関節の駆動を開始するように制御していた。

また、特許文献１には、各関節を駆動するサーボモータの位置、速度、トルク指令値からロボットの先端位置を推定し、この値が予め設定した許容範囲内となった場合に位置決め完了と判定する方法が提案されている。

また、特許文献２には、各関節の駆動機構の出力側に搭載した出力側エンコーダにより各関節の角度を検出し、検出角度に基づきロボットの先端の位置を求め、位置決め完了か否かを判定する方法が提案されている。

また、特許文献３には、ロボットの先端に作用する外力を検知するためにロボットの先端に搭載された力覚センサを用いて、ロボットの先端の振動を検知する方法が記載されている。

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

従来の、ロボットの操作者が経験則等により位置決め完了に要する時間を設定する方法では、操作者が試行錯誤的に時間を設定するため、次の動作を開始させた時に手先部の位置姿勢の精度が保証されない可能性があった。これを防止するために十分なマージンを見込んで操作者が時間を設定すると、次の動作を開始するまでの待機時間が長くなり、実効的な動作速度が低下して生産効率を高めることができなくなってしまう。

また、特許文献１に記載の技術では、関節を駆動するサーボモータの回転角度を検出するエンコーダの出力、つまり減速機入力側の検出角度値に基づいて、ロボットアームの先端位置を計算により推定している。減速機の入力側からロボットアームの先端までには、剛性が低く振動を起こしやすい減速機等が介在するので、先端の振動が収束したことを減速機の入力側の値で保証することは困難である。したがって、実際にはロボットアームの先端の振動が収束していない場合もあり、組み付け作業の精度が低下する場合があった。

また、特許文献２に記載の技術では、各関節の駆動機構の出力側に搭載した出力側エンコーダにより各関節の回転角度については検出できるが、回転軸と直交する関節軸の傾きは検出できなった。そのため、旋回動作のような回転軸と直交する方向に力が加わる動作においては、ロボットアームが撓み先端の振動が収束していない場合があり、位置決め完了を十分な精度で判定できない場合が発生する可能性があった。

また、特許文献３に記載の技術は、ロボットの先端に作用する外力を検知するためにロボット先端に搭載された力覚センサを用いるが、力覚センサより先に存在する慣性質量が小さいと、振動を検知する際の感度が低い場合があった。このため、この技術を位置決め完了の判定に適用しようとしても、十分な精度で判定できない場合が発生する可能性があった。

そこで、多関節ロボットの実効的な動作速度を向上させるため、関節を駆動してロボットの位置姿勢を変更した際の位置決め完了を、短時間内に高精度に検知する技術が求められていた。

本発明は、複数の関節を有するロボット装置であって、前記複数の関節の各関節軸を駆動するサーボモータと、前記各関節軸にかかる力を検出するトルクセンサと、前記各サーボモータをフィードバック制御するモータ制御部と、前記トルクセンサが検出した力を用いて前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める計算部と、前記計算部が求めた前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度が、所定範囲内に収束したか否かを判定する位置決め判定部と、を備えたことを特徴とするロボット装置である。

また、本発明は、複数の関節を有するロボット装置をコンピュータを用いて制御する制御方法であって、前記複数の関節の各関節軸にかかる力を検出するトルクセンサが検出した力を用いて、前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める計算工程と、前記計算工程で求めた前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度が、所定範囲内に収束したか否かを判定する位置決め判定工程と、を備えたことを特徴とするロボット装置の制御方法である。

本発明によれば、前の動作に関する各関節の駆動を終了させた後、ロボットの手先部の位置姿勢の変動が次の動作を開始させても精度を保証可能な範囲内に収まった状態、即ち位置決め完了に到達したか否かを、短時間内に高精度に検知することができる。本発明は、実効的な動作速度を向上させたロボット装置、およびロボット装置の制御方法を提供することができる。

第１実施形態に係るロボットシステムの斜視図である。 第１実施形態に係るロボット制御装置を示すブロック図である。 第１実施形態に係るロボットシステムの制御系を示す制御ブロック図である。 第１実施形態においてロボットのトルクベース力制御を説明するためのフローチャートである。 第１実施形態においてロボットの位置制御を説明するためのフローチャートである。 第１実施形態に係るロボット制御方法の各工程を示すフローチャートである。 第１実施形態に係る効果を示す図である。 第２実施形態に係るロボット制御方法の各工程を示すフローチャートである。 第３実施形態に係る準備工程を示すフローチャートである。 第３実施形態に係る位置決め完了に係る閾値と教示点のテーブルである。 第３実施形態に係るロボット制御方法の各工程を示すフローチャートである。

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

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

ロボット２００、即ちロボットアーム２５１は、複数の関節、例えば６つ関節Ｊ_１〜Ｊ_６を有している。ロボットアーム２５１は、各関節Ｊ_１〜Ｊ_６を各関節軸Ａ_１〜Ａ_６まわりにそれぞれ回転駆動する複数、例えば６つのサーボモータ２０１〜２０６を有している。ロボットアーム２５１は、複数のリンク２１０_０〜２１０_６が各関節Ｊ_１〜Ｊ_６で回転可能に連結されている。ロボットアーム２５１は、可動範囲の中であれば、任意の３次元位置で任意の３方向の姿勢に、ロボット２００の手先を向けることができる。
ロボット２００の手先の位置及び姿勢は、ロボットアーム２５１の基端、即ち台座１５０を基準とする座標系Ｔｏで表現される。ロボット２００の手先には、座標系Ｔｅが設定されている。ここで、ロボット２００の手先とは、第１実施形態では、ロボットハンド２５２が物体を把持していない場合には、ロボットハンド２５２のことである。ロボットハンド２５２が物体を把持している場合は、ロボットハンド２５２と把持している物体を含めてロボット２００の手先という。つまり、ロボットハンド２５２が物体を把持している状態であるか物体を把持していない状態であるかにかかわらず、ロボットアーム２５１の先端から先を手先という。

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

ロボットアーム２５１の内部には、各サーボモータ２０１〜２０６のモータ２１１〜２１６の駆動を制御するサーボ制御部２３０が配置されている。サーボ制御部２３０は、入力を受けた各関節Ｊ_１〜Ｊ_６に対応する各トルク指令値に基づき、各関節Ｊ_１〜Ｊ_６のトルクがトルク指令値に追従するよう、各モータ２１１〜２１６に電流を出力し、各モータ２１１〜２１６の駆動を制御する。なお、第１実施形態では、サーボ制御部２３０は、ロボットアーム２５１の内部に配置されているが、ロボットアーム２５１の外部、例えばロボット制御装置３００の筐体内部に配置されていてもよい。
また、ロボット２００のロボットアーム２５１は、各関節Ｊ_１〜Ｊ_６をそれぞれ制動する複数のブレーキ２３１〜２３６を有する。ブレーキ２３１〜２３６は、例えばディスクブレーキである。各ブレーキ２３１〜２３６を作動させることにより、各関節Ｊ_１〜Ｊ_６が動かないように各関節Ｊ_１〜Ｊ_６を固定することができる。

次に、ロボット制御装置３００について説明する。図２は、第１実施形態に係るロボット制御装置３００を示すブロック図である。ロボット制御装置３００は、コンピュータで構成されており、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）３０１を備えている。また、ロボット制御装置３００は、記憶部として、内部記憶装置であるＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）３０２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）３０３及びＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）３０４を備えている。また、ロボット制御装置３００は、ディスクドライブ３０５及びインタフェース３０６〜３１０を備えている。ＣＰＵ３０１、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、ディスクドライブ３０５、及びインタフェース３０６〜３１０は、互いに通信可能にバス３１１で接続されている。

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

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

インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、ＣＰＵ３０１の制御の下、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の記憶部である外部記憶装置３２２が接続可能に構成されている。
インタフェース３１０には、外部装置３２３が接続されている。ＣＰＵ３０１は、インタフェース３１０及びバス３１１を介して、外部装置３２３とデータの入出力を行う。外部装置３２３は、例えばＬＥＤを明滅する装置やＰＬＣ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｃｏｎｔｒｏｌｌｅｒ）といった装置である。

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

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

各関節Ｊ_１〜Ｊ_６には各センサ部２２１〜２２６が設けられ、センサ部２２１〜２２６は、エンコーダ５５１〜５５６と、トルクセンサ５４１〜５４６を備える。例えば関節Ｊ_１にはセンサ部２２１が設けられ、センサ部２２１にはエンコーダ５５１とトルクセンサ５４１とが配されている。他の関節Ｊ_２〜Ｊ_６についても、それぞれ同様の構成となっている。角度検出器であるエンコーダ５５１〜５５６は、ロータリエンコーダであり、モータ２１１〜２１６の回転角度に応じた計測値である角度値θ_１〜θ_６、および各関節Ｊ_１〜Ｊ_６の回転角度に応じた計測値である角度値ｑ_１〜ｑ_６を信号としてそれぞれ出力する。トルクセンサ５４１〜５４６は、各関節Ｊ_１〜Ｊ_６にかかる力に応じた計測値であるトルク値τ_１〜τ_６を信号としてそれぞれ出力する。

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

指示部５０１は、ロボットプログラム５０６に従って、ロボット２００の動作を制御部５１０に指示するものである。
制御部５１０は、指示部５０１の指示に従って、第１の制御である力制御、又は第２の制御である位置制御、のいずれか一方を選択してロボット２００の動作を制御する。

第１の制御である力制御を行う場合は、トルクベース力制御を実施する。トルクベース力制御では、各関節に配されたトルクセンサ５４１〜５４６が検出するトルク値τ_１〜τ_６、及び各関節に配されたエンコーダ５５１〜５５６が検出する角度値θ_１〜θ_６あるいはｑ_１〜ｑ_６に基づき、ロボット２００の動作をフィードバック制御する。
トルクベース力制御においては、制御部５１０は、フィードバック指令値として各関節のトルク指令値τ_ｒｅｆ１〜τ_ｒｅｆ６を求め、各関節のモータ制御部５３１〜５３６に所定の制御周期でこれを出力する。各モータ制御部５３１〜５３６は、トルクセンサ５４１〜５４６が検出する各トルク値τ_１〜τ_６と各トルク指令値τ_ｒｅｆ１〜τ_ｒｅｆ６との偏差が小さくなるように、各モータ２１１〜２１６に流す各電流Ｃｕｒ_１〜Ｃｕｒ_６を制御する。尚、トルクベース力制御については、後に図４を参照して詳しく説明する。

第２の制御である位置制御を行う場合は、ロボット２００の各関節に配されたエンコーダ５５１〜５５６が検出する角度値θ_１〜θ_６あるいはｑ_１〜ｑ_６に基づき、ロボット２００の動作をフィードバック制御する。
位置制御においては、制御部５１０は、フィードバック指令値として、各関節の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を求め、各関節のモータ制御部５３１〜５３６に出力する。各モータ制御部５３１〜５３６は、各角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を各モータ２１１〜２１６の角度指令値θ_ｒｅｆ１〜θ_ｒｅｆ６に変換する。各モータ制御部５３１〜５３６は、各エンコーダ５５１〜５５６が検出する各モータの角度値θ_１〜θ_６と各角度指令値θ_ｒｅｆ１〜θ_ｒｅｆ６との偏差が小さくなるように、各モータ２１１〜２１６に流す各電流Ｃｕｒ_１〜Ｃｕｒ_６を制御する。なお、各モータ制御部５３１〜５３６は、モータの回転角度である各角度値θ_１〜θ_６の代わりに、関節の回転角度である各角度値ｑ_１〜ｑ_６を用いて制御してもよい。この場合、各モータ制御部５３１〜５３６は、各エンコーダ５５１〜５５６が検出する各角度値ｑ_１〜ｑ_６と各関節の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６との偏差が小さくなるように、各モータ２１１〜２１６に流す電流Ｃｕｒ_１〜Ｃｕｒ_６を制御する。尚、位置制御については、後に図５を参照して詳しく説明する。

尚、図３においては、図示の便宜のためτ_ｒｅｆ１〜τ_ｒｅｆ６の各信号を個別に表記するのではなく、まとめてτ_{ｒｅｆ１―６}と表記している。また、ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６、Ｃｕｒ_１〜Ｃｕｒ_６、θ_１〜θ_６、τ_１〜τ_６、ｑ_１〜ｑ_６についても、図３においては同様の方法でまとめて表記している。

手先計算部５２０は、各関節に配されたトルクセンサが検出する各軸のトルク値τ_１〜τ_６、各関節に配されたエンコーダが検出する各角度値ｑ_１〜ｑ_６、及びロボットモデル５１９を用いて、手先力Ｆ、手先加速度Ａｃｃの計算を行う。尚、場合によっては、各関節に配されたエンコーダが検出する各角度値ｑ_１〜ｑ_６の代わりに、角度指示値としての各角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を用いて手先力Ｆ、手先加速度Ａｃｃの計算を行うように構成してもよい。
具体的には、手先計算部５２０は、下記の変数をもとに、手先力Ｆ、手先加速度Ａｃｃを求める計算を行う。
・ロボットモデル５１９、各角度値ｑ_１〜ｑ_６及び各角度値ｑ_１〜ｑ_６の微分値から算出されるヤコビアンＪ。
・逆動力学計算とヤコビアンＪにより算出される慣性行列Λ。
これらの変数を用いて、下記の数式１を計算する。

位置決め閾値記憶部５２１は、ロボット２００の静止状態、例えばロボット２００の起動直後の一定時間の手先計算部５２０で計算した手先力Ｆ及び手先加速度Ａｃｃの履歴から算出される手先力閾値Ｆ_ｔｈ、手先加速度閾値Ａｃｃ_ｔｈを保持する。
具体的に説明すると、位置決め閾値記憶部５２１は、下記の変数をもとに、手先力閾値Ｆ_ｔｈ、手先加速度閾値Ａｃｃ_ｔｈの計算を行う。
・手先力Ｆの履歴の平均であるＦ_ａｖｅ
・手先力Ｆの履歴の標準偏差であるＦ_σ
・手先力Ｆの履歴における最大振幅値であるＦ_{Ｗｏｒｓｔ}
・手先加速度Ａｃｃの履歴の平均であるＡｃｃ_ａｖｅ
・手先加速度Ａｃｃの履歴の標準偏差であるＡｃｃ_σ
・手先加速度Ａｃｃの履歴における最大振幅値であるＡｃｃ_{Ｗｏｒｓｔ}
・係数ｎ_Ｆ
・係数ｎ_Ａｃｃ
・係数ｍ_Ｆ
・係数ｍ_Ａｃｃ
これらの変数を用いて、下記の数式２もしくは数式３を計算する。

尚、数式２、数式３は例示であり、これ以外の数式でもよい。
係数ｎ_Ｆ、係数ｎ_Ａｃｃ、係数ｍ_Ｆ及び係数ｍ_Ａｃｃは、手先の振動が十分に収束してから次の動作を行いたい場合は小さな値を設定する。逆に、多少の手先の振動を許容し、早く次の動作を行いたい場合は、大きめの値を設定する。
比較的大きな振動を許容できる教示点には係数ｎ_Ｆ、係数ｎ_Ａｃｃ、係数ｍ_Ｆ及び係数ｍ_Ａｃｃを大きな値に設定し、振動が許容できない教示点には小さな値に設定するように、個別の教示点ごとに最適化する調整も可能である。教示点ごとに手先力閾値Ｆ_ｔｈ、手先加速度閾値Ａｃｃ_ｔｈを最適化すれば、各動作毎に必要な手先部の位置姿勢精度を担保しながら、次の動作を開始するまでの位置決め完了に要する時間を短縮することができる。
なお、手先力閾値Ｆ_ｔｈ、手先加速度閾値Ａｃｃ_ｔｈは、ティーチングペンダント４００等を用いて、操作者が入力して設定してもよい。あるいは、ロボットプログラム５０６に記載して、予め指定しておいても良い。

判定部である位置決め検知部５２２は、手先計算部５２０から出力される手先力Ｆ及び手先加速度Ａｃｃの片方もしくは両方が所定時間、位置決め閾値記憶部５２１に記憶された閾値以内に収束したら、指示部５０１に対して位置決め完了通知を送る。すなわち、ロボット装置の先端にかかる力および／またはロボット装置の先端の加速度が、所定範囲内に収束したか否かを判定して通知する。

尚、位置決め完了の判定に手先力Ｆと手先加速度Ａｃｃの片方のみを用いるか、もしくは両方を使うかは、ロボットプログラム５０６やティーチングペンダントなどで操作者が指定できるものとする。

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

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

各モータ制御部５３１〜５３６は、各トルク指令値τ_ｒｅｆ１〜τ_ｒｅｆ６を実現するよう、各トルクセンサ５４１〜５４６が計測したトルク値τ_１〜τ_６に基づいて各モータ２１１〜２１６へ電流Ｃｕｒ_１〜Ｃｕｒ_６を通電する通電制御を行う（Ｓ３）。
各モータ２１１〜２１６は、通電されることによりトルクを発生して各関節を駆動する（Ｓ４）。

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

制御部５１０は、ロボットモデル５１９及び角度値ｑ_１〜ｑ_６に基づいて、ロボット２００の手先の位置Ｐを算出する。制御部５１０は、ロボットモデル５１９、角度値ｑ_１〜ｑ_６及びトルク値τ_１〜τ_６に基づき、ロボット２００の手先にかかる手先力Ｆを算出する（Ｓ６）。
制御部５１０は、駆動が終了したか否かを判定し（Ｓ７）、終了していない場合は、ステップＳ２〜Ｓ７を繰り返す。

以上の制御手順に従って各モータ２１１〜２１６を駆動することで、ロボット２００の手先力Ｆを力目標値Ｆ_ｒｅｆに制御することが可能である。なお、トルクベース力制御の制御手順は、図４に示すフローチャートに限定されるものではなく、手順の順番を変更することもあり得る。

次に、図５のフローチャートを参照して、本実施形態におけるロボット２００の位置制御について説明する。
まず、操作者が位置目標値Ｐ_ｒｅｆをティーチングペンダント４００を用いて設定する（Ｓ１１）。位置目標値Ｐ_ｒｅｆは、外部記憶装置３２２に記憶される。なお、位置目標値Ｐ_ｒｅｆは、ティーチングペンダント４００を使わずに、ロボットプログラム５０６に直接記述してもよい。
制御部５１０は、ロボットモデル５１９に基づき、位置目標値Ｐ_ｒｅｆを各関節Ｊ_１〜Ｊ_６の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６に変換するための計算を行い、角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を算出する（Ｓ１２）。

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

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

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

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

次に、図６のフローチャートを参照して、第１実施形態に係るロボット制御方法の各工程を説明してゆく。
まず、ロボットシステム１００は、操作者からのティーチングペンダント４００での動作開始指示をうけて、ロボット２００のモータ２１１から２１６に通電を行う。その後、位置決め閾値記憶部において位置決め完了に用いる手先力閾値Ｆ_ｔｈと手先加速度閾値Ａｃｃ_ｔｈの計算を行う（Ｓ２１）。
指示部５０１は、ロボットプログラム５０６と教示データ５０８から最初の動作指令を生成し、制御部５１０に送信する（Ｓ２２）。

動作指令が、位置制御かトルクベース力制御かによって、分岐を行う（Ｓ２３）。
制御方法が位置制御の場合は、前述の図５のフローチャートに従い、位置制御を行う（Ｓ２４）。
制御方法がトルクベース力制御の場合は、前述の図４のフローチャートに従い、トルクベース力制御を行う（Ｓ２５）。
手先計算部５２０は、各エンコーダ５５１〜５５６及び各トルクセンサ５４１〜５４６の値を取得し、手先力Ｆ及び手先加速度Ａｃｃを計算する（Ｓ２６）。

位置決め検知部５２２は、手先計算部５２０からえられる手先力Ｆ及び手先加速度Ａｃｃのうち片方、もしくは両方を、位置決め閾値記憶部５２１に記憶された閾値と比較し、位置決め完了か否かを判定する（Ｓ２７）。
位置決め完了と判定されれば、位置決め完了通知を指示部５０１に送信する。位置決め完了と判断されなければ、ロボットの振動が収まるのを所定の時間待つ静定待ちをする（Ｓ２８）。なお、指定時間内に位置決め完了判定が完了しない場合は、エラー終了とする（不図示）。
Ｓ２８で所定の時間を待つことで、センサ情報が更新される（Ｓ２９）。

位置決め完了と判断した場合は、指示部５０１はロボットプログラム５０６を解釈し、動作が完了したかを判断する（Ｓ３０）。次の動作がなければ動作完了とする。次の動作が存在する場合はＳ２２に遷移する。

以上説明したように、第１実施形態に係るロボットシステム１００によれば、ロボット先端位置の振動を、各トルクセンサ５４１〜５４６により手先力Ｆもしくは手先加速度Ａｃｃを算出して判定することにより、正確に求められるようになっている。
振動が収束するまでの時間、つまり位置決め完了までの時間を高精度に把握できるため、静定待ちの時間が不必要に長くなるのを防止することができる。また、事前に位置決め完了時間を調整する必要がないため、設定を含めた作業の効率が向上する。

図７は、手先力Ｆを力覚センサで検知する場合と、トルクセンサで検知した場合を示した比較グラフである。なお、説明の簡易化のために、手先力Ｆの６要素のうち１要素のみをプロットしている。ロボットの先端に取り付けた力覚センサに比べて、トルクセンサではロボット各軸で力を検知するため、感度良く振動を検知できる。つまり、本実施形態によれば、力覚センサで振動を検知するよりも精度よく位置決め完了の判定が可能となり、静定待ちの時間を短縮することができる。

［第２実施形態］
次に、第２実施形態に係るロボットシステムについて説明する。なお、第２実施形態の全体構成は、第１実施形態と同様であるため説明を省略し、第１実施形態と異なる点を中心に説明する。
図８は、第２実施形態に係るロボット制御方法の各工程を示すフローチャートである。図６に示した第１実施形態のフローチャートとの違いは、トルクベース力制御（Ｓ３５）の遷移先が、動作完了（Ｓ４０）となっている。つまり、トルクベース力制御時には位置決め完了検知を行わないことになる。

一般的に、位置制御に比べてトルクベース力制御の計算量は多い。計算量の少ない位置制御時のみ位置決め完了検知を行うことで、ロボットシステム１００全体としての計算量の増加を防ぐことが可能となる。

以上、第２実施形態によれば、位置制御時には位置決め完了検知を行うことで、精度よく位置決め完了の判定が可能となり、静定待ちの時間を短縮することができる。一方、トルクベース力制御時には位置決め完了検知を行わないことでロボットシステム１００全体の計算量の増加を防ぎ、ＣＰＵ３０１にかけるコスト増を抑えることが可能となる。

［第３実施形態］
次に、第３実施形態に係るロボットシステムについて説明する。なお、第３実施形態の全体構成は、第１実施形態と同様であるため説明を省略し、第１実施形態と異なる点を中心に説明する。
第１実施形態では、全ての動作教示点で、共通の手先力閾値Ｆ_ｔｈ、手先加速度閾値Ａｃｃ_ｔｈを用いているが、ロボット２００の位置姿勢により、静止状態の手先力Ｆや手先加速度Ａｃｃが異なる場合がある。いろいろな位置姿勢でも精度を保証できるようにするためには、余裕をもった閾値を設定する必要があり、教示点によっては余裕分により静定待ちのロボット停止時間が必要以上に長くなる場合があり得た。

第３実施形態では、全ての動作において同一の手先力閾値Ｆ_ｔｈ、手先加速度閾値Ａｃｃ_ｔｈを用いて位置決め完了の判定を行うのではなく、適宜の閾値を用いれば位置決め判定時間が短縮できる教示点に関して、閾値の取得を個別に行う。比較的短時間のうちに振動が収束すると見込まれる教示点に関して、それに相応した手先力閾値Ｆ_ｔｈ、手先加速度閾値Ａｃｃ_ｔｈを設定することで、位置決め完了の判定時間を短縮することができる。

図９は、第３実施形態に係る準備工程を示すフローチャートである。
まず、操作者が指定した教示点に移動する（Ｓ４１）。
指定した教示点において、Ｓ２１と同様に、位置決め閾値記憶部において位置決め完了に用いる手先力閾値Ｆ_ｔｈと手先加速度閾値Ａｃｃ_ｔｈの計算を行う（Ｓ４２）。

個別に閾値を取得すべき教示点が複数存在する場合は、図９のフローを複数回行う。このようにして、指定した教示点について個別の手先力閾値Ｆ_ｔｈと手先加速度閾値Ａｃｃ_ｔｈを求めて記憶する。図１０は、教示点番号００１と教示点番号００５の教示点に対して閾値を取得した場合のテーブルを例示したものである。なお、教示点番号０００は、特定の閾値が付与されていないその他の教示点に適応される閾値を示している。

図１１は、第３実施形態に係るロボット制御方法の各工程を示すフローチャートである。第１実施形態との違いは、閾値計算（Ｓ５１）において算出される閾値は、前述の図１０の教示点番号０００に格納することである。更に、位置決め完了（Ｓ５７）に用いる閾値は、動作に用いる教示点が図１０のテーブルに存在する場合はその閾値を、それ以外は教示点番号０００に格納した閾値を用いる。このようにすることにより、教示点毎に適宜異なる閾値を用いて判定できる。

以上、第３実施形態によれば教示点毎に手先力閾値Ｆ_ｔｈや手先加速度閾値Ａｃｃ_ｔｈを取得して用いるため、第１実施形態よりも更に位置決め完了判定時間を短縮することが期待できる

［他の実施形態］
本発明の実施形態は、上述した第１〜第３の実施形態に限られるものではなく、本発明の基本思想の範囲内で、適宜変更したり組み合わせたりすることが可能である。上述した実施形態の多関節ロボットおよびその制御方法を用いて、ワークの組み立て作業を行い、物品を製造すれば、多関節ロボットの手先の位置姿勢を高い精度で高速に制御できるため、高品位の物品を短時間に製造することができる。また、種々の工具をエンドエフェクタとして実施形態の多関節ロボットに装着すれば、ワークの組み立て作業に限らず様々な加工作業を高速に実行することが可能である。

１００・・・ロボットシステム／２００・・・ロボット／２０１〜２０６・・・サーボモータ／２２１〜２２６・・・センサ部／２５１・・・ロボットアーム／２５２・・・ロボットハンド／３００・・・ロボット制御装置／４００・・・ティーチングペンダント／５０６・・・ロボットプログラム／５１９・・・ロボットモデル／５３１〜５３６・・・モータ制御部／５４１〜５４６・・・トルクセンサ／５５１〜５５６・・・エンコーダ／Ｃｕｒ_１−６・・・電流／Ｊ_１−６・・・関節／ｑ_１−６・・・角度値／ｑ_{ｒｅｆ１−６}・・・角度指令値／θ_１−６・・・角度値／τ_１−６・・・トルク値／τ_{ｒｅｆ１−６}・・・トルク指令値

Claims (9)

1. 複数の関節を有するロボット装置であって、
   前記複数の関節の各関節軸を駆動するサーボモータと、
   前記各関節軸にかかる力を検出するトルクセンサと、
   前記各サーボモータをフィードバック制御するモータ制御部と、
   前記トルクセンサが検出した力を用いて前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める計算部と、
   前記計算部が求めた前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度が、所定範囲内に収束したか否かを判定する位置決め判定部と、
   を備えたことを特徴とするロボット装置。
2. 前記各関節軸の角度を検出する角度検出器を備え、
   前記計算部は、前記トルクセンサが検出した力と前記角度検出器が検出した角度を用いて前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める、
   ことを特徴とする請求項１に記載のロボット装置。
3. 前記計算部は、前記トルクセンサが検出した力と前記サーボモータを制御するための角度指示値を用いて前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める、
   ことを特徴とする請求項１に記載のロボット装置。
4. 複数の関節を有するロボット装置をコンピュータを用いて制御する制御方法であって、
   前記複数の関節の各関節軸にかかる力を検出するトルクセンサが検出した力を用いて、前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める計算工程と、
   前記計算工程で求めた前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度が、所定範囲内に収束したか否かを判定する位置決め判定工程と、
   を備えたことを特徴とするロボット装置の制御方法。
5. 前記計算工程は、前記トルクセンサが検出した力と、前記複数の関節の各関節軸の角度を検出する角度検出器が検出した角度を用いて、前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める、
   ことを特徴とする請求項４に記載のロボット装置の制御方法。
6. 前記計算工程は、前記トルクセンサが検出した力と、前記複数の関節の角度を制御するための角度指示値を用いて、前記ロボット装置の先端にかかる力および／または前記ロボット装置の先端の加速度を求める、
   ことを特徴とする請求項４に記載のロボット装置の制御方法。
7. 請求項４乃至６の中のいずれか１項に記載のロボット装置の制御方法の各工程を、前記コンピュータに実行させるためのプログラム。
8. 請求項７に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
9. 請求項４乃至６の中のいずれか１項に記載のロボット装置の制御方法により、前記ロボット装置を制御してワークの組み立て作業を行う、
   ことを特徴とする物品の製造方法。