JP2016198855A - ロボット、制御装置、及び制御方法 - Google Patents

Abstract

【課題】他の物体との干渉の抑制及び第１物体を移動させる時間の短縮を図ることができるロボットを提供すること。【解決手段】ロボットは、アームと、アームを動作させる制御部と、を含み、制御部は、アームとともに動く第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づいて経路を生成する。【選択図】図４

Description

この発明は、ロボット、制御装置、及び制御方法に関する。

ロボットに物体同士の組み付けを行わせる技術の研究や開発が行われている。

これに関し、部品把持手段を備える組立機構を用いて複数の部品を順次組みつけて所定の組立対象物を構成する自動組立システムが知られている（特許文献１参照）。

特開２００６−３５０６２０号公報

しかしながら、この自動組立システムは、部品を移動させる速さを部品同士の相対的な位置及び姿勢に基づいて変更することができず、組み立て時間の短縮を図ることが困難であった。また、この結果により、部品同士を組み立てる動作を含む作業に要する時間が長くなってしまう場合があった。

上記課題の少なくとも一つを解決するために本発明の一態様は、アームと、前記アームを動作させる制御部と、を含み、前記制御部は、前記アームとともに動く第１物体と第２物体との相対位置姿勢、及び前記第１物体の第１位置に基づいて経路を生成する、ロボットである。
この構成により、ロボットは、アームとともに動く第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づいて経路を生成する。これにより、ロボットは、第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づく経路に沿って第１物体を移動させることができる。その結果、ロボットは、例えば、他の物体との干渉の抑制及び第１物体を移動させる時間の短縮を図ることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、前記相対位置姿勢に基づく前記第１物体の移動可能な範囲を用いて前記経路を生成する、構成が用いられてもよい。
この構成により、ロボットは、第１物体と第２物体との相対位置姿勢に基づく第１物体の移動可能な範囲を用いて経路を生成する。これにより、ロボットは、第１物体の移動可能な範囲を用いて生成した経路に沿って第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、前記移動可能な範囲に基づいて、前記相対位置姿勢に応じた前記第１物体の動かし難さ又は動かし易さの指標となる量を算出し、算出された前記指標となる量を用いて前記経路を生成する、構成が用いられてもよい。
この構成により、ロボットは、第１物体の移動可能な範囲に基づいて、第１物体と第２物体との相対位置姿勢に応じた第１物体の動かし難さ又は動かし易さの指標となる量を算出し、算出された指標となる量を用いて経路を生成する。これにより、ロボットは、第１物体の動かし難さ又は動かし易さの指標となる量を用いて生成した経路に沿って第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、前記第１位置と前記第１物体の第２位置とを用いて前記経路を生成する、構成が用いられてもよい。
この構成により、ロボットは、第１物体の第１位置と第１物体の第２位置とを用いて経路を生成する。これにより、ロボットは、第１物体の第１位置と第１物体の第２位置とを用いて生成した経路に沿って第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、更に前記第１位置に対応する第１姿勢と、前記第２位置に対応する第２姿勢とを用いて前記経路を生成する、構成が用いられてもよい。
この構成により、ロボットは、更に第１物体の第１位置に対応する第１姿勢と、第１物体の第２位置に対応する第２姿勢とを用いて経路を生成する。これにより、ロボットは、第１物体の第１位置に対応する第１姿勢と、第１物体の第２位置に対応する第２姿勢とを用いて生成した経路に沿って第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、前記相対位置姿勢と前記所定位置に基づいて前記アームを移動させる速さを決定する、構成が用いられてもよい。
この構成により、ロボットは、第１物体と第２物体との相対位置姿勢と第１物体の第１位置に基づいてアームを移動させる速さを決定する。これにより、ロボットは、第１物体と第２物体との相対位置姿勢と第１物体の第１位置に基づいて決定した速さによって第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、前記相対位置姿勢に基づく前記第１物体の移動可能な範囲を用いて前記速さを決定する、構成が用いられてもよい。
この構成により、ロボットは、第１物体と第２物体との相対位置姿勢に基づく第１物体の移動可能な範囲を用いてアームを移動させる速さを決定する。これにより、ロボットは、第１物体の移動可能な範囲を用いて決定した速さによって第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、前記相対位置姿勢に応じた前記第１物体の動かし難さ又は動かし易さの指標となる量を算出し、算出された前記指標となる量を用いて前記速さを決定する、構成が用いられてもよい。
この構成により、ロボットは、第１物体と第２物体との相対位置姿勢に応じた第１物体の動かし難さ又は動かし易さの指標となる量を算出し、算出された指標となる量を用いてアームを移動させる速さを決定する。これにより、ロボットは、第１物体の動かし難さ又は動かし易さの指標となる量を用いて決定した速さによって第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、前記第１物体の第３位置と前記第１物体の第４位置とを用いて前記速さを決定する、構成が用いられてもよい。
この構成により、ロボットは、第１物体の第３位置と第１物体の第４位置とを用いてアームを移動させる速さを決定する。これにより、ロボットは、第１物体の第３位置と第１物体の第４位置とを用いて決定した速さによって第１物体を移動させることができる。

また、本発明の他の態様は、ロボットにおいて、前記制御部は、更に前記第３位置に対応する第３姿勢と、前記第４位置に対応する第４姿勢とを用いて前記速さを決定する、構成が用いられてもよい。
この構成により、ロボットは、更に第１物体の第３位置に対応する第３姿勢と、第１物体の第４位置に対応する第４姿勢とを用いてアームを移動させる速さを決定する。これにより、ロボットは、第１物体の第３位置に対応する第３姿勢と、第１物体の第４位置に対応する第４姿勢とを用いて決定した速さによって第１物体を移動させることができる。

また、本発明の他の態様は、ロボットが備えるアームとともに動く第１物体と第２物体との相対位置姿勢、及び前記第１物体の第１位置に基づいて経路を生成する、制御装置である。
この構成により、制御装置は、ロボットが備えるアームとともに動く第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づいて経路を生成する。これにより、制御装置は、第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づく経路に沿って第１物体を移動させることができる。その結果、制御装置は、例えば、他の物体との干渉の抑制及び第１物体を移動させる時間の短縮を図ることができる。

また、本発明の他の態様は、ロボットが備えるアームとともに動く第１物体と第２物体との相対位置姿勢、及び前記第１物体の第１位置に基づいて経路を生成する、制御方法である。
この構成により、制御方法は、ロボットが備えるアームとともに動く第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づいて経路を生成する。これにより、制御方法は、第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づく経路に沿って第１物体を移動させることができる。その結果、制御方法は、例えば、他の物体との干渉の抑制及び第１物体を移動させる時間の短縮を図ることができる。

以上により、ロボット、制御装置、及び制御方法は、アームとともに動く第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づいて経路を生成する。これにより、ロボット、制御装置、及び制御方法は、第１物体と第２物体との相対位置姿勢、及び第１物体の第１位置に基づく経路に沿って第１物体を移動させることができる。その結果、ロボット、制御装置、及び制御方法は、例えば、他の物体との干渉の抑制及び第１物体を移動させる時間の短縮を図ることができる。

第１実施形態に係るロボットシステム１の一例を示す構成図である。 本実施形態における作業領域に沿ってロボット２０が操作対象Ｎを組付対象Ｏに組み付ける様子の一例を示す図である。 制御装置３０のハードウェア構成の一例を示す図である。 制御装置３０の機能構成の一例を示す図である。 制御装置３０が操作対象Ｎの経路ポテンシャルを算出し、所定の作業を行う処理の流れの一例を示すフローチャートである。 図５に示したステップＳ１００の可動領域算出処理における並進可動領域算出処理の流れの一例を示すフローチャートである。 図５に示したステップＳ１００の可動領域算出処理における回転可動領域算出処理の流れの一例を示すフローチャートである。 可動領域算出部３７がステップＳ１００において選択した座標が示す仮想空間の点に配置された操作対象Ｎの一例を示す図である。 図５に示したステップＳ１１０の可動ポテンシャル算出処理の流れの一例を示すフローチャートである。 可動ポテンシャル、並進可動ポテンシャル、及び回転可動ポテンシャルそれぞれの一例を表すグラフを示す図である。 図５に示したステップＳ１２０の距離ポテンシャル算出処理の流れの一例を示すフローチャートである。 操作対象Ｎの組付位置を示す点と、ポテンシャル算出部３８がステップＳ５１０において選択した座標が示す仮想空間の点とにそれぞれ配置された操作対象Ｎの一例を示す図である。 このようにして算出された並進距離ポテンシャルの一例を表すグラフを示す図である。 ステップＳ１３０において可動ポテンシャルと並進距離ポテンシャルを合算することによりポテンシャル算出部３８が算出した経路ポテンシャルの一例を表すグラフを示す図である。 平滑化処理部３９が生成した滑らかな経路ポテンシャルの一例を表すグラフを示す図である。 可動領域算出部３７ａによる並進可動領域算出処理の流れの一例を示すフローチャートである。 可動領域算出部３７ａによる回転可動領域算出処理の流れの一例を示すフローチャートである。 ポテンシャル算出部３８ａによる距離ポテンシャル算出処理の流れの一例を示すフローチャートである。

＜第１実施形態＞
以下、本発明の第１実施形態について、図面を参照して説明する。図１は、第１実施形態に係るロボットシステム１の一例を示す構成図である。ロボットシステム１は、ロボット２０と、制御装置３０を備える。なお、ロボットシステム１は、ロボット２０とは別体の撮像部を備える構成であってもよい。

まず、ロボット２０及び制御装置３０の構成について説明する。
ロボット２０は、アームと、力センサー２５と、制御装置３０を備えた単腕ロボットである。
単腕ロボットは、この一例における１本のアーム（腕）を備えるロボットである。なお、ロボット２０は、単腕ロボットに代えて、双腕ロボットであってもよい。双腕ロボットは、２本のアームを備えるロボットである。また、ロボット２０は、撮像部を備える構成であってもよい。

アームは、物体を把持可能な爪部を備えるエンドエフェクターＥと、マニピュレーターＭと、図示しない複数のアクチュエーターによって構成される。アームは、７軸垂直多関節型のアームである。具体的には、アームは、支持台と、エンドエフェクターＥと、マニピュレーターＭとがアクチュエーターによる連携した動作によって７軸の自由度の動作を行う。

アームが７軸の自由度で動作する場合、アームは、６軸以下の自由度で動作する場合と比較して取り得る姿勢が増える。これによりアームは、例えば、動作が滑らかになり、更にアームの周辺に存在する物体との干渉を容易に回避することができるようになる。また、アームが７軸の自由度で動作する場合、アームの制御は、アームが８軸以上の自由度で動作する場合と比較して計算量が少なく容易である。このような理由から、アームは、７軸の自由度で動作することが望ましい。それ故、この一例では、アームが７軸の自由度で動作する場合について説明する。なお、アームは、６軸以下の自由度で動作する構成であってもよく、８軸以上の自由度で動作する構成であってもよい。

