Vehicle cooperative positioning method based on robust adaptive high-order volume Kalman filtering

Abstract

The invention discloses a vehicle cooperative positioning method based on robust adaptive high-order volume Kalman filtering, belongs to the technical field of cooperative navigation, is used for processing cooperative navigation data, and comprises the following steps: establishing a system model according to GNSS / UWB tight coupling integration; carrying out nonlinear updating by utilizing a five-order volume rule; a fading factor is derived based on a strong tracking theory, and correction of a prior state covariance is realized; on the basis that a linear regression model approximates a non-linear measurement model, a Huber function is used for reconstructing measurement noise, so that the influence of non-Gaussian noise is inhibited; and optimizing measurement updating of a filtering algorithm according to the corrected priori state covariance and measurement noise. According to the method, good relative positioning performance can be realized under the condition that the system has model errors and non-Gaussian noise interference, and the precision and robustness of vehicle cooperative positioning are improved.

Description

Technical Field

[0001] The present invention relates to the field of vehicle collaborative positioning methods, and in particular to a vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering. Background Art

[0002] For applications such as intelligent transportation systems and location-based services, maintaining the availability of collaborative positioning solutions has become a research hotspot in collaborative positioning of vehicle ad hoc networks (VANETs). GNSS has become the preferred navigation method for achieving relative position perception due to its wide operating range and mature technology. However, the limited accuracy and availability of GNSS cannot meet the needs of location-based applications in VANETs. The development of wireless networks has promoted the application of collaborative positioning in VANETs. Vehicle-to-vehicle communication positioning technology can improve the performance of absolute positioning / relative positioning, and realize vehicle collaborative positioning by fusing distance and global navigation satellite system measurement data. Ultra-wideband (UWB), as a spread spectrum carrier-free communication technology, has the characteristics of strong signal anti-interference ability, high transmission rate, and anti-multipath. Therefore, scholars have studied the fusion strategy of GNSS and UWB to improve the positioning accuracy in GNSS-restricted environments. GNSS / UWB integration is mainly divided into two architectures: loose coupling and tight coupling. Compared with loose coupling, tight coupling has more advantages in positioning accuracy in complex scenarios such as frequent phase interruptions and low visibility. Therefore, tightly coupled GNSS / UWB integration is more suitable for vehicle collaborative positioning in complex urban areas.

[0003] However, the solution of tightly coupled GNSS / UWB integration is affected by three major problems: system model error, model nonlinearity, and interference from non-Gaussian noise. Although these problems can be solved by using additional high-precision sensors through redundant information, the use of additional sensors will lead to increased costs and also limit the application areas of the integrated system. In contrast, data fusion technology provides a cost-effective solution to improve the performance of tightly coupled GNSS / UWB integrated vehicle cooperative positioning. Extended Kalman filter (EKF) is usually used as a standard nonlinear filtering technique for integrated navigation information fusion. However, for tightly coupled GNSS / UWB integration, since the original GNSS pseudorange observations are directly taken into account in the system observations, the observation model has strong nonlinear characteristics, and the positioning accuracy of EKF is poor due to the linearization of the system model. Compared with EKF, unscented Kalman filter (UKF) avoids the linearization of the system model and has higher approximation accuracy. However, for high-dimensional nonlinear systems, the performance of UKF may not meet expectations. The cubature Kalman filter (CKF) uses the third-order cubature criterion to approximate the Gaussian integral, and its numerical stability and filtering accuracy are better than UKF. However, the third-order CKF cannot accurately calculate the Gaussian weighted integral of nonlinear functions. Therefore, scholars have derived a high-order cubature criterion based on the third-order cubature criterion and proposed a high-degree cubature Kalman filter (HCKF). However, all of the above nonlinear filters require accurate pre-modeling of the system model and cannot completely solve all three problems of GNSS / UWB tight coupling.

