A method for sensing obstacles of articulated vehicles in underground mines

Abstract

The present application discloses a method for perceiving obstacles of articulated vehicles in underground mines, which relates to the field of autonomous driving, including: obtaining laser radar point clouds of the articulated vehicle in the first direction and the second direction respectively as the first point cloud data and the second point cloud data; wherein the first direction is the forward direction of the center point of the articulated vehicle, and the second direction is the backward direction of the center point of the articulated vehicle; registering the first point cloud data and the second point cloud data; filtering the registered second point cloud data to obtain the point cloud data of the tunnel wall; extracting clustered point cloud clusters through Euclidean clustering based on the point cloud data of the tunnel wall, marking the clusters located in the grid of the tunnel wall and with a height less than the first height threshold as retaining walls, and marking the clusters located in the grid of the ground and with a height less than the second height threshold as low obstacles. In view of the problem of low accuracy in detecting obstacles when unloading backwards in articulated vehicles in underground mines in the prior art, the present application provides accurate perception of retaining walls and low obstacles in the tunnels of underground mines.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving, and in particular to a method for sensing obstacles of articulated vehicles in underground mines. Background Art

[0002] Articulated trucks are commonly used transportation equipment in underground mines, responsible for transporting ore and waste in narrow and complex tunnel environments. In order to ensure that articulated trucks can complete unloading operations safely and efficiently, it is necessary to accurately sense and locate obstacles behind the vehicle. At present, rear obstacle detection in mining areas is mainly used in open-pit mine environments. Image data is collected by on-board rear-facing cameras, and image processing algorithms are used to identify obstacles such as retaining walls. In areas with flat ground, the safe distance between the vehicle and obstacles can be judged more accurately.

[0003] However, the above methods face many difficulties and challenges in underground mining environments. On the one hand, the underground lighting is dim and the image quality is difficult to guarantee, so image-based recognition methods cannot stably and reliably detect obstacles. On the other hand, the retaining walls in underground mines are generally low in height and similar in material and color to the surrounding tunnel walls, making it difficult to accurately distinguish them even with intelligent recognition technologies such as machine learning. In addition, due to the harsh underground environment, the retaining wall structure is prone to deformation and presents an irregular, slender strip shape, which further increases the difficulty of target judgment and positioning.

[0004] Although LiDAR can obtain accurate three-dimensional spatial information, in the complex and ever-changing scenes underground, it is difficult for a single LiDAR to simultaneously ensure high accuracy of its own positioning and obstacle positions, and it is difficult to meet the needs of articulated vehicles for environmental perception. Summary of the invention

[0005] In response to the problem of low accuracy in obstacle detection when articulated trucks in underground mines are unloading backwards in the prior art, the present application provides a method for obstacle perception of articulated trucks in underground mines, which uses laser radars at the front and rear of the articulated truck to collect point cloud data respectively, obtains point clouds in different directions through time synchronization and aligns them, and then performs noise filtering, feature extraction, clustering and segmentation on the aligned point clouds, ultimately achieving accurate perception and identification of retaining walls and low obstacles in underground mine tunnels.

[0006] The purpose of this application is achieved through the following technical solutions.

[0007] The present application provides an obstacle perception method for an articulated vehicle in an underground mine, comprising: respectively obtaining a laser radar point cloud in a first direction and a second direction of the articulated vehicle as first point cloud data and second point cloud data; wherein the first direction is forward of a center point of the articulated vehicle, and the second direction is backward of the center point of the articulated vehicle; registering the first point cloud data and the second point cloud data; filtering the registered second point cloud data to obtain tunnel wall point cloud data; based on the tunnel wall point cloud data, extracting clustered point cloud clusters through Euclidean clustering, marking a cluster located in a tunnel wall grid and having a height less than a first height threshold as a retaining wall, and marking a cluster located in a ground grid and having a height less than a second height threshold as a low obstacle; wherein the first height threshold is greater than the second height threshold.

[0008] Among them, it refers to the forward direction of the articulated vehicle body, that is, the main driving direction of the articulated vehicle when it is moving. The forward direction is usually determined by sensors such as the inertial navigation system (INS) or odometer on the articulated vehicle. The laser radar in the first direction is installed at the front of the articulated vehicle, and the scanning range covers the area in front of the articulated vehicle, which is used to perceive the obstacles and environmental information in front. Rearward of the center point of the articulated vehicle: refers to the backward direction of the articulated vehicle body, that is, the reverse driving direction of the articulated vehicle when it is moving. The backward direction is opposite to the forward direction and points to the direction of the rear of the articulated vehicle. The laser radar in the second direction is installed at the rear of the articulated vehicle, and the scanning range covers the area behind the articulated vehicle, which is used to perceive the obstacles and environmental information in the rear.

[0009] Among them, the tunnel wall grid refers to dividing the tunnel environment into multiple grid-like areas, each grid representing a part of the tunnel wall. By mapping the point cloud data into the tunnel wall grid, the characteristics and obstacle information of the tunnel wall can be easily represented and analyzed. The size and resolution of the tunnel wall grid can be set according to actual needs and computing resources, such as a grid size of 0.5 meters x 0.5 meters. The ground grid refers to dividing the tunnel ground into multiple grid-like areas, each grid representing a part of the ground. By mapping the point cloud data into the ground grid, the characteristics and obstacle information of the ground can be easily represented and analyzed. The size and resolution of the ground grid can be set according to actual needs and computing resources, such as a grid size of 0.2 meters x 0.2 meters. Low obstacles refer to obstacles with a low height located on the ground, such as stones, fallen rocks, equipment parts, etc. The height of low obstacles is usually less than the second height threshold, such as less than 0.5 meters. Low obstacles have a certain impact on the driving and safety of articulated vehicles, and need to be detected and avoided. Through Euclidean clustering and height threshold judgment, low obstacles can be identified from point cloud clusters within the ground grid.

[0010] Furthermore, the first point cloud data and the second point cloud data are registered, including: taking the position of the articulation angle θ of the articulated vehicle as the origin, establishing a vehicle body coordinate system; obtaining a rotation matrix according to the articulation angle θ, and using the rotation matrix to perform a first registration on the first point cloud data and the second point cloud data; using the ICP algorithm, calculating the mean distance between each point in the second point cloud data after the first registration and the first point cloud data, if the distance mean is greater than a preset distance threshold , then increase the preset step length on both sides of the hinge angle θ , return to perform the first registration, calculate and select the group with the smallest mean distance as the second point cloud data after registration.

[0011] Among them, the articulation angle θ refers to the relative turning angle between the front and rear bodies of the articulated vehicle, indicating the relative posture of the front and rear bodies. The size of the articulation angle θ determines the steering degree and movement trajectory of the articulated vehicle. The articulation angle θ can be measured and obtained in real time by devices such as angle sensors or encoders on the articulated vehicle. The positive and negative values ​​of the articulation angle θ indicate the direction of steering. For example, θ>0 means that the front body turns right relative to the rear body, and θ<0 means that the front body turns left relative to the rear body. In the process of point cloud data registration, the articulation angle θ is used to calculate the rotation matrix to align the second point cloud data (rearward laser radar) to the coordinate system of the first point cloud data (forward laser radar).

