JP2017056549A - ロボット装置、ロボット制御方法、プログラム、記録媒体及び組立部品の製造方法 - Google Patents

Abstract

【課題】制御の途中で停止命令を受けた場合、ロボットやワークにかかる負荷を減らしてロボットの動作を停止させる。【解決手段】ロボット２００は、電動モータ２１１〜２１６と、トルクセンサ５４１〜５４６とを有する。ロボット制御装置３００は、手先に作用する力と力目標値との力偏差が小さくなるように各電動モータ２１１〜２１６に対するトルク指令値を求め、各トルク指令値に基づき各電動モータ２１１〜２１６の駆動を制御し、停止命令を受けたとき、力目標値を減衰させて、ロボット２００の動作を停止させる。【選択図】図３

Description

本発明は、力制御の途中で停止命令を受けた場合にロボットの動作を停止させるロボット装置、ロボット制御方法、プログラム、記録媒体及び組立部品の製造方法に関する。

ロボットは、主として位置制御と力制御のいずれかで制御が行われている。位置制御では、ワークとの接触を伴うような作業を行う際に、ワークの位置誤差などがあるとワークやロボットに負荷がかかり作業が困難になる。

このため、力制御として、位置ベース力制御（アドミッタンス制御）が行われる。位置ベース力制御では、ロボットの先端部に設けられた力センサの値に基づきワークに倣うような位置指令値を計算する。位置ベース力制御の途中で停止命令をうけた場合、位置制御に切り替えてロボットの減速停止を行っていた（特許文献１参照）。

しかし、位置ベース力制御は、位置制御をマイナーループとして利用するため、その応答性は、位置制御の応答性よりも低下する。一般的に位置制御の応答周波数は、ロボットの固有振動数に制約される。特に、接触時は、安定性の問題から応答性が高めることが難しい。

このため、応答性の高い力制御を行うロボットの開発が行われている。具体的には、トルク指令値で力制御を行うことで、ロボットの固有振動数に制約を受けずに応答性を高めることができるトルクベース力制御が開発されている。

特許第５３７１８８２号公報

しかしながら、ロボットには部品製作誤差に起因する機差があり、位置決め誤差が生じる。機差を正確に把握することは困難であり、ロボットを位置制御で動作させる場合、目標位置と少しずれた位置に移動することになる。

このため、応答性の高いトルクベース力制御において、上記特許文献１のように力制御で倣っている状態から位置制御に切り替えて停止させた際に、ロボットの機差の影響により位置制御における目標位置からずれた位置でロボットが停止する。このため、ロボットとワーク又はロボットが把持しているワークと別のワークとが接触している場合は、ロボット及びワークの双方に過負荷がかかるという問題があった。

更に、トルクベース力制御は応答性が高い。速度が速いと停止命令を受けた際にロボットが減速停止するまでの時間がかかる。このため、ロボットが停止目標位置で停止できずに行き過ぎてしまう可能性がある。ロボットが停止目標位置に対して行き過ぎた場合は、同様にロボットやワークに過負荷がかかるという問題があった。

例えば、トルクベース力制御を用いてペグインホールといった嵌合動作を行う場合、位置制御で減速停止させると、機差から生じる位置ずれや応答性が高いために生じる行き過ぎがある。このため、ワークに無理な力がかかり、ワークが嵌合穴に引っ掛かることがあった。嵌合穴に引っ掛かったワークを外すことは困難なため、復帰に時間がかかるという問題があった。嵌合動作以外でも、ワークをある一定の力で押しつけるような押しつけ動作の途中に、位置制御で減速停止させると、過剰な力で押しつけることになり、前述のようにロボットやワークに過負荷がかかるという問題があった。

そこで、本発明は、力制御の途中で停止命令を受けた場合、ロボットやワークにかかる負荷を減らしてロボットの動作を停止させることを目的とする。

本発明のロボット装置は、各関節を駆動する複数のモータと、手先に作用する力に応じた信号を生成するセンサとを有するロボットと、前記手先に作用する力に応じた信号を生成するセンサから信号を取得して、前記手先に作用する力と力目標値との力偏差が小さくなるように、前記各モータに対するトルク指令値を求め、前記各モータに対するトルク指令値に基づき前記各モータの駆動を制御し、停止命令を受けたとき、前記力目標値を減衰させて、前記各モータに対するトルク指令値を求め、前記各モータの駆動を制御することで、前記ロボットの動作を停止させる停止処理を実行する制御部と、を備えたことを特徴とする。

本発明によれば、トルクベース力制御を行っている途中で停止命令を受けた際に、ロボットやワークにかかる負荷を減らしながらロボットの動作を停止させることが可能となる。

第１実施形態に係るロボット装置の概略構成を示す斜視図である。 第１実施形態に係るロボット装置のロボット制御装置を示すブロック図である。 第１実施形態に係るロボット装置の制御系を示す制御ブロック図である。 第１実施形態においてロボットを力制御する際のフローチャートである。 第１実施形態においてロボットを位置制御する際のフローチャートである。 第１実施形態におけるロボットにより組付け作業を行っている状態を示す模式図である。 図３の力制御部の詳細を示すブロック図である。 第１実施形態に係る組立部品の製造方法を示すフローチャートである。 （ａ）〜（ｃ）は、時間に対する力目標値を示すグラフである。 時間に対する手先の位置及び位置目標値のグラフである。 （ａ）は、トルクベース力制御を維持したまま停止処理を行ったときのロボットハンドの変位を示すグラフである。（ｂ）は、位置制御に切り替えて停止処理を行ったときのロボットハンドの変位を示すグラフである。 （ａ）〜（ｄ）は、時間に対する速度目標値Ｐ_ｒｅｆ（・）を示すグラフである。 （ａ）〜（ｂ）は、時間に対する剛性係数Ｋ_ｒｅｆを示すグラフである。 （ａ）〜（ｂ）は、時間に対する粘性係数Ｄ_ｒｅｆを示すグラフである。 第５実施形態に係るロボット装置の制御系を示す制御ブロック図である。 ワーク接触時の時間に対する手先力Ｆの変化を示すグラフである。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、第１実施形態に係るロボット装置の概略構成を示す斜視図である。図１に示すように、ロボット装置１００は、多関節のロボット２００と、ロボット２００の動作を制御する制御部としてのロボット制御装置３００と、を備えている。また、ロボット装置１００は、ロボット制御装置３００に教示データを送信する教示装置としてのティーチングペンダント４００を備えている。ティーチングペンダント４００は、操作者が操作するものであり、ロボット２００やロボット制御装置３００の動作を指定するのに用いる。

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

