JPH09265313A

JPH09265313A - ロボットの速度演算装置、およびロボットの速度演算方法

Info

Publication number: JPH09265313A
Application number: JP8310579A
Authority: JP
Inventors: Taro Harima; 太郎播磨
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 1996-01-24
Filing date: 1996-11-21
Publication date: 1997-10-07
Anticipated expiration: 2016-11-21
Also published as: JP4060393B2; KR100262382B1; US6092004A; KR970058856A

Abstract

(57)【要約】【課題】原点近傍を直線補間動作にて通過する場合で
も、関節軸の定格最大角速度を超過しない範囲でロボッ
ト先端の最大速度を求める。【解決手段】ステップ１１６により直線補間の開始位
置と終了位置を結ぶ進路に沿った単位ベクトルを算出
し、次のステップ１１７によりロボットの進路とＸ軸の
成す進路傾斜を算出し、ステップ１０３では残距離を更
新し、続くステップ１０４〜１０６によりそれぞれ指定
速度の計算、加速パターンでの速度計算、減速パターン
での速度計算を行ない、続くステップ１１８により関節
角度を修正して、次のステップ１０７によりロボット先
端の直線補間動作において取り得る許容限界速度を算出
し、次のステップ１０８によりステップ１０４〜１０７
の各ステップで算出した４つの速度の中から最少のもの
を指令速度として採用する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ロボットの軌跡制
御装置や軌跡シミュレーション装置等に採用されるロボ
ットの速度演算装置ならびにロボットの速度演算方法に
関し、特に、直交座標での直線補間を行なう際の速度の
特異領域を速度過大を生ずることなく通過できるように
するロボットの速度演算装置ならびにロボットの速度演
算方法に関するものである。

【０００２】

【従来の技術】ロボット制御において直線補間による軌
跡制御とは、ロボット先端の軌跡が直線になり、その速
度が例えば台形速度パターン等になるように制御する方
法である。

【０００３】従来のロボットの直線補間による軌跡制御
方法を図１９、図２０を参照しながら説明する。図１９
は従来の直線補間のアルゴリズムのフローチャート、図
２０は従来の直線補間による軌跡制御によるロボット先
端の速度状態図を示している。

【０００４】図１９においては、ステップ１０１により
直線補間が開始される。この直線補間の手続きは、上位
の作業計画などの手続きから呼び出されることにより開
始する。この手続きが呼び出されるときには、上位の手
続きによって現在位置と最終目標位置が確定し、この手
続きに与えられるものである。

【０００５】直線補間が開始されると、まずステップ１
０２によりロボットの開始位置と最終目標位置から移動
距離が算出され、次のステップ１０３によりそのロボッ
トの現在位置と最終目標位置からロボットの移動すべき
残距離が算出される。

【０００６】そして、次のステップ１０４によりプログ
ラムの中の指定速度と、プログラムの中で指定された速
度倍率と操作盤から指定された速度倍率を乗じて、指定
速度ｖ１を算出し、次のステップ１０５により指定加速
度α１にサンプリング時間Δｔを乗じた速度増分を現在
速度ｖ０に加えて、加速パターン動作での速度ｖ２を下
記数１式により算出する。

【数１】

【０００７】次に、ステップ１０６によりステップ１０
３で算出した残距離を、プログラム中で指定された減速
加速度により一定の割合で減速する減速パターンで移動
する場合の速度を算出する。すなわち、指定された減速
の加速度α２にて減速し続けて、残距離Ｌだけ移動した
ときに速度が０となる速度ｖ３を下記数２式に基づいて
算出する。

【数２】

【０００８】次に、ステップ１３０において、ステップ
１０４、１０５、１０６で算出した３つの速度の中から
最少の値のものを指令速度として採用する。例えば、ロ
ボット先端の速度状態を示す図２０を参照して具体的に
説明すると、時刻ｔ＝ｔ１の場合は、ステップ１０５で
算出される加速パターン動作での速度ｖ２が、最少の速
度であるのでこれを採用する。一方、時刻ｔ＝ｔ２の場
合は、ステップ１０４で算出される一定速度である指定
速度ｖ１が最少の速度であるのでこれを採用し、さらに
時刻ｔ＝ｔ３の場合は、ステップ１０６で算出される減
速パターン動作での速度ｖ３が最少の速度であるので、
これを採用する。

【０００９】次に、図１９に戻り、ステップ１０９によ
り、現在位置からステップ１３０にて決定された指令速
度で移動する補間位置を算出し、続いて次のステップ１
１０により直交座標で表現されている補間位置を関節座
標の表現に座標変換し、さらに次のステップ１１１によ
り各々の関節を目標となる補間位置へ駆動して、次のス
テップ１１２により現在位置を補間位置に更新するよう
にする。

【００１０】そして、次のステップ１１３により現在位
置を最終目標位置と比較して、目標位置に到達したか否
か判定し、未到着の場合には、ステップ１０３へ実行を
移す一方、到達している場合には直線補間手続きを終了
する。

【００１１】

【発明が解決しようとする課題】しかしながら、従来の
ロボットの直線補間による軌跡制御では、上記のように
ロボット先端の速度のみに着目して直線補間を行ように
していたため、例えばスカラ型ロボット等においては、
先端速度の早い遅いにかかわらず第１軸の速度が非常に
大きな値になるので、原点近傍を直線補間動作で通過す
ることが非常に困難である、という問題があった。

【００１２】この問題点を、図２１に示す特異領域を通
過する従来技術によるロボットの動作説明図を参照して
詳細に説明する。図２１では、Ｘ軸に沿って、（１）の
位置から（４）の位置までを原点を通過して直線補間す
るスカラロボットが示されている。図中、このスカラロ
ボットは、長さ５ｃｍの第１アームＡ１と、同じく長さ
５ｃｍの第２アームＡ２を有しており、第２アームＡ２
先端がＸ軸上を図上左方向へ単位時間あたり５ｃｍで
（１）→（２）→（３）→（４）と移動する。

【００１３】図２０の場合は、（１）→（２）では、第
１アームＡ１は反時計回りに５８度、（２）→（３）の
場合は反時計回りに１１７度、（３）→（４）の場合
は、時計回りに１６度回転している。

【００１４】ところで、直線補間動作では、ロボット先
端、すなわち図２１の場合では第２アームＡ２先端の速
度が一定であるので、原点近傍をすり抜ける（２）→
（３）の動作の場合、第１アームＡ１は非常に大きな角
度を移動しなくてはならない。図２１で示した例では、
第１アームＡ１は、（２）→（３）の場合、それぞれ、
（１）→（２）の約２倍、（３）→（４）の約７倍の角
速度で動作しなければならない。

【００１５】このため、条件によっては、第１軸の能力
を超えた早い回転速度で第１軸が回転する必要が生じ、
速度過大のエラーを生じて、ロボットが動作を中断して
しまう場合もでてくるという不具合が生じる。

【００１６】これを、縦軸をロボット先端の速度ならび
に第１関節軸の回転角速度をとり、横軸に時刻をとった
速度状態を示す図２２で説明する。図２２において、台
形状の線図Ｂ１が図２０に示すようにして求めたロボッ
ト先端の速度を示しており、山形の曲線Ｂ２が第１関節
の回転角速度を示している。ここで、ロボット先端の進
行に伴い第１関節が動作するが、徐々に回転角速度が増
加し、図２１の（２）→（３）の動作のように大きな角
度を移動する場合に、第１関節の回転角速度が第１関節
の定格最大角速度Ｊ１ｍａｘを越えてしまい（尚、図２
２ではｔ＝ｔ０で越えている。）、ロボットに速度過大
のエラーが生じて、以降の運転を続行することができな
くなる不具合が生じる。

【００１７】この不具合を数学的に証明する。スカラ型
ロボットにおいて、ロボット先端位置を（Ｘ，Ｙ）、関
節角度（θ１、θ２）とした場合の先端位置の速度と関
節角速度との関係は、ヤコビ行列Ｊによって、次の数３
式により表現できる。

【数３】ここで、Ｊは、下記の数４式のように定義されるヤコビ
行列である。

【数４】

【００１８】従って、先端位置速度から関節角速度は、
逆ヤコビ行列Ｊｒにより、次の数５式の関係が成り立
つ。

【数５】ここで、Ｊｒは、下記の数６式のように定義される逆ヤ
コビ行列である。

【数６】

【００１９】ここで上記数６式は、２元連立方程式であ
る。この連立方程式の第１式は、第１軸の関節角速度と
先端位置速度の関係を表わしている。図２１に示す２自
由度のスカラロボットの場合は、Ｘ方向の速度成分のみ
をもちＹ方向成分の速度が０となるので、第１軸の関節
角速度Ｊ１は、次の数７式のようになる。

【数７】

【００２０】ここで、上記数７式において、第２アーム
Ａ２先端が原点に近づき、第１アームＡ１と第２アーム
Ａ２が重なる姿勢すなわち、θ２＝πに近づくにつれ
て、Ｊ１の分母は０へ近づくために、ロボット先端の速
度ｄＸ／ｄｔの値に関らず、第１軸の関節角速度は無限
大に近づく。

【００２１】従って、第１軸の能力を超えた早い回転速
度で第１軸は回転する必要が生じて、速度過大のエラー
を生じて、ロボットが動作を中断してしまう場合もでて
くるという不具合が生じる。

【００２２】同様にして、後述する図１０に示す円筒座
標型ロボットや、図４に示すスカラ型ロボットにおいて
は、ロボット先端位置の速度と関節速度との関係は、ヤ
コビ行列Ｊによって、次の数８式のように表現できる。

【数８】ここで、Ｊは、下記の数９式のように定義されるヤコビ
行列である。

【数９】

【００２３】従って、先端位置速度から関節角速度は、
逆ヤコビ行列Ｊｒにより、次の数１０式の関係が成り立
つ。

【数１０】ここで、Ｊｒは、下記の数１１式のように定義される逆
ヤコビ行列である。

【数１１】

【００２４】ところで、図２１と同様にＸ軸に沿った動
作を行った場合は、連立方程式である上記数６式の第２
式より、ロボット先端の速度ｄｘ／ｄｔとｄθ／ｄｔの
関係は、次の数１２式のようになる。

【数１２】よって、θ＝４５度近傍で、所定の先端速度ｄｘ／ｄｔ
にて、旋回軸が最高回転数に達している場合は、ロボッ
トの先端速度が同一であるにもかかわらず、原点近傍を
通過する時点、すなわちθ＝９０度では、旋回軸がsin
(９０°)／sin(４５°)＝２1/2≒１．４１倍、すなわち
４０％増しで旋回しなければならず、速度過大を生じて
しまう。逆に、この姿勢で速度過大が発生しないように
設定した場合は、残りの姿勢でロボットの能力に余裕が
生じ、動作範囲全体では、サインカーブの実効値は２／
π＝０．６３となり、およそ３７％の能力が生かされな
いという課題があった。

【００２５】そこで、従来のロボットの軌跡制御方法に
よる直線補間では、上記課題を解決するために、例えば
特開昭５８−１１４８８８号公報等によりフィードバッ
ク技術を用いる方法が提案されている。

【００２６】図２３は、特開昭５８−１１４８８８号公
報に示された制御方法をフローチャートにしたものであ
る。図２３を参照して、本提案の特異領域における速度
過大エラーを回避する軌跡制御方法を説明する。まず、
ステップ１０１からステップ１１０までの処理は、上述
の図１９に示す基本的な制御方法と同様である。

【００２７】そして、次のステップ１３１においては、
関節座標において速度過大の発生の可否を判定し、いず
れの関節においても速度過大が発生していない場合に
は、図１９に示す基本的制御の場合と同様のステップ１
１１以下の処理を行なう一方、いずれかの関節において
速度過大が発生した場合には、ステップ１３２の処理へ
移行して指令速度の調整を行なう。具体的には、ステッ
プ１３１において速度過大になった軸の角速度の最大速
度に対する超過割合を求め、次に超過割合の逆数を指令
速度に乗ずる等して、速度過大が発生しないように指令
速度を調節する。

【００２８】例えば、最大角速度が３．１４ｒａｄ／ｓ
の第１関節に対して、座標変換の結果、指令速度が９０
ｍｍ／ｓにより、４．７２ｒａｄ／ｓが指令として与え
られた場合を考えると、第１関節の超過割合は、４．７
２ｒａｄ／ｓ÷３．１４ｒａｄ／ｓ＝１．５倍であるの
で、その逆数０．６６７を指令速度に乗じて、６０ｍｍ
／ｓと調整する。なお、その後のステップ１１１からス
テップ１１４までの処理は、図１９に示す従来の基本的
な制御方法と同様である。

【００２９】しかしながら、上記特開昭５８−１１４８
８８号公報によって提案された課題解決方式では、上記
のようにフィードバック技術を用いているため、ステッ
プ１０４からステップ１１０の一連の演算を行い、関節
に定格の最大角速度を超える指令が生成されなければな
らない。また、それらの一連の演算の結果は一旦破棄さ
れて、再度、ステップ１０９からステップ１１０の演算
を少なくとも、もう一度計算しなければならない。ここ
で、ステップ１１０の座標変換演算は、２自由度ロボッ
トにおいて逆正接関数を２回、平方根を１回、二乗演算
を６回、４乗演算を２回、加減算を６回行うものであ
る。一般に、ロボット速度演算装置では、軌跡精度など
を高くするために、サンプリング間隔を極力短い時間に
設定しているので、サンプリング時間に、これらの複雑
な演算を２回以上繰り返す余裕が設定されていない。

【００３０】このため、この方式では、サンプリング時
間以内に演算が終了しない場合には、関節に指令が与え
られず、ロボットの軌跡に乱れを生じるか、もしくは、
サンプリング時間以内にステップ１１０の演算を２回以
上行うことができる高度な能力を有するＣＰＵ１１を準
備しなければならない、という新たな問題が生じる。

【００３１】また、特開昭５９−１６３６０９号公報、
特開昭６２−１８９５０４号公報、特開平２−４７７０
２号公報、特開平３−６６５７６号公報、特開平６−３
１４７３０号公報、特開平７−７２９１０号公報記載の
発明においても、特開昭５８−１１４８８８号公報と同
様のフィードバック技術を用いた方法が提案されている
が、これらの方法でもフィードバック技術を用いている
ために、上記特開昭５８−１１４８８８号公報記載の発
明と同様にロボットの軌跡に乱れを生じる等の新たな問
題が生じる。

【００３２】さらに、特開平２−３０８３１号公報記載
の発明においては、移動を開始する以前に、始動点から
目標点までのすべての補間点を事前に演算して速度過大
エラーによる運転中断を回避するロボットの制御方式が
提案されているが、この方式では、事前にすべての補間
点を計算する必要があり、動作開始前に演算のためにロ
ボットが動作を停止するか、若しくはロボットの停止を
回避するために高速な演算装置を必要とすると共に、さ
らに直線動作中に外部の操作盤などから動作速度の変更
指令を反映することができない、という新たな問題が生
じる。

【００３３】またさらに、特開昭６０−５７４０８号公
報記載の発明では、特異領域を通過する場合に、通過の
間だけ指令速度を無効とし、所定の微小速度にてロボッ
トを運転させ特異領域を通過することにより速度過大エ
ラーによる運転中断を回避するロボットの制御方式が提
案されているが、この方式では、本来、ロボットがより
早い速度で動きうるにもかかわらず特異領域内のすべて
の位置において速度過大を生じない微小速度で運転され
るため、ロボットの速度が非常に遅くなってしまう、と
いう問題がある。