[0012] Among them, the body coordinate system refers to a three-dimensional rectangular coordinate system established with the articulated vehicle itself as a reference, which is used to describe the relative position and posture of the various components and sensors of the articulated vehicle. The body coordinate system usually takes the articulation center of the articulated vehicle as the origin, the X-axis points to the forward direction of the articulated vehicle, the Y-axis points to the left side of the articulated vehicle, and the Z-axis is perpendicular to the X-axis and the Y-axis. The establishment of the body coordinate system can be achieved through navigation equipment such as INS and GPS on the articulated vehicle and the body structure parameters. In the process of point cloud data registration, both the first point cloud data and the second point cloud data are converted to the body coordinate system to achieve unified representation and processing of laser radar data in two directions. Through the body coordinate system, the position and posture of obstacles such as retaining walls and low obstacles relative to the articulated vehicle can be easily described, providing a reference for obstacle avoidance decisions and path planning. By using the articulation angle θ to calculate the rotation matrix, the second point cloud data (backward laser radar) can be aligned to the coordinate system of the first point cloud data (forward laser radar) to achieve preliminary registration of laser radar data in two directions. Then, the ICP algorithm is used to further optimize the registration result, calculate the mean distance between each point in the second point cloud data and the first point cloud data, and minimize the distance mean by adjusting the step size of the articulation angle θ to obtain the optimal registration result.

[0013] Furthermore, the rotation matrix expression is as follows: .

[0014] Furthermore, the second point cloud data after the registration is filtered, including: based on the second point cloud data after the registration, using a straight-through filtering algorithm to filter points whose distances are greater than a preset distance threshold to obtain the second point cloud data after the first filtration; based on the second point cloud data after the first filtration, using a statistical filtering algorithm to filter outliers to obtain the second point cloud data after the second filtration; based on the preset articulated vehicle front size, using a straight-through filtering algorithm to divide the second point cloud data after the second filtration into vehicle front point cloud data and non-vehicle front point cloud data; performing Euclidean clustering on the non-vehicle front point cloud data, using the position of the articulation angle θ as the sphere center, and the preset minimum obstacle height as the sphere radius, to establish a spherical bounding box, and using the point cloud data located within the spherical bounding box after clustering as the filtered second point cloud data.

[0015] Among them, the pass-through filtering algorithm (Pass Through Filter): Pass-through filtering is a simple and effective point cloud data filtering method, which is used to delete unnecessary points according to the position or distance of the point cloud on a certain coordinate axis. The pass-through filter removes points outside the threshold range from the point cloud by setting a threshold range on one or more coordinate axes. In the scheme, the pass-through filtering algorithm is first used to filter points whose distance is greater than the preset distance threshold, remove irrelevant point cloud data far away from the articulated vehicle, and obtain the second point cloud data after the first filtration. Then, the pass-through filtering algorithm is used to divide the second point cloud data after the second filtration into vehicle head point cloud data and non-vehicle head point cloud data according to the preset articulated vehicle head size, which is convenient for subsequent processing.

[0016] Statistical Outlier Removal Filter: Statistical filtering is a filtering method used to remove outliers from point clouds. The statistical filtering algorithm calculates the distance distribution between each point and its neighborhood points, marks points whose distance exceeds a certain standard deviation range as outliers, and removes them from the point cloud. In specific implementation, the statistical filtering algorithm usually needs to set two parameters: the number of neighborhood points K and the standard deviation multiple n. For each point, the algorithm calculates the average distance between it and the K nearest points in the neighborhood, and regards points whose distance is greater than the average distance + n times the standard deviation as outliers. In the scheme, the statistical filtering algorithm is used to remove outliers in the second point cloud data after the first filtering, thereby improving the quality and reliability of the point cloud data.

[0017] Articulated truck head: refers to the body structure at the front of the articulated truck, usually including the cab, power system, working device and other components. The front of the articulated truck is connected to the rear compartment or trailer through an articulation device, and can rotate relative to each other to form an articulation angle. In point cloud data processing, it is necessary to distinguish the point cloud data of the articulated truck itself from the point cloud data of obstacles in the environment according to the preset size of the articulated truck head. Through the straight-through filtering algorithm, the point cloud within the range of the articulated truck head can be marked as the head point cloud data according to the position of the point cloud in the vehicle body coordinate system, and the remaining point clouds are marked as non-head point cloud data. Distinguishing between head point cloud data and non-head point cloud data is helpful for subsequent obstacle perception and obstacle avoidance decisions, and avoids misdetecting the articulated truck itself as an obstacle. Through the straight-through filtering and statistical filtering algorithms, the irrelevant points and outliers in the second point cloud data after registration can be effectively removed to improve the quality and reliability of the point cloud data. Then, according to the preset front size of the articulated vehicle, the filtered point cloud data is divided into front point cloud data and non-front point cloud data to facilitate subsequent obstacle perception. The non-front point cloud data is clustered in an Euclidean manner, and the position of the articulation angle θ is used as the sphere center, the preset minimum obstacle height is used as the sphere radius, and the sphere bounding box is established. The point cloud clusters located near the articulated vehicle and meeting the height requirements can be further screened out as potential obstacle candidate areas.

[0018] Furthermore, according to the second point cloud data after the first filtering, a statistical filtering algorithm is used to filter outliers, including: calculating the average distance between each point in the second point cloud data after the first filtering and the nearest neighbor point as the neighborhood average distance of the corresponding point; wherein the number of the nearest neighbor points is k; according to the preset distance threshold , calculate the outlier coefficient of each point in the second point cloud data after the first filtering. The outlier coefficient is the average distance and the distance threshold The ratio of outlier coefficient greater than the threshold The points are marked as outliers.

[0019] Furthermore, according to the preset size of the front of the articulated truck, the second point cloud data after the second filtering is divided into front point cloud data and non-front point cloud data through a straight-through filtering algorithm, including: setting the length L, width W and height H of the front of the articulated truck, which respectively represent the maximum size of the front of the articulated truck in the forward direction, lateral direction and vertical direction; taking the center of the rear axle of the articulated truck as the origin O, establishing the articulated truck coordinate system O-XYZ, wherein the positive direction of the X-axis is the forward direction of the articulated truck, the positive direction of the Y-axis is the lateral direction of the left side of the articulated truck, and the positive direction of the Z-axis is the vertical direction of the articulated truck; in the articulated truck coordinate system O-XYZ, taking the origin O as the center, construct a cuboid with a length of L, a width of W and a height of H as the bounding box of the front of the articulated truck, and mark the vertex coordinates of the bounding box as and ; Judge each point in the second point cloud data after secondary filtering point by point , if it satisfies: , , ; then the point It is divided into the front point cloud dataset, otherwise it is divided into the non-front point cloud dataset.

