diff --git a/src/carla_slam_gmapping/README.md b/src/carla_slam_gmapping/README.md index abb80cebcb..bb190045a9 100644 --- a/src/carla_slam_gmapping/README.md +++ b/src/carla_slam_gmapping/README.md @@ -1,8 +1,7 @@ # 人车模拟 # CARLA 全局路径规划和SLAM节点 -实现基于 CARLA 模拟器的车辆全局路径规划功能,支持从车辆自身位置(通过里程计输入)到地图中随机目标点的路径生成,并通过 ROS 2 话题可视化规划结果,模拟用户预订行程后车辆的全局路径规划过程。 -新增支持 3D 建图功能,集成 GMapping 算法实现三维环境下的地图构建。 +实现基于 CARLA 模拟器的车辆全局路径规划功能,支持从车辆自身位置(通过里程计输入)到地图中随机目标点的路径生成,并通过 ROS 2 话题可视化规划结果,模拟用户预订行程后车辆的全局路径规划过程。 新增支持 3D 建图功能,集成 GMapping 算法实现三维环境下的地图构建。 ## 功能概述 该节点是 CARLA 自动驾驶仿真系统中的核心模块之一,主要功能包括: @@ -33,7 +32,6 @@ ```bash export PYTHONPATH=$PYTHONPATH:/path/to/your/carla/PythonAPI/carla/dist/carla-0.9.15-py3.8-linux-x86_64.egg ``` - 3. **其他 Python 依赖** 安装路径计算和坐标转换所需依赖: ```bash @@ -116,30 +114,97 @@ rviz2 3. 路径会以绿色线条显示在场景中 4. 添加 Map 显示项,设置 Topic 为 /map 查看实时构建的地图 -3D 建图算法说明 -GMapping 算法原理 -本项目采用经典的 GMapping 算法实现 3D 建图功能。GMapping 是一种基于粒子滤波的 SLAM( simultaneous localization and mapping)算法,它通过以下步骤实现地图构建: - -粒子滤波: 使用多个粒子表示机器人的可能位姿,每个粒子都携带一个地图副本 -运动模型更新: 根据里程计数据预测每个粒子的新位姿 -观测模型评估: 利用激光雷达扫描数据评估每个粒子的权重 -重采样: 根据权重重新分布粒子集,保留高似然的粒子 -地图更新: 基于最优粒子的位姿估计更新全局地图 -数据流 -输入传感器数据: -激光雷达扫描数据 (/scan) -里程计数据 (/odom) -输出: -2D 占据栅格地图 (/map) -机器人位姿估计 (/map 坐标系下的变换) -参数配置 +## SLAM 算法详解 + +### SLAM 概述 + +SLAM (Simultaneous Localization and Mapping) 是移动机器人领域的一项关键技术,它允许机器人在未知环境中同时构建地图并确定自身位置。这对于自主导航至关重要,因为机器人必须知道自己的位置才能有效地规划路径并避开障碍物。 + +SLAM 问题的核心挑战在于: + +- 机器人不知道自己在哪里(定位问题) +- 机器人不知道环境是什么样子(建图问题) +- 这两个问题相互依赖,必须同时解决 + + + +### GMapping 算法原理 + +本项目采用经典的 GMapping 算法实现 2D 建图功能。GMapping 是一种基于粒子滤波的 SLAM 算法,它通过以下步骤实现地图构建: + +1. **粒子滤波**: 使用多个粒子表示机器人的可能位姿,每个粒子都携带一个地图副本 +2. **运动模型更新**: 根据里程计数据预测每个粒子的新位姿 +3. **观测模型评估**: 利用激光雷达扫描数据评估每个粒子的权重 +4. **重采样**: 根据权重重新分布粒子集,保留高似然的粒子 +5. **地图更新**: 基于最优粒子的位姿估计更新全局地图 + +### GMapping 工作流程 + +#### 初始化阶段 +- 创建 N 个粒子,每个粒子代表机器人的一种可能位姿 +- 每个粒子维护一张独立的地图 +- 所有粒子初始权重相等 + +#### 循环迭代过程 +当收到新的传感器数据时,算法执行以下步骤: + +##### 预测步骤 +- 使用里程计读数更新每个粒子的位置 +- 考虑运动噪声,使粒子分布更加分散 + +##### 更新步骤 +- 对于每个粒子,比较激光雷达扫描数据与该粒子地图的预期观测 +- 计算粒子权重,权重越高表示该粒子越接近真实情况 + +##### 重采样步骤 +- 根据粒子权重进行重采样 +- 权重高的粒子被多次复制,权重低的粒子被淘汰 +- 保证粒子集始终代表最可能的机器人位姿分布 + +##### 地图更新 +- 基于最高权重粒子的信息更新全局地图 +- 将新的观测数据融合到现有地图中 + +### 数据流 + +#### 输入传感器数据 +- 激光雷达扫描数据 (`/scan`): 提供周围环境的距离测量 +- 里程计数据 (`/odom`): 提供相对运动信息 + +#### 输出结果 +- 2D 占据栅格地图 (`/map`): 表示环境的占用情况 +- 机器人位姿估计 (`/map` 坐标系下的变换): 提供机器人在地图中的位置 + +### 参数配置 + GMapping 算法的关键参数已在 launch 文件中预设,可根据具体场景调整: -maxUrange: 激光雷达最大使用距离 (单位: 米) -sigma: 扫描匹配的标准差 -particles: 粒子数量 -delta: 地图分辨率 (单位: 米/cell) -temporalUpdate: 地图更新频率 +| 参数名 | 默认值 | 描述 | +|--------|--------|------| +| `maxUrange` | 10.0 | 激光雷达最大使用距离 (单位: 米) | +| `sigma` | 0.05 | 扫描匹配的标准差 | +| `particles` | 30 | 粒子数量 | +| `delta` | 0.05 | 地图分辨率 (单位: 米/cell) | +| `temporalUpdate` | -1.0 | 地图更新频率 (-1 表示自适应更新) | +| `linearUpdate` | 1.0 | 线性移动多少米后更新地图 | +| `angularUpdate` | 0.5 | 角度旋转多少弧度后更新地图 | +| `resampleInterval` | 1 | 重采样间隔(以scan为单位) | +| `xmin`, `ymin`, `xmax`, `ymax` | -100.0 ~ 100.0 | 地图边界 | + +### 算法优势与局限性 + +#### 优势 +- 实现相对简单,易于理解和部署 +- 在中小规模环境中表现良好 +- 对传感器噪声具有较好的鲁棒性 +- 能够处理闭环检测问题 + +#### 局限性 +- 计算复杂度随粒子数量增加而线性增长 +- 在大规模环境中效率较低 +- 对快速运动或剧烈震动敏感 +- 难以处理动态环境中的移动物体 + ## 核心接口说明 ### 1. ROS 2 服务 @@ -230,8 +295,6 @@ carla_global_planner/ 添加路径平滑处理,优化车辆行驶轨迹 增加单元测试(基于 pytest 和 ros2 test) 集成其他 SLAM 算法(如 Hector SLAM、Karto SLAM)以提供更多建图选项 - - 3. **提交流程** 准备提交代码前,请确保: