JP4134812B2 - ロボット制御装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボットに作用する外力に倣う追従動作、すなわちコンプライアンス動作を行うためのロボット制御装置に関する。
【０００２】
【従来の技術】
従来の位置制御を行う産業用ロボットでは、その位置剛性が高く、高速度、高精度での位置制御の実現を目的として構成されている。
一方、ダイキャスト成形品であるワークの引き抜きや、作業者の誘導による直接教示においては、コンプライアンス動作が必要であり、従来からいくつかのコンプライアンス制御装置が提案されている。その中において、実用性を重視し、専用ツールと力センサを用いないで実現する手法がある。例えば、コンプライアンス動作中に、外力によって位置変化した各軸角度θ１をリアルタイムで取り込み、指定したツール座標系での位置／姿勢の現在値を演算し、そのうち、指定したベクトル成分の現在値を指令値に短時間に逐次置き換えしてゆき、これによってワークをツールで把持した状態で外力がそのワークに作用したとき、ワークを予め定める一方向に、そのワークの姿勢を変えることなく、移動させるロボット制御装置があった（例えば、特許文献１参照）。
また、動作開始時のロボット軸の静止摩擦を周期的に補償して、移動開始時の動作を滑らかにする手法も提案されていた（例えば、特許文献２参照）。
さらに、特に外力を検知するための力センサのような特別な装置を付加することなく、柔軟な制御を行うために、サーボゲインを低下させる方式も公知であった（例えば、特許文献３参照）。
そして、サーボゲインを低下させる方式を発展させた作業座標上での柔らかさ設定が可能な方式も知られている（例えば、特許文献４参照）。
また、ロボットに関する静止摩擦を補償する装置として、マスタ・スレーブ型ロボットのマスタアーム操作装置が提案されている（例えば、特許文献５参照）。
【０００３】
【特許文献１】
特開平０８−１５５８６８号公報
【特許文献２】
特開平０８−２８６７５９号公報
【特許文献３】
特開平０６−３３２５３８号公報
【特許文献４】
特開平０７−０２０９４１号公報
【特許文献５】
特許第２７６２９４３号公報
【０００４】
以下、特許文献１記載のロボット制御装置について、簡単に説明する。
図１６は特許文献１記載のロボット制御装置のサーボ制御回路１２３の具体的な構成を示す電気回路図である。
図において、ホスト制御回路１２４からライン１２８を介して各軸毎の位置指令値が減算手段１２９の一方の入力に与えられる。エンコーダ１２７からの各軸の軸角度を表す信号は、ライン１３０から、減算手段１２９の他方の入力に与えられる。減算手段１２９の出力は、係数器１３１に与えられ、ここで位置ゲインＫｐで増幅され、加算手段１３２に与えられる。エンコーダ１２７の出力はまた、微分回路１３３で微分され、係数器１３４で速度ゲインＫｖで増幅され、加算手段１３２に与えられる。加算手段１３２の出力は、積分器１３５に与えられ、積分演算が行われる。積分器１３５のゲインＧ１は、式１で示される。
【数１】

ここでＫ１は定数であり、ｓは演算子である。
積分器１３５の出力は位相補償器１３６に与えられて位相制御動作時に位相補償の演算が行われ、このゲインＧ２は式２で示される。
【数２】

