Location-based active compliance control method and system

[0070] Example 1:

[0071] figure 1 It is a flowchart of the position-based active compliance control method of the present invention. Such as figure 1 As shown, a position-based active compliance control method includes:

[0072] Step 101: Establish a position-based active compliance control mathematical model of the leg hydraulic drive system, which specifically includes:

[0073] Establish the dynamic kinematics relationship matrix between the foot end displacement of the leg hydraulic drive system and the displacement of each joint hydraulic drive unit;

[0074] Establish the static Jacobian matrix of the foot-end force of the leg hydraulic drive system and the force of the hydraulic drive unit, and obtain the static relationship matrix;

[0075] Establish a position control system mathematical model for each joint hydraulic drive unit of the leg hydraulic system, and the position control system mathematical model includes nonlinear factors;

[0076] According to the dynamic kinematics relationship matrix, the statics relationship matrix and the position control system mathematical model, a position-based active compliance control mathematical model of the leg hydraulic drive system is established.

[0077] Step 102: Establish a dynamic dynamics mathematical model of the foot end displacement of the leg hydraulic drive system and the force of each joint hydraulic drive unit, which specifically includes:

[0078] Establish the dynamic dynamic relationship matrix between the foot end displacement of the leg hydraulic drive system and the force of each joint hydraulic drive unit;

[0079] According to the dynamic dynamic relationship matrix, a dynamic dynamic mathematical model is determined.

[0080] Step 103: Establish an active compliance comprehensive control model according to the active compliance control mathematical model and the dynamic dynamics mathematical model, which specifically includes:

[0081] The inverse dynamics compensation method is adopted to substitute the dynamic dynamics mathematical model into the active compliance control mathematical model to obtain an active compliance comprehensive control model.

[0082] Step 104: Obtain dynamic stiffness information according to the active compliance integrated control model, which specifically includes:

[0083] According to the active compliance comprehensive control model, obtain the high-order dynamic stiffness expressions of the inner and outer loops of the position-based active compliance control of the hydraulic drive unit;

[0084] According to the high-order dynamic stiffness expression, the dynamic stiffness information is obtained. The dynamic stiffness information includes the inherent dynamic stiffness of the position control system of the hydraulic drive unit, the equivalent dynamic stiffness generated by the position inner loop closed-loop control, and the active compliance control outer loop Dynamic stiffness.

[0085] Step 105: Transform the dynamic stiffness information into a spring series-parallel structure, which specifically includes:

[0086] The dynamic stiffness information is transformed into a spring series-parallel structure, wherein the inherent dynamic stiffness of the hydraulic drive unit position control system and the equivalent dynamic stiffness generated by the position inner loop closed-loop control are transformed into a spring parallel structure, so The dynamic stiffness of the active compliance control outer ring is transformed into a structure connected in series with the parallel structure of the spring.

[0087] Step 106: Analyze the factors of the position-based active compliance control performance according to the spring series-parallel structure.

[0088] The above method of the present invention can eliminate the influence of dynamic factors on the performance of active compliance control. The present invention obtains the position-based compliance control of the hydraulic drive unit based on the dynamic stiffness expression of each part of the inner and outer ring and converts the dynamic stiffness expression of each part of the inner and outer ring into a spring series-parallel structure, and combines the mechanical structure kinematics of the leg hydraulic drive system , Statics and dynamics, the dynamic stiffness control of the hydraulic drive unit is extended to the leg hydraulic drive system, the composition mechanism of the leg dynamic stiffness control is obtained, and the root cause that affects the accuracy and speed of active compliance control is found.

[0089] Example 2:

[0090] figure 2 It is the structure diagram of the position-based active compliance control system of the present invention. Such as figure 2 As shown, a position-based active compliance control system includes:

[0091] The active compliance control mathematical model establishment module 201 is used to establish the position-based active compliance control mathematical model of the leg hydraulic drive system;

[0092] The dynamic dynamics mathematical model establishment module 202 is used to establish the dynamic dynamics mathematical model of the foot end displacement of the leg hydraulic drive system and the force of each joint hydraulic drive unit;

[0093] The active compliance integrated control model establishment module 203 is configured to establish an active compliance integrated control model according to the active compliance control mathematical model and the dynamic dynamics mathematical model;

[0094] The dynamic stiffness information determining module 204 is configured to obtain dynamic stiffness information according to the active compliance comprehensive control model;

[0095] The conversion module 205 is used to convert the dynamic stiffness information into a spring series-parallel structure;

[0096] The analysis module 206 is configured to analyze the factors of the position-based active compliance control performance according to the spring series-parallel structure.

