CN110057364B - Paddy field tractor pose detection and yaw angle extraction method and device - Google Patents

Abstract

The invention discloses a method and device for detecting the position and attitude of a paddy field tractor and extracting a yaw angle. S1. During the initial operation of the paddy field tractor, an initial camera module will photograph the running track of the paddy field tractor, and at this time, the first running track feature is collected The module collects the running track features of the paddy field tractor according to the camera, so that the standard running track determination module determines its running track, and the invention relates to the technical field of agricultural equipment. The method and device for detecting the position and attitude of the paddy field tractor and extracting the yaw angle can well detect the position and attitude of the paddy field tractor through the gyroscope. The user can check the posture and yaw angle of the paddy tractor in time through the display screen. After the paddy tractor yaw, it can be automatically corrected so that it can quickly return to the normal track, which is convenient for the operator to use. .

Description

A method and device for posture detection and yaw angle extraction of paddy field tractor

technical field

The invention relates to the technical field of agricultural equipment, in particular to a method and a device for detecting the position and attitude of a paddy field tractor and extracting a yaw angle.

Background technique

Tractors are self-propelled power machines used to haul and drive operating machinery to complete various mobile operations, and can also be used as fixed operating power. It is composed of a system or device. The power of the engine is transmitted from the transmission system to the driving wheel to make the tractor run. In real life, rubber belts are commonly used as the medium of power transmission. According to their functions and uses, they are divided into agricultural, industrial, and special-purpose tractors. According to the structure type, it is divided into wheel type, crawler type, boat tractor and self-propelled chassis.

With the rapid development of agricultural mechanization, the demand for high-precision navigation technology is also increasing. When the paddy tractor is running, because the tractor is manually controlled, the tractor often deviates from the normal track, so the field work cannot be accurate. , completed efficiently, so it is very necessary to extract the pose and yaw angle of the paddy tractor.

SUMMARY OF THE INVENTION

(1) Technical problems solved

Aiming at the deficiencies of the prior art, the present invention provides a method and device for detecting the pose and yaw angle of a paddy field tractor, which solves the problem that the paddy field tractor does not perform real-time extraction of its pose and yaw angle during operation, resulting in inability to The problem of being able to complete field work accurately and efficiently.

(2) Technical solutions

In order to achieve the above purpose, the present invention proposes a method for detecting the position and attitude of a paddy field tractor and extracting the yaw angle, including the following steps:

S1. During the initial operation of the paddy tractor, the initial camera module will photograph the running track of the paddy tractor. At this time, the first running track feature acquisition module will collect the running track features of the paddy tractor according to the camera, so that the standard running track determination module Determine the running trajectory as the standard trajectory of the paddy field tractor in the later period of operation. After the standard running trajectory determination module is completed, it will be sent to the central processing module;

S2. When the paddy tractor is in normal operation, the running camera module will record the real-time running track of the paddy tractor. At this time, the second running track feature acquisition module will collect the running track features of the paddy tractor according to the camera, so as to determine the real-time running track. The module determines the running trajectory as the real-time running trajectory of the paddy tractor, and will send it to the central processing module after the real-time running trajectory determination module is completed;

S3. After the standard trajectory and real-time trajectory of the paddy tractor are determined, the central processing module will send both the standard trajectory and the real-time trajectory to the yaw angle extraction system. At this time, the trajectory comparison module in the yaw angle extraction system will compare the standard trajectory and the real-time trajectory to the yaw angle extraction system. The trajectory and the real-time trajectory are compared. If the standard trajectory and the real-time trajectory do not match, the yaw angle calculation module in the yaw angle extraction system will calculate the yaw angle to obtain the final yaw angle;

S4. After the yaw angle is calculated by the yaw angle calculation module, the yaw angle data will be sent to the data receiving module and the driving wheel steering analysis module in the driving wheel control system respectively. At this time, the driving wheel steering analysis module will analyze the driving The wheel returns to the angle to be turned by the standard trajectory, so that the driving wheel steering control module will control the steering angle of the driving wheel according to the analysis result;

S5. When the paddy tractor is running, the gyroscope will monitor its position and attitude, and the gyroscope will send the real-time monitoring data to the data receiving module through the central processing module, so that the data receiving module will receive the received position and attitude information and yaw. The angle data is displayed on the display.

