Method for positioning accurate position of monorail crane in underground environment

Abstract

The invention relates to a method for positioning an accurate position of a monorail crane in an underground environment, and belongs to the field of positioning of underground coal mine mobile equipment. Comprising the following steps: determining two-dimensional space coordinates of the position of the monorail crane, obtaining encoder driving data of the monorail crane, establishing a state displacement relation process model of the position coordinates, the driving speed and the course angle, and speculating the state of the monorail crane at the next moment; the position of the monorail crane is measured through wireless signal positioning, position data is updated and judged, a predicted position and a wireless positioning position are processed, and prediction reliability is judged. And after the observed value and the predicted value are fused, optimal position estimation and latest uncertainty evaluation are obtained, and real-time positioning of the locomotive is realized. According to the underground coal mine single track positioning method, encoder sensor positioning and UWB and RFI radio frequency identification wireless positioning are fused, and a layered positioning system with complementary advantages is formed. And the positioning precision of the monorail crane in the coal mine tunnel is improved.

Description

Technical Field

[0001] This invention relates to a method for precise positioning of a monorail crane in underground environments, belonging to the field of positioning of underground mobile equipment in coal mines. Background Technology

[0002] Monorail locomotives are auxiliary transportation equipment that runs on the overhead rails in coal mine roadways, enabling efficient transport of personnel and materials. With the implementation of smart mines and other measures, unmanned operation of monorail locomotives has become a hot research topic. Locomotive positioning is a key aspect of achieving unmanned operation, and accurate and stable positioning technology urgently needs to be developed.

[0003] The underground environment of coal mines is complex and variable, with numerous metal structures, electromagnetic interference, and diverse environmental conditions. Traditional positioning technologies or single-sensor-based positioning schemes are insufficient to meet the requirements of high accuracy and stability. To address this issue, multi-sensor fusion technology is the preferred method for achieving precise positioning of monorail cranes. Patent "A Multi-Source Information Fusion Precise Positioning Method and System for Monorail Cranes in Coal Mines (CN114701544A)" improves positioning accuracy through data fusion. It integrates UWB positioning, strapdown inertial navigation (INS), and map building systems (such as LiDAR or visual cameras) based on the actual layout of the tunnels. A Kalman filter model is constructed to correct the monorail crane's position, addressing the cumulative errors of UWB and INS, thereby improving accuracy and anti-interference capabilities. Patent "A UWB-Based Precise Positioning System for Underground Coal Mines (CN119110389A)" installs a UWB base station on the monorail crane (as a receiver) and fixes a UWB tag to the tunnel wall or ceiling (as a transmitter). The PDOA (Phase Differential Arrival) algorithm is used to calculate the location for dispatch management and emergency rescue. Summary of the Invention

[0004] To address the aforementioned problems, this invention provides a method for precise location positioning of a monorail crane in an underground environment. This positioning method integrates positioning calculations using three sensors: UWB, encoder, and RFID radio frequency identification, achieving efficient collaborative positioning from these three sensor data sources.

[0005] To achieve the above objectives, the rapid solution adopted by the present invention is:

[0006] This invention relates to a method for precise location positioning of a monorail crane in an underground environment.

[0007] Includes the following steps,

[0008] 1) Determine the two-dimensional spatial coordinates of the monorail locomotive's position and establish an initial state model of the monorail locomotive's position in the roadway;

[0009] 2) Obtain the encoder travel data of the monorail crane, including the displacement increment and heading angle change at the current moment, and record and save it according to time;

[0010] 3) Establish a process model of the state-displacement relationship between position coordinates, travel speed, and heading angle, and predict the state of the monorail crane at the next moment;

[0011] 4) The position of the monorail crane is measured by wireless signal positioning to provide correction points and establish a position observation model based on wireless positioning;

[0012] 5) Location data update judgment: The predicted location is processed and compared with the wireless positioning location to determine the reliability of the prediction.

[0013] 5.1) If the wireless positioning observation is reliable, trust the wireless positioning location and use the wireless positioning location observation value to correct the estimated value;

[0014] 5.2) If wireless positioning observations are unreliable, rely primarily on estimated values ​​rather than relying on limited reference observations;

[0015] 5.3) If wireless positioning fails and observations are lacking, rely entirely on the predicted values;

[0016] 6) Output data: After fusing the observed and predicted values, the optimal location estimate and the latest uncertainty assessment are obtained.

[0017] 7) Repeat steps 2) through 5) to achieve real-time locomotive positioning.

