Autonomous navigation control based on improved adaptive filtering for agricultural robot

Abstract

Under the complex agricultural operation environment, reliable navigation system is the basic guarantee to realize the agricultural robot automated operation. This study focuses on improving navigation accuracy and control accuracy and conducts related research on autonomous navigation control of agricultural robots. This article discusses the advantages of using strict convergence criteria and combining Sage–Husa adaptive filtering with strong tracking Kalman filtering and then proposes an improved adaptive Kalman filter algorithm. The new algorithm can effectively suppress the filter divergence, improve the dynamic performance of the filter, and ensure its better filtering accuracy and strong adaptive ability to improve navigation accuracy of GPS. Further variable structure switching method is used to prevent proportional integral differential (PID) controller integral saturation phenomenon, which effectively solves the controller over-saturation problem. And combining this method with an improved adaptive filtering algorithm not only can effectively inhibit control interference but also achieve the anti-saturation effect, thereby enhancing the stability and accuracy of the control system. Finally, the simulation and experiment of the new method show that the proposed method greatly improves the ability of the filter to suppress divergence and control precision.

Introduction

With the progress of science and technology, and the continuous development of social informatization, as well as the scale, refinement, and diversification of agricultural production, modern agriculture has higher and higher requirements for its operation. And the human race has gradually entered the aging, the lack of agricultural labor force increasingly appears, some monotonous and heavy work will be gradually replaced by mechanized and intelligent robots. The development of computer, electronics, navigation, and other technologies has provided solid technical support for it, as well as a strong guarantee for the development of agricultural robots. As an alternative to the traditional agricultural machinery, agricultural robots in modern agriculture have begun to play an increasingly important role. ^{1 –3}

Since the end of the 1960s, the advent of mobile robots has opened a new chapter in human intelligence, which integrated real-time control, trajectory planning, target identification, and positioning, sensor, and other multisystems. With the leap in technology, the level of informatization has improved. Mobile robots have infiltrated many directions of human society and are widely used in various fields, such as industrial and agricultural production, aerospace defense, experimental teaching, and service industry. The flexibility of mobile robots restricts the popularity in practical applications, because it has become a hot topic of mobile robot research. Autonomous navigation is a basic skill necessary for mobile robots; a reliable robot navigation system is the basic guarantee for automated operation of agricultural robots. However, the navigation accuracy and reliability of the robot have always restricted the level of robotic autonomous operation, and it has become a bottleneck for agricultural automation. Agriculture navigation technology has become a hot spot of current research for agricultural robot, which also shows good progress. ^{4 –6} Agricultural machinery and equipment for autonomous navigation can not only solve the problem of insufficient labor but also improve the efficiency of agricultural production. At present, agricultural robot navigation is mainly realized by two different methods: machine vision and satellite positioning; visual navigation is sensitive to the external environment, and it is difficult to meet the requirements of automatic navigation in different environments to a certain extent ^{7 –9} ; satellite positioning and navigation has become a research popular spot in recent years, but its accuracy needs further research. ^{10 –12}

China is a great agricultural country, but the degree of agricultural mechanization and automation is generally low, but with the rising national strength and progress of science and technology, agricultural technology and productivity in rural areas has also been greatly improved. In recent years, many universities and research institutes have carried out research on intelligent vehicles and autonomous navigation system of agricultural mechanization. ^{13 –15} This article mainly studies under the farmland environment. This study is to integrate GPS/micro electro mechanical systems (MEMS) sensor signals to realize the track planning and tracking performance of mobile robots so as to realize the navigation function of mobile robots. To overcome the divergence problem of Sage–Husa adaptive filtering algorithm, we improved it with Kalman filtering algorithm to suppress the divergence of the filtering and improve the dynamic performance of the filter, proposed to improve Sage–Husa adaptive filtering algorithm to enhance its stability and improve the filtering precision. The integrated adaptive variable-structure PID (VSPID) is used to guarantee the stability of the control system and realize autonomous navigation, path planning, and tracking of mobile robots. The recent literature about real-life applications of contemporary automation techniques in different fields gives us new inspiration. ^{16 –19}

