Control method and system of bulldozer working device

Abstract

The invention discloses a control method and system for a bulldozer working device, and relates to the technical field of engineering machinery control, and the method comprises the steps: building a base coordinate system {O}, a lifting coordinate system {A}, a pitching coordinate system {B} and a scraper knife tail end tooth tip coordinate system {C} of the bulldozer working device based on a D-H coordinate system; the Z-Y-Z Euler theorem is used for conducting rotation transformation on the base coordinate system {O}, the lifting coordinate system {A}, the pitching coordinate system {B} and the scraper knife tail end tooth top coordinate system {C}, and a forward kinematics solution of the bulldozer working device is obtained; performing formula transformation on the forward kinematics solution to obtain an inverse kinematics solution of the bulldozer working device; and based on the inverse kinematics solution, the lifting angle and the pitching angle of the bulldozer working device to be controlled are obtained by combining the position of the working target surface of the bulldozer. The lifting and pitching angles of the working device can be determined, the kinematics analysis result is more accurate, and precise control over the working device of the bulldozer is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of engineering machinery control, and in particular to a control method and system for a bulldozer working device. Background Art

[0002] As a kind of construction machinery, bulldozers are mainly used for bulldozing, filling and leveling the ground in large earthwork projects. In many cases, the working environment of bulldozers is relatively harsh. For example, when working in a pile, slope and trench environment, problems such as uneven body and excessive load on the blade may occur. In harsh environments, the driver's skills are particularly tested. There may be problems such as construction errors, insufficient data during the construction process, and low construction efficiency, and the construction quality cannot be maintained. Moreover, it becomes more difficult for the driver to perform precise construction, and it is also difficult to effectively protect the driver's safety. Therefore, intelligent unmanned bulldozers will become an important development direction in the future, which will meet the market demand for intelligent bulldozers and have broad prospects and practical application value.

[0003] The working device of a bulldozer is the most important part in its actual work, especially in harsh working environments. The working device is subject to large changes in external soil loads with high frequency, so the driver is required to operate the working device to lift and pitch continuously. This leads to low working efficiency of traditional bulldozers and affects the flatness of the ground.

[0004] At present, there are many studies on the autonomous control of excavators, because the working device of the excavator can be regarded as a four-degree-of-freedom mechanical arm, and during the operation of the excavator, the chassis of the body is fixed, and only the working device performs excavation operations. The working device can be autonomously planned and controlled without combining the body posture and orientation information of the excavator. During the operation of the bulldozer, the body posture and orientation of the bulldozer change in real time when driving on a bumpy road, resulting in the posture of the working device also changing in real time, and it is impossible to accurately locate the working surface of the working device. Therefore, there are few studies on the autonomous operation of unmanned bulldozers. The existing research on the automatic adjustment of the working posture of the working device of the bulldozer only sets an initial value of the posture angle, and controls the posture change of the working device within the set range by detecting the deviation information of the angle. This method can only meet the autonomous control of the working device of the bulldozer when performing large-scale flattening operations, but cannot meet the operation requirements of the bulldozer on complex roads with continuous changes such as slopes and gullies. Summary of the invention

[0005] In view of the shortcomings of the prior art, the purpose of the present invention is to provide a control method and system for a bulldozer working device. According to the position of the working device joints, a bulldozer base coordinate system and a working device body coordinate system are constructed, and the zyz Euler theorem is used to rotate and translate the coordinate systems to obtain the relationship between the transformation law and the working device posture, and then the lifting and pitching angles of the working device are determined, so that the kinematic analysis results are more accurate and precise control of the bulldozer working device is achieved.

[0006] In order to achieve the above object, the present invention is implemented through the following technical solutions:

[0007] In a first aspect, an embodiment of the present invention provides a method for controlling a bulldozer working device, comprising:

[0008] Based on the DH coordinate system, the base coordinate system {O}, the lifting coordinate system {A}, the pitch coordinate system {B} and the blade end tooth tip coordinate system {C} of the bulldozer working device are established;

