阅读说明：本技术 铲斗三维位姿跟踪方法、装置、系统及挖掘机 (Bucket three-dimensional pose tracking method, device and system and excavator ) 是由 陈浩 刘明亮 王传宇 于 2021-07-15 设计创作，主要内容包括：本发明提供一种铲斗三维位姿跟踪方法、装置、系统及挖掘机,所述方法包括：根据铲斗宽度值及铲斗与激光雷达的相对运动范围,对第一雷达点云进行直通滤波获取第二雷达点云；基于相机的内参矩阵及激光雷达与相机之间的映射矩阵,将第二雷达点云投影到相机图像中,生成第三雷达点云；根据铲斗的颜色信息,从第三雷达点云中筛选出第四雷达点云；获取第四雷达点云与预先构建的铲斗点云模型之间的相对位姿信息,以根据相对位姿信息对铲斗点云模型进行渲染,生成铲斗分布点云。本发明仅需要将激光雷达和相机安装在驾驶室座舱上,而无需对斗杆和铲斗进行其他类型的传感器监测,也能够在线获取铲斗分布点云,结构简单、便于维护,且不易受外界环境影响。(The invention provides a bucket three-dimensional pose tracking method, a device and a system and an excavator, wherein the method comprises the following steps: performing direct filtering on the first radar point cloud according to the bucket width value and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; based on an internal reference matrix of the camera and a mapping matrix between the laser radar and the camera, projecting the second radar point cloud into the camera image to generate a third radar point cloud; screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket; and obtaining relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud. According to the invention, only the laser radar and the camera are required to be installed on the cab cabin, other types of sensor monitoring on the bucket rod and the bucket are not required, the bucket distribution point cloud can be obtained on line, the structure is simple, the maintenance is convenient, and the influence of the external environment is not easy.)

1. A bucket three-dimensional pose tracking method is characterized by comprising the following steps:

performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; the first radar point cloud is collected by the lidar;

projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud; the camera image is captured by the camera;

screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket;

and obtaining relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud.

2. The bucket three-dimensional pose tracking method according to claim 1, further comprising, before generating a third radar point cloud by projecting the second radar point cloud into a camera image:

representing a world origin of coordinates of the lidar using a first rotation matrix and a first translational vector, and representing a world origin of coordinates of the camera using a second rotation matrix and a second translational vector;

determining a relative rotation matrix between the first rotation matrix and the second rotation matrix and determining a relative translation vector between the first translation vector and the second translation vector;

and determining the mapping matrix according to the relative rotation matrix and the relative translation vector.

3. The bucket three-dimensional pose tracking method according to claim 2, wherein the projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the lidar and the camera to generate a third radar point cloud comprises:

performing pose transformation on the second radar point cloud according to the relative rotation matrix and the relative translation vector to acquire a fifth radar point cloud;

and projecting the fifth radar point cloud to the camera image based on the internal reference matrix to generate the third radar point cloud.

4. The bucket three-dimensional pose tracking method according to claim 1, further comprising, before acquiring the relative pose information between the fourth radar point cloud and the bucket point cloud model constructed in advance:

and scanning the bucket by utilizing a laser radar and a camera, and constructing the bucket point cloud model in advance.

5. The method for tracking the three-dimensional pose of the bucket according to claim 1, wherein the step of screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket comprises the following steps:

and deleting all points which are not matched with the color information of the bucket in the third radar point cloud according to the color information of the bucket to obtain the fourth radar point cloud.

6. The bucket three-dimensional pose tracking method according to claim 1, further comprising, after generating the bucket distribution point cloud: and displaying the bucket distribution point cloud by using a display screen in the cab.

7. A bucket three-dimensional pose tracking device, comprising:

the first processing unit is used for performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; the first radar point cloud is collected by the lidar;

the second processing unit is used for projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud; the camera image is captured by the camera;

the third processing unit is used for screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket;

and the fourth processing unit is used for acquiring relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud.

8. A bucket three-dimensional pose tracking system, comprising: the system comprises a laser radar, a camera, a display screen and an industrial personal computer;

