Apollo Localization 定位模块接口调用流程图与源码分析

---

  团队博客: 汽车电子社区

---

一、模块概述

  Localization模块是Apollo自动驾驶系统的"内置GPS"，负责精确确定车辆在世界坐标系中的位置和姿态。该模块通过融合多种传感器数据（GNSS、IMU、LiDAR等）提供高精度、高可靠性的定位服务，是整个自动驾驶系统的坐标基准。

二、模块架构

2.1 目录结构

modules/localization/
├── common/                     # 定位通用组件
├── msf/                       # 多传感器融合定位
│   ├── dev/                   # 开发版本
│   └── stateful/              # 状态化版本
├── ndt/                       # NDT定位算法
├── proto/                     # 消息定义
├── rtk/                       # RTK定位
├── localization_component/     # 定位主组件
└── tools/                     # 工具

2.2 核心组件

  1. LocalizationComponent - 定位主组件
  2. MSFLocalization - 多传感器融合定位
  3. NDTLocalizer - NDT定位器
  4. RTKLocalizer - RTK定位器
  5. TransformWrapper - 坐标变换封装器

三、接口调用流程图

3.1 整体定位流程图

3.2 MSF多传感器融合流程图

3.3 NDT点云配准流程图

3.4 RTK定位流程图

四、关键源码分析

4.1 主组件源码分析

4.1.1 MSFLocalizationComponent 多传感器融合定位组件

  位置: modules/localization/msf/msf_localization_component.h

  核心功能:
    - 继承自 cyber::Component<drivers::gnss::Imu>，以IMU数据作为触发
    - 融合GNSS、IMU、LiDAR等多传感器数据进行高精度定位
    - 发布定位结果和状态信息

  关键成员变量:

class MSFLocalizationComponent final : public cyber::Component<drivers::gnss::Imu> {
 private:
  // 订阅者 - 接收传感器数据
  std::shared_ptr<cyber::Reader<drivers::PointCloud>> lidar_listener_;      // 激光雷达数据
  std::shared_ptr<cyber::Reader<drivers::gnss::GnssBestPose>> bestgnsspos_listener_;  // GNSS最优位置
  std::shared_ptr<cyber::Reader<drivers::gnss::Heading>> gnss_heading_listener_;         // GNSS航向

  // 核心定位对象
  std::shared_ptr<LocalizationMsgPublisher> publisher_;            // 消息发布器
  MSFLocalization localization_;                               // 多传感器融合定位器
};

  LocalizationMsgPublisher 消息发布器:

class LocalizationMsgPublisher {
 private:
  std::shared_ptr<cyber::Node> node_;                        // Cyber节点

  // 发布者
  std::shared_ptr<cyber::Writer<LocalizationEstimate>> localization_talker_;      // 主定位结果
  std::shared_ptr<cyber::Writer<LocalizationEstimate>> lidar_local_talker_;    // LiDAR定位结果
  std::shared_ptr<cyber::Writer<LocalizationEstimate>> gnss_local_talker_;     // GNSS定位结果
  std::shared_ptr<cyber::Writer<LocalizationStatus>> localization_status_talker_; // 定位状态

  // GNSS补偿器
  LocalizationGnssCompensator* localization_gnss_compensator_;

  // TF广播器
  TransformBroadcaster tf2_broadcaster_;
};

  初始化流程 (Init() 方法):

bool MSFLocalizationComponent::Init() {
  // 1. 创建消息发布器
  publisher_.reset(new LocalizationMsgPublisher(this->node_));

  // 2. 初始化配置
  if (!InitConfig()) {
    AERROR << "Init Config failed.";
    return false;
  }

  // 3. 初始化IO
  if (!InitIO()) {
    AERROR << "Init IO failed.";
    return false;
  }

  return true;
}

  IO初始化 (InitIO() 方法):

bool MSFLocalizationComponent::InitIO() {
  // 1. 创建LiDAR数据订阅者
  cyber::ReaderConfig reader_config;
  reader_config.channel_name = lidar_topic_;
  reader_config.pending_queue_size = 1;

  std::function<void(const std::shared_ptr<drivers::PointCloud>&)> lidar_register_call = 
      std::bind(&MSFLocalization::OnPointCloud, &localization_, std::placeholders::_1);
  lidar_listener_ = this->node_->CreateReader<drivers::PointCloud>(reader_config, lidar_register_call);

  // 2. 创建GNSS最优位置订阅者
  std::function<void(const std::shared_ptr<drivers::gnss::GnssBestPose>&)> bestgnsspos_register_call = 
      std::bind(&MSFLocalization::OnGnssBestPose, &localization_, std::placeholders::_1);
  bestgnsspos_listener_ = this->node_->CreateReader<drivers::gnss::GnssBestPose>(bestgnsspos_topic_, bestgnsspos_register_call);

  // 3. 创建GNSS航向订阅者
  std::function<void(const std::shared_ptr<drivers::gnss::Heading>&)> gnss_heading_call = 
      std::bind(&MSFLocalization::OnGnssHeading, &localization_, std::placeholders::_1);
  gnss_heading_listener_ = this->node_->CreateReader<drivers::gnss::Heading>(gnss_heading_topic_, gnss_heading_call);

  // 4. 初始化发布器
  if (!publisher_->InitIO()) {
    AERROR << "Init publisher io failed.";
    return false;
  }

  // 5. 设置发布器
  localization_.SetPublisher(publisher_);

  return true;
}

  主处理流程 (Proc() 方法):

bool MSFLocalizationComponent::Proc(const std::shared_ptr<drivers::gnss::Imu>& imu_msg) {
  // 处理IMU数据，MSFLocalization内部会触发融合计算
  localization_.OnRawImuCache(imu_msg);
  return true;
}

  消息发布:

void LocalizationMsgPublisher::PublishPoseBroadcastTF(const LocalizationEstimate& localization) {
  // 1. 创建TF变换消息
  apollo::transform::TransformStamped tf2_msg;
  
  // 2. 设置时间戳
  uint64_t current_send_tf_time = apollo::cyber::Clock::Now().ToNanosecond();
  uint64_t measurement_time = apollo::cyber::Time(localization.measurement_time()).ToNanosecond();

  // 3. 设置变换关系
  tf2_msg.mutable_header()->set_timestamp_sec(current_send_tf_time / 1e9);
  tf2_msg.mutable_header()->set_frame_id(broadcast_tf_frame_id_);
  tf2_msg.set_child_frame_id(broadcast_tf_child_frame_id_);

  // 4. 设置变换数据
  auto mutable_transform = tf2_msg.mutable_transform();
  mutable_transform->mutable_translation()->set_x(localization.pose().position().x());
  mutable_transform->mutable_translation()->set_y(localization.pose().position().y());
  mutable_transform->mutable_translation()->set_z(localization.pose().position().z());

  // 5. 设置旋转四元数
  auto mutable_rotation = mutable_transform->mutable_rotation();
  mutable_rotation->set_qx(localization.pose().orientation().qx());
  mutable_rotation->set_qy(localization.pose().orientation().qy());
  mutable_rotation->set_qz(localization.pose().orientation().qz());
  mutable_rotation->set_qw(localization.pose().orientation().qw());

  // 6. 发布TF
  tf2_broadcaster_.SendTransform(tf2_msg);

  // 7. 发布定位消息
  localization_talker_->Write(std::make_shared<LocalizationEstimate>(localization));
}

4.1.2 MSFLocalization 多传感器融合算法

  核心算法特点:
    - EKF融合: 使用扩展卡尔曼滤波器进行多传感器融合
    - 状态预测: 基于IMU进行运动状态预测
    - 观测更新: 使用GNSS和LiDAR进行观测更新
    - 自适应权重: 根据传感器质量动态调整融合权重
    - 容错处理: 传感器失效时的降级处理

4.1.3 NDTLocalizationComponent NDT定位组件

  位置: modules/localization/ndt/ndt_localization_component.h

  核心功能:
    - 继承自 cyber::Component<localization::Gps>，以GPS数据作为触发
    - 使用NDT算法进行点云配准定位
    - 支持基于高精地图的精确定位

  关键成员变量:

class NDTLocalizationComponent final : public cyber::Component<localization::Gps> {
 private:
  // 订阅者
  std::shared_ptr<cyber::Reader<drivers::PointCloud>> lidar_listener_;              // 激光雷达数据
  std::shared_ptr<cyber::Reader<drivers::gnss::InsStat>> odometry_status_listener_;  // 里程计状态

