Special-shaped trunk self-adaptive whitewashing method, whitewashing system and whitewashing device

Abstract

The invention discloses a special-shaped trunk self-adaptive whitewashing method, whitewashing system and whitewashing device, and belongs to the technical field of special-shaped trunk whitewashing, and the method comprises the following steps: S1, obtaining a to-be-brushed special-shaped trunk point cloud and a trunk reference model point cloud; s2, the mass center of each point cloud block of the to-be-brushed special-shaped trunk is calculated; s3, determining whitewashing attitude parameters; S31, obtaining a total rotation matrix; s32, the calculated quaternion is converted into a whitewashing posture parameter; s4, a whitewashing path is planned; S41, a discretized segmented B spline curve is constructed, and reachable path point pairs are constructed; and S42, interpolating the reachable path point pairs by using a Bresenham algorithm, and generating a whitewashing path. By adopting the special-shaped trunk self-adaptive whitewashing method, the whitewashing system and the whitewashing device, the self-adaptive whitewashing operation of the special-shaped trunk is realized by determining the whitewashing attitude parameter and planning the path, and the efficiency of the whitewashing operation of the special-shaped trunk is improved.

Description

Technical Field

[0001] The 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 from excessive sunlight. It plays an indispensable role in promoting the healthy growth of fruit trees and optimizing forestry management strategies.

[0003] Most existing technologies use manual whitewashing, which is often insufficient when faced with tree trunks of different shapes and in difficult-to-reach locations, and cannot achieve the desired comprehensive and effective protection. And with the rapid advancement of modern agricultural automation technology, especially in the field of forest maintenance, the traditional manual whitewashing of tree trunks has increasingly shown its limitations due to its high-intensity physical labor requirements, low work efficiency, and difficulty in ensuring uniform and consistent coating effects.

[0004] In response to the above challenges, many innovative automated tree trunk whitewashing machines have emerged in recent years. Most of the above machines have built-in basic automated control systems to perform whitewashing operations, and are designed to serve upright and uniformly growing tree trunks. However, they are not flexible enough for unconventional tree trunks with variable growth postures, such as tilted, twisted, or complex curved shapes. They cannot fully adapt to the needs of different tree trunk sizes and growth patterns, and still require more manual assistance, making it difficult to find an ideal balance between improving work efficiency and ensuring coating quality. Summary of the invention

[0005] The purpose of the present invention is to provide a method, system and device for adaptively whitewashing 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 irregular tree trunks to be painted and the upright uniform tree trunks respectively, and obtain the point cloud of the irregular tree trunks to be painted and the 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 trunk reference model obtained in step S1 based on the height 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 an 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-painted posture parameters, where the white-painted posture parameters include the white-painted pitch angle and the roll angle;

[0012] S4. Planning the whitening path:

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

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

[0015] The whitewashing system of the adaptive whitewashing method for special-shaped tree trunks is characterized by comprising:

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

[0017] A preprocessing module is used to stratify the point cloud of the irregular 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 centroid of each point cloud block of the irregular 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 result, obtains the total rotation matrix by matrix transposition, calculates the corresponding quaternion according to the total rotation matrix, and converts the quaternion into white-out attitude parameters, which include white-out pitch angle and roll angle;

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

[0021] A whitening system for an adaptive whitening method for a special-shaped tree trunk comprises a three-axis lead screw slide and a two-degree-of-freedom embracing whitening mechanism, wherein the three-axis lead screw slide comprises 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 table for 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 white-painting method, white-painting system and white-painting device for special-shaped tree trunks, and 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 It is a flow chart of the adaptive whitening method for special-shaped tree trunks of the present invention;

[0027] Figure 2 It is a schematic diagram of constructing a point cloud of a special-shaped tree trunk to be painted and a point cloud of a reference model of a tree trunk in the method for adaptively painting a special-shaped tree trunk of the present invention;

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

[0029] Figure 4 Schematic diagram of ICP point cloud registration for estimating trunk pose of the special-shaped tree trunk adaptive whitening method of the present invention

