Automatic Target Alignment and Pruning System and Pruning Method Based on 3D Point Cloud

Abstract

This invention discloses an automatic target-adjusting pruning system and method based on three-dimensional point clouds, comprising: pruning equipment for pruning target trees; an information acquisition device for acquiring point cloud information of the target tree and its surrounding environment, as well as the pose information of the pruning equipment, and transmitting this information to a data processing unit; a data processing unit for processing the information transmitted from the information acquisition device to obtain the three-dimensional coordinates and pruning angle of the pruning location on the target tree, as well as the position and pose information of the pruning equipment, and transmitting this information to a control unit; and a control unit for controlling the pruning equipment to perform precise pruning based on the information transmitted from the data processing unit. This invention can construct parameters such as tree crown width, tree height, and row spacing based on the detected tree point cloud, calculate the target pruning location, and thus achieve the goal of precise pruning by changing the pruning posture according to the tree crown parameters, reducing the workload during the pruning process and improving pruning efficiency.

Description

Technical Field

[0001] This invention relates to the technical field of agricultural machinery, and in particular to an automatic target-adjusting posture pruning system and pruning method based on three-dimensional point clouds. Background Technology

[0002] In fruit tree planting and management, pruning is indispensable. By rationally removing a portion of the fruit tree's vegetative organs, the distribution, transport, accumulation, and digestion of nutrients and hormones can be regulated; tree shape can be trained, and fruiting areas can be adjusted, thereby reducing the costs of harvesting, pesticide application, and other management processes. Currently, traditional pruning mainly uses manual pruning machines, backpack pruning machines, pneumatic pruning machines, and electric pruning machines. These types of pruning machines require highly skilled pruners and have relatively low pruning efficiency, often relying on manual support to improve efficiency. In recent years, domestic researchers have studied mechanized operations using tractor-mounted reciprocating, rotary, and circular saw-type end-effector pruning tools. This method is suitable for standardized arboretums, but requires high standards for planting planning and terrain conditions, and its level of intelligence needs to be improved. The system of this invention can construct parameters such as crown width and tree height based on detected tree point clouds, calculate the target pruning location, and thus achieve precise pruning by changing the pruning posture according to the tree crown parameters. Summary of the Invention

[0003] The purpose of this invention is to overcome the shortcomings of the prior art and propose an automatic target-adjusting posture pruning system and pruning method based on three-dimensional point clouds. It can construct parameters such as tree crown width, tree height, and row spacing based on the detected tree point clouds, calculate the target pruning position of the tree, thereby achieving the purpose of changing the pruning posture according to the tree crown parameters, pruning accurately, reducing the workload in the pruning process, and improving pruning efficiency.

[0004] To achieve the above objectives, the technical solution provided by this invention is: an automatic target-adjusting posture trimming system based on three-dimensional point clouds, comprising:

[0005] Pruning equipment used to prune target trees;

[0006] The information acquisition device is used to acquire point cloud information of the target tree and its surrounding environment, as well as the pose information of the pruning equipment, and transmit it to the data processing unit.

[0007] The data processing unit is used to process the information transmitted from the information acquisition device, obtain the three-dimensional coordinates and pruning angle of the target tree pruning part, as well as the position and posture information of the pruning equipment, and transmit them to the control unit.

[0008] The control unit controls the trimming equipment to perform precise trimming based on the information transmitted from the data processing unit.

[0009] Furthermore, the pruning equipment includes a tracked chassis and a pruning mechanism mounted on a base on top of the tracked chassis. The tracked chassis is used to support and move the pruning mechanism. The pruning mechanism includes a pruning arm and a cutter head mounted at the end of the pruning arm for cutting trees. The pruning mechanism is driven by a hydraulic cylinder or an electric push rod.

[0010] Furthermore, the trimming robotic arm is composed of two connected robotic arms, namely the first robotic arm and the second robotic arm.

[0011] Furthermore, the information acquisition device includes a point cloud acquisition device and a pruning equipment pose acquisition device;

[0012] The point cloud acquisition device uses a depth camera and / or lidar to acquire point cloud information of the target tree and its surrounding environment.

[0013] The pruning equipment pose acquisition device employs a pull-rope sensor, an angle sensor, a rotary encoder, and an IMU attitude sensor. The pull-rope sensor is used to acquire the extension distance of the hydraulic cylinder or electric push rod; the angle sensor is used to acquire the angle changes of the robotic arm and cutter head; the rotary encoder is used to acquire the travel distance and speed of the tracked chassis; and the IMU attitude sensor is used to measure the attitude of the tracked chassis.

[0014] Furthermore, the data processing unit includes a point cloud processing unit, which specifically performs the following operations:

[0015] Point cloud data of tree surfaces is extracted from information acquired by a point cloud acquisition device. The point cloud data undergoes downsampling, denoising, and smoothing. Point cloud data from multiple perspectives are registered to obtain complete tree point cloud data. This complete tree point cloud data is then segmented to obtain point cloud data for individual trees. Crown width, tree height, row spacing, and volume parameters are extracted from the point cloud data of individual trees. The center 3D coordinates and pruning angle of the target pruning location are calculated. Specifically, the passthrough algorithm in the PCL library is used to set a threshold to filter out remote noise point clouds, and the RANSAC algorithm is used to filter out ground point clouds. The Radius Outlier Removal algorithm is used to remove outlier point clouds, obtaining tree group information. Then, with the minimum sum of squared clustering errors of the target set as the optimization objective, combined with the minimum-maximum distance method, K-Means clustering is used to divide the processed tree point cloud data into K clusters, each cluster being the 3D point cloud of a single tree. A 3D coordinate system is established with the initial point of the LiDAR or depth camera as the origin, and the passthrough algorithm is used to... The filter algorithm extracts the point clouds of the trunk and canopy separately and projects them onto the vertical XOY plane. The RANSAC algorithm is used to fit a circle, with the diameter of the canopy fitting circle as the canopy diameter and the center of the trunk fitting circle as the tree position. To ensure better lighting conditions for the fruit trees, the height and diameter of the fruit trees need to be controlled within a preset multiple of the row spacing. The row spacing is calculated as follows: the center of the projection circle of the trunk of two adjacent fruit tree rows is fitted as a straight line, and the distance between the two straight lines is the fruit tree row spacing. With the preset multiple of the row spacing as a constraint, the canopy layer that does not meet this constraint is defined as the pruning object in the three-dimensional point cloud of the fruit tree and filtered out in the three-dimensional point cloud. The filtered canopy layer point cloud is projected onto a plane perpendicular to the forward direction of the pruning equipment. The length of the line segment is set to fit the line segment, and the center position (x, y, z) and tilt angle θ of the line segment are defined as the cutting part and angle of the cutter head, that is, the three-dimensional coordinates of the center of the target pruning part and the pruning angle.

[0016] Furthermore, the data processing unit also includes a trimming equipment pose construction unit, which includes:

[0017] The chassis pose calculation module uses a Kalman filter algorithm to fuse rotary encoder information and IMU attitude sensor information to construct a chassis odometer.

[0018] The pose calculation module for the pruning robotic arm is used to fuse angle sensor information and rope sensor information to construct the pose information of the pruning operation mechanism.

[0019] Furthermore, the control unit includes:

[0020] The path planning module is used to plan the movement path and attitude adjustment strategy of the tracked chassis and the trimming operation mechanism based on the center three-dimensional coordinates of the target trimming part and the trimming angle.

[0021] The motion control module is used to send movement commands and attitude adjustment commands to the tracked chassis and trimming mechanism, and to receive feedback information;

[0022] The trimming control module is used to send start and stop commands to the cutter head and receive feedback information.

[0023] Furthermore, the automatic target adjustment and pruning system based on three-dimensional point clouds also includes a control cabinet, which is equipped with a touch screen for system operation. The data processing unit and control unit are arranged in the control cabinet, and the point cloud acquisition device is arranged above the control cabinet and directly in front of the pruning equipment.

[0024] The present invention also provides a trimming method for the above-mentioned automatic target adjustment posture trimming system based on three-dimensional point cloud, including a single trimming mode and a differentiated target trimming mode. If the single trimming mode is selected, step S1 is executed; if the differentiated target trimming mode is selected, steps S2-S7 are executed.

[0025] S1. In single pruning mode, the pruning equipment pose acquisition device is activated to acquire the current three-dimensional pose of the pruning equipment and display it on the touch screen. The angles of each robotic arm and cutter head of the pruning equipment are selected by dragging the touch screen multiple times to determine the pruning pose. After setting, the rotation angles (θ1, θ2, θ3) of the first robotic arm, the second robotic arm and the cutter head of the set pose are fed back to the control unit to adjust the pruning equipment pose to the target pose and drive the tracked chassis to prune. The effect of this pruning mode is suitable for the shaping and pruning of multiple plants in a unified manner.

[0026] S2. In the differentiated target pruning mode, there is a differentiated target pruning button on the touch screen. Clicking this button will start differentiated target pruning; start the tracked chassis movement of the pruning equipment, start the point cloud acquisition device, start the data processing unit, scan the target tree, and acquire the three-dimensional point cloud data of the target tree and the surrounding environment.

