JP4049956B2

JP4049956B2 - Robot controller

Info

Publication number: JP4049956B2
Application number: JP28200999A
Authority: JP
Inventors: 友宏宮崎; 公美雄斎藤
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 1999-10-01
Filing date: 1999-10-01
Publication date: 2008-02-20
Anticipated expiration: 2019-10-01
Also published as: JP2001100828A

Description

【０００１】
【発明の属する技術分野】
この発明は、ロボット制御装置に関するものである。
【０００２】
【従来の技術】
図１２は、産業用ロボットの先端を直線に沿って移動させる直線補間動作や円弧に沿って移動させる円弧補間動作を行うときの従来のロボット制御装置の構成を示すブロック図である。図において、１は補間指令、動作指令速度、教示位置などの教示データを記憶する教示データ記憶部、２は前記教示データ記憶部１に記憶された教示データを用いて直交座標系におけるロボットの動作指令を生成する補間演算部、３は前記補間演算部２で生成された動作指令を各関節の位置指令に変換する逆変換部、４は得られた関節位置指令をサーボ駆動系に出力する関節位置指令出力部、５はロボットの各関節を与えられた関節位置指令に追従して動作するようにサーボ制御するサーボ駆動系である。
【０００３】
次に、図１２に示した従来のロボット制御装置における作用と問題点を具体的に示すためにシミュレーション例について説明する。図１３は、このシミュレーションに用いた２自由度のロボットであり、１０１は第一関節、１０２は第二関節である。このロボットの二つの関節の最大速度は共に３ｒａｄ／ｓであるとする。このロボットを図１４のＡ点からＢ点まで、１５００ｍｍ／ｓの速度で直線補間動作させたときのシミュレーション結果を図１５に示す。図１５（ａ）は各関節の関節角度、同図（ｂ）は関節角速度、同図（ｃ）は関節角加速度を示しており、実線は第一関節、破線は第二関節の動きである。また、図１５（ｄ）はロボットの先端速度、同図（ｅ）は先端加速度である。
【０００４】
このとき、図１２の教示データ記憶部１には、点Ａから点Ｂまで１５００ｍｍ／ｓの動作指令速度で直線補間で移動するという内容の情報が記憶されており、補間演算部２では、教示データ記憶部１に記憶されている前記情報にもとづいてサンプリング周期毎のロボット先端の動作指令が計算される。図１５（ｄ）は補間演算部２で計算された動作指令によるロボットの先端速度を示したものであり、点Ａでの停止状態（速度＝０ｍｍ／ｓ）から１５００ｍｍ／ｓまで加速し、しばらく１５００ｍｍ／ｓの一定速度で移動した後、速度０ｍｍ／ｓまで減速して点Ｂで停止する様子を示している。補間演算部２で計算された動作指令は逆変換部３で各関節の位置指令に変換される。図１５（ａ）は逆変換部３で求められた関節位置指令である。関節位置指令出力部４では、この関節位置指令をサーボ駆動系５に出力し、サーボ駆動系５は関節位置指令に追従するようにロボットの各関節の位置を制御する。
【０００５】
図１５（ｂ）は、同図（ａ）に示した関節位置指令を時間で微分したものであり、関節の指令速度を示す。前記のように、このロボットの関節の最大速度は３ｒａｄ／ｓであるが、図１５（ｂ）に示した関節速度では第一関節１０１、第二関節１０２ともにこの最大値を超えている部分があるため、このような動作は実現不可能となり、ロボットは関節速度オーバーで異常停止することになってしまう。
【０００６】
このような関節速度が最大速度を超えて動作不可能となる状況を回避する方法の一つがたとえば特開平６−３２４７３０号公報に開示されている。図１６は、特開平６−３２４７３０号公報に開示されている技術についての制御ブロック図であり、図１２と同一または相当部分には同じ番号を付して示している。図において、２１はサーボ駆動系５に出力された関節位置指令の指令速度を記憶する関節速度記憶部、２２は関節速度記憶部２１に記憶されている関節速度から次のサンプリング周期での関節指令速度を推測し、これが関節の最大速度を超えるかどうかを推測する速度オーバー推測部、２３は最大速度を超えると推測される場合にロボット先端の動作指令速度を補正する補正係数を計算する補正係数演算部であり、各関節の最大速度は関節最大速度記憶部１１に記憶されている。補正係数演算部２３では、次のサンプル周期での関節速度の推測値が最大速度を超える場合、最大速度を速度推測値で除した値を速度補正係数として出力する。そして、補間演算部２では、補正係数演算部２３で計算された前記速度補正係数にもとづいてロボット先端の動作指令速度を遅くすることにより、関節速度がその最大速度を超えないような動作指令を生成し、ロボットを動作させる。
【０００７】
特開平６−３２４７３０号公報に開示されている方式では、関節速度が最大速度を超えないように補正係数を求めているので、図１２に示した従来技術のように関節速度オーバーでロボットが異常停止することはなくなる。しかし、この方式では、関節の指令加速度の連続性やロボット先端の動作指令の加速度の連続性については何ら触れられていない。すなわち、指令値における関節加速度や先端加速度が連続になることが必ずしも保証されているわけではないのである。これらの加速度が不連続に変動すると機械振動が発生しやすくなり好ましくない。
【０００８】
たとえば、図１７は、図１５と同じ条件のシミュレーションを行なった場合の結果である。ただし、このシミュレーションでは、補正係数を以下のようにして求めている。すなわち、次のサンプリング周期における第ｉ関節の関節指令速度をωｉとし、第ｉ関節の関節最大速度をωｉｍａｘとする。次に、第ｉ関節の関節指令速度ωｉの絶対値がその関節最大速度ωｉｍａｘを超えていないときは第ｉ関節の速度比率κｉを１とし、超えているときは関節最大速度ωｉｍａｘを関節指令速度ωｉの絶対値で除した値を第ｉ関節の速度比率κｉとする。すなわち、
κｉ＝１（｜ωｉ｜≦ωｉｍａｘのとき）（１）
κｉ＝ωｉｍａｘ／｜ωｉ｜（｜ωｉ｜＞ωｉｍａｘのとき）（２）
である。
そして、各関節の速度比率κｉの最小値を補正係数κとした。すなわち、
κ＝ｍｉｎ｛κｉ｜１≦ｉ≦Ｎ｝（３）
とした。ただし、Ｎはロボットの関節の数である。
【０００９】
図１７（ａ）は関節角速度、同図（ｂ）は関節角加速度であり、実線は第１関節、破線は第２関節の動きである。また、図１７（ｃ）は先端速度、同図（ｄ）は先端加速度、同図（ｅ）は前記のようにして求めた補正係数である。
図１５（ｂ）において関節指令速度がその関節最大速度を超えている部分では図１７（ｅ）に示す補正係数が１より小さい値を取っており動作指令速度が補正されている。図１７（ｃ）はこの補正係数を用いて補正された先端速度であり、図１７（ａ）はこのときの関節指令速度である。このように動作指令速度を補正することにより、関節指令速度は第一関節１０１、第二関節１０２ともに最大速度である３ｒａｄ／ｓ以下に抑えられており、速度オーバーとはならないことが判る。しかし、図１７（ｂ）の関節加速度や図１７（ｄ）の先端加速度を見ると所々で不連続となっており、なめらかな動作が得られていないことが判る。
【００１０】
【発明が解決しようとする課題】
従来のロボット制御装置は上記のように構成されていたので、図１２に示した従来技術では、ロボットを直線補間や円弧補間で動作させようとすると関節速度がその関節最大速度を超えることがあり、速度オーバーで異常停止するという課題があった。
【００１１】
また、特開平６−３２４７３０号公報に開示された従来のロボット制御方式では、速度オーバーによる異常停止を回避することは出来るが、関節加速度や先端加速度が連続となる保証がないため、機械振動が発生することがあるという課題があった。
【００１２】
この発明は、上記のような課題を解決するためになされたものであり、直線補間動作や円弧補間動作を行うときの速度オーバーを回避し、かつ、機械振動を発生することのない滑らかな補間動作を実現するロボット制御装置を得ることを目的とする。
【００１３】
【課題を解決するための手段】
この発明に係るロボット制御装置は、補間指令、動作指令速度、教示位置などの教示データを記憶する教示データ記憶部と、ロボットの各関節の関節最大速度を記憶する関節最大速度記憶部と、前記教示データにもとづいて関節指令速度を計算する関節速度演算部と、該関節速度演算部で計算された関節指令速度を前記関節最大速度で除した速度超過度を計算する関節速度超過度演算部と、前記各関節の速度超過度を要素とするベクトルのノルムを計算するノルム演算部と、該ノルム演算部で計算したノルムを入力とし、入出力関係を表す関数が連続微分可能で、かつ、前記入力が１以下のときは１以下の値、前記入力が１より大きいときは前記入力の逆数以下の値となる補正係数を出力する補正係数演算部と、前記教示データ記憶部に記憶された教示データと前記補正係数を乗じて補正した動作指令速度とを用いてロボットの加速度が連続な動作指令を生成する補間演算部とを備えるようにしたものである。
【００１４】
この発明に係るロボット制御装置は、ロボットの先端指令速度の最大値を記憶する先端最大速度記憶部と、教示データにもとづいて関節指令速度を計算する関節速度演算部と、前記教示データにもとづいて先端指令速度を計算する先端速度演算部と、前記関節速度演算部で計算された関節指令速度を前記関節最大速度で除して速度超過度を計算する関節速度超過度演算部と、ロボット先端の特異点からの距離を計算する特異点距離演算部と、前記先端速度演算部が計算した先端指令速度を、該特異点からの距離と前記ロボットの先端速度の最大値の積で除して先端指令速度の超過度を計算する先端速度超過度演算部と、前記各関節の速度超過度と前記先端指令速度の超過度とを要素とするベクトルのノルムを計算するノルム演算部とを備えるようにしたものである。
【００１５】
この発明に係るロボット制御装置は、関節角度に関して滑らかであり、かつ、特異点では関数値として零を取り、前記特異点以外では零以外の値を取る関数で特異点からの距離を特異点距離演算部が計算するようにしたものである。
【００１６】
この発明に係るロボット制御装置は、制御対象とするロボットの可操作度を特異点からの距離と見なし、前記可操作度を計算して前記特異点からの距離を特異点距離演算部が出力するようにしたものである。
【００２０】
【発明の実施の形態】
以下、この発明の実施の一形態について説明する。
実施の形態１．
図１は、この実施の形態１のロボット制御装置の構成を示すブロック図であり、図において、１は補間指令、動作指令速度、教示位置などの教示データを記憶する教示データ記憶部、２は前記教示データ記憶部１に記憶された教示データを用いて直交座標系におけるロボットの動作指令を生成する補間演算部、３は前記補間演算部２で生成された動作指令を各関節の位置指令に変換する逆変換部、４は得られた関節位置指令をサーボ駆動系に出力する関節位置指令出力部、５はロボットの各関節が与えられた関節位置指令に追従して動作するように前記各関節をサーボ制御するサーボ駆動系である。
【００２１】
１１はロボットの各関節の関節最大速度を記憶した関節最大速度記憶部、５１は教示データ記憶部１に記憶された教示データにもとづいて関節指令速度を計算する関節速度演算部、５２は関節速度演算部５１で計算された関節指令速度を関節最大速度記憶部１１に記憶された関節最大速度で除した速度超過度を各関節について計算する関節速度超過度演算部、５３は該各関節の速度超過度を要素とするベクトルのノルムを計算するノルム演算部、５４は該ノルムを入力とし補正係数を出力する補正係数演算部であり、補間演算部２では前記教示データ記憶部１に記憶された教示データと前記補正係数を乗じて補正した動作指令速度とを用いてロボットの動作指令を生成する。
【００２２】
次に動作について説明する。
関節速度演算部５１では計算された第ｉ関節の関節指令速度をωｉ、関節最大速度記憶部１１に記憶された第ｉ関節の関節最大速度をωｉｍａｘとすると、関節速度超過度演算部５２では第ｉ関節の速度超過度ξｉが
ξｉ＝｜ωｉ｜／ωｉｍａｘ（４）
で計算される。
ノルム演算部５３では、このξｉを要素とするベクトルξ＝［ξ１ ξ２・・・ξＮ］のノルムξｎｏｒｍを
ξｎｏｒｍ＝ｐｏｗ（ｐｏｗ（ξ１，Ｋ）＋
・・・・・・＋ｐｏｗ（ξＮ，Ｋ），１／Ｋ）（５）
を用いて計算する。ただし、Ｎはロボットの関節の数であり、ｐｏｗ（ｘ，ｙ）はｘのｙ乗を求める関数、Ｋは１以上の実数である。
【００２３】
さらに、補正係数演算部５４は、ノルム演算部５３の出力であるノルムξｎｏｒｍを入力し、補正係数を出力する。補正係数演算部５４の入出力関係は、たとえば、図２に実線で示すような関数を用いる。図２の破線は、入力であるξｎｏｒｍが１以下のときは１を出力し、ξｎｏｒｍが１より大きいときはその逆数を出力する関数を示す。すなわち、破線の関数をｆｏ（ξｎｏｒｍ）とすると、
ｆｏ（ξｎｏｒｍ）＝１（ξｎｏｒｍ≦１のとき）（６）
ｆｏ（ξｎｏｒｍ）＝１／ξｎｏｒｍ（ξｎｏｒｍ＞１のとき）（７）
である。
【００２４】
補正係数演算部５４の入出力関係を表すこの実線の関数は、破線以下の値を取り、かつ、ξｎｏｒｍに関して滑らか(連続微分可能)となるように選ぶ。補間演算部２では、教示データ記憶部１に記憶された教示データと補正係数演算部５４の出力である前記補正係数を乗じて補正した動作指令速度とを用いてロボットの動作指令を生成する。
【００２５】
このような補正により、関節速度がその最大速度を超えないことは以下のようにして示すことが出来る。関節速度演算部５１で計算される関節指令速度ωｉは動作指令速度を補正しない場合の各関節の速度であり、関節速度超過度演算部５２で計算される関節速度超過度ξｉは関節指令速度ωｉがその関節最大速度ωｉｍａｘの何倍になっているかを示す値である。したがって、補正係数演算部５４で計算される補正係数をκｆとすると、関節速度がその最大速度を超えないようにするには、すべての関節について
κｆ≦１／ξｉ（８）
が成り立つ必要がある。ところが、κｆは式（６）、式（７）のｆｏ以下であるから、
κｆ≦ｆｏ（９）
である。また、ｆｏは式（６）、式（７）より、
ｆｏ ≦１／ξｎｏｒｍ（１０）
が成り立つ。さらに、式（５）より
ξｎｏｒｍ≧ξｉ（１１）
である。したがって、式（９）、式（１０）、式（１１）より、式（８）が成り立ち、関節速度がその最大速度を超えないことを保証することが出来る。
【００２６】
さらに、関節速度演算部５１で計算される関節指令速度が時間に対して滑らかであると仮定すると、それから計算されるξｎｏｒｍも滑らかとなる。さらに、補正係数演算部５４の入出力関係も滑らかであるから、その出力である補正係数も滑らかとなり、関節加速度や先端加速度が不連続に変動することを回避することが期待できる。
【００２７】
図３は、以上の構成を用いた場合のシミュレーション結果である。図３（ａ）は関節角速度、同図（ｂ）は関節角加速度であり、実線は第一関節１０１、破線は第二関節１０２の動きである。また、図３（ｃ）は先端速度、同図（ｄ）は先端加速度、同図（ｅ）は補正係数である。図３（ａ）に示すように、各関節の関節速度はその関節最大速度である３ｒａｄ／ｓ以下となっており、速度オーバーを発生しない補間動作が実現されている。さらに、補正係数演算部５４で生成された補正係数は図３（ｅ）に示すように滑らかに変動している。これに対応して、図３（ｂ）に示す関節加速度も、図３（ｄ）に示す先端加速度も連続的に変動しており、図１７のような不連続性はなくなっている。
【００２８】
なお、この実施の形態１では、関節速度演算部５１において、教示データ記憶部１に記憶された教示データを用いて関節指令速度を求める構成としたが、図１６に示した従来技術と同様に過去の関節位置指令を用いて計算することも可能であるし、また、前回のサンプリング周期における関節指令速度を用いて計算してもよい。
【００２９】
以上のように、この実施の形態１によれば、各関節の関節指令速度をその関節最大速度で除して速度超過度を求め、各関節の速度超過度を要素とするベクトルのノルムを計算し、該ノルムから連続微分可能な関数を用いて補正係数を求め、これにより、滑らかな補正係数を得て、教示データ記憶部１に記憶された教示データと前記補正係数を乗じて補正した動作指令速度とを用いてロボットの動作指令を生成するため、直線補間動作や円弧補間動作を行うときの速度オーバーを回避でき、さらに、関節加速度や先端加速度に不連続性が生じないようにして、機械振動を発生することのない、滑らかな補間動作を実現できるロボット制御装置が得られる効果がある。
【００３０】
実施の形態２．
一般に関節型ロボットでは、特異点で自由度が縮退するため、ある特定の方向には移動できなくなる。移動できない方向にロボットを移動させるような指令を与えると、数学的には関節を無限大の速度で回転させることになるが、これは実現不可能であり速度オーバーとなる。特異点の近傍においても類似の現象が起こる。すなわち、特異点近傍ではある特定の方向に移動しにくくなり、その方向に移動するには関節が高速に回転しなければならず、速度オーバーになる可能性がある。速度オーバーを発生することなく特異点やその近傍を通過するには、ロボットの動作指令速度を遅くして、低速で通過させなければならない。
【００３１】
この実施の形態２では、特異点からの距離に比例した速度制限を設けて、ロボットの動作指令速度がこれを超えないように制御することにより、特異点やその近傍を滑らかに通過する動作指令を生成し、ロボットが特異点の近くを通過するときの加速度の急激な変動を抑制するものである。
【００３２】
図４は、この実施の形態２のロボット制御装置の構成を示すブロック図であり、図１と同一または相当部分には同じ番号を付して示している。図において、１２は先端速度の最大値を記憶する先端最大速度記憶部、６１は教示データ記憶部１に記憶された教示データにもとづいて先端指令速度を計算する先端速度演算部、６３はロボットの現在の位置と特異点との距離を計算する特異点距離演算部、６２は先端指令速度の超過度を計算する先端速度超過度演算部、６４は関節速度超過度演算部５２で計算された各関節の速度超過度と先端速度超過度演算部６２で計算された先端指令速度の超過度とを要素とするベクトルのノルムを計算するノルム演算部である。
【００３３】
次に、各ブロックの動作について説明する。
先端最大速度記憶部１２には、特異点近傍などの特殊な領域以外の大部分の動作領域で実現可能な先端最大速度が記憶されており、この値をＶｍａｘとする。特異点距離演算部６３では、ロボットの現在位置と特異点との距離Ｄが
Ｄ＝｜ｇ（θ）｜（１２）
で計算される。ただし、θはロボットの関節角度を表すベクトルであり、関数ｇ（θ）はθの関数であり、関節角度に関して滑らかであり、特異点では零、特異点以外では零以外の値をとる関数である。関数ｇ（θ）としては、関節速度と先端速度を関係付けるヤコビ行列の行列式、ロボットの可操作度などを用いることが出来るが、それ以外の関数でもよい。
【００３４】
先端速度超過度演算部６２では、先端速度演算部６１で計算された先端指令速度Ｖ、先端最大速度記憶部１２に記憶された先端最大速度Ｖｍａｘ、および、特異点距離演算部６３で求めた距離Ｄを用いて、先端指令速度の超過度ξｏが
ξｏ＝Ｖ／（ＫｄＤＶｍａｘ）（１３）
で計算される。ここで、ＫｄＤＶｍａｘは特異点近傍での先端速度の最大値であり、特異点からの距離に比例した値を取り、Ｋｄは適当な実数である。これは、特異点からの距離に比例した速度制限を設けて、ロボットの動作指令速度がこれを超えないように制御することにより、特異点への接近とともに動作指令速度を徐々に減速させるものである。これによって、特異点近傍での急激な速度変動を回避することが出来、特異点やその近傍を滑らかに通過させることが可能となる。
【００３５】
ノルム演算部６４では、このξｏと関節速度超過度演算部５２で求められた各関節の速度超過度ξｉを要素とするベクトルξ＝［ξｏ ξ１ ξ２・・・ξＮ］のノルムξｎｏｒｍを
ξｎｏｒｍ＝ｐｏｗ（ｐｏｗ（ξ０，Ｋ）＋
・・・・・・＋ｐｏｗ（ξＮ，Ｋ），１／Ｋ）（１４）
により計算する。ただし、Ｎはロボットの関節の数であり、ｐｏｗ（ｘ，ｙ）はｘのｙ乗を求める関数、Ｋは１以上の実数である。
以下、前記実施の形態１と同様に補正係数演算部５４で補正係数を計算し、補間演算部２でロボットの動作指令を生成する。
【００３６】
図５は、以上の構成を用いた場合のシミュレーション結果である。前記実施の形態１によるシミュレーション結果を示す図３と同様に、関節速度がその関節最大速度以下となっており、かつ、補正係数（図５（ｅ））、関節加速度（図５（ｂ））、先端加速度（図５（ｄ））共に滑らかであり、滑らかな動作が実現されている。
【００３７】
前記実施の形態１によるシミュレーション結果を示す図３（ｄ）では、時刻０．７秒から０．８秒にかけて先端加速度が急激に変動しているが、これはロボットが特異点の近傍を通過するためである。
これに対し、この実施の形態２では、特異点からの距離により動作指令速度を制限しているため、図５（ｄ）に示したシミュレーション結果では、同時刻においてこのような先端加速度の急激な変動は見られず、より滑らかに特異点近傍を通過していることが判る。
【００３８】
以上のように、この実施の形態２によれば、ロボット先端の先端指令速度を特異点からの距離と先端指令速度の最大値の積で除して先端指令速度の超過度を求め、関節速度超過度と前記先端指令速度の超過度とを要素とするベクトルのノルムを計算し、該ノルムから連続微分可能な関数を用いて補正係数を求め、教示データと前記補正係数を乗じて補正した動作指令速度とを用いてロボットの動作指令を生成するため、前記実施の形態１の効果に加えて、速度オーバーを発生することなくロボットの動作速度を遅くして、低速で特異点やその近傍を通過することの出来るロボット制御装置が得られる効果がある。
【００３９】
実施の形態３．
この実施の形態３のロボット制御装置は、前記従来技術で説明した式（１）〜式（３）で求めたκをそのまま補正係数とするのではなく、ローパスフィルタに通すことにより滑らかな補正係数を得るものであり、さらにこのときのローパスフィルタの遅れによる速度補正のタイミングの遅れを解消するものである。
【００４０】
図６は、この実施の形態３のロボット制御装置の構成を示すブロック図であり、図１と同一または相当部分には同じ番号を付して示している。図において、３１は教示データ記憶部１に記憶された教示データにもとづいて、現在の時刻（現在のサンプリング周期、演算周期）より一定時間先の時刻に対応した各関節ごとの関節指令速度を計算する関節速度演算部である。
３２は前記関節速度演算部３１で計算された関節指令速度が関節最大速度記憶部１１に記憶された関節最大速度を超える関節については、その関節最大速度を前記関節指令速度で除した値を速度比率とし、超えない場合は１とする速度比率を求める速度比率演算部、３３は前記速度比率演算部３２が前記各関節ごとに求めた速度比率の中からその最小値を求める速度比率最小値演算部である。３４は前記速度比率最小値演算部３３で求めた最小値を、現在の時刻から前記先の時刻までの前記一定時間と一致する遅延量を有したローパスフィルタに通して補正係数を計算する補正係数演算部である。
【００４１】
次に、動作について説明する。
現在の時刻をｔとすると、関節速度演算部３１では教示データ記憶部１に記憶された教示データにもとづいて現在時刻より一定時間Δｔだけ先の時刻における未来の関節指令速度を各関節について計算する。関節速度演算部３１で計算される第ｉ関節の関節指令速度をωｉ（ｔ＋Δｔ）とする。速度比率演算部３２では第ｉ関節の速度比率ηｉを、

