Dumping control method of dumping plough

Abstract

The invention relates to the field of full-automatic unmanned equipment applied to mines and earthworks, in particular to a dumping control method of a dumping plough. The dumping plough dumping control method comprises the following steps: acquiring a global point cloud containing a dumping arm; according to the relative position parameters of the discharge arm in the coordinate system, cutting the target point cloud of the discharge arm in a real-time state from the global point cloud; performing point cloud fitting and / or point cloud registration in the target point cloud of the discharge arm to obtain a pitch angle of the discharge arm; the relative pose of the tail end of the discharging arm is obtained, and the position of a discharging opening is determined according to the relative pose, the pitch angle of the discharging arm and a preset kinematic model; point cloud data of a dumping area is obtained by extracting point cloud data of a dumping plough operation area, point cloud segmentation is performed on the point cloud data of the dumping area to obtain point cloud data of a target object, and finally data analysis is performed on the point cloud data of the target object to obtain operation state data representing operation conditions. And the automatic monitoring of the dumping operation state is realized.

Description

Technical Field

[0001] The present application relates to the field of fully automatic unmanned equipment used in mines and earthworks, and in particular to a method for controlling the discharge of earthmoving machines. Background Art

[0002] In traditional mining operations, the earth dumper is an important earthwork handling equipment. The precise positioning and control of its discharge port is of great significance to improving operation efficiency, ensuring operation safety and reducing environmental pollution.

[0003] However, many mines are still using traditional manually operated dumpers, which have significant technical limitations in controlling the position of the dump opening. That is, the traditional manually operated dumper mainly relies on the operator's experience and judgment during the dumping process. Since dumpers are usually in complex and changeable operating environments, such as field mines and hilly areas, these environmental factors make it particularly difficult to accurately control the dump opening. The operator needs to constantly estimate the relative position of the dump opening and the pile based on factors such as the height, shape and position of the pile, and adjust the angle of the dump arm and the height of the dump opening accordingly.

[0004] However, this method of controlling the discharge port position based on manual estimation has many shortcomings: First, manual estimation often has errors, especially when there is high dust, insufficient light, obstructed vision or complex working environment. If the height difference between the discharge port and the material pile is small, the materials tend to accumulate quickly and may cause interference between the discharge arm and the material pile. On the contrary, when the discharge port is too high, the material impact is large and it is easy to cause a large amount of dust to further interfere with the line of sight. Summary of the invention

[0005] The present application provides a method for controlling the discharge of earthmoving equipment, so as to solve the above-mentioned technical problems in the prior art.

[0006] In a first aspect of the present application, a method for controlling the discharge of a soil discharger is provided, comprising: obtaining a global point cloud including a discharge arm; cropping a target point cloud of the discharge arm in real time from the global point cloud according to the relative position parameters of the discharge arm in the coordinate system; performing point cloud fitting and / or point cloud registration in the target point cloud of the discharge arm to obtain a pitch angle of the discharge arm; obtaining the relative posture of the end of the discharge arm, and determining the position of the discharge port according to the relative posture, the pitch angle of the discharge arm and a preset kinematic model.

[0007] In a further scheme of the present invention, the real-time target point cloud of the discharge arm is cropped from the global point cloud according to the relative position parameters of the discharge arm in the coordinate system, comprising: taking the image acquisition device as the reference position, and setting a region of interest according to the relative position of the discharge arm relative to the reference position; cropping the target point cloud of the discharge arm from the global point cloud according to the region of interest; and obtaining at least one of the quantity, size parameters, and morphological features of the target point cloud of the discharge arm to verify the target point cloud of the discharge arm.

[0008] In a further scheme of the present invention, the real-time target point cloud of the discharge arm is cropped from the global point cloud according to the relative position parameters of the discharge arm in the coordinate system, and also includes: when the verification result is that the number of the target point cloud of the discharge arm is less than a preset number threshold; or the size parameter is less than the size threshold, or the morphological feature does not have any of the preset reference features, the verification fails, and the size of the bounding box of the area of ​​interest is re-increased proportionally at this time; if the verification fails under the threshold number of consecutive times, the acquisition of the target point cloud of the discharge arm is stopped and an alert is issued.

[0009] In a further embodiment of the present invention, point cloud fitting and / or point cloud registration are performed in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm, including: performing plane fitting or straight line fitting on the bottom of the target point cloud of the discharge arm to obtain a first plane or a first straight line; based on the direction vector of the first plane or the first straight line, and calculating the angle between the direction vector and a preset reference horizontal vector to obtain the pitch angle of the discharge arm.

[0010] In a further embodiment of the present invention, point cloud fitting and / or point cloud registration are performed in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm, including: registering the target point cloud of the discharge arm and a preset model reference point cloud to iteratively find the transformation matrix of corresponding points between the target point cloud of the discharge arm and the model reference point cloud; calculating the Euler angle representation according to the transformation matrix, and extracting the pitch angle from the Euler angle.

[0011] In a further embodiment of the present invention, the registering the discharge arm target point cloud and the preset model reference point cloud to iteratively find a transformation matrix of corresponding points between the discharge arm target point cloud and the model reference point cloud comprises:

[0012] In a further scheme of the present invention, the discharge arm target point cloud and the preset model reference point cloud are both projected onto a two-dimensional plane in the same preset direction; on the two-dimensional plane, for each point in the discharge arm target point cloud, its nearest point in the model reference point cloud is found as a corresponding point; based on the corresponding points found, an initial transformation matrix can be used to preliminarily align the discharge arm target point cloud to the model reference point cloud; an iterative method is used to continuously update the initial transformation matrix to re-find corresponding points in each iteration, so that the average distance between the corresponding points between the discharge arm target point cloud and the model reference point cloud gradually converges; when the average distance reaches a convergence threshold or the maximum number of iterations is reached, the iteration is stopped, and the final transformation matrix is ​​output.

