WO2023145309A1 - 動作経路生成装置、方法、及びプログラム - Google Patents

Abstract

対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成する。　取得部３２が、ロボット４２の把持部により把持される主対象物９０Ａと、組立対象の副対象物９０Ｂとの組立動作における、把持部の主対象物９０Ａとの相対的な開始位置姿勢及び終了位置姿勢と、環境情報とを取得し、シミュレーション部３４が、取得された情報に基づいて、接触状態から非接触状態になるまでの主対象物９０Ａと副対象物９０Ｂとの接触状態の遷移をシミュレーションし、生成部３６が、取得された情報と、シミュレーションされた接触状態の遷移とに基づいて、初期状態からの接触状態の遷移に含まれるいずれかの接触状態に至り、いずれかの接触状態を含む接触状態の遷移を辿って目標状態に至るロボットの動作経路を生成する。

Description

動作経路生成装置、方法、及びプログラム

　本開示は、動作経路生成装置、動作経路生成方法、及び動作経路生成プログラムに関する。

　従来、ロボットにより把持された主対象物と、組立対象である副対象物とを組み立てる作業を実行するためのロボットの動作経路を生成する技術が提案されている。例えば、物体間のクリアランスが小さい組立プロセスにおける高速挿入を確実にするための多指ハンドが提案されている。この多指ハンドは、６自由度可逆駆動性（バックドライバビリティ）及びオブジェクト姿勢可観測性の特徴を有し、小さな物体同士の高速挿入を達成するために、２つの物体が互いに接触するときに、衝撃低減と位置誤差補償との両方を実現する（国際公開第２０２０／１９４３９３号）。

　また、例えば、開始状態から目標状態へのロボットの動作経路を自動生成する経路出力方法が提案されている。この経路出力方法は、外部インタフェース、記憶部、及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置に搬送する際の経路を出力する方法である。演算部が、外部インタフェースを用いて第１の位置及び第２の位置を取得し、ロボットが対象物を第１の位置から第２の位置に搬送するときの搬送条件を取得する。そして、ロボットが対象物を第１の位置から第２の位置に搬送する経路であって、当該経路の全体にわたり、搬送条件を満たす経路を生成する（特許６８６０８６３号公報）。

　しかしながら、特許文献１に記載の技術では、人が動作経路を生成するため、組立作業をロボットにティーチングする際に、専門的な知識を有する人を必要としていた。また、対象物の位置誤差を調整しながら目標の組立状態に到達するような、いわゆる探り動作が発生する動作経路を立てた場合、探る時間が長く、かつ、ばらつくため、タスクの実行が遅く、そのタスクに要する時間も見積もれない、という問題がある。また、特許文献２に記載の技術では、周辺環境との接触を回避した経路を生成するため、対象物同士の組立を対象とした作業の動作経路の生成に適用することはできない。

　本開示は、上記の点に鑑みてなされたものであり、対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成することを目的とする。

　上記目的を達成するために、本開示に係る動作経路生成装置は、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部と、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部と、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部と、を含んで構成される。

　また、本開示に係る動作経路生成方法は、取得部が、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得し、シミュレーション部が、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションし、生成部が、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する方法である。

　また、本開示に係る動作経路生成プログラムは、コンピュータを、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部、及び、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部として機能させるためのプログラムである。

　本開示に係る動作経路生成装置、方法、及びプログラムによれば、対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成することができる。

ロボット制御システム１の概略構成図である。 本実施形態における動作経路の一例を説明するための図である。 ３次元表現のコンフィギュレーション空間の一例を示す図である。 動作経路生成装置のハードウェア構成を示すブロック図である。 動作経路生成装置の機能構成の例を示すブロック図である。 面接触の一例を示す図である。 面接触の一例を示す図である。 面接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 維持変位及び離脱変位を説明するための図である。 第１提示画面の一例を示す図である。 第２提示画面の一例を示す図である。 動作経路生成処理の一例を示すフローチャートである。 分岐木生成処理の一例を示すフローチャートである。 接触状態の維持範囲の限定を説明するための図である。 分岐木生成処理の具体例を説明するための図である。 分岐木生成処理の具体例を説明するための図である。 分岐木生成処理の具体例を説明するための図である。 分岐木を説明するための図である。 維持範囲と分解能との関係を説明するための図である。 維持範囲と分解能との関係を説明するための図である。 環境との接触を考慮しないロボット制御を説明するための図である。 本開示を実装する対象としての力制御系の実現例の一例としてのアドミタンス制御を説明するための図である。 本開示を実装する対象としての力制御系の実現例の一例としてのインピーダンス制御を説明するための図である。

　以下、本開示の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

　図１に示すように、本実施形態に係るロボット制御システム１は、動作経路生成装置１０と、ロボット制御装置４０と、ロボット４２と、センサ群５０とを含んで構成される。

　ロボット４２は、ロボットアーム４４とハンド部４６とを含む。ハンド部４６は本開示の「把持部」の一例である。ロボットアーム４４は、リンクと、リンク間を接続すると共に、モータの駆動により回転又は直動伸縮するジョイントとを含んで構成される。ロボットアーム４４は、ロボット制御装置４０から出力された指令値にしたがってモータが駆動されて、ジョイントの回転角度又は伸縮状態が変更される。これにより、ハンド部４６が３次元空間において指定された位置、及び指定された姿勢となるように制御される。なお、本実施形態では、水平面内の直交する２方向をＸ軸及びＹ軸、鉛直方向をＺ軸、Ｘ軸周りの回転をφ、Ｙ軸周りの回転をθ、Ｚ軸周りの回転をψとして、位置及び姿勢が表されるものとする。

　ハンド部４６は、ロボットアーム４４の先端に設けられ、主対象物９０Ａを把持可能なツールである。ハンド部４６は、例えば、多関節多指型ロボットハンド、グリッパー型ロボットハンド、吸着パッド等としてよい。

　また、ロボット４２は、好適には、接触による外部環境からの反力によりジョイントやリンクが押し負け、外部環境になじむ振る舞いをするソフトロボットや、そのような性質を備えたハンド部４６を備えたソフトロボットとしてよい。

　センサ群５０は、複数種類のセンサを含み、各センサで取得されたセンサデータは、時系列データとしてロボット制御装置４０へ出力される。なお、図１では、センサ群５０を、ハンド部４６付近に１つのブロックで概念的に表現しているが、センサ群５０に含まれる各センサは、センサの種類及び機能に応じた位置にそれぞれ設けられる。

　各センサとしては、ロボット４２で行う作業に応じて必要なものを備えればよい。ただし、本実施形態では、接触を利用した主対象物９０Ａと副対象物９０Ｂとの組立作業の動作経路に基づいて、ロボット４２の動作が制御される。そのため、主対象物９０Ａと環境との接触時の外力を検知可能なセンサ、例えば力覚センサ、パッドの変形量を計測可能なセンサ（パッドセンサ）等がセンサ群５０には含まれる。また、モータのエンコーダも外力を検知可能なセンサとして利用可能である。この場合、モータのエンコーダの出力値と
モータへの指令値との差分が外力として検知される。センサ群５０には、その他のセンサとして、制御目標に対する主対象物９０Ａの位置及び姿勢に関する相対誤差（以下、「位置姿勢誤差」という）を認識可能なセンサ、例えばビジョンセンサ、近接センサ等が含まれる。なお、パッドセンサは、位置姿勢誤差を認識可能なセンサとしても利用可能である。

