JP4598849B2

JP4598849B2 - 嵌合の詰まり状態を修正する嵌合装置

Info

Publication number: JP4598849B2
Application number: JP2008225854A
Authority: JP
Inventors: 貴之佐藤; 宣章山岡
Original assignee: FANUC Corp
Current assignee: FANUC Corp
Priority date: 2008-09-03
Filing date: 2008-09-03
Publication date: 2010-12-15
Anticipated expiration: 2028-09-03
Also published as: DE102009038275A1; US20100057256A1; JP2010058218A; DE102009038275B4; US7949431B2

Description

本発明は、ロボットにより把持される嵌合ワークを、力制御により被嵌合ワークに嵌合する嵌合装置に関する。

仮想的なコンプライアンスを構成することによりＲＣＣ（remotecenter compliance）を模した制御則を用いて嵌合作業を行うことが従来より行われている。特許文献１には、ロボットのハンドにより把持された棒状の嵌合ワークを被嵌合ワークの嵌合穴に嵌合させる一つの手法が開示されている。

特許文献１に開示される手法によれば、嵌合ワークを被嵌合ワークに押付け、その際に作用する力およびモーメントを制御周期毎に検出し、それらが目標量になるように制御している。

さらに、特許文献２には、ディスクの穴をスピンドルに嵌合させる技術が開示されている。特許文献２によれば、ディスクの穴とスピンドルとの間に大きなずれが嵌合方向に対して垂直な方向に存在する場合には、嵌合方向の力に加えて、嵌合方向に対して垂直な方向の力を発生させて、ディスクを斜方向に移動させる。これにより、ずれを解消しつつディスクがスピンドルに嵌合する。
特公平４−４３７４４号公報特開平４−２５３２５号公報

ここで、特許文献１において嵌合ワークが被嵌合ワークに二点で嵌合する場合を考える。この場合には、二点に作用する力は互いに反対方向の成分を含んでいるので、これら二点に作用する力の合力は、それぞれの力よりも小さくなる。従って、合力から得られるモーメントも小さくなる。このモーメントがロボットの振動などによる外乱よりも小さい場合には、モーメントに基づいて嵌合ワークの姿勢を変更するのが困難な事態が生じうる。従って、嵌合ワークの姿勢を修正するのに必要とされる時間が大幅に延びることになる。さらに、二点に作用する力が同じ大きさで且つ互いに反対方向である場合には、モーメントがゼロになり、この場合には、時間をかけても嵌合ワークの姿勢を適切に変更するのは不可能である。

また、特許文献２の技術を棒状の嵌合ワークを被嵌合ワークの嵌合穴に嵌合させるのに適用することが考えられる。しかしながら、特許文献２には、嵌合方向の力とは別の力を、嵌合方向に対して垂直などの方向に発生させるかまでは開示されていない。従って、適切でない方向の力を嵌合ワークに作用させ、かえって嵌合ワークの位置／姿勢誤差が大きくなることにもなりかねない。

本発明はこのような事情に鑑みてなされたものであり、嵌合ワークと被嵌合ワークとの間に詰まりが生じた場合に、嵌合ワークの姿勢を短時間で適切に修正することのできる、嵌合装置を提供することを目的とする。

前述した目的を達成するために１番目の発明によれば、ロボットにより把持される円柱型の嵌合ワークを、該嵌合ワークが嵌合可能な被嵌合ワークの嵌合穴に力制御によって嵌合する嵌合装置において、前記嵌合ワークの底面の中心における制御点に働く力とモーメントとを検出する力検出手段と、嵌合時に、前記嵌合ワークと前記被嵌合ワークとの間に詰まりが生じているか否かを判定する判定手段と、前記判定手段により詰まりが生じていると判定された場合には、前記嵌合ワークが前記被嵌合ワークの嵌合穴に進入した距離よりも小さい値だけ、前記制御点の位置を前記嵌合ワークの中心線に沿って上方に変更して、嵌合方向に対して垂直な方向において前記嵌合ワークの外周面を前記被嵌合ワークの嵌合穴の内周面に押付け、それにより、変更後の前記制御点回りに作用するモーメントに基づく力制御によって前記嵌合ワークの姿勢を修正する変更手段とを具備した、嵌合装置が提供される。

