以下是一个基于ROS2和Nav2的室内自主导航机器人项目案例，该案例实现了一个能够在室内环境中自主建图、定位、规划路径并避开障碍物的移动机器人系统。

项目概述：室内自主导航机器人

本项目使用ROS2 Humble和Nav2导航框架，构建一个能够在办公室或家庭环境中自主移动的机器人系统。机器人可以通过SLAM构建环境地图，然后基于地图实现从起点到目标点的自主导航，并能实时避开动态障碍物。

硬件组成

• 差速驱动移动底盘（带编码器）
• RPLIDAR A1激光雷达
• IMU传感器（MPU6050）
• 树莓派4B（主控）
• 12V锂电池（供电）
• 超声波传感器（可选，辅助避障）

软件环境

• ROS2 Humble
• Nav2导航栈
• SLAM Toolbox
• RPLIDAR ROS2驱动
• Python 3.10

系统架构

系统主要包含以下模块：

1. 传感器数据采集模块（激光雷达、IMU、里程计）
2. 地图构建模块（SLAM）
3. 定位模块（AMCL）
4. 路径规划模块（全局规划+局部规划）
5. 运动控制模块（底盘驱动）
6. 用户交互模块（目标点设置）

具体实现步骤

步骤1：创建工作空间和项目结构

mkdir -p ~/nav_robot_ws/src
cd ~/nav_robot_ws/src
ros2 pkg create nav_robot --build-type ament_python
mkdir -p nav_robot/launch nav_robot/config nav_robot/maps nav_robot/scripts

步骤2：配置机器人模型（URDF）

创建nav_robot/urdf/nav_robot.urdf文件，定义机器人模型：

<?xml version="1.0"?>
<robot name="nav_robot">
  <!-- 底盘链接 -->
  <link name="base_link">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <inertial>
      <mass value="5.0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.15"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 0.8 1"/>
      </material>
    </visual>
  </link>

  <!-- 激光雷达链接 -->
  <link name="laser_link">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <inertial>
      <mass value="0.2"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.05"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
  </link>

  <!-- 激光雷达关节（安装在底盘上方） -->
  <joint name="laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="laser_link"/>
    <origin xyz="0 0 0.2" rpy="0 0 0"/>
  </joint>

  <!-- 左车轮链接 -->
  <link name="left_wheel_link">
    <origin xyz="0 0 0" rpy="0 1.5708 0"/>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 1.5708 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.03"/>
      </geometry>
      <material name="gray">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
  </link>

  <!-- 右车轮链接 -->
  <link name="right_wheel_link">
    <origin xyz="0 0 0" rpy="0 1.5708 0"/>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 1.5708 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.03"/>
      </geometry>
      <material name="gray">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
  </link>

  <!-- 左车轮关节 -->
  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <origin xyz="0 0.16 0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- 右车轮关节 -->
  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <origin xyz="0 -0.16 0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- IMU链接 -->
  <link name="imu_link">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>

  <!-- IMU关节 -->
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
  </joint>
</robot>

步骤3：配置传感器驱动

创建激光雷达和IMU的启动文件nav_robot/launch/sensors_launch.py：

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    urdf_path = PathJoinSubstitution(
        [FindPackageShare('nav_robot'), 'urdf', 'nav_robot.urdf']
    )
    
    return LaunchDescription([
        # 发布机器人模型
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            arguments=[str(urdf_path)]
        ),
        
        # 激光雷达驱动
        Node(
            package='rplidar_ros',
            executable='rplidar_composition',
            name='rplidar_node',
            parameters=[{
                'serial_port': '/dev/ttyUSB0',
                'serial_baudrate': 115200,
                'frame_id': 'laser_link',
                'inverted': False,
                'angle_compensate': True,
            }],
            output='screen'
        ),
        
        # IMU驱动
        Node(
            package='mpu6050_serial_driver',
            executable='mpu6050_serial_driver_node',
            name='imu_node',
            parameters=[{
                'serial_port': '/dev/ttyUSB1',
                'serial_baudrate': 115200,
                'frame_id': 'imu_link',
                'publish_rate': 100.0,
            }],
            output='screen'
        ),
        
        # 里程计节点
        Node(
            package='nav_robot',
            executable='odometry_node',
            name='odometry_node',
            output='screen'
        )
    ])

步骤4：实现里程计节点

