A block cooperative control method for a multi-degree-of-freedom SCARA robot arm

Abstract

This invention discloses a block-based cooperative control method for a multi-degree-of-freedom SCARA robotic arm, aiming to address the problem caused by existing control methods not fully considering the operating characteristics of joint motors, thereby improving the trajectory tracking accuracy and response speed of the robotic arm. First, an integrated model of the robotic arm system is established. Then, based on this integrated model, the robotic arm is decoupled into a rotational subsystem and a kinetic subsystem. For the rotational subsystem, this invention designs a backstepping controller with cross-coupling compensation to achieve pose control of the robotic arm's end effector in the horizontal direction. For the kinetic subsystem, this invention designs a backstepping controller to achieve position control of the robotic arm's end effector in the vertical direction. Therefore, this invention effectively solves the strong coupling problem in the control of multi-degree-of-freedom robotic arms, significantly improves the accuracy and stability of trajectory tracking, and simultaneously achieves closed-loop control of the system by introducing a cross-coupling compensator.

Description

Technical Field

[0001] This invention belongs to the field of motor control technology, specifically relating to a block cooperative control method for a multi-degree-of-freedom SCARA robotic arm. Background Technology

[0002] Industrial robotic arms, as an important indicator of a country's technological innovation and high-end manufacturing level, have extremely high value in industry. The widespread application of robotic arms can significantly improve labor productivity, reduce production costs, and improve working conditions. Therefore, their application has expanded from traditional industrial fields such as welding and assembly to more complex environments such as aerospace and deep sea.

[0003] Among numerous industrial robotic arms, the SCARA (Selective Compliance Assembly Robot Arm) is widely used due to its unique structure. It typically has three axes, all vertically distributed: the first axis is a rotational axis, used to enable the upper arm to rotate around its base in the horizontal plane, possessing one rotational degree of freedom and driven by a single articulated motor; the second axis is a rotational axis, used to enable the forearm to rotate around its upper arm in the horizontal plane, possessing one rotational degree of freedom and driven by a single articulated motor; the third axis is both a translational and rotational axis, used to enable rotation in the horizontal plane and movement in the vertical direction, possessing one translational degree of freedom and one rotational degree of freedom. Due to the special mechanical structure of the lead screw spline, the corresponding degree of freedom can be decoupled and driven by two articulated motors. This structure enables the end effector of the robotic arm to move in three-dimensional space and horizontal orientation, exhibiting high stiffness and positioning accuracy in the horizontal plane, while demonstrating good compliance in the vertical direction, making it ideal for tasks such as assembly, picking, and placement.

[0004] With the increasing demands of applications, higher requirements are being placed on the accuracy, stability, and speed of trajectory tracking control for robotic arms. The goal of trajectory tracking control is to drive the joint motors of the robotic arm through a controller, enabling its end effector to move precisely along a given desired trajectory. However, achieving high-precision trajectory tracking control faces many challenges: First, a robotic arm with two or more degrees of freedom is a highly complex nonlinear system with strong coupling between its joints; second, due to factors such as modeling errors, manufacturing tolerances, load variations, external interference, and joint friction, the mathematical model of the robotic arm inevitably contains uncertainties; finally, a robotic arm is a constrained multi-input multi-output system, and its joint motors are limited in torque, speed, and acceleration.

[0005] Currently, the mainstream control strategies applied to rigid robotic arm trajectory tracking include PID (Proportional-Integral-Derivative) control, sliding mode control, fuzzy control, iterative learning control, neural network control, model predictive control, and combinations of the above methods. PID control has a simple structure but poor adaptability and is usually used as the basis of intelligent controllers. Adaptability is achieved by dynamically adjusting control parameters through intelligent algorithms such as genetic algorithms and particle swarm optimization or fuzzy controllers. Neural network control can approximate any nonlinear system through training, but it requires a large number of high-quality training samples. Model predictive control is suitable for achieving robust control of constrained complex systems, but it has a large computational load and depends on the reliability of the predictive model.

[0006] Although existing control strategies have achieved varying degrees of performance improvement, most studies on multi-degree-of-freedom robotic arms have not fully considered the specific operational characteristics of each joint motor, such as starting, speed regulation, and braking, when designing controllers. This has led to a significant gap between the theoretical simulation and practical applications of many control strategies, thus limiting further improvements in robotic arm performance and its application in complex industrial environments. Summary of the Invention

[0007] In view of the above, the present invention provides a block cooperative control method for a multi-degree-of-freedom SCARA robotic arm. This method optimizes the control structure of the robotic arm by uniformly modeling the robotic arm and joint motors, thereby achieving high-precision pose control.

[0008] A block-based cooperative control method for a multi-degree-of-freedom SCARA robotic arm includes the following steps:

[0009] (1) Establish the kinematic and dynamic models of SCARA, and combine them with the electrical and mechanical models of the joint motors to establish an integrated model of the robotic arm system;

[0010] (2) Based on the integrated model of the robotic arm system, SCARA is decoupled into a rotary subsystem model and a mobile subsystem model;

[0011] (3) Based on the rotating subsystem model, the controller of the rotating subsystem is designed as a backstep controller based on cross-coupling compensation to realize the position control and attitude control of the SCARA end effector in the horizontal direction.

