An orchard robot adaptive filtering positioning method fusing trunk feature perception

Abstract

This invention discloses an adaptive filtering localization method for orchard robots that integrates tree trunk feature perception, belonging to the field of autonomous localization and mapping technology for orchard robots. Addressing the problem of significant interference from branches and leaves and the lack of stable planes in orchards leading to the failure of traditional localization methods, this scheme extracts high-confidence tree trunk features through multiple geometric constraints to filter out branch and leaf noise, and fuses these features with surface features to construct observation residuals. Simultaneously, an adaptive iterative filtering acceleration mechanism based on environment and motion state is introduced in the pose estimation stage. This invention overcomes the instability of unstructured orchard feature matching, significantly improving the robot's localization accuracy and robustness in complex and bumpy environments.

Description

Technical Field

[0001] This invention relates to autonomous localization and mapping technology for orchard robots, specifically to an adaptive filtering localization method for orchard robots that integrates tree trunk feature perception. Background Technology

[0002] With the development of smart agriculture, orchard production management is rapidly transforming towards precision and intelligence. Precision pesticide application directly affects fruit yield and quality and is a key factor in pest and disease control. In recent years, orchard spraying robots, with their advantages of high efficiency and environmental friendliness, have become an important direction for upgrading modern agricultural equipment.

[0003] However, orchard environments are generally characterized by rugged terrain, disordered tree trunks, and severe canopy obstruction. Traditional positioning methods based on the Global Navigation Satellite System (GNSS) are susceptible to obstruction and multipath effects in practical applications, leading to unstable positioning signals. Furthermore, in structured orchard scenarios with rugged terrain or trees of similar height, traditional positioning methods exhibit poor environmental adaptability and robustness, failing to meet the requirements for continuous and reliable operation of orchard robots. This hinders the engineering application and equipment integration of related key technologies. To address the problem of autonomous positioning in chaotic orchard environments and under conditions of limited satellite signals, simultaneous localization and mapping (SLAM) technology based on lidar has gradually become an important solution for orchard spraying robots to achieve autonomous operation. This method typically utilizes lidar and inertial measurement units (IMUs) to perform real-time perception and measurement of the orchard environment, achieving high-precision robot positioning even without stable satellite signals. By constructing a 3D point cloud map of the orchard environment, it provides a reliable reference for subsequent path planning, navigation control, and operational decisions, forming the foundation for the autonomous operation of orchard robots in complex environments.

[0004] The orchard environment is characterized by uneven ground and dense foliage, making environmental perception susceptible to motion disturbances and noise interference. Existing laser-synchronized localization (laser-localization) mapping methods are mostly used for urban or structured scenarios, and their feature extraction and registration processes typically focus on stable planar features such as walls and the ground. In unstructured environments like orchards, due to the lack of continuous, regular planar structures, laser radar often struggles to extract sufficiently stable feature points. Instead, it is easily disturbed by non-rigid, time-varying targets such as branches and leaves, leading to unstable feature matching and increased odometry estimation errors. The continuous accumulation of localization errors reduces the accuracy of the orchard robot's pose estimation and affects the accuracy and consistency of the constructed environmental map, thus adversely impacting subsequent path planning, navigation control, and operational decisions, limiting the orchard robot's autonomous operation capabilities in complex environments. Summary of the Invention

[0005] Purpose of the Invention: To overcome the problems of insufficient positioning accuracy and poor robustness of existing orchard robots in complex structured environments, and considering the characteristics of orchard working environments such as uneven ground, severe occlusion by tree branches and leaves, and high repetition of environmental structures, the purpose of this invention is to provide an adaptive filtering positioning method suitable for complex orchard environments. By introducing tree trunk structural feature constraints and a motion scene-based adaptive iterative filtering strategy into the laser-inertial fusion positioning framework, the accuracy and robustness of orchard robot positioning estimation in complex tree environments are improved. This invention is applicable to the autonomous positioning of mobile robots of different functional types in agricultural orchard environments, and features strong adaptability and high robustness.

[0006] The technical solution of this invention is: an adaptive filtering localization method for orchard robots that integrates tree trunk feature perception, specifically including the following steps:

[0007] Step S1: Acquire the high-frequency data of the 3D LiDAR point cloud and the inertial measurement unit, perform time synchronization and motion distortion correction processing on the original LiDAR point cloud data, complete the pre-integration calculation based on the inertial measurement unit results, and obtain the prior motion information for front-end state prediction.

[0008] Step S2: Utilizing the stable tree structure in the orchard environment, and based on the spatial distribution characteristics of the laser scanning lines, the point cloud processed in Step S1 is segmented and analyzed. Through multiple geometric constraints such as distance continuity, consistency of change trend, and curvature stability, a high-confidence tree trunk feature point cloud that conforms to the trunk cross-section characteristics is extracted.

[0009] Step S3: Store the trunk feature points extracted in step S2 and the synchronously obtained surface feature points in the local feature map, and divide them into voxels to construct a spatial search structure based on voxel index; when the current laser frame arrives, according to the voxel index relationship, quickly find the corresponding set of neighborhood feature points for the trunk feature points and surface feature points of the current frame in the historical local feature map, which is used for the construction of subsequent observation models and registration residuals;

