JP2021030435A - 孔探索方法及び孔探索装置 - Google Patents

Abstract

【課題】孔探索の精度と効率を向上する。【解決手段】孔探索の精度と効率を向上可能であるとともに、密集した孔端を対象とする孔探索配線タスクに適用可能な孔探索方法及び孔探索装置を提供する。孔探索方法は、複数の孔端のうちの標的孔に対する軸端の孔探索を行い、標的孔を取り囲む力制御範囲内に軸端が位置するよう、軸端を制御する孔探索ステップと、軸端に位置し、且つ軸端の中心点を通過する少なくとも１つの回動軸周りに軸端を回動させ、軸端に作用する軸方向力の変化を測定するとともに、軸方向力の変化に基づいて軸端と標的孔との接触状態を判定する接触状態判定ステップと、接触状態に基づいて、軸端に対し特定軌道のずれ補正を行うずれ補正ステップ、を含む。【選択図】図１

Description

本発明は、孔探索方法及び孔探索装置に関する。

自動化レベルの向上に伴って、配線タスクでは、ロボットによる自動配線の完了が関わるようになっている。この場合、ロボットによるケーブルと孔との孔探索及び組み付けの完了が関係してくるが、自動配線の精度や効率には孔探索戦略が大きな影響を及ぼす。

自動配線タスクにおける孔探索戦略は、軸−孔組み付け課題に分類される。しかし、配線タスクに存在する孔端の密集配置や、ケーブル先端部の端面構造の複雑さ、ケーブルが有する可撓性、及び何度もケーブルを挟持したり挿入したりせねばならないといった各種のランダムな不確定要素から、孔探索戦略は従来の軸−孔組み付け課題よりも接触状況がなお複雑であり、ノイズなどの干渉要因の影響を受けやすい。

従来の軸−孔組み付け課題では、例えば受動的コンプライアンス法を用い、コンプライアンス機構を利用して組み付けを補助している。しかし、接触情報によってロボットの運動を能動的に導けるわけではなく、環境情報に基づく能動的な位置・姿勢の調整は不可能なことから、接触状況が複雑な自動配線タスクへの適用は難しい。

このほか、例えば視覚に基づく孔探索と組み付けが用いられている。しかし、自動配線タスクでは孔端のサイズが小さく、配置も密集しているため、妨害やカメラの解像度不足、光線の多大な影響といった事態が発生しやすい。よって、完全に視覚に頼った孔探索は適用が難しい。

また、例えば、螺旋運動やランダム運動等の軌跡を予め設定することで孔探索と組み付けを行うブラインド探索法が用いられている。しかし、ロボット基端の運動軌跡が予め設定されているため、配置の密集した孔端の場合には孔への挿入ミスが発生しやすく、孔の探索時間が長くなるとの問題が発生する。

また、例えば、力情報に基づく定量演算法を用いた孔探索と組み付けが行われている。具体的には、センサで検出した力情報に軸と孔との幾何学的関係を組み合わせ、軸心と孔の中心との位置ずれを定量演算したあと、このずれをロボットにフィードバックすることで運動を修正する。しかし、配線タスクでは、ランダムな不確定要素や干渉要因が多く、力情報に歪みが生じて不正確となる結果、力情報から算出される位置ずれも不正確となる。そのため、力情報に基づく定量演算法も適用が難しい。

また、例えば、力情報に基づきリアルタイムで案内する接触状態識別法を用いた孔探索と組み付けが行われている（例えば、非特許文献１）。具体的には、まず、現時点の力情報から軸と孔との接触状態を識別することで、ずれの向きと調整方向を特定する。そして、力情報に基づきリアルタイムで案内しつつ、軸を何度も調整することで、運動を徐々に孔の中心に近付け、最終的に軸と孔を同心として組み付け条件を満たす。しかし、自動配線タスクでは、ケーブル端のサイズが過度に小さいことから、接触位置の違いによる静的力信号の差が明確でなく、ケーブルと孔との接触状況も複雑である。そのため、判断条件の探索が困難であり、接触状態の識別が難しい。且つ、複数回にわたるケーブルの挿入過程では何度もケーブルを挟持する必要があるが、ケーブルを挟持するたびに位置・姿勢が微妙に変化する。そのため、ケーブルが孔の同じ位置に接触したとしても力の値が異なり、アルゴリズムの安定性に極めて大きな影響を及ぼす。

Takahashi T, Ogata H, Muto S Y.A method for analyzing human assembly operations for use in automatically generating robot commands[C].IEEE International Conference on Robotics and Automation, 1993 Proceedings.695-700 vol.692.

非特許文献１等の上記従来技術では、配線タスクにおいて、軸線と孔との接触状況が複雑な場合に、接触状況の違いによる静的力情報の差が明らかでないといったランダムな不確定要素や干渉要因を考慮していない。そのため、孔探索の精度が低く、安定性や適応性が十分でないとの技術的課題が存在し、密集した孔端を対象とする孔探索配線タスクには適用が難しい。

そこで、本発明は、従来技術における上記技術的課題の少なくとも一つに対し、密集した孔端を対象とする孔探索配線タスクに適用できるよう、孔探索の精度と効率を向上可能な孔探索方法及び孔探索装置を提供することを目的とする。

そのため、本発明の実施形態は、複数の孔端のうちの標的孔に対する軸端の孔探索を対象とする孔探索方法を提供する。当該孔探索方法は、標的孔を取り囲む力制御範囲内に軸端が位置するよう、軸端を制御する孔探索ステップと、軸端に位置し、且つ軸端の中心点を通過する少なくとも１つの回動軸周りに軸端を回動させ、軸端に作用する軸方向力の変化を測定するとともに、軸方向力の変化に基づいて軸端と標的孔との接触状態を判定する接触状態判定ステップと、接触状態に基づいて、軸端に対し特定軌道のずれ補正を行うずれ補正ステップ、を含むことを特徴とする。

これにより、孔探索の精度と効率を向上可能である。

上記の孔探索方法において、例えば、ずれ補正ステップにおいて、軸方向力を検出し、軸方向力が所定の第１の閾値よりも小さくなるまで、軸端に対し特定軌道のずれ補正を行う。

これにより、孔探索方法の精度及び実行可能性が向上する。

上記の孔探索方法において、例えば、少なくとも１つの回動軸は、第１回動軸と、第１回動軸と直交する第２回動軸を含む。接触状態には、軸端と標的孔との第１回動軸方向におけるずれ及び第２回動軸方向におけるずれが含まれ、ずれ補正ステップにおいて、軸端に対し、ずれの向きに対応するずれ補正を行う。

これにより、複雑な接触状態に対応して正確な判定が可能となり、孔探索方法の適用性及び精度が向上する。

上記の孔探索方法において、例えば、接触状態判定ステップにおいて、軸端が未回動のときの軸方向力が第１の閾値よりも小さい場合には、ずれなし状態であると判定する。第１回動軸又は第２回動軸周りに軸端を２つの方向にそれぞれ回動させたとき、一方向への回動時に軸方向力が減少し、且つ他の方向への回動時に軸方向力が増大した場合には、接触状態が、標的孔に対して軸端が当該他の方向の側にのみずれている一方向ずれ状態であると判定する。第１回動軸又は第２回動軸周りに軸端を２つの方向にそれぞれ回動させたとき、２つの方向への回動時の軸方向力が、いずれも第１の閾値よりも大きな所定の第２の閾値を超えなかった場合には、接触状態が、標的孔に対して軸端が当該２つの方向のうちのいずれかの方向の側にのみずれてはいるが、ずれの向きが特定されない単軸ずれ状態であると判定する。第１回動軸及び第２回動軸周りに軸端を２つの方向にそれぞれ回動させたとき、軸方向力がいずれも第２の閾値を超えた場合には、接触状態が、第１回動軸方向のずれと第２回動軸方向のずれが存在する双方向ずれ状態であると判定する。

