# Autonomous Navigation and Path Tracking Control on Field Roads in Hilly Areas

Authors: Yunwu Li; Hongwei Sun; Dexiong Liu; Junjie Xu; Mingfeng Wang
Journal: Journal of Sensors
 (2019)

---

## Abstract

Hilly areas necessitate a field road vehicle with high automation to reduce the amount of labor required to transport agricultural products and to increase productivity. In this paper, an adaptive integrated navigation method (combining global navigation satellite system (GNSS) and inertial navigation system (INS)) and path tracking control strategy of field road vehicles are studied in view of the problems of frequent GNSS outages and high automatic control precision requirement in hilly areas. An indirect Kalman filter (KF) is designed for the GNSS/INS information fusion. A modified method for calculating the KF adaptive factor is proposed to effectively suppress the divergence of the KF and a threshold judgement method to abandon the abnormal GNSS measurement is proposed to deal with GNSS interruptions. To achieve automated driving, a five-layer fuzzy neural network controller, which takes the lateral deviation, heading deviation, and path curvature as input and the steering angle as output, is proposed to control vehicle autonomous tracking of the navigation trajectory accurately. The proposed system was evaluated through simulation and experimental tests on a field road. The simulation results showed that the adjusted KF fusion algorithm can effectively reduce the deviation of a single GNSS measurement and improve the overall accuracy. The test results showed the maximum deviation of the actual travel trajectory from the expected trajectory of the vehicle in the horizontal direction was 12.2 cm and the average deviation was 5.3 cm. During GNSS outages due to obstacles, the maximum deviation in the horizontal direction was 12.7 cm and the average deviation was 6.1 cm. The results show that the designed GNSS/INS integrated navigation system and trajectory tracking control strategy can control a vehicle automatically while driving along a field road in a hilly area.


---

## Body

## 1. Introduction

Field roads in hilly areas are narrow and winding. Driving conventional vehicles along such roads is challenging, making the transportation of agricultural materials and products a severe problem for agricultural production in hilly areas. In China, most agricultural materials and products are transported by motorcycles, carts, and tricycles, among other conveyances [1]. These modes of transport have some problems, such as high labor intensity, low efficiency, and low operation safety. In addition, the labor force in hilly areas is severely lacking. These factors strongly affect the development of agriculture in hilly areas. Thus, agricultural workers in hilly areas urgently need a highly automated field road vehicle to transfer agricultural materials and products aiming to reduce the required labor intensity and increase production efficiency.The keys to the automated driving of a vehicle on field roads are autonomous navigation and trajectory tracking control. Currently, the most commonly used autonomous navigation systems are the inertial navigation system (INS), satellite navigation system, magnetic navigation system, machine vision navigation system, and combined navigation systems composed of two or more subsystems [2–4]. With the rapid deployment of real-time kinematics global navigation satellite system (RTK-GNSS), GNSS positioning performance is expected to be significantly improved in terms of availability, accuracy, reliability, and continuity. Therefore, GNSS positioning has become a ubiquitous facility in outdoor conditions, and GNSS navigation is widely used in land vehicle navigation systems. In fact, short signal blockages of GNSS will occur under restricted conditions, such as leaf attenuation and temporary obstacle shadow conditions [5]. Since the positioning performance of GNSS depends directly on the continuous tracking of satellite signals, positioning methods based on GNSS observation alone are incapable of providing a continuous navigation solution in the GNSS-constrained environment [6]. To solve this problem, GNSS is often integrated with INS. The INS provides positioning information during the GNSS outage, thus assisting the GNSS signal to reacquire after the outage and reducing the search domain required to detect and correct the GNSS cycle slip. The integrated GNSS/INS system combines the advantages of both technologies by reducing INS errors and continuously providing reliable navigation data. So, the GNSS/INS integrated navigation system has favorable complementary advantages, making it superior to other integrated navigation systems in terms of performance [4, 7].The key technology of GNSS/INS integrated navigation is the fusion of GNSS observations with INS measurements to obtain more accurate navigation information [7]. Different fusion methods such as Kalman filter (KF), extended Kalman filter (EKF), particle filter (PF), and artificial intelligence (AI) approaches have been proposed and implemented to integrate the INS and GNSS data. Among the existing information fusion technologies, the most mature and widely used method is KF and its improved algorithms [8–10]. In [11], an improved KF method was used to assemble the navigation information. The computational performance, convergence speed, and convergence accuracy were better than those of the traditional extended KF. In [12], an adaptive KF was used to combine GNSS navigation information with INS navigation information to enhance the antijamming capability of agricultural machinery in order to cope with the situation where satellite signals are blocked by windbreaks during the operation of automatic agricultural machinery in the field. In [13], an innovative fuzzy adaptive KF algorithm and an adaptive interacting multimodel algorithm based on multimodel angle were introduced to measure the varying noise characteristics of a vehicle-mounted GPS/INS integrated navigation system. Recently, several AI-based techniques, such as fuzzy neural networks [14], wavelet neural network [15, 16], and adaptive neuro-fuzzy inference system [17], have been proposed to replace or mix various filtering methods to enhance the positioning accuracy of vehicular navigation systems [18]. The basic principle behind these architectures utilizing artificial neural network (ANN) is to simulate the latest vehicle dynamics as long as the GNSS signals are available. During training, the ANN is trained to simulate the input–output functional relationships associated with INS and GNSS data. In the case of GNSS outages, these AI-based models operate in predictive mode to correct for inaccuracies in INS outputs [19]. However, due to the high inherent INS sensor errors, the nonlinear complexity of the input–output functional relationship increases and the accuracy of ANN-based architecture degrades in the case of low-cost INS. In addition, some adaptive ANN-based architectures have some limitations in parameter optimization, resulting in huge computational load, and then their real-time implementation is affected [19]. In particular, when the GNSS signal is lost, both KF- and AI-based methods will cease to be valid. However, in the first few seconds of a GNSS outage, KF might outperform some AI-based technologies [20].The goal of agricultural autonomous navigation is to control the path of the vehicle to keep it along a predetermined trajectory. The development of automatic path tracking control algorithms is critical for the realization of automated vehicle operation [21]. The path tracking methods were classified into three groups: geometric approach, kinematic control law, and optimal control [22]. In [23], a PID control algorithm was designed to control the front wheels of a navigation vehicle for automatic steering. The test results showed that this control method worked well. In [24], to make better use of machine vision navigation to control vehicles, a navigation control method based on improved particle swarm optimization adaptive fuzzy control was proposed, which has characteristics such as high response speed, small overshoot, and rapid elimination of lateral errors. In [25], an adaptive robust controller with hierarchical sliding mode was proposed, and a neural network was used for trajectory tracking and stabilization of underactuated surface vessels. In [26], a novel Nussbaum-based adaptive fuzzy control scheme was proposed for trajectory tracking of a USV in the presence of complex unknown nonlinearities and completely unknown dynamics, and the tracking errors converged to an arbitrarily small neighborhood of zero. Generally, commonly used path tracking control methods include PID control, fuzzy control, neural network control, optimal control, and combined control methods (e.g., the fuzzy neural network control method).This paper aims to solve the problem of autonomous driving of agricultural machinery on field roads in hilly areas. However, because of the strong twists and turns of field roads and occasional GNSS signal outages due to obstacles such as trees, the relative study on the autonomous navigation of field road vehicles in a hilly area is very limited. In this paper, a low-cost GNSS/INS integrated navigation method and a fuzzy neural network path tracking algorithm are proposed considering the characteristics of the complex environment, narrow road, frequent short-time GNSS outages, and high-precision requirement of automatic driving control on field roads in hilly areas. The contribution of this paper includes the following: (1) a modified KF is designed for the GNSS/INS information fusion and an adjusted method for calculating the KF adaptive factor is proposed to effectively suppress the KF divergence in GNSS short-time interruption, (2) a fuzzy neural network is used to control the automatic steering of the field road vehicle to solve the nonlinear path tracking control problem, and (3) the proposed GNSS/INS integrated navigation method and the fuzzy neural network path tracking algorithm are simulated and verified by experiments.The remainder of this paper is organized as follows. Section2 presents the architecture of integrated navigation system. Section 3 describes the establishment of adaptive KF and proposes a modified method for calculating the adaptive factor. Section 4 presents the fuzzy neural network tracking control strategy. Section 5 contains the simulation results, experimental details, and the test results.


