JP2018024075A - 制御装置、ロボットおよびロボットシステム - Google Patents

Abstract

【課題】被挿入物に挿入物を挿入する際に失敗する確率を低減する。
【解決手段】可動部を有するロボットを制御する制御装置であって、前記可動部に設けられたエンドエフェクターにより挿入物を移動させ、被挿入物が有する挿入穴の中心軸に対して傾斜させて前記挿入物を前記挿入穴に接触させた後に前記挿入物を前記挿入穴に挿入する制御部を備える制御装置が構成される。
【選択図】図５

Description

本発明は、制御装置、ロボットおよびロボットシステムに関する。

ネジ締め動作を行う自動ネジ締め装置において、ネジ締めの際に受ける力のベクトルと同一線上にネジ締め機構のビットの目標姿勢を設定する構成が知られている（特許文献１、参照）。

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

従来の技術においては、ネジをネジ穴に挿入することが困難な場合があった。例えば、ネジ穴にテーパーが設けられている場合にネジがテーパーに接触し、力のベクトルがテーパーの法線方向になると、ネジがテーパーの法線方向に向くため、ネジをネジ穴に挿入することが困難だった。また、例えば、ネジ穴の中心軸（例えば、図９の線Ｄ：詳細は後述）上に他の部材が存在する場合、ネジ穴の中心軸上に沿ってネジやドライバーをネジ穴に近づけ、ネジをネジ穴に挿入することが困難であり、さらに、無理やり挿入させようとすると他の部材とドライバーとが干渉し、他の部材を破損させるおそれがあった。

上記課題の少なくとも一つを解決するために、本発明の制御装置は、可動部を有するロボットを制御する制御装置であって、前記可動部に設けられたエンドエフェクターにより挿入物を移動させ、被挿入物が有する挿入穴の中心軸に対して傾斜させて前記挿入物を前記挿入穴に接触させた後に前記挿入物を前記挿入穴に挿入する制御部を備える。

この構成によれば、被挿入物に挿入物を挿入する際に失敗する確率を低減することが可能である。例えば、中心軸上に他の部材が存在してもその部材を避けた位置を利用して挿入穴に挿入物を近づけ、挿入穴に挿入することが可能である。また、例えば、挿入穴としてのネジ穴の開口部にテーパーが形成されているとしても、テーパーの法線方向の力によって移動不能になることはなく、挿入物としてのネジをネジ穴に挿入することができる。

ロボットの斜視図である。 制御部の機能ブロック図である。 ドライバーのカバーおよびビットを示す図である。 ネジ挿入処理のフローチャートである。 ネジの挿入開始位置と姿勢を模式的に示す図である。 ネジの挿入時の動作を示す図である。 ネジに作用する力を示す図である。 挿入中のネジを示す図である。 傾斜したネジの挿入を行うワークの例を示す図である。 テーパーを有するネジ穴を中心軸に沿って見た状態を示す図である。 アームからＴＣＰに作用させる力を示す図である。

以下、本発明の実施形態について添付図面を参照しながら以下の順に説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
（１）ロボットの構成：
（２）制御装置の構成：
（３）ネジ挿入処理：
（４）教示位置：
（５）他の実施形態：

（１）ロボットの構成：
図１は本発明の一実施形態にかかるロボット１の斜視図である。図１に示すように、ロボット１は、アーム１０とエンドエフェクターとを備える。アーム１０は、３個の曲げ関節Ｂ１〜Ｂ３と３個のねじり関節Ｒ１〜Ｒ３とを有する６軸アームである。曲げ関節Ｂ１〜Ｂ３とは、アーム１０を構成する部材同士が、アーム１０の長さ方向に直交する軸を中心に回転する関節である。ねじり関節Ｒ１〜Ｒ３とは、アーム１０を構成する部材同士が、アーム１０の長さ方向の軸を中心に回転する関節である。アーム１０は、曲げ関節Ｂ１〜Ｂ３とねじり関節Ｒ１〜Ｒ３とを動作させるための駆動部としてのモーター（不図示）を備えている。

ドライバー２１を含むエンドエフェクターは、アーム１０の先端に装着されている。エンドエフェクターには、ドライバー２１とアーム１０との間に位置する力覚センサーＰが備えられている。力覚センサーＰは、当該力覚センサーＰに作用する３軸の力と、当該３軸まわりに作用するトルクに応じた力およびトルクを出力するセンサーである。本実施形態においては、力覚センサーＰの一方側の面（アーム１０と反対側の面）にはドライバー２１が備えられている。従って、ドライバー２１に作用する３軸の力と、３軸周りに作用するトルクに応じた力およびトルクを出力することができる。

なお、ドライバー２１は、ビットを備えており、ビット周りに設けられた円筒状のカバー内の気圧を低下させることにより、ビットに対してネジを吸着させることができる（図１において不図示）。従って、ドライバー２１にネジが吸着された状態においては、ネジに作用する力やトルクは実質的にドライバー２１に作用する力やトルクと同等であり、ビットに吸着されたネジに対して力やトルクが作用する場合、力覚センサーＰは、ネジに対して作用する力やトルクに応じた力やトルクを出力することもできる。なお、ドライバー２１やネジが外部の物体に接触し、外部の物体に及ぼす力およびトルクは、外部の物体からドライバー２１やネジに作用する力やトルクと向きが異なり大きさが等しい物理量である。そこで、力覚センサーＰはいずれを検出していると見なすこともできるが、ここでは、ドライバー２１やネジが外部の物体に及ぼす力およびトルクが力覚センサーＰの出力となる例について説明する。

さらに、ドライバー２１は、ビットに吸着したネジを回転させることでネジを回転させることができる。従って、ネジをネジ穴の開口部に配置させてドライバー２１のビットを回転させると、ネジがネジ穴に挿入される。アーム１０は、各関節Ｂ１〜Ｂ３、Ｒ１〜Ｒ３を回転させることによって所定の範囲にドライバー２１を配置させ、所定の姿勢（角度）にすることができる。本実施形態においてはロボット１の設置スペース付近にアーム１０の作業スペースが設けられており、図示しない搬送装置によってネジ穴Ｈを有するワークＷを搬送し、作業スペース内においてワークＷを固定することができる。作業スペース内には、ネジの供給装置Ｆが配置されており、ロボット１は、ドライバー２１の先端を供給装置Ｆに移動させてビットに対してネジを吸着させることでドライバー２１にネジを供給することができる。

ロボット１は、力覚センサーＰの出力に基づいてアーム１０を駆動してドライバー２１の位置や姿勢を変化させることができる。本実施形態においては、ドライバー２１のビットにネジを吸着した状態において、ロボット１がドライバー２１を移動させると、ネジはドライバー２１とともに任意の位置に移動する。また、ロボット１はドライバー２１にネジが吸着された状態でネジ穴Ｈの開口部までネジを移動させ、ビットによってネジを回転させることによってワークＷの各ネジ穴Ｈにネジを挿入する。従って、本実施形態において、挿入物はネジ、被挿入物はワークＷ、挿入穴はネジ穴Ｈであり、可動部は、アーム１０である。

