Robot control device and robot control method

Abstract

A robot control device (1) is provided with: a feed-forward model calculation unit (13) that calculates a state variable of a feed-forward model on the basis of a preset feed-forward model, using a command position as an input; a dynamic approximation model derivation unit (12) for deriving a dynamic approximation model, which is an approximation model of the dynamic model of the articulated robot when the representative attitude of the articulated robot is input; a first feedforward conversion unit (15) that calculates a motor position feedforward, which is a motor position of a dynamic model obtained by performing coordinate conversion from a state variable of a feedforward model to a motor position of a dynamic approximation model; and a servo control unit (16) that controls the motor using the motor position feedforward output by the first feedforward conversion unit (15) as a target position.

Description

Technical Field

[0001] This invention relates to a robot control device and a robot control method that controls the position of electric motors mounted on each joint of a multi-joint robot based on command position. Background Technology

[0002] Industrial articulated robots, such as vertical and horizontal articulated robots, are used in various factory processes, including assembly, conveying, and inspection. However, articulated robots suffer from several challenges: low mechanical rigidity in their construction leads to vibrations during high-speed movements, resulting in decreased productivity and operational accuracy. To address this issue, motion control technologies to suppress vibrations in articulated robots are being developed. For example, Patent Document 1 discloses a technique that calculates the axial torque required to move the joint angles of an articulated robot according to commanded values ​​based on a dynamic model. This technique calculates the motor angle command value based on parameters including the rigidity of the joint axis, the joint angle command value, and the axial torque, thereby suppressing vibrations in the articulated robot.

[0003] Patent Document 1: Japanese Patent Application Publication No. 2013-248683 Summary of the Invention

[0004] However, the robot control device described in Patent Document 1 has the following problem: although it can suppress vibrations caused by the low rigidity of the drive shaft of the multi-joint robot, it cannot suppress vibrations caused by the low rigidity of the parts of the multi-joint robot other than the joints, or vibrations caused by deformations other than the drive direction of the joints.

[0005] For example, horizontal multi-joint robots are typically equipped with ball screw splines for controlling the height of the end effector. However, when the end effector is lowered, vibrations of the end effector are generated due to the flexural deformation of the ball screw splines. Therefore, in the robot control device described in Patent Document 1, it is difficult to suppress the vibrations caused by the multiple vibration modes of the multi-joint robot.

[0006] The present invention is made in view of the above circumstances, and its object is to provide a robot control device that is not limited to vibration modes caused by the low rigidity of the drive shaft of the multi-joint robot, but can also suppress vibrations caused by multiple vibration modes of the multi-joint robot at the same time.

[0007] To address the aforementioned issues and achieve the objectives, the robot control device of the present invention controls the position of motors mounted on each joint of a multi-joint robot based on command positions. This robot control device includes a feedforward model calculation unit, a dynamic approximation model derivation unit, a first feedforward transformation unit, and a servo control unit. The feedforward model calculation unit takes the command position as input and calculates the state variables of the feedforward model based on a pre-set feedforward model. The dynamic approximation model derivation unit takes a representative posture of the multi-joint robot as input and derives an approximation model of the dynamics of the multi-joint robot at that representative posture, i.e., the dynamic approximation model. The first feedforward transformation unit calculates the motor position feedforward, which is obtained by transforming the state variables of the feedforward model into the motor position of the dynamic approximation model using coordinates. The servo control unit uses the motor position feedforward output from the first feedforward transformation unit as the target position to control the motors.

[0008] The effects of the invention

[0009] According to the robot control device of the present invention, the following effect is achieved: it is not limited to vibration modes caused by the low rigidity of the drive shaft of the multi-joint robot, but can also suppress vibrations caused by multiple vibration modes of the multi-joint robot at the same time. Attached Figure Description

[0010] Figure 1 This is a block diagram showing an example of the structure of a robot system that includes the robot control device according to Embodiment 1.

[0011] Figure 2 This is a perspective view showing the general structure of a robot system that includes the robot control device according to Embodiment 1.

[0012] Figure 3 It means Figure 1 The diagram shows an example of the structure of the computation unit in the feedforward model.

[0013] Figure 4 This is a diagram illustrating a structural example of the 6th feedforward model in Implementation Method 1.

[0014] Figure 5 This is a flowchart illustrating the method for deriving the coordinate transformation matrix from the state variables of the feedforward model to the state variables of the dynamic approximation model in Implementation 1.

[0015] Figure 6 It means Figure 1 The diagram shows an example of the structure of the first feedforward converter.

[0016] Figure 7This is a block diagram showing the structure of a feedforward controller that is virtually equivalent to the robot control device according to Embodiment 1.

[0017] Figure 8 This is a block diagram showing an example of the structure of a robot system that includes the robot control device according to Embodiment 2.

[0018] Figure 9 It means Figure 8 The diagram shows an example of the structure of the first feedforward converter.

[0019] Figure 10 It means Figure 8 The diagram shows an example of the structure of the second feedforward converter.

[0020] Figure 11 It means Figure 8 The diagram shows an example of the structure of the third feedforward converter.

[0021] Figure 12 This is a block diagram illustrating an example of a hardware structure that implements the functions of the robot control device described in embodiments 1 and 2.

[0022] Figure 13 This is a block diagram illustrating other examples of hardware structures that implement the functions of the robot control devices described in embodiments 1 and 2. Detailed Implementation

[0023] Hereinafter, with reference to the accompanying drawings, a robot control device and a robot control method according to embodiments of the present invention will be described in detail. Furthermore, in the following embodiments, the control of a multi-joint robot will be described, but the joints can be linear joints or rotary joints.

[0024] Implementation Method 1

[0025] Figure 1 This is a block diagram showing an example of the structure of a robot system 50 that includes the robot control device 1 according to Embodiment 1. Additionally, Figure 2 This is a perspective view showing the schematic structure of a robot system 50 including the robot control device 1 according to Embodiment 1. The robot control device 1 according to Embodiment 1 is a control device that controls the positions of servo motors 17 mounted on each joint of the multi-joint robot 40 based on command positions. Figure 1 In the diagram, servo motors 171 (J1 axis) to 174 (J4 axis) mounted on each drive axis of the multi-joint robot 40 are shown as servo motors 17. The robot control unit 1 controls the positions of the servo motors 171 (J1 axis) to 174 (J4 axis).

