Rail vehicle localization exploiting rail track georeferenced coordinates

A novel dead reckoning algorithm conceived for localization of small inspection rail vehicles in Global Navigation Satellite System (GNSS) denied environments is presented. This work focus on simplifying the rail vehicle localization task, taking into account restrictions on movement imposed by the railroad tracks. Considering that dead reckoning techniques accumulate errors over time, leading to increasing global uncertainty, a method was designed to correct the estimates and also smooth trajectory errors backwards in time, through visualization of global landmarks. Results show the effectiveness of this approach in reducing long-term position errors. The current document reports real railroad experiments, featuring a specially designed non-motorized mobile modeling vehicle. Subject Headings. Robotics, Data Processing, Railway Track. Author


Introduction
Rail vehicles may travel along several kilometers of tunnels and canyons without being able to receive GNSS signal.Nowadays long-term precise localization in GNSS-denied environments is still a challenging problem, and even more difficult to achieve through lowcost sensors.In rail inspection systems (Glaus 2006) total stations are usually employed to track the surveying vehicle.This is not a versatile solution since the total stations have to be moved constantly to maintain line-of-sight with the vehicle.By this reason, for long traveling distances, on-board localization systems would be much more convenient.Over the last few years, Simultaneous Localization and Mapping (SLAM) became an effective solution to the robotic exploration problem.Some interesting methods where evaluated in extreme scenarios, including mines (Nüchter et al. 2004;Thrun et al. 2004), however the obligation to close loops in order to reduce the global uncertainty remains a huge limitation.Loop detection is usually a hard task and in some situations the system may not be able to revisit previously explored areas, due to logistic restrictions or, in the particular case of rail vehicles, due to the absence of loops in the tracks.Furthermore, smooth and uniform geometry of the surrounding scene, especially inside tunnels, may lead to sensor ambiguity, which makes the localization process even harder.Since rail vehicles follow the fixed course enforced by the railroad, whenever a detailed description of the tracks is available, in the form of global coordinates, a model of the path can be instantiated and used to help solving the localization problem.In our case the railroad model relates two different domains, namely the path length along the rail and the vehicle localization, defined in the same global coordinate system used for the railroad

Railway Modeling
In railways design, the route of a given track is referred by the term alignment.The horizontal alignment defines the track shape in the XY plane, while the vertical alignment consists on the elevation along the Z component.Since trains have no control over lateral movement the alignment restrictions are of extreme importance to ensure both security and comfort.The overall alignment results from the combination of three basic shapes: straight lines, circular arcs and transition curves.The Euler spiral -clothoid -is the most common shape of transition curve.The curvature of a clothoid changes linearly with his curve length, resulting in a constant value of centripetal acceleration applied to a vehicle moving at constant velocity.Usually, transition curves provide a smooth change between curved and straight segments in the horizontal alignment.For the vertical alignment only straight segments and transition curves are used.Nevertheless, in either case, the resulting track shape is smooth from the mathematical point of view, i.e., the piece-wise function representing the global track configuration has continuous derivatives.Splines preserve the same properties, which makes the method ideal to construct the railroad model.

Spline Interpolation Background
The rail model is obtained by interpolation of georeferenced coordinates using cubic splines.For the two-dimensional case, assume a set of  control points,  ⃗ = � ⃗ 1,  ⃗ 2 , … ,  ⃗  � = {( 1 ,  1 ), ( 2 ,  2 ), … , (  ,   )}, with  1 <  2 < ⋯ <   .Each sub-interval [  ,  +1 ], with 1 ≤  ≤  − 1, is represented by a distinct third-order polynomial   () (See Figure 1), so the interpolation function is a collection of  − 1 polynomials expressed in a piece-wise manner as: Each cubic polynomial piece   () has 4 unknown coefficients -{  ,   ,   ,   }: The goal is to find the 4( − 1) unknowns so that polynomials join at the control points as smoothly as possible.Therefore, the following restrictions are applied so that  ′ (),  ′ () and  ′′ () preserve continuity (Burden et al. 2005): From the above constraints 4( − 1) − 2 equations can be instantiated.The system is still under-determined, hence two more constraints are needed to reach as many equations as the total number of unknowns.There are several ways to select the two additional constraints (Bartels et al. 1987), the one used here is the not-a-knot condition.It stipulates continuity for the third derivative at the points  2 and  −1 : Finally, the spline coefficients are computed by simply solving the system of linear equations defined by all previous constraints (equations 3).

