JP6894770B2 - ワークの接触状態推定装置 - Google Patents

Description

本発明は、マニピュレータの先端部に保持したワークを、目標位置まで移動させる場合に、該目標位置の周辺に存在する周辺物体とワークとの接触状態を推定する装置及び方法に関する。

従来、この種の装置としては、例えば特許文献１、あるいは、特許文献２に見られるものが知られている。これらの特許文献１、２には、ロボット（マニピュレータ）の先端部に把持した嵌合ワークを、被嵌合ワークの穴に嵌合させる技術が記載されている。

特許文献１には、嵌合ワークを被嵌合ワークの穴に進入させていく過程で、詰まりが発生すると、嵌合ワークへの力（並進力及びモーメント）の作用点たる制御点を変更した状態で力制御を行うことで、嵌合ワークの姿勢を修正する技術が記載されている。

また、特許文献２には、嵌合ワークに作用する力（並進力及びモーメント）の検出値の最大値、あるいは、嵌合ワークと被嵌合ワークとの最初の接触時の力の検出値に応じて力制御を行うことで、嵌合ワークの姿勢を修正する技術が記載されている。

ＵＳ２０１０／００５７２５６ ＵＳ２００８／０３１２７６９

嵌合ワークを被嵌合ワークの穴に挿入する作業等、ワークを所要の目標位置まで移動させる作業をマニピュレータにより行う場合、一般に、目標位置へのワークの移動途中で、該目標位置の周辺に存在する周辺物体にワークが接触し、ひいては、ワークの目標位置への円滑な移動が阻害されることが発生し得る。

この場合、周辺物体に対するワークの接触状態の形態（ワークのどの部分が周辺物体のどの部分に接触するか、あるいは、ワークが周辺物体に対してどのような位置関係もしくは姿勢関係で接触するかの形態）は、一般には、複数種類の形態を取り得る。

例えば、特許文献１、２の如く、嵌合ワークを被嵌合ワークの穴に嵌合させる場合、嵌合ワークの移動途中での被嵌合ワークに対する接触状態の形態としては、嵌合ワークの先端の角部が、被嵌合ワークの穴の開口端の周縁部に接触する形態、嵌合ワークの先端部の側面が穴の開口端に接触する形態、嵌合ワークの先端の角部が穴の側壁に接触すると共に嵌合ワークの先端部の側面が穴の開口端に接触する形態等があり得る。

そして、上記の如く、ワークの移動途中で、該ワークと周辺物体との接触（ワークの目標位置への移動を阻害する接触）が生じた場合、該ワークを目標位置に適切に移動させ得るように該ワークの位置又は姿勢を修正するためには、当該修正を、周辺物体に対するワークの接触状態の形態（パターン）に適した態様で行うことが望ましい。

例えば、嵌合ワークを被嵌合ワークの穴に嵌合させる場合、嵌合ワークの先端の角部が、被嵌合ワークの穴の開口端の周縁部に接触した場合には、嵌合ワークを穴側にスライドさせた上で、該嵌合ワークを穴に向かって移動させる必要がある。

一方、嵌合ワークの先端部の側面が穴の開口端に接触した状態、あるいは、嵌合ワークの先端の角部が穴の側壁に接触すると共に嵌合ワークの先端部の側面が穴の開口端に接触する状態では、嵌合ワークの軸心方向を穴の軸心方向に揃えるように嵌合ワークの姿勢を修正する必要がある。

このように、ワークの移動途中で、該ワークと周辺物体との接触が生じた場合には、その接触状態の形態に適した態様でワークの位置又は姿勢の修正を行うことが望ましい。そして、そのためには、周縁物体に対するワークの接触状態が、どのような形態の接触状態であるかを特定し得ることが望まれる。

しかるに、前記特許文献１、２に見られる如き従来技術では、ワークと周辺物体との接触状態の形態を特定することは行っておらず、該接触状態の形態によらずに、力制御の手法によって、ワークの姿勢の修正を行うようにしている。

より詳しくは、前記特許文献１、２に見られる技術は、嵌合ワークと被嵌合ワークとの接触により該嵌合ワークに力が作用する状態では、嵌合ワークの少なくとも先端部が、被嵌合ワークの穴内に進入していることを前提として、力制御を行うことで、嵌合ワークの姿勢を修正する技術である。

このため、嵌合ワークの先端の角部が、被嵌合ワークの穴の開口端の周縁部に接触した場合には（この状態では、嵌合ワークの先端が未だ穴に進入していない）、嵌合ワークの位置修正を適切に行うことができず、ひいては、嵌合ワークを穴に進入させることができないという状況が発生する虞がある。

また、嵌合ワークの先端が穴にある程度進入した状態であっても、嵌合ワークと被嵌合ワークとの接触状態の形態によっては、力制御のゲイン、あるいは、力の目標値が不適切なものとなって、嵌合ワークの姿勢を好適に修正できない虞がある。

本発明はかかる背景に鑑みてなされたものであり、マニピュレータにより目標位置に移動させるワークが周辺物体に接触した場合に、その接触状態がどのような形態の接触状態であるかを適切に推定することを可能とする装置を提供することを目的とする。

本発明のワークの接触状態推定装置は、上記目的を達成するために、マニピュレータの先端部に保持されたワークを、該マニピュレータにより目標位置まで移動させる作業時に、前記目標位置の周辺に存在する物体である周辺物体に対するワークの接触状態を推定する装置であって、
前記周辺物体に対する前記ワークの接触時に、前記周辺物体から前記ワークが受ける接触反力に応じて前記マニピュレータに作用する力である接触作用力の計測値を取得する計測値取得部と、
前記周辺物体に対する前記ワークの接触状態として実現可能なものとしてあらかじめ定められた複数種類の接触状態のそれぞれ毎にあらかじめ作成された前記接触作用力の値の実現可能な範囲である接触作用力実現可能範囲と、前記接触作用力の計測値とに基づいて、前記複数種類の接触状態のうちのいずれかの接触状態を、前記周辺物体に対する前記ワークの実際の接触状態として推定する推定処理部とを備えることを基本構成とする。

なお、前記「接触作用力」は並進力及びモーメントの一方又は両方を意味する。この場合、前記「接触作用力実現可能範囲」は、接触作用力の成分数の次元の空間における範囲である。

上記基本構成を有する本発明において、前記接触作用力実現可能範囲は、一般に、前記接触状態の種類（詳しくは、周辺物体に対するワークの接触状態の形態（パターン）の種別）に依存したものとなる。そこで、前記推定処理部は、前記複数種類の接触状態のそれぞれに対応してあらかじめ作成された接触作用力実現可能範囲と、前記接触作用力の計測値とに基づいて、前記複数種類の接触状態のうちのいずれかの接触状態を、前記周辺物体に対する前記ワークの実際の接触状態として推定する。これにより、前記周辺物体に対する前記ワークの実際の接触状態を適切に推定することが可能となる。

よって、上記基本構成を有する本発明によれば、マニピュレータにより目標位置に移動させるワークが周辺物体に接触した場合に、その接触状態がどのような形態の接触状態であるかを適切に推定することを可能となる。

上記基本構成を有する本発明では、前記複数種類の接触状態のそれぞれに対応する前記接触作用力実現可能範囲は、各接触状態で前記周辺物体から前記ワークが受ける接触反力と前記接触作用力との間の関係と、各接触状態で実現可能な当該接触反力の値の範囲を規定する所定の制約条件であって、前記周辺物体と前記ワークとの間の摩擦力に関する制約条件と前記周辺物体から前記ワークに作用する抗力の向きに関する制約条件とのうちの少なくとも一方の制約条件を含む制約条件とに応じて規定される範囲であることが好ましい（第８発明）。

なお、前記「接触反力」は、前記ワークと前記周辺物体との各接触箇所で、該ワークが周辺物体から受ける並進力及びモーメントのうち、前記「接触作用力」に影響を及ぼす成分を意味する。

上記第８発明によれば、各種類の接触状態に対応する接触作用力実現可能範囲を、前記摩擦力、あるいは、抗力の向きに関する力学的な制約条件を反映させて、好適に設定することができる。

なお、摩擦力に関する制約条件としては、例えば、当該摩擦力が、最大静止摩擦力を超えないという制約条件を採用し得る。また、抗力の向きに関する制約条件としては、例えば、該抗力の向きは、ワークを周辺物体に内部に進入させる向きにはならないという制約条件を採用し得る。

前記基本構成を有する本発明では、前記推定処理部は、前記接触作用力の計測値が、前記複数種類の接触状態のうちの１つの接触状態に対応する前記接触作用力実現可能範囲にのみ属する値であるか否かを判断する第１判断部を含み、該第１判断部の判断結果が肯定的である場合に、当該１つの接触状態が前記実際の接触状態であるか否かを決定するように構成され得る（第１参考発明）。

上記第１判断部の判断結果が肯定的である場合には、前記接触作用力の計測値は、前記複数種類の接触状態のうちの１つの接触状態に対応する接触作用力実現可能範囲に属する値である一方、該１つの接触状態以外の各接触状態に対応する接触作用力実現可能範囲には属さない値である。

従って、この場合には、周辺物体に対するワークの実際の接触状態は、上記１つの接触状態である可能性が高い。

そこで、第１参考発明では、前記推定処理部は、前記第１判断部の判断結果が肯定的である場合に、当該１つの接触状態が前記実際の接触状態であるか否かを決定する。これにより、前記複数種類の接触状態から、実際の接触状態を推定することを効率よく行うことが可能となる。

ただし、特に前記接触作用力の計測値が、前記１つの接触状態以外のいずれかの接触状態に対応する接触作用力実現可能範囲の境界に比較的近い値であるような場合には、前記マニピュレータによるワークの移動制御の誤差、あるいは、マニピュレータの先端部に対するワークの位置もしくは姿勢の誤差等に起因して、実際の接触状態が前記１つの接触状態以外の接触状態であるのに、前記第１判断部の判断結果が肯定的になる場合もあり得る。

そこで、上記第１参考発明では、前記推定処理部は、前記複数種類の接触状態のそれぞれ毎に、前記接触反力の任意の値に対応する前記接触作用力の値と前記計測値との偏差度合を表すように構成された所定の第１評価関数を用い、各接触状態で実現可能な前記接触反力の値の範囲を規定する所定の制約条件の下で、前記偏差度合が最小となる該接触反力の値を求めると共に、当該求めた接触反力の値に対応する前記第１評価関数の値である評価指標値を求める評価指標値算出部と、前記１つの接触状態に対応して求められた前記評価指標値により示される前記偏差度合が、当該１つの接触状態以外の各接触状態に対応して求められた前記評価指標値により示される前記偏差度合よりも小さいか否かを判断する第２判断部とをさらに含み、前記第１判断部の判断結果が肯定的となり、且つ、前記第２判断部の判断結果が肯定的となる場合に、前記１つの接触状態を前記実際の接触状態として推定するように構成されていることが好ましい（第１発明）。

