清洁机器人也会“认路+捡瓶子”？这台 VSLAM 小车把懒人科技玩明白了 🧹🤖

项目主题：基于 VSLAM 的自主移动清洁机器人 来源：第三届 3D 视觉创新应用竞赛 | 李千一 | 华南师范大学 相机型号：Astra Pro 支持设备：Jetson Nano、STM32F407 源代码链接：RobotV_Autonomous-Cleaning

1. 项目一眼看懂：让机器人自己找路、找垃圾、再动手收拾 🧭

如果说普通清洁工具是“人往哪推，它往哪走”，那本项目要做的就是让机器人具备一点“空间感”和“行动力”：它能用 Astra Pro 深度相机感知环境，在 Jetson Nano 上完成视觉建图与路径规划，再通过 STM32F407 控制底盘和机械臂，实现自主移动、目标识别、垃圾抓取与搬运。

项目的典型任务链条可以概括为：

看见环境 -> 构建地图 -> 自主导航 -> 识别垃圾 -> 靠近目标 -> 机械臂抓取 -> 运回放置

这里的“垃圾”在工程测试中主要以 bottle 作为检测类别，相当于先用瓶子做一个可控验证对象。后续若替换训练数据和识别类别，就可以扩展到垃圾袋、饮料盒、纸团等更贴近生活区清洁的目标。换句话说，现在是“捡瓶子练手”，未来可以升级成“小区清洁小管家”。

2. 为什么需要这样的机器人？小区清洁也想“自动挡” 🏙️

项目背景聚焦智慧城市与生活小区卫生管理。传统垃圾清理依赖人工巡查，容易出现垃圾堆积时间长、堆放空间脏乱、清洁响应不及时等问题。更麻烦的是，在公共生活区频繁接触垃圾，也会增加病菌传播风险。清洁机器人如果能稳定完成巡检与回收，就能把一部分重复性、低附加值、环境不友好的工作交给机器处理。

场景需求	传统痛点	项目对应能力
小区垃圾巡检	依赖人工定时巡查	自主移动到预设区域
垃圾目标发现	人眼识别费时费力	YOLOv3 检测目标
室内外路径移动	路线固定但障碍变化	move_base 动态规划
垃圾搬运	人工接触不卫生	机械臂抓取与放置
成本控制	激光雷达成本较高	深度相机替代部分激光感知

3. 硬件组成：一台小车，五脏俱全 🛠️

项目系统由感知、计算、控制、执行四类硬件组成。Astra Pro 负责获取 RGB-D 数据，Jetson Nano 负责运行 ROS、SLAM、导航和目标检测，STM32F407 负责底层电机与机械臂控制，麦轮底盘负责移动，四自由度舵机机械臂负责抓取。

模块	选型	作用
深度相机	Astra Pro	RGB-D 采集、VSLAM 建图、深度感知
上位机	Jetson Nano 4GB	ROS、ORB-SLAM2、导航、YOLO 推理
下位机	STM32F407	电机、舵机、底盘控制
移动平台	麦轮 + 电机	支持全向移动与路径执行
操作机构	四自由度舵机机械臂	目标抓取与放置
电源	12V 锂电池	为移动平台与执行机构供电

4. 软件架构：ROS 是“总调度”，各模块各司其职 🧩

代码压缩包 code.zip 中的工程主要由几个 ROS 工作空间/功能包组成。整体看，它不是单点算法演示，而是把导航、识别、抓取、底盘控制都串成了完整流程。

功能模块	关键路径	主要作用
VSLAM 建图	`iMAGE/wheeltec_robot/src/ORB_SLAM2_modified`	RGB-D 建图、稠密点云输出
点云转栅格	`iMAGE/wheeltec_robot/src/pcd2pgm`	PCD 点云滤波并转为 OccupancyGrid
深度转激光	`iMAGE/wheeltec_robot/src/depthimage_to_laserscan-melodic-devel`	将深度图转换为 LaserScan
导航底盘	`iMAGE/wheeltec_robot/src/turn_on_wheeltec_robot`	AMCL、move_base、TEB、底盘串口
目标检测	`iMAGE/wheeltec_robot/src/darknet_ros`	YOLOv3 目标检测
任务调度	`iMAGE/send_goal/src/send_goals`	巡检点发送、抓取流程调度
机械臂抓取	`iMAGE/arm/src/wheeltec_arm_pick`	视觉对准、MoveIt! 抓取、放置

