JP2015221490A - ロボット装置の制御方法及びロボット装置 - Google Patents

Abstract

【課題】専用治工具を用いることなく軌道を補正して、保持したワークを他のワーク等に対して高精度に組付可能なロボット装置の制御方法及びロボット装置を提供する。【解決手段】ハンドにより保持した第１のワークを所定の軌道上で移動させ第２のワークに対する接触動作又は非接触動作を実行する際に、例えば、第１のワーク及び第２のワークの接触状態において第１のワーク及び第２のワークの間に相対的に作用する力の変化量（反力）を検出部により検出する第１の検出工程（ステップＳ１４）と、第１の検出工程で検出した力の変化量あるいは相対位置姿勢の変位量に基づき、軌道の補正量を演算する第１の演算工程（ステップＳ１５）と、第１の演算工程で演算した補正量に基づき、軌道を補正する第１の補正工程（ステップＳ１６）と、を備える。【選択図】図５

Description

本発明は、複数の関節を有する多関節アームを備えるロボット装置の制御方法及びロボット装置に関する。

従来、垂直多関節アーム及びエンドエフェクタ（以下、ロボット本体と呼ぶ）と、ロボット本体を制御する制御装置とを備えたロボット装置が普及している。このようなロボット装置により、例えば把持したワークを他のワークに自動的に組み付けるような作業を行う場合には、高精度かつ高速で作業することが望まれている。このため、エンドエフェクタとしてのハンドの軌道が例えば直線である場合に、ロボット装置によりワーク同士の組付作業を行う前に実際の移動軌跡を予め直線に補正しておくロボット装置の制御方法が知られている（特許文献１参照）。このロボット装置の制御方法では、補正作業専用の距離センサをハンドに取り付け、ハンドを平板に沿わせて直進移動させながら、同時に平板と距離センサとの距離を計測する。そして、平板と距離センサとの距離の変化からハンドの実際の移動軌跡と直線状の軌道との誤差を演算し、その誤差に基づいて軌道を補正することで、実際の移動軌跡を直線状に補正できるようになっている。

ところが、特許文献１のロボット装置の制御方法では、ハンドがワークを保持する前に軌道補正を行っているので、実際にワークを保持した際にワークがハンドに対して位置姿勢ずれしてしまうとワークの位置姿勢を高精度に制御することはできない。この場合、把持したワークを他のワークに高精度に組み付けることは困難である。また、このロボット装置の制御方法では、ハンドへの距離センサの設置や、軌道に沿った平板の設置は、いずれも高精度に行わなければならず、各設置作業が煩雑になり補正作業に長時間を要してしまう。

これに対し、ハンドの指の内側面に距離センサを設けることで、ハンドによりワークを保持する際に、ハンドに対するワークの位置姿勢を高精度に検出可能にしたロボット装置の制御方法が知られている（特許文献２参照）。このロボット装置の制御方法によれば、ハンドに対してワークを高精度な位置姿勢関係で把持できるので、把持したワークの位置姿勢を高精度に制御することで、他のワークへの組付精度を向上することができる。また、ロボット装置による作業中にハンドによってワークを把持する度に制御を行うものであり、しかも専用治具が不要であるので、これらのことから事前の準備時間に長時間を要することはない。

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

しかしながら、特許文献２に記載されたロボット装置の制御方法では、ハンドに対するワークの位置姿勢のみを高精度に設定しており、ワークを組み付ける他のワークの位置姿勢については何ら考慮されていない。このため、ハンドに対してワークを高精度な位置姿勢で把持したとしても、例えば他のワークの位置姿勢にずれやアームの軸ずれがあった場合には組付け精度が低下してしまうという問題があった。

本発明は、専用治工具を用いることなく軌道を補正して、保持したワークを他のワーク等に対して高精度に組付可能なロボット装置の制御方法及びロボット装置を提供することを目的とする。

本発明は、複数の関節を有する多関節アームと前記多関節アームに支持されるエンドエフェクタとを有するロボット本体と、前記エンドエフェクタに連結され、第１のワーク及び第２のワークの間に相対的に作用する力の変化量と前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量との少なくとも一方を検出する検出部と、前記ロボット本体を制御する制御部と、を備えるロボット装置の制御方法において、前記制御部が、前記エンドエフェクタにより保持した前記第１のワークを所定の軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記第１のワーク及び前記第２のワークが接触する接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記第１のワーク及び前記第２のワークが接触しない非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量と、の少なくとも一方を前記検出部の出力に基づいて検出する第１の検出工程と、前記制御部が、前記第１の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量を演算する第１の演算工程と、前記制御部が、前記第１の演算工程で演算した前記補正量に基づき、前記軌道を補正する第１の補正工程と、を備えることを特徴とする。

また、本発明は、複数の関節を有する多関節アームと前記多関節アームに支持されるエンドエフェクタとを有するロボット本体と、前記エンドエフェクタに連結され、第１のワーク及び第２のワークの間に相対的に作用する力の変化量と前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量との少なくとも一方を検出する検出部と、前記ロボット本体を制御する制御部と、を備えるロボット装置において、前記制御部は、前記エンドエフェクタにより保持した前記第１のワークを所定の軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記第１のワーク及び前記第２のワークが接触する接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記第１のワーク及び前記第２のワークが接触しない非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量と、の少なくとも一方を前記検出部により検出し、検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量を演算し、演算した前記補正量に基づき、前記軌道を補正することを特徴とする。

本発明によれば、制御部が、第１のワークを所定の軌道上で移動させ第２のワークに対する接触動作又は非接触動作を実行する際に、第１のワーク及び第２のワークの接触動作による第１のワークの力の変化量あるいは相対位置姿勢の変位量を検出する。そして、制御部は、検出した力の変化量あるいは相対位置姿勢の変位量に基づいて軌道の補正量を演算し、補正量に基づいて軌道を補正する。このため、専用治工具を用いて軌道を補正することなく、保持したワークを他のワーク等に対して高精度に組み付けることができるようになる。

本発明の第１の実施形態に係るロボット装置の概略構成を示す説明図である。 本発明の第１の実施形態に係るロボット装置により第１のワークを第２のワークに装着する動作を示す正面図である。（ａ）は第１のワークが第２のワークに対して上空点に位置した状態、（ｂ）は第１のワークが第２のワークに対して接触点に位置した状態、（ｃ）は第１のワークが第２のワークに対して下降点に位置した状態の正面図である。 本発明の第１の実施形態に係るロボット装置における第１のワークの位置と第１のワークへの反力との関係を示すグラフであり、（ａ）は軌道の補正前、（ｂ）は上空点での仮想の反力の演算時、（ｃ）は補正量の演算時である。 本発明の第１の実施形態に係るロボット装置の立ち上げ時の手順を示すフローチャートである。 本発明の第１の実施形態に係るロボット装置の軌道を補正する手順を示すフローチャートである。 本発明の第１の実施形態に係るロボット装置の変形例での軌道を補正する手順を示すフローチャートである。 本発明の第２の実施形態に係るロボット装置により第３のワークを第４のワークに装着する動作を示し、（ａ）は第３のワーク及び第４のワークの分離時の斜視図、（ｂ）は第３のワークが第４のワークに対して上空点に位置した状態の正面図である。 本発明の第２の実施形態に係るロボット装置により第３のワークを第４のワークに装着する動作を示す正面図である。（ａ）は第３のワークが第４のワークに対して接触点に位置した状態、（ｂ）は第３のワークが第４のワークに対して切換点に位置した状態、（ｃ）は第３のワークが第４のワークに対して下降点に位置した状態の正面図である。 本発明の第２の実施形態に係るロボット装置における第３のワークの位置と第３のワークへの反力との関係を示すグラフであり、（ａ）は軌道の補正前、（ｂ）は上空点での仮想の反力の演算時、（ｃ）は補正量の演算時である。 本発明の第３の実施形態に係るロボット装置により第５のワークを第６のワークに装着する動作を示す正面図である。（ａ）は第５のワークが第６のワークに対して上空点に位置した状態、（ｂ）は第５のワークが第６のワークに対して接触点に位置した状態、（ｃ）は第５のワークが接触点から上昇した状態の正面図である。 本発明の第３の実施形態に係るロボット装置により第５のワークを第６のワークに装着する動作を示す正面図である。（ａ）は第５のワークの係合部を中心側に移動した状態、（ｂ）は第５のワークを下降して第６のワークに当接した状態、（ｃ）は第５のワークが第６のワークに対して下降点に位置した状態の正面図である。 本発明の第３の実施形態に係るロボット装置における第５のワークの位置と第５のワークへの反力との関係を示すグラフであり、（ａ）は軌道の補正前、（ｂ）は上空点での仮想の反力の演算時、（ｃ）は補正量の演算時である。 本発明の第４の実施形態に係るロボット装置により第７のワークを第８のワークに装着する動作を示す正面図である。（ａ）は第７のワークが第８のワークに対して上空点に位置した状態、（ｂ）は第７のワークが第８のワークに対して接触点に位置した状態の正面図である。 本発明の第４の実施形態に係るロボット装置により第７のワークを第８のワークに装着する動作を示す正面図であり、（ａ）は第７のワークを傾斜させた状態、（ｂ）は第７のワークが第８のワークに対して下降点に位置した状態である。 本発明の第４の実施形態に係るロボット装置における第７のワークの位置と第７のワークへの反力との関係を示すグラフであり、（ａ）は軌道の補正前、（ｂ）は上空点での仮想の反力の演算時、（ｃ）は補正量の演算時である。

