GNSS and INS semi-tight integrated navigation method and device and electronic equipment

Abstract

The invention provides a GNSS (Global Navigation Satellite System) and INS (Inertial Navigation System) semi-tight integrated navigation method and device and electronic equipment, and the GNSS and INS semi-tight integrated navigation method comprises the following steps: carrying out system initialization on a navigation system; the navigation system is a GNSS and INS semi-tight integrated navigation system; predicting the discrete state of the navigation system to obtain the discrete state; constructing an observation model, carrying out iterative updating on the discrete state of the navigation system, and determining the optimal state of the navigation system at the current moment; and converting the current station center coordinate into an earth-centered earth-fixed coordinate system according to the iteratively updated and converged anchor point coordinate. According to the invention, high precision, low computing power and strong robustness are considered, and the problem that navigation precision and computing power are difficult to effectively balance in the prior art is solved.

Description

Technical Field

[0001] The present invention relates to the field of satellite navigation technology, and in particular to a GNSS and INS semi-tight integrated navigation method, device and electronic equipment. Background Art

[0002] The Global Navigation Satellite System (GNSS) can provide absolute positioning at the meter or even centimeter level. However, in complex environments such as urban canyons, under forests, or in transition zones between indoors and outdoors, satellite signals can be affected by obstructions and multipath effects, resulting in a significant decrease in positioning accuracy or even complete loss of lock. The Inertial Navigation System (INS) continuously outputs its own angular velocity and acceleration information at a high frequency, providing excellent short-term relative motion measurement capabilities. However, long-term integration can lead to error divergence due to random drift and bias accumulation. Therefore, industry and academia generally adopt a combined GNSS and INS navigation strategy: using the absolute positioning information provided by GNSS to constrain the drift of the INS, achieving high-precision, all-weather positioning services.

[0003] GNSS / INS integrated navigation systems typically have three integration modes: loose, tight, and deep. Loose integration uses the position and velocity calculated independently by the receiver as measurement inputs, which are then post-filtered with the INS navigation solution. While offering a user-friendly interface and simple implementation, issues such as latency, coordinate system conversion, and inconsistencies in the two-level covariances can weaken the effectiveness of GNSS information in mitigating INS drift. Tight integration directly incorporates raw observations such as pseudorange, carrier phase, and Doppler into observation equations shared with the INS. This significantly improves positioning availability in weak signal scenarios and has become the mainstream of high-precision navigation systems. However, tight coupling requires access to receiver channel-level data, processing ephemeris, and ionosphere and tropospheric modeling, significantly increasing computational and implementation complexity. Deep integration further utilizes the INS-derived velocity closed loop to assist the GNSS signal tracking loop, enabling carrier lock maintenance in extremely low signal-to-noise ratios, even with three visible satellites. However, this requires extremely high hardware and software co-design requirements.

[0004] In light of this, a class of semi-tight coupling schemes has emerged, situated between loose coupling and tight coupling. These schemes simultaneously process INS predictions and GNSS measurements within a unified filter, eliminating the need for deep dives into the receiver baseband's raw pseudorange data. Compared to loose coupling, semi-tight coupling avoids the time delay and covariance information loss associated with two-stage computation, enabling precise measurement iterations within the same state space. Compared to tight and deep coupling, semi-tight coupling does not rely on channel-level data, is hardware-intrusive, and significantly reduces computational complexity, making it easy to deploy on conventional commercial GNSS modules and embedded flight controllers. However, existing semi-tight coupling schemes are often based on recursive filters (Extended Kalman Filters, EKFs) that linearize the complete state once, resulting in significant linearization errors.

[0005] There is currently no effective solution to the problem of difficulty in effectively balancing navigation accuracy and computing power in existing related technologies. Summary of the Invention

[0006] The present invention provides a GNSS and INS semi-tight integrated navigation method, device and electronic equipment, which are used to solve the defect of the existing related technology that it is difficult to effectively balance navigation accuracy and computing power.

[0007] In a first aspect, the present invention provides a GNSS and INS semi-tight integrated navigation method, comprising: Initialize the navigation system; the navigation system is a semi-compact integrated navigation system of GNSS and INS; Predicting the discrete state of the navigation system to obtain the discrete state; Constructing an observation model and iteratively updating the discrete states of the navigation system to determine the optimal state of the navigation system at the current moment; According to the anchor point coordinates converged by iterative updates, the current station center coordinates are converted into the Earth-centered Earth-fixed coordinate system.

