Research on the Integrated Navigation Technology of SINS with Couple Odometers for Land Vehicles

Autonomous and accurate acquisition of the position and azimuth of the vehicle is critical to the combat effectiveness of land-fighting vehicles. The integrated navigation system, consisting of a strap-down inertial navigation system (SINS) and odometer (OD), is commonly applied in vehicles. In the SINS/OD integrated system, the odometer is installed around the vehicle’s wheel, while SINS is usually installed on the base of the vehicle. The distance along SINS and OD would cause a velocity difference when the vehicle maneuvers, which may lead to a significant influence on the integration positioning accuracy. Furthermore, SINS navigation errors, especially azimuth error, would diverge over time due to gyro drifts and accelerometer biases. The azimuth error would cause the divergence of dead-reckoning positioning errors with the distance that the vehicle drives. To solve these problems, an integrated positioning and orientation method based on the configuration of SINS and couple odometers was proposed in this paper. The proposed method designed a high precision integrated navigation algorithm, which compensated the lever arm effect to eliminate the velocity difference between SINS and odometers. At the same time, by using the measured information of couple odometers, azimuth reference was calculated and used as an external measurement to suppress SINS azimuth error’s divergence over time, thus could further improve the navigation precision of the integrated system, especially the orientation accuracy. The performance of the proposed method was verified by simulations. The results demonstrated that SINS/2ODs integrated system could achieve a positioning accuracy of 0.01% D (total mileage) and orientation accuracy of ±30″ by using SINS with 0.01°/h Fiber-Optic Gyroscope (FOGs) and 50 µg accelerometers.


Introduction
In modern warfare, land-fighting vehicles are often threatened by remote detection systems and precision strike systems and usually need to move quickly to increase their survivability [1], which means that the land vehicles must have the ability to obtain the position and azimuth autonomously and accurately and could launch immediately when parking or even traveling. Therefore, it is very important to improve the land vehicles' maneuverability, viability, and rapid response capability by devising an integrated navigation technique that can obtain the position and azimuth information of vehicles autonomously and accurately [2].
To achieve the above goals, the integrated navigation system of the strap-down inertial navigation system (SINS) and odometer is applied in land vehicle applications since it can autonomously obtain relatively high-accuracy navigation reference information [3,4]. In this way, the land vehicle departs from a known coordinate position to perform a task, and it can obtain position and attitude information immediately at any launch point. Compared with the integrated navigation system of SINS and global navigation satellite system (GNSS), the SINS and odometer integrated navigation system does not need to acquire external signals and is not vulnerable to external interference. So, it has significant advantages in modern warfare [5][6][7].
The research of this integrated navigation system mainly focuses on online calibration and compensation of integrated navigation system error parameters and the measurement fault detection of odometer caused by land vehicle jumping, taxiing, skidding, and sliding. In [8], the odometer aided in-motion alignment method was proposed, and the error of odometer was comprehensively modeled as a part of an integrated dynamic model. The effectiveness of the method was verified by ground alignment experiments. In [9], the SINS and odometer integrated navigation system adopted a Kalman filter to design an alignment method to assist the SINS for initial alignment on the moving base. A high accuracy integrated algorithm was proposed in [10], and in the algorithm, whether the odometer is faulty was determined by the difference of mileage increments of the SINS and OD outputs. In [11], a fault detection method based on the adaptive Kalman filter was introduced to judge the odometer condition. It could effectively improve navigation accuracy and detect the odometer fault in time.
Recently, more and more researchers paid attention to the rotational inertial navigation system INS (RINS) [12][13][14]. A new integrated navigation system of FOG single-axis rotational INS (FRINS) and OD was presented. It adopted the rotation modulation technique to suppress the inertial sensor errors, especially z-axis FOG drift; at the same time, it combined the FRINS with odometer to suppress the divergence of INS error over time. The experiments showed that the positioning accuracy of the vehicle was effectively enhanced, and the cost of the system was saved [15].
The literature presumes that the installation positions of SINS and odometer are exactly the same. In a practical situation, however, it is impossible to ensure that the installation positions of SINS and odometer are completely a coincidence. When the land vehicle makes an azimuth or a pitch turn, the longer lever arms and higher turning speed that SINS/ODs have, the more velocity difference it would have. Therefore, the lever arm effect is a crucial factor that affects the positioning and orientation accuracy of the integrated navigation system. In the existing literature, the lever arm effect of the SINS and odometer integrated system is only simply explored.
Moreover, a single odometer only can provide velocity or mileage measurement information, and it is unable to provide azimuth measurement reference information for the integrated navigation system to solve the problem of the azimuth error's divergence over time. At the beginning of the land vehicle traveling, the errors of the integrated navigation system cannot be estimated precisely and compensated correctly, which results in large navigation error and affects the positioning and orientation accuracy of the land vehicle.
In this paper, a new integrated navigation method, based on FOG SINS and couple odometers, was proposed to improve the navigation precision, especially the orientation precision. In the proposed method, a high-precision integrated navigation algorithm was established, and the algorithm could effectively compensate for the effect of the lever arm and eliminate the difference of measurement information between SINS and odometers. Besides, by using the couple odometers, the azimuth reference information was calculated and used as the external measurements to suppress SINS azimuth errors' divergence over time. In this case, the proposed method could effectively improve the positioning accuracy, especially the orientation accuracy of land vehicles. The proposed method was implemented and verified by several simulations, and the results demonstrated that compared with the traditional single odometer and SINS integrated navigation method, the proposed method could achieve the positioning accuracy of 0.01% D (total mileage) and orientation accuracy of ±30", respectively, by using SINS with 0.01 • /h FOGs and 50 µg accelerometers.
The paper is organized as follows: the configuration of SINS and couple odometers integrated navigation system is described in Section 2, and then the working process of the proposed method is presented. In Section 3, the velocity and azimuth error models of odometer are deduced step by step. In Section 4, the state equations and measurement equations of the proposed integrated method are established in detail. Simulations, as well as their results, are conducted and analyzed in Section 5. Finally, conclusions are drawn in Section 6.

