Double agricultural machinery collaborative operation high-precision positioning method and device

Abstract

The invention provides a double agricultural machinery collaborative operation high-precision positioning method and device, and belongs to the field of intelligent agricultural machinery navigation positioning, and the method comprises the steps: the double agricultural machinery comprises an agricultural machinery A and an agricultural machinery B. The agricultural machinery A receives a wireless signal transmitted by the agricultural machinery B through DSRC, and calculates the relative distance and the relative azimuth angle with the agricultural machinery B; dSRC represents special short-distance communication of the automobile; based on the relative distance and the relative azimuth angle, establishing a combined navigation state equation and an observation equation of the agricultural machine A; and updating and resolving the navigation state by using an extended Kalman filtering algorithm, and sending the positioning information after error compensation to the agricultural machine B through DSRC, so as to realize high-precision positioning of collaborative operation of the double agricultural machines. According to the method, the system robustness is remarkably improved while the precision is guaranteed.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of intelligent agricultural machinery navigation and positioning, and particularly relates to a high-precision positioning method and device for double agricultural machinery cooperative operation. BACKGROUND

[0002] With the rapid development of agricultural automation and intelligence, the application of unmanned agricultural machinery is gradually popularizing. There are various cooperative operation scenarios in the operation process of unmanned agricultural machinery, such as cooperative harvesting of a harvester and a grain transport vehicle, a fertilizer spreader and a fertilizer supplement vehicle, and cooperative operation of the same type of agricultural machinery in the same field, and the stability and safety of multi-machine cooperative operation depend on reliable positioning technology. As a global positioning technology, the global navigation satellite system (GNSS) can provide position, speed and time information for unmanned agricultural machinery, and provide continuous position, speed and attitude information through combination with an inertial navigation system (INS).

[0003] GNSS signals can be interfered or shielded by buildings, trees or hills, or a GNSS positioning device of a vehicle can suddenly fail, at which time the industrial-grade INS system commonly used by agricultural machinery has poor accuracy, and the strapdown (a kind of inertial navigation technology) calculation of position information has the problem of rapid divergence, and the position information will no longer be available within a few minutes or even tens of seconds of GNSS signal loss, thereby causing the vehicle to lose control and collide with other vehicles. SUMMARY

[0004] In view of the problems of the combined navigation method in the cooperative operation of unmanned agricultural machinery, the present application provides a high-precision positioning method and device for double agricultural machinery cooperative operation, which obtains the relative distance and relative angle in the operation process of agricultural machinery through DSRC (Dedicated Short Range Communications) ranging, and transmits the DSRC information, fuses the traditional GNSS / INS combined navigation system equation, real-time distributed navigation filtering and dynamic optimization data fusion strategy, thereby significantly improving the system robustness while ensuring the accuracy.

[0005] To achieve the above purpose, the present application adopts the following technical solution:

[0006] A high-precision positioning method for double agricultural machinery cooperative operation, the double agricultural machinery comprising agricultural machinery A and agricultural machinery B, comprising 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 represents Dedicated Short Range Communications;

[0008] S2, based on the relative distance and relative azimuth angle, a combined navigation state equation and an observation equation of agricultural machinery A are established;

[0009] S3, 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 agricultural machine B through the DSRC to realize high-precision positioning of the double agricultural machine cooperative operation.

[0010] The application also provides a double agricultural machine cooperative operation high-precision positioning device for realizing the method, comprising the following modules:

[0011] The relative azimuth angle calculation module, the double agricultural machine comprises an agricultural machine A and an agricultural machine B, the agricultural machine A receives the wireless signal sent by the agricultural machine B through the DSRC, and calculates the relative distance and the relative azimuth angle of the agricultural machine B; the DSRC represents the dedicated short-range communication for vehicles;

[0012] The equation establishing module, based on the relative distance and the relative azimuth angle, establishes the combined navigation state equation and the observation equation of the agricultural machine A;

[0013] The positioning module, 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 agricultural machine B through the DSRC to realize high-precision positioning of the double agricultural machine cooperative operation.

[0014] The application also provides an electronic device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the double agricultural machine cooperative operation high-precision positioning method.

[0015] The application also provides a non-transitory computer readable storage medium having a computer program stored thereon, wherein the computer program is executable on a processor to implement the steps of the double agricultural machine cooperative operation high-precision positioning method.

[0016] Advantages:

[0017] The application is suitable for the case that the GNSS signal is deteriorated or part of the sensor is faulty when the unmanned agricultural machines are cooperatively operated, the relative distance and the azimuth angle information measured in real time between the agricultural machines are introduced, the relative position observation model is constructed, and the double agricultural machine state is cooperatively estimated through the distributed Kalman filtering architecture, so that the precision, the robustness and the fault tolerance of the navigation system in the complex farmland environment are significantly improved. BRIEF DESCRIPTION OF DRAWINGS

[0018] Figure 1 The application is a flowchart of a double agricultural machine cooperative operation high-precision positioning method.

[0019] Figure 2 It is a GNSS / INS combination and GNSS / INS / DCRS combination navigation error comparison chart.