各アクチュエーターは、ケーブルによって制御装置３０と通信可能に接続されている。これにより、アクチュエーターは、制御装置３０から取得される制御信号に基づいて、エンドエフェクターＥとマニピュレーターＭを動作させることができる。なお、ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ（Universal Serial Bus）等の規格によって行われる。また、アクチュエーターのうちの一部又は全部は、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって制御装置３０と接続される構成であってもよい。

力センサー２５は、エンドエフェクターＥとマニピュレーターＭの間に備えられている。力センサー２５は、エンドエフェクターＥに作用した力やモーメントの大きさを示す値を検出する。なお、力センサー２５は、エンドエフェクターＥに加わる力やモーメントの大きさを示す値を検出するトルクセンサー等の他のセンサーであってもよい。力センサー２５は、通信により力センサー情報を制御装置３０へ出力する。

力センサー情報は、力センサー２５が検出した力やモーメントの大きさを示す値を、力センサー２５の出力値として含む情報である。また、力センサー情報は、制御装置３０によるロボット２０の力センサー情報に基づく制御に用いられる。力センサー情報に基づく制御とは、例えば、インピーダンス制御等のコンプライアンス制御を示す。

力センサー２５は、ケーブルによって制御装置３０と通信可能に接続されている。ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。なお、力センサー２５は、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって制御装置３０と接続されてもよい。

上記で説明したロボット２０が備えるこれらの各機能部は、この一例において、ロボット２０に内蔵された制御装置３０から制御信号を取得する。そして、これらの各機能部は、取得した制御信号に基づいた動作を行う。なお、ロボット２０は、外部に設置された制御装置３０により制御される構成に代えて、制御装置３０を内蔵する構成であってもよい。

制御装置３０は、ロボット２０が備える各機能部に制御信号を送信することにより、ロボット２０を動作させる。また、制御装置３０は、ロボット２０に所定の作業を行わせる。この一例において、制御装置３０は、所定の作業として、ロボット２０を動作させることにより第１物体を他の第２物体に組み付ける作業を行う。

以下、第１物体及び第２物体と、ロボット２０が行う所定の作業とについて説明する。
第１物体は、例えば、図１に示したようにロボット２０がエンドエフェクターＥにより把持している操作対象Ｎのことである。なお、第１物体は、操作対象Ｎに代えて、ロボット２０のエンドエフェクターＥの所定部位やマニピュレーターＭの所定部位等であってもよい。操作対象Ｎは、この一例において、エンドエフェクターＥの爪部により把持可能な程度の高さを有する六角柱形状の物体である。なお、操作対象Ｎは、これに代えて、他の形状や大きさであってもよい。

第２物体は、ロボット２０により第１物体を組み付けられる物体のことである。なお、第２物体は、これに代えて、操作対象Ｎを移動させる際に障害となる物体や、何らかの治具、壁面、作業台等の位置及び姿勢がロボット２０の座標原点の位置及び姿勢に対して変化しないように設置された物体であれば如何なる物体であってもよい。

第２物体は、例えば、図１に示した組付対象Ｏである。すなわち、組付対象Ｏは、ロボット２０により操作対象Ｎが組み付けられる物体である。図１に示したように、この一例では、組付対象Ｏは、治具Ｆに支持されている。なお、組付対象Ｏは、これに代えて、テーブルや何らかの台等の他の場所に設置される構成であってもよい。その場合、組付対象Ｏの位置及び姿勢は、ロボット２０の座標原点の位置及び姿勢に対して変化しないように設置される。ロボット２０の座標原点の位置及び姿勢は、例えば、ロボット２０の支持台の重心の位置及び姿勢である。なお、ロボット２０の座標原点の位置及び姿勢は、これに代えて、ロボット２０に対応付けられた他の位置及び姿勢であってもよい。

制御装置３０は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢、及び操作対象Ｎの第１位置に基づいて、操作対象Ｎを組付対象Ｏに組み付けるまでに操作対象Ｎを作業領域内において移動させる経路を生成する。以下では、一例として操作対象Ｎの第１位置が、ロボット２０が行う所定の作業において操作対象Ｎが組付対象Ｏに組み付けられた後の操作対象Ｎの位置である場合について説明する。なお、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢は、第１物体と第２物体との相対位置姿勢の一例である。

また、制御装置３０は、経路を生成する際、操作対象Ｎが組付対象Ｏや他の物体との干渉が起こる可能性が低い経路であり、且つ操作対象Ｎを速く移動させることが可能な経路を生成する。

これにより、制御装置３０は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢、及び操作対象Ｎが組付対象Ｏに組み付けられた後の操作対象Ｎの位置に基づいて生成する経路に沿って操作対象Ｎを移動させることができる。その結果、制御装置３０は、他の物体との干渉の抑制及び第１物体を移動させる時間の短縮を図ることができる。なお、操作対象Ｎの第１位置は、作業領域内における操作対象Ｎの他の位置であってもよい。また、以下では、説明の便宜上、ロボット２０が行う所定の作業において操作対象Ｎが組付対象Ｏに組み付けられた後の操作対象Ｎの位置を組付位置と称して説明する。また、以下では、組付位置に対応する操作対象Ｎの姿勢を組付姿勢と称して説明する。

また、以下では、操作対象Ｎの位置及び姿勢が、操作対象Ｎの重心の位置及び姿勢である場合について説明する。なお、操作対象Ｎの位置及び姿勢は、これに代えて、操作対象Ｎとともに動く他の点の位置及び姿勢であってもよい。
また、以下では、組付対象Ｏの位置及び姿勢は、組付対象Ｏの重心の位置及び姿勢である場合について説明する。なお、組付対象Ｏの位置及び姿勢は、これに代えて、組付対象Ｏとともに固定される他の点の位置及び姿勢であってもよい。

また、この一例における作業領域は、ロボット２０が操作対象Ｎを組付対象Ｏに組み付けるために操作対象Ｎの重心を移動させることが可能な経路を含む平面又は空間の一部又は全部の領域のことである。また、作業領域には、組付対象Ｏが含まれている。以下では、説明を簡略化するため、一例として作業領域が図２に示した二次元（ｘｙ）平面である場合について説明する。

図２は、本実施形態における作業領域に沿ってロボット２０が操作対象Ｎを組付対象Ｏに組み付ける様子の一例を示す図である。図２に示したｘ軸及びｙ軸は、組付対象Ｏを含む二次元平面（すなわち、作業領域）上の座標軸である。ロボット２０は、例えば、エンドエフェクターＥにより把持した操作対象Ｎを、図２に示した矢印（前述の経路）に沿って組付対象Ｏに組み付ける。

本実施形態では、この組み付けの際、ロボット２０は、図２に示した座標軸により示される二次元平面に沿ってのみ操作対象Ｎを移動する。なお、ロボット２０は、二次元平面である作業領域に沿って操作対象Ｎを移動する構成に代えて、三次元（ｘｙｚ）空間内に広がる経路に沿って操作対象Ｎを移動する構成であってもよい。

また、制御装置３０は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢、及び操作対象Ｎの組付位置に基づいて、制御装置３０が生成した経路上の各位置においてロボット２０が操作対象Ｎを移動させる速さを決定する。

そして、制御装置３０は、生成した経路と決定した速さとに基づいてロボット２０を動作させることにより、操作対象Ｎを組付対象Ｏに組み付ける。このように操作対象Ｎを組付対象Ｏに組み付けることにより、ロボット２０は、組付対象Ｏや他の物体との干渉の抑制及び操作対象Ｎを移動させる時間の短縮を図ることができる。なお、図１に示した点線ＶＮは、組付対象Ｏに組み付けられた状態の操作対象Ｎの輪郭を示している。

なお、制御装置３０は、コンプライアンス制御によってロボット２０を動作させることにより、ロボット２０に操作対象Ｎを組付対象Ｏに組み付けさせる。これにより、制御装置３０は、操作対象Ｎと組付対象Ｏや他の物体との干渉が起きた場合であっても、例えば、操作対象Ｎや組付対象Ｏ、他の物体を変形させることなくロボット２０に操作対象Ｎを組付対象Ｏに組み付けさせることができる。

次に、図３を参照し、制御装置３０のハードウェア構成について説明する。図３は、制御装置３０のハードウェア構成の一例を示す図である。制御装置３０は、例えば、ＣＰＵ（Central Processing Unit）３１と、記憶部３２と、入力受付部３３と、通信部３４と、表示部３５を備える。また、制御装置３０は、通信部３４を介してロボット２０と通信を行う。これらの構成要素は、バスＢｕｓを介して相互に通信可能に接続されている。

ＣＰＵ３１は、記憶部３２に格納された各種プログラムを実行する。
記憶部３２は、例えば、ＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read−Only Memory）、ＲＯＭ（Read−Only Memory）、ＲＡＭ（Random Access Memory）などを含む。記憶部３２は、制御装置３０が処理する各種情報や画像、プログラム、組付対象Ｏの位置及び姿勢を示す情報、所定の作業を行う際の作業領域を示す情報等を格納する。なお、記憶部３２は、制御装置３０に内蔵されるものに代えて、ＵＳＢ等のデジタル入出力ポート等によって接続された外付け型の記憶装置でもよい。

入力受付部３３は、例えば、キーボードやマウス、タッチパッド等を備えたティーチングペンダントや、その他の入力装置である。なお、入力受付部３３は、タッチパネルとして表示部３５と一体に構成されてもよい。
通信部３４は、例えば、ＵＳＢ等のデジタル入出力ポートやイーサネット（登録商標）ポート等を含んで構成される。
表示部３５は、例えば、液晶ディスプレイパネル、あるいは、有機ＥＬ（ElectroLuminescence）ディスプレイパネルである。

次に、図４を参照し、制御装置３０の機能構成について説明する。図４は、制御装置３０の機能構成の一例を示す図である。制御装置３０は、記憶部３２と、制御部３６を備える。

制御部３６は、制御装置３０の全体を制御する。制御部３６は、可動領域算出部３７と、ポテンシャル算出部３８と、平滑化処理部３９と、経路生成部４０と、速さ決定部４１と、力センサー取得部４２と、ロボット制御部４３を備える。制御部３６が備えるこれらの機能部のうち一部又は全部は、例えば、ＣＰＵ３１が、記憶部３２に記憶された各種プログラムを実行することで実現される。また、これらの機能部のうち一部または全部は、ＬＳＩ（Large Scale Integration）やＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェア機能部であってもよい。

可動領域算出部３７は、可動領域を算出する。可動領域は、作業領域内の各点において定義される量である。また、可動領域は、作業領域内において操作対象Ｎが組付対象Ｏや他の物体と接触せずに動くことができる範囲を定量的に示す量である。

ポテンシャル算出部３８は、可動領域算出部３７が算出した可動領域に基づいて可動ポテンシャルを算出する。可動ポテンシャルは、作業領域内の各点において定義される量である。また、可動ポテンシャルは、作業領域内の各点における操作対象Ｎの動かし難さ（又は、動かし易さ）の指標となる量である。