  // 发布者
  std::shared_ptr<cyber::Writer<LocalizationEstimate>> localization_talker_;     // 主定位结果
  std::shared_ptr<cyber::Writer<LocalizationEstimate>> lidar_pose_talker_;     // LiDAR定位结果
  std::shared_ptr<cyber::Writer<LocalizationStatus>> localization_status_talker_; // 定位状态

  // 话题名称
  std::string lidar_topic_;
  std::string odometry_topic_;
  std::string localization_topic_;
};

  NDT算法核心:
    - 正态分布变换: 将点云配准问题转换为概率分布匹配
    - 迭代优化: 使用牛顿法优化配准变换
    - 鲁棒性: 对噪声和遮挡具有良好的鲁棒性
    - 精度: 能够达到厘米级的定位精度

4.2 定位算法分析

4.2.1 EKF扩展卡尔曼滤波

  状态空间模型:

// 状态向量: [x, y, z, vx, vy, vz, roll, pitch, yaw, wx, wy, wz]
// 控制向量: [ax, ay, az, wx_imu, wy_imu, wz_imu]
// 观测向量: GNSS位置、LiDAR配准结果

  预测步骤:
    1. IMU积分: 使用IMU测量值更新运动状态
    2. 误差传播: 通过状态转移矩阵传播协方差
    3. 时间更新: 更新预测时间戳

  更新步骤:
    1. 观测计算: 计算GNSS和LiDAR观测值
    2. 卡尔曼增益: 计算最优增益矩阵
    3. 状态修正: 根据观测修正预测状态
    4. 协方差更新: 更新状态协方差矩阵

4.2.2 NDT点云配准算法

  算法原理:

// 1. 构建目标分布
for each cell in point_cloud:
    mean_cell = calculate_mean(points_in_cell)
    cov_cell = calculate_covariance(points_in_cell)
    
// 2. 正态分布变换
transform_score = 0
for each point in source_cloud:
    transformed_point = transform(point)
    score = normal_distribution_pdf(transformed_point, mean_cell, cov_cell)
    transform_score += score

// 3. 优化变换矩阵
optimal_transform = optimize_transform(transform_score)

4.3 消息接口定义

4.3.1 LocalizationEstimate消息

  位置: modules/common_msgs/localization_msgs/localization.proto

message LocalizationEstimate {
  // 消息头
  apollo.common.Header header = 1;
  
  // 位置信息
  apollo.common.PointENU pose = 2;              // 位置和姿态
  apollo.common.PointENU uncertainty = 3;          // 不确定性
  
  // 时间信息
  double measurement_time = 4;                      // 测量时间
  
  // 定位质量
  LocalizationQuality quality = 5;                  // 定位质量
  
  // 融合信息
  repeated LocalizationSource sources = 6;           // 数据源信息
  
  // 状态信息
  bool is_gps_valid = 7;                          // GPS是否有效
  bool is_lidar_valid = 8;                        // LiDAR是否有效
  bool is_fusion_valid = 9;                       // 融合是否有效
  
  // 运动状态
  apollo.common.Twist linear_velocity = 10;          // 线速度
  apollo.common.Twist angular_velocity = 11;         // 角速度
  
  // 加速度
  apollo.common.Acceleration3D linear_acceleration = 12; // 线加速度
  
  // 位置协方差
  apollo.common.Point3D position_covariance = 13;     // 位置协方差
  apollo.common.Point3D orientation_covariance = 14;  // 姿态协方差
}

enum LocalizationQuality {
  UNKNOWN = 0;                     // 未知
  GOOD = 1;                        // 良好
  WARN = 2;                        // 警告
  BAD = 3;                         // 差
  DEAD = 4                          // 失效
}

message LocalizationSource {
  string source_name = 1;              // 数据源名称
  double timestamp = 2;                // 时间戳
  apollo.common.PointENU pose = 3;     // 位置姿态
  double quality = 4;                 // 质量
}

4.4 配置文件分析

4.4.1 MSF配置文件

  配置: modules/localization/conf/msf_localization.pb.txt