[0027] S3. Perform downsampling, noise reduction and smoothing on the 3D point cloud data, then register the 3D point cloud data from multiple perspectives to obtain complete tree point cloud data. Segment the complete tree point cloud data to obtain point cloud data of individual trees. Extract the crown width, tree height, row spacing and volume parameters of the trees. Calculate the target pruning parts according to agronomic requirements, and calculate the 3D coordinates and pruning angles of all target parts.

[0028] S4. Based on the three-dimensional coordinates and trimming angle of the target trimming part, plan the tracked chassis path and the attitude adjustment strategy of the trimming operation mechanism from high to low.

[0029] S5. Obtain the current status of the trimming equipment, control the tracked chassis to move to the correct position according to the trimming path planning of the target trimming part, and adjust the position and attitude of the trimming robot arm to align it with the target trimming part.

[0030] S6. Start the cutter head, and the tracked chassis will move to perform shaping and trimming on the target trimming area.

[0031] S7. Repeat steps S4-S6 until all trees have been pruned.

[0032] Furthermore, in step S5, the coordinate system of the base, the front end of each robotic arm, the front end of the cutter head, and the center is fixed. Through the transformation matrix between the coordinate systems, as shown in equation (1), the position of the center coordinate system of the end cutter head in reality is solved according to the rotation angle (θ1, θ2, θ3) of the first robotic arm, the second robotic arm, and the cutter head.

[0033]

[0034] In the formula, i is the number of the lever, ranging from 0 to 3. Lever 0 is the base, lever 1 is the first robotic arm, lever 2 is the second robotic arm, and lever 3 is the cutter head. Let be the transformation matrix of coordinate system {i} of rod i relative to coordinate system {i-1} of rod i-1. x is the coordinate system of rod i-1 i-1 Axis rotation matrix, α i-1 For the z coordinate system of rod i-1 i-1 z-axis to rod i coordinate system i x in the i-1 coordinate system of the shaft i-1 The rotation angle of the shaft; x is the coordinate system of rod i-1 i-1 Axis translation matrix, a i-1 The distance traveled is the length of link i-1; For z i Axis rotation matrix, θ i Let x be the coordinate system of rod i-1 i-1 x-axis to rod i coordinate system i z-axis of the rod in the i-axis coordinate system i The rotation angle of the axis is θ. i The angle between link i and link i-1; z in the rod i coordinate system i axis translation matrix, d i The distance traveled is the offset of link i relative to link i-1;

[0035] The position of the target cutting point is converted into coordinates in the cutter head coordinate system. Inverse kinematics is then used to solve for the motion angles of the first and second robotic arms and the cutter head, establishing an inverse kinematics geometric solution model. The purpose of inverse kinematics analysis is to calculate the individual rotation angles of all links given the three-dimensional coordinates and rotation angles of the end effector. The specific solution process is as follows: The three-dimensional coordinates of the target trimming area are transformed into the position of the cutter head center coordinate system on the x and z axes of the base coordinate system. Simultaneously, the cutter head attitude angle θ is also considered. T According to formula (2), the coordinates are converted into the coordinates of the front end coordinate system of the cutter head on the x and z axes of the base coordinate system.

[0036]

[0037] In the formula, t x t z The difference in the x and z directions between the initial positions of the coordinate system at the front end of the cutter head and the coordinate system at the center of the cutter head;

[0038] Based on the triangular relationship formed by the coordinate systems of the first and second robotic arms and the origin of the cutter head, the rotation angles θ1 of the first robotic arm and θ2 of the second robotic arm are solved according to equations (3), (4), and (5). Finally, based on the given cutter head attitude angle θ... T The difference between θ1 and θ2 can be used to obtain the rotation angle θ3 of the cutter head;

[0039]

[0040]

[0041]

[0042] In the formula, L is the distance between the coordinate system of the first robotic arm and the coordinate system of the front end of the cutter head; d0 is the distance between the coordinate system of the first robotic arm and the coordinate system of the base; ∠1 is the angle between L and the x-axis; ∠2 is the angle between L and the first robotic arm; ∠3 is the angle between the second robotic arm and the first robotic arm; a1 is the length of the first robotic arm; a2 is the length of the second robotic arm.

[0043] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0044] This invention can construct parameters such as crown width and tree height of trees based on detected tree point clouds. Combined with pruning requirements, it calculates the target pruning locations, thereby achieving precise pruning by adjusting the pruning posture according to the tree's crown parameters. This reduces workload and improves pruning efficiency. This invention is not only applicable to scenarios involving the uniform shaping and pruning of multiple trees, but also allows for targeted and precise pruning based on the differences among fruit trees, demonstrating practical application value and warranting promotion. Attached Figure Description

[0045] Figure 1 This is a schematic diagram of the trimming equipment.