[0018] According to the method for precise location positioning of a monorail crane in the underground environment, in step 1), the state model is a state vector. At time k, the formula for the system's state vector is:

[0019] ,

[0020] in: , This indicates the locomotive's position coordinates in a two-dimensional plane (unit: meters).

[0021] This indicates the locomotive's forward speed, measured in meters per second (m / s). The locomotive's heading angle is the angle between its direction of travel and the X-axis of the global coordinate system, expressed in radians.

[0022] According to the method for precise positioning of a monorail crane in the underground environment, the control input (from the encoder) in step 2) serves to obtain the displacement increment and heading angle change from the encoder. At time k, the control input obtained from the encoder is: ,

[0023] in: It is the displacement increment from the previous moment to the current moment, in meters.

[0024] It is the change in heading angle from the previous moment to the current moment, in radians.

[0025] According to the method for precise positioning of a monorail locomotive in the underground environment, in step 3), the model equation for the displacement relationship process of the monorail locomotive from time k-1 to time k is: ,

[0026] in, It is a nonlinear state transition function. It is a process noise vector with a mean of 0 and a covariance matrix of... Gaussian distribution;

[0027] Nonlinear state transition function The specific form is:

[0028] ,

[0029] ,

[0030] Or other speed models that depend on the encoder

[0031] ,

[0032] .

[0033] According to the method for precise location positioning of a monorail crane in the underground environment, in step 4), the location of the monorail crane is measured by wireless signal positioning, which can be either UWB communication positioning or RFID tag positioning.

[0034] Based on the aforementioned method for precise location positioning of a monorail crane in the underground environment, the UWB observation equation for UWB communication positioning is expressed as: ,

[0035] in: It is the observation matrix. This indicates that UWB only observes the location (px, py).

[0036] It is a UWB observation noise vector with a mean of 0 and a covariance matrix of... The Gaussian distribution.

[0037] Based on the aforementioned method for precise location positioning of a monorail crane in the underground environment, the RFID observation equation for RFID tag positioning is expressed as: , in: It is the observation matrix, and same, , It is an RFID observation noise vector with a mean of 0 and a covariance matrix of... The Gaussian distribution. Set to a very small value This means that we almost completely trust the coordinates provided by RFID.

[0038] According to the method for precise location positioning of a monorail locomotive in the underground environment, in step 5), the initial state estimate is first set to predict the current locomotive position: Set the initial state estimation error covariance matrix: For each time step k = 1, 2, ..., execute the following loop:

[0039] 5.1) Generate the Sigma point set based on the posterior estimate from the previous time step. Calculate 2L+1 Sigma points (i=0,1,...,2L), ,

[0040] i = 1,...,L

[0041] , i = L+1,...,2L,

[0042] in, ,

[0043] 5.2) Propagate Sigma points by passing each Sigma point through the process model. Propagation is performed to obtain the predicted Sigma point. ,

[0044] i=0,1,...,2L,

[0045] 5.3) Calculate the prior state prediction and prior covariance prediction, and calculate the mean of the predicted state. ,

[0046] ,

[0047] Calculate the covariance of the predicted state ,

[0048] ,

[0049] in, and Qk is the weighting coefficient calculated for the Sigma point. It is usually a diagonal matrix, and the elements on the diagonal represent the noise variance of each state dimension. The value of Qk needs to be determined through experiments and debugging to balance the confidence of predictions and observations.

[0050] According to the method for precise location positioning of a monorail crane in the underground environment, in step 6)...

[0051] If UWB or RFID wireless observation data arrives at the current moment:

[0052] Predicted Sigma points The predicted observation points are obtained through observation model transmission. ,

[0053] i=0,1,...,2L

[0054] Where H is either H_uwb or H_rfid,

[0055] Calculate the mean of the predicted observations ,

[0056] ,

[0057] Calculate the innovation covariance matrix ,

[0058] ,

[0059] Where R is or It is a covariance matrix representing the magnitude of the noise from the observed sensor, and... These factors collectively determine whether the filter places more trust in predictions or observations;

[0060] Calculate the cross-covariance matrix of state and observation. ,

[0061] ;

[0062] Calculate the Kalman gain and update the state:

[0063] Calculate Kalman gain ,

[0064] ,

[0065] Obtain actual observations ( or ),

[0066] Update the state estimate (compile the posterior state estimate) ,

[0067] ,

[0068] Update the covariance estimate (calculate the posterior error covariance matrix). ,

[0069] ,

