A cooperative vehicle positioning method based on gaussian filter belief propagation

Abstract

The application discloses a cooperative vehicle positioning method based on a Gaussian filter belief propagation, and comprises the following steps: initializing position information and a covariance matrix of all vehicles and anchor points; according to state information of the vehicles at t-1 time, the vehicles are used to calculate predicted position information of the vehicles at t time by using a motion model of the vehicles; linearization is performed on a distance measurement model to obtain approximation; belief messages between the vehicles and between the vehicles and the anchor points are calculated according to the linearized formula of the approximation, and M times of belief propagation iteration calculation are performed; posterior position estimation and covariance of the vehicles are calculated, and L times of iteration calculation are further performed, so that the position estimation value and the covariance matrix of the vehicles are finally obtained. The application effectively improves the positioning precision of the vehicles in a satellite signal-free driving environment and a random packet loss communication network.

Description

Technical Field

[0001] This invention relates to the field of vehicle cooperative positioning technology in vehicle communication, and in particular to a cooperative vehicle positioning method based on Gaussian filter belief propagation. Background Technology

[0002] Autonomous driving technology has become the current development direction of vehicle technology. A prerequisite for achieving autonomous driving is high-precision and stable vehicle positioning. Global Navigation Satellite Systems (GNSS) are currently the most widely used vehicle positioning technology, providing location information to vehicle users. However, in driving environments such as urban roads and tunnels, satellite signals can be blocked, leading to the loss of positioning information. Cooperative positioning effectively improves the problem of satellite signal loss and enhances positioning accuracy. Vehicles use onboard instruments such as radar to acquire distance information between themselves and surrounding vehicles or roadside anchor points. Anchor points are roadside units or vehicles that can provide high-precision positioning, and this information is shared with surrounding vehicles. The vehicle can then use this information to update its own location information, thereby improving positioning accuracy.

[0003] Existing vehicle cooperative localization technologies typically employ a distributed approach based on Bayesian estimation. Since cooperative localization requires data on the relative distances between vehicles, and distance measurement models are often nonlinear, localization accuracy often depends on the precision of linearization. Nonparametric belief propagation, using a large number of particles to achieve an approximation, leads to excessive computational complexity and high channel bandwidth. Parametric belief propagation can effectively reduce computational complexity and channel bandwidth, making it suitable for practical applications.

