JP2015226961A - ロボットの制御装置 - Google Patents

Abstract

【課題】ロボットの停止動作において、ロボットの振動を抑制しつつ素早く所望の位置に停止させること。【解決手段】モータＭによって駆動されるロボットＲの関節Ｊの角度を制御する制御装置１０において、関節Ｊの角度を指示する関節角度指令値θＬを算出する関節角度指令算出部１１と、ロボットＲの関節軸に発生する軸力トルクを算出する軸力トルク計算部１２と、軸力トルク及びロボットＲの関節Ｊのばね定数より算出される当該関節Ｊのたわみ量を関節角度指令値θＬが指示する関節Ｊの角度に加算してモータ指令角度θＭを算出する弾性変形補償部１３と、ロボットＲがワークＷと接触したときのモータＭの角度を検出する停止位置検出部１５と、停止位置検出部１５がモータＭの角度を出力したときに、関節角度指令算出部１１より出力された関節角度指令値θＬに代えて、停止位置検出部１５で検出したモータの角度を出力する指令角度切替部１６とを備える。【選択図】図３

Description

本発明は、ロボットの動作を制御する制御装置に関し、特に、関節角度の変化によって姿勢が決まるロボットの動作を制御する制御装置に関する。

関節角度の変化によって姿勢が決まるロボットの一例として産業用ロボットがある。例えば、産業用ロボットの一種である溶接ロボットを用いて溶接を行う際に、溶接するワーク位置をセンシングするタッチセンシングと呼ばれる動作がある。タッチセンシングとは、溶接トーチに電圧を印加した状態で溶接ロボットを動かし、溶接トーチの溶接ワイヤがワークに接触した位置（つまり、ワークと溶接ワイヤの間の通電を検知した位置）をワーク位置として検出するセンシング動作である。

このタッチセンシングにおいて、通常は、ワークへの接触を検知したときのロボットの姿勢（つまり、ロボットの各関節のモータ角度）を基にして、ワークの位置が検出される。しかし、ワークの接触を検知したときからロボットの姿勢情報を取得するまでに遅れがある。そのため、溶接ワイヤを高速で移動させてワークへ接触させると、この遅れ時間の間にも溶接ワイヤが大きく移動してしまい、実際のワーク位置からずれた位置をワークの位置として検出してしまう。

従って、溶接ワイヤをワークへ接近させるときは、当該溶接ワイヤが低速で移動するように溶接ロボットを低速で動作させる。しかし、溶接ロボットを低速で動作させるとセンシングに時間がかかるといった別の問題が生じる。
これらの問題を解決する方法として、特許文献１は、自動溶接装置のワーク検出方法を開示し、特許文献２は、溶接ロボットのワイヤタッチセンシング方法を開示している。

特許文献１に開示のワーク検出方法は、消耗電極式溶接トーチに溶接電圧とセンシング電圧とを選択的に印加し、センシング電圧の印加時に前記溶接トーチから突出した消耗電極とワークとの通電状態を検出する通電検出手段を有する自動溶接装置のワーク検出方法であって、前記溶接トーチにセンシング電圧を印加して前記溶接トーチを高速で前記ワークに接近させ、前記通電検出手段からの通電検出出力によって前記溶接トーチの動作を停止させ、続いてその通電検出出力によって前記溶接トーチを低速で離反させて、前記ワークからの前記消耗電極の離反を前記通電検出手段にて検出し、前記通電検出手段からの離反検出出力を前記自動溶接装置の制御信号として用いることを特徴とするものである。

また、特許文献２に開示のワイヤタッチセンシング方法は、電圧を印加した溶接ワイヤを移動し、該ワイヤの短絡信号を検知したとき、ワイヤを低速で逆方向に移動し、短絡解消信号を検知した位置を被溶接体の実存位置であると判定することを特徴とするものである。
これら特許文献に開示の技術では、溶接トーチに電圧を印加した状態で、溶接ワイヤをワークに接触させ、ワークとの接触を検知した後にロボットを停止させる。そして、溶接ワイヤをワークから離反させるように低速で移動させ、溶接ワイヤとワークとの間が非通電状態となったときのロボット姿勢に基づいてワークの位置を検出する。

この方法であれば、溶接ワイヤを高速でワークに接近させた後に、一度停止させ、その後の離反動作時にワークの位置を検出することができるため、センシング時間も短く、精度よくワーク位置を検出することができるとされている。
そこで、特許文献３は、高速でワークに接近させた溶接ワイヤを停止させる技術の一例として、産業用機械の加減速方法を開示している。

特許文献３に開示の加減速方法は、設置場所との関係が第１のばね振動系に近似される機台と、該機台上に固定されたアクチュエータの動作により発生する力を受けて前記機台上を移動し、前記機台との関係が第２のばね振動系に近似される移動手段を有する制御対象とを備える産業用機械における前記移動手段の加減速を制御するための産業用機械の加減速方法であって、前記移動手段を一定の加速度で加減速させようとする場合に、加速時間および減速時間が前記機台および前記アクチュエータの質量の合計と前記第１のばね振
動系のばね定数に基づいて決定される前記機台の固有振動周期の整数倍の時間に等しくなるような速度指令を生成し、前記速度指令に基づいて前記移動手段が移動するような動作指令を前記アクチュエータに出力することを特徴とするものである。

