A radio RTK integrated navigation and positioning method based on custom coordinates for agricultural scenarios

Abstract

The invention discloses a method for combined navigation and navigation of agricultural positioning and navigation technology, specifically a radio RTK combined navigation and positioning method based on customized coordinates of agricultural scenes, including the following specific steps: using a self-built radio station to perform GNSS differential positioning, and outputting real-time centimeter-level precision position information; taking the coordinates of the radio station base station as the origin, performing Gaussian projection, and converting the longitude and latitude coordinates output by the GNSS differential into customized coordinates; after unifying the time and space of multiple sensors, performing combined navigation in a customized coordinate system, and outputting high-precision posture data. The invention builds a self-built RTK radio station base station, reduces the burden and cost of the server, and improves the reliability of the data link; adopts dual-antenna RTK to ensure the accuracy and reliability of the heading angle of the carrier in an agricultural environment; establishes a customized coordinate system suitable for agricultural scenes and an inertial sensor / dual-antenna RTK combined navigation model in the coordinate system, and improves positioning accuracy, stability, and robustness.

Description

Technical Field

[0001] The present invention relates to the field of agricultural positioning and navigation technology, and in particular to a radio RTK combined navigation and positioning method based on custom coordinates of agricultural scenes. Background Art

[0002] Precision agriculture and smart agriculture in the agricultural field are still in urgent need of development. There are a series of problems and limitations. This is related to the variability and complexity of the agricultural environment. Positioning and navigation is one of the core technologies. The demand for positioning in agriculture is mainly reflected in high accuracy (centimeter level), high stability (stable positioning quality) and high reliability (positioning system is not prone to downtime).

[0003] GNSS provides high-precision three-dimensional position information for the carrier, and its positioning error will not accumulate over time, but it is susceptible to signal shielding or interference. INS uses inertial elements to calculate the position, velocity and attitude information of the moving carrier. INS has autonomous navigation capabilities and strong anti-interference capabilities, but its navigation error gradually accumulates over time. Combining GNSS with INS can obtain three-dimensional position, velocity and attitude information with better reliability, higher accuracy and faster data update rate.

[0004] RTK real-time dynamic differential positioning technology uses GNSS carrier phase observations to correct positioning results, thereby performing real-time dynamic relative positioning. RTK generally uses 4G networks to establish a data communication link between the base station and the rover, receives the carrier phase observations and location information of the base station through the communication link, and calculates high-precision location information.

[0005] GNSS is generally a latitude and longitude coordinate system, which generally needs to be converted to meters for subsequent control. The Universal Transverse Mercator Grid System UTM coordinates are a plane rectangular coordinate system. This coordinate grid system and the projection it is based on have been widely used in topographic maps, as a reference grid for satellite images and natural resource databases, and other applications that require precise positioning.

[0006] However, MEMS inertial sensors are generally used in the agricultural field. Due to the accuracy of MEMS gyroscopes, the heading angle cannot be sensed. The magnetometer is also easily disturbed by the use environment, so the accuracy of the heading angle cannot be guaranteed. In agricultural scenarios, the 4G signal is generally weak, and network RTK may not be able to run continuously, affecting the reliability of the system. UTM is a conformal projection. If the agricultural work scene happens to be far away from the two secants, resulting in a large length deformation, or is at the edge of the degree band, the transformation will be cumbersome and affect subsequent precision agriculture operations. Therefore, a radio RTK combined navigation and positioning method based on custom coordinates for agricultural scenarios is invented to solve the above problems. Summary of the invention

[0007] In view of the above-mentioned and / or existing problems in a radio RTK integrated navigation and positioning method based on customized coordinates of agricultural scenarios, the present invention is proposed.

[0008] Therefore, the purpose of the present invention is to provide a radio RTK integrated navigation and positioning method based on customized coordinates of agricultural scenes, which can solve the above-mentioned existing problems.

[0009] To solve the above technical problems, according to one aspect of the present invention, the present invention provides the following technical solutions:

[0010] A radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes includes the following specific steps:

[0011] S1: Adjust the power and frequency of the radio station to broadcast differential information in the form of radio waves, and use the multi-frequency GNSS antenna to receive satellite signals and differential data sent by the base station radio station. Finally, use the GNSS board to calculate and output real-time centimeter-level precision position information;

