A Multi-Vehicle Cooperative Localization Method Based on Belief Propagation in Satellite Denied Environment

Jiaqi Wang, Lina Wang

Abstract: The global navigation satellite system (GNSS) is currently being used extensively in the navigation system of vehicles. However, the GNSS signal will be faded or blocked in complex road environments, which will lead to a decrease in positioning accuracy. Owing to the higher-precision synchronization provided in the sixth generation (6G) network, the errors of ranging-based positioning technologies can be effectively reduced. At the same time, the use of terahertz in 6G allows excellent resolution of range and angle, which offers unique opportunities for multi-vehicle cooperative localization in a GNSS denied environment. This paper introduces a multi-vehicle cooperative localization method. In the proposed method, the location estimations of vehicles are derived by utilizing inertial measurement and then corrected by exchanging the beliefs with adjacent vehicles and roadside units. The multi-vehicle cooperative localization problem is represented using a factor graph. An iterative algorithm based on belief propagation is applied to perform the inference over the factor graph. The results demonstrate that our proposed method can offer a considerable capability enhancement on localization accuracy.

Keywords: cooperative localization; belief propagation; factor graph; inertial navigation system;internet of vehicles

1 Introduction

Nowadays, the global navigation satellite system(GNSS) plays an essential role in acquiring the location of vehicles. The GNSS can achieve meter-level or even centimeter-level positioning accuracy through some enhancement technologies [1]. However, when the vehicle is moving in an environment such as a canyon, tunnel, and forest, the positioning accuracy will seriously decrease due to the blocking of satellite signal[2–4]. Inertial navigation system (INS) can provide high positioning accuracy in a short time because its performance is less affected by the surrounding environment. Owing to the autonomy and high sample rate, INS is considered to be integrated with GNSS for higher accuracy vehicle positioning [5–7]. An attenuation factorbased adaptive Kalman filter (KF) was proposed in [8] to fuse GNSS and INS data. In addition, a hybrid strategy autoregression model based on the least-squares support vector machine and KF was constructed to compensate for the positioning error of INS [9], but it could not be used in real-time situations. Different from KF, the unscented Kalman filter can fuse nonlinear measurement sensors, but it has a large computational complexity. To reduce costs, singular value decomposition is used for the generation process of sigma points, and the probability density function (PDF) is re-approximated in [10].GNSS/INS integrated navigation method can provide high positioning accuracy when the satellite signal is lost temporarily. Still, the positioning error will increase rapidly when the satellite signal is lost for more than 15 s [11, 12].

As a result of uncertainty in the surrounding environment, it is difficult to realize high-precision positioning with a single vehicle. Cellular vehicle-to-everything (C-V2X) enables direct communication between vehicles or between vehicles and roadside units (RSUs) [13]. Therefore,enhancing positioning accuracy through multivehicle cooperation has been studied extensively over the past few years [14, 15]. In the cooperative positioning method, the distance between nodes is calculated, and then the coordinates of all nodes are calculated by the distance matrix and the position of anchor points. Hence, a large number of nodes in the cooperative network will obviously increase the time complexity of the algorithm [16]. To reduce the cost of calculation and communication, distributed cooperative localization algorithm was presented to alleviate the uncertainty of location by exchanging information between nodes with belief [17–19]. In [20],a cooperative localization algorithm called BPVMP was proposed. By combining the variational message passing method with belief propagation, the BP-VMP reduces the number of iterations and computational cost of the algorithm in the nonlinear measurement model. Consequently,multi-vehicle cooperative localization technology can effectively improve the problem of vehicle positioning in GNSS denied environments [21,22]. Compared with the 5G network, 6G can achieve higher-precision synchronization, which is beneficial to the performance improvement of time-based positioning technologies. Meanwhile,terahertz used in 6G has excellent time resolution, greatly reducing the impact of non-line of sight on positioning accuracy. Therefore, 6G provides reliable technical support for the multivehicle cooperative localization technology [23, 24].