the industrial personal computer at least comprises a bucket three-dimensional pose tracking device according to claim 7.

9. An excavator, characterized by comprising at least a bucket three-dimensional pose tracking system of claim 8;

the laser radar and the camera are horizontally arranged outside the cab of the excavator and face the bucket.

10. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor implements the three-dimensional position and orientation tracking method steps of the bucket according to any one of claims 1 to 6 when executing the computer program.

Technical Field

The invention relates to the technical field of heavy work machinery, in particular to a bucket three-dimensional pose tracking method, device and system and an excavator.

Background

The simulation of the bucket, and the tracking of the bucket tooth tip, may be used for verification of the operation of the excavator.

In the prior art, an excavator construction guide system is described, wherein positioning modules for measuring pose information of a power arm are arranged on a vehicle body and a movable arm of an excavator, and a measuring assembly for measuring the telescopic length of a bucket rod hydraulic cylinder and/or a bucket hydraulic cylinder is arranged on the excavator. The working principle is that measurement information of a positioning module and a measurement assembly is received through a measurement terminal, and pose information of a power arm, a bucket rod and a bucket is calculated; and then, establishing a construction land terrain model by using software, and determining the three-dimensional pose of the bucket simulation model according to the pose information of the bucket.

In addition, in order to realize verification of the excavator operation, a bucket of the excavator is photographed by using a high-speed camera so as to obtain the position movement thereof; and then outputting the moving coordinate position of the bucket tooth in an off-line analysis mode.

In the prior art, in the process of tracking the three-dimensional pose of the bucket, the change of the three-dimensional pose of the bucket is calculated mainly by a position sensor or a high-speed camera and other related sensors and by acquiring the position of the bucket, the length of a hydraulic rod and other information, and the deviation of an actual tracking result is often large because the working environment of an excavator is generally severe; most importantly, the prior art can only output the posture changes of the bucket and the bucket teeth in an off-line mode, and cannot output and feed back the posture changes to an operator in a cab in real time.

Disclosure of Invention

The invention provides a bucket three-dimensional pose tracking method, device and system and an excavator, which are used for solving the defect that the change of the bucket three-dimensional pose cannot be determined in real time in the prior art.

In a first aspect, the invention provides a bucket three-dimensional pose tracking method, which includes:

performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; the first radar point cloud is collected by the lidar; projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud; the camera image is captured by the camera; screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket; and obtaining relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud.

According to the bucket three-dimensional pose tracking method provided by the invention, before the second radar point cloud is projected into the camera image to generate a third radar point cloud, the method further comprises the following steps:

representing a world origin of coordinates of the lidar using a first rotation matrix and a first translational vector, and representing a world origin of coordinates of the camera using a second rotation matrix and a second translational vector; determining a relative rotation matrix between the first rotation matrix and the second rotation matrix and determining a relative translation vector between the first translation vector and the second translation vector; and determining the mapping matrix according to the relative rotation matrix and the relative translation vector.

According to the bucket three-dimensional pose tracking method provided by the invention, the second radar point cloud is projected into a camera image based on the internal reference matrix of the camera and the mapping matrix between the laser radar and the camera to generate a third radar point cloud, and the method comprises the following steps:

performing pose transformation on the second radar point cloud according to the relative rotation matrix and the relative translation vector to acquire a fifth radar point cloud; and projecting the fifth radar point cloud to the camera image based on the internal reference matrix to generate the third radar point cloud.

According to the bucket three-dimensional pose tracking method provided by the invention, before the relative pose information between the fourth radar point cloud and the bucket point cloud model which is constructed in advance is obtained, the method further comprises the following steps:

and scanning the bucket by utilizing a laser radar and a camera, and constructing the bucket point cloud model in advance.

According to the bucket three-dimensional pose tracking method provided by the invention, the step of screening out the fourth radar point cloud from the third radar point cloud according to the color information of the bucket comprises the following steps:

and deleting all points which are not matched with the color information of the bucket in the third radar point cloud according to the color information of the bucket to obtain the fourth radar point cloud.

According to the bucket three-dimensional pose tracking method provided by the invention, after the bucket distribution point cloud is generated, the method further comprises the following steps: and displaying the bucket distribution point cloud by using a display screen in the cab.

