在机器人自主导航系统中，静态栅格地图提供了环境的基本结构信息，然而实际应用中，环境往往存在动态变化。本文将深入探讨动态障碍物地图的实现原理及其在局部路径规划中的关键作用。当环境中出现未在全局地图中标注的新障碍物时，动态障碍物地图能够实时更新这些信息，从而避免导航算法与突发障碍物发生碰撞。

move_base中的障碍物地图实现

1 缓冲区初始化

在move_base框架中，障碍物地图具有独立的维护机制。通过分析ObstacleLayer类的初始化过程，我们可以发现其创建了指向ObservationBuffer的智能指针：

// 创建观测数据缓冲区
    sensor_buffers_.emplace_back(
        std::make_shared < SensorDataContainer
            > (input_topic, data_retention_duration, expected_refresh_rate, min_obstacle_altitude,
             max_obstacle_altitude, detection_threshold, scan_range, *tf_, reference_frame_,
             sensor_frame, transform_tolerance)));

此类主要负责从传感器获取原始数据，将其转换到目标坐标系并进行存储。其初始化过程需要多个关键参数：

input_topic参数指定了数据来源的主题名称。

data_retention_duration参数控制观测数据在缓冲区中的保留时长。例如设置为5秒时，缓冲区在每次更新时会自动清除超过此时间阈值的数据。对于不需要对动态障碍物过于敏感的应用场景，可适当调整此参数以平衡实时性与计算开销。

expected_refresh_rate定义了缓冲区的期望更新频率，默认值为0表示接收所有传感器数据，增大此值可实现数据过滤功能。

min_obstacle_altitude与max_obstacle_altitude用于高度过滤，在二维导航场景中影响有限。

detection_threshold与scan_range是距离相关参数，用于过滤超出有效探测范围的点云数据。考虑到路径规划的实际需求，通常只需保留机器人周围2-3米范围内的障碍物信息。

剩余四个参数与坐标变换相关，确保点云数据能从传感器坐标系正确转换到地图坐标系。

在SensorDataContainer的内部实现中，核心数据结构为：

std::deque<SensorReading> readings_queue_;

该结构维护了SensorReading对象的队列，每个SensorReading对象包含：

sensor_msgs::PointCloud2* point_cloud_;

即一组点云数据。这种设计使得缓冲区能够高效管理多帧传感器数据。data_retention_duration参数在cleanExpiredData函数中发挥作用：

// 遍历所有观测数据，移除过期数据
for (auto reading_it = readings_queue_.begin(); reading_it != readings_queue_.end(); ++reading_it)
{
SensorReading& current_reading = *reading_it;
// 检查数据是否过期，若过期则移除及其后续数据
if ((current_timestamp_ - current_reading.point_cloud_->header.stamp) > data_retention_duration_)
{
readings_queue_.erase(reading_it, readings_queue_.end());
return;
}
}

该函数在缓冲区更新后被调用，通过比较时间戳过滤掉过时的传感器数据。

2 传感器数据订阅

缓冲区初始化完成后，系统需要根据不同传感器类型获取数据。针对激光雷达、点云等不同数据类型，以及特殊值(inf)处理，系统设计了多种订阅模式：

if (data_source == "LaserScan")
{
auto scan_subscriber = std::make_shared < message_filters::Subscriber<sensor_msgs::LaserScan>
> (node_handle, input_topic, 50);

auto transform_filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(
*scan_subscriber, *tf_, reference_frame_, 50, node_handle);

if (infinity_values_valid)
{
transform_filter->registerCallback(std::bind(&ObstacleLayer::laserScanWithInfCallback, this, _1,
sensor_buffers_.back()));
}
else
{
transform_filter->registerCallback(std::bind(&ObstacleLayer::laserScanStandardCallback, this, _1, sensor_buffers_.back()));
}

data_subscribers_.push_back(scan_subscriber);
data_processors_.push_back(transform_filter);

data_processors_.back()->setTolerance(ros::Duration(0.05));
}

