Integrated navigation method and system of unmanned surface vehicle

Abstract

The invention discloses a combined navigation method and system of an unmanned surface vehicle. The method comprises the following steps: forming a GNSS (Global Navigation Satellite System) / INS (Inertial Navigation System) integrated navigation system by using a GNSS (Global Navigation Satellite System) and a strapdown inertial navigation system (INS), and establishing a mathematical model of a GNSS / INS integrated navigation system filter; the mathematical model comprises a state equation and a measurement equation; establishing a mathematical model of the influence of the GNSS auxiliary information on the GNSS positioning precision so as to preliminarily adjust the measurement noise in the measurement equation; through an improved Sage-Husa adaptive filtering algorithm, adaptive estimation is carried out on the preliminarily adjusted measurement noise, and a measurement noise estimation final value is obtained; and according to the motion characteristics of the unmanned surface vehicle, adding motion constraint conditions to the measurement equation. The GNSS auxiliary information mathematical model based on multivariate function fitting is established, the influence of the external environment on the positioning precision is objectively reflected, the measurement noise in the filter is dynamically adjusted, and the positioning precision of the integrated navigation system under the interference condition is remarkably improved.

Description

Technical Field

[0001] The present invention relates to the field of vehicle technology, and in particular to a combined navigation method and system for an unmanned surface vehicle. Background Art

[0002] With the continuous development of military combat methods and intelligent unmanned systems, unmanned surface vehicles (USVs) have become an important direction for future combat equipment due to their advantages such as small size, maneuverability, and good concealment. Currently, unmanned surface vehicles (USVs) mainly use integrated GNSS and INS navigation. By leveraging the long-term absolute positioning of GNSS and the short-term high-precision of INS, dynamic and precise positioning of the vehicle is achieved. However, due to external environmental interference and inaccurate description of the statistical characteristics of model noise, the traditional error state Kalman filter (ESKF) method is prone to filtering instability and even divergence in practical applications. Improved filtering methods are urgently needed to improve navigation accuracy and reliability. Summary of the Invention

[0003] An embodiment of the present invention provides a combined navigation method for an unmanned surface vehicle to solve the problems of low navigation accuracy and low reliability in the prior art.

[0004] To achieve the above-mentioned objectives, on the one hand, the present invention provides an integrated navigation method for an unmanned surface vehicle, the method comprising: S1, combining a GNSS positioning system (GNSS) and a strapdown inertial navigation system (INS) into a GNSS / INS integrated navigation system, and establishing a mathematical model of a GNSS / INS integrated navigation system filter; the mathematical model comprising: a state equation and a measurement equation; S2, establishing a mathematical model of the influence of GNSS auxiliary information on GNSS positioning accuracy, so as to perform a preliminary adjustment on the measurement noise in the measurement equation; S3, adaptively estimating the measurement noise after the preliminary adjustment through an improved Sage-Husa adaptive filtering algorithm, and obtaining a final value of the measurement noise estimation; S4, adding motion constraints to the measurement equation according to the motion characteristics of the unmanned surface vehicle.

[0005] Optionally, the state equation is established based on INS error; and the measurement equation is established based on the speed difference and position difference between GNSS and INS.

[0006] Optionally, the state equation is: k =Φ k,k-1 X k-1 +Γ k-1 W k-1 The measurement equation is: k =H k X k +V k Among them, X kis the system state vector, Φ k,k-1 is the state transition matrix, W k-1 is the system noise vector, Γ k-1 is the system noise matrix, Z k is the measurement vector, H k is the measurement matrix, V k is the measurement noise vector.

[0007] Optionally, the state equation and the measurement equation are solved by a filtering equation.

