A method and system for automatically controlling the motion trajectory of an excavator

Abstract

The present invention relates to the field of automatic control technology, and in particular to a method and system for automatically controlling the motion trajectory of an excavator, comprising: collecting displacement data of the boom hydraulic cylinder piston rod, the dipper arm hydraulic cylinder piston rod, and the bucket hydraulic cylinder piston rod; converting the displacement data and the angular displacement data; compensating the excavator piston rod according to the difference between the excavator's desired angular displacement and the real-time angular displacement through a PID algorithm, and converting the compensation current signal to output to a control valve; in the process of automatic excavation trajectory, analyzing the dynamic characteristics and stability characteristics according to the curve, using the PD control algorithm to judge the piston rod displacement, and executing subsequent operations based on the judgment result. The present invention conducts in-depth and reliable trajectory control algorithm research on the excavator based on the dynamic model, which can not only realize automatic excavation of the excavator, but also alleviate the large impact generated by each joint, improve the executability of the trajectory, and enable the automatic excavation trajectory of the excavator to move stably and accurately.

Description

Technical Field

[0001] The present invention relates to the field of automatic control technology, and in particular to a method and system for automatically controlling the motion trajectory of an excavator. Background Art

[0002] As an important multifunctional engineering machinery, excavators have been widely used in mechanized construction processes such as infrastructure construction, urbanization and new rural construction, farmland transformation, water conservancy projects, mining and modern military projects. They play a great role in saving labor, reducing heavy physical labor, improving labor productivity, speeding up construction, ensuring project quality and reducing costs.

[0003] Operating a traditional excavator requires the driver to manipulate various control buttons and operating handles in the cab to enable the excavator to perform basic functions such as travel, rotation, digging, lifting, loading, and bulldozing. For more complex working conditions such as leveling land and excavating on slopes, the driver needs to operate multiple operating handles to control the hydraulic cylinders that drive the boom, bucket, and bucket to achieve the combined movement of the boom, bucket, and arm. This is more difficult to operate and places higher demands on the driver's operating skills, requiring a lot of time and cost for driver training. On the other hand, the working environment of the excavator is relatively harsh. The driver not only faces environmental pollution and noise pollution, but also has a high workload and requires longer working time. Therefore, intelligent excavator control is the best choice to effectively solve the above problems.

[0004] Among the many intelligent technologies in mechanical excavators, autonomous excavation is the most complex, encompassing three key areas: identifying excavation conditions, planning excavation trajectories, and controlling the excavation process. However, most researchers approach excavator trajectory planning from a kinematic perspective, often failing to consider the significant variations in the output force of each drive hydraulic cylinder due to load fluctuations. This results in low hydraulic cylinder motion control accuracy, hindering the realization of complex motion trajectories. Therefore, linking the output force of each drive hydraulic cylinder with the operating trajectory during excavator trajectory planning, thereby facilitating trajectory motion control, is a key factor in autonomous excavation.

[0005] The accuracy of trajectory planning for an excavator robot depends not only on its kinematics but also on the accuracy of its dynamic mathematical model. Therefore, it's crucial to analyze not only the robot's kinematics but also the impact of its dynamics. The key is to minimize the error between the robot's planned trajectory and the desired trajectory, and to design a controller that meets control requirements. Summary of the Invention

[0006] In response to the shortcomings of existing algorithms, the present invention conducts in-depth and reliable trajectory control algorithm research on excavators based on dynamic models, which can not only realize automatic excavation of the excavator, but also alleviate the large impact generated by each joint, improve the executability of the trajectory, and enable the automatic excavation trajectory of the excavator to move stably and accurately.

[0007] The technical solution adopted by the present invention is: a method for automatically controlling the motion trajectory of an excavator includes the following steps:

[0008] Step 1: Using a boom displacement sensor, a bucket arm displacement sensor, and a bucket displacement sensor, respectively, collect boom hydraulic cylinder piston rod displacement data, bucket arm hydraulic cylinder piston rod displacement data, and bucket hydraulic cylinder piston rod displacement data;

[0009] Step 2: Convert the displacement data to the angular displacement data through the displacement and angular displacement conversion algorithm;

[0010] Furthermore, the displacement and angular displacement conversion algorithm includes: boom hydraulic cylinder piston rod displacement S1 and angular displacement θ1, arm hydraulic cylinder piston rod displacement S2 and angular displacement β2, and bucket hydraulic cylinder piston rod displacement S3 and angular displacement β3.

[0011] Furthermore, the formula for the boom hydraulic cylinder piston rod displacement S1 and angular displacement θ1 is:

[0012]

[0013] in, BO is the distance between the top hinge point B of the boom hydraulic cylinder piston rod and the base O, AO is the distance between the bottom hinge point A of the boom hydraulic cylinder and the base O, EO is the distance between the hinge point E at one end of the boom and the base O, BE is the distance between the hinge point E at one end of the boom and the hinge point B at the top hinge point of the boom hydraulic cylinder piston rod, θ1 is the angular displacement of the boom based on the horizontal coordinate X0, OO1 is the vertical coordinate Y0 distance between the bottom hinge point A of the boom hydraulic cylinder and the base O, ∠EOB is an acute angle of the triangle EOB constructed by the dotted line, and ∠OAO1 is an acute angle of the triangle OAO1 constructed by the dotted line.

[0014] Furthermore, the formula of the boom hydraulic cylinder piston rod displacement S2 and angular displacement β2 is:

[0015]

[0016] Among them, ∠CED=2π-∠CEO-β2-∠DEJ, CE is the distance between the hinge point C at the bottom end of the boom hydraulic cylinder and the hinge point E at one end of the boom, DE is the distance between the hinge point D at the top end of the boom hydraulic cylinder piston rod and the hinge point E at one end of the boom, CO is the distance between the hinge point C at the bottom end of the boom hydraulic cylinder and the base O, EJ is the distance between the hinge point E at one end of the boom and the hinge point J at one end of the boom, DJ is the distance between the hinge point D at the top end of the boom hydraulic cylinder piston rod and the hinge point J at one end of the boom, β2 is the angular displacement between the construction lines EO and EJ, ∠CED is an acute angle of the triangle CED constructed by the dotted line, ∠CEO is an acute angle of the triangle CEO constructed by the dotted line, and ∠DEJ is an acute angle of the triangle DEJ constructed by the dotted line.

[0017] Furthermore, the formula of bucket hydraulic cylinder piston rod displacement S3 and angular displacement β3 is:

[0018]

[0019] Among them, ∠FGH=2π-∠FGE-∠EGJ-∠JGI-∠HGI,

[0020]

[0021]

[0022] ∠GJI=2π-∠EJG-β3-∠KJI, FG is the distance between the hinge point F at the bottom end of the bucket hydraulic cylinder and the hinge point G at one end of the movable hinge frame, HG is the distance between the hinge point H at the top end of the bucket hydraulic cylinder piston rod and the hinge point G at one end of the movable hinge frame, GE is the distance between the hinge point E at one end of the boom and the hinge point G at one end of the movable hinge frame, EF is the distance between the hinge point E at one end of the boom and the hinge point F at the bottom end of the bucket hydraulic cylinder, JG is the distance between the hinge point J at one end of the bucket arm and the hinge point G at one end of the movable hinge frame, GI is the distance between the two ends of the movable hinge frame, JI is the distance between the hinge point J at one end of the bucket and the hinge point I at one end of the movable hinge frame, HI is the distance between the hinge point H at the top end of the bucket hydraulic cylinder piston rod and the hinge point I at one end of the movable hinge frame, and β3 is the distance between EJ and J K is the angular displacement between the hinge point J at one end of the bucket and the end K of the bucket, KI is the distance between the end K of the bucket and the hinge point I at one end of the movable hinge frame, ∠FGH is an acute angle of the triangle FGH constructed by the dotted line, ∠FGE is an acute angle of the triangle FGE constructed by the dotted line, ∠EGJ is an acute angle of the triangle EGJ constructed by the dotted line, ∠JGI is an acute angle of the triangle JGI constructed by the dotted line, ∠HGI is an acute angle of the triangle HGI constructed by the dotted line, ∠GJI is an acute angle of the triangle GJI constructed by the dotted line, ∠EJG is an acute angle of the triangle EJG constructed by the dotted line, and ∠KJI is an acute angle of the triangle KJI constructed by the dotted line.

[0023] Step 3: The piston rod of the excavator is compensated according to the difference between the desired angular displacement and the real-time angular displacement of the excavator through the PID algorithm, and the compensation current signal is output to the control valve to drive the piston rod of the hydraulic cylinder to perform compensation movement, thereby reducing the difference between the real-time angular displacement and the desired angular displacement;

[0024] Further, specifically including:

[0025] Step 31: converting the real-time displacement values of the piston rods of the boom hydraulic cylinder, the bucket arm hydraulic cylinder, and the bucket hydraulic cylinder detected by the boom displacement sensor, the bucket arm displacement sensor, and the bucket displacement sensor into real-time angular displacements;