[0008] According to a GNSS and INS semi-tight integrated navigation method provided by the present invention, the navigation system is initialized, including: The GNSS timestamp of the navigation system is converted from GPS time to local system time, taking into account the receiver solution delay.

[0009] According to a GNSS and INS semi-tight integrated navigation method provided by the present invention, the navigation system is initialized, including: Record the first frame of GNSS's Earth-centered Earth-fixed coordinate system position as the initial coordinate of the origin of the East-North-Sky coordinate system; Read the inertial measurement unit data and combine it with the magnetometer to determine the three-dimensional basis of the east-north-sky coordinate system; The sliding window averaging method is used to estimate the gravity direction of the body coordinate system, and the state of the navigation system in the east-north-sky coordinate system at the initial moment is determined.

[0010] According to the present invention, a semi-tight integrated navigation method of GNSS and INS is provided, which reads inertial measurement unit data and determines the three-dimensional basis of the east-north-sky coordinate system in combination with a magnetometer, including: Read the inertial measurement unit and magnetometer data, perform gyroscope short-time integration, project all magnetic vectors onto the horizontal plane, and then perform weighted average to obtain the horizontal component of the geomagnetic field. Determining an initial heading angle based on the angle between the geomagnetic horizontal component and the true north direction; The initial heading angle and the GNSS anchor point are combined to determine the three-axis basis of the east-north-sky coordinate system.

[0011] According to the present invention, a semi-tight integrated navigation method of GNSS and INS is provided, which uses a sliding window averaging method to estimate the gravity direction of the body coordinate system and determine the state of the navigation system in the east-north-sky coordinate system at the initial moment, including: Determine the zero bias of the accelerometer and gyroscope using a sliding window averaging method and estimate the direction of gravity in the body coordinate system; Define the gravity direction in the east-north-sky coordinate system and obtain the rotation matrix through singular value decomposition; The state of the navigation system in the east-north-sky coordinate system at an initial moment is determined based on the rotation matrix.

[0012] According to a GNSS and INS semi-tight integrated navigation method provided by the present invention, after initializing the navigation system, the method includes: freezing the magnetometer input and only saving the gyroscope, accelerometer and GNSS data streams.

[0013] According to a semi-tight integrated navigation method of GNSS and INS provided by the present invention, discrete states of the navigation system are predicted to obtain discrete states, including: Constructing a state transfer model of the navigation system and determining a state transfer function based on a kinematic model of an inertial measurement unit; predicting discrete states of the navigation system in combination with the state transfer function; The true state of the navigation system is assumed based on a GNSS observation model, and the error state is determined in combination with the discrete state of the navigation system.

[0014] According to a semi-tight integrated navigation method of GNSS and INS provided by the present invention, an observation model is constructed, and the discrete states of the navigation system are iteratively updated to determine the optimal state of the navigation system at the current moment, including: determine a rotation matrix from a geocentric coordinate system to an east-north-up coordinate system based on a current discrete state of the navigation system; determine GNSS observation position and velocity of the GNSS antenna in the east-north-up coordinate system according to the rotation matrix, and construct an observation model; update the discrete state of the navigation system iteratively according to a standard process of the iterative Kalman filter combined with the observation model, and determine the optimal state of the navigation system at the current time.

[0015] In a second aspect, the present application further provides a GNSS and INS semi-tight combined navigation device, comprising: an initialization module for system initialization of the navigation system; the navigation system is a GNSS and INS semi-tight combined navigation system; a prediction module for predicting the discrete state of the navigation system to obtain a discrete state; an update module for constructing an observation model and updating the discrete state of the navigation system iteratively to determine the optimal state of the navigation system at the current time; a conversion module for converting the current station-centered coordinate into a geocentric coordinate system according to the anchor point coordinate of the iterative update convergence.

[0016] In a third aspect, the present application further 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 executes the program to realize the GNSS and INS semi-tight combined navigation method according to the first aspect.

[0017] In a fourth aspect, the present application further provides a non-transitory computer readable storage medium, which stores a computer program executable by a processor to realize the GNSS and INS semi-tight combined navigation method according to the first aspect.

[0018] In a fifth aspect, the present application further provides a computer program product, comprising a computer program executable by a processor to realize the GNSS and INS semi-tight combined navigation method according to the first aspect.