＜第１の実施形態＞
以下、本発明を実施するための第１の実施形態を、図面を参照しながら詳細に説明する。尚、本実施の形態では、ハンド２１に把持される第１のワークＷ１と、これを組み付ける第２のワークＷ２とはいずれも円筒形状であって、第１のワークＷ１の内径は第２のワークＷ２の外径とほぼ一致したものとしている。これにより、第１のワークＷ１の中心軸Ｗ１ｃと第２のワークＷ２の中心軸Ｗ２ｃとを一致させることで、第２のワークＷ２を第１のワークＷ１の内周側に着脱可能に組み付けられるようになっている（図２（ｃ）参照）。

図１に示すように、ロボット装置１は、ロボット本体２と、ロボット本体２を制御する制御装置（制御部）３とを備えている。ロボット本体２は、複数の関節を有する６軸の垂直多関節アーム（以下、アームと呼ぶ）２０と、エンドエフェクタであるハンド２１とを備えている。本実施の形態では、アーム２０として６軸の垂直多関節アームを適用しているが、軸数は用途や目的に応じて適宜変更してもよい。

アーム２０は、７つのリンク６１〜６７と、各リンク６１〜６７を揺動又は回動可能に連結する６つの関節７１〜７６とを備えている。各リンク６１〜６７としては、長さが固定されたものを採用している。但し、例えば、直動アクチュエータにより伸縮可能なリンクを採用してもよい。各関節７１〜７６は、例えば、不図示の関節機構及びモータ制御部を備え、制御装置３からの指令により動作するようになっている。基端リンク６１はベース６０に固定されており、ベース６０は架台１０にボルト等により締結固定されている。また、架台１０の近傍にワーク載置台１１が設置されており、ワーク載置台１１に第２のワークＷ２が載置されている。本実施の形態では、第２のワークＷ２は、少なくとも第１のワークＷ１に対して接触状態にある時はワーク載置台１１に固定されており、第１のワークＷ１に対して反力を与えるようになっている。

ハンド２１は、アーム２０の先端リンク６７に取り付けられて支持され、アーム２０の動作により位置及び姿勢の少なくとも一自由度が調整されるようになっている。ハンド２１は、ハンド本体２４と、ハンド本体２４に対して移動可能に配設されて、第１のワークＷ１を把持可能な複数の指２３とを備えている。ハンド本体２４の先端リンク６７との連結部には、力覚センサ（検出部）２５が設けられている。力覚センサ２５は、先端リンク６７に対してハンド２１から作用する外力を３方向（Ｘ，Ｙ，Ｚ）の力と３方向のモーメント（Ｍｘ，Ｍｙ，Ｍｚ）で検出可能になっており、検出値は反力Ｆとして制御装置３に入力されるようになっている。即ち、力覚センサ２５は、第１のワークＷ１及び第２のワークＷ２の間に相対的に作用する力の変化量を検出するようになっている。

ここで、第１のワークＷ１を第２のワークＷ２に対して接触動作を行う場合あるいは非接触動作を行う場合に、第１のワークＷ１と第２のワークＷ２とが接触する際の相対的な力関係について説明する。尚、ここでの接触動作とは、第１のワークＷ１及び第２のワークＷ２を非接触状態から接触状態に変化させることであり、非接触動作とは、第１のワークＷ１及び第２のワークＷ２を接触状態から非接触状態に変化させることである。また、本実施形態では、接触動作は装着動作であり、非接触動作は離脱動作である。第２のワークＷ２はワーク載置台１１に固定されているので、ワーク載置台１１、床、架台１０、ベース６０を介してアーム２０の基端リンク６１に相対的に固定されている。一方、第１のワークＷ１はハンド２１に把持されているので、アーム２０の先端リンク６７に相対的に固定されている。このため、第１のワークＷ１と第２のワークＷ２との支持関係は閉ループを構成しており、ロボット本体２を利用して第１のワークＷ１を第２のワークＷ２に接触させることにより、第１のワークＷ１と第２のワークＷ２との間には相対的に反力が発生する。この反力は、アーム２０とハンド２１との連結部に設けられた力覚センサ２５により検出され、力覚センサ２５は第１のワークＷ１と第２のワークＷ２との間に相対的に作用する力の変化量を検出可能になっている。

制御装置３は、コンピュータにより構成され、モータを制御することによりロボット本体２を制御可能になっている。制御装置３を構成するコンピュータは、例えばＣＰＵ３０と、各部を制御するためのプログラムを記憶するＲＯＭ３１と、データを一時的に記憶するＲＡＭ３２と、入出力インタフェース回路（Ｉ／Ｆ）３３とを備えている。また、制御装置３には、教示点を教示させるため等に利用するティーチングペンダント４が接続可能になっている。

制御装置３は、第１のワークＷ１を軌道上で移動させ第２のワークＷ２に対する装着動作を実行する際に、第１のワークＷ１及び第２のワークＷ２の接触状態において相対的に作用する反力（変化量）Ｆを力覚センサ２５により検出するようになっている。また、制御装置３は、力覚センサ２５により検出した反力Ｆに基づき軌道の補正量を演算し、演算して得た補正量に基づき軌道を補正するようになっている。

ここで、図２及び図３を利用して、制御装置３が第１のワークＷ１及び第２のワークＷ２を装着動作させて反力Ｆを検出し、軌道の補正量を演算して、軌道を修正する原理について説明する。

図２（ａ）に示すように、第１のワークＷ１が第２のワークＷ２の真上で接触することなく適宜距離だけ離れた状態（非接触状態）で、第１のワークＷ１の中心軸Ｗ１ｃと第２のワークＷ２の中心軸Ｗ２ｃとが一致するように配置する。この時の第１のワークＷ１の位置姿勢を上空点（第１の教示点）とする。この時は、力覚センサ２５により検出される反力Ｆは０となる。

そして、ロボット本体２により、第１のワークＷ１の中心軸Ｗ１ｃと第２のワークＷ２の中心軸Ｗ２ｃとを一致させながら、第１のワークＷ１を下方に移動させる。これにより、図２（ｂ）に示すように、第１のワークＷ１の下端面の一部と第２のワークＷ２の上端面の一部とが接触する。この時の第１のワークＷ１の位置姿勢を接触点（第３の教示点）とする。

更に、ロボット本体２により、第１のワークＷ１の中心軸Ｗ１ｃと第２のワークＷ２の中心軸Ｗ２ｃとを一致させながら、第１のワークＷ１を下方に移動させる。これにより、図２（ｃ）に示すように、第１のワークＷ１が第２のワークＷ２に最後まで嵌合するようになる。この時の第１のワークＷ１の位置姿勢を、下降点（第２の教示点）とする。この時、第１のワークＷ１の中心軸Ｗ１ｃと第２のワークＷ２の中心軸Ｗ２ｃとの位置や角度がずれていると、第１のワークＷ１の内周面と第２のワークＷ２の外周面とが接触し、互いに反力Ｆが発生する。また、上空点と下降点とを結んだ直線が所定の軌道となる。

この反力Ｆは、図２（ｂ）に示す端面の一部（とば口）同士が接触した状態（図３（ａ）の接触点）から、図２（ｃ）に示す最後まで嵌合した状態（図３（ａ）の下降点）まで、線形的に上昇する。即ち、本実施の形態では、反力Ｆは、第１のワークＷ１及び第２のワークＷ２の接触動作により第１のワークＷ１が変位する変化量である。尚、図２（ｂ）の状態と図２（ｃ）の状態との間、即ち、接触点と下降点との間の状態が、第１のワークＷ１と第２のワークＷ２との接触状態である。

例えば、図３（ａ）に示すように、上空点から接触点までのストロークをＡ２、接触点から下降点までのストロークをＡ１とし、力覚センサ２５により検出された反力Ｆが接触点から下降点まで線形的に上昇して下降点においてＦ１になったとする。この場合、接触点から下降点までの反力の傾きΔＦは、ΔＦ＝Ｆ１／Ａ１になる。

次に、図３（ｂ）に示すように、接触点から下降点までの反力の傾きΔＦを上空点にまで延長し、第２のワークＷ２が上空点まで延長されていたとした場合に第１のワークＷ１が受けるであろう反力Ｆ２を想定して演算する。この反力Ｆ２は、Ｆ２＝−ΔＦ×Ａ２になる。尚、図３（ｂ）に示すように、反力Ｆ２は、下降点における反力Ｆ１と、接触点における反力０とを延長した直線の上空点における交点として、作図によっても求めることができる。

更に、図３（ｃ）に示すように、上空点及び下降点について、反力Ｆを例えば水平方向の長さである補正量Ｘに変換する。このため、ロボット本体２の弾性率をＫとすると、上空点での補正量Ｘ２は、Ｘ２＝Ｆ２／Ｋになり、下降点での補正量Ｘ１は、Ｘ１＝Ｆ１／Ｋになる。これら上空点及び下降点に対する補正量Ｘ１，Ｘ２が、軌道の補正量となる。そして、現在の上空点の位置姿勢に補正量Ｘ２を加算して補正し、現在の下降点の位置姿勢に補正量Ｘ１を加算して補正し、補正後の上空点と下降点とを結んだ直線を補正後の軌道とすることで、軌道の補正を実行することができる。尚、ここでは、弾性率Ｋはロボット本体２の弾性率を用いているが、これには限定されず、例えば、ハンド２１の指２３の弾性率を用いてもよい。弾性率Ｋとしてどの部位の弾性率を用いるかは、例えばロボット装置１の構成やロボット本体２等の動作により、適宜設定することができる。