[0030] Figure 5 It is a schematic structural 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", etc. indicate positions or positional relationships based on the positions or positional relationships shown in the accompanying drawings, or the positions or positional relationships in which the invented product is usually placed when in use. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention. In the description of the present invention, it should also be noted that, unless otherwise clearly 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 it can be an indirect connection through an intermediate medium, or it can be a connection between the 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 are described in detail below in conjunction with the accompanying drawings.

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

[0036] S1. Use a 3D laser radar to scan the irregular tree trunks to be painted and the upright uniform tree trunks respectively, and obtain the point cloud of the irregular tree trunks to be painted and the 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, 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 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] In the formula, 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 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] In the formula, 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 trunk reference model obtained in step S1 based on the height 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 whitening 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 reference model of the trunk 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, and the point cloud P of the irregular tree trunk to be painted and the point cloud Q of the reference model of the trunk are traversed respectively. According to the z-axis coordinate z of the ith 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] In the formula, 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] In the formula, 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] In the formula, 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 located at 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 tree 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, which is located in layer L j Point cloud block The centroid

[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 in step S23 The centroid Construct a set of centroids:

[0062]

[0063] Where S represents the centroid set.

[0064] S3. Determine the whitening posture parameters:

[0065] S31, using an 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, using the iterative closest point algorithm, the point cloud P of the irregular tree trunk to be painted in each point cloud block pair is located at layer L j Point cloud block and the trunk reference model point cloud Q located at layer L j Point cloud block Perform registration;

[0068] S3111, for 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] In the formula, 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 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 point obtained in step S311, construct the nearest neighbor point cloud block according to the layer to which it belongs.

[0072]

[0073] In the formula, 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 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 centroid of:

[0075]

[0076] In the formula, Indicates that it is located at layer L j Point cloud blocks of the irregular tree trunks to be painted The centroid of

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

[0078]

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

[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] In the formula, 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] In the formula, ←——represents the update operator;

[0094] S3118: Calculate the updated value obtained in step S3117 The point cloud blocks are composed of Nearest neighbor The average distance error

[0095]

[0096] S3119, repeat S3111 to S3118 until When it is less than the preset threshold or reaches the maximum number of iterations, the nearest point iteration algorithm is judged 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 in step S3119 Constructing the 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 tree trunk reference model to the point cloud block of the irregular tree trunk to be painted;

[0101] S313, sequentially loading 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-painted posture parameters, where the white-painted posture parameters include the white-painted 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 is the sum of the diagonal elements of the composite rotation matrix, 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. Planning the whitening path:

[0119] S41, using the centroid of each point cloud block of the irregular tree trunk to be painted obtained in step S2 as a control point, constructing a discretized piecewise B-spline curve, and constructing 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, and divide the 12 centroids into 3 groups, with 4 centroids in each group, and the grouping is as follows:

[0122]

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

[0124] S412. In order 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 vector is defined as follows:

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

[0126] In the formula, 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 node vector as the current node vector to ensure that the B-spline curve passes through the first and last control points at the beginning and end. The clamped node 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, calculating the third-order basis function and constructing a piecewise cubic B-spline curve equation;

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

[0131]

[0132] In the formula, 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-1th order basis function of the i-th element, N i+1,k-1(t) represents the k-1th 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] In the formula, B l (t) represents the cubic B-spline curve equation of the lth segment, where l = 0, 1, 2, respectively representing 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] And 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] In the formula, 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] In the formula, 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] In the formula, B l (t u ) represents the cubic B-spline curve equation of the discretized lth segment;

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

[0161] in:

[0162]

[0163] In the formula, 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, Respectively represent the centroid subset S l+1 The X-axis, Y-axis, and Z-axis coordinates of the i-th centroid in ;