[0019] Compared with the prior art, the present application has the following beneficial effects: The semi-tight combination navigation method of GNSS and INS provided by the present invention initializes the navigation system and adjusts the timestamp of the navigation system so that the navigation system can complete the attitude initialization and enter the centimeter-level precision working state within a short time after startup. Then, the discrete state of the navigation system is predicted to prepare for the subsequent state update. The observation model is then constructed to iteratively update the discrete state of the navigation system to determine the optimal state of the navigation system at the current moment, thereby reducing the approximate error caused by the nonlinear coupling of attitude, position and velocity and improving the accuracy of the navigation system. Finally, based on the anchor point coordinates converged by the iterative update, the conversion of the navigation system station center coordinates and the Earth-centered Earth-fixed coordinate system is completed. In the above process, high precision, low computing power and strong robustness are taken into account, and the problem of difficult effective balance between navigation accuracy and computing power in the existing related technologies is solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0021] Figure 1 This is a flow chart of the GNSS and INS semi-tight integrated navigation method provided by the present invention; Figure 2 is a schematic diagram of a route for navigation in an embodiment of the present invention; Figure 3 This is a schematic diagram of the positioning results using only GNSS; Figure 4 This is a schematic diagram of the positioning results using only INS; Figure 5 This is a schematic diagram of the positioning result of integrating GNSS and INS; Figure 6 1 is a schematic diagram of the visualization results of the GNSS / INS absolute trajectory error according to an embodiment of the present invention; Figure 7 This is a structural block diagram of the GNSS and INS semi-compact integrated navigation device provided by the present invention; Figure 8 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0022] In order to make the objects, technical solutions and advantages of the present application clearer, the technical solutions in the present application will be described clearly and completely below in combination with the drawings in the present application. Obviously, the described embodiments are some of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative work belong to the protection scope of the present application.

[0023] The present application provides a GNSS and INS semi-tight integrated navigation method, Figure 1 The present application provides a GNSS and INS semi-tight integrated navigation method, Figure 1 The method comprises the following steps: Step S101, system initialization of the navigation system; the navigation system is a GNSS and INS semi-tight integrated navigation system; Step S102, prediction of the discrete state of the navigation system, to obtain the discrete state; Step S103, construction of an observation model, and iterative updating of the discrete state of the navigation system, to determine the optimal state of the navigation system at the current time; Step S104, conversion of the current station-centered coordinates into the geocentric and terrestrial coordinates according to the anchor point coordinates converged by the iterative updating.

[0024] In the method, the navigation system adopts a GNSS and INS semi-tight integrated framework. First, the navigation system is initialized, and the time stamp of the navigation system is adjusted, so that the navigation system can complete attitude initialization and enter a centimeter-level precision working state within a short time after starting. Then, the discrete state of the navigation system is predicted, to prepare for subsequent state updating. Then, an observation model is constructed, and the discrete state of the navigation system is iteratively updated, to determine the optimal state of the navigation system at the current time, thereby reducing the approximation error caused by the attitude-position-velocity nonlinear coupling and improving the precision of the navigation system. Finally, the conversion of the station-centered coordinates of the navigation system into the geocentric and terrestrial coordinates is completed according to the anchor point coordinates converged by the iterative updating. In the above process, high precision, low computing power and strong robustness are taken into account, and the problem that the navigation precision and computing power are difficult to effectively balance in the prior art is solved.

[0025] Figure 2 The present application provides a GNSS and INS semi-tight integrated navigation method, Figure 2 As shown in the figure, in some embodiments, step S101, system initialization of the navigation system, comprises: converting the GNSS time stamp of the navigation system from GPS time into local system time, and considering the receiver solution delay.

[0026] For example, after power-on, the clocks of the Inertial Measurement Unit (IMU) and GNSS are synchronized first. For the GNSS timestamp, it is necessary to convert it from GPS time to local UNIX time, and take into account the receiver solution delay. The timestamp output by the GNSS receiver is generally in the form of "GPS week + seconds in week". The GPS time system starts at UTC 00:00:00 on January 6, 1980, while the local system time (such as lidar, IMU) mostly uses UNIX time (starting at UTC 00:00:00 on January 1, 1970). The timestamp conversion includes the time starting point offset and the accumulated leap seconds of UNIX time. The current leap seconds are 18 seconds. The specific conversion method is as follows:

[0027] in, Indicates UNIX time, Indicates the week number, Indicates the second of the week, Indicates leap seconds, Indicates the receiver delay.

[0028] In some embodiments, step S101 initializes the navigation system, including: recording the Earth-Centered, Earth-Fixed (ECEF) position of the first frame GNSS as the initial coordinates of the origin of the East-North-Up (ENU) coordinate system; reading the inertial measurement unit data, and determining the three-dimensional basis of the East-North-Up (ENU) coordinate system in combination with the magnetometer; using the sliding window averaging method to estimate the gravity direction of the body coordinate system, and determining the state of the navigation system in the East-North-Up (ENU) coordinate system at the initial moment.

