Vegetable inter-ridge navigation method and system based on laser radar

Abstract

The invention relates to a vegetable inter-ridge navigation method and system based on a laser radar, and the method comprises the steps: S1, measuring the surrounding environment of a robot through a three-dimensional laser radar, and obtaining three-dimensional point cloud data; s2, performing preliminary optimization on the point cloud data through a point cloud preprocessing algorithm, and reducing the density and operand of the point cloud data; s3, segmenting a ground point cloud through a ground segmentation algorithm, and taking the ground point cloud as a drivable area; s4, extracting the mass center of the ground point cloud, taking the mass center as an inter-ridge center point, and fitting the inter-ridge center point to obtain an inter-ridge center line equation; s5, setting a navigation preview distance, and calculating a navigation line equation under the preview distance; and S6, calculating an angular velocity control quantity through the navigation line equation parameters, and issuing the angular velocity control quantity to the power chassis to control the chassis to move. According to the invention, accurate automatic navigation walking between vegetable ridges is realized, and the mechanization degree of inspection, plant protection, transportation and other production operations in vegetable production is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of agricultural machinery, and more specifically, to a laser radar-based vegetable ridge navigation method and system. Background Art

[0002] The mechanization level of vegetable production is low and still relies on traditional labor. Small mobile chassis operating equipment can travel between vegetable ridges, but there is little research on universal navigation methods that can be applied to different vegetable planting ridges. In addition, due to the complex environment between vegetable ridges, the relatively narrow and complex structure of vegetable ridges, the different growth heights of crops between vegetable ridges, the uneven land, and the presence of different crop types and forms, real-time perception and response to the environment are required when working between vegetable ridges, which puts higher requirements on the accuracy of navigation between vegetable ridges. Traditional vision-based navigation methods may have difficulty in recognition, especially under weak or strong light changes. Although traditional GPS navigation systems can provide basic navigation functions, the accuracy and stability of GPS signals often cannot meet the requirements in the complex environment between vegetable ridges.

[0003] Therefore, in order to solve the problems existing in the prior art, the present invention studies a vegetable ridge navigation method and system with wide applicability, strong stability and strong practicality. It uses lidar technology to provide high-precision three-dimensional environmental perception and is not affected by changes in light. It provides a feasible solution for vegetable ridge navigation, realizes ridge inspection, plant protection, transportation and other operations for different types of vegetables, and improves the mechanization level of vegetable production. Summary of the invention

[0004] The present invention aims to overcome at least one defect (shortcoming) of the above-mentioned prior art and provide a method and system for navigating between vegetable ridges based on laser radar, which can realize accurate automatic navigation between vegetable ridges and improve the mechanization level of production operations such as inspection, plant protection, and transportation in vegetable production.

[0005] The technical solution adopted by the present invention is a vegetable ridge navigation method based on laser radar, and the method comprises the following steps:

[0006] S1: Measure the robot's surroundings through 3D laser radar to obtain 3D point cloud data;

[0007] S2: Preliminary optimization of point cloud data is performed through point cloud preprocessing algorithm to reduce the density and computational complexity of point cloud data;

[0008] S3: Segment the ground point cloud through the ground segmentation algorithm and use the ground point cloud as the drivable area;

[0009] S4: extract the centroid of the ground point cloud and use it as the center point between ridges, then fit the center point between ridges to obtain the center line equation between ridges;

[0010] S5: Set the navigation preview distance and calculate the navigation line equation at the preview distance;

[0011] S6: Calculate the angular velocity control value through the navigation line equation parameters and send it to the power chassis to control the chassis movement.

[0012] In the present application, by using laser radar to obtain three-dimensional point cloud data of environmental information and then processing these data, it is possible to accurately capture detailed information such as the terrain between vegetable ridges, crop row spacing, and crop growth conditions, and then use the ground segmentation algorithm to segment the ground point cloud to obtain the drivable area in the vegetable field, and then process the centroid of the extracted ground point cloud and the preset navigation preview distance to obtain the navigation route. Compared with traditional vision-based or GPS navigation systems, laser radar can provide more accurate operation trajectories and path planning, ensuring that agricultural machinery can travel stably along the predetermined track, and also calculate the angular velocity control amount according to the navigation line equation parameters to control the movement of the chassis, thereby realizing automatic planning of the operation route and autonomous navigation. It not only improves the operation speed, but also reduces the errors and instability caused by manual operation, makes real-time adjustments and optimizations, improves operation efficiency and accuracy, especially in large-scale agricultural production, can significantly save time and labor, and improve work efficiency; in addition, lidar technology will not be affected by external factors such as lighting changes and weather conditions, and can work stably in complex environments such as weak light, strong light, cloudy and rainy, making the lidar-based navigation system highly adaptable and able to perform tasks in different seasons and different climatic conditions, ensuring the continuity and stability of agricultural operations, and in narrow working spaces such as between vegetable ridges, lidar can accurately obtain environmental data to ensure that operations are not interfered with by obstacles, and achieve accurate path planning and real-time positioning.

[0013] Preferably, the step S2 includes the following steps:

[0014] S21: coordinate transformation, coordinate transformation of point cloud data according to the installation position of the laser radar relative to the center of the power chassis, including coordinate translation and rotation;

[0015] S22: Extraction of regions of interest, trimming the point cloud according to relevant factors and retaining the point cloud area that needs attention;

[0016] S23: Voxel filtering: voxel filtering is performed on the point cloud data to reduce the point cloud density, thereby reducing the amount of calculation and facilitating feature extraction.

[0017] In this application, by preprocessing the point cloud data, trimming off the irrelevant regions to obtain the point cloud region that needs attention, the accuracy of the point cloud data is effectively improved. Then, voxel filtering is performed on it to reduce the point cloud density, thereby reducing the computational amount and facilitating feature extraction, improving the operation efficiency.

[0018] Preferably, in the step S3, it includes:

[0019] S31: Divide the point cloud data preprocessed in step S2 into point cloud regions by using a concentric circle model, divide the point cloud into multiple concentric circles with regular intervals, and each ring is used as an independent region;

[0020] S32: Perform region-level plane fitting based on principal component analysis, perform independent ground estimation on each region, and divide the point cloud in this region into ground points and non-ground points;

[0021] S33: Judge whether the point cloud in this region is ground point cloud by estimating the verticality, average height and flatness of each region, and fit all the ground point clouds.

[0022] By performing ground segmentation processing on the preprocessed point cloud data, the ground point cloud is segmented, so as to obtain the drivable region between the vegetable ridges. According to this drivable region, path planning can be carried out, so as to ensure that the agricultural machinery can drive stably along the predetermined track.

[0023] Preferably, in the step S32, it includes:

[0024] S321: Extract several points with the lowest height in a region as the initial seed point cloud;

[0025] S322: For the initial seed point cloud, calculate the seed normal vector n and the mean vector p;

[0026] S323: Calculate the distance threshold d according to n and p calculated in the above steps;

[0027] S324: Calculate the distance r between all points points i in the region and the seed plane, and then compare the calculated distance r with the distance threshold. If r < d, add this point to the seed point cloud and enter the next iteration. After several iterations, the obtained seed point cloud is the ground point cloud, and the rest are non-ground point clouds.

[0028] Thus, the ground points and non-ground points can be accurately distinguished by using the principal component analysis iterative method, effectively improving the accuracy of path planning and navigation between the vegetable ridges, and ensuring the safety of the operation.

[0029] Preferably, in the step S4, it includes:

[0030] S41: Calculate the centroid of the ground points of each area, and use the centroid of each area as the ground center point of the area;

[0031] S42: Fit the ground center point to a straight line through the least square method, and use the obtained straight line equation as the center line between the ridges.

[0032] By extracting the centroid of the ground point cloud in each area and then fitting these centroid points into a straight line using the least squares method, the center line between ridges is obtained, ensuring that the operating machinery can travel along the center line of the narrow working space such as between vegetable ridges, ensuring that the operation is not disturbed by obstacles, thereby achieving accurate path planning.

[0033] Preferably, the step S5 specifically includes: according to the set navigation preview distance, calculating the coordinates on the center line between ridges at the preview distance, and then calculating the equation of the straight line connecting the coordinates and the coordinate origin according to the coordinates, so as to obtain the navigation line equation.

[0034] According to the preset navigation preview distance, the coordinates of the center line between ridges are calculated and connected with the coordinate origin to obtain the navigation line equation, thereby obtaining an accurate navigation route between vegetable ridges, avoiding collisions between agricultural machinery and surrounding obstacles during operation, ensuring the safety of operations, and realizing the automation and intelligence of agricultural operations.

[0035] Preferably, step S6 includes:

[0036] S61: Calculate the angle between the navigation line equation and the forward direction coordinate axis, and use the angle as the output control amount;

[0037] S62: The output control amount is optimized by a position PID algorithm, and the optimized output control amount is used as the angular velocity of the power chassis and sent to the power chassis, thereby controlling the movement of the chassis.

[0038] After determining the navigation route in this application, the output control quantity is calculated using the angle between the navigation line equation and the forward direction coordinate axis. The output control quantity can be used to control the operating machinery to move along the navigation route, thereby realizing autonomous navigation. The position PID algorithm is also used to correct the output control quantity, and the optimized output control quantity is used as the angular velocity of the power chassis to control the chassis to execute autonomous walking instructions for movement, thereby further improving the accuracy of the operation, which can significantly save time and ensure the continuity and stability of agricultural operations.

[0039] On the other hand, the present invention also provides a laser radar-based vegetable ridge navigation system, the system comprising:

[0040] Data acquisition module: used to obtain three-dimensional point cloud data by measuring the robot's surrounding environment through three-dimensional laser radar;

[0041] Data preprocessing module: used to perform preliminary optimization of point cloud data through point cloud preprocessing algorithm to reduce the density and amount of calculation of point cloud data;

[0042] Navigation route acquisition module: used to process the optimized point cloud data to obtain the navigation line equation, thereby obtaining the navigation route;

[0043] Motion control module: Calculates the angular velocity control value through the navigation line equation parameters and sends it to the power chassis to control the chassis movement.

[0044] In this system, the data acquisition module is first used to obtain the three-dimensional point cloud data of environmental information through the laser radar, and then the data preprocessing module is used to preprocess these data. Finally, the navigation route acquisition module is used to process the optimized data to obtain accurate operation trajectory and path planning, ensuring that the agricultural machinery can travel stably along the predetermined track, especially in the environment with complex terrain and narrow crop row spacing such as vegetable ridges. The motion control module is also used to calculate and optimize the angular velocity control amount to control the movement of the mechanical chassis. The chassis can autonomously navigate and walk between the ridges of different vegetables to realize transportation, plant protection, inspection and other operations, which can effectively prevent the machinery from deviating from the track, reduce the risk of collision, and ensure efficiency and safety during the operation.

[0045] Preferably, the data preprocessing module includes:

[0046] Coordinate conversion unit: used to convert the point cloud data into coordinates according to the installation position of the laser radar relative to the center of the power chassis, including coordinate translation and rotation;

