A Digital Track Map-Assisted SINS/OD Fusion Algorithm for Onboard Train Localization

Abstract

Accurate and reliable speed and position estimation plays an important role in the safety and efficiency of intelligent railway vehicles. Due to the level required of safety, reliability, and strong norms in the current practical application, intelligent railway vehicle positioning heavily relies on a large number of balises laid on the track and the onboard odometer (OD), while the other position method, GNSS introduction, is relatively slow. This article proposed a digital track map-assisted onboard railway location system using strapdown inertial navigation system (SINS) and OD. The proposed method consists of two steps. First, an SINS- and OD-integrated navigation method based on OD velocity integration is in the inner circle. Then, a map-matching algorithm based on vertical projection and heading weighting was employed, and when the matching outer circle results were obtained, the positions obtained from the matching outer circles were used to replace the positions obtained from SINS/OD for the Kalman filter combination. The performance of our algorithm was verified using field tests, and SINS/OD and SINS/OD/MM comparison data processing results prove that our proposed digital track map-assisted SINS/OD algorithm can effectively suppress the accumulation of train position errors. After nearly 80 km of navigation, the position error is 24 m, and the relative mileage accuracy is less than or equal to 0.03% distance.

1. Introduction

It is well known that speed and position are very important for an intelligent railway vehicle system. The accurate speed and position of a train is the key element of the automatic protection system and control system (ATP&ATC) of a train [1,2,3,4].

By the end of 2022, the operation mileage of China’s high-speed railway exceeded 40,000 km, and the total railway operation mileage exceeded 150,000 km. However, the current speed measurement and positioning methods of trains mainly rely on trackside equipment and onboard equipment [5,6,7]. The typical onboard equipment is the odometer (OD), which is used to estimate the speed and traveled distance of the train at the same time [8,9,10]. Considering the uncertainty and complexity of the train operation environment and the requirements of high reliability, the OD-based method has certain limitations, such as slipping in acceleration and deceleration and wear of train wheel diameter. Therefore, the use of balises to correct the speed measurement and traveled distance error of the odometer has become the standard configuration of the current train control system [1,3,11]. To satisfy the Safety Integrity Level (SIL) requirement in intelligent railway vehicles, which is 10⁻⁹/h for SIL 4, the average laying distance of the transponder is 2 km, whether in the Chinese Train Control System (CTCS) [3,4] or the European Train Control System (ETCS) [12]. Because the basile is exposed outdoors, it is easily damaged, and the high-density layout, operation, and maintenance costs are not a small burden, especially in sparsely populated areas in Western China [13].

According to the Certifiable Localization Unit with the Global Navigation Satellite System (GNSS) in the railway environment (CLUG) project funded by the European Union (EU), one of the key characteristics of the proposed onboard location unit is a multi-sensor fusion engine that utilizes a digital track map. In the CLUG project, digital track maps play a pivotal role as primary inputs to both the navigation engine and the integrity engine, which collectively constitute the foundational components of the onboard localization system. In the train positioning system, the primary method of multiple sensor fusion is based on the integration of the global position from GNSS and dead reckoning primarily through inertial navigation with OD [4,14]. Due to the complexity of the train operating environment compared to civil aviation applications, trains pass through areas with serious GNSS signal blockages and multi-path phenomena, such as dense forests, canyons, urban areas, as well as long tunnels with no GNSS signal at all [15]. In such cases, dead-reckoning systems without corrective global position signal are prone to divergence. Fortunately, the center-line coordinates in the digital track map are based on coordinates in the Earth-Centered Earth-Fixed (ECEF) system and naturally have global properties. Therefore, digital track map matching is a good substitute for when GNSS signals are unavailable [16].

Map matching is a very important source of information for train location because the train is constrained to the track and because train location is essentially one-dimensional compared to the 3D navigation of road vehicles, as long as the position of the train relative to the starting point is determined [17]. In addition to being able to improve the positional accuracy of train positioning, the use of map-matching calculations can also correct errors such as gyro drift in inertial navigation systems and scale factors in odometers. Map-matching algorithms exhibit a spectrum ranging from elementary search techniques to those using more complex mathematical techniques [18], including Kalman Filters [19] and Hidden Markov Models (HMMs) [20]. A number of different algorithms have been proposed for map matching across various application domains, each of which carries its own set of merits and demerits [21]. It includes geometric matching algorithms, probabilistic matching algorithms, tight coupling matching algorithms, and comprehensive matching algorithms [22]. Among these categories, geometric matching represents the most fundamental and intuitive approach, encompassing point-to-point matching. Point-to-point matching can be considered equivalent to a classical search problem, and a subset of feature-matching problems, which are based on similarity measures, falls within the domain of point-to-point matching algorithms. For instance, in reference [23], track irregularities are employed as a background map to correct train positioning errors. However, maps based on track irregularities are difficult to obtain, especially after railroad lines have been maintained and rebuilt. Saab endeavored to attain train positioning through map matching. The essence of his approach revolved around matching the angular rate data derived from a known map’s track segments with the measurements recorded by the onboard gyroscope. Nevertheless, in instances when the train was traversing a straight stretch, a successful matched proved elusive. The approach taken in Reference [17] primarily involves using map matching to vertically project the fused location onto the electronic track map. This projection method effectively constrains the lateral error of the train, but is less effective in correcting longitudinal errors. Compared with existing methods in the literature, the digital track-based map-assisted SINS/OD algorithm proposed in this paper can partially eliminate the role of balises and reduce the construction and operation costs; in addition, the construction of the basic layer of the track map requires only the three-dimensional discrete point coordinates of the track median, and it can effectively correct the bias error of the IMU. Lastly, the method in this paper can effectively constrain the lateral errors, which is independent of the curvature of the track.