[0029] Specifically, the inertial measurement unit data is read and combined with the magnetometer to determine the three-dimensional basis of the east-north-sky coordinate system, including: reading the inertial measurement unit and magnetometer data, projecting all magnetic vectors onto the horizontal plane through gyroscope short-time integration, and then performing weighted averaging to obtain the geomagnetic horizontal component; determining the initial heading angle based on the angle between the geomagnetic horizontal component and the true north direction; and determining the three-dimensional basis of the east-north-sky coordinate system based on the initial heading angle and the GNSS anchor point.

[0030] More specifically, a sliding window averaging method is used to estimate the gravity direction of the body coordinate system, and the state of the navigation system in the east-north-sky coordinate system at the initial moment is determined, including: using the sliding window averaging method to determine the zero bias of the accelerometer and the gyroscope, and estimating the gravity direction of the body coordinate system; defining the gravity direction in the east-north-sky coordinate system, and obtaining a rotation matrix through singular value decomposition (SVD); and determining the state of the navigation system in the east-north-sky coordinate system at the initial moment based on the rotation matrix.

[0031] After the navigation system is initialized, the magnetometer input is frozen and only the gyroscope, accelerometer and GNSS data streams are saved.

[0032] For example, the position of the Earth-centered Earth-fixed coordinate system of the first frame GNSS of the navigation system is recorded as the initial coordinate of the origin of the East-North-Sky coordinate system. The navigation system continuously reads about 0.1 s (20 frames) of IMU data, uses the short-time integration of the gyroscope to eliminate the effects of pitch and roll, projects all magnetic vectors onto the horizontal plane and performs weighted averaging to obtain the geomagnetic horizontal component. The initial heading angle is calculated from the angle between the geomagnetic horizontal component and the true north direction, and then the east, north, and sky bases of the ENU coordinate system are determined in combination with the GNSS anchor point. The sliding window averaging method is used to calculate the zero bias of the accelerometer and gyroscope, and the gravity direction of the body coordinate system is estimated. In this embodiment, the gravity direction in the ENU coordinate system is defined as , solve the rotation matrix through singular value decomposition satisfy At this point, the initial state of the navigation system in the ENU coordinate system is established. After initialization, the magnetometer input is frozen, and only the IMU gyroscope, accelerometer, and GNSS data streams are saved. The nominal state vector now consists of the attitude matrix, position, velocity, gyroscope and accelerometer biases, and the anchor point ECEF coordinates in the ENU coordinate system. The error covariance matrix assigns small initial variances to the attitude, position, and velocity, factory-calibrated values ​​to the biases, and an uncertainty of several meters to the anchor point coordinates for subsequent self-compensation.

[0033] In some embodiments, step S102 predicts the discrete state of the navigation system to obtain the discrete state, including: constructing a state transition model of the navigation system and determining the state transfer function based on the kinematic model of the inertial measurement unit; predicting the discrete state of the navigation system in combination with the state transfer function; obtaining the actual state of the navigation system, and determining the error state in combination with the discrete state of the navigation system.

[0034] For example, let Indicates that the navigation system is iThe discrete state of each IMU moment, the state transition model can be expressed as:

[0035] in, Indicates that the navigation system is i +1 discrete state of the IMU moment, Indicates that the navigation system is i The discrete state of each IMU moment, Indicates the sampling interval of IMU, represents the state transition function, Indicates the i The IMU measurement value at each moment, Indicates the i The process noise at each moment is defined as:

[0036]

[0037]

[0038] Among them, the body state is defined on the manifold M on , represents the three-dimensional rotation group in the state, represents the remaining 18-dimensional Euclidean states, Indicates that the total dimension is 21; x Indicates the state of the body. Indicates the attitude of the navigation system body coordinate system in the ENU coordinate system, Indicates the position of the navigation system body coordinate system in the ENU coordinate system, Indicates the velocity of the navigation system body coordinate system in the ENU coordinate system, Indicates the IMU gyroscope bias, Indicates the IMU accelerometer bias, represents the gravity vector in the ENU coordinate system, The ECEF coordinates representing the origin of the ENU coordinate system, i.e. the anchor point; u Represents the measurement value of IMU, represents the IMU angular velocity, represents acceleration; w represents the process noise, and represents the measurement noise, and represents Gaussian noise. Also, the IMU gyroscope bias and IMU accelerometer bias are modeled as Gaussian noise. and Driven random walk model.