[0097] The active compliance control mathematical model establishment module 201 specifically includes:

[0098] The dynamic kinematics relationship matrix establishment unit is used to establish the dynamic kinematics relationship matrix between the foot end displacement of the leg hydraulic drive system and the displacement of each joint hydraulic drive unit;

[0099] The statics relationship matrix establishment unit is used to establish the static Jacobian matrix of the foot end force of the leg hydraulic drive system and the hydraulic drive unit force to obtain the statics relationship matrix;

[0100] The mathematical model establishment unit of the position control system is used to establish the mathematical model of the position control system for the hydraulic drive units of each joint of the leg hydraulic system, and the mathematical model of the position control system includes nonlinear factors;

[0101] The active compliance control mathematical model establishment unit is used to establish a position-based active compliance control mathematical model of the leg hydraulic drive system according to the dynamic kinematics relationship matrix, the statics relationship matrix and the position control system mathematical model.

[0102] The dynamic dynamics mathematical model establishment module 202 specifically includes:

[0103] The dynamic dynamic relationship matrix establishment unit is used to establish the dynamic dynamic relationship matrix between the foot end displacement of the leg hydraulic drive system and the force of each joint hydraulic drive unit;

[0104] The dynamic dynamics mathematical model establishment unit is used to determine the dynamic dynamics mathematical model according to the dynamic dynamics relationship matrix.

[0105] The active compliance integrated control model establishment module 203 specifically includes:

[0106] The active compliance comprehensive control model establishment unit is used to adopt the inverse dynamic compensation method to substitute the dynamic dynamics mathematical model into the active compliance control mathematical model to obtain the active compliance comprehensive control model.

[0107] Example 3:

[0108] The present invention discloses the composition mechanism of the dynamic stiffness control of the inner and outer rings of the active compliance control based on the position. The specific content includes the following steps:

[0109] Step 1: Mathematical modeling of active compliance control of leg hydraulic drive system.

[0110] ①Establish the dynamic kinematics relationship matrix between the foot end displacement of the leg hydraulic drive system and the displacement of each joint hydraulic drive unit.

[0111] image 3 Is a simplified diagram of the kinematics of the leg of the foot robot, image 3 Among them, OA, OB, OC, OD, DE, DF, and EF are all known length parameters. AB and CE are the lengths of the hydraulic drive unit of the knee and ankle joints, which change in real time with the movement of the robot.

[0112] When the position change of the hydraulic drive unit of the knee and ankle joints is an independent variable, the foot movement position of the leg hydraulic drive system can be expressed as:

[0113]

[0114] Where Is the displacement of the foot in the X-axis direction, Is the movement displacement of the foot end in the Y axis direction, ΔX p1 Is the position change of the knee joint hydraulic drive unit, ΔX p2 It is the position change of the hydraulic drive unit of the ankle joint.

[0115] by image 3 A simplified diagram of the kinematics model of a single leg. By analyzing the geometric relationship of the structure of a single leg, the positive solution relationship of the kinematic position can be obtained as follows:

[0116]

[0117] When the foot end motion position of the leg hydraulic drive system is an independent variable, the position change of the knee and ankle joint hydraulic drive units can be expressed as:

[0118]

[0119] by image 3 A simplified diagram of the single-leg kinematics model. Analyzing the geometric relationship of the single-leg structure, the inverse kinematic position relationship can be obtained as follows:

[0120]

[0121] ②Establish the static Jacobian matrix of the foot-end force of the leg hydraulic drive system and the force of the hydraulic drive unit, and obtain the positive and negative statics relationship.

[0122] Combine image 3 After analysis and calculation, the inverse solution relationship of statics is as follows:

[0123]

[0124] Inverting the inverse solution relationship of statics, the positive solution relationship of statics can be obtained as follows:

[0125]

[0126] ③For the hydraulic drive unit of each joint of the leg hydraulic system, establish a mathematical model of the position control system with nonlinear factors.

[0127] Figure 4 It is a three-dimensional assembly drawing of the hydraulic drive unit. It is a high-power density integrated valve-controlled asymmetric cylinder structure. The servo valve is approximately equivalent to a second-order oscillation link, and the transfer function between the spool displacement and the input voltage of the servo amplifier board is:

[0128]

[0129] Where K axv Is the servo valve gain, ζ is the damping ratio of the servo valve, and ω is the natural frequency of the servo valve.

[0130] Since the pressure loss in the pipeline and valve cavity is much smaller than the throttling pressure loss at the valve port, it can be ignored. Considering the pressure-flow nonlinearity, the servo valve inlet oil flow can be expressed as:

[0131]

