A method and system for processing the slope of underground mine maps

Abstract

The present application discloses a method and system for processing the slope of a mine map, which involves unmanned driving, including: obtaining the IMU posture data and acceleration data of the mine unmanned vehicle, and obtaining the vehicle status data through the CAN bus; extracting the slope values ​​collected by the vehicle when driving forward and backward from the IMU posture data, and calculating the first compensation value α of the slope; estimating the slope of the road on which the unmanned vehicle is driving through a fitting algorithm, obtaining an estimated slope value, and calculating the second compensation value β of the slope; calculating the comprehensive slope compensation estimate P according to α and β; using P to correct the forward and reverse collected slope values; obtaining the true slope value of the sampling point, and calculating the third compensation value γ of the slope; calculating the final slope compensation value according to α, β and γ; using to correct the slope of the mine map. In view of the low slope accuracy of the mine map in the prior art, the present application improves the accuracy through multiple iterative corrections.

Description

Technical Field

[0001] The present application relates to the field of unmanned driving, and in particular to a method and system for processing the slope of a mine map. Background Art

[0002] Existing open-pit mines, ports, parks, etc. on public roads or in closed scenarios all use traditional GNSS+RTK to obtain high-precision latitude and longitude coordinates and elevation information, and use elevation values ​​to calculate slopes after data preprocessing. However, in underground mining scenarios, traditional GNSS+RTK cannot be used because there is no signal. Currently, IMU is used to obtain acceleration and angular velocity information of the XYZ axes for underground mining scenarios as a reference for high-precision map slope production.

[0003] There is no signal in underground mining scenarios, and the traditional GNSS+RTK method cannot be used in underground mining, and it is impossible to obtain accurate elevation values. Currently, IMU is used to obtain the acceleration and angular velocity information of three axes, and the slope angle corresponding to different positions is obtained through calculation.

[0004] Due to the uneven installation of the IMU instrument and the uneven ground in the well, the vehicle will be bumpy during the movement. In addition, the interference of factors such as temperature changes and equipment wear will affect the measurement accuracy of the instrument, so there is a certain error in the output of the original collected slope value. The slope, curvature, and heading of the high-precision map play a vital role in the control decision of unmanned driving. If the original collected slope value is directly used to make a high-precision map, the accuracy loss will be large, which will greatly affect the control effect of unmanned driving.

[0005] Before collecting slope data, it is necessary to install sensors such as IMU on the collection vehicle. Due to objective external factors, it is impossible to ensure that the instrument is completely level. In addition, the instrument is bumpy during use, the ambient temperature changes, and the instrument wears. If the IMU instrument does not have a built-in tilt compensation algorithm, the output position attitude and acceleration data are unreliable. Directly using them as a reference for making high-precision maps will have large deviations in the slope, making unmanned driving unusable. Summary of the invention

[0006] In response to the problem of low slope accuracy of underground mine maps in the prior art, the present application provides a method and system for processing the slope of underground mine maps, which improves the accuracy by obtaining the IMU posture data, acceleration data and vehicle status data of the unmanned vehicle and performing multiple iterative corrections.

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

[0008] One aspect of the present application provides a method for processing the slope of an underground mining map, comprising: obtaining IMU posture data and acceleration data of an underground mining unmanned vehicle, and obtaining vehicle status data through a CAN bus; preprocessing the acquired IMU posture data, acceleration data, and vehicle status data; extracting the slope values ​​collected when the vehicle is traveling in the forward and reverse directions from the preprocessed IMU posture data to obtain the forward collected slope value and the reverse collected slope value; calculating the first compensation value α of the slope according to each group of extracted forward collected slope values ​​and reverse collected slope values; estimating the road traveled by the unmanned vehicle through a fitting algorithm according to the preprocessed vehicle status data. The surface slope is obtained to obtain an estimated slope value; and the second compensation value β of the slope is calculated based on the estimated slope value and the forward collected slope value and the reverse collected slope value; the comprehensive slope compensation estimate P is calculated based on the first compensation value α and the second compensation value β; the forward collected slope value and the reverse collected slope value are corrected using the comprehensive slope compensation estimate P to obtain the corrected forward collected slope value and the reverse collected slope value; the real slope value of the sampling point is obtained, and the third compensation value γ of the slope is calculated based on the real slope value and the corrected forward collected slope value and the reverse collected slope value; the final slope compensation value is calculated based on the first compensation value α, the second compensation value β and the third compensation value γ ; Use the final slope compensation value Correct the slope of the underground mine map.

[0009] Further, the first compensation value α of the slope is calculated, including: extracting the forward acquisition slope value and reverse slope value collection , i=1, 2, ...., n; calculate the slope value of the i-th group of forward collection and reverse slope value collection The average value of the corresponding group is obtained ; Based on the average slope of each group , calculate the first compensation value α of the slope: , where n is a positive integer.

[0010] Further, the second compensation value β of the slope is calculated, including: the vehicle state data includes the current yaw angle, the front wheel angle, the actual vehicle speed, the expected vehicle speed, the throttle opening and the brake pedal displacement; according to the pre-processed vehicle state data, the least squares method is used for curve fitting to obtain the estimation function ,in, represents the vehicle state data vector, Represents the estimated road slope when the driverless vehicle is driving; the average slope of the i-th group Enter the estimation function , get the corresponding estimated road slope value : , calculate the estimated road slope value of group i With average slope The difference between the values ​​of : , according to the compensation value of each group , calculate the second compensation value β of the slope: .

