JP2021146409A - Inverse kinematics operation unit and inverse kinematics operation method - Google Patents

Abstract

To enable an appropriate solution concerning a transit time in a joint angle interpolation trajectory to be selected from among a plurality of solutions of an inverse kinematics operation.SOLUTION: An inverse kinematics operation device comprises: an input unit 1 which receives input information including a start point represented by a joint space representation in a robot arm having a plurality of joints and an end point represented by a Cartesian space representation; a solution set generating unit 2 which finds a plurality of solutions of an inverse kinematics operation concerning an end point included in the input information received by the input unit 1; and a solution selecting unit 3 which selects, from among solutions found by the solution set generating unit 2, a solution in which a transit time in a joint angle interpolation trajectory between the start point and the solution included in the input information received by the input unit 1 satisfies conditions.SELECTED DRAWING: Figure 1

Description

The present invention relates to an inverse kinematics calculation device and an inverse kinematics calculation method for performing an inverse kinematics calculation on a robot arm having a plurality of joints.

There are two methods for expressing the state of a robot arm having a plurality of joints (articulated robot arm): joint space expression and Cartesian space expression.

The joint space expression expresses the state of the robot arm by the angle (rotation angle) of each joint of the robot arm. In the case of a 6-axis robot arm, the state of the robot arm can be expressed by a 6-dimensional joint angle vector. In the following, the joints of the vertical articulated robot arm are represented by J1, J2, ... In order from the root side, and the joint angles are represented by θ ₁ , θ ₂ , ... In order from the root side.

The Cartesian space representation expresses the state of the robot arm by the position and posture of the hand of the robot arm in the Cartesian space. In the Cartesian space representation, the state of the robot arm can be represented by a total of 6-dimensional data of the 3-dimensional position and the 3-dimensional posture. The position is represented by X = {x, y, z}, and the posture is represented by R = {roll, pitch, yaw}. The posture may be expressed by a quaternion, and Q is uniquely obtained from R. The quaternion is represented by Q = {q _x , q _y , q _z , q _w }. In the following, the Cartesian space representation is represented by P = {X, R} = {X, Q}.

Then, the operation for converting from the joint space to the Cartesian space is called a forward kinematics operation, and the operation for converting from the Cartesian space to the Cartesian space is called an inverse kinematics operation. When the user causes the robot arm to perform some work, the user often gives a movement instruction to the robot arm in a Cartesian space representation, so that an inverse kinematics calculation is required.

There are mainly two methods of motion planning when moving the robot arm from the start point to the end point, a method by joint angle interpolation movement and a method by linear movement.
The joint angle interpolation movement is a movement along a trajectory (joint angle interpolation trajectory) connecting the start point and the end point in the joint space with a line (typically linear) in a specified range. The joint angle interpolation trajectory is generated by performing an inverse kinematics operation on the end point whose motion is planned in the joint space.
Linear movement is movement along an orbit that linearly connects the start point and the end point in Cartesian space. The trajectory of linear movement is generated by repeating inverse kinematics operations on a plurality of discrete points between the start point and the end point whose motion is planned in Cartesian space.

When there is a singular point between the start point and the end point, it is difficult to plan the motion with linear movement, so joint angle interpolation movement is mainly used. Further, in the linear movement, since the motors provided in each joint perform non-linear acceleration / deceleration, the movement time is generally longer than that in the joint angle interpolation movement. Therefore, when the user only requests to shorten the movement time of the robot arm, the joint angle interpolation movement is used.

On the other hand, there are multiple solutions for inverse kinematics operations. Therefore, it is necessary to select a solution that suits the user's purpose from a plurality of solutions (solution sets).

When a user operates a robot arm, the purpose (use case) of the operation is various. For example, when the user moves the robot arm from the start point to the end point by interpolating the joint angle, the purpose of the movement includes movement (in preparation for the next movement), transportation of an object, and the like.

When the purpose of the movement is movement, it is desirable to move the robot arm in the shortest possible time from the viewpoint of work efficiency. In the field, improvements to improve the work efficiency of the robot arm are constantly required, and it is very important to reduce the moving time of the robot arm. Since the movement time of the joint angle interpolation movement is determined by the solution of the inverse kinematics operation (joint angle vector at the end point), it is required to select a solution having an effect of reducing the movement time from a plurality of solutions.

On the other hand, conventionally, an inverse kinematics arithmetic unit that performs numerical calculation using a Jacobian matrix has been widely used (see, for example, Non-Patent Document 1). In this inverse kinematics calculation device, a solution in the gradient (Jacobian matrix) direction obtained from an initial value (joint angle vector) as input information is selected from a plurality of solutions. In this case, the user selects a solution by manipulating the initial value.