なお、前記「偏差度合」は、前記接触反力の任意の値に対応する前記接触作用力の値と前記計測値との偏差の大きさの度合を意味し、該偏差の大きさが大きいほど、大きい値を有するものである。該偏差度合としては、当該偏差の絶対値、該偏差の二乗値等を採用し得る。

また、前記偏差度合を表す第１評価関数は、より詳しくは、その値（関数値）が、前記偏差度合の増加に対して、単調増加又は単調減少となる傾向を有する関数である。

上記第１発明によれば、前記推定処理部は、前記第１判断部の判断結果が肯定的となることに加えて、さらに前記第２判断部の判断結果が肯定的となる場合、前記１つの接触状態を前記実際の接触状態として推定する。ここで、前記第２判断部の判断結果が肯定的となる場合は、前記１つの接触状態に対応して求められた前記評価指標値により示される前記偏差度合が、当該１つの接触状態以外の各接触状態に対応して求められた前記評価指標値により示される前記偏差度合よりも小さいと判断される場合である。

これにより、前記１つの接触状態を前記実際の接触状態として推定することの信頼性を高めることができる。

上記第１参考発明では、上記第１発明の代わりに、次の態様を採用することもできる。すなわち、前記推定処理部は、前記複数種類の接触状態のそれぞれ毎に、前記接触反力の任意の値に対応する前記接触作用力の値と前記計測値との偏差度合と、前記マニピュレータの先端部に対する前記ワークの任意の相対位置姿勢と該ワークの相対位置姿勢の所定の基準設定値又は計測値との偏差度合との総和の偏差度合を表すように構成された所定の第２評価関数を用い、各接触状態で実現可能な前記接触反力の値の範囲を規定する所定の制約条件の下で、前記総和の偏差度合が最小となる該接触反力の値と前記ワークの相対位置姿勢の値との組を求めると共に、当該求めた接触反力の値及び相対位置姿勢の値の組に対応する前記第２評価関数の値である評価指標値を求める評価指標値算出部と、前記１つの接触状態に対応して求められた前記評価指標値により示される前記総和の偏差度合が、当該１つの接触状態以外の各接触状態に対応して求められた前記評価指標値により示される前記総和の偏差度合よりも小さいか否かを判断する第２判断部とをさらに含み、前記第１判断部の判断結果が肯定的となり、且つ、前記第２判断部の判断結果が肯定的となる場合に、前記１つの接触状態を前記実際の接触状態として推定するように構成される（第２発明）。

なお、上記第２発明において、前記接触反力の任意の値に対応する前記接触作用力の値と前記計測値との「偏差度合」の意味は、前記第１発明における「偏差度合」と同じである。

また、前記マニピュレータの先端部に対する前記ワークの任意の相対位置姿勢と該ワークの相対位置姿勢の所定の基準設定値又は計測値との「偏差度合」は、前記ワークの任意の相対位置姿勢と該ワークの相対位置姿勢の所定の基準設定値又は計測値との偏差の大きさの度合を意味し、該偏差の大きさが大きいほど、大きい値を有するものである。該偏差度合としては、当該偏差の絶対値、該偏差の二乗値等を採用し得る。

また、上記「基準設定値」は、前記ワークをマニピュレータに保持するときのワークの相対位置姿勢としてあらかじめ定められた相対位置姿勢の値を意味する。

なお、「相対位置姿勢」は、マニピュレータの先端部に対するワークの相対位置及び相対姿勢の組を意味する。

また、前記総和の偏差度合を表す第２評価関数は、より詳しくは、その値（関数値）が、前記総和の偏差度合の増加に対して、単調増加又は単調減少となる傾向を有する関数である。

上記第２発明によれば、前記推定処理部は、前記第１判断部の判断結果が肯定的となることに加えて、さらに前記第２判断部の判断結果が肯定的となる場合、前記１つの接触状態を前記実際の接触状態として推定する。ここで、前記第２判断部の判断結果が肯定的となる場合は、前記１つの接触状態に対応して求められた前記評価指標値により示される前記総和の偏差度合が、当該１つの接触状態以外の各接触状態に対応して求められた前記評価指標値により示される前記総和の偏差度合よりも小さいと判断される場合である。

この場合、前記第２判断部の判断処理で用いる前記評価指標値には、接触作用力に係る偏差度合に加えて、ワークの相対位置姿勢に係る偏差度合が適切に反映される。

従って、第２発明によれば、前記１つの接触状態を前記実際の接触状態として推定することの信頼性をより一層高めることができる。

なお、上記第１発明又は第２発明では、前記所定の制約条件としては、前記第８発明で示した制約条件を採用し得る。

上記第１発明又は第２発明では、前記第２判断部の判断結果が否定的である場合には、前記実際の接触状態は、前記１つの接触状態と異なる種類の接触状態であるとみなし得る。

そこで、上記第１発明では、前記推定処理部は、前記第２判断部の判断結果が否定的である場合に、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するように構成され得る（第３発明）。

同様に、上記第２発明では、前記推定処理部は、前記第２判断部の判断結果が否定的である場合に、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するように構成され得る（第４発明）。

これらの第３発明及び第４発明において、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するので、当該推定処理を効率よく行うことが可能となる。

この場合、前記残りの接触状態が１つの接触状態である場合には、当該残りの接触状態を実際の接触状態として推定し得る。

また、前記残りの接触状態が複数種類の接触状態である場合には、例えば、当該残りの接触状態に対して、前記第１参考発明、第１発明、又は第２発明と同様の処理を実行することで、当該残りの接触状態から、実際の接触状態を推定し得る。

上記第１参考発明、第１〜第４発明では、前記推定処理部は、前記第１判断部の判断結果が否定的である場合に、前記接触作用力の計測値を、前記複数種類の接触状態うちのいずれか１つの接触状態に対応する接触作用力実現可能範囲にのみ属する値となるように決定した目標値に向かって変化させるように前記マニピュレータを作動させるマニピュレータ制御部と、該マニピュレータの作動により前記接触作用力の計測値が、前記目標値が属する接触作用力実現可能範囲内に変化したか否かを判断する第３判断部とをさらに含み、該第３判断部の判断結果が肯定的である場合に、前記目標値が属する接触作用力実現可能範囲に対応する接触状態を、前記実際の接触状態として推定するように構成されていることが好ましい（第５発明）。

ここで、上記第３判断部の判断結果が肯定的になるということは、実際の接触状状態が、１つの接触状態に対応する接触作用力実現可能範囲にのみ属する接触作用力の値を実現し得る接触状態であることを意味する。

そこで、第５発明では、前記推定処理部は、前記第３判断部の判断結果が肯定的である場合に、前記目標値が属する接触作用力実現可能範囲に対応する接触状態を、前記実際の接触状態として推定する。これにより、前記第１判断部の判断結果が否定的であっても、実際の接触状態を適切に推定することが可能となる。

なお、前記第３判断部のより具体的な処理としては、例えば、前記マニピュレータ制御部によるマニピュレータの上記の作動後の前記接触作用力の計測値を用いて前記第１判断部又は前記第２判断部と同じ判断処理を実行する手法を採用し得る。

上記第５発明では、前記第３判断部の判断結果が否定的である場合には、前記実際の接触状態は、前記目標値が属する接触作用力実現可能範囲に対応する接触状態と異なる種類の接触状態であるとみなし得る。

そこで、上記第５発明では、前記推定処理部は、前記第３判断部の判断結果が否定的で
ある場合に、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するように構成され得る（第６発明）。

これによれば、前記第３発明又は第４発明と同様に、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するので、当該推定処理を効率よく行うことが可能となる。

上記第５発明又は第６発明では、前記マニピュレータ制御部は、前記接触作用力の計測値を前記目標値に向かって変化させるように前記マニピュレータを作動させた後に、該マニピュレータを当該作動前の状態に復帰させるように構成されていることが好ましい（第１０発明）。

これによれば、ワークを目標位置に移動させるためのマニピュレータの動作のばらつきを抑制して、該動作のの安定性を高めることができる。

また、ワークの接触状態推定方法は、マニピュレータの先端部に把持されたワークを、該マニピュレータにより目標位置まで移動させる作業時に、前記目標位置の周辺に存在する物体である周辺物体に対するワークの接触状態を推定する方法であって、
前記周辺物体に対する前記ワークの接触時に、前記周辺物体から前記ワークが受ける接触反力に応じて前記マニピュレータに作用する力である接触作用力の計測値を取得するステップと、
前記周辺物体に対する前記ワークの接触状態として実現可能なものとしてあらかじめ定められた複数種類の接触状態のそれぞれ毎にあらかじめ作成された前記接触作用力の値の実現可能な範囲である接触作用力実現可能範囲と、前記接触作用力の計測値とに基づいて、前記複数種類の接触状態のうちのいずれかの接触状態を、前記周辺物体に対する前記ワークの実際の接触状態として推定するステップとを備えることを特徴とする（第２参考発明）。

かかる第２参考発明によれば、前記基本構成を有する本発明と同様の効果を奏することができる。なお、第２参考発明は、さらに前記第８発明、第１参考発明、第１〜第７発明と同様の態様を採用し得る。

本発明の実施形態におけるマニピュレータ及びワークの一例を示す図。 接触状態推定装置としての機能を有する制御装置の機能的構成を示すブロック図。 図３Ａ及び図３Ｂは、それぞれワークと周辺物体との接触状態の形態の例を示す図。 図４Ａ及び図４Ｂは、それぞれワークと周辺物体との接触状態の形態の例を示す図。 図５Ａ及び図５Ｂは、それぞれワークと周辺物体との接触状態の形態の例を示す図。 図６Ａ〜図６Ｆは、それぞれワークと周辺物体との接触状態の形態の他の例を示す図。 図７Ａ〜図７Ｃは、それぞれ図３Ａに示す接触状態での接触作用力実現可能範囲を例示する図。 図８Ａ〜図８Ｃは、それぞれ図３Ｂに示す接触状態での接触作用力実現可能範囲を例示する図。 図９Ａ〜図９Ｃは、それぞれ図４Ａに示す接触状態での接触作用力実現可能範囲を例示する図。 図１０Ａ〜図１０Ｃは、それぞれ図４Ｂに示す接触状態での接触作用力実現可能範囲を例示する図。 図１１Ａ〜図１１Ｃは、それぞれ図５Ａに示す接触状態での接触作用力実現可能範囲を例示する図。 図１２Ａ〜図１２Ｃは、それぞれ図５Ｂに示す接触状態での接触作用力実現可能範囲を例示する図。 図２に示す制御装置による処理を示すフローチャート。 図１４Ａ〜図１４Ｅは、それぞれ図１３のＳＴＥＰ６で抽出される一意特定範囲の例を示す図。

［第１実施形態］
本発明の第１実施形態を図１〜図１４Ｅを参照して以下に説明する。図１を参照して、本実施形態では、例えば複数のワークＡを収容箱Ｂに並べて収容する作業を、マニピュレータ１により行う場合に関して説明する。なお、図１では、ワークＡを直方体状のワークとして図示しているが、ワークＡの形状は、その他の形状であってもよい。