ロボット２００、即ちロボットアーム２５１は、複数の関節、例えば６つ関節（６軸）Ｊ_１〜Ｊ_６を有している。ロボットアーム２５１は、各関節Ｊ_１〜Ｊ_６を各関節軸Ａ_１〜Ａ_６まわりにそれぞれ回転駆動する複数（６つ）のサーボモータ２０１〜２０６を有している。

ロボットアーム２５１は、複数のリンク（フレーム）２１０_０〜２１０_６が各関節Ｊ_１〜Ｊ_６で回転可能に連結されている。ここで、基端側から先端側に向かって、リンク２１０_０〜２１０_６が順に直列に連結されている。ロボットアーム２５１は、可動範囲の中であれば、任意の３次元位置で任意の３方向の姿勢に、ロボット２００の手先（ロボットアーム２５１の先端）を向けることができる。

ロボットアーム２５１の位置及び姿勢は、座標系で表現することができる。座標系Ｔｏは、ロボットアーム２５１の基端、即ち台座Ｂに固定した座標系を表し、座標系Ｔｅはロボット２００の手先（ロボットアーム２５１の先端）に固定した座標系を表す。

ここで、ロボット２００の手先とは、第１実施形態では、ロボットハンド２５２が物体を把持（支持）していない場合には、ロボットハンド２５２のことである。ロボットハンド２５２が物体を把持（支持）している場合は、ロボットハンド２５２と把持（支持）している物体（例えば部品やツール等）とを含めてロボット２００の手先という。つまり、ロボットハンド２５２が物体を把持している状態であるか物体を把持していない状態であるかにかかわらず、ロボットアーム２５１の先端から先を手先という。

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

ロボットアーム２５１の内部には、各サーボモータ２０１〜２０６の電動モータ２１１〜２１６の駆動を制御する駆動制御部としてのサーボ制御部２３０が配置されている。

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

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

また、第１実施形態では、ティーチングペンダント４００は、操作者の操作によりロボット制御装置３００に停止命令を送信する停止操作部としての停止ボタン４０１を有する。

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

ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、各種のインタフェース３０６〜３０９が、バス３１０を介して接続されている。ＲＯＭ３０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。

ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ３０１に、後述する演算処理を実行させるためのプログラム３３０を記録するものである。ＣＰＵ３０１は、ＨＤＤ３０４に記録（格納）されたプログラム３３０に基づいてロボット制御方法の各工程を実行する。

記録ディスクドライブ３０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。

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

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

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

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

図３は、第１実施形態に係るロボット装置の制御系を示す制御ブロック図である。ロボット制御装置３００のＣＰＵ３０１は、プログラム３３０を実行することにより、力検出部５０４、力制御部５０５及び位置目標値生成部５０６として機能する。サーボ制御部２３０は、複数（６関節であるので６つ）の切替制御部５１１〜５１６と、複数（６関節であるので６つ）の位置制御部５２１〜５２６と、複数（６関節であるので６つ）のモータ制御部５３１〜５３６として機能する。

各センサ部２２１〜２２６は、各角度センサ（位置センサ）５５１〜５５６と、各トルクセンサ５４１〜５４６とを有する。各角度センサ５５１〜５５６は、各電動モータ２１１〜２１６又は各関節Ｊ_１〜Ｊ_６の角度（位置）をそれぞれ検知する、即ち角度（位置）に応じた信号を生成するロータリエンコーダ（エンコーダ）である。

第１実施形態では、各角度センサ５５１〜５５６は、各電動モータ２１１〜２１６の角度θ_１〜θ_６を直接検知する。なお、各関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６は、不図示の減速機の減速比等に基づき、各角度θ_１〜θ_６から求めることができる。したがって、各角度センサ５５１〜５５６は、各関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６を間接的に検知していることになる。

各トルクセンサ５４１〜５４６は、各関節Ｊ_１〜Ｊ_６のトルクτ_１〜τ_６をそれぞれ検知する、即ちトルクに応じた信号を生成するセンサである。複数の角度センサ５５１〜５５６でロボット２００の手先の位置（姿勢も含む）Ｐを検知する、即ち位置Ｐに応じた信号を生成するセンサである位置検知部５５０が構成されている。また、複数のトルクセンサ５４１〜５４６でロボット２００の手先に作用する力（手先力）Ｆを検知する、即ち力Ｆに応じた信号を生成するセンサである力検知部５４０が構成されている。

ティーチングペンダント４００は、操作者の操作により、力教示データ５０１に含まれる力目標値Ｆ_ｒｅｆと、位置教示データ５０２に含まれる位置目標値Ｐ_ｒｅｆとをＣＰＵ３０１に出力する。力目標値Ｆ_ｒｅｆは、ロボット２００の手先力の目標値であり、操作者がティーチングペンダント４００を用いて設定する。位置目標値Ｐ_ｒｅｆは、ロボット２００の手先位置の目標値であり、操作者がティーチングペンダント４００を用いて設定する。外部記憶装置３２２にはロボットモデル５０３が記憶されている。

また、ティーチングペンダント４００は、停止ボタン４０１が操作者に操作されたとき、停止命令をＣＰＵ３０１の力制御部５０５に出力する。

力検出部５０４は、各トルクセンサ５４１〜５４６から取得したトルクτ_１〜τ_６及び各角度センサ５５１〜５５６から取得した角度ｑ_１〜ｑ_６に応じた信号を用いて、ロボット２００の手先の現在位置Ｐ（ｔ）とそのときのロボット２００の手先力Ｆを求める。このとき力検出部５０４は、ロボット２００の手先の現在位置Ｐ（ｔ）を、各角度センサ５５１〜５５６から取得した角度ｑ_１〜ｑ_６に応じた信号を用いて求める。なお、力検出部５０４は、手先に取り付けられた力覚センサ（不図示）を用いて、ロボット２００の手先力Ｆを算出してもよい。