【００３４】またさらに、特開平３−６６５７６号公報
や特開平６−１７０７６５号公報記載の発明では、特異
領域に進入すると、指令速度に１より小さい所定の係数
を適宜乗じることで、指令速度を引き下げることにより
速度過大エラーによる運転中断を回避するロボットの制
御方式が提案がなされているが、このロボットの制御方
式では、速度過大を回避するために速度を減ずる係数の
決定方法が開示されていないので、実施に当たっては操
作者が、個々のロボットにおいて、個々の経路毎に、試
行錯誤によってその係数を求めざるをえない、という問
題がある。

【００３５】そこで、本発明は、このような問題点を解
決するためになされたもので、ロボットの軌跡に乱れを
生じたり、また高度な能力を有するＣＰＵを準備等する
ことなく、原点近傍を直線補間動作にて通過する場合に
関節軸の定格最大角速度を超過しない範囲でロボット先
端の最大速度を求めることのできるロボットの速度演算
装置および軌跡制御方法を提供することを目的とする。

【００３６】

【課題を解決するための手段】上記課題を解決するた
め、本発明では、また、本発明では、ロボットの現在位
置情報を取得する手段と、ロボットの関節の定格最高速
度情報を取得する手段と、上記ロボット現在位置情報と
上記関節定格最高速度とに基づいてロボット先端の直線
補間動作において取り得る許容限界速度を算出する手段
と、を備えたものである。

【００３７】また、本発明では、ロボットの現在関節情
報を取得する手段と、ロボットの関節の定格最高速度情
報を取得する手段と、上記現在関節情報と上記関節定格
最高速度情報とに基づいてロボット先端の直線補間動作
において取り得る許容限界速度を算出する手段と、を備
えたものである。

【００３８】また、本発明では、ロボットはスカラロボ
ットであり、ロボットの現在関節情報として上記スカラ
ロボットの現在関節角度情報を取得し、ロボットの関節
の定格最高速度情報として上記スカラロボットの関節の
定格最高角速度情報を取得するものである。

【００３９】また、本発明では、スカラロボットは水平
面内２自由度スカラロボットであり、スカラロボットの
現在関節角度情報として上記水平面内２自由度スカラロ
ボットの現在関節角度情報を取得し、スカラロボットの
関節の定格最高角速度情報として上記平面内２自由度ス
カラロボットの第一関節の定格最高角速度を取得するも
のである。

【００４０】また、本発明では、水平面内２自由度スカ
ラロボットは先端姿勢軸を有する３自由度スカラロボッ
トであり、上記先端姿勢軸を有する３自由度スカラロボ
ットの現在位置における直線移動残距離情報を取得する
手段と、上記先端姿勢軸を有する３自由度スカラロボッ
トの現在位置における先端姿勢軸移動残角度情報を取得
する手段と、上記直線移動残距離情報と上記先端姿勢軸
移動残角度情報と上記先端姿勢軸を有する３自由度スカ
ラロボットの現在関節角度情報を入力して先端姿勢軸に
よる速度寄与補正係数を演算する手段と、当該先端姿勢
軸を有する３自由度スカラロボットの現在関節角度情報
とその第一関節の定格最高角速度とに基づいて算出され
た当該先端姿勢軸を有する３自由度スカラロボット先端
の直線補間動作において取り得る許容限界速度に上記速
度寄与補正係数を乗じる手段と、をさらに備えたもので
ある。

【００４１】また、本発明では、ロボットは水平面内２
自由度円筒座標ロボットであり、ロボットの現在関節情
報として上記水平面内２自由度円筒座標ロボットの現在
関節情報を取得し、ロボットの定格最高速度情報として
上記水平面内２自由度円筒座標ロボットの旋回軸の定格
最高角速度を取得し、上記上記水平面内２自由度円筒座
標ロボットの現在関節情報と旋回軸の定格最高角速度と
に基づいて上記水平面内２自由度円筒座標ロボット先端
の直線補間動作において取り得る許容限界速度を算出す
るものである。

【００４２】また、本発明では、水平面内２自由度円筒
座標ロボットは先端姿勢軸を有する３自由度円筒座標ロ
ボットであり、上記先端姿勢軸を有する３自由度円筒座
標ロボットの現在位置における直線移動残距離情報を取
得する手段と、上記先端姿勢軸を有する３自由度円筒座
標ロボットの現在位置における先端姿勢軸移動残角度情
報を取得する手段と、上記直線移動残距離情報と上記先
端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自
由度円筒座標ロボットの現在関節角度情報とに基づいて
上記先端姿勢軸による速度寄与補正係数を演算する手段
と、上記先端姿勢軸を有する３自由度円筒座標ロボット
の現在関節情報と旋回軸の定格最高角速度とに基づいて
算出した上記先端姿勢軸を有する３自由度円筒座標ロボ
ット先端の直線補間動作において取り得る許容限界速度
に上記速度寄与補正係数を乗じる手段と、をさらに備え
たものである。

【００４３】また、本発明では、水平面内２自由度円筒
座標ロボットは垂直多関節ロボットであり、上記垂直多
関節ロボットの水平面へ投影して、上記垂直多関節ロボ
ットの第１アームおよび第２アームの水平面への投影長
を上記垂直多関節ロボットのアーム長として上記垂直多
関節ロボットの現在関節情報と旋回軸の定格最高角速度
とに基づいて上記垂直多関節ロボット先端の直線補間動
作において取り得る許容限界速度を算出するものであ
る。

【００４４】また、本発明では、算出されたロボット先
端の直線補間動作において取り得る許容限界速度の等高
線を記憶する手段と、ロボットの現在位置情報を取得す
る手段と、上記ロボットの現在位置情報に基づいて上記
予め記憶された許容限界速度の等高線から現在位置にお
ける直線補間動作において取り得る許容限界速度を検索
する手段と、をさらに備えたものである。

【００４５】また、本発明では、算出されたロボット先
端の直線補間動作において取り得る許容限界速度の等高
線を多角形、円もしくは楕円等に近似して記憶する手段
と、ロボットの現在位置情報を取得する手段と、上記ロ
ボットの現在位置情報に基づいて上記予め記憶された近
似された許容限界速度の等高線から現在位置における直
線補間動作において取り得る許容限界速度を検索する手
段と、をさらに備えたものである。

【００４６】また、本発明では、算出されたスカラロボ
ット先端の直線補間動作において取り得る許容限界速度
の等高線を正方形に近似してその辺長を記憶する手段
と、上記スカラロボットの現在位置情報を取得する手段
と、上記スカラロボットの現在位置情報に基づいて上記
予め記憶された辺長から現在位置における直線補間動作
において取り得る許容限界速度を検索する手段と、をさ
らに備えたものである。

【００４７】また、本発明では、算出されたスカラロボ
ット先端の直線補間動作において取り得る許容限界速度
の等高線を長方形に近似して原点から長方形の各辺まで
の距離を記憶する手段と、上記スカラロボットの現在位
置情報を取得する手段と、上記スカラロボットの現在位
置情報に基づいて上記予め記憶された距離から現在位置
における直線補間動作において取り得る許容限界速度を
検索する手段と、をさらに備えたものである。

【００４８】また、本発明では、スカラロボットは先端
姿勢軸を有する３自由度スカラロボットであり、上記先
端姿勢軸を有する３自由度スカラロボットの現在位置に
おける直線移動残距離情報を取得する手段と、上記先端
姿勢軸を有する３自由度スカラロボットの現在位置にお
ける先端姿勢軸移動残角度情報を取得する手段と、上記
直線移動残距離情報と上記先端姿勢軸移動残角度情報と
上記先端姿勢軸を有する３自由度スカラロボットの現在
関節角度情報に基づいて上記先端姿勢軸による速度寄与
補正係数を演算する手段と、検索された許容限界速度に
上記速度寄与補正係数を乗じる手段と、をさらに備えた
ものである。

【００４９】また、本発明では、算出された水平面内２
自由度円筒座標ロボット先端の直線補間動作において取
り得る許容限界速度の等高線を入力して得られる複数の
円からなる速度の等高線の半径を記憶する手段と、上記
水平面内２自由度円筒座標ロボットの現在位置情報を取
得する手段と、上記水平面内２自由度円筒座標ロボット
の現在位置情報に基づいて上記予め記憶された半径から
現在位置における直線補間動作において取り得る許容限
界速度を検索する手段と、をさらに備えたものである。

【００５０】また、本発明では、水平面内２自由度円筒
座標ロボットは先端姿勢軸を有する３自由度円筒座標ロ
ボットであり、上記先端姿勢軸を有する３自由度円筒座
標ロボットの現在位置における直線移動残距離情報を取
得する手段と、上記先端姿勢軸を有する３自由度円筒座
標ロボットの現在位置における先端姿勢軸移動残角度情
報を取得する手段と、上記直線移動残距離情報と上記先
端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自
由度円筒座標ロボットの現在関節角度情報とに基づいて
上記先端姿勢軸による速度寄与補正係数を演算する手段
と、検索された許容限界速度に上記速度寄与補正係数を
乗じる手段と、をさらに備えたものである。

【００５１】また、本発明では、水平面内２自由度円筒
座標ロボットは垂直多関節ロボットであり、上記垂直多
関節ロボット先端の直線補間動作において取り得る許容
限界速度は、上記垂直多関節ロボットの水平面へ投影し
て、上記垂直多関節ロボットの第１アームおよび第２ア
ームの水平面への投影長を上記垂直多関節ロボットのア
ーム長として算出するものである。

【００５２】また、本発明では、ロボットの現在位置に
おける進路方向を取得する手段と、上記ロボットの現在
位置における進路方向に基づいてその進路方向と平行な
方向へ上記ロボットの現在位置の座標を変換する手段
と、をさらに備え、ロボット先端の直線補間動作におい
て取り得る許容限界速度を算出する手段は、上記変換さ
れた上記ロボットの現在位置の座標に基づいて上記許容
限界速度を算出するものである。

【００５３】また、本発明では、直線補間を行なう際指
定された速度で移動する場合の指定速度を算出する手段
と、直線補間を行なう際指定された加速パターンで移動
する場合の速度を求める手段と、直線補間を行なう際指
定された減速パターンで移動する場合の速度を求める手
段と、上記指定速度、上記加速パターンでの速度、上記
減速パターンでの速度および許容限界速度のうちから最
少の速度を選択する手段、をさらに備えたものである。

【００５４】また、本発明では、直線補間を行なう際指
定された速度で移動する場合の指定速度を算出する手段
と、直線補間を行なう際指定された加速パターンで移動
する場合の速度を求める手段と、直線補間を行なう際指
定された減速パターンで移動する場合の速度を求める手
段と、上記指定速度、上記加速パターンでの速度、上記
減速パターンでの速度および速度寄与補正係数が乗じら
れた許容限界速度のうちから最少の速度を選択する手段
をさらに備えたものである。

【００５５】また、本発明では、ロボットの現在位置情
報を取得するステップと、ロボットの関節の定格最高速
度情報を取得するステップと、上記ロボット現在位置情
報と上記関節定格最高速度とに基づいてロボット先端の
直線補間動作において取り得る許容限界速度を算出する
ステップと、を備えたものである。

【００５６】また、本発明では、ロボットの現在関節情
報を取得するステップと、ロボットの関節の定格最高速
度情報を取得するステップと、上記現在関節情報と上記
関節定格最高速度情報とに基づいてロボット先端の直線
補間動作において取り得る許容限界速度を算出するステ
ップと、を備えたものである。

【００５７】また、本発明では、ロボットはスカラロボ
ットであり、ロボットの現在関節情報として上記スカラ
ロボットの現在関節角度情報を取得し、ロボットの関節
の定格最高速度情報として上記スカラロボットの関節の
定格最高角速度情報を取得するものである。

【００５８】また、本発明では、スカラロボットは水平
面内２自由度スカラロボットであり、スカラロボットの
現在関節角度情報として上記水平面内２自由度スカラロ
ボットの現在関節角度情報を取得し、スカラロボットの
関節の定格最高角速度情報として上記平面内２自由度ス
カラロボットの第一関節の定格最高角速度を取得するも
のである。

【００５９】また、本発明では、水平面内２自由度スカ
ラロボットは先端姿勢軸を有する３自由度スカラロボッ
トであり、上記先端姿勢軸を有する３自由度スカラロボ
ットの現在位置における直線移動残距離情報を取得する
ステップと、上記先端姿勢軸を有する３自由度スカラロ
ボットの現在位置における先端姿勢軸移動残角度情報を
取得するステップと、上記直線移動残距離情報と上記先
端姿勢軸移動残角度情報と上記先端姿勢軸を有する３自
由度スカラロボットの現在関節角度情報を入力して先端
姿勢軸による速度寄与補正係数を演算するステップと、
当該先端姿勢軸を有する３自由度スカラロボットの現在
関節角度情報とその第一関節の定格最高角速度とに基づ
いて算出された当該先端姿勢軸を有する３自由度スカラ
ロボット先端の直線補間動作において取り得る許容限界
速度に上記速度寄与補正係数を乗じるステップと、をさ
らに備えたものである。

【００６０】また、本発明では、ロボットは水平面内２
自由度円筒座標ロボットであり、ロボットの現在関節情
報として上記水平面内２自由度円筒座標ロボットの現在
関節情報を取得し、ロボットの定格最高速度情報として
上記水平面内２自由度円筒座標ロボットの旋回軸の定格
最高角速度を取得し、上記上記水平面内２自由度円筒座
標ロボットの現在関節情報と旋回軸の定格最高角速度と
に基づいて上記水平面内２自由度円筒座標ロボット先端
の直線補間動作において取り得る許容限界速度を算出す
るものである。

【００６１】また、本発明では、水平面内２自由度円筒
座標ロボットは先端姿勢軸を有する３自由度円筒座標ロ
ボットであり、上記先端姿勢軸を有する３自由度円筒座
標ロボットの現在位置における直線移動残距離情報を取
得するステップと、上記先端姿勢軸を有する３自由度円
筒座標ロボットの現在位置における先端姿勢軸移動残角
度情報を取得するステップと、上記直線移動残距離情報
と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有
する３自由度円筒座標ロボットの現在関節角度情報とに
基づいて上記先端姿勢軸による速度寄与補正係数を演算
するステップと、上記先端姿勢軸を有する３自由度円筒
座標ロボットの現在関節情報と旋回軸の定格最高角速度
とに基づいて算出した上記先端姿勢軸を有する３自由度
円筒座標ロボット先端の直線補間動作において取り得る
許容限界速度に上記速度寄与補正係数を乗じるステップ
と、をさらに備えたものである。

【００６２】また、本発明では、水平面内２自由度円筒
座標ロボットは垂直多関節ロボットであり、上記垂直多
関節ロボットの水平面へ投影して、上記垂直多関節ロボ
ットの第１アームおよび第２アームの水平面への投影長
を上記垂直多関節ロボットのアーム長として上記垂直多
関節ロボットの現在関節情報と旋回軸の定格最高角速度
とに基づいて上記垂直多関節ロボット先端の直線補間動
作において取り得る許容限界速度を算出するものであ
る。

【００６３】また、本発明では、算出されたロボット先
端の直線補間動作において取り得る許容限界速度の等高
線を記憶するステップと、ロボットの現在位置情報を取
得するステップと、上記ロボットの現在位置情報に基づ
いて上記予め記憶された許容限界速度の等高線から現在
位置における直線補間動作において取り得る許容限界速
度を検索するステップと、をさらに備えたものである。