[0010] Step S4: Based on the spatial organization and neighborhood association of feature points completed in step S3, perform geometric modeling on the trunk feature points and surface feature points in the current frame, fit the local planar model using the corresponding neighborhood feature information, and use the distance error from the point to the plane as a unified observation residual to describe the deviation relationship between the current observation and the prediction state.

[0011] Step S5: Based on the observation residual model constructed in step S4, perform observation iteration update on the predicted state in the error state space, and perform multiple rounds of correction on the robot pose state according to the observation residual until the preset convergence condition is met, and output the pose estimation result corresponding to the current laser frame.

[0012] Step S6: Treat the iterative update process in Step S5 as a fixed-point state iteration. Introduce an accelerated update mechanism based on historical iteration information in the error state space to optimize the iterative convergence process of the pose state. Based on the current driving state of the orchard robot and the environmental structure characteristics, adaptively control the start and stop of the acceleration strategy and the acceleration order. Enable acceleration in turning or dense tree conditions, and disable acceleration in straight-line conditions. At the same time, when the accelerated update exceeds the stability threshold, revert to the conventional iterative filtering update to reduce the number of iterations and ensure convergence stability. After completing the above adaptive acceleration or revert update, output the robot pose estimation result corresponding to the current laser frame as the localization result of the orchard robot.

[0013] Furthermore, step S1 specifically includes acquiring the 3D LiDAR point cloud data carried by the orchard robot and the high-frequency acceleration and angular velocity data output by the inertial measurement unit. The LiDAR data and the inertial measurement unit data are time-aligned through the robot operating system ROS, so that the data from multiple sensors are correlated under the same time reference. Using the high-frequency motion information provided by the inertial measurement unit within the laser scanning cycle, the displacement and rotation information of the robot in each laser scanning cycle are calculated using a pre-integration scheme. Motion compensation is performed on the original LiDAR point cloud to eliminate the distorted point cloud generated when the robot travels on the rugged road surface of the orchard.

[0014] Furthermore, step S2 specifically includes the following steps:

[0015] Step S2.1: The lidar on the orchard robot has a fixed installation height. By selecting laser scanning beams within a preset height range, scanning lines with elevation angles of -15° to -9° are removed to eliminate ground points, reducing the number of candidate point cloud segments to be processed on the tree trunk. The point cloud is traversed along the laser scanning direction in ascending order of azimuth angle, and segmented using a sliding window to obtain a sequence of continuous scanning points on the same beam. ;in, Point clouds obtained from the same line bundle in the order of scanning; the first The points represent the point cloud coordinates in the lidar coordinate system. Calculate the distance from the point to the origin of the lidar coordinate system. for:

[0016] ;

[0017] when At that time, among them and Based on the angular resolution of the lidar and the scale of the orchard tree trunks, a threshold is set to determine whether a point segment is a candidate for a tree trunk. Otherwise, the current point sequence is discarded to eliminate cluttered point clouds that are too close together and incomplete tree trunk point clouds that are far away. Given that tree trunks typically exhibit a regular and continuous arc structure within the laser scanning plane, the distance variation between adjacent points should have smooth and continuous geometric characteristics. Therefore, the radial distance variation rate between adjacent points is defined. As a quantitative indicator:

[0018] ;

[0019] When the candidate tree trunk segment satisfies hour, If the distance change rate threshold is set, the distance change of the current point segment is considered to be smooth and continuous, which conforms to the geometric distribution characteristics of the tree trunk, and it is retained; otherwise, it is discarded, thereby effectively filtering out invalid point segments formed by branch and leaf interference and irregular occlusion.

[0020] Step S2.2: Based on the screening in Step S2.1, further analysis is performed on the candidate tree trunk segments; since the tree trunk outline usually presents a single arc or near-arc shape on the laser scanning plane, and the spatial distribution of adjacent points should be continuous, based on these geometric characteristics, the first-order difference of the distance is used to analyze the segments. A quantitative description of the trend of the current candidate tree trunk segments is provided:

[0021] ;

[0022] In the scanning sequence corresponding to an ideal tree trunk structure, the distance change sequence typically exhibits a single convex or concave trend, with a low number of sign changes; based on this, the number of sign changes is defined. As a criterion for trend consistency:

[0023] ;

[0024] In the formula, As the noise threshold, For indicator functions, As the intersection condition, when If the current segment satisfies the single-peak variation trend of the tree trunk section, then the current segment is considered to meet the characteristics of the single-peak variation trend of the tree trunk section; otherwise, the current segment is regarded as a structurally mixed or shading area and is removed.

[0025] Furthermore, curvature stability constraints are introduced to further filter candidate point segments. By performing second-order difference operations on the ranging sequence, a curvature description sequence is constructed. Its mathematical expression is:

[0026] ;

[0027] In the scanning sequence corresponding to the actual tree trunk structure, its curvature changes smoothly; based on this, the mean and variance of the curvature description sequence are calculated:

[0028] ;

[0029] In the formula This indicates the number of second-order difference terms within the current candidate point segment of the tree trunk;

[0030] When the curvature variance of the candidate trunk segment satisfies hour, The set stable curvature judgment threshold is used to determine whether the current point segment has stable curvature change characteristics in the scanning plane and is judged as the final valid tree trunk feature point segment.

