Combined navigation method, system, electronic device and storage medium

Abstract

The present invention discloses an integrated navigation method, system, electronic device and storage medium. The integrated navigation method comprises the following steps: S1, obtaining GNSS observation data and IMU sensor data; S2, judging whether the IMU zero bias converges: if so, proceeding to the next step; if not, returning to step S2; S3, constructing a nonlinear optimization mathematical model, and calculating and obtaining the receiver position according to the nonlinear optimization mathematical model; S4, correcting the carrier phase single difference ambiguity according to the receiver position. The nonlinear optimization mathematical model constructed by the present invention can calculate the position of the receiver more accurately, thereby improving the positioning accuracy. By correcting the carrier phase single difference ambiguity, the reliability of GNSS positioning is improved, especially in a dynamic environment. By providing continuous and reliable positioning services, the user experience when using the navigation system is improved.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a combined navigation method, system, electronic equipment and storage medium. Background Art

[0002] With the rapid development of today's society, the application scenarios of GNSS high-precision navigation are becoming more and more diversified and complex, so it is necessary to ensure high reliability and high precision of positioning in diverse and complex environments.

[0003] However, the positioning reliability of GNSS in complex environments is average. After entering a complex environment, GNSS is easily affected by many factors such as multipath, which makes it impossible to fix the ambiguity, resulting in unusable positioning results and inability to provide highly reliable and high-precision navigation and positioning results. Summary of the invention

[0004] The present invention aims to solve at least one of the technical problems existing in the prior art. To this end, the present invention proposes a combined navigation method.

[0005] To achieve the above purpose, the technical solution adopted by the present invention is as follows:

[0006] A combined navigation method comprises the following steps: S1, acquiring GNSS observation data and IMU sensor data; S2, judging whether the IMU zero bias is converged: if so, proceeding to the next step; if not, returning to step S2; S3, constructing a nonlinear optimization mathematical model, and calculating and obtaining a receiver position according to the nonlinear optimization mathematical model; S4, correcting the carrier phase single difference ambiguity according to the receiver position.

[0007] Furthermore, the nonlinear optimization mathematical model is: ;in ; is the set of parameters to be optimized, are the x, y, and z coordinate values ​​of the receiver in the kth sliding window respectively; is the carrier phase double difference residual, is the carrier observation quantity, is the IMU position recursive constraint residual, is the recursive value of the IMU position, is the IMU elevation constraint residual, is the IMU elevation coordinate value, is the velocity constraint residual, is the receiver speed, To find the minimum function.

[0008] Furthermore, step S2 specifically includes:

[0009] S21, calculate the carrier phase double difference residual according to the carrier phase observation and the first calculation formula , the first calculation formula is as follows:

[0010] ;

[0011] in is the carrier phase double difference residual, is the wavelength, is the carrier phase double difference constraint weight, is the observation vector of satellite i in the kth sliding window; is the observation vector of satellite j in the kth sliding window, is the receiver position coordinate in the kth sliding window, is the coordinates of the base station in the kth sliding window, is the difference between the carrier phase single difference ambiguity between the ith satellite mobile station and the reference station and the carrier phase single difference ambiguity between the jth satellite mobile station and the reference station, is the difference between the difference in carrier phase observations of the i-th satellite by the receiver and the base station and the difference in carrier phase observations of the j-th satellite by the receiver and the base station, is the number of sliding windows, is the number of original observations of the carrier phase in the kth sliding window, and k is the sliding window number;

[0012] S22, calculate the IMU position recursion constraint residual according to the IMU position recursion and the second calculation formula , the second calculation formula is as follows:

[0013] ;

[0014] in is the IMU position recursive constraint residual, is the IMU position recursive constraint weight, is the receiver position coordinate in the k-1th sliding window, is the recursive position of the IMU in the kth sliding window, is the number of sliding windows, k is the sliding window number;

[0015] S23, calculate the IMU elevation constraint residual according to the inertial navigation IMU elevation and the third calculation formula , the third calculation formula is as follows:

[0016] ;

[0017] in is the elevation constraint residual, is the IMU elevation constraint weight, , is the rotation matrix from the ECEF coordinate system to the ENU coordinate system, is the elevation coordinate value of the inertial navigation IMU;

[0018] S24, calculating the speed constraint residual according to a fourth calculation formula, the fourth calculation formula is as follows:

[0019] ;

[0020] in is the velocity constraint residual, is the speed constraint weight, is the receiver speed in the kth sliding window, is the kth sliding window time, is the k-1th sliding window time.

