An in-motion initial alignment method aided by digital map
MAO Yu-liang(毛玉良), HAN Yong-qiang(韩勇强), CHEN Jia-bin(陈家斌)
(School of Automation, Beijing Institute of Technology, Beijing 100081, China)
Abstract: The traditional in-motion initial alignment method obtains the gravity vector in inertial coordinate system twice while parking the land vehicle, and then work out the transformation matrix according to the information obtained. This method sets a limit to the speed and route of vehicle and requires high precision in positioning of coarse alignment. In this work, digital map is introduced into the in-motion alignment. With the aid of map matching algorithm, the positioning accuracy is greatly improved in all speeds and routes, which reduces the effect of misalignment angle from coarse alignment for positioning error and improves the accuracy of the in-motion initial alignment. Road tests results show that the integration of digital map and RLG SINS eliminates the requirements of speeds and routes for land vehicles and greatly reduces the effect of coarse alignment for azimuth accuracy. It not only enhances the mobility of land vehicle but also improves the accuracy of initial alignment of inertial navigation system, which is meaningful in real application.
Key words: in-motion alignment; map matching; digital map
CLC number: TP274.1 Document code: A Article ID: 1672-7207(2011)S1-0519-05
1 Introduction
The initial alignment is the first working stage for the strapdown inertial navigation system (SINS), which provides an initial condition for the SINS to work. There are two forms of initial alignment according to the condition of the base, i.e. stationary-base and in-motion initial alignment. Currently, stationary-base initial alignment is widely used in land SINS and it usually takes 5-10 min to complete alignment with the base remaining stationary. Although the stationary-base initial alignment is of high accuracy, it restricts the rapid response of the system. To overcome this disadvantage, in-motion initial alignment is proposed, which greatly improves the mobility of the vehicle. The inertial coordinate system is used as the reference system during the in-motion alignment. The change of the gravity vector in inertial coordinate system is obtained by stopping the vehicle twice, working out the transformation matrix from the body frame to the navigation coordinate system. Then, the change of position is calculated using azimuth and odometer data, and the alignment error caused by the change of the vehicle position is compensated according to two parking positions[1]. In this scenario, the elements that influence the alignment accuracy include: 1) measurement accuracy of the gravity vector when parking; 2) gyro drift; 3) location error and position compensation when the vehicle is running. As the parking interval gets longer, the influence of measurement accuracy of gravity vector and gyro drift on azimuth become smaller, but the positioning error calculated by the approximate azimuth and odometer will get larger, thus increasing the difficulty of compensating the position deviation. Therefore, it should be guaranteed that there is enough time between two parking, and that the vehicle should not travel a long distance, which sets limits to the speed and routine (such as running curve or closed loop). In view of the above problems, digital map is introduced in this work, using map matching techniques to inhibit the divergence of the positioning error and eliminate the restrictions of the speed and route of the vehicle, thus improves the accuracy of in-motion initial alignment and the ability of rapid response.
2 Structure of system
The scheme consists mainly of the inertial measurement unit (IMU), navigation computer, odometer and upper computer loaded with MapX control and digital map. The IMU is composed of the mechanically dithered RLG and quartz flexible accelerometer, which measure the angular motion and linear motion information of the vehicle respectively. PC104 is chosen as the navigation computer to calculate the attitude angle and azimuth angle, using the compensated signal from IMU. The odometer is used for dead reckoning to work out the position of the vehicle. The upper computer is installed with Windows operating system, MapX control and digital map, in order to implement the map matching algorithm and the modification of the position. The structure of the system is shown in Fig.1.
Fig.1 Hardware structure of inertial navigation system
3 In-motion initial alignment of inertial frame
The inertial coordinate system is a stationary coordinate in the inertial space, the origin and axes of which can be chosen arbitrarily. At the beginning, the
body frame is chosen as the inertial frame, 
. The
changes of the body frame relative to inertial frame can be sensed by gyros, and 
at any time can be obtained by solving the differential equation as shown in Eq.(1):
(1)
where 
is the skew symmetric matrix of 
is the angular velocity of body frame relative to the inertial frame measured by gyros.
The attitude angle can be quickly obtained by compass level, which can be used to get the expression of the gravity vector in the body frame:
(2)
where 
is obtained by compass level and g is the gravity scalar.
When the land vehicle stays still, the gravity vector in the inertial frame 
can be sensed by the inertial device. After time t, the gravity changes from to 
because of the self-rotation of the earth, sweeping a cone in the inertial space, as shown in Fig.2.
During time t, the earth rotates through 
and the origin of the body frame goes from Oi to On(b).
Besides, 
Apparently, 
and 
constitute an orthogonal coordinate system, which we call transition coordinate system (m frame) with origin Om and three axes pointing to north, east and down respectively. The transformation matrix from the inertial frame to the transition frame is 
At time tm=t/2, m frame would coincide with local geographic coordinate system L(tm) by rotating β around axis y, and the direction cosine matrix of this transformation is 
The local geographic coordinate system is chosen as the navigation system, and the navigation coordinate N(t) at time t can be obtained by rotating α/2 of the navigation coordinate N(tm) at time tm around the rotation axis of earth. The corresponding direction cosine matrix of this
rotation is 
So far, enough information has been obtained to calculate the direction cosine matrix from b frame to N frame as shown in Eq.(3):
(3)
Fig.2 Initial alignment based on inertial frame
Fig.3 Flow chart of map matching algorithm
As the position of the vehicle changes during the in-motion initial alignment, the azimuth angle changes correspondingly, which need to be compensated with the information of position as follows:
(4)
(5)
where 
and 
are the displacement of the vehicle in the north-south direction and east-west direction respectively; R is the radius of the earth; L is the latitude of the starting point; ωie is the angular velocity of the earth; t is the time interval between two parking.
4 Map matching algorithm
The principle of the map matching is that the position, course and mileage are chosen as the input reference quantities, and the highest matching road is selected from the map database utilizing fuzzy function, then the output position of the INS is projected to the selected road[2-4]. The whole map matching algorithm consists of three sub-algorithms: search sub-algorithm, project sub-algorithm and judge sub-algorithm, as shown in Fig.3.
4.1 Search sub-algorithm
The inputs of the fuzzy inference system are: 1) current position of the vehicle, 2) current course of the vehicle, 3) direction of each road to be selected. The fuzzy sets are: 1) the extent of current position close to a certain road section, 2) the extent of the difference between current course and direction of a certain road section. If the vehicle is very close to a road section and the difference between current course and the direction of the road section is very small, the similarity of current driving section and the selected road section is given as follows:
(6)
where 
represents the membership of the distance between the vehicle and the road section; 
represents the membership of the difference between current course and direction of the road section.
In the search sub-algorithm, the membership function of the distance between the vehicle and the road section is
(7)
where d is the distance between the vehicle and the road section.
The membership function of the difference between current course and direction of the road section is
(8)
where Δθ is the angle between current course and direction of the road section.
4.2 Project sub-algorithm
When the road section is selected, the position of the vehicle is projected to this road with orthogonal projection method. Suppose the position coordinates of the vehicle is P(x, y), the two ending coordinates of the current road is P0(x0, y0) and P2(x2, y2), as shown in Fig.4.
Fig.4 Road projecting algorithm
It can be seen that vertical projection can effectively eliminate the error of the vertical path, but the error along the road cannot be eliminated because of the course error. So, an improved arc projection method is applied to compensate the position error along the road.
4.3 Judge sub-algorithm
In fact, it is inevitable to choose wrong road. Therefore, fault recognizing algorithm is introduced to enhance the robustness of the system[5]. As demonstrated in Fig.5(a), road 1 is the correct road section and road 2 is the wrong one selected by the algorithm. P0 and P1 are the position worked out by the INS, Pj0 and Pj1 are the projection in road 2. These four points are extracted in Fig.5(b) to calculate the difference between the output of the INS and the course of the wrong road.
Fig.5 Fault recognizing algorithm
(9)
When α exceeds the set threshold (30°), a wrong matching occurs and the search sub-algorithm is repeated.
5 Experiment
5.1 Method of experiment
The experimental vehicle is equipped with odometer and digital map, and the route is demonstrated in Fig.6. The two parking points are marked with A and B, and the exact positions are measured by GPS with accuracy of 0.11 m(1σ). The local geodetic coordinates are loaded at spot A, and the first leveling and coarse alignment is carried out for 30 s with the engine on and the vehicle stationary. Then the vehicle heads for spot B, and the second leveling and alignment is carried out for 15 s to calculate the attitude angle and azimuth angle.
The stationary-base initial alignment accuracy of this system is 0.8 mil (RMS). After the in-motion alignment is completed, stationary-base alignment is made for three times and the average azimuth angle is used as the reference value. Repeat the above steps for five times, and then the experiment data are processed off-line to get the in-motion initial alignment results without map matching.
Fig.6 Experiment route
5.2 Positioning error analysis with and without digital map
The positioning error and azimuth error without digital map are much larger than that with digital map, as listed in Tables 1 and 2. That is because the first leveling and coarse alignment are under the conditions of the engine on, vehicle shaking and low SNR (signal to noise ratio). After coarse alignment, the initial azimuth error is large and the positioning error of the system diverges gradually. However, in the experiment aided with digital map, the map matching algorithm is implemented when the vehicle is running. Therefore, the divergence of the positioning error is effectively inhibited in spite of large initial azimuth error. The effect of map matching is shown in Fig.7.
Table 1 Positioning error with and without digital map
Table 2 Azimuth error with and without digital map
Fig.7 Effect of map-match algorithm
In Fig.7, the line represents the road network; the small flags represent the coordinates of large error calculated by INS; the spots represent the matched coordinates. By selecting appropriate parameters. It can be seen that good road projection can be ensured with large INS error.
6 Conclusions
(1) INS in-motion initial alignment aided by digital map can greatly improve the maneuverability and rapid response of the vehicle.
(2) By introducing the map matching algorithm, the scheme significantly compensates the positioning error caused by azimuth angle error and enhances the fault tolerance of the coarse alignment and improves the alignment accuracy of the vehicle.
References
[1] Napolitano F, Gaiffe T, Cottreau Y, et al. PHINS: The first high performances inertial navigation system based on fiber optic gyroscopes[C]//Proceedings of the 9th Saint Petersburg International Conference on Integrated Navigation systems. Saint Petersburg, 2002: 296-304.
[2] Rajashri R. Joshi. A new approach to map-matching for in-vehicle navigation systems: The rotation variation metric[C]// Proceedings of IEEE Intelligent Transportation Systems Conference Proceedings. Oakland, 2001: 33-38.
[3] YANG Da-kai, CAI Bai-gen, YUAN Yi-fang. An improved map-matching algorithm used in vehicle navigation system[J]. IEEE Proceedings on Intelligent Transportation Systems. 2003, 2(2): 1246-1250.
[4] DING Lu, CHEN Jia-bin, LU Shao-lin, et al. A new map- matching algorithm and real-time parameter correction for land vehicle navigation system[C]//Proceedings of International Conference on Transportation Engineering, Chengdu, 2007: 25-30.
[5] LIAN Jun-xiang, WU Wen-qi, WU Mei-ping, et al. SINS initial alignment algorithm for marching vehicles[J]. Journal of Chinese Inertial Technology, 2007, 15(2): 156-159. (in Chinese)
(Edited by HE Yun-bin)
Received date: 2011-04-15; Accepted date: 2011-06-15
Foundation item: Project(2010AA7042007) supported by the National High Technology Research and Development Program of China
Corresponding author: MAO Yu-liang, PhD candidate; Tel: +86-13581583944; E-mail: maoyuliangready@163.com
