A Track Geometry Measuring System Based on Multibody Kinematics, Inertial Sensors and Computer Vision

Abstract

This paper describes the kinematics used for the calculation of track geometric irregularities of a new Track Geometry Measuring System (TGMS) to be installed in railway vehicles. The TGMS includes a computer for data acquisition and process, a set of sensors including an inertial measuring unit (IMU, 3D gyroscope and 3D accelerometer), two video cameras and an encoder. The kinematic description, that is borrowed from the multibody dynamics analysis of railway vehicles used in computer simulation codes, is used to calculate the relative motion between the vehicle and the track, and also for the computer vision system and its calibration. The multibody framework is thus used to find the formulas that are needed to calculate the track irregularities (gauge, cross-level, alignment and vertical profile) as a function of sensor data. The TGMS has been experimentally tested in a 1:10 scaled vehicle and track specifically designed for this investigation. The geometric irregularities of a 90 m-scale track have been measured with an alternative and accurate method and the results are compared with the results of the TGMS. Results show a good agreement between both methods of calculation of the geometric irregularities.

1. Introduction

Measurement of track geometry irregularities is a fundamental task in railway track maintenance with immense economic significance worldwide. Geometric irregularity is the most important track feature affecting safety and comfort of the rail transport. There are two different equipment that rail administrations and infrastructure managers use for track irregularity measurement: man-driven rail track trolleys (RTT) and track recording vehicles (TRV), also called laboratory vehicles or inspection vehicles. There are many companies that manufacture both types of equipment. The technology behind the RTT is simple and accurate. The relative irregularities, this is, gauge and cross-level are measured with a distance sensor, like an LVDT, and an inclinometer, respectively. The absolute irregularities, alignment and longitudinal profile require and absolute positioning system, like a total station or a very accurate GNSS. The technology used in TRV is more varied. Essentially, there are two technologies [1]: versine measurement systems (VMS), also called chord-based measurement systems and inertial measurement systems (IMS) or optical-inertial measurement systems. In addition to the sensors used, the main difference between these two methods is on the reference kinematics. The VMS use a physical reference, the chord, that can be a solid bar or a laser beam, while the IMS use an inertial reference frame or, in other words, the frame does not physically exist. The existence of the physical reference is an important advantage of the VMS that improves its accuracy. The main drawback of the VMS is that only irregularities with wave-lengths shorter than the length of the physical reference can be measured. A purely inertial IMS, as those based on accelerometer measurements, is based on the premise that the measuring device follows a trajectory that is parallel to one of the rails or the track centerline. This is in practice very difficult to obtain, or even impossible, if one wants to get the four irregularities with a single device. However, the most serious problem of the IMS is not that but the need to integrate the sensor signals in time to get the irregularities. The resulting accuracy depends very much on the filtering and other signal processing techniques that are needed to avoid the signal drift due to the integration.

The scientific foundation of the VMS is based on simple kinematics that only requires the measurements of distance sensors. The main challenge is to find the transfer function between the chord measurements and the vertical and lateral track irregularities [1,2]. The kinematics of the IMS is much more involved, due to the use of the inertial frame, as mentioned previously, and the use of different sensors (distance sensors, optical and inertial) requiring sensor fusion algorithms. To the authors’ best knowledge, the description of the TRV used in the early eighties in British Rail by Lewis [3] remains the most complete description of the kinematics and signal processing behind the track geometry measurement with IMS. This paper tries to fill this gap. In a paper by Escalona [4], the calculation of the track vertical geometry using inertial sensors mounted on an un-suspended bogie was fully described, including analytical relationships, transfer functions, sensor fusion algorithms and signal processing. However, the method was valid only for the vertical geometry and the results were not experimentally validated.

Despite the existence of different measuring equipment, there is a need for improvement of the technology in track geometry inspection. RRT are much cheaper than TRV (small rail administrations cannot afford TRV) but much slower also. As a result, the frequency of track geometry inspection is not as high as desired. Due to this reason, there is an increasing tendency in the railway industry and research teams to developed track geometry measuring systems (TGMS) to be installed in in-service vehicles. That way, the frequency of the inspection is increased, the vehicle response to the irregularity can be measured simultaneously, and the time-evolution of the irregularities can be followed closely. However, what the sensors can get in an in-service vehicle, in general, is not a measurement of the irregularity but an estimation. There are two approaches: purely kinematic or model-based methods. Weston et al. published a set of papers [5,6,7] showing the measurement of vertical and lateral track irregularities using accelerometers and gyroscopes mounted in the bogie frame and a very simple kinematic model of the vehicle motion. Although these papers do not show results in frequency domain, the measured irregularities could only be relatively accurate for low frequencies, because the bogie frame does not follow the high frequency irregularities. Lee et al. [8] presented a Kalman filter data fusion approach based on accelerometer signals mounted on the body frame and the axle-box. Tsai et al. [9,10] presented a fast inspection technique based on the Hilbert–Huang Transform, that is a very useful time–frequency analysis technique, applied to the signal of an accelerometer mounted on the axle-box of an in-service car. Both methods that are based on axle-box-mounted accelerometers are only valid to measure vertical track irregularities. Tsumashima et al. [11] estimated the vertical track geometry using the accelerations measured in the car-body of the Japanese Shinkansen using a Kalman filter based on a very basic car model. In a more recent work, Tsumashima [12] has develop a method to detect and isolate track faults that are later classified using machine learning techniques. Tsumashima and Hirose [13] have applied the time–frequency Hilbert–Huang Transform method to identify track faults using the measured car-body acceleration as the input.

This paper describes a TGMS based on inertial sensors and computer vision that can be installed in in-service vehicles. The 3D kinematics used for the geometry measurements considers a general track design geometry, and the assumed kinematic approximations and simplifications are fully described. As a result of the background of the authors in on the development of multibody dynamics models for railway simulations [14,15,16], the presented kinematic equations follow the multibody formalism. The kinematic equations used for the computer vision are adapted to this formalism. The sensor fusion algorithms and signal processing tools used to find the track irregularities are explained also. The presented TGMS has been built, tested and validated in an experimental scale track facility. Compared with the existing IMS methods that are implemented in the industry, the contribution of the method presented in this paper lies in the description of the methodology and the experimental validation. To the authors’ best knowledge, the algorithms used to turn the sensors data into measured irregularities have not been described in the scientific literature, or they are described using an over-simplified track geometry, for example, assuming that the track is straight. This information is not provided by the companies that work in the important business of track inspection. In addition, an experimental proof of the accuracy of the equipment used for this task in not generally provided by these companies. This paper provides a full description of the signal processing that can be used to turn the sensors data into track irregularities. As shown below, the kinematic analysis needed to this end is not straightforward. This paper also provides an experimental validation of the measured irregularities. Authors do not claim that the presented method is more accurate or more efficient than existing methods, simply because the data are not accessible to compare.

This paper is organized as follows: Section 2 describes the sensors needed and the main features of the TGMS. Section 3 explains the kinematics of the irregular track and the rail vehicles based on the multibody dynamics formalism. Section 4 applies this formalism to the kinematics of the computer vision with the pin-hole camera model. Section 5 explains the method that is used to find the relative position of the rail cross-sections with respect to the moving vehicle using the computer vision data. Section 6 shows the kinematic relationships that are used to find the four track irregularities: gauge variation, cross-level, alignment and vertical profile, as a function of the sensors data. Section 7 describes a Kalman filter that is used to calculate the TGMS relative orientation with respect to the track. Section 8 describes an odometry algorithm that is used to locate at any instant the vehicle along the track. Section 9 explains the method that is used to find the relative trajectory of the TGMS with respect to the track centerline. The content of Section 5, Section 6, Section 7, Section 8 and Section 9 includes all the calculations needed to obtain the track irregularities out of the sensors data. Finally, Section 10 shows the experimental setup that has been used to build and validate the TGMS system and Section 11 compares the measured irregularities with accurate reference values. Summary and conclusions are given in Section 12.

2. Description of the TGMS

The TGMS sketched in Figure 1 (only right-side equipment in shown here) comprises:

• Two video cameras.
• Two laser line-projectors.
• An IMU.
• A rotary encoder.

It is important that the cameras, lasers and IMU are installed in a solid that can be considered as a rigid body when moving with the vehicle. The lasers and cameras must be equipped with orientation mechanisms that can be fully locked when the TGMS is working. The laser projectors draw red lines (when using a red laser) in the rail-heads (one on the left, one on the right) that are filmed by the video cameras. The information provided by the position and orientation of the red lines in the rails, together with the acceleration and angular velocity acquired with the IMU, are used to find the track geometry irregularities.

The main features of the proposed system are:

• It is capable to measure track alignment, vertical profile, cross-level, gauge, twist and rail-head profile using non-contact technology.
• It can be installed in in-service vehicles. It is compact and low cost. Provided that the equipment sees the rail heads when the vehicle is moving, it can be installed in any body of the vehicle: at the wheelsets level, above primary suspension (bogie frame) or above the secondary suspension (car body).

Measuring devices must be selected taking into account the dusty environment under the train, affecting mainly the computer vision system, and the high level of accelerations that they will be subjected to. Protecting the video cameras and laser projectors may be solved, in part, locating them in a closed box with a polycarbonate window that must be cleaned frequently. Regarding the high level of accelerations, their expected value depends on the solid where the equipment is installed. The most critical body would be the axle-box (wheelset level) where the vertical accelerations can reach 100 g in high velocity trains. Reducing the possibility of damaging the measuring devices due to excessive accelerations is achieved if the equipment is installed at the car-body level.

3. Kinematics of the Irregular Track and the Railway Vehicle

This section includes the kinematic description of the rail geometry as a combination of a design geometry and the irregularities, and the kinematic description of an arbitrary body moving along the track, like the TGMS. Before presenting the kinematics, the different frames that are used and the nomenclature used to describe vectors, matrices and their components are described.

3.1. Frames of Reference

As shown in Figure 2 and Figure 3, four different frames are used in railroad kinematics:

• The inertial and global frame (GF) $<X,Y,Z>$. It is a frame fixed in space.
• The track frame (TF) $<X^t,Y^t,Z^t>$. It is not a single frame but a field defined for each value of the arc-length coordinate along the track s. The position ${\mathbf{R}}^t\left(s\right)$ and orientation matrix ${\mathbf{A}}^t\left(s\right)$ of the TF with respect to the GF are functions of an arc-length coordinate s along the center line of the design track (without irregularities). These functions are implemented computationally in a track preprocessor.
• The body frame (BF) $<X^i,Y^i,Z^i>$ of each body i. It is a frame rigidly attached to the body. In this document, the body i is the TGMS. The body frame of the TGMS is denoted as $<X^{tgms},Y^{tgms},Z^{tgms}>$
• The rail profile frames. Left rail-profile frame (LRP), $<X^{lrp},Y^{lrp},Z^{lrp}>$, and right-profile frame (RRP), $<X^{rrp},Y^{rrp},Z^{rrp}>$. These frames are not unique frames but fields defined for each value of the arc-length coordinate along the track s. These frames are rigidly attached to the rail-heads.

The definition of the TF is such that the $X^t$ axis is tangent to the track design centerline, the $Y^t$ axis is perpendicular to $X^t$ and connects the origin $O^{lrp}$ of the LRP and the origin $O^{rrp}$ of the RRP in the design track geometry (with no track irregularities) and the $Z^t$ axis is perpendicular to both $X^t$ and $Y^t$. Therefore, the TF is not the Frenet frame of the design track centerline. Each body i moving along the track has an associated TF at each instant of time. Its position and orientation can be obtained substituting the position of the body along the track, $s^i\left(t\right)$, in the functions ${\mathbf{R}}^t\left(s\right)$ and ${\mathbf{A}}^t\left(s\right)$ that are explained in next section. More details about the rail kinematics and the nomenclature used in this work for vectors and rotation matrices can be found in [17].

3.2. Kinematics of the Design Track Centerline

Track geometry is the superposition of the design geometry and the irregularities. The components of the absolute position vector of an arbitrary point on the design track centerline with respect to an inertial and global frame is a function of the arc-length s, as follows:

$${\mathbf{R}}^t\left(s\right)=\left[\begin{array}{c}{R}_x^t\left(s\right)\\ R_y^t\left(s\right)\\ R_z^t\left(s\right)\end{array}\right]$$

(1)

where ${\mathbf{R}}^t$ contains the components of vector ${\overrightarrow{R}}^t$ shown in Figure 2. The geometry of the track centerline 3D-curve is defined by the horizontal profile and the vertical profile. Both profiles are defined in the rail industry using sections of variable length. Points between two sections are called vertices. Horizontal profile vertices do not necessary coincide with vertical profile vertices. Horizontal profile includes three types of sections: tangent (straight), curve (circular) and transitions (clothoid). Vertical profile includes two types of sections: constant-slope (straight) and transitions (cubic).

At each track section, the track centerline geometry is characterized by the following geometric values:

• Horizontal curvature: ${\rho }_h$.
• Vertical curvature: ${\rho }_v$.
• Twist curvature: ${\rho }_{tw}$.
• Spatial-derivative of horizontal curvature: ${{\rho }_h}^{\prime }$.
• Vertical slope: ${\alpha }_v$.

Figure 2 shows the TF $<X^t,Y^t,Z^t>$ associated with the track centerline at each value of s. The orientation of the TF with respect to a GF can be measured with the Euler angles ${\psi }^t$ ($azimuth$ or $heading$ angle), ${\theta }^t$ (vertical slope, positive when downwards in the forward direction) and ${\phi }^t$ ($cant$ or $superelevation$$angle$). The rotation matrix from the TF to the GF is given by:

$${\mathbf{A}}^t\left(s\right)=\left[\begin{array}{ccc}\mathrm{c}{\theta }^t\mathrm{c}{\psi }^t& \mathrm{s}{\phi }^t\mathrm{s}{\theta }^t\mathrm{c}{\psi }^t-\mathrm{c}{\phi }^t\mathrm{s}{\psi }^t& \mathrm{s}{\phi }^t\mathrm{s}{\psi }^t+\mathrm{c}{\phi }^t\mathrm{s}{\theta }^t\mathrm{c}{\psi }^t\\ \mathrm{c}{\theta }^t\mathrm{s}{\psi }^t& \mathrm{c}{\phi }^t\mathrm{c}{\psi }^t+\mathrm{s}{\phi }^t\mathrm{s}{\theta }^t\mathrm{s}{\psi }^t& \mathrm{c}{\phi }^t\mathrm{s}{\theta }^t\mathrm{s}{\psi }^t-\mathrm{s}{\phi }^t\mathrm{c}{\psi }^t\\ -\mathrm{s}{\theta }^t& \mathrm{s}{\phi }^t\mathrm{c}{\theta }^t& \mathrm{c}{\phi }^t\mathrm{c}{\theta }^t\end{array}\right]$$

(2)

The azimuth ${\psi }^t$ can have an arbitrary value, however, the slope ${\theta }^t$ and cant ${\phi }^t$ angles can be considered as small angles, such that the rotation matrix from the TF to the GF can be approximated to:

$${\mathbf{A}}^t\left(s\right)\simeq \left[\begin{array}{ccc}\mathrm{c}{\psi }^t& -\mathrm{s}{\psi }^t& {\phi }^t\mathrm{s}{\psi }^t+{\theta }^t\mathrm{c}{\psi }^t\\ \mathrm{s}{\psi }^t& \mathrm{c}{\psi }^t& {\theta }^t\mathrm{s}{\psi }^t-{\phi }^t\mathrm{c}{\psi }^t\\ -{\theta }^t& {\phi }^t& 1\end{array}\right]$$

(3)

An ideal body that moves along the track, taking the same orientation as the track frame with a forward velocity V and a forward acceleration $\dot{V}$, has the following absolute velocity and acceleration:

$${\overline{\dot{\mathbf{R}}}}^{\mathit{t}}=\left[\begin{array}{c}V\\ 0\\ 0\end{array}\right],{\overline{\ddot{\mathbf{R}}}}^{\mathit{t}}=\left[\begin{array}{c}\dot{V}\\ {\rho }_h{V}^2\\ -{\rho }_v{V}^2\end{array}\right]$$

(4)

Similarly, the absolute angular velocity and the absolute angular acceleration of that body are given by:

$${\overline{\omega }}^t=\left[\begin{array}{c}{\rho }_{tw}V\\ {\rho }_{v}V\\ {\rho }_{h}V\end{array}\right],{\overline{\alpha }}^t=\left[\begin{array}{c}{\rho }_{tw}\dot{V}\\ {\rho }_v\dot{V}\\ {\rho }_h\dot{V}+{{\rho }^{\prime }}_h{V}^2\end{array}\right]$$

(5)

3.3. Kinematics of the Irregular Track

Figure 3 shows the displacement of the rail heads due to irregularity in a cross-section of the track ($Y^t-Z^t$ plane). The irregularity vectors ${\overrightarrow{r}}^{lir}$ ($lir$, left rail $ir$regularity) and ${\overrightarrow{r}}^{rir}$ ($rir$, right rail $ir$regularity) describe the displacement of the rail centerlines with respect to their design positions. The components of these vectors in the TF are functions of s, given by:

$${\overline{\mathbf{r}}}^{lir}=\left[\begin{array}{c}0\\ y^{lir}\\ z^{lir}\end{array}\right],{\overline{\mathbf{r}}}^{rir}=\left[\begin{array}{c}0\\ y^{rir}\\ z^{rir}\end{array}\right]$$

(6)

In the railway industry, the following four combinations of the rail head centerlines irregularities are measured:

• Alignment: $\hspace{1em}\hspace{1em}\hspace{1em} {\xi }_{al}=(y^{lir}+y^{rir})/2$
• Vertical profile: $\hspace{1em}\hspace{1em}{\xi }_{vp}=(z^{lir}+z^{rir})/2$
• Gauge variation: $\hspace{1em} {\xi }_{gv}=y^{lir}-y^{rir}$
• Cross level: $\hspace{1em}\hspace{1em}\hspace{1em} {\xi }_{cl}=z^{lir}-z^{rir}$

The orientation of the rail head frames with respect to the TF is given by the following rotation matrices:

$$\begin{array}{c}{\mathbf{A}}^{t,lrp}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left(\beta +\delta \right)& -sin\left(\beta +\delta \right)\\ 0& sin\left(\beta +\delta \right)& cos\left(\beta +\delta \right)\end{array}\right],\hfill \\ {\mathbf{A}}^{t,rrp}=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\left(-\beta +\delta \right)& -sin\left(-\beta +\delta \right)\\ 0& sin\left(-\beta +\delta \right)& cos\left(-\beta +\delta \right)\end{array}\right],\hfill \end{array}$$

(7)

where $\beta$ is the orientation angle of the rail profiles, $\delta =\left(z^{lir}-z^{rir}\right)/\phantom{\left(z^{lir}-z^{rir}\right)2L_r}2L_r$ is the linearized rotation angle due to the irregularity, and $L_r$ is the distance from the track centerline to the rail profile frames. These angles and distance can be observed in Figure 3.

The absolute position vectors of two points, P and Q, defined in the right and left rail heads, respectively, are given by:

$$\begin{array}{c}{\mathbf{R}}_P^{rrp}={\mathbf{R}}^t+{\mathbf{A}}^t\left({\overline{\mathbf{r}}}^{rrp}+{\overline{\mathbf{r}}}^{rir}+{\mathbf{A}}^{t,rrp}{\widehat{\mathbf{u}}}_P^{rrp}\right)\hfill \\ \\ {\mathbf{R}}_Q^{lrp}={\mathbf{R}}^t+{\mathbf{A}}^t\left({\overline{\mathbf{r}}}^{lrp}+{\overline{\mathbf{r}}}^{lir}+{\mathbf{A}}^{t,lrp}{\widehat{\mathbf{u}}}_Q^{lrp}\right)\hfill \end{array}$$

(8)

where ${\widehat{\mathbf{u}}}_P^{rrp}$ and ${\widehat{\mathbf{u}}}_Q^{lrp}$ contain the components of the position vector of points P and Q in the rail head profiles as shown in Figure 3. These vectors are parameterized following the rail head profile geometry:

$${\widehat{\mathbf{u}}}_P^{rrp}=\left[\begin{array}{c}0\\ s_2^{rr}\\ h^r\left(s_2^{rr}\right)\end{array}\right],{\widehat{\mathbf{u}}}_Q^{lrp}=\left[\begin{array}{c}0\\ s_2^{lr}\\ h^r\left(s_2^{lr}\right)\end{array}\right]$$

(9)

where $lr$ and $rr$ stand for “left rail” and “right rail”, and $h^r$ is the function that defines the rail head profile.

3.4. Kinematics of a Body Moving along the Track

The coordinates used to describe the position and orientation of an arbitrary body i, as shown in Figure 2, or the TGMS shown in Figure 1, moving along the track are:

$${\mathbf{q}}^i={\left[\begin{array}{cccccc}{s}^i& r_y^i& r_z^i& {\phi }^i& {\theta }^i& {\psi }^i\end{array}\right]}^T$$

(10)

where $s^i$ is the arc-length along the track of the position of the body, $r_y^i$ and $r_z^i$ are the non-zero components of the position vector ${\overrightarrow{r}}^i$ of the BF with respect to the TF, this is ${\overline{\mathbf{r}}}^i={\left[\begin{array}{ccc}0& r_y^i& r_z^i\end{array}\right]}^T$, and ${\phi }^i,{\theta }^i\mathrm{and}{\psi }^i$ are three Euler angles (roll, pitch and yaw, respectively) that define the orientation of the BF with respect to the TF. Note that, out of the six coordinates used for the kinematic description of a body, only the arc-length coordinate $s^i$ is an absolute coordinate, being the other 5 track-relative coordinates.

The three Euler angles are assumed to be small, such that the following kinematic linearization is used:

$${\mathbf{A}}^{t,i}\simeq \left[\begin{array}{ccc}1& -{\psi }^i& {\theta }^i\\ {\psi }^i& 1& -{\phi }^i\\ -{\theta }^i& {\phi }^i& 1\end{array}\right]$$

(11)

The absolute position vector of point P that belongs to body i, as shown in Figure 2, is given by:

$${\mathbf{R}}_P^i={\mathbf{R}}^t+{\mathbf{A}}^t\left({\overline{\mathbf{r}}}^i+{\mathbf{A}}^{t,i}{\widehat{\mathbf{u}}}_P^i\right)$$

(12)

Using basic rigid body kinematics (see details in [17]), the absolute velocity and acceleration of point P are given by:

$${\overline{\dot{\mathbf{R}}}}_P^i={\overline{\dot{\mathbf{R}}}}^t+{\dot{\overline{\mathbf{r}}}}^{\mathit{i}}+{\tilde{\overline{\omega }}}^{\mathit{t}}{\overline{\mathbf{r}}}^{\mathit{i}}+{\mathbf{A}}^{\mathit{t},\mathit{i}}\left({\tilde{\widehat{\omega }}}^{\mathit{i}}{\widehat{\mathbf{u}}}_{\mathit{P}}^{\mathit{i}}\right)$$

(13)

$${\overline{\ddot{\mathbf{R}}}}_P^i={\overline{\ddot{\mathbf{R}}}}^t+{\ddot{\overline{\mathbf{r}}}}^i+\left({\tilde{\overline{\alpha }}}^t+{\tilde{\overline{\omega }}}^t{\tilde{\overline{\omega }}}^t\right){\overline{\mathbf{r}}}^i+2{\tilde{\overline{\omega }}}^t{\dot{\overline{\mathbf{r}}}}^{\mathit{i}}+{\mathbf{A}}^{\mathit{t},\mathit{i}}\left({\tilde{\widehat{\alpha }}}^{\mathit{i}}+{\tilde{\widehat{\omega }}}^{\mathit{i}}{\tilde{\widehat{\omega }}}^{\mathit{i}}\right){\widehat{\mathbf{u}}}_{\mathit{P}}^{\mathit{i}},$$

(14)

where these vectors are projected to the track frame. In order to compute Equations (13) and (14), the orientation matrices, angular velocities and angular accelerations of the different frames need to be computed as a function of the generalized coordinates and velocities. The angular velocity ${\overline{\omega }}^t$ and acceleration ${\overline{\alpha }}^t$ vectors of the TF are given in Equation (5). The absolute angular velocity of body i is obtained as:

$${\widehat{\omega }}^i={\widehat{\omega }}^t+{\widehat{\omega }}^{t,i}={\left({\mathbf{A}}^{t,i}\right)}^T{\overline{\omega }}^t+{\widehat{\omega }}^{t,i}$$

(15)

The relative angular velocity of body i with respect to the TF, under the small-angles assumption, is given by:

$${\widehat{\omega }}^{t,i}=\left[\begin{array}{c}{\dot{\phi }}^i\\ {\dot{\theta }}^i\\ {\dot{\psi }}^i\end{array}\right],{\overline{\omega }}^{t,i}={\mathbf{A}}^{t,i}{\widehat{\omega }}^{t,i}=\left[\begin{array}{ccc}1& {\psi }^i& -{\theta }^i\\ -{\psi }^i& 1& {\phi }^i\\ {\theta }^i& -{\phi }^i& 1\end{array}\right]\left[\begin{array}{c}{\dot{\phi }}^i\\ {\dot{\theta }}^i\\ {\dot{\psi }}^i\end{array}\right]$$

(16)

The absolute angular acceleration of body i, ${\widehat{\alpha }}^t$ is simply calculated as the time-derivative of Equation (15).

4. Kinematics of the Computer Vision

Using the pin-hole model of the camera, Figure 4 shows the relation between the position vector ${\overrightarrow{n}}_{P^{\prime }}^{im}$ of an arbitrary point P in the camera frame $<X^{cam},Y^{cam},Z^{cam}>$ (in our problem it can be left cam $lcam$ or right cam $rcam$) and the position vector of the recorded point P’ in the image plane $<X^{im},Y^{im}>$.