これにより、複数の接触状態を判定可能となり、複雑な接触状態に対応して正確な判定が実現されるため、適用性及び精度が向上する。

上記の孔探索方法において、例えば、接触状態が一方向ずれ状態の場合には、ずれ補正ステップにおいて、当該ずれの向きとは逆方向に軸端を移動させるずれ補正を行う。

これにより、効率的に孔探索を完了させられるため、孔探索効率が向上する。

上記の孔探索方法において、例えば、接触状態が単軸ずれ状態の場合には、ずれ補正ステップにおいて、当該ずれの存在方向に軸端を所定の距離だけ両側へとそれぞれ移動させるずれ補正を行う。所定の距離は、最大で、当該ずれの存在方向における力制御範囲の辺縁と標的孔の辺縁との間隔と等しく、間隔は軸端のサイズに基づき特定される。

これにより、複雑な接触状態に対応して正確且つ効果的に孔探索を完了可能であるとともに、挿入エラーの発生を回避できる。

上記の孔探索方法において、例えば、接触状態が双方向ずれ状態の場合には、ずれ補正ステップにおいて、第１回動軸方向に軸端を移動させ、所定の距離だけ移動させたあと、接触状態判定ステップに基づいて接触状態を再び判定する。これを、接触状態が、標的孔に対して軸端が第２回動軸方向にのみずれた状態であると判定されるまで繰り返す。

これにより、複雑な接触状態に対応して正確且つ効果的に孔探索を完了可能であるとともに、挿入エラーの発生を回避できる。

上記の孔探索方法において、例えば、孔探索方法は、凸多角形の軸端及び孔端を対象とし、少なくとも１つの回動軸は、軸端の中心点を通過し、且つ凸多角形の各辺と平行な複数の回動軸を含む。接触状態は、複数の回動軸の各々と直交する方向における軸端と標的孔とのずれを含み、ずれ補正ステップにおいて、軸端に対し、ずれの向きに対応するずれ補正を行う。

これにより、複雑な接触状態に対応して正確な判定が可能となり、孔探索方法の適用性及び精度が向上する。

上記の孔探索方法において、例えば、孔探索方法は、円形の軸端及び孔端を対象とし、少なくとも１つの回動軸は、軸端の円心を通過し、且つ互いに直交する２つの回動軸を含む。接触状態は、２つの回動軸各々の方向における軸端と標的孔とのずれを含み、ずれ補正ステップにおいて、軸端に対し、ずれの向きに対応するずれ補正を行う。

これにより、複雑な接触状態に対応して正確な判定が可能となり、孔探索方法の適用性及び精度が向上する。

また、本発明の実施形態は孔探索装置を更に提供する。当該孔探索装置は、複数の孔端のうちの標的孔に対する軸端の孔探索を対象とし、軸端を回動可能に保持する保持部と、軸端に作用する軸方向力を測定するセンサと、標的孔を取り囲む力制御範囲内に軸端が位置するよう軸端を制御する制御部であって、軸端に位置し、且つ軸端の中心点を通過する少なくとも１つの回動軸周りに軸端を回動させ、軸端に作用する軸方向力の変化を測定するとともに、軸方向力の変化に基づいて軸端と標的孔との接触状態を判定し、接触状態に基づいて、軸端に対し特定軌道のずれ補正を行う制御部、を含むことを特徴とする。

本発明の一態様によれば、孔探索の精度と効率を向上可能である。

図１は、本発明の一実施形態に係る孔探索装置の機能構造を示すブロック図である。 図２は、制御部が実行する孔探索方法を示すフローチャートである。 図３は、力制御範囲の具体例の１つを示す図である。 図４は、軸線が回動軸周りに回動する際の軸方向力の変化の原理を説明するための図である。 図５は、四角形の軸端と標的孔に発生し得る代表的な接触状態を示す図である。 図６Ａは、回動軸周りに軸線を回動させる際の軸方向力の変化規則の一状況を示すグラフである。 図６Ｂは、回動軸周りに軸線を回動させる際の軸方向力の変化規則の一状況を示すグラフである。 図６Ｃは、回動軸周りに軸線を回動させる際の軸方向力の変化規則の一状況を示すグラフである。 図６Ｄは、回動軸周りに軸線を回動させる際の軸方向力の変化規則の一状況を示すグラフである。 図７Ａは、左右回動時の動的軸方向力情報と接触状態との対応関係を示す表である。 図７Ｂは、上下回動時の動的軸方向力情報と接触状態との対応関係を示す表である。 図８は、右ずれ状態時のずれ補正ステップを示すフローチャートである。 図９は、左右ずれ状態時のずれ補正ステップを示すフローチャートである。 図１０は、双方向ずれ状態時のずれ補正ステップを示すフローチャートである。 図１１は、円形の軸端と孔端との接触状態を示す図である。 図１２は、六角形の軸端を示す図である。

以下に、図面を参照して本発明の具体的実施形態につき説明する。なお、以下の説明は本発明の理解の便宜上提示する例にすぎず、本発明の範囲を限定するものではない。具体的実施形態において、装置が備える部材は実際の状況に応じて変更、省略又は追加してもよい。また、方法のステップは、実際の状況に応じて順序を変更、省略、追加又は変更すればよい。

図１は、一実施形態に係る孔探索装置１の機能構造を示すブロック図である。

図１に示すように、一実施形態に係る孔探索装置１は、保持部１１、センサ１２、カメラ１３及び制御部１４を備える。また、図１には、孔探索装置１を利用して孔アレイ３の標的孔３１に対する軸線２の孔探索及び組み付けを実現する場合を示している。

保持部１１は軸線２を保持し、保持部１１の移動によって、孔アレイ３の標的孔３１に対する軸線２の孔探索及び組み付けを完了する。本実施形態では、保持部１１の姿勢を制御することで、軸線２が誤差の許容範囲内で孔アレイ３が位置する平面に対し略垂直となるよう、保持部１１により軸線２を保持する。図１に示すように、孔アレイ３が位置する平面をｙｚ平面とすると、軸線２はｙｚ平面と垂直なｘ方向に保持される。

保持部１１は例えば空気圧グリッパであり、圧縮空気を保持力として軸線２を挟持するとともに、タスクの必要性に応じて、複数回にわたる軸線の挟持及び挿入を完了可能である。且つ、機械力又は電動力等により保持部１１を制御することで、軸線２をｙｚ平面と平行に移動させつつ孔探索を実施可能とし、且つ、ｘ方向に対する一定の夾角範囲において軸線２を回動可能とする。また、保持部１１はこれに限らず、例えば、油圧式グリッパ、電動式グリッパ又はマニピュレータ等としてもよい。軸線２を保持可能であるとともに、軸線２をｙｚ平面と平行に移動させつつ孔探索を実施可能であり、且つ、ｘ方向に対する一定の夾角範囲において軸線２を回動させられればよい。

センサ１２はフォースセンサであり、軸線２が位置する方向（ｘ方向）において軸線２に作用する軸方向力を少なくとも検出する。センサ１２は、保持部１１に配置してもよいし、別個に装着されるフォースセンサとしてもよい。

