【解決手段】マニピュレーターと、前記マニピュレーターに設けられる力検出器と、目標位置に基づいて前記マニピュレーターを駆動するアクチュエーターと、を備えるロボットを制御するロボット制御装置において、目標力と前記力検出器の出力とに基づいて導出される前記マニピュレーターの動作位置と、前記目標位置とを画面に表示する表示制御部、を備える。
【発明を実施するための形態】
【0010】
以下、本発明の実施の形態を以下の順序にしたがって添付図面を参照しながら説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
(1)第1実施形態
本発明の第一実施例としてのロボットシステムは、
図1に示すように、ロボット1と、エンドエフェクター2と、制御装置3(コントローラー)と、教示端末4とティーチングペンダント5と、を備えている。制御装置3と教示端末4とは、本発明のロボット制御装置の構成例である。制御装置3は図示しないケーブルによりロボット1と通信可能に接続される。制御装置3は、ロボット1に駆動電力を供給する電源部31とロボット1を制御するための制御部32とを備える。教示端末4は、本発明の教示部の構成例である。制御装置3と教示端末4とは有線ケーブルで、または無線通信可能に接続される。教示端末4は、専用のコンピューターであってもよいし、ロボット1のためのプログラムがインストールされた汎用のコンピューターであってもよい。例えばロボット1を教示するための専用装置であるティーチングペンダント5を教示装置4の代わりに用いても良い。さらに、制御装置3と教示端末4とは、
図1に示すように別々の筐体を備えていてもよいし、一体に構成されていてもよい。
【0011】
ロボット1は、アームAに各種のエンドエフェクター2を装着して使用される単腕ロボットである。アームAは6つの関節J1、J2、J3、J4、J5、J6を備える。関節J1、J2、J3、J4、J5、J6によって6個のアーム部材A1〜A6が連結される。関節J2、J3、J5は曲げ関節であり、関節J1、J4、J6はねじり関節である。関節J6には、ワークに対して把持や加工等を行うための各種のエンドエフェクター2が装着される。先端の関節J6の回転軸上の所定位置をツールセンターポイント(TCP)と表す。アームAとエンドエフェクター2は本発明のマニュピレーターの構成例である。TCPの位置は各種のエンドエフェクター2の位置の基準となる。本実施例では、ワークWを把持するエンドエフェクター2が関節J6の先端に装着されているものとして説明する。
【0012】
関節J6には力覚センサーFSが備えられている。力覚センサーFSは、6軸の力検出器である。力覚センサーFSは、固有の座標系であるセンサー座標系において互いに直交する3個の検出軸と平行な力の大きさと、当該3個の検出軸まわりのトルクの大きさとを検出する。なお、力覚センサーFSは本発明の力検出器の構成例であるが、関節J6以外の関節J1〜J5のいずれか1つ以上に力検出器としての力覚センサーを備えても良い。
【0013】
ロボット1が設置された空間を規定する座標系をロボット座標系というとき、ロボット座標系は、水平面上において互いに直交するX軸とY軸と、鉛直上向きを正方向とするZ軸とによって規定される3次元の直交座標系である。Z軸における負の方向は概ね重力方向と一致する。またX軸周りの回転角をRXで表し、Y軸周りの回転角をRYで表し、Z軸周りの回転角をRZで表す。X,Y,Z方向の位置により3次元空間における任意の位置を表現でき、RX,RY,RZ方向の回転角により3次元空間における任意の姿勢を表現できる。以下、位置と表記した場合、姿勢も意味し得ることとする。また、力と表記した場合、トルクも意味し得ることとする。制御装置3は、アームAを駆動することによって、ロボット座標系においてTCPの位置を制御する。
【0014】
図2は、ロボットシステムのブロック図である。制御部32はロボット1の制御を行うための制御プログラムがインストールされたコンピューターである。制御部32は、プロセッサーやRAMやROMを備え、これらのハードウェア資源が制御プログラムと協働する。
【0015】
ロボット1は、
図1に図示した構成のほかに、アクチュエーターとしてのモーターM1〜M6と、センサーとしてのエンコーダーE1〜E6とを備える。モーターM1〜M6を制御することはアームAを制御することを意味する。モーターM1〜M6とエンコーダーE1〜E6とは、関節J1〜J6のそれぞれに対応して備えられており、エンコーダーE1〜E6はモーターM1〜M6の回転角度を検出する。制御装置3は、モーターM1〜M6の駆動位置の組み合わせと、ロボット座標系におけるTCPの位置との対応関係U1を記憶している。また、制御装置3は、ロボット1が行う作業の工程ごとに目標位置S
tと目標力f
Stとをコマンドとして記憶する。目標位置S
tと目標力f
Stを定めるコマンドはロボット1が行う作業の工程ごとに教示によって設定される。したがって、目標位置は教示位置と言い換えることもでき、目標力は教示力と言い換えることもできる。
【0016】
制御装置3は、例えばユーザーによる教示によって設定された目標位置と目標力とがTCPにて実現されるようにアームAを制御する。目標力とは、アームAの動作に応じて力覚センサーFSが検出すべき力である。ここでSの文字は、ロボット座標系を規定する軸の方向(X,Y,Z,RX,RY,RZ)のなかのいずれか1個の方向を表すこととする。また、Sは、S方向の位置も表すこととする。例えば、S=Xの場合、ロボット座標系にて設定された目標位置のX方向成分がS
t=X
tと表記され、目標力のX方向成分がf
St=f
Xtと表記される。
【0017】
制御装置3は、モーターM1〜M6の回転角度D
aを取得すると、対応関係U1に基づいて、当該回転角度D
aをロボット座標系におけるTCPの位置S(X,Y,Z,RX,RY,RZ)に変換する。制御装置3は、TCPの位置Sと、力覚センサーFSの検出値とに基づいて、力覚センサーFSに現実に作用している作用力f
Sをロボット座標系において特定する。制御装置3は、ロボット座標系におけるTCPの位置Sごとに、力覚センサーFSのセンサー座標系における検出軸の方向を規定した対応関係U2を記憶している。従って、制御装置3は、ロボット座標系におけるTCPの位置Sと対応関係U2とに基づいて、ロボット座標系における作用力f
Sを特定できる。制御装置3は、作用力f
Sに対して重力補償を行う。重力補償とは、作用力f
Sから重力に起因する力やトルクの成分を除去することである。重力補償を行った作用力f
Sは、エンドエフェクター2に作用している重力以外の力と見なすことができる。
【0018】
本実施例のインピーダンス制御は、仮想の機械的インピーダンスをモーターM1〜M6によって実現する能動インピーダンス制御である。制御装置3は、このようなインピーダンス制御を、ワークの嵌合作業、研磨作業など、エンドエフェクター2がワークから力を受ける接触状態の工程で適用する。制御装置3は、エンドエフェクター2がワークから力を受けない非接触状態の工程では、インピーダンス制御を適用せず、目標位置から線形演算で導出する回転角度でモーターM1〜M6を制御する。目標位置から線形演算で導出する回転角度でモーターM1〜M6を制御するモードを位置制御モードといい、モーターM1〜M6をインピーダンス制御するモードを力制御モードというものとする。制御装置3は、位置制御モードと力制御モードとを力覚センサーFSの出力に基づいて自律的に切り替えても良いし、コマンドに応じて位置制御モードと力制御モードとを切り替えても良い。
【0019】
制御装置3は、目標力f
Stと作用力f
Sとをインピーダンス制御の運動方程式に代入することにより、力由来補正量ΔSを特定する。力由来補正量ΔSとは、TCPが機械的インピーダンスを受けた場合に、目標力f
Stとの力偏差Δf
S(t)を解消するために、TCPが移動すべき位置Sの大きさを意味する。下記の(1)式は、インピーダンス制御の運動方程式である。
【数1】
【0020】
(1)式の左辺は、TCPの位置Sの2階微分値に仮想慣性係数mを乗算した第1項と、TCPの位置Sの微分値に仮想粘性係数dを乗算した第2項と、TCPの位置Sに仮想弾性係数kを乗算した第3項とによって構成される。(1)式の右辺は、目標力f
Stから現実の力fを減算した力偏差Δf
S(t)によって構成される。(1)式における微分とは、時間による微分を意味する。ロボット1が行う工程において、目標力f
Stとして一定値が設定される場合もあるし、目標力f
Stとして時間の関数が設定される場合もある。
【0021】
仮想慣性係数mはTCPが仮想的に有する質量を意味し、仮想粘性係数dはTCPが仮想的に受ける粘性抵抗を意味し、仮想弾性係数kはTCPが仮想的に受ける弾性力のバネ定数を意味する。各係数m,d,kは方向ごとに異なる値に設定されてもよいし、方向に拘わらず共通の値に設定されてもよい。制御装置3は、目標位置S
tに、力由来補正量ΔSを加算することにより、インピーダンス制御を考慮した動作位置(S
t+ΔS)を特定する。
【0022】
そして、制御装置3は、対応関係U1に基づいて、ロボット座標系を規定する各軸の方向の動作位置(S
t+ΔS)を、各モーターM1〜M6の目標の回転角度である目標角度D
tに変換する。そして、制御装置3は、目標角度D
tからモーターM1〜M6の現実の回転角度であるエンコーダーE1〜E6の出力D
aを減算することにより、駆動位置偏差D
e(=D
t−D
a)を算出する。そして、制御装置3は、駆動位置偏差D
eに位置制御ゲインK
pを乗算した値と、現実の回転角度D
aの時間微分値である駆動速度との差である駆動速度偏差に、速度制御ゲインK
vを乗算した値とを加算することにより、制御量D
cを導出する。なお、位置制御ゲインK
pおよび速度制御ゲインK
vは、比例成分だけでなく微分成分や積分成分にかかる制御ゲインを含んでもよい。制御量D
cは、モーターM1〜M6のそれぞれについて特定される。以上説明した構成により、制御装置3は、目標位置S
tと目標力f
Stとに基づいてアームAをインピーダンス制御することができる。
【0023】
教示端末4には、制御装置3に目標位置S
tと目標力f
Stとを教示するための教示プログラムがインストールされている。教示端末4は、ディスプレイ43やプロセッサーやRAMやROMを備え、これらのハードウェア資源が制御プログラムと協働する。これにより、
図2に示すように、教示端末4は、表示制御部41と教示部42として機能する。
【0024】
教示部42は、教示者の操作に応じて目標力f
stと目標位置S
tを指定するコマンドを生成して制御装置3に出力する。教示部42はコマンド生成指示を受け付けるための教示画面をディスプレイ43に表示する。教示部42は工程毎にコマンドを生成し、ロボット1による一連の作業、すなわちマニュピレーターの一連の動作を複数のコマンドによって定義する。このコマンドは、例えば「Move P30 FC1 ! D50; g_StepID = 9 ! 」のように表記される。この表記例では、TCPの移動を指示するコマンド「Move」に対して、目標パラメーターとして「P30 FC1」が設定されている。「P30」はTCPについて別途定められた目標位置S
tを示す関数である。「FC1」はTCPまたは力制御座標原点について別途定められた目標力f
Stを示す関数である。またこのコマンドにおいては、「D50」が表すコマンド実行開始から、Moveコマンドで予定された時間の50パーセントを経過した位置について、「g_StepID = 9」が工程IDとして「9」を定義している。工程IDはコマンドで区切られる工程の識別子であるが、このように工程中の特定の期間についての識別子として用いてもよい。教示者は、教示部42が表示する教示画面を介して教示端末4を操作し、このようなコマンドを制御装置3に出力させることによって、目標位置Stと目標力Fstとを制御装置3に記憶させることができる。目標位置Stと目標力Fstとを指定するコマンド群は、実行順に記述され、一連の複数のコマンドを含む教示ファイルとして制御装置3に記憶される。本実施例の教示とは、このようにして目標位置Stと目標力Fstとをロボット1に記憶させることである。
【0025】
表示制御部41は、教示者が設定する目標位置S
tと目標力F
stとを教示部42から取得し、制御装置3から力由来補正量ΔSと作用力f
sとを取得する。表示制御部41は、目標位置S
tと力由来補正量ΔSとに基づいて動作位置を導出し、動作位置、目標位置等をモニター画面に表示する。
【0026】
教示端末4は、表示制御部41が表示するモニター画面と、教示部42がコマンドの入力操作を受け付ける教示画面とを、ディプレイ43の一画面に表示することもできるし、教示者の要求に応じてモニター画面と教示画面とを切り換えることもできる。従って教示端末4はモニター画面に表示された目標軌跡と動作軌跡とに基づく教示者からの指示を、教示画面を介して受け付けることができる。
【0027】
ここで表示制御部41が動作位置、目標位置等を表示するモニター画面の構成例について説明する。表示制御部41は、
図3に示すモニター画面に、XYモニター101、XZモニター102、YZモニター103、Fxモニター111、Fyモニター112、Fzモニター113、Txモニター114、Tyモニター115、Tzモニター116、ステップモニター121、表示区間設定ボックス122a、122b、122c、xモニター131、yモニター132、zモニター133、Rzモニター134、Ryモニター136、Rxモニター137、工程表125、速度計141−146を配置する。教示者が教示ファイルを指定してモニター画面の表示を教示端末4に要求すると、表示制御部41は、教示ファイルに記述された一連のコマンドの開始から終了までの期間(区間)について、モニター画面に目標位置、動作位置、作用力、力由来補正量ΔS、速度等を以下のように表示する。
【0028】
表示制御部41は教示部42から取得する目標位置S
tと制御装置3の制御部32から動作位置を取得する。そして表示制御部41は、3次元の目標位置と3次元の動作位置とを同一の2次元座標平面に投影した軌跡をXYモニター101、XZモニター102、YZモニター103に表示する。XYモニター101、XZモニター102、YZモニター103の縦軸および横軸は位置(mm)である。具体的には、XYモニター101は、3次元の目標位置と3次元の動作位置とをXY座標平面に投影した軌跡を表示する領域である。XZモニター102は、3次元の目標位置と3次元の動作位置とをXZ座標平面に投影した軌跡を表示する領域である。YZモニター103は、3次元の目標位置と3次元の動作位置とをYZ座標平面に投影した軌跡を表示する領域である。なお、目標位置の軌跡と動作位置の軌跡をどのような形態で表示するかについては、軌跡の表示色を変えるなど、2つの軌跡が識別可能であれば良いが、
図3においては目標位置の軌跡は点線で表示し、動作位置の軌跡は実線で表示している。また目標位置と動作位置が一致している部分については、動作位置の軌跡を実線で表示している。このようにして目標位置と動作位置とが同一座標平面に表示されることによって、教示者はインピーダンス制御が行われる場合に生ずる目標位置と動作位置と、これらの差とを容易に把握することができる。
【0029】
Fxモニター111は、力覚センサーFSの検出値から座標変換された作用力f
Xを時間の関数として折れ線グラフで表示する領域である。Fyモニター112は、力覚センサーFSの検出値から座標変換された作用力f
Yを時間の関数として折れ線グラフで表示する領域である。Fzモニター113は、力覚センサーFSの検出値から座標変換された作用力f
Zを時間の関数として折れ線グラフで表示する領域である。Fxモニター111、Fyモニター112、Fzモニター113の横軸は時間(秒)、縦軸は力(N)である。Txモニター114は、力覚センサーFSの検出値から座標変換された作用力f
RXを時間の関数として折れ線グラフで表示する領域である。Tyモニター114は、力覚センサーFSの検出値から座標変換された作用力f
RYを時間の関数として折れ線グラフで表示する領域である。Tzモニター114は、力覚センサーFSの検出値から座標変換された作用力f
RZを時間の関数として折れ線グラフで表示する領域である。Txモニター114、Tyモニター115、Tzモニター116の横軸は時間(秒)、縦軸はトルク(Nm)である。
【0030】
ステップモニター121は、経過時間と工程の対応関係を表示する領域である。ステップモニター121の横軸は時間(秒)、縦軸は工程IDである。工程IDによって識別される作業内容は工程表125に表示される。表示区間設定ボックス122aは、XYモニター101、XZモニター102、YZモニター103、Fxモニター111、Fyモニター112、Fzモニター113、Txモニター114、Tyモニター115、Tzモニター116、xモニター131、yモニター132、zモニター133、Rzモニター134、Ryモニター136、Rxモニター137に目標位置、動作位置等を表示する工程の工程IDを入力するためのドロップダウンテキストボックスである。表示区間設定ボックス122bは、XYモニター101、XZモニター102、YZモニター103、Fxモニター111、Fyモニター112、Fzモニター113、Txモニター114、Tyモニター115、Tzモニター116、xモニター131、yモニター132、zモニター133、Rzモニター134、Ryモニター136、Rxモニター137に目標位置、動作位置等を表示する開始時間を入力するためのドロップダウンテキストボックスである。表示区間設定ボックス122cは、XYモニター101、XZモニター102、YZモニター103、Fxモニター111、Fyモニター112、Fzモニター113、Txモニター114、Tyモニター115、Tzモニター116、xモニター131、yモニター132、zモニター133、Rzモニター134、Ryモニター136、Rxモニター137、xモニター131、yモニター132、zモニター133、Rzモニター134、Ryモニター136、Rxモニター137に目標位置、動作位置等を表示する終了時間を入力するためのドロップダウンテキストボックスである。教示者は、表示区間設定ボックス122a、122b、122cに工程ID、もしくは、開始時間及び終了時間を入力することによって、教示ファイルに記述された一連のコマンドによって実行される作業の一部の区間について、モニター画面に動作位置、作用力、力由来補正量ΔS、動作位置速度等を表示させることができる。また、工程IDと、開始時間及び終了時間とを組み合わせて区間指定できるようにしてもよい。この場合、入力された工程IDのコマンドの実行開始からの経過時間に対して開始時間と終了時間が入力されることになる。
【0031】
xモニター131は、力由来補正量ΔSのX成分を時間の関数として折れ線グラフで表示する領域である。yモニター131は、力由来補正量ΔSのY成分を時間の関数として折れ線グラフで表示する領域である。zモニターは、力由来補正量ΔSのZ成分を時間の関数として折れ線グラフで表示する領域である。xモニター131、yモニター132、zモニター133の横軸は時間(秒)、縦軸は(mm)である。Rzモニター134は、力由来補正量ΔSのRz成分を時間の関数として折れ線グラフで表示する領域である。Ryモニター134は、力由来補正量ΔSのRy成分を時間の関数として折れ線グラフで表示する領域である。Rzモニター134は、力由来補正量ΔSのRz成分を時間の関数として折れ線グラフで表示する領域である。Rzモニター134、Ryモニター136、Rxモニター137の横軸は時間(秒)、縦軸は(度)である。
【0032】
速度計141は、動作位置の速度のX成分を回転針で表示する領域である。速度計142は、動作位置の速度のY成分を回転針で表示する領域である。速度計143は、動作位置の速度のZ成分を回転針で表示する領域である。速度計141−143の単位はmm/秒である。速度計144は、動作位置の速度のRZ成分を回転針で表示する領域である。速度計145は、動作位置の速度のRY成分を回転針で表示する領域である。速度計146は、動作位置の速度のRX成分を回転針で表示する領域である。速度計144−146の単位は度/秒である。
【0033】
表示制御部41は、モードに応じてモニター画面の表示形態を次のように切り換える。リアルタイムモードでは、コマンド実行によるロボット1の動作結果としての動作位置、作用力、力由来補正量ΔS、動作位置速度等がロボット1の動作中にリアルタイムで、すなわちハードウェア性能による不可避的な遅延を除けばロボット1の動作と同期して、モニター画面に表示される。したがってリアルタイムモードでは、表示制御部41は、制御装置3から力由来補正量および作用力を取得しながら動作位置を導出して動作位置の軌跡を描画する。再生モードでは、コマンド実行によるロボット1の動作結果としての動作位置、作用力、力由来補正量ΔS、動作位置速度等がログに基づいてモニター画面に動画表示される。このようなログは、コマンド実行中に教示部42または表示制御部41が制御装置3から取得してRAMに記憶する。
【0034】
また表示制御部41は、教示者の要求に応じて目標位置と動作位置の軌跡をxyモニター101、xzモニター102、yzモニター103に表示する対象領域と表示倍率を変更する。具体的には例えば、
図3に示すようにxyモニター101が表示されている状態において教示者から表示倍率変更要求を取得すると、表示制御部41は、
図4Aに示すように目標位置と動作位置の軌跡をxyモニター101に表示する対象領域を縮小し、軌跡の表示倍率を大きくすればよい。また、
図4Aに示すようにxyモニター101が表示されている状態において教示者から表示領域変更要求を取得すると、表示制御部41は、
図4Bに示すように目標位置と動作位置の軌跡をxyモニター101に表示する対象領域を、表示倍率を維持して変更すればよい。表示制御部41は、表示倍率変更要求や表示領域変更要求を、キーボードやマウスやタッチパネルを用いて教示者から取得することができる。
【0035】
また表示制御部41は、
図4Aに示すように目標位置と動作位置の軌跡にTCPの進行方向(目標位置と動作位置の変化方向)を表示しても良い。例えば、工程毎に軌跡上に進行方向を矢印で表示しても良いし、2以上の所定数の連続する工程毎に1つの矢印で進行方向を表示しても良いし、全行程を通じた目標位置の軌跡と動作位置の軌跡に1つずつの矢印で進行方向を表示しても良い。進行方向を示す形態は、三角形等の図形でも良い。
【0036】
以上説明した実施形態によると、目標位置と動作位置とが比較可能に画面表示されるため、教示者は、力制御モードにおける目標位置と動作位置とを検証しながらコマンドのパラメーター(目標位置と目標力)を設定することができる。したがって、コマンドのパラメーターを最適化する作業が容易になり、教示に要する時間を短縮することができる。そして目標位置と動作位置とが同一座標系に表示されることで、目標位置と動作位置との比較はさらに容易になる。また目標位置と動作位置の軌跡を表示したり、目標位置と動作位置を動画表示したり、目標位置と動作位置の変化方向を表示したり、目標位置と動作位置の表示区間を設定できるため、目標位置と動作位置との比較はさらに容易である。
【0037】
(2)他の実施形態
上記実施形態では、制御装置3の制御部32が力由来補正量ΔSを導出し、力由来補正量ΔSと目標位置S
tとに基づいて教示端末4の表示制御部41が動作位置を導出する例を説明したが、教示端末4が作用力f
sと目標力f
stとに基づいて力由来補正量ΔSを導出しても良いし、また、教示端末4は、動作位置を導出する代わりに、制御装置3の制御部32から取得しても良い。
【0038】
また上記実施形態では、力由来補正量ΔSを含む動作位置と目標位置との軌跡表示を示したが、目標位置とエンコーダーの検出値が示す位置とを示してもよい。また本発明は、ロボット1を教示する段階で教示端末4に目標位置と動作位置とを表示する形態に限らず、教示後にロボット1が稼働している段階に教示端末4やティーチングペンダント5や制御装置3の画面に目標位置と動作位置とを表示する形態にも適用できる。
【0039】
また上記実施形態では、目標位置と動作位置とを2成分ずつ二次元座標系に表示したが、目標位置と動作位置とを1成分ずつ時間の関数として表示しても良い。例えば、上述した目標位置と動作位置のX成分をxモニター131と同様に時間の関数として表示しても良い。また、目標位置のXYZ成分と動作位置のXYZ成分とを3次元コンピューターグラフィックスで表示しても良い。
【0040】
また上記実施形態では、6軸単腕ロボットの制御装置に本発明を適用した例を説明したが、本発明が5軸以下のロボットに適用できることも、7軸以上のロボットに適用できることも、双腕ロボットに適用できることも、いうまでもない。