【００６４】また、本発明では、算出されたロボット先
端の直線補間動作において取り得る許容限界速度の等高
線を多角形、円もしくは楕円等に近似して記憶するステ
ップと、ロボットの現在位置情報を取得するステップ
と、上記ロボットの現在位置情報に基づいて上記予め記
憶された近似された許容限界速度の等高線から現在位置
における直線補間動作において取り得る許容限界速度を
検索するステップと、をさらに備えたものである。

【００６５】また、本発明では、算出されたスカラロボ
ット先端の直線補間動作において取り得る許容限界速度
の等高線を正方形に近似してその辺長を記憶するステッ
プと、上記スカラロボットの現在位置情報を取得するス
テップと、上記スカラロボットの現在位置情報に基づい
て上記予め記憶された辺長から現在位置における直線補
間動作において取り得る許容限界速度を検索するステッ
プと、をさらに備えたものである。

【００６６】また、本発明では、算出されたスカラロボ
ット先端の直線補間動作において取り得る許容限界速度
の等高線を長方形に近似して原点から長方形の各辺まで
の距離を記憶するステップと、上記スカラロボットの現
在位置情報を取得するステップと、上記スカラロボット
の現在位置情報に基づいて上記予め記憶された距離から
現在位置における直線補間動作において取り得る許容限
界速度を検索するステップと、をさらに備えたものであ
る。

【００６７】また、本発明では、スカラロボットは先端
姿勢軸を有する３自由度スカラロボットであり、上記先
端姿勢軸を有する３自由度スカラロボットの現在位置に
おける直線移動残距離情報を取得するステップと、上記
先端姿勢軸を有する３自由度スカラロボットの現在位置
における先端姿勢軸移動残角度情報を取得するステップ
と、上記直線移動残距離情報と上記先端姿勢軸移動残角
度情報と上記先端姿勢軸を有する３自由度スカラロボッ
トの現在関節角度情報に基づいて上記先端姿勢軸による
速度寄与補正係数を演算するステップと、検索された許
容限界速度に上記速度寄与補正係数を乗じるステップ
と、をさらに備えたものである。

【００６８】また、本発明では、算出された水平面内２
自由度円筒座標ロボット先端の直線補間動作において取
り得る許容限界速度の等高線を入力して得られる複数の
円からなる速度の等高線の半径を記憶するステップと、
上記水平面内２自由度円筒座標ロボットの現在位置情報
を取得するステップと、上記水平面内２自由度円筒座標
ロボットの現在位置情報に基づいて上記予め記憶された
半径から現在位置における直線補間動作において取り得
る許容限界速度を検索するステップと、をさらに備えた
ものである。

【００６９】また、本発明では、水平面内２自由度円筒
座標ロボットは先端姿勢軸を有する３自由度円筒座標ロ
ボットであり、上記先端姿勢軸を有する３自由度円筒座
標ロボットの現在位置における直線移動残距離情報を取
得するステップと、上記先端姿勢軸を有する３自由度円
筒座標ロボットの現在位置における先端姿勢軸移動残角
度情報を取得するステップと、上記直線移動残距離情報
と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を有
する３自由度円筒座標ロボットの現在関節角度情報とに
基づいて上記先端姿勢軸による速度寄与補正係数を演算
するステップと、検索された許容限界速度に上記速度寄
与補正係数を乗じるステップと、をさらに備えたもので
ある。

【００７０】また、本発明では、水平面内２自由度円筒
座標ロボットは垂直多関節ロボットであり、上記垂直多
関節ロボット先端の直線補間動作において取り得る許容
限界速度は、上記垂直多関節ロボットの水平面へ投影し
て、上記垂直多関節ロボットの第１アームおよび第２ア
ームの水平面への投影長を上記垂直多関節ロボットのア
ーム長として算出するものである。

【００７１】また、本発明では、ロボットの現在位置に
おける進路方向を取得するステップと、上記ロボットの
現在位置における進路方向に基づいてその進路方向と平
行な方向へ上記ロボットの現在位置の座標を変換するス
テップと、をさらに備え、ロボット先端の直線補間動作
において取り得る許容限界速度を算出するステップは、
上記変換された上記ロボットの現在位置の座標に基づい
て上記許容限界速度を算出するものである。

【００７２】また、本発明では、直線補間を行なう際指
定された速度で移動する場合の指定速度を算出するステ
ップと、直線補間を行なう際指定された加速パターンで
移動する場合の速度を求めるステップと、直線補間を行
なう際指定された減速パターンで移動する場合の速度を
求めるステップと、上記指定速度、上記加速パターンで
の速度、上記減速パターンでの速度および許容限界速度
のうちから、最少の速度を選択するステップと、をさら
に備えたものである。

【００７３】また、本発明では、直線補間を行なう際指
定された速度で移動する場合の指定速度を算出するステ
ップと、直線補間を行なう際指定された加速パターンで
移動する場合の速度を求めるステップと、直線補間を行
なう際指定された減速パターンで移動する場合の速度を
求めるステップと、上記指定速度、上記加速パターンで
の速度、上記減速パターンでの速度および速度寄与補正
係数が乗じられた許容限界速度のうちから、最少の速度
を選択するステップと、をさらに備えたものである。

【００７４】

【発明の実施の形態】

実施の形態１．図１に、この発明に係るロボットの速度
演算装置およびロボットの速度演算方法を採用したロボ
ットの軌跡制御装置と、ロボット本体との電気的構成を
示す。図１において、１はロボットの速度演算装置、２
はロボット本体、３はティーチングボックス、４は操作
盤、１１はロボット速度演算装置１に内蔵されるＣＰ
Ｕ、１２はＣＰＵ１１に接続され後述する図２に示すよ
うに動作するための制御プログラムが格納されたメモ
リ、１３はティーチングボックス３および操作盤４とＣ
ＰＵ１１との間の各種信号の仲介を行なうインターフェ
イス、１４ａ〜１４ｄはＣＰＵ１１からの指令に基づい
てロボット本体２に組み込まれた後述するモータ１５ａ
〜１５ｄを駆動するサーボドライバ、１５ａ〜１５ｄは
ロボット本体２に組み込まれてロボットアームを駆動す
るモータ、１６ａ〜１６ｄはモータ１５ａ〜１５ｄとと
もにロボット本体２の関節に組み込まれてモータ１５ａ
〜１５ｄによって駆動されるロボットアームの位置や角
度を検出するエンコーダである。

【００７５】次に上記実施の形態１の動作を図２のフロ
ーチャートを参照しながら説明する。図２は、メモリ１
２に記憶された制御プログラムによって実行されるロボ
ットの速度演算方法を示すフローチャートである。な
お、図１９に示す従来の基本的な制御方法と同様なステ
ップには同一ステップ番号を付してその説明は省略する
ものとする。

【００７６】図２において、まず、ステップ１０１およ
びステップ１０２の処理は、図１９に示す従来の基本的
な制御方法と同様である。

【００７７】そして、次のステップ１１６により、この
実施の形態１では、直線補間の開始位置と終了位置を結
ぶ進路に沿った単位ベクトルを算出する。

【００７８】続いて、次のステップ１１７においては、
図５に示すように、進路とＸ軸の成す角度φ（以下、進
路傾斜という。）を算出する。

【００７９】その後、次のステップ１０３からステップ
１０６までの処理は、図１９の従来の基本的な制御方法
と同様である。

【００８０】次のステップ１１８においては、図５に示
すように、ステップ１１７で求めた進路傾斜に基づい
て、座標を進路に平行な方向に回転変換する。すなわ
ち、次の数１３式により第１軸の関節角度から進路傾斜
を減じる。

【数１３】

【００８１】次のステップ１０７においては、現在の位
置における関節の定格最大角速度にて動作しうるロボッ
ト先端での直線補間動作速度の中で最大のもの（以下、
許容限界速度という。）を下記の数１４式にて算出する
（尚、本式が許容限界速度であることの詳細説明は後述
する。）。ただし、ここで、第１関節の関節角度θ１
は、ステップ１１８によって、進路傾斜を減じることに
よる進路に平行な座標に基づくものである。

【数１４】

【００８２】そして、次のステップ１０８においては、
ステップ１０４、１０５、１０６、１０７の各ステップ
で算出した４つの速度の中から最少のものを指令速度と
して採用する。

【００８３】例えば、縦軸をロボット先端速度と横軸を
時間とする速度状態を示す図３において、時刻ｔ＝ｔ１
の場合は、ステップ１０５で算出される加速パターン動
作での速度ｖ２が、最少の速度であるので、これを採用
する。同様に、時刻ｔ＝ｔ２の場合は、ステップ１０４
で算出される一定速度動作での速度ｖ１が、最少の速度
であるので、これを採用する。同様に、時刻ｔ＝ｔ３の
場合は、ステップ１０６で算出される減速パターン動作
での速度ｖ３が、最少の速度であるので、これを採用す
る。同様に、時刻ｔ＝ｔ４の場合は、ロボット先端が特
異領域にあるために、ステップ１０７で算出される許容
限界速度Ｖｌｉｍｉｔが最少の速度であるので、これを
採用する。

【００８４】次のステップ１０９からステップ１１４
は、図１９に示す従来の基本的な制御方法と同様であ
る。つまり、ステップ１０９により現在位置からステッ
プ１０８にて採用された指令速度で移動する補間位置を
算出し、次のステップ１１０により座標変換し、次のス
テップ１１１により各軸を駆動して、ステップ１１２に
より現在位置を更新して、次のステップ１１３により到
着判定か否かを判定し、到着していればステップ１１４
により終了する一方、未到着であれば図１９に示す場合
と同様に残距離更新処理を行なうようにする。

【００８５】次に、ステップ１０７で用いた許容限界速
度を求めるための数１４式の詳細を、図面を参照して説
明する。図４は、この実施の形態１が制御対象とする平
面内２自由度のスカラロボットの機構図である。図４の
スカラ型ロボットにおいて、ロボット先端位置を（Ｘ，
Ｙ）、関節角度（θ１、θ２）とした場合の先端位置の
速度と関節角速度との関係は、ヤコビ行列Ｊによって、
次の数１５式のように表現できる。

【数１５】

【００８６】ここで、Ｊは、下記の数１６式のように定
義されるヤコビ行列である。

【数１６】

【００８７】従って、先端位置速度から関節角速度は、
逆ヤコビ行列Ｊｒにより、次の数１７式関係が成り立
つ。

【数１７】

【００８８】ここで、Ｊｒは、下記の数１８式のように
定義される逆ヤコビ行列である。

【数１８】

【００８９】上記数１８式は、２元連立方程式である。
この連立方程式の第１式は、第１軸の関節角速度と先端
位置速度の関係を表わしている。ここで、簡単化のため
に座標を図５に示すように、経路の方向に沿うように回
転変換しておくと、Ｘ方向の速度成分のみをもちＹ方向
成分の速度が０となるので、第１軸の関節角速度Ｊ１
は、次の数１９式のようになる。

【数１９】

【００９０】ここで、第１軸の速度過大のエラーを生じ
ない定格としての最高回転角速度をＪ１ｍａｘとし、こ
の時のロボット先端の速度すなわち許容限界速度をＶｌ
ｉｍｉｔとする。

【００９１】両者の関係は、上式から次の数２０式のよ
うに表わせる。

【数２０】

【００９２】これをＶｌｉｍｉｔについて整理すると、
次の数２１式のようになる。

【数２１】

【００９３】従って、この実施の形態１によれば、ステ
ップ１０７にて許容限界速度を求め、ステップ１０８に
て従来の方法にて決定される速度と比較し、従来方法で
は関節の定格最大角速度を越えてしまうロボット先端で
の直線補間動作の速度指令が生成される場合に、関節の
定格最大角速度にて動作しうるロボット先端での直線補
間動作速度すなわち許容限界速度を、従来手法にて決定
される速度指令に代えるので、ロボット先端が原点近傍
を直線補間動作にて通過する場合において、関節に定格
最大角速度を越えた指令が生成されることを回避し、な
おかつ、関節速度過大が発生しない最大の速度で移動す
ることができる。

【００９４】ここで、図６に示す縦軸をロボット先端の
速度ならびに第１関節軸の回転角速度をとり、横軸に時
刻をとった速度状態図で説明する。図において、台形状
で高原部分に凹部を有する線図Ｂ３が、ロボット先端の
速度を表わし、凸型の線図Ｂ４が第１関節の回転角速度
を表わし、線図Ｂ５が図２のステップ１０７で求めた許
容限界速度を示している。なお、線図Ｂ２は従来技術に
よる第１軸の角速度指令を示している。

【００９５】具体的に説明すると、ロボット先端の進行
に伴い第１関節が動作し、徐々に回転角速度が増加する
と、図６においてｔ＝ｔ０１の時刻に第１軸が定格最大
角速度に達する。この時、ステップ１０７で求められた
許容限界速度が、指定速度Ｖ１より小さくなり、許容限
界速度がステップ１０８にて採用されるので、第１軸の
角速度の線図Ｂ４は、Ｊ１ｍａｘで高原状となる山形の
線図になる。ロボット先端の速度は、原点に近づくにつ
れて小さくなり、原点にもっとも接近したときに、ロボ
ットの先端の速度は最少となる。そして、原点から離れ
るにつれて大きくなり、線図Ｂ５に示す許容限界速度と
指定速度Ｖ１とが等しくなるｔ＝ｔ０２で、再び従来の
技術による軌跡制御方法に移り変わる。このため、ロボ
ット先端の速度線図Ｂ３は、高原部分に凹部を有するよ
うになる。このようにして、第１関節への指令角速度
が、定格最大角速度Ｊ１ｍａｘを超過することが回避さ
れるので、速度過大エラーの発生を回避し運転を続行す
ることができる。

【００９６】また、この実施の形態１においては、ステ
ップ１０７にて許容限界速度を求め、ステップ１０８に
て従来の方法にて決定される速度と比較し、従来方法で
は関節の定格最大角速度を越えてしまうロボット先端で
の直線補間動作の速度指令が生成される場合に、関節の
定格最大角速度にて動作しうるロボット先端での直線補
間動作速度すなわち許容限界速度を、従来手法にて決定
される速度指令に代えるので、計算負荷の大きいステッ
プ１１０での座標変換の計算結果を破棄しなければなら
ないフィードバック技術を用いることなく、原点近傍を
直線補間動作にて通過する場合に、関節軸の定格最大角
速度を超過しないロボット先端の最大速度を求めること
ができる。

【００９７】また、この実施の形態１においては、ステ
ップ１０７にて許容限界速度を求め、ステップ１０８に
て従来の方法にて決定される速度と比較し、従来方法で
は関節の定格最大角速度を越えてしまうロボット先端で
の直線補間動作の速度指令が生成される場合に、関節の
定格最大角速度にて動作しうるロボット先端での直線補
間動作速度すなわち許容限界速度を、従来手法にて決定
される速度指令に代えるので、制御的に不安定となる場
合が生じるフィードフォワード技術を用いることなく、
原点近傍を直線補間動作にて通過する場合に、関節軸の
定格最大角速度を超過しないロボット先端の最大速度を
求めることができる。

【００９８】実施の形態２．なお、上記実施の形態１で
は、平面内２自由度のスカラロボットを制御の対象とし
たが、この実施の形態２では、平面内２自由度のスカラ
ロボットのうち、先端姿勢軸を有する３自由度スカラロ
ボットを制御対象として説明する。図７に、この実施の
形態２が制御対象とする先端姿勢軸を有する３自由度ス
カラロボットの機構を示す。図において、Ａ１は第１
軸、Ａ２は第２軸、Ａ３は先端姿勢軸である第３軸、Ｋ
１は第１関節、Ｋ２は第２関節、Ｋ３は第３関節であ
る。

