基于STM32的智能物流机器人

搬运机器人可代替人工在高温、高压、多粉尘等危险环境中作业，节省大量人力资源，大幅提高工作效率。在未来智慧工厂建设中，智能物流搬运机器人技术起着举足轻重的作用^[1]。

鉴于智能物流搬运机器人将来在制造业发展中势必占有较重要的地位，本文设计实现了一种基于STM32的智能物流机器人系统，通过智能车为平台搭载机械臂和摄像头，运用AprilTag 标定跟踪与PID 控制^[2] 实现机器人的移动与跟踪抓取。最终实现自动导航以及智能识别并搬运物品的功能。

1 机器人工作原理

1.1 PID控制算法

PID 控制是指比例、积分、微分控制。将系统的测量反馈经过比例、积分、微分3 种运算后，叠加至输入中，达到控制的目的。其原理简单、易于实现，被应用于多种领域。常用的PID 算法有两种，一种是位置式PID，另一种是增量式PID，两者应用场合不同，各有优点。位置式PID 算法的表达式为：

其中，K_p为比例放大系数，T_i为积分时间常数，T_d为微分时间常数。

位置式PID 算法是根据当前实际测得的输出与要达到的预期输出的偏差e(k)，进行 PID 运算。由于积分项会对每一次的误差进行累加，使其运算量较大，且每次输出均与过去的状态相关，可能会引起大幅度超调。适用于执行机构不带积分部件的对象，如电液伺服阀。

增量式PID 算法的表达式为：

增量式 PID 公式是由将位置式 PID 的输出 u(k) −u(k−1) = Δu(k)得到，而不是对应与实际位置的偏差，没有误差累积。也就是说增量式PID 中不需要累加。控制增量Δu(k)的确定仅与最近 3 次的采样值有关。所以增量式PID 容易通过加权处理获得比较好的控制效果，相较于位置式PID 算法，在系统发生问题时，增量式PID 不会严重影响系统的工作。适用于执行机构带积分部件的对象，如步进电机等。

图1 PID控制系统图

1.2 视觉定位算法

AprilTag 是一种视觉基准系统，通过特定的标志快速检测出相对位置，AprilTag 库不需要依赖，可以轻松移植到嵌入式装备中。AprilTag 视觉定位^[3] 实现主要分为三个部分：边缘检测，四边形检测，匹配与解析，其流程如图2所示。

图2 视觉定位算法流程图

首先对图像提取边缘，查找边缘中边数大于四的凸 多边形，在查找到的多边形中筛选出四边形。对四边形中的图像进行编码，白色为0，黑色为1，1 个四边形有4 个编码，分辨是当前图像的编码和进行3 次90°旋转后的编码，计算与库中编码的最小海明距离。如果海明距离小于阈值，则认为其为AprilTag，可确定其ID，并通过矩阵变换得到位姿信息。

1.3 机械臂运动学

机械臂运动学^[4] 研究的是通过建立机械臂运动学模型、分析机械臂各关节的位姿、速度、加速度与机械臂末端在空间中的变换规律。正运动学通过各个关节之间的关系，求出机器人末端执行器的位姿，以关节角度作自变量，建立机械臂的正运动模型。

为方便的表达两个相邻关节连杆的参数关系，需要对所有关节指定一个参考系，D-H 参数法是一种常用的对机器人相邻两个连杆之间参数关系建模的数学方法，其中的参数有5 个：

各个关节坐标系之间的变换矩阵依次相乘，求出从基坐标系到机械臂末端执行器坐标系之间的坐标变换矩阵。

图3 机械臂简化几何模型

2 机器人硬件设计

2.1 硬件总体设计

本文基于STM32 设计的智能物流机器人系统，选取性能较好的硬件配置作为系统运行的外部环境支撑，根据搬运机器人机械系统功能和技术指标，设置硬件总体结构。系统硬件设备包括摄像头模块、OLED 显示器、转向舵机、直流电机，系统硬件结构图，如图4 所示。

图4 系统硬件结构图

智能物流机器人系外部设备较多，各设备之间工作电压不一致。本文采用了LM2596 和AMS1117 两颗降压芯片，调整系统运行时的电压、摄像头模块和机械臂是机器人抓取目标的基础硬件设备、直流电机和转向舵机能控制机器人的移动和方向，OLED 显示系统运行时的各种参数，是调试时的重要依据。

2.2 移动与转向设计