[0004] There are many adaptive or robust methods in the Kalman filter framework, such as adaptive fading memory filter, robust filter, covariance matching filter, etc., to solve the system model error and non-Gaussian noise interference. Among all these filters, adaptive fading memory filter and robust filter are widely used due to their effectiveness and moderate computational load. The former constructs a fading factor based on the new information vector, so that the weight of historical information is small and the weight of current observation information is large, thereby recursively adjusting the filtering process to reduce the influence of system model error. The latter converts the measurement equation into a linear regression problem of observation and state prediction, and then uses M estimation to solve it. The Huber loss function is used to reduce the weight of the disturbed measurement value to achieve estimation performance adjustment. Both have their own advantages, that is, the adaptive filter handles the system model error, while the robust filter is insensitive to abnormal observations. Summary of the invention

[0005] The purpose of the present invention is to provide a vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering to solve the problems of decreased performance and poor reliability of vehicle collaborative positioning under conditions of system nonlinearity, system model error and non-Gaussian noise interference.

[0006] The vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering proposed by the present invention is characterized by comprising the following steps: Step 1: Establish a system model based on GNSS / UWB tightly coupled integration; Step 2: Use the fifth-order volume rule for nonlinear update; Step 3: Derivation of the fading factor based on strong tracking theory , the prior state covariance is modified according to the fading factor, thereby adjusting the influence of the system model error on the state estimation; Step 4: Based on the linear regression model approximating the nonlinear measurement model, the M-estimation is used to achieve the solution, and the measurement noise is reconstructed through the Huber cost function to suppress the influence of non-Gaussian noise; Step 5: Optimize the measurement update of the filtering algorithm based on the modified prior state covariance and measurement noise.

[0007] In the above steps, the GNSS / UWB tightly coupled integrated system model is established in step 1, including the state equation and measurement equation: State equations for GNSS / UWB tightly coupled integrated systems In the formula, is the system state vector; is the observation period; is the state transfer matrix; is the process noise model; is the three-axis relative acceleration noise, which satisfies the mean value of zero and the standard deviation of The specific definition of each matrix is: , in, is the relative position vector of workshops a and b; is the relative velocity vector between the two workshops; for The identity matrix; the process noise covariance is defined as . The measurement equation of the GNSS / UWB tightly coupled integrated system is: , in, The observed quantity includes pseudorange double difference, Doppler frequency shift double difference and relative distance between two vehicles. ; is a nonlinear function; is the observation noise. When the number of publicly observable GNSS satellites is When we define the matrix : , in, represents a matrix whose elements are all 1. Let , and are the variance of pseudorange, Doppler frequency shift and UWB measurement error respectively, and the matrix , and Assuming that the measurements are independent of each other, the observation noise covariance can be defined as: . The nonlinear update using the fifth-order volume rule described in step 2 is specifically performed as follows: Assume that it is known The posterior probability density at time , based on the fifth-order volume criterion, calculate the volume points and corresponding weights , in, is the system state dimension; point set , is defined as , in, and Represents the identity matrix No. Column and List. The weight of a volume point is , Propagate volume points through the state equation and obtain the system's prior information through weighted averaging , Propagate volume points through measurement equations, calculate predicted measurements, innovation covariances, and cross-covariances . Step 3 specifically includes the following steps: The prior state covariance is corrected, and the specific strategy is: , in, is the corrected predicted state covariance; is a vanishing factor, satisfying: , in, , The specific definition is , in, It is a weakening factor, which is used to avoid possible over-adjustment and ensure smoother state estimation; The covariance matrix of the output residual vector is usually obtained by the windowing method. The present invention proposes an innovation covariance estimation method based on the gradually fading memory exponential weighting method, which allocates weight coefficients according to the law of negative exponential function. time The recursion formula is , in, is the output residual vector; For the forgetting factor.

