WO2001038048A1

WO2001038048A1 - Unite de commande de robot

Info

Publication number: WO2001038048A1
Application number: PCT/JP2000/008463
Authority: WO
Inventors: Hideo Nagata; Yasuyuki Inoue; Ken'ichi Yasuda; Toshiyuki Kurebayashi
Original assignee: Kabushiki Kaisha Yaskawa Denki
Priority date: 1999-11-29
Filing date: 2000-11-29
Publication date: 2001-05-31
Also published as: US20030025473A1; CN1283426C; US6919701B2; EP1234641A4; JP3473834B2; JP2001157975A; CN1433352A; EP1234641A1; KR100629469B1; KR20020067690A

Description

明細書

ロボットの制御装置

[技術分野]

本発明は、ロボット等の制御方式に関し、ロボットの手先先端に取り付けたッールの姿勢を一定に保ちながら、並進の柔軟動作が可能なロボットの制御装置、及び作業座標系での柔軟な特性を別途付加することができるロボットの制御装置に関するものである。

[背景技術]

従来のロボットは、図 1 3に示すような位置速度制御系にて制御が行われていた。すなわち、位置指令と位置検出器で検出された位置フィードバック信号との偏差に位置ゲイン Kp を乗じた速度指令と位置フィードバック信号を微分して得た速度フィードバック信号との偏差に速度ゲイン Κν を乗じて比例積分し、アンプで増幅して関節モータを駆動するという制御系が用いられていた。

このような制御系でワークとの接触を伴うような作業を行う際、ワークの位置ずれなどがあると、剛性を高くするために大きく設定されたゲイン Kp 、 Kv や積分器の作用により、大きなトルクを発生することになり、作業の遂行が困難になる。

例として、ダイキャスト成型品であるワークを、ロボットのツールで把持してダイキャストマシンから取り出す作業に基づいて説明する。

従来のロボットでは、油圧シリンダでワークがダイキャストマシンから強制的に押し出される際に、高ゲインの制御系のロボットでは大きなトルクが発生するためにワークを把持したまま取り出すことができない。このような作業を行う口ボットには、適切な柔軟性があるコンプライアンス動作が必要となる。

また、このようなダイキャストハンドリング作業を行うロボットには、作業座標系で適切な並進の柔軟動作が必要となる,：これは主に、外部機器からの外力が直線的に作用したり、接触する対象物の形状が直線や平面で構成されているためである。

このような問題に対して、例えば特開昭 5 8 - 4 5 8 9 1号公報に開示されている先行技術では、作用力を吸収するために専用のコンプライアンスツールを利用する方法がある。他に特開昭 6 3 - 1 3 9 6 7 8号公報に開示されている先行技術では、力センサを用いて作用力を検出して、その方向に力制御を行う方法がある。

また、ロボッ卜に特別な装置を付加することなく柔軟な制御を行う方式として、特開平 6— 3 3 2 5 3 8号公報に開示されている先行技術では、関節座標系で柔らかさの設定が可能な制御方式が開示されている。

また、近年、作業座標系で柔軟な制御を行う方式として、特開平 8— 2 2 7 3 2 0号公報に開示されている先行技術では、作業座標系で柔らかさの設定が可能な制御方式が開示されている。

さらに、特開平 8 — 1 5 5 8 6 8号公報に開示されている先行技術では、ツール座標系で指定したベクトル成分の現在位置を指令値に逐次変換する方式が開示されている。

ところが、上記の先行技術においては、次のような問題があった。

特開昭 5 8 - 4 5 8 9 1号公報に記載された方式では、各種作業に

合わせて専用のツールを作る必要があり、専用ツールの重量分だけロボットの可搬重量が減少してしまう。

また、特開昭 6 3 _ 1 3 9 6 7 8号公報に記載された方式では、力センサが必要であり、力センサを負荷することでシステムが複雑になる。また、力センサの機械的強度はロボットと比較して低く、力センサの許容トルクを越えて外力が作用すると力センサの破損を招く危険性がある。

また、特開平 6— 3 3 2 5 3 8号公報に記載された方式では、関節座標系における柔軟性を制御する方式を開示しているが、ゲイン低減により各関節軸が独立に柔軟に動作するため、総合された柔軟な方向や柔らかさがロボットの教示作業者に分かりづらい。また、ロボットが外部機器により押されるなどして姿-勢が変化した場合に、作業座標系における並進の柔軟な方向も変化していくためツール姿勢を一定に保つような動作ができない。そのため、ワークに捻りなどの予期せぬ力が作用して、ダイキャスト成型品などの複雑な形状で低剛性なワークでは破損するなどの問題がある。更に、関節軸毎に円弧状に柔軟になるため、それらの柔軟動作を合成しても作業座標系での並進の柔軟動作を完全に行うことができなレ、。

また、特開平 8— 2 2 7 3 2 0号公報に記載された方式では、作業座標系における柔軟性を制御する方式を開示しているが、関節座標系の変位と作業座標系の変位を対応させることによりゲインを求める必要があるため、演算関係式が非常に煩雑となり C P Uに対する演算負荷が大きく、ロボットの姿勢の変化に対して連続的にゲインを求めることができない。特に、特異点近傍など、関節角と作業座標間で変位の対応関係の変化率が大きなロボット姿勢では C P Uの演算負荷が大きくなる。また、ロボットの姿勢変化に対して実時間での演算が行えず、連続的なゲインの演算が困難なためロボットの柔らかさがロボットの姿勢により大幅に異なるなどの問題点を有している。

さらに、特開平 8— 1 5 5 8 6 8号公報に記載された方式では、外力により位置変化した各軸の角度を実時間で取り込み、指定したツール座標系での位置/姿勢の現在位置を演算し（順変換）、指定したベクトル成分の現在位置のみを指令値に変換している（逆変換）。しかし、ロボット全軸についての順変換と更新された指令値の逆変換が実時間で必要であるため、演算量が多く、処理時間が大幅 ίこ力力る。

[発明の開示]

そこで、本発明の課題は簡単な処理で作業座標系におけるツール姿勢を一定に保って、並進の柔軟動作を行うロボットの制御装置を提供することである。前記課題を解決するための第 1 の手段は、関節を駆動するモータを、位置及び速度の状態フィードバックル一プを持つ制御回路で制御するロボッ卜の制御装置において、

