Excavator Control Method, Device, Excavator and Storage Medium

Abstract

The present disclosure provides an excavator control method, device, excavator, and storage medium, relating to the field of motion planning. The method comprises: determining the initial position, first transition point position, second transition point position, and terminal position of the excavator from excavation to unloading completion based on the position and height information of the transport vehicle and the position information of the excavator; calculating the first angle information of each joint of the excavator at each moment from the initial position to the first transition point position using a linear motion planning algorithm; calculating the second angle information of each joint of the excavator at each moment from the first transition point position to the second transition point position using a curvilinear motion planning algorithm; calculating the third angle information of each joint of the excavator at each moment from the second transition point position to the terminal position using a linear motion planning algorithm; and controlling the motion of each joint of the excavator based on the first angle information, the second angle information, and the third angle information to perform the excavation operation.

Description

Technical Field

[0001] The present disclosure relates to the field of motion planning, and in particular to an excavator control method, an excavator control device, an excavator, and a storage medium. Background Art

[0002] Excavators, machines for digging and moving materials, are widely used in mining and construction. Due to their 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, driving a growing demand for excavator automation.

[0003] Existing excavator systems are mainly operated by humans to complete high-level tasks such as straight-line trench excavation, earthwork movement and transportation, and leveling the operating surface, resulting in low efficiency. Summary of the Invention

[0004] A technical problem to be solved by the present disclosure is to provide an excavator control method, device, excavator and storage medium, which can improve the working efficiency of the excavator.

[0005] According to one aspect of the present disclosure, a method for controlling an excavator is proposed, comprising: determining the initial posture, first transition point posture, second transition point posture and terminal posture of the excavator from digging to unloading based on the position and height information of the transport vehicle and the position information of the excavator; calculating the first angle information of each joint of the excavator at each moment from the initial posture to the first transition point posture by using a linear motion planning algorithm; calculating the second angle information of each joint of the excavator at each moment from the first transition point posture to the second transition point posture by using a curved motion planning algorithm; calculating the third angle information of each joint of the excavator at each moment from the second transition point posture to the terminal posture by using a linear motion planning algorithm; and controlling the movement of each joint of the excavator based on the first angle information, the second angle information and the third angle information to perform excavation operations.

[0006] In some embodiments, the initial posture is the posture of the excavator when the bucket of the excavator touches the ground and digging has not started; the first transition point posture is the posture of the excavator when the bucket has completed digging; the second transition point posture is the posture of the excavator when the bucket moves to the top of the transport vehicle, descends to a predetermined height, is static and has not unloaded; and the terminal posture is the posture of the excavator when the bucket has completed unloading.

[0007] In some embodiments, a linear motion planning algorithm is used to calculate the excavator's posture from an initial posture to a first transition point posture, and the first angle information of each joint at each moment includes: interpolating the posture rotation matrix corresponding to the initial posture and the initial posture transformation matrix corresponding to the first transition point posture to obtain a first posture rotation matrix interpolation sequence; and obtaining an angle sequence with a timestamp for each joint based on the first posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, wherein each joint satisfies the angle constraint, velocity constraint, and acceleration constraint of the corresponding joint during the movement process.

[0008] In some embodiments, the posture rotation matrix corresponding to the initial posture and the initial posture transformation matrix corresponding to the first transition point posture are interpolated to obtain a first posture rotation matrix interpolation sequence, including: interpolating and normalizing the speed according to the speed information of the bucket during the excavation process to obtain a first speed sequence; interpolating the displacement of the bucket of the excavator from the initial posture to the first transition point posture according to the first speed sequence to obtain a first displacement sequence, and interpolating the posture of the bucket to obtain a first posture sequence; and obtaining a first posture rotation matrix interpolation sequence based on a combination of the first displacement sequence and the first posture sequence.

[0009] In some embodiments, according to the curve motion planning algorithm, the first transition point posture to the second transition point posture is divided into a first sub-posture, a second sub-posture, a third sub-posture and a fourth sub-posture, wherein the first sub-posture is the posture of the bucket when it moves from the planned movement after excavation to the state where it can keep the material from leaking; the second sub-posture is the posture of the bucket when it moves to the first target position to the maximum extent in the direction perpendicular to the ground while keeping the material from leaking; the third sub-posture is the posture of the bucket when the joint between the rotating body and the vehicle body of the excavator moves to the target joint angle; and the fourth sub-posture is the posture of the bucket when it moves to the second target position to the maximum extent in the direction perpendicular to the ground while keeping the material from leaking, and the bucket is in a moving state.

[0010] In some embodiments, the excavator includes a first joint, a second joint, a third joint and a fourth joint, wherein the first joint is the joint between the rotating body and the vehicle body of the excavator, the second joint is the joint between the rotating body and the boom of the excavator, the third joint is the joint between the boom and the forearm of the excavator, and the fourth joint is the joint between the forearm of the excavator and the bucket.

[0011] In some embodiments, a curve motion planning algorithm is used to calculate the second angle information of each joint of the excavator at each moment from the first transition point posture to the second transition point posture, including: the angle value range of the fourth joint when determining that the bucket can keep the material from leaking; the first posture transformation matrix corresponding to the first sub-posture is obtained according to the initial posture transformation matrix corresponding to the first transition point posture and the angle value range of the fourth joint; the second posture transformation matrix corresponding to the second sub-posture is obtained according to the first posture transformation matrix, the fifth posture transformation matrix corresponding to the second transition point posture, and the angle value range of the fourth joint; the third posture transformation matrix corresponding to the third sub-posture is obtained according to the second posture transformation matrix and the fifth posture transformation matrix; the third posture transformation matrix corresponding to the third sub-posture is obtained according to the third posture transformation matrix, the fifth posture transformation matrix and the angle value range of the fourth joint. The angle value range of the four joints is used to obtain the fourth posture transformation matrix corresponding to the fourth sub-posture; the linear motion planning algorithm is used to calculate the angle sequence with timestamps of each joint between the initial posture transformation matrix and the first posture transformation matrix, the angle sequence with timestamps of each joint between the first posture transformation matrix and the second posture transformation matrix, the angle sequence with timestamps of each joint between the second posture transformation matrix and the third posture transformation matrix, the angle sequence with timestamps of each joint between the third posture transformation matrix and the fourth posture transformation matrix, and the angle sequence with timestamps of each joint between the fourth posture transformation matrix and the fifth posture transformation matrix, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0012] In some embodiments, obtaining the first posture transformation matrix corresponding to the first sub-posture according to the initial posture transformation matrix corresponding to the first transition point posture and the angle value range of the fourth joint includes: inversely calculating the initial angle of each joint of the excavator according to the initial posture transformation matrix corresponding to the first transition point posture; if the angle of the fourth joint is within the angle value range at the predetermined moment, maintaining the angle of the first joint; if the angle of the fourth joint is not within the angle value range at the predetermined moment, adjusting the angle of the fourth joint to the angle value range, and solving the angles of the first joint, the second joint and the third joint; and determining the first posture transformation matrix according to the angles of the first joint, the second joint, the third joint and the fourth joint.

[0013] In some embodiments, obtaining the second posture transformation matrix based on the first posture transformation matrix, the fifth posture transformation matrix, and the angle value range of the fourth joint includes: obtaining the first intermediate posture based on the first posture transformation matrix and the fifth posture transformation matrix; inversely calculating the angles of the first joint, the second joint, and the third joint based on the first intermediate posture; positively solving the angle of the fourth joint based on the angles of the first joint, the second joint, and the third joint; if the positively solved angle of the fourth joint meets the angle value range, determining the second posture transformation matrix based on the angles of the first joint, the second joint, the third joint, and the fourth joint.

[0014] In some embodiments, obtaining the third posture transformation matrix based on the second posture transformation matrix and the fifth posture transformation matrix includes: inversely calculating the angles of the second joint, the third joint, and the fourth joint based on the second posture transformation matrix; inversely calculating the angle of the first joint based on the fifth posture transformation matrix; and determining the third posture transformation matrix based on the angles of the first joint, the second joint, the third joint, and the fourth joint.

