Lidar perception based agricultural machine automatic row-to-row travel control algorithm

Abstract

The application discloses an agricultural machine automatic alignment driving control algorithm based on laser radar sensing, relates to the technical field of agricultural machine automation and intelligent control, and comprises the following steps: collecting crop point cloud data by a fixed laser radar and stipulating a laser radar coordinate system; transforming the laser radar coordinate system to be parallel to the ground by a point cloud transformation; filtering the ground point cloud by adopting a straight-through filtering z value filtering algorithm; predicting the front crop row direction by the vehicle path, filtering the point cloud by adopting a straight-through filtering, cutting the laser radar field of view into a region of interest of the point cloud crop row, removing outliers by adopting a statistical filtering, obtaining the crop row point cloud with the removed noise, performing down-sampling processing on the crop row point cloud by adopting a voxel filtering method, extracting a point cloud clustering center point by the Euclidean clustering, performing path fitting by adopting a least square method, storing the clustering center points extracted for N times into a list, performing crop row fitting, and performing path tracking according to the fitted crop row navigation line.

Description

Technical Field

[0001] This invention relates to the field of agricultural machinery automation and intelligent control technology, and in particular to an automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception. Background Technology

[0002] With the rapid development of modern agricultural technology, agricultural machinery automation has gradually become an important means to improve agricultural production efficiency and reduce labor intensity. LiDAR, as a high-precision distance measurement sensor, is increasingly widely used in agriculture. Early agricultural machinery automation mainly relied on GPS navigation systems, operating along preset paths. However, this GPS-based navigation method performs poorly in complex terrain or environments with significant changes in crop height, easily leading to deviations from the predetermined path.

[0003] Although existing LiDAR technology has achieved some success in agricultural machinery automation, it still has some shortcomings. First, traditional LiDAR data processing methods often lack effective mechanisms for removing ground point clouds, leading to interference from a large amount of non-target point cloud data during crop row extraction, affecting the final recognition accuracy. Second, in dynamic environments, such as changes in plant height during crop growth or the presence of field obstacles, existing technologies often struggle to update the crop row position information accurately in real time, causing deviations in vehicle travel paths. Furthermore, for complex terrain conditions, especially with slopes or uneven ground, the transformation between the LiDAR coordinate system and the ground coordinate system is not precise enough, further affecting the crop row recognition effect. In addition, most existing path fitting algorithms rely on data extracted in a single instance, ignoring the cumulative effect of multiple measurements, which may lead to path tracking jitter due to a large error in a single measurement. Summary of the Invention

[0004] In view of the aforementioned existing problems, the present invention is proposed.

[0005] Therefore, this invention provides an automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception to solve the problems of low crop row recognition accuracy, severe ground point cloud interference, and unstable path tracking caused by the lidar coordinate system not being parallel to the ground.

[0006] To solve the above-mentioned technical problems, the present invention provides the following technical solution:

[0007] In a first aspect, the present invention provides an automatic row-following driving control algorithm for agricultural machinery based on lidar perception, comprising: fixing a lidar to collect crop point cloud data and defining a lidar coordinate system; transforming the lidar coordinate system to be parallel to the ground using point cloud transformation, and filtering the ground point cloud using a pass-through filter z-value filtering algorithm; recording the vehicle's starting point coordinates through a satellite positioning system and fitting them to the crop rows; predicting the direction of the crop rows ahead based on the vehicle's already traveled path, using this as a reference line for cutting the lidar field of view; and filtering the point cloud using a pass-through filter based on the obtained lidar field of view cutting reference line. The laser radar's field of view is cut into a point cloud crop row region of interest. After determining the region of interest, statistical filtering is used to remove outliers, resulting in a noise-removed crop row point cloud. Based on the noise-removed crop row point cloud, voxel filtering is used to downsample the point cloud. Euclidean clustering is used to extract the cluster centers, and the cluster centers representing the center positions of the crop rows are used for path fitting using the least squares method. The N extracted cluster centers are stored in a list, and the crop rows are fitted using the cluster centers stored in the list. Path tracking is then performed based on the fitted crop row navigation line.

[0008] As a preferred embodiment of the automatic row-following driving control algorithm for agricultural machinery based on lidar perception described in this invention, the following steps are included: A fixed lidar is used to collect crop point cloud data, and a lidar coordinate system is defined.

[0009] A lidar is fixed above the machinery to collect crop point cloud data, with the lidar tilted relative to the horizontal plane. Spend;

[0010] The coordinate system of the lidar is specified to be a right-handed coordinate system, and the area in front of the lidar's field of view is... The axis, on the left is axis.

[0011] As a preferred embodiment of the automatic driving control algorithm for agricultural machinery based on lidar perception described in this invention, the following steps are included: transforming the lidar coordinate system to be parallel to the ground using point cloud transformation, and filtering the ground point cloud using a direct-pass filter z-value filtering algorithm.

[0012] Obtain raw point cloud data from the lidar and confirm the lidar's tilt angle relative to the horizontal plane. ;

