Positioning mapping system, method and device for drill carriage, electronic equipment and medium

Abstract

The invention provides a positioning mapping system, method and device for a drill carriage, electronic equipment and a medium, and relates to the technical field of drill carriage positioning mapping. The two laser radars are suspended and inversely mounted in front of the left and right of the top of the drill carriage, and the mounting height is higher than that of a drill boom. On one hand, the environment in front of the drill carriage can be scanned from two different angles, a large area in front of the drill carriage is covered, and blind areas on the left side and the right side of the drill carriage are reduced. On the other hand, the installation height is higher than the drill boom, interference of the drill boom on radar scanning in the operation process is avoided, the scanning sight line of the radar can cross the drill boom and part of the structure of the vehicle body, and shielding of the parts on a scanning area is reduced. And thirdly, the laser radar is suspended and inversely arranged in front of the top of the drill carriage, so that the effective scanning range of the laser radar is expanded. Therefore, the shielding of the laser radar is reduced, the scanning range of the laser radar is expanded, the laser radar can obtain more complete environment information, and the positioning mapping accuracy is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and mapping for drill jumbos, and particularly to a positioning and mapping system, method, device, electronic device and medium for drill jumbos. Background Art

[0002] A drill jumbo, also known as a rock drilling jumbo, is a construction machine used for drilling holes in hard materials such as rocks and soils, and is widely used in fields such as mine exploitation and tunnel excavation. An intelligent drill jumbo can achieve underground unmanned mining operations. In underground unmanned mining operations, it is crucial to efficiently and accurately establish an underground mine roadway environment map and achieve real-time positioning of the vehicle. The positioning and mapping technology is also the basis for the automatic driving of underground mine equipment and the automatic control of loading and unloading operations. SLAM (Simultaneous Localization and Mapping) based on lidar is the mainstream research direction of current positioning and mapping technology.

[0003] SLAM is a technical system that constructs an environmental map in real time and simultaneously determines its own position in the map. SLAM usually uses multiple sensors, such as lidar and inertial measurement unit (IMU), to sense the surrounding environment. The lidar obtains three-dimensional point cloud data of the environment by emitting laser beams and measuring the time of the reflected light. The IMU can measure the acceleration and angular velocity of the device, thereby inferring the motion state of the device. By fusing and processing these sensor data, the map and its own position information are continuously updated.

[0004] The existing lidar acquisition method usually installs a lidar on the roof of the drill jumbo to collect the environmental point cloud in front of the drill jumbo in real time. Due to the special structure of the drill jumbo, there is a drill arm structure in the front of the drill jumbo. The existing acquisition method is affected by the occlusion of the drill jumbo body and the drill arm, resulting in incomplete acquisition of point clouds and affecting the positioning and mapping effect of the rock drilling jumbo. Summary of the Invention

[0005] Embodiments of the present invention provide a positioning and mapping system, method, device, electronic device and medium for drill jumbos to solve the problem of low positioning and mapping accuracy caused by the occlusion of the lidar by the drill jumbo body in the existing lidar installation method.

[0006] In a first aspect, embodiments of the present invention provide a positioning and mapping system for drill jumbos, where the drill jumbo includes a drill arm provided at an intermediate position in front of the drill jumbo; the system includes: an inertial measurement module, a control module and two lidars; The field of view angle above the horizontal plane in the vertical field of view angle of the lidar is greater than the field of view angle below the horizontal plane; The two lidars are used to be mounted upside down in front of the top of the jumbo, where one lidar is installed at the front left of the top of the jumbo, and the other lidar is installed at the front right of the top of the jumbo; the installation height of the lidar is higher than the height of the drill arm; The lidar is used to collect environmental point cloud data of the working environment of the jumbo; The inertial measurement module is used to collect the motion state data of the jumbo; The control module performs positioning of the jumbo and mapping of the working environment based on the motion state data and the environmental point cloud data.

[0007] In a possible implementation manner, the lidar includes a solid-state lidar.

[0008] In a second aspect, an embodiment of the present invention provides a positioning and mapping method for a jumbo, which is applied to the positioning and mapping system for a jumbo described in any item of the first aspect. The method includes: Collect point cloud data in front of the jumbo through each of the upside-down lidars, and collect the motion state data of the jumbo through the inertial measurement module; For any one lidar, transform the point cloud data to the jumbo coordinate system through a coordinate transformation matrix to obtain the transformed point cloud data of this lidar; Perform point cloud registration and fusion processing on the transformed point cloud data of the two lidars with different perspectives, compensate the point cloud data of the occluded area, and obtain the fused point cloud data as the environmental point cloud data of the working environment of the jumbo; Perform positioning of the jumbo and mapping of the working environment based on the motion state data and the environmental point cloud data.

[0009] In a possible implementation manner, the obtaining the fused point cloud data as the environmental point cloud data of the working environment of the jumbo includes: Eliminate the point cloud data within the predetermined drill arm area in the fused point cloud data to obtain the point cloud data with the drill arm eliminated; the drill arm area is determined based on the relative position relationship between each lidar and the drill arm; Use the point cloud data with the drill arm eliminated as the environmental point cloud data of the working environment of the jumbo.

[0010] In a possible implementation manner, after collecting the point cloud data in front of the jumbo through each of the upside-down lidars, it further includes: For each lidar, obtain the drill arm area in the point cloud data of this lidar; the drill arm area is determined based on the relative position relationship between this lidar and the drill arm; Eliminate the data within the drill arm area in the point cloud data of this lidar to obtain the point cloud data of this lidar with the drill arm area eliminated; Correspondingly, the transformation of the point cloud data to the jumbo coordinate system by the coordinate transformation matrix to obtain the transformed point cloud data of the lidar includes: Transform the point cloud data with the boom area removed to the jumbo coordinate system by the coordinate transformation matrix to obtain the transformed point cloud data of the lidar.

[0011] In a possible implementation manner, before obtaining the boom area in the point cloud data of each lidar, it further includes: Determine the geometric model of the boom based on the shape, size, and position of the boom in the jumbo coordinate system; For each lidar, obtain the point cloud data of the lidar; Match the geometric model of the boom with the point cloud data, and extract the point cloud covered by the geometric model of the boom; Based on the point cloud covered by the geometric model of the boom, fit the convex edge of the boom by the Quickhull algorithm as the boom area in the point cloud data of the lidar.

[0012] In a possible implementation manner, based on the motion state data and the environmental point cloud data, the positioning and mapping of the jumbo include: Adopt the FAST-LIO2 framework to fuse the motion state data and the environmental point cloud data to achieve the positioning of the jumbo and the mapping of the working environment.

[0013] In a third aspect, an embodiment of the present invention provides a positioning and mapping device for a jumbo, which is applied to the positioning and mapping system for a jumbo described in any one of the first aspects; the device includes: An acquisition module, configured to acquire the point cloud data in front of the jumbo through each of the inverted lidars, and acquire the motion state data of the jumbo through an inertial measurement module; A coordinate transformation module, configured to, for any lidar, transform the point cloud data to the jumbo coordinate system by the coordinate transformation matrix to obtain the transformed point cloud data of the lidar; A fusion module, configured to perform point cloud registration and fusion processing on the transformed point cloud data of two lidars from different perspectives, compensate for the point cloud data in the occluded area, and obtain the fused point cloud data as the environmental point cloud data of the jumbo working environment; A positioning and mapping module, configured to perform the positioning of the jumbo and the mapping of the working environment based on the motion state data and the environmental point cloud data.

[0014] In a fourth aspect, an embodiment of the present invention provides an electronic device, including a memory and a processor, the memory stores a computer program, and when the processor executes the computer program, the method in the first aspect or any possible implementation manner of the first aspect is implemented.