5. 任务闭环：机器人不是乱逛，它有剧本 📜

项目的工作逻辑很清晰：先建图，再导航；导航到目标点后启动识别；识别到目标则抓取并返航放置；没有识别到目标就继续搜索下一个点。

send_goals_node.cpp 体现了这个任务闭环。代码中先启动 darknet_ros 和导航，再依次向 move_base 发送目标点：

system("gnome-terminal -- bash /home/wheeltec/send_goal/src/script/darknet_ros.sh");
ros::Duration(10).sleep();
system("gnome-terminal -- bash /home/wheeltec/send_goal/src/script/navigation.sh");

MoveBaseClient ac("move_base", true);
ac.sendGoal(goal[i]);
ac.waitForResult();

当检测状态为 find_bootle 时，系统会认为目标已找到，切换为返航放置逻辑：

if ("find_bootle" == find_bootle_state)
{
    box = 1;
    i = goal_number - 2;  // 下一步回到原点放下
    pub_locate.publish(msg_pose);
}
else if ("no_bootle" == find_bootle_state)
{
    box = 0;
    find_bootle_state = "none";
}

这段代码虽然朴素，但项目流程很完整：导航不是导航完就结束，而是要和识别、抓取、返航协同工作。机器人终于不是“走到点位发呆”的机器人了，它有后续动作。

6. 关键技术一：ORB-SLAM2 建图，先让机器人知道“世界长啥样” 🗺️

项目最初尝试过 ORB-SLAM3，但在 Jetson Nano 上实时帧率不足、内存压力较大，难以支撑长时间大范围建图。因此团队最终选择更节省计算资源的 ORB-SLAM2 作为 VSLAM 建图核心。

ORB-SLAM2 主要由三个线程构成：

线程	作用	对项目的意义
Tracking	提取 ORB 特征并估计相机位姿	让小车知道当前位置
Local Mapping	插入关键帧、生成局部地图点	建立局部空间结构
Loop Closing	回环检测与全局优化	减少累计漂移

项目对 ORB-SLAM2 做了一个关键改造：不仅保存灰度图，还保存 RGB 图，使生成的点云从灰度点云升级为彩色点云。这样调试人员在 RViz 或 PCL 可视化中更容易判断环境结构。

在 Tracking.h 中新增 RGB 图像缓存：

Frame mCurrentFrame;
cv::Mat mImRGB;   // 保存彩色图像
cv::Mat mImGray;
cv::Mat mImDepth;

在 Tracking.cc 的 RGB-D 输入处理中保存彩色图和深度图：

cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,
                                const cv::Mat &imD,
                                const double &timestamp)
{
    mImRGB = imRGB;
    mImGray = imRGB;
    mImDepth = imD;

    if ((fabs(mDepthMapFactor - 1.0f) > 1e-5) || mImDepth.type() != CV_32F)
        mImDepth.convertTo(mImDepth, CV_32F, mDepthMapFactor);

    mCurrentFrame = Frame(mImGray, mImDepth, timestamp,
                          mpORBextractorLeft, mpORBVocabulary,
                          mK, mDistCoef, mbf, mThDepth);
    Track();
    return mCurrentFrame.mTcw.clone();
}

彩色点云生成逻辑位于 pointcloudmapping.cc。它根据 RGB-D 图像恢复三维点，并把颜色写入点云：

p.z = d;
p.x = (n - kf->cx) * p.z / kf->fx;
p.y = (m - kf->cy) * p.z / kf->fy;

p.b = color.ptr<uchar>(m)[n * 3];
p.g = color.ptr<uchar>(m)[n * 3 + 1];
p.r = color.ptr<uchar>(m)[n * 3 + 2];

最终保存点云：

