Research on Orchard Navigation Technology Based on Improved LIO-SAM Algorithm

Abstract

To address the challenges in unstructured orchard environments, including high geometric similarity between fruit trees (with the measured average Euclidean distance difference between point cloud descriptors of adjacent trees being less than 0.5 m), significant dynamic interference (e.g., interference from pedestrians or moving equipment can occur every 5 min), and uneven terrain, this paper proposes an improved mapping algorithm named OSC-LIO (Orchard Scan Context Lidar Inertial Odometry via Smoothing and Mapping). The algorithm designs a dynamic point filtering strategy based on Euclidean clustering and spatiotemporal consistency within a 5-frame sliding window to reduce the interference of dynamic objects in point cloud registration. By integrating local semantic features such as fruit tree trunk diameter and canopy height difference, a two-tier verification mechanism combining “global and local information” is constructed to enhance the distinctiveness and robustness of loop closure detection. Motion compensation is achieved by fusing data from an Inertial Measurement Unit (IMU) and a wheel odometer to correct point cloud distortion. A three-level hierarchical indexing structure—”path partitioning, time window, KD-Tree (K-Dimension Tree)”—is built to reduce the time required for loop closure retrieval and improve the system’s real-time performance. Experimental results show that the improved OSC-LIO system reduces the Absolute Trajectory Error (ATE) by approximately 23.5% compared to the original LIO-SAM (Tightly coupled Lidar Inertial Odometry via Smoothing and Mapping) in a simulated orchard environment, while enabling stable and reliable path planning and autonomous navigation. This study provides a high-precision, lightweight technical solution for autonomous navigation in orchard scenarios.

1. Introduction

In recent years, with the rapid development of precision agriculture and smart agriculture, agricultural robots have gradually become a key technology for improving orchard operation efficiency and reducing labor costs [1,2]. Orchard navigation, as a core component of autonomous agricultural robot operations, places particularly high demands on high-precision and robust localization and mapping technologies. However, traditional navigation methods perform poorly in unstructured, highly similar environments such as orchards, which limits the large-scale application of robots. Therefore, this study aims to propose a lightweight, high-precision navigation algorithm suitable for orchard environments, with the goal of promoting the practical implementation of agricultural robots in real-world production.

With the rapid development of autonomous driving and agricultural robotics technology, Simultaneous Localization and Mapping (SLAM) [3] has become a prominent research focus, evolving from early probabilistic frameworks into robust perception systems capable of handling large-scale and complex environments [4]. Depending on the primary sensors employed, SLAM can be broadly categorized into two major classes: LiDAR-based SLAM and vision-based SLAM. A typical SLAM framework generally comprises four key modules: data acquisition and preprocessing, front-end odometry estimation, back-end optimization, and loop closure detection. Among these, loop closure detection serves as a critical component of the SLAM system [5], effectively eliminating accumulated errors, enhancing localization accuracy, and improving map quality, making it particularly essential for SLAM systems operating in large-scale, loop-rich, and long-duration environments [6]. Currently, mainstream loop closure detection methods can be broadly divided into two categories: descriptor-based methods and deep learning-based methods. In the agricultural domain, particularly when applying SLAM in unstructured orchard environments, unique challenges such as perceptual aliasing (e.g., high geometric similarity between trees) and dynamic interference arise. Recent surveys on navigation in unstructured agricultural environments highlight that achieving robust navigation in GPS-denied, densely vegetated settings remains a significant open challenge [7]. This context provides direct motivation for this study to develop a lightweight, high-precision navigation algorithm specifically tailored for orchard scenarios.

1.1. Descriptor-Based Loop Closure Detection Methods

In 2016, He et al. [8] proposed a global descriptor for LiDAR point clouds based on multi-scale 2D projection. This method extracts shape context features by projecting 3D point clouds onto 2D planes at different scales and encodes them into a global descriptor for loop closure detection. However, due to the need for repeated computation at multiple resolutions and the high dimensionality of the features, this approach suffers from high computational complexity and long processing times. In 2018, Kim et al. [9] introduced a method based on Scan Context (SC), which compresses 3D point clouds into a 2D planar representation. While effective, this method primarily utilizes maximum height information and does not fully exploit other features of the point clouds. Subsequently, Kim et al. [10] further proposed Scan Context++ on the basis of Scan Context to address the issue of “excessive false-positive loop closures.” In 2020, Wang et al. [11] proposed the LiDAR Iris (LI) method, which extracts binary signature images from each point cloud frame and calculates the Hamming distance between these images to assess point cloud consistency. Following this, Wang et al. [12] integrated reflectivity information into the Scan Context framework, introducing the Intensity Scan Context (ISC) descriptor to enhance the ability to distinguish subtle features in point clouds. In 2023, Ma Zhewei et al. [13] combined LiDAR and IMU data with the Scan Context method, improving the speed of loop closure detection and the accuracy of localization. Additionally, in 2024, Xu Xiaosu et al. [14] proposed a loop closure detection method based on triangular bag-of-words. This method generates triangular descriptors using the LinK3D features of point clouds and achieves precise relative pose estimation by constructing triangular bag-of-words and performing inter-frame matching based on LinK3D features. Overall, these methods mainly rely on the geometric structure and local intensity features of point clouds. When point clouds exhibit significant differences due to viewpoint changes or occlusion, their recognition performance and robustness can be easily compromised [10,11].

In 2018, Shan et al. [15] proposed an efficient and lightweight real-time localization and mapping algorithm (LEGO-LOAM). In 2020, Shan et al. [16] further developed a tightly coupled LiDAR-inertial SLAM algorithm (LIO-SAM) based on this framework. Both algorithms rely on geometric distance thresholds to determine the occurrence of loop closures. However, when returning to a previously visited location after long-distance travel, accumulated odometry errors can cause the geometric distance-based detection method to fail, leading to missed loop closures.