固有振動周期に基づいた上述の加減速方法による減速軌跡を生成することで、産業用機械を振動なく停止させることができるとされている。

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

しかしながら、特許文献１，２に開示のように、溶接ワイヤがワークと接触したときにロボットを単純に急停止させると、溶接トーチ及び溶接ワイヤだけでなくロボット全体に振動が生じてしまうことが現場の実績として明らかになっている。ロボットが振動した状態のままで溶接ワイヤをワークから離反させると正しいワークの位置を検出できないので、ロボットに振動が生じた場合、ロボットの振動が収まるまで溶接ワイヤのワークからの離反を待たなくてはならない。従って、センシングに要する時間は長くなってしまう。

ここで、ロボットを振動なく停止させるとする特許文献３に開示の技術を用いることが考えられる。しかし、特許文献３の技術は、ロボットを停止させる位置が予め決められている場合に適用可能な技術である。つまり、停止位置が予め決められている場合には、その停止位置に向けて減速軌跡を生成することができるが、停止位置がワーク接触時に決まるワイヤタッチセンシング動作のように、停止位置が予め定められていない場合には減速軌跡を生成することができない。

また、溶接ロボットのような複数の関節を有する機器は、各関節で発生する振動が他の関節へも伝播して非常に複雑な振動となり、単純に固有振動周期を考慮したのみでは振動を抑えることはできない。
従って、特許文献３に開示の技術を、ワイヤタッチセンシング動作に適用しても、ロボットの停止時に発生する振動を抑制することができない。

本発明は、上述の問題点に鑑みてなされたものであり、ロボットの停止動作において、ロボットの振動を抑制しつつ、素早く所望の位置に停止させることのできるロボットの制御装置を提供することを目的とする。

上記課題を解決するため、本発明に係るは、以下の技術的手段を講じている。
即ち、本発明に係るロボットの制御装置は、モータによって駆動されるロボットの関節の角度を制御する制御装置であって、前記ロボットに所望の動作をさせるために、前記関節の角度を指示する第１の角度指令を算出して出力する関節角度指令算出部と、前記第１の角度指令を受け取ると、前記受け取った第１の角度指令に従って動作する前記ロボットの関節軸に発生する第１の軸力トルクを、前記ロボットの動力学モデルに基づいて算出する軸力トルク計算部と、前記第１の軸力トルク、及び前記ロボットの関節の剛性を表すばね定数より算出される当該関節のたわみ量を、前記軸力トルク計算部が受け取った第１の角度指令が指示する関節の角度に加算して、前記モータの回転角度を指示するモータ指令角度を算出し出力する弾性変形補償部と、前記ロボットが外部の構造物と接触したときの前記モータの角度をモータ検出角度として検出して出力する停止位置検出部と、前記停止位置検出部が前記モータの角度を出力したときに、前記関節角度指令算出部より出力された第１の角度指令に代えて、前記停止位置検出部で検出した前記モータの角度を、前記関節の角度を指示する第２の角度指令として出力する指令角度切替部と、を備えることを特徴とする。

ここで、前記軸力トルク計算部が、前記第２の角度指令を受け取ると、前記受け取った第２の角度指令に従って動作する前記ロボットの関節軸に発生する第２の軸力トルクを、
前記ロボットの動力学モデルに基づいて算出し、前記弾性変形補償部が、前記第２の軸力トルク、及び前記関節のたわみ量を、前記軸力トルク計算部が受け取った第２の角度指令が指示する関節の角度に加算してモータ指令角度を算出するとよい。

さらに、前記軸力トルク計算部より出力される第１の軸力トルク又は第２の軸力トルクと前記関節の剛性を示すばね定数とから算出される関節のたわみ量を、前記モータ検出角度から減算して、補正後モータ検出角度を算出する逆弾性変形補償部を備え、前記停止位置検出部が、前記モータ検出角度の代わりに前記補正後モータ検出角度を検出して出力するとよい。

加えて、前記第１の角度指令を受け取ると、前記算出された関節角度指令通りに動作する際に前記モータに発生するＦＦトルクを、前記ロボットとモータの動力学モデルに基づいて算出するＦＦトルク計算部と、前記出力されたモータ指令角度に従って、前記モータに発生させるトルクを指示するトルク指令を出力する位置速度制御部と、前記位置速度制御部から出力されたトルク指令に前記ＦＦトルクを加算した指令値に従って、前記モータを制御するトルク制御部とを備えるとよい。

本発明に係るロボットの制御装置を用いることで、ロボットの停止動作において、ロボットの振動を抑制しつつ、素早く所望の位置に停止させることができる。