　ロボット制御装置４０は、動作経路生成装置１０で生成された動作経路に基づく指令値を生成し、ロボット４２へ出力する。

　ここで、図２を参照して、本実施形態における動作経路の一例について説明する。図２の例は、主対象物９０Ａである円柱状のシャフトを、副対象物９０Ｂである円柱状のリングの穴（挿入位置）に挿入する作業についての動作経路である。図２では、各動作について、主対象物９０Ａと副対象物９０Ｂとの位置関係を示す概略側面図と、その動作時のコンフィギュレーション空間とを並べて図示している。また、図２には、各動作の開始時における主対象物９０Ａであるシャフトの底面右側の位置及び姿勢を表すコンフィギュレーション空間での座標を丸印で示している。

　まず、ハンド部４６により把持された主対象物９０Ａの傾き（θ）が０°で、かつ、主対象物９０Ａと副対象物９０Ｂとが非接触の状態を初期状態とする。動作（１）は、初期状態から、主対象物９０Ａを副対象物９０Ｂの挿入位置の上方へ平行移動させる動作である。動作（２）は、主対象物９０Ａを所定角度（図２の例では、４５°）傾ける動作である。動作（３）は、主対象物９０Ａと副対象物９０Ｂとが接触するまで、主対象物９０Ａを挿入位置に向けて下降させる動作である。動作（３）のコンフィギュレーション空間は、主対象物９０Ａを傾けたことにより、主対象物９０Ａが副対象物９０Ｂの穴の最下部まで到達不可となることを表している。

　動作（４）は、主対象物９０Ａの傾き、及び主対象物９０Ａと副対象物９０Ｂとの接触を維持したまま、主対象物９０Ａの他の箇所が副対象物９０Ｂと接触するまで、主対象物９０Ａを挿入位置方向へ移動させる動作である。動作（５）は、主対象物９０Ａと副対象物９０Ｂとのアライメントを利用して、主対象物９０Ａの傾きを、主対象物９０Ａが副対象物９０Ｂの挿入位置（穴）の内周に沿って下降可能な角度（図２の例では、１０°）まで戻す動作である。動作（６）は、主対象物９０Ａを副対象物９０Ｂの挿入位置（穴）の内周に沿って下方へ移動させる動作である。動作（７）は、主対象物９０Ａを目標の角度ε°（図２の例では、０°）にする動作である。このように、本実施形態では、主対象物９０Ａと環境の１つである副対象物９０Ｂとの接触を利用した動作経路を前提とする。

　なお、図２では、コンフィギュレーション空間を２次元で表しているが、図２におけるコンフィギュレーション空間は、紙面に垂直な方向にθの軸も有している。図３に、３次元表現のコンフィギュレーション空間の一例を示す。図３には、図２の各動作（１）～（７）の開始時における主対象物９０Ａであるシャフトの底面右側の位置及び姿勢を表すコンフィギュレーション空間での座標を丸印で示している。

　動作経路生成装置１０は、シミュレーションにより動作経路を生成し、ロボット制御装置４０へ出力する。図４は、本実施形態に係る動作経路生成装置１０のハードウェア構成を示すブロック図である。図４に示すように、動作経路生成装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）１２、メモリ１４、記憶装置１６、入出力Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）１８、入出力装置２０、記憶媒体読取装置２２、及び通信Ｉ／Ｆ２４を有する。各構成は、バス２６を介して相互に通信可能に接続されている。

　記憶装置１６には、後述する動作経路生成処理を実行するための動作経路生成プログラ
ムが格納されている。ＣＰＵ１２は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ１２は、記憶装置１６からプログラムを読み出し、メモリ１４を作業領域としてプログラムを実行する。ＣＰＵ１２は、記憶装置１６に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

　メモリ１４は、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）により構成され、作業領域として一時的にプログラム及びデータを記憶する。記憶装置１６は、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、及びＨＤＤ（Ｈａｒｄ　Ｄｉｓｋ　Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）等により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

　入出力Ｉ／Ｆ１８は、動作経路生成装置１０とロボット制御装置４０とを接続するためのインタフェースである。動作経路生成装置１０で生成された動作経路は、入出力Ｉ／Ｆ１８を介して、ロボット制御装置４０へ出力される。入出力装置２０は、例えば、キーボードやマウス等の、各種の入力を行うための入力装置、ディスプレイ等の表示装置やプリンタ等の、各種の情報を出力するための出力装置である。出力装置として、タッチパネルディスプレイを採用することにより、入力装置として機能させてもよい。

　記憶媒体読取装置２２は、ＣＤ（Ｃｏｍｐａｃｔ　Ｄｉｓｃ）－ＲＯＭ、ＤＶＤ（Ｄｉｇｉｔａｌ　Ｖｅｒｓａｔｉｌｅ　Ｄｉｓｃ）－ＲＯＭ、ブルーレイディスク、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ　Ｓｅｒｉａｌ　Ｂｕｓ）メモリ等の各種記憶媒体に記憶されたデータの読み込みや、記憶媒体に対するデータの書き込み等を行う。通信Ｉ／Ｆ２４は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

　次に、本実施形態に係る動作経路生成装置１０の機能構成について説明する。

　図５は、動作経路生成装置１０の機能構成の例を示すブロック図である。動作経路生成装置１０には、開始位置におけるハンド部４６の主対象物９０Ａとの相対的な位置及び姿勢の情報（以下、「開始位置姿勢」という）、及び終了位置におけるハンド部４６の主対象物９０Ａとの相対的な位置及び姿勢の情報（以下、「終了位置姿勢」という）が入力される。また、動作経路生成装置１０には、主対象物９０Ａ及び副対象物９０Ｂを含む作業環境の形状及び配置を示す環境情報が入力される。例えば、主対象物９０Ａ及び副対象物９０Ｂの各々のＣＡＤデータ、障害物が配置された作業環境を表すコンフィギュレーション空間等が環境情報に含まれる。また、動作経路生成装置１０には、ロボット４２の動作の分解能を特定するための情報（以下、「分解能特定情報」という）が入力される。分解能特定情報には、センサ群５０に含まれるセンサの分解能、及びロボット４２を駆動するためのアクチュエータの制御精度の少なくとも一方が含まれる。

　図５に示すように、動作経路生成装置１０は、機能構成として、取得部３２と、シミュレーション部３４と、生成部３６と、表示制御部３８とを含む。各機能構成は、ＣＰＵ１２が記憶装置１６に記憶された動作経路生成プログラムを読み出し、メモリ１４に展開して実行することにより実現される。

　取得部３２は、動作経路生成装置１０に入力された開始位置姿勢、終了位置姿勢、及び環境情報を取得し、シミュレーション部３４へ受け渡す。また、取得部３２は、動作経路生成装置１０に入力された分解能特定情報を取得し、生成部３６へ受け渡す。

