A dynamic control method for the excavator boom

Abstract

The present invention discloses a method for dynamic control of an excavator arm, wherein the dynamic model of the excavator arm is linearized, the parameters of the dynamic parameter matrix of the excavator arm are identified, and the identified dynamic feedforward model of the excavator arm is obtained. The feedforward signal output by the dynamic feedforward model of the excavator arm is input into a pre-built three-loop PID control system, and the three-loop PID control system outputs the control results of each joint of the excavator arm. The method for dynamic control of an excavator arm provided by the present invention adopts a control mode of dynamic feedforward + three-loop PID feedback, calculates in real time the inertial force, Coriolis force, centripetal force, gravity and friction force generated when the excavator arm moves, and designs an excavator arm control system, which can improve the dynamic accuracy of the excavator arm during movement and improve the reliability and stability of the excavator during movement.

Description

Technical Field

[0001] The invention relates to a dynamics control method for an excavator arm, belonging to the technical field of excavator control. Background Art

[0002] An excavator, also known as a backhoe, is an earth-moving machine that uses a bucket to dig material above or below the surface and load it onto a transport vehicle or unload it into a stockpile. As a material-moving machine, it is widely used in mining and construction. Due to its wide range of applications, excavator operators must master a wide range of operating techniques to adapt to different working conditions and requirements. This makes training qualified operators a time-consuming and labor-intensive task. Furthermore, the complexity of operations and the harsh working environment make it difficult for operators to maintain efficient work for extended periods of time. This has led to a growing demand for automated excavator control.

[0003] Most current automatic control schemes for excavators use simple position feedback control schemes, some supplemented by velocity feedforward. However, these automatic control schemes only perform simple processing on both feedback and target signals, and their feedforward models are relatively crude, making them difficult to handle the nonlinearities of excavators. Consequently, these automatic control schemes can only track a few preset trajectories, and the trajectory speed is severely limited, failing to fully utilize the excavator's performance.

[0004] Existing automatic control systems for excavators are mostly designed for a specific operating condition or trajectory. Switching to a different trajectory requires redesigning or retraining the controller. Furthermore, due to limitations in the controller structure, current automatic control systems often fail to fully utilize the excavator's power and can only track slow-moving trajectories.

[0005] The first prior art improved motor three-loop control method only involves an improved motor three-loop control method, does not involve dynamic feedforward, and does not add a regulating factor when outputting the current loop PID.

[0006] The second type of prior art relates to a robot control method, device, electronic device, and readable storage medium that uses velocity feedforward but not torque feedforward, and thus cannot achieve precise force control. Moreover, the friction modeling of this solution does not consider Coulomb friction, viscous friction, and stribeck friction.

[0007] The third type of robot control system in the prior art only outlines a robot control system and does not involve dynamic feedforward control and three-loop PID control.

[0008] Based on the above prior art, those skilled in the art are in urgent need of solving the following technical problems:

[0009] (1) Solve the problem of "dead zone" of solenoid valve.

[0010] (2) Solve the problem of precise control of excavator arms.

[0011] (3) Solve the problem of "creeping" at low speed and poor accuracy at high speed. Summary of the Invention

[0012] Purpose: In order to overcome the shortcomings of the existing technology, the present invention provides a dynamic control method for the excavator arm of an excavator. The control mode of dynamic feedforward + three-loop PID feedback is adopted to calculate the inertia force, Coriolis force, centripetal force, gravity and friction force generated when the excavator arm moves in real time. The excavator arm control system is designed, which can improve the dynamic accuracy of the excavator arm during movement and improve the reliability and stability of the excavator during movement.

[0013] Technical solution: To solve the above technical problems, the technical solution adopted by the present invention is:

[0014] A method for dynamic control of an excavator arm comprises the following steps:

[0015] Step S1: performing parameter linearization processing on the pre-built excavator arm dynamics model to obtain a linear equation of the excavator arm dynamics model.