[0015] In some embodiments, obtaining the fourth posture transformation matrix based on the third posture transformation matrix, the fifth posture transformation matrix and the angle value range of the fourth joint includes: obtaining the second intermediate posture based on the third posture transformation matrix and the fifth posture transformation matrix; inversely calculating the angles of the first joint, the second joint and the third joint based on the second intermediate posture; positively solving the angle of the fourth joint based on the angles of the first joint, the second joint and the third joint; if the positively solved angle of the fourth joint meets the angle value range, then determining the fourth posture transformation matrix based on the angles of the first joint, the second joint, the third joint and the fourth joint.

[0016] In some embodiments, the angle sequence with timestamps of each joint between the initial pose transformation matrix and the first pose transformation matrix, the angle sequence with timestamps of each joint between the first pose transformation matrix and the second pose transformation matrix, the angle sequence with timestamps of each joint between the second pose transformation matrix and the third pose transformation matrix, the angle sequence with timestamps of each joint between the third pose transformation matrix and the fourth pose transformation matrix, and the angle sequence with timestamps of each joint between the fourth pose transformation matrix and the fifth pose transformation matrix are complementarily processed.

[0017] In some embodiments, a linear motion planning algorithm is used to calculate the excavator's posture from the second transition point to the end posture, and the third angle information of each joint at each moment includes: interpolating the fifth posture transformation matrix corresponding to the second transition point posture and the end posture transformation matrix corresponding to the end posture to obtain a second posture rotation matrix interpolation sequence; and obtaining a timestamp angle sequence of each joint based on the second posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0018] In some embodiments, the fifth posture transformation matrix corresponding to the second transition point posture and the end posture transformation matrix corresponding to the end posture are interpolated to obtain a second posture rotation matrix interpolation sequence, including: interpolating and normalizing the speed according to the speed information of the bucket during the unloading process to obtain a second speed sequence; interpolating the displacement of the bucket of the excavator from the second transition point posture to the end posture according to the second speed sequence to obtain a second displacement sequence, and interpolating the posture of the bucket to obtain a second posture sequence; and obtaining a second posture rotation matrix interpolation sequence based on the combination of the second displacement sequence and the second posture sequence.

[0019] According to another aspect of the present disclosure, an excavator control device is also proposed, including: a posture determination module, configured to determine the initial posture, first transition point posture, second transition point posture and end posture of the excavator from digging to unloading completion based on the position and height information of the transport vehicle and the position information of the excavator; an angle calculation module, configured to use a linear motion planning algorithm to calculate the first angle information of each joint of the excavator at each moment from the initial posture to the first transition point posture; use a curved motion planning algorithm to calculate the second angle information of each joint of the excavator at each moment from the first transition point posture to the second transition point posture; use a linear motion planning algorithm to calculate the third angle information of each joint of the excavator at each moment from the second transition point posture to the end posture; and a control module, configured to control the movement of each joint of the excavator based on the first angle information, the second angle information and the third angle information to perform excavation operations.

[0020] According to another aspect of the present disclosure, an excavator control device is provided, including: a memory; and a processor coupled to the memory, wherein the processor is configured to execute the excavator control method described above based on instructions stored in the memory.

[0021] According to another aspect of the present disclosure, an excavator is further provided, comprising: the excavator control device described above.

[0022] According to another aspect of the present disclosure, a non-transitory computer-readable storage medium is further provided, on which computer program instructions are stored. When the instructions are executed by a processor, the above-mentioned excavator control method is implemented.

[0023] In the disclosed embodiment, a segmented motion planning mechanism is adopted based on the motion characteristics from the initial position of the bucket to the end position of the bucket, and the straight line and curve motion planning algorithms are integrated. The corresponding motion planning algorithms are used in different segmented motion plans, so that the shovel teeth can quickly converge from the initial posture to the end posture. During the movement, the opening of the bucket is facing upward and remains parallel to the ground, so that the material in the bucket is always kept from leaking, thereby improving the working efficiency of the excavator and reducing the operator's work intensity.

[0024] Other features and advantages of the present disclosure will become apparent from the following detailed description of exemplary embodiments of the present disclosure with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments of the present disclosure and, together with the description, serve to explain the principles of the present disclosure.

[0026] The present disclosure can be more clearly understood from the following detailed description with reference to the accompanying drawings, in which:

[0027] Figure 1 Schematic diagram of the flow of some embodiments of the excavator control method disclosed herein;

[0028] Figure 2 Schematic diagrams of the excavator in different states of the present disclosure;

[0029] Figure 3 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed herein;

[0030] Figure 4 is a schematic diagram of some embodiments of the interpolation algorithm disclosed herein;

[0031] Figure 5 Schematic diagrams of other embodiments of the interpolation algorithm disclosed herein;

[0032] Figure 6 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed herein;

[0033] Figure 7 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed herein;

[0034] Figure 8 A schematic diagram of vectors coinciding with the starting point of the present disclosure;

[0035] Figure 9is a schematic diagram of the stable region disclosed herein;

[0036] Figure 10 It is a schematic diagram of a working cross section of the bucket rotation center disclosed in the present invention;

[0037] Figure 11 This is a schematic diagram of the position area of the present invention that can prevent material from leaking;

[0038] Figure 12 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed herein;

[0039] Figure 13 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed herein;

[0040] Figure 14 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed herein;

[0041] Figure 15 Schematic diagrams of the structures of some embodiments of the excavator control device disclosed herein;

[0042] Figure 16 Schematic diagrams of the structures of other embodiments of the excavator control device disclosed herein;

[0043] Figure 17 Schematic diagrams of the structures of some other embodiments of the excavator control device disclosed in the present invention; and

[0044] Figure 18 It is a schematic diagram of material transportation and unloading of the excavator disclosed in the present invention. DETAILED DESCRIPTION

[0045] Various exemplary embodiments of the present disclosure will now be described in detail with reference to the accompanying drawings. It should be noted that unless otherwise specifically stated, the relative arrangement of components and steps, numerical expressions and numerical values set forth in these embodiments do not limit the scope of the present disclosure.

[0046] At the same time, it should be understood that for the convenience of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship.

[0047] The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the present disclosure, its application, or uses.

[0048] Technologies, methods, and equipment known to ordinary technicians in the relevant art may not be discussed in detail, but where appropriate, the technologies, methods, and equipment should be considered part of the specification.

[0049] In all examples shown and discussed herein, any specific values should be interpreted as merely exemplary and not limiting. Therefore, other examples of the exemplary embodiments may have different values.

[0050] It should be noted that like reference numerals and letters refer to like items in the following figures, and therefore, once an item is defined in one figure, it need not be further discussed in subsequent figures.

[0051] In order to make the objectives, technical solutions and advantages of the present disclosure more clearly understood, the present disclosure is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

[0052] Figure 1 Schematic diagram of some embodiments of the excavator control method disclosed herein.

[0053] In step 110, the initial posture, first transition point posture, second transition point posture and terminal posture of the excavator from digging to unloading are determined based on the position and height information of the transport vehicle and the position information of the excavator.

[0054] In some embodiments, the initial position is the position of the excavator when the bucket is in contact with the ground and excavation has not yet begun. For example, the excavator has reached a designated position between the pile of material to be excavated and the transport vehicle to be loaded, with the bucket teeth in contact with the ground and the bucket opening facing downward, but not yet in the excavation state. The transport vehicle is, for example, an unmanned mining truck, referred to as a mining truck.

[0055] The first transition point posture is the posture of the excavator when the bucket has finished digging. For example, the posture when the bucket has finished digging and the bucket is open inward.

[0056] The second transition point position is when the bucket moves above the transport vehicle, descends to a predetermined height, and is at rest, not yet unloading. For example, after the excavator has finished excavating material and is transporting it to a transport vehicle or another location, the bucket is above the transport vehicle, opening upward, with the contents sealed, and about to unload.

