JP5450242B2 - Manipulator calibration method and robot control system - Google Patents

Description

  The present invention relates to a manipulator calibration method and a robot control system.

  In recent years, a control method for attaching a sensor device called a hand camera to the tip of a manipulator and controlling the operation of the manipulator based on information obtained from the sensor has been developed. However, accurate control of the manipulator cannot be performed unless the mounting position and orientation information of the sensor and the manipulator is accurately grasped in addition to the motor assembly error and arm machining error of the manipulator. For this reason, it is necessary to accurately derive the position / orientation information of the sensor based on the positional relationship of each link part of the manipulator and the coordinate system of the tip of the manipulator, that is, to perform calibration.

  Here, there is Patent Literature 1 as a calibration method for a manipulator having a hand camera. In Patent Document 1, a CCD camera is attached to the hand of a manipulator, a subject point is fixed at a predetermined position, and the manipulator and the camera are variously articulated to perform imaging. After that, a plurality of deviations between the positions of the subject points on the two-dimensional image of the actual camera and the positions of the subject points that are logically calculated are obtained. This is a technique for calculating a correction value by performing a least square method so as to be minimized. In Patent Document 1, there is an advantage that it is possible to save the trouble of actually measuring each link length and moving the manipulator to a certain point, and to greatly shorten the work process.

  In Patent Document 2, although the hand camera is not calibrated, a jig having a feature point is attached to the tip of the manipulator, and the feature point is detected by an imaging and aiming sensor fixed to a robot body that supports the manipulator. A method for calibrating a three-dimensional position sensor by observing a holding jig has been proposed. In Patent Document 2, a plurality of combinations of the three-dimensional position of the feature point in the reference coordinate system and the position of the feature point on the imaging surfaces of the two cameras of the sensor unit for imaging and aiming are obtained, and a triangle is obtained therefrom. The three-dimensional positions of two cameras (sensors) are calibrated based on the principle of surveying. This method has an advantage that it is possible to avoid the danger of the laser hitting the eyes of the operator because the sensor does not have to be moved manually, and the labor can be reduced.

  Further, in Patent Document 3, the posture of a robot arm having a visual sensor attached to the tip is changed, and before and after the change, the change in the work image acquired by the visual sensor and the provisional value of the camera attachment position and posture are determined. Find the position change of the workpiece. At this time, the camera mounting position can be determined by preparing a plurality of temporary values and selecting the one that minimizes the workpiece position change.

  In Patent Document 3, there is an advantage that the place where the workpiece is placed can be freely set and calibration can be performed in a state close to actual work.

Japanese Patent No. 2668263 JP 11-33962 A JP 2007-61979 A

  By the way, as can be seen in Patent Document 1 and Patent Document 3, several methods for calibrating the hand camera have been proposed. However, a special jig is required when performing calibration. In other words, Patent Documents 1 and 3 require a dedicated calibration device and jig, and there is a problem that much time and cost are required for calibration. Although Patent Document 2 is not a hand camera, it is a camera calibration for observing the position of the manipulator tip and requires a dedicated jig to be attached to the manipulator.

  An object of the present invention is to easily perform calibration of a manipulator as well as calibration of a sensor anywhere with a jig having a simple configuration having only a straight portion, and man-hours such as preparation of the jig. This makes it possible to calibrate immediately even in environments where special jigs cannot be prepared, thereby reducing unnecessary waiting time and man-hours for the original work required for manipulators. It is to provide a manipulator calibration method.

  Another object of the present invention is to provide a robot control system for executing the above calibration method.

  In order to solve the above-mentioned problem, the invention according to claim 1 is a state in which a plurality of points on the linear portion of the jig arranged around the manipulator are in a plurality of different observation positions and postures. The first step of detecting by the visual sensor provided at the tip of the manipulator, the origin position and orientation of the manipulator tip coordinate system based on the reference coordinate system, and the sensor coordinate system of the visual sensor based on the manipulator tip A second step of obtaining a reference coordinate system coordinate of a point on the straight line portion (hereinafter referred to as a detection point) for each observation position and orientation, which is a function having an origin position and orientation as an unknown variable; When two detection points on the straight line portion obtained every time are set as one set, the function is used as an element from a plurality of sets, and the straight line with reference to the reference coordinate system is used. A plurality of unit direction vectors of the part is calculated, and the calculated unit direction vector of the straight line part and the unit direction vector of the straight line part represented by the vector variable of the reference coordinate system are used to create simultaneous equations, A first calibration value related to the manipulator is obtained by a least square method, and the origin position and orientation of the sensor coordinate system of the visual sensor, and the origin position and orientation of the sensor coordinate system with reference to the tip of the manipulator is a second calibration value. The gist of the present invention is a manipulator calibration method characterized by including a third step obtained as follows.