1.2. Deep Learning-Based Loop Closure Detection Methods

To explore more effective methods for characterizing point cloud features, numerous research approaches based on neural networks have emerged in recent years. In 2018, Uy et al. [17] combined PointNet [18] and PointNetVLAD [19] to propose an end-to-end network architecture capable of directly extracting global descriptors from 3D point clouds. In 2019, Lu et al. [20] selected key points from point clouds and utilized PointNet to learn local descriptors corresponding to each key point. In the same year, Schaupp et al. [21] introduced the OREOS method, which projects 3D point clouds into 2D range images and employs convolutional neural networks to extract global descriptors. In 2020, Yin et al. [22] proposed SeqSphereVLAD, a method that projects point clouds onto spherical views for feature extraction and sorts the feature sequences to generate descriptors. Also in 2020, Chang et al. [23] presented SpoxelNet, which voxelizes point clouds based on a spherical coordinate system and integrates multi-directional features, significantly improving recognition performance in complex scenes. In 2021, Komorowski et al. [24] proposed Minkloc3D, which adopts a sparse voxelization approach to represent point clouds and extracts highly discriminative feature descriptors through sparse 3D convolution. However, such deep learning methods typically demand high computational hardware requirements, often suffer from limited real-time performance, and their generalization capabilities are prone to being influenced by the distribution of training data.

1.3. Research Motivation and Main Work

In unstructured orchard environments, fruit trees exhibit high geometric similarity (with an average similarity of point cloud descriptors between adjacent trees >0.85) and essentially identical reflection intensity. Descriptor-based methods, which primarily rely on local feature information such as point cloud geometry and intensity, result in low distinctiveness between descriptors, leading to false loop closure detections. On the other hand, deep learning-based methods demand high computational power, suffer from real-time limitations due to hardware constraints, and their accuracy depends heavily on the deep learning framework. To address these challenges, this paper proposes an improved algorithm named OSC-LIO, which integrates semantic features and a dynamic point filtering mechanism. The main contributions are as follows:

• Designed a lightweight dynamic point filtering mechanism based on Euclidean clustering and spatiotemporal consistency (with a time complexity of O(n), a single-frame processing time of <20 ms, and a memory footprint of <50 MB). This method determines static or dynamic clusters by matching cluster centroids within a sliding window, reducing the impact of dynamic interference on point cloud registration.
• Introduced local semantic features such as fruit tree trunk diameter and canopy height difference to complement the global descriptor. Established a dual-layer verification mechanism combining “global and local information” to enhance the distinctiveness between different fruit trees and effectively filter out false positive loop closures.
• Achieved point cloud distortion compensation caused by robot jitter through the fusion of IMU(200HZ) and wheel odometer data(20HZ), calculating quaternion attitudes and con-structing rotation matrices. This provides a consistent spatial reference for generating global descriptors.
• Constructed a three-level hierarchical indexing structure—”path partitioning, time window, KD-Tree”—that leverages the structured characteristics of orchard paths to reduce the number of candidate frames, thereby improving real-time performance and retrieval efficiency in large-scale orchard scenarios.
• Experimentally validated the accuracy of the improved system’s laser odometry in a simulated orchard environment and developed an orchard navigation system. Integrated with the ROS Autoware framework to achieve autonomous navigation.

2. Materials and Methods

To address the unstructured nature of orchard environments, OSC-LIO introduces the following improvements based on LIO-SAM: a dynamic point filtering mechanism is added to the point cloud processing layer to mitigate dynamic interference; local orchard semantic features are incorporated into the loop closure detection module, establishing a “global + local information” two-layer verification mechanism to enhance detection accuracy; and a three-tier hierarchical indexing structure is designed for the retrieval process to optimize real-time performance. The overall framework is illustrated in Figure 1.

2.1. Lightweiht Dynamic Point Filtering Mechanism

In orchard environments, interference factors such as pedestrians and dynamic objects may exist. Recent research on LiDAR-inertial odometry has proposed various front-end strategies for dynamic object removal [25]. The dynamic points generated by these factors can adversely affect loop closure detection. Removing dynamic points helps reduce their interference with point cloud registration, thereby improving the accuracy and stability of loop closure detection. Simultaneously, the removal of dynamic points can also reduce the volume of point cloud data to some extent, enhancing the operational efficiency of the algorithm. To mitigate the impact of dynamic points, this paper proposes a lightweight dynamic point filtering method based on Euclidean clustering and spatiotemporal consistency detection. The specific process is illustrated in Figure 2:

First, the raw point cloud set $\mathrm{P}=\{{\mathrm{p}}_1,{\mathrm{p}}_2,\dots ,{\mathrm{p}}_{\mathrm{k}}\}$ is input, where each point ${\mathrm{p}}_{\mathrm{i}}=\left({\mathrm{x}}_{\mathrm{i}},{\mathrm{y}}_{\mathrm{i}},{\mathrm{z}}_{\mathrm{i}}\right)$ represents a spatial position. The neighborhood of point ${\mathrm{p}}_{\mathrm{i}}$ is defined as the set of points whose Euclidean distance is less than a threshold $ϵ$:

$$\mathcal{N}\left({\mathrm{p}}_{\mathrm{i}}\right)=\left\{{\mathrm{p}}_{\mathrm{j}}\in \mathrm{P}∣\parallel {\mathrm{p}}_{\mathrm{i}}-{\mathrm{p}}_{\mathrm{j}}\parallel \le \mathsf{ϵ}\right\}$$

(1)