のように求める。
【００４２】
ここで、ωｉｍａｘは関節最大速度記憶部１１に記憶された第ｉ関節の関節最大速度である。つまり、速度比率ηｉは関節指令速度ωｉが関節最大速度ωｉｍａｘを超える関節についてはその関節最大速度ωｉｍａｘを関節指令速度ωｉで除した値となり、超えない場合は１となる。
【００４３】
速度比率最小値演算部３３では、各関節の速度比率ηｉの中からその最小値ηｍｉｎを求める。すなわち、
ηｍｉｎ（ｔ＋Δｔ）＝ｍｉｎ（ηｉ（ｔ＋Δｔ）｜１≦ｉ≦Ｎ）（１７）
である。
補正係数演算部３４では、このようにして求められたηｍｉｎ(ｔ)をローパスフィルタに通して補正係数κを求める。ローパスフィルタとしては種々のものを用いることが出来るが、たとえば移動平均フィルタを用いた場合は次式（１８）により補正係数κを計算する。ただし、Ｔはフィルタの時間長である。
【数１】

【００４４】
一般に、ローパスフィルタでは入出力に時間的な遅れが生じる。前記関節速度演算部３１で一定時間Δｔだけ未来の関節指令速度を計算するのは、このフィルタによる遅れを補正するためであり、前記Δｔをローパスフィルタの遅れと等しくなるように設定しておくと、時間的に遅れのない補償を行うことが出来る。移動平均フィルタの遅れはその時間長Ｔの半分であるから、Δｔ＝Ｔ／２とする。補間演算部２では、教示データ記憶部１に記憶された教示データと補正係数演算部３４の出力である補正係数κを乗じて補正した動作指令速度とを用いてロボットの動作指令を生成する。
【００４５】
この実施の形態３のロボット制御装置によるシミュレーション結果を図７に示す。図７（ｅ）に見られるように滑らかな補正係数が得られており、図７（ｂ）および図７（ｄ）を見ると関節加速度、先端加速度ともに連続となっていることが判る。また、図７（ａ）から、関節指令速度はわずかに関節最大速度を超えている部分がある程度であり、関節最大速度の制約をほぼ満足している。
【００４６】
以上のように、この実施の形態３によれば、前記従来技術で説明した式（１）、式（２）、式（３）で求めたκをそのまま補正係数とするのではなく、ローパスフィルタに通し、この場合、フィルタの遅れにより速度補正のタイミングが遅れるため、一定時間未来の関節指令速度を用いた式（１５）、式（１６）を計算することによりフィルタの遅れを補正し、滑らかな補正係数を得ることの出来るロボット制御装置が実現できる効果がある。
【００４７】
実施の形態４．
この実施の形態４のロボット制御装置は、教示データにもとづいて、現在の時刻（現在のサンプリング周期、演算周期）より一定時間先の時刻に対応したロボットの各関節ごとの関節指令速度を計算し、該関節指令速度がその関節最大速度を超える関節についてはその関節最大速度を前記関節指令速度で除した値を速度比率とし、超えない場合は１とする速度比率を求め、前記各関節ごとに求めた前記速度比率の中からその最小値を求め、現在の時刻から前記一定時間経過するまでの各演算周期ごとに求めた前記速度比率の最小値を記憶し、該記憶した前記速度比率の最小値の中からさらに最小の値を求めて出力し、この出力を、前記現在の時刻より前記先の時刻までの前記一定時間の半分とほぼ一致する遅延量を有したローパスフィルタに通して補正係数を計算し、前記教示データと前記補正係数を乗じて補正した動作指令速度とを用いてロボットの動作指令を生成し、関節の関節最大速度の制約を厳密に満足する補間動作を実現するものである。
【００４８】
図８は、この実施の形態４のロボット制御装置の構成を示すブロック図であり、前記実施の形態３の図６と同一または相当部分には同じ番号を付して示している。図において、３１は教示データ記憶部１に記憶された教示データにもとづいて現在時刻から一定時間Δｔだけ先の時刻の未来の各関節の関節指令速度を計算する関節速度演算部、３２は前記関節速度演算部３１で計算された関節指令速度が関節最大速度記憶部１１に記憶された関節最大速度を超える関節についてはその関節最大速度を前記関節指令速度で除した値を速度比率とし、超えない場合は１とする速度比率を求める速度比率演算部、３３は前記速度比率演算部３２が前記各関節ごとに求めた速度比率の中からその最小値を求める速度比率最小値演算部である。
４１は現在時刻から前記一定時間Δｔ経過するまでの各演算周期ごとに、前記関節速度演算部３１、速度比率演算部３２、速度比率最小値演算部３３の動作により求めた前記速度比率の最小値を記憶する速度比率最小値記憶部である。４２は速度比率最小値記憶部４１に記憶されたデータの中での最小の値を求めて出力する最小値検出部、４３は該最小値検出部４２の出力をローパスフィルタに通して補正係数を計算する補正係数演算部である。
【００４９】
速度比率最小値記憶部４１には、過去Δｔの間に速度比率最小値演算部３３で計算された各関節ごとの速度比率ηｉの中での最小値ηｍｉｎが記憶されている。現在時刻をｔとすると、時刻ｔ−Δｔにおいて計算されるηｍｉｎはηｍｉｎ（ｔ）であり、現在時刻ｔにおいて計算されるηｍｉｎはηｍｉｎ(ｔ＋Δｔ)であるから、速度比率最小値記憶部４１にはηｍｉｎ（ｔ）からηｍｉｎ（ｔ＋Δｔ）の値が記憶されている。最小値検出部４２では、速度比率最小値記憶部４１に記憶されているηｍｉｎのうちで最小の値が求められ出力される。最小値検出部４２の前記出力をηｌｏｗとすると、
ηｌｏｗ（ｔ）＝ｍｉｎ（ηｍｉｎ（τ）｜ｔ≦τ≦ｔ＋Δｔ）（１９）
となる。補正係数演算部４３では、このようにして求められたηｌｏｗ（ｔ）をローパスフィルタに通して補正係数κを求める。
【００５０】
図９（ａ）は速度比率最小値演算部３３で求められたηｍｉｎであり、図９（ｂ）の実線は最小値検出部４２の出力であるηｌｏｗであり、図９（ｃ）の実線は補正係数演算部４３が出力する補正係数κを示したものである。図９（ｂ）、図９（ｃ）の破線は比較のため図９（ａ）のηｍｉｎを同時に示したものである。図９（ｂ）のηｌｏｗは図９（ａ）のηｍｉｎの極小値の部分を平坦にしたような形になっており、これを補正係数演算部４３でローパスフィルタに通すことにより、図９（ｃ）のように滑らかで、かつ、すべての時刻において
κ≦ηｍｉｎ（２０）
となる補正係数κが求められている。
【００５１】
なお、この実施の形態４においても、前記ローパスフィルタの代わりに移動平均フィルタを用いることが出来、前記一定時間Δｔを前記移動平均フィルタの窓長（前記実施の形態３でのフィルタの時間長Ｔ）に一致するように設定した構成にしてもよく、この移動平均フィルタにおける遅れはΔｔ／２であるから、図９（ｂ）の実線で示す最小値検出部４２の出力はΔｔ／２遅れ、図９（ｃ）の実線で示すように滑らかで、かつ、すべての時刻において式（２０）となる補正係数κをより確実に求めることが出来る。
【００５２】
図１０は、この実施の形態４と比較するための前記実施の形態３におけるηｍｉｎと補正係数κを示したものである。図の破線がηｍｉｎであり、実線が補正係数κである。前記実施の形態３では、ηｍｉｎを補正係数演算部３４でローパスフィルタに通して補正係数κを求めていたため、部分的に
κ＞ηｍｉｎ（２１）
となっており、この時刻で関節速度がその関節最大速度をわずかではあるが超えていた。この実施の形態４では、速度比率最小値記憶部４１と最小値検出部４２を付加して、現在時刻から一定時間Δｔ先までの図９（ｂ）に示すηｌｏｗを求め、これを補正係数演算部４３でローパスフィルタまたは移動平均フィルタに通す構成にしており、これにより、常にκ≦ηｍｉｎとなり、関節指令速度はその関節最大速度を超えることがなくなる。
【００５３】
次に、この実施の形態４によるシミュレーション結果を図１１に示す。図１１（ｅ）に示すように滑らかな補正係数が得られており、図１１（ｂ）および図１１（ｄ）を見ると関節加速度、先端加速度ともに連続となっていることが判る。また、図１１（ａ）を見ると、関節指令速度もその関節最大速度以下となっている。
【００５４】
以上のように、この実施の形態４によれば、一定時間未来の関節指令速度を用いて各関節ごとの補正係数κを求め、さらに、現在時刻から前記一定時間経過するまでの演算周期ごとに、関節速度演算部３１、速度比率演算部３２、速度比率最小値演算部３３の動作により求められた前記一定時間内の速度比率の最小値を速度比率最小値記憶部４１で記憶し、該速度比率最小値記憶部４１で記憶した速度比率の中で最小の値を最小値検出部４２で検出し、これを補正係数演算部４３でローパスフィルタまたは移動平均フィルタの入力とすることで補正係数を演算し求めるため、前記実施の形態３では関節指令速度がその関節最大速度をわずかに超える場合があるのに対し、関節最大速度の制約を厳密に満足する補間動作が得られるロボット制御装置が実現できる効果がある。
【００５５】
【発明の効果】
以上のように、この発明によれば、ロボットの各関節の関節最大速度を記憶する関節最大速度記憶部と、教示データにもとづいて関節指令速度を計算する関節速度演算部と、該関節速度演算部で計算された関節指令速度を前記関節最大速度で除した速度超過度を計算する関節速度超過度演算部と、前記各関節の速度超過度を要素とするベクトルのノルムを計算するノルム演算部と、該ノルム演算部で計算したノルムを入力とし、入出力関係を表す関数が連続微分可能で、かつ、前記入力が１以下のときは１以下の値、前記入力が１より大きいときは前記入力の逆数以下の値となる補正係数を出力する補正係数演算部と、前記教示データ記憶部に記憶された教示データと前記補正係数を乗じて補正した動作指令速度とを用いてロボットの加速度が連続な動作指令を生成する補間演算部とを備えるように構成したので、直線補間動作や円弧補間動作を行うときの速度オーバーを回避でき、関節加速度や先端加速度が連続となり、機械振動が発生せず、滑らかな補間動作を実現できる効果がある。
【００５６】
この発明によれば、ロボットの先端指令速度の最大値を記憶する先端最大速度記憶部と、教示データにもとづいて関節指令速度を計算する関節速度演算部と、前記教示データにもとづいて先端指令速度を計算する先端速度演算部と、前記関節速度演算部で計算された関節指令速度を前記関節最大速度で除して速度超過度を計算する関節速度超過度演算部と、ロボット先端の特異点からの距離を計算する特異点距離演算部と、前記先端速度演算部が計算した先端指令速度を、該特異点からの距離と前記ロボットの先端指令速度の最大値の積で除して先端指令速度の超過度を計算する先端速度超過度演算部と、前記各関節の速度超過度と前記先端指令速度の超過度とを要素とするベクトルのノルムを計算するノルム演算部とを備えるように構成したので、直線補間動作や円弧補間動作を行うときの速度オーバーを回避でき、関節加速度や先端加速度が連続となり、機械振動が発生せず、滑らかな補間動作を実現できるだけでなく、特異点やその近傍を滑らかに通過する動作指令を生成できる効果がある。
【００５７】
この発明によれば、関節角度に関して滑らかであり、かつ、特異点では関数値として零を取り、前記特異点以外では零以外の値を取る関数で特異点からの距離を計算するように構成したので、特異点近傍での急激な速度変動を回避でき、特異点やその近傍での通過を滑らかに出来る補間動作を実現できる効果がある。
【００５８】
この発明によれば、制御対象とするロボットの可操作度を特異点からの距離と見なし、前記可操作度を計算して前記特異点からの距離を出力するように構成したので、特異点近傍での急激な速度変動を回避でき、特異点やその近傍での通過を滑らかに出来る補間動作を実現できる効果がある。
【図面の簡単な説明】
【図１】この発明の実施の形態１のロボット制御装置の構成を示すブロック図である。
【図２】この発明の実施の形態１のロボット制御装置における補正係数演算部の入出力関係を規定する関数をグラフで示した説明図である。
【図３】この発明の実施の形態１のロボット制御装置に対するシミュレーション結果である関節角速度などの特性図である。
【図４】この発明の実施の形態２のロボット制御装置の構成を示すブロック図である。
【図５】この発明の実施の形態２のロボット制御装置に対するシミュレーション結果である関節角速度などの特性図である。
【図６】この発明の実施の形態３のロボット制御装置の構成を示すブロック図である。
【図７】この発明の実施の形態３のロボット制御装置に対するシミュレーション結果である関節角速度などの特性図である。
【図８】この発明の実施の形態４のロボット制御装置の構成を示すブロック図である。
【図９】この発明の実施の形態４のロボット制御装置の速度比率最小値演算部で求められたηｍｉｎ、最小値検出部の出力であるηｌｏｗ、補正係数演算部が出力する補正係数κなどの計算例を示す説明図である。
【図１０】この発明の実施の形態４のロボット制御装置と比較するための実施の形態３のロボット制御装置における各関節の速度比率の最小値ηｍｉｎ、補正係数κの計算例を示す説明図である。
【図１１】この発明の実施の形態４のロボット制御装置に対するシミュレーション結果である関節角速度などの特性図である。
【図１２】従来のロボット制御装置の構成を示すブロック図である。
【図１３】従来のロボット制御装置のシミュレーションに用いたロボットの自由度構成図である。
【図１４】従来のロボット制御装置のシミュレーションに用いたロボットの動作を示す説明図である。
【図１５】従来のロボット制御装置の補間方式を用いた場合のシミュレーション結果である関節角速度などの特性図である。
【図１６】従来のロボット制御装置として特開平６−３２４７３０号公報に開示されている技術についての制御ブロック図である。
【図１７】従来のロボット制御装置として特開平６−３２４７３０号公報に開示されている技術のシミュレーション結果である関節角速度などの特性図である。
【符号の説明】
１教示データ記憶部、２補間演算部、１１関節最大速度記憶部、１２先端最大速度記憶部、３１関節速度演算部、３２速度比率演算部、３３速度比率最小値演算部、３４補正係数演算部、４１速度比率最小値記憶部、４２最小値検出部、４３補正係数演算部、５１関節速度演算部、５２関節速度超過度演算部、５３ノルム演算部、５４補正係数演算部、６１先端速度演算部、６２先端速度超過度演算部、６３特異点距離演算部、６４ノルム演算部、１０１第一関節、１０２第二関節。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a robot control apparatus.
[0002]
[Prior art]
FIG. 12 is a block diagram showing a configuration of a conventional robot control apparatus when performing a linear interpolation operation for moving the tip of an industrial robot along a straight line or a circular interpolation operation for moving along the circular arc. In the figure, 1 is a teaching data storage unit for storing teaching data such as an interpolation command, an operation command speed, and a teaching position, and 2 is an operation of a robot in an orthogonal coordinate system using the teaching data stored in the teaching data storage unit 1. An interpolation calculation unit for generating a command, 3 is an inverse conversion unit for converting the operation command generated by the interpolation calculation unit 2 into a position command for each joint, and 4 is a joint for outputting the obtained joint position command to the servo drive system. A position command output unit 5 is a servo drive system that servo-controls each joint of the robot so as to operate following a given joint position command.
[0003]
Next, a simulation example will be described in order to specifically show the operation and problems in the conventional robot control apparatus shown in FIG. FIG. 13 shows a two-degree-of-freedom robot used in this simulation, where 101 is a first joint and 102 is a second joint. It is assumed that the maximum speeds of the two joints of this robot are both 3 rad / s. FIG. 15 shows a simulation result when the robot is linearly interpolated at a speed of 1500 mm / s from point A to point B in FIG. 15A shows the joint angle of each joint, FIG. 15B shows the joint angular velocity, FIG. 15C shows the joint angular acceleration, the solid line indicates the first joint, and the broken line indicates the movement of the second joint. . FIG. 15D shows the tip speed of the robot, and FIG. 15E shows the tip acceleration.
[0004]
At this time, the teaching data storage unit 1 in FIG. 12 stores information on the content of moving from point A to point B by linear interpolation at an operation command speed of 1500 mm / s. Based on the information stored in the data storage unit 1, an operation command for the robot tip at each sampling period is calculated. FIG. 15D shows the tip speed of the robot according to the operation command calculated by the interpolation calculation unit 2. The robot accelerates from the stop state at point A (speed = 0 mm / s) to 1500 mm / s for a while. After moving at a constant speed of 1500 mm / s, the speed is reduced to 0 mm / s and stopped at point B. The motion command calculated by the interpolation calculation unit 2 is converted by the inverse conversion unit 3 into a position command for each joint. FIG. 15A shows a joint position command obtained by the inverse conversion unit 3. The joint position command output unit 4 outputs this joint position command to the servo drive system 5, and the servo drive system 5 controls the position of each joint of the robot so as to follow the joint position command.
[0005]
FIG. 15 (b) is a graph obtained by differentiating the joint position command shown in FIG. 15 (a) with respect to time, and shows the command speed of the joint. As described above, the maximum joint speed of this robot is 3 rad / s. However, in the joint speed shown in FIG. 15B, the first joint 101 and the second joint 102 both exceed this maximum value. Therefore, such an operation cannot be realized, and the robot stops abnormally when the joint speed is exceeded.
[0006]
One method for avoiding such a situation where the joint speed exceeds the maximum speed and becomes inoperable is disclosed, for example, in JP-A-6-324730. FIG. 16 is a control block diagram of the technique disclosed in Japanese Patent Laid-Open No. 6-324730, and the same or corresponding parts as those in FIG. 12 are denoted by the same reference numerals. In the figure, 21 is a joint speed storage unit for storing the command speed of the joint position command output to the

