Construction method of force sensor-free controller of robot mechanical arm system

Abstract

The invention discloses a construction method of a robot mechanical arm system force-sensor-free controller, and the method comprises the steps: achieving the observation of a driving torque and an external torque through a sensor-free control system according to a given position vector matrix and an observed position vector matrix; a gating circulation unit, an SVPWM module, an inverter, an electric sensor, a 3s / 2s module and a signal processing module jointly form a torque control system, and closed-loop control of torque is achieved. A torque control system drives a PMSM through a given torque and outputs current to a sensorless module, and the self-adaptive super-spiral sliding-mode observer and a robot dynamic model jointly form the sensorless module; the position of a motor rotor is observed through input current, an observed position vector matrix is further obtained, a torque control system, a PMSM and a sensorless module jointly form a composite controlled object, and a sensorless control system is used for achieving high-precision closed-loop control over the position vector matrix of the composite controlled object.

Description

Technical Field

[0001] The present invention belongs to the field of robot motion control, and more specifically, relates to a method for constructing a controller of a flexible joint of a robot. Background Art

[0002] Currently, the application fields of robots are constantly expanding, covering multiple fields such as manufacturing, healthcare, and logistics. The joint motors of collaborative robots are their driving cores, and sensors are usually required to achieve precise control. However, the installation of sensors will have an adverse impact on the spatial safety and system cost of the robots. Therefore, the research on sensorless control of collaborative robot joint motors is crucial for improving the safety and economy of robots.

[0003] The application of permanent magnet synchronous motors in joint motors is of great significance. Their characteristics of high efficiency, high precision, and high response make them an ideal choice in the field of collaborative robots. Through closed-loop control, precise control of joint positions can be achieved, improving the positioning accuracy and stability of the robots, which is crucial for performing precise tasks and achieving collaborative work, helping to improve the performance and safety of the robots, and promoting the development of collaborative robot technology.

[0004] The document with Chinese Patent Publication No. CN113987961A provides a method for detecting the landing of a robot jump based on a torque sensorless, constructing a second-order observer to observe the external torque of the robot and using a neural network to train the observed data to achieve the judgment of the landing state of the robot. The document with Chinese Patent Publication No. CN114800620A provides a method for detecting external forces based on force sensors, constructing a second-order observer to observe the external disturbance forces of the robot and constructing a nonlinear friction system to achieve the detection of the external forces of the robot. In the observers designed by the above two patent methods, only external variables are used as the observation objects and detected, which cannot meet the high-precision control requirements of the robots, have poor robustness, and do not specifically elaborate on the specific control methods combined with the drive system. Summary of the Invention

[0005] The purpose of the present invention is to provide a method for constructing a sensorless controller for a robot manipulator system, which can real-time observe the torque information caused by the vector position, driving force, and external disturbance of the manipulator by only using electrical signal sensors, and adopt "position-torque" double closed-loop control, with excellent static and dynamic characteristics, strong robustness, and broad application prospects in engineering practice.

[0006] The technical solution adopted by the method for constructing a sensorless torque controller for a robot joint motor of the present invention includes the following steps:

[0007] Step 1): Input the difference between the given position vector matrix and the observed position vector matrix into the inverse kinematic model of the robot, and then output four matrix signals: the inertia matrix, the Coriolis matrix, the gravity matrix, and the friction matrix. The four matrix signals are input into the new extended state observer to output the observed momentum and the external torque The first signal processing module calculates the torque based on the observed momentum and the external torque The inverse kinematic model of the robot, the new extended state observer, and the first signal processing module constitute a sensorless control system with the output being the given torque ;