（２）制御装置の構成：
本実施形態においてロボット１は制御装置４０によって制御される。従って、ロボット１と制御装置４０とでロボットシステムを構成する。制御装置４０は、制御部４０ａとドライバーコントローラー４０ｂとを有している。制御部４０ａおよびドライバーコントローラー４０ｂは、図示しないプログラムの実行部（ＣＰＵ，ＲＡＭ，ＲＯＭ等）を備えている。また、制御部４０ａおよびドライバーコントローラー４０ｂは、図示しないインタフェースを介して信号の入出力を行う。ロボット１やドライバー２１に対する電力源は種々の電力源を利用可能であり、本実施形態において制御部４０ａは、商用電力源から電力の供給を受け、アーム１０が備える各モーターに対して電力を供給する。ドライバー２１には、商用電力源から電力が供給されている。

制御部４０ａは、ロボット１のエンドエフェクターに装着された力覚センサーＰの出力に基づいて、アーム１０が備える各モーターを制御するための制御信号と、電力を供給する。また、制御部４０ａは、真空ポンプ４０ｃに接続されている。真空ポンプ４０ｃは、ドライバー２１のカバー内の気圧を低下させることが可能であり、制御部４０ａが出力する制御信号に応じて駆動する。すなわち、制御部４０ａは、真空ポンプ４０ｃを制御することにより、ドライバー２１のビットに対するネジの吸着を制御することができる。さらに、ドライバーコントローラー４０ｂは、ロボット１の力覚センサーＰに装着されたドライバー２１に対して、ビットの回転等を制御するための制御信号を出力する。また、ドライバー２１は、ビットに作用しているトルクを示す信号を出力する。

次に、制御部４０ａの構成をより詳細に説明する。図２は制御部４０ａの機能ブロック図である。制御部４０ａは、予め決められたプログラムを実行することによってロボット１を駆動する。力制御部４１，位置制御部４２は当該プログラムによって実現される機能を模式的に示している。

本実施形態において、制御部４０ａは、アーム１０の関節Ｂ１〜Ｂ３とねじり関節Ｒ１〜Ｒ３のそれぞれに対応するモーターに対して制御信号を出力する。力制御部４１は、エンドエフェクターに設けられた力覚センサーＰからの出力が目標力となるように、アーム１０の各関節を制御する。すなわち、力制御部４１は、力覚センサーＰの出力を取得してドライバー２１（またはネジ）が他の物体に及ぼしている力を検出する。そして、当該検出された力が目標力ではない場合、力制御部４１は、当該目標力が検出される状態を実現するために必要なアーム１０の駆動量（モーターの制御量）を算出し、モーターに対して制御信号を出力することにより、モーターを制御量に従って動作させる。当該モーターの制御量は、種々の手法によって決定されて良く、例えば、インピーダンス制御によって制御量が決められる構成等が採用可能である。むろん、力制御部４１は、力覚センサーＰから出力されるトルクを目標トルクにするように、アーム１０を制御しても良い。

位置制御部４２は、予め設定されたＴＣＰ（Tool Center Point：アーム１０とともに移動する基準部位）の位置が目標位置となり、ＴＣＰを有する部位の姿勢が目標姿勢となるように、アーム１０の各関節を制御する。すなわち、ロボット１において、ＴＣＰの位置は予め決められたロボット座標系（図１に示す直交ｘｙｚ座標系）において定義される。ＴＣＰを有する部位の姿勢はＴＣＰに原点を有し、ドライバー２１に対して固定された局所座標系における軸の方向に基づいて決定される。局所座標系は種々の定義が可能であるが、例えば、ビットが延びる方向の１軸と当該方向に垂直な２軸とによって定義可能である。

ＴＣＰは、種々の部位に設定可能であるが、本実施形態においては、ドライバー２１のビットに取り付けられたネジの先端の中央点に設定されている。図３は、ドライバー２１が備える円筒状のカバー２１ａを示す図であり、当該図３においては、カバー２１ａをビット２１ｂが延びる方向で切断し、ビット２１ｂおよびネジ３０とともに示している。

可動範囲内でＴＣＰが任意の座標に位置し、任意の姿勢になるために必要な各アーム１０の動作は予め特定されている。位置制御部４２は、ロボット座標系におけるＴＣＰの目標位置および目標姿勢が特定されると、当該目標位置および目標姿勢に基づいてアーム１０の駆動量（モーターの制御量）を算出する。そして、位置制御部４２は、モーターに対して制御信号を出力することにより、モーターを制御量に従って動作させる。むろん、当該制御においては、ＰＩＤ（Proportional-Integral-Derivative）制御等のフィードバック制御によってモーターの制御量が取得されても良い。

以上の構成より、制御部４０ａは、ドライバー２１またはネジ３０が他の物体に及ぼす力が目標力になるようにアーム１０を制御することができる。また、制御部４０ａは、ドライバー２１に取り付けられたネジ３０のＴＣＰを目標位置に移動させ、ＴＣＰを基準としてドライバー２１を目標姿勢にさせることができる。

（３）ネジ挿入処理：
次に、本実施形態において制御装置４０がロボット１を制御してワークＷのネジ穴Ｈに対してネジ３０を挿入してネジ締めを行わせる際のネジ挿入処理を説明する。図４は、ネジ挿入処理を示すフローチャートである。ネジ挿入処理が開始される前においては、処理中に必要となる目標位置や目標姿勢等が予め教示されている。すなわち、本実施形態において、ロボット１は、供給装置Ｆからネジ３０の供給を受け、ネジ３０をドライバー２１に吸着させた状態でネジ３０をワークＷのネジ穴Ｈに移動させてネジ３０をネジ穴に挿入する作業を繰り返す。この作業を実行できるように、各作業の開始位置が教示位置として教示され、開始位置における姿勢が教示姿勢として教示されている。

このように教示が行われた状態において、制御部４０ａがネジ挿入処理を開始すると、まず、位置制御部４２がネジ供給位置へドライバー２１を移動させる（ステップＳ１００）。すなわち、位置制御部４２は、モーターに制御信号を出力し、アーム１０を駆動してネジ３０の供給装置Ｆの上方（ｚ軸方向の正方向）における教示位置にドライバー２１を移動させる。また、位置制御部４２は、モーターに制御信号を出力し、アーム１０を駆動して、ドライバー２１の姿勢を教示姿勢にさせる。

次に、位置制御部４２は、ドライバー２１にネジ３０を供給する（ステップＳ１０５）。すなわち、位置制御部４２は、モーターに制御信号を出力し、アーム１０を駆動してドライバー２１のカバー２１ａの開口部を供給装置Ｆのネジ３０に近づける。また、制御部４０ａは、真空ポンプ４０ｃに制御信号を出力し、ドライバー２１のカバー２１ａの内部の気圧を低下させる。この結果、ネジ３０がビット２１ｂ側に吸着される。