[0021] Furthermore, the carrier phase single difference ambiguity is corrected according to the following formula: ; ; is the corrected single-difference ambiguity of the carrier phase between the i-th satellite mobile station and the base station; is the carrier phase single-difference residual, is the single-difference ambiguity of the carrier phase between the i-th satellite mobile station and the reference station, For Function to round to integer value. is the single-difference ambiguity correction value of the carrier phase between the i-th satellite mobile station and the base station.

[0022] Furthermore, the carrier phase single-difference residual Obtained by the following formula: ; In the formula is the observation vector of satellite i, For Transpose the vector. are the coordinates of the receiver location, is the base station coordinate of the current epoch, is the difference between the carrier phase observations of the i-th satellite by the receiver and base station in the k-th sliding window.

[0023] Further, step S2 specifically includes: the IMU updates data at a preset frequency, and each time the data is updated, the difference between the current heading and the previous heading is calculated, and the absolute value of the heading difference calculated each time is accumulated; it is determined whether the absolute value of the accumulated heading difference exceeds a preset threshold: if so, it is determined that the IMU zero bias converges; if not, it is determined that the IMU zero bias does not converge.

[0024] Furthermore, the preset frequency is 5 Hz, and the preset threshold is 720 degrees.

[0025] The present invention also provides an integrated navigation system, comprising: a data acquisition module for acquiring GNSS observation data and IMU sensor data; a judgment module for judging whether the IMU zero bias is converged; a model construction module for constructing a nonlinear optimization mathematical model and obtaining a receiver position according to the nonlinear optimization mathematical model; and a correction module for correcting the carrier phase single difference ambiguity according to the receiver position.

[0026] The present invention also provides an electronic device, including a processor and a memory, wherein the memory is used to store program codes and transmit the program codes to the processor; the processor is used to execute the combined navigation method according to instructions in the program codes.

[0027] The present invention also provides a storage medium, wherein the storage medium stores a computer program, and the computer program implements the combined navigation method when executed by a processor.

[0028] The present invention has the following beneficial effects:

[0029] By combining GNSS and IMU data, it is possible to maintain high positioning reliability even when GNSS signals are interfered with. By utilizing the high-precision characteristics of IMU in a short period of time and cooperating with GNSS data, more accurate positioning results can be provided in complex environments. When GNSS signals are affected by multipath effects, IMU data can be used to assist positioning and ensure positioning accuracy. By real-time monitoring of whether the IMU zero bias converges, the real-time and accuracy of the data are ensured. The constructed nonlinear optimization mathematical model can calculate the position of the receiver more accurately and improve the positioning accuracy. By correcting the carrier phase single difference ambiguity, the reliability of GNSS positioning is improved, especially in dynamic environments. By providing continuous and reliable positioning services, the user experience when using the navigation system is improved.

[0030] In addition to the above-described purposes, features and advantages, the present invention has other purposes, features and advantages. The present invention will be further described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0032] Figure 1 It is a schematic diagram of the overall process of the present invention;

[0033] Figure 2 It is a statistical diagram of experimental data of an embodiment of the present invention and a comparative example. DETAILED DESCRIPTION

[0034] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0035] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0036] It should be noted that all directional indications (such as up, down, left, right, front, back, etc.) in the embodiments of the present invention are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0037] In addition, the descriptions of "first", "second", etc. in the present invention are only used for descriptive purposes and cannot be understood as indicating or implying their relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such a combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0038] Please refer to Figure 1 A combined navigation method in a preferred embodiment of the present invention includes steps S1, S2, S3 and S4.

[0039] S1, obtain GNSS observation data and IMU sensor data. GNSS observation data includes satellite ephemeris data, satellite raw observation data and base station differential data; IMU sensor data is 100hz raw measurement data, and IMU sensor data is acceleration and angular velocity data monitored by the IMU sensor. It is usually necessary to preprocess GNSS observation data, remove unreliable data, and synchronize GNSS and IMU data based on the PPS timing system. GNSS observation data includes carrier phase observations.

[0040] S2, determine whether the IMU zero bias converges:

[0041] If yes, proceed to the next step, step S3;

[0042] If not, return to step S2. The convergence of IMU zero bias means that the measurement result of IMU is more reliable, providing accurate data for subsequent steps.

[0043] S3, construct a nonlinear optimization mathematical model, and calculate the receiver position based on the nonlinear optimization mathematical model .

[0044] S4, correct the carrier phase single difference ambiguity according to the receiver position.

