Robot control device and method for controlling robot

Abstract

A robot control device (1) comprises: a feed-forward model computation unit (13) that accepts a command position as input and computes a state variable of a feed-forward model on the basis of a preset feed-forward model; a dynamics approximation model derivation unit (12) that accepts a representative pose of an articulated robot as input and derives a dynamics approximation model, which is a model approximating a dynamics model of the articulated robot in the representative pose of the articulated robot; a first feed-forward transformation unit (15) that computes motor position feed-forward, which is a motor position of the dynamics model obtained by performing a coordinate transformation from the state variable of the feed-forward model to a motor position of the dynamics approximation model; and a servo control unit (16) that carries out control of a motor using the motor position feed-forward outputted by the first feed-forward transformation unit (15) as a target position.

Description

Robot control device and robot control method

[0001] The present disclosure relates to a robot control device and a robot control method that control the position of a motor mounted on each joint of an articulated robot based on a command position.

[0002] Articulated robots, such as industrial vertical articulated robots and horizontal articulated robots, are used in various processes in factories, such as assembly, transportation, and inspection. However, articulated robots have a structurally low mechanical rigidity, and when operated at high speeds, vibrations occur in the mechanical system, resulting in reduced productivity and work accuracy. To address this issue, motion control technologies have been developed to suppress vibrations in articulated robots. For example, Patent Document 1 discloses a technology for suppressing vibrations in articulated robots by calculating, based on a dynamics model, the axial force torque when the articulated robot's joint angles are operated according to command values, and calculating a motor angle command value based on parameters including the stiffness of the joint axes, the joint angle command value, and the axial force torque.

[0003] JP 2013-248683 A

[0004] However, while the robot control device described in Patent Document 1 is capable of suppressing vibrations caused by low rigidity of the drive shaft of a multi-joint robot, it has the problem of being unable to suppress vibrations caused by low rigidity of parts of the multi-joint robot other than the joints, or vibrations caused by deformation in directions other than the drive direction of the joints.

[0005] For example, horizontal articulated robots are generally equipped with ball screw splines for controlling the height of the end effector, but when the end effector is lowered, vibrations of the end effector occur due to flexing of the ball screw splines, making it difficult for the robot control device described in Patent Document 1 to suppress vibrations caused by the multiple vibration modes of the articulated robot.

[0006] The present disclosure has been made in consideration of the above, and aims to provide a robot control device that can simultaneously suppress vibrations caused by multiple vibration modes of a multi-joint robot, not just vibration modes caused by low rigidity of the drive shaft of the multi-joint robot.

[0007] In order to solve the above-mentioned problems and achieve the object, a robot control device according to the present disclosure is a robot control device that controls the position of a motor mounted in each joint of a multi-joint robot based on a command position, and includes a feedforward model calculation unit, a dynamics approximation model derivation unit, a first feedforward transformation unit, and a servo control unit. The feedforward model calculation unit receives a command position as input and calculates state variables of the feedforward model based on a preset feedforward model. The dynamics approximation model derivation unit receives a representative posture of the multi-joint robot as input and derives a dynamics approximation model that is an approximation model of the dynamics model of the multi-joint robot at the representative posture of the multi-joint robot. The first feedforward transformation unit calculates a motor position feedforward that is the motor position of the dynamics model obtained by performing coordinate transformation from the state variables of the feedforward model to the motor position of the dynamics approximation model. The servo control unit controls the motor using the motor position feedforward output by the first feedforward transformation unit as a target position.

[0008] The robot control device according to the present disclosure has the advantage of being able to simultaneously suppress vibrations caused by multiple vibration modes of the articulated robot, not just vibration modes caused by low rigidity of the drive shaft of the articulated robot.

[0009] FIG. 1 is a block diagram showing a configuration example of a robot system including a robot control device according to a first embodiment; FIG. 2 is a perspective view showing a schematic configuration of a robot system including a robot control device according to the first embodiment; FIG. 3 is a diagram showing a configuration example of a feedforward model calculation unit shown in FIG. 1; FIG. 4 is a diagram showing a configuration example of a sixth-order feedforward model in the first embodiment; FIG. 5 is a flowchart for explaining a method for deriving a coordinate transformation matrix from state variables of a feedforward model to state variables of a dynamic approximation model in the first embodiment; FIG. 6 is a block diagram showing a configuration example of a first feedforward transformation unit shown in FIG. 1;

[0010] A robot control device and a robot control method according to embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. Note that although the following embodiments describe control of a multi-joint robot, the joints may be either linear joints or rotary joints.

[0011] First Embodiment Fig. 1 is a block diagram showing an example of the configuration of a robot system 50 including a robot control device 1 according to the first embodiment. Fig. 2 is a perspective view showing a schematic configuration of a robot system 50 including a robot control device 1 according to the first embodiment. The robot control device 1 according to the first embodiment is a control device that controls the position of a servo motor 17 mounted on each joint of an articulated robot 40 based on a command position. Fig. 1 shows a J1-axis servo motor 17 mounted on each drive axis of the articulated robot 40 as the servo motor 17. １ ~ J4 axis servo motor 17 ４ The robot control device 1 controls the J1 axis servo motor 17 １ ~ J4 axis servo motor 17４ Controls the position of

[0012] 2 shows a configuration example in which the robot system 50 is a horizontal articulated robot as an example of an articulated robot. The robot system 50 includes a robot control device 1, a manipulator 2, a ball screw spline 5, and an end effector 6. The manipulator 2 is equipped with the ball screw spline 5 and the end effector 6. In FIG. 2, a J1-axis servo motor 17 １ J1 axis 3, which is the axis of the J2 axis servo motor 17 ２ The J2 axis 4, which is the axis of the J1 axis servo motor 17 (not shown in FIG. 2), is shown by a broken line. １ and J2 axis servo motor 17 ２ is a servo motor for driving the manipulator 2, and is a J3-axis servo motor 17 (not shown in FIG. 2). ３ and J4 axis servo motor 17 ４ is a servo motor for driving the ball screw spline 5 and the end effector 6. The robot control device 1 controls the J1-axis servo motor 17 １ ~ J4 axis servo motor 17 ４ The end effector 6 is controlled by driving the