マニピュレータ１は、本実施形態では、ワークＡを把持するハンド部２が先端部に装着された多関節アーム３を備えるロボットである。該マニピュレータ１は、移動型のロボット及び据置型のロボットのいずれであってもよい。また、マニピュレータ１は、例えばマスター・スレーブ方式で作業者が操縦し得るロボットであってもよい。

ハンド部２は、図示しないアクチュエータにより開閉自在に構成された一対の指リンク４，４を有し、該指リンク４，４の先端部の間にワークＡを挟み込む（把持する）ことで、該ワークＡを保持する。この保持状態では、ハンド部２に対するワークＡの相対的な位置及び姿勢がほぼ一定に保たれるように、該ワークＡがハンド部２に把持される。

なお、ハンド部２の構成は図１に例示した構成に限られない。例えば、ハンド部２は、３つ以上の指リンクを備えるもの、あるいは、屈伸しない指リンクを備えるものであってもよい。また、マニピュレータ１は、ハンド部２の代わりに、例えば、磁力等の吸引力もしくは吸着力を利用して、ワークＡを保持する機構を備えるものであってもよい。

マニピュレータ１の所定部位、例えば、アーム３の先端部（ハンド部２の装着部分）には、力センサ５が搭載されている。この力センサ５は、ハンド部２に把持したワークＡが外界物に接触したときに、該ワークＡが外界物から受ける接触反力の全体に応じて、該力センサ５の搭載箇所にてマニピュレータ１に作用する力（接触作用力）を検出するセンサである。該力センサ５は、例えば６軸力センサにより構成され、３軸方向の並進力と３軸周りのモーメントとを検出する。

図２は、以上の如く構成されたマニピュレータ１の作動制御を行う機能を有する制御装置１０を示している。この制御装置１０は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む１つ以上の電子回路ユニット、あるいは、１つ以上のコンピュータ、あるいは、これらの組み合わせにより構成される。この制御装置１０には、前記力センサ５の検出信号が入力される。そして、制御装置１０は、実装されたハードウェア構成又はプログラムにより実現される機能によって、マニピュレータ１の作動制御を行う。

ここで、制御装置１０によるマニピュレータ１の作動制御の概要を説明しておく。本実施形態では、制御装置１０は、収容箱Ｂに収容すべき各ワークＡを、収容箱Ｂの外部でハンド部２より把持した後、アーム３の作動によって、該ワークＡを、収容箱Ｂ内の目標位置（収容箱Ｂ内で該ワークＡを配置すべき位置）まで移動させるように、マニピュレータ１の作動制御を行う。

この場合、制御装置１０は、例えば、収容箱Ｂの位置情報、収容箱Ｂ内における各ワークＡの目標位置、収容箱Ｂへの各ワークＡの収容順位（もしくは収容箱Ｂ内に収容済のワークＡの配置情報）等に基づき、収容箱Ｂに新たに収容しようとするワークＡの目標移動軌道を決定する。そして、制御装置１０は、基本的には、該目標移動軌道に従って、ワークＡを収容箱Ｂ内の目標位置まで移動させるように、マニピュレータ１を制御する。

ただし、この場合、マニピュレータ１の制御誤差、ハンド部２によるワークＡの把持位置の誤差、あるいは、ワークＡの形状もしくは寸法のばらつき等に起因して、ワークＡの実際の移動軌道が、目標移動軌道からずれた移動軌道になる場合がある。

このような場合には、ワークＡが、収容箱Ｂ内の目標位置の周辺に存在する周辺物体（本実施形態では、収容箱Ｂの側壁Ｂａの上端部、あるいは、収容箱Ｂ内に収容済の他のワークＡ）に対して、目標移動軌道では想定されていない態様で接触する場合がある。ひいては、ワークＡを、収容箱Ｂ内の目標位置まで目標移動軌道に従って移動させることができなくなる場合がある。

このような場合には、制御装置１０は、ワークＡを収容箱Ｂ内の目標位置に到達させ得るように、該ワークＡの目標移動軌道を適宜修正する。この場合、ワークＡを収容箱Ｂ内の目標位置まで円滑に移動させ得るように該ワークＡの目標移動軌道を修正するためには、当該修正は、周辺物体に対するワークＡの接触状態の形態（パターン）に適した態様で行うことが望ましい。

例えば、図３Ａに示す如く、収容箱Ｂ内の目標位置ＴＰ(i)に移動させるべきワークＡ(i)が、収容箱Ｂの側壁Ｂａの上端面に接触した場合には、矢印Ｙａで示すように、ワークＡ(i)を横方向にスライドさせた後に、ワークＡ(i)を目標位置ＴＰ(i)まで移動させるように、ワークＡ(i)の目標移動軌道を修正することが望ましい。

なお、目標位置ＴＰ(i)は、ワークＡ(i)の代表点、例えばワークＡ(i)の下端面上の中央点の目標位置を示している。

また、例えば、図３Ｂに示す如く、収容箱Ｂ内の目標位置ＴＰ(i)に移動させるべきワークＡ(i)が、収容箱Ｂの側壁Ｂａの上端部のエッジ（上端面と側壁面との境界のエッジ）に接触した場合には、矢印Ｙｂで示すように、ワークＡ(i)を回転させた後（もしくは回転させながら）、ワークＡ(i)を目標位置ＴＰ(i)まで移動させるように、ワークＡ(i)の目標移動軌道を修正することが望ましい。

このように、周辺物体へのワークＡの接触に応じて、該接触後のワークＡの目標移動軌道を修正する場合、その修正は、周辺物体に対するワークＡの接触状態の形態（パターン）に適した態様で行うことが望ましい。

そこで、制御装置１０は、ワークＡを収容箱Ｂ内の目標位置に移動させていく過程で、目標位置の周辺の周辺物体にワークＡが接触した場合に、その接触状態を推定する（詳しくは、どのような形態での接触状態であるかを推定する）ために、本発明の接触状態推定装置としての機能を含む。

この場合、制御装置１０は、実装されたハードウェア構成又はプログラム（ソフトウェア構成）により実現される機能として、入力される力センサ５の検出信号から接触作用力の計測値を適宜取得する計測値取得部１１と、各ワークＡの移動過程で発生し得る該ワークＡと周辺物体との接触状態に関するあらかじめ作成された情報（接触状態情報）を記憶保持する接触状態情報記憶部１３と、該接触状態情報と接触作用力の計測値とを用いて、ワークＡと周辺物体との実際の接触状態を推定する推定処理部１２とを備える。

なお、推定処理部１２は、本発明における第１判断部、第２判断部、第３判断部、及び評価指標値算出部としての機能を含む。

上記接触状態情報は、本実施形態では、各ワークＡの移動過程で発生し得る該ワークＡと周辺物体との複数種類の形態の接触状態のそれぞれの種類を示す接触状態種類情報と、各接触状態（各形態の接触状態）で、ワークＡが周辺物体から受ける接触反力の全体に応じて、実現可能な接触作用力（力センサ５で検出され得る接触作用力）の値の範囲である接触作用力実現可能範囲を示す接触作用力範囲情報と、前記複数種類の接触状態の相互間の遷移情報（ある１つの接触状態から他の１つの接触状態への遷移が発生し得るか否か等を示す情報）とを含む。

かかる接触状態情報は、収容箱Ｂ内の各ワークＡの目標位置のそれぞれ毎に作成された情報である。なお、収容箱Ｂ内の複数のワークＡのそれぞれの目標位置に対応する接触状状態情報が互いに同じ情報となる場合もある。

上記接触状態情報のうち、接触状態種別情報及び遷移情報は、例えば、各ワークＡを収容箱Ｂ内の目標位置に移動させるための基準の目標移動軌道、該基準の目標移動軌道に対する実際の移動軌道の最大のずれ度合（制御誤差等に起因する最大のずれ度合）の予測値、収容箱Ｂ内の目標位置の周辺での収容箱Ｂの側壁Ｂａ又は他のワークＡの存在可能領域等を考慮して作成される。

なお、接触状態種別情報及び遷移情報を、例えば、“Generating a contact state graph of polyhedral objects for robotic application”/ Sung Jo Kwak, Seong Youb Chung, Tsutomu Hasegawa/ Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference on/ pp.4522 - 4527等に見られる公知の手法を利用して、自動的に作成することも可能である。

ワークＡを収容箱Ｂ内の目標位置に移動させる過程で発生し得る接触状態のパターンの例を図３Ａ、図３Ｂ、図４Ａ、図４Ｂ、図５Ａ及び図５Ｂに示す。これらの図３Ａ、図３Ｂ、図４Ａ、図４Ｂ、図５Ａ及び図５Ｂは、ワークＡ(i)を収容箱Ｂ内の１つの目標位置ＴＰ(i)まで移動させる場合に発生し得る複数種類（ここでは４種類）のパターンの接触状態を例示している。

図３Ａの接触状態は、ワークＡ(i)の底面のエッジが収容箱Ｂの側壁Ｂａの上端面に接触した状態、図３Ｂの接触状態は、ワークＡ(i)の側面が収容箱Ｂの側壁Ｂａの上端面のエッジに接触した状態、図４Ａの接触状態は、ワークＡ(i)の底面がワークＡ(i)の隣の収容済のワークＡの上端面のエッジに接触すると共に、ワークＡ(i)の側面が収容箱Ｂの側壁Ｂａの上端面のエッジに接触した状態、図４Ｂの接触状態は、ワークＡ(i)の底面のエッジがワークＡ(i)の隣の収容済のワークＡの側面に接触すると共に、ワークＡ(i)の側面が収容箱Ｂの側壁Ｂａの上端面のエッジに接触した状態、図５Ａの接触状態は、ワークＡ(i)の底面が隣の収容済のワークＡの上端面のエッジに接触した状態、図５Ｂの接触状態は、ワークＡ(i)の底面のエッジがワークＡ(i)の隣の収容済のワークＡの側面に接触した状態である。

以降の説明では、図３Ａ、図３Ｂ、図４Ａ、図４Ｂ、図５Ａ及び図５Ｂの接触状態を、それぞれ、接触状態３Ａ、接触状態３Ｂ、接触状態４Ａ、接触状態４Ｂ、接触状態５Ａ、接触状態５Ｂと称する。

この場合、接触状態３Ａでは、ワークＡ(i)が接触する周辺物体（収容箱Ｂの側壁Ｂａ）に対するワークＡ(i)の傾斜角は、接触状態４Ａの形態に適合するとみなし得る所定範囲内の任意の値である。

また、接触状態３Ｂでは、ワークＡ(i)が接触する周辺物体（収容箱Ｂの側壁Ｂａ）に対するワークＡ(i)の接触部分の位置（ワークＡ(i)の側面内の位置）と、該周辺物体に対するワークＡ(i)の傾斜角とは、それぞれ、接触状態３Ｂのパターンに適合するとみなし得る所定範囲内の任意の値である。

また、接触状態４Ａでは、ワークＡ(i)が接触する２つの周辺物体のうちの一方の周辺物体（収容箱Ｂの側壁Ｂａ）に対するワークＡ(i)の接触部分の位置（ワークＡ(i)の側面内の位置）と、他方の周辺物体（収容済のワークＡ）に対するワークＡ(i)の接触部分の位置（ワークＡ(i)の底面内の位置）と、これらの２つの周辺物体に対するワークＡ(i)の傾斜角とは、それぞれ、接触状態４Ａのパターンに適合するとみなし得る所定範囲内の任意の値である。