[0011] Specifically, the current yaw angle (Current Yaw Angle), the yaw angle represents the heading angle of the vehicle in the horizontal plane, that is, the angle between the vehicle's forward direction and the north direction. The current yaw angle refers to the actual heading angle of the vehicle at the current moment. The change in the yaw angle can be obtained by integrating the gyroscope data of the IMU, and then the current yaw angle is obtained by adding the initial angle. The current yaw angle reflects the real-time heading state of the vehicle and is an important parameter for judging the steering and U-turn of the vehicle. Front wheel angle (FrontWheel Angle): The front wheel angle represents the steering angle of the vehicle's front wheels, that is, the angle between the left and right front wheels and the longitudinal axis of the vehicle. The front wheel angle can be measured by the steering wheel angle sensor or steering encoder. The front wheel angle determines the steering radius and curvature of the vehicle. In vehicle steering control and trajectory tracking, the front wheel angle is an important control quantity. Actual Vehicle Speed: Actual vehicle speed indicates the actual speed of the vehicle. The wheel speed is measured by the wheel speed sensor and converted according to the wheel radius. The actual vehicle speed reflects the vehicle's motion state and is the basis for speed control and mileage calculation. In slope estimation, the actual vehicle speed can be used to determine whether the vehicle is traveling at a constant speed and to calculate the distance increment. Desired Vehicle Speed: Desired vehicle speed indicates the target speed set by the vehicle control system or the driver. In automatic driving or cruise control mode, the desired vehicle speed is given by the upper control algorithm. In manual driving mode, the desired vehicle speed can be adjusted by the accelerator pedal or the speed setter. The vehicle's speed control system will adjust the accelerator and brake according to the difference between the desired speed and the actual speed to make the vehicle speed track the desired value. Throttle opening: The throttle opening indicates the depth of the accelerator pedal, which reflects the driver's intention to accelerate. The throttle opening is measured by the throttle pedal position sensor and is usually expressed as a percentage. The larger the throttle opening, the more the driver wants the vehicle to accelerate and the engine output torque increases. In slope estimation, the throttle opening can be used to determine whether the vehicle is accelerating and to estimate the vehicle's traction. Brake pedal displacement: The brake pedal displacement indicates the pedal stroke of the brake pedal, which reflects the driver's intention to decelerate or stop. The brake pedal displacement is measured by a displacement sensor or a pressure sensor. The larger the brake pedal displacement, the more the driver wants the vehicle to decelerate or stop, and the greater the braking force of the braking system. In slope estimation, the brake pedal displacement can be used to determine whether the vehicle is decelerating and to estimate the vehicle's braking force. The six parameters of this application reflect the vehicle's driving state and the driver's control intention from different angles. Combining these parameters can more accurately estimate the road slope on which the vehicle is located.By building a vehicle dynamics model and analyzing the relationship between these parameters and the slope, the compensation amount of the slope estimation value can be obtained, thereby improving the accuracy of the slope estimation.

[0012] Furthermore, according to the preprocessed vehicle status data, the least square method is used to perform curve fitting to obtain the estimated function , including: representing the preprocessed vehicle state data as ,in, is the jth group of vehicle state data vector, including the current yaw angle, front wheel angle, actual vehicle speed, expected vehicle speed, throttle opening and brake pedal displacement; is the corresponding road slope value, , m is the number of groups of vehicle status data; construct the estimation function : ,in, is the parameter to be estimated; construct the least squares optimization objective function: ,in, is the parameter vector; solve the objective function and obtain the optimal solution for the estimated parameter vector w ; The optimal solution Substitute into the estimation function , get vehicle status data Corresponding estimated road slope : .

[0013] Furthermore, the comprehensive slope compensation estimate P is calculated, including: ,in, and is the weight.

[0014] Further, the corrected forward acquisition slope value and reverse acquisition slope value are obtained, including: using the comprehensive slope compensation estimate P to calculate the forward acquisition slope value of the i-th group Make corrections to obtain the corrected forward acquisition slope value : , using the comprehensive slope compensation estimate P to collect the reverse slope value of the i-th group Make corrections to obtain the corrected reverse acquisition slope value : Repeat the forward and reverse slope value correction to obtain a set of corrected forward acquisition slope values and a set of corrected reverse acquisition slope values .

[0015] Furthermore, the third compensation value γ of the slope is calculated, including: selecting multiple sampling points at the mine site and obtaining the true slope value of each sampling point; the sampling points include sampling points of flat road sections, sampling points of uphill sections and sampling points of downhill sections; respectively calculating the true slope value of each sampling point and the corresponding corrected forward acquisition slope value Or reverse to collect slope value The difference between the slopes of each sampling point is obtained by comparing the slope difference of each sampling point with the preset slope threshold. , the statistical slope difference is greater than the slope threshold When the slope difference is greater than the slope threshold The number of sampling points is greater than the preset threshold According to the actual slope value of each sampling point and the corresponding corrected forward slope value Or reverse to collect slope value , the third compensation value γ is obtained by fitting calculation; otherwise, the third compensation value γ is set to 0.

[0016] Further, the third compensation value γ is obtained by fitting calculation, including: the slope difference is greater than the slope threshold The actual slope value of the sampling point and the corresponding corrected forward slope value Or reverse to collect slope value Construct a fitting data set, where the slope value is collected in the positive direction Or reverse to collect slope value is the independent variable, and the true slope value is the dependent variable. , q is the slope difference greater than the slope threshold The number of sampling points; the least squares method is used to perform linear fitting on the fitting data set to obtain the fitting straight line equation: , where z is the corrected forward acquisition slope value Or reverse to collect slope value , y is the corresponding true slope value, c is the slope of the straight line, and d is the intercept of the straight line; substitute the slope c and intercept d in the fitted straight line equation into the following formula to calculate the third compensation value γ: , where z is the corrected forward acquisition slope value Or reverse to collect slope value .