[0013] In a further embodiment of the present invention, the Euler angle representation is calculated according to the transformation matrix, and the pitch angle is extracted from the Euler angle, including: the transformation matrix T is a 4-by-4 matrix, and the rotation matrix R is extracted from the transformation matrix T; the pitch angle θ is calculated by the following formula in a rotation order of ZYX;

[0014]

[0015] atan2 is the inverse tangent function, cos is the cosine function, ψ is the yaw angle in the Euler angle, Rn1,n2 and Rn1,n3 are elements in the same column of the rotation matrix R; wherein the initial model of the discharge arm is horizontally placed without yaw angle rotation, and cos(ψ)=1.

[0016] In a further embodiment of the present invention, the relative position and posture of the end of the discharge arm is obtained, including: obtaining the base of the discharge arm, the rotating joint rotate, and the end end provided with the discharge port; and obtaining the relative position and posture of the end of the discharge arm by the following formula:

[0017]

[0018]

[0019]

[0020] Among them, the is the relative position of the end of the discharge arm relative to the base; is the relative position of the end of the discharge arm relative to the rotary joint, is the translation vector of the arm end relative to the base; It is the rotation vector of the end of the discharge arm relative to the rotary joint relative to the Y axis.

[0021] In a further scheme of the present invention, the position of the discharge port is determined according to the relative posture, the pitch angle of the discharge arm and a preset kinematic model, including: calculating the first position of the end of the discharge arm in the discharge arm coordinate system according to the pitch angle of the discharge arm, and the length parameters, connection method and motion range between the rotation joints in the kinematic model; according to the relative posture, converting the first position to a coordinate system with the reference position as the origin to obtain the position of the discharge port.

[0022] In a further embodiment of the present invention, the discharge control method of the earthmoving machine also includes: acquiring a point cloud of a material pile in real time; acquiring characteristic parameters of the material pile based on the point cloud of the material pile, wherein the characteristic parameters include the height of the material pile; controlling the discharge arm to adjust the pitch angle of the discharge arm so that the discharge port position always maintains a preset discharge distance from the height of the material pile; when the height parameter is greater than a preset height threshold, controlling the discharge arm to rotate along the Z axis to the next target area for discharge.

[0023] In summary, the method, device, electronic device and medium for monitoring the discharging state of a soil dumper provided by the present application have at least the following beneficial effects:

[0024] The point cloud data of the discharge area is obtained by extracting the point cloud data of the earth-discharging machine operating area, and then the point cloud data of the discharge area is segmented to obtain the point cloud data of the target object. Finally, the point cloud data of the target object is analyzed to obtain the operating status data representing the operating conditions. Automatic monitoring of the earth-discharging operating status is realized without the need for human participation, which can avoid analysis errors and long time costs caused by human subjectivity, thereby improving the accuracy and efficiency of monitoring the discharge status of the earth-discharging machine. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the technical solutions in the specific implementation modes of the present application, the drawings required for use in the specific implementation modes will be briefly introduced below in conjunction with the accompanying drawings. It is obvious that the drawings described below are some implementation modes of the present application. For those skilled in the art, other drawings or solutions can be obtained based on these drawings without paying any creative work.

[0026] Figure 1 A flow chart of a method for controlling the discharge of earthmoving equipment according to an embodiment of the present application;

[0027] Figure 2 A specific flow chart of step S20 in a method for controlling the discharge of earthmoving equipment in one embodiment of the present application;

[0028] Figure 3 A schematic diagram of a point cloud of a discharge arm demonstrated in one embodiment of the present application;

[0029] Figure 4 A specific flow chart of step S23 in a method for controlling the discharge of earthmoving equipment in one embodiment of the present application;

[0030] Figure 5 A specific flow chart of step S30 in a method for controlling the discharge of earthmoving equipment in one embodiment of the present application;

[0031] Figure 6 It is a specific flow chart of step S30 in the method for controlling the discharge of earthmoving equipment in another embodiment of the present application;

[0032] Figure 7 It is a specific flow chart of step S31' in the method for controlling the discharging of earthmoving equipment in another embodiment of the present application;

[0033] Figure 8 A schematic diagram of a pitch angle θ of a discharge arm of a discharge arm demonstrated in one embodiment of the present application;

[0034] Fig. 9 A specific flow chart of step S40 in the method for controlling the discharging of earthmoving equipment in another embodiment of the present application; and

[0035] Fig.10 It is a specific flow chart of step S50 in the method for controlling the discharge of earthmoving equipment in another embodiment of the present application. DETAILED DESCRIPTION

[0036] In order to make the above and other features and advantages of the present application more clear, the present application is further described below in conjunction with the accompanying drawings. It should be understood that the specific embodiments given herein are for the purpose of explaining to those skilled in the art and are only exemplary and not restrictive.

[0037] In the following description, many specific details are set forth to provide a thorough understanding of the present application. However, it is apparent to those skilled in the art that specific details need not be adopted to practice the present application. In other cases, well-known steps or operations are not described in detail to avoid blurring the present application.

