JP2013132731A - Robot control system, robot system and robot control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot control system or the like capable of determining a solution of an inverse kinematics of a robot by an iterative operation method in a more efficient way in a short period of time.SOLUTION: A robot control system includes an endpoint position obtaining unit 20 for obtaining a target endpoint position of a robot 100, a true and correct model storage unit 22 for storing true and correct model information of the robot 100, an approximate model storage unit 24 for storing an approximate model information produced by approximating the true and correct model, and a processing unit 30 for determining an approximate solution using the approximate model and the target endpoint position to determine the solution of the inverse kinematics of the robot 100 by performing the iterative operation based on an iterative method using the approximate solution and the true and correct model.

Description

  The present invention relates to a robot control system, a robot system, a robot control method, and the like.

  The forward kinematics process, which is a process for obtaining the hand position / posture (end point position) from each joint angle of the robot, is an easy process. On the other hand, the process of inverse kinematics, which is a process for obtaining each joint angle from the hand position / posture, is a very difficult process. In particular, when the configuration of the robot arm becomes complicated, inverse kinematics cannot be processed analytically, and it is only possible to use an iterative method that performs successive approximation.

  However, processing of inverse kinematics by an iterative method generally has the following problems.

  First, the processing of inverse kinematics by the iterative method requires a large amount of processing as a whole, and requires a long time for processing. For example, a processing amount of several hundred times to several tens of thousands of times is required as compared with processing for obtaining an analytical solution.

  Secondly, in the inverse kinematic process by the iterative method, it may not converge to the solution after a finite number of iterations. That is, when the processing time and the number of iterations are limited, a solution with a desired accuracy cannot be obtained.

  Thirdly, in the inverse kinematic process by the iterative method, it is difficult to control which solution converges when there are a plurality of solutions that satisfy the condition. For example, when there is no analytical solution, it is difficult to estimate how many solutions exist.

  Fourth, the appropriate solution is highly dependent on the robot structure. In other words, the optimal solution for the robot, including analytical solutions, can only be determined heuristically.

  Fifth, it cannot be determined in advance whether or not there is a solution under the specified conditions. That is, it cannot be distinguished whether the solution simply does not converge or whether there is no solution.

  Sixth, even if a solution exists, whether it converges or not depends strongly on the set initial state. That is, if it starts from a bad initial state, it will not converge or it will take a long time.

  As conventional techniques for improving and solving these problems, for example, techniques disclosed in Patent Documents 1, 2, and 3 and Non-Patent Documents 1 and 2 are known.

JP-A-6-320451 Japanese Patent Application Laid-Open No. 2-205489 JP-A-4-369004

"Study on interpolation method for manipulator inverse kinematics problem" The Society of Instrument and Control Engineers, Tohoku Branch, 215th (May 27, 2004), Document No. 215-15 "Inverse kinematics regardless of solvability by the Levenberg-Marquardt method", 27th Annual Conference of the Robotics Society of Japan, RSJ2009AC2K1-03 (2009)

  The technique of Patent Document 1 is not based on a heuristic technique, but seeks to obtain an inverse kinematic solution efficiently as a general-purpose technique by using a Gröbner basis.

  The method of Patent Document 2 is a general-purpose method that efficiently solves the inverse kinematics solution of a general six-degree-of-freedom robot, and sequentially performs the inverse kinematics of three joints. It is what you want.

  The method of Patent Document 3 introduces a new control variable λ, and uses it to seek an inverse kinematics solution more efficiently.

  In the method of Non-Patent Document 1, the correspondence relationship between the joint angle and the hand position / posture is obtained in forward kinematics in advance, a finite number of combinations thereof are stored, and when the inverse kinematics is processed, the combination is obtained. By selecting an appropriate initial value from the above, the solution of inverse kinematics is obtained more efficiently.

  The method of Non-Patent Document 2 uses a method called Levenberg-Marquardt method to obtain a solution of inverse kinematics more efficiently while substantially guaranteeing convergence.

  However, the technique disclosed in Patent Document 1 has a problem in that when there are a plurality of solutions, the calculation time and memory usage required for the processing are greatly increased. In recent robots, this problem is serious because it is common for a plurality of solutions to exist.

  In the method of Patent Document 2, the target is limited to a robot with 6 degrees of freedom, and there is a problem that it is difficult to apply to other cases. In recent years, many robots have a degree of freedom of 7 or more in order to improve the degree of freedom of posture with respect to the hand.

  The method of Patent Document 3 has a problem in solution selectivity when there are a plurality of solutions. Specifically, in addition to each joint angle, an auxiliary parameter for designating one of a plurality of solutions is often required separately.

  In the method of Non-Patent Document 1, when the degree of freedom of the robot increases, or when the data in advance is made finer in order to improve the accuracy, the memory usage for storing the data is rapidly increased. There is a problem of increasing.

  In the method of Non-Patent Document 2, although the number of iterations is small, there is a problem that processing per iteration becomes complicated and processing time becomes long after all.

  According to some aspects of the present invention, it is possible to provide a robot control system, a robot system, a robot control method, and the like that can find a solution of the inverse kinematics of a robot more efficiently and in a short time by an iterative calculation method. .

  One aspect of the present invention includes an end point position acquisition unit that acquires a target end point position of a robot, a genuine model storage unit that stores information on the authentic model of the robot, and information on an approximate model that approximates the authentic model. An approximate model storage unit for storing, an approximate solution using the approximate model and the target end point position is obtained, and an iterative calculation process using an iterative method is performed using the approximate solution and the authentic model, whereby the robot It relates to a robot control system including a processing unit for obtaining a solution in the inverse kinematics.

  According to one aspect of the present invention, an authentic model of a robot and an approximate model that approximates the authentic model are prepared, and an approximate solution is obtained using the approximate model and the target end point position. Then, using the obtained approximate solution and the authentic model, iterative calculation processing by the iterative method is performed, so that a solution in the inverse kinematics of the robot can be obtained. In this way, by performing the iterative calculation process using the approximate solution based on the approximate model, it becomes possible to obtain the robot's inverse kinematics solution more efficiently and in a short time by the iterative method.

  In the aspect of the invention, the processing unit may perform the iterative calculation process by setting the approximate solution as an initial value.

  As described above, if the approximate solution based on the approximate model is set as the initial value of the iterative calculation process, it is possible to efficiently determine the initial value in the iterative method.

  In the aspect of the invention, the processing unit obtains an end point position based on the approximate solution by setting the approximate solution to a joint angle of the genuine model, and determines the end point position based on the approximate solution and the target end point. A corrected target end point position is obtained by correcting the target end point position based on an error with respect to a position, and a second approximate solution is obtained using the corrected target end point position and the approximate model. The solution in inverse kinematics of the robot may be obtained by performing the iterative calculation process using the second approximate solution and the authentic model.

  In this way, the difference between the approximate solution and the authentic model solution can be corrected in advance with respect to the target end point position, and the robot inverse kinematics solution can be obtained more efficiently and in a short time. It becomes possible.

  In the aspect of the invention, the approximate model storage unit stores information on first to m-th approximate models as models approximating the authentic model, and the processing unit stores the first to m-th approximate models. The first to mth approximate solutions are obtained using the approximate model and the target end point position, the approximate solution selected from the first to mth approximate solutions is set as an initial value, and the iteration You may obtain | require the solution in the inverse kinematics of the said robot by performing arithmetic processing.

  In this way, it is possible to set a more appropriate initial value as the initial value used in the iterative method compared to the case of using one approximate model.

  In one aspect of the present invention, the processing unit uses the i-th (1 ≦ i ≦ m) approximate model of the first to m-th approximate models and the target end point position to determine the i th An approximate solution is obtained, and the i-th approximate solution is set to the joint angle of the authentic model, thereby obtaining the i-th endpoint position and the Jacobian by the approximate solution, and the i-th endpoint position and the target endpoint. The joint angle is corrected using the first error between the position and the Jacobian, and the corrected joint angle is set to the joint angle of the genuine model, thereby obtaining the i-th corrected end point position. An approximate solution selected based on a second error between the i-th corrected end point position and the target end point position may be set as the initial value.

  In this way, an approximate solution that is closest to the target end point position can be set as the initial value of the iterative calculation process by performing, for example, the first step of the iterative method.

  In the aspect of the invention, the processing unit obtains an end point position based on the approximate solution by setting the approximate solution to a joint angle of the genuine model, and determines the end point position based on the approximate solution and the current end point. A solution in inverse kinematics of the robot may be obtained by setting the joint angle corresponding to the selected end point position from the positions to the initial value and performing the iterative calculation process.

  In this way, for example, when the current end point position can be efficiently obtained in a short time rather than using the end point position by the approximate solution, this can be appropriately dealt with. Become.

  In the aspect of the invention, the processing unit obtains a first error between the end point position by the approximate solution and the target end point position, and the current end point position, the target end point position, When the first error is smaller than the second error, the joint angle corresponding to the end point position by the approximate solution is set to the initial value. If the second error is smaller than the first error, the joint angle corresponding to the current end point position may be set to the initial value.

  In this way, the end point position with the smaller error from the target end point position is selected, and the joint angle corresponding to the selected end point position is obtained when finding the solution in inverse kinematics. The initial value can be set.

  In the aspect of the invention, when the point-to-point control of the robot is performed, the processing unit sets a joint angle corresponding to the end point position based on the approximate solution as the initial value. When continuous control of the robot is performed, a joint angle corresponding to the current end point position may be set to the initial value.

  In this way, when point-to-point control or continuous control is performed, the joint angle at the optimal endpoint position for each control is set to the initial value, and a solution is obtained by inverse kinematics. It becomes possible.

  In one aspect of the present invention, an operation history storage unit that stores an operation history end point position that is an operation history position of a past endpoint is included, and the processing unit includes the end point position based on the approximate solution and the current end point. In the inverse kinematics of the robot, the joint angle corresponding to the selected end point position is set to the initial value from the point position and the operation history end point position, and the iterative calculation process is performed. You may seek a solution.

  In this way, it is possible to set an initial value for obtaining an inverse kinematics solution in consideration of the motion history end point position in addition to the end point position by the approximate solution and the current end point position.

  In the aspect of the invention, the processing unit obtains a first error between the end point position by the approximate solution and the target end point position, and the current end point position, the target end point position, And a third error between the operation history end point position and the target end point position is obtained. The first error is the second error, and the third error is the third error. Is smaller than the first error, the joint angle corresponding to the end point position by the approximate solution is set to the initial value, and the second error is greater than the first error and the third error. Is set to the initial value, and the third error is smaller than the first error and the second error, the joint angle corresponding to the current end point position is set to the initial value. Is the operation history entry The joint angle corresponding to the point position may be set to the initial value.

  In this way, the end point position where the error from the target end point position becomes small is selected, and the joint angle corresponding to the selected end point position is the initial value when finding the solution in inverse kinematics. Can be set to

  In the aspect of the invention, the approximate model storage unit may store information on a model from which an offset of a joint of the genuine model is omitted as information on the approximate model.

  Another aspect of the present invention relates to a robot system including any one of the robot control systems described above and the robot.

  According to another aspect of the present invention, information on an authentic model of a robot is stored, information on an approximate model that approximates the authentic model is stored, a target end point position of the robot is obtained, and the approximate model and the target Using the end point position, an approximate solution is obtained, and using the approximate solution and the authentic model, an iterative calculation process is performed to obtain a solution in the inverse kinematics of the robot, and the obtained solution The present invention relates to a robot control method for controlling the robot based on the above.

