A feedforward decoupling variable damping robot joint compliant drive control method

Abstract

A feedforward decoupling variable damping robot joint compliant drive control method relates to a robot joint drive control method, which includes calculating the target quadrature axis current I according to the set damping coefficient B and stiffness coefficient K under the control system framework of the current loop impedance. q_target ; Sample the three-phase current of the motor and transform it to obtain the direct-axis current Id and the quadrature-axis current Iq; calculate the direct-axis current I d , quadrature axis current I q and the direct-axis current target value I d_target and the target value of the quadrature axis current I q_target The error; the quadrature axis and direct axis voltages are corrected by the feedforward decoupling controller; the space voltage vector U is calculated out and space voltage vector U out The corresponding electrical angle θ out The system calculates the action time of each vector within the sector and the duty cycle of each channel of the three-channel center-aligned complementary PWM. This is then used to drive the MOS switches in the motor via a three-phase inverter circuit. This achieves efficient, real-time impedance control, improving the robot's dynamic performance while maintaining compliance.

Description

Technical Field

[0001] The present invention relates to a robot joint drive control method, in particular to a feedforward decoupling variable damping robot joint compliant drive control method. Background Art

[0002] With the development of robotics technology, more and more robots need to interact with humans or work in unstructured environments, such as exoskeleton robots, collaborative robots, service robots, and humanoid robots. As core components, electric joints are increasingly moving towards miniaturization and lightweighting. Furthermore, to ensure the safety of both humans and robots during human-machine interaction and in unstructured environments, robot joints are generally required to be compliant. To make joints compliant and prevent rigid collisions, it is necessary to introduce stiffness and damping adjustment mechanisms into traditional purely rigid joint actuators to impart a certain degree of compliance to the joints. However, this often increases hardware costs and results in the robot joints being too large in overall size.

[0003] Existing robots implement compliance control within the main controller, often occupying some of the communication resources between the driver and the main controller, thereby occupying a large amount of communication bandwidth, often affecting the real-time performance and effectiveness of control. This shows that existing robot joints lack the underlying compliance control of the driver, making them vulnerable to external impact and collision damage. Summary of the Invention

[0004] The purpose of the present invention is to provide a feedforward decoupling variable damping robot joint flexible drive control method, which is a method for achieving the flexibility of the driver joint based on impedance control of the driver current loop feedforward decoupling, achieving a dynamic effect similar to that of a spring damping system, so that the robot can flexibly contact external objects during movement to avoid rigid collisions, while avoiding the loss of torque servo bandwidth due to communication between the driver and the main control, so as to achieve a more efficient impedance control effect.

[0005] The purpose of the present invention is achieved through the following technical solutions:

[0006] A feedforward decoupling variable damping robot joint compliant drive control method, the method comprising the following steps:

[0007] (1) Under the control system framework of the current loop impedance, the target quadrature axis current I is calculated according to the set damping coefficient B and stiffness coefficient K. q_target ;

[0008] (2) Sample the three-phase current of the motor to obtain I a , I b The two-phase current, the other phase current I is obtained by Kirchhoff node current equation c ;

[0009] (3) Will I a , I b , I c The three-phase current is transformed by Clark to obtain the current I in the stator α-β coordinate system α , I β ; (4) The current I in the stator α-β coordinate system α , I β The direct axis current Id and quadrature axis current Iq are obtained through Park transformation;

[0010] (5) Calculate the direct axis current I d , quadrature axis current I q and the direct axis current target value I calculated in step 1 d_target and the target value of the quadrature axis current I q_target The error, I d_target Generally, it is set to 0, and the error is input into two PI controllers to obtain the output direct axis voltage U' d and quadrature axis voltage U' q ;

[0011] (6) Correcting the quadrature-axis and direct-axis voltages through a feedforward decoupling controller;

[0012] (7) Using the decoupled direct axis voltage U d and the decoupled quadrature axis voltage U q Calculate the space voltage vector U out and space voltage vector U out The corresponding electrical angle θ out ;

[0013] (8) By the space voltage vector U out The electrical angle θ out Calculate and determine the current rotor sector, and according to the space voltage vector U out , sector S and space voltage vector U out The electrical angle θ out , calculate the action time of each vector in the sector;

[0014] (9) According to the vector action time and the sector where the rotor is currently located, the duty cycle of each channel of the three-channel center-aligned complementary PWM is calculated, and the MOS switch is driven by the three-phase inverter circuit to drive the motor.

[0015] The present invention relates to a feedforward decoupling variable damping robot joint compliant drive control method, wherein the current loop impedance control system framework is as follows: achieving a dynamic effect similar to that of a spring damping system, enabling the robot to flexibly contact external objects during movement to avoid rigid collisions, while avoiding the loss of torque servo bandwidth due to communication between the driver and the main control, thereby achieving a more efficient impedance control effect; improving the dynamic performance of the robot while ensuring the flexibility of the robot, adjusting the driver stiffness coefficient and damping coefficient to meet the requirements of use in different occasions; realizing impedance control at the bottom layer of the driver current loop, and realizing compliant force control through the driver bottom layer current loop.

[0016] The feedforward decoupling variable damping robot joint compliant drive control method, the control system of the flow loop impedance ignores the acceleration term, the target torque τ of the impedance control d The calculation method is For a general BLDC motor, the electromagnetic torque T e With the quadrature axis current I q Proportional to the electromagnetic torque T output by the motor e The target value is set to the impedance shape By controlling the quadrature axis current I q , to achieve the dynamic effect of simulating impedance.