[0008] Optionally, the S2 comprises: placing the GNSS navigation receiving module on a known point position with accurately calibrated geodetic coordinates in an open land, effectively shielding the GNSS navigation receiving module in different directions and different areas by electromagnetic shielding material, and obtaining the GNSS system positioning accuracy under different satellite numbers and satellite space geometric factors; selecting a polynomial or other function as a base function of a fitting function by a least square method, and establishing a multivariate function mathematical model of the influence of GNSS auxiliary information on GNSS positioning accuracy; the GNSS auxiliary information comprises: satellite number and satellite space geometric factor; calculating an adjustment coefficient of the measurement noise according to the multivariate function mathematical model, and updating the measurement noise in the measurement equation according to the adjustment coefficient of the measurement noise.

[0009] Optionally, the S3 comprises: dynamically estimating the measurement noise by using an innovation vector, and updating the measurement noise preliminarily adjusted by an exponential fading memory weighted average method; when a sudden change occurs, modifying the measurement noise preliminarily adjusted according to the adjustment coefficient of the measurement noise as prior information; and adaptively estimating the measurement noise modified by an improved Sage-Husa adaptive filtering algorithm, to obtain a final value of the measurement noise estimation.

[0010] Optionally, the motion constraint condition is:

[0011]

[0012] wherein, is the true speed of the unmanned surface vehicle in the vertical direction of the carrier coordinate system;

[0013] In the carrier coordinate system, the speed error of the unmanned surface vehicle can be written as:

[0014]

[0015] wherein, n and b are the navigation coordinate system and the carrier coordinate system respectively: is the speed error in the carrier coordinate system, is the transpose matrix of the direction cosine matrix of the navigation coordinate system to the carrier coordinate system in the INS; V n = [vE v N v u ] T is the true velocity of the carrier in the navigation coordinate system; δV n =[δv E δv N δv U ] T is the velocity error in the navigation coordinate system; δA=[δφ E δφ N δφ U ] is the three-dimensional attitude error vector, i.e., the 7th to 9th elements of the system state vector of the GNSS / INS integrated navigation system filter;

[0016] A is the antisymmetric matrix of δA, that is:

[0017]

[0018] On the other hand, the present invention provides an integrated navigation system for an unmanned surface vehicle, which includes: a mathematical model establishment unit, which is used to combine a GNSS positioning system (GNSS) and a strapdown inertial navigation system (INS) into a GNSS / INS integrated navigation system, and establish a mathematical model of a GNSS / INS integrated navigation system filter; the mathematical model includes: a state equation and a measurement equation; an adjustment unit, which is used to establish a mathematical model of the influence of GNSS auxiliary information on GNSS positioning accuracy, so as to perform preliminary adjustment on the measurement noise in the measurement equation; an estimation unit, which is used to adaptively estimate the measurement noise after the preliminary adjustment through an improved Sage-Husa adaptive filtering algorithm to obtain a final value of the measurement noise estimation; and a constraint unit, which is used to add motion constraint conditions to the measurement equation according to the motion characteristics of the unmanned surface vehicle.

[0019] Beneficial effects of the present invention:

[0020] The present invention provides an integrated navigation method for unmanned surface vehicles. By establishing a mathematical model of GNSS auxiliary information based on multivariate function fitting, the influence of the external environment on positioning accuracy is objectively reflected, and the measurement noise in the filter is dynamically adjusted, thereby significantly improving the positioning accuracy of the integrated navigation system under interference conditions. When the GNSS signal is weak or lost, motion constraints are used to supplement the observation variables, ensuring continuous measurement updates of the filter and enhancing system stability. The improved Sage-Husa adaptive filtering algorithm is combined with prior information to quickly correct measurement noise under sudden change conditions, ensuring the rapid response and long-term stable operation of the filter. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1is a flow chart of a combined navigation method of an unmanned surface vehicle provided by an embodiment of the present application;

[0022] Figure 2 is a structural schematic diagram of a combined navigation system of an unmanned surface vehicle provided by an embodiment of the present application. DETAILED DESCRIPTION

[0023] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to the drawings. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art without creative work fall within the scope of protection of the present application.

[0024] Figure 1 is a combined navigation method of an unmanned surface vehicle provided by an embodiment of the present application, as shown in the figure, the method comprises: Figure 1