System Configuration and Lever Arm Effect
In order to clearly illustrate the configuration of the integrated system proposed in this paper, the coordinate systems involved were configured as follows: The integrated system chose the east-north-up geographic coordinate system as the navigation frame (n-frame, O-ENU). The axis direction of body coordinate system (b-frame, O-XYZ) was defined as: x-axis was pointing to the right side of the vehicle, y-axis was along the forward side of the vehicle, z-axis was pointing to the vertical, and x, y, and z axes were in a right-handed coordinate system.
The configuration of the proposed integrated system is briefly illustrated in Figure 1. The system was mainly composed of SINS and a couple of odometers. The SINS was fixed on the front base of the land vehicle, and it contained three accelerometers and three FOGs, which have many advantages, including low cost, small size, lightweight, short start-up time, high reliability, etc. The Y-axis of SINS was along the forward side of the vehicle. The couple optical odometers, as shown on the left of Figure 1, were separately mounted on two rear wheels of the land vehicle, by which the increment data of the land vehicle could be measured in real-time. The photodetector and the grating were the main components of the odometer.

System Configuration and Lever Arm Effect
In order to clearly illustrate the configuration of the integrated system proposed in this paper, the coordinate systems involved were configured as follows: The integrated system chose the eastnorth-up geographic coordinate system as the navigation frame (n-frame, O-ENU). The axis direction of body coordinate system (b-frame, O-XYZ) was defined as: x-axis was pointing to the right side of the vehicle, y-axis was along the forward side of the vehicle, z-axis was pointing to the vertical, and x, y, and z axes were in a right-handed coordinate system.
The configuration of the proposed integrated system is briefly illustrated in Figure 1. The system was mainly composed of SINS and a couple of odometers. The SINS was fixed on the front base of the land vehicle, and it contained three accelerometers and three FOGs, which have many advantages, including low cost, small size, lightweight, short start-up time, high reliability, etc. The Y-axis of SINS was along the forward side of the vehicle. The couple optical odometers, as shown on the left of Figure 1, were separately mounted on two rear wheels of the land vehicle, by which the increment data of the land vehicle could be measured in real-time. The photodetector and the grating were the main components of the odometer.  After the system installation and pre-calibration, it was assumed that the body frame of the SINS coincided with the vehicle's frame. The relative installation positional relationship between the SINS and odometers is shown in Figure 1, as well as the center of mass (CoM) of the vehicle. The measurement point of the odometer relative to the SINS could be expressed in b-frame as: As could be seen from Figure 1, when the land vehicle had an angular motion, the relationship of velocities measured by two odometers and SINS could be written as follows: After the system installation and pre-calibration, it was assumed that the body frame of the SINS coincided with the vehicle's frame. The relative installation positional relationship between the SINS and odometers is shown in Figure 1, as well as the center of mass (CoM) of the vehicle. The measurement point of the odometer relative to the SINS could be expressed in b-frame as: As could be seen from Figure 1, when the land vehicle had an angular motion, the relationship of velocities measured by two odometers and SINS could be written as follows: The second item on the right side of the above formula is the velocity difference of the odometer and SINS caused by the lever arm effect.
where ω b ib can be directly measured by the gyros of SINS. C b n is the transformation matrix, which can be output in real-time through SINS. ω n ie is the projection of the rotation angular rate of the earth in the n-frame, and ω n en is the projection of the angular rate of n-frame related to e-frame in n-frame. ω n ie and ω n en can be calculated from the position and velocity information obtained by SINS at the current moment.

