基于STM32的智能定位分拣机器人设计

赵晋秀，尹志荣，张鑫，王羲，岳洪志

（太原工业学院机械工程系，山西 太原 030000）

0 引言

机器人研究领域，智能化、微型化是机器人的重要发展方向，具有自动搬运功能的分拣机器人一直是机器人研究领域的热点，在军用和民用方面前景广阔。随着时代发展，为解放多余劳动力、提高分拣搬运效率、减少生产成本，智能分拣搬运机器人应运而生[1-3]。

传统分拣搬运机器人通过巡线、超声波测距、视觉监测等方法实现分拣区与货物堆放区之间的移动，此方法用时长、鲁棒性小、对环境标志物依赖大[4]。

本文设计了一种通过编码器和陀螺仪自主定位、TCS3200模块识别颜色、垂直机械臂抓取的分拣搬运机器人。使用基于STM32F407ZGT6芯片的主控对传感器信号进行识别处理并控制电机到达预定坐标和姿态。通过实验解出了颜色识别算法，使得颜色识别受环境影响程度大大降低。

1 硬件系统设计

机器人硬件系统分为全向底盘和带颜色识别模块的机械臂两部分。全向底盘负责物料分拣区和堆放区移动，机械臂负责抓取和搬运。机器人使用基于STM32F407ZGT6芯片的主控进行传感器数值分析、电机驱动和机械臂控制。

图1 系统设计

1.1 全向底盘

全向底盘，顾名思义是可以全方位移动的底盘，一般使用三个以上全向轮或四个麦克纳姆轮，通过改变每个轮子的转速和转向实现全向移动。

相较于传统轮式底盘，全向底盘可以全方位移动、转向灵活、适合狭窄区域作业，故本文使用全向底盘。为保证动力充足，本文使用四直流电机搭配四全向轮结构。使用两个L298N驱动进行控制，L298N内含2个H桥的高电压大电流全桥式驱动器，可以用来驱动直流电机[5]。

1.1.1 全向轮

全向轮轮毂上有若干个小轮毂，小轮可以释放来自轮子法线方向的受力。

在实际中，为得到电机占空比和实际转速的关系，使用编码器电机作为驱动电机。为保证四个轮子电机驱动力相同，驱动轮使用四个编码器电机。由于主控芯片定时器的编码器功能口有限，仅使用一个电机的编码器功能。

1.1.2 编码器

编码器又名码盘，常用作测位移、测速传感器。本方案采用四轮全向结构，使用两无动力的欧姆龙E6A2-CW5C编码器通过联轴器连接两个小全向轮测定位移，两编码器正交安放，分别测定Χ轴和Y轴位移。

1.1.3 陀螺仪

陀螺仪常用于航海和航空领域，为航行人员和电子仪器指明航向。本方案使用MPU6050陀螺仪作为机器人角速度传感器，对角速度积分可以得到航向角。

1.2 机械臂结构

机械臂结构使用舵机进行驱动和角度控制，机械臂使用的舵机较多，工作时电流较大，直接连接到主控会导致芯片供电不稳，影响机器人平稳运行，故采用维特智能16路舵机板对舵机进行控制，并且使用独立电源供电。舵机板使用串口与主控通信，可以节约主控I/O口资源，也使得控制多个舵机变得更便捷。

使用编码器电机驱动丝杠实现机械抓上下移动，丝杠与编码器电机使用梅花联轴器联结，机械臂底部使用餐桌转盘和舵机实现机械臂与底盘的联结和整体转动。

图2 机械臂结构

1.3 颜色识别模块

颜色识别模块可以准确、快速地识别物体颜色。本方案使用TCS3200颜色识别模块，通过程序模数转化输出R、G、B三个数字量可以驱动TTL或CMOS逻辑输入，与主控相连将数据传回STM32芯片。实验中，颜色识别模块传回数据受环境光照影响较大，因此在颜色识别模块两侧使用两恒流LED灯作为补光灯，使得实验数据趋于平稳。

图3 颜色识别及补光灯

1.4 主控

主控是机器人的大脑，通过对传感器信号进行处理并做出判断，实现各部件协调运动以达到最终的动作和运动目标。本方案使用STM32F407ZGT6芯片作为主控芯片，对编码器、陀螺仪、颜色识别模块信号等进行分析和处理，驱动各模块相互结合工作[6-8]。

STM32F407ZGT6芯片提供了足够多的I/O口和定时器，可以支撑各个模块的功能需求，为机器人开发和功能实现提供了更多的可能性，其中定时器特设的编码器模式使得对编码器的控制更加便捷。

1.5 减震系统

本文采用的四全向轮结构对场地平整度要求较高，为防止某一驱动轮或编码器计数轮腾空造成误差，必须采用减震系统。

由于机器人体积较小，电机及电机支架之间距离较近，不宜使用垂直减震，底板使用玻纤板，通过机器人自重对底板造成轻微变形达到减震效果。编码器处结构较少，可以使用垂直弹簧减震。

图4 编码器及减震（弹簧已省略）

图5 底板俯视图

2 全向定位算法

2.1 建立坐标系