[0031] Step S2.3: Based on the above screening, candidate point segments are only determined as valid tree trunk structure feature point segments and included in the subsequent front-end localization and geometric constraint construction process if they simultaneously satisfy the distance range constraint, spatial continuity constraint, change trend consistency criterion, and curvature stability condition. For common interference situations in orchard operations, including instantaneous pseudo-features formed by wind blowing branches and leaves, incomplete point clouds caused by partial occlusion, and geometrical abrupt changes caused by tilted or forked tree trunks, they are effectively eliminated in the screening process because they are difficult to satisfy the above multiple geometric consistency constraints at the same time.

[0032] Further, in step S3, the extracted tree trunk structural feature points are stored in the tree trunk local feature map, and the distortion-corrected laser point cloud is stored in the surface local feature map. The tree trunk local feature map and the surface local feature map are then divided into voxel-based spatial organization structures to construct a voxel index-based spatial organization structure. When a new laser frame arrives, based on the voxel index relationship, the set of neighboring feature points for the tree trunk feature points and surface feature points corresponding to the current frame is quickly searched in the tree trunk local feature map and the surface local feature map, respectively, to establish the neighborhood information required for the subsequent observation model.

[0033] Furthermore, step S4, based on the local feature map and neighborhood association results, constructs local geometric models for the trunk feature points and surface feature points in the current frame; let the coordinates of the feature points in the current frame in the lidar coordinate system be... The current frame feature points are transformed to the local feature map coordinate system through robot-predicted pose transformation. The distance error from the point to the corresponding local plane is defined as the observation residual, and its expression is:

[0034] ;

[0035] In the formula, and Let these represent the rotation matrix and translation vector of the current frame's laser radar in the world coordinate system. Let be the unit normal vector of the local plane. These are plane offset parameters;

[0036] The geometric residual form from point to plane is uniformly adopted for both tree trunk feature points and surface feature points. Different types of environmental structural information are introduced into the observation model of the front-end localization to describe the deviation relationship between the predicted state and the actual observation. The geometric residual is used as the observation for front-end state estimation and is used to correct the robot pose state during the filtering update process.

[0037] Furthermore, in step S5, based on the observation residual model constructed in step S4, the robot pose state is iteratively estimated in the error state space; specifically, according to the deviation relationship between the predicted state of the inertial measurement unit and the observation residual of the current frame point cloud, the pose state is iteratively updated in multiple rounds, and when the result meets the preset convergence condition, the pose estimation result corresponding to the current laser frame is output.

[0038] Furthermore, step S6 specifically includes the following steps:

[0039] Step S6.1: In the iterative extended Kalman filter update process described in step S5, the error state update process is equivalent to a fixed-point state iteration, characterizing the evolution relationship of the error state in multiple rounds of updates. Its expression is:

[0040] ;

[0041] In the formula, Indicates the first The error state estimate obtained in the next iteration. This represents the state mapping function composed of the observation residuals and the filter update relationship;

[0042] Step S6.2: Based on the above fixed-point state iteration, an accelerated update strategy based on historical iteration information is introduced to optimize the iterative convergence process of the error state; by comprehensively considering the results of multiple historical iterations, the current error state update is weighted and combined to generate the accelerated state update quantity, the expression of which is:

[0043] ;

[0044] In the formula, The weighting coefficient is used to adjust the contribution ratio of historical iteration information in the accelerated update.

[0045] Step S6.3: To achieve adaptive control of the accelerated update mechanism, an accelerated update activation condition is constructed based on the robot's driving state and local environmental structural characteristics in the orchard environment; this is achieved through the robot's current angular velocity information. Characterize the degree of change in motion state, and measure it through the number of local point clouds. The density of the environmental structure is represented; when the robot's motion state changes little and the environmental structure is relatively sparse, a conventional iterative update method is used; when the motion state changes greatly or the environmental point cloud is dense, an accelerated update mechanism based on historical iteration information is enabled; the conditions for enabling accelerated updates are expressed as follows:

[0046] ;

[0047] In the formula, and These are preset angular velocity and point cloud quantity thresholds, respectively;

[0048] By using an adaptive start-stop strategy, iterative acceleration based on historical information can be reasonably applied in the complex environment of the orchard, ensuring positioning stability while avoiding unnecessary computational overhead.

[0049] Step S6.4: While enabling the accelerated update mechanism, introduce stability constraints and a rollback mechanism to ensure the convergence and reliability of the iterative update process; let the... The accelerated error state update amount in the next iteration is: The magnitude of its change compared to the previous update satisfies:

[0050] ;

[0051] In the formula, This is the maximum permissible error change threshold;

[0052] When the conditions are met, the accelerated update result is received; when the conditions are not met, it is considered that the accelerated update will destroy the convergence of the filter, and the system reverts to the regular iterative extended Kalman filter update process; after completing the adaptive update and stability criterion, the current pose estimation result of the robot is output as the final positioning data of the front-end positioning system for subsequent path planning and operation decision-making.

[0053] Compared with traditional orchard robot positioning methods, the advantages of the method of this invention are as follows:

[0054] (1) This invention integrates three-dimensional lidar point cloud data with inertial measurement unit information to perform time synchronization and motion compensation processing on the original lidar point cloud. Based on the pre-integration results of the inertial measurement unit, reliable motion prior information is obtained, which effectively reduces the point cloud deformation error caused by the movement of the orchard robot, improves the geometric consistency and measurement reliability of the front-end input data, and provides stable data for subsequent positioning and mapping.