[0038] In order to solve the shortcomings of the traditional manually operated earth-discharging machine in controlling the position of the discharge port, the applicant initially adopted the method of obtaining the real-time point cloud of the discharge arm and comparing it with the initial point cloud of the discharge arm; however, it was found in the actual process that: since the discharge arm of the earth-discharging machine occupies a large space, its point cloud data often has a large data volume, low density and high noise level. In addition, the point cloud data of the discharge arm may also contain interactive information with the surrounding environment (such as the ground, other equipment, etc.). If the conventional end detection method of the robotic arm is adopted, that is, the end posture is calculated using the initial point cloud and the real-time point cloud, and the space of the real-time point cloud is divided into a regular 3D grid, for each point in the initial point cloud, its corresponding grid in the real-time point cloud grid space is found. The registered transformation matrix is ​​obtained by calculating the normal distribution of the points of the initial point cloud in the corresponding grid. Faced with a large number of point clouds of the earthmoving machine, the response of the calculation process is very difficult to ensure, and it is particularly easy to trigger a shutdown. Therefore, an embodiment of the present invention aims to provide a new method for locating the discharge port position, which adopts point cloud recognition to directly combine the motion model of the earthmoving machine to obtain the discharge port position, and according to the real-time calculated discharge port position, the earthmoving machine is automatically controlled unmanned.

[0039] refer to Figure 1 The present application provides a method for controlling the discharge of earthmoving equipment, which is characterized by comprising:

[0040] Step S10, obtaining a global point cloud including the discharge arm;

[0041] Step S20, cutting the real-time target point cloud of the discharging arm from the global point cloud according to the relative position parameters of the discharging arm in the coordinate system;

[0042] Step S30, performing point cloud fitting and / or point cloud registration in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm;

[0043] Step S40, obtaining the relative posture of the end of the discharge arm, and determining the position of the discharge port according to the relative posture, the pitch angle of the discharge arm and a preset kinematic model.

[0044] In the embodiment of the present invention, the global point cloud is a three-dimensional point set containing the entire working area (including the discharge arm) captured by a certain 3D scanning device (such as a laser radar, a depth camera, etc.). The discharge arm is the main structure of the earth discharger, and the material is discharged through the belt conveyor on it; the relative position parameter is the known spatial position of the discharge arm in a specific coordinate system (such as a radar coordinate system). Based on these parameters, the point cloud is cropped from the global point cloud to extract only the part of the point cloud related to the discharge arm, that is, the above-mentioned "discharge arm target point cloud".

[0045] Based on the general inventive concept of the embodiment of the present invention, in an optional manner, mathematical methods (such as least squares method, RANSAC, etc.) can be used to fit the point cloud data to a predetermined geometric shape (such as a straight line, a plane, a cylinder, etc.) to extract information about the posture of the discharge arm.

[0046] Based on the general inventive concept of the embodiment of the present invention, in another optional manner, the target point cloud of the discharge arm is aligned with the preset reference point cloud to calculate the transformation relationship between the two.

[0047] In the embodiment of the present invention, the “pitch angle” is defined as the rotation angle of the discharge arm in the current state relative to the discharge arm in the reset state, and is usually used to describe the degree of inclination of the discharge arm.

[0048] The relative position and posture is the position and posture of the end of the discharge arm (usually the position of the discharge port) relative to a reference point or coordinate system. The kinematic model describes the relative position and motion relationship between the joints of the discharge arm. Combining the relative position and posture of the end of the discharge arm, the pitch angle and the kinematic model, the actual position of the discharge port in the current state can be calculated.

[0049] In a preferred embodiment, a laser radar is disposed on each side of the discharge arm, and a complete global point cloud is formed by splicing to avoid local occlusion.

[0050] In summary, the present application can extract the posture information of the discharge arm and calculate its pitch angle with high precision through one of point cloud fitting and registration, which provides an accurate data basis for the subsequent determination of the discharge port position. And compared with the traditional manual measurement or estimation method, this can more accurately reflect the actual position and posture of the discharge arm, thereby improving the accuracy of the discharge port positioning; only by combining the kinematic model of the discharge arm, by real-time acquisition of the global point cloud data containing the discharge arm, the position and posture changes of the discharge arm can be monitored in real time, so as to dynamically obtain the position coordinates of the discharge port; compared with the traditional machine arm end acquisition method, the use of the relative position parameters of the discharge arm in the coordinate system for cutting can greatly reduce the amount of calculation; and based on the simple characteristics of the earthmoving machine motion model, the embodiment of the present invention first uses the discharge arm target point cloud to perform point cloud fitting and / or point cloud registration to obtain the discharge arm pitch angle, and then obtains the discharge port position through the motion model combined with the pitch angle; thus, it is specifically suitable for continuous and stable earthmoving machine equipment, and considering its stability and reliability in complex environments, it can ensure the stable and reliable operation of the discharge process.

[0051] like Figure 2 In step S20, according to the relative position parameters of the discharging arm in the coordinate system, the target point cloud of the discharging arm in the real-time state is cut from the global point cloud, including:

[0052] Step S21, using the image acquisition device as a reference position, setting a region of interest according to the relative position of the discharge arm relative to the reference position;

[0053] Step S22, cutting out the target point cloud of the discharge arm from the global point cloud according to the region of interest;

[0054] Step S23, obtaining at least one of the quantity, size parameters, and morphological features of the target point cloud of the discharge arm to verify the target point cloud of the discharge arm.