Expanding Spline Interpolation to the 3D Space
One way of performing cubic spline interpolation of 3D points is to decouple each component and solve the problem independently for each dimension, ending up with three piece-wise polynomials -  (),   () and   () (See Figure 2).
For simplicity reasons a common domain  must be used in all spline computations.Remember, from section 2.1, that domain points have to be monotonically increasing, so this must be taken into consideration when choosing the calculation method for .Our solution is to define  to be the cumulative sum of Euclidean distances between consecutive points: This is a practical solution since  can be computed directly from the initial set of points, no extra information is needed.Moreover, for a set of non-repeated points, a monotonically increasing result will always be achieved.

Localization Method
The localization algorithm operates in two different domains, particularly: − The railroad domain -One state corresponding to the vehicle displacement along the rail tracks: (), plus two additional parameters   and spline segment index, used to relate the displacement with the railroad model; − The global domain -Position {(), (), ()} and orientation {  (),   (),   ()} defined in the ECEF (Earth-Centered Earth-Fixed) Cartesian coordinate system.Figure 3 illustrates the high level concept for the purposed localization approach.After building the railroad model, as described in the previous section, the vehicle's initial position is projected to the closest point in the splines model, to specify the localization starting point in the railroad domain.The dead reckoning method then runs exclusively in the railroad domain, integrating distance increments, provided by wheel encoders, to retrieve the vehicle position along the tracks.The domain change operation is a crucial piece that influences the design of all other blocks.It manipulates the railroad model to relate the global domain with the railroad domain and perform conversion in both directions.Two distinct algorithms deal with each direction of conversion, its details are presented next.The conversion in the other direction is slightly more complex.A sequence of operations is executed to project global coordinates to the railroad domain.The conversion is applied either to the initial global position and also global observations to find the corresponding projections on the railroad model.Throughout the procedure, relevant information, including   , and the spline segment in which the point is projected, are determined and used to finally compute the corresponding displacement in relation to the origin of the railroad model.

From Global to Railroad Domain -Spline segment and 𝛌𝛌 𝐩𝐩 computation
The goal is to project a global point   = (  ,   ,   ) into the closest point along the railroad model, to determine three parameters:   , the spline segment index  and the displacement , defined as the rail track length between the projected point and the point  ⃗ 1 -the origin of the railroad model.Following Algorithm 1, the search for the closest spline segment and the computation of   are carried out simultaneously.First, the nearest spline node to   is selected from the set of georeferenced points  ⃗.The segments adjacent to the node selected establish the two possibilities for the closest spline segment.The algorithm focus then on those two spline segments, searching for the points in each one that minimize the Euclidean distance to   .Euclidean distance and squared Euclidean distance share the same minima, so the squared Euclidean distance is minimized instead to simplify the calculations.To formalize the problem let's consider the arbitrary spline segment , the objective is to find the value of   that minimizes the squared Euclidean distance: Where: The minimum of  2 is found by computing the roots of ( 2 /) as follows: With: Equation 8 produces five solutions, although only one real solution is expected.It should be considered the hypothetical value of   for that given segment.More than one real solution is an indication of poor model resolution, hence more georeferenced rail coordinates must be used to build the railroad model.Finally, as stated in the last line of Algorithm 1, the hypotheses are evaluated through a domain check operation.Each hypothesis must comply with the corresponding segment limits, in other words, for segment  the hypothetical value must be between  −1 and   .Again, when dealing with a congruent model, only one valid hypothesis is expected.The valid hypothesis reveals also the initial spline segment.

