Bucket landing point prediction method fusing point cloud information

Abstract

The invention is suitable for the technical field of excavators, and provides a bucket landing point prediction method fusing point cloud information. According to the method, the efficient and accurate landing point prediction of the excavator in various and complex working environments is realized by combining the efficient and reliable distance measurement capability of the IMU and the advantages of abundant ground point information capture and small environmental influence of the laser radar in the complex environment. The method shows high precision under different poses and working conditions, the error range is small, and the stability and reliability of excavator operation are ensured. Specifically, the overlap ratio of the predicted landing point of the bucket and the actual landing point area reaches 85% or above, even if multiple variables and uncertain factors are faced, the method can still keep high prediction accuracy, and the requirement of intelligent operation for accuracy is fully met. The high-precision prediction not only improves the working efficiency of the excavator, but also reduces the damage to the terrain, and has important practical application value.

Description

Technical Field

[0001] The invention belongs to the technical field of excavators, and in particular relates to a method for predicting a bucket landing point by integrating point cloud information. Background Art

[0002] As an emerging technology, remote unmanned excavators can reduce the burden on operators, improve safety and work efficiency, but they still face major challenges in the remote control process. The current remote control driving technology mainly relies on transmitting on-site images to computer monitors. However, this method of excavating the three-dimensional environment of the site through a two-dimensional display significantly weakens the sense of reality and space of the working environment, making it difficult for remote-operated drivers to get a good immersive experience when facing a flat screen that lacks three-dimensional and spatial sense. In this case, the perception of details on the working surface is far less clear and three-dimensional than when observing on site. Specifically, the driver cannot accurately judge the distance between the bucket and the fuselage, the height and posture of the bucket relative to the ground, which makes it difficult to accurately locate the excavation point during the excavation operation, resulting in a low "full bucket rate". At the same time, due to the inability to accurately judge the unloading point, work efficiency will also be reduced, such as the inability to reasonably allocate space when loading, resulting in a waste of resources. In addition, it is difficult for the driver to see the ups and downs of the ground, which affects the effect of leveling the ground.

[0003] The existing technology only simply splices the image information collected by the camera, and fails to effectively solve the problem of deep space environment perception, which not only increases the difficulty of operation and the risk of accidents, but also reduces the work efficiency and accuracy. Therefore, the prediction of the landing point of the excavator bucket has become a technical problem that needs to be solved urgently to improve the above problems. To this end, the present invention proposes a bucket landing point prediction method that integrates point cloud information. Summary of the invention

[0004] The purpose of the present invention is to provide a bucket landing point prediction method integrating point cloud information, aiming to solve the problems raised in the above-mentioned background technology.

[0005] The purpose of the present invention is achieved through the following technical solutions:

[0006] A method for predicting a bucket landing point by integrating point cloud information comprises the following steps:

[0007] First, the angle information of the boom, arm and bucket is obtained through the IMU, and the IMU data is preprocessed using the Kalman filter. Then, the bucket posture information is calculated based on the preprocessed data and the kinematic model of the working device. Next, the point cloud information of the working device's surrounding environment is obtained through the lidar, and the dynamic area point cloud is extracted based on the known bucket posture. The extracted point cloud data is then reconstructed for elevation contours. Finally, the key point distance is calculated to obtain the bucket landing point information.

[0008] Furthermore, the DH parameter method is used to establish the kinematic model of the working device, and the specific process is as follows:

[0009] First, define the boom coordinate system as O1Y1X1Z1, the arm coordinate system as O2Y2X2Z2, the bucket coordinate system as O3Y3X3Z3, and the bucket tip coordinate system as O4Y4X4Z4, and set the boom coordinate system as the base coordinate; the boom rod length is l1, the arm rod length is l2, the bucket rod length is l3, the boom joint angle is θ1, the arm joint angle is θ2, the bucket joint angle is θ3, and the bucket tip posture angle is ξ;