[0070] Output As the optimal state estimate at time k;

[0071] If there is no observation data at the current moment:

[0072] Then skip the update step and directly output the result of the prediction step:

[0073] ,

[0074] .

[0075] This invention presents a single-track positioning method for underground coal mines that integrates encoder sensor positioning with UWB and RFID (Radio Frequency Identification) wireless positioning, forming a complementary and hierarchical positioning and control system. This improves the positioning accuracy of monorail cranes in coal mine roadways. UWB provides wide-area, high-precision absolute coordinate reference; the encoder provides continuous, high-frequency relative motion data; and RFID, placed at key locations along the crane's path, provides unambiguous, zero-error absolute position beacons, primarily used for safety logic verification and final calibration. UWB + encoder provides continuous positioning on a "surface" scale, while RFID provides absolute verification at a "point" scale. In this invention, unscented Kalman filtering (UKF) is used for continuous state estimation to predict and update the position, processing high-frequency, continuous but noisy data (UWB and encoder data), achieving efficient collaboration among the three sensor data sources and avoiding the insufficient accuracy issues associated with single-method positioning. Attached Figure Description

[0076] Figure 1 This is a flowchart of the positioning process algorithm of the present invention. Detailed Implementation

[0077] The following is a further explanation of the content of this invention:

[0078] The present invention provides a method for precise location positioning of a monorail crane in an underground environment, comprising the following steps.

[0079] 1) Determine the two-dimensional spatial coordinates of the monorail locomotive's position and establish an initial state model of the monorail locomotive's position in the roadway. At time k, the system's state vector is:

[0080] in: , This indicates the locomotive's position coordinates in a two-dimensional plane (unit: meters).

[0081] This indicates the locomotive's forward speed (unit: meters per second).

[0082] This represents the locomotive's heading angle, which is the angle between the direction of travel and the X-axis of the global coordinate system (unit: radians).

[0083] 2) Obtain the encoder travel data of the monorail crane, including displacement increment and heading angle change. At time k, the control input obtained from the encoder is: ,

[0084] in: It is the displacement increment from the previous moment to the current moment (unit: meters).

[0085] It is the change in heading angle from the previous moment to the current moment (unit: radians).

[0086] 3) Infer the locomotive's state at the next moment based on the encoder data, that is, infer the relevant parameters in the locomotive's state vector.

[0087] The equation for predicting the state from time k-1 to time k is:

[0088] in, It is a nonlinear state transition function. It is a process noise vector with a mean of 0 and a covariance matrix of... The Gaussian distribution.

[0089] Nonlinear state transition function The specific form is:

[0090] ,

[0091] ,

[0092] Or depends on the encoder's speed model

[0093]

[0094] ,

[0095] 4) The position of the monorail crane is measured using wireless signal positioning to provide correction points and establish a position observation model based on wireless positioning.

[0096] Wireless signal positioning uses two methods: UWB and RFID, to measure the absolute position of the locomotive and provide calibration points.

[0097] UWB positioning is affected by noise, while RFID positioning is discontinuous. Both have certain drawbacks, so we are considering combining them to provide a more accurate positioning solution.

[0098] UWB observation model:

[0099] The UWB observation equation is expressed as follows: ,

[0100] in: It is the observation matrix. This indicates that UWB only observes the location (px, py).

[0101] It is a UWB observation noise vector with a mean of 0 and a covariance matrix of... The Gaussian distribution.

[0102] RFID observation model:

[0103] The RFID observation equation is expressed as: , in: It is the observation matrix, and same, ;

[0104] It is an RFID observation noise vector with a mean of 0 and a covariance matrix of... The Gaussian distribution. Set to a very small value (e.g.) This indicates that we almost completely trust the coordinates provided by RFID.

[0105] 5) The intelligent system integrates the estimated position of the monorail locomotive with the position of the monorail locomotive observed by wireless positioning, and outputs the optimal locomotive position estimate.

[0106] The process is as follows:

[0107] 5.1) Prediction: The input data are the best estimated position at the previous moment and the encoder measurement data. The process model is used, and the state dimension is set to L = 4.

[0108] UKF parameters: α = 0.001, β = 2, k = 0,

[0109] Initialization, setting initial state estimates: Set the initial state estimation error covariance matrix: For each time step k = 1, 2, ..., execute the following loop:

[0110] Step 1: Generate the Sigma point set based on the posterior estimate from the previous time step. Calculate 2L+1 Sigma points (i=0,1,...,2L).