[0012] (4) Based on the mobile subsystem model, the controller of the mobile subsystem is designed as a backstepping controller to realize the position control of the SCARA end effector in the vertical direction.

[0013] Furthermore, the kinematic model expression of SCARA in step (1) is as follows:

[0014]

[0015] Where: x, y, z represent the three-dimensional spatial positions of the SCARA end effector, and θ represents the position of the SCARA end effector. z Let q1, q2, and q3 be the rotation angles of the three SCARA rotary joints, x4 be the movement distance of the SCARA translator joint, l1 and l2 be the lengths of the two SCARA rotary arms, and h0 be the initial height of the SCARA end effector.

[0016] Furthermore, the dynamic model expression of SCARA in step (1) is as follows:

[0017]

[0018] C = m3

[0019] D=J3

[0020] G = m3g

[0021] H = m2l1r2 + m3l1l2

[0022] in: Let be the first-order differentials of q1, q2, q3, and x4, respectively. Let f1, f2, f3, and x4 be the second derivatives of q1, q2, q3, and x4, respectively; let τ1, τ2, and τ3 be the driving torques of the three rotary joints of the SCARA; let f4 be the driving force of the traverse joint of the SCARA; let m1 and m2 be the masses of the two rotary arms of the SCARA; let m3 be the total mass of the traverse arm and the load; let r1 and r2 be the positions of the centers of mass of the two rotary arms of the SCARA; let J1 and J2 be the moments of inertia of the two rotary arms of the SCARA; let J3 be the total moment of inertia of the traverse arm and the load; let g be the acceleration due to gravity; and let A, B, C, D, G, and H be intermediate variables.

[0023] Furthermore, the electrical and mechanical model expressions of the joint motor in step (1) are as follows:

[0024] u k =(2D k -1)U Nk k = 1, 2, 3, 4

[0025]

[0026] T k =f k r k k = 1, 2, 3, 4

[0027] x k =q mk rk k = 1, 2, 3, 4

[0028]

[0029] τ k =c k T k k = 1, 2, 3, 4

[0030] Where: u k D is the input voltage of the k-th joint motor. k U represents the duty cycle of the PWM (Pulse Width Modulation) wave of the k-th joint motor. Nk Let i be the rated voltage of the k-th joint motor. k Let be the armature current of the k-th joint motor. For i k The first derivative, R k Let L be the armature resistance of the k-th joint motor. k J is the armature inductance of the k-th joint motor. mk Let T be the rotor inertia of the k-th joint motor. emk Let r be the electromagnetic torque of the motor at the k-th joint. k Let T be the rotor radius of the k-th joint motor. k f is the rotor load torque of the motor at the k-th joint. k C is the rotor tangential load force of the k-th joint motor. e φ k Let C be the potential constant of the motor at the k-th joint. t φ k Let q be the torque constant of the motor at the k-th joint. mk Let be the rotor angular velocity of the k-th joint motor. For q mk The second derivative of x k Let c be the distance traveled at the k-th joint of the motor rotor circumference. k Let q be the reduction ratio of the k-th joint motor reducer. k Let τ be the rotation angle of the k-th joint motor (the first three joint motors correspond one-to-one with the three rotary joints of SCARA, and the last joint motor corresponds to the translating joint). k This represents the driving torque of the motor at the k-th joint.

[0031] Furthermore, in step (1), the kinematic and dynamic models of SCARA are substituted into the electrical and mechanical models of the joint motor, and redundant variables are replaced to obtain the integrated model expression of the robotic arm system as follows:

[0032]

[0033]

[0034] Furthermore, in step (2), the three SCARA rotary joints are divided into a rotary subsystem, which is responsible for the pose motion of the end effector in the horizontal direction; the SCARA translating joint is divided into a translating subsystem, which is responsible for the position motion of the end effector in the vertical direction. The decoupled rotary subsystem model expression is as follows:

[0035] (x,y,θ z )=FK rot (q rot )

[0036] q rot =IK rot (x,y,θ z )

[0037]

[0038] The decoupled mobile subsystem model expression is as follows:

[0039] z = h0 + x4

[0040] x4=z-h0

[0041]

[0042]