Inverse kinematics arithmetic units that specify solutions using structural flags are also used (see, for example, Non-Patent Document 2). In this inverse kinematics arithmetic unit, attention is paid to the geometrical characteristics of the structure of the robot arm, and the solutions are classified by indexes called structural flags such as the Shoulder index, the Elbow index, and the Wrist index. In this case, the user selects the solution by specifying the structure flag.

Tomomichi Sugihara: "Numerical Solution of Inverse Kinematics", Journal of Robotics Society of Japan, Vol.34 No.3, pp.167-173, 2016. Possible Combinations of Robot Shoulder, Elbow, and Wrist Figures <URL: http://www.ccarafa.it/RC8/001954.html> John J. Crag: “Robotics (13th Edition)”, Kyoritsu Shuppan, 2014. Kelsey P. Hawkins: “Analytic Inverse Kinematics for the Universal Robots UR-5 / UR-10 Arms”, 2017. Cuong Trinh, Dimiter Zlatanov, Matteo Zoppi, and Rezia Molfino: “A GEOMETRICAL APPROACH TO THE INVERSE KINEMATICS OF 6R SERIAL ROBOTS WITH OFFSET WRISTS”, Proceedings of the ASME 2015 International Design Engineering Technical Conferences & Computers and Information in Engineering Conference, DETC2015- 47950, 2015.

As described above, conventionally, the solution of the inverse kinematics operation is selected by using an intermediate concept such as an initial value or a structure flag. However, in this intermediate concept, it is difficult to select an appropriate solution for the movement time in the joint angle interpolation trajectory from a plurality of solutions.

The present invention has been made to solve the above-mentioned problems, and an inverse kinematics operation that can select an appropriate solution for the movement time in the joint angle interpolation trajectory from a plurality of solutions of the inverse kinematics operation. The purpose is to provide the device.

The inverse kinematics arithmetic device according to the present invention receives input information including a start point represented by the joint space representation and an end point represented by the Cartesian space representation in a robot arm having a plurality of joints, and an input device. A solution set generator that finds multiple solutions for inverse kinematics operations for the end point included in the input information, and a start point included in the input information received by the input device from among the solutions obtained by the solution set generator. It is characterized by having a solution selector for selecting a solution that satisfies the movement time in the joint angle interpolation trajectory between the solution and the solution.

According to the present invention, since it is configured as described above, it is possible to select an appropriate solution for the movement time in the joint angle interpolation trajectory from a plurality of solutions of the inverse kinematics operation.

It is a figure which shows the structural example of the inverse kinematics arithmetic unit which concerns on Embodiment 1. FIG. It is a flowchart which shows the operation example of the inverse kinematics arithmetic unit which concerns on Embodiment 1. It is a figure which shows the structural example of the inverse kinematics arithmetic unit which concerns on Embodiment 2. It is a figure which shows the structural example of the link mechanism of the vertical 6-joint robot arm which has a spherical orthogonal wrist mechanism. It is a figure explaining the kinematic decomposition of the link mechanism shown in FIG. It is a figure explaining the structure flag (shoulder index). It is a figure explaining the structure flag (Elbow index). It is a figure explaining the structure flag (Wrist index). It is a figure which shows the structural example of the link mechanism of the robot arm which has an offset wrist mechanism. It is a figure which shows the structural example of the link mechanism of the robot arm (J4-6 offset arm) which has an offset wrist mechanism. It is a figure which shows the structural example of the link mechanism (simplification) of the J4-6 offset arm. It is a figure which shows an example of the evaluation function.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1.
FIG. 1 is a diagram showing a configuration example of an inverse kinematics arithmetic unit according to the first embodiment.
The inverse kinematics arithmetic unit performs inverse kinematics arithmetic on a robot arm having a plurality of joints. As shown in FIG. 1, this inverse kinematics arithmetic unit includes an input device 1, a solution set generator 2, a solution selector 3, and an output device 4.

The inverse kinematics arithmetic unit is realized by a processing circuit such as a system LSI (Large Scale Integration), a CPU (Central Processing Unit) that executes a program stored in a memory or the like, or the like.

The input device 1 receives the input information. The input information includes the start point and the end point of the robot arm. The start point is represented by the joint space representation, and the end point is represented by the Cartesian space representation (base coordinate system with the point O as the origin). In addition, the input information may include information required for selecting a solution for inverse kinematics operations.

Based on the input information received by the input device 1, the solution set generator 2 obtains a plurality of solutions for the inverse kinematics operation for the end point included in the input information. At this time, the solution set generator 2 may obtain a population of solutions (set of all solutions) as a plurality of solutions, or obtain a subset of solutions when some constraint is specified as an option. You may.

