(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024115395
(43)【公開日】2024-08-26
(54)【発明の名称】コントローラ、制御方法および制御プログラム
(51)【国際特許分類】
B25J 9/16 20060101AFI20240819BHJP
B25J 13/08 20060101ALI20240819BHJP
【ＦＩ】
B25J9/16
B25J13/08 Z
【審査請求】未請求
【請求項の数】15
【出願形態】ＯＬ
(21)【出願番号】P 2023021070
(22)【出願日】2023-02-14
(71)【出願人】
【識別番号】000002945
【氏名又は名称】オムロン株式会社
(74)【代理人】
【識別番号】110001195
【氏名又は名称】弁理士法人深見特許事務所
(72)【発明者】
【氏名】恵木 守
【テーマコード（参考）】
3C707
【Ｆターム（参考）】
3C707AS07
3C707BS12
3C707DS01
3C707ES03
3C707HS27
3C707KS34
3C707KW03
3C707KX07
3C707LT07
3C707LT12
3C707LU08
3C707LV17
3C707MS05
(57)【要約】
【課題】２つのワークを嵌合させる場合のアプローチ位置の少なくとも一部を適切に決定するための方法を提供する。
【解決手段】ロボットを用いて第１ワークと第２ワークとを嵌合させるコントローラが提供される。ロボットは、第１ワークを把持するための把持機構と力覚センサとを有している。コントローラは、ロボットに、第１ワークを予め定められた力で第２ワークに押し付ける動作と、第１ワークを第２ワークに押し付けた状態で、第２ワークに対する第１ワークの姿勢を変化させる動作とを実行させる。コントローラは、第１ワークと第２ワークとの距離に関連する評価指標に基づいて、第１ワークの第２ワークと接触する第１面と、第２ワークの第１ワークと接触する第２面とが平行になる第１ワークの姿勢を探索する。
【選択図】図１
【解決手段】ロボットを用いて第１ワークと第２ワークとを嵌合させるコントローラが提供される。ロボットは、第１ワークを把持するための把持機構と力覚センサとを有している。コントローラは、ロボットに、第１ワークを予め定められた力で第２ワークに押し付ける動作と、第１ワークを第２ワークに押し付けた状態で、第２ワークに対する第１ワークの姿勢を変化させる動作とを実行させる。コントローラは、第１ワークと第２ワークとの距離に関連する評価指標に基づいて、第１ワークの第２ワークと接触する第１面と、第２ワークの第１ワークと接触する第２面とが平行になる第１ワークの姿勢を探索する。
【選択図】図１
【特許請求の範囲】
【請求項1】
ロボットを用いて第１ワークと第２ワークとを嵌合させるためのコントローラであって、前記ロボットは、前記第１ワークを把持するための把持機構と力覚センサとを有しており、前記コントローラは、前記ロボットに、
前記第１ワークを予め定められた力で前記第２ワークに押し付ける動作と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させる動作とを実行させ、
前記コントローラは、前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面と、前記第２ワークの前記第１ワークと接触する第２面とが平行になる前記第１ワークの姿勢を探索する、コントローラ。
前記第１ワークを予め定められた力で前記第２ワークに押し付ける動作と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させる動作とを実行させ、
前記コントローラは、前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面と、前記第２ワークの前記第１ワークと接触する第２面とが平行になる前記第１ワークの姿勢を探索する、コントローラ。
【請求項2】
前記評価指標は、前記第２ワークに予め設定された面から、前記ロボットのアーム先端を基準とした座標系において設定された計測ポイントまでの距離である、請求項１に記載のコントローラ。
【請求項3】
前記計測ポイントは、前記計測ポイントを前記第１面を含む面へ投影したときに、前記第１ワークの姿勢を変化させる動作において、前記第２面と接触する前記第１面の２つの最外辺によって規定される範囲内に、当該投影位置が存在するように設定される、請求項２に記載のコントローラ。
【請求項4】
前記評価指標は、前記第１ワークを予め定められた力で前記第２ワークに押し付けるための、前記ロボットに与えられる指令についての補正量である、請求項１に記載のコントローラ。
【請求項5】
前記コントローラは、前記評価指標が極値となる姿勢を、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢として決定する、請求項１に記載のコントローラ。
【請求項6】
前記ロボットの位置および姿勢を制御するための基準ポイントは、前記第１ワークの前記第１面の内側に設定される、請求項１に記載のコントローラ。
【請求項7】
前記第２ワークに対する前記第１ワークの姿勢を変化させる動作は、
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向に回転させる動作と、
前記第１ワークを前記第１方向とは反対方向である第２方向に回転させる動作とを含む、請求項１～６のいずれか１項に記載のコントローラ。
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向に回転させる動作と、
前記第１ワークを前記第１方向とは反対方向である第２方向に回転させる動作とを含む、請求項１～６のいずれか１項に記載のコントローラ。
【請求項8】
前記コントローラは、前記第１ワークを前記第１方向に回転することで決定された前記第１ワークの第１姿勢と、前記第１ワークを前記第２方向に回転することで決定された前記第１ワークの第２姿勢との平均を、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢として決定する、請求項７に記載のコントローラ。
【請求項9】
前記第２ワークに対する前記第１ワークの姿勢を変化させる動作は、
前記第２ワークに対する前記第１ワークの姿勢を第１変化速度で変化させる動作と、
前記第２ワークに対する前記第１ワークの姿勢を前記第１変化速度とは異なる第２変化速度で変化させる動作とを含む、請求項１～６のいずれか１項に記載のコントローラ。
前記第２ワークに対する前記第１ワークの姿勢を第１変化速度で変化させる動作と、
前記第２ワークに対する前記第１ワークの姿勢を前記第１変化速度とは異なる第２変化速度で変化させる動作とを含む、請求項１～６のいずれか１項に記載のコントローラ。
【請求項10】
前記第２変化速度は、前記第１変化速度より小さい、請求項９に記載のコントローラ。
【請求項11】
前記コントローラは、
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向にのみ回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を決定し、
続いて、前記第１ワークを前記第１方向に回転することで探索された結果、および、前記第１ワークを前記第１方向とは反対方向である第２方向に回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を更新する、請求項１～６のいずれか１項に記載のコントローラ。
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向にのみ回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を決定し、
続いて、前記第１ワークを前記第１方向に回転することで探索された結果、および、前記第１ワークを前記第１方向とは反対方向である第２方向に回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を更新する、請求項１～６のいずれか１項に記載のコントローラ。
【請求項12】
前記第１ワークを第１方向にのみ回転させる場合には、前記第１ワークを第１変化速度で回転させ、
前記第１ワークを前記第１方向および前記第２方向に回転させる場合には、前記第１ワークを前記第１変化速度より小さい第２変化速度で回転させる、請求項１１に記載のコントローラ。
前記第１ワークを前記第１方向および前記第２方向に回転させる場合には、前記第１ワークを前記第１変化速度より小さい第２変化速度で回転させる、請求項１１に記載のコントローラ。
【請求項13】
前記コントローラは、前記第１ワークと前記第２ワークとが非接触の状態で、前記力覚センサから出力されている力覚情報に基づいてオフセットを除去する、請求項１～６のいずれか１項に記載のコントローラ。
【請求項14】
ロボットを用いて第１ワークと第２ワークとを嵌合させるための制御方法であって、前記第１ワークは前記ロボットの把持機構により把持されており、前記制御方法は、
前記第１ワークを予め定められた力で前記第２ワークに押し付けるステップと、
前記第１ワークを前記第２ワークに押し付けた状態で、前記ロボットが前記第２ワークに対する前記第１ワークの姿勢を変化させるステップと、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面と、前記第２ワークの前記第１ワークと接触する第２面とが平行になる前記第１ワークの姿勢を探索するステップとを備える、制御方法。
前記第１ワークを予め定められた力で前記第２ワークに押し付けるステップと、
前記第１ワークを前記第２ワークに押し付けた状態で、前記ロボットが前記第２ワークに対する前記第１ワークの姿勢を変化させるステップと、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面と、前記第２ワークの前記第１ワークと接触する第２面とが平行になる前記第１ワークの姿勢を探索するステップとを備える、制御方法。
【請求項15】
第１ワークと第２ワークとを嵌合させるための制御プログラムであって、前記第１ワークはロボットの把持機構により把持されており、前記制御プログラムは、コンピュータに、
前記第１ワークを予め定められた力で前記第２ワークに押し付けるための指令を前記ロボットに与えるステップと、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させるための指令を前記ロボットに与えるステップと、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面と、前記第２ワークの前記第１ワークと接触する第２面とが平行になる前記第１ワークの姿勢を探索するステップとを実行させる、制御プログラム。
前記第１ワークを予め定められた力で前記第２ワークに押し付けるための指令を前記ロボットに与えるステップと、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させるための指令を前記ロボットに与えるステップと、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面と、前記第２ワークの前記第１ワークと接触する第２面とが平行になる前記第１ワークの姿勢を探索するステップとを実行させる、制御プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、コントローラ、制御方法および制御プログラムに関する。
【背景技術】
【0002】
産業オートメーション（Industrial Automation）の分野において、様々な用途にロボットが利用されている。このような用途の一つとして、２つのワークを嵌合する作業がある。
【0003】
例えば、特開２０１８－１９９１７２号公報（特許文献１）は、探り動作を行わずに、挿入物を被挿入物に挿入する作業を効率的に行う方法を開示する。より具体的には、第２接触動作において、第１方向と直交する第２方向における目標力の成分を０より大きい値とした力制御を行うことにより、第１接触動作での接触部分とは異なる部分に挿入物と被挿入物とを接触させる制御方法が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開２０１８－１９９１７２号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
例えば、ロボットを用いて２つのワークを嵌合させる場合には、探り動作により、ワークの適切な位置および姿勢を決定する必要がある。探り動作を開始するためには、一方のワークを他方のワークに設けられた嵌合孔の近傍に配置する必要がある。探り動作を開始する前のワークの初期位置および初期姿勢（以下、「アプローチ位置」とも称す。）を適切に決定する必要がある。
【0006】
上述の先行技術文献は、アプローチ位置を適切に決定する方法については開示していない。
【0007】
本発明は、２つのワークを嵌合させる場合のアプローチ位置の少なくとも一部を適切に決定するための方法を提供する。
【課題を解決するための手段】
【0008】
ある実施の形態に従えば、ロボットを用いて第１ワークと第２ワークとを嵌合させるためのコントローラが提供される。ロボットは、第１ワークを把持するための把持機構と力覚センサとを有している。コントローラは、ロボットに、第１ワークを予め定められた力で第２ワークに押し付ける動作と、第１ワークを第２ワークに押し付けた状態で、第２ワークに対する第１ワークの姿勢を変化させる動作とを実行させる。コントローラは、第１ワークと第２ワークとの距離に関連する評価指標に基づいて、第１ワークの第２ワークと接触する第１面と、第２ワークの第１ワークと接触する第２面とが平行になる第１ワークの姿勢を探索する。
【0009】
この構成によれば、第１ワークと第２ワークとの距離に関連する評価指標に基づいて、第１ワークの第２ワークと接触する第１面と、第２ワークの第１ワークと接触する第２面とが平行になる第１ワークの姿勢を決定できるので、決定された第１ワークの姿勢に基づいて、アプローチ位置に必要な少なくとも一部の情報を決定できる。
【0010】
評価指標は、第２ワークに予め設定された面から、ロボットのアーム先端を基準とした座標系において設定された計測ポイントまでの距離であってもよい。この構成によれば、第２ワークに予め設定された面から第１ワークまでの距離に基づいて評価できるので、実運用に適したアプローチ位置を決定できる。
【0011】
計測ポイントは、計測ポイントを第１面を含む面へ投影したときに、第１ワークの姿勢を変化させる動作において、第２面と接触する第１面の２つの最外辺によって規定される範囲内に、当該投影位置が存在するように設定されてもよい。この構成によれば、評価指標の値は、第１面と第２面との関係をより正確に反映したものとなるので、アプローチ位置の決定精度を向上できる。
【0012】
評価指標は、第１ワークを予め定められた力で第２ワークに押し付けるための、ロボットに与えられる指令についての補正量であってもよい。この構成によれば、制御系において算出される補正量に基づいて、第１面と第２面とが平行になる第１ワークの姿勢を決定できるので、アプローチ位置の決定精度を向上できる。
【0013】
コントローラは、評価指標が極値となる姿勢を、第１面と第２面とが平行になる第１ワークの姿勢として決定してもよい。この構成によれば、第２ワークに対する第１ワークの姿勢に応じて変化する評価指標の値に基づいて、第１面と第２面とが平行になる第１ワークの姿勢を決定できる。
【0014】
ロボットの位置および姿勢を制御するための基準ポイントは、第１ワークの第１面の内側に設定されてもよい。この構成によれば、第２ワークに対する第１ワークの姿勢をより正確に制御できる。
【0015】
第２ワークに対する第１ワークの姿勢を変化させる動作は、第１ワークを第２ワークに押し付ける方向と直交する軸の周りに、第１ワークを第１方向に回転させる動作と、第１ワークを第１方向とは反対方向である第２方向に回転させる動作とを含んでもよい。この構成によれば、一方向に回転させる場合に生じ得る遅延による誤差を低減できる。
【0016】
コントローラは、第１ワークを第１方向に回転することで決定された第１ワークの第１姿勢と、第１ワークを第２方向に回転することで決定された第１ワークの第２姿勢との平均を、第１面と第２面とが平行になる第１ワークの姿勢として決定してもよい。この構成によれば、第１方向に回転させる場合に生じ得る遅延と、第２方向に回転させる場合に生じ得る遅延とを打ち消すことができるので、アプローチ位置の決定精度を向上できる。
【0017】
第２ワークに対する第１ワークの姿勢を変化させる動作は、第２ワークに対する第１ワークの姿勢を第１変化速度で変化させる動作と、第２ワークに対する第１ワークの姿勢を第１変化速度とは異なる第２変化速度で変化させる動作とを含んでいてもよい。この構成によれば、第１ワークの姿勢を変化させる変化速度を異ならせることで、短時間かつ高精度の探索を行うことができる。
【0018】
第２変化速度は、第１変化速度より小さくてもよい。この構成によれば、第１ワークの姿勢を第１変化速度で変化させて第１面と第２面とが平行になる第１ワークの姿勢を決定した後に、第１変化速度より小さい第２変化速度で第１面と第２面とが平行になる第１ワークの姿勢を決定できるので、アプローチ位置の決定精度を向上できる。
【0019】
コントローラは、第１ワークを第２ワークに押し付ける方向と直交する軸の周りに、第１ワークを第１方向にのみ回転することで探索された結果に基づいて、第１面と第２面とが平行になる第１ワークの姿勢を決定してもよい。コントローラは、続いて、第１ワークを第１方向に回転することで探索された結果、および、第１ワークを第１方向とは反対方向である第２方向に回転することで探索された結果に基づいて、第１面と第２面とが平行になる第１ワークの姿勢を更新してもよい。この構成によれば、第１ワークを第１方向にのみ回転することで探索に要する時間を短縮しつつ、第１ワークを両方向に回転することで探索の精度を向上できる。
【0020】
第１ワークを第１方向にのみ回転させる場合には、第１ワークは、第１変化速度で回転させられる。第１ワークを第１方向および第２方向に回転させる場合には、第１ワークは、第１変化速度より小さい第２変化速度で回転させられる。この構成によれば、第１ワークを第１方向および第２方向に回転させる場合には、変化速度を小さくすることで、探索の精度を向上できる。
【0021】
コントローラは、第１ワークと第２ワークとが非接触の状態で、力覚センサから出力されている力覚情報に基づいてオフセットを除去してもよい。この構成によれば、第１ワークと第２ワークとが非接触の状態において力覚センサに存在するオフセットに起因する誤差を低減できる。
【0022】
別の実施の形態に従えば、ロボットを用いて第１ワークと第２ワークとを嵌合させるための制御方法が提供される。第１ワークはロボットの把持機構により把持されている。制御方法は、第１ワークを予め定められた力で第２ワークに押し付けるステップと、第１ワークを第２ワークに押し付けた状態で、ロボットが第２ワークに対する第１ワークの姿勢を変化させるステップと、第１ワークと第２ワークとの距離に関連する評価指標に基づいて、第１ワークの第２ワークと接触する第１面と、第２ワークの第１ワークと接触する第２面とが平行になる第１ワークの姿勢を探索するステップとを含む。
【0023】
さらに別の実施の形態に従えば、第１ワークと第２ワークとを嵌合させるための制御プログラムが提供される。第１ワークはロボットの把持機構により把持されている。制御プログラムは、コンピュータに、第１ワークを予め定められた力で第２ワークに押し付けるための指令をロボットに与えるステップと、第１ワークを第２ワークに押し付けた状態で、第２ワークに対する第１ワークの姿勢を変化させるための指令をロボットに与えるステップと、第１ワークと第２ワークとの距離に関連する評価指標に基づいて、第１ワークの第２ワークと接触する第１面と、第２ワークの第１ワークと接触する第２面とが平行になる第１ワークの姿勢を探索するステップとを実行させる。
【発明の効果】
【0024】
本発明によれば、２つのワークを嵌合させる場合のアプローチ位置の少なくとも一部を適切に決定できる。
【図面の簡単な説明】
【0025】
【図1】本実施の形態に従うロボットシステムの組立動作の一例を示す模式図である。
【図2】本実施の形態に従うロボットシステムにおけるＴＣＰの設定例を示す模式図である。
【図3】本実施の形態に従うロボットシステムにおけるＴＣＰの設定例を示す模式図である。
【図4】本実施の形態に従うロボットシステムのハードウェア構成例を示す模式図である。
【図5】本実施の形態に従うアプローチ位置の校正処理の処理手順を示すフローチャートである。
【図6】本実施の形態に従うアプローチ位置の校正処理により決定される位置関係を説明するための図である。
【図7】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理の処理内容を説明するための図である。
【図8】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理におけるＴＣＰおよび計測ポイントの設定例を説明するための図である。
【図9】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理における計測ポイントの設定例を説明するための図である。
【図10】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理において受入面（設計値）からの距離を評価指標とした場合の処理内容を説明するための図である。
【図11】本実施の形態に従うＺｔ軸についての力制御を実現する制御系の一例を示す模式図である。
【図12】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理においてＺ位置補正量を評価指標とした場合の処理内容を説明するための図である。
【図13】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理においてＺ位置補正量を評価指標とした場合の処理結果の一例を示す図である。
【図14】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のより詳細な処理手順を示すフローチャートである。
【図15】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＺ校正値を決定する際の動作例を説明するための図である。
【図16】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＲｙ校正値を決定する際の動作例を説明するための図である。
【図17】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＺ位置補正量の時間変化を説明するための図である。
【図18】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理に含まれる低速探索の処理手順を示すフローチャートである。
【図19】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理に含まれる低速探索の処理手順を示すフローチャートである。
【図20】本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＺ校正値を決定する処理を説明するための図である。
【図21】本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の第１方向ベクトルを決定する処理を説明するための図である。
【図22】本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の第２方向ベクトルを決定する処理を説明するための図である。
【図23】本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理のＸｔＹｔ原点を決定する処理を説明するための図である。
【図24】本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理のより詳細な処理手順を示すフローチャートである。
【図25】本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理のより詳細な処理手順を示すフローチャートである。
【図26】本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の方向ベクトルを算出するための位置計測方法の一例を説明するための図である。
【図27】本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の方向ベクトルを算出処理の変形例を説明するための図である。
【発明を実施するための形態】
【0026】
本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰り返さない。
【0027】
＜Ａ．適用例＞
まず、本発明が適用される場面の一例について説明する。
まず、本発明が適用される場面の一例について説明する。
【0028】
図１は、本実施の形態に従うロボットシステム１の組立動作の一例を示す模式図である。図１を参照して、ロボットシステム１は、多関節ロボット（以下、単に「ロボット１０」とも称す。）と、ロボット１０を制御するコントローラ１００とを含む。
【0029】
ロボットシステム１は、第１ワーク５０と第２ワーク６０とを嵌合させる。第１ワーク５０および第２ワーク６０は、剛性をもった材料で構成されている。第２ワーク６０には嵌合孔６２が設けられており、第１ワーク５０は嵌合孔６２に挿入される。すなわち、ロボットシステム１は、第１ワーク５０を第２ワーク６０の嵌合孔６２に挿入する。
【0030】
本明細書において、「嵌合」は、任意の断面形状を有する孔（嵌合孔）に別のワークが挿入された状態と、挿入された状態にするための動作との両方を包含する。なお、孔（嵌合孔）に別のワークが挿入される動作を意図する場合には、「嵌合動作」との表現を用いることもある。嵌合孔は、どのような断面形状であってもよいが、以下の説明では、一例として断面形状四角形とする。
【0031】
嵌合動作において、通常、第２ワーク６０は予め定められた位置に配置されている。ロボット１０は、第１ワーク５０を把持した状態で、第１ワーク５０を初期の位置および姿勢（アプローチ位置）に移動させる。その後、探り動作および挿入動作が行われる。
【0032】
本明細書において、「アプローチ位置」は、第２ワーク６０（被嵌合ワーク）の嵌合孔６２に挿入される第１ワーク５０（嵌合ワーク）の初期位置および初期姿勢を意味する。説明の便宜上、「アプローチ位置」と記載するが、典型的には、位置（３要素）および姿勢（３要素）の合計６要素を含む。
【0033】
本明細書において、「把持」は、ロボットなどがワークを保持するための任意の動作を包含する。図１には、複数の爪部を用いてワークを保持する例を示すが、磁力（例えば、電磁石）を用いる方式を採用してもよいし、圧力差（負圧）を用いる方式を採用してもよいし、その他の任意の方式を採用してもよい。
【0034】
図１に示すロボット１０は、ベース１１と、複数の可動部１２，１３，１４，１５，１６，１７とを含む。可動部１２，１３，１４，１５，１６，１７は、ロボット１０のジョイントに相当する。可動部１２，１３，１４，１５，１６，１７の各々は、図１に示すような回転軸に沿ってロボット１０を構成するリンクを駆動する。
【0035】
ロボット１０のアーム先端には、把持機構２０が設けられている。把持機構２０には、第１ワーク５０を把持するための２つの爪部２１，２２を有している。爪部２１と爪部２２との間の距離を調整することで、第１ワーク５０が把持および解放される。以下に説明するアプローチ位置の校正処理において、第１ワーク５０はロボット１０の把持機構２０により把持された状態に維持される。
【0036】
ロボット１０のアーム先端と把持機構２０との間には、力覚センサ１８が設けられている。力覚センサ１８は、把持機構２０などに発生している荷重の大きさ、および、荷重の方向などを含む検出結果を出力する。力覚センサ１８の検出結果は、ベクトル値で出力されてもよい。
【0037】
このように、ロボット１０は、複数のジョイントに加えて、把持機構２０および力覚センサ１８を有している。
【0038】
コントローラ１００には、情報処理装置２００が接続されてもよい。情報処理装置２００は、例えば、汎用コンピュータである。情報処理装置２００は、コントローラ１００からの情報をユーザに提示するとともに、ユーザ操作に従ってユーザ指示をコントローラ１００へ与える。
【0039】
図１に示す例では、ロボット１０を基準としたロボット座標系２が定義されている。また、アーム先端を基準としたツール座標系３０が定義されている。ツール座標系３０は、アーム先端の面に平行なＸｔ軸およびＹｔ軸、ならびに、当該面と直交するＺｔ軸を含む。典型的には、Ｘｔ軸とＹｔ軸とは直交するようにツール座標系３０が定義されるが、Ｘｔ軸とＹｔ軸とが平行にならなければ、どのように定義してもよい。
【0040】
ロボット１０の位置および姿勢を制御するための基準ポイントをＴＣＰ（tool center point）と称す。ＴＣＰは、一例として、ツール座標系３０において定義される。ＴＣＰは、位置（Ｘ，Ｙ，Ｚ）および姿勢（Ｒｘ，Ｒｙ，Ｒｚ）の６要素を含む。位置（Ｘ，Ｙ，Ｚ）は、ツール座標系３０のＸｔ，Ｙｔ，Ｚｔ軸上のそれぞれの位置である。姿勢（Ｒｘ，Ｒｙ，Ｒｚ）は、ツール座標系３０のＸｔ，Ｙｔ，Ｚｔ軸周りのそれぞれの角度である。
【0041】
コントローラ１００は、ＴＣＰが目標の位置および姿勢となるように、ロボット１０（可動部１２，１３，１４，１５，１６，１７の角度）を制御する。
【0042】
コントローラ１００は、ロボット１０のアーム先端の位置および姿勢をオフセットベクトル（位置および姿勢）で補正することで、ＴＣＰの位置および姿勢を算出することができる。ＴＣＰは、嵌合動作の内容に応じて、任意の位置に設定してもよい。例えば、第２ワーク６０に挿入される第１ワーク５０の面（以下、「先端面５２」とも称す。）の角（かど）にＴＣＰに設定してもよいし、第１ワーク５０の先端面５２の中心をＴＣＰに設定してもよい。図１に示す例では、第１ワーク５０の先端面５２の角（かど）にＴＣＰが設定されている。
【0043】
【0044】
図２には、一例として、第２ワーク６０の嵌合孔６２の角（かど）を探索する動作に適したＴＣＰの設定例を示す。すなわち、第１ワーク５０の先端面５２の角（かど）をＴＣＰに設定することで、設定されたＴＣＰに基づいて、第１ワーク５０の角（かど）と第２ワーク６０の嵌合孔６２の角（かど）とが接触する位置および姿勢を探索できる。
【0045】
図３には、一例として、第２ワーク６０の嵌合孔６２の位置を探索するのに適したＴＣＰの設定例を示す。すなわち、第１ワーク５０の先端面５２の中心をＴＣＰに設定することで、設定されたＴＣＰに基づいて、第１ワーク５０を挿入すべき第２ワーク６０の嵌合孔６２を探索できる。また、第１ワーク５０と第２ワーク６０との接触も探索できる。
【0046】
＜Ｂ．ロボットシステム１のハードウェア構成例＞
図４は、本実施の形態に従うロボットシステム１のハードウェア構成例を示す模式図である。図４を参照して、ロボット１０は、可動部１２，１３，１４，１５，１６，１７にそれぞれ対応付けられたモータ３２，３３，３４，３５，３６，３７と、モータ３２，３３，３４，３５，３６，３７をそれぞれ駆動するドライバ４２，４３，４４，４５，４６，４７とを含む。
図４は、本実施の形態に従うロボットシステム１のハードウェア構成例を示す模式図である。図４を参照して、ロボット１０は、可動部１２，１３，１４，１５，１６，１７にそれぞれ対応付けられたモータ３２，３３，３４，３５，３６，３７と、モータ３２，３３，３４，３５，３６，３７をそれぞれ駆動するドライバ４２，４３，４４，４５，４６，４７とを含む。
【0047】
ロボット１０は、爪部２１，２２を駆動するためのモータ３８、および、モータ３８を駆動するドライバ４８を含む。
【0048】
ドライバ４２，４３，４４，４５，４６，４７，４８、力覚センサ１８、ティーチングペンダント２６は、インターフェイス４０を介して、コントローラ１００と電気的に接続される。
【0049】
ティーチングペンダント２６は、ユーザ操作に応じて、ロボット１０のティーチングなどを行う。ティーチングペンダント２６は、ロボット１０に対して着脱可能に構成されてもよい。
【0050】
コントローラ１００は、一種のコンピュータであり、主要なハードウェアコンポーネントとして、プロセッサ１０２と、メモリ１０４と、インターフェイス１０６と、ストレージ１１０とを含む。これらのコンポーネントはバス１０８を介して電気的に接続される。
【0051】
プロセッサ１０２は、例えば、ＣＰＵ（Central Processing Unit）、ＭＰＵ（Micro-Processing Unit）などで構成される。メモリ１０４は、例えば、ＤＲＡＭ（Dynamic Random Access Memory）、ＳＲＡＭ（Static Random Access Memory）などの揮発性記憶装置で構成される。ストレージ１１０は、ＳＳＤ（Solid State Disk）、フラッシュメモリなどの不揮発性記憶装置で構成される。ストレージ１１０は、基本的な処理を実現するためのシステムプログラム１１２と、ロボット１０を制御するための制御プログラム１１４とを格納する。制御プログラム１１４は、ロボット１０を制御するためのコンピュータ読取可能な命令を含む。プロセッサ１０２は、ストレージ１１０に格納されたシステムプログラム１１２および制御プログラム１１４を読み出して、メモリ１０４に展開して実行することで、後述するようなロボット１０を制御するための処理を実現する。
【0052】
インターフェイス１０６は、コントローラ１００とロボット１０との間の信号および／またはデータのやり取りを担当する。ロボットシステム１においては、ドライバ４２，４３，４４，４５，４６，４７，４８を制御するための指令がコントローラ１００からロボット１０へ送信されるとともに、力覚センサ１８による検出結果がロボット１０からコントローラ１００へ送信される。
【0053】
図４には、プロセッサ１０２がプログラムを実行することで必要な処理が提供される構成例を示したが、これらの提供される処理の一部または全部を、専用のハードウェア回路（例えば、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）など）を用いて実装してもよい。
【0054】
図４には、コントローラ１００をロボット１０から独立して構成した例を示しているが、コントローラ１００が提供する機能および処理の一部または全部をロボット１０に組み入れてもよい。ロボット１０がコントローラ１００を内蔵していてもよい。コントローラ１００は、ロボットの制御に専用化されたコントローラであってもよいし、汎用的なＰＬＣ（プログラマブルコントローラ）あるいはパーソナルコンピュータであってもよい。
【0055】
コントローラ１００が提供する機能および処理の一部または全部をいわゆるクラウドと称されるネットワーク上のコンピューティングリソースを用いて実現してもよい。
【0056】
以上のように、本実施の形態に従うロボットシステム１は、どのような構成で実装してもよい。
【0057】
＜Ｃ．アプローチ位置の校正処理＞
次に、本実施の形態に従うアプローチ位置の校正処理について説明する。
次に、本実施の形態に従うアプローチ位置の校正処理について説明する。
【0058】
図５は、本実施の形態に従うアプローチ位置の校正処理の処理手順を示すフローチャートである。図５に示す各ステップは、コントローラ１００のプロセッサ１０２がシステムプログラム１１２および／または制御プログラム１１４を実行することで実現されてもよい。
【0059】
図５を参照して、コントローラ１００は、Ｒｘ／Ｒｙ／Ｚ校正処理を実行する（ステップＳ１）。Ｒｘ／Ｒｙ／Ｚ校正処理は、アプローチ位置を定義するための、Ｘｔ軸周りの角度Ｒｘ、Ｙｔ軸周りの角度Ｒｙ、Ｚｔ軸上の位置Ｚを決定する。
【0060】
続いて、コントローラ１００は、Ｘ／Ｙ／Ｒｚ校正処理を実行する（ステップＳ２）。Ｘ／Ｙ／Ｒｚ校正処理は、アプローチ位置を定義するための、Ｘｔ軸上の位置Ｘ、Ｙｔ軸上の位置Ｙ、Ｚｔ軸周りの角度Ｒｚを決定する。
【0061】
コントローラ１００は、Ｒｘ／Ｒｙ／Ｚ校正処理において決定した情報、および、Ｘ／Ｙ／Ｒｚ校正処理において決定した情報に基づいて、第２ワーク６０の嵌合孔６２の角（かど）に対応する位置および姿勢（以下、「受けワーク角位置」とも称す。）を決定する（ステップＳ３）。コントローラ１００は、受けワーク角位置から予め定められたクリアランスΔＸｔｃ，ΔＹｔｃ，ΔＺｔｃをオフセットして、アプローチ位置を算出する（ステップＳ４）。そして、処理は終了する。
【0062】
図６は、本実施の形態に従うアプローチ位置の校正処理により決定される位置関係を説明するための図である。図６を参照して、受けワーク角位置を（Ｘｔｌ，Ｙｔｌ，Ｚｔｌ，Ｒｘｌ，Ｒｙｌ，Ｒｚｌ）とすると、アプローチ位置は、（Ｘｔｌ＋ΔＸｔｃ，Ｙｔｌ＋ΔＹｔｃ，Ｚｔｌ＋ΔＺｔｃ，Ｒｘｌ，Ｒｙｌ，Ｒｚｌ）と算出できる。すなわち、受けワーク角位置をクリアランスΔＸｔｃ，ΔＹｔｃ，ΔＺｔｃだけそれぞれの軸に沿って平行移動させることで、アプローチ位置を決定できる。
【0063】
＜Ｄ．Ｒｘ／Ｒｙ／Ｚ校正処理＞
次に、図５のＲｘ／Ｒｙ／Ｚ校正処理（ステップＳ１）の処理内容について説明する。
次に、図５のＲｘ／Ｒｙ／Ｚ校正処理（ステップＳ１）の処理内容について説明する。
【0064】
（ｄ１：基本的な考え方）
Ｒｘ／Ｒｙ／Ｚ校正処理においては、第１ワーク５０と第２ワーク６０とが面倣い状態になる角度Ｒｘおよび角度Ｒｙを探索する。「面倣い状態」とは、一方の面と他方の面とが平行になる状態を意味する。図１に示すロボットシステム１においては、第１ワーク５０の先端面５２と、第１ワーク５０を受け入れる第２ワーク６０の面（以下、「受入面６４」とも称す。）とが平行になっている状態が面倣い状態である。
Ｒｘ／Ｒｙ／Ｚ校正処理においては、第１ワーク５０と第２ワーク６０とが面倣い状態になる角度Ｒｘおよび角度Ｒｙを探索する。「面倣い状態」とは、一方の面と他方の面とが平行になる状態を意味する。図１に示すロボットシステム１においては、第１ワーク５０の先端面５２と、第１ワーク５０を受け入れる第２ワーク６０の面（以下、「受入面６４」とも称す。）とが平行になっている状態が面倣い状態である。
【0065】
Ｒｘ／Ｒｙ／Ｚ校正処理においては、一例として、Ｚｔ軸についての位置情報を用いて、面倣い状態を探索する。
【0066】
図７は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理の処理内容を説明するための図である。図７を参照して、コントローラ１００は、Ｚｔ軸について力制御を行う。より具体的には、コントローラ１００は、把持機構２０が把持した第１ワーク５０を予め定められた力で第２ワーク６０に押し付ける動作をロボットに実行させる。
【0067】
力制御において、コントローラ１００は、Ｚｔ軸方向に発生する力Ｆｚが目標値と一致するように、位置Ｚの指令を調整する。コントローラ１００は、Ｘｔ軸上の位置ＸおよびＹｔ軸上の位置Ｙについては位置制御を行う。このように、コントローラ１００は、それぞれの目標値（目標位置）と一致するように位置Ｘおよび位置Ｙを調整する。力制御および位置制御は、任意に設定されるＴＣＰに基づいて行われる。
【0068】
上述の力制御および位置制御が行われている状態において、Ｘｔ軸周りの角度ＲｘまたはＹｔ軸周りの角度Ｒｙを変化させると、第２ワーク６０に対する第１ワーク５０の姿勢が変化する。すなわち、コントローラ１００は、第１ワーク５０を第２ワーク６０に押し付けた状態で、第２ワーク６０に対する第１ワーク５０の姿勢を変化させる動作をロボットに実行させる。このとき、Ｚｔ軸についての力制御によって、第１ワーク５０の先端面５２の少なくとも一部は第２ワーク６０の受入面６４に接触した状態が維持される。
【0069】
コントローラ１００は、第１ワーク５０と第２ワーク６０との距離に関連する評価指標に基づいて、第１ワーク５０の第２ワーク６０と接触する先端面５２（第１面）と、第２ワーク６０の第１ワーク５０と接触する受入面６４（第２面）とが平行（すなわち、面倣い）になる第１ワーク５０の姿勢を探索する。
【0070】
評価指標は、第１ワーク５０の先端面５２に設定された計測ポイントに基づいて算出されてもよい。計測ポイントは、第１ワーク５０に関連付けて設定される。この場合、評価指標は、計測ポイントのＺｔ軸上の位置情報を反映する。
【0071】
面倣い状態において、計測ポイントの位置Ｚ（ロボット座標系２）は最小値となる。図７のグラフに示すように、角度Ｒｘまたは角度Ｒｙの回転量を増加させると、面倣い状態において位置Ｚは最小値となる。面倣い状態から離れるに従って、第１ワーク５０が傾くことになり、第１ワーク５０の先端面５２の角（かど）と第２ワーク６０とが接触することで、位置Ｚは増加する。
【0072】
すなわち、角度Ｒｘまたは角度Ｒｙに関して位置ＺはＶ字の軌跡を描くので、位置Ｚが極値（ピーク）または最小値となる角度Ｒｘまたは角度Ｒｙが面倣い状態であると判断できる。このように、コントローラ１００は、角度Ｒｘまたは角度Ｒｙを変化させつつ、位置Ｚの変化を収集する。コントローラ１００は、位置Ｚが極値となる姿勢（角度Ｒｘまたは角度Ｒｙ）を面倣い状態として決定する。
【0073】
面倣い状態における位置Ｚに基づいて、面倣い状態における計測ポイントのＺｔ軸上の位置を決定することもできる。
【0074】
（ｄ２：ＴＣＰおよび計測ポイントの設定例）
次に、図５のＲｘ／Ｒｙ／Ｚ校正処理におけるＴＣＰおよび計測ポイントの設定例について説明する。
次に、図５のＲｘ／Ｒｙ／Ｚ校正処理におけるＴＣＰおよび計測ポイントの設定例について説明する。
【0075】
コントローラ１００は、ＴＣＰの位置および姿勢に基づいてロボット１０を制御する。すなわち、ＴＣＰは、ロボット１０を制御するための目標値（位置および姿勢）を算出するために用いられる。ＴＣＰと計測ポイントとを一致させてもよいし、ＴＣＰとは異なる位置に計測ポイントを設定してもよい。
【0076】
面倣い状態を探索するためには、第１ワーク５０の姿勢（角度Ｒｘまたは角度Ｒｙ）の変化に応じて、計測ポイントの位置Ｚの変化量が大きい方が有利である。第１ワーク５０のＺｔ軸方向の変化量は、ＴＣＰの位置と第１ワーク５０のＸｔ軸方向の幅（長さ）およびＹｔ軸方向の幅（長さ）とに依存する。
【0077】
図８は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理におけるＴＣＰおよび計測ポイントの設定例を説明するための図である。
【0078】
図８の（Ａ）には、ＴＣＰと計測ポイントが一致している設定例を示す。図８の（Ａ）を参照して、ＴＣＰ（および、計測ポイント）は、第１ワーク５０の先端面５２の中心に設定されている。第１ワーク５０の先端面５２が四角形である場合には、対向する２辺の中点同士を結ぶ線と対向する別の２辺の中点同士を結ぶ線との交点、２つの対角線の交点、四角形の重心などにＴＣＰを設定してもよい。あるいは、ＴＣＰおよび計測ポイントは、ツール座標系３０のＺｔ軸と第１ワーク５０の先端面５２との交点に設定されてもよい。
【0079】
図８の（Ｂ）には、ＴＣＰとは異なる位置に計測ポイントが設定されている例を示す。図８の（Ｂ）に示す設定例においては、ＴＣＰは、第１ワーク５０の先端面５２の中心に設定されている。このとき、計測ポイントは、力覚センサ１８の位置に設定されてもよい。
【0080】
ロボット１０の位置および姿勢を制御するための基準ポイント（ＴＣＰ）は、第１ワーク５０の先端面５２の内側に設定されてもよい。さらに、ＴＣＰは、第１ワーク５０の先端面５２の中心に設定されてもよいし、先端面５２とＺｔ軸との交点に設定されてもよい。
【0081】
第１ワーク５０の計測ポイントは、第１ワーク５０の姿勢（角度Ｒｘまたは角度Ｒｙ）を変化させたときに、第１ワーク５０が傾いた状態において、第２ワーク６０に接触する先端面５２の２つの最外辺（対辺）の間に設定されてもよい。一方の最外辺は、第１ワーク５０を第２ワーク６０に対して一方の方向に傾けることで決定され、他方の最外辺は、第１ワーク５０を第２ワーク６０に対して逆の方向に傾けることで決定される。
【0082】
図９は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理における計測ポイントの設定例を説明するための図である。
【0083】
図９を参照して、例えば、角度Ｒｘを変化させて、第１ワーク５０を傾ける場合には、最外辺５６，５７は、Ｘｔ軸に平行な２つの辺となり、Ｙｔ軸上の２つの位置で定義される。この場合、計測ポイントは、Ｙｔ軸上の最外辺５６，５７に対応する２つの位置の間（設定可能範囲５８）に存在すれば、Ｘｔ軸上の位置はいずれであってもよい。すなわち、Ｙｔ軸上の２つの最外辺の間であれば、第１ワーク５０が存在しない位置に計測ポイントが設定されてもよい。
【0084】
一方、角度Ｒｙを変化させて、第１ワーク５０を傾ける場合には、最外辺は、Ｙｔ軸に平行な２つの辺となり、Ｘｔ軸上の２つの位置で定義される。この場合、計測ポイントは、Ｘｔ軸上の最外辺に対応する２つの位置の間に存在すれば、Ｙｔ軸上の位置はいずれであってもよい。すなわち、Ｘｔ軸上の２つの最外辺の間であれば、第１ワーク５０が存在しない位置に計測ポイントが設定されてもよい。
【0085】
さらに、計測ポイントのＺｔ軸上の位置についてもいずれであってもよい。そのため、計測ポイントは、第１ワーク５０の先端面５２上の位置以外の位置に設定されてもよい。
【0086】
このように、計測ポイントは、計測ポイントを先端面５２を含む面へ投影したときに、第１ワーク５０の姿勢を変化させる動作において、受入面６４と接触する先端面５２の２つの最外辺によって規定される範囲内に、当該投影位置が存在するように設定してもよい。
【0087】
このような位置に計測ポイントを設定することで、面倣い状態を探索する処理において、極値の検出を容易化できる。
【0088】
（ｄ３：評価指標）
次に、面倣い状態を探索するための評価指標について説明する。
次に、面倣い状態を探索するための評価指標について説明する。
【0089】
上述の図５においては、面倣い状態を探索するための評価指標として、計測ポイントの位置Ｚを例示したが、これに限らず、他の評価指標を用いることができる。
【0090】
例えば、第２ワーク６０の受入面６４がロボット座標系２（グローバル座標系）のいずれの軸とも平行ではない場合などには、受入面６４からの距離の算出が煩雑化し得る。そのため、第２ワーク６０に対して、受入面（設計値）６６を設定した上で、受入面（設計値）６６からの距離（Ｚｔ軸上の距離）を評価指標にしてもよい。
【0091】
図１０は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理において受入面（設計値）６６からの距離を評価指標とした場合の処理内容を説明するための図である。コントローラ１００は、Ｚｔ軸についての力制御を行う。
【0092】
図１０を参照して、角度Ｒｘまたは角度Ｒｙに関して、受入面（設計値）６６からの距離はＶ字の軌跡を描くので、受入面（設計値）６６からの距離が極値となる角度Ｒｘまたは角度Ｒｙが面倣い状態であると判断できる。面倣い状態における位置Ｚに基づいて、面倣い状態におけるＺｔ軸上の位置を決定することもできる。
【0093】
このように、評価指標は、第２ワーク６０に予め設定された受入面（設計値）６６から、ロボット１０のアーム先端を基準としたツール座標系３０において設定された計測ポイントまでの距離であってもよい。すなわち、評価指標は、第２ワーク６０の受入面６４に対して垂直方向における、受入面６４とロボット１０に設定した計測ポイントとの距離であってもよい。
【0094】
図７に示す計測ポイントの位置Ｚ、および、図１０に示す受入面（設計値）６６からの距離を評価指標として用いる場合には、Ｚｔ軸についての位置情報から面倣い状態を探索するので、Ｚｔ軸についての力制御の変動要因に起因する影響を低減できる。また、受入面（設計値）６６からの距離（Ｚｔ軸上の距離）を評価指標とした場合には、面倣い状態として決定された値をそのままアプローチ位置として用いることができる。
【0095】
また、受け入れ面（設計値）６６からの距離をツール座標系３０のＺｔ軸上の位置で評価する場合には、ＴＣＰの位置変化をロボット座標系２に変換することなく、Ｚｔ軸上の位置そのままで評価できるので、第２ワーク６０の配置姿勢が変化した場合でも、上述した面倣い状態を探索するアルゴリズムをそのまま利用できる。
【0096】
面倣い状態を探索するための別の評価指標として、Ｚｔ軸についての力制御に関する情報を用いてもよい。
【0097】
図１１は、本実施の形態に従うＺｔ軸についての力制御を実現する制御系の一例を示す模式図である。図１１を参照して、コントローラ１００は、第１ワーク５０を予め定められた力で第２ワーク６０に押し付けるための制御系を有している。
【0098】
より具体的には、コントローラ１００は、力覚センサフィルタ１２０と、換算処理モジュール１２２と、差分器１２４と、サンプラ１２６と、力制御演算モジュール１２８と、リミッタ１３０と、ホールダ１３２と、加算器１３４とを含む。
【0099】
力覚センサフィルタ１２０は、力覚センサ１８からの力覚情報をフィルタリングして出力する。フィルタリングにより、力覚情報に含まれるノイズ成分が低減される。
【0100】
換算処理モジュール１２２は、力覚センサフィルタ１２０からの力覚情報に基づいて、予め設定されたＴＣＰに生じている力およびモーメントを算出する。換算処理モジュール１２２は、ＴＣＰにおいてＺｔ軸方向に発生している力Ｆｚを出力する。
【0101】
差分器１２４は、換算処理モジュール１２２から出力される力Ｆｚと押付力の目標値（目標力）との差分である偏差を算出する。
【0102】
サンプラ１２６は、差分器１２４から出力される偏差をサンプリングして、力制御演算モジュール１２８に入力する。
【0103】
力制御演算モジュール１２８は、ＰＩＤ演算により、入力された偏差に対応する出力値を算出する。算出される出力値は、ロボット１０に与えられるＺｔ軸上の位置についての指令に相当する。
【0104】
リミッタ１３０は、力制御演算モジュール１２８から出力される値を予め定められた上下限値内に制限した上で出力する。
【0105】
力制御演算モジュール１２８は、サンプラ１２６が離散的に値を入力することに応じて、出力値を算出する。そのため、リミッタ１３０から出力される値も離散的になる。ホールダ１３２は、リミッタ１３０からの出力が停止しても、直前に入力された値を保持して出力し続ける。ホールダ１３２から出力される値がＺ位置補正量（ΔＺｒｅｆ）となる。
【0106】
加算器１３４は、ＴＣＰの目標位置・挙動（Ｘｒｅｆ，Ｙｒｅｆ，Ｚｒｅｆ，Ｒｘｒｅｆ，Ｒｙｒｅｆ，Ｒｚｒｅｆ）に対して、Ｚ位置補正量（ΔＺｒｅｆ）を加算して、最終的な指令を算出する。すなわち、加算器１３４は、補正後目標位置・指令（Ｘｒｅｆ，Ｙｒｅｆ，Ｚｒｅｆ＋ΔＺｒｅｆ，Ｒｘｒｅｆ，Ｒｙｒｅｆ，Ｒｚｒｅｆ）を出力する。
【0107】
図１１に示すＺ位置補正量を評価指標にしてもよい。すなわち、評価指標は、第１ワーク５０を予め定められた力で第２ワーク６０に押し付けるための、制御系が出力するロボット１０に与えられる指令についての補正量であってもよい。
【0108】
図１２は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理においてＺ位置補正量を評価指標とした場合の処理内容を説明するための図である。図１２を参照して、角度Ｒｘまたは角度Ｒｙに関して、Ｚ位置補正量はＶ字の軌跡を描くので、Ｚ位置補正量が極値となる角度Ｒｘまたは角度Ｒｙが面倣い状態であると判断できる。面倣い状態における位置Ｚに基づいて、面倣い状態におけるＺｔ軸上の位置Ｚを決定することもできる。
【0109】
図１２に示すように、なお、面倣い状態でなければ、第１ワーク５０は、第２ワーク６０の受入面６４に対して傾くことになる。先端面５２のＺｔ軸上の位置は、傾きの角度θに依存して変化する。したがって、Ｚ位置補正量は、傾きの角度θに依存することになる。
【0110】
第１ワーク５０の先端面５２の幅（Ｘｔ軸方向またはＹｔ軸方向）Ｗとすると、Ｚ位置補正量は以下のように表すことができる。
【0111】
Ｚ位置補正量＝－（Ｗ／２）ｓｉｎ（｜Δθ｜）ｃｏｓ（｜Δθ｜）
このように、Ｚ位置補正量は、角度θについての三角関数の二乗に依存する。そのため、Ｚ位置補正量を用いることで、Ｚｔ軸についての位置情報を用いる場合に比較して、より高い感度で面倣い状態を探索できる。
このように、Ｚ位置補正量は、角度θについての三角関数の二乗に依存する。そのため、Ｚ位置補正量を用いることで、Ｚｔ軸についての位置情報を用いる場合に比較して、より高い感度で面倣い状態を探索できる。
【0112】
図１３は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理においてＺ位置補正量を評価指標とした場合の処理結果の一例を示す図である。図１３には、Ｘｔ軸周りの角度Ｒｘ（または、Ｙｔ軸周りの角度Ｒｙ）を変化させた場合の測定例を示す。図１３には、面倣い状態から－２［ｄｅｇ］程度傾いた状態から処理を開始した例を示す。角度の回転速度は、－０．２［ｄｅｇ／ｓ］であり、力Ｆｚの目標値は－１［Ｎ］とした。
【0113】
図１３には、Ｚ位置補正量、軸周りのモーメント、Ｚｔ軸方向に発生する力Ｆｚ、および、角度の回転量の時間変化を示す。
【0114】
図１３において、Ｚ位置補正量が極値となる角度が面倣い状態であると判断できる。面倣い状態の前後において、Ｚｔ軸方向に発生する力Ｆｚは大きく変化するものの、比較的大きなノイズが含まれているため、面倣い状態になる角度を正確に検出することが難しい。これに対して、Ｚ位置補正量は、力Ｆｚが大きく変化しても、当該変化による影響は小さいと言える。
【0115】
面倣い状態の前後において、軸周りのモーメントの変化が小さくなるので、軸周りのモーメントに基づいて面倣い状態になる角度を正確に検出することは難しい。これに対して、Ｚ位置補正量は、面倣い状態において比較的先鋭なピークを示すので、面倣い状態になる角度をより正確に検出できる。
【0116】
Ｚ位置補正量を用いることで、ＴＣＰのＺｔ軸上の位置を正確に算出する必要がないので、面倣いの状態を探索するためのアルゴリズムを簡素化できる。
【0117】
なお、図１１に示す制御系に代えて、位置制御ループの外側にＺｔ軸についての力制御ループを設けたカスケード制御系を採用する場合には、面倣い状態を探索するための評価指標として、力制御の操作量となる位置補正量を用いてもよい。
【0118】
上述したいずれの評価指標を用いた場合であっても、評価指標は、面倣い状態において極値（極大値または極小値、あるいは、最大値または最小値）をとる。そのため、コントローラ１００は、評価指標が極値となる位置および姿勢を、第１ワーク５０の先端面５２（第１面）と受入面６４（第２面）とが平行（すなわち、面倣い）になる第１ワーク５０の位置および姿勢として決定する。
【0119】
（ｄ４：処理手順）
次に、Ｒｘ／Ｒｙ／Ｚ校正処理のより詳細な処理手順について説明する。
次に、Ｒｘ／Ｒｙ／Ｚ校正処理のより詳細な処理手順について説明する。
【0120】
図１４は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のより詳細な処理手順を示すフローチャートである。図１４に示す各ステップは、コントローラ１００のプロセッサ１０２がシステムプログラム１１２および／または制御プログラム１１４を実行することで実現されてもよい。
【0121】
Ｒｘ／Ｒｙ／Ｚ校正処理において、Ｘｔ軸周りの角度Ｒｘを変化させて面倣い状態となる角度（以下、「Ｒｘ校正値」とも称す。）を決定する処理と、Ｙｔ軸周りの角度Ｒｙを変化させて面倣い状態となる角度（以下、「Ｒｙ校正値」とも称す。）を決定する処理との組が予め定められた校正回数だけ繰り返される。
【0122】
アプローチ位置の校正処理を開始する初期位置および初期姿勢（以下、「ラフティーチング位置」とも称す。）が予め設定されているとする。ラフティーチング位置は、ユーザがティーチングペンダント２６を操作して設定してもよいし、第２ワーク６０の大きさおよび配置位置の情報に基づいて予め算出されていてもよい。
【0123】
図１４を参照して、コントローラ１００は、現在の校正回数を０に初期化する（ステップＳ１０１）。
【0124】
コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０をラフティーチング位置に移動させる（ステップＳ１０２）。移動完了後、コントローラ１００は、力覚センサ１８から出力されている力覚情報に基づいてオフセットを除去する（ステップＳ１０３）。コントローラ１００は、第１ワーク５０をラフティーチング位置に移動させた状態を初期状態とし、初期状態からの変化に応じて、Ｚｔ軸方向に発生する力Ｆｚを算出する。ステップＳ１０３の処理は、力覚情報のオフセットを除去する処理に相当する。
【0125】
コントローラ１００は、第１ワーク５０と第２ワーク６０とが非接触の状態で、力覚センサ１８から出力されている力覚情報を初期値として設定する。コントローラ１００は、力覚センサ１８のオフセットを除去した上で、第１ワーク５０の姿勢を検出する。このようなオフセットの除去は、探索処理を実行する毎に実行される。
【0126】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｘを開始角度に設定する（ステップＳ１０４）。例えば、現在のラフティーチング位置の角度Ｒｘに対して－１１［ｄｅｇ］となる角度を開始角度としてもよい。
【0127】
続いて、コントローラ１００は、ＴＣＰのＺｔ軸方向に発生する力Ｆｚが目標値と一致するように、ロボット１０に指令を与える（ステップＳ１０５）。この力制御は、Ｒｘ校正値が決定されるまで継続される。
【0128】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｘを開始角度から終了角度まで変化させる（ステップＳ１０６）とともに、Ｚ位置補正量の時間変化を収集する（ステップＳ１０７）。
【0129】
ＴＣＰの角度Ｒｘが終了角度まで変化した後、コントローラ１００は、収集したＺ位置補正量が極大値（あるいは、最大値；以下の処理において同様である）となる角度ＲｘをＲｘ校正値として決定する（ステップＳ１０８）。ラフティーチング位置の角度Ｒｘは、決定されたＲｘ校正値に更新される。
【0130】
なお、ＴＣＰの角度Ｒｘを終了角度まで変化させるのではなく、収集したＺ位置補正量が極大値となってから所定時間経過後に、Ｘｔ軸周りの回転を停止してもよい。
【0131】
ステップＳ１０３～Ｓ１０８の処理によって、Ｒｘ校正値が決定される。そして、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０を更新後のラフティーチング位置に移動させる（ステップＳ１０９）。続いて、Ｒｙ校正値を決定する処理が開始される。
【0132】
より具体的には、コントローラ１００は、力覚センサ１８から出力されている力覚情報に基づいてオフセットを除去する（ステップＳ１１０）。ステップＳ１１０の処理は、ステップＳ１０３の処理と同様に、力覚情報のオフセットを除去する処理に相当する。
【0133】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｙを開始角度に設定する（ステップＳ１１１）。例えば、現在のラフティーチング位置の角度Ｒｙに対して－１１［ｄｅｇ］となる角度を開始角度としてもよい。
【0134】
続いて、コントローラ１００は、ＴＣＰのＺｔ軸方向に発生する力Ｆｚが目標値と一致するように、ロボット１０に指令を与える（ステップＳ１１２）。この力制御は、Ｒｙ校正値が決定されるまで継続される。
【0135】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｙを開始角度から終了角度まで変化させる（ステップＳ１１３）とともに、Ｚ位置補正量の時間変化を収集する（ステップＳ１１４）。
【0136】
ＴＣＰの角度Ｒｙが終了角度まで変化した後、コントローラ１００は、収集したＺ位置補正量が極大値となる角度ＲｙをＲｙ校正値として決定する（ステップＳ１１５）。ラフティーチング位置の角度Ｒｙは、決定されたＲｙ校正値に更新される。
【0137】
なお、ＴＣＰの角度Ｒｙを終了角度まで変化させるのではなく、収集したＺ位置補正量が極大値となってから所定時間経過後に、Ｙｔ軸周りの回転を停止してもよい。
【0138】
ステップＳ１１０～Ｓ１１５の処理によって、Ｒｙ校正値が決定される。そして、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０を更新後のラフティーチング位置に移動させる（ステップＳ１１６）。
【0139】
コントローラ１００は、校正回数を１だけインクリメントし（ステップＳ１１７）、インクリメント後の校正回数が予め定められた校正回数に到達したか否かを判断する（ステップＳ１１８）。インクリメント後の校正回数が予め定められた校正回数に到達していなければ（ステップＳ１１８においてＮＯ）、ステップＳ１０３以下の処理が繰り返される。
【0140】
インクリメント後の校正回数が予め定められた校正回数に到達していれば（ステップＳ１１８においてＹＥＳ）、コントローラ１００は、第１ワーク５０が第２ワーク６０に接触する位置Ｚ（以下、「Ｚ校正値」とも称す。）を決定し（ステップＳ１１９）、Ｒｘ／Ｒｙ／Ｚ校正処理は終了する。
【0141】
（ｄ５：Ｒｘ／Ｒｙ／Ｚ校正処理の動作例）
次に、Ｒｘ／Ｒｙ／Ｚ校正処理の動作例について説明する。
次に、Ｒｘ／Ｒｙ／Ｚ校正処理の動作例について説明する。
【0142】
図１５は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＺ校正値を決定する際の動作例を説明するための図である。図１６は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＲｙ校正値を決定する際の動作例を説明するための図である。
【0143】
図１４の処理手順に示すように、角度Ｒｘおよび角度Ｒｙを同時に校正することはできないので、１つの角度ずつ順番に校正する。なお、角度Ｒｘおよび角度Ｒｙの校正を複数回繰り返すことで、センサ誤差を低減できる。
【0144】
図１５を参照して、コントローラ１００は、第１ワーク５０をラフティーチング位置に移動させて、力覚センサ１８から出力されている力覚情報のオフセットを除去する（図１５の（Ａ）：図１４のステップＳ１０２およびＳ１０３）。
【0145】
続いて、コントローラ１００は、ＴＣＰの角度Ｒｘを開始角度に設定する（図１５の（Ｂ）：図１４のステップＳ１０４）。例えば、開始角度は、ラフティーチング位置の角度Ｒｘに対して－１１［ｄｅｇ］としてもよい。
【0146】
続いて、コントローラ１００は、第１ワーク５０をＺｔ軸方向に移動（下降）させて、Ｚｔ軸方向に発生する力Ｆｚが目標値に到達すると移動を停止する（図１５の（Ｃ）：図１４のステップＳ１０５）。すなわち、コントローラ１００は、第１ワーク５０を第２ワーク６０に押し付ける。例えば、Ｚｔ軸方向の移動速度は１［ｍｍ／ｓ］としてもよいし、力Ｆｚの目標値は－３．５［Ｎ］としてもよい。
【0147】
続いて、コントローラ１００は、Ｚｔ軸方向の力制御を行いながら、ＴＣＰの角度Ｒｘを開始角度から終了角度まで変化させる（図１５の（Ｄ）：図１４のステップＳ１０６）。すなわち、コントローラ１００は、面倣い状態になる角度Ｒｘを探索する。例えば、角度Ｒｘの回転速度（変化速度）は０．４［ｄｅｇ／ｓ］としてもよい。Ｚ位置補正量が極大値となる角度ＲｘがＲｘ校正値となる。
【0148】
コントローラ１００は、角度Ｒｘが終了角度に到達すると、Ｘｔ軸周りの回転を停止させる（図１４のステップＳ１０６）。なお、Ｚ位置補正量が極大値となってから所定時間経過後に、Ｘｔ軸周りの回転を停止してもよい。
【0149】
【0150】
図１６を参照して、コントローラ１００は、第１ワーク５０を更新後のラフティーチング位置に移動させて、力覚センサ１８から出力されている力覚情報のオフセットを除去する（図１６の（Ａ）：図１４のステップＳ１０９およびＳ１１０）。
【0151】
続いて、コントローラ１００は、ＴＣＰの角度Ｒｙを開始角度に設定する（図１６の（Ｂ）：図１４のステップＳ１１１）。例えば、開始角度は、ラフティーチング位置の角度Ｒｙに対して－１１［ｄｅｇ］としてもよい。
【0152】
続いて、コントローラ１００は、第１ワーク５０をＺｔ軸方向に移動（下降）させて、Ｚｔ軸方向に発生する力Ｆｚが目標値に到達すると移動を停止する（図１６の（Ｃ）：図１４のステップＳ１１２）。すなわち、コントローラ１００は、第１ワーク５０を第２ワーク６０に押し付ける。例えば、Ｚｔ軸方向の移動速度は１［ｍｍ／ｓ］としてもよいし、力Ｆｚの目標値は－３．５［Ｎ］としてもよい。
【0153】
続いて、コントローラ１００は、Ｚｔ軸方向の力制御を行いながら、ＴＣＰの角度Ｒｘを開始角度から終了角度まで変化させる（図１６の（Ｄ）：図１４のステップＳ１１３）。すなわち、コントローラ１００は、面倣い状態になる角度Ｒｙを探索する。例えば、角度Ｒｙの回転速度（変化速度）は０．４［ｄｅｇ／ｓ］としてもよい。Ｚ位置補正量が極大値となる角度ＲｙがＲｙ校正値となる。
【0154】
コントローラ１００は、角度Ｒｙが終了角度に到達すると、Ｙｔ軸周りの回転を停止させる（図１４のステップＳ１１３）。なお、Ｚ位置補正量が極大値となってから所定時間経過後に、Ｙｔ軸周りの回転を停止してもよい。
【0155】
【0156】
（ｄ６：Ｚ位置補正量の時間変化）
次に、面倣い状態を探索するための評価指標の一例であるＺ位置補正量の時間変化について説明する。
次に、面倣い状態を探索するための評価指標の一例であるＺ位置補正量の時間変化について説明する。
【0157】
図１７は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＺ位置補正量の時間変化を説明するための図である。説明の便宜上、図１７においては、第１ワーク５０と第２ワーク６０との位置関係を強調して描いており、実際のワークの挙動とは必ずしも一致しない。
【0158】
図１７の（Ａ）を参照して、Ｒｘ／Ｒｙ／Ｚ校正処理の初期状態（ＴＣＰの角度Ｒｘが開始角度である状態）においては、第１ワーク５０の左端点が第２ワーク６０と接触している。この状態において、第１ワーク５０の回転により接触点が移動するため、押付力である力Ｆｚは目標値に到達していない。そのため、力Ｆｚを目標値に一致させるために、Ｚ位置補正量が増加する。
【0159】
その後、図１７の（Ｂ）に示すように、第１ワーク５０と第２ワーク６０とが面倣い状態になったタイミングでも、第１ワーク５０と第２ワーク６０との接触が不十分であり、力Ｆｚは目標値に到達していない。そのため、力Ｆｚを目標値に一致させるために、Ｚ位置補正量の増加も継続する。
【0160】
さらに、図１７の（Ｃ）に示すように、面倣い状態を超えて第１ワーク５０が傾くと、第１ワーク５０と第２ワーク６０とがさらに接触するため、力Ｆｚは目標値に近付く。そのため、Ｚ位置補正量の増加も緩やかになる。
【0161】
最終的に、図１７の（Ｄ）に示すように、第１ワーク５０の右端点が第２ワーク６０と接触している状態において、押付力は安定する。
【0162】
上述したように、力Ｆｚが目標値と一致するタイミングは、第１ワーク５０と第２ワーク６０とが面倣い状態になったタイミングに対して遅延し得る。但し、タイミングの遅延時間は、第１ワーク５０を時計回りに回転させた場合と、第１ワーク５０を反時計回りに回転させた場合との間で実質的に同じであると考えられる。
【0163】
そのため、第１ワーク５０を両方向にそれぞれ回転させて計測した結果を平均することで、タイミングの遅延時間を打ち消すことができる。このような遅延時間の影響を低減することで、校正結果の精度を高めることができる。そこで、以下に説明するように、低速探索を採用してもよい。
【0164】
（ｄ７：面倣い状態の探索）
図１４には、第１ワーク５０をＸｔ軸周りに回転させてＲｘ校正値を決定する処理と、第１ワーク５０をＹｔ軸周りに回転させてＲｙ校正値を決定する処理とを交互に実行する処理例を示すが、Ｒｘ校正値およびＲｙ校正値を決定する処理を高速探索および低速探索の２段階で行ってもよい。
図１４には、第１ワーク５０をＸｔ軸周りに回転させてＲｘ校正値を決定する処理と、第１ワーク５０をＹｔ軸周りに回転させてＲｙ校正値を決定する処理とを交互に実行する処理例を示すが、Ｒｘ校正値およびＲｙ校正値を決定する処理を高速探索および低速探索の２段階で行ってもよい。
【0165】
図１４に示す高速探索においては、第１ワーク５０をＸｔ軸周りの一方向に回転させる処理と、第１ワーク５０をＹｔ軸周りの一方向に回転させる処理とを繰り返し実行する。これに対して、図１８に示す低速探索においては、第１ワーク５０をＸｔ軸周りの両方向に回転させる処理と、第１ワーク５０をＹｔ軸周りの両方向に回転させる処理とを繰り返し実行する。すなわち、Ｘｔ軸周りおよびＹｔ軸周りの各々について、往復回転させることで、Ｒｘ校正値およびＲｙ校正値を決定する。
【0166】
低速探索において、第２ワーク６０に対する第１ワーク５０の姿勢を変化させる動作は、第１ワーク５０を第２ワーク６０に押し付ける方向（Ｚｔ軸方向）と直交する軸（Ｘｔ軸またはＹｔ軸）の周りに、第１ワーク５０を第１方向（例えば、時計回り）に回転させる動作（往路）と、第１ワーク５０を第１方向とは反対方向である第２方向（例えば、反時計周り）に回転させる動作とを含む。
【0167】
すなわち、低速検索においては、回転方向を異ならせて、アプローチ位置（Ｒｘ校正値および／またはＲｙ校正値）を更新する。より具体的には、第１ワーク５０を第１方向に回転させて面倣い状態になる姿勢を探索した後、第１ワーク５０を第２方向に回転させて面倣い状態になる姿勢を探索することで、アプローチ位置（Ｒｘ校正値および／またはＲｙ校正値）を更新する。
【0168】
なお、低速探索において、第１ワーク５０を第１方向（例えば、時計回り）に回転させる動作（往路）、および、第１ワーク５０を第１方向とは反対方向である第２方向（例えば、反時計周り）に回転させる動作は、複数回繰り返されてもよい。
【0169】
また、低速探索においては、第１ワーク５０を回転させる回転速度（変化速度）（絶対値）を高速探索の回転速度に比較して小さくしてもよい。すなわち、低速探索において、第２ワーク６０に対する第１ワーク５０の姿勢を変化させる動作は、第２ワーク６０に対する第１ワーク５０の姿勢を第１変化速度で変化させる動作と、第２ワーク６０に対する第１ワーク５０の姿勢を第１変化速度とは異なる第２変化速度で変化させる動作とを含んでもよい。なお、第２変化速度の絶対値は、第１変化速度の絶対値より小さくなるように設定されてもよい。
【0170】
このように、第１ワーク５０の姿勢を変化させる変化速度は、面倣い状態を探索する処理に応じて異ならせてもよい。典型的には、面倣い状態の探索回数が増加するほど、変化速度を低減させてもよい。
【0171】
図１８および図１９は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理に含まれる低速探索の処理手順を示すフローチャートである。図１８および図１９に示す処理は、図１４に示すステップＳ１１６の実行後、または、ステップＳ１１８においてＹＥＳの後に実行されてもよい。
【0172】
図１８を参照して、コントローラ１００は、力覚センサ１８から出力されている力覚情報に基づいてオフセットを除去する（ステップＳ１２０）。なお、第１ワーク５０は、更新後のラフティーチング位置に配置されている。
【0173】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｘを開始角度に設定する（ステップＳ１２１）。例えば、現在のラフティーチング位置の角度Ｒｘに対して－２．５［ｄｅｇ］となる角度を開始角度としてもよい。
【0174】
続いて、コントローラ１００は、ＴＣＰのＺｔ軸方向に発生する力Ｆｚが目標値と一致するように、ロボット１０に指令を与える（ステップＳ１２２）。この力制御は、Ｒｘ校正値が決定されるまで継続される。
【0175】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｘを所定の回転速度で回転させる（ステップＳ１２３）とともに、Ｚ位置補正量の時間変化を収集する（ステップＳ１２４）。例えば、角度Ｒｘの回転速度（変化速度）は０．２８［ｄｅｇ／ｓ］としてもよい。
【0176】
コントローラ１００は、Ｚ位置補正量が極大値となったか否かを判断する（ステップＳ１２５）。Ｚ位置補正量が極大値となれば（ステップＳ１２５においてＹＥＳ）、コントローラ１００は、Ｚ位置補正量が極大値となる角度ＲｘをＲｘ校正値（往路）として決定する（ステップＳ１２６）。
【0177】
コントローラ１００は、Ｚ位置補正量が極大値となってから所定時間経過後に、ロボット１０に指令を与えて、Ｘｔ軸周りの回転を停止させる（ステップＳ１２７）。
【0178】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｘを所定の回転速度で逆回転させる（ステップＳ１２８）とともに、Ｚ位置補正量の時間変化を収集する（ステップＳ１２９）。例えば、角度Ｒｘの回転速度（変化速度）は－０．２８［ｄｅｇ／ｓ］としてもよい。
【0179】
コントローラ１００は、Ｚ位置補正量が極大値となったか否かを判断する（ステップＳ１３０）。Ｚ位置補正量が極大値となれば（ステップＳ１３０においてＹＥＳ）、コントローラ１００は、Ｚ位置補正量が極大値となる角度ＲｘをＲｘ校正値（復路）として決定する（ステップＳ１３１）。
【0180】
コントローラ１００は、Ｚ位置補正量が極大値となってから所定時間経過後に、ロボット１０に指令を与えて、Ｘｔ軸周りの回転を停止させる（ステップＳ１３２）。
【0181】
コントローラ１００は、Ｒｘ校正値（往路）（ステップＳ１２６）、および、Ｒｘ校正値（復路）（ステップＳ１３１）に基づいて、Ｒｘ校正値を決定する（ステップＳ１３３）。例えば、Ｒｘ校正値（往路）とＲｘ校正値（復路）との平均をＲｘ校正値として決定してもよい。ラフティーチング位置の角度Ｒｘは、決定されたＲｘ校正値に更新される。
【0182】
このように、コントローラ１００は、第１ワーク５０を第１方向に回転することで決定された第１ワーク５０の第１姿勢と、第１ワーク５０を第２方向に回転することで決定された第１ワーク５０の第２姿勢との平均を、第１ワーク５０の先端面５２と第２ワーク６０の受入面６４とが平行になる第１ワーク５０の姿勢として決定する。すなわち、往復動作において、面倣い状態であるか否かを判断し、往路において決定した第１ワーク５０の姿勢と、復路において決定した第１ワーク５０の姿勢との平均を面倣い状態に対応する姿勢として決定してもよい。
【0183】
ステップＳ１２０～Ｓ１３３の処理によって、低速探索によるＲｘ校正値が決定される。そして、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０を更新後のラフティーチング位置に移動させる（ステップＳ１３４）。
【0184】
図１９を参照して、コントローラ１００は、力覚センサ１８から出力されている力覚情報に基づいてオフセットを除去する（ステップＳ１３５）。
【0185】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｙを開始角度に設定する（ステップＳ１３６）。例えば、現在のラフティーチング位置の角度Ｒｙに対して－２．５［ｄｅｇ］となる角度を開始角度としてもよい。
【0186】
続いて、コントローラ１００は、ＴＣＰのＺｔ軸方向に発生する力Ｆｚが目標値と一致するように、ロボット１０に指令を与える（ステップＳ１３７）。この力制御は、Ｒｙ校正値が決定されるまで継続される。
【0187】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｙを所定の回転速度で回転させる（ステップＳ１３８）とともに、Ｚ位置補正量の時間変化を収集する（ステップＳ１３９）。例えば、角度Ｒｙの回転速度（変化速度）は０．２８［ｄｅｇ／ｓ］としてもよい。
【0188】
コントローラ１００は、Ｚ位置補正量が極大値となったか否かを判断する（ステップＳ１４０）。Ｚ位置補正量が極大値となれば（ステップＳ１４０においてＹＥＳ）、コントローラ１００は、Ｚ位置補正量が極大値となる角度ＲｙをＲｙ校正値（往路）として決定する（ステップＳ１４１）。
【0189】
コントローラ１００は、Ｚ位置補正量が極大値となってから所定時間経過後に、ロボット１０に指令を与えて、Ｙｔ軸周りの回転を停止させる（ステップＳ１４２）。
【0190】
コントローラ１００は、ロボット１０に指令を与えて、ＴＣＰの角度Ｒｙを所定の回転速度で逆回転させる（ステップＳ１４３）とともに、Ｚ位置補正量の時間変化を収集する（ステップＳ１４４）。例えば、角度Ｒｙの回転速度（変化速度）は－０．２８［ｄｅｇ／ｓ］としてもよい。
【0191】
コントローラ１００は、Ｚ位置補正量が極大値となったか否かを判断する（ステップＳ１４５）。Ｚ位置補正量が極大値となれば（ステップＳ１４５においてＹＥＳ）、コントローラ１００は、Ｚ位置補正量が極大値となる角度ＲｙをＲｙ校正値（復路）として決定する（ステップＳ１４６）。
【0192】
コントローラ１００は、Ｚ位置補正量が極大値となってから所定時間経過後に、ロボット１０に指令を与えて、Ｙｔ軸周りの回転を停止させる（ステップＳ１４７）。
【0193】
コントローラ１００は、Ｒｙ校正値（往路）（ステップＳ１４１）、および、Ｒｘ校正値（復路）（ステップＳ１４６）に基づいて、Ｒｙ校正値を決定する（ステップＳ１４８）。例えば、Ｒｙ校正値（往路）とＲｙ校正値（復路）との平均をＲｙ校正値として決定してもよい。ラフティーチング位置の角度Ｒｙは、決定されたＲｙ校正値に更新される。
【0194】
このように、コントローラ１００は、第１ワーク５０を第１方向に回転することで決定された第１ワーク５０の第１姿勢と、第１ワーク５０を第２方向に回転することで決定された第１ワーク５０の第２姿勢との平均を、第１ワーク５０の先端面５２と第２ワーク６０の受入面６４とが平行になる第１ワーク５０の姿勢として決定する。
【0195】
ステップＳ１３５～Ｓ１４８の処理によって、低速探索によるＲｙ校正値が決定される。そして、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０を更新後のラフティーチング位置に移動させる（ステップＳ１４９）。
【0196】
コントローラ１００は、低速探索が予め定められた校正回数だけで実行されたか否かを判断する（ステップＳ１５０）。低速探索が予め定められた校正回数まで実行されていなければ（ステップＳ１５０においてＮＯ）、ステップＳ１２０以下の処理が繰り返される。
【0197】
低速探索が予め定められた校正回数まで実行されていれば（ステップＳ１５０においてＹＥＳ）、低速探索は終了する。
【0198】
上述したように、片道探索である高速探索と、往復探索である低速探索との２段階で面倣い状態に対応するアプローチ位置を決定してもよい。
【0199】
高速探索と低速探索とを組み合わせたアプローチ位置の校正処理において、コントローラ１００は、第１ワーク５０を第２ワーク６０に押し付ける方向（Ｚｔ軸方向）と直交する軸（Ｘｔ軸またはＹｔ軸）の周りに、第１ワーク５０を第１方向（例えば、時計回り）にのみ回転することで探索された結果に基づいて、第１ワーク５０の先端面５２と第２ワーク６０の受入面６４とが平行になる第１ワーク５０の姿勢を決定する。続いて、コントローラ１００は、第１ワーク５０を第１方向に回転することで探索された結果、および、第１ワーク５０を第２方向に回転することで探索された結果に基づいて、第１ワーク５０の先端面５２と第２ワーク６０の受入面６４とが平行になる第１ワーク５０の姿勢を更新する。
【0200】
なお、コントローラ１００は、第１ワーク５０を第１方向にのみ回転させる場合には、第１ワーク５０を第１変化速度で回転させ、第１ワーク５０を第１方向および第２方向に回転させる場合には、第１ワーク５０を第１変化速度より小さい第２変化速度で回転させる。
【0201】
（ｄ８：第１ワーク５０が第２ワーク６０に接触するＺ校正値の決定処理）
次に、第１ワーク５０が第２ワーク６０に接触するＺ校正値を決定する処理（図１４のステップＳ１１９）について説明する。
次に、第１ワーク５０が第２ワーク６０に接触するＺ校正値を決定する処理（図１４のステップＳ１１９）について説明する。
【0202】
Ｒｘ校正値を決定する処理、および、Ｒｙ校正値を決定する処理において、Ｒｘ校正値および／またはＲｙ校正値として角度Ｒｘおよび／または角度Ｒｙを採用したときの位置ＺをＺ校正値として取得しておいてもよい。なお、Ｚ校正値は、複数回取得されることになるので、平均などを採用してもよい。
【0203】
あるいは、Ｒｘ校正値およびＲｙ校正値を決定した上で、第１ワーク５０が第２ワーク６０に接触する位置Ｚ（Ｚ校正値）を改めて探索してもよい。
【0204】
図２０は、本実施の形態に従うＲｘ／Ｒｙ／Ｚ校正処理のＺ校正値を決定する処理を説明するための図である。図２０の（Ａ）を参照して、第１ワーク５０を更新後のラフティーチング位置に配置した状態で、力覚センサ１８から出力されている力覚情報に基づいてオフセットを除去する（オフセット除去）。そして、コントローラ１００は、第１ワーク５０をＺｔ軸方向に移動（下降）させる。
【0205】
図２０の（Ｂ）を参照して、Ｚｔ軸方向に発生する力Ｆｚが目標値に到達すると、当該タイミングの位置Ｚを第１ワーク５０が第２ワーク６０に接触するＺ校正値として決定する。決定されたＺ校正値は、第２ワーク６０の接触面（受入面６４）の位置を示す。Ｚ校正値は、ワーク面高さに相当する。その後、図２０の（Ｂ）を参照して、第１ワーク５０を予め定められたアプローチ距離だけＺｔ軸方向に移動（上昇）させる。
【0206】
以上のような処理によって、第１ワーク５０が第２ワーク６０に接触する位置Ｚ（Ｚ校正値）を決定できる。
【0207】
（ｄ９：変形例）
上述したＲｘ／Ｒｙ／Ｚ校正処理においては、Ｘｔ軸周りの角度Ｒｘ（Ｒｘ校正値）、Ｙｔ軸周りの角度Ｒｙ（Ｒｙ校正値）、および、Ｚｔ軸上の位置Ｚ（Ｚ校正値）を決定するが、一部の校正値のみを決定するようにしてもよい。
上述したＲｘ／Ｒｙ／Ｚ校正処理においては、Ｘｔ軸周りの角度Ｒｘ（Ｒｘ校正値）、Ｙｔ軸周りの角度Ｒｙ（Ｒｙ校正値）、および、Ｚｔ軸上の位置Ｚ（Ｚ校正値）を決定するが、一部の校正値のみを決定するようにしてもよい。
【0208】
例えば、Ｘｔ軸およびＹｔ軸のうち一方について位置関係が固定されているような場合には、可動できる軸についてのみ面倣い状態を探索することになる。そのため、Ｒｘ校正値を決定する処理（図１４のステップＳ１０３～Ｓ１０８）と、Ｒｙ校正値を決定する処理（図１４のステップＳ１１０～Ｓ１１５）とのうち一方のみを１または複数回実行するようにしてもよい。
【0209】
あるいは、Ｘｔ軸周りの角度ＲｘおよびＹｔ軸周りの角度Ｒｙのうち一方について、予め値が定められているような場合にも、Ｒｘ校正値を決定する処理と、Ｒｙ校正値を決定する処理とのうち一方のみを１または複数回実行するようにしてもよい。
【0210】
あるいは、Ｘｔ軸周りの角度ＲｘおよびＹｔ軸周りの角度Ｒｙのうち一方について、ユーザがティーチングペンダント２６を操作することで決定してもよい。
【0211】
また、第１ワーク５０が第２ワーク６０に接触する位置Ｚ（Ｚ校正値）については、予め定められた設計値を用いるようにしてもよい。この場合には、図１４のステップＳ１１９の処理などは省略できる。
【0212】
さらに、Ｒｘ／Ｒｙ／Ｚ校正処理によって、Ｘｔ軸周りの角度Ｒｘおよび／またはＹｔ軸周りの角度Ｒｙを決定した後に、ユーザがティーチングペンダント２６を操作して、第１ワーク５０と第２ワーク６０とを接触させることで、Ｚ校正値を決定してもよい。
【0213】
以上のように、Ｒｘ／Ｒｙ／Ｚ校正処理に含まれる処理の一部のみを実行することも本発明の技術的範囲に含まれる。
【0214】
（ｄ１０：小括）
アプローチ位置の決定において、角度Ｒｘおよび／または角度Ｒｙが正しく校正されないと、第１ワーク５０と第２ワーク６０とが十分に嵌合できないため、第１ワーク５０を第２ワーク６０に挿入できない可能性がある。また、角度Ｒｘおよび／または角度Ｒｙが適切に校正されないと、第１ワーク５０と第２ワーク６０との間で位置Ｘ、位置Ｙおよび角度Ｒｚが一致していないにもかかわらず、ロボット１０に生じるたわみにより、第１ワーク５０と第２ワーク６０とが嵌合されていると誤検出される可能性がある。
アプローチ位置の決定において、角度Ｒｘおよび／または角度Ｒｙが正しく校正されないと、第１ワーク５０と第２ワーク６０とが十分に嵌合できないため、第１ワーク５０を第２ワーク６０に挿入できない可能性がある。また、角度Ｒｘおよび／または角度Ｒｙが適切に校正されないと、第１ワーク５０と第２ワーク６０との間で位置Ｘ、位置Ｙおよび角度Ｒｚが一致していないにもかかわらず、ロボット１０に生じるたわみにより、第１ワーク５０と第２ワーク６０とが嵌合されていると誤検出される可能性がある。
【0215】
また、アプローチ位置の決定において、位置Ｚが第２ワーク６０の受入面６４が正しく校正されていないと、第１ワーク５０を挿入しすぎて、ワーク同士が干渉する可能性がある。クリアランスが狭い場合には、第１ワーク５０を第２ワーク６０に挿入できない可能性がある。
【0216】
Ｒｘ／Ｒｙ／Ｚ校正処理は、このようなワーク同士の干渉および嵌合状態の誤検出などの発生を抑制する。
【0217】
上述したように、Ｒｘ／Ｒｙ／Ｚ校正処理は、評価指標に基づいて面倣い状態を判断し、角度Ｒｘ、角度Ｒｙ、位置Ｚを校正する。Ｒｘ／Ｒｙ／Ｚ校正処理においては、第１ワーク５０を第２ワーク６０に力制御により押し付けた状態を維持しつつ、第１ワーク５０の姿勢を変化させる。そして、第１ワーク５０の押し付け方向の位置変化、または、第１ワーク５０と第２ワーク６０との相対距離に基づいて、第１ワーク５０と第２ワーク６０との面倣い状態を判断し、アプローチ位置の角度Ｒｘ、角度Ｒｙ、位置Ｚのうち少なくとも１つを決定する。
【0218】
評価指標としては、ロボット座標系における高さに加えて、力制御において算出されるＺ位置補正量を用いてもよい。評価指標としてＺ位置補正量を用いる場合には、Ｚ位置補正量が極大値（または、最大値）となるタイミングを探索する。Ｚ位置補正量を用いることで、Ｚｔ軸方向の高さを求めるための第２ワーク６０の姿勢を考慮した座標変換が不要になる。
【0219】
本実施の形態によれば、アプローチ位置の校正処理により、ラフティーチング位置からアプローチ位置を最適化することで、ティーチングに要する時間を低減できる。アプローチ位置を最適化することで、実運用において、第１ワーク５０と第２ワーク６０との配置誤差が生じても、探り動作に要する時間を短縮できる。
【0220】
面倣い状態の判断において、第１ワーク５０の位置情報を用いるので、力覚センサ１８のオフセットおよびヒステリシス、力制御の目標値（目標力）の大きさ、回転速度（変化速度）の大きさ、といった力の変動要因からの影響を受けにくく、校正精度を向上できる。
【0221】
＜Ｅ．Ｘ／Ｙ／Ｒｚ校正処理＞
次に、図５のＸ／Ｙ／Ｒｚ校正処理（ステップＳ２）の処理内容について説明する。
次に、図５のＸ／Ｙ／Ｒｚ校正処理（ステップＳ２）の処理内容について説明する。
【0222】
（ｅ１：基本的な考え方）
Ｘ／Ｙ／Ｒｚ校正処理においては、第２ワーク６０の嵌合孔６２の内面を規定する２つの方向ベクトルを決定し、２つの方向ベクトルから嵌合孔６２の角（かど）を示すＸｔＹｔ原点の位置（座標）を決定する。より具体的には、第１ワーク５０を嵌合孔６２の隣り合う２つの内面（以下、「第１面」および「第２面」とも称す。）に接触させた状態で、Ｘｔ－Ｙｔ座標上で第１ワーク５０を移動させることで、第１面に対応する第１方向ベクトルおよび第２面に対応する第２方向ベクトルを決定する。第１方向ベクトルおよび第２方向ベクトルの各々は、Ｘｔ軸およびＹｔ軸の成分を含む。
Ｘ／Ｙ／Ｒｚ校正処理においては、第２ワーク６０の嵌合孔６２の内面を規定する２つの方向ベクトルを決定し、２つの方向ベクトルから嵌合孔６２の角（かど）を示すＸｔＹｔ原点の位置（座標）を決定する。より具体的には、第１ワーク５０を嵌合孔６２の隣り合う２つの内面（以下、「第１面」および「第２面」とも称す。）に接触させた状態で、Ｘｔ－Ｙｔ座標上で第１ワーク５０を移動させることで、第１面に対応する第１方向ベクトルおよび第２面に対応する第２方向ベクトルを決定する。第１方向ベクトルおよび第２方向ベクトルの各々は、Ｘｔ軸およびＹｔ軸の成分を含む。
【0223】
このように、Ｘ／Ｙ／Ｒｚ校正処理においては、第１ワーク５０と第２ワーク６０とが接触する位置を基準にして、嵌合孔６２に対応する方向ベクトルを算出することで、位置Ｘ、位置Ｙおよび角度Ｒｚを校正する。第１ワーク５０を、第２ワーク６０の受入面６４に対して傾けた状態で、第２ワーク６０と非接触の状態を維持しつつ、嵌合孔６２に挿入する。続いて、第１ワーク５０を、受入面６４の１または複数の面に接触させることで、複数の位置計測点の座標を取得する。取得した位置計測点の座標に基づいて、各面に対応する方向ベクトルを算出する。
【0224】
図２１は、本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の第１方向ベクトルを決定する処理を説明するための図である。
【0225】
まず、第１ワーク５０は、第２ワーク６０から離れたＺｔ軸上の位置に、Ｒｘ／Ｒｙ／Ｚ校正処理によって決定されたＲｘ校正値およびＲｙ校正値の姿勢で配置される。この状態において、第１ワーク５０の先端面と第２ワーク６０の受入面６４とは実質的に平行となっている。続いて、第１ワーク５０は、Ｒｘ校正値から所定の傾斜角度だけ傾けられるとともに、Ｒｙ校正値から所定の傾斜角度だけ傾けられる。なお、Ｘｔ軸およびＹｔ軸についての傾斜角度は同一であってもよいし、異なっていてもよい。
【0226】
図２１の（Ａ）に示すように、第１ワーク５０は、傾いた状態で第２ワーク６０の嵌合孔６２に挿入される。すなわち、コントローラ１００は、ロボット１０に、把持機構２０が把持した第１ワーク５０を、第２ワーク６０と非接触の状態を維持しつつ、嵌合孔６２に挿入する動作を実行させる。なお、第１ワーク５０を傾いた状態で第２ワーク６０の嵌合孔６２に挿入する動作は、ユーザがティーチングペンダント２６を操作することで行ってもよい。
【0227】
第１ワーク５０は傾いているので、先端面の１つの角（かど）が第２ワーク６０の嵌合孔６２の第１面６８に接近する。コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第１面６８に押し付ける（例えば、３［Ｎ］）。
【0228】
続いて、図２１の（Ｂ）に示すように、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第１面６８に押し付けた状態で、第１ワーク５０を第１面６８に沿って移動させる。
【0229】
第１ワーク５０の１つの角（かど）が第２面６９に近付くと、第１ワーク５０は第２面６９からの反力を受ける。コントローラ１００は、第２面６９からの反力を受けると、当該反力を受けた位置より手前の位置計測点Ｌｏｃ１（例えば、１ｍｍ手前）を示す座標（Ｘｔ軸およびＹｔ軸の成分）を取得する。
【0230】
続いて、図２１の（Ｃ）に示すように、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第１面６８に押し付けた状態で、第１ワーク５０を第１面６８に沿って第２面６９とは反対方向に移動させる。
【0231】
第１ワーク５０の１つの角（かど）が第２面６９に対向する面に近付くと、第１ワーク５０は反力を受ける。コントローラ１００は、反力を受けると、当該反力を受けた位置より手前の位置計測点Ｌｏｃ２（例えば、１ｍｍ手前）を示す座標（Ｘｔ軸およびＹｔ軸の成分）を取得する。
【0232】
このように、第１ワーク５０の第１面６８に沿った移動中において、予め定められた値を超える反力を受けた位置から予め定められた距離だけ手前の位置計測点Ｌｏｃ１，Ｌｏｃ２の座標が取得される。
【0233】
続いて、図２１の（Ｄ）に示すように、コントローラ１００は、位置計測点Ｌｏｃ１および位置計測点Ｌｏｃ２に基づいて、第１方向ベクトルＶｅｃ１（＝Ｌｏｃ２－Ｌｏｃ１）を算出する。
【0234】
このように、コントローラ１００は、第１ワーク５０を嵌合孔６２の１つの内面である第１面６８に押し付けた状態で、第１ワーク５０を第１面６８に沿って移動させる動作を実行させる。そして、コントローラ１００は、第１ワーク５０の第１面６８に沿った移動の情報に基づいて第１面６８に対応する第１方向ベクトルＶｅｃ１を算出する。
【0235】
初期設定されていた嵌合孔６２の第１面６８の方向と、算出された第１方向ベクトルＶｅｃ１が示す方向との角度差で角度Ｒｚを補正し、補正後の角度ＲｚをＲｚ校正値として決定する。このように、コントローラ１００は、第１方向ベクトルＶｅｃ１に基づいて、Ｒｚ校正値を決定する。Ｒｚ校正値は、第１ワーク５０を第２ワーク６０の嵌合孔６２に嵌合させるための基準姿勢を示す。なお、Ｒｚ校正値は、後述する第２方向ベクトルＶｅｃ２に基づいて決定することもできる。
【0236】
なお、角度Ｒｚの基準方向と第１方向ベクトルＶｅｃ１が示す方向とがなす角度をＲｚ校正値として決定してもよい。
【0237】
図２２は、本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の第２方向ベクトルを決定する処理を説明するための図である。
【0238】
次に、図２２の（Ａ）に示すように、第１ワーク５０は、先端面の１つの角（かど）が第２ワーク６０の嵌合孔６２の第２面６９に接近した位置に配置される。
【0239】
コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第２面６９に押し付ける（例えば、３［Ｎ］）。
【0240】
続いて、図２２の（Ｂ）に示すように、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第２面６９に押し付けた状態で、第１ワーク５０を第２面６９に沿って移動させる。
【0241】
第１ワーク５０の１つの角（かど）が第１面６８に対向する面に近付くと、第１ワーク５０は第１面６８に対向する面からの反力を受ける。コントローラ１００は、第１面６８に対向する面からの反力を受けると、当該反力を受けた位置より手前の位置計測点Ｌｏｃ３（例えば、１ｍｍ手前）を示す座標（Ｘｔ軸およびＹｔ軸の成分）を取得する。
【0242】
続いて、図２２の（Ｃ）に示すように、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第２面６９に押し付けた状態で、第１ワーク５０を第２面６９に沿って第１面６８の方向に移動させる。
【0243】
第１ワーク５０の１つの角（かど）が第１面６８に近付くと、第１ワーク５０は反力を受ける。コントローラ１００は、反力を受けると、当該反力を受けた位置より手前の位置計測点Ｌｏｃ４（例えば、１ｍｍ手前）を示す座標（Ｘｔ軸およびＹｔ軸の成分）を取得する。
【0244】
このように、第１ワーク５０の第２面６９に沿った移動中において、予め定められた値を超える反力を受けた位置から予め定められた距離だけ手前の位置計測点Ｌｏｃ３，Ｌｏｃ４の座標が取得される。
【0245】
続いて、図２２の（Ｄ）に示すように、コントローラ１００は、位置計測点Ｌｏｃ３および位置計測点Ｌｏｃ４に基づいて、第２方向ベクトルＶｅｃ２（＝Ｌｏｃ４－Ｌｏｃ３）を算出する。
【0246】
このように、コントローラ１００は、第１ワーク５０を嵌合孔６２の第１面６８の隣にある内面である第２面６９に押し付けた状態で、第１ワーク５０を第２面６９に沿って移動させる動作を実行させる。そして、コントローラ１００は、第１ワーク５０の第２面６９に沿った移動の情報に基づいて第２面６９に対応する第２方向ベクトルＶｅｃ２を算出する。
【0247】
図２１および図２２に示す処理によって、第１方向ベクトルＶｅｃ１および第２方向ベクトルＶｅｃ２を算出できる。すなわち、コントローラ１００は、第１方向ベクトルＶｅｃ１および第２方向ベクトルＶｅｃ２に基づいて、ＸｔＹｔ原点を算出する。ＸｔＹｔ原点は、嵌合孔６２の角（かど）に対応する座標であり、第１ワーク５０を第２ワーク６０の嵌合孔６２に嵌合させるための位置基準を示す。
【0248】
図２３は、本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理のＸｔＹｔ原点を決定する処理を説明するための図である。図２３を参照して、コントローラ１００は、Ｘｔ－Ｙｔ座標上で、第１方向ベクトルＶｅｃ１と第２方向ベクトルＶｅｃ２との交点を算出し、算出された交点をＸｔＹｔ原点として決定する。
【0249】
すなわち、コントローラ１００は、第１方向ベクトルＶｅｃ１と第２方向ベクトルＶｅｃ２との交点を示すＸｔ軸上の位置ＸおよびＹｔ軸上の位置Ｙを、原点を示す校正値として決定する。第１方向ベクトルＶｅｃ１を含む面、および、第２方向ベクトルＶｅｃ２を含む面の交線上にＸｔＹｔ原点を決定してもよい。以下では、ＸｔＹｔ原点の位置Ｘを「Ｘ校正値」とも称し、ＸｔＹｔ原点の位置Ｙを「Ｙ校正値」とも称す。
【0250】
上述したように、コントローラ１００は、第１方向ベクトルＶｅｃ１および第２方向ベクトルＶｅｃ２の少なくとも一方に基づいて、第１ワーク５０を嵌合孔６２に嵌合させるための第１ワーク５０の位置および姿勢の少なくとも一方を決定する。
【0251】
より具体的には、コントローラ１００は、第１方向ベクトルＶｅｃ１および第２方向ベクトルＶｅｃ２のうち少なくとも１つに基づいて、嵌合孔の角度であるＲｚ校正値を決定する。なお、第１方向ベクトルＶｅｃ１に基づいてＲｚ校正値（暫定値）を決定するとともに、第２方向ベクトルＶｅｃ２に基づいてＲｚ校正値（暫定値）を決定した上で、決定された複数のＲｚ校正値（暫定値）の平均をＲｚ校正値として最終的に出力してもよい。
【0252】
また、コントローラ１００は、第１方向ベクトルＶｅｃ１および第２方向ベクトルＶｅｃ２に基づいて、嵌合孔６２の位置基準となるＸｔＹｔ原点を決定してもよい。
【0253】
（ｅ２：処理手順）
次に、Ｘ／Ｙ／Ｒｚ校正処理のより詳細な処理手順について説明する。
次に、Ｘ／Ｙ／Ｒｚ校正処理のより詳細な処理手順について説明する。
【0254】
図２４および図２５は、本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理のより詳細な処理手順を示すフローチャートである。図２４および図２５に示す各ステップは、コントローラ１００のプロセッサ１０２がシステムプログラム１１２および／または制御プログラム１１４を実行することで実現されてもよい。
【0255】
図２４を参照して、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０を、第２ワーク６０の上側にＲｘ校正値およびＲｙ校正値の姿勢で配置する（ステップＳ２０１）。続いて、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０を、Ｒｘ校正値から所定の傾斜角度だけ傾けるとともに、Ｒｙ校正値から所定の傾斜角度だけ傾ける（ステップＳ２０２）。さらに、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０を、第２ワーク６０の嵌合孔６２に挿入する（ステップＳ２０３）。
【0256】
続いて、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第１面６８に押し付け（ステップＳ２０４）、第１ワーク５０を第１面６８に沿って第２面６９の方向に移動させる（ステップＳ２０５）。コントローラ１００は、予め定められた値を超える反力を受けたか否かを判断する（ステップＳ２０６）。
【0257】
予め定められた値を超える反力を受けると（ステップＳ２０６においてＹＥＳ）、コントローラ１００は、第１ワーク５０を第１面６８に沿って第２面６９とは反対方向に所定距離だけ戻し（ステップＳ２０７）、当該戻った位置を第１位置計測点として、第１位置計測点の座標を取得する（ステップＳ２０８）。なお、第１位置計測点を決定する場合には、第１ワーク５０の１つの角（かど）を嵌合孔６２の第１面６８に押し付ける力を所定値（例えば、１［Ｎ］）まで低減してもよい（詳細は後述する。他の位置計測点を決定する場合も同様である）。
【0258】
コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第１面６８に押し付けた状態で、第１ワーク５０を第１面６８に沿って第２面６９とは反対方向に移動させる（ステップＳ２０９）。コントローラ１００は、予め定められた値を超える反力を受けたか否かを判断する（ステップＳ２１０）。
【0259】
予め定められた値を超える反力を受けると（ステップＳ２１０においてＹＥＳ）、コントローラ１００は、第１ワーク５０を第１面６８に沿って第２面６９の方向に所定距離だけ戻し（ステップＳ２１１）、当該戻った位置を第２位置計測点として、第２位置計測点の座標を取得する（ステップＳ２１２）。
【0260】
コントローラ１００は、第１位置計測点の位置および第２位置計測点の位置に基づいて、第１方向ベクトルＶｅｃ１を算出する（ステップＳ２１３）。
【0261】
コントローラ１００は、第１方向ベクトルＶｅｃ１に基づいてＲｚ校正値を決定する（ステップＳ２１４）。すなわち、コントローラ１００は、初期設定されていた嵌合孔６２の第１面６８の方向と、算出された第１方向ベクトルＶｅｃ１が示す方向との角度差で角度Ｒｚを補正し、補正後の角度ＲｚをＲｚ校正値として決定する。
【0262】
図２５を参照して、続いて、コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の別の１つの角（かど）を嵌合孔６２の第２面６９に押し付け（ステップＳ２１５）、第１ワーク５０を第２面６９に沿って第１面６８とは反対方向に移動させる（ステップＳ２１６）。コントローラ１００は、予め定められた値を超える反力を受けたか否かを判断する（ステップＳ２１７）。
【0263】
予め定められた値を超える反力を受けると（ステップＳ２１７においてＹＥＳ）、コントローラ１００は、第１ワーク５０を第２面６９に沿って第１面６８の方向に所定距離だけ戻し（ステップＳ２１８）、当該戻った位置を第３位置計測点として、第３位置計測点の座標を取得する（ステップＳ２１９）。
【0264】
コントローラ１００は、ロボット１０に指令を与えて、第１ワーク５０の１つの角（かど）を嵌合孔６２の第２面６９に押し付けた状態で、第１ワーク５０を第２面６９に沿って第１面６８の方向に移動させる（ステップＳ２２０）。コントローラ１００は、予め定められた値を超える反力を受けたか否かを判断する（ステップＳ２２１）。
【0265】
予め定められた値を超える反力を受けると（ステップＳ２２１においてＹＥＳ）、コントローラ１００は、第１ワーク５０を第２面６９に沿って第１面６８とは反対方向に所定距離だけ戻し（ステップＳ２２２）、当該戻った位置を第４位置計測点として、第４位置計測点の座標を取得する（ステップＳ２２３）。
【0266】
コントローラ１００は、第３位置計測点の位置および第４位置計測点の位置に基づいて、第２方向ベクトルＶｅｃ２を算出する（ステップＳ２２４）。
【0267】
コントローラ１００は、第１方向ベクトルＶｅｃ１と第２方向ベクトルＶｅｃ２とに基づいて、ＸｔＹｔ原点を算出し（ステップＳ２２５）、算出したＸｔＹｔ原点からＸ校正値およびＹ校正値を決定する（ステップＳ２２６）。そして、処理は終了する。
（ｅ３：位置計測の高精度化）
次に、方向ベクトルを算出するための位置をより高精度に計測する変形例について説明する。
（ｅ３：位置計測の高精度化）
次に、方向ベクトルを算出するための位置をより高精度に計測する変形例について説明する。
【0268】
図２６は、本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の方向ベクトルを算出するための位置計測方法の一例を説明するための図である。
【0269】
図２６を参照して、第１ワーク５０の１つの角（かど）を嵌合孔６２のいずれかの面に押し付ける際に、力制御の目標値が所定値（例えば、５［Ｎ］）以上であると、把持機構２０（図１参照）などが数１００μｍオーダで弾性変形し得る。このような弾性変形によって、計測される位置の精度が低下し得る。
【0270】
例えば、位置を取得する位置計測点において、力制御の目標値を弾性変形が生じない目標値（例えば、１［Ｎ］程度）に低下させてもよい。なお、目標値は、力覚センサ１８からの力覚情報により検出可能な力の最小値に対して、所定のマージンをもつ値とすることが好ましい。力制御の目標値を変更することで、位置の計測精度を向上できる。
【0271】
このように、コントローラ１００は、ロボット１０に、位置計測点Ｌｏｃ１，Ｌｏｃ２において、第１面６８に対する第１ワーク５０の押付力を低減する動作を実行させるとともに、位置計測点Ｌｏｃ３，Ｌｏｃ４において、第２面６９に対する第１ワーク５０の押付力を低減する動作とを実行させる。
【0272】
このような動作によって、ロボット１０に生じ得る弾性変形に起因する誤差を低減でき、方向ベクトルの算出精度を向上できる。
【0273】
また、ある程度の弾性変形が生じている場合には、力制御を無効化した上で、第１ワーク５０の角（かど）を押し付けていた面から離れる方向に移動させてもよい。この場合には、力覚センサ１８からの力覚情報により検出される反力が予め定められた値に低減するまで第１ワーク５０を移動させてもよい。
【0274】
このとき、コントローラ１００は、ロボット１０に、位置計測点Ｌｏｃ１，Ｌｏｃ２において、第１ワーク５０の第１面６８への押付を停止するとともに、第１ワーク５０が第１面６８に接触していることにより生じる反力が予め定められた値以下となるように、第１ワーク５０を第２面６９から遠ざける動作を実行させる。同様に、コントローラ１００は、ロボット１０に、位置計測点Ｌｏｃ３，Ｌｏｃ４において、第１ワーク５０の第２面６９への押付を停止するとともに、第１ワーク５０が第２面６９に接触していることにより生じる反力が予め定められた値以下となるように、第１ワーク５０を第２面６９から遠ざける動作を実行させる。
【0275】
上述したように、嵌合孔６２のいずれかの面に対する第１ワーク５０の角（かど）の位置を調整することで、位置の計測精度を向上できる。これによって、ロボット１０に生じ得る弾性変形に起因する誤差を低減でき、方向ベクトルの算出精度を向上できる。
【0276】
（ｅ４：方向ベクトルの算出精度向上）
上述の説明では、２つの位置から方向ベクトルを算出する処理を例示したが、これに限らず、より多くの位置を計測して方向ベクトルの算出精度を向上させてもよい。
上述の説明では、２つの位置から方向ベクトルを算出する処理を例示したが、これに限らず、より多くの位置を計測して方向ベクトルの算出精度を向上させてもよい。
【0277】
図２７は、本実施の形態に従うＸ／Ｙ／Ｒｚ校正処理の方向ベクトルを算出処理の変形例を説明するための図である。
【0278】
図２７を参照して、例えば、上述した位置の計測を複数回繰り返すことで、同一の面に対して、複数の位置を取得できる。あるいは、第１ワーク５０の１つの角（かど）を嵌合孔６２のいずれかの面に押し付けた状態で、当該角（かど）を面に沿って移動させる際に、軌跡を示す位置を順次格納することで、複数の位置を取得できる。
【0279】
取得された複数の位置を最小二乗法などの統計的手法で線形近似することで、方向ベクトルを算出してもよい。なお、統計的手法を適用する際に、取得された複数の位置に外れ値が含まれている場合には、当該外れ値を除外した上で、方向ベクトルを算出してもよい。なお、方向ベクトルが算出できれば、どのような統計的手法を用いてもよい。
【0280】
このように、コントローラ１００は、第１ワーク５０の第１面６８に沿った移動中に取得された複数の位置計測点の座標に基づいて補間することで、第１方向ベクトルＶｅｃ１を算出する。同様に、コントローラ１００は、第１ワーク５０の第２面６９に沿った移動中に取得された複数の位置計測点の座標に基づいて補間することで、第２方向ベクトルＶｅｃ２を算出する。
【0281】
複数の位置計測点の座標を用いた統計的な処理によって、方向ベクトルをより正確に算出できる。
【0282】
（ｅ５：変形例）
上述したＸ／Ｙ／Ｒｚ校正処理においては、ＸｔＹｔ原点の位置Ｘ（Ｘ校正値）、ＸｔＹｔ原点の位置Ｙ（Ｙ校正値）、Ｒｚ校正値を決定するが、一部の校正値のみを決定するようにしてもよい。例えば、Ｒｚ校正値については、予め定められた設計値を採用してもよい。この場合、Ｘ校正値およびＹ校正値のみを決定すればよい。また、Ｒｚ校正値については、ユーザがティーチングペンダント２６を操作することで決定してもよい。
上述したＸ／Ｙ／Ｒｚ校正処理においては、ＸｔＹｔ原点の位置Ｘ（Ｘ校正値）、ＸｔＹｔ原点の位置Ｙ（Ｙ校正値）、Ｒｚ校正値を決定するが、一部の校正値のみを決定するようにしてもよい。例えば、Ｒｚ校正値については、予め定められた設計値を採用してもよい。この場合、Ｘ校正値およびＹ校正値のみを決定すればよい。また、Ｒｚ校正値については、ユーザがティーチングペンダント２６を操作することで決定してもよい。
【0283】
また、第２ワーク６０の一つの辺については位置関係が固定されているような場合には、他の辺についてのみ方向ベクトルを算出するようにしてもよい。この場合、２つの方向ベクトルを用いて、ＸｔＹｔ原点を算出することになるが、１つの方向ベクトルについては予め算出された情報を用いることができる。
【0284】
以上のように、Ｘ／Ｙ／Ｒｚ校正処理に含まれる処理の一部のみを実行することも本発明の技術的範囲に含まれる。
【0285】
（ｅ６：小括）
アプローチ位置の決定において、総当たりで探索すれば、適切な位置を決定できるが、探索に要する時間が過大となり、実運用には適さない。また、第１ワーク５０の力およびモーメントを制御して探索する方法は、反力によるモーメント誤差、力覚センサ１８のオフセットおよびヒステリシスなどに起因する誤差などが生じるため、校正精度が低下し得る。
アプローチ位置の決定において、総当たりで探索すれば、適切な位置を決定できるが、探索に要する時間が過大となり、実運用には適さない。また、第１ワーク５０の力およびモーメントを制御して探索する方法は、反力によるモーメント誤差、力覚センサ１８のオフセットおよびヒステリシスなどに起因する誤差などが生じるため、校正精度が低下し得る。
【0286】
Ｘ／Ｙ／Ｒｚ校正処理においては、１つの面について、複数の位置計測点の位置をそれぞれ取得し、取得した複数の位置を補間することで当該面についての方向ベクトルを算出する。算出された方向ベクトルに基づいて、Ｚｔ軸周りの角度Ｒｚを校正する。
【0287】
角度Ｒｚの校正にモーメント情報を用いる必要がないので、反力によるモーメント誤差、力覚センサ１８のオフセットおよびヒステリシスなどの影響を受けにくく、校正精度を向上できる。
【0288】
＜Ｆ．その他の実施の形態＞
アプローチ位置の校正処理として、Ｒｘ／Ｒｙ／Ｚ校正処理（ステップＳ１）と、Ｘ／Ｙ／Ｒｚ校正処理（ステップＳ２）とを実行する処理例を説明したが、Ｒｘ／Ｒｙ／Ｚ校正処理のみ、または、Ｘ／Ｙ／Ｒｚ校正処理のみを実行するようにしてもよい。
アプローチ位置の校正処理として、Ｒｘ／Ｒｙ／Ｚ校正処理（ステップＳ１）と、Ｘ／Ｙ／Ｒｚ校正処理（ステップＳ２）とを実行する処理例を説明したが、Ｒｘ／Ｒｙ／Ｚ校正処理のみ、または、Ｘ／Ｙ／Ｒｚ校正処理のみを実行するようにしてもよい。
【0289】
例えば、Ｒｘ／Ｒｙ／Ｚ校正処理によって、第１ワーク５０と第２ワーク６０とを面倣い状態となるためのＲｘ校正値およびＲｙ校正値を決定した後、ユーザがティーチングペンダント２６を操作して、その他の値を決定してもよい。
【0290】
また、Ｒｘ校正値およびＲｙ校正値をいずれも初期値（例えば、０°）に設定した上で、Ｘ／Ｙ／Ｒｚ校正処理を実行して、ＸｔＹｔ原点を決定してもよい。
【0291】
さらに、上述したように、Ｒｘ／Ｒｙ／Ｚ校正処理によって一部の校正値のみを決定してもよく、Ｘ／Ｙ／Ｒｚ校正処理によって一部の校正値のみを決定してもよい。
【0292】
このように、本発明の技術的範囲は、Ｘ校正値、Ｙ校正値、Ｚ校正値、Ｒｘ校正値、Ｒｙ校正値、および、Ｒｚ校正値のうち少なくとも１つを決定する処理を含む。
【0293】
＜Ｇ．付記＞
上述したような本実施の形態は、以下のような技術思想を含む。
上述したような本実施の形態は、以下のような技術思想を含む。
【0294】
［構成１］
ロボット（１０）を用いて第１ワーク（５０）と第２ワーク（６０）ととを嵌合させるコントローラ（１００）であって、前記ロボットは、前記第１ワークを把持するための把持機構（２０）と力覚センサ（１８）とを有しており、前記コントローラは、前記ロボットに、
前記第１ワークを予め定められた力で前記第２ワークに押し付ける動作（Ｓ１０５；Ｓ１１２；Ｓ１２２；Ｓ１３７）と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させる動作（Ｓ１０６；Ｓ１１３；Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）とを実行させ、
前記コントローラは、前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面（５２）と、前記第２ワークの前記第１ワークと接触する第２面（６４）とが平行になる前記第１ワークの姿勢を探索する（Ｓ１０８；Ｓ１１５；Ｓ１２５，Ｓ１２６；Ｓ１３０，Ｓ１３１；Ｓ１４０，Ｓ１４１；Ｓ１４５，Ｓ１４６）、コントローラ。
ロボット（１０）を用いて第１ワーク（５０）と第２ワーク（６０）ととを嵌合させるコントローラ（１００）であって、前記ロボットは、前記第１ワークを把持するための把持機構（２０）と力覚センサ（１８）とを有しており、前記コントローラは、前記ロボットに、
前記第１ワークを予め定められた力で前記第２ワークに押し付ける動作（Ｓ１０５；Ｓ１１２；Ｓ１２２；Ｓ１３７）と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させる動作（Ｓ１０６；Ｓ１１３；Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）とを実行させ、
前記コントローラは、前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面（５２）と、前記第２ワークの前記第１ワークと接触する第２面（６４）とが平行になる前記第１ワークの姿勢を探索する（Ｓ１０８；Ｓ１１５；Ｓ１２５，Ｓ１２６；Ｓ１３０，Ｓ１３１；Ｓ１４０，Ｓ１４１；Ｓ１４５，Ｓ１４６）、コントローラ。
【0295】
［構成２］
前記評価指標は、前記第２ワークに予め設定された面（６６）から、前記ロボットのアーム先端を基準とした座標系において設定された計測ポイントまでの距離である、構成１に記載のコントローラ。
前記評価指標は、前記第２ワークに予め設定された面（６６）から、前記ロボットのアーム先端を基準とした座標系において設定された計測ポイントまでの距離である、構成１に記載のコントローラ。
【0296】
［構成３］
前記計測ポイントは、前記計測ポイントを前記第１面を含む面へ投影したときに、前記第１ワークの姿勢を変化させる動作において、前記第２面と接触する前記第１面の２つの最外辺によって規定される範囲内に、当該投影位置が存在するように設定される、構成２に記載のコントローラ。
前記計測ポイントは、前記計測ポイントを前記第１面を含む面へ投影したときに、前記第１ワークの姿勢を変化させる動作において、前記第２面と接触する前記第１面の２つの最外辺によって規定される範囲内に、当該投影位置が存在するように設定される、構成２に記載のコントローラ。
【0297】
［構成４］
前記評価指標は、前記第１ワークを予め定められた力で前記第２ワークに押し付けるための、前記ロボットに与えられる指令についての補正量である、構成１に記載のコントローラ。
前記評価指標は、前記第１ワークを予め定められた力で前記第２ワークに押し付けるための、前記ロボットに与えられる指令についての補正量である、構成１に記載のコントローラ。
【0298】
［構成５］
前記コントローラは、前記評価指標が極値となる姿勢を、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢として決定する、構成１～４のいずれか１項に記載のコントローラ。
前記コントローラは、前記評価指標が極値となる姿勢を、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢として決定する、構成１～４のいずれか１項に記載のコントローラ。
【0299】
［構成６］
前記ロボットの位置および姿勢を制御するための基準ポイントは、前記第１ワークの前記第１面の内側に設定される、構成１～５のいずれか１項に記載のコントローラ。
前記ロボットの位置および姿勢を制御するための基準ポイントは、前記第１ワークの前記第１面の内側に設定される、構成１～５のいずれか１項に記載のコントローラ。
【0300】
［構成７］
前記第２ワークに対する前記第１ワークの姿勢を変化させる動作は、
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向に回転させる動作（Ｓ１２３；Ｓ１３８）と、
前記第１ワークを前記第１方向とは反対方向である第２方向に回転させる動作（Ｓ１２８；Ｓ１４３）とを含む、構成１～６のいずれか１項に記載のコントローラ。
前記第２ワークに対する前記第１ワークの姿勢を変化させる動作は、
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向に回転させる動作（Ｓ１２３；Ｓ１３８）と、
前記第１ワークを前記第１方向とは反対方向である第２方向に回転させる動作（Ｓ１２８；Ｓ１４３）とを含む、構成１～６のいずれか１項に記載のコントローラ。
【0301】
［構成８］
前記コントローラは、前記第１ワークを前記第１方向に回転することで決定された前記第１ワークの第１姿勢と、前記第１ワークを前記第２方向に回転することで決定された前記第１ワークの第２姿勢との平均を、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢として決定する、構成７に記載のコントローラ。
前記コントローラは、前記第１ワークを前記第１方向に回転することで決定された前記第１ワークの第１姿勢と、前記第１ワークを前記第２方向に回転することで決定された前記第１ワークの第２姿勢との平均を、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢として決定する、構成７に記載のコントローラ。
【0302】
［構成９］
前記第２ワークに対する前記第１ワークの姿勢を変化させる動作は、
前記第２ワークに対する前記第１ワークの姿勢を第１変化速度で変化させる動作（Ｓ１０６；Ｓ１１３）と、
前記第２ワークに対する前記第１ワークの姿勢を前記第１変化速度とは異なる第２変化速度で変化させる動作（Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）とを含む、構成１～８のいずれか１項に記載のコントローラ。
前記第２ワークに対する前記第１ワークの姿勢を変化させる動作は、
前記第２ワークに対する前記第１ワークの姿勢を第１変化速度で変化させる動作（Ｓ１０６；Ｓ１１３）と、
前記第２ワークに対する前記第１ワークの姿勢を前記第１変化速度とは異なる第２変化速度で変化させる動作（Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）とを含む、構成１～８のいずれか１項に記載のコントローラ。
【0303】
［構成１０］
前記第２変化速度は、前記第１変化速度より小さい、構成９に記載のコントローラ。
前記第２変化速度は、前記第１変化速度より小さい、構成９に記載のコントローラ。
【0304】
［構成１１］
前記コントローラは、
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向にのみ回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を決定し（Ｓ１０６，Ｓ１０８；Ｓ１１３，Ｓ１１５）、
続いて、前記第１ワークを前記第１方向に回転することで探索された結果、および、前記第１ワークを前記第１方向とは反対方向である第２方向に回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を更新する（Ｓ１２３，Ｓ１２６，Ｓ１２８，Ｓ１３１，Ｓ１３３；Ｓ１３８，Ｓ１４１，Ｓ１４３，Ｓ１４６，Ｓ１４８）、構成１～６のいずれか１項に記載のコントローラ。
前記コントローラは、
前記第１ワークを前記第２ワークに押し付ける方向と直交する軸の周りに、前記第１ワークを第１方向にのみ回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を決定し（Ｓ１０６，Ｓ１０８；Ｓ１１３，Ｓ１１５）、
続いて、前記第１ワークを前記第１方向に回転することで探索された結果、および、前記第１ワークを前記第１方向とは反対方向である第２方向に回転することで探索された結果に基づいて、前記第１面と前記第２面とが平行になる前記第１ワークの姿勢を更新する（Ｓ１２３，Ｓ１２６，Ｓ１２８，Ｓ１３１，Ｓ１３３；Ｓ１３８，Ｓ１４１，Ｓ１４３，Ｓ１４６，Ｓ１４８）、構成１～６のいずれか１項に記載のコントローラ。
【0305】
［構成１２］
前記第１ワークを第１方向にのみ回転させる場合には、前記第１ワークを第１変化速度で回転させ、
前記第１ワークを前記第１方向および前記第２方向に回転させる場合には、前記第１ワークを前記第１変化速度より小さい第２変化速度で回転させる、構成１２に記載のコントローラ。
前記第１ワークを第１方向にのみ回転させる場合には、前記第１ワークを第１変化速度で回転させ、
前記第１ワークを前記第１方向および前記第２方向に回転させる場合には、前記第１ワークを前記第１変化速度より小さい第２変化速度で回転させる、構成１２に記載のコントローラ。
【0306】
［構成１３］
前記コントローラは、前記第１ワークと前記第２ワークとが非接触の状態で、前記力覚センサから出力されている力覚情報に基づいてオフセットを除去する（Ｓ１０３；Ｓ１１０；Ｓ１２０；Ｓ１３５）、構成１～１２のいずれか１項に記載のコントローラ。
前記コントローラは、前記第１ワークと前記第２ワークとが非接触の状態で、前記力覚センサから出力されている力覚情報に基づいてオフセットを除去する（Ｓ１０３；Ｓ１１０；Ｓ１２０；Ｓ１３５）、構成１～１２のいずれか１項に記載のコントローラ。
【0307】
［構成１４］
ロボット（１０）を用いて第１ワーク（５０）と第２ワーク（６０）とを嵌合させるための制御方法であって、前記第１ワークは前記ロボット（１０）の把持機構（２０）により把持されており、前記制御方法は、
前記第１ワークを予め定められた力で前記第２ワークに押し付けるステップ（Ｓ１０５；Ｓ１１２；Ｓ１２２；Ｓ１３７）と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記ロボットが前記第２ワークに対する前記第１ワークの姿勢を変化させるステップ（Ｓ１０６；Ｓ１１３；Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）と、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面（５２）と、前記第２ワークの前記第１ワークと接触する第２面（６４）とが平行になる前記第１ワークの姿勢を探索するステップ（Ｓ１０８；Ｓ１１５；Ｓ１２５，Ｓ１２６；Ｓ１３０，Ｓ１３１；Ｓ１４０，Ｓ１４１；Ｓ１４５，Ｓ１４６）とを備える、制御方法。
ロボット（１０）を用いて第１ワーク（５０）と第２ワーク（６０）とを嵌合させるための制御方法であって、前記第１ワークは前記ロボット（１０）の把持機構（２０）により把持されており、前記制御方法は、
前記第１ワークを予め定められた力で前記第２ワークに押し付けるステップ（Ｓ１０５；Ｓ１１２；Ｓ１２２；Ｓ１３７）と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記ロボットが前記第２ワークに対する前記第１ワークの姿勢を変化させるステップ（Ｓ１０６；Ｓ１１３；Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）と、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面（５２）と、前記第２ワークの前記第１ワークと接触する第２面（６４）とが平行になる前記第１ワークの姿勢を探索するステップ（Ｓ１０８；Ｓ１１５；Ｓ１２５，Ｓ１２６；Ｓ１３０，Ｓ１３１；Ｓ１４０，Ｓ１４１；Ｓ１４５，Ｓ１４６）とを備える、制御方法。
【0308】
［構成１５］
第１ワーク（５０）と第２ワーク（６０）とを嵌合させるための制御プログラム（１１４）であって、前記第１ワークはロボット（１０）の把持機構（２０）により把持されており、前記制御プログラムは、コンピュータ（１００）に、
前記第１ワークを予め定められた力で前記第２ワークに押し付けるための指令を前記ロボットに与えるステップ（Ｓ１０５；Ｓ１１２；Ｓ１２２；Ｓ１３７）と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させるための指令を前記ロボットに与えるステップ（Ｓ１０６；Ｓ１１３；Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）と、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面（５２）と、前記第２ワークの前記第１ワークと接触する第２面（６４）とが平行になる前記第１ワークの姿勢を探索するステップ（Ｓ１０８；Ｓ１１５；Ｓ１２５，Ｓ１２６；Ｓ１３０，Ｓ１３１；Ｓ１４０，Ｓ１４１；Ｓ１４５，Ｓ１４６）とを実行させる、制御プログラム。
第１ワーク（５０）と第２ワーク（６０）とを嵌合させるための制御プログラム（１１４）であって、前記第１ワークはロボット（１０）の把持機構（２０）により把持されており、前記制御プログラムは、コンピュータ（１００）に、
前記第１ワークを予め定められた力で前記第２ワークに押し付けるための指令を前記ロボットに与えるステップ（Ｓ１０５；Ｓ１１２；Ｓ１２２；Ｓ１３７）と、
前記第１ワークを前記第２ワークに押し付けた状態で、前記第２ワークに対する前記第１ワークの姿勢を変化させるための指令を前記ロボットに与えるステップ（Ｓ１０６；Ｓ１１３；Ｓ１２３；Ｓ１２８；Ｓ１３８；Ｓ１４３）と、
前記第１ワークと前記第２ワークとの距離に関連する評価指標に基づいて、前記第１ワークの前記第２ワークと接触する第１面（５２）と、前記第２ワークの前記第１ワークと接触する第２面（６４）とが平行になる前記第１ワークの姿勢を探索するステップ（Ｓ１０８；Ｓ１１５；Ｓ１２５，Ｓ１２６；Ｓ１３０，Ｓ１３１；Ｓ１４０，Ｓ１４１；Ｓ１４５，Ｓ１４６）とを実行させる、制御プログラム。
【0309】
＜Ｈ．利点＞
本実施の形態によれば、２つのワークを嵌合させる場合のアプローチ位置の少なくとも一部を適切に決定するための方法を実現できる。
本実施の形態によれば、２つのワークを嵌合させる場合のアプローチ位置の少なくとも一部を適切に決定するための方法を実現できる。
【0310】
今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【符号の説明】
【0311】
１ ロボットシステム、２ ロボット座標系、１０ ロボット、１１ ベース、１２，１３，１４，１５，１６，１７ 可動部、１８ 力覚センサ、２０ 把持機構、２１，２２ 爪部、２６ ティーチングペンダント、３０ ツール座標系、３２，３３，３４，３５，３６，３７，３８ モータ、４０，１０６ インターフェイス、４２，４３，４４，４５，４６，４７，４８ ドライバ、５０ 第１ワーク、５２ 先端面、５６，５７ 最外辺、５８ 設定可能範囲、６０ 第２ワーク、６２ 嵌合孔、６４ 受入面、６６ 受入面（設計値）、６８ 第１面、６９ 第２面、１００ コントローラ、１０２ プロセッサ、１０４ メモリ、１０８ バス、１１０ ストレージ、１１２ システムプログラム、１１４ 制御プログラム、１２０ 力覚センサフィルタ、１２２ 換算処理モジュール、１２４ 差分器、１２６ サンプラ、１２８ 力制御演算モジュール、１３０ リミッタ、１３２ ホールダ、１３４ 加算器、２００ 情報処理装置、Ｌｏｃ１，Ｌｏｃ２，Ｌｏｃ３，Ｌｏｃ４ 位置計測点、Ｖｅｃ１ 第１方向ベクトル、Ｖｅｃ２ 第２方向ベクトル。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図22】
【図23】
【図24】
【図25】
【図26】
【図27】