JP2016120593A - ロボット制御装置及び組立て方法 - Google Patents

Abstract

【課題】接触面が面取りされていなくても部品の嵌め合わせを行うことができるロボット制御装置及び組立て方法を提供することを目的とする。【解決手段】ロボット制御装置は、複数のアームと関節を含むマニピュレータ部と、マニピュレータ部に連結された把持部と、力センサーとを含むロボットにより部品の組み立てを行わせるロボットの制御装置において、把持部の接触状態を表す情報と、把持部を制御するための複数の制御情報のいずれかを基に、ロボットを制御する。【選択図】図３

Description

ロボット制御装置及び組立て方法に関する。

電子機器の製造工程において、組立てにおける多くの作業は自動化されている。この自動化された製造ラインでは、ロボットを用いて組み立てが行われている。例えば、ロボットの製造装置は、第１の部品に第２の部品を挿入するように制御していた。

特開平７−２４１７３３号公報

しかしながら、特許文献１の技術において、対象物を穴形状の部位にはめ合わせる場合、それらの接触面は、各々面取りがされていた。このため、特許文献１の技術では、接触した際の力やモーメントに倣うことで嵌め合いが実現出来ない場合、組み合わせ作業を行うことができないという課題があった。

本発明は上記事情に鑑みてなされたものであり、接触面が面取りされていなくても部品の嵌め合わせを行うことができるロボット制御装置及び組立て方法を提供することを目的としている。

上記の課題を解決するため、本発明の一態様であるロボット制御装置は、複数のアームと関節を含むマニピュレータ部と、前記マニピュレータ部に連結された把持部と、力センサーとを含むロボットにより部品の組み立てを行わせるロボットの制御装置において、
前記把持部の接触状態を表す情報と、前記把持部を制御するための複数の制御情報のいずれかを基に、前記ロボットを制御する。
また、本発明の一態様であるロボット制御装置において、前記制御情報は、予め検出された前記力センサーの検出値と前記接触状態を表す情報と、が関連づけられて複数組、記憶部に記憶されているようにしてもよい。
また、本発明の一態様であるロボット制御装置は、前記力センサーの検出値と、前記記憶部に記憶されている情報とを比較し、前記比較した結果に基づいて、前記接触状態を判別して前記制御情報を選択する選択部を備えるようにしてもよい。
また、本発明の一態様であるロボット制御装置は、前記接触状態毎に、前記力センサーの検出値を離散化し、前記記憶部に前記離散化後の検出値を記憶させる離散化部を備えるようにしてもよい。
また、本発明の一態様であるロボット制御装置において、前記把持部の接触状態は、前記把持部が把持する第１部品についての第２部品との接触状態であるようにしてもよい。
上記の課題を解決するため、本発明の一態様である組み立て方法は、ロボット制御装置により制御される部品の組み立て方法であって、第１部品を把持する把持部に働く力とモーメントを力センサーが検出して検出値として出力した前記検出値に基づいて、前記第１部品と第２部品との接触状態を判別し、判別した結果に基づいて、記憶部に前記接触状態に関連付けられて記憶されている情報を選択する選択手順と、前記選択した前記情報に基づいて、前記把持部を制御する制御手順と、を含む。
以下に態様例を示す。
一態様であるロボット装置は、第１部品を把持する把持部と、前記把持部に働く力とモーメントを検出値として検出する力センサーと、前記第１部品と第２部品とが接触している接触状態と遷移情報とが関連づけられて記憶され、予め検出された前記検出値である第１検出値と前記接触状態と、が関連づけられて複数組、記憶され、前記遷移情報と前記把持部を制御する制御値と、が関連づけられて複数組、記憶されている記憶部と、予め前記第１部品と前記第２部品とが接触している接触状態毎に、前記第１検出値を離散化し、前記記憶部に離散化後の検出値である第２検出値として記憶させ、前記第１部品と前記第２部品との接触状態の制御時に、前記検出値を離散化した離散化後の検出値を選択部に出力する離散化部と、前記検出値と、前記記憶部に記憶されている前記第１検出値とを比較し、比較した結果に基づいて、前記第１部品と前記第２部品との接触状態を判別し、前記離散化後の検出値と、前記第２検出値とを比較し、比較した結果に基づいて、前記第１部品と前記第２部品との接触状態を判別し、判別した結果に基づいて前記記憶部に記憶されている前記遷移情報を選択する選択部と、前記選択部が選択した前記遷移情報に基づいて、前記記憶部に記憶されている前記制御値を選択し、選択した前記制御値を用いて前記把持部を制御する制御部と、を備える。
このように構成したことにより、ロボット装置は、第１部品と第２部品との接触状態を力センサーの検出値に基づいて決定することができる。また、ロボット装置は、第１部品と第２部品との接触状態毎に、次の目標状態が記憶されている遷移情報に基づいて、第１部品と第２部品との接触状態を遷移させることができる。このため、ロボット装置は、接触面が面取りされていなくても部品の嵌め合わせを行うことができる。また、このように構成したことにより、ロボット装置は、第１部品と第２部品との接触状態を判別するための演算量を削減することができる。
一態様であるロボット装置において、前記遷移情報は、前記第１部品を前記第２部品に対して第１接触状態にさせ、前記第１接触状態後、前記第１部品を前記第２部品に対して第２接触状態にさせ、前記第２接触状態後、前記第１部品を前記第２部品の穴の面に対して第３接触状態にさせ、前記第３接触状態後、前記第１部品が、前記第２部品の穴に接している面に沿わせて、前記第１部品を前記第２部品の穴に取り付けるように順次、遷移する情報を含むようにしてもよい。
このように構成したことにより、ロボット装置は、第１部品と第２部品との接触状態に応じた遷移情報を用いて、第１部品と第２部品の穴に嵌め合わせるように制御することができる。このため、ロボット装置は、接触面が面取りされていなくても部品の嵌め合わせを行うことができる。なお、第１接触状態とは、例えば、第１部品と第２部品とが点接触している状態であり、第２接触状態とは、直線上での２点以上で点接触している状態であり、第３接触状態とは、同一面での２点以上で点接触している状態である。
一態様であるロボット装置において、前記離散化部は、前記検出値を、前記第１部品に対する物体座標系において３値化し、前記検出値を、前記第２部品に対する絶対座標系において３値化するようにしてもよい。
一態様であるロボット装置において、前記選択部は、前記離散化後の検出値と一致する前記第２検出値が複数ある場合、前記検出値と、前記第１検出値とを比較し、比較した結果に基づいて、前記第１部品と前記第２部品との接触状態を判別するようにしてもよい。
一態様であるロボット装置において、前記記憶部には、前記絶対座標系において前記第１検出値を離散化後の検出値である第３検出値と、前記物体座標系において第１検出値を離散化後の検出値である第４検出値と、前記第１検出値、複数の前記遷移情報および複数の前記制御値とが、前記接触状態毎に各々関連づけられて記憶され、前記選択部は、前記絶対座標系において、前記離散化後の検出値と前記第３検出値とを比較したとき、前記離散化後の検出値と一致する前記第３検出値が複数ある場合、前記物体座標系における前記離散化後の検出値と、前記第４検出値と比較し、比較した結果に基づいて、前記第１部品と前記第２部品との接触状態を判別するようにしてもよい。
一態様であるロボット装置において、前記第１部品と前記第２部品との接触状態を撮像する撮像装置を備え、前記選択部は、前記撮像装置が撮像した画像に基づいて、前記第１部品と前記第２部品との状態を識別するようにしてもよい。
一態様であるロボット装置において、前記把持部が取り付けられている第１アームと、前記把持部または前記第１部品と前記第２部品との接触状態を撮像する撮像装置が取り付けられている第２アームと、前記第１アームと前記第２アームとが取り付けられている本体と、前記本体に取り付けられている搬送部と、を備えるようにしてもよい。
一態様である組み立て方法は、第１部品と第２部品とが接触している接触状態と遷移情報とが関連づけられて記憶され、予め検出された検出値である第１検出値と前記接触状態と、が関連づけられて複数組、記憶され、前記遷移情報と把持部を制御する制御値と、が関連づけられて複数組、記憶されている記憶部を用いて、前記第１部品を把持する前記把持部と、前記把持部に働く力とモーメントを前記検出値として検出する力センサーとを有するロボット装置を制御して前記第１部品と第２部品とを組み立てる組み立て方法であって、離散化部が、予め前記第１部品と前記第２部品とが接触している接触状態毎に、前記第１検出値を離散化し、前記記憶部に離散化後の検出値である第２検出値として記憶させる記憶手順と、前記離散化部が、前記第１部品と前記第２部品との接触状態の制御時に、前記検出値を離散化した離散化後の検出値を選択部に出力する出力手順と、前記選択部が、前記検出値と、前記記憶手順によって前記記憶部に記憶されている前記第１検出値とを比較し、比較した結果に基づいて、前記第１部品と前記第２部品との接触状態を判別し、前記離散化後の検出値と、前記第２検出値とを比較し、比較した結果に基づいて、前記第１部品と第２部品との接触状態を判別し、判別した結果に基づいて、前記記憶部に前記接触状態に関連づけられて記憶されている遷移情報を選択する選択手順と、制御部が、前記選択手順が選択した前記遷移情報に基づいて、前記記憶部に記憶されている前記制御値を選択し、選択した前記制御値を用いて前記把持部を制御する制御手順と、を含む。
一態様である組み立てプログラムは、第１部品と第２部品とが接触している接触状態と遷移情報とが関連づけられて記憶され、予め検出された検出値である第１検出値と前記接触状態と、が関連づけられて複数組、記憶され、前記遷移情報と把持部を制御する制御値と、が関連づけられて複数組、記憶されている記憶部を用いて、前記第１部品を把持する前記把持部と、前記把持部に働く力とモーメントを前記検出値として検出する力センサーとを有し、前記第１部品と第２部品とを組み立てるロボット装置が備えるコンピューターに、予め前記第１部品と前記第２部品とが接触している接触状態毎に、前記第１検出値を離散化し、前記記憶部に離散化後の検出値である第２検出値として前記記憶部に記憶させるステップと、前記第１部品と前記第２部品との接触状態の制御時に、前記検出値を離散化した離散化後の検出値を選択部に出力するステップと、前記検出値と、前記記憶部に記憶されている前記第１検出値とを比較し、比較した結果に基づいて、前記第１部品と前記第２部品との接触状態を判別し、前記離散化後の検出値と、前記第２検出値とを比較し、比較した結果に基づいて、前記第１部品と第２部品との接触状態を判別し、判別した結果に基づいて、前記記憶部に前記接触状態に関連づけられて記憶されている遷移情報を選択するステップと、前記選択した前記遷移情報に基づいて、前記記憶部に記憶されている前記制御値を選択し、選択した前記制御値を用いて前記把持部を制御するステップと、を実行させる。

第１実施形態に係るロボットの概略斜視図である。 第１実施形態に係る力センサーの検出値を説明する図である。 第１実施形態に係るロボット制御装置の機能構成を表すブロック図である。 第１実施形態に係る座標系を説明する図である。 第１実施形態に係るテーブル記憶部に記憶されている遷移情報に基づく状態遷移図である。 第１実施形態に係るテーブル記憶部に記憶されている絶対座標系における状態ｐ１から状態ｐ８の力センサーの離散化後の検出値を示す図である。 第１実施形態に係るテーブル記憶部に記憶されている絶対座標系または物体座標系における状態ｓ２から状態ｓ１４の力センサーの離散化後の検出値を示す図である。 第１実施形態に係るテーブル記憶部に記憶されている物体座標系における状態ｐ１から状態ｐ８の力センサーの離散化後の検出値を示す図である。 第１実施形態に係るテーブル記憶部に記憶されている指令値を説明する図である。 第１実施形態に係るテーブルをテーブル記憶部に記憶する手順のフローチャートである。 第１実施形態に係る第１部品と第２部品との接触状態を判別し、第１部品と第２部品との接触状態を遷移させる手順のフローチャートである。 第２実施形態に係るｘｙ平面における第１部品における寸法と、第２部品２１０の穴２１１の寸法を説明する図である。 第２実施形態に係る第１部品と第２部品における絶対座標系と物体座標系および各頂点を説明する図である。 第２実施形態に係るテーブル記憶部に記憶されている遷移情報に基づく状態遷移図である。 第２実施形態に係るテーブル記憶部に記憶されている状態ｐ１０１から状態ｐ１０８の力センサーの離散化後の検出値を示す図である。 第２実施形態に係るテーブル記憶部に記憶されている絶対座標系における状態ｔｐ１０１から状態ｔｐ１０８の力センサーの離散化後の検出値を示す図である。 第２実施形態に係るテーブル記憶部に記憶されている絶対座標系における状態ｌ１０１から状態ｌ１１２の力センサーの離散化後の検出値を示す図である。 第２実施形態に係るテーブル記憶部に記憶されている絶対座標系における状態ｓ１０１から状態ｓ１１０の力センサーの離散化後の検出値を示す図である。 第２実施形態に係るテーブル記憶部に記憶されている状態ｐ１０１〜ｐ１０８からの状態遷移毎の指令値を説明する図である。 第２実施形態に係るテーブル記憶部に記憶されている状態ｔｐ１０１〜ｔｐ１０８からの状態遷移毎の指令値を説明する図である。 第２実施形態に係るテーブル記憶部に記憶されている状態ｌ１０２、ｌ１０３、ｌ１０５、ｌ１０６、ｌ１０８、ｌ１０９、ｌ１１１、ｌ１１２からの状態遷移毎の指令値を説明する図である。 第２実施形態に係るテーブル記憶部に記憶されている状態ｌ１０１、ｌ１０４、ｌ１０７、ｌ１１０からの状態遷移毎の指令値を説明する図である。 第２実施形態に係るテーブル記憶部に記憶されている状態ｓ１０１、ｓ１０２、ｓ１０３、ｓ１０４、ｓ１０５、ｓ１０６、ｓ１０７、ｓ１０８、ｓ１０９からの状態遷移毎の指令値を説明する図である。 第２実施形態に係る第１部品２００と第２部品２１０との接触状態を判別し、第１部品と第２部品との接触状態を遷移させる手順のフローチャートである。 第３実施形態に係るテーブル記憶部に記憶されている第１部品の頂点Ｅ１が穴に入っている状態の後の遷移情報に基づく状態遷移図である。 第３実施形態に係るテーブル記憶部に記憶されている第１部品の頂点Ｆ１が穴に入っている状態の後の遷移情報に基づく状態遷移図である。 第３実施形態に係るテーブル記憶部に記憶されている第１部品の頂点Ｇ１が穴に入っている状態の後の遷移情報に基づく状態遷移図である。 第３実施形態に係るテーブル記憶部に記憶されている第１部品の頂点Ｈ１が穴に入っている状態の後の遷移情報に基づく状態遷移図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｐ２０１およびｐ２０２の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｔｐ２０１からｔｐ２０４の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｌ２０１からｌ２０９の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｓ２０１からｓ２０４の力センサー２の離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｐ３０１からｐ３０３の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｔｐ３０３からｔｐ３０４の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｌ３０１からｌ３０８の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｓ３０１の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｐ４０１からｐ４０３の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｔｐ４０１からｔｐ４０２の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｌ４０１からｌ４０８の力センサーの３値化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｓ４０１の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｐ５０１からｐ５０３の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｔｐ５０１からｔｐ５０５の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｌ５０３からｌ５０８の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合、テーブル記憶部に記憶されている状態ｓ５０１の力センサーの離散化後の検出値を示す図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｐ２０１、ｐ２０２からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｔｐ２０１、ｔｐ２０２、ｔｐ２０４からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｌ２０１、ｌ２０２、ｌ２０４、ｌ２０８、ｌ２０９からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｅ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｓ２０１、ｓ２０２からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｐ３０１、ｐ３０２、ｐ３０３からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｔｐ３０３、ｔｐ３０４からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｌ３０１、ｌ３０２、ｌ３０８からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｆ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｓ３０１、ｓ３０２、ｓ３０３からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｐ４０１、ｐ４０２からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｔｐ４０１、ｔｐ４０２からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｌ４０１、ｌ４０８からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｇ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｓ４０１、ｓ４０４からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｐ５０１、ｐ５０２からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｔｐ５０１、ｔｐ５０４、ｔｐ５０５からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｌ５０３、ｌ５０４、ｌ５０８からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る初期姿勢が第１部品の頂点Ｈ１が穴に入っている状態の場合において、テーブル記憶部に記憶されている状態ｓ５０１、ｓ５０３からの状態遷移毎の指令値を説明する図である。 第３実施形態に係る第１部品２００と第２部品２１０との接触状態を判別し、第１部品２００と第２部品２１０との接触状態を遷移させる手順のフローチャートである。 第４実施形態に係る本実施形態に係るロボットの概略斜視図である。