カメラ１３は、例えば固定カメラ又は追従カメラとし、軸線２と孔アレイ３との位置関係を撮影して大まかな孔探索を実現するために用いられる。カメラ１３は、孔探索装置１内に設置してもよいし、別個に配置されるカメラとしてもよい。また、カメラ１３を設置せず、その他の方式で軸線２と孔アレイ３との位置関係を特定することで大まかな孔探索を実現してもよい。

制御部１４は、センサ１２が取得した力信号と、カメラ１３が取得した画像情報に基づき、特定のアルゴリズムに従って保持部１１を制御することで、標的孔３１に対する軸線２の孔探索及び組み付けを実現する。例えば、プロセッサやメモリ等を有するコンピュータ、或いは専用のハードウェア集積回路によって制御部１４の機能を実現すればよい。また、関連のプログラムを記録媒体に蓄積しておき、コンピュータのプロセッサで当該プログラムを実行することで制御部１４の機能を実現してもよい。制御部１４が実行する孔探索方法については後に詳述する。

軸線２は、例えば端面の直径が１〜２ｍｍで一定の可撓性を有するケーブルである。なお、軸線２のサイズや物理的特性はこれに限らず、例えば、端面の直径を３０〜４０ｍｍとしてもよいし、可撓性のケーブルではなく硬質の軸としてもよい。また、孔アレイ３寄りの軸線２の端面である軸端２１は、ｙｚ平面に対し略平行である。本実施形態では、図１に示すように、軸端２１の形状が四角形の場合を例示して説明する。

孔アレイ３には複数の孔が密集して配列されている。なお、孔の形状及び大きさは軸端２１の形状及び大きさと一致している。本実施形態では、孔の形状が四角形の場合を例示して説明する。複数の孔が密集して配列されているとは、例えば、孔アレイ３の孔端の間隔が軸端２１の直径（最大径）よりも小さいことをいう。なお、本実施形態は複数の孔が密集して配列される孔アレイの場合に限らない。本実施形態では、孔アレイ３の複数の孔がｙｚ平面に配置され、複数の孔の深さ方向がｘ方向であるとする。また、標的孔３１とは、複数の孔のうち軸線２との組み付けを待つ孔のことをいう。

次に、制御部１４により実行される孔探索方法について具体的に説明する。図２は、制御部１４が実行する孔探索方法を示すフローチャートである。

まず、ステップＳ１０１において、制御部１４は保持部１１が大まかな孔探索を完了するよう制御する。具体的には、標的孔３１を取り囲む力制御範囲内に軸端２１が位置するよう、保持部１１が保持する軸線２を制御する。力制御範囲とは、密集した孔端を対象に孔探索を行う際に挿入エラーが生じないよう、視覚による観察などで保持部１１を力制御するための精度範囲のことをいう。密集した孔端を対象として孔探索を行う際に力制御範囲を限定しておかないと、どの孔が標的孔３１かを判断できず、挿入エラーが発生する。例えば、軸端２１が左右２つの孔の間の仕切りに接触した場合、力覚では軸端２１が左側の孔の右辺縁に位置しているのか、右側の孔の左辺縁に位置しているのかを判断できない。本実施形態では、カメラ１３が取得した画像情報を視覚により観察して保持部１１を制御することで、軸端２１を力制御範囲内に位置させるよう制御する大まかな孔探索を実現する。なお、大まかな孔探索の実現方式としては任意の方式を用いればよい。例えば、カメラ１３を設置せず、光電センサ等のその他のデバイスで取得したパラメータ情報又は予め設定されているアルゴリズムによって大まかな孔探索を実現してもよい。

図３は、力制御範囲の具体例の１つを示す図である。図３に示す点線枠内の範囲が力制御範囲４１であり、実線枠が軸線２の挿入を待つ標的孔３１を示している。本図から明らかなように、力制御範囲とは、標的孔３１から外側に一定の範囲だけ延伸させて得られる標的孔３１を取り囲む範囲である。軸端２１の最外縁が力制御範囲４１内に入っていれば、実施形態で提示する密集した孔端を対象とする孔探索方法で孔探索を行うことが可能である。また、力制御範囲４１の辺縁と標的孔３１の辺縁との間隔は、例えば軸端２１のサイズに基づき決定される。図３に示す具体例では、ｂｗ、ｈｗがそれぞれ軸端２１の幅と高さを表す。また、α及びβは予め設定される係数である。孔探索方法の確かさを保証するために、好ましくは、αとβの値は０．５よりも小さく、より好ましくは、αとβの値を０．４とする。これより、図３に示す具体例において、標的孔３１の幅及び高さをそれぞれＬｔ及びＷｔとすると、好ましくは、力制御範囲４１の幅はＬｔ＋２＊０．４ｂｗ、力制御範囲の高さはＷｔ＋２＊０．４ｈｗである。

これにより、力制御範囲に基づく大まかな孔探索を実施することで、密集して配置される複数の孔を対象として孔探索を行う際に挿入エラーの発生を回避可能となるため、孔探索の精度と効率が向上する。

次に、動的力情報に基づいて力制御及び精密な孔探索を実施する。まず、ステップＳ１０２において、軸端２１と標的孔３１との接触状態を判定する。具体的には、軸端２１に位置し、且つ軸端２１の中心点を通過する少なくとも１つの回動軸周りに軸線２を２つの方向にそれぞれ回動させて、軸端２１に作用する軸方向力の変化を測定する。そして、軸方向力の変化に基づいて、軸端２１と標的孔との接触状態を判定する。

図４は、軸線２が回動軸周りに回動する際の軸方向力の変化の原理を説明するための図である。図４において、回動軸Ｏは軸端２１に位置し、且つ軸端２１の中心点を通過している。また、回動軸Ｏの方向はｚ方向（用紙と垂直な方向）と平行である。これにより、軸線２をｙ方向における２つの方向（＋ｙ及び−ｙ方向）に回動させる。軸線２が回動軸Ｏ周りに回動する際に、軸端２１と標的孔３１との接触線は軸端２１に対し軸方向（ｘ方向）の反作用力ｆを有する。下記式（１）で示すように、ｆの大きさは、端面と標的孔３１との接触位置における仮想変位δに正比例する。

ｆ＝Ｋδ＝ＫＲｓｉｎａ 式（１）

Ｋは軸端２１の軸方向剛性、Ｒは軸端２１の辺縁と標的孔３１との接触線から回動軸Ｏまでの距離、ａは仮想変位δに対する回動角度を表す。

図４において、軸線２は標的孔３１の右側（−ｙ方向）に位置している。よって、右側（−ｙ方向）に軸線２を回動させた場合には、偏向方向に軸端２１と標的孔３１との接触線が発生する。即ち、軸端２１と標的孔３１周りの仕切りとが接触・重畳している方向に偏向させることで、仮想変位δが増大し、これに応じて反作用力ｆ（即ち、センサ１２が検出するＸ軸の軸方向力）も増大する。一方で、左側（＋ｙ方向）に軸線２を回動させた場合には、偏向方向に軸端２１と標的孔３１との接触線は発生しない。即ち、軸端２１と標的孔３１周りの仕切りとが接触・重畳していない方向に偏向させることで、仮想変位δが減少し、これに応じて反作用力ｆ（即ち、センサ１２が検出するＸ軸の軸方向力）も減少する。このように、右側への回動時に軸方向力が次第に増大し、左側への回動時に軸方向力が次第に減少することを検出した場合には、軸線２が標的孔３１の右側に位置すると判定可能である。