[0008] Step 2): Input the difference between the given torque τ * and the estimated torque τ b and the difference between the given magnetic flux |ψ s | * and the estimated magnetic flux |ψ b | into the gated recurrent unit. The gated recurrent unit outputs the voltage u α , u β , the voltage u α , u β The voltage u α , u β After being output by SVPWM, the switching signals are sent to the inverter; the electrical sensor detects the three-phase current and voltage and inputs them into the 3s / 2s module to obtain the two-phase current i α , i β and the voltage u α , u β The current i α , i β and the voltage u α , u β After passing through the second signal processing module, the estimated torque τ b and the estimated magnetic flux |ψ b | are obtained; the gated recurrent unit, the SVPWM module, the inverter, the electrical sensor, the 3s / 2s module, and the second signal processing module together constitute a torque control system;

[0009] Step 3): Input the current difference obtained by subtracting the observed current from the currents i α , i β along with the gain coefficients μ1 and μ2 into a signal processing module, and then output the extended back electromotive force. The extended back electromotive force passes through the position calculation module to obtain the angle θ, and the angle θ passes through the rotational speed calculation module to output the electrical angular velocity ω e , Input the electrical angular velocity ω e into the adaptation rate module to obtain the gain coefficients μ1 and μ2. Input the voltage u α , u β , the electrical angular velocity ωe The gain coefficients μ1 and μ2 are input into the super-twisting sliding mode observer to output the observed current The adaptive super-twisting sliding mode observer is composed of a super-twisting sliding mode observer, a third signal processing module, a position calculation module, a rotational speed calculation module, and an adaptive law module, and its output is the angle θ;

[0010] Step 4): The sensorless module is composed of the adaptive super-twisting sliding mode observer and the robot dynamics model, and the robot dynamics model outputs the position vector matrix The composite controlled object is composed of a torque control system, a PMSM, and the sensorless module.

[0011] Furthermore, the four matrices are the inertia matrix the Coriolis matrix the gravity matrix G(θ) and the friction matrix The first signal processing module 13 obtains the torque according to the formula obtaining the torque θ is the angle of the robot manipulator, is the first derivative of θ, is the first derivative of, and T is the matrix transpose.

[0012] Furthermore, the observed momentum is the first observed variable x1, and the external torque is the second observed variable x2, τ d is the desired torque, is the first derivative of x1, is the first derivative of x2, T is the matrix transpose, and the intermediate variable

[0013] Furthermore, the torque is the first derivative of the observed momentum of.

[0014] Furthermore, the input-output relationship of the gated recurrent unit is:

[0015]

