Orchard vehicle headland navigation method based on two-dimensional laser radar

Abstract

A method for headland navigation of an orchard vehicle based on a two-dimensional laser radar, the method comprising a headland navigation start detection step, a headland navigation path extraction step, a headland navigation deviation calculation step, a headland navigation path tracking step, and a headland navigation end detection step; the orchard vehicle headland navigation method based on 2D LiDAR proposed by the present invention obtains the deviation between the vehicle position and the circular arc navigation path in real time through 2D LiDAR, and guides the vehicle to travel along the circular arc navigation path. This closed-loop control method has a strong anti-interference ability, can adapt to complex situations where the vehicle has a large lateral deviation / heading deviation at the headland and a large ground roughness, and is a supplement and improvement to the existing orchard vehicle navigation method based on 2D LiDAR.

Description

Technical Field

[0001] The present invention relates to the field of orchard vehicle navigation, in particular to orchard vehicle headland navigation, and specifically to an orchard vehicle headland navigation method based on two-dimensional laser radar Background Art

[0002] Orchard vehicle navigation technology is one of the core technologies for achieving refined orchard management. It is widely used in orchard production processes such as plant protection, weeding, and picking. It can effectively improve the accuracy and efficiency of orchard operations and reduce the labor intensity of operators.

[0003] Two-dimensional laser radar (Light detection and ranging, LiDAR) has the advantages of high ranging accuracy, high resolution, and strong anti-interference ability. It is widely used in orchard environmental perception and fruit tree information extraction, and has become a research hotspot in the field of orchard vehicle navigation. When installed, the scanning surface of the two-dimensional laser radar is parallel to the ground, and the contour information of the fruit tree trunks around the vehicle is obtained in real time, from which the navigation path is extracted, the deviation between the current position of the vehicle and the navigation path is calculated, and the vehicle is guided along the navigation path.

[0004] The above-mentioned two-dimensional laser radar-based orchard vehicle navigation method is only applied to orchard row navigation. When the vehicle turns at the headland, open-loop control is usually used to make the vehicle travel a circular arc at a fixed speed and heading angle. This method is simple and easy to implement, but has poor anti-interference ability and is difficult to adapt to complex situations where the vehicle has large lateral deviation / heading deviation at the headland and large ground roughness. Summary of the invention

[0005] The purpose of the present invention is to solve the problem of orchard vehicle headland navigation. The present invention proposes an orchard vehicle headland navigation method based on 2D LiDAR. During the headland turning process, the vehicle posture is adjusted according to the deviation between the real-time acquired vehicle position and the arc navigation path, and the vehicle is guided to travel along the arc navigation path.

[0006] The technical solution of the present invention is:

[0007] The present invention provides an orchard vehicle headland navigation method based on a two-dimensional laser radar, the method comprising:

[0008] S1, start detection of end-of-field navigation: obtain two-dimensional laser radar data to determine whether the vehicle has reached the end of the orchard row. If it has reached the end, execute S2 to start end-of-field navigation;

[0009] S2, ground navigation path extraction: extracting arc navigation path from 2D lidar data;

[0010] S3, Headland navigation deviation calculation: Calculate the lateral deviation e between the current vehicle position and the arc navigation path l and heading deviation eh ;

[0011] S4, Headland navigation path tracking: According to the lateral deviation e l and heading deviation e h , calculate the expected speed of the left and right wheels The vehicle is guided to travel along the circular navigation path, and after a time Δt, S5 is executed;

[0012] S5, end-of-field navigation end detection: obtain two-dimensional laser radar data to determine whether the vehicle has reached the end of the orchard row. If it has arrived, end the end-of-field navigation, otherwise, execute S2.

[0013] Furthermore, S1 is specifically:

[0014] S1-1. Extract the navigation path based on the 2D laser radar data and calculate the vehicle lateral deviation e l and heading deviation e h ;

[0015] S1-2, according to the vehicle lateral deviation e l and heading deviation e h , coordinate correction of two-dimensional lidar data;

[0016]

[0017] Among them, the origin of the coordinate system of the two-dimensional laser radar data is located at the center of the radar, the positive direction of the y-axis points to the forward direction of the vehicle, and the positive direction of the x-axis points to the right side of the vehicle; (x, y) and (x′, y′) are the data coordinates of the two-dimensional laser radar before and after correction respectively;

