世界技能大赛移动机器人运动控制系统设计

章安福

（广州市工贸技师学院，广州，510000）

0 引言

世界技能大赛由世界技能组织举办，被誉为“技能奥林匹克”，是世界技能组织成员展示和交流职业技能的重要平台，比赛项目共分为6个大类，分别为结构与建筑技术、创意艺术和时尚、信息与通信技术、制造与工程技术、社会与个人服务、运输与物流。

移动机器人项目属于制造与工程技术领域的赛事之一，随着制造业的转型升级，技能人才的培养也扮演着越来越重要的角色，为了更好地推广移动机器人项目，让更多的院校参与世界技能大赛，让更多的学生学会利用自动控制技术设计世界技能大赛所需的移动机器人，笔者将几年来对移动机器人电机控制、运动规划方面的一些技巧及实现做了总结，旨在让更多的参与者快速学会对移动机器人的控制，更好地推动移动机器人相关专业的发展。

1 系统组成

1.1 系统基本构成

世界技能大赛移动机器人项目一般要求参赛队伍所制作的移动机器人具有较为灵活的移动能力，为了满足这一条件，普遍采用全方位移动的机器人设计。全方位移动机器人具有全方位运动能力，其实现方式关键在于全方位的轮系结构，该结构具备每一个大轮边缘套有小轮的机构，能够避免普通轮系不能侧滑带来的非完整性运动限制，从而实现全方位运动。

在比赛中，机器人较为常用的底盘是用 3 个全向轮组成的底盘运动控制系统。其中，三个全向轮运动轴心夹角按照 120°进行设计，之间通过3条横梁互为60°连接构成，如图1所示，底盘三个全向轮由独立的电机驱动。底盘运动信息主要通过三个360线的编码器和一个9轴陀螺仪获取。

图1 三轮机器人效果图（左）及实物图（右）

1.2 系统主体框架

世界技能大赛移动机器人项目所设计的机器人，既要考虑到实用性，又要考虑到使用提供指定套件来搭建。整个指定套件提供了4个直流电机、3个舵机、1个陀螺仪、2个超声波传感器、2个红外传感器、2个限位开关，设计的机器人需要依赖于上述提供的电气元件。笔者所使用的三轮平台由核心控制模块（MYRIO）、传感器检测模块、电机驱动模块等部分组成。其中核心控制模块采用美国国家仪器公司（National Instruments ，以下简称NI）的 1900型号 MYRIO为控制核心，主要完成各外设的底层驱动、机器人的定位、导航、电机的闭环控制等；传感器检测模块包括陀螺仪、编码器，超声波，红外，QTI，其框架如图2所示。

图2 三轮机器人主体框架

在机器人框架设计及制作过程中，笔者在综合考虑移动机器人性能的同时，尽可能多地在套件基础上进行设计与制作，在框架连接上也给布线和OMS留下相应的空间和连接位置。三类机器人设计完成后，笔者发现，三轮机器人使用的材料最少，安装效率最高，框架稳定性和兼容性也能满足要求。三类机器人在框架方面的对比如表1所示。

表1 三类机器人框架性能对比

2 移动机器人控制实现

2.1 LabVIEW软件概要

LabVIEW的英文全称是Laboratory Virtual Instrument Engineering Workbench ，是一门编程语言，也是一种程序开发环境，由NI公司研制开发。LabVIEW类似于C或BASIC开发环境，但是这个软件编程不是用代码，而是用图形符号，在这一点上，它明显不同于传统的编程语言，如C、C++或Java等，这些程序语言主要是用文本方式编程。LabVIEW除了是一门编程语言外，它主要为科学家和工程师等设计者提供一种编程开发环境和运行系统，编程只是设计者工作任务的一部分。LabVIEW开发环境可以搭建在Windows、Mac或Linux系统上，这样LabVIEW的程序可以直接在相应系统中运行，LabVIEW还可以运行在Microsoft Pocket PC、Microsoft Windows CE、Palm OS以及多种嵌入式平台上，包括DSP、FPGA和微处理器等。

2.2 LabVIEW工作原理

一个LabVIEW程序是由多个(至少1个)虚拟仪器(VI)组成。LabVIEW程序的操作和外观与实际的物理仪器很相似，因此有“虚拟仪器”的称谓。即使这样，它的内部的逻辑处理思维仍然是与流行的C语言或者Java语言的编程思维一样的，同样具有主程序、子程序、函数等，设计者看到或者操作的图形符号都只是封装了这些逻辑处理的外观形象，便于设计者使用起来更加方便高效。

