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

Abstract

The invention discloses a humanoid robot integrated joint anti-disturbance control method and system, the control method controls a permanent magnet synchronous motor of a humanoid robot through a current loop vector control algorithm, and the control method specifically comprises the following steps: normalizing each step of the current loop vector control algorithm; solving the trigonometric function in the calculation formula of each step after normalization processing through a table look-up assisted numerical fitting method; the conventional operation processing of a trigonometric function occupying relatively large hardware resources is improved into a value fitting method assisted by table lookup, so that the solving speed and the solving precision can be improved to the greatest extent, and meanwhile, the table lookup depth and the table lookup breadth are reduced; therefore, the occupation of hardware resources is reduced as much as possible while the trigonometric function is rapidly and accurately solved, and the high-speed operation of the current loop vector control algorithm is realized to meet the performance requirement of high-dynamic disturbance resistance of the integrated joint of the humanoid robot.

Description

Technical Field

[0001] The present invention relates to the field of integrated joint control of humanoid robots, and in particular to an anti-disturbance control method and a control system for integrated joints of humanoid robots. Background Art

[0002] Humanoid robots will play an important role in military, medical, rescue and modern manufacturing industries, and can meet the requirements of diverse tasks and complex environments. The motion accuracy and anti-disturbance ability of the integrated joints of humanoid robots are crucial to their working effect and stability. At the same time, the joint drive system is the core component of humanoid robot movement. With the increasing demand for rapid positioning, precise grasping and stable movement of the end of humanoid robots, higher requirements are placed on the anti-disturbance control technology of integrated joints. Therefore, how to improve the motion stability of the integrated joints of humanoid robots and their anti-disturbance control methods have become the focus of research. Summary of the invention

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

[0004] A technical solution adopted by the present invention 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 formulas of each step after the normalization through a numerical fitting method assisted by table lookup.

[0005] Preferably, the table contains a total of 900 arrays, each array stores the angle value, the corresponding sine value, and the corresponding cosine value, the angle value range is 0°-89.9°, and the step size is 0.1°; the table-assisted numerical fitting method solution process is: decompose the angle value θ to be solved into the part θa before the first decimal part and the other part θb after the first decimal part, that is, θ=θa+θb; then find out the sine and cosine values ​​of θa sinθa and cosθa in the table respectively; finally, calculate the sine and cosine values ​​of the angle value θ by the fitting formula, and the fitting formula is:

[0006] The fitting parameters k1 and k2 in the fitting formula will adjust themselves according to the angle difference θb between the angle value θ and the previous angle value θa in the distance table, so as to obtain better fitting accuracy.

[0007] Compared with the related technologies, the anti-disturbance control method of the integrated joints of the humanoid robot of the present application has the following advantages: the conventional trigonometric function operation processing that occupies a lot of resources is improved into a numerical fitting method assisted by table lookup, which can maximize the solution speed and solution accuracy, while reducing the table lookup depth and table lookup breadth, thereby minimizing the occupation of hardware resources while ensuring fast and high-precision solution of trigonometric functions, and realizing high-speed operation of the current loop vector control algorithm to meet the performance requirements of high dynamic anti-disturbance of the integrated joints of the humanoid robot.

[0008] Preferably, the calculation formula of each step of the current loop vector control algorithm after normalization is:

[0009] (1) Clarke transformation:

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

[0011]

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

[0013] (2) Park transform:

[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 sector where space vector pulse width modulation is located:

[0027]

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

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

[0030]

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

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

[0033]

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

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

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

[0037] The space vector control slave system includes a position loop module, a speed loop module, an FPGA-based current loop module, 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 machine-end load, a load-end multi-turn absolute encoder, a second position and speed sensing module and a functional module. The functional module receives the motor-end angle and speed feedback from the first position and speed sensing module and the load-end angle and speed feedback from the second position and speed sensing module, and feeds back the angle feedback value to the position loop module and the speed feedback value to the speed loop module through comparison and calculation.

[0038] Compared with the related technologies, the humanoid robot integrated joint anti-disturbance control system of the present application has the following advantages: the control method of current loop disturbance suppression combined with adaptive sliding mode can not only adjust the control parameters adaptively, but also maintain a good control effect even when the control object changes or the working environment changes. By applying FPGA parallel computing and time-division multiplexing technology that accurately matches the characteristics of the algorithm, the excellent performance of the current loop vector control algorithm is achieved within 1.6us. By adaptively adjusting the control parameters to offset the external interference and noise of the control system, the stability and reliability of the system are enhanced, and finally a true integrated joint anti-disturbance control is achieved, which can not only improve the control performance, but also effectively save hardware costs. Various peripherals can be mounted on the soft-core processor through the bus, with good expansion and performance adaptation performance, compatible with various digital encoders, and support for multiple communication protocols.

[0039] Preferably, the FPGA-based current loop vector control module includes a core operation unit, a state machine, a parameter selection unit, an intermediate variable memory and an internal connection module; the core operation unit is a multiplication and addition calculation unit composed of four independent multipliers and two parallel adders, which is used to complete the two basic operation formulas shared by each step of the current loop vector control algorithm: y1=c1×x1+c2×x2.

[0040] y2=c3×x3+c4×x4

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

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

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

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

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

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

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

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

[0049] Figure 8 It is a partial enlarged diagram 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] Fig. 9 It is a position tracking diagram based on the current loop vector control algorithm of the present application.

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