[0013] The lidar coordinate system is transformed to a new coordinate system parallel to the ground using point cloud transformation, expressed as follows:

[0014] ;

[0015] in, To adjust the tilt angle of the lidar installation, , , The coordinates of the point cloud before transformation. , , The coordinates of the point cloud after transformation;

[0016] For each original point cloud data point, calculate its coordinates in the new coordinate system one by one. , , coordinate;

[0017] Traverse all point cloud data and find The maximum value of the axis;

[0018] Initial setting of the downward cut height The point cloud is processed using a pass-through filtering algorithm to retain... arrive The point cloud data between them is used to calculate the number of remaining point clouds after filtering. ;

[0019] Set the effective point cloud quantity threshold range as follows: to ;

[0020] when < If the number of point clouds is too small, increase the H value and iterate until the requirements are met;

[0021] when > At that time, the number of point clouds was too large, so the size was reduced. The value is iterated until the requirement is met;

[0022] When found The value makes In to When the result falls within the specified range, it is determined as the final filtering result.

[0023] As a preferred embodiment of the automatic row-following driving control algorithm for agricultural machinery based on lidar perception described in this invention, the following steps are included: recording the starting point coordinates of the vehicle using a satellite positioning system and fitting the crop rows; predicting the direction of the crop rows ahead based on the vehicle's already traveled path, using this as a reference line for cutting the lidar field of view; and based on the obtained lidar field of view cutting reference line, using a pass-through filter to filter the point cloud, cutting the lidar field of view into the region of interest of the point cloud crop rows, including the following steps.

[0024] Before the vehicle enters the field, align the vehicle with the crop row, ensuring that the heading of the vehicle is consistent with the overall direction of the crop row.

[0025] Use a satellite positioning system to record vehicle movement. The starting point coordinates of the time ( , );

[0026] Based on the currently known crop row positions and directions, perform the first crop row fitting;

[0027] Use point cloud viewing software to check the deviation between the fitted crop row navigation line and the actual crop row, and ensure that the deviation between the first fitted crop row navigation line and the actual crop row is less than R;

[0028] The direction of the crop rows is fitted using point cloud data and used as a reference line;

[0029] As the vehicle moves forward, at different points in time... , ,…, Record the vehicle's location coordinates;

[0030] Using the vehicle's position coordinates, a fitting algorithm is used to predict the direction of the crop row ahead;

[0031] At the point of time At that time, the direction of the crop row ahead, which is obtained by fitting historical path data, is used as the reference line for cutting the field of view of the lidar;

[0032] The length of the field-of-view cutting reference line is determined based on the vehicle's current position, the effective detection range of the lidar, and the vehicle speed. The field-of-view cutting width R is set to the crop row spacing. one-third;

[0033] The vehicle's current position is obtained through a satellite positioning system, and the distance to the field of view cutting reference line is calculated to determine the closest point between the vehicle and the reference line. , );