创建nav_robot/scripts/odometry_node.py文件，处理轮式里程计数据：

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    urdf_path = PathJoinSubstitution(
        [FindPackageShare('nav_robot'), 'urdf', 'nav_robot.urdf']
    )
    
    return LaunchDescription([
        # 发布机器人模型
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            arguments=[str(urdf_path)]
        ),
        
        # 激光雷达驱动
        Node(
            package='rplidar_ros',
            executable='rplidar_composition',
            name='rplidar_node',
            parameters=[{
                'serial_port': '/dev/ttyUSB0',
                'serial_baudrate': 115200,
                'frame_id': 'laser_link',
                'inverted': False,
                'angle_compensate': True,
            }],
            output='screen'
        ),
        
        # IMU驱动
        Node(
            package='mpu6050_serial_driver',
            executable='mpu6050_serial_driver_node',
            name='imu_node',
            parameters=[{
                'serial_port': '/dev/ttyUSB1',
                'serial_baudrate': 115200,
                'frame_id': 'imu_link',
                'publish_rate': 100.0,
            }],
            output='screen'
        ),
        
        # 里程计节点
        Node(
            package='nav_robot',
            executable='odometry_node',
            name='odometry_node',
            output='screen'
        )
    ])

步骤5：创建SLAM建图启动文件

创建nav_robot/launch/slam_launch.py文件，用于启动SLAM建图：

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution

def generate_launch_description():
    # 传感器启动文件
    sensors_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('nav_robot'),
                'launch',
                'sensors_launch.py'
            ])
        ])
    )
    
    # SLAM Toolbox 启动文件
    slam_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('slam_toolbox'),
                'launch',
                'online_async_launch.py'
            ])
        ]),
        launch_arguments={
            'use_sim_time': 'false',
            'slam_params_file': PathJoinSubstitution([
                FindPackageShare('nav_robot'),
                'config',
                'slam_params.yaml'
            ])
        }.items()
    )
    
    # 键盘控制节点
    teleop_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('teleop_twist_keyboard'),
                'launch',
                'teleop-launch.py'
            ])
        ])
    )
    
    return LaunchDescription([
        sensors_launch,
        slam_launch,
        teleop_node
    ])

步骤6：配置Nav2导航参数

创建nav_robot/config/nav2_params.yaml文件，配置Nav2导航参数：

amcl:
  ros__parameters:
    use_sim_time: false
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    laser_model_type: "likelihood_field"
    laser_z_hit: 0.5
    laser_z_short: 0.05
    laser_z_max: 0.05
    laser_z_rand: 0.5
    laser_sigma_hit: 0.2
    laser_likelihood_max_dist: 2.0
    min_particles: 500
    max_particles: 2000
    kld_err: 0.05
    kld_z: 0.99
    update_min_d: 0.2
    update_min_a: 0.5
    tf_broadcast: true
    initial_pose_x: 0.0
    initial_pose_y: 0.0
    initial_pose_a: 0.0
    scan_topic: /scan

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner/NavfnPlanner"
      tolerance: 0.5
      use_astar: true
      allow_unknown: true

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "nav2_dwb_controller/DWBController"
      debug_trajectory_details: true
      min_vel_x: 0.0
      max_vel_x: 0.5
      min_vel_y: 0.0
      max_vel_y: 0.0
      max_vel_theta: 1.0
      min_vel_theta: -1.0
      acc_lim_x: 1.0
      acc_lim_y: 0.0
      acc_lim_theta: 1.0
      decel_lim_x: 1.0
      decel_lim_y: 0.0
      decel_lim_theta: 1.0
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
      cost_factor: 3.0
      oscillation_cost_factor: 10.0
      oscillation_reset_dist: 0.05

