CN114488239A - Close combination robust relative position sensing method for vehicle collaborative navigation - Google Patents

Abstract

The invention discloses a close combination robust relative position sensing method for vehicle cooperative navigation, which relates to the technical field of position sensing, introduces a robust theory from the angle of data fusion estimation of a multi-navigation system, and further improves the sensing precision of relative positions by improving a data fusion means in a vehicle cooperative positioning solution. Aiming at the colored noise interference of a system and the precision loss caused by nonlinearity, a Robust volume filtering (RCKF) algorithm based on Huber M estimation is provided, and an Robust difference theory is integrated; firstly, time updating and measurement estimation are carried out through a three-order volume rule, so that the precision loss of a nonlinear system is avoided; and secondly, measuring, updating and controlling colored noise interference by using Huber M estimation to obtain system state estimation, fusing L1 and L2 norms, and reducing the weight of noise interference so as to improve the quality of the posterior information of the system. A new state quality control strategy is provided for VANET relative position perception.

Description

A Compact Combination Robust Relative Position Perception Method for Vehicle Cooperative Navigation

technical field

The invention relates to the field of position perception, in particular to a tight-combination robust relative position perception method for vehicle cooperative navigation.

Background technique

As a prerequisite for co-location, relative location awareness is the basis of location-based services and vehicle intelligent transportation [1-2]. Global Navigation Satellite System (GNSS) has become the preferred navigation method to realize relative position awareness and coordinate positioning due to its wide working range and mature technology. However, in urban environments, the density and height of buildings can cause relative position solutions that rely on GNSS to lose accuracy. Although the fusion of multiple satellite navigation systems can alleviate the problem of urban canyons to a certain extent, it also adds additional interference errors between multiple systems; at the same time, the increased receiver signal bandwidth to achieve multi-system fusion will inevitably absorb more noise. It also increases the cost and power consumption of the receiver.

In order to avoid the above problems and improve the co-location accuracy, researchers start with other navigation methods, introduce new observations, and propose new relative positioning solutions;

XU B,SHEN L,YAN F,2009.Vehicular Node Positioning Based on Doppler-Shifted Frequency Measurement on Highway[J].Journal of Electronics(China),26(2):265-269. Proposes to use the vehicle's own Doppler The relative position can be obtained by using the measured value of LE, and experiments show that the performance of this method is better than that based on the global positioning system (Global Positioning System, GPS) in terms of accuracy and robustness, but its realization needs to place radiation nodes on the roadside, Depends on infrastructure;

ALAM N, TABATABAEI BALAEI A, DEMPSTER A G, 2011. A DSRC Doppler-Based Cooperative Positioning Enhancement for Vehicular Networks With GPSAvailability[J]. IEEE Transactions on Vehicular Technology, 60(9): 4462–4470. Combining the positions of adjacent vehicles, Speed, using Dedicated Short Range Communication (DSRC) to obtain Doppler frequency shift and then propose a loose combination method to avoid dependence on infrastructure, and its performance is improved by 48% compared with independent GPS results;

ALAM N,TABATABAEI BALAEI A,DEMPSTER A G,2013.Relative PositioningEnhancement in VANETs:A Tight Integration Approach[J].IEEE Transactions onIntelligent Transportation Systems,14(1):47–55. Using GPS underlying data to eliminate spatial correlation errors The double-difference method, which eliminates errors such as satellite clock and ionospheric interference by performing a second-difference on the pseudorange, achieves higher positioning performance than Differential GPS (DGPS);

ALAM N, KEALY A, DEMPSTER A G, 2013. An INS-Aided Tight Integration Approach for Relative Positioning Enhancement in VANETs[J]. IEEE Transactionson Intelligent Transportation Systems, 14(4): 1992–1996. Introduction of Inertial Navigation System , INS) to further comprehensively measure the information, and combine the GPS underlying data and the relative acceleration between vehicles to obtain a co-location solution, but the algorithm only uses the INS to provide the relative acceleration, and the acquisition of the Euler angle of the system still relies on Doppler translation, which cannot be fully utilized. INS information;