力制御部５０５は、ロボットモデル５０３（仮想質量Ｍ_ｒｅｆ）、力目標値Ｆ_ｒｅｆ、位置目標値Ｐ_ｒｅｆ、剛性係数Ｋ_ｒｅｆ、粘性係数Ｄ_ｒｅｆ、現在位置Ｐ（ｔ）及び力Ｆの値を示す信号の入力を受ける。そして、力制御部５０５は、これら値を用いて、各関節Ｊ_１〜Ｊ_６に対するトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を求める。このとき、手先力Ｆと力目標値Ｆ_ｒｅｆとの力偏差、手先の位置Ｐ（ｔ）と位置目標値Ｐ_ｒｅｆとの位置偏差、及び手先の速度Ｐ（ｔ）（・）と速度目標値Ｐ_ｒｅｆ（・）との速度偏差が小さくなるようにトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を求める。力制御部５０５は、求めた各トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を各切替制御部５１１〜５１６に出力する。ここで、速度Ｐ（ｔ）（・）は、位置Ｐ（ｔ）を時間で微分することにより求まり、速度目標値Ｐ_ｒｅｆ（・）は、位置目標値Ｐ_ｒｅｆを時間で微分することにより求まる。したがって、第１実施形態では、力制御部５０５が位置検知部５５０の検知結果である手先の位置Ｐ（ｔ）から手先の速度Ｐ（ｔ）（・）を求めることになる。なお、位置検知部５５０が演算を行って手先の速度Ｐ（ｔ）（・）のデータそのものを出力するようにしてもよく、この場合は、位置検知部５５０が手先の速度を検知する速度検知部、つまり手先の速度に対応する信号を生成するセンサも兼ねることになる。なお、（・）は、時間で１階微分することを意味する。

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

各位置制御部５２１〜５２６は、各関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６と各関節Ｊ_１〜Ｊ_６の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６との角度偏差が小さくなるようトルク指令値τ_{ＭＰｒｅｆ１}〜τ_{ＭＰｒｅｆ６}を求める。なお、この角度偏差を小さくすることは、各電動モータ２１１〜２１６の角度と角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６を減速機の減速比等で換算した角度指令値との角度偏差を小さくすることと等価である。各位置制御部５２１〜５２６は、各トルク指令値τ_{ＭＰｒｅｆ１}〜τ_{ＭＰｒｅｆ６}を各切替制御部５１１〜５１６に出力する。

各切替制御部５１１〜５１６は、トルクベース力制御を行う力制御モードと、位置制御を行う位置制御モードとを切り替え制御する。各切替制御部５１１〜５１６は、力制御モード時は、各トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を各トルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}として各モータ制御部５３１〜５３６に出力する。また、各切替制御部５１１〜５１６は、位置制御モード時は、各トルク指令値τ_{ＭＰｒｅｆ１}〜τ_{ＭＰｒｅｆ６}を各トルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}として各モータ制御部５３１〜５３６に出力する。

各モータ制御部５３１〜５３６は、各電動モータ２１１〜２１６の角度（位置）θ_１〜θ_６に基づき、各トルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}を実現するように各電流Ｃｕｒ_１〜Ｃｕｒ_６を各電動モータ２１１〜２１６に通電する。

次に、ロボット２００の力制御について説明する。図４は、第１実施形態においてロボット２００を力制御する際のフローチャートである。

まず、操作者が力目標値Ｆ_ｒｅｆと位置目標値Ｐ_ｒｅｆとをティーチングペンダント４００に入力する（Ｓ１）。力目標値Ｆ_ｒｅｆは、力教示データ５０１に格納され、位置目標値Ｐ_ｒｅｆは、位置教示データ５０２に格納される。位置目標値Ｐ_ｒｅｆは、力制御動作を開始する位置である。

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

各切替制御部５１１〜５１６は、各モータ制御部５３１〜５３６にトルク指令値（力）τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}をトルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}として出力する（Ｓ３）。

各モータ制御部５３１〜５３６は、各電動モータ２１１〜２１６の角度θ_１〜θ_６に基づいて、各トルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}を実現するよう通電制御する（Ｓ４）。

各電動モータ２１１〜２１６は、通電されることにより各関節トルクτ_１〜τ_６を発生する（Ｓ５）。

各角度センサ５５１〜５５６は、各関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６（各電動モータ２１１〜２１６の角度θ_１〜θ_６）を検知する。各トルクセンサ５４１〜５４６は、各関節Ｊ_１〜Ｊ_６のトルクτ_１〜τ_６を検知する（Ｓ６）。各関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６と各関節Ｊ_１〜Ｊ_６のトルクτ_１〜τ_６は、ロボット制御装置３００のＣＰＵ３０１にフィードバックされる。

力検出部５０４（ＣＰＵ３０１）は、ロボットモデル５０３と各関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６に基づいて各関節Ｊ_１〜Ｊ_６のトルクτ_１〜τ_６を、現在の位置Ｐ（ｔ）にいてロボット２００の手先にかかる手先力Ｆに変換する（Ｓ７）。なお、関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６の代わりに電動モータ２１１〜２１６の角度θ_１〜θ_６を用いてもよい。

ＣＰＵ３０１は、駆動が終了したか否かを判定し（Ｓ８）、終了していない場合は（Ｓ８：Ｎｏ）、ステップＳ２〜Ｓ７を繰り返す。上記フローに従って各電動モータ２１１〜２１６を駆動することで、ロボット２００の手先にかかる力Ｆを所望の力目標値Ｆ_ｒｅｆに制御することが可能である。なお、図４に示すフローチャートの順番に限定するものではなく、他の順番でも力制御は可能である。

次に、ロボット２００の位置制御について説明する。図５は、第１実施形態においてロボット２００を位置制御する際のフローチャートである。

まず、操作者が位置目標値Ｐ_ｒｅｆをティーチングペンダント４００に入力する（Ｓ１１）。位置目標値Ｐ_ｒｅｆは、位置教示データ５０２に格納される。

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

各位置制御部５２１〜５２６は、各関節Ｊ_１〜Ｊ_６の角度ｑ_１〜ｑ_６が各関節Ｊ_１〜Ｊ_６の角度指令値ｑ_ｒｅｆ１〜ｑ_ｒｅｆ６に倣うよう各電動モータ２１１〜２１６のトルク指令値（角度）τ_{ＭＰｒｅｆ１}〜τ_{ＭＰｒｅｆ６}を算出する（Ｓ１３）。各関節Ｊ_１〜Ｊ_６の角度を示す信号としては、角度ｑ_１〜ｑ_６の代わりに電動モータ２１１〜２１６の角度θ_１〜θ_６を用いてもよい。

各切替制御部５１１〜５１６は、各モータ制御部５３１〜５３６にトルク指令値（角度）τ_{ＭＰｒｅｆ１}〜τ_{ＭＰｒｅｆ６}をトルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}として出力する（Ｓ１４）。

なお、ステップＳ１５，Ｓ１６，Ｓ１７，Ｓ１８は、ステップＳ４，Ｓ５，Ｓ６，Ｓ８と同じであり、説明を省略する。上記フローに従って各電動モータ２１１〜２１６を駆動することで、ロボット２００の手先位置Ｐを所望の位置目標値Ｐ_ｒｅｆに倣うよう制御することが可能である。

