Adaptive whitewashing method, system and device for painting irregular tree trunks

Abstract

The present invention discloses an adaptive whitewashing method, whitewashing system, and whitewashing device for special-shaped tree trunks, belonging to the technical field of whitewashing of special-shaped tree trunks, and includes the following steps: S1, obtaining a point cloud of the special-shaped tree trunk to be painted and a point cloud of a trunk reference model; S2, calculating the center of mass of each point cloud block of the special-shaped tree trunk to be painted; S3, determining the whitewashing posture parameters; S31, obtaining the total rotation matrix; S32, converting the calculated quaternion into the whitewashing posture parameters; S4, planning the whitewashing path; S41, constructing a discretized piecewise B-spline curve and constructing a pair of reachable path points; S42, using the Bresenham algorithm to interpolate the reachable path point pairs to generate a whitewashing path. By adopting the above-mentioned adaptive whitewashing method, whitewashing system, and whitewashing device for special-shaped tree trunks, by determining the whitewashing posture parameters and planning the path, the adaptive whitewashing operation of the special-shaped tree trunks is realized, and the efficiency of the whitewashing operation of the special-shaped tree trunks is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of whitewashing of special-shaped tree trunks, and in particular to a self-adaptive whitewashing method, a whitewashing system and a whitewashing device for special-shaped tree trunks. Background Art

[0002] Trunk whitewashing is a vital protective measure that can effectively block the invasion of pests and diseases and damage caused by excessive sunlight. It plays an indispensable role in promoting the healthy growth of fruit trees and optimizing forestry management strategies.

[0003] Existing technologies mostly rely on manual whitewashing, which often falls short of achieving the desired comprehensive and effective protection when faced with diverse tree trunk shapes and difficult-to-reach locations. Furthermore, with the rapid advancement of modern agricultural automation technology, especially in the forestry sector, traditional manual whitewashing is increasingly showing its limitations due to its high physical labor requirements, low efficiency, and difficulty ensuring a uniform and consistent coating.

[0004] To address these challenges, numerous innovative automated tree whitewashing machines have emerged in recent years. Most of these machines incorporate basic automated control systems to perform the whitewashing operation and are designed for upright, uniformly growing tree trunks. However, they are inflexible for unconventional trunks with variable growth patterns, such as tilted, twisted, or complex curving forms. They are unable to fully adapt to the demands of varying trunk sizes and growth patterns, and still require significant manual assistance, making it difficult to strike an ideal balance between efficiency and coating quality. Summary of the Invention

[0005] The purpose of the present invention is to provide a method, system and device for adaptive whitewashing of special-shaped tree trunks to solve the above-mentioned technical problems.

[0006] To achieve the above object, the present invention provides a method for adaptively whitewashing a special-shaped tree trunk, comprising the following steps:

[0007] S1. Use a 3D laser radar to scan the irregularly shaped tree trunk to be painted and the upright uniform tree trunk respectively, and obtain a point cloud of the irregularly shaped tree trunk to be painted and a point cloud of the trunk reference model;

[0008] S2. Layering the point cloud of the irregular tree trunk to be painted and the point cloud of the reference model of the tree trunk obtained in step S1 based on a height-based layering strategy to obtain point cloud blocks, pairing the point cloud blocks belonging to the same layer one by one to obtain point cloud block pairs, and calculating the centroid of each point cloud block of the irregular tree trunk to be painted;

[0009] S3. Determine the whitening posture parameters:

[0010] S31, using the iterative closest point algorithm to align the point cloud block pairs obtained in step S2, constructing a composite rotation matrix, and obtaining a total rotation matrix by matrix transposition;

[0011] S32, calculating the corresponding quaternion according to the total rotation matrix obtained in step S31, and converting the calculated quaternion into white-out posture parameters, where the white-out posture parameters include the white-out pitch angle and the roll angle;

[0012] S4. Plan the whitewashing path:

[0013] S41, using the centroids of the point cloud blocks of the irregularly shaped tree trunk to be painted obtained in step S2 as control points, constructing a discretized piecewise B-spline curve and a pair of reachable path points;

[0014] S42. Use the Bresenham algorithm to interpolate the reachable path point pairs constructed in step S41 to generate a whitened path.

[0015] A whitewashing system for an adaptive whitewashing method for a special-shaped tree trunk is characterized by comprising:

[0016] A point cloud acquisition module is used to scan point clouds of the irregularly shaped tree trunks to be painted and the upright uniform tree trunks to obtain point clouds of the irregularly shaped tree trunks to be painted and point clouds of the trunk reference model;

[0017] The preprocessing module is used to stratify the point cloud of the irregularly shaped tree trunk to be painted based on a height-based stratification strategy, obtain point cloud blocks, pair the point cloud blocks belonging to the same layer one by one to form point cloud block pairs, and calculate the center of mass of each point cloud block of the irregularly shaped tree trunk to be painted;

[0018] Point cloud registration module, which uses iterative closest point algorithm to register point cloud blocks;

[0019] The attitude conversion module constructs a composite rotation matrix based on the point cloud block registration results, obtains the total rotation matrix through matrix transposition, calculates the corresponding quaternion based on the total rotation matrix, and converts the quaternion into white-washed attitude parameters, which include white-washed pitch angle and roll angle;

[0020] The path planning module constructs a discretized piecewise B-spline curve and builds a pair of reachable path points.

[0021] A whitewashing system for an adaptive whitewashing method for a special-shaped tree trunk includes a three-axis lead screw slide and a two-degree-of-freedom encircling whitewashing mechanism, wherein the three-axis lead screw slide includes an X-axis slide, a Z-axis slide, and a Y-axis slide connected in sequence, and the X-axis slide, the Z-axis slide, and the Y-axis slide are all driven by a stepper motor;

[0022] The white coating mechanism includes an opening and closing clamp connected to the output end of the Y-axis slide in a vertical rotation. The opening and closing clamp has two degrees of freedom: pitch and roll, and each degree of freedom is controlled by a corresponding servo motor.

[0023] Therefore, the present invention adopts the above-mentioned adaptive whitewashing method, whitewashing system and whitewashing device for special-shaped tree trunks, which has the following beneficial effects:

[0024] It realizes the self-adaptation of the whitewashing operation for special-shaped tree trunks, which significantly improves the efficiency of the whitewashing operation.

[0025] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 This is a flow chart of the adaptive whitewashing method for special-shaped tree trunks of the present invention;

[0027] Figure 2 Schematic diagram of the construction of the point cloud of the irregular-shaped tree trunk to be painted and the point cloud of the reference model of the tree trunk in the adaptive white-painting method of the present invention;

[0028] Figure 3 This is a schematic diagram of the path planning after constructing a discretized piecewise B-spline curve in the adaptive whitewashing method for special-shaped tree trunks of the present invention;

[0029] Figure 4 Schematic diagram of the trunk pose estimation using ICP point cloud registration using the adaptive whitening method for irregular tree trunks of the present invention

[0030] Figure 5 It is a structural schematic diagram of the white coating device of the present invention.

[0031] Reference numerals

[0032] 1. X-axis slide; 2. Z-axis slide; 3. Y-axis slide; 4. Open and close gripper. DETAILED DESCRIPTION

[0033] In the description of the present invention, it should be noted that the terms "upper", "lower", "inside", "outside" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, or the orientations or positional relationships in which the inventive product is usually placed when in use. These are only for the convenience of describing the present invention and simplifying the description, and are not intended to indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation. Therefore, they cannot be understood as limitations on the present invention. In the description of the present invention, it should also be noted that, unless otherwise expressly specified and limited, the terms "setting", "installation" and "connection" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or an indirect connection through an intermediate medium, or it can be a communication between the internal parts of two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0034] The embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0035] like Figure 1-Figure 4 As shown, a method for adaptively whitewashing a special-shaped tree trunk is characterized by comprising the following steps:

[0036] S1. Use a 3D laser radar to scan the irregularly shaped tree trunk to be painted and the upright uniform tree trunk respectively, and obtain a point cloud of the irregularly shaped tree trunk to be painted and a point cloud of the trunk reference model;

[0037] In step S1, a 3D laser radar is used to scan the irregular tree trunk to be painted. After statistical filtering and denoising, voxel grid filtering and downsampling, and DBSCAN clustering are performed on the original point cloud, the point cloud P of the irregular tree trunk to be painted is obtained:

[0038] P={p i ∣p i =(x P,i ,y P,i ,z P,i ),i=1,2,…,N} (1);

[0039] Where p i represents the i-th point in the point cloud of the irregular tree trunk to be painted, (x P,i ,y P,i ,z P,i ) represents the three-dimensional coordinates of the i-th point in the point cloud P of the irregular-shaped tree trunk to be painted, and N represents the total number of points in the point cloud P of the tree trunk to be painted;

[0040] In step S1, a 3D radar is used to scan an upright uniform tree trunk, and its original point cloud is subjected to statistical filtering denoising, voxel grid filtering downsampling, and DBSCAN clustering to extract the trunk, and then a trunk reference model point cloud Q is constructed.

[0041] Q={q i ∣q i =(x Q,i ,y Q,i ,z Q,i ),i=1,2,…,M} (2);

[0042] Where q i represents the i-th point in the point cloud of the trunk reference model, (x Q,i ,y Q,i ,z Q,i ) represents the three-dimensional coordinates of the i-th point in the trunk reference model point cloud Q, and M represents the total number of points in the trunk reference model point cloud Q.

[0043] S2. Layering the point cloud of the irregular tree trunk to be painted and the point cloud of the reference model of the tree trunk obtained in step S1 based on a height-based layering strategy to obtain point cloud blocks, pairing the point cloud blocks belonging to the same layer one by one to obtain point cloud block pairs, and calculating the centroid of each point cloud block of the irregular tree trunk to be painted;

[0044] Step S2 specifically includes the following steps:

[0045] S21, according to the set white painting height (industry standard is 1.2m), the point cloud P of the irregular tree trunk to be painted and the point cloud Q of the trunk reference model are layered along the z-axis direction, and are divided into 12 layers of point cloud blocks with equal height. In this embodiment, they are divided into 12 point cloud blocks with a height of 10cm. The point cloud P of the irregular tree trunk to be painted and the point cloud Q of the trunk reference model are traversed respectively. According to the z-axis coordinate z of the i-th point in the point cloud P of the irregular tree trunk to be painted P,i and the z-axis coordinate z of the i-th point in the trunk reference model point cloud Q Q,i Assign it to the corresponding layer L j , where level j is determined by the following formula:

[0046] j=z / 0.1+1 (3);

[0047] Where z represents the z-axis coordinate of the point cloud; 0.1 represents the height of each layer, 0.1m;

[0048] Thus, the point cloud P of the irregular tree trunk to be painted and the point cloud Q of the tree trunk reference model are divided into 12 point cloud blocks, and the 12 point cloud blocks are located in twelve different layers L j .

[0049] Get the point cloud P of the irregular tree trunk to be painted:

[0050]

[0051] Where, Indicates that the point cloud P of the irregular tree trunk to be painted is located in layer L j Point cloud blocks;

[0052] And obtain the trunk reference model point cloud Q:

[0053]

[0054] Where, Indicates the trunk reference model point cloud Q located at layer L j Point cloud blocks, each point cloud block contains all the points belonging to the same layer in the point cloud block to which it belongs;

[0055] S22, based on the point cloud P of the irregular tree trunk to be painted obtained in step S21, located in layer L j Point cloud block and the trunk reference model point cloud Q is located in layer L j Point cloud block Pair the point cloud blocks of the irregular tree trunk to be painted and the point cloud blocks of the trunk reference model belonging to the same layer one by one to obtain point cloud block pairs:

[0056]

[0057] In the formula, Pair(L j ) indicates layer L j Point cloud block pairs;

[0058] S23, calculate the point cloud P of the irregular tree trunk to be painted obtained in step S21 in layer L j Point cloud block The center of mass

[0059]

[0060] Where n Lj Indicates that the point cloud P of the irregular tree trunk to be painted is located in layer L j Point cloud block The number of points, They represent the points in layer L in the point cloud P of the irregular tree trunk to be painted. j Point cloud block The three-dimensional coordinates of the i-th point;

[0061] S24, each point cloud block of the painted tree trunk obtained based on step S23 The center of mass Construct a set of centroids:

[0062]

[0063] Where S represents the centroid set.

[0064] S3. Determine the whitening posture parameters:

[0065] S31, using the iterative closest point algorithm to align the point cloud block pairs obtained in step S2, constructing a composite rotation matrix, and obtaining a total rotation matrix by matrix transposition;

[0066] Step S31 specifically includes the following steps:

[0067] S311, based on the point cloud block pairs obtained in step S22, use the iterative closest point algorithm to find the point cloud P of the irregular tree trunk to be painted in each point cloud block pair located in layer L j Point cloud block and the trunk reference model point cloud Q is located in layer L j Point cloud block Perform registration;

[0068] S3111, for the layer L j Point cloud block of the irregular tree trunk to be painted Each point in Make sure it is located in layer L j Point cloud block of the tree trunk reference model The nearest neighbor point in :

[0069]

[0070] Where, Indicates that it is located at layer L j Point cloud block of the irregular tree trunk to be painted The i-th point in In the layer L j Point cloud block of the tree trunk reference model The nearest neighbor point in Indicates that it is located at layer L j Point cloud block of the tree trunk reference model For the points in , || || represents the Euclidean distance;

[0071] S3112: For the nearest neighbor points obtained in step S311, construct the nearest neighbor point cloud block according to the layer to which it belongs.

[0072]

[0073] Where, Indicates that it is located at layer L j Point cloud block of the irregular tree trunk to be painted The i-th point in In the layer L j Point cloud block of the tree trunk reference model The point cloud block composed of the nearest neighbor points in ;

[0074] S3113, calculate the layer L j Point cloud blocks of the irregular tree trunks to be painted The center of mass:

[0075]

[0076] Where, Indicates that it is located at layer L j Point cloud blocks of the irregular tree trunks to be painted The center of mass;

[0077] And calculate the point cloud block Each nearest neighbor point cloud block The center of mass:

[0078]

[0079] in, Indicates that it is located at layer L j of The nearest neighbor point cloud block The center of mass;

[0080] S3114, construct cross covariance matrix

[0081]

[0082] Where T represents matrix transpose;

[0083] S3115, the cross covariance matrix obtained in step S314 Perform singular value decomposition:

[0084]

[0085] Where, represents the left singular vector matrix, ∑ represents the singular value matrix, represents the transpose of the right singular vector matrix;

[0086] S3116: Extract the rotation matrix R from the singular value decomposition result obtained in step S315 Lj and translation vectors

[0087] The rotation matrix The calculation formula is as follows:

[0088]

[0089] Translation vector The calculation formula is as follows:

[0090]

[0091] S3117, the rotation matrix obtained in step S3116 and translation vectors The transformation to Update the position of each point:

[0092]

[0093] Where, ←——represents the update operator;

[0094] S3118, calculate the updated The point cloud block is composed of Nearest neighbor The average distance error

[0095]

[0096] S3119, repeat S3111 to S3118 until When the value is less than the preset threshold or reaches the maximum number of iterations, the nearest point iteration algorithm is determined to have converged, and the rotation matrix obtained in each iteration is recorded as Where k represents the current iteration number, k = 1, 2, ..., N, and N represents the total number of iterations;

[0097] S312, based on the point cloud registration result obtained in step S311, construct a composite rotation matrix, and obtain a total rotation matrix by transposing;

[0098] S3121, based on the rotation matrix obtained in each iteration of step S3119 Constructing a composite rotation matrix

[0099]

[0100] S3122. Solve the composite rotation matrix obtained in step S3121 The inverse matrix of the rotation matrix is ​​the transpose of the rotation matrix, so it is recorded as Get the total rotation matrix from the point cloud block of the trunk reference model to the point cloud block of the irregular-shaped trunk to be painted;

[0101] S313, sequentially load the next layer of point cloud blocks Repeat steps S311 to S312 until the point cloud registration of all layers is completed.

[0102] S32, calculating the corresponding quaternion according to the total rotation matrix obtained in step S31, and converting the calculated quaternion into white-out posture parameters, where the white-out posture parameters include the white-out pitch angle and the roll angle;

[0103] Step S32 specifically includes the following steps:

[0104] S321, calculate the composite rotation matrix obtained in S3 The corresponding quaternion The four components of the quaternion The calculation formula is as follows:

[0105]

[0106]

[0107] In the formula, tr(R L,total ) represents the composite rotation matrix The trace of the composite rotation matrix is ​​the sum of the diagonal elements, sgn represents the sign function, Both represent composite rotation matrices Elements within;

[0108] S322, based on the quaternion obtained in step S321 Spraying pitch angle and roll angle

[0109] The pitch angle The calculation formula is as follows:

[0110]

[0111] Roll angle The calculation formula is as follows:

[0112]

[0113] S323, determine the control strategy according to level j: when j=1, directly output the pitch angle and roll angle Perform posture control;

[0114] When j>1, calculate the current pitch angle With the previous pitch angle Pitch angle difference

[0115] And calculate the current roll angle With the previous corner Roll angle difference

[0116]

[0117] Output pitch angle difference and roll angle difference

[0118] S4. Plan the whitewashing path:

[0119] S41, using the centroids of the point cloud blocks of the irregularly shaped tree trunk to be painted obtained in step S2 as control points, constructing a discretized piecewise B-spline curve and a pair of reachable path points;

[0120] Step S41 specifically includes the following steps:

[0121] S411. Assume that there are 12 centroids in the centroid set. Divide the 12 centroids into 3 groups, with 4 centroids in each group. The groups are as follows:

[0122]

[0123] Where, They represent the 1st, 2nd, 3rd, 4th, 5th, 6th, 7th, 8th, 9th, 10th, 11th, and 12th centroids in the centroid set respectively;

[0124] S412. To ensure that the generated path has a high degree of smoothness and adaptability, a cubic B-spline curve is used to fit each group of centroids, and the node vectors are defined as follows:

[0125] T=[t0,t1,…,t e+3 ] (31);

[0126] Where, e represents the number of centroids in each group, where e is 4, and 3 represents the degree of the B-spline curve;

[0127] S413. Set the clamped knot vector as the current knot vector to ensure that the B-spline curve passes through the first and last control points at the beginning and end. The clamped knot vector is defined as follows:

[0128] T = [0,0,0,0,1,1,1,1] (32);

[0129] S414. Based on the node vector obtained in S413, calculate the third-order basis function and construct a piecewise cubic B-spline curve equation;

[0130] S4141, calculate the zero-order basis function N i,0 (t):

[0131]

[0132] Where, t i Represents the i-th element in the Clamped node vector T, t i+1 represents the i+1th element in the clamped node vector T; t represents the element in the clamped node vector T;

[0133] S4142, recursively calculate the third-order basis function N i,3 (t):

[0134]

[0135] Where N i,k (t) represents the k-order basis function, t i+k Represents the i+kth element in the node vector T, t i+k+1 Represents the i+k+1th element in the node vector T, t i+1 Represents the i+1th element in the node vector T, N i,k-1 (t) represents the k-1 order basis function of the i-th element, N i+1,k-1(t) represents the k-1 order basis function of the i+1th element;

[0136] After three recursions, we get:

[0137] First-order basis function N i,1 (t):

[0138]

[0139] Second-order basis function N i,2 (t):

[0140]

[0141] The third-order basis function N i,3 (t):

[0142]

[0143] S4143, based on the first-order basis function N i,1 (t), second-order basis function N i,2 (t) and the third-order basis function N i,3 (t), construct the piecewise cubic B-spline curve equation:

[0144]

[0145] Where B l (t) represents the cubic B-spline curve equation of the lth segment, where l = 0, 1, 2, respectively, represents the cubic B-spline curve constructed by the four control points in the centroid subsets S1, S2, and S3, Represents the centroid subset S l+1 The i-th centroid in .

[0146] The cubic B-spline curve equation constructed by the four control points in the centroid subset S1 is as follows:

[0147]

[0148] The cubic B-spline curve equation constructed by the four control points in the centroid subset S2 is as follows:

[0149]

[0150] The cubic B-spline curve equation constructed by the four control points in the centroid subset S3 is as follows:

[0151]

[0152] Wherein, the value range of t is [0,1];

[0153] S415, discretizing the piecewise cubic B-spline curve obtained in S414, and calculating corresponding path points;

[0154] S4151. Discretize the parameter t in the interval [0,1] with a step size of Δt:

[0155] t u =uΔt (42);

[0156] Where, t u represents the discretized parameter value, u represents the number of discrete points, and u=0,1,…,N,

[0157] S4152, for each discretized parameter value t u , calculate the corresponding path points according to the piecewise cubic B-spline curve equation:

[0158]

[0159] Where B l (t u ) represents the cubic B-spline curve equation of the discretized segment l;

[0160] Rewrite as B l (t u )=(x l (t u ),y l (t u ),z l (t u ))

[0161] in:

[0162]

[0163] Where x l (t u ),y l (t u ),z l (t u ) represent B l (t u )’s X-axis, Y-axis, and Z-axis coordinates, Represents the centroid subset S respectively l+1 The X-axis, Y-axis, and Z-axis coordinates of the i-th centroid;

[0164] S416, performing coordinate transformation on the path points obtained in S415 to obtain reachable path points, and constructing reachable path point pairs;

[0165] S4161. Construct the transformation matrix T1 from the three-dimensional radar coordinate system to the three-axis ball screw slide coordinate system of the whitening device:

[0166]

[0167] Where X1, Y1, and Z1 represent the offset of the three-axis screw slide coordinate system relative to the three-dimensional radar coordinate system on the X, Y, and Z axes respectively;

[0168] S4162: Apply the transformation matrix T1 obtained in step S4161 to the path point B obtained in step S4152. l (t u ) on the path point B l (t u ) Convert from the 3D radar coordinate system to the 3-axis ball screw slide coordinate system:

[0169] B′ l (t u )=T1·B l (t u ) (48);

[0170] Where B′ l (t u ) represents the converted path point;

[0171] and rewritten as:

[0172] B′ l (t u )=(x′ l (t u ),y′ l (t u ),z′ l (t u )) (49);

[0173] Where x′ l (t u ),y′ l (t u ),z′ l (t u ) represent B′ l (t u )’s X-axis, Y-axis, and Z-axis coordinates;

[0174] S4163, construct the two-degree-of-freedom embracing whitewashing mechanism offset G p :

[0175]

[0176] Where L represents the joint length of the two-degree-of-freedom whitewashing mechanism, θx The initial pitch angle of the two-degree-of-freedom whitewashing mechanism can be directly measured;

[0177] S4164, the jaw offset G obtained in S4163 p Applied to the converted path point B obtained in step S4162 l '(t u ) to obtain the final reachable path points in the three-axis ball screw slide coordinate system:

[0178] B l '(t u )←——B l '(t u )-G P (51);

[0179] S4165: Based on the three-axis ball screw slide coordinate system obtained in step S4164, the reachable path points of the reference coordinate system are constructed in sequence, and each reachable path point pair includes the current reachable path point B. l '(t u ) and the next reachable path point B l '(t u+1 ), the formula is as follows:

[0180] P l,u =(B′ l (t u ),B′ l (t u+1 )) (52);

[0181] Where, P l,u Represents a pair of reachable path points;

[0182] Rewritten as:

[0183] P l,u =((x′ l (t u ),y′ l (t u ),z′ l (t u )),(x′ l (t u+1 ),y′ l (t u+1 ),z′ l (t u+1 ))) (53);

[0184] Where x′ l (t u+1 ),y′ l (t u+1 ),z′l (t u+1 ) represent B′ l (t u+1 )’s X-axis, Y-axis, and Z-axis coordinates;

[0185] And the starting reachable path point pair is defined as follows:

[0186] P start =(0,B'0(t0)) (54);

[0187] Where, P start represents the starting point of the reachable path, B'0(t0) represents the point obtained when t is 0 in the cubic B-spline curve constructed by the four control points in the centroid subset S1 in the three-axis slide coordinate system, that is, the starting point of the cubic B-spline curve.

[0188] S42. Use the Bresenham algorithm to interpolate the reachable path point pairs constructed in step S41 to generate a whitened path.

[0189] Step S42 specifically includes the following steps:

[0190] S421, based on the reachable path point pairs obtained in S41, calculating the displacement difference between adjacent reachable path points, determining the stepping direction and main axis direction of the white-painting device, and initializing the error value;

[0191] Step S421 specifically includes the following steps:

[0192] S4211. Based on the reachable path point pairs obtained in step S41, calculate the displacement differences of adjacent points in each pair of reachable path point pairs on the x, y, and z axes;

[0193] Among them, the displacement difference Δx on the x-axis of each pair of adjacent points on the reachable path is l,u The calculation formula is as follows:

[0194] Δx l,u =|x′ l (t u+1 )-x′ l (t u )| (55);

[0195] The displacement difference Δy on the y-axis between the adjacent points of each pair of achievable path points l,u The calculation formula is as follows:

[0196] Δy l,u =|y′ l (t u+1 )-y′ l (t u )| (56);

[0197] The displacement difference Δz on the z-axis of each pair of adjacent points in the reachable path l,u The calculation formula is as follows:

[0198] Δz l,u =|z′ l (t u+1 )-z′ l (t u )| (57);

[0199] S4212: Based on the reachable path point pair obtained in step S41, determine the stepping direction on each coordinate axis according to the coordinate values ​​of two adjacent points in the path point pair on each coordinate axis;

[0200] The calculation formula for the step direction sx on the x-axis is as follows:

[0201]

[0202] The calculation formula for the step direction sy on the y-axis is as follows:

[0203]

[0204] The calculation formula for the step direction sz on the z-axis is as follows:

[0205]

[0206] In the formula, 1 and -1 represent a step in the positive and negative directions respectively;

[0207] S4213. Based on the displacement differences of adjacent points in each pair of reachable path points obtained in step S4211 on the x, y, and z axes, determine the main axis direction by comparing the displacement differences on the x, y, and z coordinate axes:

[0208] Principal axis = argmax(Δx l,u ,Δy l,u ,Δz l,u )(61);

[0209] S4214: Initialize an error value based on the displacement differences of adjacent points in each pair of reachable path points obtained in step S4211 on the x, y, and z coordinate axes, and based on the main axis direction obtained in step S4213, to control the stepping in the non-main axis direction;

[0210] If the x-axis is the main axis, the initialization error value is calculated as follows:

[0211]

[0212] If the y-axis is the main axis, the initialization error value is calculated as follows:

[0213]

[0214] If the z-axis is the main axis, the initialization error value is calculated as follows:

[0215]

[0216] Where, Respectively represent the initial errors on the X, Z, and Y axes, used to determine whether to feed;

[0217] S422. Generate a white-painted path using the Bresenham algorithm.

[0218] Step S422 specifically includes the following steps:

[0219] S4221. Based on the reachable path point pair obtained in step S41, a scaling factor is used to convert the actual distance into the ratio of the number of stepper motor steps of the three-axis lead screw slide:

[0220]

[0221] Where d represents the scaling factor, h represents the lead of the three-axis ball screw slide, that is, the distance the ball screw slide moves when the stepper motor moves one circle, and β represents the step angle of the stepper motor.

[0222] And update the coordinates of the reachable path points. The formula is as follows:

[0223] x l '(t u )←——x l '(t u )·d(66);

[0224] y l '(t u )←——y l '(t u )·d(67);

[0225] z l '(t u )←——z l '(t u )·d(68);

[0226] S4222: Based on the reachable path point pairs obtained in step S41 and the main axis direction obtained in step S4213, set the iteration condition. The formula is as follows:

[0227] If the x-axis is the principal axis, the iteration conditions are as follows:

[0228] x l '(t u )≠x l '(t u+1) (69);

[0229] If the y-axis is the principal axis, the iteration conditions are as follows:

[0230] y′ l (t u )≠y′ l (t u+1 ) (70);

[0231] If the z-axis is the principal axis, the iteration conditions are as follows:

[0232] z′ l (t u )≠z′ l (t u+1 ) (71);

[0233] S4223: Based on the reachable path point pair obtained in step S41, the main axis direction obtained in step S4213, and the initialization error value obtained in step S4214, step control and error update are performed according to the main axis direction;

[0234] If the x-axis is the main axis, the step control and error update formulas are as follows:

[0235] x l '(t u )←——x l '(t u )+sx(72);

[0236]

[0237] If the y-axis is the main axis, the step control and error update formulas are as follows:

[0238] y l '(t u )←——y l '(t u )+sy(75);

[0239]

[0240] If the z-axis is the main axis, the step control and error update formulas are as follows:

[0241] z l '(t u )←——z l '(t u )+sz(78);

[0242]

[0243] S4224: Based on the displacement differences of adjacent points in each pair of reachable path points obtained in step S41 on the x, y, and z coordinate axes, the main axis direction obtained in step S4213, and the initialization error value obtained in step S4214, update the non-main axis error according to the main axis direction;

[0244] If the x-axis is the main axis, the error update formula for the non-main axis is as follows:

[0245]

[0246] If the y-axis is the main axis, the error update formula of the non-main axis is as follows:

[0247]

[0248] If the z-axis is the main axis, the error update formula of the non-main axis is as follows:

[0249]

[0250] The whitewashing system of the adaptive whitewashing method for special-shaped tree trunks includes:

[0251] A point cloud acquisition module is used to scan point clouds of the irregularly shaped tree trunks to be painted and the upright uniform tree trunks to obtain point clouds of the irregularly shaped tree trunks to be painted and point clouds of the trunk reference model;

[0252] The preprocessing module is used to stratify the point cloud of the irregularly shaped tree trunk to be painted based on a height-based stratification strategy, obtain point cloud blocks, pair the point cloud blocks belonging to the same layer one by one to form point cloud block pairs, and calculate the center of mass of each point cloud block of the irregularly shaped tree trunk to be painted;

[0253] Point cloud registration module, which uses iterative closest point algorithm to register point cloud blocks;

[0254] The attitude conversion module constructs a composite rotation matrix based on the point cloud block registration results, obtains the total rotation matrix through matrix transposition, calculates the corresponding quaternion based on the total rotation matrix, and converts the quaternion into white-washed attitude parameters, which include white-washed pitch angle and roll angle;

[0255] The path planning module constructs a discretized piecewise B-spline curve and builds a pair of reachable path points.

[0256] like Figure 5 As shown, the whitewashing device of the adaptive whitewashing method for special-shaped tree trunks includes a three-axis screw slide and a two-degree-of-freedom embracing whitewashing mechanism, wherein the three-axis screw slide includes an X-axis slide 1, a Z-axis slide 2, and a Y-axis slide 3 connected in sequence, and the X-axis slide 1, the Z-axis slide 2, and the Y-axis slide 3 are all driven by a stepper motor;

[0257] The white coating mechanism includes an opening and closing clamping jaw 4 which is vertically connected to the output end of the Y-axis slide 3. The opening and closing clamping jaw 4 has two degrees of freedom: pitch and roll, and each degree of freedom is controlled by a corresponding servo motor.

[0258] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit the same. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solutions of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A method for adaptive whitewashing of special-shaped tree trunks, characterized by: The following steps are involved: S1. Use a 3D laser radar to scan the irregularly shaped tree trunk to be painted and the upright uniform tree trunk respectively to obtain a point cloud of the irregularly shaped tree trunk to be painted and a point cloud of the trunk reference model; S2. Layering the point cloud of the irregular tree trunk to be painted and the point cloud of the reference model of the tree trunk obtained in step S1 based on a height-based layering strategy to obtain point cloud blocks, pairing the point cloud blocks belonging to the same layer one by one to obtain point cloud block pairs, and calculating the centroid of each point cloud block of the irregular tree trunk to be painted; S3. Determine the whitening posture parameters: S31, using the iterative closest point algorithm to align the point cloud block pairs obtained in step S2, constructing a composite rotation matrix, and obtaining a total rotation matrix by matrix transposition; S32, calculating the corresponding quaternion according to the total rotation matrix obtained in step S31, and converting the calculated quaternion into white-out posture parameters, where the white-out posture parameters include the white-out pitch angle and the roll angle; S4. Plan the whitewashing path: S41, using the centroids of the point cloud blocks of the irregularly shaped tree trunk to be painted obtained in step S2 as control points, constructing a discretized piecewise B-spline curve and a pair of reachable path points; Step S41 specifically includes the following steps: S411. Assume that there are 12 centroids in the centroid set. Divide the 12 centroids into 3 groups, with 4 centroids in each group. The groups are as follows: (1)； (2)； (3)； Where, , , They represent the 1st, 2nd, 3rd, 4th, 5th, 6th, 7th, 8th, 9th, 10th, 11th, and 12th centroids in the centroid set respectively; S412. Fit each group of centroids using a cubic B-spline curve, and define the node vectors as follows: (4)； Where, represents the number of centroids in each group; S413. Set the clamped knot vector as the current knot vector to ensure that the B-spline curve passes through the first and last control points at the beginning and end. The clamped knot vector is defined as follows: (5)； S414. Based on the node vector obtained in S413, calculate the third-order basis function and construct a piecewise cubic B-spline curve equation; S4141, calculate the zero-order basis function : (6)； Where, Represents the Clamped node vector The elements, Represents the Clamped node vector The elements; S4142, recursive calculation of third-order basis functions : （7）； Where, Indicates the elements order basis functions, Represents the Clamped node vector The elements, Represents the Clamped node vector The elements, Represents the Clamped node vector The elements, Indicates the elements order basis functions, Indicates the elements order basis functions; Calculate first-order basis functions : （8）； Calculate second-order basis functions : （9）； Calculate third-order basis functions : （10）； S4143, third-order basis function obtained based on S4142 , construct the piecewise cubic B-spline curve equation: （11）； Where, Indicates the The cubic B-spline curve equation of the segment is , respectively represent the centroid subsets 、 、 The cubic B-spline curve constructed by the four control points in Represents a subset of centroids The centroid; And the centroid subset The cubic B-spline curve equation constructed by the four control points in is as follows: （12）； Centroid subset The cubic B-spline curve equation constructed by the four control points in is as follows: （13）； Centroid subset The cubic B-spline curve equation constructed by the four control points in is as follows: （14）； Where, The value range is ; S415, discretizing the piecewise cubic B-spline curve obtained in S414, and calculating corresponding path points; S4151, set the parameters In the interval Step length Discretize: （15）； Where, represents the discretized parameter value, represents the number of discrete points, and , ; S4152, for each discretized parameter value , calculate the corresponding path points according to the piecewise cubic B-spline curve equation: （16）； Where, Represents the discretized The cubic B-spline curve equation of the segment; Rewrite formula (16) as in: （17）； （18）； （19）； Where, Respectively The X-axis, Y-axis, and Z-axis coordinates, 、 、 Represents the centroid subsets The The X-axis, Y-axis, and Z-axis coordinates of the center of mass; S416, performing coordinate transformation on the path points obtained in S415 to obtain reachable path points, and constructing reachable path point pairs; S4161, construct the conversion matrix from the three-dimensional radar coordinate system to the three-axis screw slide coordinate system of the whitening device : （20）； Where, They represent the coordinate system of the three-axis screw slide relative to the three-dimensional radar coordinate system. Offset on three axes; S4162, the conversion matrix obtained in step S4161 , applied to the path point obtained in step S4152 On the waypoint Convert from the 3D radar coordinate system to the 3-axis ball screw slide coordinate system: （21）； Where, Represents the converted path point; And rewrite formula (21) as: （22）； Where, Respectively X-axis, Y-axis, and Z-axis coordinates; S4163, construct the jaw offset of the two-degree-of-freedom encircling whitewashing mechanism : （23）； Where, The joint length of the two-degree-of-freedom whitewashing mechanism of the whitewashing device is represented by represents the initial pitch angle of the two-degree-of-freedom encircling white-painting mechanism; S4164, the jaw offset obtained in S4163 Applied to the converted path point obtained in step S4162 To obtain the final reachable path points in the three-axis ball screw slide coordinate system: （24）； Where, Represents the update operator; S4165: Based on the three-axis ball screw slide coordinate system obtained in step S4164, the reachable path points of the reference coordinate system are constructed in sequence, and each reachable path point pair includes the current reachable path point and the next reachable path point , the formula is as follows: （25）； Where, Represents a pair of reachable path points; Rewrite formula (25) as: （26）； Where, Respectively X-axis, Y-axis, and Z-axis coordinates; And the starting reachable path point pair is defined as follows: （27）； Where, represents the starting reachable path point pair, Represents the centroid subset in the three-axis slide coordinate system The cubic B-spline curve constructed by the four control points in Pick The point obtained when S42. Use the Bresenham algorithm to interpolate the reachable path point pairs constructed in step S41 to generate a whitened path.

2. The adaptive whitewashing method for special-shaped tree trunks according to claim 1, characterized in that: In step S1, a 3D laser radar is used to scan the irregular tree trunk to be painted, and the original point cloud is subjected to statistical filtering denoising, voxel grid filtering downsampling, and DBSCAN clustering to extract the irregular tree trunk, and then the point cloud of the irregular tree trunk to be painted is obtained. : （28）； Where, Indicates the first point cloud of the irregular tree trunk to be painted Points, Represents the point cloud of the irregular tree trunk to be painted The The three-dimensional coordinates of the points, Represents the point cloud of the tree trunk to be painted Total number of midpoints; In step S1, a 3D radar is used to scan an upright uniform tree trunk, and its original point cloud is subjected to statistical filtering denoising, voxel grid filtering downsampling, and DBSCAN clustering to extract the trunk, and then a trunk reference model point cloud is constructed. ; （29）； Where, Represents the first Points, Representing the point cloud of the tree trunk reference model The The three-dimensional coordinates of the points, Representing the point cloud of the tree trunk reference model The total number of midpoints.

3. The adaptive whitewashing method for special-shaped tree trunks according to claim 2, characterized in that: Step S2 specifically includes the following steps: S21. According to the set whitening height, the point cloud of the special-shaped tree trunk to be painted and the trunk reference model point cloud , layered along the Z axis, divided into 12 layers of equal-height point cloud blocks, and traversed the point cloud of the irregular tree trunk to be painted respectively and the trunk reference model point cloud , according to the point cloud of the painted special-shaped tree trunk Middle The Z-axis coordinate of the point and the trunk reference model point cloud Middle The Z-axis coordinate of the point Assign it to the corresponding layer , where the level Determined by the following formula: （30）； Where, Indicates the Z-axis coordinate of the point cloud; Obtain the point cloud of the irregular tree trunk to be painted : （31）； Where, Represents the point cloud of the irregular tree trunk to be painted Middle layer Point cloud blocks; And obtain the trunk reference model point cloud : （32）； Where, Representing the point cloud of the tree trunk reference model Middle layer Point cloud blocks; S22, based on the point cloud of the irregular tree trunk to be painted obtained in step S21 Middle layer Point cloud block and the trunk reference model point cloud Middle layer Point cloud block , pair the point cloud blocks of the irregular tree trunk to be painted and the point cloud blocks of the trunk reference model belonging to the same layer one by one to obtain the point cloud block pairs: （33）； Where, Presentation Layer Point cloud block pairs; S23, calculating the point cloud of the irregular tree trunk to be painted obtained in step S21 Middle layer Point cloud block The center of mass : （34）； Where, Represents the point cloud of the irregular tree trunk to be painted Middle layer Point cloud block The number of points, Represents the point cloud of the irregular tree trunk to be painted Middle layer Point cloud block No. The three-dimensional coordinates of a point; S24, each point cloud block of the painted tree trunk obtained based on step S23 The center of mass , construct a centroid set: （35）； Where, Represents a set of centroids.

4. The adaptive whitewashing method for special-shaped tree trunks according to claim 3, characterized in that: Step S31 specifically includes the following steps: S311, based on the point cloud block pairs obtained in step S22, use the iterative closest point algorithm to calculate the point cloud of the irregular tree trunk to be painted in each point cloud block pair. Middle layer Point cloud block and the trunk reference model point cloud Middle layer Point cloud block Perform registration; S3111, for the layer Point cloud block of the irregular tree trunk to be painted Each point in , determine whether it is located in the layer Point cloud block of the tree trunk reference model The nearest neighbor point in : （36）； Where, Indicates that it is located at the layer Point cloud block of the irregular tree trunk to be painted The points Located in the layer Point cloud block of the tree trunk reference model The nearest neighbor point in Indicates that it is located at the layer Point cloud block of the tree trunk reference model The point in represents the Euclidean distance; S3112: For the nearest neighbor points obtained in step S3111, construct the nearest neighbor point cloud block according to the layer to which it belongs. : （37）； Where, Indicates that it is located at the layer Point cloud block of the irregular tree trunk to be painted The points Located in the layer Point cloud block of the tree trunk reference model The point cloud block composed of the nearest neighbor points in ; S3113, calculation is located in the layer Point cloud blocks of the irregular tree trunks to be painted The center of mass: （38）； Where, Indicates that it is located at the layer Point cloud blocks of the irregular tree trunks to be painted The center of mass; And calculate the point cloud block Each nearest neighbor point cloud block The center of mass: （39）； in, Indicates that it is located at the layer of The nearest neighbor point cloud block The center of mass; S3114, construct cross covariance matrix : （40）； Where, Represents matrix transpose; S3115, the cross covariance matrix obtained in step S3114 Perform singular value decomposition: （41）； Where, represents the left singular vector matrix, represents the singular value matrix, represents the transpose of the right singular vector matrix; S3116: Extract the rotation matrix from the singular value decomposition result obtained in step S3115 and translation vectors : The rotation matrix The calculation formula is as follows: （42）； Translation vector The calculation formula is as follows: （43）； S3117, the rotation matrix obtained in step S3116 and translation vectors The transformation to Update the position of each point: （44）； Where, Represents the update operator; S3118, calculate the updated The point cloud block is composed of Nearest neighbor The average distance error : （45）； S3119, repeat S3111 to S3118 until When the value is less than the preset threshold or reaches the maximum number of iterations, the nearest point iteration algorithm is determined to have converged, and the rotation matrix obtained in each iteration is recorded as ,in Indicates the current iteration number, , Indicates the total number of iterations; S312, based on the point cloud registration result obtained in step S311, construct a composite rotation matrix, and obtain a total rotation matrix by transposing; S3121, based on the rotation matrix obtained in each iteration of step S3119 , construct the composite rotation matrix : （46）； S3122. Solve the composite rotation matrix obtained in step S3121 The inverse matrix of , get the total rotation matrix from the point cloud block of the trunk reference model to the point cloud block of the irregular-shaped trunk to be painted; S313, sequentially load the next layer of point cloud blocks , repeat steps S311 to S312 until the point cloud registration of all layers is completed.

5. The adaptive whitewashing method for special-shaped tree trunks according to claim 4, characterized in that: Step S32 specifically includes the following steps: S321. Calculate the composite rotation matrix obtained in S3121 The corresponding quaternion , where the four components of the quaternion are The calculation formula is as follows: （47）； （48）； （49）； （50）； Where, Represents a composite rotation matrix traces, represents the symbolic function, 、 、 、 、 、 、 、 、 Both represent composite rotation matrices Elements within; S322, based on the quaternion obtained in step S321 , spraying pitch angle and roll angle ; The pitch angle The calculation formula is as follows: （51）； Roll angle The calculation formula is as follows: （52）； S323, according to the level Determine the control strategy: When , directly output the pitch angle and roll angle Perform posture control; when Calculate the current pitch angle With the previous pitch angle Pitch angle difference : （53）； And calculate the current roll angle With the previous corner Roll angle difference : （54）； Output pitch angle difference and roll angle difference .

6. The adaptive whitewashing method for special-shaped tree trunks according to claim 5, characterized in that: Step S42 specifically includes the following steps: S421, based on the reachable path point pairs obtained in S41, calculating the displacement difference between adjacent reachable path points, determining the stepping direction and main axis direction of the white-painting device, and initializing the error value; S422. Generate a white-painted path using the Bresenham algorithm.

7. The adaptive whitewashing method for special-shaped tree trunks according to claim 6, characterized in that: Step S421 specifically includes the following steps: S4211. Based on the reachable path point pairs obtained in step S41, calculate the displacement differences of adjacent points in each pair of reachable path point pairs on the X, Y, and Z axes; Among them, the displacement difference of each pair of reachable path points on the adjacent points on the X axis is The calculation formula is as follows: （55）； The displacement difference of each pair of adjacent points on the y-axis The calculation formula is as follows: （56）； The displacement difference of each pair of adjacent points on the Z axis The calculation formula is as follows: （57）； S4212: Based on the reachable path point pair obtained in step S41, determine the stepping direction on each coordinate axis according to the coordinate values ​​of two adjacent points in the path point pair on each coordinate axis; Among them, the step direction on the X axis The calculation formula is as follows: （58）； Step direction on the Y axis The calculation formula is as follows: （59）； Step direction on the Z axis The calculation formula is as follows: （60）； In the formula, 1 and -1 represent a step in the positive and negative directions respectively; S4213. Based on the displacement differences of adjacent points in each pair of reachable path points obtained in step S4211 on the X, Y, and Z axes, determine the main axis direction by comparing the displacement differences on the X, Y, and Z coordinate axes: （61）； S4214: Initialize an error value based on the displacement differences of adjacent points in each pair of reachable path points obtained in step S4211 on the X, Y, and Z coordinate axes, and based on the main axis direction obtained in step S4213, to control the stepping in the non-main axis direction; If the X-axis is the main axis, the initialization error value is calculated as follows: （62）； If the Y axis is the main axis, the initialization error value is calculated as follows: （63）； If the Z axis is the main axis, the initialization error value is calculated as follows: （64）； Where, , , Respectively The initial error on the three axes is used to determine whether to feed; Step S422 specifically includes the following steps: S4221. Based on the reachable path point pair obtained in step S41, a scaling factor is used to convert the actual distance into the ratio of the number of stepper motor steps of the three-axis lead screw slide: （65）； Where, represents the scaling factor, Indicates the lead of the three-axis ball screw slide, Indicates the step angle of the stepper motor; And update the coordinates of the reachable path points. The formula is as follows: （66）； （67）； （68）； S4222: Based on the reachable path point pairs obtained in step S41 and the main axis direction obtained in step S4213, set the iteration condition. The formula is as follows: If the X axis is the main axis, the iteration conditions are as follows: （69）； If the Y axis is the main axis, the iteration conditions are as follows: （70）； If the Z axis is the main axis, the iteration conditions are as follows: （71）； S4223: Based on the reachable path point pair obtained in step S41, the main axis direction obtained in step S4213, and the initialization error value obtained in step S4214, step control and error update are performed according to the main axis direction; If the X-axis is the main axis, the step control and error update formulas are as follows: （72）； （73）； （74）； If the Y-axis is the main axis, the step control and error update formulas are as follows: （75）； （76）； （77）； If the Z axis is the main axis, the step control and error update formulas are as follows: （78）； （79）； （80）； S4224: Based on the displacement differences of adjacent points in each pair of reachable path points obtained in step S41 on the X, Y, and Z coordinate axes, the main axis direction obtained in step S4213, and the initialization error value obtained in step S4214, update the non-main axis error according to the main axis direction; If the X-axis is the main axis, the error update formula of the non-main axis is as follows: （81）； （82）； If the Y axis is the main axis, the error update formula of the non-main axis is as follows: （83）； （84）； If the Z axis is the main axis, the error update formula of the non-spindle is as follows: （85）； （86）。 8. The whitewashing system of any one of claims 1 to 7, wherein: include: A point cloud acquisition module is used to scan point clouds of the irregularly shaped tree trunks to be painted and the upright uniform tree trunks to obtain point clouds of the irregularly shaped tree trunks to be painted and point clouds of the trunk reference model; The preprocessing module is used to stratify the point cloud of the irregularly shaped tree trunk to be painted based on a height-based stratification strategy, obtain point cloud blocks, pair the point cloud blocks belonging to the same layer one by one to form point cloud block pairs, and calculate the center of mass of each point cloud block of the irregularly shaped tree trunk to be painted; Point cloud registration module, which uses iterative closest point algorithm to register point cloud blocks; The attitude conversion module constructs a composite rotation matrix based on the point cloud block registration results, obtains the total rotation matrix through matrix transposition, calculates the corresponding quaternion based on the total rotation matrix, and converts the quaternion into white-washed attitude parameters, which include white-washed pitch angle and roll angle; The path planning module constructs a discretized piecewise B-spline curve and builds a pair of reachable path points.

Images