[0057] The end position is the position of the excavator when the bucket has finished unloading. For example, the bucket is facing downward and is above the transport vehicle after unloading.

[0058] In step 120, a linear motion planning algorithm is used to calculate the first angle information of each joint at each moment from the initial posture of the excavator to the first transition point posture.

[0059] In some embodiments, the posture rotation matrix corresponding to the initial posture and the initial posture transformation matrix corresponding to the first transition point posture are interpolated to obtain a first posture rotation matrix interpolation sequence; based on the first posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, an angle sequence with a timestamp for each joint is obtained, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0060] In step 130, a curvilinear motion planning algorithm is used to calculate the second angle information of each joint at each moment when the excavator moves from the first transition point posture to the second transition point posture.

[0061] In some embodiments, the idea of segmented planning is continued to be used in the process of obtaining a curvilinear motion planning algorithm for the content handling and release tasks that satisfy the physical constraints of each joint.

[0062] According to the curve motion planning algorithm, the posture between the first transition point and the second transition point is divided into the first sub-posture, the second sub-posture, the third sub-posture and the fourth sub-posture, wherein the first sub-posture is the posture of the bucket when it moves from the planned movement after excavation to the state where the material cannot be leaked; the second sub-posture is the posture of the bucket when it moves to the first target position to the maximum extent in the direction perpendicular to the ground while keeping the material from leaking; the third sub-posture is the posture of the bucket when the joint between the rotating body and the vehicle body of the excavator moves to the target joint angle; and the fourth sub-posture is the posture of the bucket when it moves to the second target position to the maximum extent in the direction perpendicular to the ground while keeping the material from leaking, and the bucket is in a moving state.

[0063] In step 140 , a linear motion planning algorithm is used to calculate the third angle information of each joint at each moment from the excavator's posture at the second transition point to the end posture.

[0064] In some embodiments, the fifth posture transformation matrix corresponding to the second transition point posture and the end posture transformation matrix corresponding to the end posture are interpolated to obtain a second posture rotation matrix interpolation sequence; and based on the second posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, an angle sequence with a timestamp for each joint is obtained, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0065] In step 150 , based on the first angle information, the second angle information, and the third angle information, the movement of each joint of the excavator is controlled to perform an excavation operation.

[0066] In the above embodiment, the continuous actions of the excavator in transporting materials mainly include lifting the excavator operating arm, rotating the excavator operating arm and lowering the excavator operating arm. Since this series of continuous actions includes both linear motion and curved motion, this embodiment adopts segmented planning and integrates the linear and curved motion planning algorithms, which can improve the motion efficiency of the excavator operating arm and reduce the operator's work intensity.

[0067] In some embodiments of the present disclosure, Figure 2 As shown in the figure, the excavator includes a first joint, a second joint, a third joint, and a fourth joint. The first joint is the joint between the excavator's rotating body and the vehicle body, the second joint is the joint between the rotating body and the excavator's boom, the third joint is the joint between the boom and the excavator's forearm, and the fourth joint is the joint between the excavator's forearm and the bucket. The angle of the first joint is θ1, the angle of the second joint is θ2, the angle of the third joint is θ3, and the angle of the fourth joint is θ4. From the initial position to the first transition point, the angle θ1 of the first joint remains unchanged, indicating that the excavator has not rotated.

[0068] Before executing the excavator control method disclosed in the present invention, a kinematic model of the excavator operating arm should be established, and physical constraints of the excavator should be set.

[0069] In some embodiments, a kinematic model of the excavator operating arm is established based on the Modified_D-H (MDH) method, and a list of kinematic parameters is shown in Table 1:

[0070] Table 1 Excavator Modified_D-H parameter table

[0071]

[0072] Where j is the coordinate system number of the excavator operating arm, α is the current connecting rod length, a is the connecting rod torsion angle, d is the connecting rod offset, and d is the joint angle. Joint 5 represents the coordinate system fixed to the shovel tooth, which moves with the shovel tooth. j ,j={1,2,3,4} and d j ,j={1,2} is a constant parameter that varies with the excavator model. j , j = {1, 2, 3, 4} is a variable that can be changed during the excavator's task. Joint 4 represents the coordinate system fixed to the bucket's rotation center. Typical parameter values are shown in Table 2:

[0073] Table 2 Typical parameters

[0074]

[0075] In some embodiments, the physical constraints of the excavator refer to the angle constraints, velocity constraints, and acceleration constraints that each joint should meet during movement. The physical constraint parameters are shown in Table 3:

[0076] Table 3 Physical constraint parameters

[0077]

[0078] The following describes a method for planning the linear motion of the excavator arm during excavation by calling a linear motion planning algorithm based on the initial bucket position and the first transition point position.

[0079] Figure 3 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed in the present invention.

[0080] In step 310 , the speed is interpolated and normalized based on the speed information of the bucket during the digging process to obtain a first speed sequence.

[0081] like Figure 4 The figure shows a speed-time curve. The area of the curve is 1. From 0 to 1, the speed is first uniformly accelerated, then uniformly accelerated, and finally uniformly decelerated. The interpolation algorithm finally obtains the distribution in the interval [0,1]. Figure 4 The interpolation sequence of the speed curve characteristics shown is equivalent to performing discrete sampling at equal intervals in a continuous curve. The value of the interval depends on the number of samples Num. The interval value For example, Num can be a constant of 50.

[0082] In step 320 , the displacement of the bucket of the excavator from the initial posture to the first transition point posture is interpolated in sequence according to the first speed sequence to obtain a first displacement sequence, and the posture of the bucket is interpolated to obtain a first posture sequence.

[0083] like Figure 5 As shown, the displacement is linearly interpolated in the order of the interpolation sequence. For the vectors on the line at the end of , there is the following relationship:

[0084]

[0085] The posture is interpolated sequentially according to the interpolation sequence. The initial and final postures need to be converted into unit quaternions, and then interpolated according to the obtained interpolation sequence. This process can use mature methods and will not be further explained here.

[0086] During the interpolation process of the posture, due to the periodicity of the angle rotation, there are two interpolation directions. Considering that in practical applications, it is necessary to converge from the initial posture to the first transition point posture quickly and efficiently, the interpolation direction that can converge from the initial posture to the first transition point posture in the shortest time is selected.

[0087] In step 330 , a first pose rotation matrix interpolation sequence is obtained based on a combination of the first displacement sequence and the first pose sequence.

[0088] Displacement and attitude are combined to form a homogeneous transformation matrix sequence, with the intermediate components interpolated by an interpolation algorithm. The effect of the interpolation algorithm is that, from a displacement perspective, the teeth first accelerate, then maintain a constant speed, and finally decelerate; from an attitude perspective, the bucket first accelerates relative to the ground, then maintains a constant speed, and finally decelerates until it converges to the first transition point.

[0089] In step 340, based on the first pose rotation matrix interpolation sequence and the time interval when controlling the joint angle, a timestamp angle sequence of each joint is obtained, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0090] In some embodiments, as Figure 6 As shown, step 340 includes steps 610-6120.

[0091] At step 610, the angle sequence is initialized.

[0092] At step 620, the first pose rotation matrix interpolation sequence is traversed.

[0093] In step 630 , the angles of other joints are obtained by inversely solving the bucket posture.

[0094] In step 640 , it is determined whether any of the four newly added angles meets the corresponding angle limit. If so, step 650 is executed; otherwise, step 651 is executed.

[0095] In step 650 , it is determined whether the traversal is completed. If so, step 660 is executed; otherwise, step 620 is continued.

[0096] In step 651 , it is confirmed that the planning has failed, and the initial pose and the first transition point pose are adjusted.

[0097] In step 660 , each joint angle sequence is obtained.

[0098] In step 670, the angular velocity and angular acceleration of each point in each joint sequence are calculated according to the time interval of the controller.

[0099] In step 680 , it is determined whether the angular velocity and angular acceleration of any joint sequence point are out of range. If so, step 690 is executed; otherwise, step 6100 is executed.