次に、位置制御部４２は、ドライバー２１を挿入開始位置へ移動させ、ネジ３０を傾斜させる（ステップＳ１１０）。すなわち、図１に示すワークＷにはネジ穴Ｈが複数個形成されており、各ネジ穴Ｈへのネジ３０を挿入する際の作業の開始位置として、各ネジ穴Ｈの上方の位置が教示されている。そこで、位置制御部４２は、ワークＷが有する複数のネジ穴Ｈの中で、ネジ３０がまだ挿入されていないネジ穴Ｈを選択する。そして、位置制御部４２は、モーターに制御信号を出力してアーム１０を駆動し、選択されたネジ穴Ｈに対するネジ３０の挿入開始位置として教示された位置にドライバー２１を移動させる。

また、位置制御部４２は、モーターに制御信号を出力し、アーム１０を駆動して、ドライバー２１を教示された姿勢にする。本実施形態において当該教示された姿勢は、ネジ穴Ｈの中心軸に対して傾斜した姿勢である。図５は、ネジ３０をワークＷのネジ穴Ｈに挿入する作業の開始前における挿入開始位置と姿勢を模式的に示す図である（カバー２１ａは省略）。図５に示すように、挿入開始位置は、当該挿入開始位置からｚ軸の負方向にネジ３０を移動させた場合に、ネジ３０の先端がネジ穴Ｈに進入する確率が高い位置に設定されている。また、ネジ穴Ｈの中心軸Ａ_Hに対してネジ３０の中心軸Ａsが角度θ（例えば７°）だけ傾斜するように、傾斜角が設定されている。なお、ドライバー２１の挿入開始位置への移動と傾斜は同時であっても良いし、いずれか一方が先に行われてもよい。

ドライバー２１が挿入開始位置に移動し、ネジ３０が傾斜したら、制御部４０ａは、力覚センサーＰに制御信号を出力し、力覚センサーＰをリセットする（ステップＳ１１５）。すなわち、ステップＳ１１０にて設定された姿勢で力覚センサーＰの出力結果が既定値（例えば０）になるようにセンサーをリセットする。

次に力制御部４１は、ｘ軸方向およびｙ軸方向における目標力を０、ｚ軸の負方向の目標力を既定値に設定する（ステップＳ１２０）。すなわち、力制御部４１は、力覚センサーＰからの出力に基づいて、ｘ軸方向、ｙ軸方向およびｚ軸方向の力であるＦｘ（Ｎ）、Ｆｙ（Ｎ）およびＦｚ（Ｎ）を取得する。そして、力制御部４１は、ＦｘおよびＦｙに基づいて、ｘ−ｙ平面における合力が０になるようにアーム１０を制御する。なお、本実施形態においては、ネジ穴Ｈにネジ３０を挿入する際にネジ穴Ｈの中心軸に沿ってネジ３０が進む方向が挿入方向である。従って、本実施形態において、本実施形態においてｚ軸の負方向は挿入方向に平行であり、ｘ軸方向およびｙ軸方向は挿入方向と直交する。

具体的には、力制御部４１は、ＦｘおよびＦｙが０でない場合、力覚センサーＰの出力であるＦｘおよびＦｙがともに０になるために必要なアーム１０の駆動量（モーターの制御量）を算出する。そして、力制御部４１がモーターに対して制御信号を出力することにより、モーターを制御量に従って動作させ、力覚センサーＰからの出力を０にする。

図７は、ｘ−ｙ平面における合力がＦｘ1（すなわち、Ｆｙ＝０）である例を示している。すなわち、図７に示す例において、ネジ３０は、ネジ穴Ｈの開口部（ｚ軸の正方向の端部）に形成されたテーパーに接点３０ａにて接触しており、ネジ３０が挿入方向（ｚ軸の負方向）に移動することによってネジ３０がテーパーから反作用を受ける。ここでは、当該接点３０ａからネジ３０が受ける反作用がｘ軸の負方向を向いた大きさＦｘ1の力である例を想定する。

図７に示すように、ネジ３０はカバー２１ａ内の気圧が小さくなることでビット２１ｂに吸着されているため、ネジ３０に作用した力Ｆｘ1は、ビット２１ｂやカバー２１ａを介して力覚センサーＰに伝わる。そして、接点３０ａからネジ３０に作用する反作用と、ドライバー２１が接点３０ａに及ぼしている力は逆向きで大きさが等しい力である。従って、力覚センサーＰからは、Ｆｘ＝Ｆｘ1（ｘ軸方向の力）、Ｆｙ＝０が出力される。この場合、力制御部４１は、モーターに対して制御信号を出力してアーム１０を駆動し、Ｆｘ1と逆方向（すなわち、ｘ軸の負方向）に僅かにドライバー２１を移動させる。この結果、ネジ３０の接点３０ａがネジ穴Ｈの開口部におけるテーパーから離れるが、ネジ３０が接点３０ａから離れた瞬間、ネジ３０に対してｘ−ｙ平面に平行な方向の力が作用しなくなる。このため、力覚センサーＰの出力においてＦｘ＝Ｆｙ＝０となり、力制御部４１は、力覚センサーＰの出力が目標力になったと見なし、ｘ−ｙ平面に平行な方向の位置または力に関する制御を中断する。

このような制御の過程において、力制御部４１は、ｚ軸方向の力Ｆｚが既定の値になるようにアーム１０を制御する。具体的には、力制御部４１は、ｚ軸方向の力Ｆｚが既定の大きさ（例えば６Ｎ）であり、ｚ軸の負方向に向いた力Ｆｚ1になるために必要なアーム１０の駆動量（モーターの制御量）を算出する。そして、力制御部４１がモーターに対して制御信号を出力することにより、モーターを制御量に従って動作させ、力覚センサーＰからの出力がＦｚ1になるように調整する。力Ｆｚの既定の大きさは、種々の要素に基づいて特定されて良く、例えば、ドライバー２１の重さ等に基づいて決められる。

次に、位置制御部４２は、ネジ３０の単位時間あたりの挿入速度でｚ方向に移動させる（ステップＳ１２５）。すなわち、本実施形態においてワークＷのネジ穴Ｈに挿入されるネジ３０は、既定の規格に従って製造されたネジであり、一回転あたりの挿入方向への移動距離、すなわち、リードが規格に従って製造されている。従って、ドライバー２１の回転速度が決定すると、当該回転速度でネジ３０が回転しながらネジ穴Ｈに挿入される際に挿入方向に移動する速度が決まる。

具体的には、ネジ３０のリードをＬ（ｍｍ／回転）、ドライバーの１秒あたりの回転速度をＲ（ｒｐｓ）とした場合に、当該回転速度でネジ３０を回転させると１秒あたりにＲ×Ｌ（ｍｍ）の距離だけネジ穴Ｈに挿入される。そこで、本実施形態においては、ネジ３０がネジ穴Ｈに挿入される速度に合わせてドライバー２１がｚ軸の負方向（挿入方向）に移動するように位置制御を行う。

具体的には、ドライバーコントローラー４０ｂは、ドライバー２１に制御信号を出力し、ドライバー２１を予め決められた回転速度Ｒで回転させる。また、位置制御部４２は、ネジ３０のリードＬと回転速度Ｒとの積を速度Ｖとして取得する。さらに、位置制御部４２は、モーターに制御信号を出力し、アーム１０を駆動してドライバー２１をｚ軸の負方向に速度Ｖで移動させる。

