JP5011525B2 - Inverse kinematics of 6-degree-of-freedom robot arm by sequential search method, and robot system, control method, and program using the same - Google Patents

Description

  The present invention relates to a robot control technique, and more particularly, a robot system and a robot control for performing motion control of an articulated robot arm using inverse kinematics of a six-degree-of-freedom robot arm according to the sequential search method of the present invention. It relates to a method and a program.

  In recent years, robot control technology has progressed with the advancement of computers. In response to this, the movements required of robots have become more complex and sophisticated, and the accuracy has become higher and higher. is needed. For example, a 6-degree-of-freedom robot arm is known as a robot that requires complicated motion control. FIG. 10 shows a model of a 6-DOF robot arm known so far.

  In general, the control of the robot arm is to obtain the angle, angular velocity, and angular acceleration of the angular joint when the position and posture of the hand (also in the case of velocity and acceleration) are given. (Find inverse kinematics solution.)

  It is known that the inverse kinematic solution of the axis configuration in which the axes of the three joints of the hand represented by the PUMA type robot arm shown in FIG. 10A intersect at one point can be obtained analytically.

  However, in the 6-DOF robot arm having an arbitrary joint configuration shown in FIG. 10B and the RPY-type 6-DOF robot arm shown in FIG. However, this gave structural constraints to practical robot arms.

  In the case where an analytical solution cannot be obtained for the 6-degree-of-freedom robot arm having the structure shown in FIGS. Used. FIG. 11 shows a general example of processing for determining a parameter using an inverse kinematic method.

FIG. 11A shows a model of a 6-DOF robot arm, and FIG. 11B includes the hand placement of the 6-DOF robot arm shown in FIG. Use a formulation that uses determinants to give by parameters. As shown in FIG. 11 (b), in the 6-degree-of-freedom robot arm, when the inverse kinematics method is used, the hand placement is 6 × 6 which converts the rotation angle of each joint and the translation position and placement of the hand. It can be formulated as a determinant with a square matrix. Conventionally, an approximation of the Jacobian inverse matrix J ⁻¹ is calculated by repeatedly calculating using a numerical solution method such as the Newton-Raphson method, and then a conversion matrix for obtaining the position and orientation of the calculated result. A complex process was used to obtain an approximate solution of the transformation matrix V- ¹ that was used to obtain V- ¹ .

  By the way, the equation of motion shown in FIG. 11B gives not a sparse numerical element but a nonlinear element including parameters of position and orientation. It is relatively easy to calculate a Jacobi inverse matrix of 3 × 3 elements up to 3 degrees of freedom, and the time and convergence predictability required to acquire the desired hand placement (hereinafter referred to as calculation scalability) .) Can also be given.

  However, as described in Non-Patent Document 1 and Non-Patent Document 2, the inverse kinematic method has to use the above-described transformation matrix V including a non-linear element, so that the degree of freedom of the robot arm is increased. As a result, the calculation becomes more complicated than the usual diagonalization of the determinant. In addition, there is a problem that the scalability of the calculation cannot be guaranteed, for example, a singularity may occur during the execution of the calculation, resulting in an error termination.

  For the reasons described above, the control method for controlling the motion control of an industrial 6-degree-of-freedom robot arm using inverse kinematics has been limited to the PUMA type robot arm. Also, the inverse kinematics method, which is simply an extension of the conventional numerical analysis method such as Newton-Raphson method, allows the inverse kinematics of multi-degree-of-freedom robot arms including 6-degree-of-freedom robot arms with arbitrary joint structures. In order to solve the problem, there was a problem due to the scalability of the hand placement determination and the accuracy.

In addition to this, the determinant appearing in inverse kinematics, decomposed into position matrix V _p and posture matrix V _o, calculated the inverse matrix separately for the location matrix V _p and posture matrix V _o, calculated A numerical method for obtaining an inverse kinematic numerical solution of a 6-degree-of-freedom robot arm by performing an iterative calculation that feeds back the value of the nonlinear element using the determined position and orientation (hereinafter referred to as an efficient solution). ) Has been proposed. An efficient solution can give a numerical solution that gives a certain accuracy relatively fast compared to the Newton-Raphson method. However, the efficient solution has a problem that the error and relative weight between the position matrix and the attitude matrix cannot be evaluated during the calculation. For this reason, it does not converge depending on the initial arrangement and initial value setting, and there may be cases where the solution for the target arrangement at hand is not obtained within the required accuracy at a stretch, which is not sufficient in terms of scalability and accuracy of calculation convergence The problem was still not solved.

  Furthermore, the above two solutions cannot guarantee the existence and number of solutions for any target position / orientation, and the reliability of the solutions is not sufficient, and more than 8 solutions are effective solutions. It was pointed out that it could not be found stably.

  On the other hand, the control of a 6-DOF robot arm is also described in Patent Document 1. In the method described in Patent Document 1, instruction information is input to a 6-degree-of-freedom robot arm, and the joint turning angle is calculated in response to the determination of whether or not the posture is constant, and reflected in the joint motion. The process to make it.

