CN104359492A - Error estimation algorithm for track plotting positioning system composed of inertial navigator and wheel speed meter - Google Patents

Abstract

The present invention provides an error estimation algorithm for a dead reckoning positioning system composed of an inertial navigation and a wheel speedometer. By converting the dead reckoning coordinates and GPS latitude and longitude coordinates into the same local coordinate system, the GPS coordinates are used as standard values, and the track The estimated coordinates are reference values, calculate the error value between the dead reckoning coordinates and the GPS coordinates, set 3 error parameters, each error parameter sets a certain order of magnitude error range, and select an error amount for every segment error within this range , use the orthogonal test method to screen out some typical error combinations, find out the error parameter combination with the smallest error as the optimal value of the error parameter, and substitute it into the left and right wheel data of the inertial navigation and wheel speedometer for correction. The invention estimates the system error of the inertial navigation through a simple algorithm, and corrects it at one time. The algorithm is simple, the speed is fast, and the correction can be performed after one calculation, which simplifies the error calibration process before the use of the inertial navigation positioning system.

Description

Error Estimation Algorithm for Dead Reckoning and Positioning System Composed of Inertial Navigation and Wheel Speedometer

technical field

The invention relates to the technical field of inertial navigation systems, in particular to an error estimation algorithm for a dead reckoning positioning system composed of an inertial navigation and a wheel speedometer.

Background technique

The inertial navigation system relies on mechanical equipment and corresponding algorithms to work, and can independently complete navigation tasks. In addition, the inertial navigation system is not disturbed by external environmental factors such as weather and electromagnetic radiation, and the carrier's activities in a small range have very high reliability, so the inertial navigation system is widely used in military and civilian fields.

After the inertial navigation system of the ground vehicle is used for a long time, the installation of the inertial navigation is loose, which will cause the installation offset angle of the inertial navigation, and the inertial navigation is affected by the outside world, such as the magnetic field, which will also cause the initial orientation angle of the inertial navigation to deviate. Due to changes in tire pressure, the radius of the wheel will also deviate from the standard wheel radius, which will cause an offset error when the wheel speedometer calculates the moving distance of the vehicle. Due to the existence of this kind of error problem, it will lead to large system offset errors in the obtained coordinates and running track when the inertial navigation positioning system performs dead reckoning positioning. Therefore, before the inertial navigation system starts to work, an initial calibration is usually performed to reduce the positioning error caused by the initial installation deviation angle of the inertial navigation system, the initial error of the inertial navigation system, and the offset error of the wheel speedometer.

The initial calibration of inertial navigation is usually divided into static base analysis and dynamic error analysis. The static base test mainly measures the system errors of inertial navigation and other navigation devices. Because a large number of precision instruments are required, although the accuracy is high, it takes a long time and the cost is very high. The dynamic analysis of the inertial navigation system is mainly calibrated before the inertial navigation system starts to work, so as to reduce the installation deviation angle and the initial misalignment of the inertial navigation system, which will cause systematic errors in the positioning of the inertial navigation system. Moreover, the dynamic error test can be carried out in the outdoor field without the use of precision instruments, just use the GPS coordinates and heading angle as the reference standard, it does not take too long, it is easy to use, and the cost is very low. However, the dynamic error correction method mainly focuses on the real-time correction of the error. While the algorithm is complex, it cannot estimate the system error value and correct it at one time.

Contents of the invention

The purpose of the present invention is to simplify the error calibration process before the use of the inertial navigation and positioning system, and provide an error estimation algorithm for the dead reckoning and positioning system composed of inertial navigation and wheel speedometer. The algorithm is simple and fast, and can be corrected after one calculation.

In order to solve the problems of the technologies described above, the present invention adopts the following technical solutions:

An error estimation algorithm for a dead reckoning positioning system composed of an inertial navigation and a wheel speedometer, comprising the following steps:

(1) The test vehicle turns on the on-board inertial navigation, wheel speedometer and GPS to drive for a certain distance, and records the inertial navigation direction, wheel speedometer and GPS latitude and longitude coordinate data respectively;

(2) Perform dead reckoning according to inertial navigation data and wheel speedometer data, and obtain dead reckoning coordinates (x _n , y _n ) at time n;

(3) Set the local coordinate system with the starting point of the test vehicle as the origin, convert the GPS latitude and longitude coordinates and the dead reckoning coordinates in step (2) into local coordinate system coordinates, and take the true north as the positive direction of the Y axis, and the true east As the positive direction of the X-axis, the two coordinate data are integrated into the same coordinate system for comparison. Taking the GPS latitude and longitude coordinate point as the standard parameter, the dead reckoning coordinate point is the comparison value including the error, and the distance between the two coordinate points is difference;