bt_navigator:
  ros__parameters:
    use_sim_time: false
    default_bt_xml_filename: "nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
    plugin_lib_names:
      - "nav2_compute_path_to_pose_action_bt_node"
      - "nav2_follow_path_action_bt_node"
      - "nav2_back_up_action_bt_node"
      - "nav2_spin_action_bt_node"
      - "nav2_wait_action_bt_node"
      - "nav2_clear_costmap_service_bt_node"
      - "nav2_is_stuck_condition_bt_node"
      - "nav2_goal_reached_condition_bt_node"
      - "nav2_goal_updated_condition_bt_node"
      - "nav2_initial_pose_received_condition_bt_node"
      - "nav2_reinitialize_global_localization_service_bt_node"
      - "nav2_rate_controller_bt_node"
      - "nav2_distance_controller_bt_node"
      - "nav2_speed_controller_bt_node"
      - "nav2_truncate_path_action_bt_node"
      - "nav2_goal_updater_node_bt_node"
      - "nav2_recovery_node_bt_node"
      - "nav2_pipeline_sequence_bt_node"
      - "nav2_round_robin_node_bt_node"
      - "nav2_transform_available_condition_bt_node"
      - "nav2_time_expired_condition_bt_node"
      - "nav2_path_expiring_timer_condition_bt_node"
      - "nav2_distance_traveled_condition_bt_node"

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    recovery_plugins: ["spin", "back_up", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    back_up:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: false
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 1.0

map_server:
  ros__parameters:
    use_sim_time: false
    yaml_filename: "maps/my_map.yaml"

costmap_filter_info_server:
  ros__parameters:
    use_sim_time: false

步骤7：创建导航启动文件

创建nav_robot/launch/navigation_launch.py文件，启动完整的导航系统：

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # 声明地图参数
    map_arg = DeclareLaunchArgument(
        'map',
        default_value=PathJoinSubstitution([
            FindPackageShare('nav_robot'),
            'maps',
            'my_map.yaml'
        ]),
        description='Full path to map yaml file'
    )
    
    # 传感器启动文件
    sensors_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('nav_robot'),
                'launch',
                'sensors_launch.py'
            ])
        ])
    )
    
    # Nav2导航启动文件
    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([
                FindPackageShare('nav2_bringup'),
                'launch',
                'navigation_launch.py'
            ])
        ]),
        launch_arguments={
            'map': LaunchConfiguration('map'),
            'use_sim_time': 'false',
            'params_file': PathJoinSubstitution([
                FindPackageShare('nav_robot'),
                'config',
                'nav2_params.yaml'
            ])
        }.items()
    )
    
    # 底盘驱动节点
    chassis_driver = Node(
        package='nav_robot',
        executable='chassis_driver',
        name='chassis_driver',
        output='screen'
    )
    
    return LaunchDescription([
        map_arg,
        sensors_launch,
        nav2_launch,
        chassis_driver
    ])

步骤8：实现底盘驱动节点

创建nav_robot/scripts/chassis_driver.py文件，控制机器人底盘：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import serial
import time

class ChassisDriver(Node):
    def __init__(self):
        super().__init__('chassis_driver')
        
        # 串口配置
        self.declare_parameter('serial_port', '/dev/ttyACM0')
        self.declare_parameter('baud_rate', 115200)
        
        serial_port = self.get_parameter('serial_port').get_parameter_value().string_value
        baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
        
        # 初始化串口
        try:
            self.ser = serial.Serial(serial_port, baud_rate, timeout=0.1)
            time.sleep(2)  # 等待串口初始化
            self.get_logger().info(f"成功连接到底盘串口: {serial_port}")
        except Exception as e:
            self.get_logger().error(f"无法连接到底盘串口: {e}")
            self.ser = None
        
        # 订阅速度指令
        self.cmd_vel_sub = self.create_subscription(
            Twist, 'cmd_vel', self.cmd_vel_callback, 10)
        
        # 安全停止定时器
        self.safety_timer = self.create_timer(0.5, self.safety_check)
        self.last_cmd_time = self.get_clock().now()

    def cmd_vel_callback(self, msg):
        # 记录最后一次指令时间
        self.last_cmd_time = self.get_clock().now()
        
        if self.ser is None:
            return
        
        # 提取线速度和角速度
        linear_x = msg.linear.x
        angular_z = msg.angular.z
        
        # 转换为左右轮速度（差速模型）
        # 这里使用简单的线性转换，实际应根据机器人参数调整
        left_speed = linear_x - (angular_z * 0.32) / 2.0  # 0.32是轮距
        right_speed = linear_x + (angular_z * 0.32) / 2.0
        
        # 限制最大速度
        max_speed = 0.5
        left_speed = max(-max_speed, min(left_speed, max_speed))
        right_speed = max(-max_speed, min(right_speed, max_speed))
        
        # 发送速度指令到底盘（自定义协议）
        cmd = f"V{left_speed:.2f},{right_speed:.2f}\n"
        try:
            self.ser.write(cmd.encode())
            self.get_logger().debug(f"发送指令: {cmd.strip()}")
        except Exception as e:
            self.get_logger().error(f"发送指令失败: {e}")

    def safety_check(self):
        # 如果超过1秒没有收到指令，停止机器人
        current_time = self.get_clock().now()
        if (current_time - self.last_cmd_time).nanoseconds / 1e9 > 1.0:
            if self.ser is not None:
                try:
                    self.ser.write(b"V0.00,0.00\n")
                    self.get_logger().debug("安全停止指令发送")
                except Exception as e:
                    self.get_logger().error(f"发送停止指令失败: {e}")

    def destroy_node(self):
        # 关闭时停止机器人
        if self.ser is not None and self.ser.is_open:
            self.ser.write(b"V0.00,0.00\n")
            self.ser.close()
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    chassis_driver = ChassisDriver()
    rclpy.spin(chassis_driver)
    chassis_driver.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

