(57)【特許請求の範囲】
【請求項１】
ロボットの先端の位置情報を取得する手段と、ロボットの関節の定格最高速度情報を取得する手段と、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶する記憶手段と、上記ロボットの先端の位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索する手段と、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用する手段とを備えたことを特徴とするロボットの速度演算装置。
【請求項２】
ロボットの現在関節情報を取得する手段と、ロボットの関節の定格最高速度情報を取得する手段と、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶する記憶手段と、上記現在関節情報と上記関節定格最高速度とに基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索する手段と、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用する手段とを備えたことを特徴とするロボットの速度演算装置。
【請求項３】
ロボットはスカラロボットであり、ロボットの現在関節情報として上記スカラロボットの現在関節角度情報を取得し、ロボットの関節の定格最高速度情報として上記スカラロボットの関節の定格最高角速度情報を取得する、ことを特徴とする請求項２記載のロボットの速度演算装置。
【請求項４】
スカラロボットは水平面内２自由度スカラロボットであり、スカラロボットの現在関節角度情報として上記水平面内２自由度スカラロボットの現在関節角度情報を取得し、スカラロボットの関節の定格最高角速度情報として上記平面内２自由度スカラロボットの第一関節の定格最高角速度を取得する、ことを特徴とする請求項３記載のロボットの速度演算装置。
【請求項５】
水平面内２自由度スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報を入力して先端姿勢軸による速度寄与補正係数を演算する手段と、当該先端姿勢軸を有する３自由度スカラロボットの関節角度情報とその第一関節の定格最高角速度と、に基づく当該先端姿勢軸を有する３自由度スカラロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じる手段と、をさらに備えたことを特徴とする請求項４記載のロボットの速度演算装置。
【請求項６】
ロボットは水平面内２自由度円筒座標ロボットであり、ロボットの現在関節情報として上記水平面内２自由度円筒座標ロボットの現在関節情報を取得し、ロボットの定格最高速度情報として上記水平面内２自由度円筒座標ロボットの旋回軸の定格最高角速度を取得する、ことを特徴とする請求項２記載のロボットの速度演算装置。
【請求項７】
水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの関節情報と旋回軸の定格最高角速度と、に基づく上記先端姿勢軸を有する３自由度円筒座標ロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じる手段と、をさらに備えたことを特徴とする請求項６記載のロボットの速度演算装置。
【請求項８】
水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として上記垂直多関節ロボットの現在関節情報と旋回軸の定格最高角速度とを取得することを特徴とする請求項６もしくは請求項７記載のロボットの速度演算装置。
【請求項９】
前記座標上における近似許容限界速度として予め記憶する前記記憶手段は、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、正方形形状に近似されたものを記憶することを特徴とする請求項３または請求項４記載のロボットの速度演算装置。
【請求項１０】
前記座標上における近似許容限界速度として予め記憶する前記記憶手段は、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、長方形形状に近似されたものを記憶することを特徴とする請求項３または請求項４記載のロボットの速度演算装置。
【請求項１１】
スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報に基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じる手段と、をさらに備えたことを特徴とする請求項９または請求項１０記載のロボットの速度演算装置。
【請求項１２】
水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じる手段と、をさらに備えたことを特徴とする請求項６記載のロボットの速度演算装置。
【請求項１３】
水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボット先端の直線補間動作において取り得る前記近似許容限界速度は、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として検索されることを特徴とする請求項１２記載のロボットの速度演算装置。
【請求項１４】
ロボットの現在位置における進路方向を取得する手段と、上記ロボットの現在位置における進路方向に基づいてその進路方向と平行な方向へ上記ロボットの現在位置の座標を変換する手段と、をさらに備え、前記近似許容限界速度を検索する手段は、上記変換された上記ロボットの現在位置の座標に基づいて上記許容限界速度を検索する、ことを特徴とする請求項１〜請求項１３のいずれかに記載のロボットの速度演算装置。
【請求項１５】
ロボットの先端の位置情報を取得するステップと、ロボットの関節の定格最高速度情報を取得するステップと、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶するステップと、
上記ロボットの先端の位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索するステップと、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用するステップとを備えたことを特徴とするロボットの速度演算方法。
【請求項１６】
ロボットの現在関節情報を取得するステップと、ロボットの関節の定格最高速度情報を取得するステップと、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶するステップと、上記現在関節情報と上記関節定格最高速度とに基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索するステップと、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用するステップと
を備えたことを特徴とするロボットの速度演算方法。
【請求項１７】
ロボットはスカラロボットであり、ロボットの現在関節情報として上記スカラロボットの現在関節角度情報を取得し、ロボットの関節の定格最高速度情報として上記スカラロボットの関節の定格最高角速度情報を取得する、ことを特徴とする請求項１６記載のロボットの速度演算方法。
【請求項１８】
スカラロボットは水平面内２自由度スカラロボットであり、スカラロボットの現在関節角度情報として上記水平面内２自由度スカラロボットの現在関節角度情報を取得し、スカラロボットの関節の定格最高角速度情報として上記平面内２自由度スカラロボットの第一関節の定格最高角速度を取得する、ことを特徴とする請求項１７記載のロボットの速度演算方法。
【請求項１９】
水平面内２自由度スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報を入力して先端姿勢軸による速度寄与補正係数を演算するステップと、当該先端姿勢軸を有する３自由度スカラロボットの関節角度情報とその第一関節の定格最高角速度と、に基づく当該先端姿勢軸を有する３自由度スカラロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じるステップと、をさらに備えたことを特徴とする請求項１８記載のロボットの速度演算方法。
【請求項２０】
ロボットは水平面内２自由度円筒座標ロボットであり、ロボットの現在関節情報として上記水平面内２自由度円筒座標ロボットの現在関節情報を取得し、ロボットの定格最高速度情報として上記水平面内２自由度円筒座標ロボットの旋回軸の定格最高角速度を取得する、ことを特徴とする請求項１６記載のロボットの速度演算方法。
【請求項２１】
水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの関節情報と旋回軸の定格最高角速度と、に基づく上記先端姿勢軸を有する３自由度円筒座標ロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じるステップと、をさらに備えたことを特徴とする請求項２０記載のロボットの速度演算方法。
【請求項２２】
水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として上記垂直多関節ロボットの現在関節情報と旋回軸の定格最高角速度とを取得することを特徴とする請求項２０もしくは請求項２１記載のロボットの速度演算方法。
【請求項２３】
前記座標上における近似許容限界速度として予め記憶する前記ステップは、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、正方形形状に近似されたものを記憶することを特徴とする請求項１７または請求項１８記載のロボットの速度演算方法。
【請求項２４】
前記座標上における近似許容限界速度として予め記憶する前記ステップは、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、長方形形状に近似されたものを記憶することを特徴とする請求項１７または請求項１８記載のロボットの速度演算方法。
【請求項２５】
スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報に基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じるステップと、をさらに備えたことを特徴とする請求項２３または請求項２４記載のロボットの速度演算方法。
【請求項２６】
水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じるステップと、をさらに備えたことを特徴とする請求項２０記載のロボットの速度演算方法。
【請求項２７】
水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボット先端の直線補間動作において取り得る前記近似許容限界速度は、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として検索されることを特徴とする請求項２６記載のロボットの速度演算方法。
【請求項２８】
ロボットの現在位置における進路方向を取得するステップと、上記ロボットの現在位置における進路方向に基づいてその進路方向と平行な方向へ上記ロボットの現在位置の座標を変換するステップと、をさらに備え、前記近似許容限界速度を検索するステップは、上記変換された上記ロボットの現在位置の座標に基づいて上記許容限界速度を検索する、ことを特徴とする請求項１５〜請求項２７のいずれかに記載のロボットの速度演算方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボットの軌跡制御装置や軌跡シミュレーション装置等に採用されるロボットの速度演算装置ならびにロボットの速度演算方法に関し、特に、直交座標での直線補間を行なう際の速度の特異領域を速度過大を生ずることなく通過できるようにするロボットの速度演算装置ならびにロボットの速度演算方法に関するものである。
【０００２】
【従来の技術】
ロボット制御において直線補間による軌跡制御とは、ロボット先端の軌跡が直線になり、その速度が例えば台形速度パターン等になるように制御する方法である。
【０００３】
従来のロボットの直線補間による軌跡制御方法を図１９、図２０を参照しながら説明する。
図１９は従来の直線補間のアルゴリズムのフローチャート、図２０は従来の直線補間による軌跡制御によるロボット先端の速度状態図を示している。
【０００４】
図１９においては、ステップ１０１により直線補間が開始される。この直線補間の手続きは、上位の作業計画などの手続きから呼び出されることにより開始する。この手続きが呼び出されるときには、上位の手続きによって現在位置と最終目標位置が確定し、この手続きに与えられるものである。
【０００５】
直線補間が開始されると、まずステップ１０２によりロボットの開始位置と最終目標位置から移動距離が算出され、次のステップ１０３によりそのロボットの現在位置と最終目標位置からロボットの移動すべき残距離が算出される。
【０００６】
そして、次のステップ１０４によりプログラムの中の指定速度と、プログラムの中で指定された速度倍率と操作盤から指定された速度倍率を乗じて、指定速度ｖ１を算出し、次のステップ１０５により指定加速度α１にサンプリング時間Δｔを乗じた速度増分を現在速度ｖ０に加えて、加速パターン動作での速度ｖ２を下記数１式により算出する。
【数１】
0004060393.tif 000002
【０００７】
次に、ステップ１０６によりステップ１０３で算出した残距離を、プログラム中で指定された減速加速度により一定の割合で減速する減速パターンで移動する場合の速度を算出する。すなわち、指定された減速の加速度α２にて減速し続けて、残距離Ｌだけ移動したときに速度が０となる速度ｖ３を下記数２式に基づいて算出する。
【数２】
0004060393.tif 000003
【０００８】
次に、ステップ１３０において、ステップ１０４、１０５、１０６で算出した３つの速度の中から最少の値のものを指令速度として採用する。
例えば、ロボット先端の速度状態を示す図２０を参照して具体的に説明すると、時刻ｔ＝ｔ１の場合は、ステップ１０５で算出される加速パターン動作での速度ｖ２が、最少の速度であるのでこれを採用する。一方、時刻ｔ＝ｔ２の場合は、ステップ１０４で算出される一定速度である指定速度ｖ１が最少の速度であるのでこれを採用し、さらに時刻ｔ＝ｔ３の場合は、ステップ１０６で算出される減速パターン動作での速度ｖ３が最少の速度であるので、これを採用する。
【０００９】
次に、図１９に戻り、ステップ１０９により、現在位置からステップ１３０にて決定された指令速度で移動する補間位置を算出し、続いて次のステップ１１０により直交座標で表現されている補間位置を関節座標の表現に座標変換し、さらに次のステップ１１１により各々の関節を目標となる補間位置へ駆動して、次のステップ１１２により現在位置を補間位置に更新するようにする。
【００１０】
そして、次のステップ１１３により現在位置を最終目標位置と比較して、目標位置に到達したか否か判定し、未到着の場合には、ステップ１０３へ実行を移す一方、到達している場合には直線補間手続きを終了する。
【００１１】
【発明が解決しようとする課題】
しかしながら、従来のロボットの直線補間による軌跡制御では、上記のようにロボット先端の速度のみに着目して直線補間を行ようにしていたため、例えばスカラ型ロボット等においては、先端速度の早い遅いにかかわらず第１軸の速度が非常に大きな値になるので、原点近傍を直線補間動作で通過することが非常に困難である、という問題があった。
【００１２】
この問題点を、図２１に示す特異領域を通過する従来技術によるロボットの動作説明図を参照して詳細に説明する。
図２１では、Ｘ軸に沿って、（１）の位置から（４）の位置までを原点を通過して直線補間するスカラロボットが示されている。図中、このスカラロボットは、長さ５ｃｍの第１アームＡ１と、同じく長さ５ｃｍの第２アームＡ２を有しており、第２アームＡ２先端がＸ軸上を図上左方向へ単位時間あたり５ｃｍで（１）→（２）→（３）→（４）と移動する。
【００１３】
図２１の場合は、（１）→（２）では、第１アームＡ１は反時計回りに５８度、（２）→（３）の場合は反時計回りに１１７度、（３）→（４）の場合は、時計回りに１６度回転している。
【００１４】
ところで、直線補間動作では、ロボット先端、すなわち図２１の場合では第２アームＡ２先端の速度が一定であるので、原点近傍をすり抜ける（２）→（３）の動作の場合、第１アームＡ１は非常に大きな角度を移動しなくてはならない。図２１で示した例では、第１アームＡ１は、（２）→（３）の場合、それぞれ、（１）→（２）の約２倍、（３）→（４）の約７倍の角速度で動作しなければならない。
【００１５】
このため、条件によっては、第１軸の能力を超えた早い回転速度で第１軸が回転する必要が生じ、速度過大のエラーを生じて、ロボットが動作を中断してしまう場合もでてくるという不具合が生じる。
【００１６】
これを、縦軸をロボット先端の速度ならびに第１関節軸の回転角速度をとり、横軸に時刻をとった速度状態を示す図２２で説明する。図２２において、台形状の線図Ｂ１が図２０に示すようにして求めたロボット先端の速度を示しており、山形の曲線Ｂ２が第１関節の回転角速度を示している。ここで、ロボット先端の進行に伴い第１関節が動作するが、徐々に回転角速度が増加し、図２１の（２）→（３）の動作のように大きな角度を移動する場合に、第１関節の回転角速度が第１関節の定格最大角速度Ｊ１ｍａｘを越えてしまい（尚、図２２ではｔ＝ｔ０で越えている。）、ロボットに速度過大のエラーが生じて、以降の運転を続行することができなくなる不具合が生じる。
【００１７】
この不具合を数学的に証明する。
スカラ型ロボットにおいて、ロボット先端位置を（Ｘ，Ｙ）、関節角度（θ１、θ２）とした場合の先端位置の速度と関節角速度との関係は、ヤコビ行列Ｊによって、次の数３式により表現できる。
【数３】
0004060393.tif 000004
ここで、Ｊは、下記の数４式のように定義されるヤコビ行列である。
【数４】
0004060393.tif 000005
【００１８】
従って、先端位置速度から関節角速度は、逆ヤコビ行列Ｊｒにより、次の数５式の関係が成り立つ。
【数５】
0004060393.tif 000006
ここで、Ｊｒは、下記の数６式のように定義される逆ヤコビ行列である。
【数６】
0004060393.tif 000007
【００１９】
ここで上記数６式は、２元連立方程式である。この連立方程式の第１式は、第１軸の関節角速度と先端位置速度の関係を表わしている。図２１に示す２自由度のスカラロボットの場合は、Ｘ方向の速度成分のみをもちＹ方向成分の速度が０となるので、第１軸の関節角速度Ｊ１は、次の数７式のようになる。
【数７】
0004060393.tif 000008
【００２０】
ここで、上記数７式において、第２アームＡ２先端が原点に近づき、第１アームＡ１と第２アームＡ２が重なる姿勢すなわち、θ２＝πに近づくにつれて、Ｊ１の分母は０へ近づくために、ロボット先端の速度ｄＸ／ｄｔの値に関らず、第１軸の関節角速度は無限大に近づく。
【００２１】
従って、第１軸の能力を超えた早い回転速度で第１軸は回転する必要が生じて、速度過大のエラーを生じて、ロボットが動作を中断してしまう場合もでてくるという不具合が生じる。
【００２２】
同様にして、後述する図１０に示す円筒座標型ロボットや、図４に示すスカラ型ロボットにおいては、ロボット先端位置の速度と関節速度との関係は、ヤコビ行列Ｊによって、次の数８式のように表現できる。
【数８】
0004060393.tif 000009
ここで、Ｊは、下記の数９式のように定義されるヤコビ行列である。
【数９】
0004060393.tif 000010
【００２３】
従って、先端位置速度から関節角速度は、逆ヤコビ行列Ｊｒにより、次の数１０式の関係が成り立つ。
【数１０】
0004060393.tif 000011
ここで、Ｊｒは、下記の数１１式のように定義される逆ヤコビ行列である。
【数１１】
0004060393.tif 000012
【００２４】
ところで、図２１と同様にＸ軸に沿った動作を行った場合は、連立方程式である上記数６式の第２式より、ロボット先端の速度ｄｘ／ｄｔとｄθ／ｄｔの関係は、次の数１２式のようになる。
【数１２】
0004060393.tif 000013
よって、θ＝４５度近傍で、所定の先端速度ｄｘ／ｄｔにて、旋回軸が最高回転数に達している場合は、ロボットの先端速度が同一であるにもかかわらず、原点近傍を通過する時点、すなわちθ＝９０度では、旋回軸がsin(９０°)／sin(４５°)≒１．４１倍、すなわち４０％増しで旋回しなければならず、速度過大を生じてしまう。逆に、この姿勢で速度過大が発生しないように設定した場合は、残りの姿勢でロボットの能力に余裕が生じ、動作範囲全体では、サインカーブの実効値は２／π＝０．６３となり、およそ３７％の能力が生かされないという課題があった。
【００２５】
そこで、従来のロボットの軌跡制御方法による直線補間では、上記課題を解決するために、例えば特開昭５８−１１４８８８号公報等によりフィードバック技術を用いる方法が提案されている。
【００２６】
図２３は、特開昭５８−１１４８８８号公報に示された制御方法をフローチャートにしたものである。図２３を参照して、本提案の特異領域における速度過大エラーを回避する軌跡制御方法を説明する。
まず、ステップ１０１からステップ１１０までの処理は、上述の図１９に示す基本的な制御方法と同様である。
【００２７】
そして、次のステップ１３１においては、関節座標において速度過大の発生の可否を判定し、いずれの関節においても速度過大が発生していない場合には、図１９に示す基本的制御の場合と同様のステップ１１１以下の処理を行なう一方、いずれかの関節において速度過大が発生した場合には、ステップ１３２の処理へ移行して指令速度の調整を行なう。具体的には、ステップ１３１において速度過大になった軸の角速度の最大速度に対する超過割合を求め、次に超過割合の逆数を指令速度に乗ずる等して、速度過大が発生しないように指令速度を調節する。
【００２８】
例えば、最大角速度が３．１４ｒａｄ／ｓの第１関節に対して、座標変換の結果、指令速度が９０ｍｍ／ｓにより、４．７２ｒａｄ／ｓが指令として与えられた場合を考えると、第１関節の超過割合は、４．７２ｒａｄ／ｓ÷３．１４ｒａｄ／ｓ＝１．５倍であるので、その逆数０．６６７を指令速度に乗じて、６０ｍｍ／ｓと調整する。
なお、その後のステップ１１１からステップ１１４までの処理は、図１９に示す従来の基本的な制御方法と同様である。
【００２９】
しかしながら、上記特開昭５８−１１４８８８号公報によって提案された課題解決方式では、上記のようにフィードバック技術を用いているため、ステップ１０４からステップ１１０の一連の演算を行い、関節に定格の最大角速度を超える指令が生成されなければならない。また、それらの一連の演算の結果は一旦破棄されて、再度、ステップ１０９からステップ１１０の演算を少なくとも、もう一度計算しなければならない。ここで、ステップ１１０の座標変換演算は、２自由度ロボットにおいて逆正接関数を２回、平方根を１回、二乗演算を６回、４乗演算を２回、加減算を６回行うものである。一般に、ロボット速度演算装置では、軌跡精度などを高くするために、サンプリング間隔を極力短い時間に設定しているので、サンプリング時間に、これらの複雑な演算を２回以上繰り返す余裕が設定されていない。
【００３０】
このため、この方式では、サンプリング時間以内に演算が終了しない場合には、関節に指令が与えられず、ロボットの軌跡に乱れを生じるか、もしくは、サンプリング時間以内にステップ１１０の演算を２回以上行うことができる高度な能力を有するＣＰＵ１１を準備しなければならない、という新たな問題が生じる。
【００３１】
また、特開昭５９−１６３６０９号公報、特開昭６２−１８９５０４号公報、特開平２−４７７０２号公報、特開平３−６６５７６号公報、特開平６−３１４７３０号公報、特開平７−７２９１０号公報記載の発明においても、特開昭５８−１１４８８８号公報と同様のフィードバック技術を用いた方法が提案されているが、これらの方法でもフィードバック技術を用いているために、上記特開昭５８−１１４８８８号公報記載の発明と同様にロボットの軌跡に乱れを生じる等の新たな問題が生じる。
【００３２】
さらに、特開平２−３０８３１１号公報記載の発明においては、移動を開始する以前に、始動点から目標点までのすべての補間点を事前に演算して速度過大エラーによる運転中断を回避するロボットの制御方式が提案されているが、この方式では、事前にすべての補間点を計算する必要があり、動作開始前に演算のためにロボットが動作を停止するか、若しくはロボットの停止を回避するために高速な演算装置を必要とすると共に、さらに直線動作中に外部の操作盤などから動作速度の変更指令を反映することができない、という新たな問題が生じる。
【００３３】
さらに、特開昭６０−５７４０８号公報記載の発明では、特異領域を通過する場合に、通過の間だけ指令速度を無効とし、所定の微小速度にてロボットを運転させ特異領域を通過することにより速度過大エラーによる運転中断を回避するロボットの制御方式が提案されているが、この方式では、本来、ロボットがより早い速度で動きうるにもかかわらず特異領域内のすべての位置において速度過大を生じない微小速度で運転されるため、ロボットの速度が非常に遅くなってしまう、という問題がある。
【００３４】
またさらに、特開平３−６６５７６号公報や特開平６−１７０７６５号公報記載の発明では、特異領域に進入すると、指令速度に１より小さい所定の係数を適宜乗じることで、指令速度を引き下げることにより速度過大エラーによる運転中断を回避するロボットの制御方式が提案がなされているが、このロボットの制御方式では、速度過大を回避するために速度を減ずる係数の決定方法が開示されていないので、実施に当たっては操作者が、個々のロボットにおいて、個々の経路毎に、試行錯誤によってその係数を求めざるをえない、という問題がある。
【００３５】
そこで、本発明は、このような問題点を解決するためになされたもので、ロボットの軌跡に乱れを生じたり、また高度な能力を有するＣＰＵを準備等することなく、原点近傍を直線補間動作にて通過する場合に関節軸の定格最大角速度を超過しない範囲でロボット先端の最大速度を求めることのできるロボットの速度演算装置および軌跡制御方法を提供することを目的とする。
【００３６】
【課題を解決するための手段】
上記課題を解決するため、本発明では、
ロボットの現在位置情報を取得する手段と、ロボットの関節の定格最高速度情報を取得する手段と、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶する記憶手段と、上記ロボットの現在位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索する手段と、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用する手段とを備えたものである。
【００３７】
また、本発明では、ロボットの現在関節情報を取得する手段と、ロボットの関節の定格最高速度情報を取得する手段と、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶する記憶手段と、上記現在関節情報と上記関節定格最高速度とに基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索する手段と、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用する手段と
を備えたものである。
【００３８】
また、本発明では、ロボットはスカラロボットであり、ロボットの現在関節情報として上記スカラロボットの現在関節角度情報を取得し、ロボットの関節の定格最高速度情報として上記スカラロボットの関節の定格最高角速度情報を取得するものである。
【００３９】
また、本発明では、スカラロボットは水平面内２自由度スカラロボットであり、スカラロボットの現在関節角度情報として上記水平面内２自由度スカラロボットの現在関節角度情報を取得し、スカラロボットの関節の定格最高角速度情報として上記平面内２自由度スカラロボットの第一関節の定格最高角速度を取得するものである。
【００４０】
また、本発明では、水平面内２自由度スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報を入力して先端姿勢軸による速度寄与補正係数を演算する手段と、当該先端姿勢軸を有する３自由度スカラロボットの関節角度情報とその第一関節の定格最高角速度と、に基づく当該先端姿勢軸を有する３自由度スカラロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じる手段と、をさらに備えたものである。
【００４１】
また、本発明では、ロボットは水平面内２自由度円筒座標ロボットであり、ロボットの現在関節情報として上記水平面内２自由度円筒座標ロボットの現在関節情報を取得し、ロボットの定格最高速度情報として上記水平面内２自由度円筒座標ロボットの旋回軸の定格最高角速度を取得するものである。
【００４２】
また、本発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの関節情報と旋回軸の定格最高角速度と、に基づく上記先端姿勢軸を有する３自由度円筒座標ロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じる手段と、をさらに備えたものである。
【００４３】
また、本発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として上記垂直多関節ロボットの現在関節情報と旋回軸の定格最高角速度とを取得するものである。
【００４４】
【００４５】
【００４６】
また、本発明では、前記座標上における近似許容限界速度として予め記憶する前記記憶手段は、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、正方形形状に近似されたものを記憶するものである。
【００４７】
また、本発明では、前記座標上における近似許容限界速度として予め記憶する前記記憶手段は、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、長方形形状に近似されたものを記憶するものである。
【００４８】
また、本発明では、スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報に基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じる手段と、をさらに備えたものである。
【００４９】
【００５０】
また、本発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じる手段と、をさらに備えたものである。
【００５１】
また、本発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボット先端の直線補間動作において取り得る前記近似許容限界速度は、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として検索されるものである。
【００５２】
また、本発明では、ロボットの現在位置における進路方向を取得する手段と、上記ロボットの現在位置における進路方向に基づいてその進路方向と平行な方向へ上記ロボットの現在位置の座標を変換する手段と、をさらに備え、前記近似許容限界速度を検索する手段は、上記変換された上記ロボットの現在位置の座標に基づいて上記許容限界速度を検索するものである。
【００５３】
【００５４】
【００５５】
また、本発明では、ロボットの現在位置情報を取得するステップと、ロボットの関節の定格最高速度情報を取得するステップと、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶するステップと、上記ロボットの現在位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索するステップと、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用するステップとを備えたものである。
【００５６】
また、本発明では、ロボットの現在関節情報を取得するステップと、ロボットの関節の定格最高速度情報を取得するステップと、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶するステップと、上記ロボットの現在位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索するステップと、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用するステップとをさらに備えたものである。
【００５７】
また、本発明では、ロボットはスカラロボットであり、ロボットの現在関節情報として上記スカラロボットの現在関節角度情報を取得し、ロボットの関節の定格最高速度情報として上記スカラロボットの関節の定格最高角速度情報を取得するものである。
【００５８】
また、本発明では、スカラロボットは水平面内２自由度スカラロボットであり、スカラロボットの現在関節角度情報として上記水平面内２自由度スカラロボットの現在関節角度情報を取得し、スカラロボットの関節の定格最高角速度情報として上記平面内２自由度スカラロボットの第一関節の定格最高角速度を取得するものである。
【００５９】
また、本発明では、水平面内２自由度スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報を入力して先端姿勢軸による速度寄与補正係数を演算するステップと、当該先端姿勢軸を有する３自由度スカラロボットの関節角度情報とその第一関節の定格最高角速度と、に基づく当該先端姿勢軸を有する３自由度スカラロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じるステップと、をさらに備えたものである。
【００６０】
また、本発明では、ロボットは水平面内２自由度円筒座標ロボットであり、ロボットの現在関節情報として上記水平面内２自由度円筒座標ロボットの現在関節情報を取得し、ロボットの定格最高速度情報として上記水平面内２自由度円筒座標ロボットの旋回軸の定格最高角速度を取得するものである。
【００６１】
また、本発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの関節情報と旋回軸の定格最高角速度と、に基づく上記先端姿勢軸を有する３自由度円筒座標ロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じるステップと、をさらに備えたものである。
【００６２】
また、本発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として上記垂直多関節ロボットの現在関節情報と旋回軸の定格最高角速度とを取得するものである。
【００６３】
【００６４】
【００６５】
また、本発明では、前記座標上における近似許容限界速度として予め記憶する前記ステップは、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、正方形形状に近似されたものを記憶するものである。
【００６６】
また、本発明では、前記座標上における近似許容限界速度として予め記憶する前記ステップは、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、長方形形状に近似されたものを記憶するものである。
【００６７】
また、本発明では、スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報に基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じるステップと、をさらに備えたものである。
【００６８】
【００６９】
また、本発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じるステップと、をさらに備えたものである。
【００７０】
また、本発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボット先端の直線補間動作において取り得る前記近似許容限界速度は、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として検索されるものである。
【００７１】
また、本発明では、ロボットの現在位置における進路方向を取得するステップと、上記ロボットの現在位置における進路方向に基づいてその進路方向と平行な方向へ上記ロボットの現在位置の座標を変換するステップと、をさらに備え、前記近似許容限界速度を検索するステップは、上記変換された上記ロボットの現在位置の座標に基づいて上記許容限界速度を検索するものである。
【００７２】
【００７３】
【００７４】
【発明の実施の形態】
実施の形態１．
図１に、この発明に係るロボットの速度演算装置およびロボットの速度演算方法を採用したロボットの軌跡制御装置と、ロボット本体との電気的構成を示す。  図１において、１はロボットの速度演算装置、２はロボット本体、３はティーチングボックス、４は操作盤、１１はロボット速度演算装置１に内蔵されるＣＰＵ、１２はＣＰＵ１１に接続され後述する図２に示すように動作するための制御プログラムが格納されたメモリ、１３はティーチングボックス３および操作盤４とＣＰＵ１１との間の各種信号の仲介を行なうインターフェイス、１４ａ〜１４ｄはＣＰＵ１１からの指令に基づいてロボット本体２に組み込まれた後述するモータ１５ａ〜１５ｄを駆動するサーボドライバ、１５ａ〜１５ｄはロボット本体２に組み込まれてロボットアームを駆動するモータ、１６ａ〜１６ｄはモータ１５ａ〜１５ｄとともにロボット本体２の関節に組み込まれてモータ１５ａ〜１５ｄによって駆動されるロボットアームの位置や角度を検出するエンコーダである。
【００７５】
次に上記実施の形態１の動作を図２のフローチャートを参照しながら説明する。
図２は、メモリ１２に記憶された制御プログラムによって実行されるロボットの速度演算方法を示すフローチャートである。なお、図１９に示す従来の基本的な制御方法と同様なステップには同一ステップ番号を付してその説明は省略するものとする。
【００７６】
図２において、まず、ステップ１０１およびステップ１０２の処理は、図１９に示す従来の基本的な制御方法と同様である。
【００７７】
そして、次のステップ１１６により、この実施の形態１では、直線補間の開始位置と終了位置を結ぶ進路に沿った単位ベクトルを算出する。
【００７８】
続いて、次のステップ１１７においては、図５に示すように、進路とＸ軸の成す角度φ（以下、進路傾斜という。）を算出する。
【００７９】
その後、次のステップ１０３からステップ１０６までの処理は、図１９の従来の基本的な制御方法と同様である。
【００８０】
次のステップ１１８においては、図５に示すように、ステップ１１７で求めた進路傾斜に基づいて、座標を進路に平行な方向に回転変換する。すなわち、次の数１３式により第１軸の関節角度から進路傾斜を減じる。
【数１３】
0004060393.tif 000014
【００８１】
尚、数１３おける左辺側のθ１は図５におけるθ１を指し、右辺側のθ１は図４におけるθ１を指す。
次のステップ１０７においては、現在の位置における関節の定格最大角速度にて動作しうるロボット先端での直線補間動作速度の中で最大のもの（以下、許容限界速度という。）を下記の数１４式にて算出する（尚、本式が許容限界速度であることの詳細説明は後述する。）。
ただし、ここで、第１関節の関節角度θ１は、ステップ１１８によって、進路傾斜を減じることによる進路に平行な座標に基づくものである。
【数１４】
0004060393.tif 000015
【００８２】
そして、次のステップ１０８においては、ステップ１０４、１０５、１０６、１０７の各ステップで算出した４つの速度の中から最少のものを指令速度として採用する。
【００８３】
例えば、縦軸をロボット先端速度と横軸を時間とする速度状態を示す図３において、時刻ｔ＝ｔ１の場合は、ステップ１０５で算出される加速パターン動作での速度ｖ２が、最少の速度であるので、これを採用する。
同様に、時刻ｔ＝ｔ２の場合は、ステップ１０４で算出される一定速度動作での速度ｖ１が、最少の速度であるので、これを採用する。
同様に、時刻ｔ＝ｔ３の場合は、ステップ１０６で算出される減速パターン動作での速度ｖ３が、最少の速度であるので、これを採用する。
同様に、時刻ｔ＝ｔ４の場合は、ロボット先端が特異領域にあるために、ステップ１０７で算出される許容限界速度Ｖｌｉｍｉｔが最少の速度であるので、これを採用する。
【００８４】
次のステップ１０９からステップ１１４は、図１９に示す従来の基本的な制御方法と同様である。つまり、ステップ１０９により現在位置からステップ１０８にて採用された指令速度で移動する補間位置を算出し、次のステップ１１０により座標変換し、次のステップ１１１により各軸を駆動して、ステップ１１２により現在位置を更新して、次のステップ１１３により到着判定か否かを判定し、到着していればステップ１１４により終了する一方、未到着であれば図１９に示す場合と同様に残距離更新処理を行なうようにする。
【００８５】
次に、ステップ１０７で用いた許容限界速度を求めるための数１４式の詳細を、図面を参照して説明する。
図４は、この実施の形態１が制御対象とする平面内２自由度のスカラロボットの機構図である。
図４のスカラ型ロボットにおいて、ロボット先端位置を（Ｘ，Ｙ）、関節角度（θ１、θ２）とした場合の先端位置の速度と関節角速度との関係は、ヤコビ行列Ｊによって、次の数１５式のように表現できる。
【数１５】
0004060393.tif 000016
【００８６】
ここで、Ｊは、下記の数１６式のように定義されるヤコビ行列である。
【数１６】
0004060393.tif 000017
【００８７】
従って、先端位置速度から関節角速度は、逆ヤコビ行列Ｊｒにより、次の数１７式関係が成り立つ。
【数１７】
0004060393.tif 000018
【００８８】
ここで、Ｊｒは、下記の数１８式のように定義される逆ヤコビ行列である。
【数１８】
0004060393.tif 000019
【００８９】
上記数１８式は、２元連立方程式である。この連立方程式の第１式は、第１軸の関節角速度と先端位置速度の関係を表わしている。ここで、簡単化のために座標を図５に示すように、経路の方向に沿うように回転変換しておくと、Ｘ方向の速度成分のみをもちＹ方向成分の速度が０となるので、第１軸の関節角速度Ｊ１は、次の数１９式のようになる。
【数１９】
0004060393.tif 000020
【００９０】
ここで、第１軸の速度過大のエラーを生じない定格としての最高回転角速度をＪ１ｍａｘとし、この時のロボット先端の速度すなわち許容限界速度をＶｌｉｍｉｔとする。
【００９１】
両者の関係は、上式から次の数２０式のように表わせる。
【数２０】
0004060393.tif 000021
【００９２】
これをＶｌｉｍｉｔについて整理すると、次の数２１式のようになる。
【数２１】
0004060393.tif 000022
【００９３】
従って、この実施の形態１によれば、ステップ１０７にて許容限界速度を求め、ステップ１０８にて従来の方法にて決定される速度と比較し、従来方法では関節の定格最大角速度を越えてしまうロボット先端での直線補間動作の速度指令が生成される場合に、関節の定格最大角速度にて動作しうるロボット先端での直線補間動作速度すなわち許容限界速度を、従来手法にて決定される速度指令に代えるので、ロボット先端が原点近傍を直線補間動作にて通過する場合において、関節に定格最大角速度を越えた指令が生成されることを回避し、なおかつ、関節速度過大が発生しない最大の速度で移動することができる。
【００９４】
ここで、図６に示す縦軸をロボット先端の速度ならびに第１関節軸の回転角速度をとり、横軸に時刻をとった速度状態図で説明する。図において、台形状で高原部分に凹部を有する線図Ｂ３が、ロボット先端の速度を表わし、凸型の線図Ｂ４が第１関節の回転角速度を表わし、線図Ｂ５が図２のステップ１０７で求めた許容限界速度を示している。なお、線図Ｂ２は従来技術による第１軸の角速度指令を示している。
【００９５】
具体的に説明すると、ロボット先端の進行に伴い第１関節が動作し、徐々に回転角速度が増加すると、図６においてｔ＝ｔ０１の時刻に第１軸が定格最大角速度に達する。この時、ステップ１０７で求められた許容限界速度が、指定速度Ｖ１より小さくなり、許容限界速度がステップ１０８にて採用されるので、第１軸の角速度の線図Ｂ４は、Ｊ１ｍａｘで高原状となる山形の線図になる。ロボット先端の速度は、原点に近づくにつれて小さくなり、原点にもっとも接近したときに、ロボットの先端の速度は最少となる。そして、原点から離れるにつれて大きくなり、線図Ｂ５に示す許容限界速度と指定速度Ｖ１とが等しくなるｔ＝ｔ０２で、再び従来の技術による軌跡制御方法に移り変わる。このため、ロボット先端の速度線図Ｂ３は、高原部分に凹部を有するようになる。このようにして、第１関節への指令角速度が、定格最大角速度Ｊ１ｍａｘを超過することが回避されるので、速度過大エラーの発生を回避し運転を続行することができる。
【００９６】
また、この実施の形態１においては、ステップ１０７にて許容限界速度を求め、ステップ１０８にて従来の方法にて決定される速度と比較し、従来方法では関節の定格最大角速度を越えてしまうロボット先端での直線補間動作の速度指令が生成される場合に、関節の定格最大角速度にて動作しうるロボット先端での直線補間動作速度すなわち許容限界速度を、従来手法にて決定される速度指令に代えるので、計算負荷の大きいステップ１１０での座標変換の計算結果を破棄しなければならないフィードバック技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【００９７】
また、この実施の形態１においては、ステップ１０７にて許容限界速度を求め、ステップ１０８にて従来の方法にて決定される速度と比較し、従来方法では関節の定格最大角速度を越えてしまうロボット先端での直線補間動作の速度指令が生成される場合に、関節の定格最大角速度にて動作しうるロボット先端での直線補間動作速度すなわち許容限界速度を、従来手法にて決定される速度指令に代えるので、制御的に不安定となる場合が生じるフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【００９８】
実施の形態２．
なお、上記実施の形態１では、平面内２自由度のスカラロボットを制御の対象としたが、この実施の形態２では、平面内２自由度のスカラロボットのうち、先端姿勢軸を有する３自由度スカラロボットを制御対象として説明する。
図７に、この実施の形態２が制御対象とする先端姿勢軸を有する３自由度スカラロボットの機構を示す。図において、Ａ１は第１軸、Ａ２は第２軸、Ａ３は先端姿勢軸である第３軸、Ｋ１は第１関節、Ｋ２は第２関節、Ｋ３は第３関節である。
【００９９】
次に、この実施の形態２の動作を図８のフローチャートを参照しながら説明する。
図８に、この実施の形態２の制御プログラムによって実行されるロボットの速度演算方法を示すフローチャートである。なお、図１９に示す従来の基本的な制御方法と同様なステップには同一ステップ番号を付してその説明は省略するものとする。
まず、ステップ１０１からステップ１０７までの処理は、図２に示す実施の形態１の処理と同様である。
【０１００】
そして、次のステップ１１９により、この実施の形態２では、先端姿勢軸の寄与を次の数２２式によって算出する。なお、先端姿勢軸の寄与を算出するこの数２２式の詳細は後述することにする。
【数２２】
0004060393.tif 000023
ここで、上記数２２式中、各記号の内容は下記の通りである。
Ｄ：現在位置から目的位置までの移動残距離
Ｐrca：現在の姿勢角度から目的位置の姿勢角度までの移動残角度
【０１０１】
次いで、次のステップ１２０により、下記の数２３式により、ステップ１０７で求めた許容限界速度にステップ１１９で求めた先端姿勢軸の寄与を乗じて、許容限界速度を修正する。
【数２３】
0004060393.tif 000024
【０１０２】
その後、次のステップ１０８からステップ１１４までの処理は、図２に示す実施の形態１の場合と同様である。
【０１０３】
次に、ステップ１１９で用いた先端姿勢軸の寄与を算出する数２２式の詳細を説明する。
まず、図７に示すスカラ型ロボットにおいては、ロボット先端位置を（Ｘ，Ｙ，Ａ）（以下、Ａは先端姿勢軸の姿勢角度とする。）、関節角度（θ１，θ２，θ３）とした場合の先端位置の速度と関節角速度との関係は、ヤコビ行列Ｊによって、次の数２４式のように表現できる。
【数２４】
0004060393.tif 000025
ここで、Ｊは、下記の数２５式のように定義されるヤコビ行列である。
【数２５】
0004060393.tif 000026
【０１０４】
従って、先端位置速度から関節角速度は、逆ヤコビ行列Ｊｒにより、次の数２６式の関係が成り立つ。
【数２６】
0004060393.tif 000027
【０１０５】
ここで、Ｊｒは、下記の数２７式のように定義される逆ヤコビ行列である。
【数２７】
0004060393.tif 000028
【０１０６】
上記数２７式は、３元連立方程式である。この連立方程式の第１式は、第１軸の関節角速度と先端位置速度の関係を表わしている。ここで、簡単化のために座標を図５に示すように、経路の方向に沿うように回転変換してあるので、Ｘ方向の速度成分のみをもちＹ方向成分の速度が０となり、第１軸の関節角速度Ｊ１は、次の数２８式となる。
【数２８】
0004060393.tif 000029
【０１０７】
ここで、第１軸の速度過大のエラーを生じない定格としての最高回転角速度をＪ１ｍａｘとし、この時のロボット先端の速度すなわち許容限界速度をＶｌｉｍｉｔとする。
【０１０８】
Ｊ１ｍａｘと許容限界速度の関係は、上記数２８式から次の数２９式のように表わせる。
【数２９】
0004060393.tif 000030
ここで、上記数２９式中の各記号は、下記の通りである
Vｌｉｍｉｔ：許容限界速度
Ｊ１ｍａｘ：第１軸最大角速度
ｄＡ／ｄｔ：先端姿勢軸の角速度
Ｌ１：第１軸Ａ１の長さ
Ｌ３：ツール長さ（第３軸Ａ３の長さ）
【０１０９】
ここで、上記数２９式の第１項は、実施の形態１で求めた第２軸Ａ２先端での許容限界速度を表わしている。また、その第２項は、第３軸Ａ３である先端姿勢軸、すなわちツール軸の動作による補正項である。
【０１１０】
また、先端姿勢軸の角速度ｄＡ／ｄｔは、ロボット先端位置の並進速度と独立に定まるものではなく、並進移動の終了と姿勢軸の動作の終了とが一致しなければならない。すなわち、並進速度と姿勢角速度の比率は、並進移動残距離と姿勢軸移動残角度の比率に等しくなる。
そこで、数３０式に示すように、並進移動残距離と姿勢軸移動残角度の比率を並進移動速度Ｖｌｉｍｉｔに乗ずることにより、先端姿勢軸の角速度ｄＡ／ｄｔが求められる。
【数３０】
0004060393.tif 000031
ここで、Ｚは、下記の数３１式の通りであり、並進移動の残距離Ｄに対する姿勢軸の残移動角度Ｐｒｃａの比率を表わしている。この値が１を超えた場合は、並進移動が移動全体を支配し、逆に１未満の場合は、ツール姿勢移動が全体の移動を支配する。
【数３１】
0004060393.tif 000032
【０１１１】
先端姿勢軸の角速度ｄＡ／ｄｔの決定式をＶｌｉｍｉｔ式に代入整理して、先端姿勢軸の角速度ｄＡ／ｄｔを消去すると、次の数３２式となる。
【数３２】
0004060393.tif 000033
【０１１２】
上記数３２式の分子は、実施の形態１で求めた姿勢軸を有していないロボットにおける許容限界速度であり、分母は姿勢軸による寄与の逆数を示している。この姿勢軸による寄与をＭとすると、姿勢軸による寄与Ｍは次の数３３式のように表すことができる。
【数３３】
0004060393.tif 000034
このため、この姿勢軸の寄与の逆数Ｍを、実施の形態１で求めた姿勢軸を有していないロボットにおける許容限界速度に乗じることで、姿勢軸の影響を考慮した許容限界速度をもとめることができる。
【０１１３】
従って、この実施の形態２によれば、ステップ１１９により姿勢軸の寄与Ｍを求め、ステップ１２０により先端軸を有していないロボットにおける許容限界速度に乗じることにより、姿勢軸の寄与を考慮した許容限界速度を求めることができる。
【０１１４】
実施の形態３．
この実施の形態３は、先端姿勢軸を有する水平面内２自由度円筒座標ロボットを制御対象としたものである。
図９に、この実施の形態３の制御対象となる先端姿勢軸を有する水平面内２自由度円筒座標ロボットを示す。図において、Ａ４は円筒軸、Ａ５は円筒軸Ａ４から伸長自在な先端姿勢軸、Ｋ１は第１関節である。
【０１１５】
次に、先端姿勢軸を有する水平面内２自由度円筒座標ロボットを制御対象としたこの実施の形態３の動作を説明するが、この実施の形態３の動作は図２に示す上記実施の形態１の動作と同じで、許容限界速度を求める式が異なるのだけであるので、上記実施の形態１の動作を示す図２のフローチャートを参照しながら説明するものとする。
まず、この実施の形態３でも、ステップ１０１からステップ１１８までの処理は、上記実施の形態１の場合と同様である。
【０１１６】
そして、次のステップ１０７により、この先端姿勢軸を有する水平面内２自由度円筒座標ロボットの許容限界速度を下記の数３４式により算出する。
【数３４】
0004060393.tif 000035
【０１１７】
次に、許容限界速度を算出するこの数３４式の詳細を説明する。図９に示す先端姿勢軸を有する水平面内２自由度円筒座標ロボットにおいて、ロボット先端位置を（Ｘ，Ｙ）、関節軸座標（θ２）とした場合の先端位置の速度と関節角速度との関係は、ヤコビ行列Ｊによって、次の数３５式のように表現できる。
【数３５】
0004060393.tif 000036
【０１１８】
ここで、Ｊは、下記の数３６式のように定義されるヤコビ行列である。
【数３６】
0004060393.tif 000037
【０１１９】
従って、先端位置速度から関節角速度は、逆ヤコビ行列Ｊｒにより、次の数３７式の関係が成り立つ。
【数３７】
0004060393.tif 000038
【０１２０】
この数３７式は、２元連立方程式である。この２元連立方程式の第１式は、第１軸の関節角速度と先端位置速度の関係を表わしている。ここで、簡単化のために座標を図５に示すように、経路の方向に沿うように回転変換しておくと、Ｘ方向の速度成分のみをもちＹ方向成分の速度が０となるので、第１軸の関節角速度Ｊ１は、次の数３８式となる。
【数３８】
0004060393.tif 000039
【０１２１】
ここで、第１軸の速度過大のエラーを生じない定格としての最高回転角速度をＪ１ｍａｘとし、この時のロボット先端の速度すなわち許容限界速度をＶｌｉｍｉｔとする。
【０１２２】
両者の関係は、上記数３８式から次の数３９式のように表わすことができる。
【数３９】
0004060393.tif 000040
【０１２３】
これをＶｌｉｍｉｔについて整理すると、次の数４０式のようになる。
【数４０】
0004060393.tif 000041
【０１２４】
従って、この実施の形態３では、ステップ１０７にて許容限界速度を求め、ステップ１０８にて従来の方法にて決定される速度と比較し、従来方法では関節の定格最大角速度を越えてしまうロボット先端での直線補間動作の速度指令が生成される場合に、関節の定格最大角速度にて動作しうるロボット先端での直線補間動作速度すなわち許容限界速度を、従来手法にて決定される速度指令に代えるので、ロボット先端が原点近傍を直線補間動作にて通過する場合において、関節に定格最大角速度を越えた指令が生成されることを回避し、なおかつ、関節速度過大が発生しない最大の速度で移動することができる。
【０１２５】
また、この実施の形態３においては、ステップ１０７にて許容限界速度を求め、ステップ１０８にて従来の方法にて決定される速度と比較し、従来方法では関節の定格最大角速度を越えてしまうロボット先端での直線補間動作の速度指令が生成される場合に、関節の定格最大角速度にて動作しうるロボット先端での直線補間動作速度すなわち許容限界速度を、従来手法にて決定される速度指令に代えるので、計算負荷の大きいステップ１１０での座標変換の計算結果を破棄しなければならないフィードバック技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１２６】
また、この実施の形態３においては、ステップ１０７にて許容限界速度を求め、ステップ１０８にて従来の方法にて決定される速度と比較し、従来方法では関節の定格最大角速度を越えてしまうロボット先端での直線補間動作の速度指令が生成される場合に、関節の定格最大角速度にて動作しうるロボット先端での直線補間動作速度すなわち許容限界速度を、従来手法にて決定される速度指令に代えるので、制御的に不安定となる場合が生じるフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１２７】
実施の形態４．
次に、この発明の実施の形態４を説明する。
この実施の形態４は、垂直多関節ロボットを制御対象としたものである。
図１０（ａ）に、この実施の形態４が対象とする垂直多関節ロボットの構成を示す。図において、Ａ１は第１アーム、Ａ２は第２アーム、Ｋ４は第１関節（旋回軸）、Ｋ５は第２関節（ショルダー軸）、Ｋ６は第３関節（エルボー軸）である。
【０１２８】
次に、このように構成された実施の形態４の動作を説明する。
図１１に、図１０に示す垂直多関節ロボットを制御対象としたこの実施の形態４の動作を示す。
まず、この実施の形態４のステップ１０１からステップ１０６までの処理は、図８に示す実施の形態３の場合と同様である。
【０１２９】
そして、この実施の形態４では、次のステップ１２１において、下記式により第１アームＡ１および第２アームＡ２の水平面内への投影長Ｌを次の数４１式により算出する。
【数４１】
0004060393.tif 000042
ここで、上記４１式中、各記号は下記の通りである。
Ｌ１：第１アームＡ１の長さ
Ｌ２：第２アームＡ２の長さ
θ２：第２関節（ショルダー軸）Ｋ５の関節角度
θ３：第３関節(エルボー軸)Ｋ６の関節角度
このため、図１０（ａ）に示す垂直多関節ロボットの水平面への投影は、図１０（ｂ）に示す投影アーム長さＬと第１関節（旋回軸）Ｋ４の角度θ１で定義される実施の形態３の円筒座標ロボットと同等になる。
【０１３０】
尚、その後のステップ１１８からステップ１１４までの処理は、図８に示す実施の形態３と同様である。
【０１３１】
従って、この実施の形態４によれば、図１０（ａ）に示す垂直多関節ロボットを水平面へ投影して、図１０（ｂ）に示すように図９に示す実施の形態３の円筒座標ロボットと同等として考えることにより、自由度を縮退させることにより演算の取り扱いを簡便にすることができる。
【０１３２】
実施の形態５．
上記実施の形態１〜４においては、現在位置について、ロボット先端を全体座標で管理するに加えて、関節角度も管理されている速度演算装置もしくは速度演算方法を対象として、原点近傍を直線補間動作にて通過することを可能とする軌跡制御方法を実施したものであった。
しかし、機能の限定された廉価なロボットにおいては、コストダウン等を目的として、記憶容量の節約や計算能力の小さなＣＰＵを用いる。この場合には、現在位置における関節情報を保持しえない速度演算装置もしくは速度演算方法が採用される。そこで、現在位置における関節情報を用いずに、原点近傍を直線補間動作にて通過することを可能とする水平面内２自由度スカラロボットの軌跡制御方法の実施の形態を説明する。
【０１３３】
図４に示す水平面内２自由度スカラロボットにおいて、許容限界速度の等高線は図１２に示すように歪んだ双子楕円である。また図１３に示すようにこの双子歪み楕円に外接する双子正方形すなわち辺長比が１：２の長方形にて等高線を近似する。この場合、二つの正方形は、経路の方向と平行となっている座標軸を対象軸とする。これによって、ロボット先端の許容限界速度は、経路の方向と座標軸が平行となるように回転変換を施した座標値によって、近似等高線を検索し、簡便に許容限界速度を決定することができる。
【０１３４】
歪んだ楕円となる許容限界速度の等高線をそれに外接する正方形に近似しているので、ロボットの先端位置を回転した座標（Ｘ，Ｙ）のＸ座標とＹ座標の絶対値の何れか小さい値を辺長とする正方形が近似された許容限界速度の等高線となる。
【０１３５】
このため、予めこの正方形の辺長と許容限界速度の一次元対応表を作り、ロボットの先端位置の回転された座標（Ｘ，Ｙ）のＸ座標とＹ座標の絶対値の何れか小さい値をインデックスとしてこの表を検索することにより、許容限界速度を求めることが可能となる。この検索演算は、sin演算、cos演算などの三角関数演算と比較して大小比較演算のみで可能なので、計算の負荷が小さくなり、簡便化と高速化を図ることができる。
【０１３６】
具体的に、図１４のフローチャートを参照して説明する
【０１３７】
まず、ステップ１０１からステップ１０６までの処理は、図２に示す実施の形態１の場合と同様である。
【０１３８】
そして、この実施の形態５では、次のステップ１２２により、図１５に示すように座標を角度Ψだけ回転させて、Ｘ軸が経路方向と平行になるようにする。
【０１３９】
そして、次のステップ１２３により、回転した座標におけるＸ座標値の絶対値とＹ座標値の絶対値の何れか小さい値を選択する。
【０１４０】
さらに、次のステップ１２４により、予め用意した近似許容限界速度の等高線の正方形辺長＝許容限界速度の表１（表１の詳細については後述する。）に対して、ステップ１２３で選択した値を指標として許容限界速度を検索して算出する。
【０１４１】
ステップ１０８からステップ１１４は、上記実施の形態１と同様である。
【０１４２】
次に、ステップ１２４で用いた近似許容限界速度の等高線の正方形辺長＝許容限界速度の表１の詳細を説明する。
この表１は、ロボットアームの長さと関節軸の定格最大角速度が決まると一意に定まるため、軌跡生成の度に生成する必要はないので、ロボットの速度演算装置のＣＰＵで計算する必要はなく、別個の計算機で計算し、工場出荷段階で不揮発性メモリなどに書き込んでおくものである。
【０１４３】
図１３に示す歪んだ楕円となる許容限界速度の等高線は、関節角度θ１、θ２を助変数として、次の数４２〜４４式で表現される。
【数４２】
0004060393.tif 000043
【数４３】
0004060393.tif 000044
【数４４】
0004060393.tif 000045
【０１４４】
注目する許容限界速度Ｖｌｉｍｉｔに関して、まず、ｘの絶対値とｙの絶対値の最大値を求める。実際には上式をｘ，ｙに関して陽の形式に解くことは困難なので、最初にｘの値を逐次変化させながらｙ方向に探索を行ない、次にｙの値を逐次変化させながらｘ方向の探索を行なう。そして、この両者から最大のものを、該当する許容限界速度に対応する近似許容限界速度の等高線の正方形辺長として採用する。この操作を許容限界速度Ｖｌｉｍｉｔを逐次変更して行なう。
【０１４５】
例えば、近似許容限界速度の等高線の正方形辺長＝許容限界速度の表は、アーム長さＬ１＝Ｌ２＝３２０ｍｍで、各々の関節軸の定格最大角速度が４．６ｒａｄ／ｓの場合は、下記の表１のようになる。
【表１】
0004060393.tif 000046
【０１４６】
なお、上記実施の形態５では、水平面内２自由度スカラロボットに利用した場合について述べたが、ステップ１２４で用いる許容限界速度表を求める式において水平面内２自由度円筒座標ロボットに対しては、下記の数４５〜４７式を用いることにより同様にして、ロボット先端位置から許容限界速度を求める表を作成することが可能である。この場合のロボット先端における許容限界速度の等高線は、図１６に示すようにＹ軸上に中心を持ち、原点にて接しているＸ軸を対称軸とした双子円となっている。この等高線に外接する辺比１：２の長方形、即ち各々の円に外接する正方形を近似された許容限界速度の等高線として求めるものである。
【数４５】
0004060393.tif 000047
【数４６】
0004060393.tif 000048
【数４７】
0004060393.tif 000049
これを、まとめると下記の数４８式のようになる
【数４８】
0004060393.tif 000050
ただし、上記数４８式中、各記号は下記の通りである。
Ｖｌｉｍｉｔ：許容限界速度
Ｊｍａｘ  ：旋回軸定格最大角速度
Ｌ：伸縮軸長
θ：旋回軸角度
【０１４７】
さらに、垂直多関節ロボットに対しては、水平面内にアームを投影した長さを水平面内２自由度円筒座標ロボットの伸縮軸長さとして扱うことで同様にステップ１２４で用いる許容限界速度表を作ることが可能になる。
【０１４８】
実施の形態６．
上記実施の形態１〜５においては、本発明に係るロボットの速度演算装置をロボット本体を実際に制御するロボットの軌跡制御装置に採用して説明した。
しかし、本発明に係るロボットの速度演算装置は、ロボット本体を実際に制御するロボットの軌跡制御装置のみでなく、ロボットの動作を画面上でシミュレーションするロボットのシミュレーション装置についても適応可能であるので、この実施の形態６では、本発明に係るロボットの速度演算装置をロボットのシミュレーション装置に適用して説明する。
【０１４９】
図１７に、本発明に係るロボットの速度演算装置を採用したロボットのシミュレーション装置の電気的構成図を示す。図において、３１はロボットのシミュレーションを行なうＣＰＵ、３２はＣＰＵ３１にシミュレーション動作を行わせるためのプログラムが格納されたメモリ、３３はＣＰＵ３１やメモリ３２と表示装置３５および操作盤３４とのインターフェイスである。
【０１５０】
次に、この実施の形態６の動作を図面を参照して説明する。
図１８に、メモリ３２に記憶されたプログラムによって実行されるロボットの速度演算方法を示す。
【０１５１】
図において、まず、ステップ１０１から１１０までの処理は、図２に示す上記実施の形態１の場合と同じである。
【０１５２】
そして、次のステップ１１２において、現在位置を更新すると同時に、インターフェイス３３を通じて、シミュレートされた現在の位置（姿勢を含む）を表示装置３５に表示する
その後、ステップ１１３からステップ１１４までの処理は、図４に示す実施の形態１の場合と同じである。
【０１５３】
従って、この実施の形態６によれば、座標変換計算の結果を放棄する必要のあるフィードバック技術や、制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、関節軸の定格最大角速度を超過を生じることなく、原点近傍を直線補間動作にて通過できるようにロボットの軌跡をシミュレーションすることが可能になる。
【０１５４】
【発明の効果】
以上説明したように、本発明では、ロボットの現在位置情報を取得する手段と、ロボットの関節の定格最高速度情報を取得する手段と、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶する記憶手段と、上記ロボットの現在位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索する手段と、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用する手段とを設けたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１５５】
また、次の発明では、ロボットの現在関節情報を取得する手段と、ロボットの関節の定格最高速度情報を取得する手段と、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶する記憶手段と、上記現在関節情報と上記関節定格最高速度とに基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索する手段と、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用する手段とをさらに設けたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１５６】
また、次の発明では、ロボットはスカラロボットであり、ロボットの現在関節情報として上記スカラロボットの現在関節角度情報を取得し、ロボットの関節の定格最高速度情報として上記スカラロボットの関節の定格最高角速度情報を取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１５７】
また、次の発明では、スカラロボットは水平面内２自由度スカラロボットであり、スカラロボットの現在関節角度情報として上記水平面内２自由度スカラロボットの現在関節角度情報を取得し、スカラロボットの関節の定格最高角速度情報として上記平面内２自由度スカラロボットの第一関節の定格最高角速度を取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない水平面内２自由度スカラロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１５８】
また、次の発明では、水平面内２自由度スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報を入力して先端姿勢軸による速度寄与補正係数を演算する手段と、当該先端姿勢軸を有する３自由度スカラロボットの関節角度情報とその第一関節の定格最高角速度と、に基づく当該先端姿勢軸を有する３自由度スカラロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じる手段と、をさらに備えたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、先端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を超過しない先端姿勢軸を有する３自由度スカラロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１５９】
また、次の発明では、ロボットは水平面内２自由度円筒座標ロボットであり、ロボットの現在関節情報として上記水平面内２自由度円筒座標ロボットの現在関節情報を取得し、ロボットの定格最高速度情報として上記水平面内２自由度円筒座標ロボットの旋回軸の定格最高角速度を取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない水平面内２自由度円筒座標ロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１６０】
また、次の発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの関節情報と旋回軸の定格最高角速度と、に基づく上記先端姿勢軸を有する３自由度円筒座標ロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じる手段と、をさらに備えたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、先端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を超過しない先端姿勢軸を有する３自由度円筒座標ロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１６１】
また、次の発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として上記垂直多関節ロボットの現在関節情報と旋回軸の定格最高角速度とを取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない垂直多関節ロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１６２】
【０１６３】
【０１６４】
また、次の発明では、前記座標上における近似許容限界速度として予め記憶する前記記憶手段は、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、正方形形状に近似されたものを記憶することができるので、演算能力の小さい装置においても、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求めることができる。
【０１６５】
また、次の発明では、前記座標上における近似許容限界速度として予め記憶する前記記憶手段は、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、長方形形状に近似されたものを記憶することができるので、演算能力の小さい装置においても、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求めることができる。
【０１６６】
また、次の発明では、スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報に基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じる手段と、をさらに備えたので、先端姿勢軸の寄与を考慮して、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求めることができる。
【０１６７】
【０１６８】
また、次の発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得する手段と、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得する手段と、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算する手段と、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じる手段と、をさらに備えたので、原点近傍を直線補間動作にて通過する場合に、先端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を超過しない先端姿勢軸を有する３自由度円筒座標ロボット先端の最大速度を求めることができる。
【０１６９】
また、次の発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボット先端の直線補間動作において取り得る前記近似許容限界速度は、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として検索されるようにしたので、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない垂直多関節ロボット先端の最大速度を求めることができる。
【０１７０】
また、次の発明では、ロボットの現在位置における進路方向を取得する手段と、上記ロボットの現在位置における進路方向に基づいてその進路方向と平行な方向へ上記ロボットの現在位置の座標を変換する手段と、をさらに備え、前記近似許容限界速度を検索する手段は、上記変換された上記ロボットの現在位置の座標に基づいて上記許容限界速度を検索するようにしたので、関節軸の定格最大角速度を超過しないロボット先端の最大速度の演算を簡略化することができる。
【０１７１】
【０１７２】
【０１７３】
また、次の発明では、ロボットの現在位置情報を取得するステップと、ロボットの関節の定格最高速度情報を取得するステップと、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶するステップと、上記ロボットの現在位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索するステップと、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用するステップとを備えたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１７４】
また、次の発明では、ロボットの現在関節情報を取得するステップと、ロボットの関節の定格最高速度情報を取得するステップと、ロボット先端の直線補間動作において、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、多角形状に近似した上で、前記座標上における近似許容限界速度として予め記憶するステップと、上記ロボットの現在位置情報に基づいて、上記近似許容限界速度から現在位置における直線補間動作において取り得る前記近似許容限界速度を検索するステップと、指定された速度、加速パターンでの速度、減速パターンでの速度及び前記近似許容限界速度の中から最小の速度のものを指令速度として採用するステップとを備えたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１７５】
また、次の発明では、ロボットはスカラロボットであり、ロボットの現在関節情報として上記スカラロボットの現在関節角度情報を取得し、ロボットの関節の定格最高速度情報として上記スカラロボットの関節の定格最高角速度情報を取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１７６】
また、次の発明では、スカラロボットは水平面内２自由度スカラロボットであり、スカラロボットの現在関節角度情報として上記水平面内２自由度スカラロボットの現在関節角度情報を取得し、スカラロボットの関節の定格最高角速度情報として上記平面内２自由度スカラロボットの第一関節の定格最高角速度を取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない水平面内２自由度スカラロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１７７】
また、次の発明では、水平面内２自由度スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報を入力して先端姿勢軸による速度寄与補正係数を演算するステップと、当該先端姿勢軸を有する３自由度スカラロボットの関節角度情報とその第一関節の定格最高角速度と、に基づく当該先端姿勢軸を有する３自由度スカラロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じるステップと、をさらに備えたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、先端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を超過しない先端姿勢軸を有する３自由度スカラロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１７８】
また、次の発明では、ロボットは水平面内２自由度円筒座標ロボットであり、ロボットの現在関節情報として上記水平面内２自由度円筒座標ロボットの現在関節情報を取得し、ロボットの定格最高速度情報として上記水平面内２自由度円筒座標ロボットの旋回軸の定格最高角速度を取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない水平面内２自由度円筒座標ロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１７９】
また、次の発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの関節情報と旋回軸の定格最高角速度と、に基づく上記先端姿勢軸を有する３自由度円筒座標ロボット先端の直線補間動作における前記近似許容限界速度に、上記速度寄与補正係数を乗じるステップと、をさらに備えたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、先端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を超過しない先端姿勢軸を有する３自由度円筒座標ロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１８０】
また、次の発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として上記垂直多関節ロボットの現在関節情報と旋回軸の定格最高角速度とを取得するようにしたので、座標変換計算の結果を放棄する必要のあるフィードバック技術を用いることなく、同時に制御的に不安定を生じる恐れの在るフィードフォワード技術を用いることなく、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない垂直多関節ロボット先端の最大速度を求める際に、直線補間動作を行う前にロボット先端の位置と直線補間動作における取り得る速度の関係を計算し記憶することにより、直線補間動作のサンプリング中は三角関数などからなる演算を行うことなく、計算機にとって計算負荷の小さい比較演算を主とした検索処理によって、原点近傍を直線補間動作にて通過する場合の関節軸の定格最大角速度を超過しないロボット先端の最大速度を求めることができる。
【０１８１】
【０１８２】
【０１８３】
また、次の発明では、前記座標上における近似許容限界速度として予め記憶する前記ステップは、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、正方形形状に近似されたものを記憶するので、直線補間動作開始前に生成される等高線情報の量を小さくすることができ、同時に直線補間動作のサンプリング中に少ない比較演算にてロボット先端の最大速度を検索することができるので、演算能力の小さい装置においても、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求めることができる。
【０１８４】
また、次の発明では、前記座標上における近似許容限界速度として予め記憶する前記ステップは、経路の方向と座標軸とが平行となる座標とした場合に、前記座標上の座標軸を対象軸とする、所定の許容限界速度が形成する双子の共役楕円形状を、長方形形状に近似されたものを記憶するので、直線補間動作開始前に生成される等高線情報の量を小さくすることができ、同時に直線補間動作のサンプリング中に少ない比較演算にてロボット先端の最大速度を検索することができるので、演算能力の小さい装置においても、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求めることができる。
【０１８５】
また、次の発明では、スカラロボットは先端姿勢軸を有する３自由度スカラロボットであり、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度スカラロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度スカラロボットの現在関節角度情報に基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じるステップと、をさらに備えたので、先端姿勢軸の寄与を考慮して、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しないスカラロボット先端の最大速度を求めることができる。
【０１８６】
【０１８７】
また、次の発明では、水平面内２自由度円筒座標ロボットは先端姿勢軸を有する３自由度円筒座標ロボットであり、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における直線移動残距離情報を取得するステップと、上記先端姿勢軸を有する３自由度円筒座標ロボットの現在位置における先端姿勢軸移動残角度情報を取得するステップと、上記直線移動残距離情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自由度円筒座標ロボットの現在関節角度情報とに基づいて上記先端姿勢軸による速度寄与補正係数を演算するステップと、検索された前記近似許容限界速度に上記速度寄与補正係数を乗じるステップと、をさらに備えたので、原点近傍を直線補間動作にて通過する場合に、先端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を超過しない先端姿勢軸を有する３自由度円筒座標ロボット先端の最大速度を求めることができる。
【０１８８】
また、次の発明では、水平面内２自由度円筒座標ロボットは垂直多関節ロボットであり、上記垂直多関節ロボット先端の直線補間動作において取り得る前記近似許容限界速度は、上記垂直多関節ロボットの水平面へ投影して、上記垂直多関節ロボットの第１アームおよび第２アームの水平面への投影長を上記垂直多関節ロボットのアーム長として検索されるようにしたので、原点近傍を直線補間動作にて通過する場合に、関節軸の定格最大角速度を超過しない垂直多関節ロボット先端の最大速度を求めることができる。
【０１８９】
また、次の発明では、ロボットの現在位置における進路方向を取得するステップと、上記ロボットの現在位置における進路方向に基づいてその進路方向と平行な方向へ上記ロボットの現在位置の座標を変換するステップと、をさらに備え、前記近似許容限界速度を検索するステップは、上記変換された上記ロボットの現在位置の座標に基づいて上記許容限界速度を検索するようにしたので、関節軸の定格最大角速度を超過しないロボット先端の最大速度の演算を簡略化することができる。
【０１９０】
【０１９１】
【図面の簡単な説明】
【図１】  本発明に係るロボットの速度演算装置およびロボットの速度演算方法を採用したロボットの軌跡制御装置とロボット本体との電気的構成図である。
【図２】  本発明に係るロボット速度演算装置の実施の形態１の動作を示すフローチャートである。
【図３】  本発明に係るロボットの動作速度状態図である。
【図４】  実施の形態１が制御対象とするロボット水平面内２自由度スカラ型ロボットの機構図である。
【図５】  ロボットの動作経路説明図である。
【図６】  縦軸をロボット先端の速度ならびに第１関節軸の回転角速度をとり、横軸に時刻をとった速度状態図である。
【図７】  実施の形態２が制御対象とする先端姿勢軸を有する水平面内３自由度スカラ型ロボットの機構図である。
【図８】  実施の形態２の動作を示すフローチャートである。
【図９】  実施の形態３が制御対象とする円筒座標型ロボットの機構図である。
【図１０】  実施の形態４が制御対象とする垂直多関節型ロボットの機構図である。
【図１１】  実施の形態４の動作を示すフローチャートである。
【図１２】  水平面内２自由度スカラ型ロボットの許容限界速度の等高線図である。
【図１３】  水平面内２自由度スカラ型ロボットの長方形にて近似した許容限界速度の等高線図である。
【図１４】  実施の形態５の動作を示すフローチャートである。
【図１５】  ロボットの座標回転の説明図である。
【図１６】  円筒座標ロボットの許容限界速度の等高線図である。
【図１７】  実施の形態６であるロボットのシミュレーション装置の電気的構成図である。
【図１８】  実施の形態６のロボットのシミュレーション装置の動作を示すフローチャートである。
【図１９】  従来技術のロボット速度演算装置の直線補間方法の方法を示すフローチャートである。
【図２０】  従来技術によるロボットの動作速度状態図である。
【図２１】  特異領域を通過する従来技術によるロボットの動作説明図である。
【図２２】  縦軸をロボット先端の速度ならびに第１関節軸の回転角速度をとり、横軸に時刻をとった場合の従来のロボットの速度状態図である。
【図２３】  フィードバック技術を用いた他の従来技術によるロボット速度演算装置の直線補間方法のフローチャートである。
【符号の説明】
１  軌跡制御装置、２  ロボット本体、３  ティーチングボックス、４  操作盤、１１  ＣＰＵ、１２  メモリ、１３  インターフェイス、１４ａ〜１４ｄ  サーボドライバ、１５ａ〜１５ｄ  モータ、１６ａ〜１６ｄ  エンコーダ。
【図１】
0004060393.tif 000051
【図２】
0004060393.tif 000052
【図３】
0004060393.tif 000053
【図４】
0004060393.tif 000054
【図５】
0004060393.tif 000055
【図６】
0004060393.tif 000056
【図７】
0004060393.tif 000057
【図８】
0004060393.tif 000058
【図９】
0004060393.tif 000059
【図１０】
0004060393.tif 000060
【図１１】
0004060393.tif 000061
【図１２】
0004060393.tif 000062
【図１３】
0004060393.tif 000063
【図１４】
0004060393.tif 000064
【図１５】
0004060393.tif 000065
【図１６】
0004060393.tif 000066
【図１７】
0004060393.tif 000067
【図１８】
0004060393.tif 000068
【図１９】
0004060393.tif 000069
【図２０】
0004060393.tif 000070
【図２１】
0004060393.tif 000071
【図２２】
0004060393.tif 000072
【図２３】
0004060393.tif 000073