(57)【特許請求の範囲】
【請求項１】
ロボットの移動動作の目標位置に対する位置信号の入力を受けて、該ロボットの移動速度と該移動速度での動作時間との関係を示す信号である速度プロファイルＶａを計算して出力するプランナーと、
前記プランナーから前記速度プロファイルＶａの入力を受けて速度プロファイルＶｂを出力する一つの低域通過フィルタと、
前記低域通過フィルタの出力を受けてサーボモーターの加／減速を制御するサーボ制御部とを含み、
前記低域通過フィルタは
前記速度プロファイルＶａを入力する直列接続された多数の単位遅延素子と、
前記単位遅延素子の出力の各々に利得値を乗じる多数の第１係数乗算器と、
前記多数の第１係数乗算器の出力をすべて加える加算器と、
前記加算器の出力に前記利得値全体の和の逆数を乗じる第２係数乗算器とを含み、
前記単位遅延素子は、入力した信号を所定時間だけ遅延させて出力するものであるロボットの移動動作制御装置。
【請求項２】
前記単位遅延素子はシフトレジスタで形成される請求項１に記載のロボットの移動動作制御装置。
【請求項３】
前記利得値は時間に対して１次関数の関係を有する請求項１に記載のロボットの移動動作制御装置。
【請求項４】
時間をｔとし、前記利得値をｋ(ｔ)とし、前記サーボモーター加／減速区間の大きさをＦとし、任意の正数をａとする時、前記１次関数は下記数式の通りである請求項３に記載のロボットの移動動作制御装置。
【数１】
0004302805.tif 000006
【請求項５】
前記利得値は時間に対して２次関数の関係を有する請求項１に記載のロボットの移動動作制御装置。
【請求項６】
時間をｔとし、前記利得値をｋ(ｔ)とし、前記サーボモーター加／減速区間の大きさをＦとし、任意の正数をａとする時、前記２次関数は下記数式の通りである請求項５に記載のロボットの移動動作制御装置。
【数２】
0004302805.tif 000007
【請求項７】
ロボットの移動動作の目標位置に対する位置信号の入力を受けて、該ロボットの移動速度と該移動速度での動作時間との関係を示す信号である速度プロファイルＶａを計算する段階と、
一つの低域通過フィルタを使って前記速度プロファイルＶａに対してフィルタリングして速度プロファイルＶｂを形成する段階と、
前記速度プロファイルＶｂでサーボモーターの加／減速を制御する段階とを含み、
前記速度プロファイルＶｂを形成する段階は、
入力信号である前記速度プロファイルＶａを連続的に遅延させて順次の遅延値を有する多数の遅延信号を形成する段階と、
前記遅延信号の各々に利得値を乗じて出力する段階と、
前記利得値が乗じられた各出力を加算した値に前記利得値全体の和の逆数を乗じる段階とを含み、
前記単位遅延素子は、入力した信号を所定時間だけ遅延させて出力するものであるロボットの移動動作制御方法。
【請求項８】
前記利得値全体の和の計算はフィルタが動作する前に予め計算されて初期値として設定される請求項７に記載のロボットの移動動作制御方法。
【請求項９】
前記利得値を調節してサーボモーターの加速度の変化率を調節する請求項７に記載のロボットの移動動作制御方法。
【請求項１０】
前記利得値は時間に対して１次関数の関係を有する請求項７に記載のロボットの移動動作制御方法。
【請求項１１】
時間をｔとし、前記利得値をｋ(ｔ)とし、前記補間器の加／減速区間の大きさをＦとし、任意の正数をａとする時、前記１次関数は下記数式の通りである請求項１０に記載のロボットの移動動作制御方法。
【数３】
0004302805.tif 000008
【請求項１２】
前記利得値は時間に対して２次関数の関係を有する請求項７に記載のロボットの移動動作制御方法。
【請求項１３】
時間をｔとし、前記利得値をｋ(ｔ)とし、前記補間器の加／減速区間の大きさをＦとし、任意の正数をａとする時、前記２次関数は下記数式の通りである請求項１２に記載のロボットの移動動作制御方法。
【数４】
0004302805.tif 000009
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明はロボット制御に関するもので、より詳しくはロボットが動作する経路計画装置及び経路計画方法に関する。
【０００２】
【従来の技術】
産業用ロボットの動作を制御する場合には、動作経路が精密に維持され、線形又は環形の部分経路の間のコーナー組合せを単純で確実にすることが大変重要である。このために最近では平滑な加速及び減速を通じてロボットの動作経路を計画する方法が使われている。
【０００３】
図１に従来のロボットの経路計画装置のブロック図を図示した。
図１のように従来の経路計画装置は、目標位置の入力を受けて速度プロファイルを計算して出力するプランナー（planner）１０、プランナー１０から速度プロファイル（profile）の入力を受けて経路補間（path interpolation）を遂行して平滑化された速度プロファイルを出力する補間器（interpolator）２０、補間器２０の出力を受けてサーボモーター（servo motor）４０を制御するサーボ制御部３０とからなる。補間器２０は図２のように単位遅延素子（unit delay）２１と加算器（adder）２２及び係数乗算器（coefficient multiplier）２３で構成された線形フィルタとからなる。ここでＦはフィルタの次数（order）であり加／減速区間の長さになる。
【０００４】
かかる経路計画装置でプランナー１０が目標位置に至るために計算された速度プロファイルＶａを補間器２０に入力するとフィルタリングされた速度プロファイルＶｂが出力されサーボ制御部３０に入力されてサーボモーターを駆動するようになる。図３に入力速度プロファイルＶａを図示し、図４に出力速度プロファイルＶｂを図示した。
【０００５】
出力速度プロファイルＶｂを微分すると加速度プロファイルＡが求められ、これを再び微分するとジャークプロファイルＪを求めることができる。図４に加速度プロファイルＡとジャークプロファイルＪを各々図示した。ここでジャークは加速度の時間的変化率を指定するベクトルであって産業用ロボットにおけるロボットボディーの耐久性に大きな影響を及ぼす。図４のように従来の経路計画装置は急激なジャークを発生するのでロボットボディーに損傷を与えやすい。
【０００６】
かかるジャークの変化を減少させるためのアメリカ特許第５４３４４８９号明細書では二つの線形フィルタを使った経路計画装置を提示した。図５にこのような二つの線形フィルタを使った経路計画装置を図示した。図５のように補間器２０は二つの線形フィルタ２４，２５よりなっている。この場合、図３のような速度プロファイルＶａが補間器２０に入力されると、図６のような速度プロファイルＶｂが出力される。図６の速度プロファイルＶｂにおける加／減速部分は２次関数よりなるＳ字曲線の形態になっていることがわかる。
【０００７】
図６の速度プロファイルＶｂから加速度プロファイルＡおよびジャークプロファイルＪが求められ、これを図６に併せて図示した。図６のように二つの線形フィルタを使った場合には一つの線形フィルタを使った場合よりジャークプロファイルＪが緩やかになるので、ボットボディーの損傷を減少させて円滑な動作が得られる。しかし、依然としてジャークの変化が大きくてロボットボディーが損傷するおそれがあり、フィルタの過剰な使用によって、計算時間が増えて応答速度が遅くなると言う短所がある。
【０００８】
【発明が解決しようとする課題】
本発明はかかる問題点を解決するためのもので、本発明の課題はロボットの経路計画装置でジャークの変化を緩やかにすることによりロボットボディーの損傷を軽減し、動きを円滑にして、経路計画装置の応答速度を改善することである。
【０００９】
【課題を解決するための手段】
かかる課題を達成するために本発明では、設定された目標に対する位置信号の入力を受けて速度プロファイルを計算し、一つの低域通過フィルタを使って速度プロファイルに対して経路補間を遂行することで、平滑化された速度プロファイルを形成してサーボモーターの加／減速を制御する。
【００１０】
かかる経路計画に使われる低域通過フィルタは直列接続された多数の単位遅延素子と係数乗算器及び加算器からなる。フィルタに入力された信号は単位遅延素子によって順次に遅延する。このように遅延した各信号に所定の利得値を係数乗算器で乗算して、この値を加算器ですべて加算した後、ここに前記利得値の全体の和の逆数がまた他の係数乗算器で乗算されてフィルタ出力になる。
【００１１】
ここで、利得値を調節してサーボモーターの加速度の変化率を調節することができ、利得値を時間に対する関数で表わした時、その次数が大きければ大きいほどサーボモーターの動作がスムーズになる。すなわち、フィルタに入力される速度プロファイルが０次である時、利得値が時間に対して１次関数である場合にはジャークプロファイルは０次になり、利得値が時間に対して２次関数である場合にはジャークプロファイルは１次になってさらに平滑化される。このようにジャークプロファイルを平滑化させる経路計画はロボット制御に使われる場合、ロボットボディーの損傷を減少させることができる。
【００１２】
【発明の実施の形態】
以下、本発明のもっとも好ましい実施形態を記載する。
本発明の実施形態による経路計画装置を図７に図示した。図７のように本発明による経路計画装置は目標に対する位置信号の入力を受けて速度プロファイルＶａを計算して出力するプランナー１０と、プランナー１０から速度プロファイルＶａの入力を受けて平滑化された速度プロファイルＶｂを出力する低域通過フィルタ５０と、低域通過フィルタ５０の出力を受けてサーボモーター４０の加／減速を制御するサーボ制御部３０とからなる。低域通過フィルタ５０は経路補間を遂行する補間器の機能を有する。すなわち、本発明による実施形態では一つの低域通過フィルタ５０だけで経路補間を遂行することができる。
【００１３】
かかる低域通過フィルタ５０を図８に図示した。図８のように、低域通過フィルタ５０はシフトレジスタで形成されている多数の単位遅延素子５１と係数乗算器５２、５４及び加算器５３からなる。単位遅延素子５１は直列接続されて遅延した信号を出力し、係数乗算器５２は遅延した各々の信号に所定の利得値ｋ1、ｋ2、…、ｋFを乗じ、加算器５３は係数乗算器５２の各出力をすべて加算し、係数乗算器５４は係数乗算器５２でに乗算された利得値ｋ1、ｋ2、…、ｋF全体の和の逆数１／Ｓkを加算器５３の出力値に乗算する。ここでＦはサーボモーターの加／減速区間の大きさである。
【００１４】
一般に、デジタルフィルタの伝達関数がｆ(ｎＴ)であり、フィルタの入力信号がＶ(ｎＴ)である時、フィルタ出力ｗ(ｎＴ)は次の数５のように入力信号Ｖ(ｎＴ)と伝達関数のｆ(ｎＴ)のコンボルション（convolution）で表示される。ここでＴはサンプリング周期であり、ｎ＝０、１、２、…である。
【００１５】
【数５】
0004302805.tif 000002
【００１６】
従って、前記低域通過フィルタ５０に速度プロファイルＶａ（ｎＴ）が入力されると、出力される速度プロファイルＶｂ（ｎＴ）は次の数６の通りである。
【００１７】
【数６】
0004302805.tif 000003
【００１８】
図８のように実際にフィルタを具現する場合には各利得値の合計Ｓkは初期値として設定されるのでフィルタの計算速度にはあまり影響を与えない。
【００１９】
上記数６の利得値ｋiによってサーボモーター４０の加速度の変化率が決定される。便宜上利得値ｋiを時間ｔに対する連続関数ｋ(ｔ)で表現し、ｋ(ｔ)が次の数７のような時間に対する１次関数の関係を有すると、低域通過フィルタ５０は図３のような速度プロファイルＶａが入力された時、図６のような速度プロファイルＶｂを出力する。
【００２０】
【数７】
0004302805.tif 000004
【００２１】
ここで、ａは任意の正数で、数７の関係を図９にグラフで図示した。かかる利得値ｋ(ｔ)は低域通過フィルタ５０がデジタルフィルタであるので実際には０からＦまでの期間の間にサンプリングの周期Ｔでサンプリング処理される。図６の速度プロファイルＶｂにおいて加／減速部分は２次関数からなっているＳ字曲線の形態を有する。
【００２２】
かかる速度プロファイルＶｂを微分すると図６のような加速度プロファイルＡとジャークプロファイルＪを得ることができる。すなわち、従来の二つの線型フィルタを使った場合のようなジャークプロファイルＪを一つの低域通過フィルタだけで得ることができる。
【００２３】
これと異なり、利得値ｋ(ｔ)が次の数８のような時間に対する２次関数の関係を有すると、低域通過フィルタ５０は図３のような速度プロファイルＶａが入力された時に図１１のような速度プロファイルＶｂを出力する。
【００２４】
【数８】
0004302805.tif 000005
【００２５】
ここでａは任意の正数で、上記数８の関係を図１０にグラフで図示した。図１１の速度プロファイルＶｂを微分すると図１１の加速度プロファイルＡとジャークプロファイルＪを得ることができる。この場合にはジャークプロファイルＪが図６に図示したものよりはるかに平滑化されてサーボモーターがよりスムーズに動作できる。
【００２６】
かかるように利得値の次数が大きければ大きいほどジャークプロファイルが平滑化されてジャークによってロボットボディーに与えられる損傷を減少させることができる。
【００２７】
以下、本発明の実施形態による経路計画方法を説明する。
まず、設定された目標に対する位置信号の入力を受けて速度プロファイルＶａを計算し、速度プロファイルＶａに対して経路補間を遂行して平滑化された速度プロファイルＶｂを形成し、速度プロファイルＶｂでサーボモーターの加／減速を制御して経路計画を遂行する。この時、経路補間は一つの低域通過フィルタを使ってフィルタリングすることによって遂行される。
【００２８】
低域通過フィルタを使ってフィルタリングする方法は、入力信号を連続的に遅延させて順次の遅延値を有する多数の遅延信号を形成し、遅延信号に各々の利得値を乗じて出力した後、利得値が乗算された各出力を合わせた値に利得値全体の和の逆数を乗じてフィルタリングする。ここで利得値の全体の和の逆数に対する計算はフィルタリングする前に予め計算して初期値として設定することによってフィルタリングにかかる時間を減少させることができる。
【００２９】
かかる低域通過フィルタの利得値を調節することで、サーボモーターの加速度の変化率を調節することができる。利得値を時間に対する連続関数で表現する時、関数の次数が大きければ大きいほどサーボモーターの動作がスムーズになる。すなわち、低域通過フィルタに入力される速度プロファイルが０次である時、利得値が時間に対する１次関数である場合にはジャークプロファイルは０次になり、利得値が時間に対する２次関数である場合にはジャークプロファイルは１次になる。
ロボットの制御では、かかるジャークプロファイルの次数が増加して平滑化されればされるほどロボットボディーの損傷を減少させることができる。
【００３０】
【発明の効果】
前記のように本発明による経路制御では一つの低域通過フィルタの利得値を調節して経路補間を遂行することによってサーボモーターの動作をスムーズにし、ロボット制御に使う場合、ジャークの変化が緩やかになってロボットボディーに対する損傷を減少させることができる。また低域通過フィルタを具現する場合、利得値の和を加／減速区間が与えられると予めて計算することができるのでフィルタの計算速度を増加させることができる。
【図面の簡単な説明】
【図１】  従来のロボット制御における経路計画装置を図示したブロック図である。
【図２】  図１の経路計画装置の補間器として使われる線形フィルタを図示したブロック図である。
【図３】  図１の補間器に入力される速度プロファイルを図示したグラフである。
【図４】  図１の補間器に出力される速度プロファイルとこれを微分した加速度プロファイル及びジャークプロファイルを図示したグラフである。
【図５】  二つの線形フィルタを使った従来の経路計画装置の補間器を図示したブロック図である。
【図６】  図５の補間器から出力される速度プロファイルとこれを微分した加速度プロファイル及びジャークプロファイルを図示したグラフである。
【図７】  本発明の実施形態による経路計画装置を図示したブロック図である。
【図８】  図７の経路計画装置の補間器に使われる低域通過フィルタを図示したブロック図である。
【図９】  図８の低域通過フィルタの利得値を図示したグラフである。
【図１０】  図８の低域通過フィルタの利得値を図示したグラフである。
【図１１】  図８の低域通過フィルタから出力される速度プロファイルとこれを微分した加速度プロファイル及びジャークプロファイルを図示したグラフである。
【符号の説明】
１０  プランナー
３０  サーボ制御部
４０  サーボモーター
５０  低域通過フィルタ
５１  単位遅延素子
５２  係数乗算器
５３  加算器
【図１】
0004302805.tif 000010
【図２】
0004302805.tif 000011
【図３】
0004302805.tif 000012
【図４】
0004302805.tif 000013
【図５】
0004302805.tif 000014
【図６】
0004302805.tif 000015
【図７】
0004302805.tif 000016
【図８】
0004302805.tif 000017
【図９】
0004302805.tif 000018
【図１０】
0004302805.tif 000019
【図１１】
0004302805.tif 000020