^{1}

^{*}

^{2}

^{1}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

This paper studies the problem of multiple vehicle cooperative localization with spatial registration in the formulation of the probability hypothesis density (PHD) filter. Assuming vehicles are equipped with proprioceptive and exteroceptive sensors (with biases) to cooperatively localize positions, a simultaneous solution for joint spatial registration and state estimation is proposed. For this, we rely on the sequential Monte Carlo implementation of the PHD filtering. Compared to other methods, the concept of multiple vehicle cooperative localization with spatial registration is first proposed under Random Finite Set Theory. In addition, the proposed solution also addresses the challenges for multiple vehicle cooperative localization, e.g., the communication bandwidth issue and data association uncertainty. The simulation result demonstrates its reliability and feasibility in large-scale environments.

Accurate vehicle localization is the current trend in the field of intelligent vehicles for the purpose of autonomous driving. Single vehicle localization is often performed by fusing both proprioceptive and exteroceptive sensors presented in [

The communication bandwidth issue.

The communication bandwidth issue is introduced by Nerurkar [

The uncertainty issue.

Data association plays an important role in the cooperative localization introduced by Franchi

Based on the above issues, we proposed the PHD filter to consider the whole group behavior instead of each vehicle [

This paper enhances the previous work by joint spatial registration and states estimation simultaneously. Spatial registration is considered as follows: once the measurement is acquired form the sensor, two kinds of errors are also included. One belongs to the random noises described as Gaussian white noise, while the other is called the bias (systematic error), which is considered as a fixed value originated from the sensor calibration. The purpose of the spatial registration is to estimate the related bias during the whole process. To the best of the authors' knowledge, the spatial misregistration problem has not been considered in multiple vehicle cooperative localization under Random Finite Set Theory. The multiple vehicle states and the biases of the sensors are jointly estimated recursively via the PHD filter. The sequential Monte Carlo (SMC) method is used to implement the approach, considering non-linear and non-Gaussian conditions [

In this paper, we adopt the same framework as proposed previously: the measurements from both proprioceptive and exteroceptive sensors are projected to a global plane that constitutes of the observation set. The PHD filter recursively estimates the dynamic states and spatial biases according to the observations.

The contributions of the proposed approach are as follows:

We are among the first to consider multiple vehicle cooperative localization with unknown biases under Random Finite Set Theory. The proposed PHD filter can estimate and compensate for the measurement biases accurately, which performs better localizations. In addition, by utilizing the PHD filter, the challenges for multiple vehicle cooperative localization are also overcome [

The rest of this paper is organized as follows: Section 2 briefly describes multiple vehicle cooperative localization with measurement misregistration. Section 3 introduces the mathematic background of the PHD and its implementation. Section 4 presents simulation results. Finally, the paper is concluded in Section 5.

As illustrated in

Each vehicle is able to localize itself. Here, we assume that the corresponding measurements are in the 2D global coordinate.

Each vehicle is able to measure the relative position of the other vehicles. Here, we assume that the measurements are formatted with the range and bearing in a local coordinate. Furthermore, each measurement error is formed of white noises and fixed biases.

Vehicles are equipped with communication transceivers for information exchanging.

The communication network does not have the identification capability regarding the data association issue. Each vehicle observes the whole environment and transmits its observations over the network. There is no prior information regarding the data association issue provided by the inter-vehicle communication.

Multiple vehicle cooperative localization improves the precision of the localization. Assuming the measurements' noises for proprioceptive sensors are larger than a certain threshold, the localization may be imprecise when vehicles are aggregated in a small region. However, the precision is ensured with the help of the exteroceptive sensors. Furthermore, vehicles can localize themselves under the condition that only one proprioceptive sensor is available, with the help of cooperative localization.

Much work has been done for cooperative localization by using a centralized extended Kalman filter [

Instead of a centralized architecture, a decentralized solution is investigated, where multiple fusion centers exist and each of them handles only local information (only the observed neighbors). However, the computational demand is very high. In addition, it often leads to the over-convergence problem when handling inter-estimate correlation among various sources. Since the over-convergence problem is caused by inter-estimation correlation, a natural idea for addressing this problem is investigated controlling of the data flow within the vehicle network. Howard [

Neither centralized approaches nor decentralized approaches consider the spatial registration issue during the localization process. As we can see from

In this paper, we adopt the sequential Monte Carlo PHD filter implementation to jointly estimate the biases and the states simultaneously. The proposed SMC PHD filter handles spatial registration well under multiple vehicle cooperative scenarios during the whole process.

The PHD filter based on Random Finite Random Finite Set Theory is proposed because of its superior performance in the multiple targets tracking domain.

The random finite set is a hidden Markov chain model with set-valued state and set-valued observation, while the PHD filter is a predicted and corrected framework for recursive Bayesian filtering in such an RFS formulation. A comparison of the RFS approach and traditional multiple-target tracking methods has been given in [

Regarding the superior performance, the PHD filter has been used for various scenarios, e.g., SLAM [

This paper extends the earlier work for multiple vehicle cooperative localization [

The targets in a multi-target scenario at time _{k}_{,1},…, X_{kN}_{(}_{k}_{)}, which takes values from the state space, ^{n}^{x}.

_{k}_{|}_{k}_{− 1}(x_{k}_{k}_{−1}).

Similarly, the observations are represented as a finite set of vectors. Suppose ^{n}^{z}.

_{k}_{k}

Let
_{k}_{k}_{k}_{k}_{k}_{1:}_{k}_{1:}_{k}_{k}_{k}_{k}

Using these random finite set models, it is possible to construct process and observation models analogous to the single-target case. Randomness in _{k}_{k}_{k}

Under the above models, the multi-target Bayes filter propagates the posterior multi-target density, _{k}_{1:}_{k}_{k}_{k}

The PHD recursion is given by:
_{D,k}_{S,k}_{k}_{k}

Analogous to the standard PHD filter, the spawn process of the augmented state is modeled as Poisson RFS with intensity _{k}_{k}_{−1}). Assuming the state, _{k}_{k}_{k}

Since the sensor biases are considered as non-random parameters, the following equations are therefore acquired:

Assume the biases are independent; the PHD update step is approximated as:
_{k}

It is noted that the PHD recursion has multiple integrals that have no closed form solutions in general. Therefore, the sequential Monte Carlo method is implemented. The MCMC (Markov Chain Monte Carlo) move step is provided for increasing the particle variety after the re-sample step, without affecting the validity of the approximation [

For the process model, the state _{k}_{x,k}_{y,k}_{x,k}_{y,k}^{T}_{x,k}_{y,k}_{x,k}_{y,k}_{k}_{k}_{k}_{k}_{n}_{n}

For the measurement model, measurements are originated from both proprioceptive and exteroceptive sensors projecting to the ground plane. To map the state to the observation space, the observation matrix is _{k}_{2}, _{2}], while the measurement vector is _{x}_{y}^{T}

The measurement errors of the exteroceptive sensors are comprised of the biases

Here, we only consider the measurements from both proprioceptive and exteroceptive sensors as the set-valued observation. For instance, we measure the vehicles' relative positions according to the exteroceptive sensors. Measurements are then acquired by coordinate transformation to the global plane. Assuming there are

The simulation was implemented on the ground plane over the surveillance region [−6, 000, 6, 000] × [−6,000,6,000] m^{2} for a period of 200 s. In fact, with the number of vehicles increasing, the final results are affected, due to the uncertainty issues. In this paper, only four vehicles are implemented to illustrate the potential of the PHD filter in the field of cooperative localization. In simulation, the biases of the exteroceptive sensors are considered as ^{1} = [−25 m, 75 mrad]^{T}^{2} = [55 m, −60 mrad]^{T}^{3} = [−40 m, 25 mrad]^{T}^{4} = [35 m, −45 mrad]^{T}_{GPS}^{2},15 m^{2}], while the exteroceptive sensor is with R_{Radar}^{2}, 5 mrad^{2}]. The inter-vehicle communication is also available to exchange the information on the network. In addition, the V2V communication system does not have the ability to identify the others during the whole process. For the purpose of comparison, the simulation is also compared with the standard PHD filter, which does not consider the spatial registration [

In

It can be seen that the estimation derived by the standard PHD filter deviates from the truth, due to the the effect of the sensor bias. However, after considering the spatial registration, the estimations are more close to the true trajectories.

Furthermore, the circular position error probability (CPEP) is also utilized to evaluate the performance of both methods. Given the true and estimated state sets
_{k}_{k}_{|}_{k}_{k}_{k}

In conclusion, combined with our previous work [

First, it reduces and bounds the requirements of the communication bandwidth in a multiple vehicle environment. The inter-vehicle communication system transmits the measurements to the PHD filter, which takes little bandwidth. Compared to other methods, the proposed approach has the lowest consumption requirements for the communication bandwidth, since each vehicle only transmits its observations.

Second, it works under extreme conditions, which often happen in real environments (where the association uncertainty exists, the number of the vehicles is unknown, sensor delays and communication unavailability occur and the measurement is distracted by noise). The PHD filter not only illustrates the high performance of the localization, but also exhibits the robustness under the dynamic structure of the group.

Third, the spatial registration issue is addressed to jointly estimate the states and the corresponding biases during the whole process. Compared to others, the concept of multiple vehicle cooperative localization with spatial registration is first proposed under Random Finite Set Theory.

Because of the superiority mentioned above, we have strong reason to believe that the PHD filter has a great potential in the field of cooperative localization.

In this paper, a recursive Bayesian solution for multiple vehicle cooperative localization with spatial registration is first proposed. The sensors' biases and the vehicles' states are estimated simultaneously based on the PHD filter. In comparison to related work, the whole group of vehicles and the sensors' biases are viewed as a single set-valued state, the measurements are collected as a single set-valued observation, which is used to update the behavior of the set-valued state. The proposed approach also overcomes the challenges existing in multiple vehicle cooperative localization, e.g., low communication bandwidth and data association uncertainty. Experimental results exhibit the high performance of the joint spatial registration PHD filter.

Future work will focus on the evaluation of the proposed approach in a non-synthetic environment.

This work has been supported by the China Scholarship Council, the German Research Foundation (DFG) and the Technische Universität München within the funding programme Open Access Publishing.

The authors declare no conflicts of interest.

Multiple vehicle cooperative localization system.

Measurements from the exteroceptive sensor.

Set-valued states and set-valued observations.

Estimated positions by both filters.

The circular position error probability (CPEP) against time.

The estimated biases of the vehicles.

The estimated number of vehicles. PHD, probability hypothesis density.