ここでαは定数である。
位相補償器１３６の出力は、もう１つの加算手段１３７に与えられる。加算手段１３７には、エンコーダ１２７の出力に応答する重力補償演算回路１３８からの出力が与えられて加算され、その加算出力はライン１３９から各軸のトルク指令値として制御回路１２２の増幅回路１２６に入力される。
【０００５】
図１７は、制御回路１２２の構成および動作を設定するためのブロック図である。図において、ライン１３９を介するサーボ制御回路１２３からの各軸のトルク指令信号は、ライン１３９を経て増幅回路１２６に与えられ、これによってサーボモータ１２５が駆動される。モータ１２５の負荷であるアームには、参照符１４１で示されるように重力が作用し、したがってモータ１２５によって駆動されるアームなどの負荷には、重力１４１が引き算され、その重力１４１が、重力回路１３８の出力により、キャンセルされて補償され、アームなどが移動される。アームなどの慣性力１４３の角加速度は、積分手段１４４によって積分されることによって、角速度が求められ、さらに積分手段１４５によってその角速度を積分することによって、アームなどの回転角が求められ、この回転角はエンコーダ１２７によって前述のように検出される。
【０００６】
図１８は特許文献１記載の発明の一実施例の動作を簡略化して示す図である。ロボット１０９に適用作業をさせるとき、ロボット１０９のツール１１０に加わる外力Ｆに対して、任意の希望する移動方向１２０にそのツール１１０が逃げ動作をさせてコンプライアンス機能を達成するにあたり、外力に対して受動的にそのコンプライアンス機能が達成され、ツール１１０は図１８（１）に示されるようにコンプライアンス動作開始直前の一定の姿勢を保持し、その移動方向１２０は、ツール１１０の座標系で定める方向とし、コンプライアンス度を調整可能とし、各軸の重力補償は、計算式によって補償する構成とし、ワーク１１６への反力は、粘性抵抗力とし、静摩擦力が大きい基本軸である第１軸４などもまた動くように構成し、安全対策として、各軸の動作制限角度を設定するように構成される。このコンプライアンス度の調整のために、図１６に示されるサーボ制御回路１２３では、係数器１３１および１３４における位置ゲインＫｐおよび速度ゲインＫｖを調整することによって達成される。
外力によるツール１１０の各軸の変位を、エンコーダ１２７によって検出することによって、力センサなどを用いることが不要になる。
このように、従来のロボット制御装置は、エンコーダ１２７の位置情報用いて、位置変位が発生した時点で目標指令値を逐次更新することでコンプライアンス制御動作を実現するものである。
【０００７】
【発明が解決しようとする課題】
しかしながら、特許文献１に示す従来のロボット制御装置は、予め位置変位が発生し、それに基づく位置指令が与えられていることが前提となっており、初期の位置変位が発生する以前における、つまりロボットが静止した状態で、任意の方向から外力が作用した場合の静止摩擦力の低減については考察されておらず、静止摩擦力分の硬さがロボット側に残ったままである。
また、特許文献２に示す従来装置においては、各軸の補償方向を同期させて制御することまで考察されておらず、実際の適用にあたっては、ロボット先端の直交座標系において目的とする方向のコンプライアンスが得にくいという問題があった。
そのため、従来の両手法とも周辺機器と衝突した場合、静止摩擦分のダメージを受けてしまう問題があった。
また、作業者が直接教示をするような場合には、静止摩擦分の抵抗力が作業者に作用して操作性が悪化するといった問題があった。
そこで、本発明はこのような問題点に鑑みてなされたものであり、静止摩擦を、その方向性も考慮して、各軸の同期も考慮して効果的に補償し、ロボットが静止状態においても十分なコンプライアンス性を保証するロボット制御装置を提供することを第１の目的とする。
【０００８】
また、特許文献３および４に記載のこれらの方式ではロボット関節軸で発生する静止摩擦抵抗をそもそも考慮しておらず、コンプライアンス制御の柔軟さを発揮するには外力が静止摩擦以上である必要があり、操作者がダイレクト操作しようとしても、ロボットが動かないという問題があった。
また、ロボットに関する静止摩擦を補償する装置として、マスタ・スレーブ型ロボットのマスタアーム操作装置が上記特許文献５に記載されている。しかしながらこの特許文献５記載の操作装置も次の理由から問題があった。
【０００９】
以下、特許文献５記載の操作装置の問題点について、図面を用いて簡単に説明する。
図１９は１軸のマスタアーム操作装置を示している。
同図においいて、（Ａ）、（Ｂ）、（Ｃ）は何れも操作後の停止状態を示し、静止摩擦補償トルクの方向を説明するための図である。
図１９において、２２１はマスタアームであり、２２２はマスタアーム２２１の一端に設けた操作者が把持するグリップであり、２２３はマスタアーム２２１の軸であり、２２４は軸２２３に嵌装された歯車（１）であり、２２５はモータであり、２２６はモータ２２５の出力軸であり、２２７は出力軸２２６に嵌着された歯車（２）であり、２２８はモータ２２５に直結して設けた速度検出器である。
グリップ２２２は操作者の把持有無を検知するスイッチがある。マスタアームの操作者はグリップ２２２を把持してマスタアーム２２１を動かし、モータ２２５の速度検出器２２８に操作信号を送ることでスレーブロボットの操作を行うものとする。
【００１０】
図２０は図１９の（イ）、（ロ）、（ハ）の順でマスタアーム２２１を動かした時のタイムチャートであり、（Ａ）はスイッチのＯＮ／ＯＦＦ状態を示し、（Ｂ）は静止摩擦補償トルクの供給状態を順方向（＋）逆方向（−）として示し、（Ｃ）は速度検出器の出力電圧を順方向（＋）逆方向（−）として示したタイムチャートである。
図１９を用いて図２０のタイムチャートを説明する。
操作者がグリップ２２２を把持した瞬間（Ａ）のスイッチＯＮ／ＯＦＦ状態がＯＮとなり、（Ｂ）の静止摩擦補償トルクの供給状態が＋Ｔとなる。Ｔは軸固有の静止摩擦補償トルクであり、軸２２３を動かすために必要な静止摩擦トルクよりは通常少ない値である。このためマスタアーム２２１が単独で動くことはないが、静止摩擦補償トルクの影響で操作者は微細な力を加えることでマスタアーム２２１を操作することができる。
操作者によりマスタアーム２２１が図（Ａ）の（イ）のような順方向（反時計回り）に動いた時、（Ｃ）の速度検出器の出力電圧は順方向（＋）に変化し、マスタアーム２２１が動作したと認識する閾値である（＋α）を超えた時点で静止摩擦補償トルクの供給を停止する。操作者がマスタアーム２２１を停止し、速度検出器の出力電圧がゼロとなった瞬間、静止摩擦補償トルクの供給状態が前回の静止摩擦補償トルク（＋Ｔ）と符号が反対な−Ｔとなる。
このように静止摩擦補償トルクが反転することで次はマスタアーム２２１が図（Ｂ）の（ロ）のような逆方向（時計回り）に動きやすい状態を作り、操作者は微細な力でマスタアーム２２１を逆方向（時計回り）に操作することができる。その次は静止摩擦補償トルクの供給状態が＋Ｔとなり、図（Ｃ）の（ハ）のような順方向（反時計回り）に動きやすくなり、操作者は微細な力でマスタアーム２２１を順方向（反時計回り）に操作することができる。
このように、このマスタアーム操作装置は、マスタアーム２２１を操作する度に静止摩擦補償トルクの供給状態を反転するので、静止摩擦補償トルクの加わる方向に向かって操作者は微細な力を加えることでマスタアーム２２１を操作することができる。
【００１１】
しかしながら、このマスタアーム操作装置は、マスタアームを操作する度に静止摩擦補償トルクの供給状態を反転しているので、マスタアームの操作方向を連続的に同方向に操作者が操作したい場合、通常のマスタアーム操作よりも静止摩擦補償トルクだけ更に過大な力がマスタアーム操作に必要となり、マスタアーム操作できないという問題があった。
また、スレーブロボットへのダイレクト操作ができないという問題もあった。そこで、本発明はこのような問題点に鑑みてなされたものであり、操作者が静止摩擦補償トルクの供給状態を指示し特定することで、操作者に負担の少ないダイレクト操作を実現することができるロボット装置を提供することを第２の目的とする。
【００１２】
【課題を解決するための手段】
上記問題を解決するため、請求項１記載のロボット制御装置の発明は、作業者がロボットを直接操作して目標作業点に誘導する際の前記ロボットの摩擦補償を有するコンプライアンス制御可能なサーボ制御手段を備えたロボット制御装置において、作業内容を入力する作業命令入力手段と、前記作業内容の動作位置の違いごとの作業情報を記憶しておく作業情報記憶手段と、前記作業情報から現在のロボット制御点位置（ｐ０）に最も近接した位置（ｐ１）を持つ前記作業情報を選択し、前記最も近接した位置（ｐ１）を持つ作業情報の目標点（ｐ３）を前記目標作業点として抽出する目標作業点抽出手段と、前記目標作業点抽出手段の出力と前記現在のロボット制御点位置（ｐ０）とから、ｐ０からｐ３の方向でｐ０近傍位置（ｐ’０）までの微小移動量を算出する微小移動量導出手段と、前記微小移動量導出手段の出力から前記ロボットの各関節の動作量を算出する動作方向導出手段と、前記各関節の動作量の正負から前記サーボ制御手段の前記ロボットの関節毎の摩擦補償の方向を決定する摩擦補償方向制御手段と、を備え、前記サーボ制御手段は、前記摩擦補償方向制御手段の出力に従って摩擦補償を実行するようになっていることを特徴とする。
このようになっているため、作業方向から決まる各関節毎に摩擦補償の方向を正確に制御するため、作業方向のコンプライアンス性を十分に保証することが可能であり、ロボットへのダメージの低減と直接教示作業者の操作力低減を実現できるようになる。
また、過去の作業情報を用いて、各関節毎の摩擦補償の方向を正確に制御することで、作業方向の特定に要する時間が削減でき、作業効率が良くなる。
また、作業者が直感的なイメージで指示することが可能であり、初期の動作方向が不定な直接教示作業では有効である。
【００１３】
また、請求項２記載の発明は、請求項１記載のロボット制御装置において、前記目標作業点抽出手段が、前記作業情報の選択において、前記現在のロボット制御点位置（ｐ０）に近接した位置を持つ複数の前記作業情報を選択し、選択した前記複数の作業情報のそれぞれの目標点を前記目標作業点の複数の候補点として抽出し、前記微小移動量導出手段は、前記ｐ０から前記複数の候補点のそれぞれの方向でｐ０近傍位置までの微小移動量を算出し、前記動作方向決定手段は、前記複数の候補点に対応する前記近傍位置までの各関節の動作量を算出し、前記摩擦補償方向制御手段は、前記各関節の動作量の正負から前記サーボ制御手段の前記ロボットの関節毎の摩擦補償の方向を決定するとともに、前記複数の候補点ごとに前記各関節の摩擦補償の方向のテーブルを作成し、前記テーブルに基づいて前記各関節の摩擦補償の方向を一定周期ごとに切り替えることを特徴とする。
このようになっているため、作業方向に任意性がある場合においても、複数の作業方向を予め想定して、摩擦補償の方向を周期的に切り替えるため、外力の方向によらずコンプライアンス性を十分に保証することが可能であり、ロボットへのダメージの低減と直接教示作業者の操作力低減を実現できる。
【００１４】
次に、上記第２の課題を解決するため、本願の第２発明は、ロボット全軸の位置指令入力部と、前記位置指令入力部の位置指令とモータエンコーダの位置フィードバックからサーボトルクを導出するサーボ処理部と、前記サーボトルクにより動作するロボットとを備えたロボット制御装置において、外力が作用する少なくとも一つの軸の外力トルクを設定する外力トルク設定部と、前記外力トルクから軸毎の摩擦補償トルクを特定し補償する摩擦補償部とを備え、前記サーボトルクと前記摩擦補償トルクを加算することを特徴とするものである。
また、本願の第３発明は、前記サーボ処理部が、前記位置指令と前記位置フィードバックから指令トルクを導出する位置速度ループ処理部と、前記指令トルクを制限し制限トルクとして出力するトルク制限部と、前記位置フィードバックから動力学により各軸に加わる重力トルクを演算し重力トルクを補償する重力補償トルクを導出する重力補償部とを備え、前記制限トルクと前記重力補償トルクを加算し前記サーボトルクとして出力することを特徴とするものである。
これらの発明によれば、少なくとも１つ以上の軸に対して指定方向の摩擦補償トルクを加えることによってダイレクト操作の初期起動トルクを低減させることができ、柔軟なダイレクト操作を実現することができる。
さらに本願の第４発明は、ロボット全軸の位置指令入力部と、前記位置指令入力部の位置指令とモータエンコーダの位置フィードバックからサーボトルクを導出するサーボ処理部と、前記サーボトルクにより動作するロボットとを備えたロボット制御装置において、作業座標上における外力ベクトルを設定する外力ベクトル設定部と、前記外力ベクトルと転置ヤコビ行列から各軸のヤコビトルクを導出するヤコビ行列演算部と、前記ヤコビトルクから摩擦補償トルクパターンを生成する摩擦補償トルクパターン生成部と、外力が作用する少なくとも一つの軸の外力トルクを設定する外力トルク設定部と、前記外力トルクから軸毎の摩擦補償トルクを特定し補償する摩擦補償部とを備え、前記サーボトルクと前記摩擦補償トルクを加算することを特徴とするものである。
この発明によれば、作業座標上の外力ベクトルを設定し、外力ベクトルと転置ヤコビ行列から外力ベクトルによりロボット軸に加わるモーメントを求めることができるので作業座標上でのダイレクト操作の初期起動トルクを低減させることができ、柔軟なダイレクト操作を実現することができる。
【００１５】
【発明の実施の形態】
以下、この発明のロボット制御装置について図面に基づいて詳細に説明する。
［第１の実施の形態］
まず、第１の実施の形態について図１〜図６に基づいて説明する。
図１は本発明の第１の実施の形態の基本構成を示す図、図２は第１の実施の形態のサーボ制御手段を示す図、図３は第１の実施の形態の動作方向決定手段を示す図、図４は第１の実施の形態の作業情報記憶手段に記憶されている作業情報を示す図、図５は第１の実施の形態の作業の位置情報を示す図、図６は第１の実施の形態の摩擦補償方向決定手段が微小変位の正負の方向から摩擦補償の方向を決定するための処理フロー図である。
第１の実施の形態の基本構成を示す図１において、１は６自由度を有するロボット、２はサーボ制御手段、３は作業命令入力手段、４は動作方向決定手段、５は摩擦補償方向制御手段である。
作業命令入力手段３としては、タッチパネルや押しボタンスイッチ、音声入力などが用いられる。作業命令入力手段３の入力結果が動作方向決定手段４に入力されると、後述する方法で現在のロボット制御点位置に最も近接した位置を持つ作業の作業情報を選択し、その作業の目標点である位置を目標作業点として抽出する。摩擦補償方向制御手段５では、後述する方法で各関節の微小変位の正負の方向から、摩擦補償の方向を決定し、前記作業の動作方向にスムーズに動作可能なように摩擦補償の方向を制御できるようにする。
従って、この状態で作業者が例えばロボット１を直接操作して目標作業点誘導するような場合には極めて微小な作用力でロボット１を誘導することが可能となる。
以下、図１に示された各手段について説明する。
【００１６】
図１のサーボ制御手段２は、図２に示すように、通常の位置速度ループのほかに、トルク制限器２５と重力補償器２６と摩擦補償器２７を備えている。
図において、ホスト制御回路からラインを介して各軸毎の位置指令値が減算手段２１の一方の入力に与えられる。エンコーダからの各軸の軸角度を表すフィードバック信号はラインから減算手段２１の他方の入力に与えられる。減算手段２１の出力は、係数器２２に与えられ、ここで位置ゲインＫｐで増幅され、減算手段２３の一方の入力に与えられる。エンコーダからのフィードバック信号は微分回路２０で微分され、減算手段２３の他方の入力に与えられる。減算手段２３の出力は、係数器２４で速度ゲインＫｖで増幅され、トルク制限器２５に与えられる。
【００１７】
作業命令入力手段３は、タッチパネルや押しボタンスイッチ、音声入力などが用いられる。
【００１８】
動作方向決定手段４は、図３に示すように、目標作業点抽出手段４１と作業情報記憶手段４２と微小移動量導出手段４３と関節動作方向導出手段４４で構成される。そして、前記作業情報記憶手段４２には、図４に示す作業の異なる作業Ａと作業Ｂの作業情報が記憶されており、更に作業Ａと作業Ｂは動作位置の違いによりそれぞれ、作業Ａ１、Ａ２…，作業Ｂ１，Ｂ２…が記憶されている。
前記作業Ａの記憶されている位置（Ｐ１，Ｐ２…）のそれぞれの位置関係を図５に示す。例えば、作業Ａ１であれば、前記ロボット１の制御点がＰ１→Ｐ２→Ｐ３と動作することを意味している。
【００１９】
そこで、本発明の第１の実施の形態の動作について説明する。
前記作業命令入力手段３（図１）で、後述する図４の作業Ａの移動命令に関する作業情報が入力されているものとして以下の実施形態を説明する。前記作業命令入力手段３の入力結果が前記動作方向決定手段４（図１）に入力されると、動作方向決定手段４内で目標作業点抽出手段４１（図３）は図５に示すように、前記作業情報記憶手段４２（図３）に記憶されている作業Ａに関連する作業情報（作業Ａ１、Ａ２…）から現在のロボット制御点位置Ｐ０に最も近接した位置Ｐ１を持つ作業Ａ１の作業情報を選択し、作業Ａ１の目標点である位置Ｐ３を目標作業点として抽出する。
前記微小移動量導出手段４３（図３）では、抽出された目標作業点Ｐ３と現在の前記ロボット制御点位置Ｐ０とから、Ｐ０近傍Ｐ’０までの微小移動量を（式３）として算出する。
【数３】