In a second aspect, the present invention further provides a bucket three-dimensional pose tracking apparatus, including:

the first processing unit is used for performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; the first radar point cloud is collected by the lidar;

the second processing unit is used for projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud; the camera image is captured by the camera;

the third processing unit is used for screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket;

and the fourth processing unit is used for acquiring relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud.

In a third aspect, the present invention further provides a bucket three-dimensional pose tracking system, including:

the system comprises a laser radar, a camera, a display screen and an industrial personal computer; the industrial personal computer at least comprises the bucket three-dimensional pose tracking device.

In a fourth aspect, the invention further provides an excavator, which at least comprises the bucket three-dimensional pose tracking system; the laser radar and the camera are horizontally arranged outside the cab of the excavator and face the bucket.

In a fifth aspect, the present invention provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor executes the program to implement any of the steps of the bucket three-dimensional pose tracking method described above.

In a sixth aspect, the present invention also provides a non-transitory computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of any one of the bucket three-dimensional pose tracking methods described above.

According to the bucket three-dimensional pose tracking method, device and system and the excavator, only the laser radar and the camera are required to be installed on the cab cabin, other types of sensor monitoring on the bucket rod and the bucket is not required, bucket distribution point cloud can be obtained on line, the structure is simple, maintenance is convenient, and the bucket three-dimensional pose tracking method is not easily influenced by the external environment.

Drawings

In order to more clearly illustrate the technical solutions of the present invention or the prior art, the drawings needed for the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and those skilled in the art can also obtain other drawings according to the drawings without creative efforts.

FIG. 1 is a schematic flow chart of a bucket three-dimensional pose tracking method provided by the present invention;

FIG. 2 is a top view of a lidar and camera mounting location provided by the present invention;

FIG. 3 is a front view of a lidar and camera mounting location provided by the present invention;

FIG. 4 is a schematic structural diagram of a bucket three-dimensional pose tracking device provided by the present invention;

fig. 5 is a schematic structural diagram of an electronic device provided in the present invention.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is obvious that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

It should be noted that in the description of the embodiments of the present invention, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element. The terms "upper", "lower", and the like, indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, and are only for convenience in describing the present invention and simplifying the description, but do not indicate or imply that the referred devices or elements must have a specific orientation, be constructed and operated in a specific orientation, and thus, should not be construed as limiting the present invention. Unless expressly stated or limited otherwise, the terms "mounted," "connected," and "connected" are intended to be inclusive and mean, for example, that they may be fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood by those skilled in the art according to specific situations.

The terms first, second and the like in the description and in the claims of the present application are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It will be appreciated that the data so used may be interchanged under appropriate circumstances such that embodiments of the application may be practiced in sequences other than those illustrated or described herein, and that the terms "first," "second," and the like are generally used herein in a generic sense and do not limit the number of terms, e.g., the first term can be one or more than one. In addition, "and/or" in the specification and claims means at least one of connected objects, a character "/" generally means that a preceding and succeeding related objects are in an "or" relationship.

The laser radar mainly comprises a radar transmitting system which transmits a radar signal, the signal reaches the surface of a target obstacle to cause scattering, and the reflected radar signal is collected by a radar receiving system so as to determine the distance between the target obstacle and the laser radar by measuring the running time of the radar signal.

Compared with the traditional mechanical laser radar, the solid laser radar cancels a mechanical structure by adopting a phased array principle, and changes the emitting angle of a radar signal by adjusting the phase difference of each emitting unit in a radar emitting array. Phased arrays are generally implemented by controlling the phase of a phased array through an electric signal so that a radar signal beam points to a target to be scanned.

The bucket three-dimensional pose tracking method provided by the invention comprehensively utilizes the characteristic of the laser radar and is combined with the camera to shoot real-time images so as to realize the real-time tracking of the bucket three-dimensional pose.

The following describes a bucket three-dimensional pose tracking method, device and system and an excavator provided by the invention with reference to fig. 1 to 5.

Fig. 1 is a schematic flow chart of a bucket three-dimensional pose tracking method provided by the present invention, as shown in fig. 1, including but not limited to the following steps:

step 101: and performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud.

Wherein the first radar point cloud is acquired by the lidar.

Fig. 2 is a plan view of a laser radar and camera installation position provided by the present invention, and fig. 3 is a front view of a laser radar and camera installation position provided by the present invention, as shown in fig. 2 and 3, the laser radar and camera may be fixedly installed on the front side of the cab of the excavator in advance. The laser radar and the camera have a certain installation distance, and can be installed on the same horizontal line or the same vertical line.

Wherein, laser radar can adopt solid-state laser radar to can be according to the power arm of excavator and the position change of scraper bowl, the outgoing angle of automatic change radar signal, in order to more accurate gather in real time the first radar point cloud that contains the scraper bowl position.

The first radar point cloud not only comprises the point cloud in the bucket moving range, but also comprises the interference point cloud in other non-bucket moving ranges. According to the method, the width value of the bucket and the relative movement range of the bucket and the laser radar are obtained to determine the movement range of the bucket.

The relative movement range of the bucket and the laser radar is the maximum movement distance range of the bucket in the movement process relative to the laser radar. The width of the bucket is generally considered to be perpendicular to the relative movement range, so that the movement range of the bucket determined by the width and the relative movement range can be approximately a rectangular area.

And according to the determined bucket moving range, directly filtering the point cloud of the area where the bucket moving range is located from the first radar point cloud acquired by the laser radar, and taking the point cloud as second radar point cloud.

Optionally, the method of the above-mentioned pass filtering may be: the first radar point cloud is cut by establishing boundary coordinates of the bucket moving range (or only four vertex coordinates of a rectangular area of the bucket moving range) so as to reserve all points in the bucket moving range, and then the second radar point cloud can be obtained.

Step 102: and projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud.

Wherein the camera image is captured by the camera.

After the second radar point cloud is acquired in step 101, most of the interference point cloud is deleted, and only a small amount of point cloud related to positions such as a power arm and the like which influences the tracking of the three-dimensional pose of the bucket is obtained. According to the method, the influence of non-bucket parts on the tracking of the three-dimensional pose of the bucket is eliminated from the second radar point cloud by means of the characteristic that the color of the bucket is different from the color of parts such as a power arm and the like.

Before that, the second radar point cloud and the camera image shot by the camera are fused, and as the camera image is an RGB image and each pixel point of the camera image has corresponding color information, the third radar point cloud obtained after fusion also has color information, that is, each point in the third radar point cloud has corresponding color information.

The invention relates to a method for representing the installation position and relative position relation of a laser radar and a camera in a world coordinate system by using a mapping matrix between the laser radar and the camera. In the process of acquiring the second radar point cloud, only the first radar point cloud is cut without involving the change of the point cloud position, so that after the relative position relationship between the laser radar and the camera is determined, the second radar point cloud related to the laser radar and the camera image shot by the camera image can be quickly fused through the internal reference matrix of the camera, and the point cloud with color information acquired after fusion is used as the third radar point cloud.

Step 103: and screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket.

In step 103, the color of the bucket is mainly used, and the colors of other non-bucket parts are distinguished, so that some interference points are deleted from the third radar point cloud, and the point cloud only including the position of the bucket is obtained as a fourth radar point cloud.

Specifically, since the color of the bucket is grayish brown, and the color of other parts such as the power arm is not grayish brown, the point having no grayish brown color information in the third radar point cloud is deleted according to the color information, and the third radar point cloud including only the position of the bucket can be acquired.

Step 104: and obtaining relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a fourth radar point cloud which represents bucket point cloud distribution.

In step 104, first, the relative pose calculation is performed on the fourth radar point cloud acquired in real time and the bucket point cloud model constructed in advance.

The bucket point cloud model is a point cloud model which is constructed by scanning a bucket by using a solid state laser radar and a camera in advance according to the actual size of the bucket in an off-line mode and has color information, and the model can be regarded as a standard reference model of the bucket.

By calculating the relative pose of the fourth radar point cloud and the bucket point cloud model, the acquired relative pose information can be used for representing the real pose of the bucket under the current condition.