本発明の実施形態による多関節ロボットである溶接ロボット全体の概略構成を示す概略図である。 本発明の実施形態による制御装置の概略構成を示す図である。 本発明の第１実施形態による上位コントローラの概略構成を示すブロック図である。 本実施形態によるロボットの関節に存在するたわみ要素（弾性変形要素）を模式的に示す図である。 本実施形態によるサーボアンプの概略構成を示すブロック図である。 本実施形態（第１実施形態）による制御装置によってタッチセンシング動作を行わせたときの、ロボットの第２軸及び第３軸のモータ及びアームの挙動を表すグラフを示す図である。 停止位置検出部及び指令角度切替部を備えない制御装置によってタッチセンシング動作を行わせたときの、ロボットの第２軸及び第３軸のモータ及びアームの挙動を表すグラフを示す図である。 本発明の第２実施形態による上位コントローラの概略構成を示すブロック図である。 本実施形態（第２実施形態）による制御装置によってタッチセンシング動作を行わせたときの、ロボットの第２軸及び第３軸のモータ及びアームの挙動を表すグラフを示す図である。

以下、本発明の実施の形態に係るロボットの制御装置を、図面に基づき詳しく説明する。
なお、以下に説明する各実施形態に共通する同一の構成部材には、同一の符号及び同一の名称を付すこととする。従って、同一の符号及び同一の名称が付された構成部材については、同じ説明を繰り返さない。
［第１実施形態］
図１〜７を参照しながら、本発明の第１実施形態に係るロボットＲの制御装置１０について説明する。

まず初めに、本実施形態に係る制御装置１０によるロボットＲの一般的な制御方法について説明する。本実施形態に係る制御装置１０によって制御されるロボットＲは、少なくとも１つ以上の関節Ｊを有し、当該関節Ｊの角度をモータＭの駆動（回転）によって変更することで動作する（姿勢を変更する）ロボットである。制御装置１０は、モータＭの回
転を制御して関節の角度を変更することで、当該ロボットＲを動作させる。

本実施形態では、上述の制御装置１０によって動作が制御されるロボットＲとして、多関節の溶接ロボットＲを例示する。
図１は、多関節ロボットである溶接ロボットＲ全体の概略構成を示す概略図である。溶接ロボットＲ（以下、単にロボットＲという）は、例えば、６軸の垂直多関節型であって６つの関節Ｊ（Ｊ１〜Ｊ６）を備え、先端軸に溶接トーチ１が設けられ、溶接トーチ１から送りだされる溶接ワイヤ２によりアーク溶接が行われる。このロボットＲは、例えば、溶接トーチ１を溶接開始点と溶接終了点とを結ぶ溶接線方向に沿って移動しつつ、溶接ワイヤ２を予め定められた振幅および周波数で傾動する動作（ウィービング動作）を行う。このようなロボットＲの動作は制御装置１０によって制御されるが、その動作は、予め制御装置１０に教示されている。

図２は、制御装置１０の概略構成を示す図である。図２に示すように、ロボットＲの制御装置１０は、大きく分けて、ロボットＲ全体の動作を制御する上位コントローラＣ１と、ロボットの関節Ｊ（Ｊ１〜Ｊ６）のモータＭ（Ｍ１〜Ｍ６）の動作を制御するサーボアンプＳＡ（ＳＡ１〜ＳＡ６）とで構成されている。上位コントローラＣ１、サーボアンプＳＡ及びロボットＲは互いに通信回線によって接続されており、この通信回線を介して情報伝達が行われる。

次に、図３及び図４を参照して、上位コントローラＣ１とサーボアンプＳＡ（ＳＡ１〜ＳＡ６）について説明する。図３は、上位コントローラＣ１の概略構成を示すブロック図であり、図４は、サーボアンプＳＡ（ＳＡ１〜ＳＡ６）の概略構成を示すブロック図である。
図３に示す上位コントローラＣ１は、予め記憶されているロボットＲの動作情報（一般に、教示データと呼ばれる）に従ってロボットＲが動くように、後述するサーボアンプＳＡに対する指令値を生成する。上位コントローラＣ１は、関節角度指令算出部１１、軸力トルク計算部１２、弾性変形補償部１３及びＦＦトルク計算部１４を備え、さらに、停止位置検出部１５及び指令角度切替部１６を備える。

関節角度指令算出部１１は、ロボットＲに所望の動作をさせるために、関節Ｊの角度を指示する関節角度指令値を算出して出力するものであり、予め記憶されているロボットＲの動作情報（教示データ）に基づいて、ロボットＲの先端である溶接トーチ１の姿勢から逆変換を行ってロボットＲの各関節Ｊの角度指令値（関節角度指令値θ_Ｌ）を第１の角度指令として生成する。ロボットＲの動作情報が、ロボットＲの各関節Ｊの角度そのものである場合もあり、その場合、関節角度指令算出部１１は、動作情報である各関節Ｊの角度を、そのまま各関節Ｊの関節角度指令値とする。