[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 conversion matrix T1 from the three-dimensional radar coordinate system to the three-axis lead screw slide coordinate system of the whitening device:

[0166]

[0167] In the formula, X1, Y1, and Z1 represent the offset of the three-axis ball screw slide coordinate system relative to the three-dimensional radar coordinate system on the XYZ axes;

[0168] S4162: Apply the transformation matrix T1 obtained in step S4161 to the path point B obtained in step S4152. l (t u ) and change 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] In the formula, 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] In the formula, 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 offset G of the two-degree-of-freedom encircling white-coating mechanism p :

[0175]

[0176] Where L represents the joint length of the two-degree-of-freedom embracing white-coating mechanism of the white-coating device, θx The initial pitch angle of the two-degree-of-freedom encircling whitening 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 reachable path points of the three-axis ball screw slide coordinate system obtained in step S4164 as the reference coordinate system, construct reachable path point pairs 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] In the formula, 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 reachable path point, 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, using 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 of adjacent reachable path points, determining the stepping direction and the main axis direction of the whitening 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, calculating 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 between 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 between each pair of achievable path points and the adjacent points on the y-axis 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 between each pair of achievable path points and the adjacent points 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 size of the coordinate values ​​of two adjacent points in the path point pair on each coordinate axis;

[0200] Among them, the calculation formula of 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 direction and the reverse direction 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, the main axis direction is determined by comparing the displacement differences of the x, y, and z coordinate axes:

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

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

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

[0211]

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

[0213]

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

[0215]

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

[0217] S422, using the Bresenham algorithm to generate a whitening path.

