LiDAR-Marker ICP Precision Docking

ArUco → LiDAR trapezoid marker → ICP registration; ±3–5 cm docking on a real robot after a Nav2 approach.

Period 2025.10 – 2025.12 · Affiliation KETI (SDR national R&D project) · Role Marker recognition pipeline implementation, LiDAR geometric recognition algorithm design, ICP registration and parameter tuning, Nav2 docking integration, TETRA_S real-robot validation

Background and Problem

Conventional LiDAR autonomous navigation is effective for approaching the vicinity of a destination, but it falls short for centimeter-level precision alignment such as charging docking, designated work positions, or stopping in confined spaces. The goal was to build a docking system that works in a real environment by progressively compensating for the strengths and weaknesses of each recognition method.

Approach

Phase 1 — ArUco Vision Docking (Simulation)

Gazebo parking environment with an attached ArUco marker and the marker recognition result.

Binarization → contour/quadrilateral candidates → Homography perspective correction → ID decoding → 6-DOF pose via PnP → TF transform (camera → base_link → odom → map). After approaching with Nav2, the robot stops precisely with the marker coordinates as the target. The limitation was sensitivity to lighting, noise, and occlusion.

Phase 2 — 2D LiDAR Geometric Marker (Simulation)

Simulation environment with trapezoidal markers of various sizes.

Without a camera, an asymmetric trapezoidal marker (60° at the bottom / 120° at the top) was identified through LiDAR line segmentation and the ratios of angles and edge lengths. Scale invariance was verified — the marker is recognized regardless of size as long as the angular structure is preserved — and larger markers yielded higher point density and therefore higher accuracy.

Phase 3 — ICP Registration Docking (Real-Environment Demonstration)

A user-defined trapezoidal template \(T\) and the real-time scan \(S\) are registered with Point-to-Point ICP. KD-Tree nearest-neighbor correspondence (\(O(\log M)\)) → optimal \(R = VU^{T}\), \(t = \bar{t} - R\bar{s}\) via SVD of the centroids and cross-covariance \(H\) → convergence check (Δt, Δθ, MSE) → success/failure decision by fitness score. ICP is activated only in the low-speed segment after the Nav2 approach, real-time performance was secured through KD-Tree reuse and downsampling, and the docking parameters (retries, abort_distance, docked_distance_threshold, etc.) were finalized through repeated experiments.

Before (left) and after (right) ICP registration — white template and green LiDAR scan.
The actual docking frame and the TETRA_S robot in operation.

Results

  • Integrated two-stage control — Nav2 global approach → ICP fine alignment — and demonstrated ±3–5 cm precision docking in a real environment.
  • Combined with simulation-based battery charging logic to complete an automatic charging scenario, and later verified the Docking function on top of the OTA framework without code modification.

Tech Stack

OpenCV ArUco · PnP · Line Segmentation · ICP · KD-Tree · SVD · Nav2 Docking Server · TF · ROS 2 Humble · Gazebo · PCL · Jackal / TETRA_S · C++ / Python