JP3694790B2 - Calibration method of parallel mechanism - Google Patents

Description

【００１７】
図２から以下のように、パラレルメカニズムの幾何学的関係がモデル化される。まず、ベース座標系におけるリンク体とエンドプレートとの接点の位置ベクトルは、
【００１８】
【数１】

【００１９】
と２つの方向から表すことができるので、これらから以下の関係が成り立つ。
【００２０】
【数２】

【００２１】
同様にリンク５ａ，５ｂの接点を２つの方向から表し、等号で結ぶと、以下の関係が成り立つ。
【００２２】
【数３】

【００２３】
これら式（１），（２）がデルタ型パラレルメカニズムの機構の非線形連立方程式となる。
【００２４】
次に、上記非線形連立方程式（１），（２）を全微分し、エンドプレートの位置・姿勢Ｘ、リンク変位θ、機構パラメータＰそれぞれの微小変位δＸ，δθ，δＰの関係を求める。つまり、式（１），（２）の二式を両辺とも全微分することで、次のような線形連立方程式を得ることができる。
【００２５】
【数４】

【００２６】
このように、全微分して線形の関係式を求めることで、微小変位δＸ，δθ，δＰの関係を一度に解析することが可能となる。
【００２７】
次に、本実施形態のパラレルメカニズムの運動学について説明する。この運動学とは、逆運動学、微小運動学、順運動学をいう。まず、逆運動学について説明する。ここでいう逆運動学とは、ベース座標系におけるエンドプレート７の位置ベクトルと姿勢行列とを与えたときのリンク変位を求める計算をいう。以下、デルタ型パラレルメカニズムにおける逆運動学を導出する。式（１）において、
【００２８】
【数５】

【００２９】
とおくと、
【００３０】
【数６】

【００３１】
これより、
【００３２】
【数７】

【００３３】
と表される。同様に、式（２）において、
【００３４】
【数８】

【００３５】
とおくと、
【００３６】
【数９】

【００３７】
これより、
【００３８】
【数１０】

【００３９】
となり、逆運動学（４）（５）が得られる。
【００４０】
次に、微小運動学について説明する。微小運動学とは、リンク体５とエンドプレート７との微小変位の関係を求める計算のことである。以下、微小運動学を導出する。式（３）より、機構パラメータを定数として扱うと、
【００４１】
【数１１】

【００４２】
となる。したがって、
【００４３】
【数１２】

【００４４】
となり、次の関係式が求められる。
【００４５】
【数１３】

【００４６】
続いて、順運動学について説明する。順運動学とは、リンク変位からベース座標系におけるエンドプレート７の位置ベクトル及び姿勢行列を求める計算をいう。しかし、デルタ型パラレルメカニズムにおける順運動学は、複雑な非線形方程式となるため、極めて特殊な機構を除いては一般的には求められない。そこで、上記のように導出した逆運動学を用いて、ニュートンラプソン法の繰り返し計算により数値計算的に順運動学を求める。
【００４７】
式（６）より
【００４８】
【数１４】

【００４９】
これより、エンドプレートの位置・姿勢を繰り返し計算により求める。具体的には、以下のように計算を行う。
▲１▼ 適当な初期値ｐ₀，Ｒ₀を決める。
▲２▼ ｐ₀，Ｒ₀から逆運動学により、リンク変位の初期値θ₀を求める。
▲３▼ δθ＝θ−θ₀として、微小運動学の式（７）により、δθからδｐ₀，δＲ₀を求める。
【００５０】
【数１５】

【００５１】
▲５▼ ▲２▼に戻る。
▲２▼から▲５▼までの計算を十分微小なεに対して、
【００５２】
【数１６】

【００５３】
になるまで繰り返し、そのときのｐ_n，Ｒ_nがリンク変位θのときの位置・姿勢情報となり、順運動学が得られる。
【００５４】
次に、上記順運動学に基づくパラレルメカニズムのキャリブレーション法について説明する。一般的なキャリブレーション法では、まず、入力値として適当なリンク変位を与え、この変位に対するエンドプレートの位置・姿勢をレーザ干渉計等の計測手段により計測し、計測値を得る。このとき得られる計測値は、機構パラメータの数が３６個であることから、エンドプレートを６つの姿勢・位置にして計３６個以上の位置、姿勢を計測する必要がある。
【００５５】
そして、リンク変位から機構パラメータの初期値を用い、順運動学により位置・姿勢の計算値を得る。このとき使用される機構パラメータの初期値は、パラレルメカニズム製作用の設計値であるが、加工誤差或いは組立誤差を含んでいる。そして、計測値と計算値との差が最小になるように機構パラメータの各値を変更し、新たに変更された機構パラメータの各値を用いて、さらに順運動学により位置・姿勢を計算する。そして、計測値と計算値との差が十分に小さくなるまで、この計算を繰り返す。
【００５６】
しかしながら、微小な動作をするエンドプレートの姿勢を測定することは、非常に困難である。そこで、本実施形態では、順運動学に基づく次のアルゴリズムによりキャリブレーションを行う。
【００５７】
まず、式（３）において、リンク変位θを定数と考え、
【００５８】
【数１７】

【００５９】
これを
【００６０】
【数１８】

