A mine vehicle speed estimation method and system based on radar point cloud data, a storage medium and a program product

Abstract

The application provides a mine vehicle speed estimation method and system based on radar point cloud data, a storage medium and a program product, which are applied to a mine vehicle, and the vehicle is provided with a radar to collect point cloud data in the mine, the method comprises the following steps: acquiring current frame point cloud data, and pre-processing the point cloud data to extract radial velocity, azimuth angle and pitch angle of each point; a random sample consensus (RANSAC) algorithm is used to generate multiple speed candidate values, and the best speed is estimated; the best estimation values of multiple frames of speeds are counted, sliding window smoothing processing is performed, and the final vehicle speed estimation value is obtained. The RANSAC algorithm is used to obtain multiple speed candidate values from the point cloud data, the speed candidate value with the most inliers is selected as the best candidate speed, the noise and abnormal point interference are resisted, the sliding window average method is used for smoothing processing of multiple best candidate speeds, the speed estimation accuracy is ensured, and the demand of mine vehicle real-time navigation is met.

Description

Technical Field

[0001] This disclosure relates to the field of mine vehicle navigation technology, and in particular to a mine vehicle speed estimation method, system, computer-readable storage medium, and computer program product based on radar point cloud data. Background Technology

[0002] In mining environments, accurately obtaining vehicle speed information is crucial for achieving autonomous driving, safe navigation, and obstacle avoidance control. However, the confined space, high dust levels, and poor lighting conditions in mining environments severely limit traditional speed estimation methods, such as GPS positioning and visual sensors. GPS signals are typically unreceiveable underground, while visual sensors are easily affected by dust and lighting conditions, leading to inaccurate speed estimates.

[0003] Currently, speed estimation methods based on radar sensors are widely used in mine vehicle navigation. Radar sensors can operate stably in harsh environments, providing high-precision distance and speed information. However, radar point cloud data in the mine environment suffers from problems such as high noise, sparse point clouds, and multi-target interference, making accurate vehicle speed estimation still a challenge. Summary of the Invention

[0004] The purpose of this disclosure is to provide a method, system, computer-readable storage medium, and computer program product for estimating the speed of mining vehicles based on radar point cloud data, thereby solving the aforementioned problems existing in the prior art.

[0005] To achieve the above objectives, the technical solutions adopted in the embodiments of this disclosure are as follows:

[0006] This disclosure provides, in one aspect, a method for estimating the speed of mining vehicles based on radar point cloud data. This method is applied to vehicles equipped with radar in a mine to collect point cloud data. The method includes:

[0007] Acquire the point cloud data of the current frame, and preprocess the point cloud data to extract the radial velocity, azimuth angle and pitch angle of each point;

[0008] The Random Sample Consensus Algorithm (RANSAC) is used to calculate the vehicle candidate speed based on the radial velocity and angle of each point. The maximum number of inliers that meet the preset conditions for randomly selected candidate speeds in the entire point cloud is iteratively counted, and the average of the candidate speeds of the maximum inlier set is taken as the optimal vehicle speed.

[0009] The best speed estimates across multiple frames are statistically analyzed, and a sliding window smoothing process is applied to obtain the final vehicle speed estimate.

[0010] Optionally, the step of employing the Random Sample Consensus Algorithm (RANSAC) to calculate candidate vehicle speeds based on the radial velocity and angle of each point, iteratively counting the maximum number of interior points in the entire point cloud that satisfy the preset conditions for randomly selected candidate speeds, and using the average of the candidate speeds of the maximum interior point set as the optimal vehicle speed, includes:

[0011] Initialize the random number seed and randomly select point i from the point cloud data;

[0012] Based on the radial velocity, azimuth angle, and pitch angle of point i, determine the radial velocity v of point i. meansi Convert to vehicle speed candidate value v i ;

[0013] Based on the vehicle speed candidate value v i Statistical analysis of velocity values ​​v in all point clouds candidate Satisfy | v i -v candidate The count of interior points is | < threshold, where threshold is a preset threshold.

[0014] Repeat the iteration to select different points multiple times, and take the candidate velocity value of the point with the most interior points;

[0015] Calculate the average candidate speed of the inlier set corresponding to the candidate speed of the point with the most inliers, and output the best vehicle speed estimate.

[0016] Optionally, the radial velocity v of point i is calculated based on the radial velocity, azimuth angle, and pitch angle of point i. meansi Convert to vehicle speed candidate value v i ,include:

[0017] If the vehicle is moving in a straight line, according to the formula v i =v measi / [cos(az measi )·cos(el measi ]], thus obtaining the candidate speed value v of vehicle at point i. i , where v measi az represents the original radial velocity measured by the radar at point i. measi The angle between point i in the horizontal direction and the front of the radar, el measi This represents the angle between point i in the vertical direction and the radar horizontal plane;