なお、位置制御部４２は、各種の制御の結果、ドライバー２１（ネジ３０）の移動速度が速度Ｖになるように制御する。従って、力制御部４１の制御によって挿入方向に速度ｖfで移動する場合、位置制御部４２は、さらにＶ−ｖｆの速度が加えられることで結果としてドライバー２１（ネジ３０）の移動速度が速度Ｖとなるように、アーム１０を駆動する。例えば、力制御による速度ｖｆが（（ｚ軸方向の目標力−力覚センサーＰの出力）／ダンパー）で得られる場合、ネジ３０がネジ穴Ｈに接触するなどして力覚センサーＰの出力が変化すると力制御による速度ｖｆも変化する。なお、ダンパーはインピーダンス制御等における仮想粘性係数である。

より具体的な例としては、例えばネジ３０の種類がＭ1であり、リードＬが０．２５ｍｍ、回転速度Ｒが１３．３ｒｐｓである場合、力制御時の挿入方向の速度Ｖは、約３．３ｍｍ／ｓｅｃ（＝１３．３×０．２５）である。この場合において、インピーダンスパラメーター（仮想慣性係数ｍ（Ｎ／（ｍｍ／ｓ²））、仮想粘性係数ｄ（Ｎ／（ｍｍ／ｓ））の数値としては、例えば、Ｆxについてｍ＝０．００１，ｄ＝１、Ｆyについてｍ＝０．００１，ｄ＝１、Ｆzについてｍ＝０．００５，ｄ＝５とする例が挙げられる。また、インピーダンスパラメーター（仮想弾性係数ｋ（Ｎ／ｍｍ）の数値を、全て０（Ｆｘについて０、Ｆｙについて０、Ｆｚについて０）、挿入方向の目標力の数値を３（Ｎ）とする例が挙げられる。

さらに、ネジ３０の種類がＭ1〜Ｍ39の場合において、以下の例を採用し得る。すなわち、力制御時の挿入方向の速度として、１〜２０（ｍｍ／ｓｅｃ）、インピーダンスパラメーター（仮想慣性係数ｍ、仮想粘性係数ｄ）の数値（ｍ（Ｎ／（ｍｍ／ｓ²））、ｄ（Ｎ／（ｍｍ／ｓ）を、Ｆｘについてｍ＝０．０００５〜０．００２，ｄ＝０．５〜２、Ｆｙについてｍ＝０．０００５〜０．００２，ｄ＝０．５〜２、Ｆｚについてｍ＝０．００１〜０．０１，ｄ＝１〜１０、インピーダンスパラメーター（仮想弾性係数ｋ（Ｎ／ｍｍ））の数値として、全て０、挿入方向の目標力の数値を０（Ｎ）より大きくかつ４０（Ｎ）以下の値とする例が挙げられる。

位置制御部４２は、当該変化に合わせてドライバー２１（ネジ３０）の移動速度を制御し、ドライバー２１（ネジ３０）の移動速度を速度Ｖとする。この構成によれば、図６に示すように、ビット２１ｂが回転速度Ｒで回転しながらネジ３０の先端に設定されたＴＣＰがｚ軸の負方向に速度Ｖで移動する。なお、図６においてカバー２１ａは省略されている。なお、速度制御は、ネジ３０が挿入される際の速度がＶとなるように制御されていれば良く、位置制御による速度と力制御による速度の和によってＶ＋Ｖｆで移動することが許容されており、ネジ３０が他の物体に接触した場合やｚ軸方向に作用する力が目標力になった場合に移動速度がＶとなるような制御が採用されてもよく、種々の構成を採用可能である。

そして、ネジ３０がネジ穴Ｈに到達し、ネジ３０の溝がネジ穴Ｈの溝にかみ合い始めるとネジ３０が回転しながらネジ穴Ｈに挿入されていくが、ネジ３０のリードＬとドライバー２１の回転速度Ｒの積がＶであるため、ドライバー２１がｚ軸の負方向に移動する速度Ｖは変化せずにネジ３０がネジ穴Ｈに挿入されていく。むろん、速度Ｖでのドライバー２１の移動は、ネジ３０がネジ穴Ｈに挿入されている間に実行されればよく、ネジ３０がネジ穴Ｈに達する直前まで速度Ｖよりも高速に移動され、ネジ穴Ｈに達する段階で速度Ｖとなってもよい。

このように、本例の制御においては、ステップＳ１２５において、ドライバー２１が回転しながらドライバー２１とともにネジ３０が速度Ｖでｚ軸の負方向に移動され、さらに、ネジ３０が他の物体に接触すると、アーム１０がｚ軸の負方向に力Ｆｚ1を作用させる。従って、ネジ３０がネジ穴Ｈの溝にかみ合っており、ネジ３０に対してｚ軸の負方向に力を作用させることができる状態において、ネジ３０にはｚ軸の負方向に力Ｆｚ1が作用し続け、これとともにドライバー２１によって回転される。さらに、ネジ３０がテーパーに接触した後、ｘ−ｙ平面方向の力を０にするためにネジ３０がｘ−ｙ平面内でテーパーと逆側に僅かに移動すると、ネジ３０がｚ軸の負方向に移動する。ネジ３０がｚ軸の負方向に移動すると、再度ネジ３０に対してテーパーからｘ−ｙ平面方向の力が作用するため、当該力を０にする制御が行われてネジ３０がｘ−ｙ平面内でテーパーと逆側に僅かに移動する。

本実施形態においては、このような動作が繰り返されるため、ネジ３０がテーパーの表面に沿ってテーパーを避けながらｚ軸の負方向に徐々に挿入される。すなわち、ネジ穴の壁面に接触しながらネジ３０がネジ穴に挿入される倣い制御が実行される。そして、ネジ３０の先端がテーパーよりも奥側に達すると、ネジ３０が回転されているためにネジ３０がネジ穴Ｈに挿入されていく。すなわち、アーム１０はｚ軸の負方向に移動され、この過程においてアーム１０の姿勢は変化しないが、ネジ３０はネジ穴Ｈの溝に噛み合うため、ネジ３０がテーパーよりも奥側に達するとネジ３０の中心軸に対する傾斜は徐々に小さくなる。そして、やがてネジ３０の軸は、ネジ穴Ｈの中心軸と平行になってネジ穴Ｈに挿入されていく。図８は、ネジ３０の先端がネジ穴Ｈのテーパーよりも奥側（ｚ軸負方向）に挿入されている過程を示す図である。

このような制御が実行されている状況において、ドライバーコントローラー４０ｂは、ネジ締め完了を検知したか否かを判定する（ステップＳ１３０）。すなわち、ドライバーコントローラー４０ｂは、ドライバー２１からの出力信号に基づいてビット２１ｂに作用するトルクを検出可能であり、既定の閾値以上のトルクが既定の期間ビット２１ｂに作用した場合、ネジ締めが完了したことを検知する。なお、ドライバー２１において、ネジ３０とビット２１ｂの接触部はカバー２１ａの内側に配置されている（例えば、図３参照）。そこで、ドライバー２１においては、ネジ３０のネジ締めが完了する前にビット２１ｂがビット２１ｂの長さ方向にネジ３０を押し出しながら移動する（または、カバー２１ａがビット２１ｂの長さ方向に沿ってドライバー２１側に退避する）構成を備えている。