可動ポテンシャルは、作業領域内の各点において算出されるため、作業領域内の各点を示す座標の関数として表される。なお、可動ポテンシャルは、ポテンシャルと称しているが、物理量ではない。可動ポテンシャルをポテンシャルと称する理由は、作業領域の各点における操作対象Ｎの剛性、すなわちエンドエフェクターＥにより把持された操作対象Ｎの動かし難さ（もしくは、動かし易さ）の指標となる量を、空間の各点において物体に働く保存力に係るポテンシャルエネルギーになぞらえたためである。操作対象Ｎを動かし難い状態とは、この場合、操作対象Ｎを動かすことにより、操作対象Ｎと組付対象Ｏや他の物体とが干渉してしまう可能性が高い状態のことである。

ここで、作業領域内における操作対象Ｎの位置及び姿勢は、作業領域内において組付対象Ｏの位置及び姿勢が変化しないため、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢を表す。すなわち、ポテンシャル算出部３８は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢に基づいて操作対象Ｎの動かし難さの指標となる量を、作業領域内の各点における可動ポテンシャルとして算出する。

また、ポテンシャル算出部３８は、作業領域内における操作対象Ｎの組付位置に基づいて距離ポテンシャルを算出する。距離ポテンシャルは、作業領域内の各点において定義される量である。また、距離ポテンシャルは、作業領域内の各点において、ロボット２０が操作対象Ｎを移動させることが可能な速さの指標となる量のことである。例えば、距離ポテンシャルが低いところでは、当該速さを速くすることができ、距離ポテンシャルが高いところでは、当該速さを距離ポテンシャルが低いところに比べて遅くしなければならない。

距離ポテンシャルは、作業領域内の各点において算出されるため、作業領域内の各点を示す座標（すなわち、位置）の関数として表される。なお、距離ポテンシャルは、ポテンシャルと称しているが、物理量ではない。距離ポテンシャルをポテンシャルと称する理由は、作業領域の各点においてエンドエフェクターＥにより操作対象Ｎを移動させることが可能な速さの指標となる量を、空間の各点において物体を加速させる保存力に係るポテンシャルエネルギーになぞらえたためである。

また、ポテンシャル算出部３８は、算出した可動ポテンシャルと距離ポテンシャルを合算した経路ポテンシャルを算出する。経路ポテンシャルは、ロボット２０が操作対象Ｎを組付対象Ｏに組み付ける際に、操作対象Ｎを動かし易く、そして速く移動させることができる位置の指標となる量である。経路ポテンシャルは、可動ポテンシャル及び距離ポテンシャルが作業領域内の各点を示す座標（位置）の関数であるため、作業領域内の各点を示す座標（位置）の関数として表される。

平滑化処理部３９は、ポテンシャル算出部３８によって算出された経路ポテンシャルを表す関数に対して、ローパスフィルターを適用する。これにより、平滑化処理部３９は、経路ポテンシャルを表す関数を平滑化（すなわち、平均化）する。以下では、説明の便宜上、平滑化処理部３９が経路ポテンシャルを表す関数をこのように変換することを、滑らかなポテンシャルを生成すると称して説明する。また、以下では、ローパスフィルターを適用する前の経路ポテンシャルを表す関数を、不連続に変化する経路ポテンシャルと称して説明する。不連続に変化する経路ポテンシャルに基づいてロボット２０を制御しようとすると、例えば、アクチュエーター等に負担が掛かり、故障の原因となりかねない。そのため、平滑化処理部３９は、算出された不連続に変化する経路ポテンシャルにローパスフィルターを適用することで、滑らかなポテンシャルを生成する。

経路生成部４０は、平滑化処理部３９が平滑化した経路ポテンシャルに基づいて、ロボット２０が操作対象Ｎを組付対象Ｏに組み付けるまでに移動させる経路を生成する。経路生成部４０が生成する当該経路は、操作対象Ｎが組付対象Ｏや他の物体との干渉が起こる可能性が低い経路であり、且つ操作対象Ｎを速く移動させることが可能な経路である。
速さ決定部４１は、平滑化処理部３９が平滑化した経路ポテンシャルに基づいて、経路生成部４０が生成した経路上の各点において操作対象Ｎをロボット２０が移動させる速さを決定する。

力センサー取得部４２は、力センサー２５により検出される力センサー情報を取得する。
ロボット制御部４３は、経路生成部４０が生成した経路と、速さ決定部４１が決定した当該経路上の各点において操作対象Ｎをロボット２０が移動させる速さとに基づいて、ロボット２０を動作させ、操作対象Ｎを組付対象Ｏに組み付ける。この際、ロボット制御部４３は、力センサー取得部４２が取得した力センサー情報に基づいて、コンプライアンス制御によりロボット２０を動作させる。

以下、図５を参照し、制御装置３０が経路ポテンシャルを算出し、所定の作業を行う処理について説明する。図５は、制御装置３０が操作対象Ｎの経路ポテンシャルを算出し、所定の作業を行う処理の流れの一例を示すフローチャートである。以下では、一例として、予めロボット２０がエンドエフェクターＥにより操作対象Ｎを把持している場合について説明する。従って、制御装置３０は、ロボット２０に操作対象Ｎを把持させてからロボット２０に所定の作業を行わせる構成であってもよい。

まず、可動領域算出部３７は、操作対象Ｎの可動領域を算出するための可動領域算出処理を行う（ステップＳ１００）。この一例において、操作対象Ｎの可動領域は、作業領域を表す二次元平面上のある点から当該二次元平面内において組付対象Ｏと接触せずに操作対象Ｎがｘ方向及びｙ方向に移動可能な距離の総和（以下、並進可動領域と称する）と、当該点において組付対象Ｏと接触せずに正方向（例えば、反時計回り）及び負方向（例えば、時計回り）に回転可能な回転角の総和（以下、回転可動領域と称する）との２つの量によって定義される量であり、当該二次元平面上の各点において定義される量である。そのため、可動領域算出処理は、並進可動領域算出処理と回転可動領域算出処理の２つの処理によって構成される。

並進可動領域算出処理は、作業領域内の各点における操作対象Ｎの並進可動領域を算出する処理である。回転可動領域算出処理は、作業領域内の各点における操作対象Ｎの回転可動領域を算出する処理である。可動領域算出部３７は、可動領域算出処理において並進可動領域算出処理と回転可動領域算出処理のいずれか一方を先に行ってもよく、両方を同時に行ってもよい。なお、可動領域算出部３７は、可動領域算出処理において、並進可動領域算出処理と回転可動領域のうちいずれか一方のみを行う構成（すなわち、操作対象Ｎの可動領域は、並進可動領域と回転可動領域のうちいずれか一方によって定義される構成）であってもよい。

なお、操作対象Ｎの可動領域は、作業領域内の各点における操作対象Ｎに係る他の量によって定義されてもよい。また、操作対象Ｎの可動領域は、作業領域内の各点において定義される構成に代えて、例えば、作業領域を複数の領域に分割した場合のそれぞれの領域毎に定義される構成であってもよい。

次に、ポテンシャル算出部３８は、ステップＳ１００において可動領域算出部３７が算出した操作対象Ｎの並進可動領域と回転可動領域のうちいずれか一方又は両方に基づいて、作業領域内の各点における可動ポテンシャルを算出するための可動ポテンシャル算出処理を行う（ステップＳ１１０）。本実施形態では、ポテンシャル算出部３８は、可動ポテンシャル算出処理において、操作対象Ｎの並進可動領域と回転可動領域の両方に基づいて、作業領域内の各点における可動ポテンシャルを算出する場合について説明する。

次に、ポテンシャル算出部３８は、作業領域内の各点における距離ポテンシャルを算出するための距離ポテンシャル算出処理を行う（ステップＳ１２０）。なお、ポテンシャル算出部３８が行うステップＳ１１０の処理とステップＳ１２０の処理は、順序が逆に行われてもよく、並列に行われてもよい。
次に、ポテンシャル算出部３８は、ステップＳ１１０において算出した可動ポテンシャルと、ステップＳ１２０において算出した距離ポテンシャルを合算した経路ポテンシャルを算出する（ステップＳ１３０）。

次に、平滑化処理部３９は、ステップＳ１３０においてポテンシャル算出部３８が算出した不連続に変化する経路ポテンシャルにローパスフィルターを適用することで、滑らかに変化する経路ポテンシャルを生成（平滑化）する（ステップＳ１４０）。
次に、経路生成部４０は、ステップＳ１４０において平滑化処理部３９が平滑化した経路ポテンシャルに基づいて、ロボット２０が操作対象Ｎを組付対象Ｏに組み付けるまでに移動させる経路を生成する経路生成処理を行う（ステップＳ１５０）。

次に、速さ決定部４１は、ステップＳ１４０において平滑化処理部３９が平滑化した経路ポテンシャルに基づいて、ステップＳ１５０において経路生成部４０が生成した経路上の各点において操作対象Ｎをロボット２０が移動させる速さを決定する速さ決定処理を行う（ステップＳ１６０）。

次に、ロボット制御部４３は、ステップＳ１５０において経路生成部４０が生成した経路と、ステップＳ１６０において速さ決定部４１が決定した速さであって、経路上の各点において操作対象Ｎをロボット２０が移動させる速さとに基づいて、ロボット２０に所定の作業を行わせる（ステップＳ１７０）。なお、ロボット制御部４３は、ステップＳ１７０において、力センサー取得部４２が取得した力センサー情報に基づいて、コンプライアンス制御によってロボット２０を動作させる。

以下、図６及び図７を参照し、図５に示したステップＳ１００の可動領域算出処理について説明する。図６は、図５に示したステップＳ１００の可動領域算出処理における並進可動領域算出処理の流れの一例を示すフローチャートである。

まず、可動領域算出部３７は、組付対象Ｏの大きさや形状を示す情報と、組付対象Ｏの位置及び姿勢を示す情報と、作業領域を示す情報を記憶部３２から読み込み、読み込まれた組付対象Ｏの位置及び姿勢を示す情報と、作業領域を示す情報とに基づいて作業領域を表す仮想空間を生成する（ステップＳ１９０）。

なお、この一例において、組付対象Ｏの大きさや形状を示す情報と、組付対象Ｏの位置及び姿勢を示す情報と、作業領域を示す情報とは、予め記憶部３２に記憶されている場合について説明する。なお、作業領域を示す情報は、これに代えて、ロボットシステム１が撮像部を備え、撮像部により撮像された撮像画像に基づいて検出される構成であってもよい。また、この一例において、作業領域を示す情報は、作業領域の形状が図２に示したように長方形状の平面であるため、当該長方形の各頂点を示す座標の組み合わせである。また、組付対象Ｏの位置及び姿勢を示す情報は、これに代えて、ロボットシステム１が撮像部を備え、撮像部により撮像された撮像画像に基づいて検出される構成であってもよい。

