JP7622455B2 - ロボット制御装置、ロボット制御方法、及びロボット制御プログラム - Google Patents

Description

本開示は、ロボット制御装置、ロボット制御方法、及びロボット制御プログラムに関する。

従来、ロボットを用いて物体の位置姿勢を操作する場合は、エンドエフェクタと把持している物体の相対位置姿勢は既知で操作中に変化せず、かつ、物体の目標位置姿勢のロボット座標系における値が既知であることが条件であった。上記の条件下におけるロボット制御では、制御中は把持している物体の位置姿勢には注意せずに、ロボットは定められた動作を行うことで物体の位置姿勢を目標に至らせている。

また、従来技術として、カメラによる視覚的観測行為によって物体の位置姿勢を認識し、それを元にロボットを制御する方法も存在する。このような方法の場合、組み付け先の物体の位置姿勢を認識することで目標位置姿勢のロボット座標系における値を認識することも可能である。

また、組み立て作業を行うロボットの動作生成に関する技術がある。例えば、機械学習を利用して、遂行する作業をロボットに習得させる手法が研究されている（非特許文献１参照）。この技術では、カメラから得られる画像データに基づいて対象物を把持する動作を強化学習によりロボットに習得させる手法が提案されている。この手法によれば、対象物を把持する動作をロボットに教示する一連の処理の少なくとも一部を自動化することができる。

Dmitry Kalashnikov, et al. "QT-Opt: Scalable Deep Reinforcement Learning for Vision-Based Robotic Manipulation" arXiv preprint arXiv:1806.10293, 2018.

しかしながら、現実においてはエンドエフェクタと把持している物体の相対位置姿勢は不確実であり、また、操作中に変化しうる。しかも、物体の目標位置姿勢のロボット座標系における値は不確実である。特に、把持しようとする物体や組付け先の物体の位置姿勢を治具によって高精度に定めない場合には、これらの不確実性は大きくなる。よって、従来技術では、定められた動作によってロボット自体は動作したとしても、把持している物体の位置姿勢は目標位置姿勢には至らないため、物体を把持して他の物体に組み付ける操作が失敗する場合がある、という問題が存在した。

また、カメラによる認識手法ではイメージャーの量子化、１画素に入射する光子数の揺らぎ、照明変動等の要因により、センサ値に誤差が発生するため、認識した位置姿勢も誤差を含んだものとなる。そのため、認識した位置姿勢を元にロボットを制御しても、物体の位置姿勢を目標状態に至らせるようロボットを制御することは困難であった。

本開示は、上記事情を鑑みてなされたものであり、操作物体を精度よく操作させるようにロボットを制御するためのロボット制御装置、ロボット制御方法、及びロボット制御プログラムを提供することを目的とする。

本開示の第１態様に係るロボット制御装置は、操作対象である操作物体が環境中に位置する注目物体から離れている状態から前記操作物体が前記注目物体に特定の態様で接触している状態である完了状態に遷移するように前記操作物体を操作するロボットを制御するロボット制御装置であって、前記操作物体の前記完了状態に至るまでの移動の途中の目標状態である中間目標状態又は前記完了状態を今回移動の目標状態として設定する目標状態設定部と、前記操作物体の位置姿勢及び前記操作物体と前記注目物体との接触の有無についてのセンサによる観測結果を取得する観測部と、前記操作物体の位置姿勢の不確定さを表す粒子の集合であって、前記粒子の集合に含まれるそれぞれの前記粒子は前記操作物体のありうる位置姿勢の一つを表す、前記粒子の集合を設定する粒子集合設定部と、前記粒子のうち前記観測結果が示す前記操作物体の位置姿勢に近い位置姿勢を表す粒子ほど粒子の重みを大きくし、かつ、前記観測結果が前記接触が生じていることを示す場合には前記注目物体及び前記操作物体の形状及び相対位置関係が表現される仮想空間において前記注目物体とそれぞれの前記粒子が表す位置姿勢で配置された前記操作物体とが接触している状態に近いほど対応する前記粒子の重みを大きくする粒子集合調整部と、各前記粒子の重みが調整された前記粒子の集合に基づいて推定された前記操作物体の位置姿勢である推定状態を算出する状態推定部と、前記操作物体を前記推定状態から前記今回移動の目標状態に移動させるための行動を計画する行動計画部と、計画された前記行動の実行を前記ロボットに指令する行動部と、前記目標状態の設定、前記観測結果の取得、前記粒子の集合の設定、前記粒子の集合の調整、前記推定状態の算出、前記行動の計画、及び前記行動の実行を、前記推定状態が前記完了状態に所定の誤差内で一致するまで繰り返させる処理制御部と、を含んで構成されている。

本開示の第２態様に係るロボット制御方法は、操作対象である操作物体が環境中に位置する注目物体から離れている状態から前記操作物体が前記注目物体に特定の態様で接触している状態である完了状態に遷移するように前記操作物体を操作するロボットを制御するロボット制御方法であって、前記操作物体の前記完了状態に至るまでの移動の途中の目標状態である中間目標状態又は前記完了状態を今回移動の目標状態として設定し、前記操作物体の位置姿勢及び前記操作物体と前記注目物体との接触の有無についてのセンサによる観測結果を取得し、前記操作物体の位置姿勢の不確定さを表す粒子の集合であって、前記粒子の集合に含まれるそれぞれの前記粒子は前記操作物体のありうる位置姿勢の一つを表す、前記粒子の集合を設定し、前記粒子のうち前記観測結果が示す前記操作物体の位置姿勢に近い位置姿勢を表す粒子ほど粒子の重みを大きくし、かつ、前記観測結果が前記接触が生じていることを示す場合には前記注目物体及び前記操作物体の形状及び相対位置関係が表現される仮想空間において前記注目物体とそれぞれの前記粒子が表す位置姿勢で配置された前記操作物体とが接触している状態に近いほど対応する前記粒子の重みを大きくし、各前記粒子の重みが調整された前記粒子の集合に基づいて推定された前記操作物体の位置姿勢である推定状態を算出し、前記操作物体を前記推定状態から前記今回移動の目標状態に移動させるための行動を計画し、計画された前記行動の実行を前記ロボットに指令し、前記目標状態の設定、前記観測結果の取得、前記粒子の集合の設定、前記粒子の集合の調整、前記推定状態の算出、前記行動の計画、及び前記行動の実行を、前記推定状態が前記完了状態に所定の誤差内で一致するまで繰り返させる処理をコンピュータに実行させる。

本開示の第３態様に係るロボット制御プログラムは、操作対象である操作物体が環境中に位置する注目物体から離れている状態から前記操作物体が前記注目物体に特定の態様で接触している状態である完了状態に遷移するように前記操作物体を操作するロボットを制御するためのロボット制御プログラムであって、前記操作物体の前記完了状態に至るまでの移動の途中の目標状態である中間目標状態又は前記完了状態を今回移動の目標状態として設定し、前記操作物体の位置姿勢及び前記操作物体と前記注目物体との接触の有無についてのセンサによる観測結果を取得し、前記操作物体の位置姿勢の不確定さを表す粒子の集合であって、前記粒子の集合に含まれるそれぞれの前記粒子は前記操作物体のありうる位置姿勢の一つを表す、前記粒子の集合を設定し、前記粒子のうち前記観測結果が示す前記操作物体の位置姿勢に近い位置姿勢を表す粒子ほど粒子の重みを大きくし、かつ、前記観測結果が前記接触が生じていることを示す場合には前記注目物体及び前記操作物体の形状及び相対位置関係が表現される仮想空間において前記注目物体とそれぞれの前記粒子が表す位置姿勢で配置された前記操作物体とが接触している状態に近いほど対応する前記粒子の重みを大きくし、各前記粒子の重みが調整された前記粒子の集合に基づいて推定された前記操作物体の位置姿勢である推定状態を算出し、前記操作物体を前記推定状態から前記今回移動の目標状態に移動させるための行動を計画し、計画された前記行動の実行を前記ロボットに指令し、前記目標状態の設定、前記観測結果の取得、前記粒子の集合の設定、前記粒子の集合の調整、前記推定状態の算出、前記行動の計画、及び前記行動の実行を、前記推定状態が前記完了状態に所定の誤差内で一致するまで繰り返させる処理をコンピュータに実行させるためのプログラムである。