ステップＳ１〜Ｓ８の力制御とステップＳ１１〜Ｓ１８の位置制御とは、切替制御部５１１〜５１６にて切り替えられ、作業に応じていずれか一方の制御が選択される。

図６は、第１実施形態におけるロボットにより組付け作業を行っている状態を示す模式図である。第１実施形態では、トルクベース力制御により、ロボットハンド２５２を下方向に動かし、ロボットハンド２５２が支持（把持）した第１部品である円柱状ワークＷ１を第２部品である円環状ワークＷ２に組付（嵌合）する組付作業（嵌合作業）を行う。

図７は、図３の力制御部５０５の詳細を示すブロック図である。図８は、第１実施形態に係る組立部品の製造方法を示すフローチャートである。力制御部５０５は、停止命令確認部６０１、減速パラメータ設定部６０２及び力制御計算部６０３からなる。

まず、力制御部５０５は、組付作業を開始する（Ｓ２１）。この組付作業時に、停止命令確認部６０１は、ロボット２００の動作を停止させる停止命令を受けたか否か、つまり停止ボタン４０１が押されたか否かを判断する（Ｓ２２：判断処理、判断工程）。停止命令は、ＣＰＵ３０１がデバイスに何らかの異常があり停止する必要があると判断した場合、図１のティーチングペンダント４００を介して停止命令が出された場合等がある。また、停止命令は、ロボット２００の動作を規定するロボットプログラムに予め書き込んでおいてもよく、ロボットプログラムに基づいてロボット２００の動作を停止させる場合であってもよい。

力制御計算部６０３は、下記の変数をもとに式（１）、式（２）により計算を行う。
・与えられた位置目標値：Ｐ_ｒｅｆ
・与えられた力目標値：Ｆ_ｒｅｆ
・与えられた剛性係数：Ｋ_ｒｅｆ
・与えられた粘性係数：Ｄ_ｒｅｆ
・ロボットモデルに含まれる慣性行列：Λ_ｒｅｆ
・現在位置：Ｐ（ｔ）
・力：Ｆ
・ロボットモデル及び関節速度ｑ（・）から計算されるヤコビアン：Ｊ（ｑ）
・逆動力学計算により算出される慣性行列：Ｍ（ｑ）
・逆動力学計算により算出されるコリオリ遠心行列：ｃ（ｑ，ｑ（・））
・逆動力学計算により算出される重力ベクトル：ｇ（ｑ）
・Ｊ（ｑ）、Ｍ（ｑ）及び現在位置Ｐ（ｔ）から算出される行列：Λ（Ｐ（ｔ））

なお、式（１）は力制御の一例であり、これに限定するものではない。停止命令確認部６０１が停止命令を受けていないと判断した場合（Ｓ２２：Ｎｏ）、力制御計算部６０３は、式（１）から、図３の切替制御部５１１〜５１６に出力するトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を計算する。そして、力制御計算部６０３は、組付作業が完了したか否かを判断する（Ｓ２３）。組付作業が完了していれば（Ｓ２３：Ｙｅｓ）、そのまま処理を終了し、組付作業が完了していなければ（Ｓ２３：Ｎｏ）、ステップＳ２２の処理に戻る。

停止命令確認部６０１がロボット２００の動作を停止させる停止命令を受けたと判断した場合（Ｓ２２：Ｙｅｓ）、減速パラメータ設定部６０２は、ロボット２００の動作を停止させる際に力目標値Ｆ_ｒｅｆを減衰させる（Ｓ２４：停止処理、停止工程）。具体的には、減速パラメータ設定部６０２は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から、力目標値Ｆ_ｒｅｆを予め設定した停止目標時間内に０に減衰させる。つまり、力目標値Ｆ_ｒｅｆは、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}以後、Ｆ_ｒｅｆ＝Ｆ（ｔ）のように時間に依存した項となる。そして、力制御計算部６０３は、式（１）から、図３の切替制御部５１１〜５１６に出力するトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を計算する。

減衰方法は目的に応じて何通りか考えられる。図９（ａ）〜図９（ｃ）は、時間に対する力目標値を示すグラフである。具体的には、図９（ａ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して線形で減衰させた力目標値Ｆ_ｒｅｆを示すグラフである。図９（ｂ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対してステップ状に減衰させた力目標値Ｆ_ｒｅｆを示すグラフである。図９（ｃ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して非線形で減衰させた力目標値Ｆ_ｒｅｆを示すグラフである。力目標値Ｆ_ｒｅｆは、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、図９（ａ）〜図９（ｃ）において、簡単化のためにひとつの方向のみを図示している。

図９（ａ）〜図９（ｃ）に示す力目標値Ｆ_ｒｅｆは、時間に対して単調減少させている。図９（ａ）に示す力目標値Ｆ_ｒｅｆは、線形に減少させている。図９（ｂ）示す力目標値Ｆ_ｒｅｆは、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}で、「０」に瞬時に切り替えたものであり、図９（ａ）より減速を早く行いたい場合に用いる。例えば、停止命令時の速度が遅い場合やワークＷ１がワークＷ２に接触していない状態で可能な限り早く止めたい場合等に使用することができる。図９（ｃ）示す力目標値Ｆ_ｒｅｆは、非線形で減衰させたものであり、図９（ａ）よりも減速開始時及び停止完了時の動きを滑らかにする場合に用いることができる。例えば、よりワークＷ１に負荷をかけることなく停止させたい場合に用いることができる。なお、図示した力目標値の減衰方法は、一例であり、これに限定するものではない。また、力目標値Ｆ_ｒｅｆを０に減衰させる場合について説明したが、これに限定するものではなく、例えば０近傍の値に減衰させてもよい。

第１実施形態によれば、ＣＰＵ３０１は、トルクベース力制御中に停止命令を受けた場合、ロボット２００やワークＷ１，Ｗ２にかかる負荷を減らしながらロボット２００の動作を停止させることができる。

更に、ステップＳ２４において、第１実施形態では、減速パラメータ設定部６０２は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}以降、力制御の制御目標である位置目標値Ｐ_ｒｅｆを現在位置の値Ｐ（ｔ_{ｔｒｉｇｅｒ}）に固定する。つまり、減速パラメータ設定部６０２は、各トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を求める際の位置目標値Ｐ_ｒｅｆを固定値Ｐ（ｔ_{ｔｒｉｇｅｒ}）とする（Ｐ_ｒｅｆ＝Ｐ（ｔ_{ｔｒｉｇｅｒ}））。

