雷达

介绍 多机器人系统初始化时，各机器人的局部坐标系往往不一致，需要通过点云配准将它们统一到同一个世界坐标系。本文介绍一种三阶段配准策略：重力对齐消除姿态差异、RANSAC 全局粗配准找到大致变换、GICP 精配准优化精度。三个阶段层层递进，兼顾鲁棒性和精度。 三阶段配准策略 整个配准流程分为三个阶段，每个阶段解决不同层次的问题： graph TD A["原始点云 (body frame)"] --> B["Stage 1: 重力对齐"] B --> C["Stage 2: RANSAC 全局配准"] C --> D["Stage 3: GICP 精配准"] D --> E["变换矩阵 T (src → target)"] B -.- B1["估计重力方向\n消除 Roll/Pitch 姿态差异"] C -.- C1["FPFH 特征提取\n基于特征匹配的采样一致性估计"] D -.- D1["协方差矩阵加权\n迭代最近点优化"] 阶段 目标 方法 输出 Stage 1 消除 Roll/Pitch 姿态差异 水平平面法向量统计 旋转矩阵 $T_{gravity}$ Stage 2 全局粗配准 FPFH + RANSAC 粗配准变换 $T_{ransac}$ Stage 3 高精度精配准 GICP 协方差加权 精配准变换 $T_{fine}$ 数据准备 点云融合 每个机器人采集 init_frame_num（默认 16）帧点云并融合，提高配准鲁棒性。单帧点云的特征信息有限，多帧融合后点云密度更高，几何特征更丰富，配准成功率显著提升。 ...

点云配准方法介绍 常见的有 ICP、GICP、NDT 等，快捷实现基本基于 PCL 库中自带的点云处理实现。 GICP 原理 Generalized Iterative Closest Point (GICP) 是一种改进的 ICP 算法，通过协方差矩阵优化对应点匹配，提升配准精度。 头文件 1 2 3 4 5 6 7 #include <pcl/point_types.h> // 点类型定义 #include <pcl/point_cloud.h> // 点云容器 #include <pcl/registration/gicp.h> // GICP 算法核心 #include <pcl/registration/icp.h> // ICP 基类（部分功能依赖） #include <pcl/features/normal_3d.h> // 法向量估计（GICP 需要） #include <pcl/filters/voxel_grid.h> // 下采样（可选） #include <pcl/io/pcd_io.h> // PCD 文件读写 参数配置 参数 值 说明 setMaximumIterations 500 最大迭代次数，达到后停止配准 setTransformationEpsilon 1e-10 变换矩阵变化的收敛阈值，小于此值认为已收敛 setEuclideanFitnessEpsilon 0.0001 点到点欧氏距离误差的收敛阈值 setMaxCorrespondenceDistance 50.0 对应点搜索半径上限，超过则不考虑 setCorrespondenceRandomness 20 每次迭代随机采样对应点数量，影响计算效率和精度 setRANSACOutlierRejectionThreshold 0.05 RANSAC 离群点阈值，超过此距离的点被标记为异常值 setRANSACIterations 1000 RANSAC 迭代次数 setTransformationRotationEpsilon 1e-10 旋转矩阵变化的收敛阈值 setUseReciprocalCorrespondences false 是否使用互对应关系（source-target双向匹配） 测试结果 通过将目标点云变换作为原始点云进行测试，实际变换为 T=[10, 10, 10]m, R=[90, 0, 0]deg。 ...