上述したロボット装置１の制御方法の手順について、図４及び図５に示すフローチャートに沿って説明する。

図４に示すように、ロボット装置１の立ち上げ時には、ユーザはオフラインで仮教示点を設定することで基本となる動作プログラムを作成し（ステップＳ１）、それをロボット装置１の実機にインストールする（ステップＳ２）。その後、ユーザはプログラムのデバッグを行い（ステップＳ３）、それが完了した時点で教示作業を開始する（ステップＳ４）。

教示作業では、第２のワークＷ２又は不図示の専用治工具を用いて、第１のワークＷ１を位置決めし、それを教示点として登録することでロボット装置１に教示を行う。本実施の形態では、図２（ａ）に示す第１のワークＷ１の位置姿勢を上空点、図２（ｃ）に示す第１のワークＷ１の位置姿勢を下降点として教示する。制御装置３は、上空点と下降点とを直結した直線を軌道として設定し、動作プログラムが完成して、教示作業が終了する。軌道は、非接触状態の第１のワークＷ１の位置姿勢である上空点と、接触状態の第１のワークＷ１の位置姿勢である下降点と、接触状態にある第１のワークＷ１の位置姿勢であり、軌道上で上空点及び下降点の間に位置する接触点と、に基づいて演算される。即ち、本実施の形態では、下降点における軌道と、接触点における軌道とは、一直線状の軌道である。制御装置３は、教示作業後、図２に示すように、ロボット本体２を駆動して確認動作を行い（ステップＳ５）、実際の組立作業に移行する（ステップＳ６）。組立作業では、制御装置３は動作プログラムを連続運転させる。

ロボット装置１の保守メンテナンス後や段取り換えの後には、図５に示すように、軌道補正の作業を行ってから組立作業を行うようにする。まず、ユーザはロボット装置１を起動する（ステップＳ１１）。そして、制御装置３は、動作プログラムを読み込み（ステップＳ１２）、準備動作を実行する（ステップＳ１３）。準備動作では、ロボット本体２を低速で動作させ、第１のワークＷ１及びロボット本体２が他の部材への干渉等していないかの確認を、ユーザの目視により行う。

動作の確認後、制御装置３は、第１のワークＷ１を上空点から下降点まで低速動作で移動させ（装着動作）、力覚センサ２５により、少なくとも接触点と下降点とにおける第１のワークＷ１に作用する反力Ｆを検出する（ステップＳ１４、第１の検出工程）。ここでは、制御装置３は、力覚センサ２５により反力Ｆの検出を常時行っており、上空点から下降点までの軌道の全域に亘って反力Ｆを検出するものとしている（図３（ａ）参照）。尚、本実施の形態では、第１のワークＷ１を上空点から下降点まで低速で移動させているが、これには限られず実速度で移動するようにしてもよい。

そして、制御装置３は、検出された反力Ｆに基づいて、下降点における水平方向の位置の補正量Ｘ１、上空点における水平方向の位置の補正量Ｘ２を演算する（ステップＳ１５、第１の演算工程）（図３（ｂ）（ｃ）参照）。即ち、第１の演算工程では、制御装置３が、第１の検出工程で検出した力の変化量に基づき、軌道の補正量として、上空点及び下降点の各位置姿勢の補正量を演算する。更に、制御装置３は、位置の補正量Ｘ１，Ｘ２に基づいて、上空点及び下降点の２つの教示点の水平方向の位置を補正する（ステップＳ１６、第１の補正工程）。これにより、上空点及び下降点の間の軌道が補正される。即ち、第１の補正工程では、制御装置３が、第１の演算工程で演算した上空点及び下降点の各位置姿勢の補正量に基づき、上空点及び下降点の各位置姿勢を補正することにより、軌道を補正する。

１回目の軌道補正の後、制御装置３は、第１のワークＷ１を上空点から下降点まで低速動作で移動させ、再び力覚センサ２５により、少なくとも接触点と下降点とにおける第１のワークＷ１に作用する反力Ｆを検出する（ステップＳ１７、第２の検出工程）。ステップＳ１４における１回目の反力Ｆの検出と同様に、制御装置３は、力覚センサ２５により反力Ｆの検出を常時行っており、上空点から下降点までの軌道の全域に亘って反力Ｆを検出するものとしている。

そして、制御装置３は、検出された反力Ｆが予め設定した規定値以内であるか否かを判断する（ステップＳ１８）。ここでの規定値は、その規定値以内の反力Ｆであれば、軌道の直線性が十分であるとする値としており、適宜設定することができる。制御装置３が、検出された反力Ｆが規定値以内であると判断した場合は、軌道が補正されたものとして組立作業に移行する（ステップＳ１９）。

制御装置３が、反力Ｆが規定値以内ではないと判断した場合は、制御装置３は、検出された反力Ｆに基づいて、下降点における姿勢（傾斜）の補正量θ１、上空点における姿勢（傾斜）の補正量θ２を演算する（ステップＳ２０、第２の演算工程）。更に、制御装置３は、補正量θ１，θ２に基づいて、上空点及び下降点の２つの教示点の姿勢（傾斜）を補正する（ステップＳ２１、第２の補正工程）。これにより、上空点及び下降点の間の軌道が、再び補正される。２回目の軌道補正の後、制御装置３はステップＳ１７以降を実行し、反力Ｆが規定値以下になって軌道が補正されるまで繰り返し実行する。

上述したように本実施の形態のロボット装置１によれば、制御装置３が、第１のワークＷ１を軌道上で移動させ第２のワークＷ２に対する装着動作を実行させる。その際に、制御装置３は、第１のワークＷ１及び第２のワークＷ２の接触動作により第１のワークＷ１が変位して発生する反力Ｆを検出する。そして、制御装置３は、反力Ｆに基づいて軌道の補正量Ｘ１，Ｘ２，θ１，θ２を演算し、それらに基づいて軌道を補正する。このため、専用治工具を用いて軌道を補正することなく、保持した第１のワークＷ１を第２のワークＷ２に対して高精度に組み付けることができるようになる。しかも、本実施の形態のロボット装置１によれば、組立作業に移行した後も、同じ補正教示点を使用し続けるため、ロボット装置が特異点を使うことが無く、安定した動作を実現することができる。

また、本実施の形態のロボット装置１によれば、制御装置３は、反力Ｆに基づいて最初に第１のワークＷ１の位置を補正する（図５のステップＳ１４〜Ｓ１６）。そして、制御装置３は、この補正でも軌道のずれを規定量以内に抑えられない場合に、第１のワークＷ１の姿勢を補正する（図５のステップＳ１７〜Ｓ２１）。即ち、軌道を補正するために、２段階で補正を行っている。ここで、一般に、アーム２０のメンテナンス時における教示点の位置姿勢の誤差としては、姿勢よりも位置で発生し易い。このため、本実施の形態のように、先に位置を補正し、次に姿勢を補正することにより、最初の補正のみで軌道の誤差を規制量以内に抑えられる可能性が高く、時短で効率良い軌道補正作業を実現することができる。

尚、上述した実施の形態では、制御装置３は、反力Ｆに基づいて最初に第１のワークＷ１の位置を補正し、軌道のずれを規定量以内に抑えられない場合に、第１のワークＷ１の姿勢を補正する場合について説明したが、これには限られない。例えば、制御装置３は、反力Ｆに基づいて最初に第１のワークＷ１の姿勢を補正し、軌道のずれを規定量以内に抑えられない場合に、第１のワークＷ１の位置を補正するようにしてもよい。このように、位置と姿勢との補正を行う順番は、例えば組み付けるワーク同士の形状やロボット本体２の性能等により適宜設定することができる。

また、上述した実施の形態では、制御装置３は、第１のワークＷ１が第２のワークＷ２から受けた反力Ｆを検出部としての力覚センサ２５により検出し、その反力Ｆに基づいて各補正量を演算しているが、これには限られない。例えば、第１のワークＷ１が第２のワークＷ２との接触により位置姿勢を変位した変位量を検出部としてのカメラ等の位置センサにより検出し、その変位量に基づいて各補正量を演算するようにしてもよい。例えば、カメラを利用する場合は、力覚センサ２５を省略することができるので、ハンド２１の軽量化を図ることができる。また、検出部として力覚センサ２５を用いて反力Ｆを検出するか、カメラ５を用いて位置姿勢の変位量を検出するかは、例えば組み付けるワーク同士の形状やロボット本体２の性能等により適宜設定することができる。

ここで、検出部としてカメラ５（図１参照）を用いると共に、制御装置３が最初に第１のワークＷ１の位置を補正し、軌道のずれを規定量以内に抑えられない場合に、第１のワークＷ１の姿勢を補正する場合について、図６に沿って説明する。カメラ５は、天井に設置されたステレオカメラであるが、これには限られず、例えば第１のワークＷ１を側方から撮影可能なカメラとしたり、あるいは第１のワークＷ１の可動方向によっては単眼のカメラであってもよい。また、カメラ５は第１のワークＷ１を撮影するが、これには限られず、例えばハンド２１の指２３等を撮影することで、第１のワークＷ１の位置姿勢を検出するようにしてもよい。即ち、カメラ５は、第１のワークＷ１及び第２のワークＷ２の相対位置姿勢の変位量を検出するようになっている。尚、検出部としては、第１のワークＷ１及び第２のワークＷ２の間に相対的に作用する反力Ｆと第１のワークＷ１及び第２のワークＷ２の相対位置姿勢の変位量との少なくとも一方を検出するものであればよい。