1A to 1C are explanatory diagrams of robot joints, forward kinematics and inverse kinematics. FIG. 2A to FIG. 2D are explanatory diagrams of offsets of the joints of the robot. FIG. 3A to FIG. 3C are also explanatory diagrams of the offset of the joints of the robot. FIGS. 4A to 4C are explanatory diagrams of a genuine model and an approximate model. The structural example of the robot control system of the 1st Example of this embodiment. A specific configuration example of a robot control system. FIG. 7A and FIG. 7B are examples of a robot system. FIGS. 8A to 8D are explanatory diagrams of the method of the first embodiment. The flowchart which shows the detailed process of a 1st Example. The structural example of the robot control system of the 2nd Example of this embodiment. FIG. 11A to FIG. 11D are explanatory diagrams of the method of the second embodiment. The flowchart which shows the detailed process of 2nd Example. The structural example of the robot control system of the 3rd Example of this embodiment. FIG. 14A to FIG. 14C are explanatory diagrams of the method of the third embodiment. The flowchart which shows the detailed process of a 3rd Example. The structural example of the robot control system of the 4th Example of this embodiment. The structural example of the robot control system of the 5th Example of this embodiment. FIGS. 18A to 18C are explanatory views of the methods of the fourth and fifth embodiments. The flowchart which shows the detailed process of a 4th Example. The flowchart which shows the detailed process of a 5th Example.

  Hereinafter, preferred embodiments of the present invention will be described in detail. The present embodiment described below does not unduly limit the contents of the present invention described in the claims, and all the configurations described in the present embodiment are indispensable as means for solving the present invention. Not necessarily.

1. Inverse kinematics In the control of a robot, the process of obtaining the overall posture from the angle of each joint is called forward kinematics. On the other hand, the process of obtaining the angle of each joint that achieves a desired posture is called inverse kinematics. Inverse kinematics is a very important process for practical use of robots.

  However, in general, the forward kinematic process is an easy process, but the inverse kinematic process is a very difficult process to realize. The solution of forward kinematics is uniquely obtained by calculating the movement of the origin of the coordinate system and the rotation of the coordinate system sequentially. On the other hand, in order to obtain an inverse kinematics solution, it is generally necessary to solve nonlinear simultaneous equations. This solution can be obtained analytically only when the robot structure is simple, and in that case, a heuristic method is required.

  Considering the application of inverse kinematics and forward kinematics to a robot (manipulator), six parameters are required to specify the position and posture of the hand. In other words, the three-dimensional coordinate parameters (X, Y, Z) of the robot hand and the rotation angle parameters (U, V, W) around the respective axes (X, Y, Z) are required. , (X, Y, Z, U, V, W) are required. Therefore, in general, the robot is also required to have at least six degrees of freedom.

  If a set of joint angles is specified in a robot having an arbitrary degree of freedom, one set (X, Y, Z, U, V, W) is obtained correspondingly. In other words, the solution of forward kinematics is uniquely obtained. However, considering a case where each joint angle is obtained by giving a set of (X, Y, Z, U, V, W) to a 6-DOF robot, the solution is not necessarily one. Rather, it is more common when there are multiple solutions due to the partial symmetry of the robot structure. In recent years, in order to make a robot take a more free posture, a robot with seven degrees of freedom is often used. In this case, (X, Y, Z, U, V, W) is Even if it is specified, if there are solutions, there will be an infinite number of solutions.

  For example, FIG. 1A shows an example of a simple 6-DOF robot arm (robot in a broad sense). This robot arm has joints J1 to J6 and a plurality of links connected by these joints. Here, the joints J1, J4, and J6 are axial rotation joints as shown in FIG. 1B, and the joints J2, J3, and J5 are turning joints.

  As shown in FIG. 1C, from the joint angle parameters ([theta] 1, [theta] 2, [theta] 3, [theta] 4, [theta] 5, [theta] 6, [theta] 7) of the joints J1 to J7, the parameters (end point position in a broad sense) The process for obtaining X, Y, Z, U, V, W) is forward kinematics. On the other hand, the process of obtaining the joint angle parameters (θ1, θ2, θ3, θ4, θ5, θ6, θ7) from the hand position / posture parameters (X, Y, Z, U, V, W) is the inverse kinematics. is there.

  The solution of the inverse kinematics of a simple robot arm as shown in FIG. 1A can be obtained analytically. On the other hand, in a 6-DOF robot, there are a plurality of solutions that are partially symmetric with respect to the same hand posture. Further, in a robot with 7 degrees of freedom, since there are six hand constraint conditions, infinite solutions exist when solutions exist. In this case, even if an additional constraint condition called an arm angle is given, there are a plurality of solutions that are partially symmetric. Specifically, with respect to the basic solution, there are two solutions for base inversion, two solutions for elbow inversion, and two solutions for wrist inversion. Accordingly, there are eight solutions for the same hand position / posture by the combination of these inversions.

  Among the conditions for obtaining the inverse kinematic analytical solution, the most basic one is the case where the rotation axes intersect at one point as shown in FIG. In FIG. 2A, the rotation axes of the joints J1 and J3 are parallel to the paper surface, and these rotation axes intersect at the intersection of the centers of the joints J2. The rotation axis of the joint J2 is perpendicular to the paper surface, and this rotation axis also intersects at the same intersection point as described above. Although this is not described in detail, this relates to the fact that arbitrary three-dimensional rotation can be expressed by the ZX-Z Euler angle or the like.

  On the other hand, a condition in which an analytical solution of inverse kinematics is not normally obtained is when an offset exists in the joint as shown in FIG. This is because a plurality of joint positions and postures change due to the rotation of one joint. For example, in FIG. 2A, even if the hand (end point) is fixed and the joint J3 rotates, the position of the joint J2 does not change. On the other hand, when an offset exists as shown in FIG. 2B, that is, when the rotation axis of the joint J3 does not pass through the center of the joint J2, when the joint J3 rotates, the position of the joint J2 changes. .

  Even when there is an offset in the joint as shown in FIG. 2C, it can be converted into a mathematically equivalent one in which the offset is omitted as shown in FIG. This is because the distance between the joints J2 and J4 does not change by the rotation of the joint J3, and an analytical solution is obtained and the degree of freedom is high as in the connection of the joints J3, J4, and J5. Because there exists. However, whether such conversion is possible depends strongly on the structure of the robot arm, and even if conversion is possible, the conversion can only be determined by trial and error.

  As described above, the presence of the offset makes it difficult to process inverse kinematics. However, the offset has an advantageous effect in practice. For example, the range of motion of the joint as shown in FIGS. 3 (A) and 3 (B) is enlarged. Since an actual robot arm has a size of a motor and a structure, for example, in the case of FIG. 3A, the movable range of the joint J2 is limited due to the size of the joints J1 and J3. Occur. On the other hand, by adding an offset as shown in FIG. 3B, the movable range of the joint J2 can be widened.

  Further, in the case of FIG. 3A, since the rotating shaft is concentrated at one point, the surrounding mechanical structure becomes complicated, and as a result, the weight increases. On the other hand, in the structure as shown in FIG. 3B, a relatively simple mechanical structure can be adopted, and as a result, there is an effect that the weight can be reduced.

  In this embodiment, for example, a robot arm (robot) as shown in FIG. FIG. 3C is an example of a structure in which an inverse kinematic solution is difficult or cannot be obtained analytically. As a matter of course, the method of the present embodiment is not specialized for a robot having such a structure.

  In the present embodiment, an approximate model MA as shown in FIGS. 4B and 4C that can be analyzed (process for obtaining an analytical solution) as a model that approximates the authentic model MT in FIG. (Approximate analysis model) is prepared. The approximate model MA in FIGS. 4B and 4C is a model in which an offset (offset of the rotation axis) is omitted from the authentic model MT in FIG. 4A. In the approximate model MA in FIG. 4B, the offset is simply removed, but in the approximate model MA in FIG. 4C, the length of the diagonal line of the offset portion is used as the length of the link connecting the joints. Yes. The approximate model is not limited to the models shown in FIGS. 4B and 4B, and various modifications can be made. For example, instead of omitting offsets for all joints, a model in which offsets for some joints are omitted may be used as the approximate model.