This paper has two main contributions. Firstly, it proposes a map-matching method using integrated weighting that can effectively constrain the lateral and longitudinal errors; secondly, it proposes a filtering framework that fuses the electronic track map with the existing SINS/DR, which can improve the positioning accuracy of the train while correcting for the inertial error.

The remainder of this article is organized as follows. Section 2 presents the construction and characteristics of the digital track map that can be used for train localization. Section 3 describes the onboard train localization algorithm, which is the focus of this paper. Section 4 describes the experiments and performance analysis. Concluding remarks are summarized in Section 5.

2. Construction and Characteristics of Digital Track Map

Map matching is widely used for the calibration of SINS/DR sensor parameters. For example, regarding the calibration of the scale factor of the wheel speed sensor of the train, we can calibrate the scale factor of the odometer by comparing the reading of the wheel speed sensor with the length of the track section given by the digital track map database; if the train is travelling on a straight section shown by the map, any angular rate reading given by the gyroscope that is not equal to zero is equal to the value of the gyroscope bias. In the case of a turn, the gyroscope scale factor can theoretically be calibrated based on the angle of the direction between the two sections at the turn, as given in the digital track map’s network database. Based on these observations, we will now implement these results through an executable map-matching algorithm.

Definition and Accuracy of Required Digital Track Map

In order to meet the high-level position requirement, all absolute coordinates in the digital track map are referenced to the World Geodetic System—1984 Coordinate System (WGS84) reference system. The digital track map consists of a layered structure, consisting of a basic layer and an attribute layer, as depicted in Figure 1, where the basic layer is composed of three-dimensional coordinate points of the track’s center line in the ECEF coordinate system. For digital track maps applied to the train’s location algorithms, the logical relationships between infrastructure objects within the railway network and the absolute position of the track’s center line (longitude, latitude, and altitude) are mandatory elements. Other information, such as the radius of curvature of the track and the slope, can be calculated from the coordinates of the track’s center line and are therefore optional data.

Railroads in China are laid on a standard gauge, with a distance of 1.435 m between two rails. The distance between two rails is 1.435 m. In order to prevent side conflicts between vehicles, the positioning error of the train position in the vertical rail direction (transverse direction) is required to be less than 1.5 m. According to the requirements of the train control system on the positioning accuracy and integrity of the train, in addition to the departure and Automatic Train Operation (ATO) automatic parking requirements of the longitudinal positioning accuracy of better than 10 m, the positioning accuracy of the train interval tracking is ±5 m + 5% s [17]. In addition, as a result of Airbus’ accuracy analysis using a total error budget of 10 m for the localization algorithm, an upper bound of 0.1 m can be applied for the cross-track error. In practice, the digital map of the track is organized as a list of discrete center points, each with TrackEdge ID and geographic and offset information as shown in Equation (1). In Equation (1), the map information includes TrackEdgeAtrribute_Type, TrackEdge_ID, AttributePoint_Offset, AttributePoint_Long, AttributePoint_Lat, AttributePoint_Alt, and other geographic information. All the geographic information can be downloaded from the CLUG project.

$$\begin{array}{l}\mathrm{map}=\{\mathrm{TrackEdgeAttribute}\_\mathrm{Type},\mathrm{TrackEdge}\_\mathrm{ID},\mathrm{AttributePoint}\_\mathrm{Offset}\\ \mathrm{AttributePoint}\_\mathrm{Long},\mathrm{AttributePoint}\_\mathrm{Lat},\mathrm{AttributePoint}\_\mathrm{Alt}\dots \}\end{array}$$

(1)

To utilize map matching for the train’s lateral and longitudinal error correction, it is necessary to establish the cross-track error between the maximum distance among the map’s center points, the radius of curvature. Furthermore, the positions obtained from map matching will be used as observations in Kalman filtering, and the observations of Kalman filtering need to be set up with measurement noise R in order to form a complete closed loop of Kalman filtering.

$$e_2=R-R\cos \left(\frac{l}{2R}\right)$$

(2)