[0047] Region of Interest Extraction Unit: used to trim the point cloud according to relevant factors and retain the point cloud area that needs attention;

[0048] Voxel filtering unit: used to perform voxel filtering on point cloud data to reduce the point cloud density, thereby reducing the amount of calculation and facilitating feature extraction.

[0049] In the data preprocessing module, the point cloud data is transformed into coordinates and then irrelevant areas are cropped to obtain the point cloud area that needs attention, thereby effectively improving the accuracy of the point cloud data. Voxel filtering is then performed on the data to reduce the point cloud density, thereby reducing the amount of calculation and facilitating feature extraction, thereby improving work efficiency.

[0050] Preferably, the navigation route acquisition module includes:

[0051] The drivable area acquisition unit is used to perform point cloud area division of the pre-processed point cloud data using the concentric circle model, dividing the point cloud into multiple concentric circles with regular intervals, with each ring as an independent area; then perform regional plane fitting based on principal component analysis, perform independent ground estimation for each area, and divide the point cloud of the area into ground points and non-ground points; finally, determine whether the point cloud of this area is a ground point cloud by estimating the verticality, average height and flatness of each area, and fit all ground point clouds to obtain the drivable area;

[0052] The ridge centerline acquisition unit is used to calculate the centroid of the ground points in each area, and the centroid of each area is used as the ground center point of the area; then the ground center point is fitted into a straight line by the least square method, and the obtained straight line equation is used as the center line of the ridge;

[0053] Navigation line equation acquisition unit: used to calculate the coordinates on the center line of the ridge at the set navigation preview distance, and then calculate the straight line equation connecting the coordinates and the coordinate origin to obtain the navigation line equation, thereby obtaining the navigation route.

[0054] In the navigation route acquisition module, the drivable area acquisition unit is first used to obtain the drivable area between vegetable ridges, and path planning can be performed based on the drivable area, thereby ensuring that the agricultural machinery can stably travel along the predetermined track; then the ridge centerline acquisition unit is used to obtain the ridge centerline, thereby ensuring that the operating machinery can travel on the centerline of the narrow working space between vegetable ridges, ensuring that the operation is not interfered by obstacles, thereby achieving accurate path planning; finally, the navigation line equation acquisition unit is used to calculate the coordinates of the ridge centerline according to the preset navigation preview distance and connect it with the coordinate origin to obtain the navigation line equation, thereby obtaining an accurate vegetable ridge navigation route, avoiding collisions between agricultural machinery and surrounding obstacles during operation, ensuring the safety of operation, and realizing the automation and intelligence of agricultural operations.

[0055] Preferably, in the motion control module:

[0056] Output control quantity acquisition unit: used to calculate the angle between the navigation line and the forward direction coordinate axis according to the navigation line equation, and use the angle as the output control quantity;

[0057] The output control quantity optimization unit is used to optimize the output control quantity through a position PID algorithm, and send the optimized output control quantity to the power chassis as the angular velocity of the power chassis, thereby controlling the movement of the chassis.

[0058] In the motion control unit module, after obtaining the precise navigation route, the angle between the navigation route and the forward direction is calculated, and the angle is used as the output control quantity and optimized, so that the operating machine can be controlled to move along the navigation route to achieve autonomous navigation. It can also improve the accuracy of the operation, avoid the machine from deviating from the track, reduce the risk of collision, ensure the efficiency and safety during the operation, and improve work efficiency.

[0059] Compared with the prior art, the present invention has the following beneficial effects:

[0060] 1. The present invention is suitable for inter-ridge operation of vegetables. When the small power chassis moves between ridges, it can effectively detect and navigate the center of the ridges, and perform inspection, plant protection, transportation and other operations;

[0061] 2. The present invention utilizes laser radar to complete inter-ridge navigation, which has high real-time performance, high sensitivity, strong anti-interference ability, and little influence from the working environment.

[0062] 3. The present invention can adapt to different working environments and working objects, and is universal for ridge-to-ridge detection of different types of ridge-planted vegetables, and can be used for outdoor and indoor operations.

[0063] 4. The present invention has strong practicability and high reliability, can effectively reduce the labor intensity of vegetable production, improve operating efficiency, and provide technical support for the mechanization of vegetable production. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 A flow chart of the vegetable ridge navigation method provided by the present invention.

[0065] Figure 2 Schematic diagram of the laser radar coordinate system provided by the present invention.

[0066] Figure 3 This is a flow chart of the principal component analysis iterative method provided by the present invention.

[0067] Figure 4 This is a schematic diagram of the ground point cloud provided by the present invention.

[0068] Figure 5 This is a schematic diagram of the ridge centerline fitting provided by the present invention. DETAILED DESCRIPTION

[0069] The drawings of the present invention are only for illustrative purposes and should not be construed as limiting the present invention. In order to better illustrate the following embodiments, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of the actual product; it is understandable to those skilled in the art that some well-known structures and their descriptions in the drawings may be omitted.

[0070] Example 1

[0071] like Figure 1 As shown, this embodiment provides a vegetable ridge navigation method based on laser radar, and the method includes the following steps:

[0072] Step S1: Measure the robot's surrounding environment through a three-dimensional laser radar to obtain three-dimensional point cloud data;

[0073] Preferably, in step S1, according to Figure 2 The laser radar coordinate system shown is used to obtain the three-dimensional point cloud data of the robot's surrounding environment, where the forward direction is set to the x-axis, the forward direction is perpendicular to the x-axis and the left side is the y-axis, and the top is the z-axis.

[0074] Step S2: Preliminary optimization of the point cloud data is performed through a point cloud preprocessing algorithm to reduce the density and amount of computation of the point cloud data;