[0013] J1 axis servo motor 17 １ and J2 axis servo motor 17 ２Each of the motors drives the joints of the articulated robot 40 via a reducer (not shown). The reducers used in horizontal and vertical articulated robots generally have low rigidity, causing spring deformation between the input shaft where each motor is located and the output shaft to which the driving force is transmitted. This spring deformation causes vibration in the end effector 6. Furthermore, when the ball screw spline 5 is lowered, the ball screw spline 5 flexes. This flex also causes vibration in the end effector 6. The ball screw spline 5 flexes in two degrees of freedom, in the X-axis and Y-axis directions, within the XY plane perpendicular to the direction in which the ball screw spline 5 extends. In this paper, the input shaft sides of the J1 axis 3 and J2 axis 4 of the articulated robot 40 are referred to as the "motor shaft," and the output shaft sides are referred to as the "drive shaft." In this paper, the directions in which the flex of the ball screw spline 5 occurs, i.e., the X-axis and Y-axis, are referred to as the "non-drive shaft."

[0014] Returning to the explanation of Figure 1, the robot control device 1 includes a command generation unit 11, a dynamics approximation model derivation unit 12, a feedforward model calculation unit 13, a coordinate transformation derivation unit 14, a first feedforward transformation unit 15, and a servo control unit 16.

[0015] The dynamics approximation model derivation unit 12 receives a representative posture of the articulated robot 40, which is set in advance by a method described below, as input, and derives a dynamics approximation model that is an approximation model of the dynamics model of the articulated robot 40 in the representative posture. The dynamics model is a dynamic characteristic model of the articulated robot 40, which is the object to be controlled, and the dynamics approximation model is a dynamic characteristic approximation model that is an approximation model of the dynamic characteristic model. More specifically, the dynamics approximation model derivation unit 12 derives the dynamics approximation model of the articulated robot 40 by first-order linearization of the dynamic model of the articulated robot 40 around the representative posture, based on a dynamic model of the articulated robot 40 created in advance and a representative posture of the articulated robot 40 that is set in advance. It is generally known that the equation of motion of the articulated robot 40 can be expressed as in the following mathematical formula (1).

[0016]

[0017] Here, the variable q in Equation (1) is a variable that represents a quantity called a generalized coordinate, and includes information on the positions of the driving axis and the non-driving axis defined above. ・ " is the first time derivative of the variable q, and "q ・・ " is the second time differential of the variable q. The first and second time differentials of other variables are expressed in the same way. In addition, in the formula (1), "M(q)" represents the inertia matrix, and "C(q,q ・ )" represents the torque due to centrifugal and Coriolis forces, and "τ" represents the drive shaft torque.

[0018] In addition, assuming that deformation of the reducer and a restoring force due to bending of the ball screw spline 5 occur in the articulated robot 40 shown in FIG. 2, the equation of motion can be expressed as the following equation (2).

[0019]

[0020] In equation (2), "θm" represents the position of the motor shaft, "θ" represents the position of the drive shaft, and "φ" represents the position of the non-drive shaft. θ " represents the spring constant in the drive shaft direction, and "K φ " represents the spring constant in the non-drive shaft direction. In the formula (2), "M(q)" shown in the formula (1) is "M １１ (θ, φ)”, “M １２ (θ, φ)”, “M ２１ (θ, φ)” and “M ２２ (θ, φ)” as an element. ・ )" is "C θ (θ, φ, θ ・ , φ ・ )" and "C φ (θ, φ, θ ・ , φ ・ )" as an element, and "τ" shown in the formula (1) is "-K θ (θ-θ ｍ )" and "-K φ It is expressed as a vector with φ” as an element.

[0021] The equation of motion expressed by Equation (1) is expressed as the representative attitude q ０ =(θ０ , φ ０ ) to obtain Equation (3). Furthermore, Equation (3) is converted into a state equation to obtain Equation (4).

[0022]

[0023]

[0024] In this paper, Equation (4) is referred to as a "dynamic approximation model" or a "state space model of a dynamic approximation model." In Equation (4), "x" is a state variable, and "u" is an input variable. Furthermore, "A" and "B" in Equation (4) represent, in matrix form, coefficients assigned to the state variable x and the input variable u in the process of transforming Equation (3) into Equation (4), and "A" is referred to as a state matrix, etc., and "B" is referred to as an input matrix, etc. In this paper, they are simply referred to as "matrix A" and "matrix B."

[0025] In the case of the articulated robot 40 shown in Figure 2, the system expressed by the formula (4) is a 12-dimensional system consisting of six position variables, which are the J1-axis motor position, the J2-axis motor position, the J1-axis position, the J2-axis position, and the flexure of the ball screw spline 5 in the X-axis and Y-axis directions, and six velocity variables corresponding to these six position variables. θ , K φ may be obtained using machine learning or reinforcement learning. In this case, for example, the spring constant K θ , K φ　 Adjust.

[0026] The feedforward model calculation unit 13 performs a filter calculation on the command position generated by the command generation unit 11 based on a feedforward model that is set in advance by a method described later, and sequentially calculates state variables of the feedforward model. Fig. 3 is a diagram showing an example of the configuration of the feedforward model calculation unit 13 shown in Fig. 1. The feedforward model calculation unit 13 has a J1-axis feedforward model calculation unit 131 and a J2-axis feedforward model calculation unit 132. The feedforward model calculation unit 13 performs a filter calculation on the command position of each axis generated by the command generation unit 11, and sequentially calculates internal state variables for each axis.

[0027] The feedforward model calculation unit 13 sets the order of the feedforward model so that the order of the dynamic approximation model matches the total order of all axes of the feedforward model. As an example, in the case of the horizontal articulated robot shown in FIG. 2, the order of the dynamic approximation model of Equation (4) is 12th order, so the orders of the J1-axis feedforward model calculation unit 131 and the J2-axis feedforward model calculation unit 132 are each set to 6th order. FIG. 4 is a diagram showing an example of the configuration of a 6th-order feedforward model in the first embodiment. Here, the block labeled "1 / s" in FIG. 4 represents an integrator, and the blocks labeled w1 to w6 represent blocks that amplify the input signal by a magnification set by the feedforward model gain.

