GNSS (Global Navigation Satellite System) signal quality adaptive integrated navigation method

Abstract

The invention discloses a GNSS (Global Navigation Satellite System) signal quality adaptive integrated navigation method, and relates to the technical field of vehicle navigation. IMU and GNSS positioning information and positioning states are read, integrated navigation is realized through an ESKF loose combination algorithm, a measurement covariance matrix is changed in real time according to the GNSS positioning state and a DOP factor, and finally, an error state value is estimated and fed back to an integrated navigation system. Position and speed information errors and attitude Euler angle errors under a navigation coordinate system are fed back to INS mechanical arrangement, angular acceleration zero offset errors of the IMU are fed back to the IMU, and updating of the state quantity is completed. A measurement covariance matrix is changed in real time according to a GNSS positioning state and a DOP factor by aiming at IMU and GNSS data through a loosely combined ESKF fusion algorithm, an error state is estimated and fed back to an integrated navigation system, and the positioning precision can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle navigation, and specifically, to a GNSS signal quality adaptive integrated navigation method. Background Art

[0002] The wide use of the Global Navigation Satellite System (GNSS) provides real-time positioning and navigation services in the vehicle field. To achieve accurate positioning, a GNSS receiver needs to receive signals from at least four satellites. These four satellites should be distributed to cover different azimuths as much as possible to ensure high positioning accuracy and reliability. The quality of GNSS signals may be affected by various factors, such as the natural environment, the geometric arrangement of satellites, electromagnetic field interference, etc. Different positioning modes of GNSS directly affect the positioning accuracy. Among them: Single Point Positioning (SPS) is the most basic GNSS positioning mode, which only uses the signals from GNSS satellites for positioning, and the accuracy is relatively low, usually within a few meters; Real-Time Kinematic (RTK) technology can provide centimeter-level positioning accuracy; Differential Global Positioning System (DGPS) corrects the errors of satellite signals by using the known position information of one or more ground reference stations.

[0003] An Inertial Measurement Unit (IMU) consists of an accelerometer and a gyroscope, and is used to measure and report the physical properties of a vehicle in real time, including speed, direction, acceleration, etc. In the strapdown inertial navigation of a vehicle, dead reckoning (DR) can be performed based on the raw data of the IMU. The basic idea is to integrate the acceleration information of the IMU to obtain speed information, and then integrate the speed data again to estimate the displacement. The direction of the object is corrected or updated through the gyroscope data to improve the accuracy of displacement calculation. However, due to the error accumulation of the IMU, small measurement errors are amplified as time accumulates, affecting the positioning accuracy. At the same time, the raw data of the IMU will be affected by factors such as zero bias and scale factor, and an appropriate calibration and compensation mechanism is required to ensure the accuracy of the data.

[0004] Currently, for the loose integration fusion strategy of the IMU and GNSS, under the condition that the initial pose is known, the IMU and GNSS data are processed separately, and the calculation results of the two are fused. Prediction is performed through the dead reckoning of the IMU to calculate the navigation solution. The measurement of the GNSS is used as the observation value, and they are combined in the form of an error state Kalman filter to improve the overall positioning accuracy of the system. However, in actual applications, the positioning accuracy of the GNSS will be affected by the positioning mode and the geometric position of the satellites, thus increasing the noise of the observation quantity in the fusion algorithm. In unreliable GNSS positioning information, the integrated navigation system will be affected. The current navigation strategy usually ignores this influence and ignores the change of the GNSS positioning mode in actual applications, indirectly introducing noise to the integrated navigation system, resulting in a decrease in accuracy. Summary of the Invention

[0005] To address the deficiencies in the background art, the present invention provides an adaptive integrated navigation method for GNSS signal quality. For IMU and GNSS data, it uses a loosely coupled ESKF fusion algorithm to dynamically change the measurement covariance matrix based on the GNSS positioning status and DOP factor, estimates the error state, and feeds it back to the integrated navigation system, effectively improving the positioning accuracy.

[0006] To achieve the above object, the present invention adopts the following technical solutions: An adaptive integrated navigation method for GNSS signal quality, comprising the following steps:

[0007] Step 1: Read the IMU and GNSS positioning information and positioning status