[0055] (2) In view of the characteristics of orchard environment, such as strong regularity of tree arrangement, severe shading by branches and leaves, and complex point cloud distribution, this invention systematically analyzes the distribution pattern of point cloud in the scanning plane in orchard scene, and designs a tree trunk feature extraction mechanism based on multiple conditions such as distance continuity, consistency of change trend, and curvature stability. This method can effectively distinguish between real tree trunk structure and non-rigid interference such as branches, leaves, and shading, and extract stable and reliable high-confidence tree trunk feature point cloud to improve the quality of observation data in the localization process.

[0056] (3) This invention constructs a local feature map of the orchard environment through voxelization, thereby achieving efficient organization of feature points and rapid neighborhood association. Based on the local geometric model, the registration residual from point to surface is constructed as an observation constraint, which enables the structural information of the orchard environment to be effectively introduced into the front-end state estimation process. While ensuring computational efficiency, it improves the stability and consistency of geometric constraints in the front-end positioning process.

[0057] (4) Within the iterative filtering state estimation framework, this invention introduces an accelerated update mechanism based on historical iteration information to optimize the iterative update process of the error state. By utilizing historical iteration information, the number of iterations required for filtering is reduced. In the complex environment of orchards, this strategy can significantly improve the computational efficiency of front-end positioning and enhance the real-time performance of the system.

[0058] (5) The present invention further combines the driving state of the orchard robot with the local environmental structure characteristics to adaptively control the acceleration update mechanism: acceleration is turned off when the robot is moving straight and the environmental structure is relatively sparse, and acceleration is enabled when the robot is turning or when the trees are dense; at the same time, stability constraints and backoff mechanisms are introduced, and when the acceleration update causes instability, it automatically backoffs to the standard filter update form, so as to ensure positioning accuracy while taking into account computational efficiency and long-term reliability of system operation. Attached Figure Description

[0059] Figure 1 This is a schematic diagram of a complex orchard environment in which the present invention is applied.

[0060] Figure 2 This is a schematic diagram comparing the original laser point cloud and the extracted tree trunk structure feature point cloud in this invention, where (a) is the original laser point cloud and (b) is the tree trunk structure feature point cloud.

[0061] Figure 3 This is a schematic diagram illustrating the construction of point-to-plane observation residuals based on a local geometric model in this invention.

[0062] Figure 4 A schematic diagram of the adaptive accelerated iterative filtering module for orchard robot positioning in this invention.

[0063] Figure 5A schematic diagram of the overall process of the orchard robot adaptive filtering localization method that integrates tree trunk feature perception according to the present invention. Detailed Implementation

[0064] The following is in conjunction with the appendix Figures 1-5 The invention will be further illustrated by examples.

[0065] like Figure 1 As shown, the complex orchard structured tree scene represents a typical orchard ground environment. Considering the growth characteristics and economic efficiency of fruit trees, the roads between orchard rows are usually unpaved dirt roads, resulting in complex and rugged terrain. Furthermore, the branches in the orchard are randomly distributed, the original point cloud extracted by the lidar is sparse, and the trunks occlude each other, easily leading to incorrect matching and thus affecting positioning accuracy.

[0066] like Figure 2 The diagram showing a comparison between the original laser point cloud and the extracted tree trunk structure feature point cloud further illustrates the implementation steps of this invention:

[0067] Step S1: Acquire the high-frequency data of the 3D LiDAR point cloud and the inertial measurement unit, perform time synchronization and motion distortion correction processing on the original LiDAR point cloud data, complete the pre-integration calculation based on the inertial measurement unit results, and obtain the prior motion information for front-end state prediction.

[0068] Step S2: Utilizing the stable tree structure in the orchard environment, and based on the spatial distribution characteristics of the laser scanning lines, the point cloud processed in Step S1 is segmented and analyzed. Through multiple geometric constraints such as distance continuity, consistency of change trend, and curvature stability, a high-confidence tree trunk feature point cloud that conforms to the trunk cross-section characteristics is extracted.

[0069] Step S3: Store the trunk feature points extracted in step S2 and the synchronously obtained surface feature points in the local feature map, and divide them into voxels to construct a spatial search structure based on voxel index; when the current laser frame arrives, according to the voxel index relationship, quickly find the corresponding set of neighborhood feature points for the trunk feature points and surface feature points of the current frame in the historical local feature map, which is used for the construction of subsequent observation models and registration residuals;

[0070] like Figure 3 The diagram shown illustrates the construction of point-to-plane observation residuals based on a local geometric model. Further explanation is provided in conjunction with the implementation steps of this invention:

[0071] Step S4: Based on the spatial organization and neighborhood association of feature points completed in step S3, perform geometric modeling on the trunk feature points and surface feature points in the current frame, fit the local planar model using the corresponding neighborhood feature information, and use the distance error from the point to the plane as a unified observation residual to describe the deviation relationship between the current observation and the prediction state.

[0072] Step S5: Based on the observation residual model constructed in step S4, perform observation iteration update on the predicted state in the error state space, and perform multiple rounds of correction on the robot pose state according to the observation residual until the preset convergence condition is met, and output the pose estimation result corresponding to the current laser frame.

[0073] like Figure 4 The flowchart of the adaptive accelerated iterative filtering module for orchard robot localization shown below is further explained in conjunction with the implementation steps of this invention:

[0074] Step S6: Treat the iterative update process in Step S5 as a fixed-point state iteration. Introduce an accelerated update mechanism based on historical iteration information in the error state space to optimize the iterative convergence process of the pose state. Based on the current driving state of the orchard robot and the environmental structure characteristics, adaptively control the start and stop of the acceleration strategy and the acceleration order. Enable acceleration in turning or dense tree conditions, and disable acceleration in straight-line conditions. At the same time, when the accelerated update exceeds the stability threshold, revert to the conventional iterative filtering update to reduce the number of iterations and ensure convergence stability. After completing the above adaptive acceleration or revert update, output the robot pose estimation result corresponding to the current laser frame as the localization result of the orchard robot.

[0075] like Figure 5 The flowchart shown is an overall process diagram of the orchard robot adaptive filtering localization method that integrates tree trunk feature perception. The flowchart details the implementation process of the present invention.

[0076] Further, step S1 specifically includes acquiring the 3D LiDAR point cloud data carried by the orchard robot and the high-frequency acceleration and angular velocity data output by the inertial measurement unit; aligning the LiDAR data and the inertial measurement unit data in time using the robot operating system ROS, so that the data from multiple sensors are correlated under the same time reference; using the high-frequency motion information provided by the inertial measurement unit within the laser scanning cycle, calculating the displacement and rotation information of the robot in each laser scanning cycle using a pre-integration scheme; performing motion compensation on the original LiDAR point cloud; and eliminating the distorted point cloud generated by the robot driving on the rugged road surface of the orchard.

[0077] Furthermore, step S2 specifically includes the following steps:

[0078] Step S2.1: The lidar on the orchard robot has a fixed installation height. By selecting laser scanning beams within a preset height range, scanning lines with elevation angles of -15° to -9° are removed to eliminate ground points, reducing the number of candidate point cloud segments to be processed on the tree trunk. The point cloud is traversed along the laser scanning direction in ascending order of azimuth angle, and segmented using a sliding window to obtain a sequence of continuous scanning points on the same beam. ;in, Point clouds obtained from the same line bundle in the order of scanning; the first The points represent the point cloud coordinates in the lidar coordinate system. Calculate the distance from the point to the origin of the lidar coordinate system. for:

[0079] ;

[0080] when At that time, among them and Based on the angular resolution of the lidar and the scale of the orchard tree trunks, a threshold is set to determine whether a point segment is a candidate for a tree trunk. Otherwise, the current point sequence is discarded to eliminate cluttered point clouds that are too close together and incomplete tree trunk point clouds that are far away. Given that tree trunks typically exhibit a regular and continuous arc structure within the laser scanning plane, the distance variation between adjacent points should have smooth and continuous geometric characteristics. Therefore, the radial distance variation rate between adjacent points is defined. As a quantitative indicator:

[0081] ;

[0082] When the candidate tree trunk segment satisfies hour, If the distance change rate threshold is set, the distance change of the current point segment is considered to be smooth and continuous, which conforms to the geometric distribution characteristics of the tree trunk, and it is retained; otherwise, it is discarded, thereby effectively filtering out invalid point segments formed by branch and leaf interference and irregular occlusion.

[0083] Step S2.2: Based on the screening in Step S2.1, further analysis is performed on the candidate tree trunk segments; since the tree trunk outline usually presents a single arc or near-arc shape on the laser scanning plane, and the spatial distribution of adjacent points should be continuous, based on these geometric characteristics, the first-order difference of the distance is used to analyze the segments. A quantitative description of the trend of the current candidate tree trunk segments is provided:

[0084] ;

[0085] In the scanning sequence corresponding to an ideal tree trunk structure, the distance change sequence typically exhibits a single convex or concave trend, with a low number of sign changes; based on this, the number of sign changes is defined. As a criterion for trend consistency:

[0086] ;

[0087] In the formula, As the noise threshold, For indicator functions, As the intersection condition, when If the current segment satisfies the single-peak variation trend of the tree trunk section, then the current segment is considered to meet the characteristics of the single-peak variation trend of the tree trunk section; otherwise, the current segment is regarded as a structurally mixed or shading area and is removed.

[0088] Furthermore, curvature stability constraints are introduced to further filter candidate point segments. By performing second-order difference operations on the ranging sequence, a curvature description sequence is constructed. Its mathematical expression is:

[0089] ;

[0090] In the scanning sequence corresponding to the actual tree trunk structure, its curvature changes smoothly; based on this, the mean and variance of the curvature description sequence are calculated:

[0091] ;

[0092] In the formula This indicates the number of second-order difference terms within the current candidate point segment of the tree trunk;

[0093] When the curvature variance of the candidate trunk segment satisfies hour, The set stable curvature judgment threshold is used to determine whether the current point segment has stable curvature change characteristics in the scanning plane and is judged as the final valid tree trunk feature point segment.

[0094] Step S2.3: Based on the above screening, candidate point segments are only determined as valid tree trunk structure feature point segments and included in the subsequent front-end localization and geometric constraint construction process if they simultaneously satisfy the distance range constraint, spatial continuity constraint, change trend consistency criterion, and curvature stability condition. For common interference situations in orchard operations, including instantaneous pseudo-features formed by wind blowing branches and leaves, incomplete point clouds caused by partial occlusion, and geometrical abrupt changes caused by tilted or forked tree trunks, they are effectively eliminated in the screening process because they are difficult to satisfy the above multiple geometric consistency constraints at the same time.