[0025] S1, a GNSS positioning system (GNSS) and a strapdown inertial navigation system (INS) are combined to form a GNSS / INS combined navigation system, and a mathematical model of a GNSS / INS combined navigation system filter is established; the mathematical model comprises: a state equation and a measurement equation;

[0026] In an optional embodiment, the state equation is established based on INS error; and the measurement equation is established based on a speed difference and a position difference between the GNSS and the INS.

[0027] The state equation is: X k =Φ k,k-i X k-1 +Γ k-i W k-i

[0028] The measurement equation is: Z k =H k X k +V k

[0029] Wherein, X k is a system state vector; Φ k,k-1 is a state transition matrix, used to describe the state evolution relationship of the system from time k-1 to time k, that is, to predict the change of the state based on the system dynamics model; W k-1 is a system noise vector, usually assumed to be zero mean, Gaussian white noise, reflecting random errors or unmodeled dynamic changes in the system model; Γ k-1 is a system noise matrix, which reflects the process noise W k-1 ​The state equation is introduced to reflect the influence of noise on state evolution; Z k is the measurement vector, H k is the measurement matrix, which is used to map the state vector to the observation space. Through this matrix, it can be seen which parts of the system state are associated with the actual measurement values; V k is the measurement noise vector, also assumed to be zero-mean Gaussian white noise, describing the measurement uncertainty introduced due to sensor errors, etc.

[0030] Further, X k represents the system state vector at time k in the GNSS / INS integrated navigation system filter, which is a 15-dimensional vector, including position error (error in longitude, latitude, height), velocity error (error in east, north, and sky direction), attitude angle error (attitude error in east, north, and sky direction), gyroscope zero offset error (drift error in 3 directions), accelerometer zero offset error (drift error in 3 directions).

[0031]

[0032] δL, δλ, and δh are the longitude, latitude, and height position errors, respectively; δv E δv N δv U are the velocity errors in east, north, and sky directions, respectively; δφ E δφ N δφ U are the attitude angle errors in east, north, and sky directions, respectively; ε bx ε by ε bz are the 3 gyroscope zero offset errors; are the 3 accelerometer zero offset errors.

[0033] Z k represents the difference between the position information and velocity information output by GNSS and INS (e.g., the difference in longitude, latitude, and height, and the corresponding velocity difference), i.e., it reflects the deviation of the measurement results of the two sets of navigation systems, which is a 6-dimensional vector.

[0034]

[0035] wherein, L G -L I represents the difference between the longitude measured by GNSS and the longitude calculated by INS; λ G -λ I represents the difference between the latitude measured by GNSS and the latitude calculated by INS; h G -h I represents the difference between the height measured by GNSS and the height calculated by INS; Indicates the difference in north velocity between GNSS and INS; represents the difference in eastward velocity between GNSS and INS; Indicates the difference in celestial velocity between GNSS and INS.

[0036] In an optional embodiment, the state equation and the measurement equation are solved by filtering equations.

[0037] The filtering equation includes:

[0038] Time update (prediction) phase:

[0039] At time k-1, the state estimate and covariance at time k are predicted based on the system model:

[0040] (1) Predicted state vector:

[0041] in, is the system state vector predicted from time k-1 to time k, which represents the state value predicted based on the system model before the introduction of measurement data; The updated state prediction is measured at time k-1, which is X k-1 Estimate of Φ k,k-1 is the state transfer matrix, which describes the change relationship of the system state from time k-1 to time k;

[0042] (2) Prediction covariance matrix:

[0043] Among them, P k / (k-1) is the state covariance matrix predicted from time k-1 to time k, indicating the variance of the prediction error; P k-1 is the state covariance matrix at time k-1, which represents the uncertainty obtained by the last filtering. is the estimate of the system noise covariance matrix, which represents the uncertainty of the system internal process noise; Φ k,k-1 is the state transfer matrix, and P k-1 Propagate to time k.

[0044] Measurement update (correction) phase:

[0045] When the new measurement value Z at time k is obtained k After that, the predicted state is corrected:

[0046] (3) Calculate the Kalman gain:

[0047] Among them, K k is the Kalman gain matrix, which controls the influence of new measurement data on state estimation; k / (k-1)is the prediction error covariance matrix, which represents the uncertainty of the current state prediction; H k is the measurement matrix, which is used to map the state variables to the measurement space; is the measurement noise covariance matrix, which represents the error variance of the measurement data (sensor noise); is the covariance of the measurement residuals, which represents the uncertainty of the measurement.

[0048] Gain matrix K k Function:

[0049] If P k / (k-1) Large (large system uncertainty), K k The larger the value, the more the filter depends on the new measurement data.

[0050] like Large (large measurement noise), K k The smaller the value, the lower the filter's trust in the measured data.

[0051] (4) Status update:

[0052] in, is the updated system state vector, which combines the predicted value and the measured value; Z k is the measurement value (from sensors such as GNSS); is the predicted measurement value, that is, the result of the system prediction state conversion into the measurement space; is the innovation vector, which represents the deviation between the actual measurement and the predicted measurement; is a correction term that adjusts the state estimate based on new information.

[0053] (5) Update the covariance matrix: P k =(IK k H k )P k / (k-1)

[0054] Among them, P k is the updated state covariance matrix, which represents the uncertainty of state estimation; IK k H k is the information update coefficient, which indicates that the uncertainty of the state is reduced after the state estimate is corrected.

[0055] Update the covariance matrix P k Function:

[0056] If K k is larger, indicating that the state estimation is highly dependent on the measurement value, then P k will decrease, and the filter will be more confident in the estimate.

[0057] If K kis smaller, indicating that the state estimation is more dependent on the predicted value, then P k Not much change.

[0058] S2. Establishing a mathematical model of the effect of GNSS auxiliary information on GNSS positioning accuracy to perform preliminary adjustments to the measurement noise in the measurement equation;

[0059] To improve the filtering accuracy of the GNSS / INS integrated navigation system, a mathematical model is needed to describe the impact of GNSS auxiliary information (number of satellites, satellite spatial geometry factor (pDOP value)) on GNSS positioning accuracy. This model can also be used to make preliminary adjustments to the measurement noise in the measurement equation.

[0060] In an optional embodiment, the S2 includes:

[0061] S21. Place the GNSS navigation receiving module at a known point with accurately calibrated geodetic coordinates in an open area, and effectively shield the GNSS navigation receiving module in different directions and areas using electromagnetic shielding materials to obtain GNSS system positioning accuracy under different satellite numbers and satellite spatial geometric factors.

[0062] Specifically, select an open area free from tall buildings and electromagnetic interference sources to ensure optimal GNSS signal reception. Arrange multiple reference points with known geodetic coordinates within the area.

[0063] Place a GNSS navigation receiver module at the reference point and record the GNSS positioning results. Use electromagnetic shielding materials to block the GNSS navigation receiver module in different directions and areas to artificially control the visibility of satellite signals. Under different shielding conditions, record the number of available satellites (n) and pDOP value of the GNSS navigation receiver module, and obtain its actual positioning error (R GNSS ).

[0064] Collect GNSS positioning error data under different satellite numbers and pDOP values ​​to form a data set {(n i ,pDOP i ,R i )}; Ensure that the data covers a wide range of conditions from good GNSS signals to severe obstruction.

[0065] S22. Selecting a polynomial or other function as a basis function of the fitting function using a least squares method to establish a multivariate function mathematical model of the effect of GNSS auxiliary information on GNSS positioning accuracy; the GNSS auxiliary information includes: the number of satellites and satellite spatial geometry factors;

[0066] In order to establish the GNSS auxiliary information (nnn, pDOP) and GNSS positioning error R GNSSThe mathematical relationship between them is adopted by regression analysis method, such as polynomial fitting or nonlinear regression.

[0067] (1) Select the fitting function