[0045] By combining the data of GNSS and IMU, the present invention can maintain a high positioning reliability when the GNSS signal is interfered with. By utilizing the high-precision characteristics of IMU in a short period of time and cooperating with GNSS data, more accurate positioning results can be provided in complex environments; when the GNSS signal is affected by the multipath effect, the IMU data can be used to assist in positioning and ensure positioning accuracy. By real-time monitoring of whether the IMU zero bias converges, the real-time and accuracy of the data are ensured. The constructed nonlinear optimization mathematical model can calculate the position of the receiver more accurately and improve the positioning accuracy. By correcting the carrier phase single difference ambiguity, the reliability of GNSS positioning is improved, especially in dynamic environments. By providing continuous and reliable positioning services, the user experience when using the navigation system is improved.

[0046] The prior art also has the technology of combining GNSS with IMU to form GNSS / IMU combined navigation to provide reliable positioning results, but after entering a complex environment, GNSS is easily affected by many factors such as multipath, which makes it impossible to fix the ambiguity. In addition, the recursive accuracy of consumer-grade IMU diverges rapidly over time, and the GNSS / IMU combined positioning also diverges over time, resulting in unusable positioning results, and unable to provide low-cost, highly reliable, and highly accurate navigation positioning results. The embodiment of the present invention is based on a nonlinear optimization system, constructs a GNSS nonlinear optimization mathematical model based on IMU constraints, fully utilizes the advantages of IMU devices, and provides highly reliable and high-precision positioning results in complex environments; based on the high-precision positioning results, the GNSS observation ambiguity is corrected, thereby improving the GNSS ambiguity accuracy.

[0047] The positioning method proposed in the present invention is described below with a specific example: a board using the present method and a board using a conventional satellite navigation positioning method are installed on the same vehicle, one board is burned with a program based on the positioning method of the present invention, and the other board is burned with a program based on the satellite navigation positioning method. A sports car is driven around buildings and tree-lined roads and other areas with severe obstruction. A sports car test is conducted for one month, and the fixation rates of the two boards for a total of 62 times are counted. Figure 2It can be seen that the fixation rate of the positioning method based on the patent of the present invention is significantly improved compared with the satellite guidance positioning. The average fixation rates of the positioning method based on the patent of the present invention and the satellite guidance positioning method are calculated respectively, as shown in the following table. It can be seen that the fixation rate of the positioning method based on the patent of the present invention is improved by an average of 11% compared with the satellite guidance method, and the accuracy is significantly improved.

[0048]

[0049] In a specific embodiment of the present invention, the nonlinear optimization mathematical model is: ;

[0050] in .

[0051] is the set of parameters to be optimized, are the x, y, and z coordinate values ​​of the receiver in the kth sliding window respectively; is the carrier phase double difference residual, is the carrier observation quantity, is the IMU position recursive constraint residual, is the recursive value of the IMU position, is the IMU elevation constraint residual, is the IMU elevation coordinate value, is the velocity constraint residual, is the receiver speed, To find the minimum function, To calculate the Mahalanobis norm of the selected object. Finally, the nonlinear optimization mathematical model is used to obtain The receiver position , which can be represented by a vector.

[0052] Based on the carrier phase double difference residual, IMU position recursive constraint residual, IMU altitude constraint residual and velocity constraint residual, a GNSS nonlinear optimization mathematical model is constructed to calculate the precise receiver position. , the position is represented by , Contains three coordinate values, namely the coordinate values ​​on the x, y, and z axes respectively.

[0053] In a specific embodiment of the present invention, step S2 specifically includes steps S21, S22, S23 and S24.

[0054] S21, calculate the carrier phase double difference residual according to the carrier phase observation and the first calculation formula , the first calculation formula is as follows:

[0055]

[0056] Carrier phase observations include and ; , , , , and All are GNSS observation data.

[0057] in is the carrier phase double difference residual, is the wavelength, is the carrier phase double difference constraint weight, is the observation vector of satellite i, is the observation vector of satellite j, For Transpose the vector. It means to transpose the selected vector, that is, to convert the horizontal quantity into a column vector and the column vector into a horizontal quantity. is the receiver position coordinate in the kth sliding window, expressed as a vector; is the coordinates of the base station in the kth sliding window, is the difference between the carrier phase single difference ambiguity between the ith satellite mobile station and the reference station and the carrier phase single difference ambiguity between the jth satellite mobile station and the reference station corresponding to the kth sliding window, It is the difference between the difference in carrier phase observation of the i-th satellite by the receiver and the base station corresponding to the k-th sliding window and the difference in carrier phase observation of the j-th satellite by the receiver and the base station, that is, first calculate the value of the difference in carrier phase observation of the i-th satellite by the receiver and the base station corresponding to the k-th sliding window, then calculate the value of the difference in carrier phase observation of the j-th satellite by the receiver and the base station corresponding to the k-th sliding window, and then find the difference between these two values. is the number of sliding windows, is the number of original carrier phase observations in the kth sliding window, and k is the sliding window number.