[0017] The described feedforward decoupling variable damping robot joint compliant drive control method, the feedforward decoupling controller is used to control the direct axis and quadrature axis to accurately eliminate the above-mentioned competing voltage terms, and the system of the motor part is made equivalent to a simple RL series circuit through correction, which is equivalent to a general DC motor. In the impedance control system based on current loop feedforward decoupling, Ff_Uq is the feedforward control compensation part of the quadrature axis, and Ff_Ud is the feedforward control compensation part of the direct axis.

[0018] The feedforward decoupling variable damping robot joint compliant drive control method, wherein the feedforward decoupling controller comprises a direct axis decoupling controller and a quadrature axis decoupling controller, and the direct axis decoupling control comprises the following steps:

[0019] 1) The rotor angular velocity ω obtained by obtaining the first-order differential of the rotor position through the magnetic encoder is multiplied by the stator inductance in the motor parameters and then multiplied by the quadrature-axis current I q , we can get the direct-axis voltage compensation term ωL s I q ;

[0020] 2) The direct axis voltage U′ after passing through the direct axis PI controller d Subtract the direct-axis voltage compensation term calculated in the previous step to obtain the decoupled direct-axis voltage U d ; The quadrature axis decoupling control includes the following steps: 1) stator inductance L sMultiply by the direct axis current I d Then add the back electromotive force coefficient Ke and multiply it by the rotor angular velocity ω to obtain the quadrature axis voltage compensation ω(L s I d +K e );

[0021] 3) The quadrature axis voltage U′ is delayed by the quadrature axis PI controller q Add the quadrature axis voltage compensation calculated in the previous step to obtain the decoupled quadrature axis voltage U q .

[0022] The feedforward decoupling variable damping robot joint compliant drive control method, the calculation space voltage vector U out and space voltage vector U out The corresponding electrical angle θ out , which calculates the space voltage vector U out and space voltage vector U out The corresponding electrical angle θ out The basic steps include:

[0023] 1) Determine the direct axis voltage U d and quadrature axis voltage U q Whether it exceeds the range of the maximum linear controllable amplitude, the direct axis voltage U d and quadrature axis voltage U q Calculated as follows

[0024]

[0025] 2) Calculate the space voltage vector U out The magnitude and voltage vector U out The electrical angle θ of the vector out ; If the direct axis voltage U is input d If it is not 0, the space voltage vector is calculated as follows:

[0026]

[0027] Space voltage vector U out Electrical angle θ out Updated to:

[0028]

[0029] If the input direct axis voltage is 0, then U out The parameter calculation method is as follows, where U dc is the bus voltage

[0030]

[0031] At this time, the space voltage vector Uout Electrical angle θ out Updated to:

[0032]

[0033] In the above calculation process, the normal function is used to convert the angle to between 0 and 2PI. The calculation method is as follows

[0034]

[0035] The feedforward decoupling variable damping robot joint compliant drive control method is described, wherein the current rotor sector is determined according to the space voltage vector U out The electrical angle θ out Obtain the current sector of the rotor, simplify the sector calculation method of SVPWM, and directly calculate the sector through the quadrature-axis voltage, direct-axis voltage and rotor electrical angle θ.

[0036] The advantages and effects of the present invention are:

[0037] 1. The present invention provides a method for achieving the compliance of the driver joints by impedance control based on the feedforward decoupling of the driver current loop, achieving a dynamic effect similar to that of a spring damping system, so that the robot can flexibly contact with external objects during movement to avoid rigid collisions, while avoiding the loss of torque servo bandwidth due to communication between the driver and the main control, so as to achieve a more efficient impedance control effect. While ensuring the flexibility of the robot, the dynamic performance of the robot is improved, and the driver stiffness coefficient and damping coefficient can be adjusted to meet the use requirements of different occasions. Secondly, by simplifying the sector calculation method of the traditional vector control SVPWM, unnecessary floating-point operations are avoided, and the computing efficiency of the control system in the driver is improved. In addition, the feedforward decoupling of the direct-axis and quadrature-axis control is added to the current loop control part to avoid coupling between the direct-axis and quadrature-axis control.

[0038] 2. The present invention implements impedance control at the bottom layer of the driver current loop, avoiding bandwidth loss of torque servo caused by communication between the joint driver and the upper-level controller. It does not require the installation of a force sensor and only implements flexible force control through the driver's bottom-layer current loop, reducing system cost, volume and complexity.

[0039] 3. The present invention can set the stiffness coefficient and damping coefficient of the bottom driver according to actual conditions to produce different flexible control effects.

[0040] 4. The present invention simplifies the SVPWM sector calculation method. Compared with the traditional sector judgment and calculation method, it can eliminate the Park transformation module and floating-point operations in the stator coordinate system α-β. The sector is directly calculated based on the quadrature-axis voltage, direct-axis voltage and rotor electrical angle θ, thereby improving the calculation efficiency of the control system in the driver.

[0041] 5. The flow loop control part of the present invention adds feedforward decoupling of direct axis and quadrature axis control to achieve decoupling control of direct axis and quadrature axis.

[0042] 6. The algorithm of the present invention is simple and efficient, and can be implemented on low-cost mainstream controllers, which is conducive to promotion. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 Schematic diagram of the structure of the impedance control system based on current loop feedforward decoupling of the present invention;

[0044] Figure 2 Schematic diagram of current loop feedforward decoupling of the present invention;

[0045] Figure 3 This is the basic flow chart of the feedforward decoupling driver compliance control of the present invention;

[0046] Figure 4 This is a schematic diagram of sector division of the present invention;

[0047] Figure 5 This is a vector diagram of the first sector of the present invention;

[0048] Figure 6 This is a schematic diagram of vector superposition of the first sector of the present invention;

[0049] Figure 7 This is a schematic diagram of the driver calibration and self-test process of the present invention;