Preferably, the output end of the gyroscope is connected with the input end of the central processing module, the output end of the central processing module is connected with the input end of the data receiving module, and the output end of the data receiving module is connected with the input end of the display screen .

Preferably, the output end of the initial camera module is connected with the input end of the first running track feature acquisition module, the output end of the first running track feature acquisition module is connected with the input end of the standard running track determination module, and the standard running track The output terminal of the trajectory determination module is connected with the input terminal of the central processing module.

Preferably, the output end of the running camera module is connected to the input end of the second running track feature collection module, the output end of the second running track feature collection module is connected to the input end of the real-time running track determination module, and the real-time running track The output terminal of the trajectory determination module is connected with the input terminal of the central processing module.

Preferably, the output end of the central processing module is connected to the input end of the yaw angle extraction system, and the output end of the yaw angle extraction system is respectively connected to the input end of the data receiving module and the driving wheel control system, and the driving wheel The output end of the control system is connected with the input end of the driving wheel.

Preferably, the yaw angle extraction system includes a trajectory comparison module and a yaw angle calculation module, the output end of the trajectory comparison module is connected to the input end of the yaw angle calculation module, and the output end of the yaw angle calculation module is connected to the yaw angle calculation module. The input end of the driving wheel control system is connected, and the input end of the yaw angle calculation module is connected with the input end of the data receiving module.

Preferably, the driving wheel control system includes a driving wheel steering analysis module and a driving wheel steering control module, an output end of the driving wheel steering analysis module is connected to an input end of the driving wheel steering control module, and an output end of the driving wheel steering control module is The output end is connected with the input end of the driving wheel, and the input end of the steering analysis module of the driving wheel is connected with the output end of the yaw angle calculation module.

The invention also discloses a device for detecting posture and yaw angle of a paddy field tractor, comprising a tractor frame, the top and bottom of both sides of the tractor frame are steered and connected with driving wheels, and the lower part of the surface of the tractor frame is connected with driving wheels. A driving wheel steering control module is fixedly connected, and a gyroscope is fixedly connected to the surface of the tractor frame and located directly above the driving wheel steering control module, and the surface of the tractor frame is located above the gyroscope. The connection frame is fixedly connected There is a display screen, and two sides of the top of the tractor frame are respectively fixedly connected with an initial camera module and a running camera module.

Preferably, the second operating trajectory feature collection module in the S2-S3 will collect the operating trajectory features of the paddy field tractor according to the camera, and the yaw angle calculation module in the yaw angle extraction system will calculate the yaw angle to obtain The specific steps to obtain the final yaw angle are as follows:

First, each frame in the camera video obtained by the initial camera module taking pictures of the running track of the paddy field tractor is saved as an image to form an image database, then there are P images in the image database and saved the trajectory feature corresponding to the image;

Then match the image captured by the operating camera module in real time with the image in the image database, and the specific matching method is as follows:

Extract the pixels of the real-time captured image to obtain a pixel matrix A, which includes L rows and M columns. At the same time, because the pixels contain three values of RGB, each element in the pixel matrix A contains A set of 3 values, using formula (1) to process each element in the pixel matrix A as a grayscaled pixel matrix B with only one value,

Among them, B _it is the grayscale value of the ith row and t column of the pixel matrix A, AR _it is the R value of the pixel point in the ith row and t column of the pixel matrix A, and AG _it is the pixel point in the ith row and t column of the pixel matrix A. The G value of , AB _it is the B value of the pixel point in the i-th row and t-column of pixel matrix A, i=1, 2, 3...L, t=1, 2, 3...M, all elements are grayscaled After the matrix B is formed, the corresponding singular eigenvalues of the matrix B are calculated by formula (2),

|B*B ^T -λE|=0

Among them, B is the grayscaled matrix B, B ^T is the transpose of matrix B, E is the L-order unit matrix, λ is the value solved in the middle, σ is the singular eigenvalue of the final solution, and the solved feature The value is L values, and the L values are sorted from large to small to form a vector C;

