Keywords

1 Introduction

Global Navigation Satellite Systems (GNSSs) have widespread application in Connected Autonomous Vehicle (CAV) scenarios. However, conventional GNSS systems suffer from multipath propagation in urban environments, leading to high positioning errors [8] that can not tolerated in safety-related vehicular services. A promising solution to overcome this problem is represented by collaborative sensing systems that leverage the presence of multiple actors in the same driving area to enhance positioning accuracy thanks to the coherent fusion of localization measurements acquired by nearby vehicles, as done in Implicit Cooperative Positioning (ICP) [2].

This paper introduces a novel method for cooperative LiDAR-based vehicular self-positioning that leverages data sharing among vehicles to enhance localization accuracy. The innovative aspect of this method lies in its ability to address and mitigate key challenges such as false detections, object-measurement association, and measurement-measurement association which must be handled for real-world implementations. By integrating LiDAR data from various vehicles, the method ensures a more reliable and accurate vehicle positioning. Validation was conducted using a real-world dataset where multiple vehicles approached a zebra crossing on a two-way urban road featuring stationary road elements at the crossing. The results demonstrate the method’s effectiveness in improving positional accuracy and robustness in complex urban environments, underscoring its potential for practical applications in autonomous driving and advanced driver-assistance systems.

The rest of the paper is organized as follows. In Sect. 2, the cooperative sensing-based self-positioning solution is described. Section 3 presents the details of the experiment and provides a quantitative assessment, followed by concluding remarks in Sect. 4.

2 Cooperative Self-positioning

The proposed method fundamentals on implicit cooperative sensing and positioning. Vehicles equipped with LiDAR sensors generate point clouds. These point clouds encompass useful information for autonomous driving such as the location of road elements. In this end-to-end method, the object detector proposed in Sect. 2.1 clusters separable 3D points in the body reference system and identifies them as objects. Among these detected objects, there exist noise-derived detections. Besides, the other two onboard units are actively engaged, namely GNSS and Inertial Measurement Unit (IMU). Here, the GNSS positioning is enhanced with a Satellite-Based Augmentation System (SBAS), if available, which requires Line-Of-Sight (LOS) conditions with at least two geosatellites. Otherwise, Single-Point Positioning (SPP) is considered.

Additionally, vehicular heading is required for LiDAR-GNSS calibration. Some approaches used for the calculation of vehicular heading are visual odometry, dual-antenna GNSS localization, and fusion of GNSS and IMU [4]. Simply, we used single-antenna GNSS fixes to calculate vehicle heading and then calibrated the rotational component of LiDAR and GNSS reference frames.

While the proposed method can be implemented in a centralized or decentralized way, this paper focuses on centralized cooperation. In turn, the following data is exchanged with Road Side Unit (RSU) via Vehicle-to-Infrastructure (V2I) communications: detected object locations in the local framework and GNSS position fix. RSU synchronizes the aggregated data and projects objects onto the global reference system. At this step, a Multi-Target Tracking (MTT) filter tracks the static road elements in the presence of clutter. Here, vehicles are considered different sensors making observations in the same area and a special form of Labeled Multi-Bernoulli Mixture (LMBM), also known as multi-sensor LMBM, is utilized. This operation returns object locations with lower uncertainty to be considered as anchors for the subsequent steps. It is worth noting that, the accuracy and precision of target locations in the global reference frame are tightly correlated to the number of vehicles as the higher the number of vehicles, the better the accuracy. These objects are back-propagated to each vehicle via V2I communications. Within this transmission, GNSS position fix, detected targets with clutters, and augmented object locations are available at each vehicle. Subsequently, vehicles individually employ a custom Extended Kalman Filter (EKF) given in Sect. 2.3 and leverage their global positioning.

2.1 Grid-Based Spatial Detector