[0010] Then the joint angle information [θ1, θ2, θ3] of the boom, arm and bucket is converted to the position and attitude angle information of the bucket tip [x, y, ξ], where (x, y) is the coordinate of the bucket tip in the two-dimensional plane XOY, ξ is the attitude angle of the bucket tip, and the homogeneous transformation matrix of the adjacent coordinate system is for:

[0011]

[0012] In the formula, c i represents cosθ i ,s i represents sinθ i , l i Indicates the corresponding rod length;

[0013] Then the transformation matrix of the adjacent joint coordinate systems of the working device is:

[0014]

[0015] In the formula, Represents the transformation matrix from the arm coordinate system O2Y2X2Z2 to the polar coordinate system O1Y1X1Z1, Represents the transformation matrix from the bucket coordinate system O3Y3X3Z3 to the arm coordinate system O2Y2X2Z2, represents the transformation matrix from bucket tip coordinate system O4Y4X4Z4 to bucket coordinate system O3Y3X3Z3, c1 represents cosθ1, s1 represents sinθ1, c2 represents cosθ2, s2 represents sinθ2, c3 represents cosθ3, and s3 represents sinθ3;

[0016] Transformation matrix from bucket tip coordinate system O4Y4X4Z4 to polar coordinate system O1Y1X1Z1 for:

[0017]

[0018] In the formula, c 12represents cos(θ1+θ2), s 12 represents sin(θ1+θ2), c 123 represents cos(θ1+θ2+θ3), s 123 represents sin(θ1+θ2+θ3);

[0019] The bucket tip attitude angle ξ is:

[0020] ξ=θ1+θ2+θ3;

[0021] The final forward kinematic model of the working device is:

[0022]

[0023] Where (x, y, z) is the position coordinate of the bucket tip in the three-dimensional space coordinate system.

[0024] Furthermore, the specific process of using the Kalman filter to preprocess the IMU data is as follows:

[0025] Transfer the target from the previous moment to the current moment, expressed as:

[0026] X n =A n X n-1 +B n u n ;

[0027] In the formula, X n represents the state of the system at time n, A n represents the transfer matrix of the system at time n, X n-1 represents the state of the system at time n-1, B n represents the mapping relationship between system input and system state at time n, u n represents the input of the system at time n;

[0028] The observation equation of the system is as follows:

[0029] z n =H n X n ;

[0030] In the formula, z n represents the observed value of the system at time n, H n Represents the observation matrix at time n;

[0031] The covariance matrix is ​​introduced to represent the uncertainty of the entire system. The covariance matrix of the system satisfies the following relationship:

[0032]

[0033] Where Pn Represents the covariance matrix of the entire system at time n, P n-1 represents the covariance matrix of the entire system at time n-1, Q n represents the system input u at time n n The covariance matrix corresponding to the uncertainty, T represents the matrix transpose operation;

[0034] Using the observation information of the system state n Update the state and covariance of the system at the current moment. The formula is:

[0035]

[0036] Where I is the unit matrix, denote the a posteriori and a priori estimates of the system state, respectively. Represents the covariance matrix of the posterior probability and prior probability of the system state, K n is the Kalman gain, and is calculated as:

[0037]

[0038] In the formula, H represents the observation matrix at the current moment, R n The covariance matrix that represents the uncertainty of the observations.

[0039] Furthermore, the specific process of the dynamic area point cloud extraction step is as follows:

[0040] Firstly, the Patchwork++ algorithm is used to filter out redundant point cloud information other than the ground, including: removing reflection noise from laser point cloud data; establishing a concentric belt model to divide the point cloud into multiple regions of different sizes; fitting a vertical plane for each region; fitting the ground for each region; based on the ground segmentation results, adaptive ground likelihood estimation is used to calculate ground parameters; and temporary ground recovery technology is used to restore the under-segmented ground area to the ground.

[0041] Then, the ground point cloud in the vertical dynamic area corresponding to the bucket is extracted, specifically including: combining the IMU data and the kinematic model of the working device to obtain the spatial position coordinates (x, y, z) of the bucket tip; vertically projecting the spatial position coordinates (x, y, z) of the bucket tip to the ground to obtain the coordinates (x, y) of the bucket tip in the two-dimensional plane; taking the coordinates (x, y) as the centroid and combining the geometric dimensions of the bucket to extract the point cloud in the square area.

