Abstract
Making the sensor rigidly mounted in the target is the common characteristic of conventional navigation system. However, it is difficult or impossible to realize that for special applications such as the positioning of hostile aircraft. A novel new algorithm for target navigation and mapping is designed based on the position, attitude, and ranging information provided by laser distance detector (electronic distance measuring, LDS) and MEMS/GPS navigation, which can solve problem of the target navigation and mapping without any sensor in the target. The detailed error analysis shows that attitude error of MEMS/GPS is the main error source which dominated the accuracy of the algorithm. Based on the error analysis, a calibration algorithm is designed so as to improve the accuracy to a large extent. The result shows that, by using this new algorithm, the performance of target positioning can be efficiently improved, and the positioning error is less than 2 meters for the target within 1 kilometer range.
1. Introduction
Motion state of the target can be described by its accelerometer, velocity, position, and attitude. Navigation system is the most convenient device to provide these information by means of optical navigation, electronics navigation, dynamics navigation, acoustic navigation, and so on [1]. For the mentioned navigation system, there is a common characteristic that the navigation sensor must be mounted in the target. For example, there must be optical observation device in the target which can measure the attitude vector from the target to another known point in terrestrial navigation and celestial navigation [2]. MEMS inertial navigation system must rigidly mount the gyros and accelerometers to the target so as to measure its linear and angular movement. In communication domain, the target position method based on the cell site is also realized by communication between the cell phone and the base station [3].
Sometimes, conventional navigation system cannot act the right function in special application. For the positioning of those hard to or unable to reach places, it is difficult or expensive to mount the sensor in the target. For the positing of the enemy target in military, it is impossible [4, 5].
Optical measure device in mapping is a good choice for the target mapping because it is unnecessary to mount the sensor in the target. On the basis of the known self-position, the target position can be measured accurately by means of angle measurement [6]. For example, the total station can output precise slope distance from the instrument to a particular point by a theodolite integrated with an electronic distance meter (EDM). In a total station, the theodolite is used to measure angles in the horizontal and vertical planes, the distance between the instrument and the point is measured by the EDM. But for the mapping and navigation of the moving targets, optical measure device is unsuitable because it can only work in a relative static state [7].
MEMS/GPS integrated navigation system has the advantages of small size, light weight, and low cost; it can be applied in many navigation fields such as unmanned aircrafts, land vehicles, and robots [8]. If the MEMS/GPS navigation system is allowed to substitute the optical measure device, the framework of the MEMS gyros constituted can be used to describe the vector from the instrument to the target. This can make the instrument suitable for static and moving target after being integrated with an EDM. In this paper, a new method based on MEMS/GPS and LDS is designed to realize the mapping and navigation of static and moving target.
But due to the low accuracy of MEMS/GPS in attitude, the attitude error probably becomes the most important error source [9]. If there is not suitable calibration method, the mapping and navigation result for the target will be severely influenced by the attitude error. Based on the detailed error analysis, calibration algorithm for the attitude error is designed so as to compensate the heading and pitch error of MEMS/GPS. The performance of the designed algorithm is evaluated by simulations and the results show that the new method exhibits excellent navigation and mapping performance.
2. Positioning Algorithm of the Target
2.1. Basic Definition for the Coordinate Systems
For the convenience of the observer and the target description, two coordinate systems should be defined. The first coordinate system is the Earth-fixed coordinate system . It is a geocentric coordinate system which is rigidly bound to the Earth (see Figure 1). Its center is located at the geometric center of the Earth, the axis passes through the zero longitude in the equator plane, the axis is directed to the North Pole, and the axis completes the right-handed orthogonal triad system. The other coordinate system is geographic coordinate system (; see Figure 1) whose center is located in the observer, the axis is horizontal-east, the axis is horizontal-north, and the axis completes the right-handed orthogonal triad system.
The relationship between and can be described by the cosine transform matrix [9]: where and are, respectively, the latitude and longitude of point in .
2.2. Basic Description for the Vectors
For vectors of , , and in , all of them can be described if the unit vector in is involved. Thus, vectors of , , and can be described as where is the Cartesian coordinates of observer in ; is the Cartesian coordinates of target in ; , , is the coordinate difference between and in .
The three vectors in (2) have the following relationship:
From (3), it can be seen that the calculation of becomes the key for position calculation of the target on the basis of the known position of observer . This is also the first research activity.
2.3. Positioning Calculation for the Target T
According to the laws of Euler angle rotation, one orthogonal coordinate system can be transformed into another orthogonal coordinate system by three rotations. For example, three times rotation of , , and can realize the transform from the geographic coordinate system to the body coordinate system, where , , and are, respectively, the angle of heading, pitch, and roll (see Figure 2).
From Figure 2, it can obviously be found that the vector of is tightly concerned with the ranging between and and the attitude of in . By involving the unit vector of , , , the vector of can also be described in . That is, where is the coordinate difference between and in .
In order to calculate the vector of , assume a new vector of in : where is the distance between and .
From (5), the vector of has, in fact, the same direction as axis. Its mold is equivalent to the ranging between and .
For the transform between and , two rotations of and can accomplish the mission, because the final rotation of will not change the direction of . In Figure 2, it can be seen that the axis has the same direction as the axis.
The rotation of and can be described by the following matrix transform. The first rotation transform matrix of angle is
The second rotation transform matrix of angle is
According to the definition of and , the vector of can be expressed based on the former transform matrix:
Involving (6) and (7) into (8), the final calculation of can be got as follows:
For the vector of , its description in (2) and (4) is the same vector. On the other hand, the unit vector of , , and the unit vector of , , have the following relationship:
According to the relationship of (10), the coordinate difference between and in can be got:
Thus, the position of the target can be got based on (3) and (11):
All the variables in (12) are ideal. In order to calculate the target position, the position of , the distance between and , and the orientation of must be known by suitable measurement devices.
If the MEMS/GPS can be rigidly mounted in the observer along with the body coordinate system, the position of can be calculated from the position output of MEMS/GPS and the orientation of can be described by the attitude of MEMS/GPS. On the other hand, LDS can measure the distance if it is mounted along with the axis of the body coordinate system accurately.
Thus, define the new variables of the measurement:(1)latitude, longitude, and height measurement output of MEMS/GPS: , , ;(2)heading, pitch, and roll measurement output of MEMS/GPS: , , ;(3)ranging measurement output of LDS: .
After getting the measurement information of MEMS/GPS and LDS, the Cartesian coordinates of observer can be calculated according to the following equation [10]: where , is the long axle of the Earth, and is the eccentricity of the Earth.
By substituting the ideal variables with the measurement result, the final of Cartesian coordinates of the target can be got:
2.4. Functions for the Target Mapping and Navigation
Because MEMS/GPS and LDS can realize real-time measurement, the new algorithm can be used for both the static and the moving target. Besides the target position, the new algorithm can also measure many other pieces of information including longitude, latitude, height of the target, the slant range from one point to point, and the height difference and horizontal ranging between two targets. These pieces of information can all be calculated based on the position information of the target.
For the slant range () from one point to another point, can be calculated by the following equation: where and are, respectively, the measurement result of the Cartesian coordinates for targets and .
For the height difference () between the two targets,
3. Error Analysis for the Algorithm
As mentioned in the former introduction, MEMS/GPS has a relatively low accuracy of attitude. For example, NAV440 GPS-aided MEMS inertial system has a 0.4° (RMS) accuracy of pitch and roll and a 1.0° (RMS) accuracy of heading for all motion [11]. ADIS16480 MEMS/GPS has an accuracy of 0.1° (pitch, roll) and 0.3° (heading) for static state [12]. Besides the attitude error, the positioning error of MEMS/GPS probably has a severe impact on the position calculation of the target. For example, Omni STAR HP of Trimble has accuracy of minor to 10 centimeters [13]. On the other hand, LDS also has measurement error. For example, the accuracy of AccuRange AR500 Laser Sensor is generally specified with a linearity of about +/− 0.15% of the range [14].
3.1. Positioning Error Caused by the Positioning Error of MEMS/GPS
The positioning error of MEMS/GPS will cause two errors which are tightly concerned with the calculation of the target. Those are the position error of point and the cosine transform matrix of . Based on (12) and (14), the target positioning error caused by the positioning error MEMS/GPS can be calculated with the neglect of other error resources: where , , , .
Comparing with the item of , another item of can be neglected because the error of the transform matrix is very small. For example, the first line of has the following characteristic:
Then,
The following result can be got by using the same method:
For the distance which is minor to 1 kilometer, the relationship between and radius of the Earth will satisfy where is the radius of the Earth.
Then,
That is to say,
Thus, where is the positioning error caused by the positioning error of MEMS/GPS.
3.2. Positioning Error Caused by the Error of LDS
Based on (12) and (14), the target positioning error caused by the error of LDS can be calculated with the neglect of other error resources:
Thus,
3.3. Positioning Error Caused by the Attitude Error of MEMS/GPS
The target positioning error caused by the attitude error of MEMS/GPS can also be calculated with the neglect of other error resources:
Because and are all small error angles, then
Based on the condition of (28), the error can be got as follows:
3.4. Simulation for the Positioning Error of the Target
Assume that the initial position of the observer is east longitude 126° and north latitude 45°. The longitude and latitude error of MEMS/GPS is 0.2 m and the heading and pitch error of MEMS/GPS are, respectively, 0.2° and 0.1° [12]. LDS has a linearity error of 0.15% of the range [14]. Corresponding to three conditions, three simulation results are given in Figure 3. By the way, the heading and pitch are in swaying state in order to follow a moving target: and .
(a) Results of condition 1
(b) Results of conditions 1 and 2
(c) Results of conditions 1, 2, and 3
Condition 1. Only the position error of MEMS/GPS is involved.
Condition 2. The position error of MEMS/GPS and the linearity error of LDS are involved.
Condition 3. All errors of MEMS/GPS and LDS are involved.
As shown in Figure 3(a), the positioning error of target which is caused by the positioning error of MEMS/GPS is mainly concerned with , because the calculation of is 0.2828, and the error is almost independent of the parameter . The positioning error of the target in Figure 3(b) will reach 1.7 m when the parameter is about 1 kilometer. The main reason is the linear error of LDS besides the error of 0.28 m stimulated by condition 1. It also can be seen that the positioning error of the target added quickly after the attitude error is involved in Figure 3(c). In the meantime, the error caused by and is linear to the parameter .
4. Calibration for the Algorithm
For navigation and mapping with higher accuracy demand, the positioning algorithm probably cannot meet the demands because of the measure error. Former simulation has explained the reason which is because of errors of MEMS/GPS and LDS. Among all errors, attitude error is the most important because it dominates the accuracy of the target position. That is to say, the positioning error of point and the ranging error of can be neglected when the attitude error of MEMS/GPS is bigger than 0.1°, especially when the distance is minor to 500 m.
Besides the attitude error mentioned, the attitude accuracy of MEMS/GPS will decline because the gyro scale factor and the misalignment parameters will change with time [15]. These parameters can only be calibrated in laboratory. Thus, a new calibration method for the system should be designed to improve and maintain the performance of the system.
4.1. Description for the Known Reference Point
With the condition of another point nearby the observer, its position can be got based on the former algorithm. That is,
Because error of , , , and can be neglected when the parameter is minor to 500 m, The following equation can be got:
On the other hand, the ideal position of can be described based on (12):
4.2. Attitude Calculation Based on the Known Reference Point
If the position of the reference can be got in advance by means of MEMS/GPS, the substraction of (32) and (33) can construct a new measurement variable which can be used to calculated the parameter and . That is,
In (34), most variables can be measured by MEMS/GPS and LDS except the ideal attitude angle of and . That is to say, angle of and can be calculated so as to replace and . But (34) is the type of transcendental equation which is very difficult to solve. In order to get the ideal attitude, (34) should be converted into linear equation which can make the solution easy.
The parameters and can be substituted as
Involving (35) into (34) and using the same simplification as (28), the transcendental equation can be converted into linear equation:
Equation (36) can also be described as
Three equations are redundant for the solution of two unknown variables. Assuming and involving , and can be estimated by the least-square method [16]:
Then, and can substitute and . The calibration can improve the attitude of the MEMS/GPS, so as to get the target position with higher accuracy.
5. Simulation for the Calibration and the Target Positioning
5.1. Simulation for the Calibration
Performance of the calibration is very important for the positioning accuracy of the target. In order to verify the performance of the calibration, simulation results are provided by giving various conditions. During the simulation, all errors of MEMS/GPS and LDS are the same as the former. For the reference point, there is a random error of 0.05 m which is influenced by measurement. Three conditions are given during the simulation.
Condition 1. Only the attitude error of MEMS/GPS and random error of are involved: , , and .
Condition 2. All errors of MEMS/GPS, LDS, and are involved: , , and .
Condition 3. All errors of MEMS/GPS, LDS, and are involved: , , and .
Condition 4. Only the attitude error of MEMS/GPS and random error of are involved, , , and .
Condition 5. All errors of MEMS/GPS, LDS, and are involved: , , and .
Condition 6. All errors of MEMS/GPS, LDS, and are involved: , , and .
As shown in Figure 4(a), the calibration algorithm can estimate the angle of and , the residual errors of and are all minor to 0.02°; this is a very high accuracy of attitude for the target navigation and mapping. After involving errors of MEMS/GPS and LDS, the accuracy of the calibration decreased because of the error sources. The residual error of is about 0.07°, and the residual error of is about 0.01° in Figure 4(b). But with the increasing of , the residual error of will decrease to 00.04° and that of to 0.005°. That is to say, the performance of the calibration will increase if there is a relatively long distance of . If the type of the angle error is changed, the calibration has the same performance. The comparison of ideal and estimated heading error in Figures 4(d), 4(e), and 4(f) can demonstrate it.
(a) Results of condition 1
(b) Results of condition 2
(c) Results of condition 3
(d) Results of condition 4
(e) Results of condition 5
(f) Results of condition 6
5.2. Simulation for the Target Positioning after Calibration
Former simulations demonstrate that the calibration algorithm can estimate most attitude errors of MEMS/GPS. This will be very helpful for the accuracy improvement of the target position. The following simulation gave the final error analysis for the target position after the attitude error of MEMS/GPS is compensated. In order to compare the positioning performance to the uncalibrated algorithm, all errors of MEMS/GPS are the same as the former during the simulation except the error source demanded in the condition.
Condition 1. Only the attitude error of MEMS/GPS and random error of are involved: and , .
Condition 2. All errors of MEMS/GPS, LDS, and are involved: , , and .
Condition 3. All errors of MEMS/GPS, LDS, and are involved: , , and .
As shown in Figures 5(a) and 5(b), the positioning error has been decreased to a large extent after the attitude error of MEMS/GPS is compensated (curve of Cal). For more information in Figures 5(a) and 5(b), and are in fact the equivalent estimation of attitude error which will be influenced by the position error of and ranging error of . Because the positioning error of the target is also decreased when the distance is zero, this means that the position error of MEMS/GPS is partly compensated by the calibration algorithm. On the other hand, error of condition 2 is bigger than error of condition 3 because there is more residual error when . This is perfectly matched with the simulation result of Figure 4.
(a) Results of condition 1
(b) Results of condition 2
(c) Results of conditions 2 and 3
6. Conclusions
This study has developed a new method for target navigation and mapping. The results indicate that the proposed method can measure position, height, height difference, slant range, and horizontal range for static and moving target. After the calibration of MEMS/GPS, the algorithm can realize precise positioning with the error minor to 2 meters for the static and moving target within 1 kilometer range. The stable and superior performance make the new algorithm very suitable for the target mapping and navigation on land.
Conflict of Interests
The authors declare that they have no conflict of interests regarding the publication of this paper.
Acknowledgments
Funding for this work was provided by the National Nature Science Foundation of China under Grant no. 61374007 and no. 61104036 and the Fundamental Research Funds for the Central Universities under the Grant of HEUCFX41309. The authors would like to thank all the editors and anonymous reviewers for improving this paper.