すなわち１番目の発明においては、制御点を変更すると共に、嵌合ワークを嵌合方向に対して垂直な方向に押付けるようにしている。これにより、嵌合ワークは新たな制御点回りに回動するので、嵌合ワークの姿勢を嵌合に適した位置に修正することができる。また、制御点を変更しているので、嵌合前の初期の位置／姿勢誤差が大きい場合であっても、これを修正することが可能である。なお、制御点が嵌合ワークの底面の中心に位置決めされる場合には、制御点の位置を嵌合ワークの進入距離の半分だけ嵌合ワークの中心線方向に変更するのが好ましい。

２番目の発明によれば、１番目の発明において、前記判定手段は、前記力検出手段により検出された力とモーメントとに基づいて、詰まりが生じているか否かを判定する。
すなわち２番目の発明においては、検出されたモーメントが所定の値よりも小さい場合には、嵌合ワークが回転できないために詰まりが生じたと判定することができる。すなわち、比較的簡易な手法により、詰まりを判定できる。

３番目の発明によれば、１番目の発明において、さらに、前記嵌合ワークの位置を所定周期毎に検出する位置検出手段を具備し、前記判定手段は、前記位置検出手段により検出された前記嵌合ワークの位置の嵌合方向における時間変化に基づいて、詰まりが生じているか否かを判定する。
すなわち３番目の発明においては、所定周期毎に検出された位置の時間変化が所定の値よりも小さい場合には、嵌合ワークが移動していないので詰まりが生じたと判定することができる。すなわち、比較的簡易な手法により、詰まりを判定できる。

４番目の発明によれば、１番目から３番目のいずれかの発明において、前記判定手段により詰まりが生じていると判定された場合には、前記変更手段は、前記嵌合ワークを嵌合方向に対して垂直な少なくとも一つの方向に押付けるようにした。
すなわち４番目の発明においては、嵌合ワークを押付けた方向に対して垂直な方向回りにおける嵌合ワークの位置／姿勢誤差を修正できる。なお、嵌合ワークを押付けた方向の反対方向にも嵌合ワークを押付けて位置／姿勢誤差をさらに修正してもよい。さらに、嵌合ワークを押付けた方向および嵌合方向の両方に対して垂直な方向に、嵌合ワークをさらに押付けるのが好ましい。

５番目の発明によれば、１番目から３番目のいずれかの発明において、前記判定手段により詰まりが生じていると判定された場合には、前記変更手段は、前記嵌合ワークの嵌合方向に対して垂直な方向への押付作用を前記嵌合ワークの周方向に順次行うようにした。
すなわち５番目の発明においては、嵌合ワークの押付動作を回転させているので、あらゆる方向における位置／姿勢誤差を容易に修正することができる。

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づく嵌合装置の略図である。図１に示される嵌合装置１０は、ロボット１とロボット制御ユニット１１とを含んでいる。ロボット１は、六軸構成の垂直多間接型ロボットである。ロボット１のロボットアーム２の先端には、ハンド４が取付けられている。図示されるように、ロボットアーム２とハンド４との間には、力センサ３が配置されている。力センサ３は嵌合ワーク６０の制御点Ｐ（後述する）に作用する力および／またはモーメントを検出する。

ロボット１および力センサ３はロボット制御ユニット１１に接続されている。ロボット制御ユニット１１はデジタルコンピュータであり、嵌合装置１０のロボット１全体の動作を制御する。また、ロボット制御ユニット１１は、嵌合時に、後述する嵌合ワーク６０と被嵌合ワーク５０との間に詰まりが生じているか否かを判定する判定手段１２を含んでいる。

さらに、ロボット制御ユニット１１は変更手段１３を含んでいる。変更手段１３は、判定手段１２により詰まりが生じていると判定された場合には、嵌合ワーク６０が被嵌合ワーク５０に進入した距離に応じて嵌合ワーク６０の制御点の位置を変更して、嵌合方向に対して垂直な方向において嵌合ワーク６０を被嵌合ワーク５０に押付けるようにする。