Figure 5 shows the location of the camera in the TGMS and the relation between the position vector ${\overrightarrow{v}}_P^{cam}$ of the arbitrary point P in the camera frame and its position vector ${\overrightarrow{u}}_P^{tgms}$ in the TGMS frame.

The components of vectors ${\overrightarrow{n}}_{P^{\prime }}^{im}$ and ${\overrightarrow{u}}_P^{tgms}$ are related through the equation [18,19]:

$$c\left[\begin{array}{c}{\mathbf{n}}_{P^{\prime }}^{im}\\ 1\end{array}\right]={\mathbf{M}}^{int}{\mathbf{M}}^{ext}\left[\begin{array}{c}{\widehat{\mathbf{u}}}_P^{tgms}\\ 1\end{array}\right]$$

(17)

where c is an unknown $scale$$factor$. The matrix product on the right-hand side of this equation is called projection matrix $\mathbf{P}={\mathbf{M}}^{int}{\mathbf{M}}^{ext}$. The column matrix ${\mathbf{n}}_{P^{\prime }}^{im}$ is $2\times 1$ (image is planar), and its components are given in pixel units (dimensionless) while the column matrix ${\widehat{\mathbf{u}}}_P^{tgms}$ is $3\times 1$, and its components are given in meters. Due to these dimensions, the projection matrix P is $3\times 4$. Matrix ${\mathbf{M}}^{int}$ is $3\times 3$ and it is called matrix of intrinsic parameters of the camera, and matrix ${\mathbf{M}}^{ext}$ is $3\times 4$, it is called matrix of extrinsic parameters of the camera, and it is given by:

$${\mathbf{M}}^{ext}=\left[\begin{array}{cc}{\left({\mathbf{A}}^{tgms,cam}\right)}^T& -{\left({\mathbf{A}}^{tgms,cam}\right)}^T{\widehat{\mathbf{u}}}_{cam}^{tgms}\end{array}\right]$$

(18)

Matrices of intrinsic and extrinsic parameters can be experimentally obtained, for example, using the Zhang calibration method [18]. In this investigation, the calibration method is based on Zhang method but adapted to the application at hand. This method is fully detailed in [19].

Just using the 3 scalar equations in Equation (17), the position vector ${\widehat{\mathbf{u}}}_P^{tgms}$ of the point P cannot be obtained using the values of ${\mathbf{n}}_{P^{\prime }}^{im}$ because there are 4 unknowns (three components of ${\widehat{\mathbf{u}}}_P^{tgms}$ and the scale factor c). One exception occurs when the point P moves on a surface whose equation is known in the TGMS frame. This is the case at hand if P belongs to the plane highlighted by the laser projector. In this case, the following system of equations can be solved to find ${\widehat{\mathbf{u}}}_P^{tgms}$:

$$\left\{\begin{array}{c}c\left[\begin{array}{c}{\mathbf{n}}_{P^{\prime }}^{im}\\ 1\end{array}\right]={\mathbf{M}}^{int}{\mathbf{M}}^{ext}\left[\begin{array}{c}{\widehat{\mathbf{u}}}_P^{tgms}\\ 1\end{array}\right]\\ A^{las}{\left[u_P^{tgms}\right]}_x+B^{las}{\left[u_P^{tgms}\right]}_y+C^{las}{\left[u_P^{tgms}\right]}_z+D^{las}=0\end{array}\right.$$

(19)

where $A^{las},B^{las},C^{las}$ and $D^{las}$ are the constants that define the laser plane (left laser, $llas$, or right laser, $rlas$, in our case), and ${\left[u_P^{tgms}\right]}_{k}k=x,y,z$ means the component k of the vector ${\overrightarrow{u}}_P^{tgms}$ in the TGMS frame. The constants that define the laser planes have to be experimentally obtained in the TGMS computer vision-calibration process [19]. Equation (19) is a system of 4 algebraic equations with 4 unknowns that can be used to find vector components ${\widehat{\mathbf{u}}}_P^{tgms}$ using as input data the vector components ${\mathbf{n}}_{P^{\prime }}^{im}$ and the parameters of the cameras and the lasers.

5. Detecting the Rail Cross-Section from a Camera Frame

As a result of the solution of Equation (19) for all highlighted pixels in the image frames, a cloud of points $P_i$ in the right rail and a cloud of points $Q_i$ in the left rail, with position vectors ${\widehat{\mathbf{u}}}_P^{tgms}$ and ${\widehat{\mathbf{u}}}_Q^{tgms}$, respectively, that belong to the rails cross-sections can be identified. In fact, the points do not really belong to cross-sections, just to sections, because the laser planes are not necessarily perpendicular to the rails center line. However, because the relative angles of the TGMS with respect to the TF are very small, the irregularities are also small, and the lasers are set to project the light plane at right angles with respect to the rails, it will be assumed in this paper that the highlighted sections are actually cross-sections.

Figure 6a shows a sketch of the cloud of points and, in dashed line, the theoretical rail-head profile. The theoretical rail-head profiles, when they are new, not worn, have a known geometry that is made of circular and straight segments. An example is the UIC 54 E1 rail-head profile shown in Figure 6b. Detecting the rail cross-section from a camera frame can be solved with the well-developed computer vision algorithms of feature detection or feature tracking [20]. However, in this investigation, because the feature to be detected can be represented analytically, an optimization approach has been developed. The optimization problem consists of finding the position ${\widehat{\mathbf{u}}}_{Orp}^{tgms}$ and angle ${\phi }^{tgms,rp}$ of the rail profile that better fits the cloud of points. This will be the assumed position and orientation of the rail head profile ($lrp$ or $rrp$) in the TGMS frame when the vehicle is moving. The objective function to minimize is the sum of the squares of the distance of the points in the cloud to the theoretical profile curve shown in Figure 6b.

6. Equations for Geometry Measurement

The equations that can be used to measure the track irregularities are easily deduced with the help of Figure 7. In this figure, vectors ${\widehat{\mathbf{u}}}_{Olrp}^{tgms}\mathrm{and}{\widehat{\mathbf{u}}}_{Orrp}^{tgms}$ are output data of the computer vision algorithm explained in previous section. The following equalities can be easily identified with the help of the figure:

$$\begin{array}{c}{\overrightarrow{r}}^{tgms}+{\overrightarrow{u}}_{Olrp}^{tgms}={\overrightarrow{r}}^{lrp}+{\overrightarrow{r}}^{lir}\hfill \\ {\overrightarrow{r}}^{tgms}+{\overrightarrow{u}}_{Orrp}^{tgms}={\overrightarrow{r}}^{rrp}+{\overrightarrow{r}}^{rir}\hfill \end{array}$$

(20)

Subtracting these vector equations, one gets:

$${\overrightarrow{u}}_{Olrp}^{tgms}-{\overrightarrow{u}}_{Orrp}^{tgms}={\overrightarrow{r}}^{lrp}+{\overrightarrow{r}}^{lir}-\left({\overrightarrow{r}}^{rrp}+{\overrightarrow{r}}^{rir}\right)$$

(21)

In this equation, the position vector ${\overrightarrow{r}}^{tgms}$ of the TGMS does not appear. This vector equation can be projected in the TF, as follows:

$${\mathbf{A}}^{t,tgms}\left({\widehat{\mathbf{u}}}_{Olrp}^{tgms}-{\widehat{\mathbf{u}}}_{Orrp}^{tgms}\right)={\overline{\mathbf{r}}}^{lrp}-{\overline{\mathbf{r}}}^{rrp}+{\overline{\mathbf{r}}}^{lir}-{\overline{\mathbf{r}}}^{rir}$$

(22)

Using again the small-angles assumption, the $Y,Z$ components of this equation are given by:

$$\left[\begin{array}{cc}1& -{\phi }^{tgms}\\ {\phi }^{tgms}& 1\end{array}\right]\left[\begin{array}{c}{\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_y-{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_y\\ {\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_z-{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_z\end{array}\right]=\left[\begin{array}{c}2L^r\\ 0\end{array}\right]+\left[\begin{array}{c}{r}_y^{lir}-r_y^{rir}\\ r_z^{lir}-r_z^{rir}\end{array}\right]$$

(23)

where $L^r$ is half the distance between the rail-head profiles without irregularities. In this equation, the result ${\overline{\mathbf{r}}}^{lrp}-{\overline{\mathbf{r}}}^{rrp}={\left[\begin{array}{cc}2L^r& 0\end{array}\right]}^T$ has been used. According to the definition given in Section 3.3, the components of the last column matrix of Equation (23) are the gauge variation ($gv$) and the cross-level ($cl$). Therefore, rearranging Equation (23) yields:

$$\begin{array}{c}{\xi }_{gv}=\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_y-{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_y\right)-{\phi }^{tgms}\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_z-{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_z\right)-2L^r\hfill \\ {\xi }_{cl}={\phi }^{tgms}\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_y-{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_y\right)+\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_z-{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_z\right)\hfill \end{array}$$

(24)

Adding the vector equations Equation (20), one gets:

$$2{\overrightarrow{r}}^{tgms}+{\overrightarrow{u}}_Q^{tgms}+{\overrightarrow{u}}_P^{tgms}={\overrightarrow{r}}^{lir}+{\overrightarrow{r}}^{rir}$$

(25)

where the fact that ${\overrightarrow{r}}^{lrp}+{\overrightarrow{r}}^{rrp}=\overrightarrow{0}$ has been used. Using again the small-angles assumption, the $Y,Z$ components of this equation are given by:

$$2\left[\begin{array}{c}{r}_y^{tgms}\\ r_z^{tgms}\end{array}\right]+\left[\begin{array}{cc}1& -{\phi }^{tgms}\\ {\phi }^{tgms}& 1\end{array}\right]\left[\begin{array}{c}{\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_y+{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_y\\ {\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_z+{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_z\end{array}\right]=\left[\begin{array}{c}{r}_y^{lir}+r_y^{rir}\\ r_z^{lir}+r_z^{rir}\end{array}\right]$$

(26)

According to the definition given in Section 3.3, the components of the last column matrix of Equation (26) are twice the alignment irregularity (${\xi }_{al}$) and twice the vertical profile (${\xi }_{vp}$). Therefore, rearranging Equation (26) yields:

$$\begin{array}{c}{\xi }_{al}=\frac{1}{2}\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_y+{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_y\right)-\frac{{\phi }^{tgms}}{2}\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_z+{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_z\right)+r_y^{tgms}\hfill \\ {\xi }_{vp}=\frac{{\phi }^{tgms}}{2}\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_y+{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_y\right)+\frac{1}{2}\left({\left[{\widehat{u}}_{Olrp}^{tgms}\right]}_z+{\left[{\widehat{u}}_{Orrp}^{tgms}\right]}_z\right)+r_z^{tgms}\hfill \end{array}$$

(27)

Therefore, Equations (24) and (27) can be used to find all track irregularities. The following conclusions are highlighted:

(1)

The calculation of the relative track irregularities (${\xi }_{gv}$ and ${\xi }_{cl}$), as shown in Equation (24), needs as an input the output of the computer vision ${\widehat{\mathbf{u}}}_{Olrp}^{tgms}\mathrm{and}{\widehat{\mathbf{u}}}_{Orrp}^{tgms}$ and the roll angle of the TGMS with respect to the track ${\phi }^{tgms}$.

(2)

The calculation of the absolute track irregularities (${\xi }_{al}$ and ${\xi }_{vp}$), as shown in Equation (27), needs, in addition, the relative trajectory ${\overline{\mathbf{r}}}^{tgms}$ of the TGMS with respect to the TF.

The next section is devoted to the calculation of the TGMS to TF relative orientation, that includes the calculation of ${\phi }^{tgms}$. Section 9 is devoted to the calculation of the relative trajectory ${\overline{\mathbf{r}}}^{tgms}$ of the TGMS with respect to the TF.

7. Measurement of TGMS to TF Relative Orientation

The calculation of the relative rotation of the TGMS with respect to the TF requires the calculation of the absolute orientation of the TGMS and the absolute orientation of the TF. As given in Equations (2) and (3), finding the orientation of the TF just requires the instantaneous value of the arc-length s, because the track design geometry is known. The value of s has to be known accurately. The odometry algorithm explained in the next section has been developed to this end.

The calculation of the absolute orientation of the TGMS is obtained with the IMU sensor. Most IMU sensors come with their own internal algorithm to calculate the orientation angles or quaternions. However, these algorithms are not accurate in the application at hand. They are based on the sensor fusion of the data of the gyroscope, the accelerometer and the magnetometer, as follows:

• The gyroscope provides three signals that are proportional to the components of the angular velocity vector in the sensor frame (the sensor frame is assumed to be parallel to the TGMS frame), as follows:

  $${\omega }^{imu}={\widehat{\omega }}_{abs}^{tgms}$$

  (28)
  These components are non-linearly related to the coordinates that define the TGMS orientation and their time-derivatives, as shown later.
• The accelerometer signals are proportional to the components of the absolute acceleration in the local frame plus the absolute gravity vector field, as follows:

  $${\mathbf{a}}^{imu}={\widehat{\ddot{\mathbf{R}}}}^{tgms}+{\left({\mathbf{A}}^{tgms}\right)}^T{\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$$

  (29)
  This is the absolute acceleration in the sensor frame, plus the gravitational constant g, that is assumed to act in the absolute Z direction.
• The magnetometer signals are proportional to the components of the Earth’s magnetic field in the local frame. This information can be used to find the direction of the Earth’s magnetic north.

In the algorithm developed in this investigation to find the TGMS absolute orientation, the magnetometer’s signals are not used. This is mainly because out of the three Euler angles needed to define the orientation, the yaw angle lacks interest in our application. It is mainly the yaw angle which can be identified with the help of the magnetometer.

Following an Euler’s angle sequence yaw-pitch-roll, as done with the TF to get the rotation matrix given in Equation (2), the absolute angular velocity of the TGMS that appear in Equation (28) can be obtained as follows:

$$\begin{array}{c}{\widehat{\omega }}_{abs}^{tgms}=\left[\begin{array}{c}-sin{\theta }_{abs}^{tgms}\\ cos{\theta }_{abs}^{tgms}sin{\phi }_{abs}^{tgms}\\ cos{\theta }_{abs}^{tgms}cos{\phi }_{abs}^{tgms}\end{array}\right]{\dot{\psi }}_{abs}^{tgms}+\left[\begin{array}{c}0\\ cos{\phi }_{abs}^{tgms}\\ -sin{\phi }_{abs}^{tgms}\end{array}\right]{\dot{\theta }}_{abs}^{tgms}+\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]{\dot{\phi }}_{abs}^{tgms}=\hfill \\ =\left[\begin{array}{ccc}1& 0& -sin{\theta }_{abs}^{tgms}\\ 0& cos{\phi }_{abs}^{tgms}& cos{\theta }_{abs}^{tgms}sin{\phi }_{abs}^{tgms}\\ 0& -sin{\phi }_{abs}^{tgms}& cos{\theta }_{abs}^{tgms}cos{\phi }_{abs}^{tgms}\end{array}\right]\left[\begin{array}{c}{\dot{\phi }}_{abs}^{tgms}\\ {\dot{\theta }}_{abs}^{tgms}\\ {\dot{\psi }}_{abs}^{tgms}\end{array}\right]={\widehat{\mathbf{G}}}^{tgms}{\dot{\mathsf{\Phi }}}_{abs}^{tgms},\hfill \end{array}$$

(30)

where ${\mathsf{\Phi }}_{abs}^{tgms}={\left[\begin{array}{ccc}{\phi }_{abs}^{tgms}& {\theta }_{abs}^{tgms}& {\psi }_{abs}^{tgms}\end{array}\right]}^T$ is the set of absolute angles (with respect to the global frame) of the TGMS. This expression is non-linear in terms of the TGMS absolute angles, but linear in term of their time-derivative. This equation can be inverted to isolate the time-derivatives of the angles, as follows:

$$\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]={\left(\widehat{\mathbf{G}}\right)}^{-1}\widehat{\omega }=\left[\begin{array}{ccc}1& \frac{sin\phi sin\theta }{cos\theta }& \frac{cos\phi sin\theta }{cos\theta }\\ 0& cos\phi & -sin\phi \\ 0& \frac{sin\phi }{cos\theta }& \frac{cos\phi }{cos\theta }\end{array}\right]\left[\begin{array}{c}{\widehat{\omega }}_x\\ {\widehat{\omega }}_y\\ {\widehat{\omega }}_z\end{array}\right]$$

(31)

where, as done in the rest of this section, the superscript $tgms$ and subscript $abs$ have been eliminated in the symbols for simplicity. Using again the small-angles assumption of the roll and pitch angles, the following linearized formulas result:

$$\begin{array}{c}\dot{\phi }={\widehat{\omega }}_x+\frac{sin\phi sin\theta }{cos\theta }{\widehat{\omega }}_y+\frac{cos\phi sin\theta }{cos\theta }{\widehat{\omega }}_z\simeq {\widehat{\omega }}_x+\theta {\widehat{\omega }}_z\hfill \\ \dot{\theta }=cos\phi {\widehat{\omega }}_y-sin\phi {\widehat{\omega }}_z\simeq {\widehat{\omega }}_y-\phi {\widehat{\omega }}_z\hfill \\ \dot{\psi }=\frac{sin\phi }{cos\theta }{\widehat{\omega }}_y+\frac{cos\phi }{cos\theta }{\widehat{\omega }}_z\simeq \phi {\widehat{\omega }}_y+{\widehat{\omega }}_z\hfill \end{array}$$

(32)

For the accelerometer signals, Equation (29) can be linearized as follows:

$${\mathbf{a}}^{imu}\simeq {\widehat{\ddot{\mathbf{R}}}}^{\mathit{tgms}}+\mathit{g}\left[\begin{array}{c}-\theta \\ \phi \\ \mathit{1}\end{array}\right]$$

(33)

where:

$${\widehat{\ddot{\mathbf{R}}}}^{\mathit{tgms}}={\left[{\mathbf{A}}^{\mathit{t},\mathit{tgms}}\right]}^T{\overline{\ddot{\mathbf{R}}}}^{\mathit{tgms}},{\mathbf{A}}^{\mathit{t},\mathit{tgms}}\simeq \left[\begin{array}{ccc}1& -{\psi }^{tgms}& {\theta }^{tgms}\\ {\psi }^{tgms}& 1& -{\phi }^{tgms}\\ -{\theta }^{tgms}& {\phi }^{tgms}& 1\end{array}\right]$$

(34)

Again, because of the small-angles assumption and for the sake of simplicity, the following approximation is used:

$${\widehat{\ddot{\mathbf{R}}}}^{\mathit{tgms}}\simeq {\overline{\ddot{\mathbf{R}}}}^{\mathit{tgms}}$$

(35)

Using the result of Equation (14), the absolute acceleration of the TGMS is given by:

$${\overline{\ddot{\mathbf{R}}}}^{\mathit{tgms}}={\overline{\ddot{\mathbf{R}}}}^{\mathit{t}}+{\ddot{\overline{\mathbf{r}}}}^{\mathit{tgms}}+\left({\tilde{\overline{\alpha }}}^t+{\tilde{\overline{\omega }}}^t{\tilde{\overline{\omega }}}^t\right){\overline{\mathbf{r}}}^{\mathit{tgms}}+2{\tilde{\overline{\omega }}}^t{\dot{\overline{\mathbf{r}}}}^{\mathit{tgms}}$$

(36)

In this equation, the last three terms on the right-hand side are unknown. These are the TGMS to TF relative acceleration, the tangential and centripetal relative accelerations and the Coriolis acceleration, respectively. In order to calculate these terms, the value of the relative position vector ${\overline{\mathbf{r}}}^{\mathit{tgms}}$, velocity ${\dot{\overline{\mathbf{r}}}}^{\mathit{tgms}}$ and acceleration ${\ddot{\overline{\mathbf{r}}}}^{\mathit{tgms}}$ of the TGMS with respect to the TF are needed. As it can be observed in Figure 8, the trajectory followed by the TGMS when the vehicle is moving is a 3D curve that slightly differs with respect to the track centerline. In fact, the difference between these 3D curves is needed to measure the absolute track irregularities. However, in order to find the TGMS to TF relative orientation, it is going to be assumed that the acceleration equals the one of a particle moving along the track centerline, this is:

$${\overline{\ddot{\mathbf{R}}}}^{\mathit{tgms}}\simeq {\overline{\ddot{\mathbf{R}}}}^{\mathit{t}}=\left[\begin{array}{c}\dot{V}\\ {\rho }_h{V}^2\\ -{\rho }_v{V}^2\end{array}\right]$$

(37)

Therefore, the influence of the last three terms in Equation (36) has been neglected.

The sequence of approximations that has been assumed to calculate the acceleration of the TGMS may look rough for some readers. However, it has to be taken into account that in most algorithms used to find orientation from IMU’s signals [21,22] the true acceleration measured by the sensor is neglected in comparison with the gravity term in Equation (33). In the application at hand, this approximation is not accurate and the “compensation” introduced with Equation (36) is much better than nothing.

Finally, the following measurement equation is used to account for the accelerometer signals:

$${\mathbf{a}}_{corr}^{imu}={\mathbf{a}}_{filt}^{imu}-\left[\begin{array}{c}\dot{V}\\ {\rho }_h{V}^2\\ -{\rho }_v{V}^2\end{array}\right]\approx \mathit{g}\left[\begin{array}{c}-\theta \\ \phi \\ \mathit{1}\end{array}\right]$$

(38)

where ${\mathbf{a}}_{corr}^{imu}$ is the “corrected” accelerometer signal and ${\mathbf{a}}_{filt}^{imu}$ is the low-pass filtered acceleration signal. The reason for low-pass filtering the acceleration signals is that the effect of the TGMS to TF relative accelerations that have been neglected in Equation (36) contribute to relatively high-frequencies to the signals. This way their contribution is, in part, filtered out of the signals.

Using the two first gyroscope equations given in Equation (30) and the last two accelerometer equations given in Equation (38), the following linear dynamic system can be formulated in state-space form:

$$\begin{array}{c}\dot{\mathbf{x}}=\left[\begin{array}{c}\dot{\phi }\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}0& {\widehat{\omega }}_z\\ -{\widehat{\omega }}_z& 0\end{array}\right]\left[\begin{array}{c}\phi \\ \theta \end{array}\right]+\left[\begin{array}{c}{\widehat{\omega }}_x\\ {\widehat{\omega }}_y\end{array}\right]=\mathbf{Fx}+\mathbf{u}\hfill \\ \mathbf{z}=\left[\begin{array}{c}{\left(a_{corr}^{imu}\right)}_x\\ {\left(a_{corr}^{imu}\right)}_y\end{array}\right]=\left[\begin{array}{cc}0& -g\\ g& 0\end{array}\right]\left[\begin{array}{c}\phi \\ \theta \end{array}\right]=\mathbf{Hx}\hfill \end{array}$$

(39)

where the state vector $\mathbf{x}={\left[\begin{array}{cc}\phi & \theta \end{array}\right]}^T$ just includes the roll and pitch angles, the state transition matrix $\mathbf{F}$ is not constant but depends on the measured vertical angular velocity, the measurement matrix $\mathbf{H}$ is constant, the measurement vector $\mathbf{z}$ includes the x and y measured-corrected accelerations and the input vector $\mathbf{u}$ includes the x and y measured angular velocities. The linear system shown in (39) is a very simple set of equations that can be used with a standard Kalman filter algorithm to find the TGMS to TF relative angles $\phi$ and $\theta$.

8. Odometry Algorithm

The odometry algorithm presented here can be used when the TGMS has no access to the data of a precise odometer of the vehicle and/or a GNSS cannot be used, for example, as it happens in underground trains. Underground trains use to be metropolitan. Being metropolitan, there used to be many narrow curved sections. Curved sections facilitate the method presented next.

As shown in Figure 9, the design geometry of a railway track (horizontal profile, as explained in Section 3.2) is a succession of segments of three types: straight (s in the figure) with zero curvature, circular (c in the figure) with constant curvature and transitions (t in the figure) with linearly varying curvature. The curvature function can be decomposed into a set of zero segments (straight segments) plus a set of curvature functions that have trapezoidal shape (normal curve) or double-trapezoidal shape (S-curve). The location of the curvature functions (start and end points) is exactly identified along the track using the design geometry provided by the track preprocessor.

The curvature of the track can be experimentally approximated in the TGMS with the installed sensors. The curvature of the trajectory followed by the TGMS can be obtained as:

$${\rho }_h^{exp}\simeq \frac{{\widehat{\omega }}_z^{tgms}}{V}$$

(40)

Of course, this approximate measure is a noisy version of the track horizontal curvature. However, experimental measures show that the overall shape of the curvature functions can be clearly obtained with this approximation.

The concept of the odometry algorithm is to monitor the experimental curvature during the ride of the train using Equation (40) and to store the data together with the approximate coordinate $s_{app}^{tgms}$ obtained with the help of the installed encoder and the assumption of rolling-without-slipping of the wheel. The plot of the experimental curvature looks like the plot at the top of Figure 10. Using the track preprocessor, the design value of the curvature of the track ${\rho }_h^{design}$ in the area where the train is located, may look like the lower plot in Figure 10. As shown in the figure, this information can be used to correct the value of $s_{app}^{tgms}$ at points 1, 2, 3 and 4 located at the entry or exit of the curves. Measures of $s_{app}^{tgms}$ between these corrected points are also corrected using a linear mapping, as shown in Figure 11. Where, black arrows mean odometry corrections due to the algorithm output. Coloured arrows mean odometry correction using linear interpolation of the algorithm output. Different colours are just used to distinguish the curvature function sections

The problem is how to detect the entry and exit of the curves using the functions ${\rho }_h^{exp}\left(s_{app}^{tgms}\right)$ and ${\rho }_h^{design}\left(s^{tgms}\right)$. In fact, it is the exit of the curves that is detected first. Once the TGMS leaves a curve, the shape of the curvature function that the TGMS has ahead is known. Therefore, when the measured curvature ${\rho }_h^{exp}\left(s_{app}^{tgms}\right)$ “looks similar” to the expected curvature function ${\rho }_h^{design}\left(s^{tgms}\right)$, the exit of the curve has been reached. This similarity is computed by calculating at each instant the squared-error of the experimentally measured curvature and the expected curvature function, as follows:

$$e2\left(s\right)={\int }_{\overline{s}=s-\mathrm{\Delta }s}^{\overline{s}=s}{\left[{\rho }_h^{exp}\left(\overline{s}\right)-{\rho }_h^{design}\left(\overline{s}-s+s_{exit}\right)\right]}^{2}d\overline{s}$$

(41)

where $e2\left(s\right)$ is the squared error (s substitute $s_{app}^{tgms}$ for simplicity in the formula), $\mathrm{\Delta }s$ is the length of the expected curvature function and $s_{exit}$ is the location of the exit of the curve in the design geometry. For a better accuracy, the value of the squared error is normalized for each curvature function using the following factor:

$$I2={\int }_{\overline{s}=0}^{\overline{s}=\mathrm{\Delta }s}{\left[{\rho }_h^{design}\left(\overline{s}\right)\right]}^{2}d\overline{s}$$

(42)

The normalization factors, that are different for each curvature function, are of course computed in a preprocesing stage. The normalized squared-error is given by:

$$ne2\left(s\right)=\frac{e2\left(s\right)}{I2}$$

(43)

Thanks to the normalization, the value of $ne2$ varies between approximately 1, in straight track sections, and 0 when there is a perfect matching between ${\rho }_h^{exp}\left(s_{app}^{tgms}\right)$ and ${\rho }_h^{design}\left(s^{tgms}\right)$. A typical plot of the function is observed in Figure 12. This function used to be smooth, such that detecting the local minimum that indicates the detection of the exit of the curve is a very easy task. Once the exit of the curve is detected, the expected curvature function is substituted by the next curve ahead along the track. It can be shown that this method can be run in real-time. The main computational cost is the one associated with the calculation of the integral given in Equation (41).

9. Measurement of TGMS to TF Relative Motion

The final and most difficult step needed to find the absolute irregularities of the track using Equation (27) is to obtain the relative trajectory ${\overline{\mathbf{r}}}^{tgms}$ of the TGMS with respect to the TF shown in Figure 8. Equation (36) can be treated as a second order differential equation in terms of ${\overline{\mathbf{r}}}^{tgms}$. This equation has the following scalar components:

$$\begin{array}{c}\hfill {\overline{\ddot{\mathbf{R}}}}^{tgms}=\left[\begin{array}{c}\dot{V}\\ {\rho }_h{V}^2\\ -{\rho }_v{V}^2\end{array}\right]+\left[\begin{array}{c}0\\ {\ddot{r}}_y^{tgms}\\ {\ddot{r}}_z^{tgms}\end{array}\right]+\\ \hfill \left[\begin{array}{c}-r_y^{tgms}\left(\dot{V}{\rho }_h+V^2\left({{\rho }^{\prime }}_h-{\rho }_{tw}{\rho }_v\right)\right)+r_z^{tgms}\left(V^2{\rho }_{tw}{\rho }_h+\dot{V}{\rho }_v\right)\\ -r_y^{tgms}\left(V^2\left({\rho }_{tw}^2+{\rho }_h^2\right)\right)-r_z^{tgms}\left(-V^2{\rho }_v{\rho }_h+\dot{V}{\rho }_{tw}\right)\\ r_y^{tgms}\left(V^2{\rho }_v{\rho }_h+\dot{V}{\rho }_{tw}\right)-r_z^{tgms}\left(V^2\left({\rho }_{tw}^2+{\rho }_v^2\right)\right)\end{array}\right]+\\ \hfill +2\left[\begin{array}{c}V{\dot{r}}_z^{tgms}{\rho }_v-V{\dot{r}}_y^{tgms}{\rho }_h\\ -V{\dot{r}}_z^{tgms}{\rho }_{tw}\\ V{\dot{r}}_y^{tgms}{\rho }_{tw}\end{array}\right]\end{array}$$

(44)

Alternatively, the absolute acceleration of the TGMS can be obtained using the measurement of the accelerometer given in Equation (29). If the vectors in Equation (29) are projected to the TF, it yields:

$$\begin{array}{c}\hfill {\overline{\ddot{\mathbf{R}}}}^{tgms}={\mathbf{A}}^{t,tgms}{\mathbf{a}}^{imu}-{\left({\mathbf{A}}^t\right)}^T{\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T=\\ \hfill \left[\begin{array}{c}{a}_x^{imu}-a_y^{imu}{\psi }^{tgms}+a_z^{imu}{\theta }^{tgms}\\ a_y^{imu}+a_x^{imu}{\psi }^{tgms}-a_z^{imu}{\phi }^{tgms}\\ a_z^{imu}-a_x^{imu}{\theta }^{tgms}+a_y^{imu}{\phi }^{tgms}\end{array}\right]+\left[\begin{array}{c}g{\theta }^t\\ -g{\phi }^t\\ -g\end{array}\right]\end{array}$$

(45)

where the small-angles assumption has been used again. The left-hand sides of Equations (44) and (45) represent the same physical magnitudes, therefore, the right hand sides of these equations can be equated. Using just the second and third components of the right hand sides and rearranging yields:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\ddot{r}}_y^{tgms}\\ {\ddot{r}}_z^{tgms}\end{array}\right]+\left[\begin{array}{cc}0& -2V{\rho }_{tw}\\ 2V{\rho }_{tw}& 0\end{array}\right]\left[\begin{array}{c}{\dot{r}}_y^{tgms}\\ {\dot{r}}_z^{tgms}\end{array}\right]+\\ \hfill \left[\begin{array}{cc}-V^2\left({\rho }_{tw}^2+{\rho }_h^2\right)& V^2{\rho }_v{\rho }_h-\dot{V}{\rho }_{tw}\\ V^2{\rho }_v{\rho }_h+\dot{V}{\rho }_{tw}& -V^2\left({\rho }_{tw}^2+{\rho }_v^2\right)\end{array}\right]\left[\begin{array}{c}{r}_y^{tgms}\\ r_z^{tgms}\end{array}\right]=\\ \hfill \left[\begin{array}{c}{a}_y^{imu}+a_x^{imu}{\psi }^{tgms}-a_z^{imu}{\phi }^{tgms}-g{\phi }^t-{\rho }_h{V}^2\\ a_z^{imu}-a_x^{imu}{\theta }^{tgms}+a_y^{imu}{\phi }^{tgms}-g+{\rho }_v{V}^2\end{array}\right]\end{array}$$

(46)

This is a $2nd$ order linear system of ordinary differential equations (ODE) with time-variant coefficients (linear time-varying system, LTV). This ODE has to be integrated forward in time to find the TGMS to TF relative trajectory ($r_y^{tgms}\left(t\right)\mathrm{and}{r}_z^{tgms}\left(t\right)$). The inputs of these equations are all calculated or measured so far. These inputs are:

• The accelerometer data ${\mathbf{a}}^{\mathit{imu}}$.
• The instantaneous forward velocity V and acceleration $\dot{V}$ of the vehicle. This is obtained from the encoder data.
• The position $s^{tgms}$ of the TGMS along the track. This is the output of the odometry algorithm explained in previous section. The position $s^{tgms}$ is used as an entry to the track preprocessor to get the track design cant angle ${\phi }^t$ and the curvatures ${\rho }_{tw},{\rho }_v\mathrm{and}{\rho }_{tw}$.
• The relative orientation of the TGMS with respect to the TF that is calculated in Section 7.

In the case of a tangent (straight) track, where all track curvatures are zero, Equation (46) reduces to:

$$\left[\begin{array}{c}{\ddot{r}}_y^{tgms}\\ {\ddot{r}}_z^{tgms}\end{array}\right]=\left[\begin{array}{c}{a}_y^{imu}+a_x^{imu}{\psi }^{tgms}-a_z^{imu}{\phi }^{tgms}\\ a_z^{imu}-a_x^{imu}{\theta }^{tgms}+a_y^{imu}{\phi }^{tgms}-g\end{array}\right]$$

(47)

Numerical integration of Equation (46) can be used to find the TGMS to TF relative motion. However, a close look to Equation (46) shows that these differential equations are equivalent to the equations of motion of a linear 2-degrees of freedom system with negative-definite stiffness and damping matrices. It is a highly unstable system. Therefore, serious problems of drift of the resulting displacements are expected. As a result that the track irregularity is a relatively-high frequency component of the track geometry (being the design geometry the low-frequency component), the drift of the solution can be solved using several methods, like these two:

• Using a digital signal processing approach for the integration that combines double-integration and high-pass filtering of the signal [3].
• Using a Kalman filter approach that adds to the system dynamic equations, Equation (46), a set of measurements. These measurements are virtual sensors that in practice always provide a zero value of the TGMS to TF relative motion. The assumed covariance of these measurements is the expected covariance of the track irregularities. This method has been successfully applied in [23] to eliminate the drift in the results while keeping a good accuracy.

The second method is used in this investigation to get the results presented in Section 11.

10. Experimental Setup

A scale track has been built at the rooftop of the School of Engineering at the University of Seville. Figure 13 on the left shows an aerial view of the 90 m-track. The scale is approximately 1:10 (5-inch gauge). The track includes a tangent section (next to end A), a 26 m-curved section and a 12 m-curved section, among other features. The track is supported on a set of mechanisms that can be observed on the right of the figure that are separated 10 cm. These mechanisms can be used to create any irregularity profile. Details of the track design can be found in [24].

Figure 14 shows the scale vehicle that has been used to install the TGMS. The vehicle has a classical architecture of 4 wheelsets, 2 bogies and 1 car-body. The TGMS is attached to the car-body. The right picture shows a detail of the vehicle where the video cameras and the laser beam can be observed. The vehicle drive is an electric motor with a chain transmission. Details of the vehicle design can be found in [24]. The TGMS is equipped with two video cameras Ximea MQ003CG-CM. The VGA cameras have a resolution of 648 × 488 pixels. They include a CMOS RGB bayer matrix sensor. Their maximum frame acquisition rate reaches 500 fps. The camera has a C-mount lens FUJINN 2/3” 12.5 F1.2–F1.6 MI E1.5MP MV that allows the adjustment of focus and exposure. The projection lasers have a power of 10 mW and they illuminate in red. The IMU is a LORD Microstrain 3DM-GX5-25 AHRS, an triaxial accelerometer, gyroscope and magnetometer industrial sensor fully calibrated and temperature compensate. It has a maximum sampling rate of 1 kH. The accelerometer has two different measuring ranges ±8 g (25 g/$\sqrt{\mathrm{Hz}}$) or ±20 g (80 g/$\sqrt{\mathrm{Hz}}$). The gyroscope has an angle random walk of 0.3${}^{\circ }/\sqrt{\mathrm{h}}$. The traveled distance by the vehicle along the track is obtained using a precision digital encoder Kubler 2400 series. It is a two channel encoder with up to 1440 pulses per revolution.

In order to have a reference value of the track irregularities, an accurate track geometry measurement has been performed. The relative irregularities have been measured with the instrument that can be observed in the right of Figure 15. This instrument slides along the track. It includes a LVDT to measure the gauge variation and an inclinometer to measure the cross-level. In order to measure the absolute irregularities, alignment and longitudinal profile, the total station shown in the left hand side of the figure has been used. Details of the track geometry measurement can be found in [24].

11. Computer Implementation and Comparison of TGMS Measurement with Reference Irregularity

The methods and algorithms described in this paper have been implemented in Matlab environment. However, only built in functions of the basic package have been used, avoiding the use of functions, for example, of the computer vision toolbox. The program that produces the track irregularity out of the sensor data includes the following modules:

• Pre-process: Camera calibration module. It finds the cameras’ intrinsic and extrinsic parameters using pictures of a checkerboard pattern [19].
• Pre-process: Track pre-processor module. It is set to provide the design geometry of the track.
• Process: Computer vision module. It finds the position and orientation of the rail cross-sections using the recorded frames and the method described in Section 5.
• Process: Odometry module. As described in Section 8, it finds the position, velocity and acceleration of the TGMS every time instant.
• Process: TGMS orientation module. It finds the orientation of the TGMS using the method described in Section 7.
• Process: TGMS trajectory module. It finds the relative trajectory of the TGMS with respect to the TF using the method described in Section 9.
• Process: Irregularity module. It calculates the track irregularities with the method of Section 6.

The results of this program are now compared with the reference value of the irregularities that were obtained with the method described in the previous section. Figure 16, Figure 17, Figure 18 and Figure 19 show the comparison between the reference geometry measurement explained in the previous section and the measurement of the TGMS developed in this investigation. The experiment was done with the vehicle shown in Figure 14 at a forward velocity of 2.5 m/s, that, considering the scale, it is equivalent to 25 m/s = 90 km/h of a real train. This is approximately the maximum forward velocity of metropolitan trains. The figures show the irregularities in the first 56 m of the track. The complete length of the scale track could not be measured because the vehicle had difficulties to negotiate the 12 m-radius curve at that forward velocity. The irregularities shown in the figures are high-pass filtered with a cut-off wave length of 3 m (wavelengths above 3 m are filtered out). It is a common practice in the geometry measurement of metropolitan trains to measure just wave lengths below 30 m, what is equivalent to 3 m in our scaled track.

The cross-level shown in Figure 16 shows a good agreement between the reference geometry and the measure of the TGMS. The agreement in gauge variation shown in Figure 17 is even better. Both curves are almost identical. However, in both figures, some edge effect is observed at the beginning of the measurement.

The absolute irregularities are compared in Figure 18 and Figure 19. As it can be observed, the agreement is fine, but not as good as the agreement of the relative track irregularities. The reason is clearly the influence in the results of the approximations and noise associated with the calculation of the TGMS to TF relative trajectory explained in Section 9. Recall that this relative trajectory is not needed for the calculation of the relative irregularities. It can also be observed that the measurements of the designed TGMS shows higher spatial-frequencies than the measurement with the total station. However, these higher frequencies must not be considered as noise, because the measuring method used with the total station can not detect high-frequency (short wavelength) irregularities.

12. Summary and Conclusions

This paper explains the numeric algorithms used to calculate track geometry irregularities with a new TGMS. These algorithms are basically kinematic relationships that describe the motion of a body moving along a track. After the description of the system and the sensors needed, the multibody-based kinematics is described in detail as well as the approximations used due to the small angles assumption. In this kinematic description, the track geometry (design geometry plus irregularities) is assumed totally arbitrary. As a result that the TGMS uses a computer vision system, the kinematic equations needed to locate the recorded rail cross-sections in the TGMS frame have been deduced, as well as a method to find the relative coordinates of the rail cross-sections with respect to the TGMS frame.

Section 6 shows the equations that can be used to find the value of the four independent track irregularities: gauge and cross-level in Equation (24), and alignment and vertical profile in Equation (27), as a function of the output of the computer vision system (explained in Section 5), the relative TGMS to TF relative orientation and the relative TGMS to TF relative trajectory. However, there is a long way to go to apply these equations.

Section 7 explains the development of a linear dynamic model that can be used to find the TGMS to TF relative orientation using the installed inertial sensors, 3D accelerometer and 3D gyroscope. This model is the basis of a Kalman filter that provides as the output the required TGMS angles. The application of this algorithm requires a precise knowledge of the position of the TGMS along the track. This position together with the vehicle forward velocity are needed to calculate the accelerations due to the motion of the TGMS along the design geometry of the track. In turn, these accelerations are needed to calculate the relative orientation based on the accelerometer signals. Section 8 explains the odometry algorithm that is used in this investigation for the precise calculation of the value of the arc-length s of the TGMS along the track. This algorithm is based on the measurement of the track horizontal curvature and the knowledge of the track design geometry.

The most difficult step for the calculation of the track irregularities is to obtain the TGMS to TF relative trajectory. Fortunately, this trajectory is needed to find the absolute irregularities but not needed to find the relative irregularities. The calculation of the relative trajectory is explained in Section 9. After a set of approximations, Equation (46) shows a second-order linear system of ordinary differential equations that can be solved to find the TGMS to TF relative trajectory. However, these equations are intrinsically unstable, requiring special solution methods that need to be improved, as it can be deduced from the experimental results.

The calculation of the track irregularities has been experimentally validated in a scale track that is described in Section 10 as well as the alternative method used to measure the track irregularities. This alternative method is accurate and based on the sensors used in the commercial rail track trolleys, LVDT and inclinometer, and a total station. The comparison of the measurements of the TGMS presented in this paper and this reference measurement is shown in Section 11. The relative irregularities are measured with high accuracy. The measurement of the absolute irregularities can be considered as acceptable, but the accuracy is much less. It can be concluded that the method of calculation of the TGMS to TF relative trajectory, that is the additional input needed for the calculation of the absolute irregularities, needs to be improved.