[0016] is the input, X t =[τ * -τ b |ψ s | * -|ψ b |], H t =[u α u β , is the output, t is the current moment, and H t-1 represents the running output of the previous moment; R t ∈(0, 1) is the reset gate, Z t ∈(0, 1) is the update gate, is the candidate hidden state, ⊙ represents the element-wise multiplication operation of vectors, σ(x) is the sigmoid function, and tanh is the hyperbolic tangent function; W r , U r , B r , W z , U z , B z , W, U, B are learnable and adjustable parameters, n takes 1, and h takes 2.

[0017] Furthermore, the second signal processing module obtains the estimated torque τ b = 1.5p n (i β ∫(u α -R s i α )dt - i α ∫(u β -R s i β )dt) and b and obtains the estimated magnetic flux |ψ b |, where R s is the stator resistance and p n is the number of pole pairs of the motor used for the robotic arm.

[0018] The beneficial effects of the present invention are:

[0019] 1. The construction method proposed by the present invention collects electrical signals through built-in electrical signal sensors, obtains the position information of the robotic arm of the robot via an observer and outputs it, realizing the position closed-loop control of the target; through the built-in electrical signals, after being processed, the gated recurrent unit outputs control electrical signals, realizing the torque closed-loop control of the target. Further, the two control systems are nested to form a "position-torque" double closed-loop control system; the controller constructed based on this method enables the entire system to not only have good control accuracy, but also have a lower cost and a larger redundant design and operation space.

[0020] 2. The present invention constructs a new extended state observer to observe its own driving torque and external torque through variables related to the motor position vector; constructs a gated recurrent unit to convert torque and flux linkage information into voltage signals; constructs a sensorless module to convert electrical signals into joint position signals; and finally constructs a double closed-loop to control the drive motor, so that the external conditions and costs required for the control of the robot joint are lower and it has excellent steady-state characteristics.

[0021] 3. The present invention can achieve high-precision control without relying on additional force sensors and position sensors in the system, greatly reducing the complexity of the system and saving the space and cost of the entire control system. In the increasingly diverse and complex environments of collaborative robot applications, it can reduce the cost of the entire control system and improve economic benefits; in addition, due to the reduction in the number of sensors, more operating space is also given to the robotic arm. Therefore, the constructed controller can well meet the requirements of economy, safety, convenience, etc. in practical applications and has feasibility and practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 is the structural block diagram of the sensorless control system in the present invention;

[0023] Figure 2 is the structural block diagram of the composite controlled object in the present invention;

[0024] Figure 3 is the structural block diagram of the adaptive super-twisting sliding mode observer in the present invention;

[0025] Figure 4 is the overall structural block diagram of the present invention.

[0026] In the figure: 1. Sensorless control system; 2. Composite controlled object; 11. Robot inverse kinematic model; 12. New extended state observer; 13. First signal processing module; 21. Gated recurrent unit; 22. SVPWM module; 23. Inverter; 24. Electrical sensor; 25. Second signal processing module; 26. PMSM; 27. Adaptive super-twisting sliding mode observer; 28. Robot dynamics model; 31. Super-twisting sliding mode observer; 32. Third signal processing module; 33. Position calculation module; 34. Adaptive law module; 35. Rotational speed calculation module. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0027] The present invention will be further described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0028] 1. Construct a sensorless control system. As Figure 1As shown in the figure, the sensorless control system consists of a robot inverse kinematic model 11, a novel extended state observer 12, and a first signal processing module 13.

[0029] According to the target position given by the joints of the robot manipulator system, the given target position is expressed as a given position vector matrix Q in the form of a vector matrix. * The given position vector matrix Q * is subtracted from the observed position vector matrix of the robot manipulator The difference is input into the robot inverse kinematic model 11; after being processed by the robot inverse kinematic model 11, the difference matrix signal is converted into four matrix signals and output. Four matrix signals are obtained according to the theory of the conventional robot inverse kinematic model. The first signal is the inertia matrix The second output signal is the Coriolis matrix The third output signal is the gravity matrix G(θ), and the fourth output signal is the friction matrix where θ is the joint angle position signal of the robot manipulator, and is the first derivative of the angle θ.

[0030] The novel extended state observer 12 outputs the observed momentum and the external torque The input-output relationship is expressed as follows:

[0031]

[0032] Taking the inertia matrix as the actual momentum P, is the difference between the actual momentum P and the observed momentum ; γ1 and γ2 are error parameters, and their values need to be determined according to the specific system. x1 is the first observed variable, is the first derivative of x1, x2 is the second observed variable, is the first derivative of x2, and τ p is an intermediate variable, which is determined by the following formula (4).

[0033] In the novel extended state observer 12, the observed momentum is selected as the first observed variable x1 and is used as the output signal; the external torque is selected as the second observed variable x2 and is used as the output signal.

[0034] Under ideal conditions, the dynamic equation of the robot manipulator is expressed as follows:

[0035]

[0036] τ d is the desired torque, τ e is the ideal external torque, is the second derivative of the angle θ;

[0037] Furthermore, according to the generalized momentum theorem, the differential equation of momentum P can be expressed as follows:

[0038]

[0039] where, is the second derivative of P, τ p is an intermediate variable, and its expression is:

[0040]

[0041] According to formulas (1)-(4), the input-output relationship of the first signal processing module 13 can be expressed as follows:

[0042]

[0043] where, is the observed momentum is the first derivative of, T is the matrix transpose, and the torque is used as the output signal of the first signal processing module 13.