[0050] Figure 8 This is a block diagram of the position loop servo system of the present invention;

[0051] Figure 9 This is a block diagram of the speed loop servo system of the present invention;

[0052] Figure 10 This is a step response curve diagram of the position loop of the present invention;

[0053] Figure 11 This is a speed loop step response curve diagram of the present invention;

[0054] Figure 12 This is a position step response curve diagram of the present invention under different stiffness parameters when carrying a 2KG load;

[0055] Figure 13 This is a torque step response curve diagram of the present invention under different stiffness parameters when carrying a 2KG load;

[0056] Figure 14 This is a position step response curve diagram of the present invention with different damping parameters when carrying a 2KG load;

[0057] Figure 15This is a torque step response curve diagram of the present invention under different damping parameters when carrying a 2KG load. DETAILED DESCRIPTION

[0058] The present invention will be described in detail below with reference to the embodiments shown in the accompanying drawings.

[0059] The present invention comprises the following steps:

[0060] Step 1: Calculate the target quadrature axis current I according to the set damping coefficient B and stiffness coefficient K. q_target Combined with the subsequent steps, a closed current loop is formed to achieve the effect of flexible control of the driver. X is the current position θ current and the target position θ target The following formula (1) is the calculation formula for the position difference X: is the first-order difference of X. The differential signal needs to be added with a digital low-pass filter to filter out high-frequency noise. q Under certain conditions, the impedance control of a single joint can be based on the quadrature axis current I q control to achieve it.

[0061] X=θ current -θ target (1)

[0062]

[0063] According to the above formula, the quadrature axis current I q The expression is as follows

[0064]

[0065] in

[0066] Therefore, the target value of the current loop quadrature-axis current is set to According to the actual site conditions, select different stiffness coefficients K and damping coefficients B and calculate the corresponding target quadrature axis current I q_target , combined with the subsequent steps to form a complete current loop closed-loop control, the corresponding impedance control effect can be achieved. The basic framework of the control system is as follows Figure 1 The structural diagram of the impedance control system with current loop feedforward decoupling is shown in the figure.

[0067] Step 2: Sample the three-phase current of the motor to obtain I a , I b The two-phase current, the other phase current I is obtained by Kirchhoff node current equation c ;

[0068] I a +I b +Ic =0 (4)

[0069] Step 3: I a , I b , I c The three-phase current is transformed by Clark to obtain the current I in the stator α-β coordinate system α , I β ;

[0070]

[0071] Step 4: Convert the current I in the stator α-β coordinate system α , I β The direct axis current Id and quadrature axis current Iq are obtained through Park transformation;

[0072]

[0073] Step 5: Calculate the direct axis current I d , quadrature axis current I q and the direct axis current target value I calculated in step 1 d_target and the target value of the quadrature axis current I q_target The error, I d_target Generally, it is set to 0, and the error is input into two PI controllers to obtain the output direct axis voltage U′ d and quadrature axis voltage U′ q ;

[0074] Step 6: Correct the quadrature axis and direct axis voltages through the feedforward decoupling controller. The controller structure of the feedforward decoupling is as follows: Figure 2 The schematic diagram of the current loop feedforward decoupling is shown below, where L s is the stator inductance, Ke is the back electromotive force coefficient, both are known quantities, PI_D is the direct axis PI controller, and PI_Q is the quadrature axis PI controller. Figure 1 Schematic diagram of the impedance control system structure based on current loop feedforward decoupling, where Ff_Uq is the quadrature-axis feedforward control compensation component, and Ff_Ud is the direct-axis feedforward control compensation component. Feedforward decoupling control is used to control the direct and quadrature axes to accurately eliminate the competing voltage terms of mutual coupling. Through correction, the motor system is made equivalent to a simple RL series circuit, equivalent to a conventional DC motor. The main decoupling calculation steps are as follows:

[0075] The specific steps of direct-axis decoupling control are as follows

[0076] The rotor angular velocity ω obtained by obtaining the first-order difference of the rotor position through the magnetic encoder is multiplied by the stator inductance in the motor parameters and then multiplied by the quadrature-axis current I q , we can get the direct-axis voltage compensation term ωL s Iq

[0077] The direct axis voltage U′ after passing through the direct axis PI controller d Subtract the direct-axis voltage compensation term calculated in the previous step to obtain the decoupled direct-axis voltage U d ;

[0078] The specific steps of quadrature axis decoupling control are as follows

[0079] Stator inductance L s Multiply by the direct axis current I d Then add the back electromotive force coefficient Ke and multiply it by the rotor angular velocity ω to obtain the quadrature axis voltage compensation ω(L s I d +K e );

[0080] The quadrature axis voltage U′ is lagged by the quadrature axis PI controller. q Add the quadrature axis voltage compensation calculated in the previous step to obtain the decoupled quadrature axis voltage U q ;

[0081] Step 7: Use the decoupled direct axis voltage U d and the decoupled quadrature axis voltage U q Calculate the space voltage vector U out and space voltage vector U out The corresponding electrical angle θ out The specific steps are as follows:

[0082] 1) Determine the direct axis voltage U d and quadrature axis voltage U q Whether it exceeds the range of the maximum linear controllable amplitude, the direct axis voltage U d and quadrature axis voltage U q Out of range can be calculated as follows

[0083]

[0084] 2) Calculate the space voltage vector U out The modulus and space voltage vector U out The electrical angle θ of the vector out .

[0085] If the direct axis voltage U d If it is not 0, the space voltage vector is calculated as follows

[0086]

[0087] And the direct axis voltage U d With the quadrature axis voltage U q The direction is vertical, and the quadrature axis leads the direct axis, so the space voltage vector Uout Electrical angle θ out Updated to