开电，陀螺仪初始化完成后设陀螺仪当前角度即机器人当前所指方向角度为0°，此时以初始坐标为直角坐标系原点，设逆时针偏移角度为正，顺时针偏移角度为负。以俯视方向机器人的Y轴为正方向，顺时针旋转90°所指方向为Χ轴正向建立直角坐标系。

2.2 颜色识别算法

颜色识别模块在使用前需要进行白平衡以得到R、G、B颜色因子。由于补光灯使得每次检测时的光照环境基本相同，在程序中直接对颜色因子赋值，此方法可以省去白平衡步骤。

根据实验值确定出一个较为合理颜色因子值，以便写出颜色识别算法。图6为颜色识别算法。

图6 颜色识别算法

2.3 全场定位算法

全场定位算法使用闭环的PID算法，陀螺仪和编码器分别监测底盘的偏航角和位移。

陀螺仪测得的偏航角用于反馈底盘当前所处姿态的角度信息θ。正交摆放的编码器，测得的信号进入主控计数器，经过主控程序处理可以得到底盘在坐标系中的速度及底盘当前坐标：

主控程序给定目标坐标：

芯片执行过程中，约5ms记录一次数据，因此可将整个路程微分成无数个小的路程，微分后的单个小路程可以看作直线，此时Χ、Y轴编码器记录的位移为：

Χlast、Ylast为上个微分小段末的编码器计值，即当前微分小段编码器计值的初值，Χnow、Ynow为当前微分小段编码器计值的末值，两者取差值即为当前微分小段的位移。

图7 位移正交分解

由图可得：

其中dx，dy为分解到坐标系的微分小段位移，即机器人在坐标系的实际位移。此时，机器人在坐标系的位移为：

得到底盘当前姿态（Χnow，Ynow，θnow），将之与目标坐标姿态做差可得偏离位移，即距目标点剩余的位移和角度：

得到的剩余坐标和角度（Χerr，Yerr，θerr），使用PID算法对坐标和角度分别进行相应计算，将此三个值叠加至电机占空比即可使底盘进行全向移动。由于PID算法本身会产生震荡使得误差较大，故算法根据剩余距离设定不同区间，改变PID权重，将震荡带来的影响减少到最小。

3 实验测试

开电后，程序和各传感器初始化完成，坐标系随之建立，此时机器人处于坐标（0，0）点，且计初始角度为0°。

机器人根据设定坐标前往物料分拣区，启动丝杆降下机械臂抓取物料，开启补光灯并使用颜色识别模块识别物料颜色，根据识别到的颜色前往对应物料堆放区放下物料，回到出发区或继续前往分拣区。

实验过程中，通过调整剩余距离分区设定和各分区PID值，最终达到的最高精度为5mm/m。

为得到电机加速度能力以便调节PID的值，绘制电机时间-速度曲线。当给电机60%的速度时，得到时间-速度曲线图如图8。

图8 时间-速度曲线图

定位误差主要由以下原因造成：

（1）PID算法本身ID是由误差积累进行运转，对系统的矫正有延迟。

（2）由于自身重量和电机扭力限制，电机占空比小于某值时底盘无法移动，使得电机对底盘坐标调节有较大的滞后性。

为改善原因（1），本方案采用分级PID控制底盘，即根据剩余位移（Χerr，Yerr）划分不同区间，每个区间有各自的PID系数，每进入新区间，将I和D的累积值清空。此方法显著缓解底盘在特殊点附近震荡的问题，同时提高了定位精准度和效率；使用分级PID算法，定位精度可以达到5mm/m，但远距离移动仍会产生较大误差。

实验中，正常运动情况下，给定P的权重较大时，机器人在即将抵达目标点的最后极短距离时，会由于P的系数过大而导致系统震荡，实际表现为：在目标点处抖动，始终达不到给定的停止区间（此即为设定定点精度），因此使用分级调节，在最后的一段区域通过改变权重，将P调小，I调大，会产生如图9所示的变化。

图9 PID不同权重所产生的影响

对于远距离移动，本方案采用多次定点的方法，在每个定点处设置黑线作为定位校准基准，每次到达基准点，机器人使用光电传感器进行校准。此方法可以有效减小因远距离移动带来的累计误差对定位精度的影响。使用红外对管模块，进行黑线校准，其精度高于仅靠定位算法的精度，多次定点，多次黑线校准的方法，使得机器人在远距离移动时定位精度能够得到保障。

4 结论

使用基于STM32F407ZGT6芯片的全向底盘与带云台的机械臂，结合颜色识别模块能够快速识别、抓取、搬运带颜色的物料，对机器人定位准确度、颜色识别算法、机械臂设计与控制进行了研究。通过对RGB颜色因子的软件赋值，颜色识别模块可以精准识别黑、白、红、蓝、绿颜色。机器人自主定位精度可以达到5mm/m，满足室内搬运和机器人竞赛的要求，由于系统误差和PID算法本身的缺陷，难以进行更精准的定位。对于长远距离，尤其需要拐弯的路径，需要使用多个坐标或黑线标志才能使定位精度略为提高，但仍有待提高。