図１０は、時間に対する手先の位置及び位置目標値のグラフである。位置目標値Ｐ_ｒｅｆは、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、図１０においては簡単化のためにひとつの方向のみを図示している。図１０に示すように、停止命令を受けた瞬間、位置目標値Ｐ_ｒｅｆと実際の位置Ｐとの間の位置偏差が存在するが、停止命令を受けた直後に、位置目標値Ｐ_ｒｅｆを固定値Ｐ（ｔ_{ｔｒｉｇｅｒ}）とすることで、位置偏差は極めて小さくなる。力制御計算部６０３は、この変更したパラメータを用いて、トルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}を計算する。

第１実施形態によれば、力目標値Ｆ_ｒｅｆを減衰させ、更に位置目標値Ｐ_ｒｅｆを固定値としたことにより、ロボット２００やワークにかかる負荷を低減させながらロボット２００の手先の位置を固定した状態でロボット２００の動作を停止させることができる。

次に、図８において、ＣＰＵ３０１は、ロボット２００がステップＳ２４にて停止したか否かを判断する（Ｓ２５：停止判断処理、停止判断工程）。ロボット２００の動作が停止したか否かは、現在位置Ｐ（ｔ）から算出される現在速度Ｐ（ｔ）（・）が予め設定した閾値以内に入ったか否かで判断する。つまり、手先の速度Ｐ（・）が閾値以内に収束した場合、ロボット２００が停止したことになる。

ＣＰＵ３０１は、ステップＳ２５にてロボット２００の動作が停止したと判断した場合（Ｓ２５：Ｙｅｓ）、各ブレーキ２３１〜２３６を作動させ、ロボット２００の各関節Ｊ_１〜Ｊ_６を各ブレーキにより固定させる（Ｓ２６：固定処理、固定工程）。この場合、モータ制御部５３１〜５３６は、モータ２１１〜２１６への給電を停止する。これにより、ロボット２００の動作を停止させた後に外力がロボット２００やワークＷ１に作用しても、ロボット２００が動くことはない。

図１１（ａ）は、トルクベース力制御を維持したまま停止処理を行ったときのロボットハンド２５２のＸ方向（ロボットハンドのベースに対する水平方向）の変位を示すグラフである。図１１（ａ）は、第１実施形態の実験結果に相当する。力制御を維持したままロボット２００を減速停止させるため、手先の位置偏差が小さい。特に、位置目標値Ｐ_ｒｅｆを、停止命令を受けたときの現在位置の値（図１０参照）に固定したので、手先が停止命令を受けたときから変動するのが効果的に抑制される

図１１（ｂ）は、位置制御に切り替えて停止処理を行ったときのロボットハンド２５２のＸ方向の変位を示すグラフである。図１１（ｂ）は、比較例の実験結果に相当する。ロボット２００の機差から生じるロボットハンド２５２の位置ずれが大きく、かつロボット２００の手先の速度が速いため、位置目標値Ｐ_ｒｅｆを、停止命令を受けたときの値（図１０参照）で固定すると、手先が停止命令を受けた時から行き過ぎてしまう。これにより、停止命令後に位置偏差が大きくなっている。

第１実施形態によれば、力制御の途中に停止命令を受けた場合、力目標値を減衰させ、なおかつ位置目標値を停止命令を受けたときの現在位置に固定することによって、ロボット２００が接触中のワークＷ２に倣って停止することになる。したがって、力制御を維持したまま安定してロボット２００の動作を停止させることができる。

また、ロボット２００の手先の位置偏差が小さく、柔らかいワークの場合はワークの破壊が防止され、硬いワークの場合はロボット２００に過負荷がかかりロボット２００が破損するのを防止することができる。また、ワーク同士が引っ掛かる（かじる）といったことがないため、停止後の復帰も容易である。

［第２実施形態］
次に、第２実施形態に係るロボット装置について説明する。第２実施形態では、ロボットの停止処理において、第１実施形態で説明した位置目標値Ｐ_ｒｅｆを固定する代わりに、速度目標値Ｐ_ｒｅｆ（・）を減衰させる場合について説明する。第２実施形態におけるロボット装置の構成は、第１実施形態と同様である。また、第２実施形態におけるロボット制御方法（組立部品の製造方法）は、ＣＰＵ３０１の一部の処理、具体的には図７の減速パラメータ設定部６０２の処理が、第１実施形態と異なるため、減速パラメータ設定部６０２の処理のみについて言及する。

第２実施形態において減速パラメータ設定部６０２は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}以降、力目標値Ｆ_ｒｅｆ及び速度目標値Ｐ_ｒｅｆ（・）を減衰させる。つまり、速度目標値Ｐ_ｒｅｆ（・）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}以降、Ｐ_ｒｅｆ（・）＝Ｐ（ｔ）（・）のように時間に依存した項となる。

第２実施形態では、第１実施形態と比較し、ＣＰＵ３０１が停止命令を受けた時の実際の手先の速度が速い場合に有効である。力目標値Ｆ_ｒｅｆの減衰方法は第１実施形態と同様である。

減衰方法は目的に応じて何通りか考えられる。図１２（ａ）〜図１２（ｄ）は、時間に対する速度目標値Ｐ_ｒｅｆ（・）を示すグラフである。具体的には、図１２（ａ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して線形で減衰させた速度目標値Ｐ_ｒｅｆ（・）を示すグラフである。図１２（ｂ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対してステップ状に減衰させた速度目標値Ｐ_ｒｅｆ（・）を示すグラフである。図１２（ｃ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して非線形で減衰させた速度目標値Ｐ_ｒｅｆ（・）を示すグラフである。図１２（ｄ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}からオーバーシュートしながら時間に対して減衰させた速度目標値Ｐ_ｒｅｆ（・）を示すグラフである。速度目標値Ｐ_ｒｅｆ（・）は、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、図１２（ａ）〜図１２（ｄ）において、簡単化のためにひとつの方向のみを図示している。

図１２（ａ）〜図１２（ｃ）に示す速度目標値Ｐ_ｒｅｆ（・）は、時間に対して単調減少させている。図１２（ａ）に示す速度目標値Ｐ_ｒｅｆ（・）は、線形に減少させている。図１２（ｂ）に示す速度目標値Ｐ_ｒｅｆ（・）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}で「０」に瞬時に切り替えたものであり、図１２（ａ）よりも早く速度を低下させたい場合に用いる。例えば、停止命令時の速度が遅い場合やワークＷ１がワークＷ２に接触していない状態で可能な限り早く止めたい場合等に使用することができる。

