Tunnel personnel positioning method and device, and storage medium

Abstract

The invention discloses a tunnel personnel positioning method and device and a storage medium, and the method comprises the steps: S1, carrying out the calculation according to the pseudo-range, carrier phase and Doppler frequency shift data of a Beidou receiver, and obtaining a residual error of a first pedestrian state; wherein the first pedestrian state is pedestrian speed and position based on Beidou data; s2, according to measurement data of an inertial sensor three-axis accelerometer and a three-axis gyroscope, resolving to obtain a residual error of a second pedestrian state; wherein the second pedestrian state is pedestrian speed and position based on an inertial sensor; and S3, fusing the residual error of the first pedestrian state and the residual error of the second pedestrian state by utilizing the factor graph, solving to obtain an optimal state, and correcting an accumulated error of the inertial sensor through negative feedback. By adopting the technical scheme of the invention, the stability and reliability of navigation and positioning in the pedestrian tunnel are improved.

Description

Technical Field

[0001] This invention belongs to the field of indoor positioning technology, and particularly relates to a method and device for locating personnel in tunnels, as well as a storage medium. Background Technology

[0002] The BeiDou Navigation Satellite System (BDS), my country's independently developed global satellite navigation system, has been widely used in transportation, surveying and mapping, and intelligent driving, providing a stable spatiotemporal reference for various scenarios with its high-precision positioning capabilities. However, in enclosed or semi-enclosed environments such as tunnels and underground parking garages, satellite signals are easily blocked and reflected by tunnel walls, resulting in severe multipath interference. This leads to a sharp drop in signal reception strength, significant deviations in positioning accuracy, and even positioning failure, failing to meet the continuous positioning needs of scenarios such as personnel navigation and construction monitoring within tunnels.

[0003] Inertial navigation systems (INS) use built-in accelerometers and gyroscopes to measure the vehicle's acceleration and angular velocity in real time. Combining this with initial position information, they perform integral calculations to achieve autonomous positioning, eliminating the need for external signals and offering an irreplaceable advantage in scenarios lacking satellite signals. However, this system has significant drawbacks: because the integral calculations accumulate sensor measurement errors, the positioning error drifts continuously over time, making it difficult to maintain high-precision positioning for extended periods when used alone. This makes it unsuitable for long-distance, long-duration positioning needs, such as in tunnels. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a method and device for locating personnel in a tunnel, and a storage medium.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: A method for locating personnel in a tunnel includes: Step S1: Based on the pseudorange, carrier phase and Doppler frequency shift data of the BeiDou receiver, calculate the residual of the first pedestrian state; where the first pedestrian state is the pedestrian speed and position based on BeiDou data; Step S2: Based on the measurement data from the inertial sensor triaxial accelerometer and triaxial gyroscope, calculate the residual of the second pedestrian state; wherein, the second pedestrian state is the pedestrian speed and position based on the inertial sensor. Step S3: Using the factor graph, fuse the residuals of the first pedestrian state and the residuals of the second pedestrian state to obtain the optimal state, and correct the cumulative error of the inertial sensor through negative feedback.

[0006] As a preferred approach, when the factor graph optimization time interval exceeds a set threshold, the factor graph is no longer used for information fusion, and the optimal state is obtained instead through the step size estimation model.

[0007] The present invention also provides a tunnel personnel positioning device, comprising: The first processing module is used to calculate the residual of the first pedestrian state based on the pseudorange, carrier phase and Doppler frequency shift data of the Beidou receiver; wherein the first pedestrian state is the pedestrian speed and position based on Beidou data; The second processing module is used to calculate the residual of the second pedestrian state based on the measurement data of the inertial sensor triaxial accelerometer and triaxial gyroscope; wherein the second pedestrian state is the pedestrian speed and position based on the inertial sensor. The third processing module is used to fuse the residuals of the first pedestrian state and the residuals of the second pedestrian state using the factor graph, solve for the optimal state, and correct the cumulative error of the inertial sensor through negative feedback.

[0008] Preferably, a fourth processing module is also included, which is used to stop using the factor graph for information fusion when the factor graph optimization time interval is greater than a set threshold, and instead obtain the optimal state through the step size estimation model.

[0009] The present invention also provides a storage medium storing a computer program, which executes a tunnel personnel positioning method when running.

[0010] The present invention has the following beneficial effects: 1. Effectively suppresses the accumulation of errors in inertial navigation systems: This invention utilizes the high-precision positioning capability of the BeiDou Navigation Satellite System (BDS) in scenarios with good signal coverage to periodically provide position and velocity reference corrections to the inertial navigation system (INS), thereby controlling the positioning error of the inertial navigation system to a small range, significantly improving the positioning stability in long-term working scenarios, and solving the problem that it is difficult to meet the high-precision positioning requirements by using inertial navigation alone.

