High-precision train combined positioning method suitable for subway tunnel environment

Abstract

The invention discloses a high-precision train combined positioning method suitable for a subway tunnel environment, and the method comprises the steps: S1, obtaining distance measurement data between a train and a base station through an improved UWB distance measurement process, constructing a mileage calculation model based on double base stations, and measuring the running mileage of the train in real time; s2, converting the measured mileage information into corresponding three-dimensional positions and attitude angles through a digital track map, and taking the three-dimensional positions and attitude angles as observation input of a combined navigation system; s3, receiving information of a gyroscope and an accelerometer of an inertial measurement unit (IMU) in real time, resolving the position, the speed and the attitude, and constructing a state vector and an observation vector in an error state Kalman filter (ESKF) through a state space method; and S4, fusing the observation information provided by the UWB and the calculation result of the SINS through error state Kalman filtering by using the adaptive position attitude based on the course angle and a robust M estimation strategy, and outputting the position information. The train high-precision positioning system is suitable for the subway tunnel environment.

Description

Technical Field

[0001] This invention belongs to the field of subway train positioning technology, specifically relating to a high-precision train combination positioning method suitable for subway tunnel environments. Background Technology

[0002] Currently, subway train positioning mainly relies on wheel axle sensors and transponders, but this method suffers from problems such as error accumulation, poor anti-interference capabilities, and high maintenance costs. While Ultra-Wideband (UWB) and Strapdown Inertial Navigation Systems (SINS) have certain advantages, UWB is significantly affected by non-line-of-sight errors in complex environments, and SINS is prone to drift over time, leading to unstable positioning. Therefore, there is an urgent need for a high-precision combined positioning method that integrates UWB, SINS, and digital track maps, and possesses adaptive and robust filtering capabilities, to meet the high-precision continuous positioning requirements of subway trains in complex environments such as tunnels. Summary of the Invention

[0003] In order to solve the technical problems existing in the background art, the present invention aims to provide a high-precision train combination positioning method suitable for subway tunnel environment. The method obtains the train running mileage by improving the UWB ranging method and obtains three-dimensional pose information by combining it with digital track map. It is used as the observation input of the combined navigation and fused with the IMU calculation result in the error state Kalman filter. The method adopts adaptive and robust strategies to achieve high-precision positioning in subway tunnel environment.

[0004] To solve the technical problem, the technical solution of the present invention is as follows:

[0005] A high-precision train combination positioning method suitable for subway tunnel environments, the method comprising:

[0006] S1: By improving the UWB ranging process and adopting the AltDS-TWR structure including four communications, high-precision ranging data between the train and the base station is obtained; combined with the deployment of dual base stations, a real-time estimation model for train running mileage is constructed.

[0007] S2: Input the mileage information obtained in step S1 into the pre-built digital track map and map it into the current three-dimensional position and tangential attitude angle of the train, which is used as the observation input of the integrated navigation system;

[0008] S3: Receive gyroscope and accelerometer data from the inertial measurement unit (IMU) in real time, and calculate the position, speed and attitude of the train through the inertial navigation algorithm (SINS); use the position and attitude information provided in step S2 as observation values, and construct the state vector and observation vector in the error state Kalman filter (ESKF) through state space modeling.

[0009] S4: The SINS solution results in step S3 are fused with the UWB observation information in step S2. The measurement noise covariance matrix is ​​dynamically updated using an adaptive position and attitude adjustment and robust M estimation strategy. Multi-source data fusion is completed through an error state Kalman filter to output the train's precise position and attitude information.

[0010] Furthermore, in step S1, distance data between the train and the base station is obtained through an improved UWB ranging process, and a mileage calculation model based on dual base stations is constructed to measure the train's running mileage in real time; wherein, the calculation methods for signal flight time and ranging values ​​are as follows:

[0011]

[0012] In the formula, t tof The signal flight time is represented by d, the ranging value is represented by , and T1, T2, T3, and T4 represent the total time of the four communication processes in the AltDS-TWR method, respectively. The train mileage calculation model based on dual base stations is as follows:

[0013]