Further, according to the obtained relative pose information, the bucket point cloud model is rendered, the actual pose of the bucket is simulated, and the bucket distribution point cloud with the color information can be obtained.

After the bucket distribution point cloud is obtained, the spatial three-dimensional coordinate position of each bucket tooth can be determined according to the distribution condition of the point cloud.

According to the bucket three-dimensional pose tracking method provided by the invention, only the laser radar and the camera are required to be installed on the cab cabin, other types of sensor monitoring on the bucket rod and the bucket are not required, the bucket distribution point cloud can be obtained on line, the structure is simple, the maintenance is convenient, and the bucket three-dimensional pose tracking method is not easily influenced by the external environment.

Based on the content of the foregoing embodiment, as an optional embodiment, before the projecting the second radar point cloud into the camera image to generate a third radar point cloud, the method further includes:

representing a world origin of coordinates of the lidar using a first rotation matrix and a first translational vector, and representing a world origin of coordinates of the camera using a second rotation matrix and a second translational vector;

determining a relative rotation matrix between the first rotation matrix and the second rotation matrix and determining a relative translation vector between the first translation vector and the second translation vector;

and determining the mapping matrix according to the relative rotation matrix and the relative translation vector.

Wherein the rotation matrix describes the direction of the coordinate axes of the world coordinate system relative to the camera coordinate axes; the translation vector describes the position of the spatial origin in the camera coordinate system.

As an alternative embodiment, the world coordinate origin of the laser radar may be comprehensively represented by the first rotation matrix Ro1 and the first translational vector Tm1, and the world coordinate origin of the camera may be represented by the second rotation matrix Ro2 and the second translational vector Tm 2.

Wherein the first rotation matrix Ro1 and the second rotation matrix Ro2 are both matrices of 3 × 3, and the first translation vector Tm1 and the second translation vector Tm2 are both vectors of 1 × 3.

Thus, a relative rotation matrix Rw between the first rotation matrix Ro1 and the second rotation matrix Ro2, and a relative translation vector tw between the first translation vector Tm1 and the second translation vector Tm2 are calculated, respectively.

The relative rotation matrix Rw is also a matrix of 3 × 3, and the relative translation vector tw is also a vector of 1 × 3.

Finally, according to the relative rotation matrix Rw and the relative translation vector tw, a calculation formula for generating the mapping matrix R1 is as follows:

according to the bucket three-dimensional pose tracking method provided by the invention, the relative position relation between the laser radar and the camera is identified by adopting the mapping matrix, so that a foundation is provided for fusing the radar point cloud and the camera image in the later period, and the fusion accuracy can be effectively improved.

Based on the content of the foregoing embodiment, as an optional embodiment, the projecting the second radar point cloud into a camera image based on the camera internal reference matrix and the mapping matrix between the laser radar and the camera to generate a third radar point cloud includes:

performing pose transformation on the second radar point cloud according to the relative rotation matrix and the relative translation vector to acquire a fifth radar point cloud;

and projecting the fifth radar point cloud to the camera image based on the internal reference matrix to generate the third radar point cloud.

The expression of the fifth radar point cloud is as follows: rw Pt + tw;

wherein Rw is a relative rotation matrix, tw is a relative translation vector, and Pt is a second radar point cloud.

Internal reference matrix with cameraWherein fx, fy, and cy are internal references of the camera, respectively, and the expression of the third radar point cloud Pr is as follows:

wherein the content of the first and second substances,

wherein X, Y, H are point cloud projection intermediate variables respectively.

The invention provides a method for fusing radar point cloud and a camera image, wherein a second radar point cloud is projected into the camera image, and an acquired third radar point cloud has position information in the second radar point cloud and color information in the camera image, so that a foundation is provided for further removing point cloud which is not related to a bucket from the second radar point cloud, namely a data foundation is provided for carrying out bucket three-dimensional pose tracking in real time, and the authenticity and the accuracy of simulation are improved.

Based on the content of the foregoing embodiment, as an optional embodiment, before obtaining the relative pose information between the fourth radar point cloud and the bucket point cloud model that is constructed in advance, the method further includes: and scanning the bucket by utilizing a laser radar and a camera, and constructing the bucket point cloud model in advance.