[0042] Furthermore, the specific process of the elevation profile reconstruction step is as follows:

[0043] The Kriging interpolation method is used to convert the ground point cloud data in the vertical dynamic area corresponding to the bucket into an elevation map; assuming that Z(x,y) is a steady-state random process, its discrete value is Z(x i ,y i ), we want to find the interpolation point (x p ,y p ), the Kriging solution is:

[0044]

[0045] In the formula, Z(x p ,y p ) means that at the interpolation point (x p ,y p ) at the height value; Z(x i ,y i ) is a known point (x i ,y i ) at the elevation value; β i is the pending Kriging weight, satisfying And the weighted discrete point variance satisfies Where Var represents variance, n represents all weights, and i represents the i-th weight; then the interpolation point elevation Z(x p ,y p ) is solved as:

[0046]

[0047] Where M is the elevation value Z(x p ,y p )’s weighted median; [σ P1 ,σ P2 ,…,σ Pn ] represents the weight matrix, σ P1 ,σ P2 ,…,σ Pn is the interpolation point (x p ,y p ) and the known point (x i ,y i ) between the semivariances; represents the semivariance matrix, which contains the semivariance values ​​between all known points; Contains the height values ​​of all known points.

[0048] Furthermore, the specific process of the key point distance calculation step is as follows:

[0049] The elevation map of the bucket corresponding area is obtained by reconstructing the elevation contour, and then the plane coordinate points (x, y) in the area are traversed to extract the maximum ground height Z corresponding to the plane coordinate point (x, y)max (x, y), assuming that the spatial coordinates of the bucket are Z(x, y), the distance D between the spatial coordinates of the bucket and the highest point on the ground in the corresponding area can be obtained using the distance formula:

[0050] D=Z(x,y)-Z max (x,y).

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

[0052] The bucket landing point prediction method that integrates point cloud information proposed in the present invention achieves efficient and accurate landing point prediction of the excavator in various and complex working environments by cleverly combining the efficiency and reliability of the IMU (inertial measurement unit) to measure distance, and the advantage of the laser radar in capturing rich ground point information in complex environments and being less affected by the environment. This method shows high precision and a small error range in different postures and working conditions, ensuring the stability and reliability of the excavator operation. Specifically, the overlap between the predicted bucket landing point and the actual landing point area reaches more than 85%. Even in the face of multiple variables and uncertain factors, the method can still maintain high prediction accuracy, fully meeting the accuracy requirements of intelligent operations. This high-precision prediction not only improves the operating efficiency of the excavator, but also effectively reduces the damage to the terrain, and has important practical application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 The present invention is a flow chart of the method.

[0054] Figure 2 It is the DH coordinate system of the working device.

[0055] Figure 3 This is the flow chart of the Patchwork++ algorithm.

[0056] Figure 4 Point cloud comparison diagram, where (a) is the original point cloud, (b) is the ground point cloud, (c) is the working device point cloud, and (d) is the dynamic area point cloud.

[0057] Figure 5 It is the reconstruction of elevation contour, where (a) is the elevation information and (b) is the reconstruction of dynamic area elevation contour.

[0058] Figure 6 Calculate the distance between key points.

[0059] Figure 7 It is a simulation scene, where (a) shows three postures of the excavator working device and (b) shows three terrains. DETAILED DESCRIPTION

[0060] In order to have a clearer understanding of the technical features, purposes and beneficial effects of the present invention, the technical solution of the present invention is now described in detail below, but it should not be construed as limiting the applicable scope of the present invention.

[0061] The specific implementation of the present invention is described in detail below in conjunction with specific embodiments.

[0062] An embodiment of the present invention provides a method for predicting a bucket landing point by integrating point cloud information, comprising the following steps:

[0063] First, the angle information of each joint (boom, dipper rod and bucket) is obtained through IMU, and the IMU data is preprocessed using Kalman filtering. Then, based on the preprocessed data and the kinematic model of the working device, the bucket posture information is calculated. Then, the point cloud information of the working device's surrounding environment is obtained through LiDAR. Based on the known bucket posture, the dynamic area point cloud is extracted. Then, the extracted point cloud data is reconstructed for elevation contour. Finally, the key point distance is calculated to obtain the bucket landing point information. The flowchart of this method is as follows: Figure 1 shown.

[0064] 1. The basic work of this method is divided into two parts: establishment of the kinematic model of the working device and preprocessing of IMU data.

[0065] 1) Use the DH parameter method to establish the kinematic model of the working device, and its coordinate system is as follows Figure 2 As shown, the specific process is as follows:

[0066] First, define the boom coordinate system as O1Y1X1Z1, the arm coordinate system as O2Y2X2Z2, the bucket coordinate system as O3Y3X3Z3, and the bucket tip coordinate system as O4Y4X4Z4, and set the boom coordinate system as the base coordinate; the boom rod length is l1, the arm rod length is l2, the bucket rod length is l3, the boom joint angle is θ1, the arm joint angle is θ2, the bucket joint angle is θ3, and the bucket tip posture angle is ξ;

[0067] The forward kinematics model of the working device is the transformation model from joint space to posture. It can transform the joint angle information [θ1, θ2, θ3] of the boom, arm and bucket into the position and attitude angle information [x, y, ξ] of the bucket tip, where (x, y) is the coordinate of the bucket tip in the two-dimensional plane XOY, ξ is the attitude angle of the bucket tip, and the homogeneous transformation matrix of the adjacent coordinate system is for:

[0068]

[0069] In the formula, c i represents cosθ i ,s i represents sinθi , l i Indicates the corresponding rod length;

[0070] Then the transformation matrix of the adjacent joint coordinate system of the working device is:

[0071]

[0072] In the formula, Represents the transformation matrix from the arm coordinate system O2Y2X2Z2 to the polar coordinate system O1Y1X1Z1, Represents the transformation matrix from the bucket coordinate system O3Y3X3Z3 to the arm coordinate system O2Y2X2Z2, represents the transformation matrix from bucket tip coordinate system O4Y4X4Z4 to bucket coordinate system O3Y3X3Z3, c1 represents cosθ1, s1 represents sinθ1, c2 represents cosθ2, s2 represents sinθ2, c3 represents cosθ3, and s3 represents sinθ3;

[0073] According to the above formula, the transformation matrix from bucket tip coordinate system O4Y4X4Z4 to polar coordinate system O1Y1X1Z1 for:

[0074]

[0075] In the formula, c 12 represents cos(θ1+θ2), s 12 represents sin(θ1+θ2), c 123 represents cos(θ1+θ2+θ3), s 123 represents sin(θ1+θ2+θ3);

[0076] The bucket tip attitude angle ξ is:

[0077] ξ=θ1+θ2+θ3;

[0078] The final forward kinematic model of the working device is:

[0079]

[0080] Where (x, y, z) is the position coordinate of the bucket tip in the three-dimensional space coordinate system.

[0081] 2) IMU data preprocessing;

[0082] When the present invention uses Kalman filtering to estimate the state of the target, the state of the target is only related to the state at the previous moment and the observation at the current moment. This calculation method can greatly improve the calculation efficiency and meet the real-time calculation of the vehicle posture. The specific process of using Kalman filtering to preprocess IMU data is as follows:

[0083] Transfer the target from the previous moment to the current moment, expressed as:

[0084] X n =A n X n-1 +B n u n ;

[0085] Where, X n represents the state of the system at time n, A n represents the transfer matrix of the system at time n, X n-1 represents the state of the system at time n-1, B n represents the mapping relationship between system input and system state at time n, u n represents the input of the system at time n;

[0086] The observation equation of the system is as follows. We only need to know the initial state of the target and the input at each moment to use the algorithm to obtain the state information of the target at any moment.

[0087] z n =H n X n ;