## 2. Architecture of Autonomous Navigation System

The combined use of the GNSS and INS allows the advantages of the two systems to complement one another, synergizing these two technologies and providing more accurate and reliable navigation information. The complementary advantages of GNSS systems and INS systems are reflected in the following points: (1) the INS system is a closed, completely independent navigation system [27] that can compensate for the GNSS system’s vulnerability to interference and its large navigation errors; (2) the update frequency of INS measurements is high [28], which can compensate for the GNSS system’s low update frequency; (3) the error of the GNSS system is independent of time, which can compensate for the errors of the INS system that accumulate over time [29, 30].The field roads of hilly areas have narrow and rugged characteristics and require high precision and stability of the automated navigation system of a field road vehicle. Therefore, this paper adopts the INS as the main navigation system, uses the GNSS to correct the INS, and then formulates the loosely coupled GNSS/INS integrated navigation method to realize the autonomous navigation on field roads in hilly areas. So the GNSS/INS integrated navigation system consists of a GNSS, an INS, a main controller, and a steering system. The architecture of the system is shown in Figure1.Figure 1
Architecture of the GNSS/INS integrated navigation system.The low-cost INS used in this paper consists of a high-precision single-axis gyroscope and a 3-axis accelerometer, which can directly achieve single-axis heading angle and 3-axis acceleration. It is therefore necessary to calculate the heading angle and the angular increments of the other two axes indirectly.The GNSS is a RTK-GPS system, including a GPS base station and a vehicle-mounted GPS receiver. The GPS base station transmits its own measured GPS data and its own position coordinate information to the vehicle GPS receiver by a wireless signal transmission station. The vehicle GPS receives these data through the radio signal receiver, collects its own GPS observations, and implements a real-time kinematic (RTK) differential positioning system, which ultimately provides centimeter-level positioning results for the vehicle. The INS monitors the vehicle’s acceleration and angular velocity in real time.The positioning data obtained by the RTK-GPS and the acceleration and angular velocity data obtained by the INS are sent to the main controller. The main controller uses the designed KF to fuse the GNSS and INS data to obtain accurate and reliable navigation information. The fuzzy neural network determines the desired trajectory of the vehicle based on the navigation information.The steering system controls the swerving of the vehicle based on the calculated desired trajectory of the vehicle so that the vehicle can travel automatically.


## 3. Establishment of Adaptive KF

In the process of automated vehicle operation, the GNSS and INS measure the position and attitude information of the vehicle. Information fusion is essential for the GNSS and INS to work in conjunction. This paper uses the adaptive indirect KF to fuse the GNSS/INS data, utilizing the error equations of the GNSS/INS system to establish the KF and then the estimated error to correct the navigation parameters to formulate more accurate navigation information. In the information fusion process, if the GNSS signal is judged by the threshold to be a short-time abnormality, the GNSS measurement data are discarded.


### 3.1. Measurement Parameters

The GNSS and INS loosely integrated navigation system selects the position and velocity of the vehicle as measurement parameters. So, the measurement errors include the position difference and the velocity difference between the GNSS measurement value and the INS measurement value.The position error equation at timek is
(1)Zpk=HpXpk+Vpk,where
(2)Hp=RcosL000R0001,Xpk=δλδLδh,Vpk=nxnynh.R is the radius of the earth; δλ, δL, and δh are the position error noises of latitude, longitude, and altitude measured by INS, respectively; nx, ny, and nh are the position error noises of longitude, latitude, and altitude measured by GNSS, respectively.The velocity error equation at timek is:
(3)Zvk=HvXvk+Vvkwhere
(4)Hv=100010001,Xvk=δVxIδVyIδVzI,Vvk=nxvnyvnzv.δVxI, δVyI, and δVzI are the error noises of eastward, northward, and upward velocity measured by INS, respectively; nxv, nyv, and nzv are the error noises of eastward, northward, and upward velocity measured by GNSS, respectively.Then, the totally measurement error equation is obtained by combining the position error equation and the velocity error equation:(5)Zk=ZvkZpk=Hp03×303×3HvXpkXvk+VvkVpk


### 3.2. KF Equation

The KF equation for the field road vehicle’s GNSS/INS integrated navigation system is as follows.(1)
One-step prediction equation of the state:
(6)Ẋkk−1=FX,where F is the state transition matrix and X is the state variety
(2)
Equation for estimating the state:
(7)Ẋ^=Ẋkk−1+KkZk−HkẊkk−1,where Kk is the filter gain matrix and Hk is the measurement matrix
(3)
Filter gain equation:
(8)Kk=Pkk−1HkTHkPkk−1HkT+Rk−1,where Pkk−1 is the covariance error prediction equation and Rk is the variance matrix for measuring noise, which is determined by the parameters of the selected GNSS and INS sensors:
(9)Rk=diagδΔx2,δΔy2,δΔz2,δΔvx2,δΔvy2,δΔvz2where δΔx, δΔy, δΔz, δΔvx, δΔvy, and δΔvz represent the proportionality constants of the displacement error and velocity error in the x, y, and z directions, respectively, in the vehicle coordinate system.
(4)
Equation for predicting covariance error:
(10)Pk∣k−1=FPk−1FT+Γk−1Qk−1Γk−1T,In the equation,Qk−1 is the variance matrix of the system noise matrix Γk−1:
(11)Qk−1=diagδax2,δay2,δaz2,δωx2,δωy2,δωz2where δax, δay, δaz, δωx, δωy, and δωz represent the proportionality constants of the acceleration error and angular velocity error in the x, y, and z directions, respectively, in the vehicle coordinate system.In Equation (10), the system noise matrix Γk−1 can be obtained from the equation of state:
(12)Γk−1=G=03×303×3diag1,1,103×303×3diag1,1,19×6
(5)
Equation for estimating the covariance error:
(13)Pk=I−KkHkPkk−1The five equations above are the basic equations of the KF of the GNSS/INS integrated navigation system. The cyclical calculation uses the estimation value of the previous state to calculate that of the current state and then corrects the state estimation using the current measurement value. Through continuous iterative updates, the best estimate of the GNSS/INS integrated navigation system is ultimately obtained.


### 3.3. Adaptive Design of the KF