[0026] Step 32: Determine target displacement values of the piston rods of the boom hydraulic cylinder, the arm hydraulic cylinder, and the bucket hydraulic cylinder based on the desired angular displacement, subtract the target displacement values from the real-time displacement values detected by the boom displacement sensor, the arm displacement sensor, and the bucket displacement sensor, and perform PID calculation.

[0027] Step 33: Convert the PID control quantity into a current signal and output it to the boom control valve, the arm control valve and the bucket control valve, thereby driving the piston rods of the boom hydraulic cylinder, the arm hydraulic cylinder and the bucket hydraulic cylinder to perform compensation movement.

[0028] Step 4: During the automatic excavation trajectory, the dynamic and stability characteristics are analyzed according to the curve, the PD control algorithm is used to judge the displacement of the plug rod, and subsequent operations are performed based on the judgment results.

[0029] Further, specifically including:

[0030] Step 41: According to the dynamic equation of the Newton-Euler dynamic equilibrium method, the actual displacement values detected by the boom displacement sensor, the arm displacement sensor, and the bucket displacement sensor are written into the PLC controller through the above formulas (1), (2), and (3) and converted into corresponding angular displacements to calculate M(q), and the estimated value of G(q) and and through Consider the nonlinear feedback control law as a new input quantity;

[0031] Furthermore, the nonlinear feedback control law formula is:

[0032]

[0033] Where q = [θ1β2β3] T , q is the joint angular displacement, and the instantaneous Angular velocity and instantaneous angular acceleration vector, M(q) is the symmetric positive definite inertia matrix, is the Coriolis force and centrifugal force matrix, G(q) is the gravity vector, τ=[τ1τ2τ3] T is the control torque vector, τ1 is the boom control torque, τ2 is the arm control torque, and τ3 is the bucket control torque.

[0034] Step 42: Calculate the trajectory error by setting the weight K P and K V Make the trajectory error converge.

[0035] Furthermore, the trajectory error is expressed as:

[0036]

[0037] get:

[0038]

[0039] Among them, e is the error value between theoretical angular displacement and real-time angular displacement, The error between the theoretical angular velocity and the real-time angular velocity, is the theoretical angular acceleration K P , K V is the weight value, and formula (6) appropriately sets the weight K P and K V Make the trajectory error converge to 0.

[0040] A system for automatically controlling the motion trajectory of an excavator includes: a PLC controller, a boom control valve, a boom hydraulic cylinder, an arm control valve, an arm hydraulic cylinder, a bucket control valve, a bucket hydraulic cylinder, a boom displacement sensor, an arm displacement sensor, and a bucket displacement sensor, wherein an output end of the PLC controller is connected to an electromagnet input terminal of the boom control valve, the arm control valve, and the bucket control valve;

[0041] The boom control valve is connected to the inlet and outlet ports of the boom hydraulic cylinder through hydraulic pipelines. The boom displacement sensor is installed on the piston rod of the boom hydraulic cylinder to test the real-time displacement of the piston rod of the boom hydraulic cylinder.

[0042] The arm displacement sensor is installed on the piston rod of the arm hydraulic cylinder and is used to test the real-time displacement of the piston rod of the arm hydraulic cylinder;

[0043] The bucket displacement sensor is installed on the piston rod of the bucket hydraulic cylinder and is used to test the real-time displacement of the piston rod of the bucket hydraulic cylinder.

[0044] The PLC controller is used to collect data from the bucket displacement sensor, the dipper arm displacement sensor and the boom displacement sensor for the calculation of the PID control algorithm and the PD control algorithm, thereby realizing the control of the excavator.

[0045] Beneficial effects of the present invention:

[0046] 1. Realize automatic excavation of excavators, effectively reduce heavy physical labor, improve labor productivity, speed up construction, ensure project quality and reduce costs;

[0047] 2. To address the load force fluctuations that occur during the excavation trajectory, which cause excessive changes in the output force of each driving hydraulic cylinder and lead to low hydraulic cylinder motion control accuracy, we analyzed the dynamic and steady-state characteristics of the excavation trajectory and implemented the corresponding closed-loop PD control method. This can alleviate the large impact on each joint, improve the trajectory's executability, and enable the excavator's automatic excavation trajectory to move stably and accurately.

[0048] 3. It has the function of switching between manual and automatic operation, and is equipped with corresponding abnormal action alarm indicator lights during manual control, which increases the safety of manual control. Therefore, the invention is simple, reliable, easy to implement, highly practical, and highly automated. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 This is a simplified diagram of the dimensions of the excavator's working device;