以下、本発明を実施するための形態について、図面を参照して詳細に説明する。

［第１実施形態］
本実施形態であるロボット装置は、力センサーが検出した検出値に基づいて、嵌め合わせを行う第１部品と第２部品との接触状態を判別し、判別した結果と記憶部に記憶されている状態遷移に基づいて、第１部品と第２部品との接触状態を制御する。
また、本実施形態であるロボット装置は、第１部品と第２部品の接触状態を、第１接触状態にさせ、第１接触状態にさせた後に第２接触状態にさせ、第２接触状態にさせた後に第３接触状態にさせることで嵌め合わせを行う。さらに、本実施形態であるロボット装置は、第３接触状態において、第１部品が、第２部品の穴に接している面に沿わせて、第１部品を第２部品の穴に取り付ける（嵌め合わせる）ように順次、遷移させる。

図１は、本実施形態に係るロボット装置１の概略斜視図である。
図１に示すように、ロボット装置１は、ロボット制御装置１０、多関節ロボット２０を備える。
なお、図１における部品や構造等の縮尺は、図を明瞭なものとするために実際のものとは異なっている。

ロボット制御装置１０は、多関節ロボット２０の動作を制御する。
多関節ロボット２０は、地面に固定された支持台２０ａ、旋回可能および屈伸可能に支持台２０ａに連結されたマニピュレータ部２０ｂ、回動可能および首振り可能にマニピュレータ部２０ｂに連結された把持部２０ｃ、力センサー２０ｄ、撮像装置３０を含んで構成される。
多関節ロボット２０は、例えば６軸の垂直多関節ロボットであり、支持台２０ａ、マニピュレータ部２０ｂ、及び把持部２０ｃとの連係した動作によって６軸の自由度を有し、把持部２０ｃが把持または挟持する部品の位置および向きを自在に変更することができる。なお、本実施形態においては、把持または挟持することを「把持する」として説明する。多関節ロボット２０は、ロボット制御装置１０による制御によって、マニピュレータ部２０ｂ、把持部２０ｃのうちいずれか一つまたは組み合わせを動かす。
力センサー２０ｄは、把持部２０ｃに加わる力とモーメントを検出する。力センサー２０ｄは、多関節ロボット２０の自由度が６軸の場合、６軸力センサーである。
なお、多関節ロボット２０の自由度は６軸によるものに限られない。また、支持台２０ａは、床、壁、天井等、地面に対して固定された場所に設置してもよい。

符号２００は、第２部品２１０に組み合わせられる第１部品（「ｐｅｇ」、「くい」とも言う）である。第１部品２００は、直方体または立方体である。
第２部品２１０は、第１部品２００が組み合わされる穴２１１を有している。なお、穴２１１は、第２部品２１０を貫通していても、貫通していなくてもよい。
なお、符号２２０は、後述するように、第１部品２００と第２部品２１０との接触状態を、２次元で表す場合に用いる平面である。
なお、第１部品２００と第２部品２１０との接触状態とは、第１部品２００と第２部品との様態であり、第１部品２００と第２部品とが点接触している状態（第１接触状態）、線接触している状態（第２接触状態）、面接触している状態（第３接触状態）、第１部品２００が穴２１１に入っている状態等を含む状態である。また、線接触している状態とは、直線上での２点以上で点接触している状態である。また、面接触している状態とは、同一面での２点以上で点接触している状態である。点接触している状態とは、より具体的には、例えば後述するように、第１部品２００の頂点が、第２部品の辺または面（第２部品の表面、穴２１１の各辺、穴の各面）と接している状態である。

撮像装置３０は、例えば、第１部品２００と第２部品２１０の穴２１１との嵌め合わせ状態が確認できる位置に設置されている。撮像装置３０は、例えばＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）カメラである。撮像装置３０は、撮影した画像データをロボット制御装置１０に出力する。なお、撮像装置３０で撮像した画像データは、本実
施形態の制御に使用しても、使用しなくてもよい。

図２は、本実施形態に係る力センサー２０ｄの検出値を説明する図である。
図２に示すように、力センサー２０ｄの検出値（ｆｘ，ｆｙ，ｆｚ、ｎｘ、ｎｙ、ｎｚ）は、直交する３軸に関する力（ｆｘ，ｆｙ，ｆｚ）と、各軸のモーメント（ｎｘ、ｎｙ、ｎｚ）の合計６つを含んでいる。なお、例えば、ｆｘはｘ軸方向の力を表し、ｎｘは、ｘ軸方向のモーメントを表している。なお、力センサーは力覚センサーとも呼ばれるものである。

図３は、本実施形態に係るロボット制御装置１０の機能構成を表すブロック図である。
図３に示すように、ロボット制御装置１０は、センサー検出値取得部１０１、離散化部１０２、テーブル記憶部（記憶部）１０３、選択部１０４、制御部１０５、制御記憶部１０６、および画像取得部１０７を備えている。また、図３に示すように、ロボット制御装置１０は、多関節ロボット２０に接続されている。

センサー検出値取得部１０１は、力センサー２０ｄが出力する検出値を取得し、取得した検出値を離散化部１０２に出力する。センサー検出値取得部１０１は、テーブル作成時、予め検出された検出値を取得し、取得した検出値（離散化前の検出値、第１検出値という）をテーブル記憶部１０３に記憶させる。

離散化部１０２は、テーブル作成時、センサー検出値取得部１０１が出力した離散化前の検出値を離散化し、離散化した検出値（離散化後の検出値、第２検出値という）をテーブル記憶部１０３に記憶させる。離散化部１０２は、例えば検出値を３値（−１、０、＋１）化することで離散化する。なお、離散化部１０２は、絶対座標系および物体座標系について、各々離散化後の検出値を、テーブル記憶部１０３に記憶させる。なお、テーブル作成時、離散化部１０２は、例えば、嵌め合わせを行う対象物の剛性、対象物に加える押し付け力およびコンプライアンス・センターの位置を考慮して、所定のしきい値に対する力センサー２０ｄの出力の大小で、離散化する値を決定する。
離散化部１０２は、組み立て処理時、離散化後の検出値を選択部１０４に出力する。また、絶対座標系における離散化後の検出値を第３検出値と称し、物体座標系における離散化後の検出値を第４検出値と称する。また、第３検出値と第４検出値を総称して、第２検出値ともいう。

テーブル記憶部１０３には、第１検出値、第３検出値、第４検出値、および接触状態が関連づけられた組み合わせが、表形式で記憶されている。テーブル記憶部１０３には、後述するように、現在の接触状態から次の目標状態へ遷移させる遷移情報が、接触状態と関連づけられて記憶されている。テーブル記憶部１０３には、後述するように、当該状態から次の目標状態へ遷移させる指令値が、接触状態と関連づけられて記憶されている。

選択部１０４は、組み立て処理時（制御時）、離散化部１０２が出力した離散化後の検出値と、テーブル記憶部１０３に記憶されている第２検出値とを比較する。選択部１０４は、比較した結果に基づいて、第１部品２００と第２部品２１０との接触状態を判別し、判別した結果に基づいて、テーブル記憶部１０３に予め記憶されている遷移情報の中から、現在の接触状態から次の目標状態に遷移させる遷移情報を選択する。選択部１０４は、選択した遷移情報を制御部１０５に出力する。
また、撮像装置３０が撮像した画像データも用いて接触状態を判別する場合、選択部１０４は、画像取得部１０７が出力した画像データも使用して、第１部品２００と第２部品２１０との接触状態を判別する。なお、選択部１０４は、画像データに基づいて、周知の画像認識技術を用いて、第１部品２００と第２部品２１０との接触状態を判別する。

制御部１０５は、選択部１０４が出力した遷移情報に基づいて、テーブル記憶部１０３に記憶されている各接触状態から次の目標状態へ遷移させる指令値（以下、指令値という）を読み出す。制御部１０５は、読み出した各接触状態から次の目標状態へ遷移させる指令値に基づいて、多関節ロボット２０のマニピュレータ部２０ｂ、または把持部２０ｃを制御する。
制御記憶部１０６には、多関節ロボット２０の制御プログラム、制御値等が記憶されている。
画像取得部１０７は、撮像装置３０が撮像した画像データを取得し、取得した画像データを選択部１０４に出力する。

以上のように、本実施形態のロボット装置１は、第１部品２００を把持する把持部２０ｃと、把持部２０ｃに働く力とモーメントを検出値として検出する力センサー２０ｄと、第１部品２００と第２部品２１０とが接触している接触状態と遷移情報とが関連づけられて記憶されている記憶部（テーブル記憶部１０３）と、検出値に基づいて、第１部品２００と第２部品２１０との接触状態を判別し、判別した結果に基づいて記憶部（テーブル記憶部１０３）に記憶されている遷移情報を選択する選択部１０４と、選択部１０４が選択した遷移情報に基づいて、把持部２０ｃを制御する制御部１０５と、を備える。
このように構成したことにより、ロボット装置１は、第１部品２００と第２部品２１０との接触状態を力センサー２０ｄの検出値に基づいて決定することができる。また、ロボット装置１は、第１部品２００と第２部品２１０との接触状態毎に、次の目標状態が記憶されている遷移情報に基づいて、第１部品と第２部品との接触状態を遷移させることができる。

次に、絶対座標系と物体座標系について説明する。
図４は、本実施形態に係る座標系を説明する図である。図４は、第１部品２００および第２部品２１０を、図１に示した平面２２０について示したものである。
図４において、Σ_ｏは、絶対座標を表し、Σ_ｏｂｊは、物体座標を表している。なお、本実施形態において、絶対座標とは、第１部品２００の中心を原点とした座標系である。物体座標とは、図４に示すように、第２部品２１０の穴２１１の底辺ｃｄの中心を原点とする座標系である。
図４に示すように、第１部品２００の各頂点を、それぞれＡ、Ｂ、Ｃ、Ｄで表す。
第２部品２１０の各頂点を、ａ、ｂ、ｃ、ｄで表す。また、第２部品２１０の表面において、絶対座標の負のｘ軸方向の面をａａで表し、正のｘ軸方向の面をｂｂで表す。
なお、ｘ軸方向において、第２部品２１０の穴２１１の底の辺ｃｄの幅は、第１部品２００の底の辺ＡＤ以上である。

次に、絶対座標Σ_ｏと物体座標Σ_ｏｂｊの変換について説明する。
図１を用いて説明したように、マニピュレータ部２０ｂには、力センサー２０ｄが取り付けられている。この力センサー２０ｄは、第１部品２００に加わる力（ｆ）と、モーメント（ｎ）を検出している。
図４に示したように、絶対座標系Σ_ｏにおけるベース座標系Σ_ｂの位置・姿勢が既知であるとする。この場合、順運動学により、絶対座標系Σ_ｏから見た手先座標系Σ_ｈの位置・姿勢を算出することができる。さらに、手先座標系Σ_ｈにおける物体座標系Σ_ｏｂｊが既知であるとする。このため、絶対座標Σ_ｏから物体座標Σ_ｏｂｊへの力^ｏｆ（ｆはボールド体）および^ｏｎ（ｎはボールド体）の変換は、次式（１）及び式（２）のように表される。

なお、式（１）において、^ｏｂｊｆ（ｆはボールド体）は、物体座標系における力であり、次式（３）のように表される。式（２）において、^ｏｂｊｎ（ｎはボールド体）は、物体座標系におけるモーメントであり、次式（４）のように表される。

なお、式（３）および（４）において、^ｓｅｎｆ（ｆはボールド体）は、センサー座標系Σ_ｏから見た力センサー２０ｄに加わる力である。また、^ｓｅｎｆ（ｆはボールド体）は、３つの要素（実数）を持つ、縦ベクトルである。式（４）において、^ｓｅｎｎ（ｎはボールド体）は、センサー座標系から見た力センサー２０ｄに加わるモーメントである。^ｏｂｊＰ_ｓｅｎ（Ｐはボールド体）は、物体座標系から見たセンサー座標系の原点位置を表すベクトルである。また、^ｓｅｎｆ（ｆはボールド体）は、３つの要素（実数）を持つ、縦ベクトルである。式（３）および（４）において、^ｏｂｊＲ_ｓｅｎは、物体座標系からセンサー座標系に変換する回転行列であり、３行３列の行列である。
また、式（１）および（２）において、^ｏＲ_ｏｂｊ（Ｒはボールド体）は、絶対座標系から物体座標系に変換する回転行列であり、次式（５）のように表される。

なお、式（５）において、^ｏＲ_ｂは、絶対座標系ｏからベース座標系ｂへの回転行列である。^ｂＲ_ｈは、ベース座標系ｂから手先座標系ｈへの回転行列である。^ｈＲ_ｓｅｎは、手先座標系ｈから力センサー２０ｄへの回転行列である。^ｓｅｎＲ_ｏｂｊは、力センサー２０ｄから物体座標系ｏｂｊへの回転行列である。また、^ｏＲ_ｏｂｊ、^ｏＲ_ｂ、^ｂＲ_ｈ、^ｈＲ_ｓｅｎ、^ｓｅｎＲ_ｏｂｊは、各々３行３列の行列である。

次に、テーブル記憶部１０３に記憶されている遷移情報、力センサー２０ｄの検出値、指令値について説明する。

まず、テーブル記憶部１０３に記憶されている遷移情報について説明する。
図５は、本実施形態に係るテーブル記憶部１０３に記憶されている遷移情報に基づく状態遷移図である。
図５に示すように、テーブル記憶部１０３には、第１部品２００と第２部品２１０との接触状態と遷移情報とが、状態遷移毎に関連づけられて記憶されている。図５の例において、符号ｋ１〜ｋ６、ｐ１〜ｐ８、およびｓ１〜１４は、第１部品２００と第２部品２１０に対して横から見た状態を、２次元で表している。矢印ｔ１〜ｔ６、ｔ１１〜ｔ２２、ｔ３１〜ｔ３８は、各接触状態から次の目標状態への直接遷移させることを表している。なお、遷移情報とは、例えば、矢印ｔ１１に示したように、状態ｐ１から状態ｓ７に遷移させることを示す情報である。

図５において、状態ｋ１〜ｋ６は、初期姿勢であり、第１部品２００の傾き角度は、４５［ｄｅｇ］以下であるとする。
図５に示すように、初期姿勢ｋ１〜ｋ６は、各々、矢印ｔ１〜ｔ６のように、状態ｐ１〜ｐ６へ遷移する。
矢印ｔ１１に示すように、状態ｐ１の次の目標状態は、状態ｓ７である。矢印ｔ１２〜ｔ１４に示すように、状態ｐ２の次の目標状態は、状態ｓ２〜ｓ４のいずれかの状態である。矢印ｔ１５に示すように、状態ｐ３は、状態ｓ２へ遷移する。矢印ｔ１６に示すように、状態ｐ４の次の目標状態は、状態ｓ８である。矢印ｔ１７〜ｔ１９に示すように、状態ｐ５の次の目標状態は、状態ｓ８〜ｓ１０のいずれかの状態である。矢印ｔ２０に示すように、状態ｐ６の次の目標状態は、状態ｓ６である。矢印ｔ２１に示すように、状態ｓ６の次の目標状態は、状態ｐ３である。矢印ｔ２２に示すように、状態ｓ７の次の目標状態は、状態ｐ４である。

矢印ｔ３１に示すように、状態ｓ２の次の目標状態は、状態ｐ７である。矢印ｔ３２に示すように、状態ｓ３の次の目標状態は、状態ｐ１３である。矢印ｔ３３に示すように、状態ｓ４の次の目標状態は、状態ｐ７である。矢印ｔ３４に示すように、状態ｓ８の次の目標状態は、状態ｐ８である。矢印ｔ３５に示すように、状態ｓ９の次の目標状態は、状態ｐ１４である。矢印ｔ３６に示すように、状態ｓ１０の次の目標状態は、状態ｐ８である。矢印ｔ３７に示すように、状態ｐ７の次の目標状態は、状態ｓ３である。矢印ｔ３８に示すように、状態ｐ８の次の目標状態は、状態ｓ９である。

なお、図５において、状態ｓ１、ｓ５、ｓ１１、ｓ１２が、どの状態からも遷移していず、かつ、どの状態へも遷移していない理由は、これらの状態になった場合、第２部品２１０の穴２１１の点または線に沿って、第１部品２００を組み合わせられないためである。このため、テーブル記憶部１０３には、これらの遷移情報は記憶させないようにしてもよい。