[0026] exist Figure 2 In this example, as a multi-joint robot, a structural example of a horizontal multi-joint robot system 50 is shown. The robot system 50 includes a robot control unit 1, a robotic arm 2, a ball screw spline 5, and an end effector 6. The ball screw spline 5 and the end effector 6 are mounted on the robotic arm 2. Figure 2 In the diagram, the axis of servo motor 171 (J1 axis 3) and the axis of servo motor 172 (J2 axis 4) are shown by dashed lines. Figure 2 The J1-axis servo motor 171 and J2-axis servo motor 172 (not shown) are servo motors used to drive the robotic arm 2. Figure 2 The J3-axis servo motor 173 and J4-axis servo motor 174 (not shown) are servo motors used to drive the ball screw spline 5 and the end effector 6. The robot control unit 1 controls the end effector 6 by driving the J1-axis servo motor 171 to the J4-axis servo motor 174.

[0027] J1-axis servo motor 171 and J2-axis servo motor 172 drive the joints of the articulated robot 40 via reducers (not shown). Reducers used in horizontal and vertical articulated robots typically have low rigidity, resulting in elastic deformation between the input axis side of each motor and the output axis side where the driving force is transmitted. This elastic deformation causes vibration in the end effector 6. Furthermore, when the ball screw spline 5 is lowered, it undergoes flexural deformation. This flexural deformation also contributes to the vibration of the end effector 6. In the ball screw spline 5, flexural deformation occurs in two degrees of freedom in the X-axis and Y-axis directions within the XY plane orthogonal to the extension direction of the ball screw spline 5. In this invention, the input axis side of the J1-axis 3 and J2-axis 4 of the articulated robot 40 is referred to as the "motor axis," and the output axis side is referred to as the "drive axis." Additionally, in this invention, the directions of the flexural deformation generated by the ball screw spline 5, namely the X-axis and Y-axis, are referred to as the "non-drive axes."

[0028] return Figure 1 According to the description, the robot control device 1 has an instruction generation unit 11, a dynamic 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.

[0029] The dynamic approximation model derivation unit 12 takes a representative posture of the multi-joint robot 40, which is preset using the method described later, as input, and derives an approximate model of the dynamic model of the multi-joint robot 40 in that representative posture, i.e., a dynamic approximation model. The dynamic model is the dynamic characteristic model of the multi-joint robot 40, which is the object of control, and the dynamic approximation model is an approximation model of that dynamic characteristic model, i.e., a dynamic characteristic approximation model. To explain in more detail, the dynamic approximation model derivation unit 12 derives the dynamic approximation model of the multi-joint robot 40 by performing an approximate linearization of the dynamic model of the multi-joint robot 40 around the representative posture, based on the pre-created dynamic model of the multi-joint robot 40 and the preset representative posture of the multi-joint robot 40. Generally, the equations of motion of the multi-joint robot 40 are known to be expressed as the following mathematical formula (1).

[0030] Formula 1

[0031]

[0032] Here, the variable q in mathematical formula (1) is a variable representing a quantity called generalized coordinates, containing information about the positions of the driving axis and non-driving axis as defined above. Furthermore, in mathematical formula (1), "q..." · " is the first-order time derivative of the variable q, "q ·· “ is the second time derivative of variable q. The first and second time derivatives of other variables are expressed in the same way. In addition, in mathematical formula (1), “M(q)” represents the inertia matrix, and “C(q, q…” represents the second time derivative of variable q. · “)” indicates the torque caused by centrifugal-Coriolis force, and “τ” indicates the torque of the drive shaft.

[0033] In addition, Figure 2 In the multi-joint robot 40 shown, if we assume the restoring force of the deformation of the reducer and the flexural deformation of the ball screw spline 5, the equation of motion can be expressed as the following mathematical expression (2).

[0034]

Formula 2

[0035]

[0036] In mathematical formula (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. Additionally, "K" θ "K" represents the elastic constant in the direction of the drive shaft. φ “ represents the elastic constant in the direction of the non-driving axis. In mathematical formula (2), “M(q)” shown in mathematical formula (1) is determined by “M”. 11 (θ, φ) "M 12 (θ, φ) "M21 (θ, φ) and M 22 (θ, φ)” is represented as a matrix element. Additionally, the mathematical formula (1) shows “C(q, q · )" by "C" θ (θ, φ, θ) · , φ · ) and "C φ (θ, φ, θ) · , φ · ")" is used as a vector representation of the element, and the "τ" shown in mathematical formula (1) is derived from "-K". θ (θ-θ) m ) and "-K φ φ” is used as a vector representation of the element.

[0037] By approximating the equation of motion represented by mathematical formula (1) around the attitude q0 = (θ0, φ0), mathematical formula (3) is obtained. Furthermore, by transforming mathematical formula (3) into a state equation, mathematical formula (4) is obtained.

[0038]

Formula 3

[0039]

[0040] Formula 4

[0041]

[0042] In this invention, mathematical formula (4) is referred to as the "dynamic approximation model" or the "state-space model of the dynamic approximation model". In mathematical formula (4), "x" is the state variable and "u" is the input variable. In addition, "A" and "B" in mathematical formula (4) are the coefficients assigned to the state variable x and the input variable u in the form of matrices during the transformation of mathematical formula (3) into mathematical formula (4). "A" is called the state matrix, etc., and "B" is called the input matrix, etc. In addition, in this invention, they are simply referred to as "matrix A" and "matrix B".