[0039] According to the IMU kinematic model, the state transfer function can be specifically expressed as:

[0040] in, represents the state transition function, Indicates that the gravity vector and the anchor point coordinates are modeled as a stationary process, and the anchor point coordinates will eventually converge to a fixed coordinate.

[0041] Assume that At this moment, the discrete state of the navigation system is updated through GNSS observation and the optimal state is obtained. and covariance The new round of prediction process starts from this state and continues until the next GNSS data arrives. Assuming that the noise is 0, the original IMU measurement value is directly used for integration, which can be written as: The footer here is wrong, it should be j-1 in, and Indicates the prediction status of adjacent moments in the prediction process, Indicates the optimal state obtained by the last update. Considering the noise of the actual IMU measurement value, the real state at this time is expressed as , error state It can be expressed as:

[0042] in, Indicates the error state, Indicates the real state, Represents the predicted state, Indicates that the navigation system is i The discrete state of each IMU moment, Indicates the sampling interval of IMU, Represents the state transition function. By linearizing at , we can get the approximate propagation form of the error state:

[0043] in, express i Error state at time +1, express right The partial derivative, express i The error state at the moment, express right The partial derivative, Indicates thei process noise at time k. Let the covariance of the Gaussian white noise be Q, then the propagated form of the covariance is as follows:

[0044] where, denotes i the covariance at time k+1, denotes i the covariance at time k, and Q denotes the covariance of the Gaussian white noise. The error state satisfies the zero-mean Gaussian distribution, and the specific formula is as follows:

[0045] where, denotes i the error state at time k+1, denotes the zero-mean Gaussian distribution, denotes i the covariance at time k+1.

[0046] On the basis of the above embodiment, in step S103, an observation model is constructed, and the discrete state of the navigation system is iteratively updated to determine the optimal state of the navigation system at the current time, including: based on the current discrete state of the navigation system, a rotation matrix from the Earth-Centered Earth-Fixed coordinate system to the East-North-Sky coordinate system is determined; according to the rotation matrix, the GNSS observation position and velocity of the GNSS antenna in the East-North-Sky coordinate system are determined, and an observation model is constructed; according to the standard process of the Error-State Iterated Kalman Filter (ESIKF), the discrete state of the navigation system is iteratively updated in combination with the observation model to determine the optimal state of the navigation system at the current time.

[0047] In this embodiment, the GNSS residual is calculated using the position and velocity of the GNSS receiver antenna in the ECEF coordinate system obtained from the GNSS receiver. It is assumed that the receiver antenna and the body coordinate system are rigidly connected, and the installation position of the receiver antenna in the body coordinate system is known. To fuse it with the local state measurement of the navigation system, the anchor point is converted into the ENU coordinate system to realize the coordinate unification of the observation, and the specific modeling process is as follows: Given the current state of the anchor point, the geographic latitude and the longitude , the specific form of the rotation matrix from the ECEF coordinate system to the ENU coordinate system is as follows:

[0048] where, represents the rotation matrix, Indicates the geographical latitude of the anchor point, Indicates the geographical longitude of the anchor point. Finally, the GNSS observation position and speed in the GNSS antenna ENU coordinate system can be obtained. The specific formula is as follows:

[0049] in, Indicates the GNSS observation position in the ENU coordinate system, represents the rotation matrix, Indicates the GNSS observation position in the ECEF coordinate system, represents the estimated anchor coordinates in the current state, Indicates the GNSS observation speed in the ENU coordinate system, Indicates the GNSS observation velocity in the ECEF coordinate system.

[0050] Assume that the actual position, attitude and velocity of the navigation system in the current ENU coordinate system are 、 and , and the true angular velocity is the current gyroscope measurement value minus the bias (used to compensate for the velocity error caused by the length of the antenna arm when the body rotates). The true position and velocity in the ECEF coordinate system are and , the actual anchor point position is , we can get the following ideal observation model:

[0051] in, Indicates the true position of the navigation system in the ENU coordinate system, Indicates the true attitude of the navigation system in the ENU coordinate system, Indicates the installation position of the receiver in the body coordinate system, Represents the real rotation matrix from the ECEF coordinate system to the ENU coordinate system obtained by the real coordinates of the anchor point, Indicates the real position in the ECEF coordinate system of the navigation system, Indicates the anchor point position, Indicates the speed of the navigation system in the ENU coordinate system, represents the IMU angular velocity, Indicates the IMU gyroscope bias, Indicates the velocity of the navigation system in the ECEF coordinate system.