[0050] Figure 2 It is a structural diagram of the control system of the present invention;

[0051] Figure 3 It is a schematic diagram of the hydraulic system of the present invention;

[0052] Figure 4 It is a control flow diagram of the present invention;

[0053] Figure 5 This is a common PID control block diagram;

[0054] Figure 6 is a PD control block diagram of the present invention;

[0055] Figure 2 Middle: 1. Excavator, 2. PLC controller, 3. Boom control valve, 4. Boom hydraulic cylinder, 5. Arm control valve, 6. Arm hydraulic cylinder, 7. Bucket control valve, 8. Bucket hydraulic cylinder, 9. Boom displacement sensor, 10. Arm displacement sensor, 11. Bucket displacement sensor, 12. Boom abnormality alarm indicator, 13. Arm abnormality alarm indicator, 14. Bucket abnormality alarm indicator;

[0056] Figure 3Middle: 15. Fuel tank, 16. Engine, 17. Double-acting pump, 18. Pressure reducing valve. DETAILED DESCRIPTION

[0057] The present invention will be further described below in conjunction with the accompanying drawings and embodiments. This figure is a simplified schematic diagram, which only illustrates the basic structure of the present invention in a schematic manner, and therefore only shows the components related to the present invention.

[0058] like Figure 1 The research object of the present invention is an excavator 1, which can convert the displacement of the hydraulic cylinder into the corresponding angular displacement.

[0059] like Figure 2 The system of the automatic control method of the motion trajectory of an excavator includes an excavator 1, a PLC controller 2, a boom control valve 3, a boom hydraulic cylinder 4, an arm control valve 5, an arm hydraulic cylinder 6, a bucket control valve 7, a bucket hydraulic cylinder 8, a boom displacement sensor 9, an arm displacement sensor 10, a bucket displacement sensor 11, a boom abnormality alarm indicator light 12, an arm abnormality alarm indicator light 13 and a bucket abnormality alarm indicator light 14. A manual and automatic operation switching button is installed on the cab of the excavator 1. The automatic operation switching button is connected to the input terminal of the PLC controller 2 through an electric wire. The output terminal of the PLC controller 2 is connected to the electromagnet input terminal of the boom control valve 3, the arm control valve 5 and the bucket control valve 8 through an electric wire respectively. The boom control valve 3 is connected to the oil inlet and outlet of the boom hydraulic cylinder 4 through a hydraulic pipeline. The piston rod of the boom hydraulic cylinder 4 is equipped with a ... A boom displacement sensor 9 is provided for measuring the real-time displacement of the piston rod of the boom hydraulic cylinder 4. The arm control valve 5 is connected to the oil inlet and outlet of the arm hydraulic cylinder 6 through a hydraulic pipeline. A boom displacement sensor 10 for measuring the real-time displacement of the piston rod of the arm hydraulic cylinder 6 is installed on the piston rod of the arm hydraulic cylinder 6. The bucket control valve 7 is connected to the oil inlet and outlet of the bucket hydraulic cylinder 8 through a hydraulic pipeline. A bucket displacement sensor 11 for measuring the real-time displacement of the piston rod of the bucket hydraulic cylinder 8 is installed on the piston rod of the bucket hydraulic cylinder 8. While realizing the telescopic action of the hydraulic cylinder, the data is fed back to the PLC controller 2, thereby completing a series of closed-loop compound trajectory motions. The input terminals of the boom abnormality alarm indicator light 12, the arm abnormality alarm indicator light 13 and the bucket abnormality alarm indicator light 14 are respectively connected to the input terminals of the PLC controller 2. When the abnormality alarm indicator light is on, the action stops.