[0055] First, a reference position needs to be determined. In the embodiment of the present invention, the coordinates of the installation point of the image acquisition device (such as a laser radar, a depth camera, etc.) are used as the reference, that is, as the (0, 0, 0) coordinates in the three-dimensional space. This reference position is the reference point for all subsequent relative position measurements. According to the physical position of the discharge arm relative to the image acquisition device (such as distance, angle, etc.), the relative position of the discharge arm in the coordinate system of the image acquisition device can be calculated.

[0056] Based on the relative position of the discharge arm, a region of interest (ROI) is set within the field of view of the image acquisition device. This region can completely contain the point cloud data of the discharge arm in real time. Using the region of interest set in step S21, the point cloud data located in this region is cut out from the global point cloud data. That is, the target point cloud of the discharge arm in real time. Figure 3 As shown, the circle indicates the discharge port position.

[0057] In step S23, the number of points in the cropped discharge arm target point cloud is calculated. This number can be used as an indicator to measure the quality of the point cloud; alternatively, the size parameters of the discharge arm, such as length, width, height, etc., can be calculated based on the discharge arm target point cloud. These parameters can be compared with the preset discharge arm size to check the accuracy of the point cloud data. Alternatively, the correctness and integrity of the point cloud data can be further verified by analyzing the shape and texture of the discharge arm target point cloud. For example, it can be checked whether the outline of the discharge arm is clear, whether there is missing or redundant point cloud data, etc. During the verification process, the calculated number, size parameters and morphological features are compared with the preset threshold or standard to determine whether the discharge arm target point cloud meets the requirements. If the requirements are not met, it may be necessary to re-set the area of ​​interest or adjust the position and parameters of the image acquisition device to obtain more accurate point cloud data.

[0058] like Figure 4 In a preferred embodiment of the present invention, obtaining at least one of the quantity, size parameters, and morphological features of the target point cloud of the discharge arm to verify the target point cloud of the discharge arm includes:

[0059] Step S231, preferentially obtaining the number of target point clouds of the discharging arm and comparing it with a preset number threshold;

[0060] Step S232: When the number of the target point cloud of the discharge arm is less than or equal to the preset number threshold, the verification fails;

[0061] Step S233, when the number of the target point cloud of the discharge arm is greater than a preset number threshold, obtaining the similarity between the morphological features of the target point cloud of the discharge arm and the preset reference morphology;

[0062] Step S234: When the similarity is less than or equal to the preset similarity threshold, the verification fails, otherwise the verification succeeds.

[0063] First, the number of cropped nesting arm target point clouds is obtained first. This number is compared with the preset number threshold. The number threshold is a pre-set value used to determine whether the point cloud data is rich enough. If the number of nesting arm target point clouds is less than or equal to the preset number threshold, the verification is judged to fail. This means that the point cloud data may not be sufficient for subsequent analysis or processing.

[0064] On the contrary, when the number of the target point cloud of the discharge arm is greater than the preset number threshold, the morphological features of the point cloud are further obtained. The morphological features can be extracted through the spatial distribution, shape, texture and other information of the point cloud data. The extracted morphological features are compared with the preset reference morphology to calculate the similarity between them. The similarity can be calculated by comparing the degree of difference of the features or using a special similarity measurement algorithm. For example, the key points of the target point cloud of the discharge arm, such as n right-angle inflection points, are extracted, and the angle differences between the right-angle inflection points of the target point cloud of the discharge arm and the inflection points in the reference morphology are counted in turn and weighted addition is performed to judge the similarity.

[0065] If the calculated similarity is less than or equal to the preset similarity threshold, the verification is judged to have failed. This means that the morphological features of the cropped target point cloud of the discharge arm are quite different from the expected reference morphology and may not meet the requirements of subsequent analysis or processing. If the similarity is greater than the preset similarity threshold, the verification is judged to have succeeded, indicating that the quality of the cropped target point cloud of the discharge arm is good and can be processed and analyzed subsequently. In the above method, the number of point clouds is an intuitive indicator of data quality. When the number of point clouds is insufficient, the data may be incomplete and cannot accurately reflect the morphology and features of the target object. Therefore, prioritizing the number of point clouds can quickly filter out data of poor quality and avoid wasting time and computing resources on these invalid data. In some cases, even if the point cloud morphological features are highly similar to the preset reference morphology, if the number of point clouds is too small, the data quality may be poor. Prioritizing the number of point clouds can avoid misjudging the data quality due to similar morphological features. .

[0066] In a variation of the embodiment of the present invention, step S20 further includes:

[0067] Step S24: when verification fails, re-enlarge the size of the bounding box of the region of interest in proportion;

[0068] Step S25: If the number of consecutive verification failures reaches a preset threshold, stop acquiring the target point cloud of the discharge arm and issue an early warning.

[0069] First, preset a suitable ratio (such as 10%, 20%, etc.) to increase the bounding box size of the ROI. This ratio can be adjusted according to the actual situation to obtain as much relevant point cloud data as possible while maintaining calculation efficiency. Calculate the new bounding box size: Calculate the new bounding box size based on the current ROI bounding box size and the determined adjustment ratio. That is, the new bounding box expands outward to a certain range based on the original bounding box, so that more point cloud data can be captured in the next round of cropping; if multiple consecutive verifications fail, it may only be that there is a problem with the input global point cloud, such as a failure of the image acquisition device, a serious offset in the position of the discharge arm, etc. In this case, it is necessary to stop acquiring the target point cloud of the discharge arm and trigger an early warning.