[0018] S1-3, judge the data coordinates of the calibrated two-dimensional laser radar. If the horizontal coordinate [-2d r ,2d r ], vertical coordinate [0,2d r ] There is no 2D LiDAR data in the rectangular area enclosed by the vehicle, then the vehicle reaches the end of the line, d r Represents the row matrix of an orchard.

[0019] Furthermore, the navigation path extraction method is an inter-line navigation path extraction algorithm.

[0020] Furthermore, the circular navigation path takes the position C of the last tree trunk at the turning side as the center, and the fruit tree row spacing d r is the diameter; the position of the center C is obtained by the following steps:

[0021] When executing for the first time:

[0022] Filter out the 4D rThe two-dimensional laser radar data in the square area with a side length of is clustered to obtain several tree trunk clusters; the center of gravity of each tree trunk cluster is calculated. If it is a right turn, the center of gravity of the tree trunk cluster with the largest sum of the horizontal coordinate and the vertical coordinate is taken as the center of the circle C; if it is a left turn, the center of gravity of the tree trunk cluster with the largest difference between the vertical coordinate and the horizontal coordinate is taken as the center of the circle C;

[0023] The second execution starts:

[0024] According to the coordinates of the center C at the previous moment Estimate its position at the current time

[0025]

[0026] in: They represent the expected speed of the left and right wheels at the last moment obtained by S4, Δt represents the time interval between two adjacent navigations, D represents the wheelbase of the vehicle, d represents the distance between the center of mass of the vehicle and the geometric centers of the two driving wheels, Δθ represents the change in the heading angle of the vehicle within the Δt time interval,

[0027] Filter by Centered on 4D t The 2D laser radar data in the square area with a side length of , obtain the center of gravity of the data as the actual position of the center C at the current moment (x C ,y C ), d t It is the maximum breast diameter of fruit trees in the orchard.

[0028] Furthermore, the clustering adopts the density-based clustering method DBSCAN with noise, the parameter scanning radius is 0.1m, and the minimum number of neighborhood points is 1.

[0029] Furthermore, S3 is specifically:

[0030] Assume that the intersection of the line connecting the radar center, i.e. the origin O, and the center C of the arc navigation path and the arc navigation path is G. The lateral deviation is the distance between the origin O and G, with O being positive when it is to the right of G and negative when it is to the left. Draw an arc tangent through point G. The heading deviation is the angle between the vehicle direction and the tangent, with the vehicle deviation to the right being positive and the vehicle deviation to the left being negative.

[0031] The lateral deviation e is calculated using the following formula: l and heading deviation e h :

[0032]

[0033]

[0034] Furthermore, in S4, the fuzzy adaptive pure tracking algorithm is used to calculate the desired vehicle speeds of the left and right wheels.

[0035] Furthermore, S5 includes:

[0036] S5-1. According to the lateral deviation e obtained in S4 l and heading deviation e h , perform coordinate correction on the two-dimensional laser radar data according to S1-2;

[0037] S5-2, line head detection: when turning right, if the horizontal coordinate [-2d r ,0], vertical coordinate [-2d t ,2d t ] contains two-dimensional laser radar data, indicating that the vehicle has reached the head of the line; when turning left, if the horizontal coordinate [0,2d r ], vertical coordinate [-2d t ,2d t ] There is two-dimensional lidar data in the rectangular area enclosed by , indicating that the vehicle has arrived at the head of the line.

[0038] Furthermore, in S1 and S5, the acquired two-dimensional lidar data is collected one frame each time.

[0039] Beneficial effects of the present invention:

[0040] The orchard vehicle headland navigation method based on 2D LiDAR proposed by the present invention obtains the deviation between the vehicle position and the circular navigation path in real time through 2D LiDAR, and guides the vehicle to travel along the circular navigation path. This closed-loop control method has strong anti-interference ability and can adapt to complex situations where the vehicle has large lateral deviation / course deviation at the headland and large ground roughness, which is a supplement and improvement to the existing orchard vehicle navigation method based on 2D LiDAR.