図１２（ｃ）に示す速度目標値Ｐ_ｒｅｆ（・）は、非線形に減衰させたものであり、図１２（ａ）よりも減速開始及び減速完了時間の動きを滑らかにしたい場合に用いることができる。例えば、より負荷をかけることなく停止させたい場合に用いることができる。図１２（ｄ）に示す速度目標値Ｐ_ｒｅｆ（・）は、位置のオーバーシュート分を加味して、「０」の反対側に振った後に「０」に収束させたものであり、より速く減速停止が可能となる。なお、図示した速度目標値の減衰方法は、一例であり、これに限定するものではない。また、速度目標値Ｐ_ｒｅｆ（・）を０に減衰させる場合について説明したが、これに限定するものではなく、例えば０近傍の値に減衰させてもよい。

以上、第２実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合、力目標値を減衰させ、なおかつ速度目標値を減衰させることによって、力制御を維持したままロボット２００の動作が停止する。そのため、ワークＷ２に接触中のワークＷ１がワークＷ２に倣ってロボット２００の動作を停止させることが可能である。よって、ロボット２００やワークＷ１，Ｗ２に過負荷を与えることなくロボット２００の動作を停止させることができる。また、ワーク同士がかじるといったことがないため、停止後の復帰も容易である。

［変形例１］
第２実施形態では、ロボットの停止処理において、第１実施形態で説明した位置目標値Ｐ_ｒｅｆを固定する代わりに、速度目標値Ｐ_ｒｅｆ（・）を減衰させる場合について説明したが、これらを組み合わせてもよい。即ち、減速パラメータ設定部６０２は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}以降、力目標値Ｆ_ｒｅｆを減衰させつつ、位置目標値Ｐ_ｒｅｆを固定値Ｐ（ｔ_{ｔｒｉｇｅｒ}）に固定し、速度目標値Ｐ_ｒｅｆ（・）を減衰させる。なお、位置目標値の固定方法及び力目標値の減衰方法は第１実施形態と同様であり、速度目標値の減衰方法は、第２実施形態と同様である。

力目標値、位置目標値及び速度目標値の３つのパラメータを同時に変更するため、第１実施形態及び第２実施形態と比較し、より短い時間でロボット２００を減速停止させることが可能となる。

［第３実施形態］
次に、第３実施形態に係るロボット装置について説明する。第３実施形態では、第１実施形態にて説明した図７の減速パラメータ設定部６０２が、力目標値Ｆ_ｒｅｆの減衰、位置目標値Ｐ_ｒｅｆの固定に加え、図３の力教示データ５０１から与えられた剛性係数Ｋ_ｒｅｆの減衰を行う。つまり、ロボット２００の剛性係数Ｋ_ｒｅｆは、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}以降、Ｋ_ｒｅｆ＝Ｋ_ｒｅｆ（ｔ）のように時間に依存した項となる。

組み立てるワークや組み立て方により、剛性係数Ｋ_ｒｅｆを高めに設定する場合がある。

第３実施形態では、減速パラメータ設定部６０２は、停止処理（停止工程）において、位置目標値Ｐ_ｒｅｆを固定値とした際に、ロボット２００の剛性係数Ｋ_ｒｅｆを減衰させる。剛性係数Ｋ_ｒｅｆの減衰を行うことにより、第１実施形態に比べて柔らかくワークＷ１がワークＷ２に倣うことになるため、安定してロボット２００の動作を停止させることができる。

減衰方法は目的に応じて何通りか考えられる。図１３（ａ）〜図１３（ｂ）は、時間に対する剛性係数Ｋ_ｒｅｆを示すグラフである。具体的には、図１３（ａ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して線形で減衰させた剛性係数Ｋ_ｒｅｆを示すグラフである。図１３（ｂ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して非線形で減衰させた剛性係数Ｋ_ｒｅｆを示すグラフである。剛性係数Ｋ_ｒｅｆはマトリックスであるが、説明の簡単化のためマトリックスの一要素のみを表記している。

図１３（ａ）及び図１３（ｂ）に示す剛性係数Ｋ_ｒｅｆは、時間に対して単調減少させている。図１３（ａ）に示す剛性係数Ｋ_ｒｅｆは、線形に減衰させている。図１３（ｂ）に示す剛性係数Ｋ_ｒｅｆは、非線形に減衰させており、線形に減衰させた場合よりも減速開始時間及び減速完了時間の動きを滑らかにしたい場合に用いる。例えば、より負荷をかけることなくロボット２００の動作を停止させたい場合に用いる。

なお、図示した減衰方法は、一例であり、これに限定するものではない。また、剛性係数のマトリックス全体を減衰よいし、ロボットの動作によっては所望の方向に関連するマトリックスの要素のみを減衰させてもよい。

以上、第３実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合、力目標値、位置目標値及び剛性係数を変更することによって、力制御を維持したままロボット２００の動作が停止する。そのため、ワークＷ１に接触中のワークＷ２に倣ってロボット２００の動作を停止させることが可能である。また、剛性係数を減衰しない場合に比べて、より柔軟にワークに倣うことが可能となる。このため、ロボットやワークに負荷を与えることが少なく、ロボットの動作を停止できる。また、ワーク同士がかじるといったことがないため、停止後の復帰も容易である。

［第４実施形態］
次に、第４実施形態に係るロボット装置について説明する。第４実施形態では、第２実施形態にて説明した図７の減速パラメータ設定部６０２が、力目標値Ｆ_ｒｅｆ及び速度目標値Ｐ_ｒｅｆ（・）の減衰に加え、図３の力教示データ５０１から与えられた粘性係数Ｄ_ｒｅｆの減衰を行う。つまり、ロボット２００の粘性係数Ｄ_ｒｅｆは、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}以降、Ｄ_ｒｅｆ＝Ｄ_ｒｅｆ（ｔ）のように時間に依存した項となる。

組み立てるワークや組み立て方により、粘性係数Ｄ_ｒｅｆを高めに設定する場合がある。

第４実施形態では、減速パラメータ設定部６０２は、停止処理（停止工程）において、速度目標値Ｐ_ｒｅｆ（・）を減衰させる際に、ロボット２００の粘性係数Ｄ_ｒｅｆを減衰させる。粘性係数Ｄ_ｒｅｆの減衰を行うことにより、第２実施形態に比べて柔らかくワークに倣うことになるため、安定してロボット２００の動作を停止させることができる。