servo drive system

5, and 22 is a joint command at the next sampling cycle from the joint speed stored in the joint speed storage unit 21. A speed over estimation unit that estimates the speed and estimates whether or not this exceeds the maximum speed of the joint, and 23 is a correction coefficient that calculates a correction coefficient for correcting the motion command speed of the robot tip when it is estimated that the speed exceeds the maximum speed The maximum speed of each joint is stored in the maximum joint speed storage unit 11. When the estimated value of the joint speed in the next sample period exceeds the maximum speed, the correction coefficient calculation unit 23 outputs a value obtained by dividing the maximum speed by the estimated speed value as a speed correction coefficient. Then, the interpolation calculation unit 2 sets an operation command so that the joint speed does not exceed the maximum speed by slowing the operation command speed at the tip of the robot based on the speed correction coefficient calculated by the correction coefficient calculation unit 23. Generate and operate the robot.
[0007]
In the method disclosed in Japanese Patent Laid-Open No. 6-324730, the correction coefficient is obtained so that the joint speed does not exceed the maximum speed. Therefore, the robot is abnormal due to the joint speed being over as in the prior art shown in FIG. It will never stop. However, in this method, nothing is mentioned about the continuity of the command acceleration of the joint and the continuity of the acceleration of the motion command of the robot tip. That is, it is not always guaranteed that the joint acceleration and the tip acceleration at the command value are continuous. If these accelerations fluctuate discontinuously, mechanical vibration is likely to occur, which is not preferable.
[0008]
For example, FIG. 17 shows the results when a simulation is performed under the same conditions as in FIG. However, in this simulation, the correction coefficient is obtained as follows. That is, the joint command speed of the i-th joint in the next sampling cycle is ωi, and the joint maximum speed of the i-th joint is ωimax. Next, when the absolute value of the joint command speed ωi of the i-th joint does not exceed the joint maximum speed ωimax, the speed ratio κi of the i-th joint is set to 1, and when it exceeds, the joint maximum speed ωimax is set as the joint command speed. The value divided by the absolute value of ωi is defined as the speed ratio κi of the i-th joint. That is,
κi = 1 (when | ωi | ≦ ωimax) (1)
κi = ωimax / | ωi | (when | ωi |> ωimax) (2)
It is.
The minimum value of the speed ratio κi of each joint was used as the correction coefficient κ. That is,
κ = min {κi | 1 ≦ i ≦ N} (3)
It was. N is the number of robot joints.
[0009]
FIG. 17A shows the joint angular velocity, FIG. 17B shows the joint angular acceleration, the solid line shows the movement of the first joint, and the broken line shows the movement of the second joint. FIG. 17C shows the tip speed, FIG. 17D shows the tip acceleration, and FIG. 17E shows the correction coefficient obtained as described above.
In the part where the joint command speed exceeds the maximum joint speed in FIG. 15B, the correction coefficient shown in FIG. 17E takes a value smaller than 1, and the operation command speed is corrected. FIG. 17C shows the tip speed corrected using this correction coefficient, and FIG. 17A shows the joint command speed at this time. By correcting the motion command speed in this way, the joint command speed is suppressed to 3 rad / s or less which is the maximum speed for both the first joint 101 and the second joint 102, and it can be seen that the speed does not exceed. However, the joint acceleration in FIG. 17B and the tip acceleration in FIG. 17D are discontinuous in some places, and it can be seen that a smooth operation is not obtained.
[0010]
[Problems to be solved by the invention]
Since the conventional robot control apparatus is configured as described above, in the conventional technique shown in FIG. 12, when the robot is operated by linear interpolation or circular interpolation, the joint speed may exceed the maximum joint speed. There was a problem of abnormal stop due to overspeed.
[0011]
In addition, the conventional robot control system disclosed in Japanese Patent Laid-Open No. 6-324730 can avoid an abnormal stop due to overspeed, but there is no guarantee that joint acceleration and tip acceleration will be continuous. There was a problem that it might occur.
[0012]
The present invention has been made to solve the above-described problems, and avoids overspeed when performing linear interpolation operation or circular interpolation operation, and smooth interpolation without generating mechanical vibration. The object is to obtain a robot control device that realizes the operation.
[0013]
[Means for Solving the Problems]
A robot control apparatus according to the present invention includes a teaching data storage unit that stores teaching data such as an interpolation command, an operation command speed, and a teaching position, a joint maximum speed storage unit that stores a joint maximum speed of each joint of the robot, A joint speed calculator that calculates a joint command speed based on the teaching data; and a joint speed excess calculator that calculates a speed excess obtained by dividing the joint command speed calculated by the joint speed calculator by the maximum joint speed; A norm calculation unit for calculating a norm of a vector having the degree of excess speed of each joint as an element, a norm calculated by the norm calculation unit, and a function representing an input / output relationship is continuously differentiable, and A correction coefficient calculation unit that outputs a correction coefficient that is a value that is 1 or less when the input is 1 or less, and that is a value that is less than the reciprocal of the input when the input is greater than 1, and is stored in the teaching data storage unit The teaching data and the robot by using said correcting operation command speed coefficient corrected by multiplying the Acceleration is continuous An interpolation calculation unit that generates an operation command is provided.
[0014]
A robot control apparatus according to the present invention includes a maximum tip speed storage unit that stores a maximum value of a tip command speed of a robot, a joint speed calculation unit that calculates a joint command speed based on teaching data, and the teaching data. A tip speed calculation unit that calculates a tip command speed, a joint speed excess degree calculation unit that calculates a speed excess degree by dividing the joint command speed calculated by the joint speed calculation unit by the joint maximum speed, and a robot tip The singular point distance calculation unit that calculates the distance from the singular point and the tip command speed calculated by the tip speed calculation unit are divided by the product of the distance from the singular point and the maximum value of the tip speed of the robot. A tip speed excess degree calculating section for calculating an excess degree of the tip command speed, and a norm calculating section for calculating a norm of a vector having the speed excess degree of each joint and the tip command speed excess as elements. It is a thing.
[0015]
The robot control device according to the present invention is smooth with respect to a joint angle, and takes a zero as a function value at a singular point and takes a non-zero value at a point other than the singular point to determine a distance from the singular point as a singular point distance. The calculation unit is designed to calculate.
[0016]
In the robot control apparatus according to the present invention, the operability of the robot to be controlled is regarded as the distance from the singular point, the operability is calculated, and the distance from the singular point is output by the singular point distance calculation unit. It is what I did.
[0020]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, an embodiment of the present invention will be described.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a configuration of the robot control apparatus according to the first embodiment. In the figure, 1 is a teaching data storage unit for storing teaching data such as an interpolation command, an operation command speed, a teaching position, and the like. An interpolation calculation unit that generates a robot operation command in a Cartesian coordinate system using the teaching data stored in the teaching

