Humanoid robot integrated joint anti-disturbance control method and control system

Abstract

The application discloses a humanoid robot integrated joint anti-disturbance control method and a control system, wherein the control method controls a permanent magnet synchronous motor of the humanoid robot through a current loop vector control algorithm, and specifically comprises the following steps: normalizing each step of the current loop vector control algorithm; solving trigonometric functions in calculation formulas of each step after the normalization through a table lookup assisted numerical fitting method; and improving the conventional operation processing of the trigonometric functions which occupy a large amount of hardware resources into the table lookup assisted numerical fitting method, so that the solving speed and the solving accuracy can be maximally improved, the table lookup depth and the table lookup breadth can be reduced, the trigonometric functions can be solved quickly and accurately, the occupation of the hardware resources can be reduced as much as possible, the high-speed operation of the current loop vector control algorithm can be realized, and the performance requirements of the humanoid robot integrated joint high dynamic anti-disturbance can be met.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of humanoid robot integrated joint control, in particular to a humanoid robot integrated joint anti-disturbance control method and control system. BACKGROUND

[0002] Humanoid robots will play an important role in military, medical, rescue and modern manufacturing industry, and can meet the requirements of task diversity and environmental complexity. The motion accuracy and anti-disturbance ability of humanoid robot integrated joints are crucial to its working effect and stability, and the joint driving system is the core component of humanoid robot motion. With the increasing demand for fast positioning, accurate grasping and motion stability of the end of humanoid robot, higher requirements are put forward for the anti-disturbance control technology of integrated joints. Therefore, how to improve the motion stability of humanoid robot integrated joints and the anti-disturbance control method has become the focus of research. SUMMARY

[0003] One of the technical problems to be solved by the present application is to overcome the defects of the above related technologies, and to provide a humanoid robot integrated joint anti-disturbance control method and control system with fast operation speed, high calculation accuracy, high system anti-disturbance and reliability, and low hardware cost.

[0004] One of the technical solutions adopted by the present application to solve the technical problem is: a humanoid robot integrated joint anti-disturbance control method, which controls the permanent magnet synchronous motor of the humanoid robot through a current loop vector control algorithm, specifically including: normalizing each step of the current loop vector control algorithm; solving the trigonometric functions in the calculation formula of each step after normalization by a table-aided numerical fitting method.

[0005] As a preferred, the table contains 900 arrays in total, each array saves an angle value, a corresponding sine value and a corresponding cosine value, the angle value ranges from 0° to 89.9° with a step of 0.1°; the solving process of the table-aided numerical fitting method is: decomposing the angle value to be solved θ into a part θa before the first digit of the decimal part and a part θb after the first digit of the decimal part, i.e. θ = θa + θb; then finding out the sine and cosine values of θa in the table, sinθa and cosθa; finally calculating the sine and cosine values of the angle value θ by a fitting formula, the fitting formula is:

[0006] The fitting parameters k1 and k2 in the fitting formula will be adjusted according to the angle difference θb between the angle value θ and the previous angle value θa in the table, so that better fitting accuracy can be obtained.

[0007] Compared with related technologies, the humanoid robot integrated joint anti-disturbance control method of this application has the following advantages: the conventional calculation of trigonometric functions, which consumes a lot of resources, is improved to a numerical fitting method assisted by table lookup, which can maximize the solution speed and accuracy, while reducing the lookup depth and breadth. Thus, while ensuring fast and high-precision solution of trigonometric functions, the occupation of hardware resources is minimized, and the current loop vector control algorithm can run at high speed to meet the high dynamic anti-disturbance performance requirements of the humanoid robot integrated joint.

[0008] As a preferred option, the calculation formulas for each step of the normalized current loop vector control algorithm are as follows:

[0009] (1) Clarke transform:

[0010] c1 = 1; c2 = 0; x1 = i a x2 = 0;

[0011]

[0012] i sa =y1;i sβ =y2;

[0013] (2) Park transformation:

[0014] c1=cos(θ e c2 = sin(θ) e ); x1 = i sa x2 = i sβ ;

[0015] c3=-sin(θ e c4 = cos(θ) e ); x3 = i sa x4 = i sβ ;

[0016] i sd =θ e1 i sq =θ e2 ;

[0017] (3) PI controller:

[0018] c1 = K d new c2 = K d old x1 = e id (k); x2=e id (k-1);

[0019] c3 = K q_new c4 = K q_old x3 = e iq (k); x4 = eiq (k-1) ;

[0020] V sd (k) = V sd (k-1) + θ e1 ;

[0021] V sq (k) = V sq (k-1) + θ e2 ;

[0022] (4) Park inverse transform:

[0023] c1 = cos(θ e ); c2 = -sin(θ e ); x1 = V sd ; x2 = V sq ;

[0024] c3 = sin(θ e ); c4 = cos(θ e ); x3 = V sd ; x4 = V sq ;

[0025] V sa = θ e1 ; V sβ = θ e2 ;

[0026] (5) Space vector pulse width modulation space sector:

[0027]

[0028] V a = V sβ ; V b = θ e1 ; V c = θ e2 ;

[0029] (6) Calculate SVPWM each phase theoretical switch time:

[0030]

[0031] X = θ e1 + θ e2 ; Y = θ e1 ; Z = θ e2 ;

[0032] (7) Calculate SVPWM each phase actual pulse width modulation time:

[0033]

[0034] t 1SAT = θ e1 ; t 2SAT = θ e2 ;

[0035] wherein, is obtained by independent divider operation, θ e1 is the motor end electrical angle value obtained by analyzing the first position and speed sensing module, and θ e2 is the load end electrical angle value obtained by analyzing the second position and speed sensing module.

[0036] Another technical solution adopted by the application to solve the technical problem is: a humanoid robot integrated joint anti-disturbance control system, comprising a soft core processor, used to complete the real-time control task of the outer ring composed of a speed loop and a position loop with a lower control beat, responsible for each communication module of the upper computer and real-time monitoring of the entire control system; a space vector control slave system, used to complete the current loop vector control algorithm in the above-mentioned humanoid robot integrated joint anti-disturbance control method, wherein the space vector control slave system is mounted on the soft core processor through a bus;

[0037] The space vector control slave system comprises a position loop module, a speed loop module, a current loop module based on FPGA, an intelligent power module, a permanent magnet synchronous motor, a motor end encoder, a first position and speed sensing module, a precision harmonic reducer, a precision harmonic reducer load, a load end multi-turn absolute value encoder, a second position and speed sensing module, and a function module, wherein the function module receives the motor end rotation angle and speed fed back by the first position and speed sensing module and the load end rotation angle and speed fed back by the second position and speed sensing module, and feeds back the rotation angle feedback value to the position loop module and feeds back the speed feedback value to the speed loop module through comparison and operation.

[0038] Compared with related technologies, the humanoid robot integrated joint anti-disturbance control system of the application has the following advantages: the current loop disturbance suppression control method combined with adaptive sliding mode can not only adaptively adjust the control parameters, but also can maintain good control effect even in the case of change of the control object or change of the working environment. Through application of FPGA parallel operation and time division multiplexing technology with the characteristics of accurate matching algorithm, the excellent performance of completing the current loop vector control algorithm within 1.6us is achieved. By adaptively adjusting the control parameters to offset the external interference and noise received by the control system, the stability and reliability of the system are improved, and finally the real integrated joint anti-disturbance control is realized, which can not only improve the control performance, but also effectively save the hardware cost. Various peripherals can be mounted on the soft core processor through a bus, have good expansion and performance adaptation performance, can be compatible with various digital encoders, and support various communication protocols.

[0039] As preferred, the FPGA-based current loop vector control module comprises an operation core unit, a state machine, a parameter selection unit, an intermediate variable memory and an internal connection module; the operation core unit is a multiply-add calculation unit composed of four independent multipliers and two parallel adders, used to complete two basic operation formulas shared by each step of the current loop vector control algorithm: y1=c1x1+c2x2.

