力検出装置
【課題】力センサを用いることなく、アクチュエータに加わる接触力を推定する。
【解決手段】入力電圧に比例した電流を発生する電流発生部５と、電流発生部により発生された電流に応じた駆動力を発生し可動部１ａを移送するアクチュエータ１と、可動部の位置を検出する位置センサ２の出力と目標位置を比較してその誤差を電流発生部を通してアクチュエータにフィードバックすることにより位置決め制御を行う装置であって、電流発生部に入力する電圧及び位置センサにより検出された位置に基づいて、可動部が対象物に非接触、かつ、停止している状態で可動部に加わる重力加速度による力を推定した後に、アクチュエータを停止、もしくは一定速度で移送している状態で推定した可動部に加わる外力から重力加速度による力を差し引いて、可動部に加わる接触力を推定する外力推定部６とを備える。
【解決手段】入力電圧に比例した電流を発生する電流発生部５と、電流発生部により発生された電流に応じた駆動力を発生し可動部１ａを移送するアクチュエータ１と、可動部の位置を検出する位置センサ２の出力と目標位置を比較してその誤差を電流発生部を通してアクチュエータにフィードバックすることにより位置決め制御を行う装置であって、電流発生部に入力する電圧及び位置センサにより検出された位置に基づいて、可動部が対象物に非接触、かつ、停止している状態で可動部に加わる重力加速度による力を推定した後に、アクチュエータを停止、もしくは一定速度で移送している状態で推定した可動部に加わる外力から重力加速度による力を差し引いて、可動部に加わる接触力を推定する外力推定部６とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、例えば、アクチュエータが対象物に接触することによりアクチュエータに加わる接触力を推定して、アクチュエータを制御する力検出装置に関するものである。
【背景技術】
【０００２】
半導体部品やＭＥＭＳ（Micro Electro Mechanical Systems）部品の精密実装等の組立作業を行う際に、対象物である部品に合わせたアクチュエータの精密な位置決めが必要であるとともに、力検出装置により、対象物との接触を正確に検知して、この接触により生じる接触力が過大にならないように制御しながら作業することが必要とされる。
【０００３】
このような力検出装置によるアクチュエータの制御では、弾性体の撓みを利用し、アクチュエータ等の位置決め装置と作業ツールの間に取り付けるコンプライアンスデバイスや弾性体の剛性をボイスコイルモータで補償して、加圧力を制御するようにしたもの（例えば、特許文献１参照）や、アクチュエータに取り付けた力センサからの信号をサーボ制御系にフィードバックするようにしたもの（例えば、特許文献２参照）がある。また、力センサの代わりに外乱オブザーバを用いて、アクチュエータの駆動電流と位置から対象物との接触による接触力を推定して、アクチュエータを制御する方法も提案されている（例えば、特許文献３参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−８６３１７号公報
【特許文献２】特開平７−２４６６５号公報
【特許文献３】特開平６−０５２５６３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１に開示される弾性体の撓みを利用したパッシブ・コンプライアンスを用いる方法では、作業に必要な力に対応してコンプライアンス部品を交換する必要があるという課題がある。
また、特許文献２に開示される力センサからの信号に基づきアクチュエータを制御する方法では、力センサが高価であり、破損し易い等の課題がある。
【０００６】
また、特許文献３に開示される外乱オブザーバにより接触力を推定してアクチュエータを制御する方法では、安価で破損し難いという利点はあるが、外乱オブザーバで正確に対象物との接触による接触力を推定するためには、アクチュエータに加えられる外力から、重力加速度により生じる力や移動加速度により生じる力を差し引く必要があり、この点について考慮されていないという課題がある。
【０００７】
この発明は、上記のような課題を解決するためになされたもので、力センサを用いることなく、アクチュエータに加わる外力から重力加速度により生じる力や移動加速度により生じる力を取り除いて、対象物による接触力を容易に推定することができ、正確に制御を行うことができる力検出装置を提供することを目的としている。
【課題を解決するための手段】
【０００８】
この発明に係る力検出装置は、入力電圧に比例した電流を発生する電流発生部と、電流発生部により発生された電流に応じた駆動力を発生し可動部を移送するアクチュエータと、可動部の位置を検出する位置センサの出力と目標位置を比較してその誤差を電流発生部を通してアクチュエータにフィードバックすることにより位置決め制御を行う装置であって、電流発生部に入力する電圧及び位置センサにより検出された位置に基づいて、可動部が対象物に非接触、かつ、停止している状態で可動部に加わる重力加速度による力を推定した後に、アクチュエータを停止、もしくは一定速度で移送している状態で推定した可動部に加わる外力から重力加速度による力を差し引いて、可動部に加わる接触力を推定する外力推定部とを備えるものである。
【発明の効果】
【０００９】
この発明によれば、上記のように構成したので、力センサを用いることなく、アクチュエータの可動部に加わる外力から重力加速度により生じる力や移動加速度により生じる力を取り除いて、対象物による接触力を容易に推定することができる。
【図面の簡単な説明】
【００１０】
【図１】この発明の実施の形態１に係る力検出装置の構成を示す図である。
【図２】この発明の実施の形態１における外力推定部の構成を示す図である。
【図３】この発明の実施の形態１における外力推定部による接触力推定処理を示すフローチャートである。
【図４】この発明の実施の形態２に係る力検出装置の構成を示す図である。
【図５】この発明の実施の形態２における外力推定部の構成を示す図である。
【発明を実施するための形態】
【００１１】
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る力検出装置の構成を示す図である。
力検出装置は、アクチュエータ可動部１ａに設置されたワークやハンドが対象物と接触した際にアクチュエータ可動部１ａに加わる接触力を推定するものである。この力検出装置は、図１に示すように、アクチュエータ１、位置センサ２、第１比較器３、位相補償回路４、定電流ドライバ（電流発生部）５及び外力推定部６Ａから構成される。
なお、アクチュエータ可動部１ａに加えられる外力には、ワークやハンドが対象物と接触することにより生じる接触力の他に、アクチュエータ可動部１ａに加わる重力加速度により生じる力及びアクチュエータ１の移動加速度により生じる力が含まれている。
【００１２】
アクチュエータ１の可動部１ａには対象物である半導体部品やＭＥＭＳ部品の精密実装等の組立作業を行う装置のワークやハンドが設置され、アクチュエータ１全体が上下方向または回転方向に移送されてワークやハンドが対象物と接触することによりアクチュエータ可動部１ａも接触力を受ける。
アクチュエータ１は、入力電圧に比例した電流を発生する定電流ドライバ５からの電流がボイスコイルモータに流れることにより駆動力を発生してアクチュエータ可動部１ａを上下に移送する。
【００１３】
位置センサ２は、アクチュエータ可動部１ａの位置を検出するものである。この位置センサ２により検出されたアクチュエータ可動部１ａの位置を示す位置信号は、第１比較器３及び外力推定部６Ａに供給される。
【００１４】
第１比較器３は、予め設定された目標位置から、位置センサ２により検出されたアクチュエータ可動部１ａの位置を差し引くことにより差分値を算出するものである。この第１比較器３により算出された差分値を示す差分信号は位相補償回路４に供給される。
【００１５】
位相補償回路４は、第１比較器３により算出された差分値に基づいて、定電流ドライバ５の電流値を制御するものである。この位相補償回路４による制御信号を定電流ドライバ５を通してアクチュエータ１に供給することにより、位置センサ２出力が目標位置と一致するように位置決め制御が行われる。
【００１６】
外力推定部６Ａは、位置センサ２により検出されたアクチュエータ可動部１ａの位置及び定電流ドライバ５に入力する電圧に基づいて、ワークやハンドが対象物に接触することによりアクチュエータ可動部１ａに加わる接触力を推定するものである。
【００１７】
次に、外力推定部６Ａの構成について説明する。図２はこの発明の実施の形態１における外力推定部６Ａの構成を示す図である。
外力推定部６Ａは、図２に示すように、第１〜３演算部１１〜１３、第１，２ＬＰＦ（Low Pass Filter）１４，１５、サンプルホールド回路１６及び第２〜４比較器１７〜１９から構成される。
なお、外力推定部６Ａは、ノイズの影響を受け難くするために、特許文献３に開示される外乱オブザーバのように純粋微分系を含まない形で構成されるものであるが、実施の形態１における外力推定部６Ａでは、原理説明のため簡略化して説明を行う。
【００１８】
第１演算部１１は、定電流ドライバ５に入力する電圧ｅに、定電流ドライバ５の入力電圧と出力電流の比であるＫｉと、トルク定数Ｋｔを乗算するものである。この第１演算部１１により算出された値を示す信号は第１ＬＰＦ１４及び第３比較器１８に供給される。
この第１演算部１１により、定電流ドライバ５から供給された電流によりアクチュエータ１が発生する駆動力が推定される。
【００１９】
第２演算部１２は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒにラプラス演算子ｓを乗算するものであり、出力はアクチュエータ可動部１ａの速度となる。この第２演算部１２により算出された値を示す信号は第３演算部１３に供給される。
第３演算部１３は、第２演算部１２により算出されたアクチュエータ可動部１ａの速度の値に質量Ｍとラプラス演算子ｓを乗算するものであり、その出力はアクチュエータ１が発生する駆動力から、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値を差し引いた推定値となる。この第３演算部１３により算出された値を示す信号は第２ＬＰＦ１５及び第３比較器１８に供給される。
【００２０】
第１ＬＰＦ１４は、第１演算部１１から供給された信号から高域ノイズを除去するための低域ゲインが０ｄＢのフィルタである。この第１ＬＰＦ１４により高域ノイズが除去された信号は第２比較器１７に供給される。
第２ＬＰＦ１５は、第３演算部１３から供給された信号から高域ノイズを除去するための低域ゲインが０ｄＢのフィルタである。この第２ＬＰＦ１５により高域ノイズが除去された信号は第２比較器１７に供給される。
【００２１】
第２比較器１７は、第１ＬＰＦ１４を介して第１演算部１１から供給された信号により示される値から、第２ＬＰＦ１５を介して第３演算部１３から供給された信号により示される値を差し引くものである。この第２比較器１７により算出された値を示す信号はサンプルホールド回路１６に供給される。
【００２２】
ここで第３演算部１３の出力は、前記の通りアクチュエータ１が電流により発生する駆動力から、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度およびアクチュエータ可動部１ａの移動加速度により受ける力の加算値を差し引いた推定値であり、また第１演算部１１の出力は電流によりアクチュエータ１が発生する駆動力の推定値である。そのため、第２比較器１７において、第１演算部１１の出力に第１ＬＰＦ１４を介した出力から、第３演算部１３の出力に第２ＬＰＦ１５を介した出力を差し引くことにより、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値がノイズが除去された状態で推定される。
【００２３】
サンプルホールド回路１６は、位置センサ２の出力と目標位置が等しくなるように位置決め制御が行われていて、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていないことを示すモード信号を受信した場合に、第２比較器１７により算出された値を記憶し保持するものである。このサンプルホールド回路１６に保持された値を示す信号は第４比較器１９に供給される。
【００２４】
前記の通り第２比較器１７ではアクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値がノイズが除去された状態で推定されるが、アクチュエータ可動部１ａに加わる接触力が零であり、また、アクチュエータ可動部１ａが静止した状態では移動加速度により受ける力も零であるため、このサンプルホールド回路１６によりアクチュエータ可動部１ａに加わる重力加速度による力が保持される。
【００２５】
第３比較器１８は、第１演算部１１により算出された値から、第３演算部１３により算出された値を差し引くものである。この第３比較器１８により算出された値を示す信号は第４比較器１９に供給される。
【００２６】
第４比較器１９は、第３比較器１８により算出された値からサンプルホールド回路１６により保持された値を差し引くものである。
従って、この第４比較器１９によりアクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる移動加速度により受ける力の加算値が推定され、アクチュエータ可動部１ａが停止もしくは一定速度で移動している場合には接触力が推定される。
【００２７】
なお、外力推定部６Ａの各演算部１１〜１３に用いられるパラメータである、電圧電流変換係数Ｋｉ、トルク定数Ｋｔ、アクチュエータ可動部１ａの質量Ｍは、アクチュエータ１の物理パラメータと同一の値に設定される。
【００２８】
次に、上記のように構成される外力推定部６Ａによる接触力の推定動作について説明する。図３はこの発明の実施の形態１における外力推定部６Ａによる接触力推定処理を示すフローチャートである。
アクチュエータ可動部１ａに加えられる外力には、ワークやハンドが対象物と接触することにより生じる接触力の他に、アクチュエータ可動部１ａに加わる重力加速度により生じる力及びアクチュエータ１の移動加速度により生じる力が含まれている。
そのため、まず、図３に示すように、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていない状態でアクチュエータ可動部１ａに加わる外力を推定することで、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定する（ステップＳＴ３１）。
【００２９】
まず、第１演算部１１は、定電流ドライバ５に入力する電圧ｅに対して演算を行い、アクチュエータ１が発生する駆動力を推定する。この第１演算部１１により推定された値を示す信号は第１ＬＰＦ１４を介して第２比較器１７に供給される。
【００３０】
一方、第２演算部１２及び第３演算部１３は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒに対して演算を行い、アクチュエータ１が発生する駆動力からアクチュエータ可動部１ａに加わる外力を差し引いた値を推定する。ここで、ワークやハンドが対象物と接触する前にアクチュエータ可動部１ａを停止した状態において第２演算部１２及び第３演算部１３を介して算出された値は、接触力及び移動加速度により生じる力の影響はないため、駆動力から重力加速度により生じる力のみを差し引いた値となる。この第２演算部１２及び第３演算部１３により推定された値を示す信号は第２ＬＰＦ１５を介して第２比較器１７に供給される。
【００３１】
次いで、第２比較器１７は、第１ＬＰＦ１４を介して第１演算部１１から供給された信号により示される値から、第２ＬＰＦ１５を介して第３演算部１３から供給された信号により示される値を差し引くことにより、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定する。この第２比較器１７により推定された重力加速度により生じる力はサンプルホールド回路１６に記憶され保持される。
このように、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていない状態において、外力推定部６Ａによりアクチュエータ可動部１ａに加わる外力を推定することで、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定することができる。
【００３２】
次いで、アクチュエータ可動部１ａに移動加速度により生じる力が加わらないようにアクチュエータ１全体を緩やかに移送させて外力を推定することによって、アクチュエータ可動部１ａに加わる接触力を推定する（ステップＳＴ３２）。
まず、第１演算部１１は、定電流ドライバ５に入力した電圧に対して演算を行い、アクチュエータ１が発生する駆動力を推定する。この第１演算部１１により推定された値を示す信号は第３比較器１８に供給される。
【００３３】
一方、第２演算部１２及び第３演算部１３は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒに対して演算を行い、アクチュエータ１が発生する駆動力からアクチュエータ可動部１ａに加わる外力を差し引いた値を推定する。ここで、アクチュエータ１全体を緩やかに移送させている状態において第２演算部１２及び第３演算部１３を介して算出された値は、移動加速度により生じる力の影響はないため、駆動力から重力加速度により生じる力及び接触力を差し引いた値となる。この第２演算部１２及び第３演算部１３により推定された値を示す信号は第３比較器１８に供給される。
【００３４】
次いで、第３比較器１８は、第１演算部１１により算出された値から、第３演算部１３により算出された値を差し引くことにより、アクチュエータ可動部１ａに加わる重力加速度により生じる力と接触力とを加算した値を推定する。この第３比較器１８により推定された値を示す信号は第４比較器１９に供給される。
【００３５】
次いで、第４比較器１９は、第３比較器１８により算出された値から、サンプルホールド回路１６により保持された値を差し引くことにより、アクチュエータ可動部１ａに加わる接触力を推定する。
【００３６】
以上のように、この実施の形態１によれば、まず、ワークやハンドが対象物に接触する前にアクチュエータ可動部１を停止し、かつアクチュエータ１全体も移送されていない状態で、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定する。次いで、アクチュエータ１全体を緩やかに移送させて、アクチュエータ可動部１ａに加わる外力を推定して、この外力から重力加速度により生じる力を差し引いて接触力を推定するように構成したので、力センサを用いることなく、推定された外力から重力加速度により生じる力や移動加速度により生じる力の影響を取り除いて、接触力を容易に推定することができる。
【００３７】
また、アクチュエータ１の姿勢が変わり、重力加速度により生じる力による影響が変化した場合にも、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていない状態で重力加速度により生じる力を正確に推定することができるため、アクチュエータ可動部１ａに加わる接触力を正確に推定することができる。
【００３８】
実施の形態２．
図４はこの発明の実施の形態２に係る力検出装置の構成を示す図である。
図４に示す実施の形態２に係る力検出装置は、図１に示す実施の形態１に係る力検出装置に加速度センサ７を追加し、外力推定部６Ａを外力推定部６Ｂに変更したものである。
【００３９】
加速度センサ７は、アクチュエータ可動部１ａに取り付けられ、アクチュエータ可動部１ａに加わる重力加速度及び移動加速度を含む加速度を検出するものである。この加速度センサ７により検出された加速度を示す加速度信号は外力推定部６Ｂに供給される。
【００４０】
外力推定部６Ｂは、位置センサ２から供給された位置信号、定電流ドライバ５に入力する電圧及び加速度センサ７から供給された加速度信号に基づいて、ワークやハンドが対象物に接触することによりアクチュエータ可動部１ａに加わる接触力を推定するものである。
【００４１】
次に、外力推定部６Ｂの構成について説明する。図５はこの発明の実施の形態２における外力推定部６Ｂの構成を示す図である。
図５に示す実施の形態２における外力推定部６Ｂは、図２に示す実施の形態１における外力推定部６Ａから第１，２ＬＰＦ１４，１５、サンプルホールド回路１６及び第２比較器１７を削除し、第３ＬＰＦ２０及び第４演算部２１を追加したものである。その他の構成は、図２に示す実施の形態１における外力推定部６Ａと同様であり、同一の符号を付し、その説明を省略する。
【００４２】
第３ＬＰＦ２０は、加速度センサ７から供給された加速度信号から高域ノイズを除去するための低域ゲインが０ｄＢのフィルタである。この第３ＬＰＦ２０を通過した信号は第４演算部２１に供給される。
【００４３】
第４演算部２１は、第３ＬＰＦ２０を介して加速度センサ７から供給された加速度信号により示される加速度αにアクチュエータ可動部１ａの質量Ｍを乗算するものである。この第４演算部２１により算出された値を示す信号は第４比較器１９に供給される。
この第４演算部２１によりアクチュエータ可動部１ａに加わる加速度により生じる力がノイズが除去された状態で推定される。
【００４４】
次に、上記のように構成される外力推定部６Ｂによる接触力の推定動作について説明する。
まず、第１演算部１１は、定電流ドライバ５に入力する電圧ｅに対して演算を行い、アクチュエータ１が発生する駆動力を推定する。この第１演算部１１により推定された駆動力を示す信号は第３比較器１８に供給される。
【００４５】
一方、第２演算部１２及び第３演算部１３は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒに対して演算を行い、アクチュエータ１が電流により発生する駆動力から、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値を差し引いた値を推定する。
この第２演算部１２及び第３演算部１３により推定された値を示す信号は第３比較器１８に供給される。
【００４６】
次いで、第３比較器１８は、第１演算部１１により算出された値から、第３演算部１３により算出された値を差し引くことにより、アクチュエータ可動部１ａに加わる接触力と重力加速度及び移動加速度により生じる力を加えた外力を推定する。この第３比較器１８により推定された値を示す信号は第４比較器１９に供給される。
【００４７】
一方、第４演算部２１は、第３ＬＰＦ２０を介して加速度センサ７から供給された加速度信号により示されるアクチュエータ可動部１ａの加速度に対して演算を行い、アクチュエータ可動部１ａに加わる重力加速度及び移動加速度により生じる力を推定する。この第４演算部２１により推定された値を示す信号は第４比較器１９に供給される。
【００４８】
次いで、第４比較器１９は、第３比較器１８により算出された値から、第４演算部２１により算出された値を差し引くことにより、アクチュエータ可動部１ａに加わる接触力を推定する。
【００４９】
以上のように、この実施の形態２によれば、アクチュエータ可動部１ａに加わる重力加速度及び移動加速度を含む加速度を検出する加速度センサ７を備え、アクチュエータ可動部１ａの動作中に加わる外力から、この加速度により生じる力を差し引くことで、対象物からの接触力を精度よく推定することができる。また、アクチュエータ１の姿勢や移送に関係なく、常に、アクチュエータ可動部１ａに加わる接触力を正確に推定することができる。
【符号の説明】
【００５０】
１ アクチュエータ
１ａ アクチュエータ可動部
２ 位置センサ
３，１７〜１９ 第１〜４比較器
４ 位相補償回路
５ 定電流ドライバ（電流発生部）
６Ａ，６Ｂ 外力推定部
７ 加速度センサ
１１〜１３，２１ 第１〜４演算部
１４，１５，２０ 第１〜３ＬＰＦ
１６ サンプルホールド回路
【技術分野】
【０００１】
この発明は、例えば、アクチュエータが対象物に接触することによりアクチュエータに加わる接触力を推定して、アクチュエータを制御する力検出装置に関するものである。
【背景技術】
【０００２】
半導体部品やＭＥＭＳ（Micro Electro Mechanical Systems）部品の精密実装等の組立作業を行う際に、対象物である部品に合わせたアクチュエータの精密な位置決めが必要であるとともに、力検出装置により、対象物との接触を正確に検知して、この接触により生じる接触力が過大にならないように制御しながら作業することが必要とされる。
【０００３】
このような力検出装置によるアクチュエータの制御では、弾性体の撓みを利用し、アクチュエータ等の位置決め装置と作業ツールの間に取り付けるコンプライアンスデバイスや弾性体の剛性をボイスコイルモータで補償して、加圧力を制御するようにしたもの（例えば、特許文献１参照）や、アクチュエータに取り付けた力センサからの信号をサーボ制御系にフィードバックするようにしたもの（例えば、特許文献２参照）がある。また、力センサの代わりに外乱オブザーバを用いて、アクチュエータの駆動電流と位置から対象物との接触による接触力を推定して、アクチュエータを制御する方法も提案されている（例えば、特許文献３参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−８６３１７号公報
【特許文献２】特開平７−２４６６５号公報
【特許文献３】特開平６−０５２５６３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１に開示される弾性体の撓みを利用したパッシブ・コンプライアンスを用いる方法では、作業に必要な力に対応してコンプライアンス部品を交換する必要があるという課題がある。
また、特許文献２に開示される力センサからの信号に基づきアクチュエータを制御する方法では、力センサが高価であり、破損し易い等の課題がある。
【０００６】
また、特許文献３に開示される外乱オブザーバにより接触力を推定してアクチュエータを制御する方法では、安価で破損し難いという利点はあるが、外乱オブザーバで正確に対象物との接触による接触力を推定するためには、アクチュエータに加えられる外力から、重力加速度により生じる力や移動加速度により生じる力を差し引く必要があり、この点について考慮されていないという課題がある。
【０００７】
この発明は、上記のような課題を解決するためになされたもので、力センサを用いることなく、アクチュエータに加わる外力から重力加速度により生じる力や移動加速度により生じる力を取り除いて、対象物による接触力を容易に推定することができ、正確に制御を行うことができる力検出装置を提供することを目的としている。
【課題を解決するための手段】
【０００８】
この発明に係る力検出装置は、入力電圧に比例した電流を発生する電流発生部と、電流発生部により発生された電流に応じた駆動力を発生し可動部を移送するアクチュエータと、可動部の位置を検出する位置センサの出力と目標位置を比較してその誤差を電流発生部を通してアクチュエータにフィードバックすることにより位置決め制御を行う装置であって、電流発生部に入力する電圧及び位置センサにより検出された位置に基づいて、可動部が対象物に非接触、かつ、停止している状態で可動部に加わる重力加速度による力を推定した後に、アクチュエータを停止、もしくは一定速度で移送している状態で推定した可動部に加わる外力から重力加速度による力を差し引いて、可動部に加わる接触力を推定する外力推定部とを備えるものである。
【発明の効果】
【０００９】
この発明によれば、上記のように構成したので、力センサを用いることなく、アクチュエータの可動部に加わる外力から重力加速度により生じる力や移動加速度により生じる力を取り除いて、対象物による接触力を容易に推定することができる。
【図面の簡単な説明】
【００１０】
【図１】この発明の実施の形態１に係る力検出装置の構成を示す図である。
【図２】この発明の実施の形態１における外力推定部の構成を示す図である。
【図３】この発明の実施の形態１における外力推定部による接触力推定処理を示すフローチャートである。
【図４】この発明の実施の形態２に係る力検出装置の構成を示す図である。
【図５】この発明の実施の形態２における外力推定部の構成を示す図である。
【発明を実施するための形態】
【００１１】
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る力検出装置の構成を示す図である。
力検出装置は、アクチュエータ可動部１ａに設置されたワークやハンドが対象物と接触した際にアクチュエータ可動部１ａに加わる接触力を推定するものである。この力検出装置は、図１に示すように、アクチュエータ１、位置センサ２、第１比較器３、位相補償回路４、定電流ドライバ（電流発生部）５及び外力推定部６Ａから構成される。
なお、アクチュエータ可動部１ａに加えられる外力には、ワークやハンドが対象物と接触することにより生じる接触力の他に、アクチュエータ可動部１ａに加わる重力加速度により生じる力及びアクチュエータ１の移動加速度により生じる力が含まれている。
【００１２】
アクチュエータ１の可動部１ａには対象物である半導体部品やＭＥＭＳ部品の精密実装等の組立作業を行う装置のワークやハンドが設置され、アクチュエータ１全体が上下方向または回転方向に移送されてワークやハンドが対象物と接触することによりアクチュエータ可動部１ａも接触力を受ける。
アクチュエータ１は、入力電圧に比例した電流を発生する定電流ドライバ５からの電流がボイスコイルモータに流れることにより駆動力を発生してアクチュエータ可動部１ａを上下に移送する。
【００１３】
位置センサ２は、アクチュエータ可動部１ａの位置を検出するものである。この位置センサ２により検出されたアクチュエータ可動部１ａの位置を示す位置信号は、第１比較器３及び外力推定部６Ａに供給される。
【００１４】
第１比較器３は、予め設定された目標位置から、位置センサ２により検出されたアクチュエータ可動部１ａの位置を差し引くことにより差分値を算出するものである。この第１比較器３により算出された差分値を示す差分信号は位相補償回路４に供給される。
【００１５】
位相補償回路４は、第１比較器３により算出された差分値に基づいて、定電流ドライバ５の電流値を制御するものである。この位相補償回路４による制御信号を定電流ドライバ５を通してアクチュエータ１に供給することにより、位置センサ２出力が目標位置と一致するように位置決め制御が行われる。
【００１６】
外力推定部６Ａは、位置センサ２により検出されたアクチュエータ可動部１ａの位置及び定電流ドライバ５に入力する電圧に基づいて、ワークやハンドが対象物に接触することによりアクチュエータ可動部１ａに加わる接触力を推定するものである。
【００１７】
次に、外力推定部６Ａの構成について説明する。図２はこの発明の実施の形態１における外力推定部６Ａの構成を示す図である。
外力推定部６Ａは、図２に示すように、第１〜３演算部１１〜１３、第１，２ＬＰＦ（Low Pass Filter）１４，１５、サンプルホールド回路１６及び第２〜４比較器１７〜１９から構成される。
なお、外力推定部６Ａは、ノイズの影響を受け難くするために、特許文献３に開示される外乱オブザーバのように純粋微分系を含まない形で構成されるものであるが、実施の形態１における外力推定部６Ａでは、原理説明のため簡略化して説明を行う。
【００１８】
第１演算部１１は、定電流ドライバ５に入力する電圧ｅに、定電流ドライバ５の入力電圧と出力電流の比であるＫｉと、トルク定数Ｋｔを乗算するものである。この第１演算部１１により算出された値を示す信号は第１ＬＰＦ１４及び第３比較器１８に供給される。
この第１演算部１１により、定電流ドライバ５から供給された電流によりアクチュエータ１が発生する駆動力が推定される。
【００１９】
第２演算部１２は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒにラプラス演算子ｓを乗算するものであり、出力はアクチュエータ可動部１ａの速度となる。この第２演算部１２により算出された値を示す信号は第３演算部１３に供給される。
第３演算部１３は、第２演算部１２により算出されたアクチュエータ可動部１ａの速度の値に質量Ｍとラプラス演算子ｓを乗算するものであり、その出力はアクチュエータ１が発生する駆動力から、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値を差し引いた推定値となる。この第３演算部１３により算出された値を示す信号は第２ＬＰＦ１５及び第３比較器１８に供給される。
【００２０】
第１ＬＰＦ１４は、第１演算部１１から供給された信号から高域ノイズを除去するための低域ゲインが０ｄＢのフィルタである。この第１ＬＰＦ１４により高域ノイズが除去された信号は第２比較器１７に供給される。
第２ＬＰＦ１５は、第３演算部１３から供給された信号から高域ノイズを除去するための低域ゲインが０ｄＢのフィルタである。この第２ＬＰＦ１５により高域ノイズが除去された信号は第２比較器１７に供給される。
【００２１】
第２比較器１７は、第１ＬＰＦ１４を介して第１演算部１１から供給された信号により示される値から、第２ＬＰＦ１５を介して第３演算部１３から供給された信号により示される値を差し引くものである。この第２比較器１７により算出された値を示す信号はサンプルホールド回路１６に供給される。
【００２２】
ここで第３演算部１３の出力は、前記の通りアクチュエータ１が電流により発生する駆動力から、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度およびアクチュエータ可動部１ａの移動加速度により受ける力の加算値を差し引いた推定値であり、また第１演算部１１の出力は電流によりアクチュエータ１が発生する駆動力の推定値である。そのため、第２比較器１７において、第１演算部１１の出力に第１ＬＰＦ１４を介した出力から、第３演算部１３の出力に第２ＬＰＦ１５を介した出力を差し引くことにより、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値がノイズが除去された状態で推定される。
【００２３】
サンプルホールド回路１６は、位置センサ２の出力と目標位置が等しくなるように位置決め制御が行われていて、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていないことを示すモード信号を受信した場合に、第２比較器１７により算出された値を記憶し保持するものである。このサンプルホールド回路１６に保持された値を示す信号は第４比較器１９に供給される。
【００２４】
前記の通り第２比較器１７ではアクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値がノイズが除去された状態で推定されるが、アクチュエータ可動部１ａに加わる接触力が零であり、また、アクチュエータ可動部１ａが静止した状態では移動加速度により受ける力も零であるため、このサンプルホールド回路１６によりアクチュエータ可動部１ａに加わる重力加速度による力が保持される。
【００２５】
第３比較器１８は、第１演算部１１により算出された値から、第３演算部１３により算出された値を差し引くものである。この第３比較器１８により算出された値を示す信号は第４比較器１９に供給される。
【００２６】
第４比較器１９は、第３比較器１８により算出された値からサンプルホールド回路１６により保持された値を差し引くものである。
従って、この第４比較器１９によりアクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる移動加速度により受ける力の加算値が推定され、アクチュエータ可動部１ａが停止もしくは一定速度で移動している場合には接触力が推定される。
【００２７】
なお、外力推定部６Ａの各演算部１１〜１３に用いられるパラメータである、電圧電流変換係数Ｋｉ、トルク定数Ｋｔ、アクチュエータ可動部１ａの質量Ｍは、アクチュエータ１の物理パラメータと同一の値に設定される。
【００２８】
次に、上記のように構成される外力推定部６Ａによる接触力の推定動作について説明する。図３はこの発明の実施の形態１における外力推定部６Ａによる接触力推定処理を示すフローチャートである。
アクチュエータ可動部１ａに加えられる外力には、ワークやハンドが対象物と接触することにより生じる接触力の他に、アクチュエータ可動部１ａに加わる重力加速度により生じる力及びアクチュエータ１の移動加速度により生じる力が含まれている。
そのため、まず、図３に示すように、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていない状態でアクチュエータ可動部１ａに加わる外力を推定することで、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定する（ステップＳＴ３１）。
【００２９】
まず、第１演算部１１は、定電流ドライバ５に入力する電圧ｅに対して演算を行い、アクチュエータ１が発生する駆動力を推定する。この第１演算部１１により推定された値を示す信号は第１ＬＰＦ１４を介して第２比較器１７に供給される。
【００３０】
一方、第２演算部１２及び第３演算部１３は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒに対して演算を行い、アクチュエータ１が発生する駆動力からアクチュエータ可動部１ａに加わる外力を差し引いた値を推定する。ここで、ワークやハンドが対象物と接触する前にアクチュエータ可動部１ａを停止した状態において第２演算部１２及び第３演算部１３を介して算出された値は、接触力及び移動加速度により生じる力の影響はないため、駆動力から重力加速度により生じる力のみを差し引いた値となる。この第２演算部１２及び第３演算部１３により推定された値を示す信号は第２ＬＰＦ１５を介して第２比較器１７に供給される。
【００３１】
次いで、第２比較器１７は、第１ＬＰＦ１４を介して第１演算部１１から供給された信号により示される値から、第２ＬＰＦ１５を介して第３演算部１３から供給された信号により示される値を差し引くことにより、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定する。この第２比較器１７により推定された重力加速度により生じる力はサンプルホールド回路１６に記憶され保持される。
このように、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていない状態において、外力推定部６Ａによりアクチュエータ可動部１ａに加わる外力を推定することで、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定することができる。
【００３２】
次いで、アクチュエータ可動部１ａに移動加速度により生じる力が加わらないようにアクチュエータ１全体を緩やかに移送させて外力を推定することによって、アクチュエータ可動部１ａに加わる接触力を推定する（ステップＳＴ３２）。
まず、第１演算部１１は、定電流ドライバ５に入力した電圧に対して演算を行い、アクチュエータ１が発生する駆動力を推定する。この第１演算部１１により推定された値を示す信号は第３比較器１８に供給される。
【００３３】
一方、第２演算部１２及び第３演算部１３は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒに対して演算を行い、アクチュエータ１が発生する駆動力からアクチュエータ可動部１ａに加わる外力を差し引いた値を推定する。ここで、アクチュエータ１全体を緩やかに移送させている状態において第２演算部１２及び第３演算部１３を介して算出された値は、移動加速度により生じる力の影響はないため、駆動力から重力加速度により生じる力及び接触力を差し引いた値となる。この第２演算部１２及び第３演算部１３により推定された値を示す信号は第３比較器１８に供給される。
【００３４】
次いで、第３比較器１８は、第１演算部１１により算出された値から、第３演算部１３により算出された値を差し引くことにより、アクチュエータ可動部１ａに加わる重力加速度により生じる力と接触力とを加算した値を推定する。この第３比較器１８により推定された値を示す信号は第４比較器１９に供給される。
【００３５】
次いで、第４比較器１９は、第３比較器１８により算出された値から、サンプルホールド回路１６により保持された値を差し引くことにより、アクチュエータ可動部１ａに加わる接触力を推定する。
【００３６】
以上のように、この実施の形態１によれば、まず、ワークやハンドが対象物に接触する前にアクチュエータ可動部１を停止し、かつアクチュエータ１全体も移送されていない状態で、アクチュエータ可動部１ａに加わる重力加速度により生じる力を推定する。次いで、アクチュエータ１全体を緩やかに移送させて、アクチュエータ可動部１ａに加わる外力を推定して、この外力から重力加速度により生じる力を差し引いて接触力を推定するように構成したので、力センサを用いることなく、推定された外力から重力加速度により生じる力や移動加速度により生じる力の影響を取り除いて、接触力を容易に推定することができる。
【００３７】
また、アクチュエータ１の姿勢が変わり、重力加速度により生じる力による影響が変化した場合にも、ワークやハンドが対象物に接触していない状態でアクチュエータ可動部１ａが停止しており、かつアクチュエータ１全体も移送されていない状態で重力加速度により生じる力を正確に推定することができるため、アクチュエータ可動部１ａに加わる接触力を正確に推定することができる。
【００３８】
実施の形態２．
図４はこの発明の実施の形態２に係る力検出装置の構成を示す図である。
図４に示す実施の形態２に係る力検出装置は、図１に示す実施の形態１に係る力検出装置に加速度センサ７を追加し、外力推定部６Ａを外力推定部６Ｂに変更したものである。
【００３９】
加速度センサ７は、アクチュエータ可動部１ａに取り付けられ、アクチュエータ可動部１ａに加わる重力加速度及び移動加速度を含む加速度を検出するものである。この加速度センサ７により検出された加速度を示す加速度信号は外力推定部６Ｂに供給される。
【００４０】
外力推定部６Ｂは、位置センサ２から供給された位置信号、定電流ドライバ５に入力する電圧及び加速度センサ７から供給された加速度信号に基づいて、ワークやハンドが対象物に接触することによりアクチュエータ可動部１ａに加わる接触力を推定するものである。
【００４１】
次に、外力推定部６Ｂの構成について説明する。図５はこの発明の実施の形態２における外力推定部６Ｂの構成を示す図である。
図５に示す実施の形態２における外力推定部６Ｂは、図２に示す実施の形態１における外力推定部６Ａから第１，２ＬＰＦ１４，１５、サンプルホールド回路１６及び第２比較器１７を削除し、第３ＬＰＦ２０及び第４演算部２１を追加したものである。その他の構成は、図２に示す実施の形態１における外力推定部６Ａと同様であり、同一の符号を付し、その説明を省略する。
【００４２】
第３ＬＰＦ２０は、加速度センサ７から供給された加速度信号から高域ノイズを除去するための低域ゲインが０ｄＢのフィルタである。この第３ＬＰＦ２０を通過した信号は第４演算部２１に供給される。
【００４３】
第４演算部２１は、第３ＬＰＦ２０を介して加速度センサ７から供給された加速度信号により示される加速度αにアクチュエータ可動部１ａの質量Ｍを乗算するものである。この第４演算部２１により算出された値を示す信号は第４比較器１９に供給される。
この第４演算部２１によりアクチュエータ可動部１ａに加わる加速度により生じる力がノイズが除去された状態で推定される。
【００４４】
次に、上記のように構成される外力推定部６Ｂによる接触力の推定動作について説明する。
まず、第１演算部１１は、定電流ドライバ５に入力する電圧ｅに対して演算を行い、アクチュエータ１が発生する駆動力を推定する。この第１演算部１１により推定された駆動力を示す信号は第３比較器１８に供給される。
【００４５】
一方、第２演算部１２及び第３演算部１３は、位置センサ２により検出されたアクチュエータ可動部１ａの位置ｒに対して演算を行い、アクチュエータ１が電流により発生する駆動力から、アクチュエータ可動部１ａに加わる接触力とアクチュエータ可動部１ａに加わる重力加速度および移動加速度により受ける力の加算値を差し引いた値を推定する。
この第２演算部１２及び第３演算部１３により推定された値を示す信号は第３比較器１８に供給される。
【００４６】
次いで、第３比較器１８は、第１演算部１１により算出された値から、第３演算部１３により算出された値を差し引くことにより、アクチュエータ可動部１ａに加わる接触力と重力加速度及び移動加速度により生じる力を加えた外力を推定する。この第３比較器１８により推定された値を示す信号は第４比較器１９に供給される。
【００４７】
一方、第４演算部２１は、第３ＬＰＦ２０を介して加速度センサ７から供給された加速度信号により示されるアクチュエータ可動部１ａの加速度に対して演算を行い、アクチュエータ可動部１ａに加わる重力加速度及び移動加速度により生じる力を推定する。この第４演算部２１により推定された値を示す信号は第４比較器１９に供給される。
【００４８】
次いで、第４比較器１９は、第３比較器１８により算出された値から、第４演算部２１により算出された値を差し引くことにより、アクチュエータ可動部１ａに加わる接触力を推定する。
【００４９】
以上のように、この実施の形態２によれば、アクチュエータ可動部１ａに加わる重力加速度及び移動加速度を含む加速度を検出する加速度センサ７を備え、アクチュエータ可動部１ａの動作中に加わる外力から、この加速度により生じる力を差し引くことで、対象物からの接触力を精度よく推定することができる。また、アクチュエータ１の姿勢や移送に関係なく、常に、アクチュエータ可動部１ａに加わる接触力を正確に推定することができる。
【符号の説明】
【００５０】
１ アクチュエータ
１ａ アクチュエータ可動部
２ 位置センサ
３，１７〜１９ 第１〜４比較器
４ 位相補償回路
５ 定電流ドライバ（電流発生部）
６Ａ，６Ｂ 外力推定部
７ 加速度センサ
１１〜１３，２１ 第１〜４演算部
１４，１５，２０ 第１〜３ＬＰＦ
１６ サンプルホールド回路
【特許請求の範囲】
【請求項１】
入力電圧に比例した電流を発生する電流発生部と、
前記電流発生部により発生された電流に応じた駆動力を発生し可動部を移送するアクチュエータと、
前記可動部の位置を検出する位置センサの出力と目標位置を比較してその誤差を前記電流発生部を通して前記アクチュエータにフィードバックすることにより位置決め制御を行う装置であって、
前記電流発生部に入力する電圧及び前記位置センサにより検出された位置に基づいて、前記可動部が対象物に非接触、かつ、停止している状態で前記可動部に加わる重力加速度による力を推定した後に、前記アクチュエータを停止、もしくは一定速度で移送している状態で推定した前記可動部に加わる外力から前記重力加速度による力を差し引いて、前記可動部に加わる接触力を推定する外力推定部と
を備える力検出装置。
【請求項２】
入力電圧に比例した電流を出力する電流発生部と、
前記電流発生部により出力された電流に応じた駆動力を発生し可動部を移送するアクチュエータと、
前記可動部の位置を検出する位置センサの出力と目標位置を比較してその誤差を前記電流発生部を通して前記アクチュエータにフィードバックすることにより位置決め制御を行う装置であって、
前記可動部の加速度を検出する加速度センサと、
前記電流発生部に入力する電圧及び前記位置センサにより検出された位置に基づいて推定した前記可動部に加わる外力から、前記加速度センサにより検出された加速度に基づいて推定した前記可動部に加わる加速度により生じる力を差し引いて、前記可動部に加わる接触力を推定する外力推定部と
を備える力検出装置。
【請求項１】
入力電圧に比例した電流を発生する電流発生部と、
前記電流発生部により発生された電流に応じた駆動力を発生し可動部を移送するアクチュエータと、
前記可動部の位置を検出する位置センサの出力と目標位置を比較してその誤差を前記電流発生部を通して前記アクチュエータにフィードバックすることにより位置決め制御を行う装置であって、
前記電流発生部に入力する電圧及び前記位置センサにより検出された位置に基づいて、前記可動部が対象物に非接触、かつ、停止している状態で前記可動部に加わる重力加速度による力を推定した後に、前記アクチュエータを停止、もしくは一定速度で移送している状態で推定した前記可動部に加わる外力から前記重力加速度による力を差し引いて、前記可動部に加わる接触力を推定する外力推定部と
を備える力検出装置。
【請求項２】
入力電圧に比例した電流を出力する電流発生部と、
前記電流発生部により出力された電流に応じた駆動力を発生し可動部を移送するアクチュエータと、
前記可動部の位置を検出する位置センサの出力と目標位置を比較してその誤差を前記電流発生部を通して前記アクチュエータにフィードバックすることにより位置決め制御を行う装置であって、
前記可動部の加速度を検出する加速度センサと、
前記電流発生部に入力する電圧及び前記位置センサにより検出された位置に基づいて推定した前記可動部に加わる外力から、前記加速度センサにより検出された加速度に基づいて推定した前記可動部に加わる加速度により生じる力を差し引いて、前記可動部に加わる接触力を推定する外力推定部と
を備える力検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２０１１−１４１１５７（Ｐ２０１１−１４１１５７Ａ）
【公開日】平成２３年７月２１日（２０１１．７．２１）
【国際特許分類】
【出願番号】特願２０１０−１１４３（Ｐ２０１０−１１４３）
【出願日】平成２２年１月６日（２０１０．１．６）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り 平成２１年１１月２７日 インターネットアドレス「ｈｔｔｐ：／／ｊｐ．ａｚｂｉｌ．ｃｏｍ／ｌｉｂｒａｒｙ／ｒｅｖｉｅｗ．ｈｔｍｌ」に発表
【出願人】（０００００６６６６）株式会社山武 (1,808)
【Ｆターム（参考）】
【公開日】平成２３年７月２１日（２０１１．７．２１）
【国際特許分類】
【出願日】平成２２年１月６日（２０１０．１．６）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り 平成２１年１１月２７日 インターネットアドレス「ｈｔｔｐ：／／ｊｐ．ａｚｂｉｌ．ｃｏｍ／ｌｉｂｒａｒｙ／ｒｅｖｉｅｗ．ｈｔｍｌ」に発表
【出願人】（０００００６６６６）株式会社山武 (1,808)
【Ｆターム（参考）】
[ Back to top ]