为方便起，下文笔者将把“LabVIEW程序”称之为“VI”。注意，无论其外观和功能是否和实际的仪器相关联，笔者总是将LabVIEW程序称为VI 。

每一个VI都由三个主要部分组成：前面板、框图和图标。

2.3 LabVIEW的PID控制工具包

NI提供了在LabVIEW中使用的PID控制工具包，如图3所示，可帮助设计者结合NI数据采集设备快速有效地搭建一个数字PID控制器，精确可靠地完成系统需求。

图3 PID工具包

利用PID.vi即可搭建一个简单的PID控制器，在该VI的输入端给入PID的3个参数值（PID gains）：系统反馈值(process variable)、实际期望值(setpoint)以及微分时间(dt)，便能得到需要的输出值(output)。该VI还能控制输出值的范围。基础PID控件结构如图4所示。

图4 基础PID控件

PID Advanced.vi是高级PID的快速VI，增加了一些高级的功能，如可以设定期望值的范围（setpoint range），以及带手动无扰至自动切换的手动模式控制、非线性积分操作、2自由度控制和误差平方控制等功能。PID Autotuning.vi是为需要自整定的PID系统设计的，在给出一些基本要求后，具有自整定的功能。图5为一种高级自整定PID控件。

图5 高级自整定PID控件

PID Lead/Lag.vi可以对PID控制器前端由系统反馈来的输入信号做动态补偿。PID Setpoint Profile.vi可以在期望值间断性变化时使其变得平滑。PID Control Input Filter.vi是一个五阶的低通滤波器，放在PID控制器的“process variable”前端，可以滤去小于采样率1/10的输入值。PID Gain Schedule.vi可以写入几组增益参数，并给出条件；在执行时,当输入信号达到条件，VI便使用对应的一组参数给入到PID控制器的PID Gains上。PID Output Limiter.vi对PID控制器输出信号的变化速率进行控制，以保证外部接受控制信号部件的安全。

图6 基础PID构建的控制系统

PID工具包提供15个VI，可以满足大多数场合的应用，根据不同的现场需求，使用不同的VI搭建PID控制器，十分方便。

PID控制器输出的精准和前端的输入信号是否精确密切相关，所以采集控制系统的前端输出而得到的系统反馈尤其重要。NI的DAQ数据采集卡不仅工作稳定，结合LabVIEW编程方便，兼容性好，且种类很多，可满足设计者各类不同档次上的需求。

在NI软、硬件的配合下，设计者能够使搭建PID控制系统方便、快速、顺利，让设计工作事半功倍。

2.4 LabVIEW实现的电机调试

世界技能大赛指定了编程语言使用LabVIEW，所有的参赛机器人程序控制系统均采用LabVIEW编写完成。导航任务的实现主要完成机器人的运动控制，根据机器人当前位置坐标得到目标点坐标信息与补偿速度，再综合计算得到底盘运动目标速度并分解为三个电机驱动全向轮的目标速度，之后采用PID 算法实现电机的闭环控制。在实际调试过程中，设计者需要先设计完成基础PID的LabVIEW架构，便于后期调试。

PID控制是一种在工业生产中应用最广泛的控制方法，其最大的优点是不需要了解被控对象精确的数学模型、进行复杂的理论计算，只需要在线根据被控变量与给定值之间的偏差以及偏差的变化率等简单参数，通过工程方法对比例系数PK、积分时间IT、微分时间DT三个参数进行调整，就可以得到令人满意的控制效果。

PID控制算法可以分为位置PID和速度PID，电机速度控制则使用速度PID，LabVIEW提供了相关函数，PID函数支持数组模式，可用于多个电机控制。程序框图如图7所示。

图7 PID构建的三轮机器人底盘控制程序

在LabVIEW前面板建立友好的人机交互界面，方便设计者进行机器人调试。先设定好设定值，然后进行PID的调节，一开始可以缓慢增加P值，当快要接近设定值时候增加I值，调节界面如图8、图9所示。

图8 比例控制器调节波形图

图9 比例积分控制器调节波形图