[0100] In step 690 , the interval time of the sequence point is increased using a binary method, and then step 670 is executed.

[0101] In step 6100 , determine whether the traversal is completed. If it is completed, execute step 6110 ; otherwise, continue to execute step 670 .

[0102] In step 6110, n angle values are linearly interpolated between two adjacent angles in the sequence, where n = the interval time between points / the time interval of the controller.

[0103] In step 6120, the angle value sequence corresponding to each time point is obtained. The arrival time interval of adjacent angles in the angle sequence is t int erval , t int erval Generally, a very small value is taken, such as 0.01 seconds, for each joint to meet the corresponding physical constraints.

[0104] The specific algorithm implementation is as follows:

[0105]

[0106]

[0107] In the above embodiment, in the basic interpolation algorithm, a homogeneous rotation matrix interpolation sequence T from the initial pose to the first transition point pose is obtained. seq , using the time distribution algorithm, the time to reach each matrix is distributed to meet the physical constraints of the joint. The input parameter is the homogeneous rotation matrix interpolation sequence T seq The time intervals between angle values sent to the controller in actual applications ultimately yield a time-stamped angle sequence for each joint. Ultimately, the shovel tooth displacement remains on the straight line connecting the initial position and the first transition point, and the shovel tooth posture gradually converges from the initial posture to the first transition point posture.

[0108] The following will introduce a method for calling a curved motion planning algorithm during the process of rotating the excavator operating arm to plan the curved motion of the rotating excavator operating arm according to the first transition point posture and the second transition point posture.

[0109] Figure 7 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed in the present invention.

[0110] In step 710 , the angle value range of the fourth joint when the bucket is capable of preventing material from leaking is determined.

[0111] First, it is necessary to define what is meant by material leak-proof. In some embodiments, Figure 2 As shown, define the vector is a vector starting from joint 2 and ending at joint 3; vector is a vector starting at joint 4 and ending at the shovel tooth.

[0112] like Figure 8 As shown, express The unit vector of the projection of the vector onto the horizontal ground. Indicates four possible The dotted line shows the posture of the bucket. i ,i={1,2,3,4} represents the corresponding Angle in the -180,180 degree angle system. Positive is counterclockwise. α i ,i={1,2,3,4} represents the angle between the bucket and the horizontal ground, that is, an angle system is constructed based on the angle between the bucket and the horizontal plane.

[0113] According to this angle system, the flatness of the ground on site and the planned excavator movement trajectory, there is a stable angle α at which materials will not leak. s ∈S. Define S as an interval that is considered stable. The lower and upper bounds of the interval do not necessarily conform to the upper bound being greater than the lower bound. Instead, it starts from the lower bound and rotates in the positive direction until it reaches the upper bound (the entire process is in the -180, 180 angle system). The range traversed during this period is the area represented by S (if the upper and lower bounds are equal, it only represents one value, not a range). For example, S can be defined as [150, -150], which means that Figure 9 The shaded area shows the bucket. Any angle within this shaded area is considered to ensure the bucket remains leak-proof. However, if the stability range is critical, S can be defined as [179.9, -179.9] or even [180, 180] to maximize the bucket's parallelism to the ground and prevent leakage.

[0114] After defining S, the bucket's range of motion can be limited according to actual conditions to ensure that the entire excavator operating arm always keeps the bucket parallel to the horizontal plane during operation. No matter how the boom or dipper arm moves, the bucket can be kept parallel to the horizontal plane, thereby ensuring that material in the bucket does not leak.

[0115] In practical applications, the range of S is defined according to the on-site working conditions. If S is defined as [150, -150], θ4∈[-100, 30], then when the bucket rotation center keeps θ1 unchanged, all possible θ2, θ3 are traversed to obtain Figure 10The working space section of the bucket rotation center is shown in FIG. Figure 11 The range shown is to keep the material from leaking.

[0116] As can be seen, with the current parameter settings, not every bucket rotation center position can find a value of θ4 that meets the physical angle constraints and prevents material leakage. This means that if you want to move contents outside the stable range, you need to combine the motion planning of the excavator chassis in the world coordinate system to ensure that the final target position is within the stable region to complete the required task.

[0117] In the above embodiment, an angle system is constructed between the angle between the bucket and the horizontal ground, using the vector starting at joint 2 and ending at joint 3 as its projection onto the horizontal plane, and the unit vector starting at joint 4 and ending at the shovel tooth. The bucket's stable range is determined based on this angle system, the on-site ground flatness, and the planned excavator motion trajectory. The excavator's operating arm's motion is planned based on this stable range, ensuring that the bucket's opening remains parallel to the ground throughout the entire material transportation process, thereby ensuring that the material in the bucket remains intact.

[0118] In step 720, the first pose transformation matrix T1 corresponding to the first sub-pose is obtained according to the initial pose transformation matrix T0 corresponding to the first transition point pose and the angle value range of the fourth joint.

[0119] In some embodiments, the initial angle of each joint of the excavator is calculated by inverse solution based on the initial posture transformation matrix corresponding to the posture of the first transition point; if the angle of the fourth joint at the predetermined moment is within the angle value range, the angle of the first joint is maintained; if the angle of the fourth joint at the predetermined moment is not within the angle value range, the angle of the fourth joint is adjusted to the angle value range, and the angles of the first joint, the second joint and the third joint are solved; and based on the angles of the first joint, the second joint, the third joint and the fourth joint, the first posture transformation matrix T1 is determined.

[0120] Knowing the first three angles of the excavator (θ1, θ2, θ3) and As a benchmark In order to facilitate the development of the planning algorithm and the intuitiveness of human operation in the actual excavator system, it is determined whether the angle range of the bucket rotation center can be obtained. As a benchmark The value of θ4 corresponding to the angle that satisfies the physical constraint of the angle, and the spatial range of the bucket rotation center that can ensure that the bucket material does not leak, etc., can be known that the first three angles of the excavator and As a benchmark The angle range of θ4 is obtained from the feasible range of θ4.

[0121] Knowing the four angles of the excavator (θ1, θ2, θ3, θ4), we can get As a benchmark According to the typical inverse kinematics solution algorithm, the homogeneous transformation matrix of the shovel tooth is known, and the four angles of the excavator (θ1, θ2, θ3, θ4) can be inversely solved.

[0122] Based on the above planning algorithm, given the initial pose transformation matrix T0, the initial four excavator angles are inversely solved. Then, given the bucket material stability angle range, the possible range of θ4 that can prevent the contents from leaking is determined. Sometimes, due to an overly restrictive stability range (e.g., [180, 180]), or because the physically feasible range of θ4 angles is too small, the possible range of θ4 that can prevent the contents from leaking is empty. In this case, an error is thrown and the algorithm is terminated, deeming this situation unsuitable for the algorithm. The algorithm then determines whether the current θ4 value is within this range. If so, the value remains unchanged, indicating that no adjustment is required to prevent the contents from leaking. Otherwise, the planned θ4 value is adjusted to a value within this range, which can be midway within the range. The adjusted values of θ1, θ2, θ3, and θ4 are obtained, yielding the homogeneous transformation matrix T1 of the bucket's rotation center, output by the T0→T1 planning algorithm.

[0123] In step 730, the second posture transformation matrix T2 corresponding to the second sub-posture is obtained according to the first posture transformation matrix T1, the fifth posture transformation matrix Tf corresponding to the second transition point posture, and the angle value range of the fourth joint.

[0124] In some embodiments, the first intermediate posture T_mid1 is obtained based on the first posture transformation matrix T1 and the fifth posture transformation matrix Tf; the angles of the first joint, the second joint and the third joint are inversely calculated based on the first intermediate posture T_mid1; the angle of the fourth joint is solved based on the angles of the first joint, the second joint and the third joint; if the angle of the fourth joint solved positively satisfies the angle value range, the second posture transformation matrix T2 is determined based on the angles of the first joint, the second joint, the third joint and the fourth joint.