[0132] The return oil flow of the servo valve can be expressed as:

[0133]

[0134] Where x v Is the displacement of the servo valve spool, p s Supply pressure for the system, p 1 Is the pressure of the left cavity of the servo cylinder, p 2 Is the pressure in the right chamber of the servo cylinder, p 0 Is the return pressure of the system, K d Is the converted flow coefficient.

[0135] Among them, the converted flow coefficient K d The expression is

[0136]

[0137] Where C d Is the flow coefficient of the throttle orifice of the servo valve slide valve, W is the area gradient, and ρ is the hydraulic oil density.

[0138] Considering the influence of load characteristics on the position control system, the servo cylinder force balance equation is:

[0139]

[0140] The transfer function between the feedback voltage of the displacement sensor and the displacement of the piston rod of the servo cylinder is:

[0141]

[0142] Figure 5 Is the block diagram of the closed-loop control system of the hydraulic drive unit position, E p X p Relative to input position X r The amount of change, that is, the position deviation:

[0143] E p =X r -X p (1-13)

[0144] E p The generation of the hydraulic drive unit leads to a decrease in the accuracy of the position control of the hydraulic drive unit. The cause is mainly composed of the following two parts: The first part is the external load force F L The system position deviation caused by; the second part is input X r System position deviation caused by.

[0145] ④Combining the kinematics and statics relationship matrix of the mechanical structure of the leg hydraulic drive system, based on the force control mathematical model of the hydraulic drive unit of each joint, a position-based active compliance control mathematical model of the leg hydraulic drive system is established.

[0146] Image 6 It is the principle diagram of the overall force when the load end applies interference force to the hydraulic drive unit. Since the displacement sensor and the force sensor are installed at the hydraulic drive unit, when the load end applies load force to the hydraulic drive unit, the force can be divided into There are three parts, the first part is the force of the load end; the second part is the force of the force sensor; the third part is the force of the hydraulic drive unit.

[0147] Figure 7 It is the principle diagram of the load end force, the force balance equation of the load end can be expressed as:

[0148] ΔF sb = F L -ΔX p Z E -F f2 (1-14)

[0149] It can be seen from the above formula that if the dynamic stiffness Z E Tends to zero, then when the influence of friction is not considered, there is ΔF sb →F L.

[0150] Figure 8 It is the principle diagram of the force sensor of the hydraulic drive unit, which defines ΔF sa 'And ΔF sa With ΔF sb 'And ΔF sb They are the relationship between the acting force and the reaction force. ΔF sa 'And ΔF sb 'Is the equal and opposite force received on both sides of the force sensor, where the force of the force sensor on the piston rod (that is, the load force received by the hydraulic drive unit position control system) is defined as ΔF sa , The force of the force sensor on the load is defined as ΔF sb.

[0151] Picture 9 It is the schematic diagram of the force interference of the hydraulic drive unit, when the system is subjected to load force ΔF sa , Should produce position change ΔX p To balance the load force, the servo cylinder force balance equation can be transformed into:

[0152]

[0153] Picture 10 Block diagram of the position-based active compliance control for the converted hydraulic drive unit. The block diagram is based on considering the installation characteristics of displacement sensors and force sensors in the leg hydraulic drive system, combined with the force analysis of the hydraulic drive unit adopting the position-based active compliance control method, and the block diagram of the hydraulic drive unit position control system is transformed. of.

[0154] Step 2: Research on inverse dynamics compensation control of leg hydraulic drive system.

[0155] ①Establish the dynamic dynamics mathematical model of the foot end displacement of the hydraulic drive system of the leg and the force of the hydraulic drive unit of each joint.

[0156] Picture 11 Is a simplified diagram of the single-leg dynamics model, combined with Picture 11 By calculation, the dynamic relationship of the force on the hydraulic drive unit of each joint of the leg can be obtained as:

[0157]

[0158] The above formula contains with The term is the joint moment of inertia due to acceleration, including with The term of is the coupling moment term due to centripetal force, including The term of is the coupling moment term due to Coriolis force, including joint angular displacement θ 1 , Θ 2 The term of is the joint moment term caused by gravity.

[0159] ②According to the dynamic relationship matrix of the leg hydraulic drive system, establish a position-based active compliance control model combined with inverse dynamic compensation to eliminate the influence of dynamic factors on the performance of active compliance control.

[0160] Picture 12 This is the schematic diagram of the realization of the position-based active compliance control of the leg hydraulic drive system. When the leg hydraulic drive system adopts the position-based active compliance control, when the foot end of the leg is disturbed, the leg hydraulic drive system is realizing the position-based When the compliant control, follow three steps:

[0161] The first step is to find the foot end interference force signal in the force sensor signal in the compliance control outer loop: When the foot end of the leg receives the interference force, the force sensor of the knee joint and ankle joint hydraulic drive unit detects the force signal, which is determined by It consists of two parts. The first part is the force signal component of the interference force detected by the force sensor of each hydraulic drive unit acting on the foot end of the leg; the second part is the signal component of the gravity and inertial force of the leg mechanical structure detected by the sensor. Since the second part of the force signal is not generated by the interference force received by the leg and foot, if the force sensor detection force is directly used as the compliance control interference force signal, it will affect the control accuracy of the leg and foot movement, so it is necessary to combine the legs Partial inverse dynamics, calculate the second part of the force signal, to solve the first part of the force received by the force sensor as the position-based compliance control interference force.

[0162] The second step is to convert the interference force signal into the input signal change amount of the position control inner loop in the compliance control outer loop: After the interference force received by the knee and ankle joint hydraulic drive units is calculated through the first step, the static The positive solution of mechanics finds the interference force signal received by the leg and foot. Then, through the leg compliance solver, the position change amount corresponding to the interference force signal is solved, and then the position control inner loop input signal change amount of each hydraulic drive unit can be obtained through the kinematic position inverse solution.

[0163] The third step is to implement input and output control in the position control inner loop: when the position control inner loop gives the input signal, it is converted to the control element (servo valve) control signal, and then to the actuator (servo cylinder) to output the position signal.

[0164] Step 3: Perform a position-based active compliance control dynamic stiffness analysis of the leg hydraulic drive system.

[0165] ① Obtain the high-order dynamic stiffness expressions of the inner and outer loops of the position-based active compliance control of the hydraulic drive unit, and obtain the mechanism of the series-parallel dynamic stiffness of the inner and outer loops.

[0166] Figure 13 It is a simplified schematic diagram of the position-based active compliance control of the hydraulic drive unit. This diagram starts with the principle of position-based active compliance control. Picture 11 Simplified.

[0167] G f (s) is the transfer function of the load force acting on the system, G 1p (s), G 2p (s) and G 3p (s) is the transfer function of each part of the position control system. The specific expressions of these four transfer functions are as follows:

[0168]

[0169]

[0170]

[0171]

[0172] Figure 13 It can be seen that when the load force ΔF s When acting on the inner loop of position control, ΔF s Will output ΔX to the position control inner loop p Produce the influence of direction ① in the figure, which is equivalent to a high-order dynamic stiffness in the inner loop of the position control It is the inherent dynamic stiffness of the hydraulic drive unit position control system. When the load force ΔF s Generated system output ΔX p At this time, due to the existence of the position control closed loop, the input signal becomes E p =ΔX D -ΔX p. At this time, E p ΔF will be generated at node I through the position control inner loop s ', at ΔF s 'And E p There is a directional influence of ② in the figure, which is equivalent to the existence of a high-order dynamic stiffness It is the equivalent dynamic stiffness produced by the position inner loop closed-loop control. It can be seen that the load force acts simultaneously on with Output displacement ΔX p , That is, the output position changes caused by the two dynamic stiffnesses interact and together affect the output displacement ΔX p. Set dynamic stiffness Z sp for with The combined dynamic stiffness produced by the interaction.

[0173] by Figure 13 It can be seen that when considering the input position ΔX D The influence of the position control system inner ring position deviation E p Can be expressed as:

[0174] E p = E p1 +E p2 =ΔX D -ΔX p =ΔX r -ΔX e -ΔX p (3-5)

[0175] Where: E p1 The load force of the inner ring of the position control system passes through the dynamic stiffness Z sp The variation of inner ring position deviation produced, E p2 It is the variation of the input deviation between the input position and the output position of the position inner loop. Due to system load force ΔF s Deviation from input E p2 Independent of each other, so ΔF s The generated inner ring position deviation change and the input position ΔX D Irrelevant. The two dynamic stiffnesses of the position control inner ring satisfy Spring Hooke’s law, forming a parallel relationship of dynamic stiffness, resulting in a comprehensive dynamic stiffness of the inner ring Z sp It can be expressed as:

[0176]

[0177] Figure 13 The position change deviation caused by the direction in ③ can be expressed as:

[0178]

[0179] By solving the inverse dynamic relationship of the leg, it is possible to obtain ΔX in different motion positions p ΔF under k , If ΔF k versus Equal to load force ΔF s Subtract ΔF k , So that only the influence of interference force is considered in the outer loop of compliance control, which is equivalent to Figure 13 In the direction of ④, -ΔF k With ΔX p Equivalent dynamic stiffness between Z k , Can be expressed as:

[0180]

[0181] Calculate ΔF k Then, the position change can be calculated by the position compliance characteristics Used to offset the amount of position change That is, only the influence of interference is considered. At this time, the position deviation produced by the impedance outer loop can be expressed as:

[0182]

[0183] Define the comprehensive dynamic stiffness of the inner and outer rings of the position-based compliance control as Z Ap , Can be expressed as:

[0184]

[0185] In the above formula, Z sp With Z D Meet the Hooke's law spring series characteristics, therefore, Z Ap Is the dynamic stiffness of the inner ring of the hydraulic system Z sp With active compliance control outer ring dynamic stiffness Z op (Z op =Z D ) Series system with time-varying coefficient 1+Z k /Z D , When the compliant control outer ring is not considered, there is Z Ap =Z sp.

[0186] Figure 14 The position-based compliance control dynamic stiffness of the hydraulic drive unit is a schematic diagram (the dynamic stiffness of each part in the figure is represented by springs). The position-based compliance control system is actually a variable stiffness series-parallel hybrid system composed of three different dynamic stiffnesses. , The three dynamic stiffnesses of the system are subject to load force ΔF s When, the system will produce a position change ΔX p To balance the load. When the leg inverse dynamic calculation is not performed, the dynamic stiffness Z k = 0, the variable stiffness coefficient is 1; and when the inverse dynamics of the leg is added, the dynamic stiffness Z k0, the variable stiffness coefficient is greater than 1, which is equivalent to increasing the comprehensive dynamic stiffness Z through inverse dynamics solution Ap.

[0187] The desired position ΔX of the hydraulic drive unit based on the position Dp It can be expressed as:

[0188]

[0189] The above formula shows that if Then the desired position is only related to the change in the compliant position caused by the interference force, eliminating Impact. However, if the inverse dynamic relationship of the leg mechanical structure is not considered in the calculation, or the calculation of the inverse dynamic relationship is not accurate, it will cause That is to say, the calculated expected position of compliance is not equal to the expected position of compliance generated by the interference force, that is, the calculated expected position of compliance contains The position change caused.

[0190] Position-based compliance actual position change ΔX Ap Can be expressed as:

[0191]

[0192] The last term in the above formula indicates that when ΔX r With E p2 When it is not zero, the relationship between the actual position of the impedance and the comprehensive dynamic stiffness of the system.

[0193] Position-based compliance actual position change ΔX Ap And the desired position change ΔX Dp Deviation value D p Can be expressed as:

[0194] D p =ΔX Dp -ΔX Ap = E p1 +E p2 (3-13)

[0195] It can be seen from the above formula that the actual position change of the position-based compliance is theoretically deviated from its expected position change. At this time, the accuracy of the active compliance control is mainly determined by E p1 With E p2 Decided, E p1 With E p2 The amount of position change caused is produced by the position control inner loop.

[0196] ②Combining the kinematics, statics and dynamics relationship matrix of the leg mechanical structure, the dynamic stiffness theory of the hydraulic drive unit is extended to the leg hydraulic drive system, and the mapping relationship between the dynamic stiffness of the foot and the dynamic stiffness of each joint hydraulic drive unit is obtained. Based on the dynamic stiffness theory of the leg hydraulic drive system, the key factors affecting the performance of position-based active compliance control are obtained.

[0197] by Figure 15 The schematic diagram of the dynamic stiffness composition of the hydraulic drive unit of each joint for the position-based compliance control and Figure 16 It can be seen from the schematic diagram of the overall dynamic stiffness composition of the position-based compliance control leg hydraulic drive system that in the position-based compliance control method, the dynamic stiffness of the leg hydraulic drive system is composed of a more complex series-parallel hybrid method. When the end of the leg is under load with Due to the serial characteristics of the hydraulic drive units of the knee and ankle joints, the force ΔF s1 With ΔF s2 Satisfy the inverse solution of leg statics. Changes in the position of the feet of the legs with Can be output by two hydraulic drive units position ΔX p1 And ΔX p2 Obtained by the positive solution of the leg kinematics position. It can be seen that the accuracy of active compliance control achieved by the leg and foot end is comparable to ΔX p1 And ΔX p2 The control accuracy is directly related. ΔX p1 And ΔX p2 The control accuracy is determined by its control deviation versus Decide where:

[0198]

[0199]

[0200] Where The deviation between the input position change and the output position change of the knee joint hydraulic drive unit (mm);

[0201] The deviation (mm) between the input position change and the output position change of the ankle joint hydraulic drive unit.