[0052] Fig.11It is a U and V phase current diagram during acceleration and deceleration collected in real time based on the current loop vector control algorithm of the present application. DETAILED DESCRIPTION

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

[0054] The present invention is further described in detail below with reference to specific embodiments.

[0055] The integrated joint of the humanoid robot consists of eight core components, namely the motor-end absolute encoder (E1), the load-end multi-turn absolute encoder (E2), the frameless permanent magnet synchronous motor (PMSM), the precision harmonic reducer (Reducer), the DC motor driver, the temperature sensor, the friction brake retainer and the torque sensor.

[0056] This preferred embodiment, based on the mechanical structure and implementation functions 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. Figure 1 The SoC-based humanoid robot integrated joint anti-disturbance control system shown in the figure can be regarded as a dual-core processing system with a master-slave structure in terms of structure. It mainly includes a NiosⅡ soft-core processor as the main processing system, which is used to complete the outer loop real-time control task composed of the speed loop and position loop with a lower control beat, and is responsible for the various communication modules of the host computer and the real-time monitoring of the entire control system; a high-speed space vector control slave system is used to complete the current loop vector control algorithm; the space vector control slave system and various peripherals are mounted on the NiosⅡ soft-core processor through the high-speed Avalon bus.

[0057] The space vector control system Figure 2 The figure shows 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), an absolute encoder at the motor end (E1), a first position and speed sensing module (position and speed sensing), a precision harmonic reducer, a load at the precision harmonic reducer end (Load), a multi-turn absolute encoder at the load end (E2), a function module (Func1) and a second position and speed sensing module. These modules constitute the three-closed-loop feedback control of the servo motion.

[0058] The position loop is the outermost link in the space vector control slave system, which is composed of the motor rotor angular position module P_C and is used to control the motor rotor angular position θ. It compares the set value θ r and feedback value θ m The difference between θ , e θ Input position loop. The position loop serves as the input signal of the speed loop.

[0059] The speed loop is located within the position loop and is composed of a motor rotor speed module S_C, which is responsible for controlling the motor rotor speed ω. It compares the set value ω ref and feedback value ω m The difference between ω , e ω Enter the speed loop.

[0060] The outer loop composed of position loop + speed loop converts the given torque current i Sqref The same excitation current i Sdref Input to the current loop module.

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

[0062] The functional module receives the motor end rotation angle and speed feedback from the first position and speed sensing module and the load end rotation angle and speed feedback from the second position and speed sensing module, and converts the rotation angle feedback value θ into m Feedback to the position loop module, the speed feedback value ω m Feedback to the speed loop module.

[0063] The intelligent power module integrates logic, control, detection and protection circuits, not only integrating power switch devices and drive circuits. It also integrates overvoltage, overcurrent and overheating fault detection circuits, and can detect current signals i a and i b Feedback to the current loop module.

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

[0065] The functional module converts the rotation angle θ of the motor end m1 , speed ω m1 Angle θ with the load end m2 , speed ω m2 Compare and calculate, and finally feedback the angle value θ m Feedback to the position loop module P_C, the speed ω m The feedback value is fed back to the speed loop module S_C.

[0066] Vector control is implemented by a dedicated multiplier on the FPGA. e，ω Represents the motor angle and motor speed respectively; P and I are the proportional and integral regulators of the current loop respectively. 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 transformations, so as to achieve sdref = 0 after the rotor flux directional control, through i sqref To linearly control the motor torque. 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 Transform

[0068] i sa =i a

[0069]

[0070] Among them, i a ,i b They are the currents flowing through the U-phase and V-phase of the permanent magnet synchronous electrode respectively.

[0071] 2) Park Transform:

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

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

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