まず、ユーザはロボット装置１を起動する（ステップＳ３１）。そして、制御装置３は、動作プログラムを読み込み（ステップＳ３２）、準備動作を実行する（ステップＳ３３）。ここまでの動作は、図５のステップＳ１１〜Ｓ１３と同様である。

動作の確認後、制御装置３は、第１のワークＷ１を上空点から下降点まで低速動作で移動させ、カメラ５により、少なくとも接触点と下降点とにおける第１のワークＷ１の位置姿勢の変位量を検出する（ステップＳ３４、第１の検出工程）。ここでは、カメラ５により、非接触状態と接触状態とを切り換えた時の第１のワークＷ１及び第２のワークＷ２の相対姿勢の変位量を検出する。制御装置３は、カメラ５により第１のワークＷ１の位置姿勢の検出を常時行っており、上空点から下降点までの軌道の全域に亘って検出するものとしている。

そして、制御装置３は、検出された位置姿勢の変位量に基づいて、下降点における姿勢（傾斜）の補正量θ１、上空点における姿勢（傾斜）θ２を演算する（ステップＳ３５、第１の演算工程）。更に、制御装置３は、姿勢の補正量θ１，θ２に基づいて、上空点及び下降点の２つの教示点の姿勢を補正する（ステップＳ３６、第１の補正工程）。これにより、上空点及び下降点の間の軌道が補正される。

１回目の軌道補正の後、制御装置３は、第１のワークＷ１を上空点から下降点まで低速動作で移動させ、再びカメラ５により、少なくとも接触点と下降点とにおける第１のワークＷ１の位置姿勢の変位量を検出する（ステップＳ３７、第２の検出工程）。ここでは、カメラ５により、非接触状態と接触状態とを切り換えた時の第１のワークＷ１及び第２のワークＷ２の相対位置の変位量を検出する。ステップＳ３４における１回目の位置姿勢の変位量の検出と同様に、制御装置３は、カメラ５により変位量の検出を常時行っており、上空点から下降点までの軌道の全域に亘って変位量を検出するものとしている。

そして、制御装置３は、検出した変位量が予め設定した規定値以内であるか否かを判断する（ステップＳ３８）。ここでの規定値は、その規定値以内の変位量であれば、軌道の直線性が十分であるとする値としており、適宜設定することができる。制御装置３が、検出された変位量が規定値以内であると判断した場合は、軌道が補正されたものとして組立作業に移行する（ステップＳ３９）。

制御装置３が、変位量が規定値以内ではないと判断した場合は、制御装置３は、検出された変位量に基づいて、下降点における水平方向の位置の補正量Ｘ１、上空点における水平方向の位置の補正量Ｘ２を演算する（ステップＳ４０、第２の演算工程）。即ち、第２の演算工程では、制御装置３が、第２の検出工程で検出した相対位置姿勢の変位量に基づき、軌道の補正量として、上空点及び下降点の各位置姿勢の補正量を演算する。更に、制御装置３は、補正量Ｘ１，Ｘ２に基づいて、上空点及び下降点の２つの教示点の位置を補正する（ステップＳ４１、第２の補正工程）。これにより、上空点及び下降点の間の軌道が、再び補正される。即ち、第２の補正工程では、制御装置３が、第２の演算工程で演算した上空点及び下降点の各位置姿勢の補正量に基づき、上空点及び下降点の各位置姿勢を補正することにより、軌道を補正する。２回目の軌道補正の後、制御装置３はステップＳ３７以降を実行し、変位量が規定値以下になって軌道が補正されるまで繰り返し実行する。

また、上述した実施の形態では、例えば図５のステップＳ１４のように、第１のワークＷ１を上空点から下降点に移動させる装着動作において力覚センサ２５を用いて反力Ｆを検出する場合について説明したが、これには限られない。例えば、第１のワークＷ１と第２のワークＷ２とを装着した状態でハンド２１により第１のワークＷ１を把持し、第１のワークＷ１を下降点から上空点まで引き上げる離脱動作において力覚センサ２５を用いて反力Ｆを検出するようにしてもよい。

また、上述した実施の形態では、反力Ｆを接触点と下降点との２点で検出した場合について説明したが、これには限られない。例えば、第１のワークＷ１と第２のワークＷ２とが非接触状態であれば反力Ｆは０であるので、少なくとも下降点での反力Ｆのみを検出することで接触状態における反力Ｆの傾きΔＦを算出し、軌道を補正するようにしてもよい。あるいは、接触点と下降点との間の３点以上で反力Ｆを検出して、例えば最小二乗法等で反力Ｆの傾きΔＦを算出するようにしてもよい。

また、上述した実施の形態では、保守メンテナンス時等に軌道の補正を実施するようにした場合について説明したが、これには限られず、例えば、図４のステップＳ５の確認動作時に実施するようにしてもよい。

また、上述した実施の形態では、軌道は一直線状である場合について説明したが、これには限られない。例えば、軌道は折線状であってもよく、あるいは円弧状や螺旋状等の曲線状であってもよい。いずれの場合も、第１のワークＷ１と第２のワークＷ２との接触状態における反力Ｆや位置姿勢の変位量を検出し、それに基づいて軌道を補正することで、高精度な組み付けを実現することができる。

＜第２の実施形態＞
次に、本発明を実施するための第２の実施形態を、図面を参照しながら詳細に説明する。尚、本実施の形態では、組み付けを行う第３のワーク（第１のワーク）Ｗ３及び第４のワーク（第２のワーク）Ｗ４の形状が、第１の実施形態の各ワークＷ１，Ｗ２の形状と異なっている。なお、ロボット装置１の構成は、第１の実施形態と同様であるので、符号を同じくして詳細な説明を省略する。

本実施形態では、図７（ａ）に示すように、ハンド２１に把持される第３のワークＷ３と、これを組み付ける第４のワークＷ４とは、いずれも円筒形状であって、第３のワークＷ３の内径は第４のワークＷ４の外径とほぼ一致している。第３のワークＷ３は、内周面に中心に向けて突出した形状で、周方向に等間隔を開けて３か所に設けられた突起部Ｗ３ａを有している。

第４のワークＷ４は、外周面において凹んだ形状で、周方向に等間隔を開けて３か所に設けられた略Ｌ字形状の係合溝Ｗ４ａを有している。係合溝Ｗ４ａは、第４のワークＷ４の軸方向に平行で第４のワークＷ４の縁部から軸方向の中央部まで延設された平行部Ｗ４ｂと、平行部Ｗ４ｂの軸方向中央部側の端部に直交して連続する直交部Ｗ４ｄと、を有している。また、３つの係合溝Ｗ４ａの各平行部Ｗ４ｂは、第４のワークＷ４の軸方向の一方側に開放している。３つの係合溝Ｗ４ａの各直交部Ｗ４ｄは、各平行部Ｗ４ｂから直交する周方向を同じくしている。

第３のワークＷ３の各突起部Ｗ３ａは、それぞれ第４のワークＷ４の係合溝Ｗ４ａに挿入可能になっている。第３のワークＷ３の突起部Ｗ３ａが第４のワークＷ４の係合溝Ｗ４ａに挿入して係合することにより、第３のワークＷ３が第４のワークＷ４に対して装着される。即ち、第３のワークＷ３を第４のワークＷ４に装着する際は、第３のワークＷ３の突起部Ｗ３ａを第４のワークＷ４の軸方向一端部から平行部Ｗ４ｂに挿入する（図８（ａ）参照）。そして、第３のワークＷ３の突起部Ｗ３ａが第４のワークＷ４の平行部Ｗ４ｂの最奥部に突き当たってから（図８（ｂ）参照）、第３のワークＷ３を回転させることで、第３のワークＷ３の突起部Ｗ３ａが第４のワークＷ４の直交部Ｗ４ｄに挿入される。更に、第３のワークＷ３の突起部Ｗ３ａが第４のワークＷ４の直交部Ｗ４ｄの最奥部に突き当たると（図８（ｃ）参照）、第３のワークＷ３が第４のワークＷ４に対して装着される。一方、第３のワークＷ３を第４のワークＷ４から取り外す際は、上述と逆の動作を行う。このように、本実施の形態では、第３のワークＷ３は、スラスト方向の力を受け止めるスナップリングであり、第４のワークＷ４は、スナップリングが着脱可能に取り付けられるユニットとなっている。

ここで、図７乃至図９を利用して、制御装置３が第３のワークＷ３及び第４のワークＷ４を装着動作させて反力Ｆを検出し、軌道の補正量を演算して、軌道を修正する原理について説明する。なお、本実施形態では、反力Ｆとは、力覚センサ２５により検出された先端リンク６７に対してハンド２１から作用する外力のうち、Ｚ方向の力、即ち先端リンク６７の軸方向に沿った力としている。また、ここでの反力Ｆとしては、力覚センサ２５により検出された先端リンク６７に対してハンド２１から作用する外力のうち、Ｚ方向の力以外のＸ，Ｙ方向の力及び３方向のモーメント（Ｍｘ，Ｍｙ，Ｍｚ）は含まない。また、力覚センサ２５の座標系はハンド２１の座標系と一体化されており、ハンド２１の位置姿勢の変化に伴って力覚センサ２５の座標系も移動・回転する。