In this regard, although it is possible to use an inverse kinematic method, it does not disclose how to calculate the turning angle, nor the convergence and scalability of the calculation of the turning angle, and the posture is constant. Therefore, there is a problem that applicability is limited to the motion of the robot arm. In addition, although the patent document 2, patent document 3, patent document 4, patent document 5, and patent document 6 disclose the control method of the articulated robot, the essential problems included in the numerical analysis of the articulated robot arm are improved. There is no disclosure of how to do this.
Ono Kyoo, “Mechanical Mechanics in the Mechatronics Era”, Baifukan 1999, Chapter 4 (pp.89-91) Masaharu Takano: Anew effective solution for inverse kinematics problem (synthesis) of a robotwith any type of configuration, J. of Dynamic Systems, Measurement, Control, 102, pp.69-76, 1980. JP-A-2005-193306 JP 2005-246547 A JP 2005-238411 A JP 2005-131769 A JP 2004-009172 A JP 2003-340754 A

  In other words, it has been necessary to provide a novel inverse kinematic algorithm for the position / posture of the hand to control the position / posture of an industrial robot arm having multiple degrees of freedom.

  In addition, to date, structural restrictions such as selecting a PUMA type have been imposed on the mechanical design of industrial robot arms with multiple degrees of freedom due to computational algorithms. It was. Therefore, it can be widely applied to the mechanism of 6-DOF robot arm, improve the convergence of the inverse kinematics solution, improve the control calculation scalability and convergence speed, and more than the analytically equivalent solution There was a need for a novel algorithm that could provide a numerical solution.

  The present inventor has conducted studies in view of the above-described problems of the prior art. As a result, it is difficult to solve the transformation matrix obtained from the inverse kinematics of a 6-DOF robot arm having an arbitrary structure as it is. In spite of this, the present invention has been achieved by focusing on the fact that it is possible to solve the inverse kinematics relating to the position and posture of three degrees of freedom such as the position matrix and the posture matrix from the viewpoint of computational scalability and accuracy. .

That is, the present invention is, respectively to calculate an inverse matrix of the two sub-matrices of the position matrix V _p and orientation matrix V _o of 3 joints corresponding to the target position and attitude of the end, Synthesis and sixth joint of the two inverse matrix The inverse kinematic matrix V ⁻¹ is evaluated to evaluate the error during the iterative calculation and to calculate the weighting factor, and the weighting factor is fed back to the iterative calculation of the two inverse matrices for the position and orientation described above. The nonlinear relationship of the inverse kinematics of the three joints with respect to each of the translational position and posture is sequentially reflected in the numerical calculation. This method is defined as a sequential search method in the present invention. In addition, the present invention introduces a weighting factor that is determined according to the magnitude of the calculated error, and satisfies the target position / posture of the hand of the articulated robot arm in terms of calculation scalability, convergence speed, and accuracy. While finding an approximate solution of inverse kinematics.

That is, according to the present invention, a robot system including a multi-joint robot arm with 6 degrees of freedom and a computer device for controlling the multi-joint robot arm, the computer device includes:
Wherein the transformation matrix giving the hand placement of the articulated robot arm, decomposes into two parts a 3 × 3 matrix that gives the translational position and orientation of the hand placement, inverse matrix calculation for calculating an inverse matrix of the partial matrix respectively Means,
Error calculating means for calculating an error between the hand placement calculated using the inverse matrix of the partial matrix and the target hand placement;
Weighting factor calculating means for calculating a weighting factor used for iterative calculation as an inverse sign of the derivative with respect to the weighting factor of the error so as to reduce the error calculated by the error calculating means by iteration ;
Applying the weighting factor to the calculated hand placement and updating the hand placement until the magnitude of the error between the updated hand placement and the target hand placement is less than or equal to a set value Hand placement updating means for correcting the rotation angle of the joint that gives the updated hand placement;
Transmission means for transmitting the hand placement as control data of the articulated robot arm in response to the magnitude of the error between the updated hand placement and the target hand placement being equal to or less than a set value; A robot system can be provided.

  The weight coefficient calculation means of the present invention uses a weight dynamic system in which the magnitude of the absolute value of the error decreases with respect to the number of iterations of the hand placement update means, and the error in response to the increase in the number of iterations. A process for decreasing the absolute value of can be executed.

  In the present invention, it is possible to predict the existence of a numerical solution that gives the hand placement by using the inner product with the joint disposed on the hand side as an evaluation value.

  In the present invention, a non-volatile storage means for storing the weight coefficient can be provided.