pcl::io::savePCDFileBinary("vslam.pcd", *globalMap);

这一步相当于把“相机看到的一帧帧画面”，转成“机器人可以理解的空间地图”。机器人从此不只是看热闹，而是开始认路了。

7. 关键技术二：点云转栅格地图，把“三维世界”压成“导航地图” 🧱

导航系统常用的是二维栅格地图，而 ORB-SLAM2 改造后输出的是三维点云。项目通过 pcd2pgm 包完成点云滤波与栅格化。

核心流程如下：

pcd2pgm.cpp 中使用直通滤波和半径滤波清理点云：

PassThroughFilter(thre_z_min, thre_z_max, bool(flag_pass_through));
RadiusOutlierFilter(cloud_after_PassThrough, thre_radius, thres_point_count);
SetMapTopicMsg(cloud_after_Radius, map_topic_msg);

栅格化时，代码根据点云边界确定地图原点和尺寸，再把点云位置投影到二维栅格：

msg.info.resolution = map_resolution;
msg.info.origin.position.x = x_min;
msg.info.origin.position.y = y_min;

msg.info.width = int((x_max - x_min) / map_resolution);
msg.info.height = int((y_max - y_min) / map_resolution);

int i = int((cloud->points[iter].x - x_min) / map_resolution);
int j = int((cloud->points[iter].y - y_min) / map_resolution);
msg.data[i + j * msg.info.width] = 100;

100 表示该栅格被障碍物占据。对导航来说，这就是“这里别去，容易撞”的朴素真理。

8. 关键技术三：深度图转 LaserScan，用相机省下一台“贵贵雷达” 📡

项目希望采用视觉方案替代成熟但成本较高的激光方案。Astra Pro 本身能提供深度图，因此可以用 depthimage_to_laserscan 将深度图转换为类似激光雷达的 LaserScan 数据，供 AMCL 和 move_base 使用。

在 navigation_farplanner.launch 中，系统启动 Astra Pro 并把深度图重映射到 /scan：

<include file="$(find astra_camera)/launch/astrapro.launch" />

<node pkg="depthimage_to_laserscan"
      type="depthimage_to_laserscan"
      name="depthimage_to_laserscan">
  <remap from="image" to="/camera/depth/image_raw" />
  <param name="output_frame_id" value="laser1" />
  <remap from="scan" to="/scan" />
  <param name="range_min" type="double" value="0.45" />
  <param name="range_max" type="double" value="4" />
  <param name="scan_height" type="int" value="50" />
</node>

这段配置非常关键：它把深度相机的三维信息压缩成二维扫描数据，让传统导航栈可以继续工作。就像让相机临时戴上“激光雷达工牌”，虽然岗位不同，但能把活干起来。

9. 关键技术四：AMCL + move_base + TEB，让小车稳稳去目标点 🚗

项目导航使用 ROS navigation 体系：multi_map_server 加载地图，amcl_omni 进行定位，move_base 执行全局与局部路径规划，局部规划器选用 TEB。

在 navigation_farplanner.launch 中可以看到完整链路：

<node pkg="multi_map_server"
      type="multi_map_server"
      name="multi_map_server"
      args="$(arg map2)" />

<include file="$(find turn_on_wheeltec_robot)/launch/include/amcl_omni.launch" />
<include file="$(find turn_on_wheeltec_robot)/launch/include/teb_local_planner.launch" />

move_base_params.yaml 中指定全局规划器和局部规划器：

base_global_planner: "global_planner/GlobalPlanner"
base_local_planner: "teb_local_planner/TebLocalPlannerROS"
controller_frequency: 10.0
planner_frequency: 1.0
recovery_behavior_enabled: true

TEB 参数中配置了麦轮小车的运动约束与形状：

max_vel_x: 0.5
max_vel_y: 0.3
max_vel_theta: 1.5
acc_lim_x: 0.2
acc_lim_y: 0.2
acc_lim_theta: 0.3

footprint_model:
  type: "polygon"
  vertices: [[-0.133, -0.125], [-0.133, 0.125],
             [0.133, 0.125], [0.133, -0.125]]