The solution selector 3 selects a solution from the solutions obtained by the solution set generator 2 based on the input information received by the input device 1. At this time, the solution selector 3 selects a solution from the above solutions whose travel time in the joint angle interpolation trajectory connecting the start point and the solution included in the input information satisfies the condition. If the solution set generator 2 does not find a solution, the solution selector 3 considers an error and does not select a solution.

The output device 4 outputs data indicating the solution (joint space representation) selected by the solution selector 3.

Next, a configuration example of the solution set generator 2 will be described.
As shown in FIG. 1, the solution set generator 2 has an initial value set generator 21 and an inverse kinematics calculator 22.

The initial value set generator 21 generates a plurality of initial values which are joint angle vectors.

The inverse kinematics calculator 22 uses the initial values for each initial value generated by the initial value set generator 21 to perform an inverse kinematics calculation for the end point included in the input information received by the input device 1. Find a solution.

Next, a configuration example of the solution selector 3 will be described.
As shown in FIG. 1, the solution selector 3 includes a joint angle interpolation trajectory evaluator 31 and an evaluation index selector 32.

The joint angle interpolation trajectory evaluator 31 determines the movement time in the joint angle interpolation trajectory between the start point included in the input information received by the input device 1 and the solution for each solution generated by the solution set generator 2. Ask. Since the joint angle interpolation trajectory evaluator 31 only needs to obtain the movement time, it is not necessary to generate the joint angle interpolation trajectory.

The evaluation index selector 32 selects a solution from the solutions generated by the solution set generator 2 based on the movement time (index value) for each solution obtained by the joint angle interpolation trajectory evaluator 31.

Next, an operation example of the inverse kinematics arithmetic unit according to the first embodiment shown in FIG. 1 will be described with reference to FIG.
Here, since the start point is given from the current state of the robot arm, the joint angle interpolation trajectory from the start point to the end point depends only on the solution of the inverse kinematics calculation for the end point. Therefore, the inverse kinematics arithmetic unit according to the first embodiment controls the joint angle interpolation trajectory by "selecting the solution of the inverse kinematics arithmetic in consideration of the movement time from the start point to the end point", and the above-mentioned problem. To solve.

In the operation example of the inverse kinematics arithmetic unit according to the first embodiment shown in FIG. 1, as shown in FIG. 2, the input device 1 first receives the input information (step ST201). The input information includes a start point represented by the joint space representation in the robot arm and an end point represented by the Cartesian space representation.

Next, the initial value set generator 21 generates a plurality of initial values that are joint angle vectors (step ST202).

Here, if the initial value set generator 21 comprehensively generates a set of initial values within the range of joint movement of the robot arm, the inverse kinematics calculator 22 can obtain a population of solutions from the set of initial values. .. However, it is not practical in terms of calculation cost that the initial value set generator 21 comprehensively searches for the initial value within the range of joint movement of the robot arm because the number of combinations is enormous.

Therefore, for example, the initial value set generator 21 may reduce the number of initial values by narrowing the search range of the initial values from the joint movable range to generate a set of initial values. That is, since the joint movable range becomes the same position in a 2π cycle, if the joint movable range is, for example, ± 4π, the initial value set generator 21 narrows the initial value search range to, for example, ± 2π or ± π. You may. Further, the initial value set generator 21 may reduce the number of initial values by randomly generating a set of a certain number of initial values. Further, when a set of a certain number of initial values that can obtain as many kinds of solutions as possible can be obtained in advance based on the know-how on the geometric structure of the robot arm, the initial value set generator 21 generates the set of the initial values. , The number of initial values may be reduced. The initial value set generator 21 generates a set of initial values as described above, for example, with the calculation time as a constraint condition. Then, in the inverse kinematics calculator 22, a subset of the solution is obtained from the set of initial values as described above.

Further, when the maximum number of types of solutions is known, the inverse kinematics arithmetic unit 22 does not need to solve the inverse kinematics calculation using all the initial values generated by the initial value set generator 21, as described above. It is practically desirable to end the process when the population of the maximum number of solutions is obtained.