这种设计充分考虑了实际应用中的各种传感器数据特性。

3 回调函数处理

不同类型的传感器数据对应不同的回调函数。下面以激光雷达数据处理为例进行分析：

该回调函数位于ObstacleLayer类中，主要执行两个关键步骤：

将原始激光扫描数据转换为点云格式：

sensor_msgs::PointCloud2 cloud_data;
cloud_data.header = scan_message->header;

// 将激光扫描投影为点云
try
{
projector_.transformLaserScanToPointCloud(scan_message->header.frame_id, *scan_message, cloud_data, *tf_);
}

并将转换后的点云提交给SensorDataContainer处理：

// 将点云存入缓冲区
buffer->lock();
buffer->storeCloud(cloud_data);
buffer->unlock();

SensorDataContainer类中，storeCloud函数实现了点云数据的存储逻辑：

首先创建新的SensorReading对象：

// 在列表前端创建新的观测对象
readings_queue_.emplace_front(SensorReading());

然后存储传感器原点坐标：

// 存储传感器原点在世界坐标系中的位置
geometry_msgs::PointStamped local_sensor_origin;
local_sensor_origin.header.stamp = cloud_data.header.stamp;
local_sensor_origin.header.frame_id = sensor_frame;
local_sensor_origin.point.x = 0;
local_sensor_origin.point.y = 0;
local_sensor_origin.point.z = 0;
tf2_buffer_.transform(local_sensor_origin, global_origin, reference_frame_);
tf2::convert(global_origin.point, readings_queue_.front().sensor_origin_);

这里存储的坐标表示传感器坐标系在当前时刻在世界坐标系中的位置。

接下来存储距离过滤参数：

// 传递扫描范围参数到观测对象
readings_queue_.front().scan_range_ = scan_range_;
readings_queue_.front().detection_threshold_ = detection_threshold_;

然后对点云进行坐标系转换：

sensor_msgs::PointCloud2 world_frame_cloud;

// 转换点云到世界坐标系
tf2_buffer_.transform(cloud_data, world_frame_cloud, reference_frame_);
world_frame_cloud.header.stamp = cloud_data.header.stamp;

// 配置点云基本属性
sensor_msgs::PointCloud2& observation_cloud = *(readings_queue_.front().point_cloud_);
observation_cloud.height = world_frame_cloud.height;
observation_cloud.width = world_frame_cloud.width;
observation_cloud.fields = world_frame_cloud.fields;
observation_cloud.is_bigendian = world_frame_cloud.is_bigendian;
observation_cloud.point_step = world_frame_cloud.point_step;
observation_cloud.row_step = world_frame_cloud.row_step;
observation_cloud.is_dense = world_frame_cloud.is_dense;

最后进行高度过滤并存储有效点云：

std::vector<unsigned char>::const_iterator global_iter = world_frame_cloud.data.begin();
std::vector<unsigned char>::const_iterator global_end_iter = world_frame_cloud.data.end();
std::vector<unsigned char>::iterator obs_iter = observation_cloud.data.begin();
for (; global_iter != global_end_iter; ++global_z, global_iter += world_frame_cloud.point_step)
{
if ((*global_z) <= max_obstacle_altitude_
&& (*global_z) >= min_obstacle_altitude_)
{
// 使用std::copy高效复制点云数据
std::copy(global_iter, global_iter + world_frame_cloud.point_step, obs_iter);
obs_iter += world_frame_cloud.point_step;
++valid_point_count;
}
}

至此，单帧障碍物点云的处理流程完成。系统通过维护SensorReading对象队列，每个对象包含一帧经过坐标转换和高度过滤的点云数据，实现了障碍物信息的动态更新。

从点云数据到最终可用于路径规划的障碍物地图，还需要经过栅格化处理等后续步骤，这部分内容将在后续文章中详细介绍。