関節角度指令算出部は、生成された関節角度指令値θ_Ｌそのままを、モータＭを動かすための指令角度であるモータ指令角度θ_Ｍとして、後述するサーボアンプＳＡへ出力することができる。つまり、単純には、モータ指令角度θ_ＭをロボットＲの関節角度（アーム角度）と等しくても良いのであるが、ロボットＲの関節Ｊには図４に示すようなたわみ要素（弾性変形要素）があるため、実際にはモータ指令角度θ_Ｍ≠ロボット関節角度となる。そのため、正確にロボットＲを動作させる場合には、このたわみ（弾性変形）によって生じる誤差分を補償することが行われる。この補償のことを弾性変形補償と呼ぶ。

図４は、ロボットＲの関節Ｊに存在するたわみ要素（弾性変形要素）を模式的に示す図であり、モータＭの回転軸とアームＡとの間の減速機Ｇなどが弾性変形要素となって、モータ角度と関節角度が必ずしも一致しなくなる。
次に、この弾性変形補償を行う軸力トルク計算部１２及び弾性変形補償部１３について説明する。

軸力トルク計算部１２は、第１の角度指令である関節角度指令値θ_Ｌを受け取ると、受け取った関節角度指令値θ_Ｌに従って動作するロボットＲの関節軸に発生するトルクτ_Ｌを第１の軸力トルクとして、ロボットＲの動力学モデルに基づいて算出する。具体的に、軸力トルク計算部１２は、ロボットＲの関節角度指令値θ_Ｌから下式（１）に示すロボットＲの動力学モデルを用いて関節Ｊに生じる軸力トルクであるトルクτ_Ｌを計算して出力
する。

弾性変形補償部１３は、トルクτ_Ｌ、及びロボットＲの関節Ｊの剛性を表すばね定数より算出される当該関節Ｊのたわみ量を、軸力トルク計算部１２が受け取った関節角度指令値θ_Ｌが指示する関節Ｊの角度に加算して、モータＭの回転角度を指示するモータ指令角度を算出し出力する。具体的に、弾性変形補償部１３は、軸力トルク計算部１２からトルクτ_Ｌを受信して、受信したトルクτ_Ｌに基づいて、下式（２）のように関節Ｊのたわみ量（τ_Ｌ/Ｋ）を求め、この求めた関節Ｊのたわみ量を用いて、モータＭの回転角度を指示するモータ指令角度θ_Ｍを算出し出力する。

弾性変形補償部１３は、上記の式（２）によって算出した各関節ＪのモータＭに対するモータ指令角度θ_Ｍを後述するサーボアンプＳＡへ出力し伝達する。例えば、ロボットＲが６軸の多関節ロボットであって関節Ｊが６つある場合は、第１軸〜第６軸の６つの関節Ｊ（Ｊ１〜Ｊ６）のモータＭ１〜Ｍ６に対する６つのモータ指令角度θ_Ｍ１〜θ_Ｍ６を後述するサーボアンプＳＡ１〜ＳＡ６へ出力する。

また一方で、ロボットＲを精度よく動作させるためには、上述の軸力トルクの計算と弾性変形補償に加えて、フィードバック（ＦＢ）制御とフィードフォワード（ＦＦ）制御を組み合わせた２自由度制御が採用される。ここで、フィードバック制御とは、モータ指令角度θ_Ｍなどの指令値と、ロボットＲなどの制御対象の状態（関節角度などの実測値）との比較に基づいて行われる制御である。一方でフィードフォワード制御とは、制御対象であるロボットＲのモデルを用いて与えられた指令値を用いて、当該指令値通りにロボットＲを動作させるためのモータＭへの入力を予測する制御である。

２自由度制御の利点は、フィードバック制御だけでは指令値に対するロボット動作の制御が遅れてしまうという影響を、フィードフォワード制御のモデルに基づいた制御によって補い解消できる点にあり、この２自由度制御により指令値通りにモータＭを動作させることができる。ここで必要になるのが、ロボットＲのモデルに基づいて、当該ロボットＲを指令値通りに動作させるための入力、すなわちモータＭの駆動トルクτ_ｒを算出することである。

ＦＦトルク計算部１４は、第１の角度指令である関節角度指令値θ_Ｌを受け取ると、算出された関節角度指令値θ_Ｌ通りに動作する際にモータＭに発生するＦＦトルクを、ロボットＲとモータＭの動力学モデルに基づいて算出する。具体的に、ＦＦトルク計算部１４は、ロボットＲを指令値通りに動作させるためのモータＭの駆動トルクτ_ｒ（ＦＦトルクτ_ｒと呼ぶ）を、ロボットＲの動力学モデルとモータＭの動力学モデルに基づいて、例えば以下に示す式（３）〜（５）によって算出し出力する。

