LF-3PM: a LiDAR-based Framework for Perception-aware Planning with Perturbation-induced Metric

Kaixin Chai, Long Xu, Qianhao Wang, Chao Xu, Peng Yin, Fei Gao

TL;DR

LF-3PM is a novel framework that helps LiDAR-equipped robots choose trajectories that maximize their localization accuracy and stability. By introducing a "perturbation-induced metric" and a pre-computed Static Observation Loss Map (SOLM), the system allows robots to avoid "featureless deserts" and "geometric tunnels" in real-time without the computational burden of simulating dense LiDAR scans during the planning phase.

What Problem Does This Paper Address?

Autonomous robots rely on sensors like LiDAR to figure out where they are (localization). However, not all environments are created equal: a robot in a perfectly smooth, narrow corridor or a wide-open, featureless field can easily become "disoriented" because the sensor data looks the same regardless of small movements. This is known as the "localization degeneracy" problem.

While "Perception-aware Planning" (PAP) exists for camera-based robots, adapting it to LiDAR is difficult for two reasons:

  1. Computational Cost: Simulating a LiDAR scan is much more computationally expensive than simulating sparse visual features, making it too slow for real-time trajectory optimization.
  2. Metric Suitability: Visual metrics focus on texture and lighting, whereas LiDAR depends entirely on the geometric structure of the environment. Existing LiDAR metrics often fail to provide an independent, theoretically grounded evaluation of a single location's "localizability."

Methods

The authors propose a two-pronged solution to make LiDAR-based PAP efficient and reliable:

  • Perturbation-induced Metric: Instead of just looking at geometric features, the authors treat localization as a Least Squares problem. They derive a metric based on sensitivity analysis: if a tiny "perturbation" (noise) in the LiDAR data causes a huge jump in the estimated pose, that location has high observation loss. They offer three versions of the metric ($q^{(min)}$, $q^{(n)}$, and $q^{(max)}$) representing different levels of "conservativeness."
  • Static Observation Loss Map (SOLM): To solve the speed issue, the framework decouples observation evaluation from motion planning. Before the robot starts moving, the environment is discretized into a grid. At each grid center, a LiDAR scan is simulated once, and the observation loss is calculated and stored.
  • The Planning Pipeline:
    • Front-end: Uses Breadth-First Search (BFS) on the SOLM to find a path with low cumulative observation loss.
  • Back-end: Optimizes the trajectory using the MINCO (Minimum Control effort) framework, balancing localization quality, obstacle avoidance, and robot dynamics (smoothness and speed).

Datasets

The framework was evaluated using:

  • Unreal Engine 5 (UE5) Simulations: The authors built diverse environments including a "Gobi desert" with scattered stones, lush grasslands, and narrow corridors to test how different metrics handle unstructured vs. structured geometry.
  • Real-World Experiments: Conducted using a RoboMaster AI-2020 omnidirectional robot equipped with a Livox Mid-70 LiDAR. The ground truth for localization was provided by a high-precision NOKOV motion capture system.

Key Insights and Ideas

  • Decoupling is the Secret Sauce: By pre-calculating the SOLM, the authors turned a "heavy" simulation problem into a "light" map-lookup problem. This allows the robot to react to its environment in real-time.
  • The "Goldilocks" Metric ($q^{(n)}$): The authors found that their intermediate metric, $q^{(n)}$, performed best. The standard "degeneracy factor" ($q^{(min)}$) was often too optimistic, failing to distinguish between subtle differences in unstructured environments, while the most conservative metric ($q^{(max)}$) was too restrictive.
  • Geometry is Stable: Unlike cameras, which are affected by changing light or shadows, LiDAR's dependence on geometry means the SOLM remains valid as long as the physical environment doesn't change significantly, making it a highly reliable prior for planning.

Key Results

  • Consistency: The proposed $q^{(n)}$ metric showed high correlation with the "Mean Disturbance-induced Error" (MDE), which serves as the ground truth for localization difficulty.
  • Improved Accuracy: In real-world tests, trajectories generated by LF-3PM resulted in significantly lower localization errors compared to standard paths that ignored perception quality.
  • Active Topology Selection: The robot successfully demonstrated "active" behavior—for example, choosing to move closer to a cluster of boxes or orienting its sensor toward a wall to "anchor" its position when navigating through an otherwise empty space.

Limitations and Future Work

  • Least Squares Assumption: The metric assumes the underlying localization algorithm is essentially solving a least-squares problem. While common (e.g., ICP or GICP), it might not directly apply to emerging deep-learning-based localization methods.
  • Static Environments: The current SOLM is pre-computed for a static map. The authors acknowledge that in the future, the SOLM needs to evolve into a dynamically updated grid to handle moving obstacles or previously unmapped areas.
  • Computational Pre-processing: While the planning is fast, the initial generation of the SOLM still requires a one-time heavy computation (though this is mitigated by GPU-accelerated ray-casting).

Summary updated

Abstract

Just as humans can become disoriented in featureless deserts or thick fogs, not all environments are conducive to the Localization Accuracy and Stability (LAS) of autonomous robots. This paper introduces an efficient framework designed to enhance LiDAR-based LAS through strategic trajectory generation, known as Perception-aware Planning. Unlike vision-based frameworks, the LiDAR-based requires different considerations due to unique sensor attributes. Our approach focuses on two main aspects: firstly, assessing the impact of LiDAR observations on LAS. We introduce a perturbation-induced metric to provide a comprehensive and reliable evaluation of LiDAR observations. Secondly, we aim to improve motion planning efficiency. By creating a Static Observation Loss Map (SOLM) as an intermediary, we logically separate the time-intensive evaluation and motion planning phases, significantly boosting the planning process. In the experimental section, we demonstrate the effectiveness of the proposed metrics across various scenes and the feature of trajectories guided by different metrics. Ultimately, our framework is tested in a real-world scenario, enabling the robot to actively choose topologies and orientations preferable for localization. The source code is accessible at https://github.com/ZJU-FAST-Lab/LF-3PM.
BibTeX
@inproceedings{chai2024lf,
  author = {Kaixin Chai and Long Xu and Qianhao Wang and Chao Xu and Peng Yin and Fei Gao},
  title = {{LF-3PM: a LiDAR-based Framework for Perception-aware Planning with Perturbation-induced Metric}},
  booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems},
  year = {2024},
  url = {https://openreview.net/forum?id=USz2WepeiP},
}

Loading related content...

Figure