Construction method and application of agricultural navigation map

Abstract

A kind of construction method of agricultural navigation map and application, the construction method of agricultural navigation map, with latitude and longitude as center, while superimposed crop information generates agricultural fine navigation map, for other vehicle reference travel, including the following steps: agricultural robot adopts vision navigation along crop row of crop to carry out crop row detection, image is collected and after processing obtains image center line and crop row transverse and steering error;Image center line and crop row transverse and steering error are input into MPC controller to follow crop row straight line, record the latitude and longitude coordinates of agricultural robot every time in the process of walking;And to the latitude and longitude coordinates recorded are fitted, output the satellite coordinates after processing and generate agricultural fine navigation map.The application of the agricultural navigation map is also provided.The latitude and longitude of the crop row information introduced in the application generate driving route, which greatly avoids the damage of autonomous vehicle to crops.

Description

TECHNICAL FIELD

[0001] The present application relates to agricultural automatic driving technology, in particular to a more refined plant-centered agricultural navigation map construction method and application which takes crop information as an important reference factor when constructing a satellite navigation map. BACKGROUND

[0002] The agricultural machine automatic driving system is a comprehensive system integrating satellite receiving, positioning and control, mainly composed of a reference station, a GNSS antenna, a Beidou / GNSS M300 receiver, a display, a controller, an angle sensor and the like. The working vehicle travels according to the designed walking route of the position sensor (GNSS satellite navigation system, etc.), and the controller sends instructions in real time according to the coordinates of satellite positioning and the rotation of the wheels to control the travel of the vehicle, so as to ensure that the vehicle travels according to the route set by the navigation display.

[0003] The installation of the automatic driving navigation system is an intelligent device necessary for the intelligentization of agricultural machines, and is a necessary equipment for large agricultural machines, especially large tractors. The satellite positioning navigation scheme is the most mature scheme at present, and the generation mode of the walking route is to delineate the working area and generate a satellite navigation trajectory route with coverage as the standard. This route generation mode does not consider the information of crops, which will destroy crops, and therefore limits the application scenarios of agricultural vehicle automatic driving. SUMMARY

[0004] The present application aims to solve the above-mentioned defects of the prior art, and provides a construction method and application of an agricultural navigation map centered on crops.

[0005] In order to achieve the above-mentioned purpose, the present application provides a construction method of an agricultural navigation map, wherein an agricultural fine navigation map is generated by taking the longitude and latitude as the center and superimposing crop information, and is used for reference travel of other vehicles, including the following steps:

[0006] S100, an agricultural robot travels along a crop row of crops by visual navigation to detect the crop row, collects images and processes the images to obtain the image center line and the lateral and turning errors of the crop row;

[0007] S200, inputting the image center line and the lateral and turning errors of the crop row into an MPC controller to follow the crop row straight line, and recording the longitude and latitude coordinates of the agricultural robot at each moment during the travel; and

[0008] S300, fitting processing the recorded longitude and latitude coordinates, outputting the processed satellite coordinates and generating an agricultural fine navigation map.

[0009] The agricultural navigation map construction method, wherein the step S100 further comprises:

[0010] S101, collecting an image frame by a front camera of the agricultural robot;

[0011] S102, pre-processing the image, extracting crop information and performing feature extraction;

[0012] S103, extracting crop row information by Hough transform; and

[0013] S104, obtaining the lateral and turning errors of the image center line and the crop row.

[0014] The agricultural navigation map construction method, wherein the step S102 further comprises:

[0015] S1021, mathematically modeling the image, adopting a Cartesian coordinate system, taking the upper left corner of the image as the origin, (x, y) as the position of each pixel, R, G and B as the light intensity values of the red, green and blue channels, and each pixel of the image being:

[0016] and

[0017] S1022, converting the image to grayscale by adopting a 2G-R-B conversion algorithm, and converting each pixel of the image to a value between [0, 255].

[0018] The agricultural navigation map construction method, wherein the step S102 further comprises:

[0019] S1023, further removing noise by a threshold filter.

[0020] The agricultural navigation map construction method, wherein the step S102 further comprises:

[0021] S1024, traversing each pixel point horizontally by an edge detection filter, and storing the coordinates of the point as an edge point x1 when detecting that f(x, y) changes from 0 to 255 from left to right, and storing the coordinates of the point as an edge point x2 when detecting that f(x, y) changes from 255 to 0; and