[0043] Among them: FK rot () represents the first three terms of the SCARA forward kinematics equations, IK rot () represents the first three terms of the SCARA inverse kinematics equations, q rot =[q1 q2 q3] T Let D be the column vector of rotation angles of the revolute joints in the revolute subsystem. rot =[D1 D2 D3] T Let I be the column vector of the PWM wave duty cycle of the joint motor in the rotary subsystem. rot =[i1 i2 i3] T U is the column vector of armature currents of the joint motor in the rotary subsystem. Nrot =diag[U N1 U N2 U N3 [C] represents the rated voltage matrix of the joint motors in the rotary subsystem. e φ rot =diag[C e φ1 C e φ2 C e φ3] is the potential constant matrix of the joint motor in the rotary subsystem, Ct φ rot =diag[C t φ1 C t φ2 C t φ3] is the torque constant matrix of the joint motor in the rotary subsystem, c rot =diag[c1 c2 c3] is the reduction ratio matrix of the articulated motor reducer in the rotary subsystem, R rot =diag[R1 R2 R3] is the armature resistance matrix of the joint motor in the rotary subsystem, L rot =diag[L1 L2 L3] is the armature inductance matrix of the joint motor in the rotary subsystem, J rot =diag[J m1 J m2 J m3 [ ] represents the rotor inertia matrix of the articulated motor in the rotary subsystem, where T denotes transpose and diag[ ] denotes a diagonal matrix. and q rot The second and first derivatives, For I rot The first differential, M rot Let C be the inertia matrix of the rotating subsystem. rot Let be the centrifugal force and Coriolis force matrix of the rotating subsystem.

[0044] Furthermore, the controller of the rotary subsystem in step (3) includes a backstepping controller and a cross-coupling compensator. The backstepping controller uses the desired joint trajectory of the three rotary joints compensated by the cross-coupling compensator as the input signal based on the backstepping method, and uses the position, speed, and current measured by the joint motor sensors of the three rotary joints as the feedback signal to adjust the duty cycle of the PWM wave of the corresponding joint motor so that the actual pose of the end effector in the horizontal direction follows the desired pose. The control law expression of the backstepping controller is as follows:

[0045]

[0046]

[0047] Among them: U rot , e rot1 ,e rot2 ,e rot3 As an intermediate variable, They are respectively The first-order differential, Let be the column vector of the desired joint trajectories of the rotating subsystem. These are the expected joint trajectories of the three rotational joints of the SCARA, K. rot1 ,Krot2 ,K rot3 These are the backstepping controller parameters for the rotating subsystem;

[0048] The cross-coupling compensator is used to estimate the profile error of the end effector in the horizontal direction and to compensate for the desired joint trajectory of the three rotary joints by the following expression.

[0049]

[0050] Where: ε is the contour error of the end effector's horizontal pose, x * ,y * The desired position of the end effector in the horizontal direction. The desired orientation of the end effector in the horizontal direction. x * ,y * The first differential, ε x ,ε y , For the end effector in x, y, θ z The corresponding contour error, x c ,y c Let R be the center of the circle of curvature of the desired trajectory of the end effector in the horizontal direction, R be the radius of the circle of curvature of the desired trajectory of the end effector in the horizontal direction, and κ be the curvature of the desired trajectory of the end effector in the horizontal direction. c e is the contour error estimation constant. v As an intermediate variable;

[0051] The compensation law expression for the cross-coupled compensator is as follows:

[0052]

[0053] in: Let Δq be the column vector of the expected joint trajectories of the compensated rotary subsystem. rot This represents the expected joint trajectory compensation amount based on the contour error. Let C be the desired horizontal pose column vector of the end effector. ε This is the compensation gain constant.

[0054] Furthermore, in step (4), the controller of the moving subsystem adopts a backstepping controller. This backstepping controller uses the desired joint trajectory of the moving joint as the input signal and the position, speed, and current measured by the joint motor sensor corresponding to the moving joint as the feedback signal. It adjusts the duty cycle of the PWM wave of the corresponding joint motor so that the actual position of the end effector in the vertical direction follows the desired position. The control law expression of the backstepping controller is as follows:

[0055]

[0056] Where: u4, e mov1 ,e mov2 ,e mov3 As an intermediate variable, They are respectively The first-order differential, For the desired joint trajectory of the moving joint, K mov1 ,K mov2 ,K mov3 These are the backstepping controller parameters for the mobile subsystem.

[0057] A computer device includes a memory and a processor, wherein the memory stores a computer program and the processor executes the computer program to implement the above-described block cooperative control method for a multi-degree-of-freedom SCARA robotic arm.

[0058] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described block cooperative control method for a multi-degree-of-freedom SCARA robotic arm.

[0059] This invention aims to address the problem caused by existing control methods not fully considering the operating characteristics of joint motors, thereby improving the trajectory tracking accuracy and response speed of robotic arms. First, an integrated model of the robotic arm system is established. Then, based on this integrated model, the robotic arm is decoupled into a rotational subsystem and a kinetic subsystem. For the rotational subsystem, this invention designs a backstepping controller that incorporates cross-coupling compensation to achieve pose control of the robotic arm's end effector in the horizontal direction. For the kinetic subsystem, this invention designs a backstepping controller to achieve position control of the robotic arm's end effector in the vertical direction.

[0060] This invention effectively solves the strong coupling problem in multi-degree-of-freedom robotic arm control by decoupling the robotic arm system and designing targeted controllers for each subsystem, significantly improving the accuracy and stability of trajectory tracking. Simultaneously, the introduction of a cross-coupling compensator enables closed-loop control of the system. Therefore, this invention, through system decoupling, model-based nonlinear backstepping control, and cross-coupling compensation for contour errors, effectively overcomes the limitations of traditional open-loop control methods, solves the problems of strong joint coupling and nonlinear interference of joint motors, and thus improves the trajectory tracking accuracy and robustness of the robotic arm. Attached Figure Description

[0061] Figure 1 This is a schematic diagram of the structure and dimensional parameters of the four-degree-of-freedom SCARA robotic arm in an embodiment of the present invention.

[0062] Figure 2This is a schematic diagram of the joint motor circuit of the four-degree-of-freedom SCARA robotic arm in an embodiment of the present invention.

[0063] Figure 3(a) is a schematic diagram of the simulation model of the four-degree-of-freedom SCARA robotic arm and joint motor in an embodiment of the present invention.

[0064] Figure 3(b) is a schematic diagram of the internal structure of the PWM DC motor joint component in the simulation model of the four-degree-of-freedom SCARA robotic arm in the embodiment of the present invention.

[0065] Figure 3(c) is a schematic diagram of the internal structure of the SCARA component in the simulation model of the four-degree-of-freedom SCARA robotic arm in the embodiment of the present invention.

[0066] Figure 4 This is a block diagram of the block cooperative control strategy for a four-degree-of-freedom SCARA robotic arm in an embodiment of the present invention.

[0067] Figure 5 This is a block diagram of the traditional three-loop PID control strategy for a four-DOF SCARA robotic arm in an embodiment of the present invention.

[0068] Figure 6(a) is a schematic diagram comparing the trajectory of the end effector in the horizontal direction under the control strategy of the present invention and the traditional three-loop PID control strategy.

[0069] Figure 6(b) is a schematic diagram comparing the contour error of the end effector in the horizontal direction under the control strategy of the present invention and the traditional three-loop PID control strategy.

[0070] Figure 6(c) is a schematic diagram comparing the contour error of the end effector in the X direction under the control strategy of the present invention and the traditional three-loop PID control strategy.

[0071] Figure 6(d) is a schematic diagram comparing the contour error of the end effector in the Y direction under the control strategy of the present invention and the traditional three-loop PID control strategy.

[0072] Figure 6(e) is a schematic diagram comparing the trajectory of the end effector in the Z direction under the control strategy of the present invention and the traditional three-loop PID control strategy.

[0073] Figure 6(f) is a schematic diagram comparing the contour error of the end effector in the Z direction under the control strategy of the present invention and the traditional three-loop PID control strategy.

[0074] Figure 6(g) shows the end effector at θ under the control strategy of this invention and the traditional three-loop PID control strategy. z A diagram comparing the trajectories of different directions.

[0075] Figure 6(h) shows the end effector at θ under the control strategy of this invention and the traditional three-loop PID control strategy. zA schematic diagram comparing the contour errors of the direction.

[0076] Figure 6(i) is a schematic diagram comparing the root mean square error of the horizontal pose and contour error of the end effector under the control strategy of the present invention and the traditional three-loop PID control strategy. Detailed Implementation

[0077] To describe the present invention in more detail, the technical solution of the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0078] This implementation uses a Simulink-built simulation model of a four-DOF SCARA robotic arm driven by a wound-rotor DC motor to simulate the trajectory tracking control of the end effector. The structure and relevant dimensional parameters of the four-DOF SCARA robotic arm are as follows: Figure 1 As shown, the circuit of the joint motor is as follows: Figure 2 As shown, the four joint motors of the SCARA robotic arm are all wound-rotor DC motors, and the forward and reverse rotation control of the motors is achieved by using an H-bridge PWM drive circuit.

[0079] Figure 3(a) shows the Simulink simulation model of the four-DOF SCARA robotic arm. The PWM DC motorjoint1, 2, 3, and 4 components are the simulation components of the four joint motors. The duty cycle D and the load torque TL or load force FL fed back from the SCARA components are the inputs, and the joint rotation angle or displacement, rotational angular velocity or movement speed, and current after passing through the reducer are the outputs. The SCARA components are the dynamic simulation components of the SCARA robotic arm. The rotation angles q1, q2, and q3 of the three rotary joints and the movement distance x4 of the traverse joint are the inputs, and the driving torques tau1, tau2, and tau3 of the corresponding joints, the driving force f4, and the pose of the end effector are the outputs, and a 3D model is generated.

[0080] Figure 3(b) shows the internal structure of the PWM DC motor joint component. The joint motor is controlled by four IGBTs. When the PWM generator outputs 1, the IGBTs at the upper left and lower right corners are turned on, and the joint motor ends are input with a positive rated voltage. When the PWM generator outputs 0, the IGBTs at the upper right and lower left corners are turned on, and the joint motor ends are input with a reverse rated voltage.

[0081] Figure 3(c) shows the internal structure of the SCARA component. The Base, FirstArm, SecondArm, MoveArm, and Load components are the base, upper arm, lower arm, moving arm, and load, respectively, containing their respective physical properties and related spatial relationships. The Revolute1, Revolute2, and Cylindrical components are axis 1, axis 2, and axis 3, respectively, containing the joint's power and motion.

[0082] To achieve trajectory tracking control of the end effector of a four-degree-of-freedom SCARA robotic arm, this embodiment provides a block-based cooperative control strategy, the specific process of which is as follows:

[0083] (1) Establish the kinematic and dynamic model of the four-degree-of-freedom SCARA manipulator, and combine it with the electrical and mechanical models of the joint motors to establish an integrated model of the manipulator system;

[0084] 1.1 Establish the kinematic model of the robotic arm, whose expression is as follows:

[0085]

[0086] Where: x, y, z represent the three-dimensional spatial positions of the SCARA end effector, and θ represents the position of the SCARA end effector. z Let q1, q2, and q3 be the rotation angles of the three SCARA rotary joints, x4 be the movement distance of the SCARA translator joint, l1 and l2 be the lengths of the two SCARA rotary arms, and h0 be the initial height of the SCARA end effector.

[0087] 1.2 Establish the dynamic model of the robotic arm, whose expression is as follows:

[0088]

[0089]

[0090] C = m3

[0091] D=J3

[0092] G = m3g

[0093] H = m2l1r2 + m3l1l2

[0094] in: Let be the first-order differentials of q1, q2, q3, and x4, respectively. Let f1, f2, f3, and x4 be the second derivatives of q1, q2, q3, and x4, respectively; let τ1, τ2, and τ3 be the driving torques of the three rotary joints of the SCARA; let f4 be the driving force of the traverse joint of the SCARA; let m1 and m2 be the masses of the two rotary arms of the SCARA; let m3 be the total mass of the traverse arm and the load; let r1 and r2 be the positions of the centers of mass of the two rotary arms of the SCARA; let J1 and J2 be the moments of inertia of the two rotary arms of the SCARA; let J3 be the total moment of inertia of the traverse arm and the load; let g be the acceleration due to gravity; and let A, B, C, D, G, and H be intermediate variables.

[0095] 1.3 Establish the electrical and mechanical models of the robotic arm joint motors, with the following expressions:

[0096] u k =(2D k -1)U Nk k = 1, 2, 3, 4

[0097]

[0098] T k =f k r k k = 1, 2, 3, 4

[0099] x k =q mk r k k = 1, 2, 3, 4

[0100]

[0101] τ k =c k T k k = 1, 2, 3, 4

[0102] Where: u k D is the input voltage of the k-th joint motor. k U represents the duty cycle of the PWM wave of the k-th joint motor. Nk Let i be the rated voltage of the k-th joint motor. k Let be the armature current of the k-th joint motor. For i k The first derivative, R k Let L be the armature resistance of the k-th joint motor. k J is the armature inductance of the k-th joint motor. mk Let T be the rotor inertia of the k-th joint motor. emk Let r be the electromagnetic torque of the motor at the k-th joint. k Let T be the rotor radius of the k-th joint motor. kf is the rotor load torque of the motor at the k-th joint. k C is the rotor tangential load force of the k-th joint motor. e φ k Let C be the potential constant of the motor at the k-th joint. t φ k Let q be the torque constant of the motor at the k-th joint. mk Let be the rotor angular velocity of the k-th joint motor. For q mk The second derivative of x k Let c be the distance traveled at the k-th joint of the motor rotor circumference. k Let q be the reduction ratio of the k-th joint motor reducer. k Let τ be the rotation angle of the k-th joint motor. k This represents the driving torque of the motor at the k-th joint.

[0103] 1.4 Establish an integrated model of the robotic arm system. Substitute the expression of the robotic arm model into the expression of the joint motor model, and replace redundant variables to obtain the integrated model expression as follows:

[0104]

[0105] (2) Based on the above integrated model, the robotic arm system is decoupled into a rotation subsystem and a movement subsystem.

[0106] In this implementation, the three rotary joints are divided into a rotary subsystem, responsible for the horizontal pose motion of the end effector; the translating joints are divided into a translating subsystem, responsible for the vertical position motion. The matrix form expression of the decoupled rotary subsystem model is as follows:

[0107] (x,y,θ z )=FK rot (q rot )

[0108] q rot =IK rot (x,y,θ z )

[0109]

[0110]

[0111] The expression for the decoupled mobile subsystem model is as follows:

[0112] z = h0 + x4

[0113] x4=z-h0

[0114]

[0115] Among them: FK rot () represents the first three terms of the SCARA forward kinematics equations, IK rot () represents the first three terms of the SCARA inverse kinematics equations, q rot =[q1 q2 q3] T Let D be the column vector of rotation angles of the revolute joints in the revolute subsystem. rot =[D1 D2 D3] T Let I be the column vector of the PWM wave duty cycle of the joint motor in the rotary subsystem. rot =[i1 i2 i3] T U is the column vector of armature currents of the joint motor in the rotary subsystem. Nrot =diag[U N1 U N2 U N3 [C] represents the rated voltage matrix of the joint motors in the rotary subsystem. e φ rot =diag[C e φ1 C e φ2 C e φ3] is the potential constant matrix of the joint motor in the rotary subsystem, C t φ rot =diag[C t φ1 C t φ2 C t φ3] is the torque constant matrix of the joint motor in the rotary subsystem, c rot =diag[c1 c2 c3] is the reduction ratio matrix of the articulated motor reducer in the rotary subsystem, R rot =diag[R1 R2 R3] is the armature resistance matrix of the joint motor in the rotary subsystem, L rot =diag[L1 L2 L3] is the armature inductance matrix of the joint motor in the rotary subsystem, J rot =diag[J m1 J m2 J m3 [] represents the rotor inertia matrix of the articulated motor in the rotary subsystem, where T denotes transpose and diag[] denotes a diagonal matrix. and q rot The second and first derivatives, For I rot The first differential, M rot Let C be the inertia matrix of the rotating subsystem. rot Let be the centrifugal force and Coriolis force matrix of the rotating subsystem.

[0116] (3) Based on the rotary subsystem, its controller is designed as a backstepping controller based on cross-coupling compensation to achieve pose control of the end effector in the horizontal direction. The backstepping controller uses the desired joint trajectories of the three rotary joints compensated by the cross-coupling compensator as input signals, and the position, speed, and current measured by the joint motor sensors of the three rotary joints as feedback signals. It adjusts the duty cycle of the PWM wave of the joint motors to ensure that the actual pose of the end effector in the horizontal direction follows the desired pose. The cross-coupling compensator is used to estimate the contour error of the end effector in the horizontal direction and compensate for the desired joint trajectories of the three rotary joints. The control law of the backstepping controller of the rotary subsystem is as follows:

[0117]

[0118] Among them: U rot , e rot1 ,e rot2 ,e rot3 As an intermediate variable, They are respectively The first-order differential, Let be the column vector of the desired joint trajectories of the rotating subsystem. These are the expected joint trajectories of the three rotational joints of the SCARA, K. rot1 ,K rot2 ,K rot3 These are the backstepping controller parameters for the rotating subsystem.

[0119] The expression for estimating the profile error of the cross-coupled compensator is as follows:

[0120]

[0121] Where: ε is the contour error of the end effector's horizontal pose, x * ,y * The desired position of the end effector in the horizontal direction. The desired orientation of the end effector in the horizontal direction. x * ,y * The first differential, ε x ,ε y , For the end effector in x, y, θ z The corresponding contour error, x c ,y c Let R be the center of the circle of curvature of the desired trajectory of the end effector in the horizontal direction, R be the radius of the circle of curvature of the desired trajectory of the end effector in the horizontal direction, and κ be the curvature of the desired trajectory of the end effector in the horizontal direction. ce is the contour error estimation constant. v It is an intermediate variable.

[0122] The compensation law of the cross-coupled compensator is as follows:

[0123]

[0124] in: Let Δq be the column vector of the expected joint trajectories of the compensated rotary subsystem. rot This represents the expected joint trajectory compensation amount based on the contour error. Let C be the desired horizontal pose column vector of the end effector. ε This is the compensation gain constant.

[0125] (4) Based on the motion subsystem, its controller is designed as a backstepping controller to achieve vertical position control of the end effector. The backstepping controller of the motion subsystem uses the desired joint trajectory of the motion joint as the input signal and the position, speed, and current measured by the joint motor sensor of the motion joint as the feedback signal. It adjusts the duty cycle of the PWM wave of the joint motor so that the actual position of the end effector in the vertical direction follows the desired position. The control law of the backstepping controller of the motion subsystem is as follows:

[0126]

[0127] Where: u4, e mov1 ,e mov2 ,e mov3 As an intermediate variable, They are respectively The first-order differential, For the desired joint trajectory of the moving joint, K mov1 ,K mov2 ,K mov3 These are the backstepping controller parameters for the mobile subsystem.

[0128] After designing the rotation subsystem controller and the motion subsystem controller respectively, the block cooperative control structure for trajectory tracking of a four-DOF SCARA robotic arm is as follows: Figure 4 As shown, the desired pose trajectory of the input end effector is processed by inverse kinematics and outputs the desired joint trajectory. The rotary subsystem backstepping controller uses the desired joint trajectory of the rotary subsystem after compensation by the cross-coupling compensator as a reference and the position, speed, and current of the joint motor of the rotary subsystem as feedback. The kinetic subsystem backstepping controller uses the desired joint trajectory of the kinetic subsystem as a reference and the position, speed, and current of the joint motor of the kinetic subsystem as feedback, thereby realizing trajectory tracking control of the end effector.

[0129] Verification Example

[0130] To verify the effectiveness of the control strategy of this invention, this embodiment uses traditional three-loop PID control as a comparison method, such as... Figure 5 As shown, each joint motor in the three-loop PID control is independently controlled by a cascade closed-loop controller that includes a position loop, a speed loop, and a current loop.

[0131] The relevant parameters of the joint motors are shown in Table 1 (the parameters of each joint drive motor are the same except for the reduction ratio), the relevant parameters of the SCARA robotic arm are shown in Table 2, the parameters of the three-loop PID controller (the three-loop PID parameters of each motor are the same) are shown in Table 3, and the parameters of the block collaborative controller are shown in Table 4; the simulation step size is set to 0.00001s, the solver is ode45, and the allowable relative error is 0.0001.

[0132] Table 1

[0133] Rated excitation voltage (V) <![CDATA[U fN ]]> 48 Armature resistance (Ω) R 1.8 Armature inductance (H) L 0.0007 Magnetizing resistance (Ω) <![CDATA[R f ]]> 240 Magnetizing inductance (H) <![CDATA[L f ]]> 120 Magnet armature mutual inductance (H) <![CDATA[M af ]]> 0.4 <![CDATA[Rotational inertia of rotor (kg·m 2 )]]> J 0.0001 Rotor radius (m) r 0.003 Potential constant (V / rpm) <![CDATA[C e f]]> 0.0084 Torque constant (Nm / A) <![CDATA[C t f]]> 0.08 Reduction ratio c diag[50 50 50 5] PWM period (s) <![CDATA[T pwm ]]> 0.0001 Sensor sampling period (s) <![CDATA[T s ]]> 0.001 Low-pass filter time constant (s) <![CDATA[T LPF ]]> 0.001

[0134] Table 2

[0135]

[0136]

[0137] Table 3

[0138] Position loop I parameters <![CDATA[K LocI ]]> 5000 Position loop D parameters <![CDATA[K LocD ]]> 0 Velocity loop P parameters <![CDATA[K SpdP ]]> 0.13 Velocity loop I parameters <![CDATA[K SpdI ]]> 65 Velocity loop D parameters <![CDATA[K SpdD ]]> 0 Current loop P parameters <![CDATA[K CurP ]]> 1.75 Current loop I parameters <![CDATA[K CurI ]]> 4500 Current loop D parameters <![CDATA[K CurD ]]> 0

[0139] Table 4

[0140]

[0141] Figures 6(a) to 6(i) Experimental waveforms of conventional three-loop PID control and the block-based cooperative control of this invention are presented under the given condition of continuously changing desired trajectory with position, velocity, and acceleration. The comparison of root mean square errors in various directions of the end effector pose shows that the trajectory tracking accuracy of this invention's strategy is significantly better than that of conventional three-loop PID control. The tracking error of this invention's strategy is suppressed to a smaller range throughout the entire motion process, and its position and attitude trajectory are closer to the desired trajectory.

[0142] From the horizontal position XY and attitude θ zFrom a technical perspective, traditional three-loop PID controllers only control the individual joint motors. However, the rotational joints are coupled to each other, causing interference during movement and affecting the tracking performance of the end effector. The cross-coupling compensator in this invention compensates for the desired trajectory of the three rotational joints based on the actual horizontal pose of the end effector, suppressing the effects of coupling between the three joints, achieving closed-loop control, and thus improving the tracking performance of the end effector.

[0143] From the vertical Z-angle, traditional three-loop PID controllers only determine the PWM duty cycle of the joint motor based on the position, speed, and current errors fed back by the joint motor, without considering the impact of the nonlinear characteristics of the robotic arm and the motor. In contrast, the backstepping controller employed in this invention designs a stable control law based on the system model, reducing the impact of the motor's nonlinear characteristics.

[0144] The above description of the embodiments is provided to enable those skilled in the art to understand and apply the present invention. Those skilled in the art can readily make various modifications to the above embodiments and apply the general principles described herein to other embodiments without creative effort. Therefore, the present invention is not limited to the above embodiments, and any improvements and modifications made to the present invention by those skilled in the art based on the disclosure thereof should be within the scope of protection of the present invention.

Claims

1. A block cooperative control method for a multi-degree-of-freedom SCARA robot arm, characterized by, Includes the following steps: (1) Establish the kinematic and dynamic models of SCARA, and combine them with the electrical and mechanical models of the joint motors to establish an integrated model of the robotic arm system; (2) Based on the integrated model of the robotic arm system, SCARA is decoupled into a rotational subsystem model and a movement subsystem model; (3) Based on the rotating subsystem model, the controller of the rotating subsystem is designed as a backstepping controller based on cross-coupling compensation to realize the position control and attitude control of the SCARA end effector in the horizontal direction. The controller of the rotary subsystem includes a backstepping controller and a cross-coupling compensator. The backstepping controller uses the desired joint trajectories of the three rotary joints compensated by the cross-coupling compensator as input signals based on the backstepping method, and uses the position, speed, and current measured by the joint motor sensors of the three rotary joints as feedback signals to adjust the duty cycle of the PWM wave of the corresponding joint motors, so that the actual pose of the end effector in the horizontal direction follows the desired pose. The control law expression of the backstepping controller is as follows: in: As an intermediate variable, They are respectively The first-order differential, Let be the column vector of the desired joint trajectories of the rotating subsystem. These are the expected joint trajectories of the three rotational joints of SCARA. For the backstepping controller parameters of the rotating subsystem, This is the column vector of the PWM wave duty cycle of the joint motor in the rotary subsystem. This is the rated voltage matrix for the joint motors in the rotary subsystem. Here is the armature inductance matrix of the joint motor in the rotary subsystem. This is the armature resistance matrix of the joint motor in the rotary subsystem. Let be the column vector of armature currents of the joint motor in the rotary subsystem. Let be the potential constant matrix of the joint motor in the rotary subsystem. Let be the column vector of rotation angles of the rotary joints in the rotary subsystem. for The first-order differential, This is the reduction ratio matrix of the articulated motor reducer in the rotary subsystem. This is the torque constant matrix of the joint motor in the rotary subsystem. Let be the inertia matrix of the rotating subsystem. Here are the centrifugal and Coriolis force matrices of the rotating subsystem. Let be the rotor inertia matrix of the articulated motor in the rotary subsystem. T Indicates transpose; The cross-coupling compensator is used to estimate the profile error of the end effector in the horizontal direction and to compensate for the desired joint trajectory of the three rotary joints by the following expression. in: The contour error represents the horizontal pose of the end effector. The desired position of the end effector in the horizontal direction. The desired orientation of the end effector in the horizontal direction. They are respectively The first-order differential, For the end effector in The corresponding contour error, Let the center of the circle of curvature be the desired trajectory of the end effector in the horizontal direction. Let be the radius of the curvature circle of the desired trajectory for the horizontal position of the end effector. Let be the desired trajectory curvature of the end effector in the horizontal direction. Let be the contour error estimation constant. As an intermediate variable, The attitude of the SCARA end effector in the horizontal direction. These represent the three-dimensional spatial positions of the SCARA end effector; The compensation law expression for the cross-coupled compensator is as follows: in: The column vector of the expected joint trajectories of the compensated rotary subsystem. This represents the expected joint trajectory compensation amount based on the contour error. Let be the column vector of the desired horizontal pose of the end effector. To compensate for the gain constant, These are the first three terms of the SCARA inverse kinematics equations; (4) Based on the mobile subsystem model, the controller of the mobile subsystem is designed as a backstepping controller to realize the position control of the SCARA end effector in the vertical direction.

2. The block cooperative control method for a multi-degree-of-freedom SCARA robotic arm according to claim 1, characterized in that: The kinematic model expression for SCARA in step (1) is as follows: in: These represent the rotation angles of the three rotary joints of the SCARA. The distance the SCARA joint moves. These are the lengths of the two rotating arms of the SCARA. This represents the initial height of the SCARA end effector.

3. The block cooperative control method for a multi-degree-of-freedom SCARA robotic arm according to claim 2, characterized in that: The dynamic model expression of SCARA in step (1) is as follows: in: They are respectively The first-order differential, They are respectively The second derivative, These are the driving torques of the three rotary joints of the SCARA. The driving force for the SCARA moving joints, The masses of the two rotating arms of the SCARA are respectively. The total mass of the SCARA mobile arm and the load. These are the positions of the centroids of the two rotating arms of the SCARA. These are the moments of inertia of the two rotating arms of the SCARA. The total moment of inertia of the SCARA mobile arm and load. It is the acceleration due to gravity. It is an intermediate variable.

4. The block cooperative control method for a multi-degree-of-freedom SCARA robotic arm according to claim 3, characterized in that: The electrical and mechanical model expressions of the joint motor in step (1) are as follows: in: For the first Input voltage of each joint motor For the first The duty cycle of the PWM wave of each joint motor. For the first The rated voltage of the joint motor, For the first Armature current of a single-joint motor for The first-order differential, For the first Armature resistance of a single-joint motor For the first The armature inductance of a single-joint motor For the first Rotor moment of inertia of a single-joint motor For the first Electromagnetic torque of a single-joint motor For the first The rotor radius of a single-joint motor. For the first Rotor load torque of a single-joint motor For the first The rotor tangential load force of a single-joint motor For the first The potential constant of a single-joint motor For the first Torque constant of a single-joint motor For the first The rotor angular velocity of a single-joint motor for The second derivative, For the first The travel distance of each point on the rotor circumference of the joint motor. For the first The reduction ratio of the joint motor reducer. For the first The rotation angle of each joint motor For the first The driving torque of the joint motor.

5. The block cooperative control method for a multi-degree-of-freedom SCARA robotic arm according to claim 4, characterized in that: In step (1), the kinematic and dynamic models of SCARA are substituted into the electrical and mechanical models of the joint motor, and redundant variables are replaced to obtain the integrated model expression of the robotic arm system as follows: 。 6. The block cooperative control method for a multi-degree-of-freedom SCARA robotic arm according to claim 5, characterized in that: In step (2), the three SCARA rotary joints are divided into a rotary subsystem, which is responsible for the pose motion of the end effector in the horizontal direction; the SCARA translator joint is divided into a translator subsystem, which is responsible for the position motion of the end effector in the vertical direction. The decoupled rotary subsystem model expression is as follows: The decoupled mobile subsystem model expression is as follows: in: These are the first three terms of the SCARA forward kinematic equations. , , , , , , , , , , Represents a diagonal matrix. for The second derivative, for The first differential.

7. The block cooperative control method for a multi-degree-of-freedom SCARA robotic arm according to claim 6, characterized in that: In step (4), the controller of the moving subsystem adopts a backstepping controller. This backstepping controller is based on the backstepping method, using the desired joint trajectory of the moving joint as the input signal and the position, speed, and current measured by the joint motor sensor of the moving joint as the feedback signal. It adjusts the duty cycle of the PWM wave of the corresponding joint motor so that the actual position of the end effector in the vertical direction follows the desired position. The control law expression of the backstepping controller is as follows: in: As an intermediate variable, They are respectively The first-order differential, The desired joint trajectory for the moving joint. These are the backstepping controller parameters for the mobile subsystem.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: The processor is used to execute the computer program to implement the block cooperative control method for a multi-degree-of-freedom SCARA robotic arm as described in any one of claims 1 to 7.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by the processor, it implements the block cooperative control method for a multi-degree-of-freedom SCARA robotic arm as described in any one of claims 1 to 7.

Images