Dual-antenna attitude and orientation method and system based on integrated navigation

Abstract

The invention provides a double-antenna attitude and orientation method and system based on integrated navigation, and the method comprises the steps: combining an INS (inertial navigation system) and a double-antenna GNSS (global navigation satellite system), and enabling the system to output continuous and stable high-precision carrier attitude information when a GNSS signal is interfered by using the provided double-antenna GNSS / INS integrated navigation method. According to the method, a course updating strategy is provided, the motion state of the device is detected, course information at the first static moment in the dynamic and static conversion process is stored in time, and then Kalman filtering course updating is conducted through the information. Even if no course information is output subsequently, the system can output high-precision attitude information for a long time. According to the method, the advantages of the double-antenna GNSS and the advantages of the INS are combined, the orientation precision and the real-time performance of the system are improved when the GNSS signal is unavailable, and the method has good practical value.

Description

Technical Field

[0001] The present invention relates to the technical field of navigation, and particularly to a dual-antenna attitude and heading determination method and system based on integrated navigation. Background Art

[0002] The attitude information of a vehicle is one of the important spatial characteristics of an object and is commonly used in the fields of navigation, guidance, and control. Here, the attitude generally refers to the direction of the vehicle, and the angle information shown is the heading angle, pitch angle, and roll angle respectively. Generally, the following several methods can be used to measure the attitude information of a vehicle: The first is to use an Inertial Navigation System (INS), which measures the acceleration and angular velocity of the vehicle by using an accelerometer and a gyroscope, so as to estimate the attitude information of the vehicle; the second is to use a Global Navigation Satellite System (GNSS) for attitude measurement, and obtain the attitude information of the vehicle during movement by using the carrier phase of the Global Positioning System (GPS); the third is to use geomagnetic attitude measurement, which measures the natural magnetic field of the earth by using a magnetometer, and can also obtain the azimuth information of the vehicle.

[0003] However, geomagnetic attitude measurement is easily interfered with and usually needs to be used in combination with other measurement systems, so its application scenarios are limited. The inertial navigation system has high short-term accuracy and good dynamic response effect, but the disadvantage is that the attitude measurement error will gradually accumulate. GNSS attitude measurement has the advantages of all-weather and high accuracy and belongs to absolute attitude measurement technology. The disadvantages are that it is easily affected by the occlusion environment and the satellite observation quality, and the output frequency is usually low.

[0004] Nowadays, the application requirements of integrated navigation systems in complex environments are gradually increasing, especially in the fields of autonomous driving, driverless, and underground exploration. There is an urgent need to improve the orientation accuracy, extend the effective time limit of inertial navigation recursion, and improve the robustness and reliability of the system. Summary of the Invention

[0005] The present invention provides a dual-antenna attitude and heading determination method and system based on integrated navigation to solve the defect that the pure inertial navigation recursion accuracy has an effective time limit when GNSS is missing in the prior art.

[0006] In a first aspect, the present invention provides a dual-antenna attitude and heading determination method based on integrated navigation, including: Step 1, read IMU data and dual-antenna data. If it is determined that the inertial navigation initialization is successful, proceed to the next step; otherwise, keep the device stationary for a preset duration, and calculate the IMU zero bias, the initial position of the system, the initial velocity, and the initial attitude. Step 2: Perform inertial navigation mechanical scheduling using the read IMU data and conduct Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and determine whether the heading is available according to the preset judgment condition combination. Step 3: If it is determined that the dual-antenna orientation in Step 2 is not successful, judge the status information of the current device. If it is judged that the device is in a static state or less than the first judgment condition in the preset judgment condition combination, use the saved heading for the next step. If it is judged that the device is greater than the first judgment condition in the preset judgment condition combination, do not perform Kalman filter measurement update. Step 4: Use the heading obtained in Step 3 as measurement information for Kalman filter update.

[0007] According to a dual-antenna attitude and orientation method based on integrated navigation provided by the present invention, Step 1 includes: Calculate the zero bias of the IMU:

[0008] where represents the zero bias of the three-axis gyroscope, represents the value of the gyroscope at the i-th moment, and N represents the number of statistics; Calculate the initial position, initial velocity, and initial attitude of the device:

[0009]

[0010] where and respectively represent the roll angle and pitch angle, respectively represent the results of the accelerometers in the

[0011] According to a dual-antenna attitude and orientation method based on integrated navigation provided by the present invention, the Kalman filter in Step 2 includes: Construct the system state equation and observation equation:

[0012]

[0013] In the formula, , are respectively the state vectors of the system at the k-th moment and the (k - 1)-th moment, represents the state one-step transfer matrix from the (k - 1)-th moment to the k-th moment, is the matrix related to noise, is the observation value at the k-th moment, is the observation matrix at the k-th moment, is the system noise, is the observation noise, where and are white noises and are uncorrelated with each other; Construct the state vector:

[0014] where represents the position error, velocity error, attitude error of the integrated navigation, and the bias errors of the gyroscope and accelerometer; Then the Kalman filtering process is:

[0015] .

[0016] According to a dual-antenna attitude and heading determination method based on integrated navigation provided by the present invention, the heading of the dual-antenna obtained by the calculation in step 2 includes: Using the main and auxiliary antennas on the device to perform differential to obtain the baseline vector of the auxiliary antenna relative to the main antenna , then the calculated heading angle and pitch angle are:

[0017] .

[0018] According to a dual-antenna attitude and heading determination method based on integrated navigation provided by the present invention, step 3 specifically includes: If it is determined that the device is in a static state, it is determined that the current heading is the same as the heading at the previous moment, and the heading at the previous moment is used as the measurement value; If it is determined that the device is less than the first determination condition in the preset determination condition combination, it is determined that there is an error between the current heading and the heading at the previous moment and within the preset error range, and the heading at the previous moment is used as the measurement value for Kalman filtering correction; If it is determined that the static flag bit has been cleared, it is determined that the heading at the previous moment is unavailable, and the static flag bit is set and saved as the current heading information as the measurement value, otherwise the heading at the previous moment is directly used as the measurement value; If it is determined that the device is greater than the first determination condition in the preset determination condition combination, it is determined that neither the heading at the previous moment nor the current heading is used as the measurement value for the next moment, and the static flag bit is cleared and no Kalman filtering update is performed.

[0019] According to a dual-antenna attitude and heading determination method based on integrated navigation provided by the present invention, step 4 specifically includes:

[0020]

[0021]

[0022] Among them, is the gain matrix, is the measurement noise covariance matrix, is the optimal state estimate at time k, is the optimal state covariance estimate at time k. is the measurement value at time k.

[0023] In a second aspect, the present invention further provides a dual-antenna attitude and heading reference system based on integrated navigation, including: A first processing module, configured to read IMU data and dual-antenna data. If it is determined that the inertial navigation initialization is successful, proceed to the next step. Otherwise, place the device static for a preset duration, and calculate the IMU zero bias, system initial position, initial velocity, and initial attitude; A second processing module, configured to perform inertial navigation mechanical scheduling using the read IMU data and perform Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and combine and judge whether the heading is available according to a preset judgment condition; A third processing module, configured to, if it is determined that the dual-antenna orientation in the second processing module is not successful, judge the status information of the current device. If it is determined that the device is in a static state or less than the first judgment condition in the preset judgment condition combination, use the saved heading for the next step. If it is determined that the device is greater than the first judgment condition in the preset judgment condition combination, do not perform Kalman filter measurement update; A fourth processing module, configured to perform Kalman filter update using the heading obtained by the third processing module as measurement information.

[0024] In a third aspect, the present invention further provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the dual-antenna attitude and heading reference method based on integrated navigation as described in any one of the above.

[0025] In a fourth aspect, the present invention further provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it implements the dual-antenna attitude and heading reference method based on integrated navigation as described in any one of the above.