[0020] Figure 3It is a schematic view of a double-tractor cooperative work high-precision positioning device of the present application. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical solutions and advantages of the present application more clear, the present application is further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the present application and do not limit the present application. In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.

[0022] As shown in Figure 1 , the present application provides a double-tractor cooperative work high-precision positioning method, comprising the following steps:

[0023] S1: The cooperative work tractors are defined as tractor A and tractor B, and the relative distance and the relative azimuth angle between tractor A and tractor B are calculated according to the path loss model in the DSRC transmission process;

[0024] S2: The combined navigation state equation and the observation equation of tractor A are established according to the relative distance and the relative azimuth angle obtained in S1;

[0025] S3: The extended Kalman filter update is performed according to the combined navigation state equation and the observation equation, the error compensation feedback is performed, and the position information, the position error and the speed error information are sent to tractor B through DSRC.

[0026] Further, the S1 comprises:

[0027] S1.1 After the DSRC receiving device of tractor A receives the wireless communication signal of the DSRC sending device of tractor B, the signal power is calculated;

[0028] S1.2 According to the Shadowing model (i.e. the path loss model) in radio ranging, the relative distance d between the DSRC receiving device of tractor A and the DSRC sending device of tractor B is calculated:

[0029] ;

[0030] wherein, d is the relative distance (unit: m) between the receiving device and the sending device, is the reference distance, is the path dissipation index; is the signal power accepted by the receiving device, is a Gaussian random variable.

[0031] Further, the S2 comprises:

[0032] S2.1 The DSRC receiving device of the agricultural machine A parses the data information transmitted by the DSRC transmitting device of the agricultural machine B, and obtains the position, position error state, speed error state and other parameters of the agricultural machine B;

[0033] S2.2 Based on the relative distance provided in S1 DSRC / INS combined Kalman filter design is performed.

[0034] The 21-dimensional error state vector of the Kalman filter is defined As follows:

[0035] ;

[0036] Wherein, is the three-dimensional position error vector of the agricultural machine A, is the three-dimensional speed error vector of the agricultural machine A, is the three-dimensional attitude error vector of the agricultural machine A, is the three-axis gyro zero bias of the agricultural machine A, is the three-axis acceleration zero bias of the agricultural machine A, is the three-dimensional position error vector of the agricultural machine B, is the three-dimensional speed error vector of the agricultural machine B, and the superscript T represents the transpose of the matrix; part of the error state of the agricultural machine B is transmitted through the DSRC;

[0037] S2.3 The continuous DSRC / GNSS / INS combined Kalman filter error state equation (i.e. combined navigation state equation) is established:

[0038] ;

[0039] Wherein, represents the differential of the error state vector; is the state transition matrix, and is a 21 21-dimensional matrix, and the elements thereof are established according to the inertial navigation error differential equation; is the process noise matrix, is the process noise, which is assumed to be zero-mean Gaussian white noise.

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

[0041] ;

[0042] Wherein, is the measurement variable, and is taken , is the difference between the strapdown calculated position of the agricultural machine A and the GNSS position; is the difference between the strapdown calculated position of the agricultural machine B and the GNSS position, which is obtained through the DSRC transmission; and x is the input variable of the observation equation. is the relative distance error, the relative distance calculated by the inertial navigation strapdown solution position and the distance between the receiving device and the sending device is calculated. is the measurement matrix, the elements of which are , wherein is a 3 unit matrix with diagonal elements being 1, is a unit directional vector, and the calculation method is , is the three-dimensional position of the agricultural machine A, is the three-dimensional position of the agricultural machine B. is a matrix with m n elements of 0, m=1,3, n=3,6. is the measurement noise variable, which is assumed to be a zero-mean Gaussian white noise.

[0043] Further, the S3 comprises:

[0044] S3.1 Based on the combined navigation state equation and observation equation established in S2, the extended Kalman filter update solution is carried out to obtain the optimal estimation value of the error state vector in real time.

[0045] S3.2 The error state vector is compensated, and the compensated position information, position error information and speed error information are sent to the agricultural machine B through the DSRC for the agricultural machine B to complete the joint position estimation filtering.

[0046] Embodiment:

[0047] A straight line path for the agricultural machine to walk during the operation is generated by simulation, and the noise of the output data is set as: the gyro zero offset is 0.1 , the accelerometer zero offset is 0.1mg, the simulation data is 10 minutes long, the GNSS data is normal for the first 5 minutes, the GNSS data signal quality is poor from 5 minutes to 6 minutes, and the GNSS data is restored to normal after 6 minutes. The simulation data is filtered and corrected by the above method, and the eastward position error after processing is compared by GNSS / INS combination and GNSS / INS / DCRS combination navigation, and the curve as shown in Figure 2 is obtained. After filtering, the DCRS combination can effectively correct the position error during the GNSS signal deterioration.

[0048] As shown in Figure 3 , the present application provides a high-precision positioning device for double agricultural machine cooperative operation, which is used to realize the above method, and comprises the following modules:

[0049] The relative azimuth angle calculation module, the double agricultural machines include agricultural machine A and agricultural machine B, the agricultural machine A receives the wireless signal sent by the agricultural machine B through the DSRC, and calculates the relative distance and the relative azimuth angle of the agricultural machine B; the DSRC represents the automobile special short distance communication;