　シミュレーション部３４は、取得部３２から受け渡された情報に基づいて、主対象物９
０Ａと副対象物９０Ｂとの接触状態から、主対象物９０Ａと副対象物９０Ｂとが非接触状態になるまでの主対象物９０Ａと副対象物９０Ｂとの接触状態の遷移をシミュレーションする。

　ここで、主対象物９０Ａと副対象物９０Ｂとの接触状態について定義する。主対象物９０Ａの面と副対象物９０Ｂの面とが接触する接触状態を面接触という。また、主対象物９０Ａ及び副対象物９０Ｂの少なくとも一方の辺（又は点）と、主対象物９０Ａ及び副対象物９０Ｂの他方とが接触する接触状態を辺（又は点）接触という。以下では、辺（又は点）の「（又は点）」を省略し、単に「辺」という。また、以下では、Ｘ軸に直交する面を「Ｘ面」、Ｙ軸に直交する面を「Ｙ面」、Ｚ軸に直交する面を「Ｚ面」という。さらに、以下の各図では、面接触の接触箇所を太線で表し、辺接触の接触箇所を黒丸で表す。

　図６Ａ～図６Ｃに、面接触の一例を示す。図６Ａでは、主対象物９０Ａと副対象物９０ＢとがＺ面同士で接触しており、図６Ｂでは、主対象物９０Ａと副対象物９０ＢとがＸ面同士で接触している。このように面接触であっても、接触面が異なる場合、図６Ａと図６Ｂとはそれぞれ異なる接触状態の面接触として扱う。また、図６Ａでは、主対象物９０ＡのＺ面全体が副対象物９０Ｂと接触しているのに対し、図６Ｃでは、Ｚ面の一部のみが接触している。このような場合、図６Ａと図６Ｃとはそれぞれ異なる接触状態の面接触として扱う。

　図７Ａ～図７Ｆに、辺接触の一例を示す。図７Ａ及び図７Ｂは、主対象物９０Ａと副対象物９０Ｂとが１辺で接触している辺接触である。このように、１辺で接触している辺接触を、以下では「１辺接触」という。この例において、図７Ａでは、主対象物９０Ａの下側Ｚ面右（＝右側Ｘ面下）の１辺が、副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺と接触している。図７Ｂでは、主対象物９０Ａの下側Ｚ面右（＝右側Ｘ面下）の１辺が副対象物９０Ｂの上側Ｚ面と接触している。これらは、それぞれ異なる接触状態の１辺接触として扱う。

　また、図７Ｃ、図７Ｄ、及び図７Ｅは、主対象物９０Ａと副対象物９０Ｂとが２辺で接触している辺接触である。このように、２辺で接触している辺接触を、以下では「２辺接触」という。この例において、図７Ｃでは、主対象物９０Ａの下側Ｚ面左（＝左側Ｘ面下）の１辺が左の副対象物９０Ｂの右側Ｘ面に接触し、右の副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺が主対象物９０Ａの右側Ｘ面と接触している。また、図７Ｄでは、左の副対象物９０Ｂの上側Ｚ面右（＝右側Ｘ面上）の１辺が主対象物９０Ａの下側Ｚ面に接触し、右の副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺が主対象物９０Ａの右側Ｘ面と接触している。また、図７Ｅでは、主対象物９０Ａの下側Ｚ面左（＝左側Ｘ面下）の１辺が左の副対象物９０Ｂの上側Ｚ面右（＝右側Ｘ面上）の１辺に接触し、右の副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺が主対象物９０Ａの右側Ｘ面と接触している。このように、接触している辺の少なくとも１箇所が異なる場合には、それぞれ異なる接触状態の２辺接触として取り扱う。なお、図７Ｆは、主対象物９０Ａと副対象物９０Ｂとが３辺で接触している３辺接触の例である。

　シミュレーション部３４は、目標状態から開始して、主対象物９０Ａと副対象物９０Ｂとの接触状態が、ある方向に力をかけても動かない状態（２辺接触～面接触状態）に達すると、違う方向に力をかけ続け、それにより動かない状態に達した場合、その状態からまた違う方向に力をかけ続ける、ということを繰り返すことで、接触状態の遷移をシミュレーションする。このような接触状態の遷移を探索する場合において考慮すべき、副対象物９０Ｂに対する主対象物９０Ａの変位には、図８に示すような維持変位及び離脱変位がある。維持変位は、同一の接触状態を維持したまま主対象物９０Ａを変位させることである。図８に示すように、維持変位は、面接触のまま主対象物９０Ａを平行移動させる場合、及び１辺接触又は２辺接触を維持したまま主対象物９０Ａを回転させる場合を含む。離脱
変位は、接触状態の自由度が高くなる方向へ主対象物９０Ａを変位させることである。図８に示すように、離脱変位は、面接触している副対象物９０Ｂとの接触面と交差する方向かつ、副対象物９０Ｂと離れる方向へ主対象物９０Ａを平行移動させる場合を含む。また、離脱変位は、面接触又は２辺接触から１辺接触になるように主対象物９０Ａを回転させる場合を含む。

　シミュレーション部３４は、具体的には、上記のように接触状態の遷移を探索しながら、目標状態を根ノードとし、それぞれ異なる接触状態に対応するノードを有する分岐木を生成する。より具体的には、シミュレーション部３４は、２辺接触、３辺接触、若しくは面接触状態のいずれかの状態から、離脱変位の移動を行った直後の状態、又は、その離脱変位の移動を行った方向に対する維持変位を実行し続けて、新しい接触状態に至るか否かをシミュレーションする。また、シミュレーション部３４は、新しい接触状態に至った場合において、新しい接触状態が２辺接触、３辺接触、又は面接触の場合、その接触状態に対して、同じように探索（分岐木の生成）を実行する。また、シミュレーション部３４は、離脱変位の移動を行った直後の接触状態が１辺接触で、その離脱変位の移動を行った方向に対する維持変位を実行し続けて至る新しい状態が何にも接触していない状態になる場合は１辺接触状態だけを分岐木に登録する。シミュレーション部３４は、分岐木のノードの各々に、そのノードに対応する接触状態において、主対象物９０Ａが維持変位可能な範囲（以下、「維持範囲」）を求め、そのノードにさらに対応付ける。シミュレーション部３４は、維持範囲を、平行移動の方向（Ｘ，Ｙ，Ｚ）、及び回転の方向（φ，θ，ψ）の各方向について求める。

　生成部３６は、取得部３２により取得された情報と、シミュレーション部３４によりシミュレーションされた接触状態の遷移とに基づいて、主対象物９０Ａが開始位置姿勢である初期状態からの接触状態の遷移に含まれるいずれかの接触状態に至り、いずれかの接触状態を含む接触状態の遷移を辿って目標状態に至るロボットの動作経路を生成する。具体的には、生成部３６は、分岐木において、ロボット４２の動作の分解能よりも大きな維持範囲が対応付けられたノードを特定する。生成部３６は、複数のノードが特定される場合、分岐木の根ノードから最も近いノードを特定する。そして、生成部３６は、分岐木において、特定したノードから根ノードまでに対応する接触状態の遷移を特定し、特定した接触状態の遷移に対応するロボット４２の経路を生成する。また、生成部３６は、初期状態の位置から、特定したノードに対応する接触状態までの経路であって、周辺の障害物を回避した経路を生成する。そして、生成部３６は、初期状態から、特定したノードに対応する接触状態までの経路と、特定したノードに対応する接触状態から目標状態までの経路とを結合した経路を動作経路として表示制御部３８へ受け渡す。なお、経路とは、ロボット４２のハンド部４６の所定部分（例えば、手先）の位置及び姿勢の系列である。

