Vehicle navigation and positioning method

1: GPS/DR combined Positioning Method

2: GPS/mm combined Positioning Method

 

Application of Improved combined filtering in GPS/DR combined Positioning

Original Author: HUANG Zhi, Zhong Zhihua

I. Preface

Since the Global Position System (GPS) was officially put into use in 1994, GPS-based vehicle navigation technology has been widely used. GPS signals are transmitted in a straight line, with low energy. When encountering obstacles, the normal reception of signals will be affected. In the urban traffic environment, the high-rise buildings, viaduct, tunnel, and other obstacles are blocked, short-term communication interruptions and reduced positioning accuracy often occur. The dead reckoning (DR) method uses the on-board sensor to measure the speed, mileage, and heading angle information, and uses the space to calculate the vehicle's Azimuth, which is highly autonomous and highly precise. The Dr positioning error increases with the integration process. If the Dr positioning error is independent for a long time, the error will be divergent. GPS/DR Integrated Navigation realizes the complementary advantages and disadvantages of the two positioning technologies to improve the accuracy and reliability of the positioning system. It is one of the main technologies used by the multi-sensor integrated navigation system.

The fusion of GPS and Dr information is the key to the combined navigation and positioning technology. Kalman filter is the optimal statistical estimation for the random system. The Application of Kalman filter in the combined navigation and positioning system has a long history. Centralized Kalman filter can be used for information fusion of multi-sensor combination systems to achieve optimal statistical estimation. However, due to centralized processing, when the state dimension of the system increases,AlgorithmThere are two problems: ① The calculation workload is directly proportional to the 3rd power of the state dimension, making real-time calculation difficult; ② fault tolerance is poor, and the entire system information is contaminated due to a sensor failure.

The decentralized Kalman filter proposed at the end of 1970s is an improvement to the traditional Kalman filter. It adopts a hierarchical filter structure to improve the fault tolerance of the filter. However, the algorithm is complex and the communication between filters is large, making it difficult to implement the project.

At the end of 1980s, Carlson proposed a joint filter, which improved the decentralized filter. According to the information conservation principle, the global state estimation information and system noise information were allocated to each local filter using the information distribution coefficient. The algorithm has the advantages of simplicity, fault tolerance, and ease of engineering implementation.

The authors use federal Kalman Filter (FKF) to integrate GPS and Dr information. Based on the low accuracy of positioning sensors and the low computing capability of on-board navigation computers, improved the combined filtering algorithm to slightly reduce the fusion accuracy and improve the computing efficiency of the original algorithm.

2. GPS/DR combined Positioning

The GPS/DR combined positioning system adopted by the author is shown in Figure 1.

GPS receives satellite signals and generates positioning information and directly outputs it to the information fusion algorithm. Dr sensing units include Angular Rate Gyroscope, wheel speed sensor, and MCU signal sampling. The gyro measures the yaw rate of the vehicle and obtains the increment of the heading angle through integral points. A wheel speed sensor is a standard component of a vehicle. The wheel speed signal can be obtained from a mileage sensor or an ABS wheel speed pulse sensor. The MCU and analog-to-digital conversion circuit are used to sample sensor signals and input the sampled data to the fusion algorithm. The fusion algorithm runs in the upper navigation computer and integrates GPS and Dr unit information to generate positioning and output to the navigation software.

To reduce system costs, navigation computers generally use peripheral processors, such as arm, XScale, and MIPS. Compared with CPUs in DSP and x86 architectures, navigation computers have poor computing capabilities, in addition, if you want to run the navigation software that occupies a large amount of system resources, the computing resources allocated to the fusion algorithm are limited. The Engineering Prototype uses the Samsung s3c2440a (400mhzarm920 kernel, no floating point coprocessor) as the main control CPU navigation computer, and the computing resource allocated to the fusion algorithm is about 30 MIPS. Therefore, reducing the computing complexity of the fusion algorithm while maintaining a reasonable precision is the key to the Engineering Implementation of the fusion algorithm.