From Global to Railroad Domain -Displacement in railroad domain
The displacement calculation () can proceed by computing the splines arc-length between the models origin and the new point, defined by the previously computed   and spline segment index.The arc-length () of an arbitrary spline segment  is evaluated analytically by the next formula.
The integration limit may be simply adjusted to compute the arc-length for intermediate values of   [ −1 ,   ].This capability is essential to compute the displacement portion of the spline segment  in which the point is projected.Accordingly, the displacement will be the sum of all spline arc-lengths from the models origin to segment  − 1 plus a portion of the spline segment  ℎ arc-length:

Dead Reckoning Method and Displacement Calculation
Now with the capability of jumping between domains, the localization starting point in the railroad domain can be determined though projection of the initial global position.After that the system is ready to perform the dead reckoning algorithm to estimate the vehicle position in the railroad domain.First wheel encoder measurements are converted into distance increments and subsequently added to the previously estimated displacement.This simple operation computes the new vehicle position in the railroad domain, nevertheless the update of   is also convenient, since it is a crucial parameter when jumping from the railroad domain to the global domain.The new   value is successively approximated by iteration according to Algorithm 2. To increase readability, Algorithm 2 leaves out some details regarding transitions between consecutive spline segments and does not deal with movement in the reverse direction.The algorithm implements a proportional control scheme to refine the value of   () until the arc-length between   ( − 1) and   () resembles the actual displacement measured by the sensors.To compute the predicted displacement the arc-length equation is used (equation 10).The iteration cycle stops when an error below ℎℎ = 10 −10 is achieved.The threshold value is more than 10.000 times smaller than the resolution of our measuring system, so the error introduced here is insignificant.

Orientation Calculation
The vehicle orientation with respect to the ECEF reference frame is derived from the global position of three wheels: the two front wheels and the left rear wheel.However, only the position of the front left wheel is given by the dead reckoning algorithm.The position of the front right wheel results from the projection of the front left wheel into the closest point in the right rail track, while the rear left wheel position is estimated using a strategy similar to Algorithm 2, but, instead of searching for a given displacement, takes the Euclidean distance between those wheels to be the goal.As illustrated in Figure 4, the coordinates of those three points define two vectors, aligned with booth   and   axes of the body frame.Tacking the cross product of the two normalized vectors will result in a direction vector perpendicular to the plane defined by     �������⃗ and     ��������⃗ .There is no guarantee that vectors     �������⃗ and     ��������⃗ are orthogonal so vector     �������⃗ is re-estimated by tacking the cross product between     ��������⃗ and  � �⃗ .After this set of operations we end up with three directional vectors that define a three-dimensional coordinate system.The vectors can be organized in a matrix form to define the rotation matrix between the ECEF coordinate system and the body frame: Finally, the Euler angles are extracted from the rotation matrix using a standard algorithm.