[0012] S2: Take the coordinates of the radio station base station as the origin, perform Gaussian projection, and convert the longitude and latitude coordinates output by GNSS into custom coordinates;

[0013] S3: First, unify the time and space of the sensors, then synchronize the time of the inertial sensor and GNSS through PPS, then convert the dual antenna installation angles, and set the arm between the IMU and GNSS antennas;

[0014] S4: Based on the radio station's custom coordinate dual-antenna combined navigation, the extended Kalman filter is used to establish a model in the custom coordinate system after initial alignment. Finally, high-precision posture data is output by fusing GNSS and IMU real-time data, and the error is fed back to improve the accuracy, stability and reliability of positioning.

[0015] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, the specific process of S1 is as follows:

[0016] S11: Adjust the radio power, data transmission rate and frequency to broadcast differential information in the form of radio waves, and establish a data communication link between the base station and the nearby mobile station;

[0017] S12: First, use the multi-frequency GNSS antenna to receive satellite signals and differential data sent by the base station radio, and parse the ephemeris, pseudorange and carrier phase data received by the antenna. Finally, use the GNSS board to calculate and output real-time centimeter-level precision position information and high-precision attitude angle information.

[0018] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, the specific process of S2 is as follows:

[0019] S21: measure the coordinates of the radio station base station in the 2000 coordinate system;

[0020] S22: Compensate for the base station antenna phase offset and use this as the origin of the custom coordinate system;

[0021] S23: Take the origin longitude and latitude as the central meridian, due north as the Y axis, due east as the X axis, perform Gaussian projection, and convert the longitude and latitude coordinates output by GNSS into custom coordinates, where the height is based on the ellipsoid height in the 2000 coordinate system;

[0022] S24: To keep the X-axis coordinate in the custom coordinate system as a positive value, the X-axis coordinate output in step S23 is offset by 50,000 m according to the base station coverage and the agricultural scene.

[0023] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, the specific process of S3 is as follows:

[0024] S31: Time synchronization of inertial sensor and GNSS through PPS;

[0025] S32: transforming the attitude information output by the GNSS through a rotation matrix according to the GNSS dual antenna installation angle to obtain a real heading angle of the carrier;

[0026] S33: Set up the arm between the IMU and GNSS antenna.

[0027] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, the specific process of S4 is as follows:

[0028] S41: Coarse alignment: After the system receives the data output by the inertial sensor, it first determines the position of the carrier through GNSS, and then determines the gravity acceleration at the position. Then, the inertial sensor measures the carrier's earth rotation angular velocity V1 and earth gravity acceleration V2. At this time, the projection coordinates of the earth rotation angular velocity V1 and earth gravity acceleration V2 in two coordinates are recorded as V1 and V2 respectively. r , V1 b and Finally, the direction cosine matrix between the carrier coordinate system b and the earth coordinate system r is solved by double vector attitude determination to obtain the carrier's initial attitude rotation matrix

[0029] S42: Fine alignment: Since the MEMS inertial sensor has low accuracy and cannot sense the angular velocity of the earth's rotation, the rough alignment can be used to obtain Finally, only the accurate pitch and roll angles can be obtained, so other observations are needed to obtain the heading angle;

[0030] S43: Taking the initial posture of the carrier as a reference, solving the carrier data collected by the strapdown inertial navigation system to obtain the carrier posture information solved by the inertial navigation sensor;

[0031] S44: Establishing a combined navigation state space model in a custom coordinate system;

[0032] S45: Through the extended Kalman filter, the inertial navigation data and GNSS dual antenna data are processed and fused, and the zero bias and installation misalignment angle errors of the gyroscope and accelerometer are estimated to obtain high-precision posture information;

[0033] S46: Feedback and correct the zero bias and installation misalignment angle error of the gyro and accelerometer obtained in S45 to improve subsequent positioning accuracy;

[0034] S47: Finally, the fused positioning and attitude data with high accuracy and reliability is output.

[0035] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, wherein: the initial attitude rotation matrix of the carrier in S41 for:

[0036]

[0037] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, the specific process of S42 is as follows:

[0038] S421: Establish a precise alignment model based on the heading angle, and use the angular velocity information measured by the gyroscope, the heading angle and pitch angle output by the dual antennas, and the initial attitude rotation matrix of the carrier As input, Kalman filtering is used for state estimation. The model based on the static initial alignment of dual-antenna RTK is as follows:

[0039]

[0040] φ is the strapdown inertial navigation attitude error, δv is the strapdown inertial navigation velocity error, ε is the gyroscope random constant drift, and Z is the velocity and heading angle observation;

[0041] S422: First, the state equation and measurement equation in the strapdown inertial navigation system model are discretized, and then the state estimation is performed through Kalman filtering to output high-precision initial posture data.

[0042] As a preferred solution of the radio RTK combined navigation and positioning method based on customized coordinates of agricultural scenes described in the present invention, wherein: the carrier posture information in S43 includes attitude angle, speed and position.

[0043] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, wherein: the attitude angle is to collect angular velocity and acceleration data collected by the inertial sensor, and use the rotation vector of three sub-samples to correct the cone error, and then use the quaternion to update the attitude equation to calculate the attitude angle at the current moment;

[0044] The speed is obtained by compensating the gravity acceleration by the speed and attitude of the carrier at the previous moment, and then solving the angular velocity and acceleration data collected by the inertial sensor to obtain the speed value at the current moment;

[0045] The position is calculated by using the position, speed and geographic parameters of the previous moment to obtain the position value at the current moment.

[0046] As a preferred solution of the radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes described in the present invention, the spatial model of S44 is established as follows:

[0047]

[0048]

[0049]

[0050] Where: X is the error state, F is the state transfer matrix, G is the noise transfer matrix, is the inertial navigation output velocity error, is the RTK output velocity error, is the inertial navigation output position error, is the position error in the custom coordinate system, is the inertial navigation output attitude error, is the RTK output attitude error, H is the observation transfer matrix, V is the observation noise, W b is the system noise, is the gyroscope angular velocity measurement white noise, is the accelerometer specific force measurement white noise, δv n is the speed error, δp is the position error, ε b is the gyroscope bias, is the accelerometer bias, δl b is the lever arm error, and δt is the time synchronization error.

[0051] Compared with existing technologies:

[0052] 1. Build your own RTK radio base station and use radio communication to make up for the generally poor 4G signal in agricultural scenarios. When multiple agricultural machines are operating within a range of tens of kilometers, it can effectively reduce the burden on the server, reduce costs, and improve the reliability of positioning.

[0053] 2. The radio dual-antenna RTK is used to ensure the accuracy and reliability of the carrier heading angle in the agricultural environment, making up for the shortcomings of low-cost MEMS inertial sensors that cannot sense the heading angle and magnetometers that are easily affected by environmental interference.

[0054] 3. UTM coordinates are at the edge, subsequent processing is cumbersome, and the length is deformed. A custom coordinate system suitable for agricultural scenarios is established to improve applicability.

[0055] 4. Establish an inertial sensor / dual-antenna RTK combined navigation model in a custom coordinate system, and use data fusion to improve positioning accuracy, stability, robustness, and efficiency, ultimately improving the quality and benefits of agricultural production. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is the overall flow chart of the present invention;

[0057] Figure 2 This is a combined navigation flow chart of the present invention;

[0058] Figure 3 This is a flow chart of the Kalman filter of the present invention. DETAILED DESCRIPTION

[0059] In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0060] The present invention provides a radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes. Figure 1-Figure 3 , including the following specific steps:

[0061] S1: Adjust the power and frequency of the radio station to broadcast differential information in the form of radio waves, and use the multi-frequency GNSS antenna to receive satellite signals and differential data sent by the base station radio station. Finally, use the GNSS board to calculate and output real-time centimeter-level precision position information;

[0062] Among them: The specific process of S1 is as follows:

[0063] S11: Adjust the radio power, data transmission rate and frequency to broadcast differential information in the form of radio waves, and establish a data communication link between the base station and the nearby mobile station;

[0064] S12: First, use the multi-frequency GNSS antenna to receive satellite signals and differential data sent by the base station radio, and parse the ephemeris, pseudorange and carrier phase data received by the antenna. Finally, use the GNSS board to calculate and output real-time centimeter-level precision position information and high-precision attitude angle information;

[0065] S2: Take the coordinates of the radio station base station as the origin, perform Gaussian projection, and convert the longitude and latitude coordinates output by GNSS into custom coordinates;

