JP7311971B2

JP7311971B2 - ロボット制御装置及びロボット制御方法

Info

Publication number: JP7311971B2
Application number: JP2019003152A
Authority: JP
Inventors: 智紀川▲崎▼
Original assignee: Individual
Current assignee: Individual
Priority date: 2019-01-11
Filing date: 2019-01-11
Publication date: 2023-07-20
Anticipated expiration: 2039-01-11
Also published as: JP2020110866A

Description

本明細書は、複数の関節軸を有するロボットの動作を制御するロボット制御装置及びロボット制御方法に関する技術を開示したものである。

従来より、特許文献１（特開平６－３２４７３０号公報）に記載されているように、複数の関節軸を有するロボットの動作を制御する場合に、動作始点から動作終点までの手先の動作軌跡を直線補間や円弧補間等により設定すると共に、手先の目標動作速度を設定して、その目標動作速度で手先が動作軌跡に沿って移動するように各関節軸の回転速度を制御するようにしたものがある。

この特許文献１には、手先を目標動作速度で移動させると、いずれかの関節軸の回転速度がモータの許容最高速度を超えて動作不能（過速度エラー）になることが課題として記載され、その課題を解決する手段として、過去のサンプリング周期毎の各関節軸の回転速度のデータをメモリに記憶しておき、その記憶データに基づいて次回の動作の各関節軸の回転速度を予測し、いずれかの関節軸の予測回転速度が許容最高速度を超える場合には、当該関節軸の回転速度を許容最高速度以下に制限するための補正係数を求め、次回の動作時に最も回転速度の大きい関節軸の回転速度が許容最高速度を超えないように目標回転速度を補正係数で制限して補間演算を行うことが記載されている。

特開平６－３２４７３０号公報

しかし、上記特許文献１の構成では、過速度エラー防止のために、過去のサンプリング周期毎の各関節軸の回転速度のデータを記憶し、その記憶データに基づいて次回の動作の各関節軸の回転速度を予測して補正係数を算出する処理を行うため、その演算負荷が大きくなって処理が遅れてタクトタイムが延びてしまう可能性がある。

上記課題を解決するために、５軸垂直多関節ロボットの動作を制御するロボット制御装置において、動作始点から動作終点までの手先の動作軌跡上で第１関節軸の軸心に最も近い点を算出する第１算出部と、前記第１算出部で算出した前記第１関節軸の軸心に最も近い点で前記第１関節軸が許容最高回転速度で回転したときの前記手先の動作軌跡に沿ったＸＹＺ方向の動作可能な最高速度Ｖ _max （以下「前記手先の動作可能な最高速度Ｖ _max 」という）を算出する第２算出部と、前記第２算出部で算出した前記手先の動作可能な最高速度Ｖ _maxを前記手先の目標動作速度として設定すると共に前記手先の動作軌跡に沿ったＸＹＺ方向の動作速度が前記手先の目標動作速度となるように前記各関節軸の目標回転速度を設定する目標動作速度設定部と、前記目標動作速度設定部で設定した前記各関節軸の目標回転速度に基づいて前記動作始点から前記動作終点までの前記各関節軸の回転速度を制御する動作制御部とを備え、前記第２算出部は、前記第１関節軸の軸心をＺ軸とし、前記第１関節軸の軸心の所定の高さ位置に原点を設定した３次元の座標系において、前記第１関節軸の軸心に最も近い点で前記第１関節軸が許容最高回転速度で回転したときのＸＹ平面上に投影した前記手先の動作軌跡に沿ったＸＹ方向の動作可能な最高速度Ｖ _XYmax を算出すると共に、前記手先の動作軌跡に沿ったＸＹＺ方向とＸＹ平面とのなす角度θc を求め、前記ＸＹ方向の動作可能な最高速度Ｖ _XYmax と前記角度θc とに基づいて前記手先の動作可能な最高速度Ｖ _maxを求めるようにしたものである。