[0088] In the formula, z n represents the observed value of the system at time n, H n Represents the observation matrix at time n;

[0089] When estimating the state of the target, it will inevitably be affected by various noises. For this reason, it is necessary to introduce a covariance matrix to represent the uncertainty of the entire system. The covariance matrix of the system satisfies the following relationship:

[0090]

[0091] Where P n Represents the covariance matrix of the entire system at time n, P n-1 represents the covariance matrix of the entire system at time n-1, Q n represents the system input u at time n n The covariance matrix corresponding to the uncertainty of , T represents the matrix transpose operation.

[0092] Using the observation information of the system state nUpdate the state and covariance of the system at the current moment. The formula is:

[0093]

[0094] Where I is the unit matrix, denote the a posteriori and a priori estimates of the system state, respectively. Represents the covariance matrix of the posterior probability and prior probability of the system state, K n is the Kalman gain, and is calculated as:

[0095]

[0096] In the formula, H represents the observation matrix at the current moment, R n The covariance matrix that represents the uncertainty of the observations.

[0097] 2. The main work of this method is divided into three parts: dynamic area point cloud extraction, elevation contour reconstruction, and key point distance calculation.

[0098] 1) Dynamic area point cloud extraction;

[0099] The point cloud obtained by LiDAR not only contains rich ground point information, but also includes a large number of dynamic object point clouds such as the excavator's working device and bucket. Therefore, in order to accurately analyze the ground information, it is necessary to process these data and extract the ground points. At the same time, it is necessary to remove the point clouds of other dynamic objects in order to obtain clearer and more accurate terrain data, thereby obtaining the point cloud of the dynamic area. The specific process is as follows:

[0100] a. Use the Patchwork++ algorithm to filter out redundant point cloud information other than the ground, and maintain a high ground point recognition accuracy in complex terrain. The algorithm process is as follows: Figure 3 As shown, specifically including:

[0101] First, the laser point cloud data is processed by reflection noise removal (RNR). Specifically, this step removes point cloud data that is close to the laser radar, and at the same time filters out noise points whose height is lower than a preset threshold and whose intensity is lower than a certain threshold. This process helps to improve the accuracy of subsequent data processing and ensure the reliability and accuracy of the final result.

[0102] Next, a concentric zone model (CZM) is established to divide the point cloud into multiple regions of different sizes. The model is based on the center point and gradually expands outward to form a series of concentric zones, which can reasonably process the point cloud data in different regions. This division method helps to better analyze and process point cloud data and improve the efficiency and accuracy of the overall algorithm.

[0103] On this basis, regional vertical plane fitting (R-VPF) is performed. This method improves the accuracy of plane fitting by identifying and removing vertical points that may interfere with the fitting plane.

[0104] Then, ground fitting (R-GPF) is performed on each small area. In this way, the shape and height of the ground can be accurately fitted in each small area, thus reflecting the terrain characteristics more accurately.

[0105] In order to judge the actual ground points more robustly, adaptive ground likelihood estimation (A-GLE) is used to calculate appropriate ground parameters based on the previous ground segmentation results to more accurately reflect the actual ground conditions.

[0106] In order to solve the problem of partial ground under-segmentation, the temporary ground restoration (TGR) method is used to further restore the under-segmented ground area to the ground. Through this method, the ground points missed in the previous segmentation process can be corrected, making the final ground model more complete and accurate.

[0107] Finally, in order to facilitate subsequent operations, the processed point cloud data is converted from polar coordinates back to the rectangular coordinate system. After the above steps, the segmented ground point cloud and non-ground point cloud are obtained.

[0108] b. After filtering out the redundant point cloud information except the ground, it is also necessary to extract the ground point cloud in the vertical dynamic area corresponding to the bucket. The original point cloud is as follows Figure 4 As shown in (a), the distribution of the extracted ground point cloud and working device point cloud is as follows: Figure 4 As shown in (b) and (c). The spatial position coordinates (x, y, z) of the bucket tip can be obtained by combining the IMU data and the kinematic model of the working device; the spatial position coordinates (x, y, z) of the bucket tip are projected vertically onto the ground to obtain the coordinates (x, y) of the bucket tip in the two-dimensional plane. Taking this point as the centroid, the point cloud in the square area is extracted in combination with the geometric dimensions of the bucket, as shown in Figure 4 As shown in (d).