　また、生成部３６は、表示制御部３８から、後述するユーザ登録位置姿勢を受け渡され、動作経路の再生成を指示された場合、生成済みの動作経路にユーザ登録位置姿勢を反映して、動作経路を再生成する。詳細は後述するが、ユーザ登録位置姿勢は、動作経路内の特定の時間における主対象物９０Ａの位置及び姿勢の修正が登録されたものである。生成部３６は、生成済みの動作経路が、指定された位置及び姿勢を経由するように修正することにより、動作経路を再生成してよい。また、生成部３６は、ユーザ登録位置姿勢に対応する接触状態が対応付けられたノードを分岐木から探索し、探索されたノードに対応する接触状態を経由する接触状態の遷移に基づいて、動作経路を再生成してもよい。

　表示制御部３８は、生成部３６により生成された動作経路を表示装置に表示する。表示制御部３８は、例えば図９に示すような第１提示画面６０を表示装置に表示する。図９の例では、第１提示画面６０には、３次元ビュー６１が含まれる。３次元ビュー６１では、初期状態から目標状態に至る主対象物９０Ａ及び副対象物９０Ｂの３次元位置（Ｘ，Ｙ，
Ｚ）及び姿勢（φ，θ，ψ）が、３次元のアニメーションでプレビューされる。主対象物９０Ａ及び副対象物９０Ｂ以外の環境の３次元画像も合わせて表示してもよい。また、３次元ビュー６１において、３次元ＣＡＤソフトのように、主対象物９０Ａ及び副対象物９０Ｂの位置及び姿勢をマウスで動かせるようにしてよい。

　また、第１提示画面６０は、３次元ビュー６１で表示されるアニメーションの再生時間が表示される時間表示６２を含む。すなわち、３次元ビュー６１で表示されるアニメーションの各コマに対応付けられた時間のうち、３次元ビュー６１に表示されているコマに対応付けられた時間が時間表示６２に表示される。また、時間表示６２に任意の時間を入力し、その時間に対応するコマを３次元ビュー６１に表示するようにしてもよい。

　また、第１提示画面６０は、フェーズ表示６３を含む。フェーズ表示６３は、動作経路が示す動作の開始から終了までの時間経過を示す帯状の表示等の１軸の表示であり、動作経路において主対象物９０Ａと副対象物９０Ｂとの接触状態が変わる時間に対応する位置に第１マーク（図９のフェーズ表示６３における破線）が表示される。図９の例では、接触状態が変わる時間をそれぞれ「フェーズｉ（ｉ＝１，２，・・・）」としている。また、フェーズ表示６３には、時間表示６２に表示されている時間に対応する位置に第２マーク（図９のフェーズ表示６３における実線及び黒三角）が表示される。第２マークをフェーズ表示６３上で移動させることにより、時間表示６２に表示される時間を指定可能である。また、フェーズ表示６３には、後述するユーザ登録位置姿勢を示す第３マーク（図９のフェーズ表示６３における点線）が、ユーザ登録位置姿勢の対象の時間に対応する位置に表示される。

　また、第１提示画面６０は、登録ボタン６４を含む。ユーザは、特定の時間を指定し、その時間における主対象物９０Ａ及び副対象物９０Ｂの位置及び姿勢を３次元ビュー６１に表示し、マウス操作等により主対象物９０Ａの位置及び姿勢を修正したうえで、登録ボタン６４を選択する。これにより、動作経路内の指定された時間に対して、ユーザにより修正された主対象物９０Ａの位置及び姿勢が登録される。この登録を「ユーザ登録位置姿勢」という。また、第１提示画面６０は、再生成ボタン６５を含む。動作経路に対して位置姿勢のユーザ登録が行われて、ユーザにより再生成ボタン６５が選択された場合、表示制御部３８は、ユーザ登録位置姿勢を受け付け、受け付けたユーザ登録位置姿勢を生成部３６へ受け渡し、動作経路の再生成を指示する。

　また、表示制御部３８は、例えば図１０に示すような第２提示画面７０を表示装置に表示する。図１０の例では、第２提示画面７０には、第１提示画面６０と同様に、時間表示６２、フェーズ表示６３、登録ボタン６４、及び再生成ボタン６５が含まれる。また、第２提示画面７０には、３次元ビュー７１が含まれる。３次元ビュー７１には、位置の３自由度（Ｘ，Ｙ，Ｚ）と姿勢の３自由度（φ，θ，ψ）から選択された３変数を３軸とするコンフィギュレーション空間が表示される。第１提示画面６０の３次元ビュー６１と同様に、３次元ビュー７１においてもユーザによる位置姿勢の登録が可能である。ユーザは、３次元ビュー７１に表示されたコンフィギュレーション空間上で、座標点（図１０の３次元ビュー７１内の黒点）を移動させることにより、主対象物９０Ａの位置及び姿勢を修正可能である。また、第２提示画面７０には、３次元ビュー７１に表示するコンフィギュレーション空間の軸に対応させる３変数を選択するための選択領域７２が含まれる。

　表示制御部３８は、表示装置において、第１提示画面６０及び第２提示画面７０をそれぞれ別ウィンドウで同時に表示してよい。また、表示制御部３８は、選択する３変数がそれぞれ異なる第２提示画面７０を、それぞれ別ウィンドウで複数同時に表示してもよい。表示制御部３８は、第１提示画面６０及び第２提示画面７０を複数ウィンドウで表示した場合、各ウィンドウに対応する第１提示画面６０及び第２提示画面７０の時間表示６２を
同期させ、各ウィンドウの表示を連動させてもよい。

　次に、本実施形態に係るロボット制御システム１の作用について説明する。

　図１１は、動作経路生成装置１０のＣＰＵ１２により実行される動作経路生成処理の流れを示すフローチャートである。ＣＰＵ１２が記憶装置１６から動作経路生成プログラムを読み出して、メモリ１４に展開して実行することにより、ＣＰＵ１２が動作経路生成装置１０の各機能構成として機能し、図１１に示す動作経路生成処理が実行される。

　ステップＳ１０で、取得部３２が、動作経路生成装置１０に入力された開始位置姿勢、終了位置姿勢、環境情報、及び分解能特定情報を取得する。

　次に、ステップＳ１２で、シミュレーション部３４が、上記ステップＳ１０で取得された終了位置姿勢及び環境情報に基づいて、主対象物９０Ａと副対象物９０Ｂとが組み立てられた目標状態を特定する。具体的には、シミュレーション部３４は、終了位置姿勢において主対象物９０Ａと副対象物９０Ｂとが組み立てられた状態のうち、面接触している面の数が最も多い状態を目標状態として特定する。なお、シミュレーション部３４は、面接触している面が１つもない場合、辺接触の数が最も多い状態を目標状態として特定する。目標状態の候補が複数ある場合は、どの状態を目標状態として特定してもよい。