Then, the values of the pixel points of each image in the image database are obtained respectively to form a corresponding pixel matrix Dj, where Dj is a matrix with L rows and M columns, and then the pixel matrix Dj is grayed out using formula (1). , obtain the corresponding grayscale matrix DDj, for each grayscale matrix, use formula (2) to calculate the corresponding singular eigenvalues and sort them from large to small to form the corresponding vector Ej, and finally put all the The Ej form a matrix F, where the matrix F is a matrix of P rows and L columns, and each row represents the singular eigenvalues of an image in an image database;

Then use formula (3) to calculate the singular eigenvalue distance between vector C and each row in matrix F:

Wherein, ρ _j is the singular eigenvalue distance between the image captured in real time and the jth image in the image database, e is a natural constant, C is the vector C, C _i is the ith value of the vector C, F _ji is the value of the jth row and the ith column of the matrix F, F _j ^T is the transpose of the jth row of the matrix F, L is the number of values in each row of the matrix F, i=1, 2, 3...L , j=1, 2, 3...P, extract the minimum value ρ _S in all ρ _j to be solved, S is less than or equal to j, and the S th image in the image database corresponding to the minimum value is The image closest to the real-time captured image, the running track feature corresponding to the S-th image is the running track feature corresponding to the real-time captured image;

The running trajectory features determined by the S-2, S-1, and S images are transmitted to the real-time running trajectory determination module, and the real-time trajectory determined by the real-time running trajectory determination module and the standard running trajectory determination module are determined by the trajectory comparison module. The standard trajectories are compared to determine whether the yaw is yaw, if not, continue to run; if it is yaw, use the yaw angle calculation module in the yaw angle extraction system to calculate the yaw angle. The running track feature determined by the S-2 images is the origin, the due east direction is the X axis, and the due north direction is the Y axis, and the coordinates of the running track features of the corresponding S-1 and S images are obtained, using formula (4) Calculate the yaw angle:

Among them, tt is the intermediate parameter, [S-1] _x is the abscissa of the running track feature determined by the S-1 image, [S-1] _y is the ordinate of the running track feature determined by the S-1 image , [S] _x is the abscissa of the running track feature determined by the S-th image, [S] _y is the ordinate of the running-track feature determined by the S-th image, arcsec(tt) is the arc cosine of tt, and θ is The solved yaw angle.

(3) Beneficial effects

The invention provides a method and a device for detecting the position and attitude of a paddy field tractor and extracting the yaw angle. Compared with the prior art, it has the following beneficial effects:

(1) The method and device for detecting the position and attitude of the paddy tractor and extracting the yaw angle, through S1, during the initial operation of the paddy tractor, the initial camera module will take a picture of the running track of the paddy tractor, S2, when the paddy tractor is running normally , the running camera module will take pictures of the real-time running track of the paddy tractor. After S3 and the standard track and real-time track of the paddy tractor are determined, the central processing module will send both the standard track and the real-time track to the yaw angle extraction system. The output end of the gyroscope is connected with the input end of the central processing module, the output end of the central processing module is connected with the input end of the data receiving module, and the output end of the data receiving module is connected with the input end of the display screen. The posture and posture of the paddy field tractor have been well detected. By comparing the standard trajectory with the real-time trajectory, the yaw angle of the paddy tractor can be obtained. The user can check the posture and yaw angle of the paddy tractor in time through the display screen, so The user can adjust the tractor in time according to the posture and yaw angle, so that the paddy field tractor can operate efficiently and accurately.

(2) The method and device for posture detection and yaw angle extraction of the paddy field tractor, after calculating the yaw angle through the yaw angle calculation module of S4, the yaw data will be sent to the data receiving module and the driving wheel control respectively. The driving wheel steering analysis module in the system, the driving wheel control system includes a driving wheel steering analysis module and a driving wheel steering control module, the output end of the driving wheel steering analysis module is connected with the input end of the driving wheel steering control module, and the driving wheel steering control module The output end of the module is connected to the input end of the drive wheel, and the input end of the drive wheel steering analysis module is connected to the output end of the yaw angle calculation module. Return to the normal track, which is convenient for the operator to use.