本文所设计的机器人移动采用两路带光电编码器的直流电机，通过STM32 输出PWM 波，同时接收编码器计数，经处理后达到控制调速目的。OLED 模块显示设定速读和实时速度，选择TB6612 模块作为直流电机驱动给电机供电。所有模块形成一个完整的速度闭环控制，能对直流电机稳定快速准确的调速。机器人的转向在舵机与连杆连接两个前轮的方式下，通过STM32 输出PWM 波控制舵机的转动从而带动两个前轮同步转动以达到转向的目的。前轮的束脚^[5] 为0° ，从而在不影响转向效果的前提下能够稳定地直线行驶。

2.3 机械臂设计

本文所建立的机械臂是由6 个舵机及机械零件组成，其中5个舵机组成5个旋转关节，另外1个舵机构成末端执行器机械爪，整体为5自由度，其D-H参数表如表1 所示。

2.4 机器人视觉设计

本文选用OpenMV 模块承担机器人的视觉功能。OpenMV是集摄像头、处理器和调试器于一体的智能摄像头，内含丰富的图像处理实用库，在低算力要求下表现良好。将OpenMV 镶嵌到机械臂上即可通过机械臂控制摄像头的视野以完成机器人的巡线移动或机械臂的抓取定位等功能。

2.5 电源及驱动设计

机器人电源结构如图5 所示。

图5 电源结构图

本文所设计的机器人在满足安全稳定的指标下，采用1 个总电源通过多级降压为机器人的各个模块进行供电。总电源为12 V、6 000 mAH 锂电池， 电源通过TB6612 稳压后给两路直流电机供电，同时通过LM2596S-ADJ 芯片^[6] 将12 V 降为7.5 V 给转向舵机与机械臂的六个舵机供电。再将7.5 V 输入至AMS1117-5.0 芯片输出5 V 至OPENMV H7 模块。最后将5 V 输入至AMS1117-3.3 输出3.3 V 给STM32F103C8T6 主控芯片供电。

3 机器人软件设计

3.1 系统总体框架

系统运行流程如图6 所示。机器人开机之后等待带有二维码的物品出现，检测到物品后，扫描物品上的二维码获取目标货柜ID 信息，通过AprilTag 视觉定位获取物品位置信息，以此控制机械臂进行准确的定位与跟踪抓取并放入机器人的临时货柜中。之后机器人切换为移动状态，摄像头识别地面黑线，通过PID 控制机器人转向与移动直到到达目标位置后将临时货柜中的物品放入目标货柜中。程序采用前后台控制：基本功能和循环等待作为前台程序，中断函数为后台程序。主程序中完成芯片外设的初始化、功能处理；中断函数实现机械臂控制、电机控制、舵机控制、串口数据处理、状态更新。

图6 软件设计流程

3.2 视觉跟踪

摄像头有两种工作模式：抓取和巡线模式。抓取模式提供物品的位置信息，巡线模式提供黑线位置信息，主控芯片管理摄像头的模式。程序流程图如图7 所示。

图7 OpenMV程序流程图

首先初始化摄像头，将模式设为抓取模式，循环执行获得图像、检测AprilTag、如果存在AprilTag 则解析AprilTag位置信息、串口发送数据等操作，直到接收到模式切换命令。当收到巡线命令时，循环执行获得图像、二值化、解析黑线位置和偏角信息、串口发送数据等操作。

3.3 移动与转向控制

机器人的移动主要通过直流电机的转动，为了能准确控制电机转动速度，本文采用了增量式PID 算法来控制电机，其控制流程如图8所示。

图8 调速流程图

首先初始化外设，设置PWM 频率为10 kHz，PID采样频率为50 Hz，即每20 ms 进行一次定时器中断，在定时器中断函数中对读取到的光电编码器计数值进行处理，STM32F103C8T6 的编码器接口计数方向与编码器信号的关系如表2 所示。

当其中一路接口信号跳变时，判断另一路的相对电平确定计数方向，在设置为4 倍频模式下实际运行计数效果如图9所示。

图9 4倍频编码器模式下的计数器操作实例

结合本文所用电机的物理参数，减速比i ，光电编码器线数P ，车轮半径r ，采样频率f ，可得计数值Count与实际速度V 的计算公式为：

将由公式6 得到的实际速度V 带入增量式PID 算法公式2 中即可得到应输出的PWM 量以修正实际测得的误差。机器人的转向主要通过舵机通过连杆带动前轮实现转向，在直线行驶的情况下，黑线在摄像头中应处于图像中间且竖直，由此确定转向的反馈数据为黑线在图中的偏距ρ 和偏角θ 如图10 所示。

图10 摄像头解析图像

ρ 为黑线顶部中点与图像宽度一半的差，θ 为黑线与图像底部的夹角。将偏距ρ 和偏角θ 分别带入位置式PID 算法公式，计算出的数值相加即可得到应输出PWM 波以控制转向舵机修正摄像头观测到的误差。

3.4 机械臂控制

机械臂控制流程如图11 所示。

