基于ROS2实现自动驾驶的路径规划与导航，需结合感知、定位、规划、控制等模块，核心依赖Nav2（ROS2导航栈） 完成标准化流程。以下是具体实现步骤，涵盖环境搭建、模块配置到系统集成的全流程：

步骤1：环境搭建与依赖准备

核心任务：搭建ROS2开发环境，安装导航所需的基础库和工具。

1. 安装ROS2
   选择稳定版本（如Humble或Iron），参考ROS2官方文档完成安装（包括ros-humble-desktop及构建工具colcon）。

2. 安装导航核心依赖
   Nav2是ROS2的官方导航栈，包含路径规划、定位、行为树等组件，需安装：

   sudo apt install ros-humble-navigation2 ros-humble-nav2-bringup
   sudo apt install ros-humble-slam-toolbox  # 用于SLAM建图
   sudo apt install ros-humble-teleop-twist-keyboard  # 手动控制工具（调试用）
   

3. 创建工作空间

   mkdir -p ~/autodrive_ws/src
   cd ~/autodrive_ws
   colcon build
   source install/setup.bash
   

步骤2：传感器数据采集与预处理

核心任务：通过传感器（激光雷达、相机、IMU等）获取环境数据，并转换为ROS2标准消息格式。

1. 传感器硬件连接与驱动配置

   • 激光雷达：常用思岚A1/A2、RPLIDAR等，安装对应ROS2驱动（如rplidar_ros），确保发布/scan话题（消息类型sensor_msgs/LaserScan）。
   • IMU：发布/imu话题（消息类型sensor_msgs/Imu），提供角速度和加速度数据。
   • 轮式里程计：通过底盘编码器计算速度，发布/odom话题（消息类型nav_msgs/Odometry），包含位置和速度信息。

   示例：启动激光雷达驱动

   ros2 launch rplidar_ros rplidar.launch.py  # 具体launch文件根据驱动包调整
   

2. 坐标系（TF）配置
   导航需统一坐标系，关键坐标系包括：

   • map：全局地图坐标系（固定）；
   • odom：里程计坐标系（随运动累积误差）；
   • base_link：机器人底盘中心坐标系（与机器人刚性连接）；
   • 传感器坐标系（如laser_link、imu_link），需通过tf2发布到base_link的变换。

   实现方式：通过static_transform_publisher发布静态变换（如激光雷达相对于base_link的位置），或在URDF模型中定义（推荐）。
   示例URDF片段（激光雷达安装位置）：

   <link name="laser_link"/>
   <joint name="laser_joint" type="fixed">
     <parent link="base_link"/>
     <child link="laser_link"/>
     <origin xyz="0.1 0 0.2" rpy="0 0 0"/>  <!-- 激光雷达在base_link前方0.1m、上方0.2m -->
   </joint>
   

3. 数据预处理（可选）

   • 激光雷达：过滤噪声点（如通过laser_filters包移除远距离无效点）；
   • 相机：通过image_proc进行畸变校正，或结合depth_image_proc处理深度图。

步骤3：地图创建与管理

核心任务：通过SLAM构建环境地图，用于后续定位和路径规划。

1. 在线SLAM建图
   使用slam_toolbox（ROS2推荐的SLAM工具），结合激光雷达和里程计数据构建2D栅格地图。

   • 启动SLAM节点：

     ros2 launch slam_toolbox online_async_launch.py  # 异步SLAM（实时性好）
     

   • 手动控制机器人运动：通过teleop-twist-keyboard控制机器人遍历环境，SLAM会实时更新地图。

     ros2 run teleop_twist_keyboard teleop_twist_keyboard
     

2. 保存地图
   建图完成后，通过nav2_map_server保存地图（.pgm图像文件和.yaml配置文件）：

   ros2 run nav2_map_server map_saver_cli -f ~/autodrive_ws/src/my_robot/maps/my_map
   

   地图文件需包含分辨率（如0.05m/像素）、原点坐标等信息，后续导航需加载此地图。

步骤4：定位系统实现

核心任务：基于已知地图，通过传感器数据估计机器人在map坐标系中的实时位置。

ROS2中常用AMCL（自适应蒙特卡洛定位），基于粒子滤波匹配激光雷达数据与地图，实现定位。

1. 配置AMCL参数
   创建amcl.yaml参数文件（存放于config目录），关键参数包括：

   amcl:
     ros__parameters:
       use_sim_time: False  # 非仿真环境设为False
       laser_model_type: "likelihood_field"  # 激光匹配模型
       min_particles: 500    # 粒子数量（环境复杂时增大）
       max_particles: 2000
       initial_pose_x: 0.0   # 初始位置（与地图原点对齐）
       initial_pose_y: 0.0
       initial_pose_a: 0.0
       update_min_d: 0.2     # 移动超过0.2m时更新定位
       update_min_a: 0.5     # 旋转超过0.5rad时更新定位
   

2. 启动定位节点
   通过launch文件集成AMCL、地图服务器（加载地图）和TF变换：

   # launch/amcl_launch.py
   from launch import LaunchDescription
   from launch_ros.actions import Node
   
   def generate_launch_description():
       return LaunchDescription([
           # 加载地图
           Node(
               package='nav2_map_server',
               executable='map_server',
               parameters=[{'yaml_filename': '/path/to/my_map.yaml'}]
           ),
           # 定位（AMCL）
           Node(
               package='nav2_amcl',
               executable='amcl',
               parameters=['/path/to/amcl.yaml']
           ),
           # TF变换（odom -> map，由AMCL发布）
           Node(
               package='tf2_ros',
               executable='static_transform_publisher',
               arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']  # 初始变换（后续由AMCL更新）
           )
       ])
   

   启动后，AMCL会发布/amcl_pose话题（geometry_msgs/PoseWithCovarianceStamped），包含机器人在map中的位置估计。