[0075] Preferably, the step S2 includes the following steps:

[0076] Step S21: coordinate conversion, coordinate conversion of the point cloud data according to the installation position of the laser radar relative to the center of the power chassis, including coordinate translation and rotation. In this embodiment, the laser radar is installed 0.3m in front of the center of the vehicle body, so the point cloud coordinates need to be moved forward 0.3m to complete the coordinate conversion and obtain accurate coordinate data.

[0077] Step S22: Extract the region of interest from the point cloud data and define the range of the ROI (e.g., x min ≤x≤x max ,y min ≤y≤y max , z min ≤x≤z max ), and then traverse the point cloud to crop the point cloud, retaining only the points that meet the conditions, thereby extracting the ROI area to retain the point cloud area that needs to be paid attention to. In this embodiment, the point cloud that satisfies (0<x<3, -1<y<1, -0.5<z<0) in the spatial coordinate system is retained.

[0078] Step S23: Voxel filtering, dividing the point cloud into several small cubes with a side length of a, for each point {p1, p2, ..., p m}, the coordinates of each point are (x i ,y i , z i ), belong to the same voxel, and then calculate the coordinates of its center of mass (x c ,y c , z c ), and its calculation formula is:

[0079]

[0080] Next, the coordinates of the centroid are used to approximate several points within the cube, so that only one point is retained within each cube, reducing the point cloud density. In this embodiment, the side length a is set to 0.05 meters, so that the point cloud is divided into multiple small cubes with a side length of 0.05 meters. For the points within each small cube, the centroid of each small cube is calculated using the above formula, and the coordinates of the centroid are used to approximate several points within the cube, so that only one point is retained within each cube, reducing the point cloud density to reduce the computational amount and facilitate feature extraction.

[0081] Thus, through the above steps, the point cloud data is preprocessed, the irrelevant regions are cropped to obtain the point cloud region that needs to be concerned, effectively improving the accuracy of the point cloud data. Then, voxel filtering is performed on it to reduce the point cloud density, thereby reducing the computational amount and facilitating feature extraction, improving the operation efficiency.

[0082] Step S3: Through the ground segmentation algorithm, segment out the ground point cloud and use the ground point cloud as the drivable area;

[0083] Preferably, in the step S3, it includes:

[0084] Step S31: Perform point cloud region division of the concentric circle model on the point cloud data preprocessed in step S2, divide the point cloud into multiple concentric circles with regular intervals, and each ring is used as an independent region; specifically, use the polar coordinate method to divide the point cloud into multiple concentric circles with regular intervals, and each ring is used as an independent region. In this embodiment, the point cloud within a radius of 3m is divided into 8 concentric rings.

[0085] Step S32: Perform region-level plane fitting based on principal component analysis, perform independent ground estimation on each region, and divide the point cloud of the region into ground points and non-ground points;

[0086] As Figure 3 shown, based on the principal component analysis iterative method, perform independent ground estimation on each region, and divide the point cloud of the region into ground points and non-ground points. Specifically, first extract several point clouds with the lowest height in a region as the initial seed point cloud, calculate the seed normal vector n and the mean vector p, calculate the distance threshold d through n and p, then calculate the distance r between all points in the region and the seed plane. When r < d, add the point to the seed point cloud and enter the next iteration. After several iterations, the seed point cloud is obtained as the ground point cloud, and the rest are non-ground point clouds, thus realizing the division of the ground points and non-ground points of the point cloud in this region.

[0087] Specifically, it includes:

[0088] S321: extracting several points with the lowest height in a region as the initial seed point cloud, wherein in this embodiment, the 10 points with the lowest height in a region are extracted as the initial seed point cloud,

[0089] S322: For the initial seed point cloud, calculate the seed normal vector n and the mean vector p, where:

[0090] The calculation of the normal vector n includes:

[0091] (1) For all points (x i ,y i , z i ), calculate the center of mass of the regional point cloud (c x , c y , c z ):

[0092]

[0093] (2) Calculate the offset vector from the point to the center of mass. The calculation formula is:

[0094] q i =(x i -c x ,y i -c y ,z i -c z )

[0095] (3) Calculate the covariance matrix, the calculation formula is:

[0096]

[0097] (4) Perform eigenvalue decomposition, wherein the purpose of the eigenvalue decomposition is to decompose the covariance matrix into the following form:

[0098] C=VΛV T

[0099] Where Λ is a diagonal matrix containing the eigenvalues ​​λ1, λ2, λ3 of the covariance matrix, V is an orthogonal matrix whose column vectors are the eigenvectors corresponding to the eigenvalues, and V T is the transpose of an orthogonal matrix.

[0100] (5) Solve the eigenvalue and eigenvector: Solve the following eigenvalue equation:

[0101] det(C-λI)=0

[0102] Where I is the unit matrix, λ is the eigenvalue of the covariance matrix, and by solving the cubic equation, we get the eigenvalues ​​λ1, λ2, λ3. For each eigenvalue λ1, λ2, λ3, we solve the following linear equations to get the corresponding eigenvectors v1, v3, v3:

[0103] (C-λ i I)v i =0

[0104] Where λ i is the eigenvalue, v i is the eigenvector corresponding to the eigenvalue.

[0105] (6) Take the smallest eigenvalue λ among the three eigenvalues ​​λ1, λ2, and λ3 min , this minimum eigenvalue λ min Represents the direction of minimum change of the local surface, and the corresponding eigenvector v min As the normal vector n of the point cloud area.

[0106] The calculation of the mean vector p includes:

[0107] For all points in the region (x i ,y i , z i ), the mean vector p represents the average value of all point coordinates:

[0108]

[0109] In the above formula:

[0110]

[0111] S323: Calculating a distance threshold d according to n and p calculated in the above steps, including:

[0112] Calculate the normal vector n(n x , n y , n z ) and the mean vector p(p x , p y , p z )’s dot product k:

[0113] k=p·n=p x n x +p y n y +p z n z

[0114] Let the distance threshold d be equal to k, then:

[0115] d=k

[0116] S324: Calculate the distances r of all points points within the calculation area i from the seed plane, through the coordinates of each point (points x , points y , points z ) by calculating the dot product with the normal vector n:

[0117] r = points i ·n = points x n x + points y n y + points z n z

[0118] Then compare the calculated distance r with the distance threshold. If r < d, add this point to the seed point cloud and enter the next iteration. After several iterations, the obtained seed point cloud is the ground point cloud, and the rest is the non-ground point cloud, thus realizing the division of ground points and non-ground points in the point cloud of this area. For the number of iterations, those skilled in the art can set it according to the actual situation. In this embodiment, after algorithm testing and optimization, three iterations are optimal. Therefore, the number of iterations is set to three in this embodiment.

[0119] Step S33: Then, judge the ground point cloud obtained in step S32 again, and judge whether the point cloud in this area is the ground point cloud by estimating the verticality, average height and flatness of each area, and fit all the ground point clouds in each area. Specifically, use the plane normal vector to evaluate the verticality of the plane, use the plane mean vector to evaluate the average height of the plane, use the plane singular value to evaluate the flatness of the plane, and finally integrate all the ground point clouds. The obtained ground point cloud is as Figure 4 shown. The specific descriptions of the above three evaluations are as follows:

[0120] (1) Verticality evaluation: It is the third element value n3 of the normal vector n, representing the verticality of the ground, and the threshold is 0.7.

[0121] (2) Height evaluation: It is the third element value p3 of the mean vector p, representing the height of the plane, and the threshold is -0.5.

[0122] (3) Flatness evaluation: Calculate the ratio of the minimum singular value σ min to the total singular value (σ1 + σ2 + σ3), which is used to measure the flatness of the current area, and the threshold is 0.005. The singular value σ i is the square root of the eigenvalue λ i :

[0123]

[0124] When n3<0.7, p3<-0.5, σ min When / (σ1+σ2+σ3)<0.005, the point cloud in this area is considered to meet the ground features and is classified as a ground point, otherwise it is classified as a non-ground point.

[0125] Therefore, in this embodiment, the ground segmentation algorithm is used to perform accurate ground segmentation processing on the preprocessed point cloud data, and the ground point cloud is segmented, so as to obtain the drivable area between the vegetable ridges. Path planning can be carried out based on the drivable area to ensure that the agricultural machinery can travel stably along the predetermined track.

[0126] Step S4: extracting the centroid of the ground point cloud and taking it as the center point between ridges, and then fitting the center point between ridges to obtain the center line equation between ridges;

[0127] Preferably, step S4 includes:

[0128] Step S41: Calculate the centroid of the ground points of each area, and use the centroid of each area as the ground center point of the area;

[0129] Step S42: Fit the ground center point into a straight line by the least square method, and use the obtained straight line equation as the center line between the ridges. The obtained center line is as follows: Figure 5 shown.

[0130] Therefore, by utilizing the obtained center line between ridges, it can be ensured that the operating machinery can travel on the center line of the narrow working space such as between vegetable ridges, ensuring that the operation is not disturbed by obstacles, thereby achieving accurate path planning.

[0131] Step S5: setting the navigation preview distance, and calculating the navigation line equation at the preview distance;

[0132] Preferably, step S5 specifically includes: setting a suitable navigation preview distance, the specific value of the navigation preview distance can be set by those skilled in the art according to actual factors, and then calculating the coordinates on the center line between ridges at the preview distance, and then calculating the equation of the straight line connecting the coordinates and the origin of the coordinates based on the coordinates, thereby obtaining the navigation line equation.

[0133] Specifically, after reaching the center line between the ridges, it is necessary to track the center line between the ridges, use the preview tracking algorithm to track the path of the center line, set the preview distance of the navigation line, calculate the coordinates on the navigation line between the ridges at the preview distance (x coordinate), and calculate the equation of the straight line connecting the coordinates and the coordinate origin as the navigation line equation.

[0134] Assume that the equation of the center line between ridges is:

[0135] Ax+By+C=0

[0136] Set the navigation preview distance to αm, then substitute x=α into the ridge centerline equation to obtain the coordinates on the centerline:

[0137] (α,-(Aα+C) / B)

[0138] Then, the line equation between the origin (0,0) and (α,-(Aα+C) / B) is calculated to obtain the navigation line equation, which is as follows:

[0139] [(Aα+C) / Bα]x+y=0

[0140] In this embodiment, if the obtained equation of the center line between ridges is x-2y+1=0, and the navigation preview distance is set to 3m, then x=3 is substituted into the equation of the center line between ridges to obtain the coordinates (3,2), and then the equation of the straight line connecting the origin (0,0) and (3,2) is calculated, so that the equation of the straight line can be obtained as 2x-3y=0.

[0141] Therefore, in this embodiment, by obtaining accurate navigation routes between vegetable ridges, collisions between agricultural machinery and surrounding obstacles during operation can be avoided, the safety of the operation can be ensured, and the automation and intelligence of agricultural operations can be realized.

[0142] Step S6: Calculate the angular velocity control value through the navigation line equation parameters, and send it to the power chassis to control the chassis movement.