[0011] 2. Overcoming the positioning failure defects of the BeiDou system in scenarios with signal interference: For scenarios such as tunnels and underground where BeiDou signals are easily blocked and subject to multipath interference, this invention utilizes the autonomous positioning characteristics of the inertial navigation system that do not rely on external signals. When BeiDou positioning fails, the positioning function is seamlessly connected, ensuring that the positioning process is continuous and uninterrupted. This effectively covers the application blind spots of the BeiDou system and meets the core requirements for continuous positioning in scenarios such as personnel navigation and construction monitoring in tunnels.

[0012] 3. Achieving efficient fusion of multi-source data through factor graphs for accurate positioning in complex scenarios: This invention uses factor graphs as a fusion framework, abstracting BeiDou positioning data and inertial measurement data into observation factors and state variables, respectively. By constructing probabilistic constraint relationships between multi-source information through a belief propagation algorithm, it can fully leverage the long-term high-precision advantages of BeiDou and the short-term stability advantages of inertial navigation, while suppressing errors from a single data source. Ultimately, it achieves high-precision, continuous, and stable positioning in complex scenarios such as tunnels, providing reliable technical support for spatiotemporal reference services in multiple scenarios. Attached Figure Description

[0013] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0014] Figure 1 This is a flowchart of the tunnel personnel positioning method according to an embodiment of the present invention; Figure 2 The trajectory diagram of the tunnel personnel positioning method based on Beidou receiver and inertial sensor provided in the embodiments of the present invention; Figure 3 Error bar graph for tunnel personnel positioning method based on Beidou receiver and inertial sensor provided in the embodiments of the present invention. Detailed Implementation

[0015] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0016] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0017] Example 1: like Figure 1 As shown, an embodiment of the present invention provides a method for locating personnel in a tunnel, including: Step S1: Based on the pseudorange, carrier phase and Doppler frequency shift data of the BeiDou receiver, calculate the residual of the first pedestrian state; where the first pedestrian state is the pedestrian speed and position based on BeiDou data; Step S2: Based on the measurement data from the inertial sensor triaxial accelerometer and triaxial gyroscope, calculate the residual of the second pedestrian state; wherein, the second pedestrian state is the pedestrian speed and position based on the inertial sensor. Step S3: Using the factor graph, fuse the residuals of the first pedestrian state and the residuals of the second pedestrian state to obtain the optimal state, and correct the cumulative error of the inertial sensor through negative feedback.

[0018] In one embodiment of the present invention, in step S1, the calculation formulas for position and velocity based on pseudorange, carrier phase, and Doppler frequency shift are as follows: ; ; ; in, For receiver For satellite pseudorange, For receiver To satellite geometric distance, At the speed of light, For receiver and satellite clock bias, , For ionospheric and tropospheric delay errors, To account for pseudorange observation noise and multipath error, the pseudorange with meter-level accuracy can be obtained according to equation (1); The wavelength of the carrier signal. For carrier phase, For integer ambiguity, The carrier phase observation noise is calculated as follows: The accuracy is at the centimeter level; For measuring the Doppler frequency shift, The nominal transmission frequency of the satellite signal. The rate of change of the geometric distance between the receiver and the satellite. , For satellites and receivers in velocity vector in coordinate system The unit direction vector pointing from the receiver to the satellite. This is the frequency error caused by receiver clock drift.

[0019] By observing the Doppler shift of at least four satellites, the geocentric coordinate system of the personnel can be calculated. Three-dimensional position and velocity , ,Will , Transform to the local navigation coordinate system using a rotation matrix. The residual can be obtained after the system is established. .

[0020] ; Combine equations (1) and (2) and place them in the initial position. Linearize it: ; ; in, This represents the initial geometric distance at the initial position. , , , For the first The positions of the satellites. From this, the integer ambiguity can be calculated. Thus, the pseudorange at the centimeter level is obtained, and the precise value is calculated using equation (7). : ; ; ; in, For the first The pseudorange with centimeter-level accuracy obtained from the satellite for Tie The rotation matrix of the system, This is based on BeiDou data to determine pedestrian speed and location.

[0021] In one embodiment of the present invention, in step S2, the second pedestrian state is calculated based on the measurement data from the inertial sensor triaxial accelerometer and triaxial gyroscope. Assume the... Optimization to the 1st The time interval for the next optimization is: Then the predicted components of the second pedestrian state are and their observation equations: ; ; ; ; in, and These are the velocity and displacement prediction components based on inertial sensors, respectively. and Navigation coordinate system The speed and position under the system (i.e., the speed and position of pedestrians based on inertial sensors). It is the acceleration due to gravity. For the pre-integral quantity of the rotation matrix, for System relative to The acceleration of the system in The following is a representation of the system. The sampling time interval, To the carrier coordinate system Tie The rotation matrix of the system can be used to obtain its residuals based on the predicted components and the observation equations. .

[0022] ; In one embodiment of the present invention, in step S3, the factor graph essentially seeks to maximize the posterior probability. According to Bayes' theorem, assuming that the observations are independent of each other, the posterior probability can be transformed into a product of a series of factors: ; ; in, For system status, Let be the posterior probability of the state. It is the prior probability of the state. For the first Given the likelihood probability of each observation, and further assuming that both the observation noise and the prior information follow a Gaussian distribution, the above problem can be transformed into: ; in, To find the optimal velocity and position, and These are information matrices for the BeiDou and inertial navigation systems, respectively.