[0041] Other features and advantages of the present invention will be described in detail in the following detailed description. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The above and other objects, features and advantages of the present invention will become more apparent through a more detailed description of exemplary embodiments of the present invention in conjunction with the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present invention.

[0043] Figure 1 The orchard vehicle headland navigation flow chart of the present invention is shown;

[0044] Figure 2 A schematic diagram showing the comparison of 2D LiDAR data coordinates before and after correction in an embodiment is shown.

[0045] Figure 3 A schematic diagram of the navigation deviation position in the embodiment is shown;

[0046] Figure 4 A schematic diagram comparing vehicle trajectories navigated by the navigation method of the present invention and the open-loop control method is shown. DETAILED DESCRIPTION

[0047] The preferred embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present invention are shown in the accompanying drawings, it should be understood that the present invention can be implemented in various forms and should not be limited to the embodiments set forth herein.

[0048] The present invention provides a method for navigating an orchard vehicle headland based on a two-dimensional laser radar. Figure 1 As shown, the method includes:

[0049] S1, start detection of end-of-field navigation: collect 1 frame of 2D LiDAR data to determine whether the vehicle has reached the end of the orchard row. If it has, execute S2 to start end-of-field navigation, specifically:

[0050] S1-1. Use the inter-row navigation path extraction algorithm (select an existing algorithm, such as Li Qiujie, Ding Xudong, Deng Xian. Orchard inter-row path extraction and navigation based on laser radar [J]. Transactions of the Chinese Society of Agricultural Machinery, 2020, 51(s2): 344-350.) to extract the navigation path and calculate the vehicle lateral deviation e l and heading deviation e h ;

[0051] S1-2, according to the vehicle lateral deviation e l and heading deviation e h , coordinate correction is performed on the two-dimensional lidar data, such as Figure 2 The following is a comparison diagram of 2D LiDAR data before and after coordinate correction. Left: 2D LiDAR data before correction. Right: 2D LiDAR data after correction.

[0052]

[0053] Among them, the origin of the coordinate system of the two-dimensional laser radar data is located at the center of the radar, the positive direction of the y-axis points to the forward direction of the vehicle, and the positive direction of the x-axis points to the right side of the vehicle; (x, y) and (x′, y′) are the data coordinates of the two-dimensional laser radar before and after correction respectively;

[0054] S1-3, judge the data coordinates of the calibrated two-dimensional laser radar. If the horizontal coordinate [-2s r ,2d r ], vertical coordinate [0,2d r] There is no 2D LiDAR data in the rectangular area enclosed by the vehicle, then the vehicle reaches the end of the line, d r Represents the row matrix of an orchard.

[0055] S2. Extraction of field navigation path: Extract the circular navigation path from the two-dimensional laser radar data; the circular navigation path takes the position C of the end of the turning side line as the center, and the fruit tree row spacing d r is the diameter. Since d r Measured by the inter-row navigation method, here we only need to determine the position of C:

[0056] When executing for the first time:

[0057] Filter out the 4D r The 2D LiDAR data in a square area with a side length of is clustered using the density-based spatial clustering of applications with noise (DBSCAN) method to obtain several tree trunk clusters; the DBSCAN algorithm parameters have a scanning radius of 0.1 m and a minimum number of neighborhood points;

[0058] Calculate the center of gravity of each trunk cluster. If it is a right turn, take the center of gravity of the trunk cluster with the largest sum of the horizontal coordinate and the vertical coordinate as the center of the circle C. If it is a left turn, take the center of gravity of the trunk cluster with the largest difference between the vertical coordinate and the horizontal coordinate as the center of the circle C.

[0059] The second execution starts:

[0060] According to the coordinates of the center C at the previous moment Estimate its position at the current time

[0061]

[0062] in: They represent the expected speed of the left and right wheels at the last moment obtained by S4, Δt represents the time interval between two adjacent navigations, D represents the wheelbase of the vehicle, d represents the distance between the center of mass of the vehicle and the geometric centers of the two driving wheels, Δθ represents the change in the heading angle of the vehicle within the Δt time interval,

[0063] Filter by Centered on 4D t The 2D laser radar data in the square area with a side length of , obtain the center of gravity of the data as the actual position of the center C at the current moment (x C ,y C ), d t It is the maximum breast diameter of fruit trees in the orchard.