[0015] In a fifth aspect, an embodiment of the present invention provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the method in the first aspect or any possible implementation of the first aspect.

[0016] In an embodiment of the present invention, two laser radars are arranged, which are suspended and inverted at the left front and right front of the top of the drilling vehicle, and the installation height is higher than the drill arm. On the one hand, a laser radar is arranged on each of the left and right sides, which can scan the environment in front of the drilling vehicle from two different angles, cover a larger area in front of the drilling vehicle, and reduce the blind spots on the left and right sides of the drilling vehicle. On the other hand, the installation height of the laser radar is arranged to be higher than the drill arm, so as to avoid the interference of the drill arm on the radar scanning during the operation, so that the scanning line of sight of the radar can pass over the drill arm and part of the structure of the vehicle body, and reduce the obstruction of the scanning area by these components. Thirdly, by suspending the laser radar inverted in front of the top of the drilling vehicle, the larger field of view in the vertical field of view angle can be directed toward the working area in front of the vehicle, thereby expanding the effective scanning range of the laser radar. As a result, the obstruction of the laser radar is reduced, the scanning range of the laser radar is expanded, and the laser radar can obtain more complete environmental information, thereby improving the accuracy of positioning and mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 An application scenario diagram of the drilling vehicle positioning and mapping system provided in an embodiment of the present invention; Figure 2 It is a structural schematic diagram of a positioning and mapping system for a drilling vehicle provided by an embodiment of the present invention; Figure 3 is a schematic diagram of the horizontal field of view angle of a laser radar provided in an embodiment of the present invention; Figure 4 is a schematic diagram of the vertical field of view angle of a laser radar provided in an embodiment of the present invention; Figure 5 is a flow chart of an implementation of a positioning and mapping method for a drilling vehicle provided by an embodiment of the present invention; Figure 6 is a point cloud image of the laser radar provided in an embodiment of the present invention; Figure 7 It is a point cloud image of the inverted laser radar provided by an embodiment of the present invention; Figure 8 It is a radar point cloud image on the left provided by an embodiment of the present invention; Figure 9 It is a radar point cloud image on the right provided by an embodiment of the present invention; Figure 10 is a radar point cloud image after point cloud fusion processing provided by an embodiment of the present invention; Figure 11It is the radar point cloud map before removing the drill arm provided by the embodiment of the present invention; Figure 12 It is the radar point cloud map after removing the drill arm provided by the embodiment of the present invention; Figure 13 It is the schematic diagram of the mapping result provided by the embodiment of the present invention; Figure 14 It is the schematic structural diagram of the positioning and mapping device for the drill jumbo provided by the embodiment of the present invention; Figure 15 It is the schematic diagram of the electronic device provided by the embodiment of the present invention. Specific embodiments

[0018] Next, the embodiments of the present invention will be described in detail with reference to the accompanying drawings.

[0019] As an important mining equipment, the drill jumbo plays an irreplaceable role in the entire mine production. However, the underground operation environment of the drill jumbo is complex and dangerous. In high-risk environments such as mines and tunnel excavations, operators will face serious safety risks such as collapses and explosions. And the efficiency of manual operation is limited by the working hours and energy of the personnel.

[0020] The automated system can reduce the dependence on manual labor for underground operations and improve the safety during the operation process. Moreover, unmanned and automated equipment can maintain high-efficiency operation under continuous high-load working conditions, thus greatly improving the work efficiency. Therefore, it is very necessary to realize underground unmanned mining.

[0021] During the process of driverless operation in underground metal mines, how to efficiently and accurately establish the underground mine roadway environment map and realize the real-time positioning of the vehicle is an important content in the research of unmanned mines, and it is also the basis for driverless mining equipment such as automatic driving and automatic control of loading and unloading operations of underground mine equipment. Therefore, researching high-precision modeling and vehicle positioning in underground metal mines is of great significance for realizing unmanned mining in underground metal mines.

[0022] SLAM based on 3D lidar is the mainstream research direction for current research on positioning and mapping. However, the actual structure of the drill jumbo, as well as the installation position and performance of the lidar, will affect the quality of the collected point cloud, and thus will affect the accuracy of positioning and mapping. Therefore, it is necessary to research the simultaneous localization and mapping method applicable to solid-state lidar and drill jumbo.

[0023] Figure 1 It is the application scenario diagram of the drill jumbo positioning and mapping system provided by the embodiment of the present invention. Refer to Figure 1 , which shows the structure of a drill jumbo. The drill jumbo generally includes a vehicle body and a drill arm. The drill arm is arranged in the front of the drill jumbo. The drill arm is mainly used to support, position and move the drill rig, and adjust the working attitude of the drill rig.

[0024] It should be noted that Figure 1 What is shown in Figure 1 is not the actual working environment of the drill rig, but only a demonstration environment. For example, the drill rig usually works in the underground mine roadway environment.

[0025] Vehicle body occlusion usually occurs in areas where there is an overlap between the radar installation position and the vehicle body structure. For example, when the radar is installed on the roof of the vehicle, the area below the vehicle body may not be effectively scanned. Due to occlusion, the radar cannot capture the point cloud data of the occluded area, resulting in data loss. This will affect the comprehensive perception of the surrounding environment. For example, data loss will cause the SLAM algorithm to be unable to find enough feature points when matching point cloud data, thus affecting the positioning accuracy. The point cloud data loss caused by occlusion will make the generated map incomplete or inaccurate, affecting subsequent path planning and unmanned mining tasks.

[0026] When the radar is installed upright on the roof of the vehicle, the area below the vehicle body may not be effectively scanned because the downward range of the vertical field of view (FOV) of the radar is limited. When the radar is installed on the side of the vehicle body, the side of the vehicle body will occlude part of the scanning area, resulting in the radar being unable to capture the environmental information near the side of the vehicle body.

[0027] Figure 2 is a schematic structural diagram of a positioning and mapping system for a drill vehicle provided by an embodiment of the present invention; refer to Figure 2 , which shows the left top of the drill vehicle, and the picture perspective is the perspective of looking up at the top of the drill vehicle from the left side of the drill vehicle. Among them, two lidars are shown in the figure. Other inertial measurement modules and control modules can be arranged inside the vehicle body and are not specifically shown in the figure.

[0028] Refer to Figure 2 , an embodiment of the present invention provides a positioning and mapping system for a drill vehicle. The drill vehicle includes a drill arm arranged at the middle position in front of the drill vehicle; the system includes: an inertial measurement module, a control module, and two lidars; the field of view above the horizontal plane in the vertical field of view of the lidar is greater than the field of view below the horizontal plane; the two lidars are used to be suspended and installed upside down in front of the top of the drill vehicle, wherein, one lidar is installed at the left front of the top of the drill vehicle, and the other lidar is installed at the right front of the top of the drill vehicle; the installation height of the lidar is higher than the height of the drill arm; the lidar is used to collect the environmental point cloud data of the drill vehicle operation environment; the inertial measurement module is used to collect the motion state data of the drill vehicle; the control module performs drill vehicle positioning and mapping of the operation environment based on the motion state data and the environmental point cloud data.

[0029] In some embodiments, the drill arm is arranged at the middle position in front of the drill vehicle. The middle position in front refers to the geometric center line directly in front of the drill vehicle body. For example, the drill arm is installed at the middle position in front of the drill vehicle by means of hinge or fixation.

[0030] In some embodiments, the system includes: an inertial measurement module, a control module, and two lidars.

[0031] Exemplarily, the inertial measurement module generally includes an accelerometer and a gyroscope. The accelerometer is used to measure the acceleration of an object in the three coordinate axis directions. By integrating the acceleration, the velocity and displacement of the object can be obtained; the gyroscope is used to measure the rotational angular velocity of the object around the three coordinate axes, thereby determining the attitude of the object.