导航组件	输入	输出
multi_map_server	`map.yaml`	`/map`
AMCL	`/scan`、`/map`、`/tf`	`/amcl_pose`
GlobalPlanner	起点、目标点、全局代价地图	全局路径
TEB Local Planner	全局路径、局部障碍、速度约束	`/cmd_vel`
STM32 底盘节点	`/cmd_vel`	电机控制

10. 关键技术五：YOLOv3 识别垃圾，机器人终于知道“该捡谁” 🥤

项目使用 darknet_ros 部署 YOLOv3，并针对 bottle 类别进行训练和检测。由于 Jetson Nano 算力有限，团队从标准 YOLOv3 转向 YOLOv3-tiny，以换取可运行的实时性。

my_yolov3.yaml 中定义了模型配置：

yolo_model:
  config_file:
    name: my_yolov3-tiny.cfg
  weight_file:
    name: my_yolov3-tiny_380000.weights
  threshold:
    value: 0.3
  detection_classes:
    names:
      - bottle

ros.yaml 中定义图像订阅和检测结果发布：

subscribers:
  camera_reading:
    topic: /usb_cam/image_raw
    queue_size: 1

publishers:
  bounding_boxes:
    topic: /darknet_ros/bounding_boxes
    queue_size: 1

检测节点会输出边界框，后续视觉对准模块读取边界框中心点，用于控制小车靠近目标。识别模块的价值不只是“看见瓶子”，而是为抓取提供目标方位。它相当于告诉机器人：“对，就是这个，别把地砖当垃圾。”

11. 关键技术六：视觉对准 + MoveIt! 抓取，动手能力安排上了 🦾

识别到目标后，机器人还需要靠近并抓取。visualTracker.py 订阅 YOLO 检测框和深度图，计算目标相对图像中心的角度偏差，并发布给底盘对准模块。

在 visualTracker.py 中，检测框中心点被转成角度：

x_data = (data.bounding_boxes[0].xmin + data.bounding_boxes[0].xmax) / 2.0
y_data = data.bounding_boxes[0].ymax * 1.0

angleX = self.calculateAngleX(x_data)
angleY = self.calculateAngleY(y_data)

posMsg = PositionMsg(angleX, angleY, 0.0)
self.positionPublisher.publish(posMsg)

car_location_color.cpp 根据角度偏差做 PID 控制，让底盘微调位置：

distance_y = msg.angleX;
distance_x = msg.angleY;

target_liner_x = PID_control_x();
target_liner_y = PID_control_y();

if ((abs_float(target_liner_x) < 0.05) &&
    (abs_float(target_liner_y) < 0.05) &&
    location_flag == 0)
{
    count++;
}

当位置调整完成后，系统发布机械臂状态：

if ((location_flag == 1) && (move_flag < 2))
    msg.data = "pick";
else if (move_flag == 2)
    msg.data = "put";
else
    msg.data = "none";

机械臂抓取动作由 MoveIt! 执行。arm_pick_and_put.cpp 中定义了完整抓取和放置流程：

void arm_pick()
{
    hand.setNamedTarget("hand_open");  hand.move(); sleep(1);
    arm.setNamedTarget("arm_clamp");   arm.move();  sleep(1);
    hand.setNamedTarget("hand_close"); hand.move(); sleep(1);
    arm.setNamedTarget("arm_tmp_wait"); arm.move(); sleep(1);
    arm.setNamedTarget("arm_tmp"); arm.move(); sleep(1);
    hand.setNamedTarget("hand_open"); hand.move(); sleep(1);
}

这一段很有“机器人做家务”的味道：先张开手，伸过去，夹住，再抬起来，最后去放置。虽然动作看起来分步很机械，但这正是工程里最可靠的做法，别让机器人现场即兴发挥，容易变成“清洁事故现场”。

12. 底层控制：Jetson 负责思考，STM32 负责干活 ⚙️

Jetson Nano 运行 ROS 上位机逻辑，STM32F407 则负责更靠近硬件的控制。wheeltec_robot_and_arm.cpp 中可以看到，系统将 ROS 的 /cmd_vel 转换为串口协议发送给 STM32。

