JP6455019B2 - ロボットの制御装置及び制御方法 - Google Patents

Description

本発明は、ロボットの制御装置及び制御方法に関する。

従来、ロボットのアームを駆動するサーボモータの速度パターンを、位置及び速度の平面における関係を時間軸上の関係に変換することで最適化する方法がある（非特許文献１，２参照）。非特許文献１，２に記載の方法によれば、アームの各位置において最大の速度となるように速度パターンが作成されるため、アームの動作時間を短縮することができる。

The International Journal of Robotics Research, vol.4, No.3, Fall 1985 p3-17 "Time-Optimal Control of Robotic Manipulators Along Specified Paths" by J. E. Bobrow, S. Dubowsky, J. S. Gibson IEEE JOURNAL OF ROBOTICS AND AUTOMATION, VOL.4, NO.4, AUGUST 1988 p443-450 "Optimal Robot Path Planning Using the Minimum-Time Criterion" by J. E. Bobrow

ところで、アームの動作開始点から動作終了点までの経由点を適切化することによっても、アームの動作時間を短縮することができる。このため、アームの経由点を変更しつつ、非特許文献１，２に記載の方法で速度パターンを最適化することにより、アームの動作時間を更に短縮することができる。

しかしながら、非特許文献１，２に記載の方法では、速度パターンを最適化する計算の負荷が大きい。このため、アームの経由点の変更と速度パターンの最適化とを多数組み合わせて繰り返し行うには、非常に長い計算時間を要することとなり、こうした方法は実用的ではない。

本発明は、こうした実情に鑑みてなされたものであり、アームの動作時間を短縮することのできる実用的なロボットの制御装置及び制御方法を提供することを主たる目的とするものである。

第１の手段は、複数のアームと、前記複数のアームを連結する関節と、各関節に設けられ前記アームを駆動するサーボモータと、を備えるロボットの制御装置であって、少なくとも１つのアームよりも先端側に連結された所定アームの所定点が、動作開始点から動作終了点まで移動する際に経由する経由点を設定する経由点設定手段と、前記動作開始点から、前記経由点設定手段により設定された前記経由点を経由して前記動作終了点まで、前記所定点を移動させる際の各サーボモータの一次速度パターンを算出する一次速度パターン算出手段と、前記一次速度パターン算出手段により算出された前記一次速度パターンで各サーボを駆動する際の各サーボモータの負荷率を算出する負荷率算出手段と、前記負荷率算出手段により算出された前記負荷率が最も大きいサーボモータである対象サーボモータの前記一次速度パターンを維持しつつ、前記対象サーボモータの負荷率を低下させるように前記経由点設定手段により前記経由点を再設定させる経由点更新手段と、前記経由点設定手段により初期に設定された前記経由点を経由する際に前記負荷率算出手段により算出された前記対象サーボモータの初期の負荷率から、前記経由点設定手段により再設定された前記経由点を経由する際に前記負荷率算出手段により算出された前記対象サーボモータの負荷率への低下度合が所定度合を超えた場合に、前記一次速度パターン算出手段により算出された各サーボモータの一次速度パターンを二次速度パターンとして取得する二次速度パターン取得手段と、を備えることを特徴とする。

上記構成によれば、少なくとも１つのアームよりも先端側に連結された所定アームの所定点が、動作開始点から動作終了点まで移動する際に経由する経由点が設定される。所定アームは、先端のアーム（ハンド部）でもよいし、先端のアームに限らず少なくとも１つのアームよりも先端側に連結されたアームであればよい。所定点は、アームの先端の点でもよいし、アームの中間の点等でもよい。

一次速度パターン算出手段により、動作開始点から上記経由点を経由して動作終了点まで、上記所定点を移動させる際の各サーボモータの一次速度パターンが算出される。この一次速度パターンは、従来の台形パターン等でもよいし、上記非特許文献１，２と同様の最適の速度パターンでもよい。そして、負荷率算出手段によって、一次速度パターン算出手段により算出された一次速度パターンで各サーボを駆動する際の各サーボモータの負荷率が算出される。負荷率は各サーボモータの最大負荷に対する負荷の比率であり、負荷として電流やトルク、電流二乗値等を用いることができる。