関節動作方向導出手段４４（図３）では、各関節の微小変位（Δｑ１，Δｑ２…Δｑ６）をヤコビ行列Ｊを用いて（式４）として算出する。
【数４】

なお、ここでは、姿勢変化は無いと仮定しているが、（式４）の右辺第２項ベクトルの第４、５，６成分に角速度を設定することで姿勢変化を考慮してもよい。
前記摩擦補償方向制御手段５（図１）では、前記各関節の微小変位（Δｑ１，Δｑ２…Δｑ６）の正負の方向から、摩擦補償の方向を図６に示す処理フローに基づいて決定する。
図６において、フローがスタートすると、カウンタをＮ＝１（ステップ２）とし、Δｑ（Ｎ）を入力する（ステップ３）。Δｑ（Ｎ）の正負を調べ（ステップ４）、正ならばＳｇｎ（Ｎ）＝１とし、負ならばＳｇｎ（Ｎ）＝−１とし（ステップ５）、Ｎ＝Ｎ＋１として（ステップ６）、Ｎの最大値に達するまでステップ３からを繰り返し、Ｎが最大値に達したら終了する。
この結果は前記摩擦補償器２７（図２）に入力され、前記作業Ａ１の動作方向にスムーズに動作可能なように摩擦補償の方向を制御できる。
従って、この状態で作業者が例えば、前記ロボット１を直接操作して目標作業点Ｐ３２誘導するような場合には極めて微小な作用力で前記ロボット１を誘導することが可能である。
なお、前記作業命令入力手段３（図１）において、デカルト座標系上の動作方向を、例えば（Δｘ１，Δｙ１，Δｚ１）と入力し、前記関節動作方向導出手段４４（図３）の数２を用いて、同様に実現することが可能である。
【００２０】
［第１の実施の形態の変形例］
次に、第１の実施の形態の変形例について説明する。
前記作業命令入力手段３（図１）で、後述する図４の作業Ａの移動命令に関する作業情報が入力されているものとして以下の実施形態を説明する。但し今回は、前記目標作業点抽出手段４１（図３）が現在の前記ロボット１の制御点位置Ｐ０に近接した位置として複数の候補点を抽出した場合について説明する。
前記目標作業点抽出手段４１（図３）は、前記作業情報記憶手段４２（図３）から図７に示す複数の候補点（Ｐ３，Ｐ１３，Ｐ２３，Ｐ３３）を抽出する。
前記微小移動量導出手段４３（図３）は、第１の実施形態の場合と同様に式（３）を用いて、前記制御点位置Ｐ０から前記候補点（Ｐ３，Ｐ１３，Ｐ２３，Ｐ３３）に対応する複数の近傍点（Ｐ’１，Ｐ’２，Ｐ’３，Ｐ’４）までの、複数の微小移動量（Δｒ１，Δｒ２，Δｒ３，Δｒ４）を導出し、前記関節動作方向導出手段４４は、式（４）を用いて、複数の各関節の微小変位の組み（ΔＱ１，ΔＱ２，ΔＱ３，ΔＱ４）を算出する。
【数５】