[0052] However, GNSS measurements are noisy , the actual measured value is:

[0053] Therefore, the observation model can be expressed as:

[0054] in, This is the GNSS observation noise, which is usually determined by the precision factor given by the GNSS receiver.

[0055] According to the standard process of error state iterative Kalman filtering, the discrete state estimated by the navigation system (hereinafter referred to as the estimated state) is combined with the GNSS observation model to iteratively update the discrete state of the navigation system. This process can be modeled as a maximum a posteriori estimation problem.

[0056] The estimated state and its covariance obtained by the IMU deterministic integration constitute the prior distribution:

[0057] in, x Indicates the real state, represents the estimated state, represents a zero-mean Gaussian distribution, Rewrite the observation model equation as the error state and observation noise v Function , for the kth iteration in an update ,Right now , the observation model equation is and v = 0, and make a first-order approximation:

[0058] in, Indicates the navigation system k The discrete state of the iteration, H k represents the error state Jacobian matrix, L k represents the noise Jacobian matrix, Indicates the k The error state of the iteration. At this time, the approximate measurement distribution can be obtained as:

[0059] in, represents the likelihood distribution of observations for the state, z k represents the residual, R represents the noise distribution, The noise covariance is represented. The Kalman gain is calculated and updated iteratively, and the specific formula is as follows:

[0060]

[0061] wherein, K The Kalman gain is represented, H k The error state Jacobian matrix is represented, R -1 The inverse matrix of R is represented, The inverse matrix of is represented, I The unit matrix is represented, The estimated state is represented. After convergence, the error state and which are the optimal state at the current moment are obtained, and the covariance is updated to continue the forward propagation of the next round, and the specific formula is as follows:

[0062] wherein, The covariance of the optimal state at the current moment is represented, The covariance of the predicted state at the current moment is represented. After the update process is completed, the current ENU coordinate is converted into the navigation result of the ECEF coordinate according to the converged anchor point coordinate.

[0063] The experimental verification result of the application is shown in the accompanying Figures 3-6 , Figure 3 is a result schematic diagram of positioning only by using GNSS, Figure 4 is a result schematic diagram of positioning only by using INS, Figure 5 is a result schematic diagram of positioning by fusing GNSS and INS, Figure 6 The GNSS / INS absolute trajectory error visualization result schematic diagram in the embodiment of the application is shown. It can be seen that GNSS will not have cumulative drift, but its positioning error is large and discontinuous, and the trajectory has a large deviation relative to the real trajectory; INS has high instantaneous measurement accuracy, but only INS integration will completely drift soon. Figure 5 The positioning result after fusing GNSS and INS by the method proposed in the application is shown, the trajectory is continuous and smooth and well agrees with the real trajectory; the specific error level and distribution are shown in Figure 6 , and the positioning error of most positions on the trajectory is in the centimeter level, which verifies the effectiveness and superiority of the application.

[0064] In this method, during the navigation system startup phase, the magnetometer alone is used to perform a short-window average of the geomagnetic vector to quickly determine the initial heading angle in the ENU coordinate system, which is used to establish a stable ENU reference frame. The filter state space is defined in the ENU coordinate system and includes the attitude, position, and velocity of the aircraft relative to the ENU, as well as the IMU bias and the estimated position of the ENU origin in the ECEF.

[0065] In terms of the observation model, this method uniformly rotates the ECEF coordinate system position and velocity output by the GNSS receiver to the ENU coordinate system. These are then iteratively updated together with the ENU state predicted by the INS within the same ESIKF. The ESIKF's multiple linearization characteristics significantly reduce the approximation error caused by the nonlinear coupling of attitude, position, and velocity, improving integrated navigation accuracy. Furthermore, the ECEF coordinates of the ENU origin retained in the state vector can adaptively compensate for anchor point initialization errors during operation, further improving long-duration positioning stability.

[0066] In summary, by employing one-time magnetometer heading alignment and a semi-tightly integrated GNSS / INS fusion framework based on an error-state iterative Kalman filter, the integrated navigation system completes attitude initialization and achieves centimeter-level accuracy within a short period of time after startup. During subsequent operation, the long-term interference of magnetometer measurements on the filter is completely eliminated. Simultaneously, by coupling the GNSS position, velocity, and inertial navigation prediction covariance within the same filter, this approach achieves faster iterative convergence and higher state consistency than traditional loose integration, while maintaining computational complexity only slightly higher than that of a classic EKF. This approach combines high precision, low computational power, and strong robustness, providing a reliable navigation solution for applications such as drones, autonomous driving, and outdoor service robot inspections.