[0095] Further, in step S3, the extracted tree trunk structural feature points are stored in the tree trunk local feature map, and the distortion-corrected laser point cloud is stored in the surface local feature map. The tree trunk local feature map and the surface local feature map are then divided into voxel-based spatial organization structures to construct a voxel index-based spatial organization structure. When a new laser frame arrives, based on the voxel index relationship, the set of neighboring feature points for the tree trunk feature points and surface feature points corresponding to the current frame is quickly searched in the tree trunk local feature map and the surface local feature map, respectively, to establish the neighborhood information required for the subsequent observation model.

[0096] Further, step S4, based on the local feature map and neighborhood association results, constructs local geometric models for the trunk feature points and surface feature points in the current frame respectively; let the coordinates of the feature points in the current frame in the lidar coordinate system be... The current frame feature points are transformed to the local feature map coordinate system through robot-predicted pose transformation. The distance error from the point to the corresponding local plane is defined as the observation residual, and its expression is:

[0097] ;

[0098] In the formula, and Let these represent the rotation matrix and translation vector of the current frame's laser radar in the world coordinate system. Let be the unit normal vector of the local plane. These are plane offset parameters;

[0099] The geometric residual form from point to plane is uniformly adopted for both tree trunk feature points and surface feature points. Different types of environmental structural information are introduced into the observation model of the front-end localization to describe the deviation relationship between the predicted state and the actual observation. The geometric residual is used as the observation for front-end state estimation and is used to correct the robot pose state during the filtering update process.

[0100] Furthermore, in step S5, based on the observation residual model constructed in step S4, the robot pose state is iteratively estimated in the error state space; specifically, according to the deviation relationship between the predicted state of the inertial measurement unit and the observation residual of the current frame point cloud, the pose state is iteratively updated in multiple rounds, and when the result meets the preset convergence condition, the pose estimation result corresponding to the current laser frame is output.

[0101] Furthermore, step S6 specifically includes the following steps:

[0102] Step S6.1: In the iterative extended Kalman filter update process described in step S5, the error state update process is equivalent to a fixed-point state iteration, characterizing the evolution relationship of the error state in multiple rounds of updates. Its expression is:

[0103] ;

[0104] In the formula, Indicates the first The error state estimate obtained in the next iteration. This represents the state mapping function composed of the observation residuals and the filter update relationship;

[0105] Step S6.2: Based on the above fixed-point state iteration, an accelerated update strategy based on historical iteration information is introduced to optimize the iterative convergence process of the error state; by comprehensively considering the results of multiple historical iterations, the current error state update is weighted and combined to generate the accelerated state update quantity, the expression of which is:

[0106] ;

[0107] In the formula, The weighting coefficient is used to adjust the contribution ratio of historical iteration information in the accelerated update.

[0108] Step S6.3: To achieve adaptive control of the accelerated update mechanism, an accelerated update activation condition is constructed based on the robot's driving state and local environmental structural characteristics in the orchard environment; this is achieved through the robot's current angular velocity information. Characterize the degree of change in motion state, and measure it through the number of local point clouds. The density of the environmental structure is represented; when the robot's motion state changes little and the environmental structure is relatively sparse, a conventional iterative update method is used; when the motion state changes greatly or the environmental point cloud is dense, an accelerated update mechanism based on historical iteration information is enabled; the conditions for enabling accelerated updates are expressed as follows:

[0109] ;

[0110] In the formula, and These are preset angular velocity and point cloud quantity thresholds, respectively;

[0111] By using an adaptive start-stop strategy, iterative acceleration based on historical information can be reasonably applied in the complex environment of the orchard, ensuring positioning stability while avoiding unnecessary computational overhead.

[0112] Step S6.4: While enabling the accelerated update mechanism, introduce stability constraints and a rollback mechanism to ensure the convergence and reliability of the iterative update process; let the... The accelerated error state update amount in the next iteration is: The magnitude of its change compared to the previous update satisfies:

[0113] ;

[0114] In the formula, This is the maximum permissible error change threshold;

[0115] When the conditions are met, the accelerated update result is received; when the conditions are not met, it is considered that the accelerated update will destroy the convergence of the filter, and the system reverts to the regular iterative extended Kalman filter update process; after completing the adaptive update and stability criterion, the current pose estimation result of the robot is output as the final positioning data of the front-end positioning system for subsequent path planning and operation decision-making.