[0060] like Figure 3The figure shows the principle diagram of the hydraulic system of the present invention, in which the oil inlet and outlet A, B of the boom control valve 3 are connected to the rodless chamber oil port and the rod chamber oil port of the boom hydraulic cylinder 4 through hydraulic pipelines, the oil inlet and outlet A, B of the arm control valve 5 are connected to the rodless chamber oil port and the rod chamber oil port of the arm hydraulic cylinder 6 through hydraulic pipelines, the oil inlet and outlet A, B of the bucket control valve 7 are connected to the rodless chamber oil port and the rod chamber oil port of the bucket hydraulic cylinder 8 through hydraulic pipelines, the oil inlet P of the boom control valve 3, the arm control valve 5 and the bucket control valve 7 are respectively connected to the total oil outlet through hydraulic pipelines, the oil outlets of the boom control valve 3, the arm control valve 5 and the bucket control valve 7 are respectively connected to the total oil outlet T is connected to the main oil inlet through hydraulic pipelines and returns to the oil tank 15. The engine 16 provides power to the double-acting pump 17, and the hydraulic oil in the oil tank 15 is transported to the main oil outlet through the hydraulic pipeline. The pressure reducing valve 18 is connected to the main oil outlet to play a pressure reducing role. As shown in the hydraulic pipeline connection valve in the figure, when excavation operation is required, the proportional solenoid input terminals of the boom control valve 3, the bucket control valve 5 and the bucket control valve 7 are simultaneously input with corresponding current signals through the PLC controller 2, which can realize the telescopic position and speed control of the piston rods of the boom hydraulic cylinder 4, the bucket hydraulic cylinder 6 and the bucket hydraulic cylinder 8, thereby completing a series of compound trajectory movements.

[0061] like Figure 4 , a motion trajectory automatic control method for an excavator includes:

[0062] Step 1, initialization: Initialize the control system of the excavator 1, check whether the input and output parameters of the PLC controller 2, boom control valve 3, arm control valve 5, bucket control valve 7, boom displacement sensor 9, arm displacement sensor 10, bucket displacement sensor 11, etc. are normal. If not, do not proceed to the next step, and send an alarm signal to the boom abnormality alarm indicator 12, arm abnormality alarm indicator 13, and bucket abnormality alarm indicator 14; if normal, continue to proceed to the next step;

[0063] Step 2: Parameter setting: Set the angular displacement of the excavator's boom, arm, and bucket, the displacement-to-angular displacement parameters, the initial weight parameters, the PID controller parameters, etc., and then proceed to the next step;

[0064] Step 3, data acquisition: The PLC controller 2 collects information such as the piston rod extension displacement of the boom hydraulic cylinder 4, the bucket hydraulic cylinder 6, and the bucket hydraulic cylinder 8 through the boom displacement sensor 9, the dipper arm displacement sensor 10, and the bucket displacement sensor 11 on the excavator 1, and then proceeds to the next step;

[0065] Step 4: Automatic excavation mode determination: Determine whether the automatic excavation mode is selected by pressing the manual and automatic operation switch button on the cab of the excavator 1. If the mode is manual, proceed to step 5.1; if the mode is automatic, proceed to step 5.2.

[0066] Step 5.1, Manual mode: After the manual mode is turned on, the driver can freely operate. If an alarm is abnormal during the excavation process, the operation will be suspended. Otherwise, the excavation operation can be completed and the next operation can be carried out;

[0067] Step 5.2, Automatic mode: After the automatic mode is turned on, Figure 5 As shown, the PLC controller 2 converts the piston rod displacement values of the boom hydraulic cylinder 4, the bucket hydraulic cylinder 6 and the bucket hydraulic cylinder 8 detected by the boom displacement sensor 9, the bucket displacement sensor 10 and the bucket displacement sensor 11 into real-time angular displacement according to the displacement and angular displacement conversion formula, and determines the target displacement values of the piston rods of the boom hydraulic cylinder 4, the bucket hydraulic cylinder 6 and the bucket hydraulic cylinder 8 according to the expected angular displacement, and makes a difference between the target displacement value and the actual displacement value detected by the boom displacement sensor 9, the bucket displacement sensor 10 and the bucket displacement sensor 11, and performs PID operation, and converts the calculated value into a current signal I and outputs it to the boom control valve 3, the bucket control valve 5 and the bucket control valve 7, thereby driving the piston rods of the boom hydraulic cylinder 4, the bucket hydraulic cylinder 6 and the bucket hydraulic cylinder 8 to perform compensatory movement, thereby reducing the difference between the real-time angular displacement and the expected angular displacement. Distance, in the process of automatic excavation trajectory, according to the curve obtained by the PLC controller 2, whether the boom hydraulic cylinder 4, the dipper hydraulic cylinder 6 and the bucket hydraulic cylinder 8 in the dynamic state have jitter phenomena, generate mutation factors, and whether they meet the stability requirements. If they meet the requirements, then further judge whether the piston rod displacements of the boom hydraulic cylinder 4, the dipper hydraulic cylinder 6 and the bucket hydraulic cylinder 8 have reached the expected displacement. Otherwise, enter the control algorithm optimization program. When judging whether the piston rod displacements of the boom hydraulic cylinder 4, the dipper hydraulic cylinder 6 and the bucket hydraulic cylinder 8 have reached the expected displacement, if they have reached the expected displacement, the excavation operation is completed and the next operation is entered. Otherwise, check whether an alarm abnormality occurs. If an alarm abnormality occurs, the operation will be suspended. Otherwise, enter the control algorithm optimization program again, optimize the controller algorithm, return to the controller module operation, until the excavation operation is completed and the next operation is entered;