【００６１】
と表す。続いて、この式（９）を位置情報に相当する部分と、姿勢情報に相当する部分に分離する。つまり、
【００６２】
【数１９】

【００６３】
であるので、式（９）を上半分と下半分とに分離すればよい。このとき、
【００６４】
【数２０】

【００６５】
とすると、
【００６６】
【数２１】

【００６７】
と表すことができる。これらより、
▲１▼姿勢を測定する測定点では、以下の式（姿勢方程式）を使用し、
【００６８】
【数２２】

【００６９】
▲２▼姿勢が測定できない、或いは測定が困難な測定点では、以下の式（位置方程式）を使用する。
【００７０】
【数２３】

【００７１】
すなわち、上記式（１０）、（１１）を組み合わせ、機構パラメータの数、つまり３６個以上の計測値を得るようにする。但し、少なくとも１つの姿勢情報を計測することが必要である。これは、エンドプレートの位置が決まっても、その位置での姿勢がただ一つには決めることができないからである。例えば、エンドプレート７を適当な位置・姿勢状態にして、１箇所の姿勢情報（オイラー角θ，φ，ψ）、つまり３個の情報と、最小１１箇所の位置情報（並進変位Ｘ，Ｙ，Ｚ）、つまり３３個の情報とを計測する場合には、式（１０）、（１１）を組み合わせ、次のように関係式（組合せ連立方程式）を作ればよい。
【００７２】
【数２４】

【００７３】
ここで、
【００７４】
【数２５】

【００７５】
とすると、
【００７６】
【数２６】

【００７７】
を得る。この式に基づいて以下のニュートンラプソン法により、機構パラメータを更新していく。すなわち、
【００７８】
【数２７】

【００７９】
を用い、繰り返し計算により、
【００８０】
【数２８】

【００８１】
として機構パラメータを更新する。
【００８２】
以下、図３のフローチャートにしたがって説明すると、まず、エンドプレートの位置姿勢を変更して１箇所の姿勢情報と、１１箇所以上の位置情報を計測する（Ｓ１）。次に、上記計測時のリンク変位θから順運動学によりエンドプレート７の位置姿勢を計算する（Ｓ２）。この順運動学で使用する機構パラメータＰ_n（ｎ＝１）は、予め求められている設計値を用いる。続いて、ニュートンラプソン法により機構パラメータＰ_nを更新し（Ｓ３）、この更新された機構パラメータＰ_n+1を用いて順運動学によりエンドプレート７の位置姿勢情報を計算する（Ｓ４）。そして、位置姿勢の計算値と、計測値とを比較し（Ｓ５）、その差が所定値εより小さい場合には（Ｓ５のＹＥＳ）、計算を終了する。一方、計算値と計測値の差が最終精度を保証する所定値εより大きい場合には（Ｓ５のＮＯ）、所定値εより小さくなるまで、上記ステップ３から５を繰り返す。
【００８３】
以上のように、本実施形態によれば、パラレルメカニズムの運動学をモデル化した非線形連立方程式を微分し、得られた線形連立方程式を、エンドプレートの位置姿勢情報と機構パラメータとの関係を示す姿勢方程式、および位置情報と機構パラメータのとの関係を示す位置方程式とに分離している。そして、１個の姿勢方程式と１１個以上の位置方程式とを組み合わせてキャリブレーションを行っている。すなわち、１個の姿勢情報のみを計測してキャリブレーションを行っているため、従来困難であった姿勢情報の計測を最小限に抑えることができ、その結果、機構パラメータのキャリブレーションを容易に、かつ正確に行うことができる。
【００８４】
上記実施形態では、１個の姿勢情報のみを計測してキャリブレーションを行っているが、姿勢方程式及び位置方程式を任意に組み合わせて機構パラメータの数と同じ数の連立方程式を作ればよいため、複数個の姿勢情報を計測してキャリブレーションを行うこともできる。この場合、計測が困難な箇所を避けて計測が容易な箇所でのみ姿勢情報を計測すると、キャリブレーションが容易になる。
【００８５】
また、姿勢情報を計測する場合、次のようにすると計測がより簡単になる。すなわち、エンドプレート７に任意の姿勢状態をとらせるときに、既知の姿勢状態にエンドプレート７を保持可能な治具を使用する。これにより、エンドプレート７の姿勢状態が既知となり、このときのリンク変位のみを計測すればよいため、キャリブレーションがさらに容易になる。ここで使用される治具は、エンドプレートを既知の姿勢に保持できるものであれば、どのようなものでもよく、例えばプレートでエンドプレートを保持するもの、複数の棒状部材で保持するもの等種々のものを挙げることができる。
【００８６】
上記実施形態では、ニュートンラプソン法によりキャリブレーションを行っているが、これ以外の方法でも可能である。
【００８７】
また、上記実施形態では、デルタ型パラレルメカニズムに適用した例を示したが、これに限定されるものではなく種々のパラレルメカニズムに適用することができる。例えば図４に示すスチュワートプラットフォーム型、図５に示す回転型、図６に示す直動固定型パラレルメカニズムに適用することができる。この場合、機構パラメータ、パラレルメカニズムのモデル化は、上記したものと異なるものとなるが、上記アルゴリズムを用い任意の数の姿勢情報を計測することで、上記と同様にキャリブレーションを容易に行うことができる。
【００８８】
また、上記実施形態では、運動要素が並進変位の３自由度（並進変位Ｘ，Ｙ，Ｚ）、姿勢角度の３自由度（オイラー角θ，φ，ψ）の合計６自由度のパラレルメカニズムのキャリブレーションを行っているが、このほか、２から５自由度のパラレルメカニズムに適用することもできる。このとき、連立方程式を分離するステップでは、上記のように位置姿勢と位置の部分で分離を行う以外に、任意の運動要素に分離することができる。以下、図７に示す２自由度のパラレルメカニズムを用いてこれを説明する。
【００８９】
このパラレルメカニズム３１は、４本のリンク３３がボールジョイント（対偶）３５を介して接続されたリンク体３７を備えており、リンク体３７の両端部がベース部３９にボールジョイント４１を介して取り付けられている。この構成により、エンドエフェクタ部Ｅは２次元平面上を２自由度で移動するようになっている。機構パラメータは、各リンク３３の長さと、リンク体３７の端部におけるボールジョイント４１の位置との計６個であるため、従来は、エンドエフェクタ部Ｅを移動させ、３箇所（各箇所でＸ，Ｙ座標）で計６個の位置情報を計測する必要があった。しかしながら、本発明によれば、すべての箇所でＸ，Ｙ座標を計測する必要はなく、例えば一箇所でのみＸ，Ｙ座標を計測するとともに、他の箇所ではＸ座標のみを計測し、計６個の位置情報を得るようにすればよい。すなわち、この例では、Ｘ座標（第１運動要素群）とＹ座標（第２運動要素群）とに方程式（第１及び第２方程式）を分離してキャリブレーションを行う。こうすることによりキャリブレーションが容易になる。
【００９０】
また、上記実施形態では、非線形連立方程式を全微分し、線形連立方程式を得た後に姿勢方程式と位置方程式とに分離しているが、非線形連立方程式の状態で上記のように方程式を分離すれば、キャリブレーションを行うことも可能である。
【００９１】
【実施例】
次に、本発明の実施例について説明する。ここでは、上記図１及び図２に示すデルタ型パラレルメカニズムを用いてキャリブレーションを行う。機構パラメータとしては、以下の表２に示す真の値を準備し、これに対して表３に示す初期誤差を与えた機構パラメータのキャリブレーションを行うこととする。この初期誤差は、真の値に対して１．７％の誤差を含ませている。
【００９２】
【表２】