ステップＳ１９０において可動領域算出部３７が生成した仮想空間内には、仮想的に組付対象Ｏが配置されている。仮想的に組付対象Ｏが配置されている仮想空間内での組付対象Ｏの位置及び姿勢は、現実の作業領域内の組付対象Ｏの位置及び姿勢に対応している。

次に、可動領域算出部３７は、ステップＳ１９０において生成した仮想空間の各点を示す座標を１つずつ選択する。そして、可動領域算出部３７は、選択した座標毎に、ステップＳ２１０からステップＳ２７０までの処理を繰り返し行う（ステップＳ２００）。

ステップＳ２００において仮想空間の座標を１つ選択した後、可動領域算出部３７は、選択した座標が示す仮想空間の点に、仮想的に操作対象Ｎを配置する。この際、可動領域算出部３７は、仮想空間の点に操作対象Ｎの重心が一致するように操作対象Ｎを配置する。また、可動領域算出部３７は、仮想空間の点に配置した操作対象Ｎの姿勢を、所定の姿勢に初期化する（ステップＳ２１０）。なお、所定の姿勢とは、例えば、予め決められた操作対象Ｎの重心の位置を通る中心線と、ｘ軸又はｙ軸との成す角度がゼロ度となるような姿勢であるが、これに限られず、他の姿勢であってもよい。

次に、可動領域算出部３７は、数値を示す情報を格納可能な変数Ｄを生成し、変数Ｄの値をゼロに初期化する（ステップＳ２２０）。
次に、可動領域算出部３７は、ステップＳ２６０における操作対象Ｎの回転に係る回転角の累積値が３６０°に達したか否かを判定する（ステップＳ２３０）。

操作対象Ｎの回転に係る回転角の累積値が３６０°に達していないと判定した場合（ステップＳ２３０−Ｎｏ）、可動領域算出部３７は、ステップＳ２００において選択した座標が示す仮想空間の点に配置された操作対象Ｎがｘ軸の正方向、ｘ軸の負方向、ｙ軸の正方向、ｙ軸の負方向のそれぞれの方向（以下、４つの方向と称する）への移動可能な距離の和を算出する（ステップＳ２４０）。

なお、可動領域算出部３７が算出するこれらの移動可能な距離の和は、４つ方向それぞれへの移動可能な距離の和に代えて、４つの方向のうちのいずれか１つへの移動距離であってもよく、４つの方向それぞれへの移動可能な距離の一部の組み合わせの和であってもよく、１以上の他の方向への移動可能な距離の和（他の方向が１つのみの場合、和ではなく、その方向への移動可能な距離）であってもよく、上記の４つの方向それぞれへの移動可能な距離と他の方向への移動可能な距離との和であってもよい。例えば、可動領域算出部３７は、操作対象Ｎが移動可能な距離の和として、１以上の他の方向（例えば、ｘ軸の延伸方向とｙ軸の延伸方向との間の方向（斜め方向）等）への移動可能な距離の和を算出する場合、それらの方向への距離を三平方の定理等、距離空間において距離を算出する方法によって算出する。

次に、可動領域算出部３７は、ステップＳ２４０において算出した４つの方向への移動可能な距離の和を、ステップＳ２２０において生成した変数Ｄに格納されている値に加算する（ステップＳ２５０）。
次に、可動領域算出部３７は、ステップＳ２００において選択した座標が示す仮想空間の点に配置した操作対象Ｎの姿勢を、所定角度だけ回転させる（ステップＳ２６０）。なお、所定角度は、その倍数に３６０°を含む任意の角度であることが望ましく、この一例において、５°や１０°等であるとするが、その倍数に３６０°を含まない他の角度であってもよい。

ステップＳ２３０からステップＳ２６０までの処理をまとめて換言すると、可動領域算出部３７は、ステップＳ２００において選択した座標が示す仮想空間の点に配置した操作対象Ｎの姿勢を所定角度ずつ変化させ、それぞれの姿勢毎に前述の４つの方向への移動可能な距離の和を算出し、算出された姿勢毎の４つの方向への移動可能な距離の和を変数Ｄに加算する。

ここで、図８を参照し、ステップＳ２３０からステップＳ２６０までの処理について説明する。図８は、可動領域算出部３７がステップＳ１００において選択した座標が示す仮想空間の点に配置された操作対象Ｎの一例を示す図である。図８において、座標（ｘ，ｙ）を、可動領域算出部３７がステップＳ２００において選択した座標であるとして説明する。点Ｇ（ｘ，ｙ）は、可動領域算出部３７が座標（ｘ，ｙ）に配置した操作対象Ｎの重心の位置を示す。また、枠ＶＳは、可動領域算出部３７が生成した仮想空間であって作業領域を表す仮想空間を示す。以下では、この枠ＶＳによって囲まれた範囲を仮想空間ＶＳと称して説明する。なお、図８に示した座標軸は、この仮想空間ＶＳ上の位置及び姿勢を表す座標軸である。また、以下では、図８において示した操作対象Ｎの姿勢が、所定の姿勢である場合について説明する。

可動領域算出部３７は、座標（ｘ，ｙ）に配置した操作対象Ｎを前述の４つの方向のそれぞれに移動させる。この時、可動領域算出部３７は、操作対象Ｎの輪郭が、仮想空間ＶＳの枠や組付対象Ｏの輪郭と干渉（すなわち、接触）するまで操作対象Ｎを移動させた時の移動距離を、４つの方向のそれぞれの方向に移動可能な距離として算出する。図８において、距離Ｄ_＋ｘは、ｘ軸の正方向への移動可能な距離であり、距離Ｄ_−ｘは、ｘ軸の負方向への移動可能な距離である。また、図８において、距離Ｄ_＋ｙは、ｙ軸の正方向への移動可能な距離であり、距離Ｄ_−ｙは、ｙ軸の負方向への移動可能な距離である。可動領域算出部３７は、これらの距離Ｄ_＋ｘ、距離Ｄ_−ｘ、距離Ｄ_＋ｙ、距離Ｄ_−ｙの和を変数Ｄに格納された値に加算する。

そして、可動領域算出部３７は、操作対象Ｎの姿勢を所定角度ずつ３６０°まで変化させ、所定角度変化させる毎にその時の姿勢において、このような４つの方向への移動可能な距離の和を算出し、算出された４つの方向への移動可能な距離の和を変数Ｄに格納された値に加算していく。この４つの方向への移動可能な距離の和は、操作対象Ｎの位置及び姿勢に応じて異なる値を取る。従って、変数Ｄに加算されていく値は、操作対象Ｎの位置及び姿勢に応じた値となる。このようにして、可動領域算出部３７は、ステップＳ２３０からステップＳ２６０までの処理を行う。

一方、ステップＳ２３０において操作対象Ｎの回転に係る回転角の累積値が３６０°に達していると判定した場合（ステップＳ２３０−Ｙｅｓ）、可動領域算出部３７は、変数Ｄに格納されている値を、ステップＳ１００で選択した座標における並進可動領域Ｆ_ｔとし（ステップＳ２７０）、その後、ステップＳ２００に戻って仮想空間の次の座標を選択する。以下では、仮想空間ＶＳ内での各点を示す座標を座標（ｘ，ｙ）とした場合、その座標における並進可動領域Ｆ_ｔの値を並進可動領域Ｆ_ｔ（ｘ，ｙ）と表す。なお、この一例では、仮想空間の各点を示す座標と、作業領域の各点を示す座標とは、キャリブレーション等によって予め対応付けられている場合について説明する。そのため、仮想空間ＶＳ内の各点は、作業領域内の各点を示す。

図７は、図５に示したステップＳ１００の可動領域算出処理における回転可動領域算出処理の流れの一例を示すフローチャートである。なお、図７に示したフローチャートにおいて、ステップＳ１９０からステップＳ２１０までの処理は、図６に示したフローチャートにおけるステップＳ１９０からステップＳ２１０までの処理と同様の処理であるため、説明を省略する。

ただし、可動領域算出部３７は、図７に示したステップＳ２００において選択した座標毎に、図７に示したフローチャートのステップＳ２１０からステップＳ３６０までの処理を繰り返し行う。また、可動領域算出部３７により回転可動領域算出処理が図６に示した並進可動領域算出処理の後に（又は並進可動領域算出処理と並列に）行われる場合、すでに仮想空間が生成されているため、図７に示したステップＳ１９０において新たに仮想空間を生成しなくてもよい。

図７に示したステップＳ２１０の処理を実行した後、可動領域算出部３７は、数値を示す情報を格納可能な変数Ｈを生成し、変数Ｈの値をゼロに初期化する（ステップＳ３２０）。
次に、可動領域算出部３７は、ステップＳ３００において選択した座標が示す仮想空間の点に配置した操作対象Ｎが、図８に示した仮想空間ＶＳの枠や組付対象Ｏの輪郭に現時点で干渉（すなわち、接触）しているか否かを判定する（ステップＳ３３０）。

操作対象Ｎが仮想空間ＶＳの枠や組付対象Ｏの輪郭に接触していないと判定した場合（ステップＳ３３０−Ｎｏ）、可動領域算出部３７は、ステップＳ２００において選択した座標が示す仮想空間の点に配置した操作対象Ｎを、例えば、操作対象Ｎの重心を中心として正方向と負方向のそれぞれに、操作対象Ｎの輪郭が仮想空間ＶＳの枠や組付対象Ｏの輪郭に接触するまで回転させる。ただし、操作対象Ｎの輪郭が仮想空間ＶＳの枠や組付対象Ｏの輪郭に接触しない場合、正方向と負方向のそれぞれの回転角は、３６０°であるとする。可動領域算出部３７は、正方向への回転角と、負方向への回転角との和を回転可能角度として算出し（ステップＳ３４０）、変数Ｈに格納されている値に加算する（ステップＳ３５０）。

一方、ステップＳ３３０において操作対象Ｎが仮想空間ＶＳの枠や組付対象Ｏの輪郭に干渉（すなわち、接触）していると判定した場合（ステップＳ３３０−Ｙｅｓ）、可動領域算出部３７は、変数Ｈに格納されている値を、回転可動領域Ｆ_ｒとし（ステップＳ３６０）、その後、ステップＳ３００に戻って次の仮想空間の座標を選択する。以下では、仮想空間ＶＳ内での各点を示す座標を座標（ｘ，ｙ）とした場合、その座標における回転可動領域Ｆ_ｒの値を回転可動領域Ｆ_ｒ（ｘ，ｙ）と表す。