[0004] literature( F.Garc′ L. Svensson, and S. The paper "Cooperative Localization Using Posterior Linearization Belief Propagation" (IEEE Trans. Veh. Technol., vol. 67, no. 1, pp. 832-836) combines posterior linearization and belief propagation, applying statistical linear regression to the distance measurement model, thus reducing the demand for communication bandwidth. However, cooperative localization places stringent requirements on the communication network, and most cooperative localization algorithms do not consider the problem of random packet loss in vehicular communication networks. Summary of the Invention

[0005] The technical problem to be solved by this invention is to overcome the problems of satellite signal loss and random packet loss in communication networks. It provides a cooperative vehicle localization method based on Gaussian filter belief propagation, in which vehicles cooperate through belief propagation algorithm to provide high-precision positioning for vehicles.

[0006] To solve the above-mentioned technical problems, the present invention adopts the following technical solution.

[0007] A cooperative vehicle localization method based on Gaussian filter belief propagation includes:

[0008] Step 1: Initialize the initial position values ​​and covariance matrix of all vehicles and anchor points.

[0009]

[0010] Among them, let Indicates a collection of vehicles. Represents the set of anchor points. This represents the set of both vehicles and anchor points, i.e. Both vehicles and anchor points are considered nodes. It is the two-dimensional position of node a at the initial time. For the quantity to be estimated The mean, For the quantity to be estimated The covariance matrix;

[0011] Step 2: Based on the state information of vehicle i at time t-1 Including vehicle position and speed information, the predicted vehicle position information at time t is calculated using the vehicle's motion model. in, It is the two-dimensional velocity of vehicle i at time t-1;

[0012] Step 3: Based on the location information of vehicle i and its neighboring node j Covariance Matrix The distance measurement model is linearized to obtain linearized model parameters; where adjacent node j represents other vehicles and anchor points that communicate with vehicle i.

[0013] Step 4: Calculate the belief messages between vehicles and between vehicles and anchor points using the approximate linearized formula. And perform M iterations of belief propagation calculations;

[0014] Step 5: After M iterations, calculate the posterior position estimate of the vehicle based on the probability distribution of the vehicle's edge posterior position, and return to step 3 for L iterations.

[0015] Step 6: After L iterations, calculate the minimum mean square error estimate of the vehicle's posterior position at time t. The convergent solution.

[0016] Specifically, step 2 includes the following process:

[0017] By utilizing the vehicle's position and speed information during its movement, the instantaneous position of the vehicle and the predicted position of vehicle i at time t can be calculated when satellite signals are lost. The calculation is as follows:

[0018]

[0019] Where Δt is the length of the time slot.

[0020] Specifically, in step 3, a posterior linearization method is used, employing statistical linear regression for linearization, including:

[0021] Step 3.1, the measurement model for the distance between the vehicle and the anchor point, and between vehicles, is as follows:

[0022]

[0023] in, It is the Euclidean distance between vehicle i and its adjacent node j. The mean is zero and the variance is... Additive white Gaussian noise;

[0024] The nonlinear part of the relative distance can be linearized as follows:

[0025]

[0026] in, These are the linearization coefficients. It is a constant term. The mean is zero and the variance is... Additive white Gaussian noise;

[0027] Step 3.2: Use posterior linearization to linearize the model parameters. The calculation steps are as follows:

[0028] Step 3.2.1. Select m sigma-points (χ1,...,χ) based on the mean and covariance of the vehicle location distribution. m and weights w1,...,w m ;

[0029] Step 3.2.2. For sigma-points χ1,...,χ m Perform transformation Z j =h(x j ), j = 1, 2, ..., m;

[0030] Step 3.2.3. Calculate the following formula:

[0031]

[0032] in, It is Z j The weighted average, ψ is x j Z j The weighted covariance matrix, φ is the Z j The variance matrix;

[0033] Step 3.2.4. Parameters The calculation is as follows:

[0034]

[0035] Specifically, step 4 includes:

[0036] Step 4.1. The belief messages between vehicles and between vehicles and anchor points are:

[0037]

[0038] in, It is a relative distance measurement value The likelihood function of vehicle i, where n(i) represents the adjacent nodes of vehicle i;

[0039] Substituting the approximate linearized formula into the belief message yields:

[0040]

[0041]

[0042] in The following formula represents:

[0043]

[0044] Step 4.2. Considering the possibility of random packet loss during communication, the delay model is as follows:

[0045] y k =(1-γ) k )z k +γ k z k-1 ,k>1 (10)

[0046] Where k represents the discrete time series, γ k It is a Bernoulli random variable, p(γ) k =1)=E[γ k ] = p k When γ k =0, the vehicle receives the distance measurement value at the current moment, when γ k=1, the vehicle uses the value from the previous moment because the information reception failed due to packet loss; in calculating the belief message, The vehicle's status needs to be expanded to include... in The initial mean is variance

[0047] Step 4.3. The specific steps of the M-times belief propagation iteration algorithm are as follows:

[0048] Step 4.3.1. Let in Let P be the expected position of vehicle i at time t. i t Let i be the position covariance matrix of vehicle i at time t;

[0049] Step 4.3.2. Calculate α ki The prior estimate at time t Covariance α ki The posterior estimate at time t-1 Covariance and covariance as well as and of The specific formula is as follows:

[0050]

[0051] Step 4.3.3. Substituting (11) into the following formula will update the mean of the noise. Covariance

[0052]

[0053]

[0054] in, It is the filtering gain for estimating noise. It is y ki The prior estimate at time t-1, It is y ki The prior estimate of the covariance at time t-1; yes and covariance;

[0055] Step 4.3.4. Obtain the predicted... P i t|t-1Substituting (11) into the following formula, we can calculate the mean value of vehicle i's position at time t. Covariance P i t ;

[0056]

[0057] in, It is the filter gain for estimating the position. yes and covariance, yes and covariance;

[0058] Step 4.3.5. Calculate the belief messages between vehicles and between vehicles and anchor points according to formula (8). After the calculation is completed, return to step 4.3.1 and iterate M times.

[0059] Specifically, in step 5, the probability distribution of the edge posterior position of vehicle i is as follows:

[0060]

[0061] in, This indicates that the prior distribution of vehicle i at time t has a mean of 1. The covariance matrix is ​​P i t Gaussian distribution, It is the belief message from adjacent node j of vehicle i to vehicle i at time t.

[0062] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0063] 1. This invention introduces posterior linearization for linearization and belief propagation. The posterior linearization method linearizes the nonlinear distance measurement model, and belief propagation is performed on the linearized model. This effectively reduces the computational load on the positioning calculation and the bandwidth requirements of the communication network, while ensuring high positioning accuracy even in the absence of satellite signals. Furthermore, a Gaussian filter effectively overcomes the impact of random packet loss, reducing the decrease in positioning accuracy caused by random packet loss.