According to the present invention, there is provided a robot control method in a robot system including a robot having a multi-joint robot arm with 6 degrees of freedom and a computer device, the computer device comprising :
Wherein the transformation matrix giving the hand placement of the articulated robot arm, decomposes into two parts a 3 × 3 matrix that gives the translational position and orientation of the hand placement, inverse matrix calculation for calculating an inverse matrix of the partial matrix respectively Means,
Error calculating means for calculating an error between the hand placement calculated using the inverse matrix of the partial matrix and the target hand placement;
Weighting factor calculating means for calculating a weighting factor used for iterative calculation as an inverse sign of the derivative with respect to the weighting factor of the error so as to reduce the error calculated by the error calculating means by iteration ;
Applying the weighting factor calculated by the weighting factor calculating means to the calculated hand placement to update the hand placement and to calculate the magnitude of the error between the updated hand placement and the target hand placement A hand placement updating means for correcting the rotation angle of the joint that gives the hand placement updated until the length becomes equal to or less than a set value;
Transmission means for transmitting the hand placement as control data of the articulated robot arm in response to the magnitude of the error between the updated hand placement and the target hand placement being equal to or less than a set value;
A method is provided that functions as:

  The weight coefficient calculation means of the present invention uses a weight dynamic system in which the magnitude of the absolute value of the error decreases with respect to the number of iterations of the hand placement update means, and the error in response to the increase in the number of iterations. A process for decreasing the absolute value of can be executed.

  In the present invention, it is possible to predict the existence of a numerical solution that gives the hand placement by using the inner product with the joint disposed on the hand side as an evaluation value.

  In the present invention, it is possible to read out the weighting coefficient calculated before the process from the non-volatile storage means for storing the weighting coefficient and execute the initial setting process of the weighting coefficient.

  According to the present invention, there is provided a computer executable program for executing any one of the robot control methods described above.

  According to the present invention, the problem that the convergence and the convergence speed of the inverse kinematics solution are slow is corrected by using the appropriate weighting factor for converging the convergence of the solution in each iteration loop. Prevents solution divergence by performing sequentially. At the same time, the weighting factor is set to reduce the error in response to an increase in the number of iterations of the iteration loop, and the convergence speed can be improved.

  Furthermore, in the present invention, the angle of the joint closest to the hand is used as a variable, and a solution evaluation function using the position and posture of the two joints close to the hand is used, and the existence of an inverse kinematic solution for an arbitrary target position / posture. And the number can be confirmed at high speed, and can be fed back to the control of the hand placement of the robot arm.

  Furthermore, according to the present invention, by using the evaluation value, a solution more than twice the number of joints that can be used for position control of the three joints for position correction in the sequential search method and the three joints for posture correction is provided. It is possible to provide a more flexible robot system, robot control method, and program.

<Section A: Robot Arm Embodiment>
FIG. 1 shows an embodiment of a robot arm used in the present invention. FIG. 1A is a side view of the 6-degree-of-freedom robot arm 10, and FIG. 1B is a joint model diagram showing the robot arm. As shown in FIG. 1A, the robot arm 10 is configured as a six-degree-of-freedom robot arm, and is installed on the fixed portion 12 via the first joint 14, from the first joint, An arm 28 extends to the second joint 16. An arm 30 extends from the second joint 16 to the third joint. An arm 32 extends from the third joint 18 to the fourth joint 20. The second joint and the third joint are bending joint links having a rotation axis in a direction orthogonal to the axes of the arms 28 and 30, and the first joint 14 and the fourth joint 20 are centered on the axis of the arm 32. It is formed from a twisted joint link having a rotating shaft.

  From the fourth joint, the arm 34 further extends to the fifth joint 22, and the arm 36 further extends from the fifth joint to the sixth joint 24. A work 26 is disposed at the tip of the sixth joint via an arm 38, and various processes can be performed by the robot arm. The work 26 referred to as the hand at the tip portion provides the hand placement of the present invention, and the translation position and posture of the work 26 are controlled by the present invention. Further, as shown in FIG. 1B, the robot arm shown in FIG. 1A can describe the movement of the hand in cylindrical coordinates, and the position and posture of the hand can be determined by a roll angle. , Pitch angle, yaw angle variable. The robot arm 10 shown in FIG. 1 is a mechanism that moves more efficiently for surface contact work than a PUMA type robot arm.

In FIG. 1 (b), the rotation angle around the rotation axis of each joint is shown as θ _i (i = 1,..., 6), and the axis in the Cartesian coordinate system indicating each rotation axis. Is shown as (x, y, z). The PUMA type 6-DOF robot arm is different in that the fourth joint, the fifth joint, and the sixth joint are described not in the Euler coordinate system but in the cylindrical coordinate system.

<Section B: Robot arm model parameters and numerical formulation>
In the following, formulation for numerical analysis of the robot arm shown in FIG. 1 is performed. First, the position and arrangement of the RPY type robot arm in the three-dimensional space are formulated by the following equation (1).

上記式中、Ｐｘ, Ｐｙ,Ｐｚ, Ｏｘ, Ｏｙ,Ｏｚは、手先の目標配置を表し、Ｐｘ、Ｐｙ、Ｐｚは、手先の並進位置であり、Ｏｘ、Ｏｙ、Ｏｚは、手先の目標姿勢を表し、θ_ｉは、当該関節の回転軸周りの回転角を表し、Ｖは、並進・回転を含む座標変換行列である。

In the above formula, Px, Py, Pz, Ox, Oy, Oz represent the target arrangement of the hand, Px, Py, Pz are the translational positions of the hand, and Ox, Oy, Oz represent the target posture of the hand. Where θ _i represents a rotation angle around the rotation axis of the joint, and V represents a coordinate transformation matrix including translation and rotation.

As shown in the above equation (1), when the target arrangement of the hand is input or set, the robot arm 10 determines a value for rotating the joint, and the determined value is used as control data for the driving element of the robot arm. Are transmitted to each driving element, and a stepping motor, a hydraulic motor, or the like is driven until the driving element reaches a determined value, and the hand placement is moved to the target position and target posture. In FIG. _{1 (b), P i (} i = 1, ..., 7) represents the translational position of each joint, the target position is defined as _{P 7, P 7,} the pitch angle of the sixth articulation It is determined by designating the roll angle, yaw angle and the length of the arm 38. In addition, the desired posture of the hand is given by O _7.