　次に、ステップＳ１４で、シミュレーション部３４が、分岐木生成処理を実行する。ここで、図１２を参照して、分岐木生成処理について説明する。

　ステップＳ１４１で、シミュレーション部３４が、主対象物９０Ａが副対象物９０Ｂに接触しているか否かを判定する。接触している場合には、ループ処理Ｌ１へ移行し、接触していない場合には、分岐木生成処理を終了し、リターンする。

　ループ処理Ｌ１は、ステップＬ１Ｓで開始し、現在の接触状態から、面接触又は辺接触を保ったまま、かつ離脱変位となる方向（回転を含む）への接触状態の遷移（以下、「Ｌ１の遷移」という）の各々について、ステップＳ１４２～Ｓ１４５及びループ処理Ｌ２が実行される。そして、ループ処理Ｌ１は、現在の接触状態について過去に探索した方向を除いて遷移可能な方向がなくなった場合に、ステップＬ１Ｅで終了する。

　ステップＳ１４２では、シミュレーション部３４が、Ｌ１の遷移後の接触状態が分岐木に登録済みか否かを判定する。登録済みの場合には、ステップＳ１４６へ移行し、登録されていない場合には、ステップＳ１４３へ移行する。ステップＳ１４３では、シミュレーション部３４が、Ｌ１の遷移後の接触状態の維持範囲を求める。そして、シミュレーション部３４が、分岐木における、Ｌ１の遷移前の接触状態に対応するノードの下流に接続するノードに、Ｌ１の遷移後の接触状態及び維持範囲を対応付けて登録する。

　Ｌ１の遷移後の接触状態の維持範囲を求める際、シミュレーション部３４は、Ｌ１の遷移を戻す方向の遷移を実行した際に、Ｌ１の遷移前と同じ接触状態に戻ることが可能な範囲に維持範囲を限定する。例えば、図１３の（ａ）に示すように、Ｌ１の遷移前の接触状態が、主対象物９０Ａの下側Ｚ面の一部が副対象物９０Ｂに接触している面接触であるとする。その状態から、（ｂ）に示すように、主対象物９０Ａの下側Ｚ面右（＝右側Ｘ面下）と、副対象物９０Ｂの上側Ｚ面との１辺接触を保って主対象物９０Ａを回転させることにより、接触状態を遷移させたとする。このＬ１の遷移後の接触状態から、維持変位として、（ｃ）に示すようにＸ軸方向に主対象物９０Ａを平行移動させるとする。この場合、維持変位ではあるが、（ｃ）の状態からＬ１の遷移である回転を戻した場合、主対象物９０Ａの下側Ｚ面の全面が副対象物９０Ｂに接触する面接触となり、（ａ）とは異なる接触
状態となる。したがって、この場合、（ｃ）の状態は（ｂ）の維持範囲には含まれない。

　次に、ステップＳ１４４で、シミュレーション部３４が、Ｌ１の遷移後の接触状態は１辺接触か否かを判定する。１辺接触の場合にはループ処理Ｌ２へ移行し、１辺接触ではない場合、すなわち２辺以上の辺接触又は面接触の場合には、ステップＳ１４５へ移行する。ステップＳ１４５では、シミュレーション部３４が、Ｌ１の遷移後の接触状態を分岐木生成処理の開始時の接触状態として、分岐木生成処理を再帰的に実行する。

　ステップＳ１４６では、シミュレーション部３４が、Ｌ１の遷移後の接触状態が１辺接触以外の場合、分岐木に既に登録されている、Ｌ１の遷移後の接触状態に対応するノードと、今回登録するノードとで、分岐木の根ノードに近い方のノードに、根ノードから遠い方のノードから葉ノードまでの部分木を付け替える。そして、ループ処理Ｌ２へ移行する。なお、Ｌ１の遷移後の接触状態が１辺接触の場合、本ステップの処理はスキップする。

　ループ処理Ｌ２は、ステップＬ２Ｓで開始し、同じ方向への遷移で新しい面接触又は２辺以上の辺接触となる接触状態の遷移（以下、「Ｌ２の遷移」という）の各々について、ステップＳ１４７～Ｓ１５０が実行される。そして、ループ処理Ｌ２は、新しい面接触又は２辺以上の辺接触に至らなくなった場合に、ステップＬ２Ｅで終了する。

　ステップＳ１４７では、シミュレーション部３４が、Ｌ２の遷移前の接触状態について、上記ステップＳ１４３を実行しているか否かを判定する。実行している場合には、ステップＳ１４９へ移行し、実行していない場合には、ステップＳ１４８へ移行する。ステップＳ１４８では、上記ステップＳ１４３の処理と同様に、Ｌ２の遷移前の接触状態及び維持範囲を分岐木に登録し、ステップＳ１４９へ移行する。ステップＳ１４９では、シミュレーション部３４が、上記ステップＳ１４３の処理と同様に、Ｌ２の遷移後の接触状態及び維持範囲を分岐木に登録する。次に、ステップＳ１５０で、シミュレーション部３４が、Ｌ２の遷移後の接触状態を分岐木生成処理の開始時の接触状態として、分岐木生成処理を再帰的に実行する。

　図１４～図１６に示す具体例を用いて、上記の分岐木生成処理をより詳細に説明する。なお、図１４～図１６では、Ｘ、Ｚ、及びθの方向への遷移を中心に、説明に必要な接触状態のみを図示しており、他の方向への遷移を考慮した接触状態の図示は省略している。

　まず、図１４の（ａ）に示すように、シミュレーション部３４が、主対象物９０Ａの左側Ｘ面及び下側Ｚ面の２面が副対象物９０Ｂと接触している面接触の状態を目標状態として特定し、分岐木生成処理を開始するものとする。次に、シミュレーション部３４は、Ｌ１の遷移として、例えば、主対象物９０Ａを、下側Ｚ面での面接触を保ったままＸ軸方向へ平行移動させる遷移を実行する。この場合におけるＬ１の遷移後の接触状態を（ｂ）に示す。シミュレーション部３４は、目標状態に対応する根ノードの下流に、接触状態（ｂ）及びその維持範囲を対応付けたノードを接続することにより、接触状態（ｂ）を分岐木に登録する（ステップＳ１４３）。接触状態（ｂ）は１辺接触ではないため（ステップＳ１４４で否定判定）、シミュレーション部３４は、接触状態（ｂ）からのＬ１の遷移を実行する（ステップＳ１４５中のループ処理Ｌ１）。

　シミュレーション部３４は、接触状態（ｂ）からのＬ１の遷移として、例えば、主対象物９０Ａを、下側Ｚ面右（右側Ｘ面下）での１辺接触を保ったまま回転させる遷移を実行する。この場合におけるＬ１の遷移後の接触状態を（ｃ）に示す。シミュレーション部３４は、接触状態（ｂ）に対応するノードの下流のノードに、接触状態（ｃ）を登録する（ステップＳ１４３）。接触状態（ｃ）は１辺接触であるため（ステップＳ１４４で肯定判定）、ループ処理Ｌ２が実行される。（ｂ）から（ｃ）への遷移と同じ方向へのＬ２の遷
移を継続すると、（ｄ）に示すように、２辺接触の状態が出現する。Ｌ２の遷移前の接触状態（ｃ）については分岐木へ登録済みであるため（ステップＳ１４７で肯定判定）、シミュレーション部３４は、接触状態（ｃ）に対応するノードの下流のノードに、接触状態（ｄ）を登録する（ステップＳ１４９）。次に、シミュレーション部３４は、接触状態（ｄ）からのＬ１の遷移を実行する（ステップＳ１５０中のループ処理Ｌ１）。