[0017] Further, calculate the final slope compensation value ; Use the final slope compensation value Correction of the slope of the underground mine map, including: ,in, are the weights of the first compensation value α, the second compensation value β and the third compensation value γ respectively; for each slope value in the mine map , using the final slope compensation value Make corrections to get the corrected slope value : ,in, , r is the total number of slope values ​​in the underground mine map.

[0018] Another aspect of the present application provides a system for processing the slope of an underground mine map, which is used to execute a method for processing the slope of an underground mine map of the present application.

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

[0020] By obtaining the IMU posture data, acceleration data and vehicle status data of the unmanned vehicle and performing data preprocessing, high-quality input data can be provided for subsequent slope compensation and correction, reducing the slope error introduced by data quality issues.

[0021] By using the IMU slope data collected when the vehicle is driving forward and reverse, the first slope compensation is achieved through average value calculation, which can effectively reduce the deviation of the data in a single driving direction and improve the reliability of slope estimation.

[0022] The road slope function is estimated based on the vehicle status data, and the second slope compensation is achieved through function fitting, which makes full use of the vehicle status characteristics under the artificial driving behavior habits, making the slope estimation closer to the actual road conditions. The comprehensive slope compensation estimation is introduced, and the advantages and disadvantages of different compensation methods can be balanced by weighted average of the first compensation value and the second compensation value, so as to obtain a more robust slope correction value.

[0023] By comparing the actual slope value of the field sampling point with the corrected collected slope value, and calculating the third compensation value by fitting, the accuracy of slope correction can be further improved in the local area, and the adaptive ability of slope estimation can be enhanced. Finally, the slope correction value is obtained by weighted average of the three compensation values, and is used to correct the slope of the underground mining map. Taking into account multi-source data and multiple compensation methods, the accuracy of the underground mining map slope is greatly improved, meeting the high-precision map requirements of unmanned driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] 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:

[0025] Figure 1is an exemplary flow chart of a method for processing the slope of an underground mine map according to some embodiments of the present application;

[0026] Figure 2 It is a schematic diagram of verifying the positive and negative slope trends according to some embodiments of the present application;

[0027] Figure 3 This is a schematic diagram of verifying the slope at the same position according to some embodiments of the present application. DETAILED DESCRIPTION

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

[0029] like Figure 1 As shown, the IMU posture data and acceleration data of the unmanned vehicle in the underground mine are obtained, and the vehicle status data is obtained through the CAN bus; the obtained IMU posture data, acceleration data and vehicle status data are preprocessed; the slope values ​​collected when the vehicle is traveling in the forward and reverse directions are extracted from the preprocessed IMU posture data to obtain the forward collected slope value and the reverse collected slope value; according to each group of extracted forward collected slope values ​​and reverse collected slope values, the first compensation value α of the slope is calculated; according to the preprocessed vehicle status data, the slope of the road surface on which the unmanned vehicle is traveling is estimated by a fitting algorithm to obtain an estimated slope value; And according to the estimated slope value and the forward collected slope value and the reverse collected slope value, calculate the second slope compensation value β; according to the first compensation value α and the second compensation value β, calculate the comprehensive slope compensation estimate P; use the comprehensive slope compensation estimate P to correct the forward collected slope value and the reverse collected slope value to obtain the corrected forward collected slope value and the reverse collected slope value; obtain the real slope value of the sampling point, and calculate the third slope compensation value γ according to the real slope value and the corrected forward collected slope value and the reverse collected slope value; calculate the final slope compensation value according to the first compensation value α, the second compensation value β and the third compensation value γ ; Use the final slope compensation value Correct the slope of the underground mine map.

[0030] Specifically, select a suitable IMU device, such as a high-precision inertial navigation system or an integrated navigation system, and ensure that it has sufficient accuracy and sampling frequency; install the IMU device on the body of the unmanned mining truck and try to make it level to reduce the impact of installation errors on data collection; ensure that the IMU device is consistent with the vehicle coordinate system, that is, the X-axis points to the direction of vehicle movement, the Y-axis points to the left side of the vehicle, and the Z-axis points to the top of the vehicle; connect the IMU device to the vehicle power supply and establish a communication connection with the on-board computer or data acquisition device.

[0031] Manually start the unmanned mining truck and drive it along the regular route and at the expected speed. Use the IMU device to collect the vehicle's 3D geometric position information (such as longitude, latitude, altitude), attitude angles (such as pitch angle, roll angle, yaw angle) and acceleration data in real time. Obtain vehicle status information in real time through the CAN bus, including the current front wheel turning angle, actual speed, expected speed, throttle command, brake pedal displacement, etc.; Synchronously record the IMU data and CAN bus data and add a timestamp to ensure the timing and correspondence of the data; Perform multiple data collections on different sections and under different working conditions to obtain sufficient data samples and improve data diversity and coverage.

[0032] Store the collected IMU data and CAN bus data in the on-board computer or data acquisition device, and upload them to the data server regularly; classify, label and archive the collected data, such as classifying by collection time, road section, working conditions, etc.; perform quality checks on the data, eliminate abnormal or erroneous data, and ensure data reliability and consistency; establish a data management system to facilitate data query, access and use, and provide support for subsequent data processing and analysis.