Next, the inverse kinematics calculator 22 uses the initial value for each initial value generated by the initial value set generator 21 and reverse kinematics about the end point included in the input information received by the input device 1. Find the solution of the operation (step ST203). At this time, first, the inverse kinematics calculator 22 obtains a solution (joint angle vector) of the inverse kinematics calculation for the end point included in the input information received by the input device 1. Further, the inverse kinematics calculator 22 obtains the Jacobian matrix (gradient) from the initial values generated by the initial value set generator 21. Then, the inverse kinematics calculator 22 selects the obtained solution in the gradient direction from the obtained solutions for the inverse kinematics calculation. That is, the solution obtained by the inverse kinematics calculator 22 changes depending on the initial value. The details of the processing by the inverse kinematics arithmetic unit 22 (the method of obtaining the solution of the inverse kinematics arithmetic using the initial values) are the same as those disclosed in Non-Patent Document 1, for example, and the details will be described. Is omitted.

Next, the joint angle interpolation trajectory evaluator 31 moves in the joint angle interpolation trajectory between the start point included in the input information received by the input device 1 and the solution for each solution generated by the solution set generator 2. Find the time (step ST204).

At this time, the joint angle interpolation trajectory evaluator 31 obtains the movement time in the joint angle interpolation trajectory between the start point represented by the joint space representation and the end point represented by the joint space representation. The start point is a start point included in the input information and is represented _{by Θ start.} The end point is one of a plurality of (k) solutions generated by the solution set generator 2, and is represented _{by Θ goal.} Specifically, the joint angle interpolation trajectory evaluator 31 obtains the movement time in the joint angle interpolation trajectory by sequentially executing the following processing for each solution.

<Calculation example of travel time 1>
First, consider the uniaxial joint angle interpolation trajectory.
Here, the acceleration / deceleration pattern in which a joint has the fastest speed from the start point to the end point is a triangular acceleration / deceleration pattern. Regarding the midpoint of the orbit of the triangular acceleration / deceleration pattern, the relationship between the angular velocity and the angular acceleration is expressed by the following equation (1). In equation (1), Θ (dot) represents the angular velocity, Θ (two dots) represents the angular acceleration, and t _1d represents the travel time.

In this case, the relationship between the angle and the angular velocity is expressed by the following equation (2).

Therefore, the travel time is given by the following equation (3).

Further, when the angular velocity obtained above is larger than the maximum angular velocity value at the midpoint of the orbit of the triangular acceleration / deceleration pattern, a trapezoidal acceleration / deceleration pattern in which the angular velocity is leveled off at the maximum angular velocity value is considered.
In this case, the acceleration time is expressed by the following equation (4). In the formula (4), _{t a} represents the acceleration time, theta _{(dot) max} represents the angular velocity maximum. Further, in the equation (4), the angular acceleration represented by Θ (two dots) may be changed to the maximum angular acceleration value.

The constant velocity time is expressed by the following equation (5). In equation (5), t _c represents a constant velocity time.

Therefore, the travel time is given by the following equation (6).

In the above, the case where the acceleration / deceleration pattern is a trapezoidal acceleration / deceleration pattern is shown, but the acceleration / deceleration pattern is not limited to this, and the acceleration / deceleration pattern can be changed by changing the setting of the angular acceleration (Θ (two dots) in the equation (4)). You may. For example, by making the angular acceleration a function of time, the acceleration / deceleration pattern becomes an S-shaped acceleration / deceleration pattern.

なお以下では、ｋ番目の解に対する関節角補間軌道における移動時間（指標値）をＨ_ｋと表記する。 Then, the movement time of the joint angle interpolation trajectory for all joints is the maximum value of the movement time of each axis as shown in the following equation (7). In equation (7), t represents the movement time of the joint angle interpolation trajectory.

In the following, the movement time in the joint angle interpolation trajectories for the k-th solution (the index value) is denoted by H _k.

<Calculation example of travel time 2>
The calculation method of the travel time shown above is an example. If the angular accelerations of all axes are the same, the joint angle interpolation trajectory evaluator 31 can calculate the movement time of each axis as an approximate value as shown in the following equation (8) in order to reduce the calculation cost. good.

<Calculation example of travel time 3>
If the angular velocities and angular accelerations of all axes are the same, the joint angle interpolation trajectory evaluator 31 calculates the movement time of each axis as an approximate value as shown in the following equation (9) in order to reduce the calculation cost. You may. When the angular velocity and the angular acceleration of all axes are the same, the movement time of each axis is proportional to the angular difference (distance in the joint space).

However, there are almost no cases in which the angular acceleration of all axes is the same or the angular velocity and angular acceleration of all axes are the same. Therefore, in practice, it is difficult to select a solution having a high effect of reducing the travel time by a method of selecting a solution by simply focusing on the angle difference of each axis as in Calculation Example 3. That is, in order to select a solution having a higher effect of reducing the travel time, the joint angle interpolation trajectory evaluator 31 estimates the travel time assuming some acceleration / deceleration pattern or angular acceleration setting as in calculation example 1. Is desirable.