[0088]

[0089] If the input direct axis voltage is 0, then U out The parameter calculation method is as follows, where U dc is the bus voltage

[0090]

[0091] At this time, the space voltage vector U out Electrical angle θ out Updated to

[0092]

[0093] In the above calculation process, the normal function is used to convert the angle to a value between 0 and 2PI. The calculation method is as follows

[0094]

[0095] Where a = mod (θ, 2π).

[0096] Step 8: By calculating the space voltage vector U out The electrical angle θ out Calculate and determine the current rotor sector, and according to the space voltage vector U out , sector S and space voltage vector U out The electrical angle θ out , calculates the action time of each vector in the sector. Compared with the traditional sector judgment and calculation method, it eliminates the inverse Park transformation module and a series of floating-point calculations in the stator coordinate system, improving the algorithm's operating efficiency in the drive, especially for processors with only integer data operation units, which can significantly improve the calculation efficiency. The specific process is as follows:

[0097] 1) Divide the circumference of 360 into six equal parts, such as Figure 4 The sector diagram is shown, and the current rotor sector is calculated. The current rotor sector can be obtained according to the current electrical angle of the rotor by the following formula (13):

[0098]

[0099] 2) According to the space voltage vector U calculated in step 7 out , and the space voltage vector U out The electrical angle is used to calculate the action time of the zero vector and the other two main vectors in the sector. The specific calculation formulas are as follows (14), (15), and (16):

[0100]

[0101]

[0102] T0=1-T1-T2 (16)

[0103] Step nine, based on the vector action time and the sector the rotor is currently in, calculate the duty cycle of each channel of the three-channel center-aligned complementary PWM, and drive the motor through the three-phase inverter circuit to drive the MOS switch. The following table is a calculation table of the duty cycle of each channel of the three-channel center-aligned complementary PWM corresponding to each sector, as shown in the following table, where S represents the sector, T1 and T2 are the action time of the two main vectors in a sector in one cycle, and T0 is the zero vector action time in the cycle. Finally, the duty cycle of each channel T can be obtained. a 、T b 、T c .

[0104] Table - Calculation table of duty cycle of each channel of three-channel center-aligned complementary PWM

[0105]

[0106] Finally, the three-channel duty cycle T a 、T b 、T c By multiplying the cycle count value of the three-channel center-aligned complementary PWM of the microcontroller peripheral, the three-phase inverter circuit can be driven, and then the motor can be driven.

[0107] Furthermore, the above basic steps can be simply summarized as follows: Figure 3 The basic flow chart of feedforward decoupling drive compliance control. By repeatedly looping the above steps, the compliant drive control effect based on the position target can be achieved.

[0108] Furthermore, for step 1, since there is no accelerometer or force sensor, the acceleration signal needs to be obtained by twice differentiating the rotor position signal obtained by the magnetic encoder, which will result in large signal noise. When the acceleration term is ignored, the calculation method of the target torque of the impedance control is simplified to

[0109]

[0110] According to the motor electromagnetic torque τ and quadrature axis current I q There are the following basic relationships

[0111] T e =1.5PP[λ f I q +(Ld -L q )I d I q ] (18)

[0112] For general BLDC brushless motor L d =L q , can be further simplified as the following formula (19), where PP is the pole pair number, λ f is the rotor magnetic flux, I q is the magnitude of the quadrature axis current.

[0113] T e =1.5PP·λ f I q (19)

[0114] From the above formula, it can be concluded that the motor electromagnetic torque T e With the quadrature axis current I q A basic relationship that is proportional under certain conditions.

[0115] The following examples are further described:

[0116] Step 1: To implement impedance control based on the current loop, you first need to calculate the target quadrature axis current I q_target To achieve the effect of flexible control of the driver. For a general BLDC motor, the electromagnetic torque T e With the quadrature axis current I q Under certain conditions, the impedance of a single joint can be controlled by controlling the quadrature axis current. Figure 1 The impedance control system structure diagram of the current loop feedforward decoupling is shown as follows, where B and K are the damping coefficient and stiffness coefficient; X is the current position θ current and the target position θ target The difference, is the first-order difference of X. The differential signal needs to be filtered with a digital low-pass filter to remove high-frequency noise. The following formula (20) is the position difference calculation formula.

[0117] X=θ current -θ target (20)

[0118] The electromagnetic torque T output by the motor e The target value is set as impedance to achieve the dynamic effect of simulated impedance, as shown in the following formula

[0119]

[0120] Then we can further get the target quadrature-axis current I q_target The calculation expression is as follows

[0121]

[0122] in

[0123] Therefore, the quadrature-axis current target value I q_target Set to According to the actual site conditions, different stiffness coefficients K and damping coefficients B are selected, and combined with subsequent steps to form a complete current loop closed-loop control, the corresponding impedance control effect can be achieved.

[0124] Step 2: Sample the three-phase current to obtain three sine waves with a phase difference of 120 degrees, that is, three non-orthogonal basis vectors I a , I b , I c ,

[0125] The ADC sampling of the microcontroller is used, and the current amplification signal of the INA240 chip of Texas Instruments is obtained by DMA transmission. a , I b The two-phase current is magnified to restore the actual motor current values. According to Kirchhoff's node current equation:

[0126] I a +I b +I c =0 (23)

[0127] Step 3: I a , I b , I c The three-phase current is transformed by Clark to obtain the current I in the stator α-β coordinate system α , I β , and its transformation formula is as follows:

[0128]