Feng S, Joon W C, Andrew G D 2015. An ultra-wide bandwidth-based range/GPS tight integration approach for relative positioning in vehicular ad hocnetworks-IOPscience[EB/OL](2015)[2021–09–10]. Before summary Research by people, using Ultra Wide Band (UWB) technology with high communication volume and strong anti-interference ability to conduct vehicle ranging in vehicle ad hoc network, and use INS to obtain Euler angles in the system, and integrate pseudorange double difference, Doppler frequency shift double difference and UWB distance propose a compact combination co-location method, which achieves better localization effect than the above algorithms.

Although the relative position solution achieves higher accuracy by updating the measurement and combining the new positioning method, the data fusion methods used in the above algorithms are all Extended Kalman Filter (EKF). This estimation method, which directly performs Taylor expansion truncation approximation on the Gaussian integral, can only achieve first-order accuracy. Considering the nonlinearity of the system and the non-Gaussian interference of the actual colored noise, the estimation accuracy will be further reduced, resulting in the performance of the collaborative navigation algorithm. underutilized.

As the core technology of collaborative navigation, relative position awareness is also the key to vehicle intelligent transportation, and plays an important role in the vehicle ad hoc network (Vehicle Ad Hoc Networks, VANET). Under the same hardware conditions, the acquisition of a posteriori information of co-location is usually limited to a certain accuracy; although the existing research has carried out research and exploration from the perspective of navigation mode and observation type, which has improved the positioning accuracy, the accuracy caused by nonlinearity is still limited. There is still no effective solution to the problem of contamination of observables by loss and colored noise.

SUMMARY OF THE INVENTION

In order to solve the deficiencies of the existing technology, a method of tight-combination robust relative position perception for vehicle cooperative navigation is provided. The robust theory is introduced from the perspective of multi-navigation system data fusion estimation, and the data fusion method in the vehicle cooperative positioning solution is improved. , to further improve the perception accuracy of relative position; in view of the accuracy loss caused by colored noise interference and nonlinearity of the system, a robust volumetric filter (Robust Cubature Kalman Filter, RCKF) algorithm based on Huber M estimation is proposed, which is integrated into the robustness theory; The volume rule is used to perform time update and quantity measurement estimation to avoid the loss of accuracy of nonlinear systems; use Huber M estimation to perform measurement update and control colored noise interference to obtain system state estimation, fuse L1 and L2 norm, and reduce the weight of noise interference quantity, In order to improve the quality of system posterior information; it provides a new state quality control strategy for VANET relative position awareness;

The technical scheme adopted by the present invention is:

A compact combination robust relative position perception method for vehicle cooperative navigation, comprising the following steps;

S1: respectively equip GPS receivers on vehicle a and vehicle b, and simultaneously equip the two vehicles with UWB transceivers, turn on the GPS receivers and UWB transceivers of vehicle a and vehicle b, and start and drive the cars;

S2: use the GPS receiver to obtain the satellite pseudorange and Doppler frequency shift of vehicles a and b, and use the UWB transceiver to obtain the relative distance between the two workshops;

S3: Use the difference between the coordinates of the reference satellite base station and the satellite position and the GPS pseudorange received by the satellite base station to perform a difference, obtain the correction amount of the GPS signal, and correct the GPS signals of vehicles a and b;

S4: Calculate the GPS pseudorange and Doppler frequency shift double difference of the observation satellite;

The satellite pseudorange and Doppler frequency shift obtained by the difference between the satellite pseudoranges and Doppler frequency shifts of the satellites to vehicles a and b are compared with the satellite pseudoranges and Doppler frequency shifts of adjacent satellites to vehicles a and b. The single difference of the frequency shift amount is made difference;

S5: Use the GPS signal corrected in S3 to perform single-point positioning, obtain the initial position coordinates of the two vehicles, and obtain the vehicle position coordinates through resection of the pseudo-ranges of at least four satellites;

S6: Based on the Robust Cubature Kalman Filter (RCKF) algorithm estimated by Huber M, the initial relative position of the two vehicles obtained by S5 is used to obtain the initial relative position, combined with the relative distance measured by the UWB transceiver and the GPS receiver. The obtained Doppler frequency shift double difference and GPS pseudorange double difference are used for data fusion solution: calculate the volume point set at the initial moment, and set the covariance matrix; secondly, at the second moment, use the volume point set at the initial moment, relative The position and covariance matrix is combined with the measurement of the UWB transceiver and the GPS receiver at the current moment to perform state update and measurement update, and the relative position and covariance matrix of the current moment are obtained, and then in the next moment, the relative position and The covariance matrix is iteratively updated, and the state and measurement update process is repeated to obtain the posterior relative position at the next moment, which specifically includes the following steps:

S6.1: Calculate the volume point set according to the initial moment of S5:

According to Cubature criterion, a volume point set of equal weight is generated at the initial moment, and the relative position at the initial moment is obtained by using the difference between the initial positions of the two vehicles obtained by S5, the relative speed is set to zero, and the covariance matrix is set;

S6.2: Time update: From the second moment onwards, use the volume point set and covariance matrix, relative position and velocity of the previous moment combined with the third-order volume rule to simulate the state transition probability of the system, and obtain the prior three-dimensional relative position, three-dimensional relative velocity, and a priori covariance matrix for estimating posterior position and velocity;

S6.3: Perform measurement update:

According to the prior covariance matrix, the volume point set is updated again according to the third-order volume rule;

Using the new volume points, by performing equal-weighted fusion estimation of the measurement volume points, the measurement estimates of each sensor at the current moment are obtained, that is, the satellite pseudorange double difference, the Doppler frequency shift double difference, and the estimated value of the relative distance of the UWB transceiver;

Construct a linear regression using the current moment measurement, the estimated value of the measurement and the prior three-dimensional relative position, the relative velocity combined with the observation equation;

Use Huber M estimation to solve the above linear regression, and obtain the posterior three-dimensional relative position and relative velocity and their covariance matrix at the current moment;

S6.4 repeats S6.2 and S6.3 in the next moment, and iteratively performs time update and measurement update;

S7: The time update and measurement update in S6 are iteratively performed with the running time of the vehicle until the vehicle stops running and each sensor is turned off, and finally the covariance matrix of the posterior state estimate and the posterior state estimate is obtained.

The posterior state estimate includes three-dimensional relative position and three-dimensional relative velocity.

beneficial technical effect

The present invention proposes a tight-combination robust relative position sensing method for vehicle cooperative navigation. The EKF in the background technology is comparable in accuracy to the classical volumetric Kalman Filter (CKF), while the RCKF uses the volumetric rule for nonlinear system transfer. At the same time, the robust M estimation is integrated to suppress the interference of the colored noise in the actual measurement. Therefore, the performance is better than the EKF and CKF in terms of accuracy and robustness. By improving the data fusion method, the relative position perception accuracy can be further improved. A practical system quality control strategy is provided for the vehicle co-location scheme.

Description of drawings

FIG. 1 is a flowchart of a method for sensing a tight-combination robust relative position for vehicle cooperative navigation provided by an embodiment of the present invention;

FIG. 2 is a schematic diagram of UWB communication and ranging provided by an embodiment of the present invention;

FIG. 3 is a flowchart of an RCKF algorithm provided by an embodiment of the present invention.

Detailed ways

The specific embodiments of the present invention will be described in further detail below in conjunction with the accompanying drawings and examples;

The present invention proposes a compact-combined robust relative position perception method for vehicle cooperative navigation, introduces robust theory from the perspective of multi-navigation system data fusion estimation, and further improves the relative position reliability by improving the data fusion method in the vehicle collaborative positioning solution. Perceptual accuracy; Aiming at the accuracy loss caused by colored noise interference and nonlinearity of the system, a robust volumetric filter (Robust Cubature Kalman Filter, RCKF) based on Huber M estimation is proposed, which is integrated into the robustness theory; Update and measure estimation to avoid the loss of accuracy of nonlinear systems; secondly, use Huber M estimation to measure and update to control colored noise interference to obtain a system state estimate, fuse L1 and L2 norms, and reduce the weight of noise interference to improve the system The quality of posterior information; provides a new state quality control strategy for VANET relative position awareness;

The technical scheme adopted by the present invention is:

A compact combination robust relative position perception method for vehicle cooperative navigation, as shown in Figure 1, includes the following steps;

S1: Install GPS receivers on vehicle a and vehicle b respectively, as shown in Figure 2, and equip two UWB transceivers on the two vehicles at the same time, turn on the GPS receivers and UWB transceivers of vehicle a and vehicle b, and start the and drive the car;