[0008] Step 4 specifically includes the following steps: Construct a linear regression equation and transform the measurement update into solving a linear regression problem: , in, is the error between the predicted value and the true value of the state; is the equivalent observation matrix; define the expansion matrix , simplified linear regression model , The specific definition of each matrix is: , Use Huber M estimation to solve the above linear regression and define the loss function: , in, is the total dimension of state and observation quantities, is the residual vector No. As the cost function in M ​​estimation, Huber The function can be expressed as , in, is the constraint factor. In order to realize the constraint capability of the cost function strategy, the measurement update needs to be corrected to meet ,Right now: , in, , define the function , then , In the HCKF framework, the Huber M estimation method uses the weighting matrix To optimize the measurement information, according to The residual covariance is reweighted and the measurements are reconstructed, and the updated measurement noise covariance is . Step 5 specifically includes the following steps: Based on the reconstructed measurement noise, the innovation covariance matrix is ​​modified , Calculate the gain matrix, posterior state and corresponding covariance to complete the filtering algorithm optimization . The advantages of the present invention are: The present invention optimizes nonlinear estimation based on the fifth-order sphere-phase volume criterion; Based on the strong tracking theory, the present invention derives an adaptive fading factor to correct the prior state covariance and adjust the influence of the system model error on the state estimation; The present invention uses M estimation to achieve solution based on the linear regression model approximating the nonlinear measurement model, and reconstructs the measurement noise through the Huber cost function, thereby suppressing the influence of non-Gaussian noise. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments: Figure 1 The number of publicly visible satellites for vehicle-mounted experiments; Figure 2 A comparison chart of the relative position error of the X-axis obtained by the method provided by the present invention and the two filtering algorithms EKF and HCKF in vehicle-mounted experiments; Figure 3 A comparison chart of the Y-axis relative position error obtained by the method provided by the present invention and the two filtering algorithms EKF and HCKF in vehicle-mounted experiments; Figure 4 A comparison diagram of the relative position error of the Z axis obtained by the method provided by the present invention and the two filtering algorithms EKF and HCKF in vehicle-mounted experiments; Figure 5 A comparison chart of the distance errors obtained by the method provided by the present invention and the two filtering algorithms EKF and HCKF in vehicle-mounted experiments; Figure 6A comparison diagram of the error cumulative distribution function obtained by the method provided by the present invention and the two filtering algorithms EKF and HCKF in vehicle-mounted experiments; Figure 7 The present invention is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0010] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments: like Figure 7 As shown, the vehicle collaborative positioning method based on the robust adaptive high-order cubature Kalman filter includes the following steps: Step 1: Establish a system model based on GNSS / UWB tightly coupled integration; State equations for GNSS / UWB tightly coupled integrated systems In the formula, is the system state vector; is the observation period; is the state transfer matrix; is the process noise model; is the three-axis relative acceleration noise, which satisfies the mean value of zero and the standard deviation of The specific definition of each matrix is: , in, is the relative position vector of workshops a and b; is the relative velocity vector between the two workshops; for The identity matrix; the process noise covariance is defined as . The measurement equation of the GNSS / UWB tightly coupled integrated system is: , in, The observed quantity includes pseudorange double difference, Doppler frequency shift double difference and relative distance between two vehicles. ; is a nonlinear function; is the observation noise. When the number of publicly observable GNSS satellites is When we define the matrix : , in, represents a matrix whose elements are all 1. Let , and are the variance of pseudorange, Doppler frequency shift and UWB measurement error respectively, and the matrix , and Assuming that the measurements are independent of each other, the observation noise covariance can be defined as: . Step 2: Use the fifth-order volume rule for nonlinear update; Assume that it is known The posterior probability density at time , based on the fifth-order volume criterion, calculate the volume points and corresponding weights , in, is the system state dimension; point set , is defined as , in, and Represents the identity matrix No. Column and List. The weight of a volume point is , Propagate volume points through the state equation and obtain the system's prior information through weighted averaging , Propagate volume points through measurement equations, calculate predicted measurements, innovation covariances, and cross-covariances . Step 3: Derivation of the fading factor based on strong tracking theory , the prior state covariance is modified according to the fading factor, thereby adjusting the influence of the system model error on the state estimation; The prior state covariance is corrected, and the specific strategy is: , in, is the corrected predicted state covariance; is a vanishing factor, satisfying: , in, , The specific definition is , in, It is a weakening factor, which is used to avoid possible over-adjustment and ensure smoother state estimation; The covariance matrix of the output residual vector is usually obtained by the windowing method. The present invention proposes an innovation covariance estimation method based on the gradually fading memory exponential weighting method, which allocates weight coefficients according to the law of negative exponential function. time The recursion formula is , in, is the output residual vector; For the forgetting factor.