[0064] S3, Headland navigation deviation calculation: Calculate the lateral deviation e between the current vehicle position and the arc navigation path l and heading deviation e h , specifically:

[0065] like Figure 3 As shown, let G be the intersection of the line connecting the radar center, i.e., the origin O, and the center C of the arc navigation path, and the arc navigation path. The lateral deviation is the distance between the origin O and G, with O being positive when it is to the right of G and negative when it is to the left. Draw an arc tangent through point G, and the heading deviation is the angle between the vehicle direction and the tangent, with the vehicle deviation to the right being positive and the vehicle deviation to the left being negative.

[0066] The lateral deviation e is calculated using the following formula: l and heading deviation e h :

[0067]

[0068]

[0069] S4, Headland navigation path tracking: According to the lateral deviation e l and heading deviation e h The desired speed of the left and right wheels is calculated by using the fuzzy adaptive pure tracking algorithm (Reference: Chen Jun, Jiang Haoran, Liu Pei, et al. Curved path navigation control of orchard mobile robot [J]. Transactions of the Chinese Society of Agricultural Machinery, 2012, 43(4): 179-182.) The vehicle is guided to travel along the circular arc navigation path. After S4 is executed and a time Δt has passed, S5 is executed.

[0070] S5, end-of-field navigation end detection: collect 1 frame of 2D LiDAR data to determine whether the vehicle has reached the end of the orchard row. If it has, end the end-of-field navigation, otherwise, execute S2; specifically:

[0071] S5-1. According to the lateral deviation e obtained in S4 l and heading deviation e h , perform coordinate correction on the two-dimensional laser radar data according to S1-2;

[0072] S5-2, line head detection: when turning right, if the horizontal coordinate [-2d r ,0], vertical coordinate [-2d t ,2d t ] contains two-dimensional laser radar data, indicating that the vehicle has reached the head of the line; when turning left, if the horizontal coordinate [0,2d r ], vertical coordinate [-2d t ,2d t ] There is two-dimensional lidar data in the rectangular area enclosed by , indicating that the vehicle has arrived at the head of the line.

[0073] When implementing:

[0074] MATLAB is used to carry out simulation experiments, and the simulation parameters are shown in Table 1.

[0075] Table 1 Simulation parameters

[0076]

[0077] In order to compare the proposed method with the open-loop control headland turning algorithm, taking right turn as an example, three parameters, namely the vehicle initial lateral deviation, initial heading deviation and road roughness, were selected as test factors, and single-factor experiments were conducted respectively. When conducting experiments on a certain test factor, the other test factors were set to 0. The specific values ​​of the parameters are shown in Table 2. In order to simulate rough ground, additive white noise is added to the coordinate value of the ideal vehicle position, and the white noise power spectrum density is used as the ground roughness parameter. Table 2 shows the single-factor experimental results. Figure 4 The vehicle trajectory is given. When the vehicle has a large lateral deviation / heading deviation at the end of the ground and the ground roughness is large, compared with the open-loop control method, the proposed method has a smaller lateral deviation and can more accurately guide the vehicle to travel along the circular navigation path.

[0078] Table 2 Single factor experimental results

[0079]

[0080] The embodiments of the present invention have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and changes will be apparent to those skilled in the art without departing from the scope and spirit of the described embodiments.

Claims

