全向底盘机器人智能定位和姿态检测系统

　　摘 要： 基于正交编码器、陀螺仪等传感装置，为全向底盘机器人设计一套智能定位和姿态检测系统。将陀螺仪安装于全向底盘转动中心处，通过对角速度进行积分运算，得到行驶轨迹偏离原坐标系的角度;将编码器正交布置，作为机器人测速、计路程传感器;使用定位算法，确认机器人所处位置的坐标和姿态信息，由主控芯片作出判断，控制机器人到达目标坐标点和姿态。制作了实物模型，进行了系统应用，通过改变PID权重初步解决了PID算法造成全向底盘震荡的问题。系统误差仅为5 mm/m左右，可满足机器人竞赛和室内物料搬运等应用场景的精度要求。

　　本文源自工业技术创新 2020年5期《工业技术创新》是工业和信息化部主管、中国电子信息产业发展研究院主办的国家级科技学术类期刊。本刊主要面向工业技术创新领域的有关工业主管部门、工业企业、科研创新的学术交流平台、技术创新成果的宣传转化园地和战略政策研究的理论探讨阵地。办刊宗旨是：推动工业技术创新，促进工业转型升级，服务创新国家建设。

　　关键词： 全向底盘机器人;正交编码器;陀螺仪;智能定位;姿态检测;PID权重

　　引言

　　21世纪以来，智能机器人技术发展迅速，轮式机器人以结构简单、载重量大、性能稳定、转向灵活等特点，成为一种应用广泛的机器人[1]。全向底盘机器人是一种典型的轮式机器人，能够通过狭窄空间，实现边走边自转、弧线运动等高难度动作，以高度的机动性在各大机器人赛事、服务型机器人设计、搬运机器人应用中备受青睐[2]。

　　全向底盘在提高机动性的同时，也给坐标定位和姿态检测带来了困难[3]。使用摄像机、激光雷达、微波雷达、超声波、红外线等传感器对机器人进行定位，易受工作环境等外部因素的干擾，具有一定的局限性[4-5]。

　　据此，本文基于正交编码器、陀螺仪等传感装置，为全向底盘机器人设计一套智能定位和姿态检测系统。系统采用STM32F407芯片作为坐标姿态获取和驱动控制芯片，对正交编码器和陀螺仪信号进行分析，结合使用定位算法，得到机器人坐标和姿态;使用SPI串口向其他芯片传送全向底盘数据，同时对当前坐标和姿态与目标坐标和姿态进行对比，计算得到全向底盘上每个直流电机的占空比，实现全向底盘运行，直至到达目标坐标和姿态。

　　1 系统总体设计

　　1.1 全向底盘

　　全向底盘机器人使用3个或3个以上全向轮或4个麦克纳姆轮实现平面内的全向移动。全向轮如图1所示，由轮毂和从动轮等构成。轮毂的外圆周处均匀开设3个或3个以上轮毂齿，每两个轮毂齿之间装设有一从动轮，各从动轮的径向方向与轮毂外圆周的切线方向垂直。全向底盘结构如图2所示，为保障全向底盘有足够的驱动力和机动性，本方案在全向底盘上安装四个独立直流电机，分别通过联轴器与全向轮连接，各个直流电机和全向轮采用中心对称式结构。如此，全向底盘可以轻松实现平面全向移动和原地自转。

　　1.2 陀螺仪

　　陀螺仪最早应用于航海领域，后来在航空航天领域也有较多应用，非常适合导航、定位等系统。本方案采用的MPU6050陀螺仪通过对角速度进行积分运算，得到行驶轨迹偏离原坐标系的角度，多用于机器人、无人机等装备的姿态检测。主控使用yaw(航向角)作为全向底盘姿态数据。出于姿态调整的实际需要，全向底盘会发生自转，故陀螺仪安装在全向底盘转动中心处为佳。在全向底盘上做一标记，给直流电机一定速度，使全向底盘匀速转动，当目视标记处于转动中心处时，即可确定陀螺仪安放位置。

　　1.3 正交编码器

　　编码器又称作码盘，是目前广泛使用的测速测位移传感器。传统三轮全向底盘使用直流电机自带的编码器测速、计路程。此时编码器和直流电机本体作为整体，当动力轮打滑或悬空时，编码器仍在计数，其带来的系统误差较大，会对定位精度产生严重影响。鉴于此，本方案使用2个独立的欧姆龙E6A2-CW5C编码器作为机器人测速、计路程传感器。每个编码器内部有两对光电耦合器，输出相位差为90°的两路脉冲序列。正转和反转时，两路脉冲的超前、滞后关系刚好相反。由图3可知，在B相脉冲的上升沿，正转和反转对应的A相脉冲的电平高低刚好相反，因此使用AB相编码器，可以很容易地识别出转轴旋转的方向，并据此对坐标调整进行判断。

　　所使用欧姆龙E6A2-CW5C编码器为增量式编码器，规格是500P/R，即转一圈可产生500个脉冲，将脉冲传向主控即可得知编码器所转圈数。在编码器上连接小全向轮，正交后可记录全向底盘在X和Y方向的行程。为使正交编码器轮子与地面紧密贴合，减少因全向轮打滑和空转对精度产生的影响，正交编码器与全向底盘使用弹簧减震器连接，结构如图4所示(其中弹簧已省略，小全向轮从易)。

　　1.4 主控

