李 锶,张迅豪,祝新洋,唐 超

(湖南理工学院 机械工程学院,湖南 岳阳 414006)

0 引言

麦克纳姆轮机器人因具有高运动灵活性、高自由机动性、结构紧凑等优点[1],目前已广泛应用于RoboMaster、Robocon 等机器人赛事中,并逐步应用于物流、工业自动化等一系列场景[2].本文以第六届全国大学生工程训练综合能力竞赛(物流机器人组)为背景,提出了一种利用陀螺仪与编码器数据计算全场坐标,再根据激光测距仪与数字式灰度传感器数据对推算得到的全场坐标重新标定、纠偏的定位方法.相比于单纯使用惯性元件来推算航迹,该定位方法能够保证机器人在长时间运行下拥有较高精度且能降低成本[3];相比于单纯使用激光测距来进行全场定位,拥有优秀的动态定位性能和抗车身晃动能力.因此,该定位方法能够有效提高定位精度,提高抗外部环境干扰强度,满足物流机器人的定位需求,并提高经济性.

1 物流机器人运动学分析

为简化分析,先对模型进行简化.假设机器人底盘为刚体,质心在底盘几何中心且不随运动发生变化,赛道地面水平无坑洼,忽略麦克纳姆轮及其小辊子形变,假定轮子与赛道摩擦力足够大,不考虑轮子打滑现象.机器人麦克纳姆轮底盘安装方式主要有X-正方形、X-长方形、O-正方形、O-长方形,因O-正方形具有优秀的运动性能,故采用O-正方形安装方案,结构设计如图1所示.建立XOY本体坐标系(俯视),设四轮速度分别为ω1、ω2、ω3、ω4,轮子半径为R,O到轮子轴心点距离为L(在X、Y轴方向上的投影分别为Lx,Ly),机器人移动速度为Vx、Vy(合速度V)、ωyaw(麦克纳姆轮的轮毂轴与辊子转轴夹角为45°).

图1 机器人底盘坐标系

图2 轮子1 坐标系

在XOY本体坐标系中,轮1 轴心点O′速度为

沿X、Y轴分解可得各分量为

轮子轴心速度可分解为沿辊子方向速度V||和垂直辊子方向速度V⊥,为辊子方向单位矢量.其中V||满足方程:

联立式(2)～(3)得1 轮转速方程:

同理可得另外三轮转速方程,在此不再赘述.逆运动学模型矩阵式[4]为

反解逆运动学模型(5)可得正运动学模型(6):

2 物流机器人定位系统设计

目前常见的定位方案有基于无线信号强度推算的超带宽(UWB)定位、RFID 信标定位、Wi-Fi 定位、蓝牙定位等,但这些都需要外部设备并在前期安装调校,由于比赛前期调试时间短、调试任务繁重,因此这种方案不符合比赛需求[5];有基于惯性元器件的惯性导航技术,这种纯客户端的技术利用IMU(惯性测量单元)数据基于航位推算法计算得出机器人位置信息,但误差会随机器人运行时间与运动距离不断累积,且结构复杂、制造成本高昂[6];还有GPS 定位,常用于室外定位,室内由于建筑物遮蔽,信号差、定位精度较低且成本较高,一般不予考虑[7].

陀螺仪与编码器动态性能较好,但它们都有累计误差,需要外部参数纠正.经过综合考虑,物流机器人综合采用Z轴陀螺仪、增量式编码器、激光测距仪以及数字式灰度传感器对物流机器人自身位姿与全场位置进行检测与纠偏.

2.1 物流机器人自身位姿控制

建立赛道全场坐标系XOY与物流机器人自身坐标系X′O′Y′,如图3所示.L1为激光测距仪1(以下简称Laser-R1)测得距离,L2为 Laser-R2 测得距离.L为Laser-R1、2 之间的距离(1、2 均安装在垂直于车体的同一平面).XOY坐标系原点为赛道发车区顶点.设自身坐标系X′轴正方向与全场坐标系X轴夹角为机器人航向角(yaw).

图3 全场坐标系与物流机器人坐标系

针对比赛的具体情况,物流机器人在高速运动与搬运物料时需要保持车身稳定、不需要旋转,故控制yaw角保持0°.由激光测距得到的yaw 角:

Z轴陀螺仪采用HWT101 模块,经卡尔曼滤波后串口输出航向角yaw_gyro.由于机器人需要频繁地启停、加减速,在加速时车身晃动幅度略大,距离输出值不稳,导致对yaw_laser 干扰略大,波形不平滑,故对yaw_laser 低通滤波,而陀螺仪动态性能良好却有累计误差,二者结合采用互补滤波的方式,以陀螺仪航向角数据为基准,利用激光测距仪数据所计算出的航向角对其进行实时校准.滤波原理如图4所示.

图4 滤波原理框图

激光测得角度低通滤波:

激光测得角度与陀螺仪测得角度一阶互补滤波:

联立(9)与(10)建立融合公式:

其中 yaw_laser(n)为本次输出值,α为低通滤波系数,yaw_laser_new 为本次采样值,yaw_laser(n-1)为前次输出值;k为一阶互补滤波系数.

2.2 物流机器人全场定位算法

由于赛道场地上的单双黑线具有一定规律,可使用数字量灰度传感器识别这些黑线作为精确地标用于定位纠偏,故采用编码器、激光测距仪以及数字量灰度传感器综合定位,可降低定位难度并减少购买高精度惯性测量单元、激光测距仪、编码器等模块的成本.