状態ｐ１からｐ８は、第１部品２００と第２部品２１０とが、点と線とで接している状態（第１接触様態）である。例えば、状態ｐ１は、第１部品の頂点Ａが、第２部品の線ａａに接している状態である。例えば、状態ｐ２は、第１部品の線ＡＢが、第２部品の頂点ａに接している状態である。
なお、３次元的に見た場合、第１部品２００と第２部品とは、線と面で接している。例えば、状態ｐ１は、第１部品の頂点Ａを含む線が、第２部品の線ａａを含む面に接している状態である。

状態ｓ１、ｓ３、ｓ５、ｓ６、ｓ７、ｓ９、ｓ１１、ｓ１２は、第１部品２００と第２部品２１０とが、線と線とで接している状態（第２接触様態）である。例えば、状態ｓ１は、第１部品の線ＡＢが、第２部品の線ａａに接している状態である。
なお、３次元的に見た場合、第１部品２００と第２部品とは、面と面で接している。例えば、状態ｓ１は、第１部品の頂点ＡＢを含む面が、第２部品の線ａａを含む面に接している状態である。

状態ｓ２、ｓ４、ｓ８、ｓ１０は、第１部品２００と第２部品２１０とが、２箇所、点と線とで接している状態（第３接触様態）である。例えば、状態ｓ２は、第１部品の線ＡＢが、第２部品の頂点ａに接し、かつ第１部品の線ＡＤが、第２部品の頂点ｂに接している状態である。例えば、状態ｓ８は、第１部品の線ＡＤが、第２部品の頂点ａに接し、かつ第１部品の線ＣＤが、第２部品の点ｂに接している状態である。
なお、３次元的に見た場合、第１部品２００と第２部品２１０とは、２箇所、線と面で接している。例えば、状態ｓ２は、第１部品の線ＡＢを含む面が、第２部品の頂点ａを含む線に接し、かつ第１部品の線ＡＤを含む面が、第２部品の頂点ｂを含む線に接している状態である。

ここで、図５の状態遷移の意味合いを説明する。
ロボット制御装置１０は、後述するように、力センサー２０ｄが検出した検出値に基づいて、図５のいずれの接触状態であるかを判別する。ロボット制御装置１０は、判別した結果に基づいて、テーブル記憶部１０３に記憶されている図５の遷移情報を用いて、次の目標状態になるように、マニピュレータ部２０ｂと把持部２０ｃを制御する。このとき、ロボット制御装置１０は、図５に示したように、第１部品２００を第２部品２１０に接触させる。その後、ロボット制御装置１０は、接触状態を維持したまま第１部品２００をずらしていくことで、第１部品２００を穴２１１に嵌め合わせていく。

次に、テーブル記憶部１０３に記憶されている力センサーの離散化後の検出値について説明する。
図６は、本実施形態に係るテーブル記憶部１０３に記憶されている絶対座標系における状態ｐ１から状態ｐ８の力センサー２０ｄの離散化後の検出値を示す図である。図７は、本実施形態に係るテーブル記憶部１０３に記憶されている絶対座標系または物体座標系における状態ｓ２から状態ｓ１４の力センサー２０ｄの離散化後の検出値を示す図である。
なお、図６に示したテーブル記憶部１０３に記憶されている離散化後の検出値は、第３検出値である。図７に示したテーブル記憶部１０３に記憶されている離散化後の検出値は、第３検出値または第４検出値である。

図６に示したように、テーブル記憶部１０３には、状態ｐ１から状態ｐ８の接触状態毎に、力センサー２０ｄが検出した検出値（ｆｘ、ｆｙ、ｆｚ、ｎｘ、ｎｙ、ｎｚ）が離散化され、離散化後の検出値が、各々、関連づけられて表形式で記憶されている。例えば、２行目に示すように、状態ｐ１の離散化後の各検出値は、ｆｘ＝０、ｆｙ＝０、ｆｚ＝１、ｎｘ＝−１、ｎｙ＝０、ｎｚ＝０である。
同様に、図７に示したように、テーブル記憶部１０３には、状態ｓ２からｓ４、ｓ６からｓ１０、ｓ１３、およびｓ１４の接触状態毎に、力センサー２０ｄが検出した検出値（ｆｘ，ｆｙ，ｆｚ、ｎｘ、ｎｙ、ｎｚ）が離散化され、離散化後の検出値が、各々、関連づけられて表形式で記憶されている。例えば、２行目に示すように、状態ｓ２の離散化後の各検出値は、ｆｘ＝０、ｆｙ＝０、ｆｚ＝１、ｎｘ＝０、ｎｙ＝０、ｎｚ＝０である。

図８は、本実施形態に係るテーブル記憶部１０３に記憶されている物体座標系における状態ｐ１から状態ｐ８の力センサー２０ｄの検出値を示す図である。なお、図８に示したテーブル記憶部１０３に記憶されている離散化後の検出値は、第４検出値である。
図８に示したように、テーブル記憶部１０３には、状態ｐ１から状態ｐ８の接触状態毎に、力センサー２０ｄが検出した離散化後の検出値（ｆｘ、ｆｙ、ｆｚ、ｎｘ、ｎｙ、ｎｚ）が離散化され、離散化後の検出値が、各々、関連づけられて表形式で記憶されている。例えば、２行目に示すように、状態ｐ１の離散化後の各検出値は、ｆｘ＝１、ｆｙ＝１、ｆｚ＝１、ｎｘ＝−１、ｎｙ＝０、ｎｚ＝０である。なお、図６から図８の−１、０、＋１の各値は、力およびモーメントの向きを表している。すなわち、図６から図８において、検出値の符号が負の場合、各軸に対して反対方向の力、または反対方向のモーメントを表している。

なお、物体座標系の状態ｓ２からｓ４、ｓ６からｓ１０、ｓ１３、およびｓ１４における、力センサー２０ｄが検出した検出値（ｆｘ、ｆｙ、ｆｚ、ｎｘ、ｎｙ、ｎｚ）の離散化後の各検出値は、図７に示した絶対座標系における状態ｓ１からｐ１４の離散化後の各検出値と同じである。また、離散化後の検出値を、図６から図８に示したが、これらの値は、テーブル記憶部１０３に、グループ分けして記憶するようにしてもよく、グループ分けせずに、まとめて記憶するようにしてもよい。

次に、テーブル記憶部１０３に記憶されている指令値について説明する。
図９は、本実施形態に係るテーブル記憶部１０３に記憶されている指令値を説明する図である。
図９に示すように、テーブル記憶部１０３には、現在の接触状態から次の目標状態に遷移させる状態毎に、マニピュレータ部２０ｂ、および把持部２０ｃを制御する指令値として、角度（θ_ｘ，θ_ｙ，θ_ｚ）、力（ｆｘ，ｆｙ，ｆｚ）が、表形式で関連づけられて記憶されている。図９に示すように、テーブル記憶部１０３には、以下の指令値が記憶されている。状態ｐ１から状態ｓ７、状態ｐ２から状態ｓ２、状態ｐ２から状態ｓ３、状態ｐ２から状態ｓ４、状態ｐ３から状態ｓ２、状態ｐ４から状態ｓ８、状態ｐ５から状態ｓ８、状態ｐ５から状態ｓ９、状態ｐ５から状態ｓ１０、状態ｐ６から状態ｓ６、状態ｓ６から状態ｐ３、状態ｓ７から状態ｐ４、状態ｓ２から状態ｓ７、状態ｓ４から状態ｐ７、状態ｐ７から状態ｓ３、状態ｓ３から状態ｓ１３、状態ｓ８から状態ｐ８、状態ｓ１０から状態ｐ８、状態ｐ８から状態ｓ９、状態ｓ９から状態ｓ１４への各状態から次の状態へ遷移させる指令値が、記憶されている。
なお、図９において、符号「→」は、現在の状態から、次の状態への状態遷移を表している。例えば、２行目の「ｐ１→ｓ７」は、状態ｐ１から状態ｓ７への状態を遷移させる指令値を表している。
なお、図９に示した角度（θ_ｘ，θ_ｙ，θ_ｚ）、力（ｆｘ，ｆｙ，ｆｚ）の各値は、回転させる方向を表している。「＋」の場合、その軸方向において正方向に回転させて、次の目標状態になるように制御する指令値である。また、「−」の場合、その軸方向において負方向に回転させて、次の目標状態になるように制御する指令値である。そして、角度（θ_ｘ，θ_ｙ，θ_ｚ）、力（ｆｘ，ｆｙ，ｆｚ）の各値は、嵌め合わせを行う第１部品２００、第２部品２１０の剛性等に応じて、設定するようにしてもよい。

次に、本実施形態におけるロボット制御装置１０が行う処理手順を説明する。
まず、図６から図８に示したテーブルを、テーブル記憶部１０３に記憶する手順を説明する。図１０は、本実施形態に係るテーブルをテーブル記憶部１０３に記憶する手順のフローチャートである。

（ステップＳ１）ロボット制御装置１０の制御部１０５は、制御記憶部１０６に記憶されている制御値を読み出す。次に、制御部１０５は、読み出した制御値に基づいて、第１部品２００と第２部品２１０を、順次、図５に示した状態ｐ１〜ｐ８、および状態ｓ１〜ｓ１４の各接触状態になるように制御する。

（ステップＳ２）センサー検出値取得部１０１は、力センサー２０ｄの検出値を取得し、取得した離散化前の検出値を第１検出値としてテーブル記憶部１０３に記憶させる。また、センサー検出値取得部１０１は、取得した離散化前の検出値を離散化部１０２に出力する。

（ステップＳ３）離散化部１０２は、センサー検出値取得部１０１が出力した離散化前の検出値を、絶対座標系から物体座標系に、上述した式（１）から（５）を用いて変換する。離散化部１０２は、絶対座標系において検出値を離散化し、離散化後の検出値を第３検出値としてテーブル記憶部１０３に記憶させる。次に、離散化部１０２は、物体座標系において検出値を離散化し、離散化後の検出値を第４検出値としてテーブル記憶部１０３に記憶させる。

（ステップＳ４）制御部１０５は、予め定められている全ての状態の検出値を取得したか否かを判別する。全ての状態の検出値を取得したと判別した場合（ステップＳ４；Ｙｅｓ）、テーブルの記憶処理を終了する。全ての状態の検出値を取得したと判別できない場合（ステップＳ４；Ｎｏ）、ステップＳ１に戻って、未処理の状態についてステップＳ１からステップＳ４を繰り返す。

次に、ロボット制御装置１０が、図５に示した状態ｐ１〜ｐ８、および状態ｓ１〜ｓ１４を判別し、遷移させる手順を説明する。図１１は、本実施形態に係る第１部品２００と第２部品２１０との接触状態を判別し、第１部品２００と第２部品２１０との接触状態を遷移させる手順のフローチャートである。

（ステップＳ１１）センサー検出値取得部１０１は、力センサー２０ｄの離散化前の検出値を取得し、取得した離散化前の検出値を離散化部１０２に出力する。

（ステップＳ１２）離散化部１０２は、センサー検出値取得部１０１が出力した離散化前の検出値を、絶対座標系から物体座標系に、上述した式（１）から（５）を用いて変換する。次に、離散化部１０２は、絶対座標系における離散化前の検出値、および物体座標系における離散化前の検出値を、各々離散化する。次に、離散化部１０２は、絶対座標系における離散化前の検出値、物体座標系における離散化前の検出値、絶対座標系における離散化後の検出値、および物体座標系における離散化後の検出値を選択部１０４に出力する。

（ステップＳ１３）選択部１０４は、離散化部１０２が出力した絶対座標系における離散化後の検出値と、テーブル記憶部１０３に記憶されている第３検出値とを比較し、比較した結果に基づいて、第１部品２００と第２部品２１０との接触状態を判別する。

（ステップＳ１４）選択部１０４は、テーブル記憶部１０３に記憶されている接触状態のうち、離散化後の検出値と一致する状態が１つであるか２つであるかを判別する。離散化後の検出値と一致する状態が１つであると判別した場合（ステップＳ１４；１つ）、選択部１０４は、ステップＳ１３で判別した結果に基づいて、遷移情報を制御部１０５に出力し、ステップＳ１８に進む。離散化後の検出値と一致する状態が２つであると判別した場合（ステップＳ１４；２つ）、ステップＳ１５に進む。なお、離散化後の検出値と一致する状態が２つあるのは、図６に示したように、状態ｐ２とｐ４、状態ｐ３とｐ５、状態ｐ１とｓ７、および状態ｐ６とｓ６である。

（ステップＳ１５）離散化後の検出値と一致する状態が２つであると判別した場合、選択部１０４は、２つの状態が状態ｐ２とｐ４、または状態ｐ３とｐ５であるか、状態ｐ１とｓ７、または状態ｐ６とｓ６であるか判別する。２つの状態が状態ｐ２とｐ４、または状態ｐ３とｐ５であると判別した場合（ステップＳ１５；状態ｐ２とｐ４、または状態ｐ３とｐ５）、ステップＳ１６に進む。２つの状態が状態ｐ１とｓ７、または状態ｐ６とｓ６であると判別した場合（ステップＳ１５；状態ｐ１とｓ７、または状態ｐ６とｓ６）、ステップＳ１７に進む。

（ステップＳ１６）２つの状態が状態ｐ２とｐ４、または状態ｐ３とｐ５であると判別し
た場合、選択部１０４は、離散化前の検出値に含まれる力ｆｘとｆｚの値と、テーブル記
憶部１０３に記憶されている第１検出値に含まれる力ｆｘとｆｚの値を比較する。次に、
選択部１０４は、比較した結果に基づいて、第１部品２００と第２部品２１０との接触状
態が、状態ｐ２であるか状態ｐ４、または状態ｐ３であるか状態ｐ５を判別する。次に、
選択部１０４は、判別した結果に基づいて、遷移情報を制御部１０５に出力する。

（ステップＳ１７）２つの状態が状態ｐ１とｓ７、または状態ｐ６とｓ６であると判別した場合、選択部１０４は、物体座標系の離散化後の検出値と、図７および図８に示したテーブル記憶部１０３に記憶されている物体座標系の第４検出値とを比較する。
具体的には、選択部１０４は、２つの状態が状態ｐ１とｓ７であると判別した場合、物体座標系の離散化後の検出値のうち、力ｆｘについて第４検出値と比較する。選択部１０４は、力ｆｘが１の場合、状態ｐ１であると判別し、力ｆｘが０の場合、状態ｓ７であると判別する。なお、選択部１０４は、力ｆｙを用いて比較してもよい。この場合、選択部１０４は、力ｆｙが１の場合、状態ｐ１であると判別し、力ｆｙが０の場合、状態ｓ７であると判別する。
選択部１０４は、２つの状態が状態ｐ６とｓ６であると判別した場合、物体座標系の離散化後の検出値のうち、力ｆｘについて第４検出値と比較する。選択部１０４は、力ｆｘが１の場合、状態ｐ６であると判別し、力ｆｘが０の場合、状態ｓ６であると判別する。
なお、選択部１０４は、力ｆｙを用いて比較してもよい。この場合、選択部１０４は、力ｆｙが−１の場合、状態ｐ６であると判別し、力ｆｙが０の場合、状態ｓ７であると判別する。
次に、選択部１０４は、判別した結果に基づいて、遷移情報を制御部１０５に出力する。

（ステップＳ１８）制御部１０５は、選択部１０４が出力した遷移情報に基づいて、第１部品２００と第２部品２１０との接触状態が目標としている状態（最終目標状態）であるか否かを判別する。なお、最終目標状態とは、図５において、状態ｓ１３、または状態ｓ１４である。第１部品２００と第２部品２１０との状態が目標としている状態ではないと判別した場合（ステップＳ１８；Ｎｏ）、ステップＳ１９に進む。第１部品２００と第２部品２１０との状態が目標としている状態であると判別した場合（ステップＳ１８；Ｙｅｓ）、第１部品２００と第２部品２１０との接触状態を遷移させる処理を終了する。

（ステップＳ１９）制御部１０５は、選択部１０４が出力した遷移情報に基づいて、テーブル記憶部１０３に記憶されている指令値を選択する。例えば、遷移情報が、状態ｐ１から状態ｓ７である場合、制御部１０５は、状態ｐ１から状態ｓ７への指令値を選択する。制御部１０５は、選択した指令値に基づいてマニピュレータ部２０ｂ、および把持部２０ｃを制御する。ステップＳ１９終了後、ステップＳ１１に戻る。