The memory of the KF increases to infinity [31, 32]; all the observed data before the current moment are used to estimate the current state. With the recursion of the KF, the amount of previous data in the filter gradually increases and occupies a large proportion in all the data, while the proportion of the new data is too small, resulting in the accumulation of errors and causing the filter to seemingly diverge [33]. An effective method for overcoming the filter divergence problem is to add an adaptive factor to the filter to reduce the proportion of previous data in the filtering calculation and increase the proportion of new data.The adaptive factorλk reduces the filtering divergence by adjusting the covariance matrix in real time and increasing the emphasis on innovation, as shown in Equation (14):
(14)Pk,k−1=λkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1T,where λk is the adaptive factor, Pk,k−1 is the covariance error prediction matrix, Fk,k−1 is the state transition matrix, Pk−1,k−1 is the covariance error estimation matrix obtained in the previous cycle, Γk,k−1 is the system noise matrix, and Qk−1 is the system noise variance matrix.DefineYk as the innovation sequence of measurement matrix Zk at time tk:
(15)Yk=Zk−HkXk,k−1.Then, the covariance matrixCk for the innovation can be derived as
(16)Ck=EYkYkT=HkPk,k−1HkT+Rk.In the linear KF,Yk is the white noise sequence. We can derive the linear KF’s innovation autocorrelation function [33], as shown in Equation (17).
(17)EYk+jYkT=Hk+jFk+j−1,k+j−2⋅I−Kk+j−1Hk+j−1Fk+1,k⋅I−Bk+1Hk+1⋅Fk,k−1⋅Pk,k−1HKT−KkCk.Denote(18)Sk=Pk,k−1HKT−KkCk.In actual calculations, due to the errors in the system model, the theoretical and actual values of the innovation covariance matrix are not exactly the same; thus, the value of the autocorrelation function is not necessarily zero. Therefore, we letSk=0 by adjusting the gain matrix Kk. Then, it is possible to make the new autocorrelation function (Equation (17)) equal zero, forcing the sequence of innovations to be orthogonal.In the innovation sequence (Equation (15)), an estimation error is generated by the actual calculation. The reason for filtering divergence is that the estimation error is inconsistent with the theoretical error. Therefore, we can evaluate whether the KF exhibits divergence by comparing the square sum of the innovation and the covariance.Equation (16) is the theoretical covariance of the innovation. For an innovation with an interval length of N, the estimated value C^k of the covariance matrix Ck can be described as
(19)C^k=1N∑i=0N−1Yk−iYk−iT=C^k‐1+1NYkYkT−Yk−NYk−NT.The solving formula ofKk, and the estimated value C^k, which can substituted for the theoretical value Ck, can be taken into Equation (18) and set Sk=0, then
(20)Pk,k−1HKTI−HkPk,k−1HkT+Rk−1C^k=0.SincePk,k−1, C^k, and Hk are all full-rank symmetric matrices, the following relationship can be obtained from the above formula:
(21)HkPk,k−1HkT+Rk=C^k.Substituting Equation (14) into Equation (21), we obtain:
(22)λkHkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1THkT=C^k−Rk.In Equation (22), λk is a weight value for the whole covariance error. It is difficult to optimize each measurement parameter for a single weight value. So the elements of the main diagonal in the matrix HkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1THkT are each divided by the corresponding elements on the main diagonal in the matrix (C^k−Rk), and the obtained results are used as the weight values, as shown in Equation (23).
(23)λi=C^k−RkiiHkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1THkTiii=1,2,3,⋯,6.This method for finding weight values is equivalent to dividing the innovation covariances of the longitude error, latitude error, height error, eastward velocity error, northward velocity error, and upward velocity error by the error covariance to obtain an adaptive factor.


### 3.4. Measurement Equation Adjust

In Equation (7), Zk is the measurement error. When the vehicle is driving along a regular field road, Zk is obtained from both the GNSS and INS measurements. However, the field road conditions in hilly areas are complex and varying, and the GNSS usually suffers from large errors due to multipath, poor geometry, and high noise. Occasionally, obstacles such as trees will overshadow both sides of the field road and interrupt the GNSS signal, causing large GNSS positioning errors. Because INS requires no external reference (after initialization) and has no information exchange with outer devices, it is immune to jamming and deception. Furthermore, INS can be used as a short-term fallback while GNSS signals are unavailable. Therefore, if the GNSS positioning value changes abruptly and the deviation exceeds a certain range, only the INS measurement value is used to calculate Zk. But for long-term GNSS outages, the autonomous navigation will turn to machine vision navigation (this issue will be discussed in a follow-up paper).DefineC¯k is the average value of n innovation sequence covariance matrices:
(24)C¯k=1N∑i=0N−1Yk−iYk−iT.Equation (24) reflects the average error of measurement and estimation over a period of time. Define deterioration factor of measurement value is
(25)Ok=trYkYkT−C¯k−1.The error of estimation is gradually increasing with time. Under normal circumstances, the error of a single measurement value should be consistent with the average valueC¯k. If the measurement of GNSS is disturbed, Yk and Ok will abnormal increase. So, we can define an error threshold Olimitof the estimation, if
(26)Ok>Olimit,the error of the measurement is abnormal. Under this condition, the measurement value of GNSS in measurement error equation Zk will be abandoned and only the measurement value of INS is available, so the measurement error Equation (5) changed to Equation (27):
(27)ZIk=ZvIkZpIk=Hp03×303×3HvXpkXvk.Figure2 shows the iterative calculation process of the modified KF.Figure 2
Calculation process of the modified KF.


## 3.1. Measurement Parameters

The GNSS and INS loosely integrated navigation system selects the position and velocity of the vehicle as measurement parameters. So, the measurement errors include the position difference and the velocity difference between the GNSS measurement value and the INS measurement value.The position error equation at timek is
(1)Zpk=HpXpk+Vpk,where
(2)Hp=RcosL000R0001,Xpk=δλδLδh,Vpk=nxnynh.R is the radius of the earth; δλ, δL, and δh are the position error noises of latitude, longitude, and altitude measured by INS, respectively; nx, ny, and nh are the position error noises of longitude, latitude, and altitude measured by GNSS, respectively.The velocity error equation at timek is:
(3)Zvk=HvXvk+Vvkwhere
(4)Hv=100010001,Xvk=δVxIδVyIδVzI,Vvk=nxvnyvnzv.δVxI, δVyI, and δVzI are the error noises of eastward, northward, and upward velocity measured by INS, respectively; nxv, nyv, and nzv are the error noises of eastward, northward, and upward velocity measured by GNSS, respectively.Then, the totally measurement error equation is obtained by combining the position error equation and the velocity error equation:(5)Zk=ZvkZpk=Hp03×303×3HvXpkXvk+VvkVpk


## 3.2. KF Equation

The KF equation for the field road vehicle’s GNSS/INS integrated navigation system is as follows.(1)
One-step prediction equation of the state:
(6)Ẋkk−1=FX,where F is the state transition matrix and X is the state variety
(2)
Equation for estimating the state:
(7)Ẋ^=Ẋkk−1+KkZk−HkẊkk−1,where Kk is the filter gain matrix and Hk is the measurement matrix
(3)
Filter gain equation:
(8)Kk=Pkk−1HkTHkPkk−1HkT+Rk−1,where Pkk−1 is the covariance error prediction equation and Rk is the variance matrix for measuring noise, which is determined by the parameters of the selected GNSS and INS sensors:
(9)Rk=diagδΔx2,δΔy2,δΔz2,δΔvx2,δΔvy2,δΔvz2where δΔx, δΔy, δΔz, δΔvx, δΔvy, and δΔvz represent the proportionality constants of the displacement error and velocity error in the x, y, and z directions, respectively, in the vehicle coordinate system.
(4)
Equation for predicting covariance error:
(10)Pk∣k−1=FPk−1FT+Γk−1Qk−1Γk−1T,In the equation,Qk−1 is the variance matrix of the system noise matrix Γk−1:
(11)Qk−1=diagδax2,δay2,δaz2,δωx2,δωy2,δωz2where δax, δay, δaz, δωx, δωy, and δωz represent the proportionality constants of the acceleration error and angular velocity error in the x, y, and z directions, respectively, in the vehicle coordinate system.In Equation (10), the system noise matrix Γk−1 can be obtained from the equation of state:
(12)Γk−1=G=03×303×3diag1,1,103×303×3diag1,1,19×6
(5)
Equation for estimating the covariance error:
(13)Pk=I−KkHkPkk−1The five equations above are the basic equations of the KF of the GNSS/INS integrated navigation system. The cyclical calculation uses the estimation value of the previous state to calculate that of the current state and then corrects the state estimation using the current measurement value. Through continuous iterative updates, the best estimate of the GNSS/INS integrated navigation system is ultimately obtained.


