A visual servo motion planning method for a forest maintenance robot spraying device

Abstract

The application discloses a kind of forest maintenance robot spraying device visual servo motion planning method, comprising: multi-line laser radar is combined with height threshold filtering and space clustering algorithm obtains to be brushed trunk point cloud;To be brushed trunk point cloud is vertically divided to obtain point cloud block, and the principal component of trunk overall point cloud and each point cloud block is obtained based on principal component analysis method;Using weighted center algorithm obtains the root node of the point cloud block corresponding to the lowest point;Using the principal component of point cloud block principal component is iteratively corrected to the principal component of overall point cloud, obtains the center point of each point cloud block, and sequentially connects to obtain trunk skeleton;Trunk skeleton is carried out space Bezier curve smoothing processing, and the motion trajectory of spraying device is obtained.The application uses the above-mentioned forest maintenance robot spraying device visual servo motion planning method, effectively solves the challenge of data sparsity, noise interference and the like, realizes the accurate extraction of tree skeleton, and provides accurate and reliable structural information for robot tree painting.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a visual servo motion planning method for a forest maintenance robot spraying device. Background Technology

[0002] In recent years, with the continuous improvement of robotics technology and automation requirements, more and more outdoor robots are working in unstructured outdoor environments, making the refined perception and modeling of outdoor scenes a research hotspot. Among these, the accurate acquisition of structural information about trees, as common natural objects, is crucial for applications such as robotic tree painting, fruit harvesting, and tree health monitoring. LiDAR, with its high-precision and high-efficiency 3D information acquisition capabilities, is widely used in the digital modeling of outdoor scenes.

[0003] In the process of using LiDAR to scan outdoor scenes and construct overall maps, limitations in time, data storage, and application objectives often prevent multiple scans of each object, especially for complex objects like trees with unevenly distributed branches. In most cases, unless specifically targeted at a particular tree, each tree typically appears only in a single scan, resulting in sparse point cloud data. This makes it difficult for traditional mesh construction methods based on dense point clouds to effectively reconstruct tree structural information. Furthermore, factors such as foliage occlusion and lighting variations often introduce significant noise and outliers into the point cloud data from a single scan, further complicating the extraction of tree structural information. However, accurate tree structural information is crucial in applications such as robotic tree painting. Missing or inaccurate structural information can lead to decreased painting quality and may even damage the painting equipment.

[0004] Therefore, how to efficiently and robustly extract accurate tree structure information from sparse, noisy single-scan point cloud data has become a key problem that urgently needs to be solved in the field of robot-environment interaction. Thus, this invention addresses the problem of insufficient accuracy in restoring structural information when processing single-scan tree point cloud data in existing technologies by proposing a visual servo motion planning method for a forest maintenance robot spraying device. Summary of the Invention

[0005] The purpose of this invention is to provide a visual servo motion planning method for a forest maintenance robot spraying device, which overcomes the shortcomings of single-scan point cloud data, effectively solves challenges such as data sparsity and noise interference, and achieves accurate extraction of tree skeletons, providing more accurate and reliable structural information for applications such as robot tree painting.

[0006] To achieve the above objectives, the present invention provides a visual servo motion planning method for a forest maintenance robot spraying device, comprising the following steps:

[0007] S1. Use multi-line lidar to acquire point clouds of trees to be painted, and obtain point clouds of tree trunks to be painted by height threshold filtering and spatial clustering algorithm.

[0008] S2. The point cloud of the tree trunk to be painted obtained in step S1 is vertically segmented to obtain point cloud blocks. Kernel density estimation, point cloud weight calculation, and data digitization are performed on the overall point cloud and the point cloud blocks respectively. The first feature vectors of the overall point cloud of the tree trunk and each point cloud block are obtained based on the principal component analysis method and used as principal components.

[0009] S3. Use the weighted center algorithm to calculate the center point of the point cloud block corresponding to the lowest height and use it as the root node;

[0010] S4. Using the principal components of the point cloud blocks obtained in step S2, the principal components of the overall point cloud are corrected by the scaling factor, and then iteratively corrected. Based on the root node obtained in step S3, the center points of each point cloud block are continuously accumulated and connected in sequence to obtain the trunk skeleton.

[0011] S5. The tree trunk skeleton obtained in step S4 is smoothed by spatial Bezier curve processing to obtain the motion trajectory of the spraying device.

[0012] Preferably, in step S1, the Robosense-lidar-16 lidar is used to scan and acquire raw point cloud data, as shown below:

[0013] P = {p i |p i =(x i ,y i ,z i ),i=1,2,...,N}

[0014] Where P represents the point cloud, P i Let (x) represent the i-th point in the point cloud. i ,y i ,z i ) represents the three-dimensional coordinates of the i-th point, and N represents the total number of points in the point cloud.

[0015] Preferably, in step S1, a height threshold filtering method is used to denoise the point cloud. The height threshold is set to 1.2m, and all points in the point cloud data with Z coordinate values ​​greater than 1.2m are removed, while points with Z coordinate values ​​less than or equal to 1.2m are retained.

