A double-tractor cooperative operation high-precision positioning method and device

Abstract

The application provides a double-tractor cooperative operation high-precision positioning method and device, belongs to the field of intelligent tractor navigation positioning, and comprises the following steps: the double tractor comprises a tractor A and a tractor B; the tractor A receives wireless signals sent by the tractor B through DSRC, and calculates the relative distance and the relative azimuth angle of the tractor B; the DSRC represents the automobile special short-distance communication; based on the relative distance and the relative azimuth angle, the combined navigation state equation and the observation equation of the tractor A are established; the navigation state is updated and solved by using the extended Kalman filtering algorithm, and the positioning information after error compensation is sent to the tractor B through the DSRC, so that the high-precision positioning of the double-tractor cooperative operation is realized. The application significantly improves the system robustness while ensuring the accuracy.

Description

Technical Field

[0001] This invention belongs to the field of intelligent agricultural machinery navigation and positioning, specifically relating to a high-precision positioning method and device for collaborative operation of two agricultural machines. Background Technology

[0002] With the rapid development of agricultural automation and intelligence, the application of unmanned agricultural machinery is gradually becoming more widespread. Various collaborative operation scenarios exist during unmanned agricultural machinery operations, such as harvesters and grain transport vehicles working together, fertilizer spreaders and fertilizer application vehicles working together, and similar agricultural machinery working together on the same plot of land. The stability and safety of multi-machine collaborative operations rely on reliable positioning technology. Global Navigation Satellite Systems (GNSS), as a global positioning technology, can provide unmanned agricultural machinery with position, velocity, and time information, and by combining with Inertial Navigation Systems (INS), it can provide continuous position, velocity, and attitude information.

[0003] GNSS signals may be interfered with or blocked by buildings, trees or hills, or a vehicle's GNSS positioning equipment may suddenly malfunction. In such cases, the industrial-grade INS system devices commonly used in agricultural machinery have poor accuracy, and strapdown inertial navigation (a type of inertial navigation technology) has the problem of rapid divergence in calculating position information. If the GNSS signal is missing for a few minutes or even tens of seconds, the position information will no longer be available, which may cause the vehicle to lose control and cause collisions or other dangers. Summary of the Invention

[0004] To address the problems of integrated navigation methods in unmanned agricultural machinery collaborative operations, this invention provides a high-precision positioning method and device for dual agricultural machinery collaborative operations. It obtains the relative distance and relative angle during agricultural machinery operation through Dedicated Short Range Communications (DSRC) ranging, and integrates traditional GNSS / INS integrated navigation system equations, real-time distributed navigation filtering, and dynamic optimization data fusion strategies through DSRC information transmission, thereby significantly improving system robustness while ensuring accuracy.

[0005] To achieve the above objectives, the present invention adopts the following technical solution:

[0006] A high-precision positioning method for collaborative operation of two agricultural machines, including agricultural machine A and agricultural machine B, includes the following steps:

[0007] S1. Agricultural machinery A receives the wireless signal sent by agricultural machinery B through DSRC and calculates the relative distance and relative azimuth angle with agricultural machinery B; DSRC stands for short-range communication for automobiles.

[0008] S2. Based on the relative distance and relative azimuth angle, establish the combined navigation state equation and observation equation for agricultural machinery A;

[0009] S3. The extended Kalman filter algorithm is used to update the navigation status, and the error-compensated positioning information is sent to agricultural machine B through DSRC to achieve high-precision positioning for collaborative operation of two agricultural machines.

[0010] The present invention also provides a high-precision positioning device for dual agricultural machinery cooperative operation, used to implement the above method, comprising the following modules:

[0011] The relative azimuth angle calculation module includes two agricultural machines, agricultural machine A and agricultural machine B. Agricultural machine A receives the wireless signal sent by agricultural machine B through DSRC and calculates the relative distance and relative azimuth angle with agricultural machine B. DSRC stands for short-range communication for automobiles.

[0012] The equation-establishing module establishes the combined navigation state equation and observation equation for agricultural machinery A based on the relative distance and relative azimuth angle.

[0013] The positioning module uses the extended Kalman filter algorithm to update the navigation status and sends the error-compensated positioning information to agricultural machine B via DSRC to achieve high-precision positioning for collaborative operation of the two agricultural machines.

[0014] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the above-described high-precision positioning method for collaborative operation of two agricultural machines.

[0015] The present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the above-described high-precision positioning method for cooperative operation of two agricultural machines.

[0016] Beneficial effects:

[0017] This invention is applicable to situations where GNSS signals deteriorate or some sensors fail during collaborative operation of two unmanned agricultural machines. It introduces real-time measured relative distance and azimuth information between the agricultural machines to construct a relative position observation model, and achieves collaborative estimation of the state of the two agricultural machines through a distributed Kalman filter architecture, significantly improving the accuracy, robustness and fault tolerance of the navigation system in complex farmland environments. Attached Figure Description

[0018] Figure 1 This is a flowchart of a high-precision positioning method for dual agricultural machinery cooperative operation according to the present invention;

[0019] Figure 2 A comparison chart of navigation errors for GNSS / INS combined navigation and GNSS / INS / DCRS combined navigation;

