# 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 shown in Figure 8a, in the calibrated LIDAR point cloud the above method is used to divide the point cloud into a non-ground point cloud and a ground point cloud, where the red points are ground, and the blue points are non-ground points clouds. On the segmented non-ground point cloud, using the robot as the origin, conditional filtering is used to extract the left apple tree row region and the right apple tree row region behind the robot, respectively, and then RANSAC is used to extract the cylindrical features of the tree trunk and segment them in this region. That is, randomly select any two-point coordinates, substitute into Equation (7), and derive the error of the points in the point cloud in the cylinder model, compare with the set threshold value, and through repeated iterations, when the error is less than the threshold value, the sample point belongs to the cylinder sample point, if the error is greater than the threshold value, it is the point outside the cylinder. \[(x-x_{0})^{2}+(y-y_{0})^{2}+(z-z_{0})^{2}-{c_{r}}^{2}=\frac{\left[c_{l}(x-x_{ 0})+c_{r}(y-y_{0})+c_{n}(z-z_{0})\right]^{2}}{{c_{l}}^{2}+{c_{r}}^{2}+{c_{n}}^{ 2}} \tag{7}\]  
In Equation (7), \(c_{r}\) is the radius of the cylindrical base circle, the distance from the point on the cylindrical surface to its axis is constantly equal to the radius, \((x,y,z)\) is any point on the cylindrical surface, \((x_{0},y_{0},z_{0})\) is a point on the cylinder axis, and \((c_{l},c_{r},c_{n})\) is the cylindrical axis vector. The left and right trunks on both sides of the master robot obtained by fitting the segmentation using the above algorithm are shown in Figure 8b. The obtained coordinates of the points \(p_{l}\) and \(p_{r}\) on the center of the left and right-side trunk axes in the \(xoy\) plane of the LIDAR coordinate system are \((x_{l},y_{l})\) and \((x_{r},y_{r})\), respectively. Then, the coordinates of the center \(p_{c}\) of these two points are \((\frac{x_{l}-x_{r}}{2},\frac{y_{l}-y_{r}}{2})\), and the coordinates of the target point \(p_{g}\) at a distance from this center are \((\frac{x_{l}-x_{r}}{2}+l_{c},\frac{y_{l}-y_{r}}{2})\), where \(l_{c}\) is the entire length of the main robot after towing the trailer. Since the target point acquired by the LIDAR is relative, Equation (8) is used to obtain the coordinates of the point in the global coordinate system \(G_{x}OGy\). In Figure 9, the conversion between the LIDAR local coordinate system and the GNSS global coordinate system is shown. \[ \begin{array}{l}G_{xg}=\frac{x_{g}}{\cos{\alpha}}+G_{xt}-(x_{g}\tan{\alpha} +y_{g})\sin{\alpha}\\ G_{yg}=\frac{y_{g}}{\cos{\alpha}}+G_{yt}+(x_{g}-y_{g}\tan{\alpha})\sin{\alpha} \end{array}  \tag{8}\]  
In Equation (7), \(G_{xg}\) and \(G_{yg}\) are the horizontal and vertical coordinates of the target point \(p_{g}\) in the global coordinate system, \(x_{g}\) and \(y_{g}\) are the horizontal and vertical coordinates of the target point \(p_{g}\) in the local coordinate system of the LIDAR, \(G_{xt}\) and \(G_{yt}\) are the horizontal and vertical coordinates of the LIDAR in the global coordinate system, and \(\alpha\) is the rotation angle between the LIDAR coordinate system \(xoy\) and the global coordinate system \(G_{x}OGy\)  
Figure 8: The master robot extracts point cloud information of tree trunks on both sides. (**a**) Master robot segmentation of point clouds for ground and non-ground environments; (**b**) extraction of tree trunk feature point clouds.