In the above calculation examples 1 to 3, the maximum angular velocity value and the maximum angular acceleration value may be predetermined fixed values, may be specified by the user as input information, or the joint angle interpolation trajectory evaluator 31. May be calculated from the equation of motion.

Next, the evaluation index selector 32 selects a solution from the solutions generated by the solution set generator 2 based on the movement time (index value) for each solution obtained by the joint angle interpolation trajectory evaluator 31. (Step ST205).

Here, the evaluation index selector 32 selects a solution satisfying a predetermined condition from the above solutions. For example, the evaluation index selector 32 selects one solution having the shortest travel time as the solution having the highest travel time reduction effect. The solution with the shortest travel time can be obtained from the following equation (10). In equation (10), K represents the number of the solution with the shortest travel time.

On the other hand, it may be better for the evaluation index selector 32 not to select the solution having the shortest travel time. For example, when an external device tries to generate a trajectory for a certain solution output by the inverse kinematics arithmetic unit, it is found that self-interference occurs during the movement, and it may not be possible to realize the arm operation. Since the robot arm can be operated only after the trajectory is obtained, it is not desirable for the inverse kinematics arithmetic unit to output a solution in which the arm operation cannot be realized. Therefore, in reality, the evaluation index selector 32 selects the solution having the shortest movement time from the solutions satisfying other conditions such as the feasibility of arm operation, or sequentially from the solution having the shortest movement time. After verifying whether other conditions are met, the solution is selected. In this way, the evaluation index selector 32 can select a solution in combination with not only the travel time but also other conditions or other evaluation functions, but basically, the effect of reducing the travel time is more effective. Try to choose a higher solution. Therefore, when selecting a solution, the evaluation index selector 32 needs to select at least based on the travel time, and if other conditions or other evaluation functions are the same, the solution with the shortest travel time is prioritized. Should be selected.

Next, the output device 4 outputs data indicating the solution (joint space representation) selected by the solution selector 3 (step ST206).

As described above, according to the first embodiment, the inverse kinematics calculation device includes input information including a start point represented by the joint space representation and an end point represented by the decart space representation in a robot arm having a plurality of joints. Among the solutions obtained by the input device 1 that accepts Therefore, a solution selector 3 for selecting a solution that satisfies the movement time in the joint angle interpolation trajectory between the start point and the solution included in the input information received by the input device 1 is provided. As a result, the inverse kinematics arithmetic unit according to the first embodiment can select an appropriate solution regarding the movement time in the joint angle interpolation trajectory from the plurality of solutions of the inverse kinematics arithmetic.

Embodiment 2.
FIG. 3 is a diagram showing a configuration example of the inverse kinematics arithmetic unit according to the second embodiment. The inverse kinematics arithmetic unit according to the second embodiment shown in FIG. 3 has an initial value set generator 21 and an inverse kinematics arithmetic unit 22 with respect to the inverse kinematics arithmetic unit according to the first embodiment shown in FIG. The structure flag set generator 23 and the inverse kinematics arithmetic unit 24 have been changed. Other configurations are the same as those of the inverse kinematics arithmetic unit according to the first embodiment, and the same reference numerals are given and the description thereof will be omitted.

The structure flag set generator 23 generates a plurality of structure flags. The structure flag is an index for designating the solution of the inverse kinematics operation. Examples of this structural flag include a Shoulder index, an Elbow index, and a Wrist index.

The inverse kinematics calculator 24 uses the structural flags for each structural flag generated by the structural flag set generator 23 to perform an inverse kinematics calculation for the end point included in the input information received by the input device 1. Find a solution.

Next, an operation example of the structure flag set generator 23 will be described. First, a case where the robot arm is a vertical 6-joint robot arm having a spherical orthogonal wrist mechanism will be described.

In a vertical 6-joint robot arm with a spherical orthogonal wrist mechanism, it is clear that there are a maximum of 8 solutions for inverse kinematics calculations (provided that the range of motion of each joint is within ± 180 degrees). The solution can be uniquely classified by three indexes. Further, by specifying the type of solution with three indexes, an arbitrary solution can be obtained from eight solutions. In an industrial vertical 6-joint robot arm, a link mechanism as shown in FIG. 4 is often used.

A feature of the link mechanism of the vertical 6-joint robot arm having a spherical orthogonal wrist mechanism is that the position control mechanism and the posture control mechanism can be separated as shown in FIG. This feature is derived from the fact that the rotation axis of J4, the rotation axis of J5, and the rotation axis of J6 are orthogonal to each other at one point, and the attitude control mechanism is called a spherical orthogonal wrist mechanism.

