(57)【特許請求の範囲】
【請求項１】
モータと、所定の部位に作用する力を検出するセンサと、
前記所定の部位に作用する力が、力目標値となるように、前記モータに対して指令値を出力し、
前記指令値に基づき、前記モータの駆動を制御し、
停止指令を受けた際、前記所定の部位が、前記停止指令を受けた際の位置にとどまり、かつ前記力目標値を減少させる停止処理を実行する制御部と、を備えたことを特徴とするロボット装置。
【請求項２】
前記制御部は、
前記停止指令を受けた際、前記力目標値が減少するように設定し、かつ前記停止指令を受けた際の前記所定の部位の位置を位置指令値に設定し、
前記センサによって検出される力が減少するように設定した当該力目標値となり、かつ前記所定の部位の位置が設定した当該位置目標値となる前記指令値を出力することを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記制御部は、前記停止処理において、前記所定の部位の位置を検出するセンサに基づき、前記所定の部位の位置と前記位置目標値との偏差が小さくなるような前記指令値を出力することを特徴とする請求項２に記載のロボット装置。
【請求項４】
前記所定の部位の位置を検出するセンサはエンコーダであることを特徴とする請求項３に記載のロボット装置。
【請求項５】
前記制御部は、前記停止処理において、前記指令値を求めるのに用いる剛性係数を減少させることを特徴とする請求項１乃至４のいずれか１項に記載のロボット装置。
【請求項６】
前記制御部は、前記所定の部位の位置を検出するセンサから信号を取得して前記所定の部位の速度を求め、前記停止処理において、前記所定の部位の速度が減少するような前記指令値を出力することを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置。
【請求項７】
前記所定の部位の速度を検出するセンサをさらに有し、
前記制御部は、前記所定の部位の速度を検出するセンサから信号を取得し、前記停止処理において、前記所定の部位の速度が減少するような前記指令値を出力することを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置。
【請求項８】
前記制御部は、前記停止処理において、速度目標値を減少させることを特徴とする請求項６又は７に記載のロボット装置。
【請求項９】
前記制御部は、前記停止処理において、前記指令値を求めるのに用いる粘性係数を減少させることを特徴とする請求項８に記載のロボット装置。
【請求項１０】
前記モータを制動するブレーキを有し、
前記制御部は、前記停止処理にて停止した後、前記ブレーキにより前記ロボット装置の関節を固定させることを特徴とする請求項１乃至９のいずれか１項に記載のロボット装置。
【請求項１１】
前記所定の部位に作用する力を検出するセンサは、関節のトルクを検出するセンサであることを特徴とする請求項１乃至９のいずれか１項に記載のロボット装置。
【請求項１２】
前記制御部に前記停止指令を送信する入力装置を更に備えたことを特徴とする請求項１乃至１１のいずれか１項に記載のロボット装置。
【請求項１３】
前記制御部は、前記停止指令を受けたとき、前記停止処理を、前記所定の部位に作用する力が予め設定した所定の閾値を超える場合に実行し、前記所定の部位に作用する力が前記所定の閾値未満である場合には、前記所定の部位の位置を予め設定した所定の値に近づけることで停止させることを特徴とする請求項１に記載のロボット装置。
【請求項１４】
モータと、所定の部位に作用する力を検出するセンサと、制御部と、を有するロボット装置の制御方法であって、
前記制御部が、
前記所定の部位に作用する力が、力目標値となるように、前記モータに対して指令値を出力し、
前記指令値に基づき、前記モータの駆動を制御し、
停止指令を受けた際、前記所定の部位が、前記停止指令を受けた際の位置にとどまり、かつ前記力目標値を減少させる停止処理を実行する、ことを特徴とする制御方法。
【請求項１５】
コンピュータに請求項１４に記載の制御方法を実行させるためのプログラム。
【請求項１６】
請求項１５に記載のプログラムが記録された、コンピュータが読み取り可能な記録媒体。
【請求項１７】
請求項１乃至１３のいずれか１項に記載のロボット装置を用いて物品の製造を行うことを特徴とする物品の製造方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、力制御の途中で停止指令を受けた場合にロボットの動作を停止させるロボット装置、制御方法、プログラム、記録媒体及び物品の製造方法に関する。
【背景技術】
【０００２】
ロボットは、主として位置制御と力制御のいずれかで制御が行われている。位置制御では、ワークとの接触を伴うような作業を行う際に、ワークの位置誤差などがあるとワークやロボットに負荷がかかり作業が困難になる。
【０００３】
このため、力制御として、位置ベース力制御（アドミッタンス制御）が行われる。位置ベース力制御では、ロボットの先端部に設けられた力センサの値に基づきワークに倣うような位置指令値を計算する。位置ベース力制御の途中で停止命令をうけた場合、位置制御に切り替えてロボットの減速停止を行っていた（特許文献１参照）。
【０００４】
しかし、位置ベース力制御は、位置制御をマイナーループとして利用するため、その応答性は、位置制御の応答性よりも低下する。一般的に位置制御の応答周波数は、ロボットの固有振動数に制約される。特に、接触時は、安定性の問題から応答性が高めることが難しい。
【０００５】
このため、応答性の高い力制御を行うロボットの開発が行われている。具体的には、トルク指令値で力制御を行うことで、ロボットの固有振動数に制約を受けずに応答性を高めることができるトルクベース力制御が開発されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第５３７１８８２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、ロボットには部品製作誤差に起因する機差があり、位置決め誤差が生じる。機差を正確に把握することは困難であり、ロボットを位置制御で動作させる場合、目標位置と少しずれた位置に移動することになる。
【０００８】
このため、応答性の高いトルクベース力制御において、上記特許文献１のように力制御で倣っている状態から位置制御に切り替えて停止させた際に、ロボットの機差の影響により位置制御における目標位置からずれた位置でロボットが停止する。このため、ロボットとワーク又はロボットが把持しているワークと別のワークとが接触している場合は、ロボット及びワークの双方に過負荷がかかるという問題があった。
【０００９】
更に、トルクベース力制御は応答性が高い。速度が速いと停止命令を受けた際にロボットが減速停止するまでの時間がかかる。このため、ロボットが停止目標位置で停止できずに行き過ぎてしまう可能性がある。ロボットが停止目標位置に対して行き過ぎた場合は、同様にロボットやワークに過負荷がかかるという問題があった。
【００１０】
例えば、トルクベース力制御を用いてペグインホールといった嵌合動作を行う場合、位置制御で減速停止させると、機差から生じる位置ずれや応答性が高いために生じる行き過ぎがある。このため、ワークに無理な力がかかり、ワークが嵌合穴に引っ掛かることがあった。嵌合穴に引っ掛かったワークを外すことは困難なため、復帰に時間がかかるという問題があった。嵌合動作以外でも、ワークをある一定の力で押しつけるような押しつけ動作の途中に、位置制御で減速停止させると、過剰な力で押しつけることになり、前述のようにロボットやワークに過負荷がかかるという問題があった。
【００１１】
そこで、本発明は、力制御の途中で停止命令を受けた場合、ロボットやワークにかかる負荷を減らしてロボットの動作を停止させることを目的とする。
【課題を解決するための手段】
【００１２】
本発明のロボット装置は、モータと、所定の部位に作用する力を検出するセンサと、前記所定の部位に作用する力が、力目標値となるように、前記モータに対して指令値を出力し、前記指令値に基づき、前記モータの駆動を制御し、停止指令を受けた際、前記所定の部位が、前記停止指令を受けた際の位置にとどまり、かつ前記力目標値を減少させる停止処理を実行する制御部と、を備えたことを特徴とする。
【発明の効果】
【００１３】
本発明によれば、トルクベース力制御を行っている途中で停止命令を受けた際に、ロボットやワークにかかる負荷を減らしながらロボットの動作を停止させることが可能となる。
【図面の簡単な説明】
【００１４】
【図１】第１実施形態に係るロボット装置の概略構成を示す斜視図である。
【図２】第１実施形態に係るロボット装置のロボット制御装置を示すブロック図である。
【図３】第１実施形態に係るロボット装置の制御系を示す制御ブロック図である。
【図４】第１実施形態においてロボットを力制御する際のフローチャートである。
【図５】第１実施形態においてロボットを位置制御する際のフローチャートである。
【図６】第１実施形態におけるロボットにより組付け作業を行っている状態を示す模式図である。
【図７】図３の力制御部の詳細を示すブロック図である。
【図８】第１実施形態に係る組立部品の製造方法を示すフローチャートである。
【図９】（ａ）〜（ｃ）は、時間に対する力目標値を示すグラフである。
【図１０】時間に対する手先の位置及び位置目標値のグラフである。
【図１１】（ａ）は、トルクベース力制御を維持したまま停止処理を行ったときのロボットハンドの変位を示すグラフである。（ｂ）は、位置制御に切り替えて停止処理を行ったときのロボットハンドの変位を示すグラフである。
【図１２】（ａ）〜（ｄ）は、時間に対する速度目標値Ｐｒｅｆ（・）を示すグラフである。
【図１３】（ａ）〜（ｂ）は、時間に対する剛性係数Ｋｒｅｆを示すグラフである。
【図１４】（ａ）〜（ｂ）は、時間に対する粘性係数Ｄｒｅｆを示すグラフである。
【図１５】第５実施形態に係るロボット装置の制御系を示す制御ブロック図である。
【図１６】ワーク接触時の時間に対する手先力Ｆの変化を示すグラフである。
【発明を実施するための形態】
【００１５】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１６】
［第１実施形態］
図１は、第１実施形態に係るロボット装置の概略構成を示す斜視図である。図１に示すように、ロボット装置１００は、多関節のロボット２００と、ロボット２００の動作を制御する制御部としてのロボット制御装置３００と、を備えている。また、ロボット装置１００は、ロボット制御装置３００に教示データを送信する教示装置としてのティーチングペンダント４００を備えている。ティーチングペンダント４００は、操作者が操作するものであり、ロボット２００やロボット制御装置３００の動作を指定するのに用いる。
【００１７】
ロボット２００は、垂直多関節のロボットである。具体的に説明すると、ロボット２００は、垂直多関節のロボットアーム２５１と、ロボットアーム２５１の先端に取り付けられたエンドエフェクタとしてのロボットハンド２５２と、を備えている。以下、エンドエフェクタがロボットハンド２５２である場合について説明するが、これに限定するものではなく、ツール等であってもよい。ロボットアーム２５１の基端は、台座Ｂに固定されている。ロボットハンド２５２は、物体（部品やツール等）を把持（支持）するものである。
【００１８】
ロボット２００、即ちロボットアーム２５１は、複数の関節、例えば６つ関節（６軸）Ｊ１〜Ｊ６を有している。ロボットアーム２５１は、各関節Ｊ１〜Ｊ６を各関節軸Ａ１〜Ａ６まわりにそれぞれ回転駆動する複数（６つ）のサーボモータ２０１〜２０６を有している。
【００１９】
ロボットアーム２５１は、複数のリンク（フレーム）２１００〜２１０６が各関節Ｊ１〜Ｊ６で回転可能に連結されている。ここで、基端側から先端側に向かって、リンク２１００〜２１０６が順に直列に連結されている。ロボットアーム２５１は、可動範囲の中であれば、任意の３次元位置で任意の３方向の姿勢に、ロボット２００の手先（ロボットアーム２５１の先端）を向けることができる。
【００２０】
ロボットアーム２５１の位置及び姿勢は、座標系で表現することができる。座標系Ｔｏは、ロボットアーム２５１の基端、即ち台座Ｂに固定した座標系を表し、座標系Ｔｅはロボット２００の手先（ロボットアーム２５１の先端）に固定した座標系を表す。
【００２１】
ここで、ロボット２００の手先とは、第１実施形態では、ロボットハンド２５２が物体を把持（支持）していない場合には、ロボットハンド２５２のことである。ロボットハンド２５２が物体を把持（支持）している場合は、ロボットハンド２５２と把持（支持）している物体（例えば部品やツール等）とを含めてロボット２００の手先という。つまり、ロボットハンド２５２が物体を把持している状態であるか物体を把持していない状態であるかにかかわらず、ロボットアーム２５１の先端から先を手先という。
【００２２】
各サーボモータ２０１〜２０６は、各関節Ｊ１〜Ｊ６をそれぞれ駆動する電動モータ（モータ）２１１〜２１６と、各電動モータ２１１〜２１６にそれぞれ接続されたセンサ部２２１〜２２６とを有している。各センサ部２２１〜２２６は、各関節Ｊ１〜Ｊ６の位置（角度）を検知する、即ち位置（角度）に応じた信号を生成する位置センサ（角度センサ）を有する。また、各センサ部２２１〜２２６は、各関節Ｊ１〜Ｊ６のトルクを検知する、即ちトルクに応じた信号を生成するトルクセンサを有する。また、各サーボモータ２０１〜２０６は、不図示の減速機を有し、直接、又は不図示のベルトやベアリング等の伝達部材を介して各関節Ｊ１〜Ｊ６で駆動されるフレームに接続されている。
【００２３】
ロボットアーム２５１の内部には、各サーボモータ２０１〜２０６の電動モータ２１１〜２１６の駆動を制御する駆動制御部としてのサーボ制御部２３０が配置されている。
【００２４】
サーボ制御部２３０は、入力した各関節Ｊ１〜Ｊ６に対応する各トルク指令値に基づき、各関節Ｊ１〜Ｊ６のトルクがトルク指令値に追従するよう、各電動モータ２１１〜２１６に電流を出力し、各電動モータ２１１〜２１６の駆動を制御する。なお、第１実施形態では、サーボ制御部２３０が１つの制御装置で構成されているものとして説明しているが、各電動モータ２１１〜２１６にそれぞれ対応した複数の制御装置の集合体で構成されていてもよい。また、第１実施形態では、サーボ制御部２３０は、ロボットアーム２５１の内部に配置されているが、ロボット制御装置３００の筐体内部に配置されていてもよい。
【００２５】
また、ロボット２００のロボットアーム２５１は、各関節Ｊ１〜Ｊ６をそれぞれ制動する複数のブレーキ（例えばディスクブレーキ）２３１〜２３６を有する。各ブレーキ２３１〜２３６を作動させることにより、各関節Ｊ１〜Ｊ６が動かないように各関節Ｊ１〜Ｊ６を固定することができる。
【００２６】
また、第１実施形態では、ティーチングペンダント４００は、操作者の操作によりロボット制御装置３００に停止命令を送信する停止操作部としての停止ボタン４０１を有する。
【００２７】
次に、ロボット制御装置３００について説明する。図２は、第１実施形態に係るロボット装置のロボット制御装置を示すブロック図である。ロボット制御装置３００は、コンピュータで構成されており、処理部としてのＣＰＵ（Central  Processing  Unit）３０１を備えている。また、ロボット制御装置３００は、記憶部として、ＲＯＭ（Read  Only  Memory）３０２、ＲＡＭ（Random  Access  Memory）３０３、ＨＤＤ（Hard  Disk  Drive）３０４を備えている。また、ロボット制御装置３００は、記録ディスクドライブ３０５、各種のインタフェース３０６〜３０９を備えている。
【００２８】
ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５、各種のインタフェース３０６〜３０９が、バス３１０を介して接続されている。ＲＯＭ３０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。
【００２９】
ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ３０１に、後述する演算処理を実行させるためのプログラム３３０を記録するものである。ＣＰＵ３０１は、ＨＤＤ３０４に記録（格納）されたプログラム３３０に基づいてロボット制御方法の各工程を実行する。
【００３０】
記録ディスクドライブ３０５は、記録ディスク３３１に記録された各種データやプログラム等を読み出すことができる。
【００３１】
ティーチングペンダント４００はインタフェース３０６に接続されている。ＣＰＵ３０１はインタフェース３０６及びバス３１０を介してティーチングペンダント４００からの教示データの入力を受ける。
【００３２】
サーボ制御部２３０は、インタフェース３０９に接続されている。ＣＰＵ３０１は、サーボ制御部２３０、インタフェース３０９及びバス３１０を介して各センサ部２２１〜２２６から信号を取得する。また、ＣＰＵ３０１は、各関節のトルク指令値のデータを所定時間間隔でバス３１０及びインタフェース３０９を介してサーボ制御部２３０に出力する。
【００３３】
インタフェース３０７には、モニタ３２１が接続されており、モニタ３２１には、ＣＰＵ３０１の制御の下、各種画像が表示される。インタフェース３０８は、書き換え可能な不揮発性メモリや外付けＨＤＤ等の記憶部である外部記憶装置３２２が接続可能に構成されている。
【００３４】
なお、第１実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ３０４であり、ＨＤＤ３０４にプログラム３３０が格納される場合について説明するが、これに限定するものではない。プログラム３３０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム３３０を供給するための記録媒体としては、図２に示すＲＯＭ３０２，記録ディスク３３１、外部記憶装置３２２等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＤＶＤ−ＲＯＭ、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性メモリ、ＨＤＤ、ＲＯＭ等を用いることができる。
【００３５】
図３は、第１実施形態に係るロボット装置の制御系を示す制御ブロック図である。ロボット制御装置３００のＣＰＵ３０１は、プログラム３３０を実行することにより、力検出部５０４、力制御部５０５及び位置目標値生成部５０６として機能する。サーボ制御部２３０は、複数（６関節であるので６つ）の切替制御部５１１〜５１６と、複数（６関節であるので６つ）の位置制御部５２１〜５２６と、複数（６関節であるので６つ）のモータ制御部５３１〜５３６として機能する。
【００３６】
各センサ部２２１〜２２６は、各角度センサ（位置センサ）５５１〜５５６と、各トルクセンサ５４１〜５４６とを有する。各角度センサ５５１〜５５６は、各電動モータ２１１〜２１６又は各関節Ｊ１〜Ｊ６の角度（位置）をそれぞれ検知する、即ち角度（位置）に応じた信号を生成するロータリエンコーダ（エンコーダ）である。
【００３７】
第１実施形態では、各角度センサ５５１〜５５６は、各電動モータ２１１〜２１６の角度θ１〜θ６を直接検知する。なお、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６は、不図示の減速機の減速比等に基づき、各角度θ１〜θ６から求めることができる。したがって、各角度センサ５５１〜５５６は、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６を間接的に検知していることになる。
【００３８】
各トルクセンサ５４１〜５４６は、各関節Ｊ１〜Ｊ６のトルクτ１〜τ６をそれぞれ検知する、即ちトルクに応じた信号を生成するセンサである。複数の角度センサ５５１〜５５６でロボット２００の手先の位置（姿勢も含む）Ｐを検知する、即ち位置Ｐに応じた信号を生成するセンサである位置検知部５５０が構成されている。また、複数のトルクセンサ５４１〜５４６でロボット２００の手先に作用する力（手先力）Ｆを検知する、即ち力Ｆに応じた信号を生成するセンサである力検知部５４０が構成されている。
【００３９】
ティーチングペンダント４００は、操作者の操作により、力教示データ５０１に含まれる力目標値Ｆｒｅｆと、位置教示データ５０２に含まれる位置目標値ＰｒｅｆとをＣＰＵ３０１に出力する。力目標値Ｆｒｅｆは、ロボット２００の手先力の目標値であり、操作者がティーチングペンダント４００を用いて設定する。位置目標値Ｐｒｅｆは、ロボット２００の手先位置の目標値であり、操作者がティーチングペンダント４００を用いて設定する。外部記憶装置３２２にはロボットモデル５０３が記憶されている。
【００４０】
また、ティーチングペンダント４００は、停止ボタン４０１が操作者に操作されたとき、停止命令をＣＰＵ３０１の力制御部５０５に出力する。
【００４１】
力検出部５０４は、各トルクセンサ５４１〜５４６から取得したトルクτ１〜τ６及び各角度センサ５５１〜５５６から取得した角度ｑ１〜ｑ６に応じた信号を用いて、ロボット２００の手先の現在位置Ｐ（ｔ）とそのときのロボット２００の手先力Ｆを求める。このとき力検出部５０４は、ロボット２００の手先の現在位置Ｐ（ｔ）を、各角度センサ５５１〜５５６から取得した角度ｑ１〜ｑ６に応じた信号を用いて求める。なお、力検出部５０４は、手先に取り付けられた力覚センサ（不図示）を用いて、ロボット２００の手先力Ｆを算出してもよい。
【００４２】
力制御部５０５は、ロボットモデル５０３（仮想質量Ｍｒｅｆ）、力目標値Ｆｒｅｆ、位置目標値Ｐｒｅｆ、剛性係数Ｋｒｅｆ、粘性係数Ｄｒｅｆ、現在位置Ｐ（ｔ）及び力Ｆの値を示す信号の入力を受ける。そして、力制御部５０５は、これら値を用いて、各関節Ｊ１〜Ｊ６に対するトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を求める。このとき、手先力Ｆと力目標値Ｆｒｅｆとの力偏差、手先の位置Ｐ（ｔ）と位置目標値Ｐｒｅｆとの位置偏差、及び手先の速度Ｐ（ｔ）（・）と速度目標値Ｐｒｅｆ（・）との速度偏差が小さくなるようにトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を求める。力制御部５０５は、求めた各トルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を各切替制御部５１１〜５１６に出力する。ここで、速度Ｐ（ｔ）（・）は、位置Ｐ（ｔ）を時間で微分することにより求まり、速度目標値Ｐｒｅｆ（・）は、位置目標値Ｐｒｅｆを時間で微分することにより求まる。したがって、第１実施形態では、力制御部５０５が位置検知部５５０の検知結果である手先の位置Ｐ（ｔ）から手先の速度Ｐ（ｔ）（・）を求めることになる。なお、位置検知部５５０が演算を行って手先の速度Ｐ（ｔ）（・）のデータそのものを出力するようにしてもよく、この場合は、位置検知部５５０が手先の速度を検知する速度検知部、つまり手先の速度に対応する信号を生成するセンサも兼ねることになる。なお、（・）は、時間で１階微分することを意味する。
【００４３】
位置目標値生成部５０６は、手先の位置目標値Ｐｒｅｆから逆運動学計算により各関節Ｊ１〜Ｊ６の角度指令値（位置指令値）ｑｒｅｆ１〜ｑｒｅｆ６を求め、各角度指令値ｑｒｅｆ１〜ｑｒｅｆ６を各位置制御部５２１〜５２６に出力する。
【００４４】
各位置制御部５２１〜５２６は、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６と各関節Ｊ１〜Ｊ６の角度指令値ｑｒｅｆ１〜ｑｒｅｆ６との角度偏差が小さくなるようトルク指令値τＭＰｒｅｆ１〜τＭＰｒｅｆ６を求める。なお、この角度偏差を小さくすることは、各電動モータ２１１〜２１６の角度と角度指令値ｑｒｅｆ１〜ｑｒｅｆ６を減速機の減速比等で換算した角度指令値との角度偏差を小さくすることと等価である。各位置制御部５２１〜５２６は、各トルク指令値τＭＰｒｅｆ１〜τＭＰｒｅｆ６を各切替制御部５１１〜５１６に出力する。
【００４５】
各切替制御部５１１〜５１６は、トルクベース力制御を行う力制御モードと、位置制御を行う位置制御モードとを切り替え制御する。各切替制御部５１１〜５１６は、力制御モード時は、各トルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として各モータ制御部５３１〜５３６に出力する。また、各切替制御部５１１〜５１６は、位置制御モード時は、各トルク指令値τＭＰｒｅｆ１〜τＭＰｒｅｆ６を各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として各モータ制御部５３１〜５３６に出力する。
【００４６】
各モータ制御部５３１〜５３６は、各電動モータ２１１〜２１６の角度（位置）θ１〜θ６に基づき、各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６を実現するように各電流Ｃｕｒ１〜Ｃｕｒ６を各電動モータ２１１〜２１６に通電する。
【００４７】
次に、ロボット２００の力制御について説明する。図４は、第１実施形態においてロボット２００を力制御する際のフローチャートである。
【００４８】
まず、操作者が力目標値Ｆｒｅｆと位置目標値Ｐｒｅｆとをティーチングペンダント４００に入力する（Ｓ１）。力目標値Ｆｒｅｆは、力教示データ５０１に格納され、位置目標値Ｐｒｅｆは、位置教示データ５０２に格納される。位置目標値Ｐｒｅｆは、力制御動作を開始する位置である。
【００４９】
力制御部５０５は、手先力Ｆが力目標値Ｆｒｅｆに倣うよう、即ち手先力Ｆと力目標値Ｆｒｅｆとの偏差が小さくなるようロボットモデル５０３を用いて各モータ２１１〜２１６に対するトルク指令値（力）τＭＦｒｅｆ１〜τＭＦｒｅｆ６を算出する（Ｓ２）。
【００５０】
各切替制御部５１１〜５１６は、各モータ制御部５３１〜５３６にトルク指令値（力）τＭＦｒｅｆ１〜τＭＦｒｅｆ６をトルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として出力する（Ｓ３）。
【００５１】
各モータ制御部５３１〜５３６は、各電動モータ２１１〜２１６の角度θ１〜θ６に基づいて、各トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６を実現するよう通電制御する（Ｓ４）。
【００５２】
各電動モータ２１１〜２１６は、通電されることにより各関節トルクτ１〜τ６を発生する（Ｓ５）。
【００５３】
各角度センサ５５１〜５５６は、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６（各電動モータ２１１〜２１６の角度θ１〜θ６）を検知する。各トルクセンサ５４１〜５４６は、各関節Ｊ１〜Ｊ６のトルクτ１〜τ６を検知する（Ｓ６）。各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６と各関節Ｊ１〜Ｊ６のトルクτ１〜τ６は、ロボット制御装置３００のＣＰＵ３０１にフィードバックされる。
【００５４】
力検出部５０４（ＣＰＵ３０１）は、ロボットモデル５０３と各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６に基づいて各関節Ｊ１〜Ｊ６のトルクτ１〜τ６を、現在の位置Ｐ（ｔ）にいてロボット２００の手先にかかる手先力Ｆに変換する（Ｓ７）。なお、関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６の代わりに電動モータ２１１〜２１６の角度θ１〜θ６を用いてもよい。
【００５５】
ＣＰＵ３０１は、駆動が終了したか否かを判定し（Ｓ８）、終了していない場合は（Ｓ８：Ｎｏ）、ステップＳ２〜Ｓ７を繰り返す。上記フローに従って各電動モータ２１１〜２１６を駆動することで、ロボット２００の手先にかかる力Ｆを所望の力目標値Ｆｒｅｆに制御することが可能である。なお、図４に示すフローチャートの順番に限定するものではなく、他の順番でも力制御は可能である。
【００５６】
次に、ロボット２００の位置制御について説明する。図５は、第１実施形態においてロボット２００を位置制御する際のフローチャートである。
【００５７】
まず、操作者が位置目標値Ｐｒｅｆをティーチングペンダント４００に入力する（Ｓ１１）。位置目標値Ｐｒｅｆは、位置教示データ５０２に格納される。
【００５８】
位置目標値生成部５０６は、ロボットモデル５０３に基づき、位置目標値Ｐｒｅｆを各関節Ｊ１〜Ｊ６の角度指令値ｑｒｅｆ１〜ｑｒｅｆ６に変換する（Ｓ１２）。
【００５９】
各位置制御部５２１〜５２６は、各関節Ｊ１〜Ｊ６の角度ｑ１〜ｑ６が各関節Ｊ１〜Ｊ６の角度指令値ｑｒｅｆ１〜ｑｒｅｆ６に倣うよう各電動モータ２１１〜２１６のトルク指令値（角度）τＭＰｒｅｆ１〜τＭＰｒｅｆ６を算出する（Ｓ１３）。各関節Ｊ１〜Ｊ６の角度を示す信号としては、角度ｑ１〜ｑ６の代わりに電動モータ２１１〜２１６の角度θ１〜θ６を用いてもよい。
【００６０】
各切替制御部５１１〜５１６は、各モータ制御部５３１〜５３６にトルク指令値（角度）τＭＰｒｅｆ１〜τＭＰｒｅｆ６をトルク指令値τＭｒｅｆ１〜τＭｒｅｆ６として出力する（Ｓ１４）。
【００６１】
なお、ステップＳ１５，Ｓ１６，Ｓ１７，Ｓ１８は、ステップＳ４，Ｓ５，Ｓ６，Ｓ８と同じであり、説明を省略する。上記フローに従って各電動モータ２１１〜２１６を駆動することで、ロボット２００の手先位置Ｐを所望の位置目標値Ｐｒｅｆに倣うよう制御することが可能である。
【００６２】
ステップＳ１〜Ｓ８の力制御とステップＳ１１〜Ｓ１８の位置制御とは、切替制御部５１１〜５１６にて切り替えられ、作業に応じていずれか一方の制御が選択される。
【００６３】
図６は、第１実施形態におけるロボットにより組付け作業を行っている状態を示す模式図である。第１実施形態では、トルクベース力制御により、ロボットハンド２５２を下方向に動かし、ロボットハンド２５２が支持（把持）した第１部品である円柱状ワークＷ１を第２部品である円環状ワークＷ２に組付（嵌合）する組付作業（嵌合作業）を行う。
【００６４】
図７は、図３の力制御部５０５の詳細を示すブロック図である。図８は、第１実施形態に係る組立部品の製造方法を示すフローチャートである。力制御部５０５は、停止命令確認部６０１、減速パラメータ設定部６０２及び力制御計算部６０３からなる。
【００６５】
まず、力制御部５０５は、組付作業を開始する（Ｓ２１）。この組付作業時に、停止命令確認部６０１は、ロボット２００の動作を停止させる停止命令を受けたか否か、つまり停止ボタン４０１が押されたか否かを判断する（Ｓ２２：判断処理、判断工程）。停止命令は、ＣＰＵ３０１がデバイスに何らかの異常があり停止する必要があると判断した場合、図１のティーチングペンダント４００を介して停止命令が出された場合等がある。また、停止命令は、ロボット２００の動作を規定するロボットプログラムに予め書き込んでおいてもよく、ロボットプログラムに基づいてロボット２００の動作を停止させる場合であってもよい。
【００６６】
力制御計算部６０３は、下記の変数をもとに式（１）、式（２）により計算を行う。
・与えられた位置目標値：Ｐｒｅｆ
・与えられた力目標値：Ｆｒｅｆ
・与えられた剛性係数：Ｋｒｅｆ
・与えられた粘性係数：Ｄｒｅｆ
・ロボットモデルに含まれる慣性行列：Λｒｅｆ
・現在位置：Ｐ（ｔ）
・力：Ｆ
・ロボットモデル及び関節速度ｑ（・）から計算されるヤコビアン：Ｊ（ｑ）
・逆動力学計算により算出される慣性行列：Ｍ（ｑ）
・逆動力学計算により算出されるコリオリ遠心行列：ｃ（ｑ，ｑ（・））
・逆動力学計算により算出される重力ベクトル：ｇ（ｑ）
・Ｊ（ｑ）、Ｍ（ｑ）及び現在位置Ｐ（ｔ）から算出される行列：Λ（Ｐ（ｔ））
【００６７】
【数１】
0006794194.tif 000002
【００６８】
【数２】
0006794194.tif 000003
【００６９】
なお、式（１）は力制御の一例であり、これに限定するものではない。停止命令確認部６０１が停止命令を受けていないと判断した場合（Ｓ２２：Ｎｏ）、力制御計算部６０３は、式（１）から、図３の切替制御部５１１〜５１６に出力するトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を計算する。そして、力制御計算部６０３は、組付作業が完了したか否かを判断する（Ｓ２３）。組付作業が完了していれば（Ｓ２３：Ｙｅｓ）、そのまま処理を終了し、組付作業が完了していなければ（Ｓ２３：Ｎｏ）、ステップＳ２２の処理に戻る。
【００７０】
停止命令確認部６０１がロボット２００の動作を停止させる停止命令を受けたと判断した場合（Ｓ２２：Ｙｅｓ）、減速パラメータ設定部６０２は、ロボット２００の動作を停止させる際に力目標値Ｆｒｅｆを減衰させる（Ｓ２４：停止処理、停止工程）。具体的には、減速パラメータ設定部６０２は、停止命令を受けた時刻ｔｔｒｉｇｅｒから、力目標値Ｆｒｅｆを予め設定した停止目標時間内に０に減衰させる。つまり、力目標値Ｆｒｅｆは、停止命令を受けた時刻ｔｔｒｉｇｅｒ以後、Ｆｒｅｆ＝Ｆ（ｔ）のように時間に依存した項となる。そして、力制御計算部６０３は、式（１）から、図３の切替制御部５１１〜５１６に出力するトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を計算する。
【００７１】
減衰方法は目的に応じて何通りか考えられる。図９（ａ）〜図９（ｃ）は、時間に対する力目標値を示すグラフである。具体的には、図９（ａ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して線形で減衰させた力目標値Ｆｒｅｆを示すグラフである。図９（ｂ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対してステップ状に減衰させた力目標値Ｆｒｅｆを示すグラフである。図９（ｃ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して非線形で減衰させた力目標値Ｆｒｅｆを示すグラフである。力目標値Ｆｒｅｆは、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、図９（ａ）〜図９（ｃ）において、簡単化のためにひとつの方向のみを図示している。
【００７２】
図９（ａ）〜図９（ｃ）に示す力目標値Ｆｒｅｆは、時間に対して単調減少させている。図９（ａ）に示す力目標値Ｆｒｅｆは、線形に減少させている。図９（ｂ）示す力目標値Ｆｒｅｆは、停止命令を受けた時刻ｔｔｒｉｇｅｒで、「０」に瞬時に切り替えたものであり、図９（ａ）より減速を早く行いたい場合に用いる。例えば、停止命令時の速度が遅い場合やワークＷ１がワークＷ２に接触していない状態で可能な限り早く止めたい場合等に使用することができる。図９（ｃ）示す力目標値Ｆｒｅｆは、非線形で減衰させたものであり、図９（ａ）よりも減速開始時及び停止完了時の動きを滑らかにする場合に用いることができる。例えば、よりワークＷ１に負荷をかけることなく停止させたい場合に用いることができる。なお、図示した力目標値の減衰方法は、一例であり、これに限定するものではない。また、力目標値Ｆｒｅｆを０に減衰させる場合について説明したが、これに限定するものではなく、例えば０近傍の値に減衰させてもよい。
【００７３】
第１実施形態によれば、ＣＰＵ３０１は、トルクベース力制御中に停止命令を受けた場合、ロボット２００やワークＷ１，Ｗ２にかかる負荷を減らしながらロボット２００の動作を停止させることができる。
【００７４】
更に、ステップＳ２４において、第１実施形態では、減速パラメータ設定部６０２は、停止命令を受けた時刻ｔｔｒｉｇｅｒ以降、力制御の制御目標である位置目標値Ｐｒｅｆを現在位置の値Ｐ（ｔｔｒｉｇｅｒ）に固定する。つまり、減速パラメータ設定部６０２は、各トルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を求める際の位置目標値Ｐｒｅｆを固定値Ｐ（ｔｔｒｉｇｅｒ）とする（Ｐｒｅｆ＝Ｐ（ｔｔｒｉｇｅｒ））。
【００７５】
図１０は、時間に対する手先の位置及び位置目標値のグラフである。位置目標値Ｐｒｅｆは、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、図１０においては簡単化のためにひとつの方向のみを図示している。図１０に示すように、停止命令を受けた瞬間、位置目標値Ｐｒｅｆと実際の位置Ｐとの間の位置偏差が存在するが、停止命令を受けた直後に、位置目標値Ｐｒｅｆを固定値Ｐ（ｔｔｒｉｇｅｒ）とすることで、位置偏差は極めて小さくなる。力制御計算部６０３は、この変更したパラメータを用いて、トルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６を計算する。
【００７６】
第１実施形態によれば、力目標値Ｆｒｅｆを減衰させ、更に位置目標値Ｐｒｅｆを固定値としたことにより、ロボット２００やワークにかかる負荷を低減させながらロボット２００の手先の位置を固定した状態でロボット２００の動作を停止させることができる。
【００７７】
次に、図８において、ＣＰＵ３０１は、ロボット２００がステップＳ２４にて停止したか否かを判断する（Ｓ２５：停止判断処理、停止判断工程）。ロボット２００の動作が停止したか否かは、現在位置Ｐ（ｔ）から算出される現在速度Ｐ（ｔ）（・）が予め設定した閾値以内に入ったか否かで判断する。つまり、手先の速度Ｐ（・）が閾値以内に収束した場合、ロボット２００が停止したことになる。
【００７８】
ＣＰＵ３０１は、ステップＳ２５にてロボット２００の動作が停止したと判断した場合（Ｓ２５：Ｙｅｓ）、各ブレーキ２３１〜２３６を作動させ、ロボット２００の各関節Ｊ１〜Ｊ６を各ブレーキにより固定させる（Ｓ２６：固定処理、固定工程）。この場合、モータ制御部５３１〜５３６は、モータ２１１〜２１６への給電を停止する。これにより、ロボット２００の動作を停止させた後に外力がロボット２００やワークＷ１に作用しても、ロボット２００が動くことはない。
【００７９】
図１１（ａ）は、トルクベース力制御を維持したまま停止処理を行ったときのロボットハンド２５２のＸ方向（ロボットハンドのベースに対する水平方向）の変位を示すグラフである。図１１（ａ）は、第１実施形態の実験結果に相当する。力制御を維持したままロボット２００を減速停止させるため、手先の位置偏差が小さい。特に、位置目標値Ｐｒｅｆを、停止命令を受けたときの現在位置の値（図１０参照）に固定したので、手先が停止命令を受けたときから変動するのが効果的に抑制される
【００８０】
図１１（ｂ）は、位置制御に切り替えて停止処理を行ったときのロボットハンド２５２のＸ方向の変位を示すグラフである。図１１（ｂ）は、比較例の実験結果に相当する。ロボット２００の機差から生じるロボットハンド２５２の位置ずれが大きく、かつロボット２００の手先の速度が速いため、位置目標値Ｐｒｅｆを、停止命令を受けたときの値（図１０参照）で固定すると、手先が停止命令を受けた時から行き過ぎてしまう。これにより、停止命令後に位置偏差が大きくなっている。
【００８１】
第１実施形態によれば、力制御の途中に停止命令を受けた場合、力目標値を減衰させ、なおかつ位置目標値を停止命令を受けたときの現在位置に固定することによって、ロボット２００が接触中のワークＷ２に倣って停止することになる。したがって、力制御を維持したまま安定してロボット２００の動作を停止させることができる。
【００８２】
また、ロボット２００の手先の位置偏差が小さく、柔らかいワークの場合はワークの破壊が防止され、硬いワークの場合はロボット２００に過負荷がかかりロボット２００が破損するのを防止することができる。また、ワーク同士が引っ掛かる（かじる）といったことがないため、停止後の復帰も容易である。
【００８３】
［第２実施形態］
次に、第２実施形態に係るロボット装置について説明する。第２実施形態では、ロボットの停止処理において、第１実施形態で説明した位置目標値Ｐｒｅｆを固定する代わりに、速度目標値Ｐｒｅｆ（・）を減衰させる場合について説明する。第２実施形態におけるロボット装置の構成は、第１実施形態と同様である。また、第２実施形態におけるロボット制御方法（組立部品の製造方法）は、ＣＰＵ３０１の一部の処理、具体的には図７の減速パラメータ設定部６０２の処理が、第１実施形態と異なるため、減速パラメータ設定部６０２の処理のみについて言及する。
【００８４】
第２実施形態において減速パラメータ設定部６０２は、停止命令を受けた時刻ｔｔｒｉｇｅｒ以降、力目標値Ｆｒｅｆ及び速度目標値Ｐｒｅｆ（・）を減衰させる。つまり、速度目標値Ｐｒｅｆ（・）は、停止命令を受けた時刻ｔｔｒｉｇｅｒ以降、Ｐｒｅｆ（・）＝Ｐ（ｔ）（・）のように時間に依存した項となる。
【００８５】
第２実施形態では、第１実施形態と比較し、ＣＰＵ３０１が停止命令を受けた時の実際の手先の速度が速い場合に有効である。力目標値Ｆｒｅｆの減衰方法は第１実施形態と同様である。
【００８６】
減衰方法は目的に応じて何通りか考えられる。図１２（ａ）〜図１２（ｄ）は、時間に対する速度目標値Ｐｒｅｆ（・）を示すグラフである。具体的には、図１２（ａ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して線形で減衰させた速度目標値Ｐｒｅｆ（・）を示すグラフである。図１２（ｂ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対してステップ状に減衰させた速度目標値Ｐｒｅｆ（・）を示すグラフである。図１２（ｃ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して非線形で減衰させた速度目標値Ｐｒｅｆ（・）を示すグラフである。図１２（ｄ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒからオーバーシュートしながら時間に対して減衰させた速度目標値Ｐｒｅｆ（・）を示すグラフである。速度目標値Ｐｒｅｆ（・）は、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、図１２（ａ）〜図１２（ｄ）において、簡単化のためにひとつの方向のみを図示している。
【００８７】
図１２（ａ）〜図１２（ｃ）に示す速度目標値Ｐｒｅｆ（・）は、時間に対して単調減少させている。図１２（ａ）に示す速度目標値Ｐｒｅｆ（・）は、線形に減少させている。図１２（ｂ）に示す速度目標値Ｐｒｅｆ（・）は、停止命令を受けた時刻ｔｔｒｉｇｅｒで「０」に瞬時に切り替えたものであり、図１２（ａ）よりも早く速度を低下させたい場合に用いる。例えば、停止命令時の速度が遅い場合やワークＷ１がワークＷ２に接触していない状態で可能な限り早く止めたい場合等に使用することができる。
【００８８】
図１２（ｃ）に示す速度目標値Ｐｒｅｆ（・）は、非線形に減衰させたものであり、図１２（ａ）よりも減速開始及び減速完了時間の動きを滑らかにしたい場合に用いることができる。例えば、より負荷をかけることなく停止させたい場合に用いることができる。図１２（ｄ）に示す速度目標値Ｐｒｅｆ（・）は、位置のオーバーシュート分を加味して、「０」の反対側に振った後に「０」に収束させたものであり、より速く減速停止が可能となる。なお、図示した速度目標値の減衰方法は、一例であり、これに限定するものではない。また、速度目標値Ｐｒｅｆ（・）を０に減衰させる場合について説明したが、これに限定するものではなく、例えば０近傍の値に減衰させてもよい。
【００８９】
以上、第２実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合、力目標値を減衰させ、なおかつ速度目標値を減衰させることによって、力制御を維持したままロボット２００の動作が停止する。そのため、ワークＷ２に接触中のワークＷ１がワークＷ２に倣ってロボット２００の動作を停止させることが可能である。よって、ロボット２００やワークＷ１，Ｗ２に過負荷を与えることなくロボット２００の動作を停止させることができる。また、ワーク同士がかじるといったことがないため、停止後の復帰も容易である。
【００９０】
［変形例１］
第２実施形態では、ロボットの停止処理において、第１実施形態で説明した位置目標値Ｐｒｅｆを固定する代わりに、速度目標値Ｐｒｅｆ（・）を減衰させる場合について説明したが、これらを組み合わせてもよい。即ち、減速パラメータ設定部６０２は、停止命令を受けた時刻ｔｔｒｉｇｅｒ以降、力目標値Ｆｒｅｆを減衰させつつ、位置目標値Ｐｒｅｆを固定値Ｐ（ｔｔｒｉｇｅｒ）に固定し、速度目標値Ｐｒｅｆ（・）を減衰させる。なお、位置目標値の固定方法及び力目標値の減衰方法は第１実施形態と同様であり、速度目標値の減衰方法は、第２実施形態と同様である。
【００９１】
力目標値、位置目標値及び速度目標値の３つのパラメータを同時に変更するため、第１実施形態及び第２実施形態と比較し、より短い時間でロボット２００を減速停止させることが可能となる。
【００９２】
［第３実施形態］
次に、第３実施形態に係るロボット装置について説明する。第３実施形態では、第１実施形態にて説明した図７の減速パラメータ設定部６０２が、力目標値Ｆｒｅｆの減衰、位置目標値Ｐｒｅｆの固定に加え、図３の力教示データ５０１から与えられた剛性係数Ｋｒｅｆの減衰を行う。つまり、ロボット２００の剛性係数Ｋｒｅｆは、停止命令を受けた時刻ｔｔｒｉｇｅｒ以降、Ｋｒｅｆ＝Ｋｒｅｆ（ｔ）のように時間に依存した項となる。
【００９３】
組み立てるワークや組み立て方により、剛性係数Ｋｒｅｆを高めに設定する場合がある。
【００９４】
第３実施形態では、減速パラメータ設定部６０２は、停止処理（停止工程）において、位置目標値Ｐｒｅｆを固定値とした際に、ロボット２００の剛性係数Ｋｒｅｆを減衰させる。剛性係数Ｋｒｅｆの減衰を行うことにより、第１実施形態に比べて柔らかくワークＷ１がワークＷ２に倣うことになるため、安定してロボット２００の動作を停止させることができる。
【００９５】
減衰方法は目的に応じて何通りか考えられる。図１３（ａ）〜図１３（ｂ）は、時間に対する剛性係数Ｋｒｅｆを示すグラフである。具体的には、図１３（ａ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して線形で減衰させた剛性係数Ｋｒｅｆを示すグラフである。図１３（ｂ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して非線形で減衰させた剛性係数Ｋｒｅｆを示すグラフである。剛性係数Ｋｒｅｆはマトリックスであるが、説明の簡単化のためマトリックスの一要素のみを表記している。
【００９６】
図１３（ａ）及び図１３（ｂ）に示す剛性係数Ｋｒｅｆは、時間に対して単調減少させている。図１３（ａ）に示す剛性係数Ｋｒｅｆは、線形に減衰させている。図１３（ｂ）に示す剛性係数Ｋｒｅｆは、非線形に減衰させており、線形に減衰させた場合よりも減速開始時間及び減速完了時間の動きを滑らかにしたい場合に用いる。例えば、より負荷をかけることなくロボット２００の動作を停止させたい場合に用いる。
【００９７】
なお、図示した減衰方法は、一例であり、これに限定するものではない。また、剛性係数のマトリックス全体を減衰よいし、ロボットの動作によっては所望の方向に関連するマトリックスの要素のみを減衰させてもよい。
【００９８】
以上、第３実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合、力目標値、位置目標値及び剛性係数を変更することによって、力制御を維持したままロボット２００の動作が停止する。そのため、ワークＷ１に接触中のワークＷ２に倣ってロボット２００の動作を停止させることが可能である。また、剛性係数を減衰しない場合に比べて、より柔軟にワークに倣うことが可能となる。このため、ロボットやワークに負荷を与えることが少なく、ロボットの動作を停止できる。また、ワーク同士がかじるといったことがないため、停止後の復帰も容易である。
【００９９】
［第４実施形態］
次に、第４実施形態に係るロボット装置について説明する。第４実施形態では、第２実施形態にて説明した図７の減速パラメータ設定部６０２が、力目標値Ｆｒｅｆ及び速度目標値Ｐｒｅｆ（・）の減衰に加え、図３の力教示データ５０１から与えられた粘性係数Ｄｒｅｆの減衰を行う。つまり、ロボット２００の粘性係数Ｄｒｅｆは、停止命令を受けた時刻ｔｔｒｉｇｅｒ以降、Ｄｒｅｆ＝Ｄｒｅｆ（ｔ）のように時間に依存した項となる。
【０１００】
組み立てるワークや組み立て方により、粘性係数Ｄｒｅｆを高めに設定する場合がある。
【０１０１】
第４実施形態では、減速パラメータ設定部６０２は、停止処理（停止工程）において、速度目標値Ｐｒｅｆ（・）を減衰させる際に、ロボット２００の粘性係数Ｄｒｅｆを減衰させる。粘性係数Ｄｒｅｆの減衰を行うことにより、第２実施形態に比べて柔らかくワークに倣うことになるため、安定してロボット２００の動作を停止させることができる。
【０１０２】
減衰方法は目的に応じて何通りか考えられる。図１４（ａ）〜図１４（ｂ）は、時間に対する粘性係数Ｄｒｅｆを示すグラフである。具体的には、図１４（ａ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して線形で減衰させた粘性係数Ｄｒｅｆを示すグラフである。図１４（ｂ）は、停止命令を受けた時刻ｔｔｒｉｇｅｒから時間に対して非線形で減衰させた粘性係数Ｄｒｅｆを示すグラフである。粘性係数Ｄｒｅｆはマトリックスであるが、説明の簡単化のためマトリックスの一要素のみを表記している。
【０１０３】
図１４（ａ）及び図１４（ｂ）に示す粘性係数Ｄｒｅｆは、時間に対して単調減少させている。図１４（ａ）に示す粘性係数Ｄｒｅｆは、線形に減衰させている。図１４（ｂ）に示す粘性係数Ｄｒｅｆは、非線形に減衰させており、線形に減衰させた場合よりも減速開始時間及び減速完了時間の動きを滑らかにしたい場合に用いる。例えば、より負荷をかけることなくロボット２００の動作を停止させたい場合に用いる。
【０１０４】
なお、図示した減衰方法は、一例であり、これに限定するものではない。また、粘性係数のマトリックス全体を減衰よいし、ロボットの動作によっては所望の方向に関連するマトリックスの要素のみを減衰させてもよい。
【０１０５】
以上、第４実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合、力目標値、速度目標値及び粘性係数を変更することによって、力制御を維持したままロボット２００の動作が停止する。そのため、ワークＷ１に接触中のワークＷ２に倣ってロボット２００の動作を停止させることが可能である。また、粘性係数を減衰しない場合に比べて、より柔軟にワークに倣うことが可能となる。このため、ロボットやワークに負荷を与えることが少なく、ロボットの動作を停止できる。また、ワーク同士がかじるといったことがないため、停止後の復帰も容易である。
【０１０６】
［変形例２］
変形例２は、変形例１に対し、図７の減速パラメータ設定部６０２が、力目標値の減衰、位置目標値の固定及び速度目標値の減衰に加え、図３の力教示データ５０１から与えられる剛性係数及び粘性係数の減衰を行う。剛性係数の減衰方法は第３実施形態と同様であり、粘性係数の減衰方法は第４実施形態と同様である。
【０１０７】
剛性係数及び粘性係数の初期値が高めに与えられていた場合に、剛性係数及び粘性係数の減衰を行うことにより、ロボット２００は、変形例１に比べて柔らかくワークに倣うことになるため、安定してロボットの動作を停止させることができる。
【０１０８】
［第５実施形態］
次に、第５実施形態に係るロボット装置について説明する。図１５は、第５実施形態に係るロボット装置の制御系を示す制御ブロック図である。第５実施形態では、第１実施形態の図７記載の停止命令確認部６０１と減速パラメータ設定部６０２との間に、ワーク接触検知部６０４が設けられている。
【０１０９】
ワーク接触検知部６０４は、手先に作用する力に応じた信号を生成するセンサを用いて、手先に作用する力を求める。手先に作用する力に応じた信号を生成するセンサは、第１実施形態で説明したように、各関節のトルクに対応する信号をそれぞれ生成する複数のトルクセンサ５４１〜５４６（図３）である。つまり、力検出部５０４は、各トルクセンサ５４１〜５４６から取得したトルクτ１〜τ６及び各角度センサ５５１〜５５６から取得した角度ｑ１〜ｑ６を用いて、ロボット２００の手先の現在位置Ｐ（ｔ）とそのときのロボット２００の手先力Ｆを算出する。なお、力検出部５０４は、手先に取り付けられた力覚センサ（不図示）を用いて、ロボット２００の手先力Ｆを算出してもよい。
【０１１０】
ワーク接触検知部６０４は、算出した手先力Ｆを、予め記憶部に記憶させておいた所定の閾値と比較する。手先力Ｆが所定の閾値を超えているときは、減速パラメータ設定部６０２に進み、第１実施形態〜第４実施形態に示す通り、力制御のままでロボット２００の動作を停止させる。つまり、複数のトルクセンサ５４１〜５４６のそれぞれの値と力目標値との力偏差が小さくなるように各モータ２１１〜２１６に対するそれぞれのトルク指令値τＭｒｅｆ１〜τＭｒｅｆ６を求める。ここで、トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６は、第１実施形態で説明したトルク指令値τＭＦｒｅｆ１〜τＭＦｒｅｆ６である。以上、第５実施形態では、第１実施形態〜第４実施形態で説明した停止処理を、手先に作用する力Ｆが予め設定した所定の閾値を超える場合に実行する。なお、第５実施形態では、手先力Ｆが所定の閾値となっているときも、減速パラメータ設定部６０２に進み、第１実施形態〜第４実施形態に示す通り、力制御のままでロボット２００の動作を停止させる。
【０１１１】
ワーク接触検知部６０４は、算出した手先力Ｆが所定の閾値未満のとき、即ち接触が検知されないときは、切替制御部５１１〜５１６において力制御から位置制御に切り替えを行ってロボット２００の動作の停止を行う。つまり、複数の角度センサ５５１〜５５６のそれぞれの値がそれぞれ所定の値となるように各モータ２１１〜２１６に対するそれぞれのトルク指令値τＭｒｅｆ１〜τＭｒｅｆ６を求める。ここで、トルク指令値τＭｒｅｆ１〜τＭｒｅｆ６は、第１実施形態で説明したトルク指令値τＭＰｒｅｆ１〜τＭＰｒｅｆ６である。以上、手先に作用する力Ｆが所定の閾値未満の場合には、複数の角度センサ５５１〜５５６のそれぞれの値を予め設定した所定の値に近づける位置制御で、ロボット２００の動作を停止させる。なお、力Ｆが所定の閾値のときは、力制御でロボット２００の動作を停止させるようにしたが、位置制御でロボット２００の動作を停止させるようにしてもよい。
【０１１２】
以上、ワークへの接触前は、力制御のまま停止を行おうとすると、倣うべきワークが存在しないため、ワーク等に接触するまで停止できないことがある。ワークに接触していない場合に、位置制御に切り替えて停止させることで、速くロボット２００を停止させることができる。
【０１１３】
図１６は、ワーク接触時の時間に対する手先力Ｆの変化を示すグラフである。手先力Ｆは、並進ＸＹＺ及び回転ＸＹＺからなるベクトルであるが、簡単化のためにひとつの方向のみを図示している。
【０１１４】
ワーク接触検知は、手先力Ｆをあらかじめワークに対して設定された閾値により判定することにより行う。なお、図１６に図示した閾値による接触検知は一例であり、これに限定するものではない。例えば、手先力の変化率で閾値判定してもよい。
【０１１５】
以上、第５実施形態によれば、ＣＰＵ３０１が力制御の途中に停止命令を受けた場合かつワークに接触していないと判定される場合において、位置制御に切り替えて停止を行うため、力制御で停止させる場合に比べロボットを速く停止することができる。
【０１１６】
なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。
【０１１７】
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１１８】
また、上述の実施形態では、ロボット２００（ロボットアーム２５１）が垂直多関節の場合について説明したが、これに限定するものではない。ロボットが、例えば、水平多関節のロボット、パラレルリンクのロボット、直交ロボット等、種々のロボットであってもよい。つまり、関節の駆動方向は、回転方向の駆動だけに限らず、直動方向の駆動（伸縮駆動）も含まれる。さらに、関節を駆動するのが電動モータである場合について説明したが、これに限定するものではなく、例えば人工筋肉等であってもよい。また、上述の実施形態では、ロボットが６つの関節を有する場合について説明したが、関節の数はこれに限定されるものではない。
【０１１９】
また、上述の実施形態では、力検知部が、複数のトルクセンサで構成される場合について説明したが、これに限定するものではなく、ロボットアームの先端に設けられた力覚センサであってもよい。
【符号の説明】
【０１２０】
１００…ロボット装置、２００…ロボット、２１１〜２１６…電動モータ（モータ）、２５１…ロボットアーム、２５２…ロボットハンド、３００…ロボット制御装置（制御部）
【図１】
0006794194.tif 000004
【図２】
0006794194.tif 000005
【図３】
0006794194.tif 000006
【図４】
0006794194.tif 000007
【図５】
0006794194.tif 000008
【図６】
0006794194.tif 000009
【図７】
0006794194.tif 000010
【図８】
0006794194.tif 000011
【図９】
0006794194.tif 000012
【図１０】
0006794194.tif 000013
【図１１】
0006794194.tif 000014
【図１２】
0006794194.tif 000015
【図１３】
0006794194.tif 000016
【図１４】
0006794194.tif 000017
【図１５】
0006794194.tif 000018
【図１６】
0006794194.tif 000019