上記から明らかなように、軸端２１と標的孔３１との接触状態によって、２つの方向に軸線２を回動させたときの軸方向力の変化は異なる。そのため、上記のような回動時の軸方向力の変化を検出することで、異なる接触状態を判定可能となる。例えば、図４に示すように、軸線２をｚ方向の回動軸周りに回動させることで、軸端２１と標的孔３１のｙ方向における接触状態を判定可能である。単一の静的力信号による判定と比較して、２つの方向への回動時の一連の力信号の変化の特徴から接触状態を判定する場合には、不確定要素や干渉要因等に起因して何らかの力信号に歪みが生じたり、差が明らかでなかったりするために正確な判定が不可能になるとの事態が回避される。よって、接触状態の判定精度及び安定性が向上する。

本実施形態では、所定の規則に従って２つの方向へ回動させる場合を考える。具体的には、まず、未回動の初期状態から反時計回り方向にａ°回動させてから、逆方向である時計回り方向に２ａ°回動させ、最後に再び反時計回り方向にａ°回動させることで最初の軸線２の姿勢に戻す場合を考える。なお、角度ａ°は例えば１０°と等しくする。また、回動方式はこれに限らず、例えば、先に時計回り方向に回動させる等のその他の回動方式としてもよい。

また、複数の異なる接触状態を判定可能とするために、好ましくは、回動軸は互いに直交する２つの回動軸（第１回動軸及び第２回動軸）を含み、当該２つの回動軸の各々周りに軸線２を２つの方向にそれぞれ回動させることで、当該２つの回動軸方向における軸端２１と標的孔３１との接触状態を判定する。

本実施形態では、軸端２１及び標的孔３１の形状が四角形であって、２つの回動軸が四角形の中心を通過しており、且つ、ｙ軸及びｚ軸とそれぞれ平行な場合を例示して説明する。

図５は、四角形の軸端２１と標的孔３１に発生し得る代表的な接触状態を示す図である。また、図６Ａ、図６Ｂ、図６Ｃ及び図６Ｄは、それぞれ回動軸周りに軸線２を回動させる際の軸方向力の変化規則の一状況を示すグラフである。以下では、ｙ方向を左右方向、ｚ方向を上下方向とする。具体的には、＋ｙ方向を左方向、−ｙ方向を右方向、＋ｚ方向を上方向、−ｚ方向を下方向とする。図５の（ａ）は、軸端２１が標的孔３１の左側に位置する左ずれ状態を示す。ｚ方向の回動軸周りにまず左へａ°回動させてから（時間ｔ１〜ｔ２）、右へ２ａ°回動させ（時間ｔ２〜ｔ４）、最後に左へａ°回動させる（時間ｔ４〜ｔ５）左右回動過程を考えると、軸方向力の変化は図６Ａのようになる。即ち、左への回動時には仮想変位δが増大するため軸方向力が増大し、右への回動時には仮想変位δが減少するため軸方向力が減少する。図５の（ｂ）は、軸端２１が標的孔３１の右側に位置する右ずれ状態を示す。同様の原理で、左右回動過程において、軸方向力の変化は図６Ｂに示すように、まず小さくなってから大きくなり、最後に再び減少する。図５の（ｃ）は、軸端２１が標的孔３１の上側に位置する上ずれ状態を示す。同様の原理で、ｙ方向の回動軸周りにまず上へａ°回動させてから（時間ｔ１〜ｔ２）、下へ２ａ°回動させ（時間ｔ２〜ｔ４）、最後に上へａ°回動させる（時間ｔ４〜ｔ５）上下回動過程において、軸方向力の変化は図６Ａに示すように、まず大きくなってから小さくなり、最後に再び増大する。図５の（ｄ）は、軸端２１が標的孔３１の下側に位置する下ずれ状態を示す。同様の原理で、上下回動過程において、軸方向力の変化は図６Ｂに示すように、まず小さくなってから大きくなり、最後に再び減少する。

このほか、上記の分析は、軸端２１の中心点付近が確実に標的孔３１の孔内に入り込むことを前提条件として行っている。しかし、複数の孔が密集して配置される孔アレイ３の場合には、軸端２１の中心点付近が孔内に入り込まない状態がある。即ち、当該中心点付近が標的孔３１の辺縁の仕切りに接触する状態がある。例えば、図５の（ａ）に示す左ずれ状態において、軸端２１の中心点付近が、標的孔３１と、標的孔３１の左側の孔との間の仕切りに接触する場合を想定する。この状態では、当該中心点を通過する回動軸周りに２つの方向へ回動させる際の軸方向力の差が大きくはならず、値も比較的小さくなる。この場合の軸方向力の変化は、例えば図６Ｃに示すように、左へａ°回動させたとき（時間ｔ１〜ｔ２）には仮想変位δが小幅で増大するため、軸方向力も小幅で増大する。続いて、右へａ°回動させたとき（時間ｔ２〜ｔ３）には仮想変位δが小幅で減少するため、軸方向力も小幅で減少する。そして、右へａ°回動させたとき（時間ｔ３〜ｔ４）には仮想変位δが小幅で増大するため、軸方向力も小幅で増大する。また、再び左へａ°回動させたとき（時間ｔ４〜ｔ５）には仮想変位δが小幅で減少するため、軸方向力も小幅で減少する。なお、当該状態における軸方向力の変化は図６Ｃに示すような状況に限らず、その他の変化規則となってもよい。ただし、２つの方向へ回動させる際の軸方向力の差が大きくはならず、値も比較的小さくなるとの点は共通する。また、図５の（ｂ）、（ｃ）、（ｄ）に示すようなずれ状態の場合に、軸端２１の中心点付近が標的孔３１の辺縁の仕切りに接触する場合にも、類似の軸方向力の変化状況が発生する。

また、図５の（ｅ）、（ｆ）、（ｇ）、（ｈ）は、それぞれ軸端２１が標的孔３１の左上方、右上方、左下方及び右下方に位置する双方向ずれ状態を示している。即ち、ｙ，ｚの両方向にずれが存在する状態を示している。この状態での軸方向力の変化は、例えば図６Ｄに示すように、左へａ°回動させたとき（時間ｔ１〜ｔ２）には仮想変位δが増大するため、軸方向力が増大する。続いて、右へａ°回動させて初期状態に戻した場合（時間ｔ２〜ｔ３）には仮想変位δが減少するため、軸方向力が減少する。そして、右へａ°回動させたとき（時間ｔ３〜ｔ４）には仮想変位δが増大するため、軸方向力が増大する。続いて、左へａ°回動させて初期状態に戻した場合（時間ｔ４〜ｔ５）には仮想変位δが減少するため、軸方向力が減少する。即ち、左回動及び右回動時の軸方向力の差は大きくないが、値はいずれも比較的大きな値まで増大する。また、当該状態における上下回動過程での軸方向力の変化は、左右回動過程の場合と類似している。つまり、図６Ｄに示すような変化規則となる。

このほか、図５の（ｉ）は、軸端２１が完全に標的孔３１内に入り込んだ状態を示している。即ち、ずれのない状態を示している。当該状態では、軸線２を回動する必要がなく、軸方向力が所定の閾値よりも小さくなる。つまり、これが孔探索完了の合図となる。以下では、ずれのない状態で軸方向力が所定の閾値よりも小さくなるときの当該閾値を第１の閾値と称する。当該第１の閾値は具体的状況に応じて適宜決定すればよい。

上記のように、軸線２を左右に回動及び上下に回動させて、軸端２１に作用する軸方向力を動的力情報として測定することで、複数の接触状態を判定可能となる。図７Ａ及び７Ｂは、左右回動時及び上下回動時の動的軸方向力情報と接触状態との対応関係を示す表である。図７Ａの上２行及び図７Ｂの上２行に示すように、左右回動過程又は上下回動過程で一方向への回動時に軸方向力が増大し、他方への回動時に軸方向力が減少した場合、即ち、図６Ａ又は図６Ｂに示すような軸方向力の変化状況が発生した場合には、軸端２１と標的孔３１との接触状態が図５の（ａ）、（ｂ）、（ｃ）、（ｄ）のいずれかに限られると判定可能である。即ち、一方向ずれ状態であると判定可能である。