Figure 1 GPS/DR combined Positioning System

Iii. combined GPS/DR Filtering

(1) filter common State Equation

FKF consists of GPS, Dr local filters, and primary filters. The status XI of the XG and Dr filters of the GPS filter is the same, and the public status is used, where E and n are the east and north directions respectively, and the east and north directions are the velocity respectively; the acceleration is east and north respectively. Acceleration is described using the current acceleration statistical model

In the formula, the mean of acceleration is used, the mean of the acceleration model is used as the reciprocal of the time constant of the acceleration model, and the mean is zero and the variance is the Gaussian white noise respectively.

The system state equation is described

Formula

Using one-step acceleration prediction instead of the current mean of mobile acceleration, the simplified form of the state equation of the discrete system can be obtained.

XC (k + 1) = PHI (k) XC (k) + WC (k) (4)

In formula, t is the time step of the discrete system, WC (k) is the discrete white noise sequence, and the noise covariance is expressed as Q (k) [8 ].

Formula

(2) Dr local filter observation equation

Dr local filter's observed value obtained after error compensation DR's estimated heading angle θ and displacement s, respectively

In formula, ε θ and ε s are the observed noise of Dr heading angle and displacement respectively. The mean value is zero and the variance is respectively. Obtain the first-order linear discrete equation

Formula

WI (k) is the observed noise matrix, H (k) is H (k-1) of Xi (k-1) First Order Differential

Dr observations are affected by sensor error accumulation. Therefore, when the GPS signal is good, GPS observations are used to correct Dr observations. Take λ θ (K) and λ S (k) as GPS course and mileage observations and the corresponding Dr observations respectively. In order to suppress GPS noise, the amplitude limiting processing is performed on them, the Adaptive Error feedback correction process for Dr observations is as follows.

In formula, θ 0 is the initial heading angle, ω is the yaw velocity measured by the gyroscope, S0 is the original output of the mileage sensor, and K θ and Ks are the error feedback coefficients respectively, the value is related to the calibration status of the sensor parameters. The higher the accuracy of the sensor parameter correction, the smaller the feedback coefficient. β V is the speed information distribution coefficient of the GPS local filter. Its physical significance is: when the precision of the GPS subsystem is reduced, the weight in the fusion result is reduced by an hour (that is, β V is reduced), and the feedback in formula (8) is also reduced to suppress the GPS error pollution Dr observations. Therefore, the proposed joint filtering algorithm is fault tolerant.

(3) GPS local filter observation equation

GPS output is used to locate the east direction and north direction of the observed volume, for example and NG. The observed equation is

In formula, ω e and ω N are respectively observed noise, and the root Root Mean of noise in the open receiving environment is σ E and σ n.

The observed noise matrix is

GPS observed noise is a time-varying process. The document [9] proposes to use Recursive Estimation of system noise and measurement of noise variance matrix, but there is a large amount of computing in practical application. The hdop value is the amplification factor of the GPS positioning error on the horizontal plane [10]. Through repeated GPS static location tests in various receiving environments, the results show that it is difficult to establish a mathematical model of accurate number of receiving satellites SVS, level accuracy factor hdop and GPS observed noise. Static experiments only show the general conclusion: When SVS is ≥6, the GPS observation error is small. When SVS is less than 6, the positioning noise increases, and the hdop value roughly describes the positioning accuracy of GPS.

Based on the static test results, the adaptive adjustment method of the GPS noise matrix R is proposed.

In formula, K σ is determined according to the following rules: when the number of received satellites is SVS ≥6, K σ = hdop; otherwise K σ = max (2, hdop ), and K σ <5.

(4) global filtering process

The primary filter only integrates the output state and covariance of the local filter, calculates the covariance pm and State XM of the primary filter, and the primary filter has no time update and measurement update processes. The typical integration process is