That is, in inverse kinematics, vectors (θ ₁ , θ ₂ , θ ₃ , θ ₄ ) giving vectors (Px, Py, Pz, Ox, Oy, Oz) given by the relationship formulated as the above equation (1) are given. , θ ₅ , θ ₆ ), and generally results in the problem of obtaining the inverse matrix V ⁻¹ of the transformation matrix V numerically. However, since the transformation matrix V is not a matrix given by sparse numerical elements, even if it is a square matrix, it is not an element of an inverse matrix numerically by the Netwon-Raphson method, for example, instead of the Householder diagonalization method. Is determined.

Conventionally, a numerical solution of inverse kinematics J ⁻¹ is obtained by applying the Newton-Raphson method as it is to the Jacobian matrix J that gives a 6 × 6 transformation matrix given by the following equation (2). The solution is used to calculate a transformation matrix V ⁻¹ for further providing the target placement, and the target placement is determined.

  The present invention replaces the problem of calculating an inverse matrix of a 3 × 3 submatrix that gives a target position vector (Px, Py, Pz) and target posture vector (Ox, Oy, Oz) of a 6-DOF robot arm. If the inverse matrix of the 3 × 3 submatrix is used, it can be guaranteed that the calculation converges with sufficient speed and accuracy even if the Newton-Raphson method is used as in the past.

FIG. 2 shows a basic algorithm of the inverse matrix approximation method used in the present invention. In the present invention, the calculation of the 6 × 6 inverse matrix shown in FIG. 2A is replaced with the calculation of the inverse matrix of 3 × 3 partial matrices V _p and V _o as shown in FIG. 2B. Reduce the inverse matrix calculation process. For this reason, in the present invention, the inverse kinematics method and scalability using a 3 × 3 matrix, which has been conventionally performed, can be used for position control of a 6-DOF robot arm. In FIG. 2B, the subscripts i, j, k, m, n, and q are integers indicating joints used in the calculation. In a specific embodiment of the present invention, i = 1, j = The submatrix is calculated as 2, k = 3, m = 4, n = 5, and q = 6. In the present invention, the combination is particularly limited if it is a submatrix of 3 × 3 elements or less including joints that are continuous with each other. It is not limited.

<Section C: Approximate solution by weighted sequential iteration>
Since the target position and target posture given by the partial matrices V _p and V _o are missing the cross-term portion of the position / posture, the accuracy is reduced by eliminating the cross-term portion and the partial matrices V _p and V _o are removed. Is introduced as a weight between V _p and V _o .

The weighted sequential iteration used in the present invention introduces weighting factors W _p and W _o for V _p and V _o to ensure that the magnitude of the error from the target placement of the hand decreases with respect to the number of iterations. . The error from the target placement of the hand is given by the following equation (3).

上記式中、^ＯＰ_７ｒｅｆおよび^ＯＯ_７ｒｅｆは、それぞれ目標配置の並進位置および姿勢の値であり、^ＯＰ_７および^ＯＯ_７は、Ｖ_ｐおよびＶ_ｏから得られた並進位置および姿勢の値である。

In the above formula, ^O P _7ref and ^O O _7ref are values of the translation position and orientation of the target arrangement, respectively, and ^O P ₇ and ^O O ₇ are values of the translation position and orientation obtained from V _p and V _o , respectively. It is.

  In the present invention, in order to further cancel the directionality of the error and improve the convergence, the weight given by the following formula (4), which gives the absolute size of the error and further includes the power of the error from the viewpoint of convergence Introduce a generation function.

上記式中、Ｓは、収束性を考慮し、１以上の実数から選択することができ、本発明の特定の実施形態では、Ｓ＝２が用いられる。なお、本発明では、上述した作用を与える限り、重み生成関数の形式については特に限定されるものではない。さらに、目標配置を得るための反復計算で、重み生成関数の値が反復計算において減少するように各重みＷ_ｐ、Ｗ_ｏを設定する重み力学系を、下記式（５）で導入する。

In the above equation, S can be selected from one or more real numbers in consideration of convergence, and S = 2 is used in a specific embodiment of the present invention. In the present invention, the form of the weight generation function is not particularly limited as long as the above-described action is given. Further, a weight dynamic system for setting the weights W _p and W _o so that the value of the weight generation function decreases in the iterative calculation in the iterative calculation for obtaining the target arrangement is introduced by the following equation (5).

上記式（５）中、α_ｐおよびα_ｏは、負の定数を表し、反復計算における反復処理の変化分を規定する値に設定することができる。上記式（５）からわかるように、誤差は、反復回数Kに対して非増加とされる。このため、本発明で１以下の適当な初期値を重みを設定して反復計算を開始させることで、確実に誤差Eの極小点に停留させることが保証され、計算スケーラビリティを提供することができる。また、上記式（５）の重み力学系にしたがえば、逆に誤差を最小にする重みについても決定できることになる。また、本発明で一旦決定された重み係数を、不揮発性メモリなどに格納させておき、同様の目標位置および目標姿勢が指定された場合、反復計算の初期から不揮発性メモリに格納しておいた重みを読み込んで初期値として使用することにより、より高速な収束性を与えることができる。