msf_localization {
  # 传感器配置
  lidar_height: 1.8                    # LiDAR安装高度
  lidar_offset_x: 0.0                  # LiDAR X向偏移
  lidar_offset_y: 0.0                  # LiDAR Y向偏移
  
  # EKF参数
  process_noise: [0.1, 0.1, 0.2,      # 过程噪声
                0.5, 0.5, 0.8,
                0.3, 0.3, 0.3,
                1.0, 1.0, 1.0]
                
  # GNSS配置
  gnss_std: [0.02, 0.02, 0.05]      # GNSS标准差
  gnss_cov_factor: 1.0                # GNSS协方差因子
  
  # LiDAR配置
  lidar_std: [0.01, 0.01, 0.01]      # LiDAR标准差
  lidar_cov_factor: 1.0               # LiDAR协方差因子
  
  # 融合权重
  gnss_weight: 0.6                    # GNSS权重
  lidar_weight: 0.4                    # LiDAR权重
  
  # 质量判断
  max_position_std: 2.0               # 最大位置标准差
  max_orientation_std: 5.0              # 最大姿态标准差
  min_valid_sensor_count: 1             # 最小有效传感器数
}

# NDT配置
ndt_localization {
  # 点云预处理
  voxel_grid_size: 0.5                 # 体素网格大小
  min_points_per_voxel: 3              # 每个体素最小点数
  
  # 配准参数
  max_iterations: 100                   # 最大迭代次数
  transformation_epsilon: 0.001         # 变换收敛阈值
  euclidean_fitness_epsilon: 0.001       # 欧氏距离收敛阈值
  
  # 地图配置
  map_resolution: 0.1                  # 地图分辨率
  search_radius: 10.0                   # 搜索半径
}

4.4.2 DAG配置文件

  启动配置: modules/localization/dag/dag_streaming_msf_localization.dag

# 定义组件
component {
  name: "MSFLocalizationComponent"
  config_path: "conf/localization"
  config_name: "msf_localization.pb.txt"
}

# 定义调度器
scheduler {
  type: "SCHEDULER_CHOREOGRAPHER"
  routine: "CYBER_CPU"
  priority: 2
}

# 定义数据服务
data_service {
  readers: [
    {
      channel: "/apollo/sensor/gnss/best_pose"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    },
    {
      channel: "/apollo/sensor/gnss/heading"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    },
    {
      channel: "/apollo/sensor/lidar/PointCloud2"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    }
  ]
  writers: [
    {
      channel: "/apollo/localization/pose"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    },
    {
      channel: "/apollo/localization/msf_gnss"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    },
    {
      channel: "/apollo/localization/msf_lidar"
      qos_profile: {
        reliability: "RELIABILITY_RELIABLE"
        durability: "DURABILITY_VOLATILE"
      }
    }
  ]
}

五、性能优化特点

5.1 实时性能保障

  - 高频更新: 定位结果以高频发布(通常10Hz)
  - 增量更新: EKF采用增量式状态更新
  - 内存优化: 点云数据使用体素网格下采样
  - 并行处理: 多传感器数据并行处理

5.2 算法优化

  - 数值稳定: 使用数值稳定的卡尔曼滤波实现
  - 收敛保证: NDT算法具有全局收敛性
  - 自适应参数: 根据环境动态调整算法参数
  - 缓存机制: 缓存中间计算结果避免重复计算

5.3 精度保障

  - 多源验证: 使用多个独立数据源相互验证
  - 不确定性建模: 精确建模和传播不确定性
  - 质量评估: 实时评估定位质量和可靠性
  - 故障检测: 快速检测和隔离故障传感器

5.4 容错机制

  - 传感器冗余: 多传感器提供冗余信息
  - 降级策略: 传感器失效时的自动降级
  - 故障恢复: 从暂时的传感器故障中自动恢复
  - 平滑过渡: 传感器切换时的平滑过渡

六、总结

  Localization模块通过以下关键技术实现了高精度的车辆定位：

    1. 多传感器融合: EKF融合GNSS、IMU、LiDAR等多种传感器
    2. 先进算法: 使用NDT等先进的配准算法
    3. 不确定性建模: 精确建模和传播定位不确定性
    4. 实时性能: 优化的算法确保实时性要求
    5. 高可靠性: 多重保障机制确保定位的可靠性

  该模块为整个自动驾驶系统提供精确的时间和空间基准，是感知、规划、控制等模块能够正确工作的基础保障。通过持续的算法优化和多传感器融合，能够实现厘米级的定位精度和毫秒级的时间同步。