According to Figure 2, the maximum cross-track error $e_2$ is a function of the radius of curvature $R$ and the distance $l$ from the discrete points on the curve [24].

$$e_1=l-2R\sin \left(\frac{l}{2R}\right)$$

(3)

$e_1$ is the difference between the length of the curve and the length of the line segment $D$. The detailed summary of track’s center line distances in different curve radii can be founded in [15]. Taking the maximum cross-track error of 0.1 m as the upper bound, the larger the radius of curvature of the track, the longer the distance D between the discrete points can be, and the smaller the amount of stored data is required for the map. Because the train track is mainly composed of straight lines, curves, and spiral curves, the cross-track error will be very small when the radius of the curvature is large enough or when it is a straight line; at this time, the distance D of the discrete points on the track map can be very large. For example, when the radius of curvature is 1000 m, the distance D of the discrete points on the map is taken to be 80 m, and the maximum lateral error will still be only 0.08 m.

3. Onboard Train Localization Algorithm

The onboard train localization scheme based on SINS/OD aided by digital track map matching is principally divided into three phases: SINS/OD dead reckoning, map matching based on SINS/OD position, and data fusion, as depicted in Figure 3. In the SINS/OD dead-reckoning phase, the onboard SINS/OD system generates three-dimensional measurements for the acceleration, angular rate, and one-dimensional velocity parameters for the train. Compared to SINS, SINS/OD dead reckoning utilizes the velocity information from odometers and can further improve accuracy through closed-loop feedback to compensate the gyro and accelerometer biases. However, due to the cumulative errors associated with odometers, for the long-duration SINS/OD dead-reckoning system, the navigation accuracy can still diverge.

During the map-matching phrase, the ellipsoidal coordinates need to be projected to the plane coordinates using the Gaussian Krüger projection, and then the position outputted from the combined navigation system of SINS/OD is projected to the digital track map to constrain the cross-track error [25]. Afterward, the train’s position is calculated using the speed of the odometer and the direction of the digital track map of the track; finally, the two positions are weighted as the train position.

In the data fusion phase, a dual-loop Kalman filtering framework is employed. In the absence of map matching, the SINS/OD dead-reckoning system within the inner loop for high-frequency updates is employed. During the measurement update, the observations consist of the position difference between the DR and the position obtained from the SINS [26]. The combination position will be matched with the digital track map in this paper to perform matching every 10 m on average based on the digital track map we used. The distance depends on the density of the digital track map. If not matched, the DR result is outputted. If the position matching is successful, the matched position replaces the DR-derived position for measurement updates. Because the map data are defined in the Earth-Centered Earth-Fixed (ECEF) absolute coordinate system, obtaining accurately matched position information is equivalent to obtaining accurate absolute GNSS positioning information.

The reference coordinate frames involved in this paper are defined as follows:

(1)

e frame: ECEF orthogonal reference frame.

(2)

b frame: orthogonal reference frame aligned with Right-Forth-Up (RFU) axes.

(3)

n frame: orthogonal reference frame aligned with actual East-North-Up [27].

(4)

m frame: the odometer measurement coordinate system, abbreviated as $om$, with $o{y}_m$ axes in the ground plane in contact with the wheels of the vehicle and pointing straight ahead of the vehicle, $o{z}_m$ axes perpendicular to the ground plane and pointing upward, and $o{x}_m$ axes pointing to the right; the odometer coordinate system is a right-handed right-angled “right-front-up” coordinate system solidly attached to the body of the vehicle.

3.1. OD Measurement Modeling

The OD installation and the relative positions of the OD and SINS are shown in Figure 4. The projected component of the OD speed measurement in the OD coordinate system is ${\tilde{\mathit{v}}}_D^m={[\begin{array}{ccc}0& {\tilde{v}}_D& 0\end{array}]}^{\mathrm{T}}$. The OD speed under the n system is expressed as ${\tilde{\mathit{v}}}_D^n$. ${\tilde{\mathit{C}}}_m^b$ is the direction cosine matrix (DCM) from m frame to b frame; ${\tilde{\mathit{C}}}_b^n$ is the DCM from b system to n system. ${\alpha }_{\theta },{\alpha }_{\psi }$ are pitch and yaw misalignment angles from the m frame to the b frame, respectively. ${\mathit{\varphi }}^n$ is the attitude misalignment angle of SINS. $\delta K$ and ${\mathit{w}}_D^m$ are the scale factor of the OD and the measurement noise, respectively. $L_D^b{}_{}$ is the lever arm between the SINS and OD.

Because the SINS/OD solution is performed in the n coordinate system, the first step is to construct a measurement error model from the m system to the n system