[0016] Step S2: reorganize the linear equation of the excavator arm dynamics model to obtain a reorganized linear equation.

[0017] Step S3: Sensors are set on the boom, arm and bucket of the excavator arm, and the torque, angle, angular velocity and angular acceleration data of each joint of the boom, arm and bucket of the excavator arm under the excitation trajectory are obtained.

[0018] Step S4: Filter the torque, angle, angular velocity and angular acceleration data of each joint, and use the filtered data to solve the full-rank regression matrix in the reorganized linear equation. Use the criterion of minimizing the condition number of the full-rank regression matrix to optimize the pre-designed excitation trajectory calculation formula to obtain the parameters in the excitation trajectory calculation formula.

[0019] Step S5: Based on the filtered data, the least square method is applied to identify the parameters of the minimum inertia parameter matrix in the reorganized linear equation, and the minimum inertia parameter matrix is used as the excavator arm dynamic parameter matrix.

[0020] Step S6: Substitute the excavator arm dynamic parameter matrix into the reorganized linear equation to obtain the identified excavator arm dynamic feedforward model.

[0021] Step S7: The feedforward signal output by the excavator arm dynamics feedforward model is input into a pre-built three-loop PID control system, and the three-loop PID control system outputs the control results of each joint of the excavator arm.

[0022] As a preferred solution, step S8 is also included. Step S8: the control results of each joint of the excavator arm output by the three-loop PID control system are matched with the preset control results of each joint of the excavator arm. When the matching degree is less than the matching threshold, the process returns to step S3 and steps S3-S6 are repeated to obtain a more ideal excavator arm dynamics feedforward model.

[0023] As a preferred solution, the calculation formula of the pre-built excavator arm dynamics model is as follows:

[0024]

[0025] in, is the control torque, q, are the angle, angular velocity and angular acceleration of each joint of the excavator arm, is the moment of inertia, is the coupling torque of Coriolis moment and centripetal moment, is the gravitational moment, is the friction torque.

[0026] As a preferred option,

[0027] in: For Take the sign function, p c is the Coulomb friction coefficient, p v is the viscous friction coefficient, p s is the stribeck friction coefficient.

[0028] As a preferred solution, the calculation formula of the linear equation of the excavator arm dynamics model is as follows:

[0029]

[0030] in: To control the torque, is the regression matrix, q, are the angle, angular velocity and angular acceleration of each joint of the excavator arm, is the inertia parameter matrix.

[0031] As a preferred solution, the calculation formula of the reorganized linear equation is as follows:

[0032]

[0033] in, To control the torque, is the full-rank regression matrix, is the minimum inertia parameter matrix.

[0034] As a preferred option, represents the dynamic parameters of N joints, P N Represents the dynamic parameters of the Nth joint.

[0035] The minimum inertia matrix element of joint i can be expressed as:

[0036] P r =[I xx ,I xy ,I xz ,I yy ,I yz ,I zz ,mc x ,mc y ,mc z ,m,p c ,p v ,p s ] T

[0037] Among them, I xx ,I xy ,I xz ,I yy ,I yz ,I zz are the six parameters of the inertia tensor matrix of joint i in the x, y, and z directions, m is the mass of joint i, c x , c y , c z They are the components of the center of mass position of joint i in the x, y, and z directions of the joint coordinate system, p c is the Coulomb friction coefficient, p v is the viscous friction coefficient, p s is the stribeck friction coefficient, and ρ is the unit density of joint i.

[0038] As a preferred solution, the calculation formula of the pre-designed excitation trajectory is as follows:

[0039]

[0040] Among them, w f is the fundamental frequency. Each joint should select the same fundamental frequency to ensure the periodicity of the entire movement. N represents the number of harmonics, l represents the harmonic, and t represents the time. is the coefficient, q i0 is a constant term, and i represents a joint.

[0041] As a preferred solution, the parameters in the obtained excitation trajectory calculation formula are q i0 .