なお、図面には示さないものの、ロボット制御ユニット１１は、所定の手法により作成された位置指令に基づいてロボット１に対して位置指令を実行する位置制御部を有している。さらに、ロボット制御ユニット１１は、力センサ３により検出された力検出値と所定の手法により作成された位置指令または力指令とに基づいて、ロボット１に対して力指令を実行する力制御部を有している。

図１においては、円柱型の嵌合ワーク６０がロボット１のハンド４により把持されている。そして、嵌合ワーク６０に嵌合可能な嵌合穴５１を備えた被嵌合ワーク５０が嵌合ワーク６０に対面して配置されている。図１においては、被嵌合ワーク５０は保持具３２によって支持台３０上の所定の位置に固定されている。なお、図示されるように、被嵌合ワーク５０の嵌合穴５１の中心方向（嵌合方向）をＺ軸とし、このＺ軸に垂直な面内にＸ軸およびＹ軸を配置する。

図２は嵌合ワークおよび被嵌合ワークの拡大断面図である。図示されるように、被嵌合ワーク５０に形成された嵌合穴５１の端部には、面取部５２が形成されている。図２に示される実施形態においては、面取部５２の角度αは４５°である。ただし、角度αは４５°に限定されるものではなく、角度αの値が異なっていてもよい。また、嵌合ワーク６０の底面における中心を制御点Ｐとして設定する。この制御点Ｐにおいて嵌合ワーク６０に作用する力およびモーメントが算出され、またこの制御点Ｐは、本発明に基づく制御を行う際に嵌合ワーク６０の位置／姿勢を修正するときの基準点となる。

以下、本発明に基づく嵌合装置１０の嵌合作業を説明する。はじめに、位置制御部による位置制御下においてロボット１のハンド４により嵌合ワーク６０を把持する。次いで、把持された嵌合ワーク６０を被嵌合ワーク５０の嵌合穴５１手前まで移動させる。その後、位置制御部による位置制御から力制御部による力制御に切り換える。以下の説明は、力制御下で行われるものとする。

図３は、嵌合装置の動作を示すフローチャートである。次いで、図３のステップ１００において、嵌合ワーク６０を嵌合方向、つまりＺ方向に押付ける。そして、ステップ１０１において、以下の式（１）から式（５）に示される計算を所定の制御周期毎に行い、嵌合ワーク６０を制御する。

これら式（１）から式（５）において、ν^D _X、ν^D _Y、ν^D _Z、ω^D _X、ω^D _Y、ω^D _ZはＸＹＺ各方向における速度指令値およびそのまわりの角速度指令値である。Ｆ_ｘ、Ｆ_ｙ、Ｆ_ｚ、Ｍ_ｘ、Ｍ_ｙは各方向における力およびそのまわりのモーメントの検出値である。さらに、Ｇ_ｘ、Ｇ_ｙ、Ｇ_ｚ、Ｇ_ｗ、Ｇ_ｐは各方向における力制御ゲインを示し、Ｆ_ｄは目標力、ν_ｄは目標速度を表す。

図４（ａ）および図４（ｂ）は嵌合方向に押付ける際の嵌合ワークおよび被嵌合ワークの拡大断面図である。嵌合ワーク６０を嵌合方向に押付けるときに、嵌合ワーク６０の一部分が被嵌合ワーク５０に接触すると、モーメントＭが発生する（図４（ａ））。このモーメントＭにより、嵌合ワーク６０の位置／姿勢誤差が矢印ＭＡで示される方向に修正される。この修正は、従来技術より行われている手法であり、嵌合ワーク６０を嵌合方向に押付けることにより実施される。

そして、ステップ１０２において、嵌合ワーク６０が嵌合穴５１の所定の深さまで進入したか否かを判定する。このことは、ロボット１のアームを駆動するサーボモータに取付けられたエンコーダ（図示しない）に基づいて行われる。嵌合ワーク６０が所定の深さまで進入している場合には、嵌合作業が完了したと判断して処理を終了する。