【００９９】次に、この実施の形態２の動作を図８のフ
ローチャートを参照しながら説明する。図８に、この実
施の形態２の制御プログラムによって実行されるロボッ
トの速度演算方法を示すフローチャートである。なお、
図１９に示す従来の基本的な制御方法と同様なステップ
には同一ステップ番号を付してその説明は省略するもの
とする。まず、ステップ１０１からステップ１０７まで
の処理は、図２に示す実施の形態１の処理と同様であ
る。

【０１００】そして、次のステップ１１９により、この
実施の形態２では、先端姿勢軸の寄与を次の数２２式に
よって算出する。なお、先端姿勢軸の寄与を算出するこ
の数２２式の詳細は後述することにする。

【数２２】ここで、上記数２２式中、各記号の内容は下記の通りで
ある。Ｄ：現在位置から目的位置までの移動残距離Ｐrca：現在の姿勢角度から目的位置の姿勢角度までの
移動残角度

【０１０１】次いで、次のステップ１２０により、下記
の数２３式により、ステップ１０７で求めた許容限界速
度にステップ１１９で求めた先端姿勢軸の寄与を乗じ
て、許容限界速度を修正する。

【数２３】

【０１０２】その後、次のステップ１０８からステップ
１１４までの処理は、図２に示す実施の形態１の場合と
同様である。

【０１０３】次に、ステップ１１９で用いた先端姿勢軸
の寄与を算出する数２２式の詳細を説明する。まず、図
４に示すスカラ型ロボットにおいては、ロボット先端位
置を（Ｘ，Ｙ，Ａ）（以下、Ａは先端姿勢軸の姿勢角度
とする。）、関節角度（θ１，θ２，θ３）とした場合
の先端位置の速度と関節角速度との関係は、ヤコビ行列
Ｊによって、次の数２４式のように表現できる。

【数２４】ここで、Ｊは、下記の数２５式のように定義されるヤコ
ビ行列である。

【数２５】

【０１０４】従って、先端位置速度から関節角速度は、
逆ヤコビ行列Ｊｒにより、次の数２６式の関係が成り立
つ。

【数２６】

【０１０５】ここで、Ｊｒは、下記の数２７式のように
定義される逆ヤコビ行列である。

【数２７】

【０１０６】上記数２７式は、３元連立方程式である。
この連立方程式の第１式は、第１軸の関節角速度と先端
位置速度の関係を表わしている。ここで、簡単化のため
に座標を図５に示すように、経路の方向に沿うように回
転変換してあるので、Ｘ方向の速度成分のみをもちＹ方
向成分の速度が０となり、第１軸の関節角速度Ｊ１は、
次の数２８式となる。

【数２８】

【０１０７】ここで、第１軸の速度過大のエラーを生じ
ない定格としての最高回転角速度をＪ１ｍａｘとし、こ
の時のロボット先端の速度すなわち許容限界速度をＶｌ
ｉｍｉｔとする。

【０１０８】Ｊ１ｍａｘと許容限界速度の関係は、上記
数２８式から次の数２９式のように表わせる。

【数２９】ここで、上記数２９式中の各記号は、下記の通りである Vｌｉｍｉｔ：許容限界速度Ｊ１ｍａｘ：第１軸最大角速度ｄＡ／ｄｔ：先端姿勢軸の角速度Ｌ１：第１軸Ａ１の長さＬ３：ツール長さ（第３軸Ａ３の長さ）

【０１０９】ここで、上記数２９式の第１項は、実施の
形態１で求めた第２軸Ａ２先端での許容限界速度を表わ
している。また、その第２項は、第３軸Ａ３である先端
姿勢軸、すなわちツール軸の動作による補正項である。

【０１１０】また、先端姿勢軸の角速度ｄＡ／ｄｔは、
ロボット先端位置の並進速度と独立に定まるものではな
く、並進移動の終了と姿勢軸の動作の終了とが一致しな
ければならない。すなわち、並進速度と姿勢角速度の比
率は、並進移動残距離と姿勢軸移動残角度の比率に等し
くなる。そこで、数３０式に示すように、並進移動残距
離と姿勢軸移動残角度の比率を並進移動速度Ｖｌｉｍｉ
ｔに乗ずることにより、先端姿勢軸の角速度ｄＡ／ｄｔ
が求められる。

【数３０】ここで、Ｚは、下記の数３１式の通りであり、並進移動
の残距離Ｄに対する姿勢軸の残移動角度Ｐｒｃａの比率
を表わしている。この値が１を超えた場合は、並進移動
が移動全体を支配し、逆に１未満の場合は、ツール姿勢
移動が全体の移動を支配する。

【数３１】

【０１１１】先端姿勢軸の角速度ｄＡ／ｄｔの決定式を
Ｖｌｉｍｉｔ式に代入整理して、先端姿勢軸の角速度ｄ
Ａ／ｄｔを消去すると、次の数３２式となる。

【数３２】

【０１１２】上記数３２式の分子は、実施の形態１で求
めた姿勢軸を有していないロボットにおける許容限界速
度であり、分母は姿勢軸による寄与の逆数を示してい
る。この姿勢軸による寄与をＭとすると、姿勢軸による
寄与Ｍは次の数３３式のように表すことができる。

【数３３】このため、この姿勢軸の寄与の逆数Ｍを、実施の形態１
で求めた姿勢軸を有していないロボットにおける許容限
界速度に乗じることで、姿勢軸の影響を考慮した許容限
界速度をもとめることができる。

【０１１３】従って、この実施の形態２によれば、ステ
ップ１１９により姿勢軸の寄与Ｍを求め、ステップ１２
０により先端軸を有していないロボットにおける許容限
界速度に乗じることにより、姿勢軸の寄与を考慮した許
容限界速度を求めることができる。

【０１１４】実施の形態３．この実施の形態３は、先端
姿勢軸を有する水平面内２自由度円筒座標ロボットを制
御対象としたものである。図９に、この実施の形態３の
制御対象となる先端姿勢軸を有する水平面内２自由度円
筒座標ロボットを示す。図において、Ａ４は円筒軸、Ａ
５は円筒軸Ａ４から伸長自在な先端姿勢軸、Ｋ１は第１
関節である。

【０１１５】次に、先端姿勢軸を有する水平面内２自由
度円筒座標ロボットを制御対象としたこの実施の形態３
の動作を説明するが、この実施の形態３の動作は図２に
示す上記実施の形態１の動作と同じで、許容限界速度を
求める式が異なるのだけであるので、上記実施の形態１
の動作を示す図２のフローチャートを参照しながら説明
するものとする。まず、この実施の形態３でも、ステッ
プ１０１からステップ１１８までの処理は、上記実施の
形態１の場合と同様である。

【０１１６】そして、次のステップ１０７により、この
先端姿勢軸を有する水平面内２自由度円筒座標ロボット
の許容限界速度を下記の数３４式により算出する。

【数３４】

【０１１７】次に、許容限界速度を算出するこの数３４
式の詳細を説明する。図９に示す先端姿勢軸を有する水
平面内２自由度円筒座標ロボットにおいて、ロボット先
端位置を（Ｘ，Ｙ）、関節軸座標（θ２）とした場合の
先端位置の速度と関節角速度との関係は、ヤコビ行列Ｊ
によって、次の数３５式のように表現できる。

【数３５】

【０１１８】ここで、Ｊは、下記の数３６式のように定
義されるヤコビ行列である。

【数３６】

【０１１９】従って、先端位置速度から関節角速度は、
逆ヤコビ行列Ｊｒにより、次の数３７式の関係が成り立
つ。

【数３７】

【０１２０】この数３７式は、２元連立方程式である。
この２元連立方程式の第１式は、第１軸の関節角速度と
先端位置速度の関係を表わしている。ここで、簡単化の
ために座標を図５に示すように、経路の方向に沿うよう
に回転変換しておくと、Ｘ方向の速度成分のみをもちＹ
方向成分の速度が０となるので、第１軸の関節角速度Ｊ
１は、次の数３８式となる。

【数３８】

【０１２１】ここで、第１軸の速度過大のエラーを生じ
ない定格としての最高回転角速度をＪ１ｍａｘとし、こ
の時のロボット先端の速度すなわち許容限界速度をＶｌ
ｉｍｉｔとする。

【０１２２】両者の関係は、上記数３８式から次の数３
９式のように表わすことができる。

【数３９】

【０１２３】これをＶｌｉｍｉｔについて整理すると、
次の数４０式のようになる。

【数４０】

【０１２４】従って、この実施の形態３では、ステップ
１０７にて許容限界速度を求め、ステップ１０８にて従
来の方法にて決定される速度と比較し、従来方法では関
節の定格最大角速度を越えてしまうロボット先端での直
線補間動作の速度指令が生成される場合に、関節の定格
最大角速度にて動作しうるロボット先端での直線補間動
作速度すなわち許容限界速度を、従来手法にて決定され
る速度指令に代えるので、ロボット先端が原点近傍を直
線補間動作にて通過する場合において、関節に定格最大
角速度を越えた指令が生成されることを回避し、なおか
つ、関節速度過大が発生しない最大の速度で移動するこ
とができる。

【０１２５】また、この実施の形態３においては、ステ
ップ１０７にて許容限界速度を求め、ステップ１０８に
て従来の方法にて決定される速度と比較し、従来方法で
は関節の定格最大角速度を越えてしまうロボット先端で
の直線補間動作の速度指令が生成される場合に、関節の
定格最大角速度にて動作しうるロボット先端での直線補
間動作速度すなわち許容限界速度を、従来手法にて決定
される速度指令に代えるので、計算負荷の大きいステッ
プ１１０での座標変換の計算結果を破棄しなければなら
ないフィードバック技術を用いることなく、原点近傍を
直線補間動作にて通過する場合に、関節軸の定格最大角
速度を超過しないロボット先端の最大速度を求めること
ができる。

【０１２６】また、この実施の形態３においては、ステ
ップ１０７にて許容限界速度を求め、ステップ１０８に
て従来の方法にて決定される速度と比較し、従来方法で
は関節の定格最大角速度を越えてしまうロボット先端で
の直線補間動作の速度指令が生成される場合に、関節の
定格最大角速度にて動作しうるロボット先端での直線補
間動作速度すなわち許容限界速度を、従来手法にて決定
される速度指令に代えるので、制御的に不安定となる場
合が生じるフィードフォワード技術を用いることなく、
原点近傍を直線補間動作にて通過する場合に、関節軸の
定格最大角速度を超過しないロボット先端の最大速度を
求めることができる。

【０１２７】実施の形態４．次に、この発明の実施の形
態４を説明する。この実施の形態４は、垂直多関節ロボ
ットを制御対象としたものである。図１０（ａ）に、こ
の実施の形態４が対象とする垂直多関節ロボットの構成
を示す。図において、Ａ１は第１アーム、Ａ２は第２ア
ーム、Ｋ４は第１関節（旋回軸）、Ｋ５は第２関節（シ
ョルダー軸）、Ｋ６は第３関節（エルボー軸）である。

【０１２８】次に、このように構成された実施の形態４
の動作を説明する。図１１に、図１０に示す垂直多関節
ロボットを制御対象としたこの実施の形態４の動作を示
す。まず、この実施の形態４のステップ１０１からステ
ップ１０６までの処理は、図８に示す実施の形態３の場
合と同様である。

【０１２９】そして、この実施の形態４では、次のステ
ップ１２１において、下記式により第１アームＡ１およ
び第２アームＡ２の水平面内への投影長Ｌを次の数４１
式により算出する。

【数４１】ここで、上記４１式中、各記号は下記の通りである。Ｌ１：第１アームＡ１の長さＬ２：第２アームＡ２の長さ θ２：第２関節（ショルダー軸）Ｋ５の関節角度 θ３：第３関節(エルボー軸)Ｋ６の関節角度このため、図１０（ａ）に示す垂直多関節ロボットの水
平面への投影は、図１０（ｂ）に示す投影アーム長さＬ
と第１関節（旋回軸）Ｋ４の角度θ１で定義される実施
の形態３の円筒座標ロボットと同等になる。

【０１３０】尚、その後のステップ１１８からステップ
１１４までの処理は、図８に示す実施の形態３と同様で
ある。

【０１３１】従って、この実施の形態４によれば、図１
０（ａ）に示す垂直多関節ロボットを水平面へ投影し
て、図１０（ｂ）に示すように図９に示す実施の形態３
の円筒座標ロボットと同等として考えることにより、自
由度を縮退させることにより演算の取り扱いを簡便にす
ることができる。

【０１３２】実施の形態５．上記実施の形態１〜４にお
いては、現在位置について、ロボット先端を全体座標で
管理するに加えて、関節角度も管理されている速度演算
装置もしくは速度演算方法を対象として、原点近傍を直
線補間動作にて通過することを可能とする軌跡制御方法
を実施したものであった。しかし、機能の限定された廉
価なロボットにおいては、コストダウン等を目的とし
て、記憶容量の節約や計算能力の小さなＣＰＵを用い
る。この場合には、現在位置における関節情報を保持し
えない速度演算装置もしくは速度演算方法が採用され
る。そこで、現在位置における関節情報を用いずに、原
点近傍を直線補間動作にて通過することを可能とする水
平面内２自由度スカラロボットの軌跡制御方法の実施の
形態を説明する。

【０１３３】図４に示す水平面内２自由度スカラロボッ
トにおいて、許容限界速度の等高線は図１２に示すよう
に歪んだ双子楕円である。また図１３に示すようにこの
双子歪み楕円に外接する双子正方形すなわち辺長比が
１：２の長方形にて等高線を近似する。この場合、二つ
の正方形は、経路の方向と平行となっている座標軸を対
象軸とする。これによって、ロボット先端の許容限界速
度は、経路の方向と座標軸が平行となるように回転変換
を施した座標値によって、近似等高線を検索し、簡便に
許容限界速度を決定することができる。

【０１３４】歪んだ楕円となる許容限界速度の等高線を
それに外接する正方形に近似しているので、ロボットの
先端位置を回転した座標（Ｘ，Ｙ）のＸ座標とＹ座標の
絶対値の何れか小さい値を辺長とする正方形が近似され
た許容限界速度の等高線となる。

【０１３５】このため、予めこの正方形の辺長と許容限
界速度の一次元対応表を作り、ロボットの先端位置の回
転された座標（Ｘ，Ｙ）のＸ座標とＹ座標の絶対値の何
れか小さい値をインデックスとしてこの表を検索するこ
とにより、許容限界速度を求めることが可能となる。こ
の検索演算は、sin演算、cos演算などの三角関数演算と
比較して大小比較演算のみで可能なので、計算の負荷が
小さくなり、簡便化と高速化を図ることができる。

【０１３６】具体的に、図１４のフローチャートを参照
して説明する

【０１３７】まず、ステップ１０１からステップ１０６
までの処理は、図２に示す実施の形態１の場合と同様で
ある。

【０１３８】そして、この実施の形態５では、次のステ
ップ１２２により、図１５に示すように座標を角度Ψだ
け回転させて、Ｘ軸が経路方向と平行になるようにす
る。

【０１３９】そして、次のステップ１２３により、回転
した座標におけるＸ座標値の絶対値とＹ座標値の絶対値
の何れか小さい値を選択する。

【０１４０】さらに、次のステップ１２４により、予め
用意した近似許容限界速度の等高線の正方形辺長＝許容
限界速度の表１（表１の詳細については後述する。）に
対して、ステップ１２３で選択した値を指標として許容
限界速度を検索して算出する。

【０１４１】ステップ１０８からステップ１１４は、上
記実施の形態１と同様である。

