Construction method of joint system composite controller of collaborative robot

Abstract

The invention discloses a construction method of a composite controller of a joint system of a collaborative robot. An initial value PI control module and an extreme value optimization self-adaptive PI control module form a speed controller, wherein the input of the speed controller is actual rotating speed, rotating speed error and dq-axis current, and the output of the speed controller is control voltage; a position sensorless control module composed of a sliding mode observer and an arc tangent function is constructed, and a position controller is composed of a nonlinear tracking differentiator, a nonlinear state error feedback controller, a neural network compensator, two adjustable gains and a three-order expansion state observer. The first-order differential tracker outputs the actual rotating speed according to the rotor position angle, and the position controller, the speed controller and the first-order differential tracker jointly form a composite controller; according to the invention, the conventional three-closed-loop control is simplified into a position-current double-closed-loop control method and is combined with a sensorless control method, so that the control system has very strong static and dynamic characteristics and robustness while the requirements on external hardware and motor parameters are reduced.

Description

Technical Field

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

[0002] With the continuous development of robotics technology, collaborative robots, as an advanced system with flexibility and intelligence, have gradually become a research hotspot. The robot's joint system, as the core component connecting the mechanical structure and the control system, plays a key role in realizing the robot's complex motion tasks. The design and optimization of the joint system controller has a great impact on the performance and application of collaborative robots.

[0003] The current conventional control method of the joint system is generally a three-closed-loop control system of "position-speed-current" based on the PI module. The use of multiple PI controllers not only makes the control system have many parameters and is difficult to adjust, but also has a great impact on the dynamics, stability and robustness of the system. In response to these problems, the document with Chinese patent publication number CN117532606A provides a joint servo system control method based on an improved ADRC controller. This method designs an extended state observer based on the linear ADRC theory, and designs a new function to observe the uncertainty of the system; however, the linear ADRC controller does not consider the inherent defects of the observer to compensate for it, resulting in control errors. The document "Gao Xusheng. Design and implementation of permanent magnet synchronous motor position control system based on ADRC [D]. Yangzhou University, 2024." constructs a "position-current" double closed-loop control system based on the ADRC controller, and controls the speed by tracking the output speed position of the differentiator and controlling the speed through the PI controller; however, this method does not consider the shortcomings of the linear ADRC controller itself, and the PI controller is not closely integrated with the motor control system. Therefore, the above two solutions cannot meet the operational requirements of collaborative robots such as high precision, high robustness, and high dynamic performance, and the controller needs to be optimized in combination with the specific control system. Summary of the invention

[0004] The purpose of the present invention is to solve the above-mentioned problems existing in the current conventional control methods of joint systems, and to provide a method for constructing a composite controller for a collaborative robot joint system. The composite controller constructed by this method is based on nonlinear self-disturbance rejection control and adaptive PI algorithm, and can meet the collaborative robot's operating requirements such as high precision, high robustness, and high dynamic performance.

[0005] To achieve the above object, the technical solution adopted by the construction method of a composite controller of a joint system of a collaborative robot of the present invention comprises the following steps:

[0006] Step 1): The initial value PI control module and the extreme value optimization adaptive PI control module are composed of inputs of actual speed. Speed ​​error e ω and dq axis current i d ,i q , the output is the control voltage u d ,u q Speed ​​controller;

[0007] The relationship between the output and input of the initial value PI control module is:

[0008]

[0009] u PI0 represents the voltage matrix, and Respectively represent two sets of initial proportional-integral coefficients, d-axis error current L d is the inductance of the d-axis, R s is the resistor;

[0010] The output and input relationship of the extreme value optimization adaptive PI control module is:

[0011]

[0012] Two sets of PI parameters and It is based on the sliding mode control law and is obtained by minimizing the stability condition according to the gradient descent method;

[0013] The control voltage

[0014] Step 2): Construct a position sensorless control module consisting of a sliding mode observer and an inverse tangent function, with the input being the voltage v of the αβ axis α ,v β and current i α ,i β , the output is the rotor position angle

[0015] Step 3): The joint motor, the position sensorless control module, the 2s / 2r conversion module, the SVPWM module, the inverter, the electric sensor, the 3s / 2s conversion module, and the 2r / 2s conversion module together constitute a composite controlled object, and the input of the composite controlled object is the control voltage u d ,u q , output rotor position angle and current i d ,i q ;