[0116] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0117] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. An adaptive filtering localization method for orchard robots that integrates tree trunk feature perception, characterized in that, Includes the following steps: Step S1: Acquire the high-frequency data of the 3D LiDAR point cloud and the inertial measurement unit, perform time synchronization and motion distortion correction processing on the original LiDAR point cloud data, complete the pre-integration calculation based on the inertial measurement unit results, and obtain the prior motion information for front-end state prediction. Step S2: Utilizing the stable tree structure in the orchard environment, and based on the spatial distribution characteristics of the laser scanning lines, the point cloud processed in Step S1 is segmented and analyzed. Through multiple geometric constraints such as distance continuity, consistency of change trend, and curvature stability, a high-confidence tree trunk feature point cloud that conforms to the trunk cross-section characteristics is extracted. Step S3: Store the trunk feature points extracted in step S2 and the synchronously obtained surface feature points in the local feature map, and divide them into voxels to construct a spatial search structure based on voxel index; when the current laser frame arrives, according to the voxel index relationship, quickly find the corresponding set of neighborhood feature points for the trunk feature points and surface feature points of the current frame in the historical local feature map, which is used for the construction of subsequent observation models and registration residuals; Step S4: Based on the spatial organization and neighborhood association of feature points completed in step S3, perform geometric modeling on the trunk feature points and surface feature points in the current frame, fit the local planar model using the corresponding neighborhood feature information, and use the distance error from the point to the plane as a unified observation residual to describe the deviation relationship between the current observation and the prediction state. Step S5: Based on the observation residual model constructed in step S4, perform observation iteration update on the predicted state in the error state space, and perform multiple rounds of correction on the robot pose state according to the observation residual until the preset convergence condition is met, and output the pose estimation result corresponding to the current laser frame. Step S6: Treat the iterative update process in Step S5 as a fixed-point state iteration. Introduce an accelerated update mechanism based on historical iteration information in the error state space to optimize the iterative convergence process of the pose state. Based on the current driving state of the orchard robot and the environmental structure characteristics, adaptively control the start and stop of the acceleration strategy and the acceleration order. Enable acceleration in turning or dense tree conditions, and disable acceleration in straight-line conditions. At the same time, when the accelerated update exceeds the stability threshold, revert to the conventional iterative filtering update to reduce the number of iterations and ensure convergence stability. After completing the above adaptive acceleration or revert update, output the robot pose estimation result corresponding to the current laser frame as the localization result of the orchard robot.

2. The orchard robot adaptive filtering localization method based on fusion of tree trunk feature perception according to claim 1, characterized in that, Step S1 specifically includes acquiring the 3D LiDAR point cloud data carried by the orchard robot and the high-frequency acceleration and angular velocity data output by the inertial measurement unit. The LiDAR data and the inertial measurement unit data are time-aligned through the robot operating system ROS, so that the data from multiple sensors are correlated under the same time reference. Using the high-frequency motion information provided by the inertial measurement unit within the laser scanning cycle, the displacement and rotation information of the robot in each laser scanning cycle are calculated using a pre-integration scheme. Motion compensation is performed on the original LiDAR point cloud to eliminate the distorted point cloud generated when the robot travels on the rugged road surface of the orchard.

3. The orchard robot adaptive filtering localization method based on fusion of tree trunk feature perception according to claim 1, characterized in that, Step S2 specifically includes the following steps: Step S2.1: The lidar on the orchard robot has a fixed installation height. By selecting laser scanning beams within a preset height range, scanning lines with elevation angles of -15° to -9° are removed to eliminate ground points, reducing the number of candidate point cloud segments to be processed on the tree trunk. The point cloud is traversed along the laser scanning direction in ascending order of azimuth angle, and segmented using a sliding window to obtain a sequence of continuous scanning points on the same beam. ;in, Point clouds obtained from the same line bundle in the order of scanning; the first The points represent the point cloud coordinates in the lidar coordinate system. Calculate the distance from the point to the origin of the lidar coordinate system. for: ； when At that time, among them and Based on the angular resolution of the lidar and the scale of the orchard tree trunks, a threshold is set to determine whether a point segment is a candidate for a tree trunk. Otherwise, the current point sequence is discarded to eliminate cluttered point clouds that are too close together and incomplete tree trunk point clouds that are far away. Given that tree trunks typically exhibit a regular and continuous arc structure within the laser scanning plane, the distance variation between adjacent points should have smooth and continuous geometric characteristics. Therefore, the radial distance variation rate between adjacent points is defined. As a quantitative indicator: ； When the candidate tree trunk segment satisfies hour, If the distance change rate threshold is set, the distance change of the current point segment is considered to be smooth and continuous, which conforms to the geometric distribution characteristics of the tree trunk, and it is retained; otherwise, it is discarded, thereby effectively filtering out invalid point segments formed by branch and leaf interference and irregular occlusion. Step S2.2: Based on the screening in Step S2.1, further analysis is performed on the candidate tree trunk segments; since the tree trunk outline usually presents a single arc or near-arc shape on the laser scanning plane, and the spatial distribution of adjacent points should be continuous, based on these geometric characteristics, the first-order difference of the distance is used to analyze the segments. A quantitative description of the trend of the current candidate tree trunk segments is provided: ； In the scanning sequence corresponding to an ideal tree trunk structure, the distance change sequence typically exhibits a single convex or concave trend, with a low number of sign changes; based on this, the number of sign changes is defined. As a criterion for trend consistency: ； In the formula, As the noise threshold, For indicator functions, As the intersection condition, when If the current segment satisfies the single-peak variation trend of the tree trunk section, then the current segment is considered to meet the characteristics of the single-peak variation trend of the tree trunk section; otherwise, the current segment is regarded as a structurally mixed or shading area and is removed. Furthermore, curvature stability constraints are introduced to further filter candidate point segments. By performing second-order difference operations on the ranging sequence, a curvature description sequence is constructed. Its mathematical expression is: ； In the scanning sequence corresponding to the actual tree trunk structure, its curvature changes smoothly; based on this, the mean and variance of the curvature description sequence are calculated: ； In the formula This indicates the number of second-order difference terms within the current candidate point segment of the tree trunk; When the curvature variance of the candidate trunk segment satisfies hour, The set stable curvature judgment threshold is used to determine whether the current point segment has stable curvature change characteristics in the scanning plane and is judged as the final valid tree trunk feature point segment. Step S2.3: Based on the above screening, candidate point segments are only determined as valid tree trunk structure feature point segments and included in the subsequent front-end localization and geometric constraint construction process if they simultaneously satisfy the distance range constraint, spatial continuity constraint, change trend consistency criterion, and curvature stability condition. For common interference situations in orchard operations, including instantaneous pseudo-features formed by wind blowing branches and leaves, incomplete point clouds caused by partial occlusion, and geometrical abrupt changes caused by tilted or forked tree trunks, they are effectively eliminated in the screening process because they are difficult to satisfy the above multiple geometric consistency constraints at the same time.