图11 机械臂控制流程图

在接收到目标物品的相对坐标x、y、z后，先通过正运动学由当前关节角度计算出当前末端执行器的坐标，然后再加上物品的相对坐标，就可得到目标物品的绝对坐标X、Y、Z。通过逆运动学将目标绝对坐标位置转换为目标关节角度，利用增量式PID 完成对机械臂的控制输出。

4 实验结果及分析

整合各模块后，现对各模块进行性能测试，包括电机调速性能、AprilTag 姿态获取范围、机械臂跟踪能力。

4.1 机器人运动控制性能

控制系统性能的评价可以采用动态性能指标分析，即在典型输入信号作用下，计算系统响应的上升时间t_r ，峰值时间t_p ，调节时间t_s ，超调量σ %。以上指标可以满足系统动态过程的特征体现。

在零初始条件下，对系统给定不同的阶跃输入，分别为20、40、60、80 cm/s，测试得到系统的响应如图12 所示。

通过图 12 可知，系统输出最后趋近于给定值，能够满足控制系统最基本的稳定性要求，然后分析其动态指标如表 3。

分析表3 动态指标可知，随着目标速度的增大上升时间，峰值时间都略微增大，调节时间受误差影响较大，平均调节时间为560 ms，从总体上看，系统响应快速，在700 ms 内稳定至目标速度，虽然略微有部分超调，但仍在稳定可靠范围内，不影响整个系统的安全性。

4.2 AprilTag检测范围

实际中二维码可能会从各个角度各种距离面向摄像头，为保证识别准确性需要测量出二维码的有效范围。经过测试后有效范围如表4 所示。

其中坐标轴以机械臂末端执行器关节为原点，关节轴指向为Z 轴，X 轴与Y 轴分别为初始状态下的水平与竖直方向。由表4 分析可知，AprilTag 能在摄像头15.8 cm 的距离内准确识别，左右范围19.6 cm，上下范围12.4 cm 内能检测到AprilTag。同时ArilTag 与摄像头在70.8° 的夹角内能够被识别到。总体来讲已经能够满足机器人的视觉功能要求。

4.3 机械臂控制性能

机械臂为机器人物流功能的主要执行器，由于其控制模型为多输入多输出，涉及坐标变换较为复杂，系统响应曲线不能直观的表达系统的动态指标，所以本文采用末端执行器距目标位置的距离随时间的变化来反应机械臂的控制性能。如图13 所示。

图13 末端执行器与目标距离的变化

在距离末端执行器5 cm 时机械臂到达目标位置并开始抓取，抓取后距离不再变化。机械臂从目标物品出现到完成抓取用时2.5 s，一开始快速接近目标物品，靠近目标后不断修正位置直到能抓取目标物品。靠近目标后没有过多的振荡满足了快速性的同时也能保证系统的安全。相较于传统开环控制，减少了振荡，提高了系统的稳定性与抗干扰能力，大幅提高了抓取的成功率。

5 结束语

本文设计了基于STM32 的智能物流机器人，采用了PID 算法控制机器人移动与机械臂抓取，优化了物流机器人的控制性能，提高了系统的稳定性，实现了精准的识别与抓取物品。但本文设计的机器人未优化网络应用层协议，没有采用数据库存储数据，希望在下一次研究中优化机器人的网络通讯，以满足互联的要求。

参考文献：

[1] 董智.5G商业化应用背景下“互联网+物流”的发展趋势研究[J].中国商论,2019(22):12-13.

[2] 雷慧杰.基于STM32的直流电机PID调速系统设计[J].现代电子技术,2016,39(8):165-170.

[3] 徐庆坤,窦红乾,杜志强,李凯,谭威.基于AprilTag视觉定位的移动机器人设计[J].工业控制计算机,2022,35(4):42-43.

[4] 王裕民,秦飞舟.五自由度机械臂的运动学建模和轨迹规划研究[J].电工技术,2022(8):62-69.

[5] 彭锋,丁良奇,刘尚鸿,潘筱.车轮外倾角和前束角匹配研究[J].北京汽车,2021(3):21-23,31.

[6] 郑琪,刘琳,尚冬梅,CHEN M.基于LM2596的输出无级可调直流稳压电源的设计与实现[J].电脑知识与技术,2018,14(27):258-260.

[7] 单芝庆.基于STM32的搬运机器人机械系统设计[J].自动化应用,2021(6):1-6.

[8] 邬合林.PID在机械臂位置控制中的应用[J].冶金管理,2020(19):43-44.

[9] 杨晓峰,柳笑辰,王佳奇.基于OPENMV的智能送药小车设计[J].电大理工,2022(1):1-7.

（本文来源于《电子产品世界》杂志2023年4月期）