以下、図９を参照し、図５に示したステップＳ１１０の可動ポテンシャル算出処理について説明する。図９は、図５に示したステップＳ１１０の可動ポテンシャル算出処理の流れの一例を示すフローチャートである。まず、ポテンシャル算出部３８は、ステップＳ１００で可動領域算出部３７により算出された仮想空間ＶＳ内の各点（すなわち、作業領域内の各点）における可動領域、すなわち並進可動領域Ｆ_ｔ（ｘ，ｙ）及び回転可動領域Ｆ_ｒ（ｘ，ｙ）に基づいて、並進可動領域Ｆ_ｔ（ｘ，ｙ）の全ての点の中での最大値Ｆ_ｔＭＡＸと、回転可動領域Ｆ_ｒ（ｘ，ｙ）の全ての点の中での最大値Ｆ_ｒＭＡＸを検出する（ステップＳ４００）。

次に、ポテンシャル算出部３８は、ステップＳ４００において検出した並進可動領域Ｆ_ｔ（ｘ，ｙ）の最大値Ｆ_ｔＭＡＸと、回転可動領域Ｆ_ｒ（ｘ，ｙ）の最大値Ｆ_ｒＭＡＸとに基づいて、作業領域内の各点におけるポテンシャルを算出する（ステップＳ４１０）。ここで、このポテンシャルを算出する処理について説明する。ポテンシャル算出部３８は、以下に示す式（１）〜式（３）に基づいて作業領域内の各点における可動ポテンシャルを算出する。

Ｐ_ｔ（ｘ，ｙ）＝Ｆ_ｔＭＡＸ−Ｆ_ｔ（ｘ，ｙ） ・・・（１）

Ｐ_ｒ（ｘ，ｙ）＝Ｆ_ｒＭＡＸ−Ｆ_ｒ（ｘ，ｙ） ・・・（２）

Ｐ_ｔ＋ｒ（ｘ，ｙ）＝Ｐ_ｔ（ｘ，ｙ）＋Ｐ_ｒ（ｘ，ｙ） ・・・（３）

ここで、Ｐ_ｔ（ｘ，ｙ）は、作業領域内のある点を示す座標（ｘ，ｙ）における可動ポテンシャルであり、並進可動領域に基づいて算出される可動ポテンシャル（以下、並進可動ポテンシャルと称する）を表す。また、Ｐ_ｒ（ｘ，ｙ）は、作業領域内のある点を示す座標（ｘ，ｙ）における可動ポテンシャルであり、回転可動領域に基づいて算出される可動ポテンシャル（以下、回転可動ポテンシャルと称する）を表す。また、Ｐ_ｔ＋ｒ（ｘ，ｙ）は、作業領域内のある点を示す座標（ｘ，ｙ）における可動ポテンシャルであり、並進可動ポテンシャルと回転可動ポテンシャルを合算することにより算出される可動ポテンシャルである。以下の説明においては、単に可動ポテンシャルと称した場合、上記の式（３）によって算出されるＰ_ｔ＋ｒ（ｘ，ｙ）を示す。

上記の式（１）は、並進可動領域の最大値Ｆ_ｔＭＡＸから、作業領域内のある点を示す座標（ｘ，ｙ）における並進可動領域Ｆ_ｔ（ｘ，ｙ）を減算した値を、座標（ｘ，ｙ）における並進可動ポテンシャルとして定義する式である。このように並進可動ポテンシャルを定義することで、作業領域内の点のうち並進可動ポテンシャルが大きな点では、エンドエフェクターＥにより把持された操作対象Ｎの剛性が大きい、すなわち操作対象Ｎを動かし難い（並進させ難い）ことを表すことができる。このように操作対象Ｎを動かし難いということは、換言すると、操作対象Ｎを並進させることによって組付対象Ｏや他の物体と干渉しやすいことを示している。

また、上記の式（２）は、回転可動領域の最大値Ｆ_ｒＭＡＸから、作業領域内のある点を示す座標（ｘ，ｙ）における回転可動領域Ｆ_ｒ（ｘ，ｙ）を減算した値を、座標（ｘ，ｙ）における回転可動ポテンシャルとして定義する式である。このように回転可動ポテンシャルを定義することで、作業領域内の点のうち回転可動ポテンシャルが大きな点では、エンドエフェクターＥにより把持された操作対象Ｎの剛性が大きい、すなわち操作対象Ｎを動かし難い（回転させ難い）ことを表すことができる。このように操作対象Ｎを動かし難いということは、換言すると、操作対象Ｎを回転させることによって組付対象Ｏや他の物体と干渉しやすいことを示している。

すなわち、上記の式（３）により算出される可動ポテンシャルＰ_ｔ＋ｒ（ｘ，ｙ）が大きな点は、エンドエフェクターＥにより把持された操作対象Ｎの剛性が大きい、すなわち操作対象Ｎを動かし難い（並進又は回転させ難い）ことを表す。このように操作対象Ｎを動かし難いということは、換言すると、操作対象Ｎを並進又は回転させることによって組付対象Ｏや他の物体と干渉しやすいことを示している。

図１０（Ａ）は、このようにして算出された可動ポテンシャルの一例を表すグラフを示す図である。このグラフから、ポテンシャル算出部３８が算出した可動ポテンシャルは、その大きさが不連続に変化することが分かる。これは、可動ポテンシャルを算出するために用いる並進可動ポテンシャル及び回転可動ポテンシャルの大きさがそれぞれ不連続に変化するためである。

ここで、可動ポテンシャルに関連して、図１０（Ａ）の場合以外の例を示しておく。
図１０（Ｂ）は並進可動ポテンシャルの一例を表すグラフを示す図であり、図１０（Ｃ）は回転可動ポテンシャルの一例を表すグラフを示す図である。なお、図１０（Ａ）では並進可動ポテンシャルと回転可動ポテンシャルとが合成された結果である可動ポテンシャルの例を示したが、ここでは、他の例として、並進可動ポテンシャルと回転可動ポテンシャルとのそれぞれの例を別々に示してある。前記したように、この例は、図１０（Ａ）の場合とは異なる例であり、図１０（Ｂ）及び図１０（Ｃ）に示される並進可動ポテンシャル及び回転可動ポテンシャルが合成された結果は、図１０（Ａ）に示される可動ポテンシャルとは異なる可動ポテンシャルとなる。図１０（Ｂ）及び図１０（Ｃ）に示される例において、並進可動ポテンシャル及び回転可動ポテンシャルの大きさは不連続に変化している。

以下、図１１を参照し、図５に示したステップＳ１２０の距離ポテンシャル算出処理について説明する。図１１は、図５に示したステップＳ１２０の距離ポテンシャル算出処理の流れの一例を示すフローチャートである。なお、図１１に示したフローチャートにおいて、ステップＳ１９０及びステップＳ２００の処理は、図６に示したフローチャートにおけるステップＳ１９０及びステップＳ２００の処理と同様の処理であるため、説明を省略する。

ただし、図１１に示したステップＳ１９０及びステップＳ２００の処理を行う機能部は、図６に示したステップＳ１９０及びステップＳ２００の処理を行う可動領域算出部３７ではなく、ポテンシャル算出部３８である。また、ポテンシャル算出部３８は、図１１に示したステップＳ２００において選択した座標毎に、図１１に示したフローチャートのステップＳ５１０の処理を繰り返し行う。また、ポテンシャル算出部３８は、図５に示したステップＳ１００において既に仮想空間が生成済みであるため、ステップＳ１９０の処理を省略する構成であってもよい。

図１１に示したステップＳ２００において仮想空間の座標を選択した後、ポテンシャル算出部３８は、操作対象Ｎの組付位置を示す情報を記憶部３２から読み込む。なお、以下では、操作対象Ｎの組付位置を示す情報は、予め記憶部３２に記憶されている場合について説明する。ポテンシャル算出部３８は、読み込んだ操作対象Ｎの組付位置を示す情報と、ステップＳ２００において選択した座標とに基づいて、当該組付位置から当該座標が示す位置までの間の距離を、並進距離として算出する（ステップＳ５１０）。

ここで、図１２を参照し、ステップＳ５１０の処理について説明する。図１２は、操作対象Ｎの組付位置を示す点と、ポテンシャル算出部３８がステップＳ５１０において選択した座標が示す仮想空間の点とにそれぞれ配置された操作対象Ｎの一例を示す図である。図１２において、点線ＮＳは、組付位置を示す点Ｇ（ｘ_ｓ，ｙ_ｓ）に配置された操作対象Ｎの輪郭を表している。また、実線ＮＣは、ポテンシャル算出部３８がステップＳ５１０において選択した座標が示す仮想空間の点Ｇ（ｘ_ｃ，ｙ_ｃ）に配置された操作対象Ｎの輪郭を表している。

図１２に示したように、ポテンシャル算出部３８は点Ｇ（ｘ_ｓ，ｙ_ｓ）と点Ｇ（ｘ_ｃ，ｙ_ｃ）の間の距離Ｌを、前述の並進距離として算出する。なお、図１２では、ステップＳ５１０の処理を説明するために仮想空間ＶＳ内に操作対象Ｎを配置した様子を例にとって説明したが、ポテンシャル算出部３８は、仮想空間ＶＳ内に操作対象Ｎを配置せずに、操作対象Ｎの組付位置を示す点の仮想空間内における座標と、ポテンシャル算出部３８がステップＳ５１０において選択した座標とに基づいて並進距離を算出する構成であってもよい。以下では、仮想空間ＶＳ内での各点を示す座標を座標（ｘ，ｙ）とした場合、その座標における並進距離Ｓ_ｔの値を並進距離Ｓ_ｔ（ｘ，ｙ）と表す。

このように、ポテンシャル算出部３８は、図１１に示したステップＳ２００からステップＳ５１０までの処理を繰り返し行うことにより、操作対象Ｎの組付位置と、仮想空間内の全ての点それぞれとの間の距離を、並進距離Ｓ_ｔ（ｘ，ｙ）として算出する。

次に、ポテンシャル算出部３８は、ステップＳ５１０において算出した並進距離Ｓ_ｔ（ｘ，ｙ）に基づいて、並進距離Ｓ_ｔ（ｘ，ｙ）の全ての点の中での最大値Ｓ_ｔＭＡＸを検出する（ステップＳ５２０）。

次に、ポテンシャル算出部３８は、ステップＳ５２０において検出した並進距離Ｓ_ｔ（ｘ，ｙ）の最大値Ｓ_ｔＭＡＸに基づいて、作業領域内の各点における距離ポテンシャルを算出する（ステップＳ５３０）。ここで、この距離ポテンシャルを算出する処理について説明する。ポテンシャル算出部３８は、以下に示す式（４）に基づいて作業領域内の各点における距離ポテンシャルを算出する。

ＰＴ_ｔ（ｘ，ｙ）＝Ｓ_ｔＭＡＸ−Ｓ_ｔ（ｘ，ｙ） ・・・（４）

