Robot density self-adaptive inter-row navigation method

Abstract

The invention discloses a robot density self-adaptive inter-row navigation method, which belongs to the technical field of robot autonomous navigation, and comprises the following steps: when a driving distance is within a preset range, judging the environment density according to the maximum length of a three-dimensional cube and the radius of a point cloud; an improved random sampling consistency algorithm is adopted to fit distribution line straight lines of the objects, a navigation path is obtained according to a geometric structure formed by the objects for navigation, and the driving distance is recorded; if the environment density is high, a barrier-free object in front of the robot is determined based on an artificial potential field mechanism and two-dimensional point cloud data, a point on a geometric shape center line of the object serves as a virtual target point to provide gravitation, objects on the two sides of the virtual target point serve as repulsive obstacles to control autonomous navigation of the robot, and the driving distance is recorded; and when the driving distance is not within the preset range, resetting the driving distance and ending the iteration. The problem that the navigation effect of the robot in environments with different densities is poor is solved.

Description

Technical Field

[0001] The invention relates to a robot density adaptive inter-row navigation method, belonging to the technical field of robot autonomous navigation. Background Art

[0002] In recent years, with the rapid development of science and technology, robots have become an indispensable and important tool for achieving efficient operations, especially in complex and changing environments, where the requirements for their adaptability and intelligence are becoming increasingly stringent. Traditional navigation technologies, such as the Global Navigation Satellite System (GNSS) and Simultaneous Positioning and Mapping (SLAM) technology, play an important role in various application scenarios. GNSS technology relies on satellite signals to achieve precise positioning, but in certain specific environments, such as complex scenarios such as tree canopy occlusion, signal reflection, and multipath interference, GNSS signals may become unstable or even fail, resulting in a significant decrease in navigation accuracy. Although SLAM technology uses sensors to obtain environmental information in real time and build maps to achieve autonomous positioning, its positioning accuracy and real-time map construction will also be significantly affected in environments with dense obstacles and rapidly changing environments.

[0003] Although the existing navigation technology has achieved certain application results in many fields, there are still some defects that cannot be ignored. On the one hand, due to the particularity of complex environments, such as interference factors such as signal occlusion and reflection, the positioning accuracy and reliability of GNSS and SLAM technologies are seriously challenged. It is difficult for robots to maintain high accuracy and high reliability during automatic navigation, which in turn affects work efficiency and operation quality. On the other hand, the density of obstacles in different application scenarios varies significantly, which puts higher requirements on the navigation strategy of robots. In an environment with sparse obstacles, robots can adopt a simpler navigation strategy; but in areas with dense obstacles, robots need to plan their travel paths more carefully to avoid collisions. Therefore, the change in obstacle density makes the navigation process more complex and changeable, and robots need to have the ability to dynamically adjust navigation strategies according to the environment. However, the existing robot inter-row navigation methods often lack this dynamic adjustment mechanism, resulting in poor navigation effects of robots in environments with different densities, which is difficult to meet the needs of practical applications. Summary of the invention

[0004] The purpose of the present invention is to provide a robot density-adaptive inter-row navigation method, which enables the robot to adaptively select the optimal navigation strategy in environments with different density distributions, so that it can not only efficiently navigate in a straight line in a low-density environment, but also flexibly avoid obstacles in a high-density environment, so as to solve the problem that the existing robot inter-row navigation methods often lack such a dynamic adjustment mechanism, resulting in poor navigation effect of the robot in environments with different densities, thereby significantly improving the navigation accuracy and efficiency of the robot.

[0005] In order to solve the above technical problems, the present invention is implemented by adopting the following technical solutions.

[0006] The present invention provides a robot density adaptive inter-row navigation method, comprising:

[0007] Obtain 3D point cloud data of an environment with uneven density distribution, use radius filtering to reduce the point cloud density and limit the radius range of the point cloud;

[0008] Repeat the following steps until the navigation end condition is reached:

[0009] Performing ground point cloud segmentation on the three-dimensional point cloud data to remove the ground point cloud and then performing point cloud clustering to obtain a three-dimensional cube wrapped by the object point cloud cluster;

[0010] Record the robot's current travel distance;

[0011] When the robot's current driving distance is within the preset range, if the maximum length of the 3D cube is greater than 2 / 3 of the point cloud radius, the environment density is low, otherwise the environment density is high:

[0012] If the environment density is low, an improved random sampling consistency algorithm is used to fit the distribution straight line of the objects in the environment, and a navigation path is obtained according to the geometric structure of the objects to control the robot to navigate autonomously;

[0013] If the environment density is high, compress the three-dimensional point cloud data into two-dimensional point cloud data, determine that there are no obstacles in front of the robot based on the two-dimensional point cloud data, and use the points on the center line of the geometric shape of the object as virtual target points to provide gravity based on the artificial potential field mechanism, and use the objects on both sides of the virtual target point as repulsive obstacles to control the robot's autonomous navigation;