[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 to the ratio of the number of stepper motor steps of the three-axis lead screw slide:

[0220]

[0221] In the formula, d represents the scaling factor, h represents the lead of the three-axis screw slide, that is, the distance the 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 pair 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 spindle direction obtained in step S4213, and the initialization error value obtained in step S4214, performing step control and error update according to the spindle 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 difference of adjacent points in each pair of reachable path points obtained in step S41 on the x, y, 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 of 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 comprises:

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

[0252] A preprocessing module is used to stratify the point cloud of the irregular 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 centroid of each point cloud block of the irregular 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 result, obtains the total rotation matrix by matrix transposition, calculates the corresponding quaternion according to the total rotation matrix, and converts the quaternion into white-out attitude parameters, which include white-out pitch angle and roll angle;

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

[0256] like Figure 5 As shown, the whitening device of the adaptive whitening method for the special-shaped tree trunk comprises a three-axis lead screw slide and a two-degree-of-freedom embracing whitening mechanism, wherein the three-axis lead screw slide comprises 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 solution of the present invention rather than to limit it. 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 solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A method for adaptive whitewashing of special-shaped tree trunks, characterized in that: The following steps are involved: S1. Use a 3D laser radar to scan the irregular tree trunks to be painted and the upright uniform tree trunks respectively, and obtain the point cloud of the irregular tree trunks to be painted and the 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 trunk reference model obtained in step S1 based on the height 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 an 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-painted posture parameters, where the white-painted posture parameters include the white-painted pitch angle and the roll angle; S4. Planning the whitening path: S41, using the centroid of each point cloud block of the irregular tree trunk to be painted obtained in step S2 as a control point, 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 constructed in step S41 to generate a whitened path.

2. The method for adaptive whitewashing of 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 P of the irregular tree trunk to be painted is obtained: P={p i ∣p i =(x P,i ,y P,i ,z P,i ),i=1,2,...,N} (1); In the formula, 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 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; 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. Q={q i ∣q i =(x Q,i ,y Q,i ,z Q,i ),i=1,2,...,M} (2); In the formula, 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.

3. The method for adaptive whitewashing of 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 P of the irregular tree trunk to be painted and the point cloud Q of the reference model of the tree trunk are layered along the z-axis direction, and are divided into 12 layers of point cloud blocks of equal height, and the point cloud P of the irregular tree trunk to be painted and the point cloud Q of the reference model of the tree trunk are traversed respectively, and the z coordinate z of the i-th point in the point cloud P of the irregular tree trunk to be painted is calculated. P,i and the z 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: j=z / 0.1+1 (3); In the formula, z represents the z-axis coordinate of the point cloud; Get the point cloud P of the irregular tree trunk to be painted: In the formula, Indicates that the point cloud P of the irregular tree trunk to be painted is located in layer L j Point cloud blocks; And obtain the trunk reference model point cloud Q: In the formula, Indicates the trunk reference model point cloud Q located at layer L j Point cloud blocks; 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 located at 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 tree trunk reference model belonging to the same layer one by one to obtain point cloud block pairs: In the formula, Pair(L j ) indicates layer L j Point cloud block pairs; S23, calculate the point cloud P of the irregular tree trunk to be painted obtained in step S21, which is located in layer L j Point cloud block The centroid In the formula, 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; S24, each point cloud block of the painted tree trunk obtained in step S23 The centroid Construct a set of centroids: Where S represents the centroid set.

4. The method for adaptive whitewashing of 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, using the iterative closest point algorithm, the point cloud P of the irregular tree trunk to be painted in each point cloud block pair is located at layer L j Point cloud block and the trunk reference model point cloud Q located at layer L j Point cloud block Perform registration; S3111, for 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 : In the formula, 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 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; S3112: For the nearest neighbor point obtained in step S311, construct the nearest neighbor point cloud block according to the layer to which it belongs. In the formula, 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 layer L j Point cloud block of the tree trunk reference model The point cloud block composed of the nearest neighbor points in ; S3113, calculate the layer L j Point cloud blocks of the irregular tree trunks to be painted The centroid of: In the formula, Indicates that it is located at layer L j Point cloud blocks of the irregular tree trunks to be painted The centroid of And calculate the point cloud block Each nearest neighbor point cloud block The centroid of: in, Indicates that it is located at layer L j of The nearest neighbor point cloud block The centroid of S3114, construct cross covariance matrix Where T represents matrix transpose; S3115, the cross covariance matrix obtained in step S314 Perform singular value decomposition: In the formula, 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 S315 and translation vectors The rotation matrix The calculation formula is as follows: Translation Vector The calculation formula is as follows: S3117, the rotation matrix obtained in step S3116 and translation vectors The transformation to , update the position of each point: In the formula, ←——represents the update operator; S3118: Calculate the updated value obtained in step S3117 The point cloud blocks are composed of Nearest neighbor The average distance error S3119, repeat S3111 to S3118 until When it is less than the preset threshold or reaches the maximum number of iterations, the nearest point iteration algorithm is judged 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; 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 in step S3119 Constructing the composite rotation matrix 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 tree trunk reference model to the point cloud block of the irregular tree trunk to be painted; S313, sequentially loading the next layer of point cloud blocks Repeat steps S311 to S312 until the point cloud registration of all layers is completed.

5. The method for adaptive whitewashing of 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 S3 The corresponding quaternion The four components of the quaternion The calculation formula is as follows: In the formula, tr(R L,total ) represents the composite rotation matrix The trace of , sgn represents the sign 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: Roll Angle The calculation formula is as follows: S323, determine the control strategy according to the level j: when j=1, directly output the pitch angle and roll angle Perform posture control; When j>1, calculate the current pitch angle With the previous pitch angle Pitch angle difference And calculate the current roll angle With the previous corner Roll angle difference Output pitch angle difference and roll angle difference 6. The method for adaptive whitewashing of special-shaped tree trunks according to claim 5, characterized in that: Step S41 specifically includes the following steps: S411. Assume that there are 12 centroids in the centroid set, and divide the 12 centroids into 3 groups, with 4 centroids in each group, and the grouping is as follows: In the formula, They represent the 1st, 2nd, 3rd, 4th, 5th, 6th, 7th, 8th, 9th, 10th, 11th, and 12th centroids in the centroid set respectively; S412, using a cubic B-spline curve to fit each group of centroids, and defining the node vector as follows: T=[t0,t1,…,t e+3 ] (31); In the formula, e represents the number of centroids in each group; S413. Set the clamped node vector as the current node vector to ensure that the B-spline curve passes through the first and last control points at the beginning and end. The clamped node vector is defined as follows: T＝[0,0,0,0,1,1,1,1] (32)； S414, based on the node vector obtained in S413, calculating the third-order basis function and constructing a piecewise cubic B-spline curve equation; S4141, calculate the zero-order basis function N i,0 (t): 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; S4142, recursively calculate the third-order basis function N i,3 (t): 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-1th order basis function of the i-th element, N i+1,k-1 (t) represents the k-1th order basis function of the i+1th element; After three recursions, we get: First-order basis function N i,1 (t): Second-order basis function N i,2 (t): The third-order basis function N i,3 (t): 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: In the formula, B l (t) represents the cubic B-spline curve equation of the lth segment, where l = 0, 1, 2, respectively representing 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 . And the cubic B-spline curve equation constructed by the four control points in the centroid subset S1 is as follows: The cubic B-spline curve equation constructed by the four control points in the centroid subset S2 is as follows: The cubic B-spline curve equation constructed by the four control points in the centroid subset S3 is as follows: In the formula, the value range of t is [0,1]; S415, discretizing the piecewise cubic B-spline curve obtained in S414, and calculating corresponding path points; S4151. Discretize the parameter t in the interval [0,1] with a step size of Δt: t u =uΔt (42); Where, t u represents the discretized parameter value, u represents the number of discrete points, and u=0,1,…,N, S4152, for each discretized parameter value t u , calculate the corresponding path points according to the piecewise cubic B-spline curve equation: In the formula, B l (t u ) represents the cubic B-spline curve equation of the discretized lth segment; Rewrite as B l (t u )=(x l (t u ),y l (t u ),z l (t u )) in: In the formula, 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, Respectively represent the centroid subset S l+1 The X-axis, Y-axis, and Z-axis coordinates of the i-th centroid in ; 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 T1 from the three-dimensional radar coordinate system to the three-axis lead screw slide coordinate system of the whitening device: In the formula, X1, Y1, and Z1 represent the offset of the three-axis ball screw slide coordinate system relative to the three-dimensional radar coordinate system on the XYZ axes; S4162: Apply the transformation matrix T1 obtained in step S4161 to the path point B obtained in step S4152. l (t u ) and change the path point B l (t u )Convert from the 3D radar coordinate system to the 3-axis ball screw slide coordinate system: B' l (t u )=T1·B l (t u ) (48); In the formula, B' l (t u ) represents the converted path point; and rewritten as: B’ l (t u )=(x’ l (t u ),y’ l (t u ),z’ l (t u )) (49); In the formula, 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; S4163, construct the offset G of the two-degree-of-freedom encircling white-coating mechanism p : Where L represents the joint length of the two-degree-of-freedom embracing white-coating mechanism of the white-coating device, θ x represents the initial pitch angle of the two-degree-of-freedom encircling white-painting mechanism; 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: B’ l (t u )←——B’ l (t u )-G P (51); S4165, based on the reachable path points of the three-axis ball screw slide coordinate system obtained in step S4164 as the reference coordinate system, construct reachable path point pairs 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: P l,u =(B’ l (t u ),B’ l (t u+1 )) (52); Where P l,u Represents a pair of reachable path points; Rewritten as: 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); In the formula, 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; And the starting reachable path point pair is defined as follows: P start =(0,B'0(t0)) (54); Where P start represents the starting reachable path point pair, 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.

7. The method for adaptive whitewashing of special-shaped tree trunks according to claim 6, 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 of adjacent reachable path points, determining the stepping direction and the main axis direction of the whitening device, and initializing the error value; S422, using the Bresenham algorithm to generate a whitening path.

8. The method for adaptive whitewashing of special-shaped tree trunks according to claim 7, characterized in that: Step S421 specifically includes the following steps: S4211, based on the reachable path point pairs obtained in step S41, calculating 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 Δx on the x-axis between each pair of achievable path points and the adjacent points in them is l,u The calculation formula is as follows: Δx l,u =|x’ l (t u+1 )-x’ l (t u )| (55); The displacement difference Δy of each pair of achievable path points on the adjacent points on the y-axis l,u The calculation formula is as follows: Δy l,u =|y' l (t u+1 )-y' l (t u )| (56); The displacement difference Δz on the z-axis between each pair of achievable path points and the adjacent points l,u The calculation formula is as follows: S4212, based on the reachable path point pair obtained in step S41, determine the stepping direction on each coordinate axis according to the size of the coordinate values ​​of two adjacent points in the path point pair on each coordinate axis; Among them, the calculation formula of the step direction sx on the x-axis is as follows: The calculation formula for the step direction sy on the y-axis is as follows: The calculation formula for the step direction sz on the z-axis is as follows: In the formula, 1 and -1 represent a step in the positive direction and the reverse direction 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, the main axis direction is determined by comparing the displacement differences of the x, y, and z coordinate axes: Principal axis = argmax(Δx l,u ,Δy l,u ,Δz l,u )(61); S4214, based on the displacement difference of the adjacent points in each pair of reachable path points obtained in step S4211 on the x, y, z coordinate axes, and based on the main axis direction obtained in step S4213, initialize the error value to control the stepping in the non-main axis direction; If the x-axis is the main axis, the initialization error value calculation formula is as follows: If the y-axis is the main axis, the initialization error value calculation formula is as follows: If the z-axis is the main axis, the initialization error value calculation formula is as follows: In the formula, Respectively represent the initial errors on the X, Z, and Y axes, 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 to the ratio of the number of stepper motor steps of the three-axis lead screw slide: In the formula, d represents the scaling factor, h represents the lead of the three-axis ball screw slide, and β represents the step angle of the stepper motor; And update the coordinates of the reachable path points, the formula is as follows: x l '(t u )←——x l '(t u )·d(66); y l '(t u )←——y l '(t u )·d(67); z l '(t u )←——z l '(t u )·d(68); S4222: Based on the reachable path point pair 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 principal axis, the iteration conditions are as follows: x’ l (t u )≠x’ l (t u+1 ) (69); If the y-axis is the principal axis, the iteration conditions are as follows: y’ l (t u )≠y’ l (t u+1 ) (70); If the z-axis is the principal axis, the iteration conditions are as follows: z’ l (t u )≠z’ l (t u+1 ) (71); S4223, based on the reachable path point pair obtained in step S41, the spindle direction obtained in step S4213, and the initialization error value obtained in step S4214, performing step control and error update according to the spindle direction; If the x-axis is the main axis, the step control and error update formulas are as follows: x l '(t u )←——x l '(t u )+sx(72); If the y-axis is the main axis, the step control and error update formulas are as follows: the l '(t u )←——y l '(t u )+and(75); If the z-axis is the main axis, the step control and error update formulas are as follows: from l '(t u )←——z l '(t u )+sz(78); S4224, based on the displacement difference of adjacent points in each pair of reachable path points obtained in step S41 on the x, y, 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: If the y-axis is the main axis, the error update formula of the non-main axis is as follows: If the z-axis is the main axis, the error update formula of the non-main axis is as follows:

9. The whitewashing system of the adaptive whitewashing method for special-shaped tree trunks according to any one of claims 1 to 8, characterized in that: include: A point cloud acquisition module is used to scan the point clouds of the irregular tree trunks to be painted and the upright uniform tree trunks to obtain the point clouds of the irregular tree trunks to be painted and the point clouds of the tree trunk reference model; A preprocessing module is used to stratify the point cloud of the irregular 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 centroid of each point cloud block of the irregular 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 result, obtains the total rotation matrix by matrix transposition, calculates the corresponding quaternion according to the total rotation matrix, and converts the quaternion into white-out attitude parameters, which include white-out pitch angle and roll angle; The path planning module constructs a discretized piecewise B-spline curve and a pair of reachable path points.

10. The whitewashing device of any one of claims 1 to 8, characterized in that: It includes a three-axis lead screw slide and a two-degree-of-freedom encircling whitening 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; The white coating mechanism includes an opening and closing clamp connected to the output end of the Y-axis slide table for 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.

Images