[0070] In an embodiment of the present invention, the threshold number of times can be set according to actual conditions to ensure timely response when problems arise. After each verification failure, the number of consecutive verification failures is counted. If the number of consecutive verification failures reaches or exceeds the preset threshold number of times, subsequent operations are triggered. When the number of consecutive verification failures reaches the threshold, stop acquiring the target point cloud data of the discharge arm. This can avoid continuing to waste resources on invalid data acquisition and ensure the normal operation of the system and the effectiveness of data processing.

[0071] In an alternative approach,

[0072] like Figure 5 In step S30, point cloud fitting and / or point cloud registration are performed in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm, including:

[0073] Step S31, performing plane fitting or straight line fitting on the bottom of the target point cloud of the discharge arm to obtain a first plane or a first straight line;

[0074] Step S32: according to the direction vector of the first plane or the first straight line, the angle between the direction vector and a preset reference horizontal vector is calculated to obtain the pitch angle of the discharge arm.

[0075] It can be understood that the above steps S31 to S32 are adapted based on the morphological features of the discharge arm. When performing straight line fitting, it is preferred to project the target point cloud of the discharge arm onto a plane to reduce the dimension. That is, for each point in the target point cloud of the discharge arm, calculate its vertical distance to the projection plane, and move this point along this distance in a direction perpendicular to the plane until it is located on the plane. In this way, a two-dimensional point set is obtained (such as a two-dimensional point cloud in the xy plane, x is the horizontal direction, and y is the vertical direction), which represents the projection of the original three-dimensional point cloud on the projection plane. From the projected point set, the point cloud with the minimum y value is selected at equal intervals on the x-axis, so as to preliminarily screen out the points belonging to the bottom of the discharge arm. Subsequently, a straight line fitting algorithm, such as Hough transform, least squares method, etc., is used to fit these points to obtain a first straight line of best fit.

[0076] A direction vector can be calculated from the fitted plane or line. For a plane, this direction vector can be represented by the normal vector of the plane; for a line, this direction vector can be the direction vector of the line. A reference vector representing the horizontal direction is preset in the system. This vector is usually a fixed vector parallel to the horizontal plane. Use the vector angle calculation formula, such as the dot product formula, to calculate the angle between the fitted direction vector and the preset reference horizontal vector. This angle, or the complementary angle of the angle, is the pitch angle of the discharge arm. The size and direction of the angle can indicate the degree of inclination of the discharge arm relative to the horizontal plane.

[0077] like Figures 6 to 8 In another optional manner, point cloud fitting and / or point cloud registration are performed in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm, including:

[0078] Step S31', registering the target point cloud of the discharge arm and the preset model reference point cloud, so as to iteratively find the conversion matrix of corresponding points between the target point cloud of the discharge arm and the model reference point cloud;

[0079] Step S32', calculating the Euler angle representation according to the transformation matrix, and extracting the pitch angle from the Euler angle.

[0080] In a preferred embodiment, the target point cloud of the discharge arm is registered with the preset model reference point cloud to iteratively find the transformation matrix of the corresponding points between the target point cloud of the discharge arm and the model reference point cloud, including:

[0081] Step S311', projecting the discharge arm target point cloud and the preset model reference point cloud onto a two-dimensional plane in the same preset direction;

[0082] Step S312': on a two-dimensional plane, for each point in the target point cloud of the discharge arm, find its nearest point in the model reference point cloud as a corresponding point;

[0083] Step S313': Based on the corresponding points found, an initial transformation matrix can be generated to preliminarily align the target point cloud of the discharge arm to the reference point cloud of the model;

[0084] Step S314', using an iterative method to continuously update the initial transformation matrix, so as to re-find corresponding points in each iteration, so that the average distance between the corresponding points between the target point cloud of the discharge arm and the reference point cloud of the model gradually converges;

[0085] Step S315': when the average distance reaches a preset convergence threshold or the number of iterations reaches a preset maximum number of iterations, the iteration is stopped and the final transformation matrix is ​​output.

[0086] In the preferred method of the embodiment of the present invention, the three-dimensional point cloud data is reduced to a two-dimensional plane for subsequent processing. A suitable projection direction is selected (preferably a direction perpendicular to the ground), and then the discharge arm target point cloud and the model reference point cloud are projected onto the same two-dimensional plane along this direction. A dimensionality reduction effect is achieved and the calculation response is accelerated. For each point in the discharge arm target point cloud on the two-dimensional plane, the nearest point in the model reference point cloud is found as a potential corresponding point.

[0087] Then, for each point in the target point cloud of the nesting arm, all points in the model reference point cloud are traversed, the Euclidean distance between them is calculated, and the point with the smallest distance is found as the corresponding point. To improve efficiency, data structures such as kd trees can be used to accelerate the search for the nearest point. Based on the corresponding points found, an initial transformation matrix is ​​calculated to preliminarily align the target point cloud of the nesting arm to the model reference point cloud. The transformation matrix is ​​calculated based on the corresponding points using the least squares method or other optimization methods. The initial transformation matrix may not be optimal, but it provides a starting point for subsequent iterations.

[0088] Starting from the initial transformation matrix, the transformation matrix is ​​continuously optimized through iteration so that the average distance between the corresponding points between the target point cloud of the discharge arm and the model reference point cloud gradually decreases. In each iteration, the current transformation matrix is ​​used to transform the target point cloud of the discharge arm into the space of the model reference point cloud. Then the corresponding points are re-searched on the two-dimensional plane. Based on the new corresponding points, a new transformation matrix is ​​calculated. The above process is repeated until the convergence condition is met (such as the average distance between the corresponding points is less than a certain threshold) or the number of iterations has reached the maximum number of iterations. And the final transformation matrix is ​​output. The final transformation matrix can accurately transform the target point cloud of the discharge arm to a position aligned with the model reference point cloud.