[0014] In the formula, Dis n The distance measured by the nth base station is represented by L, the distance between the two base stations is represented by OD, and the total distance traveled by the train is represented by OD. n od represents the distance traveled by the train relative to the nth base station. n This represents the distance the train has traveled relative to its originating base station.

[0015] Furthermore, in step S1, the UWB ranging process adopts an improved AltDS-TWR structure with four communications, including four steps: Poll, Resp, Final, and Ack.

[0016] Furthermore, in step S2, the digital track map is a single-valued function track database built based on mileage, including the three-dimensional coordinates, tangential angles, and section numbers corresponding to each point within the tunnel, used to accurately provide train pose reference information; the mathematical model of the digital track map is as follows:

[0017]

[0018] In the formula, l represents the line mileage, and (x,y) represents the planar position of a point on the line. Let f1 be the tangential angle at a certain point on the line, and f2, f3 be the corresponding relational functions.

[0019] Furthermore, in step S2, attitude information at the corresponding location is obtained using a digital orbit map. This attitude information is then used to supplement the inertial navigation system and suppress errors. The standard deviation model for the heading angle error corresponding to the odometer measurement standard is as follows:

[0020]

[0021] In the formula σ ψ R represents the standard deviation of the heading angle error. min σ represents the minimum curve radius of a subway tunnel. od This represents the standard deviation of the mileage measurement error.

[0022] Furthermore, in step S3, the SINS filter state variables are modeled as follows:

[0023]

[0024] In the formula, X is the filter state variable, δr is the position error, δv is the velocity error, and φ is the misalignment angle error. ε represents the accelerometer's zero bias, and ε represents the gyroscope's zero drift. The attitude matrix calculated by SINS. Let φ(×) be the ideal attitude matrix recorded in the digital orbit map, φ(×) be the antisymmetric matrix of φ, I be the identity matrix, and p SINS For SINS location measurement data, p UWB This is UWB location measurement data, where Z is the observation vector, H is the observation matrix, and 0... n×n Represents an n x n zero matrix.

[0025] Furthermore, step S4 includes:

[0026] S401: Establish adaptive position and attitude and robust adaptive position and attitude methods to adjust the measurement noise covariance matrix in real time and perform error state Kalman filtering;

[0027] S402: Combines the Kalman filter solution results with the SINS solution results to output the train position and attitude information.

[0028] Furthermore, in step S401, the adjustment of the position measurement variance by the adaptive position attitude is as follows:

[0029]

[0030] In the formula R Pk ψ represents the variance of the position measurement at the k-th iteration. k R represents the measured heading angle at the k-th iteration. od The measurement variance representing mileage;

[0031] In step S401, the robust adaptive position and pose method model is as follows:

[0032]

[0033] In the formula With R ki The representative stated and R k The i-th diagonal element in R k Represents the measurement covariance matrix. Represents R k The equivalent measurement noise covariance matrix, R is the robustly weighted adaptive position measurement variance matrix. A Let u be the attitude measurement variance matrix, w be the robust weighting factor, and u be the robust weighting factor in the formula. i =v i / σ i variance factor k0 is taken as 1 to 1.5, k1 as 2.5 to 3, and ε as 10. -4 ;

[0034] In step S401, the error Kalman filter solution process is as follows:

[0035]

[0036] In the formula X k and P k These are the state variable matrix and error covariance matrix at time k, respectively; X k / k-1 and P k-1 These are the state prediction matrix and the error covariance prediction matrix, respectively; Φ k / k-1 Q is the corresponding state transition matrix; k-1 To predict covariance; K k Z is the corresponding Kalman filter gain matrix; k H k and R k Let be the measurement vector matrix, observation matrix, and measurement noise covariance matrix at time k, respectively; I is the identity matrix.

[0037] Furthermore, in step S402, the process of combining the Kalman filter solution result with the SINS solution result to output the train position and attitude information is as follows:

[0038]

[0039] In the formula, p, v, and a represent the position, velocity, and acceleration information calculated by SINS, and δp, δv, and δa represent the error correction information calculated by the error Kalman filter. This represents the train's position, speed, and acceleration information after information fusion.