[0028] The coordinate transformation derivation unit 14 derives a coordinate transformation matrix that performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model, based on the feedforward model set by the feedforward model calculation unit 13 and the dynamic approximation model derived by the dynamic approximation model derivation unit 12.

[0029] In the feedforward model shown in FIG. 4, the state variable z ６ By repeatedly integrating, the state variable z ６ Other state variables z with higher relative degrees than １ ~z ５Conversely, the state variable z with the largest relative degree １ By repeatedly differentiating the state variable z １ Other state variables z with lower relative degrees than ２ ~z ６ Therefore, the state variable z with the maximum relative degree can be obtained by using the state space model of the dynamic approximation model shown in Equation (4). １ After calculating the state variable z １ By repeatedly differentiating other state variables z ２ ~z ６ and constructing a coordinate transformation matrix based on the result, it is possible to derive a coordinate transformation matrix from the state variable z of the feedforward model to the state variable x of the dynamic approximation model. The derivation procedure will be specifically described below.

[0030] First, the coordinate transformation derivation unit 14 calculates the state variable z of the feedforward model from the state variable x of the dynamic approximation model. １ The output matrix C is a transformation matrix to １ is derived as shown in Equation (5). This process is carried out by deriving the state variable z １ is a state variable with the maximum relative degree of the state space model defined by Equation (4).

[0031]

[0032] Next, the coordinate transformation derivation unit 14 uses the formula (4) to derive the state variable z of the feedforward model as shown in the formula (6). １ By repeatedly differentiating with time, the state variable z of the feedforward model １ State variables other than z ２ ~z ６ The output matrix C is a transformation matrix to ２ ~C ６ is derived as shown in Equation (6). Here, the state variable z １ It should be noted that the term including the matrix B is zero due to the condition that is the state variable for which the relative degree of the state space model defined by Equation (4) is maximum.

[0033]

[0034] Next, the coordinate transformation derivation unit 14 calculates the output matrix C １ ~C ６ Using the above equation, a coordinate transformation matrix T from the state variable x of the dynamic approximation model to the state variable z of the feedforward model is calculated as shown in Equation (7).

[0035]

[0036] Next, by calculating the inverse matrix of the coordinate transformation matrix T obtained by Equation (7), the coordinate transformation matrix T from the state variable z of the feedforward model to the state variable x of the dynamic approximation model is obtained. －１ is derived.

[0037] The above series of processing steps are shown in Fig. 5. That is, Fig. 5 shows the coordinate transformation matrix T from the state variable z of the feedforward model to the state variable x of the dynamic approximation model in the first embodiment. －１ 10 is a flowchart illustrating a method for deriving

[0038] The coordinate transformation derivation unit 14 calculates the state variable x of the dynamic approximation model and the state variable z with the largest relative degree in the feedforward model. １ Based on this, the output matrix C １ is calculated (step ST1).

[0039] The coordinate transformation derivation unit 14 calculates the output matrix C obtained in step ST1. １ By successively multiplying by matrix A, other state variables z ２ ~z ６ Output matrix C ２ ~C ６ is calculated (step ST2).

[0040] The coordinate transformation derivation unit 14 calculates the output matrix C obtained in steps ST1 and ST2. １ ~C ６ By arranging these, a coordinate transformation matrix T from the state variable x of the dynamic approximation model to the state variable z of the feedforward model is obtained (step ST3).

[0041] The coordinate transformation derivation unit 14 calculates the inverse matrix of the coordinate transformation matrix T obtained in step ST3, thereby deriving the coordinate transformation matrix T from the state variable z of the feedforward model to the state variable x of the dynamic approximation model. －１ is calculated (step ST4).

[0042] Next, the function and configuration of the first feedforward transformation unit 15 will be described. The first feedforward transformation unit 15 converts the coordinate transformation matrix T －１ Based on this, the motor position feedforward is calculated by performing coordinate transformation from the state variable z of the feedforward model to the motor position of the dynamics approximation model.

[0043] 6 is a diagram showing an example of the configuration of the first feedforward transformation unit 15 shown in FIG. 1. The first feedforward transformation unit 15 has a J1-axis motor position feedforward transformation unit 151 and a J2-axis motor position feedforward transformation unit 152. In the first feedforward transformation unit 15, the J1-axis motor position feedforward transformation unit 151 and the J2-axis motor position feedforward transformation unit 152 calculate motor position feedforward by multiplying a coordinate transformation matrix from the state variable z of the feedforward model to the motor position of the corresponding axis. The coordinate transformation from the state variable z of the feedforward model to the motor position is performed using the coordinate transformation matrix T －１ The submatrix corresponding to the motor position is used.

[0044] The coordinate transformation matrix T in Equation (7) is uniquely determined once the representative posture of the articulated robot 40 is determined. Therefore, the first feedforward transformation unit 15 uses the coordinate transformation matrix T or the coordinate transformation matrix T derived by the coordinate transformation derivation unit 14 as the －１ Instead of the above, a coordinate transformation matrix obtained in advance may be stored, and the motor position feedforward may be calculated using this coordinate transformation matrix.

[0045] The servo control unit 16 controls the servo motor 17 using the motor position feedforward output by the first feedforward conversion unit 15 as a target position.

[0046] Next, the main points of the operation of the robot control device 1 according to the first embodiment will be described. As described above, the robot control device 1 according to the first embodiment derives a coordinate transformation matrix T between the state variable z of the feedforward model and the state variable x of the dynamic approximation model of the articulated robot 40, and performs servo control by using the motor position of the dynamic model obtained by coordinate transforming the state variable z of the feedforward model as the motor position feedforward. This processing can be realized by a feedforward controller having a virtual configuration shown in FIG. 7. FIG. 7 is a block diagram showing the configuration of a feedforward controller 101 that is virtually equivalent to the robot control device 1 according to the first embodiment. Components equivalent to those in FIG. 1 are assigned the same reference numerals.