步骤9：实现导航目标点客户端

创建nav_robot/scripts/navigate_to_goal.py文件，用于发送导航目标点：

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
import tf_transformations

class NavigateToGoalClient(Node):
    def __init__(self):
        super().__init__('navigate_to_goal_client')
        self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
        self.get_logger().info("导航目标客户端已启动，等待导航服务器...")

    def send_goal(self, x, y, yaw):
        # 等待导航服务器
        if not self._action_client.wait_for_server(timeout_sec=10.0):
            self.get_logger().error("导航服务器不可用！")
            return False
        
        # 创建目标消息
        goal_msg = NavigateToPose.Goal()
        
        # 设置目标点位姿
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        
        # 位置
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.position.z = 0.0
        
        # 朝向（将yaw角转换为四元数）
        quaternion = tf_transformations.quaternion_from_euler(0, 0, yaw)
        goal_msg.pose.pose.orientation.x = quaternion[0]
        goal_msg.pose.pose.orientation.y = quaternion[1]
        goal_msg.pose.pose.orientation.z = quaternion[2]
        goal_msg.pose.pose.orientation.w = quaternion[3]
        
        self.get_logger().info(f"发送导航目标: x={x}, y={y}, yaw={yaw}")
        
        # 发送目标并注册回调
        self._send_goal_future = self._action_client.send_goal_async(
            goal_msg,
            feedback_callback=self.feedback_callback
        )
        
        self._send_goal_future.add_done_callback(self.goal_response_callback)
        return True

    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info("导航目标被服务器拒绝！")
            return
        
        self.get_logger().info("导航目标已被接受，开始导航...")
        
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger().info(f"导航完成，结果: {result}")
        rclpy.shutdown()

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
        current_pose = feedback.current_pose.pose.position
        self.get_logger().info(f"当前位置: x={current_pose.x:.2f}, y={current_pose.y:.2f}")

def main(args=None):
    rclpy.init(args=args)
    
    # 解析命令行参数
    import sys
    if len(sys.argv) < 4:
        print(f"使用方法: {sys.argv[0]} x y yaw")
        print("示例: navigate_to_goal 2.0 1.5 0.0")
        return
    
    try:
        x = float(sys.argv[1])
        y = float(sys.argv[2])
        yaw = float(sys.argv[3])
    except ValueError:
        print("参数必须为数字！")
        return
    
    client = NavigateToGoalClient()
    client.send_goal(x, y, yaw)
    
    rclpy.spin(client)

if __name__ == '__main__':
    main()

项目运行与测试

1. 编译项目

cd ~/nav_robot_ws
colcon build
source install/setup.bash

2. 构建地图

# 启动SLAM建图
ros2 launch nav_robot slam_launch.py

# 在新终端中启动RViz2
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav_robot)/share/nav_robot/config/nav2_default_view.rviz

# 使用键盘控制机器人移动建图
# 在新终端中
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# 保存地图
ros2 run nav2_map_server map_saver_cli -f ~/nav_robot_ws/src/nav_robot/maps/my_map

3. 启动导航系统

# 启动完整导航系统
ros2 launch nav_robot navigation_launch.py

# 在新终端中启动RViz2
ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav_robot)/share/nav_robot/config/nav2_default_view.rviz

4. 发送导航目标

# 发送目标点（x=2.0, y=1.5, yaw=0.0）
ros2 run nav_robot navigate_to_goal.py 2.0 1.5 0.0

项目扩展方向

1. 添加视觉传感器，实现基于视觉的障碍物检测
2. 集成语音控制模块，实现语音指令导航
3. 开发图形化界面，方便用户设置多个导航点和路径规划
4. 增加电池监控和自动充电功能
5. 实现多机器人协同导航

这个项目案例展示了如何使用ROS2和Nav2构建一个完整的自主导航机器人系统，涵盖了从硬件配置、软件架构到代码实现的各个方面。通过这个案例，你可以了解ROS2导航系统的核心组件和工作原理，并根据实际需求进行扩展和优化。