High-precision integrated positioning navigation method for unmanned agricultural machine

Abstract

The invention belongs to the field of unmanned agricultural machine positioning and navigation, and discloses a high-precision integrated positioning and navigation method for an unmanned agricultural machine, which comprises the following steps: S1, establishing a state equation and a measurement equation of an integrated positioning and navigation system; s2, taking an error equation of the GNSS and the SINS as a state equation of the system, taking information differences output by the GNSS and the SINS as quantity measurement, and adopting a Kalman filter to realize high-precision integrated navigation; s3, performing combined filtering on the SINS and GNSS data by adopting an outlier identification principle based on Kalman filtering innovation orthogonality, namely corrected KF filtering for short; according to the method, whether an outlier occurs in measurement is judged by judging whether orthogonality in a Kalman filtering innovation process is lost; then, an activation function is used for weighting quantity measurement, so that the adverse effect of GNSS outliers on filtering is inhibited; and finally, high-precision integrated positioning navigation of the GNSS / SINS is realized.

Description

Technical Field

[0001] The present invention belongs to the field of positioning and navigation of unmanned agricultural machinery, and in particular relates to a high-precision combined positioning and navigation method for unmanned agricultural machinery. Background Art

[0002] GNSS (Global Navigation Satellite System) and SINS (Strapdown Inertial Navigation System) are two basic and commonly used positioning and navigation systems. Current unmanned agricultural machinery is also usually equipped with these two systems to obtain the absolute position and attitude information of the agricultural machinery. When the GNSS signal is stable and continuous, it can provide accurate position positioning information and carrier speed information, but it is extremely susceptible to the influence of the surrounding environment (such as signal blocking, multipath effect, etc.), resulting in poor signal quality. SINS is mainly composed of two modules, an accelerometer and a gyroscope, which are used to measure the vehicle's position, attitude information, speed and other information. It has strong anti-interference ability and high output frequency, but its disadvantage is that its positioning accuracy diverges over time. Due to the complementary characteristics between GNSS and SINS, the two are usually used in combination to form a GNSS / SINS combined navigation system.

[0003] The core of integrated navigation is the fusion algorithm that integrates the information from both. Common information fusion methods include Bayesian estimation, Kalman filtering (KF), weighted fusion, and neural networks. Kalman filtering is one of the most widely used fusion algorithms in integrated navigation systems. It is an optimal estimation method based on minimum variance under a Gaussian distribution model.

[0004] Hilly and mountainous areas account for more than 50% of my country's total cultivated land area. When unmanned agricultural machinery works in this area, there are problems such as satellite signals being blocked and weak signals, which will reduce the quality of satellite signals and cause wild values in GNSS measurement results.

[0005] In the GNSS / SINS combined positioning and navigation data processing, GNSS provides measurement information for the Kalman filter. The outliers in its data will directly affect the stability of the Kalman filter and the accuracy of the integrated navigation system.

[0006] However, few existing combination methods and corresponding algorithms address the low accuracy of positioning and navigation systems for unmanned agricultural machinery operating in hilly and mountainous areas with weak satellite signals. Therefore, the present invention proposes a new combined navigation data fusion algorithm based on existing combined navigation methods to enable unmanned agricultural machinery to obtain highly accurate positioning and navigation information in hilly and mountainous areas with weak satellite signals. Summary of the Invention

[0007] The present invention aims to provide a high-precision combined positioning and navigation method for unmanned agricultural machinery. This method identifies outliers in measurements by determining whether the orthogonality of the Kalman filter innovation process has been lost. An activation function is then used to weight the measurements, thereby suppressing the adverse effects of GNSS outliers on the filter. Ultimately, this method achieves high-precision combined GNSS / SINS positioning and navigation.

[0008] The technical solution adopted by this unmanned agricultural machinery high-precision combined positioning and navigation method to solve its technical problems is:

[0009] A high-precision combined positioning and navigation method for unmanned agricultural machinery is provided, comprising:

[0010] S1: Establish the state equation and measurement equation of the combined positioning and navigation system;

[0011] S2: Using the error equations of GNSS and SINS as the state equation of the system, the information difference between the GNSS and SINS outputs as the quantity measurement, and the Kalman filter to achieve high-precision integrated navigation;

[0012] S3: Use the outlier identification principle based on the orthogonality of Kalman filter innovations, referred to as modified KF filtering, to perform combined filtering on SINS and GNSS data;

[0013] S4: In the process of combined filtering, whether outliers appear in the measurement is determined by determining whether the orthogonality of the Kalman filter innovation process is lost;

[0014] S5: Use an activation function to weight the quantity measurement so that the modified innovation process can maintain the orthogonality property;

[0015] S6: Obtain the filtering results of the modified KF method including longitude, latitude and speed;

[0016] S7: Feedback the filtering result of the modified KF method to the SINS strapdown inertial navigation system for correction;

[0017] S8: Obtain the optimal estimated values of the combined positioning and navigation parameters of the unmanned agricultural machinery.

[0018] Furthermore, the state equation of the combined positioning and navigation system model includes:

[0019]

[0020] Where,

[0021] Among them, E 、ф N、ф U is the misalignment angle of the mathematical platform;

[0022] δv E ,δv N ,δv U are the eastward, northward and celestial velocity errors of the carrier respectively;

[0023] δL, δλ, and δh are the latitude error, longitude error, and altitude error, respectively;

[0024] ε x , ε y , ε z、 are the gyro random constant drift and accelerometer random constant zero bias,

[0025] The noise transfer matrix G(t) of the system is:

[0026]

[0027] The system noise vector is composed of random errors of the gyroscope and accelerometer and is expressed as:

[0028]

[0029] The state transfer matrix F(t) of the system is:

[0030]

[0031] Furthermore, the measurement equations of the combined positioning and navigation system model include:

[0032] The difference between the position and velocity output by GNSS and SINS is taken as the measurement, where the velocity measurement vector is:

[0033]

[0034] Among them, H v =[0 3x3 diag(1 1 1)0 3x9 ](3-2)

[0035] V v =[v GE v GN v GU ] T (3-3)

[0036] The position measurement vector is:

[0037]

[0038] Among them, H p =[03x6 diag(R M R N cosL 1)0 3x6 ](3-5)

[0039] V p =[N GE N GN N GU ](3-6)

[0040] In the above formulas, subscript I represents SINS, subscript G represents GNSS; v GE 、v GN 、v GU is the velocity error of GNSS along the northeast sky direction; N GE 、N GN 、N GU is the position error of GNSS along the northeast celestial direction,

[0041] Combined velocity and position measurement equations, the measurement equation for SINS / GNSS integrated navigation is:

[0042]

[0043] Furthermore, the outlier identification principle based on the orthogonality of Kalman filter innovations includes:

[0044] The basic equation of the standard discrete Kalman filter is as follows:

[0045] State one-step prediction:

[0046]

[0047] State Estimation:

[0048]

[0049] Filter gain:

[0050]

[0051] or:

[0052]

[0053] One-step prediction mean square error:

[0054]

[0055] Estimated mean squared error:

[0056]

[0057] or:

[0058] P k =(IK k H k )P k / k-1 (4-7)

[0059] or:

[0060]

[0061] Equations (4-1) to (4-8) are the basic equations of discrete Kalman filtering, where Φ k,k-1 t k-1 Time to t k One-step transfer matrix at time ; H k is the measurement matrix; Г k-1 is the noise driving array of the system,

[0062] W k is the noise sequence of the system; V k is the measurement noise sequence; Q k is the variance matrix of the system noise sequence; R k is the variance matrix of the measurement noise sequence,

[0063] As long as the initial value is given and P0, according to the measurement Z at time k k , we can recursively calculate the state estimate at time k (k=1,2,…),

[0064] The basic equation of the standard discrete Kalman filter (4-2) It is called the innovation process, denoted by e k , which reflects the new information brought by the current measurement.

[0065] It can be seen from formula (4-2) that since the Kalman filter is a linear optimal filter, when there are outliers in the measurement, according to the principle of linear superposition, the outliers will directly affect the state estimation of the Kalman filter in the form of linear superposition, causing the reliability and convergence speed of the Kalman filter to decrease, and even causing the filter to diverge and lose stability.