本発明のロボット制御装置、ロボット制御方法、及びロボット制御プログラムによれば、操作物体を精度よく操作させるようにロボットを制御することができる。

本実施形態に係るロボットを制御するための制御システムの構成を示す図である。 制御対象の一例としてのロボットの概略構成を示す図である。 動作計画装置及び制御装置のハードウェア構成を示すブロック図である。 動作計画装置及び制御装置の機能構成の例を示すブロック図である。 部品Ａ～部品Ｄを用いた組み立て手順の一例を示す図である。 把持データの相対位置の一例を示す図である。 制御装置の実行部の機能構成の例を示すブロック図である。 ガイドを生成する方法を説明するための図である。 本実施形態の制御システムの処理の流れを示すシーケンス図である。 制御装置の制御処理の流れを示すフローチャートである。

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

図１は、本実施形態に係るロボットを制御するための制御システムの構成を示す図である。図１に示すように、制御システム１は、ロボット１０と、状態観測センサ１４と、接触観測センサ１６と、動作計画装置２０と、状態遷移制御部２５と、制御装置３０とを有する。なお、状態遷移制御部２５は、動作計画装置２０の一部であってもよいし、制御装置３０の一部であってもよいし、本実施形態のように動作計画装置２０及び制御装置３０のいずれからも独立した装置として構成されていてもよい。

（ロボット）
図２及び図３は、ロボット１０の概略構成を示す図である。本実施形態におけるロボット１０は、６軸垂直多関節ロボットであり、アーム１１の先端１１ａに柔軟部１３を介してエンドエフェクタ１２が設けられる。ロボット１０は、エンドエフェクタ１２によって部品を把持して組み立て体の組立作業を行う。図３の例ではエンドエフェクタ１２は１組の挟持部１２ａのハンドを有する構成としているが、エンドエフェクタ１２を吸着パッドとして、部品を吸着するようにしてもよい。以下の説明においては、部品を吸着することも含めて部品を把持するという。部品を把持することと吸着することとを含めて部品を保持すると言い換えてもよい。

図２に示すように、ロボット１０は、関節Ｊ１～Ｊ６を備えた６自由度のアーム１１を有する。各関節Ｊ１～Ｊ６は、図示しないモータによりリンク同士を矢印Ｃ１～Ｃ６の方向に回転可能に接続する。アーム１１の先端にはエンドエフェクタ１２としてグリッパが接続されている。ここでは、垂直多関節ロボットを例に挙げたが、水平多関節ロボット（スカラーロボット）であってもよい。また、６軸ロボットを例に挙げたが、５軸や７軸などその他の自由度の多関節ロボットであってもよく、パラレルリンクロボットであってもよい。

（状態観測センサ）
状態観測センサ１４は、ロボット１０の状態を観測し、観測したデータを状態観測データとして出力する。状態観測センサ１４としては、例えば、ロボット１０の関節のエンコーダ、視覚センサ（カメラ）、モーションキャプチャ、力関連センサ等が用いられる。ロボット１０の状態として、各関節の角度からアーム１１の先端１１ａの位置及び姿勢が特定でき、視覚センサ及び／又は力関連センサから部品（作業対象物）の姿勢が推定できる。モーションキャプチャ用のマーカーがエンドエフェクタ１２に取り付けられている場合には、ロボット１０の状態としてエンドエフェクタ１２の位置及び姿勢が特定でき、エンドエフェクタ１２の位置及び姿勢から部品（作業対象物）の姿勢が推定できる。

力関連センサとは、力覚センサ及びトルクセンサの総称であり、さらにセンサを部品と接触する部位に設ける場合には触覚センサも含む総称である。力関連センサは、ロボット１０のエンドエフェクタが部品から受ける力を検出するように、エンドエフェクタ１２が部品を把持する部分の表面や、エンドエフェクタ１２内の関節部分に設けてもよい。力関連センサは、例えば、１要素または多要素の、１軸、３軸、又は６軸の力をロボット１０の状態として検出するセンサである。力関連センサを用いることで、エンドエフェクタ１２が部品をどのように把持しているか、すなわち部品の姿勢をより精度良く把握でき、適切な制御が可能となる。

また、視覚センサによっても、エンドエフェクタ１２自体やエンドエフェクタ１２が把持している部品の位置及び姿勢をロボット１０の状態として検出できる。

このように、状態観測センサ１４である各種センサによって、エンドエフェクタ１２、及び把持されている部品についての状態を検出することができる。また、各種センサの検出結果を状態観測データとして取得することができる。

（接触観測センサ）
接触観測センサ１６は、圧力センサ、力覚センサ、又は触覚センサである。触覚センサは、圧力分布を検出するセンサ、又は直交３軸方向の力および直交３軸まわりのモーメントを検出できるセンサである。圧力センサや触覚センサは、たとえばエンドエフェクタ１２の指の把持対象物に接触する部分に設けられる。力覚センサは、たとえばロボット１０のアーム１１とエンドエフェクタ１２との間の手首部分に設けられる。

（動作計画装置／制御装置）
次に、動作計画装置２０及び制御装置３０の構成について説明する。

図３は、本実施形態に係る動作計画装置２０及び制御装置３０のハードウェア構成を示すブロック図である。動作計画装置２０及び制御装置３０は同様のハードウェア構成で実現できる。動作計画装置２０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２０Ａ、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）２０Ｂ、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）２０Ｃ、ストレージ２０Ｄ、入力部２０Ｅ、表示部２０Ｆ、及び通信インタフェース（Ｉ／Ｆ）２０Ｇを有する。各構成は、バス２０Ｈを介して相互に通信可能に接続されている。制御装置３０は、ＣＰＵ３０Ａ、ＲＯＭ３０Ｂ、ＲＡＭ３０Ｃ、ストレージ３０Ｄ、入力部３０Ｅ、表示部３０Ｆ、及び通信Ｉ／Ｆ３０Ｇを有する。各構成は、バス３０Ｈを介して相互に通信可能に接続されている。以下、動作計画装置２０の場合について説明する。

本実施形態では、ＲＯＭ２０Ｂ又はストレージ２０Ｄには、プログラムが格納されている。ＣＰＵ２０Ａは、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ２０Ａは、ＲＯＭ２０Ｂ又はストレージ２０Ｄからプログラムを読み出し、ＲＡＭ２０Ｃを作業領域としてプログラムを実行する。ＣＰＵ２０Ａは、ＲＯＭ２０Ｂ又はストレージ２０Ｄに記録されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

ＲＯＭ２０Ｂは、各種プログラム及び各種データを格納する。ＲＡＭ２０Ｃは、作業領域として一時的にプログラム又はデータを記憶する。ストレージ２０Ｄは、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）、又はフラッシュメモリにより構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

入力部２０Ｅは、キーボード、及びマウス等のポインティングデバイスを含み、各種の入力を行うために使用される。表示部２０Ｆは、例えば、液晶ディスプレイであり、各種の情報を表示する。表示部２０Ｆは、タッチパネル方式を採用して、入力部２０Ｅとして機能してもよい。

通信インタフェース（Ｉ／Ｆ）２０Ｇは、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ又はＷｉ－Ｆｉ（登録商標）等の規格が用いられる。

図４は、動作計画装置２０及び制御装置３０の機能構成の例を示すブロック図である。なお、動作計画装置２０及び制御装置３０を一体として構成するようにしてもよい。