2. First Example FIG. 5 shows a configuration example of a robot control system (and a robot system including the same) according to a first example of the present embodiment. FIG. 5 is also a basic configuration example of the present embodiment including the first example and second to fifth examples described later. Note that the robot control system (robot system) of the present embodiment is not limited to the configuration shown in FIG. 5, and various modifications may be made such as omitting some components or adding other components. It is.

  The robot control system (robot control device) in FIG. 5 includes a target end point position acquisition unit 20, a genuine model storage unit 22, an approximate model storage unit 24, and a processing unit 30. Moreover, the robot control part 80 which functions as an arm drive control part etc. can be included. The robot system of this embodiment is realized by the robot control system and the robot 100 (robot arm).

  The end point position acquisition unit (hand position / posture acquisition unit) 20 acquires a target end point position of the robot 100 (robot arm). The target end point position (target end effector position) is a position (posture) that is a movement target of the end point (end effector) of the robot 100, and is specified by trajectory information (planned trajectory) described later.

  Hereinafter, the hand position / posture (X, Y, Z, U, V, W) of the robot 100 (robot arm) will be described as the end point position. The hand position / posture to be processed in the present embodiment may be the hand position (X, Y, Z) corresponding to the end point position, or the hand position (X, Y, Z) and hand position (U, V, W) may be used.

  The authentic model storage unit 22 stores information on the authentic model of the robot 100 (information on joints and links of the authentic model). The approximate model storage unit 24 stores information on an approximate model obtained by approximating a genuine model (information on joints and links of the approximate model). Specifically, the approximate model storage unit 24 stores, as approximate model information, model information from which the joint offset of the authentic model is omitted. For example, when the genuine model MT has an offset as shown in FIG. 4 (A), an approximate model MA with the offset omitted as shown in FIGS. 4 (B) and 4 (C) is prepared. The information is stored in the approximate model storage unit 24. Note that overlapping portions of the information stored in the authentic model storage unit 22 and the information stored in the approximate model storage unit 24 may be shared and stored.

  The processing unit 30 performs a process for obtaining a solution in the inverse kinematics of the robot 100 by performing an iterative calculation process by an iterative method. Specifically, the processing unit 30 obtains an approximate solution using the approximate model and the target end point position (hand position or foot position). This approximate solution is, for example, an approximate solution of a joint angle (joint variable) of the robot 100 (in the narrow sense, an approximate analytical solution; the same applies hereinafter). The processing unit 30 obtains a solution in the inverse kinematics of the robot 100 by performing an iterative calculation process by an iterative method using the approximate solution and the authentic model. That is, the solution of the joint angle (joint variable) of the robot 100 is obtained. For example, the processing unit 30 sets an approximate solution as an initial value and performs an iterative calculation process to obtain a solution in inverse kinematics. The function of the processing unit 30 can be realized by a hardware circuit such as a processor or an ASIC, a program that operates in the processor, or the like.

  The processing unit 30 includes an approximate solution calculation unit 40, an iterative calculation processing unit 50, a convergence determination unit 52, and an error processing unit 54.

  The approximate solution calculation unit 40 performs processing for obtaining an approximate solution of the joint angle of the robot. Specifically, a robot is obtained by analytical processing using the approximate model (approximate analysis model) from the approximate model storage unit 24 and the target end point position (hand position / posture) from the target end point position acquisition unit 20. To obtain an approximate analytical solution for each joint angle. For example, an approximate analytical solution of the joint angle is obtained by a function expression (such as a nonlinear simultaneous equation) in which the target end point position is an argument variable and the joint angle is a solution variable. In this case, an approximate analytical solution can be obtained by converting a nonlinear function such as a trigonometric function into a polynomial. Further, an additional constraint condition such as an arm angle may be given. For example, in the case of a robot arm having 6 degrees of freedom, a structure in which three consecutive joints out of six joints are all rotary joints and the extension lines of the three rotational axes of those joints intersect at one point. For example, the solution of inverse kinematics can be obtained analytically by reducing to two sets of three-dimensional quaternary simultaneous equations.

  The iterative calculation processing unit 50 performs processing for obtaining a solution in inverse kinematics by an iterative method. The convergence determination unit 52 determines the convergence of the iterative calculation process by the iterative method. The error processing unit 54 performs processing when an error occurs in the iterative calculation processing.

  For example, the iterative calculation processing unit 50 obtains an inverse kinematics solution (joint variable solution) by numerical calculation using Jacobian, optimization calculation, search method, or the like. For example, in inverse kinematics using Jacobian and quaternion inverse kinematics, the solution is searched while updating the angles of all joints little by little at the same time. On the other hand, in the CCD (Cyclic-Coordinate-Descent) method, only the optimal angle of one joint is obtained in one calculation step, and this is repeated in order for all joints, so that the robot arm as a whole moves backward. Explore academic solutions. In the PIK (Particle-IK) method, the inverse kinematics solution is obtained by directly manipulating the joint position, not the joint angle.

  For example, as shown in the following equation (1), consider a function f that takes m variables as inputs and gives n outputs. Alternatively, n sets of functions f which take m variables as inputs and give one output are prepared.

この場合に、ヤコビアン（ヤコビ行列）は、下式（２）に表される行列である。

In this case, the Jacobian (Jacobi matrix) is a matrix represented by the following expression (2).

つまり、ヤコビアンは、ｘの「ある値」の近傍において、ｘとｙの関係を線形近似したものである。従って、一般的には、ｘの「ある値」が変化すれば、ヤコビアンも変化する。

That is, the Jacobian is a linear approximation of the relationship between x and y in the vicinity of “a certain value” of x. Therefore, in general, when “a certain value” of x changes, the Jacobian also changes.

  Here, if x is the joint angle and y is the end point position of the robot (the hand position and posture in the narrow sense; the same applies to the following), the Jacobian (Jacobi matrix) of the above equation (2) This is a relational expression that gives a minute change in the end point position accompanying the minute change. Here, the correspondence between the minute change of each joint angle and the minute change of the end point position changes according to the posture. In other words, the Jacobian needs to be calculated each time the posture changes.

  The inverse kinematics processing to obtain the joint angle from the end point position (hand position / posture) is generally very difficult processing. If it can be obtained, a relational expression for converting a minute change in the end point position into a minute change in each joint angle can be obtained.