[0034] Starting from the nearest point ( , Along the reference line direction, use the field of view to cut the reference line length L to obtain a series of points on the reference line ( , );

[0035] A pass-through filter is used to filter the point cloud, in front of... The axis filtering range is (0, L). The axis filtering range is ( The LiDAR field of view is cut into the region of interest of the point cloud crop row.

[0036] As a preferred embodiment of the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception described in this invention, the following steps are included: after determining the region of interest for the crop row point cloud, statistical filtering is used to remove outliers to obtain a noise-removed crop row point cloud.

[0037] Set the number of nearest neighbors in the point cloud, N, and the standard deviation factor, M;

[0038] All point cloud data is input into the Kd-tree constructor and the corresponding index object is generated;

[0039] For each point in the point cloud, use a Kd-tree to search for its N nearest neighboring points;

[0040] Calculate the Euclidean distance between neighboring points, and calculate the mean and standard deviation of the distance between each point and its neighboring points;

[0041] If the distance of a point's maximum neighbor exceeds the average distance by M standard deviations, it is considered an outlier and is removed.

[0042] Traverse the entire point cloud dataset and check each point one by one to see if it is an outlier. For data points identified as outliers, remove them from the original point cloud dataset to obtain a crop row point cloud with noise removed.

[0043] As a preferred embodiment of the automatic row-following driving control algorithm for agricultural machinery based on lidar perception described in this invention, the following steps are included: Based on a noise-removed crop row point cloud, voxel filtering is used to downsample the crop row point cloud, and Euclidean clustering is used to extract the cluster centers of the point cloud, comprising the following steps.

[0044] Set the size of the voxel grid to ( , , );

[0045] Divide the entire point cloud space into sections with sides of length [missing information]. The cubic unit, each voxel represents a small cubic region;

[0046] For each point in the point cloud, it is assigned to the corresponding voxel based on its coordinate position;

[0047] For each non-empty voxel, select a representative point from all points within that voxel as the output point of that voxel.

[0048] Traverse all voxels and select a representative point for each non-empty voxel according to the above rules to generate a new point cloud dataset.

[0049] Use functions from the point cloud library to create a Kd tree object, and input the downsampled point cloud data into the object to generate an index vector;

[0050] Set cluster search radius Clustering parameters for minimum and maximum cluster sizes;

[0051] Based on the set clustering parameters, for each point, a Kd-tree is used to search for its surrounding nodes whose distance is less than or equal to... All points;

[0052] When the Euclidean distance between two points is less than or equal to If they belong to the same cluster, they are assigned to the corresponding cluster.

[0053] For each cluster formed, check whether it meets the requirements for minimum and maximum cluster size;

[0054] For each valid cluster, its geometric center is calculated as the representative point of that cluster;

[0055] Obtaining data containing Euclidean clustering , The point cloud of coordinates is used to cluster the center points, which are then used to represent the positions of crop rows.

[0056] As a preferred embodiment of the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception described in this invention, the following steps are included: path fitting is performed using the least squares method on the cluster center points representing the center positions of crop rows.

[0057] Determine the maximum value of cluster centers Value and minimum Find the value and calculate the difference;

[0058] The least squares method was used to fit a straight line to the cluster center points;

[0059] Based on the calculated difference, set the coordinates of the two endpoints of the fitted line;

[0060] Connect the two endpoints of the fitted line to form a line containing... coordinates and The straight line at the coordinates serves as the navigation line for crop rows.

[0061] As a preferred embodiment of the automatic row-following driving control algorithm for agricultural machinery based on lidar perception described in this invention, the following steps are included: storing the cluster center points extracted N times into a list, using the cluster center points stored in the list to fit crop rows, and performing path tracking based on the fitted crop row navigation line.

[0062] Create an empty list Store the center points of the crop rows extracted N times into a list. Using lists The cluster centers stored in the database are used to fit crop rows;

[0063] Each time the center point of a crop row is extracted, the newly obtained center point of the crop row is added to the list. middle;

[0064] After adding a crop row center point to the list N times, add the crop row center point again and delete the first crop row center point.

[0065] Use list The least squares method is used to fit all points to generate the final crop row navigation line;

[0066] The fitted crop row navigation lines are converted into array information and sent to the path tracking section for path tracking.

[0067] In a second aspect, the present invention provides a computer device, including a memory and a processor, wherein the memory stores a computer program, wherein: when the computer program is executed by the processor, it implements any step of the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception as described in the first aspect of the present invention.

[0068] Thirdly, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein: when the computer program is executed by a processor, it implements any step of the automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in the first aspect of the present invention.

[0069] The beneficial effects of this invention are as follows: By transforming the point cloud, the lidar coordinate system is converted into a new coordinate system parallel to the ground. A direct-pass filtering z-value filtering algorithm is applied to remove ground point clouds, effectively solving the problem of non-parallel coordinate systems caused by uneven terrain. This not only eliminates the interference of ground point clouds on crop row extraction and improves data processing efficiency, but also ensures that the number of point clouds is within the optimal range by dynamically adjusting the filtering height, avoiding the problem of excessive or insufficient point clouds affecting clustering accuracy. By dynamically adjusting the length and width of the field-of-view cutting reference line, the lidar field of view is always focused on the current crop row, reducing interference from non-target point clouds and significantly improving the adaptability of agricultural machinery in dynamic environments. Effective identification and removal of outliers not only significantly reduces the impact of noise points on crop row identification, but also improves data quality and usability by retaining key feature points. Voxel filtering is used to downsample the noise-removed point cloud data, reducing the data volume while maintaining necessary detail information. Euclidean clustering is used to extract the cluster centers of the point cloud, and least squares method is used for path fitting to generate accurate crop row navigation lines. This method not only improves the accuracy of path fitting, but also enhances the stability and smoothness of path tracking through the comprehensive fitting of multiple extraction results. Attached Figure Description

[0070] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0071] Figure 1 This is a flowchart of the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception in Example 1.

[0072] Figure 2 This is a schematic diagram of the laser radar field of view cutting in Example 1.

[0073] Figure 3 This is a schematic diagram of the pure tracking algorithm for the optimal pre-aiming point in Example 1.

[0074] Figure 4 This is a schematic diagram of the pass-through filtering z-value filtering algorithm in Example 1. Detailed Implementation

[0075] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0076] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0077] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0078] Example 1, referring to Figures 1-3 This is the first embodiment of the present invention, which provides an automatic row-following driving control algorithm for agricultural machinery based on lidar perception, including the following steps:

[0079] S1. Collect crop point cloud data using a fixed lidar and define the lidar coordinate system, including the following steps:

[0080] The lidar is fixed above the machinery to collect crop point cloud data. During installation, the lidar's tilt angle relative to the horizontal plane needs to be adjusted. The angle (the specific value is determined based on actual application requirements) is used to adapt to different terrain conditions and ensure that the lidar can effectively capture crop row information.

[0081] To facilitate subsequent data processing, it is necessary to record the data after installation. The exact value of this parameter will be used in the point cloud data transformation.