[0047] The virtual feedforward controller 101 shown in Fig. 7 has a dynamic approximation model 102 and a state feedback control unit 103. The state feedback controller 103 generates a motor torque based on a command position, and the dynamic approximation model 102 generates a motor position based on the motor torque. When generating the motor torque, the state feedback controller 103 performs feedback control using state quantities of the controlled object output from the dynamic approximation model 102. The state quantities of the controlled object output from the dynamic approximation model 102 include spring deformation of the reducer, the amount of flex of the ball screw spline 5, and the like. Therefore, the state feedback controller 103 can suppress vibrations caused by multiple vibration modes in the dynamic approximation model 102.

[0048] The virtual feedforward controller 101 simulates the motor position response in the operation of suppressing vibration of the dynamics approximation model 102, and performs servo control with the motor position response that suppresses vibration as a target position. Therefore, the virtual feedforward controller 101 can effectively suppress vibrations of multiple vibration modes that occur in the articulated robot 40.

[0049] Furthermore, the pole placement that determines the response characteristics of the virtual feedforward controller 101 is equivalent to the pole placement that determines the response characteristics of the feedforward model calculation unit 13. Therefore, by setting the feedforward model gains w1 to w6 shown in Fig. 4, it is possible to achieve feedforward control with desired response characteristics. The method of setting the feedforward model gains w1 to w6 has the advantage that the responsiveness of the feedforward control can be easily adjusted compared to the method of adjusting the feedback gain of the state feedback controller 103.

[0050] As described above, the virtual feedforward control shown in Fig. 7 is equivalent to the feedforward control performed by the robot control device 1 according to the first embodiment. Therefore, the robot control device 1 according to the first embodiment can achieve feedforward control that simultaneously suppresses vibrations caused by multiple vibration modes in the dynamic approximation model. Therefore, the robot control device 1 according to the first embodiment can simultaneously suppress vibrations caused by multiple expected vibration modes, including vibrations caused by bending of the ball screw spline 5, within an operating range in which the approximation accuracy of the dynamic approximation model does not decrease significantly.

[0051] As described above, the robot control device 1 according to embodiment 1 is useful as a robot control device that can simultaneously suppress vibrations caused by multiple vibration modes of the articulated robot, not just vibration modes caused by the low rigidity of the drive shaft of the articulated robot 40.

[0052] In the above description, the representative posture of the articulated robot 40 for deriving the dynamics approximation model is assumed to be given in advance, but this is not limiting. For example, if the dynamics approximation model is derived using the target position of positioning control as the representative posture, it becomes possible to realize feedforward control that effectively suppresses residual vibrations occurring in the manipulator 2 after positioning is complete. Furthermore, coordinate transformation matrices may be calculated for a plurality of preset representative postures, and these may be switched depending on the region in which the manipulator 2 operates.

[0053] Furthermore, in the first embodiment, the case where the articulated robot 40 to be controlled is a horizontal articulated robot has been described as an example, but the above-described means or techniques can be applied regardless of the number of drive axes and non-drive axes of the robot or the kinematics. Therefore, the robot control device 1 according to the first embodiment can also be applied to other industrial robots such as vertical articulated robots or parallel link robots, as well as machine tools, processing machines, etc.

[0054] In addition, in the first embodiment, for the sake of simplicity, the components related to the control of the J3 axis and the J4 axis that drive the ball screw spline 5 are not shown, but it goes without saying that it is also possible to configure the system to include control of these J3 axis and J4 axis.

[0055] Furthermore, in the first embodiment, the vibration due to bending of the ball screw spline 5 is treated as an example of vibration of a non-drive shaft, but it is also possible to extend the dynamic model to include vibrations other than bending of the ball screw spline 5. For example, the dynamic model may be extended to include vibrations in the Z-axis direction on the J1 axis 3 or J2 axis 4, which is the base of the arm of a horizontal articulated robot, i.e., vibrations in an axial direction other than the drive direction of the drive shaft.

[0056] Furthermore, in the first embodiment, the dynamic model is treated as a 12th-order model with six position variables and six velocity variables, but an extension may be made so that the motor torque or motor acceleration of the J1 axis 3 and the J2 axis 4 is included as a state variable by adding an integral element to the input side of the dynamic model. In this case, the order of the dynamic model and the dynamic approximation model becomes 14th, so that the order of the dynamic characteristic approximation model and the order of the feedforward model can be made to match by setting each of the J1-axis feedforward model calculation unit 131 and the J2-axis feedforward model calculation unit 132 to 7th order. With such a 7th-order configuration, feedforward control can be realized that provides smoother response of the acceleration and torque of the servo motor 17.

[0057] As described above, the robot control device according to the first embodiment controls the position of a motor mounted in each joint of a multi-joint robot based on a command position, and includes a feedforward model calculation unit, a dynamics approximation model derivation unit, a first feedforward transformation unit, and a servo control unit. The feedforward model calculation unit receives a command position as an input and calculates state variables of the feedforward model based on a preset feedforward model. The dynamics approximation model derivation unit receives a representative posture of the multi-joint robot as an input and derives a dynamics approximation model that is an approximation model of the dynamics model of the multi-joint robot at the representative posture of the multi-joint robot. The first feedforward transformation unit calculates a motor position feedforward, which is the motor position of the dynamics model obtained by performing coordinate transformation from the state variables of the feedforward model to the motor position of the dynamics approximation model. The servo control unit controls the motor using the motor position feedforward output by the first feedforward transformation unit as a target position. The robot control device configured as described above can calculate a motor position command equivalent to that obtained when stabilization feedback control is performed using all states of the controlled object, including states related to vibrations of the mechanical system. By controlling the actual motor position based on the motor position command, it is possible to simultaneously suppress vibrations caused by multiple vibration modes of the articulated robot, not just vibration modes caused by the low rigidity of the drive shaft of the articulated robot.