[0009] The ZYZ Euler theorem is used to perform rotation transformation between the base coordinate system {O} and the lifting coordinate system {A}, the pitch coordinate system {B}, and the blade end tooth tip coordinate system {C} to obtain the forward kinematic solution of the bulldozer working device;

[0010] By transforming the formula of the forward kinematics solution, the inverse kinematics solution of the bulldozer working device is obtained;

[0011] Based on the inverse kinematics solution and combined with the working target surface position of the bulldozer, the lifting angle and pitch angle of the bulldozer working device to be controlled are obtained.

[0012] As a further implementation method, the axial direction of the bulldozer joint rotation axis is defined as the Z axis, the common perpendicular line of the Z axes of two adjacent joints is defined as the X axis, and the Y axis is determined using the right-hand rule.

[0013] As a further implementation method, the hinge O between the push frame and the center of the body crawler is taken as the origin, and the forward direction of the bulldozer is taken as X. O Axis, with the vertical direction upward as Z O Axis, parallel to the working surface is Y O Axis, establish the base coordinate system {O} of the bulldozer.

[0014] As a further implementation method, the axis direction of the rotating shaft is Z A Axis, with the push frame direction as X A axis, use the right-hand rule to determine the Y A The direction of the axis is perpendicular to the push frame and upward, and the working device lifting coordinate system {A} is established;

[0015] The origins of the coordinate system {A} and the base coordinate system {O} are defined on the same rotation center O.

[0016] As a further implementation method, the hinge B between the blade and the push frame is taken as the origin, and the axis direction of the rotating shaft is taken as Z. B Axis, with the direction of the blade as X B axis, and then use the right-hand rule to know that Y B The axis is perpendicular to the blade and points upward, establishing the working device pitch coordinate system {B}.

[0017] As a further implementation method, the tooth tip coordinate system {C} is established at the middle point C of the tooth tip at the end of the blade, with C as the origin, and the directions of its three axes are consistent with the pitch coordinate system {B}.

[0018] As a further implementation, the forward kinematic solution is expressed as:

[0019]

[0020] Among them, p x , p y , p z They represent the position of the tip of the blade end in the base coordinate system {O}, cθ1 = cosθ1, sθ1 = sinθ1, θ1 is the position of the blade around Z A The angle of the axis lifting; cθ2 = cosθ2, sθ2 = sinθ2, θ2 is the angle of the bulldozer blade around Z B The pitch angle of the axis, x ab ,y ab , z ab It represents the distance from the origin of the pitch coordinate system {B} to the origin of the lift coordinate system {A};

[0021] x ab Equal to the length of the bulldozer blade push rod l1, y ab =0, z ab =0, l BW is the thickness of the scraper blade, l bh It represents the height difference from the origin of the tooth tip coordinate system {C} to the pitch coordinate system {B}.

[0022] As a further implementation method, based on the inverse kinematics solution, the lifting angle and pitch angle of the bulldozer working device to be controlled are solved given the working target surface position of the bulldozer:

[0023]

[0024] As a further implementation, the bulldozer blade revolves around Z A The angle θ1 of the shaft lift is measured by the inclination sensor.

[0025] In the second aspect, an embodiment of the present invention also provides a control system for a bulldozer working device, based on the control method, comprising a first inclination sensor installed on the back side of the blade of the bulldozer working device and a second inclination sensor installed on one side of the push frame, wherein the first inclination sensor is used to detect the lifting angle, and the second inclination sensor is used to detect the pitch angle.

[0026] The beneficial effects of the present invention are as follows:

[0027] (1) The present invention constructs a bulldozer base coordinate system and a working device body coordinate system according to the position of the working device joint, and uses the zyz Euler theorem to rotate and translate the coordinate system, and obtains the relationship between the transformation law and the working device posture, and then determines the lifting and pitching angles of the working device, making the kinematic analysis results more accurate, thereby achieving precise control of the bulldozer working device.

