1. 项目概述

本文围绕面向城市环境的智能垃圾抓取机器人系统展开，重点介绍感知、决策与执行三大核心模块的设计与实现。系统以 OpenCV 实现颜色识别与目标定位，在树莓派上构建有限状态机完成任务调度，并基于 STM32 开发电机 PID 控制与机械臂逆运动学算法，形成一套从环境感知到抓取执行的完整闭环。

2. 系统总体架构

下图展示了感知、决策与执行三个模块之间的数据流与控制流关系，以及各模块间的接口通信和闭环控制回路。

flowchart TD
    A[感知模块
OpenCV 颜色识别与目标定位] -- 识别结果/目标坐标 --> B[决策模块
树莓派有限状态机任务调度]
    B -- 控制指令/目标坐标 --> C[执行模块
STM32 电机PID控制与机械臂逆运动学]
    C -- 执行状态/完成反馈 --> B
    B -- 巡检/识别指令 --> A
    C -- 编码器转速反馈 --> C

整个机器人系统采用分层架构，自上而下分为感知模块、决策模块和执行模块三个层次。感知模块负责采集环境信息并识别目标垃圾；决策模块根据感知结果进行任务调度与状态切换；执行模块驱动底盘电机和机械臂完成实际抓取动作。

三个模块之间通过明确的接口进行通信：感知模块将识别结果以结构化数据发送给决策模块，决策模块解析后生成控制指令下发至执行模块，执行模块完成动作后反馈状态信息，形成闭环控制。

3. 感知模块：基于 OpenCV 的颜色识别与目标定位

感知模块是整个系统的"眼睛"，负责从摄像头采集的图像中识别目标垃圾并计算其在空间中的位置。本系统采用 OpenCV 作为核心视觉处理库，通过颜色特征实现垃圾的快速识别与定位。

3.1 颜色空间转换与阈值分割

摄像头采集的图像通常为 RGB 色彩空间，但 RGB 对光照变化较为敏感。为提高识别稳定性，系统先将图像从 RGB 转换到 HSV 色彩空间。HSV 将色调、饱和度和明度分离，能够更好地适应不同光照条件下的颜色识别。

转换完成后，通过设定各颜色通道的阈值范围，使用 cv2.inRange 函数生成二值掩膜，将目标颜色的区域从背景中分离出来。针对城市环境中常见的垃圾类型，系统预设了多种颜色阈值，包括塑料瓶的蓝色、易拉罐的红色和纸箱的黄色等。

3.2 轮廓检测与目标筛选

得到二值掩膜后，使用 cv2.findContours 提取目标区域的轮廓。由于噪声和光照干扰，原始轮廓中可能包含大量小面积噪点，需要通过面积阈值进行过滤，剔除过小或过大的非目标区域。

对筛选后的轮廓，使用 cv2.minAreaRect 或 cv2.boundingRect 计算最小外接矩形，从而获得目标在图像中的中心坐标、宽度和高度。这些信息将作为后续目标定位和抓取规划的基础数据。

3.3 目标定位与坐标转换

图像坐标系中的目标位置需要转换为机器人坐标系中的实际位置，才能指导机械臂完成抓取。系统通过摄像头标定获取内参矩阵和畸变系数，结合摄像头安装高度和俯仰角，利用透视变换将像素坐标映射到地面坐标系。

定位结果以目标中心点的三维坐标形式输出，包括相对于机器人底座的 X、Y 距离和高度 Z。该坐标数据通过串口或网络接口发送给决策模块，作为任务调度的输入参数。

4. 决策模块：基于树莓派的有限状态机任务调度

决策模块是整个系统的"大脑"，运行在树莓派上，负责根据感知模块的识别结果进行任务规划与调度。本系统采用有限状态机模型，将机器人的工作流程划分为若干明确的状态，通过状态转移实现任务的顺序执行和异常处理。

4.1 状态定义与划分

根据城市垃圾抓取的实际作业流程，系统定义了以下核心状态：待机状态、巡检状态、识别状态、接近状态、抓取状态、放置状态和异常处理状态。每个状态对应一组明确的动作和退出条件。

待机状态是系统的初始状态，机器人静止等待启动指令；巡检状态下机器人沿预设路径移动并持续采集图像；识别状态对采集到的图像进行目标检测，判断是否存在可抓取的垃圾；接近状态控制机器人向目标移动；抓取状态驱动机械臂完成抓取动作；放置状态将垃圾放入回收箱；异常处理状态应对识别失败、抓取失败等异常情况。

