Multi-sensor fusion underground positioning navigation system and navigation method thereof

Abstract

The invention provides a multi-sensor fusion underground positioning navigation system and a navigation method thereof. The multi-sensor fusion underground positioning navigation system comprises an inertial measurement unit, an ultra wide band positioning subsystem, a laser radar, an ultrasonic sensor, a magnetometer and a wheel speed odometer. Through multi-sensor fusion, the advantages of each sensor can be fully utilized, and the precision of positioning navigation is improved. The system can realize high-precision positioning in an underground complex environment, and meets the precise positioning requirements of vehicles and personnel. The system adopts a redundancy design concept, when a certain sensor fails or data is abnormal, the system can automatically filter abnormal data, and it is ensured that the overall positioning performance is not affected. The system is simple in structure, good in real-time performance and high in adaptability, the system can collect and process sensor data in real time, rapid positioning and navigation response are achieved, the system is suitable for complex environments such as underground long roadways, dark light and moist environments, and the limitation of a single sensor in a specific environment can be overcome.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation positioning and application, and in particular relates to a multi-sensor fusion downhole positioning navigation system and a navigation method thereof. Background Art

[0002] Currently, unmanned vehicles face some special challenges and practical operational problems when operating in mine tunnels. The basic positioning of traditional vehicles relies on GPS, but satellite signals in tunnel environments are difficult to penetrate underground structures. Therefore, the early unmanned vehicles usually rely on GPS for positioning and navigation solutions, which is not feasible. In this case, other positioning technologies may be needed in tunnels, such as inertial navigation systems, lidar systems, visual analysis systems, etc.

[0003] The real mine tunnel environment is usually more complex than the general road environment, and may have low lighting, curved passages, traffic signs, smoke, coal dust, rain, etc. In the actual operation process, the unmanned vehicle needs to be able to effectively perceive and respond to these environmental changes. The operation in the mine tunnel requires safe, efficient and reliable real-time positioning capabilities.

[0004] Due to the complexity of the underground environment, it is difficult for a single sensor to provide accurate positioning information. In addition, when operating underground, since the sensor is used alone, the sensor signal in the mine is often lost or interfered.

[0005] Chinese patent "202410248948.2" provides a downhole positioning system and a downhole positioning method, which uses multiple passive sensors corresponding to markers to achieve downhole positioning. However, this downhole positioning method still cannot avoid the problem of sensor signal loss or interference in the well.

[0006] In view of this, it is very necessary to invent a multi-sensor fusion downhole positioning and navigation system and a navigation method and a preparation method thereof. Summary of the invention

[0007] In order to solve the above technical problems, the present invention provides a multi-sensor fusion downhole positioning and navigation system and a navigation method thereof to solve the problems mentioned in the background technology.

[0008] A multi-sensor fusion downhole positioning and navigation system, comprising:

[0009] Inertial Measurement Unit (IMU): The IMU includes a three-axis gyroscope and a three-axis accelerometer. The three-axis gyroscope is used to measure the angular velocity of the carrier, and the three-axis accelerometer is used to measure the acceleration of the carrier. The attitude, velocity and position information of the carrier are obtained through integration operation;

[0010] Ultra-wideband (UWB) positioning subsystem: UWB technology determines the distance by measuring the propagation time of the signal between the base station and the tag, thereby achieving three-dimensional positioning; UWB positioning has the advantages of high precision and strong anti-interference ability, and is very suitable for complex environments such as long tunnels, dark lights, and humidity underground;

[0011] Laser radar (LiDAR): LiDAR generates a 3D point cloud map of the surrounding environment by emitting laser beams and receiving reflected light. In underground environments, LiDAR can be used for SLAM (simultaneous localization and mapping) to provide accurate environmental perception and positioning information.