ここで、ＰＴ_ｔ（ｘ，ｙ）は、作業領域内のある点を示す座標（ｘ，ｙ）における距離ポテンシャルであり、並進距離に基づいて算出される距離ポテンシャル（以下、並進距離ポテンシャルと称する）を表す。上記の式（４）は、並進距離の最大値Ｓ_ｔＭＡＸから、作業領域内のある点を示す座標（ｘ，ｙ）における並進距離Ｓ_ｔ（ｘ，ｙ）を減算した値を、座標（ｘ，ｙ）における並進距離ポテンシャルとして定義する式である。このように並進距離ポテンシャルを定義することで、作業領域内の点のうち並進距離ポテンシャルが大きな点は、操作対象Ｎが組付位置に近い、すなわち操作対象Ｎが組付対象Ｏに近いことを表すことができる。

ロボット２０は、例えば、操作対象Ｎと組付対象Ｏの間の他の物体が無い場合、操作対象Ｎが組付対象Ｏから遠い位置では、操作対象Ｎが組付対象Ｏに接触しないために速く操作対象Ｎを移動させることができる。また、ロボット２０は、操作対象Ｎが組付対象Ｏに近い位置では、操作対象Ｎが組付対象Ｏに接触してしまう可能性が高いために遅く操作対象Ｎを移動させた方が望ましい。このことから、座標（ｘ，ｙ）における並進距離ポテンシャルＰＴ_ｔ（ｘ，ｙ）の値は、座標（ｘ，ｙ）においてロボット２０が操作対象Ｎを移動させることが可能な速さの指標とすることができる。

図１３は、このようにして算出された並進距離ポテンシャルの一例を表すグラフを示す図である。図１３には、並進距離ポテンシャルの一例を表すグラフを示す。図１３に示したグラフから、ポテンシャル算出部３８が算出した並進距離ポテンシャルは、上に凸の形状のグラフとなり、操作対象Ｎの組付位置から遠ざかるほどに値が小さくなることが分かる。なお、図１３に示したグラフにおける最大値は、操作対象Ｎの組付位置における並進距離ポテンシャルの値である。

このように、ポテンシャル算出部３８は、図１１に示したステップＳ２００からステップＳ５３０までの処理を実行することにより、並進距離ポテンシャルを算出することができる。

図１４は、ステップＳ１３０において可動ポテンシャルと並進距離ポテンシャルを合算することによりポテンシャル算出部３８が算出した経路ポテンシャルの一例を表すグラフを示す図である。図１０に示したように可動ポテンシャルの大きさが不連続に変化しているため、図１４に示したようにポテンシャル算出部３８が算出した経路ポテンシャルは、その大きさが不連続に変化する。

ここで、制御装置３０が備える制御部３６の平滑化処理部３９は、図５に示したステップＳ１４０でこの不連続に変化する経路ポテンシャルにローパスフィルターを適用することで、図１５に示すような滑らかな経路ポテンシャルを生成する。図１５は、平滑化処理部３９が生成した滑らかな経路ポテンシャルの一例を表すグラフを示す図である。平滑化処理部３９が図１５に示すような滑らかな経路ポテンシャルを生成するため、ロボット制御部４３は、滑らかな経路ポテンシャルに基づいて、ロボット２０に所定の作業を行わせることができる。その結果、マニピュレーターＭＮＰのアクチュエーターに掛かる負荷を抑制することができる。

ここで、図１５に示した経路ポテンシャルを参照し、図５に示したステップＳ１５０の経路生成処理について詳しく説明する。図１５に示した経路ポテンシャルの値が他の位置に比べて相対的に低い位置は、操作対象Ｎが組付対象Ｏや他の物体と干渉する可能性が低い位置であり、且つ組付対象Ｏから遠いために速く移動させることができる位置である。また、経路ポテンシャルの値が他の位置に比べて相対的に高い位置は、操作対象Ｎが組付対象Ｏや他の物体と干渉する可能性が高い位置であり、且つ組付対象Ｏに近いために遅く移動させなければならない位置である。

そのため、ロボット２０が操作対象Ｎを組付対象Ｏに組み付けるために移動させる際、ロボット２０は、経路ポテンシャルの値が低い位置を経由して移動させることによって、操作対象Ｎと他の物体との干渉の抑制及び操作対象Ｎを移動させる時間の短縮を図ることができる。

これを実現するための方法として、ステップＳ１５０における経路生成部４０は、経路ポテンシャルの値を参照し、操作対象Ｎの組付位置に隣接する位置のうち、経路ポテンシャルの値が最も小さな位置を経由点として特定する。経由点は、ロボット２０が操作対象Ｎを初期の位置から組付位置まで移動させる際に操作対象Ｎが経由する点のことである。以下では、説明の便宜上、操作対象Ｎの初期の位置を、初期位置と称して説明する。

ここで、ある位置Ｘに隣接する位置とは、作業領域内の位置を示す座標が隣接する位置のことである。この一例において、作業領域内の位置は、図２に示した座標軸によって表される二次元平面上の位置であり、当該座標軸の座標値によって示される。この座標値を変化させる場合、その変化量は、数学的には無限に小さな値にすることができる。しかし、実際の制御においては、当該変化量は、無限に小さな値にすることができず、ある有限の最小値にしなければならない。そのため、この一例における作業領域での位置は、図２に示した座標軸の座標値によって示される複数の格子点によって表される。すなわち、ある位置Ｘに隣接する位置とは、位置Ｘを表す格子点に隣接する他の格子点が表す位置のことである。以下では、説明の便宜上、ある位置Ｘに隣接する位置を、位置Ｘの隣接位置と称して説明する。

経路生成部４０は、組付位置に隣接する経由点を特定した後、当該経由点に隣接する位置のうち、経路ポテンシャルの値が最も小さな座標を次の経由点として特定する。これを繰り返し行うことにより、経路生成部４０は、組付位置から初期位置までの間を複数の経由点を介して繋ぐ。そして、経路生成部４０は、組付位置と、初期位置と、これらの複数の経由点とのうち、隣接する位置同士を線によって結ぶことにより、操作対象Ｎを組付対象Ｏに組み付けるまでの経路を生成する。

なお、経路生成部４０は、経由点を特定する際、経路ポテンシャルの値が同じ位置が２つ以上ある場合、例えば、２つ以上ある当該位置のうち、操作対象Ｎの初期位置との間の距離が最も近い位置を次の経由点として特定する。また、経路生成部４０は、この一例において、一度経由した経由点を再び経由点として特定することを行わない場合について説明したが、これに代えて、一度経由した経由点を再び経由点として特定する構成であってもよい。この場合、経路生成部４０は、例えば、前回に特定した経由点を次の経由点として選択しない等、ある２つの経由点の間で経由点の特定が繰り返し行われてしまうことを抑制する必要がある。

ここで、図５に示したステップＳ１６０の速さ決定処理について詳しく説明する。速さ決定部４１は、ステップＳ１５０において経路生成部４０が特定した経由点のそれぞれにおいてロボット２０が操作対象Ｎを移動させる速さを決定する。この決定の際、速さ決定部４１は、例えば、図１５に示した経路ポテンシャルの値を参照し、複数の経由点のそれぞれにおける経路ポテンシャルの値の逆数に所定の速さを乗じた値を、複数の経由点のそれぞれにおいてロボット２０が操作対象Ｎを移動させる速さとして算出する。

なお、速さ決定部４１は、複数の経由点のそれぞれにおける経路ポテンシャルの値の逆数に所定の速さを加算した値を、複数の経由点のそれぞれにおいてロボット２０が操作対象Ｎを移動させる速さとして算出する方法や、複数の経由点のそれぞれにおける経路ポテンシャルの値の逆数に所定の重みを乗じた値に所定の速さを乗じた値を、複数の経由点のそれぞれにおいてロボット２０が操作対象Ｎを移動させる速さとして算出する方法等、経路ポテンシャルの値の逆数に基づく他の算出方法によって、複数の経由点のそれぞれにおいてロボット２０が操作対象Ｎを移動させる速さを算出する構成であってもよい。また、速さ決定部４１は、複数の経由点のそれぞれにおいてロボット２０が操作対象Ｎを移動させる速さに上限を設けてもよい。例えば、速さ決定部４１は、複数の経由点のそれぞれにおける経路ポテンシャルの値の逆数の値のうち、所定の上限値を超えている値を所定の上限値に変更することにより、複数の経由点のそれぞれにおいてロボット２０が操作対象Ｎを移動させる速さに上限を設ける構成であってもよい。

ここで、速さ決定部４１が決定した経路点毎の速さは、隣接する経路点同士の間で不連続に変化している。すなわち、速さ決定部４１が決定した不連続に変化している速さに基づいてロボット２０が操作対象Ｎを移動させた場合、ロボット２０が備えるアクチュエーター等に余計な負荷が掛かってしまうことがある。これを抑制するため、速さ決定部４１は、経路点毎に決定した速さが、隣接する経路点同士の間で連続的に（滑らかに）変化するように、経路点毎に決定した速さの変化を表す関数を、例えば、５次多項式を用いてフィッティングすることにより平滑化（平均化）する。なお、速さ決定部４１は、５次多項式に代えて、他の多項式や特殊関数、三角関数等を用いてフィッティングする構成であってもよい。

以上説明したように、第１実施形態におけるロボットシステム１（あるいは、ロボット２０）は、アームとともに動く操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢、及び操作対象Ｎの初期位置に基づいて経路を生成する。これにより、ロボットシステム１は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢、及び操作対象Ｎの組付位置に基づく経路に沿って操作対象Ｎを移動させることができる。その結果、ロボットシステム１は、例えば、他の物体との干渉の抑制及び操作対象Ｎを移動させる時間の短縮を図ることができる。

また、ロボットシステム１は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢に基づく操作対象Ｎの移動可能な範囲を用いて経路を生成する。これにより、ロボットシステム１は、操作対象Ｎの移動可能な範囲を用いて生成した経路に沿って操作対象Ｎを移動させることができる。

また、ロボットシステム１は、操作対象Ｎの移動可能な範囲に基づいて、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢に応じた操作対象Ｎの動かし難さ又は動かし易さの指標となる量である可動ポテンシャルを算出し、算出された可動ポテンシャルを用いて経路を生成する。これにより、ロボットシステム１は、可動ポテンシャルを用いて生成した経路に沿って操作対象Ｎを移動させることができる。

また、ロボットシステム１は、操作対象Ｎの組付位置と操作対象Ｎの作業領域内における他の位置とを用いて経路を生成する。これにより、ロボットシステム１は、操作対象Ｎの組付位置と操作対象Ｎの作業領域内における他の位置とを用いて生成した経路に沿って操作対象Ｎを移動させることができる。

また、ロボットシステム１は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢と操作対象Ｎの組付位置に基づいてアームを移動させる速さを決定する。これにより、ロボットシステム１は、操作対象Ｎと組付対象Ｏとの相対位置姿勢と操作対象Ｎの組付位置に基づいて決定した速さによって操作対象Ｎを移動させることができる。

また、ロボットシステム１は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢に基づく操作対象Ｎの移動可能な範囲を用いてアームを移動させる速さを決定する。これにより、ロボットシステム１は、操作対象Ｎの移動可能な範囲を用いて決定した速さによって操作対象Ｎを移動させることができる。

