✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 具体问题可以私信或扫描文章底部二维码。
（1）多传感器融合的玻璃环境感知系统设计
室内玻璃环境对移动机器人的核心挑战在于玻璃的透明特性会导致单一传感器失效 —— 激光雷达因光束穿透玻璃而无法识别障碍，视觉相机易受光照反射干扰，超声波传感器在中远距离测距精度不足。为此，本研究构建了 “激光雷达 + 视觉相机 + 超声波传感器” 的多传感器融合感知系统，通过各传感器优势互补实现玻璃障碍的稳定检测，具体可分为三个关键环节。
首先是传感器选型与功能定位。激光雷达选用 2D 激光雷达（如 RPLIDAR A3），其核心作用是构建室内环境的基础轮廓，针对非透明物体（如墙体、桌椅）实现高精度测距（误差≤5cm）和快速扫描（扫描频率 10Hz），为全局环境感知提供骨架数据；但需明确其局限性 —— 面对厚度≤10mm 的普通浮法玻璃，激光光束穿透率超过 85%，会将玻璃区域误判为 “空旷空间”，因此需依赖其他传感器补充。视觉相机采用 USB 高清工业相机（分辨率 1280×720，帧率 30fps），聚焦玻璃的视觉特征提取：玻璃虽透明，但存在边缘轮廓（如玻璃与框架的拼接处）、表面反射光斑（不同光照下的反光差异）以及纹理细节（如磨砂玻璃的表面纹理），通过图像处理算法可捕捉这些特征 —— 先对采集的图像进行灰度化和高斯滤波去噪，再用 Canny 边缘检测算法提取玻璃的轮廓线条，结合 SIFT 特征匹配算法识别玻璃与周边环境的边界，最终输出玻璃区域在图像坐标系中的位置坐标。超声波传感器选用 4 路超声波模块（测距范围 2cm-4m，精度 ±1cm），安装于机器人底盘四周（前、后、左、右各 1 个），其核心功能是近距离玻璃测距补盲：当机器人靠近玻璃（距离≤1m）时，超声波通过空气介质传播，遇到玻璃表面会产生反射波，通过计算发射与接收的时间差可直接获取距离数据；考虑到室内环境中空气流动、地面杂物可能导致超声波数据波动，需对原始数据进行卡尔曼滤波处理，消除随机噪声，确保测距稳定性。
其次是多传感器数据预处理。由于三种传感器的采集频率、坐标系不同，直接融合会产生数据错位问题，因此需先进行数据校准与同步。时间同步方面，通过 ROS（机器人操作系统）的时间戳机制实现 —— 为每个传感器数据添加统一的系统时间戳，当融合模块接收数据时，仅处理时间戳偏差≤50ms 的数据，避免因采集延迟导致的障碍位置误判。坐标转换方面，以机器人底盘中心为原点建立统一的车体坐标系：激光雷达安装于底盘顶部，需将其极坐标数据（角度、距离）转换为车体坐标系下的直角坐标（x,y）；视觉相机安装于底盘前端上方，需通过相机内参（焦距、主点坐标）和外参（相机相对于车体的安装位置、角度）将图像坐标系中的玻璃区域坐标转换为车体坐标系下的三维坐标（x,y,z），其中 z 轴数据用于判断玻璃是否处于机器人运动平面（排除天花板或地面的玻璃反光干扰）；超声波传感器安装于底盘四周，需根据每个传感器的安装角度（如前向传感器角度为 0°，右向为 90°），将测距数据转换为车体坐标系下的（x,y）坐标。此外，还需对激光雷达点云数据进行体素滤波，去除因传感器噪声产生的孤立点；对视觉图像进行形态学闭运算，填补玻璃轮廓中的细小缺口，确保特征提取的完整性。
最后是多传感器数据融合策略。本研究采用决策层融合方案，通过建立 “投票机制” 综合三种传感器的检测结果，判断目标区域是否为玻璃障碍。具体逻辑为：当激光雷达检测到某区域无返回信号（判定为 “空旷”），但视觉相机检测到该区域存在玻璃轮廓特征（判定为 “疑似玻璃”），且超声波传感器测距显示该区域存在物体（距离≤1.5m，判定为 “存在障碍”）时，系统综合判定该区域为 “玻璃障碍”，并标记其位置与大致范围；若仅视觉相机检测到疑似特征，而超声波无反馈，则判定为 “反光干扰”，排除障碍标记；若仅超声波检测到障碍，而视觉无特征，则判定为 “非透明障碍”（如纸箱），按常规障碍处理。为进一步提升融合精度，还引入了置信度权重 —— 根据不同环境下的传感器表现动态调整权重：在强光环境下，视觉相机易受反射干扰，降低其置信度（权重 0.3），提高超声波权重（0.5）；在弱光环境下，视觉特征提取难度增加，提高激光雷达（针对非透明区域）和超声波的权重（各 0.4），降低视觉权重（0.2）。通过该融合策略，机器人对玻璃障碍的检测准确率可提升至 92% 以上，误检率低于 5%，有效解决了单一传感器在玻璃环境中的失效问题。
（2）面向玻璃场景的 SLAM 算法优化与地图构建
SLAM（同时定位与地图构建）是移动机器人自主导航的核心技术，但其在室内玻璃环境中面临两大难题：一是玻璃无法被激光雷达识别，导致地图中出现 “虚拟空洞”，机器人易误判路径；二是玻璃表面的弱反射可能导致激光雷达产生少量杂波点，干扰机器人定位精度。为此，本研究在经典 SLAM 算法基础上，结合玻璃环境的感知数据，开展算法优化，实现精准的地图构建与自主定位。
首先是经典 SLAM 算法在玻璃环境的适应性分析。本研究选取三种主流激光 SLAM 算法（Gmapping、Cartographer、Hector SLAM）进行对比测试，测试环境为含 3 块玻璃隔断（每块尺寸 1.8m×2.5m）的办公室场景（面积约 50㎡），评价指标包括地图完整性（玻璃区域是否被正确标记）、定位误差（机器人实际位置与地图定位的偏差）、地图构建时间。测试结果显示：Gmapping 算法基于粒子滤波，在小场景下具有较高的定位精度（平均误差≤8cm）和较快的构建速度（50㎡场景约 3min），但对玻璃区域的处理完全依赖激光数据，导致地图中玻璃隔断位置出现空洞，机器人在导航时会试图穿越空洞，碰撞风险极高；Cartographer 算法采用分层优化策略，在大场景下稳定性更好，但在玻璃环境中，其对激光点云的匹配依赖于连续的特征点，玻璃区域的 “无特征” 会导致匹配失败，定位误差骤增（平均误差≥15cm），且地图构建时间较长（50㎡场景约 6min）；Hector SLAM 无需里程计数据，仅依赖激光雷达，但对激光数据的密度和精度要求极高，玻璃表面的少量杂波点会导致其地图出现 “伪障碍”，且定位漂移严重（10min 内漂移量≥30cm）。综合来看，Gmapping 算法在小场景玻璃环境中基础性能最优，因此确定以 Gmapping 为基础进行优化，重点解决玻璃区域的地图缺失问题。
其次是基于玻璃特征约束的 Gmapping 算法优化。Gmapping 算法的核心是通过粒子滤波估计机器人位姿，并用激光数据更新地图；优化的关键在于将多传感器融合得到的 “玻璃障碍信息” 融入粒子滤波的观测模型，修正地图更新逻辑。具体优化方向有两点：一是玻璃障碍的地图标记机制。原始 Gmapping 仅根据激光雷达的 “有返回信号 = 障碍”“无返回信号 = 空旷” 规则更新地图栅格（占据栅格地图）；优化后，当融合系统判定某区域为 “玻璃障碍” 时，会向 Gmapping 的地图更新模块发送该区域的坐标范围，将对应栅格标记为 “玻璃障碍栅格”（区别于普通障碍栅格，用特定数值表示，如 200），同时禁止该区域栅格被 “空旷” 状态覆盖 —— 即使激光雷达无返回信号，也不会将已标记的玻璃栅格改为空旷，避免地图空洞。二是粒子权重的修正策略。原始 Gmapping 通过计算激光点云与地图的匹配度（似然域模型）调整粒子权重，匹配度高的粒子权重更大；优化后，在似然域模型中加入玻璃特征约束：若某粒子预测的机器人位姿下，激光雷达扫描到的 “空旷区域” 与融合系统检测到的 “玻璃区域” 重叠（即粒子认为该区域空旷，但实际是玻璃），则降低该粒子的权重（权重衰减系数 0.6）；若粒子预测的位姿下，激光点云与玻璃区域无重叠，且与普通障碍匹配度高，则提高权重（权重增益系数 1.2）。通过这一修正，粒子滤波能更准确地筛选出符合实际环境的机器人位姿，减少定位漂移。此外，还对 Gmapping 的地图更新频率进行调整 —— 原始算法每接收一帧激光数据更新一次地图，优化后改为每接收 3 帧激光数据并结合 1 次视觉玻璃特征更新，避免因玻璃特征提取的延迟导致地图频繁修正，提高地图稳定性。
最后是优化后 SLAM 算法的地图构建流程与性能评价。优化后的 Gmapping 算法地图构建流程分为四步：第一步是初始化，设定机器人初始位姿（原点）、地图分辨率（0.05m / 栅格），启动粒子滤波（粒子数量 50）；第二步是位姿预测，通过机器人里程计数据（轮式编码器）预测每个粒子的当前位姿；第三步是观测更新，接收多传感器融合系统的玻璃障碍信息和激光雷达数据，计算每个粒子的权重（结合激光匹配度与玻璃特征约束），并重采样保留高权重粒子；第四步是地图更新，根据重采样后的粒子位姿，将激光雷达检测到的普通障碍和融合系统检测到的玻璃障碍分别标记到占据栅格地图中，同时更新地图的概率值（普通障碍栅格概率≥0.8，玻璃障碍栅格概率 = 0.9，空旷栅格概率≤0.2）。为验证优化效果，在相同办公室玻璃场景中进行对比实验：优化前 Gmapping 构建的地图中，3 块玻璃隔断均未被标记，出现明显空洞；优化后地图中，玻璃隔断被准确标记，空洞完全消除，且玻璃区域的位置误差≤3cm（与实际玻璃位置对比）。定位精度方面，优化前机器人在场景中运动 10min 后的定位误差为 12cm，优化后降至 6cm，定位稳定性显著提升。地图构建时间方面，优化后 50㎡场景约 3.5min，仅比原始算法增加 0.5min，在可接受范围内。此外，还在含动态玻璃门（开关频率 1 次 / 30s）的场景中测试，优化后的算法能通过视觉相机捕捉玻璃门的运动状态，实时更新地图中的玻璃障碍位置，确保机器人不会因玻璃门关闭而误判路径，进一步验证了算法在动态玻璃场景中的适应性。
（3）混合路径规划算法设计与实验平台验证
自主导航的最终目标是让机器人在玻璃环境中安全、高效地到达目标点，这需要路径规划算法结合 SLAM 构建的地图，规划出无碰撞路径并实现动态避障。本研究提出 “改进 A * 算法（全局路径规划）+ 改进 DWA 算法（局部动态避障）” 的混合路径规划方案，既保证全局路径的最优性，又能应对玻璃环境中的动态干扰（如行人、临时障碍物）。
首先是全局路径规划算法改进 —— 基于玻璃障碍代价的 A算法优化。传统 A算法通过 “启发函数 h (n)+ 代价函数 g (n)” 计算节点代价 f (n)=g (n)+h (n)，其中 g (n) 是起点到节点 n 的实际距离，h (n) 是节点 n 到终点的预估距离（通常用曼哈顿距离或欧氏距离），算法通过寻找最小 f (n) 的节点实现路径搜索。但传统 A算法对所有障碍的代价设置相同（如障碍节点代价为无穷大），未考虑玻璃障碍的特殊性 —— 玻璃虽为障碍，但碰撞后果（如玻璃破碎）比普通障碍更严重，且机器人靠近玻璃时易受反射干扰，因此需在路径规划中尽量远离玻璃。为此，本研究对 A算法的代价函数进行改进：一是引入玻璃障碍的 “安全距离代价”，在占据栅格地图中，以玻璃障碍栅格为中心，设置半径 0.5m 的 “安全缓冲区”（根据机器人尺寸设定，机器人直径 0.3m，缓冲区确保机器人与玻璃的最小距离≥0.1m），缓冲区内的非障碍栅格赋予额外代价（距离玻璃越近，代价越高）—— 如玻璃栅格周边 1 栅格（0.05m）的代价为 100，2 栅格（0.1m）的代价为 80，…，10 栅格（0.5m）的代价为 20，缓冲区外的普通空旷栅格代价为 10
#include<ros/ros.h>
#include<sensor_msgs/LaserScan.h>
#include<sensor_msgs/Image.h>
#include<std_msgs/Float32MultiArray.h>
#include<geometry_msgs/PointArray.h>
class SensorFusion{
public:
SensorFusion(){
sub_laser = nh.subscribe("/scan",10,&SensorFusion::laserCallback,this);
sub_camera = nh.subscribe("/image_raw",10,&SensorFusion::cameraCallback,this);
sub_ultrasonic = nh.subscribe("/ultrasonic",10,&SensorFusion::ultrasonicCallback,this);
pub_glass = nh.advertise<geometry_msgs::PointArray>("/glass_obstacle",10);
}
void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
laser_data = *msg;
}
void cameraCallback(const sensor_msgs::Image::ConstPtr& msg){
camera_data = *msg;
detectGlassContour();
}
void ultrasonicCallback(const std_msgs::Float32MultiArray::ConstPtr& msg){
ultrasonic_data = *msg;
filterUltrasonicData();
}
void detectGlassContour(){
glass_contour.clear();
for(int i=0;i<camera_data.height;i++){
for(int j=0;j<camera_data.width;j++){
int idx = (i*camera_data.width + j)*3;
uint8_t b = camera_data.data[idx];
uint8_t g = camera_data.data[idx+1];
uint8_t r = camera_data.data[idx+2];
if((r>200&&g>200&&b>200)||(abs(r-g)<10&&abs(g-b)<10&&abs(r-b)<10)){
geometry_msgs::Point pt;
pt.x = (j - 640)/100.0;
pt.y = (360 - i)/100.0;
pt.z = 0.0;
glass_contour.push_back(pt);
}
}
}
}
void filterUltrasonicData(){
filtered_ultrasonic.clear();
for(int i=0;i<ultrasonic_data.data.size();i++){
float dist = ultrasonic_data.data[i];
if(dist>0.02&&dist<4.0){
if(filtered_ultrasonic.empty()){
filtered_ultrasonic.push_back(dist);
}else{
float avg = 0.0;
for(float d:filtered_ultrasonic) avg += d;
avg /= filtered_ultrasonic.size();
if(abs(dist - avg)<0.2) filtered_ultrasonic.push_back(dist);
}
}
}
}
void fuseData(){
geometry_msgs::PointArray glass_msg;
float angles[] = {0.0, 1.57, 3.14, 4.71};
for(int i=0;i<filtered_ultrasonic.size();i++){
float dist = filtered_ultrasonic[i];
float angle = angles[i];
bool is_glass = false;
for(auto& pt:glass_contour){
float x = dist*cos(angle);
float y = dist*sin(angle);
if(abs(pt.x - x)<0.1&&abs(pt.y - y)<0.1){
is_glass = true;
break;
}
}
if(is_glass){
geometry_msgs::Point pt;
pt.x = x;
pt.y = y;
pt.z = 0.0;
glass_msg.points.push_back(pt);
}
}
pub_glass.publish(glass_msg);
}
void run(){
ros::Rate rate(10);
while(ros::ok()){
if(!laser_data.ranges.empty()&&!glass_contour.empty()&&!filtered_ultrasonic.empty()){
fuseData();
}
ros::spinOnce();
rate.sleep();
}
}
private:
ros::NodeHandle nh;
ros::Subscriber sub_laser,sub_camera,sub_ultrasonic;
ros::Publisher pub_glass;
sensor_msgs::LaserScan laser_data;
sensor_msgs::Image camera_data;
std_msgs::Float32MultiArray ultrasonic_data;
std::vector<geometry_msgs::Point> glass_contour;
std::vector<float> filtered_ultrasonic;
};
int main(int argc,char** argv){
ros::init(argc,argv,"sensor_fusion_node");
SensorFusion fusion;
fusion.run();
return 0;
}
如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
扫一扫
5万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
