Autoware NDT 独立定位模块

YQ_012026-08-23 14:22

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. 系统架构

#mermaid-svg-ETKxOFkYFBA0Xrpx{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}@keyframes edge-animation-frame{from{stroke-dashoffset:0;}}@keyframes dash{to{stroke-dashoffset:0;}}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-animation-slow{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 50s linear infinite;stroke-linecap:round;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-animation-fast{stroke-dasharray:9,5!important;stroke-dashoffset:900;animation:dash 20s linear infinite;stroke-linecap:round;}#mermaid-svg-ETKxOFkYFBA0Xrpx .error-icon{fill:#552222;}#mermaid-svg-ETKxOFkYFBA0Xrpx .error-text{fill:#552222;stroke:#552222;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-thickness-normal{stroke-width:1px;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-thickness-invisible{stroke-width:0;fill:none;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-ETKxOFkYFBA0Xrpx .marker{fill:#333333;stroke:#333333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .marker.cross{stroke:#333333;}#mermaid-svg-ETKxOFkYFBA0Xrpx svg{font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-ETKxOFkYFBA0Xrpx p{margin:0;}#mermaid-svg-ETKxOFkYFBA0Xrpx .label{font-family:"trebuchet ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .cluster-label text{fill:#333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .cluster-label span{color:#333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .cluster-label span p{background-color:transparent;}#mermaid-svg-ETKxOFkYFBA0Xrpx .label text,#mermaid-svg-ETKxOFkYFBA0Xrpx span{fill:#333;color:#333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .node rect,#mermaid-svg-ETKxOFkYFBA0Xrpx .node circle,#mermaid-svg-ETKxOFkYFBA0Xrpx .node ellipse,#mermaid-svg-ETKxOFkYFBA0Xrpx .node polygon,#mermaid-svg-ETKxOFkYFBA0Xrpx .node path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-ETKxOFkYFBA0Xrpx .rough-node .label text,#mermaid-svg-ETKxOFkYFBA0Xrpx .node .label text,#mermaid-svg-ETKxOFkYFBA0Xrpx .image-shape .label,#mermaid-svg-ETKxOFkYFBA0Xrpx .icon-shape .label{text-anchor:middle;}#mermaid-svg-ETKxOFkYFBA0Xrpx .node .katex path{fill:#000;stroke:#000;stroke-width:1px;}#mermaid-svg-ETKxOFkYFBA0Xrpx .rough-node .label,#mermaid-svg-ETKxOFkYFBA0Xrpx .node .label,#mermaid-svg-ETKxOFkYFBA0Xrpx .image-shape .label,#mermaid-svg-ETKxOFkYFBA0Xrpx .icon-shape .label{text-align:center;}#mermaid-svg-ETKxOFkYFBA0Xrpx .node.clickable{cursor:pointer;}#mermaid-svg-ETKxOFkYFBA0Xrpx .root .anchor path{fill:#333333!important;stroke-width:0;stroke:#333333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .arrowheadPath{fill:#333333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edgePath .path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-ETKxOFkYFBA0Xrpx .flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edgeLabel{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-ETKxOFkYFBA0Xrpx .edgeLabel p{background-color:rgba(232,232,232, 0.8);}#mermaid-svg-ETKxOFkYFBA0Xrpx .edgeLabel rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-ETKxOFkYFBA0Xrpx .labelBkg{background-color:rgba(232, 232, 232, 0.5);}#mermaid-svg-ETKxOFkYFBA0Xrpx .cluster rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-ETKxOFkYFBA0Xrpx .cluster text{fill:#333;}#mermaid-svg-ETKxOFkYFBA0Xrpx .cluster span{color:#333;}#mermaid-svg-ETKxOFkYFBA0Xrpx div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80, 100%, 96.2745098039%);border:1px solid #aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-ETKxOFkYFBA0Xrpx .flowchartTitleText{text-anchor:middle;font-size:18px;fill:#333;}#mermaid-svg-ETKxOFkYFBA0Xrpx rect.text{fill:none;stroke-width:0;}#mermaid-svg-ETKxOFkYFBA0Xrpx .icon-shape,#mermaid-svg-ETKxOFkYFBA0Xrpx .image-shape{background-color:rgba(232,232,232, 0.8);text-align:center;}#mermaid-svg-ETKxOFkYFBA0Xrpx .icon-shape p,#mermaid-svg-ETKxOFkYFBA0Xrpx .image-shape p{background-color:rgba(232,232,232, 0.8);padding:2px;}#mermaid-svg-ETKxOFkYFBA0Xrpx .icon-shape .label rect,#mermaid-svg-ETKxOFkYFBA0Xrpx .image-shape .label rect{opacity:0.5;background-color:rgba(232,232,232, 0.8);fill:rgba(232,232,232, 0.8);}#mermaid-svg-ETKxOFkYFBA0Xrpx .label-icon{display:inline-block;height:1em;overflow:visible;vertical-align:-0.125em;}#mermaid-svg-ETKxOFkYFBA0Xrpx .node .label-icon path{fill:currentColor;stroke:revert;stroke-width:revert;}#mermaid-svg-ETKxOFkYFBA0Xrpx :root{--mermaid-font-family:"trebuchet ms",verdana,arial,sans-serif;} TwistWithCovariance
PoseWithCovariance
预测位姿
PCD 点云地图
Autoware Map Loader
Autoware NDT
Livox / PointCloud2
裁剪、降采样、限频
Livox IMU
单位与坐标系转换
Odom + IMU Bridge
底盘 Odometry
Autoware EKF
map → base_link 定位结果