[0068] Since the GNSS positioning error is usually nonlinear, it is crucial to select the appropriate mathematical model. Common fitting methods include:

[0069] Polynomial fitting:

[0070] R GNSS = a0+ a1n+ a2pDOP+ a3n 2 +a4pDOP 2 +a5npDOP

[0071] Exponential function fitting:

[0072]

[0073] Neural network regression (for complex cases).

[0074] Finally, choose which fitting method, according to the distribution characteristics of the experimental data, through the least squares method (Least Squares Method, LSM) or gradient descent optimization algorithm, to calculate the optimal fitting parameters.

[0075] (2) Calculate the fitting parameters

[0076] Solve the fitting coefficients by least squares method:

[0077]

[0078] Where f(n i ,pDOP) is the selected fitting function.

[0079] The optimal function f(n i ,pDOP) obtained by calculation is the GNSS positioning error estimation model, which can be used to adaptively adjust the measurement noise covariance of the filter.

[0080] S23, according to the multivariate function mathematical model to calculate the adjustment coefficient of the measurement noise, and according to the adjustment coefficient of the measurement noise to update the measurement noise in the measurement equation.

[0081] In the GNSS / INS integrated navigation system filter, GNSS observation data (such as position, velocity) are usually used as the measurement value of the filter, and its noise covariance matrix R k Directly affects the filtering accuracy. Therefore, after establishing the mathematical model of GNSS auxiliary information, the predicted GNSS positioning error R GNSS Adaptively adjust the measurement noise covariance.

[0082] (1) Calculate the measurement noise adjustment coefficient

[0083] Let R GNSS be the positioning error estimated by the GNSS auxiliary information model, then define the adjustment coefficient:

[0084]

[0085] (2) Calculate the corrected noise covariance

[0086]

[0087] where V0 is the reference value of the measurement noise covariance under good conditions, V k is the estimated GNSS measurement noise covariance at the current time k, t k reflects the changes in the current GNSS environment and dynamically adjusts the measurement noise based on GNSS auxiliary information.

[0088] (3) Calculate the filter gain

[0089] Combine the corrected V k to calculate the Kalman filter gain:

[0090]

[0091] where P k / (k-1) is the predicted error covariance matrix, H k is the measurement matrix, and K k is the filter gain matrix.

[0092] Through the above adaptive adjustment method, the GNSS measurement noise covariance can be adaptively changed under different GNSS environments:

[0093] When the GNSS signal is good (many satellites, small pDOP), R GNSS is small, t k is also small, resulting in V k becomes small, which increases the weight of the filter on GNSS data.

[0094] When the GNSS signal is disturbed (few satellites, large pDOP), R GNSS is large, t k becomes large, resulting in V k becomes large, which reduces the dependence of the filter on GNSS data and relies more on INS data, improving robustness.

[0095] This adaptive adjustment strategy can effectively improve the reliability and accuracy of GNSS / INS integrated navigation system in complex environments.

[0096] The method can effectively improve the robustness of the combined navigation system in the case of GNSS signal interference, adaptively adjusts the reliability of the filter to GNSS data, and thus ensures the navigation accuracy of the vehicle in a complex environment.

[0097] S3, adaptively estimating the preliminarily adjusted measurement noise by the improved Sage-Husa adaptive filtering algorithm to obtain a final value of the measurement noise estimation;

[0098] In an optional implementation, the S3 comprises:

[0099] S31, dynamically estimating the measurement noise by using the innovation vector, and updating the preliminarily adjusted measurement noise by an exponentially fading memory weighted average method;

[0100] The core idea of the Sage-Husa adaptive filtering algorithm is to dynamically estimate the measurement noise by using the innovation vector and update the noise covariance by an exponentially fading memory weighted average method, and the innovation vector is expressed as: wherein, Z k is an actual sensor measurement value (position, speed, etc. measured by GNSS); is a system model prediction value (position information, etc. predicted by INS).

[0101] The innovation vector reflects the deviation between the actual measurement value and the model prediction value, and can be used to dynamically adjust the measurement noise.