　接触状態（ｄ）から、主対象物９０Ａの右側Ｘ面での１辺接触を保ったままＺ軸方向上方へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｅ）に示す。接触状態（ｅ）は１辺接触であるため（ステップＳ１４４で肯定判定）、ループ処理Ｌ２へ移行するが、（ｆ）に示すように、Ｌ１の遷移と同じ方向に維持変位させても、新たな面接触又は２辺接触は出現しない。したがって、シミュレーション部３４は、ループ処理Ｌ２を終了し、接触状態（ｄ）を初期の接触状態とする分岐生成処理のループ処理Ｌ１に戻り、（ｄ）から（ｅ）への遷移とは異なる方向へのＬ１の遷移を実行する。

　接触状態（ｄ）から下側Ｚ面右（＝右側Ｘ面下）での１辺接触を保ったまま主対象物９０Ａを回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｇ）に示す。接触状態（ｇ）は接触状態（ｃ）と同様であるため、既に分岐木に登録されており（ステップＳ１４２で肯定判定）、また、１辺接触であるため、ステップＳ１４６がスキップされ、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、（ｈ）に示すような２辺接触が出現する。Ｌ２の遷移前の接触状態（ｇ）はステップＳ１４３を経由していないため、分岐木に登録されていない（ステップＳ１４７で否定判定）。そこで、シミュレーション部３４は、接触状態（ｇ）を分岐木に登録したうえで（ステップＳ１４８）、接触状態（ｈ）を分岐木に登録する（ステップＳ１４９）。接触状態（ｈ）について、以下、上記と同様に分岐木生成処理を実行する（ステップＳ１５０中のステップＳ１５０）。

　（ａ）から（ｂ）へのＬ１の遷移について、ステップＳ１４５の分岐木生成処理の再帰的な実行が終了すると、シミュレーション部３４は、（ａ）から（ｂ）へのＬ１の遷移と同じ方向へのＬ２の遷移を実行する。この場合、図１５の（ｉ）に示すように、下側Ｚ面及び右側Ｘ面の２面が接触した新たな面接触が出現する。シミュレーション部３４は、接触状態（ｉ）を分岐木に登録し（ステップＳ１４９）、接触状態（ｉ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１５０）。

　接触状態（ｉ）から、主対象物９０Ａの右側Ｘ面での面接触を保ったままＺ軸上方へ移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｊ）に示す。シミュレーション部３４は、接触状態（ｊ）及びその維持範囲を対応付けたノードを接続することにより、接触状態（ｊ）を分岐木に登録する（ステップＳ１４３）。接触状態（ｊ）は１辺接触ではないため（ステップＳ１４４で否定判定）、シミュレーション部３４は、接触状態（ｊ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１４５）。

　接触状態（ｊ）から、主対象物９０Ａの右側Ｘ面での辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｋ）に示す。接触状態（ｋ）は接触状態（ｅ）と同様であるため、既に分岐木に登録されており（ステップＳ１４２で肯定判定）、また、１辺接触であるため、ステップＳ１４６がスキップされ、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、（ｌ）に示すような２辺接触が出現する。Ｌ２の遷移前の接触状態（ｋ）はステップＳ１４３を経由していないため、分岐木に登録されていない（ステップＳ１４７で否定判定）。そこで、シミュレーション部３４は、接触状態（ｋ）を分岐木に登録したうえで（ステップＳ１４８）、接触状態（ｌ）を分岐木に登録する（ステップＳ１４９）。接触状態（ｋ）について、以下、上記と同様に分岐木生成処理を実行する（ステップＳ１５０中のステップＳ１５０）。

　接触状態（ｌ）から、主対象物９０Ａの右側Ｘ面の１辺接触を保ったままＺ軸方向上方かつＸ軸右方向へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｍ）に示す。接触状態（ｍ）は接触状態（ｅ）と同じであり、Ｌ２の遷移でも新たな面接触又は辺接触は出現しないため、接触状態（ｍ）は分岐木に登録されない。

　また、接触状態（ｌ）から、主対象物９０Ａの左側Ｘ面の１辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｎ）に示す。シミュレーション部３４は、接触状態（ｎ）を分岐木に登録し（ステップＳ１４３）、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、（ｏ）に示すような新たな面接触が出現する。シミュレーション部３４は、接触状態（ｏ）を分岐木に登録し（ステップＳ１４９）、接触状態（ｏ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１５０）。

　接触状態（ｏ）から、主対象物９０Ａの左側Ｘ面の１辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｐ）に示す。シミュレーション部３４は、接触状態（ｐ）を分岐木に登録し（ステップＳ１４３）、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、図１６の（ｑ）に示すような新たな２辺接触が出現する。シミュレーション部３４は、接触状態（ｑ）を分岐木に登録し（ステップＳ１４９）、接触状態（ｑ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１５０）。

　接触状態（ｑ）から、主対象物９０Ａの右側Ｘ面の１辺接触を保ったままＺ軸方向上方へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｒ）に示す。シミュレーション部３４は、接触状態（ｒ）を分岐木に登録し（ステップＳ１４３）、ループ処理Ｌ２へ移行する。（ｓ）に示すように、接触状態（ｒ）からＬ２の遷移では新たな面接触又は辺接触は出現しないため、分岐木へは登録されない。

　また、接触状態（ｑ）から、主対象物９０Ａの左側Ｘ面の１辺接触を保ったままＸ軸左側かつＺ軸上方へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｔ）に示す。接触状態（ｔ）は接触状態（ｐ）と同様であるため、すでに分岐木に登録されており（ステップＳ１４２で肯定判定）、また、１辺接触であるため、ステップＳ１４６がスキップされ、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、新たな面接触又は２辺接触は出現せず、接触なし状態になる。したがって、シミュレーション部３４は、ループ処理Ｌ２を終了し、接触状態（ｑ）を初期の接触状態とする分岐生成処理のループ処理Ｌ１に戻り、（ｑ）から（ｔ）への遷移とは異なる方向へのＬ１の遷移を実行する。接触状態（ｐ）から、Ｘ面左側とＸ面右側（Ｚ面右側）の２辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｕ）に示す。シミュレーション部３４は、接触状態（ｐ）を分岐木に登録し（ステップＳ１４３）、シミュレーション部３４は、接触状態（ｕ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１４５）。