また、図７Ａの３行目及び図７Ｂの３行目に示すように、左右回動過程又は上下回動過程で、２つの方向への回動時における軸方向力の値の差が大きくはなく、且ついずれも比較的小さな値である（第２の閾値を超えない）場合、即ち、図６Ｃに示すような軸方向力の変化状況が発生した場合には、軸端２１と標的孔３１が特定の回動軸に対応する回動方向においてのみずれを生じているが、具体的なずれの向きが左ずれなのか右ずれなのか（又は、上ずれなのか下ずれなのか）を特定することはできないと判定される。即ち、左右ずれ或いは上下ずれの単軸ずれ状態であると判定される。

また、図７Ａの４行目及び図７Ｂの４行目に示すように、上下回動過程又は左右回動過程で、２つの方向への回動時の軸方向力がいずれも比較的大きな値まで増大し得る（第２の閾値を超える）場合、即ち、図６Ｄに示すような軸方向力の変化状況が発生した場合には、当該回動方向と直交する回動方向にずれが存在する状態と判定される。また、上下回動過程と左右回動過程のいずれにおいても図６Ｄに示すような軸方向力の変化状況が発生した場合には、上下のずれと左右のずれが同時に存在すると判定される。即ち、双方向ずれ状態であると判定可能である。

また、第２の閾値は、図６Ｃと図６Ｄに示す軸方向力の変化状況を区別するために設定される。当該第２の閾値は第１の閾値よりも大きく、具体的状況に応じて適宜決定すればよい。

以上の説明より、上下回動又は左右回動のうち少なくとも１つの回動方式を利用するだけで、一方向ずれ状態及び単軸ずれ状態を判定可能である。また、上下回動及び左右回動の２種類の回動方式を利用するだけで、一方向ずれ状態、単軸ずれ状態及び双方向ずれ状態を含む複数の接触状態を判定可能である。よって、本実施形態によれば、接触状態の判定効率及び精度が極めて大きく向上する。

続いて、図２に戻り、ステップＳ１０３において、制御部１４はステップＳ１０２で判定した接触状態に基づき、保持部１１が軸端２１に対し特定軌道のずれ補正を実行するよう制御する。具体的には、接触状態の違いに応じて、制御部１４は異なるアルゴリズムを特定し、保持部１１が軸線２をｙｚ平面上で平行移動させるよう制御する。また、当該平行移動の過程で、センサ１２は軸端２１に作用する軸方向力を検出する。そして、当該軸方向力が所定の第１の閾値よりも小さくなると、標的孔３１に対する軸線２の孔探索が完了したと判定し、軸と孔の組み付けを行う。以下に、接触状態の違いに応じて実行される異なるずれ補正アルゴリズムについて具体的に説明する。

まず、Ｓ１０２において、接触状態が軸端２１と標的孔３１との間にずれが存在しないずれなし状態であると判定された場合、例えば図５の（ｉ）に示すような状態であると判定された場合には、ずれ補正をしなくとも孔探索が完了するため、軸と孔の組み付けをそのまま実行可能である。

また、Ｓ１０２において、接触状態が、軸端２１と標的孔３１との間に特定方向のずれのみが存在する一方向ずれ状態であると判定された場合、例えば、図５の（ａ）、（ｂ）、（ｃ）、（ｄ）のいずれかに示す状態であると判定された場合には、当該ずれの向きとは逆方向に軸線２を移動させるずれ補正を行う。以下に、右ずれの場合を例示してずれ補正の詳細なステップにつき説明する。図８は、右ずれ状態時のずれ補正ステップを示すフローチャートである。図８に示すように、ステップＳ２０１において、制御部１４は、保持部１１が軸線２を左方向へ所定の距離だけ移動させるよう制御する。当該所定の距離は、ずれ補正の精度要求に応じて適宜決定すればよい。続いて、ステップＳ２０２において、制御部１４は、センサ１２により検出された軸方向力が所定の第１の閾値よりも小さいか否かを判定する。センサ１２は、軸方向力を連続的に検出してもよいし、距離等の所定の頻度で断続的に軸方向力を検出してもよい。軸方向力が第１の閾値よりも小さくないと判定された場合（ステップＳ２０２で「ＮＯ」と判定された場合）には、ステップＳ２０１に戻る。一方、軸方向力が第１の閾値よりも小さいと判定された場合（ステップＳ２０２で「ＹＥＳ」と判定された場合）にはステップＳ２０３を実行し、孔探索が完了した旨の信号を出力するとともに、軸と孔の組み付けを行う。

なお、その他の一方向ずれ状態におけるずれ補正については、右ずれ状態の場合と類似する。

上記より、一方向ずれ状態における孔探索が効率よく完了するため、孔探索の効率が向上する。

また、Ｓ１０２において、接触状態が、軸端２１と標的孔３１との間に特定の回動軸に対応する回動方向のずれのみが存在するが、具体的な方向については特定されない単軸ずれ状態であると判定された場合、例えば、左右ずれ状態又は上下ずれ状態であると判定された場合には、当該ずれの存在方向において、軸線２を両側にそれぞれ所定の距離だけ移動させるずれ補正を行う。所定の距離は、最大で、当該ずれの存在方向における力制御範囲の辺縁と標的孔３１の辺縁との間隔と等しい。上述したように、ステップＳ１０１の大まかな孔探索では、挿入エラーを回避するために、制御部１４が、標的孔３１を取り囲む力制御範囲内に軸端２１を位置させるよう制御する。そのため、ステップＳ１０３のずれ補正では、限定の当該力制御範囲に基づいて軸線２を移動させるべきである。詳細には、具体的なずれの向きを特定できない場合に、当該ずれの存在方向における力制御範囲の辺縁と標的孔３１の辺縁との間隔を超えて軸端２１を一方向に移動させ、孔探索を完了すると、軸線２が標的孔３１周辺の別の孔に誤って挿入される恐れがある。また、力制御範囲を設定する際に、αとβの値が０．５に近付くほど単軸ずれ状態の発生の恐れが大きくなる。よって、力制御範囲を縮小することで、即ち、αとβの値を小さくすることで、標的孔３１の辺縁が軸端２１の中心付近に接触する際に、左右回動時又は上下回動時の動的力情報の違いが明らかでなくなるとの事態を回避可能とする。これにより、単軸ずれ状態の発生が減少ないしは回避される。