[0082] The coordinate system of the lidar is defined as a right-handed coordinate system, that is, the area in front of the lidar's field of view is defined as... The positive direction of the axis, the left side is defined as The positive direction of the axis is vertically upward. The positive direction of the axis. This coordinate system helps to standardize data processing and facilitates the implementation of subsequent algorithms.

[0083] S2. Transform the lidar coordinate system to be parallel to the ground using point cloud transformation, and then use a pass-through filter z-value filtering algorithm to filter out ground point clouds, including the following steps.

[0084] Obtain raw point cloud data from the lidar and confirm the lidar's tilt angle θ relative to the horizontal plane;

[0085] Because the lidar is installed at an angle, the ground coordinate system is not parallel to the lidar coordinate system, which affects the extraction of crop rows. Point cloud transformation is used to transform the lidar coordinate system to a new coordinate system parallel to the ground. The expression is as follows:

[0086] ;

[0087] in, The tilt angle (in degrees) for installing the lidar. , , The coordinates of the point cloud before transformation (unit: meters). , , The coordinates of the point cloud after transformation (unit: meters).

[0088] For each original point cloud data point, calculate its coordinates in the new coordinate system one by one. , , Coordinates. This step requires traversing all point cloud data and performing coordinate transformation operations on each point. During the calculation process, the angles must be ensured... The degrees are converted to radians before being used in trigonometric function calculations to ensure the accuracy of the results.

[0089] Traverse all point cloud data and find Maximum value of the axis This value represents the highest point of the crop row. This can be achieved by iterating through all the z-coordinates in the point cloud data and recording the maximum value.

[0090] Initial setting of the downward cut height , used from Start filtering down a certain range of point clouds to remove ground point clouds. The initial value can be determined based on experience, and then dynamically adjusted according to the remaining number of point clouds.

[0091] The point cloud is processed using a pass-through filtering algorithm to preserve... arrive The point cloud data between the crop rows means that only those points located in the top area of ​​the crop rows will be retained in the dataset.

[0092] Calculate the number of remaining point clouds after filtering That is, satisfying < ≤ The number of points in the condition;