[0026] The dual-antenna attitude and heading determination method and system based on integrated navigation provided by the present invention combine INS with a dual-antenna GNSS system, and use the dual-antenna GNSS / INS integrated navigation method to enable the system to output continuous, stable, and high-precision carrier attitude information when the GNSS signal is interfered. In the present invention, a heading update strategy is proposed. By detecting the motion state of the device and timely saving the heading information at the first stationary moment during the dynamic and static conversion process, and then using this information for Kalman filter heading update, even if no heading information is output subsequently, it can ensure that the system outputs high-precision attitude information for a long time. This method combines the respective advantages of dual-antenna GNSS and INS, improves the orientation accuracy and real-time performance of the system when the GNSS signal is unavailable, and has good practical value. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0028] Figure 1 is one of the schematic flowcharts of the dual-antenna attitude and heading determination method based on integrated navigation provided by the present invention; Figure 2 is the second schematic flowchart of the dual-antenna attitude and heading determination method based on integrated navigation provided by the present invention; Figure 3 is the schematic diagram of the structure composition of the dual-antenna GNSS / INS device provided by the present invention; Figure 4 is the schematic diagram of the principle of inter-satellite difference and receiver difference provided by the present invention; Figure 5 is the schematic diagram of the structure of the dual-antenna attitude and heading determination system based on integrated navigation provided by the present invention; Figure 6 is the schematic diagram of the structure of the electronic device provided by the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0029] To make the objectives, technical solutions, and advantages of the present invention clearer, the following will clearly and completely describe the technical solutions in the present invention in conjunction with the drawings in the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. Based on the embodiments in the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0030] In view of the deficiencies existing in the prior art, the present invention proposes a high-precision attitude and heading reference system and device based on dual-antenna GNSS / INS integrated navigation, which utilizes the characteristics of GNSS and INS to complement each other, and uses the dual-antenna GNSS carrier-phase double difference to eliminate most of the satellite clock error, ionospheric and tropospheric errors, and receiver clock error. At the same time, a strategy for processing heading information during the operation of the GNSS / INS integrated navigation system is proposed to obtain more robust, continuous, and high-precision attitude information.

[0031] Figure 1 It is one of the schematic flowcharts of the dual-antenna attitude and heading reference method based on integrated navigation provided by the embodiments of the present invention. As Figure 1 shown, it includes: Step 1: Read IMU data and dual-antenna data. If it is determined that the inertial navigation initialization is successful, proceed to the next step; otherwise, keep the device stationary for a preset duration, and calculate the IMU zero bias, system initial position, initial velocity, and initial attitude. Step 2: Use the read IMU data for inertial navigation mechanization and perform Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and combine and judge whether the heading is available according to preset judgment conditions. Step 3: If it is determined that the dual-antenna orientation in Step 2 is not successful, judge the status information of the current device. If it is judged that the device is in a static state or less than the first judgment condition in the preset judgment condition combination, use the saved heading for the next step; if it is judged that the device is greater than the first judgment condition in the preset judgment condition combination, do not perform Kalman filter measurement update. Step 4: Use the heading obtained in Step 3 as measurement information for Kalman filter update.

[0032] Specifically, as Figure 2 shown, it includes: Step 1: Read IMU data and dual-antenna data. Judge whether the inertial navigation is initialized successfully. If it is not initialized successfully, the device needs to be stationary for 30 s, calculate the zero bias of the IMU, and calculate the initial position, velocity, and attitude of the system. If the initialization is successful, proceed to the next step.

[0033] Step 2: Use the read IMU data for inertial navigation mechanization and perform Kalman filter prediction. At the same time, judge whether the dual-antenna orientation is successful. If it is successful, calculate the heading of the dual-antenna, and judge whether the heading is available according to conditions such as "whether the heading change within 1 second before and after is less than 1°" and "dynamic rotation speed is less than 5° / s".