【００９３】
【表３】

【００９４】
測定点のデータは、以下のように１組の姿勢情報と、２０組の位置データを用いた。
【００９５】
【表４】

【００９６】
上記のようなデータを用い、ε＝１．０×１０^-6として、図３のキャリブレーションを行うと、表５の結果を得た。
【００９７】
【表５】

【００９８】
この表５は、真の機構パラメータとキャリブレーション後の機構パラメータとの差を示している。この結果、絶対精度が理論値で０．０１μｍのキャリブレーションを行うことができた。
【００９９】
【発明の効果】
以上から明らかなように、請求項１に係る発明によれば、パラレルメカニズムをモデル化した非線形方程式を、任意の第１運動要素群と機構パラメータとの関係を示す第１方程式、及び第２運動要素群と機構パラメータとの関係を示す第２方程式に分離し、これらを組み合わせてキャリブレーションを行っている。このとき、測定点を、機構パラメータと同数だけ計測しなければならないが、上記のように分離することにより、例えば計測しにくい第１運動要素群を少数の箇所で計測し、残りの計測点を計測しやすい第２運動要素群について行うことでキャリブレーションを行うことができる。したがって、キャリブレーションを容易に、かつ正確に行うことができる。
【０１００】
また、請求項２に係る発明によれば、上記と同じ効果が得られる上、さらに、非線形連立方程式を微分して線形連立方程式を得て分離を行っているため、方程式の分離を容易に行うことができる。
【０１０１】
また、請求項３に係る発明によれば、６自由度のパラレルメカニズムについて、上記と同様の効果を得ることができる。このとき、計測しにくい姿勢情報を一箇所だけ計測し、残りの計測点を計測しやすい位置情報について計測することで、従来困難とされてきた姿勢情報の計測を最低限に抑えることができるため、キャリブレーションを容易に、かつ正確に行うことができる。
【０１０２】
また、請求項５に記載の発明によれば、姿勢情報を計測する際に、エンドプレートを既知の姿勢に保持可能な治具を用いることで、姿勢情報の計測が容易になり、その結果、キャリブレーションがさらに容易になる。
【図面の簡単な説明】
【図１】本発明のキャリブレーション法が適用されるパラレルメカニズムの一実施形態を示す説明図である。
【図２】図１のパラレルメカニズムにおける機構パラメータの定義を示す説明図である。
【図３】図１のパラレルメカニズムのキャリブレーション法を示すフローチャートである。
【図４】スチュワートプラットフォーム型パラレルメカニズムを示す斜視図である。
【図５】回転型パラレルメカニズムを示す斜視図である。
【図６】直動固定型パラレルメカニズムを示す斜視図である。
【図７】本発明のキャリブレーション法が適用されるパラレルメカニズムの他の例を示す図である。
【符号の説明】
１ デルタ型パラレルメカニズム
３ ベースプレート（ベース部）
５ リンク体（リンク）
７ エンドプレート（エンドエフェクタ部）[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a mechanism parameter calibration method for controlling the position and orientation of an end effector section in a parallel mechanism.
[0002]
[Prior art]
In recent years, high-precision positioning technology is required in various fields such as processing and assembly of electronic components such as ICs, and as one of them, positioning technology using a parallel mechanism has been proposed. The parallel mechanism is a mechanism in which the base part, which is the support base, and the end effector part, which is the operating part, are connected in parallel by a plurality of links, and has high rigidity and controllability compared to joint mechanisms such as conventional serial mechanisms. In addition, since a multi-degree-of-freedom motion can be realized with a small mechanism, it has been attracting attention as a highly accurate positioning mechanism.
[0003]
In order to achieve highly accurate positioning as described above, it is necessary to accurately determine mechanism parameters such as link length and link position. These mechanism parameters are determined in advance at the time of designing, but actual ones may have errors with respect to the initial design values due to part processing and mounting errors. Therefore, in order to perform accurate control, it is necessary to obtain actual mechanism parameter values, and calibration is performed for this purpose.
[0004]
In general, in order to obtain the value of a mechanism parameter, it is necessary to solve the same number of simultaneous equations as the number of mechanism parameters. To that end, end effectors at a plurality of positions and postures that are the same as the number of the simultaneous equations. It is necessary to measure the translational displacement and posture angle of the part. For example, when there are 36 mechanism parameters to be estimated, three information (X, Y, Z) of translational displacement and three pieces of information (θ, φ, ψ) of the posture angle are obtained at one position / posture. ) In total, the translational displacement and the posture angle at at least six different positions and postures are required.
[0005]
[Problems to be solved by the invention]
However, when measuring the position / orientation information described above, the translational displacement can be easily measured with high precision using a laser distance measurement, a laser interferometer, or the like, while the attitude angle is very difficult to measure. Therefore, there is a problem that an error is included in the measured value and accurate calibration cannot be performed.
[0006]
The present invention has been made to solve the above-described problems, and in a calibration method of a multi-degree-of-freedom parallel mechanism, a calibration method capable of minimizing measurement of posture information and the like that are difficult to measure The purpose is to provide.
[0007]
[Means for Solving the Problems]
In order to achieve the above object, according to the present invention, a base portion and an end effector portion are coupled via a plurality of links, and the end effector portion is displaced with n degrees of freedom (n ≧ 2), and m mechanism parameters are obtained. A parallel mechanism calibration method configured to be controllable based on the end effector , wherein the end effector unit is displaced with n degrees of freedom composed of n motion elements selected from translational displacement and posture angle, and the n number movement elements is separated into a second motion component groups measured easily the n ₂ and difficult n ₁ piece first movement element group of measurement depending on the difficulty of the measurement (n 1 _{+ n} 2 = _n) the nonlinear system of equations kinematics modeling of the parallel kinematic mechanism, the first equation representing the relationship between the kinematic parameters and _one of the first motion component group n, and the n ₂ of the second movement element group When And separating the second equation representing the relationship between the serial kinematic parameters, s number (s <m ', m' ≧ m) and the first equation of the s or more (m'-s) number of and obtaining the set look combined combinations simultaneous equations and the second equation, the changing the position and orientation of the end effector portion, the information corresponding to the s pieces of the first motion component group, (m'-s) number Measuring information corresponding to the second motion element group, and estimating the mechanism parameters from the measured information and the combined simultaneous equations, and performing calibration. A parallel mechanism calibration method is provided.
[0008]
In order to achieve the above object, according to the present invention, a base portion and an end effector portion are connected via a plurality of links, and the end effector portion is displaced with n degrees of freedom (n ≧ 2) and m mechanisms. A parallel mechanism calibration method configured to be controllable based on a parameter, wherein the end effector unit is displaced with n degrees of freedom composed of n motion elements selected from a translational displacement and a posture angle. n pieces of movement elements is separated into a second movement element group of _two easily measured as the first motion component group is difficult n ₁ single measurement depending on the difficulty of the measurement n (n 1 _{+ n} 2 = _n) and which, said differentiating the nonlinear simultaneous equations kinematics modeling of the parallel kinematic mechanism, and obtaining a small displacement linear system, the micro-displacement of the linear system, n ₁ pieces of the first motion And separating the second equation showing a relationship between the first equation representing the relationship between the pixel group and the kinematic parameters, and n and the _two second motion component group and the kinematic parameters, s number (s <m ', m' and the first equation of ≧ m), the s or more (m'-s) and obtaining a set look combined combinations simultaneous equations and number of the second equation, the end effector portion Changing the position and orientation, measuring information corresponding to s first motion element groups, and information corresponding to (m′−s) second motion element groups, the measured information, And providing a calibration method by estimating the mechanism parameters from the combined simultaneous equations and performing calibration.
[0009]
In order to achieve the above object, the present invention connects the base part and the end effector part via a plurality of links, and the end effector part is displaced with six degrees of freedom and can be controlled based on m mechanism parameters. A parallel mechanism calibration method configured to: a step of differentiating a nonlinear simultaneous equation modeling the kinematics of the parallel mechanism to obtain a minute displacement linear simultaneous equation; and the minute displacement linear simultaneous equation, Separating into a posture equation indicating a relationship between the posture of the end effector unit and the mechanism parameter, and a position equation indicating a relationship between the position of the end effector unit and the mechanism parameter; and s (s <m ′, m 'as the attitude equation and the s or more (m'-s) number of combinations coalition how the and the position equation combined set only ≧ m) A step of changing the position and orientation of the end effector unit to measure s pieces of posture information and (m′−s) pieces of position information, the measured information, and the combined simultaneous equations And a step of performing calibration by estimating the mechanism parameter from the above, and providing a calibration method for a parallel mechanism.
[0010]
In the calibration method, s = 3 is set, and posture information in one position and posture and (m′−3) pieces of position information can be measured.
[0011]
Moreover, the said calibration method shall measure the attitude | position of the said end effector part in the state which hold | maintained the said end effector part in the known attitude | position with the jig | tool.
[0012]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, an embodiment in which the calibration method according to the present invention is applied to a linear motion fixed parallel mechanism will be described with reference to the drawings. FIG. 1 is a schematic diagram illustrating an example of a linear motion fixed parallel mechanism, and FIG. 2 is a diagram illustrating mechanism parameters of the parallel mechanism.
[0013]
First, a linear motion fixed parallel mechanism to be calibrated will be described. As shown in FIG. 1, in this parallel mechanism 1, a triangular end plate (end effector portion) 7 is attached to a triangular base plate (base portion) 3 via three sets of link bodies (links) 5. . Each link body 5 includes a first link 5a and a second link 5b attached to the intermediate portion via a rotary joint 9.
[0014]
One end of the first link 5 a is rotatably attached to each apex of the end plate 7 via the ball joint 11, and the other end is attached to the linear slider 15 via the universal joint 13. The linear slider 15 is movable on rails 17 disposed on each side of the base plate 3. The second link 5b is attached to the linear slider 19 through the universal joint 21 at the end opposite to the rotary joint 9 in the same manner as the first link 5a. The linear motion slider 19 moves on the same rail 17 as the linear motion slider 15 of the first link 5a. The three sets of link bodies 5 each have this configuration, and the position and posture of the end plate 7 change as the linear sliders 15 and 19 move on the rail 17. Hereinafter, this is referred to as a delta parallel mechanism.
[0015]
Next, modeling of this parallel mechanism will be described. In this parallel mechanism, a base coordinate system (origin O) with reference to the base plate 3 and an end plate coordinate system (origin O _h ) with respect to the end plate 7 as shown in FIG. 2 are defined. The parameters shown are set. As shown in Table 1, in the delta type parallel mechanism 1 of the present embodiment, twelve for each link body 5 and a total of 36 mechanism parameters for the three link bodies are defined.
[0016]
[Table 1]

