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

Abstract

The application provides a dual-antenna attitude determination and orientation method and system based on combined navigation, which comprises the following steps: combining an INS and a dual-antenna GNSS system together, and utilizing the proposed dual-antenna GNSS / INS combined navigation method to enable the system to output continuous and stable high-precision carrier attitude information when the GNSS signal is disturbed. In the method, a heading update strategy is proposed, the heading information of the first stationary time in the dynamic and static conversion process is saved in time by detecting the motion state of the device, and then the information is utilized for Kalman filtering heading update, so that the system can output high-precision attitude information for a long time even if no heading information is output subsequently. The method combines the respective advantages of the dual-antenna GNSS and the INS, improves the orientation accuracy and real-time performance of the system when the GNSS signal is unavailable, and has good practical value.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of navigation technology, and in particular to a dual-antenna attitude orientation method and system based on integrated navigation. BACKGROUND

[0002] The attitude information of a carrier is one of the important spatial characteristics of an object, and is often used in the fields of navigation, guidance and control. Here, the attitude usually refers to the direction of the carrier, and the angle information exhibited is the heading angle, the pitch angle and the roll angle. Generally, the following methods can be used to measure the attitude information of the carrier: the first method is to use an inertial navigation system (INS) to measure the acceleration and angular velocity of the carrier by using an accelerometer and a gyroscope, so as to estimate the attitude information of the carrier; the second method is to use a global navigation satellite system (GNSS) to measure the attitude, and the attitude information of the carrier in motion is obtained by using the global positioning system (GPS) carrier phase; the third method is to use a geomagnetic attitude measurement, and the azimuth information of the carrier can also be obtained by measuring the earth's natural magnetic field by using a magnetometer.

[0003] However, the magnetometer attitude measurement is susceptible to interference, and usually needs to be used together with other measurement systems, and the 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. The GNSS attitude measurement has the advantages of all-weather and high accuracy, and belongs to the absolute attitude measurement technology. The disadvantage is that it is easily affected by the shielding environment and the satellite observation quality, and the output frequency is usually low.

[0004] Nowadays, the application demand of integrated navigation systems in complex environments is gradually increasing, especially in the fields of automatic driving, unmanned driving, underground exploration and the like, and it is urgent to improve the orientation accuracy and prolong the effective time limit of inertial navigation recursion, and to improve the robustness and reliability of the system. SUMMARY

[0005] The present application provides a dual-antenna attitude orientation 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 application provides a dual-antenna attitude orientation method based on integrated navigation, comprising:

[0007] Step 1: read the IMU data and dual-antenna data, if it is judged that the inertial navigation initialization is successful, then go to the next step, otherwise, place the device for a preset time length, calculate the IMU zero offset, the initial position of the system, the initial speed and the initial attitude;

[0008] Step 2, using the read IMU data to carry out inertial navigation mechanical arrangement, and carrying out Kalman filter prediction, if judging that the dual antenna orientation is successful, the heading of the dual antenna is calculated, and whether the heading is available is judged according to a preset judging condition combination;

[0009] Step 3, if judging that the dual antenna orientation in step 2 is not successful, the state information of the current device is judged, if judging that the device is in static or less than the first judging condition in the preset judging condition combination, the saved heading is used to carry out the next step, if judging that the device is greater than the first judging condition in the preset judging condition combination, the Kalman filter measurement update is not carried out;

[0010] Step 4, using the heading obtained in step 3 as measurement information to carry out Kalman filter update.

[0011] According to the dual antenna orientation method for positioning and orientation based on combined navigation provided by the application, step 1 comprises:

[0012] The zero offset of the IMU is calculated:

[0013]

[0014] Wherein represents the zero offset of the three-axis gyroscope, represents the value of the gyroscope at i moment, and N represents the number of statistics;

[0015] The initial position, initial speed and initial attitude of the device are calculated:

[0016]

[0017]

[0018] Wherein and respectively represent the roll angle and the pitch angle, respectively represent the results of the accelerometer in the direction.

[0019] According to the dual antenna orientation method for positioning and orientation based on combined navigation provided by the application, the Kalman filter in step 2 comprises:

[0020] The system state equation and the observation equation are constructed:

[0021]

[0022]

[0023] In the formula, , are respectively the state vectors of the system at k moment and k-1 moment, a state transition matrix at k-1 to k, is a noise correlation matrix, is an observation value at k, is an observation matrix at k, is a system noise, is an observation noise, wherein and are white noises and are independent of each other;

[0024] constructing a state vector:

[0025]

[0026] wherein, denotes a position error, a velocity error, an attitude error, a zero bias error of a gyroscope and an accelerometer of the integrated navigation;

[0027] then the Kalman filtering process is:

[0028]

[0029] .

[0030] According to the double-antenna attitude and orientation method based on integrated navigation provided by the application, the heading of the double antenna obtained by calculation in step 2 comprises:

[0031] The baseline vector of the secondary antenna relative to the primary antenna is obtained by using the primary antenna and the secondary antenna on the device to make a difference then the heading angle and the pitch angle are calculated as:

[0032]

[0033] .

[0034] According to the double-antenna attitude and orientation method based on integrated navigation provided by the application, step 3 specifically comprises:

[0035] If it is judged that the device is in a static state, it is determined that the current heading is the same as the heading at the last time, and the heading at the last time is used as the measurement value;

[0036] If it is judged that the device is less than the first judgment condition in the preset judgment condition set, it is determined that the current heading has an error with the heading at the last time and is within a preset error range, and the heading at the last time is used as the measurement value for Kalman filtering correction;

[0037] If it is judged that the static flag bit has been cleared, it is determined that the heading at the last time is unusable, the static flag bit is set to save the current heading information as the measurement value, otherwise, the heading at the last time is directly used as the measurement value.

[0038] If the device is determined to be greater than the first determination condition in the preset determination condition combination, it is determined that neither the previous heading nor the current heading is used as the measurement value of the next time, the static flag bit is cleared and Kalman filter updating is not performed.

[0039] According to the double-antenna attitude and orientation determination method based on integrated navigation provided by the application, step 4 specifically comprises:

[0040]

[0041]

[0042]

[0043] wherein, is a gain matrix, is a measurement noise covariance matrix, is an optimal state estimation value at time k, is an optimal state covariance estimation value at time k. is a measurement value at time k.

[0044] In the second aspect, the application further provides a double-antenna attitude and orientation determination system based on integrated navigation, comprising:

[0045] The first processing module is configured to read IMU data and double-antenna data, and if the inertial navigation initialization is determined to be successful, the next step is entered, otherwise the device is placed for a preset time length, and the IMU zero offset, the initial position, the initial speed and the initial attitude of the system are calculated.

[0046] The second processing module is configured to perform inertial navigation mechanical arrangement using the read IMU data, and perform Kalman filter prediction, and if the double-antenna orientation is determined to be successful, the heading of the double-antenna is calculated, and it is determined whether the heading is available according to a preset determination condition combination.

[0047] The third processing module is configured to determine the state information of the current device if the double-antenna orientation in the second processing module is determined to be unsuccessful, and if the device is determined to be in a static state or less than the first determination condition in the preset determination condition combination, the saved heading is used for the next step, and if the device is determined to be greater than the first determination condition in the preset determination condition combination, Kalman filter measurement updating is not performed.

[0048] The fourth processing module is configured to perform Kalman filter updating using the heading obtained by the third processing module as measurement information.

[0049] In a third aspect, the present application provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the dual-antenna positioning and orientation method based on integrated navigation according to any one of the above aspects when executing the program.

[0050] In a fourth aspect, the present application provides a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the dual-antenna positioning and orientation method based on integrated navigation according to any one of the above aspects.

[0051] The dual-antenna positioning and orientation method based on integrated navigation provided by the present application combines INS and dual-antenna GNSS systems together, and uses the dual-antenna GNSS / INS integrated navigation method to enable the system to output continuous and stable high-precision carrier attitude information when GNSS signals are disturbed. In the present application, a heading update strategy is proposed, which detects the motion state of the device, saves the heading information at the first stationary time during the dynamic-to-static conversion process in time, and then uses the information to perform Kalman filtering heading update, so that even if there is no heading information output subsequently, the system can still output 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 GNSS signals are unavailable, and has good practical value. BRIEF DESCRIPTION OF DRAWINGS