$$\{\begin{array}{l}{\tilde{\mathit{v}}}_D^n={\tilde{\mathit{C}}}_b^n{\tilde{\mathit{C}}}_m^b{\tilde{\mathit{v}}}_D^m\\ {\tilde{\mathit{C}}}_b^n=\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]{\mathit{C}}_b^n,{\tilde{\mathit{C}}}_m^b=\left[{\mathit{I}}_{3\times 3}-\left(\delta \mathit{\alpha }\times \right)\right]{\mathit{C}}_m^b\\ \delta \mathit{\alpha }={\left[\begin{array}{ccc}\delta {\alpha }_{\theta }& 0& \delta {\alpha }_{\psi }\end{array}\right]}^{\mathrm{T}},{\tilde{\mathit{v}}}_D^m=\left(1+\delta K_D\right){\mathit{v}}_D^m+{\mathit{w}}_D^m\end{array}$$

(4)

Expanding the first term in Equation (4) yields:

$$\begin{array}{ll}{\tilde{\mathit{v}}}_D^n& =\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]{\mathit{C}}_b^n\left[{\mathit{I}}_{3\times 3}-\left(\delta \mathit{\alpha }\times \right)\right]{\mathit{C}}_m^b\left(\left(1+\delta K_D\right){\mathit{v}}_D^m+{\mathit{w}}_D^m\right)\\ & =\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]{\mathit{C}}_b^n\left[{\mathit{I}}_{3\times 3}-\left(\delta \mathit{\alpha }\times \right)\right]{\mathit{C}}_m^b\left({\mathit{v}}_D^m+\delta K_D{\mathit{v}}_D^m+{\mathit{w}}_D^m\right)\\ & =\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]{\mathit{C}}_b^n\left[{\mathit{v}}_D^b+\delta K_D{\mathit{v}}_D^b+{\mathit{v}}_D^b\times \delta \mathit{\alpha }+{\mathit{w}}_D^b\right]\end{array}$$

(5)

In order to convert the speed measured by the OD to the SINS measurement point, the effect of the lever arm needs to be taken into account, so we have:

$${\tilde{\mathit{v}}}_{DI}^b={\mathit{v}}_D^b+\delta K_D{\mathit{v}}_D^b+{\mathit{v}}_D^b\times \delta \mathit{\alpha }-{\mathsf{\omega }}_{eb}^b\times {\tilde{L}}_D^b+{\mathit{w}}_D^b\left(1\right)$$

(6)

We take the result of Equation (6) and bring it into Equation (5), and Equation (7) is obtained as follows:

$$\begin{array}{ll}{\tilde{\mathit{v}}}_{DI}^n& =\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]{\mathit{C}}_b^n\left[{\mathit{v}}_D^b+\delta K_D{\mathit{v}}_D^b+{\mathit{v}}_D^b\times \delta \mathit{\alpha }-{\mathsf{\omega }}_{eb}^b\times {\tilde{L}}_D^b+{\mathit{w}}_D^b\right]\\ & =\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]{\mathit{C}}_b^n\left[{\mathit{v}}_D^b-{\mathsf{\omega }}_{eb}^b\times L_D^b+\delta K_D{\mathit{v}}_D^b+{\mathit{v}}_D^b\times \delta \mathit{\alpha }-{\mathsf{\omega }}_{eb}^b\times \delta L_D^b+{\mathit{w}}_D^b\right]\\ & =\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]{\mathit{C}}_b^n\left[{\mathit{v}}_{DI}^b+\delta K_D{\mathit{v}}_D^b+{\mathit{v}}_D^b\times \delta \mathit{\alpha }-{\mathsf{\omega }}_{eb}^b\times \delta L_D^b+{\mathit{w}}_D^b\right]\\ & =\left[{\mathit{I}}_{3\times 3}-\left({\mathit{\varphi }}^n\times \right)\right]\left[{\mathit{v}}_{DI}^n+\delta K_D{\mathit{v}}_D^n+{\mathit{v}}_D^n\times \delta \mathit{\alpha }-{\mathsf{\omega }}_{eb}^n\times \delta L_D^n+{\mathit{w}}_D^n\right]\\ & \approx {\mathit{v}}_{DI}^n+{\mathit{v}}_{DI}^n\times {\mathit{\varphi }}^n+\delta K_D{\mathit{v}}_D^n+{\mathit{v}}_D^n\times \delta \mathit{\alpha }-{\mathsf{\omega }}_{eb}^n\times \delta L_D^n+{\mathit{w}}_D^n\\ & ={\mathit{v}}_{DI}^n+{\mathit{v}}_{DI}^n\times {\mathit{\varphi }}^n+{\mathit{M}}_v^n{\mathsf{\kappa }}_D-{\mathsf{\omega }}_{eb}^n\times \delta L_D^n+{\mathit{w}}_D^n\end{array}$$

(7)

where

$${\mathit{M}}_v^b=\left[\begin{array}{ccc}0& v_{Dy}^b& v_{Dx}^b\\ v_{Dz}^b& -v_{Dx}^b& v_{Dy}^b\\ -v_{Dy}^b& 0& v_{Dz}^b\end{array}\right],{\mathsf{\kappa }}_D=\left[\begin{array}{c}\delta {\alpha }_{\theta }\\ \delta {\alpha }_{\psi }\\ \delta K_D\end{array}\right]$$