また、ロボットシステム１は、操作対象Ｎと組付対象Ｏとの相対的な位置及び姿勢に応じた操作対象Ｎの動かし難さ又は動かし易さの指標となる量である可動ポテンシャルを算出し、算出された可動ポテンシャルを用いてアームを移動させる速さを決定する。これにより、ロボットシステム１は、可動ポテンシャルを用いて決定した速さによって操作対象Ｎを移動させることができる。

また、ロボットシステム１は、操作対象Ｎの組付位置と操作対象Ｎの作業領域内における他の位置とを用いてアームを移動させる速さを決定する。これにより、ロボットシステム１は、操作対象Ｎの組付位置と操作対象Ｎの作業領域内における他の位置とを用いて決定した速さによって操作対象Ｎを移動させることができる。

＜第２実施形態＞
以下、本発明の第２実施形態について、図面を参照して説明する。なお、第２実施形態では、第１実施形態と同様な構成部には、同じ符号を付してある。
第２実施形態では、制御部３６は、可動領域算出部３７とポテンシャル算出部３８に代えて、可動領域算出部３７ａとポテンシャル算出部３８ａを備える。

可動領域算出部３７ａは、可動領域算出部３７によるステップＳ１００の可動領域算出処理に代えて、以下で説明する可動領域算出処理を行う。より具体的には、可動領域算出部３７ａは、ステップＳ１００において、図１６に示す並進可動領域算出処理と、図１７に示す回転可動領域算出処理とを行う。

ポテンシャル算出部３８ａは、ポテンシャル算出部３８によるステップＳ１２０の距離ポテンシャル算出処理に代えて、以下で説明する距離ポテンシャル算出処理を行う。より具体的には、ポテンシャル算出部３８ａは、ステップＳ１２０において、図１８に示す距離ポテンシャル算出処理を行う。

以下、図１６を参照して、可動領域算出部３７ａによる並進可動領域算出処理について説明する。図１６は、可動領域算出部３７ａによる並進可動領域算出処理の流れの一例を示すフローチャートである。図１６に示した並進可動領域算出処理は、図６に示した並進可動領域を位置の関数として算出する並進可動領域算出処理と異なり、並進可動領域を位置と姿勢の関数として算出する。

まず、可動領域算出部３７ａは、作業領域を示す情報を記憶部３２から読み込み、読み込まれた作業領域を示す情報に基づいて作業領域を表す仮想空間を生成する。そして、可動領域算出部３７ａは、生成された仮想空間の各点を示す座標を１つずつ選択し、選択された座標毎に、ステップＳ６１０からステップＳ６５０までの処理を繰り返し行う（ステップＳ６００）。

次に、可動領域算出部３７ａは、ステップＳ６００で選択された座標が示す仮想空間の点に、仮想的に操作対象Ｎを配置する。この時、可動領域算出部３７ａは、仮想空間の点に操作対象Ｎの重心の位置が一致するように操作対象Ｎが配置する。また、可動領域算出部３７ａは、仮想空間の点に配置された操作対象Ｎの姿勢を、所定の初期姿勢となるまで操作対象Ｎを回転させる。そして、可動領域算出部３７ａは、操作対象Ｎの姿勢を所定角度ずつ変え、それぞれの姿勢毎にステップＳ６２０からステップＳ６５０までの処理を繰り返し行う（ステップＳ６１０）。

次に、可動領域算出部３７ａは、数値を示す情報を格納可能な変数Ｄを生成し、変数Ｄの値をゼロに初期化する（ステップＳ６２０）。次に、可動領域算出部３７ａは、ステップＳ６００で選択された座標が示す仮想空間の点に、ステップＳ６１０で選択された姿勢で配置された操作対象Ｎが４つの方向（すなわち、ｘ軸の正方向、ｘ軸の負方向、ｙ軸の正方向、ｙ軸の負方向）のそれぞれへの移動可能な距離の和を算出する（ステップＳ６３０）。次に、可動領域算出部３７ａは、ステップＳ６３０で算出された４つの方向への移動可能な距離の和を、ステップＳ６２０で生成された変数Ｄに格納されている値に加算する（ステップＳ６４０）。

以下、図１７を参照して、可動領域算出部３７ａによる回転可動領域算出処理について説明する。図１７は、可動領域算出部３７ａによる回転可動領域算出処理の流れの一例を示すフローチャートである。図１７に示した回転可動領域算出処理は、図７に示した回転可動領域を位置の関数として算出する回転可動領域算出処理と異なり、回転可動領域を位置と姿勢の関数として算出する。

まず、可動領域算出部３７ａは、作業領域を示す情報を記憶部３２から読み込み、読み込まれた作業領域を示す情報に基づいて作業領域を表す仮想空間を生成する。そして、可動領域算出部３７ａは、生成された仮想空間の各点を示す座標を１つずつ選択し、選択された座標毎に、ステップＳ７１０からステップＳ８５０までの処理を繰り返し行う（ステップＳ７００）。なお、可動領域算出部３７ａにより回転可動領域算出処理が図１３に示した並進可動領域算出処理の後に（又は並列に）行われる場合、すでに仮想空間が生成されているため、ステップＳ７００において新たに仮想空間を生成しなくてもよい。

次に、可動領域算出部３７ａは、ステップＳ７００で選択された座標が示す仮想空間の点に、仮想的に操作対象Ｎを配置する。この時、可動領域算出部３７ａは、仮想空間の点に操作対象Ｎの重心の位置が一致するように操作対象Ｎが配置する。また、可動領域算出部３７ａは、仮想空間の点に配置された操作対象Ｎの姿勢を、所定の初期姿勢となるまで操作対象Ｎを回転させる。そして、可動領域算出部３７ａは、操作対象Ｎの姿勢を所定角度ずつ変え、それぞれの姿勢毎にステップＳ７２０からステップＳ８５０までの処理を繰り返し行う（ステップＳ７１０）。

次に、可動領域算出部３７ａは、数値を示す情報を格納可能な変数Ｄ＋を生成し、変数Ｄ＋の値をゼロに初期化する（ステップＳ７２０）。次に、可動領域算出部３７ａは、ステップＳ７００で選択された座標が示す仮想空間の点に配置された操作対象Ｎであって、現在の姿勢の操作対象Ｎが、図８に示した仮想空間ＶＳの枠や組付対象Ｏの輪郭に干渉（すなわち、接触）しているか否かを判定する（ステップＳ７３０）。

操作対象Ｎが仮想空間ＶＳの枠や組付対象Ｏの輪郭に接触していないと判定した場合（ステップＳ７３０−Ｎｏ）、可動領域算出部３７ａは、ステップＳ７５０における操作対象Ｎの回転に係る回転角の累積値が３６０°に達したか否かを判定する（ステップＳ７４０）。操作対象Ｎの回転に係る回転角の累積値が３６０°に達していないと判定した場合（ステップＳ７４０−Ｎｏ）、可動領域算出部３７ａは、ステップＳ７００で選択された座標が示す仮想空間の点に配置された操作対象Ｎであって、現在の姿勢の操作対象Ｎを、例えば、操作対象Ｎの重心を中心として正方向に所定角度だけ回転させ（ステップＳ７５０）、再びステップＳ７３０の判定を行う。ここで、所定角度は、その倍数に３６０°を含む任意の角度であることが望ましく、この一例において、５°や１０°等であるとするが、その倍数に３６０°を含まない他の角度であってもよい。一方、操作対象Ｎの回転に係る回転角の累積値が３６０°に達していると判定した場合（ステップＳ７４０−Ｙｅｓ）、可動領域算出部３７ａは、ステップＳ７５０における操作対象Ｎの回転に係る回転角の累積値を変数Ｄ＋に代入する（ステップＳ７６０）。

一方、ステップＳ７３０で操作対象Ｎが仮想空間ＶＳの枠や組付対象Ｏの輪郭に干渉（すなわち、接触）していると判定した場合（ステップＳ７３０−Ｙｅｓ）、可動領域算出部３７ａは、ステップＳ７６０に遷移し、ステップＳ７５０における操作対象Ｎの回転に係る回転角の累積値を変数Ｄ＋に代入する。次に、可動領域算出部３７ａは、ステップＳ７００で選択された座標が示す仮想空間の点に配置された操作対象Ｎの姿勢を、ステップＳ７１０で選択された姿勢に戻す（ステップＳ７７０）。

次に、可動領域算出部３７ａは、数値を示す情報を格納可能な変数Ｄ−を生成し、変数Ｄ−の値をゼロに初期化する（ステップＳ７８０）。次に、可動領域算出部３７ａは、ステップＳ７００で選択された座標が示す仮想空間の点に配置された操作対象Ｎであって、現在の姿勢の操作対象Ｎが、図８に示した仮想空間ＶＳの枠や組付対象Ｏの輪郭に干渉（すなわち、接触）しているか否かを判定する（ステップＳ７９０）。

操作対象Ｎが仮想空間ＶＳの枠や組付対象Ｏの輪郭に接触していないと判定した場合（ステップＳ７９０−Ｎｏ）、可動領域算出部３７ａは、ステップＳ８１０における操作対象Ｎの回転に係る回転角の絶対値の累積値が３６０°に達したか否かを判定する（ステップＳ８００）。操作対象Ｎの回転に係る回転角の累積値が３６０°に達していないと判定した場合（ステップＳ８００−Ｎｏ）、可動領域算出部３７ａは、ステップＳ７００で選択された座標が示す仮想空間の点に配置された操作対象Ｎであって、現在の姿勢の操作対象Ｎを、例えば、操作対象Ｎの重心を中心として負方向に所定角度だけ回転させ（ステップＳ８１０）、再びステップＳ７９０の判定を行う。ここで、所定角度は、その倍数に３６０°を含む任意の角度であることが望ましく、この一例において、５°や１０°等であるとするが、その倍数に３６０°を含まない他の角度であってもよい。一方、操作対象Ｎの回転に係る回転角の累積値が３６０°に達していると判定した場合（ステップＳ８００−Ｙｅｓ）、可動領域算出部３７ａは、ステップＳ８１０における操作対象Ｎの回転に係る回転角の累積値を変数Ｄ−に代入する（ステップＳ８２０）。

一方、ステップＳ７９０で操作対象Ｎが仮想空間ＶＳの枠や組付対象Ｏの輪郭に干渉（すなわち、接触）していると判定した場合（ステップＳ７９０−Ｙｅｓ）、可動領域算出部３７ａは、ステップＳ８２０に遷移し、ステップＳ８１０における操作対象Ｎの回転に係る回転角の累積値を変数Ｄ−に代入する。次に、可動領域算出部３７ａは、変数Ｄ＋に代入されている値が変数Ｄ−に代入されている値未満であるか否かを判定する（ステップＳ８３０）。変数Ｄ＋に代入されている値が変数Ｄ−に代入されている値未満であると判定した場合（ステップＳ８３０−Ｙｅｓ）、可動領域算出部３７ａは、変数Ｄ＋の値を回転可動領域に代入する（ステップＳ８４０）。一方、変数Ｄ＋に代入されている値が変数Ｄ−に代入されている値以上であると判定した場合（ステップＳ８３０−Ｎｏ）、可動領域算出部３７ａは、変数Ｄ−の値を回転可動領域に代入する（ステップＳ８５０）。