In the formula, PG and PI are the covariance of GPS and Dr local filters respectively.

Because of the large amount of calculation for matrix inversion, the author improved the covariance fusion process. The diagonal matrix composed of the pair elements of the covariance array was used to reverse the original covariance array. The fusion accuracy was slightly reduced, but the calculation workload was reduced.

Resetting the covariance of a local filter with information allocation coefficient β is

And reset the local filter status

The information distribution coefficient is the weight of each local filter in the fusion output. Therefore, this value must reflect the state estimation accuracy of each local filter. The local filter has different precision on the estimation of each State. The GPS local filter has a high accuracy on the estimation of location, while the Dr local filter has a high accuracy on the estimation of speed and acceleration. Therefore, two information distribution coefficients, β V and β P, were used for information fusion of velocity, acceleration, and location components. Calculated by the following formula:

Iv. Test Results and Analysis

To verify the improvement FKF algorithm proposed by the author, this algorithm is used to integrate the road test data. The parameters used by the filter are shown in figure 2 (because low-precision scanning map is used, the actual trajectory deviates from the map Road), and the position is a wide-field quadrilateral section in the north area of Shenzhen Nanshan hi-tech park, SVS = 6 ~ 10. The repeatability error of the GPS track on the road section is less than 110 m. In the absence of an absolute positioning benchmark, the GPS positioning is true (true ).

Figure 2 test route

First-order Markov random noise is added to the original GPS positioning information to simulate GPS Positioning noise under certain conditions.

The noise model is

In formula, NN is Gaussian white noise, root-mean-square value is 50, Tau n is the reciprocal of the process time constant, and Tau n = 0.01s-1. the variance of east direction and North Direction Positioning noise is 26.74 and 49.84, respectively. According to the static test statistical results, the approximate equivalent is the receiving conditions of SVS = 5 and hdop = 2.2.

The Dr sensor information is synchronously collected during the road test. The synchronous signal is the second pulse output by GPS, and the sampling frequency is 40Hz. In the Dr model, the Gyro Scale Factor error is-1.6%, the zero drift error is 0.048 °/s, and the mileage Scale Factor error is-0.4%. The proposed improved algorithm is used to integrate GPS location information mixed with noise and Dr sensor information to verify the effectiveness of the algorithm.

Figure 3 compares FKF results with GPS Positioning and real locations. It can be seen that the GPS location with mixed noise obviously deviates from the real track. After the improved algorithm is used for filtering, the fusion positioning mostly falls near the real track.

Figure 3 combined Kalman Filter Position Estimation

Table 1 analyzes the statistical values of the improved joint filtering algorithm, typical joint filtering algorithm, and GPS positioning error. The improved algorithm improves the positioning accuracy of the composite system. Because the covariance fusion process of the primary filter is simplified, the accuracy of the algorithm is slightly lower than that of the typical fusion algorithm.

Table 1 Statistical Analysis of positioning errors

Another important role of m in improving FKF information fusion is to improve the heading estimation accuracy. Figure 4 improves the comparison between FKF and GPS heading angle. The GPS course is estimated to have a great deal of noise, especially at low speed. After FKF integrates DR and GPS signals, the course direction estimation accurately follows the real course.

Figure 4 FKF course Direction Estimation

After the improved algorithm is transplanted to the navigation computer, it is tested that the navigation software can still operate normally under 8/s converged computing conditions (which can be converged once per second, therefore, the efficiency of the improved algorithm basically meets the requirements.

V. Conclusion

A six-dimensional state variable FKF algorithm is proposed to simplify the covariance calculation process of the primary filter, the fusion computing workload is greatly reduced when the fusion accuracy is slightly reduced. The proposed Adaptive Error feedback compensation and GPS noise adaptive adjustment improve the accuracy of DR and GPS subsystem observation, and have good fault tolerance. The road test results verify that the proposed improved information fusion algorithm is effective.