[0066] Since the innovation process e k It has orthogonal properties, and the new information process contains all the information of the measurement process. When an outlier appears in the measurement process, it will change e k The original properties, therefore, can be judged by judging whether the orthogonality of the innovation process is lost to determine whether there are outliers in the measurement.

[0067] Furthermore, it also includes the design of filters based on the orthogonality of Kalman filter innovations to resist outliers:

[0068] Update of Kalman filter equations:

[0069] Considering the measurement correction, the corrected prediction equation shown in Equation (5-1) is obtained from Equation (4-2), and the other equations in the basic equations of the Kalman filter remain unchanged.

[0070]

[0071] where B k = [Z1(k)f1(r1)Z2(k)f2(r2)…Z m (k)f m (r m )] T ;

[0072] where f i (r i )(i = 1, 2, …, m) is an activation function with the following properties:

[0073] When r i →∞, f i (r i )→0;

[0074] f i (r i ) is monotonically decreasing and continuously differentiable;

[0075] Cut-off property. When r i <C or r i ≥C, f i (r i ) is a constant, where C is called the threshold value.

[0076] Identification of outliers in measurement values:

[0077] According to the orthogonality in the innovation process, we have:

[0078]

[0079] where is the optimal estimated value of Z k .

[0080] Expanding the right side of Equation (5-2) gives:

[0081]

[0082] Denote:

[0083]

[0084] According to the diagonal elements of the matrices on both sides of Equation (5-3), for the measurement value Z kWhether each component of is an outlier is judged by hypothesis, specifically whether the inclusion relationship of formula (5-5) is established. If formula (5-5) is established, it is considered that Z i (k) is a normal quantity measurement; otherwise, it is considered that Z i (k) is an outlier value,

[0085] M i,i (k)∈[D i,i (k)-ε i ,D i,i (k)+ε i ]i=1,2,…,m(5-5)

[0086] Where M i,i (k) and D i,i (k) respectively represent and D k The i-th element on the diagonal,

[0087] Taking into account the calculation error, a small disturbance ε is added to the above judgment i ,

[0088] Correction of outliers in measurements:

[0089] In order to keep the orthogonality of the modified Kalman filter innovation process unchanged, the activation function should be consistent with the current measurement Z k The following smooth activation function is generally used:

[0090]

[0091] in,

[0092] The idea of the above Kalman filter measurement correction algorithm is to measure the quantity Z in each filtering cycle. k Each component in the equation is identified and corrected for outliers. i When (k) is an outlier, there is r i >d i (k), then use a number d less than 1 i (k) / r i To Z i (k) Perform weighted restriction to suppress the increase of the variance of the new information process and keep its original orthogonality; when Z i When (k) is not an outlier, there is r i ≤d i (k), then Z i The weighted value of (k) is unity, which does not change the innovation sequence.

[0093] Compared with the prior art, the present invention has the following beneficial effects:

[0094] The present invention demonstrates a high-precision combined positioning and navigation method for unmanned agricultural machinery. This method determines whether outliers appear in measurements by determining whether the orthogonality of the Kalman filter innovation process is lost. An activation function is then used to weight the measurements, thereby suppressing the adverse effects of GNSS outliers on filtering. Ultimately, high-precision combined GNSS / SINS positioning and navigation is achieved.

[0095] Able to effectively identify and suppress the adverse effects of GNSS outliers on filtering in combined positioning and navigation;

[0096] For normal GNSS data, the filtering accuracy of this method is equivalent to that of the standard Kalman filter;

[0097] When outliers appear in GNSS, the filtering accuracy and stability of this method are significantly better than those of the standard Kalman filter. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] Other features, objects and advantages of the present application will become more apparent upon reading the detailed description of non-limiting embodiments made with reference to the following drawings:

[0099] Figure 1 This is a hardware schematic diagram of the combined positioning and navigation system of the present invention;

[0100] Figure 2 This is a schematic diagram of the feedback correction principle of the present invention;

[0101] Figure 3 This is a flowchart of the GNSS data anti-outlier processing method of the present invention. DETAILED DESCRIPTION