[0008] The raw data reading of the IMU includes triaxial acceleration, triaxial angular velocity, and timestamp information. The GNSS connects to the Cors base station through a 4G module to achieve the RTK positioning mode. After convergence, it obtains the positioning mode, dilution of precision (DOP), position, and timestamp information. Among them, the positioning mode information includes RTK fixed solution, RTK floating solution, DGPS, and GPS, and the position information includes longitude, latitude, and altitude. The information data of the IMU and GNSS are saved by a single-chip microcomputer through an SD card for subsequent offline data processing;

[0009] Step 2: Implement integrated navigation using the ESKF loose coupling algorithm

[0010] Design an error state Kalman filtering method. Use the north-east-down coordinate system as the navigation coordinate system and the front-right-down as the vehicle coordinate system. Define the error state vector model as follows:

[0011]

[0012] where δr n is the position information error in the navigation coordinate system, δv n is the velocity information error in the navigation coordinate system, is the attitude Euler angle error, δb g is the angular velocity zero bias error of the IMU, δb a is the acceleration zero bias error of the IMU;

[0013] Define the rotation matrix from the vehicle coordinate system to the navigation coordinate system as follows:

[0014]

[0015] where represents the estimated value of the calculated rotation matrix, φ is the roll angle, γ is the pitch angle, and η is the heading angle;

[0016] INS mechanical arrangement updates the state from time k-1 to time k, where:

[0017] Attitude update, the equation is as follows:

[0018]

[0019] In the formula, θ k-1 represents the attitude Euler angle at time k-1, represents the three-axis angular velocity information measured by the IMU at time k, represents the estimated value of the rotation matrix at time k-1, and respectively represent the projection components of the earth's angular velocity of rotation and the transport angular velocity in the navigation coordinate system at the current geographical location, and dt represents the sampling time interval;

[0020] Velocity update, the equation is as follows:

[0021]

[0022] In the formula, represents the velocity information in the navigation coordinate system at time k-1, represents the projection component of the velocity increment from time k-1 to time k in the body coordinate system, and g represents the acceleration due to gravity at the current geographical location;

[0023] Position update, the equation is as follows:

[0024] h k = h k-1 - V D * dt

[0025]

[0026] In the formula, h k-1 represents the altitude at time k-1, represents the latitude at time k-1, λ k-1 represents the longitude at time k-1, V D represents the ground speed, V N represents the northward speed, V E represents the eastward speed, R M and R N respectively represent the radius values of the earth's prime vertical and meridian;

[0027] Model the error state vector of the navigation coordinate system, and the attitude error model is established as the Fai angle model, which is expressed as follows:

[0028]

[0029] In the formula, and represent the estimated position information and the estimated velocity information in the navigation coordinate system respectively, and r n and v n represent the position information and the velocity information in the navigation coordinate system calculated by the INS mechanical arrangement respectively. I represents the identity matrix, and ψ× represents the skew-symmetric matrix formed by the deviation angle between the true Euler angle and the calculated Euler angle. represents the true value of the rotation matrix;

[0030] Considering the zero bias and white noise of the IMU, the error models of the IMU accelerometer and gyroscope are expressed as follows:

[0031]

[0032] where

[0033]

[0034] In the formula, and represent the errors of the gyroscope and the accelerometer in the body coordinate system output by the IMU respectively. b g and b a are modeled as first-order Gaussian Markov models. ω g and ω a represent the white noise of the IMU. T gb and T ab are the time constants of the model. ω gb and ω ab are the biases of the model;

[0035] The state transition matrix F is obtained by forming a state propagation model through the error state. k is as follows:

[0036]

[0037] In the formula, represents the skew-symmetric matrix formed by the projection components of the specific force output by the IMU at time k in the navigation coordinate system. represents the skew-symmetric matrix formed by the earth's angular velocity of rotation and the transport angular velocity. F rr 、F vr 、F vv 、F ψr 、F ψv are as follows respectively:

[0038]

[0039]

[0040] In the formula, ωe represents the constant of the Earth's rotation speed;

[0041] The state prediction equation of the integrated navigation system is as follows:

[0042]

[0043] where,

[0044]

[0045] In the formula, Φ k-1 represents the discrete state transition matrix at time k - 1, F k-1 represents the continuous state transition matrix at time k - 1, Q k-1 represents the IMU noise matrix at time k - 1, G k-1 represents the IMU noise driving matrix, q represents the noise covariance matrix of the IMU measurement, x k / k-1 represents the error state vector at time k predicted at time k - 1, P k / k-1 represents the state covariance matrix at time k predicted at time k - 1;

