A mechanism and method for verifying the accuracy of a tea leaf surface distance detection algorithm

Abstract

This invention discloses an accuracy verification mechanism and method for a tea canopy surface distance detection algorithm. The method obtains the distance values ​​between the two ends of a cutting blade and a simulated tea canopy surface using a two-dimensional lidar. Then, using the distance values ​​measured by the two-dimensional lidar, the distance between the cutting blade ends and the simulated tea canopy surface is used as the initial distance value. A multi-sensor fusion tea canopy surface distance detection algorithm is employed to optimally estimate the distance between the cutting blade ends and the simulated tea canopy surface. Finally, the error between the optimal estimated distance and the true distance measured directly by a high-precision sensor is calculated. The accuracy of the tea canopy surface distance detection algorithm is evaluated from the perspectives of the mean and variance of the error. Furthermore, by fine-tuning the parameter of the vertical acceleration measurement noise variance of the attitude sensor when it is static, a tea canopy surface distance detection algorithm that meets the accuracy requirements can be obtained.

Description

Technical Field

[0001] This invention belongs to the field of digital signal processing for bulk tea harvesting based on shape, and specifically relates to an accuracy verification mechanism and method for a tea canopy surface distance detection algorithm. Background Technology

[0002] Tea is widely recognized as one of the healthiest plant-based beverages of the 21st century. The development of China's tea industry has boosted global tea production. In 2021, China's tea production reached 3.18 million tons, an increase of 248,200 tons compared to 2020, nearly doubling in ten years. The domestic tea market sales exceeded 300 billion yuan, with a consumer base of nearly 500 million people.

[0003] Fresh tea leaf harvesting is a crucial step in bulk tea production. Given the labor shortage, mechanized tea-picking machines are widely used in standard tea gardens to improve harvesting efficiency. However, many domestic tea gardens are located in hilly areas with varying management standards. The unevenness of the tea canopy and the spatial and temporal variations in tea canopy height make commercial mechanized tea-picking machines unable to adapt to these variations, hindering their widespread adoption. To address this, some automated contour-following tea-picking machines have been researched, using sensors to detect changes in tea canopy height and the unevenness of the tea garden's furrows. For example, invention patent CN113039936A proposes using ultrasonic waves to measure the distance between the cutter and the tea canopy surface. However, ultrasonic sensors, being point-to-point ranging sensors, are easily affected by the gaps between tea leaves, resulting in low ranging stability. To address this, outdoor lidar is used to measure the distance to the tea canopy surface, obtaining distances from multiple sample points. However, due to the limitations of outdoor lidar ranging principles, its accuracy is insufficient. Therefore, a specific algorithm is employed to improve the lidar ranging accuracy.

[0004] In real-world tea garden environments, it is difficult to verify the effectiveness of the improved accuracy of tea canopy surface distance detection algorithms using higher-precision sensors. Therefore, there is an urgent need for an accuracy verification mechanism and related evaluation methods for tea canopy surface distance detection algorithms. Summary of the Invention

[0005] This invention addresses the problem of the inability to verify the accuracy of the tea canopy surface distance detection algorithm during the automatic shape-following harvesting of fresh tea leaves, and proposes an accuracy verification mechanism and method for the tea canopy surface distance detection algorithm.

[0006] This invention discloses an accuracy verification mechanism for a tea canopy surface distance detection algorithm, comprising a cutter and a simulated tea canopy surface base frame located below the cutter. Connecting rods are hinged to both sides of the cutter, and these connecting rods are respectively hinged to two upper sliders. The two upper sliders and two lead screws form helical pairs, and respectively form sliding pairs in the vertical direction with both sides of the frame. The two lead screws are driven by two motors. A pull rope sensor is fixed to the side of the cutter. The pull rope sensor's pull rope is connected to a pulley frame, and a pulley is hinged to the pulley frame. A simulated tea canopy surface groove on the simulated tea canopy surface base frame and the pulley form a slot-pin pair via pins. Connecting rods are hinged to both ends of the simulated tea canopy surface groove on the simulated tea canopy surface base frame, and these connecting rods are respectively hinged to two lower sliders. The two lower sliders form sliding pairs in the vertical direction with both sides of the frame, and are respectively fixed to both sides of the frame by bolts. Attitude sensors are provided on both the cutter and the simulated tea canopy surface base frame. A two-dimensional lidar is provided on the cutter.

[0007] The verification method for the accuracy verification mechanism of the tea canopy surface distance detection algorithm is as follows:

[0008] Step 1: Place the simulated tea canopy surface on the simulated tea canopy base frame and adjust the horizontal tilt angle of the simulated tea canopy base frame to the set value; the two motors rotate asynchronously and non-periodicly, so that the relative position between the cutter and the simulated tea canopy surface changes in real time; the controller obtains the real-time measured values ​​of the vertical acceleration 'a' of the cutter and the horizontal tilt angle 'α' of the cutter based on the detection signal of the attitude sensor on the cutter, and obtains the real-time measured value of the distance between the cutter and the simulated tea canopy base frame based on the detection signal of the pull rope sensor.

[0009] Step 2: The two-dimensional lidar scans the simulated tea canopy surface to obtain the point cloud coordinates of the ROI region near points C and D on the simulated tea canopy surface, and calculates the vertical distance between the cutting blade side endpoints A and B and the simulated tea canopy surface.

[0010] Step 3: Optimize the vertical distance between the two sides of the cutting blade and the simulated tea canopy surface using the tea canopy surface distance detection algorithm with the accuracy to be verified.

[0011] Step 4: Let l be the distance between the cutter and the simulated tea canopy base frame, obtained from the detection signal of the rope sensor. MN Let l0 be the distance from the top of the simulated tea canopy to the base of the simulated tea canopy. From geometric relationships, the distance between the connection point M between the rope sensor and the cutter, and the projection point F of point M onto the simulated tea canopy in the vertical direction, is:

[0012] l MF =(l MN -l0) / cosβ

[0013] Wherein, β is the horizontal tilt angle of the simulated tea canopy base frame obtained from the detection signal of the attitude sensor on the simulated tea canopy base frame.

[0014] If the pull rope sensor is installed at end point A of the cutter, then the calculated l MF Let l be the distance between endpoint A and point C. AC If the pull rope sensor is installed at end point B of the cutter, then the calculated l MF Let l be the distance between endpoint B and point D. BD .

[0015] Step 5: Take the l obtained in Step 4 MF The real-time calculated values ​​and the real-time optimal estimated values ​​obtained in step three are stored. After a set experimental time, the accuracy of the tea canopy surface distance detection algorithm to be verified is evaluated. The specific steps for the accuracy evaluation are as follows:

[0016] ① Extract N groups of l MF The values ​​and the optimal estimated values ​​obtained in step three, each group containing M values. MF The value and the optimal estimated value obtained in step three; wherein, depending on whether the rope sensor is installed at end A or end B of the cutter, the optimal estimated value for the corresponding side of the cutter obtained in step three is selected.

[0017] ②For each set of experimental data, each l MF The value is denoted as S j Let j = 1, 2, 3, ..., M, and the optimal estimates for each side corresponding to the cutter be denoted as... j = 1, 2, 3, ..., M, calculate l for each set of experimental data. MF The mean error between the value and the optimal estimate of each cutter side Error variance

[0018] ③The mean error obtained from the i-th set of experimental data is denoted as . Then the mean of the error values ​​obtained from N sets of experimental data mean of error variance

[0019] Step 6: Use E and Var as the accuracy evaluation criteria for the tea canopy surface distance detection algorithm to be verified. If either E or Var fails to meet the threshold requirement, then fine-tune σ. a Repeat steps 3.4, 4, and 5 until both E and Var meet the threshold requirements. Once these parameters are met, the tea canopy surface distance detection algorithm is considered an optional algorithm.

[0020] Preferably, step two involves the following steps:

[0021] 2.1 Establish a rectangular coordinate system xoy with the center point of the 2D lidar as the origin, where the y-axis coincides with the polar angle 0° of the 2D lidar polar coordinate system and points downwards. The cutter is perpendicular to the y-axis, and the x-axis is parallel to the cutter and rotates 90° counterclockwise from the y-axis. Establish a static coordinate system with the center point of the 2D lidar as the origin, the horizontal axis rotating 90° counterclockwise from the y' axis as x', and the vertical downward axis as y'. The coordinate system xoy differs from the coordinate system x'oy' by the horizontal tilt angle α of the cutter. Let the cutter width AB = l, and the projections of the two endpoints A and B of the cutter onto the simulated tea shed surface along the vertical axis be points C and D, respectively. The y-axis intersects the cutter at point E, and let oE = h. The 2D lidar scans the simulated tea shed surface to obtain the point cloud of the ROI region near points C and D on the simulated tea shed surface. The polar coordinates (ρ, θ) of the points in the ROI region point cloud near points C and D in the polar coordinate system are converted into coordinates in the coordinate system x'oy', i.e.:

[0022]

[0023] 2.2 In the coordinate system xoy, the coordinates of point A are... The coordinates of point B are

[0024] In coordinate system x'oy', the coordinates of point A are:

[0025]

[0026] In the x'oy' coordinate system, the coordinates of point B are:

[0027]

[0028] 2.3 In the x'oy' coordinate system, point A and point C have the same x' axis coordinates, that is... Let the absolute value of the difference between the x'-axis coordinate of a point in the point cloud of the ROI region near point C and the x'-axis coordinate of point C in the x'oy' coordinate system be:

[0029] δ=|x' C -x'| (4)

[0030] Traverse the point cloud data of the ROI region near point C, and take the point corresponding to the minimum δ as point C; similarly, take the point corresponding to the point in the ROI region near point D where the absolute value of the difference between the x'-axis coordinate of the point and the x'-axis coordinate of point D is the minimum, where the x'-axis coordinate of point D is equal to the x'-axis coordinate of point B, i.e.

