Autoware NDT 独立定位模块

这是一个从 Autoware Core 中剥离出来的 ROS 2 NDT 定位工作空间，面向不运行完整
Autoware、但希望在自己的机器人或仿真环境中使用 Autoware NDT 定位的项目。

项目已将点云地图加载、NDT scan matching、EKF 融合，以及 Livox IMU、底盘里程计
到 Autoware 定位接口的转换整合到一套 launch 中。

GitHub： 2089887529/autowera_ndt

[!WARNING]
本项目用于研发、仿真和实车测试，不是经过功能安全认证的定位系统。实车运行前必须
完成传感器外参标定、地图检查、初始位姿确认和场地安全测试。

1. 项目说明

完整 Autoware 工程规模较大。如果机器人只需要使用点云地图进行 NDT 定位，直接部署
整套 Autoware 会引入很多暂时用不到的模块。本仓库保留了定位所需的核心包，并增加了
适配普通 ROS 2 机器人传感器接口的桥接节点。

从 Autoware Core 1.9.0 剥离的包包括：

包	作用
autoware_map_loader	加载 PCD 点云地图，并向 NDT 提供差分地图服务
autoware_ndt_scan_matcher	将实时激光点云与地图匹配，输出 NDT 位姿
autoware_ekf_localizer	融合 NDT 位姿和车体速度，输出连续定位结果

本项目新增的包：

包	作用
rd_autoware_localization_bridge	点云预处理、Livox IMU 转换、Odom/IMU 桥接、自动初始化及总 launch

这里的“剥离”是指不需要启动完整 Autoware 系统，并不表示三个 Autoware 包已经变成
完全零依赖的软件。它们仍需要 Autoware 的公共消息、工具库以及 ROS 2/PCL 依赖。

2. 系统架构

主要处理过程：

1. Map Loader 读取 PCD 地图；
2. 原始点云经过距离裁剪、体素降采样和可选限频；
3. NDT 使用 EKF 预测位姿作为初始猜测，将当前点云与地图配准；
4. Livox IMU 和底盘 Odom 被转换成 EKF 所需的速度输入；
5. EKF 融合 NDT 位姿与车体速度，输出连续的定位结果。

3. 环境要求

推荐环境：

• Ubuntu 22.04；
• ROS 2 Humble；
• GCC/G++、CMake、C++17；
• colcon、rosdep；
• PCL；
• 与本仓库包版本兼容的 Autoware Humble 公共依赖；
• 包含 x/y/z 字段的 PCD 点云地图。

先安装 ROS 2 常用构建工具：

sudo apt update
sudo apt install -y \
  python3-colcon-common-extensions \
  python3-rosdep \
  libpcl-dev

如果系统还没有初始化 rosdep：

sudo rosdep init
rosdep update

rosdep init 每台机器只需要执行一次。如果提示配置文件已经存在，直接执行
rosdep update 即可。

4. 下载源码

git clone https://github.com/2089887529/autowera_ndt.git autoware_ndt
cd autoware_ndt

如果 Autoware 公共依赖安装在其他工作空间，先加载它：

source /opt/ros/humble/setup.bash
source /path/to/autoware_dependency_ws/install/setup.bash

然后安装当前工作空间能够通过 rosdep 获取的依赖：

rosdep install --from-paths src --ignore-src -r -y --rosdistro humble

检查依赖：

rosdep check --from-paths src --ignore-src

如果提示缺少 autoware_* 公共包，需要先安装完整 Autoware 环境，或把对应依赖源码
加入当前/下层 overlay；仅安装标准 ROS 2 Humble 不能保证包含全部 Autoware 依赖。

5. 编译

source /opt/ros/humble/setup.bash
# 使用外部 Autoware 依赖时，还要 source 它的 install/setup.bash

colcon build --symlink-install \
  --packages-select \
    autoware_map_loader \
    autoware_ndt_scan_matcher \
    autoware_ekf_localizer \
    rd_autoware_localization_bridge \
  --cmake-args -DCMAKE_BUILD_TYPE=Release

source install/setup.bash

NDT 必须优先使用 Release 构建。未启用编译优化时，单帧匹配耗时可能从毫秒级增加到
数百毫秒甚至更长。

确认加载的是当前工作空间：

ros2 pkg prefix autoware_ndt_scan_matcher
ros2 pkg prefix rd_autoware_localization_bridge

输出路径应位于当前工作空间的 install/ 目录。

6. 运行前配置