[0014] When the current driving distance of the robot is not within the preset range, the current driving distance of the robot is reset to 0, and this iteration ends.

[0015] Furthermore, the method further includes performing point cloud preprocessing after acquiring the three-dimensional point cloud data of the environment with uneven density distribution to obtain a preprocessed three-dimensional point cloud:

[0016] Using radius filtering to filter the point clouds of the objects on the left and right sides of the current frame of the three-dimensional point cloud data, set a circular area centered on the sensor, remove all points outside the circular area and eliminate the 180° point cloud behind the robot to obtain a focused point cloud;

[0017] Using voxel downsampling technology, the point cloud space where the focused point cloud is located is divided into three-dimensional voxel grids, and a representative point cloud is selected in each voxel grid to obtain a sparse point cloud;

[0018] Remove the noise in the thinned point cloud by using outlier filtering to obtain the preprocessed three-dimensional point cloud.

[0019] Furthermore, the improved random sample consensus algorithm is used to fit the distribution line of the objects in the environment, and the navigation path is obtained according to the geometric structure formed by the objects to control the autonomous navigation of the robot, including:

[0020] A. Preset the left object row fitting line and the right object row fitting line to represent the main distribution trends of the objects in the environment;

[0021] B. According to the left object row fitting line and the right object row fitting line, obtain the point cloud clusters representing the left and right object rows from the preprocessed three-dimensional point cloud, and randomly select a point from each of the point cloud clusters representing the left and right object rows as candidate points, and obtain a candidate line according to the two candidate points;

[0022] C. Preset a tolerance threshold, compare the distance d from the remaining points in the point cloud clusters representing the left and right object rows to the candidate line with the tolerance threshold R, and perform inlier and outlier segmentation; fit the obtained inliers to obtain the distribution line of the objects; where, if d < R, it is segmented as an inlier, otherwise it is segmented as an outlier;

[0023] D. Repeat steps B - C for multiple fittings, and take the candidate line with the largest number of inliers as the optimal distribution line of the objects;

[0024] E. Preset that the optimal distribution line of the objects belongs to the point cloud cluster of the left object row, obtain the best slope and the best intercept of the left object row fitting line to obtain the best left object row fitting line, and obtain the best right object row fitting line according to the fact that the left and right object rows are parallel and the row spacing is a fixed value;

[0025] F. Based on the best left object row fitting line and the best right object row fitting line, obtain the navigation path based on the parallel line relationship to control the autonomous navigation of the robot.

[0026] Furthermore, the navigation path is expressed as:

[0027] ;

[0028] In the formula, represents the navigation path in the case of low environmental density, represents the best slope of the left object row fitting line, represents the inliers of the best right object row fitting line, represents the best intercept of the left object row fitting line, represents the row spacing when the left object and the right object are parallel.

[0029] Furthermore, it also includes tracking the navigation path in a low-density environment using an LQR control algorithm.

[0030] Further, the three-dimensional point cloud data is compressed into two-dimensional point cloud data, and it is determined that there are no obstacle objects in front of the robot according to the two-dimensional point cloud data, and based on the artificial potential field mechanism, the point on the center line of the geometric shape of the object is used as a virtual target point to provide gravity, and the objects on both sides of the virtual target point are used as repulsive obstacles to control the robot's autonomous navigation, including:

[0031] A. compressing the three-dimensional point cloud data into two-dimensional point cloud data;

[0032] B. Taking the center position of the two-dimensional point cloud data as the base point, an initial geometric body is established in front of the robot according to the shape of the objects in the environment and the volume of the initial geometric body is gradually expanded until a certain number of two-dimensional point clouds fall into the expanded geometric body. The objects on the left and right sides of the robot can move freely.

[0033] C. Based on the artificial potential field mechanism, a repulsive potential field and a gravitational potential field are established. The objects on the left and right sides of the robot are used as obstacles to exert repulsive force on the robot. A virtual target point is set at a linear distance of 1m from the robot on the center line of the geometric body in front of the robot to exert gravitational force on the robot.

[0034] D. Perform vector synthesis of the repulsive force and the attractive force to obtain the resultant force acting on the robot, and update the linear velocity and angular velocity of the robot according to the resultant force acting on the robot to control the robot's autonomous navigation.

[0035] Furthermore, the repulsive potential field and the attractive potential field are established based on the artificial potential field mechanism;

[0036] Wherein, the repulsive potential field is expressed as:

[0037] ;

[0038] In the formula, Indicates the current position of the robot The repulsive potential field, represents the repulsive potential field coefficient, Indicates the current position of the robot To the current obstacle position The closest distance; The threshold distance for the repulsive force is the center distance of the width of the object rows on the left and right sides;

[0039] Wherein, the gravitational potential field is expressed as:

[0040] ;