[0020] Furthermore, the non-vehicle head point cloud data is subjected to Euclidean clustering, and the point cloud data located in the sphere bounding box after clustering is used as the second point cloud data after filtering, including: constructing a kinematic model of the articulated vehicle, and according to the motion state of the articulated vehicle, calculating the articulation center position coordinates (x, y, z) corresponding to the articulation angle θ at the current moment by solving the kinematic model; taking the articulation center position coordinates (x, y, z) as the sphere center, and setting the minimum height of the obstacle to the preset obstacle is the radius of the sphere, construct a sphere, use the sphere as the spatial range of obstacle clustering, and mark the vertex coordinates of the sphere bounding box as and ; with preset cluster radius and the minimum number of cluster points As a parameter, the Euclidean distance is used to perform spatial clustering on the non-vehicle head point cloud data to obtain multiple point cloud clusters; each point cloud cluster obtained is traversed to calculate the center coordinates of each cluster And the number of points in the cluster n, if it satisfies , , and n, the corresponding cluster is taken as the candidate obstacle point cloud; all candidate obstacle point clouds are taken as the filtered second point cloud data set.

[0021] Among them, the kinematic model is a mathematical model that describes the relationship between the motion state and parameters of a mechanical system, and is used to analyze and predict the motion characteristics of a mechanical system, such as position, velocity, and acceleration. In the kinematic modeling of an articulated vehicle, the geometric structure, articulation angle, wheel speed and other parameters of the articulated vehicle are usually considered to establish the motion constraint relationship between the various components of the articulated vehicle. The kinematic model of an articulated vehicle is usually based on the following assumptions and simplifications: The front and rear bodies of the articulated vehicle can be simplified as two rigid bodies connected by an articulated device. The tires of the articulated vehicle meet the pure rolling condition with the ground, and factors such as tire side deviation and slip are not considered. During the movement of the articulated vehicle, parameters such as the articulation angle and wheel speed can be measured and obtained in real time by sensors. The kinematic model of an articulated vehicle usually includes the following main parts: Geometric relationship: describes the geometric constraints between the front and rear bodies of the articulated vehicle, such as the articulation center position, articulation angle, etc. Velocity relationship: describes the relationship between the velocities of the various components of the articulated vehicle, such as the relationship between the wheel speed and the vehicle body speed. Acceleration relationship: describes the relationship between the accelerations of the various components of the articulated vehicle, and considers the dynamic characteristics of the articulated vehicle. By solving the kinematic model of the articulated vehicle, the coordinates (x, y, z) of the articulated center position of the articulated vehicle can be calculated in real time, which is used to construct a spherical bounding box for obstacle clustering. The kinematic model is usually solved by numerical integration or analytical solution. According to the real-time state of the articulated vehicle (such as articulation angle, wheel speed) and initial conditions, the position and posture of the articulated vehicle at a future moment are predicted. The kinematic model of the articulated vehicle is an important basis for autonomous navigation, path planning and obstacle avoidance decision-making. The kinematic model can be used to predict the motion trajectory of the articulated vehicle, and combined with the obstacle perception information, a suitable control strategy can be formulated.

[0022] Further, the tunnel wall point cloud data includes: projecting the filtered second point cloud data to a preset size In the grid map, count the distribution of point cloud data in each grid in the Z-axis direction; set a sliding window to count whether the number of points in the window is greater than the threshold. If the number of points in the preset number of consecutive frames is greater than the preset point threshold, mark the corresponding grid as a lane wall grid; project the filtered second point cloud data to a preset size In the grid map, the maximum and minimum point cloud heights of each grid are counted, and the grids whose difference between the maximum and minimum values ​​is less than the preset height threshold h1 are marked as obstacle grids; wherein, Less than ; The Patchwork algorithm is used to segment the filtered second point cloud data to obtain ground point cloud data and non-ground point cloud data; when the distance between the articulated vehicle and the obstacle grid is less than the preset distance, the segmented ground point cloud data is filtered according to the preset ground height threshold, and the points with a height greater than the ground height threshold are filtered, and the filtered ground point cloud is used as the final ground point cloud; the non-ground point cloud data in the obstacle grid is used as the tunnel wall point cloud data.

[0023] Among them, the grid map is a map representation method that divides the environment space into regular grids, which is usually used for robot navigation and environmental perception. In the grid map, the environment space is divided into square or rectangular grids of equal size, and each grid is called a cell. The grid map represents the attributes of the area by assigning values ​​to each grid, such as occupied state, free state, unknown state, etc. The resolution of the grid map depends on the size of the grid. The smaller the grid, the higher the resolution of the map and the more detailed the representation of the environment, but the greater the computational and storage overhead. In the scheme, the filtered second point cloud data is projected onto two different sizes ( and ) in the raster map to analyze the distribution of point cloud data and extract features such as tunnel walls and obstacles.

[0024] The roadway wall grid is a grid marked as a roadway wall in the grid map, indicating that the grid area belongs to the wall or boundary of the roadway. In the scheme, by setting a sliding window and counting the number of point cloud data in the window, if the number of points in the preset number of consecutive frames is greater than the preset point threshold, the corresponding grid is marked as a roadway wall grid. The extraction of roadway wall grids can help articulated vehicles identify the boundaries and directions of the roadway, and provide a reference for autonomous navigation and path planning.

[0025] The obstacle grid is a grid marked as an obstacle in the grid map, indicating that there are obstacles or potential collision risks in the grid area. The grid map is generated, and the maximum and minimum values ​​of the point cloud height in each grid are counted. If the difference between the maximum and minimum values ​​is less than the preset height threshold , then the grid is marked as an obstacle grid. The extraction of obstacle grids can help articulated vehicles identify obstacles in the environment and formulate obstacle avoidance strategies and path planning based on distance information.

[0026] The Patchwork algorithm is an algorithm for point cloud data segmentation, especially for the extraction and segmentation of ground point clouds. The Patchwork algorithm divides the point cloud into multiple local planes (patches) and determines whether they belong to the same plane by the similarity and continuity between the planes. The main steps of the algorithm include: dividing the point cloud into multiple local planes (patches), usually using methods such as region growing or random sampling. Calculate the normal vector and height of each local plane, and determine whether adjacent planes belong to the same plane based on the angle and height difference between the normal vectors. By iteratively merging similar local planes, the segmented ground point cloud and non-ground point cloud are finally obtained. The Patchwork algorithm has high computational efficiency and robustness, can effectively extract ground point clouds, and can achieve good segmentation effects even in complex environments. In the scheme, the Patchwork algorithm is used to segment the filtered second point cloud data to obtain ground point cloud data and non-ground point cloud data, which provides a basis for subsequent ground point cloud filtering and tunnel wall point cloud extraction.

