JP4639417B2

JP4639417B2 - ロボットの制御装置

Info

Publication number: JP4639417B2
Application number: JP2000013108A
Authority: JP
Inventors: 賢一安田; 康之井上; 英夫永田
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 2000-01-21
Filing date: 2000-01-21
Publication date: 2011-02-23
Anticipated expiration: 2020-01-21
Also published as: JP2001198870A

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボット制御装置における力センサフィードバックを用いた制御装置に関し、特に組み立て作業などの接触作業を行なうのに適した制御を実現する制御装置に関する。
【０００２】
【従来の技術】
第６図のようにロボット６０１が作業対象６０４に対して力を加えながら作業を行なう場合がある。このとき、手先に取り付けられた力センサ６０２の情報に応じてロボットアーム先端６０３から作業対象６０４に加わる力を制御することで、所望の力を加えながら作業を行なうことが可能である。このように、ロボットアームが対象物に対して接触作業を行なうための力制御法の１つとして、インピーダンス制御が知られている。
これは、第７図に示すように、ロボットアーム７０４の動的な応答が仮想的に設定された慣性７０１、粘性７０２、剛性７０３からなる機械的なインピーダンスになるように、作業対象物７０５に対する反力に応じてロボットアーム先端の運動を制御するものである。この制御はロボットアームの手先に備えた力センサ６０２によって外力を計測し、第８図のように目標インピーダンスモデル８０２によって目標軌道に対する偏差を計算し、目標軌道と偏差との和を運動指令としてサーボコントローラ８０１に与える制御方式である。
第９図に従来のインピーダンス制御のブロック図を示す。
軌道生成部９０１では現在のロボットの指令位置に対して、指定座標系から基準座標系に変換された増分値が生成される。基準座標系は通常ロボットのベースを中心とした直交座標系で表現されるため、実際のロボットの動作を行なうためにロボットの関節座標系の角度増分値に変換される。その後、モータへの回転指令として、各関節の位置・速度制御部９０２へ送られる。ロボットアーム先端に装着された力センサ９０７により検出された反力は作業座標系での力・トルクへ座標変換され、インピーダンス制御部９１０によって目標軌道に対する修正量を作成する。インピーダンスモデルは式（１）で表される。
【数１】

