📚 课程目录第3章基于NDT的激光雷达定位

3. 基于NDT的激光雷达定位

任务目标

本案例基于Autoware Universe环境，使用NDT（Normal Distributions Transform）扫描匹配算法，将Velodyne VLP-16激光雷达的实时点云与预构建的高精NDT地图进行配准，输出车辆在全局坐标系下的6自由度位姿（x, y, z, roll, pitch, yaw），并在RViz中可视化定位结果与点云匹配效果。

环境与依赖

Ubuntu 22.04 + ROS 2 Humble + Autoware Universe（>= 2024.03）
已安装的组件：autoware_ndt_scan_matcher、autoware_point_cloud_preprocessor、autoware_utils
传感器驱动：velodyne_driver（或已录制的rosbag）
高精地图：NDT格式点云地图（PCD文件，已通过NDT体素滤波下采样至约0.3m分辨率）

实现方案

采用Autoware官方提供的ndt_scan_matcher节点作为核心定位引擎。该节点内部使用PCL的NDT算法，将当前帧激光点云与静态NDT地图进行配准。系统输入为：/sensing/lidar/top/pointcloud（原始点云）、/map/pointcloud_map（NDT地图）、/localization/initial_pose（初始位姿）；输出为/localization/pose_estimator/pose（定位结果）。

关键配置包括：NDT分辨率（ndt_resolution）设为1.0m，最大迭代次数50次，变换容差0.01。为保证实时性，使用体素滤波将输入点云下采样至0.2m分辨率。初始位姿由GNSS或手动2D Pose Estimate提供，本案例使用RViz手动给定。

关键完整代码

以下为最小可运行的launch文件与参数配置，用于启动NDT定位节点并连接地图与传感器数据：

<!-- ndt_localization.launch.py -->
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os

def generate_launch_description():
    map_pcd_path = LaunchConfiguration('map_pcd_path')
    return LaunchDescription([
        DeclareLaunchArgument(
            'map_pcd_path',
            default_value='/home/user/maps/sample_map.pcd',
            description='Path to NDT map PCD file'),
        
        # 地图加载节点：将PCD发布为点云话题
        Node(
            package='autoware_point_cloud_preprocessor',
            executable='point_cloud_map_loader',
            name='point_cloud_map_loader',
            parameters=[{'map_path': map_pcd_path}],
            remappings=[('point_cloud_map', '/map/pointcloud_map')]
        ),
        
        # NDT扫描匹配节点
        Node(
            package='autoware_ndt_scan_matcher',
            executable='ndt_scan_matcher_node',
            name='ndt_scan_matcher',
            parameters=[{
                'ndt_resolution': 1.0,
                'max_iterations': 50,
                'transformation_epsilon': 0.01,
                'voxel_leaf_size': 0.2,
                'use_initial_pose_estimation': True
            }],
            remappings=[
                ('/sensing/lidar/top/pointcloud', '/sensing/lidar/top/pointcloud'),
                ('/map/pointcloud_map', '/map/pointcloud_map'),
                ('/localization/initial_pose', '/initialpose'),
                ('/localization/pose_estimator/pose', '/localization/pose_estimator/pose')
            ]
        ),
        
        # RViz可视化
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', os.path.join('/home/user/autoware_launch/config', 'ndt_demo.rviz')]
        )
    ])

核心NDT配准逻辑由ndt_scan_matcher_node内部实现，其关键处理流程为：接收当前帧点云→体素滤波→NDT配准（以初始位姿为起点）→输出位姿协方差。若需查看或修改NDT核心算法，源码位于autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp，其中ndt_.align(*output_cloud, initial_guess)为配准核心调用。

运行与验证

准备地图：将NDT格式PCD地图放置于指定路径，确保地图坐标系为ENU（东-北-天）。
启动定位：

source /opt/autoware/setup.bash
ros2 launch ndt_localization.launch.py map_pcd_path:=/path/to/your_map.pcd

播放传感器数据（新终端）：

ros2 bag play sensor_data.bag

提供初始位姿：在RViz中点击“2D Pose Estimate”按钮，在地图上车辆实际位置附近点击并拖拽方向，发布/initialpose。

通过判定标准：

RViz中/localization/pose_estimator/pose的箭头（表示车辆位姿）随点云移动而更新，且不出现跳变或漂移；
当前帧点云（红色）与地图点云（绿色）在可视化中基本重合，横向偏差小于0.5m；
终端无NDT配准失败或协方差过大警告（协方差位置标准差<1.0m）。

常见问题

定位发散：检查初始位姿是否大致正确（误差<5m），NDT为局部配准算法，初始误差过大无法收敛。
地图不匹配：确认地图与实时点云使用相同坐标系（ENU），且地图经过下采样（体素大小0.3~1.0m），过密地图会导致计算缓慢。
实时性不足：若处理帧率低于10Hz，增大voxel_leaf_size至0.3m，或降低ndt_resolution至2.0m（精度略有下降）。