[0102] The present application will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are intended only to illustrate the relevant invention and are not intended to limit the invention. It should also be noted that, for ease of description, only portions relevant to the invention are shown in the accompanying drawings.

[0103] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in this application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0104] Example 1:

[0105] This embodiment provides a high-precision combined positioning and navigation method for unmanned agricultural machinery, including:

[0106] S1: Establish the state equation and measurement equation of the combined positioning and navigation system;

[0107] S2: Using the error equations of GNSS and SINS as the state equation of the system, and the information difference output by GNSS and SINS as the quantity measurement, a Kalman filter is used to achieve high-precision integrated navigation. The hardware diagram of the integrated positioning and navigation system is shown in the figure. Figure 1 As shown;

[0108] S3: Use the outlier identification principle based on the orthogonality of Kalman filter innovations, referred to as modified KF filtering, to perform combined filtering on SINS and GNSS data;

[0109] S4: In the process of combined filtering, whether outliers appear in the measurement is determined by determining whether the orthogonality of the Kalman filter innovation process is lost;

[0110] S5: Use an activation function to weight the quantity measurement so that the modified innovation process can maintain the orthogonality property;

[0111] S6: Obtain the filtering results of the modified KF method including longitude, latitude and speed;

[0112] S7: Feedback the filtering result of the modified KF method to the SINS strapdown inertial navigation system for correction. The feedback correction principle is as follows: Figure 2 As shown;

[0113] S8: Obtain the optimal estimated values of the combined positioning and navigation parameters of the unmanned agricultural machinery.

[0114] Furthermore, the state equation of the combined positioning and navigation system model includes:

[0115]

[0116] Where,

[0117] Among them, E 、ф N 、ф U is the misalignment angle of the mathematical platform;

[0118] δv E ,δv N ,δv U are the eastward, northward and celestial velocity errors of the carrier respectively;

[0119] δL, δλ, and δh are the latitude error, longitude error, and altitude error, respectively;

[0120] ε x , ε y , ε z、 are the gyro random constant drift and accelerometer random constant zero bias,

[0121] The noise transfer matrix G(t) of the system is:

[0122]

[0123] The system noise vector is composed of random errors of the gyroscope and accelerometer and is expressed as:

[0124]

[0125] The state transfer matrix F(t) of the system is:

[0126]

[0127] Among them, F N is the corresponding 9-dimensional basic navigation parameter system matrix.

[0128] In this embodiment, the measurement equation of the combined positioning and navigation system model includes:

[0129] The difference between the position and velocity output by GNSS and SINS is taken as the measurement, where the velocity measurement vector is:

[0130]

[0131] Among them, H v =[0 3x3 diag(1 1 1)0 3x9 ](3-2)

[0132] V v =[v GE v GN v GU ] T (3-3)

[0133] The position measurement vector is:

[0134]

[0135] Among them, H p =[0 3x6 diag(R M R N cosL 1)0 3x6 ](3-5)

[0136] V p =[N GE N GN N GU ](3-6)

[0137] In the above formulas, subscript I represents SINS, subscript G represents GNSS; v GE 、vGN 、v GU is the velocity error of GNSS along the northeast sky direction; N GE 、N GN 、N GU is the position error of GNSS along the northeast celestial direction,

[0138] Combined velocity and position measurement equations, the measurement equation for SINS / GNSS integrated navigation is:

[0139] .

[0140] In this embodiment, the outlier identification principle based on the orthogonality of Kalman filter innovations includes:

[0141] The basic equation of the standard discrete Kalman filter is as follows:

[0142] State one-step prediction:

[0143]

[0144] State Estimation:

[0145]

[0146] Filter gain:

[0147]

[0148] or:

[0149]

[0150] One-step prediction mean square error:

[0151]

[0152] Estimated mean squared error:

[0153]

[0154] or:

[0155] P k =(IK k H k )P k / k-1 (4-7)

[0156] or:

[0157]

[0158] Equations (4-1) to (4-8) are the basic equations of discrete Kalman filtering, where Φ k,k-1 for tk -1One-step transfer matrix from time tk to time tk; H k is the measurement matrix; Г k-1 is the noise driving array of the system,

