四川打磨机器人系统二次开发（水泥地面打磨机器人运动控制系统设计）

在我国建筑行业中，传统水泥地面打磨主要是由工人手工工作或者配合半自动化机械产品共同协作完成，但是往往造成地面打磨光整度并不是太好，打磨过程中大量灰尘严重危害了工人健康，对于大面积地面时，工作效率非常低^[1]。水泥地面打磨机器人可以有效解决上述难题，可极大提高工作效率，减轻工作人员劳动作业量，消除灰尘对工作人员的伤害^[2]。而目前比较成熟的产品往往是由人工扶持的半自动产品，通过对现存的机器进行分析发现有的机型太过于笨重，搬运和更换磨片很费力，有的机型太轻，磨削力不足，效率低，大部分产品依然没有完成无人自动打磨任务^[3]。针对以上问题，需研发出能够在少人或者无人看守的情况下自动工作的打磨机器人，它可以依靠自身的激光雷达构建室内地图，通过运动路径规划、定位导航、运动控制配合打磨转盘控制，实现建筑水泥地面的自动打磨。

本设计的核心是配合路径规划实现复杂运动中基本运动的控制，其原因在于每一种复杂运动都可拆解为前进、后退、左转、右转等基本运动的组合，所以对基本运动的准确控制尤为重要。

在本设计中首先对所研究的机器人构建运动学模型，在分析了机器人四种基本运行方式基础上，针对机器人速度控制问题提出速度控制算法，并通过MATLAB软件完成算法的仿真。在水泥地面打磨机器人基本运动实现过程中是以STM32F103Z为运动控制系统的主控芯片，通过伺服驱动器驱动电机转动，完成机器人运动控制系统硬件平台的搭建，软件部分以C语言为编程语言，在Keil μVision5集成开发环境中编程，通过仿真器下载到STM32F103Z芯片，最后通过机器人样机进行实验。

1 机器人运动学模型建立

为了准确描述机器人在运动过程中位姿信息，对水泥地面打磨机器人的移动车体结构部分进行简化，构建该机器人的运动学模型。

如图1所示，水泥地面打磨机器人可以分为两大部分，第一部分为功能部分，主要是打磨盘以及与之相适应的驱动电机，打磨转盘与电机之间通过V带进行传动，在驱动电机尾部安装有编码器，通过编码器获得驱动电机的转速信息；另一部分是移动机器人载体，移动机器人前轮为驱动轮，通过控制驱动轮完成对机器人运动方式的控制，两个后轮是万向轮，只起到支撑作用。

将上述水泥地面打磨机器人结构进行简化，得到如图2所示的机器人运动学模型。

在图2中，X_r、Y_r分别是机器人局部参考坐标系下的横坐标与纵坐标，X、Y分别是全局坐标系下的横坐标与纵坐标，Xr总是与机器人的运动方向保持一致。O是机器人车体的中心，设差速驱动机器人的广义坐标为q=[x，y，θ]，其中x、y表示差速驱动机器人平台两个驱动轮中心的中点O的横坐标与纵坐标。即两个驱动轮连线的中点坐标。

关于所有涉及参数的含义为：ν_L、ν_R、ν分别表示驱动轮左轮速度、驱动轮右轮速度和水泥地面打磨机器人当前速度；2L表示两个驱动轮的间距，近似看做机器人车宽；r表示单个驱动轮的半径；θ表示机器人运动方向与全局坐标系X轴之间的夹角。

式(1)中两个驱动轮速度υ_L、υ_R共同决定了水泥地面打磨机器人四种运行方式：

(1)当υ_L=υ_R时，υ_L、υ_R同时正转，地面打磨机器人向前直线运动；

(2)当υ_L=υ_R时，υ_L、υ_R同时反转，地面打磨机器人向后直线运动；

(3)当υ_L>υ_R地面打磨机器人向右转弯；

(4)当υ_L<υ_R地面打磨机器人向左转弯。

针对水泥地面打磨机器人的运动，在坐标系X_r-Y_r建立运动学方程：

式(2)表示机器人运动过程中的当前位姿信息。通过对式(2)两边进行积分化简，然后经过离散化处理得到机器人空间位姿变化与机器人驱动轮移动距离之间的关系表达式：

式(3)表示机器人在t_{n 1}时刻的瞬时位姿与两驱动轮移动距离Δs_L、Δs_R之间的关系，x_{n 1}、y_{n 1}、θ_{n 1}表示在t_{n 1}时刻机器人的位姿信息，x_n、y_n、θ_n表示在t_n时刻的机器人位姿，左驱动轮的移动距离为Δs_L、右驱动轮的距离为Δs_R。

由以上公式推导过程可以得到机器人在某时刻准确的位姿信息，在实际控制过程中，机器人位姿信息可由机器人驱动电机自带编码器的数据输出，计算出两个驱动轮的位移，最终得到该机器人的实际位姿。此外，利用差速驱动原理[5]改变两驱动轮的速度，可以实现机器人运动方式的控制，为机器人运动控制系统的实现提供理论依据。