In the above formula (5), α _p and α _o represent negative constants and can be set to values that define the amount of change in the iterative process in the iterative calculation. As can be seen from the above equation (5), the error is not increased with respect to the number of iterations K. Therefore, in the present invention, by setting the weight to an appropriate initial value of 1 or less and starting the iterative calculation, it is ensured that the error E is stopped at the minimum point, and calculation scalability can be provided. . Further, according to the weight dynamic system of the above equation (5), the weight that minimizes the error can be determined. In addition, the weighting factor once determined in the present invention is stored in a nonvolatile memory or the like, and when a similar target position and target posture are specified, the weighting factor is stored in the nonvolatile memory from the beginning of the iterative calculation. By reading the weight and using it as an initial value, it is possible to provide faster convergence.

<Section D: Robot System Control Method Using the Control Processing of the Present Invention>
FIG. 3 shows a flowchart of processing of the robot control method of the present invention. The process shown in FIG. 3 starts from step S100, acquires values of the target position and target attitude, and sets initial values of W _p and W _o . The initial values of W _p and W _o can be selected and used each time a numerical value of 1 or less, or the values of W _p and W _o previously calculated for the target arrangement set at that time Can be assigned, W _p and W _o are retrieved from the set target position and target posture values, read out, and set as the weighting coefficient value.

In step S101, the initial values of the rotation angle of the joint, in step _{S102, θ 1, θ 2,} Px from 3 inverse matrix of freedom of _{V p} using the submatrix comprising theta _3, Py, Pz The value of (P ₇ ) is calculated, the error between the value calculated from the inverse matrix of V _p and the target position is calculated using the above equation (3), and the error value is expressed by the above equation (5). The new position ^o P _7new is calculated by multiplying the weighting coefficient Wp obtained from the error value given in, and simultaneously the values of θ ₁ , θ ₂ , and θ ₃ are updated.

In step S103, Ox, Oy, Ox (O ₇ ) is calculated from the inverse matrix of V _o with 3 degrees of freedom using a submatrix including θ ₄ , θ ₅ , and θ ₆ , and the above equation (3) is used. Then, an error between the value calculated from the inverse matrix of V _o and the target posture is calculated, and in the same manner as the position calculation, a new posture value ^o O _7new is calculated, and at the same time θ ₄ and θ ₅ , Θ ₆ is updated.

  In step S104, it is determined whether or not the error value is equal to or less than the set threshold value. If it is determined that the error value is equal to or less than the threshold value (yes), the process proceeds to step S105, and the iterative calculation process is stopped. Thereafter, the driving elements of the robot arm are controlled so that the acquired parameters of each joint are obtained, and the hand placement of the robot arm is set as the target position and target posture.

If it is determined in step S104 that the error is equal to or greater than the threshold value (no), the obtained values of θ _i are used to repeat the processing in steps S102, S103, and S104 again until calculation is completed. , And control data corresponding to each converged parameter is transmitted to the drive element to control the robot arm.

<Section E: Evaluation of Numerical Solution by Control Method of the Present Invention>
In the present invention, in order to minimize the error with respect to the target arrangement, the 6 × 6 matrix V is replaced with the calculation of the inverse matrix of the 3 × 3 submatrices V _p and V _o , and the iterative calculation is performed. The number of solutions obtained in each case was examined.

FIG. 4 shows a joint model used to examine the number of solutions existing in a robot system using the RPY type 6-degree-of-freedom robot arm 10. Note that the definitions of P _i and O _i are the same as those described with reference to FIG. The joint model of the 6-degree-of-freedom robot arm 10 is expressed as shown in FIG. 4, and the vector e _{5z in} the z-axis direction of the fifth joint and the z-axis of the fourth joint are set with the rotation angle θ ₆ of the sixth joint as a variable. A direction vector e _4z is calculated. At this time, the value of P ₅ and e _5z are directly calculated from the values of P ₆ , θ ₆ , and L ₆ . Further, e _4z is calculated with the position and orientation of P ₄ as the target using inverse kinematics of 3 degrees of freedom using the values of the first joint, the second joint, and the third joint with P ₅ as the target position. To do. At this time, a solution evaluation function (θ ₆ ) using the angle θ ₆ of the sixth joint as a variable is introduced by the following equation.

上記式中、「・」は、ベクトルの内積を表す。本発明の実施形態であるＲＰＹ型の６自由度ロボットアームでは、第４関節と第５関節とは、垂直関係が保存される。このため、解評価関数の値ｅ_５ｚは、０となり、これを与えるθ_６が存在するかどうかを、θ_６の値を設定して、第１関節、第２関節、第３関節の逆運動学行列を数値解析することにより、目標位置および目標姿勢を与える解の存在を評価することができる。図５に横軸をθ_６の値[rad]とし、縦軸に解評価関数の値をプロットした結果を示す。図５の実線および破線は、θ_６の角度の位相を反対として計算した結果に対応する。図５に示されるように、本発明の６自由度のロボットアームでもＰ_１〜Ｐ_４の関節の運動により与えられる４つの解に対して右手系および左手系に対応する８つの解が与えられることが示されている。ただし、一般的なマニピュレータ・アームの軸構成では、０とならないことが多いが、本発明の解評価関数について、特定のロボットアームの構成について適切な評価値を使用することにより、解の存在を迅速に判断することができる。