一般的には、ヤコビアンは正方行列ではなく、且つ、ｒａｎｋ（階数）も低く、通常の意味の逆行列は存在しない。例えば７軸のロボットでは、手先の自由度は６つしかないので、７×７ではなく、７×６の行列になってしまう。但し、通常の意味の逆行列と類似した性質を持つ一般化逆行列は存在し得る。この一般化逆行列による逆ヤコビアンＪ^−１により、エンドポイント位置の微小な変化を各関節角の微小な変化に変換することができる。例えばヤコビアンを用いた逆運動学の１つのアルゴリズム例では、現在のエンドポイント位置から目標エンドポイント位置へと延びる変位ベクトルΔＰを計算する。次に、逆ヤコビアンＪ^−１を用いて、関節角の変位ベクトルΔθ＝ΔＰＪ^−１を計算する。そして、θ＝θ＋Δθにより関節角ベクトルを更新する。そして更新後のエンドポイント位置と目標エンドポイント位置との差異が許容誤差範囲内ならば、解が収束したと判定して、反復演算処理を終了する。この場合の収束判定処理は収束判定部５２により行われる。解が収束した場合には、その解は関節角としてロボット制御部８０に出力される。一方、解が収束しなかった場合には、エラー処理部５４が、エラー時用の処理を行う。

In general, the Jacobian is not a square matrix and has a low rank (rank), and there is no inverse matrix with the usual meaning. For example, in a 7-axis robot, there are only 6 degrees of freedom of the hand, so a 7 × 6 matrix is formed instead of 7 × 7. However, there can be a generalized inverse matrix having properties similar to the inverse matrix of ordinary meaning. By the inverse Jacobian J ⁻¹ by this generalized inverse matrix, a minute change in the end point position can be converted into a minute change in each joint angle. For example, in one example of inverse kinematics using Jacobian, a displacement vector ΔP extending from the current endpoint position to the target endpoint position is calculated. Next, a joint angle displacement vector Δθ = ΔPJ ⁻¹ is calculated using the inverse Jacobian J ⁻¹ . Then, the joint angle vector is updated by θ = θ + Δθ. If the difference between the updated end point position and the target end point position is within the allowable error range, it is determined that the solution has converged, and the iterative calculation process is terminated. The convergence determination process in this case is performed by the convergence determination unit 52. When the solution converges, the solution is output to the robot control unit 80 as a joint angle. On the other hand, when the solution does not converge, the error processing unit 54 performs an error process.

  The robot control unit 80 controls the robot 100 based on the solution obtained by the processing unit 30. Specifically, the robot control is realized by performing feedback control in which the joint angles obtained as solutions are set to the joint angles of the robot 100.

  FIG. 6 shows a specific configuration example of the robot control system (robot system) of the present embodiment. The robot control system includes a force control unit 12, a target value output unit 60, and a robot control unit 80. The robot system includes the robot control system and the robot 100 (force sensor 10).

  The target value output unit 60 outputs a target value for feedback control of the robot (manipulator in a narrow sense). Based on this target value, feedback control of the robot 100 is realized.

  The target value output unit 60 includes a trajectory generation unit 62 and an inverse kinematics processing unit 64. The trajectory generator 62 outputs the trajectory information of the robot. The trajectory information can include position information (X, Y, Z) of the end effector part (end point) of the robot and rotation angle information (U, V, W) about each coordinate axis. The inverse kinematics processing unit 64 performs inverse kinematics processing (inverse kinematics processing) based on the trajectory information from the trajectory generation unit 62, and outputs, for example, joint angle information of the robot as a target value. For example, the trajectory generation unit 62 in FIG. 6 corresponds to the target end point position acquisition unit 20 in FIG. 5, and the inverse kinematics processing unit 64 corresponds to the processing unit 30.

  The force control unit 12 (impedance control unit in a narrow sense) performs force control (force control) based on sensor information from the force sensor 10 and outputs a correction value for the target value. More specifically, the force control unit 12 (impedance control unit) performs impedance control (or compliance control) based on sensor information (force information and moment information) from the force sensor 10. Force control is, for example, control in which force feedback is added to conventional position control. Impedance control is a technique for making the ease of displacement (mechanical impedance) of the end effector section (hand) with respect to external force a desirable state by control. Specifically, in the model in which the viscosity coefficient and the elastic element are connected to the mass of the end effector unit of the robot, the control is performed so as to contact the object with the viscosity coefficient and the elastic coefficient set as targets. The force sensor 10 is a sensor that detects a force or moment received as a reaction force of the force generated by the robot 100. The force sensor 10 is usually attached to the wrist portion of the arm of the robot 100, and the detected force and moment are used for various types of force control (impedance control) as sensor information.

  The robot control unit 80 performs feedback control of the robot based on the target value from the target value output unit 60. Specifically, the robot feedback control is performed based on the target value corrected by the correction value from the force control unit 12. For example, feedback control of the robot 100 is performed based on the target value corrected by the correction value and the feedback signal from the robot 100. For example, the robot control unit 80 includes a plurality of drive control units 82-1 to 82-N (in a narrow sense, motor control units), and controls the control signals for the drive units 102-1 to 102-N included in the robot 100. Is output. Here, the drive units 102-1 to 102-N are drive mechanisms for moving each joint of the robot 100, and are realized by, for example, a motor or the like.

  FIG. 7A shows an example of a robot system including the robot control system of this embodiment. This robot system includes a control device 300 (robot control system) and a robot body 310 (robot in a broad sense). The control device 300 performs control processing for the robot body 310. Specifically, control for operating the robot main body 310 is performed based on the operation sequence information (scenario information). The robot body 310 includes an arm 320 and a hand (gripping unit) 330. Then, it operates according to an operation instruction from the control device 300. For example, operations such as gripping or moving a workpiece placed on a pallet (not shown) are performed. Further, information such as the posture of the robot body 310 and the position of the workpiece is detected based on captured image information acquired by an imaging device (not shown), and the detected information is sent to the control device 300.

  The robot control system of the present embodiment is provided in, for example, the control device 300 in FIG. 7A, and the robot control system is realized by, for example, hardware or a program of the control device 300. According to the robot control system of the present embodiment, it is possible to reduce performance requirements for control hardware such as the control device 300 and to operate the robot body 310 with high responsiveness.

  In FIG. 7A, the robot body 310 (robot) and the control device 300 (robot control system) are configured separately. However, as shown in FIG. 300 may be integrally formed. Specifically, in FIG. 7B, the control device 300 is stored in the base unit portion that supports the robot body 310. The base unit portion is provided with wheels and the like so that the entire robot can move. Although FIG. 7A shows an example of a single-arm robot, the robot of this embodiment may be a multi-arm robot such as a double-arm robot as shown in FIG.

  Next, details of the technique of the first example of the present embodiment will be described. In the first embodiment, an approximate model (approximate analysis model) that can approximate a target robot and analytically obtain an inverse kinematic solution is prepared. For example, if there is an offset as shown in FIG. 2B in the authentic model of the robot, the offset is removed as shown in FIG. 2A in the approximate model. That is, in the authentic model of FIG. 2B, the rotation axis of the joint J3 does not pass through the joint center of the joint J2, and has an offset, but in the approximate model of FIG. 2A, the rotation axis of the joint J3. Passes through the joint center of the joint J2. The rotation axis of the joint J1 also passes through the joint center of the joint J2. For this reason, a solution can be obtained analytically by using the approximate model of FIG. Then, the designated target end point position (target hand position / posture) is input, and an analytical process is performed using the approximate model to obtain an approximate solution that is an analytical solution. Then, for example, using this approximate solution as an initial value, an inverse kinematic solution of the target robot is obtained by an iterative method. That is, the approximate solution is set to the initial value of the joint angle of the genuine model, and an iterative calculation process is performed by an iterative method to obtain an inverse kinematics solution. Here, an example of a method for obtaining the approximate model from the target robot is to set the length of the offset of the robot to zero. An example of a method for obtaining a solution of the inverse kinematics of the robot is a method using a Jacobian (inverse Jacobian).

  Specifically, first, as shown in FIG. 8A, a target end point position (hand position / posture) is given. The target end point position is acquired based on the trajectory information from the trajectory generation unit 62 in FIG. 6, for example.

  Next, as shown in FIG. 8B, an approximate solution is obtained using the approximate model MA and the target end point position. That is, the target end point position of the approximate model MA is set to the position shown in FIG. 8A, and an analytical solution for the joint angle is obtained as an approximate solution by analytical processing. Since the approximate model is a model that can obtain an analytical solution, an analytical solution of the joint angle can be obtained.

  Next, as shown in FIG. 8C, the obtained approximate solution is set to an initial value. Specifically, an approximate solution of each joint angle obtained from the approximate model is set for each joint angle of the authentic model MT. If the approximate model is a good model, the approximate solution should be close to the true solution, and the error between the end point position and the target end point position of the authentic model MT in FIG. 8C should be small.

  Then, the approximate solution obtained by the approximate model MA is set as the initial value of the joint angle of the authentic model MT in this way, and the iterative calculation process by the iterative method is performed as shown in FIG. That is, the error in FIG. 8C is corrected by an iterative method to find a solution in inverse kinematics. If the iterative calculation process converges, it is the solution. That is, if the error between the end point position and the target end point position is within the allowable error range, the solution is determined to have converged, and the solution at that time becomes each joint angle of the robot to be obtained by inverse kinematics. .

  FIG. 9 is a flowchart showing detailed processing of the first embodiment. First, a target end point position is acquired (S1). Next, an approximate solution is obtained from the approximate model (S2). That is, an approximate solution that is an analytical solution is obtained by an analytical process using the approximate model.

  Next, it is determined whether or not a solution has been obtained. If it has not been obtained, it is determined that the processing has failed (S3, S7). On the other hand, when it is obtained, the obtained approximate solution is used as an initial value, and a solution is obtained by an iterative method (S4). Specifically, an approximate solution is set as an initial value of each joint angle of the genuine model, and a solution in inverse kinematics is obtained by iterative calculation processing by an iterative method. If the solution converges, it is determined that the process has been successful (S5, S6), and the solution at that time is set to each joint angle of the robot to perform robot control.

  According to the method of the present embodiment described above, the inverse kinematics solution of a robot having a complicated structure can be obtained more efficiently and in a short time by an iterative method. Further, according to the present embodiment, it is possible to eliminate the large storage capacity that is necessary for determining the initial value of the conventional iterative method. Further, according to the present embodiment, it is possible to easily perform robot posture control and redundant degree-of-freedom control, which were difficult in the conventional iterative method. As described above, it is possible to realize higher-level and higher-speed control with respect to a robot having a redundant degree of freedom and a complicated structure such as an offset.