[0042] As a preferred solution, the parameters identified in the linear equation after the reorganization of the excavator arm dynamics model are the minimum inertia parameter matrix Parameters of .

[0043] As a preferred solution, a three-loop PID control system is pre-built including: position loop, speed loop and current loop.

[0044] Position loop: The input position control command P_ref and the position feedback command P_fb are fused and input into the position control module. The position control module outputs the target speed command V_ref, and the control module adopts proportional regulation.

[0045] Speed loop: The input target speed command V_ref and the speed feedback command V_fb are fused and input into the speed control module. The speed control module outputs the target acceleration command A_ref. The speed control module adopts proportional and integral regulation.

[0046] Current loop: The input target acceleration command A_ref, acceleration feedback command A_fb and the feedforward signal output by the excavator arm dynamics feedforward model are fused and input into the current control module. The output of the current control module is multiplied by a current coefficient k and used as the control signal of the joint solenoid valve. The current control module has proportional, integral and differential adjustments.

[0047] Beneficial effects: Compared with the control schemes in the prior art which are often only targeted at a specific working condition or trajectory, the method for dynamic control of the excavator arm provided by the present invention can accurately calculate the output torque required for each joint of the excavator arm in each motion cycle, thereby improving the dynamic control accuracy of the excavator arm and solving the problems of "creeping" of the excavator arm at low speeds and poor accuracy at high speeds during movement in previous controls.

[0048] Compared with the prior art, the present invention has the following beneficial technical effects:

[0049] (1) In the process of constructing the excavator arm dynamics model, the friction model was optimized and the Coulomb friction, viscous friction and Stribeck friction coefficient were comprehensively introduced, so that the constructed dynamics model can better match the hydraulic system of the excavator, adapt to the nonlinear characteristics of the hydraulic cylinder, and solve the "dead zone" problem of the solenoid valve.

[0050] (2) Design the optimal excitation trajectory and optimize the optimal excitation trajectory of the excavator arm based on the two optimization mechanisms of the parameters obtained by the sensor group and the matching degree of the control results. Further through parameter identification, the control of the excavator arm is made more precise.

[0051] (3) The three-loop PID control system is integrated with the dynamic feedforward model, and the feedforward signal output by the dynamic feedforward model is used to optimize the three-loop PID control system to improve the dynamic control accuracy of the excavator arm; the output of the current loop PID is multiplied by a current coefficient k, and a dynamic adjustment mechanism of the current coefficient k is constructed based on the excitation trajectory, which improves the reliability and stability of the excavator during movement and solves the problems of "creeping" at low speed and poor accuracy at high speed in the excavator control in the existing technology. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 Schematic diagram of the control method of the present invention.

[0053] Figure 2 Schematic diagram of the structure of the control system of the present invention. DETAILED DESCRIPTION

[0054] The present invention will be further described below with reference to specific embodiments.

[0055] like Figure 1 As shown, a method for dynamic control of an excavator arm includes the following steps:

[0056] Step S1: Constructing a dynamic model of the excavator arm.

[0057] Step S2: performing parameter linearization processing on the excavator arm dynamics model to obtain a linear equation of the excavator arm dynamics model.

[0058] Step S3: Reorganize the linear equation of the excavator arm dynamics model to obtain the reorganized linear equation, and extract the minimum inertia parameter matrix from the reorganized linear equation as the kinetic parameter matrix.

[0059] Step S4: Design the excitation trajectory.

[0060] Step S5: Sensors are set on the boom, arm and bucket of the excavator arm, and the joint torque, angle, angular velocity and angular acceleration data of the boom, arm and bucket of the excavator arm under the excitation trajectory are obtained.

[0061] Step S6: Filter the joint torque, angle, angular velocity and angular acceleration data, and use the filtered data to solve the excavator arm dynamic regression matrix. Use the solved excavator arm dynamic regression matrix to optimize the excitation trajectory in step S4 to obtain the parameters to be optimized in the excitation trajectory.