The invention of claim 2 is characterized in that, in claim 1, the simultaneous equations are linearized into a first-order approximation.
According to a third aspect of the present invention, in the second aspect, when the first calibration value and the second calibration value are each composed of a temporary real number and a sum of error variables, When the first calibration value and the second calibration value are obtained, respectively, the temporary real number is updated to the obtained first calibration value and the second calibration value, and then the error variable is equal to or less than a predetermined threshold value. The process of the third step is repeated until it becomes.

  According to a fourth aspect of the present invention, in any one of the first to third aspects, the jig has a plurality of surfaces and a plurality of ridge lines provided between the surfaces. Among the ridge lines, it is one ridge line.

  According to a fifth aspect of the present invention, there is provided control means for taking a plurality of different observation position / postures of the manipulator in order to detect a plurality of points on a linear portion of a jig arranged around the manipulator; A storage means for storing values detected by a visual sensor provided at the tip of the manipulator for each of a plurality of different points on the linear portion of the jig for each of a plurality of different observation positions and orientations, and a manipulator based on a reference coordinate system It is a function with the origin position and orientation of the tip coordinate system and the origin position and orientation of the sensor coordinate system of the visual sensor relative to the tip of the manipulator as unknown variables, and a point on the straight line portion (hereinafter referred to as a detection point) Detection point coordinate calculation means for each of the observation position and orientation, and the straight line obtained for each of the observation position and orientation When two of the upper detection points are set as one set, the function is used as an element from a plurality of sets, and a plurality of unit direction vectors of the straight line portion with respect to the reference coordinate system are calculated, and the calculated straight line A simultaneous equation is created using the fact that the unit direction vector of the part and the unit direction vector of the straight line part represented by the vector variable of the reference coordinate system are equal, and the first calibration value for the manipulator is obtained by the least square method In addition, the gist of the robot control system includes calibration value calculation means for performing processing for obtaining the origin position and orientation of the sensor coordinate system based on the tip of the manipulator as a second calibration value.

The invention of claim 6 is characterized in that, in claim 5, the calibration value calculation means calculates the calibration value after linearizing the simultaneous equations into a linear approximation.
According to a seventh aspect of the present invention, in the sixth aspect, the calibration value calculating means is configured such that the first calibration value and the second calibration value are each composed of a temporary real number and a sum of error variables, When the value and the second calibration value are respectively obtained, the temporary real number is updated to the obtained first calibration value and the second calibration value, and then the error variable is equal to or less than a predetermined threshold value. The process is repeated.

  According to the first aspect of the present invention, not only the calibration of the sensor but also the calibration of the manipulator can be easily performed everywhere with a jig having a simple configuration having only a straight portion. In addition to greatly reducing the number of man-hours required for jig preparation, it is possible to calibrate immediately even in environments where special jigs cannot be prepared, and to perform the original work required for manipulators. Unnecessary waiting time and man-hours can be reduced.

  According to the invention of claim 2, since the simultaneous equations are linearized to be a linear approximation, the calculation performed in the third step process can be simplified, compared with the case where the linearization is not made to be a linear approximation. There is an effect that the time required for the calculation can be shortened.

According to the third aspect of the present invention, an error occurs when the simultaneous equations are linearized. However, this error can be reduced to a threshold value or less, and the accuracy of the calibration value can be ensured.
According to the invention of claim 4, since the ridge line of the jig is a straight part, a special jig is not necessary, and an existing jig having a straight part such as a rectangular parallelepiped or a cube between the surfaces can be used. Or a simple jig can be used, and no special jig is required for calibration.

According to invention of Claim 5, the robot control system which performs the calibration method of said Claim 1 can be provided.
According to the invention of claim 6, since the simultaneous equation is linearized into a linear approximation formula, the calculation performed in the process of the third step can be simplified, and when the linearization is not made into the linear approximation formula. In comparison, it is possible to provide a robot control system that executes a calibration method that can shorten the time required for computation.

  According to the invention of claim 7, an error occurs when the simultaneous equations are linearized. The robot control that executes the calibration method that can reduce the error to a threshold value and ensure the accuracy of the calibration value. Can provide a system.

The block diagram which shows the structure of the robot control system of one Embodiment. Explanatory drawing which shows the state which the laser displacement sensor LS changed the observation position attitude | position with respect to the jig | tool. Explanatory drawing of the coordinate system used by embodiment.