[0031] 2.4 Take the coordinates of points in the neighborhood of the midpoint C in the polar coordinate system with a polar angle range of 2Δφ and convert them to the coordinates of the midpoints of the coordinate system x'oy' (x1', y1'), (x'2, y'2), (x'3, y'3), ..., (x' N ,y' N Given the point cloud coordinates of the ROI region within the neighborhood of point C, the mean y'-axis coordinates of all points in the ROI region within the neighborhood of point C are:

[0032]

[0033] Where N is the number of points in the point cloud of the ROI region within the neighborhood of point C.

[0034] Similarly, by taking the coordinates of points within the neighborhood of point D in the polar coordinate system with a polar angle range of 2Δφ and converting them to the coordinates of the midpoint of the coordinate system x'oy', we obtain the point cloud coordinates of the ROI region within the neighborhood of point D. Furthermore, we obtain the mean y'-axis coordinate y' of each point in the ROI region point cloud within the neighborhood of point D. D(ROI) .

[0035] 2.5 From equations (2) and (3), the y' coordinates of points A and B in the coordinate system x'oy' are respectively:

[0036]

[0037] The result calculated by equation (7) is used as the distance measurement value between endpoint A and point C, and between endpoint B and point D of the cutter:

[0038]

[0039] Preferably, step three involves the following steps:

[0040] 3.1 The kinematic equations are constructed as follows:

[0041]

[0042] Where T0 is the sampling time interval of the attitude sensor; x k Let be the vertical distance between the endpoint A or endpoint B of the cutter at time k after iteration and the simulated tea canopy surface; For x k The derivative; a k Let x be the vertical acceleration of the cutter calculated from the attitude sensor signal at time k. k The initial value x0 is taken as 0. The distance in the vertical direction between the cutting end point A or end point B and the simulated tea canopy surface is calculated by equation (7). initial value a k The initial value is 0.

[0043] 3.2 Establish the state-space model of the kinematic equations and represent it using discrete state-space representation:

[0044]

[0045] Wherein, the system state variable of the state-space model at the k-th sampling time is The system state variables of the state-space model at the (k+1)th sampling time are: The noise matrix of the state-space model during its iteration at the k-th sampling time is W. k =[w 1k w 2k ] T The vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface is expressed as a process noise w in the state-space model during iteration. 1k =0.5T0 2 w k The rate of change of the vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface is iterated in the state-space model, with process noise w. 2k =T0w k w k The vertical acceleration measurement noise in the attitude sensor detection signal is randomly selected within the accuracy range of the attitude sensor's acceleration detection and follows a normal distribution; Y k Y is the output state variable of the state-space model at the k-th sampling time. k =x k ;u k Let u be the control variable of the state-space model at the k-th sampling time. k =a' k ,a' k The system matrix represents the true value of the vertical acceleration of the cutter calculated from the attitude sensor signal at time k, after removing measurement noise. Input matrix B = [0.5T0] 2 T0] T The output matrix C =

[10] .

[0046] In equation (9), B*u k +W k =B*(u k +w a )=B*a k Equation (9) simplifies to:

[0047]

[0048] 3.3 with Z k Let represent the measured distance in the vertical direction between endpoint A or endpoint B of the cutter and the simulated tea canopy surface at the k-th sampling time.

[0049] Z k =HX k +v k (10)

[0050] Where H =

[10] is the measurement matrix; v k The measurement noise of the 2D lidar at the k-th sampling time is expressed in mm and v. k The values ​​are randomly selected within the ranging accuracy range of the two-dimensional lidar, and the values ​​satisfy a normal distribution.

[0051] 3.4 The two-dimensional lidar measurement data and the vertical acceleration measurement data of the attitude sensor on the cutter are fused using the tea canopy surface distance detection algorithm whose accuracy needs to be verified. The specific process is as follows:

[0052] ① Let the prior estimation matrix of the system state variables at the k-th sampling time be:

[0053]

[0054] in, For the k-th sampling time x k The prior estimate, For the k-th sampling time The prior estimate, Let be the prior estimate matrix of the system state variables at the (k-1)th sampling time. For the (k-1)th sampling time x k-1 The posterior estimate, For the (k-1)th sampling time The posterior estimate when k=1,

[0055] ② Calculate the prior estimation matrix at the k-th sampling time. The covariance matrix of the two variables:

[0056]

[0057] Among them, P k-1 Let be the covariance matrix of the two variables in the posterior estimation matrix at the (k-1)th sampling time; let k=1, then the covariance matrix of the two variables in the posterior estimation matrix is... A T The transpose of system matrix A; process noise covariance matrix. Q H The process noise variance, Q, is a prior estimate of the vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface. vThe process noise variance, Q, is a priori estimated for the change in vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface. H =(0.5T0) 2 ) 2 σ a Q v =T0 2 σ a , parameter σ a The initial value is taken as the vertical acceleration measurement noise variance in the signal detected by the attitude sensor when it is static.

[0058] ③ Calculate the gain at the k-th sampling time:

[0059]

[0060] Among them, H T R is the transpose of the measurement matrix H; R is the variance of the measurement noise of the two-dimensional lidar, which is equal to the variance of the normal distribution that the measurement noise of the two-dimensional lidar conforms to.

[0061] ④ Calculate the posterior estimation matrix of the system state variables at the k-th sampling time:

[0062]

[0063] in,

[0064] ⑤ Update the covariance matrix of the two variables in the posterior estimation matrix at the k-th sampling time:

[0065]

[0066] in It is a unit vector.

[0067] The beneficial effects of this invention are as follows:

[0068] (1) In view of the lack of accuracy and real-time performance of 2D-LiDAR in the application of bulk tea picking, the present invention proposes an accuracy verification mechanism and method based on multi-sensor fusion, which can accurately simulate the height and posture of the tea canopy and the cutting knife during actual operation, and accurately verify the accuracy of the canopy distance detection algorithm.

[0069] (2) This invention obtains the point cloud coordinates of the ROI region near points C and D on the simulated tea shed surface corresponding to the two endpoints of the cutter using 2D-LiDAR. Two points are selected from the ROI region point cloud coordinates near points C and D, with the condition that the absolute value of the difference between the horizontal axis coordinates of points C and D is minimized. Then, the difference between the average vertical axis coordinates of the points in the neighborhood of point C and the average vertical axis coordinates of the points in the neighborhood of point D and the vertical axis coordinates of the corresponding endpoints on the cutter is calculated and used as the distance values ​​between the two endpoints of the cutter and the simulated tea shed surface measured by 2D-LiDAR. Finally, the distance values ​​between the cutter endpoints measured by 2D-LiDAR are used as the distance values ​​between the two endpoints of the cutter and the simulated tea shed surface. The initial distance value is the distance between the cutter tip and the simulated tea canopy surface. A multi-sensor tea canopy surface distance detection algorithm (such as Kalman data filtering fusion algorithm, particle swarm algorithm, etc.) is used to optimally estimate the distance between the cutter tip and the simulated tea canopy surface. Finally, the error between the optimal estimated distance between the cutter tip and the simulated tea canopy surface and the true distance measured directly by a high-precision sensor is calculated. The accuracy of the tea canopy surface distance detection algorithm is evaluated from the perspective of the mean error and the variance of the error. Furthermore, by fine-tuning the parameter of the vertical acceleration measurement noise variance of the attitude sensor when it is static, a tea canopy surface distance detection algorithm that meets the accuracy requirements can be obtained. Attached Figure Description