ここで、負荷率が最も大きいサーボモータである対象サーボモータは、アームの動作時間を短縮する上で制約となっているサーボモータである。このため、所定点の経由点を再設定することにより、所定アームの動作を変更し、先端側に所定アームが連結された基端側のアームの動作をアシストする力を発生させることが有効である。そこで、対象サーボモータの一次速度パターンを維持しつつ、対象サーボモータの負荷率を低下させるように経由点が再設定される。経由点の更新においては、滑降シンプレックス法や、勾配法、遺伝アルゴリズム等を用いることができる。経由点が再設定されると、一次速度パターン算出手段により各サーボモータの一次速度パターンが再度算出される。算出された各サーボモータの一次速度パターンによれば、対象サーボモータの一次速度パターンを維持しつつ、対象サーボモータの負荷率、ひいては温度上昇を抑制することができる。こうした処理は、一次速度パターンの最適化を行わず、所定アームの所定点が経由する経由点の適切化を行うものであるため、計算時間が過度に長くならず実用的である。

そして、初期に設定された経由点を経由する際に算出された対象サーボモータの初期の負荷率から、再設定された経由点を経由する際に算出された対象サーボモータの負荷率への低下度合が所定度合を超えた場合に、算出された各サーボモータの一次速度パターンが二次速度パターンとして取得される。このため、二次速度パターンによれば、対象サーボモータの負荷率を所定度合低下させることができ、ひいては温度上昇を抑制することができる。したがって、例えば対象サーボモータの温度上昇に起因して駆動が制限されている場合等に、駆動の制限を解除することができ、ひいてはアームの動作時間を短縮することができる。

第２の手段では、前記二次速度パターン取得手段により前記二次速度パターンが取得された際の前記経由点を維持しつつ、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように前記二次速度パターンを最適化して三次速度パターンを取得する三次速度パターン取得手段を備える。

上記構成によれば、二次速度パターン取得手段により二次速度パターンが取得された際の経由点が維持される。そして、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように二次速度パターンが最適化され、三次速度パターンとして取得される。

ここで、二次速度パターンは、アームの経由点の適切化が行われて算出された速度パターンであり、対象サーボモータの負荷率が一次速度パターンでの負荷率よりも低下させられている。このため、二次速度パターンでは対象サーボモータの負荷率に余裕があり、二次速度パターンを最適化して動作時間を短縮することができる。したがって、三次速度パターンによれば、アームの経由点の変更と速度パターンの最適化とを繰り返し行わない場合であっても、アームの動作時間を更に短縮することができる。

第３の手段では、前記二次速度パターン取得手段により前記二次速度パターンが取得された際の前記経由点に基づいて前記経由点設定手段により前記経由点を再設定させて、前記三次速度パターン取得手段により取得された前記三次速度パターンを、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように最適化することを、所定度合まで繰り返し行って四次速度パターンを取得する四次速度パターン取得手段を備える。

上記構成によれば、二次速度パターン取得手段により二次速度パターンが取得された際の経由点に基づいて、経由点設定手段により経由点が再設定される。二次速度パターンが取得された際の経由点は既に適切化されているため、この経由点に基づいて経由点を再設定することにより経由点の最適化を容易に行うことができる。そして、三次速度パターン取得手段により取得された三次速度パターンが、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように最適化される。こうした処理を所定度合まで繰り返し行うことにより、より少ない繰り返し回数及びより少ない計算負荷で、アームの動作時間を十分に短縮することができる。

第４の手段では、前記負荷率算出手段は、前記負荷率として、各サーボモータに流すことのできる上限電流の二乗値の積算値に対する駆動時における電流の二乗値の積算値の比率を算出する。

サーボモータの温度は、駆動時の電流の二乗値に比例して上昇する。この点、上記構成によれば、負荷率として、各サーボモータに流すことのできる上限電流の二乗値の積算値に対する駆動時における電流の二乗値の積算値の比率が算出される。したがって、各サーボモータの温度上昇を適切に抑制し、対象サーボモータの温度上昇に起因する駆動制限を抑制することができ、ひいてはアームの動作時間を短縮することができる。

第５の手段では、前記負荷率算出手段は、前記負荷率として、各サーボモータに流すことのできる上限電流に対する駆動時における最大電流の比率を算出する。

通常、各サーボモータには、流すことのできる電流の上限である上限電流が設定されている。この点、上記構成によれば、負荷率として、各サーボモータに流すことのできる上限電流に対する駆動時における最大電流の比率が算出される。したがって、各サーボモータの駆動時における最大電流を抑制し、対象サーボモータの上限電流に起因する駆動制限を抑制することができ、ひいてはアームの動作時間を短縮することができる。

第６の手段は、複数のアームと、前記複数のアームを連結する関節と、各関節に設けられ前記アームを駆動するサーボモータと、を備えるロボットの制御方法であって、少なくとも１つのアームよりも先端側に連結された所定アームの所定点が、動作開始点から動作終了点まで移動する際に経由する経由点を設定する経由点設定工程と、前記動作開始点から、前記経由点設定工程により設定された前記経由点を経由して前記動作終了点まで、前記所定点を移動させる際の各サーボモータの一次速度パターンを算出する一次速度パターン算出工程と、前記一次速度パターン算出工程により算出された前記一次速度パターンで各サーボを駆動する際の各サーボモータの負荷率を算出する負荷率算出工程と、前記負荷率算出工程により算出された前記負荷率が最も大きいサーボモータである対象サーボモータの前記一次速度パターンを維持しつつ、前記対象サーボモータの負荷率を低下させるように前記経由点設定工程により前記経由点を再設定させる経由点更新工程と、前記経由点設定工程により初期に設定された前記経由点を経由する際に前記負荷率算出工程により算出された前記対象サーボモータの初期の負荷率から、前記経由点設定工程により再設定された前記経由点を経由する際に前記負荷率算出工程により算出された前記対象サーボモータの負荷率への低下度合が所定度合を超えた場合に、前記一次速度パターン算出工程により算出された各サーボモータの一次速度パターンを二次速度パターンとして取得する二次速度パターン取得工程と、を備えることを特徴とする。

上記工程によれば、第1の手段と同様の作用効果を奏することができる。

ロボットシステムの概要を示す斜視図。 最適の速度パターンを作成する処理を示すフローチャート。 対象サーボモータの速度パターン及び電流パターン、並びに対象サーボモータよりも１つ先端側のサーボモータの速度パターン及び電流パターンを示すグラフ。 対象サーボモータの負荷率を低下させるアームの動作を示す模式図。 図３のグラフに対して経由点を適切化したグラフ。 図５のグラフに対して速度を最大化したグラフ。

以下、一実施形態について、図面を参照しつつ説明する。本実施形態は、機械組立工場等において、機械等の組み立てを行うロボットシステムとして具体化している。

図１は、ロボットシステム１０の概要を示す斜視図である。同図に示すように、ロボットシステム１０は、ロボット２０及びロボットコントローラ３０を備えている。

ロボット２０は、垂直多関節型のロボットであり、複数のアーム、及び基台２２を備えている。複数のアーム（被駆動部）は、基台２２側から順に、アーム２１Ａ、アーム２１Ｂ、アーム２１Ｃ、及びハンド部（アーム）２１Ｄを含んでいる。アーム２１Ａとアーム２１Ｂとが関節２３Ａにより連結され、アーム２１Ｂとアーム２１Ｃとが関節２３Ｂにより連結され、アーム２１Ｃとハンド部２１Ｄとが関節２３Ｃにより連結されている。なお、アーム２１Ｃは、相対回転する２つの部分に分かれている。

各関節（２３Ａ，２３Ｂ，２３Ｃのみ符号を付す）には、それぞれサーボモータ２５，２６（関節２３Ｃのサーボモータは図示略）が設けられており、これらのサーボモータの回転によりアーム２１Ａ〜２１Ｄが駆動される。各サーボモータには、その出力軸を制動する電磁ブレーキと、出力軸の回転角度に応じたパルス信号を出力するエンコーダとがそれぞれ設けられている。ロボット２０は、アーム２１Ａ〜２１Ｄを動作させることにより、ワークに対する部品の組付けやワークの搬送等の作業を行う。