[0159] W k is the noise sequence of the system; V k is the measurement noise sequence; Q k is the variance matrix of the system noise sequence; R k is the variance matrix of the measurement noise sequence,

[0160] As long as the initial value is given and P0, according to the measurement Z at time k k , we can recursively calculate the state estimate at time k (k=1,2,…),

[0161] The basic equation of the standard discrete Kalman filter (4-2) It is called the innovation process, denoted by e k , which reflects the new information brought by the current measurement.

[0162] It can be seen from formula (4-2) that since the Kalman filter is a linear optimal filter, when there are outliers in the measurement, according to the principle of linear superposition, the outliers will directly affect the state estimation of the Kalman filter in the form of linear superposition, causing the reliability and convergence speed of the Kalman filter to decrease, and even causing the filter to diverge and lose stability.

[0163] Since the innovation process e k It has orthogonal properties, and the new information process contains all the information of the measurement process. When an outlier appears in the measurement process, it will change e k The original properties, therefore, can be judged by judging whether the orthogonality of the innovation process is lost to determine whether there are outliers in the measurement.

[0164] This embodiment also includes a filter design based on the orthogonality of Kalman filter innovations to resist outliers:

[0165] Kalman filter equations update:

[0166] Taking the measurement correction into account, the modified prediction equation shown in equation (5-1) is obtained from equation (4-2). The other equations in the basic equation of Kalman filter remain unchanged.

[0167]

[0168] Where B k =[Z1(k)f1(r1)Z2(k)f2(r2)…Z m (k)f m (r m )]T ;

[0169] where \(f\) i (\(r\) i )(\(i = 1, 2, \ldots, m\)) is an activation function with the following properties:

[0170] When \(r\) i \(\to \infty\), \(f\) i (\(r\) i ) \(\to 0\);

[0171] \(f\) i (\(r\) i ) is monotonically decreasing and continuously differentiable;

[0172] Cut-off property. When \(r\) i \(< C\) or \(r\) i \(\geq C\), \(f\) i (\(r\) i ) is a constant. Here, \(C\) is called the threshold value.

[0173] Identification of outliers in measurements:

[0174] According to the orthogonality in the innovation process, we have:

[0175]

[0176] In the formula, is the optimal estimated value of \(Z\) k .

[0177] Expand the right side of equation (5-2) to get:

[0178] <00​​​​​​​​​​​​​​​​​​​​​​​​​​​​​i,i (k) and D i,i (k) respectively represent and D k The i-th element on the diagonal,

[0184] Taking into account the calculation error, a small disturbance ε is added to the above judgment i，

[0185] Correction of outliers in measurements:

[0186] In order to keep the orthogonality of the modified Kalman filter innovation process unchanged, the activation function should be consistent with the current measurement Z k The following smooth activation function is generally used:

[0187] in,

[0188] The idea of the above Kalman filter measurement correction algorithm is to measure the quantity Z in each filtering cycle. k Each component in the equation is identified and corrected for outliers. i When (k) is an outlier, there is r i >d i (k), then use a number d less than 1 i (k) / r i To Z i (k) Perform weighted restriction to suppress the increase of the variance of the new information process and keep its original orthogonality; when Z i When (k) is not an outlier, there is r i ≤d i (k), then Z i The weighted value of (k) is unity, which does not change the new information sequence. The GNSS data anti-outlier processing process is as follows: Figure 3 shown.

[0189] In this embodiment, the unmanned agricultural machinery positioning and navigation experiment is set up in the farmland of hilly mountainous area;

[0190] The SINS inertial device accuracy is as follows: the gyro constant drift and random drift are both 0.1° / h, the accelerometer constant bias and random bias are both 0.0001g, and the inertial device data output frequency is 100Hz;

[0191] GNSS uses Novatel's high-precision dual-frequency carrier phase differential GPS (hereinafter referred to as DGPS, including rover and base station), with a differential positioning accuracy of centimeter level and a data output frequency of 20Hz;

[0192] Since unmanned agricultural machinery operates in a non-open environment, outliers may appear in the DGPS position and speed data, and there may be short-term lock loss.

