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