コントローラ３０（制御装置）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、位置検出回路等を備えている。ＲＯＭは、ロボット２０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたサーボモータの回転角度を検出する。ＣＰＵは、動作プログラム（プログラム）を実行することにより、位置検出回路から入力される位置情報に基づいて、アーム２１Ａ〜２１Ｄの各関節の回転角度を開始回転角度から終了回転角度まで制御する。本実施形態では、この制御に先立って、各サーボモータの最適の速度パターンを作成し、作成した速度パターンをロボット２０の自動運転時の動作において実現する。

次に、図２のフローチャートを参照して、各サーボモータの最適の速度パターンを作成する処理の概要を説明する。この一連の処理は、コントローラ３０によって実行される。

まず、アーム２１Ａ〜２１Ｄのうち、所定アームの所定点を動作させる際の動作開始点及び動作終了点を読み込む（Ｓ１１）。動作開始点及び動作終了点は、オペレータや上位のコントローラにより設定されている。所定アームは、ロボット２０において少なくとも１つのアームよりも先端側に連結されたアームであればよく、本実施形態ではアーム２１Ｃを所定アームとする。所定点は、所定アームにおいて基端側の関節よりも先端側の点であればよく、本実施形態では、アーム２１Ｃの先端の点を所定点とする。

続いて、動作開始点と動作終了点との間に初期経由点を設定する（Ｓ１２）。例えば、動作開始点と動作終了点との間を線形補間して、初期経由点として３点を設定する。経由点は、所定アームの所定点を動作開始点から動作終了点まで移動させる際に、所定点が経由する点である。

続いて、動作開始点、初期経由点、及び動作終了点の間を、例えばスプライン補間等により補間処理して（Ｓ１３）、補間後の軌道について上述した非特許文献１，２に記載のボブロー法を用いて移動時間が最短となる各サーボモータの速度パターン（一次速度パターン）を算出する（Ｓ１４）。なお、ボブロー法の詳細については、非特許文献１，２を参照。例えば、図３に示すように、アーム２１Ｂを駆動するサーボモータ２５の速度ｖａ，電流ｉａが算出され、アーム２１Ｃを駆動するサーボモータ２６の速度ｖｂ，電流ｉｂが算出される。各サーボモータの位置（回転角度）、速度（角速度）、及び加速度（角加速度）に基づいて、逆動力学計算により電流を計算することができる。

続いて、サーボモータのうち、負荷率が最も大きいサーボモータ（対象サーボモータ）を選択する（Ｓ１５）。詳しくは、負荷率として、一次速度パターンにより各サーボモータを駆動する場合において、各サーボモータに流すことのできる上限電流の二乗値の積算値に対する駆動時における電流の二乗値の積算値の比率を算出する。そして、サーボモータのうち負荷率が最も大きいサーボモータを、負荷率を低下させるべき対象サーボモータとして選択する。図３では、サーボモータ２５（速度ｖａ，電流ｉａ）の負荷率が、サーボモータ２６（速度ｖｂ，電流ｉｂ）の負荷率よりも大きいため、サーボモータ２５が対象サーボモータとして選択される。

続いて、対象サーボモータの一次速度パターンを維持しつつ、対象サーボモータの負荷率を低下させるように経由点を更新する（Ｓ１６）。詳しくは、図４（ａ），（ｂ）に示すように、アーム２１Ｃの先端の点を動作開始点から移動させる際には、アーム２１Ｂを動作終了点の方向に回転させると共に、アーム２１Ｃを動作終了点とは逆方向に回転させる。これにより、アーム２１Ｂの駆動開始時に、アーム２１Ｃの動作によって生じる反作用がアーム２１Ｂの加速をアシストするようになる。その後、アーム２１Ｃを動作終了点の方向へ回転させる。また、図４（ｃ），（ｄ）に示すように、アーム２１Ｃの先端の点を動作終了点で停止させる際には、アーム２１Ｃを動作終了点の方向に回転させる。これにより、アーム２１Ｂの駆動終了時に、アーム２１Ｃの動作によって生じる反作用がアーム２１Ｂの減速をアシストするようになる。その後、アーム２１Ｃを動作終了点とは逆方向へ回転させる。このように経由点を更新（動作起動を生成）することで、対象サーボモータの負荷率を低下させることができる。なお、この経由点の適切化においては、滑降シンプレックス法等、公知の最適化アルゴリズムを利用することができる。