また、接触状態４Ｂでは、ワークＡ(i)が接触する２つの周辺物体のうちの一方の周辺物体（収容箱Ｂの側壁Ｂａ）に対するワークＡ(i)の接触部分の位置（ワークＡ(i)の側面内の位置）と、当該２つの周辺物体に対するワークＡ(i)の傾斜角とは、それぞれ、接触状態４Ｂのパターンに適合するとみなし得る所定範囲内の任意の値である。

また、接触状態５Ａでは、ワークＡ(i)が接触する周辺物体（収容済のワークＡ）に対するワークＡ(i)の接触部分の位置（ワークＡ(i)の底面内の位置）と、該周辺物体に対するワークＡ(i)の傾斜角とは、それぞれ、接触状態５Ａのパターンに適合するとみなし得る所定範囲内の任意の値である。

また、接触状態５Ｂでは、ワークＡ(i)が接触する周辺物体（収容済のワークＡ）に対するワークＡ(i)の接触部分の位置（ワークＡ(i)の側面内の位置）と、該周辺物体に対するワークＡ(i)の傾斜角とは、それぞれ、接触状態５Ｂのパターンに適合するとみなし得る所定範囲内の任意の値である。

補足すると、ワークＡ(i)を収容箱Ｂ内の目標位置ＴＰ(i)まで移動させる場合に、ワークＡ(i)の基準の目標移動軌道、あるいは、収容箱Ｂの形状、あるいは、ワークＡ(i)の形状等に依存して、上記の６種類の接触状態３Ａ，３Ｂ，４Ａ，４Ｂ，５Ａ，５Ｂと異なる接触状態が発生する場合もある。

ただし、本実施形態では、説明が冗長になるのを避けるために、便宜上、ワークＡ(i)の移動過程で発生し得る接触状態として、上記の６種類の接触状態３Ａ，３Ｂ，４Ａ，４Ｂ，５Ａ，５Ｂを例示している。

ワークＡを収容箱Ｂ内の目標位置に移動させる過程で発生し得る接触状態のパターンは、収容箱Ｂへの収容順位等によっても相違する。当該接触状態のパターンの他の例を図６Ａ〜図６Ｆに示す。これらの図６Ａ〜図６Ｆは、収容箱Ｂに他のワークＡが未だ収容されていない状態で、１つのワークＡ(j)を収容箱Ｂ内の側壁Ｂａ寄りの目標位置ＴＰ(j)まで移動させる場合に発生し得る複数種類（ここでは６種類）のパターンの接触状態を例示している。

図６Ａの接触状態及び図６Ｃの接触状態は、それぞれ前記接触状態３Ａ、３Ｂと同じパターンの接触状態、図６Ｂの接触状態は、ワークＡ(j)の底面のエッジが収容箱Ｂの底面に接触した状態、図６Ｄの接触状態は、ワークＡ(j)の底面が、目標位置ＴＰ(j)からずれた状態で、収容箱Ｂの底面に面接触した状態、図６ＥはワークＡ(j)の上端面のエッジが収容箱Ｂの側壁Ｂａに接触すると共に、ワークＡ(j)の底面のエッジが収容箱Ｂの底面に接触した状態、図６ＦはワークＡ(j)の側面が収容箱Ｂの側壁Ｂａに面接触した状態である。

このように、ワークＡを収容箱Ｂ内の目標位置に移動させる過程で発生し得る接触状態のパターンは、収容箱Ｂ内の目標位置、収容順位等に応じて種々様々なパターンをとり得る。

接触状態情報のうち、前記接触作用力範囲情報により示される各接触状態毎の接触作用力実現可能範囲は、各接触状態（各形態の接触状態）において、ワークＡが周辺物体との各接触部にて該周辺物体から受ける接触反力と該接触反力の全体（ワークＡと周辺物体との全ての接触部における接触反力の全体）に応じて力センサ５の配置箇所に作用する接触作用力（該力センサ５により検出され得る接触作用力）との関係と、ワークＡが受ける接触反力の範囲を規定する制約条件とに基づいて作成される。

接触反力に関する制約条件としては、例えば、接触反力のうちの摩擦力（並進摩擦力）を、最大静止摩擦力以下に制限する制約条件と、接触反力のうちの抗力の向きを制限する制約条件（詳しくは、当該抗力の向きは、ワークＡを接触相手の周辺物体にめり込ませる向きにはならないという制約条件）とを使用し得る。

この場合、ワークＡが受ける接触反力と接触作用力との関係は、一般に、次式（１）の形態の線形等式により表すことができる。また、接触反力に関する制約条件は、一般に、次式（２）の形態の線形不等式により表すことができる。

↑Ｆ＝Ｇmat・↑ｆ ……（１）
Ｃmat・↑ｆ≦↑ｂ ……（２）

なお、式（２）の不等号の向きは、逆向きであってもよい。

ここで、本明細書では、↑Ｆ等、先頭に「↑」を付した参照符号は、複数の成分を並べたベクトル（縦ベクトル）を意味する。そして、式（１）における↑Ｆは、接触作用力の計測値を表現する座標系（以降、接触作用力座標系という）で見た接触作用力（並進力及びモーメント）の各成分を並べたベクトル、式（１）及び（２）における↑ｆはワークＡと周辺物体との全ての接触部分のそれぞれにおいて、ワークＡが受ける接触反力の各成分（接触作用力座標系での各成分）を並べたベクトルである。また、Ｇmat、Ｃmatは行列、↑ｂは定数成分のベクトルである。

なお、周辺物体に対するワークＡの接触部分（接触反力の作用部分）が１つである場合には、↑ｆは、当該１つの接触部分の接触反力を示すベクトルであるが、周辺物体に対するワークＡの接触部分が複数である場合には、↑ｆは、全ての接触部分のそれぞれの接触反力の成分をひとまとめに並べたベクトルである。

式（１）は、詳しくは、ワークＡが接触する周辺物体から受ける全ての接触反力の合力のうちの並進力が接触作用力の並進力に一致し、且つ、当該接触反力の合力が接触作用力座標系の原点周りに発生するモーメントが接触反力のモーメントに一致するという関係を示す等式である。

従って、式（１）におけるＧmatは、接触作用力座標系で見た接触反力の全体を、上記関係に従って、接触作用力に変換する行列である。

また、式（２）は、周辺物体に対するワークＡの各接触部分での接触作用力に関する制約条件のそれぞれを、↑ｆの各成分の線形結合値が所定値以下（又は所定値以上）という形の線形不等式で表し、さらにそれらの不等式を行列不等式の形態でひとまとめにしたものである。

従って、式（２）におけるＣmatは、複数の線形不等式のそれぞれの上記線形結合値に係る係数を各行に並べた行列、↑ｂは複数の線形不等式のそれぞれの上記所定値を並べたベクトルである。

上記式（１），（２）のより具体的な形態の例として、例えば、前記接触状態３Ａ，３Ｂ，４Ａ，４Ｂ，５Ａ，５Ｂのそれぞれに対応する式を以下に提示する。式（１−３Ａ），（２−３Ａ）は、接触状態３Ａに対応する式、式（１−３Ｂ），（２−３Ｂ）は、接触状態３Ｂに対応する式、式（１−４Ａ），（２−４Ａ）は、接触状態４Ａに対応する式、式（１−４Ｂ），（２−４Ｂ）は、接触状態４Ｂに対応する式、式（１−５Ａ），（２−５Ａ）は、接触状態５Ａに対応する式、式（１−５Ｂ），（２−５Ｂ）は、接触状態５Ｂに対応する式である。

これらの式（１−３Ａ），（２−３Ａ），…，（１−５Ａ），（２−５Ｂ）では、図３Ａ、図３Ｂ、図４Ａ、図４Ｂ、図５Ａ及び図５Ｂに示す如く、接触反力座標系は力センサ５（又はマニピュレータ１のアーム３の先端部）に対して固定された座標系である。この例では、接触反力座標系のＸ軸方向及びこれに直交するＹ軸方向は、ワークＡ(i)の底面と平行（もしくはほぼ平行）な方向、Ｚ軸方向は、Ｘ軸及びＹ軸に直交する方向である。そして、式（１−３Ａ），（１−３Ｂ），（１−４Ａ），（１−４Ｂ），（１−５Ａ），（１−５Ｂ）は、接触作用力のうちの並進力のＸ軸方向成分Ｆx及びＹ軸方向成分Ｆy、並びに、接触作用力のうちのモーメントのＺ軸周り方向成分Ｍzの３成分についての、前記式（１）に対応する式（式（１）のうちのＦx，Ｆy，Ｍzに関する式）である。

式（２−３Ａ），（２−３Ｂ），（２−４Ａ），（２−４Ｂ）は、それぞれ、前記式（２）に対応する複数の不等式である。式（２−３Ａ），（２−３Ｂ），（２−４Ａ），（２−４Ｂ），（２−５Ａ），（２−５Ｂ）は、それぞれ、式（２）の形式にまとめることができる。

前記式（１−３Ａ），（２−３Ａ）におけるｒx1，ｒy1は、それぞれ、前記接触状態３ＡでワークＡ(i)が接触する周辺物体（収容箱Ｂの側壁Ｂａ）に対するワークＡ(i)の接触部分の位置ベクトル（図３Ａに示す↑ｒ1）のＸ軸座標成分及びＹ軸座標成分、ｆx1，ｆy1は、それぞれ、当該接触部分でワークＡ(i)が受ける接触反力（図３Ａに示す↑ｆ1）のＸ軸方向成分及びＹ軸方向成分である。

前記式（１−３Ｂ），（２−３Ｂ）におけるｒx2，ｒy2は、それぞれ、前記接触状態３ＢでワークＡ(i)が接触する周辺物体に対するワークＡ(i)の接触部分の位置ベクトル（図３Ｂに示す↑ｒ2）のＸ軸座標成分及びＹ軸座標成分、ｆx2，ｆy2は、それぞれ、当該接触部分でワークＡ(i)が受ける接触反力（図３Ｂに示す↑ｆ2）のＸ軸方向成分及びＹ軸方向成分、μaはワークＡ(i)と周辺物体（収容箱Ｂの側壁Ｂａ）との間の摩擦係数（設定値）である。

なお、前記接触状態３Ｂでの接触部分の位置ベクトル↑ｒ2は所定範囲内での可変値である。