[0016] S121. Define a point cloud P′ containing only points that satisfy the height condition, as shown below:

[0017] P′={p i |p i =(xi ,y i ,z i ),z i ≤1.2, i=1,2,...,N}

[0018] Where P′ represents the point cloud after height threshold filtering, p i Represents the i-th point in the original point cloud P; (x i ,y i ,z i () represents the three-dimensional coordinates of the i-th point, z i ≤1.2 represents point p i The height shall not exceed 1.2m;

[0019] S122. Define a selection condition function χ(z) i The function in z i The value is 1 if it is ≤1.2, and 0 otherwise.

[0020]

[0021] S123, Use the filtering function χ(z) i The filtering condition for point cloud P′ is as follows:

[0022] P′={p i |p i =(x i ,y i ,z i ),χ(z i )=1}

[0023] The matrix form of the point cloud P′ is as follows:

[0024]

[0025] Where N′ is the expression that satisfies z i The number of points ≤ 1.2; therefore, the point cloud matrix P′ after being filtered by a height threshold of 1.2m is obtained.

[0026] Preferably, in step S1, the filtered point cloud P′ is processed using a density-based spatial clustering algorithm, as follows:

[0027] S131. For any point p in the point cloud, its neighborhood N ∈ (p) is defined as p i All points contained within a sphere of radius ∈ centered at x, i.e.:

[0028] N ∈ (p)={q∈R 3|dist(p,q)<∈}

[0029] Where dist(p,q) is the Euclidean distance between points p and q;

[0030] S132. If a point's ∈ neighborhood contains at least MinPts points, then that point is defined as a core point, as shown below:

[0031] |N ∈ (p i )|≥MinPts

[0032] Where, |N ∈ (p i | represents the number of elements in the set; if point q is in the neighborhood of point p, and p is a core point, then q is considered to be directly density-reachable, that is:

[0033] q∈N ∈ (p)

[0034] If there exists a series of points p1, p2, ..., p... n Make p i+1 For every i that is directly density-reachable, then point p n It is considered to be accessible from the p1 density;

[0035] S133, Global point cloud P after clustering algorithm processing G The data includes points with empty backgrounds removed, as shown below:

[0036]

[0037] Where M is the number of points retained after clustering; P is the point cloud data after height threshold filtering and spatial clustering. G This refers to point clouds with empty background data removed.

[0038] Preferably, in step S2, the point cloud of the tree trunk to be painted obtained in step S1 is vertically segmented to obtain point cloud blocks, and kernel density estimation and point cloud weight calculation are performed on the overall point cloud and the point cloud blocks respectively. The specific process is as follows:

[0039] S21. Transfer the global point cloud P G The point cloud is divided into 10 non-interfering intersecting point cloud blocks with a height of 12cm by slicing at equal intervals along the z-axis. A height threshold h is defined. i (i = 1, 2, ..., 10), as shown below:

[0040] h i =0.12(i-1)

[0041] Will satisfy h i≤p z <h i+1 The point p∈P G Divided into point cloud block B i , where p z Let P represent the z-coordinate of point p. Then the global point cloud P... G It is divided into 10 point cloud blocks, as shown below:

[0042]

[0043] Among them, B i B represents the i-th point cloud block; each point cloud block B i This includes the z-coordinate value of the global point cloud located in region [h] i ,h i+1 All points within )

[0044] S22, For each point cloud block B i Kernel density estimation is performed for each point in the point cloud block B. i For any point x in the matrix, its density value is obtained by weighting the contributions of all points in its neighborhood. The weighting function is determined by the kernel function K. The kernel density estimate at point x is as follows:

[0045]

[0046] in, This is the estimated density at point x, where n is the density of point cloud patch B. i The number of midpoints, d is the dimension of the points; h is the bandwidth parameter, which determines the width of the kernel; K is the kernel function, using a Gaussian kernel, as shown below:

[0047]

[0048] Then for each point x∈B i Its nuclear density is estimated as follows:

[0049]

[0050] S23. Assign initial weights to each point based on the kernel density estimate, and introduce an exponential decay factor to adjust the spatial distribution characteristics of the weights. Point x... j The initial weights w(x) j ), as shown below:

[0051]

[0052] The weights are normalized as follows:

[0053]

[0054] Among them, w norm (xj ) represents point x j In point cloud block B i The normalized weights in the data;

[0055] Through the above weight calculation and normalization process, each point is assigned a weight value between 0 and 1.

[0056] Preferably, in step S2, principal component analysis is used to center the point cloud data to determine the main direction of the tree point cloud. The specific process is as follows:

[0057] S241. First, perform global point cloud centralization, and centralize the global point cloud data P. G Centralized computation is performed on the global point cloud P. G Each point p i Represented as three-dimensional coordinates (x i ,y i ,z i ), calculate the mean vector μ of the point cloud, as shown below:

[0058]

[0059] Where M is the point cloud P G The number of points in;