[0034] Step 3: If the dual-antenna orientation in Step 2 is not successful, judge the status information of the current device. If it is in a static state or the dynamic rotation speed is less than 5° / s, use the saved heading for the next step. If the dynamic rotation speed of the device is greater than 5° / s, choose to end directly without performing Kalman filter measurement update.

[0035] Step 4: Use the heading obtained in Step 3 as the measurement information to perform Kalman filter update.

[0036] The present invention combines the respective advantages of dual-antenna GNSS and INS, improves the orientation accuracy and real-time performance of the system when GNSS signals are unavailable, and has good practical value.

[0037] Based on the above embodiments, some information at the initial moment of the system is calculated in Step 1, and the specific content is as follows: Calculate the zero bias of the IMU:

[0038] Where represents the zero bias of the three-axis gyroscope, represents the gyro value at the i-th moment, and N represents the number of statistics; Calculate the initial position, initial velocity, and initial attitude of the device:

[0039]

[0040] Where and represent the roll angle and pitch angle respectively, respectively represent the results of the accelerometer in the

[0041] Step 2: Use the read IMU data for inertial navigation mechanical arrangement, establish a suitable Kalman filter model and perform the Kalman filter prediction process. At the same time, judge whether the dual-antenna orientation is successful and calculate the heading of the dual-antenna as follows: Kalman filter: First, a suitable system state needs to be selected. Here, a classic 15-dimensional GNSS / INS integrated navigation filter model is selected.

[0042]

[0043] Among them, represents the position error, velocity error, attitude error, gyroscope, and accelerometer zero bias error of the integrated navigation.

[0044] Then construct the system state equation:

[0045] In the formula, and are the state vectors of the system at time k and time k - 1 respectively, represents the one-step state transition matrix from time k - 1 to time k, is a matrix related to noise, is the system noise. The state transition matrix of the Kalman filter model can be expressed as:

[0046] Among them, , is a three-dimensional identity matrix, is the specific force in the n system, and are the correlation times of the gyroscope zero bias and the accelerometer zero bias respectively.

[0047] Finally, construct the system observation equation:

[0048] In the formula, is the observation value at time k, is the observation matrix at time k, is the predicted value of the state vector, is the observation noise. The observation matrix is expressed as:

[0049] Dual-antenna heading calculation: For positioning and orientation, GNSS observation values of pseudorange and carrier phase are required. The observation equation is established as:

[0050]

[0051] where P represents the pseudorange observation value, represents the carrier phase observation value, represents the frequency identifier, represents the wavelength of the corresponding frequency signal, represents the geometric distance from the receiver to the GNSS satellite, represents the receiver, represents the receiver clock bias, represents the GPS satellite clock bias, I represents the ionospheric delay of the signal, T represents the tropospheric delay, N represents the ambiguity, M represents the multipath effect error, represents the observation noise.

[0052] Linearize the pseudorange measurement. Two receivers r1 and r2 simultaneously observe satellites G1 and G2. For example,Figure 3 Schematic diagram of the shown structure composition. The receiver makes differences in the observations relative to G1 respectively to obtain the single-difference equation of G1:

[0053]

[0054] Where represents the baseline vector between receivers r1 and r2. At this moment, the satellite clock error is eliminated, and the ionospheric and tropospheric errors are weakened.

[0055] Then, similarly, the receiver r1 and r2 make differences in the observed values of G2 to obtain the above equation of G2. Finally, the single-difference observation equations of satellites G1 and G2 are again differenced between satellites, as Figure 4 shown, to further eliminate the receiver-end clock error and further weaken the ionospheric and tropospheric errors, then we get:

[0056]

[0057] It can be seen that only the baseline vector to be solved and the double-difference ambiguity of the carrier phase remain in the formula.

[0058] At this time, if M satellites are simultaneously observed by receivers r1 and r2, taking G1 as the reference satellite, the following observation equation system can be listed:

[0059] At this time, the floating-point solution results of the baseline vector and the double-difference ambiguity can be solved. Further using the LAMBDA algorithm to fix the integer ambiguity, the fixed solution of the baseline vector can be solved. Thus, the heading angle and pitch angle can be further obtained through the following formulas:

[0060]

[0061] Step 3: Perform the next operation according to the result of the dual-antenna orientation determined in Step 2 and the motion state of the current device. When the dual-antenna orientation is not successful, if it is in a stationary state at this time, it means that the heading is the same as that of the previous moment, and the system can use the heading information of the previous moment as the measurement value. If it is in a dynamic state but the dynamic rotation speed is less than 5° / s at this time, it means that there is a certain but small error between the current heading and that of the previous moment, and it can still be used as the measurement value for Kalman filter correction. At this time, it can be judged whether the static flag bit is cleared. If it is cleared, it means that the heading information obtained at the previous moment is unavailable. At this time, the static flag bit can be set and the heading information in the above two cases can be saved as the measurement value. If it is not cleared, the previously saved heading value is directly used as the measurement value. When the dynamic rotation speed is greater than or equal to 5° / s, at this time, the heading value of the previous moment is not suitable as the measurement value, and the current heading value is not suitable as the measurement value for the next moment either. Therefore, the static flag bit is cleared and the Kalman filter is not updated. At this time, the system behaves like a state without GNSS, and the attitude of the system is directly deduced using the pure inertial mechanical arrangement to maintain a certain attitude accuracy of the system in a short time.

[0062] Step 4: Mainly use the heading information obtained in Step 3 as the measurement value to update the Kalman filter. The process is as follows:

[0063]

[0064]

[0065] In the formula, is the gain matrix, is the measurement noise covariance matrix, is the optimal state estimate value at time k, is the optimal state covariance estimate value at time k. is the measurement value at time k.

[0066] Next, the dual-antenna attitude and heading determination system based on integrated navigation provided by the present invention will be described. The dual-antenna attitude and heading determination system based on integrated navigation described below can be mutually referred to and corresponded with the dual-antenna attitude and heading determination method described above.

[0067] Figure 5 is a schematic structural diagram of the dual-antenna attitude and heading determination system based on integrated navigation provided by an embodiment of the present invention. As Figure 5 shown, it includes: a first processing module 51, a second processing module 52, a third processing module 53, and a fourth processing module 54, where: The first processing module 51 is used to read IMU data and dual-antenna data. If it is determined that the inertial navigation initialization is successful, the next step is entered; otherwise, the device is left stationary for a preset duration, and the IMU zero bias, the initial position of the system, the initial velocity, and the initial attitude are calculated. The second processing module 52 is used to perform inertial navigation mechanical scheduling using the read IMU data and perform Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, the heading of the dual-antenna is calculated, and it is determined whether the heading is available according to a preset combination of judgment conditions. The third processing module 53 is used to, if it is determined that the dual-antenna orientation in the second processing module is not successful, judge the status information of the current device. If it is determined that the device is in a static state or less than the first judgment condition in the preset combination of judgment conditions, the saved heading is used for the next step. If it is determined that the device is greater than the first judgment condition in the preset combination of judgment conditions, the Kalman filter measurement update is not performed. The fourth processing module 54 is used to perform Kalman filter update using the heading obtained by the third processing module as measurement information.

[0068] Figure 6 An example of the physical structure diagram of an electronic device is shown in Figure 6 As shown, the electronic device may include: a processor 610, a communication interface 620, a memory 630, and a communication bus 640. Among them, the processor 610, the communication interface 620, and the memory 630 complete mutual communication through the communication bus 640. The processor 610 can call the logical instructions in the memory 630 to execute the dual-antenna attitude and heading determination method based on integrated navigation. The method includes: Step 1, read IMU data and dual-antenna data. If it is determined that the inertial navigation initialization is successful, enter the next step; otherwise, leave the device stationary for a preset duration, and calculate the IMU zero bias, the initial position of the system, the initial velocity, and the initial attitude. Step 2, perform inertial navigation mechanical scheduling using the read IMU data and perform Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and determine whether the heading is available according to a preset combination of judgment conditions. Step 3, if it is determined that the dual-antenna orientation in Step 2 is not successful, judge the status information of the current device. If it is determined that the device is in a static state or less than the first judgment condition in the preset combination of judgment conditions, use the saved heading for the next step. If it is determined that the device is greater than the first judgment condition in the preset combination of judgment conditions, do not perform the Kalman filter measurement update. Step 4, perform Kalman filter update using the heading obtained in Step 3 as measurement information.

