JP2010269385A - Ｓ字加減速軌道生成方法および多関節型ロボットシステム - Google Patents

Abstract

【課題】加減速時の振動を極力抑えつつ、ロボットの動作サイクルタイム短縮や機構やモータへの過大な負荷を軽減することが可能なＳ字加減速軌道生成方法および多関節型ロボットシステムを提供する。
【解決手段】基準関節軸の暫定的なＳ字加減速軌道を生成し、この暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する。次に、予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節の推定トルクτを算出する。その後、Ｓ字加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクτが予め設定された目標トルクτ_tgになるように、基準関節軸の修正最大加速度または修正最小加速度を求める。
【選択図】図２

Description

本発明は、Ｓ字加減速軌道生成方法および多関節型ロボットシステムに係り、とりわけ加減速時の振動を極力抑えつつ、ロボットの動作サイクルタイムの短縮や機構やモータへの過大な負荷を軽減することが可能なＳ字加減速軌道生成方法および多関節型ロボットシステムに関する。

一般に、多関節型ロボットのアームを各関節軸を中心として回転させる際、回転を開始したからの時間と角速度との関係を示すグラフがＳ字を描くように制御（Ｓ字加減速制御）することが行われている。すなわち、回転開始時に角加速度を０から徐々に上昇させてゆき、次いで一定時間等角加速度とした後、角加速度を徐々に低下させ、０とする。この状態で一定時間等角速度でアームを回転した後、角加速度を０から徐々に低下させてゆき、一定時間負の等角加速度で回転させる。その後、角加速度を徐々に上昇させて０としている。

一方、特許文献１には、第ｉ軸における発生トルクが許容最大トルクとなるような限界加減速時間を求める技術が開示されている。

特開平７−２６１８２２号公報

しかしながら、特許文献１に記載された技術の場合、角加速度を変更するために加速時間全体を伸縮させている。この技術をＳ字加減速軌道制御に対して適用する場合、Ｓ字加速度到達時間（回転を開始してから等角加速度に達するまでの時間）も含めて時間を伸縮させることになる。この結果、動作パターンによっては、Ｓ字加速度到達時間が「アームの固有周波数」よりも小さくなることがある。これにより、振動が発生してしまう場合がある。この場合、制動時などに位置決め精度が低下し、非常に使いづらいものとなる。

本発明はこのような点を考慮してなされたものであり、Ｓ字加速度到達時間を一定に保持することにより、加減速時の振動を極力抑えつつ、ロボットの動作サイクルタイム短縮や機構やモータへの過大な負荷を軽減することが可能なＳ字加減速軌道生成方法および多関節型ロボットシステムを提供することを目的とする。

本発明は、複数の関節軸を有するロボットについて、各々の関節軸を駆動するためのＳ字加減速軌道を生成するＳ字加減速軌道生成方法において、ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する工程と、基準関節軸に対する暫定的なＳ字加減速軌道を生成する工程と、暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する工程と、予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する工程と、各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める工程と、このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する工程と備えたことを特徴とするＳ字加減速軌道生成方法である。

本発明は、基準関節軸の修正最大加速度または修正最小加速度を求める工程は、動力学モデルを用いることにより、暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度と、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクが予め設定された各関節軸の目標トルクに一致するとした場合の各関節軸の修正最大加速度または修正最小加速度との比である加速度変更率を全関節軸分算出する工程と、算出された各関節軸の加速度変更率のうち、最小となる最小加速度変更率を求める工程と、この最小加速度変更率を暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度に乗じることにより、基準関節軸の修正最大加速度または修正最小加速度を求める工程とを含むことを特徴とするＳ字加減速軌道生成方法である。

本発明は、複数の関節軸を有する多関節型ロボットシステムにおいて、ロボット本体と、ロボット本体を制御する制御装置とを備え、制御装置は、ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する基準関節軸決定処理部と、基準関節軸に対する暫定的なＳ字加減速軌道を生成する第１Ｓ字加減速軌道生成部と、暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する時刻算出部と、予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する演算処理部と、各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める角加速度変更処理部と、このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する第２Ｓ字加減速軌道生成部と有することを特徴とする多関節型ロボットシステムである。

本発明によれば、各関節軸の最大加速度到達時間または最小加速度到達時間（Ｓ字加速度到達時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める。この結果、加減速時の振動の発生を抑えつつ、ロボットの動作パターンや負荷条件に合わせて加速度を最適化することができる。また、Ｓ加減速軌道動作において、位置決め精度を低下させること無く、ロボットの関節軸をスムーズに加速、減速させることができる。