ステップＳ１３０において、ネジ締め完了を検知したと判定されない場合（ネジ３０が挿入穴に最後まで挿入されていない場合）、位置制御部４２および力制御部４１は、ネジ締め完了を検知したと判定されるまで待機する（ステップＳ１２０，Ｓ１２５の制御を続ける）。なお、ネジ締め完了が検知される前に、ネジ締めの失敗が検知された場合（例えば、ビット２１ｂからのネジ３０の脱落が検知された場合）には、例えば、ステップＳ１００に戻って同一のネジ穴について処理を行うなどの処理が行われる。

ステップＳ１３０において、ネジ締め完了を検知したと判定された場合、位置制御部４２および力制御部４１は、位置制御と力制御を終了する（ステップＳ１３５）。次に、制御部４０ａは、全ネジ穴Ｈに対してネジ３０の挿入が終了したか否かを判定する（ステップＳ１４０）。すなわち、ワークＷに形成されたネジ穴Ｈの数は予め決まっているため、制御部４０ａは、全てのネジ穴Ｈに対してネジ３０の挿入が完了したか否かを判定し、完了すると判定されるまでステップＳ１００以降の処理を繰り返す。

以上の構成によれば、ネジ穴Ｈにネジ３０を挿入する際に失敗する確率を低減することが可能である。例えば、ネジ３０に作用する力の法線方向にネジ３０を向けるような従来の構成において、ネジ３０がネジ穴Ｈのテーパーに接触すると、ネジ３０がテーパーの法線方向に向いてしまい、移動不能になる場合がある。しかし、本実施形態によれば、ネジ３０がテーパーの法線方向に向くことは抑制される。さらに、本実施形態においては、ネジ３０がテーパーに接触しても、テーパーを回避してネジ３０がネジ穴Ｈに挿入されるように倣い制御が行われる。従って、ネジ３０のネジ穴Ｈに対する挿入が失敗する確率を低減することができる。

さらに、本実施形態においては、ネジ３０を傾斜させてネジ穴Ｈに近づけるため、挿入穴の中心軸上に他の部材が存在してもその部材を避けた位置を利用してネジ穴にネジ３０を近づけ、ネジ穴に挿入することが可能である。図９は、このようなネジ穴Ｈを有するワークＷの例を示す図である。図９に示すように、ワークＷは、中空の６面体の１面を除去した形状であり、側面Ｓ₁にネジ穴Ｈが形成されている。当該ネジ穴Ｈの中心軸Ｄ上には反対側の側面Ｓ₂が存在するため、ネジ穴Ｈの中心軸上にドライバー２１やネジ３０を配置してネジ３０をネジ穴Ｈに挿入するのは困難である。

しかし、本実施形態においては、ネジ３０を傾斜させてネジ穴Ｈに近づけ、ネジ３０をネジ穴Ｈに挿入することができる。従って、ネジ穴Ｈの中心軸上に存在する側面Ｓ₂を避けてネジ３０をネジ穴Ｈに近づけ、挿入することができる。そして、このようにネジ３０を傾斜させ、ネジ穴Ｈの中心軸に対してネジ３０の軸が平行ではない状態でネジ３０をネジ穴Ｈに挿入させようとした場合、力制御部４１による制御がなければネジ３０がテーパーに接触することにより、ネジ３０の挿入が失敗しやすくなることが考えられる。しかし、本実施形態においては、力制御部４１による制御でテーパーを回避しながらネジ３０をネジ穴Ｈの奥側に進めることができる。従って、失敗の確率を高めることなくネジ３０をネジ穴Ｈに挿入することが可能である。

（４）教示位置：
次に、本実施形態におけるネジ３０の挿入開始位置としての教示位置を詳細に説明する。既定の作業を繰り返すロボットにおいては、通常、作業の要所毎における可動部の位置や姿勢を教示する。工業製品としてのロボットにおいては、製造誤差や経時変化等によって可動部の位置に誤差が生じ得る。教示位置や教示姿勢は、通常、種々の要素に基づいて特定されるが、一般的には、作業の成功確率が最も高くなるように教示位置および教示姿勢が決められる。

ネジ３０をネジ穴Ｈに挿入する作業においては、一般的には、ネジ３０の軸がネジ穴Ｈの中心軸に平行になるようにネジ３０の姿勢が教示される。また、中心軸に平行な視線方向でネジ穴Ｈを見た場合の中央にネジ３０の中心が配置されるように教示位置が設定される。図１０は、テーパーＴを有するネジ穴Ｈをｚ軸方向に沿って見た状態を示している。図１０においては、中心軸（ｚ軸の方向）に平行な視線方向でネジ穴Ｈを見た場合の中央を符号Ｏによって示している。

ネジ３０をネジ穴Ｈの中心軸に対して傾斜させない場合、当該中央Ｏを教示位置とすると、ネジ３０の挿入作業の成功確率が最も高くなる。しかし、ネジ３０が傾斜された状態でネジ穴Ｈに向けて移動される構成においては、図７に示すようにネジ３０におけるネジ穴Ｈ側の端部（挿入穴側の端部：ビット２１ｂと反対側の端部）がテーパーに最初に接触し得る。このため、中央Ｏが理想的な教示位置とはならないことが判明した。

すなわち、当該中央Ｏから変位方向に変位した位置に教示位置が設定される構成が好ましいことが判明した。具体的には、ネジ３０のネジ穴Ｈ側の端部がネジ穴Ｈの中心軸に対して傾斜する方向を変位方向とし中央Ｏから変位方向に変位した位置が教示位置となる構成が好ましい。なお、ここで、ネジ３０のネジ穴Ｈ側の端部がネジ穴Ｈの中心軸に対して傾斜する方向は、ネジ穴Ｈの中心軸に垂直な方向で定義されている。例えば、図５に示す例において、ネジ３０のネジ穴Ｈ側の端部Ｐe（本例ではＴＣＰと一致）がネジ穴Ｈの中心軸Ａsに対して傾斜する傾斜方向はθ方向（回転中心を基準にした回転方向）ではなく、ｘ−ｙ平面内で定義される。従って、図５に示す例では、破線で示す矢印Ｄxが傾斜方向である。また、ネジ３０のネジ穴Ｈ側の端部Ｐeはネジ穴Ｈ側の一点であれば良く、ネジ３０の中心軸Ａsがネジ穴Ｈの中心軸Ａ_H上に存在する場合と、傾斜した場合とで差異が生じるように、例えば、ネジ３０の中心軸Ａs上に定義されれば良い。

変位方向への変位量は、例えば、種々の変位量に変位させて挿入動作を繰り返し、成功および失敗の統計を取得するなどして特定可能である。図１０においては、中央Ｏからの変位量をｘ軸方向にｄｘ、ｙ軸方向にｄｙとして挿入動作を試行する際の位置Ｐtが示されている。表１は、このようにｘ軸方向にｄｘ、ｙ軸方向にｄｙだけ変位させた位置Ｐtを挿入開始位置としてネジ３０をネジ穴Ｈに挿入する動作を繰り返し、ネジ３０が適正に挿入された確率を算出した結果を示している。