[0067] The present invention also provides a GNSS and INS semi-tight integrated navigation device. The GNSS and INS semi-tight integrated navigation device provided by the present invention is described below. The GNSS and INS semi-tight integrated navigation device described below and the GNSS and INS semi-tight integrated navigation method described above can be referenced to each other. Figure 7 This is a structural block diagram of the GNSS and INS semi-tight integrated navigation device provided by the present invention, such as Figure 7 As shown, the device includes: Initialization module 701 is used to initialize the navigation system; the navigation system is a semi-compact integrated navigation system of GNSS and INS; A prediction module 702 is used to predict the discrete state of the navigation system to obtain the discrete state; An updating module 703 is used to construct an observation model and iteratively update the discrete states of the navigation system to determine the optimal state of the navigation system at the current moment; The conversion module 704 is used to convert the current station center coordinates into the Earth-centered Earth-fixed coordinate system according to the iteratively updated converged anchor point coordinates.

[0068] In this device, the navigation system adopts a semi-tight combination framework of GNSS and INS. First, the initialization module 701 initializes the navigation system and adjusts the timestamp of the navigation system so that the navigation system can complete the attitude initialization and enter the centimeter-level precision working state within a short time after startup. Then, the prediction module 702 predicts the discrete state of the navigation system to prepare for subsequent state updates. The update module 703 then constructs the observation model, iteratively updates the discrete state of the navigation system, determines the optimal state of the navigation system at the current moment, reduces the approximate error caused by the nonlinear coupling of attitude, position and velocity, and improves the accuracy of the navigation system. Finally, the conversion module 704 completes the conversion of the navigation system station center coordinates and the Earth-centered Earth-fixed coordinate system based on the anchor point coordinates converged by the iterative update. In the above process, high precision, low computing power and strong robustness are taken into account, solving the problem of difficult effective balance between navigation accuracy and computing power in existing related technologies.

[0069] Figure 8 An example of a physical structure diagram of an electronic device is shown below. Figure 8 As shown, the electronic device may include: a processor 801, a communication interface 802, a memory 803, and a communication bus 804, wherein the processor 801, the communication interface 802, and the memory 803 communicate with each other via the communication bus 804. The processor 801 may call the logic instructions in the memory 803 to execute the GNSS and INS semi-compact integrated navigation method, which includes: Initialize the navigation system; the navigation system is a semi-compact integrated navigation system of GNSS and INS; Predict the discrete state of the navigation system and obtain the discrete state; Construct an observation model and iteratively update the discrete state of the navigation system to determine the optimal state of the navigation system at the current moment; According to the anchor point coordinates converged by iterative updates, the current station center coordinates are converted into the Earth-centered Earth-fixed coordinate system.

[0070] Furthermore, the logic instructions in the aforementioned memory 803 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product, stored in a storage medium, includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the various embodiments of the method of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0071] On the other hand, the present invention further provides a computer program product, which includes a computer program. The computer program can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can perform the GNSS and INS semi-compact integrated navigation method provided by the above methods, which includes: Initialize the navigation system; the navigation system is a semi-compact integrated navigation system of GNSS and INS; Predict the discrete state of the navigation system and obtain the discrete state; Construct an observation model and iteratively update the discrete state of the navigation system to determine the optimal state of the navigation system at the current moment; According to the anchor point coordinates converged by iterative updates, the current station center coordinates are converted into the Earth-centered Earth-fixed coordinate system.

[0072] In another aspect, the present invention further provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to perform the GNSS and INS semi-compact integrated navigation method provided by the above methods, the method comprising: Initialize the navigation system; the navigation system is a semi-compact integrated navigation system of GNSS and INS; Predict the discrete state of the navigation system and obtain the discrete state; Construct an observation model and iteratively update the discrete state of the navigation system to determine the optimal state of the navigation system at the current moment; According to the anchor point coordinates converged by iterative updates, the current station center coordinates are converted into the Earth-centered Earth-fixed coordinate system.

[0073] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, i.e., they may be located in one location or distributed across multiple network units. Some or all of the modules may be selected based on actual needs to achieve the objectives of the present embodiment. Persons of ordinary skill in the art will be able to understand and implement the present invention without inventive effort.

[0074] Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented using software plus a necessary general-purpose hardware platform, or of course, hardware. Based on this understanding, the essence of the above technical solution, or the portion that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a magnetic disk, or an optical disk, and includes a number of instructions for causing a computer device (such as a personal computer, server, or network device) to execute the methods described in each embodiment or certain portions of the embodiments.