ＦＦトルク計算部１４は、上述の式（３）〜（５）によって算出された、各モータＭを指令値通りに動作させるために必要なＦＦトルクτ_ｒを後述するサーボアンプＳＡへ出力し伝達する。例えば、ロボットＲが６軸の多関節ロボットであって関節Ｊが６つある場合は、第１軸〜第６軸の６つの関節Ｊ１〜Ｊ６のモータＭ１〜Ｍ６に対する６つのＦＦトルクτ_ｒ１〜τ_ｒ６を後述するサーボアンプＳＡ１〜ＳＡ６へ伝達する。

次に、図２及び図５を参照して、サーボアンプＳＡについて説明する。図５は、サーボアンプＳＡの内部構成を示す図であり、図２に示すサーボアンプＳＡ（ＳＡ１〜ＳＡ６）の各々は、図５に示す構成を有する。
サーボアンプＳＡは、上位コントローラＣ１から出力された指令値であるモータ指令角度θ_Ｍ及びＦＦトルクτ_ｒを受信して、これら指令値に従ってモータＭを動作させるものであり、位置速度制御部１７及びトルク制御部１８を有する。サーボアンプＳＡは、第１軸〜第６軸のモータＭ（Ｍ１〜Ｍ６）の各々に対して１つずつ設けられている。例えば、モータＭ１に対して設けられたサーボアンプＳＡ１は、モータ指令角度θ_Ｍ１及びＦＦトルクτ_ｒ１を受信して、これら受信した指令値に従ってモータＭ１を動作させ、モータＭ２に対して設けられたサーボアンプＳＡ２は、モータ指令角度θ_Ｍ２及びＦＦトルクτ_ｒ２を受信して、これら受信した指令値に従ってモータＭ２を動作させる。

サーボアンプＳＡの位置速度制御部１７は、上位コントローラＣ１から出力されたモータ指令角度θ_Ｍに従って、モータＭに発生させるトルクを指示するトルク指令を出力する。具体的に、位置速度制御部１７は、上位コントローラＣ１で算出されたモータ指令角度θ_Ｍと、モータＭに備え付けられたエンコーダ等の角度検出装置により検出されたモータ検出角度θ_ＦＢ及びモータ検出速度ｄ(θ_ＦＢ)とに基づいて主にフィードバック処理を行い、例えばロボットＲの第３軸の関節をモータ指令角度θ_Ｍ３通りに動作させるためのトルクを算出する。

トルク制御部１８は、位置速度制御部１７から出力されたトルク指令にＦＦトルクτ_ｒを加算した指令値に従って、モータＭを制御する。具体的に、トルク制御部１８は、位置速度制御部１７が算出したトルクに上位コントローラＣ１で算出されたＦＦトルクτ_ｒを加算し、所望のトルクが発生するようにモータＭを制御して動作させる。
上位コントローラＣ１及びサーボアンプＳＡにおける上述の構成は、本実施形態による
ロボットＲの動作制御を行う制御装置１０の基本的な構成である。この動作制御における特徴は、関節Ｊのたわみを補償する弾性変形補償を行っている点と、指令値通りに動作するためのトルクをロボットＲの動力学モデルに基づいて計算する２自由度制御を行っている点の２点である。

本実施形態による制御装置１０は、この２点の動作制御を備えているので、上述のロボットのような複数関節を有する複雑な機体を指令値通りに動作させることが可能となり、言い換えれば、振動なく動作を行うことが可能となる。
しかし、上述の動作制御は、ロボットＲを、あくまでも予め定められた、つまり予め記憶されている動作情報（教示データ）に従った指令値に基づいて動作させる場合において有効である。予め記憶されているロボットＲの動作情報（教示データ）が無く、新たに動作情報（教示データ）を作成する際のロボット動作において、制御装置１０は、上述の構成だけではロボットＲを指令値通りに動作させることは困難となる。

具体的に述べると、溶接ロボットなどの教示データを作成する際に、図１に示すようにワークＷの位置及び形状を把握することを目的として、ロボットＲの先端軸に設けられた溶接トーチ１の溶接ワイヤ２をワークＷに接触するまで移動させるタッチセンシング動作が行われる。このタッチセンシング動作は当業者において周知のロボット動作であるが、溶接ワイヤ２がワークＷに接触したその位置でロボットＲの動作を直ちに停止しなくてはならない。このように、ロボットＲを予め決められていない位置で急に停止させると、関節Ｊの弾性変形などによる振動がロボットＲ全体に発生し、特に溶接トーチ２が設けられた先端軸近傍は大きな振動が発生する。

本実施形態による制御装置１０は、タッチセンシング動作のように停止位置が予め定められていない場合でも、ロボット動作の急停止時に発生するロボットＲの振動を抑制することを目的として、図３に示すように停止位置検出部１５及び指令角度切替部１６を備える。
停止位置検出部１５は、ロボットＲが外部の構造物であるワークＷと接触したときの当該モータＭの角度をモータ検出角度として検出して出力する。具体的に、停止位置検出部１５は、タッチセンシング動作などにおいて溶接トーチ１の溶接ワイヤ２がワークＷと接触したときに、このワーク接触が検出されたときのモータ検出角度θ_ＦＢ（モータ検出角度θ_ＦＢ１〜θ_ＦＢ６）を検出し記録する。