Hereinafter, an embodiment in which a calibration method for a manipulator having a visual sensor embodying the present invention and a robot control system are embodied will be described with reference to FIGS.
FIG. 1 is a block diagram showing the configuration of the robot control system 10. The robot control system 10 controls the work (work object) to perform a specific work. The robot control system 10 includes a manipulator M that performs the above operation, a robot control device RC that controls the manipulator M, a visual sensor that detects the shape of a workpiece, and a laser displacement sensor LS as a laser sensor.

  The manipulator M includes a base member 12 fixed to a floor or the like, and a plurality of arms 13 connected via a plurality of rotating shafts. The robot of this embodiment is a 6-axis robot. A tool (not shown) for performing the specific operation, for example, a welding torch, a paint gun, a robot hand, or the like, is provided at the distal end of the arm 13 (that is, the distal end of the manipulator M) located on the most distal side.

  The robot controller RC is composed of a computer. That is, the robot controller RC is a CPU (central processing unit) 20, a rewritable ROM 21 that stores various programs for controlling the manipulator M, a RAM 22 that is a working memory, and a rewritable nonvolatile memory that stores various data. The memory | storage part 23 which consists of is provided. The CPU 20 corresponds to control means, detection point coordinate calculation means, and calibration value calculation means. The storage unit 23 corresponds to a storage unit.

  The ROM 21 stores various robot control programs for performing calibration of the manipulator and specific work of the tool. The CPU 20 controls and drives a motor (not shown) provided between the arms 13 according to the program, whereby the position and orientation of the tool and the observation position and orientation of the laser displacement sensor LS change.

  The laser displacement sensor LS is attached to the tip of the arm 13 (that is, the tip of the manipulator) via a bracket Br so as to be attached to the tool (see FIG. 3). In this embodiment, the origin position / posture of the sensor coordinate system with an arbitrary point as a reference is obtained as a second calibration value C to be described later at the tip of the manipulator M. In the present embodiment, at the tip of the manipulator M, the mounting part of the bracket is the one arbitrary point. However, the present invention is not limited, and a specific position of the manipulator M may be an arbitrary point.

  The laser displacement sensor LS is a two-dimensional laser sensor that is a scanning type laser displacement sensor that measures the distance to the workpiece W by light emission and light reception of a laser. As shown in FIG. It has a detection range. The laser displacement sensor LS includes a laser irradiation unit Lsa that irradiates a laser, a light receiving unit (not shown) that receives a laser beam reflected by a work target such as a workpiece, and the like. The value detected by the laser displacement sensor LS is output to the robot control device RC via the sensor interface unit 24 shown in FIG.