[0016] Step 4): A position controller is composed of a nonlinear tracking differentiator, a nonlinear state error feedback controller, a neural network compensator, two adjustable gains and a third-order extended state observer. The position controller is based on the reference position angle θ * and rotor position angle Output reference speed

[0017] Step 5): The first-order differential tracker is based on the rotor position angle Output actual speed Reference speed The actual speed The speed error is obtained by subtracting ω The composite controller of the collaborative robot joint system is composed of a position controller, a velocity controller and a first-order differential tracker.

[0018] Furthermore, the extreme value optimization adaptive PI control module adopts an extreme value optimization adaptive PI algorithm based on the sliding mode control law, using two sliding mode surfaces K 1 and K 2 are two positive sliding mode gain coefficients, two sets of PI parameters and The expression is:

[0019] σ is the learning rate of the gradient descent method, the superscript indicates the number of groups of PI parameters, and the subscript indicates the type of learning rate.

[0020] Furthermore, the output of the sliding mode observer is the observed extended back electromotive force and k is the sliding mode gain coefficient, and To predict current; expand back EMF and The rotor position is calculated by the inverse tangent function

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

[0022] 1. The position controller in the present invention controls the position through an optimized nonlinear anti-disturbance controller, observes two position signals and a speed signal according to the total disturbance of the system through a third-order extended state observer, and compensates the speed signal through a neural network, thereby reducing the accuracy requirements for system parameters, so that the position controller still has higher control accuracy, dynamic and static performance and anti-interference ability under external interference.

[0023] 2. The speed controller in the present invention controls the speed through an extreme value optimization composite PI controller. Through redesign, the PI initial value module can directly output the initial value of the control voltage according to the observed speed, speed error and current signal; an extreme value optimization algorithm based on sliding mode control and gradient descent method is proposed, and the parameters are corrected based on the PI initial value; the control voltage is output directly according to the reference speed, avoiding the redundant adjustment of the current loop PI parameters, and can dynamically adapt to system changes, improve stability and robustness, and provide stable and reliable operation when the motor parameters change.

[0024] 3. The present invention simplifies the conventional three-loop control into a "position-current" double loop and combines it with a sensorless control method, so that the control system has strong static and dynamic characteristics and robustness under external interference while reducing the requirements for external hardware and motor parameters. It has broad application prospects in engineering practice. The proposal of this construction method is of great significance to the research and application in the field of collaborative robot joint system control at both the academic and practical application levels. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 It is a block diagram of a composite controller of a collaborative robot joint system constructed by the method of the present invention;

[0026] Figure 2 yes Figure 1 The block diagram of the composite controlled object;

[0027] Figure 3 yes Figure 2 The block diagram of the control module without position sensor;

[0028] Figure 4 yes Figure 1 The block diagram of the position controller;

[0029] Figure 5 yes Figure 1 Block diagram of the speed controller. DETAILED DESCRIPTION

[0030] The present invention is further described in detail below in conjunction with 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:

[0031] See also Figure 1 The composite controller of the collaborative robot joint system constructed by the method of the present invention is composed of a position controller, a speed controller and a first-order differential tracker, and is connected to a composite controlled object including a joint motor and a position sensorless control module to realize the control of the composite controlled object, i.e., the collaborative robot joint system. The input of the composite controlled object is the control voltage ud , u q , output the rotor position angle and current i d , i q ; the rotor position angle is respectively input into the position controller and the first-order differential tracker. The current i of the dq axis d , i q is input into the speed controller. The position controller outputs the reference speed * according to the reference position angle θ and the rotor position angle The first-order differential tracker outputs the actual speed according to the rotor position angle The reference speed and the actual speed are subtracted to obtain the speed error The actual speed and the speed error e ω are both input into the speed controller. The speed controller obtains the control voltage u of the dq axis according to the input actual speed ω speed error e d , i q , to control the composite controlled object. Specifically as follows: d , u q , to control the composite controlled object. Specifically as follows:

[0032] See Figure 3 , construct a sensorless control module. The sensorless control module is composed of a sliding mode observer and an arctangent function. The input of the sensorless control module is the voltage v of the αβ axis α , v β and current i α , i β , and the output is the rotor position angle The voltage v of the αβ axis output by the joint motor α , v β and current i α , i β go to the sensorless control module. The joint motor outputs the voltage signal v abc and current signal i abc to the 3s / 2r transformation module. The 3s / 2r transformation module outputs the voltage v of the αβ axis α , v β and current i α , i β to the sliding mode observer to observe the extended back electromotive force. The output of the sliding mode observer is the observed extended back electromotive force and The relationship between the output and input of the sliding mode observer is expressed as follows:

[0033]

[0034] Where k is the sliding mode gain coefficient, and To predict the current; predict the current and The differential signal can be expressed as follows:

[0035]

[0036] Among them, μ α and μ β is the sliding mode control law in the stationary reference frame, S α and S β is the sliding surface in the stationary reference frame; A is the coefficient matrix related to the motor parameters, expressed as follows:

[0037]

[0038] Among them, L d is the inductance of the d-axis, L q is the q-axis inductance, R s is the resistance, ω e is the electrical angular velocity, which can be calculated from the power supply frequency f;

[0039] ω e =2πf (4)

[0040] Predicted current and It can be expressed as follows:

[0041]

[0042] Extended Back EMF and The observed rotor position angle of the joint motor is calculated by the inverse tangent function

[0043]

[0044] Combination Figure 2As shown, the joint motor, position sensorless control module, 2s / 2r conversion module, SVPWM module, inverter, electric sensor, 3s / 2s conversion module, and 2r / 2s conversion module together constitute a composite controlled object, and the joint motor is precisely controlled according to the input voltage signal. Among them, the 2s / 2r conversion module, SVPWM module, inverter, electric sensor and 3s / 2s conversion module are connected in sequence, and the output of the 3s / 2s conversion module is connected to the 2r / 2s conversion module and the position sensorless control module respectively, and the electric sensor is connected to the joint motor. The input of the 2s / 2r conversion module is the dq axis voltage u d ,u q , converted into αβ axis voltage u by 2s / 2r α ,u β , and output to the SVPWM module, the transformation relationship is:

[0045]

[0046] Voltage u α ,u β Output six switch signals through SVPWM module 1～6 , six-way switch signal s 1～6 In the input inverter, the inverter passes the DC voltage V dc Power supply to drive joint motors.

[0047] The electrical sensor collects the voltage v output by the inverter as a digital signal a , v b , v c Signal and current i a ,i b ,i c Signal, that is, v abc Signal and i abc The signal is input into the 3s / 2s conversion module, and the voltage v of the αβ axis is output after 3s / 2s conversion. α , v β and current i α ,i β , the relationship between output and input is expressed as follows:

[0048]

[0049] Current i of αβ axis α ,i β Input into the 2r / 2s conversion module, after 2r / 2s conversion the output current i d ,i q ;

[0050]

[0051] The current signal i output by the 2s / 2r conversion module d ,i q Input to Figure 1 The speed controller in the circuit is used for feedback.

[0052] The voltage v of the αβ axis α , v β and current i α ,i β Input to Figure 3 The sensorless control module in the joint outputs the rotor position angle of the joint motor after the sensorless control. The rotor position angle Feedback to the 2s / 2r conversion module and the 2r / 2s conversion module.

[0053] The sensorless control module will observe the rotor position Output to each coordinate transformation module of the composite controlled object and Figure 1 Position feedback is realized in the position controller and first-order differential tracker.

[0054] See also Figure 1 , the rotor position angle of the input first-order differential tracker and the actual output speed The relationship between can be expressed as follows:

[0055]

[0056] See also Figure 4 , construct a position controller, which consists of a nonlinear tracking differentiator, a nonlinear state error feedback controller, a neural network compensator, two adjustable gains and a third-order extended state observer. The input of the nonlinear tracking differentiator is the given reference position angle θ * , the output is two position differential signals and in Used to track the reference position angle θ * , As θ * The approximate differential of can be expressed as The relationship between the output differential signal and the input of the nonlinear tracking differentiator is as follows:

[0057]

[0058] Among them, h is the adjustable filtering factor, which is generally 0.1 to 10; r is the adjustable tracking speed factor, which is generally 0.1 to 1; the value of the adjustable factor needs to be selected according to the system control effect, and the fhan function is the fastest control comprehensive function.

[0059] Position signal and The signal can be expressed as follows:

[0060]