[0022] S1025, obtaining the horizontal coordinate x of the centroid position of each detected object by averaging x1 and x2. c Thus, the crop information is converted into a series of coordinate points (x c , y c ).

[0023] The agricultural navigation map construction method, wherein the step S103 further comprises:

[0024] S1031, Hough transform is performed on all coordinate points (x c , y c ), and converted to a Hough space with rho as the vertical coordinate and theta as the horizontal coordinate, and the conversion formula is as follows:

[0025] rho = x c cos theta + y c sin theta; and

[0026] S1032, the Hough transform converts the points on the image plane into sinusoidal curves in the Hough space, and the edge points that are collinear in the image plane correspond to the intersection of the sinusoidal curves at a fixed point (theta t , rho t ) in the Hough space, so the straight line in the image plane space at this time is:

[0027] y = -ctg theta t x + rho t csc theta t .

[0028] The above method for constructing an agricultural navigation map, wherein step S200 further comprises:

[0029] S201, detecting a plurality of waypoints from the road, and calculating at least three times of interpolation to establish a driving trajectory;

[0030] S202, reading the current speed v, direction psi, steering angle delta and acceleration a of the agricultural robot from the sensor;

[0031] S203, calculating a first vehicle state using the current speed v, direction psi, steering angle delta and acceleration a data and a dynamic model;

[0032] S204, optimizing the control action according to the vehicle state response within a set time, setting the constraint range of the two variables acceleration and steering angle of MPC;

[0033] S205, calculating the dynamic model N times to obtain the system state of the next N time shots;

[0034] S206, giving a loss function for each sampling calculation period;

[0035] S207, using an optimizer to solve the minimum total loss from the first period to the Nth period under the constraint definition;

[0036] S208, selecting the control quantity given by the first period as the optimal control quantity and providing it to the agricultural robot;

[0037] S209, repeating steps S201-S208 to find the next optimal control quantity.

[0038] The agricultural navigation map construction method, wherein the step S300 further comprises:

[0039] S301, the ransac algorithm is used to remove the outliers of the satellite trajectory, and the outliers of the satellite trajectory coordinates are removed through the ransac linear fitting mode; and

[0040] S302, the agricultural navigation map with high-precision satellite trajectory centered on the plant is generated.

[0041] The agricultural navigation map construction method, wherein the step S301 further comprises:

[0042] S3011, a straight line 1 is determined by randomly selecting two points, a threshold t is set, and a data point set with a geometric distance less than t from the straight line 1 is determined;

[0043] S3012, the straight lines l1, l2,..., lK are obtained by repeating K random selections, n and the corresponding data point sets, and all the data point sets are sorted:

[0044]

[0045] wherein Z is the probability of obtaining good samples, and is set to 99%; w is the proportion of the inliers, and is selected according to the positioning accuracy of the satellite; n is the minimum number of points required for model parameter estimation, and at least 2 points are required for linear fitting; and

[0046] S3013, the satellite trajectory points with the most data point sets are selected for navigation, and the points outside the data sets are removed as outliers.

[0047] In order to better achieve the above-mentioned purpose, the application further provides an application of the agricultural navigation map, which is used for the navigation of a farm vehicle, and the farm vehicle uses satellite positioning, wherein the agricultural navigation map is generated by using the above-mentioned agricultural navigation map construction method.

[0048] The technical effect of the application is that:

[0049] The application introduces the latitude and longitude of the crop row information to generate a driving route, which can greatly avoid the damage of the automatic driving vehicle to the crops.

[0050] The application will be described in detail below in combination with the drawings and specific embodiments, but is not limited to the application. BRIEF DESCRIPTION OF DRAWINGS

[0051] Figure 1 It is a working principle diagram of an embodiment of the application. DETAILED DESCRIPTION

[0052] The structural principle and working principle of the present application will be described in detail below with reference to the drawings:

[0053] Referring to Figure 1 , Figure 1 is a working principle diagram of an embodiment of the present application. The agricultural navigation map construction method of the present application generates an agricultural fine navigation map by taking the latitude and longitude as the center and superimposing crop information, which is used for the driving reference of other vehicles, mainly including crop row detection, crop row following by an MPC (modle predictive control) controller and latitude and longitude coordinate optimization, and specifically including the following steps:

[0054] Step S100, the agricultural robot walks along the crop row of the crops by visual navigation to perform crop row detection, collects images and obtains the image center line and the lateral and turning errors of the crop row after processing;