(4) Set the initial error angle of the inertial navigation heading, the initial error of the left wheel speedometer and the initial error of the right wheel speedometer as the error parameters that cause errors in the coordinates obtained by dead reckoning;

(5) Determine the range of the optimal value of the error parameter value: first, the error parameter group is screened. When the error parameter value level is 1 or 2, the error parameter group is screened by the method of drawing. When the error parameter value When the number of levels is greater than or equal to 3, the error parameter group is screened by the orthogonal test method; then the screened error parameter group is substituted into the dead reckoning coordinates in the local coordinate system to calculate the error size to determine the value interval of the optimal value ;

(6) Substituting the selected error parameter group into the dead reckoning coordinates in the local coordinate system to calculate the size of the error, and whether the maximum value and mean value of the calculated comparison error can meet the accuracy requirements, and judge whether it is the optimal solution of the error parameter group;

(7) After calculating the optimal solution of the error parameter group, the optimal solution is substituted into the left and right wheel data of the inertial navigation and wheel speedometer for correction, and the corrected inertial navigation and wheel speedometer data are used to track The corrected inertial navigation positioning coordinates can be obtained by extrapolation.

In step (2), the specific method of dead reckoning is: according to the inertial navigation direction of the test vehicle and the vehicle travel distance data of the wheel speedometer, the position coordinates (x _n , y _n ) of the vehicle are calculated by the trigonometric function method, As shown in the following formula:

x _n ＝x _n-1 +△Ls*cos(heading _n-1 ) (i)

y _n ＝y _n-1 +△Ls*sin(heading _n-1 )

Among them, △Ls is the moving distance of the test vehicle in unit sampling time, and heading _n-1 is the instantaneous inertial navigation heading of the test vehicle at the n-1th moment.

The moving distance ΔLs of the test vehicle within the unit sampling time is the average value of the driving distance of the left wheel and the right wheel, as shown in the following formula:

$\mathrm{<span\; class="notranslate">\; ls</span>}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; ls</span>}}_{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; ls</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span>$

Where Ls is the average travel distance recorded by the wheel tachometer, Ls _L is the travel distance recorded by the left wheel tachometer, Ls _R is the travel distance recorded by the right wheel tachometer, and the distance traveled by the test vehicle per unit sampling time △Ls= Ls _n -Ls _n-1 , wherein Ls _n is the average value of the driving distance recorded by the left and right wheel speedometers at time n.

In step (3), the calculation process of converting GPS latitude and longitude coordinates into coordinates of the local coordinate system is specifically as follows: Let the GPS latitude and longitude coordinates be converted into coordinates (x' _n , y' _n ) in the local coordinate system at time n, as shown in the following formula:

x' _n ＝x' _n-1 +R*r _ω ，y' _n ＝y' _n-1 +R*r _φ (iii)

_rω ＝△ω*π/360

r _φ = △φ*π/360

Among them, R is the radius of the earth, △ω and △φ are the longitude and latitude difference between n time and n-1 time, r _ω and r _φ are longitude and latitude difference converted into radian value.

In step (5), the error range of the error parameter is set to twice the official label error of the inertial navigation, and the positive and negative directions are set as the distribution range of the error. Within the positive and negative distribution range of the error, set the same group equal At the same time, 0 is also used as a set of level numbers, so that the level numbers of different groups can be freely combined to form all error parameter groups.

In step (6), if there is a greater precision requirement for the error parameter set and further accurate calculation of the optimal solution is required, the response surface method can be used to fit the error surface to find the point with the smallest error on the error surface, and the corresponding The error parameter is the optimal solution of the error parameter.

As can be seen from the above technical solutions, the present invention converts dead reckoning coordinates and GPS latitude and longitude coordinates into the same local coordinate system, uses GPS coordinates as standard values, and dead reckoning coordinates as reference values to calculate dead reckoning coordinates and GPS coordinates The error value between, set 3 error parameters, assume that the error between the dead reckoning coordinates and the GPS coordinates is mainly generated by these 3 error parameters, each error parameter sets a certain order of magnitude of error range, within this range Select an error amount for the interval error, use the orthogonal test method to design some typical error combinations in all the error amount combinations, and find the error parameter combination with the smallest error in this part of the typical error combinations as the error parameter The optimal value is substituted into the left and right wheel data of the inertial navigation and wheel speedometer for correction.

This patent uses a simple algorithm to estimate the system error of the inertial navigation system and correct it at one time. The algorithm is simple and fast, and can be corrected after one calculation, which simplifies the error calibration process before the use of the inertial navigation positioning system.

Description of drawings

Fig. 1 is the flowchart of the error estimation algorithm of the dead reckoning positioning system composed of inertial navigation and wheel speedometer of the present invention;

Figure 2 is the L ₉ (3 ⁴ ) orthogonal test table;

Fig. 3 is the coordinate trajectory and the GPS coordinate trajectory calculated by the track of the experimental vehicle in the local coordinate system in the embodiment of the present invention, and the distance between each coordinate point is the error value.

Detailed ways

The present invention will be described in further detail below in conjunction with the accompanying drawings.

The purpose of the present invention is to simplify the error calibration process before the use of the inertial navigation and positioning system. During the calibration, let the test vehicle turn on the vehicle-mounted inertial navigation, wheel speedometer and GPS to run a long distance, and record the combination of the inertial navigation direction and the wheel speedometer. Estimated vehicle trajectory and GPS positioning heading data. Set the initial error of the left and right wheel speedometers and the initial error angle of the inertial navigation heading as the three error parameters to be corrected, and use the response surface method to calculate the optimal error parameters, so that the vehicle trajectory calculated by the inertial navigation and wheel speedometers The error between the coordinates and the vehicle track coordinates recorded by GPS is the smallest, and this optimal error parameter is substituted into the dead reckoning data to correct the error. Although there is a problem that the correction effect is affected by the GPS accuracy, such accuracy constraints are acceptable when only roughly correcting the inertial navigation system error.

As shown in Figure 1, the specific steps of the error estimation algorithm of the inertial navigation positioning system are as follows:

(1) The test vehicle turns on the on-board inertial navigation, wheel speedometer and GPS to drive for a certain distance, and records the inertial navigation direction, wheel speedometer and GPS latitude and longitude coordinate data respectively.

(2) Perform dead reckoning according to inertial navigation data and wheel speedometer data, and obtain dead reckoning coordinates (x _n , y _n ) at time n.

The specific method of dead reckoning is: according to the inertial navigation direction of the test vehicle and the vehicle travel distance data of the wheel speedometer, the position coordinates (x _n , y _n ) of the vehicle are calculated by the trigonometric function method, as shown in the following formula:

x _n ＝x _n-1 +△Ls*cos(heading _n-1 ) (i)

y _n ＝y _n-1 +△Ls*sin(heading _n-1 )

Among them, △Ls is the moving distance of the test vehicle in unit sampling time, and heading _n-1 is the instantaneous inertial navigation heading of the test vehicle at the n-1th moment. Mark the inertial navigation data as heading, and the heading change rate △θ=heading _n -heading _n-1 when the test vehicle is running.

When using the direction value output by the inertial navigation and the wheel speedometer for dead reckoning positioning, due to the different driving distances of the left and right wheels when the vehicle turns, the rolling force generated by the interaction between the tire grip and the vehicle inertia will make the vehicle body Rolling, compressing the suspension on the left and right sides of the vehicle, the different forces acting on the left and right sides of the vehicle will result in different air pressure in the wheels and different changes in the radius of the wheels. Since the signal of the wheel speedometer is the pulse signal generated after the vehicle turns a circle, it needs to be multiplied by the wheel radius and arc value to get the wheel travel distance. Therefore, in order to make the distance traveled by the vehicle closer to the real value, the data of the distance traveled by the left wheel and the right wheel are taken and the average value is calculated.

The moving distance ΔLs of the test vehicle within the unit sampling time is the average value of the driving distance of the left wheel and the right wheel, as shown in the following formula:

$\mathrm{<span\; class="notranslate">\; ls</span>}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; ls</span>}}_{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; ls</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span>$

Where Ls is the average travel distance recorded by the wheel tachometer, Ls _L is the travel distance recorded by the left wheel tachometer, Ls _R is the travel distance recorded by the right wheel tachometer, and the distance traveled by the test vehicle per unit sampling time △Ls= Ls _n -Ls _n-1 , wherein Ls _n is the average value of the driving distance recorded by the left and right wheel speedometers at time n.

In order to facilitate the synchronous collection of vehicle direction information and coordinate information, and to avoid unnecessary errors caused by different data collection times during dead reckoning, preferably, the inertial navigation device and the GPS receiving device are set to collect at the same initial moment and at the same cycle Vehicle heading, driving distance and GPS latitude and longitude coordinate information.

At the same time, in order to synchronize the longitude and latitude coordinate information obtained by comparing the GPS and the coordinate information obtained by the positioning system composed of wheel speedometer and inertial navigation through dead reckoning, and calculate the error value between the two coordinate points. The GPS latitude and longitude data of the vehicle coordinates and the coordinates calculated by the integrated system of the inertial wheel speedometer can be converted into the same local coordinate system to facilitate the comparison of the two coordinates. The specific method is as follows (3).

(3) Set the local coordinate system with the starting point of the test vehicle as the origin, convert the GPS latitude and longitude coordinates and the dead reckoning coordinates in step (2) into local coordinate system coordinates, and take the true north as the positive direction of the Y axis, and the true east As the positive direction of the X-axis, the two coordinate data are integrated into the same coordinate system for comparison. Taking the GPS latitude and longitude coordinate point as the standard parameter, the dead reckoning coordinate point is the comparison value including the error, and the distance between the two coordinate points is difference.

Because the experiment is carried out in a small area, in order to simplify the calculation process of converting GPS latitude and longitude coordinates into local coordinate system coordinates, the earth is assumed to be calculated as a sphere, and the average value of the earth's radius is R=6371393m. In the data obtained by GPS, the latitude at n time is φ _n , the longitude at n time is ω _n , the latitude difference between n time and n-1 time is △φ, the longitude difference is △ω, and the latitude difference It is transformed into radian value r _φ =△φ*π/360, and the longitude difference is converted into radian value r _ω =△ω*π/360. Let the GPS latitude and longitude coordinates at time n be transformed into coordinates (x' _n , y' _n ) in the local coordinate system, as shown in the following formula:

x' _n ＝x' _n-1 +R*r _ω ，y' _n ＝y' _n-1 +R*r _φ (iii)

_rω ＝△ω*π/360

r _φ = △φ*π/360

(4) Set the three deviations, the initial error angle of the inertial navigation heading, the initial error of the left wheel speedometer, and the initial error of the right wheel speedometer, as the error parameters that cause errors in the coordinates obtained by dead reckoning.

Take the calculated coordinate data as the value to be corrected, and the latitude and longitude coordinates obtained by the GPS receiver as the reference standard value. Assume that there is a deviation between the left and right wheel speeds and the initial set standard value, and the initial alignment angle of the inertial navigation is different from the actual value. Deviation, setting these three deviations is the main cause of the error in the coordinates obtained by dead reckoning. As the error parameter value, the way to correct the dead reckoning error is achieved by correcting these three deviations.

(5) Determine the range of the optimal value of the error parameter value.

To determine the range of the optimal value of the error parameter value, it is necessary to set the range of the error parameter error, which is generally set to twice the official label error of the inertial navigation, and the positive and negative directions are set as the distribution range of the error. In the distribution range of positive and negative, set the level number of equal division of the same group, and at the same time, 0 is also used as a group of level numbers, so that the level numbers of different groups can be freely combined to form all error parameter groups.

The value interval of the optimal value is determined by calculating the error size through the error parameter group. In order to simplify the calculation, it is necessary to filter the number of error parameter value groups.

There are two methods for screening. When the error parameter value level is 1 or 2, the error parameter group can be screened by drawing; when the error parameter value level is greater than or equal to 3, the error parameter group can be screened by orthogonal test method. parameter group to filter.

Orthogonal test method uses already prepared tables and orthogonal tables to arrange tests and conduct data analysis. In an orthogonal experimental design, factors can be quantitative or qualitative. The distances between the levels of quantitative factors can be equal or unequal. In the three-factor three-level conditional experiment, there are usually two methods for conducting the experiment. One is to take the combination of all levels of the three factors. The three factors and three levels are 27 trials, and each point represents a parameter combination. However, there are too many combinations to be tested in this method, and the workload is very heavy. The second is a simple comparison method, changing one factor while fixing other factors, knowing that all the optimal factors are selected to form an optimal solution combination, but the representativeness of the second method is very poor, and there are no points selected in a large range , so this method is not comprehensive, and the selected combination may not be the best among all the combinations. Secondly, when comparing conditions, only single data is compared, and the interference of errors cannot be eliminated, which will lead to unstable conclusions.

Considering the advantages of these two test methods, use the orthogonal table to select typical and representative points from the comprehensive test points. The selected test points must be evenly distributed within the test range and can reflect the overall situation. . In the three-factor three-level test, there are three planes corresponding to parameter A, and three planes corresponding to parameters B and C respectively. On the 9 planes, the number of experimental points should be the same, that is, each level of each factor should be treated equally. At the same time, every row and every column on each plane requires the same number of points.

When the number of factors and the number of levels are not too large, evenly distributed test points can be selected by means of graphing. When there are many factors and levels, the experiments are arranged according to the orthogonal table. The orthogonal table is shown in Figure 2. 1, 2, and 3 in each column appear three times. Any two columns, such as the 3rd and 4th columns, have a total of 9 types of ordered numbers from top to bottom. There is no repetition or omission, and the ordered numbers formed by any other two columns also appear once in each of these nine types, which satisfies the uniformity of the distribution of test points.

When recording the error data, the comparison reference is to take the GPS coordinates and heading as the standard parameters, and the running coordinate points calculated by the inertial navigation direction combined with the wheel speed meter are the comparison values including the error. The error value is each sampling moment, the coordinate points in the local coordinate system obtained by dead reckoning, and the latitude and longitude obtained by GPS positioning are converted to obtain the coordinate points in the same local coordinate system, and the distance value between them, that is as an error value. In order to measure the size of the error and determine whether the error is a minimum value, it is necessary to simultaneously calculate the maximum value and the average value of the error as a measure of the error size. The specific method used is as described in step (6).

(6) Substitute the selected error parameter group into the dead reckoning coordinates in the local coordinate system to calculate the error size, and judge whether it is the optimal solution of the error parameter group according to the calculation and comparison of whether the maximum value and mean value of the error can meet the accuracy requirements.

Use the orthogonal test method to calculate the maximum value and mean value of the comparison error, and after obtaining the value range of the optimal solution of the error parameter, find the minimum value by calculating the error. When the accuracy of the error parameter is not high, the error parameter group with the smallest error will be can be regarded as the optimal solution. If there is a greater accuracy requirement for the error parameter set and further accurate calculation of the optimal solution is required, the response surface method can be used to fit the error surface, and the point with the smallest error on the error surface can be found, and the corresponding error parameter is the error parameter the optimal solution of . The surface response method is an existing algorithm, the principle of the algorithm is simple, and all calculations can be automatically completed by software.

(7) After calculating the optimal solution of the error parameter group, the optimal solution is substituted into the left and right wheel data of the inertial navigation and wheel speedometer for correction, and the corrected inertial navigation and wheel speedometer data are used to track The corrected inertial navigation positioning coordinates can be obtained by extrapolation.

A kind of specific embodiment of the present invention is described as follows:

The initial standard error of the wheel radius of the test vehicle is set to 0, the initial heading error of the inertial navigation is 0, the positive and negative error range of the left and right wheels is between -0.03m and 0.03m, and the left and right error range of the heading angle is -1 degree to 1 degree.

After setting the error range, the wheel error is taken every 0.01m, and the inertial navigation error is taken every 0.25 degrees. Among all error parameter combinations, typical error parameter combinations are screened. Using the orthogonal experimental design method, select (-0.03,-0.03,1), (0,0,0.25), (0.02,0.02,-1), (0,-0.03,0.25), (0.03,-0.03 ,-1), (0.03,0,0.25), (-0.03,0.03,-1) these groups of error combinations, calculate the error value.

The result is that when the error parameter is (-0.01, 0.01, 0.25), the error between dead reckoning and GPS coordinates is the smallest. The estimation of dead reckoning and GPS coordinates is shown in Figure 3. At this time, the maximum value of the coordinate distance error is 21.76m, and the average value is 13.24m.

If the average value and maximum value of the error can meet the accuracy requirements, the results of the orthogonal test can be used as correction values. If higher accuracy is required, use software capable of surface response analysis to further calculate the optimal value to correct the systematic error of the vehicle.

The above-mentioned embodiments are only descriptions of the preferred embodiments of the present invention, and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, those skilled in the art may make various modifications to the technical solutions of the present invention. and improvements, all should fall within the scope of protection determined by the claims of the present invention.

Claims (6)

1. A dead reckoning positioning system error estimation algorithm composed of inertial navigation and wheel speedometer, is characterized in that, comprises the steps: (1) The test vehicle turns on the on-board inertial navigation, wheel speedometer and GPS to drive for a certain distance, and records the inertial navigation direction, wheel speedometer and GPS latitude and longitude coordinate data respectively; (2) Perform dead reckoning according to inertial navigation data and wheel speedometer data, and obtain dead reckoning coordinates (x _n , y _n ) at time n; (3) Set the local coordinate system with the starting point of the test vehicle as the origin, convert the GPS latitude and longitude coordinates and the dead reckoning coordinates in step (2) into local coordinate system coordinates, and take the true north as the positive direction of the Y axis, and the true east As the positive direction of the X-axis, the two coordinate data are integrated into the same coordinate system for comparison. Taking the GPS latitude and longitude coordinate point as the standard parameter, the dead reckoning coordinate point is the comparison value including the error, and the distance between the two coordinate points is difference; (4) Set the initial error angle of the inertial navigation heading, the initial error of the left wheel speedometer and the initial error of the right wheel speedometer as the error parameters that cause errors in the coordinates obtained by dead reckoning; (5) Determine the range of the optimal value of the error parameter value: first, the error parameter group is screened. When the error parameter value level is 1 or 2, the error parameter group is screened by the method of drawing. When the error parameter value When the number of levels is greater than or equal to 3, the error parameter group is screened by the orthogonal test method; then the screened error parameter group is substituted into the dead reckoning coordinates in the local coordinate system to calculate the error size to determine the value interval of the optimal value ; (6) Substituting the selected error parameter group into the dead reckoning coordinates in the local coordinate system to calculate the size of the error, and whether the maximum value and mean value of the calculated comparison error can meet the accuracy requirements, and judge whether it is the optimal solution of the error parameter group; (7) After calculating the optimal solution of the error parameter group, the optimal solution is substituted into the left and right wheel data of the inertial navigation and wheel speedometer for correction, and the corrected inertial navigation and wheel speedometer data are used to track The corrected inertial navigation positioning coordinates can be obtained by extrapolation. 2. The error estimation algorithm according to claim 1, characterized in that, in step (2), the dead reckoning is specifically: according to the inertial navigation direction of the test vehicle and the vehicle travel distance data of the wheel speedometer, by trigonometric function method Calculate the position coordinates (x _n , y _n ) of the vehicle, as shown in the following formula: x _n ＝x _n-1 +△Ls*cos(heading _n-1 ) (i) y _n ＝y _n-1 +△Ls*sin(heading _n-1 ) Among them, △Ls is the moving distance of the test vehicle in unit sampling time, and heading _n-1 is the instantaneous inertial navigation heading of the test vehicle at the n-1th moment. 3. error estimation algorithm according to claim 2, is characterized in that, the distance ΔLs that test vehicle moves within the unit sampling time is the average value of the travel distances of left wheel and right wheel, as shown in the following formula: $\mathrm{<span\; class="notranslate">\; ls</span>}<span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; ls</span>}}_{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; ls</span>}}_{\mathrm{<span\; class="notranslate">\; R</span>}}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; )</span>$ Where Ls is the average travel distance recorded by the wheel tachometer, Ls _L is the travel distance recorded by the left wheel tachometer, Ls _R is the travel distance recorded by the right wheel tachometer, and the distance traveled by the test vehicle per unit sampling time △Ls= Ls _n -Ls _n-1 , wherein Ls _n is the average value of the driving distance recorded by the left and right wheel speedometers at time n. 4. error estimation algorithm according to claim 1, it is characterized in that, in step (3), the calculation process that GPS latitude and longitude coordinates are transformed into local coordinate system coordinates is specifically as follows: suppose n moment GPS latitude and longitude coordinates are transformed into local coordinate system The coordinates (x' _n ,y' _n ), as shown in the following formula: x' _n ＝x' _n-1 +R*r _ω ，y' _n ＝y' _n-1 +R*r _φ (iii) _rω ＝△ω*π/360 r _φ = △φ*π/360 Among them, R is the radius of the earth, △ω and △φ are the longitude and latitude difference between n time and n-1 time, r _ω and r _φ are longitude and latitude difference converted into radian value. 5. The error estimation algorithm according to claim 1, characterized in that, in step (5), the error range of the error parameter is set to twice the official label error of inertial navigation, and the positive and negative directions are all set to the distribution of the error Range, within the positive and negative distribution range of the error, set the level numbers of equal divisions of the same group, and at the same time, 0 is also used as a set of level numbers, so that the level numbers of different groups can be freely combined to form all error parameter groups. 6. The error estimation algorithm according to claim 1, characterized in that, in step (6), if there is a greater accuracy requirement for the error parameter set, and the optimal solution needs to be further accurately calculated, the response surface method can be used to Fit the error surface, find the point with the smallest error on the error surface, and the corresponding error parameter is the optimal solution of the error parameter.