[0017]
From FIG. 2, the geometric relationship of the parallel mechanism is modeled as follows. First, the position vector of the contact point between the link body and the end plate in the base coordinate system is
[0018]
[Expression 1]

[0019]
Therefore, the following relationship is established from these two directions.
[0020]
[Expression 2]

[0021]
Similarly, when the contact points of the links 5a and 5b are expressed from two directions and connected by an equal sign, the following relationship is established.
[0022]
[Equation 3]

[0023]
These equations (1) and (2) are nonlinear simultaneous equations of the mechanism of the delta type parallel mechanism.
[0024]
Next, the nonlinear simultaneous equations (1) and (2) are fully differentiated to obtain the relationship between the position / posture X of the end plate, the link displacement θ, and the minute displacements δX, δθ, δP of the mechanism parameters P, respectively. That is, the following linear simultaneous equations can be obtained by fully differentiating the two expressions (1) and (2) on both sides.
[0025]
[Expression 4]

[0026]
In this way, by obtaining a linear relational expression by fully differentiating, it becomes possible to analyze the relation between the minute displacements δX, δθ, δP at a time.
[0027]
Next, the kinematics of the parallel mechanism of this embodiment will be described. This kinematics means inverse kinematics, micro kinematics, and forward kinematics. First, reverse kinematics will be explained. Here, the inverse kinematics refers to a calculation for obtaining a link displacement when a position vector and a posture matrix of the end plate 7 in the base coordinate system are given. The inverse kinematics in the delta parallel mechanism is derived below. In equation (1),
[0028]
[Equation 5]

