JP4992702B2 - ロボットの動作制御装置及びその動作制御方法 - Google Patents

Description

本発明は、複数の駆動部毎に指令値を与えて前記複数の駆動部を連動させることでロボットの動作を制御するロボットの動作制御装置及びその動作制御方法に関する。

複数の駆動部毎に指令値を与えて複数の駆動部を連動させることでロボットの動作を制御する構成がある（例えば特許文献１，２参照）。
特開昭５６−７６３９４号公報 特開２００２−１２７０５３号公報

ところで、上記した構成では、指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として発生する場合がある。この場合、軸毎に発生するサーボ遅れ時間が同じであれば、時間軸での遅れは発生するものの、実際の動作軌跡が目標軌跡から外れることはないが、一方、軸毎に発生するサーボ遅れ時間が異なっていると、時間軸での遅れが発生するのみならず、実際の動作軌跡が目標軌跡から外れてしまうという問題がある。その結果、ロボットの動的精度が低下することになり、ティーチングに要する時間が長期化するという問題がある。

本発明は、上記した事情に鑑みてなされたものであり、その目的は、ロボットの動的精度を向上させることができ、ティーチングに要する時間を短縮することができるロボットの動作制御装置及びその動作制御方法を提供することにある。

請求項１，４に記載した発明によれば、複数の駆動部に対応する軸毎に、指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算し、その計算した軸毎のサーボ遅れ時間を比較して軸毎のサーボ遅れ時間が複数の軸の全てで一致するように基準時間を決定し、その計算した軸毎のサーボ遅れ時間と当該決定した基準時間とに基づいて軸毎の補償量を計算し、その決定した軸毎の補償量を反映した指令値を複数の駆動部毎に与えて複数の駆動部を連動させることでロボットの動作を制御するようにしたので、軸毎のサーボ遅れ時間が複数の軸の全てで一致するように制御することにより、実際の動作軌跡を目標軌跡に近付けることができ、ロボットの動的精度を向上させることができ、ティーチングに要する時間を短縮することができる。

請求項２，５に記載した発明によれば、決定した軸毎の補償量と計算した軸毎のサーボ遅れ時間とに基づいて軌跡誤差を推定し、その推定した軌跡誤差を判定し、その推定した軌跡誤差が許容値以上であることを判定したときに、ロボットの動作速度を低減させるようにしたので、ロボットの動作を一定範囲内で安定させることができ、周囲設備など対する干渉を極力抑えることができる。

請求項３，６に記載した発明によれば、複数の駆動部に対応する軸毎に、指令値を多項式で補間することで、指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算するようにしたので、サーボ遅れ時間の計算精度を高めることができ、動的精度を確実に向上させることができる。

以下、本発明を、垂直多関節型ロボットの動作を制御する制御装置に適用した一実施形態について、図面を参照して説明する。図１に示すように、産業用ロボット装置としてのロボット装置１は、垂直多関節型ロボット（以下、ロボットと称する）２と、ロボット２の動作を制御する制御装置３（本発明でいう動作制御手段、サーボ遅れ時間計算手段、基準時間決定手段、補償量計算手段、軌跡誤差推定手段、軌跡誤差判定手段、動作速度低下手段）と、制御装置３に接続されているティーチングペンダント４とを備えて構成されている。

ロボット２は、例えば６軸の垂直多関節を有するロボットであり、ベース５と、ベース５に水平方向に旋回可能に支持されているショルダ部６と、ショルダ部６に上下方向に旋回可能に支持されている下アーム７と、下アーム７に上下方向に旋回可能に支持されている第１の上アーム８と、第１の上アーム８の先端部に捻り回転可能に支持されている第２の上アーム９と、第２の上アーム９に上下方向に回転可能に支持されている手首１０と、手首１０に回転（捻り動作）可能に支持されているフランジ１１とを備えて構成されている。

上記したベース５を含め、ショルダ部６、下アーム７、第１の上アーム８、第２の上アーム９、手首１０及びフランジ１１は、ロボット２におけるリンク（本発明でいう駆動部）として機能し、ベース５を除く各リンクは、下段のリンクに対して回転関節により回転可能に連結されている。最先端のリンクであるフランジ１１は、ワークを把持するためのハンド（図示せず）が取付け可能になっている。また、リンク同士を連結する回転関節には前段のリンク側に固定されているモータの回転を減速して次段のリンクに伝達する減速装置が設けられている。