[0046] Step 3: Real-time change the measurement covariance matrix R according to the GNSS positioning state and DOP factor

[0047] Read the positioning mode information of GNSS at each moment. According to different positioning mode information, initialize the value of the measurement covariance matrix according to the preset value, and calculate the measurement covariance matrix at time k according to the HDOP factor and VDOP factor, which is expressed as follows:

[0048]

[0049] In the formula, R 0 represents the value of the initial measurement covariance matrix;

[0050] The position information of the navigation coordinate system output by GNSS is used as the observation value of the ESKF algorithm. The observation equation of the integrated navigation system is expressed as follows:

[0051]

[0052] In the formula, represents the estimated value of the position information in the navigation coordinate system after compensating for the lever arm effect, l b represents the vector of the spatial positions of GNSS and IMU, z k represents the deviation vector between the predicted position information in the navigation coordinate system and the position information measured by GNSS,

[0053] represents the position vector information of the navigation coordinate system measured by GNSS, Hk represents the observation matrix of the integrated navigation system, D R represents the conversion matrix from longitude and latitude to meters, and is expressed as follows:

[0054]

[0055] Construct the Kalman filter equation to estimate the error state vector model x and the state covariance matrix P k as follows:

[0056] x k = x k / k-1 + K k (z k - H k x k / k-1 )

[0057]

[0058]

[0059] In the formula, K k represents the Kalman gain at time k;

[0060] Step 4: Estimate the value of the error state and feedback it to the integrated navigation system

[0061] Among the error state vectors calculated at time k, the position information error δr n and the velocity information error δv n in the navigation coordinate system, as well as the attitude Euler angle error are combined to form the navigation error and fed back to the INS mechanical arrangement. The angular velocity zero bias error δb g and the acceleration zero bias error δb a of the IMU are combined to form the IMU error and fed back to the IMU itself to complete the update of the state variables. Finally, the error state vector is cleared.

[0062] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention first obtains the original data of the IMU and GNSS, uses the loose-coupled ESKF fusion algorithm, combines the dilution of precision DOP, calculates the measurement covariance matrix of the ESKF filter according to the GNSS positioning quality, and feeds the calculated error state vector back to the integrated navigation system, realizing adaptive integrated navigation through the real-time changing GNSS signal quality, and has the following characteristics:

[0063] 1. Only the position measurement information of GNSS is required, without relying on additional GNSS measurement information;

[0064] 2. Applicable to different GNSS positioning systems, not only relying on the RTK positioning implemented by the Cors base station;

[0065] 3. Considering the relationship between satellite geometric position and positioning accuracy, it is possible to reduce the impact of GNSS positioning information with large errors on the system, indirectly improving the positioning accuracy.

[0066] 4. It can be switched according to different positioning modes, adaptively increasing or decreasing the fusion weight of GNSS measurement values.

[0067] 5. It can be implemented based on an embedded system to provide continuous positioning status. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 is a flowchart of the method of the present invention;

[0069] Figure 2 is a system block diagram of the method of the present invention;

[0070] Figure 3 is a schematic diagram of the positioning effect of the extended Kalman filter method and GNSS positioning points in the embodiment;

[0071] Figure 4 is a schematic diagram of the positioning effect of the method of the present invention and GNSS positioning points in the embodiment;

[0072] Figure 5 is Figure 3 a partial enlarged view of;

[0073] Figure 6 is Figure 4 a partial enlarged view of. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0074] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0075] As Figures 1 to 2 shown, a GNSS signal quality adaptive integrated navigation method includes the following steps:

[0076] Step 1: Read IMU and GNSS positioning information and positioning status

[0077] The original data of the IMU is read at a frequency of 100 Hz, including triaxial acceleration (specific force), triaxial angular velocity, and timestamp information.

[0078] The GNSS connects to the Cors base station via a 4G module using the Ntrip protocol. The 4G module sends the $GGA message in the NMEA0183 protocol to the Cors base station, and the Cors base station returns the RTCM differential data and forwards it to the GNSS receiver module to achieve the RTK positioning mode. After it converges, the NMEA0183 message in the GNSS receiver module is acquired at a frequency of 1 Hz, including the positioning mode, dilution of precision (DOP), position, and timestamp information. Among them, the positioning mode information includes RTK fixed solution, RTK float solution, DGPS, and GPS, and the position information includes longitude, latitude, and altitude.