[0060] S242. Perform point cloud block centralization, for each point cloud block B i Its mean vector, i.e., the centroid c, is calculated as follows:

[0061]

[0062] Where, n i It is point cloud block B i The number of points in, b ij These are the coordinates of the j-th point;

[0063] S243. After calculating the centroid, move the origin of the point cloud block to the centroid. The point cloud P after centering is then... c As shown below:

[0064] P c ={p i -μ∣i=1,2,...,M}

[0065] Similarly, point cloud block B i Centralized data B i,c As shown below:

[0066] B i,c ={b ij -c|j=1,2,...,n i}

[0067] Preferably, in step S2, the first feature vectors of the overall point cloud of the tree trunk and each point cloud block are obtained based on the principal component analysis method and used as principal components. The specific process is as follows:

[0068] S251. To analyze the features of point clouds, it is necessary to construct a centralized matrix X of the point cloud data. c Each row of this matrix corresponds to a centered point. As shown below:

[0069]

[0070] Using point cloud P G The centered matrix X c Calculate the point cloud covariance matrix C:

[0071]

[0072] The center matrix of the point cloud block is X i,c As shown below:

[0073]

[0074] Where n represents the number of points in the point cloud block;

[0075] S252, Point Cloud P G The eigenvalues ​​and eigenvectors of the covariance matrix C are obtained through the following characteristic equation:

[0076] Cv i =λ i v i

[0077] Where, λ i Let v represent the i-th eigenvalue of the covariance matrix C. i Represents the relationship with eigenvalue λ i The corresponding feature vector;

[0078] Similarly, the covariance matrix of a point cloud patch is obtained through the following characteristic equation:

[0079] C i u i =μ i u i

[0080] Where, μ i Represents the covariance matrix C i The i-th eigenvalue, u i Represents the relationship with eigenvalue μ i The corresponding feature vector;

[0081] S253. Select the eigenvector corresponding to the largest eigenvalue in the covariance matrix as the principal component, i.e., the principal direction of the point cloud, as shown below:

[0082]

[0083]

[0084] Where v1 is the global point cloud P G The first eigenvector, u i,1 It is point cloud block B i The first eigenvector.

[0085] Preferably, in step S3, the center point of the point cloud block corresponding to the lowest height point is calculated using the weighted center algorithm and used as the root node. The specific process is as follows:

[0086] S31. Select the weighted center point of the point cloud block containing the point with the lowest z-coordinate in the global point cloud as the starting point, with a weight of w. norm (x j ), traverse the global point cloud P G Find the point p with the lowest z-coordinate. min As shown below:

[0087]

[0088] Among them, z i It is point p i The z-coordinate value;

[0089] S32, Determine p i The point cloud block to which it belongs is denoted as B. min , for B min Calculate the weighted centroid to obtain the root node O. root As shown below:

[0090]

[0091] Among them, b min,j It is point cloud block B min The coordinates of the j-th point in the array; w norm (b min,j () is point b min,j Normalized weights; n min It is point cloud block B min The number of points in the array.

[0092] Preferably, in step S4, the principal components of the point cloud blocks obtained in step S2 are used to correct the principal components of the overall point cloud using a scaling factor, followed by iterative correction. Based on the root node obtained in step S3, the center points of each point cloud block are continuously accumulated, and then connected sequentially to obtain the tree trunk skeleton. The specific process is as follows:

[0093] S41. Let B be the i-th point cloud block. i The amount of data is N i Global point cloud P G If the amount of data is M, then the scaling factor α of the i-th point cloud block is... i As shown below:

[0094]

[0095] Using the proportionality coefficient α i Iterative corrections are made to the principal components of other point cloud blocks, that is, in each iteration, the current point cloud block B is used. i The first eigenvector u i,1 The overall point cloud P after the (i-1)th iteration is corrected. G The first feature vector v1;

[0096] S42. Let the overall eigenvector after the i-th iteration be... The formula for correcting the overall eigenvector during the i-th iteration is as follows:

[0097]

[0098] S43. After each correction, update the starting point O. i+1 Assuming the height of each point cloud block is 12cm, the scaling factor is defined as:

[0099]

[0100] The starting point update formula is as follows:

[0101]

[0102] S44. The center point O of the point cloud block obtained in each update iteration. i+1 Add it to the center point matrix O as follows:

[0103]

[0104] S45. Iterate and calculate the center point until all point cloud blocks participate in the iteration process, then stop the iteration.

[0105] Preferably, in step S5, O1, O2, ..., O n-1 A total of m = n-1 points are used as control points for the Bézier curve. The formula for the Bézier curve is as follows:

[0106]

[0107] Where t is a parameter; B i,m(t) is the basis function of the i-th control point in the n-th degree Bézier curve, as shown below:

[0108]

[0109] Therefore, the visual servo motion planning method of the above-mentioned forest maintenance robot spraying device adopted in this invention has the following beneficial effects:

[0110] 1. This invention addresses the sparsity problem of single-scan point cloud data by innovatively combining kernel density estimation and principal component analysis, and employing an iterative optimization strategy to effectively improve the accuracy of tree trunk skeleton reconstruction. This enables more accurate restoration of the tree's true structural information, providing a reliable basis for subsequent spraying path planning.

[0111] 2. The method proposed in this invention can adapt to different tree species, morphological differences, and complex situations in the natural environment, such as foliage obstruction and trunk bending. It has strong robustness and generalization ability and can be extended to a wider range of forest scenarios.

[0112] 3. Based on accurate tree trunk skeleton information, the spraying path generated by this invention is more reasonable and efficient. It can reduce the waste of spraying agent and reduce the cost of spraying operations while ensuring the spraying effect, and effectively avoid pollution to non-target areas.

[0113] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0114] Figure 1 This is a flowchart of a visual servo motion planning method for a forest maintenance robot spraying device according to the present invention;