[0109] 2) Elevation profile reconstruction;

[0110] In order to facilitate the driver's depth perception of the bucket posture and accurately locate the excavation point when the remote-controlled excavator is operating, this method uses the Kriging interpolation method to convert the extracted point cloud data in the vertical dynamic area corresponding to the bucket into an elevation map.

[0111] Assume that Z(x,y) is a steady-state random process with discrete values ​​Z(x i ,y i ), we want to find the interpolation point (x p ,y p), the Kriging solution is:

[0112]

[0113] In the formula, Z(x p ,y p ) means that at the interpolation point (x p ,y p ) at the height value; Z(x i ,y i ) is a known point (x i ,y i ) at the elevation value; β i To be determined Kr i ging rights, satisfaction And the weighted discrete point variance satisfies Where Var represents variance, n represents all weights, and i represents the i-th weight; then the interpolation point elevation Z(x p ,y p ) is solved as:

[0114]

[0115] Where M is the elevation value Z(x p ,y p )’s weighted median; [σ P1 ,σ P2 ,…,σ Pn ] represents the weight matrix, σ P1 ,σ P2 ,…,σ Pn is the interpolation point (x p ,y p ) and the known point (x i ,y i ) between the semivariances; represents the semivariance matrix, which contains the semivariance values ​​between all known points; Contains the height values ​​of all known points.

[0116] Elevation information such as Figure 5 As shown in (a), the dynamic area elevation contour reconstruction is obtained as follows Figure 5 As shown in (b).

[0117] 3) Key point distance calculation;

[0118] In order to further facilitate the driver to perceive the spatial position of the bucket and its relationship with the ground height, it is necessary to calculate the distance between the excavator bucket and the ground in its corresponding vertical area in real time. This real-time calculation can help the driver more accurately control the digging depth and operation accuracy of the bucket during operation, thereby effectively avoiding misoperation and improving operation efficiency and safety.

[0119] Through the above-mentioned elevation contour reconstruction, the elevation map of the area corresponding to the bucket can be obtained, and then the plane coordinate points (x, y) in the area can be traversed to extract the maximum ground height Z corresponding to the plane coordinate point (x, y) max (x, y), assuming that the spatial coordinates of the bucket are Z(x, y), the distance D between the spatial coordinates of the bucket and the highest point on the ground in the corresponding area can be obtained using the distance formula, such as Figure 6 As shown;

[0120] D=Z(x,y)-Z max (x,y).