[0102] Based on the innovation vector the following recursive formula is used to estimate the measurement noise covariance matrix:

[0103]

[0104] wherein, is the measurement noise covariance estimated at the current time k, is the noise covariance at the last time, d k is a weight factor, and the calculation formula is as follows: d k =(1-b) / (1-b k+1 ) wherein b is a forgetting factor used to control the weight of historical data, and the value range is (0, 1). When b takes a larger value, longer historical information is retained; when b takes a smaller value, the noise estimation is more sensitive to new data.

[0105] The measurement noise estimation value is updated by weighting the noise covariance at the last time and the variance of the current innovation vector; d kThe weights of historical information and current information are controlled. If d k is too large, the noise estimation is too sensitive to the current observation, which can easily cause oscillation; if d k is too small, the noise estimation is too slow to respond, which cannot adjust in time.

[0106] S32、In the mutation working condition, the measurement noise is modified according to the adjustment coefficient of the measurement noise as prior information.

[0107] The Sage-Husa algorithm performs well in the slowly changing noise environment, but when the GNSS signal is mutated (such as shielding and interference), the dramatic change of the noise can cause large errors. Therefore, the adjustment coefficient t k of the GNSS auxiliary information modeling is introduced as prior information for modification.

[0108] (1) Calculate the adjustment coefficient t k

[0109] The adjustment coefficient t k is calculated by the GNSS auxiliary information (number of stars, pDOP) model:

[0110] t k = R GNSS

[0111] Where R GNsS is the GNSS positioning error estimation value calculated by the GNSS auxiliary information mathematical model.

[0112] In order to normalize the adjustment coefficient t k and make it applicable to different environmental conditions, it is mapped to the interval [0, 1]:

[0113]

[0114] Where t min and t max are the minimum and maximum adjustment coefficients in the historical data, respectively.

[0115] (2) Calculate the measurement noise jump parameter

[0116] Under the mutation working condition, the measurement noise will change dramatically, so a measurement noise jump parameter is defined:

[0117] Δα k = |α k - α k-1 |

[0118] When Δα k exceeds the preset threshold Δα th , it indicates that the noise has mutated and needs to be quickly corrected.

[0119] (3) Fast correction of mutation noise

[0120] When Δα k is detected to be too large, the GNSS auxiliary information model is used to quickly correct the measurement noise:

[0121]

[0122] wherein, is the preliminary estimated measurement noise covariance, λ is the correction coefficient, and α k is related.

[0123] S33, the modified measurement noise is adaptively estimated by the improved Sage-Husa adaptive filtering algorithm to obtain the final measurement noise estimate.

[0124] After the above correction, the adjusted measurement noise is adaptively estimated by the improved Sage-Husa adaptive filtering algorithm to obtain the final noise estimate:

[0125]

[0126] The final obtained is the optimized GNSS measurement noise covariance.

[0127] S4, according to the motion characteristics of the unmanned surface vehicle, a motion constraint condition is added to the measurement equation.

[0128] In an optional implementation, the motion constraint condition is:

[0129]

[0130] wherein, is the true speed of the unmanned surface vehicle in the vertical direction of the carrier coordinate system;

[0131] In the carrier coordinate system, the speed error of the unmanned surface vehicle can be written as:

[0132]

[0133] wherein, n and b are the navigation coordinate system and the carrier coordinate system respectively: is the speed error in the carrier coordinate system, is the transpose matrix of the direction cosine matrix of the navigation coordinate system to the carrier coordinate system in the INS; V n = [v E v N v U ] T is the true speed of the carrier in the navigation coordinate system; δVn =[δv E δv N δv U ] T is the velocity error in the navigation coordinate system; δA=[δφ E δφ N δφ U ] is the three-dimensional attitude error vector, i.e., the 7th to 9th elements of the system state vector of the GNSS / INS integrated navigation system filter;

[0134] A is the antisymmetric matrix of δA, that is:

[0135]