図７（ｂ）に示すように、第３のワークＷ３が第４のワークＷ４の真上で接触することなく適宜距離だけ離れた状態（非接触状態）で、第３のワークＷ３の中心軸Ｗ３ｃと第４のワークＷ４の中心軸Ｗ４ｃとが一致するように配置する。この時の第３のワークＷ３の位置姿勢を上空点（第１の教示点）とする。この時は、力覚センサ２５により検出される反力Ｆは０となる。

そして、ロボット本体２により、第３のワークＷ３の中心軸Ｗ３ｃと第４のワークＷ４の中心軸Ｗ４ｃとを一致させながら、第３のワークＷ３を下方に移動させる。これにより、図８（ａ）に示すように、第３のワークＷ３の下端面の一部と第４のワークＷ４の上端面の一部とが接触する。この時の第３のワークＷ３の位置姿勢を接触点（第３の教示点）とする。

更に、ロボット本体２により、第３のワークＷ３の中心軸Ｗ３ｃと第４のワークＷ４の中心軸Ｗ４ｃとを一致させながら、第３のワークＷ３を下方に移動させる。これにより、図８（ｂ）に示すように、第３のワークＷ３の突起部Ｗ３ａが、第４のワークＷ４の平行部Ｗ４ｂの最下部に突き当たる。この時の第３のワークＷ３の位置姿勢を切換点とする。

そして、ロボット本体２により、第３のワークＷ３の中心軸Ｗ３ｃと第４のワークＷ４の中心軸Ｗ４ｃとを一致させながら、第３のワークＷ３をＺ軸（中心軸Ｗ３ｃ，Ｗ４ｃ）を中心にＲ方向に回転させる。これにより、図８（ｃ）に示すように、第３のワークＷ３の突起部Ｗ３ａが、第４のワークＷ４の直交部Ｗ４ｄの最奥部まで嵌合するようになる。この時の第３のワークＷ３の位置姿勢を、下降点（第２の教示点）とする。なお、第３のワークＷ３のＺ軸を中心とする回転に伴い、ハンド２１の座標系も回転し、Ｘ軸及びＹ軸の方向が変化する。このとき、第３のワークＷ３は、Ｚ軸を中心に回転するので、Ｚ軸方向の位置は変わらない。また、上空点と下降点とを結んだ直線が所定の軌道となる。即ち、本実施の形態では、接触点における軌道は直線状の軌道であり、下降点における軌道は接触点における軌道の直線と同方向の軸を中心とする回転方向の軌道である。

この反力Ｆは、図７（ｂ）に示す端面の一部（とば口）同士が接触した状態（図９（ａ）の接触点）から、図８（ｃ）に示す最後まで嵌合した状態（図９（ａ）の下降点（切換点））まで、直線的に上昇する。即ち、本実施の形態では、反力Ｆは、第３のワークＷ３及び第４のワークＷ４の接触動作により第３のワークＷ３が変位する変化量である。尚、本実施形態では、図８（ａ）の状態及び図８（ｃ）の状態、即ち、接触点と下降点との各状態が、それぞれ第３のワークＷ３と第４のワークＷ４との接触状態である。

ここで、例えば、図９（ａ）に示すように、上空点から接触点までのＺ方向のストロークをＡ２、接触点から下降点（切換点）までのＺ方向のストロークをＡ１とする。力覚センサ２５により検出されたＺ方向の反力Ｆが、接触点から下降点まで直線的に上昇して、下降点（切換点）においてＦ１になったとする。また、予め計測した相対位置姿勢の変位がない場合の参照波形を理論値により取得し、その時の力覚センサ２５により検出されたＺ方向の反力Ｆが、接触点から下降点まで折線的に上昇して下降点においてＦ０になったとする。この場合、相対位置姿勢の変位がある場合とない場合との接触点から下降点までの反力Ｆの差分の傾きΔＦは、ΔＦ＝（Ｆ１−Ｆ０）／Ａ１になる。

次に、図９（ｂ）に示すように、接触点から下降点までの反力Ｆの差分の傾きΔＦを上空点にまで延長し、第４のワークＷ４が上空点まで延長されていたとした場合に第３のワークＷ３が受けるであろう反力Ｆ２を想定して演算する。この反力Ｆ２は、Ｆ２＝ΔＦ×Ａ２になる。尚、図９（ｂ）に示すように、反力Ｆ２は、下降点（反力Ｆ１）及び接触点（反力０）を結んだ直線の上空点における交点と、下降点（反力Ｆ０）及び接触点（反力０）を結んだ直線の上空点における交点と、の差分として、作図によっても求めることができる。

更に、図９（ｃ）に示すように、上空点及び下降点について、反力ＦをＺ方向の長さである補正量Ｚに変換する。このため、ロボット本体２の弾性率をＫとすると、上空点での補正量Ｚ２は、Ｚ２＝Ｆ２／Ｋになり、下降点での補正量Ｚ１は、Ｚ１＝Ｆ１／Ｋになる。これら上空点及び下降点に対する補正量Ｚ１，Ｚ２が、軌道の補正量となる。そして、現在の上空点の位置姿勢のＺ方向に補正量Ｚ２を加算して補正し、現在の下降点の位置姿勢のＺ方向に補正量Ｚ１を加算して補正し、補正後の上空点と下降点とを結んだ直線を補正後の軌道とすることで、軌道の補正を実行することができる。

上述したロボット装置１の制御方法の手順は、第１の実施形態（図４及び図５）と同様であるので、詳細な説明を省略する。また、第３のワークＷ３に対する位置の補正と姿勢の補正との順序は、第１の実施形態と同様に適宜設定することができる。

上述したように本実施の形態のロボット装置１によっても、制御装置３が、第３のワークＷ３を軌道上で移動させ第４のワークＷ４に対する装着動作を実行させる。その際に、制御装置３は、第３のワークＷ３及び第４のワークＷ４の接触動作により第３のワークＷ３が変位して発生する反力Ｆを検出する。そして、制御装置３は、反力Ｆに基づいて軌道のＺ方向の補正量Ｚ１，Ｚ２を演算し、それらに基づいて軌道を補正する。このため、専用治工具を用いて軌道を補正することなく、保持した第３のワークＷ３を第４のワークＷ４に対して高精度に組み付けることができるようになる。

また、本実施の形態においても、第１の実施形態と同様に、力覚センサ２５を用いずに、第３のワークＷ３が第４のワークＷ４との接触により位置姿勢を変位した変位量をカメラ５により検出し、その変位量に基づいて各補正量を演算するようにしてもよい。

＜第３の実施形態＞
次に、本発明を実施するための第３の実施形態を、図面を参照しながら詳細に説明する。尚、本実施の形態では、組み付けを行う第５のワーク（第１のワーク）Ｗ５及び第６のワーク（第２のワーク）Ｗ６の形状が、第１の実施形態の各ワークＷ１，Ｗ２の形状と異なっている。なお、ロボット装置１の構成は、第１の実施形態と同様であるので、符号を同じくして詳細な説明を省略する。

本実施形態では、例えば、図１１（ａ）〜（ｃ）に示すように、ハンド２１に把持される第５のワークＷ５は円板形状であり、これを組み付ける第６のワークＷ６は円筒形状である。第５のワークＷ５は、一側面から中心軸Ｗ５ｃに沿って突出し、先端を外周側に曲折した爪形状の係合部Ｗ５ａを有している。第６のワークＷ６は、軸方向の一端部から内周側にフランジ状に突出した形状の縁部Ｗ６ａを有している。

第５のワークＷ５と第６のワークＷ６とは、第５のワークＷ５の係合部Ｗ５ａが第６のワークＷ６の縁部Ｗ６ａに係合した際に、互いの中心軸Ｗ５ｃ，Ｗ６ｃが一致するように形成されている（図１１（ｃ）参照）。そして、第５のワークＷ５の係合部Ｗ５ａが第６のワークＷ６の縁部Ｗ６ａに係合することにより、第５のワークＷ５が第６のワークＷ６に対して装着される。即ち、第５のワークＷ５を第６のワークＷ６に装着する際は、第５のワークＷ５を第６のワークＷ６の縁部Ｗ６ａ側から接近させ（図１１（ａ）参照）、第５のワークＷ５を第６のワークＷ６の縁部Ｗ６ａに面接触させる（図１１（ｂ）参照）。そして、第５のワークＷ５を軸方向に垂直に摺動させ、第５のワークＷ５の係合部Ｗ５ａを第６のワークＷ６の縁部Ｗ６ａに係合する（図１１（ｃ）参照）。これにより、第５のワークＷ５が第６のワークＷ６に対して着脱可能に装着される。一方、第５のワークＷ５を第６のワークＷ６から取り外す際は、上述と逆の動作を行う。

ここで、図１０乃至図１２を利用して、制御装置３が第５のワークＷ５及び第６のワークＷ６を装着動作させて反力Ｆを検出し、軌道の補正量を演算して、軌道を修正する原理について説明する。なお、本実施形態では、反力Ｆとは、力覚センサ２５により検出された先端リンク６７に対してハンド２１から作用する外力のうち、Ｘ方向の力としている。即ち、反力Ｆは、先端リンク６７の軸方向（Ｚ方向）に直交し、第５のワークＷ５の係合部Ｗ５ａを第６のワークＷ６の縁部Ｗ６ａに係脱する方向であるＸ方向の力としている（図１１（ｃ）等参照）。また、ここでの反力Ｆとしては、力覚センサ２５により検出された先端リンク６７に対してハンド２１から作用する外力のうち、Ｘ方向の力以外のＹ，Ｚ方向の力及び３方向のモーメント（Ｍｘ，Ｍｙ，Ｍｚ）は含まない。