前記摩擦補償方向制御手段５（図１）は、前記各関節の微小変位の組み（ΔＱ１，ΔＱ２，ΔＱ３，ΔＱ４）の各要素の正負の方向から、摩擦補償の方向を図６に示す処理フローに基づいて決定し、図８に示すテーブルＴＥを作成する。
図８のテーブルＴＥには、候補点Ｐ３についてパターン１、候補点Ｐ１３についてパターン２、候補点Ｐ２３についてパターン３、候補点Ｐ３３についてパターン４の４つのパターンが作成されている。各パターンは第１軸〜第６軸についてそれぞれｓｇｎ［Ｋ］［Ｎ］の符号が付されている。
そこで、このテーブルＴＥを用いて、図９に示す制御処理フローに基づいて、摩擦補償器２７（図２）の補償方向を周期Ｔのタイミングで切り替えて、各軸毎に図１０に示す摩擦補償の方向のパターン（図８のパターン１〜パターン４）を実現し、複数方向のコンプライアンス性を保証することが可能となる。
【００２１】
図９の制御処理フローは次のようになっている。
フローがスタートすると、まず、ｓｇｎ[Ｋ][Ｎ]を図８のテーブルから読み込み（ｓｓ２）、これをＳｇｎ[Ｎ]＝ｓｇｎ[Ｋ][Ｎ]とし（ｓｓ３）、Ｎ＝Ｎ＋１に置き換えて（ｓｓ４）、これをＮ＞６になるまで繰り返す（ｓｓ５）。Ｎ＞６になったら、Ｓｇｎ[１]、Ｓｇｎ[２]、Ｓｇｎ[３]、Ｓｇｎ[４]、Ｓｇｎ[５]、Ｓｇｎ[６]を摩擦補償器に設定する（ｓｓ５）。Ｔ秒したら（ｓｓ６）、Ｋ＝Ｋ＋１に置き換えて（ｓｓ８）、ステップｓｓ９へ進み、Ｋ＞４であればＫ＝１に置き換えてステップｓｓ１１へ進み、Ｋ＞４でなければそのままステップｓｓ１１へ進み、フィードバック速度＞閾値Ａを調べる（ｓｓ１１）。フィードバック速度＞閾値Ａでなければｓｓ３に戻り、フィードバック速度＞閾値Ａとなったら終了する。
図１０は第１の実施の形態の複数の動作方向に対応した摩擦補償の方向を周期的に切り替えている様子を示す図である。図９に示す制御処理フローに基づいて、摩擦補償器２７（図２）の補償方向を周期Ｔのタイミングで切り替えた６個の摩擦補償パターンが見られる。例えば、時間Ｔ１では第１軸〜第６軸に対して、図８のテーブルＴＥのパターン１が適用され、摩擦補償の方向は第１軸に対して＋１、第２軸に対して＋１、第３軸に対して−１、第４軸に対して−１、第５軸に対して＋１、第６軸に対して＋１が与えられる。
同じく、時間Ｔ２では第１軸〜第６軸に対して、図８のテーブルＴＥのパターン２が適用され、摩擦補償の方向は第１軸に対して＋１、第２軸に対して−１、第３軸に対して＋１、第４軸に対して−１、第５軸に対して−１、第６軸に対して＋１が与えられる。
時間Ｔ３では第１軸〜第６軸に対して、図８のテーブルＴＥのパターン３が適用され、摩擦補償の方向は第１軸に対して−１、第２軸に対して−１、第３軸に対して−１、第４軸に対して−１、第５軸に対して＋１、第６軸に対して＋１が与えられる。
時間Ｔ４では第１軸〜第６軸に対して、図８のテーブルＴＥのパターン４が適用され、摩擦補償の方向は第１軸に対して＋１、第２軸に対して−１、第３軸に対して−１、第４軸に対して−１、第５軸に対して＋１、第６軸に対して＋１が与えられる。
さらに、時間Ｔ５〜Ｔ８では同様にしてそれぞれ図８のテーブルＴＥのパターン１〜パターン４が適用される。
このようにして、複数方向のコンプライアンス性を保証することが可能となる。
【００２２】
［第２の実施の形態］
次に、本発明の第２の実施の形態について説明する。
図１１は、本発明の第２の実施の形態を示すロボット制御装置のブロック図であり、請求項５の内容に相当するものである。
図１１において、２０１は位置指令入力部、２０２はサーボ処理部、２０３はロボット、２０４は外力トルク設定部、２０５は摩擦補償部である。なお、位置指令入力部２０１、サーボ処理部２０２、外力トルク設定部２０４および摩擦補償部２０５は上位コントローラＣに含まれるものとする。
ロボット制御装置は、図１１に示すようにロボット２０３を構成する各軸モータの位置指令を取得する位置指令入力部２０１と、位置指令入力部２０１の位置指令とロボット２０３の位置フィードバックからロボット２０３の各軸モータへの指令値であるサーボトルクを出力するサーボ処理部２０２と、外力が作用する関節軸の外力方向を含んだ外力トルクを設定する外力トルク設定部と、外力トルク設定部で設定された外力トルクを用いて摩擦補償トルク分だけ補償する摩擦補償部５とから構成されている。サーボトルクと摩擦補償トルクを加えた最終的なモータへの指令値をモータトルクとする。
【００２３】
ここで、サーボ処理部２０２について図１２を用いて説明する。
図１２はサーボ処理部２０２を改良したロボット制御装置のブロック図であり、請求項６の内容に相当するものである。図１２において、２０６は位置速度ループ処理部、２０７はトルク制限部、２０８は重力補償部である。
位置速度ループ処理部２０６は位置指令入力部１の位置指令とロボット２０３の位置フィードバックから指令トルクを出力する。トルク制限部２０７は位置速度ループ処理部２０６の指令トルクに上限値と下限値による制限を加え、制限トルクとして出力する。
重力補償部２０８は予め設定されたツールを含む各リンクの重量情報とロボット２０３の位置フィードバックから動力学演算で各軸に作用する重力トルクを演算し、重力トルクに対して大きさ同じで向きが反対の重力補償トルクを出力する。
サーボ処理部２０２はトルク制限部２０７の制限トルクと重力補償部２０８の重力補償トルクを加えた値をサーボトルクとして出力する。
【００２４】
次に、図１３及び図１４を用いて外力に倣う追従動作であるコンプライアンス動作を説明する。
図１３は本発明により操作者がロボット２０３をダイレクト操作している図である。図１３において、要点に絞って説明するためにロボット２０３を２軸ロボットとし、各関節を軸１、軸２とし、各リンクをリンク１、リンク２とし、リンク上に設置した外力の方向を特定するためのスイッチを軸１外力方向スイッチ、軸２外力方向スイッチとし、操作者はリンク２の軸２外力方向スイッチを操作しながら軸２のダイレクト操作を行っている。
【００２５】
図１４は図１３のように操作者がダイレクト操作している時の図１２中の制限トルク、重力補償トルク、サーボトルク、摩擦補償トルク、モータトルク、軸２に加わる外力と軸２に加わる総トルクの時間経過である。
操作者は軸２外力方向スイッチをプラス側やマイナス側に切り替えることで、軸２の動かしたい方向をロボット制御装置に教える。
図１３中の操作者が仮に軸２外力方向スイッチをプラス側に指定した場合、ロボット制御装置内の摩擦補償部５は軸２がプラス方向に動作する向きの摩擦補償トルクを図１４（４Ｄ）のような波形として出力する。図１４（４Ｄ）中のＭは軸２の摩擦補償トルクである。
摩擦補償トルクが原因でロボットが動きださないようにするために、通常、摩擦補償トルクの大きさは軸固有の静止摩擦トルクの大きさよりも若干小さな値とする。
位置指令入力部１の位置指令がなく位置速度ループ処理部６の指令トルクがゼロもしくは、トルク制限部７の上限値と下限値が共にゼロで制限トルクがゼロ（図１４（４Ａ）参照）の時、サーボトルクは重力補償部８の重力補償トルクのみとなり、重力補償トルク（図１４（４Ｂ）参照）とサーボトルク（図１４（４Ｃ）参照）は同じ波形となる。
動作開始前の軸２の重力トルクをＧとする。なお、軸２の動作開始時点から重力補償トルクとサーボトルクが減少するのはリンク２が操作者の外力によって直立する方向（プラス方向）に動作し、軸２に影響を与えるリンク２の重力トルクが減少するためである。
モータトルクはサーボトルクと摩擦補償トルクの和なので図１４（４Ｅ）のようになる。
外力が図１４（４Ｆ）のように軸２に作用し、モータトルクと外力の和である軸２に加わる総トルクが図１４（４Ｇ）となった場合、総トルクが軸２の静止摩擦トルクを超えた時点で軸２は動作を開始することになる。
従来制御では摩擦補償部５がなく静止摩擦トルク以上の外力が必要だったが、本発明では一定の摩擦補償トルクを加えた分だけ軸２を動かすための外力が低減され、ダイレクト操作しやすくなっている。
操作時の外力ベクトル設定部９としては音声認識やプログラミングペンダント上のキーなどにより実現でき、ジョブプレイバック時の外力ベクトル設定部９としてはジョブ中のコマンド表記により実現できる。
外力方向スイッチを単なるＯＮ／ＯＦＦスイッチとせず、変化量の程度を検出する検出器とすれば、操作者が希望する摩擦補償トルクを各軸に与えることができるのは言うまでもない。
このように、少なくとも１つ以上の軸に対して指定方向の摩擦補償トルクを加えることによってダイレクト操作の初期起動トルクを低減させることができ、柔軟なダイレクト操作を実現することができる。
【００２６】
［第２の実施の形態の変形例］
図１５は、本発明の第２の実施の形態の変形例を示すロボット制御装置のブロック図１であり、請求項７の内容に相当するものである。なお、本発明の構成要素が前出の実施形態の構成要素と同じものについては同一符号を付して説明を省略し、異なる点のみ説明する。
図１５において、２０９は外力ベクトル設定部、２１０はヤコビ行列演算部、２１１は摩擦補償トルクパターン生成部である。外力ベクトル設定部２０９で作業座標上における外力ベクトルを設定する。ヤコビ行列演算部２１０は外力ベクトル設定部２０９で設定した外力ベクトルと転置ヤコビ行列から各軸のヤコビトルクを導出する。摩擦補償トルクパターン生成部２１１はヤコビ行列演算部２１０で導出したヤコビトルクから摩擦補償トルクパターンを生成する。
ロボットを２０６軸とした時、作業座標上における各軸方向の力成分をＦｘ，Ｆｙ，Ｆｚとし、各軸周りのモーメントをＭｘ，Ｍｙ，Ｍｚとした時、外力ベクトルは（Ｆｘ，Ｆｙ，Ｆｚ，Ｍｘ，Ｍｙ，Ｍｚ）として表現できる。
外力ベクトルによってロボットの各軸に加わるモーメントであるヤコビトルクをＴ１，Ｔ２，Ｔ３，Ｔ４，Ｔ５，Ｔ６とし、６行６列の転置ヤコビ行列をＪＴとした時、外力ベクトル（Ｆｘ，Ｆｙ，Ｆｚ，Ｍｘ，Ｍｙ，Ｍｚ）とヤコビトルク（Ｔ１，Ｔ２，Ｔ３，Ｔ４，Ｔ５，Ｔ６）との間には式（５）の関係が一般に知られている。
【数６】