[0062] Step S7: Apply the least squares method to identify the minimum inertia parameter matrix in step S3 based on the data after filtering in step S6 The parameters of the excavator arm are obtained by identifying the dynamic parameter matrix of the excavator arm.

[0063] Step S8: Substitute the identified excavator arm dynamic parameter matrix into the linear equation of the reorganized excavator arm dynamic model after parameter linearization to obtain the identified excavator arm dynamic feedforward model.

[0064] Step S9: Construct a three-loop PID control system, and integrate the feedforward signal output by the excavator arm dynamics feedforward model in step S8 to optimize the entire three-loop PID control system to obtain an optimized three-loop PID control system, which is used to improve the dynamic characteristics of the excavator arm during operation, reduce the deviation of the PID controller adjustment, and improve the dynamic control accuracy of the excavator arm.

[0065] Step S10: Match the control result output by the optimized three-loop PID control system in step S9 with the preset control result. When the matching degree is less than the matching threshold, further optimize the excitation trajectory in step S4, select a more ideal excitation trajectory for parameter identification, and obtain accurate excavator arm inertia parameters and dynamic model.

[0066] Furthermore, the excavator arm dynamics model is derived using the Newton-Euler equation, and the calculation formula is as follows:

[0067]

[0068] in: is the control torque, q, are the angle, angular velocity and angular acceleration of each joint of the excavator arm, is the moment of inertia, is the coupling torque of Coriolis moment and centripetal moment, is the gravitational moment, is the friction torque.

[0069] The friction torque adopts a nonlinear friction model, which is composed of Coulomb friction, viscous friction and Stribeck friction. The calculation formula is as follows:

[0070]

[0071] in: For Take the sign function, p c is the Coulomb friction coefficient, p v is the viscous friction coefficient, p s is the stribeck friction coefficient.

[0072] Conventional dynamics equations primarily consist of four components: moment of inertia, Coriolis force and centripetal moment, gravitational moment, and friction. The moment of inertia is related to the acceleration of each joint, the Coriolis force and centripetal moment are related to the velocity of each joint, the gravitational moment is related to the current angle of each joint, and friction is related to the current velocity of each joint.

[0073] The present invention takes into account the large mass of each joint of the excavator arm, and in order to improve work efficiency, the operating speed of each joint is also large in most cases. Therefore, when constructing the dynamic model, the inertia moment, Coriolis force, centripetal moment, and gravity moment are retained. Moreover, when establishing the friction model, the conventional method only considers Coulomb friction and viscous friction. However, considering the nonlinear characteristics of the hydraulic cylinder and the "dead zone" problem of the solenoid valve, it is not enough to only consider Coulomb friction and viscous friction. Moreover, the hydraulic cylinder will have a "creeping" phenomenon at low speed. The simple Coulomb friction + viscous friction model cannot match the hydraulic system of the excavator. Based on this, this solution not only considers Coulomb friction and viscous friction, but also considers the influence of stribeck friction.

[0074] The excavator arm dynamics model contains three types of parameters: kinematic parameters, inertia parameters and friction parameters.

[0075] Among them: kinematic parameters are known quantities such as joint length and torsion angle.

[0076] The inertia parameters are coupling torque and gravity torque.

[0077] The friction parameters are Coulomb friction, viscous friction and Stribeck friction.

[0078] Inertia parameters and friction parameters are difficult to measure directly.

[0079] Furthermore, the excavator arm dynamic model is subjected to parameter linearization. The specific steps are as follows:

[0080] Since the constructed excavator arm dynamic model is not linear, it will bring a lot of trouble when performing parameter identification. Therefore, it is necessary to perform parameter linearization on the derived excavator arm dynamic model equation. Parameter linearization of dynamic models belongs to the existing technology and will not be described here.

[0081] The excavator arm dynamics model is subjected to parameter linearization to obtain an equivalent linear equation. The linear equation calculation formula is as follows:

[0082]