尚、本実施形態では、第１のリンクであるベース５と第２のリンクであるショルダ部６との間を連結する回転関節の関節軸を第１軸、第２のリンクであるショルダ部６と第３のリンクである下アーム７との間を連結する回転関節の関節軸を第２軸、第３のリンクである下アーム７と第４のリンクである第１の上アーム８との間を連結する回転関節の関節軸を第３軸、第４のリンクである第１の上アーム８と第５のリンクである第２の上アーム９との間を連結する回転関節の関節軸を第４軸、第５のリンクである第２の上アーム９と第６のリンクである手首１０との間を連結する回転関節の関節軸を第５軸、第６のリンクである手首１０と第７のリンクであるフランジ１１との間を連結する回転関節の関節軸を第６軸として図示している。

ロボット２の動作を制御する制御装置３は、図２に示すように、ＣＰＵ１２と、駆動回路１３と、位置検出回路１４とを備えて構成されている。ＣＰＵ１２には、ロボット２全体のシステムプログラムや動作プログラムを作成するためのロボット言語などを記憶するＲＯＭ１５及びロボット２の動作プログラムなどを記憶するＲＡＭ１６が接続されていると共に、ティーチング作業を行なう際に使用するティーチングペンダント４が接続されている。ティーチングペンダント４は、図１に示すように、各種の操作部４ａ及び表示器４ｂを備えて構成されている。

位置検出回路１４は、ショルダ部６、各アーム７〜９、手首１０及びフランジ１１の位置を検出するためのもので、ショルダ部６、各アーム７〜９、手首１０及びフランジ１１の駆動源であるモータ１７に設けられているロータリエンコーダ１８が接続されている。位置検出回路１４は、ロータリエンコーダ１８から入力する検出信号に基づいてベース５に対するショルダ部６の回転角度、ショルダ部６に対する下アーム７の回転角度、下アーム７に対する第１の上アーム８の回転角度、第１の上アーム８に対する第２の上アーム９の回転角度、第２の上アーム９に対する手首１０の回転角度、手首１０に対するフランジ１１の回転角度を検出し、それら検出した位置検出情報をＣＰＵ１２に出力する。そして、ＣＰＵ１２は、動作プログラムに基づいてショルダ部６、各アーム７〜９、手首１０及びフランジ１１を動作させる際に、位置検出回路１４から入力する位置検出情報をフィードバック信号としてそれらの動作を制御する。

各リンクには、図１に示すように、３次元の座標が規定されている。このうち、床面に据え付けられるベース５の座標系は、不動の座標系としてロボット２の基準座標とされるものであり、ベース５の下端中心を原点とし、水平方向の２つの座標軸Ｘｂ，Ｙｂ及び垂直方向の１つの座標軸Ｚｂが規定されている。他のリンクの座標系は、各回転関節の回転により基準座標上での位置と向きが変化し、ＣＰＵ１２は、位置検出回路１４から入力するショルダ部６、各アーム７〜９、手首１０、フランジ１１の各回転関節の位置検出情報と予め記憶されている各関節の長さ情報とに基づいて各関節の座標の位置と向きとを座標変換の計算機能により基準座標上での位置と向きとに変換して認識する。

また、上記した各関節の座標系のうちフランジ１１の座標系は、フランジ１１の先端面の回転中心を原点とし、フランジ１１の先端面上で２つの座標軸Ｘｍ，Ｙｍ及びフランジ１１の回転軸上で１つの座標軸Ｚｍが規定されている。尚、これら座標軸Ｘｍ，Ｙｍ，Ｚｍに対する回転角はヨー角ＲＸ、ピッチ角ＲＹ、ロール角ＲＺで表される。

次に、上記した構成の作用について、図３及び図４を参照して説明する。
制御装置３において、ＣＰＵ１２は、ロボット２に動作指令を与えるときに、軸毎に指令値を計算し（ステップＳ１）、その計算した指令値を各軸のサーボ（駆動部）に出力する（ステップＳ２）。ここで、ロボット２は、制御装置３から指令値を入力すると、その入力した指令値に基づいてショルダ部６、各アーム７〜９、手首１０及びフランジ１１の駆動源であるモータ１７が作動し、それらショルダ部６、各アーム７〜９、手首１０及びフランジ１１の駆動源であるモータ１７が動作する。

制御装置３において、ＣＰＵ１２は、各軸の角度を検出することで実際の動作軌跡を計算し（ステップＳ３）、各軸について指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算する（ステップＳ４）。つまり、ＣＰＵ１２は、図４に示すように、例えば第２軸及び第３軸について説明すると、目標速度に対する実速度の遅れ時間を計算し、第２軸については「ｔａ−ｔ２」に相当する時間「Ｔ２」を時刻「ｔａ」におけるサーボ遅れ時間として計算し、第３軸については「ｔａ−ｔ３」に相当する時間「Ｔ３」を時刻「ｔａ」におけるサーボ遅れ時間として計算する。尚、ＣＰＵ１２は、軸毎に指令値を多項式で補間することで、指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算し、サーボ遅れ時間の計算精度を高めるようにしている。

