BDS/IMU Integrated Auto-Navigation System of Orchard Spraying Robot

Featured Application

Authors are encouraged to provide a concise description of the specific application or a potential application of the work. This section is not mandatory.

Abstract

To improve the accuracy and reliability of orchard spraying robots, an integrated navigation system was developed, consisting of a real-time kinematic positioning-Beidou satellite navigation system (RTK-BDS) receiver, an inertial measurement unit (IMU), a navigation controller, and servo motors. Using the loose coupling combination method, an error Kalman filter algorithm based on the measurement of position and heading angle is implemented to correct the error of the inertial measurement unit in real time. Combining the kinematics model and the pure pursuit model of the spraying robot, a path-tracking control algorithm is proposed. Path planning was conducted according to the terrain characteristics of orchards. Field experiments were carried out on a spraying robot to evaluate the proposed auto-navigation system. The results showed that when the spraying robot was static, the positioning performances of BDS alone and that of the BDS/IMU combined system were similar, the positioning error was less than 1.5 cm, and the heading angle errors were within 0.3°; when the spraying robot moving alone to a straight line at the speed of 0.4 m/s, the position error of the navigation system only using BDS was less than 5.29 cm, the heading angle error was within 3°, while the position error of BDS/IMU integrated navigation system was less than 2.49 cm, and the heading angle error was within 2°. The accuracy of BDS/IMU integrated navigation system is significantly improved. When the orchard spraying robot was moving at the speed of 0.4 m/s, the maximum offset error was lower than 10.77 cm, the average offset error was not higher than 3.55 cm, and the root mean square error (RMSE) of the lateral deviation was 1.19 cm. The results showed that the proposed auto-navigation system could make the spraying robot track the pre-set path smoothly and stably.

1. Introduction

The processes of orchard planting, management, and fruit harvesting in China are confronted with the problems of a low degree of mechanization, high labor intensity, and high cost [1]. Pest control is one of the most important links in the fruit production, performed by spraying pesticides 8~15 times a year, accounting for around 30% of the whole workload in the orchard management [2,3]. Recently, some automatic controlled orchard spraying robots have been developed to reduce the labor intensity and improve efficiency. The navigation system, including positioning and calculating paths, is an important part of the control system of orchard spraying robots.

With the development of navigation and positioning systems [4], technologies such as laser navigation [5,6,7], visual navigation [8,9,10] and Global Navigation Satellite System (GNSS) navigation [11,12] have become research hotspots.

Barawid Jr et al. [13] used the Hough transform algorithm to recognize the tree rows using a laser scanner as a navigation sensor, and the results performed on a tractor show that the lateral error was 0.11 m and the heading error was 1.5°. Thanpattranon et al. [14] developed a navigation system based on a laser rangefinder for a trailer whose moving direction is controlled by a sliding hitch bar, and its RMS difference was 0.275 m. Ai Changsheng et al. [15] proposed an algorithm for the lidar data obtained in the orchard, where a multi-support-vector proportion weighting method is developed to identify the ridge line of vineyards with the average angular deviation of 0.72°. The visual navigation technology, featured with wide range of detection and good cost-efficiency, is one of the mainstream navigation methods [16]. Han Zhenhao et al. [17] proposed a visual navigation method based on U-Net for path recognition, the average pixel error was 9.5 pixels, but this method is only applicable to orchards with visually explicit crop and road edges. Li Wenyang et al. [18] built a navigation system utilizing the film on the single row ridge as the navigation feature, extracted the H channel in HSV colour space, combined with the Otsu algorithm to extract the row ridge, and fitted the navigation line with the least square method. Wang Yi et al. [19] proposed a method to extract citrus trunks based on YOLO V3 convolutional neural network, where the intersections of the trunks and ground were fitted into a navigation line by the least square method. Ma Chi et al. [20] proposed a navigation mark generation method to replace the root point of the fruit tree, where the base point coordinates were extracted from the tree rows based on the cubic spline interpolation method, and the navigation line was generated according to the least square method. Radcliffe et al. [21] fitted the navigation line according to the boundary line between the canopy and the sky, the root mean square error of the horizontal deviation was 2.13 cm when testing in a peach orchard, but this method is only suitable for those kinds of orchards having tree crowns all year round. Branches overhang interferes with the extraction of the boundary line, resulting in a reduction in navigation accuracy. With the development of satellite positioning technology, the error of carrier-phase differential satellite positioning is limited to a few centimeters, on which a lot of agricultural navigation systems were built. Lou Xiwen et al. [22] developed a navigation controller for tractor linear tracking based on a PID controller and Dongfanghong X-804 tractor, using RTK-DGPS positioning technology and combining tractor motion with a hydraulic steering control model. Nagasaka et al. [23] developed a navigation controller for rice transplanting based on RTK-GPS and fiber optic gyroscope sensors to measure position and direction, using actuators to control steering, engine throttle, clutch, brake, etc, where the maximum deviation from the desired path is less than 12 cm. In order to adapt to the orchard, Xiong bin et al. [24] designed an automatic navigation system of orchard sprayer based on RTK-BDS, and built a closed-loop control of steering angle based on steering controller, electric pneumatic steering system and three-axis magnetometer. The field tests showed the maximum offset error was not over 0.13 m for the straight tracking at the speed of 2 km/h.

