智能路径规划与自主导航系统

中文版本

📋 项目简介

基于 ROS2 的自主移动机器人导航系统，集成了先进的路径规划、动态避障、环境感知和智能决策功能。系统采用模块化设计，支持未知环境探索、实时建图、精确路径跟踪等核心功能。

✨ 核心特性

🗺️ 自主探索 - 基于前沿检测的未知环境自动探索
🎯 智能规划 - 全局路径规划与局部动态避障
👁️ 环境感知 - 激光雷达实时障碍检测与间隙识别
🚀 动态避障 - 实时检测并避让动态障碍物
🎮 精确控制 - Pure Pursuit 与 Stanley 双控制器
🧠 智能决策 - 行为树状态机自主任务管理
🔄 自动恢复 - 异常情况自动恢复机制

🏗️ 系统架构

┌─────────────────────────────────────────┐
│          决策层 (Decision Layer)         │
│       Behavior Tree Navigator           │
└─────────────────────────────────────────┘
                    ↓
┌──────────┬──────────┬──────────┬──────────┐
│ 探索模块 │ 感知模块 │ 避障模块 │ 控制模块 │
│ Explorer │ Detector │ Avoider  │ Tracker  │
└──────────┴──────────┴──────────┴──────────┘
                    ↓
┌─────────────────────────────────────────┐
│       导航层 (Navigation Layer)          │
│            Nav2 Stack                   │
└─────────────────────────────────────────┘
                    ↓
┌─────────────────────────────────────────┐
│        感知层 (Perception Layer)         │
│      LiDAR + Odometry + SLAM            │
└─────────────────────────────────────────┘

📦 环境要求

操作系统: Ubuntu 22.04 LTS
ROS 版本: ROS2 Humble
Python: 3.10+
依赖库: NumPy, SciPy

🚀 快速开始

1. 安装依赖

# 安装 ROS2 核心包
sudo apt update
sudo apt install ros-humble-desktop

# 安装导航相关包
sudo apt install ros-humble-navigation2 \
                 ros-humble-nav2-bringup \
                 ros-humble-slam-toolbox \
                 ros-humble-robot-localization

# 安装 Python 依赖
pip3 install numpy scipy

2. 编译项目

# 创建工作空间
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# 克隆项目（替换为实际仓库地址）
git clone <repository-url>

# 编译
cd ~/ros2_ws
colcon build
source install/setup.bash

3. 启动系统

# 启动完整导航系统
ros2 launch maze_algorithm full_navigation.launch.py

# 在 RViz 中设置目标点
# 点击 "2D Goal Pose" 工具，在地图上点击目标位置

📁 项目结构

maze_algorithm/
├── maze_algorithm/              # 核心模块
│   ├── behavior_tree_navigator.py    # 行为树决策
│   ├── dynamic_obstacle_avoider.py   # 动态避障
│   ├── gap_detector.py               # 间隙检测
│   ├── path_tracker.py               # 路径跟踪
│   ├── frontier_explorer.py          # 前沿探索
│   ├── maze_navigator.py             # 基础导航
│   └── hole_detector.py              # 孔洞检测
├── launch/                      # 启动文件
│   ├── full_navigation.launch.py     # 完整系统
│   ├── maze_navigation.launch.py     # 基础导航
│   └── exploration.launch.py         # 探索模式
├── config/                      # 配置文件
│   ├── nav2_params.yaml              # Nav2 参数
│   └── explore_params.yaml           # 探索参数
└── rviz/                        # 可视化配置
    └── slam_desktop.rviz

🎮 模块说明

行为树导航器 (Behavior Tree Navigator)

顶层决策管理
状态机自动切换
任务超时处理
异常恢复策略

动态障碍避障 (Dynamic Obstacle Avoider)

LiDAR 点云聚类
障碍物速度估计
碰撞时间预测
安全窗口判断

间隙检测器 (Gap Detector)

激光雷达间隙识别
可通行性评估
最佳路径选择
实时可视化

路径跟踪器 (Path Tracker)

Pure Pursuit 控制
Stanley 控制
自适应速度
双控制器切换

前沿探索器 (Frontier Explorer)

未知区域检测
边界点聚类
探索目标选择
自动导航

⚙️ 参数配置

机器人参数