[0040] A high-precision train combination positioning system suitable for subway tunnel environments, characterized in that the system is used to perform any of the methods described above, the system comprising:

[0041] UWB ranging and mileage calculation module: Through an improved UWB ranging process, it adopts an AltDS-TWR structure including four communications to obtain high-precision ranging data between the train and the base station; combined with the deployment of dual base stations, it constructs a real-time estimation model for train running mileage.

[0042] Digital track map matching and 3D pose conversion module: Input the mileage information obtained from the UWB ranging and mileage calculation module into the pre-constructed digital track map, and map it into the current 3D position and tangential attitude angle of the train, which is used as the observation input of the integrated navigation system;

[0043] The IMU calculation and state vector modeling module receives gyroscope and accelerometer data from the inertial measurement unit (IMU) in real time, calculates the position, speed and attitude of the train through the inertial navigation algorithm SINS, and uses the position and attitude information provided by the digital track map matching and 3D pose conversion module as observation values ​​to construct the state vector and observation vector in the error state Kalman filter (ESKF) through state space modeling.

[0044] Data fusion and precise positioning output module: It integrates the SINS solution results from the IMU solution and state vector modeling module with the UWB observation information from the digital track map matching and 3D pose conversion module. It dynamically updates the measurement noise covariance matrix using an adaptive position and attitude adjustment and robust M estimation strategy. It completes multi-source data fusion through an error state Kalman filter and outputs the precise position and attitude information of the train.

[0045] Compared with the prior art, the advantages of the present invention are as follows:

[0046] (1) This invention integrates ultra-wideband (UWB) ranging, strapdown inertial navigation system (SINS) and digital track map (DTM). Through an improved ranging process and track mileage mapping mechanism, the UWB ranging results are converted into the three-dimensional position information and attitude angle of the train in real time, realizing continuous high-precision positioning of the train in the subway tunnel environment where satellite signals are unavailable.

[0047] (2) By constructing an error state Kalman filter (ESKF) and introducing an adaptive covariance adjustment mechanism based on heading angle and a robust M estimation method, the problem of mileage error distribution in the horizontal direction was optimized and the abnormal ranging error caused by non-line-of-sight (NLOS) interference was effectively suppressed, enhancing the robustness and stability of the system in dynamic environments and enabling the overall positioning accuracy to reach the centimeter level. Attached Figure Description

[0048] Figure 1 A flowchart of a UWB / SINS metro train combination positioning method based on digital track map assistance;

[0049] Figure 2 This is a flowchart of the UWB / SINS metro train combined positioning method based on digital track map assistance in a specific implementation of the present invention;

[0050] Figure 3 This is an improved AltDS-TWR ranging procedure in this embodiment of the invention;

[0051] Figure 4 This is a simulation diagram of the train's trajectory in an embodiment of the present invention;

[0052] Figure 5 The image shows the location combination positioning error diagram in the embodiment of the present invention (left image is 10 Hz, right image is 50 Hz);

[0053] Figure 6 The image shows the position and attitude combination positioning error diagram in the embodiment of the present invention (left image is 10 Hz, right image is 50 Hz);

[0054] Figure 7 The adaptive position and attitude combination positioning error diagram in the embodiment of the present invention (left diagram is 10 Hz, right diagram is 50 Hz);

[0055] Figure 8 The image shows the robust adaptive position and attitude combination positioning error diagram in an embodiment of the present invention (left image is 10 Hz, right image is 50 Hz);

[0056] Figure 9 The diagram shows the planar position error distribution function of ten calibration methods in the embodiments of the present invention. Detailed Implementation

[0057] The specific implementation of the present invention is described below with reference to embodiments:

[0058] It should be noted that the structures, proportions, sizes, etc. shown in this specification are only used to complement the content disclosed in the specification for those skilled in the art to understand and read, and are not intended to limit the conditions under which the present invention can be implemented. Any modifications to the structure, changes in the proportions, or adjustments to the size, without affecting the effects and objectives that the present invention can produce, should still fall within the scope of the technical content disclosed in the present invention.