[0052] In order to more clearly illustrate the technical solutions in the present application or the prior art, the following will briefly introduce the drawings needed in the embodiments or prior art description. Obviously, the drawings described below are some embodiments of the present application, and those skilled in the art can obtain other drawings according to these drawings without creative effort.

[0053] Figure 1 is one of the flowcharts of the dual-antenna positioning and orientation method based on integrated navigation provided by the present application;

[0054] Figure 2 is another flowchart of the dual-antenna positioning and orientation method based on integrated navigation provided by the present application;

[0055] Figure 3 is a structural composition diagram of the dual-antenna GNSS / INS device provided by the present application;

[0056] Figure 4 is a schematic diagram of the inter-satellite differential and receiver differential principle provided by the present application;

[0057] Figure 5 is a structural diagram of the dual-antenna positioning and orientation system based on integrated navigation provided by the present application;

[0058] Figure 6 Figure 1 is a structural schematic diagram of an electronic device provided by the present application. DETAILED DESCRIPTION

[0059] In order to make the objectives, technical solutions and advantages of the present application clearer, the technical solutions in the present application will be described clearly and completely below with reference to the drawings in the present application. Obviously, the described embodiments are some embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

[0060] In view of the defects in the prior art, the present application provides a high-precision pose orientation system and device based on a dual-antenna GNSS / INS integrated navigation, which utilizes the characteristics of GNSS and INS for complementation, and utilizes dual-antenna GNSS carrier phase double difference to eliminate most satellite end clock error, ionosphere and troposphere error, and receiver end clock error. Meanwhile, a strategy for processing heading information during operation of the GNSS / INS integrated navigation system is provided, so as to obtain more robust, continuous and high-precision attitude information.

[0061] Figure 1 Figure 1 is a flowchart of a dual-antenna pose orientation method based on integrated navigation provided by an embodiment of the present application, as shown in Figure 1, comprising the following steps. Figure 1

[0062] Step 1, reading IMU data and dual-antenna data, if it is judged that the inertial navigation initialization is successful, then entering the next step, otherwise, placing the device for a preset time length, calculating the IMU zero offset, the initial position of the system, the initial speed and the initial attitude;

[0063] Step 2, using the read IMU data to perform inertial navigation mechanical arrangement, and performing Kalman filter prediction, if it is judged that the dual-antenna orientation is successful, then calculating the heading of the dual-antenna, and judging whether the heading is available according to a preset judgment condition combination;

[0064] Step 3, if it is judged that the dual-antenna orientation in step 2 is not successful, then judging the state information of the current device, if it is judged that the device is in static or less than the first judgment condition in the preset judgment condition combination, then using the saved heading to perform the next step, if it is judged that the device is greater than the first judgment condition in the preset judgment condition combination, then not performing Kalman filter measurement update;

[0065] Step 4, using the heading obtained in step 3 as measurement information to perform Kalman filter update.

[0066] Specifically, as shown in Figure 1, comprising the following steps. Figure 2 ​​

[0067] Step one: read IMU data and dual antenna data. Determine whether the inertial navigation is successfully initialized, if not, the device needs to be stationary for 30s, calculate the zero offset of IMU and calculate the initial position, velocity and attitude of the system. If the initialization is successful, proceed to the next step.

[0068] Step two: use the read IMU data to perform inertial navigation mechanical arrangement, and perform Kalman filter prediction. At the same time, determine whether the dual antenna orientation is successful. If successful, the heading of the dual antenna is calculated, and the conditions such as "whether the heading change in the next 1 second is less than 1°" and "dynamic rotation speed is less than 5° / s" are used to determine whether the heading is available.

[0069] Step three: if the dual antenna orientation in step two is not successful, the state information of the current device is determined. If it is static or the dynamic rotation speed is less than 5° / s, the saved heading is used for the next step. If the dynamic rotation speed of the device is greater than 5° / s, the process is directly ended without Kalman filter measurement update.

[0070] Step four: use the heading obtained in step three as measurement information to perform Kalman filter update.