3. Second Example FIG. 10 shows a configuration example of a robot control system (and a robot system including the same) according to a second example of the present embodiment. In addition, about the structure of a robot control system, or its component requirement, the description which overlaps with a 1st Example is abbreviate | omitted suitably.

  The robot control system in FIG. 10 includes a target end point position acquisition unit 20, a genuine model storage unit 22, an approximate model storage unit 24, and a processing unit 30. A robot controller 80 can also be included.

  The processing unit 30 in FIG. 10 determines the end point position (hand position / posture) based on the approximate solution by setting the approximate solution (approximate analysis solution) to the joint angle (joint variable) of the authentic model. That is, the approximate solution obtained by the approximate model is set to each joint angle of the authentic model, and the end point position of the authentic model at that time is obtained as the end point position by the approximate solution. Next, the processing unit 30 obtains a corrected target end point position by correcting the target end point position based on an error between the end point position and the target end point position by the approximate solution. That is, correction is performed by shifting the target end point position by the amount of the error, and the position where the end point position is shifted is set as the corrected target end point position.

  Then, the processing unit 30 obtains a second approximate solution using the corrected target end point position and the approximate model. Specifically, an analytical process using the corrected target end point position and the approximate model is performed, and the joint angle analytical solution is obtained as the second approximate solution. And the process part 30 calculates | requires the solution in the inverse kinematics of the robot 100 by performing iterative calculation processing using the calculated | required 2nd approximate solution and a genuine model. Specifically, the second approximate solution is set as the initial value of the joint angle of the authentic model, and the iterative calculation process of the iterative method is performed to obtain the joint angle solution as a solution in inverse kinematics.

  In order to perform the above processing, the processing unit 30 in FIG. 10 includes an approximate solution calculation unit 40, a forward kinematics processing unit 42, a target endpoint position correction unit 43, an iterative calculation processing unit 50, a convergence determination unit 52, an error process. Part 54 is included.

  Specifically, the approximate solution calculation unit 40 approximates using the approximate model from the approximate model storage unit 24 and the target end point position from the target end point position acquisition unit 20 as in the first embodiment. Find a solution. Then, the forward kinematics processing unit 42 sets the obtained approximate solution for the joint angle of the authentic model from the authentic model storage unit 22, thereby performing the authentic model by forward kinematics processing (forward kinematics processing). Is determined as the end point position by the approximate solution. Next, the target end point position correcting unit 43 corrects the target end point position based on the error between the end point position and the target end point position based on the approximate solution. Then, the approximate solution calculation unit 40 performs an analytical process using the corrected target end point position and the approximate model to obtain a second approximate solution. Next, the iterative calculation processing unit 50 obtains a solution in inverse kinematics by performing an iterative calculation process using the second approximate solution and the authentic model. Then, the convergence determination unit 52 determines whether or not the solution has converged in this iterative calculation process, and when the solution has converged, outputs the solution to the robot control unit 80 as the joint angle of the robot 100. On the other hand, if the solution does not converge, the error processing unit 54 performs an error process.

  Next, details of the technique of the second example of the present embodiment will be described. In the second embodiment, an approximate model capable of approximating a target robot and analytically obtaining an inverse kinematic solution is prepared. Then, as in the first embodiment, the designated target end point position (target hand position / posture) is input, and an analytical process is performed using the approximate model to obtain an approximate solution that is an analytical solution.

  Next, using the obtained approximate solution as an initial value, the end point position of the target robot is obtained. Then, a difference between the obtained end point position and the designated target end point position is obtained as an error. Next, the above error is subtracted from the designated target end point position, and this is used as the corrected target end point position. Then, using the corrected target end point position as an input, a second approximate solution is obtained using an approximate model. Next, an inverse kinematic solution of the target robot is obtained by an iterative method with the second approximate solution as an initial value.

  The technique of the second embodiment uses an approximate model that gives an approximate solution twice. In other words, an initial value closer to the true solution can be obtained by correcting the difference between the approximate solution and the solution based on the authentic model, which occurs when using the authentic model, with respect to the target end point position in advance. It is. This is based on the idea that the second approximate solution for the modified target endpoint position should be closer to the solution using the authentic model.

  Specifically, as shown in FIG. 11A, the first approximate solution is obtained using the approximate model MA and the target end point position as in the first embodiment. Then, the first approximate solution is set to the joint angle of the authentic model MT, and the end point position by the approximate solution is obtained.

  Next, as shown in FIG. 11B, an error (difference) between the end point position (end point position of the authentic model) and the target end point position by the obtained approximate solution is obtained. Then, using the obtained error, the target end point position is corrected to obtain the corrected target end point position.

  Next, as shown in FIG. 11C, a second approximate solution is obtained using the corrected target end point position and the approximate model MA. That is, the target end point position of the approximate model MA is set as the corrected target end point position, and the analytical solution of the joint angle is obtained as the second approximate solution by analytical processing. The second approximate solution for the target end point position corrected in this way should be closer to the solution when using the authentic model MT.

  Next, as shown in FIG. 11D, the obtained second approximate solution is set to an initial value. Specifically, a second approximate solution of each joint angle obtained from the approximate model is set for each joint angle of the authentic model MT. Then, iterative calculation processing is performed by an iterative method to find a solution in inverse kinematics. If the iterative calculation process converges, it becomes a solution, and becomes each joint angle of the robot to be obtained by inverse kinematics.

  FIG. 12 is a flowchart showing detailed processing of the second embodiment. First, a target end point position is acquired (S11). Next, a first approximate solution is obtained from the approximate model (S12). That is, a first approximate solution that is an analytical solution is obtained by analytical processing on the approximate model.

  Next, it is determined whether or not a solution has been obtained. If not, it is determined that the process has failed (S13, S21). On the other hand, when it is obtained, the end point position is obtained using the obtained first approximate solution and the authentic model (S14). Specifically, the first approximate solution is set for each joint angle of the authentic model, and the end point position by the approximate solution is obtained by forward kinematics processing. Then, based on the error (difference) between the end point position and the target end point position based on the obtained approximate solution, the target end point position is corrected to determine the corrected target end point position (S15).

  Next, a second approximate solution for the corrected target end point position is obtained from the approximate model (S16). That is, a second approximate solution that is an analytical solution is obtained by analytical processing using the approximate model. Then, it is determined whether or not a solution has been obtained. If not, it is determined that the process has failed (S17, S21). On the other hand, when obtained, a solution is obtained by an iterative method using the obtained second approximate solution and the authentic model (S18). Specifically, a second approximate solution is set as an initial value of each joint angle of the authentic model, and a solution in inverse kinematics is obtained by calculation processing by an iterative method. If the solution converges, it is determined that the process has been successful (S19, S20), and the solution at that time is set to each joint angle of the robot to perform robot control. On the other hand, if no solution is obtained, it is determined that the process has failed, and error processing or the like is performed (S21).

  According to the method of the second embodiment described above, an inverse kinematic solution can be obtained by an iterative method by setting the second approximate solution closer to the true solution as an initial value. Accordingly, it is possible to realize higher-level and higher-speed control for a robot having a redundant degree of freedom and a complicated structure such as an offset.