図４に示すように、動作計画装置２０は、機能構成として、遷移作成部１１０と、把持データ特定部１１２とを有する。動作計画装置２０の各機能構成は、ＣＰＵ２０ＡがＲＯＭ２０Ｂ又はストレージ２０Ｄに記憶されたプログラムを読み出し、ＲＡＭ２０Ｃに展開して実行することにより実現される。制御装置３０は、機能構成として、取得部１３０と、実行部１３２とを有する。制御装置３０の各機能構成は、ＣＰＵ３０ＡがＲＯＭ３０Ｂ又はストレージ３０Ｄに記憶されたプログラムを読み出し、ＲＡＭ３０Ｃに展開して実行することにより実現される。

本実施形態では、ロボット１０の作業による組み立て工程を状態遷移図で表す。全体の状態遷移図を、部品又は組み立て体を要素単位に分解して、要素の遷移を表したデータを単位状態遷移データとして表す。

動作計画装置２０では、動作計画として単位状態遷移データの各々を作成するが、単位状態遷移データの作成のために必要な登録情報をユーザからの入力により受け付ける。登録情報は、部品の情報（部品のＩＤ、種類等）、エンドエフェクタ１２の種類別のＣＡＤ情報、組み立て手順（分解手順を含む）、及び部品の把持位置（姿勢を含む）等の各種情報である。なお、組み立て手順にはシミュレーション上で組み立て（又は分解）の際に記録された部品の相対軌道を含む。

遷移作成部１１０は、単位状態遷移データの各々を含む状態遷移データを作成する。単位状態遷移データの各々は登録情報のうちの組み立て手順から作成する。

例えば、遷移作成部１１０は、第１要素と第２要素とが独立に存在する状態から第１要素及び第２要素からなる組み立て体である第３要素が組み立てられている状態への遷移を表す単位状態遷移データを作成する。第１要素は部品又は組み立て体である。第２要素もまた部品又は組み立て体である。

ここで把持データ特定部１１２について説明する前に、状態遷移データに対応する組み立て手順の具体的な例を説明する。図５は、部品Ａ～部品Ｄを用いた組み立て手順の一例を示す図である。状態遷移に当てはめると、（１）～（３）の各々が単位状態遷移データに含まれる初期状態に対応する。（１）の部品Ａ及び部品Ｂを組み立てる作業工程を当該上記の単位状態遷移データに当てはめると、部品Ａ及び部品Ｂがそれぞれ第１要素及び第２要素に対応し、部品Ａ及び部品Ｂを組み立てた（２）の組み立て体が第３要素に対応する。第３要素が（１）を初期状態とする単位状態遷移データに含まれる目標状態である。（２）以降についても同様に単位状態遷移データに当てはめることができる。以下では、この部品Ａ～部品Ｄの組み立て手順を例に、把持データ特定部１１２について説明する。

把持データ特定部１１２は、登録情報のうちの部品の把持位置に基づいて、単位状態遷移データの各々について、把持データを特定する。把持データ特定部１１２は、組み立てのためにロボット１０のエンドエフェクタ１２が把持対象物である第１要素又は第２要素を把持したときの把持データを特定する。把持データは、エンドエフェクタ１２が把持対象物を把持したときの、エンドエフェクタ１２及び把持対象物の相対位置姿勢の計画値である。以下、把持データにおける相対位置姿勢について説明する。ここで相対位置姿勢の計画値とは、エンドエフェクタ１２が要素を把持したときの、把持位置及び把持姿勢の計画値である。また、把持対象物とした要素に対する、もう一方の要素が組み込み先対象物となる。また、位置姿勢は、６自由度で表される。

図６は把持データの相対位置姿勢の一例を示す図である。図６（Ａ）は、エンドエフェクタ１２を吸着パッドとした場合において、部品Ａに対するエンドエフェクタ１２の相対位置姿勢Ｒ１が矢印として表される。図６（Ａ）は、エンドエフェクタ１２を吸着パッドとした場合において、部品Ａに対するエンドエフェクタ１２の相対位置姿勢Ｒ１を表している。図６（Ｂ）は、エンドエフェクタ１２を一組の挟持部を備えたグリッパ（ハンド）とした場合において、部品Ｂに対するエンドエフェクタ１２の相対位置姿勢Ｒ２を表している。部品Ｃ及び部品Ｄについても同様に表される。

相対位置姿勢は次の式により求められる。

（Ｔ^{ｏｂｊ→ｔｃｐ}）^－１＝Ｔ^{ｔｃｐ→ｏｂｊ}

左辺が部品座標系を表しており、右辺がエンドエフェクタ１２からの相対位置姿勢（位置及び姿勢）を表している。把持データは、例えば、把持時に使用するエンドエフェクタ１２の識別ＩＤ付きＣＡＤデータと把持対象物の目標とする相対位置姿勢の計画値として計算する。相対位置姿勢の計画値は、管理者であるユーザが登録情報の部品の把持位置（部品表面上の把持されるべき位置）に含めて入力してもよいし、把持計画など既存の手法により自動で算出してもよい。

動作計画装置２０は、作成した単位状態遷移データ、並びに制御データである把持データを状態遷移制御部２５に出力する。単位状態遷移データによって表される１つの状態遷移のことをタスクともいう。なお、動作計画装置２０からタスク更新のたびに都度出力するのではなく、状態遷移データに含まれる単位状態遷移データ及び制御データを全て状態遷移制御部２５に出力しておき、状態遷移制御部２５側でいずれの単位状態遷移データを出力するかを管理するようにしてもよい。また、把持データは、状態遷移制御部２５を介さず、制御装置３０に直接出力し、制御装置３０側で管理してもよい。

状態遷移制御部２５は、状態遷移データのうちの処理対象のタスクに対応する単位状態遷移データを制御装置３０の実行部１３２に出力する。処理対象は、組み立て作業の開始時に指定し、制御装置３０からタスク完了の通知を受け付けるたびに更新される。状態遷移制御部２５は、動作計画装置２０に含まれていてもよいし、制御装置３０に含まれていてもよいし、いずれとも異なる装置であってもよい。また、動作計画装置２０、制御装置３０及び状態遷移制御部２５を含む制御システム全体が１つの装置であってもよい。

以降、制御装置３０の各処理部について説明する。

取得部１３０は、処理対象の単位状態遷移データ及び把持データを、状態遷移制御部２５から取得する。また、取得部１３０は、第１要素の位置及び第２要素の位置を観測した状態観測データを状態観測センサ１４から取得する。なお、第１要素の位置及び第２要素の位置は姿勢を含む。

また、取得部１３０は、接触観測センサ１６から得られるセンサ値である接触観測データを取得する。

実行部１３２は、状態遷移制御部２５から出力された単位状態遷移データを用いてタスクを実行する。タスクは、例えば、単位状態遷移データにおいて第３要素が組み立てられた状態を完成させるタスクである。実行部１３２は、タスクの実行により、観測データ及び把持データに基づいてエンドエフェクタ１２により第１要素及び第２要素の一方である把持対象物を把持させ、後述のガイドを最終部分に含む目標相対軌道に沿って把持対象物を他方の要素に対して相対的に移動させる。

実行部１３２のタスクの具体的な実行方法について以下に説明する。

ここで、ロボット１０の動作によって２つの物体の相対位置姿勢を操作する問題を考える。

ここでは環境中に位置している物体を注目物体、ロボット１０がエンドエフェクタ１２によって操作する物体を操作物体と呼称する。前提として、両物体のＣＡＤモデルは与えられており、両物体は剛体でＣＡＤモデルと現実の物体形状に誤差はないものとする。また、なんらかの外力によって注目物体の位置姿勢が変化しないことを前提とする。注目物体と操作物体の完了状態である相対位置姿勢は、タスクの単位状態遷移データとして与えられるものとする。

把持した部品を他の部品に組み付けるタスクの場合、注目物体は組み付け先の部品であり、操作物体は把持している部品である。完了状態は、把持している部品が組み付け先に組み付けられた相対位置姿勢である。

把持した部品を環境中の他の位置に移し変えるタスクの場合、注目物体は環境中のランドマークであり、操作物体は把持している部品である。完了状態は、所望の移動先位置のランドマークからの相対位置姿勢である。