The Grid-Based Spatial Detector (GSD) employed in this work is designed to isolate sets of points in the 3D space that objects have generated. The process is initiated by inserting a point cluster at a known position within the point cloud to mitigate segmentation noise. Subsequently, the point cloud is projected onto a 2D space and segmented to generate a density map. This density map is subsequently filtered to reduce noise-induced segments. At the core of this object detector is a density-based clustering method, namely density-based spatial clustering of application with noise (DBSCAN) [3] which is utilized to generate clusters in the density map. In turn, the inserted point cloud has been recognized among all detections and the segmentation bias has been eliminated by shifting clusters. Consequently, clusters with small dimensions are averaged and represented as single-point objects, ensuring accurate and reliable object detection.

2.2 Cooperative LMBM

MTT is a critical process performed in autonomous systems, for detecting and tracking multiple objects in a dynamic environment. An advanced MTT method is LMBM [5], efficiently handling measurement-target data association and clutter. This filter relies on a multi-Bernoulli birth model that initiates Bernoulli components at predefined locations.

The projection of detected objects on a global reference system depicts a multi-sensor configuration with independent imaging sensors. Likewise, the detected objects are independent. Therefore, Bernoulli components are independent and follow separate Bayesian recursions. A Bernoulli Random Finite Set (RFS) \(\beta \) is comprised of components at time t, \( \text {RFS}_\beta \triangleq \{ ( T, L)_{\beta }, \textrm{r}_{e,t,\beta }, \mu _{t,\beta }, \varSigma _{t,\beta } \}\), where \(T_{\beta }\) is the birth time, \(L_{\beta }\) is a unique label assigned to Bernoulli \(\beta \) born at time t, and \(\mu _{t,\beta }\) is the first central moment and \(\varSigma _{t,\beta }\) is the covariance of Gaussian distribution. Bernoulli components follow a static motion model, namely \(\mu _{t,\beta } = \mu _{t-1,\beta }\), as fixed targets in the driving scenario are considered.

For the propagation of existence probability, \(\textrm{r}_e\), and marginal association probabilities over time, Gibbs’s sampling [6] has been employed thanks to its capability to handle high dimensional problems. After conducting the gating, pruning and capping steps, target states are obtained by processing the remaining Bernoulli components. These states contain the spatial attribute of objects in 2D, denoted by the object set \( \mathcal {O} = \{\textbf{x}_{o,t}\}: |\mathcal {O}| = N_o\), and their corresponding covariance to be used by the filter introduced in the following section.

2.3 Extended Kalman Filter-Based Refinement Stage

A set of vehicles \(\mathcal {V} = \{ 1, \ldots , N_{\text {V}} \}\) cruise in a 2D area. Each vehicle v, with \(v \in \mathcal {V}\), is represented by its state \(\textbf{x}_{v,t} = [\textbf{u}_{v,t}, \textbf{v}_{v,t}]\) where \(\textbf{u}_{v,t}\) and \(\textbf{v}_{v,t}\) are the 2D position and 2D velocity, respectively. The vehicle’s state changes over time according to a nearly constant velocity motion model [7], defined as

$$\begin{aligned} \textbf{x}_{v,t|t-1} = \textbf{F} \textbf{x}_{v,t-1} + \textbf{L}\textbf{q}_{v,t-1} \, , \end{aligned}$$
(1)

where \(\textbf{F}~=~ [\textbf{I}_2 \, T_s \textbf{I}_2\,; \textbf{0}_{2\times 2} \, \textbf{I}_2]\) and \(\textbf{L}~=~[0.5 T_s^2 \textbf{I}_2\, ; T_s \textbf{I}_2]\), \(T_s\) the sampling time, while \(\textbf{q}_{v,t-1} \sim \mathcal {N}(\textbf{0},\textbf{I}_2 \sigma _{v,\text {U}}^2)\) is a zero-mean Gaussian random variable with standard deviation \(\sigma _{v,\text {U}}\) modeling the acceleration uncertainty of the v-th vehicle.

EKF is employed for tracking as follows. States are predicted according to the motion model in (1) and are exchanged with RSU along with detected objects provided by GSD explained in Sect. 2.1. Subsequently, the LMBM filter (Sect. 2.2) is utilized. The leveraged object locations are then back-propagated to vehicles and object-object association between detected objects and leveraged targets is established according to the maximum likelihood criterion.

For the update step of the EKF, both GNSS and object-derived measurements are utilized to enhance vehicular positioning. The GNSS measurement of vehicle v is modelled as follows:

$$\begin{aligned} \boldsymbol{\rho }_{g,t}^{(v)} = \textbf{h}_{g}(\textbf{x}_{v,t}) + \textbf{n}_{v,t} = \textbf{T}\textbf{x}_{v,t} + \textbf{n}_{v,t} \, , \end{aligned}$$
(2)

where \(\textbf{T} = [\textbf{I}_2, \textbf{0}_{2\times 2}]\), while \(\textbf{n}_{v,t} \sim \mathcal {N}(\textbf{0}, \textbf{I}_2 \sigma _{v}^2)\) is a zero-mean Gaussian random variable with standard deviation \(\sigma _{v}^2\) characterizing the GNSS positioning accuracy of the v-th vehicle.

Any object \(o \in \mathcal {O}\) with state \( \textbf{x}_{v,o,t} = [\textbf{u}_{x,o,t}, \textbf{u}_{y,o,t}]\) refined by the RSU is more accurate compared to the detection j, such that \(\textbf{x}_{v,j,t} = [\textbf{u}_{x,j,t}, \textbf{u}_{y,j,t}]\), generated by the same object on vehicle v. Leveraging this fact, so o and j, the measurement models \(\boldsymbol{\rho }_{r,t}^{(j,v)}\) and \(\boldsymbol{\rho }_{\alpha ,t}^{(j,v)}\), define the distance and the clockwise angle from positive x-direction between object j and vehicle v, namely they are

$$\begin{aligned} \boldsymbol{\rho }_{r,t}^{(j,v)} &= \textbf{h}_{r}(\textbf{x}_{v,j,t}) + \textbf{n}_{r,t}^{(j,v)} = \Vert \boldsymbol{\rho }_{g,t}^{(v)} - \textbf{x}_{v,j,t}\Vert ^2 + \textbf{n}^{(j,v)}_{r,t}, \end{aligned}$$
(3)
$$\begin{aligned} \boldsymbol{\rho }_{\alpha ,t}^{(j,v)} &= \textbf{h}_{\alpha }(\textbf{x}_{v,j,t}) + \textbf{n}_{\alpha ,t}^{(j,v)} = \measuredangle (\boldsymbol{\rho }_{g,t}^{(v)}, \textbf{x}_{v,j,t}) + \textbf{n}^{(j,v)}_{\alpha ,t}, \end{aligned}$$
(4)

where \(\measuredangle \) is the angle while \(\textbf{n}^{(j,v)}_{r,t} \sim \mathcal {N}(\textbf{0},\textbf{I}_2 \sigma _{r}^2)\) and \(\textbf{n}^{(j,v)}_{\alpha ,t} \sim \mathcal {N}(\textbf{0},\textbf{I}_2 \sigma _{\alpha }^2)\) are zero-mean Gaussian random variables with standard deviations \(\sigma _{r}\) and \(\sigma _{\alpha }\), respectively. EKF refinement exploits the measurements \(\boldsymbol{\rho }_{r,t}^{(j,v)}\) and \(\boldsymbol{\rho }_{\alpha ,t}^{(j,v)}\) to improve the vehicle state. Accordingly, the state estimate of the vehicle is updated as

$$\begin{aligned} \textbf{x}_{v,t} &= \textbf{x}_{v,t|t-1} + \textbf{G}_{t} \begin{pmatrix}\boldsymbol{\rho }_{v,t} - \textbf{h}\left( \textbf{x}_{v,t|t-1}, \textbf{x}_{v,1,t}, \ldots , \textbf{x}_{v,N_o,t}\right) \end{pmatrix}, \end{aligned}$$
(5)
$$\begin{aligned} \textbf{C}_{t} &= \textbf{C}_{t|t-1} - \textbf{G}_{t}\textbf{B}\textbf{C}_{t|t-1}, \end{aligned}$$
(6)