[0079] The microcontroller saves the information data of the IMU and GNSS via the SD card and performs subsequent off-line data processing.

[0080] Step 2: The ESKF loose integration algorithm realizes integrated navigation

[0081] Design an error-state Kalman filtering method. It uses the north-east-down coordinate system as the navigation coordinate system (Navi system) and the front-right-down as the vehicle coordinate system (Body system). The error-state vector model is defined as follows:

[0082]

[0083] In the formula, δr n is the position information error (longitude, latitude, altitude) in the navigation coordinate system, δv n is the velocity information error in the navigation coordinate system, is the attitude Euler angle error, δb g is the angular velocity zero bias error of the IMU, δb a is the acceleration zero bias error of the IMU.

[0084] Define the rotation matrix from the vehicle coordinate system to the navigation coordinate system as follows:

[0085]

[0086] In the formula, represents the estimated value of the calculated rotation matrix, φ is the roll angle, γ is the pitch angle, and η is the heading angle.

[0087] The INS mechanical alignment represents the update of the state from the (k - 1)th moment to the kth moment, where:

[0088] In the attitude update of the integrated navigation system, according to the angular velocity increment information output by the IMU, after deducting the earth's angular velocity of the current geographical location, the attitude update equation is expressed as follows:

[0089]

[0090] where θ k-1 represents the attitude Euler angles at time k-1, represents the triaxial angular velocity information measured by the IMU at time k, represents the estimated rotation matrix at time k-1, and respectively represent the projection components of the earth's angular velocity of rotation and the transport angular velocity at the current geographical location in the navigation coordinate system, and dt represents the sampling time interval.

[0091] In the velocity update of the integrated navigation system, considering the gravitational acceleration at the current geographical location, whose value changes with the geographical location, the current acceleration information is output by the IMU accelerometer, and then the influence of the gravity component and the Coriolis force is deducted to obtain the velocity update equation as follows:

[0092]

[0093] where represents the velocity information in the navigation coordinate system at time k-1, represents the projection component of the velocity increment from time k-1 to time k in the body coordinate system, and g represents the gravitational acceleration at the current geographical location.

[0094] In the position update of the integrated navigation system, according to the velocity information of the navigation coordinate system calculated at the current time, the longitude and latitude increments in the geodetic coordinate system are obtained, and the longitude and latitude of the previous time are added to the longitude and latitude increments to obtain the position update equation as follows:

[0095] h k = h k-1 - V D * dt

[0096]

[0097] where h k-1 represents the altitude (m) at time k-1, represents the latitude (rad) at time k-1, λ k-1 represents the longitude (rad) at time k-1, V D represents the ground speed, V N represents the northward speed, V E represents the eastward speed, R M and R N respectively represent the radii of the earth's prime vertical and meridian.

[0098] Model the error state vector of the navigation coordinate system, and the attitude error model is established as the Fai angle model, which is expressed as follows:

[0099]

[0100] In the formula, and respectively represent the estimated position information and estimated velocity information in the navigation coordinate system. r n and v n respectively represent the position information and velocity information in the navigation coordinate system calculated by the INS mechanical alignment. I represents the identity matrix, and ψ× represents the skew-symmetric matrix formed by the deviation angle between the true Euler angle and the calculated Euler angle. represents the true value of the rotation matrix.

[0101] Considering the zero bias and white noise of the IMU, the error models of the IMU accelerometer and gyroscope are expressed as follows:

[0102]

[0103] Wherein,

[0104]

[0105] In the formula, and respectively represent the errors of the gyroscope and accelerometer in the body coordinate system output by the IMU. b g and b a are modeled as first-order Gaussian Markov models. ω g and ω a represent the white noise of the IMU. T gb and T ab are the time constants of the model. ω gb and ω ab are the biases of the model.

[0106] By forming a state propagation model with the error states, the obtained state transition matrix F k is as follows:

[0107]

[0108] In the formula, represents the skew-symmetric matrix formed by the projection components of the specific force output by the IMU at time k in the navigation coordinate system. represents the skew-symmetric matrix formed by the earth's angular velocity of rotation and the transport angular velocity. F rr 、F vr 、F vv 、F ψr 、F ψv are respectively as follows:

[0109]

[0110]

[0111] In the formula, ω e represents the constant of the Earth's rotation speed. The state prediction equation of the integrated navigation system is as follows:

[0112]

[0113] Wherein,

[0114]

[0115] In the formula, Φ k-1 represents the discrete state transition matrix at time k - 1, F k-1 represents the continuous state transition matrix at time k - 1, Q k-1 represents the IMU noise matrix at time k - 1, G k-1 represents the IMU noise drive matrix, q represents the noise covariance matrix of the IMU measurement, x k / k-1 represents the error state vector at time k predicted at time k - 1, P k / k-1 represents the state covariance matrix at time k predicted at time k - 1.

[0116] Step 3: Modify the measurement covariance matrix R in real time according to the GNSS positioning state and DOP factor

[0117] Read the positioning mode information of GNSS at each moment. Different positioning mode information directly determines the positioning accuracy. Among them, the RTK fixed solution is the positioning state with the highest accuracy, followed by the RTK float solution mode, DGPS, and SPS. According to different positioning mode information, initialize the value of the measurement covariance matrix R according to the preset value. On this basis, since the magnitude of the HDOP factor reflects the sum of the squares of the errors in the horizontal longitude and latitude, it is expressed as follows:

[0118]

[0119] In the formula, σ la represents the standard deviation of latitude, σ lo represents the standard deviation of longitude.

[0120] The standard deviations of longitude and latitude directly reflect the positioning accuracy. Therefore, the HDOP factor indirectly reflects the horizontal positioning accuracy. Similarly, the VDOP factor indirectly reflects the elevation positioning accuracy.

[0121] Calculate the measurement covariance matrix at time k according to the HDOP factor and the VDOP factor, which is expressed as follows:

[0122]

[0123] In the formula, R 0Represents the value of the initial measurement covariance matrix.

[0124] The resulting R k The value changes accordingly in real time according to the positioning mode information of GNSS and the DOP factor.

[0125] Considering the "lever arm effect" of the GNSS antenna and IMU installation positions, that is, the deviation in spatial position caused by the installation of GNSS and IMU. Therefore, during installation, the spatial positions of GNSS and IMU are measured as the external parameters of the integrated navigation system. The three-dimensional position information of the navigation coordinate system output by GNSS is used as the observation value of the ESKF algorithm. The observation equation of the integrated navigation system is expressed as follows:

[0126]

[0127] In the formula, Represents the estimated value of the position information in the navigation coordinate system after compensating for the lever arm effect, l b Represents the vector of the spatial positions of GNSS and IMU, z k Represents the deviation vector between the predicted position information and the GNSS-measured position information in the navigation coordinate system, Represents the position vector information of the navigation coordinate system measured by GNSS, H k Represents the observation matrix of the integrated navigation system, D R Represents the conversion matrix from longitude and latitude to meters, which is expressed as follows:

[0128]

[0129] Construct the Kalman filter equation to estimate the error state vector model x and the state covariance matrix P k which is expressed as follows:

[0130] x k = x k / k-1 + K k (z k - H k x k / k-1 )

[0131]

[0132] In the formula, K k Represents the Kalman gain at time k.

[0133] Step 4: Estimate the value of the error state and feedback it to the integrated navigation system

[0134] Among the error state vectors calculated at time k, the position information error δr n and the velocity information error δv nand the attitude Euler angle error are combined into the navigation error and fed back to the INS mechanical alignment. The angular velocity bias error δb of the IMU g and the acceleration bias error δb a are combined into the IMU error and fed back to the IMU itself to complete the update of the state variables, and finally the error state vector is cleared.

[0135] Embodiment

[0136] Read the raw data information from the IMU and GNSS: The IMU module model is selected as WheelTec-N100, and it communicates with the single-chip microcomputer through the form of CAN bus. The single-chip microcomputer reads the specific force and angular velocity information of the IMU at a frequency of 100 Hz; the GNSS module is selected as the TAU1308 positioning module, and at the same time, it is connected to the Cors base station through the 4G module to achieve RTK positioning, communicates with the single-chip microcomputer through the serial port, and reads the position, DOP factor and positioning status information of the GNSS at a frequency of 1 Hz. Store the raw data information of the IMU module and GNSS module in the SD card for subsequent off-line data processing.

[0137] The ESKF loose integration algorithm realizes integrated navigation: The collected IMU raw data is processed offline. At each moment, INS mechanical alignment is performed to recursively obtain the attitude, velocity, and position information of the integrated navigation system; at the same time, the state transition matrix F is obtained according to the state propagation model.