[0059] Furthermore, the terms such as "upper," "lower," "left," "right," "middle," and "one" used in this specification are merely for clarity of description and are not intended to limit the scope of the invention. Any changes or adjustments to their relative relationships, without substantially altering the technical content, should also be considered within the scope of the invention.

[0060] Example 1:

[0061] like Figure 1As shown, this embodiment provides a UWB / SINS metro train combined positioning method based on digital track map assistance, including the following steps:

[0062] S1. Obtain distance data between the train and the base station through the improved UWB ranging process, and construct a mileage calculation model based on dual base stations to measure the train's running mileage in real time.

[0063] S2. The measured mileage information is converted into corresponding three-dimensional position and attitude angles through a digital orbit map, which serves as the observation input for the integrated navigation system;

[0064] S3. Receive gyroscope and accelerometer information from the inertial measurement unit (IMU) in real time, calculate position, velocity, and attitude, and construct the state vector and observation vector in the error state Kalman filter (ESKF) using the state-space method.

[0065] S4. Using an adaptive position and attitude based on the heading angle and a robust M-estimation strategy, the observation information provided by UWB and the SINS solution are fused through error state Kalman filtering and the position information is output.

[0066] In this embodiment of the invention, in S1, ranging data between the train and the base station is obtained through an improved UWB ranging process, and a mileage calculation model based on dual base stations is constructed to measure the train's running mileage in real time; wherein, the calculation methods for signal flight time and ranging value are as follows:

[0067]

[0068] In the formula, t tof The signal flight time is represented by d, the ranging value is represented by , and T1, T2, T3, and T4 represent the total time of the four communication processes in the AltDS-TWR method, respectively. The train mileage calculation model based on dual base stations is as follows:

[0069]

[0070] In the formula, Dis n The distance measured by the nth base station is represented by L, the distance between the two base stations is represented by OD, and the total distance traveled by the train is represented by OD. n od represents the distance traveled by the train relative to the nth base station. n This represents the distance the train has traveled relative to its originating base station.

[0071] like Figure 3 As shown, the UWB ranging process adopts an improved AltDS-TWR structure that includes four communications: Poll, Resp, Final, and Ack, in order to reduce ranging latency in high dynamic scenarios.

[0072] In this embodiment of the invention, in step S2, the measured mileage information is converted into corresponding three-dimensional position and attitude angles using a digital track map, serving as the observation input for the integrated navigation system. The digital track map is a single-valued function track database built based on mileage, containing the three-dimensional coordinates, tangential angles, and section numbers corresponding to each point within the tunnel, used to accurately provide train attitude reference information. The mathematical model of the digital track map is as follows:

[0073]

[0074] In the formula, l represents the line mileage, and (x,y) represents the planar position of a point on the line. Let f1 be the tangential angle at a certain point on the line, and f2, f3 be the corresponding relational functions.

[0075] Attitude information at the corresponding location is obtained using a digital orbit map, and this information is then used to supplement and suppress inertial navigation errors. The standard deviation model for the heading angle error corresponding to the odometer measurement standard is as follows:

[0076]

[0077] In the formula σ ψ R represents the standard deviation of the heading angle error. min σ represents the minimum curve radius of a subway tunnel. od This represents the standard deviation of the mileage measurement error.

[0078] In this embodiment of the invention, in S3, gyroscope and accelerometer information from the inertial measurement unit (IMU) are received in real time, and position, velocity, and attitude are calculated. The state vector and observation vector in the error state Kalman filter (ESKF) are then constructed using the state-space method. The SINS filter state variable modeling is as follows:

[0079]

[0080] In the formula, X is the filter state variable, δr is the position error, δv is the velocity error, and φ is the misalignment angle error. ε represents the accelerometer's zero bias, and ε represents the gyroscope's zero drift. The attitude matrix calculated by SINS. Let φ(×) be the ideal attitude matrix recorded in the digital orbit map, φ(×) be the antisymmetric matrix of φ, I be the identity matrix, and p SINS For SINS location measurement data, p UWB This is UWB location measurement data, where Z is the observation vector, H is the observation matrix, and 0... n×n Represents an n x n zero matrix.