[0061] Construct a third-order extended state observer. The third-order extended state observer estimates the disturbances inside and outside the system in real time and compensates for these disturbances through feedback control, thereby effectively eliminating the impact caused by them in the system, making the system have strong anti-interference ability. The input of the third-order extended state observer is the speed ω 2 and the observed rotor position angle output by the composite controlled object The output is three state variables z 1 、z 2 and z 3 , where z 1 、z 2 Used to track the position signal output by the composite controlled object, z 3 The total unknown disturbance used to track the speed of the system; the relationship between the output differential signal and the input of the third-order extended state observer is expressed as follows:

[0062]

[0063] Among them, β 1 ,β 2 and β 3 are the three adjustable gain coefficients of the third-order extended state observer; α 1 ,α 2 and α 3 are adjustable parameters of the three fal functions, generally ranging from 0 to 1; δ 1 ,δ 2 and δ 3 are the three adjustable parameters of the fal function, generally 5T s ≤δ≤10T s , where T s is the sampling time of the digital controller.

[0064] The nonlinear fal function is expressed as follows:

[0065]

[0066] Among them, x is the observed state quantity; α and δ are the adjustable parameters of the fal function.

[0067] The two position differential signals output by the nonlinear tracking differentiator and The angle z between the two observations output by the third-order extended state observer 1 、z 2 The signals are subtracted to obtain two error signals. and

[0068] Construct a nonlinear state error feedback controller. Through two error signals and The disturbance suppression and reduction are achieved, so that the system can respond to the given signal more accurately and effectively resist the influence of external disturbances. The input of the nonlinear state error feedback controller is the two error signals e obtained according to the nonlinear tracking differentiator and the third-order extended state observer. 1 and e 2 ; The output of the nonlinear state error feedback controller is the speed ω 0 signal; the relationship between the output and input of the nonlinear state error feedback controller is expressed as follows:

[0069] ω 0 =λ 1 fal(e 1 ,α 4 ,δ 4 )+λ 2 fal(e 2 ,α 5 ,δ 5 ) (15)

[0070] Among them, λ 1 and λ 2 are two error gain coefficients, α 4 and α 5 , δ 4 and δ 5 is the adjustable parameter of the fal function, and its value range is the same as that of α 1～3 and δ 1～3 ;

[0071] Construct a neural network compensator. The total disturbance is compensated according to the variables output by the third-order extended state observer, which improves the accuracy and robustness of the control system. The input of the neural network compensator is the two angle signals z output by the third-order extended state observer. 1 and z 2 ; The output of the neural network compensator is two compensation signals c 1 and c 2 ; Taking two layers as an example, the relationship between the output and input of the neural network compensator based on the multilayer perceptron is expressed as follows:

[0072]

[0073] Among them, h 1 and h 2 is the output of the hidden layer; is the weight coefficient of the neuron, its superscript indicates the number of layers, and in the present invention, it is 1 and 2; its subscript indicates the sequence number of the neuron in this layer and the sequence number of the neuron connected to the next layer, and in the present invention, it is 11, 12, 21 and 22; τ is the deviation of the neuron, its superscript indicates the number of layers, and in the present invention, it is 1 and 2; its subscript indicates the sequence number of the neuron, and in the present invention, it is 1 and 2; f(·) and g(·) are the activation functions of the hidden layer and the output layer, respectively, and can be selected as sigmoid, ReLU, tanh function, etc.;

[0074] The angle compensation signal c output by the neural network compensator 2 The speed signal ω output by the nonlinear state error feedback controller 0 The speed signal ω is obtained by superimposing the speed signal output by the adjustable gain. 2 ;ω 2 It can be expressed as follows:

[0075] ω 2 = bω 0 +c 2 (17)

[0076] Where b is the gain factor.

[0077] The compensation signal c output by the neural network compensator 1 The observed speed signal z output by the third-order extended state observer 3 Superimpose the speed signal ω output by variable gain 1 1 ;ω 1 It can be expressed as follows:

[0078]

[0079] Where b is the gain factor.

[0080] The adjustable gain output speed signal ω 1 The output of the nonlinear state error feedback controller is ω 0 Make a difference and get the reference speed output by the position controller It can be expressed as follows:

[0081]

[0082] The nonlinear tracking differentiator, nonlinear state error feedback controller, third-order extended state observer and neural network compensator are connected in sequence to form a position controller. * and the observed rotor position of the composite controlled object output Output reference speed Control the speed controller.