注目物体と操作物体の相対位置姿勢は、状態観測センサ１４を用いた視覚的観測で認識する。例えば、状態観測センサ１４であるカメラから得られる画像とそれぞれのＣＡＤモデルを用いて、ＣＡＤマッチングによって注目物体と操作物体のそれぞれのカメラ座標系における位置姿勢を認識する。２つの位置姿勢を用いて、注目物体のＣＡＤ原点から見た操作物体のＣＡＤ原点の位置姿勢を相対位置姿勢とする。なお、前述のようにそれぞれの物体の観測したカメラ座標系における位置姿勢は誤差が生じているため、求めた相対位置姿勢にも誤差が含まれている。相対位置姿勢の真値と観測値との乖離を系の観測ノイズと呼称する。なお、相対位置姿勢の真値を知る手段はないため、観測ノイズも概念上のものであってその値を求めることはできない。注目物体と操作物体の相対位置姿勢の観測値は、カメラで撮像した画像を処理して取得するほか、カメラによる観測値との間で較正ができている場合には、ロボット１０が操作物体を把持した後はエンコーダによって計測されるロボット１０の各関節の角度に基づいて演算することにより取得してもよい。

また注目物体と操作物体の接触を、接触観測センサ１６である圧力センサあるいは力覚センサや触覚センサから認識する。本実施形態では、接触観測センサ１６の接触観測値には誤りがなく接触したかどうかを正確に観測できることを前提とする。

エンドエフェクタ１２が操作物体を把持しているとき、指令された移動量だけ操作物体を移動することで相対位置姿勢を変化させようとロボット１０を制御したとき、実際に操作物体が移動した量である相対位置姿勢の変化量は、指令された移動量とは異なる。例えば、ロボット１０の取り付け位置の誤差やフォワードキネマティクスの演算誤差によってＴＣＰ（ツールセンターポイント）の移動量自体に誤差が生じる場合があり得る。現実のロボット１０の使用場面ではそれなりの事前較正が行われているので、通常これらの誤差の大きさは小さいが、それでも挿入操作のような高精度を要する操作はわずかな誤差があっても失敗する。あるいは、エンドエフェクタ１２に対する操作物体の位置姿勢を把持直後に観測し、その後観測し直していない場合は、観測してから操作物体をガイド始点まで移動させる間に、把持する力が不十分だったためエンドエフェクタ１２に対する操作物体の位置姿勢が変化する場合があり得る。さらに、操作物体を後述のガイドに沿って移動させる間に、操作物体が注目物体に接触するなど操作物体に外力が加わると、エンドエフェクタ１２に対する操作物体の位置姿勢が変化する場合があり得る。いずれにせよ、これらの誤差があると、指令された移動量だけ操作物体を移動することはできない。実際の移動量の指令された移動量からの乖離を系のシステムノイズと呼称する。

ここでは注目物体から見た操作物体の相対位置姿勢を状態ｓとし、離散的な時刻ｔ＝０，１，・・・，Ｍにおける状態ｓ_ｔを考える対象とする。状態ｓの座標系を、注目物体を基準とする相対座標系である物体座標系とする。また時刻ｔにおけるロボット座標系でのＴＣＰの位置姿勢をｘ_ｔとし、ｘ_ｔとｓ_ｔとの位置姿勢の同次変換行列をＴ_ｔとする。

位置姿勢の変換を数式（１）に記す。ｓ_ｔ、Ｔ_ｔ、ｘ_ｔは同次座標系である。

数式（１）から明らかなように、同次変換行列Ｔ_ｔは、ＴＣＰの位置姿勢のロボット座標系から物体座標系への変換だけでなく、ＴＣＰの相対位置姿勢から操作物体の相対位置姿勢への変換も含んでいる。すなわち、ｘ_ｔがＴＣＰの位置姿勢であるのに対し、ｘ_ｔがＴ_ｔによって変換された結果である状態ｓ_ｔは、ＴＣＰではなく操作物体の相対位置姿勢である。

なお、状態ｓ_ｔやＴＣＰの位置姿勢ｘ_ｔは４行４列の行列であり、その左上３行３列の成分が回転行列を表しており、第４列の第１～３行が位置ベクトルとして扱える。第４列第４行は無限遠を扱うための成分である。回転行列は性質上３次元分の情報しか持たないため、これらの行列は実質６次元として扱うことができる。同次変換行列Ｔ_ｔも４行４列の行列である。同次座標系表記の２つの行列の積は、ベクトル表記した位置姿勢の足し算に相当する。

いまロボット１０がエンドエフェクタ１２によって操作物体を把持していることを考える。

ある時刻ｔにおいてＴＣＰの位置姿勢ｘ_ｔに制御量ｕ_ｔを作用させることで、時刻ｔ＋１にＴＣＰの位置姿勢がｘ_ｔ＋１に遷移する系を考える。このとき操作物体はエンドエフェクタ１２に把持されているため、動作に伴って状態ｓ_ｔから状態ｓ_ｔ＋１に遷移する。制御量ｕ_ｔによって変化した物体座標系での相対位置姿勢の移動量をａ_ｔとおく。ただし移動中に操作物体の把持姿勢はノイズν_ｔだけ変化しうるとし、ａ_ｔにν_ｔが影響した移動量が操作物体の正味の移動量であると考える。

ν_ｔを系のシステムノイズとし、システムノイズν_ｔはある確率分布Ｐνに従うものとする。なおν_ｔも同次座標系である。

また動作中に把持姿勢がν_ｔだけ変化することにより、ｓ_ｔとｘ_ｔの同次変換行列Ｔ_ｔはν_ｔだけ変化することとなる。変化した同次変換行列が

である。

系の状態方程式を行列演算の記述で数式（２）に記す。ν_ｔとｕ_ｔおよびａ_ｔは同次座標系である。

数式（１）と（２）より、ν_ｔａ_ｔとｕ_ｔの関係として数式（４）を得る。

次に、状態観測センサ１４及び接触観測センサ１６による観測行為を考える。観測行為によって観測値ｙ_ｔが得られるものとする。観測値ｙ_ｔは状態の観測値ｓ_ｔ’と接触観測値ｃ_ｔ’から構成される。

ｙ_ｔ＝（ｓ_ｔ’，ｃ_ｔ’）

ｓ_ｔ’は物体座標系の相対位置姿勢の観測値である。ただし観測ノイズωによってｓ_ｔ’は真値ｓ_ｔからの誤差を有するものとする。ｃ_ｔ’は注目物体と操作物体の接触観測値である。ｃ_ｔ’の観測には誤差は生じないものとする。注目物体と操作物体が接触していない場合はｃ_ｔ’＝０であり、接触している場合はｃ_ｔ’＝１である。

ω_ｔを系の観測ノイズとし、観測ノイズω_ｔはある確率分布Ｐωに従うものとする。なおω_ｔも同次座標系である。

観測方程式を同次座標系の形で数式（５）に記す。

以上より、状態空間モデルを以下に定める。

状態空間モデルを確率モデルとして記述したものが数式（８）である。

ある時刻ｔまで時間が遷移した際に、観測値の集合

が与えられる。ｙ_０：ｔが与えられた際にｓ_ｔが得られる確率は以下の数式（９）で表される。

ただし、

数式（８）の状態空間モデル、および数式（９）の観測が得られた際の状態が得られる確率から、状態の制御を行う。すなわちロボット１０によって２物体間の相対位置姿勢を操作する。

以上説明したタスクの実行方法に従って、実行部１３２は、当該タスクについて、操作対象である操作物体が環境中に位置する注目物体から離れている初期状態から操作物体が注目物体に特定の態様で接触している状態である完了状態に遷移するように操作物体を操作するロボット１０を制御する。実行部１３２は、タスク毎にロボット１０を制御する。

ここで、タスクは、把持した部品を他の部品に組み付けるタスク、把持した部品を環境中の他の位置に移し変えるタスク、又は後述の部品を把持するタスクである。