本発明の一実施の形態による多関節型ロボットシステムを示す概略構成図。 本発明の一実施の形態によるＳ字加減速軌道生成方法を示すフロー図。 暫定的なＳ字加減速軌道における、時刻と基準関節軸の角速度との関係および、時刻と基準関節軸の角加速度との関係を示すグラフ。 加速時における、時刻と基準関節軸の角速度との関係および、時刻と基準関節軸の角加速度との関係を示すグラフ。

以下、本発明の一実施の形態について、図１乃至図４を参照して説明する。ここで、図１乃至図４は、本発明の一実施の形態を示す図である。

（多関節型ロボットシステムの構成）
まず、図１により、多関節型ロボットシステムの全体構成について説明する。ここでは、多関節型ロボットシステムとして、３つの関節軸を有する多関節型ロボットを含むシステムを例に挙げて説明する。

図１に示すように、多関節型ロボットシステム１００は、ロボット本体１０と、ロボット本体１０に接続されるとともに、ロボット本体１０を制御する制御装置２０とを備えている。

このうちロボット本体１０は、床面に固定された基台１１と、基端側（基台１１）から先端側に向かって順次設けられ、互いに異なる３つの関節軸Ｊ₁〜Ｊ₃を介してそれぞれ連結された３つのアーム体（第１アーム体１２、第２アーム体１３、および第３アーム体１４）とを有している。

このうち第１アーム体１２は、第１関節軸Ｊ₁を中心に基台１１に対して回動可能となっている。また第１アーム体１２と第２アーム体１３との間に第２関節軸Ｊ₂が介在され、第２アーム体１３は、第２関節軸Ｊ₂を中心に回動可能となっている。また第２アーム体１３と第３アーム体１４との間に第３関節軸Ｊ₃が介在され、第３アーム体１４は、第３関節軸Ｊ₃を中心に回動可能となっている。

一方、制御装置２０は、ＲＡＭ３０と、ＲＡＭ３０に接続されたＣＰＵ４０とを有している。

このうちＲＡＭ３０には、教示情報３１と、Ｓ字加減速パラメータ情報３２と、モデルパラメータ情報３３と、目標トルク情報３４とが記憶されている。以下、これらの情報の概要について簡単に説明する。

教示情報３１には、ユーザーにより設定される動作プログラムに基づいて定められた、ロボットの動作目標位置（例えば教示点）および動作パターン（例えば補間方法）等の情報が含まれている。

Ｓ字加減速パラメータ情報３２には、暫定的なＳ字加減速軌道を生成する際に用いられる、各関節軸Ｊ₁〜Ｊ₃の最大角速度、各関節軸Ｊ₁〜Ｊ₃の最大角加速度および最小角加速度、ならびに各関節軸Ｊ₁〜Ｊ₃の最大角加速度到達時間および最小角加速度到達時間（以下、Ｓ字加速度到達時間ともいう）等の情報が含まれている。なお、Ｓ字加減速パラメータ情報３２に含まれる各種情報は、上述した動作プログラムに関わらず、予め関節軸Ｊ₁〜Ｊ₃毎に所定の値に設定されているものである。

モデルパラメータ情報３３には、モデルパラメータ（アーム質量（Ｋｇ）、リンクオフセット量（ｍｍ）、重心オフセット（ｍｍ）、重心周り慣性モーメント（Ｋｇ・ｍ²）等）、および各関節軸Ｊ₁〜Ｊ₃の摩擦係数等の情報が含まれている。

目標トルク情報３４には、各関節軸Ｊ₁〜Ｊ₃毎に予め設定された、各関節軸Ｊ₁〜Ｊ₃の目標トルクの情報が含まれている。

他方、ＣＰＵ４０は、プログラムで与えられた目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する基準関節軸決定処理部４８と、基準関節軸の暫定的なＳ字加減速軌道を生成する第１Ｓ字加減速軌道生成部４１と、後述する時刻ｔｍ１および時刻ｔｍ２を算出する時刻算出部４２と、動力学モデルを用いて演算処理を行う演算処理部４３とを有している。