## 3.3. Adaptive Design of the KF

The memory of the KF increases to infinity [31, 32]; all the observed data before the current moment are used to estimate the current state. With the recursion of the KF, the amount of previous data in the filter gradually increases and occupies a large proportion in all the data, while the proportion of the new data is too small, resulting in the accumulation of errors and causing the filter to seemingly diverge [33]. An effective method for overcoming the filter divergence problem is to add an adaptive factor to the filter to reduce the proportion of previous data in the filtering calculation and increase the proportion of new data.The adaptive factorλk reduces the filtering divergence by adjusting the covariance matrix in real time and increasing the emphasis on innovation, as shown in Equation (14):
(14)Pk,k−1=λkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1T,where λk is the adaptive factor, Pk,k−1 is the covariance error prediction matrix, Fk,k−1 is the state transition matrix, Pk−1,k−1 is the covariance error estimation matrix obtained in the previous cycle, Γk,k−1 is the system noise matrix, and Qk−1 is the system noise variance matrix.DefineYk as the innovation sequence of measurement matrix Zk at time tk:
(15)Yk=Zk−HkXk,k−1.Then, the covariance matrixCk for the innovation can be derived as
(16)Ck=EYkYkT=HkPk,k−1HkT+Rk.In the linear KF,Yk is the white noise sequence. We can derive the linear KF’s innovation autocorrelation function [33], as shown in Equation (17).
(17)EYk+jYkT=Hk+jFk+j−1,k+j−2⋅I−Kk+j−1Hk+j−1Fk+1,k⋅I−Bk+1Hk+1⋅Fk,k−1⋅Pk,k−1HKT−KkCk.Denote(18)Sk=Pk,k−1HKT−KkCk.In actual calculations, due to the errors in the system model, the theoretical and actual values of the innovation covariance matrix are not exactly the same; thus, the value of the autocorrelation function is not necessarily zero. Therefore, we letSk=0 by adjusting the gain matrix Kk. Then, it is possible to make the new autocorrelation function (Equation (17)) equal zero, forcing the sequence of innovations to be orthogonal.In the innovation sequence (Equation (15)), an estimation error is generated by the actual calculation. The reason for filtering divergence is that the estimation error is inconsistent with the theoretical error. Therefore, we can evaluate whether the KF exhibits divergence by comparing the square sum of the innovation and the covariance.Equation (16) is the theoretical covariance of the innovation. For an innovation with an interval length of N, the estimated value C^k of the covariance matrix Ck can be described as
(19)C^k=1N∑i=0N−1Yk−iYk−iT=C^k‐1+1NYkYkT−Yk−NYk−NT.The solving formula ofKk, and the estimated value C^k, which can substituted for the theoretical value Ck, can be taken into Equation (18) and set Sk=0, then
(20)Pk,k−1HKTI−HkPk,k−1HkT+Rk−1C^k=0.SincePk,k−1, C^k, and Hk are all full-rank symmetric matrices, the following relationship can be obtained from the above formula:
(21)HkPk,k−1HkT+Rk=C^k.Substituting Equation (14) into Equation (21), we obtain:
(22)λkHkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1THkT=C^k−Rk.In Equation (22), λk is a weight value for the whole covariance error. It is difficult to optimize each measurement parameter for a single weight value. So the elements of the main diagonal in the matrix HkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1THkT are each divided by the corresponding elements on the main diagonal in the matrix (C^k−Rk), and the obtained results are used as the weight values, as shown in Equation (23).
(23)λi=C^k−RkiiHkFk,k−1Pk−1,k−1Fk,k−1T+Γk,k−1Qk−1Γk,k−1THkTiii=1,2,3,⋯,6.This method for finding weight values is equivalent to dividing the innovation covariances of the longitude error, latitude error, height error, eastward velocity error, northward velocity error, and upward velocity error by the error covariance to obtain an adaptive factor.


## 3.4. Measurement Equation Adjust

In Equation (7), Zk is the measurement error. When the vehicle is driving along a regular field road, Zk is obtained from both the GNSS and INS measurements. However, the field road conditions in hilly areas are complex and varying, and the GNSS usually suffers from large errors due to multipath, poor geometry, and high noise. Occasionally, obstacles such as trees will overshadow both sides of the field road and interrupt the GNSS signal, causing large GNSS positioning errors. Because INS requires no external reference (after initialization) and has no information exchange with outer devices, it is immune to jamming and deception. Furthermore, INS can be used as a short-term fallback while GNSS signals are unavailable. Therefore, if the GNSS positioning value changes abruptly and the deviation exceeds a certain range, only the INS measurement value is used to calculate Zk. But for long-term GNSS outages, the autonomous navigation will turn to machine vision navigation (this issue will be discussed in a follow-up paper).DefineC¯k is the average value of n innovation sequence covariance matrices:
(24)C¯k=1N∑i=0N−1Yk−iYk−iT.Equation (24) reflects the average error of measurement and estimation over a period of time. Define deterioration factor of measurement value is
(25)Ok=trYkYkT−C¯k−1.The error of estimation is gradually increasing with time. Under normal circumstances, the error of a single measurement value should be consistent with the average valueC¯k. If the measurement of GNSS is disturbed, Yk and Ok will abnormal increase. So, we can define an error threshold Olimitof the estimation, if
(26)Ok>Olimit,the error of the measurement is abnormal. Under this condition, the measurement value of GNSS in measurement error equation Zk will be abandoned and only the measurement value of INS is available, so the measurement error Equation (5) changed to Equation (27):
(27)ZIk=ZvIkZpIk=Hp03×303×3HvXpkXvk.Figure2 shows the iterative calculation process of the modified KF.Figure 2
Calculation process of the modified KF.


## 4. Fuzzy Neural Network Tracking Control

After the real-time position and velocity of the vehicle are obtained via the modified KF, corresponding control strategies are still required to control vehicle autonomous tracking of the navigation trajectory accurately. Fuzzy control and neural network control can be combined to utilize the complementary advantages of both. Based on this, a fuzzy neural network path tracking control strategy is designed for the vehicle. The function of the fuzzy neural network is to decide the automatic steering angle according to the deviation between the real-time position and velocity of the vehicle and the navigation line.


### 4.1. Structure of the Fuzzy Neural Network Controller

Controlling the automated operation of a vehicle involves a highly nonlinear complex control system composed of multiple variables [34], with lateral control (also known as steering control) being crucial [35, 36]. Lateral control is directly related to the lateral deviation, heading deviation, and path curvature. Thus, a five-layer fuzzy neural network structure with three input parameters and a single output is implemented, as shown in Figure 3. The input parameters include the lateral deviation, heading deviation, and path curvature. The output parameter is the steering angle of the field road vehicle. The lateral and heading deviation are calculated by the real-time position and velocity which are obtained via the modified KF, while the path curvature is calculated by the navigation line.Figure 3
Structure of the fuzzy neural network controller.