減衰方法は目的に応じて何通りか考えられる。図１４（ａ）〜図１４（ｂ）は、時間に対する粘性係数Ｄ_ｒｅｆを示すグラフである。具体的には、図１４（ａ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して線形で減衰させた粘性係数Ｄ_ｒｅｆを示すグラフである。図１４（ｂ）は、停止命令を受けた時刻ｔ_{ｔｒｉｇｅｒ}から時間に対して非線形で減衰させた粘性係数Ｄ_ｒｅｆを示すグラフである。粘性係数Ｄ_ｒｅｆはマトリックスであるが、説明の簡単化のためマトリックスの一要素のみを表記している。

図１４（ａ）及び図１４（ｂ）に示す粘性係数Ｄ_ｒｅｆは、時間に対して単調減少させている。図１４（ａ）に示す粘性係数Ｄ_ｒｅｆは、線形に減衰させている。図１４（ｂ）に示す粘性係数Ｄ_ｒｅｆは、非線形に減衰させており、線形に減衰させた場合よりも減速開始時間及び減速完了時間の動きを滑らかにしたい場合に用いる。例えば、より負荷をかけることなくロボット２００の動作を停止させたい場合に用いる。

なお、図示した減衰方法は、一例であり、これに限定するものではない。また、粘性係数のマトリックス全体を減衰よいし、ロボットの動作によっては所望の方向に関連するマトリックスの要素のみを減衰させてもよい。

以上、第４実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合、力目標値、速度目標値及び粘性係数を変更することによって、力制御を維持したままロボット２００の動作が停止する。そのため、ワークＷ１に接触中のワークＷ２に倣ってロボット２００の動作を停止させることが可能である。また、粘性係数を減衰しない場合に比べて、より柔軟にワークに倣うことが可能となる。このため、ロボットやワークに負荷を与えることが少なく、ロボットの動作を停止できる。また、ワーク同士がかじるといったことがないため、停止後の復帰も容易である。

［変形例２］
変形例２は、変形例１に対し、図７の減速パラメータ設定部６０２が、力目標値の減衰、位置目標値の固定及び速度目標値の減衰に加え、図３の力教示データ５０１から与えられる剛性係数及び粘性係数の減衰を行う。剛性係数の減衰方法は第３実施形態と同様であり、粘性係数の減衰方法は第４実施形態と同様である。

剛性係数及び粘性係数の初期値が高めに与えられていた場合に、剛性係数及び粘性係数の減衰を行うことにより、ロボット２００は、変形例１に比べて柔らかくワークに倣うことになるため、安定してロボットの動作を停止させることができる。

［第５実施形態］
次に、第５実施形態に係るロボット装置について説明する。図１５は、第５実施形態に係るロボット装置の制御系を示す制御ブロック図である。第５実施形態では、第１実施形態の図７記載の停止命令確認部６０１と減速パラメータ設定部６０２との間に、ワーク接触検知部６０４が設けられている。

ワーク接触検知部６０４は、手先に作用する力に応じた信号を生成するセンサを用いて、手先に作用する力を求める。手先に作用する力に応じた信号を生成するセンサは、第１実施形態で説明したように、各関節のトルクに対応する信号をそれぞれ生成する複数のトルクセンサ５４１〜５４６（図３）である。つまり、力検出部５０４は、各トルクセンサ５４１〜５４６から取得したトルクτ_１〜τ_６及び各角度センサ５５１〜５５６から取得した角度ｑ_１〜ｑ_６を用いて、ロボット２００の手先の現在位置Ｐ（ｔ）とそのときのロボット２００の手先力Ｆを算出する。なお、力検出部５０４は、手先に取り付けられた力覚センサ（不図示）を用いて、ロボット２００の手先力Ｆを算出してもよい。

ワーク接触検知部６０４は、算出した手先力Ｆを、予め記憶部に記憶させておいた所定の閾値と比較する。手先力Ｆが所定の閾値を超えているときは、減速パラメータ設定部６０２に進み、第１実施形態〜第４実施形態に示す通り、力制御のままでロボット２００の動作を停止させる。つまり、複数のトルクセンサ５４１〜５４６のそれぞれの値と力目標値との力偏差が小さくなるように各モータ２１１〜２１６に対するそれぞれのトルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}を求める。ここで、トルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}は、第１実施形態で説明したトルク指令値τ_{ＭＦｒｅｆ１}〜τ_{ＭＦｒｅｆ６}である。以上、第５実施形態では、第１実施形態〜第４実施形態で説明した停止処理を、手先に作用する力Ｆが予め設定した所定の閾値を超える場合に実行する。なお、第５実施形態では、手先力Ｆが所定の閾値となっているときも、減速パラメータ設定部６０２に進み、第１実施形態〜第４実施形態に示す通り、力制御のままでロボット２００の動作を停止させる。

ワーク接触検知部６０４は、算出した手先力Ｆが所定の閾値未満のとき、即ち接触が検知されないときは、切替制御部５１１〜５１６において力制御から位置制御に切り替えを行ってロボット２００の動作の停止を行う。つまり、複数の角度センサ５５１〜５５６のそれぞれの値がそれぞれ所定の値となるように各モータ２１１〜２１６に対するそれぞれのトルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}を求める。ここで、トルク指令値τ_{Ｍｒｅｆ１}〜τ_{Ｍｒｅｆ６}は、第１実施形態で説明したトルク指令値τ_{ＭＰｒｅｆ１}〜τ_{ＭＰｒｅｆ６}である。以上、手先に作用する力Ｆが所定の閾値未満の場合には、複数の角度センサ５５１〜５５６のそれぞれの値を予め設定した所定の値に近づける位置制御で、ロボット２００の動作を停止させる。なお、力Ｆが所定の閾値のときは、力制御でロボット２００の動作を停止させるようにしたが、位置制御でロボット２００の動作を停止させるようにしてもよい。

以上、ワークへの接触前は、力制御のまま停止を行おうとすると、倣うべきワークが存在しないため、ワーク等に接触するまで停止できないことがある。ワークに接触していない場合に、位置制御に切り替えて停止させることで、速くロボット２００を停止させることができる。

図１６は、ワーク接触時の時間に対する手先力Ｆの変化を示すグラフである。手先力Ｆは、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、簡単化のためにひとつの方向のみを図示している。

ワーク接触検知は、手先力Ｆをあらかじめワークに対して設定された閾値により判定することにより行う。なお、図１６に図示した閾値による接触検知は一例であり、これに限定するものではない。例えば、手先力の変化率で閾値判定してもよい。

以上、第５実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合かつワークに接触していないと判定される場合において、位置制御に切り替えて停止を行うため、力制御で停止させる場合に比べロボットを速く停止することができる。

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

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