具体的には、実行部１３２は、図７に示すように、ガイド設定部１４０、初期設定部１４２、行動計画部１４４、指令変換部１４６、観測部１４８、時間更新部１５０、観測更新部１５２、状態推定部１５４、反復判定部１５６、及び目標設定部１５８を備えている。なお、目標設定部１５８が、目標状態設定部の一例であり、時間更新部１５０が、粒子集合設定部の一例であり、観測更新部１５２が、粒子集合調整部の一例であり、指令変換部１４６が、行動部の一例であり、反復判定部１５６が、処理制御部の一例である。

ガイド設定部１４０は、ガイドを設定する。ガイドは、注目物体に対する操作対象物体の相対軌道の最終部分であって、注目物体に対する操作対象物体の初期相対位置にかかわらない共通の目標相対軌道とされる軌道である。ガイドは、完了状態及び完了状態に至るまでの一連の中間目標状態を含む。ここで、完了状態及び中間目標状態は、物体座標系によって表される。

例えば、ガイド設定部１４０は、単位状態遷移データについて、登録情報のうちの組み立て手順に基づいて、ガイドを作成する。ここで、ガイドは、完了状態に至る離散的な相対位置姿勢の系列である。完了状態はガイドの終端である。

ガイド設定部１４０は、動作計画装置２０に備えるようにしてもよい。その場合、ガイドも単位状態遷移データと同様にして制御装置３０に取得させる。

以下、ガイドの作成手法の一例を説明する。

図８は、部品Ｄを、部品Ａ～Ｃの組み立て体に組み付けた状態から、部品Ａ～Ｃの組み立て体に干渉せずに部品Ｄが移動可能な方向に部品Ｄを移動させた場合の部品Ｄの位置姿勢の軌跡の一例である。その軌跡の終点から始めて移動方向を逆転させた軌跡をガイドＧ１として作成する。ガイドを構成する状態の間隔は均一である必要はなく、例えば接触が予想される領域では状態を密に設定してもよい。

初期設定部１４２は、完了状態に対して一番遠い、ガイド中の中間目標状態を、今回移動の目標状態として設定する。

また、初期設定部１４２は、操作物体の位置姿勢の不確定さを表す粒子の集合であって、粒子の集合に含まれるそれぞれの粒子は操作物体のありうる位置姿勢の一つを表す、粒子の集合を初期設定する。ここで、粒子が表す操作物体の位置姿勢は、物体座標系によって表される。

行動計画部１４４は、操作物体を初期状態又は後述する推定状態から今回移動の目標状態に移動させるための行動を計画する。ここで、推定状態、及び行動は、物体座標系によって表される。

指令変換部１４６は、計画された行動をロボット１０が実行できる指令に変換して出力し、ロボット１０に指令する。

観測部１４８は、取得部１３０によって取得した状態観測データから、注目物体と操作物体の相対位置姿勢についての観測結果を取得する。ここで、観測部１４８は、注目物体と操作物体の相対位置姿勢を、物体座標系によって表される観測結果として取得する。

また、観測部１４８は、取得部１３０によって取得した接触観測データから、操作物体と注目物体との接触の有無についての観測結果を取得する。具体的には、接触観測データから、接触あるいは非接触に対応する二値に変換して、接触観測値として認識する。センサ値を二値に変換する手法は限定せず、既存の手法を用いてよい。

時間更新部１５０は、次のステップにおける粒子の集合に更新する。

観測更新部１５２は、粒子の集合に含まれる各粒子の重みを更新する。このとき、粒子のうち観測結果が示す操作物体の位置姿勢に近い位置姿勢を表す粒子ほど粒子の重みを大きく更新し、かつ、観測結果が接触が生じていることを示す場合には注目物体及び操作物体の形状及び相対位置関係が表現される仮想空間において注目物体とそれぞれの粒子が表す位置姿勢で配置された操作物体とが接触している状態に近いほど対応する粒子の重みを大きく更新する。ここで、仮想空間は、物体座標系により記述される空間である。

状態推定部１５４は、各粒子の重みが調整された粒子の集合に基づいて推定された操作物体の位置姿勢である推定状態を算出する。

反復判定部１５６は、算出される推定状態が完了状態に所定の誤差内で一致するか否かを判定する。また、反復判定部１５６は、目標設定部１５８、行動計画部１４４、指令変換部１４６、観測部１４８、時間更新部１５０、観測更新部１５２、状態推定部１５４の各処理を、推定状態が完了状態に所定の誤差内で一致するまで繰り返させる。

目標設定部１５８は、完了状態に対して遠い方から近い方に向かってガイド中の中間目標状態を今回移動の目標状態として順次設定し、完了状態を最後の今回移動の目標状態として設定する。

次に、制御システム１の作用について説明する。

図９は、本実施形態の制御システム１の処理の流れを示すシーケンス図である。ＣＰＵ２０Ａが動作計画装置２０の各部として機能することにより動作計画処理を行い、ＣＰＵ３０Ａが制御装置３０の各部として機能することにより制御処理を行う。

ステップＳ１００では、動作計画装置２０が、単位状態遷移データの各々を含む状態遷移データ及び制御データを作成する。制御データは、作成した単位状態遷移データの各々に対応する把持データの各々である。

ステップＳ１０２では、動作計画装置２０が、状態遷移データ及び制御データを状態遷移制御部２５に出力する。

ステップＳ１０４では、状態遷移制御部２５が、一連のタスク開始指示を受け付け、処理対象のタスクを開始する。タスク開始指示を受け付けた時点では処理対象として最初のタスクを開始し、タスクの進行に応じて、順次、タスクを更新していく。なお、タスク開始指示は制御装置３０で受け付けるようにしてもよい。

ステップＳ１０８では、状態遷移制御部２５が、処理対象のタスクに対応する単位状態遷移データ及び当該単位状態遷移データに対応する制御データを制御装置３０に出力する。なお、状態遷移制御部２５は、最初に、繰り返し処理の順序の最初のタスクに対応する単位状態遷移データを出力し、タスクの進行に応じて、次のタスクに対応する単位状態遷移データを出力する。

ステップＳ１１４では、制御装置３０の取得部１３０が、状態観測センサ１４及び接触観測センサ１６が観測した状態観測データ及び接触観測データを取得すると共に、実行部１３２が、処理対象のタスクを実行する。これにより、当該タスクについて、操作対象である操作物体が環境中に位置する注目物体から離れている初期状態から操作物体が注目物体に特定の態様で接触している状態である完了状態に遷移するようにロボット１０が制御される。

ステップＳ１１８では、制御装置３０の実行部１３２が、状態遷移制御部２５にタスク完了を通知する。

ステップＳ１２０では、状態遷移制御部２５が、状態遷移データの最終状態まで処理を終了したか否かを判定する。最終状態まで処理終了したと判定した場合にはステップＳ１２４へ移行する。最終状態まで処理終了していないと判定した場合には、ステップＳ１２２へ移行する。

ステップＳ１２２では、状態遷移制御部２５が、処理対象のタスクを更新し、ステップＳ１０８に戻る。タスクの更新とは、処理対象のタスクを、次のタスクに更新する処理である。タスク更新後のステップＳ１０８では、処理対象のタスクに対応した単位状態遷移データ及び制御データ（把持データ）を制御装置３０に出力し、以降の処理を繰り返す。

ステップＳ１２４では、状態遷移制御部２５が、タスクを終了し、制御システム１の処理を終了する。

図１０は、ステップＳ１１４の制御装置３０の制御処理の流れを示すフローチャートである。

ステップＳ１３０では、ガイド設定部１４０が、単位状態遷移データについて、登録情報のうちの組み立て手順に基づいて、ガイドを生成し、設定する。具体的には、注目物体と操作物体の相対位置姿勢に対する開始状態ｓ_０ ^＊、終了状態ｓ_Ｍ ^＊を定め、Ｍ＋１個の状態の集合

をガイドとして決定する。