一般に、ロボットの複数の関節軸のうち、回転速度が最も許容最高回転速度を超えやすい関節軸は、手先から最も離れた第１関節軸であり、且つ、手先の動作軌跡上で、第１関節軸の回転速度が最も許容最高回転速度を超えやすい点は、第１関節軸の軸心からの回転半径が最小となる点、つまり第１関節軸の軸心に最も近い点である。このような関係を考慮して、手先の動作軌跡のうち、第１関節軸の軸心に最も近い点で第１関節軸が許容最高回転速度で回転したときの手先の動作可能な最高速度を算出し、この最高速度を手先の目標動作速度として設定すると共に、手先の動作速度が目標動作速度となるように各関節軸の目標回転速度を設定して、各関節軸の回転速度を制御するようにすれば、手先が動作軌跡に沿って動作する際に、第１関節軸が許容最高回転速度を超えて過速度エラーが発生することを確実に防止できる。しかも、前記特許文献１の技術と比べて、過速度エラー防止のための演算処理が簡単で演算負荷を軽減でき、演算処理の高速化に貢献できる。

図１は一実施例のロボットの構成例を示す正面図である。図２はロボット制御装置の電気的構成を示すブロック図である。図３はロボット制御プログラムの処理の流れを示すフローチャートである。図４は動作始点、動作終点、第１関節軸の軸心、θa 、θb の関係を説明する図である。

以下、本明細書に開示した一実施例を図面を用いて説明する。
まず、図１に基づいてロボット１１の構成を説明する。

ロボット１１は、例えば５軸垂直多関節ロボットであり、工場フロア１２に設置された固定ベース１３と、この固定ベース１３上に第１関節軸１４（Ｊ１）を中心に回転可能に設けられた第１アーム１５と、この第１アーム１５の先端に第２関節軸１６（Ｊ２）によって旋回可能に設けられた第２アーム１７と、この第２アーム１７の先端に第３関節軸１８（Ｊ３）によって旋回可能に設けられた第３アーム１９と、この第３アーム１９の先端に第４関節軸２０（Ｊ４）によって旋回可能に設けられた手首部２１と、この手首部２１に第５関節軸２２（Ｊ５）を中心に回転可能且つ交換可能に取り付けられたエンドエフェクタ２３（手先）とから構成されている。これにより、手首部２１に取り付けたエンドエフェクタ２３は、その手首部２１の関節軸である第４関節軸２０によって旋回動作するようになっている。

この場合、エンドエフェクタ２３は、例えば、吸着ノズル、ハンド、グリッパ、溶接機等のいずれであっても良い。ロボット１１の第１～第５の各関節軸１４，１６，１８，２０，２２は、それぞれサーボモータ２５～２９（図２参照）により駆動されるようになっている。図２に示すように、各サーボモータ２５～２９には、それぞれ回転角を検出するエンコーダ３１～３５が設けられ、各エンコーダ３１～３５で検出した回転角の情報がサーボアンプ３６を経由して制御装置３７にフィードバックされる。これにより、制御装置７は、各エンコーダ３１～３５で検出した各サーボモータ２５～２９の回転角が各々の目標回転角と一致するようにサーボアンプ３６を介して各サーボモータ２５～２９をフィードバック制御することで、ロボット１１の各アーム１５，１７，１９と手首部２１とエンドエフェクタ２３の位置を各々の目標位置にフィードバック制御する。

図２の構成例では、サーボアンプ３６は、複数のサーボモータ２５～２９をフィードバック制御する多軸アンプであるが、サーボモータ２５～２９を１台ずつ別々のサーボアンプでフィードバック制御するようにしても良い。

図１に示すように、ロボット１１のアーム可動領域（手首部２１先端側のエンドエフェクタ２３が移動可能な領域）の所定位置には、作業対象となるワーク３０を作業エリア３８に供給する供給装置３９が設置されている。この供給装置３９は、コンベアで構成したものであっても良いし、振動式パーツフィーダ等、どの様な構成のパーツフィーダを用いても良い。

ロボット１１のアーム先端部である手首部２１には、作業エリア３８に供給されたワーク３０を上方から撮像するカメラ４０が取り付けられている。このカメラ４０は、光軸が第５関節軸２２と平行で、且つ、カメラ４０のレンズ４１が第５関節軸２２の先端側（アーム先端側）に位置するように固定されている。

以上のように構成したロボット１１の動作を制御するロボット制御ユニット４２は、図２に示すように、制御装置３７とサーボアンプ３６等を内蔵している。この場合、制御装置３７は、１台又は複数台のコンピュータ（ＣＰＵ）により構成され、画像処理部４３として機能すると共に、後述する図３のロボット制御プログラムを実行することで、動作軌跡設定部４４、第１算出部４５、第２算出部４６、目標動作速度設定部４７及び動作制御部４８としても機能する。

