近年来，随工业制造智能化的发展，移动机器人在工业制造领域中的应用日益增加，而移动定位精度是移动机器人的关键指标。

  采用超声波传感器做定位的精度可达±5mm以内；通过激光雷达进行路径规划结合三边定位算法的定位精度也能在5mm之内；而采取视觉定位，由于外因如光线等的影响，其精度则只能达到厘米级别。

  在工业制造环节中应用的机器人，如完成不同工艺加工机床之间的物料卸料、转运和装夹或者零部件转运加装配等连续动作任务时，对移动机器人的定位精度更高，上述定位方法的定位精度已不能够满足要求。

  为了逐步提升移动机器人的定位精度，本文针对四轮全向移动机器人的定位问题，基于增量式PID算法，采用STM32、激光测距模块和陀螺仪构建移动机器人定位系统，以实现对移动机器人定位精度的提升。

  为了能够让机器人实现全向运动，采用4个麦克纳姆轮，并将其O型布置，如图1所示。

  对整个移动机器人进行速度分解，V表示为全向移动机器人的速度，ω为角速度，R为轮子距离车身中心的距离。

  通过分解V得到x轴速度Vx，y轴速度Vy；分解R得到轮子x轴距离Rx，y轴距离Ry。则机器人的运动学解算可表示为

  机器人由当前起始位置移动到目标位置时，有必要进行初步定位。机器人初步定位时的关系如图2所示。

  机器人通过激光测距模块采集到的距离数据及陀螺仪数据，确定当前坐标。设当前坐标为(x1，y1)，目标坐标表示为(x2，y2)，机器人轮1转速为VC，则有以下表述：

  式中：α为目标点与起始点之间连线与y轴方向的夹角；S为两点坐标直线距离。

  机器人在当前坐标与目标坐标之间采用平移的运动方式，则机器人整体转动的角速度ω为0，将式(2)代入式(1)进行运动学解算后，其4个轮子的速度可表述为

  增量式PID算法原理：PID控制算法是一种经典的反馈操控方法，通过比较实际输出与期望输出之间的差异来修正控制信号。

  PID算法有多种类型，由于控制管理系统的特点不同，对PID响应速度和稳定能力的要求也不同，本文采用增量式PID，控制算法表述为

  式中：Kp为比例增益；TI为积分时间常数；D为微分时间常数；u(t)为PID控制器的输出信号；e(t)为给定值与测量值的关系。

  其中：k为采样序列(k=0，1，2，3，…)；Ki为积分环节；Kd为微分环节。

  增量式PID定位校准流程：由于外部外因的影响，机器人初步定位不能够达到预期的定位精度，所以要局部调整位置坐标。

  首先机器人完成对目标位置的初步定位后，通过将陀螺仪和激光测距模块采集到当前位置坐标信息，传递给增量式PID控制器，以此输出PWM脉冲信号实现对步进电机的闭环控制，直到机器人达到定位精度要求。

  机器人定位关键程序：移动机器人的运动学解算核心程序中给定小车的运动速度v、运动方向角度angle、旋转角速度ω，通过调用CarSpeedControl函数，即可以求解出麦克纳姆轮小车各个轮胎的线速度。

  若给定不同的运动速度v、运动方向角度angle和旋转角速度ω，则能轻松实现移动机器人的全向移动。

  函数Yaw_PID和函数Pos_PID分别实现机器人的角度闭环控制和位置闭环控制。

  在角度闭环控制中，系统的反馈输入r(t)为小车的目标角度与当前角度的差值，系统的控制输出y(t)为小车的自转角速度；

  在小车的位置闭环控制中，使用激光传感器获取小车实时的位置坐标信息，然后将位置坐标进一步计算，转化为当前坐标点与目标点的直线距离。

  系统的反馈输入r(t)为小车当前坐标到目标坐标的直线距离，系统的控制输出y(t)为小车的移动速度。

  移动机器人角度和位置双环控制程序中，将YawPos_Pid_control函数置于中断系统中，以提高总系统的响应速度；

  通过给定目标坐标和目标角度作为该函数的入口参数，机器人就可以精准的定位并调整至目标位置。

  系统硬件设计：全向移动机器人的定位系统硬件主要是采用STM32、激光测距模块和陀螺仪构建，硬件框图如图4所示。

  系统硬件结构采用STM32F103ZET6单片机作为主控芯片，通过UART1向正交布置的激光模块发送测量当前位置的指令，激光模块获取当前位置后将数据返回主控芯片。

  主控芯片对激光测距模块和陀螺仪采集的数据来进行解算处理，将解算结果发送到电机驱动模块，控制步进电机调整机器人的位姿。

  全向移动机器人的运作时的状态采用OLED屏幕显示，通过WiFi实现与上位机的通信。

  系统软件设计：机器人系统软件主要负责运动解算、控制以及数据处理，为了更好的提高机器人的稳定性和执行效率，程序采用顺序结构，以降低任务之间的相互影响。其控制流程图如图5所示。

  机器人根据给定的目标位置与激光测距模块获取的当前位置坐标(x1，y1)进行解算，导出机器人的位移S、运动速度V以及机器人到达目标位置所需要的时间T。

  系统软件将得到的速度V转换成PWM脉冲信号控制步进电机的转动速度，同时将位移S和时间T在中断程序做处理，当机器人运动时间T后，程序失能PWM脉冲信号输出，即机器人到达目标位置区域。

  激光测距模块和陀螺仪模块再次获取当前位姿信息，经系统判断，机器人是不是达到定位精度要求。

  若机器人达到定位精度要求，即返回系统判定结果至屏幕上显示，并且将信息反馈于远程上位机；

  若机器人未达到定位精度要求，则将激光测距模块和陀螺仪模块再次获取当前位姿信息反馈于增量式PID控制器，并输出PWM脉冲信号，控制机器人进行角度校准，再进行位置调整，直至机器人达到预期的定位精度要求。

  全向移动机器人测试移动范围由围在地图四周挡板确定，另外挡板也作为激光测距的参考，以标记的十字架图标作为定位基准对象，如图6所示。

  实验分别测量了45组不同位置对同一点的定位精度，将测得45组不同位置对同一点的定位精度表示为X轴偏差与Y轴偏差，整理后结果如表1所示，可得其定位精度达到2mm以内，相比于同类型的AGV移动小车定位精度更高。

  为提高全向移动机器人在室内定位精度的问题，本文基于STM32、激光测距模块和陀螺仪搭建了定位系统硬件结构，采用增量式PID算法校准机器人位姿。

  通过测试表明，该系统能满足全向机器人在室内环境下的定位精度要求，能够很好的满足大部分高精度定位应用要求。