(57)【特許請求の範囲】
【請求項１】
ロボットの各軸を駆動するサーボモータ１３を制御するために前記各軸毎に位置速度制御系１１を含むロボットの制御装置において、
前記サーボモータ１３の関節座標系に関する関節角度を計測する角度計測器１４と、
関節指令をもとに前記サーボモータ１３の運動に必要な運動トルクを演算する運動トルク演算部１５と、
前記位置速度制御系１１にて演算されたトルク指令と前記運動トルクとの差分により外乱トルクを求める外乱トルク推定部１６と、
前記関節角度をもとに前記ロボットの作業座標系と前記サーボモータ１３の関節座標系との間のヤコビ行列を演算する微小変位関係演算部１７と、
前記外乱トルクに前記ヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力に変換する外力演算部１８と、
インピーダンス制御によって前記外力の方向に応じた前記ロボットの作業座標系での位置修正量を演算する力制御部１９と、
前記ロボットの作業座標系での位置修正量に前記ヤコビ行列の逆行列を乗じて前記関節座標系での関節角度修正量に変換する関節角度修正量演算部２０とを備えたことを特徴とするロボットの制御装置。
【請求項２】
前記運動トルク演算部１５は、前記ロボットの関節部の重力トルクを演算する重力トルク演算部と、
前記サーボモータ１３の加速度トルクを演算する加速度トルク演算部と、
前記サーボモータ１３の速度を維持するための速度トルクとを演算する速度トルク演算部と、
前記重力トルクと前記加速度トルクと前記速度トルクとを加算して前記運動トルクを演算する運動トルク加算部２２とからなることを特徴とする請求項１記載のロボットの制御装置。
【請求項３】
前記運動トルク演算部１５は、前記位置速度制御系１１と同じ位置ループゲインおよび速度ループゲインを用いて前記運動トルクを演算する第２の位置速度制御系１１５と、
２慣性系モデルによって前記ロボットを模擬し、前記第２の位置速度制御系１１５が出力する運動トルクをもとに前記関節座標系に関する関節角度を演算して前記第２の位置速度制御系１１５へフィードバックする機械系模擬回路１１８とからなることを特徴とする請求項１記載のロボットの制御装置。
【請求項４】
前記力制御部１９は、インピーダンス制御によって前記外力の方向に応じた前記ロボットの作業座標系での位置修正量を演算するインピーダンス制御部２６と、
前記ロボットの作業内容に応じて、前記ロボットの作業座標系の特定の方向についての前記位置修正量を有効または無効とする修正量選択部２７とからなることを特徴とする請求項１乃至３のいずれか１項に記載のロボット制御装置。
【請求項５】
前記外力演算部１８は、前記外乱トルクに前記ヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力に変換する第１の外力演算部１８１と、
前記外乱トルクを前記外力の作用点から前記ロボットの各軸までの距離と、前記各軸の関節角度の正弦または余弦との積により除して前記ロボットの作業座標系での外力に変換する第２の外力演算部１８２と、
前記第１の外力演算部１８１と前記第２の外力演算部１８２の各出力の平均値を求める外力平均演算部２８とを備えること特徴とする請求項１乃至４のいずれか１項に記載のロボットの制御装置。
【請求項６】
ロボットの各軸を駆動するサーボモータ１３を制御するために前記各軸毎に位置速度制御系１１を含み、前記ロボット先端に配置され前記ロボットと協同して作業する外部軸を駆動するサーボモータ１３を制御するために外部軸位置速度系を含むロボットの制御装置において、
前記ロボットのサーボモータ１３の関節座標系に関する関節角度を計測する角度計測器１４と、
前記ロボットへの関節指令をもとに前記ロボットのサーボモータ１３の運動に必要なロボット軸運動トルクを演算するロボット軸運動トルク演算部１５と、
前記ロボットの位置速度制御系１１にて演算されたロボット軸トルク指令と前記ロボット軸運動トルクとの差分によりロボット軸外乱トルクを求めるロボット軸外乱トルク推定部１６と、
前記関節角度をもとに前記ロボットの作業座標系と前記サーボモータ１３の関節座標系との間のヤコビ行列を演算する微小変位関係演算部１７と、
前記ロボット軸外乱トルクに前記ヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力に変換する第１の外力演算部１８１と、
前記ロボット軸外乱トルクを前記外力の作用点から前記ロボットの各軸までの距離と、前記各軸の関節角度の正弦または余弦との積により除して前記ロボットの作業座標系での外力に変換する第２の外力演算部１８２と、
前記第１の外力演算部１８１と前記第２の外力演算部１８２の各出力の平均値を求める外力平均演算部２８と、
前記外部軸のサーボモータ１３の関節座標系に関する関節角度を計測する角度計測器１４と、
前記外部軸への関節指令をもとに前記外部軸のサーボモータ１３の運動に必要な外部軸運動トルクを演算する外部軸運動トルク演算部１５と、
前記外部軸の位置速度制御系１１から演算された外部軸トルク指令と前記外部軸運動トルクとの差分により外部軸外乱トルクを求める外部軸外乱トルク推定部１６と、
前記外部軸外乱トルクを前記ロボットの作業座標系での外力に変換する第３の外力演算部１８３とを備えることを特徴とするロボットの制御装置。
【請求項７】
前記外力平均演算部２８の出力である前記ロボットに作用する外力と前記第３の外力演算部の出力である前記外部軸に作用する外力との差分を求める外力差演算部３０を備えることを特徴とする請求項６記載のロボットの制御装置。
【請求項８】
前記外力演算部１８の出力が予め設定されたしきい値よりも大きい場合には、前記ロボット各軸又は前記外部軸の少なくとも一つを停止させる停止処理部２９を備えることを特徴とする請求項１乃至６のいずれか１項に記載のロボットの制御装置。
【請求項９】
前記ロボットを操作する操作ペンダント３１を備え、
前記外力差演算部３０の出力である前記差分を前記操作ペンダント３１上に表示することを特徴とする請求項７記載のロボットの制御装置。
【請求項１０】
ロボットの各軸を駆動するサーボモータ１３を制御するために前記各軸毎に位置速度制御系１１を含むロボットの制御装置の制御方法において、
前記ロボットの制御部を機能ブロック化すると共に前記ロボットを２次慣性系モデルとして模擬し、関節指令をもとに算出した運動トルクと前記関節指令をもとに前記位置速度制御系１１から出力されたトルク指令との差分により外乱トルクを推定し、
前記外乱トルクに前記ロボットの作業座標系と前記サーボモータ１３の関節座標系との間のヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力を推定し、
前記外力をもとにインピーダンス制御を行い前記ロボットの位置修正量を演算し、
前記位置修正量を有効または無効とすることを特徴とするロボット制御方法。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、ロボットの制御装置に関し、さらに詳しく言えば、特にロボットの作業座標系での力、トルク設定値をもとに関節を駆動するサーボモータの発生力を制御するロボットの制御装置および制御方法に関する。
【０００２】
【従来の技術】
従来のロボットはロボットの関節軸毎に位置速度制御系にて制御が行われていた。このような制御系でワークとの接触を伴うようなスポット溶接やシーム溶接のプレイバック再生を行う際、ワーク自体の位置ズレやロボットがワークを把持した位置のズレなどがあると、位置決め精度を高くするために大きく設定されたゲインや積分器の作用により、大きなトルクを発生することになる。この大きなトルクが発生することで、ワークを変形させたり溶接不良の発生や溶着などで作業の遂行が困難になって、時にはツールやロボットの破損の危険性があった。また、教示者がロボットを作業位置へ移動させる教示作業時には、教示者がスポットガンの制御点をワークの打点に慎重に移動させて、位置を登録する必要があり、誤ってガン電極をワークに押し付けて変形させる危険性があったため肉体的および精神的疲労度が非常に大きかった。
このような問題に対して、ロボットに特別な装置を付加することなく作業座標系で柔軟に力制御を行う方式としては、図１０に示すように作業座標系の各座標軸方向に関して柔らかさ（バネ定数）を設定して空間内の軸別に外力に対する順応性の大小を指定できる方式がある（特許文献１、特許文献２）。
【０００３】
また、外力の推定をして力制御を行う方式としては、関節座標系の各軸のモータへの外乱を外乱オブザーバにより推定し、ヤコビ行列を用いた座標変換により外乱推定値を作業座標系の外力推定値に変換できる方式がある（特許文献３、特許文献４）。
【０００４】
【特許文献１】
特開平８−２２７３２０号公報
【特許文献２】
特開２０００−００５８８１号公報
【特許文献３】
特開平１１−５８２８５号公報
【特許文献４】
特開平９−１０３９４５号公報
【０００５】
【発明が解決しようとする課題】
ところが、特許文献１や特許文献２に示すように、作業座標系で柔らかさ（バネ定数）を設定して空間内の方向別に外力に対する順応性の大小を指定する方式では、位置偏差が増大するとサーボモータの発生トルクが比例的に増大するため外力による位置偏差（移動距離）が大きな場合に対応できないという問題点がある。また、周辺機器やワークを取り除いて接触状態を解除しようとした場合にロボットは元の位置指令の場所に戻る力を発生させるため、再度周辺機器やワークと衝突を起して周辺機器やワークを破損させるなどの問題点を有している。
また、特許文献３や特許文献４に示すように、関節座標系の外乱を外乱オブザーバにより推定し、ヤコビ行列を用いた座標変換により外乱推定値を作業座標系の外力推定値に変換する方式が示されている。これらの従来例では、フィルタなどを使用することで検出時間にも遅れが発生し、また、摩擦や重力の影響が大きく正確な外力推定値を得ることが出来ないという問題点を有している。
そこで本発明は、センサなどを用いることなく、ストロークの大きな変位にも対応できる作業座標系の力制御を行うことを目的とする。
【０００６】
【課題を解決するための手段】
本発明では、上記問題点を解決するため、請求項１記載のロボット制御装置は、ロボットの各軸を駆動するサーボモータ１３を制御するために前記各軸毎に位置速度制御系１１を含むロボットの制御装置において、前記サーボモータ１３の関節座標系に関する関節角度を計測する角度計測器１４と、関節指令をもとに前記サーボモータ１３の運動に必要な運動トルクを演算する運動トルク演算部１５と、前記位置速度制御系１１にて演算されたトルク指令と前記運動トルクとの差分により外乱トルクを求める外乱トルク推定部１６と、前記関節角度をもとに前記ロボットの作業座標系と前記サーボモータ１３の関節座標系との間のヤコビ行列を演算する微小変位関係演算部１７と、前記外乱トルクに前記ヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力に変換する外力演算部１８と、インピーダンス制御によって前記外力の方向に応じた前記ロボットの作業座標系での位置修正量を演算する力制御部１９と、前記ロボットの作業座標系での位置修正量に前記ヤコビ行列の逆行列を乗じて前記関節座標系での関節角度修正量に変換する関節角度修正量演算部２０とを備えている。
【０００７】
また、請求項２記載のロボット制御装置は、前記運動トルク演算部１５は、前記ロボットの関節部の重力トルクを演算する重力トルク演算部と、前記サーボモータ１３の加速度トルクを演算する加速度トルク演算部と、前記サーボモータ１３の速度を維持するための速度トルクとを演算する速度トルク演算部と、前記重力トルクと前記加速度トルクと前記速度トルクとを加算して前記運動トルクを演算する運動トルク加算部２２とからなる。また、請求項３記載のロボット制御装置は、前記運動トルク演算部１５は、前記位置速度制御系１１と同じ位置ループゲインおよび速度ループゲインを用いて前記運動トルクを演算する第２の位置速度制御系１１５と、２慣性系モデルによって前記ロボットを模擬し、前記第２の位置速度制御系１１５が出力する運動トルクをもとに前記関節座標系に関する関節角度を演算して前記第２の位置速度制御系１１５へフィードバックする機械系模擬回路１１８とからなる。また、請求項４記載のロボット制御装置は、前記力制御部１９は、インピーダンス制御によって前記外力の方向に応じた前記ロボットの作業座標系での位置修正量を演算するインピーダンス制御部２６と、前記ロボットの作業内容に応じて、前記ロボットの作業座標系の特定の方向についての前記位置修正量を有効または無効とする修正量選択部２７とからなる。
【０００８】
また、請求項５記載のロボット制御装置は、前記外力演算部１８は、前記外乱トルクに前記ヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力に変換する第１の外力演算部１８１と、前記外乱トルクを前記外力の作用点から前記ロボットの各軸までの距離と、前記各軸の関節角度の正弦または余弦との積により除して前記ロボットの作業座標系での外力に変換する第２の外力演算部１８２と、前記第１の外力演算部１８１と前記第２の外力演算部１８２の各出力の平均値を求める外力平均演算部２８とを備えるものである。
【０００９】
また、請求項６記載のロボット制御装置は、ロボットの各軸を駆動するサーボモータ１３を制御するために前記各軸毎に位置速度制御系を含み、前記ロボット先端に配置され前記ロボットと協同して作業する外部軸を駆動するサーボモータ１３を制御するために外部軸位置速度系を含むロボットの制御装置において、前記ロボットのサーボモータ１３の関節座標系に関する関節角度を計測する角度計測器１４と、前記ロボットへの関節指令をもとに前記ロボットのサーボモータ１３の運動に必要なロボット軸運動トルクを演算するロボット軸運動トルク演算部１５と、前記ロボットの位置速度制御系１１にて演算されたロボット軸トルク指令と前記ロボット軸運動トルクとの差分によりロボット軸外乱トルクを求めるロボット軸外乱トルク推定部１６と、前記関節角度をもとに前記ロボットの作業座標系と前記サーボモータ１３の関節座標系との間のヤコビ行列を演算する微小変位関係演算部１７と、前記ロボット軸外乱トルクに前記ヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力に変換する第１の外力演算部１８１と、前記ロボット軸外乱トルクを前記外力の作用点から前記ロボットの各軸までの距離と、前記各軸の関節角度の正弦または余弦との積により除して前記ロボットの作業座標系での外力に変換する第２の外力演算部１８２と、前記第１の外力演算部１８１と前記第２の外力演算部１８２の各出力の平均値を求める外力平均演算部２８と、前記外部軸のサーボモータ１３の関節座標系に関する関節角度を計測する角度計測器１４と、前記外部軸への関節指令をもとに前記外部軸のサーボモータ１３の運動に必要な外部軸運動トルクを演算する外部軸運動トルク演算部１５と、前記外部軸の位置速度制御系１１から演算された外部軸トルク指令と前記外部軸運動トルクとの差分により外部軸外乱トルクを求める外部軸外乱トルク推定部１６と、前記外部軸外乱トルクを前記ロボットの作業座標系での外力に変換する第３の外力演算部１８３とを備えるものである。
【００１０】
また、請求項７記載のロボット制御装置は、前記外力平均演算部２８の出力である前記ロボットに作用する外力と前記第３の外力演算部の出力である前記外部軸に作用する外力との差分を求める外力差演算部３０を備えるものである。また、請求項８記載のロボット制御装置は、前記外力演算部１８の出力が予め設定されたしきい値よりも大きい場合には、前記ロボット各軸又は前記外部軸の少なくとも一つを停止させる停止処理部２９を備えるものである。また、請求項９記載のロボット制御装置は、前記ロボットを操作する操作ペンダント３１を備え、前記外力差演算部３０の出力である前記差分を前記操作ペンダント３１上に表示することを特徴とするものである。
【００１１】
また、請求項１０記載のロボット制御方法は、ロボットの各軸を駆動するサーボモータ１３を制御するために前記各軸毎に位置速度制御系１１を含むロボットの制御装置の制御方法において、前記ロボットの制御部を機能ブロック化すると共に前記ロボットを２次慣性系モデルとして模擬し、関節指令をもとに算出した運動トルクと前記関節指令をもとに前記位置速度制御系１１から出力されたトルク指令との差分により外乱トルクを推定し、前記外乱トルクに前記ロボットの作業座標系と前記サーボモータ１３の関節座標系との間のヤコビ行列の転置逆行列を乗じて前記ロボットの作業座標系での外力を推定し、前記外力をもとにインピーダンス制御を行い前記ロボットの位置修正量を演算し、前記位置修正量を有効または無効とすることを特徴とするものである。
【００１２】
【発明の実施の形態】
本発明の第１の実施例を図１に示して説明する。図１の11は位置速度制御ループであり、関節指令を入力してトルク指令をアンプ12へ出力することで、関節モータを駆動する。ここではよく用いられる位置比例−速度比例積分制御を用いる。この位置比例−速度比例積分制御（以後、位置速度制御とする）では、高ゲインの位置制御ループおよび速度制御ループの作用により、外部より作用する力に対して各関節軸が柔軟に変位しづらい。そこで、上位コントローラからの位置速度制御ループ11への関節指令から、運動トルク演算部15によりロボットの各リンクが動作するために必要な重力を補償するためのトルク（以下、重力トルク）と加速するためのトルク（以下、加速度トルク）と速度分のトルク（速度トルク）からなる運動トルクを演算する。求められた運動トルクから実際の位置速度制御ループ11の出力であるトルク指令を外乱トルク推定部16で減算する事により、関節座標系での外乱トルク推定値を演算することができる。
次に、角度計測器14で検出された関節角度から、微小変位関係演算部17において関節座標系と作業座標系間の微小変位関係、一般的にヤコビ行列と言われている行列を演算する。そこで、外力演算部18において、前記外乱トルク推定値に前記座標系間の微小変位関係を乗算することで、作業座標系における外乱力推定値に変換することができる。外力とは、外部からの加わる力とモーメントである。更に、力制御部19において、インピーダンス制御など力制御アルゴリズムにより前記外乱力推定値を作業座標系での位置修正量に変換し、関節角度修正量演算部20により前記位置修正量を関節座標系での関節角度修正量に変換する。
よって、変換された関節角度修正量を位置速度制御ループ11への入力である関節指令に加算する事で、ロボットが外力に応じた位置の修正を行うことが可能になる。
【００１３】
以下に、本発明の第１の具体的実施例を図２に示して、関節座標系での位置速度制御に本発明の制御を適用した制御ブロック線図を説明する。
（１）運動トルク演算方法
上位コントローラから位置速度制御ループ11への入力である関節指令を運動トルク演算部15に入力する。トルク演算部21内で、１回微分を取って関節角速度指令を求め、２回微分を取って関節角加速度指令を求め、前記関節角速度指令と前記関節角加速度指令からロボットの各リンクが動作するために必要な運動トルクを求める。ここで、運動トルクとは、各リンクの重量分に打ち勝って姿勢を維持するための重力トルクと、リンクの慣性分に打ち勝つために必要な加速度トルクと、減速器の摩擦分に打ち勝つために必要な速度トルクである。重力トルクは各リンクの重心位置と重量と関節指令から求め、加速度トルクは関節角加速度にロボットの姿勢に応じてイナーシャを乗算することで求め、速度トルクは関節角速度に粘性摩擦係数を乗算し、クーロン摩擦値を加算する事で求めることができる。よって、運動トルク加算部22において、重力トルクと加速度トルクと速度トルクを加算することで、運動トルクを求めることが可能である。
【００１４】
（２）外乱トルク推定方法
位置速度制御ループ11からの出力であるトルク指令は外乱がない場合は前記運動トルクとほぼ同一であるが、何らかの要因で外乱が入力された場合にはその分だけ運動トルクよりも増減することになる。よって、外乱トルク推定部16により、前記運動トルクから位置速度制御ループ11のトルク指令を減算する事で、関節座標系での外乱トルク推定値τpresumptionを求めることができる。
【００１５】
（３）外乱力推定値演算方法
角度計測器14で検出された関節角度から、微小変位関係演算部17において関節座標系と作業座標系間の微小変位関係、一般的にヤコビ行列と言われている行列を演算し、その転置逆行列を用いることで、関節座標系の外乱トルク推定値から作業座標系における外力推定値を演算することが可能である。
例えば、６自由度のロボットの場合は、ヤコビ行列は以下の式により演算することが可能である。
【００１６】
【数１】
0004054984.tif 000002
【００１７】
ここで、
Ｊ：ヤコビ行列（関節座標系と作業座標系間の微小変位関係式）
0ｓi：第ｉ関節座標の回転方向ベクトル（ロボットのベース座標系を基準）
0Ｐi：第ｉ関節位置ベクトル（ロボットのベース座標系を基準）
×：ベクトルの外積を示す
ｒ：ロボットの作業位置ベクトル
よって、外力演算部18において、作業座標系での外乱力推定値Ｆpresumptionは以下の式で求めることができる。
【００１８】
【数２】
0004054984.tif 000003
【００１９】
ここで、
Ｔ：転置行列を示す
−１：逆行列を示す
【００２０】
（４）角度修正量演算方法
力制御部19において、前記外力推定値Ｆpresumptionを作業座標系のインピーダンス制御部26により、作業座標系の位置修正量Ｘcompに変換することができる。
【００２１】
【数３】
0004054984.tif 000004
【００２２】
式▲３▼において、速度と加速度に関して以下の式を代入すると、
【００２３】
【数４】
0004054984.tif 000005
【００２４】
一般的には、式▲４▼を式▲３▼に代入して式を解けば良いが、簡略化のため、この中からインピーダンス制御の粘性成分のみを用いる式を採用しても良い。この時、作業座標系の位置修正量Ｘcompは、以下の式で求めることができる。
【００２５】
【数５】
0004054984.tif 000006
【００２６】
この位置修正量Ｘcompは修正量選択部27により、作業内容に応じて修正量を有効または無効にする作業座標系の軸を選択することができる。
【００２７】
次に、関節角度修正量演算部20による前記作業座標系の位置修正量Ｘcompの関節座標系の関節角度修正量θcompへの変換は、以下の式で行うことができる。
【００２８】
【数６】
0004054984.tif 000007
【００２９】
となる。(6)で求められた前記関節角度修正量θcompを上位コントローラからの関節指令に加算する事で、外乱の大きさや方向に対してロボットの位置を逐次修正して外乱を吸収することが可能になる。例えば、スポット溶接作業でガンのチップがワークに接触した場合を例に説明する。予め教示された位置よりも早い段階で固定側のチップがワークと接触した場合には、通常の位置速度制御であれば位置速度ループの高ゲインによりロボットに過大な力を発生させ、ガンやワークを破損させる危険性があった。本制御を用いるとチップに作用する力を外力演算部により作業座標系での外乱力として検出して、その方向に力制御を行い、関節角度修正量を求めることで、ロボットが過大な力を発生させないようにできる。
【００３０】
以下に、本発明の第２の具体的実施例を図３に示して、関節座標系での位置速度制御に本発明の制御を適用した制御ブロック線図を説明する。ここでは、前記第１の具体的実施例において、（１）の運動トルク演算方法の処理に差異がある。
（１’）運動トルク演算方法
上位コントローラから位置速度制御ループ11への入力である関節指令を運動トルク演算部15に入力する。運動トルク演算部15内では、前記位置速度制御系11とは別の第２の位置速度制御系115に前記関節指令が入力され、実際のロボット機構部を模擬した機械系模擬回路118の位置速度制御を行う。第2の位置速度制御系115の位置ループゲインＫｐ116は、位置速度制御ループ11の位置ループゲインＫｐ111と同一のパラメータ値である。また同様に、第2の位置速度制御系115の速度ループゲインＫｖ117は、位置速度制御ループ11の速度ループゲインＫｐ112と同一のパラメータ値である。ここで、機械系模擬回路118は、関節モータ13と同一のイナーシャと粘性係数と剛性を持った仮想的な制御系で構成されており、例えばモータイナーシャと２次側イナーシャと減速器から構成される２慣性系モデルである。この２慣性系モデルが実際のロボットと同一であれば、外乱がない場合には実際のロボットと仮想ロボットの挙動は同じになり、前記第１の位置速度制御系11の出力であるトルク指令と前記第２の位置速度制御系115の出力であるトルク指令は同一と考えられる。
よって、前記第２の位置速度制御系115の出力であるトルク指令を運動トルクとして使用する。その後の処理は、前記第１の具体的実施例における（２）〜（４）と同一の処理を行えば良い。
【００３１】
次に、本発明の第１の実施例で説明した制御の作用をスポット溶接の位置教示作業を例にして、図４を用いて説明する。
スポット溶接の位置教示作業は、作業者が操作ペンダントなどを使用して手先にスポット溶接ガン41を配置したロボットを誘導することで、ワーク43上の決められた打点位置に対して、スポット溶接ガン41の固定側電極44と、スポット溶接ガン41の他端に配置した可動側電極駆動用モータ42によって駆動される可動側電極45を位置決めして位置を登録するものである。通常の位置速度制御を用いた場合では、作業者は以下の手順で教示作業を行っていた。
（ａ）待機位置へ移動
スポット溶接ガン41上の固定側電極44と可動側電極45の間が十分に広い待機状態で、固定側電極44と可動側電極45を結んだ線上にワーク43の打点位置が入るようにロボットを誘導する。
（ｂ）固定側電極の移動
作業者は、固定側電極44がワーク43の打点位置に下方から接触する寸前までロボットを非常に低速で動作させなければならない。
（ｃ）可動側電極の移動
固定側電極44の移動完了後に、可動側電極駆動用モータ42を駆動して可動側電極45がワーク43の打点位置に上方から接触する寸前までロボットを非常に低速で動作させなければならない。
ここで、（ｂ）や（ｃ）の作業時に電極とワークが接触した場合には、位置速度制御によりロボットが過大な力を発生することでワーク43や固定側電極44、ロボットの破損の危険性もあるため、作業者の肉体的精神的疲労度が非常に大きく、教示時間も大幅に必要としていた。
【００３２】
次に、本作業における本発明の力制御の働きを、詳細に説明する。
（ａ）待機位置へ移動
スポット溶接ガン41上の固定側電極44と可動側電極45の間が十分に広い待機状態で、固定側電極44と可動側電極45を結んだ線上にワーク43の打点位置が入るようにロボットを誘導する。この誘導時には通常の位置速度制御のままである。
（ｂ）固定側電極の移動
作業者は、ロボットの動作モードを本力制御に切り替え、例えばスポット溶接ガン41の固定側電極44と可動側電極45を結んだ線を作業座標系のＺ軸に取った場合には、前記力制御部19の修正量選択部27により、Ｚ軸方向の位置修正量の有効を選択し、その他のＸＹ軸に関しては位置修正量の無効を選択する。次に、固定側電極44がワーク43の打点位置に下方から接触するまでロボットを動作させる。固定側電極44がワーク43に接触した場合には、運動トルク演算部15の運動トルクと位置速度制御ループ11の出力であるトルク指令に外乱トルク推定値として差が発生し、この外乱トルク推定値を外力演算部18と力制御部19と関節角度修正量演算部20により角度修正量として演算し、関節指令に加算することで、固定側電極44は過大な力をワーク43に加えることがなく接触した状態を維持できる。
（ｃ）可動側電極の移動
固定側電極44の移動完了後に、可動側電極駆動用モータ42を駆動して可動側電極45がワーク43の打点位置に上方から接触するまでロボットを動作させる。ここで、可動側電極駆動用モータ42の駆動の制御にも本力制御を行うことで、可動側電極45が過大な力をワーク43に加えることがなく接触した状態を維持できる。
【００３３】
以下に、本発明の第３の具体的実施例を図５に示して、関節座標系での位置速度制御に本発明の制御を適用した制御ブロック線図を説明する。ここでは、前記第１の具体的実施例において、外力演算方法の処理に差異がある。また、ロボットは図８に示すスポット溶接用ロボットを例に説明を行う。前記第１の具体的実施例においては、外力の推定は手先に作用する力（Ｆｘ，Ｆｙ，Ｆｚ）とモーメント（Ｍｘ，Ｍｙ，Ｍｚ）の６軸であった。しかし、スポット溶接においてワークの位置ズレなどに対してスポット溶接ガンの電極が自動的に位置修正をして位置ズレに倣う場合には、電極の開閉方向の接触力（Ｆｚ）のみを推定すれば良いため、電極開閉方向の力のみを推定して処理を簡略化する。電極開閉方向の外力の推定方法としては、ロボットのＪ１〜Ｊ３軸で１つの外力の推定値Ｆ１を求め、Ｊ４，Ｊ５，Ｊ６でそれぞれ外力の推定値Ｆ２，Ｆ３，Ｆ４を求める。そして、前記４個の外力推定値の平均を取ることで、誤検出低減や検出精度の向上を図ることが特徴である。（１）運動トルク演算方法〜（２）外乱トルク推定方法までの処理は、前記第１の具体的実施例における（１）〜（２）と同一の処理を行えば良い。
（３”）外乱力推定値演算方法
外力推定値Ｆ１の演算方法は、前記第１の具体的実施例における外力演算方法と同様に第１の外力演算部181で、微小変位関係演算部17において求めた座標系間の微小変位関係の転置逆行列24を用いることで、外乱トルク推定部16の出力である外乱トルク推定値τpresumption１〜３から作業座標系における外力推定値Ｆ１を求める。ここで、電極の開閉方向の接触力推定であるため、ロボットのＪ１〜Ｊ３軸からロボットの手首部分へのヤコビ行列を求めるだけで良く、以下の式により演算することが可能である。
【００３４】
【数７】
0004054984.tif 000008
【００３５】
次に、外力推定値Ｆ２，Ｆ３，Ｆ４の演算方法の説明を行う。スポット溶接ではガンの両電極を閉じることでワークを挟み込んで溶接を行うが、ロボット側の固定電極がワークに接触し、外力Ｆがガンの電極開閉方向に作用した場合の外力推定値Ｆ３を例に、図９を用いて説明する。
Ｊ５軸に作用するトルクτpresumption５は、図９（ｂ）に示すように外力ＦとＪ６軸の角度に依存しているため、以下の式で表すことができる。
【００３６】
【数８】
τpresumption５＝Ｆ・Ｌ・cos（θ６）  ・・・(8)
ここで、
Ｆ：外力
Ｌ：電極の外力作用点からＪ５軸までの距離
θ６：Ｊ６軸の角度
【００３７】
よって、ガンの電極開閉方向に作用する外力推定値Ｆ３（≒Ｆ）は以下の式で表すことができる。
【００３８】
【数９】
0004054984.tif 000009
【００３９】
よって、第２の外力演算部182において、外力推定値Ｆ３を求めることができた。同様に、Ｆ２、Ｆ４についても演算を行う。次に、外力平均演算部28において、前記外力推定値Ｆ１〜Ｆ４の平均を取ることで、外力平均値を求めることができる。
（４）角度修正量演算方法の処理も、前記第１の具体的実施例における（４）と同一の処理を、前記外力平均値を用いて行えば良い。
【００４０】
ただし、ここでは図示しないが、ガンの電極開閉方向に作用する外力は前記第１の具体的実施例と同様に、Ｊ４〜Ｊ６軸に作用するトルクとガンに設定した座標系からＪ４〜Ｊ６軸までのヤコビ行列とで演算することも可能である。つまり、Ｊ１〜Ｊ３軸とＪ４〜Ｊ６軸とでガンの開閉方向に作用する外力（Ｆｘ，Ｆｙ，Ｆｚ）をそれぞれ推定することもできる。前記第３の具体的実施例の場合には、この演算式を展開した一部を使用している。このようにＪ４〜Ｊ６軸までのヤコビ行列を使用することで、手先に作用する力（Ｆｘ，Ｆｙ，Ｆｚ）が求められ、外力の作用する方向が１軸以上の場合にも適用することが可能となる。Ｊ４〜Ｊ６軸までのヤコビ行列は、以下の演算を行う。
【００４１】
【数１０】
0004054984.tif 000010
【００４２】
後は、前記第１の具体的実施例と同様に、式▲２▼を演算することで、外力推定値Ｆpresumption４〜６を求めることができる。
【００４３】
以下に、本発明の第４の具体的実施例を図６に示して、関節座標系での位置速度制御に本発明の制御を適用した制御ブロック線図を説明する。ここでは、前記第３の具体的実施例において、力制御部と関節角度修正量演算部の部分に差異がある。前記第１〜３の具体的実施例では、外力に応じて角度修正量を演算することで、ロボットが外力に対して追従する制御方法について述べたが、ここでは外力を検出することで、ロボットの動作を停止させることを目的とする。また、ロボットは図８に示すスポット溶接用ロボットの教示作業を例に説明を行う。作業者は、ロボットを誘導することで、ロボットの先端に配置されたスポット溶接用ガンの電極間にワークを挟み込む姿勢を作る。このときには、両電極とワークには十分な距離がある。次に、作業者はガンの電極の固定側（つまりロボット）を上昇させる（ワークに近づく方向に移動させる）。ここから、前記第３の具体的実施例における（１）〜（３”）と同一の処理を行い、外力平均演算部28において外力平均値を求める。
（４’）停止処理方法
停止処理部29において、求められた前記外力平均値と予め設定された検出しきい値を比較することで、ガンの固定電極がワークに接触した際に、ロボットの動作を停止させる。ロボットが自動的に停止することでスポット溶接の教示作業が自動的に行え、作業者の肉体的精神的負担を軽減することができる。また、図示しないが、外部の周辺機器からロボットが力を加えられる場合には、ロボットを停止させる代わりに外部の周辺機器に停止信号を出力しても良い。
【００４４】
以下に、本発明の第５の具体的実施例を図７に示して、関節座標系での位置速度制御に本発明の制御を適用した制御ブロック線図を説明する。ここでは、前記第４の具体的実施例において、ロボット軸以外に外部軸を用いた部分に差異がある。前記第４の具体的実施例では、ロボットの各軸によりエンドエフェクタの制御点に作用する外力を推定したが、ここでは外部軸に作用する外力推定値と前記ロボットの外力推定値との差を求め、実際にワークに作用している力が釣り合っているかどうかを作業者に提示したり、前記第１の具体的実施例と同様に位置の修正量を求めたりする。また、ロボットは、前記第４の具体的実施例と同様に、図８に示すスポット溶接用ロボットの教示作業を例に説明を行う。
作業者は、ロボットを誘導することで、ロボットの先端に配置されたスポット溶接用ガンの電極間にワークを挟み込む姿勢を作る。このときには、両電極とワークには十分な距離がある。次に、作業者はガンの電極の固定側（つまりロボット）を上昇させる（ワークに近づく方向に移動させる）。ここから、前記第３又は４の具体的実施例における（１）〜（３”）と同一の処理を行い、外力平均部28において外力平均値を求める。
（４”）外部軸の外乱力推定値演算方法
外部軸を駆動するモータの制御系に対しても、（１）〜（３）のロボットの各軸と同様に外乱トルク推定値τpresumption-ex１を求める。ここで、ガン駆動部分の重力と摩擦成分はロボット同様に運動トルク演算部15において、別途補償しておく。
次に、第３の外力演算部183において、前記外乱トルク推定値τpresumption-ex１から、外乱力推定値F5を演算する。ここで例えば、ガンの可動電極の駆動がリードｎのボールネジの場合、以下の式で外乱力推定値Ｆ５を求めることができる。
【００４５】
【数１１】
0004054984.tif 000011
【００４６】
（５）外力提示方法
外力差演算部30において、前記外力推定値Ｆ５と前記外力平均値との差を求め、ロボットコントローラに接続された操作ペンダント31上に表示する。
または、図示しないが、操作ペンダントに表示する代わりに、停止処理部29において、求められた前記外力平均値と予め設定された検出しきい値を比較することで、ガンの固定電極がワークに接触した際に、ロボットの動作を停止でき、ガンの可動電極がワークに接触した際にもガンの動作を停止させることができる。また、両電極がワークに対して作用している力を同じになるように力制御を行うことでワークに無理な変形を生じさせず、品質を向上させることができる。よって、ロボットやガンが自動的に停止することでスポット溶接の教示作業が自動的に行え、作業者の肉体的精神的負担を軽減することができる。
【００４７】
【発明の効果】
以上述べたように、請求項１のロボットの制御装置によれば、ロボットにより接触作業を行わせる際に、センサなどを用いることなく、ストロークの大きな変位にも対応できる作業座標系の柔軟な倣い動作が可能となる。請求項２記載のロボットの制御装置によれば、ロボットの動作に必要な運動トルクを演算することにより、より正確な外乱トルクを求めることが可能である。請求項３記載のロボットの制御装置によれば、実際のロボットと同一のパラメータを持つモデルを用いた位置速度制御系からのトルク指令を運動トルクとして使用することにより、より正確な外乱トルクを求めることが可能である。請求項４記載のロボットの制御装置によれば、修正量の有効または無効を選択できるため、作業に応じてロボットに柔軟な軸方向と剛性の高い軸方向を選択することが可能になる。請求項５記載のロボット制御装置によれば、外力の推定を複数個の平均から求めることで、検出精度を上げ、誤検出を減らすことができる。請求項６記載のロボット制御装置によれば、ロボットと外部軸が作業対象に対して加えている外力を検出することができるため、製品の品質を上げることができる。請求項７記載のロボット制御装置によれば、ロボットと外部軸が作業対象に対して加えている合成した外力を検出することができるため、製品の品質を上げることができる。請求項８記載のロボット制御装置によれば、推定した外力と予め設定されたしきい値を比較することで、ロボットが作業対象と接触した場合に自動的にロボットを停止させることができ、作業者の負担を減らすことができる。請求項９記載のロボット制御装置によれば、外力差演算値を操作ペンダント上に表示することで、作業者が接触状態を判断することができ、ワークやロボットの破損を回避することができる。請求項１０記載のロボットの制御方法によれば、修正量の有効または無効を選択できるため、作業に応じてロボットに柔軟な軸方向と剛性の高い軸方向を選択することが可能になる。
【図面の簡単な説明】
【図１】本発明の第１の基本構成図
【図２】本発明の第１の具体的実施例を表す図
【図３】本発明の第２の具体的実施例を表す図
【図４】本発明の作用を示す図
【図５】本発明の第３の具体的実施例を表す図
【図６】本発明の第４の具体的実施例を表す図
【図７】本発明の第５の具体的実施例を表す図
【図８】本発明のロボットを表す図
【図９】本発明の力の作用を示す図
【図１０】従来の制御方式を示す図
【符号の説明】
１１：位置速度制御ループ
１２：アンプ
１３：関節モータ
１４：角度計測器
１５：運動トルク演算部
１６：外乱トルク推定部
１７：微小変位関係演算部
１８：外力演算部
１８１：第１の外力演算部
１８２：第２の外力演算部
１８３：第３の外力演算部
１９：力制御部
２０：関節角度修正量演算部
２１：トルク演算部
２２：運動トルク加算部
２３：ヤコビ行列
２４：ヤコビ転置行列の逆行列
２５：ヤコビ行列の逆行列
２６：インピーダンス制御部
２７：修正量選択部
２８：外力平均演算部
２９：停止処理部
３０：外力差演算部
３１：操作ペンダント
４１：スポット溶接ガン
４２：可動側電極駆動用モ
４３：ワーク
４４：固定側電極
４５：可動側電極
１１１：位置ループゲイン
１１２：速度ループゲイン
１１３：比例器
１１４：積分器
１１５：第２の位置速度制御系
１１６：位置ループゲイン
１１７：速度ループゲイン
１１８：機械系模擬回路
【図１】
0004054984.tif 000012
【図２】
0004054984.tif 000013
【図３】
0004054984.tif 000014
【図４】
0004054984.tif 000015
【図５】
0004054984.tif 000016
【図６】
0004054984.tif 000017
【図７】
0004054984.tif 000018
【図８】
0004054984.tif 000019
【図９】
0004054984.tif 000020
【図１０】
0004054984.tif 000021