步骤5：路径规划模块配置

核心任务：基于定位结果和地图，规划从起点到目标点的路径（全局路径+局部避障）。

Nav2的路径规划由全局规划器（Global Planner） 和局部规划器（Local Planner） 组成，通过行为树（Behavior Tree） 协调复杂导航逻辑（如避障、重试）。

1. 配置规划器参数
   创建nav2_params.yaml配置文件，定义全局/局部规划器及行为树参数：

   # 全局规划器（如Navfn或SmacPlanner）
   planner_server:
     ros__parameters:
       planner_plugins: ["GridBased"]
       GridBased:
         plugin: "nav2_navfn_planner/NavfnPlanner"  # 基于Dijkstra/A*的全局规划
         tolerance: 0.5  # 目标点容差（m）
   
   # 局部规划器（如DWB或TEB）
   controller_server:
     ros__parameters:
       controller_plugins: ["FollowPath"]
       FollowPath:
         plugin: "nav2_dwb_controller/DWBController"  # 动态窗口法局部避障
         max_vel_x: 0.5  # 最大线速度（m/s）
         min_vel_x: 0.0
         max_vel_theta: 1.0  # 最大角速度（rad/s）
         min_vel_theta: -1.0
         obstacle_distance: 0.3  # 避障安全距离（m）
   
   # 行为树配置（定义导航流程：定位->规划->执行->避障）
   bt_navigator:
     ros__parameters:
       bt_xml_filename: "nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
   

2. 启动规划器节点
   通过launch文件集成规划器、控制器和行为树节点：

   # launch/navigation_launch.py
   from launch import LaunchDescription
   from launch_ros.actions import Node
   
   def generate_launch_description():
       return LaunchDescription([
           # 全局规划器
           Node(
               package='nav2_planner',
               executable='planner_server',
               parameters=['/path/to/nav2_params.yaml']
           ),
           # 局部控制器
           Node(
               package='nav2_controller',
               executable='controller_server',
               parameters=['/path/to/nav2_params.yaml']
           ),
           # 行为树导航器
           Node(
               package='nav2_bt_navigator',
               executable='bt_navigator',
               parameters=['/path/to/nav2_params.yaml']
           ),
           # 生命周期管理器（管理导航节点的启动/关闭）
           Node(
               package='nav2_lifecycle_manager',
               executable='lifecycle_manager',
               parameters=[{'node_names': ['map_server', 'amcl', 'planner_server', 'controller_server', 'bt_navigator']}]
           )
       ])
   

步骤6：运动控制与执行

核心任务：将规划器输出的速度指令（cmd_vel）发送到底盘，驱动机器人运动。

1. 底盘驱动适配
   底盘需订阅/cmd_vel话题（消息类型geometry_msgs/Twist），包含线速度（linear.x）和角速度（angular.z）。

   • 若使用开源底盘（如TurtleBot3），其驱动已支持cmd_vel；
   • 自定义底盘需编写驱动节点，将Twist消息转换为电机控制信号（如PWM）。

2. 指令安全校验（可选）
   为避免危险速度，可添加限速节点（如twist_mux），过滤超阈值的cmd_vel指令。

步骤7：系统集成与测试调优

核心任务：整合所有模块，通过仿真或实车测试验证导航功能，并优化参数。

1. 全系统启动
   编写总launch文件，一次性启动传感器驱动、定位、规划、控制等节点：

   # launch/autodrive_launch.py
   from launch import LaunchDescription
   from launch_include import include_launch_description
   from launch.launch_description_sources import PythonLaunchDescriptionSource
   
   def generate_launch_description():
       return LaunchDescription([
           # 传感器驱动（激光雷达、IMU等）
           include_launch_description(
               PythonLaunchDescriptionSource('/path/to/sensor_launch.py')
           ),
           # 定位（AMCL+地图）
           include_launch_description(
               PythonLaunchDescriptionSource('/path/to/amcl_launch.py')
           ),
           # 导航规划（规划器+控制器）
           include_launch_description(
               PythonLaunchDescriptionSource('/path/to/navigation_launch.py')
           ),
           # 底盘驱动
           Node(
               package='my_chassis',
               executable='chassis_driver'
           )
       ])
   

   启动系统：

   ros2 launch my_robot autodrive_launch.py
   

2. 导航测试

   • 设置目标点：通过RViz的2D Goal Pose工具在地图上点击目标点，Nav2会自动规划路径并导航。
   • 监控话题：通过ros2 topic list查看关键话题（/plan全局路径、/local_plan局部路径、/cmd_vel速度指令），确认数据正常。

3. 参数调优
   若出现定位漂移、路径卡顿或避障失败，需调整核心参数：

   • 定位：增大AMCL粒子数量（复杂环境）、减小update_min_d（提高更新频率）；
   • 全局规划：调整tolerance（目标点精度）、切换规划器（如SmacPlanner支持平滑路径）；
   • 局部规划：降低max_vel_x（提高稳定性）、增大obstacle_distance（避障安全裕度）；
   • 工具：使用rqt_reconfigure实时调整参数，rviz2可视化定位、路径和传感器数据。

总结

基于ROS2的自动驾驶导航实现流程可概括为：
环境搭建→传感器数据采集→SLAM建图→AMCL定位→Nav2路径规划→底盘控制→测试调优。
核心依赖Nav2的标准化组件，通过参数配置和TF协调，可快速适配不同机器人平台。实际应用中需根据场景（如室内/室外、低速/高速）优化传感器选型和参数，确保导航的稳定性和安全性。