続いて、動作開始点、更新された経由点、及び動作終了点の間を、例えばスプライン補間等により補間処理して（Ｓ１７）、補間後の軌道について対象サーボモータ以外の各サーボモータの速度パターンを更新する（Ｓ１８）。ここでは、対象サーボモータの一次速度パターンは維持されているため、この一次速度パターンを基準に、補間された経由点を所定点が経由するように対象サーボモータ以外の各サーボモータの速度パターンを算出する。

続いて、対象サーボモータを上記一次速度パターンにより駆動し、対象サーボモータ以外のサーボモータを更新された速度パターンにより駆動した場合において、対象サーボモータの負荷率を算出する（Ｓ１９）。この負荷率の算出方法は、Ｓ１５の処理と同様である。このとき、図５に示すように、アーム２１Ｂの動作をアシストするようにアーム２１Ｃが動作するため、アーム２１Ｂを駆動するサーボモータ２５の電流ｉａ（実線）の最大値は、経由点を更新する前の電流ｉａ（破線）よりも低下する。なお、サーボモータ２５（対象サーボモータ）の一次速度パターンは維持されているため、動作開始から動作終了までの期間Ｔ１は、図３と図５とで同一である。

続いて、経由点の適切化が終了したか否か判定する（Ｓ２０）。詳しくは、Ｓ１２で設定された初期経由点を経由する際にＳ１５で算出されたサーボモータ２５（対象サーボモータ）の初期の負荷率から、Ｓ１６で更新された経由点を経由する際にＳ１９で算出されたサーボモータ２５の負荷率への低下度合が所定度合を超えた場合に、経由点の適切化が終了したと判定する。具体的には、Ｓ１９で算出された負荷率と前回算出された負荷率との差が所定範囲内に収束した、又は経由点の更新と速度パターンの更新とが所定回数実行された場合に、経由点の適切化が終了したと判定する。この判定において、経由点の適切化が終了していないと判定した場合（Ｓ２０：ＮＯ）、Ｓ１６の処理から再度実行する。

一方、Ｓ２０の判定において、経由点の適切化が終了したと判定した場合（Ｓ２０：ＹＥＳ）、Ｓ１８で算出された速度パターンを二次速度パターンとして取得し、Ｓ２１の処理へ進む。

そして、適切化が終了した経由点についてボブロー法を用いて移動時間が最短となる各サーボモータの速度パターンを算出する（Ｓ２１）。１回目にＳ２１の処理を実行して算出した速度パターンは、三次速度パターンに相当する。すなわち、三次速度パターンは、二次速度パターンが取得された際の経由点を維持しつつ、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように二次速度パターンを最適化した速度パターンである。

続いて、Ｓ２１で算出した速度パターンによるロボット２０のアーム２１Ａ〜２１Ｄの動作時間を算出する（Ｓ２２）。ここで、二次速度パターンは、アーム２１Ｃ（所定アーム）の先端の点（所定点）の経由点を適切化して算出した速度パターンであり、図５に示すように、サーボモータ２５（対象サーボモータ）の電流ｉａ（実線）が一次速度パターンでの電流ｉａ（破線）よりも低下させられている。このため、図６に示すように、三次速度パターンでは、サーボモータ２５の電流ｉａ（実線）の最大値を、二次速度パターンでの電流ｉａ（破線）よりも上昇させることができる。その結果、サーボモータ２５において、三次速度パターン（実線）での動作開始から動作終了までの期間Ｔ２は、二次速度パターン（破線）での動作開始から動作終了までの期間Ｔ１よりも短くなっている。この期間Ｔ２を動作時間として算出する。