[0027] Furthermore, the Patchwork algorithm is used to segment the filtered second point cloud data to obtain ground point cloud data and non-ground point cloud data, including: dividing the filtered second point cloud data into multiple plane blocks, each plane block containing a preset number of point cloud data; calculating the normal vector and height of each plane block; according to a preset ground normal vector threshold and a ground height threshold, marking the plane block whose normal vector and Z-axis angle is less than the ground normal vector threshold and whose height is less than the ground height threshold as a ground plane block; using the point cloud data in the ground plane block as the initial ground point cloud data, and using the point cloud data outside the ground plane block as the non-ground point cloud data; through the region generation method, using the initial ground point cloud data as the seed point, merging the points in the adjacent non-ground point cloud data that meet the preset ground features into the ground point cloud data to obtain the final ground point cloud data; using the point cloud data outside the final ground point cloud data as the non-ground point cloud data.

[0028] Among them, the normal vector is a unit vector perpendicular to the plane, indicating the direction and posture of the plane. In point cloud processing, the normal vector of the plane is usually calculated by fitting the points on the plane. The height is the coordinate value of each point on the Z axis (vertical direction) in the point cloud data, indicating the vertical position of the point. In the Patchwork algorithm, by calculating the normal vector and height of each plane block, it can be determined whether the plane block meets the characteristics of the ground, that is, the normal vector is close to vertical upward (small angle with the Z axis) and the height is low.

[0029] The normal vector and the Z axis refer to the angle relationship between the normal vector of the plane and the Z axis (vertical direction). In the Patchwork algorithm, by setting the ground normal vector threshold, it is determined whether the angle between the normal vector of the plane block and the Z axis is less than the threshold. If the angle between the normal vector of the plane block and the Z axis is small, it means that the direction of the plane block is close to the horizontal and is more likely to belong to the ground. The ground point cloud is a collection of points belonging to the ground or road surface in the point cloud data, usually with similar height and normal vector features. In the Patchwork algorithm, by setting the ground normal vector threshold and the ground height threshold, the point cloud data in the plane block that meets the conditions is marked as the initial ground point cloud data. Then, through the region generation method, the points that meet the ground characteristics in the adjacent non-ground point cloud data are merged into the ground point cloud data to obtain the final ground point cloud data.

[0030] Non-ground point cloud is a collection of points in point cloud data that do not belong to the ground or road surface, usually including point cloud data of obstacles, buildings, trees and other objects. In the Patchwork algorithm, point cloud data that does not belong to the ground plane block is initially marked as non-ground point cloud data. In the region generation process, point cloud data that does not meet the ground characteristics is retained in the non-ground point cloud data, and finally a complete non-ground point cloud data is obtained.

[0031] The region generation method is a point cloud segmentation algorithm that selects initial seed points and gradually merges adjacent points into the same area according to certain criteria to ultimately achieve point cloud segmentation. In the Patchwork algorithm, the region generation method is used to use the initial ground point cloud data as seed points, and merge points in adjacent non-ground point cloud data that meet ground features into the ground point cloud data. The merging criteria of the region generation method usually include the distance between points, normal vector differences, height differences, etc., and the merging conditions are controlled by setting appropriate thresholds. Through the region generation method, the initial ground point cloud data can be effectively expanded to a larger area, while ensuring that the merged points meet the ground feature requirements, thereby improving the accuracy of ground point cloud segmentation.

[0032] Furthermore, before obtaining the first point cloud and the second point cloud, the method further includes: obtaining the first point cloud and the second point cloud at a preset time interval according to the timestamp of the laser radar point cloud data in the first direction. Get the second direction LiDAR point cloud data within the preset time interval If the second direction laser radar point cloud data is obtained, the first direction laser radar point cloud data and the second direction laser radar point cloud data are time synchronized as the obtained first point cloud and second point cloud.

[0033] Compared with the prior art, the advantages of this application are:

[0034] By synchronously acquiring and registering the laser radar point cloud data in different directions, the temporal and spatial consistency of the data is ensured, overcoming the error caused by the articulated vehicle's own motion. The body coordinate system is established using the articulation angle of the articulated vehicle, and point cloud registration is achieved through the rotation matrix and ICP algorithm, which takes into account both the motion characteristics of the articulated vehicle and the registration accuracy.

[0035] In view of the characteristics of the underground mine tunnel environment, a multi-stage point cloud filtering algorithm was designed. Noise points at a long distance were removed by direct filtering, outliers were eliminated by statistical filtering, the front point cloud was extracted according to the geometric dimensions of the articulated vehicle, and the obstacle clustering range was constructed with the articulation center as the sphere center, which effectively improved the stability and reliability of obstacle perception.

[0036] On the basis of establishing a grid map, the location of the tunnel wall is determined by analyzing the distribution characteristics of the point cloud in the vertical direction, and the stability is determined by combining continuous multi-frame data, which can effectively deal with the difficulty of tunnel wall identification in complex environments. At the same time, the ground point cloud is segmented by the Patchwork algorithm, and the ground height threshold is dynamically updated according to the ground characteristics, which can adapt to the uneven road surface underground.

[0037] The Euclidean clustering algorithm is used to extract obstacles from non-vehicle head point clouds. By analyzing the geometric and topological features of clusters and combining the preset height threshold, tunnel wall and ground grid information, the retaining wall and low obstacles are accurately classified. This method does not rely on the material and appearance characteristics of the obstacles and has strong robustness.

[0038] The kinematic constraints of the articulated vehicle are fully utilized in the perception algorithm, and obstacle perception is closely integrated with the vehicle motion state through vehicle body coordinate system transformation, rotation matrix solution, articulation angle tracking, etc. At the same time, the proposed method has low computational complexity, can meet real-time requirements, and is easy to deploy and implement in the embedded system of the articulated vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The present application will be further described in the form of exemplary embodiments, which will be described in detail by the accompanying drawings. These embodiments are not restrictive, and in these embodiments, the same number represents the same structure, wherein:

[0040] Figure 1 is an exemplary flow chart of a method for sensing obstacles of an articulated vehicle in an underground mine according to the present application;

[0041] Figure 2 is an exemplary flow chart of point cloud registration according to the present application;

[0042] Figure 3 is an exemplary flow chart of point cloud filtering according to the present application;

[0043] Figure 4 It is based on the roadway wall grid judgment schematic diagram shown in this application;

[0044] Figure 5 is an exemplary flow chart for calculating the tunnel wall and low target according to the present application; DETAILED DESCRIPTION

[0045] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0046] Aiming at the problem of poor fusion effect of multiple laser radars in articulated vehicles, this application proposes a rearward perception algorithm for articulated vehicles in underground mines, such as Figure 1 As shown, the laser radar point clouds of the articulated vehicle in the first direction and the second direction are obtained respectively as the first point cloud data and the second point cloud data; wherein the first direction is the forward direction of the center point of the articulated vehicle, and the second direction is the backward direction of the center point of the articulated vehicle; the first point cloud data and the second point cloud data are registered; the registered second point cloud data are filtered to obtain the tunnel wall point cloud data; based on the tunnel wall point cloud data, the clustered point cloud clusters are extracted by Euclidean clustering, and the clusters located in the tunnel wall grid and with a height less than the first height threshold are marked as retaining walls, and the clusters located in the ground grid and with a height less than the second height threshold are marked as low obstacles; wherein the first height threshold is greater than the second height threshold.