[0143] Preferably, step S6 includes:

[0144] Step S61: Calculate the angle between the navigation line equation and the coordinate axis of the advancing direction, and use the angle as the output control amount; specifically, the calculation formula for calculating the angle between the navigation line equation and the x-axis of the advancing direction is:

[0145] θ=tan -1 [-(Aα+C) / Bα]

[0146] In this embodiment, the known equation of the navigation line is 2x-3y=0. Using the above equation, it can be calculated that the angle between the straight line and the x-axis is approximately 0.588 rad;

[0147] Step S62: Optimize the output control amount through the position PID algorithm, and use the optimized output control amount as the power chassis angular velocity (rad / s), and send it to the power chassis to control the chassis movement. Specifically including:

[0148] The angle θ (rad) between the navigation line equation and the forward direction coordinate axis is used as the PID algorithm input, that is, the deviation value, and the power chassis angular velocity z (rad / s) is used as the output control quantity. The PID algorithm formula is:

[0149]

[0150] In the formula, u(t) is the control output, e(t) is the deviation, K P is the proportional gain, K I is the integral gain, K D is the differential gain, and then the deviation value θ is substituted to calculate the angular velocity of the power chassis:

[0151] z=K P *θ+K I *∑(θ)+K D *(θ-θ ′ )

[0152] Among them, θ ′ Indicates the last deviation.

[0153] Thus, in this embodiment, the control amount is adjusted by using the proportional term and the deviation value (i.e., the angle between the navigation line direction and the forward direction). If the deviation is large, the proportional term increases, and accordingly, the chassis will adjust its direction more strongly; secondly, the integral term is used to consider the accumulation of the deviation over time, which helps the navigation route eliminate the long-standing steady-state error, and by adjusting the accumulation of the deviation, the deviation of the chassis can be gradually reduced over time until the target navigation route is reached; then the differential term is used to adjust according to the rate of change of the deviation, thereby playing a predictive role, helping to avoid excessive adjustment of the output control amount, thereby reducing overshoot in the process of optimizing the navigation route; finally, the sum of the output proportional, integral, and differential terms is used as the final output control amount to obtain the angular velocity of the power chassis, control the chassis to rotate toward the target direction, gradually eliminate the angle deviation between the navigation line direction and the current direction, ensure that the chassis travels smoothly along the target navigation route, effectively improve the accuracy of the operation, can significantly save time, and ensure the continuity and stability of agricultural operations.

[0154] Example 2

[0155] This embodiment provides a vegetable ridge navigation system based on laser radar based on the method provided in Embodiment 1, and the system includes:

[0156] Data acquisition module: used to obtain three-dimensional point cloud data by measuring the robot's surrounding environment through three-dimensional laser radar;

[0157] Data preprocessing module: used to perform preliminary optimization of point cloud data through point cloud preprocessing algorithm to reduce the density and amount of calculation of point cloud data;

[0158] Navigation route acquisition module: used to process the optimized point cloud data to obtain the navigation line equation, thereby obtaining the navigation route;

[0159] Motion control module: Calculates the angular velocity control value through the navigation line equation parameters and sends it to the power chassis to control the chassis movement.

[0160] In the system described in this embodiment, the data acquisition module is first used to acquire three-dimensional point cloud data of environmental information through laser radar, and then the data preprocessing module is used to preprocess these data. Then, the navigation route acquisition module is used to process the optimized data to obtain accurate operation trajectories and path planning, thereby ensuring that the agricultural machinery can travel stably along the predetermined track, especially in an environment with complex terrain and narrow crop row spacing such as vegetable ridges. The motion control module is also used to calculate and optimize the angular velocity control amount, thereby controlling the movement of the mechanical chassis, which can effectively prevent the machine from deviating from the track, reduce the risk of collision, and ensure efficiency and safety during the operation.

[0161] Preferably, the data preprocessing module includes:

[0162] Coordinate conversion unit: used to convert the point cloud data into coordinates according to the installation position of the laser radar relative to the center of the power chassis, including coordinate translation and rotation;

[0163] Region of interest extraction unit: used to crop the point cloud according to relevant factors and retain the point cloud area that needs attention; the relevant factors may be ridge distance, lidar installation height, etc. In this embodiment, point clouds that satisfy (0<x<3, -1<y<1, -0.5<z<0) in the spatial coordinate system are retained.

[0164] Voxel filtering unit: used to perform voxel filtering on point cloud data to reduce the density of point cloud, so as to reduce the amount of calculation and facilitate feature extraction. In this embodiment, by setting a threshold of 0.05 meters, the point cloud is divided into multiple small cubes with a side length of 0.05 meters. For each point in each small cube, the centroid of each small cube is calculated, and the coordinates of the centroid are used to approximate several points in the cube, so that only one point is retained in each cube, reducing the density of the point cloud.

[0165] In the data preprocessing module, the point cloud data is transformed into coordinates and then irrelevant areas are cropped to obtain the point cloud area that needs attention, thereby effectively improving the accuracy of the point cloud data. Voxel filtering is then performed on the data to reduce the point cloud density, thereby reducing the amount of calculation and facilitating feature extraction, thereby improving work efficiency.

[0166] Preferably, the navigation route acquisition module includes:

[0167] A drivable area acquisition unit: used to divide the point cloud data after preprocessing into point cloud regions by a concentric circle model, divide the point cloud into multiple concentric circles with regular intervals, each ring as an independent region, and in this embodiment, the point cloud within a radius of 3m is divided into 8 concentric rings; then perform region-level plane fitting based on principal component analysis, perform independent ground estimation on each region, and divide the point cloud in this region into ground points and non-ground points. Specifically, first extract the 10 point clouds with the lowest height in a region as the initial seed point cloud, calculate the seed normal vector n and the mean vector p, calculate the distance threshold d through n and p, then calculate the distance r between all points in the region and the seed plane, when r < d, add this point to the seed point cloud, enter the next iteration, after three iterations, obtain the seed point cloud as the ground point cloud, and the rest as non-ground point clouds; finally, judge whether the point cloud in this region is the ground point cloud by estimating the verticality, average height and flatness of each region, and fit all the ground point clouds, and the obtained ground point cloud is as Figure 4 shown, so as to obtain the drivable area;

[0168] An inter-ridge centerline acquisition unit: used to calculate the centroid of the ground points in each region, and the centroid of each region is used as the ground center point of this region; then fit the ground center points into a straight line by the least squares method, and use the obtained straight line equation as the centerline between the ridges, and the obtained centerline is as Figure 5 shown;

[0169] A navigation line equation acquisition unit: used to calculate the coordinates on the inter-ridge centerline at the set navigation preview distance, and then calculate the straight line equation of the line connecting it with the coordinate origin according to this coordinate to obtain the navigation line equation, so as to obtain the navigation route. Specifically, in this embodiment, if the obtained inter-ridge centerline equation is x - 2y + 1 = 0 and the set navigation preview distance is 3m, then substitute x = 3 into the inter-ridge centerline equation to obtain the coordinates (3, 2), and then calculate the straight line equation of the line connecting the origin (0, 0) and (3, 2), so as to obtain the straight line equation as 2x - 3y = 0.

[0170] Therefore, in the navigation route acquisition module, the drivable area acquisition unit is first used to obtain the drivable area between vegetable ridges, and path planning can be performed based on the drivable area, thereby ensuring that the agricultural machinery can stably travel along the predetermined track; then the ridge centerline acquisition unit is used to obtain the ridge centerline, thereby ensuring that the operating machinery can travel on the centerline of the narrow working space between vegetable ridges, ensuring that the operation is not interfered by obstacles, thereby achieving accurate path planning; finally, the navigation line equation acquisition unit is used to calculate the coordinates of the ridge centerline according to the preset navigation preview distance and connect it with the coordinate origin to obtain the navigation line equation, thereby obtaining an accurate vegetable ridge navigation route, avoiding collisions between agricultural machinery and surrounding obstacles during operation, ensuring the safety of operation, and realizing automation and intelligence of agricultural operations.

[0171] Preferably, in the motion control module:

[0172] Output control quantity acquisition unit: used to calculate the angle between the navigation line and the forward direction coordinate axis according to the navigation line equation, and use the angle as the output control quantity; specifically, in this embodiment, the forward axis is assumed to be the x-axis, and the navigation line equation is known to be 2x-3y=0, then it can be calculated that the angle between the straight line and the x-axis is approximately 0.588rad.

[0173] The output control quantity optimization unit is used to optimize the output control quantity through a position PID algorithm, and send the optimized output control quantity to the power chassis as the angular velocity of the power chassis, thereby controlling the movement of the chassis.

[0174] The output control amount calculated in the output control amount acquisition unit is used as the error of the working machine moving along the navigation line, and is corrected by the position PID algorithm so that the chassis moves along the target path. Specifically:

[0175] First, the proportional term is used to adjust the control amount through the deviation value (i.e., the angle between the navigation line direction and the forward direction). If the deviation is large, the proportional term increases, and accordingly, the chassis will adjust its direction more strongly; secondly, the integral term is used to consider the accumulation of deviation over time, helping the navigation route to eliminate long-standing steady-state errors, and by adjusting the accumulation of deviations, the chassis deviation can be gradually reduced over time until the target navigation route is reached; then the differential term is used to adjust according to the rate of change of the deviation, thereby playing a predictive role, helping to avoid excessive adjustment of the output control amount, thereby reducing overshoot in the process of optimizing the navigation route; finally, the sum of the output proportional, integral, and differential terms is used as the final output control amount to obtain the angular velocity of the power chassis, control the chassis to rotate toward the target direction, gradually eliminate the angle error between the navigation line direction and the current direction, and ensure that the chassis travels smoothly along the target navigation route.

[0176] Therefore, in the motion control unit module, after obtaining the precise navigation route, the angle between the navigation route and the forward direction is calculated, and the angle is used as the output control quantity and optimized, so that the operating machine can be controlled to move along the navigation route to achieve autonomous navigation. It can also improve the accuracy of the operation, prevent the machine from deviating from the track, reduce the risk of collision, ensure the efficiency and safety of the operation process, and improve work efficiency.

[0177] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the technical solution of the present invention, and are not intended to limit the specific implementation methods of the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the claims of the present invention shall be included in the protection scope of the claims of the present invention.

Claims

1. A vegetable ridge navigation method based on laser radar, characterized in that: The method comprises the following steps: S1: Measure the robot's surroundings through 3D laser radar to obtain 3D point cloud data; S2: Preliminary optimization of point cloud data is performed through point cloud preprocessing algorithm to reduce the density and computational complexity of point cloud data; S3: Segment the ground point cloud through the ground segmentation algorithm and use the ground point cloud as the drivable area; S4: extract the centroid of the ground point cloud and use it as the center point between ridges, then fit the center point between ridges to obtain the center line equation between ridges; S5: Set the navigation preview distance and calculate the navigation line equation at the preview distance; S6: Calculate the angular velocity control value through the navigation line equation parameters and send it to the power chassis to control the chassis movement.