前記式（１−４Ａ），（２−４Ａ）におけるｒx3，ｒy3は、それぞれ、前記接触状態４ＡでワークＡ(i)が接触する２つの周辺物体（収容箱Ｂの側壁Ｂａ及び収容済の他のワークＡ）のうちの一方の周辺物体（収容箱Ｂの側壁Ｂａ）に対するワークＡ(i)の接触部分の位置ベクトル（図４Ａに示す↑ｒ3）のＸ軸座標成分及びＹ軸座標成分、ｆx3，ｆy3は、それぞれ、当該一方の周辺物体に対する接触部分でワークＡ(i)が受ける接触反力（図４Ａに示す↑ｆ3）のＸ軸方向成分及びＹ軸方向成分、μaはワークＡ(i)と当該一方の周辺物体（収容箱Ｂの側壁Ｂａ）との間の摩擦係数（設定値）、ｒx4，ｒy4は、それぞれ、他方の周辺物体（収容済の他のワークＡ）に対するワークＡ(i)の接触部分の位置ベクトル（図４Ａに示す↑ｒ4）のＸ軸座標成分及びＹ軸座標成分、ｆx4，ｆy4は、それぞれ、当該他方の周辺物体に対する接触部分でワークＡ(i)が受ける接触反力（図４Ａに示す↑ｆ4）のＸ軸方向成分及びＹ軸方向成分、μbはワークＡ(i)と当該他方の周辺物体（収容済の他のワークＡ）との間の摩擦係数（設定値）である。

なお、前記接触状態４Ａでの各接触部分の位置ベクトル↑ｒ3，↑ｒ4は所定範囲内での可変値である。

前記式（１−４Ｂ），（２−４Ｂ）におけるｒx5，ｒy5は、それぞれ、前記接触状態４ＢでワークＡ(i)が接触する２つの周辺物体（収容箱Ｂの側壁Ｂａ及び収容済の他のワークＡ）のうちの一方の周辺物体（収容箱Ｂの側壁Ｂａ）に対するワークＡ(i)の接触部分の位置ベクトル（図４Ｂに示す↑ｒ5）のＸ軸座標成分及びＹ軸座標成分、ｆx5，ｆy5は、それぞれ、当該一方の周辺物体に対する接触部分でワークＡ(i)が受ける接触反力（図４Ｂに示す↑ｆ5）のＸ軸方向成分及びＹ軸方向成分、μaはワークＡ(i)と当該一方の周辺物体（収容箱Ｂの側壁Ｂａ）との間の摩擦係数（設定値）、ｒx6，ｒy6は、それぞれ、他方の周辺物体（収容済の他のワークＡ）に対するワークＡ(i)の接触部分の位置ベクトル（図４Ｂに示す↑ｒ6）のＸ軸座標成分及びＹ軸座標成分、ｆx6，ｆy6は、それぞれ、当該他方の周辺物体に対する接触部分でワークＡ(i)が受ける接触反力（図４Ｂに示す↑ｆ6）のＸ軸方向成分及びＹ軸方向成分、μbはワークＡ(i)と当該他方の周辺物体（収容済の他のワークＡ）との間の摩擦係数（設定値）である。

なお、前記接触状態４Ｂでの各接触部分の位置ベクトル↑ｒ5，↑ｒ6は所定範囲内での可変値である。

前記式（１−５Ａ），（２−５Ａ）におけるｒx7，ｒy7は、それぞれ、前記接触状態５ＡでワークＡ(i)が接触する周辺物体（収容済の他のワークＡ）に対するワークＡ(i)の接触部分の位置ベクトル（図５Ａに示す↑ｒ7）のＸ軸座標成分及びＹ軸座標成分、ｆx7，ｆy7は、それぞれ、当該接触部分でワークＡ(i)が受ける接触反力（図５Ａに示す↑ｆ7）のＸ軸方向成分及びＹ軸方向成分、μbはワークＡ(i)と当該周辺物体（収容済の他のワークＡ）との間の摩擦係数（設定値）である。

なお、前記接触状態５Ａでの接触部分の位置ベクトル↑ｒ7は所定範囲内での可変値である。

前記式（１−５Ｂ），（２−５Ｂ）におけるｒx8，ｒy8は、それぞれ、前記接触状態５ＢでワークＡ(i)が接触する周辺物体（収容済の他のワークＡ）に対するワークＡ(i)の接触部分の位置ベクトル（図５Ｂに示す↑ｒ8）のＸ軸座標成分及びＹ軸座標成分、ｆx8，ｆy8は、それぞれ、当該接触部分でワークＡ(i)が受ける接触反力（図５Ｂに示す↑ｆ8）のＸ軸方向成分及びＹ軸方向成分、μbはワークＡ(i)と当該周辺物体（収容済の他のワークＡ）との間の摩擦係数（設定値）である。

各接触状態毎の接触作用力実現可能範囲は、前記式（２）により示される制約条件の下で、前記式（１）の関係式を満たす接触作用力↑Ｆの値（↑Ｆの各成分の値の組）の集合の範囲を算出することで作成される。この場合、各接触状態毎の接触作用力実現可能範囲は、例えば、線形計画問題の公知の解法処理を用いて作成することができる。

接触作用力実現可能範囲の例を図７Ａ〜図１２Ｃに示す。図７Ａ、図８Ａ、図９Ａ、図１０Ａ、図１１Ａ、図１２Ａは、それぞれ、前記接触状態３Ａ、３Ｂ、４Ａ、４Ｂ、５Ａ、５Ｂのそれぞれにおける接触作用力の成分Ｆy，Ｍzの組の実現可能範囲を２次元平面上の斜線領域で示す図、図７Ｂ、図８Ｂ、図９Ｂ、図１０Ｂ、図１１Ｂ、図１２Ｂは、それぞれ、前記接触状態３Ａ、３Ｂ、４Ａ、４Ｂ、５Ａ、５Ｂのそれぞれにおける接触作用力の成分Ｆx，Ｆyの組の実現可能範囲を２次元平面上の斜線領域で示す図、図７Ｃ、図８Ｃ、図９Ｃ、図１０Ｃ、図１１Ｃ、図１２Ｃは、それぞれ、前記接触状態３Ａ、３Ｂ、４Ａ、４Ｂ、５Ａ、５Ｂのそれぞれにおける接触作用力の成分Ｆx，Ｍzの組の実現可能範囲を２次元平面上の斜線領域で示す図である。

接触状態３Ａ，３Ｂ，４Ａ，４Ｂ，５Ａ，５Ｂのそれぞれに対応する接触作用力実現可能範囲は、それぞれ、前記式（１−３Ａ），（２−３Ａ）の組、前記式（１−３Ｂ），（２−３Ｂ）の組、前記式（１−４Ａ），（２−４Ａ）の組、前記式（１−４Ｂ），（２−４Ｂ）の組、前記式（１−５Ａ），（２−５Ａ）の組、前記式（１−５Ｂ），（２−５Ｂ）の組に基づいて算出される。

そして、図７Ａ、図８Ａ、図９Ａ、図１０Ａ、図１１Ａ、図１２Ａは、このように算出される接触作用力実現可能範囲の全体のうち、Ｆy，Ｍz以外の成分（ここでは、成分Ｆx）を一定値とした場合のＦy，Ｍzの組の実現可能範囲を示している。また、図７Ｂ、図８Ｂ、図９Ｂ、図１０Ｂ、図１１Ｂ、図１２Ｂは、当該接触作用力実現可能範囲の全体のうち、Ｆx，Ｆy以外の成分（ここでは、成分Ｍz）を一定値とした場合のＦx，Ｆyの組の実現可能範囲を示している。また、図７Ｃ、図８Ｃ、図９Ｃ、図１０Ｃ、図１１Ｃ、図１２Ｃは、当該接触作用力実現可能範囲の全体のうち、Ｆx，Ｍz以外の成分（ここでは、成分Ｆy）を一定値とした場合のＦx，Ｍzの組の実現可能範囲を示している。

なお、ワークＡと周辺物体との各接触状態に対応する接触作用力実現可能範囲は、一般には、接触作用力の全ての成分を変数成分とする多次元空間における多面体領域である。

次に、制御装置１０によるワークＡの接触状態の推定処理を具体的に説明する。制御装置１０は、マニピュレータ１のハンド部２により把持した各ワークＡを、収容箱Ｂ内の目標位置に移動させる過程で、図１３のフローチャートに示す処理を実行する。なお、以降の説明では、収容箱Ｂ内に移動させようとしているワークＡを対象ワークＡという。

制御装置１０はＳＴＥＰ１において、接触状態を推定すべきタイミングであるか否かを判断する。本実施形態では、例えば、対象ワークＡを収容箱Ｂ内に移動させる過程で、該対象ワークＡが収容箱Ｂに最初に接触した時に、ＳＴＥＰ１の判断結果が肯定的になる。この場合、対象ワークＡが収容箱Ｂに接触した否かは、例えば力センサ５の検出信号により示される接触作用力の計測値に基づいて判断することができる。

そして、対象ワークＡと収容箱Ｂとの最初の接触後は、ワークＡが収容箱Ｂ内の目標位置に達するまでに、力センサ５の検出信号により示される接触作用力の計測値の変化等に応じた所定のタイミングでＳＴＥＰ１の判断結果が肯定的になる。

ＳＴＥＰ１の判断結果が否定的である場合には、制御装置１０は、ＳＴＥＰ１の判断処理を逐次繰り返す。そして、ＳＴＥＰ１の判断結果が肯定的になると、制御装置１０は、ＳＴＥＰ２において、対象ワークＡの移動を一時的に停止した状態（対象ワークＡの位置及び姿勢を一定に維持した状態）で、計測値取得部１１により、力センサ５の検出信号により示される接触作用力の計測値（現在値）を取得する。

さらに、制御装置１０は、ＳＴＥＰ３において、対象ワークＡの現在の接触状態の候補を決定する。この場合、制御装置１０は、対象ワークＡを収容すべき収容箱Ｂ内の目標位置に対応して接触状態情報記憶部１３に記憶保持されている全ての種類の接触状態のうち、現在時刻では実現の可能性が無いとみなし得る接触状態を、該対象ワークＡの接触状態の推定履歴（ワークＡの移動開始から現在時刻までの推定履歴）と、該対象ワークＡに関して接触状態情報記憶部１３に記憶保持されている接触状態相互間の遷移情報とに基づいて特定する。そして、制御装置１０は、当該特定した接触状態を、対象ワークＡについての全ての種類の接触状態から除外することで、現在の接触状態の候補を決定する。

例えば、対象ワークＡが、収容箱Ｂ内の前記目標位置ＴＰ(i)まで移動させるワークＡ(i)である場合、該ワークＡ(i)が収容箱Ｂに最初に接触した時点で、前記接触状態３Ａ，３Ｂ，４Ａ，４Ｂ，５Ａ，５Ｂの６種類の接触状態が候補として決定される。

次いで、ＳＴＥＰ４において、制御装置１０は、接触状態の各候補に対応して接触状態情報記憶部１３に記憶保持されている接触作用力範囲情報により示される接触作用力実現可能範囲を取得する。

さらに、制御装置１０は、ＳＴＥＰ５において、接触状態の全ての候補のそれぞれに対応する接触作用力実現可能範囲から、１つの接触状態を一意に特定し得る接触作用力の範囲（以降、一意特定範囲という）を抽出する。