[0136] Motion constraints are introduced into the filter as observation information: In order to enhance the filter model by utilizing the motion characteristics of the unmanned surface vehicle, the vertical velocity constraint can be used as a virtual observation and added to the measurement equation of the filter.

[0137] (1) Constructing virtual observations on the carrier coordinate system

[0138] In the carrier coordinate system, the vertical velocity constraint is introduced:

[0139]

[0140] in, Indicates the vertical velocity error in the carrier coordinate system, H bfc is the observation matrix of motion constraint.

[0141] Observation matrix H bfc Calculation:

[0142] H bfc =[0 1×3 C 31 C 32 C 33 v u C 32 -v N C 33 v E C 33 -v U C 31 v N C 31

[0143] -v E C 32 0 1×6 ]

[0144] Among them, C ij express The element in the i-th row and j-th column of the matrix, vE , v N , v U is the velocity component in the navigation coordinate system.

[0145] (2) Modification of the measurement equation

[0146] In the Kalman filtering process, the state update equation remains unchanged:

[0147] X k = FX k-1 + W k

[0148] where X k is the system state vector; F is the state transition matrix; W k is the process noise.

[0149] The measurement equation after adding the motion constraint:

[0150]

[0151] where Z GNSS is the GNSS measurement value; H GNSS is the observation matrix of GNSS; V GNSS is the GNSS measurement noise; is the virtual observation of the vertical velocity; H bfc is the motion constraint observation matrix; is the noise of the motion constraint.

[0152] Through the above motion constraint method, the observation variable of the filter model of the integrated navigation system is increased, and when the satellite signal is severely disturbed and the vehicle is in a dynamic process, the feedback and estimation of the filter model can still continue.

[0153] Figure 2 The application provides an integrated navigation system of an unmanned surface vehicle, as shown in FIG. 1, which comprises: Figure 2

[0154] A mathematical model establishing unit 201 is configured to combine a GNSS positioning system (GNSS) and a strapdown inertial navigation system (INS) to form a GNSS / INS integrated navigation system, and establish a mathematical model of a filter of the GNSS / INS integrated navigation system; the mathematical model comprises a state equation and a measurement equation.

[0155] An adjusting unit 202 is configured to establish a mathematical model of the influence of GNSS auxiliary information on the positioning accuracy of the GNSS, so as to preliminarily adjust the measurement noise in the measurement equation.

[0156] ​The estimation unit 203 is configured to perform adaptive estimation on the preliminary adjusted measurement noise by using the improved Sage-Husa adaptive filtering algorithm, and obtain a final value of the measurement noise estimation.

[0157] The constraint unit 204 is configured to add a motion constraint condition to the measurement equation according to the motion characteristics of the unmanned surface vehicle.

[0158] The present application has the following beneficial effects:

[0159] The present application provides a combined navigation method for an unmanned surface vehicle, which can objectively reflect the influence of external environment on positioning accuracy by establishing a GNSS auxiliary information mathematical model based on multi-element function fitting, and dynamically adjust the measurement noise in the filter, thereby significantly improving the positioning accuracy of the combined navigation system under interference conditions; when the GNSS signal is weak or lost, the motion constraint is used to supplement the observation variable, thereby ensuring the continuous measurement update of the filter and enhancing the system stability; the improved Sage-Husa adaptive filtering algorithm combined with prior information can quickly correct the measurement noise under sudden working conditions, thereby ensuring the rapid response and long-term stable operation of the filter.

[0160] Finally, it should be noted that: the above examples 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 examples, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing examples, 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 combined navigation method for an unmanned surface vehicle, characterized in that: include: S1. Combining a GNSS positioning system (GNSS) and a strapdown inertial navigation system (INS) into a GNSS / INS integrated navigation system, and establishing a mathematical model of a filter for the GNSS / INS integrated navigation system; the mathematical model includes a state equation and a measurement equation; S2. Establishing a mathematical model of the effect of GNSS auxiliary information on GNSS positioning accuracy to perform preliminary adjustments to the measurement noise in the measurement equation; S3, adaptively estimating the measurement noise after the preliminary adjustment by using the improved Sage-Husa adaptive filtering algorithm to obtain a final value of the measurement noise estimate; S4. Add motion constraints to the measurement equation based on the motion characteristics of the unmanned surface vehicle.

