LVCP: LiDAR-Vision Tightly Coupled Collaborative Real-time Relative Positioning

Zhuozhu Jian, Qixuan Li, Shengtao Zheng, Xueqian Wang, Xinlei Chen

Abstract

In air-ground collaboration scenarios without GPS and prior maps, the relative positioning of drones and unmanned ground vehicles (UGVs) has always been a challenge. For a drone equipped with monocular camera and an UGV equipped with LiDAR as an external sensor, we propose a robust and real-time relative pose estimation method (LVCP) based on the tight coupling of vision and LiDAR point cloud information, which does not require prior information such as maps or precise initial poses. Given that large-scale point clouds generated by 3D sensors has more accurate spatial geometric information than the feature point cloud generated by image, we utilize LiDAR point clouds to correct the drift in visual-inertial odometry (VIO) when the camera undergoes significant shaking or the IMU has a low signal-to-noise ratio. To achieve this, we propose a novel coarse-to-fine framework for LiDAR-vision collaborative localization. In this framework, we construct point-plane association based on spatial geometric information, and innovatively construct a point-aided Bundle Adjustment (BA) problem as the backend to simultaneously estimate the relative pose of the camera and LiDAR and correct the VIO drift. In this process, we propose a particle swarm optimization (PSO) based sampling algorithm to complete the coarse estimation of the current camera-LiDAR pose. In this process, the initial pose of the camera used for sampling is obtained based on VIO propagation, and the valid feature-plane association number (VFPN) is used to trigger PSO-sampling process. Additionally, we propose a method that combines Structure from Motion (SFM) and multi-level sampling to initialize the algorithm, addressing the challenge of lacking initial values.

Introduction

In air-ground robotic teams, precise relative positioning between Unmanned Aerial Vehicles (UAVs) and Unmanned Ground Vehicles (UGVs) is fundamental for collaborative tasks such as autonomous landing or joint mapping. However, operating in unknown, GPS-denied environments presents significant hurdles. Standard monocular Visual-Inertial Odometry (VIO) on drones often suffers from scale drift and can fail during aggressive maneuvers.

Experimental setup showing multiple drones and a UGV collaborating in a GPS-denied environment.

LVCP (LiDAR-Vision Collaborative Positioning) addresses these challenges by using the UGV as a "mobile anchor." By leveraging the high-precision geometric data from the UGV's LiDAR, the system corrects the drift of the drone's lighter sensors (monocular camera and IMU) in real-time without requiring prior maps or precise initial poses.

Methodology

The LVCP framework employs a coarse-to-fine optimization strategy to achieve robust relative positioning.

1. Multi-level Initialization

To overcome the "cold start" problem of unknown initial relative poses, LVCP combines Structure from Motion (SfM) with a multi-level sampling strategy. This approach matches the drone's sparse visual features to the UGV's dense LiDAR map significantly faster than traditional uniform sampling, reducing search time by up to 99.9%.

2. Event-Triggered PSO Sampling

When the drone's VIO begins to drift—detected by a decrease in valid feature-plane associations—the system triggers a Particle Swarm Optimization (PSO) algorithm. This global search provides a coarse alignment between the camera's view and the LiDAR's geometry, ensuring the system remains localized even under significant disturbance.

The LVCP system architecture, highlighting the tight coupling between drone VIO and UGV LiDAR mapping.

3. Point-aided Bundle Adjustment (BA)

The "fine" stage of the framework involves a backend optimization that tightly couples IMU pre-integration, visual reprojection errors, and point-to-plane residuals. By measuring the distance between visual features and local planes in the LiDAR cloud, the system uses the UGV's geometric data as a "ground truth" reference to provide the absolute scale that monocular vision lacks.

Experimental Results

The system was rigorously evaluated using the standard EuRoC MAV dataset and a self-built air-ground dataset featuring a UGV with HESAI LiDAR and multiple UAVs with Intel RealSense cameras.

  • Accuracy and Robustness: LVCP achieved lower Absolute Trajectory Error (ATE) than state-of-the-art methods like VINS-mono, particularly in high-motion sequences. It remained stable with initial pose disturbances up to 0.5m, whereas traditional methods often fail beyond 0.1m.
  • Real-time Multi-Agent Performance: The framework successfully managed three drones simultaneously on consumer-grade hardware, maintaining real-time performance while continuously correcting VIO drift against the shared LiDAR map.

Conclusion and Future Work

LVCP provides a robust solution for relative positioning in complex, unknown environments by treating the UGV's dynamic LiDAR cloud as a geometric reference. While highly effective, future work will address limitations regarding dynamic obstacles in the environment and the dependency on geometric structures (like walls or floors) for plane fitting. Additionally, optimizing communication protocols for long-range missions remains a key area for development.

Summary updated

Abstract

In air-ground collaboration scenarios without GPS and prior maps, the relative positioning of drones and unmanned ground vehicles (UGVs) has always been a challenge. For a drone equipped with monocular camera and an UGV equipped with LiDAR as an external sensor, we propose a robust and real-time relative pose estimation method (LVCP) based on the tight coupling of vision and LiDAR point cloud information, which does not require prior information such as maps or precise initial poses. Given that large-scale point clouds generated by 3D sensors has more accurate spatial geometric information than the feature point cloud generated by image, we utilize LiDAR point clouds to correct the drift in visual-inertial odometry (VIO) when the camera undergoes significant shaking or the IMU has a low signal-to-noise ratio. To achieve this, we propose a novel coarse-to-fine framework for LiDAR-vision collaborative localization. In this framework, we construct point-plane association based on spatial geometric information, and innovatively construct a point-aided Bundle Adjustment (BA) problem as the backend to simultaneously estimate the relative pose of the camera and LiDAR and correct the VIO drift. In this process, we propose a particle swarm optimization (PSO) based sampling algorithm to complete the coarse estimation of the current camera-LiDAR pose. In this process, the initial pose of the camera used for sampling is obtained based on VIO propagation, and the valid feature-plane association number (VFPN) is used to trigger PSO-sampling process. Additionally, we propose a method that combines Structure from Motion (SFM) and multi-level sampling to initialize the algorithm, addressing the challenge of lacking initial values.
BibTeX
@misc{jian2024lvcp,
  author = {Zhuozhu Jian and Qixuan Li and Shengtao Zheng and Xueqian Wang and Xinlei Chen},
  title = {{LVCP: LiDAR-Vision Tightly Coupled Collaborative Real-time Relative Positioning}},
  year = {2024},
  eprint = {2407.10782},
  archivePrefix = {arXiv},
  url = {https://arxiv.org/abs/2407.10782},
}
Figure