[0115] Figure 2 This is a schematic diagram of the tree trunk skeleton extraction of the present invention;

[0116] Figure 3 This is a comparative diagram of the root node extraction method using weighted extraction and the root node extraction method using unweighted extraction for the tree trunk point cloud in this invention. Detailed Implementation

[0117] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0118] like Figure 1 As shown, the present invention discloses a visual servo motion planning method for a forest maintenance robot spraying device, comprising the following steps:

[0119] S1. Use multi-line lidar to acquire point clouds of trees to be painted, and obtain point clouds of tree trunks to be painted by height threshold filtering and spatial clustering algorithm.

[0120] S2. The point cloud of the tree trunk to be painted obtained in step S1 is vertically segmented to obtain point cloud blocks. Kernel density estimation, point cloud weight calculation, and data digitization are performed on the overall point cloud and the point cloud blocks respectively. The first feature vectors of the overall point cloud of the tree trunk and each point cloud block are obtained based on the principal component analysis method and used as principal components.

[0121] S3. Use the weighted center algorithm to calculate the center point of the point cloud block corresponding to the lowest height and use it as the root node;

[0122] S4. Using the principal components of the point cloud blocks obtained in step S2, the principal components of the overall point cloud are corrected by the scaling factor, and then iteratively corrected. Based on the root node obtained in step S3, the center points of each point cloud block are continuously accumulated and connected in sequence to obtain the trunk skeleton.

[0123] S5. The tree trunk skeleton obtained in step S4 is smoothed by spatial Bezier curve processing to obtain the motion trajectory of the spraying device.

[0124] The present invention discloses a visual servo motion planning method for a tree maintenance robot spraying device. The movable tree maintenance robot configured for spraying has the following structure: the bottom of the tree maintenance robot is equipped with a tracked mobile platform, the front is equipped with a wraparound tree paint spraying device, the top is equipped with a depth camera, and it is equipped with at least a lidar.

[0125] Example

[0126] S1. Use a multi-line lidar to acquire point clouds of the trees to be painted, and then use height threshold filtering and spatial clustering algorithms to obtain point clouds of the tree trunks to be painted.

[0127] S11. Using the Robosense-lidar-16 LiDAR scanner, raw point cloud data is obtained, as shown below:

[0128] P = {p i |p i =(x i ,y i ,z i ),i=1,2,...,N}

[0129] Where P represents the point cloud, P i Let (x) represent the i-th point in the point cloud. i ,y i ,z i ) represents the three-dimensional coordinates of the i-th point, and N represents the total number of points in the point cloud.

[0130] S12. Based on the requirements for robot tree painting and considering the domestic tree painting height of 1.2m, the point cloud is preprocessed with a height threshold filter of 1.2m, i.e., z iThe number of points ≤1.2 is reduced, thereby reducing the number of point clouds that need to be calculated and improving the efficiency of the robot's tree painting operation.

[0131] A height threshold filtering method was used to denoise the point cloud. The robot's tree painting operation mainly focuses on the bottom 1.2m of the tree trunk. Point cloud data with Z-coordinate values ​​greater than 1.2m mainly come from non-target areas such as the crown and branches, and are meaningless for the painting operation. Therefore, a height threshold of 1.2m was set, and all points with Z-coordinate values ​​greater than 1.2m were removed, retaining only points with Z-coordinate values ​​less than or equal to 1.2m.

[0132] S121. Define a point cloud P′ containing only points that satisfy the height condition, as shown below:

[0133] P′={p i |p i =(x i ,y i ,z i ),z i ≤1.2, i=1,2,...,N}