ここで、状態集合ｓ_０：Ｍ ^＊はガイドであり、注目物体と操作物体のＣＡＤモデルを活用してガイドが生成される。たとえば、図８を参照して説明したように、ＣＡＤモデルにおいて、終了状態である注目物体に操作物体が組み付けられた状態から始めて、注目物体に干渉せずに操作物体が移動可能な方向に操作物体をＭ段階かけて少しずつ移動させた場合の操作物体の位置姿勢の軌跡を作成し、その軌跡の終点から始めて移動方向を逆転させた軌跡をガイドとして生成する。ガイドを構成する状態の間隔は均一である必要はなく、例えば接触が予想される領域では状態を密に設定してもよい。

ステップＳ１３２では、初期設定部１４２が、目標状態の初期値をガイドの始点の状態ｓ_０ ^＊にする。

ステップＳ１３４では、観測部１４８が、操作物体の相対位置姿勢を、状態観測センサ１４により得られた状態観測データから取得する。

ステップＳ１３６では、初期設定部１４２が、初期時刻ｔ＝０における任意のＮ個の状態の集合である、粒子集合

の初期値を定める。何らかの事前知識によって粒子の初期値ｓ_０ ^（ｎ）を定めてもよい。たとえば、操作物体の相対位置姿勢の観測値について想定される最大誤差の範囲内の領域に均一に分布するように初期値ｓ_０ ^（ｎ）を定めてもよい。

ここで、粒子集合は、操作物体の相対位置姿勢の不確定さを表している。個々の粒子は、操作物体のありうる相対位置姿勢の中の一つを表している。

状態の推定値＾ｓ_ｔは、粒子集合を代表する相対位置姿勢である。

また、Ｎ個の粒子にそれぞれ対応するＮ個の粒子重みの集合

の初期値を定める。初期値では重みをすべてｗ^（ｎ）＝１／Ｎとする。

最後に、粒子の時間更新に使用する粒子散布変数φを生成する確率分布Ｐ_φを定める。

粒子散布変数φは、仮想的に想定したシステムノイズに相当する。

ステップＳ１３８では、行動計画部１４４が、ガイドを構成する状態の集合の要素ｓ_ｍ ^＊のうち、現在の要素をｓ_ｔ ^＊、次のステップの要素をｓ_ｔ＋１ ^＊としたときに、ｓ_ｔ＋１ ^＊と現在の状態の推定値＾ｓ_ｔから物体座標系における移動量ａ_ｔを決定する。

なお、数式中のＸの上付きの”＾（ハット）”を、＾Ｘと示している。

状態ｓ_ｔの推定値＾ｓ_ｔは、後述するステップＳ１４８において求められる。＾ｓ_ｔの初期値は、上記ステップＳ１３４で取得した相対位置姿勢とする。なお、都度ＣＡＤを使った経路計画を行い、移動量を決定してもよい。ガイドを用いない場合には、現在の状態を起点として、数式（１）で記したように状態ｓ_ｔと位置姿勢ｘ_ｔはｓ_ｔ＝Ｔ_ｔｘ_ｔの関係を持つ。この関係を基に、数式（１２）で記したように、推定した状態＾ｓ_ｔとロボット座標系におけるＴＣＰの位置姿勢の値ｘ_ｔを変換する同次変換行列＾Ｔ_ｔを得る。＾Ｔ_ｔは、Ｔ_ｔの推定値に対応する。

決定した移動量ａ_ｔで操作物体を移動させる。ロボット１０で操作物体を移動するにあたり、推定した＾Ｔ_ｔを用いて移動量ａ_ｔをＴＣＰに対する制御量＾ｕ_ｔに変換することで、＾ｕ_ｔによってロボット１０を制御する。

ただし、実際には、システムノイズν_ｔが生じるため、操作物体はν_ｔａ_ｔだけ移動する。正味の相対位置姿勢の移動ν_ｔａ_ｔを、ロボット座標系のＴＣＰの移動量に変換したものが、実際の制御量ｕ_ｔであるが、ν_ｔは未知のためｕ_ｔを求めることはできないので、制御は＾ｕ_ｔを用いて行う。

ステップＳ１４０において、指令変換部１４６が、ロボット座標系におけるＴＣＰの移動量＾ｕ_ｔをインバースキネマティクスによりロボット１０の各関節の回転の指令量に変換して、ロボット１０に対して行動を指令する。

ステップＳ１４２において、観測部１４８が、状態観測センサ１４及び接触観測センサ１６から得られた状態観測データ及び接触観測データから、前述した物体座標系の相対位置姿勢、注目物体と操作物体との接触状態を一つにまとめた観測値ｙ_ｔ＝（ｓ_ｔ’，ｃ_ｔ’）を得る。

また、エンコーダによるロボット１０の各関節の角度の計測値を用いて、Ｆｏｒｗａｒｄ Ｋｉｎｅｍａｔｉｃｓによりロボット座標系におけるＴＣＰの位置姿勢の値としてｘ_ｔを得る。

ステップＳ１４４において、時間更新部１５０は、上記ステップＳ１４０での行動後、粒子集合

をａ_ｔと粒子散布変数φ_ｔによって

に更新する。また粒子重みを１／Ｎに正規化する。

ステップＳ１４６において、観測更新部１５２が、粒子の集合に含まれる各粒子の重みを更新する。

具体的には、まず、次のようにして干渉判定を行う。干渉判定を行うためのシミュレーション空間（仮想空間）内にはＣＡＤの形状データを用いて描写される注目物体を配置しておく。さらに、粒子集合に含まれる個々の粒子について、その粒子が表している位置姿勢の操作物体をＣＡＤの形状データを用いてシミュレーション空間内に配置する。ある粒子に基づいて配置された操作物体が注目物体と重なる部分があれば干渉ありと判定し、操作物体と注目物体とが離れていて重なる部分がなければ干渉なしと判定する。このようにして、粒子ごとに干渉の有無を判定する。また、シミュレーション上で注目物体の表面と操作物体の表面との最も近い距離である干渉距離も算出する。

このように、粒子集合

を用いてシミュレーション上でＣＡＤを用いた干渉判定を行い、得られる干渉判定の集合を

とし、干渉距離の集合を

とする。

干渉判定ｃ_ｔ ^（ｎ）はシミュレーション上でのｎ番目の粒子の注目物体と操作物体の干渉の有無であり、干渉時はｃ_ｔ ^（ｎ）＝０であり、非干渉時はｃ_ｔ ^（ｎ）＝１である。

干渉距離ｄ_ｔ ^（ｎ）はシミュレーション上でのｎ番目の粒子の注目物体表面と操作物体表面の最短距離であり、干渉時はｄ_ｔ ^（ｎ）＜０であり、非干渉時はｄ_ｔ ^（ｎ）＞０である。

確率ｐ（ｙ_ｔ｜ｓ_ｔ ^（ｎ））に対応する尤度Ｌ（ｙ_ｔ｜ｓ_ｔ ^（ｎ））を数式（１６）に定める。ｌ（ｓ_ｔ ^（ｎ）｜ｓ_ｔ’）は粒子の状態と観測状態との距離である。δ（・）はデルタ関数、τおよびσはハイパーパラメータである。

すなわち、接触観測センサ１６によって検出される注目物体と操作物体との現実の接触がない（ｃ_ｔ’＝０）場合は数式（１６）の上側の式が用いられ、現実の接触がある（ｃ_ｔ’＝１）場合は下側の式が用いられる。現実の接触がない場合の式に含まれるデルタ関数は、シミュレーション上で干渉がない粒子について値１を返し、干渉がある粒子について値０を返す。

数式（１６）に含まれる指数関数の部分はガウス関数である。関数系はガウス関数にせず、距離が近ければ尤度が高く、距離が遠ければ尤度が低くなる関数を使用してよい。例えば指数関数的減衰などを使用してよい。

尤度関数から得られた尤度値を用いて数式（１７）で粒子重みＷを更新する。

