(57)【特許請求の範囲】
【請求項１】
作業空間上でロボットの動作を制御するためのリアルタイムウィービングモーション制御装置において、
前記作業空間上で前記ロボットの主移動経路を決めて、前記決められた主移動経路を構成する単位モーションを発生させ、単位モーションが連結された連続モーションのうちのオフセットを動的に変更するウィービングを発生させ、前記作業環境により決定される補償変位および補償回転量のうちの少なくとも一つを生成する前処理部と、
前記単位モーションと前記ウィービング、および前記補償変位と前記補償回転量のうちの少なくとも一つによって、前記作業空間上で前記ロボットの位置および回転量を算出する作業空間モーション合成部と
を含み、
前記前処理部は、前記ロボットのモーション開始前に決められたウィービングまたはリアルタイムで生成されるウィービングを選択して前記ウィービングを発生させる、リアルタイムウィービングモーション制御装置。
【請求項２】
前記前処理部は、
前記ロボットのモーション開始前に決められた主移動経路またはリアルタイムで生成される主移動経路を連続モーション方法選択情報により決めて、前記単位モーションを生成する連続モーション処理部と、
前記ロボットのモーション開始前に決められた前記ウィービングまたはリアルタイムで生成される前記ウィービングをウィービングモーション方法選択情報により選択し、相対ウィービング量を生成するウィービングモーション処理部と、
前記作業空間の状況変化に応じた任意の変位または回転量をモーションに反映するための前記補償変位または前記補償回転量を生成する補償姿勢処理部と
を含む、請求項１に記載のリアルタイムウィービングモーション制御装置。
【請求項３】
前記連続モーション処理部は、
前記モーション開始前に決められた主移動経路が保存されている経路リスト保存部と；
前記作業空間の状況変化またはモーション経路変更の事由が発生した場合、リアルタイムで新たな単位モーションを生成するリアルタイム経路発生器と、
前記リアルタイム経路発生器から生成された単位モーションを保存する経路キューと；
前記経路キューの単位モーションおよび前記経路リスト保存部の単位モーションのうちの一つを前記連続モーション方法選択情報により選択する経路選択器と、
所定の作業開始点と作業終了点を示す連続モーション開始／終了情報によって、前記経路選択器から伝達される単位モーションを連続モーションに連結する作業空間連続モーション発生器と
を含む、請求項２に記載のリアルタイムウィービングモーション制御装置。
【請求項４】
前記作業空間連続モーション発生器は、
単位モーション開始方法により連続モーションのうちのｎ＋１番目単位モーションを開始し、以前モーションの処理方法によりｎ番目単位モーションを処理する、請求項３に記載のリアルタイムウィービングモーション制御装置。
【請求項５】
前記単位モーション開始方法は、
前記ｎ番目単位モーションの加速または等速から減速に変化する時点に前記ｎ＋１番目単位モーションを開始する方法、
前記ｎ番目単位モーションの加速から等速または減速に変換する時点に前記ｎ＋１番目単位モーションを開始する方法、
前記ｎ番目単位モーションが停止する一定時間前の時点に前記ｎ＋１番目単位モーションを開始する方法、および
前記ｎ番目単位モーションおよびｎ−１番目以前単位モーション間の重畳モーションが終了する時点にｎ＋１番目単位モーションを開始する方法
のうちの一つである、請求項４に記載のリアルタイムウィービングモーション制御装置。
【請求項６】
前記以前モーション処理方法は、
前記ｎ番目単位モーションを減速し、前記ｎ＋１番目単位モーションを開始する方法、および
前記ｎ番目単位モーションをそのまま維持し、前記ｎ＋１番目単位モーションを開始する方法
のうちの一つである、請求項４に記載のリアルタイムウィービングモーション制御装置。
【請求項７】
前記ウィービングモーション処理部は、
前記モーション開始前に決められたウィービングリストが保存されたウィービングリスト保存部と、
前記作業空間の状況変化または作業中モーション経路変更の事由が発生すると、リアルタイムでウィービングを生成するリアルタイムウィービング発生器と、
前記リアルタイムウィービング発生器から生成されたウィービングを単位ウィービング単位で保存するウィービングキューと、
前記ウィービングモーション方法選択情報によって、前記ウィービングリストに含まれている単位ウィービングまたは前記ウィービングキューの単位ウィービングを選択するウィービング選択器と、
前記ウィービング選択器によって選択された単位ウィービングを区分するためのウィービング開始／終了情報、ウィービングサンプリング時間、およびウィービング基準変数情報を受信し、前記選択された単位ウィービングおよびウィービング基準変数情報を利用して、前記ウィービングサンプリング時間内に相対ウィービング量を決める作業空間ウィービングモーション発生器と
を含み、
前記ウィービング基準変数情報は、
現在単位ウィービングのうちの次の単位ウィービングが施行されるウィービング変更起点を設定するためのウィービング基準変数を含む、請求項２に記載のリアルタイムウィービングモーション制御装置。
【請求項８】
前記作業空間ウィービングモーション発生器は、
前記ウィービング基準変数を前記連続モーションの時間、前記連続モーションの軌跡長、または前記連続モーションの軌跡回転量を基準として決める、請求項７に記載のリアルタイムウィービングモーション制御装置。
【請求項９】
前記作業空間ウィービングモーション発生器は、
前記ウィービング基準変数を決め、前記ウィービング関数を繰り返すウィービング周期および前記決められたウィービング基準変数を利用して第１変数を決め、前記ウィービング関数を前記第１変数に対する関数に生成する、請求項８に記載のリアルタイムウィービングモーション制御装置。
【請求項１０】
前記作業空間ウィービングモーション発生器は、
前記ウィービング基準変数が前記ウィービング変更起点に到達しなければ、前記ウィービング基準変数および前記第１変数を生成する、請求項９に記載のリアルタイムウィービングモーション制御装置。
【請求項１１】
前記作業空間モーション合成部、前記作業空間連続モーション発生器、前記作業空間ウィービングモーション発生器、および前記補償姿勢処理部は、前記ウィービングサンプリング時間に同期されて動作する、請求項７に記載のリアルタイムウィービングモーション制御装置。
【請求項１２】
前記作業空間モーション合成部からロボットの位置および回転量を受信して、前記ウィービングモーションを実現するために前記ロボットのジョイントの動作を生成するジョイント空間変換器をさらに含み、
前記ジョイント空間変換器は、
前記ウィービングサンプリング周期に同期されて動作する、請求項１１に記載のリアルタイムウィービングモーション制御装置。
【請求項１３】
作業空間上でロボットの動作を制御するためのリアルタイムウィービングモーション制御方法において、
前記作業空間の状況変化および作業中のモーション経路変更事由を含むリアルタイム変更事由の発生有無によりリアルタイムで単位モーションが生成され、前記ロボットのモーション開始前に決められた単位モーションまたは前記リアルタイムで生成される単位モーションのうちの一つを選択する段階と、
前記リアルタイム変更事由の発生有無によりリアルタイムで単位ウィービングが生成され、前記ロボットのモーション開始前に決められた単位ウィービングおよび前記リアルタイムで生成される単位ウィービングのうちの一つを選択する段階と、
前記作業空間の状況変化によって決定される補償変位および補償回転量のうちの少なくとも一つを生成する段階と、
前記単位モーション、前記単位ウィービング、および前記補償変位と補償回転量のうちの少なくとも一つによって、前記作業空間上で前記ロボットの位置および回転量を算出する段階と
を含む、リアルタイムウィービングモーション制御方法。
【請求項１４】
前記選択された単位ウィービングにより相対ウィービング量を決める段階をさらに含み、
前記相対ウィービング量を決める段階は、
前記選択された単位ウィービングおよびウィービング基準変数情報を利用して、ウィービングサンプリング時間内に相対ウィービング量を決める段階を含み、
前記ウィービング基準変数情報は、
現在単位ウィービングのうちの次の単位ウィービングが施行されるウィービング変更起点を設定するためのウィービング基準変数を含む、請求項１３に記載のリアルタイムウィービングモーション制御方法。
【請求項１５】
前記相対ウィービング量を決める段階は、
前記ウィービング基準変数を前記連続モーションの時間、前記連続モーションの軌跡長、または前記連続モーションの軌跡回転量を基準として決める段階と、
前記ウィービング関数を繰り返すウィービング周期および前記決められたウィービング基準変数を利用して第１変数を決め、前記ウィービング関数を前記第１変数に対する関数に生成する段階と
を含む、請求項１４に記載のリアルタイムウィービングモーション制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、作業空間で動作するロボットがリアルタイムでウィービングモーションを制御する制御装置およびその方法に関するものである。
【背景技術】
【０００２】
最近、産業現場にはロボットによって行われる作業の種類が多様化している。ロボットによって行われる作業目標地点の位置が移動する場合、ロボットのモーションを制御するための制御システムが必要である。
【０００３】
ロボットのモーション制御のための制御システムは、プログラムされたモーション経路に沿ってロボットが移動するように制御する。また、制御システムは、プログラムされた作業内容によりロボットの動作を制御する。
【０００４】
プログラムされた内容によりロボットを制御する制御システムは、ロボットの作業環境の変化およびモーション経路の変更によるリアルタイム状況変化が発生したとき、ロボットのモーションおよび動作を適切に制御することができない。
【発明の概要】
【発明が解決しようとする課題】
【０００５】
本発明の目的は、リアルタイム状況変化が発生したとき、ロボットのモーションおよび動作を適切に制御することができる制御装置およびその方法を提供することにある。
【課題を解決するための手段】
【０００６】
本発明の一実施形態によれば、作業空間上でロボットの動作を制御するためのリアルタイムウィービングモーション制御装置において、前記作業空間上で前記ロボットの主移動経路を決めて、前記決められた主移動経路を構成する単位モーションを発生させ、単位モーションが連結された連続モーションのうちのオフセットを動的に変更するウィービングを発生させ、前記作業環境により決定される補償変位および補償回転量のうちの少なくとも一つを生成する前処理部；および前記単位モーションと前記ウィービング、および補償変位と前記補償回転量のうちの少なくとも一つにより前記作業空間上で前記ロボットの位置および回転量を算出する作業空間モーション合成部を含むリアルタイムウィービングモーション制御装置が提供される。
【０００７】
前記前処理部は、前記ロボットのモーション開始前に決められた主移動経路またはリアルタイムで生成される主移動経路を連続モーション方法選択情報により決めて、前記単位モーションを生成する連続モーション処理部；前記ロボットのモーション開始前に決められたウィービングまたはリアルタイムで生成されるウィービングをウィービングモーション方法選択情報により選択して相対ウィービング量を生成するウィービングモーション処理部；および前記作業空間の状況変化による任意の変位または回転量をモーションに反映するための前記補償変位または前記補償回転量を生成する補償姿勢処理部を含むことができる。
【０００８】
前記連続モーション処理部は、前記モーション開始前に決められた主移動経路が保存されている経路リスト保存部；前記作業空間の状況変化またはモーション経路変更の事由が発生した場合、リアルタイムで新たな単位モーションを生成するリアルタイム経路発生器；前記リアルタイム経路発生器から生成された単位モーションを保存する経路キュー；前記経路キューの単位モーションおよび前記経路リスト保存部の単位モーションのうちの一つを前記連続モーション方法選択情報により選択する経路選択器；および所定の作業開始点と作業終了点を示す連続モーション開始／終了情報により、前記経路選択器から伝達される単位モーションを連続モーションに連結する作業空間連続モーション発生器を含むことができる。
【０００９】
前記作業空間連続モーション発生器は、単位モーション開始方法により連続モーションのうちのｎ＋１番目単位モーションを開始し、以前モーション処理方法によりｎ番目単位モーションを処理することができる。
【００１０】
前記単位モーション開始方法は、前記ｎ番目単位モーションの加速または等速から減速に変化する時点に前記ｎ＋１番目単位モーションを開始する方法；前記ｎ番目単位モーションの加速から等速または減速に変換する時点に前記ｎ＋１番目単位モーションを開始する方法；前記ｎ番目単位モーションが停止する一定時間前の時点に前記ｎ＋１番目単位モーションを開始する方法；および前記ｎ番目単位モーションおよびｎ−１番目以前単位モーション間の重畳モーションが終了する時点にｎ＋１番目単位モーションを開始する方法のうちの一つであることができる。
【００１１】
前記以前モーション処理方法は、前記ｎ番目単位モーションを減速し、前記ｎ＋１番目単位モーションを開始する方法；および前記ｎ番目単位モーションをそのまま維持し、前記ｎ＋１番目単位モーションを開始する方法のうちの一つであることができる。
【００１２】
前記ウィービングモーション処理部は、前記モーション開始前に決められたウィービングリストが保存されたウィービングリスト保存部；前記作業空間の状況変化または作業中モーション経路変更事由が発生すると、リアルタイムでウィービングを生成するリアルタイムウィービング発生器；前記リアルタイムウィービング発生器から生成されたウィービングを単位ウィービング単位で保存するウィービングキュー；前記ウィービングモーション方法選択情報により前記ウィービングリストに含まれている単位ウィービングまたは前記ウィービングキューの単位ウィービングを選択するウィービング選択器；および前記ウィービング選択器によって選択された単位ウィービングを区分するためのウィービング開始／終了情報、ウィービングサンプリング時間およびウィービング基準変数情報を受信し、前記選択した単位ウィービングおよびウィービング基準変数情報を利用して前記ウィービングサンプリング時間内に相対ウィービング量を決める作業空間ウィービングモーション発生器を含み、前記ウィービング基準変数情報は、現在単位ウィービングのうちの次の単位ウィービングが施行されるウィービング変更起点を設定するためのウィービング基準変数を含むことができる。
【００１３】
前記作業空間ウィービングモーション発生器は、前記ウィービング基準変数を前記連続モーションの時間、前記連続モーションの軌跡長、または前記連続モーションの軌跡回転量を基準として決めることができる。
【００１４】
前記作業空間ウィービングモーション発生器は、前記ウィービング基準変数を決め、前記ウィービング関数を繰り返すウィービング周期、および前記決められたウィービング基準変数を利用して第１変数を決め、前記ウィービング関数を前記第１変数に対する関数に生成することができる。
【００１５】
前記作業空間ウィービングモーション発生器は、前記ウィービング基準変数が前記ウィービング変更起点に到達しなければ、前記ウィービング基準変数および前記第１変数を生成することができる。
【００１６】
前記作業空間モーション合成部、前記作業空間連続モーション発生器、前記作業空間ウィービングモーション発生器、および前記補償姿勢発生器は、前記ウィービングサンプリング時間に同期されて動作することができる。
【００１７】
前記作業空間モーション合成部からロボットの位置および回転量の入力を受けて、前記ウィービングモーションを実現するために前記ロボットのジョイントの動作を生成するジョイント空間変換器をさらに含み、前記ジョイント空間変換器は、前記ウィービングサンプリング周期に同期されて動作することができる。
【００１８】
本発明の他の一実施形態によれば、作業空間上でロボットの動作を制御するためのリアルタイムウィービングモーション制御方法において、前記作業空間の状況変化および作業中モーション経路変更事由を含むリアルタイム変更事由の発生有無によりリアルタイムで単位モーションが生成され、前記ロボットのモーション開始前に決められた単位モーションまたは前記リアルタイムで生成される単位モーションのうちの一つを選択する段階；前記リアルタイム変更事由の発生有無によりリアルタイムで単位ウィービングが生成され、前記ロボットのモーション開始前に決められた単位ウィービングおよび前記リアルタイムで生成される単位ウィービングのうちの一つを選択する段階；前記作業空間の状況変化によって決定される補償変位および補償回転量のうちの少なくとも一つを生成する段階；および前記単位モーション、前記単位ウィービング、および前記補償変位と前記補償回転量のうちの少なくとも一つにより、前記作業空間上で前記ロボットの位置および回転量を算出する段階を含むリアルタイムウィービングモーション制御方法が提供される。
【００１９】
前記選択された単位ウィービングにより相対ウィービング量を決める段階をさらに含み、前記相対ウィービング量を決める段階は、前記選択された単位ウィービングおよびウィービング基準変数情報を利用してウィービングサンプリング時間内に相対ウィービング量を決める段階を含み、前記ウィービング基準変数情報は、現在単位ウィービングのうちの次の単位ウィービングが施行されるウィービング変更起点を設定するためのウィービング基準変数を含むことができる。
【００２０】
前記相対ウィービング量を決める段階は、前記ウィービング基準変数を前記連続モーションの時間、前記連続モーションの軌跡長、または前記連続モーションの軌跡回転量を基準として決める段階；および前記ウィービング関数を繰り返すウィービング周期および前記決められたウィービング基準変数を利用して第１変数を決め、前記ウィービング関数を前記第１変数に対する関数に生成する段階を含むことができる。
【発明の効果】
【００２１】
本発明は、リアルタイム状況変化が発生したとき、ロボットのモーションおよび動作を適切に制御することができる制御装置およびその方法を提供する。
【図面の簡単な説明】
【００２２】
【図１】本発明の一実施形態による溶接ロボットのモーションを制御するモーション制御システムを示した図面である。
【図２】ｎ−１番目単位モーションとｎ番目単位モーションのそれぞれの時間帯速度を示した図面である。
【図３ａ】計画されたｎ番目モーションの時間帯速度を示した図面である。
【図３ｂ】異なるモーション処理方法に従うとき、ｎ番目単位モーションとｎ＋１番目単位モーションのそれぞれの時間帯速度を示した図面である。
【図３ｃ】異なるモーション処理方法に従うとき、ｎ番目単位モーションとｎ＋１番目単位モーションのそれぞれの時間帯速度を示した図面である。
【図４】基準変数が軌跡長基準に従うとき、単位モーションと基準変数の関係を示すグラフである。
【図５】作業空間ウィービングモーション発生器の動作を示したフローチャートである。
【図６ａ】ウィービング関数がサイン関数であり、ウィービング基準変数が時間基準である場合を示した図面である。
【図６ｂ】ウィービング関数が一次関数であり、ウィービング基準変数が時間基準である場合を示した図面である。
【図６ｃ】ウィービング関数がサイン関数であり、ウィービング基準変数が軌跡長基準である場合を示した図面である。
【図６ｄ】ウィービング関数が一次関数であり、ウィービング基準変数が軌跡長基準である場合を示した図面である。
【発明を実施するための形態】
【００２３】
以下、添付した図面を参照して、本発明の実施形態について本発明が属する技術分野における通常の知識を有する者が容易に実施できるように詳細に説明する。しかし、本発明は種々の異なる形態に実現でき、ここで説明する実施形態に限られない。図面において、本発明を明確に説明するために説明上不必要な部分は省略した。明細書の全体にわたって類似する部分に対しては類似する図面符号を付けた。
【００２４】
明細書の全体において、ある部分が他の部分と「連結」されているというとき、これは「直接的に連結」されている場合だけでなく、その中間に他の素子を介して「電気的に接続」されている場合も含む。また、ある部分がある構成要素を「含む」というとき、これは特に反対になる記載がない限り、他の構成要素を除くことではなく、他の構成要素をさらに含むことができるのを意味する。
【００２５】
以下、本発明が属する技術分野における通常の知識を有する者が本発明を容易に実施できる実施形態について、添付した図面を参照しながら詳細に説明する。
【００２６】
図１は、本発明の一実施形態によるウィービングモーション制御装置および制御方法が溶接ロボットのモーションを制御するために適用された例を示した図面である。本発明の一実施形態では、産業ロボットのうち溶接を行う溶接ロボットを一例として挙げて説明している。しかし、本発明がこれに限定されることではなく、実施形態で説明される技術的な思想は他のロボットのモーション制御に用いられる。
【００２７】
図１に示されているように、モーション制御システム１は、前処理部１０、作業空間モーション合成部２０、ジョイント空間変換器３０、およびサーボ制御部４０を含む。図１には、モーション制御システム１の制御対象であるロボット器具部２が共に示されている。
【００２８】
前処理部１０は、ロボットの作業空間上で主移動経路を決め、決められた主移動経路による単位モーションを発生させ、連続モーション中オフセット（ｏｆｆ  ｓｅｔ）を動的に変更するウィービングを発生させ、作業環境により決定される補償変位または補償回転量を生成する。以下、ウィービングモーションは単位モーションとウィービングが結合されたモーションを含む。連続モーションは単位モーションの連結を意味する。補償変位または補償回転量はウィービングモーションに反映される。
【００２９】
前処理部１０は、連続モーション処理部１００、ウィービングモーション処理部２００、および補償姿勢処理部３００を含む。
【００３０】
連続モーション処理部１００は、ロボットの作業空間上で主移動経路を決め、決められた主移動経路に単位モーションを発生させる。このとき、主移動経路はモーション開始前に決定されるか、または動作中リアルタイムで生成され、モーション開始前に決められた主移動経路およびリアルタイムで生成された主移動経路のうちの一つを選択するために連続モーション方法選択情報（Ａ１）が発生する。
【００３１】
連続モーション処理部１００は、経路リスト保存部１１０、リアルタイム経路発生器１２０、経路キュー１３０、経路選択器１４０、および作業空間連続モーション発生器１５０を含む。
【００３２】
連続モーション方法選択情報（Ａ１）は、ロボットのモーション中発生可能な外部状況変化および作業中モーション経路変更によって生成される。つまり、外部状況変化およびモーション経路変更が発生すると、モーション開始前に決められた主移動経路の代わりにリアルタイムで生成される主移動経路を選択する連続モーション方法選択情報（Ａ１）が発生する。そうではない場合には、モーション開始前に決められた主移動経路を選択する連続モーション方法選択情報（Ａ１）が発生する。
【００３３】
ロボット作業において発生可能な外部状況変化の例は、次の通りである。
１）溶接による部材の変形
２）塗装噴射力による塗装対象物の変化
３）ブラスト収去のための吸入力による対象物の変更
【００３４】
作業中のモーション経路変更の事由の例は、次の通りである。
１）事前計画時に無かった外部対象物が作業領域に侵した場合
２）多重ロボット作業において、事前に計画された作業を他のロボットが代わりに処理する場合
【００３５】
上記のような内容は一例であり、ロボットの使用用途により多様な要求が発生しうる。
このような多様な要求が発生時ごとにリアルタイムで主移動経路を生成する必要がある。
【００３６】
連続モーション処理部１００は、作業空間上で決められた主移動経路にモーションを発生させる。主移動経路は単位モーションの集合である。単位モーションは、順序、ターゲットポーズ（ｐｏｓｅ）、ロボット配置（ｃｏｎｆｉｇｕｒａｔｉｏｎ）、加速度または加速時間、減速または減速時間、最大速度または等速時間、角加速度または角加速時間、角減速度または角減速時間、最大角速度または等速時間、単位モーション開始方法、および以前モーション処理方法を含むプロファイルと定義される。
【００３７】
ポーズは、ロボットの位置とロボットの回転に対する情報を意味する。経路を構成する複数の単位モーション間の先後により単位モーションの順序が決定される。単位モーションそれぞれは自分の順序情報を含む。目標ポーズは、目標位置および目標方向を含む情報である。
【００３８】
ロボット配置はロボットの姿勢を意味する。ロボットは、作業により適した姿勢に設定され、同一のポーズでも異なる姿勢でありうる。例えば、溶接ロボットの場合、人の腕と類似した形態に姿勢を取るようになるが、溶接ロボットが同一のポーズであって、左腕または右腕の姿勢を取ることができる。つまり、作業空間で溶接ロボットが同じ位置で同じ回転を行っても、左腕の姿勢なのかまたは右腕の姿勢なのかによって作業内容が変化する。
【００３９】
ロボット配置は、ロボットの軸（ａｘｉｓ）の数と種類によって異なり、ロボット別にロボット配置が決定される。
【００４０】
ロボットの移動経路は移動速度により、移動速度が増加する加速区間、最大速度に維持される等速区間、および減少する減速区間に区分されることができる。また、ロボットの回転経路は角速度により、角速度が増加する角加速区間、最大角速度に維持される角等速区間、および減少する角減速区間に区分されることができる。
【００４１】
加速度または加速時間は加速区間に、減速または減速時間は減速区間に、最大速度または等速時間は等速区間に、角加速度または角加速時間は角加速区間に、角減速度または角減速時間は角減速区間に、および最大角速度または等速時間は角等速中区間に対応する情報である。
【００４２】
経路リスト保存部１１０は、モーション開始前に決められた移動経路が保存されている。
移動経路は単位モーションの集合であるため、決められた移動経路を示す単位モーションの集合が経路リスト保存部１１０に保存されている。
【００４３】
リアルタイム経路発生器１２０は、外部状況変化または作業中モーション経路変更の事由が発生した場合、リアルタイムで新たな移動経路を生成する。つまり、リアルタイム経路発生器１２０はリアルタイムで単位モーションを生成する。
【００４４】
リアルタイム経路発生器１２０は、リアルタイムで新たな移動経路を生成するために多様な方法を使用することができる。例えば、ビジョンカメラによってポーズが選定（単位モーション生成）される方法によれば、ビジョンカメラによってビジョン処理された結果に基づいて経路の目標ポーズが変化することがある。このように生成された経路にウィービングが連動するようにウィービングモーション処理部がウィービングモーションを制御する。
【００４５】
ビジョン処理に所要される時間を含む経路生成時間は非確定的であるため、ロボットのモーションが停止しうる。これを防止するために、バッファ手段が必要であり、本発明の実施形態ではバッファ手段として「経路キュー」を使用する。具体的に、リアルタイム経路発生器１２０の単位モーション生成が遅延してモーションが停止するのを防止するために、経路キュー１３０に単位モーションを保存させることができる。
【００４６】
経路キュー１３０は、単位モーション保存のための保存空間を含み、保存空間は、単位モーション単位で分けられているメモリに具現できる。リアルタイム経路発生器１２０の単位モーション生成に所要される時間により経路キュー１３０の大きさを適切に設定することができる。
【００４７】
具体的に、連続モーション開始／終了情報（Ｂ１）が単位モーションの開始を知らせる場合、経路キュー１３０は当該単位モーションを経路選択器１４０に伝達する。そして連続モーション開始／終了情報（Ｂ１）が単位モーションの終了を知らせる場合、経路キュー１３０は当該単位モーションをメモリから削除する。
【００４８】
経路選択器１４０は、連続モーション方法選択情報（Ａ１）により経路リスト保存部１１０の単位モーションまたは経路キュー１３０から伝達される単位モーションのうちの一つを選択する。
【００４９】
作業空間連続モーション発生器１５０は、連続モーション開始／終了情報（Ｂ１）および経路選択器１４０から伝達される単位経路を受信し、所定の作業開始点と作業終了点の間の作業経路上で作業を行うロボットのモーション制御のリアルタイム応答のための軌跡を発生させてターゲットポーズを決定する。タケットポーズは、ターゲット位置を示す位置ベクトルＴ_ｐ（ｔ）と、ターゲット回転量を示す回転ベクトルＴ_ｒ（ｔ）の行列に表示される。
【００５０】
連続モーション開始／終了情報（Ｂ１）は、所定の作業開始点と作業終了点に対する情報を含む。つまり、連続モーション開始／終了情報（Ｂ１）は、複数の単位モーションのうちの一つの単位モーションを区分するための情報である。モーション制御システム１は種々の単位モーションを制御する。例えば、Ｐ２Ｐ（ＰＯＳＥ  ｔｏ  ＰＯＳＥ：開始ポーズから終了ポーズにロボットを移動させ、その間の経路は、ロボットの各ジョイント別の最適化した形態を決めてロボットを移動させる方式）、またはＪＯＧ（軸別速度または姿勢速度に対する指令によりロボットを動かす方式）方式に従う種々の単位モーション中、現在モーション制御システム１によって制御される単位モーションの開始と終了を示す情報である。
【００５１】
作業空間連続モーション発生器１５０は、連続モーション開始／終了情報（Ｂ１）により、現在生成している連続モーションと、他の単位モーションの連続モーションとを区分する。
【００５２】
作業空間連続モーション発生器１５０は、単位モーション開始方法により連続モーションのうちの各単位モーションを開始する。単位モーション開始方法は、連続する単位モーションのうちの後順の単位モーションの開始時点を決定する方法を意味する。
【００５３】
図２を参照して、単位モーション開始方法について説明する。
【００５４】
図２は、ｎ−１番目単位モーションとｎ番目単位モーションのそれぞれの時間帯速度を示した図面である。図２を参照して、ｎ＋１番目単位モーション開始方法について説明する。
【００５５】
ｎ＋１番目単位モーションを開始する方法は、下記の４つの場合中１つでありうる。
１）以前単位モーション（ｎ番目単位モーション）の加速から等速または減速に変換する時点ｔ１
２）以前単位モーション（ｎ番目単位モーション）およびその以前単位モーション（ｎ−１番目単位モーション）間の重畳モーションが終了する時点ｔ２
３）以前単位モーション（ｎ番目単位モーション）が停止する一定時間（ｔ_ｂ）前の時点ｔ３
４）以前単位モーション（ｎ番目単位モーション）の加速または等速から減速に変化する時点ｔ４
【００５６】
時点ｔ１乃至時点ｔ４のいずれか一つの時点で次の単位モーションのｎ＋１番目単位モーションが開始することができる。また、期間ｔ２−ｔ４、期間ｔ４−ｔ３、および期間ｔ３−ｔ１間の任意の時点でｎ＋１番目単位モーションが開始することができる。
【００５７】
作業空間連続モーション発生器１５０は、以前モーションの処理方法により連続モーションのうちの以前単位モーションを処理する。以前モーション処理方法は、新しく開始する単位モーションの以前単位モーションを処理する方法を意味する。
【００５８】
以下、図３ａ〜３ｃを参照して、以前モーション処理方法について説明する。
【００５９】
図３ａは、計画されたｎ番目モーションの時間帯速度を示した図面である。
【００６０】
図３ｂおよび３ｃそれぞれは、他の以前モーション処理方法に従うとき、ｎ番目単位モーションとｎ＋１番目単位モーションそれぞれの時間帯速度を示した図面である。
【００６１】
図３ｂおよび図３ｃに示されているように、以前モーション処理方法は次の通りである。
１）以前モーション（ｎ番目単位モーション）を減速し、次のモーション（ｎ＋１番目単位モーション）を開始する方法
２）以前モーション（ｎ番目単位モーション）をそのまま維持し、次のモーション（ｎ＋１番目単位モーション）を開始する方法
【００６２】
図３ｂおよび３ｃに示された以前モーション処理方法は、次の単位モーションが「１）以前単位モーション（ｎ番目単位モーション）の加速または等速から減速に変化する時点ｔ４」に従うことと仮定して説明する。本発明がこれに限定されることではなく、一実施形態で以前モーション処理方法を説明するための設定である。
【００６３】
以前モーション処理方法１によれば、計画されたｎ番目単位モーションは、次の単位モーション（ｎ＋１番目単位モーション）が開始する時点ｔ４に点線で示したようにまだ終了していないが、時点ｔ４から減速させて終了させる。
【００６４】
これとは異なり、以前モーション処理方法２）によれば、計画されたｎ番目単位モーションはそのまま維持され、時点ｔ４に次のｎ＋１番目単位モーションが開始して二つのモーションが重なる。
【００６５】
上述した次の単位モーション処理方法中、時点ｔ４以外の他の時点でも同一の原理で以前モーション処理方法が適用される。
【００６６】
作業空間連続モーション発生器１５０は、単位モーションの開始時点にロボットの初期速度が「０」の場合、ロボットの移動速度により加速、等速、および減速の三区間に分ける。
【００６７】
作業空間連続モーション発生器１５０は、設定された三区間それぞれに対する速度プロファイルを生成する。具体的に、作業空間連続モーション発生器１５０は、加速区間および減速区間の速度プロファイルを所定次数の多項式に生成し、等速区間における速度プロファイルを定数値を有する速度プロファイルを生成する。
【００６８】
作業空間連続モーション発生器１５０は、単位モーションの開始時点にロボットの初期速度が「０」でない場合、ブレンディング（ｂｌｅｎｄｉｎｇ）、等速、および減速の三区間に分けて区間を設定する。ブレンディング区間とは、初期速度が０ではないので、以前単位モーションと現在単位モーションとが結合される区間を意味する。
【００６９】
作業空間連続モーション発生器１５０は、以前単位モーションと現在単位モーションそれぞれのポーズに含まれている位置情報を加え、それぞれのポーズに含まれている回転情報行列をかけて以前単位モーションと現在単位モーションとを結合する。このことにより、ブレンディング期間の間に移動しなければならない最終位置が決定される。
【００７０】
作業空間連続モーション発生器１５０は、ブレンディング区間において初期速度で速度を「０」にする速度プロファイルと、最終位置への移動のための加速区間の速度プロファイルをブレンディングする。ブレンディング区間で移動しなければならない距離において初期速度で速度を０にする期間の間に移動した距離の差を、与えられたブレンディング区間の間に移動するための速度プロファイルを合わせてブレンディング区間の速度プロファイルを生成する。
【００７１】
初期速度が「０」でない場合の減速区間において、作業空間連続モーション発生器１５０は、所定次数の多項式で速度プロファイルを生成することができる。このとき、多項式次数は、上述した初期速度が０のとき、減速区間における速度プロファイル次数より高い。
初期速度が「０」でない場合の等速区間において、作業空間連続モーション発生器１５０は等速度の速度プロファイルを生成する。
【００７２】
作業空間連続モーション発生器１５０は、このように生成された速度プロファイルにより軌跡を発生させて、ターゲット位置を示す位置ベクトルＴ_ｐ（ｔ）と、ターゲット回転量を示す回転ベクトルＴ_ｒ（ｔ）の行列を含むターゲットポーズを生成する。作業空間連続モーション発生器１５０は、生成されたターゲットポーズを作業空間モーション合成部２０に伝達する。
【００７３】
ウィービングモーション処理部２００は、連続モーションのうちのオフセットを動的に変更するモーション、つまり、ウィービングを生成する。ウィービングモーション処理部２００は、オフセットを動的に変化させる方法として、事前に計画されたウィービングリストまたはリアルタイムウィービング発生器を利用する。
【００７４】
ウィービングモーション処理部２００は、作業空間上で決められたウィービングリストによりウィービングモーションを発生させる。先に、リアルタイム経路発生器の説明で言及した通り、外部状況変化またはモーション経路変更の事由が発生した場合、ウィービングモーション処理部２００は事前に計画されたウィービングリストをそのまま使用することができない。この場合、ウィービングモーション処理部２００は、リアルタイムウィービング発生器から生成されるウィービングリストにより相対ウィービング量を生成する。
【００７５】
つまり、ウィービングモーション処理部２００は、状況に応じて相対ウィービング量を生成するために、リアルタイムウィービングリストを生成するか、または保存されたウィービングリストを選択的に利用することができる。
【００７６】
ウィービングモーション処理部２００は、ウィービングリスト保存部２１０、リアルタイムウィービング発生器２２０、ウィービングキュー２３０、ウィービング選択器２４０、および作業空間ウィービングモーション発生器２５０を含む。
【００７７】
ウィービングリスト保存部２１０は、モーション開始前に決められたウィービングのリストを保存する。
【００７８】
ウィービングモーション方法選択情報（Ａ２）は、ロボットのモーション中発生可能な外部状況変化および作業中モーション経路変更事由の発生有無により生成される。つまり、外部状況変化およびモーション経路変更事由のうちの一つが発生すると、モーション開始前に決められた主ウィービングの代わりにリアルタイムで生成されるウィービングを選択するウィービングモーション方法選択情報（Ａ２）が発生する。そうではない場合には、モーション開始前に決められたウィービングを選択する連続モーション方法選択情報（Ａ２）が発生する。
【００７９】
リアルタイムウィービング発生器２２０は、外部状況変化または作業中モーション経路変更事由が発生した場合、リアルタイムで新たなウィービングを生成する。つまり、リアルタイムウィービング発生器２２０はリアルタイムで単位ウィービングを生成する。リアルタイムウィービング発生器２２０は、リアルタイムで新たなウィービングを生成するために多様な方法を使用することができる。
【００８０】
ウィービングキュー２３０は単位ウィービング保存のための保存空間を含み、保存空間は単位ウィービング単位で分けられているメモリに具現される。リアルタイムウィービング発生器２２０の単位ウィービング生成に所要される時間により、ウィービングキュー２３０の大きさを適切に設定することができる。ウィービングキュー１３０は、ウィービング開始／終了情報（Ｂ２）により保存された単位ウィービングを伝達および削除する。
【００８１】
具体的に、ウィービング開始／終了情報（Ｂ２）が単位ウィービングの開始を知らせる場合、ウィービングキュー２３０は当該単位ウィービングをウィービング選択器２４０に伝達する。そしてウィービング開始／終了情報（Ｂ２）が単位ウィービングの終了を知らせる場合、ウィービングキュー２３０は当該単位ウィービングをメモリから削除する。
【００８２】
ウィービング選択器２４０は、ウィービングモーション方法選択情報（Ｂ２）により、ウィービングリスト保存部１１０から伝達される単位ウィービング、またはウィービングキュー２３０から伝達される単位ウィービングのうちの一つを選択する。
【００８３】
作業空間ウィービングモーション発生器２５０は、ウィービングモーション開始／終了情報（Ｂ２）およびウィービング選択器２４０から伝達される単位ウィービングを受信して、単位ウィービングにより連続モーション中相対ウィービング量を決定する。相対ウィービング量は、位置ベクトルＷ_ｐ（ｔ）と回転ベクトルＷ_ｒ（ｔ）の行列に表示される。
【００８４】
ウィービング開始／終了情報（Ｂ２）は、作業空間ウィービングモーション発生器２５０およびウィービングキュー２３０に伝達される。ウィービングキュー２３０は、ウィービング開始／終了情報がウィービング終了を知らせれば、ウィービングキューを空ける。ウィービング開始／終了情報（Ｂ２）は、連続モーション開始／終了情報（Ｂ１）と共に複数のウィービングモーションのうちの一つのウィービングモーションを区分するための情報である。作業空間ウィービングモーション発生器２５０は、ウィービングモーション開始／終了情報（Ｂ２）により、現在生成している連続モーションのウィービングと、他の連続モーションのウィービングとを区分する。
【００８５】
上述した通り、経路キュー１３０およびウィービングキュー２３０それぞれは、連続モーション開始／終了情報（Ｂ１）およびウィービング開始／終了情報（Ｂ２）によりキューを空ける。つまり、次に開始するウィービングモーションと、現在生成されるウィービングモーションとが混ざらないように、現在ウィービングモーションに対するデータは削除される。これはウィービングモーション生成において発生しうる誤動作を防止する。
【００８６】
先に言及したウィービングリストは単位ウィービングの集合である。単位ウィービングは、方向選択、基準ベクトル、ウィービング変更起点、最大ウィービングポーズ（ｐｏｓｅ）、ウィービング周期、およびウィービング関数によって定義される。
【００８７】
単位ウィービングの方向選択は、移動方向ベクトルと基準ベクトルによって決定される。
単位ウィービングの方向を示すベクトルをウィービング方向ベクトルＷｄという。Ｗｄは、ｘｙｚ座標軸によりＷｄ_ｘ、Ｗｄ_ｙ、Ｗｄ_ｚから構成される。
【００８８】
ウィービングモーション処理部２００は、相対ウィービング量を決定するためのウィービング方向ベクトルＷｄを設定する。ウィービング方向ベクトルＷを設定する方法は多様である。
【００８９】
例えば、１）移動方向ベクトルＤと基準ベクトルＮに直角を成す方向にウィービング方向ベクトルＷを決める方法、および２）作業空間基準の任意のベクトルにウィービング方向ベクトルＷを決める方法のうちの一つによることができる。
１）の方法において、移動方向ベクトルＤは、連続モーション処理部１００で選択された経路を示すベクトルであり、基準ベクトルＮは、移動方向ベクトルＤと同一平面上で垂直のベクトルである。このとき、移動方向ベクトルＤはＤ_ｘ、Ｄ_ｙ、Ｄ_ｚであり、基準ベクトルＮはＮ_ｘ、Ｎ_ｙ、Ｎ_ｚに示される。
２）の方法の場合、移動方向ベクトルＤは１、０、０であり、基準ベクトルＮは０、１、０であり、ウィービング方向ベクトルＷは０、０、１に決定される。
【００９０】
ウィービング変更起点は、現在進行中の単位ウィービングの次の単位ウィービングが施行される起点を示す。リアルタイムで単位ウィービングが発生する場合に、ウィービングキューの大きさが０であれば、任意で所望の起点でウィービングの変更が可能なので、単位ウィービング内のウィービング変更起点は関係ない。
【００９１】
しかし、リアルタイムで単位ウィービングが発生する場合に、ウィービングキューの大きさが０でない場合、所望の起点でウィービングの変更が不可能なので、ウィービングキューの大きさにより現在進行中の単位ウィービングのうちの次の単位ウィービングが施行される起点が設定されなければならない。つまり、ウィービングキューが存在する場合（キューの大きさが０でない場合）は、次の単位ウィービングが反映されなければならない時点（ウィービング変更起点）に対する時間情報を有していなければならない。もし、この情報がなければ、ウィービングキューに保存されたウィービング情報が同時に全て施行されるようになる。
【００９２】
ウィービング変更起点は、連続モーションの単位モーションと同期されるように設定されなければならない。または、ウィービング変更起点を設定するためのウィービング基準変数「ｓ」の一定の起点、「ｓ_ｄ」をウィービング変更起点に設定することができる。
【００９３】
ｓの一定の起点、ｓ_ｄをウィービング変更起点に設定した場合、次の単位ウィービングが開始する時点は、ｓ>ｓ_ｄを最初に満足する起点である。
【００９４】
ウィービング基準変数は、時間基準、軌跡長基準、または軌跡回転量基準に設定されることができる。ウィービング基準変数が時間基準である場合、ウィービング基準変数はｓ（ｔ）となる。
【００９５】
ウィービング基準変数が軌跡長基準である場合、モーションを直線単位モーションの集合と見たとき、動いた長さＤをウィービング基準変数に設定する。このことによってウィービング基準変数はｓＤとなる。
【００９６】
もし、連続モーションが等速である場合には、時間基準のウィービング基準変数による作業空間上の結果と、軌跡長基準のウィービング基準変数による作業空間上の結果が同一である。連続モーションが等速でない場合には、軌跡長基準のウィービング基準変数による作業空間上の結果は、時間基準のウィービング基準変数による作業空間上の結果と異なる。
【００９７】
図４は、ウィービング基準変数が軌跡長基準に従うときの単位モーションとウィービング基準変数との関係を示すグラフである。
【００９８】
図４に示された連続した単位モーションｄ（１）、ｄ（２）、およびｄ（３）が合算されて、ウィービング基準変数「ｓＤ」が生成される。
【００９９】
ウィービング基準変数が軌跡回転量基準である場合、モーションを回転単位モーションの集合と見たとき、動いた回転量ｒをウィービング基準変数に設定する。先に、図４に示された軌跡長基準によるウィービング基準変数生成方式と同一である。つまり、回転単位モーションが合算されて、ウィービング基準変数「ｓ（ｒ）」が生成される。
【０１００】
最大ウィービングポーズＰ_ｍａｘは、ウィービング方向ベクトルによる相対的なポーズを決定する。相対的なポーズは、ウィービング方向ベクトルで所定の大きさおよびいずれの方向にウィービングが進めるかを示す。つまり、最大ウィービングポーズＰ_ｍａｘおよびウィービング方向ベクトルＷによりウィービングの方向および大きさが決定される。  最大ウィービングポーズＰ_ｍａｘは、Ｐ_ｘ、Ｐ_ｙ、Ｐ_ｚ、Ｐ_ｒｏｌｌ、Ｐ_ｐｉｔｃｈ、Ｐ_ｙａｗと定義される。例えば、最大ウィービングポーズＰ_ｍａｘは、均質変換（Ｈｏｍｏｇｅｎｅｏｕｓ  Ｔｒａｎｓｆｏｒｍ）行列の積の形態に算出される。均質変換行列は、位置と回転に対する情報を全て含む。このとき、Ｐ_ｘ、Ｐ_ｙ、Ｐ_ｚは、ｘｙｚ座標軸における位置であり、Ｐ_ｒｏｌｌ、Ｐ_ｐｉｔｃｈ、Ｐ_ｙａｗは、ｘｙｚ座標軸の回転程度を示す情報である。
【０１０１】
ウィービング関数ｆ（ｋ）は、最大ウィービングポーズＰ_ｍａｘをいかなるモーションで発生させるのかを決める関数である。つまり、最大ウィービングポーズＰ_ｍａｘが発生するモーション形状を決める関数である。ウィービング関数ｆ（ｋ）は、周期および最大値が１である周期関数に選ばれる［ｍａｘ（ｆ（ｋ））=１］。
【０１０２】
ウィービングの大きさは最大ウィービングポーズＰ_ｍａｘに反映されているので、ウィービング関数ｆ（ｋ）の最大大きさは１に設定し、ウィービング関数ｆ（ｋ）の周期が１となるようにウィービング関数ｆ（ｋ）設定する。
【０１０３】
例えば、ウィービング関数ｆ（ｋ）をｓｉｎ（ｋ／２ｐｉ）、ｃｏｓ（ｋ／ｐｉ）などのような三角関数に設定するか、または最大大きさが１であり、周期が１である四角パルスまたは三角パルスを設定することができる。このとき、ウィービング関数ｆ（ｋ）が三角関数である場合は連続的なウィービング関数であり、四角パルスまたは三角パルスである場合は非連続ウィービング関数である。
【０１０４】
連続的なウィービング関数ｆ（ｋ）は、時間に対する微分値（ｄ（ｆ（ｋ））／ｄｔ）が０ではなく、時点と終点は同一である［ｆ（０=ｆ１］。
【０１０５】
ウィービング周期ｓｐは、ウィービング関数が繰り返される時間を示し、ウィービング基準変数ｓ（ｔ）と変数ｋとの間には数式１のような関係がある。
［数式１］
mod(s(t), sp)=k(t)
【０１０６】
ここで、ｍｏｄ演算は、時間によるウィービング基準変数ｓ（ｔ）をウィービング周期ｓｐで割った余りを意味する。つまり、時間によるウィービング基準変数ｓ（ｔ）をウィービング周期にｓｐで割った余りが変数ｋである。
【０１０７】
ウィービングモーション処理部２００は、ウィービングサンプリング時間ｔｓを周期で動作する。したがって、ウィービングモーション処理部２００は、ウィービングサンプリング時間ｔｓ内にウィービング量を計算しなければならない。ウィービングサンプリング時間ｔｓおよびウィービング基準変数ｓの設定方法を知らせるウィービング基準変数情報ＳＩは、作業空間ウィービングモーション発生器２５０に伝達される。
【０１０８】
作業空間ウィービングモーション発生器２５０は、ウィービング基準変数情報ＳＩによりウィービング基準変数ｓを決め、ウィービング基準変数ｓが決定されれば変（）を決め、単位ウィービング、ウィービング基準変数ｓにより相対ウィービング量を生成する。このとき、作業空間ウィービングモーション発生器２５０は、ウィービングサンプリング時間ｔｓごとに一定の順序により動作して相対ウィービング量を決定する。ウィービング基準変数ｓが時間基準である場合、相対ウィービング量はＷ_ｐ（ｔ）ベクトルとＷ_ｒ（ｔ）行列である。
【０１０９】
ウィービング基準変数情報ＳＩは、ウィービング基準変数ｓが時間基準の変数なのか、軌跡長基準の変数なのか、または軌跡回転量基準の変数なのかに対する情報を含む。したがって、ウィービングモーション発生器２５０は、ウィービング基準変数情報ＳＩによりウィービング基準変数ｓを決めて、変数ｋを決定する。ウィービング基準変数情報ＳＩが時間基準の基準変数である場合、数式１により変数ｋ（ｔ）が決定される。ウィービングモーション発生器２５０にはウィービング基準変数ｓにより変数ｋを設定する方法が予め設定されている。
【０１１０】
もし、ウィービング基準変数情報ＳＩがウィービング基準変数ｓを軌跡長基準または軌跡回転量基準の変数に指示すれば、作業空間ウィービングモーション発生器２５０は、ウィービングサンプリング時間ｔｓごとに、軌跡長または軌跡回転量を生成する。作業空間連続モーション発生器１５０から単位モーションの伝達を受け、単位モーションを合算して軌跡長または軌跡回転量を生成することができる。
【０１１１】
例えば、先に、図４を参照して説明した軌跡長を合算する方法を利用して、作業空間ウィービングモーション発生器２５０は、作業空間連続モーション発生器１５０から単位ウィービング（ｄ１、ｄ２、．．．）の伝達を受け、これを合算してウィービング基準変数ｓＤを生成することができる。
【０１１２】
また、作業空間ウィービングモーション発生器２５０は、作業空間連続モーション発生器１５０から単位ウィービングの回転量（ｒ１、ｒ２、．．．）を合算してウィービング基準変数ｓ（ｒ）を生成することができる。
【０１１３】
図５は、ウィービング基準変数が時間基準である場合、作業空間ウィービングモーション発生器の動作を示すフローチャートである。下記に説明する動作は、ウィービング基準変数が軌跡長および軌跡回転量基準であるときにも同一に適用される。変数が時間から軌跡長および軌跡回転量に変更されるだけである。
【０１１４】
図５に示されているように、先ず、作業空間ウィービングモーション発生器２５０はウィービング基準変数ｓ（ｔ）および変数ｋ（ｔ）を生成する（Ｓ１００）。ウィービング基準変数ｓ（ｔ）が時間基準であると説明する。しかし、本発明がこれに限定されることではない。
【０１１５】
作業空間ウィービングモーション発生器２５０は、基準変数ｓ（ｔ）がウィービング変更起点ｓｄと同一であるかを判断する（Ｓ１１０）。
【０１１６】
判断結果、ウィービング基準変数ｓ（ｔ）がウィービング変更起点と同一である場合、作業空間ウィービングモーション発生器２５０は次の単位ウィービングを選択し、ウィービング基準変数ｓ（ｔ）を初期化する（Ｓ１２０）［ｓ（ｔ）=０］。  
判断結果ウィービング基準変数ｓ（ｔ）がウィービング変更起点と異なる場合、作業空間ウィービングモーション発生器２５０は、ウィービング基準変数ｓ（ｔ）および変数ｋ（ｔ）を生成する段階Ｓ１００を繰り返す。
【０１１７】
作業空間ウィービングモーション発生器２５０は、現在単位ウィービングのウィービング量を計算する（Ｓ１３０）。ウィービング量Ｐ_ｒ（ｔ）は、数式２のように最大ウィービングポーズＰ_ｍａｘとウィービング関数ｆ（ｋ）の積で示す。二つの因子の積は、ロボットの位置および回転を示し、ロボットの位置はｘｙｚ座標軸における座標で、ロボットの回転はｘｙｚ座標軸の回転で示すことができる。
【０１１８】
最大ウィービングポーズＰ_ｍａｘとウィービング関数ｆ（ｋ）の積は、最大ウィービングポーズＰ_ｍａｘの構成成分に対応するウィービング関数ｆ（ｋ）の成分を代入することを意味する。
［数式２］
P_r(t)=P_max＊f(k(t))
=(P_r_x(t), P_r_y(t), P_r_z(t),P_r_roll(t), P_r_pitch(t), P_r_yaw(t))
【０１１９】
このとき、位置を示すウィービング量を位置ウィービング量Ｐ_ｒ_ｐ（ｔ）といい、回転を示すウィービング量を回転ウィービング量Ｐ_ｒ_ｒ（ｔ）という。
【０１２０】
数式２において、生成されたウィービング量のうちの位置ウィービング量Ｐ_ｒ_ｐ（ｔ）は、Ｐ_ｒ_ｘ（ｔ）、Ｐ_ｒ_ｙ（ｔ）、Ｐ_ｒ_ｚ（ｔ））であり、回転ウィービング量Ｐ_ｒ_ｒ（ｔ）は、Ｐ_ｒ_ｒｏｌｌ（ｔ）、Ｐ_ｒ_ｐｉｔｃｈ（ｔ）、Ｐ_ｒ_ｙａｗ（ｔ）である。
【０１２１】
作業空間ウィービングモーション発生器２５０はウィービング方向を計算する（Ｓ１４０）。ウィービング方向はウィービングベクトルＲｗ（ｔ）で示される。作業空間ウィービングモーション発生器２５０は、上述した通り、移動方向ベクトルＤと基準ベクトルＮの外積を計算して、ウィービング方向ベクトルＲｗ（ｔ）を計算する。
【０１２２】
作業空間ウィービングモーション発生器２５０は、ウィービング方向とウィービング量の積で相対ウィービング量を計算する（Ｓ１４０）。相対ウィービング量は、相対ウィービング量ベクトルＷ_ｐ（ｔ）、Ｗ_ｒ（ｔ）でに示す。相対ウィービング量ベクトルのうちの位置相対ウィービング量ベクトルＷ_ｐ（ｔ）は、数式３の通り、ウィービング方向ベクトルＲｗ（ｔ）に位置ウィービング量Ｐ_ｒ_ｐ（ｔ）をかけた値である。相対ウィービング量ベクトルのうちの回転相対ウィービング量ベクトルＷ_ｒ（ｔ）は、数式４の通り、ウィービング方向ベクトルＲｗ（ｔ）に回転ウィービング量Ｐ_ｒ_ｒ（ｔ）をかけた値である。
［数式３］
W_p(t)=Rw(t)＊P_r_p(t)
［数式４］
W_r(t)=Rw(t)＊P_r_r(t)
【０１２３】
このような方式により、作業空間ウィービングモーション発生器２５０は相対ウィービング量を計算する。作業空間ウィービングモーション発生器２５０は、ウィービング開始／終了情報（Ｂ２）により、現在生成しているウィービングモーションのウィービングと、他のウィービングモーションのウィービングとを区分する。
【０１２４】
補償姿勢処理部３００は、外部状況変化に応じた任意の変位または回転量をモーションに反映する。外部状況の変化に応じた変位または回転量の算出は多様な方法で算出される。
本発明の実施形態では、予め設定された所定の方法により外部状況の変換に応じた任意の変位または回転量を算出する。
【０１２５】
補償姿勢処理部３００は、外部状況を受信する補償姿勢発生器３１０を含む。補償姿勢発生器３１０は外部状況を感知し、外部状況に変化が発生すると、これを補償するための変位または回転量を予め設定された方法により算出する。
【０１２６】
補償姿勢処理部３００は、外部状況に応じた任意の補償姿勢値により決定される補償変位Ｍ_ｐ（ｔ）および補償回転量（Ｍ_ｒ（ｔ））を生成する。
【０１２７】
作業空間モーション合成部２０は、作業空間連続モーション発生器１５０、作業空間ウィービングモーション発生器２５０、および補償姿勢発生器３１０それぞれの出力の入力受けて、作業空間上でロボットの位置および回転量を算出する。
【０１２８】
作業空間モーション合成部２０、作業空間連続モーション発生器１５０、作業空間ウィービングモーション発生器２５０、補償姿勢発生器３１０、およびジョイント空間変換器３０は、同一の作業空間サンプリング周期で同期化されている。作業空間サンプリング周期は、ウィービングサンプリング周期ｔｓと同期化されている。
【０１２９】
作業空間モーション合成部２０は、入力に基づいて作業空間上の位置ＴＳ_ｐ（ｔ）および作業空間上の回転量ＴＳ_ｒ（ｔ）を下記の数式５および６のように算出する。
［数式５］
TS_p(t)=T_p(t)＋W_p(t)＋M_p(t)
［数式６］
TS_r(t)=T_r(t)＊W_r(t)＊M_r(t)
【０１３０】
数式５および６のように、作業空間モーション合成器２０は、作業空間サンプリング周期ごとに施行して、リアルタイムでウィービング発生および補償姿勢値の反映が可能である。
【０１３１】
ジョイント空間変換器３０は、作業空間モーション合成部２０から作業空間上の位置および回転量を受信して、作業空間で生成されたウィービングモーションを実現するためにロボットのジョイントで行われるべきの動作を生成する。つまり、作業空間のウィービングモーションをジョイント空間のモーションに変換する。具体的な方法として、逆機構学（Ｉｎｖｅｒｓｅ  Ｋｉｎｅｍａｔｉｃｓ）がジョイント空間変換器３０に適用される。
【０１３２】
サーボ制御部４０は、インピーダンス制御、位置制御、速度制御、およびトルク制御などのようにロボットのジョイント単位の制御を行う構成である。
【０１３３】
図６ａ〜図６ｄそれぞれは、本発明の実施形態によるモーション制御システムが適用されて生成されるウィービングモーションを示したものである。
【０１３４】
図６ａは、ウィービング関数がサイン関数であり、ウィービング基準変数が時間基準である場合を示した図面である。
【０１３５】
図６ａにおいて、始点ＳＴ１から終点ＥＮ１まで行く連続モーション経路は、１点鎖線で示されている。また、連続モーションにウィービングが加われた作業空間におけるウィービングモーションは、実線で示されている。図６ａのウィービング関数ｆ（ｋ）はｓｉｎ（ｋ／２ｐｉ）である。
【０１３６】
図６ａにおいて、始点ＳＴ１と経由点ＴＨ１との間の区間ＰＥ１における速度Ｖ１が、経由点ＴＨ１と終点ＥＮ１との間の区間ＰＥ２における速度Ｖ２より速い。
【０１３７】
ウィービング関数ｆ（ｋ）は、時間基準のウィービング基準変数ｓ（ｔ）に従うので、図６ａに示されているように、区間ＰＥ２に含まれるウィービング周期が、区間ＰＥ１のウィービング周期よりさらに多い。速度Ｖ１が速度Ｖ２より速いので、同一距離を移動するのに所要される時間がさらに短い。ウィービング基準変数ｓ（ｔ）は時間基準であるので、時間が短いほど当該時間内に含まれるウィービング周期は減少する。
【０１３８】
図６ｂは、ウィービング関数が一次関数であり、ウィービング基準変数が時間基準である場合を示した図面である。図６ｂは図６ａと同様に、連続モーション経路は一点鎖線で、作業空間でウィービングモーションは実線で示されている。図６ａのウィービング関数ｆ（ｋ）はｋである。
【０１３９】
図６ｂにおいて、始点ＳＴ２と経由点ＴＨ２との間の区間ＰＥ２における速度Ｖ３が、経由点ＴＨ２と終点ＥＮ２との間の区間ＰＥ２における速度Ｖ４より速い。したがって、区間ＰＥ４に含まれるウィービング周期が、区間ＰＥ３のウィービング周期よりさらに多い。
【０１４０】
図６ｃは、ウィービング関数がサイン関数であり、ウィービング基準変数が軌跡長基準である場合を示した図面である。図６ｃは図６ａと同様に、連続モーション経路は一点鎖線で、作業空間でウィービングモーションは実線で示されている。図６ｃのウィービング関数ｆ（ｋ）はｓｉｎ（ｋ／２ｐｉ）である。
【０１４１】
図６ｃに示されているように、ウィービング基準変数ｓが軌跡長基準である場合、速度Ｖ５および速度Ｖ６に関係なしに、区間ＰＥ５および区間ＰＥ６に含まれるウィービング周期の個数は、連続モーション移動経路長さにより決定される。
【０１４２】
したがって、区間ＰＥ５および区間ＰＥ６の長さが同一である場合、各区間に含まれるウィービング周期個数は同一である。ただし、作業ロボットが作業区間ＰＥ５を通過する時間と、作業区間ＰＥ６を通過する時間との間には、速度により差がある。
【０１４３】
図６ｄは、ウィービング関数が一次関数であり、ウィービング基準変数が軌跡長基準である場合を示した図面である。図６ｄは図６ａと同様に、連続モーション経路は一点鎖線で、作業空間でウィービングモーションは実線で示されている。図６ｄのウィービング関数ｆ（ｋ）はｋである。
【０１４４】
図６ｄに示されているように、ウィービング基準変数ｓが軌跡長基準である場合、速度Ｖ７および速度Ｖ８に関係なしに、作業区間ＰＥ７および作業区間ＰＥ８それぞれに含まれるウィービング周期の個数は、連続モーション移動経路の長さにより決定される。
【０１４５】
以上、参照した図面と記載された発明の詳細な説明は単に本発明の例示的なものであり、これは本発明を説明するための目的で用いられたもので、意味の限定や特許請求の範囲に記載された本発明の範囲を制限するために用いられたものではない。したがって、本技術分野の通常の知識を有する者であれば、これらから多様な変形および均等な他の実施形態が可能であるという点を理解しなければならない。したがって、本発明の真の技術的保護範囲は、添付された特許請求の範囲の技術的な思想によって決められなければならない。
【産業上利用の可能性】
【０１４６】
このように、リアルタイムでウィービングモーションを制御できる本発明の実施形態によれば、外部作業環境の変化にリアルタイムで対応することができる。
【図１】
0005858168.tif 000002
【図２】
0005858168.tif 000003
【図３ａ】
0005858168.tif 000004
【図３ｂ】
0005858168.tif 000005
【図３ｃ】
0005858168.tif 000006
【図４】
0005858168.tif 000007
【図５】
0005858168.tif 000008
【図６ａ】
0005858168.tif 000009
【図６ｂ】
0005858168.tif 000010
【図６ｃ】
0005858168.tif 000011
【図６ｄ】
0005858168.tif 000012