ロボッ卜の関節角度を計測する手段と、ロボッ卜の手先先端に取り付けたッールの初期姿勢を記憶する手段と、

特定の関節軸の位置ゲイン又は速度ゲインを他の関節軸の位置ゲイン又は速度ゲインより小さく設定する手段と、

前記関節角度とツールの初期姿勢からロボットべ一スに対するツール姿勢を一定に保つように位置又は速度指令への修正量を演算する手段とを有するものである。

また、前記課題を解決するための第 2の手段は、関節を駆動するモータを、位置及び速度の状態フィ一ドバックループを持つ制御回路で制御するロボットの制御装置において、ロボットの関節角度を計測する手段と、

ロボットの手先先端に取り付けたツールの初期姿勢を記憶する手段と、前記関節角度に基づいて座標系間の静力学対応関係を演算する手段と、

前記関節角度とロボットの作業座標系での位置指令に基づいて前記制御回路とは別の第 2の位置，速度の状態フィードバック制御を行う手段と、

前記第 2のフィードバック制御系における特定の軸の位置ゲイン又は速度ゲインを他の関節軸の位置ゲイン又は速度ゲインより小さく設定する手段と、前記静力学対応関係を用レ、ることにより前記第 2のフィードバック制御系の出力値を関節角トルク値に変換する手段と、

前記第 2のフィードバック制御系の関節角トルク値を、前記第 1のフィ一ドバック制御系の出力値に加算する手段と、

前記関節角度と前記ツールの初期姿勢からロボットベースに対するツール姿勢を一定に保つように位置又は速度指令への修正量を演算する手段とを有するものである。

さらに前記課題を解決するための第 3の手段は、ロボッ卜の手首の位置と姿勢を制御するモータの制御装置において、

前記手首の位置の制御を作業座標の制御系で行う手段と、前記手首の姿勢の制御を関節座標の制御系で行う手段とを有するものである。

この制御装置を具体化する第 4の手段としては、前記作業座標の制御系は、ロボッ卜の関節角度を計測する手段と、

前記関節角度に基づいて座標系間の静力学対応関係を演算する手段と、前記関節角度とロボッ卜の作業座標系での位置指令に基づいて作業座標系において位置と速度の状態フィ一ドバック制御を行う手段と、前記静力学対応関係を用いることにより前記フィードバック制御系の出力値を

関節座標系の関節角トルク指令に変換する手段を有するものである。

また、第 5の手段としては、前記作業座標の制御系は、

ロボッ卜の関節角度を計測する手段と、前記関節角度に基づいて座標系間の微小変位の対応関係を演算する手段と、

前記関節角度とロボットの作業座標系での位置指令に基づいて作業座標系において位置の状態フィ一ドバック制御を行う手段と、前記微小変位の対応関係を用レ、ることにより前記位置のフィ一ドバック制御系の出力値を関節座標系の速度指令に変換する手段と、

前記速度指令と前記関節角度に基づいて関節座標系において速度の状態フィードバック制御を行う手段を有するものである。

[図面の簡単な説明]

図 1は本発明の第 1の基本構成を示すプロック図であり、図 2は本発明の第 1 の具体的実施例を示すプロック図であり、図 3は本発明の第 1の作用を示すスケルトン図である。

図 4は本発明の第 2の手段の基本構成を示すプロック図であり、図 5は本発明の第 2の具体的実施例を示すプロック図であり、図 6は本発明の第 2の作用を示すスケルトン図である。

図 7は本発明のアプリケーション適用例を示す説明図であり、図 8は本発明の第 3の手段の基本構成を示すプロック図であり、図 9は本発明の第 3の基本構成を示すブロック図であり、図 1 0は本発明の第 3の具体的実施例を示すプロック図である。

図 1 1は本発明の第 4の手段の基本構成を示すブロック図であり、図 1 2は本発明の第 4の具体的実施例を示すブロック図である。

図 1 3は従来の制御方式を示すプロック図である。

[発明を実施するための最良の形態] ·

以下、本発明の実施の形態について説明する。

本発明の第 1の実施の形態は、図 1に示すように、関節を駆動するモータを、位置及び速度の状態フィ一ドバックループを持つ制御回路 4 0で制御する口ボッ卜の制御装置において、特定の関節軸 J _m の位置ゲイン又は速度ゲインを、ゲイン低減手段 1 0により、他の軸 J i ， j , J · · · J„ の位置ゲイン又は速度ゲインより小さく設定するかトルク制限を行う。ロボットが外部機器に押されるなどして特定の関節軸が柔軟に動作する場合に、予めツール初期姿勢記憶手段

1 1に記憶されたツールの初期姿勢を保つように逆変換を行い、修正量演算手段

1 2により修正量を演算して指令を修正する。

また、図 4に示すように、ロボットの関節軸への角度指令と計測された関節角度から、関節座標系の制御回路とは別に作業座標系における第 2の位置，速度の状態フィードバックル一プ 1 3を構成する。この第 2の位置，速度の制御系における特定の軸の位置ゲイン又は速度ゲインを、ゲイン低减手段 1 0により、他の軸の位置ゲイン又は速度ゲインより小さく設定するか力制限及びトルク制限を行う。計測された関節角度に基づいて、静力学対応関係式演算手段 1 4により座標系間の静力学対応関係式を演算して、第 2の制御系の出力を関節角トルクに変換して第 1の制御系に加算する。ロボットが外部機器に押されるなどして作業座標系に沿って特定の軸方向又は軸周りに柔軟に動作する場合に、予め記憶されたッールの初期姿勢を保つように逆変換を行い、修正量を演算して指令を修正する。

さらに、前記第 2の課題を解決する手段では、図 8に示すように、ロボットの手首の位置と姿勢を制御するモータの制御装置において、ロボットの各軸の制御を、手首の位置を制御する作業座標の制御系と手首の姿勢を制御する関節座標の制御系に分ける。作業座標系の構成手段としては以下の 2種類がある。