[0032] Exemplarily, the lidar emits laser beams and measures the time from the emission of the laser beam to the reflection back after encountering an object. Using the speed of light, the distance between the object and the lidar can be calculated. By continuously emitting laser beams and scanning the surrounding environment, a large number of distance data points can be obtained, and these data points form a three-dimensional point cloud image of the surrounding environment, thereby realizing the perception and modeling of the surrounding environment.

[0033] In some embodiments, the field of view angle above the horizontal plane in the vertical field of view angle of the lidar is greater than the field of view angle below the horizontal plane.

[0034] The field of view (FOV) of the lidar determines the spatial range that the lidar can scan. The field of view angle of the lidar is divided into the horizontal field of view angle and the vertical field of view angle. The horizontal field of view angle is the scanning range of the lidar in the horizontal plane. Figure 3 It is a schematic diagram of the horizontal field of view angle of the lidar provided by the embodiment of the present invention. Refer to Figure 3 , the gray area in the figure is the scanning range of the lidar. Exemplarily, the horizontal field of view angle of the lidar is 360°. The vertical field of view angle is the scanning range of the lidar in the vertical plane. Figure 4 It is a schematic diagram of the vertical field of view angle of the lidar provided by the embodiment of the present invention. Refer to Figure 4 , exemplarily, the vertical field of view angle of the lidar is 59°. Exemplarily, the vertical field of view angle is divided by the horizontal plane into the field of view angle above the horizontal plane and the field of view angle below the horizontal plane. Regarding the up and down of the horizontal plane, the lidar in the figure is installed upright, that is, the front side is facing up. Taking the lidar as a reference to establish a coordinate system, the front side of the lidar is up and the back side is down. For example, the field of view angle above the horizontal plane is 52°. The field of view angle below the horizontal plane is 7°.

[0035] It should be noted that the field of view angle above the horizontal plane is greater than that below the horizontal plane, which is determined by the specific structure of the lidar. Usually, the lidar is installed at the front of a general vehicle, near the bottom. In this way, the lidar can better scan the 360° range in the horizontal direction around, and can also better scan the area from bottom to top in the vertical direction. However, due to the special structure of the drill rig, there is a drill arm in front of the drill rig, which makes it not suitable to place the lidar at the bottom in front of the vehicle, so it is usually installed upright on the roof of the vehicle. However, in the case of upright installation, due to the small field of view angle below the horizontal plane, it is difficult to scan the working face directly in front of the vehicle.

[0036] In some embodiments, the two lidars are used to be suspended and installed upside down in front of the top of the drill jumbo, wherein one lidar is installed at the left front of the top of the drill jumbo, and the other lidar is installed at the right front of the top of the drill jumbo; the installation height of the lidar is higher than the height of the drill arm.

[0037] It should be noted that the two radars are respectively installed at the left front and right front of the top. After the horizontal field of view angles are superimposed, they can cover the front fan-shaped area, reducing the detection blind area blocked by the drill arm directly in front of the drill jumbo. The way of suspending and installing upside down in front of the top of the drill jumbo can expand its scanning range and reduce the occlusion of the vehicle body to the area below the radar. For example, the horizontal upward field of view angle of the Livox mid-360 radar is 52 degrees, while the horizontal downward field of view angle is only 7 degrees. If the radar is installed upright on the roof of the vehicle, the area below the vehicle body may not be effectively scanned. After upside down installation, the scanning direction of the radar is downward, which can cover the area below and around the vehicle body. The upside down installed radar can reduce the missing of point cloud data caused by the occlusion of the vehicle body, ensuring that the collected point cloud data is more complete and accurate. The upside down installed radar can better scan the ground and low obstacles, which is particularly important for the underground mine environment with uneven ground. Installing the radar in front of the roof of the vehicle and suspending it can avoid the direct collision of the radar with the ground or other obstacles, improving the service life and reliability of the radar.

[0038] In some embodiments, the lidar is used to collect the environmental point cloud data of the operating environment of the drill jumbo; the inertial measurement module is used to collect the motion state data of the drill jumbo; the control module performs the positioning of the drill jumbo and the mapping of the operating environment based on the motion state data and the environmental point cloud data.

[0039] In some embodiments, the lidar includes a solid-state lidar. For example, the model of the solid-state lidar can be Livox mid-360.

[0040] In the embodiment of the present invention, two lidars are provided, which are respectively suspended and installed upside down at the left front and right front of the top of the jumbo, and the installation height is higher than that of the drill arm. On the one hand, one lidar is provided on each of the left and right sides, which can scan the environment in front of the jumbo from two different angles, covering a large area in front of the jumbo and reducing the blind areas on the left and right sides of the jumbo. On the other hand, the installation height of the lidar is set higher than that of the drill arm, avoiding the interference of the drill arm on the radar scan during the operation, enabling the radar scan line of sight to cross some structures of the drill arm and the vehicle body, and reducing the occlusion of these components on the scan area. On the third hand, the lidar is suspended and installed upside down at the front of the top of the jumbo, which can make the larger field of view in the vertical field of view face the working area in front of the vehicle, expanding the effective scan range of the lidar. Thus, the occlusion of the lidar is reduced, the scan range of the lidar is expanded, enabling the lidar to obtain more complete environmental information and improving the accuracy of positioning and mapping.

[0041] The above describes the structure of the positioning and mapping system. The following describes how to fuse multi-radar data and multi-modal data based on the above structure to eliminate the influence of the drill arm, improve the quality of point cloud acquisition, and improve the accuracy of positioning and mapping.

[0042] It should be noted that, by way of example, to implement the positioning and mapping method provided by the embodiment of the present invention, a SLAM data acquisition and processing platform can be built first. The platform includes a drill rig, a solid-state lidar, a control module, and an IMU. The drill rig is the core mechanical part of the SLAM data acquisition platform, providing the motion information and working state data of the platform. The solid-state lidar is the main device for collecting environmental point cloud data, scanning the surrounding environment through laser beams to obtain the distance and depth information of the objects relative to the device. The IMU can measure the motion state of the platform in real time and is used to compensate the radar data in the SLAM system. The control module is a computing unit responsible for receiving, processing, and coordinating the data of different sensors and systems, and performing data acquisition and processing tasks by interacting with each sensor.

[0043] The key to building the acquisition platform is to determine the installation position of the solid-state lidar. Considering that the maximum angle of the LIVOX mid-360 lidar used is 52 degrees upward horizontally, while the maximum angle downward horizontally is only 7 degrees. If the lidar is installed upright on the vehicle top, most areas near the vehicle body cannot be effectively captured. Therefore, the lidar is installed upside down on the vehicle top, and subsequent upside-down point cloud processing needs to be implemented.

[0044] Partial occlusion of the vehicle body drilling arm will cause serious loss of spatial point cloud acquisition. Therefore, multiple radars can be used for data compensation. For example, radars are installed on both the left and right sides of the vehicle body, and point cloud fusion processing is implemented to ensure the integrity of environmental perception. Further, when the radar scans, the drilling arm part will be included in the environmental mapping, affecting the accuracy of the map. Therefore, the point cloud data of the drilling arm is identified and removed. Finally, the slam algorithm can be used to construct a complete map.

[0045] Figure 5 It is the implementation flowchart of the positioning and mapping method for the drilling vehicle provided by the embodiment of the present invention. Refer to Figure 5 , the embodiment of the present invention provides a positioning and mapping method for a drilling vehicle, which is applied to the positioning and mapping system for a drilling vehicle described in any one of the above, and the method includes: Step 501, collect the point cloud data in front of the drilling vehicle through each of the inverted lidars, and collect the motion state data of the drilling vehicle through the inertial measurement module; Exemplarily, the lidars on the left and right sides respectively collect point cloud data. Due to different installation positions, the viewing angles of the lidars on the left and right sides are different.