[0041] In the formula, Indicates the current position of the robot The gravitational potential field, represents the gravitational potential field coefficient; Indicates the position of the virtual target point set at a point 1m away from the robot on the center line of the geometric body in front of the robot. The square of the Euclidean distance of the gravitational vector.

[0042] Furthermore, the objects on the left and right sides of the robot when it is moving are used as obstacles to exert repulsive force on the robot, and a virtual target point is set at a linear distance of 1m from the robot on the center line of the geometric body in front of the robot to exert gravitational force on the robot;

[0043] Wherein, the repulsive force is expressed as:

[0044] ;

[0045] In the formula, Indicates the current position of the robot The repulsive force, Indicates the current position of the robot To the current Obstacle locations The closest distance, represents the Euclidean distance of the repulsive force vector;

[0046] Wherein, the gravitational force is expressed as:

[0047] ;

[0048] In the formula, Indicates the current position of the robot The gravitational force exerted.

[0049] Further, the resultant force is expressed as:

[0050] ;

[0051] In the formula, Indicates the current position of the robot The combined force, Represents the gradient of the attractive potential field and the repulsive potential field at the current position q.

[0052] Furthermore, according to the resultant force applied to the robot, the linear velocity and angular velocity of the robot are updated and the robot is controlled to navigate autonomously;

[0053] Among them, the linear velocity of the robot is expressed as:

[0054] ;

[0055] In the formula, represents the linear velocity of the robot, Represents the robot's linear speed adjustment parameter.

[0056] The direction of the resultant force is taken as the desired driving direction of the robot, the difference between the current driving direction of the robot and the desired driving direction is calculated, and the angular velocity of the robot is adjusted using the proportional control method. The angular velocity of the robot is expressed as:

[0057] ;

[0058] in, represents the angular velocity of the robot, Represents the robot's angular velocity adjustment parameter, Indicates the desired direction of travel. Indicates the current driving direction of the robot.

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

[0060] 1. During the navigation process, the present invention can dynamically judge the environmental density according to the current driving distance of the robot, and flexibly select the improved random sampling consistency algorithm or artificial potential field mechanism for path planning, ensuring that the robot can quickly fit the object distribution straight line in a low-density environment and achieve efficient navigation. In a high-density environment, it can accurately identify obstacles and flexibly plan obstacle avoidance paths. The present invention significantly improves the navigation efficiency and accuracy of the robot in different density environments.

[0061] 2. In a low-density environment, the present invention presets the fitting straight lines of the object rows on the left and right sides, randomly selects candidate points from the preprocessed three-dimensional point cloud to generate candidate straight lines, and then performs internal and external point segmentation. After multiple fittings, the optimal object distribution straight line is determined, effectively filtering out noise points and ensuring the accuracy of the fitting straight line. Furthermore, by utilizing the characteristics that the object rows on the left and right sides are parallel and have a fixed row spacing, the fitting straight line of the best left object row and the fitting straight line of the best right object row are automatically derived, thereby quickly generating a navigation path, which not only simplifies the planning process of the navigation path, but also improves the reliability and practicality of the path. It is particularly suitable for complex and changeable environments, and provides more efficient and intelligent solutions for the fields of autonomous driving, robot navigation, etc.

[0062] 3. In high-density environments, the present invention realizes accurate identification of unobstructed objects in front of the robot and intelligent setting of virtual target points through the artificial potential field mechanism. At the same time, the present invention can dynamically sense and respond to obstacles that move freely on the left and right sides, and adjust the current driving direction and speed of the robot in real time by calculating the combined force of repulsion and gravity, thereby realizing the robot's autonomous navigation and flexible obstacle avoidance in complex environments, significantly improving the accuracy and safety of navigation.

[0063] 4. The present invention also combines the artificial potential field mechanism and the LQR control algorithm to further enhance the stability and response speed of the robot, enabling the robot to achieve more stable and reliable autonomous navigation in complex environments, which not only significantly improves the robot's work efficiency, but also greatly reduces its dependence on external monitoring and intervention. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 Shown is a schematic flow chart of a robot density adaptive inter-row navigation method provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0065] The technical solution of the present invention is described in detail below through the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations on the technical solution of the present invention. The embodiments of the present invention and the technical features in the embodiments may be combined with each other unless there is a conflict.

[0066] The term "and / or" is only a description of the association relationship between related objects, indicating that there can be three relationships. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " generally indicates that the related objects are in an "or" relationship.

[0067] Example 1

[0068] like Figure 1 As shown, this embodiment introduces a robot density adaptive inter-row navigation method, including:

[0069] Step 1: Obtain 3D point cloud data of an environment with uneven density distribution.

[0070] After acquiring the three-dimensional point cloud data of the environment with uneven density distribution, this embodiment also performs point cloud preprocessing, including:

[0071] Using radius filtering to filter the point clouds of the objects on the left and right sides of the current frame of the three-dimensional point cloud data, set a circular area centered on the sensor, remove all points outside the circular area and eliminate the 180° point cloud behind the robot to obtain a focused point cloud;