【０１４２】次に、ステップ１２４で用いた近似許容限
界速度の等高線の正方形辺長＝許容限界速度の表１の詳
細を説明する。この表１は、ロボットアームの長さと関
節軸の定格最大角速度が決まると一意に定まるため、軌
跡生成の度に生成する必要はないので、ロボットの速度
演算装置のＣＰＵで計算する必要はなく、別個の計算機
で計算し、工場出荷段階で不揮発性メモリなどに書き込
んでおくものである。

【０１４３】図１３に示す歪んだ楕円となる許容限界速
度の等高線は、関節角度θ１、θ２を助変数として、次
の数４２〜４４式で表現される。

【数４２】

【数４３】

【数４４】

【０１４４】注目する許容限界速度Ｖｌｉｍｉｔに関し
て、まず、ｘの絶対値とｙの絶対値の最大値を求める。
実際には上式をｘ，ｙに関して陽の形式に解くことは困
難なので、最初にｘの値を逐次変化させながらｙ方向に
探索を行ない、次にｙの値を逐次変化させながらｘ方向
の探索を行なう。そして、この両者から最大のものを、
該当する許容限界速度に対応する近似許容限界速度の等
高線の正方形辺長として採用する。この操作を許容限界
速度Ｖｌｉｍｉｔを逐次変更して行なう。

【０１４５】例えば、近似許容限界速度の等高線の正方
形辺長＝許容限界速度の表は、アーム長さＬ１＝Ｌ２＝
３２０ｍｍで、各々の関節軸の定格最大角速度が４．６
ｒａｄ／ｓの場合は、下記の表１のようになる。

【表１】

【０１４６】なお、上記実施の形態５では、水平面内２
自由度スカラロボットに利用した場合について述べた
が、ステップ１２４で用いる許容限界速度表を求める式
において水平面内２自由度円筒座標ロボットに対して
は、下記の数４５〜４７式を用いることにより同様にし
て、ロボット先端位置から許容限界速度を求める表を作
成することが可能である。この場合のロボット先端にお
ける許容限界速度の等高線は、図１６に示すようにＹ軸
上に中心を持ち、原点にて接しているＸ軸を対称軸とし
た双子円となっている。この等高線に外接する辺比１：
２の長方形、即ち各々の円に外接する正方形を近似され
た許容限界速度の等高線として求めるものである。

【数４５】

【数４６】

【数４７】これを、まとめると下記の数４８式のようになる

【数４８】ただし、上記数４８式中、各記号は下記の通りである。Ｖｌｉｍｉｔ：許容限界速度Ｊｍａｘ：旋回軸定格最大角速度Ｌ：伸縮軸長 θ：旋回軸角度

【０１４７】さらに、垂直多関節ロボットに対しては、
水平面内にアームを投影した長さを水平面内２自由度円
筒座標ロボットの伸縮軸長さとして扱うことで同様にス
テップ１２４で用いる許容限界速度表を作ることが可能
になる。

【０１４８】実施の形態６．上記実施の形態１〜５にお
いては、本発明に係るロボットの速度演算装置をロボッ
ト本体を実際に制御するロボットの軌跡制御装置に採用
して説明した。しかし、本発明に係るロボットの速度演
算装置は、ロボット本体を実際に制御するロボットの軌
跡制御装置のみでなく、ロボットの動作を画面上でシミ
ュレーションするロボットのシミュレーション装置につ
いても適応可能であるので、この実施の形態６では、本
発明に係るロボットの速度演算装置をロボットのシミュ
レーション装置に適用して説明する。

【０１４９】図１７に、本発明に係るロボットの速度演
算装置を採用したロボットのシミュレーション装置の電
気的構成図を示す。図において、３１はロボットのシミ
ュレーションを行なうＣＰＵ、３２はＣＰＵ３１にシミ
ュレーション動作を行わせるためのプログラムが格納さ
れたメモリ、３３はＣＰＵ３１やメモリ３２と表示装置
３５および操作盤３４とのインターフェイスである。

【０１５０】次に、この実施の形態６の動作を図面を参
照して説明する。図１８に、メモリ３２に記憶されたプ
ログラムによって実行されるロボットの速度演算方法を
示す。

【０１５１】図において、まず、ステップ１０１から１
１０までの処理は、図２に示す上記実施の形態１の場合
と同じである。

【０１５２】そして、次のステップ１１２において、現
在位置を更新すると同時に、インターフェイス３３を通
じて、シミュレートされた現在の位置（姿勢を含む）を
表示装置３５に表示するその後、ステップ１１３からス
テップ１１４までの処理は、図４に示す実施の形態１の
場合と同じである。

【０１５３】従って、この実施の形態６によれば、座標
変換計算の結果を放棄する必要のあるフィードバック技
術や、制御的に不安定を生じる恐れの在るフィードフォ
ワード技術を用いることなく、関節軸の定格最大角速度
を超過を生じることなく、原点近傍を直線補間動作にて
通過できるようにロボットの軌跡をシミュレーションす
ることが可能になる。

【０１５４】

【発明の効果】以上説明したように、本発明では、ロボ
ットの現在位置情報を取得する手段と、ロボットの関節
の定格最高速度情報を取得する手段と、上記ロボット現
在位置情報と上記関節定格最高速度とに基づいてロボッ
ト先端の直線補間動作において取り得る許容限界速度を
算出する手段と、を設けたので、座標変換計算の結果を
放棄する必要のあるフィードバック技術を用いることな
く、同時に制御的に不安定を生じる恐れの在るフィード
フォワード技術を用いることなく、原点近傍を直線補間
動作にて通過する場合に、関節軸の定格最大角速度を超
過しないロボット先端の最大速度を求めることができ
る。

【０１５５】また、次の発明では、ロボットの現在関節
情報を取得する手段と、ロボットの関節の定格最高速度
情報を取得する手段と、上記現在関節情報と上記関節定
格最高速度情報とに基づいてロボット先端の直線補間動
作において取り得る許容限界速度を算出する手段と、を
設けたので、座標変換計算の結果を放棄する必要のある
フィードバック技術を用いることなく、同時に制御的に
不安定を生じる恐れの在るフィードフォワード技術を用
いることなく、原点近傍を直線補間動作にて通過する場
合に、関節軸の定格最大角速度を超過しないロボット先
端の最大速度を求めることができる。

【０１５６】また、次の発明では、ロボットはスカラロ
ボットであり、ロボットの現在関節情報として上記スカ
ラロボットの現在関節角度情報を取得し、ロボットの関
節の定格最高速度情報として上記スカラロボットの関節
の定格最高角速度情報を取得するようにしたので、座標
変換計算の結果を放棄する必要のあるフィードバック技
術を用いることなく、同時に制御的に不安定を生じる恐
れの在るフィードフォワード技術を用いることなく、原
点近傍を直線補間動作にて通過する場合に、関節軸の定
格最大角速度を超過しないスカラロボット先端の最大速
度を求めることができる。

【０１５７】また、次の発明では、スカラロボットは水
平面内２自由度スカラロボットであり、スカラロボット
の現在関節角度情報として上記水平面内２自由度スカラ
ロボットの現在関節角度情報を取得し、スカラロボット
の関節の定格最高角速度情報として上記平面内２自由度
スカラロボットの第一関節の定格最高角速度を取得する
ようにしたので、座標変換計算の結果を放棄する必要の
あるフィードバック技術を用いることなく、同時に制御
的に不安定を生じる恐れの在るフィードフォワード技術
を用いることなく、原点近傍を直線補間動作にて通過す
る場合に、関節軸の定格最大角速度を超過しない水平面
内２自由度スカラロボット先端の最大速度を求めること
ができる。

【０１５８】また、次の発明では、水平面内２自由度ス
カラロボットは先端姿勢軸を有する３自由度スカラロボ
ットであり、上記先端姿勢軸を有する３自由度スカラロ
ボットの現在位置における直線移動残距離情報を取得す
る手段と、上記先端姿勢軸を有する３自由度スカラロボ
ットの現在位置における先端姿勢軸移動残角度情報を取
得する手段と、上記直線移動残距離情報と上記先端姿勢
軸移動残角度情報と上記先端姿勢軸を有する３自由度ス
カラロボットの現在関節角度情報を入力して先端姿勢軸
による速度寄与補正係数を演算する手段と、当該先端姿
勢軸を有する３自由度スカラロボットの現在関節角度情
報とその第一関節の定格最高角速度とに基づいて算出さ
れた当該先端姿勢軸を有する３自由度スカラロボット先
端の直線補間動作において取り得る許容限界速度に上記
速度寄与補正係数を乗じる手段と、をさらに設けたの
で、座標変換計算の結果を放棄する必要のあるフィード
バック技術を用いることなく、同時に制御的に不安定を
生じる恐れの在るフィードフォワード技術を用いること
なく、原点近傍を直線補間動作にて通過する場合に、先
端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を
超過しない先端姿勢軸を有する３自由度スカラロボット
先端の最大速度を求めることができる。

【０１５９】また、次の発明では、ロボットは水平面内
２自由度円筒座標ロボットであり、ロボットの現在関節
情報として上記水平面内２自由度円筒座標ロボットの現
在関節情報を取得し、ロボットの定格最高速度情報とし
て上記水平面内２自由度円筒座標ロボットの旋回軸の定
格最高角速度を取得し、上記上記水平面内２自由度円筒
座標ロボットの現在関節情報と旋回軸の定格最高角速度
とに基づいて上記水平面内２自由度円筒座標ロボット先
端の直線補間動作において取り得る許容限界速度を算出
するようにしたので、座標変換計算の結果を放棄する必
要のあるフィードバック技術を用いることなく、同時に
制御的に不安定を生じる恐れの在るフィードフォワード
技術を用いることなく、原点近傍を直線補間動作にて通
過する場合に、関節軸の定格最大角速度を超過しない水
平面内２自由度円筒座標ロボット先端の最大速度を求め
ることができる。

【０１６０】また、次の発明では、水平面内２自由度円
筒座標ロボットは先端姿勢軸を有する３自由度円筒座標
ロボットであり、上記先端姿勢軸を有する３自由度円筒
座標ロボットの現在位置における直線移動残距離情報を
取得する手段と、上記先端姿勢軸を有する３自由度円筒
座標ロボットの現在位置における先端姿勢軸移動残角度
情報を取得する手段と、上記直線移動残距離情報と上記
先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３
自由度円筒座標ロボットの現在関節角度情報とに基づい
て上記先端姿勢軸による速度寄与補正係数を演算する手
段と、上記先端姿勢軸を有する３自由度円筒座標ロボッ
トの現在関節情報と旋回軸の定格最高角速度とに基づい
て算出した上記先端姿勢軸を有する３自由度円筒座標ロ
ボット先端の直線補間動作において取り得る許容限界速
度に上記速度寄与補正係数を乗じる手段と、を設けたの
で、座標変換計算の結果を放棄する必要のあるフィード
バック技術を用いることなく、同時に制御的に不安定を
生じる恐れの在るフィードフォワード技術を用いること
なく、原点近傍を直線補間動作にて通過する場合に、先
端姿勢軸の寄与を考慮して、関節軸の定格最大角速度を
超過しない先端姿勢軸を有する３自由度円筒座標ロボッ
ト先端の最大速度を求めることができる。

【０１６１】また、次の発明では、水平面内２自由度円
筒座標ロボットは垂直多関節ロボットであり、上記垂直
多関節ロボットの水平面へ投影して、上記垂直多関節ロ
ボットの第１アームおよび第２アームの水平面への投影
長を上記垂直多関節ロボットのアーム長として上記垂直
多関節ロボットの現在関節情報と旋回軸の定格最高角速
度とに基づいて上記垂直多関節ロボット先端の直線補間
動作において取り得る許容限界速度を算出するようにし
たので、座標変換計算の結果を放棄する必要のあるフィ
ードバック技術を用いることなく、同時に制御的に不安
定を生じる恐れの在るフィードフォワード技術を用いる
ことなく、原点近傍を直線補間動作にて通過する場合
に、関節軸の定格最大角速度を超過しない垂直多関節ロ
ボット先端の最大速度を求めることができる。

【０１６２】また、次の発明では、算出されたロボット
先端の直線補間動作において取り得る許容限界速度の等
高線を記憶する手段と、ロボットの現在位置情報を取得
する手段と、上記ロボットの現在位置情報に基づいて上
記予め記憶された許容限界速度の等高線から現在位置に
おける直線補間動作において取り得る許容限界速度を検
索する手段と、をさらに設けたので、直線補間動作を行
う前にロボット先端の位置と直線補間動作における取り
得る速度の関係を計算し記憶することにより、直線補間
動作のサンプリング中は三角関数などからなる演算を行
うことなく、計算機にとって計算負荷の小さい比較演算
を主とした検索処理によって、原点近傍を直線補間動作
にて通過する場合の関節軸の定格最大角速度を超過しな
いロボット先端の最大速度を求めることができる。

【０１６３】また、次の発明では、算出されたロボット
先端の直線補間動作において取り得る許容限界速度の等
高線を多角形、円もしくは楕円等に近似して記憶する手
段と、ロボットの現在位置情報を取得する手段と、上記
ロボットの現在位置情報に基づいて上記予め記憶された
近似された許容限界速度の等高線から現在位置における
直線補間動作において取り得る許容限界速度を検索する
手段と、をさらに設けたので、直線補間動作開始前に生
成される等高線情報の量を小さくすることができ、同時
に直線補間動作のサンプリング中に少ない比較演算にて
ロボット先端の最大速度を検索することができるので、
演算能力の小さい装置においても、原点近傍を直線補間
動作にて通過する場合に、原点近傍を直線補間動作にて
通過する場合に、関節軸の定格最大角速度を超過しない
ロボット先端の最大速度を求めることができる。

【０１６４】また、次の発明では、算出されたスカラロ
ボット先端の直線補間動作において取り得る許容限界速
度の等高線を正方形に近似してその辺長を記憶する手段
と、上記スカラロボットの現在位置情報を取得する手段
と、上記スカラロボットの現在位置情報に基づいて上記
予め記憶された辺長から現在位置における直線補間動作
において取り得る許容限界速度を検索する手段と、をさ
らに設けたので、直線補間動作開始前に生成される等高
線情報の量を小さくすることができ、同時に直線補間動
作のサンプリング中に少ない比較演算にてロボット先端
の最大速度を検索することができるので、演算能力の小
さい装置においても、原点近傍を直線補間動作にて通過
する場合に、関節軸の定格最大角速度を超過しないスカ
ラロボット先端の最大速度を求めることができる。

【０１６５】また、次の発明では、算出されたスカラロ
ボット先端の直線補間動作において取り得る許容限界速
度の等高線を長方形に近似して原点から長方形の各辺ま
での距離を記憶する手段と、上記スカラロボットの現在
位置情報を取得する手段と、上記スカラロボットの現在
位置情報に基づいて上記予め記憶された距離から現在位
置における直線補間動作において取り得る許容限界速度
を検索する手段と、をさらに設けたので、直線補間動作
開始前に生成される等高線情報の量を小さくすることが
でき、同時に直線補間動作のサンプリング中に少ない比
較演算にてロボット先端の最大速度を検索することがで
きるので、演算能力の小さい装置においても、原点近傍
を直線補間動作にて通過する場合に、関節軸の定格最大
角速度を超過しないスカラロボット先端の最大速度を求
めることができる。