For any two points ${\mathrm{p}}_{\mathrm{i}}=\left({\mathrm{x}}_{\mathrm{i}},{\mathrm{y}}_{\mathrm{i}},{\mathrm{z}}_{\mathrm{i}}\right)$ and ${\mathrm{p}}_{\mathrm{j}}=\left({\mathrm{x}}_{\mathrm{j}},{\mathrm{y}}_{\mathrm{j}},{\mathrm{z}}_{\mathrm{j}}\right)$, in the point set, their three-dimensional Euclidean distance is defined as:

$$\parallel {\mathrm{p}}_{\mathrm{i}}-{\mathrm{p}}_{\mathrm{j}}\parallel =\sqrt{{\left({\mathrm{x}}_{\mathrm{i}}-{\mathrm{x}}_{\mathrm{j}}\right)}^2+{\left({\mathrm{y}}_{\mathrm{i}}-{\mathrm{y}}_{\mathrm{j}}\right)}^2+{\left({\mathrm{z}}_{\mathrm{i}}-{\mathrm{z}}_{\mathrm{j}}\right)}^2}$$

(2)

If $\parallel {\mathrm{p}}_{\mathrm{i}}-{\mathrm{p}}_{\mathrm{j}}\parallel \le \mathsf{ϵ}$, the two points are considered “spatially adjacent” and may belong to the same cluster. Subsequently, a connectivity-based clustering algorithm using Euclidean distance (standard Euclidean clustering) is applied to partition the point set $\mathrm{P}=\{{\mathrm{p}}_1,{\mathrm{p}}_2,\dots ,{\mathrm{p}}_{\mathrm{k}}\}$ into several mutually disjoint subsets (clusters) ${\mathrm{C}}_1,{\mathrm{C}}_2,\dots ,{\mathrm{C}}_{\mathrm{k}}$. This partition must satisfy that every point belongs to exactly one cluster ${\mathrm{U}}_{\mathrm{i}=1}^{\mathrm{n}}{\mathrm{C}}_{\mathrm{i}}=\mathrm{P}$ and ${\mathrm{C}}_{\mathrm{i}}\cap {\mathrm{C}}_{\mathrm{j}}=\mathrm{\varnothing }\left(\mathrm{i}\ne \mathrm{j}\right)$. Each cluster fulfills the following condition: any two points within the same cluster can be connected by a sequence of points such that the Euclidean distance between consecutive points along the path is less than the threshold $\mathsf{ϵ}$, whereas for any two points belonging to different clusters, their distance is greater than $\mathsf{ϵ}$ or they cannot be connected within the distance $\mathsf{ϵ}$.

Next, the two-dimensional centroid of each cluster is calculated. The centroid coordinates are computed using the formula:

$${\overline{\mathrm{c}}}_{\mathrm{k}}=\left(\begin{array}{c}{\displaystyle \frac{1}{\mathrm{N}}}{\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{N}}}{\mathrm{x}}_{\mathrm{i}},{\displaystyle \frac{1}{\mathrm{N}}}{\displaystyle \sum _{\mathrm{i}=1}^{\mathrm{N}}}{\mathrm{y}}_{\mathrm{i}}\end{array}\right)$$

(3)

Finally, a sliding window is established, storing the most recent five frames of point clouds as historical point cloud frames. For the centroid ${\overline{\mathrm{c}}}_{\mathrm{k}}$, of a cluster in the current point cloud frame, a KD-Tree is used to search for the nearest neighbor of the corresponding cluster centroid in each historical point cloud frame based on 2D Euclidean distance. If a nearest neighbor exists in all historical frames and the distance is less than 0.5 m (a threshold determined experimentally and suitable for scenarios with speeds below 1 m/s), the cluster is classified as static; otherwise, it is classified as dynamic. Subsequently, all points belonging to static clusters are retained, while points from dynamic clusters are filtered out. The current point cloud frame is then added to the buffer. If the buffer size exceeds the window capacity, the earliest frame is removed to ensure the timeliness of the sliding window. After filtering, dynamic objects in the point cloud are effectively removed, significantly enhancing the stability of point cloud matching and mapping.

A comparative visualization of the point cloud before and after dynamic point filtering is shown in Figure 3. The figure intuitively demonstrates the effect of the proposed dynamic point filtering mechanism: the left image shows the raw point cloud data without filtering, which includes dynamic interference points represented by a pedestrian. Such dynamic points can adversely affect subsequent point cloud registration, mapping, and navigation tasks. The right image displays the point cloud data after processing by the dynamic point filtering strategy, where the dynamic interference points corresponding to the pedestrian have been effectively removed, resulting in a clearer representation of the static structural features of the point cloud scene. The comparative results in Figure 3 validate the effectiveness of the proposed dynamic point filtering mechanism, providing high-quality, dynamic-interference-free point cloud data support for the subsequent mapping and autonomous navigation tasks of the SLAM system.

2.2. “Global and Local Information” Two-Layer Verification Mechanism

The core of the “global information + local information” two-layer verification mechanism is to achieve precise loop closure determination by fusing the Scan Context global descriptor with local orchard semantic features. This mechanism essentially enhances the global geometric descriptor through the incorporation of local semantic features. This strategy of utilizing semantic information to improve the distinctiveness and robustness of place recognition in challenging, easily confused environments has been recognized as a promising research direction [26]. The specific workflow is as follows: First, the global descriptor of the current frame, local features such as trunk diameter and canopy height difference, and corresponding data from historical frames are taken as input. Leveraging the structured layout of orchard paths, the path is divided into sectors at 5 m intervals. Only historical frames located in the same or adjacent sectors as the current frame, excluding the ten most recent frames, are retained as candidate frames to reduce computational load. Next, the first layer of global descriptor matching is performed. An initial sector offset is calculated based on the IMU heading angle difference, and the optimal shift alignment between the two frames’ descriptors is searched within a range of five sectors forward and backward. Candidate frames with globally similar scenes are screened by computing the normalized Euclidean distance. Then, the second layer of local semantic feature matching is conducted. The mean difference of trunk diameter sequences and the weighted mean difference of canopy height difference sequences are calculated, and these are combined to derive a local feature distance, quantifying semantic similarity. Finally, a weighted strategy incorporating both the global distance and local feature distance is used to compute the total distance. If the total distance is less than the default threshold and is the minimum among all candidate frames, it is identified as a valid loop closure. The loop closure index and relative pose are output, forming a dual guarantee of “global coarse screening + local fine verification.” This approach not only ensures overall scene consistency but also enhances the ability to discern subtle differences, effectively reducing the false positive rate of loop closures in orchard environments.