6.1 点云地图

启动时必须通过 pointcloud_map_path 指定 PCD 地图的绝对路径：

/absolute/path/to/map.pcd

单个 PCD 可以不提供 metadata，Map Loader 会直接读取点云并计算地图边界。分块 PCD
地图需要提供对应的 metadata YAML。

地图与实时点云必须使用相同的尺度和坐标定义。PCD 至少应包含：

x  y  z

地图文件默认不提交到 Git 仓库，避免大型 PCD 占用 Git 历史。如确实需要发布地图，
建议使用 Git LFS 或单独提供下载链接。

6.2 TF 坐标关系

默认坐标系：

map
 └── base_link
      ├── <lidar_frame>
      └── <imu_frame>

• map -> base_link：由 NDT/EKF 定位系统估计；
• base_link -> lidar_frame：由 URDF 或静态 TF 提供；
• base_link -> imu_frame：由 URDF 或静态 TF 提供。

检查 TF：

ros2 run tf2_ros tf2_echo base_link <lidar_frame>
ros2 run tf2_ros tf2_echo base_link <imu_frame>

没有 URDF 时，可以临时发布静态 TF。下面的数值必须替换为真实标定外参：

ros2 run tf2_ros static_transform_publisher \
  --x <x> --y <y> --z <z> \
  --roll <roll> --pitch <pitch> --yaw <yaw> \
  --frame-id base_link --child-frame-id <lidar_frame>

6.3 传感器话题

默认接口如下：

数据	默认话题	消息类型
激光点云	/livox/lidar	sensor_msgs/msg/PointCloud2
Livox IMU	/livox/imu	sensor_msgs/msg/Imu
底盘里程计	/odom	nav_msgs/msg/Odometry

启动前检查消息和 frame_id：

ros2 topic echo /livox/lidar --once --field header
ros2 topic echo /livox/imu --once --field header
ros2 topic echo /odom --once --field header

如果实际话题不同，通过 launch 参数修改，不需要改源码。

6.4 IMU 与 Odom 桥接参数

桥接配置文件位于
src/rd_autoware_localization_bridge/config/bridge.param.yaml。

常用参数：

参数	默认值	说明
acceleration_scale	9.80665	将 Livox 加速度从 g 转换为 m/s²
target_frame	base_link	IMU 输出坐标系
use_imu_angular_velocity	true	使用 IMU 角速度覆盖 Odom 角速度
max_imu_age_sec	0.1	Odom 可使用的最大 IMU 时间差
linear_velocity_scale	1.0	底盘线速度比例修正
max_abs_linear_velocity	8.0	允许的最大绝对线速度，单位 m/s
output_rate_limit_hz	30.0	EKF Twist 输出频率上限

当前桥接器只使用 Odom 中的速度，不会把 Odom 累计位姿当成 map 坐标系的全局位姿。

7. 一键启动

7.1 Gazebo / 仿真

先启动仿真、机器人模型、传感器和 TF，然后运行：

source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 launch rd_autoware_localization_bridge ndt_localization.launch.py \
  pointcloud_map_path:=/absolute/path/to/map.pcd \
  points_topic:=/livox/lidar \
  raw_imu_topic:=/livox/imu \
  odom_topic:=/odom \
  initial_x:=-15.0 \
  initial_y:=0.0 \
  initial_z:=0.0 \
  initial_yaw:=1.57 \
  use_sim_time:=true

7.2 实车

实车使用系统时间，并根据车辆在地图中的真实位置设置初始位姿：

source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 launch rd_autoware_localization_bridge ndt_localization.launch.py \
  pointcloud_map_path:=/absolute/path/to/map.pcd \
  points_topic:=/livox/lidar \
  raw_imu_topic:=/livox/imu \
  odom_topic:=/odom_raw \
  initial_x:=<map_x> \
  initial_y:=<map_y> \
  initial_yaw:=<yaw_rad> \
  ndt_resolution:=1.0 \
  use_sim_time:=false

服务器或无图形界面运行时添加：

launch_rviz:=false

查看 launch 的全部参数：

ros2 launch rd_autoware_localization_bridge ndt_localization.launch.py --show-args

8. NDT 参数调节