【０１６６】また、次の発明では、カラロボットは先端
姿勢軸を有する３自由度スカラロボットであり、上記先
端姿勢軸を有する３自由度スカラロボットの現在位置に
おける直線移動残距離情報を取得する手段と、上記先端
姿勢軸を有する３自由度スカラロボットの現在位置にお
ける先端姿勢軸移動残角度情報を取得する手段と、上記
直線移動残距離情報と上記先端姿勢軸移動残角度情報と
上記先端姿勢軸を有する３自由度スカラロボットの現在
関節角度情報に基づいて上記先端姿勢軸による速度寄与
補正係数を演算する手段と、検索された許容限界速度に
上記速度寄与補正係数を乗じる手段と、をさらに設けた
ので、先端姿勢軸の寄与を考慮して、原点近傍を直線補
間動作にて通過する場合に、関節軸の定格最大角速度を
超過しないスカラロボット先端の最大速度を求めること
ができる。

【０１６７】また、次の発明では、算出された水平面内
２自由度円筒座標ロボット先端の直線補間動作において
取り得る許容限界速度の等高線を入力して得られる複数
の円からなる速度の等高線の半径を記憶する手段と、上
記水平面内２自由度円筒座標ロボットの現在位置情報を
取得する手段と、上記水平面内２自由度円筒座標ロボッ
トの現在位置情報に基づいて上記予め記憶された半径か
ら現在位置における直線補間動作において取り得る許容
限界速度を検索する手段と、をさらに設けたので、直線
補間動作開始前に生成される等高線情報の量を小さくす
ることができ、同時に直線補間動作のサンプリング中に
少ない比較演算にてロボット先端の最大速度を検索する
ことができるので、演算能力の小さい装置においても、
原点近傍を直線補間動作にて通過する場合に、関節軸の
定格最大角速度を超過しない水平面内２自由度円筒座標
ロボット先端の最大速度を求めることができる。

【０１６８】また、次の発明では、水平面内２自由度円
筒座標ロボットは先端姿勢軸を有する３自由度円筒座標
ロボットであり、上記先端姿勢軸を有する３自由度円筒
座標ロボットの現在位置における直線移動残距離情報を
取得する手段と、上記先端姿勢軸を有する３自由度円筒
座標ロボットの現在位置における先端姿勢軸移動残角度
情報を取得する手段と、上記直線移動残距離情報と上記
先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３
自由度円筒座標ロボットの現在関節角度情報とに基づい
て上記先端姿勢軸による速度寄与補正係数を演算する手
段と、検索された許容限界速度に上記速度寄与補正係数
を乗じる手段と、をさらに設けたので、原点近傍を直線
補間動作にて通過する場合に、先端姿勢軸の寄与を考慮
して、関節軸の定格最大角速度を超過しない先端姿勢軸
を有する３自由度円筒座標ロボット先端の最大速度を求
めることができる。

【０１６９】また、次の発明では、平面内２自由度円筒
座標ロボットは垂直多関節ロボットであり、上記垂直多
関節ロボット先端の直線補間動作において取り得る許容
限界速度は、上記垂直多関節ロボットの水平面へ投影し
て、上記垂直多関節ロボットの第１アームおよび第２ア
ームの水平面への投影長を上記垂直多関節ロボットのア
ーム長として算出するようにしたので、原点近傍を直線
補間動作にて通過する場合に、関節軸の定格最大角速度
を超過しない垂直多関節ロボット先端の最大速度を求め
ることができる。

【０１７０】また、次の発明では、ロボットの現在位置
における進路方向を取得する手段と、上記ロボットの現
在位置における進路方向に基づいてその進路方向と平行
な方向へ上記ロボットの現在位置の座標を変換する手段
と、をさらに備え、ロボット先端の直線補間動作におい
て取り得る許容限界速度を算出する手段は、上記変換さ
れた上記ロボットの現在位置の座標に基づいて上記許容
限界速度を算出するようにしたので、関節軸の定格最大
角速度を超過しないロボット先端の最大速度の演算を簡
略化することができる。

【０１７１】また、次の発明では、直線補間を行なう際
指定された速度で移動する場合の指定速度を算出する手
段と、直線補間を行なう際指定された加速パターンで移
動する場合の速度を求める手段と、直線補間を行なう際
指定された減速パターンで移動する場合の速度を求める
手段と、上記指定速度、上記加速パターンでの速度、上
記減速パターンでの速度および許容限界速度のうちから
最少の速度を選択する手段と、をさらに設けたので、座
標変換計算の結果を放棄する必要のあるフィードバック
技術や、制御的に不安定を生じる恐れの在るフィードフ
ォワード技術を用いることなく、また関節軸の定格最大
角速度を超過を生じることなく、原点近傍を直線補間動
作にて通過できるようにロボットの軌跡を実際に制御し
たり、ロボットの軌跡をシミュレーションすることがで
きる。

【０１７２】また、次の発明では、直線補間を行なう際
指定された速度で移動する場合の指定速度を算出する手
段と、直線補間を行なう際指定された加速パターンで移
動する場合の速度を求める手段と、直線補間を行なう際
指定された減速パターンで移動する場合の速度を求める
手段と、上記指定速度、上記加速パターンでの速度、上
記減速パターンでの速度および速度寄与補正係数が乗じ
られた許容限界速度のうちから最少の速度を選択する手
段と、をさらに設けたので、座標変換計算の結果を放棄
する必要のあるフィードバック技術や、制御的に不安定
を生じる恐れの在るフィードフォワード技術を用いるこ
となく、また関節軸の定格最大角速度を超過を生じるこ
となく、先端姿勢軸の速度寄与を補正して、原点近傍を
直線補間動作にて通過できるようにロボットの軌跡を実
際に制御したり、ロボットの軌跡をシミュレーションす
ることができる。

【０１７３】また、次の発明では、ロボットの現在位置
情報を取得するステップと、ロボットの関節の定格最高
速度情報を取得するステップと、上記ロボット現在位置
情報と上記関節定格最高速度とに基づいてロボット先端
の直線補間動作において取り得る許容限界速度を算出す
るステップと、を設けたので、座標変換計算の結果を放
棄する必要のあるフィードバック技術を用いることな
く、同時に制御的に不安定を生じる恐れの在るフィード
フォワード技術を用いることなく、原点近傍を直線補間
動作にて通過する場合に、関節軸の定格最大角速度を超
過しないロボット先端の最大速度を求めることができ
る。

【０１７４】また、次の発明では、ロボットの現在関節
情報を取得するステップと、ロボットの関節の定格最高
速度情報を取得するステップと、上記現在関節情報と上
記関節定格最高速度情報とに基づいてロボット先端の直
線補間動作において取り得る許容限界速度を算出するス
テップと、を設けたので、座標変換計算の結果を放棄す
る必要のあるフィードバック技術を用いることなく、同
時に制御的に不安定を生じる恐れの在るフィードフォワ
ード技術を用いることなく、原点近傍を直線補間動作に
て通過する場合に、関節軸の定格最大角速度を超過しな
いロボット先端の最大速度を求めることができる。

【０１７５】また、次の発明では、ロボットはスカラロ
ボットであり、ロボットの現在関節情報として上記スカ
ラロボットの現在関節角度情報を取得し、ロボットの関
節の定格最高速度情報として上記スカラロボットの関節
の定格最高角速度情報を取得するようにしたので、座標
変換計算の結果を放棄する必要のあるフィードバック技
術を用いることなく、同時に制御的に不安定を生じる恐
れの在るフィードフォワード技術を用いることなく、原
点近傍を直線補間動作にて通過する場合に、関節軸の定
格最大角速度を超過しないスカラロボット先端の最大速
度を求めることができる。

【０１７６】また、次の発明では、スカラロボットは水
平面内２自由度スカラロボットであり、スカラロボット
の現在関節角度情報として上記水平面内２自由度スカラ
ロボットの現在関節角度情報を取得し、スカラロボット
の関節の定格最高角速度情報として上記平面内２自由度
スカラロボットの第一関節の定格最高角速度を取得する
ようにしたので、座標変換計算の結果を放棄する必要の
あるフィードバック技術を用いることなく、同時に制御
的に不安定を生じる恐れの在るフィードフォワード技術
を用いることなく、原点近傍を直線補間動作にて通過す
る場合に、関節軸の定格最大角速度を超過しない水平面
内２自由度スカラロボット先端の最大速度を求めること
ができる。

【０１７７】また、次の発明では、水平面内２自由度ス
カラロボットは先端姿勢軸を有する３自由度スカラロボ
ットであり、上記先端姿勢軸を有する３自由度スカラロ
ボットの現在位置における直線移動残距離情報を取得す
るステップと、上記先端姿勢軸を有する３自由度スカラ
ロボットの現在位置における先端姿勢軸移動残角度情報
を取得するステップと、上記直線移動残距離情報と上記
先端姿勢軸移動残角度情報と上記先端姿勢軸を有する３
自由度スカラロボットの現在関節角度情報を入力して先
端姿勢軸による速度寄与補正係数を演算するステップ
と、当該先端姿勢軸を有する３自由度スカラロボットの
現在関節角度情報とその第一関節の定格最高角速度とに
基づいて算出された当該先端姿勢軸を有する３自由度ス
カラロボット先端の直線補間動作において取り得る許容
限界速度に上記速度寄与補正係数を乗じるステップと、
をさらに設けたので、座標変換計算の結果を放棄する必
要のあるフィードバック技術を用いることなく、同時に
制御的に不安定を生じる恐れの在るフィードフォワード
技術を用いることなく、原点近傍を直線補間動作にて通
過する場合に、先端姿勢軸の寄与を考慮して、関節軸の
定格最大角速度を超過しない先端姿勢軸を有する３自由
度スカラロボット先端の最大速度を求めることができ
る。

【０１７８】また、次の発明では、ロボットは水平面内
２自由度円筒座標ロボットであり、ロボットの現在関節
情報として上記水平面内２自由度円筒座標ロボットの現
在関節情報を取得し、ロボットの定格最高速度情報とし
て上記水平面内２自由度円筒座標ロボットの旋回軸の定
格最高角速度を取得し、上記上記水平面内２自由度円筒
座標ロボットの現在関節情報と旋回軸の定格最高角速度
とに基づいて上記水平面内２自由度円筒座標ロボット先
端の直線補間動作において取り得る許容限界速度を算出
するようにしたので、座標変換計算の結果を放棄する必
要のあるフィードバック技術を用いることなく、同時に
制御的に不安定を生じる恐れの在るフィードフォワード
技術を用いることなく、原点近傍を直線補間動作にて通
過する場合に、関節軸の定格最大角速度を超過しない水
平面内２自由度円筒座標ロボット先端の最大速度を求め
ることができる。

【０１７９】また、次の発明では、水平面内２自由度円
筒座標ロボットは先端姿勢軸を有する３自由度円筒座標
ロボットであり、上記先端姿勢軸を有する３自由度円筒
座標ロボットの現在位置における直線移動残距離情報を
取得するステップと、上記先端姿勢軸を有する３自由度
円筒座標ロボットの現在位置における先端姿勢軸移動残
角度情報を取得するステップと、上記直線移動残距離情
報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を
有する３自由度円筒座標ロボットの現在関節角度情報と
に基づいて上記先端姿勢軸による速度寄与補正係数を演
算するステップと、上記先端姿勢軸を有する３自由度円
筒座標ロボットの現在関節情報と旋回軸の定格最高角速
度とに基づいて算出した上記先端姿勢軸を有する３自由
度円筒座標ロボット先端の直線補間動作において取り得
る許容限界速度に上記速度寄与補正係数を乗じるステッ
プと、を設けたので、座標変換計算の結果を放棄する必
要のあるフィードバック技術を用いることなく、同時に
制御的に不安定を生じる恐れの在るフィードフォワード
技術を用いることなく、原点近傍を直線補間動作にて通
過する場合に、先端姿勢軸の寄与を考慮して、関節軸の
定格最大角速度を超過しない先端姿勢軸を有する３自由
度円筒座標ロボット先端の最大速度を求めることができ
る。

【０１８０】また、次の発明では、水平面内２自由度円
筒座標ロボットは垂直多関節ロボットであり、上記垂直
多関節ロボットの水平面へ投影して、上記垂直多関節ロ
ボットの第１アームおよび第２アームの水平面への投影
長を上記垂直多関節ロボットのアーム長として上記垂直
多関節ロボットの現在関節情報と旋回軸の定格最高角速
度とに基づいて上記垂直多関節ロボット先端の直線補間
動作において取り得る許容限界速度を算出するようにし
たので、座標変換計算の結果を放棄する必要のあるフィ
ードバック技術を用いることなく、同時に制御的に不安
定を生じる恐れの在るフィードフォワード技術を用いる
ことなく、原点近傍を直線補間動作にて通過する場合
に、関節軸の定格最大角速度を超過しない垂直多関節ロ
ボット先端の最大速度を求めることができる。

【０１８１】また、次の発明では、算出されたロボット
先端の直線補間動作において取り得る許容限界速度の等
高線を記憶するステップと、ロボットの現在位置情報を
取得するステップと、上記ロボットの現在位置情報に基
づいて上記予め記憶された許容限界速度の等高線から現
在位置における直線補間動作において取り得る許容限界
速度を検索するステップと、をさらに設けたので、直線
補間動作を行う前にロボット先端の位置と直線補間動作
における取り得る速度の関係を計算し記憶することによ
り、直線補間動作のサンプリング中は三角関数などから
なる演算を行うことなく、計算機にとって計算負荷の小
さい比較演算を主とした検索処理によって、原点近傍を
直線補間動作にて通過する場合の関節軸の定格最大角速
度を超過しないロボット先端の最大速度を求めることが
できる。

【０１８２】また、次の発明では、算出されたロボット
先端の直線補間動作において取り得る許容限界速度の等
高線を多角形、円もしくは楕円等に近似して記憶するス
テップと、ロボットの現在位置情報を取得するステップ
と、上記ロボットの現在位置情報に基づいて上記予め記
憶された近似された許容限界速度の等高線から現在位置
における直線補間動作において取り得る許容限界速度を
検索するステップと、をさらに設けたので、直線補間動
作開始前に生成される等高線情報の量を小さくすること
ができ、同時に直線補間動作のサンプリング中に少ない
比較演算にてロボット先端の最大速度を検索することが
できるので、演算能力の小さい装置においても、原点近
傍を直線補間動作にて通過する場合に、原点近傍を直線
補間動作にて通過する場合に、関節軸の定格最大角速度
を超過しないロボット先端の最大速度を求めることがで
きる。

【０１８３】また、次の発明では、算出されたスカラロ
ボット先端の直線補間動作において取り得る許容限界速
度の等高線を正方形に近似してその辺長を記憶するステ
ップと、上記スカラロボットの現在位置情報を取得する
ステップと、上記スカラロボットの現在位置情報に基づ
いて上記予め記憶された辺長から現在位置における直線
補間動作において取り得る許容限界速度を検索するステ
ップと、をさらに設けたので、直線補間動作開始前に生
成される等高線情報の量を小さくすることができ、同時
に直線補間動作のサンプリング中に少ない比較演算にて
ロボット先端の最大速度を検索することができるので、
演算能力の小さい装置においても、原点近傍を直線補間
動作にて通過する場合に、関節軸の定格最大角速度を超
過しないスカラロボット先端の最大速度を求めることが
できる。