当P和I都调好后，编码误差在±1之间，这时就不需要调微分量D了。微分调节参数D主要起抑制作用，每个电机的参数会有一些微量的差别，但是每个参数的调法都按照P-I-D的顺序调整较为合适。

调节升降高度使用的是位置PID控制，位置PID则是将编码器的值直接连接到过程变量，输出结果连接到速度PID,如图10所示。

图10 位置PID调节程序

位置PID的调节与速度PID一致，先调P值，使其在最终目标点停稳，如果有小幅波动则调节D参数的值抑制波动。位置波形图类似梯形，如图11所示。

图11 位置PID调节波形图

在机器人运动过程中，除了编码值的采集外，还会因为场地因素导致机器人发生角度偏转，而编码器无法反馈角度偏转，因此在底盘运动控制中需要引入陀螺仪来实现闭环控制。

在陀螺仪采集控制中，所使用的模块通过I2C读写。使用LabVIEW提供的I2C操作函数，先进行打开配置，采样率为400kbps，如图12所示。

图12 陀螺仪采集函数设置

根据陀螺仪手册找到角度地址，读取角度。由于陀螺仪读取出的角度只有-180～180，当机器人旋转超过180时就会回到-180，因此，机器人如果需要旋转更多的角度则需要添加处理，如图13所示。

图13 陀螺仪采集函数示例

陀螺仪存在温漂，在实际使用中，这会给机器人底盘带来误差。所以，设计者需要编写滤波程序，并考虑其在整个控制系统中的实现。

首先确定重力矢量，

R=[Rx,Ry,Rz]。

R可由其他值推导出来，如Axr，Ayr，Azr或者cosX，cosY，cosZ，根据这些值能得到设备相对地平面的倾角值。这些数值只是由加速度计数据推导出来的，如果直接将它们用于程序，可能会产生难以忍受的噪声。

为了避免混乱，笔者重新定义加速度计的测量值。

Racc 是由加速度计测量到得的惯性力矢量，它可分解为下面的分量（在XYZ轴上的投影）：

RxAcc = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity，

RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity，

RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity。

首先，根据加速度计得到一组只来自于加速度计ADC的值。笔者把这组数据叫做“vector”，并使用下面的符号表示：

Racc = [RxAcc,RyAcc,RzAcc]。

因为Racc的各个分量可由加速度计数据得到，笔者把它当做算法的输入。

请注意，Racc测量的是重力，如果得到的矢量长度约等于1g，那么就是正确的：

|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2)。

但把矢量转换成下面的矢量非常重要：

Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| ,zAcc/|Racc|]，

这可以确保标准化Racc始终是1。

接下来，笔者引进一个新的向量：

Rest = [RxEst,RyEst,RzEst]。

Rest就是算法的输出值，它基于上一次估算的值并经过陀螺仪数据修正。

在数列初始，笔者先认定加速度值正确并赋值：

Rest(0) = Racc(0)，

Rest和Racc是向量，所以上面的公式可以用3个简单的公式代替：

RxEst（0）= RxAcc（0），

RyEst（0）= RyAcc（0），

RzEst（0）= RzAcc（0）。

然后，每隔T秒做一次测量，得到新的测量值，并定义为Racc（1），Racc（2），Racc（3）……。与此同时，在每个时间间隔，笔者也计算出新的估算值Rest（1）,Rest（2），Rest（3）……。

假设在第n步，笔者有两列已知的值可以用：

Rest(n-1) – 前一个估算值，

Rest(0) = Racc(0)。

Racc(n) 为当前加速度计测量值。

在计算Rest（n）前，笔者先引进一个新的值Rgyro，它可由陀螺仪和前一个估算值得到。

同理，Rgyro是1个矢量，并由3个分量组成：

Rgyro = [RxGyro,RyGyro,RzGyro]。

分别计算这个矢量的分量，这样就能更好地实现目标要求。

2.5 移动机器人路径规划

三轮全向移动机器人的模型如图14所示。在这个模型中，定义一个移动机器人参考坐标系R，机器人正向前行为坐标系Y方向，右方为坐标系X方向，数学表示为：

为机器人建立一个局部参考坐标系，分析每个轮子各自的矢量速度对机器人参考坐标系所作出的贡献称为正运动学。

图14 三轮运动模型建立

通俗地，已知每个轮子的转速，可以计算得到机器人参考坐标系下的速度，即每个轮子在坐标系R下的投影，这常用于机器人运动估计、推算定位、世界坐标系定位等。