画像処理部４３は、カメラ４０で撮像した画像を処理して撮像対象であるワーク３０の位置を認識する。

動作軌跡設定部４４は、動作始点から動作終点までの手先の動作軌跡を直線補間により設定する。これにより、動作始点から動作終点までの手先の動作軌跡は、動作始点と動作終点とを結ぶ直線となる。

第１算出部４５は、上記動作軌跡設定部４４で設定した手先の動作軌跡上で第１関節軸１４の軸心に最も近い点を算出する。第１関節軸１４の軸心は、Ｚ方向（鉛直方向）に延びる。

第２算出部４６は、上記第１算出部４５で算出した第１関節軸１４の軸心に最も近い点で第１関節軸１４が許容最高回転速度で回転したときの手先の動作可能な最高速度を算出する。

目標動作速度設定部４７は、上記第２算出部４６で算出した手先の動作可能な最高速度を手先の目標動作速度として設定すると共に、手先の動作速度が手先の目標動作速度となるように第１～第５の各関節軸１４，１６，１８，２０，２２の目標回転速度を設定する。この際、第１関節軸１４の目標回転速度は許容最高回転速度に設定される。

動作制御部４８は、目標動作速度設定部４７で設定した第１～第５の各関節軸１４，１６，１８，２０，２２の目標回転速度に基づいて動作始点から動作終点までの各関節軸１４，１６，１８，２０，２２の回転速度を制御する。

次に、動作軌跡設定部４４、第１算出部４５、第２算出部４６、目標動作速度設定部４７及び動作制御部４８による処理内容を詳しく説明する。

最初に、第１関節軸１４の回転速度が最大になる点、すなわち手先の動作軌跡上で第１関節軸１４の軸心に最も近い点を算出する。

まず、ロボット１１の動作始点の座標を
（Ｘ，Ｙ，Ｚ）＝（ｘ1 ，ｙ1 ，ｚ1 ） …（１）
とし、動作終点の座標を
（Ｘ，Ｙ，Ｚ）＝（ｘ2 ，ｙ2 ，ｚ2 ） …（２）
とする。尚、この３次元の座標系は、第１関節軸１４の軸心をＺ軸とし、第１関節軸１４の軸心の所定の高さ位置に原点が設定され、ＸＹ平面は水平面となる。

このとき、Ｚ成分を無視してＸＹ平面のみで考えた場合、動作始点から動作終点までの手先の動作軌跡は、直線補間により動作始点と動作終点とを結ぶ直線となり、次式で表される。
Ｙ＝ａＸ＋ｂ …（３）
ａ＝（ｙ1 －ｙ2 ）／（ｘ1 －ｘ2 ） …（４）
ｂ＝ｙ1 －ａｘ1 …（５）

但し、上式が成り立つ条件として、次の（６）、（７）式を満たす必要がある。
ｘ1 ≠ｘ2 …（６）
ｙ1 ≠ｙ2 …（７）

もし、（６）式が満たされない場合、すなわちｘ1 ＝ｘ2 の場合、動作始点と動作終点とを結ぶ直線は、Ｙ軸に平行な直線となり、次式で表される。
Ｘ＝ｘ1 …（８）

また、（７）式が満たされない場合、すなわちｙ1 ＝ｙ2 の場合、動作始点と動作終点とを結ぶ直線は、Ｘ軸に平行な直線となり、次式で表される。
Ｙ＝ｙ1 …（９）

次に、ロボット１１の手先の動作軌跡を示す直線の式に対して法線かつ原点を通る関数を求める。その関数は、（６）、（７）式が満たされる場合、次式で表される。
Ｙ＝－（１／ａ）Ｘ …（１０）

（６）式が満たされない場合、関数は、
Ｙ＝０ …（１１）
となり、Ｘ軸と一致する直線となる。

（７）式が満たされない場合、関数は、
Ｘ＝０ …（１２）
となり、Ｙ軸と一致する直線となる。

尚、（６）、（７）式の両方が満たされない場合、すなわち手先がＺ方向にしか動作しない場合、第１関節軸１４は動作しないので、説明を省略する。

次に、手先の動作軌跡の関数とそれに対する原点を通る法線との交点の座標を求める。（６）、（７）式の両方が満たされる場合、（３）、（１０）式から交点の座標は次式で求められる。