[0046] Step 502, for any lidar, transform the point cloud data to the drilling vehicle coordinate system through the coordinate transformation matrix to obtain the transformed point cloud data of the lidar; The installation direction of the radar directly affects the coordinate system of the point cloud data. The typical representation in the three-dimensional lidar coordinate system is: along the front and back direction of the vehicle body is the X-axis direction, the left and right direction is the Y-axis direction, and the up and down direction is the Z-axis direction. When the radar is installed upright, its coordinate system is the same as the vehicle coordinate system. However, when the radar is installed inverted, the coordinate system of the radar itself changes, especially the Z-axis is reversed, that is, the Z-axis no longer points upward, but downward. At this time, the position of the point cloud data captured by the radar in the vehicle coordinate system does not match the actual situation, and coordinate transformation is required to restore the correct point cloud coordinates.

[0047] The core principle of point cloud processing for the inverted radar is to transform the point cloud data collected by the inverted radar to the vehicle coordinate system through coordinate system transformation. More specifically, the inverted radar can be regarded as rotating the radar 180 degrees around the X-axis. Therefore, in actual use, a transformation matrix should be constructed according to the inverted angle of the radar to rotate each point of the point cloud to convert it from the inverted coordinate system of the radar to the normal coordinate system of the vehicle. For the case where the radar is inverted 180 degrees, the X-axis remains unchanged, and the X-axis direction of the radar is not affected and still points to the front of the vehicle; the Y-axis is reversed. Due to the inversion, the Y-axis will change from originally pointing to the left side of the vehicle to pointing to the right side of the vehicle; the Z-axis is reversed: the Z-axis originally pointed to the upper part of the vehicle, and after inversion, it points to the lower part of the vehicle. The transformation matrix is as follows: (1) When the radar acquires point cloud data, the coordinates of each point are relative to the coordinate system of the radar. The data acquired by the inverted radar will reflect the environment above the vehicle body, rather than the actual environment below the vehicle body.

[0048] Rotation transformation of the coordinate system: To convert the point cloud data to the normal coordinate system of the vehicle, a matrix transformation needs to be performed on each point in the point cloud. Assume that the point in the point cloud acquired by the radar is , and the reference coordinate system of its coordinates is the coordinate system of the inverted radar. Through the coordinate transformation matrix R in the above formula (1), the point cloud can be converted into a point in the coordinate system of the vehicle; (2) The transformation in the above formula (2) flips the point cloud coordinates acquired by the inverted radar to the correct vehicle coordinate system, enabling the vehicle to correctly perceive its surrounding environment.

[0049] Publish the point cloud to the normal coordinate system: After the coordinate transformation is completed, the point cloud has been correctly mapped to the coordinate system of the vehicle. At this time, the processed point cloud can be published to the corresponding topic of the robot operating system or other perception systems, enabling other systems (such as path planning, obstacle avoidance, etc.) to use the correct environmental perception data.

[0050] Step 503: Perform point cloud registration and fusion processing on the transformed point cloud data of two lidars with different perspectives, compensate for the point cloud data in the occluded area, and obtain the fused point cloud data as the environmental point cloud data of the drill rig operation environment; It should be noted that regarding point cloud fusion processing, in underground or other complex environments, due to the occlusion of equipment such as drill arms, a single radar cannot cover all areas around the vehicle. Multiple radars need to be used for compensation, and the point cloud data generated by multiple radars needs to be fused to obtain complete environmental perception information. The core of point cloud fusion lies in unifying the point cloud data generated by different radars into the same coordinate system, eliminating redundant data, and forming a global three-dimensional environmental model.

[0051] Step 5031: Radar installation and coordinate system calibration: When using multiple radars, it is necessary to calibrate the installation position and attitude of each radar in advance. The installation position and angle of each radar relative to the vehicle are known, so the point clouds of each radar can be unified into the global coordinate system of the vehicle through coordinate transformation. The calibration process includes determining the pose (position and direction) of each radar and constructing a transformation matrix.

[0052] Step ①: The first step in calibration is feature point extraction. Feature point extraction is a key step in point cloud registration and automatic calibration. It extracts valuable feature points for the registration process from the point cloud data, improving the accuracy and efficiency of point cloud registration.

[0053] The feature points that can be extracted from the point cloud data include the edges, planes, etc. of the object. These features usually appear simultaneously in the point clouds of different radars, and thus can be used to calculate the relative poses of multiple radars. Edge points usually appear at the contour of the object or at the obvious geometric shape transitions. These points are formed due to the sharp changes between the object surface and the surrounding environment; plane points appear on relatively flat surfaces, such as walls, floors, etc. In buildings or structured environments, plane points usually cover a wide range and are easily captured in different radar perspectives. Edge points and plane points can be obtained by calculating the curvature of the point cloud. Curvature is an index that describes the surface change rate of the point cloud. Areas with large curvature usually mean there are feature points. By analyzing the curvature of each point, the points can be classified into high curvature (edge points) and low curvature (plane points). The curvature calculation formula is as follows: (3) represents the 2n consecutive points closest to point i on the same laser beam. and represent the distances from point i to point j respectively. Based on the above formula (3), the curvature of each point can be obtained. Thus, the feature points can be determined according to the curvature, and the feature point extraction in step ① can be realized.

[0054] Step ②, the second step in calibration is feature point registration. The ICP algorithm is used to find the same feature points in the point clouds collected by each radar device and perform registration. The relative position differences between these matching points can be used to calculate the relative transformation between the radars. ICP is a classic algorithm for point cloud registration. Its purpose is to make two groups of point clouds coincide as much as possible through iterative optimization. Its core idea is to calculate an optimal rotation matrix and translation vector by minimizing the distance error between the corresponding points of the two groups of point clouds, and align one group of point clouds to the other group of point clouds. The following describes the steps of ICP: a) Initial point cloud alignment: It is usually assumed that the alignment between the initial point clouds is relatively close but not completely coincident. For the point cloud data of multiple radars, the initial alignment can be based on the approximate geometric relationship or the initial pose guess.

[0055] b) Finding the nearest point pairs: For each point in the source point cloud, the ICP algorithm will find its "nearest" point in the target point cloud. These closest points form a set of corresponding point pairs. The nearest point is usually measured using the Euclidean distance. If a certain point in the source point cloud and the nearest point in the target point cloud is , then the corresponding point pair is .

[0056] c) Calculate the error function: Based on the found corresponding point pairs, define an error function that represents the alignment error between the two point clouds. The error formula is as follows: (4) is the rotation matrix, representing the rotation of the source point cloud, is the translation vector, representing the translation of the source point cloud, is a point in the source point cloud, is the corresponding point in the target point cloud. Based on the above formula (4), the alignment error between the two point clouds can be determined. Furthermore, the feature point registration in step ② can be achieved based on the alignment error in subsequent steps.

[0057] d) Minimize the error: Use the least squares method to calculate the rotation matrix and translation vector that can minimize the error function. This process continuously adjusts the position and orientation of the source point cloud to make it coincide with the target point cloud as much as possible.

[0058] e) Update the source point cloud: Transform the source point cloud to a new position according to the calculated rotation matrix and translation vector. The position of the updated point cloud is closer to the target point cloud. The position calculation of the new source point cloud points is as follows: (5) In the above formula (5) R represents the rotation matrix, T represents the translation vector, and is transformed to the new source point cloud through rotation and translation.

[0059] f) Iteration: During the iteration process, repeat finding the nearest point pairs and updating the transformation matrix until the error function converges to a certain threshold, that is, the two point clouds are close enough, or the number of iterations reaches a predetermined value.

[0060] The above steps a) to f), as well as the formulas (4) to (5) therein, together achieve the feature point registration in step ②. The following step ③ explains how to further improve the accuracy of the registration result.

