# Development of a Combined Orchard Harvesting Robot Navigation System
## 2 Materials and Methods
### Master-Slave Navigation Strategy
#### 2.3.1 Master Robot Navigation Phase  
As the robot turns at the ground head, the GNSS sensor carried by the robot can be used to measure the ground head turn point as a targeted tracking point. This section of the article focuses on apple tree trunk feature extraction using LIDAR within apple tree rows for target tracking. Figure 7: Light detection and ranging (LIDAR) point cloud ground level calibration. (**a**) LIDAR point clouds of master robot calibration; (**b**) LIDAR point clouds of slave robot calibration. For LIDAR-based environment perception, the 3-D point cloud is usually divided into two separate subsets, ground points and non-ground points [46]; the apple tree trunk belongs to the non-ground point cloud. Therefore, it is necessary to remove the ground point cloud-first to improve the accuracy of obtaining apple tree trunk features. In recent years, there have been a large number of studies on ground point cloud extraction and segmentation algorithms. For example, some scholars segmented ground point clouds based on the ground measurement model of LIDAR [47; 48] or used RANSAC to fit the ground like a piece of a plane for segmentation [49; 50; 51], or distinguished ground and vegetation based on the threshold between ground point clouds [52]. These methods are simple and effective, but considering that the ground point cloud changes continuously above and below the horizontal plane when the robot is driving continuously in the orchard, the principle of a simulation filter (CSF) algorithm [53] is more indicative of the current terrain features than methods that rely on the height of a laser beam, a sensor or set thresholds. The point cloud decreases as the distance between the object and the LIDAR increases. Point clouds that are far beyond their travel distance are usually not used when driving autonomously. Therefore, the unwanted point clouds are first removed by conditional filtering, and then the ground point clouds are segmented using the CSF in Equations (4)-(6). \[p_{m}\frac{\partial p_{x(t)}}{\partial t^{2}}=F_{ext}(p_{x},t)+F_{int}(p_{x},t) \tag{4}\]  
\[p_{x(t+\Delta t)}=2p_{x(t)}-p_{x(t-\Delta t)}+\frac{g}{p_{m}}\Delta t^{2} \tag{5}\]  
\[\overset{\rightarrow}{d_{p}}=\frac{1}{2}p_{b}\left(\overset{\rightarrow}{p_ {i}}-\overset{\rightarrow}{p_{0}}\right)\cdot\overset{\rightarrow}{p_{n}}.p _{ri} \tag{6}\]  
where, in Equation (4), \(p_{m}\) is the weight of the particle, \(p_{x(t)}\) is the position of the point in the point cloud in the \"fabric\" at time \(t\), \(F_{ext}(p_{x},t)\) is the external driving element (such as gravity), \(F_{int}(p_{x},t)\) is the internal link between the points in the point cloud. In Equation (5), \(\Delta t\) is the time step, \(g\) is the gravity constant 9.8 m/s\({}^{2}\), and \(p_{x(t+\Delta t)}\) is the new point in the point cloud at the next iteration. In Equation (6), \(\overset{\rightarrow}{d_{p}}\) is the displacement of the particle, \(p_{b}\) is 1 when the particle is moving and 0 when it is not movable, \(\overset{\rightarrow}{p_{i}}\) is the adjacent particle of \(\overset{\rightarrow}{p_{0}}\), and \(\overset{\rightarrow}{p_{n}}\) is the unit vector that normalizes the point to the vertical direction (0,0,1)T.  
The point cloud is first divided into a grid (the size of a piece of the grid is 0.2 \(\times\) 0.2 m), and the original point cloud and the grid-like point cloud are projected onto the same plane to find the nearest point of each point cloud and save the height information of the original point cloud. For the point cloud in a single grid, the displacement of the point cloud by gravity is then calculated by Equation (4), compared with the height of the point cloud before projection and iterated until the height change reaches a set threshold or iterated to a set value (set to 500). Finally, the displacement value is obtained from Equation (6), and when the displacement is less than the set value (set to 0.2 m), it is regarded as a ground point, or if greater, is regarded as a non-ground point.