[0028] (2) The present invention installs inclination sensors on the back of the shovel blade and the side beam of the push frame, respectively, to detect the lifting and pitching angles of the working device, providing a precise rotation angle for kinematic analysis, making the kinematic analysis results more accurate and the control accuracy of the working device higher. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0030] Figure 1 is a schematic structural diagram of a bulldozer working device according to one or more embodiments of the present invention;

[0031] Figure 2 is a schematic diagram of a coordinate system of a bulldozer working device according to one or more embodiments of the present invention;

[0032] Figure 3 is a schematic diagram of a bulldozer blade coordinate system according to one or more embodiments of the present invention;

[0033] Figure 4 The present invention is a flowchart of controlling a bulldozer working device according to one or more embodiments.

[0034] Among them, 1, the shovel, 2, the push frame, 3-1, the first lifting cylinder, 3-2, the second lifting cylinder, 4-1, the first pitch cylinder, 4-2, the second pitch cylinder, 5, the first inclination sensor, 6, the second inclination sensor. DETAILED DESCRIPTION

[0035] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used in the present invention have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0036] Embodiment 1:

[0037] In order to solve the problem that the existing bulldozer working device cannot meet the operating requirements of the bulldozer on complex roads with continuous changes such as slopes and gullies when autonomously controlled, the present embodiment provides a control method for the bulldozer working device. By performing kinematic analysis on the bulldozer working device, the working device and the body posture are matched to obtain the key quantity of autonomous control, thereby ensuring precise operation and improving operation efficiency.

[0038] like Figure 1 As shown, an inclination sensor is installed on the back of the blade 1 and the side beam of the push frame 2. The specific control method includes the following steps:

[0039] S1, such as Figure 2 As shown in the figure, according to the concept of establishing the DH coordinate system, the axial direction of the joint rotation axis is defined as the Z axis, the common perpendicular line of the Z axes of two adjacent joints is defined as the X axis, the direction of the X axis is optional, but generally points to the next joint, and then the Y axis is determined by the right-hand rule, and the base coordinate system {O}, the lifting coordinate system {A}, the pitch coordinate system {B} and the blade end tooth tip coordinate system {C} of the working device are constructed respectively; the hinge point between the push frame and the center of the body crawler is defined as point O, the hinge point O is defined as the origin, and the forward direction of the bulldozer is defined as point X. O Axis, with the vertical direction upward as Z O Axis, with the direction parallel to the working surface as Y O Axis, establish the base coordinate system {O} of the bulldozer.

[0040] During the lifting movement, the push frame 2 rotates around the hinge O, with O as the origin and the direction of the rotation axis as Z. A Axis, with the length direction of the side beam of the push frame 2 as X A axis, using the right-hand rule we know that Y A The direction of the axis is perpendicular to the push frame 2 and upward, and the working device lifting coordinate system {A} is established. The origin of the coordinate system {A} and the base coordinate system {O} are defined on the same rotation center O; during the pitching motion, the blade 1 rotates around the hinge B, with B as the origin and the direction of the rotation axis as Z. B Axis, with the direction of blade 1 as X B axis, and then use the right-hand rule to know that Y BThe axis is perpendicular to the blade 1 and points upward to establish the pitch coordinate system {B} of the working device; the position of the middle tooth tip of the blade 1 is the target surface required for bulldozer operation; therefore, the tooth tip coordinate system {C} is established at the middle point C of the tooth tip at the end of the blade 1, with C as the origin, and the directions of the three axes are the same as those of the pitch coordinate system {B}.

[0041] S2. The forward kinematics solution of the bulldozer working device is obtained through the rotation transformation of the base coordinate system {O} and the lifting coordinate system {A}, the pitch coordinate system {B} and the blade end tooth tip coordinate system {C}, and the position and posture of the blade end 1 are determined based on the forward kinematics solution.