[0046] Figure 2 This is a schematic diagram of the trimming equipment.

[0047] Figure 3 The diagram illustrates the scenarios of single trimming and differentiated target trimming modes; in the diagram, (a) represents the single trimming mode and (b) represents the differentiated target trimming mode.

[0048] Figure 4 This is a schematic diagram of the inverse kinematics geometry solution model.

[0049] Wherein: 1 is the tracked chassis; 2 is the hydraulic cylinder; 3 is the engine; 4 is the first robotic arm; 5 is the second robotic arm; 6 is the motor; 7 is the cutter head; 8 is the lidar; 9 is the depth camera; 10 is the touch screen; 11 is the control cabinet; 12 is the base. Detailed Implementation

[0050] The present invention will be further described in detail below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

[0051] This embodiment discloses an automatic target-adjusting posture trimming system based on three-dimensional point clouds, including:

[0052] Pruning equipment used to prune target trees;

[0053] The information acquisition device is used to acquire point cloud information of the target tree and its surrounding environment, as well as the pose information of the pruning equipment, and transmit it to the data processing unit.

[0054] The data processing unit is used to process the information transmitted from the information acquisition device, obtain the three-dimensional coordinates and pruning angle of the target tree pruning part, as well as the position and posture information of the pruning equipment, and transmit them to the control unit.

[0055] The control unit controls the pruning equipment to perform precise pruning based on the information transmitted from the data processing unit.

[0056] Control cabinet 11, which is equipped with a touch screen 10 for system operation, see Figure 1 As shown, the data processing unit and the control unit are arranged in the control cabinet.

[0057] Specifically, see Figure 1As shown, the pruning equipment includes a tracked chassis 1 and a pruning mechanism mounted on a base 12 on top of the tracked chassis. The tracked chassis 1 is driven by an engine 3 and is used to carry and move the pruning mechanism. The pruning mechanism includes a pruning arm and a cutter head 7 driven to rotate by a motor 6 for cutting trees. The pruning arm is composed of two connected arms, namely a first arm 4 and a second arm 5. The cutter head 7 is mounted at the end of the second arm 5. The pruning mechanism can be driven by a hydraulic cylinder or an electric push rod. In this embodiment, a hydraulic cylinder 2 is used for driving.

[0058] Specifically, the information acquisition device includes a point cloud acquisition device and a pruning equipment pose acquisition device;

[0059] See Figure 1 As shown, the point cloud acquisition device uses a depth camera 9 and / or a lidar 8. In this embodiment, both a depth camera 9 and a lidar 8 are used simultaneously. The device is arranged above the control cabinet 11 and located directly in front of the pruning equipment to acquire point cloud information of the target tree and the surrounding environment.

[0060] The pruning equipment pose acquisition device employs a pull-rope sensor, an angle sensor, a rotary encoder, and an IMU attitude sensor. The pull-rope sensor is used to collect the extension distance of the hydraulic cylinder or electric push rod, and in this embodiment, it is specifically arranged at both ends of the hydraulic cylinder barrel and piston rod to collect the extension distance of the hydraulic cylinder 2. The angle sensor is arranged on the first robotic arm 4, the second robotic arm 5, and the cutter head 7, and is in close contact with them to collect the angle changes of the first robotic arm 4, the second robotic arm 5, and the cutter head 7. The rotary encoder is arranged on the drive wheel of the tracked chassis 1 to collect the travel distance and travel speed of the tracked chassis 1. The IMU attitude sensor is arranged above the tracked chassis 1 to measure the attitude of the tracked chassis 1.

[0061] Specifically, the data processing unit includes a point cloud processing unit, which performs the following operations:

[0062] Point cloud data of tree surfaces is extracted from depth and distance information acquired by depth camera 9 and LiDAR 8. The point cloud data undergoes downsampling, denoising, and smoothing. Point cloud data from multiple perspectives are registered to obtain complete tree point cloud data. This complete tree point cloud data is then segmented to obtain point cloud data for individual trees. Parameters such as crown width, tree height, row spacing, and volume are extracted from the point cloud data of individual trees. The center 3D coordinates and pruning angle of the target pruning location are calculated. Specifically, the passthrough algorithm in the PCL library is used to set a threshold to filter out remote noise point clouds, and the RANSAC algorithm is used to filter out ground point clouds. The RadiusOutlierRemoval algorithm is used to remove outlier point clouds, obtaining tree group information. Then, with the minimum sum of squared clustering errors of the target set as the optimization objective, combined with the minimum-maximum distance method, K-Means clustering is used to divide the processed tree point cloud data into K clusters, each cluster being the 3D point cloud of a single tree. A 3D coordinate system is established with the initial point of the LiDAR or depth camera as the origin, and the passthrough algorithm is used to... The filter algorithm extracts the point clouds of the trunk and crown separately and projects them onto the vertical XOY plane. The RANSAC algorithm is used to fit a circle, with the diameter of the crown fitting circle as the crown diameter and the center of the trunk fitting circle as the tree position. Taking the agronomical requirements of lychee tree pruning as an example, in order to provide better light conditions for the fruit trees, the height and diameter of the fruit trees need to be controlled at 0.7 times the row spacing. The calculation method of row spacing is as follows: the center of the projection circle of the trunk of the fruit trees in two adjacent rows is fitted as a straight line, and the distance between the two straight lines is the row spacing of the fruit trees. With 0.7 times the row spacing as a constraint, the crown layer that does not meet this constraint is defined as the pruning object in the three-dimensional point cloud of the fruit trees and filtered out in the three-dimensional point cloud. The filtered crown layer point cloud is projected onto a plane perpendicular to the forward direction of the pruning equipment. The length of the line segment is set to fit the line segment. The center position (x, y, z) and the tilt angle θ of the line segment are defined as the cutting part and angle of the cutter head, that is, the three-dimensional coordinates of the center of the target pruning part and the pruning angle.

[0063] Specifically, the data processing unit further includes a trimming equipment pose construction unit, which includes:

[0064] The chassis pose calculation module uses a Kalman filter algorithm to fuse rotary encoder information and IMU attitude sensor information to construct a chassis odometer.

[0065] The pose calculation module for the pruning robotic arm is used to fuse angle sensor information and rope sensor information to construct the pose information of the pruning operation mechanism.

[0066] Specifically, the control unit includes:

[0067] The path planning module is used to plan the movement path and attitude adjustment strategy of the tracked chassis and the trimming operation mechanism based on the center three-dimensional coordinates of the target trimming part and the trimming angle.

[0068] The motion control module is used to send movement commands and attitude adjustment commands to the tracked chassis and trimming mechanism, and to receive feedback information;

[0069] The trimming control module is used to send start and stop commands to the cutter head and receive feedback information.

[0070] See Figure 2 and Figure 3 As shown, the trimming method of the automatic target adjustment posture trimming system based on three-dimensional point cloud described above in this embodiment includes a single trimming mode and a differentiated target trimming mode. If the single trimming mode is selected, step S1 is executed; if the differentiated target trimming mode is selected, steps S2-S7 are executed.

[0071] S1. In single pruning mode, the pruning equipment pose acquisition device is activated to acquire the current three-dimensional pose of the pruning equipment and display it on the touch screen. The angles of each robotic arm and cutter head of the pruning equipment are selected by dragging the touch screen multiple times to determine the pruning pose. After setting, the rotation angles (θ1, θ2, θ3) of the first robotic arm, the second robotic arm and the cutter head of the set pose are fed back to the control unit to adjust the pruning equipment pose to the target pose and drive the tracked chassis to prune. The effect of this pruning mode is suitable for the shaping and pruning of multiple plants in a unified manner.

[0072] S2. In the differentiated target pruning mode, there is a differentiated target pruning button on the touch screen. Clicking this button will start differentiated target pruning; start the tracked chassis movement of the pruning equipment, start the point cloud acquisition device, start the data processing unit, scan the target tree, and acquire the three-dimensional point cloud data of the target tree and the surrounding environment.

[0073] S3. Perform downsampling, noise reduction and smoothing on the 3D point cloud data, then register the 3D point cloud data from multiple perspectives to obtain complete tree point cloud data. Segment the complete tree point cloud data to obtain point cloud data of individual trees. Extract the crown width, tree height, row spacing and volume parameters of the trees. Calculate the target pruning parts according to agronomic requirements, and calculate the 3D coordinates and pruning angles of all target parts.

[0074] S4. Based on the three-dimensional coordinates and trimming angle of the target trimming part, plan the tracked chassis path and the attitude adjustment strategy of the trimming operation mechanism from high to low.

[0075] S5. Obtain the current status of the trimming equipment, control the tracked chassis to move to the correct position according to the trimming path planning of the target trimming part, and adjust the position and attitude of the trimming robot arm to align it with the target trimming part.

[0076] S6. Start the cutter head, and the tracked chassis will move to perform shaping and trimming on the target trimming area.

[0077] S7. Repeat steps S4-S6 until all trees have been pruned.

[0078] Specifically, in step S5, coordinate systems are fixed on the base, the front end of each robotic arm and the center of the cutter head. Through the transformation matrix between the coordinate systems, as shown in equation (1), the position of the center coordinate system of the end cutter head in reality is solved according to the rotation angle (θ1, θ2, θ3) of the first robotic arm, the second robotic arm and the cutter head.