[0055] Step S200, the image center line and the lateral and turning errors of the crop row are input into the MPC controller to follow the crop row straight line, and the latitude and longitude coordinates of the agricultural robot at each moment are recorded during the walking process; and

[0056] Step S300, the recorded latitude and longitude coordinates are fitted and optimized, the processed satellite coordinates are output, and an agricultural fine navigation map is generated.

[0057] In step S100, the crop row detection further includes:

[0058] Step S101, a frame of image is collected by the front camera of the agricultural robot;

[0059] Step S102, the image is preprocessed, the crop information is extracted and feature extraction is performed;

[0060] Step S103, the crop row information is extracted by Hough transform; and

[0061] Step S104, the image center line and the lateral and turning errors of the crop row are obtained.

[0062] In step S102, the image preprocessing further includes:

[0063] Step S1021, the image is mathematically modeled, a Cartesian coordinate system is adopted, the upper left corner of the image is taken as the origin, (x, y) is the position of each pixel point, R, G and B are the light intensity values of the red, green and blue channels, and then each image pixel point can be described as:

[0064]

[0065] Step S1022, the image captured by the camera is then converted to grayscale. Since the agricultural robot is operated outdoors, the change of light will affect the quality of the image. Therefore, a 2G-R-B conversion algorithm is used to convert the pixels of each point of the image to a value between [0, 255]; the 2G-R-B conversion algorithm:

[0066]

[0067] Step S1023, the image at this time still has a lot of noise. In order to remove this part of the noise, a threshold filter can be used to further remove the noise:

[0068]

[0069] In this step, the image is binarized, T min , and T max are the minimum and maximum grayscale thresholds, respectively;

[0070] Step S1024, the image after binarization processing still has a lot of information irrelevant to the extraction of crop rows. Therefore, an edge detection filter can be used to traverse each pixel point horizontally. When f(x, y) changes from 0 to 255 from left to right, the point coordinate is stored as an edge point x1. When it changes from 255 to 0, the point is stored as an edge point x2.

[0071] Step S1025, the average value of x1 and x2 is obtained to obtain the centroid position horizontal coordinate x c of each detection object. Thus, the crop information is converted to a series of points, which are represented as (x c , y c ) in the image coordinate system.

[0072] In step S103, the Hough transform crop row detection further comprises:

[0073] Step S1031, after image preprocessing, the coordinate points (x c , y c ) with crop information have been obtained. At this time, the crops are still distributed in the form of points. Hough transform is performed on all coordinate points (x c , y c ) to convert to a Hough space with p as the vertical coordinate and q as the horizontal coordinate. The specific conversion formula is as follows:

[0074] p = x c cos q + y c sin q; and

[0075] Step S1032, the Hough transform converts the points on the image plane into a sinusoidal curve in the Hough space, and the edge points collinear in the image plane correspond to the sinusoidal curves intersecting at a fixed point (θ t , ρ t ) in the Hough space, and the straight line in the image plane is:

[0076] y = -ctgθ t x + ρ t cscθ t .

[0077] In the straight line following stage, the target is to make the agricultural robot follow the crop row for navigation, and to take the straight line following as the tracking error and steering error. In step S200, further comprising:

[0078] Step S201, detecting a plurality of waypoints from the road, and calculating at least three times of interpolation to establish a driving trajectory;

[0079] Step S202, reading the current speed v, direction ψ, steering angle δ and acceleration a of the agricultural robot from the sensor;

[0080] Step S203, calculating a first vehicle state using the current speed v, direction ψ, steering angle δ and acceleration a data and a dynamic model;

[0081] Step S204, optimizing the control action according to the vehicle state response in a set time (such as 1 second) (the control period is 100 ms, so there are 10 periods in 1 s), setting the constraint range of the two variables (also control quantities) of the acceleration and steering angle of the MPC;

[0082] Step S205, calculating the system state of N (such as 9) future time shots by the dynamic model for N times (such as 9 times);

[0083] Step S206, giving a loss function for each sampling calculation period;

[0084] Step S207, using an optimizer to solve the minimum total loss from the first period to the Nth period (the 9th period) under the constraint definition;

[0085] Step S208, selecting the control quantity given by the first period as the optimal control quantity, and providing it to the agricultural robot; and

[0086] Step S209, repeating steps S201-S208 to find the next optimal control quantity.