[0042] In three-dimensional Cartesian space, the relationship between two adjacent coordinate systems is represented by a homogeneous transformation matrix using ZYZ Euler angles, where the rotation matrix between the base coordinate system {O} and the lifted coordinate system {A} is expressed as:

[0043]

[0044] Among them, r(x, 90°)×R(z, θ1) means that the base coordinate system {O} is first rotated 90° around the x-axis and then rotated θ1 around the z-axis to obtain the lifted coordinate system {A};

[0045] In formula (1), cθ1 = cosθ1, sθ1 = sinθ1, θ1 is the radius of the bulldozer blade around Z A The angle of the shaft lifting can be measured by the second inclination sensor 6.

[0046] In three-dimensional Cartesian space, let the position of the coordinate system {C} attached to the tooth tip of blade 1 be Indicates that p x , p y , p z They respectively represent the position of the tooth tip at the end of blade 1 in the base coordinate system {O}, and a homogeneous transformation matrix is ​​constructed for them, as shown below, to calculate their positions.

[0047]

[0048] Since there is both translation and rotation between the pitch coordinate system {B} and the lift coordinate system {A}, the rotation matrix between the pitch coordinate system {B} and the lift coordinate system {A} is expressed as:

[0049]

[0050] and

[0051] Among them, Trans(x ab ,y ab , z ab) represents the distance from the lifting coordinate system {A} to the pitching coordinate system {B} along the x, y, and z axes; R(z,θ2) represents the pitching coordinate system {B} obtained by translating the lifting coordinate system {A} and then rotating it around the z axis by θ2;

[0052] In formula (3), cθ2 = cosθ2, sθ2 = sinθ2, θ2 is the radius of the bulldozer blade around Z B The pitch angle of the axis, x ab ,y ab , z ab represents the distance from the origin of the pitch coordinate system {B} to the origin of the lift coordinate system {A}; since the coordinate system {A} to the coordinate system {B} only undergoes a translation transformation along the x-axis, x ab Equal to the length of the bulldozer blade push rod l1, y ab =0, z ab =0, l BW is the thickness of the scraper blade, l bh It represents the height difference from the origin of the tooth tip coordinate system {C} to the pitch coordinate system {B}, such as Figure 3 As shown;

[0053] Substituting equations (1), (3) and (4) into equation (2), we obtain the kinematic solution:

[0054] p x = l bw (cθ1cθ2-sθ1sθ2)-l bh (cθ1sθ2+sθ1cθ2)+x ab cθ1-y ab sθ1

[0055] p y =-z ab

[0056] p z = l bw (sθ1cθ2-sθ1sθ2)-l bh (sθ1sθ2-cθ1cθ2)+x ab sθ1+y ab cθ1

[0057] Right now

[0058] Among them, p x , p y , p z They respectively represent the positions of the tooth tips at the end of the blade in the base coordinate system {O}.

[0059] S3, transforming the above forward kinematics solution into an inverse kinematics solution for the bulldozer working device; based on the inverse kinematics solution, the lifting and pitching angles of the working device that need to be controlled can be solved given the working target surface position of the bulldozer;

[0060] Known

[0061] Will Multiply both sides

[0062] in The inverse matrix of is:

[0063] have to:

[0064]

[0065] The two sides of the equation are equal.

[0066] In the formula

[0067] First, multiply (6) by p z And multiply (7) by p x , and then subtract the two equations,

[0068] p2(p x cθ1+p z sθ1)-p x (-p x sθ1+p2cθ1)

[0069] =p2(l bw cθ2-l bh sθ2+x ab )-p x (l bw sθ2+l bh cθ2+y ab )

[0070] Simplifying the above formula, we get:

[0071] (p x 2 +p y 2 )sθ1=(p z l bw -p x l bh )cθ2-(p z l bh +p x l bw )sθ2+x ab p z -yab p x (8)