A method for calibrating the manipulator M and the laser displacement sensor LS will be described using the robot control system 10 configured as described above.
(Work procedure)
First, the calibration procedure of the present embodiment will be described.

  A laser displacement sensor LS is attached to the tip of the manipulator M via a bracket Br. At this time, the position and orientation from the tip of the manipulator M to the laser irradiation unit Lsa are measured, or obtained based on the catalog values of the bracket Br and the laser displacement sensor LS, and the position and orientation are temporarily calculated as the second calibration value C. Are stored in advance in the storage unit 23 of the robot controller RC via an input device such as a teach pendant or a personal computer (not shown).

  A jig G is arranged on the table 30. In addition, arrangement | positioning of the jig | tool G is not limited on the table 30, You may arrange | position directly on a floor surface so that it may not move. In the present embodiment, the jig G has a plurality of surfaces and ridge lines having straight lines between adjacent surfaces. In the present embodiment, the jig G is formed in a flat plate or a rectangular parallelepiped as shown in FIG. 2, and the ridge line on the upper surface extending in the longitudinal direction of the upper surface is the straight portion 32. In other words, the jig G has a plurality of planes and a plurality of ridge lines, and among the plurality of ridge lines, one ridge line provided at the edge of the upper surface is defined as the straight line portion 32.

  Next, when the CPU 20 executes the calibration calculation program stored in the ROM 21, the CPU 20 drives and controls the motors of the rotating shafts of the manipulator M (not shown). Under the control of the manipulator M of the CPU 20, the observation position / posture for detecting the detection point, that is, the observation position / posture at the tip of the manipulator M is made different, that is, a plurality of these observation position / postures (posture 1, A plurality of different points on the linear portion 32 provided on the jig G are measured by the laser displacement sensor LS. The number of observation positions and orientations must be at least 6 points. This process corresponds to the first step.

Note that the number of observation positions and orientations (that is, the number of detection points) may be determined according to the number of axes of the robot.
For example, the parameter of the manipulator to be calibrated increases or decreases according to the increase or decrease of the number of axes of the robot, but if the number of parameters to be calibrated is p, the number of detection points q needs to satisfy the following equation.

p < _q C ₂ × 3 (where _q C ₂ represents a combination),
When this is solved, 3q ² -3q-2p> 0.
Therefore, q> √ (9 + 24p) / 6
It becomes. For this reason, in the case of the 6-axis robot of this embodiment,
Since p = 45 (= 6 × 6 + 6 + 3),
Since q> 5.5 and the number of detection points q is a positive integer, it is 6 or more. In “6 × 6 + 6 + 3”, “6 × 6” is the number of parameters of the manipulator, “6” is the number of parameters of the sensor, and “3” is the number of parameters of the straight line portion 32.

  Next, the robot controller RC takes in the detected values at the points (hereinafter referred to as detection points) measured by the laser displacement sensor LS via the sensor interface unit 24 and stores them in the storage unit 23. In FIG. 2, the detected points are indicated by K <b> 1 to K <b> 3 in the postures 1 to 3, respectively.

  Thereafter, the CPU 20 of the robot controller RC uses the detected value and the initial value of the link parameter of the manipulator M, which is stored in the storage unit 23 in advance, and the first calibration value related to the manipulator, the laser displacement sensor, and so on. Find the LS mounting error. The link parameter defines the dimensions of each arm constituting the manipulator M and the arrangement of each rotation axis by, for example, the Denavit-Hartenberg method, and is well known. Omitted. The initial value of the link parameter is stored in the storage unit 23.

(Calculation of calibration value)
Next, a calibration value calculation method executed by the CPU 20 will be described.
First, in this embodiment, the definition of a coordinate system, a coordinate transformation matrix (homogeneous transformation matrix), and vectors as basic elements will be described with reference to FIG. Each coordinate system described in the present embodiment is a right-handed orthogonal system.

  In FIG. 3, Ti is the origin position / posture of the J6 reference coordinate system (that is, the manipulator tip coordinate system) of the manipulator M tip relative to the robot coordinate system in the posture i (i = 1, 2, 3,...). Can be obtained by observation. Ti is composed of an encoder value obtained by observation and an unknown parameter serving as a first calibration value to be obtained. J6 represents the sixth when the rotation shaft of the manipulator M is counted in order from the base member 12 side. Therefore, the J6 reference coordinate system is a coordinate system based on the sixth rotation axis.

Further, the robot coordinate system is a coordinate system having a specific point of the base member 12 as an origin as shown in FIG. 3, and corresponds to a reference coordinate system. The specific point may be an arbitrary point on the base member 12.
C is the origin position and orientation of the coordinate system of the laser irradiation unit Lsa with respect to the J6 reference coordinate system at the tip of the manipulator M, and is an unknown number, and is a calibration value (that is, a second calibration value) calculated in the present embodiment. is there. Here, the coordinate system of the laser irradiation part Lsa corresponds to a sensor coordinate system. The sensor coordinate system is a right-handed orthogonal system in which the laser irradiation direction is the x-axis (+) direction.

F _i is the position of the detection point with reference to the coordinate system (sensor coordinate system) of the laser irradiation unit Lsa in the posture i, and can be obtained from the output (ie, detection value) of the laser displacement sensor LS.

P _i is the position of the detection point based on the robot coordinate system (reference coordinate system) in posture i, and can be obtained from T _i , C, and F _i .
Note that P _i and P _j correspond to functions using the link parameter and the value detected by the visual sensor as an unknown variable with Ti including the unknown parameter serving as the first calibration value and the second calibration value C. .

The initial value of the link parameter is an initial value of a temporary real number A ′ used to obtain the first calibration values x _aj , y _aj , z _aj , α _aj , β _aj , and μ _ai related to T _i in the later-described iterative calculation. Value. Here, the suffix a of x, y, z, α, β, μ means that these values are provisional real numbers.

  The initial value for C is obtained as an approximation of the translation of the calibration value C and the parameters of roll, pitch, and yaw from the shape of the bracket Br to which the laser displacement sensor LS is attached and the laser displacement sensor LS itself. These initial values are stored in the storage unit 23 in advance.

L is a unit direction vector of the straight line portion 32 based on the robot coordinate system, and is an unknown value. T _i and C are 4 × 4 homogeneous transformation matrices, and F _i , P _i, and L are four-dimensional vectors.

Next, calculation performed by the CPU 20 for each basic element will be described.
In the manipulator M, let A _ij be the homogeneous transformation matrix of the origin position and orientation of the coordinate system at the tip of the j-axis with reference to the coordinate system at the tip of the j−1 axis in the orientation i. When A _ij is expressed by parameters of translation and roll, pitch, and yaw (x, y, z, yaw, pitch, roll) = (x _aj , y _aj , z _aj , α _aj , β _aj , γ _aij ), 1).

ここで、ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊはロボット毎に決定される値である。又、γ_ａｉｊは、マニピュレータＭの各回転軸を駆動するモータに設けられたロータリーエンコーダのエンコーダ値ε_ｉｊとロボット毎に決定されるμ_ａｉの和で表される値である。ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉは本実施形態で求める前記Ｔｉに関する第１較正値である。

Here, x _aj , y _aj , z _aj , α _aj , and β _aj are values determined for each robot. Γ _aij is a value represented by the sum of the encoder value ε _ij of the rotary encoder provided in the motor that drives each rotating shaft of the manipulator M and μ _ai determined for each robot. x _aj , y _aj , z _aj , α _aj , β _aj , and μ _ai are first calibration values related to Ti obtained in the present embodiment.

又、本実施形態では、Ｔｉはベース部材１２から６つ目のアームであるＡ_ｉ６先端の座標なので式（３）で表される。

In this embodiment, since Ti is the coordinate of the tip of _Ai6, which is the sixth arm from the base member 12, it is expressed by Expression (3).

レーザ照射部Ｌｓａの座標系（センサ座標系）を基準とした姿勢ｉにおける検出点の位置Ｆｉは式（４）で表される。ここでｙ_fｉ，ｚ_fｉ はレーザ変位センサＬＳにより取得される値である。なお、式（４）式及び式（５）中の「Ｔ」は転置行列を表している。