data storage unit

1, and 3 uses the operation command generated by the interpolation calculation unit 2 as a position command for each joint. An inverse conversion unit for converting, 4 a joint position command output unit for outputting the obtained joint position command to the servo drive system, and 5 each of the robot joints operating so as to follow the given joint position command. This is a servo drive system that servo-controls joints.
[0021]
11 is a joint maximum speed storage unit that stores the maximum joint speed of each joint of the robot, 51 is a joint speed calculation unit that calculates a joint command speed based on the teaching data stored in the teaching

data storage unit

1, and 52 is a joint speed. A joint speed excess degree calculation unit 53 calculates a speed excess degree for each joint obtained by dividing the joint command speed calculated by the calculation unit 51 by the joint maximum speed stored in the joint maximum

speed storage unit

11, and 53 indicates the speed of each joint. A norm calculation unit 54 that calculates a norm of a vector having super-excess as an element, 54 is a correction coefficient calculation unit that inputs the norm and outputs a correction coefficient. The interpolation calculation unit 2 stores the correction data stored in the teaching data storage unit 1. A robot motion command is generated using the teaching data and the motion command speed corrected by multiplying the correction coefficient.
[0022]
Next, the operation will be described.
When the joint command speed calculated by the joint speed calculation unit 51 is ωi and the joint maximum speed of the i-th joint stored in the joint maximum speed storage unit 11 is ωimax, the joint speed excess degree calculation unit 52 i joint overspeed ξi is
ξi = | ωi | / ωimax (4)
Calculated by
The norm calculation unit 53 calculates a norm ξnorm of a vector ξ = [ξ1 ξ2... ΞN] having this ξi as an element.
ξnorm = pow (pow (ξ1, K) +
・・・・・・ + Pow (ξN, K), 1 / K) (5)
Calculate using. Here, N is the number of joints of the robot, pow (x, y) is a function for obtaining x to the power of y, and K is a real number of 1 or more.
[0023]
Further, the correction coefficient calculation unit 54 inputs the norm ξnorm that is the output of the norm calculation unit 53 and outputs a correction coefficient. As the input / output relationship of the correction coefficient calculation unit 54, for example, a function as shown by a solid line in FIG. 2 is used. The broken line in FIG. 2 indicates a function that outputs 1 when the input ξnorm is 1 or less and outputs the reciprocal when ξnorm is greater than 1. That is, if the function of the broken line is fo (ξnorm),
fo (ξnorm) = 1 (when ξnorm ≦ 1) (6)
fo (ξnorm) = 1 / ξnorm (when ξnorm> 1) (7)
It is.
[0024]
This solid line function representing the input / output relationship of the correction coefficient calculation unit 54 is selected so as to take a value below the broken line and to be smooth (can be continuously differentiated) with respect to ξnorm. The interpolation calculation unit 2 generates an operation command for the robot using the teaching data stored in the teaching data storage unit 1 and the operation command speed corrected by multiplying the correction coefficient output from the correction coefficient calculation unit 54.
[0025]
It can be shown as follows that the joint speed does not exceed the maximum speed by such correction. The joint command speed ωi calculated by the joint speed calculation unit 51 is the speed of each joint when the operation command speed is not corrected, and the joint speed excess degree ξi calculated by the joint speed excess degree calculation unit 52 is the joint command speed ωi. Is a value indicating how many times the maximum joint speed ωimax. Therefore, if the correction coefficient calculated by the correction coefficient calculator 54 is κf, in order to prevent the joint speed from exceeding the maximum speed, all joints are
κf ≦ 1 / ξi (8)
Need to hold. However, since κf is equal to or less than fo in the equations (6) and (7),
κf ≦ fo (9)
It is. Further, fo is obtained from the equations (6) and (7).
fo ≦ 1 / ξnorm (10)
Holds. Furthermore, from equation (5)
ξnorm ≧ ξi (11)
It is. Therefore, Expression (8) is established from Expression (9), Expression (10), and Expression (11), and it can be ensured that the joint speed does not exceed the maximum speed.
[0026]
Further, assuming that the joint command speed calculated by the joint speed calculation unit 51 is smooth with respect to time, ξnorm calculated therefrom is also smooth. Furthermore, since the input / output relationship of the correction coefficient calculation unit 54 is also smooth, the correction coefficient that is the output is also smooth, and it can be expected to avoid the joint acceleration and the tip acceleration from fluctuating discontinuously.
[0027]
FIG. 3 shows a simulation result when the above configuration is used. 3A shows the joint angular velocity, FIG. 3B shows the joint angular acceleration, the solid line shows the movement of the first joint 101, and the broken line shows the movement of the second joint 102. 3C shows the tip speed, FIG. 3D shows the tip acceleration, and FIG. 3E shows the correction coefficient. As shown in FIG. 3A, the joint speed of each joint is 3 rad / s or less, which is the maximum joint speed, and an interpolation operation that does not cause an overspeed is realized. Further, the correction coefficient generated by the correction coefficient calculator 54 varies smoothly as shown in FIG. Correspondingly, the joint acceleration shown in FIG. 3B and the tip acceleration shown in FIG. 3D continuously fluctuate, and the discontinuity as shown in FIG. 17 disappears.
[0028]
In the first embodiment, the joint speed calculation unit 51 uses the teaching data stored in the teaching data storage unit 1 to obtain the joint command speed. However, as in the prior art shown in FIG. It is possible to calculate using the past joint position command, or the joint command speed in the previous sampling cycle may be used for calculation.
[0029]
As described above, according to the first embodiment, the speed excess degree is obtained by dividing the joint command speed of each joint by the joint maximum speed, and the vector norm having the speed excess degree of each joint as an element is calculated. Then, a correction coefficient is obtained from the norm using a function that can be continuously differentiated, and thereby a smooth correction coefficient is obtained and corrected by multiplying the teaching data stored in the teaching data storage unit 1 and the correction coefficient. Because the robot motion command is generated using the command speed, it is possible to avoid speed over when performing linear interpolation operation and circular interpolation operation, and further, discontinuity in joint acceleration and tip acceleration should not occur, There is an effect that a robot controller capable of realizing a smooth interpolation operation without generating mechanical vibration can be obtained.
[0030]
Embodiment 2. FIG.
In general, an articulated robot cannot move in a specific direction because the degree of freedom is reduced at a singular point. Giving a command to move the robot in a direction in which it cannot move mathematically causes the joint to rotate at an infinite speed, but this is not feasible and overspeeds. A similar phenomenon occurs near the singular point. That is, it becomes difficult to move in a specific direction in the vicinity of the singular point, and in order to move in that direction, the joint must rotate at a high speed, which may result in overspeed. In order to pass through a singular point or the vicinity thereof without causing an overspeed, the robot operation command speed must be reduced and the robot must pass at a low speed.
[0031]
In this second embodiment, a speed limit proportional to the distance from the singular point is provided, and the motion command speed of the robot is controlled so as not to exceed this. To suppress rapid fluctuations in acceleration when the robot passes near a singular point.
[0032]
FIG. 4 is a block diagram showing a configuration of the robot control apparatus according to the second embodiment, and the same or corresponding parts as those in FIG. 1 are denoted by the same reference numerals. In the figure, 12 is a maximum tip speed storage unit for storing the maximum tip speed value, 61 is a tip speed calculation unit for calculating a tip command speed based on the teaching data stored in the teaching