[0089] Calculate the Euler angle representation based on the transformation matrix and extract the pitch angle from the Euler angle, including:

[0090] Step S321', the transformation matrix T is a 4x4 matrix, and the rotation matrix R is extracted from the transformation matrix T;

[0091] Step S322', in the rotation order of ZYX, calculate the pitch angle θ by the following formula;

[0092]

[0093] atan2 is the inverse tangent function, cos is the cosine function, ψ is the yaw angle in the Euler angle, Rn1,n2 and Rn1,n3 are elements in the same column of the rotation matrix R; the initial model of the discharge arm is horizontally placed without yaw angle rotation, and cos(ψ)=1.

[0094] For example, when extracting the pitch angle (Pitch, θ) from the rotation matrix T, the pitch angle (Pitch, θ) is extracted from the 4x4 transformation matrix T; when the 4x4 transformation matrix T is used, this matrix usually contains transformation information such as translation, rotation, and scaling. In order to extract the pitch angle (Pitch, θ) from such a matrix, first obtain a 3x3 sub-matrix, that is, obtain the rotation matrix R, which describes the rotation of the object in three-dimensional space.

[0095] That is, the 3x3 part in T. This 3x3 matrix describes the rotation of the object in three-dimensional space. Assume that this 3x3 rotation matrix is ​​R. The pitch angle θ is the angle of rotation around the Y axis, which describes the tilt of the discharge arm relative to the initial state. In order to extract the pitch angle θ from the rotation matrix R.

[0096] Assume that the third column (R_{3,n}) of the rotation matrix represents the direction of the Z axis in the original coordinate system in the rotated coordinate system, so R_{3,3} is the component of the Z axis in the Z direction in the rotated coordinate system, that is, cos(θ). So: cos(θ)=R 3,3

[0097] The present application preferably uses other elements of the rotation matrix to calculate the ratio of sin(θ) to cos(ψ). A more stable method is to use the ratio of R_{3,1} and R_{3,2} to cos(ψ), because:

[0098]

[0099] Finally, we can use the atan2 function to calculate θ:

[0100]

[0101] Among them, the atan2 function is used to calculate the inverse tangent and consider the quadrant information to return the correct angle. ψ represents the yaw angle. When the initial model of the discharge arm is horizontal, cos(ψ) = 1; if the initial model of the discharge arm is not horizontal, cos(ψ) is used to normalize R_{3,1} and R_{3,2} to ensure that when the rotation around the Z axis is large (that is, when cos(ψ) is close to zero), a reasonable pitch angle θ can still be calculated, such as Figure 8 shown.

[0102] In one embodiment of step S40 of the present invention, obtaining the relative position and posture of the end of the discharge arm includes:

[0103] Step S41, obtain the base of the discharge arm, the rotating joint rotate and the end end provided with the discharge port; obtain the relative position and posture of the end end of the discharge arm by the following formula:

[0104]

[0105]

[0106] in, is the relative position of the end of the discharge arm relative to the base; is the relative position of the end of the discharge arm relative to the rotary joint, is the translation vector of the arm tip relative to the base; It is the rotation vector of the end of the discharge arm relative to the rotation joint relative to the Y axis.

[0107] It can be understood that, taking the discharge arm with only one rotating joint as an example, the base is base, the rotating joint is rotate, and the end is end. and are all determined by the mechanical parameters (kinematic parameters). Represents the posture of end relative to rotate.

[0108] Furthermore, if the discharge arm has multiple rotation joints,

[0109] like Fig. 9 In an embodiment of step S40 of the present invention, the position of the discharge port is determined according to the relative posture, the pitch angle of the discharge arm and a preset kinematic model, and further includes:

[0110] Step S42, calculating the first position of the end of the discharge arm in the discharge arm coordinate system according to the pitch angle of the discharge arm, and the length parameters, connection mode and motion range between the rotation joints in the kinematic model;

[0111] Step S43: According to the relative posture, the first position is converted into a coordinate system with the reference position as the origin to obtain the discharge port position.

[0112] The kinematic model describes the length, connection mode and range of motion of the joints (rotational joints, etc.) of the discharge arm. Apply the DH parameter method or similar methods: The Denavit-Hartenberg (DH) parameter method or other similar methods can be used to calculate the three-dimensional coordinates (i.e., the first position) of the end of the discharge arm in the discharge arm coordinate system according to the pitch angle of the discharge arm and the length and connection mode of each joint.

[0113] The position of the end of the discharge arm in the discharge arm coordinate system calculated in step S42 is converted to a global coordinate system or a world coordinate system with the reference position as the origin, so as to obtain the position of the discharge port in the global coordinate system. The relative posture can describe the relative position and posture between the discharge arm coordinate system and the global coordinate system, and is represented by a homogeneous transformation matrix. Using the homogeneous transformation matrix in the relative posture, the position (first position) of the end of the discharge arm in the discharge arm coordinate system is converted to the global coordinate system. This is usually done by matrix multiplication. The three-dimensional coordinates obtained after the transformation are the position of the discharge port in the global coordinate system (i.e., the discharge port position).

[0114] like Fig.10 In a further embodiment of the present invention, the method for controlling the discharge of earthmoving equipment further includes:

[0115] Step S51, obtaining a point cloud of a material pile in real time;

[0116] Step S52, acquiring characteristic parameters of the pile according to the pile point cloud, wherein the characteristic parameters include the pile height;