First, considering the position control mechanism, the point A'in FIG. 6 coincides with the point A if J1 is rotated 180 degrees. Therefore, it can be seen that there are two types of joint angles (solutions of inverse kinematics calculation) indicating the position of point A. In order to distinguish between these two solutions, it is possible to determine whether the point A is on the left or right _{when considering the J1 coordinate system (when θ 1 = 0).} To put it the other way around, if you specify "to the right", you can uniquely determine the solution of the inverse kinematics operation. The flag that specifies the solution in this way is called a structure flag. In FIG. 6, there are two solutions in which the point A is the “right side” or the “left side” of the Shoulder boundary, which can be distinguished by the Shoulder index.

FIG. 7 is a conceptual diagram showing an Elbow index among the structural flags. Considering the position control mechanism, there are two solutions for the straight line connecting the points A and J2, that is, "upper" and "lower" of the Elbow boundary, and they can be distinguished by the Elbow index. ..

FIG. 8 is a conceptual diagram showing the Wrist index among the structural flags. Considering the attitude control mechanism, it can be seen that if J4 rotates, J6 and J6'match. Therefore, regarding the positive and negative of the joint angle of J5, there are two solutions, "positive side" and "negative side" of the Wrist boundary, which can be distinguished by the Wrist index.

Focusing on the structure flags as described above, the structure flag set generator 23 generates a plurality of structure flags.

Then, the inverse kinematics calculator 24 uses the structure flag for each structure flag generated by the structure flag set generator 23, and uses the inverse kinematics about the end point included in the input information received by the input device 1. Find the solution of the operation. Here, the details of the processing by the inverse kinematics arithmetic unit 24 (the method of obtaining the solution of the inverse kinematics arithmetic using the structural flag) are the same as the method disclosed in Non-Patent Document 3, for example, and the description thereof is omitted. do. Although the name of the structural flag is not used in Non-Patent Document 3, the solutions are classified into the positive side solution and the negative side solution of the ± sign in three places generated in the process of inverse kinematics calculation. The Shoulder index, Elbow index, and Wrist index are used for the classification.

From the above, for a vertical 6-joint robot arm having a spherical orthogonal wrist mechanism, the inverse kinematics calculation device performs inverse kinematics calculation for a total of eight groups of structural flags combining the Shoulder index, the Elbow index, and the Wrist index. By finding the solution of, a population of solutions can be obtained.

The inverse kinematics arithmetic unit may optionally accept the specification of the constraint of a certain structure flag to obtain a set of solutions. As a constraint of the structure flag, for example, there is a constraint that the Shoulder index is the same as the start point. If the Shoulder index is different between the start point and the end point, there is a high possibility that J1 will rotate significantly, and the possibility that the robot arm will interfere with peripheral objects will increase. Therefore, it is considered that such a restriction is practically possible.

In the above description, for the sake of simplicity, the case where the robot arm is a vertical 6-joint robot arm having a spherical orthogonal wrist mechanism has been described as an example, but the robot arm is not limited to this. Although the definition of each index of the structure flag and the number of indexes change when the structure of the robot arm changes, the inverse kinematics arithmetic unit according to the second embodiment can be similarly applied.

For example, a method for obtaining a solution of an inverse kinematics arithmetic unit when the robot arm is a robot arm as shown in FIG. 9 is disclosed in Non-Patent Document 4. In the robot arm shown in FIG. 9, in six of the joints of the robot arm, the rotation axis of J1 and the rotation axis of J4 are orthogonal (including the meaning of substantially orthogonality), and the rotation axis of J4. A robot arm having an offset wrist mechanism in which the rotation axis of J6 and the rotation axis of J6 are offset. Although the name of the structural flag is not used in Non-Patent Document 4, the solutions are classified as the positive side solution and the negative side solution of each ± sign generated in the process of inverse kinematics operation. Structural flags are used for the classification.

Further, in the inverse kinematics arithmetic unit according to the second embodiment, it is not necessary to define sufficient structural flags necessary for uniquely classifying all the solutions. For example, in the case of a robot arm as shown in FIG. 10, the Shoulder index and the Elbow index can be defined, but other indexes cannot be defined. In addition, there may be a maximum of eight or more solutions of the inverse kinematics arithmetic unit. Therefore, even when one combination of the Shoulder index and the Elbow index is given as the structure flag, there are a plurality of solutions. In the robot arm shown in FIG. 10, in six of the joints of the robot arm, the rotation axis of J1, the rotation axis of J4, and the rotation axis of J6 are parallel (including the meaning of substantially parallel). Moreover, it is a robot arm having an offset wrist mechanism in which the rotation axis of J4 and the rotation axis of J6 are offset (hereinafter, abbreviated as J4-6 offset arm).