[0050] The equation establishing module establishes the combined navigation state equation and the observation equation of the agricultural machine A based on the relative distance and the relative azimuth angle;

[0051] The positioning module updates and solves the navigation state by using the extended Kalman filtering algorithm, and sends the positioning information compensated by the error to the agricultural machine B through the DSRC, so that the high-precision positioning of the double agricultural machines is realized.

[0052] The application further provides an electronic device, including a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor realizes the steps of the double agricultural machine cooperative operation high-precision positioning method when executing the program.

[0053] The application further provides a non-transient computer readable storage medium, which stores a computer program, and the computer program realizes the steps of the double agricultural machine cooperative operation high-precision positioning method when executed by the processor.

[0054] Those skilled in the art should understand that the embodiments of the application can be provided as a method, a system or a computer program product. Therefore, the application can adopt a completely hardware embodiment, a completely software embodiment or an embodiment combining software and hardware aspects. Moreover, the application can adopt a computer program product in the form of one or more computer usable storage media containing computer usable program codes (including but not limited to disk storage, CD-ROM, optical storage, etc.). The solutions in the embodiments of the application can be implemented in various computer languages, for example, object-oriented programming language Java and direct script language JavaScript.

Claims

1. A double-tractor cooperative operation high-precision positioning method, the double-tractor including a tractor A and a tractor B, characterized in that, The method comprises the following steps: S1, the agricultural machine A receives the wireless signal sent by the agricultural machine B through DSRC, and calculates the relative distance and relative azimuth angle with the agricultural machine B; DSRC represents car special short distance communication; S2, based on the relative distance and relative azimuth angle, the combined navigation state equation and observation equation of the agricultural machine A are established; S3, the navigation state is updated and calculated by using the extended Kalman filter algorithm, and the positioning information after error compensation is sent to the agricultural machine B through DSRC, so that the high-precision positioning of the double agricultural machine cooperative operation is realized.

2. The double-tractor cooperative operation high-precision positioning method according to claim 1, characterized in that, The S1 comprises: S1.1, after the DSRC receiving device of the agricultural machine A receives the wireless communication signal of the DSRC sending device of the agricultural machine B, the signal power is calculated.

3. The double-tractor cooperative operation high-precision positioning method according to claim 2, characterized in that, The S1 further comprises: S1.2, according to the path loss model in radio ranging, the relative distance between the DSRC receiving device of the agricultural machine A and the DSRC sending device of the agricultural machine B is calculated.

4. The double-tractor cooperative operation high-precision positioning method according to claim 3, characterized in that, The S2 comprises: S2.1, the DSRC receiving device of the agricultural machine A analyzes the data information sent by the DSRC sending device of the agricultural machine B, and obtains the position information, position error information and speed error information of the agricultural machine B; S2.2, the DSRC / INS combined Kalman filter is designed based on the relative distance of S1.2; INS represents inertial navigation system.

5. The double-tractor cooperative operation high-precision positioning method according to claim 4, characterized in that, The S2 comprises: S2.3, the continuous DSRC / GNSS / INS combined Kalman filter error state equation, that is, the combined navigation state equation, is established; GNSS represents global navigation satellite system; S2.4, the observation equation is constructed based on the difference between the strapdown calculated position and GNSS position of the agricultural machine A and the agricultural machine B, and the relative distance error.

6. The double-tractor cooperative operation high-precision positioning method according to claim 5, characterized in that, The S3 comprises: S3.1, based on the combined navigation state equation and observation equation established in S2, the extended Kalman filter is updated and calculated, and the optimal estimation value of the state vector is obtained in real time.

7. The double-tractor cooperative operation high-precision positioning method according to claim 6, characterized in that, The S3 further comprises: S3.2, the error state vector of the DSRC / GNSS / INS combined Kalman filter error state equation is compensated, and the compensated position information, position error information and speed error information are sent to the agricultural machine B through DSRC, so that the agricultural machine B completes the joint position estimation filtering.

8. A high-precision positioning device for cooperative operation of double agricultural machines, the double agricultural machines comprising an agricultural machine A and an agricultural machine B, characterized in that, The method comprises the following modules: The relative azimuth angle calculation module, the agricultural machine A receives the wireless signal sent by the agricultural machine B through DSRC, and calculates the relative distance and relative azimuth angle with the agricultural machine B; DSRC represents car special short distance communication; The equation establishing module, based on the relative distance and relative azimuth angle, the combined navigation state equation and observation equation of the agricultural machine A are established; The positioning module, the navigation state is updated and calculated by using the extended Kalman filter algorithm, and the positioning information after error compensation is sent to the agricultural machine B through DSRC, so that the high-precision positioning of the double agricultural machine cooperative operation is realized.

9. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the program to realize the steps of the double agricultural machine cooperative operation high-precision positioning method in any one of claims 1 to 7. 10.A non-transitory computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to realize the steps of the double agricultural machine cooperative operation high-precision positioning method in any one of claims 1 to 7.

Images