Principle of the Integrated Navigation Method
In the proposed integrated method, three FOGs were used to measure the pitch angular rate, roll angular rate, and yaw angular rate of the vehicle, respectively, and three accelerometers were used to measure the forward acceleration, lateral acceleration, and vertical acceleration of the vehicle, respectively. The measurement information was complete and could be used for a navigation solution with the standard strap-down inertial navigation algorithm. After navigation calculation, the system obtained basic navigation information of that time, such as position, velocity, and attitude. At the same time, two different odometers' velocities, containing lever arm information, were used to carry out the dead-reckoning calculation, as well as to calculate the azimuth.
The differences of the velocity and azimuth information between the SINS and couple odometers were taken as the observation data to estimate the state vector of Kalman filter (KF). The error estimations acquired from the Kalman filter would be used to compensate for basic navigation results, including attitude, velocity, and position error estimations. The schematic of the proposed integrated method is shown in Figure 2.
The second item on the right side of the above formula is the velocity difference of the odometer and SINS caused by the lever arm effect.
ib ω can be directly measured by the gyros of SINS. b n C is the transformation matrix, which can be output in real-time through SINS. ω n ie is the projection of the rotation angular rate of the earth in the n-frame, and ω n en is the projection of the angular rate of n-frame related to e-frame in n-frame.
n ie ω and n en ω can be calculated from the position and velocity information obtained by SINS at the current moment.

Principle of the Integrated Navigation Method
In the proposed integrated method, three FOGs were used to measure the pitch angular rate, roll angular rate, and yaw angular rate of the vehicle, respectively, and three accelerometers were used to measure the forward acceleration, lateral acceleration, and vertical acceleration of the vehicle, respectively. The measurement information was complete and could be used for a navigation solution with the standard strap-down inertial navigation algorithm. After navigation calculation, the system obtained basic navigation information of that time, such as position, velocity, and attitude. At the same time, two different odometers' velocities, containing lever arm information, were used to carry out the dead-reckoning calculation, as well as to calculate the azimuth.
The differences of the velocity and azimuth information between the SINS and couple odometers were taken as the observation data to estimate the state vector of Kalman filter (KF). The error estimations acquired from the Kalman filter would be used to compensate for basic navigation results, including attitude, velocity, and position error estimations. The schematic of the proposed integrated method is shown in Figure 2.

