Research on Path Tracking of Unmanned Spray Based on Dual Control Strategy

Abstract

The high clearance spray is a type of large and efficient agricultural machinery used for plant protection, and path tracking control is the key to ensure the efficient and safe operation of spray. Sliding mode control and other methods are commonly used abroad to track vehicles, while fuzzy control, neural networks and other methods are commonly used at home. However, domestic and foreign research on autonomous agricultural machinery is mainly focused on tractors and other machinery, while research on self-propelled spray in high clearance is less abundant. This paper takes the path tracking algorithm in the integrated navigation system of spray as the main research goal, studies the path tracking control algorithm for straight lines and turning curves that can realize the automatic driving of spray by establishing the path tracking algorithm for unmanned spray based on dual control strategies, designs the path tracking controller, including the preview model theoretical path tracking controller and variable domain fuzzy controller, and determines the preview model through the design of the preview model theoretical path tracking controller. The lateral and longitudinal errors of the model algorithm are analyzed, and the driving characteristics under the complex spray road surface are analyzed. The design of the variable domain fuzzy predictor theory path tracking controller is proposed, and the design of the road model selection controller is calculated and analyzed in detail, including the determination of the road roughness coefficient and the selection of the range of the difference between the average value of the excitation before and after sampling, which improves the performance of the spray path tracking algorithm. The experiment shows that the proposed path tracking control algorithm can meet the path tracking requirements of unmanned spray in the current road environment, and provide a reliable solution for the automatic control of high clearance spray.

1. Introduction

High clearance self-propelled spray is a type of large and efficient agricultural machinery used for plant protection, and an important piece of machinery for field operation management [1]. Its application scope is not only limited to the pre-sowing and pre-seedling soil treatment of large-scale economic crops, as well as the prevention and control of diseases and pests, but also has the characteristics of being suitable for the mid- to late-stage disease and pest control of high stem crops such as sorghum and corn [2,3,4].

The working environment of high-altitude gap plant protection machinery is relatively complex, and the driver’s operation is affected by factors such as vision and plant obstruction, which can easily lead to seedling damage [5]. Therefore, the implementation of autonomous driving technology for high gap plant protection machines will play an important role in agricultural machinery operations. This technology can not only improve the accuracy of operations, but also effectively increase crop yield, further promoting the standardization level of agricultural mechanization production [6,7,8]. To achieve routine operations of spraying machinery, including spraying, walking, turning, etc., many scholars at home and abroad have conducted research on automatic driving of tractors and plant protection machinery [5,9] and navigation control systems [10,11,12,13], and have achieved certain research results. In the process of autonomous driving operations, real-time positioning and navigation control need to be carried out based on the research object of tool path tracking [14,15,16,17].

Scholars at home and abroad have conducted less research on the spray with high clearance, and most of the research focuses on the tractor field, optimizing navigation algorithms and path tracking algorithms. Path tracking algorithms include sliding mode control, fuzzy control, nonlinear control, neural network control, etc., for tracking planned trajectories.

Path tracking is used to ensure that the spray can accurately track along the expected operating path through the control algorithm. When the spray deviates from the predetermined path, the path tracking control will adjust the wheel angle to ensure the accuracy of path tracking. Path tracking control is the key to achieve efficient and safe operations [18,19,20,21].

2. Design of Path Tracking Controller for Spray

The key point to realize unmanned driving of spray is the design of a path tracking controller, and the unmanned control algorithm is the key to design the controller. Its main principle is to minimize the error between the expected path of the implement and the actual driving path [22,23]. The preview model has better adaptability to flat roads. Considering that the spray needs a fast and accurate path tracking effect in both flat roads and fields, this paper selects the preview model controller to control, and calculates the front wheel angle of the spray through the relevant parameters of the arc trajectory from the rear wheel center point to the preview point, such as curvature, the distance from the rear wheel to the preview point, etc.

2.1. Establishment of Preview Model Algorithm

The preview tracking model is a path tracking control method based on geometry. It introduces the concept of preview point, takes the rear axle center of the spray as the reference point, selects the target point on the required path at the specified appropriate distance (preview distance), calculates the front wheel steering angle according to the real-time monitored lateral and heading deviation, and enables the spray to reach the desired target point along the arc of the preview point within a certain frequency [24,25,26].