[0011] Step 4: Based on the linear regression model approximating the nonlinear measurement model, the M-estimation is used to achieve the solution, and the measurement noise is reconstructed through the Huber cost function to suppress the influence of non-Gaussian noise; Construct a linear regression equation and transform the measurement update into solving a linear regression problem: , in, is the error between the predicted value and the true value of the state; is the equivalent observation matrix; define the expansion matrix , simplified linear regression model , The specific definition of each matrix is: , Use Huber M estimation to solve the above linear regression and define the loss function: , in, is the total dimension of state and observation quantities, is the residual vector No. As the cost function in M ​​estimation, Huber The function can be expressed as , in, is the constraint factor. In order to realize the constraint capability of the cost function strategy, the measurement update needs to be corrected to meet ,Right now: , in, , define the function , then , In the HCKF framework, the Huber M estimation method uses the weighting matrix To optimize the measurement information, according to The residual covariance is reweighted and the measurements are reconstructed, and the updated measurement noise covariance is . Step 5: Optimize the measurement update of the filtering algorithm based on the modified prior state covariance and measurement noise; Based on the reconstructed measurement noise, the innovation covariance matrix is ​​modified , Calculate the gain matrix, posterior state and corresponding covariance to complete the filtering algorithm optimization .

[0012] The effect of the invention is verified by an example below. The vehicle-mounted experimental data collection site is located on Clifton Avenue between Derby Road and Loughborough Road in Nottinghamshire, England. It has an open sky view and can maximize satellite visibility. The sampling rate of the experimental data is 1Hz, and the data synchronization uses GNSS time to ensure that there is no time correlation in the measurement. The data used in the experiment is the maximum duration data with RTK reference in the collected data, which is about 14 minutes. In order to verify the actual performance of the present invention, the experiment is compared with the estimated performance of EKF and HCKF respectively. In the figure, RAACKF represents the collaborative positioning filtering algorithm of the present invention. Figure 1 The number of publicly visible satellites at each moment throughout the experiment is shown, and it can be seen that the number of visible satellites is almost always higher than the minimum required number, which is four. Figure 2-Figure 4 The relative position errors of the three axes of the three schemes are Figure 5 The relative distance error of the estimation result is given, and it can be seen that the estimation accuracy of EKF, HCKF and RAACKF is improved in turn. The present invention uses a high-order volume criterion for nonlinear transfer, establishes a fading factor based on the strong tracking theory to correct the prior state covariance, and incorporates the robust M estimation to suppress the non-Gaussian noise interference in the actual measurement. Therefore, the performance in accuracy and robustness is better than that of EKF and HCKF. Figure 6 The error cumulative distribution functions of the three schemes are given, and it can be seen that the estimation performance of the present invention is significantly better than that of EKF and HCKF.

[0013] In summary, the present invention discloses a vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering, which can improve the relative positioning accuracy under the conditions of system nonlinearity, system model error and non-Gaussian noise interference, and provide a practical system quality control strategy for vehicle collaborative positioning solutions.

Claims

