速度制御方法及び速度制御装置
【課題】物体の通過する軌跡上の各点における曲率に応じた速度で物体を移動させる速度制御方法の提供。
【解決手段】軌跡の座標データを入力、軌跡上の各点における曲率を計算、算出された各点での曲率から各点での物体の速度を（１）式に示される角速度を用いた（２）式にて計算、算出された各点での速度を物体に出力。
【解決手段】軌跡の座標データを入力、軌跡上の各点における曲率を計算、算出された各点での曲率から各点での物体の速度を（１）式に示される角速度を用いた（２）式にて計算、算出された各点での速度を物体に出力。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の速度（すなわち、軌跡の接線速度をいう）を制御する方法及びその装置に関する。
【背景技術】
【０００２】
従来、ロボットハンドの手先や搬送台車等の物体を移動させる場合、物体の移動の開始時及び終了時には、物体の速度が所定速度となるまで加速度を一定とし、物体が所定速度となった際には、加速度を０、すなわち、速度を一定とする台形型の速度制御が行われている（例えば、特許文献１参照）。
【０００３】
【特許文献１】特開平１−２３７８０６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、前記従来のような台形型の速度制御方法では、物体が移動する経路（軌跡）が曲線である場合や角部を有する場合等の物体の経路が大きく曲がっている場所、すなわち、曲率の大きい場所においても速度が一定であるため、物体が高速で移動すると、遠心力が大きくなり、決められた軌道からずれてしまうという問題があった。
【０００５】
本発明はかかる事情に鑑みてなされたもので、物体の通過する軌跡上の各点における曲率に応じた速度で物体を移動させる速度制御方法及びその装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
前記目的に沿う本発明に係る速度制御方法は、予め設定された軌跡を通過する物体の速度制御方法であって、
前記軌跡の座標データを入力する第１工程と、
前記軌跡上の各点における曲率を計算する第２工程と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する第３工程と、
前記算出された各点での速度を前記物体に出力する第４工程とを有する。
【０００７】
【数１】
【０００８】
ここで、αは、１未満であり、好ましくは、１／３を超えかつ４／５未満、より好ましくは、１／２を超えかつ３／４未満である。この範囲内において、αが１に近い場合では曲率の小さい場所と曲率の大きい場所での速度（すなわち、接線速度）の差が人間の動作で観察されるより小さくなり、人型ロボットに応用した際には動きが不自然となり、αが１の場合には、曲率ｃの大小に関わらず常に物体の速度ｖが一定となり、αが１を超える場合には、曲率が大きくなると、速度も大きくなる。また、αが１／３以下の場合には、曲率の変化に対して、速度の変化が大きくなり過ぎる。なお、本発明において、軌跡とは、物体（運動部品上の１点、一般には、機能点又は代表点）が運動中に空間に占める位置を連ねた線のことをいう。
【０００９】
本発明に係る速度制御方法において、前記物体の前記軌跡上の各点での速度を算出して、該物体の軌道生成を行うことができる。
本発明に係る速度制御方法において、前記第１工程で前記物体の最大速度を入力し、前記第３工程で前記（２）式によって算出された前記各点での前記物体の速度のデータが前記最大速度を超える場合、該物体の速度を前記最大速度以下に補正するのが好ましい。
本発明に係る速度制御方法において、前記第３工程では、前記（２）式による前記物体の速度の変化率（すなわち、加速度）が所定値以下となるように、前記物体の速度を補正してもよい。
【００１０】
本発明に係る速度制御方法において、前記第２工程では、前記曲率の変化率が所定値以下となるように、前記軌跡上の各点での曲率を補正した後、前記第３工程で該補正した曲率から前記物体の速度を計算してもよい。
本発明に係る速度制御方法において、前記（２）式では、αが２／３であるのが好ましい。ここで、αが２／３である場合、（１）式は、ビビアーニ（Viviani P.）らが人間の手先の動かし方の観察から見出した心理物理的な経験則である三分の二乗則（The 2/3 power low）に従うので、（１）式に示される角速度を用いた（２）式も三分の二乗則に従う。
本発明に係る速度制御方法を、ロボットアーム又は搬送車（物体の一例）の速度を制御するのに用いてもよい。
ここで、搬送車としては、搬送台車や、レール上を移動しない無軌道台車等がある。
【００１１】
前記目的に沿う本発明に係る速度制御装置は、予め設定された軌跡を通過する物体の速度制御装置であって、
前記軌跡の座標データを入力する初期値入力部と、
前記軌跡上の各点における曲率を計算する曲率計算部と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する速度計算部と、
前記算出された各点での速度を前記物体に出力する出力部とを有する。
本発明に係る速度制御装置において、前記出力部はロボットアーム又は搬送車（物体の一例）に前記算出された各点での速度を出力してもよい。
【発明の効果】
【００１２】
請求項１〜７に記載の速度制御方法においては、入力された軌跡の座標データから、軌跡上の各点における曲率を計算し、この算出された各点での曲率から各点での物体の速度を計算するので、曲率に応じた速度で物体を移動させることができ、特に軌跡が大きく曲がっている所では、物体の速度を遅くして、予め決められた軌跡からずれ難くなる。
特に、請求項２記載の速度制御方法においては、物体の軌跡上の各点での速度を算出して、物体の軌道生成を行うので、軌道上の各点での物体の速度を制御する軌道を作成することができる。
請求項３記載の速度制御方法においては、第３工程で算出された各点での物体の速度のデータが入力した最大速度を超える場合、物体の速度を最大速度以下に補正するので、特に、曲率が小さい場所で速度超過とならない。
【００１３】
請求項４記載の速度制御方法においては、第３工程で物体の速度の変化率が所定値以下となるように物体の速度を補正するので、物体が急激に加減速しないように速度を調整することができる。
請求項５記載の速度制御方法においては、第２工程で曲率の変化率が所定値以下となるように軌跡上の各点での曲率を補正した後、第３工程で補正した曲率から物体の速度を計算するので、物体が急激に加減速しないように速度を調整することができる。
請求項６記載の速度制御方法においては、αが２／３であるので、物体の動きが、三分の二乗則に従って滑らかになり、特に人型のロボットでは人の動作に近づかせることができる。
【００１４】
請求項８及び９に記載の速度制御装置においては、入力された軌跡の座標データから、軌跡上の各点における曲率を計算する曲率計算部と、算出された各点での曲率から各点での物体の速度を計算する速度計算部とを有するので、曲率に応じた速度で物体を移動させることができ、特に軌跡が大きく曲がっている所では、物体の速度を遅くして、予め決められた軌跡からずれ難くすることができる。
【発明を実施するための最良の形態】
【００１５】
続いて、添付した図面を参照しつつ、本発明を具体化した実施の形態につき説明し、本発明の理解に供する。
ここで、図１は本発明の一実施の形態に係る速度制御装置のブロック図、図２は本発明の一実施の形態に係る速度制御方法での物体の軌跡を示す説明図、図３は同速度制御方法での物体の経路長と曲率の関係を示すグラフ、図４は同速度制御方法での物体の経路長と接線速度の関係を示すグラフ、図５（Ａ）、（Ｂ）はそれぞれ同速度制御方法での接線速度を補正した場合の説明図、図６（Ａ）〜（Ｄ）はそれぞれ同速度制御方法での曲率を補正した場合の説明図、図７（Ａ）、（Ｂ）はそれぞれ同速度制御方法を適用したシミュレーションでの物体の概念図、二次系システムの説明図、図８は物体の速度制御装置に予め与えられた物体の軌道を示すグラフ、図９は曲率の計算結果を示すグラフ、図１０は物体の速度の計算結果を示すグラフ、図１１は物体を移動させた際の軌跡を示すグラフである。
【００１６】
図１〜図４を参照して、本発明の一実施の形態に係る物体の速度制御装置１０について説明する。
図１に示すように、速度制御装置１０は、図示しない物体（例えば、ロボットハンドの手先）が通過する予め設定された軌跡Ａ（図２参照）及び物体の最大速度ｖmaxを入力する初期値入力部１１と、初期値入力部１１に入力された軌跡Ａ上の各点Ｘにおける曲率ｃを計算する曲率計算部１２と、初期値入力部１１に入力された最大速度ｖmax及び曲率計算部１２で計算された曲率ｃに基づいて、各点Ｘでの物体の速度ｖを計算する速度計算部１３と、速度計算部１３で算出された速度ｖを物体の制御部（図示せず）に出力する出力部１４とを有し、物体が軌跡Ａに沿って速く、かつ、正確に移動するように物体の速度の制御を行っている。
【００１７】
次に、速度制御装置１０を使用した物体の速度制御方法について説明する。
（第１工程）
まず、初期値入力部１１に、軌跡Ａ及び物体の最大速度ｖmaxを入力する。ここで、図２に示すように、軌跡Ａは、中間点Ｍで連結された２本の直線からなり、中間点Ｍで折れ曲がって角部が形成された構造となっている。なお、軌跡Ａは、二次元であって、軌跡Ａ上の始点Ｘ0（ｘ0，ｙ0）から終点Ｘ1（ｘ1，ｙ1）までの複数の点Ｘi（ｘi，ｙi）（但し、０≦ｉ≦１である）によって、軌跡Ａの座標データが構成されている。初期値入力部１１には、これら軌跡Ａ上の全ての点の座標のデータが入力される。なお、軌跡は、三次元で与えられてもよく、また、ベジエ曲線等のパラメトリック曲線等の数式で与えてもよい。初期値入力部１１から曲率計算部１２に軌跡Ａ上の各点の座標を示す数値が送られ、速度計算部１３に最大速度ｖmaxを示す数値が送られる。
【００１８】
（第２工程）
次に、曲率計算部１２で軌跡Ａ上の各点Ｘにおける曲率をそれぞれ計算する。
まず、物体が軌跡Ａ上を始点Ｘ0から終点Ｘ1まで一定速度で進むと仮定し、軌跡Ａ上の点を微小な刻み時間δｔでサンプリングした際の時刻ｔでの速度ｖ（ｔ）を（３）式によって求める。また、加速度も同様に計算できるので、各点での曲率ｃ（ｔ）は（４）式によって求められる。
【００１９】
【数２】
【００２０】
ここで、軌跡Ａ上の任意の点Ｘの始点Ｘ0からの距離を経路長ｓとして、刻み時間δｔの間に刻み幅δｓで軌跡Ａ上の点をサンプリングし、時刻ｔに経路長ｓの点Ｘに到達する場合、経路長ｓの位置での接線速度ｖ（ｓ）は、（５）式で示される。従って、軌跡Ａのみが与えられた場合の経路長ｓの位置での曲率ｃ（ｓ）は（６）式で示され、軌跡Ａの経路長ｓでの曲率ｃ（ｓ）は図３のように示される。求められた曲率ｃ（ｓ）のデータは、速度計算部１３に送られる。
【００２１】
【数３】
【００２２】
（第３工程）
ここで、（５）式は、等速度運動であるので、各点Ｘでの曲率ｃ（ｓ）に応じた接線速度ｖd（ｓ）を（７）式によって計算する（図４では、一点鎖線で示す）。なお、パラメータをｕとした場合の物体の速度ｖ（ｕ）は、（１）式に示される角速度ωを用いた（２）式によって計算されており、（２）式のパラメータを経路長ｓに置き換えることにより、（７）式が得られる。また、曲率ｃが曲率半径ｒの逆数であるので、速度ｖd（ｓ）を（８）式で表すこともできる。また、速度ｖd（ｓ）の大きさは速度ｖ（ｓ）と異なるが、それらの方向は一致するので、速度ｖd（ｓ）のｘ方向成分ｘd（ｓ）及びｙ方向成分ｙd（ｓ）は、（９）式及び（１０）式で示される。
このように、物体の速度ｖd（ｓ）は、曲率ｃ（ｓ）に応じて決定されるので、軌跡Ａが大きく曲がっている、すなわち、曲率ｃが大きい所では、物体の接線速度を遅くして、物体が遠心力によって軌跡Ａからずれるのを防止することができる。
【００２３】
【数４】
【００２４】
ここで、（７）式において、αは、１未満であり、好ましくは、１／３を超えかつ４／５未満、より好ましくは、１／２を超えかつ３／４未満である。また、αを２／３とした場合には、（１）式に示す角速度ωが、人の手先の動かし方の観察から得られた心理物理的な経験則である三分の二乗則に従うので、（１）式に示される角速度ωを用いた（２）式も三分の二乗則に従う。これによって、（２）式によって計算された速度ｖで物体を動かした場合、物体が人の動きに近づいて滑らかに動く。なお、αが１／３以下の場合には、曲率がさほど大きくない場所でも速度が小さくなり過ぎ、αが１である場合には、速度が曲率に依らず一定となり、αが１を超える場合には、曲率が大きくなる、すなわち、曲率半径ｒが小さくなるに従って速度が大きくなるので不適である。
【００２５】
更に、速度計算部１３では、速度ｖd（ｓ）が初期値入力部１１に入力した物体の最大速度ｖmaxを超えている場合、速度ｖd（ｓ）を最大速度ｖmax以下（例えば、最大速度ｖmax）にする。これによって、物体が移動する速度を抑えることができ、特に、曲率ｃの大きい場所、すなわち、曲率半径の小さい場所での物体の速度を遅くして、遠心力による軌跡からのずれを防止することができる。このようにして、図４に示すように、経路長ｓと速度ｖd（ｓ）のグラフが求められる。
【００２６】
（第４工程）
速度計算部１３で算出された速度を出力部１４から物体の動きを制御する制御部に出力する。物体の制御部は、与えられた速度で物体を移動させるので、物体が軌跡Ａに沿って速く、かつ、正確に移動することができる。以上のようにして、軌道生成を行なう。
【００２７】
（第１の補正の方法）
図４に示すように、始点及び終点では、曲率の値に関わらず加減速が行われる。また、軌跡Ａが急に折れ曲がっている、すなわち、曲率ｃが急峻に大きくなる場所（中間点Ｍ）でも、物体の速度の変化率（加速度）が大きくなり、急激な加減速を行わなければならない。ここで、物体の速度の変化率とは、加速度であって、図４においてグラフの傾きで示される。
【００２８】
このように物体が急激に加減速する場所では、物体が軌道上を空回りして速度が遅くなったり、遠心力によって軌跡からずれることがあるので、軌跡上を移動する物体の速度の変化率を所定値（許容値）以下に（すなわち、図４においては、傾きを小さく）して、物体が軌跡上を速くかつ正確に移動できるように、速度の変化率の大きいところの前後の速度を補正する。なお、速度の変化率の所定値は、物体によっても、また、同じ物体でも設定する軌跡によっても異なる。この速度の変化率（加速度）の所定値は、工作機械などでは１０Ｇ（Ｇは重力加速度を示す）を超えることも可能であるが、人間が乗車する車両等では数Ｇ以内が好ましい。
【００２９】
そこで、第３工程では、速度計算部１３において、速度ｖd（ｓ）の変化率の大きいところ（図４において、傾きの大きい場所）の前後に加減速区間Ｂを設けて、急激な加減速が起きないようにする。加減速区間Ｂでは、例えば、図５（Ａ）に示すように、速度ｖd（ｓ）を多項式で変換してＳ字状に補正してもよく、図５（Ｂ）に示すように、速度ｖd（ｓ）を直線的に補正し、物体の急激な加減速を防止する。これによって、物体は、軌跡上を速くかつ正確に移動することができる。
【００３０】
（第２の補正の方法）
また、図３に示すように、軌跡Ａ上の曲率ｃが急峻に大きくなっている中間点Ｍでは、軌跡Ａの曲率の変化率、すなわち、グラフの傾きが大きくなる。このような場所では、前記したように、物体の速度ｖd（ｓ）の変化率も大きくなって、物体が急激に加減速されるので、物体が軌道上を空回りしたり、軌跡からずれたりして、軌跡上を速くかつ正確に移動できない。そこで、第２工程において、曲率演算部１２で曲率の変化率、すなわち、曲率ｃの傾きが緩やかになるように曲率ｃを補正した後、第３工程において、この補正した曲率ｃに基づいて、物体の速度を計算し、物体を急激に加減速させずに、軌跡上を速くかつ正確に移動できるようにする。なお、このような速度にする曲率の変化率の最大値を所定値という。ここで、速度の変化率と、曲率の変化率との間には、前記した（７）式の関係が成り立つので、前記した速度の変化率の所定値から曲率の変化率の所定値を求めることができる。
【００３１】
更に、速度計算部１３では、得られた曲率ｃ（ｓ）に基づいて、軌跡Ａ上の各点Ｘでの接線速度ｖd（ｓ）を（７）式によって計算すると共に、速度ｖd（ｓ）が初期値入力部１１に入力した物体の最大速度ｖmaxを超えている場合、速度ｖをｖmax以下にする。この結果、図６（Ａ）、（Ｃ）に示すように、曲率ｃ（ｓ）をＳ字状に補正した場合には、速度ｖd（ｓ）がＳ字状となり、図６（Ｂ）、（Ｄ）に示すように、曲率ｃ（ｓ）を傾きを大きくした直線状に補正した場合には、速度ｖd（ｓ）が直線状となり、物体の急激な加減速を防止して、物体を軌跡上で速くかつ正確に移動することができる。
【００３２】
次に、本発明の作用効果を確認するためにシミュレーションを行った。
図７（Ａ）に示すように、台車２０と、台車２０の上部に設けられたバネ２１と、バネ２１の先端部に取付けられた重り２２とを有する物体２３を使用し、物体２３の重り２２が、図８に示す軌跡Ｋに沿って通過するように軌跡Ｋ上の各点における接線速度Ｖd（ｓ）を求め、更に、物体２３を得られた速度Ｖd（ｓ）で動かした際の軌跡Ｋ’を求めた。
【００３３】
図７（Ｂ）に示すように、物体２３は、質点、バネ、ダンパーの二次系システムに近似でき、その運動方程式は、（１１）式及び（１２）式で示される。ここで、例えば、台車２０を工作機械とした場合、台車２０の移動経路は予め設定された軌跡Ｋとし、重り２２の移動経路が物体２３が実際に移動する軌跡Ｋ’とすることができる。また、台車２０を予め設定された軌跡Ｋを移動する車両とした場合、重り２２を車両に乗っている人間とすることもでき、この際の人の移動経路は軌跡Ｋ’で表される。
【００３４】
【数５】
【００３５】
物体２３の台車２０が通過する軌跡Ｋは、ベジエ曲線（B-spline関数）として与えられている。ベジエ曲線は、端点ｘ1、ｘ3と、その中間の制御点ｘ2とを指定して作成され、（１３）式のように示される。軌跡Ｋは、ベジエ曲線を複数、例えば、３つ組み合わせて作成している。作成した軌跡Ｋ上の各点の座標は、始点からのｘ座標及びｙ座標の配列として、初期値入力部１１に入力した。また、初期値入力部１１に、動体２３（すなわち、台車２０）の最大速度ｖmaxを入力した。
【００３６】
【数６】
【００３７】
初期値入力部１１で入力された軌跡Ｋの座標データ及び最大速度ｖmaxは、曲率計算部１２、速度計算部１３にそれぞれ送られる。まず、曲率計算部１２では、台車２０が軌跡Ｋ上を刻み時間毎に移動するものと仮定し、（３）式によって接線速度ｖ（ｔ）を、（４）式によって曲率ｃ（ｔ）を計算する。ここで、物体２３が軌跡Ｋ上を一定速度で移動しているため、得られた曲率ｃ（ｔ）は経路長ｓの関数となっていると考えられる。従って、（６）式より曲率ｃ（ｓ）が求められ、その結果を図９に示す（図９では補正前として示した）。
【００３８】
図９に示すように、得られた曲率ｃ（ｓ）には、急峻に大きくなっている場所が２カ所あり、ここでは軌跡Ｋの曲率の変化率が所定値以上となって、物体２３の速度ｖ（ｓ）の変化が大きくなり、急激な加減速を行わなければならないので、以下のように補正を行った。まず、ｃ1（ｉ）を補正前の曲率データとする。ここで、ｉはデータのサンプル点を示し、１からサンプル数Ｎまでの範囲をとる。まず、ｃ2（ｉ）＝ｃ1（ｉ）とし、ｉを２からＮまで増加させながら、（１４）式を計算する。
ｃ2（ｉ）＝βｃ2（ｉ−１） ・・・（１４）
ここで、βは減衰定数であり、０＜β＜１である。
【００３９】
もし、ｃ2（ｉ）＜ｃ1（ｉ）となった場合には、ｃ2（ｉ）＝ｃ1（ｉ）とする。次に、ｃ3（ｉ）＝ｃ2（ｉ）とし、ｉをＮ−１から１まで減少させながら、（１５）式を計算する。
ｃ3（ｉ）＝βｃ2（ｉ＋１） ・・・（１５）
もし、ｃ3（ｉ）＜ｃ2（ｉ）となった場合には、ｃ3（ｉ）＝ｃ2（ｉ）とする。このような処理を行うことで、βが大きいときには、曲率の変化率を緩やかにすることができ、図９に示すように、補正後の曲率ｃ（ｓ）は緩やかな裾野を形成することができた。
【００４０】
補正後の曲率ｃ（ｓ）を速度計算部１３に送り、曲率ｃに応じた接線速度ｖd（ｓ）を（７）式によって計算する。その結果を図１０に示す。ここで、初期値入力部１１から送られた物体２３の最大速度ｖmaxを参照して、最大速度ｖmaxを超えている部分を最大速度ｖmaxとする補正を行った。
更に、出力部１４から補正した速度ｖd（ｓ）を物体２３の制御部（図示せず）に出力し、物体２３の台車２０を移動させた場合について、コンピュータによる計算を行った。図１１に、その際の重り２２の軌跡Ｋ’を示す。
【００４１】
また、比較例として、従来の台形型の速度制御により求められた速度で物体を移動した場合の重りの通過する軌跡Ｋ”をコンピュータで計算した。比較例では、物体２３が移動を開始する時及び終了する時に、物体２３の速度は所定速度（最大速度ｖmax）となるまで加速度を一定とし、物体２３が所定速度となった際に、加速度を０、すなわち、速度を一定とする（図１０に比較例として示す）。図１１に示すように、比較例によって物体２３の速度を制御した場合には、物体２３の重り２２は軌跡Ｋ”上を通過する。
【００４２】
図１１に示すように、本発明の物体の速度制御方法を適用したシミュレーションでは、重り２２の振動が少なく、最初に与えた軌跡Ｋに近くなったが、従来の台形型の速度制御方法を適用した場合には、重り２２の振動が大きくなっている。
【００４３】
本発明は、前記した実施の形態に限定されるものではなく、本発明の要旨を変更しない範囲での変更は可能であり、例えば、前記したそれぞれの実施の形態や変形例の一部又は全部を組み合わせて本発明の速度制御方法を構成する場合も本発明の権利範囲に含まれる。
例えば、前記実施の形態の物体の速度制御方法において、適用する物体として、ロボットハンドの手先及びバネを介して重りを取付けた車台としたが、軌跡が曲線となるような動きを行うもの、例えば、搬送台車や、レール上を移動しない無軌道台車等の搬送車にも適用できる。
【図面の簡単な説明】
【００４４】
【図１】本発明の一実施の形態に係る速度制御装置のブロック図である。
【図２】本発明の一実施の形態に係る速度制御方法での物体の軌跡を示す説明図である。
【図３】同速度制御方法での物体の経路長と曲率の関係を示すグラフである。
【図４】同速度制御方法での物体の経路長と接線速度の関係を示すグラフである。
【図５】（Ａ）、（Ｂ）はそれぞれ同速度制御方法での接線速度を補正した場合の説明図である。
【図６】（Ａ）〜（Ｄ）はそれぞれ同速度制御方法での曲率を補正した場合の説明図である。
【図７】（Ａ）、（Ｂ）はそれぞれ同速度制御方法を適用したシミュレーションでの物体の概念図、二次系システムの説明図である。
【図８】物体の速度制御装置に予め与えられた物体の軌道を示すグラフである。
【図９】曲率の計算結果を示すグラフである。
【図１０】物体の速度の計算結果を示すグラフである。
【図１１】物体を移動させた際の軌跡を示すグラフである。
【符号の説明】
【００４５】
１０：物体の速度制御装置、１１：初期値入力部、１２：曲率計算部、１３：速度計算部、１４：出力部、２０：台車、２１：バネ、２２：重り、２３：物体
【技術分野】
【０００１】
本発明は、物体の速度（すなわち、軌跡の接線速度をいう）を制御する方法及びその装置に関する。
【背景技術】
【０００２】
従来、ロボットハンドの手先や搬送台車等の物体を移動させる場合、物体の移動の開始時及び終了時には、物体の速度が所定速度となるまで加速度を一定とし、物体が所定速度となった際には、加速度を０、すなわち、速度を一定とする台形型の速度制御が行われている（例えば、特許文献１参照）。
【０００３】
【特許文献１】特開平１−２３７８０６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、前記従来のような台形型の速度制御方法では、物体が移動する経路（軌跡）が曲線である場合や角部を有する場合等の物体の経路が大きく曲がっている場所、すなわち、曲率の大きい場所においても速度が一定であるため、物体が高速で移動すると、遠心力が大きくなり、決められた軌道からずれてしまうという問題があった。
【０００５】
本発明はかかる事情に鑑みてなされたもので、物体の通過する軌跡上の各点における曲率に応じた速度で物体を移動させる速度制御方法及びその装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
前記目的に沿う本発明に係る速度制御方法は、予め設定された軌跡を通過する物体の速度制御方法であって、
前記軌跡の座標データを入力する第１工程と、
前記軌跡上の各点における曲率を計算する第２工程と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する第３工程と、
前記算出された各点での速度を前記物体に出力する第４工程とを有する。
【０００７】
【数１】
【０００８】
ここで、αは、１未満であり、好ましくは、１／３を超えかつ４／５未満、より好ましくは、１／２を超えかつ３／４未満である。この範囲内において、αが１に近い場合では曲率の小さい場所と曲率の大きい場所での速度（すなわち、接線速度）の差が人間の動作で観察されるより小さくなり、人型ロボットに応用した際には動きが不自然となり、αが１の場合には、曲率ｃの大小に関わらず常に物体の速度ｖが一定となり、αが１を超える場合には、曲率が大きくなると、速度も大きくなる。また、αが１／３以下の場合には、曲率の変化に対して、速度の変化が大きくなり過ぎる。なお、本発明において、軌跡とは、物体（運動部品上の１点、一般には、機能点又は代表点）が運動中に空間に占める位置を連ねた線のことをいう。
【０００９】
本発明に係る速度制御方法において、前記物体の前記軌跡上の各点での速度を算出して、該物体の軌道生成を行うことができる。
本発明に係る速度制御方法において、前記第１工程で前記物体の最大速度を入力し、前記第３工程で前記（２）式によって算出された前記各点での前記物体の速度のデータが前記最大速度を超える場合、該物体の速度を前記最大速度以下に補正するのが好ましい。
本発明に係る速度制御方法において、前記第３工程では、前記（２）式による前記物体の速度の変化率（すなわち、加速度）が所定値以下となるように、前記物体の速度を補正してもよい。
【００１０】
本発明に係る速度制御方法において、前記第２工程では、前記曲率の変化率が所定値以下となるように、前記軌跡上の各点での曲率を補正した後、前記第３工程で該補正した曲率から前記物体の速度を計算してもよい。
本発明に係る速度制御方法において、前記（２）式では、αが２／３であるのが好ましい。ここで、αが２／３である場合、（１）式は、ビビアーニ（Viviani P.）らが人間の手先の動かし方の観察から見出した心理物理的な経験則である三分の二乗則（The 2/3 power low）に従うので、（１）式に示される角速度を用いた（２）式も三分の二乗則に従う。
本発明に係る速度制御方法を、ロボットアーム又は搬送車（物体の一例）の速度を制御するのに用いてもよい。
ここで、搬送車としては、搬送台車や、レール上を移動しない無軌道台車等がある。
【００１１】
前記目的に沿う本発明に係る速度制御装置は、予め設定された軌跡を通過する物体の速度制御装置であって、
前記軌跡の座標データを入力する初期値入力部と、
前記軌跡上の各点における曲率を計算する曲率計算部と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する速度計算部と、
前記算出された各点での速度を前記物体に出力する出力部とを有する。
本発明に係る速度制御装置において、前記出力部はロボットアーム又は搬送車（物体の一例）に前記算出された各点での速度を出力してもよい。
【発明の効果】
【００１２】
請求項１〜７に記載の速度制御方法においては、入力された軌跡の座標データから、軌跡上の各点における曲率を計算し、この算出された各点での曲率から各点での物体の速度を計算するので、曲率に応じた速度で物体を移動させることができ、特に軌跡が大きく曲がっている所では、物体の速度を遅くして、予め決められた軌跡からずれ難くなる。
特に、請求項２記載の速度制御方法においては、物体の軌跡上の各点での速度を算出して、物体の軌道生成を行うので、軌道上の各点での物体の速度を制御する軌道を作成することができる。
請求項３記載の速度制御方法においては、第３工程で算出された各点での物体の速度のデータが入力した最大速度を超える場合、物体の速度を最大速度以下に補正するので、特に、曲率が小さい場所で速度超過とならない。
【００１３】
請求項４記載の速度制御方法においては、第３工程で物体の速度の変化率が所定値以下となるように物体の速度を補正するので、物体が急激に加減速しないように速度を調整することができる。
請求項５記載の速度制御方法においては、第２工程で曲率の変化率が所定値以下となるように軌跡上の各点での曲率を補正した後、第３工程で補正した曲率から物体の速度を計算するので、物体が急激に加減速しないように速度を調整することができる。
請求項６記載の速度制御方法においては、αが２／３であるので、物体の動きが、三分の二乗則に従って滑らかになり、特に人型のロボットでは人の動作に近づかせることができる。
【００１４】
請求項８及び９に記載の速度制御装置においては、入力された軌跡の座標データから、軌跡上の各点における曲率を計算する曲率計算部と、算出された各点での曲率から各点での物体の速度を計算する速度計算部とを有するので、曲率に応じた速度で物体を移動させることができ、特に軌跡が大きく曲がっている所では、物体の速度を遅くして、予め決められた軌跡からずれ難くすることができる。
【発明を実施するための最良の形態】
【００１５】
続いて、添付した図面を参照しつつ、本発明を具体化した実施の形態につき説明し、本発明の理解に供する。
ここで、図１は本発明の一実施の形態に係る速度制御装置のブロック図、図２は本発明の一実施の形態に係る速度制御方法での物体の軌跡を示す説明図、図３は同速度制御方法での物体の経路長と曲率の関係を示すグラフ、図４は同速度制御方法での物体の経路長と接線速度の関係を示すグラフ、図５（Ａ）、（Ｂ）はそれぞれ同速度制御方法での接線速度を補正した場合の説明図、図６（Ａ）〜（Ｄ）はそれぞれ同速度制御方法での曲率を補正した場合の説明図、図７（Ａ）、（Ｂ）はそれぞれ同速度制御方法を適用したシミュレーションでの物体の概念図、二次系システムの説明図、図８は物体の速度制御装置に予め与えられた物体の軌道を示すグラフ、図９は曲率の計算結果を示すグラフ、図１０は物体の速度の計算結果を示すグラフ、図１１は物体を移動させた際の軌跡を示すグラフである。
【００１６】
図１〜図４を参照して、本発明の一実施の形態に係る物体の速度制御装置１０について説明する。
図１に示すように、速度制御装置１０は、図示しない物体（例えば、ロボットハンドの手先）が通過する予め設定された軌跡Ａ（図２参照）及び物体の最大速度ｖmaxを入力する初期値入力部１１と、初期値入力部１１に入力された軌跡Ａ上の各点Ｘにおける曲率ｃを計算する曲率計算部１２と、初期値入力部１１に入力された最大速度ｖmax及び曲率計算部１２で計算された曲率ｃに基づいて、各点Ｘでの物体の速度ｖを計算する速度計算部１３と、速度計算部１３で算出された速度ｖを物体の制御部（図示せず）に出力する出力部１４とを有し、物体が軌跡Ａに沿って速く、かつ、正確に移動するように物体の速度の制御を行っている。
【００１７】
次に、速度制御装置１０を使用した物体の速度制御方法について説明する。
（第１工程）
まず、初期値入力部１１に、軌跡Ａ及び物体の最大速度ｖmaxを入力する。ここで、図２に示すように、軌跡Ａは、中間点Ｍで連結された２本の直線からなり、中間点Ｍで折れ曲がって角部が形成された構造となっている。なお、軌跡Ａは、二次元であって、軌跡Ａ上の始点Ｘ0（ｘ0，ｙ0）から終点Ｘ1（ｘ1，ｙ1）までの複数の点Ｘi（ｘi，ｙi）（但し、０≦ｉ≦１である）によって、軌跡Ａの座標データが構成されている。初期値入力部１１には、これら軌跡Ａ上の全ての点の座標のデータが入力される。なお、軌跡は、三次元で与えられてもよく、また、ベジエ曲線等のパラメトリック曲線等の数式で与えてもよい。初期値入力部１１から曲率計算部１２に軌跡Ａ上の各点の座標を示す数値が送られ、速度計算部１３に最大速度ｖmaxを示す数値が送られる。
【００１８】
（第２工程）
次に、曲率計算部１２で軌跡Ａ上の各点Ｘにおける曲率をそれぞれ計算する。
まず、物体が軌跡Ａ上を始点Ｘ0から終点Ｘ1まで一定速度で進むと仮定し、軌跡Ａ上の点を微小な刻み時間δｔでサンプリングした際の時刻ｔでの速度ｖ（ｔ）を（３）式によって求める。また、加速度も同様に計算できるので、各点での曲率ｃ（ｔ）は（４）式によって求められる。
【００１９】
【数２】
【００２０】
ここで、軌跡Ａ上の任意の点Ｘの始点Ｘ0からの距離を経路長ｓとして、刻み時間δｔの間に刻み幅δｓで軌跡Ａ上の点をサンプリングし、時刻ｔに経路長ｓの点Ｘに到達する場合、経路長ｓの位置での接線速度ｖ（ｓ）は、（５）式で示される。従って、軌跡Ａのみが与えられた場合の経路長ｓの位置での曲率ｃ（ｓ）は（６）式で示され、軌跡Ａの経路長ｓでの曲率ｃ（ｓ）は図３のように示される。求められた曲率ｃ（ｓ）のデータは、速度計算部１３に送られる。
【００２１】
【数３】
【００２２】
（第３工程）
ここで、（５）式は、等速度運動であるので、各点Ｘでの曲率ｃ（ｓ）に応じた接線速度ｖd（ｓ）を（７）式によって計算する（図４では、一点鎖線で示す）。なお、パラメータをｕとした場合の物体の速度ｖ（ｕ）は、（１）式に示される角速度ωを用いた（２）式によって計算されており、（２）式のパラメータを経路長ｓに置き換えることにより、（７）式が得られる。また、曲率ｃが曲率半径ｒの逆数であるので、速度ｖd（ｓ）を（８）式で表すこともできる。また、速度ｖd（ｓ）の大きさは速度ｖ（ｓ）と異なるが、それらの方向は一致するので、速度ｖd（ｓ）のｘ方向成分ｘd（ｓ）及びｙ方向成分ｙd（ｓ）は、（９）式及び（１０）式で示される。
このように、物体の速度ｖd（ｓ）は、曲率ｃ（ｓ）に応じて決定されるので、軌跡Ａが大きく曲がっている、すなわち、曲率ｃが大きい所では、物体の接線速度を遅くして、物体が遠心力によって軌跡Ａからずれるのを防止することができる。
【００２３】
【数４】
【００２４】
ここで、（７）式において、αは、１未満であり、好ましくは、１／３を超えかつ４／５未満、より好ましくは、１／２を超えかつ３／４未満である。また、αを２／３とした場合には、（１）式に示す角速度ωが、人の手先の動かし方の観察から得られた心理物理的な経験則である三分の二乗則に従うので、（１）式に示される角速度ωを用いた（２）式も三分の二乗則に従う。これによって、（２）式によって計算された速度ｖで物体を動かした場合、物体が人の動きに近づいて滑らかに動く。なお、αが１／３以下の場合には、曲率がさほど大きくない場所でも速度が小さくなり過ぎ、αが１である場合には、速度が曲率に依らず一定となり、αが１を超える場合には、曲率が大きくなる、すなわち、曲率半径ｒが小さくなるに従って速度が大きくなるので不適である。
【００２５】
更に、速度計算部１３では、速度ｖd（ｓ）が初期値入力部１１に入力した物体の最大速度ｖmaxを超えている場合、速度ｖd（ｓ）を最大速度ｖmax以下（例えば、最大速度ｖmax）にする。これによって、物体が移動する速度を抑えることができ、特に、曲率ｃの大きい場所、すなわち、曲率半径の小さい場所での物体の速度を遅くして、遠心力による軌跡からのずれを防止することができる。このようにして、図４に示すように、経路長ｓと速度ｖd（ｓ）のグラフが求められる。
【００２６】
（第４工程）
速度計算部１３で算出された速度を出力部１４から物体の動きを制御する制御部に出力する。物体の制御部は、与えられた速度で物体を移動させるので、物体が軌跡Ａに沿って速く、かつ、正確に移動することができる。以上のようにして、軌道生成を行なう。
【００２７】
（第１の補正の方法）
図４に示すように、始点及び終点では、曲率の値に関わらず加減速が行われる。また、軌跡Ａが急に折れ曲がっている、すなわち、曲率ｃが急峻に大きくなる場所（中間点Ｍ）でも、物体の速度の変化率（加速度）が大きくなり、急激な加減速を行わなければならない。ここで、物体の速度の変化率とは、加速度であって、図４においてグラフの傾きで示される。
【００２８】
このように物体が急激に加減速する場所では、物体が軌道上を空回りして速度が遅くなったり、遠心力によって軌跡からずれることがあるので、軌跡上を移動する物体の速度の変化率を所定値（許容値）以下に（すなわち、図４においては、傾きを小さく）して、物体が軌跡上を速くかつ正確に移動できるように、速度の変化率の大きいところの前後の速度を補正する。なお、速度の変化率の所定値は、物体によっても、また、同じ物体でも設定する軌跡によっても異なる。この速度の変化率（加速度）の所定値は、工作機械などでは１０Ｇ（Ｇは重力加速度を示す）を超えることも可能であるが、人間が乗車する車両等では数Ｇ以内が好ましい。
【００２９】
そこで、第３工程では、速度計算部１３において、速度ｖd（ｓ）の変化率の大きいところ（図４において、傾きの大きい場所）の前後に加減速区間Ｂを設けて、急激な加減速が起きないようにする。加減速区間Ｂでは、例えば、図５（Ａ）に示すように、速度ｖd（ｓ）を多項式で変換してＳ字状に補正してもよく、図５（Ｂ）に示すように、速度ｖd（ｓ）を直線的に補正し、物体の急激な加減速を防止する。これによって、物体は、軌跡上を速くかつ正確に移動することができる。
【００３０】
（第２の補正の方法）
また、図３に示すように、軌跡Ａ上の曲率ｃが急峻に大きくなっている中間点Ｍでは、軌跡Ａの曲率の変化率、すなわち、グラフの傾きが大きくなる。このような場所では、前記したように、物体の速度ｖd（ｓ）の変化率も大きくなって、物体が急激に加減速されるので、物体が軌道上を空回りしたり、軌跡からずれたりして、軌跡上を速くかつ正確に移動できない。そこで、第２工程において、曲率演算部１２で曲率の変化率、すなわち、曲率ｃの傾きが緩やかになるように曲率ｃを補正した後、第３工程において、この補正した曲率ｃに基づいて、物体の速度を計算し、物体を急激に加減速させずに、軌跡上を速くかつ正確に移動できるようにする。なお、このような速度にする曲率の変化率の最大値を所定値という。ここで、速度の変化率と、曲率の変化率との間には、前記した（７）式の関係が成り立つので、前記した速度の変化率の所定値から曲率の変化率の所定値を求めることができる。
【００３１】
更に、速度計算部１３では、得られた曲率ｃ（ｓ）に基づいて、軌跡Ａ上の各点Ｘでの接線速度ｖd（ｓ）を（７）式によって計算すると共に、速度ｖd（ｓ）が初期値入力部１１に入力した物体の最大速度ｖmaxを超えている場合、速度ｖをｖmax以下にする。この結果、図６（Ａ）、（Ｃ）に示すように、曲率ｃ（ｓ）をＳ字状に補正した場合には、速度ｖd（ｓ）がＳ字状となり、図６（Ｂ）、（Ｄ）に示すように、曲率ｃ（ｓ）を傾きを大きくした直線状に補正した場合には、速度ｖd（ｓ）が直線状となり、物体の急激な加減速を防止して、物体を軌跡上で速くかつ正確に移動することができる。
【００３２】
次に、本発明の作用効果を確認するためにシミュレーションを行った。
図７（Ａ）に示すように、台車２０と、台車２０の上部に設けられたバネ２１と、バネ２１の先端部に取付けられた重り２２とを有する物体２３を使用し、物体２３の重り２２が、図８に示す軌跡Ｋに沿って通過するように軌跡Ｋ上の各点における接線速度Ｖd（ｓ）を求め、更に、物体２３を得られた速度Ｖd（ｓ）で動かした際の軌跡Ｋ’を求めた。
【００３３】
図７（Ｂ）に示すように、物体２３は、質点、バネ、ダンパーの二次系システムに近似でき、その運動方程式は、（１１）式及び（１２）式で示される。ここで、例えば、台車２０を工作機械とした場合、台車２０の移動経路は予め設定された軌跡Ｋとし、重り２２の移動経路が物体２３が実際に移動する軌跡Ｋ’とすることができる。また、台車２０を予め設定された軌跡Ｋを移動する車両とした場合、重り２２を車両に乗っている人間とすることもでき、この際の人の移動経路は軌跡Ｋ’で表される。
【００３４】
【数５】
【００３５】
物体２３の台車２０が通過する軌跡Ｋは、ベジエ曲線（B-spline関数）として与えられている。ベジエ曲線は、端点ｘ1、ｘ3と、その中間の制御点ｘ2とを指定して作成され、（１３）式のように示される。軌跡Ｋは、ベジエ曲線を複数、例えば、３つ組み合わせて作成している。作成した軌跡Ｋ上の各点の座標は、始点からのｘ座標及びｙ座標の配列として、初期値入力部１１に入力した。また、初期値入力部１１に、動体２３（すなわち、台車２０）の最大速度ｖmaxを入力した。
【００３６】
【数６】
【００３７】
初期値入力部１１で入力された軌跡Ｋの座標データ及び最大速度ｖmaxは、曲率計算部１２、速度計算部１３にそれぞれ送られる。まず、曲率計算部１２では、台車２０が軌跡Ｋ上を刻み時間毎に移動するものと仮定し、（３）式によって接線速度ｖ（ｔ）を、（４）式によって曲率ｃ（ｔ）を計算する。ここで、物体２３が軌跡Ｋ上を一定速度で移動しているため、得られた曲率ｃ（ｔ）は経路長ｓの関数となっていると考えられる。従って、（６）式より曲率ｃ（ｓ）が求められ、その結果を図９に示す（図９では補正前として示した）。
【００３８】
図９に示すように、得られた曲率ｃ（ｓ）には、急峻に大きくなっている場所が２カ所あり、ここでは軌跡Ｋの曲率の変化率が所定値以上となって、物体２３の速度ｖ（ｓ）の変化が大きくなり、急激な加減速を行わなければならないので、以下のように補正を行った。まず、ｃ1（ｉ）を補正前の曲率データとする。ここで、ｉはデータのサンプル点を示し、１からサンプル数Ｎまでの範囲をとる。まず、ｃ2（ｉ）＝ｃ1（ｉ）とし、ｉを２からＮまで増加させながら、（１４）式を計算する。
ｃ2（ｉ）＝βｃ2（ｉ−１） ・・・（１４）
ここで、βは減衰定数であり、０＜β＜１である。
【００３９】
もし、ｃ2（ｉ）＜ｃ1（ｉ）となった場合には、ｃ2（ｉ）＝ｃ1（ｉ）とする。次に、ｃ3（ｉ）＝ｃ2（ｉ）とし、ｉをＮ−１から１まで減少させながら、（１５）式を計算する。
ｃ3（ｉ）＝βｃ2（ｉ＋１） ・・・（１５）
もし、ｃ3（ｉ）＜ｃ2（ｉ）となった場合には、ｃ3（ｉ）＝ｃ2（ｉ）とする。このような処理を行うことで、βが大きいときには、曲率の変化率を緩やかにすることができ、図９に示すように、補正後の曲率ｃ（ｓ）は緩やかな裾野を形成することができた。
【００４０】
補正後の曲率ｃ（ｓ）を速度計算部１３に送り、曲率ｃに応じた接線速度ｖd（ｓ）を（７）式によって計算する。その結果を図１０に示す。ここで、初期値入力部１１から送られた物体２３の最大速度ｖmaxを参照して、最大速度ｖmaxを超えている部分を最大速度ｖmaxとする補正を行った。
更に、出力部１４から補正した速度ｖd（ｓ）を物体２３の制御部（図示せず）に出力し、物体２３の台車２０を移動させた場合について、コンピュータによる計算を行った。図１１に、その際の重り２２の軌跡Ｋ’を示す。
【００４１】
また、比較例として、従来の台形型の速度制御により求められた速度で物体を移動した場合の重りの通過する軌跡Ｋ”をコンピュータで計算した。比較例では、物体２３が移動を開始する時及び終了する時に、物体２３の速度は所定速度（最大速度ｖmax）となるまで加速度を一定とし、物体２３が所定速度となった際に、加速度を０、すなわち、速度を一定とする（図１０に比較例として示す）。図１１に示すように、比較例によって物体２３の速度を制御した場合には、物体２３の重り２２は軌跡Ｋ”上を通過する。
【００４２】
図１１に示すように、本発明の物体の速度制御方法を適用したシミュレーションでは、重り２２の振動が少なく、最初に与えた軌跡Ｋに近くなったが、従来の台形型の速度制御方法を適用した場合には、重り２２の振動が大きくなっている。
【００４３】
本発明は、前記した実施の形態に限定されるものではなく、本発明の要旨を変更しない範囲での変更は可能であり、例えば、前記したそれぞれの実施の形態や変形例の一部又は全部を組み合わせて本発明の速度制御方法を構成する場合も本発明の権利範囲に含まれる。
例えば、前記実施の形態の物体の速度制御方法において、適用する物体として、ロボットハンドの手先及びバネを介して重りを取付けた車台としたが、軌跡が曲線となるような動きを行うもの、例えば、搬送台車や、レール上を移動しない無軌道台車等の搬送車にも適用できる。
【図面の簡単な説明】
【００４４】
【図１】本発明の一実施の形態に係る速度制御装置のブロック図である。
【図２】本発明の一実施の形態に係る速度制御方法での物体の軌跡を示す説明図である。
【図３】同速度制御方法での物体の経路長と曲率の関係を示すグラフである。
【図４】同速度制御方法での物体の経路長と接線速度の関係を示すグラフである。
【図５】（Ａ）、（Ｂ）はそれぞれ同速度制御方法での接線速度を補正した場合の説明図である。
【図６】（Ａ）〜（Ｄ）はそれぞれ同速度制御方法での曲率を補正した場合の説明図である。
【図７】（Ａ）、（Ｂ）はそれぞれ同速度制御方法を適用したシミュレーションでの物体の概念図、二次系システムの説明図である。
【図８】物体の速度制御装置に予め与えられた物体の軌道を示すグラフである。
【図９】曲率の計算結果を示すグラフである。
【図１０】物体の速度の計算結果を示すグラフである。
【図１１】物体を移動させた際の軌跡を示すグラフである。
【符号の説明】
【００４５】
１０：物体の速度制御装置、１１：初期値入力部、１２：曲率計算部、１３：速度計算部、１４：出力部、２０：台車、２１：バネ、２２：重り、２３：物体
【特許請求の範囲】
【請求項１】
予め設定された軌跡を通過する物体の速度制御方法であって、
前記軌跡の座標データを入力する第１工程と、
前記軌跡上の各点における曲率を計算する第２工程と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する第３工程と、
前記算出された各点での速度を前記物体に出力する第４工程とを有することを特徴とする速度制御方法。
【数１】
【請求項２】
請求項１記載の速度制御方法において、前記物体の前記軌跡上の各点での速度を算出して、該物体の軌道生成を行うことを特徴とする速度制御方法。
【請求項３】
請求項１及び２のいずれか１項に記載の速度制御方法において、前記第１工程で前記物体の最大速度を入力し、前記第３工程で前記（２）式によって算出された前記各点での前記物体の速度のデータが前記最大速度を超える場合、該物体の速度を前記最大速度以下に補正することを特徴とする速度制御方法。
【請求項４】
請求項１〜３のいずれか１項に記載の速度制御方法において、前記第３工程では、前記（２）式による前記物体の速度の変化率が所定値以下となるように、前記物体の速度を補正することを特徴とする速度制御方法。
【請求項５】
請求項１〜３のいずれか１項に記載の速度制御方法において、前記第２工程では、前記曲率の変化率が所定値以下となるように、前記軌跡上の各点での曲率を補正した後、前記第３工程で該補正した曲率から前記物体の速度を計算することを特徴とする速度制御方法。
【請求項６】
請求項１〜５のいずれか１項に記載の速度制御方法において、前記（２）式では、αが２／３であることを特徴とする速度制御方法。
【請求項７】
請求項１〜６のいずれか１項に記載の速度制御方法を、ロボットアーム又は搬送車の速度を制御するのに用いることを特徴とする速度制御方法。
【請求項８】
予め設定された軌跡を通過する物体の速度制御装置であって、
前記軌跡の座標データを入力する初期値入力部と、
前記軌跡上の各点における曲率を計算する曲率計算部と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する速度計算部と、
前記算出された各点での速度を前記物体に出力する出力部とを有することを特徴とする速度制御装置。
【数１】
【請求項９】
請求項８記載の速度制御装置において、前記出力部はロボットアーム又は搬送車に前記算出された各点での速度を出力することを特徴とする速度制御装置。
【請求項１】
予め設定された軌跡を通過する物体の速度制御方法であって、
前記軌跡の座標データを入力する第１工程と、
前記軌跡上の各点における曲率を計算する第２工程と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する第３工程と、
前記算出された各点での速度を前記物体に出力する第４工程とを有することを特徴とする速度制御方法。
【数１】
【請求項２】
請求項１記載の速度制御方法において、前記物体の前記軌跡上の各点での速度を算出して、該物体の軌道生成を行うことを特徴とする速度制御方法。
【請求項３】
請求項１及び２のいずれか１項に記載の速度制御方法において、前記第１工程で前記物体の最大速度を入力し、前記第３工程で前記（２）式によって算出された前記各点での前記物体の速度のデータが前記最大速度を超える場合、該物体の速度を前記最大速度以下に補正することを特徴とする速度制御方法。
【請求項４】
請求項１〜３のいずれか１項に記載の速度制御方法において、前記第３工程では、前記（２）式による前記物体の速度の変化率が所定値以下となるように、前記物体の速度を補正することを特徴とする速度制御方法。
【請求項５】
請求項１〜３のいずれか１項に記載の速度制御方法において、前記第２工程では、前記曲率の変化率が所定値以下となるように、前記軌跡上の各点での曲率を補正した後、前記第３工程で該補正した曲率から前記物体の速度を計算することを特徴とする速度制御方法。
【請求項６】
請求項１〜５のいずれか１項に記載の速度制御方法において、前記（２）式では、αが２／３であることを特徴とする速度制御方法。
【請求項７】
請求項１〜６のいずれか１項に記載の速度制御方法を、ロボットアーム又は搬送車の速度を制御するのに用いることを特徴とする速度制御方法。
【請求項８】
予め設定された軌跡を通過する物体の速度制御装置であって、
前記軌跡の座標データを入力する初期値入力部と、
前記軌跡上の各点における曲率を計算する曲率計算部と、
前記算出された前記各点での曲率から該各点での前記物体の速度を（１）式に示される角速度を用いた（２）式に従って計算する速度計算部と、
前記算出された各点での速度を前記物体に出力する出力部とを有することを特徴とする速度制御装置。
【数１】
【請求項９】
請求項８記載の速度制御装置において、前記出力部はロボットアーム又は搬送車に前記算出された各点での速度を出力することを特徴とする速度制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２００７−２３３６１３（Ｐ２００７−２３３６１３Ａ）
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願番号】特願２００６−５３３５５（Ｐ２００６−５３３５５）
【出願日】平成１８年２月２８日（２００６．２．２８）
【出願人】（５０４１７４１３５）国立大学法人九州工業大学 (489)
【出願人】（０００１９６７０５）西部電機株式会社 (80)
【Ｆターム（参考）】
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願日】平成１８年２月２８日（２００６．２．２８）
【出願人】（５０４１７４１３５）国立大学法人九州工業大学 (489)
【出願人】（０００１９６７０５）西部電機株式会社 (80)
【Ｆターム（参考）】
[ Back to top ]