主要处理过程：

  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 常用构建工具：

bash 复制代码 
sudo apt update
sudo apt install -y \
  python3-colcon-common-extensions \
  python3-rosdep \
  libpcl-dev

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

bash 复制代码 
sudo rosdep init
rosdep update

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

rosdep update 即可。

4. 下载源码

bash 复制代码 
git clone https://github.com/2089887529/autowera_ndt.git autoware_ndt
cd autoware_ndt

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

bash 复制代码 
source /opt/ros/humble/setup.bash
source /path/to/autoware_dependency_ws/install/setup.bash

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

bash 复制代码 
rosdep install --from-paths src --ignore-src -r -y --rosdistro humble

检查依赖：

bash 复制代码 
rosdep check --from-paths src --ignore-src

如果提示缺少 autoware_* 公共包，需要先安装完整 Autoware 环境，或把对应依赖源码

加入当前/下层 overlay；仅安装标准 ROS 2 Humble 不能保证包含全部 Autoware 依赖。

5. 编译

bash 复制代码 
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 构建。未启用编译优化时，单帧匹配耗时可能从毫秒级增加到

数百毫秒甚至更长。

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

bash 复制代码 
ros2 pkg prefix autoware_ndt_scan_matcher
ros2 pkg prefix rd_autoware_localization_bridge

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

6. 运行前配置

6.1 点云地图

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

text 复制代码 
/absolute/path/to/map.pcd

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

地图需要提供对应的 metadata YAML。

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

text 复制代码 
x  y  z

地图文件默认不提交到 Git 仓库，避免大型 PCD 占用 Git 历史。如确实需要发布地图，

建议使用 Git LFS 或单独提供下载链接。

6.2 TF 坐标关系

默认坐标系：

text 复制代码 
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：

bash 复制代码 
ros2 run tf2_ros tf2_echo base_link <lidar_frame>
ros2 run tf2_ros tf2_echo base_link <imu_frame>

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

bash 复制代码 
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：

bash 复制代码 
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，然后运行：

bash 复制代码 
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 实车

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

bash 复制代码 
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

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

text 复制代码 
launch_rviz:=false

查看 launch 的全部参数：

bash 复制代码 
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 使用的原始地图。

参数示例：

bash 复制代码 
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 激活服务

运行状态检查：

bash 复制代码 
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 依赖工作空间：

bash 复制代码 
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：

bash 复制代码 
ros2 topic echo /clock --once
ros2 param get /ndt_scan_matcher use_sim_time

11. 更多文档

12. 上游项目与许可证

Autoware 包来源：

autowarefoundation/autoware_core

NDT-OMP 来源：

koide3/ndt_omp

三个 autoware_* 包声明版本为 1.9.0。仓库整体采用 Apache License 2.0；NDT 实现中

还包含 BSD-2-Clause 许可的 ndt_omp 代码。详细版权与来源见

和 <THIRD_PARTY_NOTICES.md>。

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

不是 Autoware Foundation 的官方发行版。

相关推荐
机器人梦想家1 小时前
具身机器人视觉服务的异步回调与并发控制
数据库·算法·机器人
TMT星球1 小时前
伽利略机器人亮相WRC 2026，突破形态局限发布“陆行具身系统”Galileo X
大数据·人工智能·机器人
老兵发新帖1 小时前
快速理解机器人各个坐标系的关系
机器人
老兵发新帖2 小时前
机器人坐标系：各坐标系分别是谁在用，会影响谁？
机器人
极客互动API4 小时前
企业微信 iPad 协议消息接口开发：文本 / 图片 / 群发消息的统一封装
人工智能·ios·微信·机器人·企业微信·ipad
老兵发新帖6 小时前
机器人坐标系中：odom主要是做代价地图用？
机器人
老兵发新帖6 小时前
机器人坐标系：那map到底是地图还是坐标系？
机器人
飞英思特无线充电16 小时前
严苛场景巡检机器人的无线补能设计：点位、BMS、认证与运维边界
运维·机器人
DogDaoDao1 天前
Magma：微软如何用一个模型打通数字与物理世界的 AI Agent
人工智能·微软·机器人·大模型·机器人模型·智能体·magma
热门推荐
01国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）02GitHub 镜像站点03微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新042026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？05历年考研数学一、数学二、数学三真题试卷及答案PDF06Agnes AI 免费 API 接入指南：文本、生图、生视频，一套接口全免费07AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路082026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片09VSCode+CodeX扩展，无法启动10如何新建文件夹？ 电脑新建文件夹的4种方法