# config/nav2_params.yaml
robot_radius: 0.25          # 机器人半径 (m)
max_vel_x: 0.26            # 最大线速度 (m/s)
max_vel_theta: 1.0         # 最大角速度 (rad/s)

导航参数

# launch/full_navigation.launch.py
{'goal_x': 9.5}            # 目标点 X 坐标
{'goal_y': 2.5}            # 目标点 Y 坐标
{'goal_tolerance': 0.25}   # 到达容差 (m)

感知参数

{'min_gap_width': 0.5}     # 最小间隙宽度 (m)
{'safe_distance': 0.6}     # 安全距离 (m)
{'detection_range': 3.0}   # 检测范围 (m)

📊 ROS 话题

订阅话题

/scan (sensor_msgs/LaserScan) - 激光雷达数据
/odom (nav_msgs/Odometry) - 里程计数据
/map (nav_msgs/OccupancyGrid) - SLAM 地图

发布话题

/cmd_vel (geometry_msgs/Twist) - 速度控制命令
/best_gap (geometry_msgs/PoseStamped) - 最佳间隙位置
/safe_to_proceed (std_msgs/Bool) - 安全状态
/detected_gaps (visualization_msgs/MarkerArray) - 检测间隙可视化

🔧 故障排查

编译错误

# 安装缺失依赖
rosdep install --from-paths src --ignore-src -r -y

# 清理并重新编译
rm -rf build install log
colcon build

传感器连接问题

# 检查激光雷达话题
ros2 topic list | grep scan
ros2 topic hz /scan

# 检查里程计话题
ros2 topic echo /odom --once

导航服务异常

# 检查 Nav2 节点
ros2 node list | grep nav

# 重启生命周期管理器
ros2 lifecycle set /lifecycle_manager_navigation configure
ros2 lifecycle set /lifecycle_manager_navigation activate

📈 性能优化

地图分辨率: 根据场地大小调整 (0.05-0.1m)
控制频率: 根据硬件性能调整 (10-20Hz)
激光点数: 适当降采样减少计算负担
前视距离: 根据速度动态调整

🤝 贡献指南

欢迎提交 Issue 和 Pull Request！

📄 许可证

MIT License

📧 联系方式

如有问题或建议，请通过 Issue 联系。

English Version

📋 Project Overview

An autonomous mobile robot navigation system based on ROS2, integrating advanced path planning, dynamic obstacle avoidance, environmental perception, and intelligent decision-making capabilities. The system features modular design and supports core functions including unknown environment exploration, real-time mapping, and precise path tracking.

✨ Key Features

🗺️ Autonomous Exploration - Frontier-based automatic exploration in unknown environments
🎯 Intelligent Planning - Global path planning with local dynamic obstacle avoidance
👁️ Environmental Perception - Real-time LiDAR obstacle detection and gap identification
🚀 Dynamic Avoidance - Real-time detection and avoidance of dynamic obstacles
🎮 Precise Control - Dual controllers: Pure Pursuit and Stanley
🧠 Smart Decision - Behavior tree state machine for autonomous task management
🔄 Auto Recovery - Automatic recovery mechanism for exceptional situations

🏗️ System Architecture

┌─────────────────────────────────────────┐
│       Decision Layer                    │
│    Behavior Tree Navigator              │
└─────────────────────────────────────────┘
                    ↓
┌──────────┬──────────┬──────────┬──────────┐
│ Explorer │ Detector │ Avoider  │ Tracker  │
│  Module  │  Module  │  Module  │  Module  │
└──────────┴──────────┴──────────┴──────────┘
                    ↓
┌─────────────────────────────────────────┐
│       Navigation Layer                  │
│          Nav2 Stack                     │
└─────────────────────────────────────────┘
                    ↓
┌─────────────────────────────────────────┐
│       Perception Layer                  │
│    LiDAR + Odometry + SLAM              │
└─────────────────────────────────────────┘

📦 Requirements

OS: Ubuntu 22.04 LTS
ROS: ROS2 Humble
Python: 3.10+
Libraries: NumPy, SciPy

🚀 Quick Start

1. Install Dependencies

# Install ROS2 core packages
sudo apt update
sudo apt install ros-humble-desktop