ここで、Ｍは仮想慣性、Ｂは仮想粘性、Ｋは仮想剛性であり、
ｘは平衡点からの変位（位置修正量）を、
Ｆは計測した力を示す。
この式から修正量ｘを演算し、軌道生成部９０１から出された位置指令値にこの修正量を加えたものを新たな位置指令値として位置・速度制御部９０２へ出力する。位置・速度制御部９０２ではこの新たな位置指令値を基にロボット９０６への制御指令を出力し、アンプ９０５で増幅された後、ロボット９０６に与えられる。このように、ロボットアーム先端が検出した反力に応じて位置を制御することにより、対象物に加わる力を制御することが可能である。
この制御方式は上記インピーダンスモデルによる偏差を速度や位置の指令としてサーボ系に入力することになるので、現在の位置制御方式の産業用ロボットで容易に実現できる。
【０００３】
【発明が解決しようとする課題】
しかしながら、仮想粘性を小さくとると、固い対象物に接触したときに、制御系の安定性が低下することが知られている。
また、インピーダンス制御に限らず、力制御を適用したロボットアームを剛性の高い対象物に接触させると、制御系が不安定になることが知られている。この不安定化の主な要因として以下の点が挙げられる。
１）剛性の高い対象物との接触は、力制御ループのゲインが高くなることを意味し、力フィードバックに対するロボットアームの応答の遅れ（位相遅れ）により不安定になる。
２）インピーダンスモデルの仮想粘性を小さくすると、共振周波数付近で急激に位相遅れが生じる。
従って、インピーダンス制御の場合は、接触を安定させるために仮想粘性を大きく設定する必要があり、設定の自由度が小さくなる。また、仮想粘性が大きいと外力に対する応答性が低下するため接触力が大きくなり、対象物やロボットアームを破損する恐れがある。このため、対象物への接近速度を十分に小さくする必要がある。
以上のようなことにより、安定性が失われてロボットアームの跳ね返りが起こったり、あるいは対象物に過大な接触力が発生することで、高精度な接触作業が困難になるといった問題があった。
また、これはインピーダンス制御だけでなく、他の力制御手法にも同様の問題があった。
さらに、力センサが装着されている部分よりも根元の部分は力センサで反力を検出できないため、ロボットアームが周辺物体などに接触した場合は大きな制御偏差が生じ、対象物やロボット自体が破損するおそれがあった。
そこで本発明は、固い対象物でも安定して高精度な力制御を実現すると同時に、アームのどの部分に接触しても柔軟な特性をもつことを実現するための制御装置を提供するものである。
【０００４】
【課題を解決するための手段】
上記問題を解決するため、請求項１記載のロボットの制御装置の発明は、手先に力センサ（１０７）が装着されたロボット（１０６）の位置および速度の状態フィードバックを位置・速度制御部（１０２）で施しトルク指令を出力して前記ロボット（１０６）の関節を駆動するモータの制御回路を備え、前記力センサ（１０７）からのフィードバックにより作業座標系の位置修正量をインピーダンス制御部（１１０）で求め軌道生成部（１０１）が出力するモータの動作指令と前記位置修正量との和を前記位置・速度制御部（１０２）に出力する制御装置において、
前記モータの動作指令からモータの加速トルクおよび速度を維持するための速度維持トルクを算出し、前記加速トルクと速度維持トルクを加算してモータの運動に必要なトルクを算出する運動指令トルク算出手段（１０８）と、
前記インピーダンス制御部（１１０）から出力される作業座標系の位置修正量とあらかじめ設定した慣性・粘性・剛性パラメータから、位置修正に必要なトルクを算出する修正動作トルク算出手段（１０９）と、
前記運動指令トルク算出手段（１０８）からの前記モータの運動に必要なトルクと前記修正動作トルク算出手段（１０９）からの前記位置修正に必要なトルクとを加算したトルクに所定の幅を与えてトルク制限値とし前記トルク指令を制限するトルク制限処理部（１０３）と、を有し、
前記トルク制限処理部（１０３）から出力するトルク指令で前記ロボットの関節を駆動することを特徴とするものである。
また、請求項２記載のロボットの制御装置の発明は、手先に力センサ（２０７）が装着されたロボット（２０６）の位置および速度の状態フィードバックを位置・速度制御部（２０２）で施しトルク指令を出力して前記ロボット（２０６）の関節を駆動するモータの制御回路を備え、前記力センサ（２０７）からのフィードバックにより作業座標系の位置修正量をインピーダンス制御部（２１０）で求め軌道生成部（２０１）が出力するモータの動作指令と前記位置修正量との和を前記位置・速度制御部（２０２）に出力する制御装置において、
前記モータの動作指令からモータの加速トルクおよび速度を維持するための速度維持トルクを算出し、前記加速トルクと速度維持トルクを加算してモータの運動に必要なトルクを算出する運動指令トルク算出手段（２０８）と、
前記インピーダンス制御部（２１０）から出力される作業座標系の位置修正量とあらかじめ設定した慣性・粘性・剛性パラメータから、位置修正に必要なトルクを算出する修正動作トルク算出手段（２０９）と、
前記運動指令トルク算出手段（２０８）からの前記モータの運動に必要なトルクに所定の幅を与えてトルク制限値とし前記トルク指令を制限するトルク制限処理部（２０３）と、を有し、
前記修正動作トルク算出手段（２０９）からの前記位置修正に必要なトルクを前記トルク制限処理部（２０３）から出力するトルク指令に加え、
前記ロボット（２０６）の関節を駆動することを特徴とするものである。
【０００５】
【発明の実施の形態】
以下、本発明の実施の形態を図に基づいて説明する。
第１図は本発明の第１の実施の形態を示している。
１０１は軌道生成部、１０２は位置・速度制御部、１０３はトルク制限処理部、１０４は重力補償トルク演算部、１０５はアンプ、１０６はロボット、１０７は力センサ、１０８は運動指令トルク演算部、１０９は修正動作トルク演算部、１１０はインピーダンス制御部である。
軌道生成部１０１では現在のロボットの指令位置に対して、指定座標系から基準座標系に変換された増分値が生成される。基準座標系は通常は、ロボットのベースを中心とした直交座標系で表現されるため、実際のロボットの動作を行なうためにロボットの関節座標系の角度増分値に変換される。その後、モータへの回転指令として、各関節の位置・速度制御部１０２へ送られる。
位置・速度制御部１０２の処理系では位置制御および速度制御が施される。位置制御では一般的に比例制御が用いられ、速度制御系では比例積分制御などが用いられる。
ここで、まず、ロボットアーム先端に力が加わっていない場合を説明する。
運動指令トルク演算部１０８の制御系は、運動指令に対して必要となるトルクを演算する部分である。通常のロボットの運動で必要となるトルクは、
（１）ロボットアームとロボットアーム先端の両者の慣性の負荷を加速する加速トルク、
（２）通常の運動状態で速度を維持するための速度維持トルクの２種類である。
加速トルクは慣性と加速度の積から必要となるトルクが算出される。
慣性の情報は、以下のいずれかの方法が用いられる。
１）ロボットの動きに応じて変動する値を演算により求める。
２）適応オブザーバなどのパラメータ同定手法を用いて推定する。
３）代表的な慣性の値を用いる。
また、速度維持トルクはほぼ動摩擦の値に等しい。
従って、あらかじめ求めた速度と摩擦の関係から、現在の指令速度によって必要なトルクを求めることができる。ただし、上記加速トルクと速度維持トルクはインピーダンス制御部１１０からの出力である動作修正量を加える前の、軌道生成部１０１の出力に対するものである。
この運動指令トルク演算部１０８で算出されたトルクの加算値はロボットが運動を行なうトルクとほぼ等しい値である。計算結果はトルク制限処理部１０３へ入力される。ここでは、前段の演算をもとに位置・速度制御部１０２で発生するトルクの制限値を演算し、トルクの制限処理を行なう。制限値は運動指令トルク演算部１０８で演算された値に適度な幅を設けることにより求めることができる。ここで適度な幅を設ける理由は、指令トルクと発生トルクが必ずしも一致していないことがあるため、その誤差を吸収すること、また、時間的な発生トルクのずれを吸収することのためである。時間的なずれの補償とは、指令値により発生トルクを演算した瞬間のトルクと位置・速度制御部１０２で発生するトルクが時間的なずれを生ずる可能性があり、その誤差をトルクの制限の幅で吸収することである。この時間的なずれは運動指令トルク演算部１０８の出力に時間遅れを補償するフィルタを挿入することでも吸収することができる。フィルタは一般的な１次遅れフィルタや２次遅れフィルタにより調整することが可能である。
このようなトルク制限を設けることによる物理的な作用の説明を行なう。
位置・速度制御部１０２ではロボットに外部からの力が作用した場合、位置偏差と速度偏差が発生する。位置偏差は定数倍され速度指令となり速度偏差は比例積分制御され、その出力はモータのトルク指令となる。ロボットに特別の外力が作用しない場合、モータで発生すべきトルクは加速トルク、速度維持トルクの２つである。
通常の動作でロボットが運動するためには上記のトルクを発生すれば良い。ところが、ロボットに外部から力が作用する（またはロボット自身が外部に接触する）場合には、外力によりロボットが動作し前述の制御偏差が発生することになる。従って、その時の指令トルクは運動指令トルク演算部１０８で演算を行ったトルクの制限域から逸脱したものになる。制限がなければその可能な限りのトルクが発生することになる。
適度に制限された場合には制限内のトルクが発生し、外部からの力により偏差を許すことになり、すなわちロボットが外力により動くことになる。また、重力方向に動くロボットではロボットの姿勢に応じて重力トルクが重力補償トルク演算部１０４で演算され補償されるため、トルク制限処理部１０３により位置・速度制御部１０２のトルクが小さく抑えられた場合でも、ロボットアームが重力で落下するなどのことは起こらない。
【０００６】
次に、トルクの制限の方法を第４図を用いて具体的に述べる。
第４図（ａ）はロボットアームの１つの関節４０１の運動に着目した場合である。関節４０１の運動は、時点Ｐ１で加速開始し、時点Ｐ２で等速開始し、時点Ｐ３で減速開始し、時点Ｐ４で停止するものであるとする。そのための具体的な運動指令は、第４図（ｂ）で示されるような加速、等速、減速の運動指令で与えられるものとする。第４図（ｃ）は運動指令トルク演算部１０８（図１）の演算結果であり、第４図（ｄ）は時間遅れを考慮して加速トルク演算結果（点線）に一次フィルタを通した結果（太線）の図である。また、第４図（ｅ）の太線は第４図（ｄ）の太線と同じで一次フィルタを通した結果を表し、細線はトルク制限処理部１０３でのトルク制限の上限値と下限値を表している。すなわち、このトルク制限によって、運動指令に対するトルクを補償しつつ、ロボットアームが周辺物体に衝突して制御偏差が大きくなった場合でも、外力に応じてロボットアームが柔軟に倣うことが可能になる。このトルク制限処理部１０３はセンサを用いない柔軟制御を実現するものであるので、ロボットアームのどの部分でも柔軟な特性を持たせることができる。
次に、ロボットアーム先端が対象物に接触した場合について説明する。
インピーダンス制御部１１０（図１）は力センサフィードバックによる力制御を行なうブロックである。ロボットアーム先端に取り付けられた力センサ１０７によって計測された反力は作業座標系に座標変換された後、所望のインピーダンスモデルに応じて、位置修正量を演算する。位置修正量は座標変換によって、ロボットの各関節毎の増分値に変換され、軌道生成部１０１から生成された増分値に加算される。
さらに、修正動作トルク演算部１０９において、目標インピーダンスを実現するためのトルクを演算する。ここでその一例を示す。ここで上記運動指令トルク演算部１０８と同じように加速トルク、速度維持トルクを演算したのでは、速度、加速度自体が大きく変動しているため、トルク制限処理部１０３に入力するトルクも大きく変動するものとなってしまう。そこで、インピーダンスを実現するためのトルクを式（２）のようにする。
【数２】