[0072] Using voxel downsampling technology, the point cloud space where the focused point cloud is located is divided into three-dimensional voxel grids, and a representative point cloud is selected in each voxel grid to obtain a sparsified point cloud;

[0073] Noise in the sparsified point cloud is eliminated using outlier filtering to obtain a preprocessed three-dimensional point cloud.

[0074] Step 2: Use radius filtering to reduce the point cloud density and limit the radius range of the point cloud.

[0075] Repeat the following steps until the navigation end condition is reached:

[0076] Step 2.1: After removing the ground point cloud by segmenting the ground point cloud from the three-dimensional point cloud data, perform point cloud clustering to obtain a three-dimensional cube wrapped by an object point cloud cluster.

[0077] Step 2.2: Record the current driving distance of the robot.

[0078] Step 2.3: When the current driving distance of the robot is within a preset range, if the maximum length of the three-dimensional cube is greater than 2 / 3 of the point cloud radius length, the environmental density is low, otherwise the environmental density is high:

[0079] If the environmental density is low, use an improved random sample consensus algorithm to fit the distribution of objects in the environment into a straight line, and obtain a navigation path based on the geometric structure formed by the objects, and control the robot for autonomous navigation, including:

[0080] A. Preset a left object row fitting straight line and a right object row fitting straight line to represent the main distribution trend of objects in the environment;

[0081] B. According to the left object row fitting straight line and the right object row fitting straight line, obtain point cloud clusters representing the left and right object rows from the preprocessed point cloud, and randomly select a point from each of the point cloud clusters representing the left and right object rows as candidate points, and obtain a candidate straight line based on the two candidate points;

[0082] C. Preset a tolerance threshold, compare the distance d between the remaining points in the point cloud clusters representing the left and right object rows and the candidate straight line with the tolerance threshold R, and perform inlier and outlier segmentation; Fit the obtained inliers to obtain the distribution straight line of the object; where, if d < R, it is segmented as an inlier, otherwise it is segmented as an outlier;

[0083] D. Repeat steps B - C for multiple fittings, and use the candidate straight line with the largest number of inliers obtained as the optimal distribution straight line of the object;

[0084] E. The optimal object distribution row straight line is preset to belong to the point cloud cluster of the left object row, and the optimal slope and the optimal intercept of the left object row fitting straight line are obtained, and the optimal left object row fitting straight line is obtained. According to the parallelism of the left and right object rows and the fixed row spacing, the optimal right object row fitting straight line is obtained;

[0085] F. According to the best left object row fitting line and the best right object row fitting line, the navigation path is obtained based on the parallel straight line relationship to control the robot's autonomous navigation.

[0086] Wherein, the navigation path is expressed as:

[0087] ;

[0088] In the formula, Indicates the navigation path in low environment density. It represents the optimal slope of the fitting line of the left object row. represents the inner point of the best right object row fitting line, It represents the best intercept of the fitting line of the object on the left. Indicates the line spacing when the left object is parallel to the right object.

[0089] If the environment density is high, compress the three-dimensional point cloud data into two-dimensional point cloud data, determine that there are no obstacles in front of the robot according to the two-dimensional point cloud data, and use the points on the center line of the geometric shape of the object as virtual target points to provide gravity based on the artificial potential field mechanism, and use the objects on both sides of the virtual target points as repulsive obstacles to control the robot's autonomous navigation, including:

[0090] A. compressing the three-dimensional point cloud data into two-dimensional point cloud data;

[0091] B. Taking the center position of the two-dimensional point cloud data as the base point, an initial geometric body is established in front of the robot according to the shape of the objects in the environment and the volume of the initial geometric body is gradually expanded until a certain number of two-dimensional point clouds fall into the expanded geometric body. The objects on the left and right sides of the robot can move freely.

[0092] C. Based on the artificial potential field mechanism, a repulsive potential field and a gravitational potential field are established. The objects on the left and right sides of the robot are used as obstacles to exert repulsive force on the robot. A virtual target point is set at a linear distance of 1m from the robot on the center line of the geometric body in front of the robot to exert gravitational force on the robot.

[0093] In this embodiment, the repulsive potential field is expressed as:

[0094] ;

[0095] In the formula, Indicates the current position of the robot The repulsive potential field, represents the repulsive potential field coefficient, Indicates the current position of the robot To the current obstacle position The closest distance; Indicates the threshold distance for the repulsive force, that is, the center distance of the object row width on the left and right sides.

[0096] In this embodiment, the gravitational potential field is expressed as:

[0097] ;

[0098] In the formula, Indicates the current position of the robot The gravitational potential field, represents the gravitational potential field coefficient; Indicates the position of the virtual target point set at a point 1m away from the robot on the center line of the geometric body in front of the robot. Represents the square of the Euclidean distance of the gravity vector.