2. A vegetable ridge navigation method based on laser radar according to claim 1, characterized in that: The step S2 includes the following steps: S21: coordinate transformation, coordinate transformation of point cloud data according to the installation position of the laser radar relative to the center of the power chassis, including coordinate translation and rotation; S22: Extraction of regions of interest, trimming the point cloud according to relevant factors and retaining the point cloud area that needs attention; S23: Voxel filtering: voxel filtering is performed on the point cloud data to reduce the point cloud density, thereby reducing the amount of calculation and facilitating feature extraction.

3. A laser radar based vegetable ridge navigation method according to claim 2, characterized in that: The step S3 includes: S31: performing point cloud area division of a concentric circle model on the point cloud data preprocessed in step S2, dividing the point cloud into a plurality of concentric circles with regular intervals, and each circle serves as an independent area; S32: Perform regional plane fitting based on principal component analysis, perform independent ground estimation for each region, and divide the point cloud of the region into ground points and non-ground points; S33: Determine whether the point cloud in this area is a ground point cloud by estimating the verticality, average height and flatness of each area, and fit all the ground point clouds.

4. The method for navigating between vegetable ridges based on laser radar according to claim 3, characterized in that: The step S32 includes: S321: extracting a number of points with the lowest height in a region as an initial seed point cloud; S322: For the initial seed point cloud, calculate the seed normal vector n and the mean vector p; S323: Calculate the distance threshold d according to n and p calculated in the above steps; S324: Calculate the distances r of all points points within the region from the seed plane, then compare the calculated distance r with the distance threshold. If r < d, add this point to the seed point cloud and proceed to the next iteration. After several iterations, the resulting seed point cloud is the ground point cloud, and the rest are non-ground point clouds. i The distances r of all points points within the region from the seed plane are calculated, and then the calculated distance r is compared with the distance threshold. If r < d, this point is added to the seed point cloud and the next iteration is entered. After several iterations, the resulting seed point cloud is the ground point cloud, and the rest are non-ground point clouds.

5. The method for navigating between vegetable ridges based on laser radar according to claim 3, characterized in that: The step S4 includes: S41: Calculate the centroid of the ground points of each area, and use the centroid of each area as the ground center point of the area; S42: Fit the ground center point to a straight line through the least square method, and use the obtained straight line equation as the center line between the ridges.

6. The method for navigating between vegetable ridges based on laser radar according to claim 5, characterized in that: The step S5 specifically includes: according to the set navigation preview distance, calculating the coordinates on the center line between ridges at the preview distance, and then calculating the straight line equation connecting the coordinates and the coordinate origin according to the coordinates, so as to obtain the navigation line equation.

7. The method for navigating between vegetable ridges based on laser radar according to claim 6, characterized in that: The step S6 includes: S61: Calculate the angle between the navigation line equation and the forward direction coordinate axis, and use the angle as the output control amount; S62: The output control amount is optimized by a position PID algorithm, and the optimized output control amount is used as the angular velocity of the power chassis and sent to the power chassis, thereby controlling the movement of the chassis.

8. A navigation system for the vegetable ridge navigation method based on laser radar according to any one of claims 1 to 7, characterized in that: The system comprises: Data acquisition module: used to obtain three-dimensional point cloud data by measuring the robot's surrounding environment through three-dimensional laser radar; Data preprocessing module: used to perform preliminary optimization of point cloud data through point cloud preprocessing algorithm to reduce the density and amount of calculation of point cloud data; Navigation route acquisition module: used to process the optimized point cloud data to obtain the navigation line equation, thereby obtaining the navigation route; Motion control module: calculates the angular velocity control value through the navigation line equation parameters, and sends it to the power chassis to control the movement of the chassis; wherein, in the motion control module: Output control quantity acquisition unit: used to calculate the angle between the navigation line and the forward direction coordinate axis according to the navigation line equation, and use the angle as the output control quantity; The output control quantity optimization unit is used to optimize the output control quantity through a position PID algorithm, and send the optimized output control quantity to the power chassis as the angular velocity of the power chassis, thereby controlling the movement of the chassis.

9. The laser radar-based vegetable ridge navigation system according to claim 8, characterized in that: The data preprocessing module includes: Coordinate conversion unit: used to convert the point cloud data into coordinates according to the installation position of the laser radar relative to the center of the power chassis, including coordinate translation and rotation; Region of Interest Extraction Unit: used to trim the point cloud according to relevant factors and retain the point cloud area that needs attention; Voxel filtering unit: used to perform voxel filtering on point cloud data to reduce the point cloud density, thereby reducing the amount of calculation and facilitating feature extraction.

10. The laser radar-based vegetable ridge navigation system according to claim 9, characterized in that: The navigation route acquisition module includes: The drivable area acquisition unit is used to perform point cloud area division of the pre-processed point cloud data using the concentric circle model, dividing the point cloud into multiple concentric circles with regular intervals, with each ring as an independent area; then perform regional plane fitting based on principal component analysis, perform independent ground estimation for each area, and divide the point cloud of the area into ground points and non-ground points; finally, determine whether the point cloud of this area is a ground point cloud by estimating the verticality, average height and flatness of each area, and fit all ground point clouds to obtain the drivable area; The ridge centerline acquisition unit is used to calculate the centroid of the ground points in each area, and the centroid of each area is used as the ground center point of the area; then the ground center point is fitted into a straight line by the least square method, and the obtained straight line equation is used as the center line of the ridge; Navigation line equation acquisition unit: used to calculate the coordinates on the center line of the ridge at the set navigation preview distance, and then calculate the straight line equation connecting the coordinates and the coordinate origin to obtain the navigation line equation, thereby obtaining the navigation route.

Images