### 4.2. Learning Algorithm

The input and output functions of each node of the field road vehicle fuzzy neural network are different from those of ordinary neural network nodes and have special forms. Definefl and ol are the node function and output value of l layer, respectively. The output function of each layer is the identity function. The following illustrates the derivation of the node function and output value for each layer.The first layer is the input layer, and the node function is(28)fi1=xi0=xioi1=fi1=xiwhere i=1,2,3.In the second layer, each node represents a fuzzy linguistic variable value. This layer calculates the membership function of each input componentxi belonging to each linguistic variable μij. In this paper, the fuzzy set of each input component includes 7 fuzzy variables and Gaussian function is used as the membership function. The node function of this layer is
(29)fij2=−xi1−cij2σij2oij2=μij=efij2=e−xi−cij2/σij2where i=1,2,3; j=1,2,3,⋯,7; cij and σij are the central value and width of membership function, respectively.In the third layer, each node represents a fuzzy rule. The role of the third layer is to match the premises of the fuzzy rules and calculate the usage degree of each fuzzy rule. Multiplication algorithm is adopted as the node function, that is(30)fk3=x1i12x2i22x3in2=μ1i1μ2i2μ3i3,ok3=fk3=μ1i1μ2i2μ3i3,where i1∈1,2,3,⋯,7, i2∈1,2,3,⋯,7, and i3∈1,2,3,⋯,7.In the fourth layer, the single neuron and the corresponding rules are normalized. The node function is(31)fk4=ok3∑k=1mok3,ok4=fk4.In the fifth layer, the defuzzification calculation is carried out, and the desired steering angle of the vehicle is output. The node function is(32)f5=∑k=1mωkok4,o5=y=f5.In Equations (23), (24), and (25), m=343, and k=1,2,3,⋯,343.


### 4.3. Training and Testing

The parameters of the fuzzy neural network tracking controller that need to be determined via self-learning include the weight of the last layer input variableωkk=1,2,3,⋯,343 and the central value cij and width σiji=1,2,3;j=1,2,3,⋯,7 of the membership function in the second layer. The vehicle’s fuzzy neural network controller structurally belongs to the multilayer feedforward network. According to the back-propagation (BP) network structure [37], the error back-propagation learning algorithm is selected to learn the related parameters.When training the fuzzy neural network, test data are used to verify the result for each training step, and the trend of the mean squared error (MSE) of the test data as the training time increases is obtained. In this way, we can find the most appropriate training times to ensure that the fuzzy neural network has satisfactory generalizability after learning. The change in the test data MSE with the training time in this study is shown in Figure4.Figure 4
The mean squared error of test data with respect to the training time.As the training time increases, the MSE decreases first rapidly and then slowly. When the numbers of training reaches 331, the MSE reaches a minimum of 0.01; afterwards, it begins to increase, which is due to the influence of noise and model complexity, and a tendency of overfitting occurs. Early stopping method of iterations is applied to determine the training times. Thus, the result obtained from the upper position in 331 training is the best. We take the result of this training as the final result and then determineωj, σij, and cij.In order to verify the control effect of the fuzzy neural network path tracking, the input parameters of the test data are input into the trained fuzzy neural network controller, and the steering angle of the vehicle obtained by the solution is compared with the actual measured steering angle of the vehicle. As shown in Figure5, it can be seen that the output steering angle by the fuzzy neural network can well predict the change of the actual steering angle during the testing, with an average deviation of 1.12°. It shows that the designed fuzzy neural network controller has good control performance for the automatic driving of the vehicle.Figure 5
Steering angle comparison between the actual vehicle and output by the fuzzy neural network.


## 4.1. Structure of the Fuzzy Neural Network Controller

Controlling the automated operation of a vehicle involves a highly nonlinear complex control system composed of multiple variables [34], with lateral control (also known as steering control) being crucial [35, 36]. Lateral control is directly related to the lateral deviation, heading deviation, and path curvature. Thus, a five-layer fuzzy neural network structure with three input parameters and a single output is implemented, as shown in Figure 3. The input parameters include the lateral deviation, heading deviation, and path curvature. The output parameter is the steering angle of the field road vehicle. The lateral and heading deviation are calculated by the real-time position and velocity which are obtained via the modified KF, while the path curvature is calculated by the navigation line.Figure 3
Structure of the fuzzy neural network controller.


## 4.2. Learning Algorithm

The input and output functions of each node of the field road vehicle fuzzy neural network are different from those of ordinary neural network nodes and have special forms. Definefl and ol are the node function and output value of l layer, respectively. The output function of each layer is the identity function. The following illustrates the derivation of the node function and output value for each layer.The first layer is the input layer, and the node function is(28)fi1=xi0=xioi1=fi1=xiwhere i=1,2,3.In the second layer, each node represents a fuzzy linguistic variable value. This layer calculates the membership function of each input componentxi belonging to each linguistic variable μij. In this paper, the fuzzy set of each input component includes 7 fuzzy variables and Gaussian function is used as the membership function. The node function of this layer is
(29)fij2=−xi1−cij2σij2oij2=μij=efij2=e−xi−cij2/σij2where i=1,2,3; j=1,2,3,⋯,7; cij and σij are the central value and width of membership function, respectively.In the third layer, each node represents a fuzzy rule. The role of the third layer is to match the premises of the fuzzy rules and calculate the usage degree of each fuzzy rule. Multiplication algorithm is adopted as the node function, that is(30)fk3=x1i12x2i22x3in2=μ1i1μ2i2μ3i3,ok3=fk3=μ1i1μ2i2μ3i3,where i1∈1,2,3,⋯,7, i2∈1,2,3,⋯,7, and i3∈1,2,3,⋯,7.In the fourth layer, the single neuron and the corresponding rules are normalized. The node function is(31)fk4=ok3∑k=1mok3,ok4=fk4.In the fifth layer, the defuzzification calculation is carried out, and the desired steering angle of the vehicle is output. The node function is(32)f5=∑k=1mωkok4,o5=y=f5.In Equations (23), (24), and (25), m=343, and k=1,2,3,⋯,343.


## 4.3. Training and Testing

The parameters of the fuzzy neural network tracking controller that need to be determined via self-learning include the weight of the last layer input variableωkk=1,2,3,⋯,343 and the central value cij and width σiji=1,2,3;j=1,2,3,⋯,7 of the membership function in the second layer. The vehicle’s fuzzy neural network controller structurally belongs to the multilayer feedforward network. According to the back-propagation (BP) network structure [37], the error back-propagation learning algorithm is selected to learn the related parameters.When training the fuzzy neural network, test data are used to verify the result for each training step, and the trend of the mean squared error (MSE) of the test data as the training time increases is obtained. In this way, we can find the most appropriate training times to ensure that the fuzzy neural network has satisfactory generalizability after learning. The change in the test data MSE with the training time in this study is shown in Figure4.Figure 4
The mean squared error of test data with respect to the training time.As the training time increases, the MSE decreases first rapidly and then slowly. When the numbers of training reaches 331, the MSE reaches a minimum of 0.01; afterwards, it begins to increase, which is due to the influence of noise and model complexity, and a tendency of overfitting occurs. Early stopping method of iterations is applied to determine the training times. Thus, the result obtained from the upper position in 331 training is the best. We take the result of this training as the final result and then determineωj, σij, and cij.In order to verify the control effect of the fuzzy neural network path tracking, the input parameters of the test data are input into the trained fuzzy neural network controller, and the steering angle of the vehicle obtained by the solution is compared with the actual measured steering angle of the vehicle. As shown in Figure5, it can be seen that the output steering angle by the fuzzy neural network can well predict the change of the actual steering angle during the testing, with an average deviation of 1.12°. It shows that the designed fuzzy neural network controller has good control performance for the automatic driving of the vehicle.Figure 5
Steering angle comparison between the actual vehicle and output by the fuzzy neural network.