該一意特定範囲は、接触状態の全ての候補のうちの１つの接触状態でのみ実現可能な接触作用力の範囲である。換言すれば、該一意特定範囲は、接触状態の全ての候補のうちの１つの接触状態に係る接触作用力実現可能範囲に属する範囲であると共に、他の各接触状態に係る接触作用力実現可能範囲に属さない範囲である。

すなわち、接触状態の各候補に係る接触作用力実現可能範囲に属する接触作用力の全体の集合をＳ(1)，Ｓ(2)，…，Ｓ(N)（Ｎ：候補の総数）、上記１つの接触状態に係る接触作用力の全体集合をＳ(1)とおくと、上記一意特定範囲は、Ｓ(1)−（Ｓ(1)∩（Ｓ(2)∪…∪Ｓ(N)））という集合の範囲である。

例えば、前記ワークＡ(i)が対象ワークＡであり、且つ、ＳＴＥＰ３において、前記接触状態３Ａ，３Ｂ，４Ａ，４Ｂ，５Ａ，５Ｂの６種類の接触状態が候補として決定された場合、図１４Ａに示す如く、接触状態４Ｂに係る接触作用力実現可能範囲のうちの一部の範囲（図１４Ａの斜線領域の範囲）が一意特定範囲として抽出される。なお、図１４Ａに示す一意特定範囲は、Ｆxの値がある一定値である場合のＦy、Ｍzの組についての範囲である。

次いで、制御装置１０は、前記ＳＴＥＰ２で取得した接触作用力の計測値が上記一意特定範囲内の計測値であるか否かをＳＴＥＰ６で判断する。

このＳＴＥＰ６の判断結果が肯定的である場合には、対象ワークＡの実際の接触状態は、基本的には、上記一意特定範囲に対応する候補の接触状態（詳しくは、上記一意特定範囲を接触作用力実現可能範囲に含む接触状態）である可能性が高い。

ただし、接触作用力の計測値が一意特定範囲の境界付近の値である場合（一意特定範囲に対応する候補と異なる候補の接触状態に対応する接触作用力実現可能範囲に近い値である場合）等においては、接触作用力の計測誤差、あるいは、マニピュレータ１の先端部のハンド部２に対する対象ワークＡの実際の位置又は姿勢のばらつき等に起因して、実際の接触状態が、一意特定範囲に対応する候補の接触状態でないのに、接触作用力の計測値が該一意特定範囲に属してしまう場合もあり得る。

そこで、制御装置１０は、ＳＴＥＰ６の判断結果が肯定的である場合には、さらに、ＳＴＥＰ７において、接触状態の各候補の尤度を算出する。該尤度は、各候補が、実際の接触状態であることの確からしさの度合いを示す指標値である。

本実施形態では、各候補の尤度を表す指標値として、例えば、次式（３）により定義される評価関数ＦＥ１(↑ｆ)を用いて、式（４）により算出される値が採用される。

ＦＥ１(↑ｆ)＝（↑Ｆs−Ｇmat・↑ｆ)^T・Ｄ1・(↑Ｆs−Ｇmat・↑ｆ)) ……（３）
尤度＝exp(−ＦＥ１(↑ｆe)) ……（４）

ここで、式（３）における↑Ｆsは、接触作用力の計測値、Ｄ１はあらかじめ設定された重み行列（対角行列）、Ｇmatは、各候補の接触状態に関する前記式（１）の右辺の行列である。また、式（４）における↑ｆeは各候補の接触状態に関する前記式（２）の不等式により表される制約条件の下で、式（３）の評価関数ＦＥ１(↑ｆ)の値が最小になる接触反力↑ｆの値、ＦＥ１(↑ｆe)は↑ｆ＝↑ｆeであるときの評価関数ＦＥ１(↑ｆ)の値である。

なお、exp( )は、自然対数の底の指数関数、上付き添え字「Ｔ」は転置を意味する。また、重み行列Ｄ１としては、例えば単位行列を採用し得る。この場合には、式（３）の右辺は、ベクトル(↑Ｆs−Ｇmat・↑ｆ)の２乗値となる。

上記評価関数ＦＥ１(↑ｆ)は、接触反力↑ｆの任意の値に対応する接触作用力↑Ｆの値（＝Ｇmat・↑ｆ）と、接触作用力の計測値↑Ｆsとの偏差度合（偏差の大きさの度合）を表す関数であり、その関数値が小さいほど（ゼロに近いほど）、当該偏差度合が小さいことを意味する。

従って、↑ｆeは、換言すれば、接触状態の各候補に関する前記式（２）の制約条件を満たす接触反力↑ｆの値のうち、その値に対応する接触作用力↑Ｆの値（＝Ｇmat・↑ｆ）が、計測値↑Ｆsに対して最も偏差度合が小さい（換言すれば最も一致度合が高い）ものとなる接触反力↑ｆの値となる。

そして、かかる↑ｆeに対応する↑Ｆの値（＝Ｇmat・↑ｆe）と、計測値↑Ｆsとの一致度合を表す指標値が、前記式（４）により算出される尤度の値である。この場合、式（４）により算出される尤度は、↑ｆeに対応する↑Ｆの値（＝Ｇmat・↑ｆe）と、計測値↑Ｆsとの偏差度合が小さいほど、大きくなる。

本実施形態では、ＳＴＥＰ７において、制御装置１０は、接触状態の各候補毎に、上記↑ｆeの値（評価関数ＦＥ１(↑ｆ)の値が最小になる接触反力↑ｆの値）を、例えば、制約付き線形最小二乗問題の公知の解法手法により算出する。そして、制御装置１０は、この↑ｆeの値に対応する評価関数ＦＥ１の値（＝ＦＥ１(↑ｆe)）を用いて、前記式（４）の演算により接触状態の各候補毎の尤度を算出する。

次いで、制御装置１０は、ＳＴＥＰ８において、接触状態の各候補のうち、前記一意特定範囲に対応する候補に関して求めた尤度が、他の候補に関して求めた尤度よりも所定量以上高いか否かを判断する。なお、このＳＴＥＰ８の判断処理は、前記一意特定範囲に対応する候補に関して求めた↑ｆeに対応する↑Ｆの値（＝Ｇmat・↑ｆe）と、計測値↑Ｆsとの偏差度合（又はＦＥ1(↑ｆe)）が、他の候補に関して求めた↑ｆeに対応する↑Ｆの値（＝Ｇmat・↑ｆe）と、計測値↑Ｆsとの偏差度合（又はＦＥ1(↑ｆe)）よりも所定量以上小さいか否かを判断する処理と等価である。

そして、制御装置１０は、ＳＴＥＰ８の判断結果が肯定的である場合には、ＳＴＥＰ９において、前記一意特定範囲に対応する候補を実際の接触状態として推定する。

例えば、対象ワークＡが前記ワークＡ(i)である場合において、ＳＴＥＰ２で取得された接触作用力の計測値が、図１４Ａの点Ｐ１に対応する計測値であり、且つ、実際の接触状態が前記接触状態４Ｂである場合を想定する。この場合、接触状態４Ｂに関する一意特定範囲に対して、ＳＴＥＰ６の判断結果が肯定的となると共に、ＳＴＥＰ８の判断結果が肯定的となる。この場合には、ＳＴＥＰ９において、接触状態４Ｂが、実際の接触状態として推定されることとなる。

なお、ＳＴＥＰ９の処理の後は、制御装置１０は、ＳＴＥＰ１からの処理を再び実行する。

一方、ＳＴＥＰ８の判断結果が否定的である場合には、前記一意特定範囲に対応する候補は、実際の接触状態ではないとみなし得る。この場合には、制御装置１０は、ＳＴＥＰ１３において、ＳＴＥＰ３で決定した接触状態の候補から、前記一意特定範囲に対応する候補を削除する。

次いで、ＳＴＥＰ１４において、制御装置１０は、残りの候補が１つであるか否かを判断する。このＳＴＥＰ１４の判断結果が肯定的である場合には、制御装置１０は、ＳＴＥＰ１５において、残りの１つの候補を実際の接触状態として推定する。

なお、ＳＴＥＰ１５の処理の後は、制御装置１０は、ＳＴＥＰ１からの処理を再び実行する。

ＳＴＥＰ１４の判断結果が否定的である場合には、制御装置１０は、残りの候補に関して、ＳＴＥＰ５からの処理を再び実行する。

前記ＳＴＥＰ６の判断結果が否定的である場合には、制御装置１０は、ＳＴＥＰ１０において、接触作用力の計測値を、ＳＴＥＰ５で抽出された一意特定範囲内に変化させるように、対象ワークＡの位置姿勢（詳しくは、位置及び姿勢の一方又は両方）を調整する。

具体的には、制御装置１０は、前記一意特定範囲内のうちの所定の代表点（例えば中心点）における接触作用力を目標値として設定する。さらに、制御装置１０は、この目標値と接触作用力の計測値（現在値）との偏差に応じて、該偏差をゼロに近づけるように対象ワークＡの位置姿勢の調整量を決定する。該調整量は微小な調整量である。そして、制御装置１０は、決定した調整量だけワークＡの位置姿勢を現在の位置姿勢から修正するように、マニピュレータ１を制御する。これにより接触作用力の計測値を、前記一意特定範囲内に変化させるように、対象ワークＡの位置姿勢が調整される。

次いで、ＳＴＥＰ１１において、制御装置１０は、力センサ５の検出信号に基づく接触作用力の計測値（対象ワークＡの位置姿勢の調整後の計測値）を取得した後、ＳＴＥＰ１２において、対象ワークＡの位置姿勢を、調整前の元の位置姿勢に復帰させる（マニピュレータ１の各関節の変位量を、対象ワークＡの位置姿勢の調整前の変位量に戻す）ようにマニピュレータ１を制御する。

そして、制御装置１０は、前記ＳＴＥＰ７からの処理を実行する。この場合、ＳＴＥＰ７の処理では、ＳＴＥＰ１１で取得した計測値を用いて、各候補の尤度が算出される。

ここで、対象ワークＡの実際の接触状態が、一意特定範囲に対応する候補の接触状態である場合には、ＳＴＥＰ１０の処理によって、接触作用力の計測値が、一意特定範囲内の値となる。従って、この場合には、ＳＴＥＰ８の判断結果が肯定的になる。ひいては、該一意特定範囲に対応する候補が、実際の接触状態として推定されることとなる。

例えば、対象ワークＡが前記ワークＡ(i)である場合において、ＳＴＥＰ２で取得された接触作用力の計測値が、図１４Ａの点Ｐ２ａに対応する計測値であり、且つ、実際の接触状態が前記接触状態４Ｂである場合を想定する。この場合、接触状態４Ｂに関する一意特定範囲に対して、ＳＴＥＰ６の判断結果が否定的となるものの、ＳＴＥＰ１０の処理の後に、ＳＴＥＰ１１で取得される接触作用力の計測値は、例えば図１４Ａの点Ｐ２ｂに対応する計測値に変化する。ひいては、ＳＴＥＰ８の判断結果が肯定的となる。この場合には、ＳＴＥＰ９において、接触状態４Ｂが、実際の接触状態として推定されることとなる。