[0125] like Figure 12 As shown, the process includes steps 1210-1280.

[0126] In step 1210, the first posture transformation matrix T1 of the bucket, the fifth posture transformation matrix Tf, and the bucket angle range S for preventing material leakage are input.

[0127] In step 1220 , a first intermediate pose T_mid1 is obtained by bisection.

[0128] In step 1230 , the angles θ1 , θ2 , and θ3 of the other three joints are inversely solved based on the first intermediate posture T_mid1 .

[0129] In step 1240, a positive solution is obtained based on θ1, θ2, and θ3 to calculate the bucket angle θ4.

[0130] In step 1250 , it is determined whether the bucket angle θ4 is within the interval S. If so, step 1260 is executed; otherwise, step 1270 is executed.

[0131] In step 1260, the second posture transformation matrix T2 is calculated based on the obtained values of θ1, θ2, θ3, and θ4.

[0132] In step 1270 , determine whether the error threshold is exceeded. If so, execute step 1280 ; otherwise, continue to execute step 1220 .

[0133] At step 1280, planning fails.

[0134] The specific algorithm implementation process is as follows:

[0135]

[0136]

[0137] In step 740, a third posture transformation matrix T3 corresponding to the third sub-pose is obtained according to the second posture transformation matrix T2 and the fifth posture transformation matrix Tf.

[0138] In some embodiments, the angles of the second joint, the third joint, and the fourth joint are calculated by inverse solution based on the second posture transformation matrix T2; the angle of the first joint is calculated by inverse solution based on the fifth posture transformation matrix Tf; and the third posture transformation matrix T3 is determined based on the angles of the first joint, the second joint, the third joint, and the fourth joint.

[0139] The planning algorithm before this step does not change the value of θ1. The values of θ2, θ3, and θ4 obtained by inverse solution of the second posture transformation matrix T2 remain unchanged. The value of θ1 is inversely solved according to the fifth posture transformation matrix Tf. The initial value of θ1 is the value of θ1 obtained by inverse solution of T2, and the final value is T f The value of θ1 is obtained by inverse solution. Then, based on the values of θ1, θ2, θ3, and θ4, the homogeneous transformation matrix T3 of the bucket rotation center output by the T1→T2 planning algorithm is obtained.

[0140] In step 750, a fourth posture transformation matrix T4 corresponding to the fourth sub-posture is obtained according to the third posture transformation matrix T3, the fifth posture transformation matrix Tf and the angle value range of the fourth joint.

[0141] In some embodiments, the second intermediate posture T_mid2 is obtained based on the third posture transformation matrix T3 and the fifth posture transformation matrix Tf; the angles of the first joint, the second joint and the third joint are inversely calculated based on the second intermediate posture T_mid2; the angle of the fourth joint is solved based on the angles of the first joint, the second joint and the third joint; if the angle of the fourth joint solved positively satisfies the angle value range, the fourth posture transformation matrix T4 is determined based on the angles of the first joint, the second joint, the third joint and the fourth joint.

[0142] like Figure 13 As shown, the process includes steps 1310-1380.

[0143] In step 1310, the third posture transformation matrix T3 of the bucket, the fifth posture transformation matrix Tf, and the bucket angle range S for preventing material leakage are input.

[0144] In step 1320 , a second intermediate pose T_mid2 is obtained by bisection.

[0145] In step 1330 , the angles θ1 , θ2 , and θ3 of the other three joints are inversely solved based on the second intermediate posture T_mid2 .

[0146] In step 1340, a positive solution is obtained based on θ1, θ2, and θ3 to calculate the bucket angle θ4.

[0147] In step 1350 , it is determined whether the bucket angle θ4 is in the load range S. If so, step 1360 is executed; otherwise, step 1370 is executed.

[0148] In step 1360, the fourth posture transformation matrix T4 is calculated based on the obtained values of θ1, θ2, θ3, and θ4.

[0149] In step 1370 , determine whether the error threshold is exceeded. If so, execute step 1380 ; otherwise, continue to execute step 1320 .

[0150] At step 1380, planning fails.

[0151] The specific algorithm implementation process is as follows:

[0152]

[0153]

[0154]

[0155] In step 760, a linear motion planning algorithm is used to calculate the angle sequence with timestamps of each joint between the initial pose transformation matrix T0 and the first pose transformation matrix T1, the angle sequence with timestamps of each joint between the first pose transformation matrix T1 and the second pose transformation matrix T2, the angle sequence with timestamps of each joint between the second pose transformation matrix T2 and the third pose transformation matrix T3, the angle sequence with timestamps of each joint between the third pose transformation matrix T3 and the fourth pose transformation matrix T4, and the angle sequence with timestamps of each joint between the fourth pose transformation matrix T4 and the fifth pose transformation matrix Tf, respectively. Each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0156] The given intermediate homogeneous transformation matrices are not directly obtained in one step. In order to meet the velocity and acceleration constraints of each joint, many intermediate matrices are required to obtain the angle sequence. The initial and final values of the angle sequence can be obtained by using forward kinematics to obtain T0, T f The time interval between any two adjacent angles in the above angle sequence is t interval .

[0157] In the above embodiment, the concept of segmented planning is further adopted in curved motion planning. While ensuring that the bucket does not leak material, the excavator first moves as far as possible from the first transition point in a direction perpendicular to the ground toward the target position. The excavator then controls the slewing joint to rotate to the target angle, and then continues to move as far as possible parallel to the ground toward the target position, finally converging to the second transition point. This planning method can save work time and improve work efficiency.

[0158] In some embodiments, the angle sequence with timestamps of each joint between the initial pose transformation matrix T0 and the first pose transformation matrix T1, the angle sequence with timestamps of each joint between the first pose transformation matrix T1 and the second pose transformation matrix T2, the angle sequence with timestamps of each joint between the second pose transformation matrix T2 and the third pose transformation matrix T3, the angle sequence with timestamps of each joint between the third pose transformation matrix T3 and the fourth pose transformation matrix T4, and the angle sequence with timestamps of each joint between the fourth pose transformation matrix T4 and the fifth pose transformation matrix Tf are complementarily processed.

[0159] For example, the final angle sequence is the sequential splicing of the above angle sequences, but sometimes in order to improve execution efficiency and shorten execution time, some sequences are crossed, but they will definitely go through T i ,i={1,2,3,4}). For example, execute θ corresponding to T2→T3 seq,3 In the process of T1→T2, only θ1 changes, and the θ corresponding to T1→T2 is executed.seq,2 , θ corresponding to T3→T4 seq,4 In the process of θ1, only θ2, θ3, and θ4 change. Therefore, these three sequences can complement each other according to certain rules to achieve the effect of simultaneous execution: θ1 changes according to θ seq,3 While rotating, θ2, θ3, θ4 follow θ seq,2 ,θ seq,4 Proceed in sequence so that the center of rotation of the bucket approaches the final position first in the vertical direction and then in the horizontal direction.

[0160] The following will introduce how to continue calling the linear motion planning algorithm during the material release process and plan the linear motion of the excavator operating arm based on the second transition point posture and the bucket end posture.

[0161] Figure 14 Schematic diagram of the flow chart of other embodiments of the excavator control method disclosed in the present invention.

[0162] In step 1410 , the speed is interpolated and normalized based on the speed information of the bucket during the unloading process to obtain a second speed sequence.

[0163] The process of interpolating and normalizing the velocity is similar to step 310 and will not be further elaborated here.

[0164] In step 1420, the displacement of the bucket of the excavator from the second transition point posture to the end posture is interpolated in sequence according to the second speed sequence to obtain a second displacement sequence, and the posture of the bucket is interpolated to obtain a second posture sequence.

[0165] The process of interpolating the bucket displacement and posture is similar to step 320 and will not be further elaborated here.

[0166] In step 1430 , a second pose rotation matrix interpolation sequence is obtained based on a combination of the second displacement sequence and the second pose sequence.