以下では、左右ずれ状態の場合を例示して、ずれ補正の詳細なステップにつき説明する。図９は、左右ずれ状態時のずれ補正ステップを示すフローチャートである。まず、ステップＳ３０１において、制御部１４は、保持部１１が軸線２を左方向へ所定の距離だけ移動させるよう制御する。当該所定の距離は、ずれ補正の精度要求に応じて適宜決定すればよい。当該左方向への移動は、具体的に左ずれなのか右ずれなのかを判定不可能なために実施される能動的探索動作であり、保持部１１が軸線２をまず右方向へ移動させるよう制御してもよい。続いて、ステップＳ３０２において、制御部１４は、センサ１２により検出された軸方向力が所定の第１の閾値よりも小さいか否かを判定する。そして、軸方向力が第１の閾値よりも小さくないと判定された場合（ステップＳ３０２で「ＮＯ」と判定された場合）には、ステップＳ３０３を実行する。一方、軸方向力が第１の閾値よりも小さいと判定された場合（ステップＳ３０２で「ＹＥＳ」と判定された場合）にはステップＳ３０７を実行し、孔探索が完了した旨の信号を出力するとともに、軸と孔の組み付けを行う。また、ステップＳ３０３において、制御部１４は、左方向への移動距離が距離Ｄを超えたか否かを判定する。当該距離Ｄは、左右方向における力制御範囲の辺縁と標的孔３１の辺縁との間隔と等しい。即ち、例えば図３に示すαｂｗの距離とする。制御部１４による移動距離の判定方式としては、例えば、当該所定の距離と距離Ｄとの比率関係に基づいて、複数回にわたり当該所定の距離だけ移動させた後に、移動距離が距離Ｄを超えたと判定する。ただしこれに限らず、例えば、距離センサ等により移動距離を判定してもよい。また、孔探索の効果を保証するために、当該所定の距離は距離Ｄよりも小さくなるよう適宜決定する必要がある。例えば、当該所定の距離は距離Ｄの１／１０と等しくする。左方向への移動距離が距離Ｄを超えていないと判定された場合（ステップＳ３０３で「ＮＯ」と判定された場合）には、ステップＳ３０１に戻る。一方、左方向への移動距離が距離Ｄを超えたと判定された場合（ステップＳ３０３で「ＹＥＳ」と判定された場合）には、ステップＳ３０４を実行する。

ステップＳ３０４において、制御部１４は、保持部１１が軸線２を右方向へ所定の距離だけ移動させるよう制御する。続いて、ステップＳ３０５において、制御部１４は、センサ１２により検出された軸方向力が所定の第１の閾値よりも小さいか否かを判定する。そして、軸方向力が第１の閾値よりも小さくないと判定された場合（ステップＳ３０５で「ＮＯ」と判定された場合）には、ステップＳ３０６を実行する。一方、軸方向力が第１の閾値よりも小さいと判定された場合（ステップＳ３０５で「ＹＥＳ」と判定された場合）にはステップＳ３０７を実行し、孔探索が完了した旨の信号を出力するとともに、軸と孔の組み付けを行う。また、ステップＳ３０６において、制御部１４は、右方向への移動距離が距離Ｄの２倍の距離２Ｄを超えたか否かを判定する。そして、右方向への移動距離が距離２Ｄを超えていないと判定された場合（ステップＳ３０６で「ＮＯ」と判定された場合）には、ステップＳ３０４に戻る。一方、右方向への移動距離が距離２Ｄを超えたと判定された場合（ステップＳ３０６で「ＹＥＳ」と判定された場合）には、今回のフローでは孔探索に成功しなかったことを意味し、フローを終了する。

また、上下ずれ状態等のその他の単軸ずれ状態におけるずれ補正については、左右ずれ状態の場合と類似する。

上記のように、具体的なずれの向きが特定されない単軸ずれ状態では、力制御範囲に基づき能動的探索を行うことで、効率的且つ正確な孔探索が可能になる。また、軸と孔との具体的接触状態を判定不可能な場合に孔探索方法を適用することができ、且つ、密集して配置される複数の孔を対象として孔探索を行う際に、挿入エラーの発生を回避可能となる。

また、Ｓ１０２において、接触状態が、互いに直交する２つの回動軸に対応する回動方向のずれが軸端２１と標的孔３１との間に存在する双方向ずれ状態であると判定された場合、例えば、図５の（ｅ）、（ｆ）、（ｇ）、（ｈ）のいずれかに示す状態であると判定された場合には、一方の回動軸方向に軸線２を移動させ、所定の距離だけ移動させたあとに、ステップ１０２の処理に基づいて接触状態を再び判定する。これを、標的孔３１に対して軸端２１が当該回動軸と直交する方向にのみずれた接触状態であると判定されるまで繰り返す。

以下に、ずれ補正の詳細なステップについて具体的に説明する。図１０は、双方向ずれ状態時のずれ補正ステップを示すフローチャートである。まず、ステップＳ４０１において、制御部１４は、保持部１１が軸線２を上方向へ所定の距離だけ移動させるよう制御する。当該上方向への移動は、具体的に上ずれなのか下ずれなのかを判定不可能なために実施される能動的探索動作であり、保持部１１が軸線２をまず下方向へ移動させるよう制御してもよい。続いて、ステップＳ４０２において、ステップＳ１０２と同様の方式で、軸線２を左右に回動させて軸端２１と標的孔３１との接触状態を判定する。具体的には、図７Ａの上３行に基づいて、接触状態が左ずれ、右ずれ又は左右ずれのいずれかであるか否かを判定する。続いて、ステップＳ４０３において、制御部１４は、ステップＳ４０２で判定した接触状態が、左ずれ、右ずれ又は左右ずれのいずれかであるか否かを判定する。即ち、左右方向のずれのみが存在するか否かを判定する。そして、左右方向のずれのみが存在するのではないと判定された場合（ステップＳ４０３で「ＮＯ」と判定された場合）には、ステップＳ４０４を実行する。一方で、左右方向のずれのみが存在すると判定された場合（ステップＳ４０３で「ＹＥＳ」と判定された場合）には、ステップＳ４０９を実行し、接触状態を左右方向のずれのみに変換する旨の信号を出力する。そして、接触状態が具体的に左ずれ、右ずれ又は左右ずれのいずれであるかに基づき、対応するずれ補正アルゴリズムに従ってずれ補正を行う。また、ステップＳ４０４において、制御部１４は、上方向への移動距離が距離Ｄを超えたか否かを判定する。そして、上方向への移動距離が距離Ｄを超えていないと判定された場合（ステップＳ４０４で「ＮＯ」と判定された場合）には、ステップＳ４０１に戻る。一方、上方向への移動距離が距離Ｄを超えたと判定された場合（ステップＳ４０４で「ＹＥＳ」と判定された場合）には、ステップＳ４０５を実行する。

ステップＳ４０５において、制御部１４は、保持部１１が軸線２を下方向へ所定の距離だけ移動させるよう制御する。続いて、ステップＳ４０６において、軸線２を左右に回動させて軸端２１と標的孔３１との接触状態を判定する。次に、ステップＳ４０７において、制御部１４は、ステップＳ４０６で判定した接触状態が、左ずれ、右ずれ又は左右ずれのいずれかであるか否かを判定する。即ち、左右方向のずれのみが存在するか否かを判定する。そして、左右方向のずれのみが存在するのではないと判定された場合（ステップＳ４０７で「ＮＯ」と判定された場合）には、ステップＳ４０８を実行する。一方で、左右方向のずれのみが存在すると判定された場合（ステップＳ４０７で「ＹＥＳ」と判定された場合）には、ステップＳ４０９を実行し、接触状態を左右方向のずれのみに変換する旨の信号を出力する。そして、接触状態が具体的に左ずれ、右ずれ又は左右ずれのいずれであるかに基づき、対応するずれ補正アルゴリズムに従ってずれ補正を行う。また、ステップＳ４０８において、制御部１４は、下方向への移動距離が距離２Ｄを超えたか否かを判定する。そして下方向への移動距離が距離２Ｄを超えていないと判定された場合（ステップＳ４０８で「ＮＯ」と判定された場合）には、ステップＳ４０５に戻る。一方、下方向への移動距離が距離２Ｄを超えたと判定された場合（ステップＳ４０８で「ＹＥＳ」と判定された場合）には、今回のフローでは孔探索に成功しなかったことを意味し、フローを終了する。