This paper proposes a multi-vehicle cooperative localization method based on belief propagation. The vehicles can use the C-V2X to transmit absolute location and relative ranging measurements to neighbor vehicles and RSUs. Based on the sum-product algorithm over a wireless network (SPAWN) algorithm [25], inertial measurement unit (IMU) measurement is used to obtain higher precision location estimation of mobile nodes. The factor graph is used to model the cooperative localization network, and then the location estimation of vehicles is obtained by an iterative algorithm based on belief propagation. The effectiveness of the proposed method is demonstrated through a series of simulations.

2 Cooperative Localization Model

It is assumed that each node (including the vehicles and the RSUs) in the cooperative localization network can measure the distance through C-V2X technology and send the sensor measurement and location information to neighbor nodes.Each vehicle is equipped with an IMU to measure acceleration and angular velocity during vehicle movement. Fig. 1 shows the multi-vehicle cooperative localization model.

Fig. 1 Multi-vehicle cooperative localization model

It can be seen from (6) that the optimization function in vehicle cooperative localization network can be decomposed in terms of prior PDF and state transition function and measurement function. The optimization function will be modeled using a factor graph in the next chapter.

The factorized functions of a global function can be represented by a factor graph. The marginal PDF of variables can be obtained by applying the sum-product algorithm [26].

3 Proposed Method

3.1 Multi-Vehicle Cooperative Localization Factor Graph

Fig. 2 Multi-vehicle cooperative localization factor graph

Assuming the noise obeys the Gaussian distribution, the IMU factor is given by

3.2 Iterative Algorithm Based on Belief Propagation

The variables and factors in the factor graph first compute current belief and send it as a message to the connected nodes. When nodes receive a message, they will update their belief according to the message. The message transmitted by the factorf1to the v ariablexmis defined as

An iterative algorithm is proposed to transfer the belief and update the factor graph, and the marginal PDF of the variables in the factor graph is solved to obtain the optimal estimation of the positioning vehicle. The detailed procedure is as follows.

Step 3 Repeat step 2 until the number of iterations that have occurred reachesM, which denotes the maximum iterations number.

Step 4 If the current time slotihave not reached the last time slot, step 6 is expected to be executed, otherwise, step 5 is executed.

Step 5 The time slot reaches the next sampling point, and the message of all nodes received are computed according to

And it is used as the initial belief of the new time slot. Then go back to step 2.

Step 6 At this time, vehicles can estimate their location by taking the MAP estimate of belief corresponding to variables.

4 Simulation and Evaluation

In this section, a series of MATLAB simulations are employed, and a comparison with the SWAWN algorithm and the BP-VMP algorithm is made to verify the effectiveness of the proposed method.

4.1 Simulation Setup

Consider a cooperative localization network with 25 vehicles and 10 RSUs. The nodes are placed in a 100 m×20 m area. The initial location of the nodes is shown in Tab. 1 and Fig. 3. Each vehicle is equipped with an IMU, and nodes within the communication range can use C-V2X to send and receive messages. The vehicles move at the speed between 0 m/s and 5 m/s, and the direction angle is between –0.2 rad and 0.2 rad. The simulation lasts 15 s and the sampling period is 0.5 s. The simulation parameters are set as Tab.2, whereσpandσdare the standard deviation error of vehicles initial location and distance measurement. The nodes are considered neighbors if their distance is less than the maximum communication distance, which is denoted byR.

Tab. 1 Nodes initial coordinates

Fig. 3 Nodes initial position in multi-vehicles cooperative network

Tab. 2 Parameters setting of simulation

4.2 Simulation Results and Analysis

The root mean square error (RMSE) of vehicles localization under the 4 different methods are shown in Fig. 4. The average positioning error of the method in this work is 0.7436 m, which is reduced by 82.3%, 10.8% and 8.5% compared with the other three methods, respectively.According to Fig. 5 and Tab. 3, the proposed method in this paper has a probability of 81%when the positioning error is less than 1 m, while the SPAWN algorithm and the BP-VMP algorithm are 72% and 74%. Meanwhile, the maximum error of the proposed method is 1.6002 m,which is greatly reduced compared with the other three methods. It is clearly seen that the location estimation error of vehicles are effectively mitigated by using the state information of neighbors, and the proposed method further improves the positioning accuracy and stability compared with the SPAWN algorithm and BPVMP algorithm.