data storage unit

1, and 63 is a robot. A singular point distance calculation unit that calculates the distance between the current position and the singular point, 62 is a tip speed excess degree calculation unit that calculates the excess degree of the tip command speed, and 64 is each calculated by the joint speed excess degree calculation unit 52. This is a norm calculation unit that calculates a norm of a vector having the joint speed excess degree and the tip speed excess degree calculation part 62 as an element.
[0033]
Next, the operation of each block will be described.
The maximum tip speed storage unit 12 stores the maximum tip speed that can be realized in most of the operation region other than the special region such as the vicinity of the singular point, and this value is set as Vmax. The singular point distance calculation unit 63 calculates the distance D between the current position of the robot and the singular point.
D = | g (θ) | (12)
Calculated by However, θ is a vector representing the joint angle of the robot, the function g (θ) is a function of θ, is smooth with respect to the joint angle, and is a function that takes a value other than zero at a singular point and a value other than zero at a point other than a singular point. is there. As the function g (θ), a Jacobian determinant that relates the joint speed and the tip speed, the maneuverability of the robot, and the like can be used, but other functions may be used.
[0034]
In the tip speed excess degree calculation unit 62, the tip command speed V calculated by the tip speed calculation unit 61, the tip maximum speed Vmax stored in the tip maximum speed storage unit 12, and the distance obtained by the singular point distance calculation unit 63. Using D, the tip command speed excess degree ξo is
ξo = V / (KdDVmax) (13)
Calculated by Here, KdDVmax is the maximum value of the tip velocity near the singular point, takes a value proportional to the distance from the singular point, and Kd is an appropriate real number. This is to provide a speed limit proportional to the distance from the singular point, and to control the robot's motion command speed so that it does not exceed this, gradually reducing the motion command speed as the singular point is approached. is there. As a result, rapid speed fluctuations in the vicinity of the singular point can be avoided, and the singular point and its vicinity can be passed smoothly.
[0035]
The norm calculation unit 64 calculates a norm ξnorm of a vector ξ = [ξo ξ1 ξ2... ΞN] whose elements are the ξo and the joint speed excess degree ξi obtained by the joint speed excess degree calculation unit 52.
ξnorm = pow (pow (ξ0, K) +
・・・・・・ + Pow (ξN, K), 1 / K) (14)
Calculate according to Here, N is the number of joints of the robot, pow (x, y) is a function for obtaining x to the power of y, and K is a real number of 1 or more.
Thereafter, as in the first embodiment, the correction coefficient calculation unit 54 calculates a correction coefficient, and the interpolation calculation unit 2 generates a robot motion command.
[0036]
FIG. 5 shows a simulation result when the above configuration is used. Similar to FIG. 3 showing the simulation result according to the first embodiment, the joint speed is equal to or lower than the joint maximum speed, and the correction coefficient (FIG. 5 (e)) and joint acceleration (FIG. 5 (b)). The tip acceleration (FIG. 5D) is smooth, and a smooth operation is realized.
[0037]
In FIG. 3 (d) showing the simulation result according to the first embodiment, the tip acceleration fluctuates abruptly from time 0.7 seconds to 0.8 seconds. This is because the robot passes near the singular point. Because.
On the other hand, in the second embodiment, since the operation command speed is limited by the distance from the singular point, the simulation result shown in FIG. It can be seen that there is no fluctuation and that the singularity is passed more smoothly.
[0038]
As described above, according to the second embodiment, the tip command speed excess degree is obtained by dividing the tip command speed of the robot tip by the product of the distance from the singular point and the maximum value of the tip command speed, Calculate the norm of the vector whose elements are super-excess and excess of the tip command speed, determine the correction coefficient using a function that can be continuously differentiated from the norm, and correct the multiplication by the teaching data and the correction coefficient In order to generate a robot motion command using the command speed, in addition to the effects of the first embodiment, the robot motion speed is slowed down without causing an overspeed, and a singular point or its vicinity can be detected at a low speed. There is an effect that a robot control device capable of passing can be obtained.
[0039]
Embodiment 3 FIG.
In the robot control apparatus according to the third embodiment, a smooth correction coefficient is obtained by passing κ obtained by the equations (1) to (3) described in the above prior art as a correction coefficient as it is, and by passing it through a low-pass filter. Further, the delay of the speed correction timing due to the delay of the low-pass filter at this time is eliminated.
[0040]
FIG. 6 is a block diagram showing a configuration of the robot control apparatus according to the third embodiment, and the same or corresponding parts as those in FIG. 1 are denoted by the same reference numerals. In the figure, 31 calculates a joint command speed for each joint corresponding to a time that is a fixed time ahead of the current time (current sampling cycle, calculation cycle) based on the teaching data stored in the teaching data storage unit 1. It is a joint speed calculating part.
For a joint in which the joint command speed calculated by the joint speed calculation unit 31 exceeds the joint maximum speed stored in the joint maximum speed storage unit 11, a value obtained by dividing the joint maximum speed by the joint command speed is the speed. A speed ratio calculation unit that calculates a speed ratio that is 1 if the ratio is not exceeded, and 33 is a speed ratio minimum value calculation that calculates the minimum value from the speed ratios calculated by the speed ratio calculation unit 32 for each joint. Part. 34 is a correction coefficient for calculating a correction coefficient by passing the minimum value obtained by the speed ratio minimum value calculation unit 33 through a low-pass filter having a delay amount matching the certain time from the current time to the previous time. It is a calculation part.
[0041]
Next, the operation will be described.
When the current time is t, the joint speed calculation unit 31 calculates future joint command speeds for each joint at a time ahead of the current time by a fixed time Δt based on the teaching data stored in the teaching data storage unit 1. . The joint command speed of the i-th joint calculated by the joint speed calculation unit 31 is assumed to be ωi (t + Δt). The speed ratio calculation unit 32 calculates the speed ratio ηi of the i-th joint,