where \(\textbf{G}_{t} = \textbf{C}_{t|t-1} \textbf{B}\begin{pmatrix}\textbf{B} \textbf{C}_{t|t-1} \textbf{B}^{\text {T}} +\mathbf {\Sigma } \end{pmatrix}^{-1} \) is the Kalman gain, \(\textbf{B} = [\textbf{T}, \mathbf {J_r}, \textbf{J}_{\alpha }]^{\text {T}}\) in which \(\textbf{J}_{r}\) and \(\textbf{J}_{\alpha }\) are the Jacobian matrices extracted from (3) and (4), \(\boldsymbol{\rho }_{v,t} \triangleq [\boldsymbol{\rho }_{g,t}^{(v)}, \boldsymbol{\rho }_{r,t}^{(1,v)}, \ldots , \boldsymbol{\rho }_{r,t}^{(N_{o},v)}, \boldsymbol{\rho }_{\alpha ,t}^{(1,v)}, \ldots , \boldsymbol{\rho }_{\alpha ,t}^{(N_{o},v)}]^\text {T}\), \(\mathbf {\Sigma } \triangleq \text {blkdiag}(\sigma _v^2 \textbf{I}_2, \sigma _r^2 \textbf{I}_{N_o}, \sigma _{\alpha }^2 \textbf{I}_{N_o})\), \(\textbf{h} = [\textbf{h}_{g}(\textbf{x}_{v,t|t-1}), \bar{\textbf{h}}_{r}^o, \bar{\textbf{h}_{\alpha }^o}]^{\text {T}}\) with \( \bar{\textbf{h}}_{\alpha }^o = [\textbf{h}_{\alpha }^o(\textbf{x}_{v,1,t}), \ldots , \textbf{h}_{\alpha }^o(\textbf{x}_{v,N_o,t})]^{\text {T}}\), and \( \bar{\textbf{h}}_r^o = [ \textbf{h}_r^o(\textbf{x}_{v,1,t}), \ldots , \textbf{h}_{r}^o(\textbf{x}_{v,N_o,t})]^{\text {T}}\) which are range and angle measurements calculated using augmented objects.

Fig. 1.
figure 1

Trajectories show GNSS measurements along with RTK-corrected ground truth (GT) positions, SBAS and SPP available regions along with prototypes

Full size image

3 Experimentation and Results

To assess the performance of the proposed method, we collected real-world data through several experiments using two different instrumented prototype vehicles, an electric light quadricycle and a prototype bus as shown in Fig. 1, equipped with comprehensive sensor systems. Collected data includes RTK-corrected and uncorrected GNSS measurements as well as LiDAR point clouds. Five objects detectable by LiDAR were used to enhance positioning, aiming to evaluate localization improvement compared to ICP. The setup, shown in Fig. 1, featured both vehicles converging towards an area with the deployed objects. For further elaboration on the experimental campaign and dataset we refer to [1].

During the implementation of the proposed method, an assumption of lossless and zero-latency V2I communication has been made. Data collected by sensors were aggregated and processed at a central computational unit offline. As concerns the computational cost, the bottleneck is LMBM in which the number of measurement-target association hypotheses increases exorbitantly. While this problem can be overcome by investigating lighter MTT algorithms, we limited the number of hypotheses and Gibbs’s samples and initiated Bernoullis in the center of the surveillance area, so near to objects.

Figure 2 shows the Cumulative Density Function (CDF) of the vehicle positioning error considering ICP with [1] and without multipath compensation [2], raw GNSS ego vehicle position, proposed method, referred to as Cooperative Self-Positioning (CSP), along with Circular Error Probability with 95% confidence (CEP95). When CEP95 is concerned, CSP has a positioning improvement of \(68\%\) and \(66\%\) compared to GNSS for veh1 and veh2 respectively. Besides, CSP has similar performance to the benchmark ICP which assumes perfect association among detected objects, making it unemployable in real applications.

Fig. 2.
figure 2

Performance comparison of CSP against ICP with and without NLOS compensation and Ego-GNSS.

Full size image

4 Conclusion

This paper proposed a cooperative sensing-aided positioning method relying on environmental perception with LiDAR sensors. This model-based algorithm handles the uncertainty of a dynamic environment by exploiting collaborative sensing and employing the LMBM filter. This method has been assessed in a real-world application and its effectiveness has been reported. The planned future work is to establish V2I communications and integrate RSU as both an observer and the central computational unit.