なお、ロボット制御装置１０は、図５に示した状態のうち状態ｓ１、ｓ５、ｓ１１、ｓ１２であることを撮像装置３０が撮像した画像データに基づいて判別するようにしてもよい。状態ｓ１、ｓ５、ｓ１１、ｓ１２であることを判別した場合、ロボット制御装置１０は、例えば、第１部品２００を把持し直し、初期姿勢ｋ１からｋ６のいずれかの状態から、第１部品２００と第２部品２１０との嵌め合わせを、再度、ステップＳ１から行うようにしてもよい。

ここで、ロボット制御装置１０が行う制御の一例を説明する。
図５において、ロボット制御装置１０は、初期姿勢ｋ１から、第１部品２００を、絶対座標系の負方向のｚ軸方向に状態ｐ１になるまで降ろすように制御する。状態ｐ１であることを判別した後、ロボット制御装置１０は、第１部品２００と第２部品２１０との接触状態を、状態ｓ７になるように制御する。状態ｓ７であることを判別した後、ロボット制御装置１０は、第１部品２００と第２部品２１０との接触状態を、状態ｐ４になるように制御する。状態ｐ４であることを判別した後、ロボット制御装置１０は、第１部品２００と第２部品２１０との接触状態を、状態ｓ８になるように制御する。状態ｓ８であることを判別した後、ロボット制御装置１０は、第１部品２００と第２部品２１０との接触状態を、状態ｓ９になるように制御する。状態ｓ９であることを判別した後、ロボット制御装置１０は、第１部品２００と第２部品２１０との接触状態を、状態ｓ１４になるように制御する。
また、ロボット制御装置１０は、状態ｓ９からｓ１４へは、穴２１１の辺ｂｄに、第１部品２００の辺ＣＤを接触させた状態、すなわち第１部品２００の辺ＣＤを穴２１１の辺ｂｄに押し当てたまま嵌め合わせを行っている。

以上のように、本実施形態のロボット装置１は、ロボット装置１が備える力センサー２０ｄが検出した検出値に基づいて、第１部品２００と第２部品２１０との接触状態を判別するようにした。そして、本実施形態のロボット装置１は、判別した状態に応じて、テーブル記憶部１０３に記憶されている遷移情報を選択して、第１部品２００と第２部品２１０との次の目標状態を制御するようにした。この結果、本実施形態によれば、第２部品２１０の穴２１１が面取されていなくても、第１部品を嵌め合うことができる。
さらに、本実施形態のロボット装置１は、絶対座標系における第１部品２００と第２部品２１０との接触状態を、離散化後の検出値に基づいて判別するようにした。従って、第１部品２００と第２部品２１０との接触状態の演算量を削減することができる。また、本実施形態のロボット装置１は、検出値と同じ状態が記憶部に複数記憶されている場合、絶対座標系における離散化前の検出値、絶対座標系における離散化後の検出値、および物体座標系における離散化後の検出値に基づいて判別するようにしたので、第１部品２００と第２部品２１０との接触状態を適切に判別できる。

なお、本実施形態では、検出値と一致する接触状態が記憶部に複数記憶されている場合、離散化部１０２が、検出値を絶対座標系から物体座標系に変換して、絶対座標系および限られない。ステップＳ１５で比較した結果、一致する２つの状態が状態ｐ１とｓ７、または状態ｐ６とｓ６と判別した場合、選択部１０４が絶対座標系から物体座標系に変換し、変換した検出値を離散化するようにしてもよい。これにより演算量を削減することができる。また、この場合においても、絶対座標系から物体座標系への変換、および離散化は離散化部１０２が行うようにしてもよい。

［第２実施形態］
第１実施形態では、図５に示した初期姿勢ｋ１からｋ６における第１部品２００と第２部品２１０を嵌め合わせる例を説明したが、本実施形態では、図４に示した絶対座標系におけるｘ方向の位置決めができている状態から、嵌め合わせる例を説明する。
また、本実施形態であるロボット装置は、第１部品と第２部品の接触状態を、第１接触状態にさせ、第１接触状態にさせた後に第２接触状態にさせ、第２接触状態にさせた後に第３接触状態にさせることで嵌め合わせを行う。さらに、本実施形態であるロボット装置は、第３接触状態において、第１部品が、第２部品の穴に接している面に沿わせて、第１部品を第２部品の穴に取り付ける（嵌め合わせる）ように順次、遷移させる。

なお、第１部品２００と第２部品２１０との接触状態とは、第１部品２００と第２部品との様態であり、第１部品２００と第２部品とが点接触している状態（第１接触状態）、線接触している状態（第２接触状態）、面接触している状態（第３接触状態）、第１部品２００が穴２１１に入っている状態等を含む状態である。本実施形態において、第１接触状態は、例えば、第１部品の辺と穴２１１の稜線とが１点で接している状態である。第２接触状態は、例えば、第１部品の面と穴２１１の稜線とが接触している状態である。第３接触状態は、例えば、第１部品の面と穴２１１の面とが接触している状態である。
また、ロボット装置１の概略は、第１実施形態の図１と同じ構成である。ロボット制御装置１０の構成は、第１実施形態の図３と同じ構成である。なお、ロボット制御装置１０は、ｘ方向の位置決めができている状態を、例えば、撮像装置３０が撮像した画像データに基づいて判別するようにしてもよい。

図１２は、本実施形態に係るｘｙ平面における第１部品２００における寸法と、第２部品２１０の穴２１１の寸法を説明する図である。図１３は、本実施形態に係る第１部品２００と第２部品２１０における絶対座標系と物体座標系および各頂点を説明する図である。なお、図１２において、左右方向をｘ軸方向、奥行き方向をｙ軸方向、垂直方向をｚ軸方向とする。
図１２に示すように、第１部品２００のｘ軸方向の長さはＷ_ｘであり、ｙ軸方向の長さはＷ_ｙである。また、第２部品２１０の穴２１１のｘ軸方向の長さはＷ_ｘ＋ε_ｘであり、ｙ軸方向の長さはＷ_ｙ＋ε_ｙである。すなわち、第２部品２１０の穴径の方が、第１部品２００の寸法より、ｘ軸方向にε_ｘ大きく、ｙ軸方向にε_ｙ大きい。

図１３に示すように、第２部品２１０の穴２１１の裏面ｅ１ｆ１ｇ１ｈ１の中心を、絶対座標系の原点Ｏ_ｏとする。また、ｘ軸方向をｘ_ｏで表し、ｙ軸方向をｙ_ｏで表し、ｚ軸方向をｚ_ｏで表す。第２部品２１０の穴２１１において、原点Ｏ_ｏを含む裏面側の各頂点を、各々ｅ１、ｆ１、ｇ１、およびｈ１とする。第２部品２１０の穴２１１において、原点Ｏ_ｏを含まない表面側の各頂点を、各々ａ１、ｂ１、ｃ１、およびｄ１とする。
また、図１３に示すように、第１部品２００の裏面Ｅ１Ｆ１Ｈ１Ｇ１の中心を、物体座標系の原点Ｏ_ｏｂｊとする。また、ｘ軸方向をｘ_ｏｂｊで表し、ｙ軸方向をｙ_ｏｂｊで表し、ｚ軸方向をｚ_ｏｂｊで表す。第１部品２００において、原点Ｏ_ｏｂｊを含む裏面側の各頂点を、各々Ｅ１、Ｆ１、Ｇ１、およびＨ１とする。第１部品２００において、原点Ｏ_ｏｂｊを含まない表面側の各頂点を、各々々A１、B１、C１、およびD１とする。

本実施形態における前提条件は以下である。
１．絶対座標系から見た物体座標系の姿勢の誤差を表す角度の範囲は、−４５［ｄｅｇ］〜＋４５［ｄｅｇ］未満とする。
２．物体座標系の原点と絶対座標系の原点の位置^ｏｘ_ｏｂｊおよび^ｏｙ_ｏｂｊの関係は、次式（６）で表される誤差範囲である。

３．点接触は、稜線と稜線接触のみを考慮する。
４．明らかに目標の位置・姿勢から離れる状態への遷移は、考慮しない。
５．物体座標系の原点に、コンプライアンス（剛性の逆数）・センターがあるとする。
６．第２部品２１０の穴２１１において、稜線ａ１ｂ１、またはｃ１ｄ１と第１部品２００との接触のみを考慮する。なお、稜線ａ１ｂ１、または稜線ｃ１ｄ１と第１部品２００との接触のみ考慮する理由は、稜線ｂ１ｃ１、または稜線ａ１ｄ１と第１部品２００との接触は、座標系を変えれば、同様の制御となるためである。

次に、テーブル記憶部１０３に記憶されている遷移情報、力センサー２０ｄの検出値、指令値について説明する。

まず、テーブル記憶部１０３に記憶されている遷移情報について説明する。
図１４は、本実施形態に係るテーブル記憶部１０３に記憶されている遷移情報に基づく状態遷移図である。
図１４に示すように、テーブル記憶部１０３には、第１部品２００と第２部品２１０との接触状態と遷移情報とが、接触様態毎に各々関連づけられて記憶されている。図１４において、矢印ｔ１０１〜ｔ１１６、ｔ１２１〜ｔ１２８、ｔ１３１〜ｔ１３８、ｔ１４１〜ｔ１４８、ｔ１５１〜ｔ１５８は、各接触状態から次の目標状態への直接遷移させることを表している。

図１４の例において、テーブル記憶部１０３に記憶されている遷移情報は以下の通りである。矢印ｔ１０１およびｔ１０２に示すように、状態ｐ１０１の次の目標状態は、状態ｔｐ１０１またはｌ１０２である。矢印ｔ１０３およびｔ１０４に示すように、状態ｐ１０２の次の目標状態は、状態ｔｐ１０２またはｌ１０２である。矢印ｔ１０５およびｔ１０６に示すように、状態ｐ１０３の次の目標状態は、状態ｔｐ１０３またはｌ１０５である。矢印ｔ１０７およびｔ１０８に示すように、状態ｐ１０４の次の目標状態は、状態ｔｐ１０４またはｌ１０５である。矢印ｔ１０９およびｔ１１０に示すように、状態ｐ１０５の次の目標状態は、状態ｔｐ１０５またはｌ１０８である。矢印ｔ１１１およびｔ１１２に示すように、状態ｐ１０６の次の目標状態は、状態ｔｐ１０６またはｌ１０８である。矢印ｔ１１３およびｔ１１４に示すように、状態ｐ１０７の次の目標状態は、状態ｔｐ１０７またはｌ１１１である。矢印ｔ１１５およびｔ１１６に示すように、状態ｐ１０８の次の目標状態は、状態ｔｐ１０８またはｌ１１１である。

矢印ｔ１２１に示すように、状態ｔｐ１０１の次の目標状態は、状態ｌ１０１である。矢印ｔ１２２に示すように、状態ｔｐ１０２の次の目標状態は、状態ｌ１０３である。矢印ｔ１２３に示すように、状態ｔｐ１０３の次の目標状態は、状態ｌ１０４である。矢印ｔ１２４に示すように、状態ｔｐ１０４の次の目標状態は、状態ｌ１０６である。矢印ｔ１２５に示すように、状態ｔｐ１０５の次の目標状態は、状態ｌ１０７である。矢印ｔ１２６に示すように、状態ｔｐ１０６の次の目標状態は、状態ｌ１０９である。矢印ｔ１２７に示すように、状態ｔｐ１０７の次の目標状態は、状態ｌ１１０である。矢印ｔ１２８に示すように、状態ｔｐ１０８の次の目標状態は、状態ｌ１１２である。

矢印ｔ１３１〜ｔ１３２に示すように、状態ｌ１０２、およびｌ１０３の次の目標状態は、状態ｌ１０１である。矢印ｔ１３３〜ｔ１３４に示すように、状態ｌ１０５、およびｌ１０６の次の目標状態は、状態ｌ１０４である。矢印ｔ１３５〜ｔ１３６に示すように、状態ｌ１０８、およびｌ１０９の次の目標状態は、状態ｌ１０７である。矢印ｔ１３７〜ｔ１３８に示すように、状態ｌ１１１、およびｌ１１２の次の目標状態は、状態ｌ１１０である。
なお、矢印ｔ１２１〜ｔ１２８、ｔ１３１〜ｔ１３８は、第１部品２００を、第２部品２１０の穴２１１に押し込むように、ロボット制御装置１０が制御する遷移状態である。

矢印ｔ１４１〜ｔ１４３に示すように、状態ｌ１０１の次の目標状態は、状態ｓ１０１、ｓ１０２、ｓ１０３のいずれかの状態である。矢印ｔ１４４に示すように、状態ｌ１０４の次の目標状態は、状態ｓ１０４である。矢印ｔ１４５〜ｔ１４７に示すように、状態ｌ１０７の次の目標状態は、状態ｓ１０４、ｓ１０５、ｓ１０６のいずれかの状態である。矢印ｔ１４８に示すように、状態ｌ１１０の次の目標状態は、状態ｓ１０３である。

矢印ｔ１５１に示すように、状態ｓ１０１の次の目標状態は、状態ｓ１０７である。矢印ｔ１５２に示すように、状態ｓ１０３の次の目標状態は、状態ｓ１０７である。矢印ｔ１５３に示すように、状態ｓ１０４の次の目標状態は、状態ｓ１０８である。矢印ｔ１５４に示すように、状態ｓ１０６の次の目標状態は、状態ｓ１０８である。矢印ｔ１５５に示すように、状態ｓ１０７の次の目標状態は、状態ｓ１０２である。矢印ｔ１５６に示すように、状態ｓ１０８の次の目標状態は、状態ｓ１０５である。矢印ｔ１５７に示すように、状態ｓ１０２の次の目標状態は、状態ｓ１０９である。矢印ｔ１５８に示すように、状態ｓ１０５の次の目標状態は、状態ｓ１１０である。

状態ｐ１０１は、第１部品２００の辺Ａ１Ｅ１と穴２１１の稜線ａ１ｂ１が１点で点接触している。状態ｐ１０２は、第１部品２００の辺Ｂ１Ｆ１と穴２１１の稜線ａ１ｂ１が１点で点接触している。状態ｐ１０３は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｂ１が１点で点接触している。状態ｐ１０４は、第１部品２００の辺Ｆ１Ｇ１と穴２１１の稜線ａ１ｂ１が１点で点接触している。状態ｐ１０５は、第１部品２００の辺Ｄ１Ｈ１と穴２１１の稜線ｃ１ｄ１が１点で点接触している。状態ｐ１０６は、第１部品２００の辺Ｃ１Ｇ１と穴２１１の稜線ｃ１ｄ１が１点で点接触している。状態ｐ１０７は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ｃ１ｄ１が１点で点接触している。状態ｐ１０８は、第１部品２００の辺Ｆ１Ｇ１と穴２１１の稜線ｃ１ｄ１が１点で点接触している。

状態ｔｐ１０１は、第１部品２００の辺Ａ１Ｅ１と穴２１１の稜線ａ１ｂ１、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｄ１が接している２点で接触している。状態ｔｐ１０２は、第１部品２００の辺Ｂ１Ｆ１と穴２１１の稜線ａ１ｂ１、第１部品２００の辺Ｆ１Ｇ１と穴２１１の稜線ｂ１ｃ１が接している２点で接触している。状態ｔｐ１０３は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｂ１、第１部品２００の辺Ｄ１Ｈ１と穴２１１の稜線ａ１ｄ１が接している２点で接触している。状態ｔｐ１０４は、第１部品２００の辺Ｆ１Ｇ１と穴２１１の稜線ａ１ｂ１、第１部品２００の辺Ｃ１Ｇ１と穴２１１の稜線ｂ１ｃ１が接している２点で接触している。状態ｔｐ１０５は、第１部品２００の辺Ｄ１Ｈ１と穴２１１の稜線ｃ１ｄ１、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｄ１が接している２点で接触している。状態ｔｐ１０６は、第１部品２００の辺Ｃ１Ｇ１と穴２１１の稜線ｃ１ｄ１、第１部品２００の辺Ｆ１Ｇ１と穴２１１の稜線ｂ１ｃ１が接している２点で接触している。状態ｔｐ１０７は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ｃ１ｄ１、第１部品２００の辺Ａ１Ｅ１と穴２１１の稜線ａ１ｄ１が接している２点で接触している。状態ｔｐ１０８は、第１部品２００の辺Ｇ１Ｆ１と穴２１１の稜線ｃ１ｄ１、第１部品２００の辺Ｂ１Ｆ１と穴２１１の稜線ｂ１ｃ１が接している２点で接触している。

