# GNSS and LiDAR Integrated Navigation Method in Orchards with Intermitt GNSS Dropout
## 2 Materials and Methods
### Path-Tracking Control System
#### 2.3.2 PID-Based Path-Tracking Algorithm  
Due to the uneven ground of the orchard and the soft soil, the mobile platform of the orchard often deflects and slips during the walking process, causing it to deviate from the preset navigation path. The PID control algorithm is an important part of classical control theory and has been widely used in practical engineering [29], and we use it to implement path-tracking control.  
The essence of a PID controller is a linear controller. In the PID control system, the selection of PID controller parameters \(K_{p}\), \(K_{l}\), \(K_{D}\) can significantly affect the effectiveness of the system. Continuous PID algorithm prototype formula:  
\[u(t)=K_{p}\bigg{[}e(t)+\frac{1}{T_{i}}\int_{0}^{t}e(t)d(t)+T_{d}\frac{de(t)}{ dt}\bigg{]} \tag{25}\]  
where \(u(t)\) is the PID controller output regulation, \(K_{p}\) is the proportionality coefficient, \(e(t)\) is the real value and the set value of the error, \(T_{i}\) is the integral time constant, and \(T_{d}\) is the differential time constant.  
Since the industrial control machine cannot directly use the continuous PID prototype function, the algorithm is discretized as follows:  
\[u(n)=K_{p}e(n)+\frac{k_{p}T}{T_{i}}\sum_{i=1}^{n}e(i)+\frac{K_{p}T_{d}}{T}[e(n )-e(n-1)] \tag{26}\]where \(k_{p}\), \(k_{p}/T_{i}\), \(K_{p}T_{d}\) correspond to the three parameters \(K_{p}\), \(K_{I}\), and \(K_{D}\) in the PID controller, \(e(n)\) is the error between the real value and the set value at n moments, and T is the sampling period.  
The PID controller studied in this paper takes the heading deviation \(\theta\) and lateral deviation d between the navigation process of the robotic vehicle in the orchard and the set navigation line as inputs, and the left and right wheel rotational speeds of the mobile chassis as outputs. The control principle is shown in Figure 10, and the control model is as follows:  
\[u(n)=K_{p1}\dot{x}+K_{p2}\dot{\theta}+K_{I}\sum_{i=1}^{n}d(i)+K_{D}[d(n)-d(n-1)] \tag{27}\]  
This can be obtained by combining the differential robot model above:  
\[\begin{cases}v_{r}=v-\frac{1}{2}\bigg{(}K_{p1}\dot{x}+K_{p2}\dot{\theta}+K_{I} \sum_{i=1}^{n}d(i)+K_{D}[d(n)-d(n-1)]\bigg{)}\\ v_{l}=v+\frac{1}{2}\bigg{(}K_{p1}\dot{x}+K_{p2}\dot{\theta}+K_{I}\sum_{i=1}^{n} d(i)+K_{D}[d(n)-d(n-1)]\bigg{)}\end{cases} \tag{28}\]  
The sampling time was set to 250 milliseconds, and the path-tracking speed to v = 0.25 m/s. Through testing, it was determined that when \(K_{p1}\) = 59.6, \(K_{p2}=0.65\), \(K_{I}=1.2\), and \(K_{D}=6.5\), the system exhibits the best stability and fastest response time. The simulation results of the PID control performance under the selected parameters are shown in Figure 11. The controller used in this paper has a rise time of no more than 1 s, a lateral deviation overshoot of no more than 0.1 m, a course deviation overshoot of no more than 0.2 rad, a settling time of no more than 2 s, and a steady-state error of zero. These characteristics meet the fundamental requirements of PID control.  
Figure 11: Simulation results of PID control performance. (**a**) Simulation results of lateral deviation over time; (**b**) simulation results of course deviation over time.  
Figure 10: PID navigation control model.