Seek like.
[0042]
Here, ωimax is the joint maximum speed of the i-th joint stored in the joint maximum speed storage unit 11. That is, the speed ratio ηi is a value obtained by dividing the joint maximum speed ωimax by the joint command speed ωi for a joint in which the joint command speed ωi exceeds the joint maximum speed ωimax.
[0043]
The speed ratio minimum value calculation unit 33 obtains the minimum value ηmin from the speed ratio ηi of each joint. That is,
ηmin (t + Δt) = min (ηi (t + Δt) | 1 ≦ i ≦ N) (17)
It is.
In the correction coefficient calculation unit 34, the correction coefficient κ is obtained by passing ηmin (t) thus obtained through a low-pass filter. Various filters can be used as the low-pass filter. For example, when a moving average filter is used, the correction coefficient κ is calculated by the following equation (18). Where T is the filter time length.
[Expression 1]

[0044]
In general, a low-pass filter causes a time delay in input and output. The reason why the joint speed calculation unit 31 calculates the future joint command speed for a fixed time Δt is to correct the delay due to this filter, and if Δt is set to be equal to the delay of the low-pass filter. , Compensation without time delay can be performed. Since the delay of the moving average filter is half of the time length T, Δt = T / 2. The interpolation calculation unit 2 generates an operation command for the robot using the teaching data stored in the teaching data storage unit 1 and the operation command speed corrected by multiplying by the correction coefficient κ that is the output of the correction coefficient calculation unit 34.
[0045]
A simulation result by the robot control apparatus according to the third embodiment is shown in FIG. As shown in FIG. 7E, a smooth correction coefficient is obtained, and it can be seen from FIGS. 7B and 7D that both the joint acceleration and the tip acceleration are continuous. Further, from FIG. 7 (a), the joint command speed slightly exceeds the joint maximum speed to some extent, and the restriction on the joint maximum speed is substantially satisfied.
[0046]
As described above, according to the third embodiment, κ obtained by the equations (1), (2), and (3) described in the prior art is not used as a correction coefficient as it is, but a low-pass filter. In this case, since the timing of the speed correction is delayed due to the delay of the filter, the delay of the filter is corrected by calculating the expressions (15) and (16) using the joint command speed of a certain time in the future, and smooth It is possible to realize a robot control device that can obtain a correct correction coefficient.
[0047]
Embodiment 4 FIG.
The robot control apparatus according to the fourth embodiment calculates a joint command speed for each joint of the robot corresponding to a time that is a fixed time ahead of the current time (current sampling cycle, calculation cycle) based on the teaching data. For a joint whose joint command speed exceeds the maximum joint speed, a value obtained by dividing the maximum joint speed by the joint command speed is used as a speed ratio. If not, a speed ratio of 1 is obtained for each joint. The minimum value is obtained from the obtained speed ratio, the minimum value of the speed ratio obtained for each calculation period from the current time until the fixed time elapses is stored, and the stored minimum speed ratio is stored. A minimum value is obtained from the values and output, and this output is output to a low-pass filter having a delay amount substantially equal to half of the predetermined time from the current time to the previous time. Then, a correction coefficient is calculated, a robot motion command is generated using the teaching data and the motion command speed corrected by multiplying the correction coefficient, and an interpolation operation that strictly satisfies the restriction of the joint maximum speed of the joint is performed. It is realized.
[0048]
FIG. 8 is a block diagram showing the configuration of the robot control apparatus according to the fourth embodiment, in which the same or corresponding parts as those in FIG. 6 of the third embodiment are given the same reference numerals. In the figure, 31 is a joint speed calculation unit for calculating the joint command speed of each future joint at a time ahead of the predetermined time Δt from the current time based on the teaching data stored in the teaching