[0058] S22, calculate the IMU position recursion constraint residual according to the IMU position recursion and the second calculation formula , the second calculation formula is as follows:

[0059] ;

[0060] in is the IMU position recursive constraint residual, is the IMU position recursive constraint weight, is the receiver position coordinate in the k-1th sliding window, is the recursive position of the IMU in the kth sliding window, is the number of sliding windows, and k is the sliding window number.

[0061] The IMU position is recursively derived as IMU sensor data, including .

[0062] S23, calculate the IMU elevation constraint residual according to the inertial navigation IMU elevation and the third calculation formula , the third calculation formula is as follows:

[0063]

[0064] in is the elevation constraint residual, is the IMU elevation constraint weight, To obtain the third dimension of the vector, specifically, , is the rotation matrix from the ECEF coordinate system to the ENU coordinate system, is the elevation coordinate value of the inertial navigation IMU, and is the IMU sensor data.

[0065] S24, calculating the speed constraint residual according to a fourth calculation formula, the fourth calculation formula is as follows:

[0066]

[0067] in is the velocity constraint residual, is the speed constraint weight, is the receiver speed in the kth sliding window, is the kth sliding window time, is the k-1th sliding window time.

[0068] Through the above calculations, the carrier phase double difference residual, IMU position recursive constraint residual, IMU elevation constraint residual and velocity constraint residual are obtained and substituted into the nonlinear optimization mathematical model to provide a basis for subsequent optimization calculations.

[0069] In some embodiments of the present invention, the carrier phase single difference ambiguity is corrected according to the following formula:

[0070] ;

[0071] ;

[0072] is the corrected single-difference ambiguity of the carrier phase between the i-th satellite mobile station and the base station; is the carrier phase single-difference residual, is the single-difference ambiguity of the carrier phase between the i-th satellite mobile station and the reference station, For Function to round to integer value. is the single-difference ambiguity correction value of the carrier phase between the i-th satellite mobile station and the base station.

[0073] In a specific embodiment of the present invention, the carrier phase single-difference residual Obtained by the following formula:

[0074] ;

[0075] In the formula is the observation vector of satellite i, For Transpose the vector. is the coordinate of the receiver position obtained in step S3, are the base station coordinates of the current epoch.

[0076] In the specific implementation of the present invention, step S2 specifically includes: IMU updates data at a preset frequency, and each time the data is updated, the difference between the current heading and the previous heading is calculated, and the absolute value of the heading difference calculated each time is accumulated; it is judged whether the absolute value of the accumulated heading difference exceeds the preset threshold: if so, it is judged that the IMU zero bias converges; if not, it is judged that the IMU zero bias does not converge, and the heading difference continues to be accumulated over time and judged. When it is judged that it does not converge, each subsequent data update of the IMU returns to step S2 for judgment until the accumulated heading difference exceeds the preset threshold. This method judges whether the IMU turns sufficiently based on the accumulated value of the IMU heading angle, thereby judging whether the excitation of each axis of the IMU is sufficient and the zero bias reaches a converged state.

[0077] In a specific implementation of the present invention, the preset frequency is 5 Hz, and the preset threshold is 720 degrees.

[0078] The present invention also proposes an integrated navigation system, comprising: a data acquisition module for acquiring GNSS observation data and IMU sensor data; a judgment module for judging whether the IMU zero bias has converged; a model construction module for constructing a nonlinear optimization mathematical model and obtaining a receiver position according to the nonlinear optimization mathematical model; and a correction module for correcting the carrier phase single difference ambiguity according to the receiver position.

[0079] The present invention also proposes an electronic device, including a processor and a memory, wherein the memory is used to store program codes and transmit the program codes to the processor; the processor is used to execute the combined navigation method according to instructions in the program codes.

[0080] The present invention further provides a storage medium, wherein the storage medium stores a computer program, and the computer program implements the combined navigation method when executed by a processor.