[0068] like Figure 1 The displacement and angular displacement conversion formulas include: boom hydraulic cylinder piston rod displacement S1 and angular displacement θ1, dipper hydraulic cylinder piston rod displacement S2 and angular displacement β2, and bucket hydraulic cylinder piston rod displacement S3 and angular displacement β3;

[0069] Among them, the relationship between the boom hydraulic cylinder piston rod displacement S1 and the angular displacement θ1 is:

[0070]

[0071] in, BO is the distance between the top hinge point B of the boom hydraulic cylinder piston rod and the base O, AO is the distance between the bottom hinge point A of the boom hydraulic cylinder and the base O, EO is the distance between the hinge point E at one end of the boom and the base O, BE is the distance between the hinge point E at one end of the boom and the hinge point B at the top hinge point of the boom hydraulic cylinder piston rod, θ1 is the angular displacement of the boom based on the horizontal coordinate X0, OO1 is the vertical coordinate Y0 distance between the bottom hinge point A of the boom hydraulic cylinder and the base O, ∠EOB is an acute angle of the triangle EOB constructed by the dotted line, and ∠OAO1 is an acute angle of the triangle OAO1 constructed by the dotted line.

[0072] The relationship between the piston rod displacement S2 and the angular displacement β2 of the bucket hydraulic cylinder is:

[0073]

[0074] Among them, ∠CED=2π-∠CEO-β2-∠DEJ, CE is the distance between the hinge point C at the bottom end of the boom hydraulic cylinder and the hinge point E at one end of the boom, DE is the distance between the hinge point D at the top end of the boom hydraulic cylinder piston rod and the hinge point E at one end of the boom, CO is the distance between the hinge point C at the bottom end of the boom hydraulic cylinder and the base O, EJ is the distance between the hinge point E at one end of the boom and the hinge point J at one end of the boom, DJ is the distance between the hinge point D at the top end of the boom hydraulic cylinder piston rod and the hinge point J at one end of the boom, β2 is the angular displacement between the construction lines EO and EJ, ∠CED is an acute angle of the triangle CED constructed by the dotted line, ∠CEO is an acute angle of the triangle CEO constructed by the dotted line, and ∠DEJ is an acute angle of the triangle DEJ constructed by the dotted line.

[0075] Relationship between bucket hydraulic cylinder piston rod displacement S3 and angular displacement β3:

[0076]

[0077] Among them, ∠FGH=2π-∠FGE-∠EGJ-∠JGI-∠HGI,

[0078]

[0079]

[0080] ∠GJI=2π-∠EJG-β3-∠KJI, FG is the distance between the hinge point F at the bottom end of the bucket hydraulic cylinder and the hinge point G at one end of the movable hinge frame, HG is the distance between the hinge point H at the top end of the bucket hydraulic cylinder piston rod and the hinge point G at one end of the movable hinge frame, GE is the distance between the hinge point E at one end of the boom and the hinge point G at one end of the movable hinge frame, EF is the distance between the hinge point E at one end of the boom and the hinge point F at the bottom end of the bucket hydraulic cylinder, JG is the distance between the hinge point J at one end of the bucket arm and the hinge point G at one end of the movable hinge frame, GI is the distance between the two ends of the movable hinge frame, JI is the distance between the hinge point J at one end of the bucket and the hinge point I at one end of the movable hinge frame, HI is the distance between the hinge point H at the top end of the bucket hydraulic cylinder piston rod and the hinge point I at one end of the movable hinge frame, and β3 is the distance between EJ and J K is the angular displacement between the hinge point J at one end of the bucket and the end K of the bucket, KI is the distance between the end K of the bucket and the hinge point I at one end of the movable hinge frame, ∠FGH is an acute angle of the triangle FGH constructed by the dotted line, ∠FGE is an acute angle of the triangle FGE constructed by the dotted line, ∠EGJ is an acute angle of the triangle EGJ constructed by the dotted line, ∠JGI is an acute angle of the triangle JGI constructed by the dotted line, ∠HGI is an acute angle of the triangle HGI constructed by the dotted line, ∠GJI is an acute angle of the triangle GJI constructed by the dotted line, ∠EJG is an acute angle of the triangle EJG constructed by the dotted line, and ∠KJI is an acute angle of the triangle KJI constructed by the dotted line.

