多机器人初始配准方法

介绍

多机器人系统初始化时，各机器人的局部坐标系往往不一致，需要通过点云配准将它们统一到同一个世界坐标系。本文介绍一种三阶段配准策略：重力对齐消除姿态差异、RANSAC 全局粗配准找到大致变换、GICP 精配准优化精度。三个阶段层层递进，兼顾鲁棒性和精度。

三阶段配准策略

整个配准流程分为三个阶段，每个阶段解决不同层次的问题：

数据准备

点云融合

每个机器人采集 init_frame_num（默认 16）帧点云并融合，提高配准鲁棒性。单帧点云的特征信息有限，多帧融合后点云密度更高，几何特征更丰富，配准成功率显著提升。

• Robot 0 作为参考坐标系（世界坐标系）
• Robot 1 ~ N 分别与 Robot 0 进行配准
• 配准结果存储在 drone_gicp_poses_[robot_id]

Stage 1：重力对齐

目的

不同机器人的安装姿态可能不同，导致 Roll/Pitch 存在差异。重力对齐通过估计重力方向，将点云旋转到统一的重力对齐坐标系，消除姿态差异。

实现步骤

原理说明

场景中的水平平面（地面、天花板、桌面）的法向量接近重力方向。通过统计这些法向量，可以估计重力方向并进行对齐。

1
2
3
4
5
6

重力对齐前:                    重力对齐后:

    ╱╲                              │
   ╱  ╲  Roll=10°                   │  Roll=0°
  ╱    ╲                            │
 ╱______╲                       ────┼────

局限性

• 需要场景中有足够的水平平面
• 如果点云已经重力对齐（来自 LiDAR-IMU 里程计），此步骤可能多余
• 法向量估计对点云密度和噪声敏感

Stage 2：RANSAC 全局配准

目的

在重力对齐的基础上，进行全局粗配准，找到大致的平移和 Yaw 旋转。RANSAC 通过随机采样和一致性检验，能够在存在大量误匹配的情况下找到正确变换。

FPFH 特征提取

FPFH（Fast Point Feature Histograms）是一种局部几何特征描述子，用于描述点云中每个点的局部几何形状，具有旋转不变性和平移不变性。每个点的 FPFH 特征为 33 维向量。

局部坐标系建立

对点 $p$ 和其邻近点 $q$，建立局部坐标系 $(u, v, w)$：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11

              w
              ↑
              │
              │
    q ────────┼──────── p
              │
              │
              ↓
              v

    u 指向屏幕外 (n_p 方向)

三个特征值

对点 $p$ 和其邻近点 $q$，计算三个几何特征：

特征 1：$\alpha$ (alpha)

含义为邻近点法向量 $n_q$ 在 $v$ 方向的投影，范围 $[-1, 1]$。

1
2
3
4
5
6
7
8
9

        n_q
         ↑
         │
         │   α = cos(angle between v and n_q)
         │  ╱
         │ ╱
    ─────┼───── q
         │
         ↓ v

特征 2：$\varphi$ (phi)

含义为点 $p$ 的法向量 $u$ 与连线 $(q-p)$ 的夹角余弦，范围 $[-1, 1]$。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10

        n_p = u
         ↑
         │
         │   φ = cos(angle between u and (q-p))
         │  ╱
         │ ╱
    ─────┼───── p
         │
         ↓
         q

特征 3：$\theta$ (theta)

含义为邻近点法向量 $n_q$ 在 $u$-$w$ 平面的旋转角，范围 $[0, 2\pi]$。

1
2
3
4
5
6
7
8
9

        w
        ↑
        │   n_q
        │  ╱
        │ ╱ θ
        │╱
    ────┼──── u
        │
        ↓

直方图构建

将三个特征值分别离散化为直方图，最终拼接为 33 维特征向量：

特征的物理含义

SPFH 到 FPFH 的加权累加

SPFH 只考虑直接邻近点（1 邻域），FPFH 通过加权累加考虑更远邻域（2 邻域）的影响：

其中 $k$ 为点 $p$ 的邻近点数量，$w_i$ 为点 $p$ 到 $q_i$ 的距离。

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11

1 邻域 (SPFH):                2 邻域 (FPFH):

      q1                            q1
       │                             │
       │                             │
       p ─── q2                  p ─── q2 ─── q3
       │                             │
       │                             │
      q4                            q4 ─── q5

只考虑 p 的直接邻近点          考虑 p 的邻近点的邻近点

特征不变性

• 旋转不变性：基于相对角度 $(\alpha, \varphi, \theta)$，不依赖绝对坐标系
• 平移不变性：基于相对位置 $(q - p)$，不依赖绝对位置
• 尺度鲁棒性：对点云密度变化有一定容忍度，直方图的分布模式保持稳定

RANSAC 采样一致性

RANSAC 通过随机采样和内点检验，在存在大量误匹配的情况下找到正确的刚体变换。

FPFH 在 RANSAC 中的作用：传统 ICP 只用距离找对应点，容易陷入局部最优。RANSAC + FPFH 基于几何特征匹配，找到更可靠的对应关系，对初始位置不敏感，适合全局配准。

RANSAC 参数配置

Stage 3：GICP 精配准

目的

在 RANSAC 粗配准的基础上，进行高精度精配准。GICP（Generalized Iterative Closest Point）在传统 ICP 基础上引入协方差矩阵加权，提高配准精度和鲁棒性。

误差函数

协方差矩阵的作用

协方差矩阵描述了局部表面的几何特性，不同几何区域的协方差差异决定了配准时的权重分配：

1
2
3
4
5

平坦区域:                    边缘/角点:
    ─────────                    │
    协方差矩阵大                  │ 协方差矩阵小
    权重高                        权重低
    约束强                        约束弱

实现步骤

GICP 参数配置

变换矩阵合并

DOF 约束

根据运动学约束，限制变换的自由度：

最终变换矩阵

合并过程：

1. $T_{gravity\_src}$：将源点云旋转到重力对齐坐标系
2. $T_{fine}$：在重力对齐坐标系下进行配准
3. $T_{gravity\_tgt}^{-1}$：旋转回原始坐标系

多机器人并行配准

各机器人的 GICP 配准相互独立，可以并行执行以加速初始化过程：

1
2
3
4
5
6
7

std::vector<std::thread> threads;
for (int drone_id = 1; drone_id < num_robots_; ++drone_id) {
    threads.emplace_back([this, drone_id, &target_cloud]() {
        drone_gicp_poses_[drone_id] = GicpRegistration(src_cloud, target_cloud);
    });
}
for (auto& t : threads) t.join();

配准完成后，构建 GTSAM 因子图：

• Robot 0：PriorFactor（先验，固定为世界坐标系原点）
• Robot 1 ~ N：BetweenFactor（相对约束）

配置参数汇总

流程总结

参考

• Open3D FPFH 文档：http://www.open3d.org/docs/latest/tutorial/pipelines/global_registration.html
• PCL FPFH 估计：https://pointclouds.org/documentation/classpcl_1_1_f_p_f_h_estimation.html
• GICP 论文：Segal, A., Haehnel, D., & Thrun, S. (2009). Generalized-ICP. RSS.
• FPFH 论文：Rusu, R. B., Blodow, N., & Beetz, M. (2009). Fast Point Feature Histograms (FPFH) for 3D registration. ICRA.
• GTSAM 因子图：https://gtsam.org/