[0129] Step 4: Convert the current I in the stator α-β coordinate system α , I β The Park transform yields the direct-axis current Id and quadrature-axis current Iq. The Clark transform yields the three-phase current representation in the stator α-β coordinate system. However, vector control operates on the motor's direct-axis and quadrature-axis currents, so the stator α-β coordinate system must be transformed into the rotor dq coordinate system. The transformation formula is as follows, where θ is the rotor electrical angle:

[0130]

[0131] Step 5: Calculate the current direct axis current Id and quadrature axis current Iq and the set direct axis current target value Id_target and the target value of the quadrature axis current I q_target The error, and I d_target Generally, it is set to 0, and the error is input into two PI controllers to obtain the output direct axis voltage U' d and quadrature axis voltage U′ q ;

[0132] Step 6: Use the feedforward decoupling controller to control the direct axis voltage U′ d and quadrature axis voltage U′ d Make corrections;

[0133] From the basic voltage equations (26) and (27) of the brushless motor, it can be seen that the control between the direct axis and the quadrature axis of the brushless motor is not independent of each other. Inside the motor, the direct axis will affect the control of the quadrature axis, and the quadrature axis will also affect the control of the direct axis. This effect is most severe under high-speed transient conditions. Therefore, adding feedforward decoupling control can enable the brushless motor to maintain good control performance under high-speed rotation conditions.

[0134]

[0135]

[0136] The feedforward decoupling controller is used to control the direct and quadrature axes to accurately eliminate the competing voltage terms. Through correction, the motor system is made equivalent to a simple RL series circuit, equivalent to a general DC motor. The components of the direct and quadrature axis voltages that need to be compensated are obtained from the basic voltage equation of the brushless motor, where Ls is the stator inductance and Ke is the back electromotive force coefficient, both of which are known quantities. Figure 1 Schematic diagram of the impedance control system structure based on current loop feedforward decoupling, where Ff_Uq is the feedforward control compensation part of the quadrature axis, and Ff_Ud is the feedforward control compensation part of the direct axis.

[0137] The controller structure of feedforward decoupling is as follows Figure 4 The current loop feedforward decoupling diagram is shown in the figure. s is the stator inductance and Ke is the back electromotive force coefficient, both of which are known quantities.

[0138] The specific steps of direct-axis decoupling control are as follows:

[0139] The rotor angular velocity ω obtained by obtaining the first-order difference of the rotor position through the magnetic encoder is multiplied by the stator inductance in the motor parameters and then multiplied by the quadrature-axis current I q , we can get the direct-axis voltage compensation term ωL s I q ;

[0140] The direct axis voltage U′ after passing through the direct axis PI controllerd Subtract the direct-axis voltage compensation term calculated in the previous step to obtain the decoupled direct-axis voltage U d ;

[0141] The specific steps of quadrature-axis decoupling control are as follows:

[0142] Stator inductance L s Multiply by the direct axis current I d Then add the back electromotive force coefficient Ke and multiply it by the rotor angular velocity ω to obtain the quadrature axis voltage compensation ω(L s I d +K e );

[0143] The quadrature axis voltage U′ is lagged by the quadrature axis PI controller. q Add the quadrature axis voltage compensation calculated in the previous step to obtain the decoupled quadrature axis voltage U q ;

[0144] Step 7: Through the direct axis voltage U d and quadrature axis voltage U q Calculate the space voltage vector U out and voltage vector U out The electrical angle θ of the vector out , the specific process is as follows:

[0145] 1) Determine the direct axis voltage U d and quadrature axis voltage U q Whether it exceeds the range of the maximum linear controllable amplitude, the direct axis voltage U d and quadrature axis voltage U q It can be calculated as follows

[0146]

[0147] 2) Calculate the space voltage vector U out The magnitude and voltage vector U out The electrical angle θ of the vector out .

[0148] If the direct axis voltage U d If it is not 0, the space voltage vector is calculated as follows

[0149]

[0150] And the direct axis voltage U d With the quadrature axis voltage U q The direction is vertical, and the quadrature axis leads the direct axis, so the space voltage vector U out Electrical angle θ out Updated to

[0151]

[0152] If the input direct axis voltage is 0, then U out The parameter calculation method is as follows, where U dc is the bus voltage

[0153]

[0154] At this time, the space voltage vector U out Electrical angle θ out Updated to

[0155]

[0156] In the above calculation process, the normal function is used to convert the angle to a value between 0 and 2PI. The calculation method is as follows

[0157]

[0158] Where a = mod (θ, 2π).

[0159] Step 8: By calculating the space voltage vector U out The electrical angle θ out Calculate and determine the current sector of the rotor and calculate the action time of each vector in the sector. Compared with the traditional sector judgment and calculation method, the inverse Park transformation module and a series of floating-point calculations in the stator coordinate system are eliminated, which improves the operation efficiency of the algorithm in the drive, especially for processors with only integer data operation units, which can significantly improve the calculation efficiency. The specific process is as follows:

[0160] 1) Divide the circumference of 360 into six equal parts, such as Figure 5 The sector diagram is shown, and the current rotor sector is calculated. According to the following formula (34), the space voltage vector U out The electrical angle θ out Get the current sector of the rotor.

[0161]

[0162] 2) According to the space voltage vector U calculated in step 7 out , voltage vector U out Sector s and space voltage vector U out The electrical angle θ out , calculate the action time of the zero vector and the other two vectors in the sector. The specific calculation formulas are as follows (35), (36), (37)

[0163]

[0164]

[0165] T0=1-T1-T2 (37)

[0166] Step 9: Obtain the duty cycle of the three-channel center-aligned complementary PWM according to the corresponding sectors and the three vector action times. The three-channel center-aligned complementary PWM drives the three-phase inverter circuit to drive the MOS switch to drive the motor.