このように、可動領域算出部３７ａは、ステップＳ６００からステップＳ６５０までの処理と、ステップＳ７００からステップＳ８５０までの処理とをそれぞれ繰り返し行うことで、作業領域内の各点における操作対象Ｎの並進可動領域及び回転可動領域を、操作対象Ｎの姿勢毎に算出する。
なお、ステップＳ７３０において、可動領域算出部３７ａは、変数Ｄ−に代入されている値が変数Ｄ＋に代入されている値未満であるか否かを判定する構成であってもよい。また、可動領域算出部３７ａは、ステップＳ７３０からステップＳ７６０までの処理と、ステップＳ７８０からステップＳ８２０までの処理とを逆の順で行う構成であってもよく、同時に行う構成であってもよい。

以下、図１８を参照して、ポテンシャル算出部３８ａによる距離ポテンシャル算出処理について説明する。図１８は、ポテンシャル算出部３８ａによる距離ポテンシャル算出処理の流れの一例を示すフローチャートである。図１８に示した距離ポテンシャル算出処理は、図１１に示した距離ポテンシャルを位置の関数（すなわち、並進距離ポテンシャル）として算出する距離ポテンシャル算出処理と異なり、距離ポテンシャルを位置と姿勢の関数として算出する。

まず、ポテンシャル算出部３８ａは、作業領域を示す情報を記憶部３２から読み込み、読み込まれた作業領域を示す情報に基づいて作業領域を表す仮想空間を生成する。そして、ポテンシャル算出部３８ａは、生成された仮想空間の各点を示す座標を１つずつ選択し、選択された座標毎に、ステップＳ９１０からステップＳ９３０までの処理を繰り返し行う（ステップＳ９００）。なお、可動領域算出部３７ａにより回転可動領域算出処理が図１６に示した並進可動領域算出処理の後に（又は並列に）行われる場合、すでに仮想空間が生成されているため、ステップＳ９００において新たに仮想空間を生成しなくてもよい。

次に、ポテンシャル算出部３８ａは、ステップＳ９００で選択された座標が示す仮想空間の点に、仮想的に操作対象Ｎを配置する。この時、ポテンシャル算出部３８ａは、仮想空間の点に操作対象Ｎの重心の位置が一致するように操作対象Ｎが配置する。また、ポテンシャル算出部３８ａは、仮想空間の点に配置された操作対象Ｎの姿勢を、所定の初期姿勢となるまで操作対象Ｎを回転させる。そして、ポテンシャル算出部３８ａは、操作対象Ｎの姿勢を所定角度ずつ変え、それぞれの姿勢毎にステップＳ９２０からステップＳ９３０までの処理を繰り返し行う（ステップＳ９１０）。

ポテンシャル算出部３８ａは、作業領域内における操作対象Ｎの組付位置と、ステップＳ９００において選択した座標（ｘ，ｙ）が示す位置との間の距離を並進距離Ｓ_ｔ（ｘ，ｙ）として算出する（ステップＳ９２０）。次に、ポテンシャル算出部３８ａは、例えば、作業領域内における操作対象Ｎの組付位置に対応する操作対象Ｎの組付姿勢を表す角度ベクトル（姿勢ベクトル）θ_ｓと、ステップＳ９２０において選択した操作対象Ｎの姿勢を表す角度ベクトルθとの間の距離を、回転距離Ｓ_ｒ（θ）として以下の式を用いて算出する。

Ｓ_ｒ（θ）＝√（θ_ｓ−θ）^２

ステップＳ９００からステップＳ９３０までの繰り返し処理により、ポテンシャル算出部３８ａは、仮想領域内のすべての位置及び姿勢毎に、並進距離Ｓ_ｔ（ｘ，ｙ）と回転距離Ｓ_ｒ（θ）を算出することができる。

次に、ポテンシャル算出部３８ａは、ステップＳ９００からステップＳ９３０までの繰り返し処理により算出した並進距離Ｓ_ｔ（ｘ，ｙ）及び回転距離Ｓ_ｒ（θ）を合算した合計距離Ｓ_{ｔｏｔａｌ}（ｘ，ｙ，θ）を算出する。そして、ポテンシャル算出部３８ａは、算出した合計距離Ｓ_{ｔｏｔａｌ}（ｘ，ｙ，θ）の全ての点の中での最大値Ｓ_ＭＡＸを検出する（ステップＳ９４０）。

次に、ポテンシャル算出部３８ａは、ステップＳ９４０において検出した合計距離Ｓ_{ｔｏｔａｌ}（ｘ，ｙ，θ）の最大値Ｓ_ＭＡＸに基づいて、作業領域内の各点における距離ポテンシャルを算出する（ステップＳ９５０）。より具体的には、ポテンシャル算出部３８ａは、以下に示す式（５）に基づいて作業領域内の各点における距離ポテンシャルを算出する。

ＰＴ（ｘ，ｙ，θ）＝Ｓ_ＭＡＸ−Ｓ_{ｔｏｔａｌ}（ｘ，ｙ，θ） ・・・（５）

ここで、ＰＴ（ｘ，ｙ，θ）は、作業領域内の位置及び姿勢（ｘ，ｙ，θ）における距離ポテンシャルであり、並進距離及び回転距離に基づいて算出される距離ポテンシャルを表す。上記の式（５）は、合計距離の最大値Ｓ_ＭＡＸから、作業領域内の位置及び姿勢（ｘ，ｙ，θ）における合計距離Ｓ_{ｔｏｔａｌ}（ｘ，ｙ，θ）を減算した値を、当該位置及び姿勢（ｘ，ｙ，θ）における距離ポテンシャルとして定義する式である。このように距離ポテンシャルを定義することで、作業領域内の点のうち距離ポテンシャルが大きな点は、操作対象Ｎが組付位置に近い、すなわち操作対象Ｎが組付対象Ｏに近いことを表すことができる。

以上説明したように、第２実施形態におけるロボットシステム１は、操作対象Ｎの組付位置と、当該組付位置に対応する操作対象Ｎの組付姿勢と、操作対象Ｎの作業領域内における他の位置及び姿勢とを用いて経路を生成する。これにより、ロボットシステム１は、操作対象Ｎの組付位置と、当該組付位置に対応する操作対象Ｎの組付姿勢と、操作対象Ｎの作業領域内における他の位置及び姿勢とを用いて生成した経路に沿って操作対象Ｎを移動させることができる。

また、ロボットシステム１は、操作対象Ｎの組付位置と、当該組付位置に対応する操作対象Ｎの組付姿勢と、操作対象Ｎの作業領域内における他の位置及び姿勢とを用いてアームを移動させる速さを決定する。これにより、ロボットは、操作対象Ｎの組付位置と、当該組付位置に対応する操作対象Ｎの組付姿勢と、操作対象Ｎの作業領域内における他の位置及び姿勢とを用いて決定した速さによって操作対象Ｎを移動させることができる。

なお、組付位置は、第１位置、及び第３位置それぞれの一例である。また、組付姿勢は、第１位置に対応する第１姿勢、及び第３位置に対応する第３姿勢それぞれの一例である。

以上、この発明の実施形態を、図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない限り、変更、置換、削除等されてもよい。

また、以上に説明した装置（例えば、ロボットシステム１の制御装置３０）における任意の構成部の機能を実現するためのプログラムを、コンピューター読み取り可能な記録媒体に記録し、そのプログラムをコンピューターシステムに読み込ませて実行するようにしてもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）や周辺機器等のハードウェアを含むものとする。また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｋ）−ＲＯＭ等の可搬媒体、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバーやクライアントとなるコンピューターシステム内部の揮発性メモリー（ＲＡＭ：Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）のように、一定時間プログラムを保持しているものも含むものとする。

また、上記のプログラムは、このプログラムを記憶装置等に格納したコンピューターシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピューターシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。
また、上記のプログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、上記のプログラムは、前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

１ ロボットシステム、２０ ロボット、３０ 制御装置、３１ ＣＰＵ、３２ 記憶部、３３ 入力受付部、３４ 通信部、３５ 表示部、３６ 制御部、３７ 可動領域算出部、３８ ポテンシャル算出部、３９ 平滑化処理部、４０ 経路生成部、４１ 速さ決定部、４２ 力センサー取得部、４３ ロボット制御部

Claims (12)

1. アームと、
   前記アームを動作させる制御部と、を含み、
   前記制御部は、前記アームとともに動く第１物体と第２物体との相対位置姿勢、及び前記第１物体の第１位置に基づいて経路を生成する、
   ロボット。
2. 前記制御部は、前記相対位置姿勢に基づく前記第１物体の移動可能な範囲を用いて前記経路を生成する、
   請求項１に記載のロボット。
3. 前記制御部は、前記移動可能な範囲に基づいて、前記相対位置姿勢に応じた前記第１物体の動かし難さ又は動かし易さの指標となる量を算出し、算出された前記指標となる量を用いて前記経路を生成する、
   請求項２に記載のロボット。
4. 前記制御部は、前記第１位置と前記第１物体の第２位置とを用いて前記経路を生成する、
   請求項１から３のうちいずれか一項に記載のロボット。
5. 前記制御部は、更に前記第１位置に対応する第１姿勢と、前記第２位置に対応する第２姿勢とを用いて前記経路を生成する、
   請求項４に記載のロボット。
6. 前記制御部は、前記相対位置姿勢と前記第１位置に基づいて前記アームを移動させる速さを決定する、
   請求項１から５のうちいずれか一項に記載のロボット。
7. 前記制御部は、前記相対位置姿勢に基づく前記第１物体の移動可能な範囲を用いて前記速さを決定する、
   請求項６に記載のロボット。
8. 前記制御部は、前記相対位置姿勢に応じた前記第１物体の動かし難さ又は動かし易さの指標となる量を算出し、算出された前記指標となる量を用いて前記速さを決定する、
   請求項７に記載のロボット。
9. 前記制御部は、前記第１物体の第３位置と前記第１物体の第４位置とを用いて前記速さを決定する、
   請求項７又は８に記載のロボット。
10. 前記制御部は、更に前記第３位置に対応する第３姿勢と、前記第４位置に対応する第４姿勢とを用いて前記速さを決定する、
    請求項９に記載のロボット。
11. ロボットが備えるアームとともに動く第１物体と第２物体との相対位置姿勢、及び前記第１物体の第１位置に基づいて経路を生成する、
    制御装置。
12. ロボットが備えるアームとともに動く第１物体と第２物体との相対位置姿勢、及び前記第１物体の第１位置に基づいて経路を生成する、
    制御方法。