4.2 状态转移逻辑

状态转移由事件驱动，事件来源包括感知模块的识别结果、执行模块的反馈信号以及定时器超时等。例如，当识别状态检测到目标垃圾时，触发"目标确认"事件，状态从识别转移到接近；当机械臂完成抓取并反馈成功信号时，触发"抓取完成"事件，状态从抓取转移到放置。

有限状态机的实现采用状态转移表配合事件分发机制，每个状态对应一个处理函数，函数内部根据当前事件执行相应动作并返回下一状态。这种设计使任务调度逻辑清晰、易于调试和扩展。

4.3 树莓派与各模块通信

树莓派作为决策中心，需要与感知模块和执行模块进行数据交互。感知模块的视觉处理可直接运行在树莓派上，通过 OpenCV 库完成；执行模块的 STM32 则通过串口与树莓派通信，接收控制指令并返回执行状态。

通信协议采用自定义的帧格式，包含帧头、数据长度、指令类型、数据体和校验位。树莓派通过串口发送目标坐标和抓取指令，STM32 解析后执行相应的电机控制和机械臂动作，并将完成状态回传。

5. 执行模块：基于 STM32 的电机 PID 控制与机械臂逆运动学

执行模块是整个系统的"手脚"，基于 STM32 微控制器实现。该模块负责底盘电机的运动控制和机械臂的关节驱动，通过 PID 控制算法保证运动的平稳性和准确性，通过逆运动学算法计算机械臂各关节的目标角度。

5.1 电机 PID 控制

底盘采用直流减速电机驱动，通过增量式 PID 算法实现速度闭环控制。编码器实时采集电机转速，与目标速度比较后计算偏差，PID 控制器根据比例、积分、微分三个环节的输出调节 PWM 占空比，从而精确控制电机转速。

PID 参数的整定采用先比例、后积分、再微分的顺序进行。比例系数决定响应速度，积分系数消除稳态误差，微分系数抑制超调。实际调试中，通过阶跃响应测试逐步调整参数，使系统在响应速度和稳定性之间取得平衡。

5.2 机械臂运动学建模

机械臂采用多关节结构，每个关节由独立的舵机或电机驱动。为控制机械臂末端到达目标位置，需要建立机械臂的运动学模型。正运动学根据各关节角度计算末端位置，逆运动学则根据目标位置反解各关节角度。

本系统采用几何法求解逆运动学。对于三自由度机械臂，根据末端目标坐标，利用余弦定理依次求解肩关节、肘关节和腕关节的目标角度。求解过程中需要考虑关节角度限位，避免机械臂运动到不可达区域。

5.3 抓取动作控制流程

当决策模块下发抓取指令和目标坐标后，STM32 首先通过逆运动学计算各关节目标角度，然后按照规划的运动轨迹逐步驱动关节运动。运动过程中，PID 控制器实时调节各关节电机的输出，保证运动平稳。

机械臂末端安装有夹爪，夹爪的开合由独立舵机控制。当机械臂末端到达目标位置后，夹爪闭合抓取垃圾，随后机械臂抬升并移动到放置位置，夹爪张开释放垃圾，完成一次完整的抓取任务。

6. 系统联调与实验验证

三个模块开发完成后，需要进行系统联调，验证整体功能的正确性和稳定性。联调过程分为接口测试、功能测试和场景测试三个阶段。

接口测试阶段验证树莓派与 STM32 之间的串口通信是否正常，数据帧解析是否正确；功能测试阶段分别验证颜色识别、状态调度和电机控制各模块的独立功能；场景测试阶段在模拟城市环境中进行完整的垃圾抓取实验，检验系统在真实条件下的表现。

实验结果表明，系统在光照良好的条件下能够稳定识别目标垃圾并完成抓取任务，颜色识别准确率和抓取成功率均达到预期目标。在光照变化或目标遮挡等复杂场景下，系统仍存在一定的优化空间。

7. 总结与展望

本文设计并实现了一套面向城市环境的智能垃圾抓取机器人系统，通过感知、决策、执行三个模块的协同工作，实现了垃圾的自动识别、定位与抓取。系统采用 OpenCV 进行颜色识别，有限状态机进行任务调度，STM32 配合 PID 控制和逆运动学算法完成执行动作，整体方案结构清晰、可扩展性强。

未来工作可以从以下几个方面展开：引入深度学习目标检测算法以提升复杂环境下的识别鲁棒性；优化机械臂轨迹规划算法以提高抓取效率；增加多传感器融合以增强系统对环境变化的适应能力。