[0093] Set the effective point cloud quantity threshold range as follows: to Check whether P falls within the preset effective point cloud quantity threshold range. to between( ≤P≤ If it is not within this range, then adjustments are needed. The value is then repeated and the filtering process is repeated.

[0094] like < This indicates that the number of point clouds is too small, and the clustered crop row center points are not accurately fitted to the crop rows. In this case, the number of point clouds should be increased. The values ​​are iterated to expand the filtering range to include more point cloud data; if > This indicates that the number of point clouds is too large, the code processing speed is slow, which may affect the program's running efficiency. The H value should be reduced for iteration to narrow the filtering range and optimize the calculation speed and accuracy.

[0095] according to and , Dynamic adjustment of relationships Value, until Falling to Within the specified range. Adjustment strategies can involve gradually increasing or decreasing. The value is increased / decreased by 0.1 meters each time until the condition is met.

[0096] When found The value makes In to When the data is within the specified range, it is confirmed as the final filtering result. This step ensures that neither too much useful information is lost nor too many noise points are introduced, thus providing a high-quality data foundation for subsequent crop row extraction.

[0097] S3. Record the vehicle's starting point coordinates using a satellite positioning system and fit them to the crop rows. Predict the direction of the crop rows ahead based on the vehicle's already traveled path, using this as a reference line for cutting the lidar field of view. Based on the obtained lidar field of view cutting reference line, use a pass-through filter to filter the point cloud, cutting the lidar field of view into the region of interest of the point cloud crop rows. This includes the following steps.

[0098] Before the vehicle enters the field, it should be aligned with the crop row, with the vehicle's heading consistent with the overall direction of the crop row. This is to ensure the accuracy of the initial path and provide a reliable basis for subsequent path prediction.

[0099] Use a satellite positioning system to record vehicle movement. The starting point coordinates of the time ( , );

[0100] Based on the currently known crop row positions and directions, perform the first crop row fitting;

[0101] Use point cloud viewing software to check the deviation between the fitted crop row navigation line and the actual crop row, and ensure that the deviation between the first fitted crop row navigation line and the actual crop row is less than R;

[0102] The direction of the crop rows is fitted using point cloud data and used as a reference line;

[0103] As the vehicle moves forward, at different points in time... , ,…, Record the vehicle's location coordinates.

[0104] Using the vehicle's position coordinates, the direction of the preceding crop row is predicted through a fitting algorithm (such as polynomial fitting or least squares method); Record the vehicle's current location coordinates at all times, through Time and Time coordinate fitting The reference line for constantly cutting the field of view is used. Fitting crop rows at specific times. At that moment, , , The coordinates at each moment are fitted to the reference line of the field of view cut. Until... At that moment, , , , … Fit the coordinates at time points, in At that moment, , , … , Fit the coordinates at time points, using... The coordinates of a point are used to fit the reference line for the field of view cutting.

[0105] At the point of time At that time, the direction of the crop row ahead, which is obtained by fitting historical path data, is used as the reference line for cutting the field of view of the lidar. This reference line for cutting the field of view of the lidar not only helps to determine the general direction of the crop row ahead, but also affects how the lidar focuses on the crop row in a specific area.

[0106] The length of the field-of-view cutting reference line is determined based on the vehicle's current position, the effective detection range of the lidar, and the vehicle speed. Generally, the length of the field-of-view cutting reference line is directly proportional to the vehicle speed; that is, the faster the speed, the longer the preview distance is required to ensure timely adjustment of the driving path.

[0107] Field of view cutting width Set as crop row spacing one-third, that is This is done to ensure that the lidar can accurately capture information about the target crop row without mistakenly including data from adjacent crop rows.

[0108] The vehicle's current position is obtained through a satellite positioning system, and the distance to the field of view cutting reference line is calculated to determine the closest point between the vehicle and the reference line. , This step involves geometric calculations to find the point that is perpendicular to the reference line from the vehicle's position.

[0109] With the nearest point ( , Starting from the reference line, along its direction, use the field of view to cut the reference line by its length L to obtain a series of points on the reference line. , ),in, These are unique identifiers for each point in the point cloud, used to distinguish different points. These points will be used to define the field of view of the LiDAR.

[0110] A pass-through filter is used to filter the point cloud, in front of... The axis filtering range is (0, L). The axis filtering range is ( The lidar field of view is cut into regions of interest for the point cloud crop rows. The purpose of this is to create a parallelogram-shaped region of interest and retain only the point cloud data located within this region.

[0111] It should be noted that during autonomous driving, by refitting the already traversed path to infer the location information of the crop rows ahead, the system can ensure that the crop point cloud does not contain adjacent crops due to vehicle deviation, thus avoiding deviations in crop row extraction. Furthermore, the LiDAR's field of view can be adjusted according to the actual width of the crop rows, making it suitable for extracting different types of crop rows.

[0112] S4. After determining the region of interest for the crop rows in the point cloud, statistical filtering is used to remove outliers, resulting in a noise-removed crop row point cloud. This includes the following steps.

[0113] Set the number of neighboring points N in the point cloud and the standard deviation factor M; where N is the number of neighboring points around each point, used to calculate the distance between the point and the surrounding environment; M is the standard deviation factor, used to define the threshold for outliers.

[0114] All point cloud data is input into the Kd-tree constructor and a corresponding index object is generated. It should be noted that the Kd-tree is a spatially partitioned data structure that can quickly find the nearest neighbor, which is especially important in large datasets.

[0115] For each point in the point cloud, use a Kd-tree to search for its N nearest neighboring points.

[0116] Calculate the Euclidean distance between neighboring points, and calculate the mean and standard deviation of the distance between each point and its neighboring points; if the maximum distance of a point to its nearest neighbor exceeds the mean distance by M standard deviations, it is considered an outlier and is removed.

[0117] Traverse the entire point cloud dataset and check each point one by one to see if it is an outlier. For data points identified as outliers, remove them from the original point cloud dataset to obtain a crop row point cloud with noise removed.

[0118] It should be noted that after determining the region of interest for the crop rows in the point cloud, the crop rows are still affected by noise such as outliers. Statistical filtering is used to remove outliers. Outliers are sparsely distributed in space, and invalid point clouds are filtered out by setting a statistical filtering threshold.

[0119] S5. Based on the noise-removed crop row point cloud, voxel filtering is used to downsample the crop row point cloud, and Euclidean clustering is used to extract the cluster centers of the point cloud. This includes the following steps:

[0120] Based on actual needs and application scenarios, the size of the voxel grid is set to ( , , (Unit: meters). The larger the voxel grid, the more significant the filtering effect, meaning a greater reduction in point cloud density; however, excessively large voxel grids may lead to the loss of important details. Therefore, it is necessary to comprehensively consider the balance between the data accuracy to be preserved and computational efficiency to determine the optimal voxel grid size.

[0121] Divide the entire point cloud space into sections with sides of length [missing information]. The cubic unit, where each voxel represents a small cubic region.

[0122] For each point in the point cloud, it is assigned to the corresponding voxel based on its coordinates. Specifically, if the coordinates of a point are ( , , If the voxel to which it belongs is determined, then the voxel is calculated using the following formula:

[0123] ;

[0124] in, , , Representing a point ( , , The index coordinates of the voxel to which it belongs. This indicates a floor operation, used to convert continuous spatial coordinates into discrete voxel indices.

[0125] For each non-empty voxel, a representative point is selected from all points within that voxel as the output point. Common selection strategies include: selecting the first point encountered within the voxel; calculating the average value or centroid of all points within the voxel and selecting the point closest to the centroid as the representative. This step ensures that the shape characteristics of the original point cloud are preserved as much as possible while reducing the number of points in the point cloud.

[0126] Iterate through all voxels, and select a representative point for each non-empty voxel according to the rules described above to generate a new point cloud dataset. This new point cloud dataset contains points that have been voxel filtered; their number is significantly reduced, but they still reflect the overall shape of the original point cloud quite well.

[0127] Use functions from the point cloud library to create a Kd tree object, and input the downsampled point cloud data into the object to generate an index vector;

[0128] Setting several important parameters related to clustering: The cluster search radius (in meters) indicates that points within this radius are considered to belong to the same cluster; the minimum cluster size defines the minimum number of points a cluster must contain to be considered a valid cluster; the maximum cluster size defines the maximum number of points a cluster can contain, preventing excessively large clusters from affecting the analysis results.

[0129] Based on the set clustering parameters, for each point, a Kd-tree is used to search for its surrounding nodes whose distance is less than or equal to... All points; when the Euclidean distance between two points is less than or equal to If they belong to the same cluster, they are assigned to the corresponding cluster.

[0130] For each cluster formed, check whether it meets the requirements for minimum and maximum cluster size. Clusters that do not meet the requirements will be ignored or merged into other clusters.

[0131] For each valid cluster, its geometric center is calculated as the representative point of that cluster;

[0132] Obtaining data containing Euclidean clustering , The point cloud of coordinates is used to cluster the center points, which are then used to represent the positions of crop rows.

[0133] S6. Perform path fitting using the least squares method on the cluster centers representing the center positions of crop rows, including the following steps.

[0134] To ensure that the fitted line length matches the actual crop row length, the fitting range must first be determined. Specifically, the maximum range of the cluster centers must be determined. Value and minimum The value is calculated, and the difference is determined, which is the length of the straight line fitted to the horizontal strip.

[0135] The least squares method is used to fit a straight line to the cluster center points. The goal of the least squares method is to find a straight line that minimizes the sum of the squared distances from all points to that line.

[0136] Based on the previously calculated difference, set the coordinates of the two endpoints of the fitted line;

[0137] Connect the two endpoints of the fitted line to form a line containing coordinates and The straight line representing the coordinates serves as the crop row navigation line. This line will be used to guide agricultural machinery along the crop rows.

[0138] S7. Store the cluster centers extracted N times into a list, use the cluster centers stored in the list to fit crop rows, and perform path tracking based on the fitted crop row navigation lines, including the following steps.

[0139] Create an empty list Store the center points of the crop rows extracted N times into a list. This ensures a stable data foundation for crop row fitting. Using lists... The cluster centers stored in the database are used to fit crop rows.

[0140] It should be noted that the fitted crop rows cannot be directly sent as local paths for tracking, because crop rows are updated rapidly, and the curvature of crop rows extracted at previous time steps may differ significantly from that extracted at next time step. Direct path tracking would result in issues such as vehicle swaying, leading to inaccurate path tracking. The initial variable N determines the maximum number of crop row center points stored in the list. Choosing an appropriate value for N is crucial for balancing data freshness and stability.

[0141] Each time the crop row center points are extracted (e.g., through the previous point cloud clustering and path fitting steps), the newly obtained crop row center points are added to the list. middle;

[0142] After adding points to the list N times, a new crop row center point is added, and the first crop row center point is deleted. This ensures that the number of points in the list does not exceed N, and that the list always contains the latest N crop row center points. This method effectively avoids severe jitter in vehicle tracking due to a large curvature of a crop row navigation line, ensuring the stability and smoothness of vehicle path tracking.

[0143] The final crop row navigation line is generated by performing a least-squares fit using all points in list X.

[0144] The fitted crop row navigation lines are converted into array information and sent to the path tracking section for path tracking. Tracking methods can include the Pure-Puresuit algorithm, the Stanley algorithm, and the PID algorithm. For example, a pure tracking algorithm that searches for the optimal preview point is based on the Pure-Puresuit algorithm. It selects the optimal preview point within a certain preview area based on the vehicle's current pose, inputs this preview point into the algorithm's output front wheel steering angle, obtains the new vehicle state variable for the next moment, and returns it to the control method to achieve closed-loop control.

[0145] Repeat the above steps after each vehicle has traveled a certain distance, continuously updating the list of crop row center points. And refit and path tracking are performed to adapt to the ever-changing operating environment.

[0146] This embodiment also provides a computer device applicable to the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception, including: a memory and a processor; the memory is used to store computer-executable instructions, and the processor is used to execute the computer-executable instructions to implement the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception as proposed in the above embodiment.

[0147] The computer device can be a terminal, comprising a processor, memory, communication interface, display screen, and input devices connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The communication interface is used for wired or wireless communication with external terminals; wireless communication can be achieved through Wi-Fi, carrier networks, NFC (Near Field Communication), or other technologies. The display screen can be an LCD screen or an e-ink screen. The input devices can be a touch layer covering the display screen, buttons, a trackball, or a touchpad on the computer device's casing, or an external keyboard, touchpad, or mouse.

[0148] This embodiment also provides a storage medium storing a computer program. When executed by a processor, the program implements the automatic row-following control algorithm for agricultural machinery based on lidar perception, as proposed in the above embodiments. The storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read Only Memory (EPROM), Programmable Red-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk.

[0149] In summary, this invention effectively solves the problem of non-parallel coordinate systems caused by uneven terrain by transforming the LiDAR coordinate system into a new coordinate system parallel to the ground through point cloud transformation and applying a pass-through filtering z-value filtering algorithm to remove ground point clouds. This not only eliminates the interference of ground point clouds on crop row extraction and improves data processing efficiency, but also ensures that the number of point clouds is within the optimal range by dynamically adjusting the filtering height, avoiding the problem of clustering accuracy being affected by too many or too few point clouds. Furthermore, by dynamically adjusting the length and width of the field-of-view cutting reference line, it ensures that the LiDAR field of view is always focused on the current crop row. This method reduces interference from non-target point clouds, significantly improving the adaptability of agricultural machinery in dynamic environments and ensuring the stability and continuity of path tracking. Effective identification and removal of outliers not only significantly reduces the impact of noise points on crop row identification but also improves data quality and usability by retaining key feature points. Voxel filtering is used to downsample the noise-removed point cloud data, reducing data volume while maintaining necessary details. Euclidean clustering is used to extract cluster centers from the point cloud, and least squares is used for path fitting to generate accurate crop row navigation lines. This approach not only improves the accuracy of path fitting but also enhances the stability and smoothness of path tracking through the comprehensive fitting of multiple extraction results.

[0150] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. An automatic row-following driving control algorithm for agricultural machinery based on lidar perception, characterized in that: include, A fixed lidar is used to collect crop point cloud data, and the lidar coordinate system is defined. The coordinate system of the lidar is transformed to be parallel to the ground by using point cloud transformation, and the ground point cloud is filtered out by a pass-through filter z-value filtering algorithm; The starting point coordinates of the vehicle are recorded by the satellite positioning system and fitted to the crop row. The direction of the crop row ahead is predicted by the path already taken by the vehicle and used as the reference line for cutting the field of view of the lidar. Based on the obtained lidar field of view cutting reference line, the point cloud is filtered by a pass filter and the lidar field of view is cut into the region of interest of the point cloud crop row. After determining the region of interest for the crop row in the point cloud, statistical filtering is used to remove outliers, resulting in a noise-removed crop row point cloud. Based on the crop row point cloud with noise removed, the voxel filtering method is used to downsample the crop row point cloud, the point cloud cluster center points are extracted by Euclidean clustering, and the cluster center points representing the center position of the crop row are used for path fitting by least squares method. Store the cluster centers extracted N times into a list, use the cluster centers stored in the list to fit crop rows, and perform path tracking based on the fitted crop row navigation lines.

2. The automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in claim 1, characterized in that: The process of acquiring crop point cloud data using a fixed lidar and defining the lidar coordinate system includes the following steps: A lidar is fixed above the machinery to collect crop point cloud data, with the lidar tilted relative to the horizontal plane. Spend; The coordinate system of the lidar is specified to be a right-handed coordinate system, and the area in front of the lidar's field of view is... The axis, on the left is axis.

3. The automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in claim 2, characterized in that: The lidar coordinate system is transformed to be parallel to the ground using point cloud transformation, and a pass-through filter z-value filtering algorithm is used to filter out ground point clouds. The process includes the following steps. Obtain raw point cloud data from the lidar and confirm the lidar's tilt angle relative to the horizontal plane. ; The lidar coordinate system is transformed to a new coordinate system parallel to the ground using point cloud transformation, expressed as follows: ； in, To adjust the tilt angle of the lidar installation, , , The coordinates of the point cloud before transformation. , , The coordinates of the point cloud after transformation; For each original point cloud data point, calculate its coordinates in the new coordinate system one by one. , , coordinate; Traverse all point cloud data and find The maximum value of the axis; Initial setting of the downward cut height The point cloud is processed using a pass-through filtering algorithm to retain... arrive The point cloud data between them is used to calculate the number of remaining point clouds after filtering. ; Set the effective point cloud quantity threshold range as follows: to ; when < If the number of point clouds is too small, increase the H value and iterate until the requirements are met; when > At that time, the number of point clouds was too large, so the size was reduced. The value is iterated until the requirement is met; When found The value makes In to When the result falls within the specified range, it is determined as the final filtering result.

4. The automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in claim 3, characterized in that: The starting point coordinates of the vehicle are recorded using a satellite positioning system and fitted to the crop rows. The direction of the crop rows ahead is predicted based on the vehicle's already traveled path, serving as a reference line for cutting the LiDAR field of view. Based on the obtained LiDAR field of view cutting reference line, a pass-through filter is used to filter the point cloud, cutting the LiDAR field of view into the region of interest of the point cloud crop rows. This includes the following steps. Before the vehicle enters the field, align the vehicle with the crop row, ensuring that the heading of the vehicle is consistent with the overall direction of the crop row. Use a satellite positioning system to record vehicle movement. The starting point coordinates of the time ( , ); Based on the currently known crop row positions and directions, perform the first crop row fitting; Use point cloud viewing software to check the deviation between the fitted crop row navigation line and the actual crop row, and ensure that the deviation between the first fitted crop row navigation line and the actual crop row is less than R; The direction of the crop rows is fitted using point cloud data and used as a reference line; As the vehicle moves forward, at different points in time... , ,…, Record the vehicle's location coordinates; Using the vehicle's position coordinates, a fitting algorithm is used to predict the direction of the crop row ahead; At the point of time At that time, the direction of the crop row ahead, which is obtained by fitting historical path data, is used as the reference line for cutting the field of view of the lidar; The length of the field-of-view cutting reference line is determined based on the vehicle's current position, the effective detection range of the lidar, and the vehicle speed. The field-of-view cutting width R is set to the crop row spacing. one-third; The vehicle's current position is obtained through a satellite positioning system, and the distance to the field of view cutting reference line is calculated to determine the closest point between the vehicle and the reference line. , ); Starting from the nearest point ( , Along the reference line direction, use the field of view to cut the reference line length L to obtain a series of points on the reference line ( , ); A pass-through filter is used to filter the point cloud, in front of... The axis filtering range is (0, L). The axis filtering range is ( The LiDAR field of view is cut into the region of interest of the point cloud crop row.

5. The automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in claim 4, characterized in that: After determining the region of interest (ROI) for the crop row in the point cloud, statistical filtering is used to remove outliers, resulting in a noise-removed crop row point cloud. This process includes the following steps: Set the number of nearest neighbors in the point cloud, N, and the standard deviation factor, M; All point cloud data is input into the Kd-tree constructor and the corresponding index object is generated; For each point in the point cloud, use a Kd-tree to search for its N nearest neighboring points; Calculate the Euclidean distance between neighboring points, and calculate the mean and standard deviation of the distance between each point and its neighboring points; If the distance of a point's maximum neighbor exceeds the average distance by M standard deviations, it is considered an outlier and is removed. Traverse the entire point cloud dataset and check each point one by one to see if it is an outlier. For data points identified as outliers, remove them from the original point cloud dataset to obtain a crop row point cloud with noise removed.

6. The automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in claim 5, characterized in that: Based on the noise-removed crop row point cloud, voxel filtering is used to downsample the crop row point cloud, and Euclidean clustering is used to extract the cluster centers of the point cloud. The process includes the following steps. Set the size of the voxel grid to ; Divide the entire point cloud space into sections with sides of length [missing information]. The cubic unit, each voxel represents a small cubic region; For each point in the point cloud, it is assigned to the corresponding voxel based on its coordinate position; For each non-empty voxel, select a representative point from all points within that voxel as the output point of that voxel. Traverse all voxels and select a representative point for each non-empty voxel according to the above rules to generate a new point cloud dataset. Use functions from the point cloud library to create a Kd tree object, and input the downsampled point cloud data into the object to generate an index vector; Set cluster search radius Clustering parameters for minimum and maximum cluster sizes; Based on the set clustering parameters, for each point, a Kd-tree is used to search for its surrounding nodes whose distance is less than or equal to... All points; When the Euclidean distance between two points is less than or equal to If they belong to the same cluster, they are assigned to the corresponding cluster. For each cluster formed, check whether it meets the requirements for minimum and maximum cluster size; For each valid cluster, its geometric center is calculated as the representative point of that cluster; Obtaining data containing Euclidean clustering , The point cloud of coordinates is used to cluster the center points, which are then used to represent the positions of crop rows.

7. The automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in claim 6, characterized in that: The cluster centers representing the center positions of crop rows are used for path fitting using the least squares method, including the following steps. Determine the maximum value of cluster centers Value and minimum Find the value and calculate the difference; The least squares method was used to fit a straight line to the cluster center points; Based on the calculated difference, set the coordinates of the two endpoints of the fitted line; Connect the two endpoints of the fitted line to form a line containing... coordinates and The straight line at the coordinates serves as the navigation line for crop rows.

8. The automatic row-following driving control algorithm for agricultural machinery based on lidar perception as described in claim 7, characterized in that: The cluster centers extracted N times are stored in a list. Crop rows are fitted using the cluster centers stored in the list. Path tracking is then performed based on the fitted crop row navigation lines, including the following steps. Create an empty list Store the center points of the crop rows extracted N times into a list. Using lists The cluster centers stored in the database are used to fit crop rows; Each time the center point of a crop row is extracted, the newly obtained center point of the crop row is added to the list. middle; After adding a crop row center point to the list N times, add the crop row center point again and delete the first crop row center point. Use list The least squares method is used to fit all points to generate the final crop row navigation line; The fitted crop row navigation lines are converted into array information and sent to the path tracking section for path tracking.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, it implements the steps of the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception as described in any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by the processor, it implements the steps of the automatic row-to-row driving control algorithm for agricultural machinery based on lidar perception as described in any one of claims 1 to 8.

Images