Abstract
It is a challenge to achieve high accuracy navigation for land vehicles without the aid of global navigation satellite systems (GNSS). Inertial measurement unit (IMU) and odometer (OD) are widely deployed due to their complementary properties. In this paper, SINS/OD integrated navigation algorithm based on body frame position increment is studied to improve the navigation performance. Taking the calibration errors of odometer scale factor, IMU installation angle, and lever arm into consideration, the odometer measurement model is derived. Then measurement equations based on body frame position increment are proposed to overcome the amplified random errors in the traditional velocity observation approach. Odometer fault detection and exception are conducted based on residual detection method, with the nonholonomic constraints of land vehicles applied to mitigate the standalone SINS error drift. Long distance real test is carried out using laser gyro SINS to assess the proposed algorithm, which shows that navigation performance can be effectively improved.
1. Introduction
Land vehicle navigation has been widely used in areas such as mining, agriculture, cargo handling, and borehole surveying [1]. Strapdown inertial navigation system (SINS) is always employed in these applications because it can provide all 3D attitude, velocity, and position information of the vehicles with high bandwidth and high reliability. As is known, the error drift of SINS accumulates rapidly with no bound due to the integration of noise-contaminated inertial measurements, so auxiliary information from other sensors is always introduced to mitigate the error drift. Nowadays, global navigation satellite system (GNSS) receivers are the most commonly used sensors because of their high accuracy and low cost [2–4]. The performance of SINS/GNSS integrated navigation system relies heavily on the information of GNSS. The GNSS signal, however, is prone to blockage due to shelters such as buildings, tunnels, and trees, as well as to jamming and spoofing around critical sites or in hostile environment [5–9]. In these GNSS denied scenarios, the navigation performance decays rapidly. What is worse, the position given by the system may be totally wrong when the received GNSS signal is jammed, which is disastrous for special applications like military vehicles.
Several approaches have been developed for the control of standalone SINS navigation error drift during GNSS outages, for example, zero-velocity updates (ZUPT) [10–12] and nonholonomic constraints (NHC) of land vehicles [1, 13–15]. The ZUPT approach, if frequently implemented, will restrict the manoeuvrability of vehicles; if not, satisfactory performance cannot be reached. The NHC method takes advantage of the a priori information that velocity components in the upward and transverse axes are zero for land vehicles and can always be implemented on condition that the vehicle runs normally. The upsetting fact is that the observability of NHC method is limited due to lack of forward velocity measurement [16, 17]. Above all, long distance/time self-contained high-precision navigation for land vehicles without the aid of GNSS is still a challenge up to now.
Relative sensors such us odometers (OD) and Doppler velocity log (DVL) can be used to measure the forward speed of vehicles. DVL is often used in marine vehicles to measure velocity relative to sea bottom or ocean current [18–20], while OD is widely equipped in modern land vehicles [8, 16, 17, 21, 22]. OD is cost-effective and the accuracy of velocity, different from SINS, does not degrade with time. The integration of SINS and OD is totally self-contained for both SINS and odometer neither receiving nor emitting signals, which contributes a lot to the anti-interference and concealment ability of vehicles and is especially preferred in military applications. In [8], OD and NHC are combined to provide 3D velocity for the control of SINS error drift during short-term GPS outages. In [21], a versatile strategy, comprising two coupled subproblems (i.e., self-calibration and positioning, in-motion alignment, and positioning), is suggested for SINS/OD integrated navigation. The pulse output of OD is used to calculate the forward speed of land vehicles by dividing the sampling interval. Since the sampling interval is very small, the velocity calculation process significantly amplifies random errors. Furthermore, large OD measurement errors may occur due to skid caused by bad road conditions such as soft or icy land surfaces. To solve these problems, a novel scheme based on body frame position increment is recommended in this paper. The contributions are twofold. Firstly, measurement equations based on position increment are derived, which can overcome the drawback of amplified random errors in velocity measurements. Secondly, a χ2 distribution based fault detection and exclusion (FDE) method is introduced to test and isolate large OD errors mentioned beforehand, and NHC method is applied to limit the SINS error drift during OD unavailable periods. The proposed algorithm is evaluated by a long distance field test.
The rest contents are arranged as follows. OD velocity measurement model is derived in Section 2 considering the calibration errors of OD scale factor, IMU installation angles, and OD lever arm, and the random error amplification effect is illustrated. Section 3 deals with the proposed integration algorithm for SINS and OD, where the measurement equations based on body frame position increment are constructed and residual chi-square detection method is adopted for FDE. Field test results are presented in Section 4 to verify the recommended algorithm and conclusions are drawn in Section 5.
2. Velocity Measurement Model of Odometer
Denote by frame the IMU coordinate system, its origin at the center of IMU, and its three axes pointing to the right, forward, and upward of IMU, respectively. The vehicle body frame is referred to as frame, whose three axes point to the right, forward, and upward, respectively. Choose geographical coordinate system (also called East-North-Up frame) as the navigation frame (denoted by frame). The configuration of IMU and OD on a land vehicle is shown in Figure 1, where is the IMU center, represents the OD measurement point, and vector stands for the lever arm of point relative to point (called OD lever arm afterwards).
According to the working principle of the OD applied, it outputs digital pulses proportional to the mileage increment within the sampling interval. Denoting the forward speed of point by , we havewhere is OD scale factor, is OD output, refers to the mileage increment, and denotes the sampling interval.
Denote the ground velocity of point by . When a vehicle maneuvers normally, its skyward and lateral velocities are zero, which giveswhere the superscript refers to coordinate components expressed in the frame.
Combing (2) and (3) yields velocity of point expressed in frame:where and can be seen as augmented position increment measured by OD.
Considering the error in OD scale factor, we havewhere refers to the OD scale factor error.
Substituting (5) into (4), we obtain
Denoting the ground speed vector of point as , according to Figure 1, the relationship between and satisfieswhere is the rotation rate of frame with respect to Earth frame ( frame) and is the velocity caused by OD lever arm .
It can be obtained from (7) thatwhere refers to the ground speed of point calculated from OD outputs; is the calibrated value of direct cosine matrix (DCM) from frame to frame, which, determined by the installation configuration of IMU, can be referred to as IMU installation matrix; is the calibrated value of OD lever arm expressed in frame; is the angular rate of frame with respect to frame expressed in frame and is determined with SINS outputs.
Denote by the misalignment angle between and ; is defined to meet
Substituting (6) and (9) into (8) yieldswhere is OD lever arm error and is the error of .
Considering the calculation process of , we obtainwhere is the rate of frame with respect to inertial frame measured by the gyro assembly in IMU, is the gyro drifts expressed in frame, and is the DCM from frame to frame determined from SINS outputs, and refers to the rate of Earth expressed in frame, and is the self-rotation rate of Earth.
According to the relationship between frame and frame, it yieldswhere .
Considering the definition of attitude misalignment angle in SINS, we getwhere stands for the attitude misalignment angle of SINS.
Substituting (12) and (13) while retaining the first-order approximation only, (11) can be rewritten as
Then OD measurement model can be obtained by substituting (14) into (10), that is,
Conventionally, measurement equations are established by taking the difference between and . In this scheme, the calibration errors , , and can be augmented as system states to be estimated online. However, it will amplify the random error in the process of calculating . To illustrate this phenomenon, a set of ten minutes forward speed data obtained from (2) is illustrated in Figure 2, the corresponding mileage increment obtained from (1) is shown in Figure 3, and the sampling interval is 5 ms.
It can be clearly seen from Figures 2 and 3 that the random variation amplitude of forward speed velocity is about 1 m/s while that of mileage increment is about 0.005 m. In a word, the measurement noise is enlarged as expected. The enlarged random noise deteriorates the quality of observations and thus degrades the performance of Kalman filter. Noting that the noise amplification effect originates from the division operation in (2), a natural thought is to construct measurement equations directly from (1). Section 3 deals with this approach in detail.
3. SINS/OD Integrated Navigation Algorithm Design
3.1. Measurement Equations Based on Body Frame Position Increment
The velocity output of SINS is the calculated values of frame coordinated , namely, . Converting it into frame, we have
Integrating (16) from to yieldswhere is the body frame position increment calculated from SINS outputs.
Similarly, we can obtain the body frame position increment calculated from OD outputs by integrating (15) from to , that is,
Defining observation variables as the difference between and , we have
Substituting (17) and (18), (19) can be rewritten aswith
Noting that
Equation (22) shows that measurement variable is irrelevant to the -axis component of IMU installation matrix error, so we omit it for brevity.
Substituting (22) into (20), we obtainwhere , .
Equation (23) is the new measurement equations derived. It can be seen that there is no need to calculate the forward velocity from OD outputs any more. Here, the frame expressed position increment rather than the frame expressed counterpart is applied for convenience of OD FDE, which is carefully investigated in Section 3.3.
3.2. Kalman Filter for Integrated Navigation
The error dynamic equations of SINS can be expressed as follows [23]:where is the attitude misalignment angle of SINS, the velocity error, the position error, the gyro constant drift expressed in frame, the accelerometer zero deviation expressed in frame, the gyro noise expressed in frame, and the accelerometer noise expressed in frame; the derivation and expressions of matrices in (24) can be found in [23].
Error terms , , , and can be treated as random constant states, that is,
Combining (24) and (25), we can obtain the 21-dimensional augmented state equations of Kalman filter, with (23) being the measurement equations. The Kalman filter equations can be rewritten as follows:where is the state vector, is the measurement vector, is the process noise, and is the measurement noise.
Equation (26) is the continuous form of the system equations. To conduct the Kalman filter calculation procedures, the discrete form of (26) should be derived. Let the update interval of Kalman filter be ; we have the following [24]:where the subscript denotes values at , , , , , , , .
Note that the calibration errors ( and ) are included in the system states and can be estimated online. This is especially important considering the fact that the calibration of IMU installation matrix and OD lever arm is not an easy thing in most applications; that is, it is not convenient to obtain and before the integrated navigation implementation, if not impossible. In these cases, the IMU can always be mounted to keep the misalignment between frame and frame a small quantity, as a result, can be treated as a unit matrix directly without the calibration procedure. As for OD lever arm, it is usually adequate to view as zero. Furthermore, even if and can be preobtained in some way, the corresponding true values may vary significantly due to load changing and ambient temperature, which necessitates the online estimation ability, too.
3.3. OD Fault Detection and Exception
The measurement model of OD is established on the basis that OD works normally and the vehicle runs without skidding or jumping. On condition that the above-mentioned faults occur, the established OD measurement model cannot correctly reflect the true speed of the vehicle. The error-contaminated speed measurements will pollute the whole system and reduce the precision. So it necessitates the function of FDE in the integrated system. Conventionally, the IMU is of high reliability and considered with no failure in the navigation process. The fault of OD is mainly caused by harsh road conditions. For example, if a car runs on a muddy, snowy, or icy road, forward and (or) side slip may occur, making the measured forward speed larger or lesser than the real value and the lateral zero-velocity assumption incorrect. In addition, the upward zero-velocity constraint may not be satisfied on rugged roads.
To sum up, the failures can be categorized into two types, namely, the forward speed measurement error (Type I) and the invalidation of nonholonomic constraints (Type II). If only Type I fault occurs, NHC can still be used to mitigate the SINS error drift. So we adopt a two-stage fault detection scheme here. Firstly, calculate a fault detection function with all the filter residuals to determine if some fault exists; if so, transfer to the second stage by dealing with a new function with the NHC related residuals to determine if the motion constraints are met. Once Type II fault is detected in the second stage, the measurement update procedure of Kalman filter should be avoided; otherwise, the measurement update procedure can be implemented based on NHC information.
Among the many fault detection methods, residual detection method has been widely used due to its low calculation cost and high sensitivity to residual change [24, 25]. To perform the detection method, a detection function should be formed using filter residual information. Define the detection function for stage one aswhere is the filter residual and is the covariance matrix of the filter residual .
When the system is faultless, . Once some failure appears, the statistic characteristic of no longer holds. So a binary hypothesis can be made for :where is the null hypothesis and denotes that the system is faultless and is the alternative hypothesis and indicates that certain fault occurs.
If is true, obeys the distribution of 3 degrees of freedom, namely, . Then the fault detection criteria can be set aswhere the detection threshold is determined by the false alarm rate .
Once is chosen, can be worked out by solving the corresponding upper quintile of . Under normal working conditions, the probability that is less than , which can be regarded as a small probability event. On the other hand, if should happen, it indicates that the system is working abnormally.
If is true, we introduce a new detection function to determine whether the vehicle motion constraints are satisfied (stage two). The detection function is defined aswhere and is the covariance matrix of and can be obtained by deleting the second row and the second column of .
Similar to the first stage, the detection threshold is the upper quintile of . If , it indicates that the motion constraint condition of vehicles is not satisfied, then the measurement update of Kalman filter should be avoided. If , it indicates that the constraint condition of vehicles is satisfied, then let , , and the system is put into the motion constrained navigation mode. It should be noted that once the system is transferred into vehicle motion constraint aided navigation mode, cannot be obtained due to OD failure, making it impossible to determine with (22). The solution of this problem is to calculate using the following:
A summary of the two-stage FDE is illustrated in Figure 4.
It is clear that the faults of odometer can be easily isolated just by avoiding measurement update of the Kalman filter. The significant error increase during the fault-existing period can be mitigated by taking advantage of the NHC information. When the faults disappear, the integration of SINS and OD can be successfully implemented with new measurements.
4. Field Test Results
To evaluate the proposed algorithm, a field test is carried out using the laser gyroscope SINS. The test system is composed of a car, an IMU consisting of a laser gyro triad and an accelerator triad, an odometer, and a GPS receiver. The bias stability of the laser gyro is 0.005°/h; the zero offset of the accelerometer is 50 μg; the positioning accuracy of the GPS receiver is 5 m. The SINS and GPS outputs are loosely integrated to supply the reference for measuring the errors of the SINS/OD integrated navigation system. The trajectory of the vehicle is shown in Figure 5, where the red point represents the start point and the red triangle stands for the end point; the entire test period lasts 153 min, including 10 min initial alignment and 143 min maneuver, and the total travel mileage is about 210 km.
Figures 6–8 present the navigation errors of the traditional velocity measurement approach and the body frame position increment measurement approach proposed in this paper. It can be seen from Figure 6 that the attitude errors of the proposed method converge faster than the traditional method and the accuracy is higher. Also, the new method is less prone to external disturbances. Similar conclusions can be made for velocity (see Figure 7) and position errors (see Figure 8). From Figures 7 and 8 it can be found that the performance improvement in the height channel is not significant comparing to that in the horizontal channel. The reason is that the pitch and roll angle of the vehicle is always small (see Figure 9) so random error of velocity in the height channel is minor as it is mainly determined by the upward zero-speed condition of the vehicle, while the velocity in horizontal channel is mainly affected by the speed measured by OD, which, as mentioned above, amplifies the effects of random fluctuations. In addition, the velocity errors do not increase with time because of the drift mitigation effect of OD measurements, while the position errors still grow slowly with time due to the lack of absolute position measurements. To better reflect the performance of SINS/OD integration, the position errors with respect to the travel mileage is presented in Figure 10. Once again, it is clearly seen that the proposed position increments observation method significantly reduces the horizontal position error compared with the speed observation method, yet the improvement of height error is not obvious, just as expected. The values of fault detection function are shown in Figure 11, wherein the false alarm rate is equal to 0.01 and the corresponding detection threshold is 11.34. It can be seen that, during the entire test process, is less than TD, indicating that the system works properly with no OD measurement failure, so the second stage of FDE is not implemented and the values of are not shown here.
To further verify the FDE scheme, OD failures are intentionally added. The OD outputs during 1000 s~1005 s are increased while the OD outputs from 4000 s~4010 s decreased to simulate different kinds of vehicle slipping. When the OD faults are inserted, the values of fault detection function are shown in Figure 12. It can be noted that is larger than during the fault-existing periods, which means that the faults are correctly detected. During the periods when OD faults exist, the statistic value is always less than the threshold (9.21), indicating the availability of vehicle motion constraint information. The navigation results are shown in Figures 13–16, where “FDE applied” denotes the results from the proposed navigation method while “no FDE” denotes the results from the position increment measurement approach without FDE. In Figure 13, the attitude errors suffer from an obvious increase during 1000 s~1005 s and 4000 s~4010 s in the “no FDE” method, while there is no such an error increase in the “FDE applied” way; that is, the OD faults have been successfully detected and isolated. Similar remarks can be made on the velocity errors (see Figure 14) and position errors (see Figures 15 and 16), too. From Figures 14 and 15, we can see a convergence of the attitude and velocity errors when OD faults disappear since they are observable in the position increments observation approach. The position errors, however, diverge with time for they are not observable. Moreover, the effect of OD faults on height errors is not significant. The reason is that the projection of forward speed on the upward is limited due to the small pitch and roll angles, just as mentioned above.
5. Conclusions
Traditionally, the outputs of SINS and OD are integrated in velocity observation approach, in which the OD outputs, together with the NHC information, are utilized to provide velocity measurements. To overcome adverse effect caused by the amplified random errors, a novel approach based on body frame position increment is presented. Measurement equations are derived with the consideration of the calibration errors of OD scale factor, IMU installation angles, and IMU/OD lever arm. To relieve the effect of OD failures due to severe road conditions, FDE of the integrated system, which has not been seriously treated in the current literatures, is discussed and a novel two-stage FDE method is introduced based on the newly deduced measurement equations. It is shown that not only can the drawback of the traditional method be avoided, but also the FDE procedures can be implemented conveniently. The effectiveness of the proposed algorithms is verified through the results of a long distance field test. Despite the advantages of the proposed method, the position errors still increase slowly with time/distance, which is inevitable due to the lack of absolute position sources. Consequently, combination of the current system with other sensors, such as electronic maps and altimeters is a topic deserving further investigation.
Data Availability
The data used to support the findings of this study is owned by Xi’an Institute of Hi-Tech and so cannot be made freely available. Access to these data will be considered by the author upon request, with permission of the research management department of Xi’an Institute of Hi-Tech.
Conflicts of Interest
The authors declare that there are no conflicts of interest.
Acknowledgments
This work was supported in part by the National Natural Science Foundation of China (Grants nos. 41404022 and 41174162).