[0040] y2=c3x3+c4x4

[0041] High-speed parallel implementation and time division multiplexing not only effectively improve the entire current loop vector control operation speed, but also significantly reduce the demand for precious multiplier resources of the FPGA. BRIEF DESCRIPTION OF DRAWINGS

[0042] Figure 1 is the architecture diagram of the SoC-based humanoid robot integrated joint anti-disturbance control system of the present application.

[0043] Figure 2 is the control system block diagram of the space vector control slave system of the present application.

[0044] Figure 3 is the functional unit block diagram of the FPGA-based current loop vector control module of the present application.

[0045] Figure 4 is the state timing diagram of the state machine of the FPGA-based current loop vector control module of the present application.

[0046] Figure 5 is the actual timing diagram of the current loop vector control algorithm of the present application.

[0047] Figure 6 is the hardware resource consumption diagram of the control system of the present application.

[0048] Figure 7 is the current tracking diagram of the dq axis of the permanent magnet synchronous motor based on the current loop vector control algorithm of the present application.

[0049] Figure 8 is the partial enlarged view of the current tracking diagram of the dq axis of the permanent magnet synchronous motor based on the current loop vector control algorithm of the present application.

[0050] Figure 9 is the position tracking diagram based on the current loop vector control algorithm of the present application.

[0051] Figure 10 is the speed tracking diagram based on the current loop vector control algorithm of the present application.

[0052] Figure 11is the U, V phase current graph of the acceleration and deceleration time based on the current loop vector control algorithm of the application. DETAILED DESCRIPTION

[0053] Firstly, those skilled in the art should understand that the embodiments are only used to explain the technical principles of the embodiments of the application, and are not intended to limit the protection scope of the embodiments of the application. Those skilled in the art can make adjustments as needed in order to adapt to specific application occasions.

[0054] The application will be further described in detail below in combination with specific embodiments.

[0055] The humanoid robot integrated joint is composed of eight core components, and the eight core components are respectively a motor end absolute value encoder (E1), a load end multi-turn absolute value encoder (E2), a frameless permanent magnet synchronous motor (PMSM), a precision harmonic reducer (Reducer), a direct current motor driver, a temperature sensor, a friction type brake retainer and a torque sensor.

[0056] In the preferred embodiment, according to the mechanical structure and the function of the humanoid robot, in order to improve the anti-disturbance performance of the vector controller and reduce the hardware cost of the servo control, a humanoid robot integrated joint anti-disturbance control system based on SoC as shown in Figure 1 In structure, the whole control system can be regarded as a dual-core processing system of master-slave structure: mainly including a Nios II soft core processor as a master processing system, used to complete the real-time control task of the outer loop composed of the speed loop and the position loop with low control beat, responsible for the communication module of the upper computer and the real-time monitoring of the whole control system; a high-speed running space vector control slave system, used to complete the current loop vector control algorithm; the space vector control slave system and various peripherals are mounted on the Nios II soft core processor through the high-speed Avalon bus.

[0057] The space vector control slave system as shown in Figure 2 includes a position loop module, a speed loop module, a current loop module based on FPGA, an intelligent power module (IPM), a permanent magnet synchronous motor (PMSM), a motor end absolute value encoder (E1), a first position and speed sensing module, a precision harmonic reducer, a precision harmonic reducer load (Load), a load end multi-turn absolute value encoder (E2), a function module (Func1) and a second position and speed sensing module. These modules constitute a servo motion three-closed-loop feedback control.

[0058] The position loop is the outermost element of the space vector control system, consisting of the motor rotor angular position module P_C, used to control the angular position θ of the motor rotor. It compares the position with a setpoint θ. r and feedback value θ m The difference between e θ e θ Input position loop. The position loop serves as the input signal for the velocity loop.