[0083] in: It is the regression matrix, which is calculated from the angle, angular velocity and angular acceleration of the joint and has nothing to do with the dynamic parameters; is the inertia parameter matrix to be identified. The regression matrix is not full rank at this time, and parameter identification cannot be performed directly through the regression matrix.

[0084] Furthermore, the linear equation is reorganized to obtain the reorganized linear equation, and the minimum inertia parameter matrix in the reorganized linear equation is extracted. The specific steps are as follows:

[0085] Parameters in linear equations are not mutually independent. This independence is a prerequisite for achieving high identification accuracy. Therefore, the independence of the parameters to be identified must be analyzed to find a set of independent parameters, known as the minimum parameter set. This minimum parameter set is a combination of parameter values. Currently, most kinetic parameter identification methods can only identify this combination value. However, this combination value does not affect the calculation of the kinetic parameters and can completely replace the actual parameters, simplifying the kinetic model.

[0086] The elements in the inertia parameter matrix are eliminated and reorganized using the linear relationship of the closed form rule. The minimum inertia parameter matrix that can be identified after reorganization is recorded as The corresponding full-rank regression matrix is recorded as The matrix composed of angle, angular velocity and angular acceleration is the same. The linear equation after reorganization is calculated as follows:

[0087]

[0088] in, represents the dynamic parameters of N joints, P N Represents the dynamic parameters of the Nth joint.

[0089] The minimum inertia matrix element for joint i can be expressed as:

[0090] P r =[I xx ,I xy ,I xz ,I yy ,I yz ,I zz ,mc x ,mc y ,mc z ,m,p c ,p v ,p s ] T

[0091] in:

[0092] I xx ,I xy ,I xz ,I yy,I yz ,I zz are the six parameters of the inertia tensor matrix of joint i in the x, y, and z directions.

[0093] I xx =∫∫∫ v (y 2 +z z )ρdv

[0094] I xy =∫∫∫ v xyρdv

[0095] I xz =∫∫∫ v xzρdv

[0096] I yy =∫∫∫ v (x 2 +z 2 )ρdv

[0097] I yz =∫∫∫ v yzρdv

[0098] I zz =∫∫∫ v (x 2 +y 2 )ρdv

[0099] m is the mass of joint i, c x , c y , c z They are the components of the center of mass position of joint i in the x, y, and z directions of the joint coordinate system, p c is the Coulomb friction coefficient, p v is the viscous friction coefficient, p s is the stribeck friction coefficient, and ρ is the unit density of joint i.

[0100] Furthermore, the steps of designing the excitation trajectory and obtaining the parameters in the excitation trajectory are as follows:

[0101] This invention represents the target position of each joint of the excavator arm using a fifth-order Fourier series. This is chosen because the target velocity and acceleration calculated from the target position are continuous, thus preventing errors from being introduced during parameter identification. The Fourier series of joint i is used as the excitation trajectory to control the movement of each joint of the excavator arm to the target position. Because the Fourier series is periodic, multiple sampling and averaging can be used to improve the signal-to-noise ratio, resulting in relatively ideal data.

[0102] The Fourier series of the joint i is used as the calculation formula for the excitation trajectory as follows:

[0103]

[0104] Among them, w f is the fundamental frequency. Each joint should select the same fundamental frequency to ensure the periodicity of the entire movement. N represents the number of harmonics, l represents the harmonic, and t represents the time. is the coefficient used to form the degree of freedom of the excitation trajectory optimization problem, q i0 is a constant term.

[0105] In order to improve the recognition accuracy, it is necessary to select a suitable indicator function to stimulate the trajectory. The condition number reflects the anti-noise ability of the identification method and the convergence rate of parameter estimation. The larger the condition number, the greater the degree of matrix pathology. Therefore, using formula 5 to meet the boundary conditions of the range of values of each joint angle, angular velocity, and angular acceleration, The minimum condition number is used as the optimization criterion for the excitation trajectory, and the optimization algorithm is used to obtain the and q i0 value.