状態ｌ１０１は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｌ１０２は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｂ１が線接触している。状態ｌ１０３は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触している。状態ｌ１０４は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｌ１０５は、第１部品２００の面Ｅ１Ｂ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｂ１が線接触している。状態ｌ１０６は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触している。状態ｌ１０７は、第１部品２００の面Ｃ１Ｄ１Ｈ１Ｇ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｌ１０８は、第１部品２００の面Ｃ１Ｄ１Ｈ１Ｇ１と穴２１１の稜線ｃ１ｄ１が線接触している。状態ｌ１０９は、第１部品２００の面Ｃ１Ｄ１Ｈ１Ｇ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触している。状態ｌ１１０は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｌ１１１は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ｃ１ｄ１が線接触している。状態ｌ１１２は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触している。

状態ｓ１０１は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ１０２は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｇ１が面接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ１０３は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ１０４は、第１部品２００の面Ｃ１Ｄ１Ｈ１Ｇ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ１０５は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｇ１が面接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触している。状態ｓ１０６は、第１部品２００の面Ｃ１Ｄ１Ｈ１Ｇ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ１０７は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ１０８は、第１部品２００の面Ｃ１Ｄ１Ｈ１Ｇ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ１０９は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｇ１が面接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の面ｅ１ｆ１ｇ１ｈ１が面接触している。状態ｓ１１０は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｇ１が面接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の面ｅ１ｆ１ｇ１ｈ１が面接触している。

次に、テーブル記憶部１０３に記憶されている力センサーの離散化後の検出値について説明する。
図１５は、本実施形態に係るテーブル記憶部１０３に記憶されている状態ｐ１０１から状態ｐ１０８の力センサー２０ｄの離散化後の検出値を示す図である。図１６は、本実施形態に係るテーブル記憶部１０３に記憶されている絶対座標系における状態ｔｐ１０１から状態ｔｐ１０８の力センサー２０ｄの離散化後の検出値を示す図である。図１７は、本実施形態に係るテーブル記憶部１０３に記憶されている絶対座標系における状態ｌ１０１から状態ｌ１１２の力センサー２０ｄの離散化後の検出値を示す図である。図１８は、本実施形態に係るテーブル記憶部１０３に記憶されている絶対座標系における状態ｓ１０１から状態ｓ１１０の力センサー２０ｄの離散化後の検出値を示す図である。

次に、テーブル記憶部１０３に記憶されている各接触状態から次の目標状態へ遷移させる指令値について説明する。
図１５に示すように、テーブル記憶部１０３には、状態ｐ１０１から状態ｐ１０８の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。例えば、２行目に示すように、状態ｐ１０１の離散化後の各検出値は、^ｏｆｘ＝０、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｂｊｎｘ＝−１、^ｏｂｊｎｙ＝０、^ｏｂｊｎｚ＝−１である。
なお、絶対座標系における離散化された検出値と、物体座標系における離散化された検出値との組み合わせを、本実施形態では第２検出値と称する。
なお、テーブル記憶部１０３には、絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）およびモーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）を関連づけられて記憶させ、物体座標系における力（^ｏｂｊｆｘ，^ｏｂｊｆｙ，^ｏｂｊｆｚ）およびモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）を関連づけて記憶させるようにしてもよい。この場合、選択部１０４が、テーブル記憶部１０３に記憶されている絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、および物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）を読み出して、読み出した結果に基づいて、第１部品２００と第２部品２１０との接触状態を判別するようにしてもよい。

図１６に示すように、テーブル記憶部１０３には、状態ｔｐ１０１から状態ｔｐ１０８の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、およびモーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。例えば、２行目に示すように、状態ｔｐ１０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｎｘ＝−１、^ｏｎｙ＝１、^ｏｎｚ＝０である。

図１７に示すように、テーブル記憶部１０３には、状態ｌ１０１から状態ｌ１１２の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、およびモーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。例えば、２行目に示すように、状態ｌ１０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｎｘ＝−１、^ｏｎｙ＝０、^ｏｎｚ＝０である。

図１８に示すように、テーブル記憶部１０３には、状態ｓ１０１から状態ｓ１１０の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、およびモーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。例えば、２行目に示すように、状態ｓ１０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝０、^ｏｆｚ＝１、^ｏｎｘ＝−１、^ｏｎｙ＝０、^ｏｎｚ＝０である。
なお、３値化後の各検出値を、図１５から図１８に示したが、これらの値は、テーブル記憶部１０３に、グループ分けして記憶するようにしてもよく、グループ分けせずに、まとめて記憶するようにしてもよい。

次に、テーブル記憶部１０３に記憶されている各状態からの状態遷移の指令値について説明する。
図１９は、本実施形態に係るテーブル記憶部１０３に記憶されている状態ｐ１０１〜ｐ１０８からの状態遷移毎の指令値を説明する図である。図２０は、本実施形態に係るテーブル記憶部１０３に記憶されている状態ｔｐ１０１〜ｔｐ１０８からの状態遷移毎の指令値を説明する図である。図２１は、本実施形態に係るテーブル記憶部１０３に記憶されている状態ｌ１０２、ｌ１０３、ｌ１０５、ｌ１０６、ｌ１０８、ｌ１０９、ｌ１１１、ｌ１１２からの状態遷移毎の指令値を説明する図である。図２２は、本実施形態に係るテーブル記憶部１０３に記憶されている状態ｌ１０１、ｌ１０４、ｌ１０７、ｌ１１０からの状態遷移毎の指令値を説明する図である。図２３は、本実施形態に係るテーブル記憶部１０３に記憶されている状態ｓ１０１、ｓ１０２、ｓ１０３、ｓ１０４、ｓ１０５、ｓ１０６、ｓ１０７、ｓ１０８、ｓ１０９からの状態遷移毎の指令値を説明する図である。

図１９から図２３に示すように、テーブル記憶部１０３には、現在の状態から次に遷移させる状態毎に、マニピュレータ部２０ｂ、および把持部２０ｃへの指令値である角度（θ_ｘ，θ_ｙ，θ_ｚ）、力（ｆｘ，ｆｙ，ｆｚ）が、表形式で関連づけられて記憶されている。なお、図１９から図２３において、正値は、各軸に正方向に移動させることで目標状態になるように制御する値であり、負値は、各軸に負方向に移動させることで目標状態になるように制御する指令値である。図１９から図２３に示した例では、角度が、−４５［ｄｅｇ］、−２０［ｄｅｇ］、０［ｄｅｇ］、＋２０［ｄｅｇ］および＋４５［ｄｅｇ］の値を示したが、この値は一例である。同様に、力は、−１［Ｎ（ニュートン）］、０［Ｎ］、および＋１［Ｎ］の値を示したが、この値も一例である。第１部品２００、第２部品２１０の剛性等に応じて設定するようにしてもよい。
また、図１９から図２３において、符号「→」は、現在の接触状態から、次の目標状態への状態遷移を表している。

図１９に示すように、テーブル記憶部１０３には、以下の指令値が記憶されている。状態ｐ１０１から状態ｔｐ１０１またはｌ１０２、状態ｐ１０２から状態ｔｐ１０２またはｌ１０２、状態ｐ１０３から状態ｔｐ１０３またはｌ１０５、状態ｐ１０４から状態ｔｐ１０４またはｌ１０５、状態ｐ１０５から状態ｔｐ１０５またはｌ１０８、状態ｐ１０６から状態ｌ１０８、状態ｐ１０７から状態ｔｐ１０７またはｌ１１１、状態ｐ１０８から状態ｔｐ１０８またはｔｐ１１１への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｐ１０１から状態ｔｐ１０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，４５［ｄｅｇ］，０［Ｎ］，０［Ｎ］，０［Ｎ］）である。

図２０に示すように、テーブル記憶部１０３には、状態ｔｐ１０１から状態ｌ１０１、状態ｔｐ１０２から状態ｌ１０３、状態ｔｐ１０３から状態ｌ１０４、状態ｔｐ１０４から状態ｌ１０６、状態ｔｐ１０５から状態ｌ１０７、状態ｔｐ１０６から状態ｌ１０９、状態ｔｐ１０７から状態ｌ１１０、状態ｔｐ１０８から状態ｌ１１２への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｔｐ１０１から状態ｌ１０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，−４５［ｄｅｇ］，−１［Ｎ］，０［Ｎ］，０［Ｎ］）である。

図２１に示すように、テーブル記憶部１０３には、状態ｌ１０２から状態ｌ１０１、状態ｌ１０３から状態ｌ１０１、状態ｌ１０５から状態ｌ１０４、状態ｌ１０６から状態ｌ１０４、状態ｌ１０８から状態ｌ１０７、状態ｌ１０９から状態ｌ１０７、状態ｌ１１１から状態ｌ１１０、状態ｌ１１２から状態ｌ１１０への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｌ１０２から状態ｌ１０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，−１［Ｎ］，０［Ｎ］，０［Ｎ］）である。

図２２に示すように、テーブル記憶部１０３には、状態ｌ１０１から状態ｓ１０１、ｓ１０２またはｓ１０３、状態ｌ１０４から状態ｓ１０４、状態ｌ１０７から状態ｓ１０４、ｓ１０５またはｓ１０６、状態ｌ１１０から状態ｓ１０３への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｌ１０１から状態ｓ１０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（２０［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，０［Ｎ］，０［Ｎ］，０［Ｎ］）である。

図２３に示すように、テーブル記憶部１０３には、状態ｓ１０１から状態ｓ１０７、状態ｓ１０３から状態ｓ１０７、状態ｓ１０４から状態ｓ１０８、状態ｓ１０６から状態ｓ１０８、状態ｓ１０７から状態ｓ１０２、状態ｓ１０８から状態ｓ１０５、状態ｓ１０２から状態ｓ１０９、状態ｓ１０５から状態ｓ１１０への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｓ１０１から状態ｓ１０７への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，０［Ｎ］，０［Ｎ］，０［Ｎ］）である。

なお、状態遷移毎の指令値を、図１９から図２３に示したが、これらの値は、テーブル記憶部１０３に、グループ分けして記憶するようにしてもよく、グループ分けせずに、まとめて記憶するようにしてもよい。

テーブル記憶部１０３に、接触状態毎の力センサー２０ｄの検出値（離散化前および離散化後）を記憶する手順は、図１０を用いて説明した第１実施形態と同様に行う。

次に、ロボット制御装置１０が、図１４に示した状態ｐ１０１〜ｐ１０８、状態ｔｐ１０１〜ｔｐ１０８、ｌ１０１〜ｌ１１２、ｓ１０８〜ｓ１１０を判別し、遷移させる手順を説明する。
図２４は、本実施形態に係る第１部品２００と第２部品２１０との接触状態を判別し、第１部品２００と第２部品２１０との接触状態を遷移させる手順のフローチャートである。なお、初期姿勢ｐ１０１〜ｐ１０８までの制御は、ロボット制御装置１０が制御記憶部１０６に記憶されている制御値を用いて、マニピュレータ部２０ｂと把持部２０ｃを制御することで行う。

（ステップＳ１０１〜ステップＳ１０２）ステップＳ１０１〜ステップＳ１０２は、第１実施形態（図１１）のステップＳ１１からＳ１２と同様に処理する。

（ステップＳ１０３）選択部１０４は、離散化部１０２が出力した離散化後の検出値と、テーブル記憶部１０３に記憶されている第２検出値とを比較する。この場合、図１５に示したように、比較する検出値は、絶対座標系における力の離散化後の検出値と、物体座標系におけるモーメントの離散化後の検出値との組み合わせである。

（ステップＳ１０４）選択部１０４は、比較した結果に基づいて、図１４および図１５に示した状態ｐ１０１〜ｐ１０８のうち、どの接触状態であるか判別する。選択部１０４は、判別した結果に基づいて、テーブル記憶部１０３に記憶される遷移情報の中から、遷移させる次の目標状態を判別する。この場合、遷移させる次の目標状態は、状態ｔｐ１０１〜ｔｐ１０８、ｌ１０２、ｌ１０５、ｌ１０８、ｌ１１１のいずれかである。次に、選択部１０４は、判別した結果に基づいて、遷移情報を制御部１０５に出力する。
なお、第１部品２００と第２部品２１０との接触状態が、例えば状態ｐ１０１である場合、テーブル記憶部１０３には、状態ｔｐ１０１およびｌ１０２へ遷移情報が記憶されている。このような場合、選択部１０４は、予め定められている優先順位に基づいて、遷移情報を選択する。予め定められている優先順位とは、例えば最短の制御手順になる状態遷移である。

（ステップＳ１０５）制御部１０５は、選択部１０４が出力した遷移情報に基づいて、テーブル記憶部１０３に記憶されている指令値を選択する。例えば、遷移情報が、状態ｐ１０１から状態ｔｐ１０１である場合、制御部１０５は、テーブル記憶部１０３から状態ｐ１０１から状態ｔｐ１０１に遷移させる指令値を選択する。制御部１０５は、選択した指令値に基づいて、状態ｔｐ１０１になるように第１部品２００を把持しているマニピュレータ部２０ｂ、および把持部２０ｃを制御する。

（ステップＳ１０６〜ステップＳ１０８）
ステップＳ１０６〜ステップＳ１０８は、ステップＳ１０１からＳ１０３と同様に処理する。なお、ステップＳ１０８において、選択部１０４は、遷移させる次の目標状態を判別する場合、検出値と記憶されている全ての接触状態の検出値と比較するのではなく、テーブル記憶部１０３に記憶される状態遷移に従った接触状態のみについて比較する。具体的には、現在の接触状態が状態ｌ１０１である場合、選択部１０４は、絶対座標系における離散化後の検出値と、テーブル記憶部１０３に記憶されている状態ｓ１０１、ｓ１０２、およびｓ１０３の第２検出値とを比較して、比較した結果に基づいて状態を判別する。
この場合、図１６〜図１８に示したように、比較する検出値は、絶対座標系における力の３値化後の検出値とモーメントの離散化後の検出値との組み合わせである。

（ステップＳ１０９）制御部１０５は、選択部１０４が出力した遷移情報に基づいて、第１部品２００と第２部品２１０との接触状態が目標としている状態（最終目標状態）であるか否かを判別する。なお、最終目標状態とは、図１４において、状態ｓ１０９、または状態ｓ１１０である。第１部品２００と第２部品２１０との状態が目標としている状態ではないと判別した場合（ステップＳ１０９；Ｎｏ）、ステップＳ１０６に戻り、ステップＳ１０６からＳ１０９を繰り返す。
第１部品２００と第２部品２１０との状態が目標としている状態であると判別した場合
（ステップＳ１０９；Ｙｅｓ）、第１部品２００と第２部品２１０との接触状態を遷移させる処理を終了する。

ここで、ロボット制御装置１０が行う制御の一例を説明する。
図１４において、ロボット制御装置１０は、第１部品２００と第２部品２１０との接触状態を、初期姿勢ｐ１０１から状態ｔｐ１０１になるように制御する。状態ｔｐ１０１であることを判別した後、ロボット制御装置１０は、接触状態を状態ｔｐ１０１から状態ｌ１０１になるように制御する。状態ｌ１０１であることを判別した後、ロボット制御装置１０は、接触状態を状態ｌ１０１から状態ｓ１０１になるように制御する。状態ｓ１０１であることを判別した後、ロボット制御装置１０は、接触状態を状態ｓ１０１から状態ｓ１０７になるように制御する。状態ｓ１０７であることを判別した後、ロボット制御装置１０は、接触状態を状態ｓ１０１から状態ｓ１０２になるように制御する。状態ｓ１０２であることを判別した後、ロボット制御装置１０は、接触状態を状態ｓ１０２から状態ｓ１０９になるように制御する。

以上のように、本実施形態のロボット装置１は、絶対座標系においてｘ軸方向の位置決めができている状態からの嵌め込みを行う場合、ロボット装置１が備える力センサー２０ｄが検出した検出値に基づいて、第１部品２００と第２部品２１０との接触状態を決定するようにした。そして、本実施形態のロボット装置１は、決定した状態に応じて、テーブル記憶部１０３に記憶されている遷移情報を選択して、第１部品２００と第２部品２１０との接触状態を制御するようにした。この結果、本実施形態によれば、第２部品２１０の穴２１１が面取されていなくても、第１部品を嵌め合うことができる。
この場合、第１部品２００と、穴２１１の稜線Ｂ１Ｃ１との接触、または稜線Ａ１Ｄ１との接触状態に基づいて、状態と、状態遷移をテーブル記憶部１０３に記憶させるようにしたので、第１部品２００と第２部品２１０との接触状態の判別にようする演算量を削減できる。

［第３実施形態］
本実施形態では、図１３に示した第１部品２００のいずれかの頂点が、第２部品２１０の穴２１１に入っている状態からｘｙ方向それぞれの方向に接触するまで動かして嵌め合わせる例を説明する。
また、本実施形態であるロボット装置は、第１部品と第２部品の接触状態を、第１接触状態にさせ、第１接触状態にさせた後に第２接触状態にさせ、第２接触状態にさせた後に第３接触状態にさせることで嵌め合わせを行う。さらに、本実施形態であるロボット装置は、第３接触状態において、第１部品が、第２部品の穴に接している面に沿わせて、第１部品を第２部品の穴に取り付ける（嵌め合わせる）ように順次、遷移させる。