従って、（Ｆｘ，Ｆｙ，Ｆｚ，Ｍｘ，Ｍｙ，Ｍｚ）を外力ベクトル設定部２０９に設定すれば、ヤコビ行列演算部２１０では転置ヤコビ行列ＪＴと式（１）からヤコビトルク（Ｔ１，Ｔ２，Ｔ３，Ｔ４，Ｔ５，Ｔ６）を導出する。
摩擦補償トルクパターン生成部２１１はヤコビトルク（Ｔ１，Ｔ２，Ｔ３，Ｔ４，Ｔ５，Ｔ６）の値からプラス側領域、不感帯領域、マイナス側領域のどれかに区別し、外力トルク設定部２０４に各軸の外力方向としてプラス、ゼロ、マイナスのどれか一つを設定する。摩擦補償部２０５は外力トルク設定部２０４に設定された外力方向と同方向の摩擦補償トルクを出力する。以降の処理は第２の実施の形態と同様な処理を行う。
また、外力トルクをプラス、ゼロ、マイナスの内のどれかと限定するのではなく、外力トルクを比例的に指定することもできる。その場合、摩擦補償トルクパターン生成部２１１が最大外力トルクに対するヤコビトルクの比として外力トルクを外力トルク設定部４に設定することで外力トルクの比例的な変化に対応することができる。
操作時の外力ベクトル設定部２０９としては音声認識やロボット先端のようなロボット本体に設置するハンドルスイッチなどにより実現でき、ジョブプレイバック時の外力ベクトル設定部２０９としてはジョブ中のコマンド表記により実現できる。
このように、作業座標上の外力ベクトルを設定し、外力ベクトルと転置ヤコビ行列から外力ベクトルによりロボット軸に加わるモーメントを求めることができるので作業座標上でのダイレクト操作の初期起動トルクを低減させることができ、柔軟なダイレクト操作を実現することができる。
【００２７】
【発明の効果】
以上述べたように、請求項１記載のロボット制御装置によれば、作業方向から決まる各関節毎に摩擦補償の方向を正確に制御するため、作業方向のコンプライアンス性を十分に保証することが可能であり、ロボットへのダメージの低減と直接教示作業者の操作力低減を実現できる。
また、過去の作業情報を用いて、各関節毎の摩擦補償の方向を正確に制御するため、作業方向の特定に要する時間が削減でき、作業効率が良くなる。
さらに、作業者が直感的なイメージで指示することが可能であり、初期の動作方向が不定な直接教示作業では有効である。
【００２８】
請求項２記載のロボット制御装置によれば、作業方向に任意性がある場合においても、複数の作業方向を予め想定して、摩擦補償の方向を周期的に切り替えるため、外力の方向によらずコンプライアンス性を十分に保証することが可能であり、ロボットへのダメージの低減と直接教示作業者の操作力低減を実現できる。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態の基本構成を示す図である。
【図２】第１の実施の形態のサーボ制御手段２を示す図である。
【図３】第１の実施の形態の動作方向決定手段４を示す図である。
【図４】第１の実施の形態の作業情報記憶手段４２に記憶されている作業情報を示す図である。
【図５】第１の実施の形態の作業Ａの位置情報を示す図である。
【図６】第１の実施の形態の摩擦補償方向決定手段が微小変位の正負の方向から摩擦補償の方向を決定するための処理フロー図である。
【図７】第１の実施の形態の変形例の実施内容を示す図である。
【図８】第１の実施の形態の複数の動作方向に対応した摩擦補償の方向を定めたテーブルを示す図である。
【図９】第１の実施の形態の摩擦補償器の補償方向を周期Ｔのタイミングで切り替えて所定の摩擦補償パターンを実現する制御処理フローを示す図である。
【図１０】第１の実施の形態の複数の動作方向に対応した摩擦補償の方向を周期的に切り替えている様子を示す図である。
【図１１】本発明の第２の実施の形態を示すロボット制御装置のブロック図である。
【図１２】第２の実施の形態のサーボ処理部２を改良したロボット制御装置のブロック図である。
【図１３】第２の実施の形態により操作者がダイレクト操作している図である。
【図１４】ダイレクト操作におけるロボット装置内の信号タイムチャートである。
【図１５】第２の実施の形態の変形例を示すロボット制御装置のブロック図である。
【図１６】従来のロボット制御装置の基本構成を示す図である。
【図１７】従来の制御回路の構成および動作を設定するためのブロック図である。
【図１８】従来装置の実施例の動作を簡略化して示す図である。
【図１９】従来装置の１軸マスタアーム操作装置の説明図である。
【図２０】図１９の（イ）、（ロ）、（ハ）の順でマスタアームを動かした時のタイムチャートである。
【符号の説明】
１ ロボット
２ サーボ制御手段
２１ 第１減算手段
２２ 位置ゲイン
２３ 第２減算手段
２４ 速度ゲイン
２５ トルク制限器
２６ 重力補償器
２７ 摩擦補償器
２８ 加算器
２９ 微分回路
３ 作業命令入力手段
４ 動作方向決定手段
４１ 目標作業点抽出手段
４２ 作業情報記憶手段
４３ 微小移動量導出手段
４４ 関節動作方向導出手段
５ 摩擦補償方向制御手段
１０１ ロボット
１１０ ツール
１１６ ワーク
１２０ 移動方向
１２２ 制御回路
１２３ サーボ制御回路
１２４ ホスト制御回路
１２５ サーボモータ
１２６ 増幅回路
１２７ エンコーダ
１２８ ライン
１２９ 減算手段
１３０ ライン
１３１ 係数器
１３２ 加算手段
１３３ 微分回路
１３４ 係数器
１３５ 積分器
１３６ 位相補償器
１３７ 加算手段
１３８ 重力補償演算回路
１３９ ライン
１４１ 重力
１４２ 減算手段
１４３ 慣性力
１４４ 積分手段
１４５ 積分手段
２０１ 位置指令入力部
２０２ サーボ処理部
２０３ ロボット
２０４ 外力トルク設定部
２０５ 摩擦補償部
２０６ 位置速度ループ処理部
２０７ トルク制限部
２０８ 重力補償部
２０９ 外力ベクトル設定部
２１０ ヤコビ行列演算部
２１１ 摩擦補償トルクパターン生成部
２２１ マスタアーム
２２２ グリップ
２２３ 軸
２２４ 歯車（１）
２２５ モータ
２２６ 出力軸
２２７ 歯車（２）
２２８ 速度検出器