Odometer Velocity Error Model with Lever Arm Effect
There are two matching models for the integration of SINS and odometer. One is the velocitymatching model, and the other is the position-matching model [16,17]. With the proposed method, the odometer was adopted in the velocity matching model to measure the reference velocity of land vehicle moving. However, the real output of the odometer is a series of pulses corresponding to the

Odometer Velocity Error Model with Lever Arm Effect
There are two matching models for the integration of SINS and odometer. One is the velocity-matching model, and the other is the position-matching model [16,17]. With the proposed method, the odometer was adopted in the velocity matching model to measure the reference velocity of land vehicle moving. However, the real output of the odometer is a series of pulses corresponding to the mileage that the vehicle traveled. While the odometer scale factor K D was given, the odometer pulse number N could deduce the mileage increment S; furthermore, the average speed could be calculated with the mileage increment S and the time interval ∆T. The real output model of the odometer could be written as follows: where N denotes the pulse number of the odometer in ∆T, and S represents the mileage increment during the time interval. The odometer ideal scale factor was denoted by K D , which was calculated by the wheel perimeter, and the odometer white noise was denoted by W D . The velocity of the land vehicle in the b-frame V b D could be expressed as: For the purpose of carrying on the integrated navigation, the velocity in the b frame of the land vehicle needed to be converted into n frame with the attitude of the vehicle. As a result, combined with the transition matrix C n b computed by SINS, the velocity of the land vehicle in n-frame V n D could be expressed as: Then, considering (4), (6) could be expressed as: where and v DE , v DN , v DU represent the velocities of the land vehicle in the n-frame.
Considering the scale factor error δK D of odometer, the measured velocity of land vehicle V b D originated in the true velocity V b D , which could be expressed as: Due to the misalignment angle error ϕ and the lever arm effect, the measured velocity in n-frame of the land vehicle could be expressed as: Ignoring the second-order quantities in the expansion, the right side of (9) could be expressed as: Thus, the velocity error of the odometer δV n D could be expressed as: where δV n D = δv DE δv DN δv DU T and δv DE , δv DN , δv DU are the velocity errors of the land vehicle in the n-frame.

Odometer Azimuth Error Model
In the proposed method, a couple of odometers were applied to measuring the reference azimuth of a land vehicle turning on the ground. When the land vehicle turned in traveling, two rear wheels would move forward different distances, and the couple odometers, which were mounted on the rare wheels, would output two different mileages increment S D1 and S D2 separately, as shown in Figure 3. The distance of two rare wheels D D had been precisely measured before. Then, the azimuth model of the odometer could be written as the following equation: where S D1 and S D2 are the mileage increments of the land vehicle during the sampling time dt. Use D D to represent the distance of two rear wheels.
Sensors 2020, 20, x FOR PEER REVIEW 6 of 13 where SD1 and SD2 are the mileage increments of the land vehicle during the sampling time dt. Use Thus, the azimuth error D δψ could be written as follows:

SINS Error Model
The SINS employs accelerometers and gyros to measure the acceleration and angular rate of a land vehicle and continuously provides the position, attitude, and velocity information of the vehicle with the principle of strap-down inertial navigation. Nevertheless, there are serval unavoidable errors in SINS, such as inertial sensor errors, structure errors, installation errors, various disturbances, and initial condition of the system. The navigation accuracy of the SINS would be depressed by these unavoidable errors.
The navigation error propagation equations of SINS have been studied many times [18][19][20][21]. So, the error equations in n-frame of the SINS could be expressed as (15): Considering the actual velocity measured by the odometer V b D , the actual azimuth measured by odometer ψ D could be written as follows: Thus, the azimuth error δψ D could be written as follows:

SINS Error Model
The SINS employs accelerometers and gyros to measure the acceleration and angular rate of a land vehicle and continuously provides the position, attitude, and velocity information of the vehicle with the principle of strap-down inertial navigation. Nevertheless, there are serval unavoidable errors in SINS, such as inertial sensor errors, structure errors, installation errors, various disturbances, and initial condition of the system. The navigation accuracy of the SINS would be depressed by these unavoidable errors.
The navigation error propagation equations of SINS have been studied many times [18][19][20][21]. So, the error equations in n-frame of the SINS could be expressed as (15): cos γ cos ψ + sin γ sin ψ sin θ sin ψ cos θ sin γ cos ψ − cos γ sin ψ sin θ − cos γ sin ψ + sin γ cos ψ sin θ cos ψ cos θ − sin γ sin ψ − cos γ cos ψ sin θ − sin γ cos θ sin θ cos γ cos θ a 11 a 12 a 13 a 21 a 22 a 23 a 31 a 32 a 33 where a ij (i/j = 1, 2, 3) represents the element in the transformation matrix C n b separately. In n-frame, δv n is the velocity error vector. ϕ is the misalignment angle vector, and δλ, δL, δh, respectively, denote the longitude error, latitude error, and height error. ∇ b and ε b , respectively, denote accelerometer biases and gyro drifts in b-frame. R M and R N separately represent the meridian and the prime vertical radius of the earth.

State Equation of the System
Based on the error equations of SINS and odometer, SINS errors and two odometers' errors, including scale factor error and lever arm errors, were selected as state variables. The state equation of the proposed integrated method could be established as: .
where F is the system transformation matrix, which could be obtained according to (15). G represents the transformation matrix of the system noise. Use W(t) to represent the white noise of the system. The 22-dimension state vector of the integrated system X(t) could be expressed as: W(t) = w gx w gy w gz w ax w ay w az 0 1×16 T (19) when the vehicle is driving normally on the ground, the inertial sensor errors, such as accelerometer biases ∇ and FOG drifts ε, could be reduced to the sum of random noise and deviation. Biases and drifts could be regarded as the constant in a short time: Besides the SINS errors, the scale factor error of the odometer is another important error of the integrated system. The outputs of odometers are usually affected by systemic or non-systematic factors, such as resolution, limited sampling rate, the wheels' sliding or idling, passing through objects on the road, tire inflation [22]. Before use, the scale factor of the odometer is usually calibrated, and the value of calibration would not change in a short time. Consequently, during working hours, the scale factor error could be considered as a constant: δ Similarly, in the practical situation, the SINS and the couple odometers are firmly installed on the land vehicle, and the relative lever arm parameters of the SINS and odometers would not change during the land vehicle traveling. Therefore, the lever arm parameters could be defined as constants during the working time:

Measurement Equation of the System
In the integrated navigation system, the couple odometers provide two different velocity information and azimuth information of the land vehicle. The differences between the output acquired from the SINS and the couple odometers are chosen as an observation vector of the integrated navigation system: which could be expressed as: Taking errors that the SINS and odometers have into consideration, (24) could be expressed as: where substituting (11) and (14) into (25), and (25) could be extended as follows: As a result, the measurement equation of the integrated system could be described by: where H represents the measurement matrix. Z(t) is the measurement of the system, and V(t) is the Gaussian white noise sequence. The matrix H could be presented by the following equation:

Simulation and Result Analysis
In order to verify the effectiveness of the proposed integrated method, two integrated navigation methods would be implemented for comparison: one was the proposed integrated navigation system of couple odometers and FOG SINS presented in Section 2, and the other was the traditional integrated navigation system of single odometer and FOG SINS.
Simulations were carried out with MATLAB software, and all parameters used in simulations coincided with the actual SINS and couple odometers integrated system, based on FOGs and accelerometers, which are depicted in Table 1. In addition, the initial positions of the two systems were 116.34682 • E, 39.97885 • N, and the operating frequency of the integrated navigation system was 100 Hz. The initial alignment results of two integrated navigation methods were exactly alike since the same FOG SINS and the same initial alignment algorithm were applied in two integrated systems. In FOG SINS, the initial alignment errors were mainly caused by constant inertial sensor errors, such as gyroscope drifts and accelerometer biases. According to the device precision of the inertial sensors in Table 1, the initial misalignment results of two integrated navigation systems could be set, as shown in Table 2. Applying the parameters described above, two integrated navigation systems were implanted with the trajectory, as described in Table 3. Figure 4 illustrates the procedure of simulations. The maximum speed of the vehicle was 20 m/s, and the entire trajectory was approximately 95 km.   The percentage of the horizontal position error δP was usually adopted to represent the precision of the horizontal position of the land vehicle, which was calculated using the following formula: For simplicity, only a few typical results are shown in Figure 5 to describe attitude errors, position errors, and velocity errors, rather than all simulation results. The percentage of the horizontal position error δP was usually adopted to represent the precision of the horizontal position of the land vehicle, which was calculated using the following formula: where δP N and δP E , respectively, represent horizontal position errors in the direction of north and east. D is the current mileage of the land vehicle. For simplicity, only a few typical results are shown in Figure 5 to describe attitude errors, position errors, and velocity errors, rather than all simulation results.
It could be known from Figure 5a-d that the P E error of the proposed integrated method was no more than 1 m, and the V E error was within 0.02 m/s throughout the entire trajectory. But in the traditional method, the V E error was divergent with time to the maximum of 0.45 m/s, and the P E error was about ±40 m. Besides, when the land vehicle turned, both V E and P E of the traditional method had obvious errors. The reason was that without considering the lever arm effect, the traditional integrated method could not estimate and compensate for the errors with two inconsistent velocity measurement information. In addition, as could be seen from Figure 5e,f, both roll and pitch errors of the proposed integrated method had good convergence and did not exceed ±2". Nevertheless, the traditional integrated method had pitch and roll errors of about ±10". During the first turn, the horizontal attitudes had larger errors because of the lever arm effect, and both attitude errors converged after the first turn. Comparisons of the azimuth error and the horizontal position error of two integrated navigation methods are shown in Figure 5g,h, respectively. The results demonstrated that before the first turn, the maximum δP of the traditional method was 0.05% D (the horizontal position error was about 14 m), and the azimuth error was approximately 140". During the first turn, both δP and azimuth error of the traditional method had obvious errors because of the lever arm effect. Then, the average δP and the azimuth error gradually decreased to 0.03% D (the horizontal position error was about 30 m) and ±100" after the first turn. The reason was that the observability of the integrated system became better, and navigation errors were effectively estimated and compensated with the integrated navigation algorithm. The maximum δP of the proposed integrated navigation method was 0.03% D (the horizontal position error was about 7 m) before the first turn, as well as the azimuth error quickly converged to less than ±30". After the first turn, the average δP further decreased to 0.01% D (the horizontal position error was about 11 m). Moreover, the accuracy of the horizontal position had further improvement after the second and the third turns. Consequently, compared with the traditional integrated method, the positioning and orientation accuracy of the proposed integrated navigation method was greatly improved.

Conclusions
This paper proposed a new integrated navigation method based on couple odometers and FOG SINS. Couple odometers were used to measure the velocity and azimuth of a land vehicle. The error model of the SINS/2ODs integrated system was analyzed, and the dead-reckoning algorithm compensating the lever arm effect of two odometers was proposed. At the same time, the velocity differences of two odometers were used to calculate the vehicle's azimuth, which was taken as azimuth measurement information of the integrated navigation system. Then, several long-term navigation simulations were implemented to verify the effectiveness of the proposed integrated navigation method, and the results indicated that the proposed method could be used in different scenarios, and the positioning and orientation accuracy of the proposed method could be markedly improved as compared with the traditional method. Therefore, the proposed integrated navigation method based on FOG SINS and couple odometers could effectively improve not only positioning accuracy but also orientation accuracy of the land vehicle, thus having greater application value.