図１０（ａ）に示すように、第５のワークＷ５を、第６のワークＷ６に接触することなく適宜距離だけ離れた状態（非接触状態）にして配置する。この時、第５のワークＷ５は、水平で、係合部Ｗ５ａの真下に第６のワークＷ６が存在せず、係合部Ｗ５ａの最下部が第６のワークＷ６の最上面よりも下方に位置した状態でハンド２１により保持されるようにする。この時の第５のワークＷ５の位置姿勢を上空点（第１の教示点）とする。この時は、力覚センサ２５により検出される反力Ｆは０となる。

そして、ロボット本体２により、第５のワークＷ５の中心軸Ｗ５ｃと第６のワークＷ６の中心軸Ｗ６ｃとが一致する方向（Ｘ方向）に第５のワークＷ５を移動させる。これにより、図１０（ｂ）に示すように、第５のワークＷ５の係合部Ｗ５ａが第６のワークＷ６の側面に当接する。この時の第５のワークＷ５の位置姿勢を接触点（第３の教示点）とする。

更に、ロボット本体２により、第５のワークＷ５を上方に移動させる。これにより、図１０（ｃ）に示すように、第５のワークＷ５の係合部Ｗ５ａが第６のワークＷ６の側方に位置しなくなる。そして、ロボット本体２により、第５のワークＷ５の係合部Ｗ５ａが第６のワークＷ６の縁部Ｗ６ａを超えるまで第５のワークＷ５をＸ方向に移動させる。これにより、図１１（ａ）に示すように、第５のワークＷ５の係合部Ｗ５ａが第６のワークＷ６の縁部Ｗ６ａよりも中心軸Ｗ６ｃ側に位置する。

更に、ロボット本体２により、第５のワークＷ５を下方に移動させる。これにより、図１１（ｂ）に示すように、第５のワークＷ５が、第６のワークＷ６の上面に突き当たる。このとき、第５のワークＷ５の係合部Ｗ５ａは、第６のワークＷ６の縁部Ｗ６ａの内側に入り込み、係合部Ｗ５ａと縁部Ｗ６ａとは干渉しない。

そして、ロボット本体２により、第５のワークＷ５の中心軸Ｗ５ｃと第６のワークＷ６の中心軸Ｗ６ｃとを一致させるよう、即ち係合部Ｗ５ａを縁部Ｗ６ａに係合するように第５のワークＷ５をＸ方向に移動させる。これにより、図１１（ｃ）に示すように、第５のワークＷ５の係合部Ｗ５ａが、第６のワークＷ６の縁部Ｗ６ａに係合する。この時の第５のワークＷ５の位置姿勢を、下降点（第２の教示点）とする。この時、第５のワークＷ５の中心軸Ｗ５ｃと第６のワークＷ６の中心軸Ｗ６ｃとの位置や角度がずれていると、第５のワークＷ５の係合部Ｗ５ａ及び第６のワークＷ６の縁部Ｗ６ａ以外の箇所で互いに接触し、互いに余分な反力Ｆが発生する。また、上空点と下降点とを結んだ直線が所定の軌道となる。即ち、本実施の形態では、下降点における軌道と接触点における軌道とは、同方向の直線状の軌道であり、かつ、各軌道の間では第５のワークＷ５及び第６のワークＷ６は非接触である。

この反力Ｆは、図１０（ｂ）に示す第５のワークＷ５の係合部Ｗ５ａが第６のワークＷ６の側部に接触した状態（図１２（ａ）の接触点）から、図１１（ｃ）に示す最後まで嵌合した状態（図１２（ａ）の下降点）まで、折線状に昇降する。即ち、本実施の形態では、反力Ｆは、第５のワークＷ５及び第６のワークＷ６の接触動作により第５のワークＷ５が変位する変化量である。尚、図１０（ｂ）の状態及び図１１（ｃ）の状態、即ち、接触点と下降点との各状態が、それぞれ第５のワークＷ５と第６のワークＷ６との接触状態である。

ここで、例えば、図１２（ａ）に示すように、上空点から接触点までのＸ方向のストロークをＡ２、接触点から下降点までのＸ方向のストロークをＡ１とする。力覚センサ２５により検出されたＸ方向の反力Ｆが、接触点から下降点まで折線的に昇降して、接触点においてｆ１、下降点においてＦ１になったとする。また、予め計測した相対位置姿勢の変位がない場合の参照波形を理論値により取得し、その時の力覚センサ２５により検出されたＸ方向の反力Ｆが、接触点から下降点まで折線的に昇降して、接触点においてｆ０、下降点においてＦ０になったとする。この場合、相対位置姿勢の変位がある場合とない場合との接触点から下降点までの反力Ｆの差分の傾きΔＦは、ΔＦ＝（（Ｆ１−Ｆ０）−（ｆ１−ｆ０））／Ａ１になる。

次に、図１２（ｂ）に示すように、接触点から下降点までの反力Ｆの差分の傾きΔＦを上空点にまで延長し、第６のワークＷ６が上空点まで延長されていたとした場合に第５のワークＷ５が受けるであろう反力Ｆ２を想定して演算する。この反力Ｆ２は、Ｆ２＝ΔＦ×Ａ２＋（ｆ１−ｆ０）になる。尚、同図に示すように、反力Ｆ２は、下降点（反力Ｆ１）及び接触点（反力ｆ１）を結んだ直線の上空点における交点と、下降点（反力Ｆ０）及び接触点（反力ｆ０）を結んだ直線の上空点における交点と、の差分として、作図によっても求めることができる。

更に、図１２（ｃ）に示すように、上空点及び下降点について、反力ＦをＸ方向の長さである補正量Ｘに変換する。このため、ロボット本体２の弾性率をＫとすると、上空点での補正量Ｘ２は、Ｘ２＝Ｆ２／Ｋになり、下降点での補正量Ｘ１は、Ｘ１＝Ｆ１／Ｋになる。これら上空点及び下降点に対する補正量Ｘ１，Ｘ２が、軌道の補正量となる。そして、現在の上空点の位置姿勢のＸ方向に補正量Ｘ２を加算して補正し、現在の下降点の位置姿勢のＸ方向に補正量Ｘ１を加算して補正し、補正後の上空点と下降点とを結んだ直線を補正後の軌道とすることで、軌道の補正を実行することができる。

上述したロボット装置１の制御方法の手順は、第１の実施形態（図４及び図５）と同様であるので、詳細な説明を省略する。また、第５のワークＷ５に対する位置の補正と姿勢の補正との順序は、第１の実施形態と同様に適宜設定することができる。

上述したように本実施の形態のロボット装置１によっても、制御装置３が、第５のワークＷ５を軌道上で移動させ第６のワークＷ６に対する装着動作を実行させる。その際に、制御装置３は、第５のワークＷ５及び第６のワークＷ６の接触動作により第５のワークＷ５が変位して発生する反力Ｆを検出する。そして、制御装置３は、反力Ｆに基づいて軌道のＸ方向の補正量Ｘ１，Ｘ２を演算し、それらに基づいて軌道を補正する。このため、専用治工具を用いて軌道を補正することなく、保持した第５のワークＷ５を第６のワークＷ６に対して高精度に組み付けることができるようになる。

また、本実施の形態においても、第１の実施形態と同様に、力覚センサ２５を用いずに、第５のワークＷ５が第６のワークＷ６との接触により位置姿勢を変位した変位量をカメラ５により検出し、その変位量に基づいて各補正量を演算するようにしてもよい。

＜第４の実施形態＞
次に、本発明を実施するための第４の実施形態を、図面を参照しながら詳細に説明する。尚、本実施の形態では、組み付けを行う第７のワーク（第１のワーク）Ｗ７及び第８のワーク（第２のワーク）Ｗ８の形状が、第１の実施形態の各ワークＷ１，Ｗ２の形状と異なっている。なお、ロボット装置１の構成は、第１の実施形態と同様であるので、符号を同じくして詳細な説明を省略する。

本実施形態では、例えば、図１３（ａ）〜図１４（ｂ）に示すように、ハンド２１に把持される第７のワークＷ７は円板形状であり、これを組み付ける第８のワークＷ８は円筒形状である。第７のワークＷ７は、一側面から中心軸Ｗ７ｃに沿って突出した形状の突出部Ｗ７ａと、突出部Ｗ７ａの先端部から内周側に曲折した爪形状の係合部Ｗ７ｂと、突出部Ｗ７ａの根元部から内周側に突出した形状の爪部Ｗ７ｄと、を有している。第８のワークＷ８は、軸方向の一端部から内周側にフランジ状に突出した形状の縁部Ｗ８ａを有している。

第７のワークＷ７と第８のワークＷ８とは、第８のワークＷ８に対して第７のワークＷ７が９０°傾斜して、第７のワークＷ７の突出部Ｗ７ａと係合部Ｗ７ｂと爪部Ｗ７ｄとが第８のワークＷ８の縁部Ｗ８ａに係合することで装着される（図１４（ｂ）参照）。即ち、第７のワークＷ７と第８のワークＷ８とは、第７のワークＷ７の突出部Ｗ７ａと係合部Ｗ７ｂと爪部Ｗ７ｄとが第８のワークＷ８の縁部Ｗ８ａに係合した際に、互いの中心軸Ｗ７ｃ，Ｗ８ｃが直交して装着されるように形成されている。

