(57)【要約】【課題】加減速時の振動を極力抑えつつ、ロボットの動作サイクルタイム短縮や機構やモータへの過大な負荷を軽減することが可能なＳ字加減速軌道生成方法および多関節型ロボットシステムを提供する。【解決手段】基準関節軸の暫定的なＳ字加減速軌道を生成し、この暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する。次に、予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節の推定トルクτを算出する。その後、Ｓ字加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクτが予め設定された目標トルクτtgになるように、基準関節軸の修正最大加速度または修正最小加速度を求める。【選択図】図２
2010269385.tif 000002
【特許請求の範囲】
【請求項１】
複数の関節軸を有するロボットについて、各々の関節軸を駆動するためのＳ字加減速軌道を生成するＳ字加減速軌道生成方法において、
ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する工程と、
基準関節軸に対する暫定的なＳ字加減速軌道を生成する工程と、
暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する工程と、
予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する工程と、
各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める工程と、
このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する工程と備えたことを特徴とするＳ字加減速軌道生成方法。
【請求項２】
基準関節軸の修正最大加速度または修正最小加速度を求める工程は、
動力学モデルを用いることにより、暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度と、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクが予め設定された各関節軸の目標トルクに一致するとした場合の各関節軸の修正最大加速度または修正最小加速度との比である加速度変更率を全関節軸分算出する工程と、
算出された各関節軸の加速度変更率のうち、最小となる最小加速度変更率を求める工程と、
この最小加速度変更率を暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度に乗じることにより、基準関節軸の修正最大加速度または修正最小加速度を求める工程とを含むことを特徴とする請求項１記載のＳ字加減速軌道生成方法。
【請求項３】
複数の関節軸を有する多関節型ロボットシステムにおいて、
ロボット本体と、
ロボット本体を制御する制御装置とを備え、
制御装置は、
ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する基準関節軸決定処理部と、
基準関節軸に対する暫定的なＳ字加減速軌道を生成する第１Ｓ字加減速軌道生成部と、
暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する時刻算出部と、
予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する演算処理部と、
各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める角加速度変更処理部と、
このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する第２Ｓ字加減速軌道生成部と有することを特徴とする多関節型ロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、Ｓ字加減速軌道生成方法および多関節型ロボットシステムに係り、とりわけ加減速時の振動を極力抑えつつ、ロボットの動作サイクルタイムの短縮や機構やモータへの過大な負荷を軽減することが可能なＳ字加減速軌道生成方法および多関節型ロボットシステムに関する。
【背景技術】
【０００２】
一般に、多関節型ロボットのアームを各関節軸を中心として回転させる際、回転を開始したからの時間と角速度との関係を示すグラフがＳ字を描くように制御（Ｓ字加減速制御）することが行われている。すなわち、回転開始時に角加速度を０から徐々に上昇させてゆき、次いで一定時間等角加速度とした後、角加速度を徐々に低下させ、０とする。この状態で一定時間等角速度でアームを回転した後、角加速度を０から徐々に低下させてゆき、一定時間負の等角加速度で回転させる。その後、角加速度を徐々に上昇させて０としている。
【０００３】
一方、特許文献１には、第ｉ軸における発生トルクが許容最大トルクとなるような限界加減速時間を求める技術が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−２６１８２２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１に記載された技術の場合、角加速度を変更するために加速時間全体を伸縮させている。この技術をＳ字加減速軌道制御に対して適用する場合、Ｓ字加速度到達時間（回転を開始してから等角加速度に達するまでの時間）も含めて時間を伸縮させることになる。この結果、動作パターンによっては、Ｓ字加速度到達時間が「アームの固有周波数」よりも小さくなることがある。これにより、振動が発生してしまう場合がある。この場合、制動時などに位置決め精度が低下し、非常に使いづらいものとなる。
【０００６】
本発明はこのような点を考慮してなされたものであり、Ｓ字加速度到達時間を一定に保持することにより、加減速時の振動を極力抑えつつ、ロボットの動作サイクルタイム短縮や機構やモータへの過大な負荷を軽減することが可能なＳ字加減速軌道生成方法および多関節型ロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明は、複数の関節軸を有するロボットについて、各々の関節軸を駆動するためのＳ字加減速軌道を生成するＳ字加減速軌道生成方法において、ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する工程と、基準関節軸に対する暫定的なＳ字加減速軌道を生成する工程と、暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する工程と、予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する工程と、各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める工程と、このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する工程と備えたことを特徴とするＳ字加減速軌道生成方法である。
【０００８】
本発明は、基準関節軸の修正最大加速度または修正最小加速度を求める工程は、動力学モデルを用いることにより、暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度と、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクが予め設定された各関節軸の目標トルクに一致するとした場合の各関節軸の修正最大加速度または修正最小加速度との比である加速度変更率を全関節軸分算出する工程と、算出された各関節軸の加速度変更率のうち、最小となる最小加速度変更率を求める工程と、この最小加速度変更率を暫定的なＳ字加減速軌道における基準関節軸の最大加速度または最小加速度に乗じることにより、基準関節軸の修正最大加速度または修正最小加速度を求める工程とを含むことを特徴とするＳ字加減速軌道生成方法である。
【０００９】
本発明は、複数の関節軸を有する多関節型ロボットシステムにおいて、ロボット本体と、ロボット本体を制御する制御装置とを備え、制御装置は、ロボットの動作目標位置と動作パターンとに基づいて、動作目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する基準関節軸決定処理部と、基準関節軸に対する暫定的なＳ字加減速軌道を生成する第１Ｓ字加減速軌道生成部と、暫定的なＳ字加減速軌道において、加速度が最大となる区間のうち速度が最大となる時刻ｔｍ１を算出し、または加速度が最小となる区間のうち速度が最大となる時刻ｔｍ２を算出する時刻算出部と、予め設定された動力学モデルを用いて、時刻ｔｍ１または時刻ｔｍ２における各関節軸の推定トルクを算出する演算処理部と、各関節軸毎に加速を開始してから最大加速度に到達するまでの最大加速度到達時間、または減速を開始してから最小加速度に到達するまでの最小加速度到達時間を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める角加速度変更処理部と、このようにして求めた基準関節軸の修正最大加速度または修正最小加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大加速度または修正最小加速度をもつよう修正した修正Ｓ字加減速軌道を生成する第２Ｓ字加減速軌道生成部と有することを特徴とする多関節型ロボットシステムである。
【発明の効果】
【００１０】
本発明によれば、各関節軸の最大加速度到達時間または最小加速度到達時間（Ｓ字加速度到達時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大加速度または修正最小加速度を求める。この結果、加減速時の振動の発生を抑えつつ、ロボットの動作パターンや負荷条件に合わせて加速度を最適化することができる。また、Ｓ加減速軌道動作において、位置決め精度を低下させること無く、ロボットの関節軸をスムーズに加速、減速させることができる。
【図面の簡単な説明】
【００１１】
【図１】本発明の一実施の形態による多関節型ロボットシステムを示す概略構成図。
【図２】本発明の一実施の形態によるＳ字加減速軌道生成方法を示すフロー図。
【図３】暫定的なＳ字加減速軌道における、時刻と基準関節軸の角速度との関係および、時刻と基準関節軸の角加速度との関係を示すグラフ。
【図４】加速時における、時刻と基準関節軸の角速度との関係および、時刻と基準関節軸の角加速度との関係を示すグラフ。
【発明を実施するための形態】
【００１２】
以下、本発明の一実施の形態について、図１乃至図４を参照して説明する。ここで、図１乃至図４は、本発明の一実施の形態を示す図である。
【００１３】
（多関節型ロボットシステムの構成）
まず、図１により、多関節型ロボットシステムの全体構成について説明する。ここでは、多関節型ロボットシステムとして、３つの関節軸を有する多関節型ロボットを含むシステムを例に挙げて説明する。
【００１４】
図１に示すように、多関節型ロボットシステム１００は、ロボット本体１０と、ロボット本体１０に接続されるとともに、ロボット本体１０を制御する制御装置２０とを備えている。
【００１５】
このうちロボット本体１０は、床面に固定された基台１１と、基端側（基台１１）から先端側に向かって順次設けられ、互いに異なる３つの関節軸Ｊ1〜Ｊ3を介してそれぞれ連結された３つのアーム体（第１アーム体１２、第２アーム体１３、および第３アーム体１４）とを有している。
【００１６】
このうち第１アーム体１２は、第１関節軸Ｊ1を中心に基台１１に対して回動可能となっている。また第１アーム体１２と第２アーム体１３との間に第２関節軸Ｊ2が介在され、第２アーム体１３は、第２関節軸Ｊ2を中心に回動可能となっている。また第２アーム体１３と第３アーム体１４との間に第３関節軸Ｊ3が介在され、第３アーム体１４は、第３関節軸Ｊ3を中心に回動可能となっている。
【００１７】
一方、制御装置２０は、ＲＡＭ３０と、ＲＡＭ３０に接続されたＣＰＵ４０とを有している。
【００１８】
このうちＲＡＭ３０には、教示情報３１と、Ｓ字加減速パラメータ情報３２と、モデルパラメータ情報３３と、目標トルク情報３４とが記憶されている。以下、これらの情報の概要について簡単に説明する。
【００１９】
教示情報３１には、ユーザーにより設定される動作プログラムに基づいて定められた、ロボットの動作目標位置（例えば教示点）および動作パターン（例えば補間方法）等の情報が含まれている。
【００２０】
Ｓ字加減速パラメータ情報３２には、暫定的なＳ字加減速軌道を生成する際に用いられる、各関節軸Ｊ1〜Ｊ3の最大角速度、各関節軸Ｊ1〜Ｊ3の最大角加速度および最小角加速度、ならびに各関節軸Ｊ1〜Ｊ3の最大角加速度到達時間および最小角加速度到達時間（以下、Ｓ字加速度到達時間ともいう）等の情報が含まれている。なお、Ｓ字加減速パラメータ情報３２に含まれる各種情報は、上述した動作プログラムに関わらず、予め関節軸Ｊ1〜Ｊ3毎に所定の値に設定されているものである。
【００２１】
モデルパラメータ情報３３には、モデルパラメータ（アーム質量（Ｋｇ）、リンクオフセット量（ｍｍ）、重心オフセット（ｍｍ）、重心周り慣性モーメント（Ｋｇ・ｍ2）等）、および各関節軸Ｊ1〜Ｊ3の摩擦係数等の情報が含まれている。
【００２２】
目標トルク情報３４には、各関節軸Ｊ1〜Ｊ3毎に予め設定された、各関節軸Ｊ1〜Ｊ3の目標トルクの情報が含まれている。
【００２３】
他方、ＣＰＵ４０は、プログラムで与えられた目標位置までの各関節軸の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する基準関節軸決定処理部４８と、基準関節軸の暫定的なＳ字加減速軌道を生成する第１Ｓ字加減速軌道生成部４１と、後述する時刻ｔｍ１および時刻ｔｍ２を算出する時刻算出部４２と、動力学モデルを用いて演算処理を行う演算処理部４３とを有している。
【００２４】
またＣＰＵ４０は、基準関節軸の修正最大角加速度および修正最小角加速度を求める角加速度変更処理部４４と、基準関節軸の修正最大角加速度および修正最小角加速度に基づいて、基準関節軸のＳ字加減速軌道がこの修正最大角加速度および修正最小角加速度をもつよう修正した、修正Ｓ字加減速軌道を生成する第２Ｓ字加減速軌道生成部４５とを有している。
【００２５】
さらに、ＣＰＵ４０内には、基準関節軸の修正Ｓ字加減速軌道に基づいて処理周期毎の各関節軸Ｊ1〜Ｊ3の目標位置を算出する目標位置算出部４６と、目標位置に基づいてロボット本体１０を作動させるサーボ部４７とが設けられている。
【００２６】
（基準関節軸決定方法およびＳ字加減速軌道生成方法）
次に、図２乃至図４により、本実施の形態による基準関節軸決定方法およびＳ字加減速軌道生成方法について説明する。
【００２７】
まず、ＣＰＵ４０の基準関節軸決定処理部４８は、教示情報３１に含まれるロボットの動作目標位置および動作パターンと、Ｓ字加減速パラメータ情報３２に含まれる情報、具体的には、各関節軸Ｊ1〜Ｊ3の最大角速度とに基づいて、プログラムで与えられた目標位置までの各関節軸Ｊ1〜Ｊ3の移動時間を求め、最も移動時間の長い関節軸を基準関節軸に決定する（図２のステップＳ０）。
【００２８】
次にＣＰＵ４０の第１Ｓ字加減速軌道生成部４１は、教示情報３１に含まれるロボットの動作目標位置および動作パターンと、Ｓ字加減速パラメータ情報３２に含まれる情報、具体的には、各関節軸Ｊ1〜Ｊ3の最大角速度、各関節軸Ｊ1〜Ｊ3の最大角加速度および最小角加速度、ならびに各関節軸Ｊ1〜Ｊ3の最大角加速度到達時間および最小角加速度到達時間の情報とに基づいて、基準関節軸に対する暫定的なＳ字加減速軌道を生成する（図２のステップＳ１）。この暫定的なＳ字加減速軌道を求める方法は、従来一般的に行われるＳ字加減速軌道算出方法と同様にして行うことができる。
【００２９】
この暫定的なＳ字加減速軌道に基づくグラフを図３に示す。図３において、上段のグラフは、移動開始時刻（Ｔ0）からの時刻と基準関節軸の角速度との関係を示している。また下段のグラフは、移動開始時刻（Ｔ0）からの時刻と基準関節軸の角加速度との関係を示している。
【００３０】
続いて、ＣＰＵ４０の時刻算出部４２は、暫定的なＳ字加減速軌道において、加速時に角加速度が最大となる区間（図３では時刻Ｔ1と時刻Ｔ2との間の区間）のうち、角速度が最大となる時刻ｔｍ１（図３では時刻Ｔ2）を算出する。同様に、ＣＰＵ４０の時刻算出部４２は、減速時に角加速度が最小となる区間（図３では時刻Ｔ5と時刻Ｔ6との間の区間）のうち、角速度が最大となる時刻ｔｍ２（図３では時刻Ｔ5）を算出する（図２のステップＳ２）。
【００３１】
次に、ＣＰＵ４０の演算処理部４３は、モデルパラメータ情報３３と、予め設定された動力学モデルとを用いて、時刻ｔｍ１および時刻ｔｍ２における各関節軸Ｊ1〜Ｊ3の推定トルクを算出する（図２のステップＳ３）。なお上述したように、モデルパラメータ情報３３には、モデルパラメータ（アーム質量（Ｋｇ）、リンクオフセット量（ｍｍ）、重心オフセット（ｍｍ）、重心周り慣性モーメント（Ｋｇ・ｍ2）等）、および各関節軸Ｊ1〜Ｊ3の摩擦係数等の情報が含まれる。
【００３２】
ここで動力学モデルは、以下の式で示される。
τ＝Ｍ（Θ）Θ”＋Ｂ（Θ）［Θa’Θb’］＋Ｃ（Θ）［Θ’2］＋Ｇ（Θ）＋Ｄ（Θ’）＋τFcｓｇｎ（Θ’）  ・・・（１）
【００３３】
なお、動力学モデルの式（１）中、各項の意味は以下の通りである。
τ：各関節軸の推定トルク行列（ｎ行１列の行列）
Θ、Θ’、Θ”：それぞれ関節軸角度行列、関節軸角速度行列、関節軸角加速度行列（ｎ行１列の行列）
Θ’2：同じ関節軸の角速度積行列（ｎ行１列の行列）
Θa’・Θb’：異なる軸の角速度積行列（｛ｎ・（ｎ−１）／２｝行１列の行列）
Ｍ（Θ）：慣性（質量）行列（ｎ行ｎ列の行列）
Ｂ（Θ）：コリオリ力項行列（ｎ行｛ｎ・（ｎ−１）／２｝列の行列）
Ｃ（Θ）：遠心力行列（ｎ行ｎ列の行列）
Ｇ（Θ）：重力行列（ｎ行ｎ列の行列）
Ｄ（Θ’）：粘性摩擦力行列（ｎ行１列の行列）
τFcｓｇｎ（Θ’）：クーロン摩擦力行列（ｎ行１列の行列）
なお、上記において、ｎは関節軸Ｊ1〜Ｊ3の軸数（本実施の形態においてはｎ＝３）を表している。
【００３４】
ところで、動力学モデルの式（１）のうち、コリオリ力項行列、遠心力行列、重力行列、粘性摩擦力行列およびクーロン摩擦力行列は、いずれも角加速度Θ”の関数ではないので、これらの行列を合算してτother（ｎ行１列の行列）とおく。この場合、上述した動力学モデル（１）は、以下のように表すことができる。
【００３５】
τ＝Ｍ（Θ）Θ”＋τother（Θ、Θ’）  ・・・（２）
（但し、τother（Θ、Θ’） ＝Ｂ（Θ）［Θa’Θb’］＋Ｃ（Θ）［Θ’2］＋Ｇ（Θ）＋Ｄ（Θ’）＋τFcｓｇｎ（Θ’））
【００３６】
続いて、ＣＰＵ４０の角加速度変更処理部４４は、各関節軸Ｊ1〜Ｊ3毎に加速を開始してから最大角加速度に到達するまでの最大角加速度到達時間（図３では時刻Ｔ0と時刻Ｔ1との間の時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最大角加速度を求める。
【００３７】
同様に、ＣＰＵ４０の角加速度変更処理部４４は、各関節軸Ｊ1〜Ｊ3毎に減速を開始してから最小角加速度に到達するまでの最小角加速度到達時間（図３では時刻Ｔ4と時刻Ｔ5との間の時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、基準関節軸の修正最小角加速度を求める。
【００３８】
具体的には、以下のようにして基準関節軸の修正最大角加速度および修正最小角加速度を求める。
【００３９】
まず、時刻ｔｍ１および時刻ｔｍ２において、Ｓ字加速度到達時間は一定としたまま、角加速度のみを変更することにより、目標トルク（τtg）（ｎ行１列の行列）を発生させることを考える。
【００４０】
ここで、上述した時刻ｔｍ１（図３では時刻Ｔ2）における関節軸Ｊ1〜Ｊ3の角度行列、角速度行列、角加速度行列を、それぞれΘAcc、ΘAcc’、ΘAcc”（それぞれｎ行１列の行列）とおく。
【００４１】
また、上述した時刻ｔｍ２（図３では時刻Ｔ5）における関節軸Ｊ1〜Ｊ3の角度行列、角速度行列、角加速度行列を、それぞれΘDec、ΘDec’、ΘDec”（それぞれｎ行１列の行列）とおく。
【００４２】
さらに、修正最大角加速度および修正最小角加速度を求めるための角加速度変更率を含む行列を、αAcc（加速時）、αDec（減速時）（それぞれｎ行１列の行列）とする。
【００４３】
なお、加速時の角加速度変更率（αAccの各成分）は、上述した暫定的なＳ字加減速軌道（図３、および図４（ａ）（ｂ）の点線参照）における各関節軸Ｊ1〜Ｊ3の最大角加速度（ΘAcc”の各成分）と、時刻ｔｍ１における各関節軸Ｊ1〜Ｊ3の推定トルク（τ）が予め設定された各関節軸Ｊ1〜Ｊ3の目標トルク（τtg）に一致するとした場合（すなわちτ＝τtg）の各関節軸Ｊ1〜Ｊ3の修正最大角加速度（後述するΘAcc_N”の各成分）との比となる（図４（ｂ）参照）。すなわち、加速時の角加速度変更率は、｛（ΘAcc_N”の各成分）／（ΘAcc”の各成分）｝となる。
【００４４】
同様に、減速時の角加速度変更率（αDecの各成分）は、上述した暫定的なＳ字加減速軌道（図３、および図４（ａ）（ｂ）の点線参照）における各関節軸Ｊ1〜Ｊ3の最小角加速度（ΘDec”の各成分）と、時刻ｔｍ２における各関節軸Ｊ1〜Ｊ3の推定トルク（τ）が予め設定された各関節軸Ｊ1〜Ｊ3の目標トルク（τtg）に一致するとした場合（すなわちτ＝τtg）の各関節軸Ｊ1〜Ｊ3の修正最小角加速度（後述するΘDec_N”の各成分）との比となる。すなわち、減速時の角加速度変更率は、｛（ΘDec_N”の各成分）／（ΘDec”の各成分）｝となる。
【００４５】
なお、図４（ａ）は、加速時における、移動開始時刻（Ｔ0）からの時間と各関節軸Ｊ1〜Ｊ3の角速度との関係を示しており、図４（ｂ）は、加速時における、移動開始時刻（Ｔ0）からの時間と各関節軸Ｊ1〜Ｊ3の角加速度との関係を示している。図４（ａ）（ｂ）において、上述した暫定的なＳ字加減速軌道のグラフを点線で示し、後述する修正Ｓ字加減速軌道のグラフを実線で示している。
【００４６】
この場合、上述した動力学モデル（２）は、以下のように書き直すことができる。
【００４７】
τtg＝Ｍ（ΘAcc）αAccΘAcc”＋τother（ΘAcc、ΘAcc’）  ・・・（３）
τtg＝Ｍ（ΘDec）αDecΘDec”＋τother（ΘDec、ΘDec’）  ・・・（４）
【００４８】
この（３）式および（４）式を変形すると、角加速度変更率行列（αAcc、αDec）は、以下のようにして求めることができる。
【００４９】
【数１】
2010269385.tif 000003
【００５０】
このようにして、上記（５）式を用いることにより、各関節軸Ｊ1〜Ｊ3の加速時の角加速度変更率行列（αAcc）を求め、全ての各関節軸Ｊ1〜Ｊ3の加速時の角加速度変更率（αAccの各成分）を算出する（図２のステップＳ４）。
【００５１】
同様に、上記（６）式を用いることにより、各関節軸Ｊ1〜Ｊ3の減速時の角加速度変更率行列（αDec）を求め、全ての関節軸Ｊ1〜Ｊ3の減速時の角加速度変更率（αDecの各成分）を算出する（図２のステップＳ４）。
【００５２】
次に、ＣＰＵ４０の角加速度変更処理部４４は、全ての関節軸Ｊ1〜Ｊ3の加速時の角加速度変更率（αAcc）の中からその値が最小となる関節軸を選択し、この角加速度変更率を最小角加速度変更率（αAcc_MIN）とする（図２のステップＳ５）。
【００５３】
同様に、ＣＰＵ４０の角加速度変更処理部４４は、全ての関節軸Ｊ1〜Ｊ3の減速時の角加速度変更率（α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”）をもつよう修正した修正Ｓ字加減速軌道を生成する（図２のステップＳ７）。
【００５８】
その後、ＣＰＵ４０の目標位置算出部４６が、基準関節軸の修正Ｓ字加減速軌道に基づいて処理周期毎の各関節軸Ｊ1〜Ｊ3の目標位置を算出する。次いで、サーボ部４７が、この目標位置に基づいてロボット本体１０を作動させる。
【００５９】
このように本実施の形態によれば、各関節軸Ｊ1〜Ｊ3のＳ字加速度到達時間（最大角加速度到達時間および最小角加速度到達時間）を一定に保持したまま、動力学モデルを用いて求めた推定トルクが予め設定された目標トルクになるように、修正最大角加速度および修正最小角加速度を求めている。この結果、加減速時の振動の発生を抑えつつ、ロボットの動作パターンや負荷条件に合わせて角加速度を最適化することができる。また、Ｓ加減速軌道動作において、位置決め精度を低下させること無く，スムーズに加速、減速させることができる。
【００６０】
なお本実施の形態において、加速時および減速時の両方の場合に修正最大角加速度および修正最小角加速度を求めているが、これに限らず、加速時または減速時の一方の場合にのみ、上述した方法で修正最大角加速度または修正最小角加速度を求め、この修正最大角加速度または修正最小角加速度のいずれか一方に基づいて修正Ｓ字加減速軌道を生成しても良い。
【００６１】
また本実施の形態において、各関節軸Ｊ1〜Ｊ3がそれぞれ回転動作する場合について説明してきたが、直線運動する関節についても同様の考え方を適用することができる。この場合、本実施の形態中の「角度」、「角速度」、および「角加速度」の語をそれぞれ「位置」、（狭義の）「速度」、および（狭義の）「加速度」と置き換えて読むことができる。
【符号の説明】
【００６２】
１０    ロボット本体
１１    基台
１２    第１アーム体
１３    第２アーム体
１４    第３アーム体
２０    制御装置
３０    ＲＡＭ
３１    教示情報
３２    Ｓ字加減速パラメータ情報
３３    モデルパラメータ情報
３４    目標トルク情報
４０    ＣＰＵ
４１    第１Ｓ字加減速軌道生成部
４２    時刻算出部
４３    演算処理部
４４    角加速度変更処理部
４５    第２Ｓ字加減速軌道生成部
４６    目標位置算出部
４７    サーボ部
４８    基準関節軸決定処理部
１００  多関節型ロボットシステム
Ｊ1〜Ｊ3  関節軸
【図１】
2010269385.tif 000004
【図２】
2010269385.tif 000005
【図３】
2010269385.tif 000006
【図４】
2010269385.tif 000007