2. The method according to claim 1, wherein: The state equation is established based on the INS error; the measurement equation is established based on the speed difference and position difference between GNSS and INS.

3. The method according to claim 1, wherein: The state equation is: k =Φ k,k-1 X k-1 +Γ k-1 W k-1 The measurement equation is: k =H k X k +V k Among them, X k is the system state vector, Φ k,k-1 is the state transfer matrix, W k-1 is the system noise vector, Γ k-1 is the system noise matrix, Z k is the measurement vector, H k is the measurement matrix, V k is the measurement noise vector.

4. The method according to claim 3, wherein: The state equation and the measurement equation are solved by filtering equations.

5. The method according to claim 1, wherein The S2 includes: The GNSS navigation receiver module is placed at a known point with accurately calibrated geodetic coordinates in an open area. The GNSS navigation receiver module is effectively shielded in different directions and areas using electromagnetic shielding materials to obtain the GNSS system positioning accuracy under different satellite numbers and satellite spatial geometric factors. By using the least squares method, a polynomial or other function is selected as the basis function of the fitting function to establish a multivariate function mathematical model of the effect of GNSS auxiliary information on GNSS positioning accuracy; the GNSS auxiliary information includes: the number of satellites and satellite spatial geometry factors; An adjustment coefficient of the measurement noise is calculated according to a multivariate function mathematical model, and the measurement noise in the measurement equation is updated according to the adjustment coefficient of the measurement noise.

6. The method according to claim 1, characterized in that The S3 includes: The measurement noise is dynamically estimated using the innovation vector, and the initially adjusted measurement noise is updated using the exponential fading memory weighted average method. In case of sudden change in working condition, the measurement noise after preliminary adjustment is corrected according to the adjustment coefficient of the measurement noise as a priori information; The modified measurement noise is adaptively estimated by using the improved Sage-Husa adaptive filtering algorithm to obtain the final value of the measurement noise estimation.

7. The method according to claim 1, wherein: The motion constraints are: in, is the true velocity of the unmanned surface vehicle in the vertical direction of the carrier coordinate system; In the carrier coordinate system, the speed error of the unmanned surface vehicle can be written as: Among them, n and b are the navigation coordinate system and the carrier coordinate system respectively: is the velocity error in the carrier coordinate system, V is the transposed matrix of the direction cosine matrix from the navigation coordinate system in INS to the vehicle coordinate system; n =[v E v N v U ] T is the true velocity of the carrier in the navigation coordinate system; δV n =[δv E δv N δv U ] T is the velocity error in the navigation coordinate system; δA=[δφ E δφ N δφ U ] is the three-dimensional attitude error vector, i.e., the 7th to 9th elements of the system state vector of the GNSS / INS integrated navigation system filter; A is the antisymmetric matrix of δA, that is:

8. An integrated navigation system for an unmanned surface vehicle, characterized in that: include: A mathematical model building unit is used to combine a GNSS positioning system (GNSS) and a strapdown inertial navigation system (INS) into a GNSS / INS integrated navigation system and to build a mathematical model of a filter of the GNSS / INS integrated navigation system; the mathematical model includes: a state equation and a measurement equation; an adjustment unit, configured to establish a mathematical model of the effect of GNSS auxiliary information on GNSS positioning accuracy, so as to perform preliminary adjustment on the measurement noise in the measurement equation; an estimation unit, configured to adaptively estimate the measurement noise after the preliminary adjustment by using an improved Sage-Husa adaptive filtering algorithm to obtain a final value of the measurement noise estimation; The constraint unit is used to add motion constraint conditions to the measurement equation according to the motion characteristics of the unmanned surface vehicle.

Images