[0134] Where P′ represents the point cloud after height threshold filtering, p i Represents the i-th point in the original point cloud P; (x i ,y i ,z i () represents the three-dimensional coordinates of the i-th point, z i ≤1.2 represents point p i The height should not exceed 1.2m.

[0135] S122. Define a selection condition function χ(z) i The function in z i The value is 1 if it is ≤1.2, and 0 otherwise.

[0136]

[0137] S123, Use the filtering function χ(z) i The filtering condition for point cloud P′ is as follows:

[0138] P′={p i |p i =(x i ,y i ,z i ),χ(z i )=1}

[0139] The matrix form of the point cloud P′ is as follows:

[0140]

[0141] Where N′ is the expression that satisfies z i The number of points ≤ 1.2. This yields the point cloud matrix P′ after threshold filtering with a height of 1.2m.

[0142] S13. In order to remove the empty background data scanned in the point cloud, the density-based spatial clustering of applications with noise (DBSCAN) algorithm is used to process the filtered point cloud P′.

[0143] S131. For any point p in the point cloud, its neighborhood N ∈ (p) is defined as p i All points contained within a sphere of radius ∈ centered at x, i.e.:

[0144] N ∈ (p)={q∈R 3 |dist(p,q)<∈}

[0145] Where dist(p,q) is the Euclidean distance between points p and q.

[0146] S132. If a point's ∈ neighborhood contains at least MinPts points, then that point is defined as a core point, as shown below:

[0147] |N ∈ (p i )|≥MinPts

[0148] Where, |N ∈ (p i | represents the number of elements in the set. If point q is in the neighborhood of point p, and p is a core point, then q is considered directly density-reachable, i.e.:

[0149] q∈N ∈ (p)

[0150] If there exists a series of points p1, p2, ..., p n Make p i+1 For every i that is directly density-reachable, then point p n It is considered to be attainable from the p1 density.

[0151] S133. Clustering: In the initial state, all points are unvisited; then, all points are traversed. If a point is unvisited, it is marked as visited, and its neighborhood is calculated. If the number of points in the neighborhood is not less than MinPts, a new cluster is expanded starting with that point as the core point. For each point in its neighborhood, if it is unvisited, it is marked as visited; if the neighborhood of that point contains at least MinPts points, these points are added to the neighborhood of the current cluster (expanding the cluster). Points not included in any cluster are marked as noise.

[0152] The global point cloud P after clustering algorithm processing G The data includes points with empty backgrounds removed, as shown below:

[0153]

[0154] Where M is the number of points retained after clustering. Thus, the point cloud data P after height threshold filtering and spatial clustering... G It simply removes the point cloud from the empty background data.

[0155] S2. The point cloud of the tree trunk to be painted obtained in step S1 is vertically segmented to obtain point cloud blocks. Kernel density estimation, point cloud weight calculation, and data digitization are performed on the overall point cloud and the point cloud blocks respectively. The first feature vectors of the overall point cloud of the tree trunk and each point cloud block are obtained based on the principal component analysis method and used as principal components.

[0156] S21. To facilitate segmented analysis of the tree trunk point cloud, the global point cloud P is... G The point cloud is divided into 10 non-interfering, intersecting point cloud blocks with a height of 12 cm by equal-interval slicing along the vertical direction (z-axis). Specifically, a height threshold h is defined. i (i = 1, 2, ..., 10), as shown below:

[0157] h i =0.12(i-1)

[0158] Will satisfy h i ≤p z <h i+1 The point p∈P G Divided into point cloud block B i , where p z Let z represent the z-coordinate of point p. Therefore, the global point cloud P... G It is divided into 10 point cloud blocks, as shown below:

[0159]

[0160] Among them, B i This represents the i-th point cloud block. Each point cloud block B... iThis includes the z-coordinate value of the global point cloud located in region [h] i ,h i+1 All points within )

[0161] S22. To characterize the point density distribution within each point cloud block, for each point cloud block B... i Kernel density estimation (KDE) is performed on each point in the point cloud. For point cloud block B... i For any point x in the matrix, its density value is obtained by weighting the contributions of all points in its neighborhood, with the weighting function determined by the kernel function K. Therefore, the kernel density estimate at point x is as follows:

[0162]

[0163] in, This is the estimated density at point x, where n is the density of point cloud patch B. i The number of midpoints, d is the dimension of the points (3 in this case); h is the bandwidth parameter, which determines the width of the kernel; K is the kernel function, using a Gaussian kernel, as shown below:

[0164]

[0165] Then for each point x∈B i Its nuclear density is estimated as follows:

[0166]

[0167] Wherein, the kernel density estimate for each point x It can be used in subsequent weight adjustment operations.

[0168] S23. To reflect the impact of point cloud density distribution on subsequent processing, an initial weight is assigned to each point based on the kernel density estimate, and an exponential decay factor is introduced to adjust the spatial distribution characteristics of the weights. Specifically, point x... j The initial weights w(x) j ), as shown below:

[0169]

[0170] The weights are then normalized, as shown below:

[0171]

[0172] Among them, w norm (x j ) represents point x j In point cloud block B i The normalized weights in the equation.

[0173] Through the weight calculation and normalization process described above, each point is assigned a weight value between 0 and 1, which reflects the relative importance of the point within its corresponding point cloud patch. Points in high-density areas have lower weights, while points in low-density areas have higher weights. This helps to highlight the characteristics of low-density areas in subsequent processing.