Fig. 4 RMSE of vehicles localization under different methods

Fig. 5 CDF of localization errors of vehicles under different methods

Fig. 6 shows the RMSE of vehicles localization after 20 iterations under different methods.Note that the positioning error of cooperative localization decreases with the iteration. With the same time of iterations, the proposed method can provide lower positioning error than the SPAWN algorithm, which is attributed to the motion observation information. To further investigate the impact of the iterations number on the positioning accuracy of the three cooperative localization methods, the maximum iteration timesMis decreased from 10 to 6. As shown in Fig. 7 and Tab. 4, due to the reduction ofM,the average positioning error of the SPAWN algorithm and the BP-VMP algorithm respectively increase by 46.5% and 31.9%, which is hardly increased by applying the method proposed in this paper.

Tab. 3 Error of positioning under four methods

Fig. 6 Variation curve of localization RMSE with iteration times under different methods

Additionally, Fig. 7 indicates that the performance of the SPAWN and BP-VMP are considerable at most time slots, but the positioning accuracy will seriously deteriorate at some time slots. The reason is that the vehicle does not have enough neighbors in a short time and it can only estimate the location by ranging measurement, resulting in divergence of the algorithms.When the nodes involved in the cooperative localization remain constant, the number of neighbors mainly depends on the maximum communication distance. Thus, the positioning capability of the proposed multi-vehicle cooperative localization method in a scenario with insufficient neighbors is examined by decreasingRfrom 20 m to 15 m. The simulation results of the positioning error of vehicles under different methods are shown in Fig. 8.

Fig. 7 RMSE of vehicles localization under different methods(M = 6)

Tab. 4 Localization error under different maximum iteration times

Fig. 8 RMSE of vehicles localization under different methods(R = 15 m)

After reducing the maximum communication distance, the accuracy of the SPAWN is seriously reduced. The average positioning error increases to 3.0824 m. Meanwhile, the accuracy of BP-VMP also decreased and the average error increased to 2.2354 m. In contrast, the proposed method can still estimate the vehicle position through the motion information measured by IMU and has a considerable positioning accuracy.The average positioning error is 0.9136 m, which is 144.7% lower than that of the BP-VMP algorithm.

Furthermore, the positioning accuracy when the initial measurement errorsσpare set to 3 m,5 m, and 10 m respectively is compared in Fig. 9 and Tab. 5.

Fig. 9 CDF of localization errors of vehicles under different initial position observation errors

Tab. 5 Localization error under different initial measurement error

Whenσpis increasing, the message of vehicles received is more unreliable. As observed in Fig. 9, the advantage of using the IMU measurement for cooperative localization is clearly demonstrated when the initial position of the vehicle is uncertain. The positioning error of the SPAWN algorithm will also increase with the error of the initial measurement. Tab. 5 summarizes the average localization error of the three methods when the initial measurement error increases. Whenσpis increased from 3 m to 5 m and 10 m, the errors of the SPAWN algorithm increase by 117.19% and 176.21%. The errors of the BP-VMP increase clearly during this period due to the message transmitted only including the mean state and covariance matrix of the cooperative nodes. As a comparison, the error of the proposed method does not increase significantly, remaining at about 0.9 m. It can be inferred that the proposed method in this paper is less affected by the initial measurement error of vehicles and can be applied to a harsher situation.

5 Conclusion

This paper presents a multi-vehicle cooperative localization method based on belief propagation in a satellite denied environment. The vehicle first estimates the location through IMU measurement and then corrects the location estimate through the exchange of information with neighbor nodes. A cooperative localization factor graph model is established and then uses the iterative message algorithm of belief propagation to obtain the location of the vehicles. The simulation demonstrates that the proposed method outperforms the SPAWN algorithm and BP-VMP algorithm in terms of accuracy and durability.