2.2.1. Construction of the Scan Context Global Descriptor

Scan Context is currently one of the most widely used loop closure detection methods and can be easily integrated into existing LiDAR-based SLAM algorithm frameworks. Its core concept is to structure a 3D point cloud scene into a two-dimensional global descriptor with rotation invariance, as illustrated in Figure 4. The specific implementation process is as follows: First, a cylindrical coordinate system is constructed with the sensor (or robot) position as the origin. Next, the surrounding space is divided into several concentric rings along the radial direction (horizontal distance) and into several sector divisions along the azimuthal direction, thereby forming a regular two-dimensional grid matrix akin to “rings and sectors.” Subsequently, each point in the 3D point cloud is mapped to the corresponding grid cell based on its spatial coordinates, and only the maximum height value of all points within each cell is extracted as the key feature of that grid. Through this step, the 3D point cloud is effectively compressed into a two-dimensional matrix that represents the global height distribution of the scene.

2.2.2. Semantic Feature Extraction

In unstructured orchard environments, the lack of distinct structural diversity means that relying solely on the maximum height information of point clouds is prone to false positives and missed detections during loop closure. To address this issue, this paper incorporates local orchard features—such as trunk diameter and canopy height difference—as supplementary semantic information to assist the Scan Context descriptor in loop closure detection. The extraction process of these local orchard features is illustrated in Figure 5.

First, height threshold filtering is applied based on the target fruit tree type (calibrated for apple trees in this study; thresholds are adjustable for other species). For any point $\mathrm{p}=\left(\mathrm{x},\mathrm{y},\mathrm{z}\right)$, in the point cloud, if its height satisfies $0.2<z_1<2$, it is classified as a trunk point. If its height satisfies $2<z_2<5$, it is classified as a canopy point. Then, Euclidean clustering is performed on the classified trunk points and canopy points, respectively, outputting multiple trunk clusters ${\mathrm{C}}_1,{\mathrm{C}}_2,\dots ,{\mathrm{C}}_{\mathrm{k}}$ and canopy clusters ${\mathrm{B}}_1,{\mathrm{B}}_2,\dots ,{\mathrm{B}}_{\mathrm{k}}$.

Subsequently, a circle is fitted to the trunk cross-section using the least squares method. The points of the trunk cluster ${\mathrm{C}}_{\mathrm{k}}$ are projected onto the xy−plane, resulting in a two−dimensional point set ${\mathrm{Q}}_{\mathrm{k}}=\{\left({\mathrm{x}}_{\mathrm{k}1},{\mathrm{y}}_{\mathrm{k}1}\right),\left({\mathrm{x}}_{\mathrm{k}2},{\mathrm{y}}_{\mathrm{k}2}\right),...,\left({\mathrm{x}}_{\mathrm{k}\mathrm{n}},{\mathrm{y}}_{\mathrm{k}\mathrm{n}}\right)\}$. The algebraic equation of the circle is established as follows:

$${\mathrm{x}}^2+{\mathrm{y}}^2+\mathrm{A}\mathrm{x}+\mathrm{B}\mathrm{y}+\mathrm{C}=0$$

(4)

where the coordinates of the circle center are $(\mathrm{a},\mathrm{b})=\left(-{\displaystyle \frac{\mathrm{A}}{2}},-{\displaystyle \frac{\mathrm{B}}{2}}\right)$, and the radius is $\mathrm{r}=\sqrt{{\left({\displaystyle \frac{\mathrm{A}}{2}}\right)}^2+{\left({\displaystyle \frac{\mathrm{B}}{2}}\right)}^2-\mathrm{C}}$. For a point $({\mathrm{x}}_{\mathrm{i}},{\mathrm{y}}_{\mathrm{i}})\in {\mathrm{Q}}_{\mathrm{k}}$ in the point set, the residual is obtained by substituting it into the equation:

$${\mathrm{e}}_{\mathrm{i}}={\mathrm{x}}_{\mathrm{i}}^2+{\mathrm{y}}_{\mathrm{i}}^2+\mathrm{A}{\mathrm{x}}_{\mathrm{i}}+\mathrm{B}{\mathrm{y}}_{\mathrm{i}}+\mathrm{C}$$

(5)

The center coordinates and radius are solved by minimizing the sum of squared errors, ultimately yielding the trunk diameter. For inclined or non-ideal cylindrical trunks, principal axis alignment is first performed, and the point cloud is then projected onto the fitted plane. Subsequently, a circle (or ellipse) is fitted using the least-squares method to estimate the trunk diameter. For canopy feature extraction, the canopy point set is traversed to extract all height values, from which the maximum and minimum values are identified. The difference between these two values is calculated as the canopy height difference. The local features are visualized in Figure 6, which presents the graphical representation of these features. Each circle in the figure represents a clustered fruit-tree unit, with differently colored numerical values adjacent to the circles accurately labeling the trunk diameter of each tree. The green points and accompanying green numerical values characterize the canopy height-difference information.

2.2.3. Multi-Sensor Based Motion Compensation