[0174] S24. In order to determine the principal direction of the tree point cloud (i.e., the direction of the central axis of the tree trunk), principal component analysis (PCA) is used to center the point cloud data to eliminate the influence of coordinate system selection on the calculation of the principal direction.

[0175] S241. First, perform global point cloud centralization, and centralize the global point cloud data P. G Centralized computation is performed on the global point cloud P. G Each point p i Represented as three-dimensional coordinates (x i ,y i ,z i ), calculate the mean vector μ of the point cloud, as shown below:

[0176]

[0177] Where M is the point cloud P G The number of points in the array.

[0178] S242. Perform point cloud block centralization, for each point cloud block B i The mean vector (centroid) c is calculated as follows:

[0179]

[0180] Where, n i It is point cloud block B i The number of points in, b ij These are the coordinates of the j-th point.

[0181] S243. After calculating the centroid, move the origin of the point cloud block's coordinates to the centroid. The point cloud P after centering is... c As shown below:

[0182] P c ={p i -μ∣i=1,2,...,M}

[0183] Similarly, point cloud block B i Centralized data B i,c As shown below:

[0184] B i,c ={bij -c|j=1,2,...,n i}

[0185] S25. Based on the principal component analysis method, the first feature vectors of the overall point cloud of the tree trunk and each point cloud block are obtained and used as principal components.

[0186] S251. To analyze the features of point clouds, it is necessary to construct a centralized matrix X of the point cloud data. c Each row of this matrix corresponds to a centered point. As shown below:

[0187]

[0188] Using point cloud P G The centered matrix X c Calculate the point cloud covariance matrix C:

[0189]

[0190] The center matrix of the point cloud block is X i,c As shown below:

[0191]

[0192] Where n represents the number of points in the point cloud.

[0193] S252, Point Cloud P G The eigenvalues ​​and eigenvectors of the covariance matrix C are obtained through the following characteristic equation:

[0194] Cv i =λ i v i

[0195] Where, λ i Let v represent the i-th eigenvalue of the covariance matrix C. i Represents the relationship with eigenvalue λ i The corresponding feature vector.

[0196] Similarly, the covariance matrix of a point cloud patch is obtained through the following characteristic equation:

[0197] C i u i =μ i u i

[0198] Where, μ i Represents the covariance matrix C i The i-th eigenvalue, u i Represents the relationship with eigenvalue μ i The corresponding feature vector.

[0199] S253. Select the eigenvector corresponding to the largest eigenvalue in the covariance matrix as the principal component, i.e., the principal direction of the point cloud, as shown below:

[0200]

[0201] Where v1 is the global point cloud P G The first eigenvector, u i,1 It is point cloud block B i The first eigenvector.

[0202] A schematic diagram of tree trunk skeleton extraction, as shown below. Figure 2 As shown.

[0203] S3. Use the weighted center algorithm to calculate the center point of the point cloud block corresponding to the lowest height and use it as the root node.

[0204] S31. The first step in constructing the tree trunk skeleton is to determine the starting point of the skeleton. The weighted center point of the point cloud block containing the point with the lowest z-coordinate in the global point cloud is selected as the starting point, with a weight of w. norm (x j ), traverse the global point cloud P G Find the point p with the lowest z-coordinate. min As shown below:

[0205]

[0206] Among them, z i It is point p i The z-coordinate value.

[0207] S32, Determine p i The point cloud block to which it belongs is denoted as B. min , for B min Calculate the weighted centroid to obtain the root node O. root As shown below:

[0208]