[0018] If the vehicle is traveling on a curve, according to the correction formula v i =v measi -(ω×r i )·LOS i / [cos(az measi )·cos(el measi]], thus obtaining the candidate speed value v of vehicle at point i. i , where v measi az represents the original radial velocity measured by the radar at point i. measi The angle between point i in the horizontal direction and the front of the radar, el measi This represents the angle between point i in the vertical direction and the radar horizontal plane, (ω×r) i )·LOS i This is the projection of the tangential velocity caused by rotation onto the radar line of sight.

[0019] Optionally, the step of statistically analyzing the optimal speed estimates across multiple frames and performing sliding window smoothing to obtain the final vehicle speed estimate includes:

[0020] Store the best vehicle speed in the current frame into an array of preset length n, where the array stores the best vehicle speed in previous frames;

[0021] The average of all valid speed values ​​in the array is calculated as the final vehicle speed estimate. The preset length of the array is determined based on the vehicle motion characteristics and the radar data update frequency.

[0022] Optionally, after storing the optimal vehicle speed of the current frame into an array of preset length n, the method further includes:

[0023] Determine whether the best speed of each historical frame in the array is a valid value, and compile the valid values ​​into a valid value set. The formula for calculating the valid value set is as follows:

[0024] γ valid ={v j ∈v window ||v j -v window |<∈}

[0025] Among them, v j Represents array v window The velocity of the j-th element in the vector, ∈ represents the reasonable error range or tolerance threshold of the effective velocity; v window This represents the average value in the array.

[0026] The average of the effective set is used as the final vehicle speed estimate.

[0027] Optionally, the preset length of the vehicle speed array is determined based on the vehicle's motion characteristics and the radar data update frequency, including:

[0028] If the mine car speed changes frequently, the preset length n is smaller; if the mine car speed is stable, the preset length n is larger.

[0029] If the radar frame rate is high, the preset length n increases; if the radar frame rate is low, the preset length n decreases.

[0030] Optionally, when the initial vehicle speed estimates of M consecutive frames satisfy When M≥10, σ represents the standard deviation of the initial vehicle speed estimate in M ​​frames, and μ represents the average value of the initial vehicle speed estimate in M ​​frames, the initial vehicle speed estimate is determined to be stable.

[0031] After extracting the radial velocity, azimuth, and pitch angle of each point in the next M+1 frame, the method further includes dynamic point filtering, the specific steps of which are as follows:

[0032] Based on the vehicle speed estimate output from the sliding window history frame of the previous M-th frame, calculate the theoretical radial velocity of each point in the next M+1 frame.

[0033] Calculate the absolute value of the difference between the actual radial velocity and the theoretical radial velocity, and mark the actual radial velocities whose absolute value of the difference is greater than a preset threshold as dynamic points;

[0034] Only static points are input into the RANSAC algorithm for subsequent vehicle speed estimation.

[0035] Optionally, when it is detected that the absolute value of the mean of the vehicle speed estimates for multiple consecutive frames is less than the minimum speed v min If this happens, the dynamic point filtering process is paused, and all point cloud data is re-input into the RANSAC algorithm.

[0036] Optionally, after extracting the radial velocity, azimuth angle, and pitch angle at each point, the method further includes:

[0037] The preprocessed point cloud is divided into K sectors according to the spatial azimuth angle;

[0038] K is adaptively adjusted based on the point cloud density. A minimum sector point count threshold is preset, and the K value is iteratively adjusted until the number of points in all sectors is greater than the threshold.

[0039] This disclosure provides, in one aspect, a mine vehicle speed estimation system based on radar point cloud data, applicable to vehicles in mines equipped with radar to collect point cloud data in the mine. The system includes:

[0040] The extraction module is used to acquire the point cloud data of the current frame and preprocess the point cloud data to extract the radial velocity, azimuth angle and pitch angle of each point;

[0041] The optimal speed determination module is used to calculate the vehicle candidate speed based on the radial velocity and angle of each point using the Random Sample Consensus Algorithm (RANSAC). It iteratively counts the maximum number of inliers in the entire point cloud that meet the preset conditions for randomly selected candidate speeds, and uses the average of the candidate speeds of the maximum inlier set as the optimal vehicle speed.

[0042] The estimation module is used to statistically analyze the best speed estimates across multiple frames and perform sliding window smoothing to obtain the final vehicle speed estimate.

[0043] Another aspect of this disclosure provides a computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the steps of the method described above.

[0044] Another aspect of this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the method described above.

[0045] The beneficial effects of the embodiments disclosed herein are:

[0046] The mine vehicle speed estimation method of this embodiment employs the RANSAC random sampling consensus algorithm to obtain corresponding vehicle speed candidate values ​​from the radial velocities of multiple sampled points based on the radial velocities and azimuth angles of each point in the current frame point cloud data. The method then selects the speed candidate value of the point with the largest number of points satisfying preset conditions, calculates the average candidate velocity of the inner point set corresponding to this candidate value as the best estimated vehicle speed, and smooths the best estimated values ​​from multiple frames to obtain the final vehicle speed estimate. This method effectively improves the accuracy and reliability of mine vehicle speed estimation and is suitable for environments with few other moving objects in mines. Attached Figure Description

[0047] Figure 1 This disclosure presents a schematic flowchart of a method for estimating the speed of mine vehicles based on radar point cloud data.