[0069] In addition, when the logical instructions in the above-mentioned memory 630 are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs.

[0070] On the other hand, the present invention also provides a computer program product. The computer program product includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the dual-antenna attitude and heading determination method based on integrated navigation provided by the above-mentioned various methods. The method includes: Step 1, read IMU data and dual-antenna data. If it is determined that the inertial navigation initialization is successful, proceed to the next step; otherwise, keep the device stationary for a preset duration, and calculate the IMU zero bias, the initial position, the initial velocity, and the initial attitude of the system. Step 2, perform inertial navigation mechanical arrangement using the read IMU data and conduct Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and combine and judge whether the heading is available according to a preset judgment condition. Step 3, if it is determined that the dual-antenna orientation in Step 2 is not successful, judge the status information of the current device. If it is determined that the device is in a static state or less than the first judgment condition in the preset judgment condition combination, use the saved heading for the next step. If it is determined that the device is greater than the first judgment condition in the preset judgment condition combination, do not perform Kalman filter measurement update. Step 4, use the heading obtained in Step 3 as measurement information to perform Kalman filter update.

[0071] In another aspect, the present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it implements the dual-antenna attitude and heading determination method based on integrated navigation provided by the above various methods. The method includes: Step 1, read IMU data and dual-antenna data. If it is determined that the inertial navigation initialization is successful, proceed to the next step; otherwise, keep the device stationary for a preset duration, and calculate the IMU zero bias, the initial position of the system, the initial velocity, and the initial attitude. Step 2, perform inertial navigation mechanical arrangement using the read IMU data and conduct Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and combine and determine whether the heading is available according to a preset judgment condition. Step 3, if it is determined that the dual-antenna orientation in Step 2 is not successful, judge the status information of the current device. If it is determined that the device is in a static state or less than the first judgment condition in the preset judgment condition combination, use the saved heading for the next step. If it is determined that the device is greater than the first judgment condition in the preset judgment condition combination, do not perform Kalman filter measurement update. Step 4, use the heading obtained in Step 3 as measurement information to perform Kalman filter update.

[0072] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. A person of ordinary skill in the art can understand and implement it without creative effort.

[0073] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, and of course, also by hardware. Based on this understanding, the essence of the above technical solution, or the part that contributes to the prior art, can be embodied in the form of a software product. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0074] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A dual-antenna attitude and heading determination method based on integrated navigation, characterized in that Including: Step 1: Read the data of the inertial measurement unit (IMU) and the dual-antenna data. If it is determined that the inertial navigation initialization is successful, proceed to the next step; otherwise, keep the device stationary for a preset duration and calculate the IMU zero bias, the initial position, the initial velocity, and the initial attitude of the system. Step 2: Use the read IMU data for inertial navigation mechanical arrangement and perform Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and determine whether the heading is available according to the preset combination of judgment conditions. Step 3: If it is determined that the dual-antenna orientation in Step 2 is not successful, judge the status information of the current device. If it is determined that the device is in a static state or less than the first judgment condition in the preset combination of judgment conditions, use the saved heading for the next step; if it is determined that the device is greater than the first judgment condition in the preset combination of judgment conditions, do not perform Kalman filter measurement update. Step 4: Use the heading obtained in Step 3 as the measurement information for Kalman filter update.