[0059] The speed loop is located within the position loop and consists of the motor rotor speed module S_C, which is responsible for controlling the motor rotor speed ω. It controls the speed ω by comparing it with a set value ω. ref and feedback value ω m The difference between e ω e ω Input speed loop.

[0060] The outer loop, consisting of the position loop and the speed loop, will control the given torque current i. Sqref Co-excitation current i Sdref The input is given to the current loop module.

[0061] The current loop is the innermost link of the space vector control system, responsible for controlling the motor current. The vector control structure of the current loop is mainly composed of the Clarke transform module, the Park transform module, the PI controller module, the Park inverse transform module, and the SVPWM module in sequence.

[0062] The functional module receives the motor end rotation angle and speed fed back by the first position and speed sensing module, and the load end rotation angle and speed fed back by the second position and speed sensing module, and compares and calculates the rotation angle feedback value θ. m Feedback is sent to the position loop module, transmitting the velocity feedback value ω. m Feedback is sent to the speed loop module.

[0063] The intelligent power module integrates logic, control, detection, and protection circuits, combining power switching devices and drive circuits. It also integrates fault detection circuits for overvoltage, overcurrent, and overheating, and can detect the current signal i. a and i b Feedback is sent to the current loop module.

[0064] The permanent magnet synchronous motor is a frameless permanent magnet synchronous servo motor. Connected to the motor end is an absolute encoder E1 (Encoder1), which feeds back the rotation angle and speed of the motor end to the function module (Func1) through the first position and speed sensing module. Connected to the load end is a load-side multi-turn absolute encoder E2 (Encoder2), which feeds back the rotation angle and speed of the load end to the function module through the second position and speed sensing module.

[0065] The function module compares and operates the rotation angle θ m1 and the rotation speed ω m1 of the motor end with the rotation angle θ m2 and the rotation speed ω m2 of the load end, and finally feeds back the rotation angle feedback value θ m to the position loop module P_C and feeds back the rotation speed ω m feedback value to the speed loop module S_C.

[0066] The vector control is realized by a special multiplier on the FPGA. θ e，ω in the figure respectively represents the motor rotation angle and the motor rotation speed; P and I are respectively the proportional and integral adjusters of the current loop. In the current loop vector control method, the current loop vector control unit first collects the phase current, and performs flux decoupling through Clarke and Park transformation, so as to realize linear control of the motor torque through i sqref after adopting rotor flux oriented control of i sdref = 0. The current loop vector control algorithm is mainly composed of Clarke transformation module, Park transformation module, PI controller module, Park inverse transformation module and SVPWM module in sequence. The calculation formulas of each part are shown in equations (1)-(7).

[0067] 1) Clarke transformation

[0068] i sa =i a

[0069]

[0070] Wherein, i a and i b are the currents flowing through the U phase and the V phase of the permanent magnet synchronous motor respectively.

[0071] 2) Park transformation:

[0072] i sd =i sα cos(θ e )+i sα sin(θ e )

[0073] i sq =-i sα sin(θ e )+i sβ sin(θ e )(2)

[0074] Wherein, θ e is the motor rotation angle.

[0075] 3) PI controller (implemented by incremental type)

[0076] e id (k) = i sdref -i sd

[0077] e iq (k) = i sqref -i sq

[0078] V sd (k) = V sd (k-1) + K d_new *e id (k) + K d_old x e id (k-1)

[0079] V sq (k) = V sq (k-1) + K q_new *e iq (k) + K q_old x e iq (k-1) (3)

[0080] where K d_new , K d_old , K q_new , K q_old are determined by the parameters K p , K i of the PI regulator; V sd (k), V sq (k), e id (k), e iq (k) are the current control beat values; V εd (k-1), V sq (k-1), e id (k-1), e iq (k-1) are the values of the previous control beat.

[0081] 4) Park Inverse Transformation:

[0082] V sa = V sd x cos(θ e ) - V sq x sin(θ e )