第７のワークＷ７を第８のワークＷ８に装着する際は、互いの中心軸Ｗ７ｃ，Ｗ８ｃをＺ軸方向に平行にした状態で、第７のワークＷ７の突出部Ｗ７ａを第８のワークＷ８の縁部Ｗ８ａに内周側からＸ方向に接近させる（図１３（ａ）参照）。そして、第７のワークＷ７の突出部Ｗ７ａを、第８のワークＷ８の縁部Ｗ８ａに接触させる（図１３（ｂ）参照）。第７のワークＷ７を、突出部Ｗ７ａと縁部Ｗ８ａとの接点においてＹ軸を中心に回転させる（図１４（ａ）参照）。そして、第７のワークＷ７の中心軸Ｗ７ｃを第８のワークＷ８の中心軸Ｗ８ｃに対して９０°回転させ、第７のワークＷ７の突出部Ｗ７ａと係合部Ｗ７ｂと爪部Ｗ７ｄとを第８のワークＷ８の縁部Ｗ８ａに係合する（図１４（ｂ）参照）。これにより、第７のワークＷ７が第８のワークＷ８に対して着脱可能に装着される。一方、第７のワークＷ７を第８のワークＷ８から取り外す際は、上述と逆の動作を行う。

ここで、図１３乃至図１５を利用して、制御装置３が第７のワークＷ７及び第８のワークＷ８を装着動作させて反力Ｆを検出し、軌道の補正量を演算して、軌道を修正する原理について説明する。なお、本実施形態では、反力Ｆとは、力覚センサ２５により検出された先端リンク６７に対してハンド２１から作用する外力のうち、Ｘ方向の力としている。即ち、反力Ｆは、先端リンク６７の軸方向（Ｚ方向）に直交し、第７のワークＷ７の突出部Ｗ７ａを第８のワークＷ８の縁部Ｗ８ａに係脱する方向であるＸ方向の力としている（図１４（ｂ）等参照）。また、ここでの反力Ｆとしては、力覚センサ２５により検出された先端リンク６７に対してハンド２１から作用する外力のうち、Ｘ方向の力以外のＹ，Ｚ方向の力及び３方向のモーメント（Ｍｘ，Ｍｙ，Ｍｚ）は含まない。また、力覚センサ２５の座標系はハンド２１の座標系と一体化されており、ハンド２１の位置姿勢の変化に伴って力覚センサ２５の座標系も移動・回転する。

図１３（ａ）に示すように、第７のワークＷ７を、第８のワークＷ８に接触することなく適宜距離だけ離れた状態（非接触状態）にして配置する。この時、第７のワークＷ７は、互いの中心軸Ｗ７ｃ，Ｗ８ｃをＺ軸方向に平行にした状態で、第７のワークＷ７の突出部Ｗ７ａを第８のワークＷ８の縁部Ｗ８ａに内周側に位置させる。この時の第７のワークＷ７の位置姿勢を上空点（第１の教示点）とする。この時は、力覚センサ２５により検出される反力Ｆは０となる。

そして、ロボット本体２により、第７のワークＷ７の突出部Ｗ７ａを、第８のワークＷ８の縁部Ｗ８ａに接近させる。これにより、図１３（ｂ）に示すように、第７のワークＷ７の突出部Ｗ７ａが第８のワークＷ８の縁部Ｗ８ａに当接する。この時の第７のワークＷ７の位置姿勢を接触点（第３の教示点）とする。

更に、ロボット本体２により、第７のワークＷ７を、第７のワークＷ７の突出部Ｗ７ａと第８のワークＷ８の縁部Ｗ８ａとの接点においてＹ軸を中心に回転させる。これにより、図１４（ａ）に示すように、突出部Ｗ７ａと縁部Ｗ８ａとを当接させたまま、第７のワークＷ７の姿勢が傾斜する。

そして、ロボット本体２により、第７のワークＷ７の中心軸Ｗ７ｃを第８のワークＷ８の中心軸Ｗ８ｃに対して９０°回転させる。これにより、図１４（ｂ）に示すように、第７のワークＷ７の突出部Ｗ７ａと係合部Ｗ７ｂと爪部Ｗ７ｄとが、第８のワークＷ８の縁部Ｗ８ａに係合する。この時の第７のワークＷ７の位置姿勢を、下降点（第２の教示点）とする。この時、第７のワークＷ７の中心軸Ｗ７ｃと第８のワークＷ８の中心軸Ｗ８ｃとの位置や角度がずれていると、第７のワークＷ７と第８のワークＷ８とが接触し、互いに余分な反力Ｆが発生する。また、上空点と下降点とを結んだ直線が所定の軌道となる。即ち、本実施の形態では、下降点における軌道と接触点における軌道とは、相対的に傾斜した直線状の軌道である。

この反力Ｆは、図１３（ｂ）に示す第７のワークＷ７の突出部Ｗ７ａが第８のワークＷ８の縁部Ｗ８ａに接触した状態（図１５（ａ）の接触点）から、図１４（ｂ）に示す最後まで嵌合した状態（図１５（ａ）の下降点）まで、折線状に昇降する。即ち、本実施の形態では、反力Ｆは、第７のワークＷ７及び第８のワークＷ８の接触動作により第７のワークＷ７が変位する変化量である。尚、本実施形態では、図１３（ｂ）の状態及び図１４（ｂ）の状態、即ち、接触点と下降点との各状態が、それぞれ第７のワークＷ７と第８のワークＷ８との接触状態である。

ここで、例えば、図１５（ａ）に示すように、上空点から接触点までのＸ方向のストロークをＡ２、接触点から下降点までのＸ方向のストロークをＡ１とする。力覚センサ２５により検出されたＸ方向の反力Ｆが、接触点から下降点まで折線的に昇降して、接触点においてｆ１、下降点においてＦ１になったとする。また、予め計測した相対位置姿勢の変位がない場合の参照波形を理論値により取得し、その時の力覚センサ２５により検出されたＸ方向の反力Ｆが、接触点から下降点まで折線的に昇降して、接触点においてｆ０、下降点においてＦ０になったとする。この場合、相対位置姿勢の変位がある場合とない場合との接触点から下降点までの反力Ｆの差分の傾きΔＦは、ΔＦ＝（（Ｆ１−Ｆ０）−（ｆ１−ｆ０））／Ａ１になる。

次に、図１５（ｂ）に示すように、接触点から下降点までの反力Ｆの差分の傾きΔＦを上空点にまで延長し、第８のワークＷ８が上空点まで延長されていたとした場合に第７のワークＷ７が受けるであろう反力Ｆ２を想定して演算する。この反力Ｆ２は、Ｆ２＝ΔＦ×Ａ２＋（ｆ１−ｆ０）になる。尚、同図に示すように、反力Ｆ２は、下降点（反力Ｆ１）及び接触点（反力ｆ１）を結んだ直線の上空点における交点と、下降点（反力Ｆ０）及び接触点（反力ｆ０）を結んだ直線の上空点における交点と、の差分として、作図によっても求めることができる。

更に、図１５（ｃ）に示すように、上空点及び下降点について、反力ＦをＸ方向の長さである補正量Ｘに変換する。このため、ロボット本体２の弾性率をＫとすると、上空点での補正量Ｘ２は、Ｘ２＝Ｆ２／Ｋになり、下降点での補正量Ｘ１は、Ｘ１＝Ｆ１／Ｋになる。これら上空点及び下降点に対するＸ方向の補正量Ｘ１，Ｘ２が、軌道の補正量となる。そして、現在の上空点の位置姿勢のＸ方向に補正量Ｘ２を加算して補正し、現在の下降点の位置姿勢のＸ方向に補正量Ｘ１を加算して補正し、補正後の上空点と下降点とを結んだ直線を補正後の軌道とすることで、軌道の補正を実行することができる。

上述したロボット装置１の制御方法の手順は、第１の実施形態（図４及び図５）と同様であるので、詳細な説明を省略する。また、第７のワークＷ７に対する位置の補正と姿勢の補正との順序は、第１の実施形態と同様に適宜設定することができる。

上述したように本実施の形態のロボット装置１によっても、制御装置３が、第７のワークＷ７を軌道上で移動させ第８のワークＷ８に対する装着動作を実行させる。その際に、制御装置３は、第７のワークＷ７及び第８のワークＷ８の接触動作により第７のワークＷ７が変位して発生する反力Ｆを検出する。そして、制御装置３は、反力Ｆに基づいて軌道のＸ方向の補正量Ｘ１，Ｘ２を演算し、それらに基づいて軌道を補正する。このため、専用治工具を用いて軌道を補正することなく、保持した第７のワークＷ７を第８のワークＷ８に対して高精度に組み付けることができるようになる。

また、本実施の形態においても、第１の実施形態と同様に、力覚センサ２５を用いずに、第７のワークＷ７が第８のワークＷ８との接触により位置姿勢を変位した変位量をカメラ５により検出し、その変位量に基づいて各補正量を演算するようにしてもよい。

