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

Abstract

The application provides a GNSS and INS semi-tight combination navigation method, device and electronic equipment, wherein the GNSS and INS semi-tight combination navigation method comprises: performing system initialization on a navigation system; the navigation system is a GNSS and INS semi-tight combination navigation system; predicting a discrete state of the navigation system to obtain a discrete state; constructing an observation model, and iteratively updating the discrete state of the navigation system to determine an optimal state of the navigation system at a current time; and converting the current station-centered coordinate into a geocentric geodetic coordinate system according to an anchor point coordinate of iterative update convergence. Through the application, 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 application relates to the field of satellite navigation technology, and particularly relates to a GNSS and INS semi-tight combination navigation method and device and electronic equipment. BACKGROUND

[0002] A global navigation satellite system (GNSS) can provide absolute positioning at the level of meters or even centimeters, but in complex environments such as urban canyons, under forests, or indoor-outdoor transition areas, satellite signals will be affected by shielding and multipath effects, resulting in a significant decrease in positioning accuracy or even complete loss of lock. An inertial navigation system (INS) can continuously output its own angular velocity and acceleration information at a high frequency, and has good relative motion measurement capability in the short term, but long-term integration will cause error divergence due to random drift and bias accumulation. Therefore, the industry and academia generally adopt a GNSS and INS combination navigation strategy: using the absolute positioning information provided by the GNSS to constrain the drift of the INS, to achieve high-precision, all-weather positioning services.

[0003] There are generally three combination modes for GNSS / INS combination navigation systems, namely: loose combination mode, tight combination mode and deep combination mode. Loose combination uses the position and velocity independently calculated by the receiver as measurement input, and performs post-level fusion with the INS navigation solution in the filter. The interface is friendly and the implementation is simple, but the time delay, coordinate system conversion and two-level covariance inconsistency problems will weaken the suppression efficiency of GNSS information on INS drift. Tight combination directly integrates raw observations such as pseudo-range, carrier phase and Doppler into the observation equation shared with the INS, which can significantly improve the 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, and the complexity of processing ephemeris, ionosphere and troposphere modeling, calculation and implementation is greatly increased. Deep combination further utilizes the INS calculated velocity to close the loop to assist the GNSS signal tracking loop, which can maintain carrier lock even in extremely low signal-to-noise ratio or three visible satellite scenarios, but requires extremely high design requirements for software and hardware collaboration.

[0004] In view of this, a semi-tight combination scheme between loose coupling and tight coupling appears: INS prediction and GNSS measurement are processed simultaneously in a unified filter without deep access to the original pseudorange data of the receiver baseband. Compared with loose combination, semi-tight coupling avoids the time delay and covariance information loss caused by two-stage solving, and can implement accurate measurement iteration in the same state space; compared with tight combination and deep combination, it does not depend on channel-level data, has no hardware invasion, significantly reduces the amount of calculation, and is easy to deploy on conventional commercial GNSS modules and embedded flight controls. However, the existing semi-tight combination is usually based on the recursive filter (Extended Kalman Filter, EKF) which solves the complete state once linearization, and has the defect of large linearization error.

[0005] For the problem that the navigation accuracy and the computing power are difficult to effectively balance in the existing related technology, there is no effective solution at present. SUMMARY

[0006] The application provides a GNSS and INS semi-tight combination navigation method, device and electronic equipment, to solve the defect that the navigation accuracy and the computing power are difficult to effectively balance in the existing related technology.

[0007] In a first aspect, the application provides a GNSS and INS semi-tight combination navigation method, comprising:

[0008] System initialization is performed on the navigation system; the navigation system is a GNSS and INS semi-tight combination navigation system;

[0009] The discrete state of the navigation system is predicted to obtain a discrete state;

[0010] 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;

[0011] According to the anchor point coordinates of the iterative update convergence, the current station-centered coordinates are converted into the geocentric and terrestrial coordinates.

[0012] According to the GNSS and INS semi-tight combination navigation method provided by the application, system initialization is performed on the navigation system, comprising:

[0013] The GNSS timestamp of the navigation system is converted from GPS time to local system time, and the receiver solving delay is considered.