When orchard robots operate on uneven terrain, point cloud distortion occurs due to bumps and body tilting. The core of motion compensation lies in fusing multi-sensor data from IMU and wheel odometers to transform the point cloud from the “dynamic robot coordinate system” to the “static ideal world coordinate system,” thereby eliminating motion interference. Essentially, it estimates the robot’s pose deviation based on a kinematic model and corrects the point cloud positions through coordinate transformation.

The motion compensation for orchard robots primarily relies on IMU and wheel odometer data. The pitch angle $\mathsf{\alpha }$, roll angle $\mathsf{\beta }$ and yaw angle $\mathsf{\theta }$, of the robot are calculated by integrating the angular velocity from the IMU to correct tilt deviations. The angular velocity $\mathsf{\omega }$ output by the IMU reflects the robot’s rotational rate, and the attitude is represented using quaternions. The quaternion update formula is as follows:

$$\dot{q}={\displaystyle \frac{1}{2}}q\otimes \left[\begin{array}{c}0\\ \omega \end{array}\right]={\displaystyle \frac{1}{2}}\left[\begin{array}{c}-q_v^T\\ q_{w}I+[q_v{]}_{\times }\end{array}\right]\omega$$

(6)

where $q=[q_w,q_v{]}^{\mathsf{T}}$ is the attitude quaternion (with $q_w$ being the scalar part and $q_v$ the vector part), $\omega =({\omega }_x,{\omega }_y,{\omega }_z{)}^{\mathsf{T}}$ denotes the angular velocity vector expressed in the body frame, $\otimes$ represents quaternion multiplication, and $[q_v{]}_{\times }$ indicates the skew-symmetric matrix form of a vector.

The pitch angle $\mathsf{\alpha }$ and roll angle $\mathsf{\beta }$ of the robot body are calculated from the quaternion using the following formulas:

$$\alpha =arcsin\left(-2\left(q_1{q}_3-q_0{q}_2\right)\right)$$

(7)

$$\mathsf{\beta }=\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}2\left(2\left({\mathrm{q}}_0{\mathrm{q}}_1+{\mathrm{q}}_2{\mathrm{q}}_3\right),1-2\left({\mathrm{q}}_1^2+{\mathrm{q}}_2^2\right)\right)$$

(8)

The translational distance ${\mathrm{d}}_{\mathrm{x}\mathrm{y}}$ is calculated by combining wheel odometer data, while the vertical bump distance ${\mathrm{d}}_{\mathrm{z}}$ along the z-axis is obtained by integrating the vertical acceleration from the IMU. The calculation formulas are as follows:

$${\mathrm{d}}_{\mathrm{x}\mathrm{y}}={\displaystyle \frac{{\mathrm{v}}_{\mathrm{L}}+{\mathrm{v}}_{\mathrm{R}}}{2}}\cdot \mathsf{\Delta }\mathrm{t};{\mathrm{d}}_{\mathrm{z}}={\displaystyle \frac{1}{2}}{\mathrm{a}}_{\mathrm{z}}\cdot \mathsf{\Delta }{\mathrm{t}}^2$$

(9)

where the left and right wheel rotational speeds are denoted as ${\mathrm{v}}_{\mathrm{L}}$ and ${\mathrm{v}}_{\mathrm{R}}$ Irespectively, the IMU vertical acceleration is ${\mathrm{a}}_{\mathrm{z}}$ and $\mathsf{\Delta }\mathrm{t}$ epresents the time interval;

The translation vector used to correct the displacement is denoted as $\mathrm{t}=\left({\mathrm{t}}_{\mathrm{x}},{\mathrm{t}}_{\mathrm{y}},{\mathrm{t}}_{\mathrm{z}}\right)$:

$${\mathrm{t}}_{\mathrm{x}}={\mathrm{d}}_{\mathrm{x}\mathrm{y}}\cdot \mathrm{c}\mathrm{o}\mathrm{s}\mathsf{\theta },{\mathrm{t}}_{\mathrm{y}}={\mathrm{d}}_{\mathrm{x}\mathrm{y}}\cdot \mathrm{s}\mathrm{i}\mathrm{n}\mathsf{\theta },{\mathrm{t}}_{\mathrm{z}}={\mathrm{d}}_{\mathrm{z}}$$

(10)

where the heading angle $\mathsf{\theta }$ is derived from the yaw angle estimated by the IMU.

The tilt of the body around the x-axis $\left(\mathsf{\alpha }\right)$ and y-axis $\left(\mathsf{\beta }\right)$ is corrected using the rotation matrix $\mathrm{R}$. This involves first rotating around the x-axis to correct the pitch angle $\mathsf{\alpha }$, followed by rotating around the y-axis to correct the roll angle $\mathsf{\beta }$, The rotation matrix is the product of these two rotations:

$$\mathrm{R}={\mathrm{R}}_{\mathrm{y}}\left(\mathsf{\beta }\right)\cdot {\mathrm{R}}_{\mathrm{x}}\left(\mathsf{\alpha }\right)$$

(11)

Among them, the x-axis rotation matrix (correcting pitch angle $\mathsf{\alpha }$) is ${\mathrm{R}}_{\mathrm{x}}\left(\mathsf{\alpha }\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \mathrm{c}\mathrm{o}\mathrm{s}\mathsf{\alpha }& -\mathrm{s}\mathrm{i}\mathrm{n}\mathsf{\alpha }\\ 0& \mathrm{s}\mathrm{i}\mathrm{n}\mathsf{\alpha }& \mathrm{c}\mathrm{o}\mathrm{s}\mathsf{\alpha }\end{array}\right]$, and the y−axis rotation matrix (correcting roll angle $\mathsf{\beta }$) is ${\mathrm{R}}_{\mathrm{y}}\left(\mathsf{\beta }\right)=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\mathsf{\beta }& 0& \mathrm{s}\mathrm{i}\mathrm{n}\mathsf{\beta }\\ 0& 1& 0\\ -\mathrm{s}\mathrm{i}\mathrm{n}\mathsf{\beta }& 0& \mathrm{c}\mathrm{o}\mathrm{s}\mathsf{\beta }\end{array}\right]$.