指令角度切替部１６は、停止位置検出部１５がモータＭの角度を出力したときに、関節角度指令算出部１１より出力された関節角度指令値θ_Ｌに代えて、停止位置検出部１５で検出したモータＭの角度を、関節Ｊの角度を指示する第２の角度指令として出力する。具体的に、指令角度切替部１６は、関節角度指令算出部１１が出力した関節角度指令値θ_Ｌを受信すると共に、停止位置検出部１５が検出したモータ検出角度θ_ＦＢを受信し、ワーク接触が検出されていないときは関節角度指令値θ_Ｌを出力し、ワーク接触が検出されたときは関節角度指令値θ_Ｌに代えてモータ検出角度θ_ＦＢを第２の角度指令として出力することで、関節角度指令値θ_Ｌをモータ検出角度θ_ＦＢに切り替える。

指令角度切替部１６から出力された関節角度指令値θ_Ｌ及びモータ検出角度θ_ＦＢは、上述の軸力トルク計算部１２及び弾性変形補償部１３による処理を経る。具体的には、軸力トルク計算部１２が、第２の角度指令であるモータ検出角度θ_ＦＢを受け取ると、受け取ったモータ検出角度θ_ＦＢに従って動作するロボットＲの関節軸に発生するトルクτ_Ｌを第２の軸力トルクとして、ロボットＲの動力学モデルに基づいて算出する。さらに、弾性変形補償部１３が、第２の軸力トルクであるトルクτ_Ｌ、及び関節Ｊのたわみ量を、軸力トルク計算部１２が受け取ったモータ検出角度θ_ＦＢが指示する関節Ｊの角度に加算してモータ指令角度を算出する。この算出されたモータ指令角度が、モータ指令角度θ_ＭとしてサーボアンプＳＡへ出力され、また上述のＦＦトルク計算部１４による処理を経て、ＦＦトルクτ_ｒとしてサーボアンプＳＡへ出力される。

以上に説明した上位コントローラＣ１及びサーボアンプＳＡを備える制御装置１０によってロボットＲの動作を制御すれば、例えば、教示データを作成する際のロボット動作など、予め定められていない位置でロボットＲを急停止させる必要のある動作において、ロ
ボットＲを振動させることなく素早く停止させることができる。
図６及び図７を参照して、本実施形態による制御装置１０の効果を示す。図６は、本実施形態の制御装置１０によってタッチセンシング動作を行わせたときの、ロボットＲの第２軸及び第３軸のモータＭ及びアームの挙動を表すグラフを示す。図７は、停止位置検出部１５及び指令角度切替部１６を備えない制御装置によってタッチセンシング動作を行わせたときの、ロボットＲの第２軸及び第３軸のモータＭ及びアームの挙動を表すグラフを示す。

図６を参照すると、本実施形態の制御装置１０を用いれば、第２軸及び第３軸共、時間３秒（ｓ）で示すワーク接触時刻から０.０５秒（ｓ）後の３.０５秒以降は、アーム角度は大きく変化することなく、つまり振動することなく停止したことを示しており、ロボットＲを振動なく素早く停止できることが分かる。
これに対して、図７を参照すると、停止位置検出部１５及び指令角度切替部１６を備えない制御装置１０を用いたときは、３.０５秒以降においてもアーム角度が大きく変化して、振動が続いていることを示しており、ロボットＲを振動なく素早く停止できないことが分かる。
［第２実施形態］
次に、図８及び図９を参照しながら、本発明の第２実施形態に係るロボットＲの制御装置２０について説明する。図８は、本実施形態による上位コントローラＣ２の概略構成を示すブロック図である。図９は、本実施形態の制御装置２０によってタッチセンシング動作を行わせたときの、ロボットＲの第２軸及び第３軸のモータＭ及びアームの挙動を表すグラフを示す図である。

本実施形態による制御装置２０の構成は、第１実施形態による制御装置１０の構成と比べて、第１実施形態による上位コントローラＣ１に相当する上位コントローラＣ２が逆弾性変形補償部２１を備える点が異なり、逆弾性変形補償部２１以外の構成は、第１実施形態による制御装置１０の構成と同様である。従って、以下には、逆弾性変形補償部２１についての説明と、制御装置２０が逆弾性変形補償部２１を備えることで得られる効果について記載する。

まず初めに、第１実施形態で説明した図６を参照すると、第２軸及び第３軸共、急停止による振動を抑制することに成功しているが、３.０５秒以降のアーム角度、つまりロボット停止位置は、実線で示すワーク接触時の位置から少なからず離れている。これは、指令角度切替部１６によって、単純に、関節角度指令値θ_Ｌをワーク接触時に記録したモータ検出角度θ_ＦＢに置き換えているためである。