[0167] The following table is a calculation table of the duty cycle of each channel of the three-channel center-aligned complementary PWM corresponding to each sector, as shown in the following table, where S represents the sector, T1 and T2 are the action time of the two main vectors in a sector in one cycle, and T0 is the action time of the zero vector in the cycle. Finally, the duty cycle T can be obtained. a 、T b 、T c .

[0168] Table - Calculation table of duty cycle of each channel of three-channel center-aligned complementary PWM

[0169]

[0170]

[0171] Finally, the three-channel duty cycle T a 、T b 、T c By multiplying the cycle count value of the three-channel center-aligned complementary PWM of the microcontroller peripheral, the three-phase inverter circuit can be driven, and then the motor can be driven.

[0172] Prioritize the impedance control for achieving compliant control of the driver force in step 1. Its essence is to show the characteristics of a virtual spring and simulate the dynamic characteristics of a spring-damper system. Therefore, the impedance control dynamic equation is as follows, where M, B, and K are virtual mass, damping, and stiffness, and X is the current position θ. current and the target position θ target The difference, is the first-order difference of X, is the second-order difference of X.

[0173]

[0174] In the absence of an accelerometer, the acceleration signal needs to be obtained by twice differentiating the rotor position signal obtained by the magnetic encoder, which will result in large signal noise. When the acceleration term is ignored, the target torque τ of the impedance control is d The calculation method is as follows

[0175]

[0176] According to the electromagnetic torque T e and the quadrature axis current I q There are the following basic relationships

[0177] T e =1.5PP[λ f I q +(L d -L q )I d I q ] (40)

[0178] For general BLDC brushless motor L d =L q , so we can further deduce Equation (41), where PP is the pole pair number, λ f is the rotor magnetic flux, i q is the magnitude of the quadrature axis current.

[0179] T e =1.5PP·λ f I q (41)

[0180] Prioritize step 1 by adding a digital low-pass filter to the differential signal to filter out high-frequency noise. This means differentiating the position signal obtained by the magnetic encoder. The speed signal noise after differentiation is very high, so a first-order low-pass filter module needs to be added to smooth it out to obtain a higher-quality speed signal. This first-order low-pass filter is a digital filter obtained by analyzing the S-domain model of the corresponding analog RC circuit. The following is the frequency domain model of the first-order low-pass filter, where Y(s) is the output under the Laplace transform, X(s) is the input under the Laplace transform, R is the equivalent resistance value, and C is the equivalent capacitance value.

[0181]

[0182] The S-domain model of the circuit is discretized into a Z-domain model through first-order backward differentiation and converted into the corresponding differential equation, ultimately achieving the effect of a digital first-order low-pass filter. X(z) is the Z-domain input and Y(z) is the Z-domain output.

[0183]

[0184] The model of the digital low-pass filter can be obtained by solving the differential equation through the Z inverse transform. X(n) is the time domain input of the discrete system, and Y(n) is the time domain output of the discrete system.

[0185] Y(n)=A·X(n)+(1-A)Y(n-1) (44)

[0186] The filter factor

[0187]

[0188] Since the time taken to run the code each time may vary during actual operation, the sampling period T is often not fixed. Therefore, the strategy adopted in the software is to set a fixed RC parameter. The actual RC parameter is 0.05. By accessing the timer count before and after two control cycles, the difference between the timer counters is calculated to obtain T each time.

[0189] Preferably, in step 7, the shaft voltage U d and quadrature axis voltage U q The range of the maximum linear controllable amplitude is limited. The following discusses the range of the maximum linear controllable amplitude. Taking the first sector as an example, the maximum linear controllable amplitude is calculated, and the action time of the two first sector main vectors a and b is set to T a 、T b , T o is the zero vector action time in one cycle, T is the cycle time, where the main vector V a and V b The angle is 60 degrees.

[0190] T=T a +T b +T o (46)

[0191] Then in the first sector, any space voltage vector Equal to the quadrature axis voltage vector The vector of the direct axis voltage and can be expressed as the following formula

[0192]

[0193]

[0194] Based on bus voltage U dc The basic relationship between and is the per-unit value U of the sector voltage vector

[0195]

[0196] Combined with formula (48), the modulus U of the arbitrary space voltage vector in the first sector is s and the main vector V a and V b The geometric relationship can be expressed as follows

[0197]

[0198]

[0199] By simplifying formula (50), we can get

[0200]

[0201] Using the trigonometric function difference angle formula to simplify formula (51)

[0202]

[0203] Substituting the above formula and simplifying it, we can get formula (54), (55)

[0204]

[0205]

[0206] Substituting formulas (29) and (31) into formulas (54) and (55), we can obtain expressions (35) and (36) for the action time of the main vector in the sector within the cycle in step 8.

[0207] According to equations (48) and (49), the sum of the action time of the main vectors a and b in the first sector can be obtained:

[0208]

[0209] Since the sum of the action time of the main vectors a and b within a cycle is less than the time of one cycle, it can be expressed as follows (51)

[0210]

[0211] Substituting (56) into (57) yields

[0212]

[0213] and

[0214] Therefore, it can be deduced that when the voltage vector is maximized

[0215]

[0216] Combining the relationship between bus voltage and sector voltage vector, the voltage limit of the maximum linear controllable amplitude can be obtained as follows:

[0217]

[0218] Because the above conclusion satisfies equation (57), the possibility of overmodulation problem is also eliminated.