[0121] Example 1. In order to comprehensively evaluate the performance of the bucket landing point prediction method that integrates point cloud information proposed in the present invention, three key evaluation indicators are designed. The first evaluation indicator is the vertical distance from the excavator bucket to the corresponding ground area. This is a direct standard for measuring the accuracy of the bucket position. By accurately calculating the error between the bucket and the actual distance from the ground under different working conditions, the accuracy of the bucket landing point method can be effectively evaluated. The second evaluation indicator is the overlap of the bucket landing point area. This indicator reflects the accuracy of the prediction method in predicting the bucket landing point under different operating conditions. The higher the overlap, the stronger the adaptability and robustness of the method in a changing operating environment. The third evaluation indicator is the time required for landing point prediction, which directly affects the real-time and practicality of the method. In intelligent excavation operations, the prediction of the bucket landing point requires not only high precision, but also needs to be completed within a limited time to ensure the response speed of the method in actual operations. We designed a scheme for conducting experiments in three different simulation environments (such as Figure 7 The key indicators of the experimental results are listed in Table 1.

[0122] Table 1 Simulation results

[0123]

[0124] It can be seen from the data in Table 1 that in three different postures, the vertical error obtained by the method does not exceed 60mm (considering that the bucket size is 2650×2050×1960mm), which shows that no matter what posture, the proposed prediction method has high accuracy and can maintain a small error range in a complex working environment, thereby ensuring the stability and reliability of the excavator operation. Further analysis of the accuracy of the bucket landing point shows that under different working conditions, the predicted bucket landing point of the method proposed in the present invention has a coincidence rate of more than 85% with the actual landing point area, which means that even in the presence of multiple variables and uncertain factors, the method can still maintain a high prediction accuracy, fully meeting the accuracy requirements of intelligent operations. This high-precision bucket landing point prediction not only improves the operating efficiency of the excavator, but also effectively reduces the damage to the terrain, so it has important practical application value.

[0125] The above are only preferred embodiments of the present invention. It should be pointed out that, for those skilled in the art, several modifications and improvements can be made without departing from the concept of the present invention. These should also be regarded as the protection scope of the present invention. These will not affect the effect of the implementation of the present invention and the practicality of the patent.

Claims

1. A bucket landing point prediction method integrating point cloud information, characterized in that: The following steps are involved: First, the angle information of the boom, arm and bucket is obtained through the IMU, and the IMU data is preprocessed using the Kalman filter. Then, the bucket posture information is calculated based on the preprocessed data and the kinematic model of the working device. Next, the point cloud information of the working device's surrounding environment is obtained through the lidar, and the dynamic area point cloud is extracted based on the known bucket posture. The extracted point cloud data is then reconstructed for elevation contours. Finally, the key point distance is calculated to obtain the bucket landing point information.

2. The bucket landing point prediction method integrating point cloud information according to claim 1 is characterized in that: The kinematic model of the working device is established using the DH parameter method. The specific process is as follows: First, define the boom coordinate system as O1Y1X1Z1, the arm coordinate system as O2Y2X2Z2, the bucket coordinate system as O3Y3X3Z3, and the bucket tip coordinate system as O4Y4X4Z4, and set the boom coordinate system as the base coordinate; the boom rod length is l1, the arm rod length is l2, the bucket rod length is l3, the boom joint angle is θ1, the arm joint angle is θ2, the bucket joint angle is θ3, and the bucket tip posture angle is ξ; Then the joint angle information [θ1, θ2, θ3] of the boom, arm and bucket is converted to the position and attitude angle information of the bucket tip [x, y, ξ], where (x, y) is the coordinate of the bucket tip in the two-dimensional plane XOY, ξ is the attitude angle of the bucket tip, and the homogeneous transformation matrix of the adjacent coordinate system is for: In the formula, c i represents cosθ i ,s i represents sinθ i , l i Indicates the corresponding rod length; Then the transformation matrix of the adjacent joint coordinate system of the working device is: In the formula, Represents the transformation matrix from the arm coordinate system O2Y2X2Z2 to the polar coordinate system O1Y1X1Z1, Represents the transformation matrix from the bucket coordinate system O3Y3X3Z3 to the arm coordinate system O2Y2X2Z2, represents the transformation matrix from bucket tip coordinate system O4Y4X4Z4 to bucket coordinate system O3Y3X3Z3, c1 represents cosθ1, s1 represents sinθ1, c2 represents cosθ2, s2 represents sinθ2, c3 represents cosθ3, and s3 represents sinθ3; Transformation matrix from bucket tip coordinate system O4Y4X4Z4 to polar coordinate system O1Y1X1Z1 for: In the formula, c 12 represents cos(θ1+θ2), s 12 represents sin(θ1+θ2), c 123 represents cos(θ1+θ2+θ3), s 123 represents sin(θ1+θ2+θ3); The bucket tip attitude angle ξ is: ξ=θ1+θ2+θ3; The final forward kinematic model of the working device is: Where (x, y, z) is the position coordinate of the bucket tip in the three-dimensional space coordinate system.

3. The bucket landing point prediction method integrating point cloud information according to claim 1, characterized in that: The specific process of preprocessing IMU data using the Kalman filter is as follows: Transfer the target from the previous moment to the current moment, expressed as: X n =A n X n-1 +B n u n ; Where, X n represents the state of the system at time n, A n represents the transfer matrix of the system at time n, X n-1 represents the state of the system at time n-1, B n represents the mapping relationship between system input and system state at time n, u n represents the input of the system at time n; The observation equation of the system is as follows: z n =H n X n ; In the formula, z n represents the observed value of the system at time n, H n Represents the observation matrix at time n; The covariance matrix is ​​introduced to represent the uncertainty of the entire system. The covariance matrix of the system satisfies the following relationship: Where P n Represents the covariance matrix of the entire system at time n, P n-1 represents the covariance matrix of the entire system at time n-1, Q n represents the system input u at time n n The covariance matrix corresponding to the uncertainty, T represents the matrix transpose operation; Using the observation information of the system state n Update the state and covariance of the system at the current moment. The formula is: Where I is the unit matrix, denote the a posteriori and a priori estimates of the system state, respectively. Represents the covariance matrix of the posterior probability and prior probability of the system state, K n is the Kalman gain, and is calculated as: In the formula, H represents the observation matrix at the current moment, R n The covariance matrix representing the uncertainty of the observations.

4. The bucket landing point prediction method integrating point cloud information according to claim 1, characterized in that: The specific process of the dynamic area point cloud extraction step is as follows: Firstly, the Patchwork++ algorithm is used to filter out redundant point cloud information other than the ground, including: removing reflection noise from laser point cloud data; establishing a concentric belt model to divide the point cloud into multiple regions of different sizes; fitting a vertical plane for each region; fitting the ground for each region; based on the ground segmentation results, adaptive ground likelihood estimation is used to calculate ground parameters; and temporary ground recovery technology is used to restore the under-segmented ground area to the ground. Then, the ground point cloud in the vertical dynamic area corresponding to the bucket is extracted, specifically including: combining the IMU data and the kinematic model of the working device to obtain the spatial position coordinates (x, y, z) of the bucket tip; vertically projecting the spatial position coordinates (x, y, z) of the bucket tip to the ground to obtain the coordinates (x, y) of the bucket tip in the two-dimensional plane; taking the coordinates (x, y) as the centroid and combining the geometric dimensions of the bucket to extract the point cloud in the square area.

5. The bucket landing point prediction method integrating point cloud information according to claim 1, characterized in that: The specific process of the elevation profile reconstruction step is as follows: The Kriging interpolation method is used to convert the ground point cloud data in the vertical dynamic area corresponding to the bucket into an elevation map; assuming that Z(x,y) is a steady-state random process, its discrete value is Z(x i ,y i ), we want to find the interpolation point (x p ,y p ), the Kriging solution is: In the formula, Z(x p ,y p ) indicates that at the interpolation point (x p ,y p ) at the height value; Z(x i ,y i ) is a known point (x i ,y i ) at the elevation value; β i is the pending Kriging weight, satisfying And the weighted discrete point variance satisfies Where Var represents variance, n represents all weights, and i represents the i-th weight; then the interpolation point elevation Z(x p ,y p ) is solved as: Where M is the elevation value Z(x p ,y p )’s weighted median; [σ P1 ,σ P2 ,…,σ Pn ] represents the weight matrix, σ P1 ,σ P2 ,…,σ Pn is the interpolation point (x p ,y p ) and the known point (x i ,y i ) between the semivariances; represents the semivariance matrix, which contains the semivariance values ​​between all known points; Contains the height values ​​of all known points.

6. The bucket landing point prediction method integrating point cloud information according to claim 1, characterized in that: The specific process of the key point distance calculation step is as follows: The elevation map of the bucket corresponding area is obtained by reconstructing the elevation contour, and then the plane coordinate points (x, y) in the area are traversed to extract the maximum ground height Z corresponding to the plane coordinate point (x, y) max (x, y), assuming that the spatial coordinates of the bucket are Z(x, y), the distance D between the spatial coordinates of the bucket and the highest point on the ground in the corresponding area can be obtained using the distance formula: D=Z(x,y)-Z max (x,y)。

Images