[0106] Furthermore, sensors are set on the boom, arm, and bucket of the excavator arm, and the torque, angle, angular velocity, and angular acceleration data of each joint of the excavator arm under the excitation trajectory are obtained. The specific steps are as follows:

[0107] The excavator's boom, arm, and bucket joints are each equipped with inclination sensors and angular velocity sensors. Pressure sensors are installed at the oil inlet and outlet of each joint's cylinder. Data from these sensors is collected, and the torque of each joint is calculated based on the cylinder's cross-sectional area and the pressure sensor data. Because the angular acceleration of each joint cannot be measured, the angular velocity of each joint is differentiated to obtain its angular acceleration.

[0108] Furthermore, the acquired torque, angle, angular velocity, and angular acceleration data are filtered, and the excitation trajectory is optimized using the filtered data to obtain an optimized excitation trajectory. The specific steps are as follows:

[0109] Because the differential process introduces noise, an online filter is required to filter the torque, angle, angular velocity, and angular acceleration simultaneously. Conventional filters include online and offline filters. Since online filters have no hysteresis compared to offline filters, this solution uses an online filter to filter the collected data. The design of an online filter is conventional and will not be discussed here.

[0110] It should be noted that the filtered data is fed back to the excitation trajectory, and the Optimize the excitation trajectory.

[0111] Furthermore, the least squares method is applied to identify and extract the minimum inertia parameter matrix based on the filtered data. The dynamic parameter matrix of the excavator arm is obtained. The specific steps are as follows:

[0112] Based on the filtered data, the least squares method is used to identify the minimum inertia matrix element Pr of each joint to obtain the dynamic parameter matrix of the excavator arm. It should be noted that the least squares method is an existing technology and will not be described in detail here.

[0113] Furthermore, the dynamic parameter matrix of the excavator arm is brought into the constructed excavator arm dynamic model to complete the construction of the excavator arm dynamic feedforward model.

[0114] The core of the feedforward model is to compensate for the control variable provided by the system's internal control by establishing a dynamic inertia model. This reduces the deviation between adjacent control cycles in the current loop, improves the dynamic characteristics of the excavator arm during operation, reduces position error, and improves motion accuracy. After identifying the parameters of the excavator arm's dynamic model, the theoretical torque value in the current state is calculated and superimposed on the current loop, reducing the deviation of the PID controller's adjustment, accelerating the convergence of errors, and thus improving the dynamic response characteristics of each joint.

[0115] Furthermore, a three-loop PID control system is constructed, and the feedforward signal output by the excavator arm dynamics feedforward model is integrated to optimize the entire three-loop PID control system. The output control result of the three-loop PID control system is obtained, and the dynamic control accuracy of the excavator arm is improved. The specific steps are as follows:

[0116] For a system like an excavator arm, which has a large mass and high rigidity, achieving high-speed and high-precision control during its movement is somewhat difficult. Excavator arm control currently generally adopts position control or speed control, combined with simple speed feedforward. This control method ignores the interference of external disturbances on the excavator arm control. External disturbances are mainly divided into two aspects:

[0117] 1. Interference from the external environment.

[0118] 2. Inaccurate modeling or inaccurate identification of system parameters will cause disturbances in the excavator arm system itself.

[0119] Based on this, the present invention proposes a three-loop PID feedback control method. This method can suppress external environmental disturbances. However, simple three-loop PID feedback control is insufficient because the resulting PID parameters may not perfectly fit the system, effectively failing to address system disturbances in the excavator arm control. Based on this, the present invention designs a dynamic model of the excavator arm system and constructs a three-loop PID feedback + dynamic feedforward controller.

