GFM-Planner for Robust LiDAR Navigation

• GFM-Planner is a perception-aware trajectory planning framework that leverages a geometric feature metric and MEM encoding to improve SE(2) LiDAR localization.
• It integrates a two-stage planning pipeline with hybrid A* pre-search and MINCO-based trajectory optimization to steer robots through feature-rich regions.
• Extensive simulations and real-world tests demonstrate significantly reduced localization errors and a 100-fold memory efficiency compared to full 3D grids.

GFM-Planner is a perception-aware trajectory planning framework that leverages a geometric feature metric, specifically tailored to enhance LiDAR-based localization by actively guiding robots along feature-rich paths. It achieves this by formally quantifying the geometric observability required for robust SE(2) pose estimation from laser scans, encoding the result into a compact grid-based memory, and introducing efficient planning and optimization procedures that reduce localization error both in simulation and in real-world deployment (Lin et al., 22 Jul 2025).

1. Geometric Feature Metric (GFM) Formulation

The core of GFM-Planner is the Geometric Feature Metric (GFM), mathematically derived from the Hessian structure of the LiDAR-based localization cost. Consider a robot with pose $p = [x, y, \theta]^\top$ and LiDAR points $L_i(p) \in \mathbb{R}^2$. The localization cost to minimize is: $f(p) = \frac{1}{2} \sum_{i=1}^{N} E(L_i(p))^2$ where $E(q)$ is the Euclidean distance from a scan point $q$ to the nearest obstacle boundary. The gradient and Hessian are computed, and the key term for localization observability is: $$H_1 = \sum_{i=1}^{N} [J_i^\top(p) \nabla E(q_i)][J_i^\top(p) \nabla E(q_i)]^\top$$ where $J_i(p)$ is the Jacobian of $L_i$ with respect to $p$. Under the mild condition that $J_i(p)$ is full row rank for most beams, the Hessian is positive definite, ensuring local convexity and robustness in pose estimation.

The GFM is then defined as: $$M(p) = 2N - \sum_{i=1}^{N} \operatorname{rank}(J_i(p))$$ where $N$ is the number of scan points. $M(p) = 0$ when all beams are full rank (i.e., two degrees of constraint per beam for SE(2)), indicating a feature-rich region. Higher $M(p)$ values reveal geometric degeneracy and poor localization observability.

2. Metric Encoding Map (MEM) Structure and Efficiency

To embed perception-awareness efficiently into planning, GFM-Planner introduces the Metric Encoding Map (MEM), a 2D grid structure covering the robot's workspace. For each grid cell $(x, y)$ and $L$ discrete yaw angles $\theta_l$, the full-rank status of $J_i$ is computed and packed into an $L$-bit integer $q(x, y)$, such that each bit represents whether the Jacobian for yaw $\theta_l$ is degenerate (bit set) or full rank (bit unset).

For planning queries, e.g., for a scan arc from index $i$ to $j$, the relevant submask $q_{i:j}(x,y)$ is bitwise masked from $q(x,y)$ and the Hamming weight $\mathcal{W}(q_{i:j}(x,y))$ instantly provides $M_{i:j}(x,y)$, the GFM value for that pose and scan window. Occupied (obstacle) cells have all bits set. This design allows for constant-time metric lookups during planning while reducing memory footprint compared to 3D pose grids.

3. Perception-Aware Path and Trajectory Planning

The planning algorithm integrates the GFM at two levels:

1. Heuristic Pre-Search: A hybrid-state A* (incorporating SE(2) kinematics) uses cumulative cost

$g_c = \sum_{k} \sigma_\epsilon(M(p_k))$

where $\sigma_\epsilon(\cdot)$ is a soft sigmoid function, penalizing trajectories transiting geometrically degraded (high $M$) areas. The heuristic for each node is computed as the shortest $g_c$-cost path to goal over the MEM.

1. Trajectory Optimization: Using MINCO (minimum continuity) polynomials for trajectory representation, the objective is

$J = w_l J_l + w_e J_e$

where

$$J_l = \sum_{\text{segments}} \int_0^{t_i} \sigma_\epsilon(M(p_i(t)))\,dt$$

is the localization cost integrated along the path, and $J_e$ quantifies trajectory smoothness or energy. The optimizer (L-BFGS with line search) adjusts trajectory parameters to maximize exposure to high-feature regions while respecting temporal and kinematic constraints, obstacle avoidance, and continuity.

4. Evaluation and Experimental Validation

Extensive simulation and real-world experiments validate the effectiveness of GFM-Planner. In indoor environments with long corridors or ambiguous geometric patterns, trajectories generated by GFM-Planner consistently favor feature-rich areas, resulting in substantially reduced LiDAR localization error, as compared to baseline methods such as LF-3PM (which uses simulated scan-based post-optimization). Ablation studies confirm that both the MEM-based path search and the trajectory localization penalty are needed for maximal robustness.

The system enables parallelized offline metric computation (MEM generation), real-time planning with negligible memory overhead, and direct integration into existing navigation frameworks. For most 2D environments, the use of a 2D MEM reduces memory cost by about two orders of magnitude compared to full 3D grids.

5. Practical Applications and Broader Implications

GFM-Planner's methodology is especially suitable for autonomous robots and vehicles operating in environments with non-uniform distribution of geometric features—such as indoor service robots, warehouse AGVs, and autonomous ground vehicles traversing urban scenes with visually repetitive structures.

Potential application areas include:

• Navigation in long, sparsely featured corridors (e.g., hospitals, tunnels)
• Multi-robot exploration where reliable pose estimation is mission-critical
• Mobile manipulation under degraded perception scenarios
• Extensions to aerial or underwater robots via adaptation to SE(3) with similar metric encoding principles

The approach's central innovation is the theoretical derivation of a planning-relevant, information-theoretic observability metric, followed by a scalable map encoding and integration with modern trajectory optimization. This enables systematic improvement of self-localization accuracy, with measurable performance gains.

6. Comparison to Existing Methods and Limitations

Unlike previous perception-aware planners that either use simulated scan-based post-optimization (with high memory/computation cost), heuristics, or qualitative metrics, GFM-Planner encodes the observability metric analytically and combines it with a two-stage planning pipeline. This ensures both computational efficiency (constant-time GFM queries) and theoretical soundness (convexity of the localization cost under good feature conditions). Experimental evidence demonstrates substantial improvements in localization error and robustness, particularly in degraded environments.

A noted limitation is that the method currently operates in 2D SE(2) and requires prior map availability for MEM construction. Extensions to 3D environments, adaptation for real-time dynamic mapping, and integration with active SLAM frameworks are natural directions for further development.

7. Open-Source Release and Reproducibility

The authors provide open-source code for GFM-Planner, facilitating community adoption and reproducibility of experimental results. This codebase includes routines for MEM generation, path search, trajectory optimization, and visualization tools. As such, GFM-Planner serves as a baseline for benchmarking future perception-aware planning systems in localization-critical applications.

---

GFM-Planner establishes a systematic connection between the geometric observability of LiDAR-based localization and active trajectory selection, introducing a theoretically grounded, memory-efficient, and effective planning paradigm for robust autonomous navigation in feature-variable environments (Lin et al., 22 Jul 2025).