[0014] According to the GNSS and INS semi-tight combination navigation method provided by the application, system initialization is performed on the navigation system, comprising:

[0015] Record the first frame GNSS geocentric coordinate system position as the initial coordinates of the East-North-Sky coordinate system origin;

[0016] Read the inertial measurement unit data, and determine the three-way base of the East-North-Sky coordinate system in combination with the magnetometer;

[0017] The gravity direction of the body coordinate system is estimated by using the sliding window average method, and the state of the navigation system in the East-North-Sky coordinate system at the initial time is determined.

[0018] According to the GNSS and INS semi-tight combination navigation method provided by the application, the inertial measurement unit data is read, and the three-way base of the East-North-Sky coordinate system is determined in combination with the magnetometer, which comprises:

[0019] Read the inertial measurement unit and magnetometer data, project all magnetic vectors to the horizontal plane after short-time integration by the gyroscope, and obtain the geomagnetic horizontal component by weighted average;

[0020] Determine the initial heading angle in combination with the included angle between the geomagnetic horizontal component and the north direction;

[0021] Determine the three-way base of the East-North-Sky coordinate system in combination with the initial heading angle and the GNSS anchor point.

[0022] According to the GNSS and INS semi-tight combination navigation method provided by the application, the gravity direction of the body coordinate system is estimated by using the sliding window average method, and the state of the navigation system in the East-North-Sky coordinate system at the initial time is determined.

[0023] Determine the accelerometer and gyroscope zero offset by using the sliding window average method, and estimate the gravity direction of the body coordinate system;

[0024] Define the gravity direction in the East-North-Sky coordinate system, and obtain the rotation matrix through singular value decomposition;

[0025] Determine the state of the navigation system in the East-North-Sky coordinate system at the initial time based on the rotation matrix.

[0026] According to the GNSS and INS semi-tight combination navigation method provided by the application, after the system initialization of the navigation system, it comprises: freezing the magnetometer input, and only saving the gyroscope, accelerometer and GNSS data stream.

[0027] According to the GNSS and INS semi-tight combination navigation method provided by the application, the discrete state of the navigation system is predicted to obtain the discrete state, which comprises:

[0028] Construct the state transition model of the navigation system, and determine the state transition function according to the kinematics model of the inertial measurement unit;

[0029] predicting a discrete state of the navigation system in combination with the state transition function;

[0030] assuming a true state of the navigation system according to a GNSS observation model, and determining an error state in combination with the discrete state of the navigation system.

[0031] According to the GNSS and INS semi-tight integrated navigation method provided by the application, 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, comprising:

[0032] determining a rotation matrix from the Earth-Centered Earth-Fixed coordinate system to the East-North-Up coordinate system based on the current discrete state of the navigation system;

[0033] determining the GNSS observation position and velocity of the GNSS antenna in the East-North-Up coordinate system according to the rotation matrix, and constructing an observation model;

[0034] iteratively updating the discrete state of the navigation system according to the standard process of the iterative Kalman filtering in combination with the observation model to determine the optimal state of the navigation system at the current time.

[0035] In a second aspect, the application further provides a GNSS and INS semi-tight integrated navigation device, comprising:

[0036] an initialization module for system initialization of the navigation system; the navigation system is a GNSS and INS semi-tight integrated navigation system;

[0037] a prediction module for predicting the discrete state of the navigation system to obtain the discrete state;

[0038] an updating module for constructing an observation model and iteratively updating the discrete state of the navigation system to determine the optimal state of the navigation system at the current time;

[0039] a conversion module for converting the current station-centered coordinate into the Earth-Centered Earth-Fixed coordinate system according to the anchor point coordinate of the iterative update convergence.

[0040] In a third aspect, the 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 integrated navigation method according to the first aspect.

[0041] In a fourth aspect, the present application also provides a non-transitory computer-readable storage medium having stored thereon a computer program, which, when executed by a processor, implements the GNSS and INS semi-tight integrated navigation method according to the first aspect.

[0042] In a fifth aspect, the present application also provides a computer program product comprising a computer program, which, when executed by a processor, implements the GNSS and INS semi-tight integrated navigation method according to the first aspect.