The position Fi of the detection point in the posture i with reference to the coordinate system (sensor coordinate system) of the laser irradiation unit Lsa is expressed by Expression (4). Here, y _fi and z _fi are values acquired by the laser displacement sensor LS. Note that “T” in Equation (4) and Equation (5) represents a transposed matrix.

ロボット座標系（基準座標系）を基準とした直線部３２の単位方向ベクトルＬは式（５）で表される。ここでｘ_l，ｙ_l，ｚ_lは未知の値であって、ベクトル変数である。

The unit direction vector L of the straight line portion 32 with reference to the robot coordinate system (reference coordinate system) is expressed by Expression (5). Here, x _l , y _l and z _l are unknown values and are vector variables.

そして、ＣＰＵ２０は、ロボット座標系（基準座標系）を基準とした姿勢ｉにおける検出点の位置Ｐ_ｉを式（６）で算出する。

Then, the CPU 20 calculates the position P _i of the detection point in the posture i with the robot coordinate system (reference coordinate system) as a reference, using Expression (6).

この式（６）の計算処理は、第２ステップに相当する。

The calculation process of Equation (6) corresponds to the second step.

(Calculation of constraint conditions and calibration values by CPU 20)
Since different detection points are all on the straight line portion 32 as described above, the vector from the i-th detection point to the j-th detection point is the direction vector of the straight line portion 32. When this direction vector is normalized, it becomes a unit direction vector and is equal to the unit direction vector L of the straight line portion 32 with reference to the robot coordinate system. In the equation (7), on the left side, the numerator indicates a direction vector, and the denominator indicates normalization.

なお、前記式（７）は四次元のベクトル式であるが、四次元目が全て０であるため、実質はＣとＬに含まれる未知数（すなわち、前記較正値）を変数とした関数を要素とする３次元ベクトルであり、要素ごとの３つの方程式が成り立つ。又、検出点の検出、すなわち、観測位置姿勢の組の総数を考慮すると、ＣとＬに含まれる未知数を変数とした方程式は、取得した観測位置姿勢の数をnとすると、３×（ｎ（ｎ−１））／２個取得できる。例えば、観測位置姿勢ｎが６の場合、４５個の方程式を取得できる。

The equation (7) is a four-dimensional vector equation, but since the fourth dimension is all 0, the function is essentially a function with unknowns included in C and L (that is, the calibration value) as variables. The three equations for each element hold. Further, in consideration of detection point detection, that is, the total number of observation position / posture sets, an equation using unknowns included in C and L as variables is 3 × (n (N-1)) / 2 can be acquired. For example, when the observation position / posture n is 6, 45 equations can be acquired.

The CPU 20 generates these equations, that is, generates simultaneous equations, and obtains the unknown by using the least square method. That is, the calibration values x _aj , y _aj , z _aj , α _aj , β _aj , μ _ai , and the calibration value C are obtained. The process performed by the CPU 20 corresponds to the third step.

(Linearization and repeated calculation by CPU 20)
Here, linearization will be described. In the present embodiment, linearization of variables is performed for simplification. In addition, because the number of unknowns is large, the description will be simplified, and the variable (that is, the unknown number) will be represented by a general form A in order to treat the error variable as a minute value. Here, the variable A is divided into a temporary real number A ′ and an error variable ΔA representing the difference between A and A ′, and A is represented by the sum (A ′ + ΔA) thereof. That is, A = A ′ + ΔA. Here, the unknown A is considered as the calibration value (that is, including the first calibration value and the second calibration value).

  And the calculation regarding the said calibration value (namely, unknown A) demonstrated to the preceding clause shall be performed. In order to obtain the solution of the equation by using the fact that the error variable ΔA is a minute value before solving the equation created in Equation (7), the CPU 20 linearizes each equation by first-order Macrolin expansion. .

  Here, Macrolin's expansion is a method for obtaining an nth-order approximate expression around the origin by differentiating the original function by nth order and calculating the inclination of the hyperplane near the origin. In the present embodiment, since linear approximation is performed, it can be described by a very simple expression. That is, when Expression (7) is expressed as Expression (8), a first-order approximation expression of the Macrolin expansion can be expressed as Expression (9).

又、本実施形態では、ＣＰＵ２０は、様々な検出点ｉ，ｊの組により、式（９）式から、最小二乗法により全ての未知数Ａを求める、すなわち、初期値を除いた第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉ、及び第２較正値Ｃを算出する。なお、算出した結果である第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉ、及び第２較正値Ｃには線形化により誤差が生じるため、ＣＰＵ２０はＡ（＝A' ＋ΔＡ）を新たな、A' として更新した後、始めから計算を行う。

In the present embodiment, the CPU 20 obtains all unknowns A by the least square method from the equation (9) using various combinations of detection points i and j, that is, the first calibration value excluding the initial value. x _aj , y _aj , z _aj , α _aj , β _aj , μ _ai , and the second calibration value C are calculated. Since the first calibration value _xaj , _yaj , _zaj , _αaj , _βaj , _μai , and the second calibration value C, which are calculated results, cause errors due to linearization, the CPU 20 uses A (= After updating A ′ + ΔA) as a new A ′, the calculation starts from the beginning.