なお、第１部品２００と第２部品２１０との接触状態とは、第１部品２００と第２部品との様態であり、第１部品２００と第２部品とが点接触している状態（第１接触状態）、線接触している状態（第２接触状態）、面接触している状態（第３接触状態）、第１部品２００が穴２１１に入っている状態等を含む状態である。第１接触状態〜第３接触状態は、第２実施形態と同様の状態である。
また、ロボット装置１の概略は、第１実施形態の図１と同じ構成である。ロボット制御装置１０の構成は、第１実施形態の図３と同じ構成である。第１部品２００と第２部品２１０における絶対座標系と物体座標系、および第１部品２００と第２部品２１０における各頂点は、第２実施形態の図１３と同様である。

本実施形態における前提条件は以下である。
１．初期姿勢は、第１部品の頂点Ｅ１、Ｆ１、Ｇ１、およびＨ１のいずれが、第２部品２１０の穴２１１の領域を表す面ａ１ｂ１ｃ１ｄ１内に存在する。すなわち、穴２１１の領域の面ａ１ｂ１ｃ１ｄ１内に存在する第１部品の頂点は、１つのみである。
２．第１部品２００の初期姿勢と最終目標状態の姿勢の誤差が小さいと仮定する。すなわち、絶対座標系から見た物体座標系の姿勢の誤差を表す角度の範囲は、−１０［ｄｅｇ］〜＋１０［ｄｅｇ］未満とする。
３．明らかに目標の位置・姿勢から離れる接触状態への遷移は、考慮しない。
４．物体座標系の原点にコンプライアンス・センターがあり、ダンピング特性を持っているとする。
５．ロボット制御装置１０は、初期姿勢から一定の操作を行い、第２部品２１０の穴２１１の頂ａ１回りで操作する。すなわち、ロボット制御装置１０は、第１部品２００を、初期姿勢から絶対座標系のｘ軸負の方向へ穴２１１の稜線ａ１ｄ１と接触させるまで移動させるように制御する。次に、ロボット制御装置１０は、接触させた後、絶対座標系のｙ軸負の方向へ穴２１１の稜線ａ１ｂ１と接触させるように制御する。

次に、テーブル記憶部１０３に記憶されている遷移情報、力センサー２０ｄの検出値、
指令値について説明する。

まず、テーブル記憶部１０３に記憶されている第１部品２００の頂点毎の遷移情報について説明する。
図２５は、本実施形態に係るテーブル記憶部１０３に記憶されている第１部品２００の頂点Ｅ１が穴２１１に入っている状態の後の遷移情報に基づく状態遷移図である。図２６は、本実施形態に係るテーブル記憶部１０３に記憶されている第１部品２００の頂点Ｆ１の遷移情報に基づく状態遷移図である。図２７は、本実施形態に係るテーブル記憶部１０３に記憶されている第１部品２００の頂点Ｇ１が穴２１１に入っている状態の後の遷移情報に基づく状態遷移図である。図２８は、本実施形態に係るテーブル記憶部１０３に記憶されている第１部品２００の頂点Ｈ１が穴２１１に入っている状態の後の遷移情報に基づく状態遷移図である。
なお、テーブル記憶部１０３に記憶されている頂点毎の遷移情報とは、例えば図２５において、初期姿勢ｋ１から状態ｐ２０１、初期姿勢ｋ１から状態ｐ２０２、状態ｐ２０１から状態ｔｐ２０１など、矢印ｔ２０１〜ｔ２６１で示した遷移させる情報である。また、図２５に示した頂点Ｅ１の場合の遷移情報は関連づけられて記憶されている。同様に、図２６に示した頂点Ｆ１の場合の遷移情報は関連づけられて記憶されている。図２７に示した頂点Ｇ１の場合の遷移情報は関連づけられて記憶されている。図２８に示した頂点Ｈ１の場合の遷移情報は関連づけられて記憶されている。
なお、以下、図２５〜図２８を、各頂点における状態遷移の関係という。

図２５の例において、テーブル記憶部１０３に記憶されている遷移情報は以下の通りである。矢印ｔ２０１およびｔ２０２に示すように、初期姿勢ｋ１１の次の目標状態は、状態ｐ２０１またはｐ２０２である。矢印ｔ２１１およびｔ２１２に示すように、状態ｐ２０１の次の目標状態は、状態ｔｐ２０１またはｔｐ２０２である。矢印ｔ２２３に示すように、状態ｐ２０４の次の目標状態は、状態ｌ２０３である。矢印ｔ２２４に示すように、状態ｌ２０２の次の目標状態は、状態ｌ２０１である。矢印ｔ２３１に示すように、状態ｌ２０１の次の目標状態は、状態ｓ２０１である。矢印ｔ２３２に示すように、状態ｌ２０３の次の目標状態は、状態ｓ２０２である。矢印ｔ２４１に示すように、状態ｓ２０１の次の目標状態は、状態ｌ２０８である。矢印ｔ２４２に示すように、状態ｓ２０２の次の目標状態は、状態ｌ２０９である。矢印ｔ２５１およびｔ２５２に示すように、状態ｌ２０８およびｌ２０９の次の目標状態は、状態ｓ２０３である。矢印ｔ２６１に示すように、状態ｓ２０３の次の目標状態は、状態ｓ２０４である。

初期姿勢ｋ１１は、第１部品２００の頂点Ｅ１が穴２１１に入っている状態である。
状態ｐ２０１およびｐ２０２は、初期姿勢ｋ１１から、ｘ軸方向の負方向に第１部品２００を移動させた後の第１部品２００と穴２１１の接触状態を表している。この状態では、第１部品２００の頂点Ｅ１が、穴２１１の稜線ａ１ｂ１と接している。

状態ｐ２０１は、第１部品２００の辺Ａ１Ｅ１と穴２１１の稜線ａ１ｄ１が１点で点接触している。状態ｐ２０２は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｄ１が１点で点接触している。状態ｔｐ２０１は、第１部品２００の辺Ａ１Ｅ１と穴２１１の稜線ａ１ｄ１、第１部品２００の辺Ｅ１Ｆ１と穴２１１の稜線ａ１ｂ１の２点で点接触している。状態ｔｐ２０２は、第１部品２００の辺Ａ１Ｅ１と穴２１１の頂点ａ１が１点で点接触している。状態ｔｐ２０４は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｄ１、第１部品２００の辺Ａ１Ｅ１と穴２１１の稜線ａ１ｂ１の２点で点接触している。

状態ｌ２０１は、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触している。状態ｌ２０２は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｄ１が線接触、または、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の頂点ａ１が点接触している。状態ｌ２０３は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｌ２０８は、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触している。状態ｌ２０９は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。

状態ｓ２０１は、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ｂ１ｃ１が線接触し、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触している。状態ｓ２０２は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触している。状態ｓ２０３は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ２０４は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｈ１ｅ１が面接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の面ｅ１ｆ１ｇ１ｈ１が面接触している。

図２６の例において、テーブル記憶部１０３に記憶されている遷移情報は以下の通りである。矢印ｔ３０１に示すように、初期姿勢ｋ１２の次の目標状態は、状態ｐ３０１である。矢印ｔ３１１およびｔ３１２に示すように、状態ｐ３０１の次の目標状態は、状態ｔｐ３０３またはｔｐ３０４である。矢印ｔ３２１に示すように、状態ｔｐ３０３の次の目標状態は、状態ｌ３０１である。矢印ｔ３２２に示すように、状態ｔｐ３０４の次の目標状態は、状態ｌ３０２である。矢印ｔ３２３に示すように、状態ｌ３０１の次の目標状態は、状態ｌ３０２である。矢印ｔ３３１に示すように、状態ｌ３０２の次の目標状態は、状態ｓ２０１である。矢印ｔ３４１に示すように、状態ｓ３０１の次の目標状態は、状態ｌ３０８である。矢印ｔ３５１に示すように、状態ｌ３０８の次の目標状態は、状態ｓ３０４である。矢印ｔ３６１に示すように、状態ｓ３０４の次の目標状態は、状態ｓ３０６である。

初期姿勢ｋ１２は、第１部品２００の頂点Ｆ１が穴２１１に入っている状態である。
状態ｐ３０１は、初期姿勢ｋ１２から、ｘ軸方向の負方向に第１部品２００を移動させた後の第１部品２００と穴２１１の接触状態を表している。この状態では、第１部品２００の頂点Ｆ１が、穴２１１の稜線ａ１ｂ１と接している。

状態ｐ３０１は、第１部品２００の辺Ｅ１Ｆ１と穴２１１の稜線ａ１ｄ１が１点で点接触している。状態ｔｐ３０３は、第１部品２００の辺Ｅ１Ｆ１と穴２１１の頂点ａ１の１点で点接触している。状態ｔｐ３０４は、第１部品２００の辺Ｅ１Ｆ１と穴２１１の稜線ａ１ｄ１、第１部品２００の辺Ｂ１Ｆ１と穴２１１の稜線ａ１ｂ１の２点で点接触している。

状態ｌ３０１は、第１部品２００の辺Ｅ１Ｆ１と穴２１１の稜線ａ１ｂ１が線接触し、または、第１部品２００の辺Ｅ１Ｆ１と穴２１１の頂点ａ１が点接触している。状態ｌ３０２は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｄ１が線接触、または、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｄ１ｆ１ｅ１が面接触している。状態ｌ３０８は、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の稜線ｂ１ｃ１が線接触、または、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触している。

状態ｓ３０１は、第１部品２００の面Ｂ１Ｃ１Ｆ１Ｇ１と穴２１１の稜線ｂ１ｃ１が線接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触している。状態ｓ３０２は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触している。状態ｓ３０６は、第１部品２００の面Ａ１Ｂ１Ｆ１Ｅ１と穴２１１の面ａ１ｂ１ｆ１ｅ１が面接触し、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の面ｅ１ｆ１ｇ１ｈ１が面接触している。

図２７の例において、テーブル記憶部１０３に記憶されている遷移情報は以下の通りである。矢印ｔ４０１に示すように、初期姿勢ｋ１３の次の目標状態は、状態ｐ４０１である。矢印ｔ４１１およびｔ４１２に示すように、状態ｐ４０１の次の目標状態は、状態ｔｐ４０１またはｔｐ４０２である。矢印ｔ４１３に示すように、状態ｔｐ４０１の次の目標状態は、状態ｔｐ４０２である。矢印ｔ４２１に示すように、状態ｔｐ４０２の次の目標状態は、状態ｌ４０１である。矢印ｔ４３１に示すように、状態ｌ４０１の次の目標状態は、状態ｓ４０１である。矢印ｔ４４１に示すように、状態ｓ４０１の次の目標状態は、状態ｌ４０８である。矢印ｔ４５１に示すように、状態ｌ４０８の次の目標状態は、状態ｓ４０４である。矢印ｔ４６１に示すように、状態ｓ４０４の次の目標状態は、状態ｓ４０６である。

初期姿勢ｋ１３は、第１部品２００の頂点Ｇ１が穴２１１に入っている状態である。
状態ｐ４０１は、初期姿勢ｋ１３から、ｘ軸方向の負方向に第１部品２００を移動させた後の第１部品２００と穴２１１の接触状態を表している。この状態では、第１部品２００の頂点Ｇ１が、穴２１１の稜線ａ１ｂ１と接している。

状態ｐ４０１は、第１部品２００の辺Ｈ１Ｇ１と穴２１１の稜線ａ１ｄ１が１点で点接触している。状態ｔｐ４０１は、第１部品２００の辺Ｈ１Ｇ１と穴２１１の頂点ａ１の１点で点接触している。状態ｔｐ４０２は、第１部品２００の辺Ｈ１Ｇ１と穴２１１の稜線ａ１ｄ１、第１部品２００の辺Ｆ１Ｇ１と穴２１１の稜線ａ１ｂ１の２点で点接触している。

状態ｌ４０１は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の面ｄ１ｃ１ｇ１ｈ１が面接触している。状態ｌ４０８は、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の稜線ｂ１ｃ１が線接触し、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の面ｄ１ｃ１ｇ１ｈ１が面接触している。

状態ｓ４０１は、第１部品２００の面Ｂ１Ｃ１Ｆ１Ｇ１と穴２１１の稜線ｂ１ｃ１が線接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｄ１が線接触し、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の面ｄ１ｃ１ｇ１ｈ１が面接触している。状態ｓ４０４は、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触し、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の面ｄ１ｃ１ｇ１ｈ１が面接触している。状態ｓ４０６は、第１部品２００の面Ｂ１Ｃ１Ｇ１Ｆ１と穴２１１の面ｂ１ｃ１ｇ１ｆ１が面接触し、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の面ｄ１ｃ１ｇ１ｈ１が面接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の面ｅ１ｆ１ｇ１ｈ１が面接触している。

図２８の例において、テーブル記憶部１０３に記憶されている遷移情報は以下の通りである。矢印ｔ５０１およびｔ５０２に示すように、初期姿勢ｋ１４の次の目標状態は、状態ｐ５０１またはｐ５０２である。矢印ｔ５１１に示すように、状態ｐ５０１の次の目標状態は、状態ｔｐ５０１である。矢印ｔ５１２およびｔ５１３に示すように、状態ｐ５０２の次の目標状態は、状態ｔｐ５０４またはｔｐ５０５である。矢印ｔ５２１に示すように、状態ｔｐ５０１の次の目標状態は、状態ｌ５０３である。矢印ｔ５２２に示すように、状態ｔｐ５０４の次の目標状態は、状態ｌ５０３である。矢印ｔ５２３に示すように、状態ｔｐ５０５の次の目標状態は、状態ｌ５０４である。矢印ｔ５２４に示すように、状態ｌ５０４の次の目標状態は、状態ｌ５０３である。矢印ｔ５３１に示すように、状態ｌ５０３の次の目標状態は、状態ｓ５０１である。矢印ｔ５４１に示すように、状態ｓ５０１の次の目標状態は、状態ｌ５０８である。矢印ｔ５５１に示すように、状態ｌ５０８の次の目標状態は、状態ｓ５０３である。矢印ｔ５６１に示すように、状態ｓ５０３の次の目標状態は、状態ｓ５０４である。

初期姿勢ｋ１４は、第１部品２００の頂点Ｈ１が穴２１１に入っている状態である。
状態ｐ５０１およびｐ５０２は、初期姿勢ｋ１４から、ｘ軸方向の負方向に第１部品２００を移動させた後の第１部品２００と穴２１１の接触状態を表している。この状態では、第１部品２００の頂点Ｈ１が、穴２１１の稜線ａ１ｂ１、または稜線ｄ１ｃ１と接している。

状態ｐ５０１は、第１部品２００の辺Ｄ１Ｈ１と穴２１１の稜線ａ１ｄ１が１点で点接触している。状態ｐ５０２は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｄ１が１点で点接触している。状態ｔｐ５０１は、第１部品２００の辺Ｄ１Ｈ１と穴２１１の稜線ａ１ｄ１、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｂ１の２点で点接触している。状態ｔｐ５０４は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の稜線ａ１ｄ１、第１部品２００の辺Ｇ１Ｈ１と穴２１１の稜線ａ１ｂ１の２点で点接触している。
状態ｔｐ５０５は、第１部品２００の辺Ｅ１Ｈ１と穴２１１の頂点ａ１の１点で点接触している。

状態ｌ５０３は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｌ５０４は、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の稜線ａ１ｄ１が線接触し、または、第１部品２００の辺Ｅ１Ｈ１と穴２１１の頂点ａ１が点接触している。状態ｌ５０８は、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。

状態ｓ５０１は、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の稜線ａ１ｂ１が線接触し、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の稜線ｃ１ｄ１が線接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。
状態ｓ５０３は、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の面ｄ１ｃ１ｇ１ｈ１が面接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触している。状態ｓ５０４は、第１部品２００の面Ｄ１Ｃ１Ｇ１Ｈ１と穴２１１の面ｄ１ｃ１ｇ１ｈ１が面接触し、第１部品２００の面Ａ１Ｄ１Ｈ１Ｅ１と穴２１１の面ａ１ｄ１ｈ１ｅ１が面接触し、第１部品２００の面Ｅ１Ｆ１Ｇ１Ｈ１と穴２１１の面ｅ１ｆ１ｇ１ｈ１が面接触している。

