Abstract
This research proposes a novel in-motion fine alignment algorithm for vehicular dead reckoning (DR) with odometer-aided strapdown inertial navigation system (SINS) while the map matching result is used for a group of landmark points to estimate misalignment angles. The proposed algorithm is designed based on principle of similarity, that is, trajectory of DR is similar to the true trajectory that the main difference between these two trajectories is rotation and scale. Further, the results from map matching are introduced as a group of landmark points to estimate the residual of azimuth error angle after coarse alignment and the scale factor error of the odometer. It is theoretically proved that the alignment effectiveness based on the results from map matching is equivalent to that on single zero error landmark point. Finally, digital simulations are conducted to verify the presented algorithm and test the performance.
1. Introduction
Strapdown inertial navigation system (SINS), considered as a dead reckoning system, utilizes the measurements from gyros and accelerometers to calculate the real-time attitude, position, and velocity of the vehicle. Initial alignment based on a preknown initial position directly determines the performance of SINS. Usually, methods for initial alignment are divided into two different types: self-alignment and transfer alignment. As to the autonomous vehicular application, self-alignment is required. Generally speaking, rapidity, accuracy, and autonomy are the basic requirements of the self-alignment methods. Especially rapid self-alignment is one of the key requirements for the circumstances such as military manoeuvers and rescue missions. However, rapidity and accuracy are contradiction to each other for the noise problem, that is, more time will be consumed to achieve an accurate self-alignment. Thus, in-motion alignment scheme is required for rapid response missions.
Many works have been done in field of in-motion alignment. Park et al. presented SINS/GPS alignment by using carrier phase rate information from on-boarded two-antenna GPS receiver [1]. Li et al. developed a DVL- (Doppler velocity log-) aided in-motion alignment algorithm [2]. Wan et al. presented an in-motion alignment method by using the position information from GPS [3]. Bimal and Ashok also studied DVL-aided in-motion alignment scheme [4]. Zhang et al. utilized the measurements from GPS and odometer (OD) to conduct fast alignment [5]. Cui et al. studied in-motion alignment for SINS/GPS under random misalignment [6]. Kaygısız and Şen presented in-motion alignment for a low-cost GPS/INS under large heading error [7]. Silson developed a rapid in-motion alignment method by using GPS position and velocity measurements [8]. Wang et al. proposed an odometer-aided in-motion alignment algorithm [9]. Yan presented an in-motion alignment algorithm for SINS/OD DR system utilizing a known landmark point based on principle of similarity [10].
It can be found that many of them require the information from GPS. However, GPS measurement information is not always available for signal blocking problem. Thus, more scholars put their attentions on the navigation without GPS that the self-alignment is independent to GPS. Inspired by the concept of principle of similarity using a known landmark point [10], a novel iterative in-motion fine alignment algorithm for vehicular DR based on SINS/OD is developed in this paper, where a group of virtual landmark points from the map matching result is used to estimate the azimuth error of SINS and the scale factor error of OD. Benefiting from the novel algorithm, the in-motion fine alignment can be achieved without GPS or even a zero velocity stop.
The formulation of the DR algorithm based on SINS/OD is reviewed firstly, and then positioning error analysis for DR is presented in Section 2. Novel iterative in-motion fine alignment algorithm utilizing a group of noised points from map matching result is presented in Section 3. Digital simulations are presented in Section 4. Conclusions are presented in Section 5.
2. Problem Statements
2.1. Review of Dead Reckoning Algorithm Based on SINS/OD
First of all, the coordinate frames used in this paper are defined as follows [11]. The inertial frame is a stationary Earth-centered frame and shares its polar axis but not rotate with it. The Earth frame is an Earth-centered-Earth-fixed frame whose -axis points at the intersection of the Greenwich meridian and the equatorial plane -axis points to the North Pole. The navigation frame moves with the vehicle about the surface of the Earth. The axes of point to the direction of east, north, and geodetic latitude (radial). The vehicular body frame is a fixed frame to the vehicle body. The -axis is pointing forward along the symmetric axis of the vehicle while the -axis is orthogonal to the -axis and points to the right wing, and -axis completes the right-hand orthogonal set.
It is assumed that the wheels cling to the ground without any side skidding and jumping during moving, the frame of SINS is coincident with the body frame of the vehicle. Then, the vehicle’s velocity in the body frame can be governed by where is the speed of the vehicle measured by OD.
The velocity in navigation frame can be expressed by where , , and are the components along the three axes of navigation frame.
Thus, the position differential equations for DR are modeled as follows [10]: where , , and are the longitude, latitude, and height, respectively, and is the prime radius of curvature while is the meridian radius of curvature.
By assuming the updating period of OD is during which samples are available, the iterative updating algorithm can be governed by [10]
2.2. Position Error Analysis for Dead Reckoning Algorithm Based on SINS/OD
2.2.1. Sensor Error Models
Usually, initial attitude matrix for SINS from its body frame to the navigation frame can be obtained from coarse alignment by taking advantage of double-vector method, with the measurements of gyro and accelerometer [12]. Generally speaking, after a fast coarse alignment, horizontal attitude angle error would be small, but the azimuth angle error could not be well estimated. Thus, the main uncertainty for SINS after coarse alignment is the azimuth angle error. The residual of the azimuth angle error is modeled as a constant
Then, the attitude matrix approximates to the following expression where denotes the vector while stands for the cross-product operation.
Next, the accuracy of OD is another key parameter to the dead reckoning system. The speed accuracy of OD is mainly affected by the status of the road surface, tire inflation, and abrasion [13]. Because it is impossible to precisely model these factors, the scale factor error for OD is considered in this manuscript. The scale factor error is assumed to include two parts: random constant and noise which are modeled as a one-order Markov process where denotes the random constant, is the noise with a correlation time , and is a Gaussian noise.
2.2.2. Error Model for Dead Reckoning
Defining the velocity measurement error of OD in the body frame is , and then the velocity in the navigation frame can modeled as
Substituting (6) into (8) yields and then the velocity error in navigation frame can be obtained
Substituting (1) into (10) produces
Next, by conducting partial differential operation to (3) with respect to the time variable, the position error equations for DR system based on SINS/OD can be achieved
Denoting , rephrasing (12) into matrix form, and substituting (11) into it yield where
3. Iterative In-Motion Fine Alignment Algorithm for DR
As can be seen from (13), the position uncertainty of DR mainly depends on the azimuth error , the scale factor error , and the error of the preknown initial position, among which is usually unknown and can be considered to be pretty small. Thus, in order to achieve a good alignment performance, the azimuth error and scale factor error are required to be accurately estimated.
In [10], an in-motion alignment scheme is proposed where one accurate landmark point is required for the algorithm. However, accurate landmark point is usually unavailable if the vehicle does not pass by the required area. In [14], the experiments have shown that a couple of points can be obtained by map matching algorithm based on SINS/OD/Map database, where there is no requirement of appointed route for the vehicle. Thus, the map-matched points can be considered as a group of virtual landmark points to be used to estimate the misalignment error. In the following parts, the map-matched points are assumed to be available.
3.1. Fine Alignment Method Based on Single Landmark Point
As depicted in Figure 1, the DR trajectory can be considered as the rotating and stretching trajectory of the true trajectory based on the principle of similarity [10], that is, where is the up-direction attitude error angle, and is the horizontal projection of the true trajectory, . It should be emphasized that this conclusion is under the assumptions: (a) azimuth angle error is a small constant; (b) horizontal attitude error can be ignored.
Then, can be easily derived based on the triangle geometry as follows:
Because the horizontal attitude angel error is usually pretty small, approximates to the azimuth angle error, that is, .
Next, the scale factor error of OD can be achieved by using the true distance and the DR distance as follows:
3.2. Iterative Fine Alignment Algorithm Based on the Results from Map Matching
According to the results of map matching experiments in [14], a couple of position points can be obtained through map matching algorithm. These position points are uniformly distributed while the circular error probability is about 3 m. Thus, these map matching position points are utilized as a group of landmark points to estimate the azimuth angle error and the scale factor error .
As shown in Figure 2, is the th truth position, and is the output of DR. or is a potential of the map-mapped points which depend on the map matching result. Some of the map-mapped points used as landmark will lead to a smaller azimuth than while other map-mapped points will lead to a larger azimuth than. . As to descript the problem more compactly, is used to denote the map-mapped points of . Thus, when map-mapped points are available, the estimated azimuth angle error can be modeled as where is random noise which is uniformly distributed in ; is a random constant that depends on the map matching algorithm. Then, noises satisfy
Next, the following equation based on measurements can be obtained
Surprisingly, the estimation mean of the azimuth angle error based on map-mapped points is equal to the result based on one accurate landmark point.
As a result, the estimation algorithm based on the map-mapped points can be obtained
Till now, the estimation for and based on the results of map matching has been derived. However, several assumptions are required to make for the derivation. Therefore, there will still have residual errors after the error compensation by using the estimation results shown in (21) and (22). In order to compensate these residuals, the iterative alignment method is considered in this paper. (i)Do a fast coarse initial alignment for SINS and then drive the vehicle.(ii)Estimate and according to (21) and (22) when map-mapped points are available.(iii)Use to modify the initial attitude matrix and use to update the scale factor of OD.(iv)Redo the calculation of DR with the memorized measurements of SINS and OD, that is, angular velocity, specific force, and speed, and then use the newest DR points to estimate and again.(v)Turn to (iii).
4. Numerical Simulation Results
In order to verify the validity and test the performance of the proposed algorithm, a set of digital simulations have been conducted. The parameters for the motion of the vehicle are presented in Table 1. The motions of station keeping, acceleration, uniform, and turning are mainly included where the nominal trajectory is shown in Figure 3 while the nominal azimuth is depicted in Figure 4.
The random drifting rate of gyro is set to be 0.1°/h while the intensity of the random noise is selected as 0.02°/h. The bias of the accelerometer is whilst the intensity of the random noise is . The scale factor error of the OD is 0.3%. Moreover, the initial attitude error angles are and for horizontal and for azimuth.
Additional, the CEP of MM points is 3 m while the modification is conducted at epoch 500 s. Total simulation period is 600 s while the step is 0.1 s.
As shown in Table 2, four comparison cases are simulated: (1) fine alignment aided by one accurate landmark point, (2) fine alignment aided by one landmark point with a 2 m uncertainty, (3) fine alignment aided by landmark points from map matching with a 2 m uniformly distributed noise, and (4) iterative fine alignment aided by landmark points from map matching with a 2 m uniformly distributed noise.
The alignment results for case 1 are shown in Figure 5. The estimation value for is 0.0022 according to (22), which is about 26.7% different to the nominal scale factor error. The estimation for is about according to (21) while the after-correction azimuth angle error is . One thing should be pointed out is that the gyro drifts about at the end of 500 s.
The results for case 2 are presented in Figure 6, where estimations for and are 0.0021 and , respectively. The estimation error of is about 22%. The after-correction azimuth angle error is . In this case, the vehicle moves 9.7 km for 500 s while the displacement is 7 km. Thus, a 2 m uncertainty would introduce a 2/7000 rad () error. Additionally, gyro drift is the same as case 1.
As shown in Figure 7 are the results for case 3. One thousand map matching points with zero-mean 2 m standard deviation noise during 490 s to 500 s are utilized as the landmark point group. The same as the other cases, the estimation and correction are conducted at epoch 500 s. The estimation values for and are 0.0023 and , respectively. The estimation error of is about 23% with respect to the nominal value. The after-correction azimuth angle error is . As a result, it can be concluded that the fine alignment for azimuth angle based on map matching points group almost shares the same accuracy with that based on one landmark point whose position is accurately known.
As shown in Figure 8 are the results for case 4. The algorithm executed in this case is presented at the end of Subsection 3.2. Firstly, do the same estimation as case 3 and then use the estimation to correct the initial azimuth of SINS and scale factor of OD. After that, use the corrected initial parameters to redo the DR process. Next, estimate and again when it comes to the epoch 500 s. The final azimuth error is about (no more than ).
Further, by comparing Figures 5–8, it can be seen that horizontal attitude angle errors of all cases could not be corrected because the proposed algorithm can only estimate the azimuth angle. Anyway, the residual of horizontal angle errors after coarse alignment is small that these errors drift slowly, that is, the drifting of 600 s is no more than . Moreover, the azimuth errors of four cases have a hopping at epochs 50, 70, 170, and 180 s. The reason for this problem is that these epochs are the starting and ending time of the turning motion which would continually change the azimuth angle. Above all, estimation error of the cases can be summarized in Table 3.
In summary, the proposed iterative fine alignment algorithm can achieve the highest accuracy while the alignment based on map matching points shares the same accuracy level with that based on one zero error landmark point. However, the iterative algorithm requires to store the sensor data for a while and more computational resources to redo the DR calculation. In MATLAB environment, the time consumption for the recalculation is less than 5 s and the storage for the data is no more than 10 M. Therefore, the requirements of the storage and computing time are easy to be satisfied for modern computer technology.
5. Conclusions
This paper developed a novel iterative fine alignment for in-motion vehicle based on the virtual landmark points from the results of map matching. In detail, firstly, the analysis of position error for dead reckoning algorithm based on SINS/OD was conducted. Depending on the result of the error analysis, the azimuth error and scale factor error were chosen to be the estimated variables for fine alignment. Then, the fine alignment method by using single landmark point was reviewed, based on which the novel iterative fine alignment algorithm by utilizing the map matching results as virtual landmark points was developed. Lastly, the accuracy of the proposed algorithm was determined and presented by four comparison simulation cases. As expected, the proposed iterative alignment algorithm could achieve the accuracy level of second of arc. Detailed, the initial attitude error of SINS and the scale factor error could be effectively estimated and compensated based on the proposed algorithm while no requirement of other aided information or zero velocity is required.
Next, the proposed algorithm will be further investigated by in-motion vehicular experiments.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work is supported in part by the State Key Laboratory of Satellite Navigation System and Equipment Technology (CEPNT-2017KF-02) and the Foundation of Science and Technology on Aerospace Flight Dynamics Laboratory (61422100306707).