続いて、速度パターンの最適化が終了したか否か判定する（Ｓ２３）。詳しくは、二次速度パターンによるロボット２０のアームの動作時間（期間Ｔ１）から、Ｓ２１で算出された速度パターンによる動作時間への低下度合が所定度合を超えた場合に、速度パターンの最適化が終了したと判定する。具体的には、Ｓ２２で算出された動作時間と前回算出された動作時間との差が所定範囲内に収束した、又は最大速度パターンの算出（Ｓ２１）が所定回数実行された場合に、速度パターンの最適化が終了したと判定する。この判定において、速度パターンの最適化が終了していないと判定した場合（Ｓ２３：ＮＯ）、対象サーボモータの負荷率を低下させるように経由点を更新する（Ｓ２４）。対象サーボモータの負荷率を低下させる経由点の考え方は、Ｓ１６の処理と同様である。なお、この経由点の適切化においては、滑降シンプレックス法等、公知の最適化アルゴリズムを利用することができる。

続いて、動作開始点、更新された経由点、及び動作終了点の間を、例えばスプライン補間等により補間処理して（Ｓ２５）、補間後の軌道について各サーボモータの最大速度パターンを再度算出する（Ｓ２１）。その後、Ｓ２２の処理から再度実行する。

一方、Ｓ２３の判定において、速度パターンの最適化が終了したと判定した場合（Ｓ２３：ＹＥＳ）、Ｓ２１で算出された速度パターンを四次速度パターンとして取得し、この一連の処理を終了する（ＥＮＤ）。すなわち、四次速度パターンは、三次速度パターンを、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように最適化することを、所定度合まで繰り返し行って取得した速度パターンである。

なお、Ｓ１２の処理が経由点設定手段としての処理（経由点設定工程）に相当し、Ｓ１４及びＳ１８の処理が一次速度パターン算出手段としての処理（一次速度パターン算出工程）に相当し、Ｓ１５及びＳ１９の処理が負荷率算出手段としての処理（負荷率算出工程）に相当し、Ｓ１６の処理が経由点更新手段としての処理（経由点更新工程）に相当し、Ｓ２０の処理が二次速度パターン取得手段としての処理（二次速度パターン取得工程）に相当し、Ｓ２１の処理が三次速度パターン取得手段としての処理（三次速度パターン取得工程）に相当し、Ｓ２１〜Ｓ２５の処理が四次速度パターン取得手段としての処理（四次速度パターン取得工程）に相当する。

以上詳述した本実施形態は、以下の利点を有する。

・負荷率が最も大きいサーボモータである対象サーボモータ（サーボモータ２５）は、アーム２１Ａ〜２１Ｄの動作時間を短縮する上で制約となっているサーボモータである。このため、所定点（アーム２１Ｃの先端の点）の経由点を再設定することにより、所定アーム（アーム２１Ｃ）の動作を変更し、先端側に所定アームが連結された基端側のアーム２１Ｂの動作をアシストする力を発生させることが有効である。そこで、対象サーボモータの一次速度パターンを維持しつつ、対象サーボモータの負荷率を低下させるように経由点が再設定される。経由点が再設定されると、Ｓ１８の処理により各サーボモータの一次速度パターンが再度算出される。算出された各サーボモータの一次速度パターンによれば、対象サーボモータの一次速度パターンを維持しつつ、対象サーボモータの負荷率、ひいては温度上昇を抑制することができる。こうした処理は、一次速度パターンの最適化を行わず、所定アームの所定点が経由する経由点の適切化を行うものであるため、計算時間が過度に長くならず実用的である。

・Ｓ１２の処理で初期に設定された経由点を経由する際に算出された対象サーボモータの初期の負荷率から、Ｓ１６の処理で再設定された経由点を経由する際に算出された対象サーボモータの負荷率への低下度合が所定度合を超えた場合に、算出された各サーボモータの一次速度パターンが二次速度パターン（図５参照）として取得される。このため、二次速度パターンによれば、対象サーボモータの負荷率を所定度合低下させることができ、ひいては温度上昇を抑制することができる。したがって、例えば対象サーボモータ（サーボモータ２５）の温度上昇に起因して駆動が制限されている場合等に、駆動の制限を解除することができ、ひいてはアーム２１Ａ〜２１Ｄの動作時間を短縮することができる。