【０１８４】また、次の発明では、算出されたスカラロ
ボット先端の直線補間動作において取り得る許容限界速
度の等高線を長方形に近似して原点から長方形の各辺ま
での距離を記憶するステップと、上記スカラロボットの
現在位置情報を取得するステップと、上記スカラロボッ
トの現在位置情報に基づいて上記予め記憶された距離か
ら現在位置における直線補間動作において取り得る許容
限界速度を検索するステップと、をさらに設けたので、
直線補間動作開始前に生成される等高線情報の量を小さ
くすることができ、同時に直線補間動作のサンプリング
中に少ない比較演算にてロボット先端の最大速度を検索
することができるので、演算能力の小さい装置において
も、原点近傍を直線補間動作にて通過する場合に、関節
軸の定格最大角速度を超過しないスカラロボット先端の
最大速度を求めることができる。

【０１８５】また、次の発明では、カラロボットは先端
姿勢軸を有する３自由度スカラロボットであり、上記先
端姿勢軸を有する３自由度スカラロボットの現在位置に
おける直線移動残距離情報を取得するステップと、上記
先端姿勢軸を有する３自由度スカラロボットの現在位置
における先端姿勢軸移動残角度情報を取得するステップ
と、上記直線移動残距離情報と上記先端姿勢軸移動残角
度情報と上記先端姿勢軸を有する３自由度スカラロボッ
トの現在関節角度情報に基づいて上記先端姿勢軸による
速度寄与補正係数を演算するステップと、検索された許
容限界速度に上記速度寄与補正係数を乗じるステップ
と、をさらに設けたので、先端姿勢軸の寄与を考慮し
て、原点近傍を直線補間動作にて通過する場合に、関節
軸の定格最大角速度を超過しないスカラロボット先端の
最大速度を求めることができる。

【０１８６】また、次の発明では、算出された水平面内
２自由度円筒座標ロボット先端の直線補間動作において
取り得る許容限界速度の等高線を入力して得られる複数
の円からなる速度の等高線の半径を記憶するステップ
と、上記水平面内２自由度円筒座標ロボットの現在位置
情報を取得するステップと、上記水平面内２自由度円筒
座標ロボットの現在位置情報に基づいて上記予め記憶さ
れた半径から現在位置における直線補間動作において取
り得る許容限界速度を検索するステップと、をさらに設
けたので、直線補間動作開始前に生成される等高線情報
の量を小さくすることができ、同時に直線補間動作のサ
ンプリング中に少ない比較演算にてロボット先端の最大
速度を検索することができるので、演算能力の小さい装
置においても、原点近傍を直線補間動作にて通過する場
合に、関節軸の定格最大角速度を超過しない水平面内２
自由度円筒座標ロボット先端の最大速度を求めることが
できる。

【０１８７】また、次の発明では、水平面内２自由度円
筒座標ロボットは先端姿勢軸を有する３自由度円筒座標
ロボットであり、上記先端姿勢軸を有する３自由度円筒
座標ロボットの現在位置における直線移動残距離情報を
取得するステップと、上記先端姿勢軸を有する３自由度
円筒座標ロボットの現在位置における先端姿勢軸移動残
角度情報を取得するステップと、上記直線移動残距離情
報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸を
有する３自由度円筒座標ロボットの現在関節角度情報と
に基づいて上記先端姿勢軸による速度寄与補正係数を演
算するステップと、検索された許容限界速度に上記速度
寄与補正係数を乗じるステップと、をさらに設けたの
で、原点近傍を直線補間動作にて通過する場合に、先端
姿勢軸の寄与を考慮して、関節軸の定格最大角速度を超
過しない先端姿勢軸を有する３自由度円筒座標ロボット
先端の最大速度を求めることができる。

【０１８８】また、次の発明では、平面内２自由度円筒
座標ロボットは垂直多関節ロボットであり、上記垂直多
関節ロボット先端の直線補間動作において取り得る許容
限界速度は、上記垂直多関節ロボットの水平面へ投影し
て、上記垂直多関節ロボットの第１アームおよび第２ア
ームの水平面への投影長を上記垂直多関節ロボットのア
ーム長として算出するようにしたので、原点近傍を直線
補間動作にて通過する場合に、関節軸の定格最大角速度
を超過しない垂直多関節ロボット先端の最大速度を求め
ることができる。

【０１８９】また、次の発明では、ロボットの現在位置
における進路方向を取得するステップと、上記ロボット
の現在位置における進路方向に基づいてその進路方向と
平行な方向へ上記ロボットの現在位置の座標を変換する
ステップと、をさらに備え、ロボット先端の直線補間動
作において取り得る許容限界速度を算出するステップ
は、上記変換された上記ロボットの現在位置の座標に基
づいて上記許容限界速度を算出するようにしたので、関
節軸の定格最大角速度を超過しないロボット先端の最大
速度の演算を簡略化することができる。

【０１９０】また、次の発明では、直線補間を行なう際
指定された速度で移動する場合の指定速度を算出するス
テップと、直線補間を行なう際指定された加速パターン
で移動する場合の速度を求めるステップと、直線補間を
行なう際指定された減速パターンで移動する場合の速度
を求めるステップと、上記指定速度、上記加速パターン
での速度、上記減速パターンでの速度および許容限界速
度のうちから最少の速度を選択するステップと、をさら
に設けたので、座標変換計算の結果を放棄する必要のあ
るフィードバック技術や、制御的に不安定を生じる恐れ
の在るフィードフォワード技術を用いることなく、また
関節軸の定格最大角速度を超過を生じることなく、原点
近傍を直線補間動作にて通過できるようにロボットの軌
跡を実際に制御したり、ロボットの軌跡をシミュレーシ
ョンすることができる。

【０１９１】また、次の発明では、直線補間を行なう際
指定された速度で移動する場合の指定速度を算出するス
テップと、直線補間を行なう際指定された加速パターン
で移動する場合の速度を求めるステップと、直線補間を
行なう際指定された減速パターンで移動する場合の速度
を求めるステップと、上記指定速度、上記加速パターン
での速度、上記減速パターンでの速度および速度寄与補
正係数が乗じられた許容限界速度のうちから最少の速度
を選択するステップと、をさらに設けたので、座標変換
計算の結果を放棄する必要のあるフィードバック技術
や、制御的に不安定を生じる恐れの在るフィードフォワ
ード技術を用いることなく、また関節軸の定格最大角速
度を超過を生じることなく、先端姿勢軸の速度寄与を補
正して、原点近傍を直線補間動作にて通過できるように
ロボットの軌跡を実際に制御したり、ロボットの軌跡を
シミュレーションすることができる。

【図面の簡単な説明】

【図１】本発明に係るロボットの速度演算装置および
ロボットの速度演算方法を採用したロボットの軌跡制御
装置とロボット本体との電気的構成図である。

【図２】本発明に係るロボット速度演算装置の実施の
形態１の動作を示すフローチャートである。

【図３】本発明に係るロボットの動作速度状態図であ
る。

【図４】実施の形態１が制御対象とするロボット水平
面内２自由度スカラ型ロボットの機構図である。

【図５】ロボットの動作経路説明図である。

【図６】縦軸をロボット先端の速度ならびに第１関節
軸の回転角速度をとり、横軸に時刻をとった速度状態図
である。

【図７】実施の形態２が制御対象とする先端姿勢軸を
有する水平面内３自由度スカラ型ロボットの機構図であ
る。

【図８】実施の形態２の動作を示すフローチャートで
ある。

【図９】実施の形態３が制御対象とする円筒座標型ロ
ボットの機構図である。

【図１０】実施の形態４が制御対象とする垂直多関節
型ロボットの機構図である。

【図１１】実施の形態４の動作を示すフローチャート
である。

【図１２】水平面内２自由度スカラ型ロボットの許容
限界速度の等高線図である。

【図１３】水平面内２自由度スカラ型ロボットの長方
形にて近似した許容限界速度の等高線図である。

【図１４】実施の形態５の動作を示すフローチャート
である。

【図１５】ロボットの座標回転の説明図である。

【図１６】円筒座標ロボットの許容限界速度の等高線
図である。

【図１７】実施の形態６であるロボットのシミュレー
ション装置の電気的構成図である。

【図１８】実施の形態６のロボットのシミュレーショ
ン装置の動作を示すフローチャートである。

【図１９】従来技術のロボット速度演算装置の直線補
間方法の方法を示すフローチャートである。

【図２０】従来技術によるロボットの動作速度状態図
である。

【図２１】特異領域を通過する従来技術によるロボッ
トの動作説明図である。

【図２２】縦軸をロボット先端の速度ならびに第１関
節軸の回転角速度をとり、横軸に時刻をとった場合の従
来のロボットの速度状態図である。

【図２３】フィードバック技術を用いた他の従来技術
によるロボット速度演算装置の直線補間方法のフローチ
ャートである。

【符号の説明】

１軌跡制御装置、２ロボット本体、３ティーチン
グボックス、４操作盤、１１ＣＰＵ、１２メモ
リ、１３インターフェイス、１４ａ〜１４ｄサーボド
ライバ、１５ａ〜１５ｄモータ、１６ａ〜１６ｄエ
ンコーダ。

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号庁内整理番号ＦＩ技術表示箇所Ｇ０５Ｂ 19/407 Ｋ

Claims

【特許請求の範囲】速度演算装置において、