[0029]
After all,
[0030]
[Formula 6]

[0031]
Than this,
[0032]
[Expression 7]

[0033]
It is expressed. Similarly, in equation (2):
[0034]
[Equation 8]

[0035]
After all,
[0036]
[Equation 9]

[0037]
Than this,
[0038]
[Expression 10]

[0039]
Thus, inverse kinematics (4) and (5) are obtained.
[0040]
Next, micro kinematics will be described. The micro kinematics is a calculation for obtaining a relationship of a minute displacement between the link body 5 and the end plate 7. In the following, we derive microkinematics. From equation (3), if the mechanism parameter is treated as a constant,
[0041]
[Expression 11]

[0042]
It becomes. Therefore,
[0043]
[Expression 12]

[0044]
Thus, the following relational expression is obtained.
[0045]
[Formula 13]

[0046]
Next, forward kinematics will be explained. The forward kinematics is a calculation for obtaining a position vector and a posture matrix of the end plate 7 in the base coordinate system from the link displacement. However, forward kinematics in a delta parallel mechanism is a complex nonlinear equation and is generally not required except for very specific mechanisms. Therefore, using the inverse kinematics derived as described above, the forward kinematics is calculated numerically by the iterative calculation of the Newton-Raphson method.
[0047]
From equation (6)
[Expression 14]