In this embodiment, the Leica GS10 GPS receiver Novatel INS-LCI (Integrated GNSS-INS) is respectively equipped on the vehicle a and the vehicle b to obtain the high precision of the vehicle a and the vehicle b based on the carrier phase difference (Real Time Kinematic, RTK). The position is used as the real reference value to calculate the position error. At the same time, two UWB transceivers are equipped on the two vehicles respectively, and the satellite receivers and UWB transceivers of vehicle a and vehicle b are turned on, and the vehicle is started and driven;

S2. Use the GPS receiver to obtain the satellite pseudorange and Doppler frequency shift of vehicles a and b, and the UWB transceiver to obtain the relative distance between the two workshops;

S3: Use the difference between the coordinates of the reference satellite base station and the satellite position and the GPS pseudorange received by the satellite base station to perform a difference, obtain the correction amount of the GPS signal, and correct the GPS signals of vehicles a and b;

S4: Calculate the GPS pseudorange and Doppler frequency shift double difference of the observation satellite;

The pseudorange/Doppler frequency shift single difference obtained by the difference between the pseudorange/Doppler frequency shift of the satellites to vehicles a and b is compared with the pseudorange/Doppler frequency shift single difference of the adjacent satellites to vehicles a and b. do bad;

S5: Use the GPS signal corrected in S3 to perform single-point positioning, obtain the initial position coordinates of the two vehicles, and obtain the vehicle position coordinates through resection of the pseudo-ranges of at least four satellites;

S6: Based on the Robust Cubature Kalman Filter (RCKF) algorithm estimated by Huber M, the initial relative position of the two vehicles obtained by S5 is used to obtain the initial relative position, as shown in Figure 3, combined with the relative position measured by the UWB transceiver The distance, the Doppler frequency shift double difference measured by the GPS receiver and the GPS pseudorange double difference are used for data fusion calculation: the volume point set is calculated at the initial moment, and the covariance matrix is set; secondly, at the second moment, the initial moment is used. The volume point set, relative position and covariance matrix of the current time are combined with the measurement of the UWB transceiver and GPS receiver at the current time to perform state update and measurement update to obtain the relative position and covariance matrix at the current time. The relative position and covariance matrix at one moment are iteratively updated, and the state and measurement update process is repeated to obtain the posterior relative position at the next moment; the specific process is as follows:

S6.1: Calculate the volume point set according to the initial moment of S5:

According to Cubature criterion, a volume point set of equal weight is generated at the initial moment, and the relative position at the initial moment is obtained by using the difference between the initial positions of the two vehicles obtained by S5, the relative speed is set to zero, and the covariance matrix is set according to experience;

S6.2: Time update: From the second moment onwards, use the volume point set and covariance matrix, relative position and velocity of the previous moment combined with the third-order volume rule to simulate the state transition probability of the system, and obtain the prior three-dimensional relative position, three-dimensional relative velocity, and a priori covariance matrix for estimating posterior position and velocity;

S6.3: Perform measurement update:

According to the prior covariance matrix, the volume point set is updated again according to the third-order volume rule;

Using the new volume points, by performing equal-weighted fusion estimation of the measurement volume points, the measurement estimates of each sensor at the current moment are obtained, that is, the satellite pseudorange double difference, the Doppler frequency shift double difference, and the estimated value of the relative distance of the UWB transceiver;

Construct a linear regression using the current moment measurement, the estimated value of the measurement and the prior three-dimensional relative position, the relative velocity combined with the observation equation;

Use Huber M estimation to solve the above linear regression, and obtain the posterior three-dimensional relative position and relative velocity and their covariance matrix at the current moment;

S6.4 repeats S6.2 and S6.3 in the next moment, and iteratively performs time update and measurement update;

S7: The time update and measurement update in S6 are iteratively performed with the running time of the vehicle until the vehicle stops running and each sensor is turned off, and finally the covariance matrix of the posterior state estimate and the posterior state estimate is obtained.

The posterior state estimate includes three-dimensional relative position and three-dimensional relative velocity.

In order to verify the authenticity of this experiment, as shown in Table 1, under the same experimental conditions, robust volumetric filtering (RobustCubature Kalman Filtering, RCKF) relative to Extended Kalman Filter (Extended Kalman Filter, EKF), volumetric filtering (Cubature Kalman Filtering, RCKF) , CKF) in Root Mean Squared (RMS), Accuracy (Accuracy), Robustness (Precision) improvement;