[0048] Figure 2 This is a schematic diagram of the complete process from data acquisition to speed estimation result output in the mine vehicle speed estimation method based on radar point cloud data according to an embodiment of the present disclosure;

[0049] Figure 3 This disclosure presents a schematic diagram of a mine vehicle speed estimation system based on radar point cloud data.

[0050] Figure 4 This is another schematic diagram of the process for estimating the speed of mine vehicles based on radar point cloud data, according to an embodiment of this disclosure. Detailed Implementation

[0051] To make the objectives, technical solutions, and advantages of the embodiments of this disclosure clearer, the embodiments of this disclosure will be further described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are merely illustrative of the embodiments of this disclosure and are not intended to limit the embodiments of this disclosure.

[0052] Example 1, as Figure 1 and Figure 2As shown, this disclosure proposes a method for estimating the speed of mine vehicles based on radar point cloud data. This method is applied to vehicles equipped with radar in mines to collect point cloud data. The method includes:

[0053] Step S100: Obtain the point cloud data of the current frame, and after preprocessing the point cloud data by removing noise points, filtering and downsampling, extract the radial velocity, azimuth angle and pitch angle of each point.

[0054] In this embodiment, step S100 involves collecting point cloud data using a radar sensor installed in front of the mining vehicle and transmitting the collected point cloud data to the data processing unit. The radar sensor can be a millimeter-wave radar sensor or an FMCW lidar sensor, etc., capable of obtaining the radial velocity of the point cloud. The radar actually used in this embodiment is a short-range radar with a range resolution of 0.16m and a velocity resolution of 0.126m / s. This results in the number of stationary point clouds being far greater than the number of moving objects in most cases. The radar collects a large number of point clouds, while the point clouds of moving objects in the mining environment are very few, far fewer than the point clouds of stationary objects. In a mining environment, using this radar provides strong real-time prediction of vehicle speed and is accurate. The data processing unit preprocesses the collected point cloud data, including noise removal and filtering, to improve data quality. a) Noise point filtering based on signal strength and spatial distribution outlier detection: remove points with signal strength below a threshold or spatial outliers; b) Exclusion of dynamic object point clouds based on radial velocity thresholds and Doppler velocity-based removal of moving object point clouds; c) Reduction of point cloud density through voxel grid sampling: reduce point cloud density while preserving geometric features. Specifically, radial velocity |v is removed. radial |>v vehicle＿max The point cloud of +Δv, v vehicle_max For the maximum design speed of the mining truck, Δv can be 5 km / h. Point clouds of mobile devices / personnel are excluded to avoid contaminating the static background dataset. A radar sensor is installed in front of the mining truck to collect point cloud data. A data processing unit is connected to the radar sensor to perform preprocessing and subsequent steps. The data processing unit includes a microprocessor, memory, and input / output interfaces. The azimuth angle is based on the vehicle's direction of travel as 0°.

[0055] The data preprocessing step also includes downsampling of the point cloud data to reduce the amount of data processed. After preprocessing, most of the moving point clouds can be filtered out.

[0056] Step S200: Using the Random Sample Consensus Algorithm (RANSAC), calculate the vehicle candidate speed based on the radial velocity and angle of each point, iteratively count the maximum number of inliers in the entire point cloud that meet the preset conditions for randomly selected candidate speeds, and use the average of the candidate speeds of the maximum inlier set as the optimal vehicle speed.

[0057] It should be noted that the method of this disclosure is designed for mining environments characterized by GPS rejection, complex structures, low visibility, and few dynamic objects. Step S200 of this disclosure uses the RANSAC algorithm to directly estimate the vehicle's own speed based on the radial velocity of the point cloud (Doppler velocity) in a single frame. This is primarily because stationary background point clouds constitute the majority of the mining environment, and their radial velocities are entirely caused by the vehicle's own motion.

[0058] To ensure uniform sampling, prior to step 200, the method further includes dividing the preprocessed point cloud into K sectors based on spatial azimuth angles.

[0059] K is adaptively adjusted based on point cloud density. A minimum sector point count threshold is set, such as 20 points. The K value is iteratively adjusted until the total number of points in all sectors exceeds the threshold. Non-uniform sector division can be used. For dense point cloud areas, such as azimuth ±30°, more sectors are subdivided, and sparse areas are merged. For straight tunnel scenarios in mines: K = 6~8, such as horizontal coverage of 180° field of view, with each sector covering 30°~45°.

[0060] Curved / intersection scenarios: K=10~12, requiring higher directional resolution.

[0061] Step S200 specifically includes the following steps:

[0062] Step S210: Initialize the random number seed and randomly select point i from the point cloud data.

[0063] In each iteration of step S210, a sector is randomly selected, and a point cloud data point i is randomly selected within that sector.

[0064] If the point cloud is particularly dense in a certain area, such as the tunnel wall directly in front, pure random sampling may over-concentrate on this area, ignoring points in other directions, such as the side walls or behind. Uniform sampling helps to find all better solutions.

[0065] When a point cloud data point i is randomly selected, the probability of each point i being selected is proportional to its radar reflection intensity. Alternatively, sampling probabilities can be assigned based on the number of points in a sector, such as sectors with more points being selected more frequently. Points with high reflection intensity, such as tunnel walls, are sampled first to improve the signal-to-noise ratio. Points with elevation angles close to 0° are sampled first (horizontal plane points better fit the motion assumption). Alternatively, the sampling probability can be determined by weighting the reflection intensity and elevation angle.

[0066] Based on the prior knowledge that mining trucks mainly travel in a fixed direction within the roadway, point cloud data points located in the areas in front of and behind the vehicle's direction of travel are preferentially selected for sampling.

[0067] Step S220: Based on the radial velocity, azimuth angle, and pitch angle of point i, calculate the radial velocity v of point i. meansi Convert to vehicle speed candidate value v i .

[0068] When the vehicle is moving in a straight line, if there is no rotation, the rotational angular velocity ω = 0, and the radial velocity of the point cloud is caused only by the vehicle's own velocity:

[0069] v measi =v i ·cos(θ i )

[0070] Where, θ i θ is the angle between the vehicle's velocity direction and the radar's line-of-sight direction. i =az measi In mines, vehicles typically move along the axis of the roadway, with a pitch angle of el. measi ≈0, along the sloping tunnel, pitch angle el measi v cannot be ignored measi =v i ·cos(az measi )·cos(el measi Then, by projecting in three-dimensional space, the candidate value of vehicle speed v is obtained. i v i The calculation formula is as follows:

[0071]

[0072] Among them, v i This represents the candidate vehicle speed value after the transformation of point i; v measi This represents the original radial velocity measured by the radar at point i, i.e., the projection of the relative velocity between the radar and the target point onto the radar's line-of-sight direction; az measi The azimuth angle is the angle between point i in the horizontal direction and the direction directly in front of the radar; el measi This represents the elevation angle, i.e., the angle between point i in the vertical direction and the radar horizontal plane. It is expressed by the radial velocity v. measi Divide by the cosine of the azimuth and pitch angles, project the radial velocity onto the vehicle's direction of motion, and obtain candidate values ​​for the vehicle velocity.

[0073] When the vehicle is moving in a curved section of a mine or is not moving in a straight line, the azimuth angle changes. Directly converting the speed using the above formula will introduce errors. The vehicle has a rotational angular velocity ω, and the true azimuth angle change rate of the point cloud is d(az). i ) / dt≠0.

[0074] Based on the radial velocity including a tangential component: v measi =v vehicle ·cos(θ i )·cos(elmeasi )+(ω×r i )·LOS i , where θ i Let ω be the angle between the velocity vector and the radar line of sight. i LOS is the tangential velocity vector generated by the rotation of point i. i This is the radar line-of-sight unit vector. The angular velocity ω is obtained using IMU or point cloud matching between consecutive frames. It is necessary to ensure that ω and r... i LOS i Within the same coordinate system (usually the radar coordinate system), if ω comes from the IMU, it needs to be transformed to the radar coordinate system, which will not be elaborated here.

[0075] Radial velocity v at the curve correction point i measicorrected =v measi -[ω×r i ]·LOS i Among them, LOS i Let r be the line-of-sight vector pointing to point i from the radar, i.e., the unit vector of the line-of-sight direction of point i, obtained directly or indirectly from radar measurements; i Let be the Euclidean distance from point i to the radar, which is the straight-line distance from the radar's transmitted beam to the point cloud.

[0076] Where, r i =(x i ,y i ,z i ) is the position vector of point i in the radar coordinate system; It is the Euclidean distance from point i to the radar.

[0077] The corrected radial velocity only contains the translation component, so it can be directly substituted into formula (1) to obtain the corrected formula for the candidate vehicle velocity in the curve scenario, that is, the corrected model is:

[0078]

[0079] (ω×r i )·LOS i This is the projection of the tangential velocity caused by rotation onto the radar line of sight.

[0080] Tested on typical curves (curvature radius R > 50m): If |ω| < 5° / s, the tangential component can be ignored; otherwise, the modified model is enabled, that is, it is enabled on sharp curves or forks.

[0081] Step S230: Based on the candidate vehicle speed value v i Statistical analysis of velocity values ​​v in all point clouds candidate Satisfy | v i -v candidateThe count of interior points where | < threshold. Where v i The threshold is the candidate velocity value for the currently randomly selected point i, and the threshold is the adaptive threshold set according to the sensor and the environment, i.e., the preset threshold.

[0082] RANSAC statistics calculate the support (inliers) of all point clouds for candidate values. It calculates the support for a candidate value v at that velocity. i The count of points in point cloud data within a certain threshold range.

[0083] |v i -v candidate | <threshold (2)

[0084]

[0085] Among them, v candidate Candidate value for velocity v i Candidate vehicle speeds at nearby points that meet preset conditions, where the preset condition is |v|. i -v candidate | <threshold, v candidate The calculation method adopts the vehicle speed candidate value conversion formula 1 for point i, or the aforementioned curve vehicle candidate speed correction formula, to convert the radial velocity of each point into the vehicle candidate speed; N is the total number of point cloud data points in the current frame; count represents the count value that satisfies formula (2); σ is the indicator function, defined as:

[0086]

[0087] In step S230, when generating candidate velocity values, the preset threshold is three doppler bins of the radar sensor's velocity resolution, and can also be adjusted according to the mine environment. The RANSAC in-point threshold is dynamically adjusted as threshold = 3δ + k·ω, where δ is the radar velocity resolution and k is the rotation sensitivity coefficient.

[0088] Step S240: Repeat the iteration multiple times to select different points, and take the candidate velocity value of the point with the most interior points.

[0089] Repeat steps S210 to S230 multiple times in the current frame, randomly selecting a new point i each time and re-counting the data. Through sufficient iterations, the point cloud cluster corresponding to the true velocity is likely to be covered. In other words, each iteration calculates the number of interior points that satisfy the preset conditions for the velocity candidate value of point i, and calculates the average velocity v of all interior points in the k-th iteration with the largest number of interior points. k* .

[0090] Step S250: Calculate the average candidate speed of the inner point set corresponding to the candidate speed of the inner point with the most inner points, and output the best vehicle speed estimate.

[0091] It should be noted that after step S240, the candidate velocity value v with the largest number of points in the point cloud data that meets the preset conditions is selected. i The system counts the number of points within the point cloud and checks if the total number of points in the point cloud is greater than N / 3, where N is the total number of points in the point cloud data. The total number of points detected and output by the radar is N. If count > N / 3, then the system calculates the velocity v of all points that meet the preset conditions. candidate Perform a cumulative average to obtain v k* After repeating steps S210 to S230 multiple times, select the average velocity v of the interior point corresponding to the value with the largest number of interior points count in the k-th iteration. k* v, the best estimate of vehicle speed in the current frame best However, when the maximum number of interior points is less than or equal to N / 3, v best =999, which is an abnormal speed value.

[0092] v best =v k* (4)

[0093] k * =argmax k∈{1，2，...，K} c k (5)

[0094] Among them, c k =max(c1, c2, ..., c K ), c k This represents the count of candidate values ​​that satisfy the conditions in the k-th iteration, i.e., the maximum number of point cloud values ​​that satisfy the preset conditions in the k-th iteration; v k* Indicates the corresponding c k Candidate value index; v k* Let k be the iteration number. * The mean velocity of the candidate vehicle's interior points corresponding to this time, v k* It's the calculated average velocity of the interior points. best This represents the best estimate for the current frame. If the number of point clouds meeting the condition exceeds N / 3 of the total number of points, then the velocity v of these points is taken. candidate Average; otherwise, mark as an outlier (999).

[0095] If the statistical velocity candidate value v i The maximum number of point cloud data points that meet the preset speed conditions (count) max , but count max If the value is less than or equal to N / 3, the error will be relatively large, so the current frame is discarded.

[0096] RANSAC searches for candidate vehicle velocities (i.e., radial velocities corresponding to stationary objects) that are consistent with the majority of the point cloud through random sampling. The radial velocities of moving objects (such as other vehicles) deviate from the group of stationary objects and are therefore filtered out as noise.

[0097] Step S300: Calculate the best speed estimates across multiple frames, perform sliding window smoothing, and obtain the final vehicle speed estimate.

[0098] The generated optimal velocity estimates for each frame are smoothed using the sliding window averaging method after the above steps. Velocity smoothing: The optimal velocity estimates for each frame are smoothed using the sliding window averaging method to reduce fluctuations in velocity estimates and output a more stable final velocity.

[0099] The sliding window smoothing process specifically includes: storing the current best estimate in a fixed-length array; and calculating the average of all valid speed values ​​in the array as the final vehicle speed estimate.

[0100] In the speed smoothing process, the size of the fixed-length array is set according to the vehicle motion characteristics and the radar data update frequency.

[0101] The specific steps are as follows:

[0102] Store the current best estimate into an array v of a predetermined length n. window =[v1,v2,……v n The fixed-length array is a first-in, first-out queue used to store the best estimates of vehicle speeds across multiple frames.

[0103] v iinsert =v best (6)

[0104] Among them, i insert =i insert mod n represents the array v window The position to insert in, v iinsert This represents the inserted value. The core function of the fixed-length array is to cache historical data, storing the most recent n velocity estimates, covering velocity changes over a period of time. It also smooths noise by calculating the mean of the effective values ​​within the window, suppressing random errors in single-frame estimations.

[0105] The window length n can be dynamically adjusted based on two factors: vehicle motion characteristics and radar data update frequency. Vehicle motion characteristics: If the mine car speed changes frequently (e.g., frequent starts and stops), a smaller n (e.g., n=5) should be selected to quickly respond to speed changes. If the speed is stable (e.g., constant speed travel), a larger n (e.g., n=10~20) can be selected to improve smoothing.

[0106] Radar data update frequency: If the radar frame rate is high (e.g., 10Hz), n can be increased appropriately (due to short time intervals and high data redundancy). If the frame rate is low (e.g., 1Hz), n needs to be decreased to avoid excessive lag.

[0107] When v iinsert Insert array v window After, i insert =i insert +1, to indicate the position for the next insertion.

[0108] Calculate the average of all valid speed values ​​in the array to obtain the final vehicle speed estimate v. 最终 .

[0109] Determine whether the optimal speed of each historical frame in the array is a valid value, and define invalid values ​​as v. invalid Then the set of valid values ​​is:

[0110] γ valid ={v j ∈v window ||v j -v window |<∈}

[0111]

[0112] Where, |γ valid | represents the number of valid values. In Formula 7, v window Select the normal and effective velocity γ valid Then, the selected effective speeds are summed and averaged to obtain the estimated vehicle speed v. final The aforementioned steps primarily filter out v best =999, an abnormal value. j Represents array v window The velocity of the j-th element in the set γ. ∈ represents the reasonable error range or tolerance threshold for the effective velocity, used to determine whether a velocity value belongs to the effective set γ. valid v window This represents the average speed within the array.

[0113] If the optimal speed of historical frame j satisfies v j ∈v window ||v j -v window |<∈, then v j The effective values ​​are the effective set, and the average speed of the effective set is used as the final vehicle speed estimate.

[0114] In other words, invalid values ​​were eliminated.

[0115] Step S400: Output the final estimated vehicle speed.

[0116] This embodiment of the disclosure outputs the result through an output interface; the final vehicle speed estimate is output to the vehicle control system for vehicle navigation and control.

[0117] For example, the values ​​in the example are not the actual values; the current best estimate is stored in an array of fixed length 3.

[0118] Calculate the average of all valid speed values ​​in the array to obtain the final vehicle speed estimate.

[0119] 1. For example, v_window = {-1.3, 999, -1.32};

[0120] 2. Calculate γ valid ={-1.3, -1.32};

[0121] 3. Calculate = 2;

[0122] 4. v_out=(-1.3-1.32) / 2.

[0123] The output interface is connected to the data processing unit and is used to output the final vehicle speed estimate to the vehicle control system. The output interface is a CAN bus interface or an Ethernet interface.

[0124] The mine vehicle speed estimation method based on radar point cloud data disclosed in this embodiment has the following advantages: Strong anti-interference capability: Employing the RANSAC algorithm to extract speed information from point cloud data effectively resists interference from noise and outliers, improving the accuracy of speed estimation. Good adaptability: By adaptively adjusting the threshold range, it can adapt to different mine environments and radar sensor characteristics, improving the method's versatility. High real-time performance: Using a sliding window averaging method for speed smoothing ensures the accuracy of speed estimation while meeting the real-time navigation needs of mine vehicles. High reliability: Through statistical analysis of multiple point cloud data points and fusion processing of historical speed data, the reliability of speed estimation is effectively improved.

[0125] like Figure 3 As shown, another aspect of this disclosure proposes a mine vehicle speed estimation system based on radar point cloud data, applicable to vehicles in mines. The vehicles are equipped with radar to collect point cloud data in the mine. The system includes:

[0126] The extraction module 100 is used to acquire the point cloud data of the current frame and preprocess the point cloud data to extract the radial velocity, azimuth angle and pitch angle of each point.

[0127] The optimal speed determination module 200 is used to calculate the vehicle candidate speed based on the radial velocity and angle of each point using the Random Sample Consensus Algorithm (RANSAC), iteratively count the maximum number of inliers in the entire point cloud that meet the preset conditions of the randomly selected candidate speed, and use the average of the candidate speeds of the maximum inlier set as the optimal vehicle speed.

[0128] The estimation module 300 is used to statistically analyze the best speed estimates across multiple frames and perform sliding window smoothing to obtain the final vehicle speed estimate.

[0129] The system of this embodiment further includes a data acquisition module for acquiring point cloud data and transmitting the acquired point cloud data to a data processing unit. The data processing unit includes an acquisition module 100, an estimation module 200, and an estimation module 300. The data processing unit also includes an output interface connected to the vehicle control system, used to output the final estimated vehicle speed to the vehicle control system.

[0130] In Example 2, in Example 1, unnecessary areas and points with low signal-to-noise ratios are filtered out first. To improve stability and distinguish between moving and stationary objects, data filtering requires passing through a certain number of frames, such as 10 frames. Step S300 obtains a relatively stable vehicle speed, which is used to distinguish between moving and stationary points. Then, stationary points are selected and output to step S200 to re-enter a new estimation state. When step S300 outputs an estimated vehicle speed of 0, it waits for a certain number of frames again and outputs a stable vehicle speed to distinguish between moving and stationary points. When the vehicle speed is always 0, moving and stationary points are not distinguished. This operation achieves a stable effect.

[0131] like Figure 4 As shown, in Embodiment 1, after estimating the optimal speed for M consecutive frames, the initial vehicle speed estimates from the M consecutive frames are satisfied before step S300 in frame M+1. Under the condition that M≥10, σ represents the standard deviation of the initial vehicle speed estimate in M ​​frames, μ represents the average value of the initial vehicle speed estimate in M ​​frames, and w can be 0.2, which is considered as stable initial vehicle speed estimation. A continuous M-frame refers to the effective frames corresponding to consecutive effective optimal speeds.

[0132] After extracting the radial velocity, azimuth angle, and pitch angle of each point in the next M+1 frame, the method further includes:

[0133] Step S10: Based on the vehicle speed estimate output by the sliding window history frame of the previous M-th frame, calculate the theoretical radial velocity of each point in the next M+1 frame.

[0134] The theoretical radial velocity calculation formula for point i is: v theoryi =v besti ·cos(azmeasi )·cos(el measi ).

[0135] Step S20: Calculate the absolute value of the difference between the actual radial velocity and the theoretical radial velocity, and mark the actual radial velocity with an absolute value greater than the preset threshold ε as a dynamic point.

[0136] Dynamic point marking conditions: Where ε is the adaptive tolerance threshold, ε=max(0.5m / s,3×δ), and δ is the radar velocity resolution.

[0137] Step S30: Input only static points into the RANSAC algorithm for subsequent vehicle speed estimation.

[0138] It should be noted that dynamic points are removed from the raw point cloud data of the next M+1 frame, and the remaining static points are input into the RANSAC algorithm in the subsequent step S200 for subsequent vehicle speed estimation.

[0139] After obtaining the speed estimate of the current M+1 frame in step S300, if the mean of the vehicle speed estimates of consecutive P frames satisfies the zero-speed condition, that is, the absolute value of the mean is less than the minimum speed, i.e., |μ(v best )|<v min v min If the speed can be 0.1 m / s and P can be 15, then the dynamic point filtering process is paused, i.e., steps S10 to S30 are paused, and all point cloud data is re-input into the RANSAC algorithm. A zero-degree timer is started, T = 3 s; if the speed remains at zero, v is output. final =0.

[0140] v can be updated once every K frames. theoryi If the current frame is a periodically updated frame, then use the new v. theory If the mutation condition is triggered, then use the updated v. theory Otherwise, use the previous frame v. theory When the vehicle speed changes abruptly, the dynamic and static points are immediately separated again for speed change detection; |dv / dt|>1m / s 2 When traveling at a constant speed, the theoretical speed of the point cloud in each frame can be updated periodically; sudden braking triggers an immediate update; normal acceleration results in periodic updates.

[0141] The system further includes: a vehicle speed stability judgment module and a dynamic-static separation module. The vehicle speed stability judgment module is used to determine whether the initial vehicle speed estimate of the current consecutive M frames meets the following criteria: When M≥10, σ represents the standard deviation of the initial vehicle speed estimate in M ​​frames, and μ represents the average value of the initial vehicle speed estimate in M ​​frames, the initial vehicle speed estimate is determined to be stable.

[0142] The static / dynamic separation module includes: calculating the theoretical radial velocity of each point in the next M+1 frame based on the vehicle speed estimate output from the previous sliding window historical frame; calculating the absolute value of the difference between the actual radial velocity and the theoretical radial velocity, marking the actual radial velocities with an absolute value greater than a preset threshold as dynamic points; and inputting the static points into the RANSAC algorithm for subsequent vehicle speed estimation. Steps S10 to S30 are executed.

[0143] Another aspect of this disclosure provides a computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the steps of the method described above.

[0144] Another aspect of this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the method described above.

[0145] The above description is only a preferred embodiment of the present disclosure. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present disclosure, and these improvements and modifications should also be considered within the protection scope of the present disclosure.

Claims

1. A mine vehicle speed estimation method based on radar point cloud data, applied to a mine vehicle, the vehicle being provided with a radar to collect point cloud data in the mine, characterized in that, The method includes: Acquire the point cloud data of the current frame, and preprocess the point cloud data to extract the radial velocity, azimuth angle and pitch angle of each point; The Random Sample Consensus Algorithm (RANSAC) is used to calculate the vehicle candidate speed based on the radial velocity and angle of each point. The maximum number of inliers that meet the preset conditions for randomly selected candidate speeds in the entire point cloud is iteratively counted, and the average of the candidate speeds of the maximum inlier set is taken as the optimal vehicle speed. The best speed estimates across multiple frames are statistically analyzed, and a sliding window smoothing process is applied to obtain the final vehicle speed estimate. The Random Sample Consensus Algorithm (RANSAC) is used to calculate candidate vehicle speeds based on the radial velocity and angle of each point. Iteratively, the maximum number of interior points in the entire point cloud that satisfy the preset conditions for randomly selected candidate speeds is counted. The average of the candidate speeds from the maximum interior point set is taken as the optimal vehicle speed. This includes: Initializing a random number seed, randomly selecting a point in the point cloud data i ; According to the radial velocity, azimuth angle and elevation angle of the point i , the radial velocity i of the point v is converted into a vehicle speed candidate value meansi v i ; According to the vehicle speed candidate value v i , statistics the speed value in all point clouds v candidate satisfies | I v i - v candidate | < I threshold The number of inner points count, wherein threshold is a preset threshold value; Repeat the iteration to select different points multiple times, and take the candidate velocity value of the point with the most interior points; Calculate the mean of the candidate velocities of the inlier set corresponding to the candidate velocity values ​​of the inlier with the most inliers, and output the best vehicle speed estimate. The point i radial velocity, azimuth angle and elevation angle, the point i radial velocity v meansi is converted into a vehicle speed candidate value v i , comprising: If the vehicle is moving in a straight line, according to the formula v i = v measi / [cos( az measi )·cos( el measi )], to obtain points i Candidate values ​​for vehicle speed v i ,in, v measi Indicates radar point i The measured original radial velocity, az measi point i The angle between the horizontal direction and the front of the radar, el measi Point i The angle between the vertical direction and the radar horizontal plane; If the vehicle is traveling on a curve, according to the correction formula v i = v measi -( ω × r i )·LOS i / [cos( az measi )·cos( el measi )], to obtain points i Vehicle speed candidate value v i ,in, v measi Indicates radar point i The measured original radial velocity, az measi point i The angle between the horizontal direction and the front of the radar, el measi Point i The angle between the vertical direction and the radar horizontal plane, (ω×r) i )·LOS i ω is the projection of the tangential velocity caused by rotation onto the radar line of sight; r is the three-dimensional rotational angular velocity vector of the vehicle; i Let i be the position vector of point i in the radar coordinate system; LOS i This is the radar line-of-sight unit vector.