[0071] The application combines the advantages of dual antenna GNSS and INS, improves the directional accuracy and real-time performance of the system when GNSS signal is unavailable, and has good practical value.

[0072] Based on the above embodiment, some information of the system at the initial time is calculated in step 1, and the specific content is as follows:

[0073] Calculate the zero offset of IMU:

[0074]

[0075] Wherein represents the zero offset of the three-axis gyroscope, represents the value of the gyroscope at time i, and N represents the number of statistics;

[0076] Calculate the initial position, initial velocity and initial attitude of the device:

[0077]

[0078]

[0079] Wherein and respectively represent the roll angle and the pitch angle, respectively represent the results of the accelerometer in the x direction and the y direction.

[0080] Step two: use the read IMU data to do inertial navigation mechanical arrangement, and establish a suitable Kalman filter model and 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:

[0081] Kalman filter:

[0082] First, the appropriate system state needs to be selected. Here, the classic 15-dimensional GNSS / INS integrated navigation filter model is selected.

[0083]

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

[0085] Then build the system state equation:

[0086]

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

[0088]

[0089] Among them, , is a three-dimensional unit matrix, is the specific force under n, and are the gyroscope and accelerometer bias related time respectively.

[0090] Finally, build the system observation equation:

[0091]

[0092] 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:

[0093]

[0094] Dual antenna heading calculation:

[0095] The positioning and orientation need to use GNSS observation values for pseudo-range and carrier phase, and the observation equation is established as:

[0096]

[0097]

[0098] wherein P represents a pseudo-range observation value, represents a carrier phase observation value, represents a frequency identifier, represents a wavelength of a corresponding frequency signal, represents a geometric distance from a receiver to a GNSS satellite, represents a receiver, represents a receiver clock error, represents a GPS satellite end clock error, I represents an ionospheric delay, T represents a tropospheric delay, N represents an ambiguity, and M represents a multipath effect error, represents an observation noise.

[0099] The pseudo-range measurement is linearized, and two receivers r1 and r2 simultaneously observe satellites G1 and G2, as shown in the structural composition diagram of FIG. 1, the observations of the receivers with respect to G1 are subtracted, respectively, to obtain a single-difference equation of G1: Figure 3

[0100]

[0101]

[0102] wherein represents a baseline vector of the receivers r1 and r2. At this moment, the satellite clock error is eliminated, and the ionospheric and tropospheric errors are weakened.

[0103] Then, the observations of the receivers r1 and r2 with respect to G2 are subtracted, respectively, to obtain the above equation of G2. Finally, the single-difference observation equations of the two satellites G1 and G2 are subtracted again, as shown in FIG. 2, to further eliminate the receiver end clock error and further weaken the ionospheric and tropospheric errors, and then the following equation is obtained: Figure 4

[0104]

[0105]

[0106] It can be seen that only the baseline vector to be solved and the double-difference ambiguity of the carrier phase are left in the equation.

[0107] ​​If M satellites are simultaneously observed by receivers r1 and r2, and G1 is used as the reference satellite, the observation equations can be set up as follows:

[0108]

[0109] At this point, the floating-point solutions for the baseline vector and double-difference ambiguity can be obtained. Further, by using the LAMBDA algorithm to fix the integer ambiguity, the baseline vector can be solved. The fixed solution. Therefore, the heading angle and pitch angle can be further calculated using the following formulas:

[0110]

[0111]

[0112] Step 3 proceeds to the next operation based on the dual-antenna orientation result determined in Step 2 and the current motion state of the device. If dual-antenna orientation fails, and the device is stationary, the heading is the same as the previous moment, and the system can use the previous heading information as the measurement value. If the device is in motion but the dynamic rotation speed is less than 5° / s, the heading has a small but significant error compared to the previous moment, and can still be used as a measurement value for Kalman filtering correction. At this point, it can be determined whether the static flag is cleared. If it is cleared, the heading information obtained in the previous moment is unusable, and the static flag can be set and the heading information in both cases can be saved as the measurement value. If it is not cleared, the previously saved heading value is used directly as the measurement value. When the dynamic rotation speed is greater than or equal to 5° / s, the heading value from the previous moment is not suitable as a measurement value, and the current heading value is also not suitable as a measurement value for the next moment. Therefore, the static flag is cleared and no Kalman filtering update is performed. At this point, the system is in a state without GNSS, and the attitude of the system is recursively calculated by using pure mechanical inertial navigation, thereby maintaining a certain attitude accuracy of the system for a short period of time.