(3) The method and device for detecting the position and attitude of the paddy tractor and extracting the yaw angle are connected with driving wheels through the top and bottom of both sides of the tractor frame, and the driving wheel steering control module is fixedly connected below the surface of the tractor frame. , a gyroscope is fixedly connected to the surface of the tractor frame and located directly above the steering control module of the driving wheel, and a display screen is fixedly connected to the surface of the tractor frame and located above the gyroscope through the connecting frame, and the two sides of the top of the tractor frame are respectively The initial camera module and the running camera module are fixedly connected, and the components are centrally installed on the frame, which is convenient for later maintenance.

Description of drawings

Fig. 1 is the structural principle block diagram of the system of the present invention;

Fig. 2 is the structural principle block diagram of the yaw angle extraction system of the present invention;

Fig. 3 is the structural principle block diagram of the driving wheel control system of the present invention;

4 is a top view of the structure of the tractor frame, gyroscope and display screen of the present invention.

In the figure, 1. Initial camera module; 2. First running track feature collection module; 3. Standard running track determination module; 4. Central processing module; 5. Running camera module; 6. Second running track feature collection module; 7 , Real-time running trajectory determination module; 8. Yaw angle extraction system; 9. Data receiving module; 10. Driving wheel control system; 11. Driving wheel; 12. Gyroscope; 13. Display screen; 14. Tractor frame; 101 , drive wheel steering analysis module; 102, drive wheel steering control module; 801, trajectory comparison module; 802, yaw angle calculation module.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

Referring to FIGS. 1-4, an embodiment of the present invention provides a technical solution: a method for detecting the position and attitude of a paddy field tractor and extracting the yaw angle, including the following steps:

S1. During the initial operation of the paddy tractor, the initial camera module 1 will photograph the running track of the paddy tractor. At this time, the first running track feature collection module 2 will collect the running track features of the paddy tractor according to the camera, so as to standardize the running track. The determination module 3 determines the running track as the standard track for the later operation of the paddy tractor. After the standard running track determination module 3 is determined, it will be sent to the central processing module 4;

S2. When the paddy tractor is running normally, the running camera module 5 will record the real-time running track of the paddy tractor. At this time, the second running track feature acquisition module 6 will collect the running track features of the paddy tractor according to the camera, so that the real-time running The trajectory determination module 7 determines the running trajectory as the real-time trajectory of the paddy field tractor's operation, and will send it to the central processing module 4 after the real-time running trajectory determination module 7 completes the determination;

S3. After the standard trajectory and real-time trajectory of the paddy tractor are determined, the central processing module 4 will send both the standard trajectory and the real-time trajectory to the yaw angle extraction system 8. At this time, the trajectory comparison module in the yaw angle extraction system 8 801 will compare the standard trajectory and the real-time trajectory. If the standard trajectory and the real-time trajectory do not match, the yaw angle calculation module 802 in the yaw angle extraction system 8 will calculate the yaw angle to obtain the final yaw angle. horn;

S4. After the yaw angle is calculated by the yaw angle calculation module 802, the yaw angle data will be sent to the data receiving module 9 and the driving wheel steering analysis module 101 in the driving wheel control system 10 respectively. At this time, the driving wheel steering analysis is performed. The module 101 will analyze the steering angle of the driving wheel 11 to return to the standard trajectory, so that the driving wheel steering control module 102 will control the steering angle of the driving wheel 11 according to the analysis result;

S5. When the paddy field tractor is running, the gyroscope 12 will monitor its position and attitude, and the gyroscope 12 will send the real-time monitoring data to the data receiving module 9 through the central processing module 4, so that the data receiving module 9 will receive the position and attitude. Attitude information and yaw angle data are displayed on the display screen 13 .

In the present invention, the output end of the gyroscope 12 is connected with the input end of the central processing module 4, the output end of the central processing module 4 is connected with the input end of the data receiving module 9, and the output end of the data receiving module 9 is connected with the input end of the display screen 13. input connection.

In the present invention, the output end of the initial camera module 1 is connected with the input end of the first running track feature collection module 2, the output end of the first running track feature collection module 2 is connected with the input end of the standard running track determination module 3, and the standard The output end of the running track determination module 3 is connected to the input end of the central processing module 4 .