[0120] The main purpose of constructing a three-loop PID feedback + dynamic feedforward controller is to achieve torque control of the excavator arm. During the movement of the excavator arm, due to the large mass of the excavator arm itself, simple position control or speed control cannot achieve a good control effect. Especially when the excavator arm moves at a fast speed, the inertia force, centripetal force, Coriolis force and gravity have a comprehensive impact on the control of the excavator arm. In addition, the excavator arm is similar to a cantilever beam structure. If the excavator arm system cannot be accurately modeled, not only will the high-speed and high-precision control effect not be achieved, but the control accuracy will also not reach the expected value at low speed. Furthermore, the excavator arm has a large rigidity and is very suitable for modeling it using rigid body dynamics. Therefore, the present invention constructs a three-loop PID feedback + dynamic feedforward controller for the excavator arm, such as Figure 2 shown.

[0121] The three-loop PID control system includes: position loop, speed loop and current loop.

[0122] Position loop: Input position control instruction P_ref, that is, the current excavator arm is controlled to reach the required position. The current position of each joint is collected by the inclination sensor in the excavator arm as the position feedback instruction P_fb. Conventional position loop control generally adopts proportional regulation or proportional integral regulation. In the position control mode designed in the present invention, the system performs operations in all three loops. At this time, the system has the largest amount of system operations and the slowest dynamic response speed. In order to maximize the dynamic response speed, the position control module in the position loop only adopts proportional regulation and outputs the target speed instruction V_ref.

[0123] Speed loop: The output of the position loop PID is the target speed command V_ref. The current speed of each joint collected by the speed sensor in the excavator arm is used as the speed feedback command V_fb. The speed control module in the speed loop adopts proportional and integral regulation to output the target acceleration command A_ref. Combining the advantages of proportional and integral, proportional regulation is used to quickly offset the influence of interference, and the integral link is used to eliminate residual errors.

[0124] Current Loop: The output of the velocity loop PID is the target acceleration command A_ref. The current acceleration of each joint, obtained by differential velocity in the excavator arm, serves as the acceleration feedback command A_fb. This is added to the feedforward signal output by the excavator arm's dynamic feedforward model. The current control module in the current loop features proportional, integral, and differential adjustments. The output of the current loop PID is multiplied by a current coefficient k, which can be selected through dynamic adjustment. Specifically, given a motion trajectory, the trajectory is discretized. By analyzing the deviation between the theoretical and actual trajectories throughout the entire motion cycle, if the sum of the squares of the deviations at each discrete moment is too large, the parameter k is dynamically adjusted. Ultimately, based on the required accuracy of system control, the parameter k corresponding to a smaller sum of squared deviations is found.

[0125] The output signal of the current control module is given to the solenoid valve, which constructs a three-loop PID feedback + dynamic feedforward controller.

[0126] Three-loop PID control is also known as position loop, speed loop, and current loop cascade control. The expected position of the outermost loop is subtracted from the value after the position loop PID control as the expected value of the speed loop. The expected speed of the middle loop is subtracted from the value after the speed loop PID control as the expected value of the current loop. This can make the system reach the expected position and speed faster and reduce oscillation at the expected position and speed.

[0127] S10: Match the control result output by the three-loop PID control system with the preset control result. When the matching degree is less than the matching threshold, further optimize the excitation trajectory.

[0128] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A method for dynamic control of an excavator arm, characterized by: The steps include: Step S1: performing parameter linearization processing on the pre-built excavator arm dynamics model to obtain a linear equation of the excavator arm dynamics model; Step S2: reorganizing the linear equation of the excavator arm dynamics model to obtain a reorganized linear equation; Step S3: Sensors are set on the boom, arm, and bucket of the excavator arm, and the torque, angle, angular velocity, and angular acceleration data of each joint of the boom, arm, and bucket of the excavator arm under the excitation trajectory are obtained; Step S4: Filter the torque, angle, angular velocity, and angular acceleration data of each joint, and use the filtered data to solve the full-rank regression matrix in the reorganized linear equation. Optimize the pre-designed excitation trajectory calculation formula using the criterion of minimizing the condition number of the full-rank regression matrix to obtain the parameters in the excitation trajectory calculation formula. Step S5: applying the least square method based on the filtered data to identify the parameters of the minimum inertia parameter matrix in the reorganized linear equation, and using the minimum inertia parameter matrix as the excavator arm dynamic parameter matrix; Step S6: Substitute the excavator arm dynamic parameter matrix into the reorganized linear equation to obtain the identified excavator arm dynamic feedforward model; Step S7: inputting the feedforward signal output by the excavator arm dynamics feedforward model into a pre-built three-loop PID control system, and the three-loop PID control system outputs the control results of each joint of the excavator arm; The calculation formula of the pre-built excavator arm dynamics model is as follows: ； in, To control the torque, 、 、 are the angle, angular velocity and angular acceleration of each joint of the excavator arm, is the moment of inertia, is the coupling torque of Coriolis moment and centripetal moment, is the gravitational moment, is the friction torque.

