(57)【特許請求の範囲】
【請求項１】
ロボットの各リンクをそれぞれのアクチュエータで駆動するロボットの制御装置であって、
教示プログラムに基づくロボットの手先の目標位置姿勢における各関節部に発生するたわみ又は前記各関節部及び各リンクに発生するたわみをロボットの動力学に基づいて求めるたわみ算出手段と、
該求められたたわみに基づいて前記手先の目標位置とのずれ、若しくは前記手先の目標位置とのずれと前記手先の目標姿勢とのずれとを求める手先ずれ算出手段と、
該ずれと大きさが等しく、方向が逆向きの補正を加えた手先の位置若しくは位置及び姿勢に動作させるアクチュエータの位置を求める補正位置算出手段と、
を備えたロボット制御装置。
【請求項２】
前記たわみ算出手段は、前記各関節部分のたわみと前記リンクのたわみを分離した形で求めることを特徴とする請求項１記載のロボット制御装置。
【請求項３】
前記補正位置算出手段は、たわみを考慮した順変換処理とたわみを無視した逆変換処理を交互に繰り返し行う収束計算により求める請求項１又は請求項２記載のロボット制御装置。
【請求項４】
前記たわみ補正による補正を行う前と行った後の前記アクチュエータの位置の差分を求める手段を設け、この差分がしきい値を越えているときには、アラーム処理を行う請求項１乃至３の内いずれか１項記載のロボット制御装置。
【請求項５】
前記補正位置算出手段は、サーボ制御のための計算周期よりも長い周期で補正されたアクチュエータの位置を求める請求項１乃至４の内いずれか１項記載のロボット制御装置。
【請求項６】
前記周期で求められた補正後のアクチュエータの位置を内挿補間して、指令位置として各関節部のアクチュエータに出力する請求項５記載のロボット制御装置。
【請求項７】
前記求めた手先の目標位置とのずれ、若しくは前記手先の目標位置とのずれと前記手先の目標姿勢とのずれとを表示する手段を備えたことを特徴とする請求項１乃至６の内いずれか１項記載のロボット制御装置。
【請求項８】
前記手先に取り付けられたツールに生じるたわみが生じる軸を指定する手段と、該軸回りのたわみを求め、該たわみによる手先の位置ずれ及び／又は姿勢のずれを補正する手段とを備えた請求項１記載のロボット制御装置。
【請求項９】
前記たわみ算出手段において使用される剛性パラメータは、ロボットを動作させた手先の位置又は姿勢とそのとき作用するモーメントにより求める請求項１乃至８の内いずれか１項記載のロボット制御装置。
【請求項１０】
ロボットの各リンクをそれぞれのアクチュエータで駆動し、ロボット手先を押し付けて作業するロボットの制御装置であって、
ロボット手先を対象物に押し付けたときの反力を検出するセンサと、
教示プログラムに基づくロボットの手先の目標位置姿勢における各関節部に発生するたわみ又は前記各関節部及び各リンクに発生するたわみをロボットの動力学に基づいて求めるたわみ算出手段と、
該求められたたわみに基づいて前記手先の目標位置とのずれ、若しくは前記手先の目標位置とのずれと前記手先の目標姿勢とのずれとを求める手先ずれ算出手段と、該ずれと大きさが等しく、方向が逆向きの補正を加えた手先の位置若しくは位置及び姿勢に動作させるアクチュエータの位置を求める補正位置算出手段とを備えたロボット制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、多関節ロボットの制御装置に関する。特にロボット機構部のたわみを考慮した高精度の位置姿勢が得られるロボット制御装置に関する。
【０００２】
【従来の技術】
ロボットの軽量化、ロボット動作速度の高速化への要求が高まるにつれ、ロボット機構部のたわみが大きくなり、精度に及ぼす影響が無視できなくなってきている。このような要望に対して、従来のロボットでは、１軸毎の減速器の低剛性に起因する２慣性モデルのたわみの補償にて高精度の軌跡制御を盛んに行って来た。また、ロボットアーム自身に関しては、無次元（共振周波数の２次モード、３次モード・・・・が存在する。）の１軸柔軟アームをはりの方程式からモデル化し、低次元化し制御していた。又、特許第３１２００２８号公報等には、アーム（リンク）の弾性変形の制御を簡易的にモータ回転方向の成分に分解して制御する方法が提案されている。
【０００３】
【発明が解決しようとする課題】
多関節ロボットになると、１軸の制御では問題とならなかった、軸の影響がでて、運動を増幅させ振動するというような問題が発生する。又、柔軟アームの制御では計算処理時間がかかるという欠点がある。又、多軸まで考慮されることは今まで存在しなかった。さらに、前述した特許第３１２００２８号公報で開示されているものは、アーム（リンク）の弾性変形を非常に簡単に考慮しているだけであるため、完全な制御ができなかった。
【０００４】
そこで、本発明は、上述した従来技術の問題点を改善することにある。
【０００５】
【課題を解決するための手段】
前記課題を解決するために、本願請求項１に係わる発明は、ロボットの各リンクをそれぞれのアクチュエータで駆動するロボットの制御装置であって、教示プログラムに基づくロボットの手先の目標位置姿勢における各関節部に発生するたわみ又は前記各関節部及び各リンクに発生するたわみをロボットの動力学に基づいて求めるたわみ算出手段と、該求められたたわみに基づいて前記手先の目標位置とのずれ、若しくは前記手先の目標位置とのずれと前記手先の目標姿勢とのずれとを求める手先ずれ算出手段と、該ずれと大きさが等しく、方向が逆向きの補正を加えた手先の位置若しくは位置及び姿勢に動作させるアクチュエータの位置を求める補正位置算出手段とを設けることによって、ロボット全体のたわみを補正して目標とする位置姿勢に位置決めすることができる。
【０００６】
又、請求項２に係わる発明は、前記たわみ算出手段において、前記各関節部分のたわみと前記リンクのたわみを分離した形で求めるようにする。さらに、請求項３に係わる発明は、前記補正位置算出手段を、たわみを考慮した順変換処理とたわみを無視した逆変換処理を交互に繰り返し行う収束計算により行うものとした。
【０００７】
さらに、請求項４に係わる発明は、さらに、前記たわみ補正による補正を行う前と行った後の前記アクチュエータの位置の差分を求める手段を設け、この差分がしきい値を越えているときには、アラーム処理を行うようにした。
請求項５に係わる発明は、前記補正位置算出手段による補正されたアクチュエータの位置を求める周期を、サーボ制御のため計算周期よりも長い周期とした。そして、請求項６に係わる発明は、この周期で求められた補正後のアクチュエータの位置を内挿補間して、指令位置として各関節部のアクチュエータに出力するようにした。
【０００８】
請求項７に係わる発明は、前記求めた手先の目標位置とのずれ、若しくは前記手先の目標位置とのずれと前記手先の目標姿勢とのずれとを表示する手段を備えるものとした。又、請求項８に係わる発明は、前記手先に取り付けられたツールに生じるたわみが生じる軸を指定する手段と、該軸回りのたわみを求め、該たわみによる手先の位置ずれ若しくは位置ずれ及び姿勢のずれを補正する手段とをさらに追加して、ツールのたわみも補正できるようにした。請求項９に係わる発明は、前記たわみ算出手段において使用される剛性パラメータは、ロボットを動作させた手先の位置又は姿勢とそのとき作用するモーメントにより求めるようにした。
【０００９】
又、請求項１０に係わる発明は、ロボットの各リンクをそれぞれのアクチュエータで駆動し、ロボット手先を押し付けて作業するロボットの制御装置であって、
ロボット手先を対象物に押し付けたときの反力を検出するセンサと、教示プログラムに基づくロボットの手先の目標位置姿勢における各関節部に発生するたわみ又は前記各関節部及び各リンクに発生するたわみをロボットの動力学に基づいて求めるたわみ算出手段と、該求められたたわみに基づいて前記手先の目標位置とのずれ、若しくは前記手先の目標位置とのずれと前記手先の目標姿勢とのずれとを求める手先ずれ算出手段と、該ずれと大きさが等しく、方向が逆向きの補正を加えた手先の位置若しくは位置及び姿勢に動作させるアクチュエータの位置を求める補正位置算出手段とを備え、ロボット手先を対象物に押し付けたときに生じる反力によるたわみを補正してロボットの位置姿勢を目標値に保持できるようにした。
【００１０】
【発明の実施の形態】
本発明の一実施形態を６軸垂直多関節ロボットを制御するロボット制御装置を例にとって、まず本発明の原理方法について説明する。他の型のロボットにおいても同様である。又、ロボットの物理パラメータとして、例えば、修正Ｄ−Ｈパラメータを用いる。他のパラメータを用いた場合も同様である。
【００１１】
ロボットの手先のワールド座標系上の位置姿勢ｘは、次の式で表される。
【００１２】
ｘ＝Ａ０Ａ１・・・・Ａ６ｘｔ
上記式において、ｘｔはツール座標系上の手先位置であり、Ａ０からＡ６は４×４の座標変換行列である。この座標変換を表すパラメータとして、修正Ｄ−Ｈパラメータ（ｄ、ａ、θ、α、β）が知られている。この修正Ｄ−Ｈパラメータは、図１に示すように、隣接する軸において、ａはＸ軸方向の軸間のオフセット値、ｄはＺ軸方向の軸間のオフセット値であり、θはＺ軸回り、αはＸ軸回り、βはＹ軸回りの回転角である。通常は、剛性パラメータとして減速器の軸回りのバネ性のみを考え、Ｚ軸回り回転θのみを問題としていたが、本発明においては、Ｘ軸回り、Ｙ軸回りの回転角α、βにもバネ性があると考え、この回転角、すなわち、ねじれをたわみとする。又、この修正Ｄ−Ｈパラメータを用いたときの座標変換行列Ａ（Ａ０、Ａ１・・・・Ａ６）は次の（１）式で表される。
【００１３】
【数１】
0003808321.tif 000002
そこで、本発明においては、ロボットの動力学の計算方法として広く知られているＮｅｗｔｏｎ−Ｅｕｌｅｒ法を用いて、まずバネ性を無視して、その時点の各軸の角度と指令速度から各関節部分にかかる力及びトルクを求める。この力及びトルクを求める計算量は、ロボットの軸数に比例して増大するが、比較的小さな計算量で求めることができる。
【００１４】
こうして求められた各関節のＸ、Ｙ、Ｚ軸回りのトルクを図２に示すように、Ｔｘ、Ｔｙ、Ｔｚとし、Ｘ、Ｙ、Ｚ軸回りのバネ定数をｋｘ、ｋｙ、ｋｚとすると、Ｘ、Ｙ、Ｚ軸回りのねじれ量Δα、Δβ、Δθは、それぞれ
Δα＝Ｔｘ／ｋｘ
Δβ＝Ｔｙ／ｋｙ
Δθ＝Ｔｚ／ｋｚ
として求められる。なお、バネ定数をｋｘ、ｋｙ、ｋｚは予め測定しておく。
【００１５】
そして、以下の式により、修正Ｄ−Ｈパラメータを計算する。
【００１６】
α＝αｓ＋Δα
β＝βｓ＋Δβ
θ0＝θ0s＋Δθ
なお、上記式において、θ0は教示操作盤上で（指令上で）軸角度が「０」のときのこの修正Ｄ−Ｈパラメータにおける回転角θを示すものである。そして教示操作盤上で軸角度がｊであるとすると、修正Ｄ−Ｈパラメータにおける回転角θは、「θ＝θ0＋ｊ」として表される。又、αｓ、βｓ、θ0s は、ねじれがないときの修正Ｄ−Ｈパラメータα、β、θであり、これらの修正Ｄ−Ｈパラメータは予めキャリブレーション等の方法で求めておく。
【００１７】
この修正Ｄ−Ｈパラメータα、β、θ0 を用いて、上記（１）式で示される座標変換行列Ａを用いて逆変換を行ってロボットの手先（ツール先端点）の目標位置ｘから各軸角度θを求める。この各軸回転角θを求めるときは、逆変換と順変換を交互に行い、手先位置姿勢がの差分が所定値以下まで収斂するまで行い、所定値以下となったときの回転角θより指令回転角ｊを求め記憶する。この回転角ｊは、関節部のＸ、Ｙ、Ｚ回りのねじれ（たわみ）を考慮した上で、ロボットの手先（ツール先端点）を目標点に動作させることができるものとなる。
【００１８】
上述した説明では、弾性が各関節部のＸ、Ｙ、Ｚ軸回りに存在するとしたものであるが、さらに、それ以外の部分、例えばアーム（リンク）の途中に弾性がある場合には、その部分にダミーの座標系を作り、ロボットの軸数をダミーを設けた数だけ増えたものとして、上述した方法により、ねじれ（たわみ）を考慮した上で、ロボットの手先（ツール先端点）の目標点を求める。
【００１９】
又、上述した説明では、リンク部分のたわみを３次元の座標系の関節部のねじれで仮定して説明している。リンク自体がたわむことにより、変化する姿勢の影響が含まれていない。減速器部分（関節部）のねじれとリンク全体におよぶたわみを別に考え、リンク全体の部分は姿勢に影響を及ぼすものとして、姿勢への影響への補正を与えるようにする。この場合、図３に示すように、リンクＬが関節においてΔθたわむと共にリンクＬ全体が均等にたわむことによりリンク先端点がγだけ回転した位置にきたとする。この時リンク先端の姿勢を考えると、図３に示すように直線Ｑで近似した場合に対して、さらにγだけ姿勢が回転している。この場合の座標変換行列Ａは次の（２）式となる。
【００２０】
【数２】
0003808321.tif 000003
リンク全体のたわみをも考慮する場合は、上記（２）式で示される座標変換行列Ａを用いる。なお、この場合、たわみがないときのリンク全体のたわみは「０」である。すなわち、γs＝０である。他は上述した関節部のねじれによる補正の場合と同一である。
【００２１】
以下、本発明の一実施形態について説明する。
【００２２】
図４は本発明の一実施形態の６軸垂直多関節ロボットの制御装置の要部ブロック図であり、従来の６軸垂直多関節ロボットの制御装置と同一構成である。符号７で示されるバスに、メインプロセッサ１、ＲＡＭ、ＲＯＭ、不揮発性メモリ（ＥＥＰＲＯＭなど）からなるメモリ２、教示操作盤用インターフェイス３，外部装置用のインターフェイス６及びサーボ制御部５が接続されている。又、教示操作盤用インターフェイス３には教示操作盤４が接続されている。
【００２３】
ロボット及びロボット制御装置の基本機能を支えるシステムプログラムは、メモリ２のＲＯＭに格納されている。又、アプリケーションに応じて教示されるロボットの動作プログラム並びに関連設定データは、メモリ２の不揮発性メモリに格納される。メモリ２のＲＡＭは、プロセッサ１が行う各種演算処理におけるデータの一時記憶の記憶領域として使用される。
【００２４】
サーボ制御部５は、サーボ制御器５a1〜５a6を備えており、各サーボ制御器５a1〜５a6は、プロセッサ、ＲＯＭ、ＲＡＭ等で構成され、各軸を駆動するサーボモータの位置・速度のループ制御、さらには電流ループ制御を行っている。いわゆる、ソフトウエアで位置、速度、電流のループ制御を行うデジタルサーボ制御器を構成している。サーボ制御器５a1〜５a6の出力は各サーボアンプ５b1〜５b6を介して各軸サーボモータＭ１〜Ｍ６を駆動制御する。なお、図示はしていないが、各サーボモータＭ１〜Ｍ６には位置・速度検出器が取り付けられており、該位置・速度検出器で検出した各サーボモータの位置、速度は各サーボ制御器５a1〜５a6にフィードバックされるようになっている。又、入出力インターフェイス６には、ロボットに設けられたセンサや周辺機器のアクチュエータやセンサが接続されている。
【００２５】
上述したロボット制御装置の構成は、従来のロボット制御装置の構成と何等変わりはない。本実施形態は、このようなロボット制御装置において、ロボットの関節部及びリンクにおけるたわみを考慮し、このたわみ分を補正して各軸のアクチュエータであるサーボモータＭ１〜Ｍ６を駆動し、ロボットの手先（ツール先端点）を正確に目標位置姿勢どおりに駆動制御しようとするものである。
【００２６】
図５は、このロボット制御装置のプロセッサ１が実行する、関節部のたわみを考慮してロボットの手先（ツール先端点）の目標位置姿勢を求める補正処理のフローチャートである。この処理はサーボ制御における計算周期と同じ又はそれよりも長い所定周期で実行されるものである。なお、各種設定値は予め設定しておく。又各関節部の座標系における各軸回りのねじれがないときの修正Ｄ−Ｈパラメータαｓ、βｓ、θ0s はキャリブレーションによって予め求め、設定されているものとする。さらに、各関節部の座標系における各軸回りの剛性パラメータのバネ定数ｋｘ、ｋｙ、ｋｚも予め測定し設定しておく。
【００２７】
この剛性パラメータと各関節部の各軸回りのモーメント及び手先位置姿勢は次の関係にある。
【００２８】
ｆ（モーメント、剛性）＝手先位置又は姿勢
これより、
剛性＝ｆ−１（手先位置又は姿勢、モーメント）
よって、ロボットを動作させ、手先位置姿勢を測定し、かつ、Ｎｅｗｔｏｎ−Ｅｕｌｅｒ法等により各軸回りのモーメントを求め上記剛性パラメータｋｘ、ｋｙ、ｋｚを求め設定しておく。
【００２９】
そこで、まず、教示プログラムを実行し、現時点で指令されている目標位置姿勢ｘ、すなわち、教示点若しくは補間点ｘを読むと共に初期値ｘｓｔとして記憶する（ステップＳ１）。なお、この実施形態では、教示プログラムではワールド座標系に基づいた直交座標系での位置姿勢ｘが指令されているものとする。
【００３０】
次に、この時点における各関節部の角度、各関節部の実回転速度を読み取る（ステップＳ２）。この読み取った各関節部の角度、各関節部の実回転速度に基づいてＮｅｗｔｏｎ−Ｅｕｌｅｒ法を用いて各関節部毎に各軸回りのトルクＴｘ、Ｔｙ、Ｔｚを求める（ステップＳ３）。さらに、この各軸回りのトルクＴｘ、Ｔｙ、Ｔｚを、予め測定され設定されている関節部の各軸のバネ定数ｋｘ、ｋｙ、ｋｚでそれぞれ割り、各関節部毎にＸ、Ｙ、Ｚ軸回りのねじれ、すなわち、たわみΔα、Δβ、Δθを求める（ステップＳ４）。
【００３１】
このねじれ（たわみ）Δα、Δβ、Δθに、それぞれキャリブレーションによって求められ設定されているねじれがないときの修正Ｄ−Ｈパラメータαｓ、βｓ、θ0ｓを加算して、ねじれ（たわみ）Δα、Δβ、Δθを考慮した修正Ｄ−Ｈパラメータα、β、θ0 を求める（ステップＳ５）。そして、まず、ねじれがないときの修正Ｄ−Ｈパラメータαｓ、βｓ、θ0ｓを用いてステップＳ１で読み込んだ目標位置姿勢ｘから逆変換して回転角θ’を求め、この回転角θ’を補正開始時の回転角θstとして記憶する（ステップＳ６）。
【００３２】
次に、カウンタＮを「０」にセットして（ステップＳ７）、ステップＳ５で求めた修正Ｄ−Ｈパラメータα、β、θ0 を用いて、ステップＳ６で求めた回転角θ’から順変換を行って、位置姿勢ｘ’を求め（ステップＳ８）、求めた位置姿勢ｘ’からステップＳ１で読み込んだ目標位置姿勢の初期値ｘｓｔを減じてその差分Δｘを算出する（ステップＳ９）。この差分Δｘの絶対値が予め設定されている基準値Ｔより小さいか、又はカウンタＮの値が設定値Ｎmax を越えているか判断し（ステップＳ１０）、差分Δｘの絶対値が基準値Ｔを越えていて、かつ、カウンタＮの値が設定値Ｎmax を越えていなければ、現在の目標位置姿勢ｘからステップＳ９で算出した差分Δｘを減じて、新しい目標位置姿勢ｘとする（ステップＳ１１）。
【００３３】
次に、再びねじれがないときの修正Ｄ−Ｈパラメータαｓ、βｓ、θ0ｓを用いてステップＳ１１で求めた目標位置姿勢ｘから逆変換して回転角θ’を求め更新し（ステップＳ１２）、カウンタＮを１カウントアップし（ステップＳ１３）、ステップＳ８に戻る。
【００３４】
以下、ステップＳ１０で差分Δｘの絶対値が基準値Ｔより小さくなるか、又は、カウンタＮの値が設定値Ｎmax を越えたと判断されるまで、ステップＳ８からステップＳ１３の処理を繰り返し実行する。すなわち、順変換、逆変換を交互に繰り返し実行し、新しい目標位置姿勢を求めると共に、先に求めた目標位置姿勢の初期値ｘｓｔと順変換して新たに求めた位置姿勢ｘ’との差分Δｘの絶対値が基準値Ｔより小さくなるまで実行する。差分Δｘの絶対値が基準値Ｔより小さくなるか、若しくは、カウンタＮの値が設定値Ｎmaxとなると、ステップＳ１０からステップＳ１４に移行し、現在求められている回転角θ’を最終回転角θとして記憶する（ステップＳ１４）。
【００３５】
さらに、現在の差分Δｘを表示する（ステップＳ１５）。すなわち、目標手先位置ｘ、ｙ、ｚとの差分、目標姿勢ｗ、ｐ、ｒとの差分を教示操作盤４の表示部や図示していない、このロボット制御装置に接続されたパーソナルコンピュータの表示部に表示する（ステップＳ１５）。
【００３６】
次に、最終回転角θと、ステップＳ６で求めた補正開始時の回転角θstとの差の絶対値が設定しきい値θmax 以上か判断し（ステップＳ１６）、しきい値θmax 以上であれば、アラームを出力し、ロボットにブレーキをかけ各軸サーボモータに減速のトルクの命令を出力し急停止させる等のアラーム処理を行う（ステップＳ１８）。一方、設定値θmax 以上でなければ、最終回転角θからステップＳ５で求めた初期の回転角θ0を減じて、教示操作盤上の回転角（教示指令上の回転角）ｊを求め記憶する（ステップＳ１７）。
【００３７】
次に、図３に示すような、リンク全体がたわむ点をも考慮して補正する場合の第２の実施形態について説明する。
この第２の実施形態においては、図５に示す処理フローチャートで示される処理と相違する点は、次の点である。
▲１▼．ステップＳ４において、Δα、Δβ、Δθ以外に図３に示すようなリンク全体のたわみ角γをも算出する。このたわみ角γは、ステップＳ３で求めたＺ軸回りのトルクＴｚを予め設定されている、リンクのたわみを求めるための係数ｋｚ’で割ってたわみ角γを求める。
【００３８】
γ＝Ｔｚ／ｋｚ’
▲２▼．ステップＳ６，ステップＳ１２、ステップＳ８の逆変換処理、順変換処理は（２）式で示す座標変換行列Ａを使用する。
▲３▼．ステップＳ６，ステップＳ１２の逆変換処理では、（２）式で示す座標変換行列Ａにおけるγの値は「０」とする。すなわち上述したように、たわみのない状態であるからγs＝０とするものである。
【００３９】
以上の点が相違するのみで他は前述した関節部のたわみのみを考慮した場合と同一である。
【００４０】
すなわち、当該時点で指令されている目標位置姿勢（教示点、補間点）ｘを読み出すと共に、各関節の軸角度、実軸速度を読み取り、Ｎｅｗｔｏｎ−Ｅｕｌｅｒ法を用いて関節部の各軸回りのトルクＴｘ、Ｔｙ、Ｔｚを求める。これらのトルクＴｘ、Ｔｙ、Ｔｚを予め設定されているバネ定数ｋｘ、ｋｙ、ｋｚで割り各軸回りのねじれΔα、Δβ、Δθを求めると共に、トルクＴｚを設定係数ｋｚ’で割ってリンクのたわみ角γを求める（ステップＳ１〜Ｓ４に対応）。
【００４１】
γ＝Ｔｚ／ｋｚ’
次に、ねじれがないときの修正Ｄ−Ｈパラメータαs、βs、θ0sにそれぞれΔα、Δβ、Δθを加算して、たわみ（ねじれ）を考慮した修正Ｄ−Ｈパラメータα、β、θ0を求め、さらにγについては先に、γ＝Ｔｚ／ｋｚ’として先のステップＳ４で求めた値とする（ステップＳ５に対応）。
【００４２】
次に、修正Ｄ−Ｈパラメータαs、βs、θ0s及びγ＝０を用いて２式の座標変換行列Ａより逆変換を行って目標位置姿勢ｘから回転角θ’を求める。この回転角θ’を補正開始時の回転角θstとして記憶する（ステップＳ６に対応）。
【００４３】
修正Ｄ−Ｈパラメータα、β、θ0、及びγの値を用い２式の座標変換行列Ａを用いて順変換してθ’より位置姿勢ｘ’を求める。目標位置姿勢ｘとの差Δｘ（＝ｘ’−ｘ）を求め、目標位置姿勢ｘからこの差Δｘを引き新たな目標位置姿勢として、パラメータαs、βs、θ0s及びγ＝０を用いて２式の座標変換行列Ａより逆変換を行って目標位置姿勢ｘから回転角θ’を求め更新する（ステップＳ８〜Ｓ１３に対応）。以下、この順変換と逆変換を交互に行い、上記差Δｘが基準値Ｔより小さくなるか、又は上記順変換、逆変換の繰り返し処理が設定数Ｎmaxを越えたときには、求めた回転角θ’を最終的回転角θとして記憶し、上記目標位置姿勢と求めた位置姿勢との差Δｘを表示器に表示し、回転角θ補正処理開始時の回転角θstがしきい値θmax以上か判断し、以上ならばアラーム処理を行い、以上でなければ、求めた回転角θから初期の回転角θ0を減じて教示操作盤に表示する回転角、すなわち、指令の回転角ｊを求める（ステップＳ１４〜Ｓ１８に対応）。
【００４４】
以上が、関節部のたわみ（ねじれ）、さらには、関節部のたわみ（ねじれ）にリンク自体のたわみをも考慮して、これらのたわみを補正してロボットを目標位置姿勢に位置決めするための各関節部の回転角ｊを求める処理である。これら処理を、サーボ制御の割り込み周期よりも長い割り込み周期で行う場合もある。この場合には、教示プログラムを実行し得られた目標指令位置姿勢としての教示点、補間点に対して上記処理を実行し、まず最初は、上記処理を２回行い、たわみ（ねじれ）補正された回転角ｊを２つ求め、その間を補間して、各関節部のサーボ制御器５a1〜５a6に出力しサーボモータＭ１〜Ｍ６を駆動制御する。以後は、順次、当該周期の上記処理によって得られた新たな回転角ｊと１つ前の周期の上記処理によって得られた回転角ｊとの間を内挿補間して、各関節部のサーボ制御器５a1〜５a6に出力する。すなわち、上記たわみ（ねじれ）補正周期の２周期分遅れて、実際のロボットは動作開始することになる。
【００４５】
上記実施形態では、ロボットの関節部における座標系のＸ、Ｙ、Ｚ軸回りのねじれ（たわみ）、さらにはリンク自体のたわみについて補正するようにしたものであるが、関節部以外の部分で弾性が有り、この部分でねじれ（たわみ）が生じるような場合には、この部分にダミーの座標系（関節部）を作り、ロボットの軸数が増加したものとして、上述した図５の処理を行えば良い。この場合、実際にはこのダミー部分にはアクチュエータとしてのサーボモータがないものであるから、指令する回転角ｊは「０」である。
【００４６】
又、ロボットのアーム先端に取り付けられるツールにおいて剛性が弱い部分がある場合には、この弱い部分の軸を教示操作盤４より指定し入力する。例えば、この軸を直線式で表して入力する。又このツールの重さ形状なども入力しておく。そして、この軸の直線式に基づいて、この剛性の弱い部分をダミーの関節部として上述した方法で、この弱い部分のたわみも補正する指令を出力するようにする。
【００４７】
さらに、上記実施形態においては、関節部の座標系における各軸回りのトルクＴｘ、Ｔｙ、ＴｚをＮｅｗｔｏｎ−Ｅｕｌｅｒ法を用いて計算により求めたが、力センサやトルクセンサを用いてこの各軸回りのトルクＴｘ、Ｔｙ、Ｔｚを検出しても良く、この場合には、上記図５に示す処理においては、ステップＳ２，Ｓ３の処理をこれらセンサから得られた力またはトルクとすれば良い。
【００４８】
さらに、本発明はバリ取り等のロボットの手先を対象物に押し付け作業動作を行うロボットにおいて、この押し付けによって生じるたわみについて補正する場合にも適用できるものである。この場合には、上述したようなセンサで押し付けによる反力を検出し、この反力に基づいて、上述した補正処理を行うことにより、外力によるたわみから発生する手先位置のずれをも補償するようにする。
【００４９】
さらに、上記実施形態では、教示プログラムが直交座標系上の位置でプログラムされている例を示したが、各関節軸の回転角で教示されている教示プログラムの場合では、ステップＳ１で、各関節軸の指令回転角ｊを読み、回転角θに変換し、これをステップＳ６では、回転角θ’及びθstとして、回転角θ’より順変換して直交座標系上の位置姿勢ｘを求める処理となる。他は図５に示す処理と同一である。
【００５０】
【発明の効果】
本発明においては、多関節の柔軟アームのロボットにおいて発生するたわみを補正して、該ロボットを高精度に高速で動作させることができる。関節部のねじれだけでなく、リンク全体がたわむことによる変化する姿勢の影響をも考慮した補正を行うことにより、更に高精度な動作を行うことができる。
万一、補正量が異常に大きくなった場合にも、アラームを発生させることにより、急激な動作を行わないようにすることができる。
【００５１】
補正の計算時間がサーボ制御のための計算周期では間に合わない場合には、サーボ制御のための計算周期よりも長い周期で補正の計算を行うことができ、サーボ制御自体の性能を下げる必要がない。
又、手先の目標位置とのずれ、若しくは前記手先の目標位置とのずれと前記手先の目標姿勢とのずれとを表示することにより、ロボットの精度や剛性を知ることができる。
ロボット本体だけではなく、必要に応じて、ロボットの手先に取り付けられたツールに生じるたわみを補正することもできる。
取得したデータから剛性パラメータを簡単に計算で求めることができ、わずらわしい調整作業が不要である。
ロボット手先を押し付けて作業するアプリケーションにおいても、高精度な動作が可能である。
【図面の簡単な説明】
【図１】修正Ｄ−Ｈパラメータの説明図である。
【図２】関節部の座標系における各軸回りのトルクの説明図である。
【図３】リンク自体のたわみの説明図である。
【図４】本発明の一実施形態のロボット制御装置の要部説明図である。
【図５】同実施形態におけるたわみ（ねじれ）補正処理のフローチャートである。
【符号の説明】
１  メインＣＰＵ
２  メモリ
４  教示操作盤
５  サーボ制御部
Ｍ１〜Ｍ６  サーボモータ
【図１】
0003808321.tif 000004
【図２】
0003808321.tif 000005
【図３】
0003808321.tif 000006
【図４】
0003808321.tif 000007
【図５】
0003808321.tif 000008