In the above formula, “·” represents an inner product of vectors. In the RPY type 6-degree-of-freedom robot arm according to the embodiment of the present invention, the vertical relationship is preserved between the fourth joint and the fifth joint. For this reason, the value e _5z of the solution evaluation function is 0, and whether or not there is θ ₆ that gives this is set as the value of θ ₆ and the inverse motion of the first joint, the second joint, and the third joint. By numerical analysis of the academic matrix, it is possible to evaluate the existence of a solution that gives the target position and target posture. FIG. 5 shows the result of plotting the value of the solution evaluation function on the vertical axis, with the horizontal axis being the value of θ ₆ [rad]. The solid line and the broken line in FIG. 5 correspond to the results calculated by inverting the phase of the angle of θ ₆ . As shown in FIG. 5, the six-degree-of-freedom robot arm of the present invention provides eight solutions corresponding to the right-handed system and the left-handed system with respect to the four solutions given by the movements of the joints P _{1 to} P _4. It has been shown. However, although the axis configuration of a general manipulator arm is often not 0, the existence of a solution can be determined by using an appropriate evaluation value for a specific robot arm configuration for the solution evaluation function of the present invention. Judgment can be made quickly.

  Furthermore, in the present invention, since there are two phase solutions with different coordinate system representations for the fifth joint and the sixth joint, the point where the value of the solution evaluation function (evaluation value) becomes zero is reversed. From the kinematic point of view, there are two overlapping solutions. For this reason, the control method of the present invention can guarantee 8 × 2 = 16 solutions for the target position and target posture of the 6-DOF RPY robot arm, and the conventional 6-DOF robot arm reverse motion It is shown that it is possible to find many solutions more than twice the number of joints compared to the case of using a control method using a science.

<Section F: Robot System>
FIG. 6 is a diagram showing an embodiment of the robot system of the present invention. The robot system 40 of the present invention is controlled by a computer device 44, which is programmed under a programming language such as assembler, Fortran, COBOL, C, C ++, etc. A program that applies the method is implemented. The computer device 44 determines the optimal numerical solution using the inverse kinematics method of the present invention to provide a robot control sequence or a target placement given by user input, such as from a mouse 50 or keyboard 46, and the robot Control data for controlling the hand placement of the arm 10 is transmitted to the robot arm 10 via the bus line 54.

  The bus line 54 is connected to the robot arm 10 via an interface means including software / hardware such as a general-purpose interface I / F 52 such as GP-IB and a hardware interface that issues a command to a driving element. Yes. The control data activates driving elements such as a step motor and a hydraulic motor for driving the robot arm, and controls the movement of the hand arrangement of the robot arm 10 to the target arrangement.

In the present invention, the program for executing the processing can be stored in the hard disk drive (HDD) 48. When the program is executed, the computer device 44 reads out the program from the HDD 48 as a nonvolatile storage means. Execute the process. Further, the computer device 44 may be provided with an external or built-in nonvolatile storage means such as an EEPROM (not shown), an EPROM (not shown), or a flash memory separately from the HDD 48, and once converges. Weighted weights W _o and W _p can be stored. When the weighting factor is stored in the non-volatile storage means in the present invention, the computer device 44 stores the value of θ _i when the weighting factor is calculated and the target hand arrangement at the same time. Can do.

When the target value of the hand placement specified at that time is approximate and the current arm placement is close to θ _i when the weight coefficient is obtained, the computer device 44 determines that the current robot arm The stored optimum weighting factor is searched using the position and the target arrangement data, and the searched weighting factor can be set as a default weighting factor. In the case of this embodiment, iterative calculation can be started from a more optimized weighting factor based on the past history, so that a new position and posture of the robot arm can be determined more efficiently. .

  Note that the computer device 44 used in the present invention can be configured as a general-purpose personal computer or a workstation, or is mounted on a robot system as an ASIC (Application Specific Integrated Circuit) for the purpose of control only. be able to.

  FIG. 7 is a functional block diagram of the computer device 44 of the present invention. As shown in FIG. 7, the computer device 44 is equipped with a control processing module 56 that executes the processing of the present invention, and the control processing module 56 includes IDE (International Device Electronics), SCSI (Small Computer System Interface), and the like. This is realized by a CPU (not shown) calling a control program from the HDD 48 connected via the interface, and expanding the program to the RAM 60 that provides an execution space to execute the program.

  The computer device 44 includes a ROM 58 and the like. The ROM 58 stores a basic input output system (BIOS) and the like, and passes data used for initial setting and control of the computer device 44 to the CPU. . When the computer device 44 of the present invention is configured as a workstation or server, a computer having a UNIX (registered trademark) or LINUX (registered trademark) architecture that can be described as a kernel / OS / application configuration. A device 44 can be used.

  In the computer device 44, the target placement setting unit 66 receives the target placement target setting value given as the output of the sensor / keyboard / sequence control program and the initial value, and the target placement setting unit 66 receives the parameters. And the subsequent control processing module 56 can be used. When the processing is started, the control processing module 56 calculates the numerical elements of the submatrix corresponding to the position matrix and the attitude matrix from the array data describing the transformation matrix V, and activates the inverse matrix calculation means 68 to start Newton- The inverse matrix is calculated using Raphson method or the like, and the calculated hand placement value is passed to the error calculation means 70.

  The error calculation means 70 calculates the error between the calculated hand placement and the target placement and passes it to the weight coefficient calculation means 72. The weighting coefficient calculating means calculates a weighting coefficient by applying a weight generation function and passes it to the hand placement updating means 74. The hand position updating means 74 updates the hand placement using the calculated error and weighting factor, and if the updated value is not within the set accuracy, returns the result to the inverse matrix calculating means 68. Continue updating the hand placement. On the other hand, if the updated value is within the set accuracy, the value is transmitted to the robot arm (not shown) via the interface I / F 52 as control data for the robot arm.

  The weighting coefficient calculated when it converges within the set accuracy is stored in the storage device 62 composed of flash memory, EEPROM, EPROM, etc., and data can be stored later for processing of the control processing module. .

  In FIG. 7, the I / F 52 is described as being arranged outside the computer device 44. However, when the interface I / F 52 is configured using, for example, GP-IB, the GP- The IB board can also be built in the computer device 44. Further, by using an infrared ray or a short-range / far-distance wireless communication interface as the interface I / F 52, it is possible to perform wireless control on the robot.

  In the embodiment of the present invention in which the weight coefficient is stored in the non-volatile storage means, in the conventional Newton-Raphson method, the numerical analysis must always be started from the initial setting for each optimization. The coefficient is stored as an optimization history, and it is possible to control the robot arm that can always reach the target posture and target position in the shortest time from the current posture.