・二次速度パターンが取得された際の経由点が維持され、Ｓ２１の処理で各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように二次速度パターンが最適化され、三次速度パターン（図６参照）として取得される。ここで、二次速度パターンは、所定アームの所定点が経由する経由点の適切化が行われて算出された速度パターンであり、対象サーボモータ（サーボモータ２５）の負荷率が一次速度パターンでの負荷率よりも低下させられている。このため、二次速度パターンでは対象サーボモータの負荷率に余裕があり、二次速度パターンを最適化して動作時間を短縮することができる。したがって、三次速度パターンによれば、所定アームの所定点が経由する経由点の変更と速度パターンの最適化とを繰り返し行わない場合であっても、アーム２１Ａ〜２１Ｄの動作時間を更に短縮することができる。

・Ｓ２０の処理により二次速度パターンが取得された際の経由点に基づいて、Ｓ２４の処理により経由点が再設定される。二次速度パターンが取得された際の経由点は既に適切化されているため、この経由点に基づいて経由点を再設定することにより経由点の最適化を容易に行うことができる。そして、Ｓ２１の処理により取得された速度パターンが、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように最適化される。こうした処理を所定度合まで繰り返し行うことにより、より少ない繰り返し回数及びより少ない計算負荷で、アーム２１Ａ〜２１Ｄの動作時間を十分に短縮することができる。

・サーボモータの温度は、駆動時の電流の二乗値に比例して上昇する。この点、負荷率として、各サーボモータに流すことのできる上限電流の二乗値の積算値に対する駆動時における電流の二乗値の積算値の比率が算出される。したがって、各サーボモータの温度上昇を適切に抑制し、対象サーボモータの温度上昇に起因する駆動制限を抑制することができ、ひいてはアーム２１Ａ〜２１Ｄの動作時間を短縮することができる。

なお、上記実施形態を、以下のように変更して実施することもできる。

・一次速度パターンは、ボブロー法等を用いて算出した移動時間が最短となる各サーボモータの速度パターンに限らず、従来の台形パターン等でもよい。この場合であっても、対象サーボモータの一次速度パターンを維持しつつ、対象サーボモータの負荷率を低下させるように経由点を再設定すればよい。その結果、対象サーボモータの一次速度パターンを変えることなく、対象サーボモータの負荷率、ひいては温度上昇を抑制することができる。

・負荷率は、各サーボモータに流すことのできる上限電流の二乗値の積算値に対する駆動時における電流の二乗値の積算値の比率に限らず、各サーボモータの最大負荷に対する駆動時の負荷の比率であればよい。負荷としては、電流やトルク、電流二乗値、トルク二乗値、電流積算値、トルク積算値等を用いることができる。また、負荷率として、各サーボモータの電流等を用いる場合には、サーボモータの定格電流の相違や、温度上昇量の相違に応じて重み付けを行うとよい。

・特に、負荷率として、各サーボモータに流すことのできる上限電流に対する駆動時における最大電流の比率を算出するといった構成を採用することが有効である。通常、各サーボモータには、流すことのできる電流の上限である上限電流が設定されている。この点、上記構成によれば、負荷率として、各サーボモータに流すことのできる上限電流に対する駆動時における最大電流の比率が算出される。したがって、各サーボモータの駆動時における最大電流を抑制し、対象サーボモータの上限電流に起因する駆動制限を抑制することができ、ひいてはアームの動作時間を短縮することができる。

・所定アームは、少なくとも１つのアームよりも先端側に連結されたアームであればよく、例えば先端のアーム（ハンド部２１Ｄ）でもよい。また、所定点は、アームの先端の点に限らず、アームの中間の点等でもよく、所定アームにおいて基端側の関節よりも先端側の点であればよい。

・垂直多関節型のロボット２０に代えて、水平多関節型のロボット等を採用することもできる。

２０…ロボット、２１Ａ…アーム、２１Ｂ…アーム、２１Ｃ…アーム、２１Ｄ…ハンド部（アーム）、２３Ａ…関節、２３Ｂ…関節、２３Ｃ…関節、２５…サーボモータ、２６…サーボモータ、３０…コントローラ（制御装置）。

Claims (6)