【請求項１】ロボットの現在位置情報を取得する手段
と、ロボットの関節の定格最高速度情報を取得する手段
と、上記ロボット現在位置情報と上記関節定格最高速度
とに基づいてロボット先端の直線補間動作において取り
得る許容限界速度を算出する手段と、を備えたことを特
徴とするロボットの速度演算装置。
【請求項２】ロボットの現在関節情報を取得する手段
と、ロボットの関節の定格最高速度情報を取得する手段
と、上記現在関節情報と上記関節定格最高速度情報とに
基づいてロボット先端の直線補間動作において取り得る
許容限界速度を算出する手段と、を備えたことを特徴と
するロボットの速度演算装置。
【請求項３】ロボットはスカラロボットであり、ロボ
ットの現在関節情報として上記スカラロボットの現在関
節角度情報を取得し、ロボットの関節の定格最高速度情
報として上記スカラロボットの関節の定格最高角速度情
報を取得する、ことを特徴とする請求項２記載のロボッ
トの速度演算装置。
【請求項４】スカラロボットは水平面内２自由度スカ
ラロボットであり、スカラロボットの現在関節角度情報
として上記水平面内２自由度スカラロボットの現在関節
角度情報を取得し、スカラロボットの関節の定格最高角
速度情報として上記平面内２自由度スカラロボットの第
一関節の定格最高角速度を取得する、ことを特徴とする
請求項３記載のロボットの速度演算装置。
【請求項５】水平面内２自由度スカラロボットは先端
姿勢軸を有する３自由度スカラロボットであり、上記先
端姿勢軸を有する３自由度スカラロボットの現在位置に
おける直線移動残距離情報を取得する手段と、上記先端
姿勢軸を有する３自由度スカラロボットの現在位置にお
ける先端姿勢軸移動残角度情報を取得する手段と、上記
直線移動残距離情報と上記先端姿勢軸移動残角度情報と
上記先端姿勢軸を有する３自由度スカラロボットの現在
関節角度情報を入力して先端姿勢軸による速度寄与補正
係数を演算する手段と、当該先端姿勢軸を有する３自由
度スカラロボットの現在関節角度情報とその第一関節の
定格最高角速度とに基づいて算出された当該先端姿勢軸
を有する３自由度スカラロボット先端の直線補間動作に
おいて取り得る許容限界速度に上記速度寄与補正係数を
乗じる手段と、をさらに備えたことを特徴とする請求項
４記載のロボットの速度演算装置。
【請求項６】ロボットは水平面内２自由度円筒座標ロ
ボットであり、ロボットの現在関節情報として上記水平
面内２自由度円筒座標ロボットの現在関節情報を取得
し、ロボットの定格最高速度情報として上記水平面内２
自由度円筒座標ロボットの旋回軸の定格最高角速度を取
得し、上記上記水平面内２自由度円筒座標ロボットの現
在関節情報と旋回軸の定格最高角速度とに基づいて上記
水平面内２自由度円筒座標ロボット先端の直線補間動作
において取り得る許容限界速度を算出する、ことを特徴
とする請求項２記載のロボットの速度演算装置。
【請求項７】水平面内２自由度円筒座標ロボットは先
端姿勢軸を有する３自由度円筒座標ロボットであり、上
記先端姿勢軸を有する３自由度円筒座標ロボットの現在
位置における直線移動残距離情報を取得する手段と、上
記先端姿勢軸を有する３自由度円筒座標ロボットの現在
位置における先端姿勢軸移動残角度情報を取得する手段
と、上記直線移動残距離情報と上記先端姿勢軸移動残角
度情報と上記先端姿勢軸を有する３自由度円筒座標ロボ
ットの現在関節角度情報とに基づいて上記先端姿勢軸に
よる速度寄与補正係数を演算する手段と、上記先端姿勢
軸を有する３自由度円筒座標ロボットの現在関節情報と
旋回軸の定格最高角速度とに基づいて算出した上記先端
姿勢軸を有する３自由度円筒座標ロボット先端の直線補
間動作において取り得る許容限界速度に上記速度寄与補
正係数を乗じる手段と、をさらに備えたことを特徴とす
る請求項６記載のロボットの速度演算装置。
【請求項８】水平面内２自由度円筒座標ロボットは垂
直多関節ロボットであり、上記垂直多関節ロボットの水
平面へ投影して、上記垂直多関節ロボットの第１アーム
および第２アームの水平面への投影長を上記垂直多関節
ロボットのアーム長として上記垂直多関節ロボットの現
在関節情報と旋回軸の定格最高角速度とに基づいて上記
垂直多関節ロボット先端の直線補間動作において取り得
る許容限界速度を算出する、ことを特徴とする請求項６
もしくは請求項７記載のロボットの速度演算装置。
【請求項９】算出されたロボット先端の直線補間動作
において取り得る許容限界速度の等高線を記憶する手段
と、ロボットの現在位置情報を取得する手段と、上記ロ
ボットの現在位置情報に基づいて上記予め記憶された許
容限界速度の等高線から現在位置における直線補間動作
において取り得る許容限界速度を検索する手段と、をさ
らに備えたことを特徴とする請求項１〜請求項８のいず
れかに記載のロボットの速度演算装置。
【請求項１０】算出されたロボット先端の直線補間動
作において取り得る許容限界速度の等高線を多角形、円
もしくは楕円等に近似して記憶する手段と、ロボットの
現在位置情報を取得する手段と、上記ロボットの現在位
置情報に基づいて上記予め記憶された近似された許容限
界速度の等高線から現在位置における直線補間動作にお
いて取り得る許容限界速度を検索する手段と、をさらに
備えたことを特徴とする請求項１〜請求項８のいずれか
に記載のロボットの速度演算装置。
【請求項１１】算出されたスカラロボット先端の直線
補間動作において取り得る許容限界速度の等高線を正方
形に近似してその辺長を記憶する手段と、上記スカラロ
ボットの現在位置情報を取得する手段と、上記スカラロ
ボットの現在位置情報に基づいて上記予め記憶された辺
長から現在位置における直線補間動作において取り得る
許容限界速度を検索する手段と、をさらに備えたことを
特徴とする請求項３または請求項４記載のロボットの速
度演算装置。
【請求項１２】算出されたスカラロボット先端の直線
補間動作において取り得る許容限界速度の等高線を長方
形に近似して原点から長方形の各辺までの距離を記憶す
る手段と、上記スカラロボットの現在位置情報を取得す
る手段と、上記スカラロボットの現在位置情報に基づい
て上記予め記憶された距離から現在位置における直線補
間動作において取り得る許容限界速度を検索する手段
と、をさらに備えたことを特徴とする請求項３または請
求項４記載のロボットの速度演算装置。
【請求項１３】スカラロボットは先端姿勢軸を有する
３自由度スカラロボットであり、上記先端姿勢軸を有す
る３自由度スカラロボットの現在位置における直線移動
残距離情報を取得する手段と、上記先端姿勢軸を有する
３自由度スカラロボットの現在位置における先端姿勢軸
移動残角度情報を取得する手段と、上記直線移動残距離
情報と上記先端姿勢軸移動残角度情報と上記先端姿勢軸
を有する３自由度スカラロボットの現在関節角度情報に
基づいて上記先端姿勢軸による速度寄与補正係数を演算
する手段と、検索された許容限界速度に上記速度寄与補
正係数を乗じる手段と、をさらに備えたことを特徴とす
る請求項１１または請求項１２記載のロボットの速度演
算装置。
【請求項１４】算出された水平面内２自由度円筒座標
ロボット先端の直線補間動作において取り得る許容限界
速度の等高線を入力して得られる複数の円からなる速度
の等高線の半径を記憶する手段と、上記水平面内２自由
度円筒座標ロボットの現在位置情報を取得する手段と、
上記水平面内２自由度円筒座標ロボットの現在位置情報
に基づいて上記予め記憶された半径から現在位置におけ
る直線補間動作において取り得る許容限界速度を検索す
る手段と、をさらに備えたことを特徴とする請求項６記
載のロボットの速度演算装置。
【請求項１５】水平面内２自由度円筒座標ロボットは
先端姿勢軸を有する３自由度円筒座標ロボットであり、
上記先端姿勢軸を有する３自由度円筒座標ロボットの現
在位置における直線移動残距離情報を取得する手段と、
上記先端姿勢軸を有する３自由度円筒座標ロボットの現
在位置における先端姿勢軸移動残角度情報を取得する手
段と、上記直線移動残距離情報と上記先端姿勢軸移動残
角度情報と上記先端姿勢軸を有する３自由度円筒座標ロ
ボットの現在関節角度情報とに基づいて上記先端姿勢軸
による速度寄与補正係数を演算する手段と、検索された
許容限界速度に上記速度寄与補正係数を乗じる手段と、
をさらに備えたことを特徴とする請求項１４記載のロボ
ットの速度演算装置。
【請求項１６】水平面内２自由度円筒座標ロボットは
垂直多関節ロボットであり、上記垂直多関節ロボット先
端の直線補間動作において取り得る許容限界速度は、上
記垂直多関節ロボットの水平面へ投影して、上記垂直多
関節ロボットの第１アームおよび第２アームの水平面へ
の投影長を上記垂直多関節ロボットのアーム長として算
出する、ことを特徴とする請求項１４もしくは請求項１
５記載のロボットの速度演算装置。
【請求項１７】ロボットの現在位置における進路方向
を取得する手段と、上記ロボットの現在位置における進
路方向に基づいてその進路方向と平行な方向へ上記ロボ
ットの現在位置の座標を変換する手段と、をさらに備
え、ロボット先端の直線補間動作において取り得る許容
限界速度を算出する手段は、上記変換された上記ロボッ
トの現在位置の座標に基づいて上記許容限界速度を算出
する、ことを特徴とする請求項１〜請求項１６のいずれ
かに記載のロボットの速度演算装置。
【請求項１８】直線補間を行なう際指定された速度で
移動する場合の指定速度を算出する手段と、直線補間を
行なう際指定された加速パターンで移動する場合の速度
を求める手段と、直線補間を行なう際指定された減速パ
ターンで移動する場合の速度を求める手段と、上記指定
速度、上記加速パターンでの速度、上記減速パターンで
の速度および許容限界速度のうちから最少の速度を選択
する手段と、をさらに備えたことを特徴とする請求項
１、請求項２、請求項３、請求項４、請求項６、請求項
８、請求項９、請求項１０、請求項１１、請求項１２、
請求項１４、請求項１６または請求項１７記載のロボッ
トの速度演算装置。
【請求項１９】直線補間を行なう際指定された速度で
移動する場合の指定速度を算出する手段と、直線補間を
行なう際指定された加速パターンで移動する場合の速度
を求める手段と、直線補間を行なう際指定された減速パ
ターンで移動する場合の速度を求める手段と、上記指定
速度、上記加速パターンでの速度、上記減速パターンで
の速度および速度寄与補正係数が乗じられた許容限界速
度のうちから最少の速度を選択する手段と、をさらに備
えたことを特徴とする請求項５、請求項７、請求項１３
または請求項１５記載のロボットの速度演算装置。
【請求項２０】ロボットの現在位置情報を取得するス
テップと、ロボットの関節の定格最高速度情報を取得す
るステップと、上記ロボット現在位置情報と上記関節定
格最高速度とに基づいてロボット先端の直線補間動作に
おいて取り得る許容限界速度を算出するステップと、を
備えたことを特徴とするロボットの速度演算方法。
【請求項２１】ロボットの現在関節情報を取得するス
テップと、ロボットの関節の定格最高速度情報を取得す
るステップと、上記現在関節情報と上記関節定格最高速
度情報とに基づいてロボット先端の直線補間動作におい
て取り得る許容限界速度を算出するステップと、を備え
たことを特徴とするロボットの速度演算方法。
【請求項２２】ロボットはスカラロボットであり、ロ
ボットの現在関節情報として上記スカラロボットの現在
関節角度情報を取得し、ロボットの関節の定格最高速度
情報として上記スカラロボットの関節の定格最高角速度
情報を取得する、ことを特徴とする請求項２１記載のロ
ボットの速度演算方法。
【請求項２３】スカラロボットは水平面内２自由度ス
カラロボットであり、スカラロボットの現在関節角度情
報として上記水平面内２自由度スカラロボットの現在関
節角度情報を取得し、スカラロボットの関節の定格最高
角速度情報として上記平面内２自由度スカラロボットの
第一関節の定格最高角速度を取得する、ことを特徴とす
る請求項２２記載のロボットの速度演算方法。
【請求項２４】水平面内２自由度スカラロボットは先
端姿勢軸を有する３自由度スカラロボットであり、上記
先端姿勢軸を有する３自由度スカラロボットの現在位置
における直線移動残距離情報を取得するステップと、上
記先端姿勢軸を有する３自由度スカラロボットの現在位
置における先端姿勢軸移動残角度情報を取得するステッ
プと、上記直線移動残距離情報と上記先端姿勢軸移動残
角度情報と上記先端姿勢軸を有する３自由度スカラロボ
ットの現在関節角度情報を入力して先端姿勢軸による速
度寄与補正係数を演算するステップと、当該先端姿勢軸
を有する３自由度スカラロボットの現在関節角度情報と
その第一関節の定格最高角速度とに基づいて算出された
当該先端姿勢軸を有する３自由度スカラロボット先端の
直線補間動作において取り得る許容限界速度に上記速度
寄与補正係数を乗じるステップと、をさらに備えたこと
を特徴とする請求項２３記載のロボットの速度演算方
法。
【請求項２５】ロボットは水平面内２自由度円筒座標
ロボットであり、ロボットの現在関節情報として上記水
平面内２自由度円筒座標ロボットの現在関節情報を取得
し、ロボットの定格最高速度情報として上記水平面内２
自由度円筒座標ロボットの旋回軸の定格最高角速度を取
得し、上記上記水平面内２自由度円筒座標ロボットの現
在関節情報と旋回軸の定格最高角速度とに基づいて上記
水平面内２自由度円筒座標ロボット先端の直線補間動作
において取り得る許容限界速度を算出する、ことを特徴
とする請求項２１記載のロボットの速度演算方法。
【請求項２６】水平面内２自由度円筒座標ロボットは
先端姿勢軸を有する３自由度円筒座標ロボットであり、
上記先端姿勢軸を有する３自由度円筒座標ロボットの現
在位置における直線移動残距離情報を取得するステップ
と、上記先端姿勢軸を有する３自由度円筒座標ロボット
の現在位置における先端姿勢軸移動残角度情報を取得す
るステップと、上記直線移動残距離情報と上記先端姿勢
軸移動残角度情報と上記先端姿勢軸を有する３自由度円
筒座標ロボットの現在関節角度情報とに基づいて上記先
端姿勢軸による速度寄与補正係数を演算するステップ
と、上記先端姿勢軸を有する３自由度円筒座標ロボット
の現在関節情報と旋回軸の定格最高角速度とに基づいて
算出した上記先端姿勢軸を有する３自由度円筒座標ロボ
ット先端の直線補間動作において取り得る許容限界速度
に上記速度寄与補正係数を乗じるステップと、をさらに
備えたことを特徴とする請求項２５記載のロボットの速
度演算方法。
【請求項２７】水平面内２自由度円筒座標ロボットは
垂直多関節ロボットであり、上記垂直多関節ロボットの
水平面へ投影して、上記垂直多関節ロボットの第１アー
ムおよび第２アームの水平面への投影長を上記垂直多関
節ロボットのアーム長として上記垂直多関節ロボットの
現在関節情報と旋回軸の定格最高角速度とに基づいて上
記垂直多関節ロボット先端の直線補間動作において取り
得る許容限界速度を算出する、ことを特徴とする請求項
２５もしくは請求項２６記載のロボットの速度演算方
法。
【請求項２８】算出されたロボット先端の直線補間動
作において取り得る許容限界速度の等高線を記憶するス
テップと、ロボットの現在位置情報を取得するステップ
と、上記ロボットの現在位置情報に基づいて上記予め記
憶された許容限界速度の等高線から現在位置における直
線補間動作において取り得る許容限界速度を検索するス
テップと、をさらに備えたことを特徴とする請求項２０
〜請求項２７のいずれかに記載のロボットの速度演算方
法。
【請求項２９】算出されたロボット先端の直線補間動
作において取り得る許容限界速度の等高線を多角形、円
もしくは楕円等に近似して記憶するステップと、ロボッ
トの現在位置情報を取得するステップと、上記ロボット
の現在位置情報に基づいて上記予め記憶された近似され
た許容限界速度の等高線から現在位置における直線補間
動作において取り得る許容限界速度を検索するステップ
と、をさらに備えたことを特徴とする請求項２０〜請求
項２７のいずれかに記載のロボットの速度演算方法。
【請求項３０】算出されたスカラロボット先端の直線
補間動作において取り得る許容限界速度の等高線を正方
形に近似してその辺長を記憶するステップと、上記スカ
ラロボットの現在位置情報を取得するステップと、上記
スカラロボットの現在位置情報に基づいて上記予め記憶
された辺長から現在位置における直線補間動作において
取り得る許容限界速度を検索するステップと、をさらに
備えたことを特徴とする請求項２２または請求項２３記
載のロボットの速度演算方法。
【請求項３１】算出されたスカラロボット先端の直線
補間動作において取り得る許容限界速度の等高線を長方
形に近似して原点から長方形の各辺までの距離を記憶す
るステップと、上記スカラロボットの現在位置情報を取
得するステップと、上記スカラロボットの現在位置情報
に基づいて上記予め記憶された距離から現在位置におけ
る直線補間動作において取り得る許容限界速度を検索す
るステップと、をさらに備えたことを特徴とする請求項
２２または請求項２３記載のロボットの速度演算方法。
【請求項３２】スカラロボットは先端姿勢軸を有する
３自由度スカラロボットであり、上記先端姿勢軸を有す
る３自由度スカラロボットの現在位置における直線移動
残距離情報を取得するステップと、上記先端姿勢軸を有
する３自由度スカラロボットの現在位置における先端姿
勢軸移動残角度情報を取得するステップと、上記直線移
動残距離情報と上記先端姿勢軸移動残角度情報と上記先
端姿勢軸を有する３自由度スカラロボットの現在関節角
度情報に基づいて上記先端姿勢軸による速度寄与補正係
数を演算するステップと、検索された許容限界速度に上
記速度寄与補正係数を乗じるステップと、をさらに備え
たことを特徴とする請求項３０または請求項３１記載の
ロボットの速度演算方法。
【請求項３３】算出された水平面内２自由度円筒座標
ロボット先端の直線補間動作において取り得る許容限界
速度の等高線を入力して得られる複数の円からなる速度
の等高線の半径を記憶するステップと、上記水平面内２
自由度円筒座標ロボットの現在位置情報を取得するステ
ップと、上記水平面内２自由度円筒座標ロボットの現在
位置情報に基づいて上記予め記憶された半径から現在位
置における直線補間動作において取り得る許容限界速度
を検索するステップと、をさらに備えたことを特徴とす
る請求項２５記載のロボットの速度演算方法。
【請求項３４】水平面内２自由度円筒座標ロボットは
先端姿勢軸を有する３自由度円筒座標ロボットであり、
上記先端姿勢軸を有する３自由度円筒座標ロボットの現
在位置における直線移動残距離情報を取得するステップ
と、上記先端姿勢軸を有する３自由度円筒座標ロボット
の現在位置における先端姿勢軸移動残角度情報を取得す
るステップと、上記直線移動残距離情報と上記先端姿勢
軸移動残角度情報と上記先端姿勢軸を有する３自由度円
筒座標ロボットの現在関節角度情報とに基づいて上記先
端姿勢軸による速度寄与補正係数を演算するステップ
と、検索された許容限界速度に上記速度寄与補正係数を
乗じるステップと、をさらに備えたことを特徴とする請
求項３３記載のロボットの速度演算方法。
【請求項３５】水平面内２自由度円筒座標ロボットは
垂直多関節ロボットであり、上記垂直多関節ロボット先
端の直線補間動作において取り得る許容限界速度は、上
記垂直多関節ロボットの水平面へ投影して、上記垂直多
関節ロボットの第１アームおよび第２アームの水平面へ
の投影長を上記垂直多関節ロボットのアーム長として算
出する、ことを特徴とする請求項３２もしくは請求項３
４記載のロボットの速度演算方法。
【請求項３６】ロボットの現在位置における進路方向
を取得するステップと、上記ロボットの現在位置におけ
る進路方向に基づいてその進路方向と平行な方向へ上記
ロボットの現在位置の座標を変換するステップと、をさ
らに備え、ロボット先端の直線補間動作において取り得
る許容限界速度を算出するステップは、上記変換された
上記ロボットの現在位置の座標に基づいて上記許容限界
速度を算出する、ことを特徴とする請求項２０〜請求項
３５のいずれかに記載のロボットの速度演算方法。
【請求項３７】直線補間を行なう際指定された速度で
移動する場合の指定速度を算出するステップと、直線補
間を行なう際指定された加速パターンで移動する場合の
速度を求めるステップと、直線補間を行なう際指定され
た減速パターンで移動する場合の速度を求めるステップ
と、上記指定速度、上記加速パターンでの速度、上記減
速パターンでの速度および許容限界速度のうちから、最
少の速度を選択するステップと、をさらに備えたことを
特徴とする請求項２０、請求項２１、請求項２２、請求
項２３、請求項２５、請求項２７、請求項２８、請求項
２９、請求項３０、請求項３１、請求項３３、請求項３
５または請求項３６記載のいずれかに記載のロボットの
速度演算方法。
【請求項３８】直線補間を行なう際指定された速度で
移動する場合の指定速度を算出するステップと、直線補
間を行なう際指定された加速パターンで移動する場合の
速度を求めるステップと、直線補間を行なう際指定され
た減速パターンで移動する場合の速度を求めるステップ
と、上記指定速度、上記加速パターンでの速度、上記減
速パターンでの速度および速度寄与補正係数が乗じられ
た許容限界速度のうちから、最少の速度を選択するステ
ップと、をさらに備えたことを特徴とする請求項２４、
請求項２６、請求項３２または請求項３４記載のロボッ
トの速度演算方法。