一方、所定深さまで進入していない場合には、ステップ１０３に進み、ロボット制御ユニット１１の判定手段１２が、嵌合ワーク６０が被嵌合ワーク５０に詰まったか否かを判定する。判定手段１２は、以下の式（６）から式（１０）に基づいて嵌合ワーク６０の詰まりを判定する。

ここで、Ｔ_Ｆ、Ｔ_Ｍは、それぞれ力およびモーメントが目標値に近づいたこと判断するための閾値である。また、ロボットの振動等に起因する外乱により詰まりの判定が困難である場合には、検出された力およびモーメントのデータにフィルタをかけてもよい。

さらに、以下の式（１１）から式（１５）に基づいて、嵌合ワーク６０の詰まりを判定してもよい。

ここで、ν_Ｘ、ν_Ｙ、ν_Ｚ、ω_Ｘ、ω_Ｙはロボットアームに装着したアクチュエータ位置から算出した各方向での実際の速度および角速度である。Ｔ_ν、Ｔ_ωはそれぞれ詰まりを判定するための速度および角速度の閾値である。外乱により正確な判定が困難な場合には、実際の速度および角速度にフィルタをかけてもよい。さらに、エンコーダにより嵌合ワーク６０の位置を制御周期毎に検出し、嵌合ワーク６０の位置の嵌合方向における時間変化が所定値よりも小さい場合に、詰まりが生じたと判定するようにしてもよい。

ステップ１０３において、詰まりが発生していると判定された場合にはステップ１０４に進み、嵌合ワーク６０の嵌合方向への押付を停止する。なお、詰まりが発生していないと判定された場合には、ステップ１００に戻り、処理を繰返す。

次いで、図３に示されるステップ１０５において、ロボット制御ユニット１１の変更手段１３が嵌合ワーク６０の制御点Ｐの位置を変更する。図５は、嵌合ワークおよび被嵌合ワークの他の拡大断面図である。図５においては、嵌合ワーク６０の中心線Ｋ１が嵌合穴５１の嵌合方向Ｚと角度θをなした状態で、嵌合ワーク６０が被嵌合ワーク５０に詰まっている。また、図５においては、嵌合ワーク６０は距離Ｄ１だけ嵌合穴５１内に進入している。

変更手段１３によって、制御点Ｐの位置は嵌合ワーク６０の底面の中心から中心線Ｋ１に沿って上方に移動される。制御点Ｐの移動距離は、ゼロよりも大きくて進入距離Ｄ１よりも小さい。好ましくは、制御点Ｐの移動距離は進入距離Ｄ１の半分である。この場合には、嵌合方向に対して垂直ないずれの方向に嵌合ワーク６０を被嵌合ワーク５０に対して押付けたとしても、ほぼ同じ大きさのモーメントが嵌合ワーク６０に作用して、嵌合ワーク６０の位置／姿勢誤差を同程度に修正できる。

図５から分かるように、厳密にいえば、中心線Ｋ１における制御点Ｐの移動距離は０．５×Ｄ１×ｃｏｓθであるが、実際にはθ≒０であるので、制御点Ｐの移動距離として、０．５×Ｄ１を採用してよい。

次いで、図３のステップ１０６およびステップ１０７において、嵌合ワーク６０を嵌合方向に対して垂直に被嵌合ワーク５０に押付ける。図６（ａ）は、嵌合ワーク６０の端面図である。図６（ａ）において矢印Ａ１で示されるように、はじめに、嵌合ワーク６０を被嵌合ワーク５０に対して＋Ｘ方向に押付ける。

このときには、以下の式（１６）〜式（２０）に基づいて嵌合ワーク６０を制御する。

図７（ａ）は姿勢修正時における嵌合ワークおよび被嵌合ワークの拡大断面図である。本発明においては、制御点Ｐの位置を制御点Ｐ’まで上方に移動させているので、嵌合ワーク６０に対してモーメントが作用しやすい状態になっている。従って、＋Ｘ方向への押付動作により、図示される力Ｆが嵌合ワーク６０に作用して、制御点Ｐ’回りのモーメントＭが嵌合ワーク６０に作用する。これにより、嵌合ワーク６０は実線で示される状態から破線で示される状態まで回動し、Ｙ方向回りの位置／姿勢誤差が修正される。