またＣＰＵ４０は、基準関節軸の修正最大角加速度および修正最小角加速度を求める角加速度変更処理部４４と、基準関節軸の修正最大角加速度および修正最小角加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大角加速度および修正最小角加速度をもつよう修正した、修正Ｓ字加減速軌道を生成する第２Ｓ字加減速軌道生成部４５とを有している。

さらに、ＣＰＵ４０内には、基準関節軸の修正Ｓ字加減速軌道に基づいて処理周期毎の各関節軸Ｊ₁〜Ｊ₃の目標位置を算出する目標位置算出部４６と、目標位置に基づいてロボット本体１０を作動させるサーボ部４７とが設けられている。

（基準関節軸決定方法およびＳ字加減速軌道生成方法）
次に、図２乃至図４により、本実施の形態による基準関節軸決定方法およびＳ字加減速軌道生成方法について説明する。

まず、ＣＰＵ４０の基準関節軸決定処理部４８は、教示情報３１に含まれるロボットの動作目標位置および動作パターンと、Ｓ字加減速パラメータ情報３２に含まれる情報、具体的には、各関節軸Ｊ₁〜Ｊ₃の最大角速度とに基づいて、プログラムで与えられた目標位置までの各関節軸Ｊ₁〜Ｊ₃の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する（図２のステップＳ０）。

次にＣＰＵ４０の第１Ｓ字加減速軌道生成部４１は、教示情報３１に含まれるロボットの動作目標位置および動作パターンと、Ｓ字加減速パラメータ情報３２に含まれる情報、具体的には、各関節軸Ｊ₁〜Ｊ₃の最大角速度、各関節軸Ｊ₁〜Ｊ₃の最大角加速度および最小角加速度、ならびに各関節軸Ｊ₁〜Ｊ₃の最大角加速度到達時間および最小角加速度到達時間の情報とに基づいて、基準関節軸に対する暫定的なＳ字加減速軌道を生成する（図２のステップＳ１）。この暫定的なＳ字加減速軌道を求める方法は、従来一般的に行われるＳ字加減速軌道算出方法と同様にして行うことができる。

この暫定的なＳ字加減速軌道に基づくグラフを図３に示す。図３において、上段のグラフは、移動開始時刻（Ｔ₀）からの時刻と基準関節軸の角速度との関係を示している。また下段のグラフは、移動開始時刻（Ｔ₀）からの時刻と基準関節軸の角加速度との関係を示している。

続いて、ＣＰＵ４０の時刻算出部４２は、暫定的なＳ字加減速軌道において、加速時に角加速度が最大となる区間（図３では時刻Ｔ₁と時刻Ｔ₂との間の区間）のうち、角速度が最大となる時刻ｔｍ１（図３では時刻Ｔ₂）を算出する。同様に、ＣＰＵ４０の時刻算出部４２は、減速時に角加速度が最小となる区間（図３では時刻Ｔ₅と時刻Ｔ₆との間の区間）のうち、角速度が最大となる時刻ｔｍ２（図３では時刻Ｔ₅）を算出する（図２のステップＳ２）。

次に、ＣＰＵ４０の演算処理部４３は、モデルパラメータ情報３３と、予め設定された動力学モデルとを用いて、時刻ｔｍ１および時刻ｔｍ２における各関節軸Ｊ₁〜Ｊ₃の推定トルクを算出する（図２のステップＳ３）。なお上述したように、モデルパラメータ情報３３には、モデルパラメータ（アーム質量（Ｋｇ）、リンクオフセット量（ｍｍ）、重心オフセット（ｍｍ）、重心周り慣性モーメント（Ｋｇ・ｍ²）等）、および各関節軸Ｊ₁〜Ｊ₃の摩擦係数等の情報が含まれる。

ここで動力学モデルは、以下の式で示される。
τ＝Ｍ（Θ）Θ”＋Ｂ（Θ）［Θ_a’Θ_b’］＋Ｃ（Θ）［Θ’²］＋Ｇ（Θ）＋Ｄ（Θ’）＋τ_Fcｓｇｎ（Θ’） ・・・（１）