将逆运动学模型(5)中速度ωR替换成距离d,得到位置跟踪模型:

将编码器数据累加代入模型即可计算得到机器人在全场坐标系下的坐标(dx,dy).

因主动轮在加减速时易打滑,误差累积后编码器数据计算得到的坐标逐渐偏离真实值,故当机器人每越过一次单线就标定一次全场X轴坐标,每完全越过一次双线标定一次全场Y轴坐标.激光测距仪测得的距离也可换算成全场坐标,但车身晃动时坐标数据会偏离真实值,故当|yaw| ＞1.5时不予采用,|yaw| ＜1.5时重新标定一次全场坐标.激光测距本身具有误差,置信度低于精确地标,所以激光测距标定优先级低于地标标定.

3 任务信息与物料图像识别系统

为节约开发时间,降低开发难度,二维码识别采用高完成度的影像式扫码模块GM65,只需编写接收中断服务函数,通过串口接收二维码数据并对其解析.Wi-Fi 通信采用ESP8266 接收无线广播中包含有任务抓取顺序的数据,并通过串口返回数据给主控.最后,主控将这些任务信息实时显示在OLED 屏上.

物料颜色识别采用OpenMV4 嵌入式图像处理模组,该模组使用STM32H7 高性能处理器,通过MicroPython 语言编程实现机器视觉,在一般的色块追踪算法基础上按照边缘检测算法的思想改进扫描算法[8].首先对全幅图像扫描找到对应目标色块颜色(红、绿、蓝三色),得到该色块的中心坐标以及长和宽,依据这三者设置包含色块在内的ROI(感兴趣区);然后在ROI 内再次追踪目标色块,若未找到目标色块则重复以上步骤.该算法通过裁剪图像去除ROI 外的噪声,并减少了图像处理的工作量,提高了追踪的准确率与速率,经过测试可以显著提高色块追踪速度,识别帧率从25～28 帧提高到58 帧左右,满足识别性能需求,可以实现机器人在高速运动过程中识别物料颜色并将数据经处理后通过串口发送给主控.

4 控制系统硬件结构设计

本物流机器人采用双MCU 设计,采用意法半导体公司的STM32F103VET6(主控制器)和STM32F103RCT6 单片机(辅助控制器),主控制器负责机械手臂控制、里程数据采集、惯性元件数据采集、机器人运动控制、任务执行、OPENMV摄像头数据采集以及多信息融合,辅助控制器负责收集激光测距仪数据、Wi-Fi 数据接收以及将这些数据打包发送给主控制器.控制系统硬件结构如图5所示.

图5 控制系统硬件结构

5 物流机器人软件设计

主控作为整个系统的“大脑”需要处理各传感器得到的数据,根据数据做出正确决定并控制物流机器人做出合理的反应.主控主程序内容如下:

完成主控及各传感器初始化,接收Wi-Fi 数据得到任务顺序.机器人途经物料存放区时,在运动中识别三个物料颜色顺序(从左至右),分三次将待加工物料按顺序搬运至加工区.完成后机器人移动至指定位置识别含有成品搬运顺序数据二维码,按顺序依次将全部成品搬运至成品区后,回到机器人发车区并停机.主程序流程如图6所示.

本机器人程序采用模块化编程,且按照单片机操作系统的思想,每隔5 ms 处理一次全场位置数据,确保小车位置的实时性、准确性.5 ms 周期事件流程如图7所示.其中PID 控制小车跟随采用双闭环控制方案.双闭环控制方案如图8所示.

图6 主程序流程

图7 5 ms 周期事件流程

图8 双闭环控制方案

6 现场测试

为测试所设计定位系统的可行性和准确性,在实际场地中进行现场测试.对比测试了纯编码器解算和全场定位系统的定位性能,30 次统计数据记录如图9所示,定位评估数据记录见表1.

根据实验结果可知,本文设计的全场定位系统定位稳定性较高、精度较高,性能优于单纯依靠编码器来定位,满足竞赛定位需求.

图9 统计数据

表1 定位数据评估

7 结论

本文从全国大学生工程训练赛(电控组)出发,提出基于麦克纳姆轮全方位移动机器人的多传感器综合定位方案,解决了纯惯导系统有效定位持续性差和纯测距定位系统动态性能差的问题.经实际测试,该系统简单有效,具有良好的运动控制效果,同时能够满足精确定位、准确识别等需求.本文提出的物流机器人控制系统和定位方案在物流机器人领域具有一定的应用价值.

物流机器人是一个非常复杂的非线性系统,涵盖了机械、动力学、嵌入式系统、控制理论基础等众多领域的技术.对于整个系统而言,本文做的只是初步探索、尝试,仍然还有许多不足之处,如:

(1)激光测距仪于小车有装配误差,激光发射面并非完全垂直于小车侧面会使数据偏离真实值,导致出现定位误差.

(2)电机与小车有装配误差,电机轴线与车体X轴不平行,且麦轮未安装悬挂,导致按编码器数据计算出的全场坐标有误差.

(3)麦克纳姆轮在光滑表面打滑,导致全场坐标计算产生误差.

(4)陀螺仪累计误差.

后期研究工作还可以从以下方面去深入研究:

(1)对麦克纳姆轮进行滑动的监测与预防.

(2)通过设计相关算法减少陀螺仪的累计误差.