すなわち本発明においては、制御点Ｐを制御点Ｐ’まで変更しているので、押付動作時にモーメントが確実に作用するようになる。従来技術で説明したように嵌合ワーク６０の二点に作用する力が同じ大きさで且つ互いに反対方向である場合であっても、本発明においては、モーメントを作用させて嵌合ワーク６０の位置／姿勢誤差を修正できる。

そして、図３のステップ１０８において、以下の式（２１）〜式（２５）の全てが満たされた場合には、＋Ｘ方向への押付動作を終了する。

これらのうち、式（２１）は、押付方向（この場合には＋Ｘ方向）の力が目標力に近くなったかどうかを表している。式（２２）〜式（２５）は、嵌合ワーク６０の位置／姿勢誤差が修正され力／モーメントが０に近くなったかを表す。

ここで、図７（ａ）等に示されるように嵌合ワーク６０の姿勢がずれている場合には、モーメントＭが作用するので、式（２４）または式（２５）は満たされない。そして図７（ａ）と同様な図８（ａ）に示されるように嵌合ワーク６０の周面が被嵌合ワーク５０の嵌合穴５１に線接触するようになると、嵌合ワーク６０の位置／姿勢誤差は解消する。この場合には、モーメントＭが嵌合ワーク６０に作用しないようになり、式（２４）および式（２５）を満たすようになる。

これら式（２１）〜式（２５）が満たされるようになると、嵌合ワーク６０のＹ方向回りの位置／姿勢誤差の修正が完了したと判断する。次いで、図６（ａ）において矢印Ａ２で示される＋Ｙ方向に嵌合ワーク６０を押付けて、嵌合ワーク６０のＸ方向回りの位置／姿勢誤差を修正する。この場合にも、前述したのと同様な式に基づいて嵌合ワーク６０が制御されると共に、押付動作の終了を判断するのが分かるであろう。

その後、図６（ａ）において矢印Ａ３で示される方向（−Ｘ方向）へ嵌合ワーク６０を同様に押付け（図７（ａ）および図８（ａ）を参照されたい）、さらに、図６（ａ）において矢印Ａ４で示される方向（−Ｙ方向）へ嵌合ワーク６０を同様に押付ける。これにより、嵌合ワーク６０のＸ方向およびＹ方向回りにおける位置／姿勢誤差が修正される。なお、誤差がＸ方向回りまたはＹ方向回りのいずれかの姿勢においてのみ存在する場合であっても、押付動作によって新たな誤差が生じることはない。

なお、矢印Ａ１〜矢印Ａ４への押付動作の順番または或る押付方向における押付回数は適宜変更してもよい。さらに、矢印Ａ１および矢印Ａ３のうちのいずれか一方、および矢印Ａ２および矢印Ａ４のうちのいずれか一方のみに方向へ嵌合ワーク６０を押付ける場合であっても、嵌合ワーク６０のＸ方向およびＹ方向回りにおける位置／姿勢誤差を修正することが可能である。

全ての方向において嵌合ワーク６０位置／姿勢誤差の修正が完了すると、ステップ１００に戻る。そして、嵌合ワーク６０を嵌合方向にさらに押付け、嵌合ワーク６０が所定の深さに挿入されるまで処理を繰返す。つまり、嵌合ワーク６０の詰まりが再び生じる度毎に、嵌合ワーク６０の被嵌合ワーク５０に対する押付作用を実施する。これにより、嵌合ワーク６０を被嵌合ワーク５０に適切に嵌合させられる。

このように、本発明においては、制御点Ｐを制御点Ｐ’まで変更しているので、押付動作時にモーメントが嵌合ワーク６０に確実に作用して、嵌合ワーク６０の位置／姿勢誤差を修正することが可能である。さらに、上記した理由により、位置／姿勢誤差の修正に要する時間は短くて済む。さらに、嵌合前の初期許容位置／姿勢誤差が比較的大きい場合であっても、嵌合ワーク６０を被嵌合ワーク５０に嵌合させられるのが分かるであろう。