[0070] Figure 1 This is a schematic diagram of the accuracy verification mechanism of the present invention;

[0071] Figure 2 This is a schematic diagram of the assembly of the rope sensor, pulley, and simulated tea canopy base frame in this invention;

[0072] Figure 3 This is a simplified kinematic diagram of the accuracy verification mechanism of the present invention;

[0073] Figure 4 This is a comparison curve of the estimated values ​​from the Kalman fusion filtering algorithm, the measured values ​​from the two-dimensional lidar, and the true values ​​from the rope sensor. Detailed Implementation

[0074] The invention will now be further described with reference to the accompanying drawings.

[0075] like Figure 1 and Figure 2As shown, a precision verification mechanism for a tea canopy surface distance detection algorithm includes a cutter 001 and a simulated tea canopy surface base 002 located below the cutter 001. Connecting rods 003 are hinged to both sides of the cutter 001. These connecting rods 003 are hinged to two upper sliders 011, which in turn form helical pairs with two lead screws 010 and sliding pairs along the vertical direction with both sides of the frame 009. The two lead screws 010 are driven by two motors 014. The two motors 014 work together to adjust the position and horizontal tilt angle (angle with the horizontal plane) of the cutter 001. A pull rope sensor 006 is fixed to the side of the cutter 001; the pull rope 012 of the pull rope sensor 006 is connected to a pulley frame, and a pulley 007 is hinged on the pulley frame; the simulated tea canopy base frame 002 has a simulated tea canopy slide groove 013, which forms a groove-pin pair with the pulley 007 through a pin; connecting rods 003 are hinged at both ends of the simulated tea canopy base frame 002 at the simulated tea canopy slide groove 013, and these connecting rods 003 are respectively hinged to two lower sliders 004; the two lower sliders 004 and the two sides of the frame 009 respectively form sliding pairs in the vertical direction, and are fixed to the two sides of the frame 009 respectively by bolts. Both the cutter 001 and the simulated tea canopy base frame 002 are equipped with attitude sensors 005 to obtain the horizontal tilt angle of the simulated tea canopy base frame 002 and the attitude data of the cutter 001 (including the vertical acceleration a of the cutter 001 and the horizontal tilt angle α of the cutter 001). Adjusting the positions of the two sliding blocks 004 changes the posture of the simulated tea canopy base frame 002. Driven by two motors 014, the cutter 001 adjusts its posture and height to simulate the relative position of the actual tea canopy surface and the cutter. Because the pulley 007 follows the movement of the pull rope sensor, and under the action of the spring in the pull rope sensor, the pull rope 012 remains perpendicular to the simulated tea canopy surface groove 013 and the simulated tea canopy base frame 002. Based on the distance measured by the pull rope sensor 006 and the horizontal tilt angle information of the simulated tea canopy base frame 002, the true value of the vertical distance between the cutter 001 and the simulated tea canopy base frame 002 can be obtained. The cutter 001 is also equipped with a two-dimensional lidar (2D-LiDAR) 008 to acquire point cloud information of the simulated tea canopy surface on the simulated tea canopy base frame 002. The motors 014 are controlled by a controller, and the signal outputs of the pull rope sensor 006, posture sensor 005, and 2D lidar are all connected to the controller.

[0076] The verification method for the accuracy verification mechanism of the tea canopy surface distance detection algorithm is as follows:

[0077] Step 1: Place the simulated tea canopy surface on the simulated tea canopy surface base 002. Adjust the horizontal tilt angle of the simulated tea canopy surface base 002 until the horizontal tilt angle of the simulated tea canopy surface base 002, as detected by the controller based on the signal detected by the attitude sensor 005 on the simulated tea canopy surface base 002, reaches the set value β. β can be set according to actual working conditions. The two motors 014 rotate asynchronously and non-periodically, thereby adjusting the position of the cutter 001 in real time, causing the relative position of the cutter 001 and the simulated tea canopy surface to change in real time. The asynchronous and non-periodic rotation of the two motors 014 can better simulate the random change process of the distance to the tea canopy surface, thus simulating the random interference process caused by the unevenness of the furrows during the actual tea harvester's movement in the furrows. The controller obtains the real-time measured values ​​of the vertical acceleration 'a' and the horizontal tilt angle 'α' of the cutter 001 based on the detection signal from the attitude sensor 005 on the cutter 001, and obtains the real-time measured value of the distance between the cutter and the simulated tea canopy surface base 002 based on the detection signal from the rope sensor.

[0078] Step 2: The 2D LiDAR 008 scans the simulated tea canopy surface to obtain the point cloud coordinates of the ROI (Region of Interest) near points C and D on the simulated tea canopy surface. The vertical distance between the cutter's side endpoints A and B and the simulated tea canopy surface is calculated. Compared to existing single-point ranging methods using ultrasonic sensors, this eliminates the influence of the tea canopy blade gaps on the measurement. The specific steps are as follows:

[0079] 2.1 As Figure 3 As shown, a rectangular coordinate system xoy is established with the center point of the two-dimensional lidar as the origin. The y-axis coincides with the polar angle 0° of the two-dimensional lidar's polar coordinate system and points downwards. The cutter 001 is perpendicular to the y-axis, and the x-axis is parallel to the cutter 001 and rotates 90° counterclockwise from the y-axis. A static coordinate system is established with the center point of the two-dimensional lidar as the origin, the horizontal axis rotating 90° counterclockwise from the y' axis as x', and the vertical downward axis as y'. Let the coordinates of any point in the two-dimensional lidar polar coordinate system be (ρ, θ). Then the coordinates of this point in the rectangular coordinate system xoy are (ρsinθ, ρ...). cosθ); the difference between coordinate system xoy and coordinate system x'oy' is the horizontal tilt angle α of the cutter; let the cutter width AB = l, and the projections of the two endpoints A and B of the cutter along the vertical axis onto the simulated tea shed surface are points C and D, respectively; the y-axis intersects the cutter at point E, and let OE = h; the two-dimensional lidar 008 scans the simulated tea shed surface to obtain the point cloud of the ROI region (region of interest) near points C and D on the simulated tea shed surface, and converts the polar coordinates (ρ, θ) of the points in the ROI region point cloud near points C and D in the polar coordinate system into coordinates in coordinate system x'oy', that is:

[0080]

[0081] 2.2 In the coordinate system xoy, the coordinates of point A are... The coordinates of point B are

[0082] In coordinate system x'oy', the coordinates of point A are:

[0083]

[0084] In the x'oy' coordinate system, the coordinates of point B are:

[0085]

[0086] 2.3 In the x'oy' coordinate system, point A and point C have the same x' axis coordinates, that is... The point cloud of the simulated tea awning surface acquired by the 2D lidar is discrete data. Let the absolute value of the difference between the x'-axis coordinate of a point in the ROI region near point C and the x'-axis coordinate of point C in the x'oy' coordinate system be:

[0087] δ=|x' C -x'| (4)

[0088] Traverse the point cloud data of the ROI region near point C, and take the point corresponding to the minimum δ as point C; similarly, take the point corresponding to the point in the ROI region near point D where the absolute value of the difference between the x'-axis coordinate of the point and the x'-axis coordinate of point D is the minimum, where the x'-axis coordinate of point D is equal to the x'-axis coordinate of point B, i.e.

[0089] 2.4 Take the coordinates of points in the neighborhood of the midpoint C in the polar coordinate system with a polar angle range of 2Δφ and convert them to the coordinates of the midpoints of the coordinate system x'oy' (x1', y1'), (x'2, y'2), (x'3, y'3), ..., (x' N ,y' N Given the point cloud coordinates of the ROI region within the neighborhood of point C, the mean y'-axis coordinates of all points in the ROI region within the neighborhood of point C are:

[0090]

[0091] Where N is the number of points in the point cloud of the ROI region within the neighborhood of point C.

[0092] Similarly, by taking the coordinates of points within the neighborhood of point D in the polar coordinate system with a polar angle range of 2Δφ and converting them to the coordinates of the midpoint of the coordinate system x'oy', we obtain the point cloud coordinates of the ROI region within the neighborhood of point D. Furthermore, we obtain the mean y'-axis coordinate y' of each point in the ROI region point cloud within the neighborhood of point D. D(ROI) .

[0093] 2.5 From equations (2) and (3), the y' coordinates of points A and B in the coordinate system x'oy' are respectively:

[0094]

[0095] The result calculated by equation (7) is used as the distance measurement value between endpoint A and point C, and between endpoint B and point D of the cutter:

[0096]

[0097] Step 3: Utilize the tea canopy surface distance detection algorithm (with accuracy to be verified) to optimally estimate the vertical distance between the two sides of the cutting blade and the simulated tea canopy surface (this embodiment verifies the accuracy of the Kalman fusion filtering algorithm), as detailed below:

[0098] 3.1 The kinematic equations are constructed as follows:

[0099]

[0100] Where T0 is the sampling time interval of the attitude sensor, in seconds; x k The distance in the vertical direction between the endpoint A or endpoint B of the cutter at time k after iteration and the simulated tea canopy surface, in mm; For x k The derivative of , in mm / s; a k Let k be the vertical acceleration of the cutter calculated from the signal detected by the attitude sensor at time k, in mm / s². 2 Among them, x k The initial value x0 is taken as 0. The distance in the vertical direction between the cutting end point A or end point B and the simulated tea canopy surface is calculated by equation (7). initial value a k The initial value is 0.

[0101] 3.2 Establish the state-space model of the kinematic equations and represent it using discrete state-space representation:

[0102]

[0103] Wherein, the system state variable of the state-space model at the k-th sampling time is The system state variables of the state-space model at the (k+1)th sampling time are: The noise matrix of the state-space model during its iteration at the k-th sampling time is W. k =[w 1k w 2k ] T The vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface is expressed as a process noise w in the state-space model during iteration.1k =0.5T0 2 w k The rate of change of the vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface is iterated in the state-space model, with process noise w. 2k =T0w k w k The vertical acceleration measurement noise in the attitude sensor detection signal (mainly caused by circuit and mechanical noise), in mm / s². 2 , in [-1.5*10 -5 g, 1.5*10 -5 The values ​​are randomly selected within the range of g, and follow a normal distribution, where g is the acceleration due to gravity; Y k Y is the output state variable of the state-space model at the k-th sampling time. k =x k ;u k Let u be the control variable of the state-space model at the k-th sampling time. k =a' k ,a' k The system matrix represents the true value of the vertical acceleration of the cutter calculated from the attitude sensor signal at time k, after removing measurement noise. Input matrix B = [0.5T0] 2 T0] T The output matrix is ​​C = [1 0].

[0104] In equation (9), B*u k +W k =B*(u k +w a )=B*a k Equation (9) simplifies to:

[0105]

[0106] 3.3 with Z k Let represent the measured distance in the vertical direction between endpoint A or endpoint B of the cutter and the simulated tea canopy surface at the k-th sampling time.

[0107] Z k =HX k +v k (10)

[0108] Where H = [1 0] is the measurement matrix; v k The measurement noise of the 2D lidar at the k-th sampling time is expressed in mm and v. k The ranging accuracy range of the two-dimensional lidar is randomly selected (in this embodiment, it is taken as -30 to 30), and the selected values ​​satisfy a normal distribution.

[0109] 3.4 The distance detection algorithm for the tea canopy surface with the accuracy to be verified fuses the two-dimensional lidar measurement data (the vertical distance measurement between the end point A or end point B of the cutter and the simulated tea canopy surface) with the vertical acceleration measurement data from the attitude sensor on the cutter 001. The specific process is as follows:

[0110] ① Let the prior estimation matrix of the system state variables at the k-th sampling time be:

[0111]

[0112] in, For the k-th sampling time x k The prior estimate, For the k-th sampling time The prior estimate, Let be the prior estimate matrix of the system state variables at the (k-1)th sampling time. For the (k-1)th sampling time x k-1 The posterior estimate, For the (k-1)th sampling time The posterior estimate when k=1, visible, Can be used It is obtained through recursion using the state equation.

[0113] ② Calculate the prior estimation matrix at the k-th sampling time. The covariance matrix of the two variables:

[0114]

[0115] Among them, P k-1 Let be the covariance matrix of the two variables in the posterior estimation matrix at the (k-1)th sampling time; let k=1, then the covariance matrix of the two variables in the posterior estimation matrix is... A T The transpose of system matrix A; process noise covariance matrix. Q H The process noise variance, Q, is a prior estimate of the vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface. v The process noise variance, Q, is a priori estimated for the change in vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface. H and Q v All of these are important parameters that determine the performance of the tea canopy surface distance detection algorithm, which is to be verified. H =(0.5T0) 2 )2 σ a Q v =T0 2 σ a , parameter σ a The initial value is determined by the characteristics of the attitude sensor itself, but due to the influence of circuit and mechanical noise, it often needs to be adjusted according to the actual working conditions. The initial value is taken as the variance of the vertical acceleration measurement noise in the sensor's static detection signal, while σ... a The coefficients mentioned above are directly determined by the state-space model.