Post-processing Method
As explained before, at least one global reference is needed to initialize the system in the railroad domain.Any additional global observation, taken during the vehicle operation, can be projected to the railroad model to stipulate precisely the current vehicle displacement.The displacement error is then used to compensate past encoder measurements until the instant of the previous observation.
The proportion between the displacement error   accumulated between observations at time  1 and  2 , and the error expressed as a function of the displacement   1 → 2 between those observations becomes: For a vehicle moving only in one direction, the displacement error can be compensated by applying the above correction constant to the encoder measurements: () = () + () * Where   [ 1 ,  2 [.This procedure stretches or compresses the displacement measured by the wheel encoders.It compensates various sources of error, namely the lack of adherence between the wheels and the rail, inaccuracy in the wheel radius measurement, and the rail model error.
The dead-reckoning algorithm must be executed again to process the corrected measurements between instants  1 and  2 .After that, the process can be repeated for the next observation 3 and so on.

Results and Discussion
The dataset was collected in a disabled rail line with an extension of near 300 meters, see Figure 5.The non-motorized modeling vehicle used for data collection carried a variety of sensors along with a computational system and a power unit.The two front wheels were equipped with encoders with a resolution of 16000 ticks per revolution, which associated to a wheel with a 10 centimeters radius assure a metric resolution of 0.039 mm.Measurements provided by a INS/GPS iMAR iNAV-FMS-E, further processed in a tightly-coupled fashion using a commercial software (Waypoint Inertial Explorer), produce the ground truth used to evaluate the performance of the purposed localization method.The railroad model was built using 10 rail points extracted by GPS, with approximate distance of 30 meters between each other.Finally, the landmarks introduced in the post-processing stage consist on reflective targets detected by a RIEGL VZ-400 laser scanner.Two variants of the post-processing algorithm were tested, one -denominated as smoothing -processes a series of landmarks placed across the rail.The other methodology behaves as a calibration procedure, where a short path, with approximately 40 meters, is processed with only two global observations, at the beginning and at the end of the path.The objective is to compute the overall error and apply the compensation to the measurements collected in a longer experiment.
The results shown below characterize the performance of the dead reckoning approach and the two post-processing methods.In the test presented the vehicle traveled forward for about 220 meters stopped for a while and traveled back towards the starting point.During motion, five landmarks are observed twice -one observation in each direction.Position errors in all components (see Figure 6) clearly show the advantage of the post-processing methods in improving the dead reckoning results.The calibration method takes less information than the smoothing post-processing solution, this justifies the higher errors of the calibration approach.Still, the outcome is notably superior when compared with the plain dead reckoning method.Analysis of the absolute error (Euclidean distance to the ground truth), indicates once more the ability of the post-processing solutions to reduce the error of the dead reckoning approach.
Interesting error symmetry is observed between forward and reverse directions.At the same rail zones a similar error behavior, regardless the traveling direction, indicates rail model deficiencies in those particular spots.The rail model errors are intrinsic to the system and cannot be totally smoothed through the post-processing technique, nevertheless the gain in accuracy given by the post-processing phase is obvious.The orientation errors depicted in Figure 7 reflect mainly the railroad model deficiencies, due to the heavy influence of the railroad model in the orientation calculation.Therefore, the orientations computed based on the post-processed trajectories do no achieve a significant improvement regarding the ones computed through dead-reckoning.In the worst case the post-processing methods present an error inferior to 2.5 degrees, which is still quite high and unacceptable for 3D mobile modeling applications.

Conclusions and Future Work
A localization method for rail vehicles was presented.It relies on previous geometric measurements to establish rigid rules for the rail vehicle movement.Those restrictions complement a simple and inexpensive perception system, and enable the localization estimation in six degrees of freedom.The post-processing solutions, purposed to reduce the global position errors, accomplished meritorious results with errors almost ten times smaller than the ones achieved by the dead reckoning solution.Moreover its application prevents the unbounded error growth over time.
The orientation calculation method revealed itself highly sensitive to the railroad model error, and neither the dead reckoning nor the post-processing trajectories led to acceptable orientation estimates.In the future, the inclusion of orientation observations, provided for instance by cameras, may be considered to reduce orientation and railroad model errors.

Figure 1 :
Figure 1: Interpolation of n points using cubic splines

Figure 2 :
Figure 2: Together, the three cubic splines   (),   () and   (), defined in the same domain , encode the complexity of a three-dimensional spiral   ()

Figure 3 :
Figure 3: Conceptual diagram of the localization system3.1.Domain ChangeThrough the railroad model global coordinates are projected to the railroad domain and vice-versa.In addition to the displacement along the tracks, the domain change operation demands for additional redundant information in the railroad domain, such as the value of   corresponding to the current vehicle position and the index of the spline segment in which the vehicle is located.With that knowledge, coordinates in the global domain can be easily recovered by replacing   in spline equations (equation 4).The conversion in the other direction is slightly more complex.A sequence of operations is executed to project global coordinates to the railroad domain.The conversion is applied either to the initial global position and also global observations to find the corresponding projections on the railroad model.Throughout the procedure, relevant information, including   , and the spline segment in which the point is projected, are determined and used to finally compute the corresponding displacement in relation to the origin of the railroad model.

Figure 4 :
Figure 4: Representation of the body frame and vectors used to compute the vehicle orientation

Figure 5 :
Figure 5: Picture of the mobile modeling vehicle during data acquisition in a railroad.A reflective target can be seen at the right of the vehicle wheels

Figure 6 :
Figure 6: Position errors in each component and Euclidean distance error of the three localization variants regarding the ground truth.

Figure 7 :
Figure 7: Orientation errors in each component of the three localization variants regarding the ground truth.