[0066] Among them: The specific process of S2 is as follows:

[0067] S21: measure the coordinates of the radio station base station in the 2000 coordinate system;

[0068] S22: Compensate for the base station antenna phase offset and use this as the origin of the custom coordinate system;

[0069] S23: Take the origin longitude and latitude as the central meridian, due north as the Y axis, due east as the X axis, perform Gaussian projection, and convert the longitude and latitude coordinates output by GNSS into custom coordinates, where the height is based on the ellipsoid height in the 2000 coordinate system;

[0070] S24: To keep the X-axis coordinate in the custom coordinate system as a positive value, the X-axis coordinate output in step S23 is offset by 50000m according to the base station coverage and the agricultural scene;

[0071] S3: First, unify the time and space of the sensors, then synchronize the time of the inertial sensor and GNSS through PPS, then convert the dual antenna installation angles, and set the arm between the IMU and GNSS antennas;

[0072] Among them: The specific process of S3 is as follows:

[0073] S31: Time synchronization of inertial sensor and GNSS through PPS;

[0074] S32: transforming the attitude information output by the GNSS through a rotation matrix according to the GNSS dual antenna installation angle to obtain a real heading angle of the carrier;

[0075] S33: Setting the arm between the IMU and the GNSS antenna;

[0076] S4: Based on the radio station's custom coordinate dual-antenna integrated navigation, after initial alignment, the extended Kalman filter is used to establish a model in the custom coordinate system. Finally, high-precision posture data is output by fusing GNSS and IMU real-time data, and errors are fed back to improve positioning accuracy, stability and reliability.

[0077] Among them: The specific process of S4 is as follows:

[0078] S41: Coarse alignment: After the system receives the data output by the inertial sensor, it first determines the position of the carrier through GNSS, and then determines the gravity acceleration at the position. Then, the inertial sensor measures the carrier's earth rotation angular velocity V1 and earth gravity acceleration V2. At this time, the projection coordinates of the earth rotation angular velocity V1 and earth gravity acceleration V2 in two coordinates are recorded as V1 and V2 respectively. r , V1 b and Finally, the direction cosine matrix between the carrier coordinate system b and the earth coordinate system r is solved by double vector attitude determination to obtain the carrier's initial attitude rotation matrix Its carrier initial posture rotation matrix for:

[0079]

[0080] S42: Fine alignment: Since the MEMS inertial sensor has low accuracy and cannot sense the angular velocity of the earth's rotation, the rough alignment can be used to obtain Finally, only the accurate pitch angle and roll angle can be obtained, so other observations are needed to obtain the heading angle. The specific process is as follows:

[0081] S421: Establish a precise alignment model based on the heading angle, and use the angular velocity information measured by the gyroscope, the heading angle and pitch angle output by the dual antennas, and the initial attitude rotation matrix of the carrier As input, Kalman filtering is used for state estimation. The model based on the static initial alignment of dual-antenna RTK is as follows:

[0082]

[0083] X=[φ E φ N φ U δv E δv N ε N ε U ] T ;

[0084] φ is the strapdown inertial navigation attitude error, δv is the strapdown inertial navigation velocity error, ε is the gyroscope random constant drift, and Z is the velocity and heading angle observation;

[0085] S422: first discretize the state equation and measurement equation in the strapdown inertial navigation system model, and then perform state estimation through Kalman filtering to output high-precision initial posture data;

[0086] S43: Taking the initial posture of the carrier as a reference, the carrier data collected by the strapdown inertial navigation system is solved to obtain the carrier posture information solved by the inertial navigation sensor, and the carrier posture information includes attitude angle, speed and position; the attitude angle is the angular velocity and acceleration data collected by the inertial sensor, and the cone error is corrected by using the rotation vector of the three sub-samples, and then the attitude equation is updated with the quaternion to calculate the attitude angle at the current moment; the speed is to compensate for the gravity acceleration by the speed and posture of the carrier at the previous moment, and then solve it in combination with the angular velocity and acceleration data collected by the inertial sensor to obtain the speed value at the current moment; the position is to solve the position value at the current moment by the position and speed at the previous moment and the geographic parameters;

[0087] S44: Establish a combined navigation state space model in a custom coordinate system, taking into account the arm error and time synchronization error. The space model is established as follows:

[0088]

[0089]