The point cloud coordinates in the robot coordinate system are denoted as ${\mathrm{p}}_{\mathrm{b}}={\left({\mathrm{x}}_{\mathrm{b}},{\mathrm{y}}_{\mathrm{b}},{\mathrm{z}}_{\mathrm{b}}\right)}^{\mathrm{T}}$, and the coordinates in the ideal world coordinate system are denoted as ${\mathrm{p}}_{\mathrm{w}}={\left({\mathrm{x}}_{\mathrm{w}},{\mathrm{y}}_{\mathrm{w}},{\mathrm{z}}_{\mathrm{w}}\right)}^{\mathrm{T}}$. The coordinate transformation relationship between them is:

$${\mathrm{p}}_{\mathrm{w}}=\mathrm{R}{·}_{{\mathrm{p}}_{\mathrm{b}}}+\mathrm{t}$$

(12)

Through the aforementioned transformation, all point clouds are registered into a tilt-and jitter-free ideal coordinate system, thereby providing a consistent spatial reference for the subsequent generation of SC descriptors.

2.2.4. “Path Partitioning-Time Window-KD-Tree” Three-Level Hierarchical Index

When orchard robots operate in large-scale scenarios spanning hundreds of acres, the number of historical descriptors becomes exceedingly large. Traditional brute-force matching methods are inefficient and struggle to meet real-time requirements. The core of index optimization lies in leveraging the characteristics of orchards—such as “structured orchard rows and periodic inspection paths”—to construct a three-level hierarchical index of “Path Partitioning-Time Window-KD-Tree.” This approach compresses the number of candidate frames through multi-dimensional filtering and combines efficient retrieval to accelerate matching. The specific steps are as follows:

First-Level Index: Path Partitioning Filtering. Leveraging the regular row-structured layout of the orchard, the map is divided into multiple consecutive sectors along the operational path direction at intervals of 5 m (corresponding to the typical tree row spacing), with each sector assigned a unique identifier. Once the robot determines its current sector via real-time localization, only historical descriptors from the same sector and adjacent sectors are retrieved as candidates. This approach directly excludes spatially distant cross-sector frames that are unlikely to form valid loop closures, significantly narrowing the search scope and enhancing the real-time performance of the system.

Second-Level Index: Time Window Filtering. Orchard inspections typically follow periodic fixed paths, and loop closures often occur with historical frames from similar time periods. Building upon the first-level sector filtering, only historical frames within the past week and falling within a ±1 h window around the current time are retrieved. This further eliminates frames that are temporally unlikely to form loops, reducing unnecessary matching computations. During the initial week or when no historical data exists, the time window is expanded to cover the entire day; when the patrol pattern changes, the window range is dynamically adjusted based on localization confidence.

Third-Level Index: KD-Tree Efficient Retrieval. For the candidate frames filtered through the first two levels, a KD-tree is constructed based on the feature dimensions of the descriptors. The KD-tree enables efficient nearest neighbor searches through spatial binary partitioning, allowing rapid identification of the historical frame most similar to the current frame. The time complexity of this three-level indexing structure primarily depends on the KD-Tree retrieval process. Let $\mathrm{N}$ denote the total number of historical frames. After path partitioning and time-window filtering, the number of candidate frames is reduced to $\mathrm{M}$ (typically $\mathrm{M}\ll \mathrm{N}$). The time complexity of nearest-neighbor retrieval in a KD-Tree is $\mathrm{O}\left(\log \mathrm{M}\right)$, which is far superior to the $\mathrm{O}\left(\mathrm{M}\right)$ complexity of brute-force matching.

3. Results

3.1. Experimental Platform

This study evaluated the performance of the upgraded OSC-LIO system in a simulated real-world orchard environment. The assessment employed the Absolute Trajectory Error (ATE) to reflect global localization accuracy and the Relative Pose Error (RPE) to gauge the accuracy of local motion estimation. The experimental platform was a mobile robot based on an Ackermann chassis driven by DC motors. The host computer was a laptop equipped with an Intel Core i5-12450H processor (2.00 GHz) and 16 GB of RAM, operating under Ubuntu 18.04 and the Robot Operating System (ROS Melodic) framework. The algorithm ran in post-processing mode, with an average processing time of approximately 45 ms per frame. The host computer communicated with the chassis via a USB-to-serial module, with the baud rate set to 115,200. For environmental perception, the study utilized a Velodyne 16-line LiDAR (Velodyne LiDAR Inc., San Jose, CA, USA) and an N100 9-axis IMU (Wheeltec, Shenzhen, Guangdong, China) as the core sensors. The Velodyne 16-line LiDAR is a compact 3D LiDAR that retains adjustable motor rotation speed, capable of outputting real-time measurements of surrounding distances and reflectivity. The LiDAR was set to a scanning frequency of 20 Hz, with a measurement range from 0.3 m to 100 m, an accuracy of ±3 cm, a vertical field of view of ±15 degrees, and an output rate of up to 300,000 points per second. This provides dense point cloud and geometric information for the SLAM algorithm. The N100 module is a new, miniaturized, high-performance IMU core that undergoes rigorous factory calibration. It features a powerful Sigma-Point Kalman Filter (SPKF) and a set of high-performance algorithms, with a sensor sampling frequency of up to 1000 Hz. It incorporates compensation for coning and sculling motions and exhibits strong resistance to magnetic interference.