[0117] Step S53, controlling the discharging arm to adjust the pitch angle of the discharging arm so that the position of the discharging port always maintains a preset discharging distance from the height of the material pile;

[0118] Step S54: when the height parameter is greater than a preset height threshold, the discharge arm is controlled to rotate along the Z axis to the next target area for discharge.

[0119] Similarly, use some kind of sensor (such as LiDAR, depth camera, etc.) to scan the pile in real time and obtain its point cloud data. Set up and calibrate the sensor to ensure that the shape and height of the pile can be accurately captured. Use point cloud processing algorithms (such as clustering, fitting, etc.) to analyze the point cloud data. Determine the highest point of the pile as the characteristic parameter of the pile.

[0120] According to the height of the material pile, adjust the pitch angle of the discharge arm to ensure that the position of the discharge port and the height of the material pile maintain the preset discharge distance. For example: set a preset discharge distance, which is the safe distance from the discharge port to the top of the material pile. According to the height of the material pile and the current position of the discharge arm, calculate the pitch angle that needs to be adjusted. Control the pitch mechanism of the discharge arm so that the discharge arm reaches the calculated pitch angle. When the height of the material pile exceeds a certain threshold, in order to avoid excessive accumulation or overflow, control the discharge arm to move to a new area for discharge. Determine the position of the next target area and calculate the angle that the discharge arm needs to rotate along the Z axis. Control the motion mechanism of the discharge arm to rotate along the Z axis to the new target area and continue the discharge operation.

[0121] It is understandable that real-time acquisition of the material pile point cloud data can quickly and accurately determine the height and shape of the material pile, thereby optimizing the discharge strategy. By adjusting the pitch angle of the discharge arm, the discharge port and the material pile are kept at the preset discharge distance, avoiding material waste or uneven accumulation due to improper distance. When the height of the material pile exceeds the preset threshold, the discharge arm is automatically adjusted to a new area for discharge, avoiding excessive accumulation and overflow, and improving discharge efficiency.

[0122] It should be understood that the specific features, operations and details described hereinabove with respect to the method of the present application may also be similarly applied to the device of the present application, or vice versa. In addition, each step of the method of the present application described above may be performed by the corresponding components or units of the device of the present application.

[0123] It should be understood that each module / unit of the device of the present application can be implemented in whole or in part by software, hardware, firmware or a combination thereof. Each module / unit can be embedded in the processor of the electronic device or independent of the processor in the form of hardware or firmware, or can be stored in the memory of the electronic device in the form of software for the processor to call to perform the operation of each module / unit. Each module / unit can be implemented as an independent component or module, or two or more modules / units can be implemented as a single component or module.

[0124] The present application provides an electronic device, which includes a processor and a memory storing computer program instructions. When the processor executes the computer program instructions, each step of the above-mentioned method for monitoring the state of material discharge of a soil dumper is implemented. The electronic device can be broadly a server, a terminal, or any other electronic device with necessary computing and / or processing capabilities.

[0125] In one embodiment, the electronic device may include a processor, a memory, a network interface, a communication interface, etc. connected through a system bus. The processor of the electronic device may be used to provide necessary computing, processing and / or control capabilities. The memory of the electronic device may include a non-volatile storage medium and an internal memory. The non-volatile storage medium may store an operating system, a computer program, etc. The internal memory may provide an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The network interface and the communication interface of the electronic device may be used to connect and communicate with external devices through a network. When the computer program is executed by the processor, the steps of the method of the present application are executed.

[0126] The above-mentioned electronic device, because the processor it includes implements the various steps of the above-mentioned method for monitoring the discharge state of the earthmoving machine when executing computer program instructions, can also realize automatic monitoring of the earthmoving operation state, and the accuracy and efficiency of monitoring the discharge state of the earthmoving machine are high.

[0127] In addition, the present application provides a computer-readable storage medium on which computer program instructions are stored. When the computer program instructions are executed by a processor, the above-mentioned method for monitoring the material discharging status of a soil discharging machine is implemented.

[0128] The above-mentioned computer-readable storage medium implements the various steps of the above-mentioned method for monitoring the discharge state of the earthmoving machine when the computer program instructions stored therein are executed. Similarly, automatic monitoring of the earthmoving operation status can be realized, and the accuracy and efficiency of monitoring the discharge state of the earthmoving machine are high.

[0129] Those skilled in the art will appreciate that the method steps of the present application can be completed by instructing related hardware such as electronic devices or processors through a computer program, and the computer program can be stored in a non-temporary computer-readable storage medium, and the computer program causes the steps of the present application to be executed when it is executed. Depending on the circumstances, any reference to memory, storage or other media herein may include non-volatile or volatile memory. Examples of non-volatile memory include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), flash memory, magnetic tape, floppy disk, magneto-optical data storage device, optical data storage device, hard disk, solid state disk, etc. Examples of volatile memory include random access memory (RAM), external cache memory, etc.

[0130] The various technical features described above can be combined arbitrarily. Although all possible combinations of these technical features are not described, any combination of these technical features should be considered to be covered by this specification as long as there is no contradiction in such combination.

[0131] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit it. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein by equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A method for controlling the discharge of earthmoving equipment, characterized in that: include: Get the global point cloud including the discharge arm; According to the relative position parameters of the discharge arm in the coordinate system, the target point cloud of the discharge arm in the real-time state is cut out from the global point cloud; Performing point cloud fitting and / or point cloud registration in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm; The relative posture of the end of the discharge arm is obtained, and the position of the discharge port is determined according to the relative posture, the pitch angle of the discharge arm and a preset kinematic model.