[0081] In this embodiment of the invention, the mileage error is modeled as follows:

[0082]

[0083] In the formula ε od The total error in mileage measurement is μ. od Let σ be the mean error of mileage measurement. od Let ε be the variance of the mileage measurement error. dynamic This is a dynamic error, the value of which is related to hardware delay and train speed changes. This paper assumes that the hardware delay is stable and the train runs at a constant speed, and will take the value as 0.3m. The NLOS error after calculation using the odometer model is ε before calculation. nlos The positive deviation b is uniformly distributed with values ​​ranging from 0.5 to 1 m. The standard deviation of the NLOS error after calculation using the mileage model is given, and the standard deviation σ before calculation is given. nlos The value is 2 times σ static ;σ static The static error standard deviation is related to hardware performance and is calibrated to 0.11m. σ is the static error coefficient, with a value of 0.8. system Take the value 0.08m; let The calculated standard deviation of the UWB odometer measurement error is no more than 0.17m under line-of-sight conditions. The error of the position information obtained after digital orbit map conversion is also no more than 0.17m. Considering the influence of NLOS error, the actual error may occasionally be larger. Therefore, σ is ultimately taken as... od It is 0.2m.

[0084] In this embodiment of the invention, S4 includes the following sub-steps:

[0085] S41. Establish adaptive position and attitude and robust adaptive position and attitude methods to adjust the measurement noise covariance matrix in real time and perform error state Kalman filtering.

[0086] S42. Combine the Kalman filter solution result with the SINS solution result to output the train position and attitude information. In this embodiment of the invention, in S41, the adjustment of the position measurement variance by the adaptive position and attitude is as follows:

[0087]

[0088] In the formula R Pk ψ represents the variance of the position measurement at the k-th iteration. k R represents the measured heading angle at the k-th iteration. od The measurement variance representing mileage;

[0089] The robust adaptive position and pose method model is as follows:

[0090]

[0091] In the formula With R ki The representative stated and R k The i-th diagonal element in R k Represents the measurement covariance matrix. Represents R k The equivalent measurement noise covariance matrix, R is the robustly weighted adaptive position measurement variance matrix. A Let u be the attitude measurement variance matrix, w be the robust weighting factor, and u be the robust weighting factor in the formula. i =v i / σ i variance factor k0 is taken as 1 to 1.5, k1 as 2.5 to 3, and ε as 10. -4 .

[0092] The error Kalman filter solution process is as follows:

[0093]

[0094] In the formula X k and P k These are the state variable matrix and error covariance matrix at time k, respectively; X k / k-1 and P k-1 These are the state prediction matrix and the error covariance prediction matrix, respectively; Φ k / k-1 Q is the corresponding state transition matrix; k-1 To predict covariance; K k Z is the corresponding Kalman filter gain matrix; k H k and R k Let be the measurement vector matrix, observation matrix, and measurement noise covariance matrix at time k, respectively; I is the identity matrix.

[0095] In this embodiment of the invention, the process of combining the Kalman filter solution result with the SINS solution result to output the train position and attitude information in S42 is as follows:

[0096]

[0097] In the formula, p, v, and a represent the position, velocity, and acceleration information calculated by SINS, and δp, δv, and δa represent the error correction information calculated by the error Kalman filter. This represents the train's position, speed, and acceleration information after information fusion.

[0098] The information fusion scheme of this invention is as follows: Figure 2As shown, A1 and A2 are dual UWB base stations, A3 is an onboard UWB tag, and the onboard UWB tag can obtain the train's mileage information (A4) by receiving the distance information from the two UWB base stations; A5 is a digital track map, and the mileage information from A4 is transmitted to the digital track map to obtain the train's position and attitude information; A7 is an IMU, which uses SINS to calculate and transmits the filtering parameters along with the position and attitude information obtained from A5 to A6 for error Kalman filtering and error correction data; after obtaining the correction data, it is retransmitted to A8 for calculation, and finally the output in A9 is obtained.

[0099] To verify the subway train positioning method of this invention, MATLAB software was used to simulate the subway train's trajectory in a tunnel. The initial position was set with latitude of 30.770556°, longitude of 103.983611°, and altitude of 525m. The train started from the initial position and headed due east, maintaining a speed of 30m / s (108km / h). The turning speeds were designed as ω1, ω2, and ω3, with magnitudes of 3° / s, 2° / s, and 1° / s, respectively, corresponding to track curvature radii of 572.9578m, 859.4367m, and 1718.8734m. The minimum radius strictly adhered to the minimum curvature radius of 650m under a speed limit of 100km / h. The specific travel plan is shown in Table 1 in chronological order, and the subway trajectory is as follows. Figure 4 As shown.

[0100] Table 1

[0101]

[0102]

[0103] The simulation describes the positioning of a subway train traveling at a constant speed of 30 m / s (108 km / h) within a section after leaving the station. At the start of positioning, the initial position is set with an error of 1 m from the initial point of the simulation trajectory (latitude 30.770556°, longitude 103.983611°, altitude 525 m), an initial speed error of 1 km / h, and an initial misalignment angle error of φ. E =1,φ N =-1,φ U =20, in minutes. The accuracy of UWB mileage measurement is set to 0.2m based on the error model estimate, evenly distributed across the horizontal directions. Considering the small variation in altitude, the standard deviation of the position error is set to... The standard deviation of the attitude error was set to [0.0003, 0.0003, 0.0003]. Additionally, considering that UWB might be used for communication simultaneously, leading to a decrease in the positioning frequency, simulations were performed with update frequencies of 10Hz and 50Hz respectively. The strapdown inertial navigation measurement frequency was 100Hz, updated using the two-sample algorithm, with an output frequency of 50Hz. The triaxial accelerometer measurement zero bias was set to... Speed ​​random walk set Because UWB has a high calibration frequency and accuracy, high-precision positioning results can be obtained using rate-level inertial navigation. Therefore, the gyroscope's zero bias and random walk are set as follows:

[0104]

[0105] The subscripts E, N, and U represent the components in the east, north, and sky directions, respectively.

[0106] Using inertial sensor data under the ideal simulation trajectory, and adding corresponding noise errors, a simulation of the SINS update algorithm and the combined UWB / SINS positioning algorithm is performed. The resulting trajectory is then compared with the ideal trajectory to obtain the positioning error of the basic method. Figure 5 and Figure 6 The positioning error of the improved method is as follows: Figure 7 and Figure 8 As shown, a comparison chart of the cumulative error distribution between the improved method and the basic method is then created, as shown in the figure. Figure 9 As shown.

[0107] Depend on Figure 5 It can be seen that under high-precision, high-frequency UWB measurement and calibration, the positioning error of the rate-level inertial navigation system does not exceed 10cm, and centimeter-level positioning accuracy can be achieved. Figure 6 Compared to Figure 5 With the addition of attitude calibration information, as shown in the figure comparison, the misalignment angle error of the integrated navigation is suppressed to a very small range after the addition of attitude calibration information, and the positioning accuracy is significantly improved. From the perspective of the average horizontal error, the positioning accuracy of both positioning methods is significantly improved at different calibration frequencies, with an improvement of about 42.3% at a calibration frequency of 10Hz and about 43.3% at 50Hz.

[0108] Depend on Figure 7 It can be seen that after adaptively processing the position error using the heading angle, the extreme value of the horizontal positioning error is slightly reduced, while the mean horizontal error is improved by approximately 2.3% at calibration frequencies of 10Hz and 50Hz. Figure 8It can be seen that after robust improvement of the adaptive position and attitude combination positioning method, the mean value of the overall horizontal error of the filter decreases by about 29.2% and 31.0%, respectively, the positioning accuracy is significantly improved, the convergence speed is significantly accelerated, and the filter oscillation amplitude caused by the initial positioning error interference is significantly reduced. At the calibration frequency of 10Hz and 50Hz, the extreme values ​​of the horizontal error in the initial stage of filtering decrease by about 36.3% and 27.0%, respectively.