[0033] Preprocess the acquired IMU posture data, acceleration data and vehicle status data; specifically, perform sliding average filtering on the angle data output by the IMU to remove high-frequency noise interference. The filter window size can be adjusted according to the data sampling frequency and the vehicle motion characteristics, and a time window of 0.5 seconds to 2 seconds is usually selected. Perform outlier detection and removal on the filtered angle data. Use median filtering or threshold judgment methods to eliminate outliers that exceed the normal range to avoid the impact of extreme data on subsequent calculations. Use the accelerometer data of the IMU to compensate for gravity on the angle data. The components of gravity acceleration on the three axes are measured by the accelerometer, and the inclination angle of the IMU relative to the direction of gravity is calculated to correct the gravity effect in the angle data.

[0034] The three-axis acceleration data output by the IMU is low-pass filtered to remove interference from high-frequency mechanical vibration and electrical noise. The cutoff frequency of the filter can be set according to the installation location of the IMU and the vibration characteristics of the vehicle. Generally, a low-pass filter of 20Hz to 50Hz is selected. Zero bias compensation is performed on the filtered acceleration data. When the vehicle is stationary, the acceleration data for a period of time is recorded, and the average value of the three axes is calculated as the zero bias. When used later, the zero bias value is subtracted from the original data. The motion state of the vehicle is detected according to the acceleration data to determine whether it is in a stationary, uniform, accelerating or decelerating state. The method of using the rate of change of acceleration and threshold judgment can be used to achieve the division of motion states, providing a reference for subsequent data analysis.

[0035] Time synchronization is performed on the vehicle's speed, throttle, brake and other status data. Since the sampling frequencies of different sensors may differ, it is necessary to align the timestamps and unify the data to the same time base. Detect and remove outliers in the status data. According to the physical characteristics and normal working range of the vehicle, set reasonable thresholds to remove outliers that exceed the normal range to avoid erroneous data affecting subsequent analysis. Extract statistical features of vehicle status data, such as the mean and variance of speed, and the working time and frequency of throttle and brake. These statistical features can reflect the driver's behavioral habits and provide useful information for the estimation of the slope function.

[0036] The pre-processed IMU posture data, acceleration data and vehicle status data are aligned according to the timestamp to form a synchronized data sequence. The data of different sensors are fused using data fusion algorithms such as Kalman filtering or complementary filtering to obtain more accurate and stable posture estimation and motion state estimation. Feature extraction is performed on the fused data, such as extracting the statistical characteristics of acceleration, the rate of change of attitude angle, etc., to provide richer feature input for subsequent slope compensation calculations.

[0037] The slope values ​​collected when the vehicle is traveling in the forward and reverse directions are extracted from the preprocessed IMU posture data to obtain the forward slope value and the reverse slope value. Specifically, the heading angle data of the IMU is used to determine the direction of the vehicle. The heading angle data of the IMU reflects the deflection angle of the vehicle relative to the reference direction (such as due north). By analyzing the change trend of the heading angle data, it can be determined whether the vehicle is traveling forward or backward, and a heading angle change threshold is set. When the heading angle changes beyond the threshold, it is considered that the vehicle may have made a U-turn or a U-turn. The driving direction is verified by combining the vehicle speed and acceleration data. The vehicle speed data can reflect the speed and direction of the vehicle (positive value means forward, negative value means backward). The acceleration data can reflect the acceleration change of the vehicle on each axis. When the vehicle speed changes from a positive value to a negative value, and the acceleration on a certain axis (such as the x-axis) changes significantly, it can be determined that the vehicle has made a U-turn. By setting the threshold values ​​of vehicle speed and acceleration changes and combining them with the change in heading angle, the vehicle's driving direction can be accurately determined. The IMU posture data is divided into forward and reverse driving segments according to the driving direction. The IMU posture data is analyzed in time series to find out the time point when the driving direction changes. The IMU data is divided into different driving segments based on the driving direction change point. A segment of data in which the driving direction remains unchanged is a forward or reverse driving segment. In the forward driving segment, the vehicle continuously travels in the direction of the underground mining tunnel, and the corresponding collected slope value is the forward collected slope. In the reverse driving segment, the vehicle is in the opposite direction of the tunnel, and the corresponding collected slope value is the reverse collected slope.

[0038] The IMU attitude solution algorithm is used to calculate the pitch angle of the vehicle at each moment. Specifically, assuming that the vehicle is stationary, the accelerometer is only affected by gravity. At this time, the acceleration vector points in the opposite direction of gravity. The initial pitch angle of the vehicle relative to the horizontal plane can be calculated through the acceleration data. and initial roll angle , initial pitch angle ; Initial roll angle ,in, They are the components of acceleration on the x, y, and z axes of the vehicle coordinate system. Based on the angular velocity data, the change in the vehicle attitude angle can be calculated. Commonly used algorithms for attitude calculation include the Euler angle method and the quaternion method. Here, the Euler angle method is taken as an example. Assume that the pitch angle at the current time t is , the roll angle is , the heading angle is , the angular velocity measured by the gyroscope is , then the attitude angle at the next moment t+1 is: , , ,in, is the sampling time interval. Due to the drift and deviation of angular velocity data, long-term solution will lead to cumulative errors in attitude angle. It is necessary to use acceleration and other data to correct the solution results. Commonly used correction algorithms include complementary filtering and Kalman filtering. Through attitude correction, a more accurate and smooth pitch angle estimation value can be obtained. According to the start and end time of the forward driving segment and the reverse driving segment obtained previously, the time series of the vehicle pitch angle θ in each driving segment is extracted. The pitch angle sequence in the forward driving segment is a set of forward collected slope values, and the pitch angle sequence in the reverse driving segment is a set of reverse collected slope values. Through the IMU attitude solution algorithm, using acceleration and angular velocity data, the pitch angle of the vehicle at each moment during driving can be continuously calculated. When the vehicle is driving forward or reverse, the road slope is consistent with the vehicle's pitch angle. Therefore, the pitch angle sequence in the driving segment is extracted, and the slope collection value of the corresponding road section is obtained. In order to improve the accuracy of slope estimation, it is also necessary to correct the attitude solution results to reduce the influence of IMU drift and deviation.