2. The method of claim 1, wherein, The process of statistically analyzing the optimal speed estimates across multiple frames and then smoothing them using a sliding window to obtain the final vehicle speed estimate includes: Store the best vehicle speed in the current frame into an array of preset length n, where the array stores the best vehicle speed in previous frames; The average of all valid speed values ​​in the array is calculated as the final vehicle speed estimate. The preset length of the array is determined based on the vehicle motion characteristics and the radar data update frequency. The preset length of the vehicle speed array is determined based on the vehicle's motion characteristics and the radar data update frequency, including: If the mine car speed changes frequently, the preset length n is smaller; if the mine car speed is stable, the preset length n is larger. If the radar frame rate is high, the preset length n increases; if the radar frame rate is low, the preset length n decreases.

3. The method of claim 2, wherein, After storing the optimal vehicle speed for the current frame into an array of preset length n, the method further includes: Determine whether the best speed of each historical frame in the array is a valid value, and form a set of valid values; The average of the set of valid values ​​is used as the final vehicle speed estimate.

4. The method according to any one of claims 1 to 3, characterized in that, When the initial vehicle speed estimation value of the continuous M frames satisfies wherein M≥10, σ the standard deviation of the M-frame initial vehicle speed estimation value is represented by μ the average value of the M-frame initial vehicle speed estimation value is represented by After extracting the radial velocity, azimuth, and pitch angle of each point in the next M+1 frame, the method further includes dynamic point filtering, the specific steps of which are as follows: Based on the vehicle speed estimate output from the sliding window history frame of the previous M-th frame, calculate the theoretical radial velocity of each point in the next M+1 frame. Calculate the absolute value of the difference between the actual radial velocity and the theoretical radial velocity, and mark the actual radial velocities whose absolute value of the difference is greater than a preset threshold as dynamic points; Only static points are input into the RANSAC algorithm for subsequent vehicle speed estimation.