## 5. Results and Discussion

### 5.1. Simulation of Information Fusion

To verify the correctness of the fusion processing and the improving effect of the positioning accuracy of the modified KF algorithm, the information fusions were simulated by MATLAB R2015. The simulation is based on the following steps:(a)
Set the initial state of the field road vehicle. According to the geographical location of Chongqing, the vehicle was located at 30° N., 106° E., and 0 height. The initial speed of the vehicle was set to 2 m/s with the positive south direction
(b)
A series of accelerations of the vehicle are randomly generated. The acceleration values of thex-axis (i.e., the lateral direction), the y-axis (i.e., the longitudinal direction), and the z-axis (i.e., the plumb direction) were limited within ±0.2 m/s2, ±0.5 m/s2, and ± 0.1 m/s2, respectively. The maximum speed of the vehicle was limited to 10 m/s
(c)
Calculate the attitude angle of the vehicle according to the kinematic model of the vehicle and the random acceleration generated in step (b)
(d)
Calculate the simulated trajectory of the vehicle based on the time integration of the random acceleration and the attitude angle
(e)
Simulate the GNSS output data. According to the performance of the selected GNSS, a series of noise values were added to the trajectory as the output values of the GNSS. The deviation between the simulated GNSS output data and the simulated trajectory of the vehicle is shown in Figure6
(f)
Simulate the INS output data. According to the performance of the selected INS module, a series of noise values were added to the simulated acceleration and the attitude angle as the output values of the INS. The simulated INS output data are shown in Figure7
(g)
Calculate the driving trajectory of the vehicle utilizing the proposed KF fusion of the simulated vehicle trajectory, simulated GNSS output data, and INS output data. The driving trajectory was taken as the result of the KF information fusionFigure 6
The deviation between the simulated GNSS output and the simulated trajectory.Figure 7
The simulated INS output data.Figure6 shows that the deviation between the simulated GNSS output value and the simulated trajectory fluctuates around zero with a deviation of 5 cm, which is consistent with the performance of the GNSS used in the test.Figure7 shows the simulated INS output data, which has slight fluctuations compared to the simulated driving state of the vehicle. The simulated INS output is obtained through adding noise to the simulated vehicle trajectory, which is consistent with the performance of the tested INS module.From Figure7, the acceleration and rotation of the simulated trajectory, which are irregular and mutable, can reflect the objective situation of the vehicle driving on the field roads in hilly areas. The simulated GNSS and INS output values, which include simulated measurement noises, are also consistent with the performance of the selected equipment. So, the simulated trajectory and GNSS and INS output values can be used to verify the modified KF information fusion algorithm.Comparing the calculated driving trajectory through the modified KF fusion algorithm and the simulated trajectory, the deviation between them is obtained, as shown in Figure8.Figure 8
The deviation between the KF fused trajectory and the simulated trajectory.From Figure8, it can be seen that the eastward deviation, northward deviation, and upward deviation between the KF fused driving trajectory and the simulated trajectory fluctuate around 0, and the maximum deviation value is within 3 cm. Compared with the simulated GNSS output in Figure 6, the deviation trend of the KF fusion output is consistent with that of the simulated GNSS output, but the maximum deviation of the KF fusion output is reduce by 40%. This shows that the proposed GNSS/INS integrated navigation and the adjusted KF fusion algorithm can effectively reduce the deviation of a single GNSS measurement and improve the overall accuracy. It provides a more reliable parameter basis for the automatic driving control.In order to verify the information fusion functionality when the GNSS signal is lost for a short time, an artificial noise error is added to the original GNSS data from 781 s to 800 s as shown in Figure9 to simulate an abnormal GNSS signal. The improved vehicle Kalman filter designed by this paper is compared with the simulated vehicle trajectory to verify the effectiveness of the proposed algorithm in the case of GNSS signal loss during a short time. Figure 9 shows the eastward, northward, and upward deviation between adaptive Kalman output and simulated vehicle’s track from 770 s to 810 s, respectively.Figure 9
The deviation between adaptive Kalman output and simulated vehicle’s track.It can be seen from Figure9 that the deviation between the output value of the Kalman filter information fusion and the simulated vehicle travel trajectory increases at 780 s (point A). The deviation reaches the maximum at 800 s (point B), with an eastward deviation of 14 cm, a northward deviation of 16 cm, and a upward deviation of 23 cm. After crossing the point B, the difference between the output value of the Kalman filter information fusion and the simulated vehicle travel trajectory is rapidly reduced, and the output value of the Kalman filter information fusion returns to normal at 805 s.It can be seen from the simulation results that when the GNSS signal is lost, the Kalman filter information fusion relies on the output data of the INS to calculate the driving state of the vehicle. Although the deviation of the solution will accumulate over time, it can still be used in a short period of time. When the GNSS signal returns to normal, the Kalman filter information fusion algorithm uses the output data of GNSS and INS at the same time, which can quickly correct the previous deviation.


### 5.2. Experiment on Normal Field Roads

To verify the effectiveness of the GNSS/INS integrated navigation system and the fuzzy neural network tracking control strategy for the vehicle, automated driving tests were performed on normal field roads and roads where the GNSS suffers from occasional outages in terms of the position. The length of the field road vehicle is 1.13 m, its wheelbase is 0.76 m, its tread is 0.45 m, and its maximum load capacity is 150 kg. The experimental field road is 1.2 m wide, with significant changes in altitude and curvature. The speed of the vehicle is 2 m/s. The high-accuracy RTK-GPS (model K706, manufactured by ComNav Technology Ltd.), which includes a fixed base station and a rover on the vehicle to reduce the position error, was used to collect the middle line coordinate information of the road and store it as the autonomous navigation path. The positioning accuracy of the RTK-GPS is 2 cm. The INS module (model LPMS-NAV2, manufactured by LP-RESEARCH Inc.), which is composed of a high-accuracy one-axis gyroscope and a 3-axis accelerometer, is low cost with a resolution of 0.122 mg/LSB and an angle resolution of 0.01°.During the test, the vehicle is placed in the starting position facing the forward direction of the road. The driving mode of the vehicle is set to autonomous travel. The automated vehicle driving scene is shown in Figure10.Figure 10
Automated vehicle driving experiment.In the process of autonomous traveling, the main controller records the real-time longitude, latitude, and altitude data of the GNSS/INS integrated navigation system and compares this information with the expected path information to obtain the actual path deviation. The eastward, northward, and upward deviations of the autonomous traveling trajectory are shown in Figure11.Figure 11
Deviation of the vehicle’s automated driving trajectory.The deviations of the autonomous traveling trajectory all fluctuate around 0 because of the unevenness of the accelerometer measurement due to the roughness of the road surfaces coupled with GNSS positioning errors, mechanical errors of the steering system, and other errors. In the eastward direction, the maximum deviation is 8.9 cm and the average deviation is 4.6 cm. In the northward direction, the maximum deviation is 11.9 cm and the average deviation is 4.9 cm. In the upward direction, the maximum deviation is 17.3 cm and the average deviation is 6.8 cm.The deviation in the horizontal direction, which reflects the accuracy of the path tracking control, could be directly calculated by vector operation of the eastward deviation and the northward deviation. The maximum deviation in the horizontal direction is 12.2 cm and the average deviation is 5.3 cm. The test results show that the designed GNSS/INS integrated navigation system can accurately control the automated driving of a vehicle on a field road in hilly areas.


### 5.3. Experiment during GNSS Outages

In hilly areas, obstacles such as trees and crops along the field road may can block the signals of the GNSS. As a result, the GNSS positioning information may be strongly distorted or unavailable. To verify the effectiveness of the designed GNSS/INS integrated navigation system during GNSS outages, the following experiment is designed.In the experiment, a small segment of the field road along which the GNSS system signal is blocked by a tall plant is selected (see Figure12).Figure 12
Automated vehicle operation during GNSS outages.When the vehicle travels along the road section shown in Figure12, the GNSS suffers from outages in position due to communication link failures and loss of satellite lock due to occlusion by the fruit trees along both sides of the field road, resulting in a large positioning error. The measured position coordinate of the vehicle during automated driving is shown in Figure 13. Starting from point A, the GNSS signal begins to be distorted, and the positioning information strongly deviates. After the vehicle travels to point B, the GNSS signal returns to normal.Figure 13
The measured position coordinate of the vehicle.The eastward deviation, northward deviation, and upward deviation of the automated operation of the vehicle during GNSS outages are shown in Figure14.Figure 14
Deviation of the vehicle’s automated driving trajectory.Figure14 shows that the deviations of the vehicle still slightly fluctuate around 0 during automated operation. In the eastward direction, the maximum deviation is 12.3 cm and the average deviation is 5.6 cm. In the northward direction, the maximum deviation is 7.2 cm and the average deviation is 3.8 cm. In the upward direction, the maximum deviation is 14.1 cm and the average deviation is 5.2 cm.The maximum deviation in the horizontal direction calculated from the eastward deviation and the northward deviation is 12.7 cm and the average deviation is 6.1 cm, which shows that the vehicle can still proceed normally along the planned road when the GNSS is disturbed for a short time.An analysis of the AB segment while the GNSS is perturbed shows that the eastward deviation continues to increase until reaching a maximum deviation of 12.3 cm. The northward deviation declines at a slow rate, and the skyward deviation is relatively fluctuant along this segment. This result principally occurs because the GNSS/INS integrated navigation system uses only the navigation information measured by the INS during GNSS outages. The INS error accumulates over time, leading to an increase in bias and a slow correcting rate in the eastward and northward directions. Since the INS device is composed of a single-axis gyroscope and a three-axis accelerometer, the height information is converted using the accelerometer measurements. Due to the roughness of the field road surfaces and the shaking of the vehicle, the accelerometer’s measurement noise is large, resulting in large errors in the converted height information and the phenomenon of fluctuation of the skyward deviation.In summary, the GNSS system is affected by vegetation on both sides of the road during automated operation of the vehicle, resulting in a lack of signal availability and a large positioning error. At that time, the GNSS/INS integrated navigation system uses the measurements of the INS to calculate the navigation information and imports the result into the fuzzy neural network controller. As a result, the vehicle can still automatically travel along the predetermined path of the field road.


## 5.1. Simulation of Information Fusion

To verify the correctness of the fusion processing and the improving effect of the positioning accuracy of the modified KF algorithm, the information fusions were simulated by MATLAB R2015. The simulation is based on the following steps:(a)
Set the initial state of the field road vehicle. According to the geographical location of Chongqing, the vehicle was located at 30° N., 106° E., and 0 height. The initial speed of the vehicle was set to 2 m/s with the positive south direction
(b)
A series of accelerations of the vehicle are randomly generated. The acceleration values of thex-axis (i.e., the lateral direction), the y-axis (i.e., the longitudinal direction), and the z-axis (i.e., the plumb direction) were limited within ±0.2 m/s2, ±0.5 m/s2, and ± 0.1 m/s2, respectively. The maximum speed of the vehicle was limited to 10 m/s
(c)
Calculate the attitude angle of the vehicle according to the kinematic model of the vehicle and the random acceleration generated in step (b)
(d)
Calculate the simulated trajectory of the vehicle based on the time integration of the random acceleration and the attitude angle
(e)
Simulate the GNSS output data. According to the performance of the selected GNSS, a series of noise values were added to the trajectory as the output values of the GNSS. The deviation between the simulated GNSS output data and the simulated trajectory of the vehicle is shown in Figure6
(f)
Simulate the INS output data. According to the performance of the selected INS module, a series of noise values were added to the simulated acceleration and the attitude angle as the output values of the INS. The simulated INS output data are shown in Figure7
(g)
Calculate the driving trajectory of the vehicle utilizing the proposed KF fusion of the simulated vehicle trajectory, simulated GNSS output data, and INS output data. The driving trajectory was taken as the result of the KF information fusionFigure 6
The deviation between the simulated GNSS output and the simulated trajectory.Figure 7
The simulated INS output data.Figure6 shows that the deviation between the simulated GNSS output value and the simulated trajectory fluctuates around zero with a deviation of 5 cm, which is consistent with the performance of the GNSS used in the test.Figure7 shows the simulated INS output data, which has slight fluctuations compared to the simulated driving state of the vehicle. The simulated INS output is obtained through adding noise to the simulated vehicle trajectory, which is consistent with the performance of the tested INS module.From Figure7, the acceleration and rotation of the simulated trajectory, which are irregular and mutable, can reflect the objective situation of the vehicle driving on the field roads in hilly areas. The simulated GNSS and INS output values, which include simulated measurement noises, are also consistent with the performance of the selected equipment. So, the simulated trajectory and GNSS and INS output values can be used to verify the modified KF information fusion algorithm.Comparing the calculated driving trajectory through the modified KF fusion algorithm and the simulated trajectory, the deviation between them is obtained, as shown in Figure8.Figure 8
The deviation between the KF fused trajectory and the simulated trajectory.From Figure8, it can be seen that the eastward deviation, northward deviation, and upward deviation between the KF fused driving trajectory and the simulated trajectory fluctuate around 0, and the maximum deviation value is within 3 cm. Compared with the simulated GNSS output in Figure 6, the deviation trend of the KF fusion output is consistent with that of the simulated GNSS output, but the maximum deviation of the KF fusion output is reduce by 40%. This shows that the proposed GNSS/INS integrated navigation and the adjusted KF fusion algorithm can effectively reduce the deviation of a single GNSS measurement and improve the overall accuracy. It provides a more reliable parameter basis for the automatic driving control.In order to verify the information fusion functionality when the GNSS signal is lost for a short time, an artificial noise error is added to the original GNSS data from 781 s to 800 s as shown in Figure9 to simulate an abnormal GNSS signal. The improved vehicle Kalman filter designed by this paper is compared with the simulated vehicle trajectory to verify the effectiveness of the proposed algorithm in the case of GNSS signal loss during a short time. Figure 9 shows the eastward, northward, and upward deviation between adaptive Kalman output and simulated vehicle’s track from 770 s to 810 s, respectively.Figure 9
The deviation between adaptive Kalman output and simulated vehicle’s track.It can be seen from Figure9 that the deviation between the output value of the Kalman filter information fusion and the simulated vehicle travel trajectory increases at 780 s (point A). The deviation reaches the maximum at 800 s (point B), with an eastward deviation of 14 cm, a northward deviation of 16 cm, and a upward deviation of 23 cm. After crossing the point B, the difference between the output value of the Kalman filter information fusion and the simulated vehicle travel trajectory is rapidly reduced, and the output value of the Kalman filter information fusion returns to normal at 805 s.It can be seen from the simulation results that when the GNSS signal is lost, the Kalman filter information fusion relies on the output data of the INS to calculate the driving state of the vehicle. Although the deviation of the solution will accumulate over time, it can still be used in a short period of time. When the GNSS signal returns to normal, the Kalman filter information fusion algorithm uses the output data of GNSS and INS at the same time, which can quickly correct the previous deviation.


## 5.2. Experiment on Normal Field Roads

To verify the effectiveness of the GNSS/INS integrated navigation system and the fuzzy neural network tracking control strategy for the vehicle, automated driving tests were performed on normal field roads and roads where the GNSS suffers from occasional outages in terms of the position. The length of the field road vehicle is 1.13 m, its wheelbase is 0.76 m, its tread is 0.45 m, and its maximum load capacity is 150 kg. The experimental field road is 1.2 m wide, with significant changes in altitude and curvature. The speed of the vehicle is 2 m/s. The high-accuracy RTK-GPS (model K706, manufactured by ComNav Technology Ltd.), which includes a fixed base station and a rover on the vehicle to reduce the position error, was used to collect the middle line coordinate information of the road and store it as the autonomous navigation path. The positioning accuracy of the RTK-GPS is 2 cm. The INS module (model LPMS-NAV2, manufactured by LP-RESEARCH Inc.), which is composed of a high-accuracy one-axis gyroscope and a 3-axis accelerometer, is low cost with a resolution of 0.122 mg/LSB and an angle resolution of 0.01°.During the test, the vehicle is placed in the starting position facing the forward direction of the road. The driving mode of the vehicle is set to autonomous travel. The automated vehicle driving scene is shown in Figure10.Figure 10
Automated vehicle driving experiment.In the process of autonomous traveling, the main controller records the real-time longitude, latitude, and altitude data of the GNSS/INS integrated navigation system and compares this information with the expected path information to obtain the actual path deviation. The eastward, northward, and upward deviations of the autonomous traveling trajectory are shown in Figure11.Figure 11
Deviation of the vehicle’s automated driving trajectory.The deviations of the autonomous traveling trajectory all fluctuate around 0 because of the unevenness of the accelerometer measurement due to the roughness of the road surfaces coupled with GNSS positioning errors, mechanical errors of the steering system, and other errors. In the eastward direction, the maximum deviation is 8.9 cm and the average deviation is 4.6 cm. In the northward direction, the maximum deviation is 11.9 cm and the average deviation is 4.9 cm. In the upward direction, the maximum deviation is 17.3 cm and the average deviation is 6.8 cm.The deviation in the horizontal direction, which reflects the accuracy of the path tracking control, could be directly calculated by vector operation of the eastward deviation and the northward deviation. The maximum deviation in the horizontal direction is 12.2 cm and the average deviation is 5.3 cm. The test results show that the designed GNSS/INS integrated navigation system can accurately control the automated driving of a vehicle on a field road in hilly areas.


## 5.3. Experiment during GNSS Outages

In hilly areas, obstacles such as trees and crops along the field road may can block the signals of the GNSS. As a result, the GNSS positioning information may be strongly distorted or unavailable. To verify the effectiveness of the designed GNSS/INS integrated navigation system during GNSS outages, the following experiment is designed.In the experiment, a small segment of the field road along which the GNSS system signal is blocked by a tall plant is selected (see Figure12).Figure 12
Automated vehicle operation during GNSS outages.When the vehicle travels along the road section shown in Figure12, the GNSS suffers from outages in position due to communication link failures and loss of satellite lock due to occlusion by the fruit trees along both sides of the field road, resulting in a large positioning error. The measured position coordinate of the vehicle during automated driving is shown in Figure 13. Starting from point A, the GNSS signal begins to be distorted, and the positioning information strongly deviates. After the vehicle travels to point B, the GNSS signal returns to normal.Figure 13
The measured position coordinate of the vehicle.The eastward deviation, northward deviation, and upward deviation of the automated operation of the vehicle during GNSS outages are shown in Figure14.Figure 14
Deviation of the vehicle’s automated driving trajectory.Figure14 shows that the deviations of the vehicle still slightly fluctuate around 0 during automated operation. In the eastward direction, the maximum deviation is 12.3 cm and the average deviation is 5.6 cm. In the northward direction, the maximum deviation is 7.2 cm and the average deviation is 3.8 cm. In the upward direction, the maximum deviation is 14.1 cm and the average deviation is 5.2 cm.The maximum deviation in the horizontal direction calculated from the eastward deviation and the northward deviation is 12.7 cm and the average deviation is 6.1 cm, which shows that the vehicle can still proceed normally along the planned road when the GNSS is disturbed for a short time.An analysis of the AB segment while the GNSS is perturbed shows that the eastward deviation continues to increase until reaching a maximum deviation of 12.3 cm. The northward deviation declines at a slow rate, and the skyward deviation is relatively fluctuant along this segment. This result principally occurs because the GNSS/INS integrated navigation system uses only the navigation information measured by the INS during GNSS outages. The INS error accumulates over time, leading to an increase in bias and a slow correcting rate in the eastward and northward directions. Since the INS device is composed of a single-axis gyroscope and a three-axis accelerometer, the height information is converted using the accelerometer measurements. Due to the roughness of the field road surfaces and the shaking of the vehicle, the accelerometer’s measurement noise is large, resulting in large errors in the converted height information and the phenomenon of fluctuation of the skyward deviation.In summary, the GNSS system is affected by vegetation on both sides of the road during automated operation of the vehicle, resulting in a lack of signal availability and a large positioning error. At that time, the GNSS/INS integrated navigation system uses the measurements of the INS to calculate the navigation information and imports the result into the fuzzy neural network controller. As a result, the vehicle can still automatically travel along the predetermined path of the field road.


## 6. Conclusions

In this paper, based on the characteristics of vehicles driving along field roads in hilly areas, these authors design a loosely coupled GNSS/INS integrated navigation system structure and utilize modified indirect KF for information fusion. The fuzzy neural network is used as a path tracking controller for the vehicle.Simulation results showed that, compared with the simulated GNSS output, the deviation trend of the KF fusion output is consistent with that of the simulated GNSS output, but the maximum deviation of the KF fusion output is reduce by 40%. The proposed GNSS/INS integrated navigation and the adjusted KF fusion algorithm can effectively reduce the deviation of single GNSS measurement and improve the overall accuracy.In the normal field road experiment, the maximum deviation in the horizontal direction is 12.2 cm and the average deviation is 5.3 cm. In the GNSS short-term outage experiment, the maximum deviation in the horizontal direction is 12.7 cm and the average deviation is 6.1 cm. The field road experiment verifies that the proposed GNSS/INS integrated navigation system can accurately control the automated operation of a vehicle along field roads in a hilly area.The GNSS/INS integrated navigation system used in this paper can deal with the GNSS outages in a short period of time. However, if the GNSS signals were lost for a long time, the navigation error will increase due to the accumulation of INS system errors, resulting to the deviation of vehicles from the narrow mountain roads in the hilly mountains. Therefore, complementary navigation systems such as machine vision navigation have been added to subsequent studies in this project, which can improve the ability of the navigation system to cope with the harsh navigation environment in hilly areas.



---

*Source: 6738594-2019-11-25.xml*