Gravitational field compensation method for embedded inertial navigation system

Abstract

A gravitational field compensation method for an embedded inertial navigation system mainly comprises the following steps: packaging gravitational potential spherical harmonic coefficients into a binary file according to a certain data format, and storing the binary file in a DDR2 space of a DSP (Digital Signal Processor); after inertial navigation reads data, current longitude and latitude heights serve as input, resolving is conducted through a main function outside DSP interruption, and northeast sky gravitational acceleration under a geographical system is obtained; and compensating gravity abnormity and vertical line deviation in a speed updating algorithm. The method can effectively solve the problems that storage is difficult due to large data volume and calculation cannot be completed due to large calculation amount, a speed updating algorithm in an embedded system partially compensates plumb line deviation and gravity anomaly, and the long-time navigation precision of an inertial navigation system is improved.

Description

Technical Field

[0001] The present invention relates to the field of inertial navigation technology, and in particular to a high-precision gravity field compensation method for an embedded inertial navigation system. Background Art

[0002] The strapdown inertial navigation system collects gyro and accelerometer data, and uses the navigation computer to calculate the carrier's real-time speed, position, attitude, heading and other information. In the speed update solution of inertial navigation, the geometric motion acceleration of the carrier in the navigation system can only be obtained after deducting the harmful acceleration from the accelerometer output. The acceleration is integrated once to obtain the speed, and then integrated again to obtain the position. The harmful acceleration includes the Coriolis acceleration caused by the carrier movement and the rotation of the earth, the centripetal acceleration caused by the carrier movement, and the gravity acceleration.

[0003] In common calculations, a simple mathematical function is used to fit the Earth's gravity field, which is called the normal gravity field. The normal gravity field describes the gravity field generated under the assumption of a regular Earth, but the actual Earth is not regular. The normal gravity field is only a good approximation of the actual gravity field. In actual calculations, the geographic vertical line based on the ideal rotating ellipsoid does not coincide with the actual gravity direction, and there is a vertical deviation. The calculated celestial gravity is different from the actual celestial gravity, and there is a gravity anomaly. In fact, if the vertical deviation is large and cannot be accurately compensated, it will bring some adverse effects, which is equivalent to the accelerometer bias error or horizontal misalignment angle error, and will eventually cause positioning errors.

[0004] Existing research uses the 2190-order EIGEN-6C4 earth gravity model to calculate the northeast sky gravity value under the geographic system. Using the 2190-order EIGEN-6C4 earth gravity model, the number of bytes required for the gravity bit spherical harmonic coefficients is 2191×2191×8, which is about 37Mbytes. Each calculation of gravity data requires about 50 million double floating point multiplications. However, the actual embedded inertial navigation system uses a DSP processor such as TMS320C6748, with a main frequency of 456MHz, which is completely impossible to complete within an interrupt. If the gravity bit spherical harmonic coefficients are stored in the DDR2 space of the DSP as global variables, the compilation and burning of the L138 program will take a lot of time, seriously reducing the actual efficiency. Therefore, a high-precision gravity field compensation method for embedded inertial navigation systems is needed to store gravity bit spherical harmonic coefficients and perform calculations. Summary of the invention

[0005] The present invention discloses a high-precision gravity field compensation method for an embedded inertial navigation system. In an actual embedded system, in the process of calculating vertical line deviation and gravity anomaly, there are two problems: storage difficulty caused by a large amount of data and inability to complete the calculation due to a large amount of calculation.

[0006] The present disclosure provides a high-precision gravity field compensation method for an embedded inertial navigation system, comprising the following steps:

[0007] S1, add frame header, frame tail, frame length and checksum to the gravitational spherical harmonic coefficients, package them into a binary file according to a certain data format, and store them in the DDR2 space of DSP;

[0008] S2, after the inertial navigation reads the data, it uses the current latitude and longitude as input and performs calculation in the main function outside the DSP interrupt to obtain the gravity acceleration of the northeast sky in the geographic system;

[0009] S3, compensates for gravity anomaly and vertical deviation in the velocity update algorithm.

[0010] Furthermore, the step S1 specifically includes:

[0011] Add a frame header, a frame tail, a frame length, and a checksum to the gravitational spherical harmonic coefficients, and package them into a binary file in the data format of the frame header, data length, data, checksum, and frame tail;