[0039] For the IMU posture data of the forward driving section, the pitch angle is extracted as the slope value. The IMU posture data usually includes the pitch angle (Pitch), roll angle (Roll) and heading angle (Yaw). The pitch angle represents the angle between the vehicle's forward direction and the horizontal plane, and is an important parameter for determining the road slope. From the IMU data of the forward driving section, the pitch angle value at each moment is extracted one by one, and the extracted pitch angle values ​​are formed into a time series, which corresponds to the timestamp of the IMU data one by one. The extracted pitch angle time series is used as the initial estimate of the road slope. When the pitch angle is positive, it means that the vehicle is driving uphill and the slope is positive. When the pitch angle is negative, it means that the vehicle is driving downhill and the slope is negative. The size of the pitch angle reflects the steepness of the slope. The larger the angle, the steeper the slope. Due to the bumps and vibrations during vehicle driving, the pitch angle data collected by the IMU will fluctuate instantaneously. In order to reduce the impact of fluctuations on slope estimation, the slope value sequence needs to be smoothed. Commonly used smoothing methods are sliding average and low-pass filtering. Sliding average method: select an appropriate window size (such as 5 or 10 data points), average the slope values ​​in the window, and use it as the smoothing result of the center point. The window slides in sequence to obtain a smoothed slope sequence. Low-pass filtering method: design a low-pass filter, such as a first-order or second-order IIR filter, select a suitable cutoff frequency, filter the slope sequence, filter out high-frequency noise and fluctuations, and obtain a smooth slope sequence. In order to match the slope value with the vehicle's travel position, it is necessary to record the collection time or mileage of each slope value. IMU data usually carries timestamp information, and the timestamp can be directly associated with the corresponding slope value. If the IMU data does not have a timestamp, the time of each data point can be calculated through the sampling frequency and serial number. According to the vehicle speed and the sampling frequency of the IMU data, the mileage position corresponding to each slope value can also be estimated. Through the timestamp or mileage position, the smoothed slope sequence can be matched with the vehicle's travel position trajectory.

[0040] Through the above processing, the mapping relationship between the vehicle driving position and the corresponding slope value in the forward driving segment is obtained. Two forms of slope value sequences can be generated: using timestamp as index, recording the slope value at each moment, reflecting the trend of slope change over time; using mileage position as index, recording the slope value at each position, reflecting the distribution of slope along the driving path. The form of spatiotemporal sequence can be selected according to the needs of subsequent applications.

[0041] For the IMU posture data of the reverse driving segment, the pitch angle is also extracted as the slope value. However, after the vehicle turns around, the sign of the pitch angle is opposite to that of the forward driving, so the slope value needs to be reversed. The extracted slope value is smoothed to reduce the instantaneous fluctuations caused by bumps and vibrations. The same smoothing method as the forward driving segment can be used. According to the acquisition time or mileage, the slope value sequence of the reverse driving segment is matched with the corresponding position information to form a time series or spatial series of reverse acquisition slope values.

[0042] Synchronize and align the forward and reverse slope value sequences according to timestamps or mileage. Ensure that the slope values ​​in the two directions have a corresponding relationship at the same position. For some special sections, such as crossroads or roundabouts, vehicles may pass the same location multiple times. It is necessary to distinguish and mark the slope values ​​collected multiple times based on the location information and timestamp. Detect and remove outliers in the forward and reverse slope value sequences. Use statistical methods or physical constraints to identify outliers that significantly deviate from the normal range. For detected outliers, interpolation or local averaging can be used to correct or eliminate them to ensure the continuity and stability of the slope value sequence.

[0043] According to each group of forward slope values ​​and reverse slope values ​​extracted, the first compensation value α of the slope is calculated; specifically, the forward slope value sequence and the reverse slope value sequence are grouped according to the location information (such as mileage or coordinates). Each group contains the slope values ​​collected at the same location in both the forward and reverse directions.

[0044] For each set of data, extract the forward acquisition slope value and reverse slope value collection , where i represents the i-th group of data, and the value range is 1 to n, where n is a positive integer representing the total number of data groups. For the i-th group of data, calculate the forward acquisition slope value and reverse slope value collection The average slope of the group is obtained by averaging Average slope The calculation formula is: Repeat the above steps to calculate the average slope of all n groups of data and obtain an average slope sequence containing n elements. According to the average slope of each group , calculate the first compensation value α of the slope. The calculation formula for the first compensation value α is: That is, the average slopes of all groups are summed up and then divided by the total number of data groups n to obtain the first compensation value α.

[0045] According to the preprocessed vehicle state data, the slope of the road on which the unmanned vehicle is traveling is estimated by a fitting algorithm to obtain an estimated slope value; and according to the estimated slope value and the forward collected slope value and the reverse collected slope value, the second compensation value β of the slope is calculated; specifically, the preprocessed vehicle state data is represented as a set of vectors and scalars: , where m is the number of vehicle status data groups. Specifically, the vehicle status data contains 6 variables: current yaw angle, front wheel angle, actual vehicle speed, expected vehicle speed, throttle opening and brake pedal displacement. These 6 variables reflect the vehicle's motion state and the driver's control intention from different angles, and have a certain correlation with the road slope. These 6 variables are combined into a vehicle status data vector ,in: represents the current yaw angle of the jth group of data, represents the front wheel turning angle of the jth group of data, represents the actual vehicle speed of the jth group of data, represents the expected vehicle speed of the jth group of data, represents the throttle opening of the jth group of data, Represents the brake pedal displacement of the jth group of data. Each group of vehicle state data vector Corresponding to a road slope value , forming a data point For the jth group of data, is the vehicle state data vector, which includes k state variables such as current yaw angle, front wheel angle, actual vehicle speed, expected vehicle speed, throttle opening and brake pedal displacement. is the road slope value corresponding to the jth group of data, which can be directly measured by sensors such as IMU or estimated according to other methods.