[0167] In step 1440, based on the interpolation sequence of the second posture rotation matrix and the time interval when controlling the joint angle, a timestamp angle sequence of each joint is obtained, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0168] Step 1440 is similar to step 340 and will not be further elaborated here.

[0169] In the above-mentioned embodiment of the present disclosure, a content transportation motion planning algorithm that satisfies the physical constraints of each joint is adopted, and the idea of segmented planning is adopted to combine straight line planning and spatial curve planning. A series of transition points are selected in the coordinated working space of the excavator and the mining truck. The above two planning methods are used for combined planning, which can complete the entire loading and unloading process at the fastest speed and improve work efficiency.

[0170] Figure 15 15 is a schematic structural diagram of some embodiments of the excavator control device disclosed herein, wherein the excavator control device includes a posture determination module 1510 , an angle calculation module 1520 and a control module 1530 .

[0171] The posture determination module 1510 is configured to determine the initial posture, first transition point posture, second transition point posture and terminal posture of the excavator from digging to unloading based on the position and height information of the transport vehicle and the position information of the excavator.

[0172] In some embodiments, the initial posture is the posture of the excavator when the bucket of the excavator touches the ground and digging has not started; the first transition point posture is the posture of the excavator when the bucket has completed digging; the second transition point posture is the posture of the excavator when the bucket moves to the top of the transport vehicle, descends to a predetermined height, is static and has not unloaded; and the terminal posture is the posture of the excavator when the bucket has completed unloading.

[0173] The angle calculation module 1520 is configured to use a linear motion planning algorithm to calculate the first angle information of each joint at each moment from the initial position of the excavator to the first transition point position; use a curved motion planning algorithm to calculate the second angle information of each joint at each moment from the first transition point position to the second transition point position; and use a linear motion planning algorithm to calculate the third angle information of each joint at each moment from the second transition point position to the end position.

[0174] In some embodiments, the angle calculation module 1520 interpolates the posture rotation matrix corresponding to the initial posture and the initial posture transformation matrix corresponding to the first transition point posture to obtain a first posture rotation matrix interpolation sequence; and according to the first posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, obtains the angle sequence of each joint with a timestamp, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

[0175] According to the speed information of the bucket during the excavation process, the speed is interpolated and normalized to obtain a first speed sequence; the displacement of the bucket of the excavator from the initial posture to the first transition point posture is interpolated according to the first speed sequence to obtain a first displacement sequence, and the posture of the bucket is interpolated to obtain a first posture sequence; and based on the combination of the first displacement sequence and the first posture sequence, a first posture rotation matrix interpolation sequence is obtained.

[0176] In some embodiments, according to the curve motion planning algorithm, the first transition point posture to the second transition point posture is divided into a first sub-posture, a second sub-posture, a third sub-posture and a fourth sub-posture, wherein the first sub-posture is the posture of the bucket when it moves from the planned movement after excavation to the state where it can keep the material from leaking; the second sub-posture is the posture of the bucket when it moves to the first target position to the maximum extent in the direction perpendicular to the ground while keeping the material from leaking; the third sub-posture is the posture of the bucket when the joint between the rotating body and the vehicle body of the excavator moves to the target joint angle; and the fourth sub-posture is the posture of the bucket when it moves to the second target position to the maximum extent in the direction perpendicular to the ground while keeping the material from leaking, and the bucket is in a moving state.

[0177] The excavator includes a first joint, a second joint, a third joint and a fourth joint, wherein the first joint is the joint between the rotating body of the excavator and the vehicle body, the second joint is the joint between the rotating body and the boom of the excavator, the third joint is the joint between the boom and the forearm of the excavator, and the fourth joint is the joint between the forearm of the excavator and the bucket.

[0178] The angle calculation module 1520 determines the angle value range of the fourth joint when the bucket can keep the material from leaking; obtains the first posture transformation matrix corresponding to the first sub-posture according to the initial posture transformation matrix corresponding to the first transition point posture and the angle value range of the fourth joint; obtains the second posture transformation matrix corresponding to the second sub-posture according to the first posture transformation matrix, the fifth posture transformation matrix corresponding to the second transition point posture, and the angle value range of the fourth joint; obtains the third posture transformation matrix corresponding to the third sub-posture according to the second posture transformation matrix and the fifth posture transformation matrix; obtains the fourth posture transformation matrix corresponding to the fourth sub-posture according to the third posture transformation matrix, the fifth posture transformation matrix and the angle value range of the fourth joint. transformation matrix; using a linear motion planning algorithm, respectively calculate the angle sequence with a timestamp of each joint between the initial posture transformation matrix and the first posture transformation matrix, the angle sequence with a timestamp of each joint between the first posture transformation matrix and the second posture transformation matrix, the angle sequence with a timestamp of each joint between the second posture transformation matrix and the third posture transformation matrix, the angle sequence with a timestamp of each joint between the third posture transformation matrix and the fourth posture transformation matrix, and the angle sequence with a timestamp of each joint between the fourth posture transformation matrix and the fifth posture transformation matrix, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the motion process.

[0179] For example, based on the initial posture transformation matrix corresponding to the posture of the first transition point, the initial angle of each joint of the excavator is calculated by inverse solution; if the angle of the fourth joint at the predetermined moment is within the angle value range, the angle of the first joint is maintained; if the angle of the fourth joint at the predetermined moment is not within the angle value range, the angle of the fourth joint is adjusted to the angle value range, and the angles of the first joint, the second joint and the third joint are solved; and based on the angles of the first joint, the second joint, the third joint and the fourth joint, the first posture transformation matrix is determined.

[0180] For another example, based on the first pose transformation matrix and the fifth pose transformation matrix, the first intermediate pose is obtained; based on the first intermediate pose, the angles of the first joint, the second joint and the third joint are inversely calculated; based on the angles of the first joint, the second joint and the third joint, the angle of the fourth joint is solved forward; if the angle of the fourth joint solved forward meets the angle value range, the second pose transformation matrix is determined based on the angles of the first joint, the second joint, the third joint and the fourth joint.

[0181] For another example, based on the second posture transformation matrix, the angles of the second joint, the third joint and the fourth joint are calculated by inverse solution; based on the fifth posture transformation matrix, the angle of the first joint is calculated by inverse solution; based on the angles of the first joint, the second joint, the third joint and the fourth joint, the third posture transformation matrix is determined.

[0182] For another example, based on the third posture transformation matrix and the fifth posture transformation matrix, the second intermediate posture is obtained; based on the second intermediate posture, the angles of the first joint, the second joint and the third joint are inversely calculated; based on the angles of the first joint, the second joint and the third joint, the angle of the fourth joint is solved forward; if the angle of the fourth joint solved forward meets the angle value range, then the fourth posture transformation matrix is determined based on the angles of the first joint, the second joint, the third joint and the fourth joint.

[0183] In some embodiments, the angle calculation module 1520 performs complementary processing on the angle sequence with timestamps of each joint between the initial pose transformation matrix and the first pose transformation matrix, the angle sequence with timestamps of each joint between the first pose transformation matrix and the second pose transformation matrix, the angle sequence with timestamps of each joint between the second pose transformation matrix and the third pose transformation matrix, the angle sequence with timestamps of each joint between the third pose transformation matrix and the fourth pose transformation matrix, and the angle sequence with timestamps of each joint between the fourth pose transformation matrix and the fifth pose transformation matrix.

[0184] In some embodiments, the angle calculation module 1520 interpolates the fifth posture transformation matrix corresponding to the second transition point posture and the end posture transformation matrix corresponding to the end posture to obtain a second posture rotation matrix interpolation sequence; and according to the second posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, obtains the angle sequence of each joint with a timestamp, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement process.

[0185] According to the speed information of the bucket during the unloading process, the speed is interpolated and normalized to obtain a second speed sequence; the displacement of the bucket of the excavator from the second transition point posture to the end posture is interpolated in sequence according to the second speed sequence to obtain a second displacement sequence, and the posture of the bucket is interpolated to obtain a second posture sequence; and based on the combination of the second displacement sequence and the second posture sequence, a second posture rotation matrix interpolation sequence is obtained.