[0113] Step four mainly uses the heading information obtained in step three as the measurement value for Kalman filtering update, and the process is as follows:

[0114]

[0115]

[0116]

[0117] In the formula, Here is the gain matrix. To measure the noise covariance matrix, This is the optimal estimate of the state at time k. This is the optimal estimate of the state covariance at time k. is the measurement value at time k.

[0118] The following describes a dual-antenna attitude and orientation system based on combined navigation provided by the present application. The dual-antenna attitude and orientation system based on combined navigation described below can be referred to in correspondence with the dual-antenna attitude and orientation method based on combined navigation described above.

[0119] Figure 5 is a structural schematic diagram of the dual-antenna attitude and orientation system based on combined navigation provided by an embodiment of the present application, as shown in Figure 5 includes a first processing module 51, a second processing module 52, a third processing module 53, and a fourth processing module 54, wherein:

[0120] The first processing module 51 is configured 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 placed for a preset time length, and the IMU zero offset, the initial position of the system, the initial speed, and the initial attitude are calculated. The second processing module 52 is configured to perform inertial navigation mechanical arrangement using the read IMU data, and perform Kalman filtering 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 determination condition combination. The third processing module 53 is configured to determine the state information of the current device if it is determined that the dual-antenna orientation in the second processing module is not successful. If it is determined that the device is in a static state or is less than a first determination condition in the preset determination condition combination, the saved heading is used for the next step. If it is determined that the device is greater than the first determination condition in the preset determination condition combination, no Kalman filtering measurement update is performed. The fourth processing module 54 is configured to perform Kalman filtering update using the heading obtained by the third processing module as measurement information.

[0121] Figure 6 An example of an entity structure schematic diagram of an electronic device is shown in Figure 6As shown, the electronic device can include a processor 610, a communications interface 620, a memory 630, and a communications bus 640, wherein the processor 610, the communications interface 620, and the memory 630 complete mutual communication through the communications bus 640. The processor 610 can invoke a logic instruction in the memory 630 to execute a dual-antenna pose orientation method based on combined navigation, which includes: step 1, reading IMU data and dual-antenna data, if it is judged that the inertial navigation initialization is successful, then entering the next step, otherwise, the device is stationary for a preset time length, and the IMU zero offset, the system initial position, the initial speed, and the initial attitude are calculated; step 2, using the read IMU data to perform inertial navigation mechanical arrangement, and performing Kalman filter prediction, if it is judged that the dual-antenna orientation is successful, then the heading of the dual-antenna is calculated, and whether the heading is available is combined according to a preset judgment condition combination; step 3, if it is judged that the dual-antenna orientation in step 2 is not successful, then the state information of the current device is judged, if it is judged that the device is in a static state or less than a first judgment condition in the preset judgment condition combination, then the saved heading is used to perform the next step, if it is judged that the device is greater than the first judgment condition in the preset judgment condition combination, then the Kalman filter measurement update is not performed; step 4, using the heading obtained in step 3 as measurement information to perform Kalman filter update.

[0122] In addition, the logic instruction in the memory 630 described above can be implemented in the form of a software function unit and sold or used as an independent product, and can be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present application essentially or the part that contributes to the prior art or part of the technical solutions can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a plurality of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the method described in various embodiments of the present application. The foregoing storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk or an optical disk, and various media that can store program codes.