data storage unit

1, and 32 is the joint For a joint in which the joint command speed calculated by the speed calculation unit 31 exceeds the joint maximum speed stored in the joint maximum speed storage unit 11, a value obtained by dividing the joint maximum speed by the joint command speed is used as a speed ratio and does not exceed it. In this case, a speed ratio calculating unit 33 obtains a speed ratio of 1, and 33 is a speed ratio minimum value computing unit that obtains the minimum value from the speed ratios obtained by the speed ratio calculating unit 32 for each joint.
41 is the minimum value of the speed ratio obtained by the operation of the joint speed calculation unit 31, the speed ratio calculation unit 32, and the speed ratio minimum value calculation unit 33 for each calculation cycle from the current time until the fixed time Δt elapses. Is a speed ratio minimum value storage unit. 42 is a minimum value detector for obtaining and outputting the minimum value in the data stored in the speed ratio minimum

value storage unit

41, and 43 is a correction coefficient obtained by passing the output of the minimum value detector 42 through a low-pass filter. It is a correction coefficient calculation unit to calculate.
[0049]
The speed ratio minimum value storage unit 41 stores the minimum value ηmin in the speed ratio ηi for each joint calculated by the speed ratio minimum value calculation unit 33 during the past Δt. When the current time is t, ηmin calculated at time t−Δt is ηmin (t), and ηmin calculated at current time t is ηmin (t + Δt). Values from ηmin (t) to ηmin (t + Δt) are stored. The minimum value detection unit 42 obtains and outputs the minimum value among ηmin stored in the speed ratio minimum value storage unit 41. When the output of the minimum value detector 42 is ηlow,
ηlow (t) = min (ηmin (τ) | t ≦ τ ≦ t + Δt) (19)
It becomes. In the correction coefficient calculation unit 43, the correction coefficient κ is obtained by passing ηlow (t) thus obtained through a low-pass filter.
[0050]
9A is ηmin obtained by the speed ratio minimum value calculation unit 33, the solid line in FIG. 9B is ηlow that is the output of the minimum value detection unit 42, and the solid line in FIG. 9C is The correction coefficient (kappa) which the correction coefficient calculating part 43 outputs is shown. The broken lines in FIGS. 9B and 9C simultaneously show ηmin in FIG. 9A for comparison. Ηlow in FIG. 9B has a shape in which the portion of the minimum value of ηmin in FIG. 9A is flattened, and this is passed through a low-pass filter by the correction coefficient calculation unit 43, so that FIG. c) Smooth and at all times
κ ≦ ηmin (20)
A correction coefficient κ is obtained.
[0051]
In the fourth embodiment, a moving average filter can be used in place of the low-pass filter, and the fixed time Δt is set to the window length of the moving average filter (the filter time length T in the third embodiment). ), And the delay in this moving average filter is Δt / 2. Therefore, the output of the minimum value detector 42 shown by the solid line in FIG. 9B is delayed by Δt / 2, As shown by the solid line in FIG. 9C, the correction coefficient κ which is smooth and can be obtained from the equation (20) at all times can be obtained more reliably.
[0052]
FIG. 10 shows ηmin and correction coefficient κ in the third embodiment for comparison with the fourth embodiment. The broken line in the figure is ηmin, and the solid line is the correction coefficient κ. In the third embodiment, ηmin is passed through the low-pass filter by the correction coefficient calculation unit 34 to obtain the correction coefficient κ.
κ> ηmin (21)
At this time, the joint speed slightly exceeded the maximum joint speed. In the fourth embodiment, a speed ratio minimum value storage unit 41 and a minimum value detection unit 42 are added to obtain ηlow shown in FIG. 9B from the current time to a certain time Δt ahead, and this is calculated as a correction coefficient. The unit 43 is configured to pass through a low-pass filter or a moving average filter, so that κ ≦ ηmin is always satisfied, and the joint command speed does not exceed the maximum joint speed.
[0053]
Next, the simulation result according to the fourth embodiment is shown in FIG. As shown in FIG. 11 (e), a smooth correction coefficient is obtained, and it can be seen from FIG. 11 (b) and FIG. 11 (d) that both the joint acceleration and the tip acceleration are continuous. Moreover, when Fig.11 (a) is seen, the joint command speed is also below the joint maximum speed.
[0054]
As described above, according to the fourth embodiment, the correction coefficient κ for each joint is obtained using the joint command speed of a certain time in the future, and further, every calculation cycle until the certain time elapses from the current time. The minimum value of the speed ratio within the predetermined time obtained by the operation of the joint speed calculation unit 31, the speed ratio calculation unit 32, and the speed ratio minimum value calculation unit 33 is stored in the speed ratio minimum value storage unit 41, and the speed The minimum value among the speed ratios stored in the ratio minimum value storage unit 41 is detected by the minimum value detection unit 42, and this is input to the low-pass filter or moving average filter by the correction coefficient calculation unit 43, thereby correcting the correction coefficient. Since the joint command speed may slightly exceed the joint maximum speed in the third embodiment to calculate and calculate, the robot control can obtain an interpolation operation that strictly satisfies the maximum joint speed constraint. There is an effect that location can be realized.
[0055]
【The invention's effect】
As described above, according to the present invention, the maximum joint speed storage unit that stores the maximum joint speed of each joint of the robot, the joint speed calculation unit that calculates the joint command speed based on the teaching data, and the joint speed calculation A joint speed excess degree calculation unit that calculates a speed excess degree obtained by dividing the joint command speed calculated by the unit by the maximum joint speed, and a norm calculation part that calculates a norm of a vector having the speed excess degree of each joint as an element And the norm calculated by the norm calculation unit as an input, the function representing the input / output relationship is continuously differentiable, and when the input is 1 or less, the value is 1 or less, and when the input is greater than 1, the function Using a correction coefficient calculation unit that outputs a correction coefficient that is a value equal to or less than the reciprocal of the input, and an operation command speed corrected by multiplying the teaching data stored in the teaching data storage unit and the correction coefficient, Acceleration is continuous Since it is configured to include an interpolation calculation unit that generates an operation command, it is possible to avoid speed over when performing linear interpolation operation and circular interpolation operation, joint acceleration and tip acceleration become continuous, mechanical vibration does not occur, There is an effect that a smooth interpolation operation can be realized.
[0056]
According to this invention, the tip maximum speed storage unit that stores the maximum value of the tip command speed of the robot, the joint speed calculation unit that calculates the joint command speed based on the teaching data, and the tip command speed based on the teaching data. A tip speed calculation unit that calculates a joint speed excess degree calculation unit that calculates a degree of speed excess by dividing the joint command speed calculated by the joint speed calculation unit by the joint maximum speed, and a robot tip The singular point distance calculation unit that calculates the distance from the singular point and the tip command speed calculated by the tip speed calculation unit are divided by the product of the distance from the singular point and the maximum value of the tip command speed of the robot. A tip speed excess degree calculating unit that calculates an excess degree of the tip command speed, and a norm calculation unit that calculates a norm of a vector having the speed excess degree of each joint and the tip command speed excess degree as elements. Therefore, it is possible to avoid over-speed when performing linear interpolation operation or circular interpolation operation, joint acceleration and tip acceleration are continuous, mechanical vibration does not occur, smooth interpolation operation can be realized, and unique There is an effect that it is possible to generate an operation command that smoothly passes through a point and its vicinity.
[0057]
According to the present invention, the distance from the singular point is calculated with a function that is smooth with respect to the joint angle and takes a zero as a function value at a singular point and takes a non-zero value at a point other than the singular point. Therefore, it is possible to avoid an abrupt speed fluctuation in the vicinity of the singular point and to realize an interpolation operation that can smoothly pass the singular point and the vicinity thereof.
[0058]
According to the present invention, the operability of the robot to be controlled is regarded as the distance from the singular point, and the operability is calculated and the distance from the singular point is output, so the vicinity of the singular point In this way, it is possible to avoid an abrupt speed fluctuation at the point, and to realize an interpolation operation that can smoothly pass the singular point and the vicinity thereof.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a configuration of a robot control apparatus according to a first embodiment of the present invention.
FIG. 2 is an explanatory diagram illustrating, in a graph, functions that define the input / output relationship of a correction coefficient calculation unit in the robot control apparatus according to the first embodiment of the present invention.
FIG. 3 is a characteristic diagram such as a joint angular velocity, which is a simulation result for the robot control apparatus according to the first embodiment of the present invention.
FIG. 4 is a block diagram showing a configuration of a robot control apparatus according to a second embodiment of the present invention.
FIG. 5 is a characteristic diagram such as a joint angular velocity which is a simulation result for the robot control apparatus according to the second embodiment of the present invention.
FIG. 6 is a block diagram showing a configuration of a robot control apparatus according to a third embodiment of the present invention.
FIG. 7 is a characteristic diagram such as a joint angular velocity as a simulation result for the robot control apparatus according to the third embodiment of the present invention.
FIG. 8 is a block diagram showing a configuration of a robot control apparatus according to a fourth embodiment of the present invention.
FIG. 9 shows ηmin obtained by the speed ratio minimum value calculation unit of the robot control apparatus according to Embodiment 4 of the present invention, ηlow that is the output of the minimum value detection unit, correction coefficient κ output by the correction coefficient calculation unit, and the like. It is explanatory drawing which shows the example of a calculation.
FIG. 10 is an explanatory diagram showing a calculation example of the minimum value ηmin and the correction coefficient κ of the speed ratio of each joint in the robot control device of the third embodiment for comparison with the robot control device of the fourth embodiment of the present invention. is there.
FIG. 11 is a characteristic diagram such as a joint angular velocity as a simulation result for the robot control apparatus according to the fourth embodiment of the present invention.
FIG. 12 is a block diagram showing a configuration of a conventional robot control device.
FIG. 13 is a configuration diagram of the degree of freedom of the robot used for the simulation of the conventional robot control device.
FIG. 14 is an explanatory diagram showing an operation of a robot used for a simulation of a conventional robot control device.
FIG. 15 is a characteristic diagram such as a joint angular velocity, which is a simulation result when an interpolation method of a conventional robot control device is used.
FIG. 16 is a control block diagram of a technique disclosed in Japanese Patent Laid-Open No. 6-324730 as a conventional robot control device.
FIG. 17 is a characteristic diagram such as a joint angular velocity which is a simulation result of a technique disclosed in Japanese Patent Laid-Open No. 6-324730 as a conventional robot control device.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Teaching data memory | storage part, 2 Interpolation calculating part, 11 Joint maximum speed memory | storage part, 12 Tip maximum speed memory | storage part, 31 Joint speed calculating part, 32 Speed ratio calculating part, 33 Speed ratio minimum value calculating part, 34 Correction coefficient calculating part 41 speed ratio minimum value storage unit, 42 minimum value detection unit, 43 correction coefficient calculation unit, 51 joint speed calculation unit, 52 joint speed excess calculation unit, 53 norm calculation unit, 54 correction coefficient calculation unit, 61 tip speed calculation Part, 62 tip speed excess degree calculating part, 63 singular point distance calculating part, 64 norm calculating part, 101 first joint, 102 second joint.