1. 複数のアームと、前記複数のアームを相対回転可能に連結する関節と、各関節に設けられ前記アームを駆動するサーボモータと、を備えるロボットの制御装置であって、
   少なくとも１つのアームよりも先端側に前記少なくとも１つのアームの回転方向と同方向及び逆方向に回転可能に連結された所定アームの所定点が、動作開始点から動作終了点まで移動する際に経由する経由点を設定する経由点設定手段と、
   前記動作開始点から、前記経由点設定手段により設定された前記経由点を経由して前記動作終了点まで、前記所定点を移動させる際の各サーボモータの一次速度パターンを算出する一次速度パターン算出手段と、
   前記一次速度パターン算出手段により算出された前記一次速度パターンで各サーボを駆動する際の各サーボモータの負荷率を算出する負荷率算出手段と、
   前記負荷率算出手段により算出された前記負荷率が最も大きいサーボモータである対象サーボモータの前記一次速度パターンを維持しつつ、前記対象サーボモータの負荷率を低下させるように前記経由点設定手段により前記経由点を再設定させる経由点更新手段と、
   前記経由点設定手段により初期に設定された前記経由点を経由する際に前記負荷率算出手段により算出された前記対象サーボモータの初期の負荷率から、前記経由点設定手段により再設定された前記経由点を経由する際に前記負荷率算出手段により算出された前記対象サーボモータの負荷率への低下度合が所定度合を超えた場合に、前記一次速度パターン算出手段により算出された各サーボモータの一次速度パターンを二次速度パターンとして取得する二次速度パターン取得手段と、
   を備えることを特徴とするロボットの制御装置。
2. 前記二次速度パターン取得手段により前記二次速度パターンが取得された際の前記経由点を維持しつつ、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように前記二次速度パターンを最適化して三次速度パターンを取得する三次速度パターン取得手段を備える請求項１に記載のロボットの制御装置。
3. 前記二次速度パターン取得手段により前記二次速度パターンが取得された際の前記経由点に基づいて前記経由点設定手段により前記経由点を再設定させて、前記三次速度パターン取得手段により取得された前記三次速度パターンを、各サーボモータの最大速度条件及び最大加減速度条件に基づいて最短の動作時間となるように最適化することを、所定度合まで繰り返し行って四次速度パターンを取得する四次速度パターン取得手段を備える請求項２に記載のロボットの制御装置。
4. 前記負荷率算出手段は、前記負荷率として、各サーボモータに流すことのできる上限電流の二乗値の積算値に対する駆動時における電流の二乗値の積算値の比率を算出する請求項１〜３のいずれか1項に記載のロボットの制御装置。
5. 前記負荷率算出手段は、前記負荷率として、各サーボモータに流すことのできる上限電流に対する駆動時における最大電流の比率を算出する請求項１〜３のいずれか1項に記載のロボットの制御装置。
6. 複数のアームと、前記複数のアームを相対回転可能に連結する関節と、各関節に設けられ前記アームを駆動するサーボモータと、を備えるロボットの制御方法であって、
   少なくとも１つのアームよりも先端側に前記少なくとも１つのアームの回転方向と同方向及び逆方向に回転可能に連結された所定アームの所定点が、動作開始点から動作終了点まで移動する際に経由する経由点を設定する経由点設定工程と、
   前記動作開始点から、前記経由点設定工程により設定された前記経由点を経由して前記動作終了点まで、前記所定点を移動させる際の各サーボモータの一次速度パターンを算出する一次速度パターン算出工程と、
   前記一次速度パターン算出工程により算出された前記一次速度パターンで各サーボを駆動する際の各サーボモータの負荷率を算出する負荷率算出工程と、
   前記負荷率算出工程により算出された前記負荷率が最も大きいサーボモータである対象サーボモータの前記一次速度パターンを維持しつつ、前記対象サーボモータの負荷率を低下させるように前記経由点設定工程により前記経由点を再設定させる経由点更新工程と、
   前記経由点設定工程により初期に設定された前記経由点を経由する際に前記負荷率算出工程により算出された前記対象サーボモータの初期の負荷率から、前記経由点設定工程により再設定された前記経由点を経由する際に前記負荷率算出工程により算出された前記対象サーボモータの負荷率への低下度合が所定度合を超えた場合に、前記一次速度パターン算出工程により算出された各サーボモータの一次速度パターンを二次速度パターンとして取得する二次速度パターン取得工程と、
   を備えることを特徴とするロボットの制御方法。

Images