[0087] In this embodiment, the trajectory model is a third-order polynomial function fitted by the coordinates of the six waypoints, and the trajectory model is used to predict the y coordinate and the body angle

[0088] y = f(x) = a3x 3 +a2x 2 +a1x+a0;

[0089]

[0090] where a3, a2, a1, a0 are coefficients of the cubic polynomial, is the body angle relative to the x-axis.

[0091] Then create a dynamic model, using the state at time t to predict the state at time t+1, using the vehicle dynamic model can be derived from the latest time of sampling next time position, body angle and speed:

[0092]

[0093]

[0094]

[0095] v t+1 = v t +a t dt;

[0096] In the formula, x, y represents the coordinate position, represents the body angle, v is the vehicle speed, l f is the distance between the steering wheel and the drive wheel, δ represents the input steering angle of the vehicle, a represents the input acceleration of the vehicle.

[0097] At the same time, add the state quantity of trajectory tracking error and body angle error:

[0098]

[0099]

[0100] In the formula, cte is the lateral trajectory tracking error, is the body angle error.

[0101] In MPC, the loss function needs to be defined to optimize the path, and several key points need to be optimized: maintain the target speed, avoid sudden acceleration and deceleration, and suppress the change rate of acceleration and deceleration and steering. Therefore, the loss function is constructed as follows:

[0102]

[0103] where w rcte is the crop row tracking error; w cte is the tracking error term weight; is a steering error term weight; w v is a speed loss term weight; w δ is a steering loss weight; w a is an acceleration loss weight; w rateδ is a steering rate of change weight; w tatea is an acceleration rate of change weight; v target is a target speed; a t is an acceleration.

[0104] The vehicle records the position coordinates at each time during driving by the satellite positioning system carried by it. Since there is an error in the positioning accuracy of the satellite, the trajectory obtained at this time has some outliers, which can be removed by the ransac algorithm. In step S300, the latitude and longitude coordinate optimization further comprises:

[0105] Step S301, the outliers of the satellite trajectory are removed by using the ransac algorithm, and the outliers of the satellite trajectory coordinates are removed by the ransac straight line fitting method; and

[0106] Step S302, an agricultural navigation map with high-precision satellite trajectory centered on the plant is generated.

[0107] Step S301 further comprises:

[0108] Step S3011, randomly select two points, determine a straight line l from the two points, set a threshold t, and determine a data point set whose geometric distance from the straight line l is less than t;

[0109] Step S3012, repeat K random selections to obtain straight lines l1, l2,..., l n and the corresponding data point sets, and sort all the data point sets:

[0110]

[0111] wherein Z is the probability of obtaining good samples, which is set to 99%; w is the proportion of inliers, which is selected according to the positioning accuracy of the satellite, n is the minimum number of points required for model parameter estimation, and at least 2 points are required for straight line fitting; and

[0112] Step S3013, select the satellite trajectory point with the most data point sets as the navigation satellite trajectory point, and remove the points outside the data set as outliers.

[0113] The application further provides an application of the agricultural navigation map for navigation of agricultural vehicle using satellite positioning, wherein the agricultural navigation map is generated by the construction method of the plant-centered agricultural navigation map.

[0114] In an embodiment of the application, a frame of image can be collected by a front camera of the agricultural robot; the image is preprocessed; crop row information is extracted by Hough transform; transverse and turning errors of the image center line and the crop row are obtained; the two errors are input into an MPC controller for crop row following; satellite coordinate trajectories generated during driving of the agricultural robot are recorded; ransac algorithm is used to eliminate outliers of the satellite trajectories; and high-precision satellite trajectories are generated for reference driving of other vehicles.

[0115] The application introduces latitude and longitude of crop row information to generate driving routes, which can greatly avoid damage of autonomous vehicles to crops.

[0116] Of course, the application can have other various embodiments, and those skilled in the art can make various corresponding changes and modifications according to the application without departing from the spirit and essence of the application. However, these corresponding changes and modifications shall belong to the protection scope of the claims attached to the application.

Claims