[0109] Depend on Figure 9 It can be seen that the improved combined positioning method of this invention has a steeper planar position error distribution function curve, with the error concentrated in a smaller range, thus effectively improving the positioning accuracy. Among them, the improved robust adaptive position and attitude combined positioning method has the best accuracy, with its absolute mean error not exceeding 4cm and most errors within 20cm, indicating that its maximum error does not exceed 20cm after the filter stabilizes.

[0110] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0111] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0112] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0113] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0114] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the above embodiments. Within the scope of knowledge possessed by those skilled in the art, various changes can be made without departing from the spirit of the present invention.

[0115] Many other changes and modifications can be made without departing from the concept and scope of this invention. It should be understood that this invention is not limited to the specific embodiments, and the scope of this invention is defined by the appended claims.

Claims

1. A high-precision train combination positioning method suitable for subway tunnel environments, characterized in that, The method includes: S1: By improving the UWB ranging process and adopting the AltDS-TWR structure including four communications, high-precision ranging data between the train and the base station is obtained; combined with the deployment of dual base stations, a real-time estimation model for train running mileage is constructed. S2: Input the mileage information obtained in step S1 into the pre-built digital track map and map it into the current three-dimensional position and tangential attitude angle of the train, which is used as the observation input of the integrated navigation system; S3: Receive gyroscope and accelerometer data from the inertial measurement unit (IMU) in real time, and calculate the position, speed and attitude of the train through the inertial navigation algorithm (SINS); use the position and attitude information provided in step S2 as observation values, and construct the state vector and observation vector in the error state Kalman filter (ESKF) through state space modeling. S4: The SINS solution results in step S3 are fused with the UWB observation information in step S2. The measurement noise covariance matrix is ​​dynamically updated using an adaptive position and attitude adjustment and robust M estimation strategy. Multi-source data fusion is completed through an error state Kalman filter to output the train's precise position and attitude information.

2. The high-precision train combination positioning method suitable for subway tunnel environments according to claim 1, characterized in that, In step S1, distance data between the train and the base station is obtained through an improved UWB ranging process, and a mileage calculation model based on dual base stations is constructed to measure the train's running mileage in real time. The calculation methods for signal flight time and ranging values ​​are as follows: In the formula, t tof The signal flight time is represented by d, the ranging value is represented by , and T1, T2, T3, and T4 represent the total time of the four communication processes in the AltDS-TWR method, respectively. The train mileage calculation model based on dual base stations is as follows: In the formula, Dis n The distance measured by the nth base station is represented by L, the distance between the two base stations is represented by OD, and the total distance traveled by the train is represented by OD. n od represents the distance traveled by the train relative to the nth base station. n This represents the distance the train has traveled relative to its originating base station.

3. The high-precision train combination positioning method suitable for subway tunnel environments according to claim 1, characterized in that, In step S1, the UWB ranging process adopts an improved AltDS-TWR structure with four communications, including four steps: Poll, Resp, Final, and Ack.

4. The high-precision train combination positioning method suitable for subway tunnel environments according to claim 1, characterized in that, In step S2, the digital track map is a single-valued function track database built based on mileage, including the three-dimensional coordinates, tangential angles, and section numbers corresponding to each point within the tunnel, used to accurately provide train position and orientation reference information; the mathematical model of the digital track map is as follows: In the formula, l represents the line mileage, and (x,y) represents the planar position of a point on the line. Let f1 be the tangential angle at a certain point on the line, and f2, f3 be the corresponding relational functions.

5. A high-precision train combination positioning method suitable for subway tunnel environments according to claim 1, characterized in that, In step S2, attitude information at the corresponding location is obtained using a digital orbit map. This attitude information is then used to supplement the inertial navigation system and suppress errors. The standard deviation model for the heading angle error corresponding to the odometer measurement standard is as follows: In the formula σ ψ R represents the standard deviation of the heading angle error. min σ represents the minimum curve radius of a subway tunnel. od This represents the standard deviation of the mileage measurement error.