（６）式のみが満たされない場合、交点のＸＹ座標は次式となる。
（Ｘ，Ｙ）＝（ｘ1 ，０） …（１４）
（７）式のみが満たされない場合、交点のＸＹ座標は次式となる。
（Ｘ，Ｙ）＝（０，ｙ1 ） …（１５）

ロボット１１の手先の動作軌跡の関数とそれに対する原点を通る法線との交点が手先の動作軌跡上にある場合、（１３）、（１４）、（１５）式のＸＹ座標が第１関節軸１４の軸心に最も近い点となる。

しかし、動作軌跡によっては、交点が動作軌跡上に無い場合がある。その場合は、動作始点か動作終点のどちらかが動作軌跡上で第１関節軸１４の軸心に最も近い点となる。動作始点か動作終点のどちらが第１関節軸１４の軸心に近いかは原点からの距離で決まる。従って、交点が動作軌跡上に無い場合、動作軌跡上で第１関節軸１４の軸心に最も近い点は、
ｘ1 ²＋ｙ1 ²＞ｘ2 ²＋ｙ2 ² …（１６）
の条件が満たされる場合には、次式となる。
（Ｘ，Ｙ）＝（ｘ2 ，ｙ2 ） …（１７）
（１６）式が満たされない場合には、次式となる。
（Ｘ，Ｙ）＝（ｘ1 ，ｙ1 ） …（１８）

以上により、ＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点を求めた後、この点で第１関節軸１４の許容最高回転速度から手先の動作可能な最高速度を次のようにして算出する。

ここで、簡略化のためにＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点を次式で定義する。
（Ｘ，Ｙ）＝（ｘi ，ｙi ） …（１９）

まず、ＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点から原点までのベクトルを次式で定義する。

次に、ＸＹ平面上で動作始点と動作終点との間を結ぶベクトルを定義する。
計算の都合上、ＸＹ平面上で動作始点と動作終点との間を結ぶベクトルは、ＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点から原点までのベクトルとなす角度を鈍角にしたいので、ベクトルの方向は動作始点と動作終点それぞれと第１関節軸１４の軸心との間の距離を参照して決定する。

（１６）式が満たされる場合は、ＸＹ平面上で動作始点と動作終点との間を結ぶベクトルは次式となる。

（１６）式が満たされない場合は、ＸＹ平面上で動作始点と動作終点との間を結ぶベクトルは次式となる。

従って、ＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点から原点までのベクトルとＸＹ平面上で動作始点と動作終点との間を結ぶベクトルとのなす角度θa （鈍角）は、（２０）、（２１）、（２２）式から次式で求められる。

次に、第１関節軸１４の速度方向と手先速度方向との角度差θb （図４参照）を算出する。
第１関節軸１４の速度方向は、ＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点から原点までのベクトルと直角となっているので、第１関節軸１４の速度方向と手先速度方向との角度差θb は、次式で求められる。
θb ＝θa －π／２ …（２４）

従って、第１関節軸１４の許容最高回転速度をω_maxとするとＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点で手先が動作可能なＸＹ平面上の最高速度Ｖ_XYmaxは次式で求められる。

このＸＹ平面上の最高速度Ｖ_XYmaxにＺ方向の速度成分を合成して手先が動作可能な最高速度を求めることができる。手先の動作軌跡とＸＹ平面とのなす角度θc は、次式で求められる。

従って、手先が動作可能な最高速度Ｖ_maxは、次式で求められる。
Ｖ_max＝Ｖ_XYmax／ｃｏｓθc …（２７）

以上説明した本実施例のロボット１１の制御は、制御装置３７によって図３のロボット制御プログラムに従って実行される。以下、図３のロボット制御プログラムの処理内容を説明する。

制御装置３７は、図３のロボット制御プログラムを起動すると、まず、ステップ１００で動作速度算出タイミングになるまで待機する。ここで、動作速度算出タイミングは、手先が動作終点に到達する毎に、次の動作を開始するまでの間に設定されたタイミングである。次の動作を開始する前であれば、動作速度算出タイミングはいつであっても良く、他の処理の演算負荷が少ない期間に動作速度算出タイミングを設定すれば良い。

動作速度算出タイミングになった時点で、ステップ１０１に進み、前記動作終点の座標を次の動作始点の座標とし、次の動作始点の座標を入力した後、ステップ１０２に進み、動作始点の座標と動作終点の座標からＸＹ平面上の動作軌跡上で第１関節軸１４の軸心に最も近い点を算出する。