4). Third Example FIG. 13 shows a configuration example of a robot control system (and a robot system including the same) according to a third example of the present embodiment. In addition, about the structure of a robot control system, or its component requirement, the description which overlaps with the 1st, 2nd Example is abbreviate | omitted suitably.

  The robot control system of FIG. 13 includes a target endpoint position acquisition unit 20, a genuine model storage unit 22, an approximate model storage unit 24, and a processing unit 30. A robot controller 80 can also be included.

  The approximate model storage unit 24 in FIG. 13 stores information on the first to m-th approximate models MA-1 to MA-m (m is an integer of 2 or more) as models approximating the authentic model. The processing unit 30 obtains first to mth approximate solutions using the first to mth approximate models MA-1 to MA-m and the target end point position. Then, an approximate solution selected from the first to mth approximate solutions is set as an initial value, and an iterative calculation process is performed to obtain a solution in the inverse kinematics of the robot 100.

  Specifically, the processing unit 30 uses the i-th (1 ≦ i ≦ m) approximate model and the target end point position among the first to m-th approximate models MA-1 to MA-m, and Find the approximate solution of i. That is, an i-th approximate solution, which is an analytical solution, is obtained by analytical processing using the i-th approximate model and the target end point position.

  Next, the processing unit 30 obtains the i-th end point position and the Jacobian by the approximate solution by setting the obtained i-th approximate solution as the joint angle of the genuine model. Then, the joint angle is corrected using the first error between the i-th end point position and the target end point position and the Jacobian (inverse Jacobian). That is, the iterative calculation process by the inverse kinematics iteration method using Jacobian is executed, for example, only once (a predetermined number of times in a broad sense). For example, in the iterative method of inverse kinematics using Jacobian, as described above, the displacement of the joint angle (displacement vector) is obtained using the displacement of the end point position (displacement vector) and the inverse Jacobian. In the third embodiment, the joint angle is corrected by the amount of displacement of the joint angle obtained using the first error corresponding to the displacement of the end point position and the inverse Jacobian. The processing unit 30 obtains the i-th corrected end point position by setting the corrected joint angle to the joint angle of the genuine model. Then, an approximate solution selected based on the second error between the determined i-th corrected end point position and the target end point position is set as an initial value. Specifically, the above-described processing is performed using each of the first to mth approximate models MA-1 to MA-m, and the first to mth corrected end point positions are obtained. Then, a second error between the obtained first to mth correction end point positions and the target end point position is obtained, and an approximate solution corresponding to the correction end point position at which the second error becomes the smallest is obtained. select. Then, the selected approximate solution is set as an initial value of the joint angle of the genuine model, and an iterative calculation process of the iterative method is performed to obtain a solution in inverse kinematics.

  In order to perform the above processing, the processing unit 30 in FIG. 13 includes an approximate solution calculation unit 40-1 to 40-m, an approximate solution selection unit 44, an iterative calculation processing unit 50, a convergence determination unit 52, and an error processing unit 54. Including.

  Specifically, the approximate solution calculation units 40-1 to 40-m are connected to the first to m-th approximate models MA-1 to MA-m from the approximate model storage unit 24 and the target end point position acquisition unit 20. The first to m-th approximate solutions are obtained using the target end point positions. That is, each approximate solution calculation unit 40-1 to 40-m obtains an approximate solution by the same method as in the first embodiment. Then, the approximate solution selection unit 44 selects, from among the obtained first to mth approximate solutions, an approximate solution that is evaluated to have the least error, for example. For example, by setting the i-th approximate solution of the first to m-th approximate solutions to the joint angle of the authentic model, the i-th end point position and the Jacobian by the approximate solution are obtained, and the i-th end point The joint angle is corrected by using the first error between the position and the target end point position and the Jacobian, and the corrected joint angle is set to the joint angle of the genuine model, whereby the i-th corrected end point Suppose that the position is determined. In this case, the approximate solution selection unit 44 selects an approximate solution based on the second error between the i-th correction end point position and the target end point position of the first to m-th correction end point positions. To do. For example, the correction end point position with the smallest second error is selected. Then, the iterative calculation processing unit 50 obtains a solution in inverse kinematics by performing iterative calculation processing using the selected approximate solution and the authentic model. That is, the selected approximate solution is set as the initial value of the authentic model, and an iterative calculation process is performed to obtain a solution for the joint angle. When the solution has converged, the convergence determination unit 52 outputs the solution to the robot control unit 80 as the joint angle of the robot 100.

  Next, details of the technique of the third example of the present embodiment will be described. In the third embodiment, a plurality of approximate models capable of approximating a target robot and analytically obtaining an inverse kinematic solution are prepared. The plurality of approximate models are models in which the lengths of the arms are different from each other, for example. Specifically, as shown in FIGS. 4B and 4C, approximate models with different arm length settings can be assumed. Alternatively, approximate models may be used in which the offset is omitted and the constraint conditions are different from each other.

  As in the first embodiment, the designated target end point position (target hand position / posture) is input, and analytical processing is performed using a plurality of approximate models, and a plurality of approximate solutions that are analytical solutions are obtained. Ask for.

  Next, an optimal solution is selected from the plurality of approximate solutions based on the following criteria, and is used as an initial value to determine the end point position (hand position / posture) of the target robot. Specifically, a first error (first difference) between the designated target end point position and the end point position corresponding to each of the plurality of approximate solutions is obtained. Further, the Jacobian at each end point position (hand position / posture) is obtained. Then, the angle joint angle is corrected by the first error and the Jacobian. Next, a correction end point (corrected hand position / posture) is obtained from the corrected joint angle. Then, a second error (second difference) between the target end point position and the corrected end point position is obtained. Then, the proximity solution corresponding to the smallest second error is set as an initial value, and an inverse kinematic solution of the target robot is obtained by an iterative method.

  The technique of the third embodiment prepares a plurality of approximate models that give approximate solutions. In other words, a plurality of approximate models are prepared, and further, which approximate model is better is processed by the first one step of the iterative method, and the result closest to the target is selected. It is a technique. Simply by selecting the solution that is not likely to be close to or far from the target and that is likely to approach the true solution efficiently, based on the result of the above one-step process. is there.

  First, the Jacobian will be briefly described. For the sake of simplification, a one-dimensional case will be described. In FIG. 14A, TR represents the solution of the joint angle with respect to the end point position, and corresponds to the authentic model. The Jacobian at P1 corresponding to the approximate solution corresponds to the tangent (differential value) TG at P1. FIG. 14B shows the angular displacement for moving to the target position (target end point position) using this Jacobian. Specifically, a corrected approximate solution is obtained from the intersection P2 between the tangent line TG representing the Jacobian and the target position (target end point position). This corrected approximate solution is closer to the solution of P3 than the approximate solution, and the error between the target position and the corrected position (corrected end point position) is small. On the other hand, in FIG. 14C, the corrected approximate solution of P5 is obtained from the tangent TG corresponding to the Jacobian at P4 corresponding to the approximate solution. In FIG. 14C, the error between the target position and the correction position is larger than in FIG. 14B. In FIG. 14C, it is considered that the convergence of the subsequent iterative calculation process is also poor. That is, when the Jacobian at the corrected approximate solution is obtained and further corrected, the intersection of the tangent corresponding to the Jacobian and the target position is away from the solution, so that the convergence of the iterative calculation process becomes worse.

  It is very difficult to judge whether a solution is good or bad. For example, there is a high possibility of making a judgment mistake simply by the magnitude of the error of the first approximate solution. On the other hand, the method of judging by the error after correcting once with Jacobian as in the method of the third embodiment is not necessarily the best method. However, experimentally, when the convergence to the solution is good, the first two or three corrections quickly approach the vicinity of the true solution. On the other hand, in the case of poor convergence, such a characteristic is not seen. Therefore, in this sense, it is considered effective to use the error after the correction is performed once as in the method of the third example of the present embodiment.

  FIG. 15 is a flowchart showing detailed processing of the third embodiment. First, a target end point position is acquired (S31). Next, it is determined whether or not all approximate models have been processed. If not, one approximate model is selected (S32, S33). Then, as in the first embodiment, an approximate solution (approximate analytical solution) is obtained using the selected approximate model (S34).

  Next, it is determined whether or not a solution has been obtained (S35). If not obtained, the process returns to step S32. On the other hand, if it is obtained, the end point position is obtained using the obtained approximate solution and the authentic model as in the second embodiment (S36). Specifically, an approximate solution is set for each joint angle of the authentic model, and the end point position by the approximate solution is obtained by forward kinematics processing. Specifically, an approximate position at P1 in FIG. Further, a Jacobian (inverse Jacobian) is obtained using the obtained approximate solution and the authentic model (S37). This corresponds to obtaining the tangent TG at P1 in FIG.

  Next, the joint angle is corrected using the first error between the obtained end point position and the target end point position and the Jacobian (inverse Jacobian) (S38). In FIG. 14B, a corrected approximate solution (joint angle after correction) is obtained from the approximate position (end point position by the approximate solution), the target position (target end point position), and the tangent TG (Jacobiane). Equivalent to.

  Next, a corrected end point position is obtained using the corrected joint angle and the authentic model (S39). This corresponds to obtaining a correction position (correction end point position) from TR (authentic model) and the corrected approximate solution (corrected joint angle) in FIG. Then, the second error between the corrected end point position and the target end point position and the corrected joint angle are stored in the storage unit (S40). That is, the error between the target position and the correction position in FIG. 14B and the correction approximate solution are stored.

  If it is determined in step S32 that the processing for all approximate models has been performed, the joint angle that gives the solution closest to the target end point position is selected as the initial value (S41). Specifically, in FIG. 14B, the correction approximate solution that minimizes the second error between the correction position and the target position is selected as the initial value. This selection process can be realized by using the second error of each approximate model stored in step S40 and the corrected joint angle.

  Next, a solution is obtained by an iterative method using the selected initial value and the authentic model (S42). Specifically, a selected initial value is set for each joint angle of the authentic model, and a solution in inverse kinematics is obtained by arithmetic processing by an iterative method. If the solution converges, it is determined that the process has been successful (S43, S44), and the solution at that time is set to each joint angle of the robot to perform robot control. On the other hand, if no solution is obtained, it is determined that the process has failed, and error processing or the like is performed (S45). Note that steps S37 to S39 in FIG. 15 correspond to one step of the iterative calculation process in step S42.

  According to the method of the third embodiment described above, by using a plurality of approximate models, a corrected joint angle (corrected approximate solution) closer to the true solution is set to an initial value, and inverse kinematics is performed by an iterative method. Can be obtained. Accordingly, it is possible to realize higher-level and higher-speed control for a robot having a redundant degree of freedom and a complicated structure such as an offset.