[0090]

[0091] Where: X is the error state, F is the state transfer matrix, G is the noise transfer matrix, is the inertial navigation output velocity error, is the RTK output velocity error, is the inertial navigation output position error, is the position error in the custom coordinate system, is the inertial navigation output attitude error, is the RTK output attitude error, H is the observation transfer matrix, V is the observation noise, W b is the system noise, is the gyroscope angular velocity measurement white noise, is the accelerometer specific force measurement white noise, δv n is the speed error, δp is the position error, ε b is the gyroscope bias, is the accelerometer bias, δl b is the lever arm error, δt is the time synchronization error;

[0092] S45: Through the extended Kalman filter, the inertial navigation data and GNSS dual antenna data are processed and fused, and the zero bias and installation misalignment angle errors of the gyroscope and accelerometer are estimated to obtain high-precision posture information;

[0093] S46: Feedback and correct the zero bias and installation misalignment angle error of the gyro and accelerometer obtained in S45 to improve subsequent positioning accuracy;

[0094] S47: Finally, the fused positioning and attitude data with high accuracy and reliability is output.

[0095] Although the present invention has been described above with reference to the embodiments, various modifications may be made thereto and parts thereof may be replaced by equivalents without departing from the scope of the present invention. In particular, as long as there is no structural conflict, the various features in the embodiments disclosed in the present invention may be used in combination with each other in any manner, and the fact that these combinations are not exhaustively described in this specification is only for the sake of omitting space and saving resources. Therefore, the present invention is not limited to the specific embodiments disclosed herein, but includes all technical solutions falling within the scope of the claims.

Claims

1. A radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes, characterized in that: The specific steps are as follows: S1: Adjust the power and frequency of the radio station to broadcast differential information in the form of radio waves, and use the multi-frequency GNSS antenna to receive satellite signals and differential data sent by the base station radio station. Finally, use the GNSS board to calculate and output real-time centimeter-level precision position information; S2: Take the coordinates of the radio station base station as the origin, perform Gaussian projection, and convert the longitude and latitude coordinates output by GNSS into custom coordinates; S3: First, the sensor time and space are unified, and then the inertial sensor and GNSS are synchronized through PPS, followed by the dual antenna installation angle conversion and the arm setting between the IMU and GNSS antennas; S4: Based on the radio station's custom coordinate dual-antenna integrated navigation, after initial alignment, the extended Kalman filter is used to establish a model in the custom coordinate system. Finally, high-precision posture data is output by fusing GNSS and IMU real-time data, and errors are fed back to improve positioning accuracy, stability and reliability. The specific process of S2 is as follows: S21: measure the coordinates of the radio station base station in the 2000 coordinate system; S22: Compensate for the base station antenna phase offset and use this as the origin of the custom coordinate system; S23: Take the origin longitude and latitude as the central meridian, due north as the Y axis, due east as the X axis, perform Gaussian projection, and convert the longitude and latitude coordinates output by GNSS into custom coordinates, where the height is based on the ellipsoid height in the 2000 coordinate system; S24: To keep the X-axis coordinate in the custom coordinate system as a positive value, the X-axis coordinate output in step S23 is offset by 50,000 m according to the base station coverage and the agricultural scene.

2. According to claim 1, a radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes is characterized in that: The specific process of S1 is as follows: S11: Adjust the radio power, data transmission rate and frequency to broadcast differential information in the form of radio waves, and establish a data communication link between the base station and the nearby mobile station; S12: First, use the multi-frequency GNSS antenna to receive satellite signals and differential data sent by the base station radio, and parse the ephemeris, pseudorange and carrier phase data received by the antenna. Finally, use the GNSS board to calculate and output real-time centimeter-level precision position information and high-precision attitude angle information.

3. The radio RTK integrated navigation and positioning method based on agricultural scene custom coordinates according to claim 1 is characterized in that: The specific process of S3 is as follows: S31: Time synchronization of inertial sensor and GNSS through PPS; S32: transforming the attitude information output by the GNSS through a rotation matrix according to the GNSS dual antenna installation angle to obtain a real heading angle of the carrier; S33: Set up the arm between the IMU and GNSS antenna.