　接触状態（ｂ）以降の分岐木生成処理の再帰的な実行が全て終了し、接触状態（ａ）、すなわち目標状態からの分岐木生成処理に戻ると、シミュレーション部３４は、左側Ｘ面の面接触を保ったまま、主対象物９０ＡをＺ軸上方へ移行移動させる遷移をＬ１の遷移として実行する。この場合におけるＬ１の遷移後の接触状態を図１４の（ｗ）に示す。接触状態（ｗ）は、図１５の接触状態（ｏ）と同じである。そこで、シミュレーション部３４は、分岐木の根ノード（接触状態（ａ））に近い方のノードとなる接触状態（ｗ）のノードに、接触状態（ｏ）に対応するノードから葉ノードまでの部分木を、接触状態（ｗ）のノードに付け替える（ステップＳ１４６）。

　上記のような処理により、図１４～図１６に示すような、各接触状態に対応するノードが接続された分岐木が生成される。

　図１７を参照して、上記分岐木生成処理により生成される分岐木について説明する。図１７において、丸は各接触状態に対応するノードを表し、矢印は、上記Ｌ１の遷移又はＬ２の遷移を表す。また、ノードのうち、網掛の丸で表されたノードは、探索により見つかる接触状態（図１４～図１６の（ａ）～（ｗ）の各状態に相当）のうち、１辺接触の状態に対応するノードである。また、白丸のノードは、探索により見つかる接触状態のうち、２辺接触、３辺接触、又は面接触の状態に対応するノードである。

　図１７に示すように、１辺接触の状態から遷移する状態は、必ず１つで、枝分かれしない。すなわち、枝分かれするのは、図１７において白丸のノードに対応する状態のみである。ただし、白丸のノードに対応する状態であっても、状態が枝分かれしない場合もある。一般には、葉ノード（それ以上の遷移がない状態、末端）は、１辺接触の状態になる。ただし、白丸のノードに対応する状態の先にある、網掛のノードに対応する状態が既に分岐木に登録されていた場合に限り、白丸のノードが葉ノードになる。１辺接触の状態から、１辺接触の状態に遷移することはあり得る。また、２辺以上の接触状態（白丸のノードに対応する状態）から２辺以上の接触状態に遷移することはあり得る。

　動作経路生成処理（図１１）に戻って、次のステップＳ１６で、生成部３６が、上記ステップＳ１０で取得された分解能特定情報に基づいて、ロボットの動作の分解能を決定する。具体的には、生成部３６は、センサ群５０に含まれるセンサのうち、主対象物９０Ａと副対象物９０Ｂとが非接触状態の場合において、主対象物９０Ａと副対象物９０Ｂとの位置及び姿勢の誤差を認識可能なセンサの各方向（ｄ＝Ｘ，Ｙ，Ｚ，φ，θ，ψ）の分解能をε_ｓｄｉとする。ｉは各センサに付与した識別番号である。生成部３６は、例えば、識別番号ｖのビジョンセンサの分解能ε_ｓｄｖを、ロボット制御にフィードバック可能な大きさから決定する。より具体的には、生成部３６は、ε_ｓｄｖ＝（主対象物９０Ａの長辺の長さ）×２／（ビジョンセンサの画角の短辺の画素数）（ｄ＝Ｘ，Ｙ）として決定してよい。これは、主対象物９０Ａの長辺がビジョンセンサの画角の短辺の半分以下になるようにビジョンセンサを利用する場合にビジョンセンサが認識可能な長さとするものである。生成部３６は、そのセンサで認識できない方向の分解能は、取り得る最大の値とし、６次元のベクトルとしてε_ｓｄｉを求める。そして、生成部３６は、ε_ｓｄ＝ｍｉｎ（ε_ｓｄｉ）をロボットの分解能として決定する。

　次に、ステップＳ１８で、生成部３６が、分岐木において維持範囲が分解能より大きい接触状態を特定する。例えば、生成部３６は、１辺をε_ｓｄとする領域に内包可能な維持範囲が対応付けられたノードを分岐木から特定する。該当のノードが複数存在する場合には、分岐木の根ノードから最も近いノードを特定する。分岐木の根ノードから最も近いノードは、例えば、根ノードから辿るノードの個数が少ないノードとしてもよいし、物理的な距離を測定して最も近いノードとしてもよい。前者の場合は、制御のロバスト性が高く、後者の場合は、実行時間が短くなるという利点がある。本実施形態では前者を採用するものとする。そして、生成部３６は、特定したノードに対応付けられた接触状態を特定する。

　維持範囲より分解能の方が大きい場合には、非接触状態から、特定した接触状態に到達するために探り動作が必要になる場合がある。例えば、分解能の中心位置にハンド部４６で把持された主対象物９０Ａを移動させるようにロボット４２を制御する場合、図１８に示すように、特定した接触状態の維持範囲に到達できない。一方、図１９に示すように、維持範囲より分解能の方が小さければ、センサにより認識可能な位置及び姿勢の誤差に基
づいて、確実に非接触状態から、特定した接触状態の維持範囲に到達することができる。

　次に、ステップＳ２０で、生成部３６が、特定した接触状態から目標状態までの接触状態の遷移に基づく動作経路を生成する。具体的には、生成部３６は、分岐木において、上記ステップＳ１８で特定した接触状態に対応するノードから根ノードに至るまでの各ノードに対応する接触状態からハンド部４６の位置及び姿勢を特定して並べることで、特定した接触状態から目標状態までの軌跡を生成する。

　次に、ステップＳ２２で、生成部３６が、上記ステップＳ１０で取得された開始位置姿勢から、上記ステップＳ１８で特定した接触状態に対応する位置及び姿勢までの経路であって、障害物を回避する経路を生成する。そして、生成部３６が、上記ステップＳ２０で生成した経路と、本ステップで生成した経路とを結合し、各位置及び姿勢に接触状態を対応付けて動作経路として生成する。

　次に、ステップＳ２４で、表示制御部３８が、上記ステップＳ２２で生成された動作経路に基づいて、第１提示画面６０及び第２提示画面の少なくとも一方を表示装置に表示する。そして、表示制御部３８は、ユーザによる主対象物９０Ａの位置及び姿勢の修正であるユーザ登録位置姿勢を受け付ける。

　次に、ステップＳ２６で、生成部３６が、上記ステップＳ２４でユーザ登録位置姿勢が受け付けられた場合には、上記ステップＳ２４で生成した動作経路に、受け付けられたユーザ登録位置姿勢を反映して動作経路を再生成する。そして、生成部３６が、最終的に生成した動作経路をロボット制御装置４０へ出力し、動作経路生成処理は終了する。

　ロボット制御装置４０は、動作経路生成装置１０から入力された動作経路を実現する力制御のための指令値を生成し、ロボット４２に出力する。

　主対象物９０Ａと環境との接触を考慮しない従来のロボット制御では、図２０に示すように、動作経路に基づく目標位置θ_ｒｅｆから、その目標位置θ_ｒｅｆを実現するためのモータトルクを位置制御器により算出し、ロボットを制御する。そして、制御されたロボットから（バネ性を有する）減速機出力後の関節角度θ_ａｃｔを取得し、位置制御器へフィードバックする。