Claims (2)

1. 作業者がロボットを直接操作して目標作業点に誘導する際の前記ロボットの摩擦補償を有するコンプライアンス制御可能なサーボ制御手段を備えたロボット制御装置において、
   作業内容を入力する作業命令入力手段と、
   前記作業内容の動作位置の違いごとの作業情報を記憶しておく作業情報記憶手段と、
   前記作業情報から現在のロボット制御点位置（ｐ０）に最も近接した位置（ｐ１）を持つ前記作業情報を選択し、前記最も近接した位置（ｐ１）を持つ作業情報の目標点（ｐ３）を前記目標作業点として抽出する目標作業点抽出手段と、
   前記目標作業点抽出手段の出力と前記現在のロボット制御点位置（ｐ０）とから、ｐ０からｐ３の方向でｐ０近傍位置（ｐ’０）までの微小移動量を算出する微小移動量導出手段と、
   前記微小移動量導出手段の出力から前記ロボットの各関節の動作量を算出する動作方向導出手段と、
   前記各関節の動作量の正負から前記サーボ制御手段の前記ロボットの関節毎の摩擦補償の方向を決定する摩擦補償方向制御手段と、
   を備え、
   前記サーボ制御手段は、前記摩擦補償方向制御手段の出力に従って摩擦補償を実行するようになっていることを特徴とするロボット制御装置。
2. 前記目標作業点抽出手段は、前記作業情報の選択において、前記現在のロボット制御点位置（ｐ０）に近接した位置を持つ複数の前記作業情報を選択し、選択した前記複数の作業情報のそれぞれの目標点を前記目標作業点の複数の候補点として抽出し、
   前記微小移動量導出手段は、前記ｐ０から前記複数の候補点のそれぞれの方向でｐ０近傍位置までの微小移動量を算出し、
   前記動作方向決定手段は、前記複数の候補点に対応する前記近傍位置までの各関節の動作量を算出し、
   前記摩擦補償方向制御手段は、前記各関節の動作量の正負から前記サーボ制御手段の前記ロボットの関節毎の摩擦補償の方向を決定するとともに、前記複数の候補点ごとに前記各関節の摩擦補償の方向のテーブルを作成し、前記テーブルに基づいて前記各関節の摩擦補償の方向を一定周期ごとに切り替えることを特徴とする請求項１記載のロボット制御装置。

Images