この後、ステップ１０３に進み、第１関節軸１４の軸心に最も近い点で第１関節軸１４が許容最高回転速度で回転したときに手先が動作可能なＸＹ平面上の最高速度を算出する。この後、ステップ１０４に進み、手先が動作可能なＸＹ平面上の最高速度にＺ方向の速度成分を合成して手先が動作可能な最高速度を求めて、その最高速度を手先の目標動作速度として設定する。

この後、ステップ１０５に進み、手先の動作速度が目標動作速度となるように第１～第５の各関節軸１４，１６，１８，２０，２２の目標回転速度を設定する。この際、第１関節軸１４の目標回転速度は許容最高回転速度に設定する。

次のステップ１０６で、動作開始タイミングになるまで待機して、動作開始タイミングになった時点で、ステップ１０７に進み、各関節軸１４，１６，１８，２０，２２を各々の目標回転速度で動作させて手先を動作始点から動作終点まで動作させる。以後、手先が動作終点に到達する毎に、上述したステップ１００～１０７の処理を実行して、ロボット１１の動作を制御する。

一般に、ロボット１１の複数の関節軸１４，１６，１８，２０，２２のうち、回転速度が最も許容最高回転速度を超えやすい関節軸は、手先から最も離れた第１関節軸１４であり、且つ、手先の動作軌跡のうち、第１関節軸１４の回転速度が最も許容最高回転速度を超えやすい点は、第１関節軸１４の軸心からの回転半径が最小となる点、つまり第１関節軸１４の軸心に最も近い点である。

このような関係を考慮して、本実施例では、手先の動作軌跡のうち、第１関節軸１４の軸心に最も近い点で第１関節軸１４が許容最高回転速度で回転したときの手先の動作可能な最高速度を算出し、この最高速度を手先の目標動作速度として設定すると共に、手先の動作速度が目標動作速度となるように各関節軸１４，１６，１８，２０，２２の目標回転速度を設定して、各関節軸１４，１６，１８，２０，２２の回転速度を制御するようにしている。これにより、手先が動作軌跡に沿って動作する際に、第１関節軸１４が許容最高回転速度を超えて過速度エラーが発生することを確実に防止できる。しかも、前記特許文献１の技術と比べて、過速度エラー防止のための演算処理が簡単で演算負荷を軽減でき、演算処理の高速化に貢献できる。

特に、本実施例のように、制御装置３７が画像処理部４３としても機能する場合は、その画像処理により制御装置３７の演算負荷が増大する傾向があるが、本実施例のロボット制御を行うことで、処理が遅れてタクトタイムが延びることを極力防止できる。

しかも、本実施例では、手先が動作可能なＸＹ平面上の最高速度を算出し、そのＸＹ平面上の最高速度にＺ方向の速度成分を合成することで手先の動作可能な最高速度を求めるようにしたので、手先の動作可能な最高速度を、Ｚ方向の速度成分を含む最高速度として求めることができ、第１関節軸１４以外の各関節軸１６，１８，２０，２２についても、各々の目標回転速度をより高い回転速度に設定することができる。但し、本発明は、Ｚ方向の速度成分を合成しないＸＹ平面上の最高速度を手先の目標動作速度に設定して各関節軸１６，１８，２０，２２の目標回転速度を設定しても良い。

尚、本実施例では、手先の動作軌跡を直線補間により設定したが、円弧補間等、他の方法で手先の動作軌跡を設定しても良い。
また、ロボット１１は、５軸垂直多関節ロボットに限定されず、４軸以下又は６軸以上の多関節ロボットであっても良い。

その他、本発明は、上記実施例に限定されず、例えば、カメラ４０と画像処理部４３を省略した構成としても良い等、要旨を逸脱しない範囲内で種々変更して実施できることは勿論である。

１１…ロボット、１４…第１関節軸、１５…第１アーム、１６…第２関節軸、１７…第２アーム、１８…第３関節軸、１９…第３アーム、２０…第４関節軸、２１…手首部（アーム先端部）、２２…第５関節軸、２３…エンドエフェクタ（手先）、２５～２９…サーボモータ、３１～３５…エンコーダ、３６…サーボアンプ、３７…制御装置、３８…作業エリア、３９…供給装置、４０…カメラ、４２…ロボット制御ユニット、４３…画像処理部、４４…動作軌跡設定部、４５…第１算出部、４６…第２算出部、４７…目標動作速度設定部、４８…動作制御部