[0061] Step ③, the third step in calibration, is to implement optimizations during the calibration process. These optimization steps can further improve the accuracy of the registration result and prevent incorrect feature point matching from affecting the overall calibration effect. The optimizations include two steps, namely, using a non - linear optimization method to minimize the error function and outlier rejection based on RANSAC. The goal of RANSAC is to find the best model by fitting a subset of the data points in the dataset. First, assume that the model to be fitted is M, which depends on the subset in the dataset. The parameters of this model can be estimated from the data points of the smallest subset , and the formula is as follows: (6) Among them, is the model fitting parameter obtained from the subset . Secondly, inlier calculation is performed. For each point in the dataset, calculate its distance to the model, and judge whether the point is an inlier according to a preset threshold . Specifically, if , then it is considered that is an inlier: (7) If , then the point is an inlier, indicating that it conforms to the model. Then evaluate the model. After each fitting, calculate the number of inliers of the model, that is, the number of points that satisfy . The goal of RANSAC is to maximize the number of inliers: (8) Among them, is the total number of points in the dataset, 1 is the indicator function, and it takes the value of 1 when , indicating that the point is an inlier. Finally, select the final model. Among all iterations, select the model with the most inliers as the final fitting result: (9) The above formulas (6) to (9) achieve the elimination of outliers based on RANSAC. The above step 5031 illustrates the radar installation and coordinate system calibration, specifically illustrating the feature point extraction, registration, and outlier elimination. The following step 5032 illustrates the point cloud acquisition and transformation.

[0062] Step 5032, Point cloud acquisition and transformation: The point cloud data collected by each radar is initially in its own coordinate system. To achieve fusion, first, the point cloud needs to be transformed into the global coordinate system of the vehicle. The formula for this process is: (10) Among them, is the coordinate of the point in the point cloud collected by the radar, is the coordinate transformation matrix of the radar relative to the vehicle, is the coordinate of the point cloud in the global coordinate system of the vehicle. By transforming the point cloud of each radar through the above formula (10), the data of all radars can be unified into the same global coordinate system.

[0063] Step 5033, Point Cloud Registration and Fusion: The point cloud data generated by different radars may have overlapping areas. To avoid redundancy, it is necessary to register and fuse the point cloud data. The purpose of point cloud registration is to accurately align the point clouds of multiple radars in space, and the ICP algorithm is used for this purpose. The main step in the fusion after registration is to eliminate duplicate points. In the overlapping area, multiple radars may generate the same point cloud data. A method based on distance threshold can be used to eliminate duplicate points. The steps of this method are as follows: Step ①, Set the distance threshold: First, set a reasonable distance threshold according to the specific application scenario , and this threshold is usually determined according to the resolution of the radar and the requirements of the actual environment.

[0064] Step ②, Calculate the distance between points: For each point cloud point, check other points in its neighborhood and calculate the Euclidean distance between it and the neighboring points. The distance formula is: (11) where and are the coordinates of two points respectively. Based on the above formula (11), the Euclidean distance between two points can be obtained, and then distance comparison and redundant point removal can be carried out in subsequent steps.

[0065] Step ③, Distance comparison: If the distance between two points is less than the set distance threshold , then these two points are considered duplicate points, and only one point is retained while the others are deleted.

[0066] Step ④, Repeat the process: Repeat this process for all points in the point cloud, and finally obtain the point cloud data after removing redundant points.

[0067] In a multi-radar system, the radar scan will capture equipment such as the drill arm and include it as part of the environment in the point cloud data, which will affect the accuracy of map construction and even misidentify the drill arm as an obstacle, interfering with navigation and path planning. To avoid this situation, it is necessary to identify and remove the data of the drill arm in point cloud processing. Exemplarily, the specific steps for removing drill arm point cloud are as follows: Step A1, Drill arm model establishment and point cloud classification; First, it is necessary to establish a model for the drill arm to understand its accurate position, shape and size in the vehicle or equipment coordinate system. The pre-determined geometric information of the drill arm can be used in subsequent processing to identify its position in the point cloud. In actual operation, the geometric information of the drill arm, including position, size and shape, can be obtained through precise measurement or CAD design. Since the position of the drill arm is usually fixed relative to the vehicle, the static area where the drill arm is located in the radar view can be calibrated before point cloud processing.

[0068] Step A2, Preprocessing of Point Cloud Filtering; After obtaining real-time point cloud data, preprocessing can be performed on the entire point cloud. This step includes denoising the data and removing unnecessary points to ensure the quality and processing efficiency of the point cloud. In practical applications, voxel grid filtering can be performed on the point cloud data to divide the point cloud into blocks and simplify the data. This process can not only reduce the amount of point cloud data but also maintain sufficient spatial accuracy for subsequent drill arm recognition.

[0069] The basic idea of voxel filtering is as follows: Divide the space into equal-sized cubes (voxels), and then select a representative point (usually the centroid of the points within the voxel or any arbitrary point) within each voxel to represent all the points within that voxel. In this way, the number of points in the point cloud will be reduced, but the overall spatial distribution and geometric features are retained. The basic steps are as follows: Step A2-1, Spatial Division: Assume that the given point cloud data is , where the coordinates of each point are . When dividing the point cloud into a grid, first determine the size of the voxel , that is, the size of the voxel in the x, y, and z directions. Then map each point in the point cloud to the corresponding voxel grid. The mapping method is: (12) Step A2-2, Calculate the Centroid: For each voxel , calculate the centroid of all the points within it: (13) Step A2-3, Generate the Downsampled Point Cloud: Combine the centroids of all voxels to form new point cloud data. The number of points in the finally obtained downsampled point cloud is a subset of the original point cloud and is usually much smaller than the original point cloud.

[0070] The above steps A2-1 to A2-3, as well as formulas (12) to (13), reduce the number of points in the point cloud through voxel filtering, retain the overall spatial distribution and geometric features, and achieve the preprocessing of point cloud filtering in step A2. The following step A3 further explains the recognition of the drill arm point cloud.

[0071] Step A3, Detection and Recognition of Drill Arm Point Cloud; The core of recognizing the drill arm point cloud lies in determining the position of the drill arm under the current radar view and removing it from the point cloud. The key point of this step is to compare and match the known drill arm geometric model with the points in the actual point cloud.

[0072] Since the position and size of the drill boom are usually known and fixed, a specific area in the radar scan can be directly marked as the drill boom area. In this method, any point appearing within this area is considered as the point cloud of the drill boom.

[0073] Step A3-1, Fit the polygon model of the drill boom using the Quickhull algorithm: First, determine the reference line. Select the leftmost point and the rightmost point in the point set : (14) (15) Connect and , forming the initial convex hull boundary.

[0074] Step A3-2, Next, calculate the distance from the point to the line. For each point in the point set, calculate its distance to the reference line segment. The distance from a point to a line can be calculated using the following formula: (16) where and are the coordinates of the reference points and respectively.

[0075] Step A3-3, Then, find the point farthest from the reference line segment from all the points in the set and add it to the convex hull. Finally, connect all the points on the convex hull in order to obtain the complete boundary of the convex hull.

[0076] Step A3-4, Remove the point cloud through distance calculation: Calculate the distance from each point in the point cloud to the polygon according to the known polygon model. If a certain point is too far from the polygon, it is considered not to belong to the point cloud of the drill boom and can be retained. For each point , calculate its distance to the polygon model

[0077] (17) If , then it is considered that this point is not part of the drill boom and can be retained.

[0078] The above steps A3-1 to A3-4, and formulas (14) to (16), through fitting the polygon model of the drill boom, calculating the distance from the point to the line, obtaining the complete boundary of the convex hull, and removing the point cloud through distance calculation, achieve the detection and recognition of the drill boom point cloud in step A3.

[0079] The above steps A1 - A3 illustrate the process of removing the drill arm point cloud. The following provides an embodiment to further illustrate removing the drill arm point cloud, eliminating interference, and obtaining the point cloud of the drill jumbo operation environment.

[0080] In a possible implementation manner, obtaining the fused point cloud data as the environmental point cloud data of the drill jumbo operation environment includes: removing the point cloud data within a pre - determined drill arm area from the fused point cloud data to obtain the point cloud data with the drill arm removed; the drill arm area is determined based on the relative position relationship between each lidar and the drill arm; using the point cloud data with the drill arm removed as the environmental point cloud data of the drill jumbo operation environment.

[0081] It should be noted that after removing the drill arm area from the point cloud data collected at the current position of the drill rig, the data of the original drill arm area is missing. Since the drill rig is constantly moving and the radar field of view is different at different positions. The point cloud data collected at other times and other positions can make up for the occluded part of the drill arm area missing at the current position. Therefore, removing the drill arm area point cloud after fusion here can avoid including the drill arm as part of the environment in the point cloud data and will not cause the loss of environmental data.

[0082] Step 504: Based on the motion state data and the environmental point cloud data, perform drill jumbo positioning and mapping of the operation environment.

[0083] In a possible implementation manner, performing drill jumbo positioning and mapping of the operation environment based on the motion state data and the environmental point cloud data includes: using the FAST - LIO2 framework to fuse the motion state data and the environmental point cloud data to achieve drill jumbo positioning and mapping of the operation environment.

[0084] Using the FAST - LIO2 framework to implement the modeling of the environmental map, which is based on processing and fusing the point cloud obtained by lidar scanning and IMU data, so as to obtain a high - precision pose estimation. The specific steps are as follows: Step 5041: IMU pre - integration: The acceleration and angular velocity data of the IMU change rapidly in the time series, but they do not directly provide pose information in the global coordinate system. Therefore, FAST - LIO2 converts the IMU data into relative pose changes through pre - integration, and this process can be represented by the following formula: (18) Among them, represents the pose of the robot or vehicle, is the velocity, is the IMU acceleration, is the gravity vector, is the time increment. The above formula (18) converts IMU data into relative pose changes through pre-integration.

[0085] Step 5042, Point cloud registration: FAST-LIO2 estimates the pose of the robot through local features of the point cloud and uses the ICP algorithm for pose estimation.

[0086] Step 5043, Extended Kalman Filter (EKF): In FAST-LIO2, IMU and LiDAR data are fused through the Extended Kalman Filter (EKF). It uses the acceleration and angular velocity data provided by the IMU to predict the pose of the system according to the motion model. The prediction of the IMU is corrected by the matching between the LiDAR point cloud and the map to update the pose estimation. The state vector contains parameters such as the position, attitude, velocity, and sensor bias of the robot. The state model is set as: (19) where, is the current position information of the robot, is the current attitude of the robot, is the velocity of the robot, is the bias of the IMU. EKF performs pose estimation through two stages (prediction and update).

[0087] Step B-1, Prediction stage: In the prediction stage, EKF uses the data provided by the IMU (acceleration and angular velocity) to predict the state at the current moment. First, perform state prediction (20) represents the predicted state at the current moment, represents the state transition function of the system, which updates the state based on IMU data, represents the predicted state at the previous moment, is the control input, which is the acceleration and angular velocity provided by the IMU. Finally, covariance prediction is realized, and the covariance matrix needs to be predicted according to the non-linear model of state transition. The linearization of state transition is achieved by calculating the Jacobian matrix: (21) where, is the Jacobian matrix of the state transition function, representing the linearization of the system. is the state covariance matrix at time k-1. is the process noise covariance matrix, representing the variance of the prediction error.

[0088] Step B-2, Update Phase: In the update phase, the EKF updates the predicted state through LiDAR point cloud observations. At this time, the LiDAR data provides the observation information, and the predicted state is corrected by calculating the observation residual. First, calculate the Kalman gain : (22) where is the Jacobian matrix of the observation equation, which describes the relationship between the state and the observation. is the covariance matrix of the observation noise. Secondly, update the state (23) is the observed value, is the predicted observed value.

[0089] Finally, update the covariance. The updated covariance represents the accuracy of the estimation. The update formula is: (24) The above formulas (19) to (24) fuse the IMU and LiDAR data through the extended Kalman filter, that is, the fusion of inertial data and LiDAR point cloud data, so as to obtain a high-precision pose estimation.

[0090] Step 5044, Map Update and Optimization: FAST-LIO2 updates the local map by using the current and historical point cloud data through a sliding window method. As new scan data arrives, the system continuously optimizes and updates the map to ensure the accuracy of the map.

[0091] In the embodiment of the present invention, the point cloud data in front of the drill rig is collected by each of the inverted lidars, effectively expanding the scanning range of the lidar and reducing the occlusion of the vehicle body and the drill arm on the radar scanning area, ensuring that the radar can capture the environmental information around and below the vehicle body. For any lidar, the point cloud data is transformed into the drill rig coordinate system through the coordinate transformation matrix, ensuring that the point cloud data collected from different perspectives can be accurately corresponded to the same coordinate system, providing a basis for subsequent data fusion and processing. The transformed point cloud data of two lidars from different perspectives are subjected to point cloud registration and fusion processing to compensate for the point cloud data in the occluded area, generating a complete three-dimensional environmental model and improving the integrity and accuracy of environmental perception.

[0092] In a possible implementation, after each of the inverted laser radars collects point cloud data in front of the drilling vehicle, it also includes: for each laser radar, obtaining a drill arm area in the point cloud data of the laser radar; the drill arm area is determined based on the relative position relationship between the laser radar and the drill arm; and the data in the drill arm area in the point cloud data of the laser radar is eliminated to obtain the point cloud data of the laser radar excluding the drill arm area.

[0093] It should be noted that for each laser radar, the drill arm area in the laser radar point cloud data is obtained. This drill arm area is predetermined based on the relative position relationship between the laser radar and the drill arm. Since the position of the drill arm is fixed relative to the vehicle body, it is possible to calculate which areas in the point cloud data correspond to the drill arm based on the installation position and angle of the laser radar. The data belonging to the drill arm area in the laser radar point cloud data is removed. The purpose of this is to prevent the point cloud data of the drill arm from being mistaken for part of the environment, thereby avoiding interference with subsequent environmental mapping and positioning.

[0094] Correspondingly, the step of transforming the point cloud data into the drilling vehicle coordinate system through the coordinate transformation matrix to obtain the transformed point cloud data of the laser radar includes: transforming the point cloud data excluding the drill arm area into the drilling vehicle coordinate system through the coordinate transformation matrix to obtain the transformed point cloud data of the laser radar.

[0095] It should be noted that after removing the data of the drill arm area, the remaining point cloud data is transformed into the coordinate system of the drill vehicle through the coordinate transformation matrix. This ensures that all lidar data are unified into the same coordinate system, which is convenient for subsequent point cloud registration and fusion processing.

[0096] In a possible implementation, before obtaining the drill arm area in the laser radar point cloud data for each laser radar, the method further includes: determining a geometric model of the drill arm based on the shape, size and position of the drill arm in the drilling vehicle coordinate system; obtaining the point cloud data of the laser radar for each laser radar; matching the geometric model of the drill arm with the point cloud data, and extracting the point cloud covered by the drill arm geometric model; and fitting the convex edge of the drill arm by the Quickhull algorithm based on the point cloud covered by the drill arm geometric model as the drill arm area in the laser radar point cloud data.

