位置決め装置
【課題】 精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる多軸の位置決め装置を提供する。
【解決手段】 位置決め装置４は、駆動軸４４を駆動するリニアアクチュエータ４２と、リンク機構４３とが駆動軸４４によって連結され、リニアアクチュエータ４２が駆動軸を駆動することでリンク機構４３が駆動されて部品の位置決めを行う。コントローラ４１は、変位センサ４５及び位置座標変換演算部４１４が求めたハンドの位置、速度、及び加速度から装置のインピーダンスを算出し、算出した装置のインピーダンスからリニアアクチュエータ４２の駆動力を算出する。
【解決手段】 位置決め装置４は、駆動軸４４を駆動するリニアアクチュエータ４２と、リンク機構４３とが駆動軸４４によって連結され、リニアアクチュエータ４２が駆動軸を駆動することでリンク機構４３が駆動されて部品の位置決めを行う。コントローラ４１は、変位センサ４５及び位置座標変換演算部４１４が求めたハンドの位置、速度、及び加速度から装置のインピーダンスを算出し、算出した装置のインピーダンスからリニアアクチュエータ４２の駆動力を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、ロボットにより半導体部品などの微小部品や精密部品を実装する作業において、それらの部品を精密に位置決めするための位置決め装置に関する。
【背景技術】
【０００２】
半導体部品やＭＥＭＳ（Micro Electro Mechanical Systems）技術によって製造される精密部品を組み立てる作業時には、部品にあわせて精密な位置決めが必要であるとともに、部品の接触を正確に検知して、接触力が過大にならないように位置決めを制御することが必要である。
【０００３】
このような高精度の制御を行う装置の一例として、半導体素子を基板に実装するときに加圧するアームを、平行板ばねで構成した弾性案内に取り付け、ボイスコイルモータで加圧力を与えるように構成し、ボイスコイルモータのコイルに流す電流を電流調節器で調節することによって加圧力を制御するようにしたものが知られている（下記特許文献１）。
【０００４】
また、ロボットの手首に取り付けた力センサからの信号をサーボ制御系にフィードバックするものが知られている（下記特許文献２）。これは、ロボットの作業中、力をモニタリングすることによって部品位置の認識と作業の成否確認を行う一方、力情報を制御系にフィードバックすることによってコンプライアンス制御を実現し、作業の信頼性を向上させるようにしたものである。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平７−８６３１７号公報
【特許文献２】特開平７−２４６６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、従来の位置決め制御の技術には、次のような問題があった。
【０００７】
上記特許文献１の技術では、静止時には精密は加圧力を加えることができるが、部品搬送時の振動を抑えることや押し付けながら移動する作業で接触力を精密に制御することは難しい。また、接触を検知するためには、別に接触センサを付加する必要がある。
【０００８】
上記特許文献２の技術では、アームの質量や減速器の摩擦力の影響により、微小な接触力を維持しながら作業を行うことは難しい。
【０００９】
また、特許文献１、２のいずれの技術においても、ロボットの先のハンドや冶具の質量の影響により、位置が変化したり接触力が過大になったりする。そのような過大な質量による衝突を回避するためには、作業速度を遅くして作業を慎重に行わなければならず、それだけ作業時間が増大してしまう。
【００１０】
更に、特許文献１、２のいずれの技術も、単一軸の構造であるため、例えば、軸挿入作業やすみ肉半田付けなどの複雑な組立作業を行うには、複雑な調整作業が必要となり、安定した制御を行うことは難しい。
【００１１】
具体的には、単一軸の構造で軸挿入作業を制御するためには、Ｘ−Ｙ軸方向の並進とＸ−Ｙ軸周りの回転方向の調整が必要とされる。また、単一軸構造で、すみ肉半田付けを制御するには、稜線と直交する２つの並進方向の制御が必要とされる。単一軸構造で、このような制御を安定して行うことは難しい。
【００１２】
本発明は、上記の問題を解決するものとして、精密又は微小な部品を多軸構造で位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる位置決め装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
本発明は、部品を精密に位置決めするための位置決め装置であって、複数の駆動軸を駆動するアクチュエータと、前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部と、前記部品操作部の変位を計測する計測手段と、前記計測手段で計測された変位と該変位から算出した速度及び加速度と前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成するコントローラとを備えたことを特徴とする。
【００１４】
本発明の位置決め装置において、コントローラは、複数の駆動軸に連結されてアクチュエータにより駆動されるリンク機構を介して駆動される部品操作部の変位と、この変位から算出した速度及び加速度と、アクチュエータ及びリンク機構を含む機械系のインピーダンスとに基づいて、アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成する。
【００１５】
この出力に応じて、アクチュエータは、複数の駆動軸を駆動し、各駆動軸を介してリンク機構を駆動するため、リンク機構は複数の自由度を有する。部品操作部は、このリンク機構によって駆動される。これにより、単一の駆動軸で部品操作部を駆動する場合と比較して、部品操作部の位置をきめ細かく調整することが可能となる。
【００１６】
また、コントローラは、部品操作部の位置、速度及び加速度から機械系のインピーダンスを制御することができる。このため、微小な接触力を維持しながら位置決めを実行することが可能となる。
【００１７】
従って、本発明の位置決め装置によれば、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００１８】
上記の発明において、前記計測手段は、前記複数の駆動軸にそれぞれ取り付けられ、各駆動軸の変位を計測する変位センサと、前記変位センサが計測した各駆動軸の変位から、前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を算出する位置座標変換演算手段とを備え、前記コントローラは、前記位置座標変換演算手段によって算出された部品操作部の位置、部品操作部の速度、及び部品操作部の加速度と、前記複数のアクチュエータを含む機械系のインピーダンスに基づいて、接触位置における駆動力を算出する多自由度インピーダンス制御演算手段と、前記多自由度インピーダンス制御演算手段によって算出された部品操作部における駆動力から、前記アクチュエータの駆動軸毎の駆動力を算出する駆動力座標変換演算手段と、前記駆動力座標変換演算手段が算出した駆動軸毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって、当該アクチュエータを駆動してリンク機構を動作させる駆動電流生成手段とを備えることが好ましい。
【００１９】
この態様によれば、変位センサが、各駆動軸の変位を計測し、位置座標変換演算手段が、変位センサが計測した各駆動軸の変位から、部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を算出する。多自由度インピーダンス制御演算手段が、位置座標変換演算手段が算出した部品操作部の位置、部品操作部の速度、及び部品操作部の加速度と、複数のリニアアクチュエータを含む機械系のインピーダンスに基づいて、部品操作部における駆動力を算出する。駆動力座標変換演算手段が、多自由度インピーダンス制御演算手段が算出した部品操作部における駆動力から、リニアアクチュエータの駆動軸毎の駆動力を算出する。駆動電流生成手段が、駆動力座標変換演算手段が算出した駆動軸毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって当該リニアアクチュエータを駆動してリンク機構を動作させる。
【００２０】
これにより、部品操作部の位置を直接計測できない場合でも、複数の駆動軸の位置から部品操作部の位置を算出して部品操作部における駆動力を算出することが可能となり、多軸の位置決め制御装置において、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００２１】
上記の発明において、前記多自由度インピーダンス制御演算手段は、前記インピーダンスを表す部品操作部の位置、部品操作部の速度、部品操作部の加速度の各フィードバックゲインに前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を乗算して得られる値と、任意に設定した外力の値とから部品操作部における駆動力を算出することが好ましい。
【００２２】
この態様によれば、多自由度インピーダンス制御演算手段は、インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインに部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を乗算して得られる値と、任意に設定した外力の値とから部品操作部における駆動力を算出するようにしている。
【００２３】
これにより、多軸の位置決め装置においても、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００２４】
上記の発明において、請求項３記載の位置決め装置において、前記多自由度インピーダンス制御演算手段は、前記リンク機構及びアクチュエータの剛性、粘性、及び慣性を装置の先端の直交座標で表した剛性行列、粘性行列、及び慣性行列を、前記インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインとして用いることが好ましい。
【００２５】
この態様によれば、多自由度インピーダンス制御演算手段は、リンク機構及びリニアアクチュエータの剛性、粘性、及び慣性を部品操作部の直交座標で表した剛性行列、粘性行列、及び慣性行列を、インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインとして用いる。
【００２６】
これにより、部品操作部の変位を直接計測できない場合でも、複数の駆動軸の変位から部品操作部の位置を算出して部品操作部における駆動力を算出することが可能となり、多軸の位置決め装置においても、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００２７】
上記の発明において、前記コントローラは、前記部品操作部の位置、部品操作部の速度、部品操作部の加速度、及び部品操作部の駆動力と、自装置の剛性行列、粘性行列、及び慣性行列の推定値とから、外力の推定値を算出して外部に通知する外力推定演算手段を更に備えることが好ましい。
【００２８】
この態様によれば、外力推定演算手段が、部品操作部の位置、部品操作部の速度、部品操作部の加速度、及び部品操作部の駆動力と、自装置の剛性行列、粘性行列、及び慣性行列の推定値とから、外力の推定値を算出して外部に通知するようにしている。
【００２９】
これにより、位置決め装置の上位コントローラが、外力の推定値から、部品操作部から部品への重力や接触力を認識可能となり、作業状態に応じた制御を行うことが可能となる。
【００３０】
本発明によれば、複数の駆動軸を駆動するアクチュエータと、前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部とを有し、前記アクチュエータを駆動して部品を精密に位置決めする位置決め装置の制御方法であって、前記部品操作部の変位を計測する計測ステップと、前記計測手段で計測された変位と、該変位から算出した速度及び加速度と、前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成する制御ステップとを含むことを特徴とする。
【００３１】
本発明によれば、複数の駆動軸に連結されてアクチュエータによって駆動されるリンク機構を介して駆動される部品操作部の変位と、この変位から算出した部品操作部の速度及び加速度と、アクチュエータ及びリンク機構を含む機械系のインピーダンスとに基づいて、アクチュエータの駆動力を演算し、演算した駆動力に応じて各アクチュエータを駆動する出力を生成するようにしている。
【００３２】
これにより、多軸の位置決め装置において、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【図面の簡単な説明】
【００３３】
【図１】ロボットのアームの先端に位置決め装置を取り付けた状態を示す外観図。
【図２】位置決め装置の一構成例を示す模式図。
【図３】位置決め装置の力学モデルの説明図。
【図４】位置決め装置の制御系を示すブロック図。
【発明を実施するための形態】
【００３４】
図１に示すように、本発明の実施形態の位置決め装置は、ロボット１のアーム２の先端に取り付けられる位置決め機構４と部品を把持するハンド３とを備えて構成される。ハンド３は、本発明における部品操作部に相当する。位置決め機構４は、ハンド３にコンプライアンスを与える。
【００３５】
位置決め機構４は、図２に示すように、ロボット１のアーム２に取り付けられるｎ（ｎは２以上の自然数で、本実施形態ではｎ＝２）個のリニアアクチュエータ４２（以下、２つのアクチュエータを４２−１，４２−２で示す）と、ハンド３に取り付けられ、リニアアクチュエータ４２によって駆動されるリンク機構４３と、リニアアクチュエータ４２の作動を制御するコントローラ４１（図４）とを備えている。
【００３６】
リニアアクチュエータ４２は、例えば、サーボモータ、ボイスコイルモータ（ＶＣＭ）或いはシリンダ機構などで構成される。リンク機構４３は、複数の自由度を有し、２つの駆動軸４４（以下、２つの駆動軸を４４−１，４４−２で示す）によってリニアアクチュエータ４２と接続される。
【００３７】
図２において、リンク機構４３は、連結点ａにてリンク４３１の一端とリンク４３４の一端が接続され、連結点ｂにてリンク４３１の他端とリンク４３２の一端が接続され、連結点ｃにてリンク４３２の他端とリンク４３３の一端が接続され、連結点ｄにてリンク４３３の他端とリンク４３４の他端が接続されてなる平行リンクと、この平行リンクと連結点ｄにて接続されるリンク４３５とを備えている。
【００３８】
平行リンクは、連結点ａにおいて第１リニアアクチュエータ４２−１によって駆動される第１駆動軸４４−１と接続される。また、連結点ｄにおいて一端が平行リンクと接続されたリンク４３５の他端は、点ｅにおいて、第２リニアアクチュエータ４２−２によって駆動される第２駆動軸４４−２に接続される。また、平行リンクのリンク４３３がハンド３と接続される。従って、リンク機構４３は、Ｘ−Ｚ軸に対して２自由度を有し、２個のリニアアクチュエータ４２−１，４２−２で駆動される。
【００３９】
位置決め装置の制御系の動作は、下記の力学モデルに基づくコントローラ４１での演算処理によって実行されるものであり、以下これについて説明する。
【００４０】
図３において、位置決め装置のハンド３の位置を表す座標ｘの右方向を正（＋）とすると、位置決め装置の運動方程式は、下記の式（１）のように表される。
【００４１】
ｆa＋ｆd＝Ｍaｘ”＋Ｄaｘ’＋Ｋaｘ …（１）
但し、ｘ’、ｘ”は位置決め装置の速度、加速度を表す。
【００４２】
Ｍa：位置決め装置の慣性行列（ｎ×ｎ）
Ｄa：位置決め装置の粘性行列（ｎ×ｎ）
Ｋa：位置決め装置の剛性行列（ｎ×ｎ）
ｎ：リンク機構４３の自由度
ｆa：位置決め装置が発生する駆動力
ｆd：外力（重力、接触力など）
なお、上記慣性行列、粘性行列及び剛性行列は、リンク機構４３及びリニアアクチュエータ４２の慣性、粘性及び剛性を、位置決め装置の先端部位（ハンド３の位置）の直交座標で表したものである。
【００４３】
上記駆動力ｆaは、次式で表される。
【００４４】
ｆa＝−Ｍcｘ”−Ｄcｘ’−Ｋcｘ＋ｆc …（２）
ここで、ｆcは重力相殺や作業のために必要な力、Ｍc、Ｄc、Ｋcは加速度、速度、位置の各フィードバックゲインであり、それぞれ任意に決められるパラメータである。
【００４５】
以下、位置決め装置の先端の位置ｘ、速度ｘ’、加速度ｘ”をそれぞれ装置位置、装置速度、装置加速度と呼ぶことがある。
【００４６】
上式（１）及び（２）から、
ｆd＝Ｍvｘ”＋Ｄvｘ’＋Ｋvｘ−ｆc …（３）
が導かれる。但し、Ｍv、Ｄv、Ｋvは、下記の式（４）〜（６）で表される力学モデルのインピーダンスである。
【００４７】
Ｍv＝Ｍa＋Ｍc …（４）
Ｄv＝Ｄa＋Ｄc …（５）
Ｋv＝Ｋa＋Ｋc …（６）
従って、パラメータＭc、Ｄc、Ｋcをそれぞれ適当に設定することにより、外力に対する機械系の応答を決めるインピーダンス（Ｍv、Ｄv、Ｋv）を定めることができる。すなわち、コントローラ４１により、機械系のインピーダンスを制御することができる。
【００４８】
なお、駆動力ｆaは、式（２）において加速度フィードバック（−Ｍcｘ”）や速度フィードバック（Ｄcｘ’）の項を省略し、次式のようにしてもよい。
【００４９】
ｆa＝−Ｋcｘ＋ｆc …（７）
上式（７）の場合は、機械系が本来持っている慣性及び粘性での応答となる。
【００５０】
また、リンク機構４３では、位置決め装置の先端部（この場合、ハンド３）にセンサを取り付けることが困難な場合があり、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を直接計測できないことがある。そのような場合は、各軸のリニアアクチュエータの先端にそれぞれセンサを取り付けて、リニアアクチュエータの位置（角度）から、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を求めるようにしてもよい。
【００５１】
一般に、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”は、軸位置ｑを用いて、下記の式（８）〜式（１０）で表される。
【００５２】
ｘ＝Ｔ（ｑ） …（８）
ｘ’＝Ｊｑ’ …（９）
ｘ”＝Ｊ’ｑ’＋Ｊｑ” …（１０）
但し、
Ｔ：位置変換式
ｑ：各軸の位置（角度）
ｑ’：各軸の速度
ｑ”：各軸の加速度
Ｊ：ヤコビ行列
Ｊ’：Ｊの時間微分。
【００５３】
図４は、図２に示した位置決め機構４の制御装置（コントローラ）を含む機能ブロック図である。
【００５４】
本実施形態の制御系は、Ｘ−Ｚ軸に対してｎ＝２自由度を有するリンク機構４３と、多自由度インピーダンス制御演算部４１１、駆動力座標変換演算部４１２、駆動電流生成部４１３、位置座標変換演算部４１４及び外力推定演算部４１５を有するコントローラ４１と、コントローラ４１によって制御されてリンク機構４３を駆動する２つのリニアアクチュエータ４２−１，４２−２と、各リニアアクチュエータに取り付けられて各リニアアクチュエータの位置（変位）を計測する変位センサ４５−１，４５−２とを含んでいる。変位センサ４５としては、例えば、接近センサ、ひずみケージ、リニアエンコーダなどが用いられる。
【００５５】
なお、変位センサ４５と位置座標変換演算部４１４とで、本発明における計測手段を構成している。
【００５６】
前述したように、実施形態は、自由度ｎ＝２の位置決め装置であるため、２個のリニアアクチュエータ４２−１，４２−１を備え、各リニアアクチュエータが各々連結した駆動軸４４−１，４４−２を介してリンク機構４３を駆動する。ｎ＝３の場合は、リニアアクチュエータ、変位センサ及び駆動軸の組は３つとなる。すなわち、位置決め装置は、少なくとも自由度ｎに相当する個数のリニアアクチュエータ及び駆動軸を有するものとなる。
【００５７】
変位センサ４５は、リニアアクチュエータ４２に取り付けられ、当該リニアアクチュエータ４２の先端の位置（角度）ｑを計測する。図示の例では、２つの変位センサ４５−１，４５−２が、リニアアクチュエータ４２−１，４２−２に取り付けられている。以下、変位センサ４５が計測するリニアアクチュエータ４２の先端の位置を、駆動軸の位置ということがある。
【００５８】
位置座標変換演算部４１４は、上式（８）〜（１０）を用いて、変位センサ４５が計測した各軸のリニアアクチュエータ４２の位置（各駆動軸の位置）ｑから装置位置ｘ、装置速度ｘ’、及び装置加速度ｘ”を算出する。
【００５９】
多自由度インピーダンス制御演算部４１１は、上記位置座標変換演算部４１４によって算出された装置位置ｘ、装置速度ｘ’、装置加速度ｘ”と、上位コントローラ（この場合、ロボット１のコントローラ）から入力されるパラメータ情報に基づいて、位置決め装置が発生すべき駆動力を求める。ここで、パラメータ情報とは、上式（１）〜（６）による演算に必要な情報であり、重力相殺や作業のために必要な力ｆcと、フィードバックゲインを表すパラメータＭc、Ｄc、Ｋcである。
【００６０】
駆動力座標変換演算部４１２は、上記多自由度インピーダンス制御演算部４１１によって求められた駆動力を、リニアアクチュエータ４２の駆動力に変換する。多自由度インピーダンス制御演算部４１１が求める駆動力は、位置決め装置が発生すべき駆動力、すなわち作業座標（装置位置ｘの座標）における駆動力である。
【００６１】
従って、駆動力座標変換演算部４１２は、作業座標の駆動力を、リニアアクチュエータ４２毎の駆動力に変換する。更に、駆動力座標変換演算部４１２は、リニアアクチュエータ４２毎の駆動力を、それぞれ当該リニアアクチュエータ４２を駆動する電流の値を示す電流指示値に変換する。
【００６２】
リニアアクチュエータ毎の駆動力は、次式によって表される。
【００６３】
τ＝ＪＴｆa …（１１）
但し、
τ：各駆動軸の駆動力
Ｊ：ヤコビ行列（ｘ’＝Ｊｑ’）
ＪＴ：Ｊの転置行列
ｑ：各駆動軸の位置（角度）。
【００６４】
駆動力座標変換演算部４１２は、上式（１０）を用いてリニアアクチュエータ毎の駆動力を求め、求めた駆動力を電流指示値に変換する。
【００６５】
駆動電流生成部４１３は、駆動力座標変換演算部４１２によって変換されたリニアアクチュエータ毎の電流指示値に応じた駆動電流をそれぞれ生成し、この駆動電流を当該リニアアクチュエータに供給してリニアアクチュエータを駆動する。これにより、リニアアクチュエータ４２がリンク機構４３を動作させて位置決めが行われる。
【００６６】
外力推定演算部４１５は、上記多自由度インピーダンス制御演算部４１１によって求められた駆動力と、位置座標変換演算部４１４によって算出された装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”に基づいて、外力を推定する。
【００６７】
外力は、上式（１）より
ｆd＝Ｍaｘ”＋Ｄaｘ’＋Ｋaｘ−ｆa …（１２）
で表される。
【００６８】
しかし、実際上、位置決め装置の慣性行列Ｍa、粘性行列Ｄa及び剛性行列Ｋaの真の値はわからないので、上式（１２）において慣性行列Ｍa、粘性行列Ｄa及び剛性行列Ｋaの値には推定値を用いる。
【００６９】
これにより、外力の推定値<ｆd>は、次式（１３）で表される。
【００７０】
<ｆd>＝<Ｍa>ｘ”＋<Ｄa>ｘ’＋<Ｋa>ｘ−ｆa …（１３）
但し、
<Ma>：慣性行列の推定値
<Ｄa>：粘性行列の推定値
<Ｋa>：剛性行列の推定値
外力推定演算部４１５は、上式（１３）を用いて外力推定値を求める。
【００７１】
次に、本実施形態の制御系の動作を説明する。
【００７２】
まず、変位センサ４５によりリニアアクチュエータ４２の位置が計測される。変位センサ４５は、計測したリニアアクチュエータ４２の位置を位置座標変換演算部４１４に通知する。
【００７３】
位置座標変換演算部４１４は、変位センサ４５が計測した各軸のリニアアクチュエータ４２の位置と上式（８）〜（１０）を用いて、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を算出する。位置座標変換演算部４１４は、算出した装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を、多自由度インピーダンス制御演算部４１１及び外力推定演算部４１５に通知する。
【００７４】
多自由度インピーダンス制御演算部４１１は、通知された装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”と、上位コントローラから入力されるパラメータ情報（重力相殺や作業のために必要な力ｆc、及びパラメータMc、Dc、Ｋc）とを用いて、上式（２）に基づき位置決め装置が発生すべき駆動力ｆa（ｎ次元ベクトル）を算出する。そして、算出した駆動力を駆動力座標変換演算部４１２及び外力推定演算部４１５に通知する。
【００７５】
駆動力座標変換演算部４１２は、通知された駆動力ｆa、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を用いて、上式（１１）により、各リニアアクチュエータの駆動力τ（軸駆動力ベクトル）を算出する。そして、算出した各リニアアクチュエータの駆動力τを、当該リニアアクチュエータを駆動する電流の値を示す電流指示値に変換する。駆動力座標変換演算部４１２は、電流指示値を当該リニアアクチュエータ４２に通知する。
【００７６】
なお、駆動力τから電流指示値への変換は、予め定められた式に基づいて行ってもよく、或いは、予め駆動力と電流指示値とを対応付けた変換テーブルを作成しておき、この変換テーブルを用いるようにしてもよい。
【００７７】
駆動電流生成部４１３は、通知された電流指示値に応じた駆動電流を生成し、当該リニアアクチュエータ４２に出力（供給）する。これにより、リニアアクチュエータ４２は、駆動力座標変換演算部４１２によって算出された駆動力に応じてリンク機構４３を動作させ、ハンド３の位置決めを行う。
【００７８】
一方、外力推定演算部４１５は、通知された駆動力ｆa、装置位置ｘ、装置速度ｘ’、装置加速度ｘ”、慣性行列の推定値<Ｍa>、粘性行列の推定値<Ｄa>、及び剛性行列の推定値<Ｋa>を用いて、上式（１３）に基づき、外力推定値<ｆd>を算出する。
【００７９】
なお、慣性行列、粘性行列、剛性行列の各推定値<Ｍa>、<Ｄa>、<Ｋa>は、コンプライアンスの設計値である。コントローラ４１は、上位コントローラからの指令により、動作モードやコンプライアンスの設定値を決定する。外力推定演算部４１５は、これらの推定値<Ｍa>、<Ｄa>、<Ｋａ>として、コントローラ４１が決定したコンプライアンスの設定値を用いる。
【００８０】
外力推定演算部４１５は、算出した外力推定値<ｆd>を上位コントローラに通知する。上位コントローラは、外力推定演算部４１５から通知された外力推定値により作業状態を検出して作業結果を確認するとともに、ロボット１の動作を決定する。
【００８１】
以上のように、本実施形態においては、コントローラ４１は、リニアアクチュエータ４２により駆動軸４４及びリンク機構４３を介して駆動されるハンド３の変位と、この変位から算出した部品操作部の速度及び加速度と、リニアアクチュエータ４２及びリンク機構４３を含む機械系のインピーダンスとに基づいて、リニアアクチュエータ４２の駆動力を演算し、得られた駆動力に応じて各リニアアクチュエータを駆動する出力を生成する。
【００８２】
従って、実施形態の位置決め装置は、駆動軸４４を駆動するリニアアクチュエータ４２とリンク機構４３とが駆動軸４４によって連結され、リニアアクチュエータ４２が駆動軸４４を駆動することでリンク機構４３が駆動されて、部品操作部としてのハンド３による部品の位置決めを行うものである。
【００８３】
ここで、変位センサ４５が、リニアアクチュエータ４２とリンクとを連結する各駆動軸４４の変位を計測し、位置座標変換演算部４１４が、変位センサ３３が計測した各駆動軸４４の変位から、ハンド３の位置で表される装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を算出する。
【００８４】
多自由度インピーダンス制御演算部４１１は、駆動力座標変換演算部４１２が算出した装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”と、複数のリニアアクチュエータ４２を含む機械系のインピーダンスに基づいて、ハンド３の位置における駆動力ｆaを算出する。
【００８５】
駆動力座標変換演算部４１２は、多自由度インピーダンス制御演算部４１１が算出したハンド３の位置における駆動力から、リニアアクチュエータの駆動軸４４毎の駆動力を算出する。
【００８６】
駆動電流生成部４１３は、駆動力座標変換演算部４１２が算出した駆動軸４４毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって当該リニアアクチュエータ４２を駆動してリンク機構４３を動作させてハンド３の位置を決める。
【００８７】
上記のように、リニアアクチュエータ４２は、当該駆動軸４４を駆動し、各駆動軸４４を介してリンク機構４３を駆動するため、リンク機構４３は複数の自由度を有する。ハンド３は、このリンク機構４３によって駆動される。そのため、単一の駆動軸でハンド３を駆動する場合と比較して、ハンド３の位置をきめ細かに調整することが可能となる。
【００８８】
また、多自由度インピーダンス制御演算部４１１は、ハンド３の位置、速度及び加速度からインピーダンスを制御できる。そのため、微小な接触力を維持しながら位置決めを実行することが可能となる。
【００８９】
また、位置座標変換演算部４１４は、変位センサ４５が計測した各駆動軸４４の位置から、ハンド３の位置、速度、及び加速度を算出するため、ハンド３の位置を直接計測できない場合でも、ハンド３の位置、速度及び加速度を求めることが可能となる。
【００９０】
これにより、ハンド３の位置を直接計測できない場合でも、複数の駆動軸４４の位置からハンド３の位置を算出してハンド３における駆動力を算出することが可能となり、多軸の位置決め装置において、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００９１】
また、多自由度インピーダンス制御演算部４１１は、装置位置ｘ、装置速度ｘ’、装置加速度ｘ”の各フィードバックゲインＭc、Ｄc、Ｋcに装置位置ｘ、装置速度ｘ’、装置加速度ｘ”を乗算して得られる値と、任意に設定した外力の値とから、ハンド３の位置における駆動力を算出する。
【００９２】
すなわち、多自由度インピーダンス制御演算部４１１は、リンク機構４３とリニアアクチュエータ４２の剛性、粘性及び慣性を装置の先端の直交座標で表した剛性行列、粘性行列及び慣性行列を、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”の各フィードバックゲインとして用いる。
【００９３】
これにより、ハンド３の位置を直接計測できない場合でも、複数の駆動軸４４の位置からハンド３の位置を算出してハンド３における駆動力を算出することが可能となり、多軸の位置決め装置においても、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００９４】
また、本実施形態においては、外力推定演算部４１５が、装置位置ｘ、装置速度ｘ’、装置加速度ｘ”及びハンド３の駆動力と、装置の剛性行列、粘性行列及び慣性行列の推定値とから、外力の推定値を算出して上位コントローラに通知する。
【００９５】
これにより、上位コントローラが、外力の推定値から、位置決め装置によって制御されるリンク機構４３に取り付けられたハンド３から部品に作用する重力や接触力を認識することができ、作業状態に応じた制御を行うことが可能となる。
【００９６】
以上のとおり実施形態について説明したが、本発明は上記実施形態に限定されるものではない。
【００９７】
例えば、上記実施形態においては、駆動軸を駆動するアクチュエータをリニアアクチュエータとし、リニアアクチュエータと駆動軸とを１対１で接続しているが、駆動軸を駆動するアクチュエータはリニアアクチュエータに限らず、複数の駆動軸を駆動できるものであればよい。
【００９８】
また、アクチュエータと駆動軸との接続は、１対１のみならず、１つのアクチュエータで複数の駆動軸を駆動するようにしてもよい。
【符号の説明】
【００９９】
１…ロボット、２…アーム、３…ハンド、４…位置決め機構、４１…コントローラ、４２−１，４２−２…リニアアクチュエータ、４３…リンク機構、４４−１，４４−２…駆動軸、４５−１，４５−２…変位センサ、４１１…多自由度インピーダンス制御演算部、４１２…駆動力座標変換演算部、４１３…駆動電流生成部、４１４…位置座標変換演算部、４１５…外力推定演算部。
【技術分野】
【０００１】
本発明は、例えば、ロボットにより半導体部品などの微小部品や精密部品を実装する作業において、それらの部品を精密に位置決めするための位置決め装置に関する。
【背景技術】
【０００２】
半導体部品やＭＥＭＳ（Micro Electro Mechanical Systems）技術によって製造される精密部品を組み立てる作業時には、部品にあわせて精密な位置決めが必要であるとともに、部品の接触を正確に検知して、接触力が過大にならないように位置決めを制御することが必要である。
【０００３】
このような高精度の制御を行う装置の一例として、半導体素子を基板に実装するときに加圧するアームを、平行板ばねで構成した弾性案内に取り付け、ボイスコイルモータで加圧力を与えるように構成し、ボイスコイルモータのコイルに流す電流を電流調節器で調節することによって加圧力を制御するようにしたものが知られている（下記特許文献１）。
【０００４】
また、ロボットの手首に取り付けた力センサからの信号をサーボ制御系にフィードバックするものが知られている（下記特許文献２）。これは、ロボットの作業中、力をモニタリングすることによって部品位置の認識と作業の成否確認を行う一方、力情報を制御系にフィードバックすることによってコンプライアンス制御を実現し、作業の信頼性を向上させるようにしたものである。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平７−８６３１７号公報
【特許文献２】特開平７−２４６６５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、従来の位置決め制御の技術には、次のような問題があった。
【０００７】
上記特許文献１の技術では、静止時には精密は加圧力を加えることができるが、部品搬送時の振動を抑えることや押し付けながら移動する作業で接触力を精密に制御することは難しい。また、接触を検知するためには、別に接触センサを付加する必要がある。
【０００８】
上記特許文献２の技術では、アームの質量や減速器の摩擦力の影響により、微小な接触力を維持しながら作業を行うことは難しい。
【０００９】
また、特許文献１、２のいずれの技術においても、ロボットの先のハンドや冶具の質量の影響により、位置が変化したり接触力が過大になったりする。そのような過大な質量による衝突を回避するためには、作業速度を遅くして作業を慎重に行わなければならず、それだけ作業時間が増大してしまう。
【００１０】
更に、特許文献１、２のいずれの技術も、単一軸の構造であるため、例えば、軸挿入作業やすみ肉半田付けなどの複雑な組立作業を行うには、複雑な調整作業が必要となり、安定した制御を行うことは難しい。
【００１１】
具体的には、単一軸の構造で軸挿入作業を制御するためには、Ｘ−Ｙ軸方向の並進とＸ−Ｙ軸周りの回転方向の調整が必要とされる。また、単一軸構造で、すみ肉半田付けを制御するには、稜線と直交する２つの並進方向の制御が必要とされる。単一軸構造で、このような制御を安定して行うことは難しい。
【００１２】
本発明は、上記の問題を解決するものとして、精密又は微小な部品を多軸構造で位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる位置決め装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
本発明は、部品を精密に位置決めするための位置決め装置であって、複数の駆動軸を駆動するアクチュエータと、前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部と、前記部品操作部の変位を計測する計測手段と、前記計測手段で計測された変位と該変位から算出した速度及び加速度と前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成するコントローラとを備えたことを特徴とする。
【００１４】
本発明の位置決め装置において、コントローラは、複数の駆動軸に連結されてアクチュエータにより駆動されるリンク機構を介して駆動される部品操作部の変位と、この変位から算出した速度及び加速度と、アクチュエータ及びリンク機構を含む機械系のインピーダンスとに基づいて、アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成する。
【００１５】
この出力に応じて、アクチュエータは、複数の駆動軸を駆動し、各駆動軸を介してリンク機構を駆動するため、リンク機構は複数の自由度を有する。部品操作部は、このリンク機構によって駆動される。これにより、単一の駆動軸で部品操作部を駆動する場合と比較して、部品操作部の位置をきめ細かく調整することが可能となる。
【００１６】
また、コントローラは、部品操作部の位置、速度及び加速度から機械系のインピーダンスを制御することができる。このため、微小な接触力を維持しながら位置決めを実行することが可能となる。
【００１７】
従って、本発明の位置決め装置によれば、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００１８】
上記の発明において、前記計測手段は、前記複数の駆動軸にそれぞれ取り付けられ、各駆動軸の変位を計測する変位センサと、前記変位センサが計測した各駆動軸の変位から、前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を算出する位置座標変換演算手段とを備え、前記コントローラは、前記位置座標変換演算手段によって算出された部品操作部の位置、部品操作部の速度、及び部品操作部の加速度と、前記複数のアクチュエータを含む機械系のインピーダンスに基づいて、接触位置における駆動力を算出する多自由度インピーダンス制御演算手段と、前記多自由度インピーダンス制御演算手段によって算出された部品操作部における駆動力から、前記アクチュエータの駆動軸毎の駆動力を算出する駆動力座標変換演算手段と、前記駆動力座標変換演算手段が算出した駆動軸毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって、当該アクチュエータを駆動してリンク機構を動作させる駆動電流生成手段とを備えることが好ましい。
【００１９】
この態様によれば、変位センサが、各駆動軸の変位を計測し、位置座標変換演算手段が、変位センサが計測した各駆動軸の変位から、部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を算出する。多自由度インピーダンス制御演算手段が、位置座標変換演算手段が算出した部品操作部の位置、部品操作部の速度、及び部品操作部の加速度と、複数のリニアアクチュエータを含む機械系のインピーダンスに基づいて、部品操作部における駆動力を算出する。駆動力座標変換演算手段が、多自由度インピーダンス制御演算手段が算出した部品操作部における駆動力から、リニアアクチュエータの駆動軸毎の駆動力を算出する。駆動電流生成手段が、駆動力座標変換演算手段が算出した駆動軸毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって当該リニアアクチュエータを駆動してリンク機構を動作させる。
【００２０】
これにより、部品操作部の位置を直接計測できない場合でも、複数の駆動軸の位置から部品操作部の位置を算出して部品操作部における駆動力を算出することが可能となり、多軸の位置決め制御装置において、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００２１】
上記の発明において、前記多自由度インピーダンス制御演算手段は、前記インピーダンスを表す部品操作部の位置、部品操作部の速度、部品操作部の加速度の各フィードバックゲインに前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を乗算して得られる値と、任意に設定した外力の値とから部品操作部における駆動力を算出することが好ましい。
【００２２】
この態様によれば、多自由度インピーダンス制御演算手段は、インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインに部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を乗算して得られる値と、任意に設定した外力の値とから部品操作部における駆動力を算出するようにしている。
【００２３】
これにより、多軸の位置決め装置においても、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００２４】
上記の発明において、請求項３記載の位置決め装置において、前記多自由度インピーダンス制御演算手段は、前記リンク機構及びアクチュエータの剛性、粘性、及び慣性を装置の先端の直交座標で表した剛性行列、粘性行列、及び慣性行列を、前記インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインとして用いることが好ましい。
【００２５】
この態様によれば、多自由度インピーダンス制御演算手段は、リンク機構及びリニアアクチュエータの剛性、粘性、及び慣性を部品操作部の直交座標で表した剛性行列、粘性行列、及び慣性行列を、インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインとして用いる。
【００２６】
これにより、部品操作部の変位を直接計測できない場合でも、複数の駆動軸の変位から部品操作部の位置を算出して部品操作部における駆動力を算出することが可能となり、多軸の位置決め装置においても、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００２７】
上記の発明において、前記コントローラは、前記部品操作部の位置、部品操作部の速度、部品操作部の加速度、及び部品操作部の駆動力と、自装置の剛性行列、粘性行列、及び慣性行列の推定値とから、外力の推定値を算出して外部に通知する外力推定演算手段を更に備えることが好ましい。
【００２８】
この態様によれば、外力推定演算手段が、部品操作部の位置、部品操作部の速度、部品操作部の加速度、及び部品操作部の駆動力と、自装置の剛性行列、粘性行列、及び慣性行列の推定値とから、外力の推定値を算出して外部に通知するようにしている。
【００２９】
これにより、位置決め装置の上位コントローラが、外力の推定値から、部品操作部から部品への重力や接触力を認識可能となり、作業状態に応じた制御を行うことが可能となる。
【００３０】
本発明によれば、複数の駆動軸を駆動するアクチュエータと、前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部とを有し、前記アクチュエータを駆動して部品を精密に位置決めする位置決め装置の制御方法であって、前記部品操作部の変位を計測する計測ステップと、前記計測手段で計測された変位と、該変位から算出した速度及び加速度と、前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成する制御ステップとを含むことを特徴とする。
【００３１】
本発明によれば、複数の駆動軸に連結されてアクチュエータによって駆動されるリンク機構を介して駆動される部品操作部の変位と、この変位から算出した部品操作部の速度及び加速度と、アクチュエータ及びリンク機構を含む機械系のインピーダンスとに基づいて、アクチュエータの駆動力を演算し、演算した駆動力に応じて各アクチュエータを駆動する出力を生成するようにしている。
【００３２】
これにより、多軸の位置決め装置において、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【図面の簡単な説明】
【００３３】
【図１】ロボットのアームの先端に位置決め装置を取り付けた状態を示す外観図。
【図２】位置決め装置の一構成例を示す模式図。
【図３】位置決め装置の力学モデルの説明図。
【図４】位置決め装置の制御系を示すブロック図。
【発明を実施するための形態】
【００３４】
図１に示すように、本発明の実施形態の位置決め装置は、ロボット１のアーム２の先端に取り付けられる位置決め機構４と部品を把持するハンド３とを備えて構成される。ハンド３は、本発明における部品操作部に相当する。位置決め機構４は、ハンド３にコンプライアンスを与える。
【００３５】
位置決め機構４は、図２に示すように、ロボット１のアーム２に取り付けられるｎ（ｎは２以上の自然数で、本実施形態ではｎ＝２）個のリニアアクチュエータ４２（以下、２つのアクチュエータを４２−１，４２−２で示す）と、ハンド３に取り付けられ、リニアアクチュエータ４２によって駆動されるリンク機構４３と、リニアアクチュエータ４２の作動を制御するコントローラ４１（図４）とを備えている。
【００３６】
リニアアクチュエータ４２は、例えば、サーボモータ、ボイスコイルモータ（ＶＣＭ）或いはシリンダ機構などで構成される。リンク機構４３は、複数の自由度を有し、２つの駆動軸４４（以下、２つの駆動軸を４４−１，４４−２で示す）によってリニアアクチュエータ４２と接続される。
【００３７】
図２において、リンク機構４３は、連結点ａにてリンク４３１の一端とリンク４３４の一端が接続され、連結点ｂにてリンク４３１の他端とリンク４３２の一端が接続され、連結点ｃにてリンク４３２の他端とリンク４３３の一端が接続され、連結点ｄにてリンク４３３の他端とリンク４３４の他端が接続されてなる平行リンクと、この平行リンクと連結点ｄにて接続されるリンク４３５とを備えている。
【００３８】
平行リンクは、連結点ａにおいて第１リニアアクチュエータ４２−１によって駆動される第１駆動軸４４−１と接続される。また、連結点ｄにおいて一端が平行リンクと接続されたリンク４３５の他端は、点ｅにおいて、第２リニアアクチュエータ４２−２によって駆動される第２駆動軸４４−２に接続される。また、平行リンクのリンク４３３がハンド３と接続される。従って、リンク機構４３は、Ｘ−Ｚ軸に対して２自由度を有し、２個のリニアアクチュエータ４２−１，４２−２で駆動される。
【００３９】
位置決め装置の制御系の動作は、下記の力学モデルに基づくコントローラ４１での演算処理によって実行されるものであり、以下これについて説明する。
【００４０】
図３において、位置決め装置のハンド３の位置を表す座標ｘの右方向を正（＋）とすると、位置決め装置の運動方程式は、下記の式（１）のように表される。
【００４１】
ｆa＋ｆd＝Ｍaｘ”＋Ｄaｘ’＋Ｋaｘ …（１）
但し、ｘ’、ｘ”は位置決め装置の速度、加速度を表す。
【００４２】
Ｍa：位置決め装置の慣性行列（ｎ×ｎ）
Ｄa：位置決め装置の粘性行列（ｎ×ｎ）
Ｋa：位置決め装置の剛性行列（ｎ×ｎ）
ｎ：リンク機構４３の自由度
ｆa：位置決め装置が発生する駆動力
ｆd：外力（重力、接触力など）
なお、上記慣性行列、粘性行列及び剛性行列は、リンク機構４３及びリニアアクチュエータ４２の慣性、粘性及び剛性を、位置決め装置の先端部位（ハンド３の位置）の直交座標で表したものである。
【００４３】
上記駆動力ｆaは、次式で表される。
【００４４】
ｆa＝−Ｍcｘ”−Ｄcｘ’−Ｋcｘ＋ｆc …（２）
ここで、ｆcは重力相殺や作業のために必要な力、Ｍc、Ｄc、Ｋcは加速度、速度、位置の各フィードバックゲインであり、それぞれ任意に決められるパラメータである。
【００４５】
以下、位置決め装置の先端の位置ｘ、速度ｘ’、加速度ｘ”をそれぞれ装置位置、装置速度、装置加速度と呼ぶことがある。
【００４６】
上式（１）及び（２）から、
ｆd＝Ｍvｘ”＋Ｄvｘ’＋Ｋvｘ−ｆc …（３）
が導かれる。但し、Ｍv、Ｄv、Ｋvは、下記の式（４）〜（６）で表される力学モデルのインピーダンスである。
【００４７】
Ｍv＝Ｍa＋Ｍc …（４）
Ｄv＝Ｄa＋Ｄc …（５）
Ｋv＝Ｋa＋Ｋc …（６）
従って、パラメータＭc、Ｄc、Ｋcをそれぞれ適当に設定することにより、外力に対する機械系の応答を決めるインピーダンス（Ｍv、Ｄv、Ｋv）を定めることができる。すなわち、コントローラ４１により、機械系のインピーダンスを制御することができる。
【００４８】
なお、駆動力ｆaは、式（２）において加速度フィードバック（−Ｍcｘ”）や速度フィードバック（Ｄcｘ’）の項を省略し、次式のようにしてもよい。
【００４９】
ｆa＝−Ｋcｘ＋ｆc …（７）
上式（７）の場合は、機械系が本来持っている慣性及び粘性での応答となる。
【００５０】
また、リンク機構４３では、位置決め装置の先端部（この場合、ハンド３）にセンサを取り付けることが困難な場合があり、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を直接計測できないことがある。そのような場合は、各軸のリニアアクチュエータの先端にそれぞれセンサを取り付けて、リニアアクチュエータの位置（角度）から、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を求めるようにしてもよい。
【００５１】
一般に、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”は、軸位置ｑを用いて、下記の式（８）〜式（１０）で表される。
【００５２】
ｘ＝Ｔ（ｑ） …（８）
ｘ’＝Ｊｑ’ …（９）
ｘ”＝Ｊ’ｑ’＋Ｊｑ” …（１０）
但し、
Ｔ：位置変換式
ｑ：各軸の位置（角度）
ｑ’：各軸の速度
ｑ”：各軸の加速度
Ｊ：ヤコビ行列
Ｊ’：Ｊの時間微分。
【００５３】
図４は、図２に示した位置決め機構４の制御装置（コントローラ）を含む機能ブロック図である。
【００５４】
本実施形態の制御系は、Ｘ−Ｚ軸に対してｎ＝２自由度を有するリンク機構４３と、多自由度インピーダンス制御演算部４１１、駆動力座標変換演算部４１２、駆動電流生成部４１３、位置座標変換演算部４１４及び外力推定演算部４１５を有するコントローラ４１と、コントローラ４１によって制御されてリンク機構４３を駆動する２つのリニアアクチュエータ４２−１，４２−２と、各リニアアクチュエータに取り付けられて各リニアアクチュエータの位置（変位）を計測する変位センサ４５−１，４５−２とを含んでいる。変位センサ４５としては、例えば、接近センサ、ひずみケージ、リニアエンコーダなどが用いられる。
【００５５】
なお、変位センサ４５と位置座標変換演算部４１４とで、本発明における計測手段を構成している。
【００５６】
前述したように、実施形態は、自由度ｎ＝２の位置決め装置であるため、２個のリニアアクチュエータ４２−１，４２−１を備え、各リニアアクチュエータが各々連結した駆動軸４４−１，４４−２を介してリンク機構４３を駆動する。ｎ＝３の場合は、リニアアクチュエータ、変位センサ及び駆動軸の組は３つとなる。すなわち、位置決め装置は、少なくとも自由度ｎに相当する個数のリニアアクチュエータ及び駆動軸を有するものとなる。
【００５７】
変位センサ４５は、リニアアクチュエータ４２に取り付けられ、当該リニアアクチュエータ４２の先端の位置（角度）ｑを計測する。図示の例では、２つの変位センサ４５−１，４５−２が、リニアアクチュエータ４２−１，４２−２に取り付けられている。以下、変位センサ４５が計測するリニアアクチュエータ４２の先端の位置を、駆動軸の位置ということがある。
【００５８】
位置座標変換演算部４１４は、上式（８）〜（１０）を用いて、変位センサ４５が計測した各軸のリニアアクチュエータ４２の位置（各駆動軸の位置）ｑから装置位置ｘ、装置速度ｘ’、及び装置加速度ｘ”を算出する。
【００５９】
多自由度インピーダンス制御演算部４１１は、上記位置座標変換演算部４１４によって算出された装置位置ｘ、装置速度ｘ’、装置加速度ｘ”と、上位コントローラ（この場合、ロボット１のコントローラ）から入力されるパラメータ情報に基づいて、位置決め装置が発生すべき駆動力を求める。ここで、パラメータ情報とは、上式（１）〜（６）による演算に必要な情報であり、重力相殺や作業のために必要な力ｆcと、フィードバックゲインを表すパラメータＭc、Ｄc、Ｋcである。
【００６０】
駆動力座標変換演算部４１２は、上記多自由度インピーダンス制御演算部４１１によって求められた駆動力を、リニアアクチュエータ４２の駆動力に変換する。多自由度インピーダンス制御演算部４１１が求める駆動力は、位置決め装置が発生すべき駆動力、すなわち作業座標（装置位置ｘの座標）における駆動力である。
【００６１】
従って、駆動力座標変換演算部４１２は、作業座標の駆動力を、リニアアクチュエータ４２毎の駆動力に変換する。更に、駆動力座標変換演算部４１２は、リニアアクチュエータ４２毎の駆動力を、それぞれ当該リニアアクチュエータ４２を駆動する電流の値を示す電流指示値に変換する。
【００６２】
リニアアクチュエータ毎の駆動力は、次式によって表される。
【００６３】
τ＝ＪＴｆa …（１１）
但し、
τ：各駆動軸の駆動力
Ｊ：ヤコビ行列（ｘ’＝Ｊｑ’）
ＪＴ：Ｊの転置行列
ｑ：各駆動軸の位置（角度）。
【００６４】
駆動力座標変換演算部４１２は、上式（１０）を用いてリニアアクチュエータ毎の駆動力を求め、求めた駆動力を電流指示値に変換する。
【００６５】
駆動電流生成部４１３は、駆動力座標変換演算部４１２によって変換されたリニアアクチュエータ毎の電流指示値に応じた駆動電流をそれぞれ生成し、この駆動電流を当該リニアアクチュエータに供給してリニアアクチュエータを駆動する。これにより、リニアアクチュエータ４２がリンク機構４３を動作させて位置決めが行われる。
【００６６】
外力推定演算部４１５は、上記多自由度インピーダンス制御演算部４１１によって求められた駆動力と、位置座標変換演算部４１４によって算出された装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”に基づいて、外力を推定する。
【００６７】
外力は、上式（１）より
ｆd＝Ｍaｘ”＋Ｄaｘ’＋Ｋaｘ−ｆa …（１２）
で表される。
【００６８】
しかし、実際上、位置決め装置の慣性行列Ｍa、粘性行列Ｄa及び剛性行列Ｋaの真の値はわからないので、上式（１２）において慣性行列Ｍa、粘性行列Ｄa及び剛性行列Ｋaの値には推定値を用いる。
【００６９】
これにより、外力の推定値<ｆd>は、次式（１３）で表される。
【００７０】
<ｆd>＝<Ｍa>ｘ”＋<Ｄa>ｘ’＋<Ｋa>ｘ−ｆa …（１３）
但し、
<Ma>：慣性行列の推定値
<Ｄa>：粘性行列の推定値
<Ｋa>：剛性行列の推定値
外力推定演算部４１５は、上式（１３）を用いて外力推定値を求める。
【００７１】
次に、本実施形態の制御系の動作を説明する。
【００７２】
まず、変位センサ４５によりリニアアクチュエータ４２の位置が計測される。変位センサ４５は、計測したリニアアクチュエータ４２の位置を位置座標変換演算部４１４に通知する。
【００７３】
位置座標変換演算部４１４は、変位センサ４５が計測した各軸のリニアアクチュエータ４２の位置と上式（８）〜（１０）を用いて、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を算出する。位置座標変換演算部４１４は、算出した装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を、多自由度インピーダンス制御演算部４１１及び外力推定演算部４１５に通知する。
【００７４】
多自由度インピーダンス制御演算部４１１は、通知された装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”と、上位コントローラから入力されるパラメータ情報（重力相殺や作業のために必要な力ｆc、及びパラメータMc、Dc、Ｋc）とを用いて、上式（２）に基づき位置決め装置が発生すべき駆動力ｆa（ｎ次元ベクトル）を算出する。そして、算出した駆動力を駆動力座標変換演算部４１２及び外力推定演算部４１５に通知する。
【００７５】
駆動力座標変換演算部４１２は、通知された駆動力ｆa、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を用いて、上式（１１）により、各リニアアクチュエータの駆動力τ（軸駆動力ベクトル）を算出する。そして、算出した各リニアアクチュエータの駆動力τを、当該リニアアクチュエータを駆動する電流の値を示す電流指示値に変換する。駆動力座標変換演算部４１２は、電流指示値を当該リニアアクチュエータ４２に通知する。
【００７６】
なお、駆動力τから電流指示値への変換は、予め定められた式に基づいて行ってもよく、或いは、予め駆動力と電流指示値とを対応付けた変換テーブルを作成しておき、この変換テーブルを用いるようにしてもよい。
【００７７】
駆動電流生成部４１３は、通知された電流指示値に応じた駆動電流を生成し、当該リニアアクチュエータ４２に出力（供給）する。これにより、リニアアクチュエータ４２は、駆動力座標変換演算部４１２によって算出された駆動力に応じてリンク機構４３を動作させ、ハンド３の位置決めを行う。
【００７８】
一方、外力推定演算部４１５は、通知された駆動力ｆa、装置位置ｘ、装置速度ｘ’、装置加速度ｘ”、慣性行列の推定値<Ｍa>、粘性行列の推定値<Ｄa>、及び剛性行列の推定値<Ｋa>を用いて、上式（１３）に基づき、外力推定値<ｆd>を算出する。
【００７９】
なお、慣性行列、粘性行列、剛性行列の各推定値<Ｍa>、<Ｄa>、<Ｋa>は、コンプライアンスの設計値である。コントローラ４１は、上位コントローラからの指令により、動作モードやコンプライアンスの設定値を決定する。外力推定演算部４１５は、これらの推定値<Ｍa>、<Ｄa>、<Ｋａ>として、コントローラ４１が決定したコンプライアンスの設定値を用いる。
【００８０】
外力推定演算部４１５は、算出した外力推定値<ｆd>を上位コントローラに通知する。上位コントローラは、外力推定演算部４１５から通知された外力推定値により作業状態を検出して作業結果を確認するとともに、ロボット１の動作を決定する。
【００８１】
以上のように、本実施形態においては、コントローラ４１は、リニアアクチュエータ４２により駆動軸４４及びリンク機構４３を介して駆動されるハンド３の変位と、この変位から算出した部品操作部の速度及び加速度と、リニアアクチュエータ４２及びリンク機構４３を含む機械系のインピーダンスとに基づいて、リニアアクチュエータ４２の駆動力を演算し、得られた駆動力に応じて各リニアアクチュエータを駆動する出力を生成する。
【００８２】
従って、実施形態の位置決め装置は、駆動軸４４を駆動するリニアアクチュエータ４２とリンク機構４３とが駆動軸４４によって連結され、リニアアクチュエータ４２が駆動軸４４を駆動することでリンク機構４３が駆動されて、部品操作部としてのハンド３による部品の位置決めを行うものである。
【００８３】
ここで、変位センサ４５が、リニアアクチュエータ４２とリンクとを連結する各駆動軸４４の変位を計測し、位置座標変換演算部４１４が、変位センサ３３が計測した各駆動軸４４の変位から、ハンド３の位置で表される装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”を算出する。
【００８４】
多自由度インピーダンス制御演算部４１１は、駆動力座標変換演算部４１２が算出した装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”と、複数のリニアアクチュエータ４２を含む機械系のインピーダンスに基づいて、ハンド３の位置における駆動力ｆaを算出する。
【００８５】
駆動力座標変換演算部４１２は、多自由度インピーダンス制御演算部４１１が算出したハンド３の位置における駆動力から、リニアアクチュエータの駆動軸４４毎の駆動力を算出する。
【００８６】
駆動電流生成部４１３は、駆動力座標変換演算部４１２が算出した駆動軸４４毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって当該リニアアクチュエータ４２を駆動してリンク機構４３を動作させてハンド３の位置を決める。
【００８７】
上記のように、リニアアクチュエータ４２は、当該駆動軸４４を駆動し、各駆動軸４４を介してリンク機構４３を駆動するため、リンク機構４３は複数の自由度を有する。ハンド３は、このリンク機構４３によって駆動される。そのため、単一の駆動軸でハンド３を駆動する場合と比較して、ハンド３の位置をきめ細かに調整することが可能となる。
【００８８】
また、多自由度インピーダンス制御演算部４１１は、ハンド３の位置、速度及び加速度からインピーダンスを制御できる。そのため、微小な接触力を維持しながら位置決めを実行することが可能となる。
【００８９】
また、位置座標変換演算部４１４は、変位センサ４５が計測した各駆動軸４４の位置から、ハンド３の位置、速度、及び加速度を算出するため、ハンド３の位置を直接計測できない場合でも、ハンド３の位置、速度及び加速度を求めることが可能となる。
【００９０】
これにより、ハンド３の位置を直接計測できない場合でも、複数の駆動軸４４の位置からハンド３の位置を算出してハンド３における駆動力を算出することが可能となり、多軸の位置決め装置において、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００９１】
また、多自由度インピーダンス制御演算部４１１は、装置位置ｘ、装置速度ｘ’、装置加速度ｘ”の各フィードバックゲインＭc、Ｄc、Ｋcに装置位置ｘ、装置速度ｘ’、装置加速度ｘ”を乗算して得られる値と、任意に設定した外力の値とから、ハンド３の位置における駆動力を算出する。
【００９２】
すなわち、多自由度インピーダンス制御演算部４１１は、リンク機構４３とリニアアクチュエータ４２の剛性、粘性及び慣性を装置の先端の直交座標で表した剛性行列、粘性行列及び慣性行列を、装置位置ｘ、装置速度ｘ’及び装置加速度ｘ”の各フィードバックゲインとして用いる。
【００９３】
これにより、ハンド３の位置を直接計測できない場合でも、複数の駆動軸４４の位置からハンド３の位置を算出してハンド３における駆動力を算出することが可能となり、多軸の位置決め装置においても、精密又は微小な部品を位置決めしながら組立作業を行うロボットの接触力や位置・姿勢を、組立作業に応じて調整することができる。
【００９４】
また、本実施形態においては、外力推定演算部４１５が、装置位置ｘ、装置速度ｘ’、装置加速度ｘ”及びハンド３の駆動力と、装置の剛性行列、粘性行列及び慣性行列の推定値とから、外力の推定値を算出して上位コントローラに通知する。
【００９５】
これにより、上位コントローラが、外力の推定値から、位置決め装置によって制御されるリンク機構４３に取り付けられたハンド３から部品に作用する重力や接触力を認識することができ、作業状態に応じた制御を行うことが可能となる。
【００９６】
以上のとおり実施形態について説明したが、本発明は上記実施形態に限定されるものではない。
【００９７】
例えば、上記実施形態においては、駆動軸を駆動するアクチュエータをリニアアクチュエータとし、リニアアクチュエータと駆動軸とを１対１で接続しているが、駆動軸を駆動するアクチュエータはリニアアクチュエータに限らず、複数の駆動軸を駆動できるものであればよい。
【００９８】
また、アクチュエータと駆動軸との接続は、１対１のみならず、１つのアクチュエータで複数の駆動軸を駆動するようにしてもよい。
【符号の説明】
【００９９】
１…ロボット、２…アーム、３…ハンド、４…位置決め機構、４１…コントローラ、４２−１，４２−２…リニアアクチュエータ、４３…リンク機構、４４−１，４４−２…駆動軸、４５−１，４５−２…変位センサ、４１１…多自由度インピーダンス制御演算部、４１２…駆動力座標変換演算部、４１３…駆動電流生成部、４１４…位置座標変換演算部、４１５…外力推定演算部。
【特許請求の範囲】
【請求項１】
部品を精密に位置決めするための位置決め装置であって、
複数の駆動軸を駆動するアクチュエータと、
前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、
前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部と、
前記部品操作部の変位を計測する計測手段と、
前記計測手段で計測された変位と該変位から算出した速度及び加速度と前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成するコントローラとを備えたことを特徴とする位置決め装置。
【請求項２】
請求項１記載の位置決め装置において、
前記計測手段は、
前記複数の駆動軸にそれぞれ取り付けられ、各駆動軸の変位を計測する変位センサと、
前記変位センサが計測した各駆動軸の変位から、前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を算出する位置座標変換演算手段とを備え、
前記コントローラは、
前記位置座標変換演算手段によって算出された部品操作部の位置、部品操作部の速度、及び部品操作部の加速度と、前記複数のアクチュエータを含む機械系のインピーダンスに基づいて、接触位置における駆動力を算出する多自由度インピーダンス制御演算手段と、
前記多自由度インピーダンス制御演算手段によって算出された部品操作部における駆動力から、前記アクチュエータの駆動軸毎の駆動力を算出する駆動力座標変換演算手段と、
前記駆動力座標変換演算手段が算出した駆動軸毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって、当該アクチュエータを駆動してリンク機構を動作させる駆動電流生成手段とを備えることを特徴とする位置決め装置。
【請求項３】
請求項２記載の位置決め装置において、
前記多自由度インピーダンス制御演算手段は、
前記インピーダンスを表す部品操作部の位置、部品操作部の速度、部品操作部の加速度の各フィードバックゲインに前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を乗算して得られる値と、任意に設定した外力の値とから部品操作部における駆動力を算出することを特徴とする位置決め装置。
【請求項４】
請求項３記載の位置決め装置において、
前記多自由度インピーダンス制御演算手段は、
前記リンク機構及びアクチュエータの剛性、粘性、及び慣性を装置の先端の直交座標で表した剛性行列、粘性行列、及び慣性行列を、前記インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインとして用いることを特徴とする位置決め装置。
【請求項５】
請求項１〜４の何れか１項記載の位置決め装置において、
前記コントローラは、
前記部品操作部の位置、部品操作部の速度、部品操作部の加速度、及び部品操作部の駆動力と、自装置の剛性行列、粘性行列、及び慣性行列の推定値とから、外力の推定値を算出して外部に通知する外力推定演算手段を更に備えることを特徴とする位置決め装置。
【請求項６】
複数の駆動軸を駆動するアクチュエータと、
前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、
前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部とを有し、前記アクチュエータを駆動して部品を精密に位置決めする位置決め装置の制御方法であって、
前記部品操作部の変位を計測する計測ステップと、
前記計測手段で計測された変位と、該変位から算出した速度及び加速度と前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成する制御ステップとを含むことを特徴とする位置決め装置の制御方法。
【請求項１】
部品を精密に位置決めするための位置決め装置であって、
複数の駆動軸を駆動するアクチュエータと、
前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、
前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部と、
前記部品操作部の変位を計測する計測手段と、
前記計測手段で計測された変位と該変位から算出した速度及び加速度と前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成するコントローラとを備えたことを特徴とする位置決め装置。
【請求項２】
請求項１記載の位置決め装置において、
前記計測手段は、
前記複数の駆動軸にそれぞれ取り付けられ、各駆動軸の変位を計測する変位センサと、
前記変位センサが計測した各駆動軸の変位から、前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を算出する位置座標変換演算手段とを備え、
前記コントローラは、
前記位置座標変換演算手段によって算出された部品操作部の位置、部品操作部の速度、及び部品操作部の加速度と、前記複数のアクチュエータを含む機械系のインピーダンスに基づいて、接触位置における駆動力を算出する多自由度インピーダンス制御演算手段と、
前記多自由度インピーダンス制御演算手段によって算出された部品操作部における駆動力から、前記アクチュエータの駆動軸毎の駆動力を算出する駆動力座標変換演算手段と、
前記駆動力座標変換演算手段が算出した駆動軸毎の駆動力に応じた駆動電流を生成し、生成した駆動電流によって、当該アクチュエータを駆動してリンク機構を動作させる駆動電流生成手段とを備えることを特徴とする位置決め装置。
【請求項３】
請求項２記載の位置決め装置において、
前記多自由度インピーダンス制御演算手段は、
前記インピーダンスを表す部品操作部の位置、部品操作部の速度、部品操作部の加速度の各フィードバックゲインに前記部品操作部の位置、部品操作部の速度、及び部品操作部の加速度を乗算して得られる値と、任意に設定した外力の値とから部品操作部における駆動力を算出することを特徴とする位置決め装置。
【請求項４】
請求項３記載の位置決め装置において、
前記多自由度インピーダンス制御演算手段は、
前記リンク機構及びアクチュエータの剛性、粘性、及び慣性を装置の先端の直交座標で表した剛性行列、粘性行列、及び慣性行列を、前記インピーダンスを表す部品操作部の位置、部品操作部の速度、及び部品操作部の加速度の各フィードバックゲインとして用いることを特徴とする位置決め装置。
【請求項５】
請求項１〜４の何れか１項記載の位置決め装置において、
前記コントローラは、
前記部品操作部の位置、部品操作部の速度、部品操作部の加速度、及び部品操作部の駆動力と、自装置の剛性行列、粘性行列、及び慣性行列の推定値とから、外力の推定値を算出して外部に通知する外力推定演算手段を更に備えることを特徴とする位置決め装置。
【請求項６】
複数の駆動軸を駆動するアクチュエータと、
前記複数の駆動軸に連結されて前記アクチュエータにより駆動されるリンク機構と、
前記リンク機構を介して駆動されて前記部品の位置決めを行う部品操作部とを有し、前記アクチュエータを駆動して部品を精密に位置決めする位置決め装置の制御方法であって、
前記部品操作部の変位を計測する計測ステップと、
前記計測手段で計測された変位と、該変位から算出した速度及び加速度と前記アクチュエータ及び前記リンク機構を含む機械系のインピーダンスとに基づいて、前記アクチュエータの駆動力を演算し、演算した駆動力に応じてアクチュエータを駆動する出力を生成する制御ステップとを含むことを特徴とする位置決め装置の制御方法。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２０１０−２２１３２２（Ｐ２０１０−２２１３２２Ａ）
【公開日】平成２２年１０月７日（２０１０．１０．７）
【国際特許分類】
【出願番号】特願２００９−６９９６９（Ｐ２００９−６９９６９）
【出願日】平成２１年３月２３日（２００９．３．２３）
【出願人】（０００００６６６６）株式会社山武 (1,808)
【Ｆターム（参考）】
【公開日】平成２２年１０月７日（２０１０．１０．７）
【国際特許分類】
【出願日】平成２１年３月２３日（２００９．３．２３）
【出願人】（０００００６６６６）株式会社山武 (1,808)
【Ｆターム（参考）】
[ Back to top ]