なお、動力学モデルの式（１）中、各項の意味は以下の通りである。
τ：各関節軸の推定トルク行列（ｎ行１列の行列）
Θ、Θ’、Θ”：それぞれ関節軸角度行列、関節軸角速度行列、関節軸角加速度行列（ｎ行１列の行列）
Θ’²：同じ関節軸の角速度積行列（ｎ行１列の行列）
Θ_a’・Θ_b’：異なる軸の角速度積行列（｛ｎ・（ｎ−１）／２｝行１列の行列）
Ｍ（Θ）：慣性（質量）行列（ｎ行ｎ列の行列）
Ｂ（Θ）：コリオリ力項行列（ｎ行｛ｎ・（ｎ−１）／２｝列の行列）
Ｃ（Θ）：遠心力行列（ｎ行ｎ列の行列）
Ｇ（Θ）：重力行列（ｎ行ｎ列の行列）
Ｄ（Θ’）：粘性摩擦力行列（ｎ行１列の行列）
τ_Fcｓｇｎ（Θ’）：クーロン摩擦力行列（ｎ行１列の行列）
なお、上記において、ｎは関節軸Ｊ₁〜Ｊ₃の軸数（本実施の形態においてはｎ＝３）を表している。

ところで、動力学モデルの式（１）のうち、コリオリ力項行列、遠心力行列、重力行列、粘性摩擦力行列およびクーロン摩擦力行列は、いずれも角加速度Θ”の関数ではないので、これらの行列を合算してτ_other（ｎ行１列の行列）とおく。この場合、上述した動力学モデル（１）は、以下のように表すことができる。

τ＝Ｍ（Θ）Θ”＋τ_other（Θ、Θ’） ・・・（２）
（但し、τ_other（Θ、Θ’） ＝Ｂ（Θ）［Θ_a’Θ_b’］＋Ｃ（Θ）［Θ’²］＋Ｇ（Θ）＋Ｄ（Θ’）＋τ_Fcｓｇｎ（Θ’））

続いて、ＣＰＵ４０の角加速度変更処理部４４は、各関節軸Ｊ₁〜Ｊ₃毎に加速を開始してから最大角加速度に到達するまでの最大角加速度到達時間（図３では時刻Ｔ₀と時刻Ｔ₁との間の時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大角加速度を求める。

同様に、ＣＰＵ４０の角加速度変更処理部４４は、各関節軸Ｊ₁〜Ｊ₃毎に減速を開始してから最小角加速度に到達するまでの最小角加速度到達時間（図３では時刻Ｔ₄と時刻Ｔ₅との間の時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最小角加速度を求める。

具体的には、以下のようにして基準関節軸の修正最大角加速度および修正最小角加速度を求める。

まず、時刻ｔｍ１および時刻ｔｍ２において、Ｓ字加速度到達時間は一定としたまま、角加速度のみを変更することにより、目標トルク（τ_tg）（ｎ行１列の行列）を発生させることを考える。

ここで、上述した時刻ｔｍ１（図３では時刻Ｔ₂）における関節軸Ｊ₁〜Ｊ₃の角度行列、角速度行列、角加速度行列を、それぞれΘ_Acc、Θ_Acc’、Θ_Acc”（それぞれｎ行１列の行列）とおく。

また、上述した時刻ｔｍ２（図３では時刻Ｔ₅）における関節軸Ｊ₁〜Ｊ₃の角度行列、角速度行列、角加速度行列を、それぞれΘ_Dec、Θ_Dec’、Θ_Dec”（それぞれｎ行１列の行列）とおく。

さらに、修正最大角加速度および修正最小角加速度を求めるための角加速度変更率を含む行列を、α_Acc（加速時）、α_Dec（減速時）（それぞれｎ行１列の行列）とする。

なお、加速時の角加速度変更率（α_Accの各成分）は、上述した暫定的なＳ字加減速軌道（図３、および図４（ａ）（ｂ）の点線参照）における各関節軸Ｊ₁〜Ｊ₃の最大角加速度（Θ_Acc”の各成分）と、時刻ｔｍ１における各関節軸Ｊ₁〜Ｊ₃の推定トルク（τ）が予め設定された各関節軸Ｊ₁〜Ｊ₃の目標トルク（τ_tg）に一致するとした場合（すなわちτ＝τ_tg）の各関節軸Ｊ₁〜Ｊ₃の修正最大角加速度（後述するΘ_{Acc_N}”の各成分）との比となる（図４（ｂ）参照）。すなわち、加速時の角加速度変更率は、｛（Θ_{Acc_N}”の各成分）／（Θ_Acc”の各成分）｝となる。

