Session B2b: Autonomous Navigation and Safety-Critical Applications

Offline Covariance Prediction for Lidar-Based Map-Matching in Autonomous Systems
Hadi S. Wassaf and Jon Poage, USDOT Volpe Center; Jason H. Rife, Tufts University
Location: Beacon B

Background:
Automated vehicles are increasingly being tested on open roadways. Ensuring their safe operation strongly relies on high-quality navigation information. Lidar-based positioning solutions serve as a critical alternative to satellite navigation in environments where satellite signals are spoofed (Kujur et al., 2024) or obstructed, such as in tunnels, large parking structures, and deep urban canyons (Nagai et al., 2020; Nagai et al., 2024). Among various Lidar positioning methodologies (Rife et al, 2024), map-matching has gained significant attention because it does not require additional infrastructure installation or modification. However, for Lidar map-matching to be viable, navigation uncertainty must be quantified.
Defining a covariance matrix for map-matching is a first critical step in uncertainty quantification, as the covariance matrix provides a general assessment of lidar accuracy and plays a key role in fusing lidar measurements with other sensor data, for example, via a Kalman filter. Several attempts have been made to assess lidar map-matching covariance through online processing (Stoyanov et al, 2012; Kanhere & Gao, 2019; Shetty & Gao, 2019; Yuan et al, 2023; McDermott & Rife, 2024). Efforts have also been made to characterize map quality for position-tracking offline, both for Lidar (Akai et al, 2017) and for related perception sensors like cameras (Gupta and Gao, 2021). However, no efforts to date have developed offline map-based covariance estimates to describe the full positioning problem, from wide-area acquisition to local-area tracking.
In this paper, we present an innovative approach that utilizes offline Lidar point cloud data (PCD) to construct a reference map with embedded error characterization for the full positioning problem. Specifically, we characterize associated errors using a mean and covariance matrix corresponding to the center of each road segment. This proposed method introduces minimal additional real-time processing complexity and latency, as all integrity prediction computations are conducted offline to be readily utilized by the online system. In concept, a conservative bound could be generated from our map-based error characterization, which could in turn be used to define an integrity bound. An advantage of a map-based integrity bound would be the opportunity to conduct route planning, where the availability of integrity along a route could be checked at the beginning of a trip and revised continuously throughout the trip if reference map or associated integrity bound information were updated. Additionally, this approach can be extended to generate multiple algorithm-specific integrity bounds both at the trip's outset and dynamically throughout its duration, thereby enabling optimal selection of map-matching algorithms along the route.
Methodology:
Our methodology begins by segmenting a route into smaller along-route road segments, which can be either disjoint or overlapping. Each segment captures characteristics representing the local environmental surroundings. The segment's length is generally selected to be small enough for the positioning error to be considered stationary, yet sufficiently long to ensure an adequate number of measurements to characterize the position-error covariance. In the representative data we process, each segment contains a portion of the route within 25 meters radially from the segment’s center anchor point. For straight segments, this amounts to a length around 50 meters (+/- 25 meters from the anchor point) corresponding to approximately 3.7 seconds of travel time at a driving speed of 30 mph. A standard automotive Lidar can capture 10 scans per second, yielding a minimum of 37 available measurements per road segment at that same 30 mph for an empirical estimate of the error mean and multivariate covariance matrix associated with the center point of that segment.
To validate and demonstrate our approach, we equipped a vehicle with a VLP-16 Lidar to collect the necessary PCDs for generating the offline map and testing our approach in online processing. The vehicle is further outfitted with a Novatel PwrPak 7 (PP7) receiver with integrated IMU, to provide ground truth data for measuring error time series for both offline and online processing. Ideally, traversing the same route twice would yield one trip for offline processing to create the reference map and associated error covariance, with the second trip for online processing to assess the validity of these bounds. However, since data was collected during a single trip, we down-sample or demultiplex the data into two distinct sets of PCDs: the first for offline map creation, as well as mean and error covariance estimation; and the second to evaluate the validity of the offline error bounds through online processing.
In the offline processing of the dataset, we follow a series of steps for each road segment to generate the error time series and an associated statistical characterization. First, precise point positioning (PPP) is applied to correct the ionospheric, tropospheric, and satellite ephemeris errors in the PP7 pseudorange data. The resulting PPP measurements are used as an accurate ground truth position time series for each segment. To provide resilience of the ground truth through brief GNSS outages (e.g., below overpasses), the PPP measurements are fused with IMU measurements using Novatel’s Waypoint post-processing software.
A local submap is constructed around each central offline PCD using a subset of the remaining geo-rectified offline PCDs in its vicinity. The extent of the submap is determined by the upper bound of the vehicle's initial position uncertainty, which is assumed to be a known, large value. The purpose of limiting the submap size is twofold: to reduce map-matching errors and to minimize processing time. In this demonstration, we reasonably assume that the upper bound of the initial position error is 10 meters. Consequently, a submap is created from all geo-rectified PCDs centered within 25 meters of the estimated initial position, or 2.5 times the size of the known initial position error bound. This is done to ensure a high map density within 10 meters of the position of the center PCD. Although the 25-meter radius of the PCD neighborhood coincides with the radial size of the segment, these two parameters do not necessarily need to align, as they are selected based on different criteria.
Next, Lidar-based positioning is performed on the central PCD using the submap constructed around it. This process is repeated to derive a Lidar positioning measurement for the vehicle corresponding to each offline PCD. The map-matching algorithm consists of two stages: First, coarse Lidar positioning is conducted using scan context descriptors (Kim and Kim, 2018) to obtain a rough pose prediction based on the loose initial pose estimate. This is followed by a refinement step using the Normal Distribution Transform (NDT) algorithm (Magnusson, 2009). The rough pose prediction from the scan context descriptors serves as the initial pose estimate for the NDT algorithm.
This end-to-end method combines the strengths of both algorithms while compensating for their respective limitations, ultimately providing a refined pose measurement that is less sensitive to the initial pose estimate. Specifically, the NDT algorithm performs optimally when the initial position is within one to two meters of the true position but may converge to incorrect locations if the initial estimate is farther off. Conversely, the scan context descriptor algorithm can tolerate much larger initial position errors but is only effective in bringing the estimate within one to two meters of the true position.
We subsequently compute the difference between the Lidar-based positions and the ground truth positions to generate the error time series. Finally, we use these error measurements to calculate the sample covariance for the center point of each overlapping road segment. In our processing we employ maximum overlap between segments, which amounts to a sliding window, for the purpose of assigning an error mean and covariance measurements for each offline PCD.
For the online dataset processing, we start by analyzing the GNSS and IMU measurements using the same method outlined in the offline processing to yield the ground truth positions for the online PCDs. Subsequently, we perform Lidar-based localization for each online PCD utilizing the corresponding geo-rectified offline submap, using the context descriptor coarse localization followed by the NDT position refinement step and calculate the measurement error accordingly. We then compare the online errors across the full route against the error bounds established from the appropriate offline error mean and covariance matrices to evaluate how well the online errors are represented by the offline error statistics. For each online Lidar-based position, the offline error statistics used are the ones corresponding to the nearest neighbor offline PCD center. Lessons learned from this process will, in future work, inform the development of rigorous map-based integrity bounds.
REFERENCES
Gupta, S., & Gao, G. (2021). Data?driven protection levels for camera and 3D map?based safe urban localization. NAVIGATION, 68(3), 643-660. https://doi.org/10.1002/navi.445
Akai, N., Morales, L. Y., Takeuchi, E., Yoshihara, Y., & Ninomiya, Y. (2017). Robust Localization Using 3D NDT Scan Matching with Experimentally Determined Uncertainty and Road Marker Matching. 2017 IEEE Intelligent Vehicles Symposium (IV), pp. 1356-1363. https://doi.org/10.1109/IVS.2017.7995900
Kanhere, A. V., & Gao, G. X. (2019). LiDAR SLAM utilizing normal distribution transform and measurement consensus. In Proceedings of the 32nd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2019), pp. 2228-2240. https://doi.org/10.33012/2019.16954
McDermott, M., & Rife, J. (2024). ICET Online Accuracy Characterization for Geometry-Based Laser Scan Matching. NAVIGATION, 71(2). https://doi.org/10.33012/navi.647
Shetty, A., & Xingxin Gao, G. (2019). Adaptive covariance estimation of LiDAR?based positioning errors for UAVs. NAVIGATION, 66(2). https://doi.org/10.1002/navi.307
Stoyanov, T., Magnusson, M., Andreasson, H., & Lilienthal, A. J. (2012). Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations. The International Journal of Robotics Research, 31(12), 1377-1393. https://doi.org/10.1177/0278364912460895
Yuan, R. H., Taylor, C. N., & Nykl, S. L. (2023). Accurate covariance estimation for pose data from iterative closest point algorithm. NAVIGATION, 70 (2). https://doi.org/10.33012/navi.562
Kim, G., & Kim, A. (2018). Scan Context: Egocentric Spatial Descriptor for Place Recognition Within 3D Point Cloud Map. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4802-4809. https://doi.org/10.1109/IROS.2018.8593953
Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform --- an Efficient Representation for Registration, Surface Analysis, and Loop Detection. [Doctoral Dissertation, Örebro University]. Örebro universitet.