[0047] Time synchronization and fusion of primary lidar and backward lidar point cloud data: setting up two point cloud data cache queues and , which are used to temporarily store the point cloud data of the main laser radar (the first laser radar) and the backward laser radar (the second laser radar). The length of the cache queue L can be determined according to the frequency f of the laser radar and the time synchronization accuracy requirements. To set, for example , indicating that the cache queue can accommodate Point cloud data within time. Whenever the main laser radar receives a new frame of point cloud data , along with the timestamp Push them into the main lidar cache queue Similarly, the point cloud data of the backward LiDAR and timestamp Also push into the backward lidar cache queue in the same way . The timestamp of the main lidar As a benchmark, in the backward LiDAR cache queue Search for point cloud data that meets the following conditions : ;in, It is the preset time interval threshold, which indicates the allowable range of the timestamp difference between two lidar data. The size of can be set according to factors such as the time synchronization accuracy of the laser radar and the movement speed of the articulated vehicle. Generally, it can be 1 to 2 times the laser radar scanning period T, for example .

[0048] If in the backward LiDAR cache queue Find the point cloud data that meets the timestamp condition , then compare it with the main lidar point cloud data When stitching, it is necessary to consider the installation position and posture differences of the two-directional laser radar in the vehicle body coordinate system, and transform the backward point cloud data into Unified to the main laser radar coordinate system. Assume that the position of the main laser radar relative to the vehicle body coordinate system is , the attitude angle is . Assume that the position of the rear-facing laser radar relative to the vehicle body coordinate system is , the attitude angle is . Through the rotation matrix and and the translation vector and ,Will Convert to the main laser radar coordinate system to obtain the spliced ​​point cloud data If the backward LiDAR cache queue If no point cloud data that meets the timestamp condition is found in the fusion, the fusion can be suspended and the main lidar point cloud data can be directly Output. Wait for the next frame of main lidar data to arrive before fusion to ensure the continuity and real-time nature of the point cloud data. In order to ensure the timeliness of the data in the cache queue, it is necessary to regularly clean up expired point cloud data.

[0049] Set a time threshold , set the timestamp to be greater than the current time Point cloud data from the cache queue and Delete it. The size of can be set according to the cache queue length L and real-time requirements, for example , indicating that the cache queue retains the most recent The time stamp information of the two lidar data is used to synchronize the data through the cache queue and timestamp matching. At the same time, the backward point cloud data is unified to the main lidar coordinate system through coordinate transformation, realizing the spatial fusion of data. By setting the appropriate time threshold and cache queue length, the real-time and integrity of the data can be balanced.

[0050] like Figure 2As shown, the main laser radar needs to be calibrated in advance to perform coordinate system conversion and registration of the main laser radar and the rear laser radar point cloud data: the installation position and posture of the main laser radar and the rear laser radar in the vehicle body coordinate system (base_link) are determined in advance through calibration. The transformation matrix from the laser radar coordinate system to the base_link coordinate system can be solved by hand-eye calibration or feature point matching. . It is a 4x4 homogeneous transformation matrix that represents the position (x, y, z) and attitude (roll, pitch, yaw) of the origin of the LiDAR coordinate system under base_link.

[0051] According to the structural characteristics of the articulated vehicle, the origin of the base_link coordinate system is defined at the position of the articulation angle θ. The x-axis of the base_link coordinate system points to the direction of the articulated vehicle, the z-axis is perpendicular to the ground and upward, and the y-axis is determined according to the right-hand coordinate system. Whenever a frame of main lidar point cloud data is received When using the pre-calibrated transformation matrix , each point in the point cloud The conversion formula from the main lidar coordinate system to the base_link coordinate system is: in, The coordinates of the converted point under base_link. For backward LiDAR point cloud data , using the same method, using the transformation matrix Convert it to the base_link coordinate system. Read the current articulation angle θ from the articulated vehicle chassis control system or sensor. The value range of θ is ,in is the maximum articulation angle. According to the articulation angle θ, the rotation matrix is ​​constructed Assuming that the articulation direction of the articulated vehicle is the y-axis, the rotation matrix It can be expressed as: .

[0052] Using the rotation matrix Backward LiDAR point cloud Transform it from the base_link coordinate system to the main lidar point cloud In the aligned coordinate system, the transformation formula is: ,in, The backward LiDAR point cloud after rotation alignment. With the main lidar point cloud Stitching is performed to obtain the complete point cloud data of the first registration .

[0053] Use the ICP algorithm to fine-tune and optimize the point cloud data after the first registration in the following way: Set a preset distance threshold and preset step size . Used to judge the effect of the first alignment. It can usually be set according to factors such as the measurement accuracy of the lidar and the point cloud density, for example, a value of 0.10.5m. It is used to perturb the articulation angle when the first registration fails. Usually a smaller value can be used, such as 15°. Each point in , in the first point cloud data Find the nearest neighbor in . You can use efficient data structures such as KD-Tree to perform nearest neighbor search. Calculate the sum of the distances of all nearest neighbor point pairs and divide it by the number of point pairs to get the mean distance. , where N is the number of points in the second point cloud data. If the distance mean Less than or equal to the preset distance threshold , it is considered that the first registration has achieved a good effect, and directly As the second point cloud data after registration, the algorithm ends. Greater than the preset distance threshold , then the first registration effect is not ideal and needs to be optimized. Increase the preset step length on both sides of the articulation angle θ , and obtain two new articulation angles: ; ; respectively and is the articulation angle, and two sets of backward point cloud data after rotation alignment are obtained repeatedly and . Calculate separately and and The mean distance and .Compare , and , select the group with the smallest mean distance as the second point cloud data after registration .if Minimum, then ;if Minimum, then ;if Minimum, then .Will and Stitching to obtain the complete point cloud data after final registration .

[0054] Preferably, the following heuristic strategy is used to accelerate the convergence of the ICP algorithm: And the second point cloud data Downsample to obtain low-resolution point cloud data and . Downsampling can use voxel gridding, random sampling and other methods to reduce the amount of point cloud data. Set the step size r of resolution increase, for example r=2, which means that the resolution is doubled each time. Initialize the current resolution level=0, corresponding to the resolution of the original point cloud data. While (level < max_level): According to the current resolution level, and Downsampling, we get and Using ICP algorithm and Perform registration and obtain the transformation matrix .Will As the initial value, the point cloud data of the next level of resolution is registered. level = level + 1; finally, the precise registration result at high resolution is obtained .

[0055] In underground mining environments, it is necessary to correctly filter the point cloud of the vehicle body and the top of the tunnel to ensure that obstacles that affect the abnormal operation of the vehicle can be correctly identified. Figure 3 As shown, step 1: filter out distant points through straight-through filtering, and then use statistical filtering to filter the discrete point cloud, including: filtering the second point cloud data and removing outliers: straight-through filtering, setting the distance threshold of the straight-through filtering , is determined according to the effective measurement range of the laser radar and the application requirements, for example, a value of 20 to 50 meters. For each point p(x, y, z) in the image, calculate its distance d to the lidar origin: ,if , then mark the point as a long-distance point and remove it from the point cloud data. Get the second point cloud data after direct filtering .

[0056] Statistical filtering: Set the number of nearest neighbor points k, which is determined according to the point cloud density and noise level, for example, a value of 10 to 50. Set the distance threshold , determined according to the distribution characteristics of the point cloud and the noise level, for example, the value is 0.1 to 0.5m. Set the outlier coefficient threshold , is determined according to the discreteness of the point cloud, for example, the value is 1.5~3.0. Each point in , perform the following steps: Use KD-Tree and other data structures to Search The k nearest neighbors of .calculate The average distance between its nearest neighbors : ;calculate The outlier coefficient : ;if , then Mark as outliers. Remove all the points marked as outliers from Eliminate the point cloud data and obtain the second point cloud data after statistical filtering. Through filtering, by setting a distance threshold, points beyond the effective range are quickly removed, thus reducing the amount of data to be processed later. Statistical filtering, by analyzing the local distribution characteristics of the point cloud and utilizing the distance relationship between the nearest neighboring points, adaptively identifies and removes outliers, thus preserving the main structure and features of the point cloud.

[0057] According to the preset front size of the articulated vehicle, the second point cloud data is divided into front point cloud and non-front point cloud using the straight-through filtering algorithm: Set the front size parameters of the articulated vehicle: Length L: Indicates the maximum size of the front of the articulated vehicle in the forward direction, determined according to the actual vehicle model, for example, the value is 4 to 8 meters. Width W: Indicates the maximum size of the front of the articulated vehicle in the horizontal direction, determined according to the actual vehicle model, for example, the value is 2 to 3 meters. Height H: Indicates the maximum size of the front of the articulated vehicle in the vertical direction, determined according to the actual vehicle model, for example, the value is 2 to 4 meters.

[0058] With the center of the rear axle of the articulated vehicle as the origin O, establish a right-handed coordinate system O-XYZ. The positive direction of the X-axis is the forward direction of the articulated vehicle, the positive direction of the Y-axis is the horizontal direction of the left side of the articulated vehicle, and the positive direction of the Z-axis is the vertical upward direction of the articulated vehicle. The unit of the coordinate system is meter (m). Construct a bounding box for the front of the vehicle: In the articulated vehicle coordinate system O-XYZ, with the origin O as the center, construct a rectangular block as the bounding box of the front of the vehicle. The length of the bounding box is L, the width is W, and the height is H, which is parallel to the three axes of the articulated vehicle coordinate system. Calculate the vertex coordinates of the bounding box and : ; ; .

[0059] Point cloud data division: the second point cloud data after the second filtering For each point P (x, y, z) in , perform the following judgment: If the following conditions are met: ; ; ; then divide the point P (x, y, z) into the front point cloud dataset Otherwise, the point P (x, y, z) is divided into the non-vehicle head point cloud dataset Finally, we get the front point cloud dataset and non-vehicle head point cloud dataset .

[0060] Perform Euclidean clustering on non-vehicle head point cloud data, and use the articulation angle position to construct a spherical bounding box to filter out potential obstacle point clouds: Construct an articulated vehicle kinematic model: Define the structural parameters of the articulated vehicle: : The wheelbase of the front body of an articulated vehicle, that is, the distance from the front axle to the center of articulation, in meters (m). : The wheelbase of the rear body of the articulated vehicle, that is, the distance from the center of articulation to the rear axle, in meters (m). W: The wheelbase of the articulated vehicle, that is, the distance between the left and right wheels, in meters (m). Define the motion state variables of the articulated vehicle: v: The speed of the articulated vehicle, in meters per second (m / s). α: The steering angle of the articulated vehicle, that is, the steering angle of the front body relative to the rear body, in radians (rad). θ: The articulation angle of the articulated vehicle, that is, the articulation angle of the front body relative to the rear body, in radians (rad).

[0061] Establish the kinematic model of the articulated vehicle: Take the rear axle center of the rear vehicle body as the origin O, and establish the right-hand coordinate system O-XYZ. The positive direction of the X-axis is the forward direction of the articulated vehicle, the positive direction of the Y-axis is the horizontal direction of the left side of the articulated vehicle, and the positive direction of the Z-axis is the vertical upward direction of the articulated vehicle. The position coordinates of the articulation center in the O-XYZ coordinate system are (x, y, z), and the initial moment is According to the motion state variables of the articulated vehicle, the relationship between the articulation center position coordinates (x, y, z) and the vehicle speed v, steering angle α, and articulation angle θ is established: ; ; z = 0; where the articulation angle θ can be calculated from the steering angle α and the vehicle speed v: Solve the kinematic model of the articulated vehicle: According to the current motion state of the articulated vehicle, that is, the vehicle speed v, steering angle α and articulation angle θ, substitute the corresponding formula in the kinematic model. Calculate the position coordinates (x, y, z) of the articulation center at the current moment: ; ;z=0; Output the coordinates of the hinge center position (x, y, z).

[0062] Constructing a sphere bounding box: Setting the minimum height of the preset obstacle , determined according to the actual scene and obstacle characteristics, for example, a value of 0.5 to 1.0 m. The coordinates of the hinge center position (x, y, z) are taken as the center of the sphere. For the sphere radius, construct a sphere. Calculate the vertex coordinates of the sphere bounding box and : , ; , ; , ; Euclidean clustering: Set clustering parameters: cluster radius : Determine the neighborhood range of the cluster, which is determined according to the point cloud density and obstacle size, for example, the value is 0.2~0.5m. Minimum number of cluster points : Determine the minimum number of points for a valid cluster, which is determined based on the minimum size of the obstacle, for example, a value of 10~50.

[0063] Non-vehicle point cloud data Perform Euclidean clustering: Initialize the cluster set C to be empty. For each point p in , perform the following steps: if p has been visited, skip it; otherwise mark p as visited. Create a new cluster c and add p to c. For each point q in c, Find the distance between q and q less than Point set .right For each point r in , if r has not been visited, mark r as visited and add r to c. Repeat the above two steps until c no longer increases. Add c to the cluster set C. Get the cluster set . Obstacle point cloud screening: Initialize the candidate obstacle point cloud set Is empty. For each cluster in the cluster set C , perform the following steps: Calculate The center coordinates of : , , ,in , n is The number of inliers. If the following conditions are met: , , ,and ; then Add to candidate obstacle point cloud collection The candidate obstacle point cloud collection As the second point cloud dataset after filtering.