<Section G: Examples>
<Example 1>
The present invention creates a robot system from the RPY 6-degree-of-freedom robot arm shown in FIG. 1 and the computer on which the control program shown in FIG. 3 is installed, and controls the hand, posture control, and existence of solutions Convergence and computational scalability were investigated for the number and the number. In the computer experiment, data giving the target arrangement was given to the computer, the calculation was executed, and the execution result of the iterative calculation was performed by evaluating the error of the target arrangement with respect to the number of the iterative calculations. The calculation was made as Example 1 and Example 2 using two target arrangements, respectively. Moreover, the calculation result by Newton-Raphson (conventional example 1) and the efficient solution (conventional example 2) was used as the comparative example, respectively using the same parameter. The parameters used in the experiment are shown in Table 1 below. Moreover, the result obtained about the precision of the target position is shown in FIG. 8 and FIG. The initial value of the weighting factor W _p used in the present invention was set to 0.7.

FIG. 8 shows the results of Example 1. According to the result shown in FIG. 8, when the control method of the present invention is used, positional accuracy of about 6 × 10 ⁻⁴ m or less (60 μm or less) is given compared to Conventional Example 1 using the Newton-Raphson method. The number of iterations up to is about 17, and it has been shown that the conventional example 1 does not give the position accuracy of the present invention even when the number of iterations is about 23. In addition, when the number of iterations shown in FIG. 8 is expressed by a calculation time until the position accuracy of about 100 μm is given, it is about 1.5 ms in the first example, about 35 ms in the conventional example 1, and about 23 Double convergence speed was improved. The reason for this is that if the number of matrix elements to be calculated decreases, the amount of calculation per iteration is approximately the square of [O] ² , where the number of matrix elements to be calculated is [O]. This is considered to decrease correspondingly. However, in the present invention, it has been shown that the convergence is much improved. The reason for this is that, by introducing a weight dynamic system in the present invention and improving the convergence and scalability at the same time, the number of iterations can be reduced more than the amount of computation, resulting in a decrease in matrix elements. This is because the calculation time is shortened more than the influence of. Similar results were obtained for the target posture.

On the other hand, in the second conventional example using two matrices having 3 × 3 elements, the initial position and the initial attitude are calculated because the calculation is performed independently without considering the mutual relationship between the matrices V _p and V _o. As a result, the calculation did not converge, and the iterative calculation did not end. After the results shown in FIG. 8, no improvement in accuracy was found even when the iterative calculation was repeated up to 500 times.

  From the above results, it was shown that the present invention enables higher-speed control than the conventional one while guaranteeing convergence and computational scalability for the control of the RPY type robot arm.

<Example 2>
In FIG. 9, the result of Example 2 is shown. As shown in FIG. 9, in Example 2, iterative calculation is performed about 12 times (calculation time is about 1 ms) in order to give a position accuracy of about 60 μm, and in Conventional Example 1, the number of iterative calculations is about 25. The result was obtained that the calculation time was about 40 ms, and it was found that about 40 times higher speed can be achieved in the calculation time. Further, in the conventional example 2 using the efficient solution, although the result shown in FIG. 9 gives the same high speed as the present invention, whether or not the calculation converges considering the result of the first example. It can be said that the reliability of the calculation scalability is inferior, and that the calculation speed is lower than that of the present invention from the viewpoint of high-speed calculation.

  From the results of Example 1 and Example 2 described above, according to the present invention, it was shown that the control of the RPY type robot arm can be speeded up while ensuring the convergence of the calculation.

  Although the present invention has been described so far with the embodiment, the present invention is not limited to the embodiment, and an inverse kinematic solution of a submatrix of 3 × 3 elements having 5 or more joints is provided. The present invention can be applied to a 6-DOF articulated robot arm having an arbitrary structure having applicable segments.

  According to the present invention, a controller for position control and posture control of each joint of a conventional industrial robot can be applied to the structure of a 6-DOF robot arm having an arbitrary joint structure. It can be replaced with a higher-speed control device for the three-axis target position and the target posture around the three axes, and the control of the industrial articulated robot arm can be made with higher accuracy and speed.

  In addition, according to the present invention, since more solutions than before can be confirmed by a novel and high-speed method, the optimal solution for position / posture control can be efficiently reached in the control of the articulated robot arm. be able to.

  Therefore, the present invention is an industrial robot such as welding and grinding, disaster rescue robot, aerospace application robot, industrial robot that needs to avoid obstacles such as cables, rubble and other structures, A control method and program for this purpose can be provided, which is an industrially extremely effective invention.

