多传感器融合建图及定位的工程化落地方案，多线雷达+rtk；室内室外导航都适用。 包含部署文档和代码注释；包含工程落地角度的优化。 不含运动控制。

室外场景用RTK信号稳如老狗，一进厂房立马抓瞎；多线雷达在室内横扫千军，到了开阔地带反而容易丢帧——这事儿咱们做机器人的谁没经历过？今天聊的这套方案就是把两个传感器的屎都吃干净，把糖留下来。

硬件组合拳：16线雷达+双天线RTK

选型上别整花活，Velodyne VLP-16这种工业级雷达虽然线数不高，但胜在性价比和抗造。RTK模块建议走双天线定向方案，比如司南的K803T，直接输出带航向角的定位数据。重点在于物理安装时，必须保证雷达和RTK天线的相对位置固定，我习惯用3D打印个支架把两者刚性连接。

时间同步是第一个坑，分享个取巧方法：

# 雷达驱动片段
def sync_callback(cloud_msg, rtk_msg):
    # 对齐时间戳，允许50ms误差
    if abs(cloud_msg.header.stamp - rtk_msg.header.stamp) < 0.05:
        fused_data = do_fusion(cloud_msg, rtk_msg)
        # 时间补偿因子动态调整
        self.time_compensation *= 0.9

这种软同步方案比硬同步省事，实测在车速5m/s内误差可控。注意补偿因子要放在配置文件中，不同场景动态调整。

坐标系战争与卡尔曼魔法

传感器坐标系不统一会要命。建议在启动时做一次手眼标定：

// 标定核心逻辑
Eigen::Matrix4d calibrateSensor(PointCloud& radar_pts, GPSPoint& gps_pts) {
    // 选取三个特征点建立对应关系
    for(int i=0; i<3; ++i){
        A_matrix.block<3,1>(i*3,0) = radar_pts[i].cast<double>();
        B_matrix.block<3,1>(i*3,0) = gps_pts[i].cast<double>();
    }
    return KabschAlgorithm(A_matrix, B_matrix); // 这才是灵魂
}

标定完成后把变换矩阵烧写到设备的EEPROM里，下次上电自动读取。实际部署时建议做个地面标记物，让设备自动检测标定点。

滤波算法别整太复杂，我推荐改进版ESKF：

class ESKF:
    def predict(self, imu_data):
        # 状态传播只用加速度计和陀螺仪
        self.x = self._motion_model(self.x, imu_data)
        # 协方差预测加入IMU内参误差
        self.P = self.F @ self.P @ self.F.T + self.Q * imu_data.temp

    def update(self, sensor_type, data):
        if sensor_type == 'RTK':
            # RTK观测时降低过程噪声
            self.Q[:3,:3] *= 0.1
            H = self.H_rtk
        elif sensor_type == 'Radar':
            # 雷达更新时增大位置权重
            H = self.H_radar
            H[3:, :] *= 2.0
        # 自适应卡尔曼增益更新...

这种设计让RTK主要修正全局位置，雷达负责姿态微调。实测在隧道场景，当RTK失锁时系统能自动切换为雷达主导模式。

工程化生存指南

内存管理是第一个拦路虎。处理16线雷达数据时，试试这个内存池技巧：

class PointCloudPool {
public:
    PointCloudPtr getCloud() {
        if(!pool_.empty()){
            auto cloud = pool_.back();
            pool_.pop_back();
            return cloud;
        }
        return boost::make_shared<PointCloud>();
    }
    
    void recycle(PointCloudPtr cloud) {
        if(pool_.size() < 10){
            cloud->clear();
            pool_.push_back(cloud);
        }
    }
private:
    std::vector<PointCloudPtr> pool_;
};

对象复用比频繁申请内存快3倍以上，特别是在ARM工控机上效果显著。记得在消息回调里立即归还内存。

多传感器融合建图及定位的工程化落地方案，多线雷达+rtk；室内室外导航都适用。 包含部署文档和代码注释；包含工程落地角度的优化。 不含运动控制。

算法加速方面别迷信GPU，多试试OpenMP并行：

#pragma omp parallel for num_threads(4)
for(size_t i=0; i<cloud->size(); ++i){
    auto& pt = cloud->points[i];
    pt.x = transform_matrix(0,0)*pt.x + ...;
    // 自动处理数据竞争？不存在的，每个点独立变换
}

对于万级点云，四线程能砍掉40%的处理时间。注意线程数要根据CPU核心数动态配置，别写死在代码里。

部署时踩过的坑

1. 供电问题：雷达+RTK+工控机的峰值电流能到5A，普通USB线扛不住。建议用带电流保护的主动供电Hub

1. 电磁干扰：RTK天线别和电机驱动放一起，至少隔30cm。曾有个项目因此定位漂移10米

1. 温度补偿：-20℃环境里雷达需要预热5分钟，在启动脚本里加个延迟：

#!/bin/bash
sleep 300  # 工业设备开机先暖和会儿
roslaunch mapping_kit cold_start.launch

1. 日志要带传感器状态：

logger.info(f"[RTK]状态:{rtk_status} 卫星数:{gps_sats} 航向:{heading:.2f}")
logger.warning(f"雷达丢帧: {lost_frame} 次/分钟")

用ELK收集日志，设置阈值自动报警，比人肉查日志高效得多。

完整的部署文档建议用mkdocs生成，结构可以这样：

docs/
   ├── hardware_guide.md
   ├── calibration_procedure.md
   └── troubleshooting.md

重点记录客户现场遇到的奇葩问题，比如某钢铁厂的地磁干扰解决方案，某物流仓库的金属货架滤波参数等。

代码注释别写废话，要这样写：

// 此处不能用互斥锁！会引发雷达数据堆积
// 改用无锁队列，详见RFC-789解决方案
void enqueueCloud(const PointCloud& cloud) {
    queue_.push_non_blocking(cloud); 
}

把设计决策背后的原因写清楚，方便后人维护。

这套方案在多个AGV项目上跑过，从-30℃的冷库到50℃的炼钢车间都扛住了。最后说个真事：有次调试时RTK天线被叉车撞歪了15度，系统居然靠雷达把姿态纠回来了——这大概就是融合的魅力吧。