すなわち、表１では、変位量をｘ軸方向、ｙ軸方向のそれぞれに０．２ｍｍ毎に変化させて確率を算出している。なお、この例において使用されたネジ３０はＭ１．２×２ｍｍの並目、ＳＵＳ製のネジであり、ワークＷは鉄製である。

ここでは、表１において成功率が１００％となる変位量に着目する。成功率１００％となる変位量のｙ軸方向における変化に着目すると、ｄｙ＝０．０ｍｍを中心としてｙ軸方向に対称に分布している（ｄｘ＝−０．２，ｄｙ＝−０．２は例外）。例えば、ｄｘ＝０．０ｍｍにおいて、成功率が１００％である分布はｄｙが−０．４ｍｍ〜０．４ｍｍの範囲に分布しており、その分布中心のｄｙは０．０ｍｍである。従って、ｙ軸方向の変位量は０が好ましい。

一方、ｄｙが０．０ｍｍである場合における成功率のｘ軸方向の分布に着目すると、成功率１００％はｄｘが−０．２ｍｍから０．８ｍｍの範囲に分布している。従って、成功率１００％の分布の分布中心はｄｘ＝０．３ｍｍである。このため、ｄｙ＝０．０ｍｍ、ｄｘ＝０．３ｍｍの位置を教示位置とすれば、ネジ３０を傾斜させる本実施形態におけるネジ３０の挿入の成功率を最も高くすることができる。

以上のように、変位方向にずれた位置を挿入開始位置としてネジ３０を挿入する作業を複数の位置で複数回実施し、当該変位方向内で成功率が所定の確率（本例では１００％であるが８０％等であっても良い。）以上となる変位量の分布を特定し、当該分布の中心（本例ではｄｘ＝０．３ｍｍ）を教示位置とする構成を採用すれば、ネジ３０を傾斜させる構成において、ネジ３０を傾斜させない構成と同等の成功率でネジ３０をワークＷに対して挿入させることが可能である。なお、ネジ３０を傾斜させた場合において成功率が１００％となる変位量の分布は、ネジ３０を傾斜させない場合において成功率が１００％となる変位量の分布よりも広い。従って、ネジ３０を傾斜させた場合、挿入開始位置の教示が厳密でなかったとしても、ネジ３０を傾斜させない場合と比較してネジ３０の挿入が成功する確率を高い確率とすることができる。

（５）他の実施形態：
以上の実施形態は本発明を実施するための一例であり、他にも種々の実施形態を採用可能である。例えば、制御装置は、ロボットに内蔵されていても良いし、ロボットの設置場所と異なる場所、例えば外部のサーバ等に備えられていても良い。さらに、制御装置が複数のロボットを制御するように構成されていても良い。むろん、制御装置が複数の装置に分散して配置されていても良い。例えば、制御装置の一部がロボットに内蔵され、他の一部がロボットの外部のサーバ等に配置されていても良い。

さらに、上述の実施形態の一部の構成が省略されてもよいし、処理の順序が変動または省略されてもよい。例えば、力覚センサーのリセットがネジ１個毎に行われるのではなく、複数のネジの作業について１回リセットが行われる構成であっても良い。

さらに、ロボットは可動部を有していればよく、ロボットは可動部を動作させることによって任意の機能を実現することができればよい。可動部は、ロボットの設置位置に対して相対的に移動し、姿勢が変化するように構成されていれば良く、その自由度（可動軸の数等）は任意である。少なくとも、挿入物を挿入穴の中心軸に対して傾斜させて挿入物を挿入穴に挿入することができればよい。従って、ロボットの態様は限定されず、軸の数やアームの数、エンドエフェクターの態様等は種々の態様を採用可能である。

また、可動部は、当該可動部が駆動されることにより、可動部の動作に伴って挿入物を移動させることができればよい。従って、挿入物は可動部や可動部に設けられたエンドエフェクター等に取り付けられるように構成されていれば良く、取り付けの態様は種々の態様を採用可能である。例えば、気圧や磁力等を利用した吸着や把持、挟持等やこれらの組み合わせによって挿入物が可動部や可動部に設けられたエンドエフェクター等に取り付けられる構成を実現可能である。可動部に対して挿入物を取り付けるための装置（例えば、吸着のための装置やエンドエフェクター等）は、可動部が有していても良いし、着脱可能であっても良い。

被挿入物は、挿入物が挿入される挿入穴を有する物体であれば良く、挿入物が挿入可能であれば形状や材質等は任意である。被挿入物は、挿入物の挿入動作が開始される前に、予め決められた位置に配置されていれば良く、その姿勢等は種々の姿勢であって良い。例えば、上述の図１に示す例において、被挿入物としてのワークＷの姿勢が図１に示す姿勢と異なっており、ネジ穴Ｈの中心軸がｘ軸方向やｙ軸方向、他の方向を向いていても良い。

挿入穴は、挿入物を挿入可能な部位であれば良く、例えば、被挿入物に形成された穴等が挿入穴となり得る。挿入穴は、挿入物が挿入される形状であるため、挿入穴の中心軸に平行な視線方向で挿入穴を眺めた場合、挿入穴の内壁が挿入物の外壁に接するような形状等になるが、挿入物を挿入穴に挿入した後に挿入物が挿入穴内で移動しないようにするためには、挿入穴の内壁が挿入物の外壁よりも僅かに大きく形状がほぼ同一である構成が好ましい。

また、挿入穴の挿入方向は、当該挿入方向に沿って挿入物が移動した場合に挿入物が挿入される方向であり、主に直線方向が想定されるが、曲線に沿った方向（例えば、トーラス体の一部を挿入する挿入穴等）も想定し得る。

挿入物は、被挿入物の挿入穴に対して挿入される物体であれば良く、被挿入物の挿入穴に挿入可能であれば形状や材質等は任意である。挿入物は、挿入穴に挿入される前に可動部によって移動可能になっていれば良く、供給装置等によって可動部に供給されても良い。挿入は、組み立ての一部であっても良いし、締結の一部であっても良い。後者がネジによって行われる場合、挿入のために挿入物が移動する移動方向は挿入方向のみではなく、挿入穴の中心を軸とした回転方向にも移動し得る。

挿入物の中心軸に対する傾斜は、少なくとも挿入物が挿入穴の内壁に接触する場合において実現されていれば良く、挿入物の挿入穴への挿入が開始された場合（例えば、ネジ締めのため動作が開始された場合）には、傾斜していても良いし、傾斜した状態が終了しても良いし、傾斜角が徐々に０に近づいても良い。むろん、挿入物を挿入方向に対して傾斜させない状態で倣い制御を行う構成が採用されてもよく、この場合においても高い成功確率で挿入物を被挿入物に挿入することができる。

制御部は、可動部を制御することによって挿入物を移動させることができればよく、当該移動に伴って挿入物の姿勢を変化させることができればよい。従って、制御部としては、例えば、複数の自由度を有する可動部に対して変形や回転等の制御指示を行い、モーターやアクチュエーター等の駆動部を駆動する制御部が挙げられる。