つまり、ロボット関節角度をワーク接触時に記録したモータ検出角度で置き換えているためである。第１実施形態で述べたようにモータ指令角度θ_Ｍ≠ロボット関節角度であるため、関節角度指令値θ_Ｌを単純にモータ検出角度θ_ＦＢに置き換えてしまうと、関節Ｊのたわみ量分だけずれてしまう。
本実施形態による制御装置２０は、このロボット停止位置がワーク接触時の位置からずれてしまうという問題を解決するために、逆弾性変形補償部２１を備えている。

逆弾性変形補償部２１について、説明する。
逆弾性変形補償部２１は、軸力トルク計算部１２より出力されるトルクτ_Ｌ（第１の軸力トルク又は第２の軸力トルク）と関節の剛性を示すばね定数とから算出される関節のたわみ量を、モータ検出角度θ_ＦＢから減算して、補正後モータ検出角度を算出する。具体的に、逆弾性変形補償部２１は、軸力トルク計算部１２で算出されたトルクτ_Ｌを基に、モータ検出角度θ_ＦＢに対して下式（６）に示す補正（減算）を行って、新たに補正後のモータ検出角度（補正後モータ検出角度）を生成し、停止位置検出部１５へ出力する。

そして、停止位置検出部１５は、モータ検出角度θ_ＦＢの代わりに補正後モータ検出角度を検出して記録すると共に出力し、上述した第１実施形態と同様の処理を行う。指令角度切替部１６は、関節角度指令算出部１１が出力した関節角度指令値θ_Ｌを受信すると共に、停止位置検出部１５が検出した補正後モータ検出角度を受信し、第１実施形態と同様の動作を行う。

本実施形態による制御装置２０の技術的ポイントは、ワーク接触が検出されたときのモータ検出角度を逆弾性変形補償部２１によって補正した補正後モータ検出角度を、指令角度切替部１６からロボット関節角度（関節角度指令値θ_Ｌ）に代えて出力することにあるといえる。
すなわち、本実施形態の弾性変形補償部２１は、前述したとおり、ロボットＲの関節Ｊのたわみ量を、軸力トルク計算部１２が受け取った関節角度指令値θ_Ｌが指示する関節の角度に加算することで、ロボットＲ自体に存在する弾性変形の影響を補償するものである。この補償により、ロボットＲの急停止による振動を抑制することが可能となっているが、停止後であっても弾性変形補償部１３による補償が作用し、ワーク接触時の位置から少なからず離れた状態で、ロボットＲが停止することとなる。

そこで、弾性変形補償部１３の特性とは全く逆の特性を有する逆弾性変形補償部２１を用意し、逆弾性変形補償部２１によって補正した補正後モータ検出角度を、指令角度切替部１６からロボット関節角度に代えて出力することで、ロボットＲの停止後における弾性変形補償部１３による補償を打ち消すことが可能となる。
それ故、本実施形態による制御装置２０は、上述の逆弾性変形補償部２１を有する上位コントローラＣ２を備える制御装置２０によってロボットＲの動作を制御すれば、例えば、教示データを作成する際のロボット動作など、予め定められていない位置でロボットＲを急停止させる必要のある動作において、ロボットＲを振動させることなく素早く停止させることに加えて、所望の停止位置（ワーク接触時の位置近傍）に正確に停止させることができる。従って、ロボット停止位置がワーク接触時の位置からずれてしまうという問題を解決することができる。

図９を参照して、本実施形態による制御装置２０の効果を示す。図９は、本実施形態の制御装置２０によってタッチセンシング動作を行わせたときの、ロボットＲの第２軸及び第３軸のモータＭ及びアームの挙動を表すグラフを示す。
図９を参照すると、本実施形態の制御装置２０を用いれば、第２軸及び第３軸共、時間３秒（ｓ）で示すワーク接触時刻から０.０５秒（ｓ）後の３.０５秒以降は、アーム角度は大きく変化することなく、つまり振動することなく停止したことを示しており、ロボットＲを振動なく素早く停止できることが分かる。これに加えて、ロボット停止位置は、実線で示すワーク接触時の位置にほぼ一致しており、ロボット停止位置がワーク接触時の位置からほとんどずれていないことが分かる。

なお、今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。特に、今回開示された実施形態において、明示的に開示されていない事項、例えば、運転条件や操業条件、各種パラメータ、構成物の寸法、重量、体積などは、当業者が通常実施する範囲を逸脱するものではなく、通常の当業者であれば、容易に想定するこ
とが可能な値を採用している。

１ 溶接トーチ
２ 溶接ワイヤ
１０，２０ 制御装置
１１ 関節角度指令算出部
１２ 軸力トルク計算部
１３ 弾性変形補償部
１４ ＦＦトルク計算部
１５ 停止位置検出部
１６ 指令角度切替部
１７ 位置速度制御部
１８ トルク制御部
２１ 逆弾性変形補償部
Ａ アーム
Ｇ 減速機
Ｊ 関節
Ｍ モータ
Ｒ 溶接ロボット（ロボット）
Ｗ ワーク
Ｃ１，Ｃ２ 上位コントローラ
ＳＡ サーボアンプ