1. A vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering, characterized in that: The following steps are involved: Step 1: Establish a system model based on GNSS / UWB tightly coupled integration; Step 2: Use the fifth-order volume rule for nonlinear update; Step 3: Derivation of the fading factor based on strong tracking theory , the prior state covariance is modified according to the fading factor, thereby adjusting the influence of the system model error on the state estimation; Step 4: Based on the linear regression model approximating the nonlinear measurement model, the M-estimation is used to achieve the solution, and the measurement noise is reconstructed through the Huber cost function to suppress the influence of non-Gaussian noise; Step 5: Optimize the measurement update of the filtering algorithm based on the modified prior state covariance and measurement noise.

2. The vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering according to claim 1 is characterized in that: In step 1, a GNSS / UWB tightly coupled integrated system model is established, including state equations and measurement equations: State equations for GNSS / UWB tightly coupled integrated systems In the formula, is the system state vector; is the observation period; is the state transfer matrix; is the process noise model; is the three-axis relative acceleration noise, which satisfies the mean value of zero and the standard deviation of The specific definition of each matrix is: , in, is the relative position vector of workshops a and b; is the relative velocity vector between the two workshops; for The identity matrix; the process noise covariance is defined as . The measurement equation of the GNSS / UWB tightly coupled integrated system is: , in, The observed quantity includes pseudorange double difference, Doppler frequency shift double difference and relative distance between two vehicles. ; is a nonlinear function; is the observation noise. When the number of publicly observable GNSS satellites is When we define the matrix : , in, represents a matrix whose elements are all 1. Let , and are the variance of pseudorange, Doppler frequency shift and UWB measurement error respectively, and the matrix , and Assuming that the measurements are independent of each other, the observation noise covariance can be defined as: 。 3. The vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering according to claim 1 is characterized in that: In step 2, the fifth-order volume rule is used for nonlinear updating. The specific process is as follows: Assume that it is known The posterior probability density at time , based on the fifth-order volume criterion, calculate the volume points and corresponding weights ， in, is the system state dimension; point set , is defined as , in, and Represents the identity matrix No. Column and List. The weight of a volume point is , Propagate volume points through the state equation and obtain the system's prior information through weighted averaging , Propagate volume points through measurement equations, calculate predicted measurements, innovation covariances, and cross-covariances 。 4. The vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering according to claim 1 is characterized in that: Step 3 The prior state covariance is corrected, and the specific strategy is: , in, is the corrected predicted state covariance; is a vanishing factor, satisfying: , in, , The specific definition is , in, It is a weakening factor, which is used to avoid possible over-adjustment and ensure smoother state estimation; The covariance matrix of the output residual vector is usually obtained by the windowing method. The present invention proposes an innovation covariance estimation method based on the gradually fading memory exponential weighting method, which allocates weight coefficients according to the law of negative exponential function. time The recursion formula is , in, is the output residual vector; For the forgetting factor.

5. The vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering according to claim 1 is characterized in that: Step 4 specifically includes the following steps: Construct a linear regression equation and transform the measurement update into solving a linear regression problem: , in, is the error between the predicted value and the true value of the state; is the equivalent observation matrix; define the expansion matrix , simplified linear regression model ， The specific definition of each matrix is: , Use Huber M estimation to solve the above linear regression and define the loss function: , in, is the total dimension of state and observation quantities, is the residual vector No. As the cost function in M ​​estimation, Huber The function can be expressed as , in, is the constraint factor. In order to realize the constraint capability of the cost function strategy, the measurement update needs to be corrected to meet ,Right now: , in, , define the function , then ， In the HCKF framework, the Huber M estimation method uses the weighting matrix To optimize the measurement information, according to The residual covariance is reweighted and the measurements are reconstructed, and the updated measurement noise covariance is 。 6. The vehicle collaborative positioning method based on robust adaptive high-order cubature Kalman filtering according to claim 1 is characterized in that: Step 5 specifically includes the following steps: Based on the reconstructed measurement noise, the innovation covariance matrix is ​​modified , Calculate the gain matrix, posterior state and corresponding covariance to complete the filtering algorithm optimization 。

Images