[0097] It should be noted that based on the shape, size, and position of the drill boom in the drill rig coordinate system, the geometric model of the drill boom is determined in advance. This step obtains the geometric information of the drill boom, including position, size, and shape, through precise measurement or CAD design. The geometric model can be a three-dimensional model for subsequent point cloud matching. For each lidar, the point cloud data collected by it is obtained. These data contain the environmental information scanned by the lidar, including the point cloud data of the drill boom. The geometric model of the drill boom is matched with the point cloud data collected by the lidar. Through the matching, it can be determined which point cloud data belongs to the area of the drill boom. This step is usually achieved through feature point matching or geometric shape matching. Through the matching process, the point cloud data covered by the geometric model of the drill boom is extracted. These point cloud data correspond to the position of the drill boom in the radar view. The Quickhull algorithm is used to fit the point cloud covered by the drill boom to obtain the convex edge of the drill boom. The convex edge represents the boundary of the drill boom in the radar view, thereby determining the drill boom area.

[0098] It should be further noted that the geometric model of the drill boom is established based on its position and shape in the drill rig coordinate system. This step ensures the accuracy of the model and its consistency with the actual drill boom. The point cloud data collected by the lidar contains the information of all objects in the environment, including the drill boom. These data need to be further processed to identify the area of the drill boom. By matching the geometric model with the point cloud data, it can be accurately identified which point cloud data belongs to the drill boom. This step utilizes the fixed position and shape information of the drill boom. The extracted point cloud data is the specific manifestation of the drill boom in the radar view, providing a basis for subsequent convex edge fitting. The Quickhull algorithm is used to fit the convex edge of the drill boom, ensuring the accurate identification of the drill boom area. Convex edge fitting can effectively define the boundary of the drill boom and avoid misidentification.

[0099] The following uses a comprehensive embodiment to illustrate the steps of the localization and mapping method. This embodiment takes a drill rig and a solid-state lidar as objects. The method of simultaneous localization and mapping applicable to the drill rig based on the solid-state lidar is used to map the environment. The specific steps are as follows: The first step is to determine the model of the drill rig, the model of the solid-state lidar, and its installation position and installation method. For example, the drill rig model is a rock drilling jumbo, and the solid-state lidar is a livox mid-360.

[0100] The second step is to use the solid-state lidar to collect the point cloud information of the test site and store the collected point cloud results to prepare for the subsequent data processing.

[0101] The third step is to implement point cloud flipping processing. The point cloud data collected by the lidar in the flipped state is restored to the vehicle coordinate system through coordinate transformation. Figure 6 It is the point cloud map of the lidar in the upright state provided by the embodiment of the present invention; Figure 7It is the point cloud map of the lidar flip provided by the embodiment of the present invention; refer to Figure 6 and Figure 7 , the scanning range increases after the lidar is flipped.

[0102] Fourth step, realize point cloud fusion processing. Unify the point cloud data generated by different radars into the same coordinate system, eliminate redundant data, and form a global three-dimensional environment model. Figure 8 It is the left radar point cloud map provided by the embodiment of the present invention; Figure 9 It is the right radar point cloud map provided by the embodiment of the present invention; Figure 10 It is the radar point cloud map after point cloud fusion processing provided by the embodiment of the present invention. Refer to Figures 8 to 10 , after point cloud fusion, the influence of vehicle body occlusion is reduced.

[0103] Fifth step, drill arm point cloud rejection processing. First, establish a drill arm model and realize point cloud classification, and then perform point cloud detection and rejection on the drill arm. Figure 11 It is the radar point cloud map before the drill arm is rejected provided by the embodiment of the present invention; Figure 12 It is the radar point cloud map after the drill arm is rejected provided by the embodiment of the present invention. Refer to Figure 11 and Figure 12 , the radar point cloud map after the drill arm is rejected avoids taking the drill arm as part of the environment and improves the mapping accuracy.

[0104] Sixth step, positioning and mapping. Use the FAST-LIO2 framework to realize the modeling of the environment map, which is based on the processing and fusion of the point cloud and IMU data obtained by lidar scanning, so as to obtain a high-precision pose estimation. Figure 13 It is the schematic diagram of the mapping result provided by the embodiment of the present invention. Refer to Figure 13 , showing the mapping result of the underground mine roadway.

[0105] Exemplarily, the above processing and experiments all use the Linux 18.04 operating system and the ROS melodic operating system, and the relevant experimental results are all displayed in rviz.

[0106] The present invention provides a simultaneous localization and mapping method based on a solid-state lidar for a rock drilling jumbo. This method first builds a data acquisition and processing platform, which includes a drill rig, a solid-state lidar, and an IMU. The key part of this section is to determine the installation position of the solid-state lidar. Considering that the maximum upward angle of the LIVOX mid-360 lidar used is 52 degrees, while the maximum downward angle is only 7 degrees. If the lidar is installed upright on the vehicle roof, most areas near the vehicle body cannot be effectively captured. Therefore, the lidar is installed upside down on the vehicle roof and upside-down point cloud processing is realized. This installation method can avoid the occlusion of the space on the vehicle roof and ensure that the lidar effectively scans the area around the vehicle body, thereby improving the comprehensiveness and accuracy of environmental perception.

[0107] The present invention uses the point cloud upside-down method to achieve a comprehensive perception of the environment. In practical applications, considering that it is mainly necessary to scan the front scene, and installing the lidar at the bottom of the vehicle may cause problems of collision with the ground. At the same time, installing the lidar on the drill arm will affect the rock drilling and other operations of the drill jumbo. Therefore, it is selected to install the lidar in the front at the height of the vehicle body. In this way, it can not only ensure the scanning of the front environment but also not affect the operation of the drill jumbo.

[0108] Then, due to the partial occlusion of the drill arm of the vehicle body, the acquisition of spatial point clouds will be missing. Therefore, multiple lidars are used for data compensation. Thus, lidars are installed on both the left and right sides of the vehicle body to supplement the areas occluded by the drill arm and realize point cloud fusion processing to ensure the integrity of environmental perception.

[0109] When the lidar is installed on the vehicle body, the presence of the drill arm will inevitably affect the point cloud acquisition of the lidar. No matter how the scanning position of the lidar changes, the drill arm will appear as a static object in the point cloud data and will thus be wrongly incorporated into the process of constructing the environmental map. Considering that the drill arm part will be incorporated into the environmental mapping during the lidar scanning, affecting the accuracy of the map, the point cloud data of the drill arm is identified and removed. Finally, the slam algorithm is used to realize the construction of a complete map for subsequent processing.

[0110] It should be understood that the magnitudes of the sequence numbers of the steps in the above embodiments do not mean the order of execution. The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation to the implementation process of the embodiments of the present invention.

[0111] The following is the device embodiment of the present invention. For the details not described in detail therein, reference can be made to the corresponding method embodiments above.

[0112] Figure 14 The structural schematic diagram of the positioning and mapping device for a drill jumbo provided by the embodiment of the present invention is shown. For the sake of convenience of description, only the parts related to the embodiment of the present invention are shown and are described in detail as follows: AsFigure 14 As shown in the figure, an embodiment of the present invention provides a positioning and mapping device 14 for a drilling jumbo, which is applied to the positioning and mapping system for a drilling jumbo described in any one of the above; the device includes: An acquisition module 141, configured to acquire point cloud data in front of the drilling jumbo through each of the inverted lidars, and acquire the motion state data of the drilling jumbo through an inertial measurement module; A coordinate transformation module 142, configured to, for any lidar, transform the point cloud data to the drilling jumbo coordinate system through a coordinate transformation matrix to obtain the transformed point cloud data of the lidar; A fusion module 143, configured to perform point cloud registration and fusion processing on the transformed point cloud data of two lidars with different perspectives, compensate the point cloud data in the occluded area, and obtain the fused point cloud data as the environmental point cloud data of the drilling jumbo operation environment; A positioning and mapping module 144, configured to perform positioning of the drilling jumbo and mapping of the operation environment based on the motion state data and the environmental point cloud data.