[0186] The control module 1530 is configured to control the movement of each joint of the excavator based on the first angle information, the second angle information, and the third angle information to perform an excavation operation.

[0187] In the above embodiment, a material transportation motion planning algorithm that meets the physical constraints of each joint is proposed. At the same time, the idea of segmented planning is adopted, and linear planning and spatial curve planning are combined for planning. A series of transition points are selected in the coordinated working space of the excavator and the transport vehicle. The above two planning methods are used for combined planning, which can complete the entire loading and unloading process at the fastest speed, realize complex automation tasks, and improve work efficiency.

[0188] Figure 16 Schematic diagrams of other embodiments of the excavator control device disclosed herein include a memory 1610 and a processor 1620. Memory 1610 may be a disk, flash memory, or any other non-volatile storage medium. Memory 1610 is used to store instructions in the aforementioned embodiments. Processor 1620, coupled to memory 1610, may be implemented as one or more integrated circuits, such as a microprocessor or microcontroller. Processor 1620 is used to execute instructions stored in memory.

[0189] In this embodiment, by storing data instructions in a memory and then processing the instructions through a processor, the operator's work intensity can be reduced and the operating efficiency of the excavator can be improved in complex and harsh working environments.

[0190] Figure 17 Figure 1 is a schematic diagram of the structure of another embodiment of the excavator control device disclosed herein. The excavator control device includes an excavator collaborative operation management module 1710, an Ethernet communication module 1720, a CAN bus communication module 1730, a manipulator arm position and positioning module 1740, a central processing unit 1750, a power unit module 1760, a manipulator arm electrical control unit module 1770, and a trajectory planning module 1780. The excavator collaborative operation management module 1710 and the trajectory planning module 1780 run on the central processing unit 1750. The excavator collaborative operation management module 1710 and the trajectory planning module 1780 can share data. The excavator collaborative operation management module 1710 establishes a communication connection with a mining truck via the Ethernet communication module 1720 to obtain data such as the truck's position and posture data, vehicle specifications, and operating status. The trajectory planning module 1780 can receive position and posture data of the excavator and the mining truck, excavator specifications, and data on the angles of each manipulator arm joint and connecting rod. The excavator's posture data, excavator specifications, and data on the angles of each joint and link of the manipulator arm are all obtained from the manipulator arm posture positioning module 1740. The central processing unit 1750 has CAN bus and Ethernet communication interfaces. Based on the trajectory planning results output by the trajectory planning module 1780, the central processing unit 1750 transmits them to the manipulator arm electronic control unit 1770 via the CAN bus communication module 1730. The manipulator arm electronic control unit 1770 then transmits the control information to the power system, driving the manipulator arm to perform the planned movements.

[0191] In some embodiments of the present disclosure, an excavator is protected, which includes the excavator control device in the above-mentioned embodiments. After the excavator control device is deployed on the excavator, the excavator can realize complex automation tasks, has good engineering practical value, can greatly liberate manpower, and improve production efficiency.

[0192] In some specific embodiments, Figure 18 As shown, the excavator stops at a position suitable for loading the mining truck, and the driver completes the excavation action by operating the machine. The driver presses the automatic transport button. The trajectory planning unit determines the first transition point, the second transition point, and the final posture point based on the posture data of the excavator and the mining truck, the excavator specification data, the angle data of each joint and connecting rod of the robotic arm, the height of the mining truck cargo box, and the initial posture according to the motion planning method of this application. And perform segmented planning and output an angle sequence with a timestamp. The angle sequence data is transmitted to the robotic arm electronic control unit module through the CAN bus communication unit. The robotic arm electronic control unit module transmits the control information to the power system to drive the robotic arm to operate according to the planned action. At this point, the handling and unloading actions are completed.

[0193] In other embodiments, a computer-readable storage medium stores computer program instructions thereon, which, when executed by a processor, implement the steps of the method in the above-described embodiment. Those skilled in the art will appreciate that the embodiments of the present disclosure may be provided as methods, devices, or computer program products. Therefore, the present disclosure may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present disclosure may take the form of a computer program product implemented on one or more computer-usable non-transient storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0194] The present disclosure is described with reference to flowcharts and / or block diagrams of methods, devices (systems) and computer program products according to embodiments of the present disclosure. It should be understood that each process and / or block in the flowchart and / or block diagram and the combination of processes and / or blocks in the flowchart and / or block diagram can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0195] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0196] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0197] The present disclosure has been described in detail so far. To avoid obscuring the concept of the present disclosure, some details known in the art have not been described. Based on the above description, those skilled in the art can fully understand how to implement the technical solutions disclosed herein.

[0198] Although some specific embodiments of the present disclosure have been described in detail by way of examples, those skilled in the art will appreciate that the above examples are for illustrative purposes only and are not intended to limit the scope of the present disclosure. Those skilled in the art will appreciate that modifications may be made to the above embodiments without departing from the scope and spirit of the present disclosure. The scope of the present disclosure is defined by the appended claims.

Claims

1. A method for controlling an excavator, comprising: Determine the initial posture, first transition point posture, second transition point posture, and terminal posture of the excavator from digging to unloading based on the position and height information of the transport vehicle and the position information of the excavator; Utilizing a linear motion planning algorithm, calculating the first angle information of each joint of the excavator at each moment from the initial posture to the first transition point posture, wherein the posture rotation matrix corresponding to the initial posture and the initial posture transformation matrix corresponding to the first transition point posture are interpolated to obtain a first posture rotation matrix interpolation sequence, and obtaining an angle sequence with a timestamp for each joint based on the first posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, wherein each joint satisfies the angle constraint, velocity constraint, and acceleration constraint of the corresponding joint during the motion process; Calculating, by using a curvilinear motion planning algorithm, second angle information of each joint of the excavator at each moment from the first transition point posture to the second transition point posture; Calculating, using a linear motion planning algorithm, third angle information of each joint of the excavator at each moment from the second transition point posture to the terminal posture; and Based on the first angle information, the second angle information, and the third angle information, the movement of each joint of the excavator is controlled to perform an excavation operation.

2. The excavator control method according to claim 1, wherein: The initial posture is the posture of the excavator when the bucket of the excavator touches the ground and excavation has not yet begun; The first transition point posture is the posture of the excavator when the bucket has completed excavation; The second transition point posture is the posture of the excavator when the bucket moves to above the transport vehicle, descends to a predetermined height, is in a static state and is not unloading; and The terminal posture is the posture of the excavator when the bucket has finished unloading.

3. The excavator control method according to claim 2, wherein: Interpolating the posture rotation matrix corresponding to the initial posture and the initial posture transformation matrix corresponding to the first transition point posture to obtain a first posture rotation matrix interpolation sequence includes: interpolating and normalizing the speed according to speed information of the bucket during the digging process to obtain a first speed sequence; sequentially interpolating the displacement of the bucket of the excavator from the initial posture to the first transition point posture according to the first speed sequence to obtain a first displacement sequence, and interpolating the posture of the bucket to obtain a first posture sequence; and Based on the combination of the first displacement sequence and the first posture sequence, the first posture rotation matrix interpolation sequence is obtained.

4. The excavator control method according to any one of claims 2 to 3, wherein: According to the curve motion planning algorithm, the first transition point posture to the second transition point posture is divided into a first sub-posture, a second sub-posture, a third sub-posture and a fourth sub-posture, wherein, The first sub-posture is the posture of the bucket when it moves from the planned state after excavation to the state where the material can be kept from leaking; The second sub-posture is the posture of the bucket when it moves to the first target position to the maximum extent in a direction perpendicular to the ground while preventing material from leaking; The third sub-posture is the posture of the bucket when the joint between the rotating body and the vehicle body of the excavator moves to a target joint angle; and The fourth sub-posture is the posture of the bucket when it moves to the second target position to the maximum extent in a direction perpendicular to the ground while keeping the material from leaking, and when the bucket is in a moving state.