Each variable constitutes an error .DELTA.A, for example, in the case of the second calibration value _{C, Δxc, Δy c, Δz} c, Δα c, Δβ c, the magnitude of [Delta] [gamma] _c is the formula (12) below, This process is repeated until expression (13) is satisfied.

  This ensures the accuracy of the second calibration value C finally obtained. Expressions (12) and (13) are examples of comparison processing performed by the CPU 20 with the threshold value S and the threshold value θ. The threshold value S and the threshold value θ are position and orientation threshold values, respectively.

なお、Ｔｉに関する第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉに関しても、Ｃと同様にそれぞれ設けられた閾値よりも小さな値となるまで、ＣＰＵ２０はＡ（＝A' ＋ΔＡ）を新たなA' として更新した後、前記処理を繰り返す。このことにより、最終的に求められるＴｉに関する第１較正値ｘ_ａｊ，ｙ_ａｊ，ｚ_ａｊ，α_ａｊ，β_ａｊ，μ_ａｉの精度を保障している。

Note that the first calibration values x _aj , y _aj , z _aj , α _aj , β _aj , and μ _ai related to Ti are similar to C, until the CPU 20 becomes A (= After updating A ′ + ΔA) as a new A ′, the above process is repeated. This guarantees the accuracy of the first calibration values x _aj , y _aj , z _aj , α _aj , β _aj , and μ _ai regarding the finally obtained Ti.

Now, according to this embodiment, there are the following features.
(1) In the calibration method of the present embodiment, as a first step, a plurality of points on the straight line portion 32 of the jig G arranged around the manipulator M are regarded as a plurality of different observation position / postures of the manipulator M. In this state, the laser displacement sensor LS detects each. As a second step, the origin position / posture Ti of the J6 reference coordinate system (manipulator tip coordinate system) with reference to the robot coordinate system (reference coordinate system) and the laser irradiation of the laser displacement sensor LS with the tip of the manipulator M as a reference. In addition to the functions Pi and Pj having the origin position and orientation C of the coordinate system (sensor coordinate system) of the part Lsa as unknown variables, the reference coordinate system coordinates (robot coordinate system coordinates) of the detection point on the straight line part 32 are set as the observation position. Obtain for each posture.

Further, as a third step, when two detection points on the straight line portion 32 obtained for each observation position and orientation are taken as one set, functions Pi and Pj are used as elements from a plurality of sets, and the robot coordinate system ( A plurality of unit direction vectors of the straight line portion 32 with respect to the reference coordinate system) are calculated. Furthermore, the calculated unit direction vector of the straight line portion 32 and the unit direction vector L of the straight line portion 32 represented by the vector variables (x ₁ , y ₁ , z ₁ ) of the robot coordinate system (reference coordinate system) are equal. Use it to make simultaneous equations. Further, the first calibration value for the manipulator M is obtained by the least square method, and the origin position / posture C of the coordinate system (sensor coordinate system) of the laser irradiation unit Lsa with the tip of the manipulator M as a reference is obtained as the second calibration value.

  As a result, not only the sensor calibration but also the manipulator M can be easily calibrated anywhere by using the jig G having a simple configuration having only the linear portion 32. In addition to greatly reducing the number of work steps such as jig preparation, the manipulator M can be calibrated immediately in an environment where a special jig cannot be prepared. Unnecessary waiting time and man-hours for carrying out can be reduced.

  (2) In the calibration method of the present embodiment, since the simultaneous equations are linearized to be a primary approximation, the calculation performed in the process of the third step can be simplified and is not linearized to be a primary approximation. In comparison, the calculation time can be shortened.

  (3) In the calibration method of the present embodiment, the first calibration value and the second calibration value C are each composed of a temporary real number (approximate value) and a sum of error variables. In the third step, When the first calibration value and the second calibration value C are obtained, respectively, the temporary real number is updated to the obtained first calibration value and the second calibration value C, and then the error variable is equal to or less than a predetermined threshold value. The process of the third step is repeated until it becomes.