[0075] 3) PI controller (implemented using incremental method)

[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 ×e id (k-1)

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

[0080] Among them, K d_new , K d_old , K q_new , K q_old is the parameter K of the PI regulator p , K i Decision; V sd (k), V sq (k), e id (k), e iq (k) is the current control beat value; V εd (k-1), V sq (k-1), e id (k-1), e iq (k-1) is the value of the previous control beat.

[0081] 4) Park inverse transform:

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

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

[0084] 5) Space sector where space vector pulse width modulation (SVPWM) is located:

[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 time of U, V and W phases respectively. The specific operation is shown in Table 1:

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

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

[0098] Through analysis, it is found that the main operations of Clarke transformation module, Park transformation module, PI controller module, and Park inverse transformation module can be completed by a multiplication and addition calculation unit composed of four independent multipliers and two parallel adders, but the calculation of the SVPWM module is relatively complex and cannot be completed by the above units in one operation. The calculation process of SVPWM is 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×x1+c2×x2

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

[0101] In order 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 transform:

[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 using incremental method):

[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 transform:

[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 sector where space vector pulse width modulation (SVPWM) is located:

[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 the theoretical switching time of each phase of SVPWM:

[0124]

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

[0126]

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

[0128] in, It is operated by an independent divider. aon ,t bon ,tcon And logical judgment are calculated independently.

[0129] (8) Processing of trigonometric functions

[0130] Conventional trigonometric function calculations occupy a large amount of FPGA resources, and the calculation of trigonometric functions cannot be avoided in the above (1)-(7) processes. In order to minimize the occupation of FPGA hardware resources while ensuring fast and high-precision solutions to trigonometric functions, the present invention designs a numerical fitting solution method based on fast table lookup assistance. The fast table lookup-assisted numerical fitting solution method can maximize the solution speed and solution accuracy while reducing the table lookup depth and table lookup breadth. The fast table lookup-assisted numerical fitting solution method designs a special structure of the sine and cosine value table, which contains a total of 900 arrays, each of which stores the angle value, the corresponding sine value, and the corresponding cosine value. The angle value range is 0°-89.9° and the step size is 0.1°. When solving, first decompose the angle value θ into the part before the first decimal place θa and the other part θb after the first decimal place, that is, θ=θa+θb; then find out 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 is as follows: Figure 3 As shown. Since the current loop vector controller performs operations in sequence according to the order of each step, the core operation unit is time-division multiplexed under the control of the state machine, and calculates each step in sequence until the complete vector control calculation of 7 steps in one beat is completed. The state quantity S[2..0] is Figure 4 As shown in the state machine timing, state 0 is used for the conversion of the motor rotor angle to the electrical angle and the compensation of the magnetic deflection angle. Under the triggering of the control beat clock, the state quantity S[2..0] corresponding to the 7 steps of vector control and the latch signal for storing the intermediate variable are generated. The state quantity is used to control the selector to select the input corresponding to the current state to the fast basic operation unit to complete the vector calculation of the current step. The latch signal falls and latches the current calculation result to the register selected by the state quantity as the intermediate variable required for the next beat. For example, in state four, the C selector and the X selector will c1=cos(θ e ); c2 = -sin(θ e ); x1 = V sd ; x2=V sq ; c3 = sin(θ e ); c4 = cos(θ e ); x3 = V sd ; x4 = V sq; At the end of state 2, V sa =θ e1 ; V sβ =θ e2 ; Latched into the corresponding register as the calculation input of the next step state 5.

[0133] The above control method and control system are verified, and an integrated joint anti-disturbance control system for a humanoid robot 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 equipment 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.8Mh, the maximum power is 850W, and the feedback encoder of the linear motor uses a 2500-line incremental encoder. The entire control system (NiosΠ 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. Figure 5 shown.

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

[0135] Figure 7-Figure 11 It is the experimental data collected in real time when the linear motor is making reciprocating motion. Figure 7 , Figure 8 It shows the tracking performance of the current on the d-axis and q-axis under directional magnetic field control. It can be seen from the figure that id_cal obtained after decoupling the measured current and position is close to zero, and 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. Fig. 9 Position tracking curve and Fig.10 The speed tracking curve shows that the servo motion control has good dynamic performance and repeatability. Fig.11 The real-time acquisition of the U and V phase current values ​​during acceleration and deceleration shows that the SVPWM in the vector controller has very smooth commutation performance, and also shows that the current sampling digital filter has a good ability to suppress current noise.

[0136] The above data show that on a mid-to-low-end Altera Cyclone series FPGA, not only a complete humanoid robot integrated joint control system is implemented, but the current loop vector control algorithm can also 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. It improves the response speed of the control system, weakens system vibration, and improves the anti-disturbance performance of the system, and has great application prospects in the field of high-precision control.

[0138] The above is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application shall be based on the protection scope of the claims.

Claims

1. 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 comprising: Each step of the current loop vector control algorithm is normalized; the characteristic is that the trigonometric functions in the calculation formulas of each step after the normalization are solved by a numerical fitting method assisted by table lookup.

2. The method for anti-disturbance control of an integrated joint of a humanoid robot according to claim 1, characterized in that: The table contains 900 arrays in total, each of which stores the angle value, the corresponding sine value and the corresponding cosine value. The angle value ranges from 0° to 89.9°, and the step length is 0.1°. The table-assisted numerical fitting method solves the problem by decomposing the angle value θ to be solved into the part θa before the first decimal place and the other part θb after the first decimal place, that is, θ=θa+θb; then respectively finding the sine and cosine values ​​of θa, sinθa and cosθa, in the table; finally, calculating the sine and cosine values ​​of the angle value θ by the fitting formula, and the fitting formula is:

3. The method for anti-disturbance control of integrated joints of a humanoid robot according to claim 2, characterized in that: The calculation formula for each step of the normalized current loop vector control algorithm is: (1) Clarke transformation: c1=1;c2=0;x1=i a ;x2=0; x3=i a ;x4=i b ; I sa =y1;i sβ =y2; (2) Park transform: c1=cos(θe);c2=sin(θ e );x1=i sa ;x2=i sβ ; c3=-sin(θ e );c4=cos(θ e );x3=i sa ;x4=i sβ ; I sd =θ e1 ;i sq =θ e2 ; (3) PI controller: c1=K dnew ;c2=K dold ;x1=e id (k);x2=e id (k-1); c3−K q_new ;c4=K q_old ;x3=e iq (k);x4=e iq (k-1); V sd (k)=V sd (k-1)+θ e1 ; V sq (k)=V sq (k-1)+θ e2 ; (4) Park inverse transform: c1=cos(θ e );c2=-sin(θ e );x1=V sd ;x2=V sq ; c3=sin(θ e );c4=cos(θ e );x3=V sd ;x4=V sq ; V sa =θ e1 ;V sβ =θ e2 ; (5) The spatial sector where SVPWM is located: c2=-1;x1=V sa ;x2=V sβ ; c4=-1;x3=V sa ;x4=V sβ ; V a =V sβ ;V b =θ e1 ;V c =θ e2 ; (6) Calculate the theoretical switching time of each phase of SVPWM: x1=V sβ ;x2=V sa ; x3=V sβ ;x4=V sa ; X=θ e1 +θ e2 ;Y=θ e1 ;Z=θ e2 ; (7) Calculate the actual pulse width modulation time of each phase of SVPWM: c2=0;x1=t1;x2=0; c4=0;x3=t2;x4=0; t 1SAT =θ e1 ;t 2SAT =θ e2 ; in, Operated by an independent divider, θ e1 is the electrical angle value of the motor end obtained by the first position and speed sensing module, θ e2 It is the electrical angle value of the load end obtained by analyzing the second position and speed sensing module.

4. A humanoid robot integrated joint anti-disturbance control system, characterized in that: It includes a soft-core processor, which is used to complete the outer loop real-time control task composed of a speed loop and a position loop with a lower control beat, and is responsible for the real-time monitoring of each communication module of the host computer and the entire control system; a space vector control slave system, which is used to complete the current loop vector control algorithm in the anti-disturbance control method of the integrated joint of the humanoid robot as claimed in any one of claims 1 to 3, and the space vector control slave system is mounted on the soft-core processor through a bus; The space vector control slave system includes 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 encoder, a functional module and a second position and speed sensing module, wherein the functional module receives the motor end rotation angle and speed feedback from the first position and speed sensing module and the load end rotation angle and speed feedback from the second position and speed sensing module, and converts the rotation angle feedback value θ into the rotation angle feedback value θ by comparison and operation. m Feedback to the position loop module, the speed feedback value ω m Feedback to the speed loop module.

5. The humanoid robot integrated joint anti-disturbance control system according to claim 4, characterized in that: The FPGA-based current loop module includes a computing core unit, a state machine, a parameter selection unit, an intermediate variable memory, and an internal connection module; the computing core unit is a multiplication-addition calculation unit composed of four independent multipliers and two parallel adders, which is used to complete the two basic calculation formulas shared by each step of the current loop vector control algorithm:

Images