Control device for multi-axis robot, robot system, and control program

Abstract

To provide a controller for multi-axis robot, a robot system, and a control program that can change a position attitude of the multi-axis robot while avoiding collision between the multi-axis robot and an obstacle.SOLUTION: A controller 20 for multi-axis robot comprises a position attitude correction part 22, and an attitude control part 23. The position attitude correction part 22 is configured to determine a corrected intermediate position attitude of a tip 38 of a multi-axis robot 30 in which a distance between the multi-axis robot 30 and an obstacle exceeds a threshold with respect to a position attitude, in which the distance is the threshold or less, of a plurality of intermediate position attitudes through which a position attitude of the tip 38 of the multi-axis robot 30 changes to a final position attitude. The attitude control part 23 is so configured to control the multi-axis robot 30 so that the position attitude of the tip 38 of the multi-axis robot 30 changes to the final position attitude through the corrected intermediate position attitude determined by the position attitude correction part 22.SELECTED DRAWING: Figure 1

Description

[Technical field]

[0001] The present disclosure relates to a control device for a multi-axis robot, a robot system, and a control program. [Background technology]

[0002] Conventionally, there is known a control device for a multi-axis robot that determines whether the position and posture of the multi-axis robot can be changed to a target position and posture. For example, in the control device for a multi-axis robot disclosed in Patent Document 1, a change in the distance between the multi-axis robot and an obstacle when the position and posture of the multi-axis robot changes is calculated in advance. Based on the calculation result, it is determined whether a collision between the multi-axis robot and the obstacle will occur. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2008-279524 A Summary of the Invention [Problem to be solved by the invention]

[0004] However, Patent Document 1 does not disclose a configuration for identifying a path for the multi-axis robot to avoid collision with an obstacle.

[0005] An object of the present disclosure is to provide a control device, a robot system, and a control program for a multi-axis robot that can change the position and posture of the multi-axis robot while avoiding collision between the multi-axis robot and an obstacle. [Means for solving the problem]

[0006] A control device for a multi-axis robot according to at least one embodiment of the present invention comprises: a position and posture correction unit for determining a corrected intermediate position and posture of the tip of the multi-axis robot in which a distance between the multi-axis robot and an obstacle exceeds a threshold value, for a position and posture in which the distance is equal to or smaller than a threshold value, among a plurality of intermediate positions and postures that the multi-axis robot passes through before the tip of the multi-axis robot changes to a final position and posture; a position and orientation control unit for controlling the multi-axis robot so that the position and orientation of the tip of the multi-axis robot changes from the corrected intermediate position and orientation obtained by the position and orientation correction unit to the final position and orientation; Equipped with.

[0007] A robot system for a multi-axis robot according to at least one embodiment of the present invention comprises: A controller for the multi-axis robot; The multi-axis robot; Equipped with.

[0008] A control program for a multi-axis robot according to at least one embodiment of the present invention comprises: On the computer, a position and posture correcting step for determining a corrected intermediate position and posture of the tip of the multi-axis robot in which a distance between the multi-axis robot and an obstacle is equal to or smaller than a threshold value among a plurality of intermediate positions and postures that the tip of the multi-axis robot passes through before the tip of the multi-axis robot changes to a final position and posture in which the distance exceeds the threshold value; a position and orientation control step for controlling the multi-axis robot so that the position and orientation of the tip of the multi-axis robot changes from the corrected intermediate position and orientation obtained by the position and orientation correcting step to the final position and orientation; Execute the command. Effect of the Invention

[0009] According to the present disclosure, it is possible to provide a control device, a robot system, and a control program for a multi-axis robot that can change the position and posture of the multi-axis robot while avoiding collision between the multi-axis robot and an obstacle. [Brief description of the drawings]

[0010] [Figure 1] FIG. 1 is an explanatory diagram illustrating a robot system 1 according to an embodiment. [Diagram 2] 11 is a diagram showing an intermediate point C corrected by the position and orientation correction unit 22 according to an embodiment. FIG. [Diagram 3] 11 is a conceptual diagram showing a process in which the position and orientation correction unit 22 according to an embodiment searches for a corrected intermediate position and orientation. FIG. [Figure 4] FIG. 2 is a diagram illustrating a configuration of a position and orientation correction unit 22 according to an embodiment. [Diagram 5] FIG. 4 is a conceptual diagram showing the principle of calculating an update amount Δq of each axis state amount based on equation (1) according to one embodiment. [Figure 6] 1A and 1B are diagrams illustrating a model 50 set in the multi-axis robots 30A and 30B according to an embodiment. [Figure 7] FIG. 13 is a diagram illustrating a method for calculating a distance parameter according to an embodiment. [Figure 8] 1 is a block diagram showing an electrical configuration of a robot system 1 according to an embodiment. [Figure 9] 11 is a flowchart illustrating a robot control process according to an embodiment. [Figure 10] 11 is a flowchart illustrating a position and orientation correction process according to an embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0011] Hereinafter, some embodiments of the present invention will be described with reference to the accompanying drawings. However, the dimensions, materials, shapes, relative arrangements, etc. of components described as the embodiments or shown in the drawings are merely illustrative examples and are not intended to limit the scope of the present invention. For example, expressions expressing relative or absolute configuration, such as "in a certain direction," "along a certain direction," "parallel," "orthogonal," "center," "concentric," or "coaxial," not only strictly express such a configuration, but also express a state in which there is a relative displacement with a tolerance or an angle or distance to the extent that the same function is obtained. For example, expressions indicating that things are in an equal state, such as "identical," "equal," and "homogeneous," not only indicate a state of strict equality, but also indicate a state in which there is a tolerance or a difference to the extent that the same function is obtained. For example, expressions describing shapes such as a rectangular shape or a cylindrical shape do not only refer to rectangular shapes, cylindrical shapes, etc. in the strict geometric sense, but also refer to shapes that include uneven portions, chamfered portions, etc., to the extent that the same effect is obtained. On the other hand, the expressions "comprise," "include," "have," "includes," or "have" of one element are not exclusive expressions excluding the presence of other elements.

[0012] FIG. 1 is a diagram showing a robot system 1 for a multi-axis robot (hereinafter, simply referred to as "robot system 1") according to one embodiment.

[0013] A robot system 1 according to one embodiment includes a multi-axis robot 30 and a control device 20 for the multi-axis robot (hereinafter also simply referred to as the "control device 20"). In one embodiment, the control device 20 controls the position and orientation of a tip 38 of the multi-axis robot 30, so that a tool 39 attached to the multi-axis robot 30 moves along a target movement path 33. As a result, the multi-axis robot 30 performs an operation on a workpiece 5 using the tool 39. Examples of the operation performed on the workpiece 5 include cutting, gripping, lifting, pressing, drilling, photographing, blowing air, suction, and water spraying. In one embodiment, the workpiece 5 is transported by a transport device 7 such as a belt conveyor. The transported workpiece 5 is photographed by a camera device 8, and the movement path 33 is set based on the image captured by the camera device 8. In another embodiment, the workpiece 5 may be transported by a person. Also, the camera device 8 does not need to be provided. The workpiece 5 may be any object on which an operation is performed. For example, the workpiece 5 may be food, daily necessities, parts of industrial products, or chemical materials. In one embodiment, the workpiece 5 is food such as agricultural products, livestock products, or marine products. The workpiece 5 illustrated in FIG. 1 is fresh meat (similar to FIG. 2). In one embodiment, the control device 20 includes a robot controller and a personal computer as components. The control device 20 does not have to include a personal computer.