[0081] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A combined navigation method, characterized in that: The steps include: S1, obtain GNSS observation data and IMU sensor data; S2, determine whether the IMU zero bias converges: If yes, proceed to the next step; If not, return to step S2; S3, constructing a nonlinear optimization mathematical model, and calculating and obtaining a receiver position according to the nonlinear optimization mathematical model; S4, correct the carrier phase single difference ambiguity according to the receiver position; The nonlinear optimization mathematical model is: ; in ; is the set of parameters to be optimized, are the x, y, and z coordinate values ​​of the receiver in the kth sliding window respectively; is the carrier phase double difference residual, is the carrier observation quantity, is the IMU position recursive constraint residual, is the recursive value of the IMU position, is the IMU elevation constraint residual, is the IMU elevation coordinate value, is the velocity constraint residual, is the receiver speed, To find the minimum function; Step S2 specifically includes: S21, calculate the carrier phase double difference residual according to the carrier phase observation and the first calculation formula , the first calculation formula is as follows: ； in is the carrier phase double difference residual, is the wavelength, is the carrier phase double difference constraint weight, is the observation vector of satellite i in the kth sliding window; is the observation vector of satellite j in the kth sliding window, is the receiver position coordinate in the kth sliding window, is the coordinates of the base station in the kth sliding window, is the difference between the carrier phase single difference ambiguity between the ith satellite mobile station and the reference station and the carrier phase single difference ambiguity between the jth satellite mobile station and the reference station, is the difference between the difference in carrier phase observations of the i-th satellite by the receiver and the base station and the difference in carrier phase observations of the j-th satellite by the receiver and the base station, is the number of sliding windows, is the number of original observations of the carrier phase in the kth sliding window, and k is the sliding window number; S22, calculate the IMU position recursion constraint residual according to the IMU position recursion and the second calculation formula , the second calculation formula is as follows: ； in is the IMU position recursive constraint residual, is the IMU position recursive constraint weight, is the receiver position coordinate in the k-1th sliding window, is the recursive position of the IMU in the kth sliding window, is the number of sliding windows, k is the sliding window number; S23, calculate the IMU elevation constraint residual according to the inertial navigation IMU elevation and the third calculation formula , the third calculation formula is as follows: ； in is the elevation constraint residual, is the IMU elevation constraint weight, To obtain the third dimension of the vector, is the rotation matrix, is the elevation coordinate value of the inertial navigation IMU; S24, calculating the speed constraint residual according to a fourth calculation formula, the fourth calculation formula is as follows: ； in is the velocity constraint residual, is the speed constraint weight, is the receiver speed in the kth sliding window, is the kth sliding window time, is the k-1th sliding window time.

2. The integrated navigation method according to claim 1, characterized in that: The carrier phase single difference ambiguity is corrected according to the following formula: ； ； is the corrected single-difference ambiguity of the carrier phase between the i-th satellite mobile station and the base station; is the carrier phase single-difference residual, is the single-difference ambiguity of the carrier phase between the i-th satellite mobile station and the reference station, For Function to round to integer value. is the single-difference ambiguity correction value of the carrier phase between the i-th satellite mobile station and the base station.

3. The integrated navigation method according to claim 2, characterized in that: Carrier phase single difference residual Obtained by the following formula: ； In the formula is the observation vector of satellite i, For Transpose the vector. are the coordinates of the receiver location, is the base station coordinate of the current epoch, is the difference between the carrier phase observations of the i-th satellite by the receiver and base station in the k-th sliding window.

4. The integrated navigation method according to claim 1, characterized in that: Step S2 specifically includes: The IMU updates data at a preset frequency. Each time the data is updated, the difference between the current heading and the previous heading is calculated, and the absolute value of the calculated heading difference is accumulated each time; Determine whether the absolute value of the accumulated heading difference exceeds the preset threshold: If so, the IMU zero bias is determined to be convergent; If not, it is determined that the IMU zero bias does not converge.

5. The integrated navigation method according to claim 4, characterized in that: The preset frequency is 5 Hz, and the preset threshold is 720 degrees.

6. An integrated navigation system, used for implementing the integrated navigation method according to any one of claims 1 to 5, characterized in that: include: Data acquisition module, to obtain GNSS observation data and IMU sensor data; Judgment module, judges whether the IMU zero bias converges; A model building module is used to build a nonlinear optimization mathematical model and obtain the receiver position according to the nonlinear optimization mathematical model; The correction module corrects the carrier phase single difference ambiguity according to the receiver position.

7. An electronic device, characterized in that: including a processor and a memory, The memory is used to store program code and transmit the program code to the processor; The processor is used to execute the combined navigation method according to any one of claims 1 to 5 according to the instructions in the program code.

8. A storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the combined navigation method according to any one of claims 1 to 5 is implemented.

Images