一方、対象ワークＡの実際の接触状態が、一意特定範囲に対応する候補の接触状態でない場合には、ＳＴＥＰ１０の処理を実行しても、接触作用力の計測値を、一意特定範囲内の値に変化させることができない。従って、この場合には、ＳＴＥＰ８の判断結果が否定的となる。ひいては、該一意特定範囲に対応する候補は、ＳＴＥＰ１３において、実際の接触状態の候補から除外されることとなる。

例えば、対象ワークＡが前記ワークＡ(i)である場合において、ＳＴＥＰ２で取得された接触作用力の計測値が、図１４Ａの点Ｐ２ａに対応する計測値であり、且つ、実際の接触状態が前記接触状態４Ｂ以外の接触状態である場合を想定する。この場合、接触状態４Ｂに関する一意特定範囲に対して、ＳＴＥＰ６の判断結果が否定的となる。そして、ＳＴＥＰ１０の処理を実行しても、ＳＴＥＰ１１で取得される接触作用力の計測値は、接触状態４Ｂに関する一意特定範囲内まで変化することができない。ひいては、ＳＴＥＰ８の判断結果が否定的となる。この場合には、ＳＴＥＰ１３において、接触状態４Ｂが、実際の接触状態の候補から削除されることとなる。

なお、対象ワークＡが前記ワークＡ(i)である場合において、ＳＴＥＰ１３において、接触状態４Ｂが、実際の接触状態の候補から削除された場合におけるＳＴＥＰ５の処理では、例えば、図１４Ｂに示す如く、接触状態５Ｂに関する一意特定範囲（図示例ではＦy，Ｍzの組に関する一意特定範囲）を抽出することができる。さらに、ＳＴＥＰ１３において、接触状態４Ｂ及び接触状態５Ｂが、実際の接触状態の候補から削除された場合におけるＳＴＥＰ５の処理では、例えば、図１４Ｃに示す如く、接触状態３Ａに関する一意特定範囲（図示例ではＦx，Ｍzの組に関する一意特定範囲））を抽出することができる。

さらに、ＳＴＥＰ１３において、接触状態４Ｂ、接触状態５Ｂ及び接触状態３Ａが、実際の接触状態の候補から削除された場合におけるＳＴＥＰ５の処理では、例えば、図１４Ｄに示す如く、接触状態４Ａに関する一意特定範囲（図示例ではＦx，Ｆyの組に関する一意特定範囲））を抽出することができる。さらに、ＳＴＥＰ１３において、接触状態４Ｂ、接触状態５Ｂ、接触状態３Ａ及び接触状態４Ａが、実際の接触状態の候補から削除された場合におけるＳＴＥＰ５の処理では、例えば、図１４Ｅに示す如く、接触状態５Ａに関する一意特定範囲（図示例ではＦy，Ｍzの組に関する一意特定範囲））を抽出することができる。

従って、最終的に対象ワークＡとしてのワークＡ(i)の実際の接触状態が、６種類の候補（接触状態３Ａ，３Ｂ，４Ａ，４Ｂ，５Ａ，５Ｂ）のうちのいずれの接触状態であるかを推定できる。

制御装置１０は、以上説明した処理によって、ＳＴＥＰ１の判断結果が肯定的になる毎に、対象ワークＡの接触状態を推定する。

なお、以上の説明では、対象ワークＡがワークＡ(i)である場合を代表例として説明したが、図６Ａ〜図６Ｆに示したワークＡ(j)等、他のワークＡについても上記と同様の仕方で接触状態を推定することができる。

ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、ＳＴＥＰ３〜１０、１２〜１５の処理が推定処理部１２の処理、ＳＴＥＰ２、１１の処理が計測値取得部１１の処理である。そして、前記評価関数ＦＥ１(↑ｆ)は、本発明における第１評価関数に相当し、ＦＥ１(↑ｆe)が本発明における評価指標値に相当する。

また、ＳＴＥＰ６の判断処理が、本発明における第１判断部の処理に相当し、ＳＴＥＰ７の処理内で、ＦＥ１(↑ｆe)の値を算出する処理が、本発明における評価指標値算出部の処理に相当し、ＳＴＥＰ６の判断結果が肯定的である場合に実行されるＳＴＥＰ８の処理が本発明における第２判断部の処理に相当し、ＳＴＥＰ１２の処理の後に実行されるＳＴＥＰ８の処理が、本発明における第３判断部の処理に相当する。

また、ＳＴＥＰ１０，１２の処理が、本発明におけるマニピュレータ制御部の処理に相当する。

以上説明した本実施形態によれば、複数のワークＡのそれぞれを収容箱Ｂに収容する作業をマニピュレータ１に行わせる場合に、各ワークＡの移動途中でのワークＡと周辺物体（本実施形態では、収容箱Ｂの側壁Ｂａ又は収容済の他のワークＡ）との実際の接触状態が、複数種類の接触状態の候補のうちのどの候補の接触状態であるかを、接触作用力の計測値と、複数種類の接触状態の候補のそれぞれに対応する接触作用力実現可能範囲とを用いて、該複数種類の候補から選択的に推定することができる。

このため、ワークＡと周辺物体との接触後に、ワークＡを収容箱Ｂ内の目標位置まで移動させるために、ワークＡをどのように移動させるべきかの方針が明確になる。従って、ワークＡと周辺物体との接触状態に適したパターンでワークＡの位置又は姿勢を適宜、変化させながら、該ワークＡを収容箱Ｂ内の目標位置まで移動させることができる。ひいては、ワークＡを周辺物体に過剰に押し付けてしまうようなこと等を防止しつつ、該ワークＡを滑らかに収容箱Ｂ内に収容することが可能となる。

［第２実施形態］
次に、本発明の第２実施形態を説明する。なお、本実施形態は、制御装置１０の一部の処理（詳しくは、ＳＴＥＰ７の処理）だけが前記第１実施形態と相違するものである。このため、第１実施形態と同一の事項については説明を省略する。

本実施形態では、前記ＳＴＥＰ７の処理において、制御装置１０は、評価関数ＦＥ１の代わりに、例えば次式（５）により定義される評価関数ＦＥ２(↑ｘ，↑θ，↑ｆ)を使用して、次式（６）により接触状態の各候補の尤度を算出する。

ＦＥ２(↑ｘ，↑θ，↑ｆ)
＝（Ｒ(↑ｘ，↑θ)・↑Ｆs−Ｇmat・↑ｆ)^T・Ｄ1
・(Ｒ(↑ｘ，↑θ)・↑Ｆs−Ｇmat・↑ｆ))
＋（↑ｘs−↑ｘ)^T・Ｄ2・(↑ｘs−↑ｘ))
＋（↑θs−↑θ)^T・Ｄ3・(↑θs−↑θ)) ……（５）
尤度＝exp(−ＦＥ２(↑ｘe，↑θe，↑ｆe)) ……（６）

ここで、式（５）における↑Ｆs，Ｇmatは、前記式（３）と同じである。また、↑ｘは、マニピュレータ１のアーム３の先端部に対するワークＡの相対位置を示すベクトル、↑θは、マニピュレータ１のアーム３の先端部に対するワークＡの相対姿勢を示すベクトル、Ｒ(↑ｘ，↑θ)は、↑ｘ及び↑θの値の組に応じて決定される座標変換行列、↑ｘsは↑ｘの誤差を含んだ計測値、↑θsは↑θの誤差を含んだ計測値、Ｄ1，Ｄ2，Ｄ3はそれぞれ重み行列（対角行列）である。なお、ワークＡの相対位置↑ｘ及び相対姿勢↑θとしては、例えば前記接触反力座標系で見たワークＡの相対位置及び相対姿勢を採用し得る。

そして、式（６）における↑ｘe，↑θe，↑ｆeは、各候補の接触状態に関する前記式（２）の不等式により表される制約条件の下で、式（５）の評価関数ＦＥ２(↑ｘ，↑θ，↑ｆ)の値が最小になる↑ｘ，↑θ，↑ｆの値の組、ＦＥ２(↑ｘe，↑θe，↑ｆe)は、↑ｘ＝↑ｘe、且つ↑θ＝↑θe、且つ↑ｆ＝↑ｆeであるときの評価関数ＦＥ２(↑ｘ，↑θ，↑ｆ)の値である。

重み行列Ｄ1，Ｄ2，Ｄ3としては例えば、単位行列を採用し得る。この場合には、式（４）の右辺は、ベクトル（Ｒ(↑ｘ，↑θ)・↑Ｆs−Ｇmat・↑ｆ)の２乗値と、（↑ｘs−↑ｘ)の２乗値と、（↑θs−↑θ)の２乗値との総和になる。

また、座標変換行列Ｒ(↑ｘ，↑θ)は、↑ｘ、↑θがそれぞれ、各接触状態に対応する接触作用力実現可能範囲をあらかじめ決定するときに想定されている基準位置、基準姿勢に一致するときに、単位行列となるように決定される。

なお、↑ｘe，↑θe，↑ｆeの値の組は、制約付き非線形最小二乗問題の公知の解法手法により算出することができる。

上記評価関数ＦＥ２(↑ｘ，↑θ，↑ｆ)は、接触反力↑ｆの任意の値に対応する接触作用力↑Ｆの値と、接触作用力の計測値↑Ｆsの座標変換値Ｒ(↑ｘ，↑θ)・↑Ｆsとの偏差度合（式（５）の右辺の第１項）と、ワークＡの相対位置及び相対姿勢の組↑ｘ，↑θの任意の値と計測値↑ｘs，↑θsの組との偏差度合（式（５）の右辺第２項及び第３項）との総和の偏差度合を表す関数であり、その関数値が小さいほど（ゼロに近いほど）、当該総和の偏差度合が小さいことを意味する。

従って、↑ｘe，↑θe，↑ｆeの組は、換言すれば、接触状態の各候補に関する前記式（２）の制約条件の下で、上記総和の偏差度合が最小となる↑ｘ，↑θ，↑ｆの値の組である。

上記評価関数ＦＥ２(↑ｘ，↑θ，↑ｆ)を用いて接触状態の各候補の尤度を算出することで、接触作用力座標系で見たワークＡの実際の位置姿勢のばらつき（基準の位置姿勢からのずれ度合）を反映させて、各候補の尤度を算出することができる。このため、該尤度の信頼性を高めることができる、ひいては、ワークＡの接触状態の推定結果の信頼性を高めることができる。

ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、前記評価関数ＦＥ２(↑ｘ，↑θ，↑ｆ)は、本発明における第２評価関数に相当し、ＦＥ２(↑ｘe，↑θe，↑ｆe)が本発明における評価指標値に相当する。また、ＳＴＥＰ７の処理内で、ＦＥ２(↑ｘe，↑θe，↑ｆe)の値を算出する処理が、本発明における評価指標値算出部の処理に相当する。

これ以外は、本実施形態と本発明との対応関係は、前記第１実施形態と同じである。