＜頂点Ｅ１の場合の状態と力センサーの検出値＞
次に、初期姿勢が第１部品の頂点Ｅ１が穴２１１に入っている状態の場合において、予め測定され、テーブル記憶部１０３に記憶されている離散化後の検出値について説明する。
図２９は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｐ２０１およびｐ２０２の力センサー２０ｄの離散化後の検出値を示す図である。図３０は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｔｐ２０１からｔｐ２０４の力センサー２０ｄの離散化後の検出値を示す図である。図３１は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｌ２０１からｌ２０９の力センサー２０ｄの離散化後の検出値を示す図である。図３２は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｓ２０１からｓ２０４の力センサー２０ｄの離散化後の検出値を示す図である。

図２９に示すように、テーブル記憶部１０３には、状態ｐ２０１およびｐ２０２の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｐ２０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝０、^ｏｆｚ＝１、^ｏｂｊｎｘ＝０、^ｏｂｊｎｙ＝１、^ｏｂｊｎｚ＝１である。
なお、絶対座標系における離散化された検出値と、物体座標系における離散化された検出値との組み合わせを、本実施形態では第２検出値と称する。

図３０に示すように、テーブル記憶部１０３には、状態ｔｐ２０１、ｔｐ２０２、およびｔｐ２０４の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｔｐ２０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｂｊｎｘ＝−１、^ｏｂｊｎｙ＝０、^ｏｂｊｎｚ＝０である。

図３１に示すように、テーブル記憶部１０３には、状態ｌ２０１からｌ２０３、ｌ２０８およびｌ２０９の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｌ２０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｎｘ＝０、^ｏｎｙ＝１、^ｏｎｚ＝０である。

図３２に示すように、テーブル記憶部１０３には、状態ｓ２０１、ｓ２０２、ｓ２０３、およびｓ２０４の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｓ２０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝０、^ｏｆｚ＝１、^ｏｎｘ＝０、^ｏｎｙ＝０、^ｏｎｚ＝０である。

＜頂点Ｆ１の場合の状態と力センサーの検出値＞
次に、初期姿勢が第１部品の頂点Ｆ１が穴２１１に入っている状態の場合において、予め測定され、テーブル記憶部１０３に記憶されている離散化後の検出値について説明する。
図３３は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｐ３０１からｐ３０３の力センサー２０ｄの離散化後の検出値を示す図である。図３４は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｔｐ３０３からｔｐ３０４の力センサー２０ｄの離散化後の検出値を示す図である。図３５は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｌ３０１からｌ３０８の力センサー２０ｄの離散化後の検出値を示す図である。図３６は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｓ３０１の力センサー２０ｄの離散化後の検出値を示す図である。

図３３に示すように、テーブル記憶部１０３には、状態ｐ３０１の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
２行目に示すように、状態ｐ３０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝０、^ｏｆｚ＝１、^ｏｂｊｎｘ＝−１、^ｏｂｊｎｙ＝１、^ｏｂｊｎｚ＝０である。

図３４に示すように、テーブル記憶部１０３には、状態ｔｐ３０３、およびｔｐ３０４の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｔｐ３０３の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｂｊｎｘ＝０、^ｏｂｊｎｙ＝０、^ｏｂｊｎｚ＝１である。

図３５に示すように、テーブル記憶部１０３には、状態ｌ３０１、ｌ３０２、およびｌｌ３０８の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｌ３０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｎｘ＝−１、^ｏｎｙ＝１、^ｏｎｚ＝０である。

図３６に示すように、テーブル記憶部１０３には、状態ｓ３０１、ｓ３０４、およびｓ３０６の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｓ３０１の離散化後の各検出値は、^ｏｆｘ＝０、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｎｘ＝０、^ｏｎｙ＝０、^ｏｎｚ＝０である。

＜頂点Ｇ１の場合の状態と力センサーの検出値＞
次に、初期姿勢が第１部品の頂点Ｇ１が穴２１１に入っている状態の場合において、予
め測定され、テーブル記憶部１０３に記憶されている離散化後の検出値について説明する。
図３７は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｐ４０１からｐ４０３の力センサー２０ｄの離散化後の検出値を示す図である。図３８は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｔｐ４０１からｔｐ４０２の力センサー２０ｄの離散化後の検出値を示す図である。図３９は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｌ４０１からｌ４０８の力センサー２０ｄの離散化後の検出値を示す図である。図４０は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｓ４０１の力センサー２０ｄの離散化後の検出値を示す図である。

図３７に示すように、テーブル記憶部１０３には、状態ｐ４０１の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
２行目に示すように、状態ｐ４０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝０、^ｏｆｚ＝１、^ｏｂｊｎｘ＝１、^ｏｂｊｎｙ＝１、^ｏｂｊｎｚ＝−１である。

図３８に示すように、テーブル記憶部１０３には、状態ｔｐ４０１、およびｔｐ４０２の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｔｐ４０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｂｊｎｘ＝０、^ｏｂｊｎｙ＝−１、^ｏｂｊｎｚ＝０である。

図３９に示すように、テーブル記憶部１０３には、状態ｌ４０１、およびｌ４０８の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｌ４０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝−１、^ｏｆｚ＝１、^ｏｎｘ＝０、^ｏｎｙ＝−１、^ｏｎｚ＝０である。

図４０に示すように、テーブル記憶部１０３には、状態ｓ４０１、ｓ４０４、およびｓ４０６の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｓ４０１の離散化後の各検出値は、^ｏｆｘ＝０、^ｏｆｙ＝−１、^ｏｆｚ＝１、^ｏｎｘ＝０、^ｏｎｙ＝０、^ｏｎｚ＝０である。

＜頂点Ｈ１の場合の状態と力センサーの検出値＞
次に、初期姿勢が第１部品の頂点Ｈ１が穴２１１に入っている状態の場合において、予め測定され、テーブル記憶部１０３に記憶されている離散化後の検出値について説明する。
図４１は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｐ５０１からｐ５０３の力センサー２０ｄの離散化後の検出値を示す図である。図４２は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｔｐ５０１からｔｐ５０５の力センサー２０ｄの離散化後の検出値を示す図である。図４３は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｌ５０３からｌ５０８の力センサー２０ｄの離散化後の検出値を示す図である。図４４は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている状態ｓ５０１の力センサー２０ｄの離散化後の検出値を示す図である。

図４１に示すように、テーブル記憶部１０３には、状態ｐ５０１、およびｐ５０２の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｐ５０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝０、^ｏｆｚ＝１、^ｏｂｊｎｘ＝０、^ｏｂｊｎｙ＝１、^ｏｂｊｎｚ＝−１である。

図４２に示すように、テーブル記憶部１０３には、状態ｔｐ５０１、ｔｐ５０４、およびｔｐ５０５の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｔｐ５０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｂｊｎｘ＝０、^ｏｂｊｎｙ＝−１、^ｏｂｊｎｚ＝０である。

図４３に示すように、テーブル記憶部１０３には、状態ｌ５０３、ｌ５０４、およびｌｌ５０８の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｌ５０３の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝１、^ｏｆｚ＝１、^ｏｎｘ＝１、^ｏｎｙ＝０、^ｏｎｚ＝０である。

図４４に示すように、テーブル記憶部１０３には、状態ｓ５０１、ｓ５０３、およびｓ５０４の絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、モーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）が関連づけられて記憶されている。
例えば、２行目に示すように、状態ｓ５０１の離散化後の各検出値は、^ｏｆｘ＝１、^ｏｆｙ＝０、^ｏｆｚ＝１、^ｏｎｘ＝０、^ｏｎｙ＝０、^ｏｎｚ＝０である。

なお、図２９、図３０、図３３、図３４、図３７、図３８、図４１、および図４２において、テーブル記憶部１０３には、絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）が関連づけられて記憶されている例を説明したが、これに限られない。テーブル記憶部１０３には、絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）およびモーメント（^ｏｎｘ，^ｏｎｙ，^ｏｎｚ）を関連づけて記憶させ、物体座標系における力（^ｏｂｊｆｘ，^ｏｂｊｆｙ，^ｏｂｊｆｚ）およびモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）を関連づけて記憶させるようにしてもよい。この場合、選択部１０４が、テーブル記憶部１０３に記憶されている絶対座標系における力（^ｏｆｘ，^ｏｆｙ，^ｏｆｚ）、および物体座標系におけるモーメント（^ｏｂｊｎｘ，^ｏｂｊｎｙ，^ｏｂｊｎｚ）を読み出して、読み出した結果に基づいて第１部品２００と第２部品２１０との接触状態を判別するようにしてもよい。

次に、テーブル記憶部１０３に記憶されている指令値について、図４５から図６０を用いて説明する。
図４５から図６０に示すように、テーブル記憶部１０３には、現在の状態から次に遷移させる接触状態毎に、マニピュレータ部２０ｂ、および把持部２０ｃへの指令値である角度（θ_ｘ，θ_ｙ，θ_ｚ）、力（ｆｘ，ｆｙ，ｆｚ）が、表形式で関連づけられて記憶されている。なお、図４５から図６０において、符号「→」は、現在の状態から、次の状態への状態遷移を表している。図４５から図６０に示した指令値は、第２実施形態と同様に一例の値である。

＜頂点Ｅ１の場合の指令値＞
次に、初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている指令値について説明する。
図４５は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｐ２０１、ｐ２０２からの状態遷移毎の指令値を説明する図である。図４６は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｔｐ２０１、ｔｐ２０２、ｔｐ２０４からの状態遷移毎の指令値を説明する図である。図４７は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｌ２０１、ｌ２０２、ｌ２０４、ｌ２０８、ｌ２０９からの状態遷移毎の指令値を説明する図である。図４８は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｅ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｓ２０１、ｓ２０２、ｓ２０３からの状態遷移毎の指令値を説明する図である。

図４５に示すように、テーブル記憶部１０３には、状態ｐ２０１から状態ｔｐ２０１、状態ｐ２０１から状態ｔｐ２０２、状態ｐ２０２から状態ｔｐ２０４への各指令値が、記憶されている。
例えば、２行目のように状態ｐ２０１から状態ｔｐ２０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，０［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図４６に示すように、テーブル記憶部１０３には、状態ｔｐ２０１から状態ｌ２０１、状態ｔｐ２０２から状態ｌ２０２、状態ｔｐ２０４から状態ｌ２０３への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｔｐ２０１から状態ｌ２０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，−４５［ｄｅｇ］，−１［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図４７に示すように、テーブル記憶部１０３には、状態ｌ２０１から状態ｓ２０１、状態ｌ２０２から状態ｌ２０１、状態ｌ２０３から状態ｓ２０２、状態ｌ２０８から状態ｓ２０３、状態ｌ２０９から状態ｓ２０３への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｌ２０１から状態ｓ２０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，−４５［ｄｅｇ］，−１［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図４８に示すように、テーブル記憶部１０３には、状態ｓ２０１から状態ｌ２０８、状態ｓ２０２から状態ｌ２０９、状態ｓ２０３から状態ｓ２０４への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｓ２０１から状態ｌ２０８への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，−４５［ｄｅｇ］，−１［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

＜頂点Ｆ１の場合の指令値＞
次に、初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている指令値について説明する。
図４９は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｐ３０１、ｐ３０２、ｐ３０３からの状態遷移毎の指令値を説明する図である。図５０は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｔｐ３０３、ｔｐ３０４からの状態遷移毎の指令値を説明する図である。図５１は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｌ３０１、ｌ３０２、ｌ３０８からの状態遷移毎の指令値を説明する図である。図５２は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｆ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｓ３０１、ｓ３０２、ｓ３０３からの状態遷移毎の指令値を説明する図である。

図４９に示すように、テーブル記憶部１０３には、状態ｐ３０１から状態ｔｐ３０３、状態ｐ３０１から状態ｔｐ３０４への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｐ３０１から状態ｔｐ３０３への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，０［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図５０に示すように、テーブル記憶部１０３には、状態ｔｐ３０３から状態ｌ３０１、状態ｔｐ３０４から状態ｌ３０２への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｔｐ３０３から状態ｌ３０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，−４５［ｄｅｇ］，０［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図５１に示すように、テーブル記憶部１０３には、状態ｌ３０１から状態ｌ３０２、状態ｌ３０２から状態ｓ３０１、状態ｌ３０８から状態ｓ３０４への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｌ３０１から状態ｌ３０２への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，−４５［ｄｅｇ］，０［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図５２に示すように、テーブル記憶部１０３には、状態ｓ３０１から状態ｌ３０８、状態ｌ３０８から状態ｓ３０４、状態ｓ３０４から状態ｓ３０６への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｓ３０１から状態ｌ３０８への指令値（θ_ｘ，θ_ｙ， _ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，４５［ｄｅｇ］，０［ｄｅｇ］，１［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

＜頂点Ｇ１の場合の指令値＞
次に、初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている各状態からの指令値について説明する。
図５３は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｐ４０１、ｐ４０２からの状態遷移毎の指令値を説明する図である。図５４は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｔｐ４０１、ｔｐ４０２からの状態遷移毎の指令値を説明する図である。図５５は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｌ４０１、ｌ４０８からの状態遷移毎の指令値を説明する図である。図５６は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｇ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｓ４０１、ｓ４０４からの状態遷移毎の指令値を説明する図である。

図５３に示すように、テーブル記憶部１０３には、状態ｐ４０１から状態ｔｐ４０１、状態ｐ４０１から状態ｔｐ４０２への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｐ４０１から状態ｔｐ４０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，０［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図５４に示すように、テーブル記憶部１０３には、状態ｔｐ４０１から状態ｔｐ４０２、状態ｔｐ４０２から状態ｌ４０１への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｔｐ４０１から状態ｔｐ４０２への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，−４５［ｄｅｇ］，０［Ｎ］，０［Ｎ］，０［Ｎ］）である。

図５５に示すように、テーブル記憶部１０３には、状態ｌ４０１から状態ｓ４０１、状態ｌ４０８から状態ｓ４０４への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｌ４０１から状態ｓ４０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，４５［ｄｅｇ］，０［ｄｅｇ］，０［Ｎ］，１［Ｎ］，０［Ｎ］）である。

図５６に示すように、テーブル記憶部１０３には、状態ｓ４０１から状態ｌ４０８、状態ｌ４０８から状態ｓ４０４、状態ｓ４０４から状態ｓ４０６、への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｓ４０１から状態ｌ４０８への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，−４５［ｄｅｇ］，０［ｄｅｇ］，１［Ｎ］，１［Ｎ］，０［Ｎ］）である。

＜頂点Ｈ１の場合の指令値＞
次に、初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合、テーブル記憶部１０３に記憶されている各状態からの指令値について説明する。
図５７は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｐ５０１、ｐ５０２からの状態遷移毎の指令値を説明する図である。図５８は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｔｐ５０１、ｔｐ５０４、ｔｐ５０５からの状態遷移毎の指令値を説明する図である。図５９は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｌ５０３、ｌ５０４、ｌ５０８からの状態遷移毎の指令値を説明する図である。図６０は、本実施形態に係る初期姿勢が第１部品２００の頂点Ｈ１が穴２１１に入っている状態の場合において、テーブル記憶部１０３に記憶されている状態ｓ５０１、ｓ５０３からの状態遷移毎の指令値を説明する図である。

図５７に示すように、テーブル記憶部１０３には、状態ｐ５０１から状態ｔｐ５０１、状態ｐ５０２から状態ｔｐ５０４、状態ｐ５０２から状態ｔｐ５０５への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｐ５０１から状態ｔｐ５０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，０［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図５８に示すように、テーブル記憶部１０３には、状態ｔｐ５０１から状態ｌ５０３、状態ｔｐ５０４から状態ｌ５０３、状態ｔｐ５０５から状態ｌ５０４への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｔｐ５０１から状態ｔｐ５０３への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（０［ｄｅｇ］，４５［ｄｅｇ］，４５［ｄｅｇ］，−１［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図５９に示すように、テーブル記憶部１０３には、状態ｌ５０３から状態ｓ５０１、状態ｌ５０４から状態ｌ５０３、状態ｌ５０８から状態ｓ５０３への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｌ５０３から状態ｓ５０１への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（−４５［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，−１［Ｎ］，−１［Ｎ］，０［Ｎ］）である。

図６０に示すように、テーブル記憶部１０３には、状態ｓ５０１から状態ｌ５０８、状態ｓ５０３から状態ｓ５０４への各状態遷移の指令値が、記憶されている。
例えば、２行目のように状態ｓ５０１から状態ｌ５０８への指令値（θ_ｘ，θ_ｙ，θ_ｚ，ｆｘ，ｆｙ，ｆｚ）は、（４５［ｄｅｇ］，０［ｄｅｇ］，０［ｄｅｇ］，−１［Ｎ］，１［Ｎ］，−１［Ｎ］）である。