4. The orchard robot adaptive filtering localization method based on fusion of tree trunk feature perception according to claim 1, characterized in that, Step S3 stores the extracted tree trunk structural feature points in the tree trunk local feature map and the distortion-corrected laser point cloud in the area local feature map. The tree trunk local feature map and the area local feature map are then divided into voxel-based spatial organization structures to construct a voxel index-based spatial organization structure. When a new laser frame arrives, based on the voxel index relationship, the set of neighboring feature points for the tree trunk feature points and area feature points corresponding to the current frame is quickly searched in the tree trunk local feature map and the area local feature map, respectively, to establish the neighborhood information required for subsequent observation models.

5. The orchard robot adaptive filtering localization method based on trunk feature perception according to claim 1, characterized in that, Step S4, based on the local feature map and neighborhood association results, constructs local geometric models for the trunk feature points and surface feature points in the current frame; let the coordinates of the feature points in the current frame in the lidar coordinate system be... The current frame feature points are transformed to the local feature map coordinate system through robot-predicted pose transformation. The distance error from the point to the corresponding local plane is defined as the observation residual, and its expression is: ； In the formula, and Let these represent the rotation matrix and translation vector of the current frame's laser radar in the world coordinate system. Let be the unit normal vector of the local plane. These are plane offset parameters; The geometric residual form from point to plane is uniformly adopted for both tree trunk feature points and surface feature points. Different types of environmental structural information are introduced into the observation model of the front-end localization to describe the deviation relationship between the predicted state and the actual observation. The geometric residual is used as the observation for front-end state estimation and is used to correct the robot pose state during the filtering update process.

6. The orchard robot adaptive filtering localization method based on trunk feature perception according to claim 1, characterized in that, In step S5, based on the observation residual model constructed in step S4, the robot pose state is iteratively estimated in the error state space; specifically, according to the deviation relationship between the predicted state of the inertial measurement unit and the observation residual of the current frame point cloud, the pose state is iteratively updated in multiple rounds, and when the result meets the preset convergence condition, the pose estimation result corresponding to the current laser frame is output.

7. The orchard robot adaptive filtering localization method based on fusion of tree trunk feature perception according to claim 1, characterized in that, Step S6 specifically includes the following steps: Step S6.1: In the iterative extended Kalman filter update process described in step S5, the error state update process is equivalent to a fixed-point state iteration, characterizing the evolution relationship of the error state in multiple rounds of updates. Its expression is: ； In the formula, Indicates the first The error state estimate obtained in the next iteration. This represents the state mapping function composed of the observation residuals and the filter update relationship; Step S6.2: Based on the above fixed-point state iteration, an accelerated update strategy based on historical iteration information is introduced to optimize the iterative convergence process of the error state; By synthesizing the results of multiple historical iterations, the current error state update is weighted and combined to generate an accelerated state update quantity, the expression of which is: ； In the formula, The weighting coefficient is used to adjust the contribution ratio of historical iteration information in the accelerated update. Step S6.3: To achieve adaptive control of the accelerated update mechanism, construct the accelerated update activation judgment condition based on the robot's driving state and local environmental structure characteristics in the orchard environment; Based on the robot's current angular velocity information Characterize the degree of change in motion state, and measure it through the number of local point clouds. The density of the environmental structure is represented; when the robot's motion state changes little and the environmental structure is relatively sparse, a conventional iterative update method is used; when the motion state changes greatly or the environmental point cloud is dense, an accelerated update mechanism based on historical iteration information is enabled; the conditions for enabling accelerated updates are expressed as follows: ； In the formula, and These are preset angular velocity and point cloud quantity thresholds, respectively; By using an adaptive start-stop strategy, iterative acceleration based on historical information can be reasonably applied in the complex environment of the orchard, ensuring positioning stability while avoiding unnecessary computational overhead. Step S6.4: While enabling the accelerated update mechanism, introduce stability constraints and a rollback mechanism to ensure the convergence and reliability of the iterative update process; let the... The accelerated error state update amount in the next iteration is: The magnitude of its change compared to the previous update satisfies: ； In the formula, This is the maximum permissible error change threshold; When the conditions are met, the accelerated update result is received; when the conditions are not met, it is considered that the accelerated update will destroy the filter convergence, and the system reverts to the regular iterative extended Kalman filter update process. After completing the adaptive update and stability criterion, the robot's current pose estimation result is output as the final positioning data of the front-end positioning system for subsequent path planning and operation decision-making.

Images