[0138] The measurement covariance matrix R is updated in real time according to the GNSS positioning status and DOP factor: The GNSS information obtains the initial measurement covariance matrix R according to different positioning mode information 0 and, according to the DOP factor at each moment, calculates the measurement covariance matrix R in real time according to the proposed method for calculating the R matrix k .

[0139] The initial measurement covariance matrix R set in this embodiment 0 is shown in Table 1:

[0140] Positioning mode <![CDATA[R 0 [1,1]]]> <![CDATA[R 0 [2,2]]]> <![CDATA[R 0 [3,3]]]> RTK fixed solution 0.0004 0.0004 0.0025 RTK float solution 0.09 0.09 0.09 DGPS 1 1 3 SPS 5 5 9

[0141] Construct the Kalman filter equation to estimate the error state vector x and the state covariance matrix P k for estimation.

[0142] Estimate the value of the error state and feed it back to the integrated navigation system: The integrated navigation system error (including position information error, velocity information error, and attitude error) calculated according to the ESKF method is fed back to the INS mechanical alignment, and the calculated IMU error (including the angular velocity bias and acceleration bias of the IMU) is fed back to the error compensation of the IMU.

[0143] The operating-related parameters of the design and the filter parameters are as follows:

[0144] The spatial position vector l of the IMU relative to the GNSS module b = [-0.073, 0.302, 0.087];

[0145] The initial north-east-down velocity of the integrated navigation system

[0146] The initial position r of the integrated navigation system 0 = [45.73073055, 126.6283149, 23];

[0147] The initial Euler angles θ of the integrated navigation system 0 = [0.02255, 0.02333, 3.12367];

[0148] IMU module parameters:

[0149]

[0150] In this embodiment, the effectiveness of the method of the present invention is verified under different GNSS positioning environments. By comparing the extended Kalman filter method and the method of the present invention through comparative experiments, the positioning effects of the two are combined with the GNSS positioning points respectively Figure 3 and Figure 4 As shown, the significant positions are magnified and shown respectively in combination with Figure 5 and Figure 6 As shown, it can be seen that the method of the present invention can effectively make rapid adjustments according to the change of GNSS positioning quality, and the positioning accuracy will not be overly affected by bad GNSS positioning signals, verifying the superiority of the method of the present invention.

[0151] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above exemplary embodiments, and can be implemented in other forms without departing from the spirit or basic characteristics of the present invention. Therefore, from any point of view, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, all changes falling within the meaning and scope of the equivalent conditions of the claims are intended to be included in the present invention. Any reference signs in the claims should not be regarded as limiting the claimed rights.