[0058] In the robot control device according to the first embodiment, the dynamic model may include vibration modes other than the deformation of the drive shaft, which makes it possible to suppress vibrations caused by vibration modes other than the deformation of the drive shaft.

[0059] Furthermore, in the robot control device according to the first embodiment, an extension may be made to include the motor acceleration or motor torque in the state variables by adding an integral element to the input side of the dynamic model. In this way, feedforward control can be realized that results in a smoother response of the motor acceleration or motor torque.

[0060] Furthermore, in the robot control device according to embodiment 1, the representative posture of the articulated robot can be set as the target posture of positioning control. In the robot control device according to embodiment 1, if vibration suppression control is performed while approximating the dynamic model with the target posture of positioning control, residual vibration after positioning is completed can be effectively suppressed.

[0061] In the robot control device according to the first embodiment, the dynamics approximation model derivation unit calculates a parameter related to vibration, for example, a spring constant K θ , K φ A dynamic approximation model may be derived by adjusting the parameters related to vibration using machine learning or reinforcement learning. If the parameters related to vibration are obtained using machine learning or reinforcement learning, the parameters related to vibration can be easily adjusted, and the vibration suppression control in the first embodiment can be performed appropriately.

[0062] Furthermore, the robot control method according to the first embodiment is a control method performed using a robot control device that controls the position of a motor mounted on each joint of a multi-joint robot based on a command position. This control method includes the following first to fifth steps. The first step is a step of using a command position as input and calculating state variables of a feedforward model based on a preset feedforward model. The second step is a step of using a representative posture of the multi-joint robot as input and deriving a dynamic approximation model that is an approximation model of the dynamic model of the multi-joint robot at the representative posture of the multi-joint robot. The third step is a step of deriving a coordinate transformation matrix that performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model based on the feedforward model and the dynamic approximation model. The fourth step is a step of calculating a motor position feedforward, which is the motor position of the dynamic model obtained by performing coordinate transformation from the state variables of the feedforward model to the motor position of the dynamic approximation model using the coordinate transformation matrix. The fifth step is a step of controlling the motor using the motor position feedforward as a target position. Of these first to fifth steps, by having the robot control device execute the first to fourth steps, it is possible to calculate a motor position command equivalent to a case in which stabilization feedback control is performed using all states of the controlled object, including states related to vibrations of the mechanical system. Then, by controlling the actual motor position in the fifth step based on the motor position command calculated in the first to fourth steps, it is possible to simultaneously suppress vibrations caused by multiple vibration modes of the articulated robot, not just vibration modes caused by low rigidity of the drive shaft of the articulated robot.

[0063] Second Embodiment. Figure 8 is a block diagram showing an example configuration of a robot system 50a including a robot control device 1a according to a second embodiment. Comparing the robot control device 1a included in the robot system 50a shown in Figure 8 with the robot control device 1 included in the robot system 50 shown in Figure 1, in Figure 8 the first feedforward conversion unit 15 shown in Figure 1 has been replaced with a first feedforward conversion unit 15a. Also, in Figure 8, a second feedforward conversion unit 18 and a third feedforward conversion unit 19 have been added between the first feedforward conversion unit 15a and the servo control unit 16. In Figure 8, components that are the same as or equivalent to those in Figure 1 are designated by the same reference numerals.

[0064] Next, a description will be given of the functions and operations of the robot control device 1a according to embodiment 2. Note that, for functions and operations common to the robot control device 1 according to embodiment 1, the description of those functions and operations will be omitted as appropriate.

[0065] The first feedforward transformation unit 15a converts the coordinate transformation matrix T derived by the coordinate transformation derivation unit 14 into －１ Based on this, coordinate transformation is performed from the state variable z of the feedforward model to the state variable x of the dynamic approximation model. Fig. 9 is a diagram showing an example of the configuration of the first feedforward transformation unit 15a shown in Fig. 8. The first feedforward transformation unit 15a has a drive shaft position feedforward transformation unit 153, a drive shaft velocity feedforward transformation unit 154, a drive shaft acceleration feedforward transformation unit 155, a non-drive shaft position feedforward transformation unit 156, and a non-drive shaft velocity feedforward transformation unit 157.

[0066] Here, the position, speed, and acceleration of the drive axes represent the position, speed, and acceleration of the output axes of the axes such as the J1 axis 3 and the J2 axis 4. The position and speed of the non-drive axes represent the position and speed of the bending of the ball screw spline 5. The first feedforward transformation unit 15a calculates the coordinate transformation matrix T －１ Based on this, coordinate transformation is performed from the state variable z of the feedforward model to the state variable x of the dynamic approximation model.

[0067] 10 is a diagram showing an example of the configuration of the second feedforward conversion unit 18 shown in FIG. 8. The second feedforward conversion unit 18 shown in FIG. 10 includes a drive shaft acceleration calculation unit 181, a non-drive shaft acceleration calculation unit 182, a drive shaft speed integration unit 183, a non-drive shaft speed integration unit 184, a drive shaft position integration unit 185, and a non-drive shaft position integration unit 186. The second feedforward conversion unit 18 receives the state variables of the dynamic approximation model output by the first feedforward conversion unit 15a, and calculates the state variables of the dynamic model by correcting errors between the dynamic model and the dynamic approximation model based on the dynamic model. The processing performed by each component of the second feedforward conversion unit 18 will be described below.

[0068] The drive shaft acceleration calculation unit 181 corrects the drive shaft acceleration of the dynamic approximation model and calculates the drive shaft acceleration of the dynamic model based on the state variables of the dynamic approximation model output by the first feedforward transformation unit 15a and the state variables of the dynamic model output by the drive shaft speed integration unit 183, the non-drive shaft speed integration unit 184, the drive shaft position integration unit 185, and the non-drive shaft position integration unit 186. An example of a calculation formula for correcting the drive shaft acceleration of the dynamic approximation model and calculating the drive shaft acceleration of the dynamic model is shown in Equation (8).

[0069]