[0081] like Figure 6 The PD control block diagram of the control algorithm of the present invention is shown. According to the dynamic equation of the Newton-Euler dynamic equilibrium method, as shown in formula (4), the actual displacement values detected by the boom displacement sensor 9, the dipper arm displacement sensor 10 and the bucket displacement sensor 11 are written into the PLC controller 2 through the above formulas (1), (2) and (3) and converted into corresponding angular displacements, and M(q) and and the estimated value of G(q) and And through Consider the nonlinear feedback control law as a new input:

[0082]

[0083] Where q = [θ1β2β3] T , q is the joint angular displacement, and the instantaneous Angular velocity and instantaneous angular acceleration vector, M(q) is the symmetric positive definite inertia matrix, is the Coriolis force and centrifugal force matrix, G(q) is the gravity vector, τ=[τ1τ2τ3] T is the control torque vector, τ1 is the boom control torque, τ2 is the arm control torque, and τ3 is the bucket control torque.

[0084] The trajectory error is expressed as:

[0085]

[0086] You can get:

[0087]

[0088] Where, e is the error between the theoretical angular displacement and the actual angular displacement, The error between the theoretical angular velocity and the actual angular velocity, is the theoretical angular acceleration Formula (6) appropriately sets the weight K P and K V Make the trajectory error converge to 0.

[0089] With the above-described preferred embodiments of the present invention as a guide, and with reference to the above description, relevant personnel are fully capable of making various changes and modifications without departing from the technical scope of this invention. The technical scope of this invention is not limited to the contents of the specification and must be determined according to the scope of the claims.

Claims

1. A method for automatically controlling the motion trajectory of an excavator, characterized in that: The following steps are involved: Step 1: Using a boom displacement sensor, a bucket arm displacement sensor, and a bucket displacement sensor, respectively, collect boom hydraulic cylinder piston rod displacement data, bucket arm hydraulic cylinder piston rod displacement data, and bucket hydraulic cylinder piston rod displacement data; Step 2: Convert the displacement data to the angular displacement data through the displacement and angular displacement conversion algorithm; The displacement and angular displacement conversion algorithm includes: boom hydraulic cylinder piston rod displacement S1 and angular displacement θ1, arm hydraulic cylinder piston rod displacement S2 and angular displacement β2, and bucket hydraulic cylinder piston rod displacement S3 and angular displacement β3; Step 3: The piston rod of the excavator is compensated according to the difference between the desired angular displacement and the real-time angular displacement of the excavator through the PID algorithm, and the compensation current signal is output to the control valve to drive the piston rod of the hydraulic cylinder to perform compensation movement, thereby reducing the difference between the real-time angular displacement and the desired angular displacement; Step three specifically includes: Step 31: converting the real-time displacement values of the piston rods of the boom hydraulic cylinder, the bucket arm hydraulic cylinder, and the bucket hydraulic cylinder detected by the boom displacement sensor, the bucket arm displacement sensor, and the bucket displacement sensor into real-time angular displacements; Step 32: Determine target displacement values of the piston rods of the boom hydraulic cylinder, the arm hydraulic cylinder, and the bucket hydraulic cylinder based on the desired angular displacement, subtract the target displacement values from the real-time displacement values detected by the boom displacement sensor, the arm displacement sensor, and the bucket displacement sensor, and perform PID calculation. Step 33: Convert the PID control quantity into a current signal and output it to the boom control valve, the arm control valve, and the bucket control valve, thereby driving the piston rods of the boom hydraulic cylinder, the arm hydraulic cylinder, and the bucket hydraulic cylinder to perform compensatory motion; Step 4: During the automatic excavation trajectory, the dynamic and stability characteristics are analyzed according to the curve, the PD control algorithm is used to judge the displacement of the piston rod, and subsequent operations are performed according to the judgment results; Step 4 specifically includes: Step 41: According to the dynamic equation of the Newton-Euler dynamic equilibrium method, the actual displacement values detected by the boom displacement sensor, the arm displacement sensor, and the bucket displacement sensor are written into the PLC controller and converted into corresponding angular displacements to calculate M(q), and the estimated value of G(q) and and through Consider the nonlinear feedback control law as a new input quantity; Step 42: Calculate the trajectory error by setting the weight K P and K V Make the trajectory error converge; The formula for trajectory error is: get: Where e is the theoretical angular displacement q d The error value with the actual angular displacement q, Theoretical angular velocity The actual angular velocity The error value, is the theoretical angular acceleration K P , K V is the weight value.