[0049]
From this, the position and orientation of the end plate are obtained by repeated calculation. Specifically, the calculation is performed as follows.
(1) Determine appropriate initial values p ₀ and R ₀ .
{Circle around (2)} The initial value θ ₀ of the link displacement is obtained by inverse kinematics from p ₀ and R ₀ .
(3) Assuming that δθ = θ−θ ₀ , δp ₀ and δR ₀ are obtained from δθ by the equation (7) of micro kinematics.
[0050]
[Expression 15]

[0051]
(5) Return to (2).
For calculations from (2) to (5) for a sufficiently small ε,
[0052]
[Expression 16]

[0053]
Until p _n and R _{n at} that time become position / posture information when the link displacement θ, and forward kinematics is obtained.
[0054]
Next, a parallel mechanism calibration method based on the forward kinematics will be described. In a general calibration method, first, an appropriate link displacement is given as an input value, and the position / posture of the end plate with respect to this displacement is measured by a measuring means such as a laser interferometer to obtain a measured value. Since the number of mechanism parameters obtained at this time is 36, it is necessary to measure a total of 36 or more positions and postures with the end plate in six postures / positions.
[0055]
Then, using the initial value of the mechanism parameter from the link displacement, the position / posture calculation values are obtained by forward kinematics. The initial value of the mechanism parameter used at this time is a design value of the parallel mechanism manufacturing action, but includes a machining error or an assembly error. Then, change each value of the mechanism parameter so that the difference between the measured value and the calculated value is minimized, and further calculate the position and posture by forward kinematics using each newly changed value of the mechanism parameter . Then, this calculation is repeated until the difference between the measured value and the calculated value becomes sufficiently small.
[0056]
However, it is very difficult to measure the attitude of the end plate that performs minute movements. Therefore, in this embodiment, calibration is performed by the following algorithm based on forward kinematics.
[0057]
First, in equation (3), the link displacement θ is considered as a constant,
[0058]
[Expression 17]

[0059]
[0060]
[Expression 18]

[0061]
It expresses. Subsequently, the equation (9) is separated into a portion corresponding to position information and a portion corresponding to posture information. That means
[0062]
[Equation 19]

[0063]
Therefore, what is necessary is just to isolate | separate Formula (9) into an upper half and a lower half. At this time,
[0064]
[Expression 20]

[0065]
Then,
[0066]
[Expression 21]

[0067]
It can be expressed as. From these,
(1) At the measurement point for measuring posture, use the following formula (posture equation)
[0068]
[Expression 22]

[0069]
(2) The following equation (position equation) is used at a measurement point where the posture cannot be measured or is difficult to measure.
[0070]
[Expression 23]

[0071]
That is, the above formulas (10) and (11) are combined to obtain the number of mechanism parameters, that is, 36 or more measured values. However, it is necessary to measure at least one piece of posture information. This is because even if the position of the end plate is determined, the posture at that position cannot be determined solely. For example, with the end plate 7 in an appropriate position / posture state, one piece of posture information (Euler angles θ, φ, ψ), that is, three pieces of information and a minimum of 11 pieces of position information (translational displacements X, Y, Z), that is, when measuring 33 pieces of information, equations (10) and (11) may be combined to create a relational equation (combined simultaneous equations) as follows.
[0072]
[Expression 24]

[0073]
here,
[0074]
[Expression 25]

[0075]
Then,
[0076]
[Equation 26]

[0077]
Get. Based on this equation, the mechanism parameters are updated by the following Newton-Raphson method. That is,
[0078]
[Expression 27]

[0079]
And by repeated calculation,
[0080]
[Expression 28]