As a result, according to the present embodiment, an error occurs when the simultaneous equations are linearized. However, this error can be reduced to a threshold value or less, and the accuracy of the calibration value can be ensured.
(4) In the calibration method of the present embodiment, the jig G has a plurality of ridge lines provided between the plurality of surfaces, and the straight line portion 32 is configured by one ridge line among the plurality of ridge lines. ing. As a result, since the ridge line of the jig G is the straight portion 32, a special jig is not necessary, and an existing jig having a straight portion such as a rectangular parallelepiped or a flat plate between the surfaces can be used, or simply A jig with a simple shape is better, and a jig with a special shape for calibration is not required.

  (5) In the robot control system 10 of the present embodiment, the CPU 20 of the robot control device RC detects a plurality of points on the linear portion 32 of the jig G arranged around the manipulator M as a control means. A plurality of observation positions / postures of the manipulator M are different from each other.

  Further, the storage unit 23 of the robot control device RC serves as storage means, and the laser displacement sensor LS detects a plurality of different points on the linear part 32 of the jig G for each of a plurality of different observation positions and orientations of the manipulator M. The value was memorized. Further, the CPU 20 serves as a detection point coordinate calculation means as a laser displacement sensor based on the origin position and orientation of the J6 reference coordinate system (manipulator tip coordinate system) based on the robot coordinate system (reference coordinate system) and the tip of the manipulator M. It is a function with the origin position and orientation of the coordinate system (sensor coordinate system) of the laser irradiation unit Lsa of the LS as an unknown variable, and the robot coordinate system (reference coordinate system) coordinates of the detection point on the straight line portion 32 are the observation position and orientation. I asked for it every time. Furthermore, the CPU 20 uses, as a calibration value calculation means, two detection points on the straight line portion 32 obtained for each observation position and orientation as one set, and functions Pi and Pj from a plurality of sets as elements. A plurality of unit direction vectors of the straight line portion 32 based on the robot coordinate system (reference coordinate system) are calculated. Further, the CPU 20 uses the calculated unit direction vector of the straight line portion 32 and the unit direction of the straight line portion 32 represented by the vector variable (xl, yl, zl) of the robot coordinate system (reference coordinate system) as a calibration value calculation means. A simultaneous equation is created using the fact that the vectors L are equal. Further, the CPU 20 obtains a first calibration value related to the manipulator M by a least square method as a calibration value calculation means, and also the origin position / posture of the coordinate system (sensor coordinate system) of the laser irradiation unit Lsa with the tip of the manipulator M as a reference. C is determined as the second calibration value.

As a result, the robot control system 10 of the present embodiment can provide a robot control system that executes the calibration method (1).
(6) In the robot control system 10 of the present embodiment, the CPU 20 (calibration value calculation means) calculates the calibration value C after linearizing the simultaneous equations into a linear approximation. As a result, the robot control system 10 can simplify the calculation performed in the process of the third step, and can execute the calibration method that can shorten the time required for the calculation as compared with the case where the calculation is linearized and not the primary approximation formula. A control system can be provided.

  (7) In the robot control system 10 of the present embodiment, the CPU 20 (calibration value calculation means) assumes that the first calibration value and the second calibration value C are each composed of the sum of a temporary real number and an error variable, When the first calibration value and the second calibration value C are respectively obtained, the provisional real number is updated to the obtained first calibration value and the second calibration value C, and then the error variable is equal to or less than a predetermined threshold value. The process was repeated until.

As a result, according to the present embodiment, an error occurs when the simultaneous equations are linearized. However, this error can be reduced to a threshold value or less, and the accuracy of the calibration value can be ensured.
(8) It should be noted that many of the prior arts are intended for two-dimensional image information, three-dimensional sensors, etc., and sensor calibration for a two-dimensional laser sensor has not been proposed. Patent Documents 1 to 3 are calibration methods using a CCD camera. In these methods, calibration is realized with a laser sensor that can acquire only one point of the Y axis and the corresponding Z axis with respect to the sensor origin. Can not. In contrast, the method and the robot control system 10 of the present embodiment can perform sensor calibration of a two-dimensional laser sensor.

In addition, this invention is not limited to the said embodiment, You may comprise as follows.
In the embodiment, the visual sensor is a two-dimensional laser sensor. However, the visual sensor is not limited to the two-dimensional laser sensor, and may be an imaging unit such as a CCD camera. In this case, in the image obtained by the imaging unit, with respect to an arbitrary pixel, for example, the center position of the captured image is used as the reference, and the pixel point of the straight line portion closest to the center position is determined. If the detection point is used, calibration can be performed in the same manner as in the above embodiment.

  In the above embodiment, the jig G is a flat plate or the ridge line of the rectangular parallelepiped is a straight portion, but the jig G is not limited to a flat plate or a rectangular parallelepiped, and may be a cube. Alternatively, the shape of the jig G is not limited as long as the ridgeline is a straight line. The straight ridgeline may be a straight line portion.

  -Also, the straight line portion is not limited to the ridge line, for example, a straight groove formed on one side surface having a flat surface, for example, the upper surface or a side surface adjacent to the upper surface is formed linearly. And it is good also as a straight part. The groove has a depth that can be detected by the laser displacement sensor LS. Further, the straight portion 32 is not limited to the groove, and may be a protrusion that protrudes from the upper surface and extends linearly along the upper surface in the position direction. In this case, the protrusion has a height that can be detected from the upper surface by the laser displacement sensor LS.

Further, the surface of the jig G on which the linear portion 32 is provided is not limited to the upper surface, and may be a side surface adjacent to the upper surface.
Further, a step portion having a height is provided on a part of the upper surface of the jig G, and the step portion is formed so as to extend linearly in one direction along the upper surface. The shape portion may be a straight portion.

  In the above embodiment, the 6-axis robot has been described. However, the present invention is not limited to the 6-axis robot. The method and the robot control system may be changed to a robot with a small number of axes such as five axes, four axes, etc., fewer than six axes, or a robot with seven axes or more.