6. The high-precision train combination positioning method suitable for subway tunnel environments according to claim 1, characterized in that, In step S3, the SINS filter state variables are modeled as follows: In the formula, X is the filter state variable, δr is the position error, δv is the velocity error, and φ is the misalignment angle error. ε represents the accelerometer's zero bias, and ε represents the gyroscope's zero drift. The attitude matrix calculated by SINS. Let φ(×) be the ideal attitude matrix recorded in the digital orbit map, φ(×) be the antisymmetric matrix of φ, I be the identity matrix, and p SINS For SINS location measurement data, p UWB This is UWB location measurement data, where Z is the observation vector, H is the observation matrix, and 0... n×n Represents an n x n zero matrix.

7. A high-precision train combination positioning method suitable for subway tunnel environments according to claim 1, characterized in that, Step S4 includes: S401: Establish an adaptive position and attitude method and a robust adaptive position and attitude method to adjust the measurement noise covariance matrix in real time and perform error state Kalman filtering; S402: Combines the Kalman filter solution results with the SINS solution results to output the train position and attitude information.

8. A high-precision train combination positioning method suitable for subway tunnel environments according to claim 7, characterized in that, In step S401, the adjustment of the position measurement variance by the adaptive position attitude is as follows: In the formula R Pk ψ represents the variance of the position measurement at the k-th iteration. k R represents the measured heading angle at the k-th iteration. od The measurement variance representing mileage; In step S401, the robust adaptive position and pose method model is as follows: In the formula With R ki The representative stated and R k The i-th diagonal element in R k Represents the measurement covariance matrix. Represents R k The equivalent measurement noise covariance matrix, R is the robustly weighted adaptive position measurement variance matrix. A Let u be the attitude measurement variance matrix, w be the robust weighting factor, and u be the robust weighting factor in the formula. i =v i / σ i variance factor k0 is taken as 1 to 1.5, k1 as 2.5 to 3, and ε as 10. -4 ; In step S401, the error Kalman filter solution process is as follows: In the formula X k and P k These are the state variable matrix and error covariance matrix at time k, respectively; X k / k-1 and P k-1 These are the state prediction matrix and the error covariance prediction matrix, respectively; Φ k / k-1 Q is the corresponding state transition matrix; k-1 To predict covariance; K k Z is the corresponding Kalman filter gain matrix; k H k and R k Let be the measurement vector matrix, observation matrix, and measurement noise covariance matrix at time k, respectively; I is the identity matrix.

9. A high-precision train combination positioning method suitable for subway tunnel environments according to claim 7, characterized in that, In step S402, the process of combining the Kalman filter solution result with the SINS solution result to output the train position and attitude information is as follows: In the formula, p, v, and a represent the position, velocity, and acceleration information calculated by SINS, and δp, δv, and δa represent the error correction information calculated by the error Kalman filter. This represents the train's position, speed, and acceleration information after information fusion.

10. A high-precision train combination positioning system suitable for subway tunnel environments, characterized in that, The system is used to perform the method according to any one of claims 1-9, the system comprising: UWB ranging and mileage calculation module: Through an improved UWB ranging process, it adopts an AltDS-TWR structure including four communications to obtain high-precision ranging data between the train and the base station; combined with the deployment of dual base stations, it constructs a real-time estimation model for train running mileage. Digital track map matching and 3D pose conversion module: Input the mileage information obtained from the UWB ranging and mileage calculation module into the pre-constructed digital track map, and map it into the current 3D position and tangential attitude angle of the train, which is used as the observation input of the integrated navigation system; The IMU calculation and state vector modeling module receives gyroscope and accelerometer data from the inertial measurement unit (IMU) in real time, calculates the position, speed and attitude of the train through the inertial navigation algorithm SINS, and uses the position and attitude information provided by the digital track map matching and 3D pose conversion module as observation values ​​to construct the state vector and observation vector in the error state Kalman filter (ESKF) through state space modeling. Data fusion and precise positioning output module: It integrates the SINS solution results from the IMU solution and state vector modeling module with the UWB observation information from the digital track map matching and 3D pose conversion module. It dynamically updates the measurement noise covariance matrix using an adaptive position and attitude adjustment and robust M estimation strategy. It completes multi-source data fusion through an error state Kalman filter and outputs the precise position and attitude information of the train.

Images