上記課題を解決するため、本発明に係るは、以下の技術的手段を講じている。
即ち、本発明に係るロボットの制御装置は、モータによって駆動されるロボットの関節の角度を制御する制御装置であって、前記ロボットに所望の動作をさせるために、前記関節の角度を指示する第１の角度指令を算出して出力する関節角度指令算出部と、前記第１の角度指令を受け取ると、前記受け取った第１の角度指令に従って動作する前記ロボットの関節軸に発生する第１の軸力トルクを、前記ロボットの動力学モデルに基づいて算出する軸力トルク計算部と、前記第１の軸力トルク、及び前記ロボットの関節の剛性を表すばね定数より算出される当該関節のたわみ量を、前記軸力トルク計算部が受け取った第１の角度指令が指示する関節の角度に加算して、前記モータの回転角度を指示するモータ指令角度を算出し出力する弾性変形補償部と、前記ロボットが外部の構造物と接触したときの前記モータの角度をモータ検出角度として検出して出力する停止位置検出部と、前記停止位置検出部が前記モータの角度を出力したときに、前記関節角度指令算出部より出力された第１の角度指令に代えて、前記停止位置検出部で検出した前記モータの角度を、前記関節の角度を指示する第２の角度指令として出力する指令角度切替部と、を備えていて、前記軸力トルク計算部が、前記第２の角度指令を受け取ると、前記受け取った第２の角度指令に従って動作する前記ロボットの関節軸に発生する第２の軸力トルクを、前記ロボットの動力学モデルに基づいて算出し、前記弾性変形補償部が、前記第２の軸力トルクを基に算出された前記関節のたわみ量を、前記軸力トルク計算部が受け取った第２の角度指令が指示する関節の角度に加算してモータ指令角度を算出するものとされ、
前記軸力トルク計算部より出力される第１の軸力トルク又は第２の軸力トルクと前記関節の剛性を示すばね定数とから算出される関節のたわみ量を、前記モータ検出角度から減算して、補正後モータ検出角度を算出する逆弾性変形補償部を備え、前記停止位置検出部が、前記モータ検出角度の代わりに前記補正後モータ検出角度を検出して出力することを特徴とする。

Claims (4)

1. モータによって駆動されるロボットの関節の角度を制御する制御装置であって、
   前記ロボットに所望の動作をさせるために、前記関節の角度を指示する第１の角度指令を算出して出力する関節角度指令算出部と、
   前記第１の角度指令を受け取ると、前記受け取った第１の角度指令に従って動作する前記ロボットの関節軸に発生する第１の軸力トルクを、前記ロボットの動力学モデルに基づいて算出する軸力トルク計算部と、
   前記第１の軸力トルク、及び前記ロボットの関節の剛性を表すばね定数より算出される当該関節のたわみ量を、前記軸力トルク計算部が受け取った第１の角度指令が指示する関節の角度に加算して、前記モータの回転角度を指示するモータ指令角度を算出し出力する弾性変形補償部と、
   前記ロボットが外部の構造物と接触したときの前記モータの角度をモータ検出角度として検出して出力する停止位置検出部と、
   前記停止位置検出部が前記モータの角度を出力したときに、前記関節角度指令算出部より出力された第１の角度指令に代えて、前記停止位置検出部で検出した前記モータの角度を、前記関節の角度を指示する第２の角度指令として出力する指令角度切替部と、を備えることを特徴とするロボットの制御装置。
2. 前記軸力トルク計算部が、前記第２の角度指令を受け取ると、前記受け取った第２の角度指令に従って動作する前記ロボットの関節軸に発生する第２の軸力トルクを、前記ロボットの動力学モデルに基づいて算出し、
   前記弾性変形補償部が、前記第２の軸力トルク、及び前記関節のたわみ量を、前記軸力トルク計算部が受け取った第２の角度指令が指示する関節の角度に加算してモータ指令角度を算出することを特徴とする請求項１に記載のロボットの制御装置。
3. 前記軸力トルク計算部より出力される第１の軸力トルク又は第２の軸力トルクと前記関節の剛性を示すばね定数とから算出される関節のたわみ量を、前記モータ検出角度から減算して、補正後モータ検出角度を算出する逆弾性変形補償部を備え、
   前記停止位置検出部が、前記モータ検出角度の代わりに前記補正後モータ検出角度を検出して出力することを特徴とする、請求項１又は２に記載のロボットの制御装置。
4. 前記第１の角度指令を受け取ると、前記算出された関節角度指令通りに動作する際に前記モータに発生するＦＦトルクを、前記ロボットとモータの動力学モデルに基づいて算出するＦＦトルク計算部と、
   前記出力されたモータ指令角度に従って、前記モータに発生させるトルクを指示するトルク指令を出力する位置速度制御部と、
   前記位置速度制御部から出力されたトルク指令に前記ＦＦトルクを加算した指令値に従って、前記モータを制御するトルク制御部とを備えることを特徴とする請求項１〜３のいずれかに記載のロボットの制御装置。