# In-Motion Iterative Fine Alignment Algorithm for On-Board Vehicular Odometer-Aided SINS.

1. IntroductionStrapdown 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]. Kaygisiz and Sen 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 i is a stationary Earth-centered frame and shares its polar axis but not rotate with it. The Earth frame e is an Earth-centered-Earth-fixed frame whose x-axis points at the intersection of the Greenwich meridian and the equatorial plane z-axis points to the North Pole. The navigation frame n moves with the vehicle about the surface of the Earth. The axes of n point to the direction of east, north, and geodetic latitude (radial). The vehicular body frame b is a fixed frame to the vehicle body. The y-axis is pointing forward along the symmetric axis of the vehicle while the x-axis is orthogonal to the y-axis and points to the right wing, and z-axis completes the righthand 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

[v.sup.b.sub.D] = [[0 [v.sub.D] 0].sup.T], (1)

where [v.sub.D] is the speed of the vehicle measured by OD. The velocity in navigation frame can be expressed by

[v.sup.n.sub.D] = [C.sup.n.sub.b][v.sup.b.sub.D] = [[v.sup.n.sub.DE] [v.sup.n.sub.DN] [v.sup.n.sub.DU]].sup.T], (2)

where [v.sup.n.sub.DE], [v.sup.[n.sub.Dn], and [v.sup.n.sub.DU] are the components along the three axes of navigation frame.

Thus, the position differential equations for DR are modeled as follows [10]:

[mathematical expression not reproducible], (3)

where L, [lambda], and h are the longitude, latitude, and height, respectively, and [R.sub.M] is the prime radius of curvature while [R.sub.N] is the meridian radius of curvature.

By assuming the updating period of OD is [T.sub.D] during which N samples are available, the iterative updating algorithm can be governed by [10]

[mathematical expression not reproducible]. (4)

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

[??] = 0 (5)

Then, the attitude matrix approximates to the following expression

[[??].sup.n.sub.b] = [C'.sup.n.sub.n] [C.sup.n.sub.b] = (I-[phi] x) [C.sup.n.sub.b], (6)

where [phi] denotes the vector [[0 0 [phi]].sup.T] while [*] x 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 [delta][K.sub.D] 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

[mathematical expression not reproducible], (7)

where [delta][K.sub.Db] denotes the random constant, [delta][K.sub.Dm] is the noise with a correlation time [tau], and [w.sub.Dm] is a Gaussian noise.

2.2.2. Error Model for Dead Reckoning. Defining the velocity measurement error of OD in the body frame is [delta][v.sup.b.sub.D], and then the velocity in the navigation frame can modeled as

[mathematical expression not reproducible]. (8)

Substituting (6) into (8) yields

[[??]V.sup.n.sub.D] = [v.sup.n.sub.D] - [phi] x [v.sup.n.sub.D] + [C.sup.n.sub.b][delta][v.sup.b.sub.D], (9)

and then the velocity error in navigation frame can be obtained

[delta][v.sup.n.sub.D] = [[??].sup.n.sub.D] - [v.sup.n.sub.D] = - [phi] x [v.sup.n.sub.D] + [C.sup.n.sub.b][delta][v.sup.b.sub.D]. (10)

Substituting (1) into (10) produces

[delta][v.sup.n.sub.D] = [C.sup.n.sub.b] [v.sup.b.sub.D] x [phi] + [C.sup.n.sub.b] [v.sup.b.sub.D] * [delta][K.sub.D]. (11)

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

[mathematical expression not reproducible]. (12)

Denoting [delta]p = [[[delta]L, [delta][lambda], [delta]h].sup.T], rephrasing (12) into matrix form, and substituting (11) into it yield

[mathematical expression not reproducible], (13)

where

[mathematical expression not reproducible]. (14)

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 [phi], the scale factor error [delta][K.sub.D], and the error [delta]p of the preknown initial position, among which [delta]p 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,

[mathematical expression not reproducible], (8)

where [[phi].sub.U] is the up-direction attitude error angle, and [delta][S.sup.h] is the horizontal projection of the true trajectory, [mathematical expression not reproducible]. 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, [[phi].sub.U] can be easily derived based on the triangle geometry as follows:

[mathematical expression not reproducible]. (16)

Because the horizontal attitude angel error is usually pretty small, [[phi].sub.U] approximates to the azimuth angle error, that is, [delta][psi] [approximately equal to] [[phi].sub.U].

Next, the scale factor error of OD can be achieved by using the true distance and the DR distance as follows:

[DELTA][K.sub.D] = [absolute value of [DELTA][[??].sup.h]] / [absolute value of [DELTA][S.sup.h]] - 1. (17)

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 [phi] and the scale factor error [delta][K.sub.D].

As shown in Figure 2, [P.sub.i] is the ith truth position, and [P.sub.i] is the output of DR. [P'.sub.i] or [P".sub.i] 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 [P.sub.i] while other map-mapped points will lead to a larger azimuth than. [P.sub.i] . As to descript the problem more compactly, P' is used to denote the map-mapped points of P. Thus, when n map-mapped points are available, the estimated azimuth angle error can be modeled as

[delta][[psi]'.sub.i] = [delta][psi] + [[epsilon].sub.i], (18)

where [[epsilon].sub.i] is random noise which is uniformly distributed in [-c, c]; c is a random constant that depends on the map matching algorithm. Then, n noises satisfy

[n.summation over (i=1)] [[epsilon].sub.i] = 0. (19)

Next, the following equation based on n measurements can be obtained

[delta][bar.[psi]'] = 1/n [n.summation over (i=1)] [delta][[psi]'.sub.i] = 1/n [n.summation over (i=1)] ([delta][psi] + [[epsilon].sub.i] = [delta][bar.[psi]]. 20

Surprisingly, the estimation mean of the azimuth angle error based on n map-mapped points is equal to the result based on one accurate landmark point.

As a result, the estimation algorithm based on the n map-mapped points can be obtained

[mathematical expression not reproducible], (21)

[delta][K.sub.D] [approximately equal to] 1/n [n.summation over (i=1)] [absolute value of [DELTA][[??].sup.h.sub.i]/ [[DELTA].sup.h.sub.i]] - 1. (22)

Till now, the estimation for [[delta].sub.y] and [delta][K.sub.D] 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 [[delta].sub.[psi]] and [delta][K.sub.D] according to (21) and (22) when map-mapped points are available.

(iii) Use [delta][psi] to modify the initial attitude matrix and use [delta][K.sub.D] 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 [delta][psi] and [delta][K.sub.D] 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[degrees]/h while the intensity of the random noise is selected as 0.02[degrees]/h. The bias of the accelerometer is 2x [10.sup.-4] g whilst the intensity of the random noise is 1x [10.sup.-4]g. The scale factor error of the OD is 0.3%. Moreover, the initial attitude error angles are 0.7' and 0.5' for horizontal and 30' 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 2m 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 [delta][[??].sub.D] is 0.0022 according to (22), which is about 26.7% different to the nominal scale factor error. The estimation for [delta][psi] is about 26.170 according to (21) while the after-correction azimuth angle error is 4.634 . One thing should be pointed out is that the gyro drifts about 0.83' at the end of 500 s.

The results for case 2 are presented in Figure 6, where estimations for [delta][[??].sub.D] and [delta][psi] are 0.0021 and 25.898', respectively. The estimation error of [delta][[??].sub.D] is about 22%. The aftercorrection azimuth angle error is 4.905'. 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 ([approximately equal to] 1.029') error. Additionally, 0.83' 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 [delta][[??].sub.D] and [delta][psi] are 0.0023 and 26.121', respectively. The estimation error of [delta][[??].sub.D] is about 23% with respect to the nominal value. The after-correction azimuth angle error is 4.680'. 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 [delta][[??].sub.D] and [delta][psi] again when it comes to the epoch 500 s. The final azimuth error is about 0.809' (no more than 50").

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 l \ 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.

https://doi.org/10.1155/2018/7368018

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).

References

[1] J. G. Park, J. G. Lee, and C. G. Park, "SDINS/GPS in-flight alignment using GPS carrier phase rate," GPS Solutions, vol. 8, no. 2, 2004.

[2] W. Li, J. Wang, L. Lu, and W. Wu, "A novel scheme for DVL-aided SINS in-motion alignment using UKF techniques," Sensors, vol. 13, no. 1, pp. 1046-1063, 2013.

[3] Z. Wan, G. Yang, Y. Liu, and X. Bai, "Study on vehicular SINS in-moving alignment algorithm," in 2016 3rd International Conference on Information Science and Control Engineering (ICISCE), pp. 1149-1153, Beijing, China, July 2016.

[4] R. Bimal and J. Ashok, "In-motion alignment of inertial navigation system with Doppler speed measurements," in AIAA Guidance, Navigation, and Control Conference, Kissimmee, FL, USA, January 2015.

[5] C. Zhang, L. Ran, and L. Song, "Fast alignment of SINS for marching vehicles based on multi-vectors of velocity aided by GPS and odometer," Sensors, vol. 18, no. 2, p. 137, 2018.

[6] X. Cui, C. Mei, Y. Qin, G. Yan, and Q. Fu, "In-motion alignment for low-cost SINS/GPS under random misalignment angles," Journal of Navigation, vol. 70, no. 6, pp. 1224-1240, 2017.

[7] B. H. Kaygisiz and B. Sen, "In-motion alignment of a low-cost GPS/INS under large heading error," Journal of Navigation, vol. 68, no. 2, pp. 355-366, 2015.

[8] P. M. G. Silson, "Coarse alignment of a ship's strapdown inertial attitude reference system using velocity loci," IEEE Transactions on Instrumentation and Measurement, vol. 60, no. 6, pp. 1930-1941,2011.

[9] Y. G. Wang, J. S. Yang, Y. Y, and Y. L. Lei, "On-the-move alignment for SINS based on odometer aiding," System Engineering Electronics, vol. 35, pp. 1060-1063, 2013.

[10] G. Yan, Study on the vehicular autonomous position and orientation determination system, Northwestern Polytechnical University, Xi'an, China, 2006, [Ph.D. Thesis].

[11] E. Bekir, Introduction to Modern Navigation Systems, World Scientific Publishing Co, 2007.

[12] C. Jekeli, Inertial Navigation Systems with Geodetic Applications, Walter de Gruyter GmbH & Co, 2000.

[13] J. Zhang, Research on the performance upgrading of high-resolution photoelectric odometer, Northwestern Polytechnical University, Xi'an, China, 2009, [Ph.D. Thesis].

[14] B. C. Gong, J. J. Luo, S. L. Li, and X. Li, "Novel map-matching algorithm based on moving-related least squares," Journal of Chinese Inertial Technology, vol. 20, no. 4, pp. 435-429, 2012.

Baichun Gong, (1,2) Chenglong He, (2) Xiaoyue Wang, (3) and Xin Li (4)

(1) Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China

(2) State Key Laboratory of Satellite Navigation System and Equipment Technology, Shijiazhuang 050081, China

(3) No. 454 Hospital of PLA, Nanjing 210002, China

(4) Institute of Air Combat, Naval Research Academy, Shanghai 200436, China

Correspondence should be addressed to Baichun Gong; baichun.gong@nuaa.edu.cn

Received 18 February 2018; Accepted 4 June 2018; Published 31 July 2018

Academic Editor: Giuseppe Maruccio

Caption: FIGURE 1: The similarity between DR trajectory and true trajectory.

Caption: FIGURE 2: Distribution of the azimuth error angle.

Caption: FIGURE 3: Nominal trajectory of the vehicle.

Caption: FIGURE 4: Nominal azimuth of the vehicle.

Caption: FIGURE 5: Attitude angle error for case 1.

Caption: FIGURE 6: Attitude angle error for case 2.

Caption: FIGURE 7: Attitude angle error for case 3.

Caption: FIGURE 8: Attitude angle error for case 4.

TABLE 1: The motions of the vehicle. Phase Motion Start time (s) Period (s) 1 Station keeping 0 10 2 Uniform acceleration motion 10 10 3 Uniform motion 20 30 4 Turn left 50 20 5 Uniform motion 70 100 6 Turn right 170 10 7 Uniform motion 180 90 8 Turn left 270 30 9 Turn right 300 300 Phase Angular Acceleration Initial speed acceleration (m/[s.sup.2]) (m/[s.sup.2]) ([degrees]/s) 1 0 0 0 2 0 2 0 3 0 0 20 4 -3 0 20 5 0 0 20 6 4 0 20 7 0 0 20 8 -0.5 0 20 9 0.09 0 20 TABLE 2: Simulation cases. Cases Position uncertainty (1) Alignment with one landmark point 0m (2) Alignment with one landmark point 2m (3) Alignment with landmark points from MM 2m (4) Iterative alignment with landmark points from MM 2m TABLE 3: Estimation error for [delta][psi] and [delta][K.sub.D]. [delta][psi] [delta][K.sub.D] (angle min) Case 1 4.634 26.7% Case 2 4.905 22% Case 3 4.680 23% Case 4 0.809 0.31%

Printer friendly Cite/link Email Feedback | |

Title Annotation: | Research Article |
---|---|

Author: | Gong, Baichun; He, Chenglong; Wang, Xiaoyue; Li, Xin |

Publication: | Journal of Sensors |

Geographic Code: | 9CHIN |

Date: | Jan 1, 2018 |

Words: | 4073 |

Previous Article: | A Preferable Airborne Integrated Navigation Method Based on INS and GPS. |

Next Article: | Corrigendum to "The State-of-the-Art of Knowledge-Intensive Agriculture: A Review on Applied Sensing Systems and Data Analytics". |

Topics: |