[0043] Compared with the prior art, the present application has the following beneficial effects:

[0044] The GNSS and INS semi-tight integrated navigation method provided by the present application initializes the navigation system, adjusts the time stamp of the navigation system, 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. An observation model is constructed to iteratively update the discrete state of the navigation system, determine the optimal state of the navigation system at the current time, reduce the approximation error caused by the attitude-position-velocity nonlinear coupling, and improve the precision of the navigation system. Finally, the anchor point coordinates of the iterative update convergence are used to complete the conversion between the station-centered coordinates and the geocentric and geodetic coordinate system. In the above process, high precision, low computing power and strong robustness are considered, and the problem of difficult effective balance between navigation precision and computing power in the prior art is solved. BRIEF DESCRIPTION OF DRAWINGS

[0045] 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 to be used in the embodiments or prior art description. Obviously, the drawings in the following description are some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0046] Figure 1 is a flowchart of the GNSS and INS semi-tight integrated navigation method provided by the present application;

[0047] Figure 2 is a schematic diagram of a route for navigation in an embodiment of the present application;

[0048] Figure 3 is a result schematic diagram of positioning by only using GNSS;

[0049] Figure 4 is a result schematic diagram of positioning by only using INS;

[0050] Figure 5 is a result schematic diagram of positioning by fusing GNSS and INS;

[0051] Figure 6 is a schematic diagram of GNSS / INS absolute trajectory error visualization result in the embodiment of the application;

[0052] Figure 7 is a structural block diagram of the GNSS and INS semi-tight combination navigation device provided by the application;

[0053] Figure 8 is a structural schematic diagram of the electronic device provided by the application. DETAILED DESCRIPTION

[0054] 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 with reference to the drawings in the present application. Obviously, the described embodiments are part 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 skilled in the art without creative labor fall within the protection scope of the present application.

[0055] The present application provides a GNSS and INS semi-tight combination navigation method, Figure 1 is a flowchart of the GNSS and INS semi-tight combination navigation method provided by the application, as Figure 1 shown, the method comprises the following steps:

[0056] Step S101, system initialization is performed on the navigation system; the navigation system is a GNSS and INS semi-tight combination navigation system;

[0057] Step S102, the discrete state of the navigation system is predicted to obtain the discrete state;

[0058] 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;

[0059] Step S104, according to the anchor point coordinates of the iterative update convergence, the current station-centered coordinates are converted into the earth-centered earth-fixed coordinate system.

[0060] In the method, the navigation system adopts a GNSS and INS semi-tight combination framework. First, the navigation system is initialized, the timestamp of the navigation system is adjusted, 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, and preparation is made for subsequent state updating. Then, an observation model is constructed, the discrete state of the navigation system is iteratively updated, the optimal state of the navigation system at the current time is determined, the approximation error caused by the attitude-position-velocity nonlinear coupling is reduced, and the precision of the navigation system is improved. Finally, according to the anchor point coordinates of the iterative update convergence, the conversion between the station-centered coordinate system and the geocentric geodetic coordinate system of the navigation system is completed. In the above process, high precision, low computing power and strong robustness are considered, and the problem that the navigation precision and computing power are difficult to effectively balance in the prior art is solved.

[0061] Figure 2 is a route schematic diagram for navigation in the embodiment of the application, as Figure 2 shown, in some embodiments, step S101, system initialization of the navigation system includes: converting the GNSS timestamp of the navigation system from GPS time to local system time, and considering receiver solution delay.

[0062] For example, after power-on, the clock of the inertial measurement unit (IMU) and the GNSS is first synchronized, for the GNSS timestamp, it needs to be converted from GPS time to local UNIX time, and the receiver solution delay is considered. The timestamp output by the GNSS receiver is generally in the form of "GPS week + intra-week second", the GPS time system takes January 6, 1980 UTC 00:00:00 as the starting point, and the local system time (such as laser radar, IMU) generally uses UNIX time (takes January 1, 1970 UTC 00:00:00 as the starting point). The timestamp conversion includes time starting point offset and UNIX time accumulated leap second number, and the current leap second is 18 seconds. The specific conversion method is as follows:

[0063]

[0064] wherein, represents UNIX time, represents the number of weeks, represents intra-week seconds, represents leap seconds, represents receiver time delay.

[0065] In some embodiments, the step S101 of initializing the navigation system includes: recording a first frame GNSS Earth-Centered, Earth-Fixed (ECEF) position as an initial coordinate of an East-North-Up (ENU) origin; reading inertial measurement unit data and determining an ENU three-direction base in combination with a magnetometer; estimating a gravity direction of a body coordinate system using a sliding window average method, and determining a state of the navigation system in the ENU coordinate system at an initial time.

[0066] Specifically, reading the inertial measurement unit data and determining the ENU three-direction base in combination with the magnetometer includes: reading the inertial measurement unit and magnetometer data, projecting all magnetic vectors to a horizontal plane and then performing weighted averaging to obtain a geomagnetic horizontal component through short-time integration of a gyroscope; determining an initial heading angle in combination with an included angle between the geomagnetic horizontal component and a north direction; and determining the ENU three-direction base in combination with the initial heading angle and a GNSS anchor point.

[0067] More specifically, estimating the gravity direction of the body coordinate system using the sliding window average method, and determining the state of the navigation system in the ENU coordinate system at the initial time includes: determining zero offsets of the accelerometer and the gyroscope using the sliding window average method, and estimating the gravity direction of the body coordinate system; defining a gravity direction in the ENU coordinate system, and obtaining a rotation matrix through singular value decomposition (SVD); and determining the state of the navigation system in the ENU coordinate system at the initial time based on the rotation matrix.

[0068] After the navigation system is initialized, the method further includes: freezing the magnetometer input, and only saving the gyroscope, accelerometer and GNSS data streams.

[0069] For example, a first frame GNSS ECEF position of the navigation system is recorded as an initial coordinate of an ENU origin. The navigation system continuously reads about 0.1 s (20 frames) of IMU data, utilizes short-time integration of a gyroscope to eliminate pitch and roll effects, projects all magnetic vectors to a horizontal plane and then performs weighted averaging to obtain a geomagnetic horizontal component. An initial heading angle is calculated from an included angle between the geomagnetic horizontal component and a north direction, and an ENU east, north and sky three-direction base is determined in combination with a GNSS anchor point. A sliding window average method is used to calculate zero offsets of the accelerometer and the gyroscope, and a gravity direction of a body coordinate system is estimated. In this embodiment, the gravity direction in the ENU coordinate system is defined as A rotation matrix is solved through singular value decomposition satisfying 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. At this time, the nominal state vector consists of the attitude matrix, position, velocity, gyroscope and accelerometer biases, and anchor point ECEF coordinates in the ENU coordinate system. The error covariance matrix provides a small initial variance for attitude, position, and velocity, a manufacturer-calibrated value for the biases, and an uncertainty on the order of meters for the anchor point coordinates, which is used for subsequent self-compensation.

[0070] In some embodiments, step S102, predicting the discrete state of the navigation system to obtain the discrete state, includes: constructing a state transition model of the navigation system and determining a state transition function based on the kinematic model of the inertial measurement unit; predicting the discrete state of the navigation system by combining the state transition function; obtaining the true state of the navigation system and determining the error state by combining the discrete state of the navigation system.

[0071] For example, let Indicates the navigation system in the i Given the discrete states at IMU times, the state transition model can be expressed as:

[0072]

[0073] in, Indicates the navigation system in the i Discrete state at +1 IMU time step, Indicates the navigation system in the i Discrete state of IMU at each time step Indicates the sampling interval of the IMU. Represents the state transition function. Indicates the first i The IMU measurement at each time point Indicates the first i The process noise at each moment is specifically defined as:

[0074]

[0075]

[0076]

