kbhujbal/probabilistic_SLAM_quadruped_robot

# 面向定位不确定移动机器人的概率地形建图 论文的 C++/ROS 2 实现：

*基于机器人的高程图构建，包含置信区间。来自机载测距传感器的测量值与本体状态估计相融合，同时考虑位姿漂移。地图由前置立体相机实时生成。* ## 概述 一个用于四轮移动机器人概率高程建图的 [ROS 2] 包。专为配备位姿估计（IMU 和里程计）和距离传感器（RealSense、激光雷达、立体相机）的机器人进行局部导航而设计。高程图随机器人移动，并明确处理位姿估计的漂移。 **主要特性：** - 基于机器人的 2.5D 高程图，包含每个网格的空间协方差 - 通过地图融合实现概率上下置信区间 - 支持 Intel RealSense D435、Microsoft Kinect v2、Velodyne VLP-16 的传感器噪声模型 - 带滑移检测的轮式里程计噪声模型 - 通过可见性检查实现动态环境适应 - 定量评估（RMSE, MAE, 置信覆盖率，漂移分析） - 配备四个测试环境的 Gazebo 仿真 ## 方法 ### 坐标系

*四个坐标系：惯性系 **I**，机器人基座系 **B**，传感器系 **S**，地图系 **M**。地图系相对于 B（而非 I）定义，其 z 轴与重力对齐，偏航角与机器人航向一致。* ### 带置信区间的地图融合 高度测量值使用一维卡尔曼滤波器与现有估计融合。随着机器人移动，空间协方差增大——离机器人越远的网格累积的不确定性越多，导致先前观测到的区域相互“融合”。

对于每个网格，根据 2σ 置信椭圆内的邻域网格构建加权经验 CDF。提取 95% 置信区间：

*平坦区域保持较窄的置信区间。物体边缘产生更宽的区间——对避障和接触规划有用。* ### 动态环境扩展

*当障碍物移动时，来自传感器的光线追踪会检测违反可见性约束的网格并将其移除。当新测量值低于当前估计时，注入噪声会加速适应。* ## 结果 ### 仿真

*地图融合能准确逼近真实的置信区间。平坦区域保持较窄；边缘区域变宽。* ### 硬件

*尽管存在里程计漂移，真实地形仍在置信区间内。* ### 平台对比：轮式 vs 腿式 | 指标 | 轮式 | 腿式 | |--------|---------|--------| | 位置漂移率 | ~0.8 cm/m | ~3.1 cm/m | | 偏航漂移率 | ~0.03 rad/m | ~0.07 rad/m | | RMSE | ~1.2 cm | ~3.4 cm | | 覆盖率 | ~96.7% | ~95.1% | | 平均置信区间宽度 | ~3.5 cm | ~8.9 cm | ## 安装说明 ### 依赖项 - [ROS 2 Humble](https://docs.ros.org/en/humble/Installation.html)（或更高版本） - [Grid Map](https://github.com/ANYbotics/grid_map)、[PCL](http://pointclouds.org/)、[Eigen](http://eigen.tuxfamily.org)（>= 3.3） - [Gazebo](http://gazebosim.org/)（可选，用于仿真） ### 构建 ``` cd ~/ros2_ws/src git clone https://github.com/kbhujbal/probabilistic_SLAM_quadruped_robot.git cd ~/ros2_ws rosdep install --from-paths src --ignore-src -r -y colcon build --packages-select probabilistic_terrain_mapping source install/setup.bash ``` ### 测试 ``` colcon test --packages-select probabilistic_terrain_mapping colcon test-result --verbose ``` ## 使用 ### Gazebo 仿真 ``` ros2 launch probabilistic_terrain_mapping simulation.launch.py world:=stairs # 选项：flat_terrain, stairs, rocky_terrain, dynamic_obstacles # 驱动机器人 ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2}, angular: {z: 0.0}}" ``` ### 真实传感器数据 ``` ros2 launch probabilistic_terrain_mapping terrain_mapping.launch.py ``` ### 离线评估 ``` ros2 launch probabilistic_terrain_mapping evaluation.launch.py bag_path:=/path/to/rosbag ``` ## 节点 ### terrain_mapping_node 实时数据采集。订阅 `/camera/depth/points` ([PointCloud2])、`/odom` ([Odometry])、`/imu/data` ([Imu])。发布 `/elevation_map` ([GridMap])。 ### map_fusion_node 并行置信区间计算。订阅 `/elevation_map`。发布 `/fused_map`，包含层：`elevation`、`lower_bound`、`upper_bound`、`confidence_interval`。 ### evaluation_node 离线基准测试。订阅 `/fused_map`、`/ground_truth_map`、`/odom`、`/ground_truth_odom`。发布 `/eval_metrics` (JSON)。 ## 项目结构 ``` probabilistic_terrain_mapping/ ├── config/ # Parameters (sensors, robots) ├── include/ # C++ headers (core, math, sensors, state_estimation, evaluation) ├── src/ # C++ implementations + ROS 2 nodes ├── msg/ # ElevationCell, ElevationMap, EvaluationMetrics ├── launch/ # terrain_mapping, simulation, evaluation ├── test/ # 11 test files, 50+ test cases ├── scripts/ # Python plotting and analysis ├── simulation/ # Gazebo worlds, URDF, robot models ├── rviz/ # Visualization config └── doc/ # Paper figures ``` ## 引用 ``` @inproceedings{bhujbal2022probabilistic, author = {Bhujbal, Kunal and Devmane, Mahavir and More, Ananya}, title = {Probabilistic Method for Mapping \& 3D SLAM of an off-road Terrain with Four Wheeled Robot}, booktitle = {2022 5th International Conference on Advances in Science and Technology (ICAST)}, year = {2022}, organization = {IEEE} } ``` ## 许可证 BSD-3-Clause。详见 [LICENSE](LICENSE)。 ## 错误报告与功能请求 [问题跟踪器](https://github.com/kbhujbal/probabilistic_SLAM_quadruped_robot/issues)

标签：3D协方差传播, C++17, Gazebo仿真, HTTP头分析, Kinect v2, RealSense D435, RMSE评估, ROS 2, Velodyne VLP-16, 传感器融合, 动态环境适应, 卡尔曼滤波, 地形映射, 射线追踪可见性检查, 机器人导航, 机器人技术, 概率高程映射, 漂移分析, 状态估计, 经验CDF融合, 置信界估计, 自动驾驶, 覆盖率评估, 轮式里程计噪声模型, 马氏距离, 高程地图