# GNSS and LiDAR Integrated Navigation Method in Orchards with Intermitt GNSS Dropout
## 2 Materials and Methods
### GNSS-LiDAR Fusion Positioning Principle
#### 2.2.1 Coordinate System Integration of GNSS and LiDAR
\begin{array}{l}x=L_{s}+\frac{M}{2\sigma_{2}^{2}}l^{2}\text{sin}\,N \text{cos}\,N+\frac{M}{24\sigma_{2}^{2}}l^{4}\text{sin}\,N\text{cos}^{3}\,N \big{(}5-t^{2}+9\eta^{2}+4\eta^{2}\big{)}+\\ \frac{M}{2\sigma_{2}^{2}}l^{6}\text{sin}\,N\text{cos}^{5}\,N\big{(}61-58t^{2}+ t^{4}\big{)}\\ y=\frac{M}{\rho_{2}}l^{2}\text{cos}\,N+\frac{M}{6\rho_{2}^{2}}l^{3}\text{cos}^{3}\,N \big{(}1-t^{2}+\eta^{2}\big{)}+\\ \frac{M}{120\
u_{2}^{2}}l^{5}\text{cos}^{5}\,N\big{(}5-18t^{2}+t^{4}+14\eta^{2 }-58\eta^{2}t^{2}\big{)}\end{array} \right. \tag{8}\]  
where \(l\) is the difference in longitude from the position of the point to the position of the central meridian, \(\rho_{2}\) is a constant, and \(\eta\) is an auxiliary parameter, with  
\[\rho_{2}=\frac{180}{\pi}\times 3600 \tag{9}\]  
\[l=E-L_{0} \tag{10}\]  
\[\eta=e_{2\text{cos}\,N} \tag{11}\]  
By using Gauss-Kruger projection algorithm, the \((x,y)\) coordinate representation of latitude and longitude in a planar coordinate system is obtained. To ensure stability and consistency in the switching and usage of the two sensors, it is necessary to unify the GNSS navigation planar coordinate system with the three-dimensional LiDAR-SLAM navigation coordinate system after Gaussian projection. Bydoing so, descriptions of self-positioning and environmental information under the unified coordinate system can be obtained. As shown in Figure 5, \(X_{l}O_{l}Y_{l}\) is the laser odometer reference coordinate system, and \(X_{g}O_{g}Y_{g}\) is the coordinate system after Gauss-Kruger projection. This paper takes the laser odometer reference coordinate system \(X_{l}O_{l}Y_{l}\) as the unified coordinate system, with \((x_{0},y_{0})\) as the initial position after the robot operates the navigation system. At the same time, \((x_{0},y_{0})\) is also the origin of the laser odometer reference coordinate system. This paper obtains the angle \(\theta\) between the \(Y_{l}\) axis of the laser odometer reference coordinate system and the true north from the heading angle output by the INS. Then, the coordinates \((x_{1},y_{1})\) are transformed through rotation and translation transformations to obtain \(\left(x_{1}^{{}^{\prime\prime}},y_{1}^{{}^{\prime\prime}}\right)\) in the laser odometer coordinate system as follows. First, the laser odometer coordinate system \(X_{l}O_{l}Y_{l}\) is used as the unified coordinate system, and \((x_{0},y_{0})\) is the starting position of the autonomous positioning navigation system. The angle between the \(X_{0}\) axis of the laser odometer coordinate system and true north is obtained by receiving the heading angle output from INS as \(\theta\), and then the GNSS navigation plane coordinates \((x,y)\) after Gaussian projection are transformed by translation to obtain \((x_{1},y_{1})\) in the laser odometer coordinate system as  
\[ \begin{bmatrix}x_{1}^{{}^{\prime\prime}}\\ y_{1}^{{}^{\prime}}\end{bmatrix} = \begin{bmatrix}\cos\theta&-\sin\theta\\ \sin\theta&\cos\theta\end{bmatrix}\begin{bmatrix}x_{1}-x_{0}\\ y_{1}-y_{0}\end{bmatrix}  \tag{12}\]  
By rotating and translating the coordinate system, an accurate description of the laser odometer positioning information, the GNSS positioning information, and the navigation path in the same coordinate system can be obtained. Following the conversion of the coordinate system, the system now articulates the positioning information from both the laser odometer and GNSS, along with the navigation path defined by latitude and longitude, in a unified coordinate framework.