A Cooperative Positioning Method of Connected and Automated Vehicles with Direction-of-Arrival and Relative Distance Fusion

,e 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. ,e inability of CAV to accurately perceive self-localization information has become an urgent issue to be addressed. ,is 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. ,e 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%.


Introduction
Connected and automated vehicles (CAV) are promising methods worldwide to improve traffic safety, enhance driving comfort, and reduce energy consumption [1][2][3]. As an essential CAV function, accurate localization technology is all-important for CAV to realize high-grade performances such as vehicle platooning [4] and cooperative merging on highway ramps [5,6] and has been widely studied. e 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][8][9]. Besides, when cars drive as a group, the small intervehicle distance demands that the vehicular positioning accuracy is centimeter-level [10], 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][12][13][14][15]. Differential GNSS is proposed in [16] to achieve higher positioning accuracy by eliminating the common biases through a network of fixed reference stations. e real-time kinematic (RTK) technique is employed based on carrier phase measurements in [17] 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][21][22]. e 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. [23] 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 [24]. 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) [25]. 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. [26] adopt the particlefilter probability-statistics method to conduct information fusion for the distance between the front and rear vehicles to increase relative position precision. Song et al. [27] develop a cooperative localization method based on the Bayesian framework, which uses GPS, distances, and azimuth between vehicles. e effects of vehicle speed, acceleration, and variable spacing on vehicle localization accuracy are analyzed. Naseri et al. [28] 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. [29] 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][24][25][26][27][28][29], the filter algorithms may not meet real-time localization performance because of the computational complexity. is paper investigates the fusion of DOA-RD information without filter algorithms to achieve the cooperative localization of CAVs in the flyover environment. e localization framework based on leastsquares DOA-RD information fusion is also established. e 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.
en, 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. e presented cooperative localization method based on connected and automated vehicles on the localization accuracy is finally analyzed. e 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.

System Model and Cooperative Localization
is paper will focus on the cooperative localization of multiple vehicles by using the DOA and RD information.
is section will introduce the modeling of cooperative localization and its fundamental principle.

System Model.
e localization in the multistack interchange is challenging due to the intricate positions of the vehicles.
is 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 n vehicles. e position of the target 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. e relative DOA between vehicles are azimuth angles α 1 , α 2 , α 3 , . . . , α i and pitch angles β 1 , β 2 , β 3 , . . . , β i , while the relative RD between vehicles is D 12 , D 13 , D 23 , . . . , D ij . ere are n CAVs communicating with the target vehicle to measure the DOA and RD. e DOA and RD between target and assistance vehicles are where α i , β i , and D ij 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 W � [α i , β i , D ij ] T . Since sensor noise exists, the system model is depicted as are the measurement noise of azimuth angle, pitch angle, and intervehicle distance, respectively. ey are Gaussian white noises and uncorrelated.

Cooperative Localization Principle.
e 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 as where ε v x i , ε v y i , and ε v z i represent the localization noise of GNSS, which are Gaussian white noises.
According to the distance between the CAVs, the location of the assistance vehicle i can be calculated by the where Δx i , Δy i , and Δz i are the distances in three directions. Since the measured values are independent of each other, the If the locations are measured by n times, the position of the target CAV can be obtained as At the same time, the variances of the least-squares method are Var Host CAV

Assistance CAV
Assistance CAV Assistance CAV Assistance CAV

Cooperative Localization Algorithm Design
is section will introduce the linear and nonlinear leastsquares optimization methods to improve the positioning accuracy by fusing the DOA and RD information between vehicles.

Vehicle Initial Position.
e initial position of assistance vehicle is provided by the GNSS, i.e., V n0 � [v x 10 , v y 10 , v z 10 , . . . , v x n0 , v y n0 , v z n0 ] T . According to the three-point positioning principle, the position of the target CAV, v t0 � [v x t0 , v y t0 , v z t0 ] T , can be calculated by using the positions of two known CAVs and the DOA information between them. en, if two assistance vehicles are selected, the position of the target CAV is calculated by erefore, the positions of all vehicles are

