Advanced Modeling, Estimation, and Control for Electrified VehiclesView this Special Issue
A Cooperative Positioning Method of Connected and Automated Vehicles with Direction-of-Arrival and Relative Distance Fusion
The rapid development of science and technology has created favorable conditions for Connected and Automated Vehicles (CAVs). Accurate localization is one of the fundamental functions of CAV to realize some advanced operations such as vehicle platooning. However, complicated urban traffic environments, such as the flyover, significantly influence vehicular positioning accuracy. The inability of CAV to accurately perceive self-localization information has become an urgent issue to be addressed. This paper proposed a novel cooperative localization method by introducing the relative Direction-of-Arrival (DOA) and Relative Distance (RD) into CAV to improve the localization accuracy of CAV in the multivehicle environment. First, the three-dimensional positioning error model of the host vehicle concerning adjacent vehicles in azimuth angle and pitch angle and intervehicle distances under the vehicle-to-vehicle communication was established. Second, two least-squares estimation algorithms, linear and nonlinear, are established to decrease the position errors by combining relative DOA and RD measurement information. To verify the proposed algorithm's effect, the PreScan-Simulink joint simulation is carried out. The results show that the host vehicle's localization accuracy by the proposed method can be improved by 25% compared with direct linearization. Besides, by combining relative DOA and relative RD measurement, the locating capability of the least-square-based nonlinear optimization method can be enhanced by 22%.
Connected and automated vehicles (CAV) are promising methods worldwide to improve traffic safety, enhance driving comfort, and reduce energy consumption [1–3]. As an essential CAV function, accurate localization technology is all-important for CAV to realize high-grade performances such as vehicle platooning  and cooperative merging on highway ramps [5, 6] and has been widely studied. The most common localization technique used in CAV is realized by the global navigation satellite system (GNSS). However, the urban forest in downtowns, such as dense buildings, usually weakens the signal quality of GNSS and reduces the positioning precision of vehicles [7–9]. Besides, when cars drive as a group, the small intervehicle distance demands that the vehicular positioning accuracy is centimeter-level , which is much higher than the current GNSS road-positioning accuracy level.
Various techniques have been proposed to improve vehicular positioning accuracy further to meet multivehicle cooperative control requirements [11–15]. Differential GNSS is proposed in  to achieve higher positioning accuracy by eliminating the common biases through a network of fixed reference stations. The real-time kinematic (RTK) technique is employed based on carrier phase measurements in  to realize centimeter-level accuracy. Moreover, higher vehicle positioning accuracy is achieved using the inertial navigation system (INS) in [18, 19]. Nonetheless, the methods mentioned above depend on the expensive infrastructures or onboard devices. Some scholars recently studied cooperative localization-based multi-UAVs’ technical possibility to simultaneously reduce costs and achieve lane-level positioning accuracy [20–22].
The cooperative localization usually requires two additional pieces of information to enhance the positioning accuracy, i.e., Direction-of-Arrival (DOA) and Relative Distance (RD) between vehicles. Tomic et al.  developed a suboptimal estimator to calculate the location of multiple targets in a three-dimensional wireless sensor network by using DOA measurement, whose model is linearized. Wang et al. presented a DOA estimation method based on sparse Bayesian learning (SBL) to improve vehicle localization accuracy . Another study proposed by Yin et al. measured the RD between the base station and target vehicle by combining Time Difference of Arrival (TDOA) and Angle of Arrival (AOA) . Localization errors are reduced by solving the mean square error matrix.
Also, some researchers attempt to combine the DOA and RD information by several fusion algorithms to improve the positioning accuracy. Rohani et al.  adopt the particle-filter probability-statistics method to conduct information fusion for the distance between the front and rear vehicles to increase relative position precision. Song et al.  develop a cooperative localization method based on the Bayesian framework, which uses GPS, distances, and azimuth between vehicles. The effects of vehicle speed, acceleration, and variable spacing on vehicle localization accuracy are analyzed. Naseri et al.  propose a Message Passing Hybrid Localization (MPHL) algorithm, a distributed algorithm based on information propagation and Markov chain Monte Carlo sampling. Joint estimation of DOA and RD is conducted to solve the cooperative distributed localization problem. Additionally, Yin et al.  propose a cooperative multisensor Edge Cloud Cooperative Localization (ECCL) method, which has relative distance and relative angle observations from neighbor nodes and absolute coordinate positioning systems (such as GPS) to obtain relevant information. A centralized cooperative fusion unit is built in the cloud to fuse the multiple localization information.
Although the information of DOA, RD, or DOA-RD is fused by filter algorithms in [23–29], the filter algorithms may not meet real-time localization performance because of the computational complexity. This paper investigates the fusion of DOA-RD information without filter algorithms to achieve the cooperative localization of CAVs in the flyover environment. The localization framework based on least-squares DOA-RD information fusion is also established. The main contributions of this article are threefold. First, host vehicles' three-dimensional positioning error model concerning adjacent vehicles in azimuth angle and pitch angle and intervehicle distances was established. Second, to eliminate the position errors, a least-square-based linear localization method is designed by combining relative DOA and RD measurement information. Then, in light of the positioning results obtained by the linear localization method, a nonlinear calculation method fusing DOA- and RD-related communication between vehicles is proposed to optimize cars' positioning accuracy further. The presented cooperative localization method based on connected and automated vehicles on the localization accuracy is finally analyzed.
The remainder of this paper is organized as follows: Section 2 introduces the system model and the framework of cooperative localization. In Section 3, the calculation process of the cooperative localization optimization algorithm is presented. Section 4 gives the simulation and results. Section 5 concludes this paper.
2. System Model and Cooperative Localization
This paper will focus on the cooperative localization of multiple vehicles by using the DOA and RD information. This section will introduce the modeling of cooperative localization and its fundamental principle.
2.1. System Model
The localization in the multistack interchange is challenging due to the intricate positions of the vehicles. This paper will focus on this scenario. Figure 1 shows a typical multistack interaction, where the cars drive on different levels.
Figure 2 is the simplified model of the cooperative localization for vehicles. The position of the target vehicle is , and the position of the assistance vehicle is = . Note that only the vehicle with onboard sensors to measure the DOA and RD could be the assistance vehicle. Each car could obtain the basic localization information using the GNSS. The relative DOA between vehicles are azimuth angles and pitch angles , while the relative RD between vehicles is .
There are CAVs communicating with the target vehicle to measure the DOA and RD. The DOA and RD between target and assistance vehicles arewhere αi, , and are the azimuth, pitch, and intervehicle distance between the target and assistance vehicles. Besides, the azimuth angle, pitch angle, and distance between vehicles can be expressed as . Since sensor noise exists, the system model is depicted aswhere is the measurement value of sensors with noise and , , and are the measurement noise of azimuth angle, pitch angle, and intervehicle distance, respectively. They are Gaussian white noises and uncorrelated.
2.2. Cooperative Localization Principle
The main idea of cooperative localization is using the measured azimuth, pitch angle, and distances between vehicles to improve the positioning accuracy of all cars in the multivehicle system. Assuming that the DOA and RD errors are zero, we will derive the following vehicle positions. Since there exist errors by using GNSS for each vehicle, the position can be expressed aswhere , , and represent the localization noise of GNSS, which are Gaussian white noises.
According to the distance between the CAVs, the location of the assistance vehicle can be calculated by the target vehicle , i.e.,where , , and are the distances in three directions. Since the measured values are independent of each other, the measured location of vehicle is
If the locations are measured by times, the position of the target CAV can be obtained as
At the same time, the variances of the least-squares method are
The positioning accuracy of the target vehicle is improved with the increase in the cooperation vehicle number.
3. Cooperative Localization Algorithm Design
This section will introduce the linear and nonlinear least-squares optimization methods to improve the positioning accuracy by fusing the DOA and RD information between vehicles.
3.1. Vehicle Initial Position
The initial position of assistance vehicle is provided by the GNSS, i.e., . According to the three-point positioning principle, the position of the target CAV, , can be calculated by using the positions of two known CAVs and the DOA information between them. Then, if two assistance vehicles are selected, the position of the target CAV is calculated by
Therefore, the positions of all vehicles are
3.2. Linear Least-Squares Optimization
Assuming that there are CAVs in a flyover scene, the corresponding coordinate positions in three directions can be set as , , and , which can be represented as
The mean values of , , and in the three-dimensional coordinates are zero while satisfying the uncorrelated white Gaussian noise.
According to system model (1), it is necessary to perform a first-order Taylor expansion at the initial vehicle value (the error of the second order and above is small and can be ignored). The linearization solution is as follows.
Azimuth linearization iswhere
Linearization of the pitch angle iswhere
Linearization of vehicle distance can be written as
The positions of vehicles can be solved as
The relationship of the observation equation iswhere
According to Taylor expansion coefficients and constant terms, least-squares equation (18) is solved as
The high-precision position coordinates of CAVs can be solved as
The position of the target CAV can be solved by .
3.3. Nonlinear Least-Squares Optimization
The nonlinear optimization algorithm mainly adopts the minimum residual sum of squares (RSS). Let be the functional relationship from the position to the observation . We can obtain the following equation:where is the measured value with noise and is Gaussian white noise. Assuming that the measurement error of each sensor is zero mean and Gaussian white noise is not correlated with each other, the positioning error formula iswhere and is the Jacobian matrix as follows:
Among them, according to , when the value of tends to 0, the residual reaches a minimum. When achieves the minimum value, the optimal state quantity is obtained, which can be calculated by using the Gauss–Newton iteration method:where is the number of iterations.
According to the estimation of the least-squares method, the root mean square error (RMSE) matrix is obtained:where , is the variance error matrix, and where , , and are the standard deviation of the error for the azimuth angle, pitch angle, and distance between vehicles. The azimuth angle obtains the initial value of the target CAV , which is treated as the starting point of the least-squares iteration. By the Newton–Raphson method, is continuously iterated until reaches the minimum value. The target localization can be obtained as the optimal solution.
4. Simulation Results and Discussion
4.1. Simulation Method
We use PreScan-Simulink joint simulation software to perform Monte Carlo simulations on the flyover driving scene and analyze the localization accuracy according to the RMSE. The localization accuracy in the direction is defined as follows:
4.2. Simulation Conditions
We set up a flyover scenario to conduct the simulation analysis. The initial positions, directions, and speeds of multi-CAVs have been given. The accuracy of the onboard sensors is shown in Table 1.
4.3. Simulation Results
We can only solve the target CAV's localization when using the relative direction information to obtain the RMSE of the target vehicle. The simulation results show that the single relative direction information cannot improve each connected vehicle's localization accuracy. The RMSE result of the target CAV is shown in Figure 3, and the RMSE statistics of the target CAV are shown in Table 2.
In single relative direction information, the RMSE after linearization is solved by the least-squares method. The RMSE obtained by the linear optimization solution method is shown in Figure 4, and the RMSE statistics are shown in Table 3.
When the target CAV's initial position is obtained by single relative direction information, the RMSE of the target CAV is obtained as shown in Figure 5 through combining the least-squares method with the nonlinear optimization method, and the RMSE statistics are shown in Table 4.
When the target CAV's initial position is obtained by relative-direction finding and ranging, the nonlinear optimization algorithm solves the RMSE of the target CAV. The simulation results are shown in Figure 6, and the RMSE statistics are shown in Table 5.
4.4. Analysis of Factors Affecting Localization Accuracy
Vehicle speed is one of the most important factors for road safety. Therefore, the reasonable target speeds of CAVs and the distance between the assistance CAVs are necessary to guarantee the flyover scenario's driving safety. The localization accuracies of the target CAV under different conditions are simulated and analyzed by using the linear least-squares fusion algorithm. The simulation results are shown in Figures 7–9. In Figure 7, two assistance CAVs are assumed driving on the road with constant speeds of 30 km/h and the distance between them is 100 m. The velocity of target CAV is changed between 10 and 80 km/h. In Figure 8, the target CAV and the assistance CAVs are hypothesized to run along different roads and the relative speeds of assistance vehicles vary from 10 km/h to 80 km/h. The previous figure changes the distance between the assistance CAVs from 20 m to 200 m. Note that the cooperative positioning method proposed in this paper is based on the azimuth and the distance between the assistance CAVs. Therefore, similar to the spatial three-point positioning principle, at least two assistance CAVs are needed to complete the cooperative positioning of the host vehicle in the application.
From Figures 7 and 8, we can see that both the host vehicle speed and the relative speeds of assistance vehicles can affect the position accuracy of the host vehicle and the location accuracy is weakened with augments of these two factors. By observing Figure 9, one can get that the positioning error of the host vehicle decreases first and then increases and reaches the lowest point between 80 km/h and 120 km/h. When the distance between two assistance CAVs is 20 m, a significant positioning error occurs because the GNSS system works. However, with the functioning of the cooperative location method, the positioning error is gradually reduced and the positioning error is less than 5 m at between 80 km/h and 120 km/h. As the distance between the two cars grows, the localization accuracy of the target CAV decreases, possibly due to the phenomena of communication delay and data dropout in vehicle-to-vehicle wireless communication networks. Therefore, the changing trend of target vehicle positioning RMSE is finally shown in Figure 9.
4.5. Result Analysis
By Figures 3–6, the average value of vehicle localization errors is calculated and a method to reduce the error percentage is proposed. When the self-localization accuracy of CAVs given by the simulation is 10 m, the result shows the following:(1)When only relative DOA is employed to obtain the localization of the target CAV, the low-accuracy position values of CAVs are directly used for calculation. Due to the low accuracy of the comparative DOA method, the positioning error of the target CAV is about 9–12 m, unable to improve the localization accuracy of the CAVs.(2)The least-squares method linearization optimization for solving the localization of the target CAV can be concluded that the target CAV's localization error is about 5–7 m, which improves the target's localization accuracy of CAV.(3)Using the least-squares linearization optimization solution method combining relative DOA and relative RD measurement, the target CAV's localization value is about 4–5 m. Compared with direct linearization, the accuracy is improved by 25%.(4)Using the nonlinear optimization solution method of the least-squares method combining relative DOA and relative RD measurement, the target CAV's localization value is about 3–4 m. Compared with the linear optimization solution method, the improvement of the least-squares method nonlinear optimization solution method is about 22%.(5)The localization accuracy of the target CAV among the multi-CAVs decreases and increases as the distance of the assistance multi-CAVs increases. When the assistance CAV space is 100–120 m, the localization error reaches the minimum.
This paper studies the localization accuracy of the CAV in the flyover scenario when the accurate localization information of the vehicle is hard to obtain. The fusion algorithm that assists the relative DOA and relative RD measurement of multi-CAVs for cooperative localization is also researched. Using nonlinear optimization estimation and linearized least-squares estimation algorithm, the simulation results show that the proposed method improves the localization accuracy of the target CAV in road scenes such as flyover without increasing infrastructure costs. It can also conclude that reducing the CAV speed in the multivehicle cooperative of the CAV can increase the localization accuracy of the vehicle and enable driving. Reducing the distance between the assistance CAVs can increase the localization accuracy of the target CAV. This method is used for the cooperative localization of multivehicles in the flyover road scene. It can also be used for multi-aircraft combined attacks or target tracking in aerospace.
No data were used to support this study.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
This work was supported by the National Natural Science Foundation of China under Grants 51975118, 52025121, and 52002066 and in part by the Key R&D Program of Jiangsu Province under Grant BE2019004.
F. Wang, W. Zhuang, G. Yin, S. Liu, Y. Liu, and H. Dong, “Robust inter-vehicle distance measurement using cooperative vehicle localization,” Sensors, vol. 21, no. 6, 2021.View at: Google Scholar
N. Sun, M. G. Yan, and J. Ni, “A study on multi-vehicle cooperative positioning based on GRI,” Automotive Engineering, vol. 40, no. 4, pp. 117–122+128, 2018.View at: Google Scholar
B. Nan, T. Yuan, L. Ye, Z. Yuan, Z. Xiao, and J. Zhou, “A high-precision and low-cost IMU-based indoor pedestrian positioning technique,” IEEE Sensors Journal, vol. 20, no. 12, pp. 6716–6726, 2020.View at: Google Scholar
K. X. Guo, X. X. Li, and L. H. Xie, “Ultra-Wideband and Odometry-Based Cooperative Relative Localization with Application to Multi-Uav Formation Control,” IEEE Transactions Cybernetics, vol. 50, no. 6, pp. 2590–2603, 2019.View at: Google Scholar
M. Rohani, D. Gingras, V. Vigneron, and D. Gruyer, “A new decentralized bayesian approach for cooperative vehicle localization based on fusion of GPS and VANET based inter-vehicle distance measurement,” IEEE Intelligent Transportation Systems Magazine, vol. 7, no. 2, pp. 85–95, 2015.View at: Publisher Site | Google Scholar