制御部における制御は、少なくとも、挿入物の移動と挿入物の姿勢の調整（傾斜）と挿入物の挿入穴への挿入とを行うための可動部の制御が含まれていれば良く、可動部の位置と姿勢との少なくとも一方が変化する制御によって実現可能である。むろん、制御の際の制御目標は、種々の目標を採用可能であり、可動部や挿入物を目標位置まで移動させる位置制御や、可動部や挿入物に作用する力が目標力になるように可動部や挿入物を動作させる力制御、可動部や挿入物に作用する加速度が目標加速度になるように可動部や挿入物を動作させる加速度制御等であっても良いし、複数の制御の組み合わせであっても良い。

制御の具体例として、挿入物を挿入穴へ挿入する間に、制御部が、挿入物において挿入穴の挿入方向に作用する力が、挿入方向と直交する方向に作用する力よりも大きくなるように制御する構成であっても良い。すなわち、制御部は、制御によって可動部を駆動することが可能であり、当該駆動の結果、可動部の位置や可動部に作用する力が変化する。このような制御によれば、挿入物に作用する力を制御して、挿入物の挿入方向に作用する力が、当該挿入方向と直交する方向に作用する力よりも大きい力となるように制御することができる。

そして、挿入物において、挿入方向と直交する方向に作用する力よりも大きい力を挿入方向に作用させると、挿入物に対して、挿入方向に移動させるための力、すなわち、挿入物を挿入穴に挿入させるための力を作用させることができる。また、挿入方向に直交する方向に作用する力が０であれば挿入物が他の物体に接触した場合に他の物体と逆側に移動し、挿入方向に直交する方向に作用する力が０より大きければ多くの場合その力に応じて挿入物が当該直交方向に移動し得る。

図１１においては、図７と同様の状態において、ネジ３０が他の物体から受ける力ではなく、アーム１０の駆動によってビット２１ｂがＴＣＰに作用させている力をＦｘ3、Ｆｘ4として示している。すなわち、アーム１０の駆動により、ビット２１ｂにはｚ方向の力Ｆｘ4が作用している。従って、ビット２１ｂは、力Ｆｘ4によってネジ３０をネジ穴Ｈに挿入する。

また、本例では、ビット２１ｂに作用するｘ方向の力がＦｘ3（ｙ方向の力は０）となるようにアーム１０が制御されている。当該力Ｆｘ3が０であれば、ネジ３０がテーパーに対して接点３０ａで接触したとしてもその反作用を打ち消すようにアーム１０が制御されるため、ネジ３０がテーパーを回避し、ネジ３０が他の物体の接触に対して他の物体の逆側に移動して当該他の物体を回避しているように動作する。また、テーパーの接点３０ａから受ける反作用と同一の方向に向けた０より大きい力Ｆｘ3をＴＣＰに作用させると、テーパーなどの物体を回避するようにアーム１０が制御される。

このように、挿入物に着目した場合に、挿入穴の挿入方向に作用する力が挿入方向と直交する方向に作用する力より大きい状態は、各種の目標力によって実現可能である。すなわち、アーム１０に連動する任意の部位（図１１に示すネジ３０の接点３０ａやＴＣＰ）に作用する力は、演算によって変換可能であるため、アーム１０に連動する任意の部位に作用する力を目標力とすることができる。従って、図１１に示したＴＣＰに対する目標力が直接的に定義されて制御装置４０が力制御を行っている構成に限定されず、どのような部位に対してどのような目標力が設定されて制御が行われてもよい。そして、挿入物に着目した場合に、挿入穴の挿入方向に作用する力が挿入方向と直交する方向に作用する力より大きいのであれば、挿入穴の中心軸に対して傾斜させて挿入物を挿入穴に接触させた後に、挿入物を挿入穴に挿入するための制御を行うことができる。

以上のように、挿入物の挿入方向への移動を妨げる他の物体が挿入物に接触したとしても、挿入物が当該物体を避ける。そして、挿入方向への力が挿入方向に直交する方向の力よりも大きいことにより、他の物体を避けながら挿入物が挿入穴に挿入されるように制御することができる。なお、挿入物を挿入穴へ挿入する間は、挿入物が挿入穴に挿入され始めてから挿入が完了するまでの間の少なくとも一部の期間であればよく、例えば、挿入物としてのネジが挿入穴としてのネジ穴のテーパーに接触している間等であってもよい（以下同様）。

さらに、制御の具体例として、制御部が、可動部に設けられた力検出部からの出力を目標力にさせる力制御部を備えてもよい。すなわち、可動部（または可動部とともに移動する挿入物）が他の物体に接触すると、当該他の物体から可動部に力が及ぼされ、可動部から他の物体に力を及ぼす。これらの力は互いに逆向きであり同一の大きさである。力検出部は、可動部が他の物体に及ぼす力を検出することができる。

そして、制御部が可動部を移動させるための力を当該可動部に作用させると、その結果として可動部が他の物体に及ぼす力が変化し得る。従って、制御部が可動部の位置等を制御すると、力検出部からの出力が変化し得る。このため、制御部は、可動部の位置等を制御することにより、力検出部からの出力が特定の目標力になるように可動部を制御することができる。

なお、力検出部は可動部に設けられており、可動部に作用する力を検出するが、ここでは、可動部とともに移動する挿入物を挿入穴に対する挿入する際の制御が行われるため、力検出部において検出する力は挿入物に作用する力と実質的に同等であることが好ましい。すなわち、吸着等によって挿入物が可動部に対して実質的に一体化されることにより、可動部とともに挿入物が移動する構成においては、力検出部によって検出する力は挿入物に作用する力と実質的に同等と見なすことができる。

そして、以上のように、可動部の制御として力制御を実行可能である構成において、挿入物を挿入穴へ挿入する間に、力制御部が、挿入方向を向いた力を目標力にする構成であっても良い。この構成によれば、力制御部は、可動部を介して挿入物に対して、挿入方向を向いた力が作用するように可動部を制御することができる。この結果、挿入物が挿入穴に挿入されるように可動部を制御することが可能になる。なお、力検出部からの出力が、可動部から他の物体に及ぼす力ではなく、他の物体からの反作用としての力であれば、目標力の向きは逆向き（従って、挿入方向の逆向きを向いた力）になる。

さらに、挿入方向を向いた力を目標力とする構成において、挿入方向と直交する方向に目標力を設定しても良い。例えば、挿入物を挿入穴へ挿入する間に、力制御部が、挿入方向と直交する方向における目標力を０にする構成が採用されてもよい。すなわち、挿入方向と直交する方向から可動部（または挿入物）に他の物体が接触していなければ、挿入方向と直交する方向に関して力検出部で検出される力は０である。従って、この状況において挿入方向と直交する方向の目標力を０にしても実質的に意味は無い。

一方、挿入方向と直交する方向から可動部（または挿入物）に他の物体が接触すると、挿入方向と直交する方向に作用する力として力検出部から有限の値（≠０）が出力される。従って、このように、挿入方向と直交する方向から可動部（または挿入物）に他の物体が接触している状況において、挿入方向と直交する方向の目標力を０にすると有限の値を０にするように可動部が駆動されることになる。この結果、可動部は、可動部（または挿入物）に接触している他の物体を避ける移動を行うように制御される。