次いで、ＣＰＵ１２は、それら計算した軸毎のサーボ遅れ時間を比較して軸毎のサーボ遅れ時間が複数の軸の全てで一致するように基準時間（基準値）を決定する（ステップＳ５）。つまり、ＣＰＵ１２は、上記した例えば第２軸及び第３軸について説明すると、図４に示した状態では、第２軸のサーボ遅れ時間である「Ｔ２」と第３軸のサーボ遅れ時間「Ｔ３」とを比較し、第２軸のサーボ遅れ時間の方が第３軸のサーボ遅れ時間よりも短いことから、第２軸のサーボ遅れ時間を基準時間として決定する。尚、ＣＰＵ１２は、このように最短のサーボ遅れ時間を基準時間として決定するのではなく、例えば両者の平均に相当する時間などの別の時間を基準時間として決定しても良い。

次いで、ＣＰＵ１２は、その計算した軸毎のサーボ遅れ時間と当該決定した基準時間とに基づいて軸毎の補償トルク（補償量）を計算する（ステップＳ６）。そして、ＣＰＵ１２は、計算した軸毎の補償トルクと軸毎のサーボ遅れ時間とに基づいて軌跡誤差を推定し（ステップＳ７）、その推定した軌跡誤差が予め設定した許容値以上であるか否かを判定する（ステップＳ８）。

ここで、ＣＰＵ１２は、推定した軌跡誤差が許容値以上でない（許容値未満である）と判定すると（ステップＳ８にて「ＮＯ」）、補償トルクを反映した指令値を各軸のサーボに出力し、サーボ遅れ時間が全ての軸で一致するように、つまり、軸間のサーボ遅れ時間の差が消滅するように、フィードバック制御し（ステップＳ９）、上記したステップＳ３に戻り、これ以降、上記した処理を繰返して行う。尚、ＣＰＵ１２は、フィードバック制御として、Ｐ制御（比例制御）、ＰＩ制御（比例積分制御）或いはＰＩＤ制御（比例積分微分制御）を行う。一方、ＣＰＵ１２は、推定した軌跡誤差が許容値以上であると判定すると（ステップＳ８にて「ＹＥＳ」）、動作速度を低減させ（ステップＳ１０）、上記したステップＳ３に戻り、これ以降、上記した処理を繰返して行う。尚、以上は、第２軸と第３軸との２軸について説明したが、他の軸同士についても同様であり、３軸以上についても同様である。

以上に説明したように本実施形態によれば、軸毎に指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算し、その計算した軸毎のサーボ遅れ時間を比較して軸毎のサーボ遅れ時間が複数の軸の全てで一致するように基準時間を決定し、その計算した軸毎のサーボ遅れ時間と当該決定した基準時間とに基づいて軸毎の補償トルクを計算し、その計算した軸毎の補償トルクを反映した指令値を各軸のサーボに出力してロボット２の動作を制御するようにしたので、実際の動作軌跡を目標軌跡に近付けることができ、ロボット２の動的精度を向上させることができ、ティーチングに要する時間を短縮することができる。

また、計算した軸毎のサーボ遅れ時間のうち最短のサーボ遅れ時間を基準時間として決定するようにしたので、時間軸での遅れを極力抑えることができる。また、計算した軸毎の補償トルクと計算した軸毎のサーボ遅れ時間とに基づいて軌跡誤差を推定し、その推定した軌跡誤差が許容値以上であることを判定すると、ロボット２の動作速度を低減させるようにしたので、ロボットの動作を一定範囲内で安定させることができ、周囲設備など対する干渉を極力抑えることができる。

さらに、軸毎に指令値を多項式で補間することで、指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算するようにしたので、サーボ遅れ時間の計算精度を高めることができ、ロボット２の動的精度を確実に向上させることができる。

本発明は、上記した実施形態にのみ限定されるものではなく、以下のように変形又は拡張することができる。
垂直多関節型ロボットは、６軸のものに限られない。

本発明の一実施形態を示すもので、産業用ロボット装置の斜視図 機能ブロック図 フローチャート サーボ遅れ時間を概略的に示す図

符号の説明