[0099] In this embodiment, the repulsive force is expressed as:

[0100] ;

[0101] In the formula, Indicates the current position of the robot The repulsive force, Indicates the current position of the robot To the current Obstacle locations The closest distance, represents the square of the Euclidean distance of the repulsive vector;

[0102] In this embodiment, the gravitational force is expressed as:

[0103] ;

[0104] In the formula, Indicates the current position of the robot The gravitational force exerted.

[0105] D. Perform vector synthesis of the repulsive force and the attractive force to obtain the resultant force acting on the robot, and update the linear velocity and angular velocity of the robot according to the resultant force acting on the robot, and control the robot to navigate autonomously.

[0106] In this embodiment, the resultant force is expressed as:

[0107] ;

[0108] In the formula, Indicates the current position of the robot The combined force, Represents the gradient of the attractive potential field and the repulsive potential field at the current position q.

[0109] In this embodiment, the linear velocity of the robot is expressed as:

[0110] ;

[0111] In the formula, represents the linear velocity of the robot, Represents the robot's linear speed adjustment parameter.

[0112] In this embodiment, the direction of the resultant force is taken as the desired driving direction of the robot, the difference between the current driving direction of the robot and the desired driving direction is calculated, and the angular velocity of the robot is adjusted using a proportional control method. The angular velocity of the robot is expressed as:

[0113] ;

[0114] in, represents the angular velocity of the robot, Represents the robot's angular velocity adjustment parameter, Indicates the desired direction of travel. Indicates the current driving direction of the robot.

[0115] Step 2.4: When the current driving distance of the robot is not within the preset range, the current driving distance of the robot is reset to 0, and this iteration ends.

[0116] Example 2

[0117] Based on the same inventive concept as Example 1, this example introduces an example of applying a robot density adaptive row navigation method to an orchard environment for robot autonomous navigation. This example targets two different environmental characteristics of low density and high density in an orchard, and uses a simple geometric structure of parallel planting in an orchard to perform navigation without pre-building a map, including the following steps:

[0118] Step 1: Obtain the 3D point cloud data of the orchard environment, use radius filtering to reduce the point cloud density and limit the radius range of the point cloud.

[0119] In this embodiment, after collecting three-dimensional point cloud data of the orchard environment using a laser radar, the three-dimensional point cloud data is preprocessed to obtain the preprocessed three-dimensional point cloud, and the steps include:

[0120] Using radius filtering, a circular area centered on the sensor is set for the point clouds of the fruit trees on the left and right sides of the current frame of the three-dimensional point cloud data, all points outside the circular area are removed, and the 180° point cloud behind the robot is eliminated to obtain a focused point cloud;

[0121] Using voxel downsampling technology, the point cloud space where the focused point cloud is located is divided into three-dimensional voxel grids, and a representative point cloud is selected in each voxel grid to obtain a sparse point cloud;

[0122] Outlier filtering is used to eliminate noise in the sparse point cloud to obtain the preprocessed three-dimensional point cloud.

[0123] Step 2: Repeat the following steps until the navigation end condition is reached:

[0124] Step 2.1: Perform ground point cloud segmentation on the three-dimensional point cloud data to remove the ground point cloud and then perform point cloud clustering to obtain a three-dimensional cube wrapped by the fruit tree point cloud cluster.

[0125] Step 2.2: Record the current travel distance of the robot;

[0126] In this embodiment, the onboard odometer is monitored while the robot is navigating to record the current distance traveled by the robot.

[0127] Step 2.3: When the robot's current driving distance is within the preset range, if the maximum length of the 3D cube is greater than 2 / 3 of the point cloud radius, the environment density is low, otherwise the environment density is high:

[0128] In this embodiment, since high-density fruit trees have protruding crowns, the maximum length of the three-dimensional cube wrapped by the fruit tree point cloud cluster is compared with 2 / 3 of the point cloud radius length defined by the radius filter to determine the density environment.

[0129] If the environment density is low, an improved random sampling consistency algorithm is used to fit the distribution straight line of objects in the environment, and a navigation path is obtained according to the geometric structure formed by the objects to control the robot's autonomous navigation.

[0130] In a low-density orchard environment, due to the low density of fruit trees, there are missing point clouds, and the point cloud clustering of the fruit tree rows on the left and right sides is not continuous. The traditional RANSAC method will fit the straight line of the fruit tree ridge line, which will have a large error. Therefore, this embodiment uses the simple geometric structure of fruit trees in the orchard that are generally planted in parallel, and improves the RANSAC method to fit the fruit tree ridge line in a low-density environment and extract the navigation path.

[0131] An improved random sampling consensus algorithm is used to fit the distribution line of the fruit trees, and the navigation path is obtained according to the geometric structure of the fruit trees for navigation, including:

[0132] A. Preset a left fruit tree row fitting straight line and a right fruit tree row fitting straight line to represent the main distribution trend of fruit trees in the environment.

