(57)【要約】  【課題】ロボットアームの目標状態にかかわらず、ロボットアームの軌道計画に要する時間を、従来よりもなるべく短縮する技術を提供する。  【解決手段】アームの軌道計画を行う軌道計画装置であって、アームについて複数の基準状態を記憶しておく基準記憶部と、アームの作業点の目標状態に応じて、複数の基準状態から１つの基準状態を選択する基準選択部と、選択された基準状態を基準として、作業点を目標状態へ導くための仮ジョイント角を計算する計算部と、当該仮ジョイント角を、実空間上のジョイント角に変換する変換部と、を備える。【選択図】図２
2013146840.tif 000002
【特許請求の範囲】
【請求項１】
アームの軌道計画を行う軌道計画装置であって、
前記アームについて複数の基準状態を記憶しておく基準記憶部と、
前記アームの作業点の目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択部と、
選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算部と、
前記仮ジョイント角を、実空間上のジョイント角に変換する変換部と、を備える、
ことを特徴とする軌道計画装置。
【請求項２】
請求項１に記載の軌道計画装置であって、
前記基準選択部は、
前記作業点の目標状態に最も近い基準状態を選択する、
ことを特徴とする軌道計画装置。
【請求項３】
請求項１又は２に記載の軌道計画装置であって、
前記計算部は、
選択された基準状態に応じて、前記仮ジョイント角の計算に用いるヤコビ行列を異ならせる、
ことを特徴とする軌道計画装置。
【請求項４】
請求項１乃至３のいずれか１項に記載の軌道計画装置であって、
前記複数の基準状態は、
各基準状態におけるアームの作業点の位置が、それぞれ分散している、
ことを特徴とする軌道計画装置。
【請求項５】
請求項１乃至３のいずれか１項に記載の軌道計画装置であって、
前記複数の基準状態は、
各基準状態におけるアームの作業点の位置が、前記目標状態として指定されやすい領域に集中している、
ことを特徴とする軌道計画装置。
【請求項６】
アームの軌道計画を行う軌道計画装置で行われる軌道計画方法であって、
前記軌道計画装置は、
前記アームの複数の基準状態を記憶している基準記憶部を備えており、
前記アームの作業点についての目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択ステップと、
選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算ステップと、
前記仮ジョイント角を、実空間上のジョイント角に変換する変換ステップと、を行う、
ことを特徴とする軌道計画方法。
【請求項７】
アームを備え、前記アームの軌道計画を行うロボットであって、
前記アームについて複数の基準状態を記憶しておく基準記憶部と、
前記アームの作業点の目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択部と、
選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算部と、
前記仮ジョイント角を、実空間上のジョイント角に変換する変換部と、
前記実空間上のジョイント角に基づいて、前記アームを制御するアーム制御部と、を備える、
ことを特徴とするロボット。
【請求項８】
アームを備えるロボットと、前記アームの軌道計画を行う軌道計画装置と、を備えるロボットシステムであって、
前記軌道計画装置は、
前記アームについて複数の基準状態を記憶しておく基準記憶部と、
前記アームの作業点の目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択部と、
選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算部と、
前記仮ジョイント角を、実空間上のジョイント角に変換する変換部と、
前記実空間上のジョイント角に基づいて、前記アームを制御するアーム制御部と、を備える、
ことを特徴とするロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、軌道計画装置、および、軌道計画方法に関する。
【背景技術】
【０００２】
産業用ロボットの中には、ロボットアームを制御して各種作業を行うものがある（例えば、特許文献１）。
【０００３】
ロボットアームの制御では、例えば、
（１）ロボットアームの目標状態（目標位置、目標姿勢）を定める。
（２）現状態から目標状態となるまでのロボットアームの軌道を計画する。
（３）軌道計画に従って、ロボットアームを目標状態まで移動させる。
という（１）〜（３）の動作が行われる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００３−１９６８２号
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、上記（１）で定められたロボットアームの目標状態によっては、ロボットアームの軌道計画（すなわち、上記（２）の処理）に時間がかかる。
【０００６】
本発明は、ロボットアームの目標状態にかかわらず、ロボットアームの軌道計画に要する時間を、従来よりもなるべく短縮する技術を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本願は、上記課題を解決する手段を複数含んでいるが、その例を挙げるならば、以下のとおりである。
【０００８】
上記の課題を解決するための本発明の第一の態様は、アームの軌道計画を行う軌道計画装置であって、前記アームについて複数の基準状態を記憶しておく基準記憶部と、前記アームの作業点の目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択部と、選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算部と、前記仮ジョイント角を、実空間上のジョイント角に変換する変換部と、を備える、ことを特徴とする。
【０００９】
上記の構成によれば、ロボットアームの目標状態にかかわらず、ロボットアームの軌道計画にかかる時間を、従来よりも短縮できる。
【００１０】
また、前記基準選択部は、前記作業点の目標状態に最も近い基準状態を選択する、ようにしてもよい。
【００１１】
また、前記計算部は、選択された基準状態に応じて、前記仮ジョイント角の計算に用いるヤコビ行列を異ならせる、ようにしてもよい。
【００１２】
上記の構成によれば、どのような基準状態が選択された場合であっても、ロボットアームの軌道計画を行うことができる。
【００１３】
また、前記複数の基準状態は、各基準状態におけるアームの作業点の位置が、それぞれ分散している、ようにしてもよい。
【００１４】
上記の構成によれば、目標状態がどこに指定された場合であっても、軌道計画時に行われる反復計算の回数を減らせる確率が高まる。
【００１５】
また、前記複数の基準状態は、各基準状態におけるアームの作業点の位置が、前記目標状態として指定されやすい領域に集中している、ようにしてもよい。
【００１６】
上記の構成によれば、各基準状態におけるアームの作業点の位置を分散させる場合と比較して、軌道計画時に行われる反復計算の回数を大幅に減らせる確率が高まる。
【００１７】
上記の課題を解決するための本発明の第二の態様は、アームの軌道計画を行う軌道計画装置で行われる軌道計画方法であって、前記軌道計画装置は、前記アームの複数の基準状態を記憶している基準記憶部を備えており、前記アームの作業点についての目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択ステップと、選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算ステップと、前記仮ジョイント角を、実空間上のジョイント角に変換する変換ステップと、を行う、ことを特徴とする。
【００１８】
上記の本発明の第二の態様によっても、ロボットアームの目標状態にかかわらず、ロボットアームの軌道計画にかかる時間を、従来よりも短縮できる。
【００１９】
上記の課題を解決するための本発明の第三の態様は、アームを備え、前記アームの軌道計画を行うロボットであって、前記アームについて複数の基準状態を記憶しておく基準記憶部と、前記アームの作業点の目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択部と、選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算部と、前記仮ジョイント角を、実空間上のジョイント角に変換する変換部と、前記実空間上のジョイント角に基づいて、前記アームを制御するアーム制御部と、を備える、ことを特徴とする。
【００２０】
上記の課題を解決するための本発明の第四の態様は、アームを備えるロボットと、前記アームの軌道計画を行う軌道計画装置と、を備えるロボットシステムであって、前記軌道計画装置は、前記アームについて複数の基準状態を記憶しておく基準記憶部と、前記アームの作業点の目標状態に応じて、前記複数の基準状態から１つの基準状態を選択する基準選択部と、選択された前記基準状態を基準として、前記作業点を前記目標状態へ導くための仮ジョイント角を計算する計算部と、前記仮ジョイント角を、実空間上のジョイント角に変換する変換部と、前記実空間上のジョイント角に基づいて、前記アームを制御するアーム制御部と、を備える、ことを特徴とする。
【００２１】
上記した以外の課題、構成、及び効果は、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【００２２】
【図１】ロボット１の外観例を示す図である。
【図２】ロボット１の概略構成の一例を示すブロック図である。
【図３】アーム制御タスクの一例について説明するためのフロー図である。
【図４】（Ａ）可動部３０の現状態の一例を示す図である。（Ｂ）作業点の目標状態の一例を示す図である。
【図５】可動部３０の基準となる状態（初期基準）の一例を示す図である。
【図６】（Ａ）可動部３０の第１の仮基準となる状態（第１の仮基準）の一例を示す図である。（Ｂ）可動部３０の第２の仮基準となる状態（第２の仮基準）の一例を示す図である。（Ｃ）可動部３０の第３の仮基準となる状態（第３の仮基準）の一例を示す図である。（Ｄ）可動部３０の第４の仮基準となる状態（第４の仮基準）の一例を示す図である。（Ｅ）可動部３０の第５の仮基準となる状態（第５の仮基準）の一例を示す図である。
【図７】（Ａ）可動部３０の現状態を再現する例を示す図である。（Ｂ）反復計算により求まる可動部３０の目標状態の一例を示す図である。
【発明を実施するための形態】
【００２３】
以下、本発明の実施形態について図面を用いて説明する。
【００２４】
図１は、本実施形態にかかるロボット１についての外観例を示す図である。
【００２５】
図示するように、ロボット１は、例えば、可動部３０と、撮像部２０と、制御部１０と、を備えるアーム型のロボットである。
【００２６】
可動部３０は、ロボット１の可動部分である。図示する例では、可動部３０として、１本のロボットアームを示しており、点線で囲まれたアーム部４と、アーム部４の先端に取り付けられたハンド部５と、を含む。なお、いわゆる双腕ロボットのように２本のロボットアームを有していてもよい。
【００２７】
アーム部４は、複数のジョイント（関節）６と、複数のリンク７と、を含む。
【００２８】
各ジョイント６は、リンク７同士や、ロボットの台座（胴体）とリンク７、などを、回動自在に（ただし、所定の可動範囲内で回動可能に）連結している。本図の例では、アーム部４は、３つのジョイント（第１ジョイントＪ１、第２ジョイントＪ２、第３ジョイントＪ３）を備えた３軸アームとなっている。また、図示するように、各リンク７を、第１リンクＬ１、第２リンクＬ２、第３リンクＬ３等と名付ける。
【００２９】
ハンド部５は、例えば、複数の指を備え、隣り合う指を近付ける方向に移動させ、少なくとも２本の指で把持対象物を挟むことができる。なお、本実施形態では、ハンド部５の位置を、可動部３０の「作業点」とよぶ（いわゆる、エンドエフェクター）。
【００３０】
また、可動部３０（具体的には、各ジョイント６やハンド部５）には、それらを動作させるための、例えば、アクチュエーター３１（図２に示す）が備えられる。アクチュエーター３１は、例えば、サーボモーターやエンコーダーなどを備える。エンコーダーが出力するエンコーダー値は、制御部１０によるロボット１のフィードバック制御に使用される。
【００３１】
また、可動部３０には、ハンド部５などにセンサー３２（図２に示す）を備えている。センサー３２は、例えば、可動部３０に加えられた外力などを検出する。
【００３２】
撮像部２０は、ロボット１の外部環境（例えば、図示する作業台付近）を撮影して、画像データを生成するユニットである。撮像部２０は、例えば、カメラを含み、作業台、天井、壁などに設けられる。撮像部２０は、制御部１０の制御により、可動部３０の作業点についての目標位置方向の撮影を行う。これに伴い、撮像部２０は、画像データを生成し、制御部１０に出力する。
【００３３】
制御部１０は、ロボット１の全体を制御する処理を行う。制御部１０は、例えば、ロボット１の本体（可動部３０）に内蔵しておいてもよいし、図示するように、ロボット１の本体とは離れた場所に（遠隔操作可能に）配置しておいてもよい。なお、ロボットの態様は、上記に限られない。例えば、図１の可動部３０を「ロボット」と呼び、図１のロボット１を「ロボットシステム」と呼ぶこともできる。この場合も、制御部１０は、ロボットに内蔵されていてもいなくてもよい。また、撮像部２０は、ロボットに内蔵されていてもいなくてもよい。
【００３４】
図２は、ロボット１の概略構成の一例を示すブロック図である。図示するように、制御部１０は、演算装置であるＣＰＵ１１と、揮発性の記憶装置であるＲＡＭ１２と、不揮発性の記憶装置であるＲＯＭ１３と、制御部１０と他のユニットを接続するインターフェイス（Ｉ／Ｆ）回路１４と、ロボット１の外部の装置と通信を行う通信装置１５と、を備える。
【００３５】
次に、制御部１０の機能構成例について説明する。図２の下部には、制御部１０についての機能構成例が示されている。
【００３６】
図示するように、制御部１０は、中央制御部１０１と、記憶部１０２と、撮像制御部１０３と、軌道計画部１０４と、エンコーダー読取部１０５と、アーム制御部１０６と、を備える。
【００３７】
中央制御部１０１は、他の各部（１０２〜１０６）を統括的に制御する。
【００３８】
記憶部１０２は、各種データやプログラムを記憶する。例えば、記憶部１０２は、可動部３０についての軌道計画を行うプログラム、その軌道計画に従って可動部３０を制御するプログラム等を記憶する。
【００３９】
また、記憶部１０２は、可動部３０の基準となる状態（以下では「初期基準」という）を特定する情報を記憶している。また、記憶部１０２は、可動部３０の仮の基準となる状態（以下では「仮基準」という）を特定する情報を、複数個（後述する第１の仮基準〜第５の仮基準）、記憶している。なお、仮の基準となる状態を特定する情報は、可動部３０についての軌道計画を従来よりも高速に行うために必要となる。初期基準、仮基準の詳細については、後述する。
【００４０】
撮像制御部１０３は、撮像部２０を制御する。例えば、撮像制御部１０３は、撮像部２０に対して、作業点の目標位置方向を撮影させる。そして、撮像制御部１０３は、撮像部２０で撮影された画像データを取得する。
【００４１】
軌道計画部１０４は、可動部３０の作業点を現状態（位置と姿勢）から目標状態（位置と姿勢）へ導くときの、可動部３０についての軌道を計画する。
【００４２】
具体的には、軌道計画部１０４は、作業点を上記の目標状態へ導くのに必要な各ジョイント６の総回転角、および、各ジョイント６の単位時間（例えば、１ｍｓ）ごとの回転角を求める。
【００４３】
このとき、軌道計画部１０４は、上記の複数の仮基準の中から選択された１つの仮基準を基準座標として、各ジョイント６の回転角（総回転角と、所定単位時間ごとの回転角を含み、以下では「仮のジョイント角」ともいう）を求める。
【００４４】
なお、ここで選択される仮基準は、可動部３０の作業点の状態（位置、姿勢）が、目標状態（位置、姿勢）に最も近いものとする。
【００４５】
また、軌道計画部１０４は、求めた仮のジョイント角を、実空間上のジョイント角（すなわち、上記の初期基準を基準座標とした角度）に変換することによって、実際に可動部３０の作業点を現状態から目標状態へ導くのに必要な各ジョイント６の回転角を求める。
【００４６】
以上のように各ジョイント６の回転角を求めることにより、計算量を従来よりも（例えば、上記の初期基準を基準座標として求める場合と比較して）減らすことができる。その結果、従来よりも高速に軌道計画ができる。
【００４７】
なお、本実施形態において、作業点の「現状態」には、少なくとも作業点の「現在の位置」と「現在の姿勢」が含まれる。同様に、作業点の「目標状態」には、少なくとも作業点の「目標とする位置」と「目標とする姿勢」が含まれる。
【００４８】
エンコーダー読取部１０５は、アーム部４の各ジョイント６に設けられたアクチュエーター３１のエンコーダー値を読み取る。
【００４９】
アーム制御部１０６は、軌道計画部１０４による軌道計画に従って、作業点を目標状態へ導く制御を行う。すなわち、アーム制御部１０６は、軌道計画部１０４により求められた単位時間ごとの回転角（ただし、初期基準を基準座標とする）に従って、各ジョイント６を回転させる。
【００５０】
なお、アーム制御部１０６は、アクチュエーター３１のエンコーダー値や、センサー３２のセンサー値などを参照して、可動部３０を制御する。これにより、軌道の誤差を修正することができる。
【００５１】
上記の記憶部１０２以外の各機能部は、例えば、ＣＰＵ１１がＲＯＭ１３に格納された所定のプログラムをＲＡＭ１２に読み出して実行することにより実現される。記憶部１０２は、例えば、ＲＡＭ１２やＲＯＭ１３により実現される。なお、前記所定のプログラムは、例えば、予めＲＯＭ１３にインストールされてもよいし、通信装置１５を介してネットワークからダウンロードされてインストール又は更新されてもよい。
【００５２】
以上のロボット１の構成は、本実施形態の特徴を説明するにあたって主要構成を説明したのであって、上記の構成に限られない。可動部３０及び制御部１０を有していれば、どのような構造であってもよい。また、一般的なロボットが備える構成を排除するものではない。
【００５３】
例えば、図１にはアーム部４として３軸アームが示されているが、軸数（ジョイント数）をさらに増加させてもよいし減らしてもよい。リンクの数を増減させてもよい。また、アーム部４、ハンド部５、ジョイント６、リンク７等の各種部材の形状、大きさ、配置、構造等も適宜変更してよい。
【００５４】
また、上記した制御部１０の各機能構成は、制御部１０の構成を理解容易にするために、主な処理内容に応じて分類したものである。構成要素の分類の仕方や名称によって、本願発明が制限されることはない。制御部１０の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。また、各構成要素の処理は、１つのハードウェアで実行されてもよいし、複数のハードウェアで実行されてもよい。
【００５５】
次に、本実施形態における、上記構成からなるロボット１の特徴的な動作について説明する。
【００５６】
＜アーム制御タスク＞
アーム部４を制御することによって、可動部３０の作業点を目標状態まで導くタスクを、以下では「アーム制御タスク」とよぶ。
【００５７】
図３は、アーム制御タスクの一例について説明するためのフロー図である。
【００５８】
中央制御部１０１は、ユーザーからの指示に基づいて、アーム制御タスクを開始する。
【００５９】
アーム制御タスクが開始されると、まず、軌道計画部１０４は、可動部３０の現状態（位置、姿勢）を特定する（ステップＳ１０１）。
【００６０】
具体的には、ステップＳ１０１において、軌道計画部１０４は、まず、各ジョイント６に設けられたアクチュエーター３１のエンコーダー値を読み取る。それから、軌道計画部１０４は、読み取ったエンコーダー値に相当する各ジョイント６のジョイント角から、作業点の現状態（位置、姿勢）を求める（順運動学等の手法によって求める）。
【００６１】
図４（Ａ）は、可動部３０の現状態の一例を示す図である。図示するように、ステップＳ１０１において、軌道計画部１０４は、第１ジョイントＪ１のジョイント角θｃ１、第２ジョイントＪ２のジョイント角θｃ２、第３ジョイントＪ３のジョイント角θｃ３に基づき、作業点の現状態Ｍ０を求める。ただし、作業点の現状態Ｍ０には、現在の位置（Ｘｃ，Ｙｃ）と、現在の姿勢（φｃ）と、が含まれている。
【００６２】
ただし、各ジョイント角（θｃ１、θｃ２、θｃ３）、作業点の現在の位置（Ｘｃ，Ｙｃ）、作業点の現在の姿勢（φｃ）は、それぞれ、実空間上での座標や角度を表すものであり、具体的には上記の初期基準を基準座標としている。
【００６３】
ここで、Ｘ軸とＹ軸は、図示するように互いに直交する軸である。そして、Ｘ軸とＹ軸で張られる平面を、例えば、地面に平行な平面とする。
【００６４】
また、本実施形態では、説明を簡単にするため２次元空間を用いているが、Ｘ軸、Ｙ軸、Ｚ軸で張られる３次元空間であってもよい。
【００６５】
ところで、図５は、可動部３０の基準となる状態（すなわち、初期基準）の一例を示す図である。図示するように、本実施形態では、可動部３０の初期基準を、各リンク７（Ｌ１、Ｌ２、Ｌ３）がＸ軸の正方向に直線となる状態とする。このような初期基準の状態において、各ジョイント６のジョイント角を「０」とし、作業点の位置、姿勢は、それぞれ「（Ｘ，Ｙ）＝（Ｌ１＋Ｌ２＋Ｌ３，０）」、「φ＝０」となる。
【００６６】
なお、可動部３０の初期基準となる状態は、図５に示すものに限定されない。例えば、可動部３０の初期基準を、各リンク７（Ｌ１、Ｌ２、Ｌ３）がＹ軸の正方向に直線となる状態としてもよい。
【００６７】
ステップＳ１０１で作業点の現状態（位置、姿勢）が求められると、軌道計画部１０４は、次に、作業点の目標状態を決定する（ステップＳ１０２）。
【００６８】
具体的には、ステップＳ１０２において、起動計画部１０４は、撮像部２０によって撮像された目標位置方向の画像を取得する。そして、起動計画部１０４は、当該画像について画像認識を行い、目標位置として指定可能な対象物（或いは、合焦点）を特定する。それから、起動計画部１０４は、特定した対象物の位置や角度に基づき、作業点の目標状態（位置、姿勢）を求める。
【００６９】
ただし、ステップＳ１０２の処理は、上記のような方法によらず、ユーザーの入力によって、作業点の目標状態（位置、姿勢）を決定してもよい。
【００７０】
また、ステップＳ１０２で決定される作業点の目標状態、すなわち、作業点の目標位置（Ｘａ，Ｙａ）、作業点の目標姿勢（φａ）についても、それぞれ、実空間上での座標や角度を表すものであり、具体的には上記の初期基準を基準座標としている。
【００７１】
図４（Ｂ）は、作業点の目標状態の一例を示す図である。図示するように、ステップＳ１０２において、作業点の目標状態が決定される。
【００７２】
上記のように現状態と目標状態が決定すると、起動計画部１０４は、可動部３０を現状態から目標状態へ導くための軌道計画をたてる。
【００７３】
そこで、軌道計画部１０４は、
（１）  Θ（ｋ＋１）＝Θ（ｋ）＋Ｊ（Θ（ｋ））−１×（Ｍａ−Ｍ（ｋ））
という漸化式を用いてジョイント６ごとに動作計画をたてる。
【００７４】
ここで、Θ（ｋ）は、中継点ｋでのジョイント角（上述した仮のジョイント角）を表している。軌道計画部１０４は、ｋをインクリメントしながら当該（１）の漸化式を反復計算していくことにより、各ジョイント６の仮のジョイント角Θ（ｋ＋１）を求めることができる。
【００７５】
なお、Ｊ（Θ（ｋ））−１は、ヤコビ行列の疑似逆行列である。また、Ｍａは、ステップＳ１０２で決定された作業点の目標状態（目標位置でもよい）であり、Ｍ（ｋ）は、中継点ｋでの作業点の状態（位置でもよい）である。ただし、ｋは０以上の整数とする。
【００７６】
ところで、従来の技術では、上記（１）の漸化式は、上述した初期基準の状態（図５）などを基準として反復計算されるのが一般的である。
【００７７】
しかし、作業点の目標状態によっては、その反復計算に時間がかかってしまう場合がある。
【００７８】
そこで、本実施形態では、反復計算にかかる時間を、従来よりも短縮すべく、軌道計画部１０４は、ステップＳ１０２で決定された作業点の目標状態に応じて、仮基準となる状態を選択し（ステップＳ１０３）、選択した仮基準の状態を基準として（ステップＳ１０４）、反復計算を行う（ステップＳ１０５）。
【００７９】
なお、ステップＳ１０３で選択可能な仮基準の状態は、上述した通り、複数個、用意されている。
【００８０】
図６（Ａ）〜（Ｅ）は、可動部３０の第１〜第５の仮基準となる状態の一例を示す図である。
【００８１】
図６（Ａ）に示すように、本実施形態では、可動部３０の第１の仮基準を、各リンク７（Ｌ１、Ｌ２、Ｌ３）がＸ軸の正方向に直線となる状態とする。このような第１の仮基準の状態において、作業点の位置、姿勢は、それぞれ「（Ｘｓ，Ｙｓ）＝（Ｌ１＋Ｌ２＋Ｌ３，０）」、「φｓ＝０」となる。また、各ジョイント６のジョイント角は、「（θｓ１、θｓ２、θｓ３）＝（０，０，０）」となる。
【００８２】
また、図６（Ｂ）に示すように、本実施形態では、可動部３０の第２の仮基準を、各リンク７（Ｌ１、Ｌ２、Ｌ３）がＹ軸の正方向に直線となる状態とする。このような第２の仮基準の状態において、作業点の位置、姿勢は、それぞれ「（Ｘｓ，Ｙｓ）＝（０，Ｌ１＋Ｌ２＋Ｌ３）」、「φｓ＝π／２」となる。また、各ジョイント６のジョイント角は、「（θｓ１、θｓ２、θｓ３）＝（π／２，０，０）」となる。
【００８３】
また、図６（Ｃ）に示すように、本実施形態では、可動部３０の第３の仮基準を、各リンク７（Ｌ１、Ｌ２、Ｌ３）がＸ軸の負方向に直線となる状態とする。このような第３の仮基準の状態において、作業点の位置、姿勢は、それぞれ「（Ｘｓ，Ｙｓ）＝（−Ｌ１−Ｌ２−Ｌ３，０）」、「φｓ＝π」となる。また、各ジョイント６のジョイント角は、「（θｓ１、θｓ２、θｓ３）＝（π，０，０）」となる。
【００８４】
また、図６（Ｄ）に示すように、本実施形態では、可動部３０の第４の仮基準を、各リンク７（Ｌ１、Ｌ２、Ｌ３）がＹ軸の負方向に直線となる状態とする。このような第４の仮基準の状態において、作業点の位置、姿勢は、それぞれ「（Ｘｓ，Ｙｓ）＝（０，−Ｌ１−Ｌ２−Ｌ３）」、「φｓ＝−π／２」となる。また、各ジョイント６のジョイント角は、「（θｓ１、θｓ２、θｓ３）＝（−π／２，０，０）」となる。
【００８５】
また、図６（Ｅ）に示すように、本実施形態では、可動部３０の第５の仮基準を、各リンク７（Ｌ１、Ｌ２、Ｌ３）が折れ線となる状態とする。このような第５の仮基準の状態において、作業点の位置、姿勢は、それぞれ「（Ｘｓ，Ｙｓ）＝（Ｌ２，Ｌ１＋Ｌ３）」、「φｓ＝π／２」となる。また、各ジョイント６のジョイント角は、「（θｓ１、θｓ２、θｓ３）＝（π／２，−π／２，π／２）」となる。
【００８６】
ところで、ステップＳ１０３の具体的な処理として、例えば、軌道計画部１０４は、ステップＳ１０２で決定された目標状態（Ｘａ、Ｙａ、φａ）に最も近い仮基準の状態を選択すればよい。
【００８７】
より詳細に言えば、軌道計画部１０４は、ステップＳ１０３において、
（２）  Δ＝（Ｘａ−Ｘｓ）２＋（Ｙａ−Ｙｓ）２＋（φａ−φｓ）２
を最小にする仮基準の状態を、上記複数の仮基準の中から選択すればよい。
【００８８】
そして、ステップＳ１０４では、軌道計画部１０４は、ステップＳ１０３で選択された仮基準を基準とする座標系で適切なジョイント角分だけオフセットすることで、ステップＳ１０１で特定された可動部３０の現状態を再現する。
【００８９】
具体的には、軌道計画部１０４は、ステップＳ１０１で特定された現在の各ジョイント角（θｃ１、θｃ２、θｃ３）から、ステップＳ１０３で選択された仮基準の状態を実現する各ジョイント角（θｓ１、θｓ２、θｓ３）を減算した値をオフセット値とする。すなわち、仮基準を基準とした場合の現状態を再現するジョイント角（Θ１０、Θ２０、Θ３０）は、（θｃ１−θｓ１、θｃ２−θｓ２、θｃ３−θｓ３）となる。
【００９０】
図７（Ａ）は、可動部３０の現状態をオフセットする場合について示す図である。図示する例では、第５の仮基準（点線内）を基準座標として適切なジョイント角分だけオフセットすることで、図４（Ａ）に示す現状態の可動部３０を実現している。第５の仮基準を実現する状態に対して、ステップＳ１０１で特定された現在の各ジョイント角（θｃ１、θｃ２、θｃ３）から、ステップＳ１０３で選択された仮基準の状態を実現する各ジョイント角（θｓ１、θｓ２、θｓ３）を減算することで求まる各ジョイント角（Θ１０、Θ２０、Θ３０）（＝（θｃ１−θｓ１、θｃ２−θｓ２、θｃ３−θｓ３））を加えることにより、第５の仮基準を基準座標とした場合において、現状態を再現する。
【００９１】
それから、ステップＳ１０５では、軌道計画部１０４は、ここで求めた各ジョイント角（Θ１０、Θ２０、Θ３０）と、ステップＳ１０１で求めた現状態Ｍ０とを、上記（１）の漸化式の初期値（ｋ＝０の場合）として代入し、中継点ｋごとに反復計算を行う。
【００９２】
なお、軌道計画部１０４は、ステップＳ１０３で選択された仮基準の状態に応じて、上記（１）の反復計算に用いるヤコビ行列を異ならせる。
【００９３】
例えば、第１の仮基準が選択された場合には、軌道計画部１０４は、以下の行列式１をヤコビ行列Ｊとして反復計算を行う。
【００９４】
【数１】
2013146840.tif 000003
2013146840.tif 000004
【００９５】
また、第２の仮基準が選択された場合には、軌道計画部１０４は、以下の行列式２をヤコビ行列Ｊとして反復計算を行う。
【００９６】
【数２】
2013146840.tif 000005
【００９７】
また、第３の仮基準が選択された場合には、軌道計画部１０４は、以下の行列式３をヤコビ行列Ｊとして反復計算を行う。
【００９８】
【数３】
2013146840.tif 000006
【００９９】
また、第４の仮基準が選択された場合には、軌道計画部１０４は、以下の行列式４をヤコビ行列Ｊとして反復計算を行う。
【０１００】
【数４】
2013146840.tif 000007
【０１０１】
また、第５の仮基準が選択された場合には、軌道計画部１０４は、以下の行列式５をヤコビ行列Ｊとして反復計算を行う。
【０１０２】
【数５】
2013146840.tif 000008
【０１０３】
以上のようなヤコビ行列Ｊを用意しておくことにより、ステップＳ１０３でどの仮基準が選択された場合であっても、上記（１）の漸化式を計算することができる。
【０１０４】
また、上記（１）の（Ｍａ−Ｍ（ｋ））の項は、作業点の目標状態Ｍａと、中継点ｋでの状態Ｍ（ｋ）と、の距離（「近さ」ともいう）を表している。
【０１０５】
そして、軌道計画部１０４は、この距離が所定の閾値以下となったときに、作業点が目標状態に収束したものと判断する（ステップＳ１０６）。
【０１０６】
具体的には、軌道計画部１０４は、
（３）  （Ｍａ−Ｍ（ｋ））２≦Ｅｒｒ
という数式を満たす場合に、作業点が目標状態に収束したものと判定する。ただし、Ｅｒｒは所定の定数とする。
【０１０７】
ここで、作業点が目標状態に収束していない場合には（ステップＳ１０６；Ｎｏ）、軌道計画部１０４は、中継点ｋをインクリメントして、処理をステップＳ１０５に戻す。
【０１０８】
以上のように、軌道計画部１０４は、ステップＳ１０５を繰り返すことで、上記（３）の数式（条件）を満たすようになるまで、数式（１）の漸化式を反復計算する。これにより、軌道計画部１０４は、所定単位時間ごと（すなわち、中継点ｋごと）に、各ジョイント６の仮のジョイント角（Θ１（ｋ＋１）、Θ２（ｋ＋１）、Θ３（ｋ＋１））を求めることができる。
【０１０９】
図７（Ｂ）は、反復計算により求まる可動部３０の目標状態の一例を示す図である。図示するように、各ジョイント６の仮のジョイント角（Θ１（ｋ＋１）、Θ２（ｋ＋１）、Θ３（ｋ＋１））は、比較的小さい角度となる。
【０１１０】
以上のように、作業点が目標状態に収束し（ステップＳ１０６；Ｙｅｓ）、仮のジョイント角（Θ１（ｋ＋１）、Θ２（ｋ＋１）、Θ３（ｋ＋１））が求まると、軌道計画部１０４は、処理をステップＳ１０７へ移行する。
【０１１１】
ところで、ここで求まる仮のジョイント角（Θ１（ｋ＋１）、Θ２（ｋ＋１）、Θ３（ｋ＋１））は、ステップＳ１０３で選択された仮基準の状態を基準とした角度である。
【０１１２】
そのため、軌道計画部１０４は、求めた仮のジョイント角を、実空間上のジョイント角（すなわち、上記の初期基準を基準座標とした角度）に変換する。これにより、軌道計画部１０４は、実際に可動部３０の作業点を現状態から中継点（ｋ＋１）まで導くのに必要な各ジョイント６の回転角θを求める（ステップＳ１０７）。
【０１１３】
具体的には、軌道計画部１０４は、ステップＳ１０５の反復計算により求まる、所定単位時間ごと（中継点ｋごと）の仮のジョイント角（Θ１（ｋ＋１）、Θ２（ｋ＋１）、Θ３（ｋ＋１））に、ステップＳ１０３で選択された仮基準の状態を実現する各ジョイント角（θｓ１、θｓ２、θｓ３）を加算すればよい。すなわち、作業点を現状態から中継点ｋまで導くのに必要な各ジョイント６の回転角θを、「（θ１（ｋ＋１），θ２（ｋ＋１），θ３（ｋ＋１））＝（Θ１（ｋ＋１）、Θ２（ｋ＋１）、Θ３（ｋ＋１））＋（θｓ１、θｓ２、θｓ３）」の計算式により求めることができる。
【０１１４】
また、中継点ｋ＋１を、作業点が目標状態に収束（到達）したときの値とすれば、実際に可動部３０の作業点を現状態から目標状態まで導くのに必要な各ジョイント６の回転角θａを求めることができる。すなわち、軌道計画部１０４は、「（θａ１，θａ２，θａ３）＝（Θ１（ｋ＋１）、Θ２（ｋ＋１）、Θ３（ｋ＋１））＋（θｓ１、θｓ２、θｓ３）」の計算式により回転角θａを求めることができる（ただし、ｋ＋１は作業点が目標状態に収束したときの値）。
【０１１５】
以上のように、各ジョイント６の動作計画がたてられると、次に、アーム制御部１０６は、その動作計画に従って、作業点を目標状態へ導く制御を行う（ステップＳ１０８）。すなわち、アーム制御部１０６は、軌道計画部１０４により求められた単位時間ごとの回転角θ（初期基準）に従って、各ジョイント６を回転させる。
【０１１６】
そして、作業点が目標状態まで導かれると、アーム制御部１０６は、本フロー（アーム制御タスク）を終了する。
【０１１７】
以上の処理が、本実施形態にかかるアーム制御タスクである。このように、本実施形態にかかるアーム制御タスクによれば、可動部３０（ロボットアーム）の目標状態にかかわらず、軌道計画に要する時間を、従来よりも短縮できる。
【０１１８】
なお、上記した各フローの各処理単位は、ロボット１を理解容易にするために、主な処理内容に応じて分割したものである。処理ステップの分類の仕方やその名称によって、本願発明が制限されることはない。ロボット１が行う処理は、さらに多くの処理ステップに分割することもできる。また、１つの処理ステップが、さらに多くの処理を実行してもよい。
【０１１９】
また、上記の実施形態は、本発明の要旨を例示することを意図し、本発明を限定するものではない。多くの代替物、修正、変形例は当業者にとって明らかである。
【０１２０】
例えば、上記実施形態では、５種類の仮基準（第１〜第５の仮基準）を用意している。しかし、本発明は、これに限定されず、さらに増加させてもよいし減らしてもよい。
【０１２１】
また、上記実施形態で示した第１〜第４の仮基準は、作業点の位置がそれぞれ分散している。このように作業点の位置が分散するように複数の仮基準を用意しておくことにより、目標状態がどこに指定（決定）された場合であっても、上記（１）の漸化式の反復回数を減らせる確率が高まる。
【０１２２】
また、上記実施形態に限定されず、目標状態として指定されやすい領域に作業点が集中するように、複数の仮基準を用意しておいてもよい。例えば、可動部３０の作業点が到達できる範囲の空間を、複数の小領域に分割し、過去に目標状態として指定された回数が（最も）多い小領域に、各仮基準の作業点の位置が集中するようにしておく。これにより、各仮基準の作業点の位置を分散させる場合と比較して、上記（１）の漸化式の反復回数を大幅に減らせる確率が高まる。
【符号の説明】
【０１２３】
１・・・ロボット、４・・・アーム部、５・・・ハンド部、６・・・ジョイント、７・・・リンク、１０・・・制御部、１１・・・ＣＰＵ、１２・・・ＲＡＭ、１３・・・ＲＯＭ、１４・・・Ｉ／Ｆ、１５・・・通信装置、２０・・・撮像部、３０・・・可動部、１０１・・・中央制御部、１０２・・・記憶部、１０３・・・撮像制御部、１０４・・・軌道計画部、１０５・・・エンコーダー読取部、１０６・・・アーム制御部。
【図１】
2013146840.tif 000009
【図２】
2013146840.tif 000010
【図３】
2013146840.tif 000011
【図４】
2013146840.tif 000012
【図５】
2013146840.tif 000013
【図６】
2013146840.tif 000014
【図７】
2013146840.tif 000015