[0023] Furthermore, when the factor graph optimization time interval exceeds a set threshold, factor graphs are no longer used for information fusion; instead, the optimal state is obtained through a step size estimation model. ; ; in, To estimate the coefficients and These are the maximum and minimum acceleration values ​​within the window, respectively. This represents the window duration.

[0024] By using the optimal velocity and position to replace the velocity and position obtained from the current inertial sensor integration, the goal of correcting the inertial sensor integration error can be achieved.

[0025] This invention provides users with superior spatiotemporal positioning services in tunnel and surrounding environments. The positioning effectiveness is clearly demonstrated through experimental data: the specific positioning trajectory is shown in Figure 2, and the positioning error data is shown in Figure 3. Compared to traditional pedestrian dead reckoning (PDR) methods, the positioning trajectory of this invention shows significantly improved alignment with the reference trajectory, and the positioning error is significantly reduced, fully validating its superior positioning accuracy.

[0026] Furthermore, this invention deeply mines and integrates the core information of the BeiDou Navigation Satellite System and the Inertial Navigation System to form a complementary mechanism: on the one hand, by leveraging the autonomous positioning characteristics of inertial navigation, it effectively alleviates the interference problems caused by the obstruction and multipath effect of BeiDou signals in tunnels, thus avoiding positioning interruptions; on the other hand, by relying on the high-precision positioning benchmark of BeiDou, it significantly suppresses the error accumulation phenomenon that is prone to occur in the inertial navigation system during long-term independent operation, ultimately providing stable, reliable and high-precision technical support for personnel positioning in tunnel scenarios, meeting the positioning needs in practical applications.

[0027] Example 2: This invention also provides a tunnel personnel positioning device, comprising: The first processing module is used to calculate the residual of the first pedestrian state based on the pseudorange, carrier phase and Doppler frequency shift data of the Beidou receiver; wherein the first pedestrian state is the pedestrian speed and position based on Beidou data; The second processing module is used to calculate the residual of the second pedestrian state based on the measurement data of the inertial sensor triaxial accelerometer and triaxial gyroscope; wherein the second pedestrian state is the pedestrian speed and position based on the inertial sensor. The third processing module is used to fuse the residuals of the first pedestrian state and the residuals of the second pedestrian state using the factor graph, solve for the optimal state, and correct the cumulative error of the inertial sensor through negative feedback.

[0028] As one embodiment of the present invention, it also includes a fourth processing module, which is used to stop using the factor graph for information fusion when the factor graph optimization time interval is greater than a set threshold, and instead obtain the optimal state through the step size estimation model.

[0029] Example 3: This invention also provides a storage medium storing a computer program that executes a tunnel personnel positioning method during runtime.

[0030] The embodiments described above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Various modifications and improvements made to the technical solutions of the present invention by those skilled in the art without departing from the spirit of the present invention should fall within the protection scope defined by the claims of the present invention.

Claims

1. A method for locating personnel in a tunnel, characterized in that, include: Step S1: Based on the pseudorange, carrier phase and Doppler frequency shift data of the BeiDou receiver, calculate the residual of the first pedestrian state; where the first pedestrian state is the pedestrian speed and position based on BeiDou data; Step S2: Based on the measurement data from the inertial sensor triaxial accelerometer and triaxial gyroscope, calculate the residual of the second pedestrian state; wherein, the second pedestrian state is the pedestrian speed and position based on the inertial sensor. Step S3: Using the factor graph, fuse the residuals of the first pedestrian state and the residuals of the second pedestrian state to obtain the optimal state, and correct the cumulative error of the inertial sensor through negative feedback.

2. The tunnel personnel positioning method as described in claim 1, characterized in that, When the time interval for factor graph optimization exceeds a set threshold, factor graphs are no longer used for information fusion; instead, the optimal state is obtained through a step size estimation model.

3. A tunnel personnel positioning device, characterized in that, include: The first processing module is used to calculate the residual of the first pedestrian state based on the pseudorange, carrier phase and Doppler frequency shift data of the Beidou receiver; wherein the first pedestrian state is the pedestrian speed and position based on Beidou data; The second processing module is used to calculate the residual of the second pedestrian state based on the measurement data of the inertial sensor triaxial accelerometer and triaxial gyroscope; wherein the second pedestrian state is the pedestrian speed and position based on the inertial sensor. The third processing module is used to fuse the residuals of the first pedestrian state and the residuals of the second pedestrian state using the factor graph, solve for the optimal state, and correct the cumulative error of the inertial sensor through negative feedback.

4. The tunnel personnel positioning device as described in claim 3, characterized in that, It also includes a fourth processing module, which, when the factor graph optimization time interval is greater than a set threshold, stops using factor graphs for information fusion and instead obtains the optimal state through a step size estimation model.

5. A storage medium, characterized in that, The storage medium stores a computer program, which executes the tunnel personnel positioning method as described in any one of claims 1 to 2 when it runs.

Images