[0083] V sβ = V sd x sin(θ e ) + V sq x cos(θ e ) (4)

[0084] 5) Space sector of SVPWM:

[0085] V a = V sβ

[0086]

[0087] Sect No = A + 2B + 4C;

[0088] 6) Calculate the theoretical switching time of each phase of SVPWM:

[0089]

[0090] Where, PWMPRD is the pulse width modulation period, V dc is the bus voltage.

[0091] 7) Calculate the actual pulse width modulation time of each phase of SVPWM:

[0092]

[0093] t bon = t aon + t 1s

[0094] t con = t bon + t 2s (7)

[0095] taon, tbon and tcon are the conduction times of U, V and W phases respectively, and their specific work is shown in Table 1:

[0096] Sect_No 1 2 3 4 5 6 CMPA [cat bon ]]> <![CDATA[t aon ]]> <![CDATA[t aon ]]> [cat con ]]> [cat con ]] <![CDATA[t bon ]]> CMPB <![CDATA[t aon ]]> <![CDATA[t con ]]> <![CDATA[t bon ]]> <![CDATA[t bon ]]> <![CDATA[t aon ]]> [cat con ]] CMPC [CAT con ]] [CAT bon ]]> [cat con ]]> [cat aon ]]> [CAT bon ]]> [cat aon ]]>

[0097] Table 1 Modulation time of each phase in different sectors

[0098] Through analysis, it is found that the main operation of Clarke transformation module, Park transformation module, PI controller module and Park inverse transformation module can be completed by a multiplication-addition calculation unit composed of four independent multipliers and two parallel adders, but the calculation of SVPWM module is relatively complex and cannot be completed by the above unit once. The calculation process of SVPWM can be divided according to the calculation unit, and the SVPWM module can be divided into SVPWM space sector calculation, SVPWM theoretical switch calculation and SVPWM actual pulse width modulation calculation. Therefore, it can be seen that the basic calculation mode of each step is as follows:

[0099] y1 = c1 x x1 + c2 x x2

[0100] y2 = c3 x x3 + c4 x x4 (8)

[0101] To better implement the calculation in each step on the FPGA, the algorithm of each step is normalized, as shown in the following equations (9)-(15).

[0102] (1) Clarke transformation:

[0103] c1 = 1; c2 = 0; x1 = i a ; x2 = 0;

[0104]

[0105] i sa = y1; i sβ = y2; (9)

[0106] (2) Park transformation:

[0107] c1 = cos(θ e ); c2 = sin(θ e ); x1 = i sa ; x2 = i sβ ;

[0108] c3 = -sin(θ e ); c4 = cos(θ e ); x3 = i sa ; x4 = i sβ ;

[0109] i sd = θ e1 ; i sq = θ e2 ; (10)

[0110] (3) PI controller (implemented in incremental form):

[0111] c1 = K d_new ; c2 = K d_old ; x1 = e id (k); x2 = e id (k-1);

[0112] c3 = K q_new ; c4 = K q_old ; x3 = e iq (k); x4 = e iq (k-1);

[0113] V sd (k) = V sd (k-1) + θ e1 ;

[0114] V sq(k) = V sq (k - 1) + θ e2 ; (11)

[0115] (4) Park inverse transformation:

[0116] c1= cos(θ e ); c2= -sin(θ e ); x1= V sd ; x2= V sq ;

[0117] c3= sin(θ e ); c4= cos(θ e ); x3= V sd ; x4= V sq ;

[0118] V sa = θ e1 ; V sβ = θ e2 ; (12)

[0119] (5) Space vector pulse width modulation (SVPWM) space sector:

[0120]

[0121] V a = V sβ ; V b = θ e1 ; V c = θ e2 ; (13)

[0122] Logical judgment and Sect No are implemented in additional logic units.

[0123] (6) Calculate SVPWM each phase theoretical switch time:

[0124]

[0125] (7) Calculate SVPWM each phase actual pulse width modulation time:

[0126]

[0127] t 1SAT = θ e1 t 2SAT = θ e2 ; (15)

[0128] wherein, is operated by an independent divider. t aon , t bon , tcon Logical judgments are performed separately.

[0129] (8) Treatment of trigonometric functions

[0130] Conventional trigonometric function calculations consume significant FPGA resources, and the calculations of trigonometric functions cannot be avoided in the processes described in (1)-(7). To minimize FPGA hardware resource consumption while ensuring fast and high-precision trigonometric function solving, this invention designs a numerical fitting solution method based on fast lookup table assistance. This method maximizes the solution speed and accuracy while reducing the lookup depth and breadth. The fast lookup table-assisted numerical fitting solution method employs a specially structured sine and cosine value table containing 900 arrays. Each array stores the angle value, the corresponding sine value, and the corresponding cosine value. The angle value ranges from 0° to 89.9° with a step size of 0.1°. When solving, first decompose the angle value θ into the part before the first decimal place θa and the other parts after the first decimal place θb, that is, θ = θa + θb; then find the sine and cosine values ​​of θa sinθa and cosθa in the table respectively; finally calculate the sine and cosine values ​​of θ by fitting formula (16).

[0131]

[0132] The core computing unit of vector control, such as Figure 3 As shown. Since the current loop vector controller performs calculations sequentially according to the order of each step, the core unit of the calculation is time-division multiplexed under the control of the state machine, calculating each step sequentially until the complete vector control calculation of 7 steps in one cycle is completed. Among them, the state variable S[2..0] is generated by... Figure 4 The state machine timing diagram shows that state 0 is used for the conversion of the motor rotor angle to the electrical angle and for magnetic declination compensation. Triggered by the control cycle clock, state quantities S[2..0] corresponding to the seven steps of vector control are generated, along with latch signals for storing intermediate variables. The state quantities are used to control the selector to select the input corresponding to the current state to the fast basic arithmetic unit, completing the vector calculation for the current step. The falling edge of the latch signal latches the current calculation result to the register selected by the state quantity as the intermediate variable needed for the next cycle. For example, in state four, the C selector and the X selector will set c1 = cos(θ). e c2 = -sin(θ) e x1 = V sd x2 = V sq c3 = sin(θ) e c4 = cos(θ) e x3 = V sd x4 = V sq; so that at the end of state two we will have V sa = θ e1 ; V sβ = θ e2 ; latched into the corresponding register as the input for the next step, state 5.

[0133] To verify the control method and control system, a humanoid robot integrated joint anti-disturbance control system based on EP4C30F23 Altera FPGA is designed. The control system is divided into three layers. The first layer is the FPGA core board, the second layer is the peripheral device and signal conditioning layer, and the third layer is the power supply and power drive layer. The servo control takes the linear motor as the control object. The phase resistance of the linear motor is 3.4 Ω, the phase inductance is 10.8 Mh, and the maximum power is 850 W. The feedback encoder of the linear motor uses a 2500 line incremental encoder. The entire control system (Nios II plus current loop vector control unit) consumes 21041 logic gates and 417306 bit storage units, of which the current loop vector control unit consumes 9621 logic gates, as shown in Figure 5 .

[0134] When the current loop vector control unit is running, it needs to go through state 1-state 7 to complete a current loop vector control, as shown in Figure 6 . The high level part of signal 1 in the figure is its working time, and the grid is 500 ns, so the time to complete a current loop vector control is about 1.6 us, which is much smaller than 5 us. The signal conversion of the state machine is shown in signal 2 in the figure. State 0 starts from the falling edge, and state 7 starts from the last rising edge.