同様に、減速時の角加速度変更率（α_Decの各成分）は、上述した暫定的なＳ字加減速軌道（図３、および図４（ａ）（ｂ）の点線参照）における各関節軸Ｊ₁〜Ｊ₃の最小角加速度（Θ_Dec”の各成分）と、時刻ｔｍ２における各関節軸Ｊ₁〜Ｊ₃の推定トルク（τ）が予め設定された各関節軸Ｊ₁〜Ｊ₃の目標トルク（τ_tg）に一致するとした場合（すなわちτ＝τ_tg）の各関節軸Ｊ₁〜Ｊ₃の修正最小角加速度（後述するΘ_{Dec_N}”の各成分）との比となる。すなわち、減速時の角加速度変更率は、｛（Θ_{Dec_N}”の各成分）／（Θ_Dec”の各成分）｝となる。

なお、図４（ａ）は、加速時における、移動開始時刻（Ｔ₀）からの時間と各関節軸Ｊ₁〜Ｊ₃の角速度との関係を示しており、図４（ｂ）は、加速時における、移動開始時刻（Ｔ₀）からの時間と各関節軸Ｊ₁〜Ｊ₃の角加速度との関係を示している。図４（ａ）（ｂ）において、上述した暫定的なＳ字加減速軌道のグラフを点線で示し、後述する修正Ｓ字加減速軌道のグラフを実線で示している。

この場合、上述した動力学モデル（２）は、以下のように書き直すことができる。

τ_tg＝Ｍ（Θ_Acc）α_AccΘ_Acc”＋τ_other（Θ_Acc、Θ_Acc’） ・・・（３）
τ_tg＝Ｍ（Θ_Dec）α_DecΘ_Dec”＋τ_other（Θ_Dec、Θ_Dec’） ・・・（４）

この（３）式および（４）式を変形すると、角加速度変更率行列（α_Acc、α_Dec）は、以下のようにして求めることができる。

このようにして、上記（５）式を用いることにより、各関節軸Ｊ₁〜Ｊ₃の加速時の角加速度変更率行列（α_Acc）を求め、全ての各関節軸Ｊ₁〜Ｊ₃の加速時の角加速度変更率（α_Accの各成分）を算出する（図２のステップＳ４）。

同様に、上記（６）式を用いることにより、各関節軸Ｊ₁〜Ｊ₃の減速時の角加速度変更率行列（α_Dec）を求め、全ての関節軸Ｊ₁〜Ｊ₃の減速時の角加速度変更率（α_Decの各成分）を算出する（図２のステップＳ４）。

次に、ＣＰＵ４０の角加速度変更処理部４４は、全ての関節軸Ｊ₁〜Ｊ₃の加速時の角加速度変更率（α_Acc）の中からその値が最小となる関節軸を選択し、この角加速度変更率を最小角加速度変更率（α_{Acc_MIN}）とする（図２のステップＳ５）。

同様に、ＣＰＵ４０の角加速度変更処理部４４は、全ての関節軸Ｊ₁〜Ｊ₃の減速時の角加速度変更率（α_Dec）の中からその値が最小となる関節軸を選択し、この角加速度変更率を最小角加速度変更率（α_{Dec_MIN}）とする（図２のステップＳ５）。

続いて、この最小角加速度変更率（α_{Acc_MIN}、α_{Dec_MIN}）を、暫定的なＳ字加減速軌道における基準関節軸の最大角加速度および最小角加速度にそれぞれ乗じることにより、基準関節軸の修正最大角加速度および修正最小角加速度を求める（図２のステップＳ６）。

すなわち、加速時における基準関節軸の修正最大角加速度を、Θ_{Acc_NB}”、暫定的なＳ字加減速軌道における基準関節軸の最大角加速度をΘ_{Acc_B}”とおくと、この修正最大角加速度Θ_{Acc_NB}”は、、
Θ_{Acc_NB}”＝α_{Acc_MIN}・Θ_{Acc_B}” ・・・（７）
により求められる。

同様に、減速時における基準関節軸の修正最小角加速度を、Θ_{Dec_NB}”、暫定的なＳ字加減速軌道における基準関節軸の最小角加速度をΘ_{Dec_B}”とおくと、この修正最小角加速度Θ_{Dec_NB}”は、
Θ_{Dec_NB}”＝α_{Dec_MIN}・Θ_{Dec_B}” ・・・（８）
により求められる。