[0113] Figure 15 is a schematic diagram of an electronic device provided by an embodiment of the present invention. As Figure 15 shown, the electronic device 15 in this embodiment includes: a processor 150 and a memory 151. The memory 151 stores a computer program 152. When the processor 150 executes the computer program 152, the steps in each of the above method embodiments are implemented. Alternatively, when the processor 150 executes the computer program 152, the functions of each module / unit in each of the above device embodiments are implemented.

[0114] Exemplarily, the computer program 152 can be divided into one or more modules / units, and the one or more modules / units are stored in the memory 151 and executed by the processor 150 to complete the present invention. The one or more modules / units can be a series of computer program instruction segments capable of performing specific functions, and the instruction segments are used to describe the execution process of the computer program 152 in the electronic device 15.

[0115] The electronic device 15 may include, but is not limited to, a processor 150 and a memory 151. Those skilled in the art can understand that Figure 15 this is only an example of the electronic device 15 and does not constitute a limitation on the electronic device 15. It may include more or fewer components than shown in the figure, or combine certain components, or different components. For example, the electronic device 15 may further include input / output devices, network access devices, buses, etc.

[0116] The processor 150 may be a central processing unit (CPU), or may also be other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0117] The memory 151 may be an internal storage unit of the electronic device 15, such as a hard disk or memory of the electronic device 15. The memory 151 may also be an external storage device of the electronic device 15, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc. equipped on the electronic device 15. Further, the memory 151 may also include both the internal storage unit and the external storage device of the electronic device 15. The memory 151 is used to store the computer program 152 and other programs and data required by the electronic device 15. The memory 151 may also be used to temporarily store the data that has been output or is to be output.

[0118] For the convenience and simplicity of description, only the above division of each functional module / unit is used as an example for illustration. In practical applications, the above functions may be allocated to different functional modules / units according to needs. The above modules / units may be implemented in the form of hardware, or may be implemented in the form of software, or may be implemented in the form of a combination of hardware and software.

[0119] The embodiment of the present invention also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the methods in the above method embodiments are implemented.

[0120] The embodiment of the present invention also provides a computer program product, including a computer program. When the computer program is executed by a processor, the methods in the above method embodiments are implemented.

[0121] Among them, the computer program includes computer program code, and the computer program code can be in the form of source code, object code, executable file or some intermediate form, etc. The computer-readable medium can include: any entity or device capable of carrying computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disc, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal, and software distribution medium, etc.

[0122] In the above embodiments, the descriptions of each embodiment have their own emphases. For the parts not detailed or recorded in a certain embodiment, reference can be made to the relevant descriptions of other embodiments. Without special instructions and logical conflicts, the terms and / or descriptions among different embodiments are consistent and can be referenced to each other. The technical features in different embodiments can be combined to form new embodiments according to their inherent logical relationships.

[0123] The above-described embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or equivalently replace some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of each embodiment of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A positioning and mapping system for a drilling jumbo, characterized in that The roadheader includes a boom disposed at the middle position in front of the roadheader; the system includes: an inertial measurement module, a control module, and two lidars; In the vertical field of view angle of the lidar, the field of view angle above the horizontal plane is greater than the field of view angle below the horizontal plane; The two lidars are used to be suspended and installed upside down in front of the top of the roadheader. Among them, one lidar is installed at the left front of the top of the roadheader, and the other lidar is installed at the right front of the top of the roadheader; the installation height of the lidar is higher than the height of the boom; The lidar is used to collect environmental point cloud data of the operation environment of the roadheader; The inertial measurement module is used to collect the motion state data of the roadheader; Based on the motion state data and the environmental point cloud data, the control module performs positioning of the roadheader and mapping of the operation environment.

2. The positioning and mapping system for a jumbo drill according to claim 1, characterized in that The lidar includes a solid-state lidar.

3. A positioning and mapping method for a drilling jumbo, characterized in that, Applied to the positioning and mapping system for the roadheader described in any one of claims 1 to 2; the method includes: Collect the point cloud data in front of the roadheader through each of the upside-down lidars, and collect the motion state data of the roadheader through the inertial measurement module; For any one lidar, transform the point cloud data to the roadheader coordinate system through a coordinate transformation matrix to obtain the transformed point cloud data of the lidar; Perform point cloud registration and fusion processing on the transformed point cloud data of the two lidars from different perspectives, compensate the point cloud data in the occluded area, and obtain the fused point cloud data as the environmental point cloud data of the operation environment of the roadheader; Based on the motion state data and the environmental point cloud data, perform positioning of the roadheader and mapping of the operation environment.

4. The positioning and mapping method for a jumbo according to claim 3, characterized in that, The obtaining the fused point cloud data as the environmental point cloud data of the operation environment of the roadheader includes: Exclude the point cloud data within the pre-determined boom area in the fused point cloud data to obtain the point cloud data with the boom excluded; the boom area is determined based on the relative position relationship between each lidar and the boom; Use the point cloud data with the boom excluded as the environmental point cloud data of the operation environment of the roadheader.

5. The positioning and mapping method for a jumbo according to claim 3, characterized in that, After collecting the point cloud data in front of the roadheader through each of the upside-down lidars, it further includes: For each lidar, obtain the boom area in the point cloud data of the lidar; the boom area is determined based on the relative position relationship between the lidar and the boom; Exclude the data within the boom area in the point cloud data of the lidar to obtain the point cloud data of the lidar with the boom area excluded; Correspondingly, the transforming the point cloud data to the roadheader coordinate system through a coordinate transformation matrix to obtain the transformed point cloud data of the lidar includes: Transform the point cloud data with the boom area excluded to the roadheader coordinate system through a coordinate transformation matrix to obtain the transformed point cloud data of the lidar.

6. The positioning and mapping method for a jumbo according to claim 5, characterized in that, Before obtaining the boom area in the point cloud data of each lidar for each lidar, it further includes: Determine the geometric model of the boom based on the shape, size, and position of the boom in the roadheader coordinate system; For each lidar, obtain the point cloud data of the lidar; Match the geometric model of the boom with the point cloud data, and extract the point cloud covered by the geometric model of the boom; Based on the point cloud covered by the drill arm geometric model, the convex edge of the drill arm is fitted by the Quickhull algorithm and used as the drill arm area in the lidar point cloud data.

7. The positioning and mapping method for a jumbo according to claim 3, characterized in that, Based on the motion state data and the environmental point cloud data, the positioning of the drill jumbo and the mapping of the working environment include: Using the FAST-LIO2 framework, the motion state data and the environmental point cloud data are fused to achieve the positioning of the drill jumbo and the mapping of the working environment.

8. A positioning and mapping device for a drilling jumbo, characterized in that, Applied to the positioning and mapping system for a drill jumbo according to any one of claims 1 to 2; the device includes: An acquisition module for acquiring the point cloud data in front of the drill jumbo through each of the inverted lidars and acquiring the motion state data of the drill jumbo through an inertial measurement module; A coordinate transformation module for, for any lidar, transforming the point cloud data to the drill jumbo coordinate system through a coordinate transformation matrix to obtain the transformed point cloud data of the lidar; A fusion module for performing point cloud registration and fusion processing on the transformed point cloud data of two lidars from different perspectives, compensating the point cloud data in the occluded area, and obtaining the fused point cloud data as the environmental point cloud data of the drill jumbo working environment; A positioning and mapping module for positioning the drill jumbo and mapping the working environment based on the motion state data and the environmental point cloud data.

9. An electronic device, characterized in that, It includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements the positioning and mapping method for a drill jumbo according to any one of claims 3 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, it implements the positioning and mapping method for a drill jumbo according to any one of claims 3 to 7.

Images