なお、上下方向に軸線２を移動させるとともに、左右方向に軸線２を回動させることで接触状態を判定し、接触状態を左右方向のずれのみが存在するよう変換する場合を例示して説明したが、左右方向に軸線２を移動させるとともに、上下方向に軸線２を回動させることで接触状態を判定し、接触状態を上下方向のずれのみが存在するよう変換してもよい。

上記より、互いに直交する２つの方向のずれが同時に存在する双方向ずれであると判定された場合には、能動的探索動作の実行過程に動的力信号に基づく接触状態の判定を組み合わせることで、能動的探索動作に必要な試行回数が減少するため、接触状態を効率よく一方向ずれ状態又は単軸ずれ状態に変換可能となる。また、その後、一方向ずれ状態又は単軸ずれ状態の場合のアルゴリズムに従ってずれ補正を実行可能である。これにより、ずれ補正を複雑な接触状態に適用可能となり、正確且つ効果的に孔探索を完了させられる。また、軸と孔との具体的接触状態を判定不可能な場合に孔探索方法を適用することができ、且つ、密集して配置される複数の孔を対象として孔探索を行う際に、挿入エラーの発生を回避可能となる。

上記の説明では、軸端２１及び孔端の形状が四角形の場合を例示して説明した。しかし、軸端２１及び孔端の形状はこれに限らず、孔端の形状と軸端２１の形状が一致しており、且つ、上記の孔探索方法を利用して孔探索及び組み付けを実行できればよい。

（第１変形例）
第１変形例の孔探索方法では、円形の軸端及び孔端を対象として孔探索を行う。なお、第１変形例のうち上記実施形態と同一の部分については、具体的な説明を省略又は簡略化する。第１変形例は、軸端及び孔端の形状を円形に変更した点で上記実施形態と異なる。

円形の軸端及び標的孔を対象として、まず、ステップＳ１０１において、標的孔を取り囲む力制御範囲内に軸端が位置するよう、保持部１１が保持する軸線を制御する。当該力制御範囲は、例えば標的孔と同心であり、且つ半径が標的孔よりも大きな範囲とする。また、アルゴリズムの安定性を高めるために、好ましくは、力制御範囲を、軸端の円心と標的孔の円心との位置ずれが軸端の半径よりも小さくなるよう設定し、より好ましくは、軸端の円心と標的孔の円心との位置ずれが軸端の半径の０．４倍よりも小さくなるよう設定する。

ステップＳ１０２において、軸端の円心を通過し、且つ互いに直交する２つの回動軸周りに軸線を回動させて、当該２つの回動軸各々の方向における軸端と標的孔とのずれを判定する。当該２つの回動軸は、例えばｙｚ平面における左右方向（ｙ方向）の回動軸と、上下方向（ｚ方向）の回動軸である。

図１１は、円形の軸端２１と孔３１の端との接触状態を示す図である。図１１に示すように、軸端２１の円心Ｏを回動中心とし、軸を順に左右回動及び上下回動させてから、左回動時と右回動時、上回動時と下回動時の力の変化規則をそれぞれ比較して接触状態を特定する。式（１）で示す仮想変位の原理に基づけば、円形の軸端が、標的孔の外側部分において、軸の回動による探索過程で障害物である孔と孔の間の仕切りと接触した場合には、比較的大きな仮想変位が発生するため、軸端に作用する軸方向力も比較的大きくなる。また、円形の軸端のうち孔内の部分の方向に向かって回動させた場合には、仮想変位が比較的小さくなるため、発生する軸方向力も比較的小さくなる。このように、例えば、ｙｚ平面の左右方向（ｙ方向）と上下方向（ｚ方向）の軸を回動軸として回動させることで、ｙ方向又はｚ方向における特定方向の一方向ずれ状態と、ｙ方向又はｚ方向における不特定方向の単軸ずれ状態、ｙ方向及びｚ方向の双方にずれが存在する双方向ずれ状態を判定可能となる。

また、軸と孔の円心位置のずれが軸端の半径よりも小さい場合には、軸端の円形の辺縁と回動軸とが交差する４つの交点のうち少なくとも１つが常に標的孔に入り込んでいる。孔の外側に位置するその他の交点は、軸の回動による探索過程において、孔の外側で障害物と接触して比較的大きな仮想変位を発生させるため、軸方向力も比較的大きくなる。これに対し、標的孔に入り込んだ交点の方向に向かって回動させた場合には、仮想変位が比較的小さくなるため、発生する作用力も比較的小さくなる。これにより、回動過程で発生する軸方向力が比較的小さくなる側を孔の中心方向、即ちずれ補正方向とみなすことができる。

ステップＳ１０３では、ステップＳ１０２で判定した接触状態に基づいて、ずれが存在すると判定された方向とは逆方向に軸線を移動させて孔探索を完了する。また、複数の方向にずれが存在すると判定された場合には、複数回にわたる接触状態の判定とずれ補正を行うことで、最終的に軸と孔との位置合わせを実現し、孔探索を完了する。

（第２変形例）
第２変形例の孔探索方法では、凸多角形の軸端及び孔端を対象として孔探索を行う。なお、第２変形例のうち上記実施形態又は第１変形例と同一の部分については、具体的な説明を省略又は簡略化する。第２変形例は、軸端及び孔端の形状を凸多角形に変更した点で上記実施形態と異なる。

まず、ステップＳ１０１において、標的孔を取り囲む力制御範囲内に軸端が位置するよう、保持部１１が保持する軸線を制御する。また、アルゴリズムの安定性を高めるために、好ましくは、当該凸多角形の軸端の少なくとも１つの辺が完全に孔内に位置するよう力制御範囲を設定する。

ステップＳ１０２では、軸端の中心を通過し、且つ互いに直交する２つの回動軸周りに軸線を回動させるだけでなく、軸端の中心を通過し、且つ凸多角形の各辺とそれぞれ平行な複数の回動軸周りに軸線を回動させることで、当該複数の回動軸の各々と直交する方向における軸端と標的孔とのずれを判定する。また、軸端が軸対称形状である凸多角形の軸と孔（例えば、六角形、八角形等）の場合には、半分の辺長方向及びその逆方向にのみ回動させて探索すればよい。これは、半分の辺長方向の逆方向が、残り半分の辺長が位置する方向となるためである。

以下に、六角形の軸端及び孔端の場合を例示して説明する。図１２は、六角形の軸端を示す図である。図１２に示すように、六角形軸の６つの頂点を、それぞれＰ１、Ｐ２、Ｐ３、Ｐ４、Ｐ５、Ｐ６とする。六角形の軸線には６本の辺が存在し、それぞれをＰ１２、Ｐ２３、Ｐ３４、Ｐ４５、Ｐ５６、Ｐ６１と称する。また、六角形軸の軸端の中心を通過する３本の回動軸を考える。このうち、２つの頂点Ｐ１とＰ４の接続線を回動軸Ｌ１、２つの頂点Ｐ２とＰ５の接続線を回動軸Ｌ２、２つの頂点Ｐ３とＰ６の接続線を回動軸Ｌ３と称する。各回動軸の垂直方向には２つの辺が存在する。例えば、回動軸Ｌ１の垂直方向には２つの辺Ｐ２３とＰ５６が存在し、回動軸Ｌ２の垂直方向には２つの辺Ｐ３４とＰ６１が存在し、回動軸Ｌ３の垂直方向には２つの辺Ｐ１２とＰ４５が存在する。軸端が孔の表面に接触したあと、ずれの向きを識別する際には、軸線を回動軸Ｌ１、Ｌ２、Ｌ３周りに３回ずつ回動させて力の変化規則を観察し、接触状態を判定する。