The bucket point cloud model is pre-constructed in an offline mode, and the construction method comprises the following steps:

the bucket is scanned independently by using the solid-state laser radar and the camera (namely, the scanning object only comprises the bucket), and the obtained radar point cloud and the camera image are fused to obtain the colored bucket radar point cloud.

Further, interference points of the obtained bucket radar point cloud can be removed in a manual processing mode, only part of the point cloud of the bucket is reserved, and finally the obtained bucket radar point cloud is used as a bucket point cloud model.

According to the method, a standard bucket point cloud model is constructed in advance in an off-line mode, so that after the fourth radar point cloud is obtained, the fourth radar point cloud is compared with the bucket point cloud model, the relative pose information between the fourth radar point cloud and the bucket point cloud model can be obtained, the bucket point cloud model is rendered by using the relative pose information, and the radar point cloud of the current bucket can be obtained.

Therefore, the reconstruction of the bucket three-dimensional radar point cloud can be quickly finished only by acquiring the relative pose information needing to carry out pose transformation on the bucket point cloud model, and all radar point clouds of the bucket do not need to be reconstructed, so that the tracking response speed is effectively improved, the real-time tracking is possible, and the acquired bucket distribution point cloud distribution is clearer and more real.

It should be noted that, in the bucket three-dimensional pose tracking method provided by the present invention, the method for obtaining the relative pose information between the fourth radar Point Cloud and the bucket Point Cloud model may be implemented by using an Iterative Closest Point algorithm (ICP) in a Point Cloud Registration algorithm (Point Cloud Registration), that is, calculating the relative pose information between the fourth radar Point Cloud and the bucket Point Cloud model, which is expressed by using a relative rotation matrix Rk and a relative translation vector tk, and the present invention is not specifically limited.

Based on the content of the foregoing embodiment, as an optional embodiment, the screening out the fourth radar point cloud from the third radar point cloud according to the color information of the bucket includes:

and deleting all points which are not matched with the color information of the bucket in the third radar point cloud according to the color information of the bucket to obtain the fourth radar point cloud.

According to the method, all interference points of positions corresponding to other non-bucket positions are deleted from the third radar point cloud mainly by means of the difference between the color of the bucket and the colors of other non-bucket positions, and therefore the point cloud only containing the position of the bucket is obtained and serves as the fourth radar point cloud.

Specifically, since the color of the bucket is grayish brown, and the color of other parts such as the power arm is not grayish brown, the point having no grayish brown color information in the third radar point cloud is deleted according to the color information, and the third radar point cloud including only the position of the bucket can be acquired.

Optionally, the color of the bucket can be manually set to be a brighter color to improve the color difference between the bucket and other non-bucket parts, so that the accuracy of pose tracking can be further improved.

According to the bucket three-dimensional pose tracking method, the color information of the camera image is fully utilized, so that the distinctiveness between the external color of the bucket and other non-bucket positions is utilized, the fourth radar point cloud only containing the position of the bucket can be accurately and quickly screened from the third radar point cloud, and the identification precision is greatly improved.

Based on the content of the foregoing embodiment, as an optional embodiment, after generating the bucket distribution point cloud, the method further includes: and displaying the bucket distribution point cloud by using a display screen in the cab.

The bucket three-dimensional pose tracking method provided by the invention mainly provides reference for the operation of a driver of the excavator by constructing the bucket distribution point cloud, so that in the bucket three-dimensional pose tracking method provided by the invention, after the bucket distribution point cloud is obtained, the bucket distribution point cloud is displayed on a display screen in a cab, and the driver can check the pose of the bucket in real time in the process of driving and controlling the excavator, so that the defect of poor operation precision caused by the fact that the driver operates only by subjective feeling in the prior art is overcome.

Optionally, when the bucket distribution point cloud is displayed, the spatial three-dimensional coordinate position of the bucket teeth can be output in real time in the display screen, so that guidance can be provided for an operator to further improve the operation precision.

Fig. 4 is a schematic structural diagram of the bucket three-dimensional pose tracking apparatus provided by the present invention, as shown in fig. 4, mainly including a first processing unit 41, a second processing unit 42, a third processing unit 43, and a fourth processing unit 44, where:

the first processing unit 41 is mainly configured to perform direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar, and obtain a second radar point cloud; the first radar point cloud is collected by the lidar.

The second processing unit 42 is mainly configured to project the second radar point cloud into the camera image based on an internal reference matrix of the camera and a mapping matrix between the laser radar and the camera, so as to generate a third radar point cloud; the camera image is captured by the camera.

The third processing unit 43 is mainly configured to screen out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket.

The fourth processing unit 44 is mainly configured to acquire relative pose information between the fourth radar point cloud and a bucket point cloud model that is constructed in advance, so as to render the bucket point cloud model according to the relative pose information, and generate a bucket distribution point cloud.

It should be noted that, when the bucket three-dimensional pose tracking apparatus provided in the embodiment of the present invention is specifically operated, the bucket three-dimensional pose tracking method described in any of the above embodiments may be executed, and details of this embodiment are not described herein.

According to the bucket three-dimensional pose tracking device provided by the invention, only the laser radar and the camera are required to be installed on the cab cabin, other types of sensor monitoring on the bucket rod and the bucket are not required, the bucket distribution point cloud can be obtained on line, the structure is simple, the maintenance is convenient, and the bucket three-dimensional pose tracking device is not easily influenced by the external environment.

Based on the content of the above embodiment, as an optional embodiment, the present invention further provides a bucket three-dimensional pose tracking system, which mainly includes: the system comprises a laser radar, a camera, a display screen and an industrial personal computer; the industrial personal computer at least comprises the bucket three-dimensional pose tracking device.

It should be noted that, when the bucket three-dimensional pose tracking system provided in the embodiment of the present invention is specifically operated, the bucket three-dimensional pose tracking method described in any of the above embodiments may be executed, and details of this embodiment are not described herein.

As an alternative embodiment, the invention further provides an excavator, which at least comprises the bucket three-dimensional pose tracking system; the laser radar and the camera are horizontally arranged outside the cab of the excavator and face the bucket.

Fig. 5 is a schematic structural diagram of an electronic device provided in the present invention, and as shown in fig. 5, the electronic device may include: a processor (processor)510, a communication Interface (Communications Interface)520, a memory (memory)530 and a communication bus 540, wherein the processor 510, the communication Interface 520 and the memory 530 communicate with each other via the communication bus 540. Processor 510 may invoke logic instructions in memory 530 to perform a bucket three-dimensional pose tracking method, the method comprising: performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; the first radar point cloud is collected by the lidar; projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud; the camera image is captured by the camera; screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket; and obtaining relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud.

Furthermore, the logic instructions in the memory 530 may be implemented in the form of software functional units and stored in a computer readable storage medium when the software functional units are sold or used as independent products. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.

In another aspect, the present invention also provides a computer program product including a computer program stored on a non-transitory computer-readable storage medium, the computer program including program instructions that, when executed by a computer, enable the computer to perform the bucket three-dimensional pose tracking method provided by the above methods, the method including: performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; the first radar point cloud is collected by the lidar; projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud; the camera image is captured by the camera; screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket; and obtaining relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud.

In yet another aspect, the present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored, the computer program being implemented by a processor to perform the bucket three-dimensional pose tracking method provided by the above embodiments, the method including: performing direct filtering on the first radar point cloud according to the width value of the bucket and the relative movement range of the bucket and the laser radar to obtain a second radar point cloud; the first radar point cloud is collected by the lidar; projecting the second radar point cloud into a camera image based on an internal reference matrix of a camera and a mapping matrix between the laser radar and the camera to generate a third radar point cloud; the camera image is captured by the camera; screening out a fourth radar point cloud from the third radar point cloud according to the color information of the bucket; and obtaining relative pose information between the fourth radar point cloud and a bucket point cloud model which is constructed in advance, and rendering the bucket point cloud model according to the relative pose information to generate a bucket distribution point cloud.

The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.

Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware. With this understanding in mind, the above-described technical solutions may be embodied in the form of a software product, which can be stored in a computer-readable storage medium such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the methods described in the embodiments or some parts of the embodiments.

Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.