Launch 参数	默认值	作用
pointcloud_leaf_size	0.15	输入点云体素降采样尺寸，单位 m
pointcloud_min_range	1.0	删除雷达附近点，单位 m
pointcloud_max_range	100.0	最大点云使用距离，单位 m
pointcloud_max_rate_hz	0.0	点云最高频率；0 表示不主动限频
map_visualization_leaf_size	0.1	RViz 地图降采样，不影响 NDT 匹配
ndt_resolution	1.5	NDT 目标地图体素尺寸，单位 m
ndt_trans_epsilon	0.1	迭代收敛阈值
ndt_max_iterations	40	单帧最大迭代次数
ndt_num_threads	4	NDT OpenMP 线程数
ndt_score_threshold	0.9	NDT 位姿最低可信分数

常用调节思路：

• CPU 占用过高：增大 pointcloud_leaf_size、减小
  pointcloud_max_range，或设置较低的 pointcloud_max_rate_hz；
• 地图细节丰富但定位不稳定：尝试减小 ndt_resolution，同时重新标定分数阈值；
• 匹配经常达到最大迭代次数：检查初始位姿、TF 和地图，而不是只增加迭代次数；
• RViz 卡顿：增大 map_visualization_leaf_size，不会改变 NDT 使用的原始地图。

参数示例：

ros2 launch rd_autoware_localization_bridge ndt_localization.launch.py \
  pointcloud_map_path:=/absolute/path/to/map.pcd \
  pointcloud_leaf_size:=0.25 \
  pointcloud_max_range:=60.0 \
  pointcloud_max_rate_hz:=10.0 \
  ndt_resolution:=1.0 \
  ndt_num_threads:=4

9. 主要输出与检查

话题/服务	用途
/localization/points_filtered	NDT 使用的预处理点云
/map/pointcloud_map_visualization	RViz 显示用降采样地图
/ndt_pose	NDT 位姿
/ndt_pose_with_covariance	带协方差的 NDT 位姿
/ekf_pose_with_covariance	EKF 融合定位结果
/points_aligned	NDT 对齐后的点云
/diagnostics	Autoware 定位诊断信息
/ekf/trigger_node	EKF 激活服务
/ndt/trigger_node	NDT 激活服务

运行状态检查：

ros2 topic hz /localization/points_filtered
ros2 topic hz /ndt_pose_with_covariance
ros2 topic hz /ekf_pose_with_covariance
ros2 topic echo /diagnostics --once
ros2 run tf2_ros tf2_echo map base_link

RViz 默认配置会显示地图、NDT 对齐点云、RobotModel、Axes 和 TF。定位成功后，
/ndt_pose_with_covariance 与 /ekf_pose_with_covariance 应持续输出。

10. 常见问题

编译时找不到 Autoware 包

确认先加载了 Autoware 依赖工作空间：

source /opt/ros/humble/setup.bash
source /path/to/autoware_dependency_ws/install/setup.bash

再清理当前构建产物并重新编译。不要把其他机器生成的 build/、install/、log/
目录提交或复制过来。

地图加载失败

• 确认 pointcloud_map_path 使用绝对路径；
• 确认文件存在且当前用户有读取权限；
• 确认 PCD 至少包含 x/y/z 字段；
• 分块地图必须提供正确的 metadata YAML。

NDT 没有输出

依次检查：

1. 原始点云是否持续发布；
2. 点云 frame_id 到 base_link 的 TF 是否存在；
3. PCD 地图是否成功加载；
4. 初始位姿是否落在地图有效区域；
5. /ekf/trigger_node 和 /ndt/trigger_node 是否已激活；
6. /diagnostics 中是否存在地图范围、超时或分数异常。

仿真中没有输出

确认 /clock 正常发布，并且所有相关节点的 use_sim_time 都为 true：

ros2 topic echo /clock --once
ros2 param get /ndt_scan_matcher use_sim_time

11. 更多文档

• RD 桥接包、参数和调优说明
• NDT 分终端启动与排查流程
• Autoware NDT Scan Matcher 上游文档
• Autoware Map Loader 上游文档
• Autoware EKF Localizer 上游文档

12. 上游项目与许可证

Autoware 包来源：
autowarefoundation/autoware_core

NDT-OMP 来源：
koide3/ndt_omp

三个 autoware_* 包声明版本为 1.9.0。仓库整体采用 Apache License 2.0；NDT 实现中
还包含 BSD-2-Clause 许可的 ndt_omp 代码。详细版权与来源见
LICENSE 和 THIRD_PARTY_NOTICES.md。

Autoware 是 Autoware Foundation 的项目。本仓库是面向特定机器人集成的独立工程，
不是 Autoware Foundation 的官方发行版。