[0070] Here, in formula (8), variables marked with a hat symbol "^" are state variables of the dynamic approximation model calculated by the first feedforward conversion unit 15a, and variables not marked with a hat symbol "^" represent state variables of the dynamic model calculated by the drive shaft speed integration unit 183, the non-drive shaft speed integration unit 184, the drive shaft position integration unit 185, and the non-drive shaft position integration unit 186, respectively. In formula (8), the drive shaft acceleration θ^ calculated by the first feedforward conversion unit 15a ・・ By adding feedback terms (terms 2 to 5) related to the error between the corresponding variables of the dynamic model and the dynamic approximation model to (term 1), the drive shaft acceleration θ of the dynamic model is calculated. ・・ is calculated.

[0071] The non-drive shaft acceleration calculation unit 182 calculates the non-drive shaft acceleration of the dynamic model based on the drive shaft acceleration of the dynamic model calculated by the drive shaft acceleration calculation unit 181, the drive shaft speed, non-drive shaft speed, drive shaft position, and non-drive shaft position of the dynamic model calculated by the drive shaft speed integration unit 183, non-drive shaft speed integration unit 184, drive shaft position integration unit 185, and non-drive shaft position integration unit 186, and the dynamic model expressed by Equation (2). The non-drive shaft acceleration can be obtained by the following Equation (9). Equation (9) can be obtained by modifying the equation on the non-drive shaft side of Equation (2).

[0072]

[0073] The drive shaft speed integrator 183, the non-drive shaft speed integrator 184, the drive shaft position integrator 185, and the non-drive shaft position integrator 186 are integrators, and calculate the drive shaft speed, the non-drive shaft speed, the drive shaft position, or the non-drive shaft position by integrating the drive shaft acceleration, the non-drive shaft acceleration, the drive shaft speed, or the non-drive shaft speed, which are input signals to each component.

[0074] FIG. 11 is a diagram showing an example of the configuration of the third feedforward conversion unit 19 shown in FIG. 8. The third feedforward conversion unit 19 shown in FIG. 11 includes a drive shaft torque calculation unit 191, a spring correction amount calculation unit 192, and a motor position feedforward calculation unit 193. The third feedforward conversion unit 19 calculates the drive shaft torque of the articulated robot 40 based on the dynamics model and the state variables of the dynamics model calculated by the second feedforward conversion unit 18. The third feedforward conversion unit 19 also calculates a motor position feedforward, which is the motor position of the dynamics model, based on the state variables of the dynamics model and the drive shaft torque of the articulated robot 40. The processing performed by each component of the third feedforward conversion unit 19 will be described below.

[0075] The drive shaft torque calculation unit 191 calculates the drive shaft torque based on the state variables of the dynamic model calculated by the second feedforward transformation unit 18. The drive shaft torque can be obtained by Equation (10), which can be obtained by modifying the equation on the drive shaft side of Equation (2).

[0076]

[0077] The spring correction amount calculation unit 192 calculates the amount of correction for spring deformation occurring in the joint of the articulated robot 40 based on the drive shaft torque calculation unit 191 and preset physical parameters related to deformation of the joint. An example of a formula for calculating the amount of correction for spring deformation is shown in formula (11). In formula (11), the spring constant K of the reducer mounted on the joint is multiplied by the drive shaft torque τ calculated by the drive shaft torque calculation unit 191. θ The correction amount for the spring deformation occurring in the joint is calculated by multiplying by the reciprocal of

[0078]

[0079] Furthermore, the spring correction amount calculation unit 192 can calculate the correction amount for spring deformation occurring in the joint, taking into account not only the spring deformation of the reducer but also the viscous friction occurring between the input and output shafts of the reducer. In this case, the relationship between the drive shaft torque τ and the correction amount for spring deformation can be expressed, for example, by the following equation (12).

[0080]

[0081] By rearranging this formula (12), the following formula (13) can be obtained.

[0082]

[0083] In the formula (13), “D θ " represents the viscous friction constant in the drive shaft direction. The spring correction amount calculation unit 192 can calculate the correction amount for the spring deformation occurring in the joint according to the differential equation expressed by equation (13). Note that the spring constant K θ and viscous friction constant D θmay be obtained using machine learning or reinforcement learning. In this case, for example, the spring constant K θ and viscous friction constant D θ Adjust.

[0084] The motor position feedforward calculation unit 193 calculates the motor position feedforward by adding the correction amount for the spring deformation occurring in the joint calculated by the spring correction amount calculation unit 192 to the drive axis position of the dynamic model calculated by the second feedforward conversion unit 18.

[0085] The servo control unit 16 controls the servo motor 17 using the motor position feedforward output by the third feedforward conversion unit 19 as the target position.

[0086] Next, the main points of the operation of the robot control device 1 according to the second embodiment will be described. The robot control device 1a according to the second embodiment includes, as its components, a second feedforward transformation unit 18 and a third feedforward transformation unit 19. Within these components, motor position feedforward is calculated based on a dynamic model of the articulated robot 40 that is not an approximation. Therefore, the state variables of the dynamic model calculated within each component of the second feedforward transformation unit 18 will match the actual state variables of the articulated robot 40, assuming that the parameters of the dynamic model do not contain errors.

[0087] Furthermore, in the robot control device 1a according to the second embodiment, the first feedforward transformation unit 15a performs coordinate transformation from the state variable z of the feedforward model to the state variable x of the dynamic approximation model. As a result, the trajectory of the state variable x of the dynamic approximation model output by the first feedforward transformation unit 15a becomes a trajectory without residual vibration that converges to a steady state in accordance with the response characteristics set in the feedforward model.

[0088] Therefore, in the robot control device 1a according to the second embodiment, the trajectory of the state variable x of the dynamics approximation model without residual vibration output by the first feedforward transformation unit 15a is converted into the state variable x' of the non-approximate dynamic model by the second feedforward transformation unit 18, and the motor position feedforward is calculated based on the drive shaft torque calculated from the state variable x' of the non-approximate dynamic model by the third feedforward transformation unit 19. This makes it possible to simultaneously suppress vibrations caused by multiple possible vibration modes, including vibrations of the non-drive shaft of the non-approximate dynamic model.