5. The excavator control method according to claim 4, wherein: The excavator includes a first joint, a second joint, a third joint and a fourth joint, wherein the first joint is the joint between the rotating body and the vehicle body of the excavator, the second joint is the joint between the rotating body and the upper arm of the excavator, the third joint is the joint between the upper arm and the forearm of the excavator, and the fourth joint is the joint between the forearm of the excavator and the bucket.

6. The excavator control method according to claim 5, wherein: The curvilinear motion planning algorithm is used to calculate the position from the first transition point to the second transition point. The second angle information of each joint of the excavator at each moment includes: The angle value range of the fourth joint when determining that the bucket can keep the material from leaking; Obtaining a first pose transformation matrix corresponding to the first sub-pose according to the initial pose transformation matrix corresponding to the first transition point pose and the angle value range of the fourth joint; Obtaining a second posture transformation matrix corresponding to the second sub-posture according to the first posture transformation matrix, the fifth posture transformation matrix corresponding to the second transition point posture, and the angle value range of the fourth joint; Obtaining a third posture transformation matrix corresponding to the third sub-pose according to the second posture transformation matrix and the fifth posture transformation matrix; Obtaining a fourth posture transformation matrix corresponding to the fourth sub-posture according to the third posture transformation matrix, the fifth posture transformation matrix, and the angle value range of the fourth joint; and Using a linear motion planning algorithm, the angle sequence with a timestamp of each joint between the initial posture transformation matrix and the first posture transformation matrix, the angle sequence with a timestamp of each joint between the first posture transformation matrix and the second posture transformation matrix, the angle sequence with a timestamp of each joint between the second posture transformation matrix and the third posture transformation matrix, the angle sequence with a timestamp of each joint between the third posture transformation matrix and the fourth posture transformation matrix, and the angle sequence with a timestamp of each joint between the fourth posture transformation matrix and the fifth posture transformation matrix are calculated respectively, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

7. The excavator control method according to claim 6, wherein: According to the initial posture transformation matrix corresponding to the first transition point posture and the angle value range of the fourth joint, obtaining the first posture transformation matrix corresponding to the first sub-posture includes: Inversely calculating the initial angle of each joint of the excavator according to the initial posture transformation matrix corresponding to the first transition point posture; If the angle of the fourth joint at the predetermined time is within the angle value range, the angle of the first joint is maintained; if the angle of the fourth joint at the predetermined time is not within the angle value range, the angle of the fourth joint is adjusted to be within the angle value range, and the angles of the first joint, the second joint, and the third joint are solved; and The first posture transformation matrix is determined according to the angles of the first joint, the second joint, the third joint, and the fourth joint.

8. The excavator control method according to claim 6, wherein: According to the first posture transformation matrix, the fifth posture transformation matrix, and the angle value range of the fourth joint, the second posture transformation matrix is obtained, which includes: Obtaining a first intermediate posture according to the first posture transformation matrix and the fifth posture transformation matrix; Inversely calculating the angles of the first joint, the second joint, and the third joint according to the first intermediate posture; Correctly solving the angle of the fourth joint based on the angles of the first joint, the second joint, and the third joint; and If the angle of the fourth joint obtained by correct solution satisfies the angle value range, the second posture transformation matrix is determined according to the angles of the first joint, the second joint, the third joint and the fourth joint.

9. The excavator control method according to claim 6, wherein: Obtaining the third pose transformation matrix according to the second pose transformation matrix and the fifth pose transformation matrix includes: Inversely calculate the angles of the second joint, the third joint, and the fourth joint according to the second posture transformation matrix; Inversely solving the angle of the first joint according to the fifth posture transformation matrix; and The third posture transformation matrix is determined according to the angles of the first joint, the second joint, the third joint, and the fourth joint.

10. The excavator control method according to claim 6, wherein: According to the third posture transformation matrix, the fifth posture transformation matrix and the angle value range of the fourth joint, the fourth posture transformation matrix is obtained, which includes: Obtaining a second intermediate posture according to the third posture transformation matrix and the fifth posture transformation matrix; Inversely calculating the angles of the first joint, the second joint, and the third joint according to the second intermediate posture; Correctly solving the angle of the fourth joint based on the angles of the first joint, the second joint, and the third joint; and If the angle of the fourth joint obtained by correct solution satisfies the angle value range, the fourth posture transformation matrix is determined according to the angles of the first joint, the second joint, the third joint and the fourth joint.

11. The excavator control method according to claim 6, further comprising: Complementary processing is performed on the angle sequence with timestamps of each joint between the initial pose transformation matrix and the first pose transformation matrix, the angle sequence with timestamps of each joint between the first pose transformation matrix and the second pose transformation matrix, the angle sequence with timestamps of each joint between the second pose transformation matrix and the third pose transformation matrix, the angle sequence with timestamps of each joint between the third pose transformation matrix and the fourth pose transformation matrix, and the angle sequence with timestamps of each joint between the fourth pose transformation matrix and the fifth pose transformation matrix.

12. The excavator control method according to any one of claims 2 to 3, wherein: Using a linear motion planning algorithm, the excavator's posture from the second transition point to the end posture is calculated. The third angle information of each joint at each moment includes: interpolating the fifth posture transformation matrix corresponding to the second transition point posture and the terminal posture transformation matrix corresponding to the terminal posture to obtain a second posture rotation matrix interpolation sequence; and According to the interpolation sequence of the second posture rotation matrix and the time interval when controlling the joint angle, an angle sequence with a timestamp for each joint is obtained, wherein each joint satisfies the angle constraint, velocity constraint and acceleration constraint of the corresponding joint during the movement.

13. The excavator control method according to claim 12, wherein: Interpolating the fifth posture transformation matrix corresponding to the second transition point posture and the terminal posture transformation matrix corresponding to the terminal posture to obtain a second posture rotation matrix interpolation sequence includes: interpolating and normalizing the speed according to speed information of the bucket during the unloading process to obtain a second speed sequence; sequentially interpolating the displacement of the bucket of the excavator from the second transition point posture to the end posture according to the second speed sequence to obtain a second displacement sequence, and interpolating the posture of the bucket to obtain a second posture sequence; and Based on the combination of the second displacement sequence and the second posture sequence, the second posture rotation matrix interpolation sequence is obtained.

14. An excavator control device comprising: A posture determination module is configured to determine an initial posture, a first transition point posture, a second transition point posture, and an end posture of the excavator during the process from digging to unloading based on the position and height information of the transport vehicle and the position information of the excavator; an angle calculation module configured to calculate, by using a linear motion planning algorithm, first angle information of each joint of the excavator at each moment from the initial posture to the first transition point posture; Calculating, by using a curvilinear motion planning algorithm, second angle information of each joint of the excavator at each moment from the first transition point posture to the second transition point posture; Utilizing a linear motion planning algorithm, calculating the third angle information of each joint of the excavator at each moment from the second transition point posture to the end posture, wherein the posture rotation matrix corresponding to the initial posture and the initial posture transformation matrix corresponding to the first transition point posture are interpolated to obtain a first posture rotation matrix interpolation sequence, and according to the first posture rotation matrix interpolation sequence and the time interval when controlling the joint angle, obtaining a timestamp-equipped angle sequence of each joint, wherein each joint satisfies the angle constraint, velocity constraint, and acceleration constraint of the corresponding joint during the motion process; and The control module is configured to control the movement of each joint of the excavator based on the first angle information, the second angle information and the third angle information to perform an excavation operation.

15. An excavator control device comprising: Memory; as well as A processor coupled to the memory, wherein the processor is configured to execute the excavator control method according to any one of claims 1 to 13 based on instructions stored in the memory.

16. An excavator comprising: The excavator control device according to claim 14 or 15.

17. A non-transitory computer-readable storage medium having computer program instructions stored thereon, wherein when the instructions are executed by a processor, the excavator control method according to any one of claims 1 to 13 is implemented.

Images