[0152] In addition, it should be understood that although this specification is described according to embodiments, not every embodiment only contains an independent technical solution. This narrative way of the specification is only for clarity. Those skilled in the art should regard the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A GNSS signal quality adaptive integrated navigation method, characterized by: The following steps are involved: Step 1: Read IMU and GNSS positioning information and positioning status The raw data read from IMU includes three-axis acceleration, three-axis angular velocity and timestamp information. GNSS connects to the Cors base station through the 4G module to realize RTK positioning mode. After convergence, the positioning mode, positioning precision factor DOP, position and timestamp information are obtained. Among them, the positioning mode information includes RTK fixed solution, RTK floating point solution, DGPS and GPS, and the position information includes longitude, latitude and altitude. The information data of IMU and GNSS are saved through the SD card through the single-chip microcomputer, and the subsequent data offline processing is carried out; Step 2: ESKF loose combination algorithm to achieve integrated navigation The error state Kalman filter method is designed, using the north-east-ground coordinate system as the navigation coordinate system and the front-right-bottom as the carrier coordinate system. The error state vector model is defined as follows: In the formula, δr n is the position information error in the navigation coordinate system, δv n is the velocity information error in the navigation coordinate system, is the attitude Euler angle error, δb g is the angular velocity bias error of the IMU, δb a is the acceleration bias error of IMU; The rotation matrix from the carrier coordinate system to the navigation coordinate system is defined as follows: In the formula, represents the calculated rotation matrix estimate, φ is the roll angle, γ is the pitch angle, and η is the heading angle; The INS mechanically arranges the state update from time k-1 to time k, where: The posture update equation is expressed as follows: In the formula, θ k-1 represents the Euler angle of the posture at time k-1, Represents the three-axis angular velocity information measured by the IMU at time k, represents the estimated value of the rotation matrix at time k-1, and They represent the projection components of the earth's rotation angular velocity and the angular velocity of the current geographical location in the navigation coordinate system, and dt represents the sampling time interval; Velocity update, the equation is expressed as follows: In the formula, Represents the speed information in the navigation coordinate system at time k-1, It represents the projection component of the velocity increment from time k-1 to time k in the carrier coordinate system, and g represents the gravitational acceleration at the current geographic location; Position update, the equation is expressed as follows: h k =h k-1 -V D *dt In the formula, h k-1 represents the height at time k-1, represents the latitude at time k-1, λ k-1 represents the longitude at time k-1, V D represents the ground velocity, V N Indicates the north velocity, V E represents the eastward speed, R M and R N They represent the radius of the earth's meridian circle and meridian circle respectively; The error state vector of the navigation coordinate system is modeled, and the attitude error model is established as the Fai angle model, which is expressed as follows: In the formula, and They represent the estimated value of position information and the estimated value of velocity information in the navigation coordinate system, r n and v n They represent the position information and velocity information in the navigation coordinate system calculated by the INS mechanical arrangement, I represents the unit matrix, ψ× represents the antisymmetric matrix composed of the deviation angle between the real Euler angle and the calculated Euler angle, Represents the true value of the rotation matrix; Taking into account the zero bias and white noise of the IMU, the error model of the IMU accelerometer and angular velocity meter is expressed as follows: in, In the formula, and They represent the errors of the angular velocity meter and accelerometer in the carrier coordinate system output by the IMU, and b g and b a Modeled as a first-order Gaussian Markov model, ω g and ω a represents the white noise of IMU, T gb and T ab is the time constant of the model, ω gb and ω ab is the bias of the model; The state propagation model is formed by the error state, and the state transfer matrix F is obtained. k as follows: In the formula, Represents the antisymmetric matrix composed of the projection components of the specific force output by the IMU at time k in the navigation coordinate system, It represents the antisymmetric matrix composed of the earth's rotation angular velocity and the associated angular velocity, F rr 、F vr 、F vv 、F ψr 、F ψv They are as follows: In the formula, ω e Represents the earth's rotation speed constant; the state prediction equation of the integrated navigation system is as follows: in, In the formula, Φ k-1 represents the discrete state transfer matrix at time k-1, F k-1 represents the continuous state transfer matrix at time k-1, Q k-1 represents the IMU noise matrix at time k-1, G k-1 represents the IMU noise driving matrix, q represents the noise covariance matrix measured by the IMU, and x k / k-1 represents the error state vector at time k predicted at time k-1, P k / k-1 Represents the state covariance matrix at time k predicted at time k-1; Step 3: Change the measurement covariance matrix R in real time according to the GNSS positioning status and DOP factor Read the GNSS positioning mode information at each moment, initialize the value of the measurement covariance matrix according to the preset value according to the different positioning mode information, and calculate the measurement covariance matrix at moment k according to the HDOP factor and VDOP factor, as shown below: Where R0 represents the value of the initial measurement covariance matrix; The position information of the navigation coordinate system output by GNSS is used as the observation value of the ESKF algorithm. The observation equation of the integrated navigation system is expressed as follows: In the formula, represents the estimated value of the position information in the navigation coordinate system after compensating for the lever arm effect, l b Vector representing the spatial position of GNSS and IMU, z k Represents the deviation vector between the predicted position information in the navigation coordinate system and the position information measured by GNSS, Represents the position vector information in the navigation coordinate system measured by GNSS, H k Denotes the observation matrix of the integrated navigation system, D R The conversion matrix representing the conversion of longitude and latitude to meters is as follows: Construct the Kalman filter equation, and model the error state vector x and the state covariance matrix P k The estimation is expressed as follows: x k =x k / k-1 +K k (z k -H k x k / k-1 ) In the formula, K k represents the Kalman gain at time k; Step 4: Estimate the value of the error state and feed it back to the integrated navigation system In the error state vector calculated at time k, the position information error δr in the navigation coordinate system is n and velocity information error δv n And attitude Euler angle error The navigation error is fed back to the INS mechanical arrangement, and the angular velocity zero bias error δb of the IMU g and acceleration bias error δb a The IMU error is fed back to the IMU itself to complete the update of the state quantity, and finally the error state vector is cleared.

Images