[0046] Assuming that there is a certain functional relationship between the road slope on which the driverless vehicle is traveling and the vehicle status data, a linear function can be used to approximate this relationship. Construct an estimation function : ,in, is the parameter to be estimated. Estimation function Indicates that given vehicle status data In the case of , the estimated road slope value. In order to find the optimal estimation function parameters, the least squares optimization objective function is constructed: ,in, is the parameter vector.

[0047] Objective Function Represents the estimated function The output and the true slope value The sum of squared errors between , by minimizing this sum of squared errors, we can get the optimal parameter estimation. Use the least squares optimization algorithm, such as gradient descent method, conjugate gradient method, etc., to solve the objective function and get the optimal solution for estimating the parameter vector w . Substitute the optimal parameter solution w into the estimation function , get vehicle status data Corresponding estimated road slope : For each set of vehicle status data , calculate the corresponding estimated slope value , and obtain a sequence of estimated slope values.

[0048] For the i-th group of data, the average slope Enter the estimation function , get the corresponding estimated road slope value : . Calculate the estimated road slope value for group i With average slope The difference between the values ​​of : Repeat the above steps to calculate the compensation values ​​of all n groups of data. , , i is a positive integer in this application. According to the compensation value of each group , calculate the second compensation value β of the slope: , that is, sum the compensation values ​​of all groups and then divide it by the total number of data groups n.

[0049] According to the first compensation value α and the second compensation value β, the comprehensive slope compensation estimate P is calculated; specifically, according to the reliability and importance of the first compensation value α and the second compensation value β, their weight coefficients in the comprehensive slope compensation estimate P are determined. and . Weight coefficient and The value range is from 0 to 1 and satisfies The two weight coefficients can usually be set based on experience or test results. For example, if the first compensation value α is more reliable, A larger weight; if the second compensation value β takes more influencing factors into account, it can be given The weighted average method is used to calculate the comprehensive slope compensation estimate P. The calculation formula for the comprehensive slope compensation estimate P is: Multiply the first compensation value α and the second compensation value β by their corresponding weight coefficients and , and then add them together to get the weighted sum, and then divide them by the sum of the weight coefficients to get the final comprehensive slope compensation estimate P. The comprehensive slope compensation estimate P combines the results of the two compensations, balances the contributions of the two compensations by weighted average, and obtains a more accurate and stable compensation value.

[0050] The forward acquisition slope value and the reverse acquisition slope value are corrected using the comprehensive slope compensation estimate P to obtain the corrected forward acquisition slope value and the reverse acquisition slope value; specifically, for the forward acquisition slope value of the i-th group , use the comprehensive slope compensation estimate P to make corrections and get the corrected forward acquisition slope value Corrected forward acquisition slope value The calculation formula is: Add the composite slope compensation estimate P to the original forward acquisition slope value The corrected forward slope value is obtained. For the reverse collection slope value of group i , use the comprehensive slope compensation estimate P to make corrections and get the corrected reverse acquisition slope value . Corrected reverse acquisition slope value The calculation formula is: Add the composite slope compensation estimate P to the original reverse acquisition slope value The corrected reverse acquisition slope value is obtained. Repeat the above correction process for all n groups of forward slope values ​​and reverse slope values. Get a group of corrected forward slope values , and a set of corrected reverse-collected slope values The corrected slope value sequence reflects the correction effect of the comprehensive slope compensation estimate P on the original collected slope value, making the slope values ​​collected in the forward and reverse directions closer to the actual road slope.

[0051] The actual slope value of the sampling point is obtained, and the third compensation value γ of the slope is calculated according to the actual slope value and the corrected forward and reverse collected slope values, including: selecting multiple representative sampling points at the mine site, covering different road types such as flat road sections, uphill sections and downhill sections. The sampling points should be selected as evenly as possible to reflect the slope changes of the entire mine road. For each sampling point, its location information (such as mileage or coordinates) is recorded to match it with the corresponding forward and reverse collected slope values.

[0052] For each sampling point, use high-precision measuring equipment (such as total station, level, etc.) to measure its true slope value. The measurement should be carried out strictly in accordance with the operating specifications to minimize human errors and environmental interference to ensure the accuracy of the measurement results. The true slope value of each sampling point is recorded and corresponds to its location information to form a data set of true slope values.

[0053] For each sampling point, find the corresponding corrected forward slope value according to its location information. Or reverse to collect slope value Calculate the true slope value of each sampling point and the corresponding corrected forward slope value Or reverse to collect slope value The difference between the slope values ​​of each sampling point is obtained by calculating the slope difference between the corrected collected slope value and the true slope value. The larger the difference, the worse the correction effect.

[0054] Set a preset slope threshold , as the basis for judging whether the third compensation is needed. For each sampling point, compare its slope difference with the slope threshold The size of the statistical slope difference is greater than the slope threshold The number of sampling points. Set a preset number threshold sum0 as another basis for judging whether the third compensation is needed. If the slope difference is greater than the slope threshold If the number of sampling points is greater than the quantity threshold sum0, it is considered that the correction effect is not ideal and a third compensation is required; otherwise, the third compensation value γ is set to 0 and no compensation is performed.