[0193] The Kalman filter anti-outlier positioning and navigation method based on the orthogonality of new information (abbreviated as modified KF filtering) provided by the present invention is used to perform combined filtering on SINS and DGPS data;

[0194] In the process of combined filtering, whether outliers appear in the measurement is determined by judging whether the orthogonality of the Kalman filter innovation process is lost;

[0195] Then an activation function is used to weight the quantity measurement so that the modified innovation process can maintain the orthogonality property;

[0196] Then the filtering results of the modified KF method are obtained, especially the longitude, latitude and speed;

[0197] The filtering results of the modified KF method are then fed back to the SINS strapdown inertial navigation system to calibrate it, and finally the optimal estimated values of the positioning and navigation parameters of the unmanned agricultural machinery are obtained.

[0198] This method determines whether outliers appear in the measurement by determining whether the orthogonality of the Kalman filter innovation process is lost. An activation function is then used to weight the measurements, thereby suppressing the adverse effects of GNSS outliers on the filter. Ultimately, high-precision GNSS / SINS combined positioning and navigation is achieved.

[0199] Able to effectively identify and suppress the adverse effects of GNSS outliers on filtering in combined positioning and navigation;

[0200] For normal GNSS data, the filtering accuracy of this method is equivalent to that of the standard Kalman filter;

[0201] When outliers appear in GNSS, the filtering accuracy and stability of this method are significantly better than those of the standard Kalman filter.

[0202] The above description is merely a preferred embodiment of the present application and an illustration of the technical principles employed. Those skilled in the art should understand that the scope of the invention herein is not limited to the technical solutions formed by the specific combination of the above-mentioned technical features, but also encompasses other technical solutions formed by any combination of the above-mentioned technical features or their equivalents without departing from the inventive concept. For example, a technical solution formed by replacing the above-mentioned features with (but not limited to) technical features having similar functions disclosed in this application.

[0203] Except for the technical features described in the specification, the remaining technical features are known technologies to those skilled in the art. In order to highlight the innovative features of the present invention, the remaining technical features will not be described here in detail.

Claims

1. A high-precision combined positioning and navigation method for unmanned agricultural machinery, characterized in that: include: S1: Establish the state equation and measurement equation of the combined positioning and navigation system; S2: Using the error equations of GNSS and SINS as the state equation of the system, the information difference between the GNSS and SINS outputs as the quantity measurement, and the Kalman filter to achieve high-precision integrated navigation; S3: Use the outlier identification principle based on the orthogonality of Kalman filter innovations, referred to as modified KF filtering, to perform combined filtering on SINS and GNSS data; S4: In the process of combined filtering, whether outliers appear in the measurement is determined by determining whether the orthogonality of the Kalman filter innovation process is lost; S5: Use an activation function to weight the quantity measurement so that the modified innovation process can maintain the orthogonality property; S6: Obtain the filtering results of the modified KF method including longitude, latitude and speed; S7: Feedback the filtering result of the modified KF method to the SINS strapdown inertial navigation system for correction; S8: Obtain the optimal estimated values of the combined positioning and navigation parameters of the unmanned agricultural machinery.

2. The high-precision combined positioning and navigation method for unmanned agricultural machinery according to claim 1 is characterized in that: The state equations of the combined positioning and navigation system model include: where X = [ф E ф N ф U δv E δv N δv U δLδλδhε x ε y ε z ▼x▼y▼z] T Among them, E 、ф N 、ф U is the misalignment angle of the mathematical platform; δv E ,δv N ,δv U are the eastward, northward and celestial velocity errors of the carrier respectively; δL, δλ, and δh are the latitude error, longitude error, and altitude error, respectively; ε x , ε y , ε z、 ▼x, ▼y, ▼z are the gyro random constant drift and accelerometer random constant zero bias respectively. The noise transfer matrix G(t) of the system is: The system noise vector is composed of random errors of the gyroscope and accelerometer and is expressed as: in=[in εx In εy In εz w▼xw▼yw▼z] T The state transfer matrix F(t) of the system is: Among them, F N is the corresponding 9-dimensional basic navigation parameter system matrix.