第 1の手段は、ロボットの関節角度とロボットの作業座標系での位置指令に基づいて作業座標系において位置と速度の状態フィードバック制御を構成する。このフィードバック制御系の出力値は、関節角度から演算された座標系間の静力学対応関係を用いて、関節座標系の関節角トルク指令に変換することができる。第 2の手段は、ロボットの関節角度とロボットの作業座標系での位置指令に基づいて作業座標系において位置の状態フィードバック制御を構成する。このフィ一ドバック制御系の出力値は、関節角度から演算された座標系間の微小変位の対応関係を用いて、関節座標系の関節角速度指令に変換することができる。この関節角速度指令と関節角度に基づいて、関節座標系において速度の状態フィードバック制御を構成する。

それぞれの作業座標系での状態フィードバック制御において、ゲインを低减するか、各制御系の出力を制限することにより、作業座標系での並進の柔軟動作を実現することができる。

以下、本発明の実施例を説明する。

図 2は第 1の具体的実施例を示すもので、通常の関節座標系での位置速度制御系に本発明の制御を適用した制御プロック線図を表している。関節座標系での位置速度制御の内部ループは通常比例積分制御が行われるが、定常的に作用する力は静的補償要素により補償されるものとする。通常の位置速度制御状態では、高ゲインの位置制御ループ及び速度制御ループの作用により、外部より作用する力に対して各関節軸が柔軟に変位しづらい。ここで、位置ループゲイン及び速度ループゲインを低减することで、外力に対して各関節軸が柔軟に変位可能である。しかし、ロボットの各関節が個々に不規則に柔軟動作してしまうと、柔軟な方向を任意に選択できない。そのため、ロボットの手先先端に付いたツールの姿勢を一定に保つ並進の柔軟動作は困難になる。そこで、本発明の制御は、各関節の制御系を従来の高剛性な位置速度ループと低剛性な位置速度ループとに分け、低剛性な関節軸で計測された関節角度とツールの初期姿勢からロボットベースに対するツール姿勢を一定に保つように、高剛性な関節軸の制御系への角度修正量を実時間で演算するものである。つまり、柔軟な軸の変位量に応じてその他の軸を変位させることで、ツール初期姿勢を保つたままで、並進の柔軟動作が可能となる。以下に各関節の位置速度制御ループの構成を述べる。

ロボットの関節の自由度は n個あり、 m番目の軸 J _m ( 1≤m≤ n ) の制御系を低剛性に設定する。ここで、低剛性にする軸を 2軸以上に設定しても何ら問題ない。低剛性にする手段として、ゲイン低減手段 1 0によって、 J _m 軸の制御系の位置ループゲイン及び速度ループゲインを他の軸の位置ゲイン又は速度ゲインよりも低減する。もしくは、制御系の後端にトルクリミットを設けて、トルク指令自体を小さく絞っても良い。このように制御系を低剛性にする場合、重力などの静的負荷は別途補償しておく。

次に、ロボットが実際の作業で使用する作業座標系でのツール姿勢角度 R _X ， R Y ， R _Z をツール初期姿勢記憶手段 1 1に記憶又は設定する。これは、実際に柔軟動作に行う直前のツール姿勢角度をツール初期姿勢として記憶するか、作業内容が予め分かっている場合にはその姿勢角度を数値入力しても良い。

実際の作業において、ロボットの手先先端に取り付けられたツールに外力が作用すると、柔軟な軸である J _m 軸のみが外力に倣うように変位する。その他の軸は高いループゲインにより制御系が高剛性であるため変位しない。ここで、関節角度計測手段を用いて、 J _m 軸の角度 8の値0 1 ^ を計測してぃく（F B : フイードバックとする）。計測された J _m 軸の角度 F B値 Θ fb_m 、作業実行時に記憶又は設定されたツール姿勢角度 R x ， R Y , R z ) 及び J 〜J _n 軸の角度指令 Θ ΓΘ^〜 0refn_n Θ fb_m を除く）を用いて、 J _m 軸以外の J丄 - J„ 軸への角度指令修正量 Δ θ ΓΘ^〜Δ Θ refn_n 求める。このとき、〜 J _N 軸（J _M を除く）は高剛性で角度指令に対する角度 F Bはほぼ同じ値を取るため、角度指令の代わりに角度 F B Θ fbi 〜0 fb_n を用いても良い。

修正量演算手段 1 2により角度指令修正量 Δ Θ ^〜Δ eref „ 求める具体的方法としては、一般に逆変換又は逆運動学と言われている変換式を用いる。これは通常、ロボット手先先端のツール位置と姿勢から、各関節の角度を求める変換式である。この変換式については、多くの文献（例えば、「ロボット制御基礎論」：吉川恒夫著）に記載されているため、ここでは割愛する。

角度指令修正量 Δ Θ refi〜A Θ ref _n を求める手段を以下に順を追って示す。

( 1 ) 角度指令 0 〜 Sref „ と角度 F B 6fb_m により、一般的に順変換又は順運動学と呼ばれる変換式を用いて、作業座標系における手首軸の中心点である点（以降は P点と呼ぶ）の位置を求める。

( 2) 求められた P点の位置 Τχ , Τ_Υ , Τζ と作業実行前に記憶されたツール姿勢角度 R_x , RY , Rz から逆変換式を用いて、 J 〜J _n 軸の新たな角度指

Θ ref!~ Θ ref „ を求める。

( 3) 現在の角度指令 e re eref _n と差分することで角度指令修正量

△ ere A eref _n を求め、角度指令修正量を J i 〜J _n 軸の角度指令に加算する。

この演算を実時間で求めることで、 J _m 軸が外力によって移動しても、ロボットベースに対して初期のツール姿勢を保つように J_m 軸以外の軸が動作する。図 3に 6自由度のロボットに本制御方法を適用した例を示して説明する。柔軟に設定する軸は J 3 軸として、その他の J i ， J 2 , J 4 , J 5 , J 6 軸は高いループゲインにして制御系を高剛性にする。外部からの力がロボット手先先端に付けられたツールに鉛直下向きに作用すると仮定する。

ツールに鉛直下向きの力が加わると、唯一柔軟な J ₃ 軸が下方向（紙面上で時計回り）に変位する。それ以外の軸は制御系が高剛性であるため変位しない。よつて、ツールは点線で描かれた姿勢を取り、結果としてツールの姿勢が下に向いて変化してしまう。そこで、以下の演算を行うことで J 軸の姿勢を修正して、ツール姿勢を一定に保ったまま、ほぼ並進の柔軟動作を行うことができる。

( 1 ) 位置指令 Θ 〜 Θ ref _n と位置 F B 0 fb₃ により、手首軸中心の P点についての順変換式を演算して、作業座標系における P点の位置 Τ_χ , Τν , Τζ を求める。

(2) 求められた Ρ点の位置 Τ_χ ， Τ_Υ ， Τζ と作業実行前に記憶されたツール姿勢角度 R_x , R_Y ， R_z から逆変換式を用いて、 J ₅ 軸の新たな角度指令 6 ref₅を求める。初めに、ツール角度姿勢 Rx ， R_Y ， R_z から、（3， 3) の回転行列 ROTを求める。 XYZ固定角などの表現を用いると、以下のように表すことができる。

( 1 ) と置く。ここで、 c R= c o s R， s R= s i n Rである。

次に逆正接により、新たな角度指令 Θ ref₅を求める。

Θ ref₅ = tan 一¹ ( (- c Θ ref！ s θ a c Θ ref₄+ s Θ ref _t s Θ refj A_x — （一 s Θ ref i s Θ a c Θ ref + c Θ ref 1 s Θ ref ) A _Y + c Θ a c Θ ref ₄ A _z ) / ( c Θ a ( c Θ ref ! Ax + s Θ ref _x A_Y ) + s θ a A_z ) } · · . ( 2) ここで、 Θ c は J ₃ 〜 J ₅ までのリンクの水平面に対する角度で、 J 2 と J 3 力求めることができる。

c Θ ref _n = c o s Θ ref _n . s Θ ref „ = s i n Θ ref _n である。

(3 ) 現在の角度指令 Θ ref ₅と差分することで、角度指令修正量 Δ Θ ref ₅を求めることができ、この角度指令修正量を J 軸の角度指令に加算する。

ここで、 P点の変位量が J からツールの外力作用点までの距離に対して十分に小さい場合は、並進の柔軟制御と考えることができるが、より長いストロークを望む場合には次に述べる方法を取る。

本発明の第 2の具体的実施例を図 4に示して説明する。

制御方法としては第 1の具体的実施例と同様であるが、ツールの並進の柔軟動作を確実に行うことを目的にしている。ツールの並進動作を行うためには手首中心の P点が並進動作すれば良い。作業座標系で P点の並進動作を行うため、通常の関節座標系位置速度ループの他に、作業座標系位置速度ループ 1 3を併用する方法を取る。

本実施例では、 J , 〜 J _m 軸については、作業座標系位置速度ループ 1 3を構成して、この作業座標系位置速度ループ 1 3で柔軟な動作を行うことで、確実な並進の柔軟動作が可能になる。 J _{m + 1} 〜 J _n 軸については、従来通りの関節座標系位置速度ループで制御を行う。

直交 3軸と回転 3軸についての作業座標系位置速度ループ 1 3の構成方法について以下に述べる。

関節座標系における位置速度制御ループ上の角度指令と角度 F Bの情報を、一般的に順変換又は順運動学と呼ばれる関節変位とロボットの作業位置の関係式を用い（ブロック 1 5 ) 、作業座標系での制御点についての位置指令と位置 F Bを求める。これらの位置指令と位置 F Bに基づいて、作業座標系の X Y Z軸方向と X Y Z軸周りに位置速度制御ループ 1 3を構成させる。ここで、この作業座標系の位置速度制御ループ 1 3の出力値は作業座標系での X Y Z軸方向への力指令と X Y Z軸周りのトルク指令である。作業座標系の位置速度制御ループ 1 3はゲイン低减手段 1 0によって、 X Y Z軸の何れかの制御系の位置ループゲイン及び速度ループゲインを他の軸の位置ゲイン又は速度ゲインよりも低减する。もしくは、制御系の後端にリミットを設けて、力指令又はトルク指令自体を小さく絞って ¾反レ、。

よって、外部より作用する力に対して、作業座標系上である特定な軸方向にのみ P点の並進の柔軟動作をする場合は、その制御系のゲインを低減するかリミットによって力を制限する。これにより、特定の軸方向に対しては位置偏差を許容 1 し、それ以外の軸方向及び軸周りへは位置偏差を許容しないようにすることが可能である。

次に、ロボットの現在の状態から一般的にヤコビアンと呼ばれる行列式の転置行列である関節座標系と作業座標系の静力学対応関係式をブロック 1 4で求め、作業座標系の力指令及びトルク指令から関節座標系におけるトルク指令を算出することが可能である。

例えば 3自由度のロボットでヤコビアンの転置行列の算出式は下記の式で表される。

(3) ここで、

J ：ヤコビアン

O_{s i} ：第 i 関節座標の回転方向べクトル（ロボットのベース座標系を基準）

Op, ：第 i 関節位置べクトル（ロボットのベース座標基準）

X ：べクトルのタト積を示す

r ：ロボッ卜の作業位置べクトル

T ：行列の転置を表す

従って、作業座標系の出力値を

ここで、

F ：力べクトル

Fx , F_Y , F_z ：作業座標系での力

関節座標系に変換した関節角トルク値を

τ = [ て丄，て ₂ ，て ₃ ] ^τ · · · (5)

ここで、

r ：関節座標系でのトルクべクトルて i ：第 i軸の関節座標系のトルク

とおくと、関節座標系でのトルクは以下の関係より求めることができる。

τ = J ^T F · · · ( 6 )

よって、ロボットの姿勢の変化に対して（3 ) ， ( 6 ) 式の演算を行い、関節制御系におけるトルク指令に、（5 ) 式に示されるトルク指令を加算することで、ロボッ卜の全動作領域で作業座標上における並進の柔軟動作が可能なロボットの柔軟制御系を構成することができる。

次に、図 5に実際のロボットに適用した制御系の構成を示して、各要素の働きを説明する。ロボットの関節の自由度を 6として、 J i 〜J ₃ 軸と J 4 〜J ₆ 軸で動作させる制御方法を分ける。作業座標系における手首軸中心の P点の位置を制御する J 〜 J ₃ 軸では、作業座標系の位置速度ループ 1 3を構成する。作業座標系における手首軸中心の P点に対するツールの姿勢を制御する J 4 〜 J ₆ 軸では、関節座標系の位置速度ループを構成する。このように、位置と姿勢の制御を分けることで、並進の柔軟動作を位置の制御で実行し、ツールの姿勢変化の修正を姿勢の制御で実行することができる。

( 1 ) 作業座標系の位置速度ループを構成するため、〜 J ₃ 軸の角度指令 θ ref\〜 Θ ref₃と角度 F Β Θ fb i 〜 Θ fb ₃ をそれぞれ順変換式 1 5を用い、作業座標系での P点についての位置指令及び位置 F Bを求める。

( 2 ) 作業座標系での位置指令と位置 F Bに基づいて、作業座標系の X Y Z軸方向に位置速度制御ループを構成する。ここで、作業座標系の位置速度制御ループの出力値は作業座標系での X Y Z軸方向への力指令となる。作業座標系の位置速度制御ループはゲイン低減手段 1 0によって、 X Y Zの何れかの軸に関して制御系の位置ループゲイン及び速度ループゲインを他の軸の位置ゲイン又は速度ゲインよりも低減する。もしくは、制御系の後端にリミットを設けて、力指令自体を小さく絞っても良い。

ここで、作業座標系の X軸方向に P点の並進の柔軟動作を行うと仮定する。作業座標系の X軸制御系の位置ループゲイン及び速度ループゲインをゲイン低減手段 1 0によって他の軸の位置ゲイン又は速度ゲインよりも低減する。これにより、 P点の位置は外力によって X軸方向には柔軟に動作することができるが、 Y Z 軸方向へは作業座標系の位置速度ループが高剛性であるため、偏差が生じないように力を発生させて動作が制限されるようになる。

(3) 次に、ブロック 1 4によりロボットの角度 F B Θ fb! 〜0 fb₃ から関節座標系と作業座標系の静力学対応関係式を求め、その転置行列式を用いることで、作業座標系の力指令から関節座標系におけるトルク指令を算出する。

(4) このトルク指令を J i 〜 J ₃ 軸の関節座標系のトルク指令に加算することで

、作業座標系における P点の X軸方向への並進の柔軟動作と YZ軸方向への高剛性な動作が可能となる。このとき、 J 〜 J ₃ 軸の関節座標系の位置速度ループ 1 3からのトルク指令を零にしておくことで、トルク指令の干渉を防ぐことができる。重力などの静的負荷は別途補償しておく。，

実際にロボットの手先先端のツールに外力が作用した場合の演算方法を以下に示す。 P点の位置が X軸方向に柔軟動作することによって、 J 〜 J ₃ 軸の角度がそれ

ぞれ変化していく。これらの軸の角度 F B Θ fin 〜0 fb₃ と手首軸 J ₄ 〜 J ₆ 軸の角度 F B 0 fb₄ 〜 0 fb₆ を関節角度計測手段により計測する。

外力が作用する前に記憶されたツールの初期姿勢角度 R_x , RY ， Rz と角度 F B Θ f b！〜 0 fb₃ から、逆変換式を用いることで、ツール姿勢が初期姿勢を保つような手首軸の角度指令 6ref₄〜 0ref₆を新規に求めることができる。この新しい角度指令 Θ ref₄〜 6ref₆と現在の手首の角度 F B Θ fb., 〜 6 fb₆ (もしくは角度指令 e ref₄〜 eref_e) から、差分を取ることによって、角度指令修正量

Δ Θ ref₄〜A Θ ref₆を求めることができる。この角度指令修正量 Δ Θ ref₄〜

Δ Θ ref₆を角度指令 Θ ref₄〜 Θ ref₆に加算する。

J ₅ 軸の角度指令 eref₅に関しては式（ 1 ) で求めることができる。 J 4 軸と J ₆ 軸の角度指令 6 ref₄と 6ref_eに関しては以下の式で求めることができる。

Θ ref₄= t a n"¹ { ( s Θ ref ! Α_χ — c Θ ref ! A_Y )

/ ( c θ a A_z - s Θ a ( c Θ ref! A_x + s Θ ref _t A_Y ) ) } · · . ( 7 )

Θ ref 6 = t a n { ( ( c 0refi s Θ a s e ref₄+ s 6refi c 0ref₄) N_x + ( s Θ ref ! s Θ ひ s Θ ref 4 - c Θ ref ! c Θ ref₄) Ny - c θ a s Θ ref₄N_z )

/ ( ( c Θ ref i s Θ ref + s Θ ref i c Θ ref ₄) Ox + ( s Θ ref i s Θ a s Θ ref ₄ 一 c Θ ref ! c Θ ref ₄ ) O _y - c θ a s Θ ref ₄ 0 _z ) } · · · ( 8 ) 以上の演算量は非常に少ないため実時間で解くことが可能である。

図 6に 6自由度のロボットに本制御方法を適用した動作例を示して説明する。柔軟に設定する軸は作業座標系の Z軸として、その他の X Y軸及び手首軸の J ₄ 〜 J _e 軸は高いループゲインにして制御系を高剛性にする。外部からの力がロボット手先先端に付けられたツールに鉛直下向きに作用すると仮定する。手首軸の中心である P点は作業座標系の位置速度ループの働きによって、外力が作用することで鉛直方向（Z軸方向）にしか柔軟に動作しないため、図中の点線上を移動することになる。 P点が下方に移動することで、手首軸の角度（ここでは J ₅ 軸）は点線で示したようになる。しカゝし、 P点の移動に合わせて Δ J ₅ を演算して、 J ₅ 軸が変化することでロボットベースに対するツールの初期姿勢を保ったまま、 J ₅ 軸は実線に示したようになり、ツールは鉛直方向に平行移動する。つまり、ツールも図中の点線上を移動することになる。このときに手首軸に関しては、初期のツール姿勢を教示するのみで、 P点の並進動作に合わせてッ —ル姿勢が一定になるようにロボットを動作させることが可能になる。

例えば、本発明をダイキャストハンドリングに適応した場合を図 7に示して説明する。ダイキャストハンドリングは、ダイキャスト成型品であるワーク 2 3をロボット 2 1のツール 2 2で把持してダイキャストマシンから取り出す作業である。ロボット 2 1はハンド 2 4でワーク 2 3を把持したまま、ピストン 2 5によつて铸型 2 6から押される力 Fを作業座標系の Y軸に沿った方向 aに設定し、口ボット 2 1は方向 aに沿って移動しなければならない。このときに方向 bに少しでも動いてしまうと、捻りの力がワーク 2 3に作用して铸型 2 6の中で変形したり破損したりしてしまう。従来では、ハンド 2 4部分にスライダなどのメカ的な機構を付けて力を吸収していたためロボットの可搬重量の減少となり、スライダの動作方向に外力の方向を正確に教示しなければならないなどの手間がかかる。本実施例では、作業座標系の位置速度ループを方向 aに沿って構成し、 Y軸方向を柔軟に設定（ゲイン低減か力制限）することで、 P点が方向 a上のみを柔軟に動作するようになる。ロボット 2 1の P点がビストン 2 5により押し出されるが、作業座標系におけるハンド 2 4の姿勢を一定に保つように手首部分への修正量が演算されるため、ワーク 2 3は铸型 2 6から真っ直ぐに押し出される。このとき、関節軸単位でなく、作業座標系でロボットの柔軟な方向や柔らかさが設定できるため、教示作業が非常に簡単に行える。

次に、本発明の第 3の具体的実施例を図 9に示して説明する。

図 9は通常の関節座標系での位置速度制御系に本発明の制御を適用した制御ブ口ック線図を表している。

前述の実施例では、 P点の位置の制御系を作業座標系の位置速度ループと関節座標系の位置速度ループを併用していた。

そこで、本実施例の制御は、ロボットの各関節の制御系を手首軸の中心点の位置（以下では P点）を制御する直交座標系の位置速度制御系 3 0とツールの姿勢を制御する関節座標系の位置速度制御系 4 0に完全に分け、直交座標系の位置速度制御の特定の軸を柔軟にすることで、作業座標系における並進の柔軟動作を可能にする。回転を除いた並進のみに絞って直交座標系だけで位置速度ループを構成することで、演算量を半減することができる。

以下に本実施例における各関節の位置速度制御ループの構成を述べる。ロボットの関節の自由度を n個とする。その内、ロボットのベースから m番目までの軸（以下では基本軸）の制御系を直交座標系の位置速度ループで構成する。また、 m + l〜n番目の軸（以下では手首軸）の制御系を関節座標系の位置速度ループ 4 1で構成する。直交座標系の構成手段は、基本軸の各関節の角度検出値より、一般的に順変換又は順運動学と呼ばれる関節変位とロボットの作業位置の関係式を用い、作業座標系での手首中心点（以下では P点）についての位置 F Bを求める。この位置 F Bと作業座標系での P点の位置指令とに基づいて、作業座標系の X Y Z軸方向に位置速度制御ループを構成させる。ここで、作業座標系の位置速度制御ループの出力値は作業座標系での X Y Z軸方向への力指令になる。作業座標系の位置速度制御ループはゲイン低減手段 3 2によって、 X Y Z軸の何れかの制御系の位置ループゲイン及び速度ループゲインを他の軸の位置ゲイン又は速度ゲインよりも低減する。もしくは、図示しないが、速度制御系の後端にリミットを設けて、力指令を小さく絞っても良い。このように制御系を低剛性にする場合、重力などの静的負荷は別途補償しておく。

次に、ロボットの現在の状態から一般的にヤコビアンと呼ばれる行列の転置行列である関節座標系と作業座標系の静力学対応関係式を求め、作業座標系の力指令から関節座標系におけるトルク指令を算出することが可能である。これには、前述の（3) 式〜（6) 式を用い、ロボットの姿勢の変化に対して（3) ， ( 6) 式の演算を行い、関節制御系におけるトルク指令に、（5) 式に示されるトルク指令を加算することで、ロボットの全動作領域で作業座標上における並進の柔軟動作が可能なロボットの柔軟制御系を構成することができる。

手首軸の関節座標系の位置制御ループでは、通常の位置ループゲイン，速度ル

—プゲインで高剛性に位置制御を行う力、位置ループゲイン，速度ループゲインを他の軸の位置ゲイン又は速度ゲインよりも小さくすることで低剛性に柔軟制御を行う。また、低剛性の手段としては、図示しないがゲイン低減の代わりに速度制御器の後段にリミットを設けても良い。

次に、図 1 0に実際のロボットに適用した制御系の構成を示して、各要素の働きを説明する。

口ボットの関節の自由度を 6として、 J i 〜J ₃ 軸と J ₄ 〜J ₆ 軸で動作させる制御方法を分ける。作業座標系における P点の位置を制御する J i 〜 J ₃ 軸では、作業座標系 30の位置速度ループ 3 1を構成する。作業座標系におけるツールの姿勢を制御する J ₄ 〜 J ₆ 軸では、関節座標系 40の位置速度ループ 4 1を構成する。このように、位置と姿勢の制御を分けることで、並進の柔軟動作を位置の制御で実行し、ツールの姿勢変化の修正を姿勢の制御で実行することができる。

( 1 ) 作業座標系の位置速度ループ 4 1を構成するため、 J i 〜J ₃ 軸の角度検出値 Θ Α^ 〜6fb₃ から順変換式を用い、作業座標系での P点についての位置（角度） F B (検出値）を求める。

(2) 作業座標系での位置指令と位置 F Bに基づいて、作業座標系の XYZ軸方向に位置速度制御ループ 3 1を構成する。ここで、作業座標系の位置速度制御ル —プ 3 1の出力値は作業座標系での XY Z軸方向への力指令 [F_x ， F_Y ， F_z

] となる。作業座標系の位置速度制御ループ 3 1はゲイン低減手段 3 2によって、 X Y Zの何れかの軸に関して制御系の位置ループゲイン及び速度ループゲインを他の軸の位置ゲイン又は速度ゲインよりも低減する。もしくは、速度制御系の後端にリミットを設けて、力指令自体を小さく絞る。

(3) 次に、ロボットの角度 F B Θ fbi 〜6 fb₃ から関節座標系と作業座標系の静力学対応関係式を求め、その転置行列式を用いることで、 X Y Z軸の作業座標系の力指令から J i 〜 J ₃ 軸の関節座標系におけるトルク指令を算出する。

( 4 ) このトルク指令を用いて各関節のモータを制御することで、作業座標系における P点の並進の柔軟動作が可能となる。このとき、重力などの静的負荷は別途補償しておく。

次に、 6自由度のロボットに本制御方法を適用した動作例を前記の図 6により説明する。

作業座標系の Z軸方向に P点の並進の柔軟動作を行うと仮定する。作業座標系の Z軸制御系の位置ループゲイン及び速度ループゲインをゲイン低減手段によつて他の軸の位置ゲイン又は速度ゲインよりも低減する。これにより、 P点の位置は外力によって Z軸方向には柔軟に動作することができるが、 X Y軸方向へは作業座標系の位置速度ループが高剛性に働くため、偏差が生じないように力を発生させて動作が制限されるようになる。

手首軸の中心である P点は作業座標系の位置速度ループの働きによって、外力が作用することで鉛直方向（Z軸方向）にしか柔軟に動作しないため、図中の点線上を移動することになる。 P点が下方に移動することで、手首軸の角度（ここでは J ₅ 軸）は点線で示したようになる。しカゝし、 P点の移動速度に合わせて Δ

J 5 を教示しておくことでロボットベースに対するツールの初期姿勢を保ったまま、ツールは鉛直方向に平行移動する。つまり、ツールも図中の点線上を移動することになる。

次に、本発明の第 4の具体的実施例を図 1 1に示して説明する。

初めに、本発明における各関節の位置速度制御ループの構成を述べる。

ロボットの関節の自由度を n個とするつその内、ロボットのベースから m番目までの軸 J t 〜J _m (以下では基本軸）の制御系 5 0を直交座標系の位置ループ 5 1 と関節座標系での速度ループ 5 2で構成する。また、 m + l〜n番目の軸 J _{m + 1} 〜 J _n (以下では手首軸）の制御系 6 0を関節座標系の位置速度ループ 6 1 で構成する。

直交座標系の構成手段は、第 3の具体的実施例と同様に、基本軸の各関節の角度検出値より作業座標系での P点についての位置（角度） F Bを求める。この位置 F Bと作業座標系での P点の位置指令とに基づいて、作業座標系の X Y Z軸方向に位置制御ループを 5 1を構成させる。ここで、作業座標系の位置制御ループ 5 1の出力値は作業座標系での XYZ軸方向への速度指令になる。作業座標系の位置制御ループ 51はゲイン低減手段 53によって、 XYZ軸の何れかの制御系の位置ループゲインを他の軸の位置ゲインよりも低減する。もしくは、図示しないが、位置制御系の後端にリミットを設けて、速度指令を小さく絞っても良い。このように制御系を低剛性にする場合、重力などの静的負荷は別途補償次に、ロボットの現在の状態から一般的にヤコビアンと呼ばれる行列式の逆行列である関節座標系と作業座標系の微小変位の対応関係式を求め、作業座標系の速度指令から関節座標系における速度指令を算出することが可能である。

例えば 3自由度のロボットでヤコビアンの逆行列の算出式は以下の式で表される。

• · · · (9) 作業座標系の速度指令を

v= [V_x , V_Y , V_z ] ^τ · · · (10)

ここで、

V ：速度指令べクトル

ν_χ , ν_Υ ， v_z ：作業座標系での速度指令

関節座標系に変換した関節角速度指令を

V = [V！ , v₂ , v₃ ] ^τ · · · (1 1)

ここで、

V ；関節座標系での速度指令べクトル

V i ：第 i軸の関節座標系の速度指令

とおくと、関節座標系での速度指令は以下の関係より求めることができる。

V = J -¹ V · · · (1 2)

よって、ロボットの姿勢の変化に対して（9) ，（1 2) 式の演算を行い、（ 1 1 ) 式に示される関節制御系における速度指令と、各関節の角度検出から求められた速度 F Bに基づいて、関節座標系の速度フィードバック制御を行い、関節座標系でのトルク指令を求める。手首軸の関節座標系の位置制御ループは、第 3の具体的実施例と同様にする。

次に、図 1 2に実際のロボットに適用した制御系の構成を示して、各要素の働きを説明する。

ロボットの関節の自由度を 6として、 J 〜 J ₃ 軸と J ₄ 〜 J _e 軸で動作させる制御方法を分ける。作業座標系における P点の位置を制御する J , 〜 J ₃ 軸では、作業座標系の位置ループ 5 1 と関節座標系での速度ループ 5 2を構成する。作業座標系におけるツールの姿勢を制御する J 〜 J s 軸では、関節座標系の位置速度ループ 6 1を構成する- このように、位置と姿勢の制御を分けることで、並進の柔軟動作を位置の制御で実行し、ツールの姿勢変化の修正を姿勢の制御で実行することができる。