[0135] Figure 7- Figure 11 is the real-time experimental data collected when the linear motor is reciprocating. Among them, Figure 7 , Figure 8 shows the tracking performance of the current on the d-axis and q-axis under the control of the oriented magnetic field. From the figure, it can be seen that the id_cal obtained by decoupling the measured current and position is close to zero, and the Iq_cal can quickly track the reference value lq_ref, which shows that the control system has good linearity after flux decoupling, and the vector control has good dynamic performance. Figure 9 The position tracking curve and Figure 10 the speed tracking curve show that the servo motion control has good dynamic performance and repeat positioning accuracy. Figure 11 The real-time collected U, V phase current values during acceleration and deceleration show that the SVPWM in the vector controller has very smooth commutation performance, and also shows that the current sampling digital filter current noise has good suppression ability.

[0136] The above data show that on a low-end Altera Cyclone series FPGA, not only is a complete humanoid robot integrated joint control system implemented, but the current loop vector control algorithm can be completed in less than 1.6us.

[0137] It can be concluded that the high-speed current loop vector control unit based on the state machine effectively improves the dynamic performance of the control system, suppresses current noise, and interference between system control beats. The response speed of the control system is improved, the system vibration is weakened, and the anti-disturbance performance of the system is improved, which has a great application prospect in the field of high-precision control.

[0138] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited to this. Any person skilled in the art can easily think of changes or replacements within the technical scope disclosed in the present application, which should be covered within the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A humanoid robot integrated joint anti-disturbance control method, which controls a permanent magnet synchronous motor of a humanoid robot through a current loop vector control algorithm, specifically comprising: The steps of the current loop vector control algorithm are normalized; characterized in that the trigonometric functions in the calculation formula of the normalized steps are solved by a table-aided numerical fitting method; The table contains 900 arrays in total, each array storing an angle value, a corresponding sine value and a corresponding cosine value, the angle value ranging from 0° to 89.9° with a step of 0.1°; the table-aided numerical fitting method solving process is as follows: decompose the angle value to be solved θ into a part θa before the first digit of the decimal part and another part θb after the first digit of the decimal part, i.e. θ = θa + θb; then find the sine and cosine values of θa in the table; finally, calculate the sine and cosine values of the angle value θ by a fitting formula, the fitting formula being: ； The calculation formula of the steps of the current loop vector control algorithm after normalization is as follows: (1) Clarke transformation: (2) Park transformation: (3) PI controller: (4) Park inverse transformation: (5) SVPWM space sector: (6) Calculation of SVPWM phase theoretical switch time: (7) Calculation of SVPWM phase actual pulse width modulation time: wherein, is calculated by an independent divider, is the electrical angle value of the motor end obtained by analyzing the first position and speed sensing module, is the electrical angle value of the load end obtained by analyzing the second position and speed sensing module.

2. A humanoid robot integrated joint disturbance control system, characterized by, The soft core processor is used to complete the real-time control task of the outer loop composed of the low-speed speed loop and the position loop, is responsible for the communication with the upper computer and the real-time monitoring of the whole control system; the space vector control slave system is used to complete the current loop vector control algorithm in the human-shaped robot integrated joint anti-disturbance control method of claim 1, and the space vector control slave system is mounted on the soft core processor through a bus. The space vector control system comprises a position loop module, a speed loop module, a current loop module based on FPGA, an intelligent power module, a permanent magnet synchronous motor, a motor end encoder, a first position and speed sensing module, a precision harmonic reducer, a precision harmonic reducer end load, a load end multi-turn absolute value encoder, a function module and a second position and speed sensing module. m The function module receives the motor end rotation angle and speed fed back by the first position and speed sensing module and the load end rotation angle and speed fed back by the second position and speed sensing module, compares and calculates the rotation angle feedback value θ m And feeds back the speed feedback value ω m To the speed loop module.

3. The integrated joint disturbance rejection control system for humanoid robots according to claim 2, wherein The FPGA-based current loop module comprises an operation core unit, a state machine, a parameter selection unit, an intermediate variable memory and an internal connection module; the operation core unit is a multiply-add calculation unit composed of four independent multipliers and two parallel adders, and is used for completing two basic operation formulas shared by each step of the current loop vector control algorithm: .

Images