[0079]

[0080] In the formula, i is the number of the lever, ranging from 0 to 3. Lever 0 is the base, lever 1 is the first robotic arm, lever 2 is the second robotic arm, and lever 3 is the cutter head. Let be the transformation matrix of coordinate system {i} of rod i relative to coordinate system {i-1} of rod i-1. x is the coordinate system of rod i-1 i-1 Axis rotation matrix, α i-1 For the z coordinate system of rod i-1 i-1 z-axis to rod i coordinate system i x in the i-1 coordinate system of the shaft i-1 The rotation angle of the shaft; x is the coordinate system of rod i-1 i-1 Axis translation matrix, a i-1 The distance traveled is the length of link i-1; For z i Axis rotation matrix, θ i Let x be the coordinate system of rod i-1 i-1 x-axis to rod i coordinate system i z-axis of the rod in the i-axis coordinate system i The rotation angle of the axis is θ. i The angle between link i and link i-1; z in the rod i coordinate system i axis translation matrix, d i The distance traveled is the offset of link i relative to link i-1;

[0081] The position of the target cutting point is converted into coordinates in the cutter head coordinate system, and the motion angles of the first robotic arm, the second robotic arm, and the cutter head are solved using inverse kinematics; the implementation process is described in [link to implementation details]. Figure 4 As shown, an inverse kinematics geometric solution model is established. The purpose of inverse kinematics analysis is to calculate the rotation angles of all members given the three-dimensional coordinates and rotation angles of the end members. The specific solution process is as follows: The three-dimensional coordinates of the target trimming part are transformed into the position of the cutter head center coordinate system on the x and z axes of the base coordinate system. Simultaneously, the cutter head attitude angle θ is also considered. T According to formula (2), the coordinates are converted into the coordinates of the front end coordinate system of the cutter head on the x and z axes of the base coordinate system.

[0082]

[0083] In the formula, t x t z The difference in the x and z directions between the initial positions of the coordinate system at the front end of the cutter head and the coordinate system at the center of the cutter head;

[0084] Based on the triangular relationship formed by the origins of the first robotic arm coordinate system (Frame 1 in the figure), the second robotic arm coordinate system (Frame 2 in the figure), and the front end coordinate of the cutter head (Frame 3 in the figure), the rotation angles θ1 of the first robotic arm and θ2 of the second robotic arm are solved according to equations (3), (4), and (5). Finally, based on the given cutter head attitude angle θ T The difference between θ1 and θ2 can be used to obtain the rotation angle θ3 of the cutter head;

[0085]

[0086]

[0087]

[0088] In the formula, L is the distance between the coordinate system of the first robotic arm and the coordinate system of the front end of the cutter head; d0 is the distance between the coordinate system of the first robotic arm and the coordinate system of the base; ∠1 is the angle between L and the x-axis; ∠2 is the angle between L and the first robotic arm; ∠3 is the angle between the second robotic arm and the first robotic arm; a1 is the length of the first robotic arm; a2 is the length of the second robotic arm.