[0044] Subtract the torque signal output by the first signal processing module 13 from the external torque signal output by the new extended state observer 12 to obtain the given torque signal expressed as follows:

[0045]

[0046] Use the given torque τ * as the output signal of the sensorless control system.

[0047] The robot inverse dynamics model 11, the new extended state observer 12, and the signal processing module 13 together constitute a sensorless control system.

[0048] 2. Construct a torque control system. As Figure 2 shown, connect the gated recurrent unit 21, the SVPWM module 22 (i.e., SVPWM22), the inverter 23, the electrical sensor 24, the 3s / 2s module, and the second signal processing module 25 in sequence to jointly form a torque control system. The electrical sensor 24 is connected to the PMSM 26 (robot manipulator 26) to detect the three-phase current signal i abcand the voltage signal u abc ..

[0049] The given torque τ output by the sensorless control system * and the estimated torque τ fed back by the second signal processing module 25 b The difference τ * -τ b and the given flux linkage magnitude |ψ s | * and the estimated flux linkage magnitude |ψ fed back by the second signal processing module 25 b | The difference |ψ s | * -|ψ b | are jointly used as input signals and input into the gated recurrent unit 21. The gated recurrent unit 21 constructs a gated recurrent unit according to the two input difference signals. The input-output relationship of the gated recurrent unit 21 can be expressed as follows:

[0050]

[0051] Among them, is the input, is the output, t is the current moment, and H t-1 represents the operating output of the previous moment; R t ∈(0, 1) is the reset gate, Z t ∈(0, 1) is the update gate, is the candidate hidden state, ⊙ represents the vector element multiplication operation, σ(x) is the sigmoid function, and tanh is the hyperbolic tangent function; W r ,U r ,B r ,W z ,U z ,B z ,W, U, B are learnable and adjustable parameters. In the present invention, n takes 1 and h takes 2. The expressions of X t and H t are:

[0052] X t =[τ * -τ b |ψ s | * -|ψ b |] (8)