[0111]

[0112] , i = 1,...,L

[0113] i = L+1,...,2L

[0114] in,

[0115] The second part involves propagating the Sigma points, passing each Sigma point through the process model. Propagation is performed to obtain the predicted Sigma point. .

[0116] i=0,1,...,2L

[0117] The third part calculates the prior state prediction and prior covariance prediction, and calculates the mean of the predicted state (prior state estimate). :

[0118] ,

[0119] Calculate the covariance of the predicted state (prior error covariance matrix). ,

[0120] ,

[0121] in, and These are the weighting coefficients calculated for the Sigma points. Qk is typically a diagonal matrix, with the diagonal elements representing the noise variance for each state dimension. The value of Qk needs to be determined through experimentation and tuning to balance the confidence levels of predictions and observations.

[0122] 5.2) Update: Input data includes the predicted location plus the actual observed location (UWB or RFID). A decision is made based on the prediction reliability, and the predicted Sigma point is updated. The predicted observation points are obtained through the observation model H. .

[0123] i=0,1,...,2L

[0124] Where H is either H_uwb or H_rfid

[0125] Calculate the mean of the predicted observations

[0126] ,

[0127] Calculate the innovation covariance matrix

[0128] ,

[0129] Where R is or , which is a covariance matrix representing the magnitude of the observed sensor noise, is a crucial parameter in the UKF algorithm, and is related to... Together, they determine whether the filter trusts the predictions or the observations more.

[0130] Calculate the cross-covariance matrix of state and observation.

[0131] .

[0132] If the observation is reliable ( or (Very small), trust the observations, use the observations to correct the predictions, and significantly reduce the "uncertainty";

[0133] If the observation is unreliable ( or The difference is significant; for example, the UWB signal is relatively poor. With fewer reference observations and a greater reliance on predictions, the "uncertainty" is reduced very little.

[0134] If there is no observation (e.g., UWB signal loss), the predicted value can be completely trusted, but it is also clear that the "uncertainty" will increase.

[0135] 6) Calculate the Kalman gain and update the state:

[0136] Calculate Kalman gain ,

[0137] ,

[0138] Obtain actual observations ( or ).

[0139] Update the state estimate (compile the posterior state estimate)

[0140] ,

[0141] Update the covariance estimate (calculate the posterior error covariance matrix). :

[0142] ,

[0143] Output As the optimal state estimate at time k.

[0144] If there is no observation data at the current moment:

[0145] Then skip the update step and directly output the result of the prediction step:

[0146] ,

[0147] .

[0148] Output data after update: optimal position estimate after fusion and the latest uncertainty assessment .

[0149] Repeat steps 2-6 in sequence to achieve locomotive positioning.

[0150] This invention presents a single-track positioning method for underground coal mines that integrates encoder sensor positioning with UWB and RFID (Radio Frequency Identification) wireless positioning, forming a complementary and hierarchical positioning and control system. UWB provides wide-area, high-precision absolute coordinate reference; the encoder provides continuous, high-frequency relative motion data; and RFID, placed at key locations along the locomotive's path, provides unambiguous, zero-error absolute position beacons, primarily used for safety logic verification and final calibration. UWB + encoder provides continuous positioning on a "surface" scale, while RFID provides absolute verification at a "point" scale. Furthermore, this invention employs unscented Kalman filtering (UKF) for continuous state estimation, processing high-frequency, continuous but noisy data (UWB and encoder), achieving efficient collaboration among the three sensor data sources. This improves the positioning accuracy of monorail locomotives in coal mine roadways.

Claims

1. A method for precise location positioning of a monorail crane in an underground environment, characterized in that, Includes the following steps, 1) Determine the two-dimensional spatial coordinates of the monorail locomotive's position and establish an initial state model of the monorail locomotive's position in the roadway; 2) Obtain the encoder travel data of the monorail crane, including the displacement increment and heading angle change at the current moment, and record and save it according to time; 3) Establish a process model of the state-displacement relationship between position coordinates, travel speed, and heading angle, and predict the state of the monorail crane at the next moment; 4) The position of the monorail crane is measured by wireless signal positioning to provide correction points and establish a position observation model based on wireless positioning; 5) Location data update judgment: The predicted location is processed and compared with the wireless positioning location to determine the reliability of the prediction. 5.1) If the wireless positioning observation is reliable, trust the wireless positioning location and use the wireless positioning location observation value to correct the estimated value; 5.2) If wireless positioning observations are unreliable, rely primarily on estimated values ​​rather than relying on limited reference observations; 5.3) If wireless positioning fails and observations are lacking, rely entirely on the predicted values; 6) Output data: After fusing the observed and predicted values, the optimal location estimate and the latest uncertainty assessment are obtained. 7) Repeat steps 2) through 5) to achieve real-time locomotive positioning.