[0116] ③ Calculate the gain at the k-th sampling time:

[0117]

[0118] Among them, H T R is the transpose of the measurement matrix H; R is the variance of the measurement noise of the two-dimensional lidar, which is equal to the variance of the normal distribution that the measurement noise of the two-dimensional lidar conforms to.

[0119] ④ Calculate the posterior estimation matrix of the system state variables at the k-th sampling time:

[0120]

[0121] in,

[0122] ⑤ Update the covariance matrix of the two variables in the posterior estimation matrix at the k-th sampling time:

[0123]

[0124] in It is a unit vector.

[0125] Step 4: Let l be the distance between the cutter and the simulated tea canopy base frame 002, obtained from the detection signal of the rope sensor. MN Let the distance from the top of the simulated tea canopy surface (since the curvature of the part of the tea canopy surface in contact with the cutter changes very little during actual operation, it can be approximated as a plane; therefore, the simulated tea canopy surface directly adopts a shape with a flat top) to the base frame 002 of the simulated tea canopy surface be l0 (pre-measured). Figure 3 As shown, based on geometric relationships, the distance between the connection point M between the rope sensor and the cutter and the projection point F of point M along the vertical direction onto the simulated tea canopy surface is:

[0126] l MF =(l MN -l0) / cosβ (16)

[0127] Wherein, β is the horizontal tilt angle of the simulated tea canopy base frame 002 obtained from the detection signal of the attitude sensor 005 on the simulated tea canopy base frame 002, in rad. Figure 3 In the diagram, point N is the connection point between the pull rope sensor and the simulated tea canopy base frame 002, and points P and Q are the hinge points at both ends of the simulated tea canopy base frame 002 and the two connecting rods 003.

[0128] If the pull rope sensor is installed at end point A of the cutter, then l is calculated according to equation (16). MF Let l be the distance between endpoint A and point C. AC If the rope sensor is installed at end point B of the cutter, then l is calculated according to equation (16). MF Let l be the distance between endpoint B and point D. BD Because the distance measurement error of the pull-string sensor and the tilt angle measurement error of the attitude sensor are both very small, l MF It can be used as the true value of the contour distance.

[0129] Step 5: Take the l obtained in Step 4 MF The real-time calculated values ​​and the real-time optimal estimated values ​​obtained in step three are stored. After a set experimental time, the accuracy of the tea canopy surface distance detection algorithm to be verified is evaluated. The specific steps for the accuracy evaluation are as follows:

[0130] ① Extract N groups of l MF The values ​​and the optimal estimated values ​​obtained in step three, each group containing M values. MF The value and the optimal estimated value obtained in step three; wherein, depending on whether the rope sensor is installed at end A or end B of the cutter, the optimal estimated value for the corresponding side of the cutter obtained in step three is selected.

[0131] ②For each set of experimental data, each l MF The value is denoted as S j Let j = 1, 2, 3, ..., M, and the optimal estimates for each side corresponding to the cutter be denoted as... j = 1, 2, 3, ..., M, calculate l for each set of experimental data. MF The mean error between the value and the optimal estimate of each cutter side Error variance

[0132] ③The mean error obtained from the i-th set of experimental data is denoted as . Then the mean of the error values ​​obtained from N sets of experimental data mean of error variance

[0133] Step 6: Use E and Var as the accuracy evaluation criteria for the tea canopy surface distance detection algorithm to be verified. If either E or Var fails to meet the threshold requirement, then fine-tune σ.a Repeat steps 3.4, 4, and 5 until both E and Var meet the threshold requirements. Once these parameters are met, the tea canopy surface distance detection algorithm is considered an optional algorithm.

[0134] Figure 4 The diagram illustrates the comparison curves between the Kalman fusion filter algorithm estimation value and the two-dimensional lidar measurement value on the cutting side and the simulated tea shed surface in the vertical direction, and the true value measured by the rope sensor on the cutting side. The curves show that the mean error between the two-dimensional lidar measurement value and the true value is 36.53 mm, with a standard deviation of 23.21 mm; the mean error of the Kalman fusion filter algorithm estimation value is 8.56 mm, with a standard deviation of 6.31 mm. Therefore, the accuracy of the Kalman fusion filter algorithm estimation value is better than the accuracy of the direct measurement by the two-dimensional lidar.