In the present invention, the output end of the running camera module 5 is connected with the input end of the second running track feature collection module 6, the output end of the second running track feature collection module 6 is connected with the input end of the real-time running track determination module 7, and the real-time The output end of the running track determination module 7 is connected to the input end of the central processing module 4 .

In the present invention, the output end of the central processing module 4 is connected with the input end of the yaw angle extraction system 8, and the output end of the yaw angle extraction system 8 is respectively connected with the input end of the data receiving module 9 and the driving wheel control system 10, and The output end of the drive wheel control system 10 is connected to the input end of the drive wheel 11 .

In the present invention, the yaw angle extraction system 8 includes a trajectory comparison module 801 and a yaw angle calculation module 802. The output end of the trajectory comparison module 801 is connected to the input end of the yaw angle calculation module 802, and the yaw angle calculation module 802 has a The output end is connected with the input end of the driving wheel control system 10 , and the output end of the yaw angle calculation module 802 is connected with the input end of the data receiving module 9 .

In the present invention, the driving wheel control system 10 includes a driving wheel steering analysis module 101 and a driving wheel steering control module 102. The output end of the driving wheel steering analysis module 101 is connected to the input end of the driving wheel steering control module 102, and the driving wheel steering controls The output end of the module 102 is connected with the input end of the driving wheel 11 , and the input end of the driving wheel steering analysis module 101 is connected with the output end of the yaw angle calculation module 802 .

The invention also discloses a device for detecting the position and attitude of a paddy field tractor and extracting the yaw angle. A driving wheel steering control module 102 is fixedly connected, and a gyroscope 12 is fixedly connected to the surface of the tractor frame 14 and is located directly above the driving wheel steering control module 102. The space runs the detection device around the angle of one or two axes orthogonal to the axis of rotation. The surface of the tractor frame 14 and above the gyroscope 12 are fixedly connected with the display screen 13 through the connecting frame. The two sides of the top of the tractor frame 14 The initial camera module 1 and the running camera module 5 are fixedly connected respectively.

In the present invention,

The second operating trajectory feature collection module 6 in the S2-S3 will collect the operating trajectory features of the paddy field tractor according to the camera, and the yaw angle calculation module 802 in the yaw angle extraction system 8 will calculate the yaw angle. The specific steps to obtain the final yaw angle are as follows:

First, each frame of the camera video obtained by the initial camera module 1 taking pictures of the running track of the paddy tractor is saved as an image to form an image database, then there are P images in the image database, and save the trajectory feature corresponding to the image;

Then match the image captured by the operating camera module 5 in real time with the image in the image database, and the specific matching method is as follows:

Extract the pixels of the real-time captured image to obtain a pixel matrix A, which includes L rows and M columns. At the same time, because the pixels contain three values of RGB, each element in the pixel matrix A contains A set of 3 values, using formula (1) to process each element in the pixel matrix A as a grayscaled pixel matrix B with only one value,

Among them, B _it is the grayscale value of the ith row and t column of the pixel matrix A, AR _it is the R value of the pixel point in the ith row and t column of the pixel matrix A, and AG _it is the pixel point in the ith row and t column of the pixel matrix A. The G value of , AB _it is the B value of the pixel point in the ith row and t column of the pixel matrix A, i=1, 2, 3...L, t=1, 2, 3...M, all elements are grayscaled After forming matrix B,

Using formula (1), the three values of each element in the pixel matrix of the picture can be converted into one value, which avoids the situation of excessive calculation. Different weights are given respectively during grayscale, so that the amount of information lost by the grayscaled pixels is the least compared to the original pixel matrix.

Calculate the corresponding singular eigenvalues of matrix B using formula (2),

|B*B ^T -λE|=0

Among them, B is the grayscaled matrix B, B ^T is the transpose of matrix B, E is the L-order unit matrix, λ is the value solved in the middle, σ is the singular eigenvalue of the final solution, and the solved feature The value is L values, and the L values are sorted from large to small to form a vector C;