[0081]
Update the mechanism parameters.
[0082]
3 will be described below. First, the position / posture of the end plate is changed to measure one piece of posture information and eleven or more pieces of position information (S1). Next, the position and orientation of the end plate 7 are calculated from the link displacement θ at the time of measurement by forward kinematics (S2). As a mechanism parameter P _n (n = 1) used in the forward kinematics, a design value obtained in advance is used. Subsequently, the mechanism parameter P _n is updated by the Newton-Raphson method (S3), and the position and orientation information of the end plate 7 is calculated by forward kinematics using the updated mechanism parameter P _{n + 1} (S4). Then, the calculated value of the position and orientation is compared with the measured value (S5). If the difference is smaller than the predetermined value ε (YES in S5), the calculation is terminated. On the other hand, when the difference between the calculated value and the measured value is larger than the predetermined value ε that guarantees the final accuracy (NO in S5), the above steps 3 to 5 are repeated until it becomes smaller than the predetermined value ε.
[0083]
As described above, according to the present embodiment, the nonlinear simultaneous equations that model the kinematics of the parallel mechanism are differentiated, and the obtained linear simultaneous equations are related to the position and orientation information of the end plate and the mechanism parameters. It is separated into a posture equation and a position equation indicating the relationship between position information and mechanism parameters. Then, calibration is performed by combining one posture equation and 11 or more position equations. In other words, since calibration is performed by measuring only one piece of posture information, measurement of posture information, which has been difficult in the past, can be minimized, and as a result, calibration of mechanism parameters can be easily performed. And can be done accurately.
[0084]
In the above embodiment, calibration is performed by measuring only one piece of posture information, but it is sufficient to create the same number of simultaneous equations as the number of mechanism parameters by arbitrarily combining posture equations and position equations. Calibration can also be performed by measuring individual posture information. In this case, if posture information is measured only at a location where measurement is easy while avoiding a location where measurement is difficult, calibration is facilitated.
[0085]
When posture information is measured, the measurement becomes easier if the following is performed. That is, when the end plate 7 is in an arbitrary posture state, a jig that can hold the end plate 7 in a known posture state is used. As a result, the posture state of the end plate 7 becomes known, and it is only necessary to measure the link displacement at this time, so that calibration is further facilitated. The jig used here may be anything as long as it can hold the end plate in a known posture, for example, holding the end plate with the plate, holding the end plate with a plurality of rod-shaped members, etc. Can be mentioned.
[0086]
In the above embodiment, the calibration is performed by the Newton-Raphson method, but other methods are also possible.
[0087]
Moreover, although the example applied to the delta parallel mechanism was shown in the said embodiment, it is not limited to this, It can apply to various parallel mechanisms. For example, the present invention can be applied to the Stewart platform type shown in FIG. 4, the rotary type shown in FIG. 5, and the direct acting fixed type parallel mechanism shown in FIG. In this case, the modeling of the mechanism parameters and the parallel mechanism is different from the above, but calibration can be easily performed in the same manner as described above by measuring an arbitrary number of posture information using the above algorithm. Can do.
[0088]
In the above-described embodiment, the motion element is a parallel mechanism having a total of 6 degrees of freedom including three degrees of freedom of translational displacement (translational displacement X, Y, Z) and three degrees of freedom of posture angle (Euler angles θ, φ, ψ). Although calibration is performed, it can also be applied to a parallel mechanism having 2 to 5 degrees of freedom. At this time, in the step of separating the simultaneous equations, it can be separated into arbitrary motion elements other than the separation at the position and orientation and the position as described above. Hereinafter, this will be described using a parallel mechanism of two degrees of freedom shown in FIG.
[0089]
The parallel mechanism 31 includes a link body 37 in which four links 33 are connected via ball joints (pairs) 35, and both ends of the link body 37 are attached to the base portion 39 via the ball joints 41. It has been. With this configuration, the end effector portion E moves on the two-dimensional plane with two degrees of freedom. Since there are six mechanism parameters, that is, the length of each link 33 and the position of the ball joint 41 at the end of the link body 37, conventionally, the end effector portion E is moved to three positions (X at each position). , Y coordinates), it was necessary to measure a total of six pieces of position information. However, according to the present invention, it is not necessary to measure the X and Y coordinates at all locations. For example, the X and Y coordinates are measured only at one location, and only the X coordinates are measured at other locations. What is necessary is just to obtain the position information. That is, in this example, calibration is performed by separating equations (first and second equations) for the X coordinate (first motion element group) and the Y coordinate (second motion element group). This facilitates calibration.
[0090]
In the above embodiment, the nonlinear simultaneous equations are fully differentiated, and after obtaining the linear simultaneous equations, the posture equations and the position equations are separated. However, if the equations are separated as described above in the state of the nonlinear simultaneous equations, It is also possible to perform calibration.
[0091]
【Example】
Next, examples of the present invention will be described. Here, calibration is performed using the delta type parallel mechanism shown in FIGS. As the mechanism parameters, the true values shown in Table 2 below are prepared, and the mechanism parameters given the initial error shown in Table 3 are calibrated. This initial error includes an error of 1.7% with respect to the true value.
[0092]
[Table 2]

[0093]
[Table 3]

[0094]
As the measurement point data, one set of posture information and 20 sets of position data were used as follows.
[0095]
[Table 4]

[0096]
When the calibration as shown in FIG. 3 was performed with ε = 1.0 × 10 ⁻⁶ using the above data, the results shown in Table 5 were obtained.
[0097]
[Table 5]