[0055] If a third compensation is required, the slope difference is greater than the slope threshold. The actual slope value of the sampling point and the corresponding corrected forward slope value Or reverse to collect slope value The fitting data set is formed. In the fitting data set, the slope value is collected in the positive direction. Or reverse to collect slope value is the independent variable, and the true slope value is the dependent variable. Assume that the fitting data set contains q data points, that is, the slope difference is greater than the slope threshold The number of sampling points is q.

[0056] The least squares method is used to perform linear fitting on the fitting data set to obtain the fitting line equation: Among them, z is the corrected forward acquisition slope value Or reverse to collect slope value , y is the corresponding true slope value, c is the slope of the line, and d is the intercept of the line. The goal of linear fitting is to find a straight line that minimizes the sum of the squares of the vertical distances from all points in the fitting data set to the line. Substitute the slope c and intercept d in the fitting line equation into the following formula to calculate the third compensation value γ: Among them, z is the corrected forward acquisition slope value Or reverse to collect slope value The third compensation value γ represents the residual deviation between the corrected collected slope value and the true slope value. The collected slope value can be further corrected by γ to make it closer to the true slope value. The third compensation is based on the first two compensations, and the true slope value is introduced as a reference. The mapping relationship between the collected slope value and the true slope value is established by the linear fitting method, which further improves the accuracy of the slope estimation. At the same time, by setting the slope threshold and the quantity threshold, it can be determined whether the third compensation is needed, avoiding unnecessary calculations and improving the efficiency of the algorithm.

[0057] According to the first compensation value α, the second compensation value β and the third compensation value γ, the final slope compensation value is calculated. ; Use the final slope compensation value Correct the slope of the underground mine map. Specifically, according to the reliability and importance of the first compensation value α, the second compensation value β and the third compensation value γ, determine their role in the final slope compensation value. The weight coefficient in . Weight coefficient The value range is from 0 to 1 and satisfies The three weight coefficients can usually be set based on experience or test results. For example, if the first compensation value α has the highest reliability, A greater weight; if the third compensation value γ introduces the real slope value as a reference, K3 can be given a greater weight. The final slope compensation value is calculated using the weighted average method. Final slope compensation value The calculation formula is: Multiply the first compensation value α, the second compensation value β and the third compensation value γ by their corresponding weight coefficients , then add them together to get the weighted sum, and then divide by the sum of the weight coefficients to get the final slope compensation value Final slope compensation value The results of the three compensations are combined, and the contributions of the three compensations are balanced through weighted averaging to obtain a more accurate and stable compensation value.

[0058] For each slope value in the mine map , using the final slope compensation value Make corrections to get the corrected slope value Corrected slope value The calculation formula is: . The final slope compensation value Add to original slope value The corrected slope value is obtained Assuming that there are r slope values ​​in the mine map, the value range of i is 1 to r, that is, .

[0059] When the slope difference of most sampling points is greater than the preset accuracy threshold When the slope difference of a small number of sampling points exceeds the accuracy threshold, it is considered that there is a systematic overall deviation in the slope, and secondary compensation is required within the slope accuracy range. When the accuracy exceeds the threshold, local correction is considered, and global compensation is not performed. The local correction method can be to compensate the local area where the sampling points that exceed the accuracy threshold are located separately, or to directly correct the slope values ​​of these sampling points so that they fall within the accuracy range. According to the final optimized compensation value , correct the slope value of the collected trajectory again, so that the corrected slope value is closer to the actual slope value within the range that meets the accuracy requirements. Take the sampling points of the high-precision map trajectory as the benchmark, find the matching trajectory sampling points, and assign the corrected slope value of the original trajectory sampling point to the high-precision map. After assigning the slope of the high-precision map, a quality check is required to ensure the accuracy of the assignment. Use visualization to verify whether the slope trends of the forward and reverse acquisitions are opposite, such as Figure 2 In the slope values ​​collected in the forward and reverse directions, the slope values ​​at the same location cannot differ too much, such as Figure 3 For non-conformities found during the verification process, it is necessary to continue to analyze and adjust the compensation estimates and weights until the verification effect is optimal.

[0060] This application calculates the final slope compensation value based on the first compensation value α, the second compensation value β and the third compensation value γ , and use the final slope compensation value Correct the slope of the underground mine map. Final slope compensation value The results of the three compensations were combined to obtain a more accurate and stable compensation value through weighted averaging. At the same time, according to the slope difference of the sampling points, it was determined whether local correction was needed, which improved the flexibility of slope correction. Finally, by verifying the correction effect, it was ensured that the corrected slope value was closer to the actual slope value and met the accuracy requirements. This method of multiple iterative compensation and verification can effectively improve the accuracy and reliability of the slope of the underground mine map.

Claims

1. A method for processing the slope of a mine map, characterized in that: include: Obtain IMU posture data and acceleration data of unmanned vehicles in underground mines, and obtain vehicle status data through the CAN bus; Preprocess the acquired IMU posture data, acceleration data and vehicle status data; Extract the slope values ​​collected when the vehicle is traveling in the forward and reverse directions from the preprocessed IMU posture data to obtain the forward slope value and the reverse slope value; Calculate the first compensation value α of the slope according to each group of forward slope values ​​and reverse slope values ​​extracted; According to the preprocessed vehicle state data, the slope of the road on which the driverless vehicle is traveling is estimated by a fitting algorithm to obtain an estimated slope value; And according to the estimated slope value, the forward collected slope value and the reverse collected slope value, the second compensation value β of the slope is calculated; Calculate the comprehensive slope compensation estimate P according to the first compensation value α and the second compensation value β; The forward acquisition slope value and the reverse acquisition slope value are corrected by using the comprehensive slope compensation estimate P to obtain the corrected forward acquisition slope value and the reverse acquisition slope value; Obtain the true slope value of the sampling point, and calculate the third compensation value γ of the slope according to the true slope value and the corrected forward collected slope value and reverse collected slope value; According to the first compensation value α, the second compensation value β and the third compensation value γ, the final slope compensation value is calculated. ; Use the final slope compensation value Correct the slope of the underground mine map.