Linear Least-Squares Optimization.
Assuming that there are n CAVs in a flyover scene, the corresponding coordinate positions in three directions can be set as x → , y → , and z → , which can be represented as in the three-dimensional coordinates are zero while satisfying the uncorrelated white Gaussian noise.
According to equations (2) and (10), we can define where ω � [α i , β i , D ij , x, y, z] T represents the actual measured value with noise.
According to system model (1), it is necessary to perform a first-order Taylor expansion at the initial vehicle value V 0 (the error of the second order and above is small and can be ignored). e linearization solution is as follows.
Azimuth linearization is where Linearization of the pitch angle is where Linearization of vehicle distance can be written as e positions of vehicles can be solved as e relationship of the observation equation According to Taylor expansion coefficients and constant terms, least-squares equation (18) is solved as e high-precision position coordinates of CAVs can be solved as e position of the target CAV can be solved by

Nonlinear Least-Squares Optimization.
e nonlinear optimization algorithm mainly adopts the minimum residual sum of squares (RSS). Let h(V t ) be the functional relationship from the position V t to the observation W. We can obtain the following equation: where W is the measured value with noise and � 1, 2, . . . , n) 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 is

Mathematical Problems in Engineering
where dv t � [dv x t , dv y t , dv z t ] T and H � zh/zv t is the Jacobian matrix as follows: Also, Among them, according to W � hv x t + δ, when the value of W − hv x t tends to 0, the residual δ reaches a minimum. When W − hv 2 x t achieves the minimum value, the optimal state quantity is obtained, which can be calculated by using the Gauss-Newton iteration method: where j is the number of iterations. According to the estimation of the least-squares method, the root mean square error (RMSE) matrix is obtained: where E[dv x t dv T x t ], Q is the variance error matrix, � 1, 2, . . . , n)， where σε α i , σε β i , and σε D ij are the standard deviation of the error for the azimuth angle, pitch angle, and distance between vehicles. e azimuth angle obtains the initial value of the target CAV V 0 � [v x 10 , v y 10 , v z 10 ] T , which is treated as the starting point of the least-squares iteration. By the Newton-Raphson method, Δv x t is continuously iterated until ‖W − hV x t ‖ 2 reaches the minimum value. e target localization can be obtained as the optimal solution.

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. e localization accuracy in the x − y − z direction is defined as follows:

Simulation Conditions.
We set up a flyover scenario to conduct the simulation analysis. e initial positions, directions, and speeds of multi-CAVs have been given. e accuracy of the onboard sensors is shown in Table 1.

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. e simulation results show that the single relative direction information cannot improve each connected vehicle's localization accuracy. e 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. e 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. e simulation results are shown in Figure 6, and the RMSE statistics are shown in Table 5.

Analysis of Factors Affecting Localization Accuracy.
Vehicle speed is one of the most important factors for road safety. erefore, the reasonable target speeds of CAVs and the distance between the assistance CAVs are necessary to guarantee the flyover scenario's driving safety. e localization accuracies of the target CAV under different conditions are simulated and analyzed by using the linear least-squares fusion algorithm. e 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. e       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. e 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.
erefore, 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. erefore, the changing trend of target vehicle positioning RMSE is finally shown in Figure 9. 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 selflocalization accuracy of CAVs given by the simulation is 10 m, the result shows the following:

Result Analysis.
(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) e least-squares method linearization optimization for solving the localization of the target CAV can be   x-axis y-axis z-axis    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) e 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.

Conclusion
is paper studies the localization accuracy of the CAV in the flyover scenario when the accurate localization information of the vehicle is hard to obtain. e 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. is 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.

Data Availability
No data were used to support this study.

Conflicts of Interest
e authors declare that there are no conflicts of interest regarding the publication of this paper.