4. The radio RTK integrated navigation and positioning method based on custom coordinates of agricultural scenes according to claim 1 is characterized in that: The specific process of S4 is as follows: S41: Coarse alignment: After the system receives the data output by the inertial sensor, it first determines the position of the carrier through GNSS, and then determines the gravity acceleration at the position. Then, the inertial sensor measures the carrier's earth rotation angular velocity V1 and earth gravity acceleration V2. At this time, the projection coordinates of the earth rotation angular velocity V1 and earth gravity acceleration V2 in two coordinates are recorded as V1 and V2 respectively. r , V1 b and Finally, the direction cosine matrix between the carrier coordinate system b and the earth coordinate system r is solved by double vector attitude determination to obtain the carrier's initial attitude rotation matrix S42: Fine alignment: Since the MEMS inertial sensor has low accuracy and cannot sense the angular velocity of the earth's rotation, the rough alignment can be used to obtain Finally, only the accurate pitch and roll angles can be obtained, so other observations are needed to obtain the heading angle; S43: Taking the initial posture of the carrier as a reference, solving the carrier data collected by the strapdown inertial navigation system to obtain the carrier posture information solved by the inertial navigation sensor; S44: Establishing a combined navigation state space model in a custom coordinate system; S45: Through the extended Kalman filter, the inertial navigation data and GNSS dual antenna data are processed and fused, and the zero bias and installation misalignment angle errors of the gyroscope and accelerometer are estimated to obtain high-precision posture information; S46: Feedback and correct the zero bias and installation misalignment angle error of the gyro and accelerometer obtained in S45 to improve subsequent positioning accuracy; S47: Finally, the fused positioning and attitude data with high accuracy and reliability is output.

5. The radio RTK integrated navigation and positioning method based on agricultural scene custom coordinates according to claim 4 is characterized in that: The initial posture rotation matrix of the carrier in S41 for:

6. The radio RTK integrated navigation and positioning method based on agricultural scene custom coordinates according to claim 4 is characterized in that: The specific process of S42 is as follows: S421: Establish a precise alignment model based on the heading angle, and use the angular velocity information measured by the gyroscope, the heading angle and pitch angle output by the dual antennas, and the initial attitude rotation matrix of the carrier As input, Kalman filtering is used for state estimation. The model based on the static initial alignment of dual-antenna RTK is as follows: X=[φ E f N f U dv E dv N e N e U ] T ; φ is the strapdown inertial navigation attitude error, δv is the strapdown inertial navigation velocity error, ε is the gyroscope random constant drift, and Z is the velocity and heading angle observation; S422: First, the state equation and measurement equation in the strapdown inertial navigation system model are discretized, and then the state estimation is performed through Kalman filtering to output high-precision initial posture data.

7. The radio RTK integrated navigation and positioning method based on agricultural scene custom coordinates according to claim 4 is characterized in that: The carrier posture information in S43 includes posture angle, speed and position.

8. The radio RTK integrated navigation and positioning method based on agricultural scene custom coordinates according to claim 7, characterized in that: The attitude angle is obtained by collecting the angular velocity and acceleration data collected by the inertial sensor, and using the rotation vector of the three sub-samples to correct the cone error, and then using the quaternion to update the attitude equation to calculate the attitude angle at the current moment; The speed is obtained by compensating the gravity acceleration by the speed and attitude of the carrier at the previous moment, and then solving the angular velocity and acceleration data collected by the inertial sensor to obtain the speed value at the current moment; The position is calculated by using the position, speed and geographic parameters of the previous moment to obtain the position value at the current moment.

9. The radio RTK integrated navigation and positioning method based on agricultural scene custom coordinates according to claim 5, characterized in that: The spatial model of S44 is established as follows: X=[φ T (dv n ) T (δp) T (e b ) T (▽ b ) T (δl b ) T δt] T ; Where: X is the error state, F is the state transfer matrix, G is the noise transfer matrix, is the inertial navigation output velocity error, is the RTK output velocity error, is the inertial navigation output position error, is the position error in the custom coordinate system, is the inertial navigation output attitude error, is the RTK output attitude error, H is the observation transfer matrix, V is the observation noise, W b is the system noise, is the gyroscope angular velocity measurement white noise, is the accelerometer specific force measurement white noise, δv n is the speed error, δp is the position error, ε b is the gyroscope bias, is the accelerometer bias, δl b is the lever arm error, and δt is the time synchronization error.

Images