[0077] Among them, the body state is defined in the manifold M on , Represents the three-dimensional rotation group in the state. Represents the remaining 18-dimensional Euclidean states. This indicates a total dimension of 21. x Indicates the state of the organism. denotes the pose of the navigation system body frame in the ENU frame, denotes the position of the navigation system body frame in the ENU frame, denotes the velocity of the navigation system body frame in the ENU frame, denotes the IMU gyroscope bias, denotes the IMU accelerometer bias, denotes the gravity vector in the ENU frame, denotes the ECEF coordinates of the origin of the ENU frame, i.e. the anchor point; u denotes the IMU measurements, denotes the IMU angular velocity, denotes the acceleration; w denotes the process noise, and denotes the measurement noise, and denotes the Gaussian noise. Also, the IMU gyroscope bias and the IMU accelerometer bias are modeled as Gaussian noise and driven random walk model.

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

[0079]

[0080] wherein, denotes the state transition function, denotes that the gravity vector and the anchor point coordinates are modeled as stationary processes, and the anchor point coordinates will eventually converge to a fixed coordinate.

[0081] Suppose at time , the discrete state of the navigation system has undergone an update through GNSS observation, obtaining the optimal state and the covariance . The new round of prediction process starts from this state and lasts until the next GNSS data arrives. Assuming that the noise is 0, the integral of the original IMU measurement is directly used, which can be specifically written as:

[0082] Here the subscript is wrong, it should be j-1

[0083] wherein, and denote the predicted states of adjacent time points in the prediction process, denotes the optimal state obtained by the last update. Considering the noise of the actual IMU measurement, the true state at this time can be expressed as , and the error state can be expressed as:

[0084]

[0085] in, Indicates the error status. Represents the true state. Indicates the predicted state. Indicates the navigation system in the i Discrete state of each IMU at each time step Indicates the sampling interval of the IMU. This represents the state transition function. By linearizing the error state, we can obtain an approximate propagation form of the error state:

[0086]

[0087] in, express i Error state at time +1 express right The partial derivative, express i Error state at time, express right The partial derivative, Indicates the first i The process noise at each moment. Let Q be the covariance of the Gaussian white noise. The propagation form of the covariance is as follows:

[0088]

[0089] in, express i Covariance at time +1 express i The covariance at time step Q represents the covariance of the Gaussian white noise. Error state. It follows a zero-mean Gaussian distribution, and the specific formula is as follows:

[0090]

[0091] in, express i Error state at time +1 This represents a Gaussian distribution with zero mean. express i Covariance at time +1.

[0092] On the basis of the above embodiment, step S103, the 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, comprising: determining the rotation matrix from the Earth-Centered Earth-Fixed coordinate system to the East-North-Sky coordinate system based on the current discrete state of the navigation system; determining the GNSS observation position and velocity of the GNSS antenna in the East-North-Sky coordinate system according to the rotation matrix, and constructing an observation model; according to the standard process of Error-State Iterated Kalman Filter (ESIKF), combining the observation model, iteratively updating the discrete state of the navigation system, and determining the optimal state of the navigation system at the current time.

[0093] In the present embodiment, the GNSS residual calculation is performed 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 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:

[0094] The current state is known, the anchor point geographic latitude and longitude , and the specific form of the rotation matrix from the ECEF coordinate system to the ENU coordinate system is:

[0095]

[0096] wherein, represents the rotation matrix, represents the anchor point geographic latitude, represents the anchor point geographic longitude. Finally, the GNSS observation position and velocity of the GNSS antenna in the ENU coordinate system can be obtained, and the specific formula is as follows:

[0097]

[0098] wherein, represents the GNSS observation position in the ENU coordinate system, represents the rotation matrix, represents the GNSS observation position in the ECEF coordinate system, represents the estimated anchor point coordinate in the current state, represents the GNSS observation velocity in the ENU coordinate system, represents the GNSS observation velocity in the ECEF coordinate system.

[0099] Assume that the real position, attitude and velocity of the navigation system in the current ENU coordinate system are , and , and the real angular velocity is the current gyro measurement minus the bias (used to compensate for the velocity error caused by the antenna boom length when the body rotates), the real position and velocity in the ECEF coordinate system are and , and the real anchor position is , then the following ideal observation model can be obtained:

[0100]

[0101] wherein represents the real position of the navigation system in the ENU coordinate system, represents the real attitude of the navigation system in the ENU coordinate system, represents 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, represents the real position of the navigation system in the ECEF coordinate system, represents the anchor position, represents the velocity of the navigation system in the ENU coordinate system, represents the IMU angular velocity, represents the IMU gyro bias, represents the velocity of the navigation system in the ECEF coordinate system.

[0102] However, the GNSS measurement has noise , and the actual measurement value is:

[0103]

[0104] Therefore, the observation model can be represented as:

[0105]

[0106] wherein is the observation noise of the GNSS, which is usually determined by the precision factor given by the GNSS receiver.

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

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

[0109]

[0110] in, x Represents the true state. Indicates the estimated state. This represents a Gaussian distribution with zero mean. This represents the covariance of the estimated state. The observation model equations are rewritten in terms of the error state. and observation noise v function For the k-th iteration in an update ,Right now The observation model equations are in and v Linearize at the point = 0, and perform a first-order approximation:

[0111]

[0112] in, Indicates the navigation system's first k Discrete states of the next iteration. H k The Jacobian matrix represents the error state. L k Represents the noise Jacobian matrix. Indicates the first k The error state of the next iteration. At this point, the approximate measurement distribution can be obtained as:

[0113]

[0114] in, This represents the likelihood distribution of observations with respect to the state. z k R represents the residual, and R represents the noise distribution. This represents the noise covariance. The Kalman gain is calculated and iteratively updated, using the following formula:

[0115]

[0116]

[0117] in, K Indicates Kalman gain, H k R represents the error state Jacobian matrix. -1 Describe the inverse matrix of R. express The inverse matrix, I Represents the identity matrix. This represents the estimated state. Upon convergence, the error state that maximizes the posterior probability is obtained. and is the optimal state at the current moment and update the covariance, continue the next round of forward propagation, the specific formula is as follows:

[0118]

[0119] wherein, indicates the covariance of the optimal state at the current moment, indicates the covariance of the predicted state at the current moment. After the update process is completed, the current ENU coordinates are converted into the navigation results of the ECEF coordinates according to the converged anchor point coordinates.

[0120] The experimental verification results of the application are shown in the accompanying Figures 3-6 , wherein, 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 can be seen. 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 true trajectory; INS has high instantaneous measurement accuracy, but only INS integration will soon completely drift. Figure 5 The positioning results after fusing GNSS and INS by the method proposed in the application are shown, the trajectory is continuous and smooth and well agrees with the true 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.

[0121] In the above method, only the geomagnetic vector is averaged by the magnetometer in the navigation system starting stage, the initial heading angle in the ENU reference frame is quickly obtained, and a stable ENU reference frame is established. The filter state space is defined in the ENU coordinate system, including the attitude, position, velocity of the body relative to the ENU, and the position estimation value of the IMU bias and the ENU origin in the ECEF.

[0122] In terms of the observation model, the ECEF coordinate system position and velocity output by the GNSS receiver are uniformly rotated to the ENU coordinate system, and are iteratively updated in the same ESIKF with the INS predicted ENU state; the multiple linearization characteristics of the ESIKF significantly reduce the approximation error caused by the nonlinear coupling of the attitude-position-velocity, and improve the integrated navigation accuracy. At the same time, the ECEF coordinates of the ENU origin reserved in the state vector can adaptively compensate the anchor point initialization error in the running, further improving the long-time positioning stability.

[0123] In summary, by using the magnetic force meter one-time heading alignment and the semi-tight combination GNSS / INS fusion framework based on error state iterative Kalman filtering, the combined navigation system can complete the attitude initialization and enter the centimeter-level precision working state in a short time after starting; In the subsequent running process, the long-term interference of the magnetic force meter measurement on the filtering is completely eliminated, and the GNSS position, velocity and inertial prediction covariance coupling in the same filter are utilized, so that faster iterative convergence and higher state consistency than the traditional loose combination are realized, and the calculation complexity is only slightly higher than the classical EKF. The method takes into account high precision, low computing power and strong robustness, and provides a reliable navigation solution for unmanned aerial vehicles, automatic driving and outdoor service robot inspection applications.