[0083] like Figure 5 As shown in the figure, the speed controller consists of an initial value PI control module and an extreme value optimization adaptive PI control module. The speed controller based on the extreme value optimization adaptive composite PI controller is constructed to replace the speed loop PI controller and the current loop PI controller in the conventional control scheme, which simplifies the structure of the control system and avoids the increase in cost caused by redundant control links. The input of the speed controller is the speed error The current signal i output by the composite controlled object d ,i q , the actual speed output by the first-order differential tracker The output of the speed controller is a voltage signal u d ,u q .

[0084] The relationship between the output and input of the initial value PI control module is expressed as follows:

[0085]

[0086] Among them, u PIO Represents the output voltage matrix of the initial value PI control module; and They represent two sets of initial proportional-integral coefficients in the initial value PI control module, which can be adjusted according to the actual effect after the calculated values ​​are obtained. The approximate range of the proportional coefficient is 0.1 to 100, and the range of the integral coefficient is 0.01 to 10; i e is the d-axis error current, which can be expressed as follows:

[0087]

[0088] In order to adaptively adjust the initial PI coefficient, an extreme value optimization adaptive PI algorithm based on the sliding mode control law is proposed; the two sliding mode surfaces used in the present invention are selected as follows:

[0089]

[0090] Among them, K 1 and K 2 are two positive sliding mode gain coefficients.

[0091] According to the gradient descent method, the first-order differential expression of minimizing the stability condition to obtain the adaptive PI coefficient under stability is as follows:

[0092]

[0093] Among them, σ is the learning rate of the gradient descent method, its superscript indicates the number of groups of PI parameters, and the subscript indicates the type corresponding to the learning rate; and Represents two sets of PI parameters in the extreme value optimization adaptive PI control module, expressed as follows:

[0094]

[0095] The relationship between the output and input of the extreme value optimization adaptive PI control module is expressed as follows:

[0096]

[0097] Furthermore, the relationship between the output and input of the speed controller can be expressed as follows:

[0098]

[0099] Then we have:

[0100]

[0101] in, is the matrix form of dynamic error, which can be expressed as follows:

[0102]

[0103] Among them, μ 1 and μ 2 is the positive gain coefficient.

[0104] like Figure 1 As shown, the position controller, speed controller and first-order differential tracker are connected in sequence to form a composite controller, which completes the movement of the collaborative robot joint system according to the input voltage signal.

[0105] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modification, equivalent replacement and improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for constructing a composite controller for a collaborative robot joint system, characterized in that The following steps are involved: Step 1): The initial value PI control module and the extreme value optimization adaptive PI control module are composed of inputs of actual speed. Speed ​​error e ω and dq axis current i d ,i q , the output is the control voltage u d ,u q Speed ​​controller; The relationship between the output and input of the initial value PI control module is: u PI0 represents the voltage matrix, and Respectively represent two sets of initial proportional-integral coefficients, d-axis error current L d is the inductance of the d-axis, R s is the resistor; The output and input relationship of the extreme value optimization adaptive PI control module is: Two sets of PI parameters and It is based on the sliding mode control law and is obtained by minimizing the stability condition according to the gradient descent method; The control voltage Step 2): Construct a position sensorless control module consisting of a sliding mode observer and an inverse tangent function, whose input is the voltage v of the αβ axis α ,v β and current i α ,i β , the output is the rotor position angle Step 3): The joint motor, the position sensorless control module, the 2s / 2r conversion module, the SVPWM module, the inverter, the electric sensor, the 3s / 2s conversion module, and the 2r / 2s conversion module together constitute a composite controlled object, and the input of the composite controlled object is the control voltage u d ,u q , output rotor position angle and current i d ,i q ; Step 4): A position controller is composed of a nonlinear tracking differentiator, a nonlinear state error feedback controller, a neural network compensator, two adjustable gains and a third-order extended state observer. The position controller is based on the reference position angle θ * and rotor position angle Output reference speed Step 5): The first-order differential tracker is based on the rotor position angle Output actual speed Reference speed The actual speed The speed error is obtained by subtracting ω The composite controller of the collaborative robot joint system is composed of a position controller, a velocity controller and a first-order differential tracker.

2. The method for constructing a composite controller for a collaborative robot joint system according to claim 1, characterized in that: The extreme value optimization adaptive PI control module adopts the extreme value optimization adaptive PI algorithm based on the sliding mode control law, using two sliding mode surfaces K1 and K2 are two positive sliding mode gain coefficients, two sets of PI parameters and The expression is: σ is the learning rate of the gradient descent method, the superscript indicates the number of groups of PI parameters, and the subscript indicates the type of learning rate.