In this case, what is necessary is just to change the coordinate system of the manipulator M front-end | tip in Formula (2) of the said embodiment according to the number of axes after a change. For example, in the case of a 7-axis robot, Ti is changed to be the coordinates of the tip of _Ai7, which is the seventh arm from the base member 12,
T7 = A _i1 A _i2 A _i3 A _i4 A _i5 A _i6 A _i7
And it is sufficient. In this case, since the number of unknown variables included in C and L increases, the observation position / posture n may be increased more than the number of observation positions / postures of the 6-axis robot.

The number of detection points is as described above according to the number of axes of the robot.
A positive integer satisfying q> √ (9 + 24p) / 6 may be set.
In the above-described embodiment, the simultaneous equations are linearized by macrolin expansion and the calculation is simplified. However, the solution of the simultaneous equations may be obtained while maintaining the nonlinearity without performing macrolin expansion.

M ... Manipulator, G ... Jig, RC ... Robot controller,
LS ... Laser displacement sensor (visual sensor),
10 ... Robot control system,
20 ... CPU (control means, detection point coordinate calculation means, calibration value calculation means),
21... ROM, 23... Storage unit (storage means), 32.

Claims (7)

A first step of detecting a plurality of points on a linear portion of a jig arranged around the manipulator by a visual sensor provided at a tip of the manipulator in a state where a plurality of different observation positions and postures are taken;
The origin position and orientation of the manipulator tip coordinate system with reference to the reference coordinate system and the origin position and orientation of the sensor coordinate system of the visual sensor with respect to the manipulator tip as an unknown variable, and A second step of obtaining reference coordinate system coordinates of points (hereinafter referred to as detection points) for each of the observation positions and orientations;
When two detection points on the straight line portion obtained for each observation position and orientation are taken as a set, the function is an element from a plurality of sets, and the unit of the straight line portion is based on the reference coordinate system. A plurality of direction vectors are calculated, a simultaneous equation is created using the fact that the calculated unit direction vector of the straight line portion is equal to the unit direction vector of the straight line portion represented by the vector variable of the reference coordinate system, and the least square method To obtain a first calibration value relating to the manipulator, and to obtain an origin position / posture of the sensor coordinate system of the visual sensor as a second calibration value. A manipulator calibration method comprising three steps.   The manipulator calibration method according to claim 1, wherein the simultaneous equations are linearized into a linear approximation. When the first calibration value and the second calibration value are each composed of a temporary real number and a sum of error variables,
In the third step, when the first calibration value and the second calibration value are obtained, respectively, the temporary real number is updated to the obtained first calibration value and the second calibration value, and then the error variable is calculated. The manipulator calibration method according to claim 2, wherein the process of the third step is repeated until a predetermined threshold value or less is reached. The jig has a plurality of ridgelines provided between the plurality of surfaces,
4. The manipulator calibration method according to claim 1, wherein the straight line portion is one ridge line among a plurality of ridge lines. 5. Control means for taking a plurality of different observation positions and postures of the manipulator in order to detect a plurality of points on a linear portion of a jig arranged around the manipulator;
For each of a plurality of different observation positions and postures of the manipulator, a storage means for storing values respectively detected by a visual sensor provided at the tip of the manipulator for a plurality of different points on the linear portion of the jig, and a reference coordinate system It is a function using the origin position and orientation of the manipulator tip coordinate system as a reference and the origin position and orientation of the sensor coordinate system of the visual sensor with respect to the manipulator tip as unknown variables, and a point on the straight line portion (hereinafter, Detection point coordinate calculation means for obtaining a reference coordinate system coordinate (referred to as a detection point) for each observation position and orientation;
When two detection points on the straight line portion obtained for each observation position and orientation are taken as a set, the function is an element from a plurality of sets, and the unit of the straight line portion is based on the reference coordinate system. A plurality of direction vectors are calculated, a simultaneous equation is created using the fact that the calculated unit direction vector of the straight line portion is equal to the unit direction vector of the straight line portion represented by the vector variable of the reference coordinate system, and the least square method And a calibration value calculating means for obtaining a first calibration value related to the manipulator and performing a process for obtaining the origin position and orientation of the sensor coordinate system with the tip of the manipulator as a second calibration value. system.   The robot control system according to claim 5, wherein the calibration value calculation means calculates the calibration value after linearizing the simultaneous equations into a first-order approximation.   The calibration value calculation means assumes that the first calibration value and the second calibration value are each composed of a temporary real number and a sum of error variables, and the first calibration value and the second calibration value are obtained, respectively. The temporary real number is updated to the first calibration value and the second calibration value obtained, and then the process is repeated until the error variable is equal to or less than a predetermined threshold. 7. The robot control system according to 6.

Images