[0124] The application further provides a GNSS and INS semi-tight combination navigation device, which is described as follows. Figure 7 is a structural block diagram of the GNSS and INS semi-tight combination navigation device provided by the application, as shown in Figure 7 The device comprises:

[0125] An initialization module 701 is configured to perform system initialization on the navigation system; the navigation system is a GNSS and INS semi-tight combination navigation system.

[0126] A prediction module 702 is configured to predict the discrete state of the navigation system to obtain the discrete state.

[0127] An update module 703 is configured to 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 time.

[0128] A conversion module 704 is configured to convert the current station-centered coordinates into the geocentric and terrestrial coordinates according to the anchor point coordinates of the iterative update convergence.

[0129] In the device, the navigation system adopts a GNSS and INS semi-tight combination framework. First, the initialization module 701 initializes the navigation system, adjusts the timestamp of the navigation system, so that the navigation system can complete attitude initialization and enter a centimeter-level precision working state within a short time after startup. Then, the prediction module 702 predicts the discrete state of the navigation system, preparing for subsequent state updating. The update module 703 constructs an observation model, iteratively updates the discrete state of the navigation system, determines the optimal state of the navigation system at the current time, reduces the approximation error caused by the attitude-position-velocity nonlinear coupling, and improves the precision of the navigation system. Finally, the conversion module 704 converts the station-centered coordinates of the navigation system to the geocentric and geodetic coordinate system according to the anchor point coordinates of the iterative update convergence. 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 precision and computing power in the existing related technologies.

[0130] Figure 8 An example of an entity structure diagram of an electronic device is shown in Figure 8 The electronic device can include a processor 801, a communications interface 802, a memory 803, and a communications bus 804, wherein the processor 801, the communications interface 802, and the memory 803 communicate with each other through the communications bus 804. The processor 801 can invoke the logic instructions in the memory 803 to execute the GNSS and INS semi-tight combination navigation method, which includes:

[0131] System initialization of the navigation system; the navigation system is a GNSS and INS semi-tight combination navigation system;

[0132] Predicting the discrete state of the navigation system to obtain the discrete state;

[0133] Constructing an observation model and iteratively updating the discrete state of the navigation system to determine the optimal state of the navigation system at the current time;

[0134] According to the anchor point coordinates of the iterative update convergence, the current station-centered coordinates are converted to the geocentric and geodetic coordinate system.

[0135] Further, the logic instructions in the memory 803 described above can be implemented in the form of software functional units and sold or used as independent products, and can be stored in a computer readable storage medium. Based on such understanding, the technical solutions of the present application essentially or the parts that make contributions to the prior art or parts of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium, and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods according to the embodiments of the present application. The aforementioned 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.

[0136] In another aspect, the present application also provides a computer program product, the computer program product comprising a computer program, the computer program being stored in a non-transitory computer readable storage medium, and the computer program being executable by a processor to cause a computer to execute the GNSS and INS semi-tight integrated navigation method provided by the above-mentioned methods, the method comprising:

[0137] initializing the navigation system; the navigation system being a GNSS and INS semi-tight integrated navigation system;

[0138] predicting the discrete state of the navigation system to obtain a discrete state;

[0139] constructing an observation model, and iteratively updating the discrete state of the navigation system to determine an optimal state of the navigation system at a current time;

[0140] converting the current station-centered coordinates into the geocentric geodetic coordinates according to the anchor point coordinates of the iterative updating convergence.

[0141] In another aspect, the present application also provides a non-transitory computer readable storage medium, the non-transitory computer readable storage medium storing a computer program, the computer program being executable by a processor to implement the GNSS and INS semi-tight integrated navigation method provided by the above-mentioned methods, the method comprising:

[0142] initializing the navigation system; the navigation system being a GNSS and INS semi-tight integrated navigation system;

[0143] predicting the discrete state of the navigation system to obtain a discrete state;

[0144] constructing an observation model, and iteratively updating the discrete state of the navigation system to determine an optimal state of the navigation system at a current time;

[0145] According to the iterative updating convergence of the anchor point coordinates, the current station-centered coordinates are converted into the geocentric geodetic coordinate system.