また、上述の実施形態では、ロボット２００（ロボットアーム２５１）が垂直多関節の場合について説明したが、これに限定するものではない。ロボットが、例えば、水平多関節のロボット、パラレルリンクのロボット、直交ロボット等、種々のロボットであってもよい。つまり、関節の駆動方向は、回転方向の駆動だけに限らず、直動方向の駆動（伸縮駆動）も含まれる。さらに、関節を駆動するのが電動モータである場合について説明したが、これに限定するものではなく、例えば人工筋肉等であってもよい。また、上述の実施形態では、ロボットが６つの関節を有する場合について説明したが、関節の数はこれに限定されるものではない。

また、上述の実施形態では、力検知部が、複数のトルクセンサで構成される場合について説明したが、これに限定するものではなく、ロボットアームの先端に設けられた力覚センサであってもよい。

１００…ロボット装置、２００…ロボット、２１１〜２１６…電動モータ（モータ）、２５１…ロボットアーム、２５２…ロボットハンド、３００…ロボット制御装置（制御部）

Claims (17)

1. 各関節を駆動する複数のモータと、手先に作用する力に応じた信号を生成するセンサとを有するロボットと、
   前記手先に作用する力に応じた信号を生成するセンサから信号を取得して、前記手先に作用する力と力目標値との力偏差が小さくなるように、前記各モータに対するトルク指令値を求め、
   前記各モータに対するトルク指令値に基づき前記各モータの駆動を制御し、
   停止命令を受けたとき、前記力目標値を減衰させて、前記各モータに対するトルク指令値を求め、前記各モータの駆動を制御することで、前記ロボットの動作を停止させる停止処理を実行する制御部と、を備えたことを特徴とするロボット装置。
2. 前記ロボットは、前記手先の位置に応じた信号を生成するセンサを有し、
   前記制御部は、前記停止処理において、前記手先の位置に応じた信号を生成するセンサから信号を取得して、前記手先の位置と位置目標値との位置偏差が小さくなるように、前記各トルク指令値を求めることを特徴とする請求項１に記載のロボット装置。
3. 前記制御部は、前記停止処理において、前記位置目標値を固定値とすることを特徴とする請求項２に記載のロボット装置。
4. 前記制御部は、前記停止処理において、前記各モータに対するトルク指令値を求めるのに用いる前記ロボットの剛性係数を減衰させることを特徴とする請求項３に記載のロボット装置。
5. 前記手先の位置に応じた信号を生成するセンサは、前記各モータ又は前記各関節の位置に応じた信号を生成する複数のエンコーダであることを特徴とする請求項２乃至４のいずれか１項に記載のロボット装置。
6. 前記制御部は、前記手先の位置に応じた信号を生成するセンサから信号を取得して前記手先の速度を求め、前記停止処理において、前記手先の速度と速度目標値との速度偏差が小さくなるように前記各モータに対するトルク指令値を求めることを特徴とする請求項２乃至５のいずれか１項に記載のロボット装置。
7. 前記ロボットは、前記手先の速度に応じた信号を生成するセンサを有し、
   前記制御部は、前記手先の速度に応じた信号を生成するセンサから信号を取得し、前記停止処理において、前記手先の速度と速度目標値との速度偏差が小さくなるように、前記各モータに対するトルク指令値を求めることを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置。
8. 前記制御部は、前記停止処理において、前記速度目標値を減衰させることを特徴とする請求項６又は７に記載のロボット装置。
9. 前記制御部は、前記停止処理において、前記各モータに対するトルク指令値を求めるのに用いる前記ロボットの粘性係数を減衰させることを特徴とする請求項８に記載のロボット装置。
10. 前記ロボットは、前記各関節をそれぞれ制動するブレーキを有し、
    前記制御部は、前記ロボットが前記停止処理にて停止した後、前記各ブレーキにより前記ロボットの各関節を固定させることを特徴とする請求項１乃至９のいずれか１項に記載のロボット装置。
11. 前記手先に作用する力に応じた信号を生成するセンサは、前記各関節のトルクに応じた信号をそれぞれ生成するセンサであることを特徴とする請求項１乃至１０のいずれか１項に記載のロボット装置。
12. 前記制御部に前記停止命令を送信する停止操作部を更に備えたことを特徴とする請求項１乃至１１のいずれか１項に記載のロボット装置。
13. 前記各モータ又は前記各関節の位置に応じた信号を生成する複数のエンコーダを更に備え、
    前記制御部は、前記停止命令を受けたとき、前記停止処理を、前記手先に作用する力が予め設定した所定の閾値を超える場合に実行し、前記手先に作用する力が前記所定の閾値未満である場合には、前記複数のエンコーダのそれぞれの値を予め設定した所定の値に近づける制御で、前記ロボットの動作を停止させることを特徴とする請求項１に記載のロボット装置。
14. 各関節を駆動する複数のモータと、手先に作用する力に応じた信号を生成するセンサとを有するロボットの制御方法であって、
    制御部が、
    前記手先に作用する力に応じた信号を生成するセンサから信号を取得して、前記手先の力と力目標値との力偏差が小さくなるように、前記各モータに対するトルク指令値を求め、
    前記各モータに対するトルク指令値に基づき前記各モータの駆動を制御し、
    停止命令を受けたとき、前記力目標値を減衰させて、前記各モータに対するトルク指令値を求め、前記各モータの駆動を制御することで、前記ロボットの動作を停止させることを特徴とするロボット制御方法。
15. コンピュータに請求項１４に記載のロボット制御方法を実行させるためのプログラム。
16. 請求項１５に記載のプログラムが記録された、コンピュータが読み取り可能な記録媒体。
17. ロボットが、複数の関節を駆動する複数のモータと、手先に作用する力に応じた信号を生成するセンサとを有しており、制御部が、前記手先に作用する力に応じた信号を生成するセンサから信号を取得して、前記手先に作用する力と力目標値との力偏差が小さくなるように、前記各モータに対するトルク指令値を求め、前記各モータに対するトルク指令値に基づき前記複数のモータの駆動を制御することで前記ロボットを動作させて、前記ロボットにより組立部品を製造する組立部品の製造方法であって、
    前記制御部が、
    前記ロボットに第１部品を把持させて前記第１部品を第２部品に組付ける組付作業時に、停止命令を受けたとき、前記力目標値を減衰させて、前記各モータに対するトルク指令値を求め、前記各モータの駆動を制御することで、前記ロボットの動作を停止させることを特徴とする組立部品の製造方法。

Images