别只盯着建图！用思岚A1激光雷达和ROS，5分钟实现一个动态障碍物检测Demo

用思岚A1激光雷达和ROS实现动态障碍物检测的5分钟实战

激光雷达在机器人领域的应用早已超越了基础的环境建图。当你能在RViz中看到那些跳动的扫描点时，是否想过如何让这些数据"活"起来？本文将带你用思岚A1激光雷达和ROS，在5分钟内打造一个能实时感知动态障碍物的实用系统。

1. 环境准备与数据流理解

在开始编码前，我们需要确保基础环境正常运行。假设你已经完成了以下准备工作：

• 思岚A1激光雷达通过USB正确连接
• 安装了rplidar_ros驱动包
• 能够通过roslaunch rplidar_ros rplidar.launch启动雷达节点
• 在RViz中能够正常显示/scan话题的激光扫描数据

激光雷达的核心数据流是这样的：

雷达硬件 → /scan话题 → 处理节点 → 检测结果输出

思岚A1的/scan话题包含以下关键信息：

• angle_min和angle_max: 扫描角度范围（通常为-π到π）
• angle_increment: 每次扫描的角度增量
• ranges: 各角度对应的距离值数组
• intensities: 各角度对应的反射强度（可选）

注意：实际检测时，建议先检查ranges数组长度是否与预期角度分辨率匹配，避免数组越界。

2. 快速实现区域入侵检测

动态障碍物检测最直观的应用就是区域入侵报警。下面我们创建一个Python节点，监测特定扇形区域内的物体接近情况。

首先创建一个新的ROS包（如果尚未创建）：

cd ~/catkin_ws/src catkin_create_pkg obstacle_detection rospy sensor_msgs cd ~/catkin_ws && catkin_make

然后创建scripts/sector_monitor.py文件：

#!/usr/bin/env python import rospy from sensor_msgs.msg import LaserScan def scan_callback(data): # 定义监测区域（正前方±30度） start_angle = -0.5236 # -30度(弧度) end_angle = 0.5236 # 30度(弧度) # 计算对应的数组索引 start_idx = int((start_angle - data.angle_min) / data.angle_increment) end_idx = int((end_angle - data.angle_min) / data.angle_increment) # 提取区域内的距离数据 sector_ranges = data.ranges[start_idx:end_idx] # 过滤无效数据（0表示无效测量） valid_ranges = [r for r in sector_ranges if r > 0] if valid_ranges: min_dist = min(valid_ranges) rospy.loginfo(f"最近障碍物距离: {min_dist:.2f}米") if min_dist < 1.0: # 1米阈值 rospy.logwarn("警告：障碍物接近！") if __name__ == '__main__': rospy.init_node('sector_monitor') rospy.Subscriber('/scan', LaserScan, scan_callback) rospy.spin()

给文件添加执行权限后运行：

chmod +x sector_monitor.py rosrun obstacle_detection sector_monitor.py

这个简单实现已经能完成：

• 监测机器人正前方±30度扇形区域
• 实时输出最近障碍物距离
• 当障碍物小于1米时触发警告

3. 使用laser_filters进行动态障碍物分离

对于更复杂的场景，我们可以利用ROS的laser_filters包来预处理激光数据。安装方法：

sudo apt-get install ros-$ROS_DISTRO-laser-filters

创建一个配置文件config/obstacle_filter.yaml：

scan_filter_chain: - name: range type: laser_filters/LaserScanRangeFilter params: lower_threshold: 0.3 upper_threshold: 12.0 - name: shadow type: laser_filters/LaserScanShadowFilter params: min_angle: 10 max_angle: 170 neighbors: 20 window: 1

然后创建启动文件launch/obstacle_filter.launch：

<launch> <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter"> <rosparam command="load" file="$(find obstacle_detection)/config/obstacle_filter.yaml" /> <remap from="scan" to="/scan" /> <remap from="scan_filtered" to="/scan_filtered" /> </node> </launch>

启动后，/scan_filtered话题将提供：

• 去除无效距离值（<0.3m或>12m）
• 滤除激光阴影效应造成的假障碍物
• 更干净的数据供后续处理

4. 高级动态障碍物追踪实现

结合上述技术，我们可以构建更强大的动态障碍物追踪系统。创建一个新的Python节点scripts/dynamic_tracker.py：

#!/usr/bin/env python import rospy import numpy as np from sensor_msgs.msg import LaserScan from geometry_msgs.msg import PointStamped class DynamicTracker: def __init__(self): self.last_scan = None self.pub = rospy.Publisher('/obstacle_position', PointStamped, queue_size=10) def scan_callback(self, data): if self.last_scan is None: self.last_scan = data return # 计算相邻两次扫描的变化 current = np.array(data.ranges) last = np.array(self.last_scan.ranges) diff = np.abs(current - last) # 找出变化显著的点（动态障碍物） moving_mask = (diff > 0.2) & (current > 0) & (last > 0) moving_indices = np.where(moving_mask)[0] if len(moving_indices) > 0: # 取变化最大的点作为代表 main_idx = moving_indices[np.argmax(diff[moving_indices])] angle = data.angle_min + main_idx * data.angle_increment distance = data.ranges[main_idx] # 转换为笛卡尔坐标并发布 point = PointStamped() point.header.stamp = rospy.Time.now() point.header.frame_id = "laser" point.point.x = distance * np.cos(angle) point.point.y = distance * np.sin(angle) self.pub.publish(point) self.last_scan = data if __name__ == '__main__': rospy.init_node('dynamic_tracker') tracker = DynamicTracker() rospy.Subscriber('/scan_filtered', LaserScan, tracker.scan_callback) rospy.spin()

这个实现可以：

1. 比较连续两次激光扫描数据
2. 识别距离变化显著的点（动态障碍物）
3. 将最主要的动态障碍物位置发布为/obstacle_position话题
4. 在RViz中可视化动态障碍物的实时位置

5. 系统集成与性能优化

将上述组件整合为一个完整系统，创建launch/full_detection.launch：

<launch> <!-- 启动雷达 --> <include file="$(find rplidar_ros)/launch/rplidar.launch" /> <!-- 激光数据过滤 --> <include file="$(find obstacle_detection)/launch/obstacle_filter.launch" /> <!-- 动态障碍物追踪 --> <node pkg="obstacle_detection" type="dynamic_tracker.py" name="dynamic_tracker" output="screen" /> <!-- 区域监测 --> <node pkg="obstacle_detection" type="sector_monitor.py" name="sector_monitor" output="screen" /> </launch>

性能优化建议：

• 降低计算负载：在LaserScan消息回调中，只处理必要的角度范围
• 时间同步：对于多传感器融合，使用message_filters进行时间同步
• 参数配置：将阈值、监测区域等参数改为ROS参数，便于动态调整

# 在节点初始化时读取参数 self.threshold = rospy.get_param('~distance_threshold', 1.0) self.angle_range = rospy.get_param('~angle_range', [-0.5, 0.5])

实际部署时，我发现将激光数据预处理（如滤波）与业务逻辑（如障碍物检测）分离，能显著提高系统可维护性。当检测逻辑需要调整时，无需重新配置滤波器参数。