[0014] The multi-axis robot 30 in one embodiment includes a plurality of joint axes. Each joint axis is a rotary joint or a linear joint having one degree of freedom. In one embodiment, each axis state quantity (joint variable), which is the state quantity of each joint axis, is represented by the angle or axial position of the joint axis. The position and orientation of the tip 38 of the multi-axis robot 30 changes as each of these axis state quantities changes. The tip 38 may be the tip of a link 35 of the multi-axis robot 30 that is located closest to the tip, or may be the tip of a tool 39 attached to the link 35. The above-mentioned state quantities of each axis are represented by an axis state quantity vector q that includes the state quantities of each joint axis as elements. For example, if the multi-axis robot 30 has six joint axes, each axis state quantity vector q is represented by six axis state quantities q 1 ~q 6 The number of joint axes of the multi-axis robot 30 may be, for example, five, or seven or more, instead of six.

[0015] The position and orientation of the tip 38 is represented by a position and orientation vector p that includes position and orientation parameters as elements. In one embodiment, there are six position and orientation parameters. More specifically, the six position and orientation parameters are three position parameters (X, Y, and Z) that indicate the position of the tip 38 and three orientation parameters (R X , R Y , and R Z In one embodiment, these parameters are classified into two types: 0 As shown in

[0016] The relationship between the position and posture vector p of the multi-axis robot 30 and each axis state quantity vector q is defined by the following equation (2). p = fr(q) Equation (2) fr is a function that converts the joint angle into a position and orientation vector. Specifically, when the number of joint axes (i.e., the number of joints) of the multi-axis robot 30 is n, this is expressed as the following equation (2A) using the function fp. p=fp( 0 T n )...Equation (2A) 0 T n is a homogeneous transformation matrix, and the reference coordinate system Σ 0 and the coordinate system Σ of the n-th joint at the tip end of the multi-axis robot 30. n This defines the relationship between the reference coordinate system Σ 0 can be arbitrarily set depending on the application of the multi-axis robot 30. 0 is set on the transportation path of the transportation device 7. In another embodiment, the reference coordinate system Σ 0 may be set on, for example, the base 9. 0 The coordinates in are sometimes called world coordinates. 0 T n is defined by equation (3). 0 T n = 0 T 1 1 T 2 ... n―1 T n ...Equation (3) In formula (3), 0 T 1 is the reference coordinate system Σ 0 In the coordinate system of the most proximal joint, Σ 1 is the homogeneous transformation matrix for transforming 1 T 2 is the coordinate system Σ 1 In the coordinate system of the second joint from the base end, Σ 2 In this way, each homogeneous transformation matrix shown on the right side of equation (3) defines the relationship between the coordinate systems of two adjacent joints. In one embodiment, each homogeneous transformation matrix shown on the right side of equation (3) is a matrix with 4 rows and 4 columns. Each homogeneous transformation matrix includes, as elements, a rotation matrix with 3 rows and 3 columns, and a translation vector with 1 row and 3 columns that indicates the positional relationship between the two coordinate systems of interest. For example, n―1 T n is expressed by the following equation (4).

number

[0017] In one embodiment, the multi-axis robot 30 includes multi-axis robots 30A and 30B. A tool 39A attached to the multi-axis robot 30A is a knife. A tool 39B attached to the multi-axis robot 30B is a clamper. In one embodiment, the multi-axis robots 30A and 30B are operated under the control of the control device 20, so that the workpiece 5 is gripped and cut. In one embodiment, the multi-axis robots 30A and 30B operate asynchronously with each other. In another embodiment, the multi-axis robots 30A and 30B may operate synchronously with each other.

[0018] In one embodiment, obstacles are included in each of the movable areas of the multi-axis robots 30A and 30B. Therefore, the position and orientation of the tip 38 of each of the multi-axis robots 30A and 30B needs to change while avoiding collision between the multi-axis robots 30A and 30B and the obstacles. In one embodiment, the working envelopes of the multi-axis robots 30A, 30B overlap each other, and therefore the multi-axis robot 30B is an obstacle for the multi-axis robot 30A. In other embodiments, a component of the robot system 1, such as the transport device 7, may be an obstacle for the multi-axis robot 30, or another object may be an obstacle.

[0019] The distance between the multi-axis robot 30 according to the embodiment and the obstacle is represented by a distance vector including at least one distance parameter. The distance vector is, for example, a column vector. For example, when the number of distance parameters is two, the elements of the distance vector are d 1 and d 2 In Figure 1, one distance parameter is denoted by d m As illustrated by: If at least one distance parameter included in the distance vector is equal to or smaller than a threshold, there is a high possibility that the multi-axis robot 30 will collide with an obstacle. In one embodiment, the threshold is a value equal to or greater than 0 that is set in association with each distance parameter. The threshold may be a positive value that reflects a safety factor.

[0020] In one embodiment, the above distance vector can be determined based on each axis state vector q. Specifically, it is as follows. If the state vector q of each axis of the multi-axis robot 30 is known, the position parameters (X, Y, and Z) of the tip 38 can be obtained based on the formula (1). 0 T n-1 If is applied to equations (2) and (2A), the position parameters of the (n-1)th link 35 can also be found. In a similar manner, the position parameters of any part of the multi-axis robot 30 can be found based on each axis state quantity vector q and equation (1). This allows the distance vector between the multi-axis robot 30 and an obstacle to be found. In an embodiment in which the multi-axis robot 30B is an obstacle to the multi-axis robot 30A, the distance parameters of the multi-axis robots 30A and 30B are calculated on the assumption that the multi-axis robots 30A and 30B are stationary at each time step. The method for calculating the distance parameter will be described in further detail below.

[0021] In one embodiment, a control device 20 for controlling a multi-axis robot 30 includes a path setting unit 21, a position and orientation correction unit 22, and a position and orientation control unit . In one embodiment, the path setting unit 21 sets the movement path 33 of the tip 38 when a task is performed on the workpiece 5. For example, the path setting unit 21 sets the movement path 33 of the tip 38 according to the shape or position of the workpiece 5. The movement path 33 in one embodiment includes a start point D, a plurality of intermediate points C, and an end point G. The position and orientation of the tip 38 (i.e., the position and orientation parameters of the tip 38) are set at each of these points included in the movement path 33. Hereinafter, the position and orientation of the tip 38 at each intermediate point C will also be referred to as an intermediate position and orientation, and the position and orientation of the tip 38 at the end point G will also be referred to as a final position and orientation. In one embodiment, a distance parameter determined based on the position and orientation parameters is equal to or smaller than a threshold value at at least one of the multiple intermediate points C. Such intermediate points C are treated as correction targets.

[0022] The position and orientation correction unit 22 in one embodiment is configured to obtain a corrected intermediate position and orientation of the tip 38 at which the distance between the multi-axis robot 30 and an obstacle exceeds a threshold value, for an intermediate position and orientation of the intermediate point C to be corrected. In other words, the position and orientation correction unit 22 is configured to obtain a corrected intermediate position and orientation to be corrected, among multiple intermediate positions and orientations that the tip 38 passes through before changing to a final position and orientation. The position and attitude correction unit 22 in one embodiment is configured to obtain the corrected intermediate position and attitude by searching for each axis state quantity of the multi-axis robot 30. More specifically, the position and attitude correction unit 22 is configured to search for candidate values ​​of each axis state quantity such that the values ​​of the position and attitude parameters and the distance parameters fall within the allowable ranges. In one embodiment, by making the values ​​of the position and orientation parameters fall within the allowable range, it is possible to prevent the corrected intermediate position and orientation obtained by the position and orientation correction unit 22 from deviating too far from the ideal position and orientation. The ideal position and orientation is, for example, the initial position and orientation of the intermediate point C to be corrected. In one embodiment, the value of the distance parameter falls within an acceptable range, preventing the distance parameter from deviating too far from an ideal value (e.g., a threshold value), thereby reducing waste in the operation of the multi-axis robot 30 as the tip 38 passes through the corrected intermediate positions and orientations.

[0023] In one embodiment, the position and attitude control unit 23 is configured to control the multi-axis robot 30 so that the position and attitude of the tip 38 of the multi-axis robot 30 changes from the corrected intermediate position and attitude obtained by the position and attitude correction unit 22 to the final position and attitude. In one embodiment, the position and orientation control unit 23 controls the multi-axis robot 30 so that the tip 38 moves along the movement path 33 .

[0024] Fig. 2 is a diagram showing intermediate points C corrected by the position and orientation correction unit 22 according to one embodiment. Fig. 2 illustrates a view seen along the transport direction of the transport device 7. In one embodiment, numbers are assigned to each of the multiple intermediate points C in order from the upstream side of the movement path 33. In one embodiment, the first intermediate point C 1and the i+1th midpoint C i+1 In the above, the distance parameter is equal to or smaller than the threshold value, and the position and orientation correcting unit 22 searches for a corrected intermediate position and orientation. In one embodiment, the midpoint C 1 and midpoint C i+1 In either case, the position and orientation correction unit 22 selects the orientation parameter (R X , R Y , or R Z ) is corrected. Note that the position and orientation correction unit 22 in other embodiments may search for a corrected intermediate position and orientation in which at least one of the position parameters (X, Y, or Z) among the position and orientation parameters is corrected, or may search for a corrected intermediate position and orientation in which all six position and orientation parameters are corrected. When the position and orientation control unit 23 controls the multi-axis robot 30, the position and orientation of the tip 38 is set to the intermediate point C 1 and midpoint C i+1 The position and orientation are changed to the corrected intermediate position and orientation found by the position and orientation correction unit 22.

[0025] The position and orientation parameters to be corrected by the position and orientation correction unit 22 are not necessarily the same throughout the search for the corrected intermediate position and orientation. In other words, the position and orientation parameters to be corrected can be dynamically switched depending on the search status or search conditions for the intermediate position and orientation. For example, the number of position and orientation parameters to be corrected may increase as the number of searches increases (or as the threshold value that the distance parameter should satisfy becomes smaller), or the type of the position and orientation parameters to be corrected may change but the number itself may be constant. Alternatively, a combination of these two may be executed. As a more specific example, until the number of searches for the corrected intermediate position and orientation reaches a specified number, R X Only R is the target of correction. XIf an appropriate corrected intermediate position and posture in which only R is corrected (i.e., an intermediate position and posture in which the distance parameter exceeds the threshold) is identified, the search ends. At this time, a corrected intermediate position and posture that is very close to the pre-correction position and posture can be identified. On the other hand, if an appropriate corrected intermediate position and posture (a corrected intermediate position and posture that allows collision avoidance, etc.) is not found even after the number of searches reaches a specified number, R X In addition to (or R X instead of ), e.g. R Y and R Z can be added as a correction target. This makes it possible to avoid problems in which the appropriate intermediate position and orientation cannot be specified. As described above, it is possible to search for a corrected intermediate position and orientation that is as close as possible to the target position and orientation, and it is also possible to increase the probability of finding a corrected intermediate position and orientation that allows collision avoidance, etc.

[0026] 3 is a conceptual diagram showing a process in which the position and orientation correction unit 22 according to an embodiment searches for a corrected intermediate position and orientation. In the example of FIG. 3, the first intermediate point C 1 3 shows a process for determining the corrected intermediate position and orientation. In addition, in FIG. 3, a part of the movement path 33 is omitted. midpoint C 1 If the distance parameter is less than the threshold, the intermediate point C 1 The intermediate position and orientation are determined to be the object of correction. In this case, in one embodiment, the position and orientation correction unit 22 calculates the initial midpoint C 1 The midpoint C 1、1 Update the intermediate point C 1 Midpoint C as a candidate for 1、2 Request. midpoint C 1、2 An example of a specific method for determining a candidate for the midpoint C is as follows. 1、1 The corresponding axis state quantity q 1、1 is calculated based on equation (5). Then, the midpoint C 1、1 The deviation of the position and orientation parameters and the target value at the midpoint C 1、1 Based on the distance parameter and the deviation from the target value, Δq 1、1The position and orientation correction unit 22 calculates the update amount (more details on how to calculate the update amount will be described later). As a result, each axis state amount q 1、2 (=q 1、1 +Δq 1、1 The position and orientation correction unit 22 calculates the state quantity q of each axis using the formula (2). 1、2 Using and, find the midpoint C 1、2 Calculate the position and orientation vector p (position and orientation parameters). midpoint C 1、2 If at least one of the position and orientation parameters or the distance parameters at the intermediate point C 1、2 Then, we further update it to find a new candidate midpoint C 1、3 The method of calculation is the same as above, and the latest candidate value of each axis state quantity, q 1、2 to Δq 1、2 By adding 1、3 The position and orientation parameters are calculated. The search for each axis state quantity is repeated until both the position and orientation parameters and the distance parameters fall within the allowable range. Then, the intermediate point C identified in the k-1th update is 1、k If the values ​​of the above parameters are within the allowable range at 1、k is the midpoint C after correction 1 That is, the midpoint C 1、k The intermediate position and orientation of the initial intermediate point C 1 are treated as corrected intermediate positions and orientations instead of the intermediate positions and orientations. During this repetition, the position and orientation parameters to be corrected (e.g., R X , R Y By dynamically switching or adding these depending on the conditions, the degree of freedom in correcting the trajectory can be increased. Although detailed illustration and description are omitted, the i+1th intermediate point C i+1 The same process is repeated even when the target of correction is the intermediate point C i+1、k If the position and orientation parameters and the distance parameters are within the allowable range, i+1、k is the midpoint C after correction i+1 is treated as.

[0027] FIG. 4 is a diagram showing the configuration of the position and orientation correction unit 22 according to an embodiment. The position and attitude correction unit 22 includes a first matrix acquisition unit 11 for acquiring a first matrix, a second matrix acquisition unit 12 for acquiring a second matrix, and an update unit 13 for updating candidate values ​​of each axis state quantity based on the first matrix and the second matrix.

[0028] In one embodiment, the first matrix acquired by the first matrix acquisition unit 11 indicates at least the sensitivity of one or more position and orientation parameters to each axis state quantity. The first matrix is, for example, a Jacobian matrix. For example, if the axis state vector q includes six axis state quantities, and the first matrix includes six position and orientation parameters (X, Y, Z, R X , R Y , and R Z ) to each axis state quantity, the first matrix (J 1s ) is expressed by equation (6).

number

[0029] In one embodiment, the second matrix acquired by the second matrix acquisition unit 12 indicates at least the sensitivity of the distance parameter to each axis state quantity. An example of the second matrix is ​​a Jacobian matrix. For example, the axis state vector q includes six axis state quantities, and the second matrix includes a plurality of distance parameters (d 1 , , d m ) to each axis state quantity, the second matrix (J 2s ) is expressed by equation (7).

number

[0030] The update unit 13 in one embodiment is configured to update the candidate values ​​of each axis state quantity based on the first deviation and the second deviation in addition to the above-mentioned first matrix and second matrix. The first deviation is a deviation of one or more position and orientation parameters corresponding to candidate values ​​of each axis-state amount from a target value. In one embodiment, the target value is a position and orientation parameter of the position and orientation to be corrected. The second deviation is a deviation of a distance parameter corresponding to the candidate value of each axis state amount from a target value. In one embodiment, the target value is a threshold value determined corresponding to the distance parameter. In one embodiment, the position and orientation parameters handled by the first deviation are the same as the position and orientation parameters handled by the first matrix. For example, if the first matrix indicates the sensitivity of six position and orientation parameters to each axis state quantity, the first deviation is a deviation with respect to these six parameters. Similarly, the distance parameters handled by the second deviation are the same as the distance parameters handled by the second matrix. For example, if the second matrix indicates the sensitivity of m distance parameters to each axis state quantity, the second deviation is a deviation with respect to the same distance parameter. In one embodiment, when the first deviation and the first matrix are known, the update amount of each axis state amount for bringing the position and orientation parameters for the candidate value before the update closer to the target value is obtained, for example, by applying a gradient method. Similarly, when the second deviation and the second matrix are known, the update amount of each axis state amount for bringing the distance parameter corresponding to the candidate value before the update closer to the target value is obtained. An update unit 13 in one embodiment calculates an update amount (Δq) for updating the latest candidate value of each axis state amount based on a first update amount for each axis state amount calculated from the first deviation and the first matrix, and a second update amount for each axis state amount calculated from the second deviation and the second matrix.

[0031] As a more specific example, the update unit 13 calculates the update amount (Δq) of each axis state amount based on the following formulas (8), (9), and (10). + means the inverse matrix of J. In other formulas, a matrix with a subscript "+" also means the pseudo-inverse matrix of the matrix without the subscript. Δq=Δq 1 +Δq 2 ...Equation (8) Δq 1 =J + Δx Equation (9) Δq2 =(J C (IJ + J)) + (Δx C -J C J + Δx) Equation (10) In formulas (8) to (10), Δq 1 is either the first update amount or the second update amount described above, and Δq 2 is the other of the first update amount or the second update amount. In one embodiment, Δq 1 is the first update amount, and Δq 2 is the second update amount. In this case, Δx is the first vector (for example, a column vector) containing the first deviation as an element, and J is the first matrix. C is the second vector (for example, a column vector) containing the second deviation as an element, and J C is the second matrix. That is, the combination of Δx and J is the combination of the first vector and the first matrix including the first deviation as an element, and Δx C and J. C is a combination of a second vector and a second matrix including the second deviation as an element. In another embodiment, Δq 1 is the second update amount, and Δq 2 is the first update amount. In this case, the above combination is reversed. That is, the combination of Δx and J is the combination of the second vector and the second matrix, and Δx C and J. C is a combination of the first vector and the first matrix. For example, if the first deviation is the deviation of six position and orientation parameters from the target value, and the second deviation is the deviation of m distance parameters, the first matrix (J or J C One of them is J in Eq. (6). 1s and the second matrix (J or J C The other one) is J in Eq. (7). 2s Matches. In the formulas (8) to (10), making Δx smaller meansC This means that Δq is obtained in which priority is given to making Δq smaller than making Δq smaller. Prior to explaining in detail the principle by which such Δq can be obtained, the formula (1) derived from the formulas (8) to (10) is shown below. Δq=J + Δx+(J C (IJ + J)) + (Δx C -J C J + Δx) Equation (1)

[0032] 5 is a conceptual diagram showing the principle of determining the update amount Δq of each axis state amount based on the formula (1) according to one embodiment. C is conceptually shown using two-dimensional coordinates. In the following, Δq 1 is the first update amount, and Δq 2 Assume that is the second update amount. In other words, the combination of Δx and J is the combination of the first vector and the first matrix, and Δx C and J. C Assume that the combination of is a combination of the second vector and the second matrix. Note that even if the combination is reversed, the principle of finding the update amount Δq remains the same. Furthermore, it is assumed that equations (11) and (12) hold true. Also, f(q) is upwardly convex and g(q) is downwardly convex. x=f(q)...Equation (11) x C =g(q)...Equation (12) Also, the target value and the tolerance of the vector x are x tgt , x tgt -τ, and vector x C The target value and tolerance of x C_tgt , x C_tgt +τ C Furthermore, the candidate values ​​of the current state quantities of each axis are q cur Let us assume that.

[0033] The update amount of each axis state quantity to keep the vector x within the allowable range is Δq 1The principle by which this is determined is as follows. q cur x, which is the vector x corresponding to cur corresponds to the height position of point P1. As mentioned above, the first matrix (J) indicates the sensitivity of the position and orientation parameters to each axis state quantity. In other words, the slope of the tangent line to the graph at point P1 corresponds to the first matrix. Therefore, the first deviation Δx (=x cur -x tgt ) is defined by the following equation (13). Δx=JΔq 1 ...Equation (13) By transforming equation (13), the above equation (9) is derived. This results in Δq 1 is an update amount for bringing the position and orientation parameters handled by the vector x closer to the target values, and it is understood that this update amount reflects the first deviation.

[0034] Vector x C Δq is the update amount of each axis state quantity to keep it within the allowable range. 2 The principle by which this is determined is as follows. q cur Vector x in C x C_cur corresponds to the height position of point Q1. The tangent to the graph at point Q1 is the second matrix (J C ), and x C_cur -x C_tgt is the second deviation Δx C Shows. Δq 2 When determining, the second deviation, Δx C Instead, Δx C -J C J + The deviation defined by Δx (deviation using the second deviation) is used. Also, instead of the tangent line of the graph at point Q1 (the slope of the tangent line corresponds to the second matrix), J C (IJ + J) is treated as a straight line having a slope defined by That is, the vector x C The target value x C_tgt Δq to get closer to 2is defined by the formula (14). By transforming the formula (14), the above formula (10) can be obtained. Δx C -J C J + Δx=J C (IJ + J)Δq 2 ...Equation (14) Suppose the second deviation is Δx C and the tangent to point Q1, Δq 2d is required. Δx C =J C Δq 2d ...Equation (15) Δq calculated based on equation (10) 2 and Δq calculated based on equation (15) 2d are both vectors x C is the update amount for bringing the parameters (including the distance parameter) handled by the second deviation closer to the target value, and is the update amount that reflects the second deviation.

[0035] The state of each axis is updated using the above Δq 1 and Δq 2 is used, and q cur +Δq 1 +Δq 2 is the new q cur In FIG. 5, which is a conceptual example, Δq 2 is Δq 2d is smaller than the first update result, q cur +Δq 1 +Δq 2 can be understood as the state quantity of each axis for reducing the first deviation with priority over the second deviation. In other words, by using formula (1), the vector x is C Prioritize Δq (=Δq 1 +Δq 2 ) is obtained. In the example of Figure 5, point P2 indicates the updated vector x, and point P3 indicates the updated vector x C Since all of the points Q2 indicating the above fall within the allowable range, the updating of each axis state quantity vector q is completed. Suppose vector x and vector x C If at least one of the above deviations from the allowable range, the update of each axis state vector q is further executed. For example, even if the first deviation becomes 0 as a result of the update of each axis state vector, the vector x C If falls outside the tolerance range, Δq is further updated.

[0036] In another embodiment, the first matrix may indicate sensitivity to some of the axis state quantities of the six position and orientation parameters, in which case the first vector handles differences of some of the six position and orientation parameters with respect to their target values. For example, the position and orientation parameters handled by the first matrix are X, Y, Z, and R. Z If so, the first matrix (J 1v ) is expressed by equation (16). In this case, the first vector including the first deviation as an element is the position and orientation parameters X, Y, Z, R Z The deviation from the target value is shown.

number

[0037] Fig. 6 is a diagram for explaining a model 50 set in the multi-axis robots 30A and 30B according to an embodiment. Fig. 7 is a diagram showing a method for calculating a distance parameter according to an embodiment. Figs. 6 and 7 illustrate the multi-axis robots 30A and 30B including six rotational joints. In Fig. 6, the model 50 is shown by a two-dot chain line, whereas in Fig. 7, the model 50 is shown by a solid line.

[0038] In one embodiment, a model 50 encompassing at least a portion of the multi-axis robot 30 is virtually set up, and the distance between the model 50 and an obstacle is treated as a distance parameter indicating the distance between the multi-axis robot 30 and the obstacle. The model 50 may include all parts of the multi-axis robot 30, or may include only a part of the multi-axis robot 30. For example, if a part of the multi-axis robot 30 that may collide with an obstacle is specified in advance, the model 50 may be set to include only that part. This reduces the number of distance parameters, improving the efficiency of calculations for calculating the distance.

[0039] The model 50 in one embodiment includes a model 50A set on the link 35 on the most distal end side of the multi-axis robot 30A, and a model 50B set on the second link 35 from the distal end side. As an example, the model 50A includes all parts of the tool 39A. In one embodiment, a model 50 may be set for an obstacle of the multi-axis robot 30A. The model 50 does not need to have the same shape as the model 50 set for the multi-axis robot 30, and may have a shape corresponding to the obstacle, for example. In one embodiment, a model 50 is also set for the multi-axis robot 30B, which is an obstacle to the multi-axis robot 30A. As a more specific example, the model 50 includes a model 50C set for the link 35 on the most distal end side of the multi-axis robot 30B. In one embodiment, each of the models 50A to 50C is formed by a cylindrical portion 55 along the axial direction of the link 35, and two hemispherical portions 56 connected to both ends of the cylindrical portion 55.

[0040] In one embodiment, the distance vector of the multi-axis robot 30A, 30B includes a distance parameter d that indicates the distance between the model 50A and the model 50C. 1 and the distance parameter d indicating the distance between model 50B and model 50C. 2 This includes: The distance parameter d in one embodiment 1 is obtained as follows. The coordinates of the reference points of model 50A (in the example of FIG. 7, points A1 and A2 located at both ends of center line 57A of cylindrical portion 55) are obtained based on candidate values ​​of each axis state quantity of multi-axis robot 30A. In addition, the coordinates of the reference points of model 50B (in the example of FIG. 7, points B1 and B2 located at both ends of center line 57C of cylindrical portion 55) are obtained assuming that multi-axis robot 30B is stopped. The shortest distance among the four distances between the reference points (two points) of model 50A and the reference points (two points) of model 50B is the distance parameter d 1 is treated as such. The specified distance parameter d 1 If the value of is equal to or smaller than the diameter of the hemisphere 56, the models 50A and 50C will interfere with each other at the updated candidate values ​​of each axis state quantity. In other words, when the actual state quantity of each axis of the multi-axis robot 30A changes to the candidate value, a collision between the multi-axis robot 30A and the multi-axis robot 30B may occur. Therefore, the candidate values ​​of each axis state quantity are further updated. Distance parameter d 2 The method for determining the value of the specified distance parameter d2 If is equal to or smaller than the diameter of the hemisphere 56, the candidate values ​​of the axial state quantities are updated. The shapes of the models 50A to 50C are not limited to those shown in Fig. 6 and Fig. 7. The models 50A to 50C may be, for example, spherical in shape to include at least a part of the multi-axis robots 30A and 30B. In this case, the center position of the spherical model corresponds to the reference point of the model. Furthermore, the shapes of the models 50A to 50C may be appropriately changed depending on the work performed by the multi-axis robots 30A and 30B.

[0041] 8 is a block diagram showing an electrical configuration of the robot system 1 according to an embodiment. The control device 20 of the robot system 1 includes a processor 91 that controls the robot system 1. The processor 91 is configured to read out a control program 95 stored in a ROM 92, load it into a RAM 93, and execute instructions included in the loaded control program 95. The processor 91 is a CPU, a GPU, an MPU, a DSP, or any other type of arithmetic device, or a combination of these. The processor 91 may be realized by an integrated circuit such as a PLD, an ASIC, an FPGA, or an MCU. The ROM 92 is an example of a storage device. The memory 94, which is a component of the control device 20, temporarily or non-temporarily stores various data to be processed in association with the execution of the control program 95. The memory 94 is, for example, a flash memory. As an example, when the power supply of the robot system 1 is turned off, the information stored in the RAM 93 is non-temporarily stored in the memory 94. The control device 20 may include at least one of the ROM 92, the RAM 93, and the memory 94. The processor 91 is electrically connected to the multi-axis robot 30 , the transport device 7 , and the imaging device 8 . In one embodiment, the processor 91 is configured to control a plurality of driving units of the multi-axis robot 30. Each of the plurality of driving units is a motor, an air cylinder, or the like. In one embodiment, each of the driving units is a motor. In this case, the processor 91 acquires an output result of an encoder included in the motor, thereby acquiring each axis state quantity vector q. The processor 91 is configured to control a drive unit of the conveying device 7. The processor 91 is configured to transmit a photographing instruction signal to the photographing device 8 and to acquire a photographed image generated by the photographing device 8.

[0042] Fig. 9 is a flowchart showing a robot control process according to an embodiment. Fig. 10 is a flowchart showing a position and orientation correction process according to an embodiment. Hereinafter, the robot control process will be described with reference to the correspondence between the processes executed by the processor 91 and the components of the control device 20 described above. In the following explanation, Δq 1 is the first update amount, and Δq 2 In other words, a combination of Δx and J is a combination of a first vector and a first matrix, and Δx C and J. C is a combination of a second vector and a second matrix. Furthermore, in the following description, step may be abbreviated as "S".

[0043] 9, the processor 91 sets a movement route 33 (S11). The processor 91 executing S11 functions as the route setting unit 21. For example, the workpiece 5 transported by the transport device 7 is photographed by the photographing device 8. The processor 91 acquires the photographed image generated by the photographing device 8, and sets the movement path 33 according to the shape or position of the workpiece 5 shown in the photographed image. In one embodiment, the positions and orientations of the tip 38 at each of the start point D, the multiple intermediate points C, and the end point G included in the movement path 33 are determined in conjunction with the setting of the movement path 33. Therefore, it is determined at this point in time which of the multiple intermediate points C the distance parameter is equal to or smaller than the threshold value.

[0044] The processor 91 selects the i-th intermediate point C among the multiple intermediate points C. iis set as a determination target for whether it satisfies an allowable condition (described later) (S13). Note that the number i is stored in, for example, the RAM 93, and is set to 1 at the start of the robot control process. The processor 91 determines the i-th intermediate point C i Based on the position and orientation parameters, i In step S15, the state quantity of each axis is obtained in accordance with the following equation (5):

[0045] The processor 91 determines the intermediate point C i It is determined whether the position and orientation parameters and the distance parameters satisfy an allowable condition (S17). The allowable condition is that both the position and orientation parameters and the distance parameters fall within an allowable range. As an example, the tolerance of the position and orientation parameters is i The range is set based on the initial intermediate position and orientation of the object. The allowable range of the distance parameter is set based on a threshold value, for example. For example, in step S17, the first time the robot control process is started, the intermediate point C 1 It is judged whether the tolerance condition is satisfied. In this case, the position and orientation parameters to be judged are the same as the reference position and orientation parameters, so the midpoint C 1 The position and orientation parameters of are within the allowable range. 1 If the value of the distance parameter of the midpoint C deviates from the tolerance range, 1 is determined not to satisfy the tolerance condition, and the processor 91 selects the intermediate point C 1 is set as the correction target.

[0046] midpoint C i When it is set that the permissible condition is not satisfied (S17: NO), the processor 91 i It is determined whether the update count k has reached an upper limit (S19). The update count k is stored in, for example, the RAM 93, and is set to 1 when the robot control process starts. If the update count k has reached the upper limit (S19: YES), the processor 91 ends this control process.

[0047] On the other hand, if the update count k is less than the upper limit (S19: NO), the processor 91 i The processor 91 that executes S21 functions as the position and orientation correcting unit 22. In the position and orientation correction process of one embodiment, the midpoint C i A candidate value of each axis state quantity to replace each axis state quantity is searched. For example, at the midpoint C i If the distance parameter corresponding to the intermediate point C is equal to or smaller than the threshold and is set as the correction target (S17: NO), 1 In the position correction process, a candidate value for each axis state quantity to replace the original axis state quantity is searched for. As will be described in detail later, each time the position and orientation correction process is executed, the update count k is incremented.

[0048] After executing the position and orientation correction process (S21), the processor 91 returns to S17. In this case, the position and orientation parameters and the distance parameters are calculated based on the axial state quantities updated in the position and orientation correction process, and it is determined whether the permissible conditions are satisfied (S17). If the permissible conditions are not satisfied (S17: NO), the processor 91 repeats S17 to S21.

[0049] When it is determined that the position and orientation corresponding to the candidate value of each axis state quantity satisfies the permissible condition (S17: YES), the processor 91 converts the position and orientation into C i The corrected intermediate position and posture are determined as follows (S23). Then, the processor 91 determines the intermediate point C i It is determined whether the intermediate point C is the most downstream intermediate point on the moving path 33 (S25). i If there is another midpoint C downstream of (S25: NO), the processor 91 increments the number i and initializes the update count k to 1 (S27). After that, the processor 91 returns the process to S13. When the processor 91 repeats steps S11 to S27, the intermediate point C icoincides with the midpoint C on the most downstream side (S25: YES). After that, the processor 91 controls the multi-axis robot 30 so that the tip 38 moves along the movement path 33 (S29). In this case, the processor 91 controls the multi-axis robot 30 so that the position and posture of the tip 38 changes from a corrected intermediate position and posture to a final position and posture. The processor 91 that executes S29 functions as the position and posture control unit 23.

[0050] In addition, in the process (S13) of setting the intermediate point C as the judgment target in another embodiment, the intermediate point C may be set as the judgment target in order from the downstream side of the movement path 33, or the intermediate point C may be set as the judgment target randomly.

[0051] The position and orientation correction process will be described with reference to FIG. 10. At the start of this process, the midpoint C i does not satisfy the admissibility condition.

[0052] The processor 91 calculates the candidate value q of each axis state quantity. i、k Set (S51). For example, midpoint C 1 does not satisfy the allowable condition (S17: NO) and the update count k is 1, the processor 91 selects q as a candidate value of each axis state quantity. 1、1 (S51). For example, q 1、1 is the midpoint C 1 are the axis state quantities at the initial intermediate position and posture. In another embodiment, q 1、1 is the midpoint C 1 For example, q 1、1 is the midpoint C 1 The position and orientation of the starting point D may be the position and orientation of a nearby point of the intermediate point C. 1 In the calculation process, it is possible to regard the intermediate point as being one intermediate point upstream of the intermediate point A.

[0053] The processor 91 calculates the first deviation Δx _i、k and the second deviation Δx C_i、k (S53). For example, the candidate value of each axis state quantity, q i、k The position and orientation parameters corresponding to the distance parameter are calculated based on the position and orientation parameters. The processor 91 calculates a first deviation Δx _i、k and the second deviation Δx C_i、k Request.

[0054] The processor 91 calculates the latest candidate value q of each axis state quantity. i、k The first matrix in (J _i、k ) (S55), and q i、k The second matrix in (J C_i、k The processor 91 that executes S55 functions as the first matrix acquisition unit 11, and the processor 91 that executes S57 functions as the second matrix acquisition unit 12.

[0055] The processor 91 updates the state quantities of each axis by Δq i、k is obtained using equation (1) (S59). Equation (1) includes the q i、k , the first deviation Δx obtained by Eq. _i、k and the second deviation Δx C_i、k , J obtained in S55 _i、k , and Jc obtained in S57 _i、k is assigned. The processor 91 calculates the Δq i、k The latest q i、k By adding to q i、k The processor 91 that executes S61 functions as the update unit 13. The processor 91 increments the number of updates k (S63), ends the position and orientation correction process, and returns to the robot control process.

[0056] In the position and orientation correction process according to another embodiment, Δq 1 is the second update amount, and Δq 2 may be the first update amount. In other words, a combination of Δx and J is a combination of a second vector and a second matrix, and ΔxC and J. C may be a combination of a first vector and a first matrix. In this case, in S55, the first deviation is Δxc _i、k and the second deviation is Δx _i、k Furthermore, the first matrix obtained in S55 is treated as Jc _i、k The second matrix obtained in S57 is J _i、k is treated as. In addition, the first matrix (J _i、k ) may be a first matrix based on each axis state quantity deviated from the latest candidate value instead of the first matrix based on the latest candidate value of each axis state quantity. Similarly, the second matrix (J C_i、k ) may be a second matrix based on each axis state quantity deviated from the latest candidate value instead of the second matrix at the latest candidate value of each axis state quantity.

[0057] Hereinafter, a control device 20 for a multi-axis robot, a robot system 1 for a multi-axis robot, and a control program 95 for a multi-axis robot according to some embodiments will be described.

[0058] (1) A control device 20 for a multi-axis robot according to at least one embodiment of the present invention includes: a position and posture correction unit for determining a corrected intermediate position and posture of the tip of the multi-axis robot in which a distance between the multi-axis robot and an obstacle is equal to or smaller than a threshold value among a plurality of intermediate positions and postures through which the tip of the multi-axis robot passes before changing to a final position and posture, the corrected intermediate position and posture of the tip of the multi-axis robot in which the distance exceeds the threshold value; a position and orientation control unit 23 for controlling the multi-axis robot 30 so that the position and orientation of the tip 38 of the multi-axis robot 30 changes from the corrected intermediate position and orientation obtained by the position and orientation correction unit 22 to the final position and orientation; Equipped with.

[0059] According to the configuration (1) above, if a collision between the multi-axis robot 30 and an obstacle may occur during a process in which the position and posture of the tip 38 of the multi-axis robot 30 is changed, there exists an intermediate position and posture in which the distance between the multi-axis robot 30 and the obstacle is equal to or less than a threshold. In this case, after the position and posture correction unit 22 determines the corrected intermediate position and posture, the position and posture of the tip 38 of the multi-axis robot 30 is changed by the position and posture correction unit 22 to the target position and posture via the corrected intermediate position and posture. Thus, a control device 20 for a multi-axis robot is realized that can change the position and posture of the multi-axis robot 30 while avoiding a collision between the multi-axis robot 30 and an obstacle.

[0060] (2) In some embodiments, in the configuration of (1) above, The position and attitude correction unit 22 is configured to search for each axis state quantity of the multi-axis robot 30 such that the values ​​of a position and attitude parameter indicating the position and attitude of the tip 38 of the multi-axis robot 30 and a distance parameter indicating the distance fall within an allowable range.

[0061] According to the above configuration (2), the position and posture corresponding to each axis state amount searched for by the position and posture correction unit 22 is treated as the corrected intermediate position and posture. Therefore, it is possible to suppress the value of the position and posture parameters and the value of the distance parameter of the corrected intermediate position and posture from deviating too far from the ideal value. For example, the more the position and orientation parameters deviate from the ideal values, the more likely it is that the path of the tip 38, which moves under the control of the position and orientation control unit 23, will deviate from the movement path 33 set by the path setting unit 21, resulting in inappropriate operation on the workpiece 5. In this regard, since the values ​​of the position and orientation parameters are prevented from deviating too much from the ideal values, the robot system 1 can perform appropriate operation on the workpiece 5. Furthermore, the greater the distance to the obstacle, the more wasteful the operation of the multi-axis robot 30 to avoid a collision with the obstacle. In this regard, by preventing the distance from deviating too far from the ideal value, the position and orientation of the tip 38 of the multi-axis robot 30 can be changed with less waste.

[0062] (3) In some embodiments, in the configuration of (2) above, The position and orientation correction unit 22 a first matrix acquisition unit 11 for acquiring a first matrix indicating at least a sensitivity of one or more of the position and orientation parameters to each of the axis state quantities; a second matrix acquisition unit 12 for acquiring a second matrix indicating at least the sensitivity of the distance parameter to each of the axis state quantities; an update unit 13 for updating the candidate value of each of the axis state quantities by using a first deviation from a target value of the one or more position and attitude parameters corresponding to the candidate value of each of the axis state quantities, a second deviation from a target value of the distance parameter corresponding to the candidate value of each of the axis state quantities, and the first matrix and the second matrix; Includes.

[0063] According to the above configuration (3), since each axis state amount is updated based on the first deviation, the first matrix, the second deviation, and the second matrix, the first deviation and the second deviation are reflected in each updated axis state amount, which makes it easier to determine a corrected intermediate position and orientation in which the values ​​of the position and orientation parameters and the distance parameters are within the respective allowable ranges.

[0064] (4) In some embodiments, in the configuration of (3) above, the update unit 13 updates the candidate value by adding Δq represented by equation (1) to the latest candidate value of each of the axis state quantities; In the formula (1), A combination of Δx and J is one of a combination of a first vector including the first deviation as an element and the first matrix, or a combination of a second vector including the second deviation as an element and the second matrix, Δx C and J. C is the other of the combination of the first vector and the first matrix or the combination of the second vector and the second matrix. Δq=J + Δx+(J C (IJ + J)) + (Δx C -J CJ + Δx) Equation (1)

[0065] According to the above configuration (4), it is given priority that either the value of the position and orientation parameter included in the first vector or the value of the distance parameter included in the second vector falls within the allowable range rather than the other. This allows the corrected intermediate position and orientation to be found while determining the relative merits of one parameter over the other. Therefore, in the corrected intermediate position and orientation, the value of the position and orientation parameter with the higher priority can be set close to the ideal value.

[0066] (5) In some embodiments, in the configuration of (4), A combination of Δx and J is a combination of the first vector and the first matrix including the first deviation as an element, Δx C and J. C is a combination of the second vector and the second matrix including, as elements, a deviation between a target value and one or more of the position and orientation parameters other than the one or more position and orientation parameters among the plurality of position and orientation parameters, and the second deviation.

[0067] According to the above configuration (5), the corrected intermediate position and orientation is obtained while giving priority to the value of the position and orientation parameter included in the first vector being within an allowable range. This makes it possible to set at least one of the position and orientation parameters in the corrected intermediate position and orientation close to an ideal value.

[0068] (6) In some embodiments, in any one of the configurations (1) to (5) above, The position and attitude correction unit 22 virtually sets up a model 50 that encompasses at least a portion of the multi-axis robot 30, treats the distance between the model and the obstacle as the distance between the multi-axis robot 30 and the obstacle, and calculates the amount of operation of each axis.

[0069] According to the configuration (6) above, the position and attitude correction unit 22 can reduce the load of calculating the distance between the multi-axis robot 30 and the obstacle by regarding the distance between the virtually set model 50 and the obstacle as the distance between the multi-axis robot 30 and the obstacle.

[0070] (7) In some embodiments, in the configuration of (6), The model 50 (50A to 50C) is A cylindrical portion 55 along the axial direction of the link 35 of the multi-axis robot 30; Two hemispherical portions 56 connected to both ends of the cylindrical portion 55; Includes.

[0071] According to the above configuration (7), the model 50 is set in a range close to the link 35 of the multi-axis robot 30. This prevents the distance required to avoid a collision with an obstacle from becoming excessively large.

[0072] (8) A robot system 1 for a multi-axis robot according to at least one embodiment of the present invention includes: A control device 20 for a multi-axis robot according to any one of (1) to (7) above; The multi-axis robot 30 Equipped with.

[0073] According to the configuration of (8) above, for the same reason as in (1) above, a robot system 1 for a multi-axis robot is realized that can change the position and posture of the multi-axis robot 30 while avoiding collision between the multi-axis robot 30 and an obstacle.

[0074] (9) A control program 95 for a multi-axis robot according to at least one embodiment of the present invention includes: On the computer, a position and posture correction step (S21) for determining a corrected intermediate position and posture of the tip 38 of the multi-axis robot 30 in which a distance between the multi-axis robot 30 and an obstacle is equal to or smaller than a threshold value among a plurality of intermediate positions and postures that the tip 38 of the multi-axis robot 30 passes through before the tip 38 of the multi-axis robot 30 changes to a final position and posture, the corrected intermediate position and posture of the tip 38 of the multi-axis robot 30 in which the distance exceeds the threshold value; a position and orientation control step (S29) for controlling the multi-axis robot 30 so that the position and orientation of the tip 38 of the multi-axis robot 30 changes from the corrected intermediate position and orientation obtained in the position and orientation correction step to the final position and orientation; Execute the command.

[0075] According to the configuration of (9) above, for the same reason as in (1) above, a control program 95 for a multi-axis robot is realized that can change the position and posture of the multi-axis robot 30 while avoiding collision between the multi-axis robot 30 and an obstacle.

[0076] In the above description, in FIG. 9, the point indicating the position and orientation that satisfies the tolerance condition in S17 and the intermediate point C determined in S23 i In another embodiment, the intermediate points C that are determined to satisfy the permissible condition in S17 and are included in the corrected trajectory of the multi-axis robot 30 are matched. i (For example, 5 intermediate points) are assigned weighting coefficients, and an arbitrary number of intermediate points C i It is also possible to fine-tune the position and orientation in (i.e., the corrected trajectory can be fine-tuned). It is preferable that all of the fine-tuned waypoints satisfy the tolerance conditions. However, it is also possible to adopt a fine-tuned waypoint that does not satisfy the tolerance conditions set in S17 but satisfies a collision avoidance condition that is looser than the tolerance conditions. This will take us to a certain intermediate point C i is any of several intermediate points C downstream of that intermediate point. i This allows the multi-axis robot 30 to move smoothly along a smooth trajectory. [Explanation of symbols]

[0077] 1: Robot system 11: First matrix acquisition part 12:Second matrix acquisition part 13: Update section 20: Control device 22:Position and orientation correction section 23: Position and orientation control unit 30: Multi-axis robot 35: Link 38: Tip 50: Model 55: Cylindrical section 56: Hemisphere d 1 ~d 3 :Distance parameters q 1 ~q 6 : Axis state quantity

Claims

1. 1. An apparatus for controlling a multi-axis robot to which a knife as a tool is attached, comprising: a path setting unit that sets a movement path of the tip of the multi-axis robot when the multi-axis robot performs a cutting operation of the workpiece using the knife, according to a shape or a position of the workpiece captured in a photographed image of the workpiece being transported by a transport device; a position and posture correction unit that calculates a corrected intermediate position and posture of the tip of the multi-axis robot in which a distance between the multi-axis robot and an obstacle is equal to or smaller than a threshold value among a plurality of intermediate positions and postures that the tip of the multi-axis robot passes through before the tip of the multi-axis robot changes to a final position and posture as the tip of the multi-axis robot moves on the movement path of the multi-axis robot, and in which the distance exceeds the threshold value; a position and orientation control unit for controlling the multi-axis robot so that, during a cutting operation of the workpiece by the multi-axis robot using the knife, the position and orientation of the tip of the multi-axis robot changes from the corrected intermediate position and orientation obtained by the position and orientation correction unit to the final position and orientation; A control device for a multi-axis robot comprising:

2. 2. The control device for a multi-axis robot according to claim 1, wherein the position and attitude correction unit is configured to search for each axis state quantity of the multi-axis robot such that values ​​of a position and attitude parameter indicating the position and attitude of the tip of the multi-axis robot and a distance parameter indicating the distance fall within an allowable range.

3. a position and posture correction unit for determining a corrected intermediate position and posture of the tip of the multi-axis robot in which a distance between the multi-axis robot and an obstacle exceeds a threshold value, for a position and posture in which the distance is equal to or smaller than a threshold value, among a plurality of intermediate positions and postures that the multi-axis robot passes through before the tip of the multi-axis robot changes to a final position and posture; a position and orientation control unit for controlling the multi-axis robot so that the position and orientation of the tip of the multi-axis robot changes from the corrected intermediate position and orientation obtained by the position and orientation correction unit to the final position and orientation; Equipped with the position and attitude correction unit is configured to search for each axis state quantity of the multi-axis robot such that values ​​of a position and attitude parameter indicating the position and attitude of the tip of the multi-axis robot and a distance parameter indicating the distance fall within a tolerance range; The position and orientation correction unit a first matrix acquisition unit for acquiring a first matrix indicating at least a sensitivity of one or more of the position and orientation parameters to each of the axis state quantities; a second matrix acquisition unit for acquiring a second matrix indicating at least a sensitivity of the distance parameter to each of the axis state quantities; an updating unit for updating the candidate value of each of the axis state quantities by using a first deviation of the one or more of the position and orientation parameters from a target value corresponding to a candidate value of each of the axis state quantities, a second deviation of the distance parameter from a target value corresponding to the candidate value of each of the axis state quantities, and the first matrix and the second matrix; A controller for a multi-axis robot comprising:

4. the update unit updates the candidate value by adding Δq represented by Equation (1) to the latest candidate value of each of the axis state quantities; In the formula (1), A combination of Δx and J is one of a combination of a first vector and the first matrix including the first deviation as an element, or a combination of a second vector and the second matrix including the second deviation as an element, 4. The control device for a multi-axis robot according to claim 3, wherein the combination of ΔxC and JC is the other of the combination of the first vector and the first matrix or the combination of the second vector and the second matrix. Δq=J+Δx+(JC(I-J+J))+(ΔxC-JCJ+Δx)...Formula (1)

5. A combination of Δx and J is a combination of the first vector and the first matrix including the first deviation as an element, 5. The control device for a multi-axis robot according to claim 4, wherein a combination of ΔxC and JC is a combination of the second vector and the second matrix including, as elements, a deviation between a target value and one or more of the position and posture parameters other than the one or more position and posture parameters among the plurality of position and posture parameters, and the second deviation.

6. 6. The control device for a multi-axis robot according to claim 1, wherein the position and attitude correction unit virtually sets a model including at least a portion of the multi-axis robot, and treats a distance between the model and the obstacle as the distance between the multi-axis robot and the obstacle.

7. The model is A cylindrical portion along an axial direction of the link of the multi-axis robot; Two hemispherical portions connected to both ends of the cylindrical portion; 7. The controller for a multi-axis robot according to claim 6, comprising:

8. An apparatus for controlling a multi-axis robot to which a knife as a tool is attached, comprising: a path setting unit that sets a movement path of the tip of the multi-axis robot when the multi-axis robot performs a cutting operation of a workpiece using the knife; a position and posture correction unit that calculates a corrected intermediate position and posture of the tip of the multi-axis robot in which a distance between the multi-axis robot and an obstacle is equal to or smaller than a threshold value among a plurality of intermediate positions and postures that the tip of the multi-axis robot passes through before the tip of the multi-axis robot changes to a final position and posture as the tip of the multi-axis robot moves on the movement path of the multi-axis robot, and in which the distance exceeds the threshold value; a position and orientation control unit for controlling the multi-axis robot so that, during a cutting operation of the workpiece by the multi-axis robot using the knife, the position and orientation of the tip of the multi-axis robot changes from the corrected intermediate position and orientation obtained by the position and orientation correction unit to the final position and orientation; Equipped with The obstacle is another multi-axis robot equipped with a clamper for gripping the workpiece as a tool. A control device for a multi-axis robot.

9. A control device for a multi-axis robot according to any one of claims 1 to 8; The multi-axis robot; A robot system for a multi-axis robot comprising:

10. A program for controlling a multi-axis robot to which a knife as a tool is attached, On the computer, a path setting step of setting a movement path of the tip of the multi-axis robot when the multi-axis robot performs a cutting operation of the workpiece using the knife, according to a shape or a position of the workpiece captured in a photographed image of the workpiece being transported by a transport device; a position and orientation correcting step for determining a corrected intermediate position and orientation of the tip of the multi-axis robot in which a distance between the multi-axis robot and an obstacle is equal to or smaller than a threshold value among a plurality of intermediate positions and orientations that the tip of the multi-axis robot passes through before the tip of the multi-axis robot changes to a final position and orientation as the tip of the multi-axis robot moves on the movement path, the corrected intermediate position and orientation of the tip of the multi-axis robot in which the distance exceeds the threshold value; a position and orientation control step for controlling the multi-axis robot so that the position and orientation of the tip of the multi-axis robot changes from the corrected intermediate position and orientation obtained in the position and orientation correction step to the final position and orientation while the multi-axis robot is performing a cutting operation of the workpiece using the knife; A control program for a multi-axis robot that executes the following:

Images