Claims

５軸垂直多関節ロボットの動作を制御するロボット制御装置において、
動作始点から動作終点までの手先の動作軌跡上で第１関節軸の軸心に最も近い点を算出する第１算出部と、
前記第１算出部で算出した前記第１関節軸の軸心に最も近い点で前記第１関節軸が許容最高回転速度で回転したときの前記手先の動作軌跡に沿ったＸＹＺ方向の動作可能な最高速度Ｖ _max （以下「前記手先の動作可能な最高速度Ｖ _max 」という）を算出する第２算出部と、
前記第２算出部で算出した前記手先の動作可能な最高速度Ｖ _maxを前記手先の目標動作速度として設定すると共に前記手先の動作軌跡に沿ったＸＹＺ方向の動作速度が前記手先の目標動作速度となるように各関節軸の目標回転速度を設定する目標動作速度設定部と、
前記目標動作速度設定部で設定した前記各関節軸の目標回転速度に基づいて前記動作始点から前記動作終点までの前記各関節軸の回転速度を制御する動作制御部と
を備え、
前記第２算出部は、前記第１関節軸の軸心をＺ軸とし、前記第１関節軸の軸心の所定の高さ位置に原点を設定した３次元の座標系において、前記第１関節軸の軸心に最も近い点で前記第１関節軸が許容最高回転速度で回転したときのＸＹ平面上に投影した前記手先の動作軌跡に沿ったＸＹ方向の動作可能な最高速度Ｖ _XYmax を算出すると共に、前記手先の動作軌跡に沿ったＸＹＺ方向とＸＹ平面とのなす角度θc を求め、前記ＸＹ方向の動作可能な最高速度Ｖ _XYmax と前記角度θc とに基づいて前記手先の動作可能な最高速度Ｖ _maxを求める、ロボット制御装置。
前記手先の動作軌跡を直線補間により設定する動作軌跡設定部を備える、請求項１に記載のロボット制御装置。
前記第１算出部は、前記手先が前記動作終点に到達する毎に、次の動作開始に備えて、前記動作終点を次の動作始点とし、新たに設定した次の動作終点までの前記手先の動作軌跡上で前記第１関節軸の軸心に最も近い点を算出して、前記第２算出部に前記手先の動作可能な最高速度Ｖ _maxを算出させる、請求項１又は２に記載のロボット制御装置。
作業対象となるワークを撮像するカメラと、
前記カメラの撮像画像を処理して前記ワークを認識する画像処理を行う画像処理部とを備える、請求項１乃至３のいずれかに記載のロボット制御装置。
５軸垂直多関節ロボットの動作を制御するロボット制御方法において、
動作始点から動作終点までの手先の動作軌跡上で第１関節軸の軸心に最も近い点を算出する第１算出処理と、
前記第１算出処理で算出した前記第１関節軸の軸心に最も近い点で前記第１関節軸が許容最高回転速度で回転したときの前記手先の動作軌跡に沿ったＸＹＺ方向の動作可能な最高速度Ｖ _max （以下「前記手先の動作可能な最高速度Ｖ _max 」という）を算出する第２算出処理と、
前記第２算出処理で算出した前記手先の動作可能な最高速度Ｖ _maxを前記手先の目標動作速度として設定すると共に前記手先の動作軌跡に沿ったＸＹＺ方向の動作速度が前記手先の目標動作速度となるように各関節軸の目標回転速度を設定する目標動作速度設定処理と、
前記目標動作速度設定処理で設定した前記各関節軸の目標回転速度に基づいて前記動作始点から前記動作終点までの前記各関節軸の回転速度を制御する動作制御処理とを含み、前記第２算出処理では、前記第１関節軸の軸心をＺ軸とし、前記第１関節軸の軸心の所定の高さ位置に原点を設定した３次元の座標系において、前記第１関節軸の軸心に最も近い点で前記第１関節軸が許容最高回転速度で回転したときのＸＹ平面上に投影した前記手先の動作軌跡に沿ったＸＹ方向の動作可能な最高速度Ｖ _XYmax を算出すると共に、前記手先の動作軌跡に沿ったＸＹＺ方向とＸＹ平面とのなす角度θc を求め、前記ＸＹ方向の動作可能な最高速度Ｖ _XYmax と前記角度θc とに基づいて前記手先の動作可能な最高速度Ｖ _maxを求める、ロボット制御方法。