# Development of a Combined Orchard Harvesting Robot Navigation System
## 2 Materials and Methods
### Control System  
The pure pursuit algorithm is a kinematic constraint-based algorithm that has been successfully applied in the trajectory tracking control of many robots [54]. The algorithm can determine the arc length to be traveled by the robot to the tracking path based on the set pre-sight distance. In picking and transporting robot driving, the spacing between target feature points extracted using LIDAR is short. When the orchard robot is driving at a fixed speed, the target feature point spacing changes less, so here a fixed arc length is used for target tracking. As shown in Figure 11, the distance \(L\) of the target feature point from the robot is first calculated using Equation (13), and then the distance \(L_{d}\) between the final robot pre-visual distance point and the robot's current position point is obtained using Equation (12). \(L_{d}\) is equal to \(L\) when \(L_{d}\) is less than the set threshold value of 1 m, or vice versa, is equal to the threshold value of 1 m. The calculated distance \(L_{d}\) is used in Equation (13) to obtain the curvature of the robot walking according to the curve. The crawler robot uses a differential motion model, in which the angular velocities \(\omega_{l}\) and \(\omega_{r}\) of the left and right sides of the robot can be determined based on the wheel spacing \(l\), the drive radius \(r\), the angular velocity \(v\), and linear velocity \(\omega\) from the odometer output in Equation (14). The influence of the angular velocities on both sides can be calculated by Equation (16) to the displacement point coordinates of the crawler robot at the next moment. The actual path of the robot can be controlled by inputting the new displacement point coordinates into Equations (13)-(16). \[L=\sqrt{\left(G_{xg}-G_{xt}\right)^{2}+\left(G_{yg}-G_{yt}\right)^{2}} \tag{11}\]  
\[L_{d}=\left\{ \begin{array}{ll}L&L<1\ m\\ 1&L\geq 1\ m\end{array} \right. \tag{12}\]  
\[\frac{1}{R}=\frac{\theta_{err}^{t}}{L_{d}}=\frac{2\sin(\arctan\theta_{t}- \alpha)}{L_{d}}=\frac{2\sin(\arctan(\frac{G_{yg}-G_{yt}}{G_{xg}-G_{xt}})- \alpha)}{L_{d}} \tag{13}\]  
\[\left\{ \begin{array}{ll}\omega_{r}=\frac{v-\frac{l}{2}\omega}{\tau}\\ \omega_{l}=\frac{v+l\omega}{\tau}\end{array} \right. \tag{14}\]\[\left[ \begin{array}{c}G_{x(t+1)}\\ G_{y(t+1)}\\ \theta_{t+1}^{t+1}\end{array} \right]=\left[ \begin{array}{c}G_{xt}\\ G_{yt}\\ \theta_{rr}^{t}\end{array} \right]+\left[ \begin{array}{cc}\cos(\theta_{err}^{t} )&0\\ \sin(\theta_{err}^{t})&0\\ 0&1\end{array} \right]\left[ \begin{array}{cc}\frac{r}{2}&\frac{r}{2}\\ \frac{r}{l}&-\frac{r}{l}\end{array} \right]\left[ \begin{array}{c}\omega_{r} \\ \omega_{l}\end{array} \right] \tag{15}\] \[\left[ \begin{array}{c}G_{x(m+1)}\\ G_{y(m+1)}\\ \theta_{m}^{t+1}\end{array} \right]=\left[ \begin{array}{c}G_{xm}\\ G_{ym}\\ \theta_{m}^{t}\end{array} \right]+\left[ \begin{array}{cc}\cos\theta_{m}^{t} \cos(\theta_{t}-\theta_{m}^{t})&\cos\theta_{m}^{t}\sin(\theta_{t}-\theta_{m}^{ t})\\ \sin\theta_{m}^{t}\cos(\theta_{t}-\theta_{m}^{t})&\sin\theta_{m}^{t}\sin( \theta_{t}-\theta_{m}^{t})\\ \sin(\theta_{t}-\theta_{m}^{t})&-\cos(\theta_{t}-\theta_{m}^{t})\end{array}  \right]\left[ \begin{array}{cc}\frac{r}{l}&\frac{r}{l}\\ \frac{l}{l}&-\frac{l}{l}\end{array} \right]\left[ \begin{array}{c}\omega_{r} \\ \omega_{l}\end{array} \right] \tag{16}\]  
After inputting the ideal path points of the picking and transporting robot walking in the orchard in Matlab, the algorithm is simulated with the robot's linear velocity \(v\) set to 0.5 m/s and angular velocity \(w\) to 0.5 m/s. The simulation results are shown in Figure 12. The ideal path point and the simulated tracking path are compared in Figure 12a, where it can be seen that the robot tracks the target point better in a short straight line, while it deviates from the target point at the corner of the turn, but then adjusts it.