[0012] In the case of ARM carrying an operating system, the data is placed in the SD card in advance in the form of a binary file in the operating system. After the inertial navigation is started, the data is transferred to the DDR2 space of the DSP through the shared memory using ARM.

[0013] In the case where ARM has no operating system, the data file is sent to the inertial navigation through the UART port. After the inertial navigation completes receiving the data, it is stored in the FLASH in binary form. After the inertial navigation is started, the DSP is used to directly read from the FLASH and store it in the DDR2 space of the DSP.

[0014] Furthermore, in step S2, the step of calculating the northeast sky gravity value in the geographic coordinate system includes:

[0015] 1) Calculate the geocentric rectangular coordinates (x, y, z) from the carrier's geographic location (L, λ, h), then calculate the geocentric spherical coordinates (r, θ, λ), and get the latitude difference in is the geocentric latitude;

[0016] 2) Recursive calculation of the normalized Legendre function and its derivatives

[0017] 3) Read in the gravitational potential spherical harmonic coefficients Calculate g′ E ,g′ N ,g′ U ;

[0018] 4) Transformed into the final g through a certain formulaE ,g N ,g U .

[0019] Furthermore, the step S2 further includes the following steps:

[0020] After the inertial navigation finishes reading the data, it first checks the read data to determine whether the received frame header, frame tail, frame length and checksum are correct to determine whether the data is correct. If the data is normal, the inertial navigation starts calculating based on the current latitude and longitude as input, otherwise no subsequent calculations are performed.

[0021] Furthermore, the specific method of step S3 includes:

[0022] The principle of the velocity update algorithm is the specific force equation, which is the basic equation for inertial navigation solution near the earth's surface. The geographic coordinate system is used as the navigation coordinate system. In the "east-north-sky" navigation coordinate system n system, there is:

[0023] Among them, the matrix It represents the attitude of the carrier system b relative to the navigation coordinate system n. is the specific force measured by the accelerometer, is the Coriolis acceleration caused by the carrier motion and the Earth's rotation, is the centripetal acceleration caused by the carrier motion, g n is the gravitational acceleration, and the velocity can be obtained by integrating the acceleration once; the update algorithm for discretized velocity solution is derived as follows:

[0024]

[0025] in, and The time period T = t m -t m-1 The internal navigation system compares the velocity increment and the velocity increment of harmful acceleration; and t m-1 and t m The inertial velocity at the moment is

[0026]

[0027] Discretize the above two equations, have

[0028]

[0029] The accelerometer is in the time period [t m-1 ,t m ] (T = t m -tm-1 ) was sampled twice at equal intervals, with velocity increments of Δv m1 and Δv m2 , Δv m =Δv m1 +Δv m2 , is the velocity increment of the whole cycle, × is The antisymmetric matrix of ; using the two-sample speed paddling error compensation algorithm, there are:

[0030]

[0031] for have

[0032] Typically, where g U0 It is calculated by the normal gravity model formula;

[0033] Based on the northeast sky gravity value in the geographic coordinate system calculated in step S2, we get where g E and g N is the vertical deviation, g U -g U0 is the gravity anomaly, substitute Velocity update and solution are performed in the process, and finally compensation for gravity anomaly and vertical deviation is achieved.

[0034] Compared with the prior art, the beneficial effects of the present invention are as follows: (1) a high-precision gravity field compensation method for an embedded inertial navigation system is proposed, and the velocity update algorithm in the embedded system partially compensates for the vertical deviation and gravity anomaly; (2) a main function outside the DSP interrupt is used to perform the solution to realize the entire calculation process, and finally the gravity acceleration of the northeast sky under the geographic system is solved, and the gravity anomaly and the vertical deviation are compensated in the velocity update algorithm. In actual tests, all calculations can be completed once in about 41 seconds, solving the two problems of storage difficulties caused by the large amount of data and inability to complete the calculation due to the large amount of calculation in the process of calculating the vertical deviation and gravity anomaly; (3) the long-term navigation accuracy of the inertial navigation system is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

[0036] Figure 1 is a flow chart of gravity calculation and compensation according to the present disclosure. DETAILED DESCRIPTION

[0037] The preferred embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to make the present disclosure more thorough and complete, and to fully convey the scope of the present disclosure to those skilled in the art.

[0038] The present disclosure discloses a high-precision gravity field compensation method for an embedded inertial navigation system. In an exemplary embodiment, the process is as shown in the attached figure. Figure 1 As shown, it mainly includes the following steps:

[0039] 1. First, add the frame header, frame tail, frame length and checksum to the gravity bit spherical harmonic coefficients, and package them into a binary file according to a certain data format. For example, the file can be stored in the form of frame header, data length, data, checksum and frame tail. The data includes two coefficient matrices, egmC[2191×2191] and egmS[2191×2191]. Both matrices are lower triangular matrices. When storing, the 0 value in the upper right corner can be discarded. They are stored in rows. Each coefficient is a double variable. The total number of data bytes is 2191×2191×8. The checksum is the cumulative sum of the lower eight bits except the frame header. The file size is about 37Mbytes.

[0040] 2. In the case where ARM carries the operating system, the data is placed in the SD card in advance in the form of a binary file through data transmission in the operating system and named a specific name. After the inertial navigation is started, ARM is used to recognize the name and the data is transferred to the DDR2 space of the DSP through shared memory for retention.

[0041] In the case of ARM without operating system, with the help of serial port debugging assistant and other similar software, the data file is sent to the inertial navigation through the UART port. The inertial navigation receives data through the UART port. According to the baud rate of 115200bps, it takes less than an hour to transmit. After the inertial navigation completes the data reception, it verifies the received data to determine whether the received frame header, frame tail, frame length and checksum are correct. If the data is correct, it is stored in the FLASH in binary form. After the inertial navigation is started, the DSP is used to read directly from the FLASH and stored in the DSP's DDR2 space for retention.

[0042] 3. After the inertial navigation finishes reading the data, it first verifies the read data to determine whether the received frame header, frame tail, frame length and checksum are correct to determine whether the data is correct. If the data is normal, the inertial navigation starts calculating based on the current latitude and longitude as input, otherwise no subsequent calculations are performed.

[0043] 4. In practice, the amount of calculation is very large and cannot be completed in one or several interrupts. In order not to affect the execution of the normal interrupt function, the main function outside the DSP interrupt is used in this embodiment to solve the entire calculation process, and finally the gravity acceleration of the northeast sky under the geographic system is solved, and the gravity anomaly and vertical deviation are compensated in the speed update algorithm. In actual tests, all calculations can be completed in about 41 seconds. After each calculation is completed, the real-time inertial navigation latitude and longitude are obtained to start the next calculation.

[0044] The steps for calculating the northeast sky gravity value in the geographic coordinate system are as follows:

[0045] 1) Calculate the geocentric rectangular coordinates (x, y, z) from the carrier's geographic location (L, λ, h), then calculate the geocentric spherical coordinates (r, θ, λ), and get the latitude difference in is the geocentric latitude;

[0046] 2) Recursive calculation of the normalized Legendre function and its derivatives

[0047] 3) Read in the gravitational potential spherical harmonic coefficients Calculate g′ E ,g′ N ,g′ U ;

[0048] 4) Transformed into the final g through a certain formula E ,g N ,g U .

[0049] The method to compensate for gravity anomaly and vertical deviation is as follows:

[0050] The principle of the velocity update algorithm is the specific force equation, which is the basic equation for inertial navigation solution near the earth's surface. The geographic coordinate system is used as the navigation coordinate system. In the "east-north-sky" navigation coordinate system n system, there is:

[0051] Among them, the matrix It represents the attitude of the carrier system b relative to the navigation coordinate system n. is the specific force measured by the accelerometer, is the Coriolis acceleration caused by the carrier motion and the Earth's rotation, is the centripetal acceleration caused by the carrier motion, g n is the gravitational acceleration, and the velocity can be obtained by integrating the acceleration once; the update algorithm for discretized velocity solution is derived as follows:

[0052]

[0053] in, and The time period T = t m -t m-1 The internal navigation system compares the velocity increment and the velocity increment of harmful acceleration; and t m-1 and t m The inertial velocity at the moment is

[0054]

[0055] Discretize the above two equations, have

[0056]

[0057] The accelerometer is in the time period [t m-1 ,t m ] (T = t m -t m-1 ) was sampled twice at equal intervals, with velocity increments of Δv m1 and Δv m2 , Δv m =Δv m1 +Δv m2 , is the velocity increment of the whole cycle, for The antisymmetric matrix of ; using the two-sample speed paddling error compensation algorithm, there are:

[0058]

[0059] for have

[0060] Typically, where g U0 It is calculated by the normal gravity model formula;