続いて、ＣＰＵ４０の第２Ｓ字加減速軌道生成部４５は、この変更された基準関節軸の修正最大角加速度（Θ_{Acc_NB}”） および修正最小角加速度（Θ_{Dec_NB}”）に基づいて、基準関節軸のＳ字加減速軌道が修正最大角加速度（Θ_{Acc_NB}”）または修正最小角加速度（Θ_{Dec_NB}”）をもつよう修正した修正Ｓ字加減速軌道を生成する（図２のステップＳ７）。

その後、ＣＰＵ４０の目標位置算出部４６が、基準関節軸の修正Ｓ字加減速軌道に基づいて処理周期毎の各関節軸Ｊ₁〜Ｊ₃の目標位置を算出する。次いで、サーボ部４７が、この目標位置に基づいてロボット本体１０を作動させる。

このように本実施の形態によれば、各関節軸Ｊ₁〜Ｊ₃のＳ字加速度到達時間（最大角加速度到達時間および最小角加速度到達時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、修正最大角加速度および修正最小角加速度を求めている。この結果、加減速時の振動の発生を抑えつつ、ロボットの動作パターンや負荷条件に合わせて角加速度を最適化することができる。また、Ｓ加減速軌道動作において、位置決め精度を低下させること無く，スムーズに加速、減速させることができる。

なお本実施の形態において、加速時および減速時の両方の場合に修正最大角加速度および修正最小角加速度を求めているが、これに限らず、加速時または減速時の一方の場合にのみ、上述した方法で修正最大角加速度または修正最小角加速度を求め、この修正最大角加速度または修正最小角加速度のいずれか一方に基づいて修正Ｓ字加減速軌道を生成しても良い。

また本実施の形態において、各関節軸Ｊ₁〜Ｊ₃がそれぞれ回転動作する場合について説明してきたが、直線運動する関節についても同様の考え方を適用することができる。この場合、本実施の形態中の「角度」、「角速度」、および「角加速度」の語をそれぞれ「位置」、（狭義の）「速度」、および（狭義の）「加速度」と置き換えて読むことができる。

１０ ロボット本体
１１ 基台
１２ 第１アーム体
１３ 第２アーム体
１４ 第３アーム体
２０ 制御装置
３０ ＲＡＭ
３１ 教示情報
３２ Ｓ字加減速パラメータ情報
３３ モデルパラメータ情報
３４ 目標トルク情報
４０ ＣＰＵ
４１ 第１Ｓ字加減速軌道生成部
４２ 時刻算出部
４３ 演算処理部
４４ 角加速度変更処理部
４５ 第２Ｓ字加減速軌道生成部
４６ 目標位置算出部
４７ サーボ部
４８ 基準関節軸決定処理部
１００ 多関節型ロボットシステム
Ｊ₁〜Ｊ₃ 関節軸

Claims (3)

1. 複数の関節軸を有するロボットについて、各々の関節軸を駆動するためのＳ字加減速軌道を生成するＳ字加減速軌道生成方法において、
   ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する工程と、
   基準関節軸に対する暫定的なＳ字加減速軌道を生成する工程と、
   暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する工程と、
   予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する工程と、
   各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める工程と、
   このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する工程と備えたことを特徴とするＳ字加減速軌道生成方法。
2. 基準関節軸の修正最大加速度または修正最小加速度を求める工程は、
   動力学モデルを用いることにより、暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度と、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクが予め設定された各関節軸の目標トルクに一致するとした場合の各関節軸の修正最大加速度または修正最小加速度との比である加速度変更率を全関節軸分算出する工程と、
   算出された各関節軸の加速度変更率のうち、最小となる最小加速度変更率を求める工程と、
   この最小加速度変更率を暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度に乗じることにより、基準関節軸の修正最大加速度または修正最小加速度を求める工程とを含むことを特徴とする請求項１記載のＳ字加減速軌道生成方法。
3. 複数の関節軸を有する多関節型ロボットシステムにおいて、
   ロボット本体と、
   ロボット本体を制御する制御装置とを備え、
   制御装置は、
   ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する基準関節軸決定処理部と、
   基準関節軸に対する暫定的なＳ字加減速軌道を生成する第１Ｓ字加減速軌道生成部と、
   暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する時刻算出部と、
   予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する演算処理部と、
   各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める角加速度変更処理部と、
   このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する第２Ｓ字加減速軌道生成部と有することを特徴とする多関節型ロボットシステム。

Images