void turn_on_robot::Cmd_Vel_Callback(const geometry_msgs::Twist &twist_aux)
{
    Send_Data.tx[0] = FRAME_HEADER;
    Send_Data.tx[1] = 1;
    Send_Data.tx[2] = 0;

    transition = twist_aux.linear.x * 1000;
    Send_Data.tx[4] = transition;
    Send_Data.tx[3] = transition >> 8;

    transition = twist_aux.linear.y * 1000;
    Send_Data.tx[6] = transition;
    Send_Data.tx[5] = transition >> 8;

    transition = twist_aux.angular.z * 1000;
    Send_Data.tx[8] = transition;
    Send_Data.tx[7] = transition >> 8;

    Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
    Send_Data.tx[10] = FRAME_TAIL;
    Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
}

这就是典型的机器人分层控制：上位机做感知、规划和决策，下位机做实时控制与执行。Jetson 像“脑子”，STM32 像“手脚神经”，谁也别抢谁的活。

13. 测试与部署：从办公室走廊开始，稳扎稳打 🧪

根据项目报告，团队在 Ubuntu 16.04 与 ROS 环境下部署系统，地图构建、导航和抓取主要使用 C++，物体识别部分使用 Python，并基于奥比中光 OpenNI2 SDK 获取相机数据。

测试环节	验证目标	观察重点
Jetson + STM32 通信	底盘和舵机能否响应	串口稳定性、控制延迟
ORB-SLAM2 建图	能否生成可用点云	帧率、内存、地图质量
点云转栅格	能否用于导航	障碍边界、地图分辨率
AMCL 定位	能否在地图中定位	粒子收敛、位姿漂移
move_base 导航	能否到达目标点	路径稳定性、避障效果
YOLOv3 检测	能否识别 bottle	帧率、误检、漏检
机械臂抓取	能否完成夹取放置	对准精度、动作成功率

项目也记录了一个重要工程取舍：由于 Jetson Nano 资源有限，建图线程与导航线程被分离，避免 ORB-SLAM2、导航、识别等模块同时运行造成内存压力。这个选择很务实，机器人项目里“能稳定跑起来”往往比“所有功能同时炫起来”更重要。

14. 难点与创新：这台小车不只是“会动” 🧠

类别	难点	解决思路
算力受限	Jetson Nano 无法长时间稳定跑 ORB-SLAM3	改用 ORB-SLAM2
成本控制	激光雷达成本高	Astra Pro 深度相机替代部分激光感知
地图转换	点云不能直接用于二维导航	PCD 转栅格地图
目标检测	标准 YOLOv3 计算量大	使用 YOLOv3-tiny
抓取闭环	检测结果不能直接抓	视觉对准 + MoveIt! 动作序列
多模块协同	导航、识别、机械臂容易互相打架	ROS 话题和脚本进行流程调度

项目的创新点可以概括为三句话：

用视觉 SLAM 和深度相机降低移动清洁机器人的传感器成本。

将导航、识别、机械臂抓取整合成完整清洁任务链路。

针对 Jetson Nano 算力约束进行工程取舍，提升系统可运行性。

15. 总结：让清洁机器人从“会跑”进化到“会干活” ✅

基于 VSLAM 的自主移动清洁机器人把 Astra Pro 深度相机、ORB-SLAM2 建图、点云栅格化、depthimage_to_laserscan、AMCL、move_base、YOLOv3 和 MoveIt! 机械臂控制串联起来，构成了一个面向生活区清洁的完整机器人系统。

它的亮点不在于单个算法多么花哨，而在于把多个成熟模块整合成一个真实可执行的任务流程：机器人知道自己在哪，知道要去哪，能识别目标，还能伸手抓取并带回。对于智慧小区、公共空间巡检、垃圾回收辅助等应用来说，这类系统具备很好的扩展价值。

一句话总结：它不是一台只会转圈的清洁小车，而是一位正在学习“看地图、找垃圾、动手收拾”的机器人实习生。表现不错，值得转正。🤖