補足すると、各接触状態に対応する接触作用力実現可能範囲をあらかじめ決定した後に、ワークＡを実際にマニピュレータ１により移動させるときのワークＡの相対位置姿勢（マニピュレータ１のアーム３の先端部に対する相対位置及び相対姿勢の組）の目標値を、接触作用力実現可能範囲の決定時に想定されている基準位置姿勢から変更する場合があり得る。このような場合には、上記評価関数ＦＥ２における↑ｘs，↑θsの値として上記目標値（これは本発明における基準設定値に相当する）を採用し得る。

このようにした場合には、ワークＡの相対位置姿勢の目標値の変更に応じて、各接触状態に対応する接触作用力実現可能範囲を改めて作成することを省略することができる。

［変形態様］
次に、以上説明した第１実施形態及び第２実施形態に関連する変形態様をいくつか説明する。

以上説明した実施形態では、ワークＡを収容箱Ｂ内に収容する作業をマニピュレータ１に行わせる場合を一例として説明したが、本発明は、当該作業に限定されるものではない。例えば、任意のワークＡを物体に組み付ける作業等をマニピュレータに行わせる場合についても本発明を適用できる。

また、マニピュレータ１は、マスタ・スレーブ方式で作業者が操縦し得るマニピュレータであってもよい。この場合は、推定した接触状態を作業者に報知することで、作業者は、実際の接触状態に応じてワークの移動経路を調整することが可能となる。

また、前記ＳＴＥＰ８では、尤度を使用する代わりに、一意特定範囲に対応する候補に関して算出した評価関数の値（ＦＥ１(↑ｆe)又はＦＥ２(↑ｘe，↑θe，↑ｆe)）が、他の各候補に関して算出した評価関数の値（ＦＥ１(↑ｆe)又はＦＥ２(↑ｘe，↑θe，↑ｆe)）よりも、所定量以上、小さいか否かを判断するようにしてもよい。

前記第１実施形態に係る評価関数ＦＥ１として、その関数値が、↑ＦsとＧmat・↑ｆとの偏差度合が大きいほど、小さくなるように構成された評価関数を採用することもできる。同様に、前記第２実施形態に係る評価関数ＦＥ２として、その関数値が、↑ＦsとＧmat・↑ｆとの偏差度合と、↑ｘs，↑θsの組と、↑ｘ，↑θの組との偏差度合との総和の偏差度合が大きいほど、小さくなるように構成された評価関数を採用することもできる。

また、前記第１実施形態に係る評価関数ＦＥ１として、例えば、↑ＦsとＧmat・↑ｆとの偏差の絶対値に応じた関数を採用することもできる。同様に、前記第２実施形態に係る評価関数ＦＥ２として、例えば、↑ＦsとＧmat・↑ｆとの偏差の絶対値と、↑ｘs，↑θsのそれぞれのと、↑ｘ，↑θのそれぞれとの偏差の絶対値との総和に応じた関数を採用することもできる。

また、前記ＳＴＥＰ６の判断結果が肯定的である場合に、例えば、接触反力の計測値が、一意特定範囲（ＳＴＥＰ５で抽出された一意特定範囲）の境界から比較的離れた値であるような場合には、直ちに、該一意特定範囲に対応する候補を実際の接触状態として推定してもよい。

また、ＳＴＥＰ１２の処理の実行後に、ＳＴＥＰ８の判断処理に加えて、接触作用力の計測値が、ＳＴＥＰ１０で決定した目標値に一致もしくはほぼ一致する値まで変化したか否かを判断する処理を実行し、それらの両方の判断処理の判断結果、あるいは、いずれか一方の判断処理の判断結果が肯定的である場合に、一意特定範囲に対応する候補を実際の接触状態として推定することも可能である。

あるいは、ＳＴＥＰ１２の処理の実行後に、ＳＴＥＰ８の判断処理の代わりに、接触作用力の計測値が、ＳＴＥＰ１０で決定した目標値に一致もしくはほぼ一致する値まで変化したか否かを判断する処理を実行し、該判断処理の判断結果が肯定的である場合に、一意特定範囲に対応する候補を実際の接触状態として推定することも可能である。

なお、接触状態の推定結果の信頼性を高める上では、ＳＴＥＰ８の判断処理を含むことが望ましい。

１…マニピュレータ、１０…制御装置（マニピュレータ制御部）、１１…計測値取得部、１２…推定処理部（第１判断部、第２判断部、第４判断部、評価指標値算出部）、Ａ…ワーク。

Claims (8)

1. マニピュレータの先端部に保持されたワークを、該マニピュレータにより目標位置まで移動させる作業時に、前記目標位置の周辺に存在する物体である周辺物体に対するワークの接触状態を推定する装置であって、
   前記周辺物体に対する前記ワークの接触時に、前記周辺物体から前記ワークが受ける接触反力に応じて前記マニピュレータに作用する力である接触作用力の計測値を取得する計測値取得部と、
   前記周辺物体に対する前記ワークの接触状態として実現可能なものとしてあらかじめ定められた複数種類の接触状態のそれぞれ毎にあらかじめ作成された前記接触作用力の値の実現可能な範囲である接触作用力実現可能範囲と、前記接触作用力の計測値とに基づいて、前記複数種類の接触状態のうちのいずれかの接触状態を、前記周辺物体に対する前記ワークの実際の接触状態として推定する推定処理部とを備えており、
   前記推定処理部は、前記接触作用力の計測値が、前記複数種類の接触状態のうちの１つの接触状態に対応する前記接触作用力実現可能範囲にのみ属する値であるか否かを判断する第１判断部と、前記複数種類の接触状態のそれぞれ毎に、前記接触反力の任意の値に対応する前記接触作用力の値と前記計測値との偏差度合を表すように構成された所定の第１評価関数を用い、各接触状態で実現可能な前記接触反力の値の範囲を規定する所定の制約条件の下で、前記偏差度合が最小となる該接触反力の値を求めると共に、当該求めた接触反力の値に対応する前記第１評価関数の値である評価指標値を求める評価指標値算出部と、前記１つの接触状態に対応して求められた前記評価指標値により示される前記偏差度合が、当該１つの接触状態以外の各接触状態に対応して求められた前記評価指標値により示される前記偏差度合よりも小さいか否かを判断する第２判断部とを含み、前記第１判断部の判断結果が肯定的となり、且つ、前記第２判断部の判断結果が肯定的となる場合に、前記１つの接触状態を前記実際の接触状態として推定するように構成されていることを特徴とするワークの接触状態推定装置。
2. マニピュレータの先端部に保持されたワークを、該マニピュレータにより目標位置まで移動させる作業時に、前記目標位置の周辺に存在する物体である周辺物体に対するワークの接触状態を推定する装置であって、
   前記周辺物体に対する前記ワークの接触時に、前記周辺物体から前記ワークが受ける接触反力に応じて前記マニピュレータに作用する力である接触作用力の計測値を取得する計測値取得部と、
   前記周辺物体に対する前記ワークの接触状態として実現可能なものとしてあらかじめ定められた複数種類の接触状態のそれぞれ毎にあらかじめ作成された前記接触作用力の値の実現可能な範囲である接触作用力実現可能範囲と、前記接触作用力の計測値とに基づいて、前記複数種類の接触状態のうちのいずれかの接触状態を、前記周辺物体に対する前記ワークの実際の接触状態として推定する推定処理部とを備えており、
   
   前記推定処理部は、前記接触作用力の計測値が、前記複数種類の接触状態のうちの１つの接触状態に対応する前記接触作用力実現可能範囲にのみ属する値であるか否かを判断する第１判断部と、前記複数種類の接触状態のそれぞれ毎に、前記接触反力の任意の値に対応する前記接触作用力の値と前記計測値との偏差度合と、前記マニピュレータの先端部に対する前記ワークの任意の相対位置姿勢と該ワークの相対位置姿勢の所定の基準設定値又は計測値との偏差度合との総和の偏差度合を表すように構成された所定の第２評価関数を用い、各接触状態で実現可能な前記接触反力の値の範囲を規定する所定の制約条件の下で、前記総和の偏差度合が最小となる該接触反力の値と前記ワークの相対位置姿勢の値との組を求めると共に、当該求めた接触反力の値及び相対位置姿勢の値の組に対応する前記第２評価関数の値である評価指標値を求める評価指標値算出部と、前記１つの接触状態に対応して求められた前記評価指標値により示される前記総和の偏差度合が、当該１つの接触状態以外の各接触状態に対応して求められた前記評価指標値により示される前記総和の偏差度合よりも小さいか否かを判断する第２判断部とを含み、前記第１判断部の判断結果が肯定的となり、且つ、前記第２判断部の判断結果が肯定的となる場合に、前記１つの接触状態を前記実際の接触状態として推定するように構成されていることを特徴とするワークの接触状態推定装置。
3. 請求項１記載のワークの接触状態推定装置において、
   前記推定処理部は、前記第２判断部の判断結果が否定的である場合に、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するように構成されていることを特徴とする応じてワークの接触状態推定装置。
4. 請求項２記載のワークの接触状態推定装置において、
   前記推定処理部は、前記第２判断部の判断結果が否定的である場合に、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するように構成されていることを特徴とする応じてワークの接触状態推定装置。
5. 請求項１〜４のいずれか１項に記載のワークの接触状態推定装置において、
   前記推定処理部は、前記第１判断部の判断結果が否定的である場合に、前記接触作用力の計測値を、前記複数種類の接触状態うちのいずれか１つの接触状態に対応する接触作用力実現可能範囲にのみ属する値となるように決定した目標値に向かって変化させるように前記マニピュレータを作動させるマニピュレータ制御部と、該マニピュレータの作動により前記接触作用力の計測値が、前記目標値が属する接触作用力実現可能範囲内に変化したか否かを判断する第３判断部とをさらに含み、該第３判断部の判断結果が肯定的である場合に、前記目標値が属する接触作用力実現可能範囲に対応する接触状態を、前記実際の接触状態として推定するように構成されていることを特徴とするワークの接触状態推定装置。
6. 請求項５記載のワークの接触状態推定装置において、
   前記推定処理部は、前記第３判断部の判断結果が否定的である場合に、前記複数種類の接触状態から前記１つの接触状態を除外してなる残りの接触状態から前記実際の接触状態を推定する処理を実行するように構成されていることを特徴とする応じてワークの接触状態推定装置。
7. 請求項５又は６記載のワークの接触状態推定装置において、
   前記マニピュレータ制御部は、前記接触作用力の計測値を前記目標値に向かって変化させるように前記マニピュレータを作動させた後に、該マニピュレータを当該作動前の状態に復帰させるように構成されていることを特徴とするワークの接触状態推定装置。
8. 請求項１〜７のいずれか１項に記載のワークの接触状態推定装置において、
   前記複数種類の接触状態のそれぞれに対応する前記接触作用力実現可能範囲は、各接触状態で前記周辺物体から前記ワークが受ける接触反力と前記接触作用力との間の関係と、各接触状態で実現可能な当該接触反力の値の範囲を規定する所定の制約条件であって、前記周辺物体と前記ワークとの間の摩擦力に関する制約条件と前記周辺物体から前記ワークに作用する抗力の向きに関する制約条件とのうちの少なくとも一方の制約条件を含む制約条件とに応じて規定される範囲であることを特徴とするワークの接触状態推定装置。

Images