ここで、Ｔ_impはインピーダンスを実現するためのトルク、
Ｔ_fricは各関節の摩擦分のトルク、
Ｊ^Tはヤコビ行列の転置行列、
Ｆ_refはインピーダンス分（慣性、粘性、剛性）の力である。
Ｆ_refはインピーダンス制御部１１０で設定したインピーダンスモデルに応じて演算可能である。また、Ｔ_fricはインピーダンス制御部１１０の出力の速度で演算を行なうと、その速度が安定したものでないため、例えば、Ｆ_refの情報をもとに演算を行なう。
ここで、修正動作トルク演算部１０９から出力された値をトルク制限処理部１０３に入力することにより、トルク制限処理部１０３において、運動指令トルクと修正動作トルクを加えた、上限・下限のトルク制限値が生成される。
次にトルク制限処理部１０３の機能を説明する。ロボットアーム先端が対象物に接触して作業を行なう場合、もし対象物が固い物体であれば跳ね返りや振動など制御的に不安定な状態になる。上記で述べたのと同様に、このときトルク制限がされていなければ、可能な限りのトルクが発生し、大きな跳ね返りや振動が起こってしまう。トルク制限処理部１０３において適度に制限された場合には、位置・速度制御部１０２から出力されたトルク指令が上限、あるいは下限のトルク制限値に張り付いてしまうか、もしくは振動したとしてもトルク制限値以内におさまるので、安定した接触が実現される。また、ある一定の幅を持たせているので、（２）式によるインピーダンスを実現するためのトルクの演算誤差を吸収することが可能である。
【０００７】
第２図は第２の実施の形態を示している。
２０１は軌道生成部、２０２は位置・速度制御部、２０３はトルク制限処理部、２０４は重力・摩擦補償トルク演算部、２０５はアンプ、２０６はロボット、２０７は力センサ、２０８は運動指令トルク演算部、２０９は修正動作トルク演算部、２１０はインピーダンス制御部である。
上記第１の実施の形態では、修正動作トルク演算部１０９の出力をトルク制限処理部１０３へ入力していたが、ここでは第２図のように修正動作トルク演算部２０９の出力をトルク制限処理後の指令トルクに直接加えているのが特徴である。この場合も位置・速度制御部２０２からのトルク指令はトルク制限処理部２０３によりトルクが制限されているので、上記と同様の効果が期待できる。すなわち、接触安定性を保って精密な力制御が可能となる。
さらに、第３図は第３の実施の形態を示している。
３０１は軌道生成部、３０２は位置・速度制御部、３０３はトルク制限処理部、３０４は重力・摩擦補償トルク演算部、３０５はアンプ、３０６はロボット、３０７は力センサ、３０８は運動指令トルク演算部、３０９は修正動作トルク演算部、３１０はインピーダンス制御部である。
ここでは、３０４において図１の重力補償トルクの演算部１０８に加えてロボットからの状態フィードバックにより摩擦補償トルクの演算部を加えているのが特徴である。また、これを考慮して運動指令トルク演算部３０８で速度維持トルクが演算される。このように摩擦補償を加えることにより、各関節の柔軟性が向上するという効果がある。
ここではインピーダンス制御を例にとって説明したが、力センサの力フィードバックを用いるさまざまな力制御で、同様の問題解決が図れる。
【０００８】
第５図は上記各実施の形態に用いられるハードウェア構成を示している。
第１図の軌道生成部１０１が第５図の軌道発生ブロック５００と軌道生成ブロック５１０に対応している。
軌道発生ブロック５００において、ティーチングペンダントで教示したデータは不揮発性メモリ５０６に記憶される。また、ロボットの機構的、制御的なパラメータもこの不揮発性メモリに記憶されており、バスで共有化されている。軌道発生の機能はＲＯＭ５０４に格納されたプログラムをＣＰＵ５０３が実行することによって実現される。
軌道生成ブロック５１０においては、軌道生成や座標変換の機能はＲＯＭ５１３に格納されたプログラムをＣＰＵ５１２が実行することによって実現される。
また、第１図の位置・速度制御部や運動指令トルク演算部１０８、インピーダンス制御部１１０、修正動作トルク演算部１０９、トルク制限処理部１０３などが第５図の運動制御ブロック５２０のソフトウェアとして実現される。
第１図、第２図、第３図における位置・速度制御や力制御は運動制御ブロック５２０のサーボ基板５２１の中のＲＯＭ５２３にプログラムされている。このプログラムをＣＰＵ５２２が読み込んで実行することによって実際にトルク制限処理が行われ、その結果がサーボアンプ５２６に出力される。
また、力センサや力情報の取り込みはセンサブロック５３０で構成される。
力センサ５３６からの検出信号がセンサ基板５３１内のＡＤ変換部５３３に読み込まれ、ＲＯＭ５３４の命令にしたがってＣＰＵ５３２が検出信号を処理し、インピーダンス制御などの力制御手段によって位置修正量等がバス変換部５３５へ出力する。
それぞれのブロック５００、５１０、５２０、５３０は共有バス５５０によって接続されているので、センサブロック５３０からの位置修正量は共有バス５５０によって運動制御ブロック５２０へ送られ、そのバス変換部５２５を介してＣＰＵ５２２に入る。ＣＰＵ５２２はこの位置修正量を読み込んでトルク制限処理を実行し、サーボＩＦ５２４を介してその結果をサーボアンプ５２６に出力する。
サーボアンプ５２６はロボット５４０を駆動し、そのロボットのモータも回転状況はサーボモータエンコーダ５４１によってサーボアンプ５２６へフイードバックされる。
【０００９】
【発明の効果】
以上述べたように、本発明によれば、上下に設定されたトルク制限により、固い対象物との接触でも安定した接触を可能とし、高精度な接触作業を実現するロボットの制御装置を提供することができる。
さらに、力センサが検出できない部分で接触してもよい柔軟な動作が可能となるので、周辺物体に衝突しても安全なロボットを提供できる。
このように、本発明の制御装置によって力センサを用いた高精度な力制御と力センサレスの柔軟な制御の両方のメリットが享受できる。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態を示す制御ブロック図
【図２】本発明の第２の実施の形態を示す制御ブロック図
【図３】本発明の第３の実施の形態を示す制御ブロック図
【図４】本発明の作用を示す図
【図５】各実施の形態で用いられるハードウェア構成を示す図
【図６】本発明で想定する接触作業の概念を示す図
【図７】本発明に適用されるインピーダンス機構の概念を示す図
【図８】本発明に適用されるインピーダンス制御の概念を示す図
【図９】従来のインピーダンス制御ブロック図
【符号の説明】
１０１、２０１、３０１軌道生成部
１０２、２０２、３０２位置・速度制御部
１０３、２０３、３０３トルク制限処理部
１０４、２０４、３０４重力補償トルク演算部
１０５、２０５、３０５アンプ
１０６、２０６、３０６ロボット
１０７、２０７、３０７力センサ
１０８、２０８、３０８運動指令トルク演算部
１０９、２０９、３０９修正動作トルク演算部
１１０、２１０、３１０インピーダンス制御部
５００軌道発生ブロック
５１０軌道生成ブロック
５２０運動制御ブロック
５３０センサブロック
５４０ロボット
５５０共有バス