[0219] First, for the duty cycle calculation table of each channel of the three-channel center-aligned complementary PWM in step nine, a more detailed theoretical explanation is given below. Assume that 1 is the upper bridge arm MOS conduction and 0 is the lower bridge arm MOS conduction. From the switch combination state of the three-phase inverter circuit, 8 different combination vectors can be obtained, namely V1 (011), V2 (010), V3 (011), V4 (100), V5 (101), V6 (110) and two zero vectors V0 (000) and V7 (111). The six main vectors from V1 to V6 are divided into six sectors, and the difference between each main vector is 60 degrees. The schematic diagram of the sector is as follows Figure 4 The sector zoning diagram is shown as follows.

[0220] Taking the first sector as an example, the duty cycle of the center-aligned complementary PWM is analyzed. In one switching period T, the basic voltage vectors are combined so that their average value is equal to the given voltage vector. Figure 6 The first sector vector superposition diagram shows that any space voltage vector in the first sector can be formed by the superposition of the two main vectors that constitute the sector. In order to reduce the harmonic components and minimize the number of switches, only one switch is used each time, and the main vector is decomposed into two segments with equal module lengths. Then, four small main vectors appear in the cycle, namely Adhere to the principle of minimum switching quantity and add zero vector in the first and middle parts and There are seven vector segments, representing an equivalent arrangement of the principal vectors of the space voltage vector. The horizontal rows in the table below represent the principal vectors, while the vertical rows show the switching conditions of each bridge arm in the three-phase inverter circuit. A value of 1 indicates that the upper bridge arm MOS is on, and a value of 0 indicates that the lower bridge arm MOS is on.

[0221] Table-I Sector vector minimum switch quantity arrangement table

[0222]

[0223] Assume that the durations of the main vectors V4 and V6 within a cycle are T1 and T2, respectively, and T0 is the duration of the zero vector within the cycle. The duration of channel A is T1 + T2 + T0 / 2, the duration of channel B is T2 + T0 / 2, and the duration of channel C is T0 / 2. By deriving the durations of the vectors for each sector using the above method, we can obtain a table for calculating the duty cycle of each channel in a three-channel center-aligned complementary PWM.

[0224] In addition to the above basic functions, the driver also has self-calibration and self-test functions. The basic steps are as follows:

[0225] Get the difference angle between the encoder zero position and the motor direct axis zero point.

[0226] Given a target motor direct-axis current, continuously accumulate the electrical angle value over one electrical angle cycle, allowing the motor to follow the motor. Record the angle value φ1 after the accumulation. Then, keep the motor direct-axis current constant, decrement the electrical angle, allowing the motor to follow the motor, and record the encoder angle value φ2 after the decrement. Take the difference between the accumulated and decremented angle values, take the modulus, and multiply by the pole pair number PP.

[0227] moved=|φ1-φ2|·PP (61)

[0228] Determine whether the driver and motor can work normally.

[0229] If moved≤0.01, it means that the motor has hardly moved. In this case, turn off the drive motor enable and report an error. Otherwise, it means that the motor is working normally and proceed to the next test.

[0230] Test the forward and reverse direction of the motor rotation.

[0231] If φ1>φ2, the motor direction flag is set to -1, indicating reverse rotation;

[0232] Otherwise, the motor direction flag is set to -1, indicating forward rotation;

[0233] Test if there is a problem with setting the number of pole pairs.

[0234] The difference between moved and 2PI is calculated. If the difference is greater than 0.5, it means that the set number of motor pole pairs is incorrect and needs further calculation and correction. The logarithmic correction formula is as follows

[0235]

[0236] The above basic process of self-calibration and self-test function can be simply summarized as follows: Figure 7 Schematic diagram of the drive calibration and self-test process.

[0237] The above scheme is experimentally verified below. The driver chip model is STM32F103C8T6, the current amplifier chip is Texas Instruments INA240, the motor model is GB6010, the rated voltage is 24V, the rated current is 0.75A, the rated maximum torque is 0.8NM, the line resistance is 7.1Ω, the phase-to-phase inductance is 12.62mH, the torque constant is 0.6NM / A, and the rotor inertia is 748g·cm 2 The number of pole pairs is 14. A 14-bit AS5048A magnetic encoder is used to provide rotor position feedback. The three-channel center-aligned PWM carrier frequency is set to 20 kHz. A JLink-V9 is used to collect data during the algorithm's operation, with a sampling rate of 10 kHz.

[0238] The first is the basic function of the drive speed loop control, its basic structure is as follows Figure 9 As shown in the block diagram of the speed loop servo system, in the quadrature axis PI controller, the proportional coefficient is 0.5 and the integral coefficient is 100.0; in the direct axis PI controller, the proportional coefficient is 0.5 and the integral coefficient is 100.0; in the speed loop PID controller, the proportional coefficient is 1.0 and the integral coefficient is 2.0. Figure 11 According to the speed step curve response diagram, the target speed is set to 10rad / s. It can be seen that the motor speed enters a steady state after about 50ms.

[0239] The second is the basic function of the driver position loop control, its basic structure is as follows Figure 8 As shown in the block diagram of the position loop servo system, the quadrature-axis PI controller has a proportional coefficient of 0.5 and an integral coefficient of 100.0; the direct-axis PI controller has a proportional coefficient of 0.5 and an integral coefficient of 100.0; and the velocity loop PID controller has a proportional coefficient of 1.0 and an integral coefficient of 10.0. The position loop is pure P control with a proportional coefficient of 5.0. The position step response is tested with a target position of 10 rad. The experimental results show that Figure 10 It can be seen from the position loop step response curve that the position response reaches a steady state without overshoot after about 125ms.