2. The method for precise positioning of a monorail crane in an underground environment according to claim 1, characterized in that, In step 1), the state model is a state vector. At time k, the state vector formula of the system is: ， in: , This indicates the locomotive's position coordinates in a two-dimensional plane (unit: meters). This indicates the locomotive's forward speed, measured in meters per second (m / s). The locomotive's heading angle is the angle between its direction of travel and the X-axis of the global coordinate system, expressed in radians.

3. The method for precise positioning of a monorail crane in an underground environment according to claim 1, characterized in that, In step 2), the control input (from the encoder) serves to obtain the displacement increment and heading angle change from the encoder. At time k, the control input obtained from the encoder is: ， in: It is the displacement increment from the previous moment to the current moment, in meters. It is the change in heading angle from the previous moment to the current moment, in radians.

4. The method for precise positioning of a monorail crane in an underground environment according to claim 1, characterized in that, In step 3), the model equation for the displacement relationship process of the monorail locomotive from time k-1 to time k is: ， in, It is a nonlinear state transition function. It is a process noise vector with a mean of 0 and a covariance matrix of... Gaussian distribution; Nonlinear state transition function The specific form is as follows: ， ， Or other speed models that depend on the encoder, ， 。 5. The method for precise positioning of a monorail crane in an underground environment according to claim 4, characterized in that, In step 4), the location of the monorail crane is measured by wireless signal positioning using UWB communication positioning or RFID tag positioning.

6. The method for precise positioning of a monorail crane in an underground environment according to claim 5, characterized in that, The UWB observation equation for UWB communication positioning is expressed as: ， in: It is the observation matrix. This indicates that UWB only observes the location (px, py). It is a UWB observation noise vector with a mean of 0 and a covariance matrix of... The Gaussian distribution.

7. The method for precise positioning of a monorail crane in an underground environment according to claim 5 or 6, characterized in that, The RFID observation equation for RFID tag positioning is expressed as: ， in: It is the observation matrix, and same, , It is an RFID observation noise vector with a mean of 0 and a covariance matrix of... Gaussian distribution, Set to a very small value This means that we almost completely trust the coordinates provided by RFID.

8. The method for precise positioning of a monorail crane in an underground environment according to claim 7, characterized in that, In step 5), the initial state estimate is first set to predict the current locomotive position: Set the initial state estimation error covariance matrix: For each time step k = 1, 2, ..., execute the following loop: 5.1) Generate the Sigma point set based on the posterior estimate from the previous time step. Calculate 2L+1 Sigma points (i=0,1,...,2L), , ,i = 1,...,L, , i = L+1,...,2L, in, , 5.2) Propagate Sigma points by passing each Sigma point through the process model. Propagation is performed to obtain the predicted Sigma point. , , i=0,1,...,2L, 5.3) Calculate the prior state prediction and prior covariance prediction, and calculate the mean of the predicted state. , ， Calculate the covariance of the predicted state , ， in, and Qk is the weighting coefficient calculated for the Sigma point. It is usually a diagonal matrix, and the elements on the diagonal represent the noise variance of each state dimension. The value of Qk needs to be determined through experiments and debugging to balance the confidence of predictions and observations.

9. The method for precise positioning of a monorail crane in an underground environment according to claim 8, characterized in that, In step 6), If UWB or RFID wireless observation data arrives at the current moment: Predicted Sigma points The predicted observation points are obtained through observation model transmission. , ,i=0,1,...,2L, Where H is either H_uwb or H_rfid, Calculate the mean of the predicted observations , ， Calculate the innovation covariance matrix , ， Where R is or It is a covariance matrix representing the magnitude of the noise from the observed sensor, and... Together, they determine whether the filter trusts the predictions or the observations more. Calculate the cross-covariance matrix of state and observation. , ； Calculate the Kalman gain and update the state: Calculate Kalman gain , ， Obtain actual observations ( or ), Update the state estimate (compile the posterior state estimate) , ， Update the covariance estimate (calculate the posterior error covariance matrix). , ， Output As the optimal state estimate at time k; If there is no observation data at the current moment: Then skip the update step and directly output the result of the prediction step: ， 。

Images