5. Fourth and Fifth Examples FIGS. 16 and 17 show configuration examples of a robot control system (and a robot system including the same) according to fourth and fifth examples of the present embodiment. In addition, about the structure of a robot control system, or its structural requirement, the description which overlaps with the 1st, 2nd, 3rd Example is abbreviate | omitted suitably.

  The robot control system of FIGS. 16 and 17 includes a target endpoint position acquisition unit 20, a genuine model storage unit 22, an approximate model storage unit 24, and a processing unit 30. A robot controller 80 can also be included. In FIG. 17, an operation history storage unit 26 is further provided.

  For example, the processing unit 30 of the fourth embodiment shown in FIG. 16 determines the end point position based on the approximate solution by setting the approximate solution to the joint angle of the authentic model. Specifically, the end point position is obtained by forward kinematics processing. Then, the processing unit 30 sets the joint angle corresponding to the selected end point position (hand position / posture) from the end point position by the approximate solution and the current end point position to an initial value, and performs an iterative calculation process. To find the solution in the inverse kinematics of the robot. For example, the processing unit 30 includes a current end point position acquisition unit 45, and the current end point position acquisition unit 45 acquires the current end point position and joint angle from the robot control unit 80. For example, the position of the end effector portion of the arm and the joint angle of the arm in the current posture are acquired. Then, the initial value selection unit 46 selects the joint angle corresponding to the end point position selected from the end point position based on the approximate solution and the current end point position as the initial value used for the iterative calculation processing.

  More specifically, the processing unit 30 obtains a first error between the end point position and the target end point position based on the approximate solution. Also, a second error between the current end point position and the target end point position is obtained. Then, when the first error is smaller than the second error, the processing unit 30 sets the joint angle corresponding to the end point position by the approximate solution (joint angle that is the approximate solution) to the initial value. . On the other hand, when the second error is smaller than the first error, the joint angle corresponding to the current end point position (the joint angle of the current arm) is set to the initial value.

  Alternatively, when the point-to-point control of the robot is performed, the processing unit 30 sets the joint angle corresponding to the end point position based on the approximate solution to the initial value. On the other hand, when the continuous control of the robot is performed, the joint angle corresponding to the current end point position is set to the initial value.

  Here, the point-to-point control is called PTP control, and is control performed by a command specifying only the pose (posture) of a jump point, for example. For example, a representative point on the operation route is taught, only the point is controlled, and the operation route (movement route) toward each point does not matter. On the other hand, the continuous control is called CP control, and is control performed by a command designating all tracks or all routes, for example. For example, the robot arm is caused to follow by continuously interpolating between predetermined points. When this control is performed, a continuous operation path is taught.

  The robot control system of the fifth embodiment of FIG. 17 further includes an operation history storage unit 26. The operation history storage unit 26 stores an operation history end point position that is an operation history position of a past end point and a joint angle at that time. For example, the operation history storage unit 26 is configured by a FIFO, and the end point position and joint angle at each timing are sequentially stored in the FIFO as operation history data.

  The processing unit 30 in FIG. 17 selects the end point position based on the approximate solution, the current end point position from the current end point position acquiring unit 45, and the operation history end point position from the operation history storage unit 26. The joint angle corresponding to the determined end point position is set to an initial value, and an iterative calculation process is performed to obtain a solution in the inverse kinematics of the robot. The initial value selection is performed by the initial value selection unit 46.

  Specifically, the processing unit 30 obtains a first error between the end point position and the target end point position by the approximate solution. Also, a second error between the current end point position and the target end point position is obtained. Further, a third error between the operation history end point position and the target end point position is obtained. Then, when the first error is smaller than the second error and the third error, the processing unit 30 sets the joint angle corresponding to the end point position by the approximate solution to the initial value. When the second error is smaller than the first error and the third error, the joint angle corresponding to the current end point position is set to the initial value. If the third error is smaller than the first error and the second error, the joint angle corresponding to the motion history end point position is set to the initial value. That is, the end point position with the smallest error from the target end point position is selected from the end point position by the approximate solution, the current end point position, and the operation history end point position, and the end point position is selected. A corresponding joint angle (joint angle at the time of the hand position / posture) is set to an initial value in the iterative calculation processing.

  Next, details of the methods of the fourth and fifth examples of the present embodiment will be described. In the method of the fourth embodiment, the designated target end point position (target hand position / posture) is compared with the current end point position (current hand position / posture), and the difference (error) between them is small. The solution is obtained by an iterative method with the joint angle at the current end point position as an initial value. Specifically, it is determined which of the approximate end point position and the current end point position is closer to the target end point position, and the closer joint angle is adopted as the initial value. On the other hand, in the fifth embodiment, the robot has history data for storing a predetermined number of past end point positions (hand position and posture) and joint angles at that time, and a designated target end point position (target position). When the difference (error) is minimum and the difference is smaller than a predetermined difference, a solution is obtained by an iterative method using the minimum history data as an initial value. That is, it has an operation history storage unit 26 for storing past operation history, and adopts the closest value to the target position among the approximate solution based on the approximate model, the current position, and the operation history as an initial value.

  For example, FIG. 18A shows an example in which the joint angle at the end point position based on the approximate solution is set as the initial value. For example, in PTP control (point-to-point control), the current end point position before the jump by the PTP control and the target end point position after the jump are often separated in distance, and the operation between them The route is controlled regardless of the route. In this case, as shown in FIG. 18A, the joint angle, which is an approximate solution, is set as an initial value, and the inverse kinematics iterative calculation process is performed. In this way, when the current end point position and the target end point position are distant from each other as in PTP control, the approximate end point approaches the target end point position, and the remaining error is processed by iterative calculation processing. It becomes possible to correct.

  FIG. 18B shows an example in which the joint angle (current arm joint angle) corresponding to the current end point position is set as the initial value. For example, in CP control (continuous control), the current end point position and the target end point position are often close in distance, and the designated point of the operation path is set finely. In this case, as shown in FIG. 18B, the joint angle corresponding to the current end point position is set to the initial value, and the inverse kinematics iterative calculation process is performed. In this way, when the current end point position and the target end point position are close in distance and finely controlled as in CP control, the approximate solution is more used than the initial value. An inverse kinematic solution can be obtained efficiently and in a short time.

  FIG. 18C shows an example where the joint angle corresponding to the motion history end point position is set as the initial value. For example, when there is an end point position closer to the target end point position than the end point position based on the approximate solution in the past operation history end point positions, the operation history end point position is selected. Then, the joint angle corresponding to the selected motion history end point position is set to an initial value, and the inverse kinematics iterative calculation process is performed. In this way, in some cases, it is possible to obtain an inverse kinematics solution more efficiently and in a shorter time than when an approximate solution is set to an initial value.

  FIG. 19 is a flowchart showing detailed processing of the fourth embodiment. First, a target end point position is acquired (S51). Then, as in the first embodiment, an approximate solution is obtained using an approximate model (S52).

  Next, it is determined whether or not a solution has been obtained (S53). If not obtained, the current joint angle (joint angle corresponding to the current end point position) is selected as an initial value (S62). . On the other hand, when a solution is obtained, the obtained approximate solution is used as an initial value, and the solution is obtained by an iterative method (S54). Then, it is determined whether or not a solution is obtained (S55). If not obtained, the current joint angle is selected as an initial value (S62). On the other hand, if it is obtained, the end point position by the approximate solution is obtained from the obtained approximate solution and the authentic model (S56). For example, the end point position is obtained by forward kinematics processing.

  Next, a first error between the obtained end point position and the target end point position is obtained (S57). Further, the current end point position is acquired (S58), and a second error between the current end point position and the target end point position is obtained (S59).

  Next, the first error obtained in step S57 is compared with the second error obtained in step S59 (S60). If the first error is smaller than the second error, an approximate solution is selected as an initial value as shown in FIG. 18A (S61). On the other hand, if the second error is smaller than the first error, the current joint angle is selected as the initial value as shown in FIG. 18B (S62).

  FIG. 19 is a flowchart showing detailed processing of the fifth embodiment. The processing in steps S71 to S79 in FIG. 19 is the same as the processing in steps S51 to S59 in FIG.

  After step S79, an error between the operation history end point position and the target end point position is obtained as a third error (S80). Next, it is determined whether or not the first error obtained in step S77 is minimum (S81). If the first error is minimum, the joint of the approximate solution as shown in FIG. The corner is selected as an initial value (S82). On the other hand, if the first error is not minimum, it is determined whether or not the second error is minimum (S83). If the second error is minimum, the current joint angle is selected as the initial value as shown in FIG. 18B (S84). On the other hand, when the second error is not minimum, the operation history data with the minimum error is selected as an initial value (S85). For example, in FIG. 18C, an operation history end point position that minimizes an error from the target end point position is selected from a plurality of operation history end point positions, and corresponds to the selected operation history end point position. Select the joint angle to be used as the initial value.

  Although the present embodiment has been described in detail as described above, it will be easily understood by those skilled in the art that many modifications can be made without departing from the novel matters and effects of the present invention. Accordingly, all such modifications are intended to be included in the scope of the present invention. For example, a term (hand position / posture, approximate analytical solution, etc.) described together with a different term (endpoint position, approximate solution, etc.) in a broader sense or the same meaning at least once in the specification or drawing, It can also be replaced with the different terminology. Further, all combinations of the examples and modifications of the present embodiment are also included in the scope of the present invention. In addition, the configuration and operation of the robot control system, the robot system, the calculation method of the approximate solution, the calculation method of the inverse kinematics solution, the configuration of the approximate model and the authentic model are not limited to those described in this embodiment, and various Can be implemented.