However, in unstructured orchards, the terrain, illumination, and climate are complicated, which limits the performance of the orchard navigation system built on such technologies as laser, computer vision, or GNSS. In addition, processing lidar data consumes lots of computing power, and it is also difficult to extract navigation features [25]; computer vision is severely affected by ambient lighting and shadows from surrounding plants such as weeds [26]; GNSS is prone to loss signal due to occlusions [27]. To handle those problems, researchers proposed multi-sensor data fusion methods to develop orchard navigation systems [28,29], such as combining BDS and IMU together [30,31], where BDS features high precision, low-cost and good reliability, but it has the problem of losing signals due to interferences from the environment; IMU is isolated from the environment, running with good precision in a short time and generating a high frequency of data, but its error accumulates over time.

In this study, a BDS/IMU integrated navigation algorithm based on the error Kalman filter for position and heading measurement was designed to solve the problems of the unstableness of BDS’s signal and the accumulative errors of IMU’s data. Combined with the advantages of the two, the reliability and accuracy of the navigation system for long-time operation in the orchard were improved. By combining the kinematics model of the spraying robot with the pure tracking model, a path-tracking auto-navigation system was developed. The accuracy and reliability of the system were evaluated in orchard experiments.

2. Materials and Methods

2.1. Overall Design of the System

The automatic navigation system of the spraying robot, as shown in Figure 1, consists of four parts; namely a positioning unit, a controller, an actuator, and a feedback unit. Among these parts, the BDS receiver and IMU form a positioning unit, which is used to obtain the position and attitude information of the spraying robot in real-time. The controller part is composed of an industrial computer and single chip microcomputer, and the former provides sufficient computing power for the navigation control algorithm, the later provides reliable control for real-time transmission. The actuator part adopts a high-precision servo motor to drive the master wheel through the reducer, and the master wheel outputs power to the track, which controls the track with high precision and high torque to realize the motion control of the spraying robot. The feedback unit detects the servo motor speed through a single chip microcomputer, then transmits it to the industrial computer and forms a closed-loop control.

According to the technical parameters of orchard spraying, a dual-motor independent drive crawler chassis was designed with a crawler center distance of 0.62 m and a crawler length of 2.2 m. Figure 2 shows the industrial computer, BDS positioning antenna and receiver (F-V111, Qianxun Weizhi Internet Inc., Shanghai, China), IMU (100D2, Beijing Sanchi Inertial Technology Co., Ltd., Beijing, China), servo motor, motor driver (KYDBL4875-2E, Jinan Keya Electron and Technology Inc., Jinan, China), single chip microcomputer development board, and encoder installed on the spraying robot. The positioning accuracy of the FV-111 BDS receiver reaches the centimeter level, with both horizontal and vertical static positioning errors below 1.5 cm. The 100D2 has a built-in three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, of which the three-axis accelerometer has a measurement range of ±2 g, the zero bias of 5 mg, and the noise density of 110 $\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}$; the three-axis gyroscope has a measurement range of ±1000 °/s, the zero bias of 50 °/h, and the noise density of 0.005 $°/\mathrm{s}/\sqrt{\mathrm{Hz}}$. The system obtains the position and attitude information through the combination of the BDS receiver and IMU, and the industrial computer is utilized as the control terminal of the spraying robot for information acquisition and decision calculation, which was transmitted to the single chip microcomputer. The single chip microcomputer outputted speed and direction signals to the motor drivers, which controlled the rotation of the servo motors, and receives the encoder information to feedback to the industrial computer.