The LiDAR is connected to the host computer via an Ethernet cable, with its point cloud data and positional information sampled at a stable rate of 20 Hz. The IMU is connected to the host computer through a USB serial interface, collecting position and velocity data at a higher frequency of 200 Hz. The data from both sensors are synchronized using the timestamp interpolation mechanism within ROS, aligning the timing of the LiDAR (20 Hz) and IMU (200 Hz). During the data acquisition process, the robot records the raw data streams from these two types of sensors in real-time and packages them into ROS Bag files. These Bag files serve as input for the offline post-processing stage, where the enhanced OSC-LIO algorithm is executed on the robotic system to ultimately generate a 3D point cloud map representing the environment. The overall setup of the experimental platform is shown in Figure 7.

3.2. Experimental Validation

3.2.1. Laser Odometry Error Analysis

To evaluate the operational stability of the developed orchard navigation robot and the effectiveness of the improved odometry, the robot was controlled to move along an approximately elliptical trajectory with a circumference of about 70 m at a constant speed of 0.50 m/s for one full lap, with a total duration of approximately 280 s. For the quantitative assessment and comparison of the localization accuracy of different SLAM algorithms, the experiment used the ground truth trajectory as the benchmark to analyze the performance of the trajectories generated by the OSC-LIO algorithm and the original LIO-SAM algorithm, respectively. The EVO evaluation tool was employed to comprehensively assess the localization performance of both algorithms. This evaluation focused on two key metrics—Absolute Trajectory Error (ATE) and Relative Pose Error (RPE)—to provide a holistic judgment from the perspectives of global consistency and local smoothness.

To visually demonstrate the algorithm performance, Figure 8 shows a 2D planar comparison of three trajectories: the OSC-LIO trajectory in green, the LIO-SAM trajectory in red, and the Ground Truth trajectory in black. It can be observed that when semantic-augmented descriptor-based loop closure detection is employed, the robot’s trajectory aligns well with the target path. In contrast, when using the original loop closure detection method, significant deviations from the target trajectory occur, particularly during robot turning maneuvers. In the simulated orchard environment, OSC-LIO closely follows the ground truth, while the LIO-SAM trajectory exhibits more pronounced drift at path corners, with its deviation magnitude substantially exceeding that of OSC-LIO. These results preliminarily indicate that incorporating semantic information to assist descriptor-based loop closure detection offers superior robustness in pose estimation.

The Absolute Trajectory Error (ATE) is used to evaluate the global consistency level of the trajectory. The Absolute Pose Error (APE) time-series curve in Figure 9 provides a more intuitive observation of the dynamic error variations between the two trajectories. Specifically, the error curve of LIO-SAM exhibits peaks with significantly larger magnitudes at multiple time points, which aligns with the trajectory drift observed during turning phases in Figure 8. The statistical analysis results of the translation component of the Absolute Pose Error (APE), as shown in

Table 1. Statistical Comparison of Absolute Trajectory Error (ATE).

Algorithm	RMSE	Mean	Max	Std	Median
OSC-LIO	0.226777	0.218541	0.959164	0.132790	0.221330
LIO-SAM	0.296683	0.242842	0.882412	0.134632	0.208101

, further indicate that OSC-LIO outperforms LIO-SAM across all key evaluation metrics.

As shown in Table 1, the RMSE of the Absolute Pose Error (ATE) for OSC-LIO (0.226 m) is approximately 23.5% lower than that of LIO-SAM (0.296 m). This fully demonstrates that OSC-LIO can consistently provide pose estimations closer to the ground truth throughout the entire motion process.

The reduction in ATE error of OSC-LIO primarily stems from the proposed “global descriptor+local features” two-layer verification mechanism. By incorporating semantic features such as trunk diameter and canopy height difference, the distinctiveness of descriptors between fruit trees is enhanced, thereby reducing false loop closures. To quantify this improvement, the descriptor similarity of OSC-LIO and LIO-SAM was statistically analyzed using Euclidean distance (where a smaller Euclidean distance indicates higher similarity). The results are shown in

Table 2. Comparison of Descriptor Similarity Between Fruit Trees: OSC-LIO vs. LIO-SAM.

Algorithm	Evaluation Metric	Average Similarity	Maximum Similarity	Minimum Similarity	Similarity Reduction Rate
OSC-LIO	Euclidean Distance	0.35	0.52	0.18
LIO-SAM	Euclidean Distance	0.50	0.68	0.32	42.9

.

Statistical analysis of descriptor similarity across 15 pairs of fruit trees (Table 2) shows that the average similarity of OSC-LIO (0.35) is 42.9% lower than that of LIO-SAM (0.50). This indicates that OSC-LIO can more accurately distinguish between morphologically similar fruit trees, avoiding false loop closures caused by the lack of distinctiveness between descriptors—as evidenced by the trajectory drift of LIO-SAM at orchard-row turns (Figure 8). Consequently, the global pose estimation accuracy is improved, validating the conclusion that the ATE error is reduced by 23.5%.

The Relative Pose Error (RPE) serves as a key metric for evaluating the accuracy of local motion. According to the statistical results in

Table 3. Statistical Comparison of Relative Pose Error (RPE).

Algorithm	RMSE	Mean	Max	Std	Median
OSC-LIO	0.015818	0.011005	0.083757	0.011363	0.007926
LIO-SAM	0.022183	0.014680	0.115326	0.016630	0.010252

, it is evident that OSC-LIO holds a clear advantage in terms of local motion precision. Figure 10 further illustrates the temporal variation of the inter-frame relative pose error. The statistical analysis derived from this figure aligns with the earlier conclusions drawn from the Absolute Pose Error (APE): both the average RPE of OSC-LIO (approximately 0.014 m) and its standard deviation (Std) are lower than those of LIO-SAM. These findings indicate that OSC-LIO not only achieves higher accuracy in global trajectory estimation but also demonstrates superior smoothness and stability in continuous inter-frame motion estimation.