[0072] Multiply equation (6) by p x , and multiply equation (7) by p z , and then add the two equations together to get:

[0073] (p x 2 +p y 2 )cθ1=(p x l bw +p z l bh )cθ2+(p z l bw -p x l bh )sθ2+x ab p x +y ab p z (9)

[0074] Dividing equations (8) and (9) gives the expression for θ1:

[0075]

[0076] Substituting θ1 into the original formula (6), we can simplify the expression of θ2 to obtain:

[0077]

[0078] Since the equation about θ1 in equation (10) contains θ2, we can substitute equation (11) into equation (6) and simplify it to:

[0079]

[0080] Therefore, given the working target surface position of the unmanned bulldozer, that is, given the position of the tooth tip at the end of the blade 1, the lifting angle and pitch angle to be controlled can be solved.

[0081] This embodiment obtains the forward and inverse kinematic solutions of the working device and finally obtains the lifting angle and pitching angle to be controlled, thereby realizing the control of the bulldozer working device; according to the positions of the working device joints, the bulldozer base coordinate system and the working device body coordinate system are constructed, and the zyz Euler theorem is used to rotate and translate the coordinate systems, and the relationship between the transformation law and the working device posture is obtained, and then the lifting and pitching angles of the working device are determined, so that the kinematic analysis results are more accurate.

[0082] Embodiment 2:

[0083] This embodiment provides a control system for a bulldozer working device, such as Figure 1 As shown, the bulldozer working device includes a blade 1, a push frame 2, a lifting cylinder and a pitch cylinder. The blade 1 is a structure that is in direct contact with the soil when the bulldozer is working. The push frame 2 is installed on the back side of the blade 1. Several pitch cylinders are connected between the blade 1 and the push frame 2, and the push frame 2 is also connected to the lifting cylinder.

[0084] In this embodiment, the pushing frame 2 is a U-shaped structure, and the two ends of the cross beam of the pushing frame 2 are hinged to the blade 1, so that the blade 1 can perform pitching movement around the hinge; one end of the lifting cylinder is hinged to the pushing frame 2, and the other end is fixed to the body. The two ends of the pushing frame 2 are used to connect the crawler wheels on both sides of the bulldozer body. The pushing frame 2 can drive the blade 1 to perform lifting movement around the hinge.

[0085] The pushing frame 2 is connected to two lifting cylinders and two pitch cylinders. For the convenience of description, this embodiment uses "first" and "second" to distinguish the lifting cylinders and the pitch cylinders, that is, the first pitch cylinder 4-1 and the second pitch cylinder 4-2 are connected between the blade 1 and the pushing frame 2, and the first lifting cylinder 3-1 and the second lifting cylinder 3-2 are connected to the pushing frame 2.

[0086] A first inclination sensor 5 is installed on the back side of the blade 1 to measure the pitch angle of the blade 1; a second inclination sensor 6 is installed on one side of the push frame 2 to measure the lifting angle of the blade 1. The lifting movement of the blade 1 is controlled by the first lifting cylinder 3-1 and the second lifting cylinder 3-2. When the lifting cylinder contracts, the blade 1 is lifted, and when the cylinder extends, the blade 1 is lowered, and the changes of the first lifting cylinder 3-1 and the second lifting cylinder 3-2 are exactly the same. The pitch movement of the blade 1 is controlled by the first pitch cylinder 4-1 and the second pitch cylinder 4-2. When the pitch cylinder contracts, the blade 1 tilts upward, and when the pitch cylinder extends, the blade 1 tilts downward, and the changes of the first pitch cylinder 4-1 and the second pitch cylinder 4-2 are also exactly the same.

[0087] This embodiment installs inclination sensors on the back of the blade 1 and the push frame 2 to detect the lifting and pitching angles of the working device, providing a precise rotation angle for kinematic analysis, making the kinematic analysis results more accurate and the control accuracy of the working device higher.