なお、当該制御によって、挿入方向と直交する方向から可動部（または挿入物）に他の物体が接触しない状態になると、力検出部からの出力は０（すなわち目標力）になり、目標力による可動部の制御が行われない状態になる。このため、挿入方向において、当該挿入方向を向いた力が目標力となっていることにより、挿入方向への移動を妨げる物体（ネジ穴のテーパー等）に可動部（または挿入物）が接触している状態においては、当該物体を避けながら挿入物が挿入方向に徐々に挿入されるように制御される。すなわち、挿入穴の壁面に接触しながら挿入物が挿入穴に挿入される倣い制御が実行される。

さらに、挿入方向を向いた力を目標力とする構成において、力以外の他の物理量を用いて倣い制御が実行されてもよい。例えば、制御部が、可動部とともに移動する基準部位の位置を目標位置に移動させる位置制御部を備える構成を採用可能である。すなわち、可動部とともに移動する基準部位が目標位置に移動されると、当該基準部位を目標位置にすることで可動部や挿入物の位置を制御する位置制御が実行されることになる。なお、基準部位は、当該基準部位の位置が制御部によって把握され、任意の座標系で任意の位置に移動され得る部位である。基準部位としては、例えば、可動部において設定されたＴＣＰ（Tool Center Position）や、エンドエフェクターの重心、挿入物の特定の部位（ネジの先端等）など、種々の部位が想定可能である。

このように位置制御が可能な状況において、挿入物を挿入穴へ挿入する間に、位置制御部が、基準部位の位置を目標位置に移動させる制御を行う構成が採用されてもよい。この構成によれば、挿入方向に向いた力が挿入物に作用している状態において、可動部（または挿入物）が挿入を妨げる物体に接触したとしても、基準部位が目標位置に移動されることにより、当該物体に接触した状態で静止することを回避できる可能性が高くなる。当該物体を回避して挿入が行われるならば、当該物体の回避を繰り返すなどして物体の回避と挿入物の挿入とが進む状況となり得る。この場合、倣い制御が行われているといえる。

位置制御においては、種々の目標位置を設定することが可能であるが、例えば、少なくとも、挿入方向と直交する方向であって、挿入物の挿入を妨げる物体が存在しない方向に目標位置を変化させる構成であれば、挿入物の挿入方向への移動を妨げる物体に挿入物が接触していたとしても、当該物体を回避する確率を高めることができる。また、可動部（または挿入物）を予め決められた速度で移動させる制御によって位置制御が実現されても良い。

なお、位置制御における目標位置は種々の手法によって決定されて良く、例えば、挿入物としてのネジを挿入穴としてのネジ穴に挿入する例において、ネジにおいて挿入穴の中心方向に向いた（挿入方向に直角な方向の）大きな力が作用している場合、ネジがネジ穴のテーパーに接触していると推定される。一方、ネジにおいて挿入穴の中心方向に向いた力が小さい場合ネジ穴に入り始めたと考えられる。従って、挿入穴の中心方向に向いた力が大きい場合にその力の方向に移動する移動制御が行われれば、テーパーからネジが逃げるが、挿入方向には力が作用しているためテーパーに沿って奥にネジが移動する。このため、倣い制御が実現される。むろん、挿入方向を向いた力が目標力になっている状態で倣い制御が行われるためには、他にも種々の制御が採用可能であり、種々の位置制御や速度制御等が組み合わされる構成等が採用されてもよい。

さらに、可動部（または挿入物）を予め決められた速度で移動させるために設定される基準部位の位置の速度は、ドライバーの単位時間あたりの回転数とネジのリードとの積に基づいて決められていれば良く、積の値自体が速度であっても良いし、積の値に対してマージンが設けられた値が速度であっても良い。なお、マージンとしては、種々の例が挙げられ、例えば、積の０．５倍以上、２倍以下の値を速度とする構成が採用されてもよい。すなわち、積の０．５倍未満の速度であるとネジの挿入速度が遅くなり効率が悪くなり、また、ネジの挿入が失敗する確率が増加してしまう。一方、積の２倍を超える速度であるとネジやネジ穴に作用する力が増加し、また、ネジの挿入が失敗する確率が増加してしまう。単位時間は、秒単位であっても良いし分単位であっても良く種々の単位を採用可能である。

１…ロボット、１０…アーム、２１…ドライバー、２１ａ…カバー、２１ｂ…ビット、３０…ネジ、３０ａ…接点、４０…制御装置、４０ａ…制御部、４０ｂ…ドライバーコントローラー、４０ｃ…真空ポンプ、４１…力制御部、４２…位置制御部

Claims (9)

1. 可動部を有するロボットを制御する制御装置であって、
   前記可動部に設けられたエンドエフェクターにより挿入物を移動させ、被挿入物が有する挿入穴の中心軸に対して傾斜させて前記挿入物を前記挿入穴に接触させた後に前記挿入物を前記挿入穴に挿入する制御部、
   を備える制御装置。
2. 前記挿入物を前記挿入穴へ挿入する間に、前記挿入物において前記挿入穴の挿入方向に作用する力は、前記挿入方向と直交する方向に作用する力よりも大きい、
   請求項１に記載の制御装置。
3. 前記制御部は、
   前記可動部に設けられた力検出部からの出力を目標力にさせる力制御部を備え、
   前記力制御部は、
   前記挿入物を前記挿入穴へ挿入する間に、前記挿入方向を向いた力を前記目標力にする、
   請求項２に記載の制御装置。
4. 前記力制御部は、
   前記挿入物を前記挿入穴へ挿入する間に、前記挿入方向と直交する方向における前記目標力を０にする、
   請求項３に記載の制御装置。
5. 前記制御部は、
   前記可動部とともに移動する基準部位の位置を目標位置に移動させる位置制御部を備え、
   前記位置制御部は、
   前記挿入物を前記挿入穴へ挿入する間に、前記基準部位の位置を前記目標位置に移動させる制御を行う、
   請求項３または請求項４のいずれかに記載の制御装置。
6. 前記エンドエフェクターは、ドライバーを含み、
   前記挿入物は、
   前記ドライバーに取り付けられたネジであり、
   前記位置制御部は、
   前記ドライバーの単位時間あたりの回転数と前記ネジのリードとの積に基づいて設定された速度で前記基準部位の位置を前記挿入方向に移動させる、
   請求項５に記載の制御装置。
7. 前記制御部は、
   前記可動部とともに移動する基準部位を教示位置に移動させた後に、前記挿入物を前記挿入穴へ挿入させる制御を開始させ、
   前記教示位置は、
   前記挿入穴の中央から変位方向に変位した位置に設定されており、
   前記挿入穴の中央は、
   前記中心軸に平行な視線方向で前記挿入穴を見た場合の中央であり、
   前記変位方向は、
   前記挿入物の前記挿入穴側の端部が前記中心軸に対して傾斜する方向である、
   請求項１〜請求項６のいずれかに記載の制御装置。
8. 請求項１〜請求項７に記載の前記制御装置によって制御されるロボット。
9. 請求項１〜請求項７に記載の前記制御装置と、前記制御装置によって制御される前記ロボットとを備えるロボットシステム。

Images