[0240] For the flexible control of the variable stiffness coefficient of the driver, a step response test is performed based on position impedance control when the motor is in no-load state. When the damping coefficient B' is fixed at 0.25 and the stiffness coefficient K' is set at 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, and 0.4 respectively, the position step response curve and the step response of the motor torque are recorded. Figure 13 The torque step response curves under different stiffness parameters with a load of 2KG show that increasing the stiffness coefficient will increase the peak torque, which is consistent with the basic dynamic characteristics of impedance control. Figure 12 The position step response curves under different stiffness parameters with a 2KG load show that when the damping coefficient remains unchanged, gradually increasing the stiffness coefficient will make the position response speed faster and the rise time shorter, which is in line with the basic characteristics of impedance control.

[0241] For the flexible control of the driver with variable damping coefficient, the step response test is performed based on position impedance control in the no-load state of the motor. When the stiffness coefficient K' is fixed at 0.25 and the damping coefficient B' is set at 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, and 0.4 respectively, the position step response curve and the motor torque step response are recorded respectively. Figure 15 The torque step response curves under different damping parameters with a load of 2KG show that increasing the damping coefficient will reduce the peak torque and prolong the adjustment time, which is consistent with the dynamic characteristics of impedance. Figure 14The position step response curves for different damping parameters with a 2 kg load are shown. It can be seen that when the stiffness coefficient remains unchanged, the damping coefficient gradually increases and the position response gradually slows down, which is consistent with the basic characteristics of the impedance control model.

Claims

1. A feedforward decoupling variable damping robot joint compliant drive control method, characterized in that: The method includes the following steps: (1) Under the control system framework of the current loop impedance, the target quadrature axis current I is calculated according to the set damping coefficient B and stiffness coefficient K. q_target ; The target torque τ of the impedance control system of the current loop impedance is ignoring the acceleration term. d The calculation method is For a BLDC motor, the electromagnetic torque T e With the quadrature axis current I q Proportional to the electromagnetic torque T output by the motor e The target value is set as impedance By controlling the quadrature axis current I q_target , to achieve the dynamic effect of simulating impedance; Where PP is the number of pole pairs, λ f is the rotor magnetic flux, B is the damping coefficient, K is the stiffness coefficient; X is the current position θ current and the target position θ target The difference, is the first-order difference of X; (2) Sample the three-phase current of the motor to obtain I a , I b The two-phase current, the other phase current I is obtained by Kirchhoff node current equation c ; (3) Will I a , I b , I c The three-phase current is transformed by Clark to obtain the current I in the stator α-β coordinate system α , I β ; (4) The current I in the stator α-β coordinate system α , I β After Park transformation, the direct axis current I is obtained d and the quadrature axis current I q ; (5) Calculate the direct axis current I d and the target direct-axis current I d_target Error; calculate the quadrature axis current I q and the target quadrature-axis current I q_target The error, I d_target Set to 0, input the error to the direct axis PI controller and the quadrature axis PI controller respectively; get the output direct axis voltage U' d and quadrature axis voltage U' q ; (6) Correcting the quadrature-axis voltage and the direct-axis voltage through a feedforward decoupling controller; The feedforward decoupling controller includes a direct-axis decoupling controller and a quadrature-axis decoupling controller, and the direct-axis decoupling control includes the following steps: 1) The rotor angular velocity ω obtained by obtaining the first-order differential of the rotor position through the magnetic encoder is multiplied by the stator inductance L in the motor parameters s Multiply it by the quadrature axis current I q , we can get the direct-axis voltage compensation term ωL s I q ; 2) The direct axis voltage U' d Subtract the direct-axis voltage compensation term to obtain the decoupled direct-axis voltage U d ; The quadrature axis decoupling control includes the following steps: 1) stator inductance L s Multiply by the direct axis current I d Add the back electromotive force coefficient K e , and finally multiply it by the rotor angular velocity ω to obtain the quadrature axis voltage compensation ω(L s I d +K e ); 3) The quadrature axis voltage U′ q Add the quadrature axis voltage compensation to obtain the decoupled quadrature axis voltage U q ; (7) Using the decoupled direct axis voltage U d and the decoupled quadrature axis voltage U q Calculate the space voltage vector U out and space voltage vector U out The corresponding electrical angle θ out ; (8) Through the space voltage vector U out The electrical angle θ out Calculate and determine the current rotor sector, and according to the space voltage vector U out , sector S and space voltage vector U out The electrical angle θ out , calculate the action time of each vector in the sector; (9) According to the action time of the vector in the sector and the sector where the rotor is currently located, the duty cycle of each channel of the three-channel center-aligned complementary PWM is calculated, and the MOS switch is driven by the three-phase inverter circuit to drive the motor.

2. A feedforward decoupling variable damping robot joint compliant drive control method according to claim 1, characterized in that: The calculated space voltage vector U out and space voltage vector U out The corresponding electrical angle θ out , which calculates the space voltage vector U out and space voltage vector U out The corresponding electrical angle θ out The steps include: 1) Determine the direct axis voltage U d and quadrature axis voltage U q Whether it exceeds the range of the maximum linear controllable amplitude, the direct axis voltage U d and quadrature axis voltage U q Calculated as follows 2) Calculate the space voltage vector U out The magnitude and voltage vector U out The electrical angle θ of the vector out ; If the direct axis voltage U is input d If it is not 0, the space voltage vector is calculated as follows: Space voltage vector U out Electrical angle θ out Updated to: If the input direct axis voltage is 0, then U out The parameter calculation method is as follows, where U dc is the bus voltage At this time, the space voltage vector U out Electrical angle θ out Updated to: Where a = mod (θ, 2π).

3. The feedforward decoupling variable damping robot joint compliant drive control method according to claim 1, characterized in that: The current sector of the rotor is determined based on the space voltage vector U out The electrical angle θ out Obtain the current sector of the rotor and directly calculate the sector through the quadrature-axis voltage, direct-axis voltage and rotor electrical angle θ.

Images