MT genuine model, MA (MA-1 to MAm) approximate model,
10 force sensor, 12 force control unit (impedance control unit),
20 target end point position acquisition unit, 22 authentic model storage unit,
24 approximate model storage unit, 26 operation history storage unit,
30 processing units, 40 (40-1 to 40-m) approximate solution calculation unit,
42 forward kinematics processing unit, 43 target end point correction unit,
44 approximate solution selection unit, 45 current endpoint position acquisition unit, 46 initial value selection unit,
50 iteration processing unit, 52 convergence determination unit, 54 error processing unit,
60 target value output unit, 62 trajectory generation unit, 64 inverse kinematics processing unit,
80 Robot controller, 82-1 to 82-N Drive controller,
100 robot, 102-1 to 102-N drive unit,
300 control device, 310 robot body, 320 arm, 330 hand

Claims (13)

An end point position acquisition unit for acquiring a target end point position of the robot;
An authentic model storage unit for storing information on the authentic model of the robot;
An approximate model storage unit that stores information of an approximate model that approximates the authentic model;
An approximate solution is obtained using the approximate model and the target end point position, and an iterative calculation process is performed using the approximate solution and the authentic model, thereby obtaining a solution in inverse kinematics of the robot. A processing unit;
A robot control system comprising: In claim 1,
The processor is
A robot control system, wherein the approximate solution is set as an initial value and the iterative calculation process is performed. In claim 1,
The processor is
By setting the approximate solution to the joint angle of the authentic model, the end point position by the approximate solution is obtained,
By correcting the target end point position based on an error between the end point position by the approximate solution and the target end point position, a corrected target end point position is obtained,
Using the corrected target end point position and the approximate model, a second approximate solution is obtained,
A robot control system characterized in that a solution in inverse kinematics of the robot is obtained by performing the iterative calculation process using the second approximate solution and the authentic model. In claim 1,
The approximate model storage unit includes:
As a model that approximates the authentic model, information on the first to m-th approximate models is stored,
The processor is
Using the first to mth approximate models and the target endpoint position, find first to mth approximate solutions,
A robot characterized in that an approximate solution selected from the first to mth approximate solutions is set as an initial value, and a solution in inverse kinematics of the robot is obtained by performing the iterative calculation process Control system. In claim 4,
The processor is
Using the i th (1 ≦ i ≦ m) approximate model of the first to m th approximate models and the target end point position, an i th approximate solution is obtained,
By setting the i-th approximate solution to the joint angle of the genuine model, the i-th endpoint position and the Jacobian by the approximate solution are obtained,
Using the first error between the i-th end point position and the target end point position and the Jacobian, the joint angle is corrected, and the corrected joint angle is set as the joint angle of the genuine model. To obtain the i-th correction end point position,
A robot control system, wherein an approximate solution selected based on a second error between the i-th corrected end point position and the target end point position is set as the initial value. In claim 1,
The processor is
By setting the approximate solution to the joint angle of the authentic model, the end point position by the approximate solution is obtained,
By setting the joint angle corresponding to the selected end point position to the initial value from the end point position by the approximate solution and the current end point position, and performing the iterative calculation process, A robot control system characterized by finding solutions in inverse kinematics. In claim 6,
The processor is
Determining a first error between the end point position by the approximate solution and the target end point position;
Determining a second error between the current endpoint position and the target endpoint position;
When the first error is smaller than the second error, the joint angle corresponding to the end point position by the approximate solution is set to the initial value,
When the second error is smaller than the first error, a joint angle corresponding to the current end point position is set to the initial value. In claim 6,
The processor is
When point-to-point control of the robot is performed, the joint angle corresponding to the end point position by the approximate solution is set to the initial value,
A robot control system characterized in that, when continuous control of the robot is performed, a joint angle corresponding to the current end point position is set to the initial value. In any of claims 6 to 8,
An operation history storage unit that stores an operation history end point position that is an operation history position of a past end point;
The processor is
The joint angle corresponding to the selected end point position is set to the initial value from among the end point position by the approximate solution, the current end point position, and the operation history end point position, and the iterative calculation process To obtain a solution in the inverse kinematics of the robot. In claim 9,
The processor is
Determining a first error between the end point position by the approximate solution and the target end point position;
Determining a second error between the current endpoint position and the target endpoint position;
Determining a third error between the action history endpoint position and the target endpoint position;
When the first error is smaller than the second error and the third error, the joint angle corresponding to the end point position by the approximate solution is set to the initial value,
When the second error is smaller than the first error and the third error, the joint angle corresponding to the current end point position is set to the initial value,
When the third error is smaller than the first error and the second error, a joint angle corresponding to the motion history end point position is set to the initial value. Robot control system. In any one of Claims 1 thru | or 10.
The approximate model storage unit includes:
A robot control system, wherein information of a model in which an offset of a joint of the genuine model is omitted is stored as information of the approximate model. The robot control system according to any one of claims 1 to 11,
The robot;
A robot system characterized by including: Stores information about the authentic model of the robot,
Storing information of an approximate model approximating the authentic model;
Obtaining the target end point position of the robot;
An approximate solution is obtained using the approximate model and the target endpoint position, and an iterative calculation process is performed using the approximate solution and the authentic model to obtain a solution in the inverse kinematics of the robot. ,
A robot control method, comprising: controlling the robot based on a determined solution.

Images