[0012] Ultrasonic sensor: Ultrasonic sensors transmit ultrasonic waves into the underground space and receive ultrasonic signals reflected by obstacles (such as personnel, equipment, walls, etc.); the sensor measures the time difference between ultrasonic emission and reception, and calculates the distance between the sensor and the object being measured by combining the speed of sound (. Ultrasonic sensors can achieve non-contact distance measurement and positioning in underground environments;

[0013] Magnetometer: Magnetometer determines the location of underground personnel or equipment by measuring the strength of the earth's magnetic field at a specific position and direction. The earth itself is a large magnet, and its magnetic field has a certain regular distribution on the earth's surface and nearby space. Magnetometer can sense these magnetic field changes and convert them into electrical signals for processing and analysis.

[0014] Wheel speed odometer: The wheel speed odometer calculates the vehicle's travel distance and direction by measuring the wheel speed and steering angle; although its positioning accuracy is relatively low, it can serve as an auxiliary data source for other sensors to improve the stability and reliability of the system.

[0015] A multi-sensor fusion downhole positioning and navigation method comprises the following steps:

[0016] Step 1: Data collection: The sensors collect data in real time, including: the angular velocity and acceleration data of the carrier collected by the inertial measurement unit, the ranging data of the ultra-wideband positioning subsystem, the point cloud data of the laser radar, the non-contact ranging data of the ultrasonic sensor, the positioning data of the magnetometer, and the driving data of the wheel speed odometer;

[0017] Step 2: Data preprocessing: Perform preprocessing operations such as filtering and denoising on the collected raw data to improve the reliability and accuracy of the data;

[0018] Step 3: Multi-sensor fusion: Input the pre-processed data into the multi-sensor fusion algorithm;

[0019] Step 4: Positioning and solving: Using the fused data and prior map information (if any), the real-time position, speed and attitude of the carrier are calculated through the positioning and solving algorithm;

[0020] Step 5: Navigation control: Based on the positioning results and the navigation task requirements, perform path planning and motion control for the underground vehicle or robot.

[0021] Furthermore, the fusion algorithm used in the multi-sensor fusion in step 3 is one of the Kalman filter algorithm and the particle filter algorithm. Weights are assigned according to the characteristics and accuracy of each sensor to achieve organic fusion of data.

[0022] Furthermore, the fusion algorithm adopts the Kalman filter algorithm, and the steps are as follows:

[0023] (1) Establish the state equation and observation equation:

[0024] According to the specific characteristics of the downhole positioning system, the state equation and observation equation of the system are established. The state equation describes the state of the system and establishes the state equation and observation equation:

[0025] According to the specific characteristics of the downhole positioning system, the state equation and observation equation of the system are established; the state equation describes the change law of the system state over time, and the observation equation describes the relationship between the observed data and the system state.

[0026] (2) Establish the state equation and observation equation:

[0027] According to the specific characteristics of the downhole positioning system, the state equation and observation equation of the system are established; the state equation describes the change law of the system state over time, and the observation equation describes the relationship between the observed data and the system state.

[0028] (3) Initialization parameters:

[0029] Set the initial parameters of the Kalman filter algorithm, including the initial state estimate, the initial error covariance matrix, the process noise covariance matrix, and the observation noise covariance matrix.

[0030] (4) Iterative calculation:

[0031] Through iterative calculations of the prediction step and the update step, the system state estimate and error covariance matrix are continuously updated; the prediction step predicts the state estimate and error covariance matrix at the current moment according to the state equation; the update step updates the state estimate and error covariance matrix according to the observation equation and observation data.

[0032] (5) Output results:

[0033] Output the final position estimation result as the output of the downhole positioning system.

[0034] Furthermore, the fusion algorithm adopts a particle filter algorithm, and the steps are as follows:

[0035] A. Initialization: Draw a set of particles from the prior distribution to represent the initial estimate of the state;

[0036] B. Prediction: Based on the state transfer function and control input, predict each particle to get the state estimate at the next moment;

[0037] C. Update: Based on the observed value, the weight of each particle is adjusted to reflect the possibility of the observation;

[0038] D. Resampling: Resample the particles according to the updated weights to ensure that particles with high weights are replicated more and particles with low weights are removed, so as to obtain the state estimate at the next moment;

[0039] E. Repeat: Repeat the prediction, update, and resampling steps to continuously improve the estimate of the posterior distribution of the system state.

[0040] Compared with the prior art, the present invention has the following beneficial effects:

[0041] (1) Through multi-sensor fusion, the advantages of each sensor can be fully utilized to improve the accuracy of positioning and navigation. The system can achieve high-precision positioning in complex underground environments and meet the precise positioning needs of vehicles and personnel.

[0042] (2) The multi-sensor fusion downhole positioning and navigation system and navigation method of the present invention are highly robust. The system adopts a redundant design concept. When a sensor fails or the data is abnormal, the system can automatically filter out the abnormal data to ensure that the overall positioning performance is not affected.

[0043] (3) The present invention provides a multi-sensor fusion underground positioning and navigation system and its navigation method with good real-time performance and strong adaptability. The system can collect and process sensor data in real time to achieve rapid positioning and navigation response. It is suitable for complex environments such as long underground tunnels, dark lights, and humidity, and can overcome the limitations of a single sensor in a specific environment. DETAILED DESCRIPTION

[0044] The present invention is further described below:

[0045] Embodiment 1:

[0046] A multi-sensor fusion downhole positioning and navigation system, comprising:

[0047] Inertial Measurement Unit (IMU): The IMU includes a three-axis gyroscope and a three-axis accelerometer. The three-axis gyroscope is used to measure the angular velocity of the carrier, and the three-axis accelerometer is used to measure the acceleration of the carrier. The attitude, velocity and position information of the carrier are obtained through integration operation;

[0048] Ultra-wideband (UWB) positioning subsystem: UWB technology determines the distance by measuring the propagation time of the signal between the base station and the tag, thereby achieving three-dimensional positioning; UWB positioning has the advantages of high precision and strong anti-interference ability, and is very suitable for complex environments such as long tunnels, dark lights, and humidity underground;

[0049] Laser radar (LiDAR): LiDAR generates a 3D point cloud map of the surrounding environment by emitting laser beams and receiving reflected light. In underground environments, LiDAR can be used for SLAM (simultaneous localization and mapping) to provide accurate environmental perception and positioning information.

[0050] Ultrasonic sensor: Ultrasonic sensors transmit ultrasonic waves into the underground space and receive ultrasonic signals reflected by obstacles (such as personnel, equipment, walls, etc.); the sensor measures the time difference between ultrasonic emission and reception, and calculates the distance between the sensor and the object being measured by combining the speed of sound (. Ultrasonic sensors can achieve non-contact distance measurement and positioning in underground environments;

[0051] Magnetometer: Magnetometer determines the location of underground personnel or equipment by measuring the strength of the earth's magnetic field at a specific position and direction. The earth itself is a large magnet, and its magnetic field has a certain regular distribution on the earth's surface and nearby space. Magnetometer can sense these magnetic field changes and convert them into electrical signals for processing and analysis.

[0052] Wheel speed odometer: The wheel speed odometer calculates the vehicle's travel distance and direction by measuring the wheel speed and steering angle; although its positioning accuracy is relatively low, it can serve as an auxiliary data source for other sensors to improve the stability and reliability of the system.

[0053] A multi-sensor fusion downhole positioning and navigation method comprises the following steps:

[0054] Step 1: Data collection: The sensors collect data in real time, including: the angular velocity and acceleration data of the carrier collected by the inertial measurement unit, the ranging data of the ultra-wideband positioning subsystem, the point cloud data of the laser radar, the non-contact ranging data of the ultrasonic sensor, the positioning data of the magnetometer, and the driving data of the wheel speed odometer;

[0055] Step 2: Data preprocessing: Perform preprocessing operations such as filtering and denoising on the collected raw data to improve the reliability and accuracy of the data;

[0056] Step 3: Multi-sensor fusion: Input the pre-processed data into the multi-sensor fusion algorithm;

[0057] Step 4: Positioning and solving: Using the fused data and prior map information (if any), the real-time position, speed and attitude of the carrier are calculated through the positioning and solving algorithm;

[0058] Step 5: Navigation control: Based on the positioning results and the navigation task requirements, perform path planning and motion control for the underground vehicle or robot.

[0059] In this embodiment, the fusion algorithm used is the Kalman filter algorithm, and the specific algorithm is as follows:

[0060] 1. Prediction Step

[0061] Predict current status:

[0062]

[0063] in, It is the predicted value of the state at time k based on the state at time k-1.

[0064] Forecast error covariance:

[0065]

[0066] Among them, P k|k-1 is the forecast error covariance matrix, Q k is the process noise covariance matrix.

[0067] 2. Update steps

[0068] Calculate the Kalman gain:

[0069]

[0070] Among them, K k is the Kalman gain, R k is the observation noise covariance matrix.

[0071] Update the state estimate:

[0072]

[0073] in, It is the state estimate after updating based on the observation value at time k.

[0074] Update the error covariance:

[0075] P k|k =(IK k H k )k|k-1

[0076] Among them, P k|k is the updated error covariance matrix.

[0077] The state equation of the above Kalman filter algorithm is as follows:

[0078] x k =F k x k-1 +B k u k +w k

[0079] x k Is the system status.

[0080] F k is the state transition matrix, which describes how the system transitions from one state to the next.

[0081] B k is the control input matrix, describing the effect of the control input on the state.

[0082] u k is the control input.

[0083] w k is the process noise, assumed to be Gaussian white noise.

[0084] Observation equation:

[0085] z k =H k x k +v k

[0086] z k is the observed value.

[0087] H k is the observation matrix, describing how to obtain observations from the state vector.

[0088] v k is the observation noise, which is also assumed to be Gaussian white noise.

[0089] Example 2

[0090] Compared with Example 1, the fusion algorithm adopted in this embodiment is a particle filter algorithm, and the specific steps are as follows:

[0091] A. Initialization: A set of particles is drawn from the prior distribution to represent the initial estimate of the state;

[0092] B. Prediction: Based on the state transfer function and control input, predict each particle to get the state estimate at the next moment;

[0093] C. Update: Based on the observed value, the weight of each particle is adjusted to reflect the possibility of the observation;

[0094] D. Resampling: Resample the particles according to the updated weights to ensure that particles with high weights are replicated more and particles with low weights are removed, so as to obtain the state estimate at the next moment;

[0095] E. Repeat: Repeat the prediction, update, and resampling steps to continuously improve the estimate of the posterior distribution of the system state.

[0096] Compared with the prior art, the present invention has the following beneficial effects:

[0097] (1) Through multi-sensor fusion, the advantages of each sensor can be fully utilized to improve the accuracy of positioning and navigation. The system can achieve high-precision positioning in complex underground environments and meet the precise positioning needs of vehicles and personnel.

[0098] (2) The multi-sensor fusion downhole positioning and navigation system and navigation method of the present invention are highly robust. The system adopts a redundant design concept. When a sensor fails or the data is abnormal, the system can automatically filter out the abnormal data to ensure that the overall positioning performance is not affected.

[0099] (3) The present invention provides a multi-sensor fusion underground positioning and navigation system and its navigation method with good real-time performance and strong adaptability. The system can collect and process sensor data in real time to achieve rapid positioning and navigation response. It is suitable for complex environments such as long underground tunnels, dark lights, and humidity, and can overcome the limitations of a single sensor in a specific environment.

[0100] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A multi-sensor fusion downhole positioning and navigation system, comprising: Inertial measurement unit, ultra-wideband positioning subsystem, lidar, ultrasonic sensors, magnetometer, and wheel speed odometer.

2. A multi-sensor fusion downhole positioning and navigation system as claimed in claim 1, characterized in that: The inertial measurement unit includes a three-axis gyroscope and a three-axis accelerometer. The three-axis gyroscope is used to measure the angular velocity of the carrier, and the three-axis accelerometer is used to measure the acceleration of the carrier. The attitude, velocity and position information of the carrier are obtained through integration operation.

3. The multi-sensor fusion downhole positioning and navigation system according to claim 1, characterized in that: The ultra-wideband positioning subsystem determines the distance by measuring the propagation time of the signal between the base station and the tag, thereby achieving three-dimensional positioning.

4. The multi-sensor fusion downhole positioning and navigation system according to claim 1, characterized in that: The laser radar generates a three-dimensional point cloud map of the surrounding environment by emitting laser beams and receiving reflected light; in an underground environment, the laser radar provides accurate environmental perception and positioning information through SLAM.

5. The multi-sensor fusion downhole positioning and navigation system according to claim 1, characterized in that: The ultrasonic sensor transmits ultrasonic waves into the underground space and receives ultrasonic signals reflected by obstacles. The sensor measures the time difference between ultrasonic emission and reception and calculates the distance between the sensor and the object being measured based on the speed of sound. The ultrasonic sensor can achieve non-contact distance measurement and positioning in the underground environment.

6. The multi-sensor fusion downhole positioning and navigation system according to claim 1, characterized in that: The magnetometer determines the position of underground personnel or equipment by measuring the strength of the earth's magnetic field at a specific position and direction. The magnetometer senses changes in the magnetic field and converts them into electrical signals for processing and analysis.

7. The multi-sensor fusion downhole positioning and navigation system according to claim 1, characterized in that: The wheel speed odometer calculates the travel distance and direction of the vehicle by measuring the rotation speed and steering angle of the wheels.

8. The multi-sensor fusion downhole positioning and navigation system according to claim 1, characterized in that: The navigation method of the navigation system specifically comprises the following steps: Step 1: Data collection: The sensors collect data in real time, including: the angular velocity and acceleration data of the carrier collected by the inertial measurement unit, the ranging data of the ultra-wideband positioning subsystem, the point cloud data of the laser radar, the non-contact ranging data of the ultrasonic sensor, the positioning data of the magnetometer, and the driving data of the wheel speed odometer; Step 2: Data preprocessing: Perform preprocessing operations such as filtering and denoising on the collected raw data to improve the reliability and accuracy of the data; Step 3: Multi-sensor fusion: Input the pre-processed data into the multi-sensor fusion algorithm; Step 4: Positioning and solving: Using the fused data and prior map information (if any), the real-time position, speed and attitude of the carrier are calculated through the positioning and solving algorithm; Step 5: Navigation control: Based on the positioning results and the navigation task requirements, perform path planning and motion control for the underground vehicle or robot.

9. The navigation method of a multi-sensor fusion downhole positioning and navigation system according to claim 8, characterized in that: The fusion algorithm adopts the Kalman filter algorithm, and the steps are as follows: (1) Establish the state equation and observation equation: According to the specific characteristics of the downhole positioning system, the state equation and observation equation of the system are established. The state equation describes the state of the system and establishes the state equation and observation equation: According to the specific characteristics of the downhole positioning system, the state equation and observation equation of the system are established; the state equation describes the change law of the system state over time, and the observation equation describes the relationship between the observed data and the system state. (2) Establish the state equation and observation equation: According to the specific characteristics of the downhole positioning system, the state equation and observation equation of the system are established; the state equation describes the change law of the system state over time, and the observation equation describes the relationship between the observed data and the system state. (3) Initialization parameters: Set the initial parameters of the Kalman filter algorithm, including the initial state estimate, the initial error covariance matrix, the process noise covariance matrix, and the observation noise covariance matrix. (4) Iterative calculation: Through iterative calculations of the prediction step and the update step, the system state estimate and error covariance matrix are continuously updated; the prediction step predicts the state estimate and error covariance matrix at the current moment according to the state equation; the update step updates the state estimate and error covariance matrix according to the observation equation and observation data. (5) Output results: Output the final position estimation result as the output of the downhole positioning system.

10. The navigation method of a multi-sensor fusion downhole positioning and navigation system according to claim 8, characterized in that: The fusion algorithm adopts the particle filter algorithm, and the steps are as follows: A. Initialization: A set of particles is drawn from the prior distribution to represent the initial estimate of the state; B. Prediction: Based on the state transfer function and control input, predict each particle to get the state estimate at the next moment; C. Update: Based on the observed value, the weight of each particle is adjusted to reflect the possibility of the observation; D. Resampling: Resample the particles according to the updated weights to ensure that particles with high weights are replicated more and particles with low weights are removed, so as to obtain the state estimate at the next moment; E. Repeat: Repeat the prediction, update, and resampling steps to continuously improve the estimate of the posterior distribution of the system state.

Images