[0064] like Figure 4 and Figure 5 As shown, the filtered second point cloud data is projected into the grid map, and the tunnel wall grid is marked by sliding window and threshold judgment: Set the preset size of the grid map , set according to the size of the obstacle and the required detection accuracy, for example, L2 = 5 meters, with a resolution of 0.05 meters / grid. Project to In a grid map of size, each grid corresponds to a point set For each grid (i, j), calculate the maximum value of the point cloud data in the Z-axis direction. and minimum value Set the preset height threshold h1 according to the height characteristics of the obstacle, for example Meters, indicating that grids with a height difference of more than 0.3 meters may contain obstacles. For each grid (i, j), if , it is marked as an obstacle grid , otherwise marked as .

[0065] The Patchwork algorithm is used to filter the second point cloud data. The specific steps are as follows: Divide into multiple sizes Plane blocks, each of which contains points, for example, N_p=100. For each plane block , fitting a plane , calculate its normal vector and height . Set the ground normal vector threshold and ground height threshold , set according to the normal vector and height characteristics of the ground, for example , Meters. For each plane block ,if The angle with the Z axis is less than ,and Less than , it is marked as a ground plane block , otherwise marked as . All The points in the plane block are used as the initial ground point cloud , the remaining points are regarded as non-ground point clouds .

[0066] The initial ground point cloud is expanded using the region growing method to obtain the final ground point cloud: seed point selection, from the initial ground point cloud Select each point as a seed point in turn , for example, traverse in order of point index. For each seed point , perform the following steps to perform region growing. Neighborhood point check: Take the seed point As the center, determine a neighborhood radius , set according to the density of the point cloud and the smoothness of the ground, for example m. of Search all non-ground points in the neighborhood , that is, belongs to For each non-ground point in the neighborhood , perform the following steps to determine the ground point. Distance and normal vector deviation calculation: Calculate the non-ground point To seed point The distance of the plane :First, according to Belonging plane block The normal vector and height , construct the plane equation Then, Coordinates Substitute the plane equation and calculate the directed distance from the point to the plane . Calculate non-ground points The normal vector , can be achieved by The principal component analysis (PCA) of the surrounding points is obtained. Calculate the normal vector deviation ,Right now and Angle of: .

[0067] Ground point judgment: set preset distance threshold and preset angle thresholds , set according to the ground undulation and normal vector changes, for example rice, .if and , then the non-ground points Mark as ground points and add them to the final ground point cloud If or , then keep For non-ground points, no marking or adding operations are performed. Iterative expansion: The ground points in the , are used as new seed points, and the neighborhood point check and ground point judgment are repeated. The expansion process is iterated until no new points are added That is, all possible ground points have been identified.

[0068] Non-ground point cloud extraction: will not be part of the final ground point cloud All points of the non-ground point cloud . The points in the ground may include obstacles, suspended objects, noise, etc., which need further analysis and processing. The local smoothness and continuity of the ground are utilized, and the points that meet the distance and normal vector deviation conditions are gradually marked as ground points through seed point expansion and neighborhood point judgment. At the same time, by setting an appropriate threshold, the undulations and slopes of the ground can be effectively processed, and the robustness of ground point cloud extraction can be improved.

[0069] Ground point cloud filtering: When the distance between the articulated vehicle and the obstacle grid is less than the preset distance When the segmented ground point cloud Filter. Set a preset ground height threshold , set according to the ground height change around the obstacle, for example h_f = 0.1 meters. Each point in , calculate its height .if Greater than , then from Remove it and get the filtered ground point cloud . Tunnel wall point cloud extraction: For each obstacle grid , extract the non-ground point cloud inside . All Merge into tunnel wall point cloud .

[0070] Perform Euclidean clustering on the tunnel wall point cloud data, and identify retaining walls and low obstacles based on the clustering results and height thresholds: Euclidean clustering, set the distance threshold for clustering , is set according to the size of the obstacle and the density of the point cloud, for example Meters, indicating that points with a distance less than 0.3 meters belong to the same cluster. Each point in , perform the following steps to cluster: If has been assigned to a cluster, skip this point and process the next point. If it is not assigned to any cluster, create a new cluster. ,Will Add to In. Centered on All points in the neighborhood whose distance is less than Points that are not assigned to any cluster are added to For new For each point in , recursively execute the previous step until No more expansion. Repeat the above process until All points in are assigned to a cluster. After clustering, a set of point cloud clusters is obtained. , each cluster contains a set of spatially adjacent points.

[0071] Retaining wall identification: Setting the first height threshold , set according to the height characteristics of the retaining wall, e.g. Meters, indicating that clusters with a height less than 1.5 meters may be retaining walls. , perform the following steps to identify the retaining wall: Calculate Height range ,Right now The minimum and maximum Z coordinates of the midpoint. If , then Marked as retaining wall cluster, denoted as .examine Whether the XY coordinates of are within the roadway wall grid, that is, whether .if If it is within the roadway wall grid, it is identified as a retaining wall and added to the retaining wall collection middle.

[0072] Low obstacle recognition: Setting the second height threshold , set according to the height characteristics of low obstacles, such as Meters, indicating that clusters with a height less than 0.5 meters may be low obstacles. , perform the following steps to identify low obstacles: Calculate Height range ,Right now The minimum and maximum Z coordinates of the midpoint. If , then Marked as a low obstacle cluster, denoted as .examine Whether the XY coordinates of are within the ground grid, that is, whether .if If it is within the ground grid, it is identified as a low obstacle and added to the low obstacle collection. middle.

[0073] Threshold setting: First height threshold and the second height threshold The following relationship needs to be satisfied: , that is, the height threshold of the retaining wall should be greater than the height threshold of the low obstacle in order to distinguish the two types of obstacles. The specific threshold setting needs to be adjusted according to the actual environment and the characteristics of the obstacles, and the appropriate value can be determined through experiments and statistical analysis. The differences in spatial distribution and height characteristics of obstacles are utilized, and spatially adjacent points are combined into clusters through Euclidean clustering, and judged and marked according to the height range of the cluster and the type of grid in which it is located. At the same time, by setting different height thresholds, retaining walls and low obstacles can be effectively distinguished, and the accuracy of obstacle recognition can be improved.

Claims