[0135] This invention enables convenient and rapid verification of the tea canopy distance detection algorithm, has a wide range of applications, and is simple to operate. By combining the verified tea canopy distance detection algorithm of this invention with multiple sensors, the distance between the cutter and the tea canopy surface can be more accurately estimated, which helps improve measurement accuracy and real-time performance.

Claims

1. A precision verification mechanism for a tea canopy surface distance detection algorithm, characterized in that: The device includes a cutter and a simulated tea canopy base frame located below the cutter. Connecting rods are hinged to both sides of the cutter, and these connecting rods are hinged to two upper sliders. The two upper sliders and two lead screws form helical pairs, and together with the sides of the frame, they form sliding pairs in the vertical direction. The two lead screws are driven by two motors. A pull rope sensor is fixed to the side of the cutter. The pull rope sensor's pull rope is connected to a pulley frame, and pulleys are hinged to the pulley frame. The simulated tea canopy base frame has a simulated tea canopy groove, and the pulleys form a groove-pin pair via pins. Connecting rods are hinged to both ends of the simulated tea canopy groove on the simulated tea canopy base frame, and these connecting rods are hinged to two lower sliders. The two lower sliders form sliding pairs in the vertical direction with the sides of the frame, and are fixed to the sides of the frame by bolts. Attitude sensors are installed on both the cutter and the simulated tea canopy base frame. A two-dimensional lidar is installed on the cutter.