Materials and methods

Device platform

The platform of this study is a kind of fan-driven multifunctional working vehicle. The central control device of the platform includes ARM9 embedded mini2440 master controller, Linux operating system, LCD screen display information such as battery power and electronic map navigation and can realize trajectory planning. The main control signals of the central controller include driving signal of motor, working signal of the mower, position adjustment signal of the mower, working signal of the baiter, and so on. The main signals collected by the central controller include GPS and attitude module to collect longitude and latitude coordinates and directional angle signals, and visual recognition device.

Navigation system—RTK-GPS-X300 system: The accuracy is 1–5 m and 2 cm in RTK-GPS mode.

The mobile platform is equipped with a high-performance laptop, Wi-Fi communication, a built-in high-precision inertial attitude course measurement unit, and four rotary encoders as odometer. The mobile robot also has three sonar arrays front and back.

Agricultural robot modeling

Ignoring the friction between the road surface and the agricultural robot, constructing an agricultural robot vehicle model based on the vehicle geometry, Figure 1 is a wheeled vehicle turning model based on Ackerman steering geometry.

Figure 1.

Agricultural robot model

Figure 2.

Flowchart of improved adaptive filtering.

The motion model of a four-wheeled agricultural robot can be described by formula (1)

$\left\{\begin{array}{l}X={\left[\beta \hspace{0.17em}\hspace{0.17em}\gamma \hspace{0.17em}\hspace{0.17em}\delta \right]}^{\text{T}}\\ u=\delta \\ A=\left[\begin{array}{ccc}0& 0& \frac{l_{\text{r}}}{l}\\ 0& 0& \frac{V}{l}\\ 0& 0& 0\end{array}\right]\\ B={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^{\text{T}}\end{array}\right.$

1

In formula (1), V represents the longitudinal speed of the vehicle, l _f, l _r, respectively, represent the distances from the vehicle’s center of gravity to the front and rear axles, δ is the steering angular velocity, take the lateral slip angle β, the steering system angular velocity δ, and the turning angular velocity of vehicle body γ as state variable X. In this project, the average velocity of the mobile robot is V = 3 km/h the distances from the vehicle’s center of gravity to the front and rear axles is l _r = l _f = 0.2 m.

Using the ground coordinate system shown in Figure 1, the vehicle position information $\left(x,y\right)$ and the expressions ϕ of orientation information can be calculated from the state variable X. The initial state of the mobile robot is assumed as $\left(\begin{array}{ccc}{x}_0,& y_0,& {\varphi }_0\end{array}\right)$ , then the mobile robot position information can be obtained according to formula (2)

$\left\{\begin{array}{l}x=x_0+V{\displaystyle {\int }_0^t\text{sin}(\beta +\varphi )\hspace{0.17em}\text{d}t}\hfill \\ y=y_0+V{\displaystyle {\int }_0^t\text{cos}(\beta +\varphi )\hspace{0.17em}\text{d}t}\hfill \\ \varphi ={\varphi }_0+{\displaystyle {\int }_0^t\gamma \hspace{0.17em}\text{d}t}\hfill \end{array}\right.$

2

The model has been relatively simplified. From the formula, it can be known that the state of the mobile robot in the whole movement process only depends on the moving speed and the size of the vehicle structure, without considering the mechanical effect between the road surface and the wheels. In other words, if the model is used, the mobile robot will get the same result in the case of hard road and muddy road, which is obviously inconsistent with the actual situation. Therefore, the model has certain limitations, but it will not affect the experiment at the playground.

Improved adaptive filtering

Adaptive filtering ^{20 –23} is a filtering method that can suppress the divergence of the filter. In the filtering calculation, on the one hand, the predicted values are continuously corrected by the measurement, and unknown or uncertain system model parameters and noise statistical parameters are estimated or corrected, so that the filter gain is automatically adapted to the observed data to reduce the estimation error.

Systematic mathematical model

Positioning agricultural mobile robot is the most basic part of mobile robot navigation, that is, the initial position of the mobile robot is known, then the pose position and orientation of robot are estimated and corrected by cumulative errors of the heading attitude system. As the uncertainty errors came into being with the process of agricultural robot movement under the interference of the complex external environment, a large cumulated error of pose position and orientation will be generated when the position and pose are updated with the heading attitude system, which will reduce the positioning accuracy. Kalman filter can effectively improve the accuracy of the attitude. For the agricultural robot described in formula (2), the discrete Kalman filter model can be expressed as

$X_k={\varphi }_{k-1}{X}_{k-1}+{\varphi }_{k-1}{u}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}$

3

Based on the metrical information of the system, the status variable of the system is taken as $X_k={\left[\begin{array}{cccc}{x}_k& y_k& {\epsilon }_{v_k}& b_k\end{array}\right]}^{\text{T}}$ , among $x_k,y_k$ indicate longitude and latitude, respectively, ${\epsilon }_{v_k}$ and b_k indicate the measurement error of the speed and the drift error of the gyroscope, respectively. ${\varphi }_k$ is the deterministic estimation matrix. Therefore, the status equation and the measurement equation of the system are

$Z_k=H_k{X}_k+V_k$

4

In formula (3), the state-transform matrix of the system from time t_k to time $t_{k+1}$ is

${\Phi }_k=\left[\begin{array}{cccc}1& 0& \Delta t\text{sin}{\varphi }_f{}_k& V_{G_k}\Delta t\text{cos}{\varphi }_f{}_k\\ 0& 1& \Delta t\text{cos}{\varphi }_f{}_k& V_{G_k}\Delta t\text{sin}{\varphi }_f{}_k\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

5

${\Gamma }_{k-1}$ is the noise driving matrix of the system, W_k is the system excitation noise sequence, $W_k=[0\hspace{0.17em}0\hspace{0.17em}{\xi }_{V_k}\hspace{0.17em}{\xi }_{{\varphi }_k}]$ , ${\xi }_{V_k}$ and ${\xi }_{{\varphi }_k}$ represent the Gaussian noise associated with velocity and orientation, respectively, H_k is the measurement matrix, $H_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$ , V_k is the measurement noise sequence, $V_{G_k}$ is the true speed, and ${\varphi }_{f_k}$ is the true azimuth angle.

Sage–Husa adaptive filtering

The Sage–Husa adaptive filtering algorithm can be described as

$\left\{\begin{array}{c}{X}_{k|k-1}={\Phi }_{k,k-1}{X}_{k-1}\\ K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^{\text{T}}+R_k)}^{-1}\\ P_{k|k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^{\text{T}}+Q_{k-1}\\ {\upsilon }_k=Z_k-H_k{X}_{k|k-1}\\ X_k=X_{k|k-1}+K_k{\upsilon }_k\\ P_k=\left(I-K_k{H}_k\right)P_{k|k-1}\end{array}\right.$

6

In the formula, K_k is the filter gain matrix, ${\upsilon }_k$ is the discrepancy vector, and R_k is the measure noise variance matrix.

$R_k=\left(1-d_k\right)R_{k-1}+d_k\left({\nu }_k{\nu }_k^{\text{T}}-H_k{P}_{k,k-1}{H}_k^{\text{T}}\right)$

7

where d_k is the weighting coefficient

$d_k=\left(1-b\right)/\left(1-b^k\right)$

8

Forgetting factor $0<b<1$ (generally, the value is 0.95–0.99).

Using this algorithm, the calculation is simple and has strong adaptability. To improve the filtering dynamic response speed, the memory length of the filter is limited by the forgetting factor b, and the filter can make timely estimation with the change of the measured value.

Strong tracking Kalman filtering algorithm

To ensure the convergence of the filter, the filtering stability is exchanged at the expense of a certain precision, such as increasing the process noise of the system and the variance matrix of the observed noise, and a large number of unmodeled errors are included to make the algorithm simple and reliable. The strong tracking Kalman filtering idea ^{24 –26} is to multiply the weighted coefficient ${\lambda }_k$ in front of the pretest covariance matrix of the status estimation error, as shown in equation (9). The method has a strong tracking capacity to mutation

$P_{k,k-1}={\lambda }_k{A}_{k-1}{P}_{k-1}{A}_{k-1}^{\text{T}}+Q_{k-1}$

9

Among,

${\lambda }_k=\text{diag}\left[{\lambda }_{1,k},{\lambda }_{2,k},\dots ,{\lambda }_{n,k}\right]$

10

When the status is abrupt, the weighting coefficient ${\lambda }_i{}_k$ increases, so that the tracking ability of the filter is also enhanced, and the reliability of the system is improved.

Improved adaptive filtering algorithm

Sage–Husa adaptive filtering algorithm and strong tracking Kalman filtering algorithm have their advantages and disadvantages, respectively. The accuracy of the former is higher but the computational cost is large; the latter has simple structure, high reliability, and strong adaptive capacity, but the filtering precision is reduced. In this study, two filtering algorithms are organically combined to form a filtering algorithm, and square root filtering is added to calculate the posterior state to ensure the symmetry and semi-positiveness of the system variance matrix that can not only ensure the filtering convergence but also maintain high accuracy, as shown in Figure 2.

Since $v_k^{\text{T}}{v}_k$ is the quadratic sum of the innovation sequence and contains the actual estimation error information, it can be used to describe the actual estimation error, while the theoretical prediction error is expressed by the variance matrix $E\left[v_k{v}_k^{\text{T}}\right]$

$E\left[v_k{v}_k^{\text{T}}\right]=H{P}_{k,k-1}{H}^{\text{T}}+R_k$

11

Therefore, the below equation could be used

$v_k^{\text{T}}{v}_k\le r\cdot \text{tr}\hspace{0.17em}E\left[v_k{v}_k^{\text{T}}\right]$

12

to determine whether the filter converges. In the formula, $r\ge 1$ is the adjustable coefficient. When the filter meets the convergence criterion, the Sage–Husa algorithm is used to estimate the state optimally. When the convergence criterion condition is not met, it means that the actual error has exceeded r times predicted by the theory; at this point, the filter has a divergence trend, and it needs to adjust the $P_{k,k-1}$ value with the weighted coefficient ${\lambda }_k$ , so that it can better play the role of measurement value to suppress the divergence of the filter.

In practical engineering, the residual error is usually used to calculate the check quantity to determine whether the filter is in normal working state. If normal, it is estimated by general filtering, otherwise, the fading factor ${\lambda }_k$ value is estimated at any time according to the calculation. Here, we use the strictest criterion of convergence, which is $r=1$ , then

$v_k^{\text{T}}{v}_k=r\cdot \text{tr}\hspace{0.17em}E\left[v_k{v}_k^{\text{T}}\right]=H\left({\lambda }_k{A}_{k-1}{P}_{k-1}{A}_{k-1}^{\text{T}}+Q_{k-1}\right)H^{\text{T}}+R_k$

13

The fading factor ${\lambda }_k$ to prevent the filter from divergence is

${\lambda }_k=\frac{v_k^{\text{T}}{v}_k-\text{tr}\left[H{Q}_{k-1}{H}^{\text{T}}+R_k\right]}{\text{tr}\left[H{A}_{k-1}{P}_{k-1}{A}_{k-1}^{\text{T}}{H}^{\text{T}}\right]}$

14

So, ${\lambda }_k$ value is very important to control the divergence of the filter effectively. In the formula, the elimination factor ${\lambda }_k\ge 1$ .

The algorithm is based on the weight algorithm through which the estimation error of a one-step predicting variance matrix $P_{k,k-1}$ lets the filter gain automatically adept to the observation data and reduces the estimation error to achieve the purpose of suppressing the divergence of the filter, showing more flexible and reliable advantages.

Steps of the improved adaptive filtering algorithm

Aiming at the limitations of standard Kalman filtering, the traditional Sage–Husa estimator is improved by combining the idea of square root filtering, that is, the new linear adaptive square root Kalman filtering algorithm, as shown in Figure 2. The specific algorithm is as follows:

Step 1:Initialize, $\mathit{k}=0$

$\left\{\begin{array}{c}\widehat{x}\left(0\right)=E\left[x\left(0\right)\right]\\ S\left(0\right)=\text{chol}\left\{E\left[\begin{array}{cc}x\left(0\right)-\widehat{x}\left(0\right)& {\left(x\left(0\right)-\widehat{x}\left(0\right)\right)}^{\text{T}}\end{array}\right]\right\}\\ \sqrt{\widehat{Q}\left(0\right)}=S\left(0\right)\\ \sqrt{\widehat{R}\left(0\right)}=\text{chol}\left\{E\left[\begin{array}{cc}z\left(0\right)-\widehat{z}\left(0\right)& {\left(z\left(0\right)-\widehat{z}\left(0\right)\right)}^{\text{T}}\end{array}\right]\right\}\end{array}\right.$

15

Step 2: Initialize, $k=1,2,\dots ,n$ , to do iteration

• 1. Time to update

$\left\{\begin{array}{c}{\widehat{x}}^{-}\left(k\right)={\Phi }_{k,k-1}\widehat{x}\left(k-1\right)+B_{k,k-1}u\left(k-1\right)\\ S^{-}\left(k\right)=qr\left\{\left[\begin{array}{cc}{\Phi }_{k,k-1}s\left(k-1\right)& \sqrt{\widehat{Q}\left(k-1\right)}\end{array}\right]\right\}\end{array}\right.$

16

• 2. Measure the update

Calculate the measurement residuals

$\overline{z}\left(k\right)=z\left(k\right)-H\left(k\right){\widehat{x}}^{-}\left(k\right)$

17

Estimate the statistical characteristics of measurement noise

$\left\{\begin{array}{c}\sqrt{R^{\text{**}}}=\text{chol}\left[\sqrt{1-d\left(k\right)}\sqrt{\widehat{R}\left(k-1\right)},\left|\overline{z}\left(k\right)\right|,d\left(k\right)\right]\\ \sqrt{R^{\text{*}}}=\text{chol}\left[\sqrt{R^{\text{**}}},H\left(k\right)S^{-}\left(k\right),-d\left(k\right)\right]\\ \sqrt{\widehat{R}\left(k\right)}=\text{diag}\left[\sqrt{\text{diag}\left(\sqrt{R^{\text{*}}}{\sqrt{R^{\text{*}}}}^{\text{T}}\right)}\right]\end{array}\right.$

18

Calculate the gain of filtering

$\begin{array}{l}{P}_{xz}\left(k\right)=S^{-}\left(k\right){\left[S^{-}\left(k\right)\right]}^{\text{T}}{\left[H\left(k\right)\right]}^{\text{T}}\\ S_z\left(k\right)=qr\left[\begin{array}{cc}H\left(k\right)S^{-}\left(k\right)& \sqrt{\widehat{R}\left(K\right)}\end{array}\right]\\ K\left(k\right)=P_{xz}\left(k\right){\left\{S_z\left(k\right){\left[S_z\left(k\right)\right]}^{\text{T}}\right\}}^{-1}\end{array}$

19

Estimate the corrected status value

$\widehat{x}\left(k\right)={\widehat{x}}^{-}\left(k\right)+k\left(k\right)\overline{z}\left(k\right)$

20

Find the square root of the posterior status variance

$\left\{\begin{array}{c}U=K\left(k\right)S_z\left(k\right)\\ S\left(k\right)=\text{chol}\left[S^{-}\left(k\right),U,-1\right]\end{array}\right.$

21

Update and estimate the statistical characteristics of process noise

$\left\{\begin{array}{c}\sqrt{Q^{\text{**}}}=\text{chol}\left[\sqrt{\widehat{Q}\left(k-1\right)},\left|\widehat{x}\left(k\right)-{\widehat{x}}^{-}\left(k\right)\right|,d\left(k\right)\right]\\ \sqrt{Q^{\text{*}}}=\text{chol}\left[\sqrt{Q^{\text{**}}},U,-d\left(k\right)\right]\\ \sqrt{\widehat{Q}\left(k\right)}=\text{diag}\left[\sqrt{\text{diag}\left(\sqrt{Q^{\text{*}}}{\sqrt{Q^{\text{*}}}}^{\text{T}}\right)}\right]\end{array}\right.$

22

Compared with the standard Kalman and Sage–Husa algorithms, the improved algorithm directly calculates the square root of the system state variance matrix, ensuring the symmetry and semi-positive character of the system variance matrix, overcoming the shortcoming of filtering divergence due to the loss of nonnegative character of the system state variance matrix caused by the accumulation of rounding error of standard Kalman formula.

VSPID controller based on adaptive Kalman filtering

In the course of the robot’s navigation and tracking, the tracking of the target path requires the feedback control of the vehicle’s lateral deviation and course deviation. In classical control, theory of PID control is the most commonly used because the PID control algorithm is most widely used in the engineering application of control algorithm, the advantages including simple algorithm process, strong robustness and high reliability, especially in the vehicle control, often based on position deviation and course Angle deviation for the input method of PID control.

For mobile robots, it is difficult to build a precise mathematical model. In the navigation process, the control algorithm cannot well suppress the system error, and it may bring unstable factors to cause divergence of the navigation system. The saturation phenomenon of actuator generally exists in the control system. When the deviation produces a jump change, the PID output will increase or decrease sharply. If the actuator reaches saturation, the static difference cannot be eliminated, and the integral term will continue to increase or decrease. If this nonlinear effect is not considered in system design, it will affect the performance of the system in large signal input, which may cause large overshoot and even cause the instability of the whole control system.

Anti-saturation VSPID controller

For the phenomena of integral saturation, VSPID is used in this article to cope with integral saturation. The controller structure is shown in Figure 3.

Figure 3.

Diagram of anti-windup VSPID controller. VSPID: variable-structure PID.

By adjusting the adaptive coefficient, the VSPID controller integrates the saturation error $u_n-u_s$ . When u_n enters the saturation state, the VSPID controller will dynamically prompt the integrator to pull u_n out of the saturation region edge. VSPID uses adaptive coefficient η to adjust the integrator

$\Delta \eta ,\eta =\left\{\begin{array}{cc}e& u_n=u_s\\ -\alpha \left(u_n-u_s\right)/K_I& u_n\ne u_s,e\left(u_n-\overline{u}>0\right)\end{array}\right.$

23

where the constant $\alpha >0$ makes u_n rapidly converge to the nearest extreme point in the interval $uma{x}_{\text{min}}$ .

$\overline{u}=(uma{x}_{\text{min}}$ , $u_{\text{max}}$ and $u_{\text{min}})$ are the maximum and minimum values of the control input signal, respectively.

VSPID control algorithm based on anti-windup is as follows

$u\left(t\right)=k_{p}e\left(t\right)+k_i\eta +k_{d}e\left(t\right)$

24

VSPID controller with adaptive Kalman filtering

In this study, the improved adaptive Kalman filter is combined with VSPID controller to reduce the noise impact and improve the control quality. The position error signal e can be obtained by comparing the current position information and the filtered position information of the agricultural robot. The error signal e is applied to the VSPID controller to obtain the output u of the controller. In the control system, the parameters and structure of the VSPID controller can be changed in real time according to the error signal e, so it has strong robustness, as well as can reduce the requirements for system model accuracy. After the control quantity u is superimposed with the control interference signal w, it acts on the controlled object (agricultural robot) to obtain the input quantity of the filter, and the signal is filtered by the filter, it obtains the filtered position signal y_e . The system structure diagram is as shown in Figure 4.

Figure 4.

Diagram of VSPID control system based on improved Kalman filter. VSPID: variable-structure PID.

In Figure 3, r represents the controlled amount, in this study, it represents the position signal of the agricultural robot, v is the measurement noise, $Object$ is the controlled object (in this study it refers to the agricultural robot), and y represents the output of the controlled Object. y_v is the output of the controlled object after superimposing the noise, that is, the input of the filter, y_e is the output result of the filter, that is, the filtered position signal, and e, w, u are the same as above.

Experimental results and discussion

Simulation experiment of improved adaptive filtering

To verify the accuracy of the improved filtering algorithm, simulation experiment is arranged and the continuous 800 sets of GPS fixed-point data are collected. The longitudinal axis is the longitude, the transverse axis is the number of iterations, and the positioning data are filtered by three filtering methods respectively, and Figures 5 and 6 are displayed intuitively.

Figure 5.

Comparison of the Sage–Husa and improved adaptive filtering.

Figure 6.

Comparison of the ST-Kalman and improved adaptive filtering.

It can be seen from the Matlab simulation that the filtering divergence can be effectively restrained by adding the fade factor, but the weight of the old quantity is too large to make accurate estimation in time with the change of the measured value. The improved algorithm pays much attention to the function of updating sequence ${\upsilon }_k$ of residual error, when the system state estimation is carried out, the noise covariance matrix R_k is estimated to improve the filtering accuracy. After the forgetfulness factor b is introduced, the weight of the new measured value is increased, making the estimation closer to the real value.

When solving practical problems, the established mathematical model cannot truly reflect the actual physical process, and when the model does not match the measured value obtained, the filter will diverge. In this article, based on Sage–Husa filtering algorithm, by establishing the one-step estimation error variance matrix $P_{k|k-1}$ weighting algorithm to automatically adapt the filtering gain to the observation data, reduce the estimation error, and achieve the purpose of suppressing the divergence of the filtering, which shows more flexible and reliable advantages.

Simulation experiment of VSPID controller

Experiment of anti-saturation VSPID controller

The PID and VSPID controllers were simulated by Simulink, and the square wave signal with a amplitude of 5 V was tracked with a time of 10 s, and the control input was limited to [−5,5]. The results are shown in Figures 7 and 8. Among them, $T=1\hspace{0.17em}\text{ms}$ , $T=1\hspace{0.17em}\text{ms}$ , $T=1\hspace{0.17em}\text{ms}$ , and $T=1\hspace{0.17em}\text{ms}$ .

Figure 7.

The square wave response of traditional PID when integral windup.

Figure 8.

The square wave response of anti-windup VSPID. VSPID: variable-structure PID.

As shown in Figure 8, although the anti-saturation constant $T=1\hspace{0.17em}\text{ms}$ is a little small, the VSPID controller performs satisfactorily, arrives quickly, and maintains the expected value, while the traditional PID cannot exit the saturation state.

The selection of parameter $T=1\hspace{0.17em}\text{ms}$ has a great impact on the performance and stability of VSPID because VSPID eliminates the competition between the anti-windup $T=1\hspace{0.17em}\text{ms}$ and the integrator $T=1\hspace{0.17em}\text{ms}$ under normal operation, and there is no need to pursue high gain to remain in the linear zone.

Because the driving speed of the agricultural robot in this system is low, the main requirement is that the overshoot of the controller response curve is higher, and the square wave response overshoot of the anti-windup VSPID control becomes smaller, so the VSPID controller can achieve a relatively stable control effect. Moreover, at the current rise time, the response speed of the system can be satisfied due to the low speed of the robot.

As shown in Figure 6, when $T=1\hspace{0.17em}\text{ms}$ , the tracking could not achieve satisfactory results, and for the discrete system, the integral dynamics of the linear region would also be affected by $T=1\hspace{0.17em}\text{ms}$ .

Experiment of improved VSPID controller

• Simulation example: A mobile robot is used as a control object, the sampling interval is taken as $T=1\hspace{0.17em}\text{ms}$ , and the simulation time is 5 s. The control noise w and measurement noise v are both Gaussian white noise of 0.001 v, and the input signal is square wave signal of amplitude 5 V. The improved adaptive filtering is used to filter VSPID, suppose $Q=1$ , $R=1$ (see Figures 9 and 10).

Figure 9.

Waveform of VSPID with non-filtering. VSPID: variable-structure PID.

Figure 10.

Waveform of VSPID based on improved filtering. VSPID: variable-structure PID.

It can be seen from the square wave response diagram that the VSPID method quickly pushes the system out of the saturation area, and the tracking performance remains good, while the traditional PID has obvious tracking error.

Outdoor navigation experiment

This experiment was conducted in Dongshan playground of Jiangsu University to prove the feasibility and effectiveness of the above method. The red line of 1–2–3–4 is the planned path of the mobile robot, and the yellow line is the navigation and tracking curve of the system. The navigation path diagram of each algorithm is as shown in Figure 11.

Figure 11.

Trajectory diagram of initial condition navigation.

As shown in Figure 11, in the case of unstable signal or large disturbance, although the mobile robot can complete the established route, its accuracy and control efficiency need to be further improved. From the navigation chart, it can be seen that from the starting point to the no. 1 navigation point, the tracking trajectory of the robot is relatively good and the driving is relatively stable. From the navigation point 1 to the navigation point 2, it initially follows the established route, and then the error increases rapidly and the tracking efficiency decreases rapidly. Tracking effect from navigation point 2 to navigation point 3 is general. From the navigation point 3 to the navigation point 4, the intensity of geomagnetic interference increased, and the tracking route became active. Although it finally reached the navigation point 4, the path was extremely poor. The navigation experiment under original condition failed.

The controller used in the original condition navigation is simple, cannot eliminate the static error of steady state, and has weak anti-interference ability. When the signal interference is large, the control system becomes saturated and caused system unstable easily. In the early stage of the experiment, the tracking effect is good, basically walking along the established route, but with the accumulation of errors and signal interference, the implementation efficiency of the controller is reduced, which can even lead to the robot tracking “out of control.”

As shown in Figure 12, the improved adaptive filtering algorithm and the anti-saturation VSPID control technology proposed in this article were applied to mobile robots. From the experimental navigation map, it can be seen that the robots basically completed the established route, and the navigation effect was good.

Figure 12.

Trajectory diagram of navigation by improved self-adaption filtering and VSPID. VSPID: variable-structure PID.

The new method improves the signal accuracy, speeds up the response speed of the navigation system, and eliminates the influence of static error. It is applied to navigation system of mobile robot to improve navigation accuracy, anti-interference ability, and to ensure control efficiency.

As shown in Figure 13, repeated tests were carried out on the new technology and constantly adjust the parameter forgetting factor b (0 < b < 1), finally satisfactory tracking effect was obtained.

Figure 13.

Trajectory diagram of navigation experiment after parameter coordinated.

Conclusion

The improved adaptive filtering algorithm can effectively reduce the system status error and restrain the signal divergence. The system is guaranteed to be real time, stable, and adaptive, and the filtering precision is improved. The improved VSPID controller can greatly improve the control efficiency by selecting appropriate parameters and realize the integral switching and saturation error feedback mechanism to maintain good tracking performance by selecting appropriate parameters. The new method can greatly improve the precision of GPS positioning and enhance the anti-interference ability of the system. After further adjustment of the parameters, the established path tracking achieved the desired effect.