2 加减速控制算法

水泥地面打磨机器人在运动中会产生噪声以及振动，如果没有合适的运动控制算法将会导致机器人稳定性降低，产生的振动也会加速机器人机械部件的磨损，并且会使得机器人定位产生较大误差，针对此问题，本设计提出基于“S”型曲线的速度控制算法，该算法能够有效解决该机器人在运动控制中稳定性低、定位误差大等问题^[6]。

“S”型曲线具有加速度呈现线性、加速度连续性、速度平滑过渡的特点。由于速度曲线的形状，这个轨迹称为双“S”速度曲线。曲线是由加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段和减减速阶段这七个阶段组成的，因此又叫七段式轨迹^[7]，如图3所示。

由以上曲线图可得“S”型速度控制算法数学表达式。

速度计算数学表达式：

在该机器人速度控制算法中，设定地面打磨机器人最大的运行速度为0.8 m/s，最大加速度为0.5 m/s，接着对“S”型加减速控制算法在MATLAB进行仿真，仿真结果如图4、图5所示。

通过图4可以看出，机器人在前2.5 s处于加速状态，在2.5 s时完成加速过程，速度到达最大值80 cm/s，在加速过程中，速度曲线光滑连续，没有尖点；在减速时，情况类似，符合水泥地面打磨机器人速度控制要求。

通过图5可以看出，机器人在前1 s加速度匀速增加，在1 s时，加速度到达最大值50 cm/s，随后加速度保持不变，在1.6 s时加速度开始减小，2.5 s加速度减为零，机器人完成加速阶段。在加速度减小时，情况类似；整个加速过程曲线连续，满足加速度要求。

通过图4、图5可知：S型曲线在整个速度调节过程中平滑过渡，不存在加速度突变现象，表明水泥地面打磨机器人运动平稳，无较大冲击，可以保证机器人在运动过程中平稳运行。

3 运动控制系统硬件设计

从水泥地面打磨机器人工作角度考虑，运动控制系统整体设计主要包括：车体、驱动轮、伺服电机及伺服驱动器、运动控制器、供电电源、串口通信电路和按键模块。其中，STM32F103Z芯片是运动控制器的核心，主要是通过向两个驱动器发送一定的频率脉冲PWM信号，协调控制机器人两驱动轮的转速和方向，最终实现对打磨机器人本体的控制以及打磨盘转速的调节^[8]。

整个系统的能量来源是48 V锂电池通过降压模块输出适配各个模块的电压要求，运动控制系统硬件框图如图6所示。

本系统采用STM32F103Z作为控制系统中的控制芯片^[9]，STM32的I/O模块连接按键通过高低电平的转换，实现信号的输入，完成相应的功能。通过STM32的高级定时器TIM8模块输出PWM波值到伺服驱动器，伺服驱动器将收到的信号进行分析驱动伺服电机以及打磨电机转动，电机转动过程中的位置以及速度信息通过编码器经过伺服驱动器间接传递到微控制器中，用于机器人的位姿计算。其电路如图7所示。

编码器采集数据的反馈电路采用EL357NC光耦芯片，将伺服驱动器产生的三对互补信号A A-B B-Z Z-通过高速光耦转化为单端信号A /B /Z 输出到控制器STM32，本设计的机器人运动控制系统电路共有左右两个驱动轮的数据输出，该模块仅以右侧编码器数据采集进行介绍，电路原理图如图8所示。

电源模块主要由锂电池以及专用电源转换芯片组成，供电电压为48 V，其中打磨电机、机器人驱动电机采用锂电池直接供电，其他电路通过电压转换模块将48 V转换为24 V用于吸尘电机供电，24 V转5 V用于给编码器等外围电路供电，5 V转3.3 V用于给STM32系统供电。

4 运动控制系统软件设计

该控制系统采用C语言进行软件编程，控制系统软件功能主要是完成控制指令的接收与发送、机器人速度控制算法的解算、对驱动轮速度进行计算和驱动以及参数的存储。

软件设计主要包括以下几个模块：程序的初始化、串口通信模块、定时器中断模块、伺服电机驱动模块、打磨盘驱动模块以及电源电量显示模块等，运动控制系统软件的主要工作流程如图9所示。

在机器人通电后，首先对控制系统进行初始化，包括定时器、PWM函数、串口函数，接着开启外部中断，当系统接收按键发出指令时，STM32F103芯片输出PWM波值，程序运行。机器人开始运动，同时编码器对电机的位置进行检测，通过定时器计算出当前机器人位置，当机器人系统即将到达预设目标值时，开始调用“S”曲线速度控制算法进行速度控制直至到达目标值保证机器人的运动精度。

5 运动误差测试与分析

水泥地面打磨机器人实物图如图10所示，在测试之前，先打开强电电源开关用于给两个驱动电机以及打磨电机供电，然后打开弱电控制开关，用于给控制系统供电。然后通过按键控制机器人的直线行走、半径转弯。