5. The method of claim 3, wherein, When it is detected that the absolute value of the mean of the multi-frame vehicle speed estimation value is less than the minimum speed v min When it is detected that the absolute value of the mean of the multi-frame vehicle speed estimation value is less than the minimum speed 6. The method according to any one of claims 1 to 3, characterized in that, After extracting the radial velocity, azimuth angle, and pitch angle at each point, the method further includes: The preprocessed point cloud is divided into K sectors according to the spatial azimuth angle; K is adaptively adjusted based on the point cloud density. A minimum sector point count threshold is preset, and the K value is iteratively adjusted until the number of points in all sectors is greater than the threshold.

7. A system for estimating the speed of a mine vehicle based on radar point cloud data according to any one of claims 1-6, applied to a mine vehicle, the vehicle being provided with a radar to collect point cloud data in the mine, characterized in that, The system includes: The extraction module is used to acquire the point cloud data of the current frame and preprocess the point cloud data to extract the radial velocity, azimuth angle and pitch angle of each point; The optimal speed determination module is used to calculate the vehicle candidate speed based on the radial velocity and angle of each point using the Random Sample Consensus Algorithm (RANSAC). It iteratively counts the maximum number of inliers in the entire point cloud that meet the preset conditions for randomly selected candidate speeds, and uses the average of the candidate speeds of the maximum inlier set as the optimal vehicle speed. The estimation module is used to statistically analyze the best speed estimates across multiple frames and perform sliding window smoothing to obtain the final vehicle speed estimate.

8. A computer-readable storage medium having stored thereon a computer program, characterized in that, When executed by a processor, the computer program implements the steps of the method according to any one of claims 1 to 6.

Images