なお、状態遷移毎の指令値を、図４５から図６０に示したが、これらの値は、テーブル記憶部１０３に、グループ分けして記憶するようにしてもよく、グループ分けせずに、まとめて記憶するようにしてもよい。

テーブル記憶部１０３に、予め検出した接触状態毎の力センサー２０ｄの検出値（離散化前および離散化後）を記憶する手順は、図１０を用いて説明した第１実施形態と同様である。

次に、ロボット制御装置１０が、図２５から図２８に示した状態を判別し、遷移させる手順を説明する。
図６１は、本実施形態に係る第１部品２００と第２部品２１０との接触状態を判別し、第１部品２００と第２部品２１０との接触状態を遷移させる手順のフローチャートである。

（ステップＳ２０１）制御部１０５は、制御記憶部１０６に記憶されている制御値に基づいて、第１部品２００をマニピューレ−タ部２０ｂおよび把持部２０ｃに把持させる。次に、制御部１０５は、第２部品２１０の穴２１１に対して、図１３に示した絶対座標系においてｚ軸方向の負方向に把持した第１部品２００を移動させるように、制御値に基づいてマニピュレータ部２０ｂおよび把持部２０ｃを制御する。制御部１０５は、制御値に基づいて第１部品２００のいずれかの頂点（Ｅ１、Ｆ１、Ｇ１、Ｈ１）が、穴２１１に入るように制御する。第１部品２００のいずれかの頂点が、穴２１１に入ったか否かの判別は、例えば選択部１０４が撮像装置３０により撮像された画像データを用いて行う。第１部品２００のいずれかの頂点が、穴２１１に入った状態とは、図２５〜図２８に示した初期姿勢ｋ１１〜ｋ１４のいずれかの状態である。

（ステップＳ２０２）制御部１０５は、図１３に示した絶対座標系においてｘ軸方向の負方向に把持した第１部品２００を移動させるように、マニピュレータ部２０ｂおよび把持部２０ｃを制御する。この遷移は、図２５の矢印ｔ２０１、ｔ２０２、図２６の矢印ｔ３０１、図２７の矢印ｔ４０１、図２８の矢印ｔ５０１、ｔ５０２の遷移である。

（ステップＳ２０３、ステップＳ２０４）ステップＳ２０３およびステップＳ２０４は、第１実施形態（図１１）のステップＳ１１およびＳ１２と同様に処理する。

（ステップＳ２０５）選択部１０４は、離散化部１０２が出力した離散化後の検出値と、テーブル記憶部１０３に記憶されている第２検出値とを比較する。この場合、比較する離散化後の検出値は、絶対座標系における力の離散化後の検出値と、物体座標系におけるモーメントの離散化後の検出値との組み合わせである。

（ステップＳ２０６）選択部１０４は、比較した結果に基づいて、第１部品２００と第２部品２１０とが、接触状態が、状態ｐ２０１、ｐ２０２、ｐ３０１、ｐ４０１、ｐ５０１、ｐ５０２のうちのいずれであるかを判別する。

（ステップＳ２０７）選択部１０４は、判別した結果に基づいて、テーブル記憶部１０３に記憶されている状態遷移のうち、どの状態遷移の関係（図２５〜図２８）を用いるのか選択する。
例えば、状態ｐ２０１であると判別した場合、選択部１０４は、テーブル記憶部１０３に記憶されている頂点Ｅ１における状態遷移の関係を選択する。

（ステップＳ２０８）選択部１０４は、選択した状態遷移の関係を用いて、遷移させる状態を選択し、選択した状態に基づいて遷移情報を選択する。次に、選択部１０４は、選択した遷移情報を制御部１０５に出力する。
例えば、ステップＳ２０６で状態ｐ２０１であると判別した場合、選択部１０４は、遷移させる状態として、状態ｔｐ２０１またはｔｐ２０２を選択する。選択可能な状態が複数ある場合、選択部１０４は、予め定められている優先順位に基づいて、指令値を選択する。予め定められている優先順位とは、例えば最短の制御手順になる状態遷移である。例えば、遷移させる状態としてｔｐ２０１を選択した場合、選択部１０４は、状態ｐ２０１からｔｐ２０１に遷移させることを示す遷移情報を制御部１０５に出力する。

（ステップＳ２０９）制御部１０５は、選択部１０４が出力した遷移情報に基づいて、テーブル記憶部１０３に記憶されている指令値を選択し、選択した指令値に基づいて、第１部品２００を把持しているマニピュレータ部２０ｂ、および把持部２０ｃを制御する。

（ステップＳ２１０〜ステップＳ２１２）
ステップＳ２１０〜ステップＳ２１２は、ステップＳ２０３からＳ２０５と同様に処理する。
なお、ステップＳ２１２において、比較する離散化後の検出値と比較するテーブル記憶部１０３に記憶されている予め検出された検出値は、以下の組み合わせのいずれかである。比較に用いる検出値の組み合わせは、絶対座標系における力の離散化後の検出値と、物体座標系におけるモーメントの離散化後の検出値との組み合わせ、または絶対座標系における力の離散化後の検出値とモーメントの離散化後の検出値との組み合わせである。どちらの組み合わせを用いて比較するかは、テーブル記憶部１０３に記憶されている状態遷移に基づいて行う。
具体的には、頂点Ｅ１の場合、状態が、状態ｐ２０１、ｐ２０２、ｔｐ２０１、ｔｐ２０２、ｔｐ２０４である場合、選択部１０４は、絶対座標系における力の離散化後の検出値と、物体座標系におけるモーメントの３値化後の検出値との組み合わせを用いて、検出値を比較する。状態が、状態ｌ２０１、ｌ２０２、ｌ２０３、ｌ２０８、ｌ２０９、ｓ２０１〜ｓ２０４である場合、選択部１０４は、絶対座標系における力の離散化後の検出値と、物体座標系におけるモーメントの３値化後の検出値との組み合わせを用いて、離散化後の検出値を比較する。

（ステップＳ２１３）選択部１０４は、テーブル記憶部１０３に記憶されている接触状態のうち、離散化後の検出値と一致する状態が１つであるか２つであるかを判別する。検出値と一致する状態が１つであると判別した場合（ステップＳ２１３；１つ）、ステップＳ２１５に進む。検出値と一致する状態が２つであると判別した場合（ステップＳ２１３；２つ）、ステップＳ２１４に進む。

（ステップＳ２１４）検出値と一致する状態が２つであると判別した場合、選択部１０４は、物体座標系の離散化後の検出値と、テーブル記憶部１０３に記憶されている予め測定された物体座標系の離散化後の検出値とを比較する。次に、選択部１０４は、判別した結果に基づいて、遷移情報を選択し、選択した遷移情報を制御部１０５に出力する。

（ステップＳ２１５）制御部１０５は、選択部１０４が出力した遷移情報に基づいて、第１部品２００と第２部品２１０との状態が目標としている状態（最終目標状態）であるか否かを判別する。なお、最終目標状態とは、図２５の状態ｓ２０４、図２６の状態ｓ３０６、図２７の状態ｓ４０６、または図２８の状態ｓ５０４である。第１部品２００と第２部品２１０との状態が目標としている状態ではないと判別した場合（ステップＳ２１５；Ｎｏ）、ステップＳ２０８に戻り、ステップＳ２０８からＳ２１５を繰り返す。第１部品２００と第２部品２１０との状態が目標としている状態であると判別した場合（ステップＳ２１５；Ｙｅｓ）、第１部品２００と第２部品２１０との接触状態を遷移させる処理を終了する。

以上のように、本実施形態のロボット装置１は、第１部品２００のいずれかの頂点が第２部品２１０の穴２１１に入っている状態からｘｙ方向それぞれの方向に接触するまで動かして嵌め合嵌め込みを行う場合、ロボット装置１が備える力センサー２０ｄが検出した検出値に基づいて、第１部品２００と第２部品２１０との接触状態を決定するようにした。そして、本実施形態のロボット装置１は、決定した状態に応じて、テーブル記憶部１０３に記憶されている遷移情報を選択して、第１部品２００と第２部品２１０との接触状態を制御するようにした。この結果、本実施形態によれば、第２部品２１０の穴２１１が面取されていなくても、第１部品を嵌め合うことができる。

なお、本実施形態において、頂点Ｅ１、Ｆ１、Ｇ１およびＨ１の場合について説明したが、これに限られない。穴２１１に挿入される第１部品２００の面に応じて、頂点Ａ１、Ｂ１、Ｃ１およびＤ１、頂点Ｂ１、Ｃ１、Ｇ１およびＦ１等の組み合わせであってもよい。

［第４実施形態］
第１〜第３実施形態では、１つのアーム（マニピュレータ部）を有するロボットについて説明したが、本実施形態では、２つのアームを備える例を説明する。
図６２は、本実施形態に係るロボット装置１ａの概略斜視図である。図６２に示すように、ロボット装置１ａは、多関節ロボット２０’、本体３０２を備えている。
なお、第１〜第３実施形態と同様のものは、同じ符号を用いて説明を省略する。

ロボット制御装置１０ａは、マニピュレータ部２０Ａ（第１アーム）および２０Ｂ（第２アーム）、把持部２０ａｃ、ハンド２０Ｂｃを制御する。ロボット制御装置１０ａの構成は、図３で説明したロボット制御装置１０と同様である。本実施形態では本体３０２の中部にロボット制御装置１０ａを配置したが、ロボット制御装置１０ａは本体の外部に配置されてもよい。
多関節ロボット２０’は、２つのマニピュレータ部２０Ａおよび２０Ｂ、把持部２０ａｃ、ハンド２０Ｂｃ、力センサー２０Ａｄ、可動部３０１を備えている。２つのマニピュレータ部２０Ａおよび２０Ｂは、可動部３０１に取り付けられている。可動部３０１は、本体３０２に取り付けられている。ハンド２０Ｂｃの先端には、撮像装置３０が取り付けられている。

力センサー２０Ａｄは、第１〜第３実施形態と同様に、把持部２０ａｃに加わる力とモーメントを検出する。
撮像装置３０は、撮像した画像データをロボット制御装置１０ａに出力する。

本体３０２は、移動可能なように搬送部３０３Ａおよび３０３Ｂを備えている。本体３０２内には、ロボット制御装置１０ａを備えている。
搬送部３０３Ａおよび３０３Ｂは、例えば車輪、キャタピラ等である。

本実施形態において、ロボット制御装置１０ａは、ハンド２０Ｂｃに取り付けられている撮像装置３０を、第１〜第３実施形態で説明した第１部品２００と第２部品２１０の接触状態を撮像できる位置に移動するように制御する。
次に、ロボット制御装置１０ａは、第１部品２００と第２部品２１０との接触状態を、力センサー２０Ａｄが検出した検出値（離散化後および離散化前）、撮像装置３０が撮像した画像データを使用して判別する。
次に、ロボット制御装置１０ａは、判別した結果に基づいて、第１〜第３実施形態と同様に、第１部品２００と第２部品２１０の接触状態を制御して、第１部品２００を第２部品２１０の穴２１１に嵌め合わせるように制御する。

以上のように、本実施形態のロボット装置１ａは、第１〜第３実施形態と同様に、力センサー２０Ａｄが検出した検出値に基づいて、第１部品２００と第２部品２１０との接触状態を決定するようにした。そして、本実施形態のロボット装置１ａは、決定した状態に応じて、テーブル記憶部１０３に記憶されている遷移情報を選択して、第１部品２００と第２部品２１０との接触状態を制御するようにした。この結果、本実施形態によれば、第２部品２１０の穴２１１が面取されていなくても、第１部品を嵌め合うことができる。

なお、第４実施形態において、マニピュレータ部２０Ｂのハンド２０Ｂｃに撮像装置３０が取り付けられている例を説明したが、これに限られない。撮像装置３０は、第１〜第３実施形態と同様に、第１部品２００と第２部品２１０の接触状態が撮像可能な位置に設置するようにしてもよい。この場合、ハンド２０Ｂｃは把持部であってもよい。

なお、第１から第３実施形態において、把持部２０ｃ（含む２０ａｃ）が第１部品２００を把持し、第２部品２１０の穴２１１に嵌め込む例を説明したが、これに限られない。把持部２０ｃ（含む２０ａｃ）が第２部品２１０を把持し、第１部品２００に嵌め込むように制御するようにしてもよい。この場合においても、テーブル記憶部１０３に、予め第１部品２００と第２部品２１０との接触状態毎の力センサーの検出値、遷移情報を記憶させ、記録させたこれらの情報を用いて、ロボット制御装置１０が、第１部品２００と第２部品２１０との接触状態を制御することで、第２部品２１０の穴２１１に第１部品２００を挿入するように制御するようにしてもよい。

なお、第１から第４実施形態において、テーブル記憶部１０３に記憶する状態毎の力センサー２０ｄの検出値（３値化前および３値化後）を、実測して記憶させる例を説明したがこれに限れない。テーブル記憶部１０３に記憶する接触状態毎の力センサー２０ｄの検出値（離散化前および離散化後）は、物理モデルを用いて数値計算や解析的な計算により算出し、算出した値を記憶させるようにしてもよい。

なお、第１から第４実施形態において、ロボット装置１（含む１ａ）は、例えばスカラーロボットであってよい。

また、各実施形態におけるロボット制御装置１０、１０ａの一部の機能をコンピューターで実現するようにしてもよい。この場合、その制御機能を実現するための位置検出プログラムをコンピューター読み取り可能な記録媒体に記録して、この記録媒体に記録された位置検出プログラムをコンピューターシステムに読み込ませ、実行することによって実現してもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳ（ＯｐｅｒａｔｉｎｇＳｙｓｔｅｍ）や周辺装置のハードウェアを含むものである。また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、光ディスク、メモリーカード等の可搬型記録媒体、コンピューターシステムに内蔵される磁気ハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間の間、動的にプログラムを保持するもの、その場合のサーバー装置やクライアントとなるコンピューターシステム内部の揮発性メモリのように、一定時間プログラムを保持するものを含んでもよい。また上記のプログラムは、前述した機能の一部を実現するためのものであってもよく、さらに前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせにより実現するものであってもよい。

１・・・ロボット装置
１０・・・ロボット制御装置
２０・・・多関節ロボット
３０・・・撮像装置
２０ａ・・・支持台
２０ｂ、２０Ａ、２０Ｂ・・・マニピュレータ部
２０ｃ、２０Ａｃ、２０Ｂｃ・・・把持部
２０ｄ、２０Ａｄ・・・力センサー
１０１・・・センサー検出値取得部（取得部）
１０２・・・離散化部
１０３・・・テーブル記憶部（記憶部）
１０４・・・選択部
１０５・・・制御部
１０６・・・制御記憶部
１０７・・・画像取得部
２００・・・第１部品
２１０・・・第２部品
２１１・・・穴
３０２・・・本体
３０３Ａ、３０３Ｂ・・・搬送部

Claims (6)

1. 複数のアームと関節を含むマニピュレータ部と、前記マニピュレータ部に連結された把持部と、力センサーとを含むロボットにより部品の組み立てを行わせるロボットの制御装置において、
   前記把持部の接触状態を表す情報と、前記把持部を制御するための複数の制御情報のいずれかを基に、前記ロボットを制御する、ロボット制御装置。
2. 前記制御情報は、予め検出された前記力センサーの検出値と前記接触状態を表す情報と、が関連づけられて複数組、記憶部に記憶されている、請求項１に記載のロボット制御装置。
3. 前記力センサーの検出値と、前記記憶部に記憶されている情報とを比較し、前記比較した結果に基づいて、前記接触状態を判別して前記制御情報を選択する選択部を備える、請求項２に記載のロボット制御装置。
4. 前記接触状態毎に、前記力センサーの検出値を離散化し、前記記憶部に前記離散化後の検出値を記憶させる離散化部を備える、請求項２に記載のロボット制御装置。
5. 前記把持部の接触状態は、前記把持部が把持する第１部品についての第２部品との接触状態である、請求項１から請求項４のいずれか１項に記載のロボット制御装置。
6. ロボット制御装置により制御される部品の組み立て方法であって、
   第１部品を把持する把持部に働く力とモーメントを力センサーが検出して検出値として出力した前記検出値に基づいて、前記第１部品と第２部品との接触状態を判別し、判別した結果に基づいて、記憶部に前記接触状態に関連付けられて記憶されている情報を選択する選択手順と、
   前記選択した前記情報に基づいて、前記把持部を制御する制御手順と、
   を含む組み立て方法。

Images