Table 1

Claims (4)

1.A close combination robust relative position sensing method for vehicle collaborative navigation is characterized in that: comprises the following steps;

s1, respectively arranging GPS receivers on a vehicle a and a vehicle b, simultaneously respectively arranging UWB transceivers on the two vehicles, turning on the GPS receivers and the UWB transceivers of the vehicle a and the vehicle b, and starting and driving the vehicles;

s2, acquiring satellite pseudo ranges and Doppler frequency shift quantities of the vehicles a and b by using a GPS receiver, and acquiring a relative distance between the vehicles by using a UWB transceiver;

s3: the difference value of the reference satellite reference station coordinates and the satellite position is combined with the GPS pseudo range received by the satellite reference station for difference, the correction value of the GPS signal is obtained, and the GPS signals of the vehicles a and b are corrected;

s4: calculating the double difference of the GPS pseudo range and the Doppler frequency shift of an observation satellite;

s5: performing single-point positioning by using the GPS signal corrected in S3 to obtain initial position coordinates of two vehicles, and performing backward intersection by using pseudo ranges of at least four satellites to obtain position coordinates of the vehicles;

s6: based on RCKF algorithm of Huber M estimation, utilize the initial position coordinate of two cars that S5 obtained to obtain initial relative position, combine the relative distance that UWB transceiver measured, Doppler shift double difference and GPS pseudo range double difference that GPS receiver measured to carry on the data fusion to solve: calculating a volume point set at an initial moment, and setting a covariance matrix; secondly, at a second moment, performing state updating and measurement updating by using a volume point set, a relative position and a covariance matrix at an initial moment and combining a UWB transceiver and a GPS receiver measurement at the current moment to obtain the relative position and the covariance matrix at the current moment, then performing iterative updating by using the relative position and the covariance matrix at the previous moment at the next moment, and repeating the state and measurement updating process to obtain the posterior relative position at the next moment;

s7: and (4) iteratively executing time updating and measurement updating in the step S6 along with the running time of the vehicle until the vehicle stops running, and shutting down each sensor to finally obtain the posterior state estimation and the covariance matrix of the posterior state estimation.

2. The method for close-coupled robust relative position sensing for vehicle collaborative navigation according to claim 1, wherein: the specific process of S6 is as follows: and the satellite pseudo range and the Doppler frequency shift single difference obtained by the satellite to the satellite pseudo range and the Doppler frequency shift of the vehicles a and b are subtracted from the satellite pseudo range and the Doppler frequency shift single difference of the adjacent satellite to the vehicles a and b.

3. The method for close-coupled robust relative position sensing for vehicle collaborative navigation according to claim 1, wherein: the specific process of S6 includes the following steps:

s6.1: calculating a volume point set according to the initial time of S5:

generating a volume point set with equal weight at the initial moment according to the Cubasic rule, obtaining the relative position at the initial moment by using the difference between the initial positions of the two vehicles obtained by S5, setting the relative speed to be zero and setting a covariance matrix;

s6.2: and (3) time updating: from the second moment, simulating the state transition probability of the system by utilizing the volume point set and the covariance matrix at the previous moment and the relative position and speed combined with a third-order volume rule to obtain a prior three-dimensional relative position, a prior three-dimensional relative speed and a prior covariance matrix for estimating a posterior position and speed;

s6.3: and (3) carrying out measurement updating:

updating the volume point set according to the prior covariance matrix and the third-order volume rule again;

carrying out weighting fusion estimation on the measured volume points by using the new volume points, and obtaining the measured estimation values of each sensor at the current moment, namely satellite pseudo-range double differences, Doppler frequency shift double differences and estimation values of the relative distance of a UWB transceiver;

measuring the estimated value and the prior three-dimensional relative position by using the current moment quantity and the quantity, and combining the relative speed with an observation equation to construct a linear regression;

solving the linear regression by using Huber M estimation to obtain a three-dimensional relative position and relative speed of the posterior at the current moment and a covariance matrix thereof;

s6.4 repeats S6.2 and S6.3 at the next time, iteratively performing the time update and the measurement update.

4. The method for close-coupled robust relative position sensing for vehicle collaborative navigation according to claim 1, wherein: the a posteriori state estimates include three-dimensional relative positions and three-dimensional relative velocities.

Images