Claims

手先に力センサ（１０７）が装着されたロボット（１０６）の位置および速度の状態フィードバックを位置・速度制御部（１０２）で施しトルク指令を出力して前記ロボット（１０６）の関節を駆動するモータの制御回路を備え、前記力センサ（１０７）からのフィードバックにより作業座標系の位置修正量をインピーダンス制御部（１１０）で求め軌道生成部（１０１）が出力するモータの動作指令と前記位置修正量との和を前記位置・速度制御部（１０２）に出力する制御装置において、
前記モータの動作指令からモータの加速トルクおよび速度を維持するための速度維持トルクを算出し、前記加速トルクと速度維持トルクを加算してモータの運動に必要なトルクを算出する運動指令トルク算出手段（１０８）と、
前記インピーダンス制御部（１１０）から出力される作業座標系の位置修正量とあらかじめ設定した慣性・粘性・剛性パラメータから、位置修正に必要なトルクを算出する修正動作トルク算出手段（１０９）と、
前記運動指令トルク算出手段（１０８）からの前記モータの運動に必要なトルクと前記修正動作トルク算出手段（１０９）からの前記位置修正に必要なトルクとを加算したトルクに所定の幅を与えてトルク制限値とし前記トルク指令を制限するトルク制限処理部（１０３）と、を有し、
前記トルク制限処理部（１０３）から出力するトルク指令で前記ロボットの関節を駆動することを特徴とするロボット（１０６）の制御装置。
手先に力センサ（２０７）が装着されたロボット（２０６）の位置および速度の状態フィードバックを位置・速度制御部（２０２）で施しトルク指令を出力して前記ロボット（２０６）の関節を駆動するモータの制御回路を備え、前記力センサ（２０７）からのフィードバックにより作業座標系の位置修正量をインピーダンス制御部（２１０）で求め軌道生成部（２０１）が出力するモータの動作指令と前記位置修正量との和を前記位置・速度制御部（２０２）に出力する制御装置において、
前記モータの動作指令からモータの加速トルクおよび速度を維持するための速度維持トルクを算出し、前記加速トルクと速度維持トルクを加算してモータの運動に必要なトルクを算出する運動指令トルク算出手段（２０８）と、
前記インピーダンス制御部（２１０）から出力される作業座標系の位置修正量とあらかじめ設定した慣性・粘性・剛性パラメータから、位置修正に必要なトルクを算出する修正動作トルク算出手段（２０９）と、
前記運動指令トルク算出手段（２０８）からの前記モータの運動に必要なトルクに所定の幅を与えてトルク制限値とし前記トルク指令を制限するトルク制限処理部（２０３）と、を有し、
前記修正動作トルク算出手段（２０９）からの前記位置修正に必要なトルクを前記トルク制限処理部（２０３）から出力するトルク指令に加え、
前記ロボット（２０６）の関節を駆動することを特徴とするロボット（２０６）の制御装置。