図面中、２は垂直多関節型ロボット（ロボット）、３は制御装置（動作制御手段、サーボ遅れ時間計算手段、基準時間決定手段、補償量計算手段、軌跡誤差推定手段、軌跡誤差判定手段、動作速度低下手段）、６はショルダ部（駆動部）、７〜９はアーム（駆動部）、１０は手首（駆動部）、１１はフランジ（駆動部）である。

Claims (6)

1. 複数の駆動部毎に指令値を与えて前記複数の駆動部を連動させることでロボットの動作を制御する動作制御手段を備えたロボットの動作制御装置において、
   前記複数の駆動部に対応する軸毎に、前記指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算するサーボ遅れ時間計算手段と、
   前記サーボ遅れ時間計算手段により計算された軸毎のサーボ遅れ時間を比較して軸毎のサーボ遅れ時間が複数の軸の全てで一致するように基準時間を決定する基準時間決定手段と、
   前記サーボ遅れ時間計算手段により計算された軸毎のサーボ遅れ時間と前記基準時間決定手段により決定された基準時間とに基づいて軸毎の補償量を計算する補償量計算手段とを備え、
   前記サーボ遅れ時間計算手段は、時刻ｔａにおける指令値からモデル化して算出される目標速度、及び前記時刻ｔａにおける前記複数の駆動部に対応する軸の実速度に基づいて、前記時刻ｔａと、前記モデル化した目標速度において前記時刻ｔａにおける前記実速度に相当する指令値が出力された時刻ｔとの差をサーボ遅れ時間として算出し、
   前記補償量計算手段は、前記複数の駆動部に対応する軸の前記サーボ遅れ時間の平均値を基準時間Ｔとして、この基準時間Ｔと前記複数の駆動部に対応する軸毎のサーボ遅れ時間とに基づいて前記補償量を算出し、
   前記動作制御手段は、前記補償量計算手段により計算された軸毎の補償量を反映した指令値を前記複数の駆動部毎に与えて前記複数の駆動部を連動させることでロボットの動作を制御することを特徴とするロボットの動作制御装置。
2. 請求項１に記載したロボットの動作制御装置において、
   前記補償量計算手段により計算された軸毎の補償量と前記サーボ遅れ時間計算手段により計算された軸毎のサーボ遅れ時間とに基づいて軌跡誤差を推定する軌跡誤差推定手段と、
   前記軌跡誤差推定手段により推定された軌跡誤差を判定する軌跡誤差判定手段と、
   前記軌跡誤差推定手段により推定された軌跡誤差が許容値以上であることを前記軌跡誤差判定手段が判定したときに、前記ロボットの動作速度を低減させる動作速度低下手段とを備えたことを特徴とするロボットの動作制御装置。
3. 請求項１又は２に記載したロボットの動作制御装置において、
   前記サーボ遅れ時間計算手段は、前記複数の駆動部に対応する軸毎に、前記指令値を多項式で補間することで、前記指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算することを特徴とするロボットの動作制御装置。
4. 複数の駆動部毎に指令値を与えて前記複数の駆動部を連動させることでロボットの動作を制御する方法において、
   前記複数の駆動部に対応する軸毎に、時刻ｔａにおける前記指令値に対してモデル化して算出される目標速度と前記時刻ｔａにおける前記複数の駆動部に対応する軸の実際の動作速度とに基づいて、前記時刻ｔａと、前記モデル化した目標速度において前記時刻ｔａにおける前記実際の動作速度に相当する指令値が出力された時刻ｔとの差をサーボ遅れ時間として計算し、その計算した軸毎のサーボ遅れ時間の平均値を基準時間Ｔとして決定し、その計算した軸毎のサーボ遅れ時間と当該決定した基準時間とに基づいて軸毎の補償量を計算し、その決定した軸毎の補償量を反映した指令値を前記複数の駆動部毎に与えて前記複数の駆動部を連動させることでロボットの動作を制御することを特徴とするロボットの動作制御方法。
5. 請求項４に記載したロボットの動作制御方法において、
   決定した軸毎の補償量と計算した軸毎のサーボ遅れ時間とに基づいて軌跡誤差を推定し、その推定した軌跡誤差を判定し、その推定した軌跡誤差が許容値以上であることを判定したときに、前記ロボットの動作速度を低減させることを特徴とするロボットの動作制御方法。
6. 請求項４又は５に記載したロボットの動作制御方法において、
   前記複数の駆動部に対応する軸毎に、前記指令値を多項式で補間することで、前記指令値に対する目標軌跡と実際の動作軌跡との差をサーボ遅れ時間として計算することを特徴とするロボットの動作制御方法。

Images