2. The method for automatically controlling the motion trajectory of an excavator according to claim 1, characterized in that: The formula for boom hydraulic cylinder piston rod displacement S1 and angular displacement θ1 is: in, BO is the distance between the top hinge point B of the boom hydraulic cylinder piston rod and the base O, AO is the distance between the bottom hinge point A of the boom hydraulic cylinder and the base O, EO is the distance between the hinge point E at one end of the boom and the base O, BE is the distance between the hinge point E at one end of the boom and the hinge point B at the top hinge point of the boom hydraulic cylinder piston rod, θ1 is the angular displacement of the boom based on the horizontal coordinate X0, and OO1 is the vertical coordinate Y0 distance between the bottom hinge point A of the boom hydraulic cylinder and the base O.

3. The method for automatically controlling the motion trajectory of an excavator according to claim 1, wherein: The formula of the boom hydraulic cylinder piston rod displacement S2 and angular displacement β2 is: Among them, ∠CED=2π-∠CEO-β2-∠DEJ, CE is the distance between the hinge point C at the bottom end of the boom hydraulic cylinder and the hinge point E at one end of the boom, DE is the distance between the hinge point D at the top end of the boom hydraulic cylinder piston rod and the hinge point E at one end of the boom, CO is the distance between the hinge point C at the bottom end of the boom hydraulic cylinder and the base O, EJ is the distance between the hinge point E at one end of the boom and the hinge point J at one end of the boom, DJ is the distance between the hinge point D at the top end of the boom hydraulic cylinder piston rod and the hinge point J at one end of the boom, and β2 is the angular displacement between the construction line EO and EJ.

4. The method for automatically controlling the motion trajectory of an excavator according to claim 1, wherein: The formula of bucket hydraulic cylinder piston rod displacement S3 and angular displacement β3 is: Among them, ∠FGH=2π-∠FGE-∠EGJ-∠JGI-∠HGI, ∠GJI=2π-∠EJG-β3-∠KJI, FG is the distance between the hinge point F at the bottom end of the bucket hydraulic cylinder and the hinge point G at one end of the movable hinge frame, HG is the distance between the hinge point H at the top end of the bucket hydraulic cylinder piston rod and the hinge point G at one end of the movable hinge frame, GE is the distance between the hinge point E at one end of the boom and the hinge point G at one end of the movable hinge frame, EF is the distance between the hinge point E at one end of the boom and the hinge point F at the bottom end of the bucket hydraulic cylinder, JG is the distance between the hinge point J at one end of the bucket arm and the hinge point G at one end of the movable hinge frame, GI is the distance between the two ends of the movable hinge frame, JI is the distance between the hinge point J at one end of the bucket and the hinge point I at one end of the movable hinge frame, HI is the distance between the hinge point H at the top end of the bucket hydraulic cylinder piston rod and the hinge point I at one end of the movable hinge frame, β3 is the angular displacement between EJ and JK, KJ is the distance between the hinge point J at one end of the bucket and the end K of the bucket, and KI is the distance between the end K of the bucket and the hinge point I at one end of the movable hinge frame.

5. A system using the method for automatically controlling the motion trajectory of an excavator according to any one of claims 1 to 4, characterized in that: include: A PLC controller, a boom control valve, a boom hydraulic cylinder, an arm control valve, an arm hydraulic cylinder, a bucket control valve, a bucket hydraulic cylinder, a boom displacement sensor, an arm displacement sensor, and a bucket displacement sensor, wherein the output end of the controller is connected to the electromagnet input terminal of the boom control valve, the arm control valve, and the bucket control valve; The boom control valve is connected to the inlet and outlet ports of the boom hydraulic cylinder through hydraulic pipelines. The boom displacement sensor is installed on the piston rod of the boom hydraulic cylinder to test the real-time displacement of the piston rod of the boom hydraulic cylinder. The arm displacement sensor is installed on the piston rod of the arm hydraulic cylinder and is used to test the real-time displacement of the piston rod of the arm hydraulic cylinder; The bucket displacement sensor is installed on the piston rod of the bucket hydraulic cylinder and is used to test the real-time displacement of the piston rod of the bucket hydraulic cylinder; The PLC controller is used to collect data from the bucket displacement sensor, the dipper arm displacement sensor and the boom displacement sensor for the calculation of the PID control algorithm and the PD control algorithm to realize the control of the excavator.

Images