(57)【要約】  【課題】ロボットアームを目標位置まで移動させている途中に、その目標位置が変更された場合において、従来よりも簡易な構成によって、なるべく高速に当該ロボットアームの軌道を再計画する技術を提供する。  【解決手段】アームの動作を制御するアーム制御装置（制御部１０）であって、ジョイントの角速度または角加速度を指定して、アームの軌道計画を行う軌道計画部と、軌道計画に従って、アームを目標位置まで移動させるアーム制御部と、アームの移動中に前記目標位置が変更された場合、ジョイントの加減速の大きさを所定に維持して、アームの軌道を再計画する軌道再計画部と、を備える。【選択図】図１
2013132696.tif 000002
【特許請求の範囲】
【請求項１】
アームの動作を制御するアーム制御装置であって、
ジョイントの角速度または角加速度を指定して、前記アームの軌道計画を行う軌道計画部と、
前記軌道計画に従って、前記アームを目標位置まで移動させるアーム制御部と、
前記アームの移動中に前記目標位置が変更された場合、前記ジョイントの加減速の大きさを所定に維持して、当該アームの軌道を再計画する軌道再計画部と、を備える、
ことを特徴とするアーム制御装置。
【請求項２】
請求項１に記載のアーム制御装置であって、
前記アームには複数のジョイントが含まれており、
前記軌道計画部は、
前記アームを目標位置まで移動させるのに、前記複数のジョイントのうち、動作の終了タイミングが最も遅くなるジョイントを主ジョイントと決定し、
当該主ジョイント以外のジョイントについて、動作の終了タイミングを当該主ジョイントに合わせる、
ことを特徴とするアーム制御装置。
【請求項３】
請求項１又は２に記載のアーム制御装置であって、
前記軌道再計画部は、
前記アームの移動中に前記目標位置が変更された場合、前記ジョイントの加速度（Ａｍａｘ）、および減速度（−Ａｍａｘ）を固定値として、
Ｄ＝｛（Ｖｉ＋Ｖｐｅａｋ）×Ｔａ／２｝＋｛Ｖｐｅａｋ×Ｔｃ／２｝、
Ｔａ＝（Ｖｐｅａｋ−Ｖｉ）／Ａｍａｘ、
Ｔｃ＝Ｖｐｅａｋ／（−Ａｍａｘ）、
（ただし、Ｄは目標位置にアームが到達するまでにジョイントを回転させる角度、Ｖｉは初角速度、Ｖｐｅａｋは角速度の極値、Ｔａは加速時間、Ｔｃは減速時間）
の数式を用いて、前記ジョイントの角速度の極値（Ｖｐｅａｋ）を算出し、
前記ジョイントを、前記加速度（Ａｍａｘ）で前記加速時間（Ｔａ）回転させ、角速度が前記極値（Ｖｐｅａｋ）に達すると、前記減速度（−Ａｍａｘ）で前記減速時間（Ｔｃ）回転させる、ように再計画する、
ことを特徴とするアーム制御装置。
【請求項４】
請求項３に記載のアーム制御装置であって、
前記軌道再計画部は、
算出した前記ジョイントの角速度の極値（Ｖｐｅａｋ）が、角速度の上限値（Ｖｍａｘ）を超えている場合には、
Ｄ＝｛（Ｖｉ＋Ｖｍａｘ）×Ｔａ／２｝＋｛Ｖｍａｘ×Ｔｂ｝＋｛Ｖｍａｘ×Ｔｃ／２｝、
Ｔａ＝（Ｖｍａｘ−Ｖｉ）／Ａｍａｘ、
Ｔｃ＝Ｖｍａｘ／（−Ａｍａｘ）、
（ただし、Ｄは目標位置にアームが到達するまでにジョイントを回転させる角度、Ｖｉは初角速度、Ｔａは加速時間、Ｔｂは等速時間、Ｔｃは減速時間）
の数式を用いて、前記ジョイントの等速時間（Ｔｂ）を算出し、
前記ジョイントを、前記加速度（Ａｍａｘ）で前記加速時間（Ｔａ）回転させ、角速度が前記上限値（Ｖｍａｘ）に達すると、等速の角速度（Ｖｍａｘ）で前記等速時間（Ｔｂ）回転させ、それから、前記減速度（−Ａｍａｘ）で前記減速時間（Ｔｃ）回転させる、ように再計画する、
ことを特徴とするアーム制御装置。
【請求項５】
請求項１乃至４のいずれか１項に記載のアーム制御装置であって、
前記軌道再計画部は、
前記アームに複数のジョイントが含まれている場合には、当該ジョイントごとに動作を再計画し、
変更された目標位置まで前記アームを移動させるのに、前記複数のジョイントのうち、動作の終了タイミングが最も遅くなるジョイントを主ジョイントと決定し、
当該主ジョイント以外のジョイントについて、動作の終了タイミングを当該主ジョイントに合わせる、
ことを特徴とするアーム制御装置。
【請求項６】
アームの動作を制御するアーム制御装置で行われるアーム制御方法であって、
ジョイントの角速度または角加速度を指定して、前記アームの軌道計画を行う軌道計画ステップと、
前記軌道計画に従って、前記アームを目標位置まで移動させるアーム制御ステップと、
前記アームの移動中に前記目標位置が変更された場合、前記ジョイントの加減速の大きさを所定に維持して、当該アームの軌道を再計画する軌道再計画ステップと、を行う、
ことを特徴とするアーム制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、アーム制御装置、および、アーム制御方法に関する。
【背景技術】
【０００２】
産業用ロボットの中には、ロボットアームを制御して各種作業を行うものがある（例えば、特許文献１）。
【０００３】
ロボットアームの制御では、例えば、
（１）ロボットアームの目標位置を定める。
（２）現在位置から目標位置までロボットアームを移動させる軌道について計画する。
（３）軌道計画に従って、ロボットアームを目標位置まで移動させる。
という（１）〜（３）の動作が繰り返される。
【０００４】
もちろん、ロボットアームを移動させている途中で、目標位置が変更された場合には、再度、上記（２）、（３）の動作を実行しなければならない。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平７−１４１０１５号
【発明の概要】
【発明が解決しようとする課題】
【０００６】
その場合には、ロボットアームを移動させている途中で、その軌道について計画し直すことになるため、迅速に軌道計画を行う必要がある。
【０００７】
本発明は、ロボットアームを目標位置まで移動させている途中に、その目標位置が変更された場合において、従来よりも簡易な構成によって、なるべく高速に当該ロボットアームの軌道を再計画する技術を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本願は、上記課題を解決する手段を複数含んでいるが、その例を挙げるならば、以下のとおりである。
【０００９】
上記の課題を解決するための本発明の第一の態様は、アームの動作を制御するアーム制御装置であって、ジョイントの角速度または角加速度を指定して、前記アームの軌道計画を行う軌道計画部と、前記軌道計画に従って、前記アームを目標位置まで移動させるアーム制御部と、前記アームの移動中に前記目標位置が変更された場合、前記ジョイントの加減速の大きさを所定に維持して、当該アームの軌道を再計画する軌道再計画部と、を備える、ことを特徴とする。
【００１０】
上記の構成によれば、簡単な計算方法によって各ジョイントの動作について再計画できる。そのため、アームを目標位置に向けて移動させている途中に、その目標位置が変更された場合であっても、即座に各ジョイントの動作について再計画できる。
【００１１】
また、前記アームには複数のジョイントが含まれており、前記軌道計画部は、前記アームを目標位置まで移動させるのに、前記複数のジョイントのうち、動作の終了タイミングが最も遅くなるジョイントを主ジョイントと決定し、当該主ジョイント以外のジョイントについて、動作の終了タイミングを当該主ジョイントに合わせる、ようにしてもよい。
【００１２】
上記の構成によれば、各ジョイントの動作を、ほぼ同時に終了させることができ、アームが滑らかに動作しているようにみえる。
【００１３】
また、前記軌道再計画部は、前記アームの移動中に前記目標位置が変更された場合、前記ジョイントの加速度（Ａｍａｘ）、および減速度（−Ａｍａｘ）を固定値として、
Ｄ＝｛（Ｖｉ＋Ｖｐｅａｋ）×Ｔａ／２｝＋｛Ｖｐｅａｋ×Ｔｃ／２｝、
Ｔａ＝（Ｖｐｅａｋ−Ｖｉ）／Ａｍａｘ、
Ｔｃ＝Ｖｐｅａｋ／（−Ａｍａｘ）、
（ただし、Ｄは目標位置にアームが到達するまでにジョイントを回転させる角度、Ｖｉは初角速度、Ｖｐｅａｋは角速度の極値、Ｔａは加速時間、Ｔｃは減速時間）
の数式を用いて、前記ジョイントの角速度の極値（Ｖｐｅａｋ）を算出し、前記ジョイントを、前記加速度（Ａｍａｘ）で前記加速時間（Ｔａ）回転させ、角速度が前記極値（Ｖｐｅａｋ）に達すると、前記減速度（−Ａｍａｘ）で前記減速時間（Ｔｃ）回転させる、ように再計画する、ようにしてもよい。
【００１４】
上記の構成によれば、アームの目標位置が変更された地点から、変更後の目標位置まで、ジョイントの回転方向を正方向に保てる場合において、簡単な計算方法によってジョイントの動作について再計画できる。
【００１５】
また、前記軌道再計画部は、算出した前記ジョイントの角速度の極値（Ｖｐｅａｋ）が、角速度の上限値（Ｖｍａｘ）を超えている場合には、
Ｄ＝｛（Ｖｉ＋Ｖｍａｘ）×Ｔａ／２｝＋｛Ｖｍａｘ×Ｔｂ｝＋｛Ｖｍａｘ×Ｔｃ／２｝、
Ｔａ＝（Ｖｍａｘ−Ｖｉ）／Ａｍａｘ、
Ｔｃ＝Ｖｍａｘ／（−Ａｍａｘ）、
（ただし、Ｄは目標位置にアームが到達するまでにジョイントを回転させる角度、Ｖｉは初角速度、Ｔａは加速時間、Ｔｂは等速時間、Ｔｃは減速時間）
の数式を用いて、前記ジョイントの等速時間（Ｔｂ）を算出し、前記ジョイントを、前記加速度（Ａｍａｘ）で前記加速時間（Ｔａ）回転させ、角速度が前記上限値（Ｖｍａｘ）に達すると、等速の角速度（Ｖｍａｘ）で前記等速時間（Ｔｂ）回転させ、それから、前記減速度（−Ａｍａｘ）で前記減速時間（Ｔｃ）回転させる、ように再計画する、ようにしてもよい。
【００１６】
上記の構成によれば、アームの目標位置が変更された地点から、変更後の目標位置まで、ジョイントの回転方向を正方向に保てる場合であって、かつ、ジョイントの角速度の極値（Ｖｐｅａｋ）が、角速度の上限値（Ｖｍａｘ）を超えている場合において、簡単な計算方法によってジョイントの動作について再計画できる。
【００１７】
また、前記軌道再計画部は、前記アームに複数のジョイントが含まれている場合には、当該ジョイントごとに動作を再計画し、変更された目標位置まで前記アームを移動させるのに、前記複数のジョイントのうち、動作の終了タイミングが最も遅くなるジョイントを主ジョイントと決定し、当該主ジョイント以外のジョイントについて、動作の終了タイミングを当該主ジョイントに合わせる、ようにしてもよい。
【００１８】
上記の構成によれば、ジョイントの動作計画が再計画された場合でも、アーム全体の動作が滑らかにみえるようになる。
【００１９】
上記の課題を解決するための本発明の第二の態様は、アームの動作を制御するアーム制御装置で行われるアーム制御方法であって、ジョイントの角速度または角加速度を指定して、前記アームの軌道計画を行う軌道計画ステップと、前記軌道計画に従って、前記アームを目標位置まで移動させるアーム制御ステップと、前記アームの移動中に前記目標位置が変更された場合、前記ジョイントの加減速の大きさを所定に維持して、当該アームの軌道を再計画する軌道再計画ステップと、を行う、ことを特徴とする。
【００２０】
上記の本発明の第二の態様によっても、簡単な計算方法によって各ジョイントの動作について再計画できる。そのため、アームを目標位置に向けて移動させている途中に、その目標位置が変更された場合であっても、即座に各ジョイントの動作について再計画できる。
【００２１】
上記した以外の課題、構成、及び効果は、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【００２２】
【図１】ロボット１の外観例を示す図である。
【図２】ロボット１の概略構成の一例を示すブロック図である。
【図３】アーム制御タスクの一例について説明するためのフロー図である。
【図４】（Ａ）第１のジョイント（第１軸）についての軌道計画の一例を示す図である。（Ｂ）第２のジョイント（第２軸）についての軌道計画の一例を示す図である。（Ｃ）第３のジョイント（第３軸）についての軌道計画の一例を示す図である。（Ｄ）主ジョイントに合わせて第２のジョイントの終了タイミングを調整する場合の例を示す図である。（Ｅ）主ジョイントに合わせて第３のジョイントの終了タイミングを調整する場合の例を示す図である。
【図５】目標位置が変更されてから停止するまでにジョイントが回転する回転量Ｄ’を計算する方法について説明する図である。
【図６】（Ａ）ジョイントの軌道を再計画する場合の一例を示す図である（ただし、「Ｄ’＜Ｄ”」の場合）。（Ｂ）ジョイントの軌道を再計画する場合の一例を示す図である（ただし、「Ｄ’＜Ｄ”」かつ「Ｖｐｅａｋ＞ＶＭＡＸ」場合）。
【図７】（Ａ）ジョイントの軌道を再計画する場合の一例を示す図である（ただし、「Ｄ’＞Ｄ”」の場合）。（Ｂ）ジョイントの軌道を再計画する場合の一例を示す図である（ただし、「Ｄ’＞Ｄ”」かつ「Ｖｐｅａｋ＜−ＶＭＡＸ」場合）。
【図８】（Ａ）第２のジョイント（第２軸）ついて再計画された軌道計画の一例を示す図である。（Ｂ）第１のジョイント（第１軸）ついて再計画された軌道計画の一例を示す図である。（Ｃ）第３のジョイント（第３軸）ついて再計画された軌道計画の一例を示す図である。（Ｄ）各ジョイントの軌道が再計画されたときに、主ジョイントに合わせて第１のジョイントの終了タイミングを調整する場合の例を示す図である。（Ｅ）各ジョイントの軌道が再計画されたときに、主ジョイントに合わせて第３のジョイントの終了タイミングを調整する場合の例を示す図である。
【図９】目標位置計算タスクの一例について説明するためのフロー図である。
【発明を実施するための形態】
【００２３】
以下、本発明の実施形態について図面を用いて説明する。
【００２４】
図１は、本実施形態にかかるロボット１についての外観例を示す図である。
【００２５】
図示するように、ロボット１は、例えば、可動部３０と、撮像部２０と、制御部１０と、を備えるアーム型のロボットである。
【００２６】
可動部３０は、ロボット１の可動部分である。図示する例では、可動部３０として、１本のロボットアームを示しており、点線で囲まれたアーム部４と、アーム部４の先端に取り付けられたハンド部５と、を含む。なお、いわゆる双腕ロボットのように２本のロボットアームを有していてもよい。
【００２７】
アーム部４は、複数のジョイント（関節）６と、複数のリンク７と、を含む。
【００２８】
各ジョイント６は、リンク７同士や、ロボットの台座（胴体）とリンク７、などを、回動自在に（ただし、所定の可動範囲内で回動可能に）連結している。各ジョイント６は、例えば、回転ジョイントであり、リンク７間の角度を変化させたり、リンク７を軸回転させたりできるように設けられている。従って、各ジョイント６を連動させて駆動することにより、作業点（例えば、ハンド部５）を、自在に（ただし、所定の可動範囲内で）移動させることができるとともに、自由な方向へ向けることもできる。本図の例では、アーム部４は、３つのジョイントを備えた３軸アームとなっている。
【００２９】
ハンド部５は、例えば、複数の指を備え、隣り合う指を近付ける方向に移動させ、少なくとも２本の指で把持対象物を挟んで把持することができる。
【００３０】
撮像部２０は、ロボット１の外部環境（例えば、図示する作業台付近）を撮影して、画像データを生成するユニットである。撮像部２０は、例えば、カメラを含み、天井や壁などに設けられる。撮像部２０は、制御部１０の制御により、作業点（例えば、ハンド部５）の移動先、すなわち目標位置の撮影を行うとともに、画像データを生成し、制御部１０に出力する。
【００３１】
制御部１０は、ロボット１の全体を制御する処理を行う。制御部１０は、例えば、ロボット１の本体（可動部３０）に内蔵しておいてもよいし、図示するように、ロボット１の本体とは離れた場所に（遠隔操作可能に）配置しておいてもよい。
【００３２】
図２は、ロボット１の概略構成の一例を示すブロック図である。図示するように、制御部１０は、演算装置であるＣＰＵ１１と、揮発性の記憶装置であるＲＡＭ１２と、不揮発性の記憶装置であるＲＯＭ１３と、制御部１０と他のユニットを接続するインターフェイス（Ｉ／Ｆ）回路１４と、ロボット１の外部の装置と通信を行う通信装置１５と、を備える。
【００３３】
次に、制御部１０の機能構成例について説明する。図２の下部には、制御部１０についての機能構成例が示されている。
【００３４】
図示するように、制御部１０は、中央制御部１０１と、記憶部１０２と、撮像制御部１０３と、目標位置計算部１０４と、軌道計画部１０５と、エンコーダー読取部１０６と、アーム制御部１０７と、軌道再計画部１０８と、を備える。
【００３５】
中央制御部１０１は、他の各部（１０２〜１０７）を統括的に制御する。
【００３６】
記憶部１０２は、各種データやプログラムを記憶する。例えば、記憶部１０２は、可動部３０についての軌道計画を行うプログラム、その軌道計画に従って可動部３０を制御するプログラム等を記憶する。
【００３７】
撮像制御部１０３は、撮像部２０を制御する。例えば、撮像制御部１０３は、撮像部２０に対して、作業点（例えば、ハンド部５）の移動先、すなわち目標位置の周辺を撮影させる。そして、撮像制御部１０３は、撮像部２０で撮影された画像データを取得する。
【００３８】
目標位置計算部１０４は、撮像部２０で撮像された画像データについて画像認識を行う。目標位置計算部１０４は、この画像認識に基づき、作業点の移動先である目標位置（例えば、３次元座標）を計算する。
【００３９】
軌道計画部１０５は、作業点（例えば、ハンド部５）を目標位置まで移動させるときの、可動部３０についての軌道を計画する。
【００４０】
具体的には、軌道計画部１０５は、各ジョイント６を回転させる角速度または角加速度の時間変化について指定（決定）する。
【００４１】
また、軌道計画部１０５は、作業点（例えば、ハンド部５）を目標位置まで移動させるのに、複数のジョイント６のうち、動作の終了タイミングが最も遅くなるジョイント６を主ジョイントと決定する。そして、軌道計画部１０５は、主ジョイント以外のジョイント６について、動作の終了タイミングを主ジョイントに合わせる。これにより、各ジョイント６の動作は、ほぼ同時に終了することになり、可動部３０が滑らかに動作しているようにみえる。
【００４２】
エンコーダー読取部１０６は、アーム部４の各ジョイント６に設けられたアクチュエーター３１のエンコーダー値を読み取る。こうしたエンコーダー値は、各ジョイント６の回転角度を特定できるため、各ジョイント６の角速度、角加速度などを把握するのに利用され、また、可動部３０の位置、移動方向、移動量、速度、加速度などについて把握するのにも利用される。
【００４３】
アーム制御部１０７は、軌道計画部１０５による軌道計画に従って、作業点を目標位置に向けて移動させる制御を行う。すなわち、アーム制御部１０７は、指定された角速度または角加速度で各ジョイント６を回転させる。
【００４４】
このような制御を行うためには、例えば、アーム制御部１０７は、エンコーダー読取部１０６で読み取られたエンコーダー値や、各種センサー３２の出力信号に基づいて、作業点が目的の位置（すなわち、目標位置）及び姿勢となるようにアクチュエーター３１を制御すればよい。
【００４５】
また、作業点を目標位置に向けて移動させている途中に、その目標位置が変更されてしまう場合がある。
【００４６】
この場合には、軌道再計画部１０８は、ジョイント６が回転する加減速の大きさ（すなわち、角加速度と角減速度）を所定に維持して、可動部３０の軌道について再計画する。
【００４７】
また、可動部３０の軌道について再計画する場合においても、軌道再計画部１０８は、作業点（例えば、ハンド部５）を、変更された目標位置まで移動させるのに、複数のジョイント６のうち、動作の終了タイミングが最も遅くなるジョイント６を主ジョイントと決定する。そして、軌道再計画部１０８は、主ジョイント以外のジョイント６について、動作の終了タイミングを主ジョイントに合わせる。
【００４８】
上記の記憶部１０２以外の各機能部は、例えば、ＣＰＵ１１がＲＯＭ１３に格納された所定のプログラムをＲＡＭ１２に読み出して実行することにより実現される。記憶部１０２は、例えば、ＲＡＭ１２やＲＯＭ１３により実現される。なお、前記所定のプログラムは、例えば、予めＲＯＭ１３にインストールされてもよいし、通信装置１５を介してネットワークからダウンロードされてインストール又は更新されてもよい。
【００４９】
以上のロボット１の構成は、本実施形態の特徴を説明するにあたって主要構成を説明したのであって、上記の構成に限られない。可動部３０を有していれば、どのような構造であってもよい。また、一般的なロボットが備える構成を排除するものではない。
【００５０】
例えば、図１にはアーム部４として３軸アームが示されているが、軸数（ジョイント数）をさらに増加させてもよいし減らしてもよい。リンクの数を増減させてもよい。また、アーム部４、ハンド部５、リンク、ジョイント等の各種部材の形状、大きさ、配置、構造等も適宜変更してよい。
【００５１】
また、上記した制御部１００の各機能構成は、制御部１００の構成を理解容易にするために、主な処理内容に応じて分類したものである。構成要素の分類の仕方や名称によって、本願発明が制限されることはない。制御部１００の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。また、各構成要素の処理は、１つのハードウェアで実行されてもよいし、複数のハードウェアで実行されてもよい。
【００５２】
次に、本実施形態における、上記構成からなるロボット１の特徴的な動作について説明する。
【００５３】
＜アーム制御タスク＞
アーム部４を制御することによって、作業点（例えば、ハンド部５）を目標位置まで移動させるタスクを、以下では「アーム制御タスク」とよぶ。
【００５４】
図３は、アーム制御タスクの一例について説明するためのフロー図である。
【００５５】
中央制御部１０１は、ユーザーからの指示に基づいて、アーム制御タスクを開始する。
【００５６】
アーム制御タスクが開始されると、まず、エンコーダー読取部１０６は、アーム部４の各ジョイント６に設けられたアクチュエーター３１のエンコーダー値を読み取る（ステップＳ１０１）。ここで、読み取られたエンコーダー値は、各ジョイント６の基準位置からの回転角に相当し、例えば、ジョイント６を介して連結された２つのリンク７がなす角を特定できる。
【００５７】
このステップＳ１０１の処理により、現時点（初期）の可動部３０の状態（各ジョイント６や作業点の位置、姿勢）を知ることができる。
【００５８】
次に、目標位置計算部１０４は、作業点（例えば、ハンド部５）の目標位置を計算するために、目標位置計算タスクを開始する（ステップＳ１０２）。なお、目標位置計算タスクの詳細については後述する。
【００５９】
目標位置計算タスクにより、作業点の目標位置（例えば、３次元座標）が定まると、軌道計画部１０５は、作業点を目標位置まで移動させるときの、可動部３０についての軌道を計画する（ステップＳ１０３）。
【００６０】
具体的には、ステップＳ１０３では、軌道計画部１０５は、ジョイント６ごとに、回転させる角速度または角加速度を指定して動作計画を行う。
【００６１】
図４（Ａ）は、第１のジョイント（第１軸）についての動作計画の一例を示す図である。また、図４（Ｂ）は、第２のジョイント（第２軸）についての動作計画の一例を示す図である。また、図４（Ｃ）は、第３のジョイント（第３軸）についての動作計画の一例を示す図である。
【００６２】
各図に示すように、ステップＳ１０３では、軌道計画部１０５は、加速区間、等速区間、減速区間に分けて、各ジョイント６の動作計画を行う。加速区間では、各ジョイント６が加速し得る最大の角加速度（ＡＭＡＸ）で回転させる。また、等速区間では、各ジョイント６の角速度の上限値（ＶＭＡＸ）で等速回転させる。また、減速区間では、各ジョイント６が減速し得る最大の角減速度（−ＡＭＡＸ）で回転させる。このように、軌道計画部１０５は、各ジョイント６の角速度波形が台形となるように動作計画を行う。
【００６３】
なお、軌道計画部１０５は、作業点（例えば、ハンド部１０５）を移動させる距離とその方向に応じて、加速時間（加速区間の時間）Ｔａ、等速時間（等速区間の時間）Ｔｂ、減速時間（減速区間の時間）Ｔｃ、をジョイント６ごとに決定する。
【００６４】
例えば、軌道計画部１０５は、作業点を現在位置から目標位置まで移動させるために必要な各ジョイント６の回転量Ｄを求める。そして、軌道計画部１０５は、各ジョイント６の角速度波形（台形）の面積が、先に求めた各ジョイント６の回転角度Ｄと一致するように、加速時間Ｔａ、等速時間Ｔｂ、減速時間Ｔｃを決定する。
【００６５】
すなわち、軌道計画部１０５は、
Ｄ＝（ＶＭＡＸ×Ｔａ）／２＋（ＶＭＡＸ×Ｔｂ）＋（ＶＭＡＸ×Ｔｃ）／２、
ＡＭＡＸ＝ＶＭＡＸ／Ｔａ、
−ＡＭＡＸ＝−ＶＭＡＸ／Ｔｃ、
の３式を満たす、「加速時間Ｔａ」、「等速時間Ｔｂ」、「減速時間Ｔｃ」を、ジョイント６ごとに求める。
【００６６】
ここで、「ＶＭＡＸ」と「ＡＭＡＸ」はそれぞれ予め定められた定数であり、例えば、記憶部１０２に記憶されている。また、ジョイント６ごとに「ＶＭＡＸ」の値が異なる場合や、ジョイント６ごとに「ＡＭＡＸ」の値が異なる場合には、ジョイント６ごとに記憶しておけばよい。
【００６７】
上記のような計画方法によれば、各ジョイント６の総動作時間（Ｔａ＋Ｔｂ＋Ｔｃ）は、それぞれ最短となる。ただし、各ジョイント６の動作終了タイミングは不揃いとなるため（図示するＴ１、Ｔ２、Ｔ３）、可動部３０全体の動作が滑らかにみえない場合がある。
【００６８】
そこで、可動部３０全体の動作を滑らかにみせるために、軌道計画部１０５は、ステップＳ１０３において、各ジョイント６の動作終了タイミングが揃うように、軌道計画を修正する。
【００６９】
具体的には、軌道計画部１０５は、全てのジョイント６のうち、動作の終了タイミングが最も遅くなるジョイント６を主ジョイントと決定する。従って、図示する例では、第１のジョイント（第１軸）の動作に最も時間がかかるため（Ｔ１＞Ｔ２、Ｔ３）、第１のジョイントを主ジョイントとして決定する。
【００７０】
それから、軌道計画部１０５は、主ジョイント以外のジョイント６について、動作の終了タイミングを主ジョイントに合わせる。従って、図示する例では、軌道計画部１０５は、第２のジョイント（第２軸）の動作終了タイミングＴ２を、主ジョイントの動作終了タイミングＴ１に変更する。また、第３のジョイント（第３軸）の動作終了タイミングＴ３を、主ジョイントの動作終了タイミングＴ１に変更する。
【００７１】
ただし、このように主ジョイント以外のジョイント６について、動作終了タイミングを変更する場合であっても、作業点を現在位置から目標位置まで移動させるときの、各ジョイント６の回転角度Ｄについては変更するわけにはいかない。
【００７２】
そこで、軌道計画部１０５は、各ジョイント６の角速度波形の面積（斜線部分）を一定に保ったまま、主ジョイント以外のジョイント６について、動作終了タイミングを変更する。
【００７３】
図４（Ｄ）は、主ジョイントに合わせて第２のジョイントの動作終了タイミングを変更する場合の例を示す図である。また、図４（Ｅ）は、主ジョイントに合わせて第３のジョイントの動作終了タイミングを変更する場合の例を示す図である。
【００７４】
各図に示すように、軌道計画部１０５は、主ジョイント以外のジョイント６について、
（１）Ｄ＝（ＶＭＡＸ’×Ｔａ’）／２＋（ＶＭＡＸ’×Ｔｂ’）＋（ＶＭＡＸ’×Ｔｃ’）／２、
（２）ＡＭＡＸ＝ＶＭＡＸ’／Ｔａ’、
（３）−ＡＭＡＸ＝−ＶＭＡＸ’／Ｔｃ’、
（４）Ｔａ’＋Ｔｂ’＋Ｔｃ’＝Ｔ１
の４式を満たす、「加速時間Ｔａ’」、「等速時間Ｔｂ’」、「減速時間Ｔｃ’」、「仮の上限値ＶＭＡＸ’」を求める。
【００７５】
ここで、「ＶＭＡＸ’」は、ジョイント６の角速度についての仮の上限値ＶＭＡＸ’であり、「Ｔ１」は、主ジョイントの動作終了タイミング（動作開始から動作終了までの時間）である。
【００７６】
上記のように主ジョイント以外のジョイント６について、動作計画を修正することによって、各ジョイント６の動作終了タイミングが揃うようになる（図４（Ｄ）、（Ｅ）に示す「Ｔ１」）。こうして、可動部３０全体の動作が滑らかにみえるようになる。
【００７７】
以上のステップＳ１０３の処理を終えると、アーム制御部１０７は、ステップＳ１０３で作成された軌道計画に従って、可動部３０を制御する（ステップＳ１０４）。
【００７８】
具体的には、アーム制御部１０７は、ステップＳ１０３で作成された軌道計画（図４（Ａ）、（Ｄ）、（Ｅ））で指定されている角速度または角加速度で、各ジョイント６を回転させる。
【００７９】
所定時間（例えば、数秒）回転後、アーム制御部１０７は、所定時間（例えば、１ｍｓ）待機し（ステップＳ１０５）、処理をステップＳ１０６へ移行する。
【００８０】
ところで、本フロー（アーム制御タスク）の制御に並行して、ステップＳ１０２で開始された目標位置計算タスクは実行され続けている。そして、作業点（例えば、ハンド部５）を目標位置に向けて移動させている途中に、その目標位置が変更された場合には、目標位置の変更指令が目標位置計算部１０４から送られてくる仕組みになっている。
【００８１】
そこで、ステップＳ１０６に処理が移行すると、軌道再計画部１０８は、目標位置計算部１０４から目標位置の変更指令が送られているかどうか判別する（ステップＳ１０６）。
【００８２】
軌道再計画部１０８は、目標位置の変更指令が送られていない場合には（ステップＳ１０６；Ｎｏ）、目標位置は変更されていないものと判定し、そのまま処理をステップＳ１０８へ移行する。
【００８３】
一方、軌道再計画部１０８は、目標位置の変更指令が送られている場合には（ステップＳ１０６；Ｙｅｓ）、目標位置が変更されたものと判定し、可動部３０の軌道について再計画する（ステップＳ１０７）。
【００８４】
具体的には、ステップＳ１０７では、軌道再計画部１０８は、各ジョイント６が回転する加減速の大きさ、すなわち、角加速度（ＡＭＡＸ）と角減速度（−ＡＭＡＸ）を所定に維持して（変えずに）、各ジョイント６の動作計画をやり直す。
【００８５】
以下に、ステップＳ１０７の処理について詳細に説明する。
【００８６】
まず、ステップＳ１０７に処理が移行すると、軌道再計画部１０８は、ジョイント６の角速度を現在の角速度（Ｖｉ）から最大の角減速度（−ＡＭＡＸ）で減速させて、その回転が停止するまでに回転する回転量Ｄ’を求める。なお、ここで回転量Ｄ’を求めているのは、求めた回転量Ｄ’に応じて、ジョイント６の動作計画をやり直す方法が異なるためである（後述する再計画方法１〜４）。
【００８７】
図５は、この回転量Ｄ’を計算する方法について説明する図である。
【００８８】
図示するように、軌道再計画部１０８は、縦縞部分の面積を求めることによって、回転量Ｄ’を計算する。
【００８９】
すなわち、軌道再計画部１０８は、
（１）Ｄ’＝Ｖｉ×Ｔ０／２
（２）−ＡＭＡＸ＝−Ｖｉ／Ｔ０
の２式を満たす、「回転量Ｄ’」を求める。
【００９０】
ここで、「Ｔ０」は、目標位置が変更されてからジョイント６の回転が停止するまでにかかる時間を表す。また、「−ＡＭＡＸ」は上述した通り予め定められた定数である。また、「Ｖｉ」は、各ジョイント６に設けられたアクチュエーター３１のエンコーダー値から求められる。
【００９１】
次に、軌道再計画部１０８は、作業点を、現在位置（すなわち、目標位置が変更されたポイント）Ｓから、変更された目標位置Ｇ’まで移動させるのに、必要な各ジョイント６の回転量Ｄ”を求める。
【００９２】
そして、軌道再計画部１０８は、ここで求めた回転量Ｄ”と、上記の回転量Ｄ’と、を比較する。
【００９３】
（再計画方法１）
このとき、「Ｄ’＜Ｄ”」であれば、現在位置Ｓから変更後の目標位置Ｇ’までの間において、ジョイント６の回転方向は正方向の状態（すなわち、ジョイント６の角速度Ｖ＞０）が保たれているのが好ましい。
【００９４】
図６（Ａ）は、「Ｄ’＜Ｄ”」の条件が成立しているときに、ジョイント６の動作を再計画する場合の一例を示す図である。
【００９５】
図示するように、軌道再計画部１０８は、まずは、加速区間と減速区間に分けて、各ジョイント６の動作を再計画する。具体的には、軌道再計画部１０８は、各ジョイント６を、初速度（Ｖｉ）から角加速度（ＡＭＡＸ）で回転させ（加速区間）、角速度が極値（Ｖｐｅａｋ）に達すると、角減速度（−ＡＭＡＸ）で回転させる（減速区間）、ように再計画する。このように、軌道再計画部１０８は、各ジョイント６の角速度波形が極値（Ｖｐｅａｋ）をもつ四辺形（台形＋三角形）となるように動作の再計画を行う（太線）。
【００９６】
なお、軌道再計画部１０８は、作業点を現在位置（目標位置が変更されたポイント）Ｓから変更後の目標位置Ｇ’まで移動させる距離とその方向に応じて、加速時間Ｔａ、減速時間Ｔｃ、を決定する。
【００９７】
例えば、軌道再計画部１０８は、各ジョイント６の角速度波形の面積（網掛け部分）が、先に求めておいた各ジョイント６の回転量Ｄ”と一致するように、加速時間Ｔａ、減速時間Ｔｃを決定する。
【００９８】
すなわち、軌道再計画部１０８は、ジョイント６の角加速度（ＡＭＡＸ）、および角減速度（−ＡＭＡＸ）を固定値として、
（１）Ｄ”＝｛（Ｖｉ＋Ｖｐｅａｋ）×Ｔａ／２｝＋｛Ｖｐｅａｋ×Ｔｃ／２｝、
（２）Ｔａ＝（Ｖｐｅａｋ−Ｖｉ）／ＡＭＡＸ、
（３）Ｔｃ＝Ｖｐｅａｋ／（−ＡＭＡＸ）、
の３式を満たす「加速時間Ｔａ」、「減速時間Ｔｃ」、「極値Ｖｐｅａｋ」を、ジョイント６ごとに求める。
【００９９】
以上のような方法によれば、ジョイント６の角加速度（ＡＭＡＸ）、および角減速度（−ＡＭＡＸ）を固定値としているため、上記のような簡単な計算式によって各ジョイント６の動作について再計画できる。その結果、作業点を目標位置に向けて移動させている途中に、その目標位置が変更された場合であっても、即座に各ジョイント６の動作について再計画できる。
【０１００】
（再計画方法２）
また、「Ｄ’＜Ｄ”」であったとしても、求めた「極値Ｖｐｅａｋ」が、角速度の上限値ＶＭＡＸを上回る場合には、上記の再計画方法１を採用できない。
【０１０１】
図６（Ｂ）は、「Ｄ’＜Ｄ”」かつ「Ｖｐｅａｋ＞ＶＭＡＸ」の条件が成立するときに、ジョイント６の動作を再計画する場合の一例を示す図である。
【０１０２】
図示するように、軌道再計画部１０８は、加速区間、等速区間、減速区間に分けて、各ジョイント６の動作を再計画する。具体的には、軌道再計画部１０８は、各ジョイント６を、初速度（Ｖｉ）から角加速度（ＡＭＡＸ）で回転させ（加速区間）、角速度が上限値（ＶＭＡＸ）に達すると、等速の角速度（ＶＭＡＸ）で回転させ（等速区間）、それから、角減速度（−ＡＭＡＸ）で回転させる（減速区間）、ように再計画する。このように、軌道再計画部１０８は、各ジョイント６の角速度波形が五角形（台形＋矩形＋三角形）となるように動作の再計画を行う（太線）。
【０１０３】
なお、軌道再計画部１０８は、作業点を現在位置（目標位置が変更されたポイント）Ｓから変更後の目標位置Ｇ’まで移動させる距離とその方向に応じて、加速時間Ｔａ、等速時間Ｔｂ、減速時間Ｔｃ、を決定する。
【０１０４】
例えば、軌道再計画部１０８は、各ジョイント６の角速度波形の面積（網掛け部分）が、先に求めておいた各ジョイント６の回転量Ｄ”と一致するように、加速時間Ｔａ、等速時間Ｔｂ、減速時間Ｔｃを決定する。
【０１０５】
すなわち、軌道再計画部１０８は、ジョイント６の角加速度（ＡＭＡＸ）、および角減速度（−ＡＭＡＸ）を固定値として、
（１）Ｄ”＝｛（Ｖｉ＋ＶＭＡＸ）×Ｔａ／２｝＋｛ＶＭＡＸ×Ｔｂ｝＋｛ＶＭＡＸ×Ｔｃ／２｝、
（２）Ｔａ＝（ＶＭＡＸ−Ｖｉ）／ＡＭＡＸ、
（３）Ｔｃ＝ＶＭＡＸ／（−ＡＭＡＸ）、
の３式を満たす「加速時間Ｔａ」、「減速時間Ｔｂ」、「減速時間Ｔｃ」を、ジョイント６ごとに求める。
【０１０６】
このように各ジョイント６の動作について再計画すれば、「Ｖｐｅａｋ＞ＶＭＡＸ」となる場合であっても問題なく再計画でき、上記再計画方法１と同様の効果が得られる。
【０１０７】
（再計画方法３）
また、「Ｄ’＞Ｄ”」となる場合には、現在位置Ｓから変更後の目標位置Ｇ’までの一部区間において、ジョイント６の回転を逆回転（すなわち、ジョイント６の角速度Ｖ＜０）させる必要がある。
【０１０８】
図７（Ａ）は、「Ｄ’＞Ｄ”」の条件が成立しているときに、ジョイント６の動作を再計画する場合の一例を示す図である。
【０１０９】
図示するように、軌道再計画部１０８は、まずは、第１の減速区間、第２の減速区間、加速区間に分けて、各ジョイント６の動作を再計画する。具体的には、軌道再計画部１０８は、各ジョイント６を、初速度（Ｖｉ）から角減速度（−ＡＭＡＸ）で回転させ（第１の減速区間）、角速度が０に到達してもそのまま角減速度（−ＡＭＡＸ）で回転させ（第２の減速区間）、角速度が極値（Ｖｐｅａｋ）に達すると、角加速度（ＡＭＡＸ）で回転させる（加速区間）、ように再計画する。このように、軌道再計画部１０８は、各ジョイント６の角速度波形が時間軸の正負にまたがる（正の三角形＋負の三角形）ように動作の再計画を行う（太線）。
【０１１０】
なお、軌道再計画部１０８は、作業点を現在位置（目標位置が変更されたポイント）Ｓから変更後の目標位置Ｇ’まで移動させる距離とその方向に応じて、第１の減速時間Ｔｃ、第２の減速時間Ｔｃ’、加速時間Ｔａ、を決定する。
【０１１１】
例えば、軌道再計画部１０８は、各ジョイント６の角速度波形の面積、すなわち、図示する正の三角形の面積Ｄ’から負の三角形の面積ＤＬを差し引いた面積が、先に求めておいた各ジョイント６の回転量Ｄ”と一致するように、第１の減速時間Ｔｃ、第２の減速時間Ｔｃ’、加速時間Ｔａを決定する。
【０１１２】
すなわち、軌道再計画部１０８は、ジョイント６の角加速度（ＡＭＡＸ）、および角減速度（−ＡＭＡＸ）を固定値として、
（１）Ｄ”＝｛Ｖｉ×Ｔｃ／２｝−｛｜Ｖｐｅａｋ｜×（Ｔｃ’＋Ｔａ）／２｝、
（２）Ｔｃ＝−Ｖｉ／（−ＡＭＡＸ）、
（３）Ｔｃ’＝−｜Ｖｐｅａｋ｜／（−ＡＭＡＸ）、
（４）Ｔａ＝｜Ｖｐｅａｋ｜／ＡＭＡＸ、
の４式を満たす「第１の減速時間Ｔｃ」、「第２の減速時間Ｔｃ’」、「加速時間Ｔａ」、「極値Ｖｐｅａｋ」を、ジョイント６ごとに求める。
【０１１３】
このように各ジョイント６の動作について再計画すれば、「Ｄ’＞Ｄ”」となる場合であっても問題なく再計画でき、上記再計画方法１、２と同様の効果が得られる。
【０１１４】
（再計画方法４）
また、「Ｄ’＞Ｄ”」である場合に、求めた「極値Ｖｐｅａｋ」が、角速度の下限値（−ＶＭＡＸ）下回る場合には、上記の再計画方法３を採用できない。
【０１１５】
図７（Ｂ）は、「Ｄ’＞Ｄ”」かつ「Ｖｐｅａｋ＜−ＶＭＡＸ」の条件が成立するときに、ジョイント６の動作を再計画する場合の一例を示す図である。
【０１１６】
図示するように、軌道再計画部１０８は、第１の減速区間、第２の減速区間、等速区間、加速区間に分けて、各ジョイント６の動作を再計画する。具体的には、軌道再計画部１０８は、各ジョイント６を、初速度（Ｖｉ）から角減速度（−ＡＭＡＸ）で回転させ（第１の減速区間）、角速度が０に到達してもそのまま角減速度（−ＡＭＡＸ）で回転させ（第２の減速区間）、角速度が下限値（−ＶＭＡＸ）に達すると、等速の角速度（−ＶＭＡＸ）で回転させ（等速区間）、それから、角加速度（ＡＭＡＸ）で回転させる（加速区間）、ように再計画する。このように、軌道再計画部１０８は、各ジョイント６の角速度波形が時間軸の正負にまたがる（正の三角形＋負の台形）ように動作の再計画を行う（太線）。
【０１１７】
なお、軌道再計画部１０８は、作業点を現在位置（目標位置が変更されたポイント）Ｓから変更後の目標位置Ｇ’まで移動させる距離とその方向に応じて、第１の減速時間Ｔｃ、第２の減速時間Ｔｃ’、等速時間Ｔｂ、加速時間Ｔａ、を決定する。
【０１１８】
例えば、軌道再計画部１０８は、各ジョイント６の角速度波形の面積、すなわち、図示する正の三角形の面積Ｄ’から負の台形の面積ＤＬを差し引いた面積が、先に求めておいた各ジョイント６の回転量Ｄ”と一致するように、第１の減速時間Ｔｃ、第２の減速時間Ｔｃ’、等速時間Ｔｂ、加速時間Ｔａを決定する。
【０１１９】
すなわち、軌道再計画部１０８は、ジョイント６の角加速度（ＡＭＡＸ）、および角減速度（−ＡＭＡＸ）を固定値として、
（１）Ｄ”＝｛Ｖｉ×Ｔｃ／２｝−｛ＶＭＡＸ×（Ｔｃ’＋２×Ｔｂ＋Ｔａ）／２｝、
（２）Ｔｃ＝−Ｖｉ／（−ＡＭＡＸ）、
（３）Ｔｃ’＝−ＶＭＡＸ／（−ＡＭＡＸ）、
（４）Ｔａ＝ＶＭＡＸ／ＡＭＡＸ、
の４式を満たす「第１の減速時間Ｔｃ」、「第２の減速時間Ｔｃ’」、「等速時間Ｔｂ」、「加速時間Ｔａ」を、ジョイント６ごとに求める。
【０１２０】
このように各ジョイント６の動作について再計画すれば、「Ｖｐｅａｋ＜−ＶＭＡＸ」となる場合であっても問題なく再計画でき、上記の再計画方法１〜３と同様の効果が得られる。
【０１２１】
なお、上記の再計画方法１〜４において、ジョイント６の現在の角速度Ｖｉが上限値ＶＭＡＸに一致する場合（すなわち、「Ｖｉ＝ＶＭＡＸ」）には、各数式において「Ｖｉ＝ＶＭＡＸ」として計算すればよい。
【０１２２】
ところで、図８（Ａ）〜（Ｃ）は、第１〜第３のジョイントについての再計画後の動作計画の一例を示す図である。
【０１２３】
上記の再計画方法１〜４のどの方法で再計画した場合であっても、各ジョイント６の総動作時間（図示するＴＳからＴＧ’までの時間）は、それぞれ最短となる。しかし、図示するように、各ジョイント６の動作終了タイミングは不揃いとなるため（図示するＴＧ１’、ＴＧ２’、ＴＧ３’）、各ジョイントの動作を再計画することによって可動部３０全体の動作が滑らかにみえなくなる場合がある。
【０１２４】
そこで、各ジョイントの動作を再計画する場合にも、可動部３０全体の動作を滑らかにみせるために、軌道再計画部１０８は、各ジョイント６の動作終了タイミングが揃うように、動作計画を修正する。
【０１２５】
具体的には、軌道再計画部１０８は、全てのジョイント６のうち、再計画後の動作の終了タイミングが最も遅くなるジョイント６を主ジョイントと決定する。従って、図示する例では、再計画後の第２のジョイント（第２軸）の動作に最も時間がかかるため（ＴＧ２’＞ＴＧ１’、ＴＧ３’）、第２のジョイントを主ジョイントとして決定する。
【０１２６】
それから、軌道再計画部１０８は、主ジョイント以外のジョイント６について、動作の終了タイミングを主ジョイントに合わせる。従って、図示する例では、軌道再計画部１０８は、第１のジョイント（第１軸）の動作終了タイミングＴＧ１’を、主ジョイントの動作終了タイミングＴＧ２’に変更する。また、第３のジョイント（第３軸）の動作終了タイミングＴＧ３’を、主ジョイントの動作終了タイミングＴＧ２’に変更する。
【０１２７】
ただし、このように主ジョイント以外のジョイント６について、動作終了タイミングを変更する場合であっても、作業点を現在位置（すなわち、目標位置が変更されたポイント）Ｓから変更後の目標位置Ｇ’まで移動させるときの、各ジョイント６の回転量Ｄ”については変更するわけにはいかない。
【０１２８】
そこで、軌道再計画部１０８は、各ジョイント６の角速度波形の面積Ｄ”を一定に保ったまま、主ジョイント以外のジョイント６について、動作終了タイミングを変更する。
【０１２９】
図８（Ｄ）は、第１のジョイントについて動作終了タイミングを変更する場合の例を示す図である。また、図８（Ｅ）は、第３のジョイントについて動作終了タイミングを変更する場合の例を示す図である。
【０１３０】
例えば、図８（Ｄ）に示すように、上記の再計画方法１、２によって動作が再計画されたジョイント６については、軌道再計画部１０８は、
（１）Ｄ”＝（Ｖｉ＋ＶＨ）×Ｔｃ／２＋（ＶＨ×Ｔｂ）＋（ＶＨ×Ｔｃ）／２、
（２）−ＡＭＡＸ＝（ＶＨ−Ｖｉ）／Ｔａ、
（３）−ＡＭＡＸ＝−ＶＨ／Ｔｃ’、
（４）Ｔｃ＋Ｔｂ＋Ｔｃ’＝ＴＧ２’
の４式を満たす、「第１の減速時間Ｔｃ」、「等速時間Ｔｂ」、「第２の減速時間Ｔｃ’」、「等加速度ＶＨ」を求める。
【０１３１】
ここで、「ＶＨ」は、ジョイント６が等角速度で回転するときの角速度の値である。
【０１３２】
また、図８（Ｅ）に示すように、上記の再計画方法３、４によって動作が再計画されたジョイント６については、軌道再計画部１０８は、
（１）Ｄ”＝Ｖｉ×Ｔｃ／２−（｜ＶＨ｜×（Ｔｃ’＋２×Ｔｂ＋Ｔａ）／２）
（２）−ＡＭＡＸ＝−Ｖｉ／Ｔｃ、
（３）−ＡＭＡＸ＝−｜ＶＨ｜／Ｔｃ’、
（４）ＡＭＡＸ＝｜ＶＨ｜／Ｔａ、
（５）Ｔｃ＋Ｔｂ＋Ｔｃ’＝ＴＧ２’
の５式を満たす、「第１の減速時間Ｔｃ」、「第２の減速時間Ｔｃ’」、「等速時間Ｔｂ」、「加速時間Ｔａ」、「等加速度ＶＨ」、を求める。
【０１３３】
ここで、「ＶＨ」は、ジョイント６が等加速度で回転するときの角速度の値である。
【０１３４】
このように主ジョイント以外のジョイント６について動作計画を修正することによって、再計画後の動作計画においても、各ジョイント６の動作終了タイミングが揃うようになる（図８（Ｄ）、（Ｅ）に示す「ＴＧ２’」）。そのため、ステップＳ１０７で動作計画が再計画された場合でも、可動部３０全体の動作が滑らかにみえるようになる。
【０１３５】
以上がステップＳ１０７における処理となる。以上のように、可動部３０の軌道を再計画し終えると、軌道再計画部１０８は、処理をステップＳ１０８へ移行する。
【０１３６】
図３に戻り説明する。図示するように、処理がステップＳ１０８へ移行すると、アーム制御部１０７は、ステップＳ１０３で作成された軌道計画の終了時刻（例えば、図４（Ｄ）（Ｅ）に示す主ジョイントの動作終了タイミングＴ１）であるか否か判別する（ステップＳ１０８）。もちろん、ステップＳ１０７で軌道計画が再計画されている場合には、再計画後の終了時刻（例えば、図８（Ｄ）（Ｅ）に示す主ジョイントの動作終了タイミングＴＧ２’）であるか否か判別する。
【０１３７】
ここで、アーム制御部１０７は、終了時刻ではないと判別した場合には（ステップＳ１０８；Ｎｏ）、処理をステップＳ１０４に戻し、未実行の残りの制御を続行する。
【０１３８】
一方、アーム制御部１０７は、終了時刻であると判別した場合には（ステップＳ１０８；Ｙｅｓ）、本フローを終了する。
【０１３９】
以上の制御が、本実施形態にかかるアーム制御タスクである。このように、本実施形態にかかるアーム制御タスクを採用すれば、作業点を目標位置に向けて移動させている途中のどのタイミングでその目標位置が変更された場合であっても、即座に各ジョイント６の動作について再計画できる。
【０１４０】
また、本実施形態にかかるアーム制御タスクでは、加速区間での角加速度、及び、減速区間での角減速度を固定している（ＡＭＡＸ、−ＡＭＡＸ）ため、角加速度や角減速度を可変とする制御を行う場合と比較して、安定した制御となる。
【０１４１】
また、本実施形態にかかるアーム制御タスクでは、可動部３０の目標位置までの距離によらず、軌道計画に従って可動部３０を制御するため、目標位置までの距離に応じて制御速度を変更する場合と比較して、高速に可動部３０を制御できる。
【０１４２】
＜目標位置計算タスク＞
次に、上記の目標位置計算タスク（Ｓ１０２）の詳細について説明する。
【０１４３】
図９は、目標位置計算タスク（Ｓ１０２）の一例について説明するためのフロー図である。
【０１４４】
上述したように、目標位置計算部１０４は、処理がステップＳ１０２に移行するタイミングで、本フローを開始する。
【０１４５】
本フローを開始すると、撮像制御部１０３は、まず、作業点を移動させる目標位置の画像を取得する（ステップＳ２０１）。例えば、撮像制御部１０３は、ユーザーからの指示に基づき、撮像部２０に撮像の指示を出す。このとき、撮像部２０は、ユーザーによって指定された目標位置方向を撮像する。こうして、撮像制御部１０３は、目標位置の画像を取得できる。
【０１４６】
次に、目標位置計算部１０４は、ステップＳ２０１で取得された画像を用いて、作業点の目標位置を計算する（ステップＳ２０２）。例えば、目標位置計算部１０４は、ステップＳ２０１で取得した画像について画像認識を行い、目標位置に指定可能な対象物（或いは、合焦点）を特定する。それから、目標位置計算部１０４は、特定した対象物の位置を、撮像方向との差分から求め、作業点の目標位置とする。
【０１４７】
それから、目標位置計算部１０４は、ステップＳ２０２で計算した作業点の目標位置（例えば、３次元座標）を、目標位置の初期設定、或いは、目標位置の変更指令として、制御部１０へ送信する（ステップＳ２０３）。
【０１４８】
次に、目標位置計算部１０４、上記のアーム制御タスクが終了したか否か判別する（ステップＳ２０４）。
【０１４９】
ここで、目標位置計算部１０４、アーム制御タスクが終了していない場合には（ステップＳ２０４；Ｎｏ）、処理をステップＳ２０１に戻し、アーム制御タスクが実行中である間は、目標位置の変更指示を待機する。
【０１５０】
一方、目標位置計算部１０４、アーム制御タスクが終了した場合には（ステップＳ２０４；Ｎｏ）、本フローを終了する。
【０１５１】
以上の制御が、本実施形態にかかる目標位置計算タスクである。このように、本実施形態にかかる目標位置計算タスクによれば、アーム制御タスクが実行中である間は、随時、作業点の目標位置について変更を受け付けることができる。
【０１５２】
なお、上記した各フローの各処理単位は、ロボット１を理解容易にするために、主な処理内容に応じて分割したものである。処理ステップの分類の仕方やその名称によって、本願発明が制限されることはない。ロボット１が行う処理は、さらに多くの処理ステップに分割することもできる。また、１つの処理ステップが、さらに多くの処理を実行してもよい。
【０１５３】
また、上記の実施形態は、本発明の要旨を例示することを意図し、本発明を限定するものではない。多くの代替物、修正、変形例は当業者にとって明らかである。
【０１５４】
例えば、上記実施形態では、作業点の目標位置について、撮像部２０で撮像された画像を用いて指定している。しかし、本発明は、これに限定されず、どのような方法で目標位置を指定してもよい。例えば、ユーザーが目標位置（例えば、３次元座標）を手入力することにより、作業点の目標位置を指定してもよい。
【０１５５】
また、上記実施形態では、作業点が目標位置まで最短時間で到達するように軌道計画している。しかし、作業点が目標位置に近づくにつれて、可動部３０の移動速度を小さくするように制御してもよい。
【０１５６】
この場合には、例えば、目標位置計算部１０４は、上記のステップＳ２０３において、作業部の目標位置（３次元座標）とともに、作業点と目標位置の距離に応じて変動する速度係数Ｃｖを送信するようにしてもよい。そして、軌道再計画部１０８は、上記ステップＳ１０７の軌道再計画時において、上記ＶＭＡＸにその速度係数Ｃｖを乗算すればよい。
【０１５７】
こうすれば、例えば、作業点が目標位置まで遠いほど、高速に可動部３０を移動させることができ、作業点が目標位置に近づくほど、低速に可動部３０を移動させることができる。
【０１５８】
また、上記実施形態では、各ジョイント６の動作計画において、加速区間での角加速度、減速区間での角減速度を、それぞれ、各ジョイント６が回転可能な最大値としている（ＡＭＡＸ、−ＡＭＡＸ）。しかし、本発明は、各ジョイント６の動作計画において、角加速度と角減速度に固定値を用いればよく、各ジョイント６が回転可能な最大値である必要はない。
【０１５９】
また、ジョイント６ごとに、ＶＭＡＸやＡＭＡＸを異ならせてもよい。
【符号の説明】
【０１６０】
１・・・ロボット、４・・・アーム部、５・・・ハンド部、６・・・ジョイント、７・・・リンク、１０・・・制御部、１１・・・ＣＰＵ、１２・・・ＲＡＭ、１３・・・ＲＯＭ、１４・・・Ｉ／Ｆ、１５・・・通信装置、２０・・・撮像部、３０・・・可動部、３１・・・アクチュエーター、３２・・・センサー、１０１・・・中央制御部、１０２・・・記憶部、１０３・・・撮像制御部、１０４・・・目標位置計算部、１０５・・・軌道計画部、１０６・・・エンコーダー読取部、１０７・・・アーム制御部、１０８・・・軌道再計画部。
【図１】
2013132696.tif 000003
【図２】
2013132696.tif 000004
【図３】
2013132696.tif 000005
【図４】
2013132696.tif 000006
【図５】
2013132696.tif 000007
【図６】
2013132696.tif 000008
【図７】
2013132696.tif 000009
【図８】
2013132696.tif 000010
【図９】
2013132696.tif 000011