[0089] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A pruning method for an automatic target-alignment attitude adjustment pruning system based on 3D point clouds, wherein the automatic target-alignment attitude adjustment pruning system comprises: Pruning equipment used to prune target trees; The information acquisition device is used to acquire point cloud information of the target tree and its surrounding environment, as well as the pose information of the pruning equipment, and transmit it to the data processing unit. The data processing unit is used to process the information transmitted from the information acquisition device, obtain the three-dimensional coordinates and pruning angle of the target tree pruning part, as well as the position and posture information of the pruning equipment, and transmit them to the control unit. The control unit controls the pruning equipment to perform precise pruning based on the information transmitted from the data processing unit. The pruning method is characterized by including a single pruning mode and a differentiated target pruning mode. If the single pruning mode is selected, step S1 is executed; if the differentiated target pruning mode is selected, steps S2-S7 are executed. S1. In single pruning mode, the pruning equipment pose acquisition device is activated to acquire the current three-dimensional pose of the pruning equipment and display it on the touch screen. The angles of each robotic arm and cutter head of the pruning equipment are selected by dragging the touch screen multiple times to determine the pruning pose. After setting, the rotation angles (θ1, θ2, θ3) of the first robotic arm, the second robotic arm and the cutter head of the set pose are fed back to the control unit to adjust the pruning equipment pose to the target pose and drive the tracked chassis to prune. The effect of this pruning mode is suitable for the shaping and pruning of multiple plants in a unified manner. S2. In the differentiated target pruning mode, there is a differentiated target pruning button on the touch screen. Clicking this button will start differentiated target pruning; start the tracked chassis movement of the pruning equipment, start the point cloud acquisition device, start the data processing unit, scan the target tree, and acquire the three-dimensional point cloud data of the target tree and the surrounding environment. S3. Perform downsampling, noise reduction and smoothing on the 3D point cloud data, then register the 3D point cloud data from multiple perspectives to obtain complete tree point cloud data. Segment the complete tree point cloud data to obtain point cloud data of individual trees. Extract the crown width, tree height, row spacing and volume parameters of the trees. Calculate the target pruning parts according to agronomic requirements, and calculate the 3D coordinates and pruning angles of all target parts. S4. Based on the three-dimensional coordinates and trimming angle of the target trimming part, plan the tracked chassis path and the attitude adjustment strategy of the trimming operation mechanism from high to low. S5. Obtain the current status of the trimming equipment, and according to the trimming path planning for the target trimming area, control the tracked chassis to move to the correct position, and adjust the position and attitude of the trimming robot arm to align it with the target trimming area, as follows: In the base, the front end of each robotic arm, the front end of the cutter head, and the central fixed coordinate system, the transformation matrix between the coordinate systems is shown in Equation (1), so as to solve the position of the center coordinate system of the end cutter head in reality based on the rotation angle (θ1, θ2, θ3) of the first robotic arm, the second robotic arm, and the cutter head. In the formula, i is the number of the lever, ranging from 0 to 3. Lever 0 is the base, lever 1 is the first robotic arm, lever 2 is the second robotic arm, and lever 3 is the cutter head. Let be the transformation matrix of coordinate system {i} of rod i relative to coordinate system {i-1} of rod i-1. x is the coordinate system of rod i-1 i-1 Axis rotation matrix, α i-1 For the z coordinate system of rod i-1 i-1 z-axis to rod i coordinate system i x in the i-1 coordinate system of the shaft i-1 The rotation angle of the shaft; x is the coordinate system of rod i-1 i-1 Axis translation matrix, a i-1 The distance traveled is the length of link i-1; For z i Axis rotation matrix, θ i Let x be the coordinate system of rod i-1 i-1 x-axis to rod i coordinate system i z-axis of the rod in the i-axis coordinate system i The rotation angle of the axis is θ. i The angle between link i and link i-1; z in the rod i coordinate system i axis translation matrix, d i The distance traveled is the offset of link i relative to link i-1; The position of the target cutting point is converted into coordinates in the cutter head coordinate system. Inverse kinematics is then used to solve for the motion angles of the first and second robotic arms and the cutter head, establishing an inverse kinematics geometric solution model. The purpose of inverse kinematics analysis is to calculate the individual rotation angles of all links given the three-dimensional coordinates and rotation angles of the end effector. The specific solution process is as follows: The three-dimensional coordinates of the target trimming area are transformed into the position of the cutter head center coordinate system on the x and z axes of the base coordinate system. Simultaneously, the cutter head attitude angle θ is also considered. T According to formula (2), the coordinates are converted into the coordinates of the front end coordinate system of the cutter head on the x and z axes of the base coordinate system. In the formula, t x t z The difference in the x and z directions between the initial positions of the coordinate system at the front end of the cutter head and the coordinate system at the center of the cutter head; Based on the triangular relationship formed by the coordinate systems of the first and second robotic arms and the origin of the cutter head, the rotation angles θ1 of the first robotic arm and θ2 of the second robotic arm are solved according to equations (3), (4), and (5). Finally, based on the given cutter head attitude angle θ... T The difference between θ1 and θ2 can be used to obtain the rotation angle θ3 of the cutter head; In the formula, L is the distance between the coordinate system of the first robotic arm and the coordinate system of the front end of the cutter head; d0 is the distance between the coordinate system of the first robotic arm and the coordinate system of the base; ∠1 is the angle between L and the x-axis; ∠2 is the angle between L and the first robotic arm; ∠3 is the angle between the second robotic arm and the first robotic arm; a1 is the length of the first robotic arm; a2 is the length of the second robotic arm. S6. Start the cutter head, and the tracked chassis will move to perform shaping and trimming on the target trimming area. S7. Repeat steps S4-S6 until all trees have been pruned.

2. The trimming method of the automatic target-adjusting posture trimming system based on three-dimensional point clouds according to claim 1, characterized in that, The pruning equipment includes a tracked chassis and a pruning mechanism mounted on a base on top of the tracked chassis. The tracked chassis is used to support and move the pruning mechanism. The pruning mechanism includes a pruning arm and a cutter head mounted at the end of the pruning arm for cutting trees. The pruning mechanism is driven by a hydraulic cylinder or an electric push rod.