1. A method for constructing an agricultural navigation map, characterized in that, A detailed agricultural navigation map is generated by overlaying crop information onto latitude and longitude coordinates for reference by other vehicles. This process includes the following steps: S100, the agricultural robot uses visual navigation to walk along the crop rows to detect crop rows, collect images and process them to obtain the lateral and turning errors between the image centerline and the crop rows; S200: Input the lateral and turning errors of the image centerline and the crop row into the MPC controller for crop row following, so as to follow the straight line of the crop row, and record the latitude and longitude coordinates of the agricultural robot at each moment during the walking process; and S300: Fit the recorded latitude and longitude coordinates, output the processed satellite coordinates, and generate a fine agricultural navigation map; Step S100 further includes: S101. A frame of image is captured by the front-mounted camera of the agricultural robot; S102. Preprocess the image to extract crop information and perform feature extraction; S103. Extracting crop row information using Hough transform; and S104. Obtain the lateral and directional errors of the image centerline and crop rows; Step S200 further includes: S201. Detect multiple waypoints from the road and calculate at least 3 interpolations to establish a driving trajectory; S202. Read the current speed v, direction ψ, turning angle δ and acceleration a of the agricultural robot from the sensor; S203. Calculate the first vehicle state using the current speed v, direction ψ, steering angle δ, and acceleration a data and the dynamic model; S204. Optimize control actions based on vehicle state response within a set time period, and set the constraint ranges for the two variables of MPC: acceleration and steering angle. S205. Calculate the dynamic model N times to obtain the system state for the next N time frames; S206. Give the loss function for each sampling calculation period; S207. Use the optimizer to calculate the minimum total loss from the first period to the Nth period under the constraint definition; S208. Select the control quantity given in the first cycle as the optimal control quantity and provide it to the agricultural robot; S209. Repeat steps S201-S208 to find the next optimal control variable; Step S300 further includes: S301. The RANSAC algorithm is used to remove extraneous points from the satellite trajectory. This is done by using RANSAC line fitting to eliminate extraneous points in the satellite trajectory coordinates. S302. Generate an agricultural navigation map with high-precision satellite trajectories centered on plants.

2. The method for constructing an agricultural navigation map as described in claim 1, characterized in that, Step S102 further includes: S1021. Perform mathematical modeling on the image, using a Cartesian coordinate system with the top left corner of the image as the origin, (x, y) as the position of each pixel, and R, G, and B as the light intensity values ​​of the red, green, and blue channels. Then, each image pixel is: as well as S1022. The image is converted to grayscale using the 2G-RB transform algorithm, converting each pixel of the image to a value between [0, 255].

3. The method for constructing an agricultural navigation map as described in claim 2, characterized in that, Step S102 also includes: S1023, Further remove noise using a threshold filter.

4. The method for constructing an agricultural navigation map as described in claim 3, characterized in that, Step S102 also includes: S1024. Using an edge detection filter, traverse each pixel horizontally. From left to right, when f(x, y) changes from 0 to 255, store the coordinates of that point as an edge point x1; when it changes from 255 to 0, store the point as an edge point x2. S1025. Take the average of x1 and x2 to obtain the x-coordinate of the centroid position of each detected object. c This converts the crop information into a series of coordinate points (x, y). c y c ).

5. The method for constructing an agricultural navigation map as described in claim 4, characterized in that, Step S103 further includes: S1031, For all coordinate points (x c y c Perform a Hough transform to convert the space to Hough space with ρ as the ordinate and θ as the abscissa. The transformation formula is as follows: p=x c cosθ+y c sinth; S1032. The Hough transform converts points on the image plane into sinusoidal curves in Hough space. The sinusoidal curves corresponding to collinear edge points on the image plane intersect at a fixed point (θ) in Hough space. t , ρ t If ), then the straight line in the image plane space at this time is: y=-ctgθ t x+r t cscθ t 。 6. The method for constructing an agricultural navigation map as described in claim 1, characterized in that, Step S301 further includes: S3011. Randomly select two points to determine a straight line l, set a threshold t, and determine the set of data points whose geometric distance from the straight line l is less than t; S3012, Repeat K to randomly select and obtain lines l1, l2, ... l n And the corresponding data point sets, and sort all the data point sets: Where Z is the probability of obtaining a good sample, set to 99%; w is the proportion of inliers, selected based on the satellite's positioning accuracy; n is the minimum number of points required for model parameter estimation, with at least 2 points needed for straight-line fitting; and S3013. Select the satellite trajectory points with the most data points for navigation, and remove points outside the dataset as outliers.

7. An application of an agricultural navigation map for navigation of agricultural machinery vehicles, wherein the agricultural machinery vehicles utilize satellite positioning, characterized in that... The agricultural navigation map is generated using the agricultural navigation map construction method described in any one of claims 1-6.

Images