The figure which showed the prior art example of the process which determines a parameter using an inverse kinematics method. The figure which showed the basic algorithm of the inverse matrix approximate solution used by this invention. The flowchart of the process of the robot control method of this invention. The figure which showed the joint model used in order to examine the number of the existence solutions of the solution in the robot system using a 6-degree-of-freedom robot arm of RPY type. The figure which showed the result of having plotted the value of the solution evaluation function on the vertical axis | shaft with the value [rad] of (theta) _{6 on} the horizontal axis. The figure which showed embodiment of the robot system of this invention. The functional block diagram of the computer apparatus of this invention. The figure which showed the result obtained about the precision of the target position. The figure which showed the result obtained about the precision of the target position. The figure which showed the joint model of the conventional 6 degree-of-freedom robot arm. The figure which showed the prior art example of the process which determines a parameter using an inverse kinematics method.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 ... Robot arm, 12 ... Fixed part, 14 ... 1st joint, 16 ... 2nd joint, 18 ... 3rd joint, 20 ... 4th joint, 22 ... 5th joint, 24 ... 6th joint, 26 ... Workpiece | work, 28, 30, 32, 36, 38 ... arm, 40 ... robot system, 42 ... display device, 44 ... computer device, 46 ... keyboard, 48 ... HDD, 50 ... mouse, 52 ... interface (I / F), 54 ... Bus line, 56 ... Control processing module, 58 ... ROM, 60 ... RAM, 62 ... Storage device, 64 ... IDE / SCSI, 66 ... Target arrangement setting unit, 68 ... Inverse matrix calculation means, 70 ... Error calculation means, 72 ... Weight coefficient calculation means, 74 ... Hand arrangement update means

Claims (9)

A robot system comprising a multi-joint robot arm with 6 degrees of freedom and a computer device for controlling the multi-joint robot arm, the computer device comprising:
Wherein the transformation matrix giving the hand placement of the articulated robot arm, decomposes into two parts a 3 × 3 matrix that gives the translational position and orientation of the hand placement, inverse matrix calculation for calculating an inverse matrix of the partial matrix respectively Means,
Error calculating means for calculating an error between the hand placement calculated using the inverse matrix of the partial matrix and the target hand placement;
Weighting factor calculating means for calculating a weighting factor used for iterative calculation as an inverse sign of the derivative with respect to the weighting factor of the error so as to reduce the error calculated by the error calculating means by iteration ;
Applying the weighting factor to the calculated hand placement and updating the hand placement until the magnitude of the error between the updated hand placement and the target hand placement is less than or equal to a set value Hand placement updating means for correcting the rotation angle of the joint that gives the updated hand placement;
Transmission means for transmitting the hand placement as control data of the articulated robot arm in response to the magnitude of the error between the updated hand placement and the target hand placement being equal to or less than a set value; A robot system comprising:   The weighting factor calculating means uses a weight dynamic system in which the magnitude of the absolute value of the error decreases with respect to the number of iterations of the hand placement updating means, and in response to the increase in the number of iterations, the absolute value of the error The robot system according to claim 1, wherein a process for decreasing the number of times is executed.   3. The robot system according to claim 1, wherein the robot system predicts the existence of a numerical solution that gives the hand placement by using an inner product with a joint arranged on the hand side as an evaluation value. 4. Robot system.   The robot system according to claim 1, further comprising a non-volatile storage unit that stores the weight coefficient. A robot control method in a robot system including a robot having a multi-joint robot arm with 6 degrees of freedom and a computer device, the computer device comprising :
Wherein the transformation matrix giving the hand placement of the articulated robot arm, decomposes into two parts a 3 × 3 matrix that gives the translational position and orientation of the hand placement, inverse matrix calculation for calculating an inverse matrix of the partial matrix respectively Means,
Error calculating means for calculating an error between the hand placement calculated using the inverse matrix of the partial matrix and the target hand placement;
Weighting factor calculating means for calculating a weighting factor used for iterative calculation as an inverse sign of the derivative with respect to the weighting factor of the error so as to reduce the error calculated by the error calculating means by iteration ;
Applying the weighting factor calculated by the weighting factor calculating means to the calculated hand placement to update the hand placement and to calculate the magnitude of the error between the updated hand placement and the target hand placement A hand placement updating means for correcting the rotation angle of the joint that gives the hand placement updated until the length becomes equal to or less than a set value;
Transmission means for transmitting the hand placement as control data of the articulated robot arm in response to the magnitude of the error between the updated hand placement and the target hand placement being equal to or less than a set value;
To act as a way.   The weighting factor calculating means uses a weight dynamic system in which the magnitude of the absolute value of the error decreases with respect to the number of iterations of the hand placement updating means, and in response to the increase in the number of iterations, the absolute value of the error The method according to claim 5, wherein a process of decreasing the frequency is executed.   The method according to claim 5, wherein the robot system predicts the existence of a numerical solution that gives the hand placement by using an inner product with a joint arranged on the hand side as an evaluation value.   The method according to claim 5 or 6, wherein a weighting coefficient calculated before the process is read from a nonvolatile storage unit that stores the weighting coefficient, and an initial setting process of the weighting coefficient is executed.   A computer-executable program for executing the robot control method according to any one of claims 5 to 8.

Images