3. The high-precision combined positioning and navigation method for unmanned agricultural machinery according to claim 1 is characterized in that: The measurement equations of the combined positioning and navigation system model include: The difference between the position and velocity output by GNSS and SINS is taken as the measurement, where the velocity measurement vector is: Among them, H v =[0 3x3 diag(1 1 1)0 3x9 ](3-2) V v =[v GE v GN v GU ] T (3-3) The position measurement vector is: Among them, H p =[0 3x6 diag(R M R N cosL 1)0 3x6 ](3-5) V p =[N GE N GN N GU ] (3-6) In the above formulas, subscript I represents SINS, subscript G represents GNSS; v GE 、v GN 、v GU is the velocity error of GNSS along the northeast sky direction; N GE 、N GN 、N GU is the position error of GNSS along the northeast celestial direction, Combined velocity and position measurement equations, the measurement equation for SINS / GNSS integrated navigation is:

4. The high-precision combined positioning and navigation method for unmanned agricultural machinery according to claim 1, characterized in that: The outlier identification principle based on Kalman filter innovation orthogonality includes: The basic equation of the standard discrete Kalman filter is as follows: State one-step prediction: State Estimation: Filter gain: or: One-step prediction mean square error: Estimated mean squared error: or: P k =(I-K k H k )P k / k-1 (4-7) or: Equations (4-1) to (4-8) are the basic equations of discrete Kalman filtering, where Φ k,k-1 t k-1 Time to t k One-step transfer matrix at time ; H k is the measurement matrix; Г k-1 is the noise driving array of the system, W k is the noise sequence of the system; V k is the measurement noise sequence; Q k is the variance matrix of the system noise sequence; R k is the variance matrix of the measurement noise sequence, As long as the initial value is given and P0, according to the measurement Z at time k k , we can recursively calculate the state estimate at time k (k=1,2,…), The basic equation of the standard discrete Kalman filter (4-2) It is called the innovation process, denoted by e k , which reflects the new information brought by the current measurement.

5. The high-precision combined positioning and navigation method for unmanned agricultural machinery according to claim 1, characterized in that: It also includes the design of filters that are resistant to outliers based on the orthogonality of Kalman filter innovations: Kalman filter equations update: Taking the measurement correction into account, the modified prediction equation shown in equation (5-1) is obtained from equation (4-2). The other equations in the basic equation of Kalman filter remain unchanged. Where B k =[Z1(k)f1(r1)Z2(k)f2(r2)…Z m (k)f m (r m )] T ; where f i (r i )(i=1,2,…,m) is the activation function with the following properties: When r i →∞, f i (r i )→0; f i (r i ) is monotonically decreasing and continuously differentiable; Cut-off property: when r i <C or r i ≥ C, f i (r i ) is a constant, where C is called the threshold value. Identification of outliers in measurements: According to the orthogonality of the innovation process: Where, Z k The best estimate of Expand the right side of formula (5-2) to get: remember: According to the diagonal elements of the matrices on both sides of formula (5-3), the quantity Z is measured k Whether each component of is an outlier is judged by hypothesis, specifically whether the inclusion relationship of formula (5-5) is established. If formula (5-5) is established, it is considered that Z i (k) is a normal quantity measurement; otherwise, it is considered that Z i (k) is an outlier value, M i,i (k)∈[D i , i (k)-ε i ,D i , i (k)+ε i ]i=1,2,…,m (5-5) Where M i,i (k) and D i,i (k) respectively represent and D k The i-th element on the diagonal, Taking into account the calculation error, a small disturbance ε is added to the above judgment i , Correction of outliers in measurements: In order to keep the orthogonality of the modified Kalman filter innovation process unchanged, the activation function should be consistent with the current measurement Z k The following smooth activation function is generally used: in, The idea of the above Kalman filter measurement correction algorithm is to measure the quantity Z in each filtering cycle. k Each component in the equation is identified and corrected for outliers. i When (k) is an outlier, there is r i >d i (k), then use a number d less than 1 i (k) / r i To Z i (k) Perform weighted restriction to suppress the increase of the variance of the new information process and keep its original orthogonality; when Z i When (k) is not an outlier, there is r i ≤d i (k), then Z i The weighted value of (k) is unity, which does not change the innovation sequence.

Images