[0098]
Table 5 shows the difference between the true mechanism parameter and the mechanism parameter after calibration. As a result, it was possible to perform calibration with an absolute accuracy of 0.01 μm as a theoretical value.
[0099]
【The invention's effect】
As is clear from the above, according to the first aspect of the present invention, the nonlinear equation that models the parallel mechanism is changed into the first equation indicating the relationship between the arbitrary first motion element group and the mechanism parameter, and the second motion. separating the second equation representing the relationship between the element group and the mechanism parameters are calibrated by these combine seen. At this time, the same number of measurement points as the mechanism parameters must be measured, but by separating as described above, for example, the first movement element group that is difficult to measure is measured at a small number of locations, and the remaining measurement points are determined. Calibration can be performed by performing the second movement element group that is easy to measure. Therefore, calibration can be performed easily and accurately.
[0100]
Further, according to the invention according to claim 2, the same effect as described above can be obtained, and further, the linear simultaneous equations are obtained by differentiating the nonlinear simultaneous equations and the separation is performed, so that the equations are easily separated. be able to.
[0101]
Moreover, according to the invention which concerns on Claim 3, the effect similar to the above can be acquired about the parallel mechanism of 6 degrees of freedom. At this time, it is possible to minimize the measurement of posture information, which has been considered difficult in the past, by measuring posture information that is difficult to measure, and measuring the remaining measurement points for easy-to-measure position information. Calibration can be performed easily and accurately.
[0102]
Further, according to the invention described in claim 5, when measuring posture information, by using a jig capable of holding the end plate in a known posture, the posture information can be easily measured. Calibration becomes easier.
[Brief description of the drawings]
FIG. 1 is an explanatory diagram showing an embodiment of a parallel mechanism to which a calibration method of the present invention is applied.
FIG. 2 is an explanatory diagram showing the definition of mechanism parameters in the parallel mechanism of FIG. 1;
FIG. 3 is a flowchart showing a calibration method of the parallel mechanism of FIG. 1;
FIG. 4 is a perspective view showing a Stewart platform type parallel mechanism.
FIG. 5 is a perspective view showing a rotary parallel mechanism.
FIG. 6 is a perspective view showing a linear motion fixed parallel mechanism.
FIG. 7 is a diagram showing another example of a parallel mechanism to which the calibration method of the present invention is applied.
[Explanation of symbols]
1 Delta type parallel mechanism 3 Base plate (base part)
5. Link body (link)
7 End plate (end effector)

Claims (5)

Calibration of a parallel mechanism in which a base part and an end effector part are connected via a plurality of links, and the end effector part is displaced with n degrees of freedom (n ≧ 2) and can be controlled based on m mechanism parameters. Which is
Said end effector portion is displaced with n degrees of freedom constituted by n pieces of movement elements selected from the translational displacement and the orientation angles, the n number of motion elements, measurement difficult n ₁ piece depending on the difficulty of the measurement Of the first movement element group and n ₂ (n ₁ + n ₂ = n) second movement element groups that are easy to measure ,
The nonlinear system of equations kinematics modeling of the parallel mechanism, the first equation, and the n ₂ of the second movement element group indicating a relationship between the kinematic parameters and _one of the first motion component group n Separating into a second equation showing the relationship with the mechanism parameters;
and obtaining the s (s <m ', m' ≧ m) and the first equation of the s or more (m'-s) pieces of combination simultaneous equations and a second equation combines seen,
Changing the position and orientation of the end effector unit and measuring information corresponding to s first motion element groups and information corresponding to (m′−s) second motion element groups; ,
A parallel mechanism calibration method comprising: a step of performing calibration by estimating the mechanism parameters from the measured information and the combined simultaneous equations. Calibration of a parallel mechanism in which a base part and an end effector part are connected via a plurality of links, and the end effector part is displaced with n degrees of freedom (n ≧ 2) and can be controlled based on m mechanism parameters. Which is
Said end effector portion is displaced with n degrees of freedom constituted by n pieces of movement elements selected from the translational displacement and the orientation angles, the n number of motion elements, measurement difficult n ₁ piece depending on the difficulty of the measurement Of the first movement element group and n ₂ (n ₁ + n ₂ = n) second movement element groups that are easy to measure ,
Differentiating a nonlinear simultaneous equation modeling the kinematics of the parallel mechanism to obtain a small displacement linear simultaneous equation;
Said small displacement linear system, the first equation representing the relationship between _one of the first motion component group n and the kinematic parameters, and n and the _two second motion element group of a relation between said kinematic parameters Separating into the second equation shown;
the s (s <m ', m' ≧ m) and the first equation of, and obtaining the s or more (m'-s) number set and the second equations of viewing the combined combination simultaneous equations ,
Changing the position and orientation of the end effector unit and measuring information corresponding to s first motion element groups and information corresponding to (m′−s) second motion element groups;
A parallel mechanism calibration method comprising: a step of performing calibration by estimating the mechanism parameter from the measured information and the combined simultaneous equations. A parallel mechanism calibration method in which a base part and an end effector part are connected via a plurality of links, and the end effector part is displaced in six degrees of freedom and can be controlled based on m mechanism parameters. ,
Differentiating a nonlinear simultaneous equation modeling the kinematics of the parallel mechanism to obtain a small displacement linear simultaneous equation;
Separating the micro-displacement linear simultaneous equations into a posture equation indicating a relationship between the posture of the end effector unit and the mechanism parameter, and a position equation indicating a relationship between the position of the end effector unit and the mechanism parameter;
obtaining a s number (s <m ', m' ≧ m) the posture equations and the s or more (m'-s) number set and the position equations look combined combined simultaneous equations,
Changing the position and orientation of the end effector unit and measuring s pieces of posture information and (m′−s) pieces of position information;
A parallel mechanism calibration method comprising: a step of performing calibration by estimating the mechanism parameter from the measured information and the combined simultaneous equations.   4. The parallel mechanism calibration method according to claim 3, wherein s = 3, and posture information in one position and posture and (m'-3) pieces of position information are measured.   5. The parallel mechanism calibration method according to claim 3, wherein the posture of the end effector is measured in a state where the end effector is held in a known posture by a jig. 6.

Images