2. The method for verifying the accuracy of the tea canopy surface distance detection algorithm using the accuracy verification mechanism according to claim 1, characterized in that: The specific steps are as follows: Step 1: Place the simulated tea canopy surface on the simulated tea canopy surface base and adjust the horizontal tilt angle of the base to the set value; the two motors rotate asynchronously and non-periodicly, causing the relative position of the cutter and the simulated tea canopy surface to change in real time; the controller obtains the vertical acceleration of the cutter based on the detection signal from the attitude sensor on the cutter. and the horizontal tilt angle of the cutter Real-time measurement values, and obtain the real-time measurement value of the distance between the cutter and the simulated tea canopy base frame based on the detection signal of the pull rope sensor; Step 2: The two-dimensional lidar scans the simulated tea shed surface to obtain the point cloud coordinates of the ROI region near points C and D on the simulated tea shed surface, and calculates the vertical distance between the cutting blade side endpoints A and B and the simulated tea shed surface. Step 3: Optimize the vertical distance between the two sides of the cutter and the simulated tea canopy surface using the tea canopy surface distance detection algorithm with the accuracy to be verified; Step 4: Let l be the distance between the cutter and the simulated tea canopy base frame, obtained from the detection signal of the rope sensor. MN Let the distance from the top of the simulated tea canopy to the base of the simulated tea canopy be... Based on geometric relationships, the distance between the connection point M between the rope sensor and the cutter and the projection point F of point M onto the simulated tea canopy surface in the vertical direction is: Wherein, β is the horizontal tilt angle of the simulated tea canopy base frame obtained from the detection signal of the attitude sensor on the simulated tea canopy base frame; If the pull-cord sensor is installed at end point A of the cutter, then the calculated... Let l be the distance between endpoint A and point C. AC If the rope sensor is installed at end point B of the cutter, then the calculated... Let l be the distance between endpoint B and point D. BD ; Step 5: Obtain the results from Step 4 The real-time calculated values ​​and the real-time optimal estimated values ​​obtained in step three are stored. After a set experimental time, the accuracy of the tea canopy surface distance detection algorithm to be verified is evaluated. The specific steps for the accuracy evaluation are as follows: ① Extraction Group The values ​​and the optimal estimated values ​​obtained in step three, for each group indivual The value and the optimal estimated value obtained in step three; wherein, depending on whether the rope sensor is installed at end A or end B of the cutter, the optimal estimated value of the corresponding side of the cutter obtained in step three is selected; ②For each set of experimental data, each The value is denoted as The optimal estimates for each side corresponding to the cutter are denoted as follows: Calculate each of the following in each set of experimental data: The mean error between the value and the optimal estimate of each cutter side Error variance ; ③According to the first The mean error obtained from the group of experimental data is denoted as According to The mean of the error values ​​obtained from the group of experimental data Mean of error variance ; Step Six, with and As an accuracy evaluation standard for the tea canopy surface distance detection algorithm to be verified, if and If any one of them does not meet the threshold requirement, then fine-tuning is performed. Repeat steps 3.4, 4, and 5 until the value is reached. and After all threshold requirements are met, this parameter The tea awning surface distance detection algorithm is considered an optional algorithm; The specific steps for step two are as follows: 2.1 Establish a rectangular coordinate system xoy with the center point of the two-dimensional lidar as the origin, where the y-axis coincides with the polar angle 0° of the two-dimensional lidar's polar coordinate system and points downwards, the cutter is perpendicular to the y-axis, and the x-axis is parallel to the cutter and rotates 90° counterclockwise from the y-axis; with the center point of the two-dimensional lidar as the origin, The horizontal axis in the direction of the axis rotating counterclockwise 90° is Vertically downwards Establish a static coordinate system along the axes; the coordinate system xoy is parallel to the coordinate system... Horizontal tilt angle of phase cutter Let the width of the cutter AB = l, and the projections of the two endpoints A and B of the cutter onto the simulated tea shed surface along the vertical axis be points C and D, respectively; the y-axis intersects the cutter at point E, and let oE = h; a two-dimensional lidar scans the simulated tea shed surface to obtain the point cloud of the ROI region near points C and D on the simulated tea shed surface, and the polar coordinates of the points in the ROI region point cloud near points C and D are then converted to polar coordinates. Transformed into coordinate system The coordinates in the middle, that is: = * （1） 2.2 In the coordinate system xoy, the coordinates of point A are... The coordinates of point B are ; In coordinate system In the diagram, the coordinates of point A are: = * （2） exist In the coordinate system, the coordinates of point B are: = * （3） 2.3 In In the coordinate system, the coordinates of point A and point C are... The axis coordinates are the same, that is ; Record the points in the point cloud of the ROI region near point C. In the coordinate system The coordinates of the axes and the coordinates of point C The absolute value of the difference between the axis coordinates is: （4） Traverse the point cloud data of the ROI region near point C, and The point corresponding to the minimum value is designated as point C; similarly, the points in the point cloud of the ROI region near point D are designated as points... The coordinates of the axes and the coordinates of point D The point corresponding to the minimum absolute value of the difference between the axial coordinates is designated as point D, where point D's... The coordinates of point B are equal to the coordinates of point B. Axis coordinates, i.e. ; 2.4 Take the range of polar angles near the midpoint C of the polar coordinate system as follows: Find the coordinates of points in the neighborhood and convert them to a coordinate system. Midpoint coordinates , , , ..., If the point cloud coordinates of the ROI region within the neighborhood of point C are obtained, then the points within the ROI region point cloud of point C... The mean of the axis coordinates is: （5） Where N is the number of points in the point cloud of the ROI region within the neighborhood of point C; Similarly, the range of polar angles near the midpoint D of the polar coordinate system is taken as... Find the coordinates of points in the neighborhood and convert them to a coordinate system. By using the midpoint coordinates, we obtain the point cloud coordinates of the ROI region within the neighborhood of point D, and then obtain the coordinates of each point within the point cloud of the ROI region within the neighborhood of point D. Mean of axis coordinates ; 2.5 From equations (2) and (3), we can obtain the coordinate system of points A and B. In The axis coordinates are as follows: （6） The result calculated by equation (7) is used as the distance measurement value between endpoint A and point C, and between endpoint B and point D of the cutter: （7） Step 3 is detailed below: 3.1 The kinematic equations are constructed as follows: （8） in, The sampling time interval of the attitude sensor; For iteration The vertical distance between the endpoint A or endpoint B of the cutting tool and the simulated tea awning surface; for The derivative; for The vertical acceleration of the cutter is calculated continuously based on the signals detected by the attitude sensor; among which, initial value Take the distance in the vertical direction between the cutting end point A or B and the simulated tea canopy surface calculated by equation (7) at time 0. initial value =0, The initial value is 0; 3.2 Establish the state-space model of the kinematic equations and represent it using discrete state-space representation: （9） Among them, the state-space model in the th The system state variables at each sampling time are: The state-space model in the first The system state variable at +1 sampling time is The state-space model in the first... The process noise matrix at each sampling time iteration is: The vertical distance between the endpoint A or endpoint B of the cutter and the simulated tea canopy surface is considered as process noise during iteration in the state-space model. The rate of change of the vertical distance between the endpoint A or B of the cutter and the simulated tea canopy surface is iterated in the state-space model as process noise. , The vertical acceleration measurement noise in the attitude sensor detection signal is randomly selected within the accuracy range of the attitude sensor's acceleration detection and satisfies a normal distribution law; For the state-space model in the th The output state variables at each sampling time ; For the state-space model in the th The control quantity at each sampling time = , for The true value of the vertical acceleration of the cutter calculated from the attitude sensor signals at all times, after removing measurement noise; system matrix. Input matrix Output matrix ; In equation (9), = Equation (9) simplifies to: 3.3 with Indicates the first The measured distance in the vertical direction between the endpoint A or endpoint B of the cutter and the simulated tea canopy surface at each sampling time is then... （10） in, For measurement matrix; For two-dimensional lidar in the first Measurement noise at each sampling time, in mm. The values ​​are randomly selected within the ranging accuracy range of the two-dimensional lidar, and the values ​​satisfy a normal distribution. 3.4 The two-dimensional lidar measurement data and the vertical acceleration measurement data from the attitude sensor on the cutter are fused using a tea canopy surface distance detection algorithm with the accuracy to be verified. The specific process is as follows: ①Let the first The prior estimate matrix of the system state variables at each sampling time is: （11） in, , For the first Each sampling time The prior estimate, For the first Each sampling time The prior estimate, For the first The prior estimate matrix of the system state variables at each sampling time. , For the first Each sampling time The posterior estimate, For the first Each sampling time The posterior estimate when k=1, ; ② Calculate the first Prior estimation matrix at each sampling time The covariance matrix of the two variables: （12） in, For the first The covariance matrix of the two variables in the posterior estimation matrix at each sampling time point; when k=1, the covariance matrix of the two variables in the posterior estimation matrix. ; For the system matrix Transpose of; process noise covariance matrix , The process noise variance is a prior estimate of the vertical distance between the endpoint A or endpoint B of the cutter and the simulated tea canopy surface. The process noise variance is estimated a priori for the change in the vertical distance between the endpoint A or endpoint B of the cutter and the simulated tea canopy surface. , ,parameter The initial value is taken as the vertical acceleration measurement noise variance in the signal detected by the attitude sensor when it is static; ③ Calculate the first Gain at each sampling time: （13） in, It is a measurement matrix transpose; It is the variance of the measurement noise of the two-dimensional lidar, and its value is equal to the variance of the normal distribution that the measurement noise of the two-dimensional lidar conforms to. ④ Calculate the first The posterior estimation matrix of the system state variables at each sampling time: （14） in, ; ⑤ Update the covariance matrix of the two variables in the posterior estimation matrix at the k-th sampling time: （15） in It is a unit vector.

Images