(57)【特許請求の範囲】
【請求項１】
動作１の完了前に次の動作である動作２を開始することにより、２つ以上の連続動作の動作時間を短縮する軌跡接続方式において、動作１の軌跡接続を行わない区間及び動作２の軌跡接続を行わない区間の両方を保証する軌跡接続可能地点を求め、動作１の完了前に次の動作である動作２を開始する場合に、各関節での速度が許容値以上となるかどうかの判別を行う速度超過条件の判別、又は各関節でのトルクが許容値以上となるかどうかの判別を行うトルク超過条件の判別を、動作２の開始前に行い、当該判別結果に応じて動作１の減速パラメータ又は動作２の加速パラメータを修正し、動作１が軌跡接続可能地点を通過した後に動作２を開始することを特徴とするロボット制御方法。
【請求項２】
速度超過条件が成立しない場合には、動作２を軌跡接続可能地点で開始することを特徴とする請求項１に記載のロボット制御方法。
【請求項３】
速度超過条件が成立する場合には、動作1の減速パラメータ及び動作２の加速パラメータを修正し、動作１が軌跡接続可能地点に到達し、かつ、動作１が減速を開始した後に動作２を開始することを特徴とする請求項１に記載のロボット制御方法。
【請求項４】
トルク超過条件が成立しない場合には、動作１の減速パラメータ及び動作２の加速パラメータに対してトルクを考慮しての変更は行わず、動作１が軌跡接続可能地点に到達した後に動作２を開始することを特徴とする請求項１に記載のロボット制御方法。
【請求項５】
トルク超過条件が成立する場合には、動作１の減速パラメータ及び動作２の加速パラメータのうち、少なくとも一方を修正し、動作１が軌跡接続可能地点に到達した後に動作２を開始することを特徴とする請求項１に記載のロボット制御方法。
【請求項６】
軌跡接続区間以外では、動作１の減速パラメータ及び動作２の加速パラメータを、それぞれが単独で動作する場合の値を使用する請求項１に記載のロボット制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、モータを用いてロボットを駆動するロボット制御方法に関するものであり、特に２つの動作を滑らかに接続し動作時間を短縮する軌跡接続方式に関するものである。
【０００２】
【従来の技術】
従来よりロボットなどの自動機械を教示点に沿って厳密に動かす必要のない場合は、第１の動作である動作１が終了する前に第２の動作である動作２を開始することにより、動作時間を短縮するとともに、ロボットを滑らかに動作させることが行われてきた。例えば、特開昭64-26911号公報に記載されているように、動作１の減速開始と同時に動作２の加速を開始することにより、２つの動作を滑らかに接続する軌道を動作１と動作２の合成により生成する方式が用いられている。ＡからＢへの直線動作である動作１とＢからＣへの直線動作である動作２を上記従来技術で接続した場合のロボットの軌道を図１２に示す。また、特開昭64-26911号公報に記載の方法では、動作１の移動方向と動作２の移動方向のなす角度に応じて動作１の減速度と動作２の加速度を求め直し、求め直した減速度で動作１の減速を行うとともに、求め直した加速度で動作２の加速を行っている。例えば図１３のように動作１と動作２の動作方向のなす角度が０度の場合は、図１４のように動作１、動作２の加減速度は変更せずそのまま合成する。図１５のように動作１と動作２のなす角度が１８０度の場合は、図１６のように動作１の減速度、動作２の加速度をそれぞれ半減し、合成した結果が元々の加減速度と一致するように変更する。
【０００３】
【発明が解決しようとする課題】
上記従来技術では、図１に示す動作２の開始点Ｄと動作１の終了点Ｅを直接指定できない。そのため、障害物を回避しながら動作せるために、動作１及び動作２の速度を変更するなどの試行錯誤が必要となる。また、第１動作の減速開始後にしか第２動作が開始されないため、必ずしも動作時間は最短とはならないという問題点があった。
【０００４】
さらに、上記従来技術では、動作１の移動方向と動作２の移動方向のみを考慮して動作１の減速度及び動作２の加速度を修正しているが、ロボットでは加減速度が同一でもロボットを駆動するために必要なトルクの大きさは大きく異なる。従って、あらゆる姿勢でトルクの許容値を超えないようにするためには、最も苦しい姿勢においてもトルクの許容値を越えないような加減速度に基づいて動作１及び動作２の加減速度を修正するため、多くの場合にロボットの持つ能力を十分には発揮できず、動作時間が長くなるといった問題点があった。
【０００５】
この発明は上記のような問題点を解消するためになされたもので、2つの連続した動作において、中間点が厳密な位置決めを必要としない地点では、中間点の近傍を滑らかに通過させることにより、ロボットの動作時間を短縮することができるロボット制御方法を提供することを目的とする。
【０００６】
【課題を解決するための手段】
この発明の第１の構成によるロボット制御方法は、動作１の完了前に次の動作である動作２を開始することにより、２つ以上の連続動作の動作時間を短縮する軌跡接続方式において、動作１の軌跡接続を行わない区間及び動作２の軌跡接続を行わない区間の両方を保証する軌跡接続可能地点を求め、動作１の完了前に次の動作である動作２を開始する場合に、各関節での速度が許容値以上となるかどうかの判別を行う速度超過条件の判別、又は各関節でのトルクが許容値以上となるかどうかの判別を行うトルク超過条件の判別を、動作２の開始前に行い、当該判別結果に応じて動作１の減速パラメータ又は動作２の加速パラメータを修正し、動作１が軌跡接続可能地点を通過した後に動作２を開始するものである。
【０００７】
この発明の第２の構成によるロボット制御方法は、速度超過条件が成立しない場合には、動作２を軌跡接続可能地点で開始することを特徴とするものである。
【０００８】
この発明の第３の構成によるロボット制御方法は、速度超過条件が成立する場合には、動作1の減速パラメータ及び動作２の加速パラメータを修正し、動作１が軌跡接続可能地点に到達し、かつ、動作１が減速を開始した後に動作２を開始することを特徴とするものである。
【０００９】
この発明の第４の構成によるロボット制御方法は、トルク超過条件が成立しない場合には、動作１の減速パラメータ及び動作２の加速パラメータに対してトルクを考慮しての変更は行わず、動作１が軌跡接続可能地点に到達した後に動作２を開始することを特徴とするものである。
【００１０】
この発明の第５の構成によるロボット制御方法は、トルク超過条件が成立する場合には、動作１の減速パラメータ及び動作２の加速パラメータのうち、少なくとも一方を修正し、動作１が軌跡接続可能地点に到達した後に動作２を開始することを特徴とするものである。
【００１１】
この発明の第６の構成によるロボット制御方法は、軌跡接続区間以外では、動作１の減速パラメータ及び動作２の加速パラメータを、それぞれが単独で動作する場合の値を使用するものである。
【００１３】
【発明の実施の形態】
この発明の基本的事項はつぎの通りであり、具体的には実施の形態１〜６において図を用いて説明する。
【００１４】
まず、動作２がまだ開始する前に動作１が移動する区間である動作１の非軌跡接続区間と、動作１が動作終了してから動作２が移動する区間である動作２の非軌跡接続区間を指定する。非軌跡接続区間の指定は、距離、各動作にしめる非軌跡接続区間の割合、時間のいずれであってもよい。また、非軌跡接続区間の指定は、非軌跡接続区間の距離、割合などを指定してもよいし、軌跡接続を行ってもよい区間を動作１、動作２のそれぞれに対して指定してもかまわない。動作１が動作１の非軌跡接続区間を通過し、かつ、動作２の非軌跡接続区間を満たすために動作１が動作２の開始前に通過しなければ行けない区間を通過した後に、動作２を開始する。
【００１５】
動作２を開始する前に、動作１と動作２を合成することにより、各関節での速度が許容値以上になるかどうかの判別（速度超過条件の判別と呼ぶ）を行う。速度超過条件が成立しない場合（動作１と動作２を合成しても各関節での速度が必ず許容値以下となる場合）、動作１及び動作２の加減速パラメータを速度超過の観点からは変更しない。速度超過条件が成立する場合（そのまま動作１と動作２を合成すると少なくとも１つの関節の速度が許容値以上となる場合）は、軌跡接続区間における動作１の減速パラメータ、動作２の加速パラメータの少なくとも一方を修正する。
【００１６】
速度超過条件の判別を行い、必要に応じて動作１及び動作２の加減速パラメータを変更した後、動作１と動作２を合成することにより、各関節でのトルクが許容値以上となるかどうかの判別（トルク超過条件の判別と呼ぶ）を行う。トルク超過条件が成立しない場合（動作１と動作２を合成しても各関節でのトルクが必ず許容値以下となる場合）、動作１及び動作２の加減速パラメータをトルク超過の観点からは変更しない。トルク超過条件が成立する場合（そのまま動作１と動作２を合成すると少なくとも１つの関節のトルクが許容値以上となる場合）は、軌跡接続区間における動作１の減速パラメータ、動作２の加速パラメータの少なくとも一方を修正する。
【００１７】
速度超過条件判別、トルク超過条件判別の結果に応じて動作１の減速パラメータ、動作２の加速パラメータを修正し、動作１が動作１の非軌跡接続区間を通過し、かつ、動作２の非軌跡接続区間を満たすために動作１が動作２の開始前に通過しなければ行けない区間を通過した後に、動作２を開始する。また、速度超過条件が成立する場合は、動作１が動作１の非軌跡接続区間を通過し、かつ、動作２の非軌跡接続区間を満たすために動作１が動作２の開始前に通過しなければ行けない区間を通過し、かつ、動作１が減速を開始してから動作２を開始する。
【００１８】
作用の概略はつぎの通りである。動作１の非軌跡接続区間と動作２の非軌跡接続区間を陽に指定し、速度超過条件判別、トルク超過条件判別の結果に応じて動作１及び動作２の加減速パラメータを変更するため、動作１及び動作２の非軌跡接続区間を保証すること、速度が許容値を超えないこと、トルクが許容値を超えないことの３つの制約条件を満たす範囲で最短の動作時間がとなるような軌跡接続方式が実現できる。
【００１９】
実施の形態１．
図１の様に、動作１がＡ点からＢ点への移動動作、動作２がＢ点からＣ点への移動動作で、動作１の移動区間のなかで、動作１がＢ点に到達する前に動作２を開始することにより直線ＡＢ（もしくは曲線AB）からずれてもよい区間をＤＢの長さをｌ1、動作２の移動区間の中で、動作１がＢ点に到達する前に動作２を開始することにより直線ＢＣ（もしくは曲線BC）からずれてもよい区間をＢＥの長さをｌ2で指定する。この時動作２を開始するタイミングを図２のフローで決定する。まず、動作１の残距離ｌz1を指令生成周期毎に計算し（ＳＴ１）、残距離ｌz1がｌ1以下になったかどうかの判別を行う（ＳＴ２）。動作１の残距離ｌz1がｌ1以下になった後、動作２がｌ2で指定された軌跡接続を行ってよい区間ＢＥを通過するのに要する時間ｔl2を算出する（ＳＴ３）。次に動作１が終了するまでの残り時間である残時間ｔz1がｔl2より大きいかどうかの判別を指令生成周期毎に実施し（ＳＴ４，ＳＴ５）、残時間ｔz1がｔl2より小さくなった時点で動作２を開始する。動作２開始後も動作１はＢ点への移動動作を続け、動作１と動作２の合成によりロボット動作が決定される。残距離ｌz1がｌ1以下になった地点ですでに残時間ｔz1がｔl2より小さくなっている場合は、図１のｆのような軌道上をロボット手先は動作する。残距離ｌz1がｌ1以下になってしばらく後に残時間ｔz1がｔl2より小さくなる場合は、図３のｆの様な軌道上をロボットは動作する。
【００２０】
実施の形態２．
図４の様に、動作１がＡ点からＢ点への移動動作、動作２がＢ点からＣ点への移動動作で、動作１の移動区間のなかで、動作１がＢ点に到達する前に動作２を開始することにより直線ＡＢ（もしくは曲線AB）からずれてもよい区間をＤＢが直線ＡＢに占める割合をｒ1、動作２の移動区間の中で、動作１がＢ点に到達する前に動作２を開始することにより直線ＢＣ（もしくは曲線BC）からずれてもよい区間をＢＥが直線ＢＣに占める割合をｒ2で指定する。この時動作２を開始するタイミングを図５のフローで決定する。まず、動作１の残距離ｌz1が動作１移動量全体に占める割合ｒz1を指令生成周期毎に計算し（ＳＴ６）、ｒz1がｒ1以下になったかどうかの判別を行う（ＳＴ７）。ｒz1がｒ1以下になった後、動作２がｒ2で指定された軌跡接続を行ってよい区間ＢＥを通過するのに要する時間ｔl2を算出する（ＳＴ８）。次に動作１が終了するまでの残り時間である残時間ｔz1がｔl2より大きいかどうかの判別を指令生成周期毎に実施し（ＳＴ９，ＳＴ１０）、残時間ｔz1がｔl2より小さくなった時点で動作２を開始する。動作２開始後も動作１はＢ点への移動動作を続け、動作１と動作２の合成によりロボット動作が決定される。ｒz1がｒ1以下になった地点ですでに残時間ｔz1がｔl2より小さくなっている場合は、図４のｆのような軌道上をロボット手先は動作する。ｒz1がｒ1以下になってしばらく後に残時間ｔz1がｔl2より小さくなる場合は、図６のｆの様な軌道上をロボットは動作する。
【００２１】
実施の形態３．
図１の様に、動作１がＡ点からＢ点への移動動作、動作２がＢ点からＣ点への移動動作で、動作１の移動区間のなかで、動作１がＢ点に到達する前に動作２を開始することにより直線ＡＢ（もしくは曲線AB）からずれてもよい区間をＤＢの長さをｌ1、動作２の移動区間の中で、動作１がＢ点に到達する前に動作２を開始することにより直線ＢＣ（もしくは曲線BC）からずれてもよい区間をＢＥの長さをｌ2で指定する。この時動作２を開始するタイミングを図２のフローで決定する。まず、動作１の残距離ｌz1を指令生成周期毎に計算し、残距離ｌz1がｌ1以下になったかどうかの判別を行う。動作１の残距離ｌz1がｌ1以下になった後、動作２がｌ2で指定された軌跡接続を行ってよい区間ＢＥを通過するのに要する時間ｔl2を算出する。次に動作１が終了するまでの残り時間である残時間ｔz1がｔl2より大きいかどうかの判別を指令生成周期毎に実施し、残時間ｔz1がｔl2より小さくなった時点で速度超過条件の判別を行う。
【００２２】
速度超過条件の判別は例えば下記のように行う。まず特開平7-200033号公報に記載のようにロボットの運動方程式に基づいて動作毎の加減速時間を求める際、加速終了地点及び減速開始地点での速度を求めているので、（図７参照）動作1及び動作2の加速終了地点の速度及び減速開始地点での速度を記憶しておく。次に動作1の減速開始地点の速度ｖgs1と動作2の加速加速終了地点の速度ｖke2を加算する。加算した結果、少なくとも１つの関節の速度が許容値を超えていれば、速度超過条件が成立したと判別する。加算した結果、いずれの関節の速度も許容値以下となっていれば、速度超過条件が成立しないと判別する。
【００２３】
速度超過条件が成立しない場合、動作１及び動作２の加減速パラメータは変更しない。動作１の残距離ｌz1がｌ1以下になった後、残時間ｔz1がｔl2より小さくなった時点で直ちに動作２を開始する。
【００２４】
速度超過条件が成立する場合、動作１の減速時間ｔ1gv及び動作２の加速時間ｔ2kvをそれぞれ下記の式で算出しなおす。
ｔ1gv＝ｍａｘ（ｔ1g0，ｍｉｎ（ｔ1g1，ｔ2k1））                （１）
ｔ2kv＝ｍａｘ（ｔ2k0，ｍｉｎ（ｔ1g1，ｔ2k1））                （２）
ここで、ｔ1g0は動作１の元々の（軌跡接続を行わない場合の）減速時間、ｔ2k0は動作２の元々の（軌跡接続を行わない場合の）加速時間であり、ｔ1g1はＤＢを動作１の減速区間にした場合の動作１の減速時間であり、ｔ2k1はＢＥを動作２の加速区間にした場合の動作２の加速時間である。動作１の減速時間をｔ1gv、動作２の加速時間をｔ2kvに変更し，動作１の残距離ｌz1がｌ1以下になった後、残時間ｔz1がｔl2より小さくなり、かつ、減速時間を修正した動作１が減速区間を開始した時点で、加速時間をｔ2kvに変更した動作２を開始する。
【００２５】
実施の形態４．
図１の様に、動作１がＡ点からＢ点への移動動作、動作２がＢ点からＣ点への移動動作で、動作１の移動区間のなかで、動作１がＢ点に到達する前に動作２を開始することにより直線ＡＢ（もしくは曲線AB）からずれてもよい区間をＤＢの長さをｌ1、動作２の移動区間の中で、動作１がＢ点に到達する前に動作２を開始することにより直線ＢＣ（もしくは曲線BC）からずれてもよい区間をＢＥの長さをｌ2で指定する。この時動作２を開始するタイミングを図２のフローで決定する。まず、動作１の残距離ｌz1を指令生成周期毎に計算し、残距離ｌz1がｌ1以下になったかどうかの判別を行う。動作１の残距離ｌz1がｌ1以下になった後、動作２がｌ2で指定された軌跡接続を行ってよい区間ＢＥを通過するのに要する時間ｔl2を算出する。次に動作１が終了するまでの残り時間である残時間ｔz1がｔl2より大きいかどうかの判別を指令生成周期毎に実施し、残時間ｔz1がｔl2より小さくなった時点でトルク超過条件の判別を行う。
【００２６】
トルク超過条件の判別は下記のように行う。まず、動作１及び動作２の加速時間、減速時間をそれぞれ図７のフローで求める。図７においてｋは指定された繰り返し回数である。各動作の加速時間及び減速時間の算出各動作を開始するまでに行えばよい。加速開始地点及び加速終了地点での加速時間の算出は下記のように行う。
【００２７】
ロボットの運動方程式は各軸の駆動トルクから構成されるベクトルをτ、慣性行列をＭ、各軸の加速度から構成されるベクトルをａ、各軸の遠心・コリオリ力から構成されるベクトルをｈ、各軸の重力から構成されるベクトルをｇ、各軸の摩擦力から構成されるベクトルをｆとすると、
τ＝Ｍa＋ｈ＋ｇ＋ｆ                                            （３）
とかける。さらにベクトルtorhをtorh=h+g+fと定義すればロボットの運動方程式は、
τ＝Mａ＋torh                                                  （４）
となる。各動作における最高速度から構成されるベクトルをｖ、加速時間をｔとすると、加速区間ではａ＝ｖ／ｔであるから加速区間のロボットの運動方程式は
τ＝Ｍｖ／ｔ＋torh                                              （５）
となる。mddt=Mvで定義し，mddt、torh、の第ｉ軸要素と第ｉ軸の許容トルクをそれぞれmddti、torhi、tmaxiとすると、それぞれの軸において駆動トルクが許容トルク以下となる制約を満たす範囲で最短の加速時間t1iは、
t1i＝mddti／（tmaxi− torhi）      （mddti＞０の時）        （６）
t1i＝mddti／（−tmaxi− torhi）    （上記以外）            （７）
で求められる。従って加速開始地点及び加速終了地点でのmddt、torhを計算すれば（６）、（７）式で各軸の許容トルク以下となる範囲で最短の加速時間が計算できる。加速開始地点のデータから計算した（６）式もしくは（７）式の値をt1ia、加速終了地点のデータから計算した（６）、（７）式の値をt1ib、加速開始地点のデータから決定する加速時間をt1a、加速終了地点のデータから決定する加速時間をt1bとすると、t1aはt1iaの最大値、t1bはt1ibの最大値である。
【００２８】
減速時間をｔとすると減速区間での運動方程式は
τ＝−Ｍｖ／ｔ＋torh                                （８）
となる。従って、それぞれの軸において駆動トルクが許容トルク以下となる制約を満たす範囲で最短の減速時間t2iは
t2i＝mddti／（tmaxI＋ torhi）    （mddti＞０の時）        （９）
t2i＝−mddti／（tmaxi− torhi）  （上記以外）          （１０）
で求められる。減速開始地点及び減速終了地点でのmddt、torhを計算すれば（９）、（１０）式で各軸の許容トルク以下となる範囲で最短の加速時間が計算できる。減速開始地点のデータから計算した（９）式もしくは（１０）式の値をt2ia、減速終了地点のデータから計算した（９）、（１０）式の値をt2ib、減速開始地点のデータから決定する減速時間をt2a、減速終了地点のデータから決定する減速時間をt2bとすると、t2aはt2iaの最大値、t2bはt2ibの最大値である。
【００２９】
動作1及び動作2の加速時間、減速時間を算出する際，繰り返しの最終回における加速開始地点、加速終了地点、減速開始地点、減速終了地点のデータに基づいて計算したmddt、torhを記憶しておく。動作1の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるmddtをそれぞれmddt1a, mddt1b, mddt1c, mddt1d、動作1の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるtorhをそれぞれtorh1a, torh1b, torh1c, torh1dとし、動作２の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるmddtをそれぞれmddt2a, mddt2b, mddt2c, mddt2d、動作２の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるtorhをそれぞれtorh2a, torh2b, torh2c, torh2dとする。
【００３０】
動作1の残時間ｔz1がｔl2より小さくなった時点で、各軸毎に
0003666341.tif 000002
を計算する。ここで、下付き添え字iは第i軸要素を意味し，t1gは動作1の減速時間である。次にt2k1i、t2k2i、t2k3iの最大値t2kaを算出する。t2kaが動作2の加速時間t2kより小さい場合はトルク超過条件が成立しないと判別する。トルク超過条件が成立しない場合は、動作2の加速時間をt2kのまま動作2を開始する。t2kaがt2kより大きい場合は、トルク超過条件が成立すると判別し，動作2の加速時間をt2kaに変更してから動作2を開始する。
【００３１】
実施の形態５．
図１の様に、動作１がＡ点からＢ点への移動動作、動作２がＢ点からＣ点への移動動作で、動作１の移動区間のなかで、動作１がＢ点に到達する前に動作２を開始することにより直線ＡＢ（もしくは曲線AB）からずれてもよい区間をＤＢの長さをｌ1、動作２の移動区間の中で、動作１がＢ点に到達する前に動作２を開始することにより直線ＢＣ（もしくは曲線BC）からずれてもよい区間をＢＥの長さをｌ2で指定する。この時動作２を開始するタイミングを図２のフローで決定する。まず、動作１の残距離ｌz1を指令生成周期毎に計算し、残距離ｌz1がｌ1以下になったかどうかの判別を行う。動作１の残距離ｌz1がｌ1以下になった後、動作２がｌ2で指定された軌跡接続を行ってよい区間ＢＥを通過するのに要する時間ｔl2を算出する。次に動作１が終了するまでの残り時間である残時間ｔz1がｔl2より大きいかどうかの判別を指令生成周期毎に実施し、残時間ｔz1がｔl2より小さくなった時点で速度超過条件の判別及びトルク超過条件の判別を行う。
【００３２】
速度超過条件の判別は実施の形態３と全く同様に行う。速度超過条件が成立しない場合，動作１の減速時間は変更しない。動作２の加速時間も変更しない。従って速度超過を考慮した動作１の減速時間と、速度超過を考慮した動作２の加速時間ｔ2kvは、
ｔ1gv＝ｔ1g0                                                （１７）
ｔ2kv＝ｔ2k0                                                （１８）
となる。ここで、ｔ1g0は動作１の元々の（軌跡接続を行わない場合の）減速時間、ｔ2k0は動作２の元々の（軌跡接続を行わない場合の）加速時間である。
【００３３】
速度超過条件が成立する場合、速度超過を考慮した動作１の減速時間ｔ1gvと動作２の加速時間ｔ2kvを（１）、（２）式で算出する。
【００３４】
次にトルク超過条件の判別を下記のように行う。まず、実施の形態4と全く同様にして、動作1及び動作2の加速時間、減速時間を算出し，加速時間、減速時間の算出の際に、加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるmddt、torhを記憶しておく。動作1の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるmddtをそれぞれmddt1a, mddt1b, mddt1c, mddt1d、動作1の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるtorhをそれぞれtorh1a, torh1b, torh1c, torh1dとし、動作２の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるmddtをそれぞれmddt2a, mddt2b, mddt2c, mddt2d、動作２の加速開始地点、加速終了地点、減速開始地点、減速終了地点におけるtorhをそれぞれtorh2a, torh2b, torh2c, torh2dとする。
【００３５】
次に、
0003666341.tif 000003
0003666341.tif 000004
を計算する。ここで、下付き添え字iは第i軸要素を意味する。次にt2k1i、t2k2i、t2k3iの最大値t2kaを算出する。t2kaが動作2の速度超過を考慮した加速時間ｔ2kvより小さい場合はトルク超過条件が成立しないと判別し、大きい場合はトルク超過条件が成立すると判別する。トルク超過条件が成立しない場合は、動作1の減速時間はｔ1gvのままとし、動作2の加速時間はｔ2kvのままとする。トルク超過条件が成立する場合は、動作1の減速時間はｔ1gvのままとし、動作2の加速時間はt2kaに変更する。
【００３６】
速度超過条件が成立しない場合，動作1の残時間ｔz1がｔl2より小さくなった時点で動作2が開始される。その際の動作2の加速時間はトルク超過条件が成立しない場合はｔ2kvであり、トルク超過条件が成立する場合はt2kaである。
【００３７】
速度超過条件が成立する場合、動作1の残時間ｔz1がｔl2より小さくなり、かつ、減速時間をｔ1gvに変更した動作1が減速を開始した地点で動作2を開始する。その際の動作2の加速時間はトルク超過条件が成立しない場合はｔ2kvであり、トルク超過条件が成立する場合はt2kaである。
【００３８】
実施の形態６．
実施の形態５と全く同様に速度超過条件、トルク超過条件の判別を行い、判別結果に応じて動作1及び動作2の加速時間、減速時間を修正する。ただし、軌跡接続終了後も動作2の加速区間が残る場合は，動作2の加速時間を元々の加速時間に戻してから動作2を継続する。
【００３９】
【発明の効果】
この発明の第１の構成であるロボット制御方法によれば、ロボットの第１の動作である動作１が終了する前に動作２を開始し、動作１と動作２を合成してロボットの動作指令を生成することによりロボット動作を高速化する軌跡接続方式において、動作１および動作２の非軌跡接続区間を保証でき、速度あるいはトルクが許容値を超えない制約条件を満たす範囲で最短の動作時間となるような軌跡接続方式が実現できる効果がある。
【００４０】
この発明の第２および第３の構成であるロボット制御方法によれば、ロボットの第１の動作である動作１が終了する前に動作２を開始し、動作１と動作２を合成してロボットの動作指令を生成することによりロボット動作を高速化する軌跡接続方式において、速度が許容値を超えない制約条件を満たす範囲で最短の動作時間となるような軌跡接続方式が実現できる効果がある。
【００４１】
この発明の第４および第５の構成であるロボット制御方法によれば、ロボットの第１の動作である動作１が終了する前に動作２を開始し、動作１と動作２を合成してロボットの動作指令を生成することによりロボット動作を高速化する軌跡接続方式において、トルクが許容値を超えない制約条件を満たす範囲で最短の動作時間となるような軌跡接続方式が実現できる効果がある。
【００４２】
この発明の第６の構成であるロボット制御方法によれば、ロボットの第１の動作である動作１が終了する前に動作２を開始し、動作１と動作２を合成してロボットの動作指令を生成することによりロボット動作を高速化する軌跡接続方式において、軌跡接続終了後は加減速度を元に戻すため、必要以上に動作時間が長くなることを防止できる効果がある。
【図面の簡単な説明】
【図１】  実施の形態１における軌跡接続を行ってよい区間の指定と軌跡（その１）を示す図である。
【図２】  実施の形態１における動作２開始タイミング決定のフローチャートである。
【図３】  実施の形態１における軌跡接続を行ってよい区間の指定と軌跡（その２）を示す図である。
【図４】  実施の形態２における軌跡接続を行ってよい区間の指定と軌跡（その１）を示す図である。
【図５】  実施の形態２における動作２開始タイミング決定のフローチャートである。
【図６】  実施の形態２における軌跡接続を行ってよい区間の指定と軌跡（その２）を示す図である。
【図７】  動作１及び動作２の元々の加速時間・減速時間決定のフローチャートである。
【図８】  実施の形態３における軌跡接続のフローチャートである。
【図９】  実施の形態４における軌跡接続のフローチャートである。
【図１０】  実施の形態５における軌跡接続のフローチャートである。
【図１１】  実施の形態６における軌跡接続のフローチャートである。
【図１２】  従来技術における軌跡接続時のロボットの軌道を示す図である。
【図１３】  動作１と動作２の動作方向のなす角度が０度の例を示す図である。
【図１４】  従来技術において動作１と動作２の動作方向のなす角度が０度の場合の速度指令を示す図である。
【図１５】  動作１と動作２の動作方向のなす角度が180度の例を示す図である。
【図１６】  従来技術において動作１と動作２の動作方向のなす角度が180度の場合の速度指令を示す図である。
【符号の説明】
ＳＴ１  動作１残距離ｌz1 計算、ＳＴ２  ｌz1＜ｌ1、ＳＴ３  ｔl2計算、ＳＴ４  動作１残時間ｔz1計算、ＳＴ５  ｔz1＜ｔl2、ＳＴ６  動作１残距離の動作１全体に占める割合ｒz1計算、ＳＴ７  ｒz1＜ｒ1、ＳＴ８  ｔl2計算、ＳＴ９  動作１残時間ｔz1が計算、ＳＴ１０  ｔz1＜ｔl2。
【図１】
0003666341.tif 000005
【図２】
0003666341.tif 000006
【図３】
0003666341.tif 000007
【図４】
0003666341.tif 000008
【図５】
0003666341.tif 000009
【図６】
0003666341.tif 000010
【図７】
0003666341.tif 000011
【図８】
0003666341.tif 000012
【図９】
0003666341.tif 000013
【図１０】
0003666341.tif 000014
【図１１】
0003666341.tif 000015
【図１２】
0003666341.tif 000016
【図１３】
0003666341.tif 000017
【図１４】
0003666341.tif 000018
【図１５】
0003666341.tif 000019
【図１６】
0003666341.tif 000020