反之，已知参考坐标系下的速度，需要求解每个轮子的矢量速度称为逆运动学问题。逆运动学在这里可以理解为设定机器人X方向、Y方向的速度，计算得到每个电机的矢量速度。

这里定义一个基向量为v ，方向为0°，长度r为圆半径，则每个轮子的坐标表达式如下:

机器人定位计算任务主要实现机器人实时定位，通过程序读取得到的陀螺仪、编码器数据进行计算，得到底盘实时的坐标、位姿、速度等信息；完成底盘的全场定位，获取机器人的实时运动速度，执行导航控制得到目标速度，将目标速度实时分解为三个电机驱动全向轮的目标速度，完成每个电机驱动全向轮的速度闭环控制；从而实现机器人从任意坐标位姿（x1，y1，θ1）跟踪 Bezier 曲线路径到达另一坐标位姿（x2，y2，θ2）。

位姿控制程序框图如图15所示。

图15 位姿控制程序

关于全向底盘的运动控制，LabVIEW提供了相应的函数，可以使设计者更快速实现调试。

在移动机器人调试过程中，无论机器人在场地的任意一个角落，机器人需要知道自己所在位置的坐标，这样才能够实现机器人的自动控制。因此，在控制过程中，机器人需要实现全场定位功能。

全场定位即是将函数Get Velocity from Motors输出的坐标进行积分，程序框图如图16所示，坐标通过陀螺仪读取的值旋转然后积分。

图16 全场定位控制程序

路径规划与速度规划是机器人具备行走能力的基础，对轮式机器人更是如此。机器人理想的运动路径应是一条光滑连续的曲线，并且运动速度应对时间可导，由此实现平稳运动。三阶Bezier 曲线具有光滑、连续等数学性质，是比较合适的路径规划方法，并已在比赛中得到广泛应用，如图17所示；良好的速度规划可以减小底盘因加速度不恰当引起的冲击，利于机器人平稳运动。笔者考虑到比赛中机器人跑动的路径相对固定，采用三阶 Bezier 曲线规划机器人路径，之后通过插补法完成机器人位姿、速度规划。

图17 路径规划效果演示

在实际比赛中，移动机器人需要在不同的场地环境下完成相似的任务，对场地自动规划及建模会减少比赛过程中测量场地及计算坐标的过程。可通过LabVIEW绘制场地图，绘制场地图使用LabVIEW提供的图片绘制函数，如图18所示。

图18 LabVIEW绘图功能函数

事先准备好每个模块，如零件架、工作站、零件库、机器人、场地隔板等元素，方便调用，然后根据现场测量记录绘制场地图，如图19所示。

图19 LabVIEW绘制场地效果图

在比赛过程中，移动机器人需要移动到不同的工作站点，并完成抓取、放置等动作，机器人必须在场地中绕开隔板进行移动，所以，在绘制好场地图之后，需要放上机器人的模型作为路径点，这样获取的理论路径点就可以实际应用到比赛场地的调试中。将机器人模型放置到场地中各个位置，模拟移动机器人实际比赛过程中的路径点，在此基础上完成相应的控制动作，实现对机器人的运动控制，机器人在场地中的实际效果如图20所示。

图20 移动机器人在场地中各个位置的效果图

在此基础上，通过场地坐标运算得出相应的坐标地点，通过数据转换程序计算出实际机器人需要移动的坐标位置，程序得出所有位置并填到机器人控制面板中，如图21所示。

图21 坐标运算填写完成情况

3 经验总结

本文详细介绍了基于LabVIEW电机PID控制实现Bezier曲线路径规划的机器人导航算法，并以第45届世界技能大赛参赛机器人为平台，研究三轮全向轮底盘的运动特性，结合外部传感器实现对机器人的运动控制，使其能够跟随基于 Bezier曲线规划的路径运动。实践表明，该套控制系统具有良好的运动控制精度，在应用中满足比赛对高速、精确的底盘运动控制的要求，可以为全国参赛院校提供参考。

需要指出的是，在实际调试中，机器人会有底盘误差和传感器误差，使得调试工作并不简单。想要在更短的时间内完成调试工作，设计者需要平时不断的训练、发现并解决问题、积累各种经验。随着经验的不断积累，设计者将会更快速适应新场地的调试，运动控制系统的学习也将变得更加便捷。