Using formula (2) can change the original need to study a relatively complex matrix to study only one vector, which greatly reduces the amount of subsequent calculations, and formula (2) can overcome the problem of finding When the matrix of eigenvalues is not a square matrix and the corresponding eigenvalues cannot be solved, the corresponding singular eigenvalues can be solved for any matrix by formula (2), and the singular eigenvalues are sorted from large to small, so that the vector The value of the front vector contains more information than the value of the latter vector. When calculating in formula (3), the amount of information that may be discarded is less, and the calculation of the correlation degree is more accurate.

Then, the values of the pixel points of each image in the image database are obtained respectively to form a corresponding pixel matrix Dj, where Dj is a matrix with L rows and M columns, and then the pixel matrix Dj is grayed out using formula (1). , obtain the corresponding grayscale matrix DDj, for each grayscale matrix, use formula (2) to calculate the corresponding singular eigenvalues and sort them from large to small to form the corresponding vector Ej, and finally put all the The Ej form a matrix F, where the matrix F is a matrix of P rows and L columns, and each row represents the singular eigenvalues of an image in an image database;

Then use formula (3) to calculate the singular eigenvalue distance between vector C and each row in matrix F:

Wherein, ρ _j is the singular eigenvalue distance between the image captured in real time and the jth image in the image database, e is a natural constant, C is the vector C, C _i is the ith value of the vector C, F _ji is the value of the jth row and the ith column of the matrix F, F _j ^T is the transpose of the jth row of the matrix F, L is the number of values in each row of the matrix F, i=1, 2, 3...L , j=1, 2, 3...P, extract the minimum value ρ _S in all ρ _j to be solved, S is less than or equal to j, and the S th image in the image database corresponding to the minimum value is The image closest to the real-time captured image, the running track feature corresponding to the S-th image is the running track feature corresponding to the real-time captured image;

When using formula (3) to solve the distance of singular eigenvalues, according to the characteristics of the different sizes of singular eigenvalues in the same matrix, the amount of information contained is different. The weight of , so that the weight containing more information has a greater impact on the distance.

The running trajectory features determined by the S-2, S-1, and S images are transferred to the real-time running trajectory determining module 7, and the real-time running trajectory determined by the real-time running trajectory determining module 7 and the standard running trajectory are determined by the trajectory comparison module 801. The standard trajectories determined by the module 3 are compared to determine whether the yaw is yaw, and if not, the operation is continued; , the calculation step is to take the running trajectory feature determined by the S-2 image as the origin, the due east direction is the X axis, and the due north direction is the Y axis, and the coordinates of the corresponding running trajectory features of the S-1 and S images are obtained. , use formula (4) to calculate the yaw angle:

Among them, tt is the intermediate parameter, [S-1] _x is the abscissa of the running track feature determined by the S-1 image, [S-1] _y is the ordinate of the running track feature determined by the S-1 image , [S] _x is the abscissa of the running track feature determined by the S-th image, [S] _y is the ordinate of the running-track feature determined by the S-th image, arcsec(tt) is the arc cosine of tt, and θ is The solved yaw angle.

When the above-mentioned technology is used to determine the running trajectory features, it is possible to pass only the video captured by the initial camera module 1 and the pictures captured by the running camera module without using an external database or human intervention. Some simple operations can accurately determine the characteristics of the running trajectory, so that the real-time running trajectory determination module 7 can determine the running trajectory, and at the same time judge whether there is a yaw through the determination of the trajectory, and automatically calculate and adjust the yaw angle if there is a yaw. .

It should be noted that, in this document, relational terms such as first and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any relationship between these entities or operations. any such actual relationship or sequence exists. Moreover, the terms "comprising", "comprising" or any other variation thereof are intended to encompass a non-exclusive inclusion such that a process, method, article or device that includes a list of elements includes not only those elements, but also includes not explicitly listed or other elements inherent to such a process, method, article or apparatus.

Although embodiments of the present invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, and substitutions can be made in these embodiments without departing from the principle and spirit of the invention and modifications, the scope of the present invention is defined by the appended claims and their equivalents.

Claims (6)