2. The method for processing the slope of a mine map according to claim 1, characterized in that: Calculate the first compensation value α of the slope, including: Extract the forward acquisition slope value and reverse slope value collection ; Calculate the slope value of the i-th group of forward collection and reverse slope value collection The average value of the corresponding group is obtained ; According to the average slope of each group , calculate the first compensation value α of the slope: ； Wherein, n is a positive integer.

3. The method for processing the slope of a mine map according to claim 2, characterized in that: Calculate the second compensation value β of the slope, including: The vehicle status data includes the current yaw angle, front wheel angle, actual vehicle speed, expected vehicle speed, throttle opening and brake pedal displacement; According to the preprocessed vehicle status data, the least squares method is used to perform curve fitting to obtain the estimated function ,in, represents the vehicle state data vector, represents the estimated road slope when the autonomous vehicle is traveling; The average slope of group i Enter the estimation function , get the corresponding estimated road slope value : ； Calculate the estimated road slope value for group i With average slope The difference between the values ​​of : ； According to the compensation value of each group , calculate the second compensation value β of the slope: 。 4. The method for processing the slope of a mine map according to claim 3 is characterized in that: According to the preprocessed vehicle status data, the least squares method is used to perform curve fitting to obtain the estimated function ,include: The preprocessed vehicle status data is represented as ,in, is the jth group of vehicle state data vector, including the current yaw angle, front wheel angle, actual vehicle speed, expected vehicle speed, throttle opening and brake pedal displacement; is the corresponding road slope value; m is the number of vehicle status data groups; k represents a positive integer; Constructing the estimation function : ； in, is the parameter to be estimated; Construct the least squares optimization objective function: ； in, is the parameter vector; Solve the objective function and get the parameter vector The optimal solution ; The optimal solution Substitute into the estimation function , get the jth group of vehicle state data vector Corresponding estimated road slope : 。 5. The method for processing the slope of a mine map according to claim 1, characterized in that: Calculate the comprehensive slope compensation estimate P, including: ； in, and is the weight.

6. The method for processing the slope of an underground mine map according to any one of claims 2 to 5, characterized in that: The forward and reverse collected slope values ​​are corrected using the comprehensive slope compensation estimate P, including: The forward slope value of the i-th group is collected using the comprehensive slope compensation estimate P Make corrections to obtain the corrected forward acquisition slope value : ； The slope value of the i-th group is collected in reverse using the comprehensive slope compensation estimate P Make corrections to obtain the corrected reverse acquisition slope value : ； Repeat the forward and reverse slope value correction to obtain a set of corrected forward acquisition slope values and a set of corrected reverse acquisition slope values .

7. The method for processing the slope of a mine map according to claim 6, characterized in that: Calculate the third compensation value γ of the slope, including: Select multiple sampling points at the mine site and obtain the true slope value of each sampling point; the sampling points include flat road section sampling points, uphill road section sampling points and downhill road section sampling points; Calculate the true slope value of each sampling point and the corresponding corrected forward acquisition slope value Or reverse to collect slope value The difference between them is used to obtain the slope difference of each sampling point; Compare the slope difference of each sampling point with the preset slope threshold , the statistical slope difference is greater than the slope threshold the number of When the slope difference is greater than the slope threshold The number of sampling points is greater than the preset threshold According to the actual slope value of each sampling point and the corresponding corrected forward slope value Or reverse to collect slope value , the third compensation value γ is obtained by fitting calculation; otherwise, the third compensation value γ is set to 0.

8. The method for processing the slope of a mine map according to claim 7, characterized in that: The third compensation value γ is obtained by fitting calculation, including: The slope difference is greater than the slope threshold The actual slope value of the sampling point and the corresponding corrected forward slope value Or reverse to collect slope value Construct a fitting data set, where the slope value is collected in the positive direction Or reverse to collect slope value is the independent variable, and the true slope value is the dependent variable. , q is the slope difference greater than the slope threshold The number of sampling points; The least squares method is used to perform linear fitting on the fitting data set to obtain the fitting line equation: , where z is the corrected forward acquisition slope value Or reverse to collect slope value , y is the corresponding true slope value, c is the slope of the straight line, and d is the intercept of the straight line; Substitute the slope c and intercept d in the fitting straight line equation into the following formula to calculate the third compensation value γ: ； Among them, z is the corrected forward acquisition slope value Or reverse to collect slope value .

9. The method for processing the slope of a mine map according to claim 8, characterized in that: Using the final slope compensation value Correction of the slope of the underground mine map, including: Calculate the final slope compensation value , through the following formula: ； in, are the weights of the first compensation value α, the second compensation value β and the third compensation value γ respectively; For each slope value in the mine map , using the final slope compensation value Make corrections to get the corrected slope value : ； in, , r is the total number of slope values ​​in the underground mine map.

10. A system for processing the slope of a mine map, characterized in that: include: At least one processing unit; used to execute instructions to implement the method for processing the slope of an underground mine map as described in any one of claims 1 to 9.

Images