In this paper, we propose a fusion algorithm of multifarious and multiple sensors to enhance the accuracy and reliability of position and velocity estimation for the vehicles. We proposed an adaptive Kalman filter for multiple sensor fusion to provide a fault tolerant estimation. We verified the multiple sensor fusion estimator can provide a fault tolerant estimation through Matlab simulation and laboratory equipped experiments. We also proposed a fusion algorithm of multifarious sensors in order to enhance the velocity estimation accuracy. We proposed a Kalman filter error correction for compensate the accumulative error in the main sensor with the other type of sensor which has characteristic of biased error. We also developed a fusion algorithm for compensate the error in the position measuring with the velocity measuring. We made experiments for estimating position and velocity of vehicle simultaneously through the fusion of multifarious and multiple sensors and showed that average position error was 1.5764 m and average velocity accuracy was 99.81%.
Autonomous driving has been said to be the next big disruptive innovation in the years to come. The technology for autonomous vehicle navigation gains increasing importance in various growing application areas [1]. An intelligent autonomous vehicle can be defined as a vehicle that navigates to its destination without human operator’s manipulation and determines its own decision on path generation, environment recognition, and obstacle avoidance. The autonomous vehicle generally includes a detection system for measuring its own position and recognizing an obstacle, a central control device for giving commands such as acceleration, deceleration and steering based on the information, and devices for following the control commands.
The most important information for autonomous navigation is the reliable and accurate measuring for position and velocity of the vehicle. The central control unit first determines most of the control commands based on the position and the velocity information of the vehicle. Then, the central control unit combines the other information and finally issues the modified control command. Therefore, it is basically required to detect position and velocity information with reliability and accuracy. A single sensor based position and velocity detection system is difficult to guarantee stability and reliability due to limitations such as time, space constraints, inaccuracies and uncertainties of single sensor, and danger of sensor failure.
Therefore, in this study, we propose a sensor fusion model using multiple Doppler, tachometer and GPS sensors to improve the reliability and accuracy of position and velocity measuring. There are many studies on the vehicle state estimation with various methods and approaches. Feng et al. [2] proposed an algorithm to consider the correlation between sensors in a sequential filter, and Smyth and Wu [3] designed a multi-rate Kalman filter for sensor fusion at different periods. Olfati-Saber [4] presented a modified DKF algorithm that uses two identical consensus filters for fusion of the sensor data and covariance information. Lee [5] applied reliability indexed sensor fusion to estimate the vehicle velocity.
In particular, various researches have been conducted on a technique of fusing GPS and other sensors to detect the position and velocity of an autonomous vehicle. Thrapp et al. [6] used the Extended Kalman filter(EKF) [7] to integrate DGPS and odometry information for position estimation and improved accuracy by setting the number of satellites used for position detecting as a measure of DGPS accuracy, and changing the measurement error covariance. Ohno et al. [8] also used EKF to integrate DGPS and odometry information and improved accuracy by excluding DGPS information, which is considered to be inaccurate. Laneurit et al. [9] presented an approach of multi-sensor fusion able to locate a vehicle with a decimeter precision. They got the precision by taking into account a complete model of errors on GPS data as well as the data provided by an original approach coupling a vision algorithm with a precise numerical map.
In this paper, we propose a technique to integrate information provided by multiple sensor types in multiple to obtain reliability and accuracy. The proposed fusion model consists of a multiple sensor fusion model that integrates the same type of sensors and a multifarious sensor fusion model that integrates the different type of sensors. We propose a distributed Kalman filter for the multiple sensor fusion that uses weighted averages to reflect the reliability of sensors as a way to improve fault tolerance. The multifarious sensor fusion model uses a Kalman filter error correction for compensate the accumulative error in the main sensor with the other type of sensor which has characteristic of biased error. We utilize the works in [12] to integrate digital map, GPS sensor data, and speed sensor information to acquire the accurate position information. We also propose a Kalman filter model to integrate the local multiple sensor fusion estimates.
To have no performance degradation even in single sensor failure, and to acquire the sensor information secured with fault tolerance with high reliability, this can be solved through the homogeneous multiple sensor fusion. The multiple sensor fusion model with fault tolerance provided in this study is based on the distributed Kalman Filter [7] as the basic structure. The distributed Kalman filter is the form of performing the estimate process in the local filter on the mutually independent sensors to integrate the information provided from the local filter by the master filter. These structures can handle the faults occurred from the individual sensor in the local filter level, and can provide continuous solution even when the fault is occurring, so it has the characteristics of having tolerance to faults.
In this study, the reliability of each sensor used in the multiple sensor fusion is evaluated and weighted value is granted to select the main sensor, and the method that the error weighted average with each sensor is compensated to find the fusion sensor result value is presented.
Figure 1 shows the adaptive Kalman filter structure that is provided for the multiple sensor fusion. The master filter outputs the final sensor value minimized with the sensor effect occurred with fault based on the error estimate value on each sensor value inputted from each local Kalman filter. The local Kalman filter estimates the measurement value of each sensor and the error value with main sensor, and the main filter calibrates this to be used as the estimate value Ŝ_{i} of each sensor. The reliability on the estimate value Ŝ_{i} of each sensor is defined by the total of deviation on the estimate value for each sensor used in the fusion as shown in
where SD_{k} is the reliability of sensor k, and Ŝ_{i} shows the estimate value of sensor i. The master filter reflects the reliability SD_{k} defined in
The proposed multiple sensor fusion model was validated through the simulation. The velocity pattern was produced on one random section, and in the process of multiple 5 sensors measuring this, the case of having each difference pattern of error on two random sensors was performed with the simulation on the signal estimation performance of the proposed fusion model. Figure 2 shows the sensor measurement values used in the simulation. In a specific point, the sensor1 was supposed to show abnormal high value to assume the fault situation, and the sensor2 was supposed to model a big noise. Figure 3 shows the simulation result, and it was verified that the final output signal did not deviate greatly from the standard signal even though the two sensors among the five sensors had malfunction.
In this study, the method is proposed on having the tachometer as the main sensor and the Doppler sensor as the auxiliary sensor to compensate the position error of the main sensor through the auxiliary sensor. The tachometer has the advantage of measuring the moving distance and velocity of the vehicle continuously, but there is a problem of having difficulty in applying independently in the automatic unmanned driving system requiring precision control due to the occurrence of error on the velocity and position information when there is slip or slide in the wheels on the acceleration or braking of the vehicle. The Doppler sensor has the possibility to have error over the tolerance on measuring the sensor in the non-contact method, so it has the disadvantage of not being able to guarantee the safety of the measurement signal when used independently. In this study, the method is proposed on improving the performance by enabling the fusion of heterogeneous sensors with difference characteristics for measuring the velocity and position.
The position measurement error δr and the velocity measurement error δv of the tachometer are shown as follows.
where v and r represent the vehicle velocity and position, respectively. In Figure 4, a, r_{m}, and r_{d} represent the vehicle acceleration, the position measurement value of the tachometer and Doppler sensor, respectively. By taking into account the noise w_{m} and w_{d} of the tachometer and Doppler sensor, we can establish the following equation:
The difference of the tachometer position measurement value and the Doppler sensor position measurement value of z is shown as follows:
From this, the state
We can derive discrete form of the sate
where the state variable is
By applying the system model (
The simulation environment was developed to evaluate the performance of the position estimation system. The resolution of the tachometer was assumed to be 200 pulse/revolution and vehicle movement velocity as 2 m/sec. Also, the variance of the tachometer of 0.001 m/sec was added for the noise. The error caused by the slip or slide that can occur on acceleration and deceleration was also considered by adding noise on the acceleration and deceleration profile. We modeled the characteristic of Doppler sensor that is not accumulated by adding the noise of 0 mean and 0.01 m/sec variance.
In this simulation, it is verified that the proposed estimation model can compensate for the disadvantages of the two sensors by integrating a tachometer with high uncertainty but low inaccuracy and Doppler sensor with relatively low uncertainty but inaccuracy. Figure 5 shows the result of the simulation. It can be seen that the error of the fused position estimate through the Kalman filter is less dispersed and stable than the data of the simple position information without fusion.
We utilize the works in [12] to integrate digital map, GPS sensor data, and speed sensor information to acquire the accurate position estimate. In the paper, latitude longitude directional moving patterns from the velocity and position data of the vehicle moving on a specific path are applied to the state transition matrix of the Kalman filter to estimate the position and velocity. The estimated position is used for the map-matching technique to estimate the final position.
The data obtained from GPS is matched to digital map to reduce an error contained in GPS. The matched position data is used for the input of the adaptive Kalman filter of which system model is represented as
where
The α represents an estimated acceleration obtained by approximating the moving pattern. In
In Figure 6, the s_{A}, v_{A} and s_{B}, v_{B} represent the estimated position and velocity at the sampling time k−1 and k, respectively, and v_{C} represent the velocity projected on the approximated velocity profile. Then, we can obtain the approximated acceleration α as shown in
The α plays a role of changing the estimated velocity in state transition matrix depending on the moving pattern. The measurement equation applied to the Kalman filter is as shown in
The map based GPS error correction filter was applied to an experiment of measuring the position of vehicle moving on predefined path. In the experiment, the error was defined as the distance between the measured position and the reference absolute position. Figure 7 shows the variation of measurement error with points for the cases of point-to-curve matching and GPS error filter matching.
Figure 8 shows the overall structure of the multifarious and multiple sensor fusion model proposed in this study. In the hierarchical sensor signal processing structure for permitting single sensor fault, the lower part enables multiple sensor fusion to improve the fault tolerance, and each sensor data passing through the local filter in the lower structure is ultimately fused in the upper position and velocity filter to provide the position and velocity estimation improved with precision and reliability.
The sensor data provided from the lower local filter passes through the velocity estimation filter, position estimation filter and direction estimation filter to provide the final position and velocity estimation. The state variables to be assumed can be expressed as follows:
where V^{n} and R^{n} represent the vehicle velocity and position in the navigation coordinate, respectively. The acceleration f^{b} measured in the vehicle coordinate can be transformed into navigation coordinate, and the gravity can be removed to obtain the acceleration v^{n} in the navigation coordinate as shown in
where
We can express
We can obtain the discrete form of
The velocity vector of the system model is the reference of navigation system coordinate, and
In this study, the estimate value of the system model based the adaptive Kalman filter is used as the measurement value to estimate the final position through the main filter. The measurement model of the main filter estimating the final position is shown as follows:
The latitude ϕ, longitude λ and height h can be transformed into the TM coordinate system as in
where R_{N} and R_{E} are the radius of curvature in the north direction of the meridian and east, respectively, and ϕ_{0} and λ_{0} are the latitude and longitude of the origin of the TM coordinate system.
Converting the TM coordinates in
By introducing the matrix D to denote the process of converting the position r that is estimated by the GPS and map fusion position filter into the navigation coordinate system, the position measurement model z_{k} can be expressed as
In order to verify the validity of the proposed multifarious and multiple sensor fusion model in this paper, three times of experimental navigating were conducted on the specific route of approximately 3.7 km length from Gireum IC to Kookmin University as shown in Figure 9. The accuracy of the position estimation was compared with the reference absolute position and the accuracy of the velocity estimation was compared with the average velocity between the specific points on the moving range to test the validity of the proposed sensor fusion model.
Figure 10 shows the test result in a graph, comparing the GPS data, the GPS information corrected through the map matching algorithm, and the information estimated through the fusion filter proposed in this paper. As shown in Figure 10, the GPS information contains errors and shows a significant deviation from the actual road, and the errors are irregular. The correction position to which the map matching algorithm is applied does not deviate from the road but still contains errors for the reference position. In this paper, we show that the proposed location estimation results show more accurate results.
In this paper, we propose a method of estimating sensor information with reliability and accuracy through fusion of multiple sensors in realizing position and velocity device of autonomous navigation vehicle. In order to obtain reliable information without performance deterioration in sensor failure or fault, we propose a method of integrating multiple sensors by applying an adaptive Kalman filter. We proposed multiple sensor fusion method for the same kind of sensors to overcome the disadvantages of different sensors with different characteristics and to utilize the advantages. We also proposed a fusion model that integrates GPS information with digital map information to improve the accuracy of position measurement.
The multiple sensor fusion and the multifarious sensor fusion model were verified to be improved in fault tolerance performance and accuracy of velocity estimate through each simulation, and the performance of the map information based GPS fusion algorithm was verified through the actual driving experiment. In this paper, we propose a multifarious and multiple sensor fusion that estimates the position and velocity using information from each local filter. As a result of experiment on the driving vehicle mounting the device implemented with the proposed estimation algorithm to perform the actual road driving test for verifying the performance, the average position error was 1.5764 m and the accuracy of the speed estimate was 99.81% in the total area.
This research was supported by Seokyeong University in 2017.
No potential conflict of interest relevant to this article was reported.
Multiple sensor fusion model.
Simulated sensor profiles.
Simulation result of sensor fusion.
Multifarious sensor fusion model.
Simulation result of multifarious sensor model.
Approximation of moving pattern and the
Measurement error of the compared map matching algorithms. (a) Point-to-curve matching, (b) GPS error filter matching.
Block diagram of the position and velocity estimator.
Experimented navigation route.
Experimental results of position estimating.
E-mail: ywcho@skuniv.ac.kr
E-mail: lhjin@hknu.ac.kr