[0075] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A GNSS and INS semi-tight integrated navigation method, characterized in that: include: Initialize the navigation system; The navigation system is a semi-compact integrated navigation system of GNSS and INS; Predicting the discrete state of the navigation system to obtain the discrete state; Constructing an observation model and iteratively updating the discrete states of the navigation system to determine the optimal state of the navigation system at the current moment; According to the anchor point coordinates converged by iterative updates, the current station center coordinates are converted into the Earth-centered Earth-fixed coordinate system.

2. The GNSS and INS semi-tight integrated navigation method according to claim 1, characterized in that: Initialize the navigation system, including: The GNSS timestamp of the navigation system is converted from GPS time to local system time, taking into account the receiver solution delay.

3. The GNSS and INS semi-tight integrated navigation method according to claim 1, characterized in that: Initialize the navigation system, including: Record the first frame of GNSS's Earth-centered Earth-fixed coordinate system position as the initial coordinate of the origin of the East-North-Sky coordinate system; Read the inertial measurement unit data and combine it with the magnetometer to determine the three-dimensional basis of the east-north-sky coordinate system; The sliding window averaging method is used to estimate the gravity direction of the body coordinate system, and the state of the navigation system in the east-north-sky coordinate system at the initial moment is determined.

4. The GNSS and INS semi-tight integrated navigation method according to claim 3, characterized in that: Read the inertial measurement unit data and combine it with the magnetometer to determine the three-dimensional basis of the east-north-sky coordinate system, including: Read the inertial measurement unit and magnetometer data, perform gyroscope short-time integration, project all magnetic vectors onto the horizontal plane, and then perform weighted average to obtain the horizontal component of the geomagnetic field. Determining an initial heading angle based on the angle between the geomagnetic horizontal component and the true north direction; The initial heading angle and the GNSS anchor point are combined to determine the three-axis basis of the east-north-sky coordinate system.

5. The GNSS and INS semi-tight integrated navigation method according to claim 3, characterized in that: The sliding window averaging method is used to estimate the gravity direction of the body coordinate system and determine the state of the navigation system in the east-north-sky coordinate system at the initial moment, including: Determine the zero bias of the accelerometer and gyroscope using a sliding window averaging method and estimate the direction of gravity in the body coordinate system; Define the gravity direction in the east-north-sky coordinate system and obtain the rotation matrix through singular value decomposition; The state of the navigation system in the east-north-sky coordinate system at an initial moment is determined based on the rotation matrix.

6. The GNSS and INS semi-tight integrated navigation method according to claim 1, characterized in that: After the navigation system is initialized, the magnetometer input is frozen and only the gyroscope, accelerometer and GNSS data streams are saved.

7. The GNSS and INS semi-tight integrated navigation method according to claim 1, characterized in that: Predicting the discrete state of the navigation system to obtain the discrete state includes: Constructing a state transfer model of the navigation system and determining a state transfer function based on a kinematic model of an inertial measurement unit; predicting discrete states of the navigation system in combination with the state transfer function; The true state of the navigation system is assumed based on a GNSS observation model, and the error state is determined in combination with the discrete state of the navigation system.

8. The GNSS and INS semi-tight integrated navigation method according to claim 7, characterized in that: Constructing an observation model and iteratively updating the discrete states of the navigation system to determine the optimal state of the navigation system at the current moment, including: determining a rotation matrix from an Earth-centered Earth-fixed coordinate system to an East-North-Sky coordinate system based on a current discrete state of the navigation system; Determine the GNSS observation position and velocity of the GNSS antenna in the east-north-sky coordinate system according to the rotation matrix, and construct an observation model; According to the standard process of iterative Kalman filtering of the error state, in combination with the observation model, the discrete state of the navigation system is iteratively updated to determine the optimal state of the navigation system at the current moment.

9. A GNSS and INS semi-compact integrated navigation device, characterized in that: include: Initialization module, used for initializing the navigation system; The navigation system is a semi-compact integrated navigation system of GNSS and INS; A prediction module, configured to predict the discrete state of the navigation system to obtain the discrete state; An updating module, configured to construct an observation model and iteratively update the discrete states of the navigation system to determine the optimal state of the navigation system at the current moment; The conversion module is used to convert the current station center coordinates into the Earth-centered Earth-fixed coordinate system according to the anchor point coordinates converged by iterative updates.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the GNSS and INS semi-tight integrated navigation method according to any one of claims 1 to 8 is implemented.

Images