1. a paddy field tractor pose detection and yaw angle extraction method, is characterized in that: specifically comprise the following steps: S1. During the initial operation of the paddy tractor, the initial camera module (1) will photograph the running trajectory of the paddy tractor. At this time, the first running trajectory feature acquisition module (2) will collect the running trajectory characteristics of the paddy tractor according to the camera. Therefore, the standard running trajectory determination module (3) determines the running trajectory as a standard trajectory for the later operation of the paddy tractor, and will send it to the central processing module (4) after the standard running trajectory determining module (3) is determined; S2. When the paddy tractor is in normal operation, the running camera module (5) will photograph the real-time running track of the paddy tractor, and at this time, the second running track feature collection module (6) will collect the running track features of the paddy tractor according to the camera. , so that the real-time running trajectory determining module (7) determines the running trajectory as the real-time running trajectory of the paddy tractor, and will send it to the central processing module (4) after the real-time running trajectory determining module (7) is determined; S3. After the standard trajectory and real-time trajectory of the paddy tractor are determined, the central processing module (4) will send both the standard trajectory and the real-time trajectory to the yaw angle extraction system (8). At this time, the yaw angle extraction system (8) The trajectory comparison module (801) in ) will compare the standard trajectory and the real-time trajectory. If the standard trajectory and the real-time trajectory do not match, the yaw angle calculation module (802) in the yaw angle extraction system (8) will compare the standard trajectory and the real-time trajectory. The yaw angle is calculated to obtain the final yaw angle; S4. After the yaw angle is calculated by the yaw angle calculation module (802), the yaw angle data are respectively sent to the data receiving module (9) and the driving wheel steering analysis module (101) in the driving wheel control system (10). ), at this time, the driving wheel steering analysis module (101) will analyze the angle to be turned by the driving wheel (11) to return to the standard trajectory, so that the driving wheel steering control module (102) will control the steering of the driving wheel (11) according to the analysis result. angle; S5. When the paddy field tractor is running, the gyroscope (12) will monitor its posture, and the gyroscope (12) will send the real-time monitoring data to the data receiving module (9) through the central processing module (4), so as to receive the data The module (9) will display the received pose information and yaw angle data on the display screen (13); The output end of the gyroscope (12) is connected with the input end of the central processing module (4), the output end of the central processing module (4) is connected with the input end of the data receiving module (9), and the data receiving module ( The output end of 9) is connected with the input end of the display screen (13); The second running track feature acquisition module (6) in the S2-S3 will collect the running track features of the paddy field tractor according to the camera, and the yaw angle calculation module (802) in the yaw angle extraction system (8) will The specific steps of calculating the yaw angle to obtain the final yaw angle are as follows: First, each frame in the camera video obtained by the initial camera module (1) taking pictures of the running track of the paddy tractor is saved as an image to form an image database, then there are P images in the image database , and save the trajectory feature corresponding to the image; Then the image captured in real time by the operating camera module (5) is matched with the image in the image database, and the specific matching method is as follows: Extract the pixels of the real-time captured image to obtain a pixel matrix A, which includes L rows and M columns. At the same time, because the pixels contain three values of RGB, each element in the pixel matrix A contains A set of 3 values, using formula (1) to process each element in the pixel matrix A as a grayscaled pixel matrix B with only one value

Among them, B _it is the grayscale value of the ith row and t column of the pixel matrix A, AR _it is the R value of the pixel point in the ith row and t column of the pixel matrix A, and AG _it is the pixel point in the ith row and t column of the pixel matrix A. The G value of , AB _it is the B value of the pixel point in the ith row and t column of the pixel matrix A, i=1, 2, 3...L, t=1, 2, 3...M, all elements are grayscaled After the matrix B is formed, the corresponding singular eigenvalues of the matrix B are calculated by formula (2), |B*B ^T -λE|=0

Among them, B is the grayscaled matrix B, B ^T is the transpose of matrix B, E is the L-order unit matrix, λ is the value solved in the middle, σ is the singular eigenvalue of the final solution, and the solved feature The value is L values, and the L values are sorted from large to small to form a vector C; Then, the values of the pixel points of each image in the image database are obtained respectively to form a corresponding pixel matrix Dj, where Dj is a matrix with L rows and M columns, and then the pixel matrix Dj is grayed out using formula (1). , obtain the corresponding grayscale matrix DDj, for each grayscale matrix, use formula (2) to calculate the corresponding singular eigenvalues and sort them from large to small to form the corresponding vector Ej, and finally put all the The Ej form a matrix F, where the matrix F is a matrix of P rows and L columns, and each row represents the singular eigenvalues of an image in an image database; Then use formula (3) to calculate the singular eigenvalue distance between vector C and each row in matrix F:

Wherein, ρ _j is the singular eigenvalue distance between the image captured in real time and the jth image in the image database, e is a natural constant, C is the vector C, C _i is the ith value of the vector C, F _ji is the value of the jth row and the ith column of the matrix F, F _j ^T is the transpose of the jth row of the matrix F, L is the number of values in each row of the matrix F, i=1, 2, 3...L , j=1, 2, 3...P, extract the minimum value ρ _S in all ρ _j to be solved, S is less than or equal to j, and the S th image in the image database corresponding to the minimum value is The image closest to the real-time captured image, the running track feature corresponding to the S-th image is the running track feature corresponding to the real-time captured image; The running trajectory features determined by the S-2, S-1, and S images are transmitted to the real-time running trajectory determining module (7), and the real-time running trajectory determined by the real-time running trajectory determining module (7) is analyzed by the trajectory comparison module (801). The trajectory is compared with the standard trajectory determined by the standard running trajectory determination module (3), so as to determine whether it is yaw, if not, continue to run; if it is yaw, use the yaw in the yaw angle extraction system (8) The angle calculation module (802) calculates the yaw angle, and the calculation step is to take the running trajectory feature determined by the S-2 image as the origin, the due east direction is the X axis, and the due north direction is the Y axis, and the corresponding S-th image is obtained. -1. The coordinates of the running track features of the S images, and the yaw angle is calculated by formula (4):

Among them, tt is the intermediate parameter, [S-1] _x is the abscissa of the running track feature determined by the S-1 image, [S-1] _y is the ordinate of the running track feature determined by the S-1 image , [S] _x is the abscissa of the running track feature determined by the S-th image, [S] _y is the ordinate of the running-track feature determined by the S-th image, arcsec(tt) is the arc cosine of tt, and θ is The solved yaw angle. 2. A paddy field tractor pose detection and yaw angle extraction method according to claim 1, characterized in that: the output end of the initial camera module (1) and the output of the first running track feature acquisition module (2) The input end is connected, the output end of the first running track feature acquisition module (2) is connected with the input end of the standard running track determining module (3), and the output end of the standard running track determining module (3) is connected with the central processing module ( 4) input terminal connection. 3. A paddy field tractor pose detection and yaw angle extraction method according to claim 1, characterized in that: the output end of the operating camera module (5) and the output of the second operating trajectory feature acquisition module (6) The input end is connected, the output end of the second running track feature acquisition module (6) is connected with the input end of the real-time running track determination module (7), and the output end of the real-time running track determination module (7) is connected with the central processing module ( 4) input terminal connection. 4. A paddy field tractor posture detection and yaw angle extraction method according to claim 1, characterized in that: the output end of the central processing module (4) and the input end of the yaw angle extraction system (8) connection, the output end of the yaw angle extraction system (8) is respectively connected with the input end of the data receiving module (9) and the driving wheel control system (10), and the output end of the driving wheel control system (10) is connected with the driving wheel (11) input terminal connection. 5. A paddy field tractor posture detection and yaw angle extraction method according to claim 1, characterized in that: the yaw angle extraction system (8) comprises a trajectory comparison module (801) and a yaw angle calculation module (802), the output end of the trajectory comparison module (801) is connected to the input end of the yaw angle calculation module (802), and the output end of the yaw angle calculation module (802) is connected to the output end of the driving wheel control system (10) The input end is connected, and the output end of the yaw angle calculation module (802) is connected with the input end of the data receiving module (9). 6. The method for detecting the pose and yaw angle of a paddy tractor according to claim 1, wherein the driving wheel control system (10) comprises a driving wheel steering analysis module (101) and a driving wheel steering control Module (102), the output end of the driving wheel steering analysis module (101) is connected with the input end of the driving wheel steering control module (102), and the output end of the driving wheel steering control module (102) is connected with the driving wheel (11) The input end of the driving wheel steering analysis module (101) is connected with the output end of the yaw angle calculation module (802).

Images