3. The method for constructing a composite controller for a collaborative robot joint system according to claim 1, characterized in that: The output of the sliding mode observer is the observed extended back electromotive force and k is the sliding mode gain coefficient, and To predict current; expand back EMF and The rotor position is calculated by the inverse tangent function 4. The method for constructing a composite controller for a collaborative robot joint system according to claim 3, characterized in that: The predicted current and The differential form of is: μ α and μ β is the sliding mode control law in the stationary reference frame, S α and S β is the sliding surface under the stationary reference system; A is the coefficient matrix related to the motor parameters. L d is the inductance of the d-axis, L q is the q-axis inductance, R s is the resistance, ω e is the electrical angular velocity.

5. The method for constructing a composite controller for a collaborative robot joint system according to claim 1, characterized in that: The relationship between the output differential signal and the input of the nonlinear tracking differentiator is: h is the adjustable filtering factor, r is the adjustable tracking speed factor, and fhan function is the fastest control comprehensive function.

6. The method for constructing a composite controller for a collaborative robot joint system according to claim 5, characterized in that: The relationship between the differential signal output by the third-order extended state observer and the input is: z1, z2 and z3 are three state variables, β1, β2 and β3 are three adjustable gain coefficients; α1, α2 and α3 are three adjustable parameters of the fal function, δ1, δ2 and δ3 are three adjustable parameters of the fal function, take 5T s ≤δ≤10T s , T s is the sampling time; The nonlinear fal function is: x is the observed state; α and δ are the adjustable parameters of the fal function.

7. The method for constructing a composite controller for a collaborative robot joint system according to claim 6, characterized in that: The relationship between the output speed ω0 signal and the input of the linear state error feedback controller is: ω0=λ1fal(e1,α4,δ4)+λ2fal(e2,α5,δ5), λ1 and λ2 are two error gain coefficients, α4 and α5, δ4 and δ5 are adjustable parameters of the fal function, 8. The method for constructing a composite controller for a collaborative robot joint system according to claim 7, characterized in that: The relationship between the two angle compensation signals c1, c2 output by the neural network compensator and the input is: h1 and h2 are the outputs of the hidden layer; is the weight coefficient of the neuron, its superscript indicates the number of layers, and takes 1 and 2; its subscript indicates the sequence number of the neuron in this layer and the sequence number of the neuron connected to the next layer, and takes 11, 12, 21 and 22; τ is the deviation of the neuron, its superscript indicates the number of layers, and takes 1 and 2; its subscript indicates the sequence number of the neuron, and takes 1 and 2; f(·) and g(·) are the activation functions of the hidden layer and the output layer respectively.

9. The method for constructing a composite controller for a collaborative robot joint system according to claim 8, characterized in that: The angle compensation signal c2 is superimposed on the speed ω0 through the first adjustable gain output to obtain the speed ω2 = bω0 + c2, where b is the gain coefficient; the angle compensation signal c1 is superimposed on the speed state variable z3 through the second variable gain output speed Reference speed 10. The method for constructing a composite controller for a collaborative robot joint system according to claim 1, characterized in that: The 2s / 2r conversion module, SVPWM module, inverter, electric sensor and 3s / 2s conversion module are connected in sequence. The output of the 3s / 2s conversion module is connected to the 2r / 2s conversion module and the position sensorless control module respectively. The electric sensor is connected to the joint motor. The input of the 2s / 2r conversion module is the dq axis voltage u d ,u q , converted into αβ axis voltage u by 2s / 2r α ,u β , and output to the SVPWM module, the SVPWM module outputs six switch signals s 1～6 In the input inverter, the inverter drives the joint motor through the DC voltage power supply; the electrical sensor collects the voltage v output by the inverter a , v b , v c Signal and current i a ,i b ,i c The signal is input into the 3s / 2s conversion module, and the voltage v of the αβ axis is output after 3s / 2s conversion. α , v β and current i α ,i β , current i α ,i β Input into the 2r / 2s conversion module, after 2r / 2s conversion the output current i d ,i q ; The current i output by the 2s / 2r conversion module d ,i q Input to the speed controller for feedback, the voltage v α , v β and current i α ,i β Input to the sensorless control module, and output the rotor position angle of the joint motor after sensorless control. The rotor position angle Feedback to the 2s / 2r conversion module and the 2r / 2s conversion module.

Images