(8)

Using Equation (7), we can obtain ${\mathit{v}}_D^n$, and then follows the DR position update algorithm

$${\dot{\mathit{p}}}_{DR}=\left[\begin{array}{c}{\dot{L}}_{DR}\\ {\dot{\lambda }}_{DR}\\ {\dot{h}}_{DR}\end{array}\right]=\left[\begin{array}{ccc}0& 1/R_{MhDR}& 0\\ \sec L_{DR}/R_{NhDR}& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\mathit{v}}_{ED}^n\\ {\mathit{v}}_{ND}^n\\ {\mathit{v}}_{UD}^n\end{array}\right]$$

(9)

where, $L$, $\lambda$, and $h$ are latitude, longitude, and height. $R_{MhDR}$ and $R_{NhDR}$ are the radius of the meridian circle and the prime circle.

3.2. Map Matching Utilizing SINS/OD Dead-Reckoning Results

First of all, in order to control the longitudinal error of the train, as shown in Figure 5, assuming that $P$ and $P^{\prime }$ are the positions of the train at moments $t$ and $t^{\prime }$, respectively, the position at $P$ is a known position computed from the previous moment, and using the highly accurate heading angle of the digital map of the track as well as the speed output of the OD, we are able to compute the position of the train at the next moment using Equation (6).

$$\begin{array}{l}\delta x=\left({\displaystyle \sum _{\mathrm{t}}^{t^{\prime }}\Delta v_{od}\Delta t}\right)\cos {{\psi }^{\prime }}_{}\\ \delta y=\left({\displaystyle \sum _{\mathrm{t}}^{t^{\prime }}\Delta v_{od}\Delta t}\right)\sin {\psi }^{\prime }\end{array}$$

(10)

where $\Delta v_{od}$ is the OD speed, and ${\psi }^{\prime }=\pi -\psi$ is the digital track map heading angle, because we consider clockwise to be the positive direction. By using the heading of the digital track map, together with the short-term high-precision speed information from the OD, we can control the longitudinal error because the short-term OD speed error will not accumulate.

$$W\alpha =\cos (\left|{\psi }_{track}-{\psi }_{SINS/DR}\right|)$$

(11)

First, the measurement of similarity between the heading values was obtained by the SINS/DR algorithm, and that obtained by the orbital digital map was calculated. The smaller the difference of $\left|{\psi }_{track}-{\psi }_{SINS/DR}\right|$, the greater the similarity between the two and the greater the decreasing function cos and vice versa.

On the other hand, in order to control the lateral error of the train, we projected the estimated train position to the electronic map of the track as shown in Figure 6.

$$\begin{array}{c}{x}^{\prime }=\frac{\left(x_2-x_1\right)\left[x\left(x_2-x_1\right)+y\left(y_2-y_1\right)\right]+\left(y_2-y_1\right)\left(x_1{y}_2-x_2{y}_1\right)}{{\left(x_2-x_1\right)}^2+{\left(y_2-y_1\right)}^2}\\ y^{\prime }=\frac{\left(y_2-y_1\right)\left[x\left(x_2-x_1\right)+y\left(y_2-y_1\right)\right]-\left(x_2-x_1\right)\left(x_1{y}_2-x_2{y}_1\right)}{{\left(x_2-x_1\right)}^2+{\left(y_2-y_1\right)}^2}\end{array}$$

(12)

$$\begin{array}{c}{x}^{\prime }=\frac{\left(x_2-x_1\right)\left[x\left(x_2-x_1\right)+y\left(y_2-y_1\right)\right]+\left(y_2-y_1\right)\left(x_1{y}_2-x_2{y}_1\right)}{{\left(x_2-x_1\right)}^2+{\left(y_2-y_1\right)}^2}\\ y^{\prime }=\frac{\left(y_2-y_1\right)\left[x\left(x_2-x_1\right)+y\left(y_2-y_1\right)\right]-\left(x_2-x_1\right)\left(x_1{y}_2-x_2{y}_1\right)}{{\left(x_2-x_1\right)}^2+{\left(y_2-y_1\right)}^2}\end{array}$$

(13)

The perpendicular distance from the estimation point to the center line of the track is shown in Equation (8). The vertical distance $d$ can be used as a basis for weighting or as a basis for outlier rejection.

$$Wd=D/d$$

(14)

3.3. Data Fusion

In the SINS update calculation, attitude updates were already performed. In the dead-reckoning (DR) navigation algorithm, there is no need to perform attitude updates separately. Instead, the attitude matrix from the inertial navigation can be directly used to transform OD measurements into the navigation frame, obtaining the DR navigation velocity in the navigation frame [28,29]. At this point, both the inertial navigation and dead-reckoning navigation use the same attitude matrix, which means they share the same misalignment angle errors. In this way, inertial navigation errors and dead-reckoning navigation errors are combined into the following state vector:

We considered the misalignment errors ${\mathit{\varphi }}^n$, velocity errors $\delta {\mathit{v}}_I^n$, position errors $\delta \mathit{p}$, gyro drift errors ${\mathsf{\epsilon }}_{}^b$, accelerometer biases ${\mathbf{\nabla }}_{}^b$, OD scale factor error, pitch installation angle error, yaw installation angle error ${\mathsf{\kappa }}_D={\left[\begin{array}{ccc}\delta {\alpha }_{\theta }& \delta {\alpha }_{\psi }& \delta K_D\end{array}\right]}^{\mathrm{T}}$, and arm lever error $\delta {\mathit{L}}_D^b$. According to the SINS algorithm and the DR algorithm, the error states of the SINS/OD can be written as follows [30,31]:

$${\mathit{X}}_D={\left[\begin{array}{ccccc}{\left({\mathit{\varphi }}^n\right)}^{\mathrm{T}}& {\left(\delta {\mathit{v}}_I^n\right)}^{\mathrm{T}}& {\left(\delta \mathit{p}\right)}^{\mathrm{T}}& \begin{array}{ccc}{\left({\mathsf{\epsilon }}_{}^b\right)}^{\mathrm{T}}& {\left({\mathbf{\nabla }}_{}^b\right)}^{\mathrm{T}}& {\left({\mathsf{\kappa }}_D\right)}^{\mathrm{T}}\end{array}& {\left(\delta {\mathit{L}}_D^b\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(15)

We defined ${\mathit{F}}_D$ as the state transfer matrix, ${\mathit{G}}_D$ as the system noise-driving matrix, and ${\mathit{W}}_D={\left[\begin{array}{cc}{\mathsf{\epsilon }}_w^b& {\mathbf{\nabla }}_w^b\end{array}\right]}^{\mathrm{T}}$ as the system excitation noise matrix. Considering the OD correlation error state as a random constant, the 21-state SINS/DR-integrated system are modeled as

$${\dot{\mathit{X}}}_D={\mathit{F}}_D{\mathit{X}}_D+{\mathit{G}}_D{\mathit{W}}_D$$

(16)

In which

$${\mathit{F}}_D=\left[\begin{array}{cc}{\mathit{F}}_{15\times 15}& {\mathbf{0}}_{15\times 6}\\ {\mathbf{0}}_{6\times 15}& {\mathbf{0}}_{6\times 6}\end{array}\right],{\mathit{G}}_D=\left[\begin{array}{c}\begin{array}{cc}-{\mathit{C}}_b^n& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{C}}_b^n\end{array}\\ {\mathbf{0}}_{15\times 6}\end{array}\right]$$

(17)

$${\mathit{F}}_{15\times 15}=\left[\begin{array}{c}\begin{array}{ccccc}-{\mathsf{\omega }}_{in}^n\times & {\mathit{F}}_{12}& {\mathit{F}}_{13}& -{\mathit{C}}_b^n& {\mathbf{0}}_{3\times 3}\\ {\mathit{f}}_{sf}^n\times & {\mathit{F}}_{22}& {\mathit{F}}_{23}& {\mathbf{0}}_{3\times 3}& {\mathit{C}}_b^n\\ {\mathbf{0}}_{3\times 3}& {\mathit{F}}_{32}& {\mathit{F}}_{33}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\\ {\mathbf{0}}_{6\times 15}\end{array}\right]$$

(18)

where, $\left({\mathit{f}}_{sf}^n\times \right)$ denotes the antisymmetric matrix; ${\mathsf{\omega }}_{in}^n={\mathsf{\omega }}_{ie}^n+{\mathsf{\omega }}_{en}^n$ denotes the rotation of the $n$ system with respect to the $i$ system; ${\mathsf{\omega }}_{ie}^n={\left[\begin{array}{ccc}\mathbf{0}& {\omega }_{ie}^{}\cos L& {\omega }_{ie}^{}\sin L\end{array}\right]}^{\mathrm{T}}$ denotes the projection of the angular velocity of the Earth’s rotation under the $n$ system; ${\omega }_{ie}^{}$ denotes the angular velocity of the Earth’s rotation; $L$ denotes the latitude where the train is situated; ${\mathsf{\omega }}_{en}^n={\left[\begin{array}{ccc}-v_N^n/(R_M+h)& v_E^n/(R_N+h)& v_E^n\tan L/(R_N+h)\end{array}\right]}^{\mathrm{T}}$ denotes the angular velocity of the rotation of the navigation system; ${\mathit{v}}^n={\left[\begin{array}{ccc}{v}_E^n& v_N^n& v_U^n\end{array}\right]}^{\mathrm{T}}$ denotes the projection of the speed of the $n$ system; $v_E^n$, $v_N^n$, and $v_N^n$ denote the easterly speed, the northerly speed, and the celestial speed, respectively; $R_M$ denotes the radius of curvature of the meridian circle; $R_N$ denotes the radius of curvature of the prime vertical circle; and $h$ denotes the altitude. In Equation (18), we define details in ${\mathit{F}}_{15\times 15}$.

$$\begin{array}{l}\begin{array}{l}{\mathit{M}}_1=\left[\begin{array}{ccc}0& 0& 0\\ -{\omega }_{ie}\sin L& 0& 0\\ {\omega }_{ie}\cos L& 0& 0\end{array}\right]\hfill \end{array}\begin{array}{l}{\mathit{M}}_2=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h}& 0\\ \frac{1}{R_N+h}& 0& 0\\ \frac{\tan L}{R_N+h}& 0& 0\end{array}\right]\hfill \end{array}\\ \begin{array}{l}{\mathit{M}}_3=\left[\begin{array}{ccc}0& 0& \frac{v_N}{{\left(R_N+h\right)}^2}\\ 0& 0& -\frac{v_E}{{\left(R_N+h\right)}^2}\\ \frac{v_E{\sec }^{2}L}{R_N+h}& 0& -\frac{v_E\tan L}{{\left(R_N+h\right)}^2}\end{array}\right]\hfill \end{array},{\mathit{F}}_{32}=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{1}{(R_N+h)\cos L}& 0& 0\\ 0& 0& 1\end{array}\right]\\ \begin{array}{l}\begin{array}{l}{\mathit{F}}_{33}=\left[\begin{array}{ccc}0& 0& -\frac{v_N}{{\left(R_N+h\right)}^2}\\ \frac{v_E\sec L\tan L}{R_N+h}& 0& -\frac{v_E\sec L}{{\left(R_N+h\right)}^2}\\ 0& 0& 0\end{array}\right]\hfill \end{array}\{\begin{array}{c}{\mathit{F}}_{12}={\mathit{M}}_2\\ {\mathit{F}}_{13}={\mathit{M}}_1+{\mathit{M}}_3\\ {\mathit{F}}_{22}=-\left[\left({\mathit{v}}^n\times \right){\mathit{F}}_{12}+\left(2{\mathsf{\omega }}_{ie}^n+{\mathsf{\omega }}_{en}^n\right)\times \right]\\ {\mathit{F}}_{23}=\left({\mathit{v}}^n\times \right)\left(2{\mathit{M}}_1+{\mathit{M}}_3\right)\end{array}\hfill \end{array}\end{array}$$

(19)

4. Results and Discussion

4.1. Experiment Description

In the field experiment, onboard measuring sensors were used to collect data from Oranienburg to Frankfurt, France. The environment is urban and open country, as de-pictured in Figure 7. Figure 8 shows the German ATL train used for data acquisition. The test train operated in Germany is a diesel-driven ICE test train, called Advanced TrainLab (ATL). The train consists of four segments, two traction units and two mid-segments. The diesel–electric traction unit permits it to run at a maximum speed of 200 km/h as shown in Figure 9. The ATL has a specific mounting rack for the flexible installation of perception and localization sensors. The inertial sensor used was IMAR’s iMAR iNAT-RQT-4001-3, as shown on the left in Figure 10, with the parameters and performance shown in

Table 1. INS iMAR iNAT-RQT-4001-3 parameters.

Sensors		Contents	Parameter Settings
IMU	Gyroscope	Angular rate bias stability	0.004°/h
		Angular rate range	±400°/s
		Random walk	<0.005 deg/sqrt (h)
	Accelerometer	Accelerometer bias stability	0.05 mg
		Accelerometer range	±20 g
		Random walk	<12 μg/sqrt (Hz)

. The OD uses the DF16 series photoelectric speed sensor, as shown on the right in Figure 10. Its speed range is 0 to 1500 rpm (round per minute) or 0 to 3000 rpm. It is a piece of high-precision speed- and distance-measuring equipment based on the photoelectric effect from the German company DEUTA, which can be used for the systematic detection of a locomotive’s running direction, traveling speed, acceleration, idling, and skidding. It is characterized by high testing accuracy, long service life, robustness, shock resistance, high reliability, and high pulse resolution.

During the experiment, the train’s trajectory is shown in Figure 8, and the train’s speed profile was captured using an OD as shown in Figure 9. The train wants to remain stationary for 100 s, and then because the train needs to keep turning, the train will keep accelerating and decelerating, and finally the train keeps a uniform speed of 45 m/s. We utilized IMAR’s high-precision iNAT-RQT-4001-3 to obtain IMU measurement data and the German company’s DEUTA DF16 series photoelectric speed sensor to obtain high-precision speed and mile data. These data were stored first, and we used post-processing to analyze and validate our proposed algorithm according to Section 3.

4.2. Comparative Performance Analysis

Using the IMU and OD data collected by the train’s onboard equipment as well as the digital track map, we validated the proposed SINS/OD algorithm assisted by digital track map matching. First, the combined SINS/OD navigation algorithm presented in Section 3 was utilized for OD-assisted inertial navigation solving, because the initial attitude needed to be determined using the initial alignment algorithm prior to the combined navigation. The initial alignment was not the focus in this paper, so the initial attitude solution provided by the truth value was used. Because the train was strictly constrained on the track, when assisted by the forward speed of the OD, we simultaneously made a nonholonomic constraint (NHC): the eastward and upward speeds of the train were set to zero in order to obtain more accurate solving results. Secondly, because the attitudes used in the SINS/OD combination process were derived from the inertial guidance solving results, the SINS attitudes were slowly dispersed due to the lack of absolute position correction, and in addition, due to wheel diameter’s wear and tear, thermal expansion and contraction, etc., there were errors in the OD scale factor that resulted in the dispersion of the navigation position of the SINS/OD combination navigation results with the increase in navigation mileage and navigation time. Therefore, the SINS/OD combination navigation results were corrected by using the digital track map-matching algorithm mentioned in Section 3.

The results of the OD-assisted inertial guidance trajectory and the ground truth superimposed results are shown in Figure 11. As can be seen from the figure, after a big sharp curve, the SINS/OD-solved trajectories ceased to be overlapped and then began to slowly diverge at the end of the navigation 80 km, and after 59 min, the error in the east direction reached 5998.65 m, and the error in the north direction reached 3407.05 m, and the detailed results are shown in

Table 2. Statistical results of SINS/DO solving and SINS/OD/MM solving for onboard train tests.

Distance (km)	SINS/OD North Position Error (m)	SINS/OD East Position Error (m)	SINS/OD/MM North Position Error (m)	SINS/OD/MM East Position Error (m)
5	86.55	13.55	−3.22	0.91
10	301.15	18.15	−5.95	0.05
15	411.25	452.40	−5.42	5.48
20	663.70	673.50	−6.65	5.46
25	976.05	921.65	−6.66	5.18
30	1366.75	1152.50	−7.20	1.61
35	1833.05	1249.45	−5.72	2.93
40	2226.80	1554.95	−0.13	3.01
45	2310.40	2390.90	0.15	−0.63
50	2624.35	2811.90	5.07	−2.64
55	2826.25	3404.05	0.67	−10.72
60	2899.32	4120.80	5.34	−13.35
65	3110.30	4696.25	6.91	−17.24
70	3215.46	4948.55	−3.22	0.918
75	3326.50	5282.60	8.39	−21.24
80	3407.05	5998.65	3.13	−23.82

.

The latitude and longitude of the SINS/OD combination and the latitude and longitude provided by the ground truth were superimposed as shown in Figure 12, and the result of plotting the east-to-north position errors of the combined navigation is shown in Figure 13, which shows that the train’s position error is constantly dispersing without positional assistance and the dispersion speed is becoming faster and faster.

When the position obtained from map matching was used to correct SINS/OD, the position obtained from SINS/OD/MM converged significantly, as shown in Figure 14. As shown in the heading attitude angle in Figure 15, the train makes a big turn at 500 s, at which time the northward position error turns from negative to positive, and thereafter, the position error stays within 10 m until 2000 s, which is a significant improvement compared to the position error of SINS/OD that was dispersed all the time, illustrating the validity of the proposed SINS/OD assisted by the digital map of the track. Combined with Figure 9 and Figure 15, after 2500 s, the train’s movement state is close to being in a uniform linear state, and at this time, the position error is slowly increasing, which is mainly due to the fact that the projected map-matching method can well constrain the lateral error of the train, and the longitudinal error will still be affected by the cumulative effect of the error such as the OD scale factor. And after the train is in a uniform straight-line state, according to the observability theory analysis results, the zero heading deviation was still not observed; at this time, the attitude error will be further transferred to the position error of SINS/OD. However, using the map-matching position proposed in this paper to significantly reduce the speed of the position error dispersion, the eastward and northward position errors at the end of navigation are −23.82 m and 3.13 m, respectively, and the detailed results are shown in Table 2.

5. Conclusions

In this paper, the contribution and novelty are that we propose a novel map-assisted SINS/OD train location system that is assisted by a digital track map, an SINS/OD navigation that is built on OD velocity in the inner circle. Then, we utilized a map-matching algorithm that takes into account vertical projection and heading weighting. Once we obtained matching results from the outer circle, we replaced the positions acquired from SINS/OD with the matching positions for a Kalman filter measurement update. The data processing results from comparisons between SINS/OD and SINS/OD/MM show that our digital track map-assisted SINS/OD algorithm significantly reduces train position error accumulation. The algorithm’s performance is validated through field testing. The position error after nearly 80 km of navigation is 24 m, with relative mileage accuracy equal to or less than 0.03% DT.

In future studies, low-accuracy IMUs can be used to validate the algorithms in this paper in order to obtain the best balance between optimal navigation accuracy and IMU cost.