2. The excavator arm dynamics control method according to claim 1, characterized in that: The method further includes step S8, in which the control results of each joint of the excavator arm output by the three-loop PID control system are matched with the preset control results of each joint of the excavator arm. When the matching degree is less than the matching threshold, the method returns to step S3 and repeats steps S3-S6 to obtain a more ideal excavator arm dynamics feedforward model.

3. The excavator arm dynamics control method according to claim 1 or 2, characterized in that: ； in: For Take the sign function, is the Coulomb friction coefficient, is the viscous friction coefficient, is the stribeck friction coefficient.

4. The excavator arm dynamics control method according to claim 1 or 2, characterized in that: The calculation formula of the linear equation of the excavator arm dynamic model is as follows: ； in: To control the torque, is the regression matrix, 、 、 are the angle, angular velocity and angular acceleration of each joint of the excavator arm, is the inertia parameter matrix.

5. The excavator arm dynamics control method according to claim 4, characterized in that: The calculation formula of the reorganized linear equation is as follows: ； in, To control the torque, is the full-rank regression matrix, is the minimum inertia parameter matrix.

6. The excavator arm dynamics control method according to claim 5, characterized in that: represents the dynamic parameters of N joints, Represents the dynamic parameters of the Nth joint; The minimum inertia matrix element of joint i can be expressed as: ； in, are the six parameters of the inertia tensor matrix of joint i in the x, y, and z directions, is the mass of joint i, , , are the components of the center of mass position of joint i in the x, y, and z directions of the joint coordinate system, is the Coulomb friction coefficient, is the viscous friction coefficient, is the stribeck friction coefficient, is the unit density of joint i.

7. The excavator arm dynamics control method according to claim 1 or 2, characterized in that: The calculation formula of the pre-designed excitation trajectory is as follows: ； in, is the fundamental frequency. Each joint should select the same fundamental frequency to ensure the periodicity of the entire movement. N represents the number of harmonics. represents harmonics, Indicates the moment, , is the coefficient, is a constant term, i represents the joint; The parameters in the calculation formula of the obtained excitation trajectory are , , .

8. The excavator arm dynamics control method according to claim 1 or 2, characterized in that: The identified parameters in the linear equation of the reorganized excavator arm dynamics model are the minimum inertia parameter matrix Parameters of .

9. The excavator arm dynamics control method according to claim 1 or 2, characterized in that: Pre-built three-loop PID control system includes: position loop, speed loop and current loop; Position loop: The input position control command P_ref and the position feedback command P_fb are fused and input into the position control module. The position control module outputs the target speed command V_ref. The control module adopts proportional regulation. Speed loop: The input target speed command V_ref and the speed feedback command V_fb are fused and input into the speed control module. The speed control module outputs the target acceleration command A_ref. The speed control module adopts proportional and integral regulation. Current loop: The input target acceleration command A_ref, acceleration feedback command A_fb and the feedforward signal output by the excavator arm dynamics feedforward model are fused and input into the current control module. The output of the current control module is multiplied by a current coefficient k and used as the control signal of the joint solenoid valve. The current control module has proportional, integral and differential adjustments.

Images