　図２１に、本開示を実装する対象としての力制御系の実現例の一例としてアドミタンス制御の場合を示す。この場合、動作経路に応じた力目標τ_ｒｅｆから、環境から受ける外部トルクτ_ｅを差し引いた値を力制御器で補正した補正値に、ユーザが指定する目標位置θ_ｒｅｆを加え、そこからモータの現在位置θ_ｍを差し引くことで位置目標値を算出する。そして、その位置目標値を実現するためのモータトルクを位置制御器により算出し、ロボットを制御する。そして、制御されたロボットからモータの現在位置θ_ｍを取得し、位置制御器へフィードバックする。また、ロボットと環境との接触に基づいて（バネ性を有する）減速機出力後の関節角度θ_ａｃｔを取得し、θ_ｍからθ_ａｃｔを差し引いた値を、ロボットのギア及びトルクセンサのバネ性に基づいて、環境から受ける外部トルクτ_ｅに変換し、力制御器へフィードバックする。

　また、図２２に、本開示を実装する対象としての力制御系の実現例の一例としてインピーダンス制御の場合を示す。この場合、動作経路に応じた目標位置θ_ｒｅｆから（バネ性を有する）減速機出力後の関節角度θ_ａｃｔを差し引いた値を位置制御器で補正した補正値に、ユーザが指定する力目標τ_ｒｅｆを加え、そこから環境から受ける外部トルクτ_ｅを差し引くことで力目標値を算出する。そして、その力目標値を実現するためのモータトルクを力制御器により算出し、ロボットを制御する。そして、制御されたロボットからモ
ータの現在位置θ_ｍを取得し、θ_ｍからθ_ａｃｔを差し引いた値を、ロボットのギア及びトルクセンサのバネ性に基づいて、環境から受ける外部トルクτ_ｅに変換し、力制御器へフィードバックする。また、（バネ性を有する）減速機出力後の関節角度θ_ａｃｔを位置制御器へフィードバックする。

　以上説明したように、本実施形態に係るロボット制御システムでは、動作経路生成装置が、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、把持部の主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、主対象物及び副対象物を含む作業環境の形状情報とを取得する。また、動作経路生成装置が、取得された情報に基づいて、主対象物と副対象物との接触状態から、主対象物と副対象物とが非接触状態になるまでの主対象物と副対象物との接触状態の遷移をシミュレーションする。そして、取得された情報と、シミュレーションされた接触状態の遷移とに基づいて、主対象物が開始位置及び姿勢である初期状態からの接触状態の遷移に含まれるいずれかの接触状態に至り、いずれかの接触状態を含む接触状態の遷移を辿って目標状態に至るロボットの動作経路を生成する。これにより、対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成することができる。

　これにより、ロボットによる作業を高速に実行できる。また、自動で自律的な組立動作をロボットが実行できるようになるため、ティーチング工数が激減する。また、実際の位置姿勢と制御目標との間に位置姿勢誤差があっても、ロボットが自律的に位置及び姿勢を補正して、作業を完遂できるようになる。

　なお、上記実施形態では、終了位置姿勢に対応する状態を目標状態とする場合について説明したが、分岐木を生成する際の目標状態として、中間点の位置姿勢を設定してもよい。この場合、中間点から接触状態を辿って終了位置姿勢に辿り着くことができる場合は、終了位置姿勢の状態が含まれる接触状態の遷移を表す分岐木が生成される。なお、生成された分岐木に終了位置姿勢が含まれない場合は、指定した中間点から、終了位置姿勢に到達できないことを表す。

　なお、上記実施形態では、動作経路生成装置とロボット制御装置とを別々の装置とする場合について説明したが、ロボット制御装置内に動作経路生成装置を組み込む形態としてもよい。また、動作経路生成装置の各機能構成を、それぞれ異なる装置で実現し、上記の動作経路生成処理を分散処理により実現してもよい。

　また、上記実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した動作経路生成処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ　Ｌｏｇｉｃ　Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、動作経路生成処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

　また、上記実施形態では、動作経路生成プログラムが記憶装置に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、ブルーレイディスク、ＵＳＢメモリ等の記憶媒体に記憶された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロード
される形態としてもよい。

１ ロボット制御システム
１０ 動作経路生成装置
１２ ＣＰＵ
１４ メモリ
１６ 記憶装置
１８ 入力装置
２０ 出力装置
２２ 記憶媒体読取装置
２４ 通信Ｉ／Ｆ
２６ バス
３２ 取得部
３４ シミュレーション部
３６ 生成部
３８ 表示制御部
４０ ロボット制御装置
４２ ロボット
４４ ロボットアーム
４６ ハンド部
５０ センサ群
６０ 第１提示画面
７０ 第２提示画面
９０Ａ 主対象物
９０Ｂ 副対象物

Claims (12)

1. 　ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部と、
   　前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部と、
   　前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部と、
   　を含む動作経路生成装置。
2. 　前記シミュレーション部は、前記目標状態から、前記主対象物と前記副対象物との接触状態が、第１の方向に力をかけても動かない状態に達すると、前記第１の方向とは異なる第２の方向に力をかけ続けて動かない状態に達した場合、前記動かない状態から前記第１の方向及び前記第２の方向とは異なる方向に力をかけ続けるというシミュレーションを繰り返すことで、前記接触状態の遷移を探索することを含む請求項１に記載の動作経路生成装置。
3. 　前記シミュレーション部は、前記接触状態の遷移を順次探索しながら、前記目標状態を根ノードとし、それぞれ異なる接触状態をノードとする分岐木を生成する請求項２に記載の動作経路生成装置。
4. 　前記シミュレーション部は、前記分岐木のノードの各々に、前記ノードに対応付けられた接触状態を維持したまま前記主対象物が変位可能な維持範囲を対応付ける請求項３に記載の動作経路生成装置。
5. 　前記取得部は、前記ロボットの動作の分解能を特定するための情報を取得し、
   　前記生成部は、前記分岐木において、前記分解能よりも大きな前記維持範囲が対応付けられたノードに対応付けられた接触状態を、前記いずれかの接触状態として特定する
   　請求項４に記載の動作経路生成装置。
6. 　前記生成部は、前記いずれかの接触状態が複数特定される場合、前記分岐木の根ノードから最も近いノードに対応けられた接触状態を前記いずれかの接触状態として特定する請求項５に記載の動作経路生成装置。
7. 　前記ロボットの動作の分解能を特定するための情報は、前記作業環境を検知するセンサの分解能、及び前記ロボットを駆動するためのアクチュエータの制御精度の少なくとも一方を含む請求項５に記載の動作経路生成装置。
8. 　前記生成部により生成された前記動作経路を表示装置に表示する表示制御部を含む請求項１～請求項７のいずれか１項に記載の動作経路生成装置。
9. 　前記表示制御部は、表示した前記動作経路に対する修正をユーザから受け付け、
   　前記生成部は、受け付けられた前記修正に基づいて、前記動作経路を再生成する
   　請求項８に記載の動作経路生成装置。
10. 　前記ロボットは、接触による外部環境からの反力に押し負ける柔軟構造を有するソフトロボットである請求項１～請求項７のいずれか１項に記載の動作経路生成装置。
11. 　取得部が、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得し、
    　シミュレーション部が、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションし、
    　生成部が、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する
    　動作経路生成方法。
12. 　コンピュータを、
    　ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部、
    　前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部、及び、
    　前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部
    　として機能させるための動作経路生成プログラム。