( 1 ) 作業座標系の位置速度ループを構成するため、 J i 〜 J ₃ 軸の角度検出値 Θ fbi 〜 Sfb₃ から順変換式を用い、作業座標系での P点についての位置 F B ( 検出値）を求める。

(2) 作業座標系での位置指令と位置 FBに基づいて、作業座標系の XYZ軸方向に位置制御ループを構成する。ここで、作業座標系の位置制御ループの出力値は作業座標系での XYZ軸方向への速度指令 [V_x ， V_v ， V_z ] となる: 作業座標系の位置制御ループはゲイン低減手段 53によって、 XYZの何れかの軸に関して制御系の位置ループゲインを他の軸の位置ゲインよりも低減する。もしくは、位置制御系の後端にリミットを設けて、速度指令自体を小さく絞る。

(3) 次に、ロボットの角度 F B Θ fbi 〜 S fb₃ から関節座標系と作業座標系の微小変位の対応関係式を求め、その逆行列式を用いることで、 XYZ軸の作業座標系の速度指令から J i 〜 J ₃ 軸の関節座標系における速度指令を算出する。

(4) この速度指令と角度検出値を用いて、関節座標系 50での速度制御ループ 5 2を構成し、トルク指令を演算する。このトルク指令で各関節のモータを制御することで、作業座標系における P点の並進の柔軟動作が可能となる。このとき、重力などの静的負荷は別途補償しておく。以上の演算は、ロボットを構成している軸に対して、手首の位置について作業座標系を構成して並進の柔軟動作を行うようにしたため、演算量が少なく、実時間で解くことが可能である。

以上述べたように本発明によれば、下記の効果を奏する。

( 1 ) 各関節の制御系を従来の高剛性の位置速度ループと低剛性の位置速度ループとに分け、外力によって低剛性の関節軸で変位した関節角度と予め記憶されたツールの初期姿勢から、ロボットべ一スに対するツール姿勢を一定に保つように高剛性な関節軸への角度修正量を実時間で演算することで、専用ツールや力センサが不要で演算量も格段に少なくて良く、作業座標系でのツール姿勢を一定に保つた並進の柔軟制御を行うことができる。

( 2 ) 従来は必要であった力センサゃスライダなどのメカ的な機構が不要になり、ロボットの可搬重量も減少させることもなく、教示作業も楽に、作業座標系での並進の柔軟制御を非常に簡単に実現できる。

( 3 ) 更に、関節座標系と作業座標系でそれぞれ位置，速度の状態フィードバックループを組み、関節角の位置の情報を用レ、た関節座標系と作業座標系の諍力学対応関係式を用い、両制御系の出力を組み合わせることで、外部より作用する力に対して作業座標系座標系上である特定の方向にのみ先端の作業位置の動作を正確に制限でき、ロボットベースに対するツールの初期姿勢を保ったままで、並進の柔軟制御を非常に簡単な演算を解くことで容易に実現できる。

( 4 ) また、ロボットの各関節の制御系を手首の位置を制御する作業座標系でのフィ一ドバックループと手首の姿勢を制御する関節座標系でのフィ一ドバックル —ブに分け、作業座標系における特定の軸のゲインを低減すること、又はリミツトを設けることで、専用ッ一ルゃカセンサが不要で演算量も格段に少なくて良く、作業座標系での並進の柔軟制御を行うことができる。

[産業上の利用可能性]

本発明は、簡単な処理で作業座標系におけるツール姿勢を一定に保って、並進の柔軟動作を行うロボットの制御装置として有用である。

Claims

請求の範囲

1 . 関節を駆動するモータを、位置及び速度の状態フィードバックループを持つ制御回路で制御するロボットの制御装置において、

ロボットの関節角度を計測する手段と、

ロボットの手先先端に取り付けたツールの初期姿勢を記憶する手段と、特定の関節軸の位置ゲイン又は速度ゲインを他の関節軸の位置ゲイン又は速度ゲインより小さく設定する手段と、

前記関節角度とツールの初期姿勢からロボットべ一スに対するツール姿勢を一定に保つように位置又は速度指令への修正量を演算する手段と

を有することを特徴とする口ボッ卜の制御装置。

2 . 関節を駆動するモータを、位置及び速度の状態フィ一ドバックループを持つ制御回路で制御するロボットの制御装置において、

ロボットの関節角度を計測する手段と、

ロボットの手先先端に取り付けたッ一ルの初期姿勢を記憶する手段と、前記関節角度に基づいて座標系間の静力学対応関係を演算する手段と、前記関節角度とロボットの作業座標系での位置指令に基づいて前記制御回路とは別の第 2の位置，速度の状態フィードバック制御を行う手段と、

前記第 2のフィードバック制御系における特定の軸の位置ゲイン又は速度ゲインを他の関節軸の位置ゲイン又は速度ゲインより小さく設定する手段と、前記静力学対応関係を用いることにより前記第 2のフィードバック制御系の出力値を関節角トルク値に変換する手段と、

前記第 2のフィードバック制御系の関節角トルク値を、前記第 1のフィードバック制御系の出力値に加算する手段と、

前記関節角度と前記ツールの初期姿勢からロボットベースに対するツール姿勢を一定に保つように位置又は速度指令への修正量を演算する手段と

を有することを特徴とするロボットの制御装置。

3 . 前記特定の関節軸の位置ゲイン又は速度ゲインを小さく設定する代わりに速度制御器の後段にトルクリミツトを設ける手段を有することを特徴とする請求項 1または 2記載の口ボッ卜の制御装置。

4 . ロボットの手首の位置と姿勢を制御するモータの制御装置において、前記手首の位置の制御を作業座標の制御系で行う手段と、

前記手首の姿勢の制御を関節座標の制御系で行う手段と

を有することを特徴とするロボットの制御装置。

5 . 前記作業座標の制御系は、

ロボットの関節角度を計測する手段と、

前記関節角度に基づいて座標系間の静力学対応関係を演算する手段と、前記関節角度とロボットの作業座標系での位置指令に基づいて作業座標系において位置と速度の状態フィ一ドバック制御を行う手段と、

前記静力学対応関係を用いることにより前記フィードバック制御系の出力値を関節座標系の関節角トルク指令に変換する手段

を有することを特徴とする請求項 4記載の口ボットの制御装置。

6 . 前記作業座標系の位置，速度の状態フィードバック制御における特定の軸の位置ゲイン又は速度ゲインを他の軸の位置ゲイン又は速度ゲインより小さく設定する手段を有することを特徴とする請求項 5記載のロボットの制御装置。

7 . 前記作業座標系の位置，速度の状態フィードバック制御における特定の軸の位置ゲイン又は速度ゲインを小さく設定する代わりに速度制御器の後段にリミットを設ける手段を有することを特徴とする請求項 5記載のロボットの制御装置。

8 . 前記作業座標の制御系は、

ロボッ卜の関節角度を計測する手段と、

前記関節角度に基づいて座標系間の微小変位の対応関係を演算する手段と、前記関節角度とロボッ卜の作業座標系での位置指令に基づいて作業座標系において位置の状態フィ一ドバック制御を行う手段と、

前記微小変位の対応関係を用いることにより前記位置のフィ一ドバック制御系の出力値を関節座標系の速度指令に変換する手段と、

前記速度指令と前記関節角度に基づいて関節座標系において速度の状態フィードバック制御を行う手段

を有することを特徴とする請求項 4記載のロボットの制御装置- 9 . 前記作業座標系の位置の状態フィードバック制御における特定の関節軸の位置ゲインを他の軸の位置ゲインより小さく設定する手段

を有することを特徴とする請求項 8記載のロボットの制御装置。

1 0 . 前記作業座標系の位置の状態フィードバック制御における特定の関節軸の位置ゲインを小さく設定する代わりに位置制御器の後段にリミットを設ける手段を有することを特徴とする請求項 8記載の口ボットの制御装置。