[0061] Based on the previously calculated northeastern sky gravity value in the geographic coordinate system, we get where g E and g N is the vertical deviation, g U -g U0 is the gravity anomaly, substitute Velocity update and solution are performed in the process, and finally compensation for gravity anomaly and vertical deviation is achieved.

[0062] The above technical scheme is only an exemplary embodiment of the present invention. For those skilled in the art, it is easy to make various types of improvements or modifications based on the application methods and principles disclosed in the present invention, and it is not limited to the method described in the above specific embodiment of the present invention. Therefore, the method described above is only preferred and does not have a restrictive meaning.

Claims

1. A gravity field compensation method for an embedded inertial navigation system, comprising the following steps: S1, add frame header, frame tail, frame length and checksum to the gravitational spherical harmonic coefficients, package them into a binary file according to a certain data format, and store them in the DDR2 space of DSP; S2, after the inertial navigation reads the data, it uses the current latitude and longitude as input, and performs calculations in the main function outside the DSP interrupt to obtain the gravity acceleration of the northeast sky in the geographic system; S3, compensates for gravity anomaly and vertical deviation in the velocity update algorithm.

2. The method according to claim 1, characterized in that The step S1 specifically includes: Add a frame header, a frame tail, a frame length, and a checksum to the gravitational spherical harmonic coefficients, and package them into a binary file in the data format of the frame header, data length, data, checksum, and frame tail; In the case of ARM carrying an operating system, the data is placed in the SD card in advance in the form of a binary file in the operating system. After the inertial navigation is started, the data is transferred to the DDR2 space of the DSP through the shared memory using ARM. In the case where ARM has no operating system, the data file is sent to the inertial navigation through the UART port. After the inertial navigation completes receiving the data, it is stored in the FLASH in binary form. After the inertial navigation is started, the DSP is used to directly read from the FLASH and store it in the DDR2 space of the DSP.

3. The method according to claim 1 or 2, characterized in that: In step S2, the step of calculating the northeast sky gravity value in the geographic coordinate system includes: 1) Calculate the geocentric rectangular coordinates (x, y, z) from the carrier's geographic location (L, λ, h), then calculate the geocentric spherical coordinates (r, θ, λ), and get the latitude difference in is the geocentric latitude; 2) Recursive calculation of the normalized Legendre function and its derivatives 3) Read in the gravitational potential spherical harmonic coefficients Calculate g′ E ,g′ N ,g′ U ; 4) Transformed into the final g through a certain formula E ,g N ,g U .

4. The method according to claim 1, wherein step S2 further comprises the following steps: After the inertial navigation finishes reading the data, it first checks the read data to determine whether the received frame header, frame tail, frame length and checksum are correct to determine whether the data is correct. If the data is normal, the inertial navigation starts calculating based on the current latitude and longitude as input, otherwise no subsequent calculations are performed.

5. The method according to claim 1, characterized in that The specific method of step S3 includes: The principle of the velocity update algorithm is the specific force equation, which is the basic equation for inertial navigation solution near the earth's surface. The geographic coordinate system is used as the navigation coordinate system. In the "east-north-sky" navigation coordinate system n system, there is: Among them, the matrix It represents the attitude of the carrier system b relative to the navigation coordinate system n. is the specific force measured by the accelerometer, is the Coriolis acceleration caused by the carrier motion and the Earth's rotation, is the centripetal acceleration caused by the carrier motion, g n is the gravitational acceleration, and the velocity can be obtained by integrating the acceleration once; the update algorithm for discretized velocity solution is derived as follows: in, and The time period T = t m -t m-1 The internal navigation system compares the velocity increment and the velocity increment of harmful acceleration; and t m-1 and t m The inertial velocity at the moment is Discretize the above two equations, have The accelerometer is in the time period [t m-1 ,t m ] (T = t m -t m-1 ) was sampled twice at equal intervals, with velocity increments of Δv m1 and Δv m2 , Δv m =Δv m1 +Δv m2 , is the velocity increment of the whole cycle, for The antisymmetric matrix of the two-sample speed paddling error compensation algorithm is: for have Typically, where g U0 is calculated by the normal gravity model formula; based on the northeast sky gravity value in the geographic coordinate system calculated in step S2, we get where g E and g N is the vertical deviation, g U -g U0 is the gravity anomaly, substitute Velocity update and solution are performed in the process, and finally compensation for gravity anomaly and vertical deviation is achieved.

Images