[0089] In the robot control device 1 according to the first embodiment, the servo motor 17 is controlled using a motor position feedforward calculated based on an approximation model of the dynamic model of the articulated robot 40 as the target position, which can result in residual vibrations due to approximation errors between the dynamic model and the dynamic approximation model. In contrast, the robot control device 1a according to the second embodiment controls the servo motor 17 using a motor position feedforward calculated based on a dynamic model that is not an approximation as the target position, which does not result in residual vibrations due to approximation errors between the dynamic model and the dynamic approximation model. Therefore, the robot control device 1a according to the second embodiment can further enhance the vibration suppression effect compared to the robot control device 1 according to the first embodiment.

[0090] As described above, the robot control device according to the second embodiment is a robot control device that controls the position of a motor mounted in each joint of a multi-joint robot based on a command position, and includes a feedforward model calculation unit, a dynamics approximation model derivation unit, a first feedforward transformation unit, a second feedforward transformation unit, a third feedforward transformation unit, and a servo control unit. The feedforward model calculation unit receives a command position as an input and calculates state variables of the feedforward model based on a preset feedforward model. The dynamics approximation model derivation unit receives a representative posture of the multi-joint robot as an input and derives a dynamics approximation model that is an approximation model of the dynamics model of the multi-joint robot at the representative posture of the multi-joint robot. The first feedforward transformation unit performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamics approximation model. The second feedforward transformation unit receives the state variables of the dynamics approximation model output by the first feedforward transformation unit as an input and calculates the state variables of the dynamics model by correcting errors between the dynamics model and the dynamics approximation model based on the dynamics model and the dynamics approximation model. The third feedforward transformation unit calculates the drive shaft torque of the articulated robot based on the dynamic model and the state variables of the dynamic model, and calculates a motor position feedforward, which is the motor position of the dynamic model, based on the state variables of the dynamic model and the drive shaft torque. The servo control unit controls the motor using the motor position feedforward output by the third feedforward transformation unit as a target position. The robot control device according to the second embodiment adds a component that corrects the error between the dynamic approximation model and the original dynamic model to the configuration of the robot control device according to the first embodiment. This makes it possible to further enhance the vibration suppression effect compared to the robot control device according to the first embodiment.

[0091] Furthermore, a robot control method according to the second embodiment is a control method performed using a robot control device that controls the position of a motor mounted on each joint of a multi-joint robot based on a command position. This control method includes the following first to seventh steps. The first step is a step of using a command position as an input and calculating state variables of a feedforward model based on a preset feedforward model. The second step is a step of using a representative posture of the multi-joint robot as an input and deriving a dynamic approximation model that is an approximation model of the dynamic model of the multi-joint robot at the representative posture of the multi-joint robot. The third step is a step of deriving a coordinate transformation matrix that performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model based on the feedforward model and the dynamic approximation model. The fourth step is a step of performing coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model using the coordinate transformation matrix. The fifth step is a step of using the state variables of the dynamic approximation model as an input and calculating the state variables of the dynamic model by performing error correction between the dynamic model and the dynamic approximation model based on the dynamic model and the dynamic approximation model. The sixth step is a step of calculating joint torques of the articulated robot based on the dynamic model calculated in the fifth step and the state variables of the dynamic model, and calculating a motor position feedforward, which is the motor position of the dynamic model, based on the state variables and the joint torque. The seventh step is a step of controlling the motor using the motor position feedforward as a target position. The robot control method according to the second embodiment adds to the robot control method according to the first embodiment a fifth step of correcting an error between the dynamic approximation model and the original dynamic model, and a sixth step of calculating a motor position feedforward using the dynamic model after error correction. This makes it possible to further enhance the vibration suppression effect compared to the robot control device according to the first embodiment.

[0092] Next, a hardware configuration for realizing the functions of the robot control device 1, 1a according to the first and second embodiments will be described with reference to the drawings in Fig. 12 and Fig. 13. Fig. 12 is a block diagram showing an example of a hardware configuration for realizing the functions of the robot control device 1, 1a according to the first and second embodiments. Fig. 13 is a block diagram showing another example of a hardware configuration for realizing the functions of the robot control device 1, 1a according to the first and second embodiments.

[0093] When realizing some or all of the functions of the robot control device 1, 1a according to embodiments 1 and 2, the configuration can include a processor 300 that performs calculations, a memory 302 that stores programs read by the processor 300, and an interface 304 that inputs and outputs signals, as shown in FIG. 12.

[0094] The processor 300 is an example of a computing unit. The processor 300 may be a computing unit called a microprocessor, a microcomputer, a central processing unit (CPU), or a digital signal processor (DSP). Examples of the memory 302 include non-volatile or volatile semiconductor memory such as random access memory (RAM), read-only memory (ROM), flash memory, erasable programmable read-only memory (EPROM), and electrically programmable read-only memory (EEPROM), as well as a magnetic disk, a flexible disk, an optical disk, a compact disk, a minidisk, and a digital versatile disk (DVD).

[0095] The memory 302 stores a program that executes the functions of the robot control device 1, 1a according to the first and second embodiments. The processor 300 exchanges necessary information via the interface 304, executes the program stored in the memory 302, and refers to the table stored in the memory 302, thereby performing the above-mentioned processing. The calculation results by the processor 300 can be stored in the memory 302.

[0096] Furthermore, when realizing part of the functions of the robot control devices 1 and 1a according to the first and second embodiments, a processing circuit 303 shown in Fig. 13 can be used. The processing circuit 303 may be a single circuit, a composite circuit, an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a combination thereof. Information input to and output from the processing circuit 303 can be exchanged via an interface 304.

[0097] In addition, some of the processing in the robot control device 1, 1a may be performed by the processing circuit 303, and the processing that is not performed by the processing circuit 303 may be performed by the processor 300 and the memory 302.

[0098] The configurations shown in the above embodiments are merely examples, and may be combined with other known technologies, or different embodiments may be combined with each other. It is also possible to omit or modify parts of the configurations as long as they do not deviate from the gist of the invention.