軸線を回動軸Ｌ１、Ｌ２、Ｌ３周りに３回ずつ回動させる際に、いずれかの回動軸周りの回動過程において、時計回りの回動時と反時計回りの回動時との動的力情報の違いが比較的大きい場合には、当該回動軸の垂直方向に位置ずれが存在することを意味する。式（１）で示す仮想変位の原理によれば、いずれかの回動軸周りに軸線を回動させる際に、回動軸の垂直方向におけるいずれかの辺が孔内に位置し、回動軸の垂直方向における他方の辺が孔の外側の仕切り上に位置する場合には、一方向への回動時に軸方向力が減少し、他方への回動時に軸方向力が増大する。例えば、図６Ａ又は図６Ｂに示すような軸方向力の変化曲線が発生した場合には、当該回動軸と直交する方向にずれが存在することを意味する。上記より、当該複数の回動軸の各々と直交する方向におけるずれを判定可能である。

ステップＳ１０３では、ステップＳ１０２で判定した接触状態に基づいて、ずれが存在すると判定された方向とは逆方向に軸線を移動させて孔探索を完了する。また、複数の方向にずれが存在すると判定された場合には、複数回にわたる接触状態の判定とずれ補正を行うことで、最終的に軸と孔との位置合わせを実現し、孔探索を完了する。

以上、図面を参照して本発明の実施形態につき説明した。上記で説明した実施形態は本発明の具体例にすぎず、本発明を理解するためのものであって、本発明の範囲を限定するものではない。当業者は、本発明の技術思想に基づいて、実施形態につき各種の変形、組み合わせ及び要素の合理的省略を実施可能であり、これらにより得られる方式もまた本発明の範囲に含まれる。

１ 孔探索装置、２ 軸線、３ 孔アレイ、１１ 保持部、１２ センサ、１３ カメラ、１４ 制御部、２１ 軸端、３１ 標的孔、４１ 力制御範囲

Claims (10)

1. 複数の孔端のうちの標的孔に対する軸端の孔探索方法であって、
   前記標的孔を取り囲む力制御範囲内に前記軸端が位置するよう、前記軸端を制御する孔探索ステップと、
   前記軸端に位置し、且つ前記軸端の中心点を通過する少なくとも１つの回動軸周りに前記軸端を回動させ、前記軸端に作用する軸方向力の変化を測定するとともに、前記軸方向力の変化に基づいて前記軸端と前記標的孔との接触状態を判定する接触状態判定ステップと、
   前記接触状態に基づいて、前記軸端に対し特定軌道のずれ補正を行うずれ補正ステップと、を含むことを特徴とする孔探索方法。
2. 前記ずれ補正ステップにおいて、前記軸方向力を検出し、前記軸方向力が所定の第１の閾値よりも小さくなるまで、前記軸端に対し特定軌道のずれ補正を行う請求項１に記載の孔探索方法。
3. 前記少なくとも１つの回動軸は、第１回動軸と、前記第１回動軸と直交する第２回動軸を含み、
   前記接触状態には、前記軸端と前記標的孔との前記第１回動軸の方向におけるずれ及び前記第２回動軸の方向におけるずれが含まれ、
   前記ずれ補正ステップにおいて、前記軸端に対し、前記ずれの向きに対応するずれ補正を行う請求項２に記載の孔探索方法。
4. 前記接触状態判定ステップにおいて、
   前記軸端が未回動のときの前記軸方向力が前記第１の閾値よりも小さい場合には、ずれなし状態であると判定し、
   前記第１回動軸又は前記第２回動軸周りに前記軸端を２つの方向にそれぞれ回動させたとき、一方向への回動時に前記軸方向力が減少し、且つ他の方向への回動時に前記軸方向力が増大した場合には、前記接触状態が、前記標的孔に対して前記軸端が前記他の方向の側にのみずれている一方向ずれ状態であると判定し、
   前記第１回動軸又は前記第２回動軸周りに前記軸端を２つの方向にそれぞれ回動させたとき、２つの方向への回動時の前記軸方向力が、いずれも前記第１の閾値よりも大きな所定の第２の閾値を超えなかった場合には、前記接触状態が、前記標的孔に対して前記軸端が前記２つの方向のうちのいずれかの方向の側にのみずれてはいるが、ずれの向きが特定されない単軸ずれ状態であると判定し、
   前記第１回動軸及び前記第２回動軸周りに前記軸端を２つの方向にそれぞれ回動させたとき、前記軸方向力がいずれも前記第２の閾値を超えた場合には、前記接触状態が、前記第１回動軸の方向のずれと前記第２回動軸の方向のずれが存在する双方向ずれ状態であると判定する請求項３に記載の孔探索方法。
5. 前記接触状態が前記一方向ずれ状態の場合には、前記ずれ補正ステップにおいて、前記ずれの向きとは逆方向に前記軸端を移動させるずれ補正を行う請求項４に記載の孔探索方法。
6. 前記接触状態が前記単軸ずれ状態の場合には、前記ずれ補正ステップにおいて、前記ずれの存在方向に前記軸端を所定の距離だけ両側へとそれぞれ移動させるずれ補正を行い、前記所定の距離は、最大で、前記ずれの存在方向における前記力制御範囲の辺縁と前記標的孔の辺縁との間隔と等しく、前記間隔は前記軸端のサイズに基づき特定される請求項４に記載の孔探索方法。
7. 前記接触状態が前記双方向ずれ状態の場合には、前記ずれ補正ステップにおいて、前記第１回動軸の方向に前記軸端を所定の距離だけ移動させたあとに前記接触状態判定ステップに基づいて前記接触状態を再び判定する移動判定ステップを実行し、前記移動判定ステップを、前記接触状態が、前記標的孔に対して前記軸端が前記第２回動軸の方向にのみずれた状態であると判定されるまで繰り返す請求項４に記載の孔探索方法。
8. 前記孔探索方法は、凸多角形の前記軸端及び前記孔端を対象とし、
   前記少なくとも１つの回動軸は、前記軸端の中心点を通過し、且つ前記凸多角形の各辺と平行な複数の回動軸を含み、
   前記接触状態は、前記複数の回動軸の各々と直交する方向における前記軸端と前記標的孔とのずれを含み、
   前記ずれ補正ステップにおいて、前記軸端に対し、前記ずれの向きに対応するずれ補正を行う請求項１〜７のいずれか１項に記載の孔探索方法。
9. 前記孔探索方法は、円形の前記軸端及び前記孔端を対象とし、
   前記少なくとも１つの回動軸は、前記軸端の円心を通過し、且つ互いに直交する２つの回動軸を含み、
   前記接触状態は、前記２つの回動軸の各々の方向における前記軸端と前記標的孔とのずれを含み、
   前記ずれ補正ステップにおいて、前記軸端に対し、前記ずれの向きに対応するずれ補正を行う請求項１〜７のいずれか１項に記載の孔探索方法。
10. 複数の孔端のうちの標的孔に対する軸端の孔探索装置であって、
    前記軸端を回動可能に保持する保持部と、
    前記軸端に作用する軸方向力を測定するセンサと、
    前記標的孔を取り囲む力制御範囲内に前記軸端が位置するよう前記軸端を制御する制御部であって、前記軸端に位置し、且つ前記軸端の中心点を通過する少なくとも１つの回動軸周りに前記軸端を回動させ、前記軸端に作用する軸方向力の変化を測定するとともに、前記軸方向力の変化に基づいて前記軸端と前記標的孔との接触状態を判定し、前記接触状態に基づいて、前記軸端に対し特定軌道のずれ補正を行う制御部と、を含むことを特徴とする孔探索装置。