# Install navigation packages
sudo apt install ros-humble-navigation2 \
                 ros-humble-nav2-bringup \
                 ros-humble-slam-toolbox \
                 ros-humble-robot-localization

# Install Python dependencies
pip3 install numpy scipy

2. Build Project

# Create workspace
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# Clone repository (replace with actual URL)
git clone <repository-url>

# Build
cd ~/ros2_ws
colcon build
source install/setup.bash

3. Launch System

# Launch full navigation system
ros2 launch maze_algorithm full_navigation.launch.py

# Set goal in RViz
# Click "2D Goal Pose" tool and click target position on map

📁 Project Structure

maze_algorithm/
├── maze_algorithm/              # Core modules
│   ├── behavior_tree_navigator.py    # Behavior tree
│   ├── dynamic_obstacle_avoider.py   # Dynamic avoidance
│   ├── gap_detector.py               # Gap detection
│   ├── path_tracker.py               # Path tracking
│   ├── frontier_explorer.py          # Exploration
│   ├── maze_navigator.py             # Basic navigation
│   └── hole_detector.py              # Hole detection
├── launch/                      # Launch files
│   ├── full_navigation.launch.py     # Full system
│   ├── maze_navigation.launch.py     # Basic nav
│   └── exploration.launch.py         # Exploration
├── config/                      # Configuration
│   ├── nav2_params.yaml              # Nav2 params
│   └── explore_params.yaml           # Explore params
└── rviz/                        # Visualization
    └── slam_desktop.rviz

🎮 Module Description

Behavior Tree Navigator

Top-level decision management
Automatic state machine switching
Task timeout handling
Exception recovery strategy

Dynamic Obstacle Avoider

LiDAR point cloud clustering
Obstacle velocity estimation
Collision time prediction
Safety window determination

Gap Detector

LiDAR gap identification
Passability assessment
Optimal path selection
Real-time visualization

Path Tracker

Pure Pursuit control
Stanley control
Adaptive velocity
Dual controller switching

Frontier Explorer

Unknown area detection
Boundary point clustering
Exploration target selection
Automatic navigation

⚙️ Configuration

Robot Parameters

# config/nav2_params.yaml
robot_radius: 0.25          # Robot radius (m)
max_vel_x: 0.26            # Max linear velocity (m/s)
max_vel_theta: 1.0         # Max angular velocity (rad/s)

Navigation Parameters

# launch/full_navigation.launch.py
{'goal_x': 9.5}            # Goal X coordinate
{'goal_y': 2.5}            # Goal Y coordinate
{'goal_tolerance': 0.25}   # Arrival tolerance (m)

Perception Parameters

{'min_gap_width': 0.5}     # Minimum gap width (m)
{'safe_distance': 0.6}     # Safety distance (m)
{'detection_range': 3.0}   # Detection range (m)

📊 ROS Topics

Subscribed Topics

/scan (sensor_msgs/LaserScan) - LiDAR data
/odom (nav_msgs/Odometry) - Odometry data
/map (nav_msgs/OccupancyGrid) - SLAM map

Published Topics

/cmd_vel (geometry_msgs/Twist) - Velocity commands
/best_gap (geometry_msgs/PoseStamped) - Best gap position
/safe_to_proceed (std_msgs/Bool) - Safety status
/detected_gaps (visualization_msgs/MarkerArray) - Gap visualization

🔧 Troubleshooting

Build Errors

# Install missing dependencies
rosdep install --from-paths src --ignore-src -r -y

# Clean and rebuild
rm -rf build install log
colcon build

Sensor Connection Issues

# Check LiDAR topic
ros2 topic list | grep scan
ros2 topic hz /scan

# Check odometry topic
ros2 topic echo /odom --once

Navigation Service Issues

# Check Nav2 nodes
ros2 node list | grep nav

# Restart lifecycle manager
ros2 lifecycle set /lifecycle_manager_navigation configure
ros2 lifecycle set /lifecycle_manager_navigation activate

📈 Performance Optimization

Map Resolution: Adjust based on field size (0.05-0.1m)
Control Frequency: Adjust based on hardware (10-20Hz)
LiDAR Points: Downsample to reduce computation
Lookahead Distance: Dynamically adjust based on velocity

🤝 Contributing

Issues and Pull Requests are welcome!

📄 License