[0064] 2. Compared with the standard posterior linearized belief propagation cooperative localization method, the cooperative vehicle localization method based on Gaussian filter belief propagation in this invention can effectively reduce localization errors caused by communication network congestion and packet loss. Experimental results show that this invention can effectively improve the vehicle localization accuracy. Attached Figure Description

[0065] Figure 1This is a vehicle network system model diagram of a cooperative vehicle localization method based on Gaussian filter belief propagation according to the present invention.

[0066] Figure 2 This is a flowchart of a cooperative vehicle localization method based on Gaussian filter belief propagation according to the present invention.

[0067] Figure 3 This is a simulation result of the cumulative distribution of positioning error for a cooperative vehicle positioning method based on Gaussian filter belief propagation according to the present invention. Detailed Implementation

[0068] This invention discloses a cooperative vehicle localization method based on Gaussian filter belief propagation, comprising: initializing the position information and covariance matrix of all vehicles and anchor points; wherein, anchor points are facilities such as roadside units that can provide high-precision positioning; calculating the predicted position information of the vehicles at time t based on the vehicle state information at time t-1 using the vehicle motion model; linearizing the distance measurement model to obtain an approximation; calculating the belief messages between vehicles and between vehicles and anchor points according to the approximate linearization formula, and performing M iterations of belief propagation calculations; calculating the posterior position estimate and covariance of the vehicles, and then performing L iterations of calculations to finally obtain the vehicle's position estimate and covariance matrix. This invention can effectively improve the positioning accuracy of vehicles in driving environments without satellite signals and in communication networks with random packet loss.

[0069] The present invention will now be described in further detail with reference to the accompanying drawings.

[0070] like Figure 1 As shown, in the cooperative vehicle positioning method based on Gaussian filter belief propagation of the present invention, roadside anchor points can obtain high-precision location information. The anchor points can be other roadside units or vehicles that can provide high-precision positioning. If a vehicle loses satellite signals and cannot obtain positioning information in real time, it can measure the distance between itself and other vehicles and roadside anchor points through vehicle-mounted equipment such as radar, and share information through communication. Based on the shared information, it can calculate its own location information.

[0071] like Figure 2 As shown, the cooperative vehicle localization method based on Gaussian filter belief propagation of the present invention includes the following steps:

[0072] Step 1: Initialize the initial position values ​​and covariance matrix of all vehicles and anchor points.

[0073]

[0074] Among them, let Indicates a collection of vehicles. Represents the set of anchor points. This represents the set of both vehicles and anchor points, i.e. Both vehicles and anchor points are considered nodes. It is the two-dimensional position of node a at the initial time. For the quantity to be estimated The mean, For the quantity to be estimated The covariance matrix;

[0075] Step 2: Based on the state information of vehicle i at time t-1 Including vehicle position and speed information, the predicted vehicle position information at time t is calculated using the vehicle's motion model. in, It is the two-dimensional velocity of vehicle i at time t-1;

[0076] Step 3: Based on the location information of vehicle i and its neighboring node j Covariance Matrix The distance measurement model is linearized to obtain linearized model parameters; where adjacent node j represents other vehicles and anchor points that communicate with vehicle i.

[0077] Step 4: Calculate the belief messages between vehicles and between vehicles and anchor points using the approximate linearized formula. And perform M iterations of belief propagation calculations;

[0078] Step 5: After M iterations, calculate the posterior position estimate of the vehicle based on the probability distribution of the vehicle's edge posterior position, and return to step 3 for L iterations.

[0079] Step 6: After L iterations, calculate the minimum mean square error estimate of the vehicle's posterior position at time t. The convergent solution.

[0080] In step 2, by utilizing the vehicle's position and speed information during its movement, the instantaneous position of the vehicle can be estimated when satellite signals are lost. The predicted position of vehicle i at time t is calculated as follows:

[0081]

[0082] Where Δt is the length of the time slot.

[0083] Step 3.1, the measurement model for the distance between the vehicle and the anchor point, and between vehicles, is as follows:

[0084]

[0085] in, It is the Euclidean distance between vehicle i and its adjacent node j. The mean is zero and the variance is... Additive white Gaussian noise;

[0086] The nonlinear part of the relative distance can be linearized as follows:

[0087]

[0088] in, These are the linearization coefficients. It is a constant term. The mean is zero and the variance is... Additive white Gaussian noise;

[0089] Step 3.2: Use posterior linearization to linearize the model parameters. The calculation steps are as follows:

[0090] Step 3.2.1. Select m sigma-points (χ1,...,χ) based on the mean and covariance of the vehicle location distribution. m and weights w1,...,w m ;

[0091] Step 3.2.2. For sigma-points χ1,...,χ m Perform transformation Z j =h(x j ), j = 1, 2, ..., m;

[0092] Step 3.2.3. Calculate the following formula:

[0093]

[0094] in, It is Z j The weighted average, ψ is x j Z j The weighted covariance matrix, φ is the Z j The variance matrix;

[0095] Step 3.2.4. Parameters The calculation is as follows:

[0096]

[0097] Step 4 specifically includes:

[0098] Step 4.1. The belief messages between vehicles and between vehicles and anchor points are:

[0099]

[0100] in, It is a relative distance measurement value The likelihood function of node i, where n(i) represents the neighboring nodes of node i;

[0101] Substituting the approximate linearized formula into the belief message yields:

[0102]

[0103] in The following formula represents:

[0104]

[0105] Step 4.2. Considering the possibility of random packet loss during communication, the delay model is as follows:

[0106] y k =(1-γ) k )z k +γ k z k-1 ,k>1 (10)

[0107] Where k represents the discrete time series, γ k It is a Bernoulli random variable, p(γ) k =1)=E[γ k ] = p k When γ k =0, the vehicle receives the distance measurement value at the current moment, when γ k =1, the vehicle uses the value from the previous moment because the information reception failed due to packet loss; in calculating the belief message, The vehicle's status needs to be expanded to include... in The initial mean is variance

[0108] Step 4.3. The specific steps of the M-times belief propagation iteration algorithm are as follows:

[0109] Step 4.3.1. Let in Let P be the expected position of vehicle i at time t. i t Let i be the position covariance matrix of vehicle i at time t;

[0110] Step 4.3.2. Calculate α ki The prior estimate at time t Covariance α ki The posterior estimate at time t-1 Covariance and covariance as well as and of The specific formula is as follows:

[0111]

[0112] Step 4.3.3. Substituting (11) into the following formula will update the mean of the noise. Covariance

[0113]

[0114] in, It is the filtering gain for estimating noise. It is y ki The prior estimate at time t-1, It is y ki The prior estimate of the covariance at time t-1; yes and covariance;

[0115] Step 4.3.4. Obtain the predicted... P i t|t-1 Substituting (11) into the following formula, we can calculate the mean value of vehicle i's position at time t. Covariance P i t ;

[0116]

[0117]

[0118] in, It is the filter gain for estimating the position. yes and covariance, yes and covariance;

[0119] Step 4.3.5. Calculate the belief messages between vehicles and between vehicles and anchor points according to formula (8). After the calculation is completed, return to step 4.3.1 and iterate M times.

[0120] In step 5, the probability distribution of the edge posterior position of vehicle i is:

[0121]

[0122] in, This indicates that the prior distribution of vehicle i at time t follows a mean of . The covariance matrix is ​​P i t Gaussian distribution, It is the belief message from adjacent node j of vehicle i to vehicle i at time t.

[0123] like Figure 3 As shown, according to a cooperative vehicle localization method based on Gaussian filter belief propagation according to the present invention, a simulation experiment was conducted on cooperative vehicle localization. The scenario road was set as a three-lane one-way road, containing 11 anchor points and 20 vehicles. Both vehicles and anchor points were simulated as individual nodes. Each vehicle traveled at a constant speed of 15 m / s in its lane, and the communication radius of each vehicle was 200 m. The distance interval between anchor points was 100 m. Vehicles obtained initial position information via satellite signals. For anchor points, the prior error of position was set to 0.1 m, and for vehicle nodes, the prior error was set to 5 m. After entering a tunnel, the vehicles lost satellite signals and only exchanged information with neighboring vehicles and roadside anchor points within their communication range for cooperative localization. The speed measurement noise was 0.2 m / s, the variance of the relative distance measurement was 0.2 m, and the random packet loss was simulated as a Bernoulli distribution with a packet loss probability of 50%. Compared with the standard posterior linearized belief propagation cooperative localization method, the cooperative vehicle localization method based on Gaussian filter belief propagation of this invention can effectively reduce localization errors caused by communication network congestion and packet loss.

Claims