[0123] In another aspect, the present application also provides a computer program product comprising a computer program, which can be stored on a non-transitory computer readable storage medium, and the computer program is executable by a processor to enable a computer to perform the dual-antenna positioning and orientation method based on integrated navigation provided by the above-mentioned methods, which comprises: step 1, reading IMU data and dual-antenna data, if it is determined that the inertial navigation initialization is successful, then entering the next step, otherwise, placing the device for a preset time length, calculating the IMU zero offset, the initial position of the system, the initial speed and the initial attitude; step 2, using the read IMU data to perform inertial navigation mechanical arrangement, and performing Kalman filter prediction, if it is determined that the dual-antenna orientation is successful, then calculating the heading of the dual-antenna, and determining whether the heading is available according to a preset combination of determination conditions; step 3, if it is determined that the dual-antenna orientation in step 2 is not successful, then determining the state information of the current device, if it is determined that the device is in a static state or is less than a first determination condition in the preset combination of determination conditions, then using the saved heading to perform the next step, if it is determined that the device is greater than the first determination condition in the preset combination of determination conditions, then not performing Kalman filter measurement update; step 4, using the heading obtained in step 3 as measurement information to perform Kalman filter update.

[0124] In another aspect, the present application also provides a non-transitory computer readable storage medium having a computer program stored thereon, and the computer program is executable by a processor to implement the dual-antenna positioning and orientation method based on integrated navigation provided by the above-mentioned methods, which comprises: step 1, reading IMU data and dual-antenna data, if it is determined that the inertial navigation initialization is successful, then entering the next step, otherwise, placing the device for a preset time length, calculating the IMU zero offset, the initial position of the system, the initial speed and the initial attitude; step 2, using the read IMU data to perform inertial navigation mechanical arrangement, and performing Kalman filter prediction, if it is determined that the dual-antenna orientation is successful, then calculating the heading of the dual-antenna, and determining whether the heading is available according to a preset combination of determination conditions; step 3, if it is determined that the dual-antenna orientation in step 2 is not successful, then determining the state information of the current device, if it is determined that the device is in a static state or is less than a first determination condition in the preset combination of determination conditions, then using the saved heading to perform the next step, if it is determined that the device is greater than the first determination condition in the preset combination of determination conditions, then not performing Kalman filter measurement update; step 4, using the heading obtained in step 3 as measurement information to perform Kalman filter update.

[0125] The device embodiments described above are merely illustrative, wherein the units described as separate components can or can not be physically separate, and the components displayed as units can or can not be physical units, i.e., can be located in one place, or can be distributed to multiple network units. Part or all of the modules can be selected to achieve the purposes of the embodiments according to actual needs. Those skilled in the art can understand and implement without creative labor.

[0126] Through the description of the above embodiments, those skilled in the art can clearly understand that the embodiments can be realized by means of software and the necessary general hardware platform, and of course can also be realized by hardware. Based on such understanding, the above technical solutions can be embodied in the form of a software product, which can be stored in a computer readable storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute the methods described in each embodiment or some parts of the embodiments.

[0127] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for part 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 application.

Claims

1. A dual-antenna pose and orientation determination method based on integrated navigation, characterized in that, Comprise: Step 1, read inertial measurement unit IMU data and double antenna data, if it is judged that inertial navigation initialization is successful, then enter the next step, otherwise the device is stationary for a preset duration, calculate the IMU zero offset, the initial position of the system, the initial speed and the initial attitude; Step 2, use the read IMU data to carry out inertial navigation mechanical arrangement, and carry out Kalman filter prediction, if it is judged that double antenna orientation is successful, then the heading of the double antenna is calculated, and whether the heading is available is judged according to the preset judgment condition combination; Step 3, if it is judged that the double antenna orientation in step 2 is not successful, then the state information of the current device is judged, if it is judged that the device is in static or less than the first judgment condition in the preset judgment condition combination, then the saved heading is used for the next step, if it is judged that the device is greater than the first judgment condition in the preset judgment condition combination, then the Kalman filter measurement update is not carried out; Step 4, use the heading obtained in step 3 as measurement information to carry out Kalman filter update; Wherein, the preset judgment condition combination includes whether the heading change in the previous and next 1 second is less than 1°, and the first judgment condition, the first judgment condition is that the dynamic speed is less than 5° / s.