2.2. Error Kalman Filter Integrated Navigation Algorithm Based on the Measurement of Position and Heading Angle

The integrated navigation system consists of an independent BDS receiver and an IMU, applying a loosely integrated navigation mode, which can provide high-precision positioning information and good stability.

2.2.1. IMU Solution

The IMU solution is carried out under the east-north-sky navigation coordinate system, denoted by n; the spraying robot coordinate system is denoted by b, the geocentric geostationary coordinate system is denoted by e, and the geocentric inertial coordinate system is denoted by i. The specific solution process is shown in Figure 3, and the navigation differential equation can be expressed as follows:

$$\left[\begin{array}{c}{\dot{v}}_{eb}^n\\ {\dot{C}}_n^e\\ {\dot{C}}_b^n\\ \dot{h}\end{array}\right]=\left[\begin{array}{c}{C}_b^n{f}_{ib}^b-\left(2{\omega }_{ie}^n\times +{\omega }_{en}^n\times \right)v_{eb}^n+g_l^n\\ C_n^e\left({\omega }_{en}^n\times \right)\\ C_b^n\left({\omega }_{ib}^b\times \right)-\left({\omega }_{in}^n\times \right)C_b^n\\ v_U\end{array}\right]$$

(1)

where, ${\omega }_{ie}^n\times$ is the rotation vector of e frame reference i frame under n frame; ${\omega }_{en}^n\times$ is the rotation vector of n frame reference e frame under n frame; ${\omega }_{ib}^b\times$ is the rotation vector of b frame reference i frame under b frame; $g_l^n$ is the local gravitational acceleration under n frame, m/s²; $v_U$ is the vertical velocity of download body under n frame, m/s.

The specific force equation and attitude matrix calculation equation can be expressed as follows:

$$\{\begin{array}{c}{\dot{v}}_{eb}^n=f_{ib}^n-\left({\omega }_{en}^n\times +2{\omega }_{ie}^n\times \right)v_{eb}^n+g_l^n\hfill \\ f_{ib}^n=C_b^n{f}_{ib}^b\hfill \end{array}$$

(2)

$${\dot{C}}_b^n=C_b^n\left({\omega }_{ib}^b\times \right)-\left({\omega }_{in}^n\times \right)C_b^n$$

(3)

The position and attitude information of the spraying robot in the east-north-sky coordinate system is obtained by the above IMU solution.

2.2.2. BDS/IMU Integrated Navigation Model

The IMU has accumulated errors, and the integrated navigation using BDS positioning information improved the accuracy of long-term operation. The algorithm flow for estimating the error of the IMU using the error Kalman filter algorithm and correcting it by feedback is shown in Figure 4.

• Continuous-time error Kalman filter equation for BDS/IMU integrated navigation;

When combining navigation systems for information fusion, we first established state equations that reflect the dynamic characteristics of the system state vectors. Due to the existence of navigation system errors and inertial sensor random errors, a suitable error propagation model was established to improve the navigation accuracy by using the external observation of optimal estimation errors. Considering the estimation accuracy and application environment, the IMU navigation parameter errors and inertial sensor drift errors (attitude error, velocity error, position error and device error) were selected as the state vector of the filter. The complexity of the filter is reduced while the accuracy of the navigation system is better satisfied. The state equation of integrated navigation system can be expressed as follows:

$$\dot{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$$

(4)

The state vector of system state equation can be expressed as follows:

$$X\left(t\right)={\left[\begin{array}{ccccccccccccccc}{\varnothing }_E& {\varnothing }_N& {\varnothing }_U& \delta L& \delta \lambda & \delta h& \delta V_E& \delta V_N& \delta V_U& {\epsilon }_{gx}^b& {\epsilon }_{gy}^b& {\epsilon }_{gz}^b& {\nabla }_{ax}^b& {\nabla }_{ay}^b& {\nabla }_{az}^b\end{array}\right]}^T$$

(5)

where, $\left[\begin{array}{ccc}{\varnothing }_E& {\varnothing }_N& {\varnothing }_U\end{array}\right]$ are the attitude angle errors of the east, north and sky axes in the spraying robot coordinate system, respectively, rad; $\left[\begin{array}{ccc}\delta L& \delta \lambda & \delta h\end{array}\right]$ are the position errors of the spraying robot (latitude, longitude and height); $\left[\begin{array}{ccc}\delta V_E& \delta V_N& \delta V_U\end{array}\right]$ are the velocity errors of the east, north and sky axes in the spraying robot coordinate system, respectively, m/s; $\left[\begin{array}{ccc}{\epsilon }_{gx}^b& {\epsilon }_{gy}^b& {\epsilon }_{gz}^b\end{array}\right]$ are the three-axis gyro drift of the spraying robot, respectively, rad/s; $\left[\begin{array}{ccc}{\nabla }_{ax}^b& {\nabla }_{ay}^b& {\nabla }_{az}^b\end{array}\right]$ are the three-axis accelerometer drift of the spraying robot, respectively, m/s².

The transfer matrix $F\left(t\right)$ in the system state equation was obtained by decomposing the error vector equation of the IMU navigation system.

The system noise vector can be expressed as follows:

$$W\left(t\right)={\left[\begin{array}{cccccc}{\omega }_{gx}^b& {\omega }_{gy}^b& {\omega }_{gz}^b& v_{ax}^b& v_{ay}^b& v_{az}^b\end{array}\right]}^T$$

(6)

where, $\left[\begin{array}{ccc}{\omega }_{gx}^b& {\omega }_{gy}^b& {\omega }_{gz}^b\end{array}\right]$ are the white noise of the three-axis gyroscope of the spraying robot, respectively; $\left[\begin{array}{ccc}{v}_{ax}^b& v_{ay}^b& v_{az}^b\end{array}\right]$ are the white noise of the three-axis accelerometer of the spraying robot, respectively.

The noise transfer matrix can be expressed as follows:

$$G\left(t\right)=\left[\begin{array}{cc}{C}_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ 0_{9\times 3}& 0_{9\times 3}\end{array}\right]$$

(7)

where, $C_b^n$ is the attitude transfer matrix.

The measurement equation explains the nature of the integrated navigation system and relates the navigation information to the error equation of the navigation system. The position and heading angle accuracy of the dual-antenna BDS F-V111 receiver can meet the integrated navigation requirements, so the heading angle and position were used as the observed quantities. The measured value in the measurement equation was the difference of the heading angle and position output by IMU and F-V111 receiver, respectively, and the system measurement equation can be expressed as follows:

$$Z\left(t\right)=\left[\begin{array}{c}ya{w}_I-ya{w}_B\\ L_I-L_B\\ {\lambda }_I-{\lambda }_B\end{array}\right]=HX\left(t\right)+V\left(t\right)$$

(8)

where, $\left[\begin{array}{ccc}ya{w}_I& L_I& {\lambda }_I\end{array}\right]$ are the heading angle, latitude and longitude output by IMU; $\left[\begin{array}{ccc}ya{w}_B& L_B& {\lambda }_B\end{array}\right]$ are the heading angle, latitude and longitude output by F-V111; $V\left(t\right)$ is the noise of the measurement system.

The measurement matrix in system measurement equation can be expressed as follows:

$$H=\left[\begin{array}{ccc}{0}_{3\times 2}& diag\left[\begin{array}{ccc}1& 1& 1\end{array}\right]& 0_{3\times 10}\end{array}\right]$$

(9)

2.

Discrete error Kalman filter equation for BDS/IMU integrated navigation;

To meet the computer computing requirements, the continuous-time error Kalman filter equation was transformed into a discrete-error Kalman filter equation.

$$\{\begin{array}{c}{X}_k=A{X}_{k-1}+{\omega }_{k-1}\\ Z_k=H{X}_k+v_k \end{array}$$

(10)

where, $X_k$, $X_{k-1}$ are the states of the system at moments k and k − 1; $A$ is the system state transition matrix; ${\omega }_{k-1}$ is the Gaussian noise during state transfer at moment k − 1; $Z_k$ is the measured value of the system at moment k; $H$ is the system parameters of the measurement system; $v_k$ is the Gaussian noise of the measurement system at moment k.

3.

The error Kalman filter equation;

$$\{\begin{array}{c}{\widehat{X}}_{k/k-1}=A{\widehat{X}}_{k-1}\hfill \\ {\widehat{X}}_{k/k}={\widehat{X}}_{k/k-1}+K_k\left(Z_k-H{\widehat{X}}_{k/k-1}\right) \hfill \\ K_k=P_{k/k-1}{H}^T{\left(H{P}_{k/k-1}{H}^T+R_k\right)}^{-1}\hfill \\ P_{k/k-1}=A{P}_{k-1/k-1}{A}^T+Q_{k-1}\hfill \\ P_{k/k}=\left(I-K_{k}H\right)P_{k/k-1}\hfill \end{array}$$

(11)

where, ${\widehat{X}}_{k/k-1}$ is the predicted value of the state at moment k − 1 for moment k; ${\widehat{X}}_{k/k}$ is the state estimate at moment k; $K_k$ is the filtering gain at moment k; $Q_{k-1}$ is the system noise variance matrix at moment k − 1; $P_{k/k-1}$ is the covariance matrix of the prediction error estimates at moment k − 1 for moment k; $P_{k/k}$ is the covariance matrix of the error estimate at moment k; $R_k$ is the measurement noise error matrix at moment k.

2.2.3. Design of the BDS/IMU Integrated Navigation Program

The integrated navigation program is shown in Figure 5. The on-board navigation system is powered on to initialise the industrial computer, F-V111 receiver, 100D2, etc. (1) The industrial computer checks the motherboard, graphics card and memory, and initializes peripherals such as the USB port; (2) The F-V111 receiver and 100D2 are configured; (3) The parameters of the integrated navigation system are configured, including the initial zero deviation and installation error angle of the 100D2, the bar arm between the F-V111 receiver antenna and 100D2, and the initial state vector $X_0$, the initial error estimation covariance matrix $P_0$, the initial system noise variance matrix $Q_0$, the initial measurement noise variance matrix $R_0$; (4) The system enters polling and the performs static coarse alignment under the static state of the sprayer. The initial pitch angle and roll angle are calculated by the accelerometer of IMU, and the initial heading angle and longitude and latitude are calculated by F-V111 receiver; (5) The data of 100D2 is collected and the data of F-V111 receiver is waiting to be updated; (6) The longitude and latitude are obtained, and the integrated navigation solution is carried out to obtain the heading angle and position of spraying robot; (7) The system polls for the data of 100D2 and waits for the next data of F-V111 receiver update.

2.3. Control Model of the System

2.3.1. Navigation Control Principle of the Spraying Robot

The automatic navigation control system of the spraying robot was composed of the path tracking system. The structure of the path tracking system is shown in Figure 6. The path to be tracked is selected through the user interface and the automatic navigation start command is sent. The sprayer obtains the real-time information of position and heading, the path tracking algorithm searches the target point on the path and calculates the control parameters, sends the control parameters to the single chip microcomputer through the RS232 communication interface. The single chip microcomputer verifies the integrity of the control parameters, converts them into PWM signals to send to the servo motor driver, which controls the motor to drive the crawler moment. The path tracking algorithm keeps cycling until the distance between the actual position of the spraying robot and the last path point is less than 0.1 m or the automatic navigation termination command is sent out.

2.3.2. Kinematic Model of the Spraying Robot

The chassis of the spraying robot adopts a track structure and adopts the principle of sliding steering to realize steering by controlling the relative speed of the tracks on both sides. Assuming that the spraying robot does not slip when driving, and the mass division of the robot is uniform, and the center of mass is located in the geometric longitudinal symmetry line of the robot, the coordinate system of the spraying robot is shown in Figure 7. According to the right-hand rule, the center of mass CM of spraying robot is the coordinate origin, the forward motion direction is the positive direction of the X-axis, the left is the positive direction of the Y-axis, the Z-axis is perpendicular to the paper facing outward, and OC is the center of rotation center of the sprayer, $v_c$ is the linear velocity of the center of mass, $r_c$ is the turning radius of the center of mass, $v_l,v_r$ are the linear velocities of the left and right driving wheels, $d_{lr}$ is the distance between the two track centers, which is 0.62 m. The angular velocity ω of steering around the OC can be expressed as follows:

$$\omega =\frac{v_c}{r_c}=\frac{v_r}{r_c+d_{lr}/2}=\frac{v_l}{r_c-d_{lr}/2}$$

(12)

$$\omega =\left(v_r-v_l\right)/d_{lr}$$

(13)

The linear velocity of the spraying robot centroid can be expressed as follows:

$$v_c=\left(v_l+v_r\right)/2$$

(14)

The turning radius of the spraying robot can be expressed as follows:

$$r_c=\frac{\left(v_l+v_r\right)d_{lr}}{2\left(v_r-v_l\right)}$$

(15)

The forward kinematics model of the spraying robot can be expressed as follows:

$$\left[\begin{array}{c}{v}_c\\ \omega \end{array}\right]=\left[\begin{array}{cc}1/2& 1/2\\ 1/d_{lr}& -1/d_{lr}\end{array}\right]\left[\begin{array}{c}{v}_r\\ v_l\end{array}\right]$$

(16)

The inverse kinematics model of the spraying robot can be expressed as follows:

$$\left[\begin{array}{c}{v}_r\\ v_l\end{array}\right]=\left[\begin{array}{cc}1& d_{lr}/2\\ 1& -d_{lr}/2\end{array}\right]\left[\begin{array}{c}{v}_c\\ \omega \end{array}\right]$$

(17)

2.3.3. Pure Pursuit Model

The pure pursuit model is a method based on the geometric principle, in which the center of the rear axis of the spraying robot is the tangent point and the longitudinal direction of the body is the tangent line, and the spraying robot travels along the arc by controlling the steering angle. As shown in Figure 8, the spraying robot tracks the path AB, the current point C ($x_c$, $y_c$) and the forward-looking distance $l_d$ are known, and the point G ($x_g$, $y_g$) with the distance $l_d$ from the current position on the path is obtained. G is the next tracked point. R is the radius of the driving arc of the rear axle at a given steering angle, α is the angle between the body and the next tracked point. $v_c$ is the linear velocity at the center of the rear axle, ${\omega }_c$ is the steering angular velocity around the OC.

$$R=\frac{l_d}{2\sin \alpha }$$

(18)

The steering angular velocity can be expressed as follows:

$${\omega }_c=\frac{V_c}{R}=2V_c\sin \alpha /l_d$$

(19)

In each control cycle, the next point to be tracked is updated, the desired steering angle of the spraying robot in that control cycle is calculated and further the spraying robot path tracking is achieved by controlling the steering angular velocity ${\omega }_c$.

2.3.4. Path Planning

In view of the terrain characteristics and planting pattern of the orchard, the navigation path was designed as shown in Figure 9. The line with arrows represents the pre-set path, point 1 is the starting point, points 2 and 3 are the turning points, and point 4 is the stopping point. The point P is any point on the driving trajectory of the spraying robot operation, the vertical distance from the point P to the pre-navigation path is D, and D is used to evaluate the path tracking error. $D_1$ is the minimum safe distance for the spraying robot operation without touch the fruit trees. D is less than $D_1$ when the spraying robot operates, and $D_1>d_{lr}/2$.

3. Results

3.1. Static Positioning

In this experiment, the performance of the navigation system was tested under the static state of the spraying robot, and the results of BDS positioning alone and BDS/IMU combined positioning were compared. The test was conducted in the fruit tree Innovation Park of the Horticultural Research Institute of Hunan Academy of Agricultural Sciences, Changsha City, Hunan Province, China in March 2022. During the test, the spraying robot was static, and the system outputted the data of BDS positioning alone and BDS/IMU combined positioning at the same time. The deviation of position and heading angle from their corresponding mean values was used as error to measure the positioning and orientation accuracy. The heading angles and positions obtained from BDS and BDS/IMU integrated navigation system are shown in Figure 10. In BDS positioning, the position error was less than 1.5 cm, the mean values in X and Y direction were 0.413 and 0.574 cm, and the variances were 0.062 and 0.096, respectively. The heading angle error was less than 0.3°, the mean value was 129.409°, and the variance was 0.003223. In BDS/IMU integrated positioning, the position error was less than 1.5 cm, the mean values in the X and Y directions were 0.430 and 0.235 cm, and the variances were 0.060 and 0.087, respectively; The heading angle error was less than 0.25°, the mean value was 129.421°, and the variance was 0.002052.

It can be seen from Figure 10b that the signal variation in the last part of Figure 10d was caused by the variation in BDS measured heading angle. In the static state, the heading angle directly obtained by BDS itself has large random noise. At the same time, the disturbance of wind on the tree crown in the orchard block and interfere with the satellite signal to varying degrees, resulting in large fluctuations in BDS measured heading angle. Comparing Figure 10b,d, the heading angle measured by the untreated BDS fluctuates greatly. After integrating the BDS and IMU, the error caused by random drift is reduced and the accuracy of heading angle measurement is improved. The mean values of BDS/IMU integrated positioning and BDS positioning were closed, but the variance of integrated navigation was smaller, the positioning data of integrated navigation was relatively centralized, and the system was more stable. In terms of orientation, there is no obvious difference between the two. The BDS receiver with dual antennas can provide higher orientation accuracy under static conditions.

3.2. Dynamic Positioning

The performance of the navigation system was tested under the straight-line driving state of the spraying robot, and the BDS positioning and BDS/IMU integrated positioning were compared. In the test, the spraying robot ran along the preset straight line at a speed of 0.4 m/s, both BDS and BDS/IMU integrated positioning data were output. The heading angle and path data of the spraying robot for the BDS positioning and the BDS/IMU integrated positioning are shown in Figure 11. In BDS positioning, the position error was less than 5.29 cm, the mean errors in X and Y directions were 1.42 and 1.18 cm, and the variances were 1.95 and 1.35, respectively. The heading angle error was less than 3°, the mean value was 40.179°, and the variance was 0.5543. In BDS/IMU integrated positioning, the position error was less than 2.49 cm, the mean errors in X and Y directions were −0.37 and 0.31 cm, and the variances were 0.87 and 0.60, respectively. The heading angle error was less than 2°, the mean value was 40.430°, and the variance was 0.237.

It can be seen that in the moving state, the results of BDS/IMU integrated positioning are more accurate than that of BDS.

3.3. Auto-Navigation Performances in Orchard

In order to verify the reliability and rationality of the automatic navigation system mounted on the sprayer, a linear navigation test was carried out in the orange orchard, as shown in Figure 12. The spacing of planting rows was 5 m, and the ridge length was about 35 m. Considering the outstretched part of the branches of orange trees, the actual width of the walkable area between ridges was 2 m, and the width of the body was 0.62 m. Therefore, the minimum safe distance was 0.69 m, which satisfied the path planning requirements. Two continuous ridges of orange orchards were randomly selected as the test site. Before the test, the high-precision positioning board was used to collect the coordinates of points 1–4 on the planned path, the path planning scheme shown in Figure 9 was employed for the orchard path tracking test and the automatic navigation driving speed of the sprayer was 0.4 m/s. The experiment was conducted at the fruit tree Innovation Park of the Horticultural Research Institute of Hunan Academy of Agricultural Sciences, Changsha City, Hunan Province on 7 April 2022.

When the sprayer was spraying in the orange orchard, the straight-line navigation automatic driving experiments were carried out five times. The maximum lateral tracking error was as low as 8.58 cm, if the path tracking algorithm is stable. However, the maximum lateral tracking error of the spraying robot was 10.77 cm due to the orchard field soil environment, such as side slip resulting from field ridge and steep hill. However, the tracking accuracy is acceptable because of its large operation width. We can conclude that the orchard soil condition and ground-surface undulation, especially for heavier spraying robots when the medicine box is filled with pesticides. Since the heading is only employed to correct the positioning error, the lever-arm error coming from pitch and roll is not compensated. The navigation deviation information on the fourth route was shown in Figure 13. The results of the five groups of experiments were summarized in

Table 1. The results of the navigation experiments in the citrus orchard.

Line Number	Time/s	Maximum Error/cm	Minimum Error/cm	Average Absolute Error/cm	Root Mean Square Error/cm
1	109.00	8.58	0.01	3.23	1.21
2	108.01	9.36	0.06	2.96	0.35
3	108.01	10.77	0.06	3.93	1.73
4	108.01	9.98	0.11	4.16	2.37
5	110.00	9.32	0.01	3.48	0.29
Overall	/	/	/	3.55	1.19

. Through calculation, the error of the average absolute value was 3.55 cm, and the overall root mean square error of the lateral deviation absolute value was 1.19 cm. The field tests showed that the maximum offset error was no more than 10.77 cm. Take a certain navigation line to perform linear trajectory fitting, and the navigation effect is shown in Figure 14.

4. Discussion and Conclusions

To address the unstableness of the satellite navigation system, which is caused by blocking, signal loss, and random error, an integrated navigation algorithm was built using the error Kalman filter that is based on measurements of position and heading angle to enhance the performance of the navigation system for long-term operation. The BDS and BDS/IMU integrated navigation system positioning tests were carried out in the closed orchard. The static experiments showed that the positioning error of the BDS was within 1.5 cm, the heading angle error was within 0.3°, and the variance was 0.003223. The positioning error of BDS/IMU integrated navigation system was within 1.5 cm, the heading angle error was within 0.25°, and the variance was 0.002052. The results indicated that the positioning and heading angle measurement performance of the BDS/IMU integrated navigation system was slightly better than that of BDS, which had more centralized data and better stability. The dynamic straight-line driving positioning experiments showed that the positioning error of BDS was within 5.29 cm, the heading angle error was within 3°, and the variance was 0.5543. The positioning error of BDS/IMU integrated navigation system was within 2.49 cm, the heading angle error was within 2°, and the variance was 0.237. These results implied that the performance of positioning and heading angle measurements of BDS/IMU integrated navigation system was significantly higher than that of BDS, and better than the integrated navigation system using Kalman filter with non-error observation. The kinematic model and pure tracking model of the pesticide robot were established, and the path tracking control algorithm of the sprayer robot was designed. The path planning was carried out in combination with the orchard terrain, and the navigation test was conducted in the closed orchard. The test results showed that when the operating speed of the sprayer robot was 0.4 m/s, the maximum error of linear tracking was less than 10.77 cm, and the average absolute error was 3.55 cm, which was superior to the tractor automatic navigation system based on DGPS (the maximum error of straight-line tracking is less than 15 cm) and the orchard sprayer automatic navigation system based on BDS (the maximum error of straight-line tracking is less than 13 cm). The field tests indicated that the auto-navigation system was appropriated to orchard spraying robots.

In the future, the BDS/IMU integrated navigation systems should be enhanced by integrating lidar and image sensors, which not only provide higher precision position and attitude data but also ensure the navigation functionalities, such as automatic obstacle avoidance and automatic in and out of warehouses, in orchards and agricultural machinery warehouses, when the tree crown or roof completely blocks the satellite signal. In addition, although the integrated navigation method based on Kalman filter can fuse the sensor information at different rates through the data synchronization processing method and calculate the navigation solution in real time, in order to maintain the data synchronization, it is often necessary to discard some measured values, resulting in a waste of information. At the same time, the standard Kalman filter can only solve the linear problem, while most sensor models contain nonlinear components, the estimated value is not optimal, and does not support the expansion of new navigation sensors. Therefore, the nonlinear optimization method is further studied to realize the fusion of multi-sensor asynchronous measurement data, and the plug and play function can be realized according to the availability of sensors; however, there are problems remaining to be further investigated, such as the unconstrained vibrations of the spraying robot, and large horizontal deviation when it is at the corner. To reduce the horizontal deviation and vibration of the automatic navigation system, new control strategies, such as coupling with neural networks, should be implemented in the future to eliminate the impact of sideslip and row change on path tracking during spraying.