2. The dual-antenna attitude and heading determination method based on integrated navigation according to claim 1, wherein Step 1 includes: Calculate the zero bias of the IMU: wherein represents the zero bias of the three-axis gyroscope, represents the value of the gyroscope at the i-th moment, and N represents the number of statistical times; Calculate the initial position, the initial velocity, and the initial attitude of the device: wherein and respectively represent the roll angle and the pitch angle, respectively represent the results of the accelerometer in the direction.

3. The dual-antenna attitude and heading determination method based on integrated navigation according to claim 1, wherein The Kalman filter in Step 2 includes: Construct the system state equation and the observation equation: wherein, and are the state vectors of the system at time k and time k-1 respectively, represents the one-step state transition matrix from time k-1 to time k, is the matrix related to noise, is the observation value at time k, is the observation matrix at time k, is the system noise, is the observation noise, where and are white noises and are uncorrelated with each other; Construct the state vector: Among them, represents the position error, velocity error, attitude error of the integrated navigation, and the zero bias errors of the gyroscopes and accelerometers; Then the Kalman filter process is: 。 4. The method for dual-antenna attitude and heading determination based on integrated navigation according to claim 1, wherein The calculation of the heading of the dual-antenna obtained in Step 2 includes: The differential is performed using the main and auxiliary antennas on the device to obtain the baseline vector of the auxiliary antenna relative to the main antenna , and then the heading angle and pitch angle are calculated as follows: 。 5. The dual-antenna attitude and heading determination method based on integrated navigation according to claim 1, wherein Step 3 specifically includes: If it is determined that the device is in a static state, determine that the current heading is the same as the heading at the previous moment, and use the heading at the previous moment as the measurement value; If it is determined that the device is less than the first judgment condition in the preset combination of judgment conditions, determine that there is an error between the current heading and the heading at the previous moment and within the preset error range, and use the heading at the previous moment as the measurement value for Kalman filter correction; If it is determined that the static flag bit has been cleared, determine that the heading at the previous moment is unavailable, set the static flag bit and save it as the current heading information as the measurement value; otherwise, directly use the heading at the previous moment as the measurement value; If it is determined that the device is greater than the first judgment condition in the preset combination of judgment conditions, determine that neither the heading at the previous moment nor the current heading is used as the measurement value for the next moment, clear the static flag bit and do not perform Kalman filter update.

6. The method for dual-antenna attitude and heading determination based on integrated navigation according to claim 1, wherein Step 4 specifically includes: Among them, is the gain matrix, is the measurement noise covariance matrix, is the optimal state estimate at time k, is the optimal state covariance estimate at time k. is the measurement value at time k.

7. A dual-antenna attitude and heading reference system based on integrated navigation, characterized in that, Including: The first processing module is used to read the IMU data and the dual-antenna data. If it is determined that the inertial navigation initialization is successful, proceed to the next step; otherwise, keep the device stationary for a preset duration and calculate the IMU zero bias, the initial position, the initial velocity, and the initial attitude of the system. The second processing module is used to use the read IMU data for inertial navigation mechanical arrangement and perform Kalman filter prediction. If it is determined that the dual-antenna orientation is successful, calculate the heading of the dual-antenna, and determine whether the heading is available according to the preset combination of judgment conditions. The third processing module is used to, if it is determined that the dual-antenna orientation in the second processing module is not successful, judge the status information of the current device. If it is determined that the device is in a static state or less than the first judgment condition in the preset combination of judgment conditions, use the saved heading for the next step; if it is determined that the device is greater than the first judgment condition in the preset combination of judgment conditions, do not perform Kalman filter measurement update. The fourth processing module is used to use the heading obtained by the third processing module as the measurement information for Kalman filter update.

8. An electronic device, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the dual-antenna attitude and orientation determination method based on integrated navigation according to any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium storing a computer program thereon, characterized in that, When the computer program is executed by a processor, it implements the dual-antenna attitude and orientation determination method based on integrated navigation according to any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the dual-antenna attitude and orientation determination method based on integrated navigation according to any one of claims 1 to 6.

Images