[0209] Among them, b min,j It is point cloud block B min The coordinates of the j-th point in the array; w norm (b min,j () is point b min,j Normalized weights; n min It is point cloud block B min The number of points in the tree trunk point cloud. A comparative illustration of weighted and unweighted root node extraction, as shown below. Figure 3 As shown.

[0210] S4. Using the principal components of the point cloud blocks obtained in step S2, the principal components of the overall point cloud are corrected by the scaling factor, and then iteratively corrected. Based on the root node obtained in step S3, the center points of each point cloud block are continuously accumulated and connected in sequence to obtain the trunk skeleton.

[0211] S41. Let B be the i-th point cloud block. i The amount of data is N i Global point cloud P G If the amount of data is M, then the scaling factor α of the i-th point cloud block is... i As shown below:

[0212]

[0213] Using the proportionality coefficient α i Iterative corrections are made to the principal components of other point cloud blocks, that is, in each iteration, the current point cloud block B is used. i The first eigenvector u i,1 The overall point cloud P after the (i-1)th iteration is corrected. G The first eigenvector v1.

[0214] S42. Let the overall eigenvector after the i-th iteration be... The formula for correcting the overall eigenvector during the i-th iteration is as follows:

[0215]

[0216] S43. To ensure height consistency among point cloud blocks, the starting point coordinates need to be updated after each correction. Considering that the height of each point cloud block is 12cm, when updating the starting point, the z-axis component of the feature vector needs to be linearly transformed to correspond to an actual height of 12cm. The scaling factor is defined as:

[0217]

[0218] Specifically, the starting point update formula is:

[0219]

[0220] S44. The center point O of the point cloud block obtained in each update iteration. i+1 Add it to the center point matrix O as follows:

[0221]

[0222] S45. Iterate and calculate the center point until all point cloud blocks participate in the iteration process, then stop the iteration.

[0223] S5. The tree trunk skeleton obtained in step S4 is smoothed by spatial Bezier curve processing to obtain the motion trajectory of the spraying device.

[0224] In three-dimensional space, the tree trunk skeleton can be viewed as a combination of smooth spatial curves. Spatial Bézier curves are selected to smooth the extracted skeleton. O1, O2, ..., O n-1 A total of m = n-1 points are used as control points for the Bézier curve. The formula for the Bézier curve is as follows:

[0225]

[0226] Where t is a parameter; B i,m (t) is the basis function of the i-th control point in the n-th degree Bézier curve, as shown below:

[0227]

[0228] Therefore, the present invention adopts the visual servo motion planning method of the above-mentioned forest maintenance robot spraying device, which overcomes the shortcomings of single-scan point cloud data, effectively solves the challenges of data sparsity and noise interference, realizes the accurate extraction of tree skeleton, and provides more accurate and reliable structural information for applications such as robot tree painting.

[0229] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A visual servo motion planning method for a forest maintenance robot spraying device, characterized in that, Includes the following steps: S1. Use multi-line lidar to acquire point clouds of trees to be painted, and obtain point clouds of tree trunks to be painted by height threshold filtering and spatial clustering algorithm. S2. The point cloud of the tree trunk to be painted obtained in step S1 is vertically segmented to obtain point cloud blocks. Kernel density estimation, point cloud weight calculation, and data digitization are performed on the overall point cloud and the point cloud blocks respectively. The first feature vectors of the overall point cloud of the tree trunk and each point cloud block are obtained based on the principal component analysis method and used as principal components. S3. Use the weighted center algorithm to calculate the center point of the point cloud block corresponding to the lowest height and use it as the root node; S4. Using the principal components of the point cloud blocks obtained in step S2, the principal components of the overall point cloud are corrected by scaling factors, and then iteratively corrected. Based on the root node obtained in step S3, the center points of each point cloud block are continuously accumulated, and then connected sequentially to obtain the tree trunk skeleton. The specific process is as follows: S41, Let the first A point cloud block The amount of data is Global point cloud The amount of data is Then the first The ratio of point cloud blocks As shown below: ； Use the scaling factor Iterative corrections are made to the principal components of other point cloud blocks, that is, the current point cloud block is used in each iteration. First eigenvector Amendment No. The overall point cloud after the second iteration correction First eigenvector ; S42, Let the first The overall eigenvector after the next iteration correction is: , No. The formula for correcting the overall eigenvector in the next iteration is as follows: ； S43. Update the starting point after each correction. Assuming the height of each point cloud block is 12cm, the scaling factor is defined as: ； The starting point update formula is as follows: ； S44. Calculate the center point of the point cloud block obtained in each update iteration. Add to the center point matrix As shown below: ； S45. Iterate and calculate the center point until all point cloud blocks participate in the iteration process, then stop the iteration. S5. The tree trunk skeleton obtained in step S4 is smoothed by spatial Bezier curve processing to obtain the motion trajectory of the spraying device.

2. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 1, characterized in that, In step S1, the Robosense-lidar-16 lidar is used to scan and acquire raw point cloud data, as shown below: ； in, Represents point clouds, Represents the first point in the original point cloud. One point, Indicates the first The three-dimensional coordinates of the points This represents the total number of points in the point cloud.

3. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 1, characterized in that: In step S1, a height threshold filtering method is used to denoise the point cloud. The height threshold is set to 1.2m. All points with Z coordinate values ​​greater than 1.2m in the point cloud data are removed, and points with Z coordinate values ​​less than or equal to 1.2m are retained. S121. Define a point cloud containing only points that satisfy the height condition. As shown below: ； in, This represents the point cloud after height threshold filtering. Represents the original point cloud The first in One point; Indicates the first The three-dimensional coordinates of the points Point The height shall not exceed 1.2m; S122. Define a filtering condition function. The function is in The value is 1 if it is 1, otherwise it is 0. ； S123, Filter function As point cloud The filtering criteria, and the filtered point clouds As shown below: ； Then dotted clouds The matrix form is shown below: ； in, To meet The number of points; therefore, the point cloud matrix after 1.2m height threshold filtering is obtained. .

4. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 1, characterized in that, In step S1, a density-based spatial clustering algorithm is used to process the filtered point cloud. The specific process is as follows: S131. For any point in the point cloud ,That Neighborhood Defined as Centered on, with radius as All points contained within the spherical space, namely: ； in, It is a point and The Euclidean distance between them; S132, If a point The neighborhood contains at least If there are 1 point, then that point is defined as the core point, as shown below: ； in, Indicates the number of elements in the set; if the point At point of Within the neighborhood, and It is a core point, then It is considered to be directly density-reachable, that is: ； If there exists a series of points Make For each If all are directly density-reachable, then the point... Considered to be from Density is achievable; S133, Global point cloud after clustering algorithm processing The data includes points with empty backgrounds removed, as shown below: ； in, It refers to the number of points retained after clustering; the point cloud data after height threshold filtering and spatial clustering. This refers to point clouds with empty background data removed.

5. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 1, characterized in that, In step S2, the point cloud of the tree trunk to be painted obtained in step S1 is vertically segmented to obtain point cloud blocks. Kernel density estimation and point cloud weight calculation are then performed on the overall point cloud and the point cloud blocks respectively. The specific process is as follows: S21. Convert the global point cloud along The point cloud is divided into 10 non-interfering, intersecting point cloud blocks with a height of 12cm by equal-interval slicing along the axis. A height threshold is defined. As shown below: ； Will satisfy point Divided into point cloud blocks ,in Point of Coordinate values, then global point cloud It is divided into 10 point cloud blocks, as shown below: ； in, Indicates the first Each point cloud block; Includes global point cloud Coordinate values ​​are located in the area All points within; S22, For each point cloud block Kernel density estimation is performed on each point in the point cloud. any point in Its density value is obtained by weighting the contributions of all points in the neighborhood, and the weighting function is derived from the kernel function. If you're sure, then click. The kernel density estimate at that location is shown below: ； in, It is at point The estimated density at that location, It is a point cloud block The number of midpoints It is the dimension of a point; It is the bandwidth parameter that determines the width of the core; It's a kernel function, using a Gaussian kernel, as shown below: ； Then for each point Its nuclear density is estimated as follows: ； S23. Assign initial weights to each point based on the kernel density estimate, and introduce an exponential decay factor to adjust the spatial distribution characteristics of the weights. initial weights As shown below: ； The weights are normalized as follows: ； in, Point In the point cloud block The normalized weights in the data; Through the above weight calculation and normalization process, each point is assigned a weight value between 0 and 1.

6. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 5, characterized in that, In step S2, principal component analysis is used to center the point cloud data and determine the main direction of the tree point cloud. The specific process is as follows: S241. First, perform global point cloud centralization and process the global point cloud data. Centralized computation, global point cloud Each point Represented as three-dimensional coordinates Calculate the mean vector of the point cloud. As shown below: ； in, It is a point cloud The number of points in; S242. Centralize point cloud blocks, for each point cloud block Its mean vector, i.e., the centroid, is calculated. As shown below: ； in, It is a point cloud block The number of points in It is the first The coordinates of the points; S243. After calculating the centroid, move the origin of the point cloud block's coordinates to the centroid. The point cloud after centering is... As shown below: ； Similarly, point cloud blocks Centralized data As shown below: 。 7. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 6, characterized in that, In step S2, the first feature vectors of the overall point cloud of the tree trunk and each point cloud block are obtained based on the principal component analysis method and used as principal components. The specific process is as follows: S251. To analyze the characteristics of point clouds, it is necessary to construct a centralized matrix of the point cloud data. Each row of this matrix corresponds to a centered point. As shown below: ； Using point clouds Centralized matrix Calculate the point cloud covariance matrix : ； The center matrix of the point cloud block is As shown below: ； in, This indicates the number of points in the point cloud block; S252, Point Cloud covariance matrix The eigenvalues ​​and eigenvectors are obtained through the following characteristic equation: ； in, Represents the covariance matrix The 1 eigenvalue, Representation and eigenvalues The corresponding feature vector; Similarly, the covariance matrix of a point cloud patch is obtained through the following characteristic equation: ； in, Represents the covariance matrix The 1 eigenvalue, Representation and eigenvalues The corresponding feature vector; S253. Select the eigenvector corresponding to the largest eigenvalue in the covariance matrix as the principal component, i.e., the principal direction of the point cloud, as shown below: ； ； in, It is a global point cloud The first eigenvector, It is a point cloud block The first eigenvector.

8. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 1, characterized in that, In step S3, the center point of the point cloud block corresponding to the lowest height point is calculated using the weighted center algorithm and used as the root node. The specific process is as follows: S31. Select the global point cloud. The starting point is the weighted center point of the point cloud block containing the point with the lowest coordinates, with a weight of... Traverse the global point cloud ,turn up The point with the lowest coordinates As shown below: ； in, It is a point of Coordinate values; S32, Confirm The point cloud block to which it belongs is denoted as ,right Calculate the weighted centroid to obtain the root node. As shown below: ； in, It is a point cloud block The first in The coordinates of the points; It is a point Normalized weights; It is a point cloud block The number of points in the array.

9. The visual servo motion planning method for a forest maintenance robot spraying device according to claim 1, characterized in that, In step S5, select total These points are used as control points for the Bézier curve. The formula for the Bézier curve is as follows: ； in, It is a parameter; It is the first Each control point is The basis functions of the sub-Bézier curve are shown below: 。

Images