[0133] In this embodiment, the left fruit tree row fitting straight line is preset as:

[0134] ;

[0135] In the formula, represents the left fruit tree row fitting straight line, represents the slope of the left fruit tree row fitting straight line, represents the inlier of the left fruit tree row fitting straight line, represents the intercept of the left fruit tree row fitting straight line.

[0136] In this embodiment, the right fruit tree row fitting straight line is preset as:

[0137] ;

[0138] In the formula represents the right fruit tree row fitting straight line, represents the slope of the right fruit tree row fitting straight line, represents the inlier of the right fruit tree row fitting straight line, represents the intercept of the right fruit tree row fitting straight line.

[0139] B. According to the left fruit tree row fitting straight line and the right fruit tree row fitting straight line, obtain point cloud clusters representing the left and right fruit tree rows from the preprocessed three-dimensional point cloud, randomly select a point from each of the point cloud clusters representing the left and right fruit tree rows as candidate points, and obtain a candidate straight line according to the two candidate points.

[0140] C. Preset a tolerance threshold, compare the distance d from the remaining points in the point cloud clusters representing the left and right fruit tree rows to the candidate straight line with the tolerance threshold R, and perform inlier and outlier segmentation; fit the obtained inliers to obtain the distribution straight line of the object; where, if d < R, it is segmented as an inlier, otherwise it is segmented as an outlier.

[0141] D. Repeat steps (2) and (3) for multiple fittings, and use the candidate straight line with the largest number of inliers obtained as the distribution straight line of the optimal fruit tree.

[0142] E. Preset that the distribution straight line of the optimal fruit tree belongs to the point cloud cluster of the left fruit tree row, obtain the best slope and the best intercept of the left fruit tree row fitting straight line, obtain the best left fruit tree row fitting straight line, and obtain the best right fruit tree row fitting straight line according to the parallelism of the left and right fruit tree rows and the fixed row spacing;

[0143] ;

[0144] In the formula, represents the best right side fruit tree row fitting straight line, It represents the best slope of the best left fruit tree row fitting line. represents the inner point of the best right fruit tree row fitting line, It represents the best intercept of the best left fruit tree row fitting line. Indicates the row spacing when the fruit trees on the left and right are parallel.

[0145] F. According to the best left fruit tree row fitting straight line and the best right fruit tree row fitting straight line, a navigation path is obtained based on the parallel straight line relationship, where the navigation path is expressed as:

[0146] ;

[0147] In the formula, Indicates the navigation path in low environment density. It represents the optimal slope of the fitting line of the left fruit tree row. represents the inner point of the best right fruit tree row fitting line, It represents the best intercept of the fitting line of the left fruit tree row. Indicates the row spacing when the fruit trees on the left and right are parallel.

[0148] If the environment density is high, the three-dimensional point cloud data is compressed into two-dimensional point cloud data, and it is determined that there are no obstacles in front of the robot according to the two-dimensional point cloud data, and the point on the center line of the cone is used as a virtual target point to provide gravity, and the fruit trees on both sides of the virtual target point are used as repulsive obstacles to control the robot to navigate autonomously, including:

[0149] A. compressing the three-dimensional point cloud data into two-dimensional point cloud data;

[0150] B. With the center of the 2D point cloud data as the base point, an initial cone is established in front of the robot and the volume of the cone is gradually expanded until a certain number of 2D point clouds fall into the expanded cone;

[0151] C. Based on the artificial potential field mechanism, a repulsive potential field and a gravitational potential field are established. The fruit tree rows on the left and right sides of the robot are used as obstacles to exert repulsive force on the robot. A virtual target point is set at a linear distance of 1m from the robot on the center line of the cone in front of the robot to exert gravitational force on the robot.

[0152] D. Perform vector synthesis of the repulsive force and the attractive force to obtain the resultant force acting on the robot, and update the linear velocity and angular velocity of the robot according to the resultant force acting on the robot, and control the robot to navigate autonomously.

[0153] In this embodiment, it also includes using the LQR control algorithm to design a controller by minimizing a quadratic cost function including state deviation and control input, and using LQR to track the navigation path in a low-density environment so that the robot maintains stability and good performance.

[0154] Step 2.4: When the current driving distance of the robot is not within the preset range, the current driving distance of the robot is reset to 0, and this iteration ends.

[0155] To summarize the above embodiments, during the navigation process, the present invention can dynamically judge the environmental density according to the current driving distance of the robot, and flexibly select the improved random sampling consistency algorithm or artificial potential field mechanism for path planning, thereby ensuring that the robot can quickly fit the object distribution straight line in a low-density environment and achieve efficient navigation, and can accurately identify obstacles in a high-density environment and flexibly plan obstacle avoidance paths. The present invention significantly improves the navigation efficiency and accuracy of the robot in environments of different densities.