As shown in Figure 1, xoy is the navigation coordinate system, XOY is the vehicle coordinate system, and OY is the vehicle direction. The front wheel angle of the spray is $\delta$, which is the included angle between the front wheel and the Y axis of the vehicle. The planned path intersects with the vehicle translation at point P, where the coordinates are (p1, p2). The angle between the planned path and the vehicle translation line is an ${\varphi }_e$ error. The arc from point O on the rear wheel of the vehicle to point P is the target track. The curve OP is a circular arc drawn by OA as the radius, with $\chi$ as its calculated curvature, $\gamma$ as the angle corresponding to the curve OP, $l_p$ (straight line OP) as the preview distance, and $l_d$ (straight line OK) as the lateral error. According to the angle $\gamma$ between the center of the circle, it can be concluded that:

$$p1=\frac{1}{\chi }-\frac{1}{\chi }\cos \gamma =\frac{1-\cos \gamma }{\chi }$$

(1)

$$p2=\frac{\sin \gamma }{\chi }$$

(2)

Then the preview distance $l_p$ can be obtained:

$$l_p=\sqrt{{(p1)}^2+{(p2)}^2}$$

(3)

Among them, the curvature is negative when driving counterclockwise, and positive when driving clockwise. When the forward direction is on the right side of the expected navigation path, the lateral offset is positive, and when it is on the left side of the expected path, the lateral offset is negative. The difference between the current heading angle and the expected path heading angle is the yaw angle.

In Figure 1, there is a geometric relationship between ΔOPK, ΔOHG, and ΔPKH:

$$\left\{\begin{array}{l}p1=OG=OH\cdot \cos {\varphi }_e\\ OH=KH+l_d\\ KH=PK\tan {\varphi }_e\end{array}\right.$$

(4)

which can be derived from the sine, cosine, and tangent theorems:

$$p1=(\tan {\phi }_e\sqrt{{l_p}^2-{l_d}^2}+l_d)\cos {\phi }_e$$

(5)

The curvature can be derived from Formulas (1)–(5):

$${\chi }^2=\frac{2(1-\cos \gamma )}{{l_p}^2}=\frac{2\cos {\phi }_e(\tan {\phi }_e\sqrt{{l_p}^2-{l_d}^2}+l_d)}{{l_p}^2}$$

(6)

Based on Figure 1, the dynamic model, the tangent relationship of the steering angle, and Equation (6), it can be inferred that:

$$\left\{\begin{array}{l}\dot{\psi }=v\chi =\frac{v\tan \delta }{l_c}\\ \tan \delta =\chi l_c\end{array}\right.$$

(7)

When the wheel turns right, the steering angle is positive, and when turning left, the steering angle is negative. $\dot{\psi }$ is the rate of change in heading angle, and the steering angle is obtained from the above two equations:

$$\delta =\arctan (\frac{l_c}{l_p}\sqrt{2\cos {\phi }_e(\tan {\phi }_e\sqrt{{l_p}^2-{l_d}^2}+l_d)})$$

(8)

The steering angle is determined by the wheelbase $l_c$, preview distance $l_p$, lateral error $l_d$, and heading angle error ${\varphi }_e$ of the vehicle itself, as determined by the above equation. Both lateral and heading errors need to be calculated.

Figure 2 is the schematic diagram of the definition of spray travel coordinates, when the spray moves along the planned path, the path is punctuated, which is composed of P0, P1, …, Pk−1, Pk, Pk+1, …, Pk in turn. Set the initial vehicle system coordinates of the spray (x0, y0), and take the line oP as the preview distance approximately.

Given the initial vehicle coordinate system (x0, y0), the preview point and another intersection point T between the arc and the planned path have been determined. If the planned path is a straight line, then the lateral error that will occur is $l_d$:

$$l_d=\frac{\left|A{x}_0-y_0+B\right|}{\sqrt{1+A^2}}$$

(9)

The heading angle error ${\varphi }_e$ is determined by the difference between the output value ${\psi }_i$ of the inertial element and the angle ${\psi }_{EX}$ between the obtained line and the horizontal axis of the navigation coordinate system. It is judged as positive or negative based on the angle between ${\psi }_{EX}$ and the horizontal axis.

$${\varphi }_e={\psi }_i-{\psi }_{EX}$$

(10)

Due to the fact that the steering angle of the front wheels is considered a small angle during the steering process, therefore:

$$\tan {\phi }_e\approx {\phi }_e$$

(11)

So, the final determination of the front wheel steering angle is:

$$\delta =\arctan (\frac{l_c}{l_p}\sqrt{2\cos {\phi }_e({\phi }_e\sqrt{{l_p}^2-{l_d}^2}+l_d)})$$

(12)

2.2. Design of Path Tracking Variable Universe Fuzzy Control

When the spray travels on the road with large changes in road excitation, its path tracking control uses variable universe fuzzy control. In the preview distance $l_p$ used in the preview model, except for the speed, the selected preview cardinality and coefficient are fixed [27], and the speed changes rapidly with the change of road excitation. Variable universe fuzzy improves the parameters. A variable universe fuzzy controller with two inputs and one output is used, and the inputs of the controller are lateral deviation $l_d$ and speed $v$, respectively, output as preview distance $l_p$. The preview distance $l_p$ is rephrased as:

$$l_p=\alpha v+\beta l_d$$

(13)

By adjusting the stretching factor $\alpha ,\beta$, the value of preview distance $l_p$ can be changed. In fact, controlling the stretching factor is adjusting the impact of vehicle speed and lateral deviation on the preview distance, in actual control of front wheel angle $\delta$, the selection of speed $v$ and lateral error $l_d$ is inversely proportional. We will simplify the independent (α β) and attribute them to one parameter α. Reference [28] expresses the above formula as:

$$l_p=\alpha l_d+(1-\alpha )v,\alpha \in [0,1]$$

(14)

At this point, changing the scaling factor $\alpha$ can change the weight allocation of the two parameters. The commonly used formulas for scaling factor $\alpha$ are as follows:

$$\begin{array}{cc}\{& \begin{array}{}\mathrm{(15)}& \alpha =\lambda {\left|\frac{l_d}{E}\right|}^{\mu }+\epsilon ,\hspace{1em}0<\mu <1,0\le \epsilon <R^{+}\hfill \mathrm{(16)}& \alpha =1-\lambda \exp (-k{x}^2),\hspace{1em}\lambda \in (0,1),\hspace{1em}k>0\hfill \end{array}\end{array}$$

According to the description in the literature [8,29,30,31], the above two scaling factors can achieve good simulation results in the simulation environment, but the processor performance should also be considered in practical application, and the algorithm with a moderate amount of calculation to meet the real-time performance of the processor should be selected. Formula (16), compared with (15), requires an integration operation in each step of the calculation, which requires a large amount of calculation, and requires high performance of the spray processor. Three parameters in Formula (15) need to be adjusted, however, through early parameter adjustment and formula simplification, the spray processor can meet the calculation requirements, so Formula (15) is selected as the expansion factor and simplified as follows:

$$\alpha =\lambda {\left|\frac{l_d}{E}\right|}^{\mu }, 0<\mu <1$$

(17)

Among them, $\lambda$ is the coefficient parameter, $E$ is the maximum value of lateral deviation, and $\mu$ is the power of the adjustment function.

The input universe is represented as $X_1$, $X_2$, and the output universe is $Y$. Among them, the maximum universe $X_1[0,{\alpha }_1{u}_1]=[0,10]$ of the lateral deviation $l_d$ (m) of the spray, the basic universe [0, u] = [0, 1], the fuzzy quantization set is {−1, −2/3, −1/3, 0, 1/3, 2/3, 1}, and the expansion factor ${\alpha }_1$; universe $X_2[0,(1-{\alpha }_1)u_2]=[0,4]$ with a velocity of $v$ (m/s), basic universe [0, u] = [0, 1], scaling factor $1-{\alpha }_1$, fuzzy quantization set {0, 1/7, 2/7, 3/7, 4/7, 5/7, 6/7, 1}. The maximum output lateral error is $l_d$, with a maximum universe of $Y[-\beta u,\beta u]=[3,12]$. The basic universe [−u, u] = [−1, 1], and the fuzzy quantization set is {−1, −2/3, −1/3, 0, 1/3, 2/3, 1}.

2.3. Design of Road Model Selection Controller

Due to the special operating environment of the unmanned spray, including bumpy road sections and flat roads, and the operating speed is within 15 KM/h, the controller is required to have a fast response to the flat road surface and the ability to adapt to bumpy road sections at low speed. Based on this, a path tracking controller is designed according to the road excitation switching control strategy, including a variable universe fuzzy control algorithm and preview tracking control algorithm, which correspond to the bumpy road surface and flat road surface, respectively. The switching algorithm determines the algorithm model selected according to the longitudinal displacement of the sensor.

The purpose of designing path tracking algorithms is to ensure that A-level road tracking algorithms respond quickly, while B-level and above roads are adaptively controlled based on fuzzy control algorithms to achieve the best control effect. Considering that the switching algorithm is only a switch between two algorithms, three dimensions are used for control: excitation mean, excitation time, and driving speed. Before calculating the results, as shown in

Table 1. Road surface levels and control methods.

Control Methods		Road Level
		A	B	C
Speed (km/h)	[0, 10)	control mode 0	control mode 1	control mode 1
	[10, 15]	control mode 0	control mode 0	/

, it is specified that “control mode 0” represents preview control, “control mode 1” represents variable universe fuzzy control, and “/” represents not considering this operating condition.

The geometric mean of power spectral density in the time universe can be expressed as:

$$G_x\left(n\right)=G_x\left(n_0\right){\left(\frac{n}{n_0}\right)}^{-W} (n_{\min }\le n\le n_{\max })$$

(18)

In the formula, $G_x\left(n\right)$ is the spatial power spectral density, $G_x\left(n_0\right)$ is the road roughness coefficient, $n$ is the spatial frequency, in units of ${\mathrm{m}}^{-1}$, $n_0$ is the spatial reference frequency, $n_0=0.1 {\mathrm{m}}^{-1}$, and $W$ are the slopes of the logarithmic curve. $W=2$ is taken under general road conditions [32], and set T represents the range of the difference between the average excitation values before and after the selector sampling, taken as [–10, 10].

The selector switch control function $K_v$ can be determined by the speed range, and the specific formula is:

$$K_v=\left\{\begin{array}{l}0,\hspace{1em} 0\le V\le V_{set}\\ 1, V_{set}\le V\le V_{\max }\end{array}\right.$$

(19)

Among them, $V_{set}$ sets the switching speed to 8 km/h, and $V_{\max }$ sets the maximum operating speed to 15 km/h. From this, the excitation output function can be determined:

$$f_G=K_v{G}_{\overline{x}}$$

(20)

Function $f_G$ is composed of the mean excitation and driving speed. In order to increase the robustness of the algorithm, the excitation duration is increased:

$$G_{\overline{x}}(t+1)-G_{\overline{x}}(t)={\delta }_G$$

(21)

Among them, time t is the judgment sampling interval, where the value is 2 s. At this time, determine the model selector judgment condition function:

$$(f_G\ge G_{set})\&({\delta }_G\in T)$$

(22)

If the above equation is true, choose control method 1; if it is not true, choose control method 0.

3. Simulation Analysis of Path Tracking Algorithm

In order to verify the feasibility of the path tracking algorithm of the spray, modeling and simulation are carried out in Simulink for verification. The system parameters of the spray are set according to the actual situation. The road parameters are set according to the operating environment of the spray. The simulation speed and path shape are determined according to the driving road conditions of the spray. According to the driving conditions of the spray, the paths of the unmanned spray are mainly divided into two categories:

(1) When the spray runs on a flat road, the curvature of the curve is relatively small, and the speed is fast. Within the range of 10–15 km/h, it mainly deals with the situation from the hangar to the field. (2) When driving on rugged roads and fields, the spray needs to consider the complex road environment and curved roads with large curvature. The driving speed is slow, within 10 km/h.

The overall effect of the verification simulation of different road surface path tracking algorithms mainly verifies the tracking effect and algorithm switching of path tracking algorithms under different road surfaces and speeds. The overall validation of path tracking algorithms is carried out to ensure the stable operation of the entire algorithm in analyzing front wheel steering angles. According to the maximum speed of the sprayer and the consideration of driving safety, the speed of the sprayer on the road with poor road conditions will not exceed 10 km/h, so the maximum 10 km/h is selected as the B-level road speed. The normal operating speed is 6 km/h, and the maximum traveling speed of the sprayer is 15 km/h. Therefore, the algorithm is compared between the speed of 6 km/h and C-class road surface, the speed of 10 km/h and B-class road surface, and the speed of 15 km/h and A-class road surface under three typical working conditions, and the data of movement trajectory, lateral deviation and front wheel angle are compared. Select combinations of different speeds and road surfaces to verify the adaptability of the path tracking algorithm to changes in various factors for low-speed C-class road surfaces, medium speed (critical speed) B-class road surfaces, and high-speed A-class road surfaces, corresponding to normal operating conditions, intermediate conditions, and transition conditions, as well as to verify the combination of the switching algorithm and tracking algorithm.

The parameters of the simulation test are set according to the operating environment of the spray. The road parameters are A, B and C, three level roads, with an initial speed of 0.5 m/s, a mass center ground clearance of 2.2 m, a wheelbase of 3.2 m, a vehicle mass of 6000 kg, and a tire diameter of 1.25 m. This paper uses Simulink via MATLAB version R2018b for simulation.

According to Figure 3, Figure 4 and Figure 5, it can be concluded that in the simulation of bow-shaped motion trajectories at 6 km/h, 10 km/h, and 15 km/h, the preview control and variable universe fuzzy control in the straight driving stage also have good effects. In section P1P2, the road surface grade is A and the speed is 6 km/h. By comparing the lateral deviation at turning, it can be seen that the variable universe fuzzy control effect reduces the lateral deviation by about 63% compared to the preview control, with a lateral deviation of about 0.04 m. In the P2P3 segment, the road surface is classified as class B and the speed is 10 km/h. The lateral deviation output of the two control algorithms is almost the same. However, due to preview control, the response time of variable universe fuzzy control is reduced by about 3 s. Therefore, variable universe fuzzy control is selected for class B road surface at 10 km/h. In section P3P4, the road surface grade is class C and the speed is 15 km/h. It can be clearly seen from Figure 6 that the variable universe fuzzy control is not as effective as the preview control. The preview control reduces the lateral deviation by about 35% compared to the variable universe fuzzy control, and the lateral deviation is about 0.05 m. Therefore, the control strategy should choose preview control.

Based on the simulation results, the overall output strategy of the controller is adjusted, that is, variable universe fuzzy control is more suitable for operation in low-speed complex road conditions, while preview control is mainly matched with higher speed flat road surfaces. For the special point of 10 km/h, variable universe fuzzy control is selected, and for C-level road surfaces and higher speed driving, preview control is selected. By optimizing and modifying, the output road switching control timing is shown in Figure 7.

From Figure 6, it can be seen that at point P2, due to the optimization of the control strategy, the controller output remains unchanged and is 1, which is variable universe fuzzy control. At point P3, after delay confirmation, the controller output becomes 0, which is preview control and meets the expected changes. The total lateral deviation is shown in Figure 7. It can be seen that the controller output is relatively stable, and the absolute value of the maximum lateral deviation is controlled at about 0.05 m, which meets the control accuracy requirements of spray path tracking.

4. Testing Method

4.1. Software and Hardware Scheme for Unmanned Driving Path Tracking Test

The path tracking test equipment of unmanned spray is shown in Figure 8, which mainly includes spray body, inertial sensor, angle sensor, positioning and orientation receiver, a pair of external radio stations, antenna, electric steering wheel, connecting line and other equipment.

The width of the spray bar is 18 m, the ground clearance is 2.3 m, the outer diameter of the tire is 1.25 m, the RTK positioning accuracy is 1 cm, the output frequency is 20 Hz, and the IMU output frequency is 100 Hz.

4.2. Path Tracking Control System

As shown in Figure 9, the control system of the spray uses the path planning system as an input. By comparing with the BDS-IMU integrated navigation information output, the lateral error $l_d$ and heading error ${\varphi }_e$ are obtained. The front wheel angle $\delta$ is generated through the path planning algorithm. At this time, the measured value ${\delta }_m$ of the angle sensor is used as a negative feedback to correct the front wheel angle $\delta$, and is input to the electronic steering wheel to control the direction. At the same time, the spray ECU receives the speed command through the CAN bus to control the spray’s driving speed, including starting and stopping, and then the spray starts to move. The on-board Beidou navigation equipment and inertial sensor IMU obtain data feedback integrated navigation data to the path tracking system. Regarding pavement excitation, the longitudinal displacement is determined by the inertial sensor IMU, and the pavement power spectrum is established, the sampling frequency is 100 Hz.

5. Real Vehicle Testing

In order to verify the effectiveness of the path tracking control algorithm on bumpy road sections and the stability of the control strategy switching algorithm, we simulated the actual scene of the spray’s automatic driving from the flat road section to the bumpy road section, and then entering the farmland operation, selecting the 148th Regiment Shihezi, Xinjiang, China for the experiment, as shown in Figure 10. Starting from the flat road section, turn right to the bumpy road section, drive a distance, and then turn left to enter the farmland. After this, walking a certain distance in the farmland, it ends in a “Z” shape overall. On the day of the experiment, the weather was clear and there were no high buildings blocking the signal nearby. There was no standing water on the ground, and there were shallow pits on the road surface. The farmland was a plot completed by harvesting cotton, and the soil was relatively solid, with pits of different heights.

The total length of the experiment is about 340 m, with a driving speed of 10 km/h on flat roads and 5 km/h on bumpy roads and farmland. The path tracking effect is shown in Figure 11. The straight driving part basically coincides with the planned path, with errors at two turns and oscillates after turning. The lateral error is shown in Figure 12. The driving error on a flat road surface is about ±1 cm, and the error at the beginning of the turn (47th sample) is ±2.5 cm, with an error variation of about ±1.5 cm. The driving error on bumpy roads is about ±1.5 cm, with a maximum error of 2 cm (around the 110th sample), and the error change is relatively obvious. The error at the turning where it intersects with farmland is ±4.5 cm (around the 150th sample). After entering the farmland, the error was ±2 cm, with a maximum error of 4.5 cm (around the 265th sample). The lateral error peaked and quickly declined, returning to normal error. According to experimental data analysis, the lateral error generated by the path tracking algorithm is within the expected range and basically meets the target.

The control method of the spray is shown in Figure 13. When driving on the flat road, it uses preview control, and the control strategy is changed from the corner (the 39th sampling). When driving on the bumpy road and farmland, it uses variable universe fuzzy control, and there is no control strategy switching fluctuation. Around the 25th sample in the driving stage of the flat road, and near the 50th, 108th, 150th, and 260th samples in the bumpy road and farmland, the lateral deviation fluctuated significantly, and the control strategy does not switch beyond the expected design, verifying the feasibility of the algorithm under the current conditions.

6. Conclusions

The results of some foreign researchers [14,33] have shown that the minimum error on flat ground can be around 2 cm, and the maximum error can be maintained at around 10 cm. Their research results have achieved good results on a single road section, but they have not conducted detailed classification experiments on different road conditions. This is precisely the focus of this study, including the use of different algorithms on different road conditions to achieve the best path tracking effect.

The algorithm designed in this paper gives full play to the advantages of the preview model control algorithm and the fuzzy control algorithm. According to the size of the road excitation, the problem that the pre-sight control algorithm has a poor effect on complex path tracking and the fuzzy control is poor in real-time is suppressed. Compared with the single path tracking algorithm, the adaptability of the algorithm to the environment is greatly increased. The designed path tracking control algorithm can meet the algorithm switching under common roads. The preview control algorithm and the variable universe fuzzy control algorithm can effectively control the unmanned spray driving on flat roads, bumpy roads and farmland mixed roads. The driving error on flat roads is about ±1 cm, that on rough roads is about ±1.5 cm, that on farmland is ±2 cm, and the overall maximum lateral error is within 4.5 cm. The error falls back quickly, meeting the path tracking requirements of unmanned spray in the current road environment.