すなわち、注目物体と操作物体との現実の接触がない場合、尤度関数は、シミュレーション上で干渉がある粒子（接触なしの観測結果に反する粒子）の重みを０にし、シミュレーション上で干渉がない粒子についてはその粒子の状態ｓ_ｔ ^（ｎ）が操作物体の観測された状態ｓ_ｔ’に近いほど重みを大きくする。

一方、注目物体と操作物体との現実の接触がある場合、尤度関数は、粒子の状態ｓ_ｔ ^（ｎ）が操作物体の観測された状態ｓ_ｔ’に近いほどその粒子の重みを大きくする作用と、注目物体表面と操作物体表面との最短距離が小さいほど粒子の重みを大きくする作用とを有する。その結果、現実の接触がある場合にはシミュレーション上で注目物体と操作物体の表面同士が接触している状態に近い状態を表している粒子の重みが大きく更新され、操作物体が注目物体から大きく離れていたり操作物体が注目物体に深く侵入したりしている状態を表している粒子の重みが小さく更新される。

ステップＳ１４８において、状態推定部１５４が、各粒子の重みが調整された粒子の集合に基づいて推定された操作物体の位置姿勢である推定状態を算出する。このとき、上記のようにして、操作物体の状態の推定値＾ｓ_ｔは、操作物体の観測された位置姿勢に近づくように修正され、かつ、接触観測センサ１６により現実の接触が観測されている場合にはシミュレーション上でも操作物体と注目物体の表面同士が接触している状態に近づくように修正される。

具体的には、時刻ｔにおける状態の推定値＾ｓ_ｔは、粒子集合に対する期待値として数式（１８）から求められる。数式（１８）中のｌｏｇは行列の対数、ｅｘｐは行列の指数をそれぞれ意味している。

更新された粒子重み

に比例する確率で粒子集合

から粒子を無作為に選択し、

のリサンプリングを行う。

ステップＳ１５０において、反復判定部１５６が、状態の推定値＾ｓ_ｔが所定の誤差範囲内でガイドの終了状態ｓ_Ｍ ^＊と一致するか否かを判定する。状態の推定値＾ｓ_ｔが所定の誤差範囲内でガイドの終了状態ｓ_Ｍ ^＊と一致していれば制御処理を終了する。一致していなければＳ１５２に進む。

ステップＳ１５２では、目標設定部１５８が、目標状態をガイド中の次の状態に設定する。また、時刻を１ステップ進め、それまでのｔ＋１を新たなｔとする。そして、ステップＳ１３８に戻る。

なお、上記図１０のフローチャートに示した処理の順序は、目的が達成できる限り適宜変更してもよい。例えば、ステップＳ１４２の観測は、上記ステップＳ１４４の後に行ってもよい。

また、操作物体を把持した場所からガイド始点まで移動させる部分について、以下のようにロボット１０を制御してもよい。

まず、操作物体を把持した場所からガイド始点までの移動経路は、直線でつなぐ、もしくは従来のＭｏｔｉｏｎ Ｐｌａｎｎｉｎｇの手法を用いて作成する。また、把持した場所からガイド始点までの操作物体の移動は、例えば次のようにして行う。

まず、状態観測センサ１４のカメラで得られた状態観測データから、注目物体のカメラ座標系における位置姿勢を観測し、注目物体を基準とした座標系である物体座標系を持つ仮想空間を展開する。これにより、カメラ座標系と物体座標系とが互いに変換可能になる。

次に、ガイドを仮想空間内に配置する。ここで、ガイドはエンドエフェクタ１２ではなく操作物体の軌道を表す。

次に、カメラで得られた状態観測データから、カメラ座標系における把持前の操作物体の位置姿勢を観測し、物体座標系の位置姿勢に変換して操作物体を仮想空間内に配置する。そして、エンコーダで得られた状態観測データから、ロボット座標系におけるエンドエフェクタ１２の位置姿勢を取得する。

次に、カメラで得られた状態観測データから、カメラ座標系におけるエンドエフェクタ１２の位置姿勢を観測し、物体座標系の位置姿勢に変換してエンドエフェクタ１２を仮想空間に配置する。これにより、ロボット座標系と物体座標系とが変換可能になる。あらかじめカメラ座標系とロボット座標系の対応付けが較正されているならカメラによるエンドエフェクタ１２の観測は省略可能である。

次に、エンドエフェクタ１２で操作物体を把持する。

把持後にカメラで得られた状態観測データから、カメラ座標系における操作物体の位置姿勢を観測し、物体座標系における操作物体の位置姿勢を修正する。これにより、把持誤差が解消される。

そして、仮想空間内で、操作物体の初期の、把持されたときの位置姿勢からガイド始点における位置姿勢までの移動の経路を計画する。

次に、仮想空間内の経路計画をロボット座標系空間内の経路計画に変換し、経路計画に従って操作物体を移動させる。なお、移動中は、誤差が解消されない。

そして、移動後の操作物体およびエンドエフェクタ１２の位置姿勢を、カメラで得られた状態観測データから取得し、ガイド始点における位置姿勢に一致するように操作物体を移動させる。カメラ座標系、物体座標系、ロボット座標系相互の変換関係を修正する。これにより、ガイド始点における操作物体の位置姿勢誤差が解消される。また、座標系相互の変換誤差も小さくなる。

以上のように、本実施形態の制御システム１によれば、２物体の相対位置姿勢についての粒子集合を想定し、観測情報と尤度関数から各粒子の尤度を求めて、相対位置姿勢を推定する。推定した相対位置姿勢と次時刻での目標相対位置姿勢から、物体の移動量を決定する。推定した相対位置姿勢をロボット座標系におけるＴＣＰの位置姿勢に変換する同次変換行列を求め、変換行列から物体の移動量をＴＣＰの移動量に変換してロボット１０を制御し、２物体の相対位置姿勢を目標相対位置姿勢に至らせる。このとき、操作物体と注目物体とが接触したという情報を用いて、物体間の接触検知結果とＣＡＤを使った干渉判定シミュレーションによって２物体の相対位置姿勢を推定し、推定結果から操作物体の移動量を決定する。これにより、観測や行動にノイズが加わる不確実な環境下においてロボット１０を用いて物体を操作し、２物体の相対位置姿勢を精度よく目標の状態に至らせることができる。また、エンドエフェクタ１２と操作物体の相対位置姿勢が不明であり、かつ、制御中に変化するという制御が不確かな環境下でも、また状態観測センサ１４による相対位置姿勢の認識が誤差を含んでいるという観測が不確かな環境下でも、２物体間の相対位置姿勢を目標の状態に至らせることが可能である。

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

また、上記実施形態では、プログラムがＲＯＭ２０Ｂ（３０Ｂ）又はストレージ２０Ｄ（３０Ｄ）に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

また、上記実施形態では、エンドエフェクタ１２が、挟持部１２ａを有するグリッパ、又は吸着パッドである場合を例に説明したが、これに限定されない。物体を保持する構成であれば、他の構成でもよく、例えば、エンドエフェクタ１２が、真空吸着チャック、磁力吸着チャック、対象物をすくうヘラなどであってもよい。

また、単位状態遷移データの完了状態からガイドを生成する場合を例に説明したが、これに限定されない。単位状態遷移データとして完了状態のみでなく、完了状態に至る離散的な相対位置姿勢の系列が与えられていてもよい。

また、ガイドとして、相対位置姿勢の系列を自動的に生成する場合を例に説明したが、ガイドを生成しないようにしてもよい。この場合は、上記実施形態において、ガイドが単一の状態で構成される特殊な事例と見なせばよい。

また、ガイドが、完了状態ではない状態に至る離散的な相対位置姿勢の系列であってもよい。