3. The trimming method of the automatic target-adjusting posture trimming system based on three-dimensional point clouds according to claim 2, characterized in that, The trimming robotic arm consists of two connected robotic arms, namely the first robotic arm and the second robotic arm.

4. The trimming method of the automatic target-adjusting posture trimming system based on three-dimensional point clouds according to claim 3, characterized in that, The information acquisition device includes a point cloud acquisition device and a pruning equipment pose acquisition device. The point cloud acquisition device uses a depth camera and / or lidar to acquire point cloud information of the target tree and its surrounding environment. The pruning equipment pose acquisition device employs a pull-rope sensor, an angle sensor, a rotary encoder, and an IMU attitude sensor. The pull-rope sensor is used to acquire the extension distance of the hydraulic cylinder or electric push rod; the angle sensor is used to acquire the angle changes of the robotic arm and cutter head; the rotary encoder is used to acquire the travel distance and speed of the tracked chassis; and the IMU attitude sensor is used to measure the attitude of the tracked chassis.

5. The trimming method of the automatic target-adjusting posture trimming system based on three-dimensional point clouds according to claim 4, characterized in that, The data processing unit includes a point cloud processing unit, which specifically performs the following operations: Point cloud data of tree surfaces is extracted from information acquired by a point cloud acquisition device. The point cloud data undergoes downsampling, denoising, and smoothing. Point cloud data from multiple perspectives are registered to obtain complete tree point cloud data. This complete tree point cloud data is then segmented to obtain point cloud data for individual trees. Crown width, tree height, row spacing, and volume parameters are extracted from the point cloud data of individual trees. The center 3D coordinates and pruning angle of the target pruning location are calculated. Specifically, the passthrough algorithm in the PCL library is used to set a threshold to filter out remote noise point clouds, and the RANSAC algorithm is used to filter out ground point clouds. The Radius Outlier Removal algorithm is used to remove outlier point clouds, obtaining tree group information. Then, with the minimum sum of squared clustering errors of the target set as the optimization objective, combined with the minimum-maximum distance method, K-Means clustering is used to divide the processed tree point cloud data into K clusters, each cluster being the 3D point cloud of a single tree. A 3D coordinate system is established with the initial point of the LiDAR or depth camera as the origin, and the passthrough algorithm is used to... The filter algorithm extracts the point clouds of the trunk and canopy separately and projects them onto the vertical XOY plane. The RANSAC algorithm is used to fit a circle, with the diameter of the canopy fitting circle as the canopy diameter and the center of the trunk fitting circle as the tree position. To ensure better lighting conditions for the fruit trees, the height and diameter of the fruit trees need to be controlled within a preset multiple of the row spacing. The row spacing is calculated as follows: the center of the projection circle of the trunk of two adjacent fruit tree rows is fitted as a straight line, and the distance between the two straight lines is the fruit tree row spacing. With the preset multiple of the row spacing as a constraint, the canopy layer that does not meet this constraint is defined as the pruning object in the three-dimensional point cloud of the fruit tree and filtered out in the three-dimensional point cloud. The filtered canopy layer point cloud is projected onto a plane perpendicular to the forward direction of the pruning equipment. The length of the line segment is set to fit the line segment, and the center position (x, y, z) and tilt angle θ of the line segment are defined as the cutting part and angle of the cutter head, that is, the three-dimensional coordinates of the center of the target pruning part and the pruning angle.

6. The trimming method of the automatic target-adjusting posture trimming system based on three-dimensional point clouds according to claim 5, characterized in that, The data processing unit further includes a trimming equipment pose construction unit, which includes: The chassis pose calculation module uses a Kalman filter algorithm to fuse rotary encoder information and IMU attitude sensor information to construct a chassis odometer. The pruning robot pose calculation module is used to fuse angle sensor information and rope sensor information to construct the pose information of the pruning operation mechanism.

7. The trimming method of the automatic target-adjusting posture trimming system based on three-dimensional point clouds according to claim 6, characterized in that, The control unit includes: The path planning module is used to plan the movement path and attitude adjustment strategy of the tracked chassis and the trimming operation mechanism based on the center three-dimensional coordinates of the target trimming part and the trimming angle. The motion control module is used to send movement commands and attitude adjustment commands to the tracked chassis and trimming mechanism, and to receive feedback information; The trimming control module is used to send start and stop commands to the cutter head and receive feedback information.

8. The trimming method of the automatic target-adjusting posture trimming system based on three-dimensional point clouds according to claim 7, characterized in that, It also includes a control cabinet, which is equipped with a touch screen for system operation. The data processing unit and control unit are arranged in the control cabinet, and the point cloud acquisition device is arranged above the control cabinet and directly in front of the pruning equipment.

Images