図６（ｂ）等に示される別の実施形態においては、押付作用を嵌合ワーク６０の周方向に順次行うようにしてもよい。この場合には、以下の式（２６）から式（３１）に基づいて嵌合ワーク６０を制御し、それにより、嵌合ワーク６０の押付動作を一回転させる。

なお、式（３１）の文字ｔは、押付方向の変更を開始してからの時間を表す。つまり、これらの式においては、嵌合ワーク６０の押付作用は２０秒かけて嵌合ワーク６０の周面に沿って順次行われる。この時間が経過したら嵌合方向に対して垂直な方向への押付動作を終了する。

なお、式（２６）〜式（３０）は、＋Ｘ方向から開始して周方向に順次押付ける場合を表しているが、他の方向から開始することも可能である。また、嵌合ワーク６０の周面に沿った順次の押付作用の時間、押付作用の移動方向などを適宜変更してよいのは明らかであろう。

本発明に基づく嵌合装置の略図である。嵌合ワークおよび被嵌合ワークの拡大断面図である。本発明に基づく嵌合装置の動作を示すフローチャートである。（ａ）嵌合方向に押付ける際の嵌合ワークおよび被嵌合ワークの拡大断面図である。（ｂ）嵌合方向に押付ける際の嵌合ワークおよび被嵌合ワークの他の拡大断面図である。嵌合ワークおよび被嵌合ワークの他の拡大断面図である。（ａ）嵌合ワークの端面図である。（ｂ）嵌合ワークの他の端面図である。（ａ）姿勢修正時における嵌合ワークおよび被嵌合ワークの拡大断面図である。（ｂ）姿勢修正時における嵌合ワークおよび被嵌合ワークの他の拡大断面図である。（ａ）姿勢修正時における嵌合ワークおよび被嵌合ワークの拡大断面図である。（ｂ）姿勢修正時における嵌合ワークおよび被嵌合ワークの他の拡大断面図である。

符号の説明

１ロボット
２ロボットアーム
３力センサ（力検出手段）
４ハンド
１０嵌合装置
１１ロボット制御ユニット
１２判定手段
１３変更手段
３０支持台
３２保持具
５０被嵌合ワーク
５１嵌合穴
５２面取部
６０嵌合ワーク
Ｐ、Ｐ’ 制御点

Claims

ロボットにより把持される円柱型の嵌合ワークを、該嵌合ワークが嵌合可能な被嵌合ワークの嵌合穴に力制御によって嵌合する嵌合装置において、
前記嵌合ワークの底面の中心における制御点に働く力とモーメントとを検出する力検出手段と、
嵌合時に、前記嵌合ワークと前記被嵌合ワークとの間に詰まりが生じているか否かを判定する判定手段と、
前記判定手段により詰まりが生じていると判定された場合には、前記嵌合ワークが前記被嵌合ワークの嵌合穴に進入した距離よりも小さい値だけ、前記制御点の位置を前記嵌合ワークの中心線に沿って上方に変更して、嵌合方向に対して垂直な方向において前記嵌合ワークの外周面を前記被嵌合ワークの嵌合穴の内周面に押付け、それにより、変更後の前記制御点回りに作用するモーメントに基づく力制御によって前記嵌合ワークの姿勢を修正する変更手段とを具備した、嵌合装置。
前記判定手段は、前記力検出手段により検出された力とモーメントとに基づいて、詰まりが生じているか否かを判定する、請求項１に記載の嵌合装置。
さらに、前記嵌合ワークの位置を所定周期毎に検出する位置検出手段を具備し、
前記判定手段は、前記位置検出手段により検出された前記嵌合ワークの位置の嵌合方向における時間変化に基づいて、詰まりが生じているか否かを判定する、請求項１に記載の嵌合装置。
前記判定手段により詰まりが生じていると判定された場合には、前記変更手段は、前記嵌合ワークを嵌合方向に対して垂直な少なくとも一つの方向に押付けるようにした、請求項１から３のいずれか一項に記載の嵌合装置。
前記判定手段により詰まりが生じていると判定された場合には、前記変更手段は、前記嵌合ワークの嵌合方向に対して垂直な方向への押付作用を前記嵌合ワークの周方向に順次行うようにした請求項１から３のいずれか一項に記載の嵌合装置。