1. A cooperative vehicle localization method based on Gaussian filter belief propagation, characterized in that, include: Step 1: Initialize the initial position values ​​and covariance matrix of all vehicles and anchor points. (1)； Among them, let Indicates a collection of vehicles. Represents the set of anchor points. This represents the set of both vehicles and anchor points, i.e. Both vehicles and anchor points are considered nodes. ; It is the two-dimensional position of node a at the initial time. For the quantity to be estimated The mean, For the quantity to be estimated The covariance matrix; Step 2: Based on the state information of vehicle i at time t-1 , This includes the vehicle's position and speed information. Using the vehicle's motion model, the predicted vehicle position information at time t is calculated. ;in, It is the two-dimensional velocity of vehicle i at time t-1; Step 3: Based on the location information of vehicle i and its neighboring node j Covariance Matrix The distance measurement model is linearized to obtain linearized model parameters; where adjacent node j represents other vehicles and anchor points that communicate with vehicle i. ; Step 4: Calculate the belief messages between vehicles and between vehicles and anchor points using the approximate linearized formula. And perform M iterations of belief propagation calculation; Step 5: After M iterations, calculate the posterior position estimate of the vehicle based on the probability distribution of the vehicle's edge posterior position, and return to step 3 for L iterations. Step 6: After L iterations, calculate the minimum mean square error estimate of the vehicle's posterior position at time t. The convergent solution; Step 2 includes the following process: By utilizing the vehicle's position and speed information during its movement, the instantaneous position of the vehicle and the predicted position of vehicle i at time t can be calculated when satellite signals are lost. The calculation is as follows: (2)； in, The length of the time slot; In step 3, a posterior linearization method is used, employing statistical linear regression for linearization, including: Step 3.1, the measurement model for the distance between the vehicle and the anchor point, and between vehicles, is as follows: (3)； in, It is the Euclidean distance between vehicle i and its adjacent node j. The mean is zero and the variance is... Additive white Gaussian noise; The nonlinear part of the relative distance can be linearized as follows: (4)； in, These are the linearization coefficients. It is a constant term. The mean is zero and the variance is... Additive white Gaussian noise; Step 3.2: Use posterior linearization to linearize the model parameters. The calculation steps are as follows: Step 3.2.

1. Select m sigma-points based on the mean and covariance of the vehicle location distribution. and weight ; Step 3.2.

2. For sigma-points Transform ; Step 3.2.

3. Calculate the following formula: ； (5)； ； in, yes The weighted average, yes The weighted covariance matrix, yes The variance matrix; Step 3.2.

4. Parameters The calculation is as follows: ； (6)； ； Step 4 specifically includes: Step 4.

1. The belief messages between vehicles and between vehicles and anchor points are as follows: (7)； in, It is a relative distance measurement value The likelihood function, Indicates the adjacent nodes of vehicle i; Substituting the approximate linearized formula into the belief message yields: ； (8)； ； in The following formula represents: (9)； Step 4.

2. Considering the possibility of random packet loss during communication, the delay model is as follows: (10)； Where k represents the discrete time series. It is a Bernoulli random variable. ,when The vehicle receives the distance measurement value at the current moment, when The vehicle uses the value from the previous moment because the information reception failed due to packet loss; in calculating the belief message, , The vehicle's state needs to be expanded to include... ,in The initial mean is ,variance ; Step 4.

3. The specific steps of the M-times belief propagation iterative algorithm are as follows: Step 4.3.

1. Let , ,in Let be the expected position of vehicle i at time t. Let i be the position covariance matrix of vehicle i at time t; Step 4.3.

2. Calculation The prior estimate at time t Covariance , The posterior estimate at time t-1 Covariance , and covariance as well as and of The specific formula is as follows: ； ； ； (11) ； ； Step 4.3.

3. Substituting (11) into the following formula will update the mean of the noise. Covariance ; ； ； ； (12)； ； ； in, It is the filtering gain for estimating noise. yes The prior estimate at time t-1, yes The prior estimate of the covariance at time t-1; yes and covariance; Step 4.3.

4. Obtain the predicted... , Substituting (11) into the following formula, we can calculate the mean value of vehicle i's position at time t. Covariance ; ； ； (13)； ； ； in, It is the filter gain for estimating the position. yes and covariance, yes and covariance; Step 4.3.

5. Calculate the belief messages between vehicles and between vehicles and anchor points according to formula (8). After the calculation is completed, return to step 4.3.1 and iterate M times.

2. The cooperative vehicle localization method based on Gaussian filter belief propagation according to claim 1, characterized in that, In step 5, the probability distribution of the posterior edge position of vehicle i is as follows: (14)； in, This indicates that the prior distribution of vehicle i at time t has a mean of 1. The covariance matrix is Gaussian distribution, It is the belief message from adjacent node j of vehicle i to vehicle i at time t.

Images