[0020] Figure 3This is a schematic diagram of a high-precision positioning device for collaborative operation of two agricultural machines according to the present invention. Detailed Implementation

[0021] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0022] like Figure 1 As shown, the present invention provides a high-precision positioning method for collaborative operation of two agricultural machines, comprising the following steps:

[0023] S1: Collaborative agricultural machinery is defined as agricultural machinery A and agricultural machinery B. Agricultural machinery A calculates the relative distance and relative azimuth angle with agricultural machinery B based on the path loss model in the DSRC transmission process.

[0024] S2: Using the relative distance and relative azimuth angle obtained in S1, establish the combined navigation state equation and observation equation for agricultural machinery A;

[0025] S3: Based on the integrated navigation state equation and observation equation, perform extended Kalman filter updates, perform error compensation feedback, and send the position information, position error, and velocity error information to agricultural machine B via DSRC.

[0026] Further, S1 includes:

[0027] S1.1 After receiving the wireless communication signal from the DSRC transmitter of agricultural machinery B, the DSRC receiver of agricultural machinery A calculates the signal power;

[0028] S1.2 Calculate the relative distance d between the DSRC receiver of agricultural machinery A and the DSRC transmitter of agricultural machinery B based on the Shadowing model (i.e., path loss model) in radio ranging:

[0029] ;

[0030] in, The relative distance between the receiving and transmitting devices (in meters). For reference distance, The path divergence index; The signal power received by the receiving device. It is a Gaussian random variable.

[0031] Further, S2 includes:

[0032] S2.1 The DSRC receiving device of agricultural machinery A parses the data information sent by the DSRC transmitting device of agricultural machinery B, and obtains parameters such as the position, position error status, and speed error status of agricultural machinery B;

[0033] S2.2 is based on the relative distance provided by S1 Design a DSRC / INS combined Kalman filter.

[0034] Define the 21-dimensional error state vector of the Kalman filter. as follows:

[0035] ;

[0036] in, Let A be the three-dimensional position error vector of agricultural machinery A. Let A be the three-dimensional velocity error vector of agricultural machinery A. Let A be the three-dimensional attitude error vector of agricultural machinery A. Zero bias for the three-axis gyroscope of agricultural machinery A. For agricultural machinery A, the three-axis acceleration is zero bias. Let B be the three-dimensional position error vector of agricultural machinery. This is the three-dimensional velocity error vector of agricultural machinery B, with the superscript T indicating the transpose of the matrix; some error states of agricultural machinery B are transmitted through DSRC;

[0037] S2.3 Establish the error state equation of the continuous DSRC / GNSS / INS combined Kalman filter (i.e., the combined navigation state equation):

[0038] ;

[0039] in, Represents the differential of the error state vector; The state transition matrix is ​​21. A 21-dimensional matrix, whose elements are established based on the differential equation of inertial navigation error; The process noise matrix is... The noise is assumed to be zero-mean Gaussian white noise.

[0040] The S2.4 observation equation can be expressed as:

[0041] ;

[0042] in, For measurement variables, take , The difference between the location calculated by the strapdown system of agricultural machinery A and the GNSS location; The difference between the location of agricultural machinery B and the GNSS location is calculated via strapdown and obtained through DSRC transmission; x is the input variable of the observation equation; The relative distance error is calculated by using inertial navigation strapdown to determine the position, and is related to the distance between the receiving and transmitting devices. Calculated; This is a measurement matrix, and its elements take values ​​of... ,in 3 with diagonal elements of 1 3 identity matrices It is a unit direction vector, and its calculation method is as follows: , For the three-dimensional position of agricultural machinery A, The three-dimensional position of agricultural machinery B; The element is m A matrix of n zeros, where m = 1, 3, n = 3, 6. To measure the noise variables, we assume that they are all Gaussian white noise with zero mean.

[0043] Further, S3 includes:

[0044] S3.1 Based on the integrated navigation state equation and observation equation established in S2, extended Kalman filter is used to update the solution and obtain the error state vector in real time. The optimal estimate.

[0045] S3.2 Error State Vector Compensation is performed, and the compensated position information, position error information, and speed error information are sent to agricultural machine B via DSRC, so that agricultural machine B can complete the joint position estimation filtering.

[0046] Example:

[0047] The simulation generates a straight path for the agricultural machinery during operation, with the noise level of the output data set to: gyroscope zero bias 0.1. The accelerometer zero bias was 0.1 mg. The simulation data was 10 minutes long. The GNSS data was normal for the first 5 minutes, the signal quality deteriorated from 5 to 6 minutes, and returned to normal after 6 minutes. The simulation data was filtered and corrected using the method described above. The processed eastward position error was compared with GNSS / INS combined navigation and GNSS / INS / DCRS combined navigation, yielding the following results: Figure 2 The curve shown can be effectively corrected by adding DCRS combination during the period of GNSS signal deterioration after filtering.

[0048] like Figure 3 As shown, the present invention provides a high-precision positioning device for dual agricultural machinery cooperative operation, used to implement the above method, comprising the following modules:

[0049] The relative azimuth angle calculation module includes two agricultural machines, agricultural machine A and agricultural machine B. Agricultural machine A receives the wireless signal sent by agricultural machine B through DSRC and calculates the relative distance and relative azimuth angle with agricultural machine B. DSRC stands for short-range communication for automobiles.

[0050] The equation-establishing module establishes the combined navigation state equation and observation equation for agricultural machinery A based on the relative distance and relative azimuth angle.

[0051] The positioning module uses the extended Kalman filter algorithm to update the navigation status and sends the error-compensated positioning information to agricultural machine B via DSRC to achieve high-precision positioning for collaborative operation of the two agricultural machines.

[0052] The present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the above-described high-precision positioning method for collaborative operation of two agricultural machines.

[0053] The present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the above-described high-precision positioning method for cooperative operation of two agricultural machines.

[0054] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of the present invention can be implemented using various computer languages, such as the object-oriented programming language Java and the interpreted scripting language JavaScript.

Claims

1. A high-precision positioning method for cooperative operation of two agricultural machines, the two agricultural machines including agricultural machine A and agricultural machine B, characterized in that, Includes the following steps: S1. Agricultural machinery A receives the wireless signal sent by agricultural machinery B through DSRC and calculates the relative distance and relative azimuth angle with agricultural machinery B; DSRC stands for short-range communication for automobiles. S2. Based on the relative distance and relative azimuth, establish the combined navigation state equation and observation equation for agricultural machinery A, including: S2.1 The DSRC receiving device of agricultural machinery A parses the data information sent by the DSRC transmitting device of agricultural machinery B, and obtains the position information, position error information, and speed error information of agricultural machinery B; S2.2 Design of a DSRC / INS combined Kalman filter based on relative distance; INS represents inertial navigation system; S2.3 Establish the error state equation of the continuous DSRC / GNSS / INS combined Kalman filter, i.e., the combined navigation state equation; GNSS represents the Global Navigation Satellite System; S2.4 Based on the difference between the strapdown solution position of agricultural machinery A and agricultural machinery B and the GNSS position, and the relative distance error, the observation equation is constructed. S3. The navigation state is updated and calculated using the extended Kalman filter algorithm, and the error-compensated positioning information is sent to agricultural machine B via DSRC to achieve high-precision positioning for collaborative operation of two agricultural machines, including: S3.1 Based on the integrated navigation state equation and observation equation established in S2, extended Kalman filter is used to update the solution and obtain the optimal estimate of the state vector in real time. S3.2 compensates the error state vector of the error state equation of the DSRC / GNSS / INS combined Kalman filter, and sends the compensated position information, position error information and velocity error information to agricultural machine B through DSRC, so that agricultural machine B can complete the joint position estimation filtering.

2. The high-precision positioning method for dual-farm machinery cooperative operation according to claim 1, characterized in that, S1 includes: S1.1 After receiving the wireless communication signal from the DSRC transmitter of agricultural machinery B, the DSRC receiver of agricultural machinery A calculates the signal power.

3. The high-precision positioning method for dual agricultural machinery cooperative operation according to claim 2, characterized in that, S1 further includes: S1.2 Calculate the relative distance between the DSRC receiving device of agricultural machinery A and the DSRC transmitting device of agricultural machinery B based on the path loss model in radio ranging.

4. A high-precision positioning device for cooperative operation of two agricultural machines, comprising agricultural machine A and agricultural machine B, characterized in that, Includes the following modules: The relative azimuth angle calculation module allows agricultural machinery A to receive wireless signals from agricultural machinery B via DSRC and calculate the relative distance and relative azimuth angle with agricultural machinery B; DSRC stands for short-range communication for automobiles. The equation-building module, based on the relative distance and relative azimuth, establishes the combined navigation state equation and observation equation for agricultural machinery A, including: The DSRC receiving device of agricultural machinery A parses the data information sent by the DSRC transmitting device of agricultural machinery B to obtain the position information, position error information, and speed error information of agricultural machinery B; Design of a DSRC / INS combined Kalman filter based on relative distance; INS represents inertial navigation system; Establish a continuous DSRC / GNSS / INS combined Kalman filter error state equation, i.e., a combined navigation state equation; GNSS represents the Global Navigation Satellite System. The observation equation is constructed based on the difference between the strapdown calculated position and the GNSS position of agricultural machinery A and agricultural machinery B, and the relative distance error. The positioning module uses the extended Kalman filter algorithm to update the navigation state and sends the error-compensated positioning information to agricultural machine B via DSRC to achieve high-precision positioning for collaborative operation of two agricultural machines, including: Based on the integrated navigation state equation and observation equation, an extended Kalman filter is used to update the solution and obtain the optimal estimate of the state vector in real time. The error state vector of the error state equation of the DSRC / GNSS / INS combined Kalman filter is compensated, and the compensated position information, position error information and velocity error information are sent to agricultural machine B through DSRC, so that agricultural machine B can complete the joint position estimation filtering.

5. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of a high-precision positioning method for dual agricultural machinery cooperative operation as described in any one of claims 1 to 3.

6. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of a high-precision positioning method for collaborative operation of two agricultural machines as described in any one of claims 1 to 3.

Images