　　主控是机器人的核心部件，负责接收各个传感器的信号、对信号进行相应的处理、对处理结果作出反应等。本方案采用STM32F407芯片作为全向底盘主控芯片，负责接收产生于编码器的脉冲信号和陀螺仪角速度信息，将角速度进行积分，即可得到当前姿态角度。STM32F407芯片将定时器设置为编码器模式，通过访问对应寄存器，可实时监测编码器所转过的圈数和接收到的脉冲数，从而得到编码器走过路程为

　　其中，为全向轮半径。根据路程和姿态角度，按照定位算法，计算出机器人当前坐标，并按照目标坐标，给直流电机相应信号，驱使机器人运动。通过SPI串口，其他芯片可以给定目标坐标和姿态，也可将全向底盘数据传给其他芯片。

　　2 机器人运动坐标系建立

　　为适应小型机器人结构需要和安装便捷需求，将正交编码器安装于直流电机间隙处，如图2所示全向底盘结构。坐标系建立如图5所示，正交编码器法线交点即为坐标零点。启动机器人时，陀螺仪初始化，约定逆时针转动为正，顺时针转动为负。机器人正前所指方向即为0°方向，设此0°方向为轴正方向，顺时针旋转90°为轴正方向。陀螺仪偏转方向即为机器人相对于坐标系的偏转方向。

　　3 机器人定位算法

　　STM32F407芯片仅供全向底盘使用，执行功能较少。芯片约5 ms记录一次数据，因此可将机器人路线离散成无数个5 ms路程。由于时间间隔较小，陀螺仪角度变化微小，所以每一小段路程均可以看作直线运动。

　　取一小段路程，基于直线运动假设，陀螺仪读数为一定值。设当前陀螺仪读数为，即机器人与坐标系角度为。

　　得到剩余路程和角度。将、方向剩余路程差值和航向角差值分别用PID处理并叠加后，传送给直流电机，直流电机据此运转，即可到达指定坐标和姿态。

　　4 应用与讨论

　　制作了实物模型，进行了系统应用。上电后，系统程序和传感器初始化完成，建立以编码器法线交点为坐标原点，机器人指向方向为轴正方向，顺时针旋转90°方向为轴正方向的直角坐标系。由全向底盘主控程序或其他芯片通过串口给出目标坐标和姿态，芯片将根据定位算法驱动直流电机，使全向底盘逐渐逼近目标坐标和姿态。机器人工作流程如图7所示。

　　使用传统PID算法进行坐标矫正总会出现超调和在目标点附近震荡的情况，这是PID算法的固有缺陷。解决方法如下：

　　(1)使用分级PID。根据距离目标点远近改变PID值。使用多套PID，根据距离选择合适的PID值进行调节。

　　(2)改变PID权重。根据距离目标点远近改变PID影响直流电机驱动的权重。将距离变量与PID值关联，越靠近目标点，P权重越小，I、D权重越大，并且一段时间清零一次累计误差。

　　经实验，分级PID方法简单，但是调试流程繁琐。最终决定采用改变PID权重方法。

　　可通过程序调节定点精度，随着定点精度的提高，机器人在目标点的调整时间也在改变。如图8所示，精度越高，在定点附近微调时间越长。

　　当误差低于5 mm/m时，程序将难以跳出循环，全向底盘会在定点附近震荡，原因如下：

　　(1)由于全向底盘自重和载重限制，直流电机速度低于一定值时，全向底盘不会发生移动。

　　(2)由于I、D均依靠累计误差进行速度调节，因此当直流电机速度达到了可以使全向底盘移动的速度时，累计误差已经较大，必将出现超调现象。

　　笔者对原因(2)进行了优化，每经过一段时间便将累计误差清零，略微提高了精度，但未予彻底解决。

　　5 结束语

　　本文设计了一种基于正交编码器和陀螺仪的全向底盘机器人智能定位和姿态检测系统，可以直接通过主控程序给出目标坐标，也可由其他芯片通过SPI串口给出目标坐标，机器人将智能移动到达指定坐标位置。这种模块化控制方法更加灵活，使得机器人开发更加简便。控制程序使用了PID算法，且PID的值随距离目标坐标远近而调节，通过实时计算坐标、实时检测机器人姿態，并与目标坐标和姿态进行对比，最大程度地保障了运行平稳度和精度。经实验，这种全向底盘机器人误差约为5 mm/m，满足机器人竞赛和室内物料搬运等应用场景的精度要求。

　　参考文献

　　[1] 徐明钊， 于海涛， 杨春， 等. 基于陀螺仪和码盘的自主定位机器人系统[J]. 兵工自动化， 2016， 35(10)： 60-63.

　　[2] 赵伟雄. 全方位移动机器人控制系统研究与实现[D]. 邯郸：河北工程大学， 2016.

　　[3] 许伦辉， 朱群强， 胡发焕. 基于Euler算法的全向轮机器人位姿检测[J]. 计算机测量与控制， 2015， 23(7)： 2295-2297， 2301.

　　[4] 熊家新， 刘丽， 徐鹤， 等.全向轮式移动机器人的定位导航算法[J]. 长春理工大学学报(自然科学版)，2014(4)： 83-88.

　　[5] 李洁， 李果， 黄鸿， 等.全向轮机器人基于双全向轮： 陀螺仪模式坐标定位[J].硅谷， 2010(8)： 34.