[0053] H t =[u α u β (9)

[0054] In the gated recurrent unit 21, the voltage signal u α is used as the first output signal, and uβ As the second output signal; input the output signal of the gated recurrent unit 21 into the SVPWM module 22.

[0055] The SVPWM module 22 is based on the voltage u α , u β signal and outputs six-way switching signals S 1-6 to the inverter 23, and the inverter 23 outputs three-phase drive electrical signals to drive the robotic arm 26.

[0056] The electrical sensor 24 detects the three-phase drive electrical signals output by the inverter 23. Its first output signal is the three-phase current i abc , and the second output signal is the three-phase voltage u abc . Input the three-phase drive electrical signals output by the inverter 23 into the PMSM 26, and the PMSM 26 obtains the actual torque τ.

[0057] The three-phase current i abc and the three-phase voltage u abc output by the electrical sensor 24 are input into the 3s / 2s module for coordinate transformation to obtain the two-phase current i α , i β and the two-phase voltage u α , u β . The two-phase current i α , i β and the two-phase voltage u α , u β are input into the second signal processing module 25 to obtain the estimated torque τ b and the estimated magnitude of the magnetic flux |ψ b |, and their input-output relationship can be expressed as follows:

[0058]

[0059] where R s is the stator resistance, p n is the number of pole pairs of the motor for the robotic arm, |ψ b | is the magnitude of the estimated magnetic flux, and τ b is the estimated torque.

[0060] 3. Construct an adaptive super-twisting sliding mode observer. Refer to Figure 2 , construct an adaptive super-twisting sliding mode observer 27 according to the two-phase current i α , i β and the two-phase voltage u α , u β signals output by the 3s / 2s module. The 3s / 2s module in the torque control system is connected to the adaptive super-twisting sliding mode observer 27. Refer to Figure 3The adaptive super-helical sliding mode observer 27 is composed of a super-helical sliding mode observer 31 , a third signal processing module 32 , a position calculation module 33 , a speed calculation module 35 and an adaptive law module 34 .

[0061] The current i α ,i β and the observed current output by the super-helical sliding mode observer 31 After making a difference The gain coefficients μ1 and μ2 output by the adaptive law module 34 are input to the third signal processing module 32. The output signal of the third signal processing module 32 is the expanded back electromotive force. Its expression is:

[0062]

[0063] in μ1 and μ2 are gain coefficients, which can be calculated by the adaptive rate module 34. The observed current signal Calculated by the super-helical sliding mode observer 31.

[0064] Will expand the back EMF The position is input to the position calculation module 33, and a position signal with an angle of θ is obtained as an output signal. The angle θ is also the output signal of the adaptive super-helical sliding mode observer 27, and its expression is:

[0065]

[0066] The input of the speed calculation module 35 is the angle θ position signal output by the position calculation module 33, and the output signal of the speed calculation module 35 is the electrical angular velocity ω e , whose expression is:

[0067]

[0068] The electrical angular velocity ω e As input signals, it is input into the adaptive rate module 34 to obtain gain coefficients μ1, μ2. The gain coefficients μ1, μ2 are respectively input into the signal processing module 32 and the super spiral sliding mode observer 31 as input signals. The expression of the gain coefficient is:

[0069]

[0070] Where λ1, λ2 are adaptive coefficients, g(ω e ) is a function related to the electrical angular velocity, that is, the state equation of various motors. For example, the permanent magnet motor is the dq voltage equation, and other types of motors are the three-phase voltage equation.

[0071] The voltage u α, u β , the electrical angular velocity ω e and the gain coefficients μ1, μ2 are input into the super-twisting sliding mode observer 31 as input signals, and the super-twisting sliding mode observer 31 outputs the observed current The relationship between the input and output signals is:

[0072]

[0073] where the matrix A is expressed as:

[0074]

[0075] where, L d and L q are the components of the inductor on the d-q axes.

[0076] 4. Construct a sensorless module. The sensorless module is composed of an adaptive super-twisting sliding mode observer 27 and a robot dynamics model 28 connected together. The angle θ output by the adaptive super-twisting sliding mode observer 27 is input into the robot dynamics model 28, and according to the conventional robot dynamics model 28, the output is the observed position vector matrix

[0077] 5. As Figure 2 shown, a composite controlled object is jointly composed of a torque control system, a PMSM 26, and a sensorless module.

[0078] 6. As Figure 4 shown, the observed position vector matrix output by the composite controlled object 2 is fed back to the sensorless control system 1, and the sensorless control system 1 is connected in series before the composite controlled object 2 to form a sensorless controller to realize the control of the PMSM 26.

[0079] The constructed sensorless controller realizes the observation of the driving torque * and the external torque through the sensorless control system 1 from the given position vector matrix Q and the observed position vector matrix According to the observed driving torque and the external torque the required given torque τ * is obtained. The gated recurrent unit 21, the SVPWM module 22, the inverter 23, the electrical sensor 24, the 3s / 2s module, and the second signal processing module 25 jointly form a torque control system to realize the closed-loop control of the torque; through the torque control system, the given torque τ *Realize the drive of PMSM26 and output the current signal to the sensorless module. The adaptive super-twisting sliding mode observer 27 and the robot dynamics model 28 together constitute the sensorless module; the motor rotor position is observed through the input current signal and the observed position vector matrix is further obtained. The torque control system, PMSM26 and the sensorless module together constitute the composite controlled object 2, and the sensorless control system 1 is used to realize the closed-loop control of the position vector matrix for the composite controlled object 2.

[0080] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for constructing a powerless sensor controller for a robot manipulator system, characterized in that The following steps are involved: Step 1): Compare the given position vector matrix with the observed position vector matrix The difference is input into the robot inverse kinematics model and then outputs four matrix signals: inertia matrix, Coriolis matrix, gravity matrix and friction matrix. The four matrix signals are outputted through the new extended state observer to observe the momentum. and external torque The first signal processing module is based on the observed momentum and external torque Calculate the torque The output is a given torque. Sensorless control system; Step 2): Given the torque τ * and the estimated moment τ b The difference and the given flux |ψ s | * and the estimated flux |ψ b The difference between | is input into the gated cyclic unit, and the gated cyclic unit outputs a voltage u α ,u β , voltage u α ,u β The SVPWM outputs the switching signal to the inverter; the electrical sensor detects the three-phase current and voltage and inputs them into the 3s / 2s module to obtain the two-phase current i α ,i β and voltage u α ,u β , current i α ,i β and voltage u α ,u β The estimated torque τ is obtained through the second signal processing module b and estimate the flux linkage |ψ b |; The torque control system is composed of a gate control cycle unit, an SVPWM module, an inverter, an electric sensor, a 3s / 2s module and a second signal processing module; Step 3): Set the current i α ,i β and observe the current The current difference and gain coefficients μ1 and μ2 after the difference are input into the signal processing module to output the expanded back electromotive force. The expanded back electromotive force is passed through the position calculation module to obtain the angle θ, and the angle θ is passed through the speed calculation module to output the electrical angular velocity ω. e , the electrical angular velocity ω e Input to the adaptive rate module to obtain gain coefficients μ1, μ2, and convert the voltage u α ,u β , electrical angular velocity ω e And gain coefficients μ1, μ2 are input to the super spiral sliding mode observer to output the observed current The adaptive super-helical sliding mode observer is composed of the super-helical sliding mode observer, the third signal processing module, the position calculation module, the speed calculation module and the adaptive law module, and its output is the angle θ; Step 4): The position sensorless module is composed of the adaptive super-helical sliding mode observer and the robot dynamics model, and the robot dynamics model outputs the position vector matrix The composite controlled object is composed of the torque control system, PMSM and position sensorless module.

2. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 1, characterized in that: The four matrices are the inertia matrix Coriolis Matrix Gravity matrix G(θ) and friction matrix The first signal processing module 13 is based on the formula Get torque θ is the angle of the robot arm, is the first-order derivative of θ, for The first-order derivative of , T is the matrix transpose.

3. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 2, characterized in that: Observing Momentum is the first observed variable x1, the external moment is the second observed variable x2, τ d is the expected torque, is the first-order derivative of x1, is the first-order derivative of x2, T is the matrix transpose, and the intermediate variable 4. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 3, characterized in that: Torque To observe momentum The first derivative of .

5. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 1, characterized in that: The input-output relationship of the gated recurrent unit is: is the input, X t =[τ * -τ b |ψ s | * -|ψ b |],H t =[u α u β ], is the output, t is the current time, H t-1 Indicates the running output at the last moment; R t ∈(0, 1) is the reset gate, Z t ∈(0, 1) is the update gate, is a candidate hidden state, ⊙ represents the vector element multiplication operation, σ(x) is the sigmoid function, and tanh is the hyperbolic tangent function; W r , U r , B r , W z , U z , B z , W, U, B are learnable and adjustable parameters, n is 1, and h is 2.

6. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 1, characterized in that: The second signal processing module is based on the formula τ b =1.5p n (iβ∫(uα-R s iα)dt-iα∫(uβ-R s iβ)dt) to get the estimated torque τ b and Get the estimated magnetic flux |ψ b |, R s is the stator resistance, p n is the number of pole pairs of the motor used in the robot arm.

7. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 1, characterized in that: Expanding Back EMF for:

8. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 7, characterized in that: angle 9. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 1, characterized in that: Gain coefficients μ1, μ2 are λ1, λ2 are adaptive coefficients in the range of 0-10, g(ω e ) is a function related to the electrical angular velocity.

10. The method for constructing a powerless sensor controller for a robot manipulator system according to claim 1, characterized in that: Observed current output by the spiral sliding mode observer for: matrix L d and L q is the component of the inductance on the dq axis.

Images