また、以上述べた第１〜第４の実施形態では、接触動作は装着動作であり、非接触動作は離脱動作である場合について説明したが、これには限られない。例えば、接触動作としては、第１のワークを工具とし、第２のワークに対して加工を施す場合や、第１のワークをラベルとし、第２のワークに貼付する場合等、装着以外の接触動作を含めることができる。

尚、以上述べた第１〜第４の実施形態の各処理動作は具体的にはＣＰＵ３０により実行されるものである。従って、上述した機能を実現するソフトウェアのロボット制御プログラムを記録した記録媒体をＣＰＵ３０に供給し、ＣＰＵ３０が記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した各実施の形態の機能を実現することになり、ロボット制御プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。

また、第１〜第４の実施形態では、コンピュータ読み取り可能な記録媒体がＲＯＭ３１であり、ＲＯＭ３１にプログラムが格納される場合について説明したが、これに限定するものではない。プログラムは、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、ＨＤＤ、外部記憶装置、記録ディスク等を用いてもよい。

１…ロボット装置、２…ロボット本体、３…制御装置（制御部）、５…カメラ（検出部）、２０…多関節アーム、２１…ハンド（エンドエフェクタ）、２５…力覚センサ（検出部）、３０…ＣＰＵ、７１〜７６…複数の関節、Ｗ１…第１のワーク、Ｗ２…第２のワーク、Ｗ３…第３のワーク（第１のワーク）、Ｗ４…第４のワーク（第２のワーク）、Ｗ５…第５のワーク（第１のワーク）、Ｗ６…第６のワーク（第２のワーク）、Ｗ７…第７のワーク（第１のワーク）、Ｗ８…第８のワーク（第２のワーク）

Claims (14)

1. 複数の関節を有する多関節アームと前記多関節アームに支持されるエンドエフェクタとを有するロボット本体と、前記エンドエフェクタに連結され、第１のワーク及び第２のワークの間に相対的に作用する力の変化量と前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量との少なくとも一方を検出する検出部と、前記ロボット本体を制御する制御部と、を備えるロボット装置の制御方法において、
   前記制御部が、前記エンドエフェクタにより保持した前記第１のワークを所定の軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記第１のワーク及び前記第２のワークが接触する接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記第１のワーク及び前記第２のワークが接触しない非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量と、の少なくとも一方を前記検出部の出力に基づいて検出する第１の検出工程と、
   前記制御部が、前記第１の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量を演算する第１の演算工程と、
   前記制御部が、前記第１の演算工程で演算した前記補正量に基づき、前記軌道を補正する第１の補正工程と、を備える、
   ことを特徴とするロボット装置の制御方法。
2. 前記軌道は、前記第１のワーク及び前記第２のワークが前記接触状態にある場合と、前記非接触状態にある場合とで、一直線状である、
   ことを特徴とする請求項１記載のロボット装置の制御方法。
3. 前記軌道は、前記非接触状態にある前記第１のワークの位置姿勢である第１の教示点と、前記接触状態にある前記第１のワークの位置姿勢である第２の教示点と、に基づいて演算され、
   前記第１の演算工程では、前記制御部が、前記第１の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量として、前記第１の教示点及び前記第２の教示点の各位置姿勢の補正量を演算し、
   前記第１の補正工程では、前記制御部が、前記第１の演算工程で演算した前記第１の教示点及び前記第２の教示点の各位置姿勢の補正量に基づき、前記第１の教示点及び前記第２の教示点の各位置姿勢を補正することにより、前記軌道を補正する、
   ことを特徴とする請求項１又は２に記載のロボット装置の制御方法。
4. 前記軌道は、前記非接触状態にある前記第１のワークの位置姿勢である第１の教示点と、前記接触状態にある前記第１のワークの位置姿勢である第２の教示点と、前記接触状態にある前記第１のワークの位置姿勢であり、前記軌道上で前記第１の教示点及び前記第２の教示点の間に位置する第３の教示点と、に基づいて演算され、
   前記第１の演算工程では、前記制御部が、前記第１の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量として、前記第１の教示点及び前記第２の教示点の各位置姿勢の補正量を演算し、
   前記第１の補正工程では、前記制御部が、前記第１の演算工程で演算した前記第１の教示点及び前記第２の教示点の各位置姿勢の補正量に基づき、前記第１の教示点及び前記第２の教示点の各位置姿勢を補正することにより、前記軌道を補正する、
   ことを特徴とする請求項１記載のロボット装置の制御方法。
5. 前記第２の教示点における軌道と、前記第３の教示点における軌道とは、一直線状の軌道である、
   ことを特徴とする請求項４記載のロボット装置の制御方法。
6. 前記第２の教示点における軌道と、前記第３の教示点における軌道とは、同方向の直線状の軌道であり、かつ、各軌道の間では前記第１のワーク及び前記第２のワークは非接触である、
   ことを特徴とする請求項４記載のロボット装置の制御方法。
7. 前記第２の教示点における軌道と、前記第３の教示点における軌道とは、相対的に傾斜した直線状の軌道である、
   ことを特徴とする請求項４記載のロボット装置の制御方法。
8. 前記第３の教示点における軌道は直線状の軌道であり、前記第２の教示点における軌道は前記直線と同方向の軸を中心とする回転方向の軌道である、
   ことを特徴とする請求項４記載のロボット装置の制御方法。
9. 前記第１の検出工程では、前記変化量は、前記第１のワーク及び前記第２のワークの間に前記直線の方向に相対的に作用する力の変化量であるか、または前記変位量は、前記第１のワーク及び前記第２のワークの前記直線の方向の相対位置の変位量である、
   ことを特徴とする請求項５乃至８のいずれか１項に記載のロボット装置の制御方法。
10. 前記第１の検出工程では、前記制御部が、前記エンドエフェクタにより保持した前記第１のワークを前記所定の軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対位置の変位量と、の少なくとも一方を前記検出部により検出し、
    前記第１の演算工程では、前記制御部が、前記第１の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量として、前記第１の教示点及び前記第２の教示点の各位置の補正量を演算し、
    前記第１の補正工程では、前記制御部が、前記第１の演算工程で演算した前記補正量に基づき、前記第１の教示点及び前記第２の教示点の各位置を補正することにより、前記軌道を補正し、
    前記制御部が、前記エンドエフェクタにより保持した前記第１のワークを前記第１の補正工程で補正した前記軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対姿勢の変位量と、の少なくとも一方を前記検出部により検出する第２の検出工程と、
    前記制御部が、前記第２の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量として、前記第１の教示点及び前記第２の教示点の各姿勢の補正量を演算する第２の演算工程と、
    前記制御部が、前記第２の演算工程で演算した前記補正量に基づき、前記第１の教示点及び前記第２の教示点の各姿勢を補正することにより、前記軌道を補正する第２の補正工程と、を備える、
    ことを特徴とする請求項３乃至９のいずれか１項に記載のロボット装置の制御方法。
11. 前記第１の検出工程では、前記制御部が、前記エンドエフェクタにより保持した前記第１のワークを前記所定の軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対姿勢の変位量と、の少なくとも一方を前記検出部により検出し、
    前記第１の演算工程では、前記制御部が、前記第１の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量として、前記第１の教示点及び前記第２の教示点の各姿勢の補正量を演算し、
    前記第１の補正工程では、前記制御部が、前記第１の演算工程で演算した前記補正量に基づき、前記第１の教示点及び前記第２の教示点の各姿勢を補正することにより、前記軌道を補正し、
    前記制御部が、前記エンドエフェクタにより保持した前記第１のワークを前記第１の補正工程で補正した前記軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対位置の変位量と、の少なくとも一方を前記検出部により検出する第２の検出工程と、
    前記制御部が、前記第２の検出工程で検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量として、前記第１の教示点及び前記第２の教示点の各位置の補正量を演算する第２の演算工程と、
    前記制御部が、前記第２の演算工程で演算した前記補正量に基づき、前記第１の教示点及び前記第２の教示点の各位置を補正することにより、前記軌道を補正する第２の補正工程と、を備える、
    ことを特徴とする請求項３乃至９のいずれか１項に記載のロボット装置の制御方法。
12. 請求項１乃至１１のいずれか１項に記載のロボット装置の制御方法の各工程をコンピュータに実行させるためのロボット制御プログラム。
13. 請求項１２に記載のロボット制御プログラムが記録されたコンピュータが読み取り可能な記録媒体。
14. 複数の関節を有する多関節アームと前記多関節アームに支持されるエンドエフェクタとを有するロボット本体と、前記エンドエフェクタに連結され、第１のワーク及び第２のワークの間に相対的に作用する力の変化量と前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量との少なくとも一方を検出する検出部と、前記ロボット本体を制御する制御部と、を備えるロボット装置において、
    前記制御部は、前記エンドエフェクタにより保持した前記第１のワークを所定の軌道上で移動させ前記第２のワークに対する接触動作又は非接触動作を実行する際に、前記第１のワーク及び前記第２のワークが接触する接触状態において前記第１のワーク及び前記第２のワークの間に相対的に作用する力の変化量と、前記第１のワーク及び前記第２のワークが接触しない非接触状態と前記接触状態とを切り換えた時の前記第１のワーク及び前記第２のワークの相対位置姿勢の変位量と、の少なくとも一方を前記検出部により検出し、検出した前記力の変化量あるいは前記相対位置姿勢の変位量に基づき、前記軌道の補正量を演算し、演算した前記補正量に基づき、前記軌道を補正する、
    ことを特徴とするロボット装置。

Images