1. A method for sensing obstacles of articulated vehicles in underground mines, characterized in that: include: Obtaining laser radar point clouds in a first direction and a second direction of the articulated vehicle respectively as first point cloud data and second point cloud data; wherein the first direction is forward of the center point of the articulated vehicle, and the second direction is backward of the center point of the articulated vehicle; Registering the first point cloud data with the second point cloud data; Filtering the registered second point cloud data to obtain tunnel wall point cloud data; According to the tunnel wall point cloud data, clusters of clustered point clouds are extracted by Euclidean clustering, and clusters located in the tunnel wall grid and with a height less than a first height threshold are marked as retaining walls, and clusters located in the ground grid and with a height less than a second height threshold are marked as low obstacles; wherein the first height threshold is greater than the second height threshold; Filter the registered second point cloud data, including: According to the registered second point cloud data, a straight-through filtering algorithm is used to filter points whose distance is greater than a preset distance threshold, so as to obtain the second point cloud data after the first filtering; According to the second point cloud data after the first filtering, outliers are filtered using a statistical filtering algorithm to obtain the second point cloud data after the second filtering; According to the preset articulated vehicle head size, the second point cloud data after the second filtering is divided into vehicle head point cloud data and non-vehicle head point cloud data by a straight-through filtering algorithm; Perform Euclidean clustering on the non-vehicle head point cloud data, take the position of the hinge angle θ as the sphere center, preset the lowest obstacle height as the sphere radius, establish a spherical bounding box, and use the clustered point cloud data within the spherical bounding box as the second point cloud data after filtering; According to the second point cloud data after the first filtering, the statistical filtering algorithm is used to filter outliers, including: Calculate the average distance between each point in the second point cloud data after the first filtering and the nearest neighbor point as the neighborhood average distance of the corresponding point; where the number of nearest neighbor points is k; According to the preset distance threshold , calculate the outlier coefficient of each point in the second point cloud data after the first filtering. The outlier coefficient is the average distance and the distance threshold The ratio of The outlier coefficient is greater than the threshold The points are marked as outliers.

2. The obstacle sensing method for articulated vehicles in underground mines according to claim 1 is characterized in that: The first point cloud data and the second point cloud data are registered, including: Taking the position of the articulation angle θ of the articulated vehicle as the origin, a vehicle body coordinate system is established; obtaining a rotation matrix according to the articulation angle θ, and using the rotation matrix to perform a first registration on the first point cloud data and the second point cloud data; The ICP algorithm is used to calculate the average distance between each point in the second point cloud data after the first registration and the first point cloud data. If the average distance is greater than the preset distance threshold , then increase the preset step length on both sides of the hinge angle θ , return to perform the first registration, calculate and select the group with the smallest mean distance as the second point cloud data after registration.

3. The obstacle sensing method for articulated vehicles in underground mines according to claim 2 is characterized in that: The rotation matrix expression is as follows: in, Indicates the articulation angle of the articulated vehicle.

4. The obstacle sensing method for articulated vehicles in underground mines according to claim 1, characterized in that: According to the preset articulated vehicle head size, the second point cloud data after the second filtering is divided into head point cloud data and non-head point cloud data through the straight-through filtering algorithm, including: Set the length L, width W and height H of the front of the articulated truck, which represent the maximum dimensions of the front of the articulated truck in the forward direction, lateral direction and vertical direction respectively; With the center of the rear axle of the articulated vehicle as the origin O, establish the articulated vehicle coordinate system O-XYZ, where the positive direction of the X axis is the forward direction of the articulated vehicle, the positive direction of the Y axis is the horizontal direction of the left side of the articulated vehicle, and the positive direction of the Z axis is the vertical direction of the articulated vehicle; In the articulated vehicle coordinate system O-XYZ, with the origin O as the center, construct a cuboid with a length of L, a width of W, and a height of H as the bounding box of the front of the articulated vehicle, and mark the vertex coordinates of the bounding box as and ; Determine each point in the second point cloud data after secondary filtering point by point , if it satisfies: , , , then the point It is divided into the front point cloud dataset, otherwise it is divided into the non-front point cloud dataset.

5. The obstacle sensing method for articulated vehicles in underground mines according to claim 4, characterized in that: The lowest obstacle height is preset as the sphere radius, a sphere bounding box is established, and the point cloud data within the sphere bounding box after clustering is used as the second point cloud data after filtering, including: Construct a kinematic model of the articulated vehicle, and calculate the coordinates (x, y, z) of the articulation center position corresponding to the articulation angle θ at the current moment by solving the kinematic model according to the motion state of the articulated vehicle; Take the coordinates (x, y, z) of the hinge center as the center of the sphere and preset the lowest obstacle height is the radius of the sphere, construct a sphere, use the sphere as the spatial range of obstacle clustering, and mark the vertex coordinates of the sphere bounding box as and ; With preset cluster radius and the minimum number of cluster points As a parameter, the Euclidean distance is used to perform spatial clustering on the non-vehicle head point cloud data to obtain multiple point cloud clusters; Traverse each point cloud cluster obtained and calculate the center coordinates of each cluster And the number of points n in the cluster, if it satisfies: , , and , the corresponding cluster is taken as the candidate obstacle point cloud; All candidate obstacle point clouds are used as the filtered second point cloud dataset.

6. The obstacle sensing method for articulated vehicles in underground mines according to claim 1, characterized in that: Tunnel wall point cloud data, including: Project the filtered second point cloud data to the preset size In the grid map, the distribution of point cloud data in each grid in the Z-axis direction is counted; a sliding window is set to count whether the number of points in the window is greater than a threshold value, and if the number of points in a preset number of consecutive frames is greater than the preset point threshold, the corresponding grid is marked as a lane wall grid; Project the filtered second point cloud data to the preset size In the grid map, count the maximum and minimum point cloud heights of each grid, and count the points whose difference between the maximum and minimum values ​​is less than the preset height threshold. The grid of is marked as an obstacle grid; among them, Less than ; The Patchwork algorithm is used to segment the filtered second point cloud data to obtain ground point cloud data and non-ground point cloud data; When the distance between the articulated vehicle and the obstacle grid is less than the preset distance, the segmented ground point cloud data is filtered according to the preset ground height threshold, and the points with heights greater than the ground height threshold are filtered, and the filtered ground point cloud is used as the final ground point cloud; The non-ground point cloud data within the obstacle grid is used as the tunnel wall point cloud data.

7. The obstacle sensing method for articulated vehicles in underground mines according to claim 6, characterized in that: The Patchwork algorithm is used to segment the filtered second point cloud data to obtain ground point cloud data and non-ground point cloud data, including: Dividing the filtered second point cloud data into a plurality of plane blocks, each plane block containing a preset number of point cloud data; Calculate the normal vector and height of each plane block; According to the preset ground normal vector threshold and ground height threshold, the plane blocks whose normal vector and Z axis angle is less than the ground normal vector threshold and whose height is less than the ground height threshold are marked as ground plane blocks; The point cloud data within the ground plane block is used as the initial ground point cloud data, and the point cloud data outside the ground plane block is used as the non-ground point cloud data; Through the region generation method, the initial ground point cloud data is used as the seed point, and the points in the adjacent non-ground point cloud data that meet the preset ground features are merged into the ground point cloud data to obtain the final ground point cloud data; The point cloud data other than the final ground point cloud data is regarded as non-ground point cloud data.

8. The method for sensing obstacles of articulated vehicles in underground mines according to any one of claims 2 to 7, characterized in that: Before obtaining the first point cloud and the second point cloud, it also includes: According to the timestamp of the laser radar point cloud data in the first direction, at the preset time interval Obtaining the second direction laser radar point cloud data; If at the preset time interval If the second direction laser radar point cloud data is obtained, the first direction laser radar point cloud data and the second direction laser radar point cloud data are time synchronized as the obtained first point cloud and second point cloud.

Images