5.1 运动误差测试

本文对机器人的运动误差测试主要分为两部分：直线运行时产生的误差、半径转弯时产生的误差。

(1)直线运行时产生的误差

在机器人直线运行误差测试时，选择的路程分别为200 cm、400 cm、600 cm，在相同的路程以不同的速度进行测试，并将设定的目标路程与实际机器人运行的路程之间的差值记为系统产生的定位误差。

整个误差测试过程：第一步，通过按键发送指令到控制器，在程序中设置机器人的目标路程；第二步，按下按键，机器人按照设定的路程行走；第三步，通过卷尺对机器人实际运行的距离进行测量，当运行距离较小时，采用游标卡尺进行辅助测量；第四步，记录测试数据绘制表格；直线运行误差数据结果如表1所示。

通过表1测试结果可知，不同的路程或者速度设置不同时，均存在一定的误差，随着路程的增加，机器人所产生的误差逐渐增加；速度增加，机器人的误差也呈现增加趋势。平均误差范围在1.0 cm至1.9 cm之间，最大误差2.9 cm时，实验记录距离为600 cm。通过最大误差和标准差可知，该控制系统在一定程度上保证了直线行驶时的精度。

(2)转弯运行时产生的误差

在进行机器人转弯误差测试时，选择半径为400 cm的圆弧线路，通过设置不同速度，测量机器人转弯运行误差，为了保证测试更加精确采用卷尺与游标卡尺相结合进行测试。测试过程与直线测试方法相同，误差结果采用平均误差作为参考值，当机器人在半径为400 cm运行时，误差结果如表2所示。

通过表2测试结果可知：当机器人运转速度逐渐增加时，机器人的转弯定位误差越大；当机器人转弯角度增加时，机器人转弯定位误差也是呈现逐渐增加的趋势，平均误差范围在1.1 cm至2.0 cm之间，最大误差3.1 cm时，速度为70 cm/s，旋转角度为360°，通过最大误差与标准差可知，该控制系统在一定程度保证了转弯运行的精度。

5.2 误差分析

通过测试发现水泥地面打磨机器人能够完成设定的功能，如直行、半径转弯、速度控制，但其中产生误差的原因可能具有以下原因：

(1)地面打磨机器人在安装过程中，存在安装误差。该误差使得机器人在执行何种类型的动作时, 都会产生偏差。

(2)控制运动过程中存在轻微抖动现象，使得机器人在运行中出现偏差，在消除误差时候，需要对控制系统算法进一步优化。

(3)地面打磨机器人测试过程中都在水泥的地面上进行，地面的粗糙程度，在一定程度上导致机器人在运动动作产生误差。

在下一步的研究中，将对机械结构做优化，并进一步对算法进行优化，以保证在不同平整度的地面上工作的情况下运动的稳定性。

6 结论

本文开发设计了水泥地面打磨机器人的基本运动控制系统,并在实际样机中对相关功能的进行验证。本文首先针对水泥地面打磨机器人建立运动学模型,在该模型的基础上，提出了一种差速驱动转向结合“S”曲线算法控制水泥地面打磨机器人的基本运动控制方案，并对该算法进行仿真验证，接着对机器人基本运动控制系统进行软硬件设计，最后通过样机进行试验。该实验表明本设计的控制系统在一定程度上保证了机器人的直线运行和转弯运行精度，实现了水泥地面打磨机器人的基本运动控制，为水泥地面打磨机器人的进一步研发提供基础。

参考文献

[1] 付巍巍.抛光机器人的力控制研究及应用[D].武汉：武汉理工大学，2017.

[2] 葛君华.建筑施工用打磨机器人的工作方法：CN108756-177A[P].2018-11-06.

[3] 冯强，叶长汀.全自动地坪抛光研磨机[J].石材，2016(9)：45-46，54.

[4] 任工昌，吴梦珂，朱爱斌，等.带单节拖车的机器人控制系统的设计与实现[J].电子技术应用，2018，44(11)：41-44，49.

[5] 古顺东，陈紫娟，陈珂，等.一种差速驱动的探险救援机器人研制[J].广东石油化工学院学报，2018，28(3)：53-57.

[6] 王玉，王旗华，赵建光，等.焊接机器人虚拟样机轨迹模拟和运动仿真分析[J].焊接学报，2012，33(4)：109-112，118.

[7] 潘海鸿，袁山山，黄旭丰，等.全类型非对称七段式S型曲线加减速控制算法研究[J].机械科学与技术，2018，37(12)：1928-1935.

[8] 李玉娟.基于STM32的变电站巡检机器人运动控制系统设计与实现[D].成都：西南交通大学，2013.

[9] 马妍，宋爱国.基于STM32的力反馈型康复机器人控制系统设计[J].测控技术，2014，33(1)：74-78.

作者信息:

胡春生1，修 瑞2，王 德1

(1.宁夏大学 机械工程学院，宁夏 银川750001；2.中国电子科技集团公司第二十一研究所，上海200233)

,