また、タスクは、注目物体である部品を把持するタスクであってもよい。上記実施形態では、環境中に位置している物体を注目物体とし、エンドエフェクタ１２が把持する物体を操作物体としてきたが、部品を把持するタスクでは、エンドエフェクタ１２によって操作（把持）されるために環境中に置かれた物体を注目物体とし、エンドエフェクタ１２自体を操作物体とする。エンドエフェクタ１２はロボットアームによって操作されるものであるので、エンドエフェクタ１２もロボットによって操作される物体とみなすことができる。部品を把持するタスクでは、操作物体であるエンドエフェクタ１２が環境中に置かれた注目物体を把持した状態であってまだ注目物体を移動させていない状態が完了状態となる。

１ 制御システム
１０ ロボット
１１ アーム
１２ エンドエフェクタ
１４ 状態観測センサ
１６ 接触観測センサ
２０ 動作計画装置
３０ 制御装置
１３０ 取得部
１３２ 実行部
１４０ ガイド設定部
１４２ 初期設定部
１４４ 行動計画部
１４６ 指令変換部
１４８ 観測部
１５０ 時間更新部
１５２ 観測更新部
１５４ 状態推定部
１５６ 反復判定部
１５８ 目標設定部

Claims (7)

1. 操作対象である操作物体が環境中に位置する注目物体から離れている状態から前記操作物体が前記注目物体に特定の態様で接触している状態である完了状態に遷移するように前記操作物体を操作するロボットを制御するロボット制御装置であって、
   前記操作物体の前記完了状態に至るまでの移動の途中の目標状態である中間目標状態又は前記完了状態を今回移動の目標状態として設定する目標状態設定部と、
   前記操作物体の位置姿勢及び前記操作物体と前記注目物体との接触の有無についてのセンサによる観測結果を取得する観測部と、
   前記操作物体の位置姿勢の不確定さを表す粒子の集合であって、前記粒子の集合に含まれるそれぞれの前記粒子は前記操作物体のありうる位置姿勢の一つを表す、前記粒子の集合を設定する粒子集合設定部と、
   前記粒子のうち前記観測結果が示す前記操作物体の位置姿勢に近い位置姿勢を表す粒子ほど粒子の重みを大きくし、かつ、前記観測結果が前記接触が生じていることを示す場合には前記注目物体及び前記操作物体の形状及び相対位置関係が表現される仮想空間において前記注目物体とそれぞれの前記粒子が表す位置姿勢で配置された前記操作物体とが接触している状態に近いほど対応する前記粒子の重みを大きくする粒子集合調整部と、
   各前記粒子の重みが調整された前記粒子の集合に基づいて推定された前記操作物体の位置姿勢である推定状態を算出する状態推定部と、
   前記操作物体を前記推定状態から前記今回移動の目標状態に移動させるための行動を計画する行動計画部と、
   計画された前記行動の実行を前記ロボットに指令する行動部と、
   前記目標状態の設定、前記観測結果の取得、前記粒子の集合の設定、前記粒子の集合の調整、前記推定状態の算出、前記行動の計画、及び前記行動の実行を、前記推定状態が前記完了状態に所定の誤差内で一致するまで繰り返させる処理制御部と
   を備えたロボット制御装置。
2. さらに、前記完了状態及び前記完了状態に至るまでの一連の前記中間目標状態を含むガイドを設定するガイド設定部を備え、
   前記目標状態設定部は、前記完了状態に対して遠い方から近い方に向かって前記ガイド中の前記中間目標状態を前記今回移動の目標状態として順次設定し、前記完了状態を最後の前記今回移動の目標状態として設定する
   請求項１に記載のロボット制御装置。
3. 前記完了状態、前記中間目標状態、前記粒子が表す前記操作物体の位置姿勢、前記推定状態、及び前記行動は、前記注目物体を基準とする相対座標系である物体座標系によって表され、
   前記仮想空間は、前記物体座標系により記述される空間であり、
   前記観測部は、前記センサにより検出された前記操作物体の位置姿勢を前記物体座標系によって表される観測結果として提供し、
   前記行動部は、前記行動を前記ロボットが実行できる指令に変換して出力する
   請求項１又は請求項２に記載のロボット制御装置。
4. 前記操作物体は、前記ロボットのエンドエフェクタによって操作される物体である
   請求項１～請求項３の何れか１項に記載のロボット制御装置。
5. 前記操作物体は、前記ロボットのアームに取り付けられているエンドエフェクタであり、前記注目物体は、前記完了状態において前記エンドエフェクタによる保持が完了する保持対象物体である
   請求項１～請求項３の何れか１項に記載のロボット制御装置。
6. 操作対象である操作物体が環境中に位置する注目物体から離れている状態から前記操作物体が前記注目物体に特定の態様で接触している状態である完了状態に遷移するように前記操作物体を操作するロボットを制御するロボット制御方法であって、
   前記操作物体の前記完了状態に至るまでの移動の途中の目標状態である中間目標状態又は前記完了状態を今回移動の目標状態として設定し、
   前記操作物体の位置姿勢及び前記操作物体と前記注目物体との接触の有無についてのセンサによる観測結果を取得し、
   前記操作物体の位置姿勢の不確定さを表す粒子の集合であって、前記粒子の集合に含まれるそれぞれの前記粒子は前記操作物体のありうる位置姿勢の一つを表す、前記粒子の集合を設定し、
   前記粒子のうち前記観測結果が示す前記操作物体の位置姿勢に近い位置姿勢を表す粒子ほど粒子の重みを大きくし、かつ、前記観測結果が前記接触が生じていることを示す場合には前記注目物体及び前記操作物体の形状及び相対位置関係が表現される仮想空間において前記注目物体とそれぞれの前記粒子が表す位置姿勢で配置された前記操作物体とが接触している状態に近いほど対応する前記粒子の重みを大きくし、
   各前記粒子の重みが調整された前記粒子の集合に基づいて推定された前記操作物体の位置姿勢である推定状態を算出し、
   前記操作物体を前記推定状態から前記今回移動の目標状態に移動させるための行動を計画し、
   計画された前記行動の実行を前記ロボットに指令し、
   前記目標状態の設定、前記観測結果の取得、前記粒子の集合の設定、前記粒子の集合の調整、前記推定状態の算出、前記行動の計画、及び前記行動の実行を、前記推定状態が前記完了状態に所定の誤差内で一致するまで繰り返させる
   処理をコンピュータに実行させるロボット制御方法。
7. 操作対象である操作物体が環境中に位置する注目物体から離れている状態から前記操作物体が前記注目物体に特定の態様で接触している状態である完了状態に遷移するように前記操作物体を操作するロボットを制御するためのロボット制御プログラムであって、
   前記操作物体の前記完了状態に至るまでの移動の途中の目標状態である中間目標状態又は前記完了状態を今回移動の目標状態として設定し、
   前記操作物体の位置姿勢及び前記操作物体と前記注目物体との接触の有無についてのセンサによる観測結果を取得し、
   前記操作物体の位置姿勢の不確定さを表す粒子の集合であって、前記粒子の集合に含まれるそれぞれの前記粒子は前記操作物体のありうる位置姿勢の一つを表す、前記粒子の集合を設定し、
   前記粒子のうち前記観測結果が示す前記操作物体の位置姿勢に近い位置姿勢を表す粒子ほど粒子の重みを大きくし、かつ、前記観測結果が前記接触が生じていることを示す場合には前記注目物体及び前記操作物体の形状及び相対位置関係が表現される仮想空間において前記注目物体とそれぞれの前記粒子が表す位置姿勢で配置された前記操作物体とが接触している状態に近いほど対応する前記粒子の重みを大きくし、
   各前記粒子の重みが調整された前記粒子の集合に基づいて推定された前記操作物体の位置姿勢である推定状態を算出し、
   前記操作物体を前記推定状態から前記今回移動の目標状態に移動させるための行動を計画し、
   計画された前記行動の実行を前記ロボットに指令し、
   前記目標状態の設定、前記観測結果の取得、前記粒子の集合の設定、前記粒子の集合の調整、前記推定状態の算出、前記行動の計画、及び前記行動の実行を、前記推定状態が前記完了状態に所定の誤差内で一致するまで繰り返させる
   処理をコンピュータに実行させるためのロボット制御プログラム。