[0043] exist Figure 2 In the case of the multi-joint robot 40 shown, the system represented by mathematical formula (4) becomes a 12-dimensional system consisting of 6 position variables and 6 velocity variables corresponding to these 6 position variables. These 6 position variables consist of the position of the motor on the J1 axis, the position of the motor on the J2 axis, the position of the J1 axis, the position of the J2 axis, and the flexural deformation of the ball screw spline 5 in the X and Y directions. In addition, the elastic constant K in mathematical formula (3) θ K φIt can also be obtained using machine learning or reinforcement learning. In this case, as an example, based on the acceleration of the end effector 6 obtained by an accelerometer located on the end effector 6, machine learning or reinforcement learning is used to determine the elastic constant K. θ K φ Adjustments will be made.

[0044] The feedforward model calculation unit 13 performs filtering operations on the instruction positions generated by the instruction generation unit 11 based on the feedforward model preset by the method described later, and performs calculations on the state variables of the feedforward model one by one. Figure 3 It means Figure 1 The diagram shows an example of the structure of the feedforward model calculation unit 13. The feedforward model calculation unit 13 includes a J1-axis feedforward model calculation unit 131 and a J2-axis feedforward model calculation unit 132. The feedforward model calculation unit 13 calculates the internal state variables of each axis sequentially by performing filtering operations on the command positions of each axis generated by the command generation unit 11.

[0045] The feedforward model calculation unit 13 sets the degree of the feedforward model in such a way that the degree of the dynamic approximation model is consistent with the total degree of all axes of the feedforward model. As an example, in... Figure 2 In the case of the horizontal multi-joint robot shown, the number of times the dynamic approximation model of mathematical formula (4) is 12, therefore, the number of times of the feedforward model calculation unit 131 of the J1 axis and the feedforward model calculation unit 132 of the J2 axis are set to 6 respectively. Figure 4 This is a diagram illustrating a structural example of the 6th feedforward model in Implementation Method 1. Here, Figure 4 The modules marked "1 / s" represent integrators, and the modules marked w1 to w6 represent modules that amplify the input signal by a factor set by the gain of the feedforward model.

[0046] The coordinate transformation derivation unit 14 derives a coordinate transformation matrix for performing coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model, based on the feedforward model calculation unit 13 and the dynamic approximation model derivation unit 12.

[0047] exist Figure 4In the feedforward model shown, the structure is such that by repeatedly integrating the state variable z6 with the lowest relative degree, other state variables z1 to z5 with higher relative degrees than z6 are obtained. Conversely, by repeatedly differentiating the state variable z1 with the highest relative degree, other state variables z2 to z6 with lower relative degrees than z1 are obtained. Therefore, after obtaining the state variable z1 with the highest relative degree using the state-space model of the dynamic approximation model shown in mathematical formula (4), other state variables z2 to z6 are obtained by repeatedly differentiating this state variable z1. Based on the results, a coordinate transformation matrix is ​​constructed, thereby deriving the coordinate transformation matrix from the state variable z of the feedforward model to the state variable x of the dynamic approximation model. The following is a detailed explanation of this derivation process.

[0048] First, the coordinate transformation derivation part 14 derives the transformation matrix C1 from the state variable x of the dynamic approximation model to the state variable z1 of the feedforward model, as in mathematical formula (5). This process can be performed using the condition that the state variable z1 of the feedforward model is the state variable with the largest relative degree in the state-space model defined in mathematical formula (4).

[0049]

Formula 5

[0050]

[0051] Next, the coordinate transformation derivation unit 14 repeatedly performs time differentiation on the state variable z1 of the feedforward model using mathematical formula (4), as in mathematical formula (6), thereby deriving the transformation matrix, i.e., the output matrix C2 to C6, of the state variables z2 to z6 of the feedforward model other than the state variable z1, as in mathematical formula (6). Here, it should be noted that, based on the condition that the state variable z1 is the state variable with the largest relative degree in the state-space model defined in mathematical formula (4), the terms containing matrix B are zero.

[0052]

Formula 6

[0053]

[0054] Next, the coordinate transformation derivation unit 14 uses the output matrices C1 to C6 of mathematical formulas (5) and (6) to calculate the coordinate transformation matrix T from the state variable x of the dynamic approximation model to the state variable z of the feedforward model, as shown in mathematical formula (7).

[0055]

Formula 7

[0056]

[0057] Next, by calculating the inverse matrix of the coordinate transformation matrix T obtained based on mathematical formula (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 derived. -1 .

[0058] Figure 5 This illustrates the series of processing steps described above. That is, Figure 5 It is the coordinate transformation matrix T used to transform the state variable z of the feedforward model to the state variable x of the dynamic approximation model in Implementation 1. -1 The flowchart illustrates the export method.

[0059] The coordinate transformation derivation section 14 calculates the output matrix C1 based on the state variable x of the dynamic approximation model and the state variable z1 with the largest relative degree in the feedforward model (step ST1).

[0060] The coordinate transformation derivation unit 14 calculates the output matrices C2 to C6 of other state variables z2 to z6 by successively multiplying the output matrix C1 obtained in step ST1 by matrix A (step ST2).

[0061] The coordinate transformation derivation unit 14 arranges the output matrices C1 to C6 obtained in steps ST1 and ST2 to obtain the coordinate transformation matrix T from the state variable x of the dynamic approximation model to the state variable z of the feedforward model (step ST3).

[0062] The coordinate transformation derivation unit 14 calculates the inverse of the coordinate transformation matrix T obtained in step ST3, thereby obtaining the coordinate transformation matrix T from the state variable z of the feedforward model to the state variable x of the dynamic approximation model. -1 (Step ST4).

[0063] Next, the function and structure of the first feedforward transformation unit 15 will be explained. The first feedforward transformation unit 15 is based on the coordinate transformation matrix T derived by the coordinate transformation derivation unit 14. -1 The motor position feedforward is calculated by performing a coordinate transformation from the state variable z of the feedforward model to the motor position of the dynamic approximation model.

[0064] Figure 6 It means Figure 1The diagram shows a structural example of the first feedforward transformation unit 15. The first feedforward transformation unit 15 includes 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, in both the J1-axis motor position feedforward transformation unit 151 and the J2-axis motor position feedforward transformation unit 152, the motor position feedforward is calculated by multiplying by 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 uses the coordinate transformation matrix T derived by the coordinate transformation derivation unit 14. -1 The submatrix corresponding to the position of the motor.

[0065] Furthermore, if the representative pose of the multi-joint robot 40 is determined, then the coordinate transformation matrix T of mathematical formula (7) is uniquely determined. Therefore, the first feedforward transformation unit 15 can also replace the coordinate transformation matrix T or the coordinate transformation matrix T derived by the coordinate transformation derivation unit 14. -1 Instead, it saves the pre-calculated coordinate transformation matrix and uses this coordinate transformation matrix to calculate the motor position feedforward.

[0066] The servo control unit 16 feeds forward the motor position output by the first feedforward conversion unit 15 as the target position and controls the servo motor 17.

[0067] Next, the key points of the operation of the robot control device 1 according to Embodiment 1 will be explained. As described above, the robot control device 1 according to Embodiment 1 derives the coordinate transformation matrix T between the state variable z of the feedforward model and the state variable x of the approximate dynamic model of the multi-joint robot 40, and uses the motor position of the dynamic model obtained by performing coordinate transformation on the state variable z of the feedforward model as the motor position feedforward for servo control. This process can be performed by... Figure 7 The feedforward controller implementation of the virtual structure shown. Figure 7 This is a block diagram illustrating the structure of a feedforward controller 101 virtually equivalent to the robot control device 1 according to Embodiment 1. Regarding... Figure 1 Equivalent structural parts are labeled with the same reference numerals.

[0068] Figure 7The virtual feedforward controller 101 shown has a dynamic approximation model 102 and a state feedback control unit 103. The state feedback controller 103 generates the motor torque based on the command position, and the dynamic approximation model 102 generates the motor position based on the motor torque. When generating the motor torque, the state feedback controller 103 uses the state variables of the controlled object output from the dynamic approximation model 102 for feedback control. The state variables of the controlled object output from the dynamic approximation model 102 include the elastic deformation of the reducer, the deflection deformation of the ball screw spline 5, etc. Therefore, the state feedback controller 103 can suppress the vibration caused by multiple vibration modes in the dynamic approximation model 102.

[0069] The virtual feedforward controller 101 simulates the motor position response during the action of suppressing vibrations in the dynamic approximation model 102, and performs servo control using the vibration-suppressed motor position response as the target position. Therefore, the virtual feedforward controller 101 can effectively suppress vibrations in multiple vibration modes generated in the multi-joint robot 40.

[0070] Furthermore, the extreme configuration that determines the response characteristics of the virtual feedforward controller 101 is equivalent to the extreme configuration that determines the response characteristics of the feedforward model computation unit 13. Therefore, by setting... Figure 4 The feedforward model gains w1 to w6 shown are used to achieve feedforward control with the desired response characteristics. Setting the feedforward model gains w1 to w6, compared to adjusting the feedback gain of the state feedback controller 103, has the advantage of allowing for easier adjustment of the feedforward control responsiveness.

[0071] As mentioned above, Figure 7 The virtual feedforward control shown is equivalent to the feedforward control implemented by the robot control device 1 according to Embodiment 1. Therefore, the robot control device 1 according to Embodiment 1 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 Embodiment 1 can simultaneously suppress vibrations caused by multiple hypothetical vibration modes, including vibrations caused by the flexural deformation of the ball screw spline 5, within an action range in which the approximation accuracy of the dynamic approximation model does not decrease significantly.

[0072] Therefore, the robot control device 1 according to Embodiment 1 is useful as a robot control device that is not limited to the vibration mode caused by the low rigidity of the drive shaft of the multi-joint robot 40, but can also suppress vibration caused by multiple vibration modes of the multi-joint robot at the same time.

[0073] Furthermore, while the above description pre-assigns representative postures for deriving the approximate dynamic model of the multi-joint robot 40, it is not a limitation. For example, by deriving the approximate dynamic model using the target position for positioning control as the representative posture, feedforward control can be implemented to effectively suppress residual vibrations generated in the robotic arm 2 after positioning. Alternatively, coordinate transformation matrices for multiple pre-defined representative postures can be calculated in advance, and these matrices can be switched according to the area where the robotic arm 2 performs its movements.

[0074] Furthermore, in Embodiment 1, the case where the multi-joint robot 40, which is the object of control, is a horizontal multi-joint robot was described as an example. However, regardless of the number of drive axes and non-drive axes of the robot, or its kinematics, the above-described means or methods can be applied. Therefore, the robot control device 1 according to Embodiment 1 can also be applied to other industrial robots, work machines, processing machines, etc., such as vertical multi-joint robots or parallel linkage robots.

[0075] In addition, in Embodiment 1, for the sake of simplicity, structural elements related to the control of the J3 and J4 axes that drive the ball screw spline 5 are not shown, but of course, it is also possible to provide a structure that includes the control of the J3 and J4 axes.

[0076] Furthermore, in Embodiment 1, as an example of vibration of a non-drive axis, the vibration of the flexural deformation of the ball screw spline 5 was addressed. However, it is also possible to extend the model to include vibrations other than the flexural deformation of the ball screw spline 5. For example, the model can also be extended to include vibrations in the Z-axis direction of the root of the arm of a horizontal multi-joint robot, i.e., vibrations in axial directions other than the driving direction of the drive axis.

[0077] Furthermore, in Embodiment 1, the dynamic model is considered as a 12th-order model with 6 position variables and 6 velocity variables. However, it can also be extended by adding integral elements to the input side of the dynamic model to include the motor torque or motor acceleration of the J1 axis 3 and J2 axis 4 as state variables. In this case, the order of the dynamic model and the dynamic approximation model becomes 14th-order. Therefore, by setting the order of the J1 axis feedforward model calculation unit 131 and the J2 axis feedforward model calculation unit 132 to 7th-order, the order of the dynamic characteristic approximation model can be made consistent with the order of the feedforward model. With such a 7th-order structure, feedforward control can be achieved to make the acceleration and torque of the servo motor 17 respond more smoothly.

[0078] As explained above, the robot control device according to Embodiment 1 controls the position of the motors mounted on each joint of a multi-joint robot based on the command position. This robot control device includes a feedforward model calculation unit, a dynamic approximation model derivation unit, a first feedforward transformation unit, and a servo control unit. The feedforward model calculation unit takes the command position as input and calculates the state variables of the feedforward model based on a pre-set feedforward model. The dynamic approximation model derivation unit takes a representative posture of the multi-joint robot as input and derives an approximation model of the dynamics of the multi-joint robot at the representative posture, i.e., the dynamic approximation model. The first feedforward transformation unit calculates the motor position feedforward, which is obtained by performing a coordinate transformation from the state variables of the feedforward model to the motor position of the dynamic approximation model. The servo control unit uses the motor position feedforward output by the first feedforward transformation unit as the target position to control the motor. According to the robot control device configured as described above, it is possible to calculate a motor position command equivalent to that of a case where stabilization feedback control is applied, which uses all states of the controlled object, including states related to the vibration of the mechanical system. Therefore, by controlling the actual motor position based on the motor position command, it is possible not only to suppress vibration modes caused by the low rigidity of the drive shaft of the multi-joint robot, but also to suppress vibrations caused by multiple vibration modes of the multi-joint robot at the same time.

[0079] Furthermore, in the robot control device according to Embodiment 1, vibration modes other than the deformation of the drive shaft can also be included in the dynamic model. In this way, vibrations caused by vibration modes other than the deformation of the drive shaft can be suppressed.

[0080] Furthermore, in the robot control device according to Embodiment 1, the motor acceleration or motor torque can be included in the state variables by adding an integral element to the input side of the dynamic model. This enables feedforward control where the motor acceleration or motor torque provides a smoother response.

[0081] Furthermore, in the robot control device according to Embodiment 1, the representative posture of the multi-joint robot can be set as the target posture for positioning control. In the robot control device according to Embodiment 1, as long as vibration suppression control is performed while approximating the dynamic model through the target posture for positioning control, residual vibration after positioning can be effectively suppressed.

[0082] Furthermore, in the robot control device according to Embodiment 1, the dynamic approximation model derivation unit can also use machine learning or reinforcement learning to derive parameters such as the elastic constant K, which are related to vibration. θ Kφ Adjustments are made to derive an approximate dynamic model. If vibration-related parameters are obtained using machine learning or reinforcement learning, adjusting these parameters becomes easier, enabling appropriate vibration suppression control as described in Implementation 1.

[0083] Furthermore, the robot control method according to Embodiment 1 is a control method using a robot control device that controls the position of the motors mounted on each joint of a multi-joint robot based on command positions. This control method includes steps 1 to 5 as shown below. Step 1 is a step of calculating the state variables of a feedforward model based on a pre-set feedforward model, using the command position as input. Step 2 is a step of deriving an approximate model of the dynamics of the multi-joint robot, i.e., a dynamic approximate model, using a representative pose of the multi-joint robot as input. Step 3 is a step of deriving a coordinate transformation matrix based on the feedforward model and the dynamic approximate model, performing a coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximate model. Step 4 is a step of calculating the motor position feedforward, which is the motor position of the dynamic model obtained by performing a coordinate transformation from the state variables of the feedforward model to the motor position of the dynamic approximate model using the coordinate transformation matrix. Step 5 is a step of controlling the motors using the motor position feedforward as the target position. By executing steps 1 to 4 of steps 1 to 5 above, the robot control device can calculate a motor position command equivalent to that obtained with stabilization feedback control, which utilizes all states of the controlled object, including those related to the vibration of the mechanical system. Furthermore, based on the motor position command calculated in steps 1 to 4, the actual motor position is controlled in step 5. This not only suppresses vibrations caused by the low rigidity of the drive shaft of the jointed robot, but also simultaneously suppresses vibrations caused by multiple vibration modes of the jointed robot.

[0084] Implementation Method 2

[0085] Figure 8 A block diagram showing an example of the structure of a robot system 50a including the robot control device 1a according to embodiment 2 is provided. Figure 8 The robot control device 1a of the robot system 50a shown is... Figure 1 Compared to the robot control device 1 of the robot system 50 shown, in Figure 8 In the middle, Figure 1 The first feedforward converter 15 shown is replaced with a first feedforward converter 15a. Furthermore, in Figure 8In this configuration, a second feedforward conversion unit 18 and a third feedforward conversion unit 19 are added between the first feedforward conversion unit 15a and the servo control unit 16. Figure 8 In the middle, to and Figure 1 Identical or equivalent structural elements are indicated by the same labels.

[0086] Next, the functions and operations of the robot control device 1a according to Embodiment 2 will be described. Furthermore, descriptions of functions or operations common to the robot control device 1 according to Embodiment 1 will be omitted where appropriate.

[0087] The first feedforward transformation unit 15a derives the coordinate transformation matrix T based on the coordinate transformation derivation unit 14. -1 The coordinate transformation is performed from the state variable z of the feedforward model to the state variable x of the dynamic approximation model. Figure 9 It means Figure 8 The diagram shows a structural example of the first feedforward conversion unit 15a. The first feedforward conversion unit 15a includes a drive shaft position feedforward conversion unit 153, a drive shaft speed feedforward conversion unit 154, a drive shaft acceleration feedforward conversion unit 155, a non-drive shaft position feedforward conversion unit 156, and a non-drive shaft speed feedforward conversion unit 157.

[0088] Here, the position, velocity, and acceleration of the drive shafts represent the position, velocity, and acceleration of the output shaft sides of axes such as J1 axis 3 and J2 axis 4. Additionally, the position and velocity of the non-drive shafts represent the position and velocity of the flexural deformation of the ball screw spline 5. The first feedforward transformation unit 15a is based on the coordinate transformation matrix T derived by the coordinate transformation derivation unit 14. -1 The coordinate transformation is performed from the state variable z of the feedforward model to the state variable x of the dynamic approximation model.

[0089] Figure 10 It means Figure 8 A diagram showing an example of the structure of the second feedforward converter 18. Figure 10 The second feedforward converter 18 shown includes a drive shaft acceleration calculation unit 181, a non-drive shaft acceleration calculation unit 182, a drive shaft velocity integration unit 183, a non-drive shaft velocity integration unit 184, a drive shaft position integration unit 185, and a non-drive shaft position integration unit 186. The second feedforward converter 18 takes the state variables of the dynamic approximation model output by the first feedforward converter 15a as input, and performs error correction between the dynamic model and the dynamic approximation model based on the dynamic model and the dynamic approximation model, thereby calculating the state variables of the dynamic model. The processing performed by each structural element in the second feedforward converter 18 will be described below.

[0090] The drive shaft acceleration calculation unit 181 corrects the drive shaft acceleration of the dynamic approximation 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 velocity integration unit 183, the non-drive shaft velocity integration unit 184, the drive shaft position integration unit 185, and the non-drive shaft position integration unit 186, and calculates the drive shaft acceleration of the dynamic model. Mathematical formula (8) shows an example of the calculation formula for correcting the drive shaft acceleration of the dynamic approximation model and calculating the drive shaft acceleration of the dynamic model.

[0091]

Form 8

[0092]

[0093] Here, in mathematical formula (8), the variables with the hat symbol "^" are the state variables of the approximate dynamic model calculated by the first feedforward transform unit 15a, and the variables without the hat symbol "^" represent the state variables of the dynamic model calculated by the drive shaft velocity integral unit 183, the non-drive shaft velocity integral unit 184, the drive shaft position integral unit 185, and the non-drive shaft position integral unit 186, respectively. In mathematical formula (8), the drive shaft acceleration θ^ calculated by the first feedforward transform unit 15a is... ·· (Item 1) Adding feedback terms (Items 2-5) related to the error between the corresponding variables of the dynamic model and the approximate dynamic model, thus affecting the acceleration θ of the driving shaft of the dynamic model. ·· Perform the calculation.

[0094] The non-drive shaft acceleration calculation unit 182 calculates the non-drive shaft acceleration of the dynamic model based on the drive shaft acceleration calculated by the drive shaft acceleration calculation unit 181, the drive shaft velocity, non-drive shaft velocity, drive shaft position, and non-drive shaft position calculated by the drive shaft velocity integration unit 183, the non-drive shaft velocity integration unit 184, the drive shaft position integration unit 185, and the non-drive shaft position integration unit 186, and the dynamic model represented by mathematical formula (2). The non-drive shaft acceleration can be obtained by the following mathematical formula (9). This mathematical formula (9) is obtained by transforming the equation on the non-drive shaft side of mathematical formula (2).

[0095]

Form 9

[0096]

[0097] 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 that perform calculations on the drive shaft speed, non-drive shaft speed, drive shaft position, or non-drive shaft position by integrating the input signals to each structural element, namely drive shaft acceleration, non-drive shaft acceleration, drive shaft speed, or non-drive shaft speed.

[0098] Figure 11 It means Figure 8 A diagram showing an example of the structure of the third feedforward converter 19. Figure 11 The third feedforward converter 19 shown includes a drive shaft torque calculation unit 191, an elastic correction amount calculation unit 192, and a motor position feedforward calculation unit 193. The third feedforward converter 19 calculates the drive shaft torque of the multi-joint robot 40 based on the dynamic model and the state variables of the dynamic model calculated by the second feedforward converter 18. Furthermore, the third feedforward converter 19 calculates the motor position, i.e., the motor position feedforward, of the dynamic model based on the state variables of the dynamic model and the drive shaft torque of the multi-joint robot 40. The processing performed by each structural element in the third feedforward converter 19 will be explained below.

[0099] 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 mathematical formula (10). This mathematical formula (10) is obtained by transforming the equation on the drive shaft side of mathematical formula (2).

[0100]

Formula 10

[0101]

[0102] The elasticity correction calculation unit 192 calculates the correction amount for the elastic deformation generated in the joints of the multi-joint robot 40 based on the drive shaft torque calculation unit 191 and physical parameters related to the deformation of the joints as preset. Mathematical formula (11) shows an example of the calculation formula for the correction amount of elastic deformation. In mathematical formula (11), the drive shaft torque τ calculated by the drive shaft torque calculation unit 191 is multiplied by the elastic constant K of the reducer mounted on the joint. θ The reciprocal of the formula is used to calculate the correction amount for the elastic deformation generated at the joint.

[0103]

Formula 11

[0104]

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

[0106]

Formula 12

[0107]

[0108] If the mathematical expression (12) is transformed, the following mathematical expression (13) can be obtained.

[0109]

Formula 13

[0110]

[0111] In mathematical expression (13), “D θ "" represents the viscous friction constant in the direction of the drive shaft. The elastic correction calculation unit 192 can calculate the correction amount of the elastic deformation generated in the joint according to the differential equation expressed by mathematical formula (13). In addition, the elastic constant K in mathematical formulas (12) and (13) θ and viscous friction constant D θ It can also be obtained using machine learning or reinforcement learning. In this case, as an example, based on the acceleration of the end effector 6 obtained by an accelerometer located on the end effector 6, machine learning or reinforcement learning can be used to determine the elastic constant K. θ and viscous friction constant D θ Adjustments will be made.

[0112] The motor position feedforward calculation unit 193 calculates the motor position feedforward by adding the correction amount of elastic deformation generated at the joint calculated by the elastic correction amount calculation unit 192 to the drive shaft position of the dynamic model calculated by the second feedforward transformation unit 18.

[0113] The servo control unit 16 uses the motor position output by the third feedforward conversion unit 19 as the target position to control the servo motor 17.

[0114] Next, the key points of the operation of the robot control device 1 according to Embodiment 2 will be explained. The robot control device 1a according to Embodiment 2 includes a second feedforward transformation unit 18 and a third feedforward transformation unit 19 in its structural elements. Within these structural elements, the motor position feedforward is calculated based on a dynamic model of the multi-joint robot 40, which is not an approximation. Therefore, the state variables of the dynamic model calculated within each structural element of the second feedforward transformation unit 18 are consistent with the actual state variables in the multi-joint robot 40, assuming that the parameters of the dynamic model are free of errors.

[0115] Furthermore, in Embodiment 2, the robot control device 1a performs a coordinate transformation from the state variable z of the feedforward model to the state variable x of the dynamic approximation model in the first feedforward transformation unit 15a. 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 that converges to a steady state without residual vibration according to the response characteristics set in the feedforward model.

[0116] Therefore, the robot control device 1a according to Embodiment 2 transforms the trajectory of the state variable x without residual vibration of the dynamic approximation model output by the first feedforward transformation unit 15a into the state variable x' of the non-approximation dynamic model through the second feedforward transformation unit 18, and calculates the motor position feedforward based on the drive shaft torque calculated according to the state variable x' of the non-approximation dynamic model through the third feedforward transformation unit 19. Therefore, it is possible to simultaneously suppress vibrations caused by multiple imagined vibration modes, including vibrations of the non-drive shaft, from the non-approximation dynamic model.

[0117] In the robot control device 1 according to Embodiment 1, the servo motor 17 is controlled by using the motor position feedforward calculated based on an approximate model of the dynamic model of the multi-joint robot 40 as the target position. Therefore, residual vibration may occur due to the approximation error between the dynamic model and the dynamic approximation model. In contrast, the robot control device 1a according to Embodiment 2 controls the servo motor 17 by using the motor position feedforward calculated based on a non-approximate dynamic model as the target position. Therefore, residual vibration caused by the approximation error between the dynamic model and the dynamic approximation model is not generated. Therefore, compared with the robot control device 1 according to Embodiment 1, the robot control device 1a according to Embodiment 2 can further improve the vibration suppression effect.

[0118] As explained above, the robot control device according to Embodiment 2 controls the position of the motors mounted on each joint of the multi-joint robot based on the command position. This robot control device includes a feedforward model calculation unit, a dynamic 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 takes the command position as input and calculates the state variables of the feedforward model based on a pre-set feedforward model. The dynamic approximation model derivation unit takes a representative pose of the multi-joint robot as input and derives an approximate model of the dynamics of the multi-joint robot at that representative pose, i.e., a dynamic approximation model. The first feedforward transformation unit performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model. The second feedforward transformation unit takes the state variables of the dynamic approximation model output by the first feedforward transformation unit as input, performs error correction between the dynamic model and the dynamic approximation model based on the dynamic model and the dynamic approximation model, and thus calculates the state variables of the dynamic model. The third feedforward transformation unit calculates the drive shaft torque of the multi-joint robot based on the dynamic model and its state variables. Based on the state variables and drive shaft torque of the dynamic model, it calculates the motor position (i.e., the motor position feedforward) of the dynamic model. The servo control unit uses the motor position feedforward output by the third feedforward transformation unit as the target position to control the motor. The robot control device according to Embodiment 2 adds structural elements to the structure of the robot control device according to Embodiment 1 to correct the error between the approximate dynamic model and the original dynamic model. Therefore, compared to the robot control device according to Embodiment 1, the vibration suppression effect can be further improved.

[0119] Furthermore, the robot control method according to Embodiment 2 is a control method using a robot control device that controls the position of the motors mounted on each joint of a multi-joint robot based on command positions. This control method includes steps 1 to 7 as shown below. Step 1 is a step of calculating the state variables of a feedforward model based on a pre-set feedforward model, using the command position as input. Step 2 is a step of deriving an approximate model of the dynamics of the multi-joint robot, i.e., a dynamic approximate model, using a representative pose of the multi-joint robot as input. Step 3 is a step of deriving a coordinate transformation matrix based on the feedforward model and the dynamic approximate model, performing a coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximate model. Step 4 is a step of performing a coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximate model using the coordinate transformation matrix. Step 5 is a step of calculating the state variables of the dynamic model by using the state variables of the dynamic approximate model as input, performing error correction between the dynamic model and the dynamic approximate model based on the dynamic model and the dynamic approximate model, thereby calculating the state variables of the dynamic model. Step 6 calculates the joint torque of the multi-joint robot based on the dynamic model calculated in Step 5 and the state variables of that dynamic model. Based on the state variables and joint torque, it calculates the motor position feedforward of the dynamic model. Step 7 controls the motor by using the motor position feedforward as the target position. The robot control method according to Embodiment 2 adds steps 5 and 6 compared to the robot control method according to Embodiment 1. Step 5 performs error correction between the approximate dynamic model and the original dynamic model, and step 6 uses the error-corrected dynamic model to calculate the motor position feedforward. Therefore, compared to the robot control device according to Embodiment 1, the vibration suppression effect can be further improved.

[0120] Next, refer to Figure 12 and Figure 13 The accompanying drawings illustrate the hardware structure for implementing the functions of the robot control devices 1 and 1a according to embodiments 1 and 2. Figure 12 This is a block diagram illustrating an example of a hardware structure that implements the functions of robot control devices 1 and 1a according to embodiments 1 and 2. Figure 13 This is a block diagram illustrating other examples of hardware structures that implement the functions of robot control devices 1 and 1a according to embodiments 1 and 2.

[0121] In cases where some or all of the functions of the robot control devices 1 and 1a according to embodiments 1 and 2 are implemented, such as Figure 12As shown, it can be configured to include a processor 300 for performing calculations, a memory 302 for storing programs read by the processor 300, and an interface 304 for inputting and outputting signals.

[0122] Processor 300 is an example of an arithmetic unit. Processor 300 can be an arithmetic unit referred to as a microprocessor, microcomputer, CPU (Central Processing Unit), or DSP (Digital Signal Processor). Additionally, memory 302 can exemplify non-volatile or volatile semiconductor memories such as RAM (Random Access Memory), ROM (Read Only Memory), flash memory, EPROM (Erasable Programmable ROM), and EEPROM (Electrically EPROM), as well as disks, floppy disks, optical disks, high-density disks, mini-disks, and DVDs (Digital Versatile Discs).

[0123] The memory 302 stores programs that execute the functions of the robot control devices 1 and 1a according to embodiments 1 and 2. The processor 300 receives and sends necessary information via the interface 304, executes the programs stored in the memory 302, and performs the aforementioned processing by referring to tables stored in the memory 302. The calculation results obtained by the processor 300 can be stored in the memory 302.

[0124] Furthermore, when implementing a portion of the functions of the robot control devices 1 and 1a according to embodiments 1 and 2, it is also possible to use Figure 13 The processing circuit 303 is shown. The processing circuit 303 can 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 transmitted and received via interface 304.

[0125] In addition, a portion of the processing of the robot control devices 1 and 1a can be performed by the processing circuit 303, and processing not performed by the processing circuit 303 can be performed by the processor 300 and the memory 302.

[0126] The structure shown in the above embodiments is an example and can be combined with other known technologies. The embodiments can also be combined with each other, and parts of the structure can be omitted or changed without departing from the main idea.

[0127] Explanation of the label

[0128] 1. 1a Robot control device; 2 Robotic arm; 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; 171 J1 axis servo motor; 172 J2 axis servo motor; 173 J3 axis servo motor; 174 J4 axis servo motor; 18 Second feedforward transformation unit; 19 Third feedforward transformation unit; 40 Multi-joint 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 transformation unit; 152 J2 axis motor position feedforward transformation unit; 153 154 Drive shaft position feedforward conversion unit; 155 Drive shaft speed feedforward conversion unit; 156 Drive shaft acceleration feedforward conversion unit; 157 Non-drive shaft position feedforward conversion unit; 181 Drive shaft acceleration calculation unit; 182 Non-drive shaft acceleration calculation unit; 183 Drive shaft speed integration unit; 184 Non-drive shaft speed integration unit; 185 Drive shaft position integration unit; 186 Non-drive shaft position integration unit; 191 Drive shaft torque calculation unit; 192 Elastic correction 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 motors mounted on each joint of a multi-joint robot based on command positions. The robot control device is characterized by having: The feedforward model computation unit takes the instruction position as input and performs calculations on the state variables of the feedforward model based on a pre-set feedforward model. The dynamic approximation model derivation unit takes the representative posture of the multi-joint robot as input and derives an approximate model of the dynamic model of the multi-joint robot when the representative posture is taken; that is, the dynamic approximation model. The first feedforward transformation unit performs calculations on the motor position feedforward, which is obtained by performing a coordinate transformation from the state variables of the feedforward model to the motor position of the approximate dynamic model, thus representing the motor position of the dynamic model; and The servo control unit feeds forward the motor position output by the first feedforward conversion unit as the target position and controls the motor.

2. A robot control device that controls the position of motors mounted on each joint of a multi-joint robot based on commanded positions. The robot control device is characterized by having: The feedforward model computation unit takes the instruction position as input and performs calculations on the state variables of the feedforward model based on a pre-set feedforward model. The dynamic approximation model derivation unit takes the representative posture of the multi-joint robot as input and derives an approximate model of the dynamic model of the multi-joint robot when the representative posture is taken; that is, the dynamic approximation model. The first feedforward transformation unit performs coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model; The second feedforward transformation unit takes the state variables of the approximate dynamic model output by the first feedforward transformation unit as input, performs error correction between the dynamic model and the approximate dynamic model based on the dynamic model and the approximate dynamic model, and then performs calculations on the state variables of the dynamic model. The third feedforward transformation unit calculates the drive shaft torque of the multi-joint robot based on the dynamic model and the state variables of the dynamic model, and calculates the motor position, i.e. the motor position feedforward of the dynamic model based on the state variables of the dynamic model and the drive shaft torque. as well as The servo control unit feeds forward the motor position output by the third feedforward conversion unit as the target position and controls the motor.

3. The robot control device according to claim 1 or 2, characterized in that, The dynamic model includes state variables related to the vibration modes of the multi-joint robot, excluding the drive direction of the joints.

4. The robot control device according to any one of claims 1 to 3, characterized in that, By adding an integral element to the input side of the dynamic model, an extension is made to include the acceleration or torque of the electric motor in the state variables.

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

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

7. A method for controlling a robot, which is performed using a robot control device that controls the positions of motors mounted on each joint of a multi-joint robot based on command positions. The control method for this robot is characterized by including the following steps: Step 1: Using the command position as input, calculate the state variables of the feedforward model based on the pre-set feedforward model; The second step is to take the representative posture of the multi-joint robot as input and derive an approximate model of the dynamic model of the multi-joint robot when the representative posture is taken as input, namely the dynamic approximate model. Step 3: Based on the feedforward model and the dynamic approximation model, derive the coordinate transformation matrix for performing coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model; Step 4 involves calculating the motor position feedforward. This motor position feedforward is obtained by transforming the state variables of the feedforward model to the motor position of the approximate dynamic model using the coordinate transformation matrix, thus obtaining the motor position of the dynamic model. Step 5: The motor position is fed forward as the target position for motor control.

8. A method for controlling a robot, which is performed using a robot control device that controls the position of motors mounted on each joint of a multi-joint robot based on command positions. The control method for this robot is characterized by including the following steps: Step 1: Take the command position as input and calculate the state variables of the feedforward model based on the pre-set feedforward model; Step 2: Using the representative posture of the multi-joint robot as input, derive an approximate model of the dynamics model of the multi-joint robot in the representative posture, i.e., the approximate dynamics model. Step 3: Based on the feedforward model and the dynamic approximation model, derive the coordinate transformation matrix for performing coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model; Step 4: Use the coordinate transformation matrix to perform coordinate transformation from the state variables of the feedforward model to the state variables of the dynamic approximation model; Step 5 involves taking the state variables of the approximate dynamic model as input, and performing error correction between the dynamic model and the approximate dynamic model based on the dynamic model and the approximate dynamic model, thereby calculating the state variables of the dynamic model. Step 6: Based on the dynamic model calculated in Step 5 and the state variables of the dynamic model, calculate the joint torque of the multi-joint robot; and based on the state variables and the joint torque, calculate the motor position, i.e., the motor position feedforward, of the dynamic model; and Step 7: The motor position is fed forward as the target position for motor control.

Images