2. The dual-antenna pose and orientation determination method based on integrated navigation according to claim 1, characterized in that, Step 1 comprises: Calculate the zero offset of the IMU: wherein represents the bias of the three-axis gyro, represents the value of the gyro at time i, and N represents the number of statistics; Calculate the initial position, initial speed and initial attitude of the device: wherein and φ and θ represent the roll angle and the pitch angle, respectively, φ and θ represent the roll angle and the pitch angle, respectively, the results of the accelerometer in the direction.

3. The dual-antenna pose and orientation determination method based on integrated navigation according to claim 1, characterized in that, The Kalman filter in step 2 comprises: Construct the system state equation and the observation equation: wherein, , are the state vectors of the system at time k and k-1 respectively, denotes the state transition matrix from time k-1 to k, is a noise correlation matrix, is the observation at time k, is the observation matrix at time k, is the system noise, is the observation noise, wherein and are white noise and are mutually uncorrelated; Construct the state vector: wherein, denotes the position error, velocity error, attitude error, and bias errors of the gyroscope and accelerometer of the combined navigation; Then the Kalman filter process is: wherein represents a one-step transition matrix of the state at time k-1 to k, is a noise-related matrix.

4. The dual-antenna pose and orientation determination method based on integrated navigation according to claim 1, characterized in that, The calculation of the heading of the double antenna in step 2 comprises: The baseline vector of the sub antenna relative to the main antenna is obtained by using the main and sub antennas on the device to make a difference The heading angle and the pitch angle are calculated as follows: 。 5. The dual-antenna pose and orientation determination method based on integrated navigation according to claim 1, characterized in that, Step 3 specifically comprises: If it is judged that the device is in static, then it is determined that the current heading is the same as the heading at the last time, and the heading at the last time is used as the measurement value; If it is judged that the device is less than the first judgment condition in the preset judgment condition combination, then it is determined that there is an error between the current heading and the heading at the last time and it is within the preset error range, and the heading at the last time is used as the measurement value for Kalman filter correction; If it is judged that the static flag bit has been cleared, then it is determined that the heading at the last time is not available, the static flag bit is set to save the current heading information as the measurement value, otherwise the heading at the last time is directly used as the measurement value; If it is judged that the device is greater than the first judgment condition in the preset judgment condition combination, then it is determined that neither the heading at the last time nor the current heading is used as the measurement value at the next time, the static flag bit is cleared and the Kalman filter update is not carried out.

6. The dual-antenna pose and orientation determination method based on integrated navigation according to claim 1, characterized in that, Step 4 specifically comprises: wherein, is a gain matrix, is a measurement noise covariance matrix, is an optimal estimate of the state at time k, is an optimal estimate of the state covariance at time k, is a measurement at time k, is an observation matrix at time k.

7. A dual-antenna pose and orientation system based on integrated navigation, characterized in that, Comprise: A first processing module for reading IMU data and double antenna data, if it is judged that inertial navigation initialization is successful, then enter the next step, otherwise the device is stationary for a preset duration, calculate the IMU zero offset, the initial position of the system, the initial speed and the initial attitude; A second processing module for using the read IMU data to carry out inertial navigation mechanical arrangement, and carrying out Kalman filter prediction, if it is judged that double antenna orientation is successful, then the heading of the double antenna is calculated, and whether the heading is available is judged according to the preset judgment condition combination; The third processing module is configured to judge the state information of the current device, if the dual-antenna orientation in the second processing module is not successful, if the device is in a static state or is less than the first judgment condition in the preset judgment condition combination, the next step is performed using the saved heading, if the device is greater than the first judgment condition in the preset judgment condition combination, the Kalman filter measurement update is not performed; The fourth processing module is configured to perform the Kalman filter update using the heading obtained by the third processing module as the measurement information; The preset judgment condition combination includes whether the heading change in the previous 1 second and the next 1 second is less than 1°, and the first judgment condition, and the first judgment condition is that the dynamic speed is less than 5° / s.

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, The processor executes the program to realize the dual-antenna orientation and positioning method based on combined navigation according to any one of claims 1 to 6. 9.A non-transitory computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to realize the dual-antenna orientation and positioning method based on combined navigation according to any one of claims 1 to 6.

10. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to realize the dual-antenna orientation and positioning method based on combined navigation according to any one of claims 1 to 6.

Images