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

TL;DR

This paper addresses LAS degradation in feature-sparse environments by introducing a LiDAR-specific Perception-aware Planning framework. It defines a perturbation-induced metric to quantify LiDAR observation loss and builds a Static Observation Loss Map (SOLM) to decouple observation evaluation from motion planning. The framework uses SOLM for BFS-based topology search and cubic-spline trajectory optimization, underpinning efficient planning with LAS-oriented objectives. Experimental results across simulations and real-world tests demonstrate improved LAS and localized performance when trajectories are guided by the proposed metric, with open-source code available for reproducibility.

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.

Figures (7)

• Figure 1: (a) Narrow corridor between two buildings. (b) Lush grasslands. (c) Bird's eye view of the entire map. (d) SOLM of the map with the observation model given by Eq. (\ref{['equ:observation']}), where areas with smaller observation loss are preferable for localization and black areas indicate obstacles.
• Figure 2: A special example for illustrating the proposed metric, where $E(\lVert\Delta A\rVert,\lVert\Delta\boldsymbol b\rVert)=\sqrt{\delta\boldsymbol{t}_1^2+8\delta\boldsymbol{t}_2^2},\ \delta\boldsymbol{t}=[\delta\boldsymbol{t}_1,\delta\boldsymbol{t}_2]^\text{T}\in\mathbb{R}^2$. Figure (a) illustrates the results of directly solving for $F$ on the derivative of $\delta\boldsymbol{t}$, where $\lVert\partial F/\partial\delta\boldsymbol{t}\rVert_2^2=(\delta\boldsymbol{t}_1^2+64\delta\boldsymbol{t}_2^2)/(\delta\boldsymbol{t}_1^2+8\delta\boldsymbol{t}_2^2)$. The figure to the right is a top view of the surface on the left. Since the derivative of $F$ does not exist at zero, there is a hole in the surface. Figure (b) shows how the proposed metric looks like in the polar coordinate system $\theta-r$, where $\boldsymbol{\beta}=[\cos\theta,\sin\theta]^\text{T},\ r=\lvert q_o\rvert=\lvert d_2\rvert=\sqrt{\cos\theta^2+8\sin\theta^2}$. We can clearly see the effect of perturbations with different directions on the sensitivity of the upper bound $E$ from this figure.
• Figure 3: The proposed perception-aware planning framework. The top part represents the SOLM calculation process, while the bottom part represents the motion planning process.
• Figure 4: Four representative seances: (a) Houses, (b) Single house on the meadow, (c) Meadow, (d) Wall.
• Figure 5: Robot trajectories by the proposed framework.

Theorems & Definitions (2)

• Lemma 1
• proof