[0146] 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 distributed on multiple network units. Part or all of the modules can be selected to achieve the purpose of the embodiment scheme according to actual needs. Those skilled in the art can understand and implement without creative labor.

[0147] 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 plurality 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.

[0148] 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 GNSS and INS semi-tight integrated navigation method, characterized in that, The method comprises the following steps: system initialization of the 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 a discrete state; constructing an observation model and iteratively updating the discrete state of the navigation system to determine the optimal state of the navigation system at the current time; converting the current station-centered coordinates into the geocentric geodetic coordinate system according to the anchor point coordinates of the iterative update convergence; predicting the discrete state of the navigation system to obtain a discrete state, comprising: constructing a state transition model of the navigation system, and determining a state transition function according to a kinematic model of an inertial measurement unit; combining the state transition function to predict the discrete state of the navigation system; assuming the true state of the navigation system according to a GNSS observation model, and combining the discrete state of the navigation system to determine an error state; constructing an observation model and iteratively updating the discrete state of the navigation system to determine the optimal state of the navigation system at the current time, comprising: determining a rotation matrix from the geocentric geodetic coordinate system to the east-north-up coordinate system based on the current discrete state of the navigation system; determining the GNSS observation position and velocity of the GNSS antenna in the east-north-up coordinate system according to the rotation matrix, and constructing an observation model; iteratively updating the discrete state of the navigation system according to the standard process of the iterative Kalman filtering, combining the observation model, and determining the optimal state of the navigation system at the current time.

2. The GNSS and INS semi-tight integrated navigation method of claim 1, wherein, system initialization of the navigation system, comprising: converting the GNSS timestamp of the navigation system from GPS time to local system time, and considering the receiver solution delay.

3. The GNSS and INS semi-tight integrated navigation method of claim 1, wherein, system initialization of the navigation system, comprising: recording the geocentric geodetic coordinate system position of the first frame of GNSS as the initial coordinates of the east-north-up coordinate system origin; reading the inertial measurement unit data and combining the magnetometer to determine the three-direction base of the east-north-up coordinate system; estimating the gravity direction of the body coordinate system by the sliding window average method, and determining the state of the navigation system in the east-north-up coordinate system at the initial time.

4. The GNSS and INS semi-tight integrated navigation method of claim 3, wherein, reading the inertial measurement unit data and combining the magnetometer to determine the three-direction base of the east-north-up coordinate system, comprising: reading the inertial measurement unit and magnetometer data, projecting all magnetic vectors to the horizontal plane, and then weighting and averaging to obtain the horizontal component of the geomagnetic field; combining the initial heading angle and the GNSS anchor point to determine the three-direction base of the east-north-up coordinate system. estimating the gravity direction of the body coordinate system by the sliding window average method, and determining the state of the navigation system in the east-north-up coordinate system at the initial time, comprising:

5. The GNSS and INS semi-tight integrated navigation method of claim 3, wherein, determining the zero offset of the accelerometer and gyroscope by the sliding window average method, and estimating the gravity direction of the body coordinate system; defining the gravity direction in the east-north-up coordinate system, and obtaining the rotation matrix through singular value decomposition; determining the state of the navigation system in the east-north-up coordinate system at the initial time based on the rotation matrix. ​ 6. The GNSS and INS semi-tight integrated navigation method of claim 1, wherein, After system initialization of the navigation system, including: freezing the magnetometer input, only saving the gyroscope, accelerometer and GNSS data stream.

7. A GNSS and INS semi-tight navigation device for implementing the GNSS and INS semi-tight navigation method according to any one of claims 1 to 6, characterized in that, Including: An initialization module, configured to perform system initialization on the navigation system; The navigation system is a GNSS and INS semi-tight combined navigation system; A prediction module, configured to predict the discrete state of the navigation system to obtain a discrete state; An update module, configured to 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 time; A conversion module, configured to convert the current station-centered coordinates into the geocentric geodetic coordinate system according to the anchor point coordinates of the iterative update convergence.

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 implements the GNSS and INS semi-tight combined navigation method according to any one of claims 1 to 6 when executing the program.

Images