[0099] REFERENCE SIGNS LIST 1, 1a Robot control device, 2 Manipulator, 3 J1 axis, 4 J2 axis, 5 Ball screw spline, 6 End effector, 11 Command generation unit, 12 Dynamics approximation model derivation unit, 13 Feedforward model calculation unit, 14 Coordinate transformation derivation unit, 15, 15a First feedforward transformation unit, 16 Servo control unit, 17 Servo motor, 17 １ J1 axis servo motor, 17 ２ J2 axis servo motor, 17 ３ J3 axis servo motor, 17 ４J4-axis servo motor, 18 second feedforward conversion unit, 19 third feedforward conversion unit, 40 articulated robot, 50, 50a robot system, 101 feedforward controller, 102 dynamics approximation model, 103 state feedback controller, 131 J1-axis feedforward model calculation unit, 132 J2-axis feedforward model calculation unit, 151 J1-axis motor position feedforward conversion unit, 152 J2-axis motor position feedforward conversion unit, 153 drive axis position feedforward conversion unit, 154 drive axis velocity feedforward conversion unit, 155 drive axis acceleration feedforward conversion unit, 156 non-drive axis position feedforward conversion unit, 157 non-drive axis velocity feedforward conversion unit, 181 drive axis acceleration calculation unit, 182 non-drive axis acceleration calculation unit, 183 drive axis velocity integration unit, 184 non-drive axis velocity integration unit, 185 drive axis position integration unit, 186 Non-drive shaft position integration unit, 191 drive shaft torque calculation unit, 192 spring compensation amount calculation unit, 193 motor position feedforward calculation unit, 300 processor, 302 memory, 303 processing circuit, 304 interface.

Claims

1. A robot control device that controls the position of a motor mounted on each joint of a multi-joint robot based on a command position, comprising: a feedforward model calculation unit that receives the command position as an input and calculates state variables of the feedforward model based on a preset feedforward model; a dynamics approximation model derivation unit that receives a representative posture of the multi-joint robot as an input and derives a dynamics approximation model, which is an approximation model of the dynamics model of the multi-joint robot in the representative posture of the multi-joint robot; a first feedforward transformation unit that calculates a motor position feedforward, which is the motor position of the dynamics model obtained by performing coordinate transformation from the state variables of the feedforward model to the motor position of the dynamics approximation model; and a servo control unit that controls the motor using the motor position feedforward output by the first feedforward transformation unit as a target position.

2. A robot control device that controls the position of a motor mounted on each joint of a multi-joint robot based on a command position, comprising: a feedforward model calculation unit that receives the command position as an input and calculates state variables of the feedforward model based on a preset feedforward model; a dynamics approximation model derivation unit that receives a representative posture of the multi-joint robot as an input and derives a dynamics approximation model which is an approximation model of the dynamics model of the multi-joint robot in the representative posture of the multi-joint robot; a first feedforward transformation unit that performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamics approximation model; and a second feedforward transformation unit that receives state variables of the dynamics approximation model output by the first feedforward transformation unit and calculates state variables of the dynamics model by performing error correction between the dynamics model and the dynamics approximation model based on the dynamics model and the dynamics approximation model. a third feedforward conversion unit that calculates a drive shaft torque of the articulated robot based on the dynamics model and a state variable of the dynamics model, and calculates a motor position feedforward, which is a motor position of the dynamics model, based on the state variable of the dynamics model and the drive shaft torque; and a servo control unit that controls the motor using the motor position feedforward output by the third feedforward conversion unit as a target position.

3. A robot control device according to claim 1 or 2, characterized in that the dynamics model includes state variables related to vibration modes other than the drive directions of the joints of the articulated robot.

4. A robot control device according to any one of claims 1 to 3, characterized in that an extension is performed to include the acceleration or torque of the motor in the state variables by adding an integral element to the input side of the dynamics model.

5. A robot control device according to any one of claims 1 to 4, characterized in that the representative posture is a target posture for positioning control.

6. A robot control device according to any one of claims 1 to 5, characterized in that the dynamics approximation model derivation unit derives the dynamics approximation model by adjusting vibration-related parameters using machine learning or reinforcement learning.

7. A method for controlling a robot using a robot control device that controls the position of a motor mounted on each joint of a multi-joint robot based on a command position, comprising: a first step of taking a command position as input and calculating state variables of the feedforward model based on a preset feedforward model; a second step of taking a representative posture of the multi-joint robot as input and deriving a dynamics approximation model which is an approximation model of the dynamics model of the multi-joint robot in the representative posture of the multi-joint robot; a third step of deriving a coordinate transformation matrix that performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamics approximation model based on the feedforward model and the dynamics approximation model; a fourth step of calculating a motor position feedforward which is the motor position of the dynamics model obtained by performing coordinate transformation from the state variables of the feedforward model to the motor position of the dynamics approximation model using the coordinate transformation matrix; and a fifth step of controlling the motor with the motor position feedforward as a target position.

8. A method for controlling a robot using a robot control device that controls the position of a motor mounted on each joint of a multi-joint robot based on a command position, comprising: a first step of taking a command position as input and calculating state variables of the feedforward model based on a preset feedforward model; a second step of taking a representative posture of the multi-joint robot as input and deriving a dynamics approximation model which is an approximation model of the dynamics model of the multi-joint robot in the representative posture of the multi-joint robot; a third step of deriving a coordinate transformation matrix for performing coordinate transformation from the state variables of the feedforward model to the state variables of the dynamics approximation model based on the feedforward model and the dynamics approximation model; a fourth step of performing coordinate transformation from the state variables of the feedforward model to the state variables of the dynamics approximation model using the coordinate transformation matrix; and a fifth step of taking the state variables of the dynamics approximation model as input and calculating the state variables of the dynamics model by performing error correction between the dynamics model and the dynamics approximation model based on the dynamics model and the dynamics approximation model; a sixth step of calculating joint torques of the articulated robot based on the dynamics model calculated in the fifth step and state variables of the dynamics model, and calculating a motor position feedforward, which is a motor position of the dynamics model, based on the state variables and the joint torque; and a seventh step of controlling the motor with the motor position feedforward as a target position.

Images