2. The method for controlling the discharge of earthmoving equipment according to claim 1, characterized in that: The method of cutting out the target point cloud of the discharge arm in the real-time state from the global point cloud according to the relative position parameter of the discharge arm in the coordinate system comprises: Taking the image acquisition device as a reference position, setting the region of interest according to the relative position of the discharge arm relative to the reference position; Cutting out a discharge arm target point cloud from the global point cloud according to the region of interest; At least one of the quantity, size parameters, and morphological features of the target point cloud of the discharge arm is obtained to verify the target point cloud of the discharge arm.

3. The method for controlling the discharge of earthmoving equipment according to claim 2, characterized in that: The method of cutting the target point cloud of the discharge arm in the real-time state from the global point cloud according to the relative position parameter of the discharge arm in the coordinate system also includes: Prioritize obtaining the quantity of the target point cloud of the discharge arm and compare it with the preset quantity threshold; When the number of target point clouds of the discharge arm is less than or equal to the preset number threshold, the verification fails; When the number of the target point cloud of the discharge arm is greater than a preset number threshold, the similarity between the morphological features of the target point cloud of the discharge arm and the preset reference morphology is obtained; When the similarity is less than or equal to the preset similarity threshold, the verification fails, otherwise the verification succeeds.

4. The method for controlling the discharge of earthmoving equipment according to claim 1, characterized in that: Performing point cloud fitting and / or point cloud registration in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm includes: Performing plane fitting or straight line fitting on the bottom of the target point cloud of the discharge arm to obtain a first plane or a first straight line; According to the direction vector of the first plane or the first straight line, the included angle between the direction vector and a preset reference horizontal vector is calculated to obtain the pitch angle of the discharge arm.

5. The method for controlling the discharge of earthmoving equipment according to claim 2, characterized in that: Performing point cloud fitting and / or point cloud registration in the target point cloud of the discharge arm to obtain the pitch angle of the discharge arm includes: The discharge arm target point cloud and the preset model reference point cloud are registered to iteratively find the transformation matrix of corresponding points between the discharge arm target point cloud and the model reference point cloud; An Euler angle representation is calculated based on the transformation matrix, and a pitch angle is extracted from the Euler angle.

6. The method for controlling the discharge of earthmoving equipment according to claim 5, characterized in that: The registering the discharge arm target point cloud and the preset model reference point cloud to iteratively find the transformation matrix of corresponding points between the discharge arm target point cloud and the model reference point cloud includes: Projecting the discharge arm target point cloud and the preset model reference point cloud onto a two-dimensional plane in the same preset direction; On a two-dimensional plane, for each point in the target point cloud of the discharge arm, find its nearest point in the reference point cloud of the model as a corresponding point; Based on the corresponding points found, an initial transformation matrix can be used to preliminarily align the target point cloud of the discharge arm to the reference point cloud of the model; The initial transformation matrix is ​​continuously updated by an iterative method to find corresponding points again in each iteration, so that the average distance between the corresponding points between the target point cloud of the discharge arm and the reference point cloud of the model gradually converges; When the average distance reaches a convergence threshold or reaches a maximum number of iterations, the iteration is stopped and the final transformation matrix is ​​output.

7. The method for controlling the discharge of earthmoving equipment according to claim 5, characterized in that: The step of calculating the Euler angle representation according to the transformation matrix and extracting the pitch angle from the Euler angle comprises: The transformation matrix T is a 4-by-4 matrix, and the rotation matrix R is extracted from the transformation matrix T; In the rotation order of ZYX, the pitch angle θ is calculated by the following formula; atan2 is the inverse tangent function, cos is the cosine function, ψ is the yaw angle in the Euler angle, R n1,n2 and R n1,n3 are elements in the same column of the rotation matrix R; the initial model of the discharge arm is horizontally placed without yaw angle rotation, cos(ψ)=1.

8. The method for controlling the discharge of earthmoving equipment according to claim 5, characterized in that: Get the relative position of the end of the discharge arm, including: Get the base of the discharge arm, the rotating joint rotate, and the end with the discharge port end; get the relative position of the end of the discharge arm by the following formula: Among them, the is the relative position of the end of the discharge arm relative to the base; is the relative position of the end of the discharge arm relative to the rotary joint, is the translation vector of the arm end relative to the base; It is the rotation vector of the end of the discharge arm relative to the rotary joint relative to the Y axis.

9. The method for controlling the discharge of earthmoving equipment according to claim 7, characterized in that: Determining the position of the discharge port according to the relative posture, the pitch angle of the discharge arm and a preset kinematic model includes: Calculate the first position of the end of the discharge arm in the discharge arm coordinate system according to the pitch angle of the discharge arm, and the length parameters, connection mode and motion range between the rotation joints in the kinematic model; According to the relative posture, the first position is converted into a coordinate system with the reference position as the origin to obtain a discharge port position.

10. The method for controlling the discharging of a soil discharging machine according to any one of claims 1 to 9, characterized in that: The soil dumper discharging control method further comprises: Obtain the point cloud of the material pile in real time; Acquire characteristic parameters of the pile according to the pile point cloud, wherein the characteristic parameters include the pile height; Controlling the discharging arm to adjust the pitch angle of the discharging arm so that the position of the discharging port always maintains a preset discharging distance from the height of the material pile; When the height parameter is greater than a preset height threshold, the discharge arm is controlled to rotate along the Z axis to the next target area for discharge.

Images