The experimental results demonstrate that, in the test scenario, incorporating semantic features into the loop closure detection of the algorithm yields comprehensively superior performance compared to using the original trajectory generated solely by the Scan Context module. OSC-LIO, leveraging its unique local orchard feature extraction and matching algorithm, exhibits stronger robustness when confronting challenges such as the unstructured orchard environment and high geometric similarity, thereby achieving higher localization accuracy. However, it is important to note that OSC-LIO, as a complete SLAM framework, integrates global optimization and loop closure capabilities, which are crucial for suppressing accumulated errors during long-term and large-scale operations.

3.2.2. Mapping and Navigation Experiments

To validate the comprehensive performance of the OSC-LIO algorithm in mapping and navigation tasks, experiments were conducted in a simulated orchard test field at North China University of Water Resources and Electric Power. The scenario consists of eight simulated fruit trees arranged in rows, with a row spacing of 3 m and a tree spacing of 2.5 m, conforming to the typical layout of an apple orchard. During the experiment, the robot was remotely controlled via Bluetooth to move uniformly along a predefined path for two laps to collect complete environmental point cloud data. The experimental scene is illustrated in Figure 11.

In this experiment, we employed the improved OSC-LIO algorithm for mapping to generate a 3D map of the orchard environment. The mapping result is shown in Figure 12. The mean error in detected trunk center positions is 0.15 m (standard deviation: 0.08 m), with uniform point cloud density and clearly defined geometric contours of the inter-row passages.

The map from Figure 12 was imported as a prior map into ROS Autoware for navigation experiments, as shown in Figure 13. Using the Autoware algorithm, a global path was extracted from the 3D point cloud map of the simulated orchard environment. Based on this global path, the robot’s velocity and motion strategy were calculated to control its autonomous movement toward the target location. The robot accurately tracked the planned path, maintained stable navigation through inter-row passages, and successfully avoided static obstacles. Ultimately, it successfully reached the target point, completing the navigation task.

The experimental results demonstrate that the map constructed by the OSC-LIO algorithm not only meets localization accuracy requirements but also enables the robot to plan a reasonable path based on this map, allowing it to pass smoothly between trees without engaging in ineffective obstacle avoidance. This indicates that the proposed map optimization method, combined with the dynamic point filtering strategy, significantly enhances the navigation precision and task reliability of robots in complex orchard environments.

4. Discussion

The OSC-LIO algorithm proposed in this study demonstrates superior localization and mapping performance compared to LIO-SAM in a simulated orchard environment, providing preliminary validation for the effectiveness of integrating local semantic features and a hierarchical indexing strategy. Experimental results show that OSC-LIO re-duces the Root Mean Square Error (RMSE) of the Absolute Trajectory Error (ATE) by ap-proximately 23.5% and the Relative Pose Error (RPE) by approximately 28.7%, achieving more stable trajectory estimation.

However, the proposed method still has several limitations. First, its performance re-lies on the clear extraction of trunk and canopy structures from the point cloud. In or-chards with extremely dense foliage or row spacing less than 1.5 m, severe occlusions may cause feature extraction to fail, thereby affecting the reliability of loop closure detection Second, the currently employed local semantic features (e.g., trunk diameter, canopy height difference) and their thresholds are calibrated primarily for specific fruit tree varieties (such as apple trees). For different tree species (e.g., citrus, grapevines) or across different growth seasons (e.g., fruit swelling period and leaf-fall period), tree morphology and point cloud characteristics vary significantly, and the algorithm’s generalization and self-adaptation capabilities require further verification.

The three-level indexing structure of “path partitioning–time window–KD-Tree” pro-posed in this paper provides an effective approach for reducing computational load. However, in real-world scenarios covering hundreds of hectares with varied terrain, ac-cumulated errors from long-term operation, the impact of adverse weather conditions (e.g., fog, rain) on sensor data, and the non-strict periodicity of operational paths all represent pressing challenges that must be addressed to achieve fully autonomous and highly reliable navigation. Furthermore, seasonal changes impose additional demands on long-term system operation. As the orchard environment evolves through the four seasons, vegetation, ground conditions, and even spatial structures undergo continuous change. Future systems will need to incorporate online learning and dynamic map-updating capabilities to adapt to such persistent environmental variations. Future research efforts will focus on the following directions:

• Conduct field testing in real apple orchards to evaluate the system’s applicability and stability under varying terrain, lighting, and seasonal conditions.
• Integrate LiDAR-based modules, such as fruit tree species classification, to enable the robot to simultaneously perform agricultural information collection during navigation.
• Develop an adaptive dynamic point filtering threshold mechanism and investigate trajectory prediction for dynamic objects (e.g., workers, other agricultural machinery) to enhance system safety in unstructured dynamic environments.
• Future research could integrate this SLAM system with higher-level navigation and obstacle avoidance modules, drawing on the sensor-fusion architectures employed in applications such as orchard spraying robots [27,28].

5. Conclusions

This study presented OSC-LIO, an improved LiDAR-inertial odometry system tailored for orchard navigation. By synergistically integrating local semantic features (trunk diameter, canopy height difference) into a global descriptor framework, introducing a lightweight dynamic point filtering mechanism, and employing a hierarchical “path-time-feature” indexing strategy, the system effectively addresses the key challenges of perceptual aliasing and dynamic interference in unstructured orchard rows. Experimental validation in a simulated environment confirms its superior performance, achieving a 23.5% reduction in ATE RMSE and a 28.7% reduction in RPE compared to LIO-SAM.

While the results demonstrate a significant advancement in robustness and accuracy for structured agricultural scenes, the algorithm’s performance is contingent upon clear geometric feature extraction and may require recalibration for different tree species or growth stages. Future work will focus on real-world deployment and enhancing the system’s adaptability. Overall, OSC-LIO provides a high-precision, lightweight, and practical technical pathway towards reliable autonomous navigation for agricultural robots in orchard environments.