On the other hand, a method has been proposed in which structural flags (Shoulder index and Elbow index) are defined for the J4-6 offset arm, and a solution is obtained by numerical calculation when a pair of Shoulder index and Elbow index are specified. (See, for example, Non-Patent Document 5). Hereinafter, an operation example of the inverse kinematics arithmetic unit 24 when the J4-6 offset arm is targeted will be described. The structure flag set generator 23 generates a pair of Shoulder index and Elbow index.

FIG. 11 is a diagram showing a link mechanism kinematically equivalent to the link mechanism of the J4-6 offset arm. Using a simplified link mechanism as shown in FIG. 11 facilitates kinematics calculations.
In FIG. 11, the point O is the root of the robot arm and the origin of the base coordinate system. The point P is the tip (hand) of the robot arm, and the position and posture at the point P are input as input information for the inverse kinematics calculation. The point G is a point where the point C is projected onto the xy plane of the base coordinate system. The point J is a point obtained by adding _{l 1} to the z coordinate of the base coordinate system of the point G. l ₁ represents the length between the points O and E. l ₂ represents the length between the points E and D. l ₃ represents the length between point D and point C. l ₄ represents the length between point C and point B. l ₅ represents the length between point B and point A. l ₆ represents the length between points A and P.

Non-Patent Document 5 proposes a method of performing inverse kinematics calculation focusing on the geometric characteristics in which the vector BC and the vector BA shown in FIG. 11 are orthogonal to each other in the J4-6 offset arm.
In this method, the inner product of the vector BC and the vector BA is used as the evaluation function. The evaluation function is represented by f. Then, first, searches the theta ₆ to be f (θ ₆₎ = 0 numerically. When f (θ ₆₎ = 0 and becomes theta ₆ is required, it is possible to determine the theta ₅ through? ₁ from the distal end of the robot arm in order. Since the definition of the evaluation function and the method of obtaining _{θ 5} to θ _{1 when θ 6} is given are disclosed in Non-Patent Document 5, the description thereof will be omitted.

An example of the evaluation function is shown in FIG. As shown in FIG. 12, _{there are a plurality of θ 6} (roots of the evaluation function) at which _{f (θ 6) = 0.} Solutions that could not be uniquely classified by the Shoulder index and Elbow index appear as a plurality of roots, and there are solutions for inverse kinematics operations for each root. Therefore, in order to obtain the population of the solution, all the roots of the evaluation function need to be found. As a method of finding all the roots of the evaluation function, for example, an algorithm composed of two stages of a global search and a detailed search may be used.

First, the global search by the inverse kinematics arithmetic unit 24 will be described.
As the global search, for example, a method by grid search can be used. That is, first, the inverse kinematics calculator 24 _{divides the range of θ 6} into a plurality of sections by Δθ, and obtains all the spaces including the roots. Here, the range is _{represented by [θ lower} , θ _upper ]. θ _lower represents the start point of the range and θ _upper represents the end point of the range. It should be _noted that | θ lower |, | θ _upper | ≤ π. Δθ represents a minute interval width. The interval is _{represented by [θ s} , θ _e ]. θ _s represents the start point of the section and θ _e represents the end point of the section. Further, for the i-th interval, θ _s [i] and θ _e [i] can be expressed as the following equations (11) and (12).

Then, the inverse kinematics calculator 24 makes a zero point inclusion determination for _{the plurality of divided [θ s} , θ _e]. That is, in the inverse kinematics calculator 24, when f (θ ₆ ) changes monotonically _{in [θ s} , θ _e ], | f (θ _s ) | <E or | f with respect to _{θ s and θ e.} When (θ _e ) | <E is satisfied, or when f (θ _s ) and f (θ _e ) have different codes, f (θ ₆ ) = 0 _{in [θ s} , θ _e]. It is determined that _{θ 6 is included.} E is a minute value. As E, it is desirable that a large value (for example, 10 ^-4 _{) is set so as not to overlook θ 6 such that f (θ 6} ) = 0.

Next, a detailed search by the inverse kinematics calculator 24 will be described.
The inverse kinematics calculator 24 obtains _{θ 6 such} that f (θ ₆ ) = 0 _{for [θ s} , θ _{e] obtained by the global search.} That is, the inverse kinematics calculator 24 obtains the root of f by numerical calculation. Examples of the algorithm for obtaining the root of an unknown nonlinear function by numerical calculation include a dichotomy method, a secant method, and a false position method. Here, the case where the inverse kinematics arithmetic unit 24 uses the Anderson-Bjork method (AB method) will be described.

The AB method is an improved method of the false position method. In the AB method, as in the false position method, θ _s or θ _e is updated to narrow down the section including the root, and the root is obtained. This AB method has an advantage that the root can be obtained with a smaller number of calculations than the pinch shooting method.

The calculation method (update method) of the update point by the AB method is as shown in the following formula (13), which is the same as the pinch shooting method. In equation (13), θ _emp represents an update point.

Here, in the inverse kinematics arithmetic unit 24, when f (θ _s ) and f (θ _temp ) have the same sign, there is no root in _{[θ s} , θ _{emp ], so θ s is set} to θ _temp. Update with. In this case, in the AB method, the coefficient m is used, and f (θ _e ) is replaced with mf (θ _{e ) in the next calculation of θ emp.} m is defined by the following equation (14). If m becomes negative as a result of the calculation, m = 0.5.

The method of calculating a fixed value at m = 0.5 is called the Illinois method. The speed of convergence is in the order of the pinch shooting method <Illinois method ≤ AB method.

On the other hand, in the inverse kinematics arithmetic unit 24, when f (θ _s ) and f (θ _emp ) have different signs, there is a root in _{[θ s} , θ _{emp ], so θ e is set} to θ _emp . Update. In this case, m is used in the AB method, and f (θ _s ) is replaced with mf (θ _{s ) in the next calculation of θ temp.} m is defined by the following equation (15). If m becomes negative as a result of the calculation, m = 0.5.

As described above, when the combination of the Shoulder index and the Elbow index is given as the structural flag, the inverse kinematics calculator 24 can find all the roots by the global search and the detailed search, and each of them is a set of solutions. The solution (joint angle vector) for the root can be obtained. Therefore, even in the case of the J4-6 offset arm, the inverse kinematics calculator 24 according to the second embodiment is applicable and effective.

In the present invention, within the scope of the invention, it is possible to freely combine each embodiment, modify any component of each embodiment, or omit any component in each embodiment. be.

1 Input device 2 Solution set generator 3 Solution selector 4 Outputr 21 Initial value set generator 22 Inverse kinematics calculator 23 Structural flag set generator 24 Inverse kinematics calculator 31 Joint angle interpolation trajectory evaluator 32 Evaluation index selection vessel

Claims (6)

An input device that receives input information including a start point represented by a joint space representation and an end point represented by a Cartesian space representation in a robot arm having multiple joints.
A solution set generator for finding a plurality of solutions of inverse kinematics operations for the end point included in the input information received by the input device, and
From the solutions obtained by the solution set generator, select a solution that satisfies the movement time in the joint angle interpolation trajectory between the start point and the solution included in the input information received by the input device. Inverse kinematics arithmetic unit equipped with a device. The solution set generator
An initial value set generator that generates multiple initial values that are joint angle vectors,
Inverse kinematics calculator that uses the initial values for each initial value generated by the initial value set generator to find the solution of the inverse kinematics operation for the end point included in the input information received by the input device. The inverse kinematics calculation device according to claim 1, wherein the device has. The solution set generator
A structural flag set generator that generates multiple structural flags that are indicators for specifying the solution of inverse kinematics operations,
For each structure flag generated by the structure flag set generator, the inverse kinematics arithmetic unit for finding the solution of the inverse kinematics operation for the end point included in the input information received by the input device using the structure flag. The inverse kinematics arithmetic unit according to claim 1, wherein the device has. The inverse kinematics arithmetic unit according to claim 3, wherein the robot arm has a structure in which a structure flag and a solution of an inverse kinematics arithmetic are one-to-many. The robot arm has six or more joints, and in six of the joints, the rotation axis of the first joint, the rotation axis of the fourth joint, and the rotation axis of the sixth joint from the root side. Is a vertical articulated robot arm in which the rotation axes of the fourth joint and the rotation axis of the sixth joint are offset.
The solution set generator performs numerical calculation on the angle of the sixth joint on the six joints, and calculates the angle of the remaining joints based on the angle obtained by the numerical calculation. The inverse kinematics calculation device according to claim 1, claim 3 or claim 4, wherein the solution of the inverse kinematics calculation is obtained. A step in which the input device receives input information including a start point represented by the joint space representation and an end point represented by the Cartesian space representation in a robot arm having a plurality of joints.
A step in which the solution set generator finds a plurality of solutions of the inverse kinematics operation for the end point included in the input information received by the input device.
The solution selector satisfies the movement time in the joint angle interpolation trajectory between the start point included in the input information received by the input device and the solution from the solutions obtained by the solution set generator. An inverse kinematics calculation method with steps to select a solution.

Images