[0088] The above description is only the preferred embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for controlling a bulldozer working device, characterized in that: include: Based on the DH coordinate system, the base coordinate system {O}, the lifting coordinate system {A}, the pitch coordinate system {B} and the blade end tooth tip coordinate system {C} of the bulldozer working device are established; The ZYZ Euler theorem is used to perform rotation transformation between the base coordinate system {O} and the lifting coordinate system {A}, the pitch coordinate system {B}, and the blade end tooth tip coordinate system {C} to obtain the forward kinematic solution of the bulldozer working device; By transforming the formula of the forward kinematics solution, the inverse kinematics solution of the bulldozer working device is obtained; Based on the inverse kinematics solution and combined with the working target surface position of the bulldozer, the lifting angle and pitch angle of the bulldozer working device to be controlled are obtained.

2. The control method of a bulldozer working device according to claim 1, characterized in that: The axial direction of the bulldozer joint rotation axis is defined as the Z axis, the common perpendicular line of the Z axes of two adjacent joints is defined as the X axis, and the Y axis is determined using the right-hand rule.

3. The control method of a bulldozer working device according to claim 2, characterized in that: The hinge O between the push frame and the center of the body track is taken as the origin, and the forward direction of the bulldozer is taken as X. O Axis, with the vertical direction upward as Z O Axis, parallel to the working surface is Y O Axis, establish the base coordinate system {O} of the bulldozer.

4. The control method of a bulldozer working device according to claim 3, characterized in that: The axis direction of the shaft is Z A Axis, with the push frame direction as X A axis, use the right-hand rule to determine the Y A The direction of the axis is perpendicular to the push frame and upward, and the working device lifting coordinate system {A} is established; The origins of the coordinate system {A} and the base coordinate system {O} are defined on the same rotation center O.

5. A method for controlling a bulldozer working device according to claim 3 or 4, characterized in that: Take the hinge B between the blade and the push frame as the origin and the axis direction of the shaft as Z. B Axis, with the direction of the blade as X B axis, and then use the right-hand rule to know that Y B The axis is perpendicular to the blade and points upward, establishing the working device pitch coordinate system {B}.

6. The control method of a bulldozer working device according to claim 5, characterized in that: The tooth tip coordinate system {C} is established at the middle point C of the tooth tip at the end of the blade, with C as the origin, and the directions of its three axes are consistent with the pitch coordinate system {B}.

7. The control method of a bulldozer working device according to claim 1, characterized in that: The forward kinematic solution is expressed as: Among them, p x , p y , p z They represent the position of the tip of the blade end in the base coordinate system {O}, cθ1 = cosθ1, sθ1 = sinθ1, θ1 is the position of the blade around Z A The angle of the axis lifting; cθ2 = cosθ2, sθ2 = sinθ2, θ2 is the angle of the bulldozer blade around Z B The pitch angle of the axis, x ab ,y ab , z ab It represents the distance from the origin of the pitch coordinate system {B} to the origin of the lift coordinate system {A}; x ab Equal to the length of the bulldozer blade push rod l1, y ab =0, z ab =0, l BW is the thickness of the scraper blade, l bh It represents the height difference from the origin of the tooth tip coordinate system {C} to the pitch coordinate system {B}.

8. The control method of a bulldozer working device according to claim 7, characterized in that: Based on the inverse kinematics solution, the lifting angle and pitch angle of the bulldozer working device to be controlled are solved given the working target surface position of the bulldozer:

9. A method for controlling a bulldozer working device according to claim 7 or 8, characterized in that: Bulldozer blade around Z A The angle θ1 of the shaft lift is measured by the inclination sensor.

10. A control system for a bulldozer working device, characterized in that: Based on the control method as described in any one of claims 1-9, it includes a first inclination sensor installed on the back side of the blade of the bulldozer working device and a second inclination sensor installed on one side of the push frame, the first inclination sensor is used to detect the lifting angle, and the second inclination sensor is used to detect the pitch angle.

Images