Claims

In robot control devices with interpolation functions such as linear interpolation and circular interpolation,
A teaching data storage unit for storing teaching data such as an interpolation command, an operation command speed, and a teaching position;
A joint maximum speed storage unit for storing the joint maximum speed of each joint of the robot;
A joint speed calculator for calculating a joint command speed based on the teaching data;
A joint speed excess degree calculation unit that calculates a speed excess degree obtained by dividing the joint command speed calculated by the joint speed calculation unit by the maximum joint speed;
A norm calculation unit for calculating a norm of a vector having the degree of overspeed of each joint as an element;
When the norm calculated by the norm calculation unit is input, the function representing the input / output relationship is continuously differentiable, and when the input is 1 or less, the value is 1 or less, and when the input is greater than 1, the input A correction coefficient calculator that outputs a correction coefficient that is a reciprocal value or less;
An interpolation calculation unit that generates an operation command in which the acceleration of the robot is continuous using the teaching data stored in the teaching data storage unit and the operation command speed corrected by multiplying by the correction coefficient is provided. Robot control device.

In robot control devices with interpolation functions such as linear interpolation and circular interpolation,
A teaching data storage unit for storing teaching data such as an interpolation command, an operation command speed, and a teaching position;
A joint maximum speed storage unit for storing the joint maximum speed of each joint of the robot;
A maximum tip speed storage unit that stores the maximum value of the tip command speed of the robot;
A joint speed calculator for calculating a joint command speed based on the teaching data;
A tip speed calculator for calculating a tip command speed based on the teaching data;
A joint speed excess degree calculation unit that calculates a degree of speed excess by dividing the joint command speed calculated by the joint speed calculation unit by the maximum joint speed;
A singular point distance calculation unit for calculating the distance from the singular point of the robot tip ;
A tip speed excess degree calculation unit that calculates the degree of excess of the tip command speed by dividing the tip command speed calculated by the tip speed calculation unit by the product of the distance from the singular point and the maximum value of the tip command speed of the robot When,
A norm calculation unit that calculates a norm of a vector whose elements are the degree of excess speed of each joint and the degree of excess of the tip command speed;
When the norm is an input, the function representing the input / output relationship is continuously differentiable, and when the input is 1 or less, the value is 1 or less, and when the input is greater than 1, the value is less than the reciprocal of the input. A correction coefficient calculator that outputs a correction coefficient;
An interpolation calculation unit that generates an operation command in which the acceleration of the robot is continuous using the teaching data stored in the teaching data storage unit and the operation command speed corrected by multiplying by the correction coefficient is provided. Robot control device.

The singularity distance calculation unit
The distance from the singular point is calculated by a function that is smooth with respect to the joint angle and takes a zero as a function value at a singular point and takes a non-zero value at a point other than the singular point. Robot control device.

The singularity distance calculation unit
3. The robot control apparatus according to claim 2, wherein the operable degree of the robot to be controlled is regarded as a distance from a singular point, and the operable degree is calculated and output.