[0156] In a low-density environment, the present invention presets the fitting straight lines of the object rows on the left and right sides, randomly selects candidate points from the preprocessed three-dimensional point cloud to generate candidate straight lines, and then performs internal and external point segmentation. After multiple fittings, the optimal object distribution straight line is determined, effectively filtering out noise points and ensuring the accuracy of the fitting straight line. Furthermore, by utilizing the characteristics that the object rows on the left and right sides are parallel and have a fixed row spacing, the fitting straight line of the best left object row and the fitting straight line of the best right object row are automatically derived, thereby quickly generating a navigation path, which not only simplifies the planning process of the navigation path, but also improves the reliability and practicality of the path. It is particularly suitable for complex and changeable environments, and provides more efficient and intelligent solutions for the fields of autonomous driving, robot navigation, etc.

[0157] In high-density environments, the present invention uses an artificial potential field mechanism to achieve accurate identification of unobstructed objects in front of the robot and intelligent setting of virtual target points. At the same time, the present invention can dynamically sense and respond to obstacles that move freely on the left and right sides, and adjust the robot's current driving direction and speed in real time by calculating the combined force of repulsion and gravity, thereby achieving autonomous navigation and flexible obstacle avoidance of the robot in complex environments, significantly improving the accuracy and safety of navigation.

[0158] The present invention also combines the artificial potential field mechanism and the LQR control algorithm to further enhance the stability and response speed of the robot, enabling the robot to achieve more stable and reliable autonomous navigation in complex environments, which not only significantly improves the robot's work efficiency, but also greatly reduces its dependence on external monitoring and intervention.

[0159] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0160] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0161] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0162] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0163] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the purpose of the present invention and the claims, which all fall within the protection of the present invention.

Claims

1. A robot density adaptive inter-row navigation method, characterized in that: Including: Obtain the three-dimensional point cloud data of an environment with uneven density distribution, and use radius filtering to reduce the point cloud density and limit the radius range of the point cloud; Repeat the following steps until the navigation end condition is reached: Perform ground point cloud segmentation on the three-dimensional point cloud data to remove the ground point cloud, and then perform point cloud clustering to obtain a three-dimensional cube wrapped by the object point cloud clusters; Record the current driving distance of the robot; When the current driving distance of the robot is within the preset range, if the maximum length of the three-dimensional cube is greater than 2 / 3 of the point cloud radius length, the environment density is low, otherwise the environment density is high: If the environment density is low, use an improved random sample consensus algorithm to fit the distribution line of the objects in the environment, obtain the navigation path according to the geometric structure formed by the objects, and control the robot for autonomous navigation; If the environment density is high, compress the three-dimensional point cloud data into two-dimensional point cloud data, determine the obstacle-free objects in front of the robot according to the two-dimensional point cloud data, and provide gravity with the points on the center line of the object geometry as virtual target points based on the artificial potential field mechanism, and use the objects on both sides of the virtual target point as repulsive obstacles to control the robot for autonomous navigation; Among them, when the current driving distance of the robot is not within the preset range, reset the current driving distance of the robot to 0 and end this iteration.

2. The robot density adaptive inter-row navigation method according to claim 1, characterized in that: It also includes performing point cloud preprocessing after obtaining the three-dimensional point cloud data of an environment with uneven density distribution to obtain the preprocessed three-dimensional point cloud: Use radius filtering on the point clouds of the objects on the left and right sides of the current frame of the three-dimensional point cloud data, set a circular area centered on the sensor, remove all points outside the circular area and剔除 the point cloud at 180° behind the robot to obtain the focused point cloud; Use voxel downsampling technology to divide the point cloud space where the focused point cloud is located into three-dimensional voxel grids, and select a representative point cloud in each voxel grid to obtain the sparsified point cloud; Use outlier filtering to eliminate the noise in the sparsified point cloud to obtain the preprocessed three-dimensional point cloud.

3. The robot density adaptive inter-row navigation method according to claim 2, characterized in that: The step of using an improved random sample consensus algorithm to fit the distribution line of the objects in the environment, obtain the navigation path according to the geometric structure formed by the objects, and control the robot for autonomous navigation includes: A. Preset the left object row fitting line and the right object row fitting line to represent the main distribution trends of the objects in the environment; B. According to the left object row fitting line and the right object row fitting line, obtain the point cloud clusters representing the left and right object rows from the preprocessed three-dimensional point cloud, and randomly select a point from each of the point cloud clusters representing the left and right object rows as candidate points, and obtain a candidate line according to the two candidate points; C. Preset a tolerance threshold, compare the distance d between the remaining points in the point cloud clusters representing the left and right object rows and the candidate line with the tolerance threshold R, and perform inlier and outlier segmentation; fit the obtained inliers to obtain the distribution line of the objects; where, if d < R, it is segmented as an inlier, otherwise it is segmented as an outlier; D. Repeat steps B - C for multiple fittings, and use the candidate line with the largest number of inliers obtained as the optimal distribution line of the objects; E. The optimal object distribution row straight line is preset to belong to the point cloud cluster of the left object row, and the optimal slope and the optimal intercept of the left object row fitting straight line are obtained, and the optimal left object row fitting straight line is obtained; according to the fact that the left and right object rows are parallel and the row spacing is a fixed value, the optimal right object row fitting straight line is obtained; F. According to the best left object row fitting line and the best right object row fitting line, the navigation path is obtained based on the relationship of parallel lines, and the robot is controlled to navigate autonomously.

4. The robot density adaptive inter-row navigation method according to claim 3, characterized in that: The navigation path is represented as: ； In the formula, Indicates the navigation path in low environment density. It represents the optimal slope of the fitting line of the left object row. represents the inner point of the best right object row fitting line, It represents the best intercept of the fitting line of the object on the left. Indicates the line spacing when the left object is parallel to the right object.

5. The robot density adaptive inter-row navigation method according to claim 3, characterized in that: Also included is tracking the navigation path in a low-density environment using an LQR control algorithm.

6. The robot density adaptive inter-row navigation method according to claim 1, characterized in that: The three-dimensional point cloud data is compressed into two-dimensional point cloud data, and according to the two-dimensional point cloud data, it is determined that there is no obstacle in front of the robot, and based on the artificial potential field mechanism, the point on the center line of the geometric shape of the object is used as a virtual target point to provide gravity, and the objects on both sides of the virtual target point are used as repulsive obstacles to control the robot's autonomous navigation, including: A. compressing the three-dimensional point cloud data into two-dimensional point cloud data; B. Taking the center position of the two-dimensional point cloud data as the base point, an initial geometric body is established in front of the robot according to the shape of the objects in the environment and the volume of the initial geometric body is gradually expanded until a certain number of two-dimensional point clouds fall into the expanded geometric body. The objects on the left and right sides of the robot can move freely. C. Based on the artificial potential field mechanism, a repulsive potential field and a gravitational potential field are established. The objects on the left and right sides of the robot are used as obstacles to exert repulsive force on the robot. A virtual target point is set at a linear distance of 1m from the robot on the center line of the geometric body in front of the robot to exert gravitational force on the robot. D. Perform vector synthesis of the repulsive force and the attractive force to obtain the resultant force acting on the robot, and update the linear velocity and angular velocity of the robot according to the resultant force acting on the robot, and control the robot to navigate autonomously.

7. The robot density adaptive inter-row navigation method according to claim 6, characterized in that: The repulsive potential field and the gravitational potential field are established based on the artificial potential field mechanism; Wherein, the repulsive potential field is expressed as: ； In the formula, Indicates the current position of the robot The repulsive potential field, represents the repulsive potential field coefficient, Indicates the current position of the robot To the current obstacle position The closest distance; The threshold distance for the repulsive force is the center distance of the object row width on the left and right sides; Wherein, the gravitational potential field is expressed as: ； In the formula, Indicates the current position of the robot The gravitational potential field, represents the gravitational potential field coefficient; Indicates the position of the virtual target point set at a point 1m away from the robot on the center line of the geometric body in front of the robot. Represents the square of the Euclidean distance of the gravity vector.

8. The robot density adaptive inter-row navigation method according to claim 7, characterized in that: The objects on the left and right sides of the robot when it is moving are used as obstacles to exert repulsive force on the robot, and a virtual target point is set at a linear distance of 1m from the robot on the center line of the geometric body in front of the robot to exert gravitational force on the robot; Wherein, the repulsive force is expressed as: ； In the formula, Indicates the current position of the robot The repulsive force, Indicates the current position of the robot To the current Obstacle locations The closest distance, represents the Euclidean distance of the repulsive force vector; Wherein, the gravitational force is expressed as: ； In the formula, Indicates the current position of the robot The gravitational force exerted.

9. The robot density adaptive inter-row navigation method according to claim 8, characterized in that: The resultant force is expressed as: ； In the formula, Indicates the current position of the robot The combined force, Represents the gradient of the attractive potential field and the repulsive potential field at the current position q.

10. The robot density adaptive inter-row navigation method according to claim 9, characterized in that: According to the resultant force applied to the robot, the linear velocity and angular velocity of the robot are updated and the robot is controlled to navigate autonomously; Among them, the linear velocity of the robot is expressed as: ； In the formula, represents the linear velocity of the robot, Indicates the linear speed adjustment parameter of the robot; The direction of the resultant force is taken as the desired driving direction of the robot, the difference between the current driving direction of the robot and the desired driving direction is calculated, and the angular velocity of the robot is adjusted using the proportional control method. The angular velocity of the robot is expressed as: ； in, represents the angular velocity of the robot, represents the robot's angular velocity adjustment parameter, Indicates the desired direction of travel. Indicates the current driving direction of the robot.

Images