1. A method for orchard vehicle navigation based on two-dimensional laser radar, characterized in that: The method includes: S1, start detection of end-of-field navigation: obtain two-dimensional laser radar data to determine whether the vehicle has reached the end of the orchard row. If it has reached the end, execute S2 to start end-of-field navigation; S2, ground navigation path extraction: extracting arc navigation path from 2D lidar data; S3, Headland navigation deviation calculation: Calculate the lateral deviation e between the current vehicle position and the arc navigation path l and heading deviation e h ; S4, ground navigation path tracking: according to the lateral deviation e l and heading deviation e h , calculate the expected speed of the left and right wheels The vehicle is guided to travel along the circular navigation path, and after a time Δt, S5 is executed; S5, end-of-field navigation detection: obtain two-dimensional laser radar data to determine whether the vehicle has reached the end of the orchard row. If it has, end the end-of-field navigation, otherwise, execute S2; S1 is specifically: S1-1. Extract the navigation path based on the 2D laser radar data and calculate the vehicle lateral deviation e l and heading deviation e h ; S1-2, according to the vehicle lateral deviation e l and heading deviation e h , coordinate correction of two-dimensional lidar data; Among them, the origin of the coordinate system of the two-dimensional laser radar data is located at the center of the radar, the positive direction of the y-axis points to the forward direction of the vehicle, and the positive direction of the x-axis points to the right side of the vehicle; (x, y) and (x′, y′) are the data coordinates of the two-dimensional laser radar before and after correction respectively; S1-3, judge the data coordinates of the calibrated two-dimensional laser radar. If the horizontal coordinate [-2d r ,2d r ], vertical coordinate [0,2d r ] There is no 2D LiDAR data in the rectangular area enclosed by the vehicle, then the vehicle reaches the end of the line, d r It indicates the row square of orchard; In S2, the circular navigation path takes the position C of the last tree trunk at the turn as the center, and the row spacing of fruit trees is d. r is the diameter; the position of the center C is obtained by the following steps: When executing for the first time: Filter out the 4D r The two-dimensional laser radar data in the square area with a side length of is clustered to obtain several tree trunk clusters; the center of gravity of each tree trunk cluster is calculated. If it is a right turn, the center of gravity of the tree trunk cluster with the largest sum of the horizontal coordinate and the vertical coordinate is taken as the center of the circle C; if it is a left turn, the center of gravity of the tree trunk cluster with the largest difference between the vertical coordinate and the horizontal coordinate is taken as the center of the circle C; The second execution starts: According to the coordinates of the center C at the previous moment Estimate its position at the current time in: They represent the expected speed of the left and right wheels at the last moment obtained by S4, Δt represents the time interval between two adjacent navigations, D represents the wheelbase of the vehicle, d represents the distance between the center of mass of the vehicle and the geometric centers of the two driving wheels, Δθ represents the change in the heading angle of the vehicle within the Δt time interval, Filter by Centered on 4D t The 2D laser radar data in the square area with a side length of , obtain the center of gravity of the data as the actual position of the center C at the current moment (x C ,y C ), d t The maximum breast diameter of fruit trees in the orchard; S3 is specifically: Assume that the intersection of the line connecting the radar center, i.e. the origin O, and the center C of the arc navigation path and the arc navigation path is G. The lateral deviation is the distance between the origin O and G, with O being positive when it is to the right of G and negative when it is to the left. Draw an arc tangent through point G. The heading deviation is the angle between the vehicle direction and the tangent, with the vehicle deviation to the right being positive and the vehicle deviation to the left being negative. The lateral deviation e is calculated using the following formula: l and heading deviation e h :

2. The orchard vehicle headland navigation method based on two-dimensional laser radar according to claim 1 is characterized in that: The navigation path extraction method is an inter-line navigation path extraction algorithm.

3. The orchard vehicle headland navigation method based on two-dimensional laser radar according to claim 1 is characterized in that: The clustering adopts the density-based clustering method DBSCAN with noise, and the parameter scanning radius is 0.1m and the minimum number of neighborhood points is 1.

4. The orchard vehicle headland navigation method based on two-dimensional laser radar according to claim 1 is characterized in that: In S4, the fuzzy adaptive pure tracking algorithm is used to calculate the expected vehicle speed of the left and right wheels.

5. The orchard vehicle headland navigation method based on two-dimensional laser radar according to claim 1, characterized in that S5 include: S5-1. According to the lateral deviation e obtained in S4 l and heading deviation e h , perform coordinate correction on the two-dimensional laser radar data according to S1-2; S5-2, line head detection: when turning right, if the horizontal coordinate [-2d r ,0], vertical coordinate [-2d t ,2d t ] contains two-dimensional laser radar data, indicating that the vehicle has reached the head of the line; when turning left, if the horizontal coordinate [0,2d r ], vertical coordinate [-2d t ,2d t ] There is two-dimensional lidar data in the rectangular area enclosed by , indicating that the vehicle has arrived at the head of the line.

6. The orchard vehicle headland navigation method based on two-dimensional laser radar according to claim 1, characterized in that In S1 and S5, the acquired two-dimensional lidar data is collected one frame each time.

Images